diff --git a/src/sp140/ble/ble_core.cpp b/src/sp140/ble/ble_core.cpp index 9c420d7..71fcba9 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,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(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(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(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) { @@ -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, diff --git a/src/sp140/main.cpp b/src/sp140/main.cpp index c84a863..41d762f 100644 --- a/src/sp140/main.cpp +++ b/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");