Refactor BLE pairing, whitelist, and advertising

Add pairingModeTransitionActive to suppress advertising restarts during bond-clear transitions and prevent advertising while the whitelist is being modified. Replace clearWhiteList with syncWhiteListFromBonds that prunes stale whitelist entries and only adds missing bonded addresses, and ensure advertising is stopped before changing whitelist to avoid BLE_HS_EBUSY. Make startAdvertising respect the transition flag and avoid immediate restarts from onDisconnect during transitions. Implement enterBLEPairingMode to safely disconnect, stop advertising, and clear bonds (with retries), and add BLE_PAIRING_HOLD_MS plus related button-handler changes to enter pairing mode on a long hold (removing the previous multi-tier unbond/delete flow). These changes improve robustness of whitelist and pairing operations.

Author: zjwhitehead

src/sp140/ble/ble_core.cpp

@@ -26,6 +26,7 @@ TimerHandle_t gConnTuneTimer = nullptr;
 TimerHandle_t gPairingTimer = nullptr;
 TimerHandle_t gAdvertisingWatchdogTimer = nullptr;
 bool pairingModeActive = false;
+bool pairingModeTransitionActive = false;
 
 // Store the active connection handle for conn param updates
 uint16_t activeConnHandle = 0;
@@ -36,18 +37,24 @@ void stopPairingModeTimer() {
   }
 }
 
-void clearWhiteList() {
-  while (NimBLEDevice::getWhiteListCount() > 0) {
-    NimBLEDevice::whiteListRemove(NimBLEDevice::getWhiteListAddress(0));
-  }
-}
-
 size_t syncWhiteListFromBonds() {
-  clearWhiteList();
+  // Reconcile the whitelist to the current bond store. Advertising must be
+  // stopped before calling this — the BLE controller rejects whitelist changes
+  // while advertising with a filter (rc=524 / BLE_HS_EBUSY). Prune stale
+  // entries first, then add any missing bonded addresses.
+  for (size_t i = NimBLEDevice::getWhiteListCount(); i > 0; --i) {
+    const NimBLEAddress addr = NimBLEDevice::getWhiteListAddress(i - 1);
+    if (!NimBLEDevice::isBonded(addr)) {
+      NimBLEDevice::whiteListRemove(addr);
+    }
+  }
 
   const int bondCount = NimBLEDevice::getNumBonds();
   for (int i = 0; i < bondCount; ++i) {
-    NimBLEDevice::whiteListAdd(NimBLEDevice::getBondedAddress(i));
+    const NimBLEAddress addr = NimBLEDevice::getBondedAddress(i);
+    if (!NimBLEDevice::onWhiteList(addr)) {
+      NimBLEDevice::whiteListAdd(addr);
+    }
   }
 
   return NimBLEDevice::getWhiteListCount();
@@ -77,21 +84,26 @@ void applyPreferredLinkParams(TimerHandle_t timer) {
 }
 
 bool shouldAdvertiseWhilePowered() {
-  return pairingModeActive || NimBLEDevice::getNumBonds() > 0;
+  return !pairingModeTransitionActive &&
+         (pairingModeActive || NimBLEDevice::getNumBonds() > 0);
 }
 
 bool startAdvertising(NimBLEServer *server) {
-  if (server == nullptr) {
+  if (server == nullptr || pairingModeTransitionActive) {
     return false;
   }
 
   const size_t bondCount = static_cast<size_t>(NimBLEDevice::getNumBonds());
   const bool allowOpenAdvertising = pairingModeActive;
+
+  // Stop advertising BEFORE modifying the whitelist — the BLE controller
+  // rejects whitelist changes while advertising with a filter (BLE_HS_EBUSY).
+  auto *advertising = server->getAdvertising();
+  advertising->stop();
+
   const size_t whiteListCount = syncWhiteListFromBonds();
 
   if (!allowOpenAdvertising && bondCount == 0) {
-    auto *advertising = server->getAdvertising();
-    advertising->stop();
     USBSerial.println(
         "[BLE] No bonds present and pairing mode inactive; advertising stopped");
     return false;
@@ -132,8 +144,6 @@ bool startAdvertising(NimBLEServer *server) {
                              static_cast<uint8_t>(allowOpenAdvertising ? 0x01 : 0x00)};
   scanRsp.setManufacturerData(mfrData, sizeof(mfrData));
 
-  auto *advertising = server->getAdvertising();
-  advertising->stop();
   advertising->removeAll();
   const bool configured = advertising->setInstanceData(kExtAdvInstance, adv);
   const bool scanRspConfigured =
@@ -148,11 +158,6 @@ bool startAdvertising(NimBLEServer *server) {
       static_cast<unsigned>(bondCount), static_cast<unsigned>(whiteListCount));
   return started;
 #else
-  auto *advertising = server->getAdvertising();
-
-  // Stop before reconfiguring (safe even if not running)
-  advertising->stop();
-
   // Configure payload once — NimBLE accumulates addServiceUUID calls
   static bool payloadConfigured = false;
   if (!payloadConfigured) {
@@ -252,9 +257,11 @@ class BleServerConnectionCallbacks : public NimBLEServerCallbacks {
 
     USBSerial.printf("Device disconnected reason=%d\n", reason);
 
-    // Restart immediately, then let the watchdog keep retrying forever if this
-    // first post-disconnect start does not stick.
-    startAdvertising(server);
+    // Suppress the immediate advertising restart during a pairing transition —
+    // enterBLEPairingMode() issues its own startAdvertising after clearing bonds.
+    if (!pairingModeTransitionActive) {
+      startAdvertising(server);
+    }
   }
 
   void onAuthenticationComplete(NimBLEConnInfo &connInfo) override {
@@ -380,7 +387,43 @@ void restartBLEAdvertising() {
 }
 
 void enterBLEPairingMode() {
+  // Block advertising restarts (e.g. from onDisconnect) during this transition.
+  pairingModeTransitionActive = true;
+
+  // Single-bond model: disconnect the current peer so we can safely clear bonds.
+  if (deviceConnected && pServer != nullptr &&
+      connectedHandle != BLE_HS_CONN_HANDLE_NONE) {
+    pServer->disconnect(connectedHandle);
+    vTaskDelay(pdMS_TO_TICKS(100));
+  }
+
+  // Stop advertising before modifying bonds — NimBLE rejects bond deletion
+  // while the controller is advertising (BLE_HS_EBUSY).
+  if (pServer != nullptr) {
+    auto *adv = pServer->getAdvertising();
+    if (adv != nullptr) {
+      adv->stop();
+    }
+  }
+  vTaskDelay(pdMS_TO_TICKS(50));
+
+  const int bondCount = NimBLEDevice::getNumBonds();
+  bool cleared = false;
+  if (bondCount > 0) {
+    cleared = NimBLEDevice::deleteAllBonds();
+    if (!cleared) {
+      for (int i = NimBLEDevice::getNumBonds() - 1; i >= 0; --i) {
+        NimBLEDevice::deleteBond(NimBLEDevice::getBondedAddress(i));
+      }
+      cleared = NimBLEDevice::getNumBonds() == 0;
+    }
+  }
+  USBSerial.printf("[BLE] Cleared bonds: %s (was %d, now %d)\n",
+                   cleared ? "OK" : (bondCount == 0 ? "NONE" : "FAILED"),
+                   bondCount, NimBLEDevice::getNumBonds());
+
   pairingModeActive = true;
+  pairingModeTransitionActive = false;
 
   if (gPairingTimer == nullptr) {
     gPairingTimer = xTimerCreate("blePair", kPairingTimeoutTicks, pdFALSE,

src/sp140/main.cpp

@@ -74,7 +74,8 @@ int8_t bmsCS = MCP_CS;
 #define SECOND_HOLD_TIME_MS 2000  // How long to hold on second press to arm
 #define CRUISE_HOLD_TIME_MS 2000
 #define BUTTON_SEQUENCE_TIMEOUT_MS 1500  // Time window for arm/disarm sequence
-#define PERFORMANCE_MODE_HOLD_MS 3000   // Longer hold time for performance mode
+#define PERFORMANCE_MODE_HOLD_MS 3000  // Longer hold time for performance mode
+#define BLE_PAIRING_HOLD_MS 10000      // Hold duration to enter BLE pairing mode
 
 // Throttle control constants moved to inc/sp140/throttle.h
 #define CRUISE_MAX_PERCENTAGE                                                  \
@@ -851,7 +852,6 @@ void buttonHandlerTask(void *parameter) {
   uint32_t lastDebounceTime = 0;
   bool lastButtonState = HIGH;
   bool buttonState;
-  bool unbondHoldHandled = false;
   bool pairingHoldHandled = false;
 
   while (true) {
@@ -865,7 +865,6 @@ void buttonHandlerTask(void *parameter) {
 
         if (buttonState == LOW) {  // Button pressed
           buttonPressed = true;
-          unbondHoldHandled = false;
           pairingHoldHandled = false;
           buttonPressStartTime = currentTime;
           USBSerial.println("Button pressed");
@@ -883,7 +882,7 @@ void buttonHandlerTask(void *parameter) {
           // Disarmed long hold toggles performance mode on release.
           if (currentState == DISARMED &&
               holdDuration >= PERFORMANCE_MODE_HOLD_MS &&
-              holdDuration < 10000 && !unbondHoldHandled &&
+              holdDuration < BLE_PAIRING_HOLD_MS &&
               !pairingHoldHandled) {
             perfModeSwitch();
             lastButtonState = buttonState;
@@ -928,38 +927,9 @@ void buttonHandlerTask(void *parameter) {
       } else if (buttonPressed) {  // Only handle other button actions if we're not in an arm sequence
         uint32_t currentHoldTime = currentTime - buttonPressStartTime;
 
-        // Tiered long hold while disarmed:
-        //   10s = enter BLE pairing mode (single vibration)
-        //   20s = delete all bonds (double vibration + reboot)
-        if (currentState == DISARMED && currentHoldTime >= 20000 &&
-            !unbondHoldHandled) {
-          // Tier 2: Delete all bonds
-          const bool deleted = NimBLEDevice::deleteAllBonds();
-          USBSerial.printf("[BLE] Delete all bonds: %s\n",
-                           deleted ? "OK" : "FAILED");
-          if (deviceConnected && pServer != nullptr &&
-              connectedHandle != BLE_HS_CONN_HANDLE_NONE) {
-            pServer->disconnect(connectedHandle);
-            vTaskDelay(pdMS_TO_TICKS(40));
-          }
-          pulseVibeMotor();
-          vTaskDelay(pdMS_TO_TICKS(300));
-          pulseVibeMotor();
-          USBSerial.println("[BLE] Bonds cleared. Rebooting with BLE locked "
-                            "until pairing mode is reopened...");
-          vTaskDelay(pdMS_TO_TICKS(500));
-          diagnosticsMarkPlannedRestart(
-              PlannedRestartReason::BLE_UNBOND_REBOOT);
-          ESP.restart();
-          unbondHoldHandled = true;
-          buttonPressed = false;
-          buttonPressStartTime = currentTime;
-          continue;
-        }
-
-        if (currentState == DISARMED && currentHoldTime >= 10000 &&
-            !pairingHoldHandled) {
-          // Tier 1: Enter pairing mode (open advertising for 60s)
+        // Long hold while disarmed: enter BLE pairing mode (clears bonds).
+        if (currentState == DISARMED &&
+            currentHoldTime >= BLE_PAIRING_HOLD_MS && !pairingHoldHandled) {
           enterBLEPairingMode();
           pulseVibeMotor();
           USBSerial.println("[BLE] Pairing mode activated via button hold");