From 73ab0d881361c215b01e34cae9d3ba8064bef576 Mon Sep 17 00:00:00 2001
From: liquidraver <504870+liquidraver@users.noreply.github.com>
Date: Thu, 4 Dec 2025 11:48:01 +0100
Subject: [PATCH] Improve SerialBLEInterface

---
 src/helpers/nrf52/SerialBLEInterface.cpp | 376 ++++++++++++++++-------
 src/helpers/nrf52/SerialBLEInterface.h   |  32 +-
 2 files changed, 289 insertions(+), 119 deletions(-)

diff --git a/src/helpers/nrf52/SerialBLEInterface.cpp b/src/helpers/nrf52/SerialBLEInterface.cpp
index dbe6f393..b4811a20 100644
--- a/src/helpers/nrf52/SerialBLEInterface.cpp
+++ b/src/helpers/nrf52/SerialBLEInterface.cpp
@@ -1,193 +1,353 @@
 #include "SerialBLEInterface.h"
+#include <stdio.h>
+#include <string.h>
+#include "ble_gap.h"
+#include "ble_hci.h"
 
-static SerialBLEInterface* instance;
+#define BLE_HEALTH_CHECK_INTERVAL  10000  // Advertising watchdog check every 10 seconds
+
+static SerialBLEInterface* instance = nullptr;
 
 void SerialBLEInterface::onConnect(uint16_t connection_handle) {
-  BLE_DEBUG_PRINTLN("SerialBLEInterface: connected");
-  // we now set _isDeviceConnected=true in onSecured callback instead
+  BLE_DEBUG_PRINTLN("SerialBLEInterface: connected handle=0x%04X", connection_handle);
+  if (instance) {
+    instance->_conn_handle = connection_handle;
+    instance->_isDeviceConnected = false;
+    instance->clearBuffers();
+  }
 }
 
 void SerialBLEInterface::onDisconnect(uint16_t connection_handle, uint8_t reason) {
-  BLE_DEBUG_PRINTLN("SerialBLEInterface: disconnected reason=%d", reason);
-  if(instance){
-    instance->_isDeviceConnected = false;
-    instance->startAdv();
+  BLE_DEBUG_PRINTLN("SerialBLEInterface: disconnected handle=0x%04X reason=%u", connection_handle, reason);
+  if (instance) {
+    if (instance->_conn_handle == connection_handle) {
+      instance->_conn_handle = BLE_CONN_HANDLE_INVALID;
+      instance->_isDeviceConnected = false;
+      instance->clearBuffers();
+    }
   }
 }
 
 void SerialBLEInterface::onSecured(uint16_t connection_handle) {
-  BLE_DEBUG_PRINTLN("SerialBLEInterface: onSecured");
-  if(instance){
-    instance->_isDeviceConnected = true;
-    // no need to stop advertising on connect, as the ble stack does this automatically
+  BLE_DEBUG_PRINTLN("SerialBLEInterface: onSecured handle=0x%04X", connection_handle);
+  if (instance) {
+    if (instance->isValidConnection(connection_handle, true)) {
+      instance->_isDeviceConnected = true;
+      
+      // Connection interval units: 1.25ms, supervision timeout units: 10ms
+      // Apple: "The product will not read or use the parameters in the Peripheral Preferred Connection Parameters characteristic."
+      // So we explicitly set it here to make Android & Apple match
+      ble_gap_conn_params_t conn_params;
+      conn_params.min_conn_interval = 12;  // 15ms
+      conn_params.max_conn_interval = 24;  // 30ms
+      conn_params.slave_latency = 0;
+      conn_params.conn_sup_timeout = 200;  // 2000ms
+      
+      uint32_t err_code = sd_ble_gap_conn_param_update(connection_handle, &conn_params);
+      if (err_code == NRF_SUCCESS) {
+        BLE_DEBUG_PRINTLN("Connection parameter update requested: 15-30ms interval, 2s timeout");
+      } else {
+        BLE_DEBUG_PRINTLN("Failed to request connection parameter update: %lu", err_code);
+      }
+    } else {
+      BLE_DEBUG_PRINTLN("onSecured: ignoring stale/duplicate callback");
+    }
+  }
+}
+
+bool SerialBLEInterface::onPairingPasskey(uint16_t connection_handle, uint8_t const passkey[6], bool match_request) {
+  (void)connection_handle;
+  (void)passkey;
+  BLE_DEBUG_PRINTLN("SerialBLEInterface: pairing passkey request match=%d", match_request);
+  return true;
+}
+
+void SerialBLEInterface::onPairingComplete(uint16_t connection_handle, uint8_t auth_status) {
+  BLE_DEBUG_PRINTLN("SerialBLEInterface: pairing complete handle=0x%04X status=%u", connection_handle, auth_status);
+  if (instance) {
+    if (instance->isValidConnection(connection_handle)) {
+      if (auth_status == BLE_GAP_SEC_STATUS_SUCCESS) {
+        BLE_DEBUG_PRINTLN("SerialBLEInterface: pairing successful");
+      } else {
+        BLE_DEBUG_PRINTLN("SerialBLEInterface: pairing failed, disconnecting");
+        instance->disconnect();
+      }
+    } else {
+      BLE_DEBUG_PRINTLN("onPairingComplete: ignoring stale callback");
+    }
+  }
+}
+
+void SerialBLEInterface::onBLEEvent(ble_evt_t* evt) {
+  if (!instance) return;
+  
+  if (evt->header.evt_id == BLE_GAP_EVT_CONN_PARAM_UPDATE_REQUEST) {
+    uint16_t conn_handle = evt->evt.gap_evt.conn_handle;
+    if (instance->isValidConnection(conn_handle)) {
+      BLE_DEBUG_PRINTLN("CONN_PARAM_UPDATE_REQUEST: handle=0x%04X, min_interval=%u, max_interval=%u, latency=%u, timeout=%u",
+                       conn_handle,
+                       evt->evt.gap_evt.params.conn_param_update_request.conn_params.min_conn_interval,
+                       evt->evt.gap_evt.params.conn_param_update_request.conn_params.max_conn_interval,
+                       evt->evt.gap_evt.params.conn_param_update_request.conn_params.slave_latency,
+                       evt->evt.gap_evt.params.conn_param_update_request.conn_params.conn_sup_timeout);
+      
+      uint32_t err_code = sd_ble_gap_conn_param_update(conn_handle, NULL);
+      if (err_code == NRF_SUCCESS) {
+        BLE_DEBUG_PRINTLN("Accepted CONN_PARAM_UPDATE_REQUEST (using PPCP)");
+      } else {
+        BLE_DEBUG_PRINTLN("ERROR: Failed to accept CONN_PARAM_UPDATE_REQUEST: 0x%08X", err_code);
+      }
+    } else {
+      BLE_DEBUG_PRINTLN("CONN_PARAM_UPDATE_REQUEST: ignoring stale callback for handle=0x%04X", conn_handle);
+    }
   }
 }
 
 void SerialBLEInterface::begin(const char* device_name, uint32_t pin_code) {
-
   instance = this;
 
   char charpin[20];
-  sprintf(charpin, "%d", pin_code);
-
+  snprintf(charpin, sizeof(charpin), "%lu", (unsigned long)pin_code);
+  
+  // If we want to control BLE LED ourselves, uncomment this:
+  // Bluefruit.autoConnLed(false);
   Bluefruit.configPrphBandwidth(BANDWIDTH_MAX);
-  Bluefruit.configPrphConn(250, BLE_GAP_EVENT_LENGTH_MIN, 16, 16);  // increase MTU
-  Bluefruit.setTxPower(BLE_TX_POWER);
   Bluefruit.begin();
+  
+  // Connection interval units: 1.25ms, supervision timeout units: 10ms
+  ble_gap_conn_params_t ppcp_params;
+  ppcp_params.min_conn_interval = 12;  // 15ms
+  ppcp_params.max_conn_interval = 24;  // 30ms
+  ppcp_params.slave_latency = 0;
+  ppcp_params.conn_sup_timeout = 200;  // 2000ms
+  
+  uint32_t err_code = sd_ble_gap_ppcp_set(&ppcp_params);
+  if (err_code == NRF_SUCCESS) {
+    BLE_DEBUG_PRINTLN("PPCP set: 15-30ms interval, 2s timeout");
+  } else {
+    BLE_DEBUG_PRINTLN("Failed to set PPCP: %lu", err_code);
+  }
+  
+  Bluefruit.setTxPower(BLE_TX_POWER);
   Bluefruit.setName(device_name);
 
   Bluefruit.Security.setMITM(true);
   Bluefruit.Security.setPIN(charpin);
+  Bluefruit.Security.setIOCaps(true, false, false);
+  Bluefruit.Security.setPairPasskeyCallback(onPairingPasskey);
+  Bluefruit.Security.setPairCompleteCallback(onPairingComplete);
 
   Bluefruit.Periph.setConnectCallback(onConnect);
   Bluefruit.Periph.setDisconnectCallback(onDisconnect);
   Bluefruit.Security.setSecuredCallback(onSecured);
 
-  // To be consistent OTA DFU should be added first if it exists
-  //bledfu.begin();
+  Bluefruit.setEventCallback(onBLEEvent);
 
-  // Configure and start the BLE Uart service
   bleuart.setPermission(SECMODE_ENC_WITH_MITM, SECMODE_ENC_WITH_MITM);
   bleuart.begin();
-  
-}
+  bleuart.setRxCallback(onBleUartRX);
 
-void SerialBLEInterface::startAdv() {
-
-  BLE_DEBUG_PRINTLN("SerialBLEInterface: starting advertising");
-  
-  // clean restart if already advertising
-  if(Bluefruit.Advertising.isRunning()){
-    BLE_DEBUG_PRINTLN("SerialBLEInterface: already advertising, stopping to allow clean restart");
-    Bluefruit.Advertising.stop();
-  }
-
-  Bluefruit.Advertising.clearData(); // clear advertising data
-  Bluefruit.ScanResponse.clearData(); // clear scan response data
-  
-  // Advertising packet
   Bluefruit.Advertising.addFlags(BLE_GAP_ADV_FLAGS_LE_ONLY_GENERAL_DISC_MODE);
   Bluefruit.Advertising.addTxPower();
-  
-  // Include the BLE UART (AKA 'NUS') 128-bit UUID
   Bluefruit.Advertising.addService(bleuart);
 
-  // Secondary Scan Response packet (optional)
-  // Since there is no room for 'Name' in Advertising packet
   Bluefruit.ScanResponse.addName();
 
-  /* Start Advertising
-   * - Enable auto advertising if disconnected
-   * - Interval:  fast mode = 20 ms, slow mode = 152.5 ms
-   * - Timeout for fast mode is 30 seconds
-   * - Start(timeout) with timeout = 0 will advertise forever (until connected)
-   * 
-   * For recommended advertising interval
-   * https://developer.apple.com/library/content/qa/qa1931/_index.html   
-   */
-  Bluefruit.Advertising.restartOnDisconnect(false); // don't restart automatically as we handle it in onDisconnect
   Bluefruit.Advertising.setInterval(32, 244);
-  Bluefruit.Advertising.setFastTimeout(30);      // number of seconds in fast mode
-  Bluefruit.Advertising.start(0);                // 0 = Don't stop advertising after n seconds
+  Bluefruit.Advertising.setFastTimeout(30);
+
+  Bluefruit.Advertising.restartOnDisconnect(true);
 
 }
 
-void SerialBLEInterface::stopAdv() {
+void SerialBLEInterface::clearBuffers() {
+  send_queue_len = 0;
+  recv_queue_len = 0;
+  bleuart.flush();
+}
 
-  BLE_DEBUG_PRINTLN("SerialBLEInterface: stopping advertising");
-  
-  // we only want to stop advertising if it's running, otherwise an invalid state error is logged by ble stack
-  if(!Bluefruit.Advertising.isRunning()){
-    return;
+void SerialBLEInterface::shiftSendQueueLeft() {
+  if (send_queue_len > 0) {
+    send_queue_len--;
+    for (uint8_t i = 0; i < send_queue_len; i++) {
+      send_queue[i] = send_queue[i + 1];
+    }
   }
-
-  // stop advertising
-  Bluefruit.Advertising.stop();
-
 }
 
-// ---------- public methods
+void SerialBLEInterface::shiftRecvQueueLeft() {
+  if (recv_queue_len > 0) {
+    recv_queue_len--;
+    for (uint8_t i = 0; i < recv_queue_len; i++) {
+      recv_queue[i] = recv_queue[i + 1];
+    }
+  }
+}
 
-void SerialBLEInterface::enable() { 
+bool SerialBLEInterface::isValidConnection(uint16_t handle, bool requireWaitingForSecurity) const {
+  if (_conn_handle != handle) {
+    return false;
+  }
+  BLEConnection* conn = Bluefruit.Connection(handle);
+  if (conn == nullptr || !conn->connected()) {
+    return false;
+  }
+  if (requireWaitingForSecurity && _isDeviceConnected) {
+    return false;
+  }
+  return true;
+}
+
+bool SerialBLEInterface::isAdvertising() const {
+  ble_gap_addr_t adv_addr;
+  uint32_t err_code = sd_ble_gap_adv_addr_get(0, &adv_addr);
+  return (err_code == NRF_SUCCESS);
+}
+
+void SerialBLEInterface::enable() {
   if (_isEnabled) return;
 
   _isEnabled = true;
   clearBuffers();
+  _last_health_check = millis();
 
-  // Start advertising
-  startAdv();
+  Bluefruit.Advertising.start(0);
+}
+
+void SerialBLEInterface::disconnect() {
+  if (_conn_handle != BLE_CONN_HANDLE_INVALID) {
+    sd_ble_gap_disconnect(_conn_handle, BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION);
+  }
 }
 
 void SerialBLEInterface::disable() {
   _isEnabled = false;
-  BLE_DEBUG_PRINTLN("SerialBLEInterface::disable");
+  BLE_DEBUG_PRINTLN("SerialBLEInterface: disable");
 
-#ifdef RAK_BOARD
-  Bluefruit.disconnect(Bluefruit.connHandle());
-#else
-  uint16_t conn_id;
-  if (Bluefruit.getConnectedHandles(&conn_id, 1) > 0) {
-    Bluefruit.disconnect(conn_id);
-  }
-#endif
-
-  Bluefruit.Advertising.restartOnDisconnect(false);
+  disconnect();
   Bluefruit.Advertising.stop();
-  Bluefruit.Advertising.clearData();
-
-  stopAdv();
+  _last_health_check = 0;
 }
 
 size_t SerialBLEInterface::writeFrame(const uint8_t src[], size_t len) {
   if (len > MAX_FRAME_SIZE) {
-    BLE_DEBUG_PRINTLN("writeFrame(), frame too big, len=%d", len);
+    BLE_DEBUG_PRINTLN("writeFrame(), frame too big, len=%u", (unsigned)len);
     return 0;
   }
 
-  if (_isDeviceConnected && len > 0) {
+  bool connected = isConnected();
+  if (connected && len > 0) {
     if (send_queue_len >= FRAME_QUEUE_SIZE) {
       BLE_DEBUG_PRINTLN("writeFrame(), send_queue is full!");
       return 0;
     }
 
-    send_queue[send_queue_len].len = len;  // add to send queue
+    send_queue[send_queue_len].len = len;
     memcpy(send_queue[send_queue_len].buf, src, len);
     send_queue_len++;
-
+    
     return len;
   }
   return 0;
 }
 
-#define  BLE_WRITE_MIN_INTERVAL   60
-
-bool SerialBLEInterface::isWriteBusy() const {
-  return millis() < _last_write + BLE_WRITE_MIN_INTERVAL;   // still too soon to start another write?
-}
-
 size_t SerialBLEInterface::checkRecvFrame(uint8_t dest[]) {
-  if (send_queue_len > 0   // first, check send queue
-    && millis() >= _last_write + BLE_WRITE_MIN_INTERVAL    // space the writes apart
-  ) {
-    _last_write = millis();
-    bleuart.write(send_queue[0].buf, send_queue[0].len);
-    BLE_DEBUG_PRINTLN("writeBytes: sz=%d, hdr=%d", (uint32_t)send_queue[0].len, (uint32_t) send_queue[0].buf[0]);
-
-    send_queue_len--;
-    for (int i = 0; i < send_queue_len; i++) {   // delete top item from queue
-      send_queue[i] = send_queue[i + 1];
-    }
-  } else {
-    int len = bleuart.available();
-    if (len > 0) {
-      bleuart.readBytes(dest, len);
-      BLE_DEBUG_PRINTLN("readBytes: sz=%d, hdr=%d", len, (uint32_t) dest[0]);
-      return len;
+  if (send_queue_len > 0) {
+    if (!isConnected()) {
+      BLE_DEBUG_PRINTLN("writeBytes: connection invalid, clearing send queue");
+      send_queue_len = 0;
+    } else {
+      Frame frame_to_send = send_queue[0];
+      
+      size_t written = bleuart.write(frame_to_send.buf, frame_to_send.len);
+      if (written == frame_to_send.len) {
+        BLE_DEBUG_PRINTLN("writeBytes: sz=%u, hdr=%u", (unsigned)frame_to_send.len, (unsigned)frame_to_send.buf[0]);
+        shiftSendQueueLeft();
+      } else if (written > 0) {
+        BLE_DEBUG_PRINTLN("writeBytes: partial write, sent=%u of %u, dropping corrupted frame", (unsigned)written, (unsigned)frame_to_send.len);
+        shiftSendQueueLeft();
+      } else {
+        if (!isConnected()) {
+          BLE_DEBUG_PRINTLN("writeBytes failed: connection lost, dropping frame");
+          shiftSendQueueLeft();
+        } else {
+          BLE_DEBUG_PRINTLN("writeBytes failed (buffer full), keeping frame for retry");
+        }
+      }
     }
   }
+  
+  if (recv_queue_len > 0) {
+    size_t len = recv_queue[0].len;
+    memcpy(dest, recv_queue[0].buf, len);
+    
+    BLE_DEBUG_PRINTLN("readBytes: sz=%u, hdr=%u", (unsigned)len, (unsigned)dest[0]);
+    
+    shiftRecvQueueLeft();
+    return len;
+  }
+  
+  // Advertising watchdog: periodically check if advertising is running, restart if not
+  // Only run when truly disconnected (no connection handle), not during connection establishment
+  unsigned long now = millis();
+  if (_isEnabled && !isConnected() && _conn_handle == BLE_CONN_HANDLE_INVALID) {
+    if (now - _last_health_check >= BLE_HEALTH_CHECK_INTERVAL) {
+      _last_health_check = now;
+      
+      if (!isAdvertising()) {
+        BLE_DEBUG_PRINTLN("SerialBLEInterface: advertising watchdog - advertising stopped, restarting");
+        Bluefruit.Advertising.start(0);
+      }
+    }
+  }
+  
   return 0;
 }
 
-bool SerialBLEInterface::isConnected() const {
-  return _isDeviceConnected;
+void SerialBLEInterface::onBleUartRX(uint16_t conn_handle) {
+  if (!instance) {
+    return;
+  }
+  
+  if (instance->_conn_handle != conn_handle || !instance->isConnected()) {
+    while (instance->bleuart.available() > 0) {
+      instance->bleuart.read();
+    }
+    return;
+  }
+  
+  while (instance->bleuart.available() > 0) {
+    if (instance->recv_queue_len >= FRAME_QUEUE_SIZE) {
+      while (instance->bleuart.available() > 0) {
+        instance->bleuart.read();
+      }
+      BLE_DEBUG_PRINTLN("onBleUartRX: recv queue full, dropping data");
+      break;
+    }
+    
+    int avail = instance->bleuart.available();
+    
+    if (avail > MAX_FRAME_SIZE) {
+      BLE_DEBUG_PRINTLN("onBleUartRX: WARN: BLE RX overflow, avail=%d, draining all", avail);
+      uint8_t drain_buf[32];
+      while (instance->bleuart.available() > 0) {
+        int chunk = instance->bleuart.available() > 32 ? 32 : instance->bleuart.available();
+        instance->bleuart.readBytes(drain_buf, chunk);
+      }
+      continue;
+    }
+    
+    int read_len = avail;
+    instance->recv_queue[instance->recv_queue_len].len = read_len;
+    instance->bleuart.readBytes(instance->recv_queue[instance->recv_queue_len].buf, read_len);
+    instance->recv_queue_len++;
+  }
+}
+
+bool SerialBLEInterface::isConnected() const {
+  return _isDeviceConnected && Bluefruit.connected() > 0;
+}
+
+bool SerialBLEInterface::isWriteBusy() const {
+  return send_queue_len >= (FRAME_QUEUE_SIZE - 1);
 }
diff --git a/src/helpers/nrf52/SerialBLEInterface.h b/src/helpers/nrf52/SerialBLEInterface.h
index bf29892d..557b86c5 100644
--- a/src/helpers/nrf52/SerialBLEInterface.h
+++ b/src/helpers/nrf52/SerialBLEInterface.h
@@ -11,41 +11,51 @@ class SerialBLEInterface : public BaseSerialInterface {
   BLEUart bleuart;
   bool _isEnabled;
   bool _isDeviceConnected;
-  unsigned long _last_write;
+  uint16_t _conn_handle;
+  unsigned long _last_health_check;
 
   struct Frame {
     uint8_t len;
     uint8_t buf[MAX_FRAME_SIZE];
   };
 
-  #define FRAME_QUEUE_SIZE  4
-  int send_queue_len;
+  #define FRAME_QUEUE_SIZE  12
+  
+  uint8_t send_queue_len;
   Frame send_queue[FRAME_QUEUE_SIZE];
+  
+  uint8_t recv_queue_len;
+  Frame recv_queue[FRAME_QUEUE_SIZE];
 
-  void clearBuffers() { send_queue_len = 0; }
+  void clearBuffers();
+  void shiftSendQueueLeft();
+  void shiftRecvQueueLeft();
+  bool isValidConnection(uint16_t handle, bool requireWaitingForSecurity = false) const;
+  bool isAdvertising() const;
   static void onConnect(uint16_t connection_handle);
   static void onDisconnect(uint16_t connection_handle, uint8_t reason);
   static void onSecured(uint16_t connection_handle);
+  static bool onPairingPasskey(uint16_t connection_handle, uint8_t const passkey[6], bool match_request);
+  static void onPairingComplete(uint16_t connection_handle, uint8_t auth_status);
+  static void onBLEEvent(ble_evt_t* evt);
+  static void onBleUartRX(uint16_t conn_handle);
 
 public:
   SerialBLEInterface() {
     _isEnabled = false;
     _isDeviceConnected = false;
-    _last_write = 0;
+    _conn_handle = BLE_CONN_HANDLE_INVALID;
+    _last_health_check = 0;
     send_queue_len = 0;
+    recv_queue_len = 0;
   }
 
-  void startAdv();
-  void stopAdv();
   void begin(const char* device_name, uint32_t pin_code);
-
-  // BaseSerialInterface methods
+  void disconnect();
   void enable() override;
   void disable() override;
   bool isEnabled() const override { return _isEnabled; }
-
   bool isConnected() const override;
-
   bool isWriteBusy() const override;
   size_t writeFrame(const uint8_t src[], size_t len) override;
   size_t checkRecvFrame(uint8_t dest[]) override;