Merge pull request #1177 from liquidraver/btfixv6

Fix BLE semaphore leak and improve SerialBLEInterface

arch/nrf52/extra_scripts/patch_bluefruit.py

@@ -0,0 +1,198 @@
+"""
+Bluefruit BLE Patch Script
+
+Patches Bluefruit library to fix semaphore leak bug that causes device lockup
+when BLE central disconnects unexpectedly (e.g., going out of range, supervision timeout).
+
+Patches applied:
+1. BLEConnection.h: Add _hvn_qsize member to track semaphore queue size
+2. BLEConnection.cpp: Store hvn_qsize and restore semaphore on disconnect
+
+Bug description:
+- When a BLE central disconnects unexpectedly (reason=8 supervision timeout),
+  the BLE_GATTS_EVT_HVN_TX_COMPLETE event may never fire
+- This leaves the _hvn_sem counting semaphore in a decremented state
+- Since BLEConnection objects are reused (destructor never called), the
+  semaphore count is never restored
+- Eventually all semaphore counts are exhausted and notify() blocks/fails
+
+"""
+
+from pathlib import Path
+
+Import("env")  # pylint: disable=undefined-variable
+
+
+def _patch_ble_connection_header(source: Path) -> bool:
+    """
+    Add _hvn_qsize member variable to BLEConnection class.
+    
+    This is needed to restore the semaphore to its correct count on disconnect.
+    
+    Returns True if patch was applied or already applied, False on error.
+    """
+    try:
+        content = source.read_text()
+        
+        # Check if already patched
+        if "_hvn_qsize" in content:
+            return True  # Already patched
+        
+        # Find the location to insert - after _phy declaration
+        original_pattern = '''    uint8_t  _phy;
+
+    uint8_t  _role;'''
+        
+        patched_pattern = '''    uint8_t  _phy;
+    uint8_t  _hvn_qsize;
+
+    uint8_t  _role;'''
+        
+        if original_pattern not in content:
+            print("Bluefruit patch: WARNING - BLEConnection.h pattern not found")
+            return False
+        
+        content = content.replace(original_pattern, patched_pattern)
+        source.write_text(content)
+        
+        # Verify
+        if "_hvn_qsize" not in source.read_text():
+            return False
+        
+        return True
+    except Exception as e:
+        print(f"Bluefruit patch: ERROR patching BLEConnection.h: {e}")
+        return False
+
+
+def _patch_ble_connection_source(source: Path) -> bool:
+    """
+    Patch BLEConnection.cpp to:
+    1. Store hvn_qsize in constructor
+    2. Restore _hvn_sem semaphore to full count on disconnect
+    
+    Returns True if patch was applied or already applied, False on error.
+    """
+    try:
+        content = source.read_text()
+        
+        # Check if already patched (look for the restore loop)
+        if "uxSemaphoreGetCount(_hvn_sem)" in content:
+            return True  # Already patched
+        
+        # Patch 1: Store queue size in constructor
+        constructor_original = '''  _hvn_sem   = xSemaphoreCreateCounting(hvn_qsize, hvn_qsize);'''
+        
+        constructor_patched = '''  _hvn_qsize = hvn_qsize;
+  _hvn_sem   = xSemaphoreCreateCounting(hvn_qsize, hvn_qsize);'''
+        
+        if constructor_original not in content:
+            print("Bluefruit patch: WARNING - BLEConnection.cpp constructor pattern not found")
+            return False
+        
+        content = content.replace(constructor_original, constructor_patched)
+        
+        # Patch 2: Restore semaphore on disconnect
+        disconnect_original = '''    case BLE_GAP_EVT_DISCONNECTED:
+      // mark as disconnected
+      _connected = false;
+    break;'''
+        
+        disconnect_patched = '''    case BLE_GAP_EVT_DISCONNECTED:
+      // Restore notification semaphore to full count
+      // This fixes lockup when disconnect occurs with notifications in flight
+      while (uxSemaphoreGetCount(_hvn_sem) < _hvn_qsize) {
+        xSemaphoreGive(_hvn_sem);
+      }
+      // Release indication semaphore if waiting
+      if (_hvc_sem) {
+        _hvc_received = false;
+        xSemaphoreGive(_hvc_sem);
+      }
+      // mark as disconnected
+      _connected = false;
+    break;'''
+        
+        if disconnect_original not in content:
+            print("Bluefruit patch: WARNING - BLEConnection.cpp disconnect pattern not found")
+            return False
+        
+        content = content.replace(disconnect_original, disconnect_patched)
+        source.write_text(content)
+        
+        # Verify
+        verify_content = source.read_text()
+        if "uxSemaphoreGetCount(_hvn_sem)" not in verify_content:
+            return False
+        if "_hvn_qsize = hvn_qsize" not in verify_content:
+            return False
+        
+        return True
+    except Exception as e:
+        print(f"Bluefruit patch: ERROR patching BLEConnection.cpp: {e}")
+        return False
+
+
+def _apply_bluefruit_patches(target, source, env):  # pylint: disable=unused-argument
+    framework_path = env.get("PLATFORMFW_DIR")
+    if not framework_path:
+        framework_path = env.PioPlatform().get_package_dir("framework-arduinoadafruitnrf52")
+
+    if not framework_path:
+        print("Bluefruit patch: ERROR - framework directory not found")
+        env.Exit(1)
+        return
+
+    framework_dir = Path(framework_path)
+    bluefruit_lib = framework_dir / "libraries" / "Bluefruit52Lib" / "src"
+    patch_failed = False
+    
+    # Patch BLEConnection.h
+    conn_header = bluefruit_lib / "BLEConnection.h"
+    if conn_header.exists():
+        before = conn_header.read_text()
+        success = _patch_ble_connection_header(conn_header)
+        after = conn_header.read_text()
+        
+        if success:
+            if before != after:
+                print("Bluefruit patch: OK - Applied BLEConnection.h fix (added _hvn_qsize member)")
+            else:
+                print("Bluefruit patch: OK - BLEConnection.h already patched")
+        else:
+            print("Bluefruit patch: FAILED - BLEConnection.h")
+            patch_failed = True
+    else:
+        print(f"Bluefruit patch: ERROR - BLEConnection.h not found at {conn_header}")
+        patch_failed = True
+    
+    # Patch BLEConnection.cpp
+    conn_source = bluefruit_lib / "BLEConnection.cpp"
+    if conn_source.exists():
+        before = conn_source.read_text()
+        success = _patch_ble_connection_source(conn_source)
+        after = conn_source.read_text()
+        
+        if success:
+            if before != after:
+                print("Bluefruit patch: OK - Applied BLEConnection.cpp fix (restore semaphore on disconnect)")
+            else:
+                print("Bluefruit patch: OK - BLEConnection.cpp already patched")
+        else:
+            print("Bluefruit patch: FAILED - BLEConnection.cpp")
+            patch_failed = True
+    else:
+        print(f"Bluefruit patch: ERROR - BLEConnection.cpp not found at {conn_source}")
+        patch_failed = True
+    
+    if patch_failed:
+        print("Bluefruit patch: CRITICAL - Patch failed! Build aborted.")
+        env.Exit(1)
+
+
+# Register the patch to run before build
+bluefruit_action = env.VerboseAction(_apply_bluefruit_patches, "Applying Bluefruit BLE patches...")
+env.AddPreAction("$BUILD_DIR/${PROGNAME}.elf", bluefruit_action)
+
+# Also run immediately to patch before any compilation
+_apply_bluefruit_patches(None, None, env)

platformio.ini

@@ -79,7 +79,9 @@ extends = arduino_base
 platform = nordicnrf52
 platform_packages =
   framework-arduinoadafruitnrf52 @ 1.10700.0
-extra_scripts = create-uf2.py
+extra_scripts = 
+  create-uf2.py
+  arch/nrf52/extra_scripts/patch_bluefruit.py
 build_flags = ${arduino_base.build_flags}
   -D NRF52_PLATFORM
   -D LFS_NO_ASSERT=1

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");
+    }
   }
 }
 
-void SerialBLEInterface::begin(const char* device_name, uint32_t pin_code) {
+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();
-  
-}
-
-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();
-  }
+  bleuart.setRxCallback(onBleUartRX);
 
-  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();
+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];
+    }
+  }
+}
 
+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;
 }
 
-// ---------- public methods
+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() { 
+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");
-
-#ifdef RAK_BOARD
-  Bluefruit.disconnect(Bluefruit.connHandle());
-#else
-  uint16_t conn_id;
-  if (Bluefruit.getConnectedHandles(&conn_id, 1) > 0) {
-    Bluefruit.disconnect(conn_id);
-  }
-#endif
+  BLE_DEBUG_PRINTLN("SerialBLEInterface: disable");
 
-  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];
+  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");
+        }
+      }
     }
-  } 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 (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;
 }
 
+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;
+  return _isDeviceConnected && Bluefruit.connected() > 0;
+}
+
+bool SerialBLEInterface::isWriteBusy() const {
+  return send_queue_len >= (FRAME_QUEUE_SIZE - 1);
 }

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;