From 1a3f7a7ea985289ed7d2e430e131b1ea4f6739f9 Mon Sep 17 00:00:00 2001 From: liquidraver <504870+liquidraver@users.noreply.github.com> Date: Thu, 4 Dec 2025 11:47:41 +0100 Subject: [PATCH 1/2] Fix BLE semaphore leak in Bluefruit library 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). Co-authored-by: Liam Cottle Co-authored-by: oltaco --- arch/nrf52/extra_scripts/patch_bluefruit.py | 198 ++++++++++++++++++++ platformio.ini | 4 +- 2 files changed, 201 insertions(+), 1 deletion(-) create mode 100644 arch/nrf52/extra_scripts/patch_bluefruit.py diff --git a/arch/nrf52/extra_scripts/patch_bluefruit.py b/arch/nrf52/extra_scripts/patch_bluefruit.py new file mode 100644 index 00000000..b43bffb5 --- /dev/null +++ b/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) diff --git a/platformio.ini b/platformio.ini index 3907cf64..75d37e86 100644 --- a/platformio.ini +++ b/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 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 2/2] 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 +#include +#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;