From 116f747fc3af93aeb7c710bd9702ab19b55d637e Mon Sep 17 00:00:00 2001 From: liquidraver <504870+liquidraver@users.noreply.github.com> Date: Sun, 28 Dec 2025 14:21:16 +0100 Subject: [PATCH] unify nRF and ESP32 BLE interface implementations --- platformio.ini | 3 + src/helpers/SerialBLECommon.h | 195 ++++++++ src/helpers/esp32/SerialBLEInterface.cpp | 569 ++++++++++++++++------- src/helpers/esp32/SerialBLEInterface.h | 99 ++-- src/helpers/nrf52/SerialBLEInterface.cpp | 309 ++++++++---- src/helpers/nrf52/SerialBLEInterface.h | 48 +- 6 files changed, 840 insertions(+), 383 deletions(-) create mode 100644 src/helpers/SerialBLECommon.h diff --git a/platformio.ini b/platformio.ini index 75d37e869..b99ec912e 100644 --- a/platformio.ini +++ b/platformio.ini @@ -60,6 +60,9 @@ extra_scripts = merge-bin.py build_flags = ${arduino_base.build_flags} ; -D ESP32_CPU_FREQ=80 ; change it to your need build_src_filter = ${arduino_base.build_src_filter} +lib_deps = + ${arduino_base.lib_deps} + h2zero/NimBLE-Arduino @ ^2.3.7 [esp32_ota] lib_deps = diff --git a/src/helpers/SerialBLECommon.h b/src/helpers/SerialBLECommon.h new file mode 100644 index 000000000..96fcd568d --- /dev/null +++ b/src/helpers/SerialBLECommon.h @@ -0,0 +1,195 @@ +#pragma once + +#include "BaseSerialInterface.h" +#include + +// Units: interval=1.25ms, timeout=10ms +#define BLE_MIN_CONN_INTERVAL 12 +#define BLE_MAX_CONN_INTERVAL 36 +#define BLE_SLAVE_LATENCY 3 +#define BLE_CONN_SUP_TIMEOUT 500 + +// Sync mode: higher throughput (min 15ms for Apple compliance) +#define BLE_SYNC_MIN_CONN_INTERVAL 12 +#define BLE_SYNC_MAX_CONN_INTERVAL 24 +#define BLE_SYNC_SLAVE_LATENCY 0 +#define BLE_SYNC_CONN_SUP_TIMEOUT 300 + +#define BLE_SYNC_INACTIVITY_TIMEOUT_MS 5000 + +// Units: advertising interval=0.625ms +// ESP randomly chooses between 32 and 338 +// max seems slow, but we can wait a few seconds for it to connect, worth the battery +#define BLE_ADV_INTERVAL_MIN 32 +#define BLE_ADV_INTERVAL_MAX 338 +#define BLE_ADV_FAST_TIMEOUT 30 + +#define BLE_HEALTH_CHECK_INTERVAL 10000 +#define BLE_RETRY_THROTTLE_MS 250 +#define BLE_MIN_SEND_INTERVAL_MS 8 +#define BLE_RX_DRAIN_BUF_SIZE 32 + +#define SERVICE_UUID "6E400001-B5A3-F393-E0A9-E50E24DCCA9E" +#define CHARACTERISTIC_UUID_RX "6E400002-B5A3-F393-E0A9-E50E24DCCA9E" +#define CHARACTERISTIC_UUID_TX "6E400003-B5A3-F393-E0A9-E50E24DCCA9E" + +#define BLE_SYNC_FRAME_SIZE_THRESHOLD 40 +#define BLE_SYNC_LARGE_FRAME_COUNT_THRESHOLD 3 +#define BLE_SYNC_LARGE_FRAME_WINDOW_MS 1500 + +#define BLE_CONN_HANDLE_INVALID 0xFFFF + +// BLE specific MTU target, ESP can do more, but we don't need it, so stay at max nRF52 +#define BLE_MAX_MTU 247 + +// ESP needs this to set manually, nRF52 handles it automatically +#define BLE_DLE_MAX_TX_OCTETS 251 +#define BLE_DLE_MAX_TX_TIME_US 2120 + +// nRF only, NimBLE cannot set TX power on ESP, so ESP is fixed 0dBm +#ifndef BLE_TX_POWER +#define BLE_TX_POWER 4 +#endif + +struct SerialBLEFrame { + uint8_t len; + uint8_t buf[MAX_FRAME_SIZE]; +}; + +// 12 is adequate for most use cases +#define FRAME_QUEUE_SIZE 12 + +struct CircularFrameQueue { + SerialBLEFrame frames[FRAME_QUEUE_SIZE]; + uint8_t head; + uint8_t tail; + uint8_t count; + + void init() { + head = 0; + tail = 0; + count = 0; + } + + bool isEmpty() const { + return count == 0; + } + + bool isFull() const { + return count >= FRAME_QUEUE_SIZE; + } + + SerialBLEFrame* peekFront() { + if (isEmpty()) return nullptr; + return &frames[tail]; + } + + SerialBLEFrame* getWriteSlot() { + if (isFull()) return nullptr; + return &frames[head]; + } + + void push() { + if (!isFull()) { + head = (head + 1) % FRAME_QUEUE_SIZE; + count++; + } + } + + void pop() { + if (!isEmpty()) { + tail = (tail + 1) % FRAME_QUEUE_SIZE; + count--; + } + } + + uint8_t size() const { + return count; + } +}; + +#if BLE_DEBUG_LOGGING && ARDUINO + #include + #define BLE_DEBUG_PRINT(F, ...) Serial.printf("BLE: " F, ##__VA_ARGS__) + #define BLE_DEBUG_PRINTLN(F, ...) Serial.printf("BLE: " F "\n", ##__VA_ARGS__) +#else + #define BLE_DEBUG_PRINT(...) {} + #define BLE_DEBUG_PRINTLN(...) {} +#endif + +class SerialBLEInterfaceBase : public BaseSerialInterface { +protected: + bool _isEnabled; + bool _isDeviceConnected; + uint16_t _conn_handle; + unsigned long _last_health_check; + unsigned long _last_retry_attempt; + unsigned long _last_send_time; + unsigned long _last_activity_time; + bool _sync_mode; + bool _conn_param_update_pending; + uint8_t _large_frame_count; + unsigned long _large_frame_window_start; + + CircularFrameQueue send_queue; + CircularFrameQueue recv_queue; + + void clearTransferState() { + send_queue.init(); + recv_queue.init(); + _last_retry_attempt = 0; + _last_send_time = 0; + _last_activity_time = 0; + _sync_mode = false; + _conn_param_update_pending = false; + _large_frame_count = 0; + _large_frame_window_start = 0; + } + + void popSendQueue() { + send_queue.pop(); + } + + void popRecvQueue() { + recv_queue.pop(); + } + + bool noteFrameActivity(unsigned long now, size_t frame_len) { + if (frame_len < BLE_SYNC_FRAME_SIZE_THRESHOLD) { + return false; + } + + _last_activity_time = now; + + if (_large_frame_window_start == 0 || + (now - _large_frame_window_start) > BLE_SYNC_LARGE_FRAME_WINDOW_MS) { + _large_frame_count = 1; + _large_frame_window_start = now; + } else if (_large_frame_count < 255) { + _large_frame_count++; + } + + return (!_sync_mode && _large_frame_count >= BLE_SYNC_LARGE_FRAME_COUNT_THRESHOLD); + } + + bool isWriteBusyCommon() const { + return send_queue.size() >= (FRAME_QUEUE_SIZE * 2 / 3); + } + + void initCommonState() { + _isEnabled = false; + _isDeviceConnected = false; + _conn_handle = BLE_CONN_HANDLE_INVALID; + _last_health_check = 0; + _last_retry_attempt = 0; + _last_send_time = 0; + _last_activity_time = 0; + _sync_mode = false; + _conn_param_update_pending = false; + _large_frame_count = 0; + _large_frame_window_start = 0; + send_queue.init(); + recv_queue.init(); + } +}; + diff --git a/src/helpers/esp32/SerialBLEInterface.cpp b/src/helpers/esp32/SerialBLEInterface.cpp index 7ec937238..a932b6496 100644 --- a/src/helpers/esp32/SerialBLEInterface.cpp +++ b/src/helpers/esp32/SerialBLEInterface.cpp @@ -1,242 +1,459 @@ #include "SerialBLEInterface.h" +#include "../SerialBLECommon.h" +#include +#include + +void SerialBLEInterface::onConnect(NimBLEServer* pServer, NimBLEConnInfo& connInfo) { + BLE_DEBUG_PRINTLN("SerialBLEInterface: connected conn_handle=%d", connInfo.getConnHandle()); + if (pServer->getConnectedCount() > 1) { + bool success = pServer->disconnect(connInfo.getConnHandle()); + if (!success) { + BLE_DEBUG_PRINTLN("SerialBLEInterface: failed to disconnect second connection"); + } else { + BLE_DEBUG_PRINTLN("SerialBLEInterface: rejecting second connection, already have %d connection", pServer->getConnectedCount() - 1); + } + return; + } + + _conn_handle = connInfo.getConnHandle(); + _isDeviceConnected = false; + clearBuffers(); // this seems redundant, but there were edge cases where stuff stuck in the buffers on rapid disconnect-connects +} -// See the following for generating UUIDs: -// https://www.uuidgenerator.net/ - -#define SERVICE_UUID "6E400001-B5A3-F393-E0A9-E50E24DCCA9E" // UART service UUID -#define CHARACTERISTIC_UUID_RX "6E400002-B5A3-F393-E0A9-E50E24DCCA9E" -#define CHARACTERISTIC_UUID_TX "6E400003-B5A3-F393-E0A9-E50E24DCCA9E" - -#define ADVERT_RESTART_DELAY 1000 // millis +void SerialBLEInterface::onDisconnect(NimBLEServer* pServer, NimBLEConnInfo& connInfo, int reason) { +#if BLE_DEBUG_LOGGING + const char* initiator; + if (reason == 0x16) { + initiator = "local"; + } else if (reason == 0x08) { + initiator = "timeout"; + } else { + initiator = "remote"; + } + BLE_DEBUG_PRINTLN("SerialBLEInterface: disconnected conn_handle=%d reason=0x%02X (initiated by %s)", + connInfo.getConnHandle(), reason, initiator); +#endif + if (_conn_handle == connInfo.getConnHandle()) { + _conn_handle = BLE_CONN_HANDLE_INVALID; + _isDeviceConnected = false; + clearBuffers(); + _last_health_check = millis(); + + if (_isEnabled) { + NimBLEAdvertising* pAdvertising = NimBLEDevice::getAdvertising(); + if (pAdvertising && !pAdvertising->isAdvertising()) { + pAdvertising->start(0); + BLE_DEBUG_PRINTLN("SerialBLEInterface: restarting advertising on disconnect"); + } + } + } +} -void SerialBLEInterface::begin(const char* device_name, uint32_t pin_code) { - _pin_code = pin_code; +void SerialBLEInterface::onAuthenticationComplete(NimBLEConnInfo& connInfo) { + BLE_DEBUG_PRINTLN("SerialBLEInterface: onAuthenticationComplete conn_handle=%d", connInfo.getConnHandle()); + if (isValidConnection(connInfo.getConnHandle(), true)) { + if (!connInfo.isAuthenticated()) { + BLE_DEBUG_PRINTLN("SerialBLEInterface: authentication failed, disconnecting"); + disconnect(); + return; + } + + BLE_DEBUG_PRINTLN("SerialBLEInterface: authentication successful"); + _isDeviceConnected = true; + + if (pServer) { + pServer->updateConnParams(connInfo.getConnHandle(), + BLE_MIN_CONN_INTERVAL, + BLE_MAX_CONN_INTERVAL, + BLE_SLAVE_LATENCY, + BLE_CONN_SUP_TIMEOUT); + BLE_DEBUG_PRINTLN("Connection parameter update requested: %u-%ums interval, latency=%u, %ums timeout", + BLE_MIN_CONN_INTERVAL * 5 / 4, + BLE_MAX_CONN_INTERVAL * 5 / 4, + BLE_SLAVE_LATENCY, + BLE_CONN_SUP_TIMEOUT * 10); + + extern int ble_gap_set_data_len(uint16_t conn_handle, uint16_t tx_octets, uint16_t tx_time); + int err_code = ble_gap_set_data_len(connInfo.getConnHandle(), BLE_DLE_MAX_TX_OCTETS, BLE_DLE_MAX_TX_TIME_US); + if (err_code == 0) { + BLE_DEBUG_PRINTLN("Data Length Extension requested: max_tx_octets=%u, max_tx_time=%uus", + BLE_DLE_MAX_TX_OCTETS, BLE_DLE_MAX_TX_TIME_US); + } else { + BLE_DEBUG_PRINTLN("Failed to request Data Length Extension: %d", err_code); + } + } + } else { + BLE_DEBUG_PRINTLN("onAuthenticationComplete: ignoring stale/duplicate callback"); + } +} - // Create the BLE Device - BLEDevice::init(device_name); - BLEDevice::setSecurityCallbacks(this); - BLEDevice::setMTU(MAX_FRAME_SIZE); +void SerialBLEInterface::onConnParamsUpdate(NimBLEConnInfo& connInfo) { + uint16_t interval_ms = connInfo.getConnInterval() * 5 / 4; + uint16_t timeout_ms = connInfo.getConnTimeout() * 10; + BLE_DEBUG_PRINTLN("SerialBLEInterface: onConnParamsUpdate conn_handle=%d, interval=%ums, latency=%u, timeout=%ums", + connInfo.getConnHandle(), + interval_ms, + connInfo.getConnLatency(), + timeout_ms); + + if (connInfo.getConnHandle() != _conn_handle) { + return; + } + + uint16_t interval = connInfo.getConnInterval(); + uint16_t latency = connInfo.getConnLatency(); + uint16_t timeout = connInfo.getConnTimeout(); + + if (latency == BLE_SYNC_SLAVE_LATENCY && + timeout == BLE_SYNC_CONN_SUP_TIMEOUT && + interval >= BLE_SYNC_MIN_CONN_INTERVAL && + interval <= BLE_SYNC_MAX_CONN_INTERVAL) { + if (!_sync_mode) { + BLE_DEBUG_PRINTLN("Sync mode confirmed by connection parameters"); + _sync_mode = true; + _last_activity_time = millis(); + } + } else if (latency == BLE_SLAVE_LATENCY && + timeout == BLE_CONN_SUP_TIMEOUT && + interval >= BLE_MIN_CONN_INTERVAL && + interval <= BLE_MAX_CONN_INTERVAL) { + if (_sync_mode) { + BLE_DEBUG_PRINTLN("Default mode confirmed by connection parameters"); + _sync_mode = false; + } + } + _conn_param_update_pending = false; +} - BLESecurity sec; - sec.setStaticPIN(pin_code); - sec.setAuthenticationMode(ESP_LE_AUTH_REQ_SC_MITM_BOND); +void SerialBLEInterface::onWrite(NimBLECharacteristic* pCharacteristic, NimBLEConnInfo& connInfo) { + if (!isConnected()) { + return; + } + + if (connInfo.getConnHandle() != _conn_handle) { + BLE_DEBUG_PRINTLN("onWrite: ignoring write from stale connection handle %d (expected %d)", + connInfo.getConnHandle(), _conn_handle); + return; + } + + auto val = pCharacteristic->getValue(); + size_t len = val.length(); + + BLE_DEBUG_PRINTLN("onWrite: len=%u, queue=%u", (unsigned)len, (unsigned)recv_queue.size()); + + if (len > MAX_FRAME_SIZE) { + BLE_DEBUG_PRINTLN("onWrite: frame too big, len=%u", (unsigned)len); + return; + } + + if (recv_queue.isFull()) { + BLE_DEBUG_PRINTLN("onWrite: recv queue full, dropping data"); + return; + } + + const uint8_t* data = val.data(); + if (data == nullptr && len > 0) { + BLE_DEBUG_PRINTLN("onWrite: invalid data pointer"); + return; + } + + SerialBLEFrame* frame = recv_queue.getWriteSlot(); + if (frame) { + frame->len = len; + memcpy(frame->buf, data, len); + recv_queue.push(); + } - //BLEDevice::setPower(ESP_PWR_LVL_N8); + unsigned long now = millis(); + if (noteFrameActivity(now, len)) { + requestSyncModeConnection(); + } +} - // Create the BLE Server - pServer = BLEDevice::createServer(); +void SerialBLEInterface::begin(const char* device_name, uint32_t pin_code) { + NimBLEDevice::init(device_name); + NimBLEDevice::setSecurityAuth(true, true, true); + NimBLEDevice::setSecurityIOCap(BLE_HS_IO_DISPLAY_ONLY); + NimBLEDevice::setSecurityPasskey(pin_code); + NimBLEDevice::setMTU(BLE_MAX_MTU); + + pServer = NimBLEDevice::createServer(); + if (!pServer) { + BLE_DEBUG_PRINTLN("SerialBLEInterface: failed to create BLE server"); + return; + } pServer->setCallbacks(this); - // Create the BLE Service pService = pServer->createService(SERVICE_UUID); + if (!pService) { + BLE_DEBUG_PRINTLN("SerialBLEInterface: failed to create BLE service"); + return; + } - // Create a BLE Characteristic - pTxCharacteristic = pService->createCharacteristic(CHARACTERISTIC_UUID_TX, BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_NOTIFY); - pTxCharacteristic->setAccessPermissions(ESP_GATT_PERM_READ_ENC_MITM); - pTxCharacteristic->addDescriptor(new BLE2902()); + pTxCharacteristic = pService->createCharacteristic( + CHARACTERISTIC_UUID_TX, + NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::NOTIFY | NIMBLE_PROPERTY::READ_AUTHEN | NIMBLE_PROPERTY::READ_ENC + ); + if (!pTxCharacteristic) { + BLE_DEBUG_PRINTLN("SerialBLEInterface: failed to create TX characteristic"); + return; + } - BLECharacteristic * pRxCharacteristic = pService->createCharacteristic(CHARACTERISTIC_UUID_RX, BLECharacteristic::PROPERTY_WRITE); - pRxCharacteristic->setAccessPermissions(ESP_GATT_PERM_WRITE_ENC_MITM); + pRxCharacteristic = pService->createCharacteristic( + CHARACTERISTIC_UUID_RX, + NIMBLE_PROPERTY::WRITE | NIMBLE_PROPERTY::WRITE_NR | NIMBLE_PROPERTY::WRITE_AUTHEN | NIMBLE_PROPERTY::WRITE_ENC + ); + if (!pRxCharacteristic) { + BLE_DEBUG_PRINTLN("SerialBLEInterface: failed to create RX characteristic"); + return; + } pRxCharacteristic->setCallbacks(this); - pServer->getAdvertising()->addServiceUUID(SERVICE_UUID); -} - -// -------- BLESecurityCallbacks methods + if (!pService->start()) { + BLE_DEBUG_PRINTLN("SerialBLEInterface: failed to start BLE service"); + return; + } -uint32_t SerialBLEInterface::onPassKeyRequest() { - BLE_DEBUG_PRINTLN("onPassKeyRequest()"); - return _pin_code; + NimBLEAdvertising* pAdvertising = NimBLEDevice::getAdvertising(); + pAdvertising->setConnectableMode(BLE_GAP_CONN_MODE_UND); + pAdvertising->setDiscoverableMode(BLE_GAP_DISC_MODE_GEN); + pAdvertising->addTxPower(); + pAdvertising->addServiceUUID(SERVICE_UUID); + pAdvertising->setMinInterval(BLE_ADV_INTERVAL_MIN); + pAdvertising->setMaxInterval(BLE_ADV_INTERVAL_MAX); + pAdvertising->setPreferredParams(BLE_MIN_CONN_INTERVAL, BLE_MAX_CONN_INTERVAL); + pAdvertising->enableScanResponse(true); + + NimBLEAdvertisementData scanRespData; + scanRespData.setName(device_name); + pAdvertising->setScanResponseData(scanRespData); } -void SerialBLEInterface::onPassKeyNotify(uint32_t pass_key) { - BLE_DEBUG_PRINTLN("onPassKeyNotify(%u)", pass_key); +void SerialBLEInterface::clearBuffers() { + clearTransferState(); } -bool SerialBLEInterface::onConfirmPIN(uint32_t pass_key) { - BLE_DEBUG_PRINTLN("onConfirmPIN(%u)", pass_key); +bool SerialBLEInterface::isValidConnection(uint16_t conn_handle, bool requireWaitingForSecurity) const { + if (_conn_handle != conn_handle) { + return false; + } + if (_conn_handle == BLE_CONN_HANDLE_INVALID) { + return false; + } + if (requireWaitingForSecurity && _isDeviceConnected) { + return false; + } return true; } -bool SerialBLEInterface::onSecurityRequest() { - BLE_DEBUG_PRINTLN("onSecurityRequest()"); - return true; // allow +bool SerialBLEInterface::isAdvertising() const { + NimBLEAdvertising* pAdvertising = NimBLEDevice::getAdvertising(); + return pAdvertising && pAdvertising->isAdvertising(); } -void SerialBLEInterface::onAuthenticationComplete(esp_ble_auth_cmpl_t cmpl) { - if (cmpl.success) { - BLE_DEBUG_PRINTLN(" - SecurityCallback - Authentication Success"); - deviceConnected = true; - } else { - BLE_DEBUG_PRINTLN(" - SecurityCallback - Authentication Failure*"); +void SerialBLEInterface::enable() { + if (_isEnabled) return; - //pServer->removePeerDevice(pServer->getConnId(), true); - pServer->disconnect(pServer->getConnId()); - adv_restart_time = millis() + ADVERT_RESTART_DELAY; + if (!pServer) { + BLE_DEBUG_PRINTLN("SerialBLEInterface: enable() failed - pServer is null"); + return; } -} - -// -------- BLEServerCallbacks methods - -void SerialBLEInterface::onConnect(BLEServer* pServer) { -} - -void SerialBLEInterface::onConnect(BLEServer* pServer, esp_ble_gatts_cb_param_t *param) { - BLE_DEBUG_PRINTLN("onConnect(), conn_id=%d, mtu=%d", param->connect.conn_id, pServer->getPeerMTU(param->connect.conn_id)); - last_conn_id = param->connect.conn_id; -} - -void SerialBLEInterface::onMtuChanged(BLEServer* pServer, esp_ble_gatts_cb_param_t* param) { - BLE_DEBUG_PRINTLN("onMtuChanged(), mtu=%d", pServer->getPeerMTU(param->mtu.conn_id)); -} -void SerialBLEInterface::onDisconnect(BLEServer* pServer) { - BLE_DEBUG_PRINTLN("onDisconnect()"); - if (_isEnabled) { - adv_restart_time = millis() + ADVERT_RESTART_DELAY; + _isEnabled = true; + clearBuffers(); + _last_health_check = millis(); - // loop() will detect this on next loop, and set deviceConnected to false + NimBLEAdvertising* pAdvertising = NimBLEDevice::getAdvertising(); + if (pAdvertising) { + pAdvertising->start(0); + BLE_DEBUG_PRINTLN("SerialBLEInterface: enable() - advertising started"); } } -// -------- BLECharacteristicCallbacks methods - -void SerialBLEInterface::onWrite(BLECharacteristic* pCharacteristic, esp_ble_gatts_cb_param_t* param) { - uint8_t* rxValue = pCharacteristic->getData(); - int len = pCharacteristic->getLength(); - - if (len > MAX_FRAME_SIZE) { - BLE_DEBUG_PRINTLN("ERROR: onWrite(), frame too big, len=%d", len); - } else if (recv_queue_len >= FRAME_QUEUE_SIZE) { - BLE_DEBUG_PRINTLN("ERROR: onWrite(), recv_queue is full!"); - } else { - recv_queue[recv_queue_len].len = len; - memcpy(recv_queue[recv_queue_len].buf, rxValue, len); - recv_queue_len++; +void SerialBLEInterface::disconnect() { + if (_conn_handle != BLE_CONN_HANDLE_INVALID && pServer) { + pServer->disconnect(_conn_handle); } } -// ---------- public methods - -void SerialBLEInterface::enable() { - if (_isEnabled) return; - - _isEnabled = true; - clearBuffers(); - - // Start the service - pService->start(); - - // Start advertising - - //pServer->getAdvertising()->setMinInterval(500); - //pServer->getAdvertising()->setMaxInterval(1000); - - pServer->getAdvertising()->start(); - adv_restart_time = 0; -} - void SerialBLEInterface::disable() { _isEnabled = false; + BLE_DEBUG_PRINTLN("SerialBLEInterface: disable"); - BLE_DEBUG_PRINTLN("SerialBLEInterface::disable"); - - pServer->getAdvertising()->stop(); - pServer->disconnect(last_conn_id); - pService->stop(); - oldDeviceConnected = deviceConnected = false; - adv_restart_time = 0; + disconnect(); + NimBLEAdvertising* pAdvertising = NimBLEDevice::getAdvertising(); + if (pAdvertising) { + pAdvertising->stop(); + } + _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 (deviceConnected && len > 0) { - if (send_queue_len >= FRAME_QUEUE_SIZE) { + bool connected = isConnected(); + if (connected && len > 0) { + if (send_queue.isFull()) { BLE_DEBUG_PRINTLN("writeFrame(), send_queue is full!"); return 0; } - send_queue[send_queue_len].len = len; // add to send queue - memcpy(send_queue[send_queue_len].buf, src, len); - send_queue_len++; - - return len; + SerialBLEFrame* frame = send_queue.getWriteSlot(); + if (frame) { + frame->len = len; + memcpy(frame->buf, src, len); + send_queue.push(); + 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(); - pTxCharacteristic->setValue(send_queue[0].buf, send_queue[0].len); - pTxCharacteristic->notify(); - - 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.isEmpty()) { + if (!isConnected()) { + BLE_DEBUG_PRINTLN("writeBytes: connection invalid, clearing send queue"); + send_queue.init(); + } else { + unsigned long now = millis(); + bool throttle_active = (_last_retry_attempt > 0 && (now - _last_retry_attempt) < BLE_RETRY_THROTTLE_MS); + bool send_interval_ok = (_last_send_time == 0 || (now - _last_send_time) >= BLE_MIN_SEND_INTERVAL_MS); + + if (!throttle_active && send_interval_ok && pTxCharacteristic) { + SerialBLEFrame* frame_to_send = send_queue.peekFront(); + if (frame_to_send) { + pTxCharacteristic->setValue(frame_to_send->buf, frame_to_send->len); + bool success = pTxCharacteristic->notify(); + + if (success) { + BLE_DEBUG_PRINTLN("writeBytes: sz=%u, hdr=%u", (unsigned)frame_to_send->len, (unsigned)frame_to_send->buf[0]); + _last_retry_attempt = 0; + _last_send_time = now; + if (noteFrameActivity(now, frame_to_send->len)) { + requestSyncModeConnection(); + } + popSendQueue(); + } else { + if (!isConnected()) { + BLE_DEBUG_PRINTLN("writeBytes failed: connection lost, dropping frame"); + _last_retry_attempt = 0; + _last_send_time = 0; + popSendQueue(); + } else { + BLE_DEBUG_PRINTLN("writeBytes failed (buffer full), keeping frame for retry, queue=%u", (unsigned)send_queue.size()); + _last_retry_attempt = now; + } + } + } + } } } - - if (recv_queue_len > 0) { // check recv queue - size_t len = recv_queue[0].len; // take from top of queue - memcpy(dest, recv_queue[0].buf, len); - - BLE_DEBUG_PRINTLN("readBytes: sz=%d, hdr=%d", len, (uint32_t) dest[0]); - - recv_queue_len--; - for (int i = 0; i < recv_queue_len; i++) { // delete top item from queue - recv_queue[i] = recv_queue[i + 1]; + + if (!recv_queue.isEmpty()) { + SerialBLEFrame* frame = recv_queue.peekFront(); + if (frame) { + size_t len = frame->len; + memcpy(dest, frame->buf, len); + + BLE_DEBUG_PRINTLN("readBytes: sz=%u, hdr=%u", (unsigned)len, (unsigned)dest[0]); + + popRecvQueue(); + return len; } - return len; } - - if (pServer->getConnectedCount() == 0) deviceConnected = false; - - if (deviceConnected != oldDeviceConnected) { - if (!deviceConnected) { // disconnecting - clearBuffers(); - - BLE_DEBUG_PRINTLN("SerialBLEInterface -> disconnecting..."); - - //pServer->getAdvertising()->setMinInterval(500); - //pServer->getAdvertising()->setMaxInterval(1000); - - adv_restart_time = millis() + ADVERT_RESTART_DELAY; - } else { - BLE_DEBUG_PRINTLN("SerialBLEInterface -> stopping advertising"); - BLE_DEBUG_PRINTLN("SerialBLEInterface -> connecting..."); - // connecting - // do stuff here on connecting - pServer->getAdvertising()->stop(); - adv_restart_time = 0; + + unsigned long now = millis(); + if (isConnected() && _sync_mode && _last_activity_time > 0 && + send_queue.isEmpty() && recv_queue.isEmpty()) { + if (now - _last_activity_time >= BLE_SYNC_INACTIVITY_TIMEOUT_MS) { + requestDefaultConnection(); } - oldDeviceConnected = deviceConnected; } - - if (adv_restart_time && millis() >= adv_restart_time) { - if (pServer->getConnectedCount() == 0) { - BLE_DEBUG_PRINTLN("SerialBLEInterface -> re-starting advertising"); - pServer->getAdvertising()->start(); // re-Start advertising + + 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"); + NimBLEAdvertising* pAdvertising = NimBLEDevice::getAdvertising(); + if (pAdvertising) { + pAdvertising->start(0); + } + } } - adv_restart_time = 0; } + return 0; } bool SerialBLEInterface::isConnected() const { - return deviceConnected; //pServer != NULL && pServer->getConnectedCount() > 0; + return _isDeviceConnected && _conn_handle != BLE_CONN_HANDLE_INVALID && pServer && pServer->getConnectedCount() > 0; +} + +bool SerialBLEInterface::isWriteBusy() const { + return isWriteBusyCommon(); +} + +void SerialBLEInterface::requestSyncModeConnection() { + if (!pServer || !isConnected()) { + return; + } + + if (_sync_mode) { + return; + } + + if (_conn_param_update_pending) { + return; + } + _conn_param_update_pending = true; + + BLE_DEBUG_PRINTLN("Requesting sync mode connection: %u-%ums interval, latency=%u, %ums timeout", + BLE_SYNC_MIN_CONN_INTERVAL * 5 / 4, + BLE_SYNC_MAX_CONN_INTERVAL * 5 / 4, + BLE_SYNC_SLAVE_LATENCY, + BLE_SYNC_CONN_SUP_TIMEOUT * 10); + + pServer->updateConnParams(_conn_handle, + BLE_SYNC_MIN_CONN_INTERVAL, + BLE_SYNC_MAX_CONN_INTERVAL, + BLE_SYNC_SLAVE_LATENCY, + BLE_SYNC_CONN_SUP_TIMEOUT); + BLE_DEBUG_PRINTLN("Sync mode connection parameter update requested successfully"); +} + +void SerialBLEInterface::requestDefaultConnection() { + if (!pServer || !isConnected()) { + return; + } + + if (!_sync_mode) { + return; + } + + if (!send_queue.isEmpty() || !recv_queue.isEmpty()) { + return; + } + + if (_conn_param_update_pending) { + return; + } + _conn_param_update_pending = true; + + BLE_DEBUG_PRINTLN("Requesting default connection: %u-%ums interval, latency=%u, %ums timeout", + BLE_MIN_CONN_INTERVAL * 5 / 4, + BLE_MAX_CONN_INTERVAL * 5 / 4, + BLE_SLAVE_LATENCY, + BLE_CONN_SUP_TIMEOUT * 10); + + pServer->updateConnParams(_conn_handle, + BLE_MIN_CONN_INTERVAL, + BLE_MAX_CONN_INTERVAL, + BLE_SLAVE_LATENCY, + BLE_CONN_SUP_TIMEOUT); + BLE_DEBUG_PRINTLN("Default connection parameter update requested successfully"); } diff --git a/src/helpers/esp32/SerialBLEInterface.h b/src/helpers/esp32/SerialBLEInterface.h index 29ad897ae..8ea5238eb 100644 --- a/src/helpers/esp32/SerialBLEInterface.h +++ b/src/helpers/esp32/SerialBLEInterface.h @@ -1,85 +1,46 @@ #pragma once -#include "../BaseSerialInterface.h" -#include -#include -#include -#include - -class SerialBLEInterface : public BaseSerialInterface, BLESecurityCallbacks, BLEServerCallbacks, BLECharacteristicCallbacks { - BLEServer *pServer; - BLEService *pService; - BLECharacteristic * pTxCharacteristic; - bool deviceConnected; - bool oldDeviceConnected; - bool _isEnabled; - uint16_t last_conn_id; - uint32_t _pin_code; - unsigned long _last_write; - unsigned long adv_restart_time; - - struct Frame { - uint8_t len; - uint8_t buf[MAX_FRAME_SIZE]; - }; - - #define FRAME_QUEUE_SIZE 4 - int recv_queue_len; - Frame recv_queue[FRAME_QUEUE_SIZE]; - int send_queue_len; - Frame send_queue[FRAME_QUEUE_SIZE]; - - void clearBuffers() { recv_queue_len = 0; send_queue_len = 0; } - -protected: - // BLESecurityCallbacks methods - uint32_t onPassKeyRequest() override; - void onPassKeyNotify(uint32_t pass_key) override; - bool onConfirmPIN(uint32_t pass_key) override; - bool onSecurityRequest() override; - void onAuthenticationComplete(esp_ble_auth_cmpl_t cmpl) override; - - // BLEServerCallbacks methods - void onConnect(BLEServer* pServer) override; - void onConnect(BLEServer* pServer, esp_ble_gatts_cb_param_t *param) override; - void onMtuChanged(BLEServer* pServer, esp_ble_gatts_cb_param_t* param) override; - void onDisconnect(BLEServer* pServer) override; - - // BLECharacteristicCallbacks methods - void onWrite(BLECharacteristic* pCharacteristic, esp_ble_gatts_cb_param_t* param) override; +#include "../SerialBLECommon.h" +#include +#include +#include + +class SerialBLEInterface : public SerialBLEInterfaceBase, + public NimBLEServerCallbacks, + public NimBLECharacteristicCallbacks { + NimBLEServer* pServer; + NimBLEService* pService; + NimBLECharacteristic* pTxCharacteristic; + NimBLECharacteristic* pRxCharacteristic; + + void clearBuffers(); + bool isValidConnection(uint16_t conn_handle, bool requireWaitingForSecurity = false) const; + bool isAdvertising() const; + void requestSyncModeConnection(); + void requestDefaultConnection(); + + void onConnect(NimBLEServer* pServer, NimBLEConnInfo& connInfo) override; + void onDisconnect(NimBLEServer* pServer, NimBLEConnInfo& connInfo, int reason) override; + void onAuthenticationComplete(NimBLEConnInfo& connInfo) override; + void onConnParamsUpdate(NimBLEConnInfo& connInfo) override; + void onWrite(NimBLECharacteristic* pCharacteristic, NimBLEConnInfo& connInfo) override; public: SerialBLEInterface() { - pServer = NULL; - pService = NULL; - deviceConnected = false; - oldDeviceConnected = false; - adv_restart_time = 0; - _isEnabled = false; - _last_write = 0; - last_conn_id = 0; - send_queue_len = recv_queue_len = 0; + pServer = nullptr; + pService = nullptr; + pTxCharacteristic = nullptr; + pRxCharacteristic = nullptr; + initCommonState(); } 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; }; - -#if BLE_DEBUG_LOGGING && ARDUINO - #include - #define BLE_DEBUG_PRINT(F, ...) Serial.printf("BLE: " F, ##__VA_ARGS__) - #define BLE_DEBUG_PRINTLN(F, ...) Serial.printf("BLE: " F "\n", ##__VA_ARGS__) -#else - #define BLE_DEBUG_PRINT(...) {} - #define BLE_DEBUG_PRINTLN(...) {} -#endif diff --git a/src/helpers/nrf52/SerialBLEInterface.cpp b/src/helpers/nrf52/SerialBLEInterface.cpp index eb1e90bb7..93484e862 100644 --- a/src/helpers/nrf52/SerialBLEInterface.cpp +++ b/src/helpers/nrf52/SerialBLEInterface.cpp @@ -1,45 +1,50 @@ #include "SerialBLEInterface.h" +#include "../SerialBLECommon.h" #include #include #include "ble_gap.h" #include "ble_hci.h" -// Magic numbers came from actual testing -#define BLE_HEALTH_CHECK_INTERVAL 10000 // Advertising watchdog check every 10 seconds -#define BLE_RETRY_THROTTLE_MS 250 // Throttle retries to 250ms when queue buildup detected - -// Connection parameters (units: interval=1.25ms, timeout=10ms) -#define BLE_MIN_CONN_INTERVAL 12 // 15ms -#define BLE_MAX_CONN_INTERVAL 24 // 30ms -#define BLE_SLAVE_LATENCY 4 -#define BLE_CONN_SUP_TIMEOUT 200 // 2000ms - -// Advertising parameters -#define BLE_ADV_INTERVAL_MIN 32 // 20ms (units: 0.625ms) -#define BLE_ADV_INTERVAL_MAX 244 // 152.5ms (units: 0.625ms) -#define BLE_ADV_FAST_TIMEOUT 30 // seconds - -// RX drain buffer size for overflow protection -#define BLE_RX_DRAIN_BUF_SIZE 32 - -static SerialBLEInterface* instance = nullptr; +SerialBLEInterface* SerialBLEInterface::instance = nullptr; void SerialBLEInterface::onConnect(uint16_t connection_handle) { BLE_DEBUG_PRINTLN("SerialBLEInterface: connected handle=0x%04X", connection_handle); if (instance) { + if (Bluefruit.connected() > 1) { + uint32_t err_code = sd_ble_gap_disconnect(connection_handle, BLE_HCI_LOCAL_HOST_TERMINATED_CONNECTION); + if (err_code != NRF_SUCCESS) { + BLE_DEBUG_PRINTLN("SerialBLEInterface: failed to disconnect second connection: 0x%08lX", err_code); + } else { + BLE_DEBUG_PRINTLN("SerialBLEInterface: rejecting second connection, already have %d connection", Bluefruit.connected() - 1); + } + return; + } + instance->_conn_handle = connection_handle; instance->_isDeviceConnected = false; - instance->clearBuffers(); + instance->clearBuffers(); // this seems redundant, but there were edge cases where stuff stuck in the buffers on rapid disconnect-connects } } void SerialBLEInterface::onDisconnect(uint16_t connection_handle, uint8_t reason) { - BLE_DEBUG_PRINTLN("SerialBLEInterface: disconnected handle=0x%04X reason=%u", connection_handle, reason); +#if BLE_DEBUG_LOGGING + const char* initiator; + if (reason == BLE_HCI_LOCAL_HOST_TERMINATED_CONNECTION) { + initiator = "local"; + } else if (reason == BLE_HCI_CONNECTION_TIMEOUT) { + initiator = "timeout"; + } else { + initiator = "remote"; + } + BLE_DEBUG_PRINTLN("SerialBLEInterface: disconnected handle=0x%04X reason=0x%02X (initiated by %s)", + connection_handle, reason, initiator); +#endif if (instance) { if (instance->_conn_handle == connection_handle) { instance->_conn_handle = BLE_CONN_HANDLE_INVALID; instance->_isDeviceConnected = false; instance->clearBuffers(); + instance->_last_health_check = millis(); } } } @@ -50,9 +55,6 @@ void SerialBLEInterface::onSecured(uint16_t connection_handle) { 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 = BLE_MIN_CONN_INTERVAL; conn_params.max_conn_interval = BLE_MAX_CONN_INTERVAL; @@ -62,10 +64,10 @@ void SerialBLEInterface::onSecured(uint16_t connection_handle) { 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: %u-%ums interval, latency=%u, %ums timeout", - conn_params.min_conn_interval * 5 / 4, // convert to ms (1.25ms units) + conn_params.min_conn_interval * 5 / 4, conn_params.max_conn_interval * 5 / 4, conn_params.slave_latency, - conn_params.conn_sup_timeout * 10); // convert to ms (10ms units) + conn_params.conn_sup_timeout * 10); } else { BLE_DEBUG_PRINTLN("Failed to request connection parameter update: %lu", err_code); } @@ -101,7 +103,39 @@ void SerialBLEInterface::onPairingComplete(uint16_t connection_handle, uint8_t a void SerialBLEInterface::onBLEEvent(ble_evt_t* evt) { if (!instance) return; - if (evt->header.evt_id == BLE_GAP_EVT_CONN_PARAM_UPDATE_REQUEST) { + if (evt->header.evt_id == BLE_GAP_EVT_CONN_PARAM_UPDATE) { + uint16_t conn_handle = evt->evt.gap_evt.conn_handle; + if (instance->isValidConnection(conn_handle)) { + ble_gap_conn_params_t* params = &evt->evt.gap_evt.params.conn_param_update.conn_params; + uint16_t min_interval = params->min_conn_interval; + uint16_t max_interval = params->max_conn_interval; + uint16_t latency = params->slave_latency; + uint16_t timeout = params->conn_sup_timeout; + + BLE_DEBUG_PRINTLN("CONN_PARAM_UPDATE: handle=0x%04X, min_interval=%u, max_interval=%u, latency=%u, timeout=%u", + conn_handle, min_interval, max_interval, latency, timeout); + + if (latency == BLE_SYNC_SLAVE_LATENCY && + timeout == BLE_SYNC_CONN_SUP_TIMEOUT && + min_interval >= BLE_SYNC_MIN_CONN_INTERVAL && + max_interval <= BLE_SYNC_MAX_CONN_INTERVAL) { + if (!instance->_sync_mode) { + BLE_DEBUG_PRINTLN("Sync mode confirmed by connection parameters"); + instance->_sync_mode = true; + instance->_last_activity_time = millis(); + } + } else if (latency == BLE_SLAVE_LATENCY && + timeout == BLE_CONN_SUP_TIMEOUT && + min_interval >= BLE_MIN_CONN_INTERVAL && + max_interval <= BLE_MAX_CONN_INTERVAL) { + if (instance->_sync_mode) { + BLE_DEBUG_PRINTLN("Default mode confirmed by connection parameters"); + instance->_sync_mode = false; + } + } + instance->_conn_param_update_pending = false; + } + } else 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", @@ -129,12 +163,9 @@ void SerialBLEInterface::begin(const char* device_name, uint32_t pin_code) { char charpin[20]; 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.begin(); - // Connection interval units: 1.25ms, supervision timeout units: 10ms ble_gap_conn_params_t ppcp_params; ppcp_params.min_conn_interval = BLE_MIN_CONN_INTERVAL; ppcp_params.max_conn_interval = BLE_MAX_CONN_INTERVAL; @@ -144,10 +175,10 @@ void SerialBLEInterface::begin(const char* device_name, uint32_t pin_code) { uint32_t err_code = sd_ble_gap_ppcp_set(&ppcp_params); if (err_code == NRF_SUCCESS) { BLE_DEBUG_PRINTLN("PPCP set: %u-%ums interval, latency=%u, %ums timeout", - ppcp_params.min_conn_interval * 5 / 4, // convert to ms (1.25ms units) + ppcp_params.min_conn_interval * 5 / 4, ppcp_params.max_conn_interval * 5 / 4, ppcp_params.slave_latency, - ppcp_params.conn_sup_timeout * 10); // convert to ms (10ms units) + ppcp_params.conn_sup_timeout * 10); } else { BLE_DEBUG_PRINTLN("Failed to set PPCP: %lu", err_code); } @@ -185,30 +216,10 @@ void SerialBLEInterface::begin(const char* device_name, uint32_t pin_code) { } void SerialBLEInterface::clearBuffers() { - send_queue_len = 0; - recv_queue_len = 0; - _last_retry_attempt = 0; + clearTransferState(); bleuart.flush(); } -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]; - } - } -} - -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; @@ -226,6 +237,7 @@ bool SerialBLEInterface::isValidConnection(uint16_t handle, bool requireWaitingF bool SerialBLEInterface::isAdvertising() const { ble_gap_addr_t adv_addr; uint32_t err_code = sd_ble_gap_adv_addr_get(0, &adv_addr); + (void)adv_addr; // address not needed, only return code return (err_code == NRF_SUCCESS); } @@ -241,7 +253,7 @@ void SerialBLEInterface::enable() { void SerialBLEInterface::disconnect() { if (_conn_handle != BLE_CONN_HANDLE_INVALID) { - sd_ble_gap_disconnect(_conn_handle, BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION); + sd_ble_gap_disconnect(_conn_handle, BLE_HCI_LOCAL_HOST_TERMINATED_CONNECTION); } } @@ -262,68 +274,85 @@ size_t SerialBLEInterface::writeFrame(const uint8_t src[], size_t len) { bool connected = isConnected(); if (connected && len > 0) { - if (send_queue_len >= FRAME_QUEUE_SIZE) { + if (send_queue.isFull()) { BLE_DEBUG_PRINTLN("writeFrame(), send_queue is full!"); return 0; } - send_queue[send_queue_len].len = len; - memcpy(send_queue[send_queue_len].buf, src, len); - send_queue_len++; - - return len; + SerialBLEFrame* frame = send_queue.getWriteSlot(); + if (frame) { + frame->len = len; + memcpy(frame->buf, src, len); + send_queue.push(); + return len; + } } return 0; } size_t SerialBLEInterface::checkRecvFrame(uint8_t dest[]) { - if (send_queue_len > 0) { + if (!send_queue.isEmpty()) { if (!isConnected()) { BLE_DEBUG_PRINTLN("writeBytes: connection invalid, clearing send queue"); - send_queue_len = 0; + send_queue.init(); } else { unsigned long now = millis(); bool throttle_active = (_last_retry_attempt > 0 && (now - _last_retry_attempt) < BLE_RETRY_THROTTLE_MS); - - if (!throttle_active) { - 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]); - _last_retry_attempt = 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); - _last_retry_attempt = 0; - shiftSendQueueLeft(); - } else { - if (!isConnected()) { - BLE_DEBUG_PRINTLN("writeBytes failed: connection lost, dropping frame"); + bool send_interval_ok = (_last_send_time == 0 || (now - _last_send_time) >= BLE_MIN_SEND_INTERVAL_MS); + + if (!throttle_active && send_interval_ok) { + SerialBLEFrame* frame_to_send = send_queue.peekFront(); + if (frame_to_send) { + 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]); + _last_retry_attempt = 0; + _last_send_time = now; + if (noteFrameActivity(now, frame_to_send->len)) { + requestSyncModeConnection(); + } + popSendQueue(); + } else if (written > 0) { + BLE_DEBUG_PRINTLN("writeBytes: partial write, sent=%u of %u, dropping corrupted frame", (unsigned)written, (unsigned)frame_to_send->len); _last_retry_attempt = 0; - shiftSendQueueLeft(); + _last_send_time = now; + popSendQueue(); } else { - BLE_DEBUG_PRINTLN("writeBytes failed (buffer full), keeping frame for retry"); - _last_retry_attempt = now; + if (!isConnected()) { + BLE_DEBUG_PRINTLN("writeBytes failed: connection lost, dropping frame"); + _last_retry_attempt = 0; + popSendQueue(); + } else { + BLE_DEBUG_PRINTLN("writeBytes failed (buffer full), keeping frame for retry"); + _last_retry_attempt = now; + } } } } } } - 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; + if (!recv_queue.isEmpty()) { + SerialBLEFrame* frame = recv_queue.peekFront(); + if (frame) { + size_t len = frame->len; + memcpy(dest, frame->buf, len); + + BLE_DEBUG_PRINTLN("readBytes: sz=%u, hdr=%u", (unsigned)len, (unsigned)dest[0]); + + popRecvQueue(); + 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 (isConnected() && _sync_mode && _last_activity_time > 0 && + send_queue.isEmpty() && recv_queue.isEmpty()) { + if (now - _last_activity_time >= BLE_SYNC_INACTIVITY_TIMEOUT_MS) { + requestDefaultConnection(); + } + } + if (_isEnabled && !isConnected() && _conn_handle == BLE_CONN_HANDLE_INVALID) { if (now - _last_health_check >= BLE_HEALTH_CHECK_INTERVAL) { _last_health_check = now; @@ -351,7 +380,7 @@ void SerialBLEInterface::onBleUartRX(uint16_t conn_handle) { } while (instance->bleuart.available() > 0) { - if (instance->recv_queue_len >= FRAME_QUEUE_SIZE) { + if (instance->recv_queue.isFull()) { while (instance->bleuart.available() > 0) { instance->bleuart.read(); } @@ -372,16 +401,102 @@ void SerialBLEInterface::onBleUartRX(uint16_t conn_handle) { } 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++; + SerialBLEFrame* frame = instance->recv_queue.getWriteSlot(); + if (frame) { + frame->len = read_len; + instance->bleuart.readBytes(frame->buf, read_len); + instance->recv_queue.push(); + + unsigned long now = millis(); + if (instance->noteFrameActivity(now, read_len)) { + instance->requestSyncModeConnection(); + } + } } } bool SerialBLEInterface::isConnected() const { - return _isDeviceConnected && Bluefruit.connected() > 0; + return _isDeviceConnected && _conn_handle != BLE_CONN_HANDLE_INVALID && Bluefruit.connected() > 0; } bool SerialBLEInterface::isWriteBusy() const { - return send_queue_len >= (FRAME_QUEUE_SIZE * 2 / 3); + return isWriteBusyCommon(); +} + +void SerialBLEInterface::requestSyncModeConnection() { + if (!isConnected()) { + return; + } + + if (_sync_mode) { + return; + } + + if (_conn_param_update_pending) { + return; + } + _conn_param_update_pending = true; + + BLE_DEBUG_PRINTLN("Requesting sync mode connection: %u-%ums interval, latency=%u, %ums timeout", + BLE_SYNC_MIN_CONN_INTERVAL * 5 / 4, + BLE_SYNC_MAX_CONN_INTERVAL * 5 / 4, + BLE_SYNC_SLAVE_LATENCY, + BLE_SYNC_CONN_SUP_TIMEOUT * 10); + + ble_gap_conn_params_t conn_params; + conn_params.min_conn_interval = BLE_SYNC_MIN_CONN_INTERVAL; + conn_params.max_conn_interval = BLE_SYNC_MAX_CONN_INTERVAL; + conn_params.slave_latency = BLE_SYNC_SLAVE_LATENCY; + conn_params.conn_sup_timeout = BLE_SYNC_CONN_SUP_TIMEOUT; + + uint32_t err_code = sd_ble_gap_conn_param_update(_conn_handle, &conn_params); + if (err_code == NRF_SUCCESS) { + BLE_DEBUG_PRINTLN("Sync mode connection parameter update requested successfully"); + } else { + _conn_param_update_pending = false; + if (err_code != NRF_ERROR_BUSY) { + BLE_DEBUG_PRINTLN("Failed to request sync mode connection: %lu", err_code); + } + } +} + +void SerialBLEInterface::requestDefaultConnection() { + if (!isConnected()) { + return; + } + + if (!_sync_mode) { + return; + } + + if (!send_queue.isEmpty() || !recv_queue.isEmpty()) { + return; + } + + if (_conn_param_update_pending) { + return; + } + _conn_param_update_pending = true; + + BLE_DEBUG_PRINTLN("Requesting default connection: %u-%ums interval, latency=%u, %ums timeout", + BLE_MIN_CONN_INTERVAL * 5 / 4, + BLE_MAX_CONN_INTERVAL * 5 / 4, + BLE_SLAVE_LATENCY, + BLE_CONN_SUP_TIMEOUT * 10); + + ble_gap_conn_params_t conn_params; + conn_params.min_conn_interval = BLE_MIN_CONN_INTERVAL; + conn_params.max_conn_interval = BLE_MAX_CONN_INTERVAL; + conn_params.slave_latency = BLE_SLAVE_LATENCY; + conn_params.conn_sup_timeout = BLE_CONN_SUP_TIMEOUT; + + uint32_t err_code = sd_ble_gap_conn_param_update(_conn_handle, &conn_params); + if (err_code == NRF_SUCCESS) { + BLE_DEBUG_PRINTLN("Default connection parameter update requested successfully"); + } else { + _conn_param_update_pending = false; + if (err_code != NRF_ERROR_BUSY) { + BLE_DEBUG_PRINTLN("Failed to request default connection: %lu", err_code); + } + } } diff --git a/src/helpers/nrf52/SerialBLEInterface.h b/src/helpers/nrf52/SerialBLEInterface.h index 25968d78f..4b7053830 100644 --- a/src/helpers/nrf52/SerialBLEInterface.h +++ b/src/helpers/nrf52/SerialBLEInterface.h @@ -1,38 +1,19 @@ #pragma once -#include "../BaseSerialInterface.h" +#include "../SerialBLECommon.h" #include -#ifndef BLE_TX_POWER -#define BLE_TX_POWER 4 -#endif - -class SerialBLEInterface : public BaseSerialInterface { +class SerialBLEInterface : public SerialBLEInterfaceBase { BLEUart bleuart; - bool _isEnabled; - bool _isDeviceConnected; - uint16_t _conn_handle; - unsigned long _last_health_check; - unsigned long _last_retry_attempt; - - struct Frame { - uint8_t len; - uint8_t buf[MAX_FRAME_SIZE]; - }; - #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]; + static SerialBLEInterface* instance; void clearBuffers(); - void shiftSendQueueLeft(); - void shiftRecvQueueLeft(); bool isValidConnection(uint16_t handle, bool requireWaitingForSecurity = false) const; bool isAdvertising() const; + void requestSyncModeConnection(); + void requestDefaultConnection(); + static void onConnect(uint16_t connection_handle); static void onDisconnect(uint16_t connection_handle, uint8_t reason); static void onSecured(uint16_t connection_handle); @@ -43,13 +24,7 @@ class SerialBLEInterface : public BaseSerialInterface { public: SerialBLEInterface() { - _isEnabled = false; - _isDeviceConnected = false; - _conn_handle = BLE_CONN_HANDLE_INVALID; - _last_health_check = 0; - _last_retry_attempt = 0; - send_queue_len = 0; - recv_queue_len = 0; + initCommonState(); } void begin(const char* device_name, uint32_t pin_code); @@ -62,12 +37,3 @@ class SerialBLEInterface : public BaseSerialInterface { size_t writeFrame(const uint8_t src[], size_t len) override; size_t checkRecvFrame(uint8_t dest[]) override; }; - -#if BLE_DEBUG_LOGGING && ARDUINO - #include - #define BLE_DEBUG_PRINT(F, ...) Serial.printf("BLE: " F, ##__VA_ARGS__) - #define BLE_DEBUG_PRINTLN(F, ...) Serial.printf("BLE: " F "\n", ##__VA_ARGS__) -#else - #define BLE_DEBUG_PRINT(...) {} - #define BLE_DEBUG_PRINTLN(...) {} -#endif