From a2b92eca169bef75790709ae5684f7633a78b67e Mon Sep 17 00:00:00 2001 From: Robert Ekl Date: Tue, 17 Mar 2026 00:26:03 -0500 Subject: [PATCH] Synchronize ESP32 BLE serial frame queues --- src/helpers/esp32/SerialBLEInterface.cpp | 108 +++++++++++++++++------ src/helpers/esp32/SerialBLEInterface.h | 21 ++++- 2 files changed, 98 insertions(+), 31 deletions(-) diff --git a/src/helpers/esp32/SerialBLEInterface.cpp b/src/helpers/esp32/SerialBLEInterface.cpp index dcfa0e1e..39e9684e 100644 --- a/src/helpers/esp32/SerialBLEInterface.cpp +++ b/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; } diff --git a/src/helpers/esp32/SerialBLEInterface.h b/src/helpers/esp32/SerialBLEInterface.h index 965e90fd..fecff474 100644 --- a/src/helpers/esp32/SerialBLEInterface.h +++ b/src/helpers/esp32/SerialBLEInterface.h @@ -5,6 +5,8 @@ #include #include #include +#include +#include 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; } /**