refactor: use FreeRTOS BLE receive queue

src/helpers/BoundedQueue.h

@@ -1,39 +0,0 @@
-#pragma once
-
-#include <stddef.h>
-
-namespace mesh {
-
-template <typename T, size_t Capacity>
-class BoundedQueue {
-  static_assert(Capacity > 0, "BoundedQueue capacity must be greater than zero");
-
-  T _items[Capacity];
-  size_t _head = 0;
-  size_t _size = 0;
-
-public:
-  bool push(const T& item) {
-    if (_size == Capacity) return false;
-    _items[(_head + _size) % Capacity] = item;
-    _size++;
-    return true;
-  }
-
-  bool pop(T& item) {
-    if (_size == 0) return false;
-    item = _items[_head];
-    _head = (_head + 1) % Capacity;
-    _size--;
-    return true;
-  }
-
-  void clear() {
-    _head = 0;
-    _size = 0;
-  }
-
-  size_t size() const { return _size; }
-};
-
-}  // namespace mesh

src/helpers/esp32/SerialBLEInterface.cpp

@@ -123,11 +123,7 @@ void SerialBLEInterface::onWrite(BLECharacteristic* pCharacteristic, esp_ble_gat
     frame.len = len;
     memcpy(frame.buf, rxValue, len);
 
-    portENTER_CRITICAL(&recv_queue_mux);
-    bool queued = recv_queue.push(frame);
-    portEXIT_CRITICAL(&recv_queue_mux);
-
-    if (!queued) {
+    if (xQueueSend(recv_queue, &frame, 0) != pdTRUE) {
       BLE_DEBUG_PRINTLN("ERROR: onWrite(), recv_queue is full!");
     }
   }
@@ -136,9 +132,7 @@ void SerialBLEInterface::onWrite(BLECharacteristic* pCharacteristic, esp_ble_gat
 // ---------- public methods
 
 void SerialBLEInterface::clearBuffers() {
-  portENTER_CRITICAL(&recv_queue_mux);
-  recv_queue.clear();
-  portEXIT_CRITICAL(&recv_queue_mux);
+  xQueueReset(recv_queue);
   send_queue_len = 0;
 }
 
@@ -216,11 +210,7 @@ size_t SerialBLEInterface::checkRecvFrame(uint8_t dest[]) {
   }
 
   Frame frame = {};
-  portENTER_CRITICAL(&recv_queue_mux);
-  bool received = recv_queue.pop(frame);
-  portEXIT_CRITICAL(&recv_queue_mux);
-
-  if (received) {
+  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;

src/helpers/esp32/SerialBLEInterface.h

@@ -1,12 +1,12 @@
 #pragma once
 
 #include "../BaseSerialInterface.h"
-#include "../BoundedQueue.h"
 #include <BLEDevice.h>
 #include <BLEServer.h>
 #include <BLEUtils.h>
 #include <BLE2902.h>
 #include <freertos/FreeRTOS.h>
+#include <freertos/queue.h>
 
 class SerialBLEInterface : public BaseSerialInterface, BLESecurityCallbacks, BLEServerCallbacks, BLECharacteristicCallbacks {
   BLEServer *pServer;
@@ -26,8 +26,9 @@ class SerialBLEInterface : public BaseSerialInterface, BLESecurityCallbacks, BLE
   };
 
   #define FRAME_QUEUE_SIZE  4
-  portMUX_TYPE recv_queue_mux = portMUX_INITIALIZER_UNLOCKED;
-  mesh::BoundedQueue<Frame, FRAME_QUEUE_SIZE> recv_queue;
+  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];
 
@@ -60,6 +61,9 @@ public:
     _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;
   }
 

test/test_bounded_queue/test_bounded_queue.cpp

@@ -1,58 +0,0 @@
-#include <gtest/gtest.h>
-
-#include <helpers/BoundedQueue.h>
-
-TEST(BoundedQueue, PreservesOrderAcrossWraparound) {
-  mesh::BoundedQueue<int, 4> queue;
-  int value = 0;
-
-  for (int i = 0; i < 4; i++) EXPECT_TRUE(queue.push(i));
-  EXPECT_FALSE(queue.push(4));
-
-  EXPECT_TRUE(queue.pop(value));
-  EXPECT_EQ(0, value);
-  EXPECT_TRUE(queue.pop(value));
-  EXPECT_EQ(1, value);
-
-  EXPECT_TRUE(queue.push(4));
-  EXPECT_TRUE(queue.push(5));
-
-  for (int expected = 2; expected < 6; expected++) {
-    EXPECT_TRUE(queue.pop(value));
-    EXPECT_EQ(expected, value);
-  }
-  EXPECT_FALSE(queue.pop(value));
-}
-
-TEST(BoundedQueue, SurvivesRepeatedFillAndDrainCycles) {
-  mesh::BoundedQueue<int, 4> queue;
-  int value = 0;
-
-  for (int cycle = 0; cycle < 1000; cycle++) {
-    for (int i = 0; i < 4; i++) EXPECT_TRUE(queue.push(cycle * 4 + i));
-    for (int i = 0; i < 4; i++) {
-      EXPECT_TRUE(queue.pop(value));
-      EXPECT_EQ(cycle * 4 + i, value);
-    }
-  }
-}
-
-TEST(BoundedQueue, ClearDropsPendingItems) {
-  mesh::BoundedQueue<int, 4> queue;
-  int value = 0;
-
-  EXPECT_TRUE(queue.push(1));
-  EXPECT_TRUE(queue.push(2));
-  queue.clear();
-
-  EXPECT_EQ(0u, queue.size());
-  EXPECT_FALSE(queue.pop(value));
-  EXPECT_TRUE(queue.push(3));
-  EXPECT_TRUE(queue.pop(value));
-  EXPECT_EQ(3, value);
-}
-
-int main(int argc, char **argv) {
-  ::testing::InitGoogleTest(&argc, argv);
-  return RUN_ALL_TESTS();
-}