Merge a2b92eca16 into 5f3b7f25d0

src/helpers/esp32/SerialBLEInterface.cpp

@@ -10,6 +10,77 @@
 
 #define ADVERT_RESTART_DELAY  1000   // millis
 
+void SerialBLEInterface::clearBuffers() {
+  portENTER_CRITICAL(&_queue_lock);
+  recv_queue_read_idx = recv_queue_write_idx = recv_queue_len = 0;
+  send_queue_read_idx = send_queue_write_idx = send_queue_len = 0;
+  portEXIT_CRITICAL(&_queue_lock);
+}
+
+bool SerialBLEInterface::enqueueRecvFrame(const uint8_t src[], size_t len) {
+  bool queued = false;
+
+  portENTER_CRITICAL(&_queue_lock);
+  if (recv_queue_len < FRAME_QUEUE_SIZE) {
+    Frame *frame = &recv_queue[recv_queue_write_idx];
+    frame->len = len;
+    memcpy(frame->buf, src, len);
+    recv_queue_write_idx = (recv_queue_write_idx + 1) % FRAME_QUEUE_SIZE;
+    recv_queue_len++;
+    queued = true;
+  }
+  portEXIT_CRITICAL(&_queue_lock);
+
+  return queued;
+}
+
+bool SerialBLEInterface::dequeueRecvFrame(Frame *frame) {
+  bool found = false;
+
+  portENTER_CRITICAL(&_queue_lock);
+  if (recv_queue_len > 0) {
+    *frame = recv_queue[recv_queue_read_idx];
+    recv_queue_read_idx = (recv_queue_read_idx + 1) % FRAME_QUEUE_SIZE;
+    recv_queue_len--;
+    found = true;
+  }
+  portEXIT_CRITICAL(&_queue_lock);
+
+  return found;
+}
+
+bool SerialBLEInterface::enqueueSendFrame(const uint8_t src[], size_t len) {
+  bool queued = false;
+
+  portENTER_CRITICAL(&_queue_lock);
+  if (send_queue_len < FRAME_QUEUE_SIZE) {
+    Frame *frame = &send_queue[send_queue_write_idx];
+    frame->len = len;
+    memcpy(frame->buf, src, len);
+    send_queue_write_idx = (send_queue_write_idx + 1) % FRAME_QUEUE_SIZE;
+    send_queue_len++;
+    queued = true;
+  }
+  portEXIT_CRITICAL(&_queue_lock);
+
+  return queued;
+}
+
+bool SerialBLEInterface::dequeueSendFrame(Frame *frame) {
+  bool found = false;
+
+  portENTER_CRITICAL(&_queue_lock);
+  if (send_queue_len > 0) {
+    *frame = send_queue[send_queue_read_idx];
+    send_queue_read_idx = (send_queue_read_idx + 1) % FRAME_QUEUE_SIZE;
+    send_queue_len--;
+    found = true;
+  }
+  portEXIT_CRITICAL(&_queue_lock);
+
+  return found;
+}
+
 void SerialBLEInterface::begin(const char* prefix, char* name, uint32_t pin_code) {
   _pin_code = pin_code;
 
@@ -118,12 +189,8 @@ void SerialBLEInterface::onWrite(BLECharacteristic* pCharacteristic, esp_ble_gat
 
   if (len > MAX_FRAME_SIZE) {
     BLE_DEBUG_PRINTLN("ERROR: onWrite(), frame too big, len=%d", len);
-  } else if (recv_queue_len >= FRAME_QUEUE_SIZE) {
+  } else if (!enqueueRecvFrame(rxValue, len)) {
     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++;
   }
 }
 
@@ -166,15 +233,11 @@ size_t SerialBLEInterface::writeFrame(const uint8_t src[], size_t len) {
   }
 
   if (deviceConnected && len > 0) {
-    if (send_queue_len >= FRAME_QUEUE_SIZE) {
+    if (!enqueueSendFrame(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;
@@ -187,31 +250,22 @@ bool SerialBLEInterface::isWriteBusy() const {
 }
 
 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
-  ) {
+  Frame frame;
+
+  if (millis() >= _last_write + BLE_WRITE_MIN_INTERVAL && dequeueSendFrame(&frame)) {
     _last_write = millis();
-    pTxCharacteristic->setValue(send_queue[0].buf, send_queue[0].len);
+    pTxCharacteristic->setValue(frame.buf, frame.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];
-    }
+    BLE_DEBUG_PRINTLN("writeBytes: sz=%d, hdr=%d", (uint32_t)frame.len, (uint32_t) frame.buf[0]);
   }
 
-  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);
+  if (dequeueRecvFrame(&frame)) {
+    size_t len = frame.len;
+    memcpy(dest, frame.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];
-    }
     return len;
   }
 

src/helpers/esp32/SerialBLEInterface.h

@@ -5,6 +5,8 @@
 #include <BLEServer.h>
 #include <BLEUtils.h>
 #include <BLE2902.h>
+#include <freertos/FreeRTOS.h>
+#include <freertos/portmacro.h>
 
 class SerialBLEInterface : public BaseSerialInterface, BLESecurityCallbacks, BLEServerCallbacks, BLECharacteristicCallbacks {
   BLEServer *pServer;
@@ -24,12 +26,21 @@ class SerialBLEInterface : public BaseSerialInterface, BLESecurityCallbacks, BLE
   };
 
   #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];
+  uint8_t recv_queue_read_idx;
+  uint8_t recv_queue_write_idx;
+  uint8_t recv_queue_len;
+  uint8_t send_queue_read_idx;
+  uint8_t send_queue_write_idx;
+  uint8_t send_queue_len;
+  portMUX_TYPE _queue_lock;
 
-  void clearBuffers() { recv_queue_len = 0; send_queue_len = 0; }
+  void clearBuffers();
+  bool enqueueRecvFrame(const uint8_t src[], size_t len);
+  bool dequeueRecvFrame(Frame *frame);
+  bool enqueueSendFrame(const uint8_t src[], size_t len);
+  bool dequeueSendFrame(Frame *frame);
 
 protected:
   // BLESecurityCallbacks methods
@@ -58,7 +69,9 @@ public:
     _isEnabled = false;
     _last_write = 0;
     last_conn_id = 0;
-    send_queue_len = recv_queue_len = 0;
+    recv_queue_read_idx = recv_queue_write_idx = recv_queue_len = 0;
+    send_queue_read_idx = send_queue_write_idx = send_queue_len = 0;
+    _queue_lock = portMUX_INITIALIZER_UNLOCKED;
   }
 
   /**