ESP32 BLE: retain sync replies when notification submission fails

.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.

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 =
+  -<*>
+  +<helpers/esp32/SerialBLEInterface.cpp>
+lib_deps =
+  google/googletest @ 1.17.0

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;

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;
   }
 
   /**

test/mocks/esp32_ble/Arduino.h

@@ -0,0 +1,7 @@
+#pragma once
+#include <cstddef>
+#include <cstdint>
+#include <cstring>
+#include <cstdio>
+inline unsigned long test_millis = 0;
+inline unsigned long millis() { return test_millis; }

test/mocks/esp32_ble/BLE2902.h

@@ -0,0 +1,2 @@
+#pragma once
+#include "FakeBLE.h"

test/mocks/esp32_ble/BLEDevice.h

@@ -0,0 +1,2 @@
+#pragma once
+#include "FakeBLE.h"

test/mocks/esp32_ble/BLEServer.h

@@ -0,0 +1,2 @@
+#pragma once
+#include "FakeBLE.h"

test/mocks/esp32_ble/BLEUtils.h

@@ -0,0 +1,2 @@
+#pragma once
+#include "FakeBLE.h"

test/mocks/esp32_ble/FakeBLE.h

@@ -0,0 +1,103 @@
+#pragma once
+#include "Arduino.h"
+#include <vector>
+
+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<uint8_t> value;
+  std::vector<std::vector<uint8_t>> 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 : &rx;
+  }
+  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) {}
+};

test/mocks/esp32_ble/esp_mac.h

@@ -0,0 +1,4 @@
+#pragma once
+#include <cstdint>
+#include <cstring>
+inline void esp_efuse_mac_get_default(uint8_t* addr) { memset(addr, 0, 6); }

test/mocks/esp32_ble/freertos/FreeRTOS.h

@@ -0,0 +1,2 @@
+#pragma once
+constexpr int pdTRUE = 1;

test/mocks/esp32_ble/freertos/queue.h

@@ -0,0 +1,19 @@
+#pragma once
+#include <deque>
+#include <vector>
+#include <cstring>
+struct StaticQueue_t { size_t size, capacity; std::deque<std::vector<uint8_t>> 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<const uint8_t*>(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;
+}

test/test_esp32_ble/test_notify_retry.cpp

@@ -0,0 +1,111 @@
+#include <gtest/gtest.h>
+#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<uint8_t>({0x18}));
+  advance(); poll();
+  ASSERT_EQ(tx().sent.size(), 2U);
+  EXPECT_EQ(tx().sent[1], std::vector<uint8_t>({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<uint8_t>({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<uint8_t>({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<uint8_t>({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<uint8_t>({0x05}));
+}
+
+int main(int argc, char** argv) {
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}