Skip to content

Synchronize ESP32 BLE serial frame queues #2063

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
robekl wants to merge 1 commit into
base: dev
Choose a base branch
from
+98 −31
Open

Synchronize ESP32 BLE serial frame queues #2063

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
108 changes: 81 additions & 27 deletions src/helpers/esp32/SerialBLEInterface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down Expand Up @@ -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++;
}
}

Expand Down Expand Up @@ -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;
Expand All @@ -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;
}

Expand Down
21 changes: 17 additions & 4 deletions src/helpers/esp32/SerialBLEInterface.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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
Expand Down Expand Up @@ -58,7 +69,9 @@ class SerialBLEInterface : public BaseSerialInterface, BLESecurityCallbacks, BLE
_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;
}

/**
Expand Down
Loading