From b6a26b3226c2be78acdf1219dc77cdfa2e46310b Mon Sep 17 00:00:00 2001
From: Mark Qvist <mark@unsigned.io>
Date: Sun, 30 Oct 2022 13:51:36 +0100
Subject: [PATCH] Added Bluetooth driver

---
 Bluetooth.h        | 116 +++++++++++++++++++++++++++++++++++++++++++--
 Framing.h          |   2 +
 RNode_Firmware.ino |  16 ++++++-
 3 files changed, 129 insertions(+), 5 deletions(-)

diff --git a/Bluetooth.h b/Bluetooth.h
index d28b6c9..b898fd4 100644
--- a/Bluetooth.h
+++ b/Bluetooth.h
@@ -1,5 +1,113 @@
+#include "BluetoothSerial.h"
+#include "esp_bt_main.h"
+#include "esp_bt_device.h"
+
+BluetoothSerial SerialBT;
+#define BT_PAIRING_TIMEOUT 30000
+uint32_t bt_pairing_started = 0;
+
+#define BT_DEV_ADDR_LEN 6
+#define BT_DEV_HASH_LEN 16
+char bt_da[BT_DEV_ADDR_LEN];
+char bt_dh[BT_DEV_HASH_LEN];
+char bt_devname[11];
+
 bool bt_ready = false;
-bool bt_init() {
-    bt_state = BT_STATE_OFF;
-    return true;
-}
\ No newline at end of file
+bool bt_enabled = true;
+bool bt_allow_pairing = false;
+
+#if MCU_VARIANT == MCU_ESP32
+
+  void bt_confirm_pairing(uint32_t numVal) {
+    if (bt_allow_pairing) {
+      bt_state = BT_STATE_ON;
+      SerialBT.confirmReply(true);
+    } else {
+      bt_state = BT_STATE_ON;
+      SerialBT.confirmReply(false);
+    }
+  }
+
+  void bt_pairing_complete(boolean success) {
+    if (success) {
+      Serial.println("Paired");
+    } else {
+      Serial.println("Not Paired");
+    }
+  }
+
+  void bt_enable_pairing() {
+    bt_allow_pairing = true;
+    bt_pairing_started = millis();
+    bt_state = BT_STATE_PAIRING;
+  }
+
+  void bt_disable_pairing() {
+    bt_allow_pairing = false;
+    bt_state = BT_STATE_ON;
+  }
+
+  void bt_disable() {
+    if (bt_state != BT_STATE_OFF) {
+      SerialBT.end();
+      bt_state = BT_STATE_OFF;
+    }
+  }
+
+  void bt_enable() {
+    if (bt_state == BT_STATE_OFF) {
+      SerialBT.begin(bt_devname);
+      bt_state = BT_STATE_ON;
+     }
+  }
+
+  bool bt_setup_hw() {
+    if (!bt_ready) {
+      if (btStart()) {
+        if (esp_bluedroid_init() == ESP_OK) {
+          if (esp_bluedroid_enable() == ESP_OK) {
+            const uint8_t* bda_ptr = esp_bt_dev_get_address();
+            char *data = (char*)malloc(BT_DEV_ADDR_LEN);
+            for (int i = 0; i < BT_DEV_ADDR_LEN; i++) {
+                data[i] = bda_ptr[i];
+            }
+            unsigned char *hash = MD5::make_hash(data, BT_DEV_ADDR_LEN);
+            memcpy(bt_dh, hash, BT_DEV_HASH_LEN);
+            sprintf(bt_devname, "RNode %02X%02X", bt_dh[14], bt_dh[15]);
+            free(data);
+
+            SerialBT.enableSSP();
+            SerialBT.onConfirmRequest(bt_confirm_pairing);
+            SerialBT.onAuthComplete(bt_pairing_complete);
+            
+            bt_ready = true;
+            return true;
+
+          } else { return false; }
+        } else { return false; }
+      } else { return false; }
+    }
+  }
+
+  bool bt_init() {
+      bt_state = BT_STATE_OFF;
+      if (bt_enabled) {
+        if (bt_setup_hw()) {
+          // TODO: Read from conf
+          bt_enable();
+          return true;
+        } else {
+          return false;
+        }
+      } else {
+        return false;
+      }
+  }
+
+  void update_bt() {
+    if (millis()-bt_pairing_started >= BT_PAIRING_TIMEOUT) {
+      bt_disable_pairing();
+    }
+  }
+  
+#endif
diff --git a/Framing.h b/Framing.h
index 0233f2f..eaadb42 100644
--- a/Framing.h
+++ b/Framing.h
@@ -31,6 +31,8 @@
 	#define CMD_FB_EXT      0x41
 	#define CMD_FB_READ     0x42
 	#define CMD_FB_WRITE    0x43
+	#define CMD_FB_READL    0x44
+	#define CMD_BT_CTRL     0x45
 
 	#define CMD_BOARD		0x47
 	#define CMD_PLATFORM    0x48
diff --git a/RNode_Firmware.ino b/RNode_Firmware.ino
index b1f38c1..6e1b545 100644
--- a/RNode_Firmware.ino
+++ b/RNode_Firmware.ino
@@ -102,7 +102,7 @@ void setup() {
   validateStatus();
 
   #if HAS_BLUETOOTH
-    bt_ready = bt_init();
+    bt_init();
   #endif
 }
 
@@ -674,6 +674,16 @@ void serialCallback(uint8_t sbyte) {
       if (sbyte != 0x00) {
         kiss_indicate_fb();
       }
+    } else if (command == CMD_BT_CTRL) {
+      #if HAS_BLUETOOTH
+        if (sbyte == 0x00) {
+          bt_disable();
+        } else if (sbyte == 0x01) {
+          bt_enable();
+        } else if (sbyte == 0x02) {
+          bt_enable_pairing();
+        }
+      #endif
     }
   }
 }
@@ -849,6 +859,10 @@ void loop() {
   #if HAS_PMU
     if (pmu_ready) update_pmu();
   #endif
+
+  #if HAS_BLUETOOTH
+    if (bt_ready) update_bt();
+  #endif
 }
 
 volatile bool serial_polling = false;