From c2f03beecad2d620e636cf736a730f1968538706 Mon Sep 17 00:00:00 2001 From: Gavin Mai <7184919+gmaijoe@users.noreply.github.com> Date: Fri, 13 Nov 2026 03:39:20 -0400 Subject: [PATCH] Retry rejected ESP32 BLE notifications without dropping sync replies --- .github/workflows/run-unit-tests.yml | 2 +- platformio.ini | 16 +++- src/helpers/esp32/SerialBLEInterface.cpp | 25 +++-- src/helpers/esp32/SerialBLEInterface.h | 3 + test/mocks/esp32_ble/Arduino.h | 7 ++ test/mocks/esp32_ble/BLE2902.h | 2 + test/mocks/esp32_ble/BLEDevice.h | 2 + test/mocks/esp32_ble/BLEServer.h | 2 + test/mocks/esp32_ble/BLEUtils.h | 2 + test/mocks/esp32_ble/FakeBLE.h | 103 ++++++++++++++++++++ test/mocks/esp32_ble/esp_mac.h | 4 + test/mocks/esp32_ble/freertos/FreeRTOS.h | 2 + test/mocks/esp32_ble/freertos/queue.h | 19 ++++ test/test_esp32_ble/test_notify_retry.cpp | 111 ++++++++++++++++++++++ 14 files changed, 292 insertions(+), 8 deletions(-) create mode 100644 test/mocks/esp32_ble/Arduino.h create mode 100644 test/mocks/esp32_ble/BLE2902.h create mode 100644 test/mocks/esp32_ble/BLEDevice.h create mode 100644 test/mocks/esp32_ble/BLEServer.h create mode 100644 test/mocks/esp32_ble/BLEUtils.h create mode 100644 test/mocks/esp32_ble/FakeBLE.h create mode 100644 test/mocks/esp32_ble/esp_mac.h create mode 100644 test/mocks/esp32_ble/freertos/FreeRTOS.h create mode 100644 test/mocks/esp32_ble/freertos/queue.h create mode 100644 test/test_esp32_ble/test_notify_retry.cpp diff --git a/.github/workflows/run-unit-tests.yml b/.github/workflows/run-unit-tests.yml index 5d48f4c67be..284b2a70a5c 100644 --- a/.github/workflows/run-unit-tests.yml +++ b/.github/workflows/run-unit-tests.yml @@ -20,7 +20,7 @@ jobs: uses: ./.github/actions/setup-build-environment - name: Run Unit Tests - run: pio test -e native -e native_kiss_modem -vv + run: pio test -e native -e native_kiss_modem -e native_esp32_ble -vv - name: Upload Test Results # Upload test results even if the test step failed. diff --git a/platformio.ini b/platformio.ini index f66fbf2e5cf..3753f112ca8 100644 --- a/platformio.ini +++ b/platformio.ini @@ -167,7 +167,7 @@ build_flags = -std=c++17 -I src -I test/mocks test_build_src = yes -test_ignore = test_kiss_modem +test_ignore = test_kiss_modem, test_esp32_ble build_src_filter = -<*> +<../src/Utils.cpp> @@ -191,3 +191,17 @@ build_src_filter = +<../examples/kiss_modem/KissModem.cpp> lib_deps = google/googletest @ 1.17.0 + +[env:native_esp32_ble] +platform = native +test_framework = googletest +build_flags = -std=c++17 + -I test/mocks/esp32_ble + -I src +test_build_src = yes +test_filter = test_esp32_ble +build_src_filter = + -<*> + + +lib_deps = + google/googletest @ 1.17.0 diff --git a/src/helpers/esp32/SerialBLEInterface.cpp b/src/helpers/esp32/SerialBLEInterface.cpp index 6371cf33537..a9032ef28a7 100644 --- a/src/helpers/esp32/SerialBLEInterface.cpp +++ b/src/helpers/esp32/SerialBLEInterface.cpp @@ -45,6 +45,7 @@ void SerialBLEInterface::begin(const char* prefix, char* name, uint32_t pin_code 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->setCallbacks(this); BLECharacteristic * pRxCharacteristic = pService->createCharacteristic(CHARACTERISTIC_UUID_RX, BLECharacteristic::PROPERTY_WRITE); pRxCharacteristic->setAccessPermissions(ESP_GATT_PERM_WRITE_ENC_MITM); @@ -130,6 +131,13 @@ void SerialBLEInterface::onWrite(BLECharacteristic* pCharacteristic, esp_ble_gat // ---------- public methods +void SerialBLEInterface::onStatus(BLECharacteristic* pCharacteristic, Status status, uint32_t code) { + // Bluedroid reports notification submission status synchronously from notify(). + if (pCharacteristic == pTxCharacteristic) { + _notify_success = status == Status::SUCCESS_NOTIFY; + } +} + void SerialBLEInterface::clearBuffers() { xQueueReset(recv_queue); send_queue_len = 0; @@ -189,7 +197,8 @@ size_t SerialBLEInterface::writeFrame(const uint8_t src[], size_t len) { #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_len >= FRAME_QUEUE_SIZE + || millis() < _last_write + BLE_WRITE_MIN_INTERVAL; } size_t SerialBLEInterface::checkRecvFrame(uint8_t dest[]) { @@ -198,18 +207,22 @@ size_t SerialBLEInterface::checkRecvFrame(uint8_t dest[]) { ) { _last_write = millis(); pTxCharacteristic->setValue(send_queue[0].buf, send_queue[0].len); + _notify_success = false; pTxCharacteristic->notify(); - BLE_DEBUG_PRINTLN("writeBytes: sz=%d, hdr=%d", (uint32_t)send_queue[0].len, (uint32_t) send_queue[0].buf[0]); + if (_notify_success) { + 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]; + send_queue_len--; + for (int i = 0; i < send_queue_len; i++) { // delete top item from queue + send_queue[i] = send_queue[i + 1]; + } } } Frame frame; - if (xQueueReceive(recv_queue, &frame, 0) == pdTRUE) { + // Leave room for the command's reply while a notification is being retried. + if (send_queue_len < FRAME_QUEUE_SIZE && 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; diff --git a/src/helpers/esp32/SerialBLEInterface.h b/src/helpers/esp32/SerialBLEInterface.h index 9fb6adba943..55eb530bea9 100644 --- a/src/helpers/esp32/SerialBLEInterface.h +++ b/src/helpers/esp32/SerialBLEInterface.h @@ -31,6 +31,7 @@ class SerialBLEInterface : public BaseSerialInterface, BLESecurityCallbacks, BLE QueueHandle_t recv_queue; int send_queue_len; Frame send_queue[FRAME_QUEUE_SIZE]; + bool _notify_success; void clearBuffers(); @@ -50,6 +51,7 @@ class SerialBLEInterface : public BaseSerialInterface, BLESecurityCallbacks, BLE // BLECharacteristicCallbacks methods void onWrite(BLECharacteristic* pCharacteristic, esp_ble_gatts_cb_param_t* param) override; + void onStatus(BLECharacteristic* pCharacteristic, Status status, uint32_t code) override; public: SerialBLEInterface() { @@ -65,6 +67,7 @@ class SerialBLEInterface : public BaseSerialInterface, BLESecurityCallbacks, BLE FRAME_QUEUE_SIZE, sizeof(Frame), recv_queue_storage, &recv_queue_state ); send_queue_len = 0; + _notify_success = false; } /** diff --git a/test/mocks/esp32_ble/Arduino.h b/test/mocks/esp32_ble/Arduino.h new file mode 100644 index 00000000000..497c1a36dbb --- /dev/null +++ b/test/mocks/esp32_ble/Arduino.h @@ -0,0 +1,7 @@ +#pragma once +#include +#include +#include +#include +inline unsigned long test_millis = 0; +inline unsigned long millis() { return test_millis; } diff --git a/test/mocks/esp32_ble/BLE2902.h b/test/mocks/esp32_ble/BLE2902.h new file mode 100644 index 00000000000..6283ac59f6b --- /dev/null +++ b/test/mocks/esp32_ble/BLE2902.h @@ -0,0 +1,2 @@ +#pragma once +#include "FakeBLE.h" diff --git a/test/mocks/esp32_ble/BLEDevice.h b/test/mocks/esp32_ble/BLEDevice.h new file mode 100644 index 00000000000..6283ac59f6b --- /dev/null +++ b/test/mocks/esp32_ble/BLEDevice.h @@ -0,0 +1,2 @@ +#pragma once +#include "FakeBLE.h" diff --git a/test/mocks/esp32_ble/BLEServer.h b/test/mocks/esp32_ble/BLEServer.h new file mode 100644 index 00000000000..6283ac59f6b --- /dev/null +++ b/test/mocks/esp32_ble/BLEServer.h @@ -0,0 +1,2 @@ +#pragma once +#include "FakeBLE.h" diff --git a/test/mocks/esp32_ble/BLEUtils.h b/test/mocks/esp32_ble/BLEUtils.h new file mode 100644 index 00000000000..6283ac59f6b --- /dev/null +++ b/test/mocks/esp32_ble/BLEUtils.h @@ -0,0 +1,2 @@ +#pragma once +#include "FakeBLE.h" diff --git a/test/mocks/esp32_ble/FakeBLE.h b/test/mocks/esp32_ble/FakeBLE.h new file mode 100644 index 00000000000..a1537e6d4af --- /dev/null +++ b/test/mocks/esp32_ble/FakeBLE.h @@ -0,0 +1,103 @@ +#pragma once +#include "Arduino.h" +#include + +struct esp_ble_auth_cmpl_t { bool success; uint8_t fail_reason; }; +struct esp_ble_gatts_cb_param_t { + struct { uint16_t conn_id; } connect, mtu; + struct { uint16_t conn_id; uint8_t reason; } disconnect; +}; +constexpr int ESP_LE_AUTH_REQ_SC_MITM_BOND = 1; +constexpr int ESP_GATT_PERM_READ_ENC_MITM = 1; +constexpr int ESP_GATT_PERM_WRITE_ENC_MITM = 2; +class BLECharacteristic; +class BLEServer; +class BLESecurityCallbacks { +public: + virtual ~BLESecurityCallbacks() = default; + virtual uint32_t onPassKeyRequest() { return 0; } + virtual void onPassKeyNotify(uint32_t) {} + virtual bool onConfirmPIN(uint32_t) { return true; } + virtual bool onSecurityRequest() { return true; } + virtual void onAuthenticationComplete(esp_ble_auth_cmpl_t) {} +}; +class BLEServerCallbacks { +public: + virtual ~BLEServerCallbacks() = default; + virtual void onConnect(BLEServer*) {} + virtual void onConnect(BLEServer*, esp_ble_gatts_cb_param_t*) {} + virtual void onDisconnect(BLEServer*) {} + virtual void onDisconnect(BLEServer*, esp_ble_gatts_cb_param_t*) {} + virtual void onMtuChanged(BLEServer*, esp_ble_gatts_cb_param_t*) {} +}; +class BLECharacteristicCallbacks { +public: + enum Status { SUCCESS_NOTIFY, ERROR_GATT, ERROR_NOTIFY_DISABLED, ERROR_NO_CLIENT }; + virtual ~BLECharacteristicCallbacks() = default; + virtual void onWrite(BLECharacteristic*, esp_ble_gatts_cb_param_t*) {} + virtual void onStatus(BLECharacteristic*, Status, uint32_t) {} +}; +class BLE2902 {}; +class BLECharacteristic { +public: + enum { PROPERTY_READ=1, PROPERTY_NOTIFY=2, PROPERTY_WRITE=4 }; + BLECharacteristicCallbacks* callbacks = nullptr; + BLECharacteristicCallbacks::Status result = BLECharacteristicCallbacks::SUCCESS_NOTIFY; + bool report_status = true; + unsigned attempts = 0; + std::vector value; + std::vector> sent; + void setCallbacks(BLECharacteristicCallbacks* cb) { callbacks = cb; } + void setAccessPermissions(int) {} + void addDescriptor(BLE2902* descriptor) { delete descriptor; } + void setValue(const uint8_t* data, size_t len) { value.assign(data, data + len); } + uint8_t* getData() { return value.data(); } + size_t getLength() { return value.size(); } + void notify() { + attempts++; + if (result == BLECharacteristicCallbacks::SUCCESS_NOTIFY && report_status) sent.push_back(value); + // Arduino-ESP32 Bluedroid invokes onStatus inline from notify(). + if (callbacks && report_status) callbacks->onStatus(this, result, 0); + } +}; +class BLEService { +public: + BLECharacteristic tx, rx; + BLECharacteristic* createCharacteristic(const char*, int properties) { + return properties & BLECharacteristic::PROPERTY_NOTIFY ? &tx : ℞ + } + void start() {} + void stop() {} +}; +class BLEAdvertising { +public: + void addServiceUUID(const char*) {} + void start() {} + void stop() {} +}; +class BLEServer { +public: + BLEService service; + BLEAdvertising advertising; + BLEServerCallbacks* callbacks = nullptr; + void setCallbacks(BLEServerCallbacks* cb) { callbacks = cb; } + BLEService* createService(const char*) { return &service; } + BLEAdvertising* getAdvertising() { return &advertising; } + uint16_t getPeerMTU(uint16_t) { return 176; } + uint16_t getConnId() { return 0; } + void disconnect(uint16_t) {} + unsigned getConnectedCount() { return 0; } +}; +class BLEDevice { +public: + static inline BLEServer server; + static void init(const char*) { server = BLEServer(); } + static void setSecurityCallbacks(BLESecurityCallbacks*) {} + static void setMTU(int) {} + static BLEServer* createServer() { return &server; } +}; +class BLESecurity { +public: + void setStaticPIN(uint32_t) {} + void setAuthenticationMode(int) {} +}; diff --git a/test/mocks/esp32_ble/esp_mac.h b/test/mocks/esp32_ble/esp_mac.h new file mode 100644 index 00000000000..2e451ae7a34 --- /dev/null +++ b/test/mocks/esp32_ble/esp_mac.h @@ -0,0 +1,4 @@ +#pragma once +#include +#include +inline void esp_efuse_mac_get_default(uint8_t* addr) { memset(addr, 0, 6); } diff --git a/test/mocks/esp32_ble/freertos/FreeRTOS.h b/test/mocks/esp32_ble/freertos/FreeRTOS.h new file mode 100644 index 00000000000..8655becac83 --- /dev/null +++ b/test/mocks/esp32_ble/freertos/FreeRTOS.h @@ -0,0 +1,2 @@ +#pragma once +constexpr int pdTRUE = 1; diff --git a/test/mocks/esp32_ble/freertos/queue.h b/test/mocks/esp32_ble/freertos/queue.h new file mode 100644 index 00000000000..cf7cfe7377e --- /dev/null +++ b/test/mocks/esp32_ble/freertos/queue.h @@ -0,0 +1,19 @@ +#pragma once +#include +#include +#include +struct StaticQueue_t { size_t size, capacity; std::deque> frames; }; +using QueueHandle_t = StaticQueue_t*; +inline QueueHandle_t xQueueCreateStatic(size_t count, size_t size, uint8_t*, StaticQueue_t* queue) { + queue->size = size; queue->capacity = count; return queue; +} +inline void xQueueReset(QueueHandle_t queue) { queue->frames.clear(); } +inline int xQueueSend(QueueHandle_t queue, const void* frame, int) { + if (queue->frames.size() == queue->capacity) return 0; + const auto* bytes = static_cast(frame); + queue->frames.emplace_back(bytes, bytes + queue->size); return pdTRUE; +} +inline int xQueueReceive(QueueHandle_t queue, void* frame, int) { + if (queue->frames.empty()) return 0; + memcpy(frame, queue->frames.front().data(), queue->size); queue->frames.pop_front(); return pdTRUE; +} diff --git a/test/test_esp32_ble/test_notify_retry.cpp b/test/test_esp32_ble/test_notify_retry.cpp new file mode 100644 index 00000000000..c8e8a1cf3f8 --- /dev/null +++ b/test/test_esp32_ble/test_notify_retry.cpp @@ -0,0 +1,111 @@ +#include +#include "helpers/esp32/SerialBLEInterface.h" + +class TestBLEInterface : public SerialBLEInterface { +public: + using SerialBLEInterface::onAuthenticationComplete; + using SerialBLEInterface::onDisconnect; +}; + +class BLESendTest : public ::testing::Test { +protected: + TestBLEInterface interface; + uint8_t received[MAX_FRAME_SIZE] = {}; + BLECharacteristic& tx() { return BLEDevice::server.service.tx; } + void SetUp() override { + test_millis = 100; + char name[] = "test"; + interface.begin("MeshCore-", name, 123456); + interface.enable(); + interface.onAuthenticationComplete({true, 0}); + interface.checkRecvFrame(received); + } + void enqueue(uint8_t value) { ASSERT_EQ(interface.writeFrame(&value, 1), 1U); } + size_t poll() { return interface.checkRecvFrame(received); } + void advance() { test_millis += 60; } +}; + +TEST_F(BLESendTest, RetriesFailedNotificationInOrderWithoutDuplicateSuccess) { + enqueue(0x18); + enqueue(0x04); + tx().result = BLECharacteristicCallbacks::ERROR_GATT; + poll(); + EXPECT_TRUE(tx().sent.empty()); + EXPECT_EQ(tx().attempts, 1U); + poll(); + EXPECT_EQ(tx().attempts, 1U); // preserve pacing even when the stack is congested + advance(); + tx().result = BLECharacteristicCallbacks::SUCCESS_NOTIFY; + poll(); + ASSERT_EQ(tx().sent.size(), 1U); + EXPECT_EQ(tx().sent[0], std::vector({0x18})); + advance(); poll(); + ASSERT_EQ(tx().sent.size(), 2U); + EXPECT_EQ(tx().sent[1], std::vector({0x04})); + advance(); poll(); + EXPECT_EQ(tx().sent.size(), 2U); +} + +TEST_F(BLESendTest, WaitsForNotificationSubscription) { + enqueue(0x13); + tx().result = BLECharacteristicCallbacks::ERROR_NOTIFY_DISABLED; + poll(); + advance(); + tx().result = BLECharacteristicCallbacks::SUCCESS_NOTIFY; + poll(); + ASSERT_EQ(tx().sent.size(), 1U); + EXPECT_EQ(tx().sent[0], std::vector({0x13})); +} + +TEST_F(BLESendTest, DoesNotReuseSuccessFromPreviousNotification) { + enqueue(0x18); poll(); + enqueue(0x04); + tx().report_status = false; + advance(); poll(); + tx().report_status = true; + advance(); poll(); + ASSERT_EQ(tx().sent.size(), 2U); + EXPECT_EQ(tx().sent[1], std::vector({0x04})); +} + +TEST_F(BLESendTest, FullSendQueueDefersCommandsUntilAReplyFits) { + for (uint8_t i = 0; i < 4; i++) enqueue(i); + EXPECT_TRUE(interface.isWriteBusy()); + auto& rx = BLEDevice::server.service.rx; + const uint8_t command[] = {31, 0}; + rx.setValue(command, sizeof(command)); + rx.callbacks->onWrite(&rx, nullptr); + tx().result = BLECharacteristicCallbacks::ERROR_GATT; + EXPECT_EQ(poll(), 0U); + advance(); + EXPECT_TRUE(interface.isWriteBusy()); + tx().result = BLECharacteristicCallbacks::SUCCESS_NOTIFY; + EXPECT_EQ(poll(), sizeof(command)); + EXPECT_EQ(received[0], 31); + EXPECT_EQ(received[1], 0); + enqueue(0x18); // space for the command's response was preserved + for (int i = 0; i < 4; i++) { advance(); poll(); } + ASSERT_EQ(tx().sent.size(), 5U); + EXPECT_EQ(tx().sent[4], std::vector({0x18})); +} + +TEST_F(BLESendTest, DisconnectDiscardsFailedFramesBeforeNextSession) { + enqueue(0x18); + tx().result = BLECharacteristicCallbacks::ERROR_GATT; + poll(); + interface.onDisconnect(&BLEDevice::server); + tx().result = BLECharacteristicCallbacks::ERROR_NO_CLIENT; + advance(); poll(); + interface.onAuthenticationComplete({true, 0}); + poll(); + tx().result = BLECharacteristicCallbacks::SUCCESS_NOTIFY; + enqueue(0x05); + advance(); poll(); + ASSERT_EQ(tx().sent.size(), 1U); + EXPECT_EQ(tx().sent[0], std::vector({0x05})); +} + +int main(int argc, char** argv) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}