Retain required Pico BLE replies through notification backpressure

This commit is contained in:
mikecarper committed 2026-10-16 05:30:17 -07:00
1 parent ebc103f085
commit b1197c5352
8 files changed
+737 -38

No files matched your search

+121 -36
View File
@@ -1,6 +1,7 @@
// only built when the env enables the core BLE stack (build_as_lib.py globs this dir)
#ifdef PIO_FRAMEWORK_ARDUINO_ENABLE_BLUETOOTH
#include "SerialBLEInterface.h"
#include "../CompanionFrameQueue.h"
#include <BluetoothLock.h>
#include <stdio.h>
#include <string.h>
@@ -22,7 +23,11 @@ SerialBLEInterface::SerialBLEInterface()
_tx(BLEUUID(CHARACTERISTIC_UUID_TX), BLERead | BLENotify, nullptr, ATT_SECURITY_AUTHENTICATED, ATT_SECURITY_AUTHENTICATED)
{
_isEnabled = false;
_device_connected = false;
_advertising_start_pending = false;
_tx_pending = false;
_tx_retry_pending = false;
_tx_retry_started = 0;
send_queue_len = 0;
recv_queue_len = 0;
memset(&_can_send, 0, sizeof(_can_send));
@@ -76,20 +81,31 @@ void SerialBLEInterface::clearBuffers() {
send_queue_len = 0;
recv_queue_len = 0;
_tx_pending = false; // a stale registration just fires into an empty queue; BTstack ignores double-adds
_tx_retry_pending = false;
_tx_retry_started = 0;
_tx_stall_watchdog.reset();
}
void SerialBLEInterface::onConnect(BLEServer* s) {
BLE_DEBUG_PRINTLN("connected handle=0x%04X", _tx.conHandle());
_device_connected = true;
_advertising_start_pending = false;
clearBuffers();
_tx_disconnect_recovery.complete();
// Advertising disable is asynchronous. Close a connection-complete event
// already in flight instead of leaving a disabled live link behind.
if (!_isEnabled) _tx_disconnect_recovery.begin();
}
void SerialBLEInterface::onDisconnect(BLEServer* s) {
BLE_DEBUG_PRINTLN("disconnected");
_device_connected = false;
clearBuffers(); // BTstack re-enables advertising on its own
_tx_disconnect_recovery.complete();
}
void SerialBLEInterface::onWrite(BLECharacteristic* c) {
if (c != &_rx) return;
if (c != &_rx || !_isEnabled || _tx_disconnect_recovery.pending()) return;
size_t len = _rx.valueLen();
if (len == 0 || len > MAX_FRAME_SIZE) {
BLE_DEBUG_PRINTLN("onWrite: bad frame len=%u", (unsigned)len);
@@ -106,12 +122,20 @@ void SerialBLEInterface::onWrite(BLECharacteristic* c) {
// caller holds the BT lock (or is in the BT context)
void SerialBLEInterface::kickSend() {
if (_tx_pending) return;
if (_tx_pending || send_queue_len == 0 || !_isEnabled
|| !_device_connected || _tx_disconnect_recovery.pending()) return;
if (_tx_retry_pending
&& !mesh::bleElapsedAtLeast((uint32_t)millis(), _tx_retry_started, 10)) return;
_tx_pending = true;
_can_send.callback = onCanSend;
_can_send.context = this;
if (att_server_register_can_send_now_callback(&_can_send, _tx.conHandle()) != 0) {
const uint8_t status = att_server_register_can_send_now_callback(&_can_send, _tx.conHandle());
// The callback can run synchronously and clear _tx_pending. A duplicate
// registration instead means the existing node is still waiting to run.
if (status != ERROR_CODE_SUCCESS && status != ERROR_CODE_COMMAND_DISALLOWED) {
_tx_pending = false;
_tx_retry_pending = true;
_tx_retry_started = (uint32_t)millis();
}
}
@@ -119,61 +143,125 @@ void SerialBLEInterface::kickSend() {
void SerialBLEInterface::sendNext() {
_tx_pending = false;
if (send_queue_len == 0) return;
if (!isConnected()) {
BLE_DEBUG_PRINTLN("sendNext: not connected, clearing send queue");
send_queue_len = 0;
return;
}
if (!_isEnabled || !_device_connected || _tx_disconnect_recovery.pending()) return;
uint16_t h = _tx.conHandle();
Frame& f = send_queue[0];
uint16_t mtu = att_server_get_mtu(h);
if (f.len + 3 > mtu) {
// att would silently truncate; drop instead (client must negotiate MTU >= MAX_FRAME_SIZE+3)
BLE_DEBUG_PRINTLN("sendNext: frame len=%u exceeds mtu=%u, dropping", f.len, mtu);
} else {
uint8_t err = att_server_notify(h, _tx.valueHandle(), f.buf, f.len);
if (err == BTSTACK_ACL_BUFFERS_FULL) {
kickSend();
return;
}
if (err) {
BLE_DEBUG_PRINTLN("sendNext: notify failed err=%u, dropping", err);
} else {
BLE_DEBUG_PRINTLN("writeBytes: sz=%u, hdr=%u", f.len, f.buf[0]);
}
const bool required = mesh::companionFrameRequiresDelivery(f.buf, f.len);
const bool ready = _tx.notifyEnabled() && mtu > 3 && f.len <= mtu - 3;
// BTstack returns a status, not a byte count. Never submit an oversized
// protocol frame: the app requires one complete frame per notification.
const bool sent = ready && att_server_notify(h, _tx.valueHandle(), f.buf, f.len) == ERROR_CODE_SUCCESS;
if (!sent && required) {
// Re-registering here can invoke this callback synchronously forever.
// Retain the whole frame and let loop() perform a throttled retry.
_tx_retry_pending = true;
_tx_retry_started = (uint32_t)millis();
_tx_stall_watchdog.noteBlocked(_tx_retry_started);
return;
}
_tx_retry_pending = false;
_tx_stall_watchdog.reset();
send_queue_len--;
memmove(&send_queue[0], &send_queue[1], send_queue_len * sizeof(Frame));
if (send_queue_len > 0) kickSend();
}
void SerialBLEInterface::enable() {
BluetoothLock lock;
if (_isEnabled) return;
_isEnabled = true;
clearBuffers();
BLE.startAdvertising();
if (_device_connected || _tx_disconnect_recovery.pending()) {
// Do not rebuild ATT or advertise while the old controller link closes.
_advertising_start_pending = true;
} else {
BLE.startAdvertising();
}
}
void SerialBLEInterface::disconnect() {
uint16_t h = _tx.conHandle();
if (h) gap_disconnect(h);
BluetoothLock lock;
if (_device_connected) recoverStalledTx();
}
void SerialBLEInterface::disable() {
BluetoothLock lock;
_isEnabled = false;
_advertising_start_pending = false;
BLE_DEBUG_PRINTLN("disable");
disconnect();
clearBuffers();
BLE.stopAdvertising();
}
bool SerialBLEInterface::isConnected() const {
BluetoothLock lock;
// notifications can only be enabled once the link is authenticated (CCCD inherits the perms)
return _isEnabled && _tx.conHandle() != 0 && _tx.notifyEnabled();
return _isEnabled && _device_connected && !_tx_disconnect_recovery.pending()
&& _tx.conHandle() != 0 && _tx.notifyEnabled();
}
bool SerialBLEInterface::hasRequiredReplyCapacity() const {
if (send_queue_len < FRAME_QUEUE_SIZE) return true;
for (unsigned i = 0; i < send_queue_len; ++i) {
if (!mesh::companionFrameRequiresDelivery(send_queue[i].buf, send_queue[i].len)) return true;
}
return false;
}
void SerialBLEInterface::serviceTxRecovery(uint32_t now) {
if (_tx_disconnect_recovery.pending() && _device_connected
&& _tx_disconnect_recovery.shouldAttempt(now)) {
// A successful request is not proof of closure. Retry until the actual
// disconnect callback, even if CCCD was disabled or the first call failed.
gap_disconnect(_tx.conHandle());
}
}
void SerialBLEInterface::recoverStalledTx() {
if (!_tx_disconnect_recovery.pending()) {
clearBuffers();
_tx_disconnect_recovery.begin();
}
serviceTxRecovery((uint32_t)millis());
}
void SerialBLEInterface::loop() {
BluetoothLock lock;
const uint32_t now = (uint32_t)millis();
if (_tx_disconnect_recovery.pending()) {
serviceTxRecovery(now);
return;
}
if (!_isEnabled) return;
if (_advertising_start_pending && !_device_connected) {
_advertising_start_pending = false;
BLE.startAdvertising();
}
if (send_queue_len == 0) {
_tx_stall_watchdog.reset();
return;
}
// Attempt progress before declaring a timeout after a scheduler gap.
if (!_tx_pending) kickSend();
if (send_queue_len == 0) {
_tx_stall_watchdog.reset();
return;
}
if (mesh::companionFrameRequiresDelivery(send_queue[0].buf, send_queue[0].len)) {
if (_tx_stall_watchdog.noteBlocked((uint32_t)millis())) {
recoverStalledTx();
return;
}
} else {
_tx_stall_watchdog.reset();
}
}
bool SerialBLEInterface::isWriteBusy() const {
BluetoothLock lock;
return send_queue_len >= (FRAME_QUEUE_SIZE * 2 / 3);
return _tx_disconnect_recovery.pending() || send_queue_len >= (FRAME_QUEUE_SIZE * 2 / 3);
}
bool SerialBLEInterface::isReadBusy() const {
@@ -183,31 +271,28 @@ bool SerialBLEInterface::isReadBusy() const {
bool SerialBLEInterface::hasPendingIO() const {
BluetoothLock lock;
return recv_queue_len > 0 || send_queue_len > 0 || _tx_pending;
return recv_queue_len > 0 || send_queue_len > 0 || _tx_pending || _tx_disconnect_recovery.pending();
}
size_t SerialBLEInterface::writeFrame(const uint8_t src[], size_t len) {
if (len == 0 || len > MAX_FRAME_SIZE) {
if (src == nullptr || len == 0 || len > MAX_FRAME_SIZE) {
BLE_DEBUG_PRINTLN("writeFrame(), frame too big, len=%u", (unsigned)len);
return 0;
}
if (!isConnected()) return 0;
BluetoothLock lock;
if (send_queue_len >= FRAME_QUEUE_SIZE) {
if (!isConnected()) return 0;
if (!mesh::enqueueCompanionFrame(send_queue, send_queue_len, FRAME_QUEUE_SIZE, src, len)) {
BLE_DEBUG_PRINTLN("writeFrame(), send_queue is full!");
return 0;
}
send_queue[send_queue_len].len = len;
memcpy(send_queue[send_queue_len].buf, src, len);
send_queue_len++;
kickSend();
return len;
}
size_t SerialBLEInterface::checkRecvFrame(uint8_t dest[]) {
BluetoothLock lock;
if (recv_queue_len == 0) return 0;
if (dest == nullptr || !isConnected() || recv_queue_len == 0
|| !hasRequiredReplyCapacity()) return 0;
size_t len = recv_queue[0].len;
memcpy(dest, recv_queue[0].buf, len);
+11
View File
@@ -4,6 +4,7 @@
#include <BLE.h>
#include <btstack.h>
#include <helpers/BluetoothMac.h>
#include <helpers/BleTxStallWatchdog.h>
// Nordic UART service over the arduino-pico BLE library (BTstack on the CYW43).
// Build with -D PIO_FRAMEWORK_ARDUINO_ENABLE_BLUETOOTH so the core links liblwip-bt.
@@ -26,7 +27,13 @@ class SerialBLEInterface : public BaseSerialInterface, BLEService, BLEServerCall
Characteristic _rx;
Characteristic _tx;
bool _isEnabled;
bool _device_connected;
bool _advertising_start_pending;
bool _tx_pending; // a can-send-now callback is registered
bool _tx_retry_pending;
uint32_t _tx_retry_started;
mesh::BleTxStallWatchdog _tx_stall_watchdog;
mesh::BleDisconnectRecovery _tx_disconnect_recovery;
btstack_context_callback_registration_t _can_send;
uint8_t _scan_rsp[31]; // complete local name; BTstack keeps the pointer
@@ -36,6 +43,9 @@ class SerialBLEInterface : public BaseSerialInterface, BLEService, BLEServerCall
Frame recv_queue[FRAME_QUEUE_SIZE];
void clearBuffers();
bool hasRequiredReplyCapacity() const;
void serviceTxRecovery(uint32_t now);
void recoverStalledTx();
void kickSend();
void sendNext();
static void onCanSend(void* ctx) { ((SerialBLEInterface*)ctx)->sendNext(); }
@@ -69,6 +79,7 @@ public:
void enable() override;
void disable() override;
bool isEnabled() const override { return _isEnabled; }
void loop() override;
bool isConnected() const override;
bool isReadBusy() const override;
bool isWriteBusy() const override;