diff --git a/src/sp140/ble/ble_core.cpp b/src/sp140/ble/ble_core.cpp index 9ea844b..72d4ae0 100644 --- a/src/sp140/ble/ble_core.cpp +++ b/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,26 @@ 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. The caller stops + // advertising before entering here so we can safely prune stale entries + // without the unbounded remove loop that previously hit BLE_HS_EBUSY. + for (size_t i = NimBLEDevice::getWhiteListCount(); i > 0; --i) { + const NimBLEAddress addr = NimBLEDevice::getWhiteListAddress(i - 1); + if (!NimBLEDevice::isBonded(addr)) { + NimBLEDevice::whiteListRemove(addr); + } + } + // Add bonded addresses that aren't already present. Skip addresses already + // on the whitelist because NimBLE's whiteListAdd() touches the controller + // whitelist immediately. 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 +86,27 @@ 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(NimBLEDevice::getNumBonds()); const bool allowOpenAdvertising = pairingModeActive; + + // Stop advertising BEFORE modifying the whitelist — the BLE controller + // rejects whitelist changes while advertising with a whitelist filter + // (rc=524 / BLE_HS_EBUSY), which previously caused an infinite loop. + 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; @@ -122,8 +137,6 @@ bool startAdvertising(NimBLEServer *server) { // Flutter app's `startScan()` filters for CONFIG_SERVICE_UUID. adv.addServiceUUID(NimBLEUUID(CONFIG_SERVICE_UUID)); - auto *advertising = server->getAdvertising(); - advertising->stop(); advertising->removeAll(); const bool configured = advertising->setInstanceData(kExtAdvInstance, adv); const bool started = configured && advertising->start(kExtAdvInstance); @@ -133,11 +146,6 @@ bool startAdvertising(NimBLEServer *server) { static_cast(bondCount), static_cast(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) { @@ -224,7 +232,9 @@ class BleServerConnectionCallbacks : public NimBLEServerCallbacks { // Restart immediately, then let the watchdog keep retrying forever if this // first post-disconnect start does not stick. - startAdvertising(server); + if (!pairingModeTransitionActive) { + startAdvertising(server); + } } void onAuthenticationComplete(NimBLEConnInfo &connInfo) override { @@ -245,6 +255,26 @@ class BleServerConnectionCallbacks : public NimBLEServerCallbacks { if (!connInfo.isEncrypted() || !connInfo.isBonded()) { USBSerial.println("[BLE] Rejecting untrusted BLE session"); + + // If this address had a bond in NVS the phone must have forgotten its + // side of the keys. Remove the stale bond so the controller stops + // advertising to a device that can never reconnect. + const NimBLEAddress identityAddr = connInfo.getIdAddress(); + const NimBLEAddress peerAddr = connInfo.getAddress(); + bool removedBond = NimBLEDevice::deleteBond(identityAddr); + if (!removedBond && identityAddr != peerAddr) { + removedBond = NimBLEDevice::deleteBond(peerAddr); + } + if (removedBond) { + NimBLEDevice::whiteListRemove(identityAddr); + if (identityAddr != peerAddr) { + NimBLEDevice::whiteListRemove(peerAddr); + } + USBSerial.printf( + "[BLE] Removed stale bond for id=%s peer=%s (phone-side forget)\n", + identityAddr.toString().c_str(), peerAddr.toString().c_str()); + } + if (pServer != nullptr) { pServer->disconnect(connInfo.getConnHandle()); } @@ -337,7 +367,44 @@ void restartBLEAdvertising() { } void enterBLEPairingMode() { + pairingModeTransitionActive = true; + + // Single-bond model: clear any existing bond so the next device that + // connects becomes the sole bonded peer. + if (deviceConnected && pServer != nullptr && + connectedHandle != BLE_HS_CONN_HANDLE_NONE) { + pServer->disconnect(connectedHandle); + vTaskDelay(pdMS_TO_TICKS(100)); + } + + // Stop advertising before modifying bonds — NimBLE's bond deletion can + // fail (EBUSY) if the controller is actively advertising. + if (pServer != nullptr) { + auto *advertising = pServer->getAdvertising(); + if (advertising != nullptr) { + advertising->stop(); + } + } + vTaskDelay(pdMS_TO_TICKS(50)); + + const int bondCount = NimBLEDevice::getNumBonds(); + bool cleared = false; + if (bondCount > 0) { + cleared = NimBLEDevice::deleteAllBonds(); + if (!cleared) { + // Retry individual bond deletion as fallback + 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, diff --git a/src/sp140/main.cpp b/src/sp140/main.cpp index d9fdd00..ecea3d7 100644 --- a/src/sp140/main.cpp +++ b/src/sp140/main.cpp @@ -75,6 +75,7 @@ int8_t bmsCS = MCP_CS; #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 BLE_PAIRING_HOLD_MS 10000 // 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,8 +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 && - !pairingHoldHandled) { + holdDuration < BLE_PAIRING_HOLD_MS && !pairingHoldHandled) { perfModeSwitch(); lastButtonState = buttonState; continue; @@ -928,38 +926,10 @@ 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 && + // Long hold while disarmed: 10s = enter BLE pairing mode + // Clears existing bonds and opens advertising for 60s. + if (currentState == DISARMED && currentHoldTime >= BLE_PAIRING_HOLD_MS && !pairingHoldHandled) { - // Tier 1: Enter pairing mode (open advertising for 60s) enterBLEPairingMode(); pulseVibeMotor(); USBSerial.println("[BLE] Pairing mode activated via button hold");