diff --git a/platformio.ini b/platformio.ini index f296f83821..724d69438a 100644 --- a/platformio.ini +++ b/platformio.ini @@ -62,8 +62,13 @@ monitor_filters = esp32_exception_decoder extra_scripts = merge-bin.py build_flags = ${arduino_base.build_flags} -D ESP32_PLATFORM + -D CONFIG_BT_NIMBLE_ROLE_CENTRAL_DISABLED + -D CONFIG_BT_NIMBLE_ROLE_OBSERVER_DISABLED + -D CONFIG_BT_NIMBLE_MAX_CONNECTIONS=1 ; -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.5.1 [esp32_ota] lib_deps = diff --git a/src/helpers/SerialBLECommon.h b/src/helpers/SerialBLECommon.h new file mode 100644 index 0000000000..58eaa6eac1 --- /dev/null +++ b/src/helpers/SerialBLECommon.h @@ -0,0 +1,95 @@ +#pragma once + +#include "BaseSerialInterface.h" +#include +#include + +// Nordic UART service +#define BLE_UART_SERVICE_UUID "6E400001-B5A3-F393-E0A9-E50E24DCCA9E" +#define BLE_UART_RX_UUID "6E400002-B5A3-F393-E0A9-E50E24DCCA9E" +#define BLE_UART_TX_UUID "6E400003-B5A3-F393-E0A9-E50E24DCCA9E" + +// 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 + +// Units: advertising interval=0.625ms +#define BLE_ADV_INTERVAL_MIN 32 +#define BLE_ADV_INTERVAL_MAX 338 +#define BLE_ADV_FAST_TIMEOUT 30 // seconds + +#define BLE_MAX_MTU 247 // nRF52 max +#define BLE_DLE_MAX_TX_OCTETS 251 + +#define BLE_FRAME_QUEUE_SIZE 2048 // bytes: frames are packed by size, so a burst of small commands fits + +// name is IN/OUT: "@@MAC" is replaced by the address in hex, printed addr[0] first (or addr[5] first if reversed) +inline void makeBLEName(char* dev_name, size_t size, const char* prefix, char* name, const uint8_t addr[6], bool reversed) { + if (strcmp(name, "@@MAC") == 0) { + for (int i = 0; i < 6; i++) { + sprintf(&name[i * 2], "%02X", addr[reversed ? 5 - i : i]); + } + } + snprintf(dev_name, size, "%s%s", prefix, name); +} + +struct SerialBLEFrame { + uint8_t len; + const uint8_t* buf; +}; + +// single producer / single consumer ring, so the BLE task may push while the main loop pops (and clears). +// Frames are stored as [len][bytes] and never wrap, so peek() hands out one contiguous buffer. +template +class SerialBLEFrameQueue { + static_assert(N > 2 * (MAX_FRAME_SIZE + 1), "queue too small"); // any frame fits once empty + static const uint8_t WRAP = 0xFF; // as a length: rest of the buffer unused, continue at 0 + uint8_t _buf[N]; + volatile uint16_t _head = 0, _tail = 0; // read and write offsets, equal when empty + +public: + void clear() { _head = _tail; } + bool isEmpty() const { return _head == _tail; } + uint16_t used() const { uint16_t h = _head, t = _tail; return t >= h ? t - h : N - h + t; } + + // caller ensures len <= MAX_FRAME_SIZE + bool push(const uint8_t* src, size_t len) { + uint16_t h = _head, t = _tail, need = len + 1, at = t; + if (t < h) { + if (need >= h - t) return false; // a full ring must not look empty + } else if (N - t < need + (h == 0)) { + if (need >= h) return false; + at = 0; + } + _buf[at] = len; + memcpy(&_buf[at + 1], src, len); + if (at != t) _buf[t] = WRAP; + __sync_synchronize(); + _tail = (at + need) % N; + return true; + } + SerialBLEFrame peek() const { + uint16_t h = _head; + if (_buf[h] == WRAP) h = 0; + return { _buf[h], &_buf[h + 1] }; + } + void pop() { + uint16_t h = _head; + if (h == _tail) return; + if (_buf[h] == WRAP) h = 0; + h = (h + 1 + _buf[h]) % N; + __sync_synchronize(); + _head = h; + } +}; + +#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/esp32/SerialBLEInterface.cpp b/src/helpers/esp32/SerialBLEInterface.cpp index 6371cf3353..db7d19efda 100644 --- a/src/helpers/esp32/SerialBLEInterface.cpp +++ b/src/helpers/esp32/SerialBLEInterface.cpp @@ -1,156 +1,137 @@ #include "SerialBLEInterface.h" #include "esp_mac.h" - -// 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 +#include + +extern "C" void ble_gatts_set_clt_cfg_perm_flags(uint8_t flags); // NimBLE host, missing from its public headers + +#define ADVERT_CHECK_INTERVAL 1000 // millis +#define BLE_RETRY_THROTTLE_MS 10 // back off when the stack is out of TX buffers + +// bonds made by the old Bluedroid stack are useless to NimBLE, free their NVS space +static void eraseBluedroidBonds() { + nvs_handle_t h; + size_t used = 0; + if (nvs_open("bt_config.conf", NVS_READONLY, &h) != ESP_OK) return; + nvs_get_used_entry_count(h, &used); + nvs_close(h); + if (used > 0 && nvs_open("bt_config.conf", NVS_READWRITE, &h) == ESP_OK) { + nvs_erase_all(h); + nvs_commit(h); + nvs_close(h); + } +} void SerialBLEInterface::begin(const char* prefix, char* name, uint32_t pin_code) { - _pin_code = pin_code; - - if (strcmp(name, "@@MAC") == 0) { - uint8_t addr[8]; - memset(addr, 0, sizeof(addr)); - esp_efuse_mac_get_default(addr); - sprintf(name, "%02X%02X%02X%02X%02X%02X", // modify (IN-OUT param) - addr[5], addr[4], addr[3], addr[2], addr[1], addr[0]); - } + uint8_t addr[8]; + memset(addr, 0, sizeof(addr)); + esp_efuse_mac_get_default(addr); char dev_name[32+16]; - sprintf(dev_name, "%s%s", prefix, name); - - // Create the BLE Device - BLEDevice::init(dev_name); - BLEDevice::setSecurityCallbacks(this); - BLEDevice::setMTU(MAX_FRAME_SIZE); - - BLESecurity sec; - sec.setStaticPIN(pin_code); - sec.setAuthenticationMode(ESP_LE_AUTH_REQ_SC_MITM_BOND); - - //BLEDevice::setPower(ESP_PWR_LVL_N8); - - // Create the BLE Server - pServer = BLEDevice::createServer(); - pServer->setCallbacks(this); - - // Create the BLE Service - pService = pServer->createService(SERVICE_UUID); - - // 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()); - - BLECharacteristic * pRxCharacteristic = pService->createCharacteristic(CHARACTERISTIC_UUID_RX, BLECharacteristic::PROPERTY_WRITE); - pRxCharacteristic->setAccessPermissions(ESP_GATT_PERM_WRITE_ENC_MITM); + makeBLEName(dev_name, sizeof(dev_name), prefix, name, addr, true); + + eraseBluedroidBonds(); + + NimBLEDevice::init(dev_name); + NimBLEDevice::setMTU(BLE_MAX_MTU); + NimBLEDevice::setSecurityAuth(true, true, true); // bonding, MITM, secure connections + NimBLEDevice::setSecurityIOCap(BLE_HS_IO_DISPLAY_ONLY); + NimBLEDevice::setSecurityPasskey(pin_code); + // phones use private addresses, so the identity key is needed to recognise them on reconnect + NimBLEDevice::setSecurityInitKey(BLE_SM_PAIR_KEY_DIST_ENC | BLE_SM_PAIR_KEY_DIST_ID); + NimBLEDevice::setSecurityRespKey(BLE_SM_PAIR_KEY_DIST_ENC | BLE_SM_PAIR_KEY_DIST_ID); + + // like Bluefruit on nRF52: subscribing needs an authenticated link, so the phone's first request is + // rejected with Insufficient Authentication and the phone starts pairing (Apple guidelines 11.10) + ble_gatts_set_clt_cfg_perm_flags(BLE_ATT_F_READ | BLE_ATT_F_WRITE | BLE_ATT_F_WRITE_ENC | BLE_ATT_F_WRITE_AUTHEN); + + pServer = NimBLEDevice::createServer(); + pServer->setCallbacks(this, false); + + NimBLEService* pService = pServer->createService(BLE_UART_SERVICE_UUID); + pTxCharacteristic = pService->createCharacteristic(BLE_UART_TX_UUID, + NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::READ_ENC | NIMBLE_PROPERTY::READ_AUTHEN | NIMBLE_PROPERTY::NOTIFY); + NimBLECharacteristic* pRxCharacteristic = pService->createCharacteristic(BLE_UART_RX_UUID, + NIMBLE_PROPERTY::WRITE | NIMBLE_PROPERTY::WRITE_NR | NIMBLE_PROPERTY::WRITE_ENC | NIMBLE_PROPERTY::WRITE_AUTHEN); pRxCharacteristic->setCallbacks(this); + pTxCharacteristic->setCallbacks(this); // for onSubscribe() - pServer->getAdvertising()->addServiceUUID(SERVICE_UUID); + NimBLEAdvertising* pAdvertising = NimBLEDevice::getAdvertising(); + pAdvertising->addServiceUUID(BLE_UART_SERVICE_UUID); + pAdvertising->addTxPower(); + 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); + pAdvertising->setName(dev_name); } -// -------- BLESecurityCallbacks methods +// -------- NimBLEServerCallbacks methods -uint32_t SerialBLEInterface::onPassKeyRequest() { - BLE_DEBUG_PRINTLN("onPassKeyRequest()"); - return _pin_code; +void SerialBLEInterface::onConnect(NimBLEServer* pServer, NimBLEConnInfo& connInfo) { + BLE_DEBUG_PRINTLN("onConnect(), conn_handle=%d", connInfo.getConnHandle()); + _conn_handle = connInfo.getConnHandle(); + pServer->setDataLen(_conn_handle, BLE_DLE_MAX_TX_OCTETS); // before the MTU exchange (Apple guidelines 11.11) + // don't rely on the phone to ask; once settled (even at 23) frames too big for it can be dropped + auto done = [](uint16_t, const ble_gatt_error*, uint16_t, void* arg) { ((SerialBLEInterface*)arg)->_mtu_done = true; return 0; }; + if (ble_gattc_exchange_mtu(_conn_handle, done, this) != 0) _mtu_done = true; // eg. the phone already did it } -void SerialBLEInterface::onPassKeyNotify(uint32_t pass_key) { - BLE_DEBUG_PRINTLN("onPassKeyNotify(%u)", pass_key); -} - -bool SerialBLEInterface::onConfirmPIN(uint32_t pass_key) { - BLE_DEBUG_PRINTLN("onConfirmPIN(%u)", pass_key); - return true; +void SerialBLEInterface::onDisconnect(NimBLEServer* pServer, NimBLEConnInfo& connInfo, int reason) { + BLE_DEBUG_PRINTLN("onDisconnect(), reason=0x%02X", reason); + deviceConnected = false; + _subscribed = false; + _mtu_done = false; + _conn_handle = BLE_HS_CONN_HANDLE_NONE; } -bool SerialBLEInterface::onSecurityRequest() { - BLE_DEBUG_PRINTLN("onSecurityRequest()"); - return true; // allow +void SerialBLEInterface::onMTUChange(uint16_t MTU, NimBLEConnInfo& connInfo) { + BLE_DEBUG_PRINTLN("onMTUChange(), mtu=%d", MTU); } -void SerialBLEInterface::onAuthenticationComplete(esp_ble_auth_cmpl_t cmpl) { - if (cmpl.success) { - BLE_DEBUG_PRINTLN(" - SecurityCallback - Authentication Success"); +void SerialBLEInterface::onAuthenticationComplete(NimBLEConnInfo& connInfo) { + if (connInfo.isEncrypted() && connInfo.isAuthenticated()) { + BLE_DEBUG_PRINTLN("onAuthenticationComplete() - success"); + _conn_handle = connInfo.getConnHandle(); // bonded peers can encrypt before onConnect() arrives deviceConnected = true; + pServer->updateConnParams(_conn_handle, BLE_MIN_CONN_INTERVAL, BLE_MAX_CONN_INTERVAL, BLE_SLAVE_LATENCY, BLE_CONN_SUP_TIMEOUT); } else { - BLE_DEBUG_PRINTLN(" - SecurityCallback - Authentication Failure*"); - - //pServer->removePeerDevice(pServer->getConnId(), true); - pServer->disconnect(pServer->getConnId()); - adv_restart_time = millis() + ADVERT_RESTART_DELAY; + BLE_DEBUG_PRINTLN("onAuthenticationComplete() - failure"); + pServer->disconnect(connInfo); } } -// -------- BLEServerCallbacks methods +// -------- NimBLECharacteristicCallbacks methods -void SerialBLEInterface::onConnect(BLEServer* pServer) { -} +void SerialBLEInterface::onWrite(NimBLECharacteristic* pCharacteristic, NimBLEConnInfo& connInfo) { + NimBLEAttValue value = pCharacteristic->getValue(); -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()"); - deviceConnected = false; - if (_isEnabled) { - adv_restart_time = millis() + ADVERT_RESTART_DELAY; + if (value.size() > MAX_FRAME_SIZE) { + BLE_DEBUG_PRINTLN("ERROR: onWrite(), frame too big, len=%d", value.size()); + } else if (!recv_queue.push(value.data(), value.size())) { + BLE_DEBUG_PRINTLN("ERROR: onWrite(), recv_queue is full!"); } } -// -------- 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 { - Frame frame = {}; - frame.len = len; - memcpy(frame.buf, rxValue, len); - - if (xQueueSend(recv_queue, &frame, 0) != pdTRUE) { - BLE_DEBUG_PRINTLN("ERROR: onWrite(), recv_queue is full!"); - } - } +void SerialBLEInterface::onSubscribe(NimBLECharacteristic* pCharacteristic, NimBLEConnInfo& connInfo, uint16_t subValue) { + BLE_DEBUG_PRINTLN("onSubscribe(), subValue=%d", subValue); + _subscribed = subValue & 1; } // ---------- public methods void SerialBLEInterface::clearBuffers() { - xQueueReset(recv_queue); - send_queue_len = 0; + recv_queue.clear(); + send_queue.clear(); + _last_retry = 0; } -void SerialBLEInterface::enable() { +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; + pServer->advertiseOnDisconnect(true); + NimBLEDevice::startAdvertising(); } void SerialBLEInterface::disable() { @@ -158,11 +139,12 @@ void SerialBLEInterface::disable() { BLE_DEBUG_PRINTLN("SerialBLEInterface::disable"); - pServer->getAdvertising()->stop(); - pServer->disconnect(last_conn_id); - pService->stop(); - oldDeviceConnected = deviceConnected = false; - adv_restart_time = 0; + pServer->advertiseOnDisconnect(false); + NimBLEDevice::stopAdvertising(); + if (_conn_handle != BLE_HS_CONN_HANDLE_NONE) { + pServer->disconnect(_conn_handle); + } + deviceConnected = false; } size_t SerialBLEInterface::writeFrame(const uint8_t src[], size_t len) { @@ -172,80 +154,70 @@ size_t SerialBLEInterface::writeFrame(const uint8_t src[], size_t len) { } if (deviceConnected && len > 0) { - if (send_queue_len >= FRAME_QUEUE_SIZE) { + if (!send_queue.push(src, len)) { 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; } 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? + return send_queue.used() >= BLE_FRAME_QUEUE_SIZE * 2 / 3; } 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 (!deviceConnected) { + if (oldDeviceConnected) { + clearBuffers(); + oldDeviceConnected = false; } + // NimBLE drops the disconnect event of a link lost before onConnect() (eg. stale bond), so advertising never restarts. + // Workaround for NimBLE-Arduino <= 2.5.1 only: fixed on master by h2zero/NimBLE-Arduino#1104 (see #1082), + // delete this check, ADVERT_CHECK_INTERVAL and _last_adv_check once we're on a release that includes it. + if (_isEnabled && millis() - _last_adv_check >= ADVERT_CHECK_INTERVAL) { + _last_adv_check = millis(); + if (pServer->getConnectedCount() == 0 && !NimBLEDevice::getAdvertising()->isAdvertising()) { + NimBLEDevice::startAdvertising(); + } + } + return 0; } + oldDeviceConnected = true; - Frame frame; - if (xQueueReceive(recv_queue, &frame, 0) == pdTRUE) { - memcpy(dest, frame.buf, frame.len); - BLE_DEBUG_PRINTLN("readBytes: sz=%d, hdr=%d", (uint32_t) frame.len, (uint32_t) dest[0]); - return frame.len; - } - - 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; + unsigned long now = millis(); + if (!send_queue.isEmpty() // first, check send queue + && _subscribed // notify() to a conn handle skips the CCCD check + && (_last_retry == 0 || now - _last_retry >= BLE_RETRY_THROTTLE_MS) + ) { + const SerialBLEFrame& frame = send_queue.peek(); + uint16_t mtu = pServer->getPeerMTU(_conn_handle); + + if (frame.len + 3 > mtu) { + if (_mtu_done) { // else: MTU exchange not done yet, wait for it + BLE_DEBUG_PRINTLN("writeBytes: sz=%d exceeds MTU, dropped", (uint32_t)frame.len); + send_queue.pop(); + } + } else if (pTxCharacteristic->notify(frame.buf, frame.len, _conn_handle)) { + BLE_DEBUG_PRINTLN("writeBytes: sz=%d, hdr=%d", (uint32_t)frame.len, (uint32_t)frame.buf[0]); + _last_retry = 0; + send_queue.pop(); } 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; + _last_retry = now; // stack out of buffers, back off } - 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 - } - adv_restart_time = 0; + if (!recv_queue.isEmpty()) { + size_t len = recv_queue.peek().len; + memcpy(dest, recv_queue.peek().buf, len); + recv_queue.pop(); + BLE_DEBUG_PRINTLN("readBytes: sz=%d, hdr=%d", (uint32_t) len, (uint32_t) dest[0]); + return len; } return 0; } bool SerialBLEInterface::isConnected() const { - return deviceConnected; //pServer != NULL && pServer->getConnectedCount() > 0; + return deviceConnected; } diff --git a/src/helpers/esp32/SerialBLEInterface.h b/src/helpers/esp32/SerialBLEInterface.h index 9fb6adba94..94256c9029 100644 --- a/src/helpers/esp32/SerialBLEInterface.h +++ b/src/helpers/esp32/SerialBLEInterface.h @@ -1,70 +1,48 @@ #pragma once -#include "../BaseSerialInterface.h" -#include -#include -#include -#include -#include -#include +#include "../SerialBLECommon.h" +#include -class SerialBLEInterface : public BaseSerialInterface, BLESecurityCallbacks, BLEServerCallbacks, BLECharacteristicCallbacks { - BLEServer *pServer; - BLEService *pService; - BLECharacteristic * pTxCharacteristic; +class SerialBLEInterface : public BaseSerialInterface, NimBLEServerCallbacks, NimBLECharacteristicCallbacks { + NimBLEServer *pServer; + NimBLECharacteristic *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; + bool _subscribed; + bool _mtu_done; + uint16_t _conn_handle; + unsigned long _last_retry; + unsigned long _last_adv_check; - struct Frame { - uint8_t len; - uint8_t buf[MAX_FRAME_SIZE]; - }; - - #define FRAME_QUEUE_SIZE 4 - StaticQueue_t recv_queue_state; - uint8_t recv_queue_storage[FRAME_QUEUE_SIZE * sizeof(Frame)]; - QueueHandle_t recv_queue; - int send_queue_len; - Frame send_queue[FRAME_QUEUE_SIZE]; + SerialBLEFrameQueue recv_queue; + SerialBLEFrameQueue send_queue; void clearBuffers(); 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; + // NimBLEServerCallbacks methods + void onConnect(NimBLEServer* pServer, NimBLEConnInfo& connInfo) override; + void onDisconnect(NimBLEServer* pServer, NimBLEConnInfo& connInfo, int reason) override; + void onMTUChange(uint16_t MTU, NimBLEConnInfo& connInfo) override; + void onAuthenticationComplete(NimBLEConnInfo& connInfo) override; - // BLECharacteristicCallbacks methods - void onWrite(BLECharacteristic* pCharacteristic, esp_ble_gatts_cb_param_t* param) override; + // NimBLECharacteristicCallbacks methods + void onWrite(NimBLECharacteristic* pCharacteristic, NimBLEConnInfo& connInfo) override; + void onSubscribe(NimBLECharacteristic* pCharacteristic, NimBLEConnInfo& connInfo, uint16_t subValue) override; public: SerialBLEInterface() { pServer = NULL; - pService = NULL; + pTxCharacteristic = NULL; deviceConnected = false; oldDeviceConnected = false; - adv_restart_time = 0; _isEnabled = false; - _last_write = 0; - last_conn_id = 0; - recv_queue = xQueueCreateStatic( - FRAME_QUEUE_SIZE, sizeof(Frame), recv_queue_storage, &recv_queue_state - ); - send_queue_len = 0; + _subscribed = false; + _mtu_done = false; + _last_retry = 0; + _last_adv_check = 0; + _conn_handle = BLE_HS_CONN_HANDLE_NONE; } /** @@ -86,12 +64,3 @@ class SerialBLEInterface : public BaseSerialInterface, BLESecurityCallbacks, BLE 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 6ed16bc86d..3919e87f93 100644 --- a/src/helpers/nrf52/SerialBLEInterface.cpp +++ b/src/helpers/nrf52/SerialBLEInterface.cpp @@ -7,17 +7,7 @@ // 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 +#define BLE_HVN_TX_SLOTS 3 // notification queue size set by configPrphBandwidth(BANDWIDTH_MAX) // RX drain buffer size for overflow protection #define BLE_RX_DRAIN_BUF_SIZE 32 @@ -27,9 +17,15 @@ static 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) { + BLE_DEBUG_PRINTLN("SerialBLEInterface: rejecting second connection"); + sd_ble_gap_disconnect(connection_handle, BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION); + return; + } instance->_conn_handle = connection_handle; instance->_isDeviceConnected = false; - instance->clearBuffers(); + instance->_tx_in_flight = 0; + instance->bleuart.flush(); } } @@ -39,7 +35,8 @@ void SerialBLEInterface::onDisconnect(uint16_t connection_handle, uint8_t reason if (instance->_conn_handle == connection_handle) { instance->_conn_handle = BLE_CONN_HANDLE_INVALID; instance->_isDeviceConnected = false; - instance->clearBuffers(); + instance->bleuart.flush(); + instance->_last_health_check = millis(); } } } @@ -120,6 +117,33 @@ void SerialBLEInterface::onBLEEvent(ble_evt_t* evt) { } else { BLE_DEBUG_PRINTLN("CONN_PARAM_UPDATE_REQUEST: ignoring stale callback for handle=0x%04X", conn_handle); } + } else if (evt->header.evt_id == BLE_GATTS_EVT_HVN_TX_COMPLETE && evt->evt.gatts_evt.conn_handle == instance->_conn_handle) { + // plain read-modify-write is safe: the loop task can't preempt the BLE task + uint8_t done = evt->evt.gatts_evt.params.hvn_tx_complete.count, n = instance->_tx_in_flight; + instance->_tx_in_flight = done < n ? n - done : 0; + } +} + +void SerialBLEInterface::trySendNextFrame() { + if (_last_retry_attempt > 0 && millis() - _last_retry_attempt < BLE_RETRY_THROTTLE_MS) return; + + // only write into a free TX slot: otherwise bleuart.write() blocks the main loop for up to 100 ms + while (isConnected() && !send_queue.isEmpty() && _tx_in_flight < BLE_HVN_TX_SLOTS) { + const SerialBLEFrame& frame = send_queue.peek(); + __atomic_add_fetch(&_tx_in_flight, 1, __ATOMIC_RELAXED); // before write(): its TX_COMPLETE can arrive first + size_t written = bleuart.write(frame.buf, frame.len); + if (written == frame.len) { + BLE_DEBUG_PRINTLN("writeBytes: sz=%u, hdr=%u", (unsigned)frame.len, (unsigned)frame.buf[0]); + send_queue.pop(); + } else if (written > 0) { + BLE_DEBUG_PRINTLN("writeBytes: partial write, sent=%u of %u, dropping corrupted frame", (unsigned)written, (unsigned)frame.len); + send_queue.pop(); + } else { + __atomic_sub_fetch(&_tx_in_flight, 1, __ATOMIC_RELAXED); + BLE_DEBUG_PRINTLN("writeBytes failed (buffer full), keeping frame for retry"); + _last_retry_attempt = millis(); // back off, eg. notifications not enabled yet + break; + } } } @@ -138,15 +162,10 @@ void SerialBLEInterface::begin(const char* prefix, char* name, uint32_t pin_code Bluefruit.configPrphBandwidth(BANDWIDTH_MAX); Bluefruit.begin(); + ble_gap_addr_t addr = {}; + sd_ble_gap_addr_get(&addr); char dev_name[32+16]; - if (strcmp(name, "@@MAC") == 0) { - ble_gap_addr_t addr; - if (sd_ble_gap_addr_get(&addr) == NRF_SUCCESS) { - sprintf(name, "%02X%02X%02X%02X%02X%02X", // modify (IN-OUT param) - addr.addr[5], addr.addr[4], addr.addr[3], addr.addr[2], addr.addr[1], addr.addr[0]); - } - } - sprintf(dev_name, "%s%s", prefix, name); + makeBLEName(dev_name, sizeof(dev_name), prefix, name, addr.addr, true); // Connection interval units: 1.25ms, supervision timeout units: 10ms ble_gap_conn_params_t ppcp_params; @@ -206,30 +225,12 @@ void SerialBLEInterface::begin(const char* prefix, char* name, uint32_t pin_code } void SerialBLEInterface::clearBuffers() { - send_queue_len = 0; - recv_queue_len = 0; + send_queue.clear(); + recv_queue.clear(); _last_retry_attempt = 0; 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; @@ -285,68 +286,37 @@ 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.push(src, len)) { 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++; - + trySendNextFrame(); return len; } return 0; } size_t SerialBLEInterface::checkRecvFrame(uint8_t dest[]) { - if (send_queue_len > 0) { - if (!isConnected()) { - BLE_DEBUG_PRINTLN("writeBytes: connection invalid, clearing send queue"); - send_queue_len = 0; - } 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"); - _last_retry_attempt = 0; - shiftSendQueueLeft(); - } else { - BLE_DEBUG_PRINTLN("writeBytes failed (buffer full), keeping frame for retry"); - _last_retry_attempt = now; - } - } - } - } + if (!isConnected()) { + send_queue.clear(); // here rather than in the BLE callbacks, as only the consumer may clear + recv_queue.clear(); + } else { + trySendNextFrame(); // frames that waited for a free TX slot } - if (recv_queue_len > 0) { - size_t len = recv_queue[0].len; - memcpy(dest, recv_queue[0].buf, len); + if (!recv_queue.isEmpty()) { + size_t len = recv_queue.peek().len; + memcpy(dest, recv_queue.peek().buf, len); BLE_DEBUG_PRINTLN("readBytes: sz=%u, hdr=%u", (unsigned)len, (unsigned)dest[0]); - shiftRecvQueueLeft(); + recv_queue.pop(); return len; } + unsigned long now = millis(); // 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; @@ -374,14 +344,6 @@ void SerialBLEInterface::onBleUartRX(uint16_t conn_handle) { } 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) { @@ -394,10 +356,11 @@ void SerialBLEInterface::onBleUartRX(uint16_t conn_handle) { 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++; + uint8_t buf[MAX_FRAME_SIZE]; + int read_len = instance->bleuart.readBytes(buf, avail); + if (!instance->recv_queue.push(buf, read_len)) { + BLE_DEBUG_PRINTLN("onBleUartRX: recv queue full, dropping frame"); + } } } @@ -406,5 +369,5 @@ bool SerialBLEInterface::isConnected() const { } bool SerialBLEInterface::isWriteBusy() const { - return send_queue_len >= (FRAME_QUEUE_SIZE * 2 / 3); + return send_queue.used() >= (BLE_FRAME_QUEUE_SIZE * 2 / 3); } diff --git a/src/helpers/nrf52/SerialBLEInterface.h b/src/helpers/nrf52/SerialBLEInterface.h index de1030548f..c20316ed78 100644 --- a/src/helpers/nrf52/SerialBLEInterface.h +++ b/src/helpers/nrf52/SerialBLEInterface.h @@ -1,6 +1,6 @@ #pragma once -#include "../BaseSerialInterface.h" +#include "../SerialBLECommon.h" #include #ifndef BLE_TX_POWER @@ -15,23 +15,13 @@ class SerialBLEInterface : public BaseSerialInterface { uint16_t _conn_handle; unsigned long _last_health_check; unsigned long _last_retry_attempt; + volatile uint8_t _tx_in_flight; - 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]; + SerialBLEFrameQueue send_queue; + SerialBLEFrameQueue recv_queue; void clearBuffers(); - void shiftSendQueueLeft(); - void shiftRecvQueueLeft(); + void trySendNextFrame(); bool isValidConnection(uint16_t handle, bool requireWaitingForSecurity = false) const; bool isAdvertising() const; static void onConnect(uint16_t connection_handle); @@ -49,8 +39,7 @@ class SerialBLEInterface : public BaseSerialInterface { _conn_handle = BLE_CONN_HANDLE_INVALID; _last_health_check = 0; _last_retry_attempt = 0; - send_queue_len = 0; - recv_queue_len = 0; + _tx_in_flight = 0; } /** @@ -70,12 +59,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 diff --git a/src/helpers/rp2040/SerialBLEInterface.cpp b/src/helpers/rp2040/SerialBLEInterface.cpp index a1fa487e7b..5be7280ced 100644 --- a/src/helpers/rp2040/SerialBLEInterface.cpp +++ b/src/helpers/rp2040/SerialBLEInterface.cpp @@ -23,8 +23,6 @@ SerialBLEInterface::SerialBLEInterface() { _isEnabled = false; _tx_pending = false; - send_queue_len = 0; - recv_queue_len = 0; memset(&_can_send, 0, sizeof(_can_send)); _rx.setCallbacks(this); addCharacteristic(&_rx); @@ -38,13 +36,10 @@ void SerialBLEInterface::begin(const char* prefix, char* name, uint32_t pin_code // the full name goes in the scan response BLE.begin(prefix); - if (strcmp(name, "@@MAC") == 0) { - bd_addr_t a; - gap_local_bd_addr(a); - sprintf(name, "%02X%02X%02X%02X%02X%02X", a[0], a[1], a[2], a[3], a[4], a[5]); // modify (IN-OUT param) - } + bd_addr_t a; + gap_local_bd_addr(a); char dev_name[32+16]; - snprintf(dev_name, sizeof(dev_name), "%s%s", prefix, name); + makeBLEName(dev_name, sizeof(dev_name), prefix, name, a, false); BLE.server()->setName(dev_name); // GAP device name characteristic BLE.server()->addService(this); @@ -66,8 +61,8 @@ void SerialBLEInterface::begin(const char* prefix, char* name, uint32_t pin_code } void SerialBLEInterface::clearBuffers() { - send_queue_len = 0; - recv_queue_len = 0; + send_queue.clear(); + recv_queue.clear(); _tx_pending = false; // a stale registration just fires into an empty queue; BTstack ignores double-adds } @@ -88,13 +83,9 @@ void SerialBLEInterface::onWrite(BLECharacteristic* c) { BLE_DEBUG_PRINTLN("onWrite: bad frame len=%u", (unsigned)len); return; } - if (recv_queue_len >= FRAME_QUEUE_SIZE) { + if (!recv_queue.push(_rx.valueData(), len)) { BLE_DEBUG_PRINTLN("onWrite: recv queue full, dropping frame"); - return; } - recv_queue[recv_queue_len].len = len; - memcpy(recv_queue[recv_queue_len].buf, _rx.valueData(), len); - recv_queue_len++; } // caller holds the BT lock (or is in the BT context) @@ -111,14 +102,14 @@ void SerialBLEInterface::kickSend() { // BT context: one notification per can-send-now, re-arm while frames remain void SerialBLEInterface::sendNext() { _tx_pending = false; - if (send_queue_len == 0) return; + if (send_queue.isEmpty()) return; if (!isConnected()) { BLE_DEBUG_PRINTLN("sendNext: not connected, clearing send queue"); - send_queue_len = 0; + send_queue.clear(); return; } uint16_t h = _tx.conHandle(); - Frame& f = send_queue[0]; + const SerialBLEFrame& f = send_queue.peek(); uint16_t mtu = att_server_get_mtu(h); if (f.len + 3 > mtu) { // att would silently truncate; drop instead (client must negotiate MTU >= MAX_FRAME_SIZE+3) @@ -135,9 +126,8 @@ void SerialBLEInterface::sendNext() { BLE_DEBUG_PRINTLN("writeBytes: sz=%u, hdr=%u", f.len, f.buf[0]); } } - send_queue_len--; - memmove(&send_queue[0], &send_queue[1], send_queue_len * sizeof(Frame)); - if (send_queue_len > 0) kickSend(); + send_queue.pop(); + if (!send_queue.isEmpty()) kickSend(); } void SerialBLEInterface::enable() { @@ -165,7 +155,7 @@ bool SerialBLEInterface::isConnected() const { } bool SerialBLEInterface::isWriteBusy() const { - return send_queue_len >= (FRAME_QUEUE_SIZE * 2 / 3); + return send_queue.used() >= (BLE_FRAME_QUEUE_SIZE * 2 / 3); } size_t SerialBLEInterface::writeFrame(const uint8_t src[], size_t len) { @@ -176,25 +166,21 @@ size_t SerialBLEInterface::writeFrame(const uint8_t src[], size_t len) { if (!isConnected()) return 0; BluetoothLock lock; - if (send_queue_len >= FRAME_QUEUE_SIZE) { + if (!send_queue.push(src, len)) { 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++; kickSend(); return len; } size_t SerialBLEInterface::checkRecvFrame(uint8_t dest[]) { BluetoothLock lock; - if (recv_queue_len == 0) return 0; + if (recv_queue.isEmpty()) return 0; - size_t len = recv_queue[0].len; - memcpy(dest, recv_queue[0].buf, len); - recv_queue_len--; - memmove(&recv_queue[0], &recv_queue[1], recv_queue_len * sizeof(Frame)); + size_t len = recv_queue.peek().len; + memcpy(dest, recv_queue.peek().buf, len); + recv_queue.pop(); BLE_DEBUG_PRINTLN("readBytes: sz=%u, hdr=%u", (unsigned)len, dest[0]); return len; } diff --git a/src/helpers/rp2040/SerialBLEInterface.h b/src/helpers/rp2040/SerialBLEInterface.h index 2e085a6cc7..a23f57512a 100644 --- a/src/helpers/rp2040/SerialBLEInterface.h +++ b/src/helpers/rp2040/SerialBLEInterface.h @@ -1,6 +1,6 @@ #pragma once -#include "../BaseSerialInterface.h" +#include "../SerialBLECommon.h" #include #include @@ -15,13 +15,6 @@ class SerialBLEInterface : public BaseSerialInterface, BLEService, BLEServerCall bool notifyEnabled() const { return _notificationEnabled; } }; - struct Frame { - uint8_t len; - uint8_t buf[MAX_FRAME_SIZE]; - }; - - #define FRAME_QUEUE_SIZE 8 - Characteristic _rx; Characteristic _tx; bool _isEnabled; @@ -29,10 +22,8 @@ class SerialBLEInterface : public BaseSerialInterface, BLEService, BLEServerCall btstack_context_callback_registration_t _can_send; uint8_t _scan_rsp[31]; // complete local name; BTstack keeps the pointer - uint8_t send_queue_len; - Frame send_queue[FRAME_QUEUE_SIZE]; - uint8_t recv_queue_len; - Frame recv_queue[FRAME_QUEUE_SIZE]; + SerialBLEFrameQueue send_queue; + SerialBLEFrameQueue recv_queue; void clearBuffers(); void kickSend(); @@ -64,12 +55,3 @@ class SerialBLEInterface : public BaseSerialInterface, BLEService, BLEServerCall 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