From 2536c26f505f83df826bce0d1e539dc283265c7b Mon Sep 17 00:00:00 2001
From: Mark Qvist <mark@unsigned.io>
Date: Tue, 19 Sep 2023 18:32:29 +0200
Subject: [PATCH] Added ability to set custom display address

---
 Config.h           |  1 +
 Display.h          | 18 +++++++++++++++---
 Framing.h          |  1 +
 RNode_Firmware.ino | 15 +++++++++++++++
 ROM.h              |  1 +
 Utilities.h        |  4 ++++
 6 files changed, 37 insertions(+), 3 deletions(-)

diff --git a/Config.h b/Config.h
index aef43a2..dd3c8b3 100644
--- a/Config.h
+++ b/Config.h
@@ -408,6 +408,7 @@
     float battery_percent = 0.0;
     uint8_t battery_state = 0x00;
     uint8_t display_intensity = 0xFF;
+    uint8_t display_addr = 0xFF;
     bool display_diagnostics = true;    
     bool device_init_done = false;
     bool eeprom_ok = false;
diff --git a/Display.h b/Display.h
index 9998973..51bff3f 100644
--- a/Display.h
+++ b/Display.h
@@ -25,16 +25,20 @@
   #define DISP_ADDR 0x3C
 #elif BOARD_MODEL == BOARD_TBEAM
   #define DISP_RST 13
-  #define DISP_ADDR 0x3D
-  // #define DISP_ADDR 0x3C
+  #define DISP_ADDR 0x3C
+  #define DISP_CUSTOM_ADDR true
 #elif BOARD_MODEL == BOARD_HELTEC32_V2 || BOARD_MODEL == BOARD_LORA32_V1_0
   #define DISP_RST 16
   #define DISP_ADDR 0x3C
   #define SCL_OLED 15
   #define SDA_OLED 4
+#elif BOARD_MODEL == BOARD_RNODE_NG_21
+  #define DISP_RST -1
+  #define DISP_ADDR 0x3C
 #else
   #define DISP_RST -1
   #define DISP_ADDR 0x3C
+  #define DISP_CUSTOM_ADDR true
 #endif
 
 #define SMALL_FONT &Org_01
@@ -104,8 +108,16 @@ bool display_init() {
       digitalWrite(pin_display_en, HIGH);
       Wire.begin(SDA_OLED, SCL_OLED);
     #endif
+
+    #if DISP_CUSTOM_ADDR == true
+      uint8_t display_address = EEPROM.read(eeprom_addr(ADDR_CONF_DADR));
+      if (display_address == 0xFF) display_address = DISP_ADDR;
+    #else
+      uint8_t display_address = DISP_ADDR;
+    #endif
+
     
-    if(!display.begin(SSD1306_SWITCHCAPVCC, DISP_ADDR)) {
+    if(!display.begin(SSD1306_SWITCHCAPVCC, display_address)) {
       return false;
     } else {
       set_contrast(&display, display_contrast);
diff --git a/Framing.h b/Framing.h
index 97dce7c..d95bf93 100644
--- a/Framing.h
+++ b/Framing.h
@@ -52,6 +52,7 @@
   #define CMD_FB_WRITE    0x43
   #define CMD_FB_READL    0x44
   #define CMD_DISP_INT    0x45
+  #define CMD_DISP_ADDR   0x63
   #define CMD_BT_CTRL     0x46
   #define CMD_BT_PIN      0x62
 
diff --git a/RNode_Firmware.ino b/RNode_Firmware.ino
index e5852ea..953948c 100644
--- a/RNode_Firmware.ino
+++ b/RNode_Firmware.ino
@@ -898,6 +898,21 @@ void serialCallback(uint8_t sbyte) {
             di_conf_save(display_intensity);
         }
 
+      #endif
+    } else if (command == CMD_DISP_ADDR) {
+      #if HAS_DISPLAY
+        if (sbyte == FESC) {
+            ESCAPE = true;
+        } else {
+            if (ESCAPE) {
+                if (sbyte == TFEND) sbyte = FEND;
+                if (sbyte == TFESC) sbyte = FESC;
+                ESCAPE = false;
+            }
+            display_addr = sbyte;
+            da_conf_save(display_addr);
+        }
+
       #endif
     }
   }
diff --git a/ROM.h b/ROM.h
index 9155910..05c89dc 100644
--- a/ROM.h
+++ b/ROM.h
@@ -63,6 +63,7 @@
 	#define ADDR_CONF_BT   0xB0
 	#define ADDR_CONF_DSET 0xB1
 	#define ADDR_CONF_DINT 0xB2
+	#define ADDR_CONF_DADR 0xB3
 
 	#define INFO_LOCK_BYTE 0x73
 	#define CONF_OK_BYTE   0x73
diff --git a/Utilities.h b/Utilities.h
index 97697b2..0c7f741 100644
--- a/Utilities.h
+++ b/Utilities.h
@@ -1212,6 +1212,10 @@ void di_conf_save(uint8_t dint) {
 	eeprom_update(eeprom_addr(ADDR_CONF_DINT), dint);
 }
 
+void da_conf_save(uint8_t dadr) {
+	eeprom_update(eeprom_addr(ADDR_CONF_DADR), dadr);
+}
+
 bool eeprom_have_conf() {
 	if (EEPROM.read(eeprom_addr(ADDR_CONF_OK)) == CONF_OK_BYTE) {
 		return true;