Retain required Pico BLE replies through notification backpressure
This commit is contained in:
1 parent
ebc103f085
commit
b1197c5352
8 files changed
+737
-38
No files matched your search
@@ -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);
|
||||
|
||||
@@ -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;
|
||||
|
||||
Reference in new issue
Block a user