diff --git a/src/helpers/rp2040/SerialBLEInterface.cpp b/src/helpers/rp2040/SerialBLEInterface.cpp index c372b8b08..5e2bc2358 100644 --- a/src/helpers/rp2040/SerialBLEInterface.cpp +++ b/src/helpers/rp2040/SerialBLEInterface.cpp @@ -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 #include #include @@ -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); diff --git a/src/helpers/rp2040/SerialBLEInterface.h b/src/helpers/rp2040/SerialBLEInterface.h index c85f8cd9f..d0f5f5882 100644 --- a/src/helpers/rp2040/SerialBLEInterface.h +++ b/src/helpers/rp2040/SerialBLEInterface.h @@ -4,6 +4,7 @@ #include #include #include +#include // 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; diff --git a/test/fixtures/pico_ble_queues/mocks/Arduino.h b/test/fixtures/pico_ble_queues/mocks/Arduino.h new file mode 100644 index 000000000..5b7280ba1 --- /dev/null +++ b/test/fixtures/pico_ble_queues/mocks/Arduino.h @@ -0,0 +1,6 @@ +#pragma once +#include +#include +#include +namespace fake_pico { inline uint32_t now = 0; } +inline uint32_t millis() { return fake_pico::now; } diff --git a/test/fixtures/pico_ble_queues/mocks/BLE.h b/test/fixtures/pico_ble_queues/mocks/BLE.h new file mode 100644 index 000000000..e06de0391 --- /dev/null +++ b/test/fixtures/pico_ble_queues/mocks/BLE.h @@ -0,0 +1,103 @@ +#pragma once + +#include +#include + +class BLEServer; +class BLECharacteristic; +class BLECharacteristicCallbacks { +public: + virtual void onWrite(BLECharacteristic*) {} +}; +class BLEServerCallbacks { +public: + virtual void onConnect(BLEServer*) {} + virtual void onDisconnect(BLEServer*) {} +}; +struct BLEUUID { + std::array bytes; + explicit BLEUUID(const uint8_t* value) { std::copy(value, value + 16, bytes.begin()); } +}; +constexpr unsigned BLERead = 2, BLEWrite = 8, BLENotify = 16; +constexpr unsigned BLESecurityJustWorks = 1; + +class BLECharacteristic { +protected: + uint16_t _valueHandle = 1, con_handle = 0; + bool _notificationEnabled = false; +public: + BLEUUID uuid; + BLECharacteristicCallbacks* callbacks = nullptr; + fake_pico::Bytes input; + BLECharacteristic(BLEUUID value, unsigned, const char*, uint8_t, uint8_t) : uuid(value) {} + void setCallbacks(BLECharacteristicCallbacks* value) { callbacks = value; } + size_t valueLen() const { return input.size(); } + const void* valueData() const { return input.data(); } + void setConHandle(uint16_t value) { con_handle = value; } + void disconnected() { _notificationEnabled = false; } // Real core retains con_handle. + void setNotify(bool value) { _notificationEnabled = value; } + void hostWrite(const fake_pico::Bytes& value) { + ++fake_pico::callback_depth; input = value; + if (callbacks) callbacks->onWrite(this); + --fake_pico::callback_depth; + } +}; + +class BLEService { +public: + std::vector characteristics; + explicit BLEService(BLEUUID) {} + void addCharacteristic(BLECharacteristic* value) { characteristics.push_back(value); } +}; + +namespace fake_pico { +inline bool advertising_requested = false, advertising_active = false; +inline unsigned advertising_starts = 0, advertising_stops = 0; +} +class BLEServer { +public: + BLEServerCallbacks* callbacks = nullptr; + std::vector services; + void setName(const char*) {} + void addService(BLEService* value) { services.push_back(value); } + bool setCallbacks(BLEServerCallbacks* value) { callbacks = value; return true; } + void connect(uint16_t handle = 1) { + fake_pico::connection_live = true; fake_pico::handle = handle; + fake_pico::advertising_active = false; + for (auto* service : services) for (auto* characteristic : service->characteristics) + characteristic->setConHandle(handle); + ++fake_pico::callback_depth; + if (callbacks) callbacks->onConnect(this); + --fake_pico::callback_depth; + } + void disconnectEvent() { + fake_pico::connection_live = false; fake_pico::pending.clear(); + for (auto* service : services) for (auto* characteristic : service->characteristics) + characteristic->disconnected(); + // Match BLEServer.cpp: callbacks see cleared CCCD but stale char handles; + // server's own handle is not cleared until after onDisconnect returns. + ++fake_pico::callback_depth; + if (callbacks) callbacks->onDisconnect(this); + --fake_pico::callback_depth; + fake_pico::handle = 0; + fake_pico::advertising_active = fake_pico::advertising_requested; + } +}; +class BLEClass { + BLEServer server_; +public: + void setSecurity(unsigned) {} + void begin(const char*) {} + BLEServer* server() { return &server_; } + void startAdvertising() { + assert(!fake_pico::connection_live + && "advertising and ATT rebuild must wait until the old link closes"); + ++fake_pico::advertising_starts; + fake_pico::advertising_requested = fake_pico::advertising_active = true; + } + void stopAdvertising() { + ++fake_pico::advertising_stops; + fake_pico::advertising_requested = fake_pico::advertising_active = false; + } +}; +inline BLEClass BLE; diff --git a/test/fixtures/pico_ble_queues/mocks/BluetoothLock.h b/test/fixtures/pico_ble_queues/mocks/BluetoothLock.h new file mode 100644 index 000000000..a9b2cf565 --- /dev/null +++ b/test/fixtures/pico_ble_queues/mocks/BluetoothLock.h @@ -0,0 +1,6 @@ +#pragma once +#include +struct BluetoothLock { + BluetoothLock() { ++fake_pico::lock_depth; } + ~BluetoothLock() { --fake_pico::lock_depth; } +}; diff --git a/test/fixtures/pico_ble_queues/mocks/btstack.h b/test/fixtures/pico_ble_queues/mocks/btstack.h new file mode 100644 index 000000000..274f78ad4 --- /dev/null +++ b/test/fixtures/pico_ble_queues/mocks/btstack.h @@ -0,0 +1,94 @@ +#pragma once + +#include +#include +#include +#include +#include + +using bd_addr_t = uint8_t[6]; +struct btstack_context_callback_registration_t { + void* next; + void (*callback)(void*); + void* context; +}; +constexpr uint8_t ERROR_CODE_SUCCESS = 0; +constexpr uint8_t ERROR_CODE_UNKNOWN_CONNECTION_IDENTIFIER = 2; +constexpr uint8_t ERROR_CODE_COMMAND_DISALLOWED = 0x0c; +constexpr uint8_t BTSTACK_ACL_BUFFERS_FULL = 0x57; +constexpr uint8_t ATT_SECURITY_AUTHENTICATED = 2; +constexpr uint8_t IO_CAPABILITY_DISPLAY_ONLY = 0; +constexpr uint8_t SM_AUTHREQ_MITM_PROTECTION = 4; +constexpr uint8_t SM_AUTHREQ_BONDING = 1; +constexpr uint8_t BLUETOOTH_DATA_TYPE_COMPLETE_LOCAL_NAME = 9; + +namespace fake_pico { +using Bytes = std::vector; +inline unsigned lock_depth = 0, callback_depth = 0, max_callback_depth = 0; +inline unsigned registrations = 0, notification_attempts = 0, disconnects = 0; +inline uint32_t callback_advance_ms = 0; +inline bool synchronous = true, callbacks_enabled = true, connection_live = false; +inline bool duplicate_registration = false, synchronous_disconnect = false; +inline uint8_t registration_error = 0, disconnect_error = 0; +inline uint16_t handle = 0, mtu = 23; +inline std::deque notify_errors; +inline std::vector notified; +inline std::deque pending; +inline void (*disconnect_event)() = nullptr; +inline void require_context() { assert(lock_depth || callback_depth); } +inline void fireOne() { + if (pending.empty()) return; + auto* node = pending.front(); pending.pop_front(); + ++callback_depth; + max_callback_depth = std::max(max_callback_depth, callback_depth); + assert(callback_depth <= 16 + && "blocked notification must defer instead of reentering its callback"); + now += callback_advance_ms; + node->callback(node->context); + --callback_depth; +} +} + +// Contracts from resolved Arduino-Pico 1.50501.0's BTstack att_server.h/.c: +// registration can invoke its callback inline; a duplicate node is already +// pending; notification returns an error status, not an accepted byte count. +inline uint8_t att_server_register_can_send_now_callback( + btstack_context_callback_registration_t* node, uint16_t handle) { + fake_pico::require_context(); ++fake_pico::registrations; + if (!fake_pico::connection_live || handle != fake_pico::handle) + return ERROR_CODE_UNKNOWN_CONNECTION_IDENTIFIER; + if (fake_pico::registration_error) return fake_pico::registration_error; + auto existing = std::find(fake_pico::pending.begin(), fake_pico::pending.end(), node); + const bool duplicate = existing != fake_pico::pending.end() || fake_pico::duplicate_registration; + if (existing == fake_pico::pending.end()) fake_pico::pending.push_back(node); + if (fake_pico::synchronous && fake_pico::callbacks_enabled) fake_pico::fireOne(); + return duplicate ? ERROR_CODE_COMMAND_DISALLOWED : ERROR_CODE_SUCCESS; +} +inline uint16_t att_server_get_mtu(uint16_t handle) { + fake_pico::require_context(); + return fake_pico::connection_live && handle == fake_pico::handle ? fake_pico::mtu : 0; +} +inline uint8_t att_server_notify(uint16_t handle, uint16_t, const uint8_t* data, uint16_t length) { + fake_pico::require_context(); ++fake_pico::notification_attempts; + if (!fake_pico::connection_live || handle != fake_pico::handle) + return ERROR_CODE_UNKNOWN_CONNECTION_IDENTIFIER; + assert(length + 3 <= fake_pico::mtu && "protocol frames must never be truncated or fragmented"); + if (!fake_pico::notify_errors.empty()) { + const uint8_t error = fake_pico::notify_errors.front(); fake_pico::notify_errors.pop_front(); + if (error) return error; + } + fake_pico::notified.emplace_back(data, data + length); + return ERROR_CODE_SUCCESS; +} +inline uint8_t gap_disconnect(uint16_t) { + fake_pico::require_context(); ++fake_pico::disconnects; + if (fake_pico::disconnect_error) return fake_pico::disconnect_error; + if (fake_pico::synchronous_disconnect && fake_pico::disconnect_event) + fake_pico::disconnect_event(); + return ERROR_CODE_SUCCESS; // Request accepted, not proof the link closed. +} +inline void gap_local_bd_addr(uint8_t address[6]) { memset(address, 1, 6); } +inline void gap_scan_response_set_data(uint8_t, const uint8_t*) {} +inline void sm_set_io_capabilities(uint8_t) {} +inline void sm_set_authentication_requirements(uint8_t) {} +inline void sm_use_fixed_passkey_in_display_role(uint32_t) {} diff --git a/test/fixtures/pico_ble_queues/test.cpp b/test/fixtures/pico_ble_queues/test.cpp new file mode 100644 index 000000000..addd3424f --- /dev/null +++ b/test/fixtures/pico_ble_queues/test.cpp @@ -0,0 +1,262 @@ +#include +#include +#include +#include +#include + +using fake_pico::Bytes; +@QUERY_PRODUCER@ + +struct Fixture { + SerialBLEInterface node; + BLECharacteristic *rx = nullptr, *tx = nullptr; + Fixture() { + fake_pico::now = 100; + assert(node.begin("MeshCore-", "PicoW", 123456)); node.enable(); + for (auto* characteristic : BLE.server()->services[0]->characteristics) { + if (characteristic->uuid.bytes[3] == 2) rx = characteristic; + if (characteristic->uuid.bytes[3] == 3) tx = characteristic; + } + assert(rx && tx); + fake_pico::disconnect_event = []() { BLE.server()->disconnectEvent(); }; + BLE.server()->connect(); tx->setNotify(true); fake_pico::mtu = 247; + assert(node.isConnected()); + } + void write(const Bytes& frame) { assert(node.writeFrame(frame.data(), frame.size()) == frame.size()); } + void query() { rx->hostWrite(Bytes{CMD_DEVICE_QUERY, 3}); } + size_t receive(uint8_t* input) { return node.checkRecvFrame(input); } + void dispatch() { + uint8_t input[MAX_FRAME_SIZE]{}; + const size_t length = receive(input); assert(length == 2); + ActualDeviceQuery mesh; mesh._serial = &node; mesh.dispatch(input, length); + } + void tick(uint32_t delay = 10) { fake_pico::now += delay; node.loop(); } + void drain() { + for (unsigned i = 0; i < 20; ++i) { + if (!fake_pico::pending.empty()) fake_pico::fireOne(); + tick(); + } + } +}; + +struct HoldingUsb : BaseSerialInterface { + bool pending = true; + void enable() override {} + void disable() override {} + bool isEnabled() const override { return true; } + bool isConnected() const override { return true; } + bool isReadBusy() const override { return pending; } + bool isWriteBusy() const override { return false; } + size_t writeFrame(const uint8_t*, size_t length) override { return length; } + size_t checkRecvFrame(uint8_t* out) override { + if (!pending) return 0; + pending = false; out[0] = 4; return 1; + } +}; + +static void test_stall(const std::string& scenario) { + Fixture f; + fake_pico::now = scenario == "stall-wrap" ? UINT32_MAX - 5000 : 0; + fake_pico::synchronous = scenario == "stall-register" || scenario == "stall-notify" || scenario == "stall-mtu"; + if (scenario == "stall-register") fake_pico::registration_error = ERROR_CODE_UNKNOWN_CONNECTION_IDENTIFIER; + if (scenario == "stall-notify") fake_pico::notify_errors = {BTSTACK_ACL_BUFFERS_FULL, BTSTACK_ACL_BUFFERS_FULL}; + if (scenario == "stall-mtu") fake_pico::mtu = 23; + fake_pico::disconnect_error = ERROR_CODE_COMMAND_DISALLOWED; + Bytes required(scenario == "stall-mtu" ? 82 : 2, 0x41); required[0] = 0x09; + f.write(required); f.node.loop(); + if (scenario == "stall-cccd") f.tx->setNotify(false); + f.tick(9999); assert(fake_pico::disconnects == 0); + f.tick(1); + assert(fake_pico::disconnects == 1 && fake_pico::connection_live); + assert(!f.node.isConnected() && f.node.isWriteBusy() && f.node.hasPendingIO()); + uint8_t input[MAX_FRAME_SIZE]{}; + f.query(); assert(f.receive(input) == 0); + const uint8_t reply[] = {0x09, 0x42}; + assert(f.node.writeFrame(reply, sizeof(reply)) == 0); + f.tx->setNotify(false); f.tick(999); assert(fake_pico::disconnects == 1); + f.tick(1); assert(fake_pico::disconnects == 2 && !f.node.isConnected()); + fake_pico::disconnect_error = 0; + f.tick(1000); assert(fake_pico::disconnects == 3 && fake_pico::connection_live); + // Request acceptance and missing CCCD cannot end recovery before closure. + assert(!f.node.isConnected() && f.node.hasPendingIO()); + BLE.server()->disconnectEvent(); assert(!f.node.hasPendingIO()); + assert(fake_pico::advertising_active); + fake_pico::synchronous = true; + fake_pico::registration_error = 0; fake_pico::notify_errors.clear(); fake_pico::mtu = 247; + BLE.server()->connect(2); f.tx->setNotify(true); + assert(f.node.isConnected()); f.query(); f.dispatch(); f.tick(); + assert(fake_pico::notified.size() == 1 && fake_pico::notified[0].size() == 82); + assert(fake_pico::notified[0][0] == 13); +} + +int main(int argc, char** argv) { + assert(argc == 2); + const std::string scenario = argv[1]; + if (scenario.compare(0, 6, "stall-") == 0) { + test_stall(scenario); + } else { + Fixture f; + if (scenario == "normal") { + Bytes maximum(MAX_FRAME_SIZE); + for (unsigned i = 0; i < maximum.size(); ++i) maximum[i] = uint8_t(i); + f.write(maximum); f.write(Bytes{0x09, 0x41}); + assert(fake_pico::notified == (std::vector{maximum, Bytes{0x09, 0x41}})); + assert(!f.node.hasPendingIO() && fake_pico::max_callback_depth == 1); + assert(f.node.writeFrame(nullptr, 1) == 0); + assert(f.node.writeFrame(maximum.data(), 0) == 0); + assert(f.node.writeFrame(maximum.data(), MAX_FRAME_SIZE + 1) == 0); + assert(f.node.checkRecvFrame(nullptr) == 0); + f.rx->hostWrite({}); f.rx->hostWrite(Bytes(MAX_FRAME_SIZE + 1, 1)); + uint8_t input[MAX_FRAME_SIZE + 2]{}; input[0] = input[MAX_FRAME_SIZE + 1] = 0xa5; + assert(f.receive(input + 1) == 0); + f.rx->hostWrite(maximum); assert(f.receive(input + 1) == MAX_FRAME_SIZE); + assert(input[0] == 0xa5 && input[MAX_FRAME_SIZE + 1] == 0xa5); + assert(std::equal(maximum.begin(), maximum.end(), input + 1)); + } else if (scenario == "admission") { + fake_pico::synchronous = false; + for (unsigned i = 0; i < 8; ++i) f.write(Bytes{0x09, uint8_t(i)}); + f.query(); uint8_t input[MAX_FRAME_SIZE]{}; + assert(f.receive(input) == 0 + && "a full required TX queue must retain the command until its reply fits"); + fake_pico::fireOne(); assert(f.receive(input) == 2); + ActualDeviceQuery mesh; mesh._serial = &f.node; mesh.dispatch(input, 2); + f.drain(); + assert(fake_pico::notified.size() == 9 && fake_pico::notified.back().size() == 82); + assert(fake_pico::notified.back()[0] == 13 && !f.node.hasPendingIO()); + } else if (scenario == "priority") { + fake_pico::synchronous = false; + const Bytes log{0x88, 0x41}; + for (unsigned i = 0; i < 7; ++i) f.write(log); + assert(f.node.writeFrame(log.data(), log.size()) == 0); + f.query(); f.dispatch(); f.drain(); + assert(fake_pico::notified.size() == 8 && fake_pico::notified[0].size() == 82); + assert(fake_pico::notified[0][0] == 13); + for (unsigned i = 1; i < 8; ++i) assert(fake_pico::notified[i] == log); + fake_pico::notified.clear(); + for (unsigned i = 0; i < 30; ++i) f.write(Bytes{0x83}); + f.drain(); assert(fake_pico::notified == std::vector{Bytes{0x83}}); + fake_pico::notified.clear(); + for (unsigned i = 0; i < 7; ++i) f.write(log); + f.write(Bytes{0x85, 1}); f.write(Bytes{0x09, 2}); f.drain(); + assert(fake_pico::notified.size() == 8); + assert(fake_pico::notified[0] == (Bytes{0x09, 2})); + assert(fake_pico::notified[1] == (Bytes{0x85, 1})); + } else if (scenario == "mtu") { + fake_pico::mtu = 23; f.query(); f.dispatch(); + assert(f.node.hasPendingIO() && fake_pico::notified.empty() + && "required DEVICE_QUERY response must survive an initial MTU of 23"); + const unsigned attempts = fake_pico::registrations; + f.tick(9); assert(fake_pico::registrations == attempts); + f.tick(1); assert(fake_pico::notified.empty()); + fake_pico::mtu = 247; f.tick(); f.tick(); + assert(fake_pico::notified.size() == 1 && fake_pico::notified[0].size() == 82); + assert(fake_pico::notified[0][0] == 13 && !f.node.hasPendingIO()); + } else if (scenario == "notify") { + Bytes maximum(MAX_FRAME_SIZE, 0x31); maximum[0] = 13; + fake_pico::notify_errors = {BTSTACK_ACL_BUFFERS_FULL, ERROR_CODE_COMMAND_DISALLOWED, 0}; + f.write(maximum); assert(fake_pico::notified.empty() && f.node.hasPendingIO()); + assert(fake_pico::registrations == 1 && fake_pico::max_callback_depth == 1); + f.tick(9); assert(fake_pico::registrations == 1); + f.tick(1); assert(fake_pico::registrations == 2 && fake_pico::notified.empty()); + f.tick(); f.tick(); + assert(fake_pico::notified == std::vector{maximum}); + assert(fake_pico::notification_attempts == 3 && !f.node.hasPendingIO()); + } else if (scenario == "registration" || scenario == "registration-sync-duplicate") { + fake_pico::synchronous = scenario == "registration-sync-duplicate"; + fake_pico::duplicate_registration = true; + f.write(Bytes{0x09, 1}); + if (!fake_pico::synchronous) { + for (unsigned i = 0; i < 10; ++i) f.tick(); + assert(fake_pico::registrations == 1 && fake_pico::pending.size() == 1); + fake_pico::fireOne(); + } + assert(fake_pico::notified == (std::vector{Bytes{0x09, 1}})); + assert(!f.node.hasPendingIO()); + f.write(Bytes{0x09, 2}); + if (!fake_pico::synchronous) fake_pico::fireOne(); + assert(fake_pico::notified.size() == 2 && fake_pico::registrations == 2); + } else if (scenario == "registration-error" || scenario == "callback-clock") { + fake_pico::registration_error = ERROR_CODE_UNKNOWN_CONNECTION_IDENTIFIER; + f.write(Bytes{0x09, 1}); assert(fake_pico::registrations == 1); + fake_pico::registration_error = 0; + if (scenario == "callback-clock") fake_pico::now += 9; + else f.tick(9); + assert(fake_pico::registrations == 1); + if (scenario == "callback-clock") { + fake_pico::callback_advance_ms = 1; + fake_pico::notify_errors.push_back(BTSTACK_ACL_BUFFERS_FULL); + } + f.tick(1); + assert(fake_pico::disconnects == 0 && f.node.isConnected() + && "a synchronous callback must not backdate the stall watchdog"); + f.tick(); + assert(fake_pico::notified == (std::vector{Bytes{0x09, 1}})); + } else if (scenario == "deadline-progress") { + fake_pico::mtu = 23; f.query(); f.dispatch(); + fake_pico::mtu = 247; f.tick(10000); + assert(fake_pico::notified.size() == 1 && fake_pico::disconnects == 0); + } else if (scenario == "best-effort") { + fake_pico::mtu = 23; Bytes log(64, 1); log[0] = 0x88; f.write(log); + f.tick(100000); assert(fake_pico::disconnects == 0 && !f.node.hasPendingIO()); + fake_pico::mtu = 247; + fake_pico::notify_errors.push_back(BTSTACK_ACL_BUFFERS_FULL); + f.write(Bytes{0x88, 1}); f.tick(100000); + assert(fake_pico::disconnects == 0 && f.node.isConnected() && !f.node.hasPendingIO()); + } else if (scenario == "locked") { + HoldingUsb usb; MultiSerialInterface manager; + assert(manager.addInterface(InterfaceType::USB, &usb)); + assert(manager.addInterface(InterfaceType::Bluetooth, &f.node)); manager.enable(); + uint8_t input[MAX_FRAME_SIZE]{}; + assert(manager.checkRecvFrame(input) == 1); manager.lockReplyRoute(); + fake_pico::mtu = 23; Bytes required(82, 1); required[0] = 13; f.write(required); + f.query(); assert(manager.checkRecvFrame(input) == 0); + fake_pico::mtu = 247; fake_pico::now += 10; manager.loop(); + assert(fake_pico::notified == std::vector{required}); + assert(manager.captureReplyRoute() == &usb && manager.checkRecvFrame(input) == 0); + fake_pico::mtu = 23; f.write(required); + fake_pico::now += 10000; manager.loop(); + assert(fake_pico::disconnects == 1 && !manager.isReplyRouteAvailable(&f.node)); + assert(manager.captureReplyRoute() == &usb && manager.isConnected()); + } else if (scenario == "disabled-reenable" || scenario == "disabled-closed") { + fake_pico::disconnect_error = ERROR_CODE_COMMAND_DISALLOWED; + f.node.disable(); assert(!f.node.isConnected() && !fake_pico::advertising_requested); + if (scenario == "disabled-reenable") f.node.enable(); + assert(fake_pico::advertising_starts == 1 && !fake_pico::advertising_active); + f.tick(1000); assert(fake_pico::disconnects == 2 && !f.node.isConnected()); + BLE.server()->disconnectEvent(); + assert(!fake_pico::advertising_active); f.tick(); + const bool enabled = scenario == "disabled-reenable"; + assert(fake_pico::advertising_active == enabled); + assert(fake_pico::advertising_starts == unsigned(enabled ? 2 : 1)); + assert(!f.node.hasPendingIO()); + } else if (scenario == "late-connect") { + BLE.server()->disconnectEvent(); f.node.disable(); + BLE.server()->connect(2); // Controller completed connection after stopAdvertising. + assert(!f.node.isConnected() && f.node.hasPendingIO()); + f.node.enable(); assert(fake_pico::advertising_starts == 1); + f.tick(); assert(fake_pico::disconnects == 1 && !f.node.isConnected()); + BLE.server()->disconnectEvent(); f.tick(); + assert(fake_pico::advertising_active && fake_pico::advertising_starts == 2); + } else if (scenario == "synchronous-disconnect") { + fake_pico::synchronous_disconnect = true; + f.node.disconnect(); + assert(!fake_pico::connection_live && !f.node.hasPendingIO() && !f.node.isConnected()); + assert(fake_pico::disconnects == 1); + f.tick(1000); assert(fake_pico::disconnects == 1); + } else if (scenario == "pending-cccd") { + fake_pico::synchronous = false; f.query(); f.dispatch(); + f.tx->setNotify(false); fake_pico::fireOne(); + assert(f.node.hasPendingIO() && fake_pico::notified.empty()); + f.tx->setNotify(true); f.tick(); fake_pico::fireOne(); + assert(fake_pico::notified.size() == 1 && fake_pico::notified[0].size() == 82); + assert(!f.node.hasPendingIO()); + } else if (scenario == "cccd") { + f.tx->setNotify(false); f.query(); uint8_t input[MAX_FRAME_SIZE]{}; + assert(f.receive(input) == 0); + f.tx->setNotify(true); f.dispatch(); + assert(fake_pico::notified.size() == 1 && fake_pico::notified[0].size() == 82); + } else assert(false && "unknown scenario"); + } + assert(fake_pico::lock_depth == 0 && fake_pico::callback_depth == 0); + std::printf("PASS: Pico BLE delivery %s\n", scenario.c_str()); +} diff --git a/test/test_pico_ble_queues.py b/test/test_pico_ble_queues.py index a80fd47c4..f1694ef94 100644 --- a/test/test_pico_ble_queues.py +++ b/test/test_pico_ble_queues.py @@ -1,20 +1,145 @@ -"""Execute Pico W's production queue predicates used by companion sleep control.""" +"""Execute Pico W BLE queue, atomic notification, and recovery contracts.""" from pathlib import Path +import os +import re +import shutil import subprocess +import sys import tempfile import unittest from test_radio_receive_contract import method ROOT = Path(__file__).resolve().parents[1] +FIXTURE = ROOT / "test/fixtures/pico_ble_queues" +SANITIZERS = (["-fsanitize=address,undefined", "-fno-sanitize-recover=all", "-fno-pie", "-no-pie"] + if sys.platform.startswith("linux") else []) +SCENARIOS = ("normal", "admission", "priority", "mtu", "notify", "registration", + "registration-sync-duplicate", "registration-error", "callback-clock", + "deadline-progress", "best-effort", "locked", "stall-zero", "stall-wrap", + "stall-cccd", "stall-register", "stall-notify", "stall-mtu", + "disabled-reenable", "disabled-closed", "late-connect", + "synchronous-disconnect", "cccd", "pending-cccd") class PicoBleQueueTests(unittest.TestCase): + @classmethod + def setUpClass(cls): + compiler = os.environ.get("CXX") or shutil.which("g++") or shutil.which("clang++") + if compiler is None: + raise unittest.SkipTest("a host C++17 compiler is required") + mesh = (ROOT / "examples/companion_radio/MyMesh.cpp").read_text() + header = (ROOT / "examples/companion_radio/MyMesh.h").read_text() + helpers = (ROOT / "src/helpers/TxtDataHelpers.cpp").read_text() + version = re.search(r"^#define FIRMWARE_VER_CODE\s+(\d+)", header, re.MULTILINE).group(1) + producer = r''' +namespace StrHelper { void strzcpy(char*, const char*, size_t); } +@COPY@ +#define CMD_DEVICE_QUERY 22 +#define RESP_CODE_DEVICE_INFO 13 +#define FIRMWARE_VER_CODE @VERSION@ +#define MAX_CONTACTS 100 +#define MAX_GROUP_CHANNELS 8 +#define FIRMWARE_VERSION "host-regression" +#define FIRMWARE_BUILD_DATE "16 Oct 2026" +struct { const char* getManufacturerName() const { return "PicoW"; } } board; +struct ActualDeviceQuery { + BaseSerialInterface* _serial = nullptr; + uint8_t app_target_ver = 0; + uint32_t _active_ble_pin = 123456; + struct { bool isRepeatEn() const { return false; } uint8_t path_hash_mode = 2; } _prefs; + void dispatch(uint8_t* cmd_frame, size_t len) { + uint8_t out_frame[MAX_FRAME_SIZE]{}; + @QUERY@ + } +}; +'''.replace("@COPY@", method(helpers, "void StrHelper::strzcpy(")) + producer = producer.replace("@VERSION@", version).replace( + "@QUERY@", method(mesh, "if (cmd_frame[0] == CMD_DEVICE_QUERY && len >= 2)")) + source = (ROOT / "src/helpers/rp2040/SerialBLEInterface.cpp").read_text() + check = method(source, "size_t SerialBLEInterface::checkRecvFrame(") + # Exact legacy admission behavior consumed RX without any TX capacity + # check. Keep the rest of the implementation unchanged for its oracle. + old_admission = check.replace("dest == nullptr || !isConnected() || recv_queue_len == 0\n" + " || !hasRequiredReplyCapacity()", "recv_queue_len == 0") + if old_admission == check: + raise AssertionError("production reply admission gate not found") + blocked = method(source, "if (!sent && required)") + old_mtu = source.replace(blocked, "(void)required; (void)sent;", 1) + immediate_retry = source.replace(blocked, "if (!sent && required) { kickSend(); return; }", 1) + # The throttle also prevents immediate recursion. Remove only that + # throttle for this deliberate synchronous-rearm negative control. + retry_guard = " if (_tx_retry_pending\n && !mesh::bleElapsedAtLeast((uint32_t)millis(), _tx_retry_started, 10)) return;\n" + if source.count(retry_guard) != 1: + raise AssertionError("production retry throttle not found") + immediate_retry = immediate_retry.replace(retry_guard, "", 1) + old_clock = source.replace("_tx_stall_watchdog.noteBlocked((uint32_t)millis())", + "_tx_stall_watchdog.noteBlocked(now)", 1) + if old_clock == source: + raise AssertionError("production post-callback watchdog clock not found") + variants = {"production": source, "legacy_admission": source.replace(check, old_admission, 1), + "legacy_mtu_drop": old_mtu, "immediate_retry": immediate_retry, "old_clock": old_clock} + cls.work = tempfile.TemporaryDirectory(prefix="meshcore-pico-ble-") + work = Path(cls.work.name) + fixture = work / "test.cpp" + fixture.write_text((FIXTURE / "test.cpp").read_text().replace("@QUERY_PRODUCER@", producer), encoding="ascii") + cls.binaries = {} + try: + for variant, implementation in variants.items(): + cpp = work / (variant + ".cpp") + cpp.write_text(implementation.replace('#include "SerialBLEInterface.h"', + '#include ').replace( + '#include "../CompanionFrameQueue.h"', '#include '), encoding="ascii") + binary = work / (variant + ".exe") + result = subprocess.run([ + compiler, "-std=c++17", "-Wall", "-Wextra", "-Werror", "-Wno-unused-parameter", + *SANITIZERS, "-DPIO_FRAMEWORK_ARDUINO_ENABLE_BLUETOOTH=1", + f"-I{FIXTURE / 'mocks'}", f"-I{ROOT / 'src'}", str(fixture), str(cpp), + "-o", str(binary)], capture_output=True, text=True, timeout=60) + if result.returncode: + raise AssertionError(result.stdout + result.stderr) + cls.binaries[variant] = binary + except BaseException: + cls.work.cleanup() + raise + + @classmethod + def tearDownClass(cls): + cls.work.cleanup() + + def run_delivery(self, variant, scenario): + return subprocess.run([str(self.binaries[variant]), scenario], capture_output=True, + text=True, timeout=10) + + def test_actual_pico_transport_admission_notifications_and_recovery(self): + for scenario in SCENARIOS: + with self.subTest(scenario=scenario): + result = self.run_delivery("production", scenario) + self.assertEqual(result.returncode, 0, result.stdout + result.stderr) + self.assertIn(f"PASS: Pico BLE delivery {scenario}", result.stdout) + + def test_old_admission_mtu_drop_and_synchronous_retry_controls(self): + for variant, scenario, oracle in ( + ("legacy_admission", "admission", "a full required TX queue must retain the command until its reply fits"), + ("legacy_mtu_drop", "mtu", "required DEVICE_QUERY response must survive an initial MTU of 23"), + ("immediate_retry", "mtu", "blocked notification must defer instead of reentering its callback"), + ("old_clock", "callback-clock", "a synchronous callback must not backdate the stall watchdog")): + with self.subTest(variant=variant): + result = self.run_delivery(variant, scenario) + self.assertNotEqual(result.returncode, 0) + self.assertIn(oracle, result.stderr) + # Unchanged healthy delivery is a positive control for every mutant. + for variant in self.binaries: + with self.subTest(positive=variant): + result = self.run_delivery(variant, "normal") + self.assertEqual(result.returncode, 0, result.stdout + result.stderr) + def test_pending_frames_keep_the_transport_awake_below_high_water(self): source = (ROOT / 'src/helpers/rp2040/SerialBLEInterface.cpp').read_text() methods = '\n'.join(method(source, 'bool SerialBLEInterface::' + name + '() const') for name in ('isReadBusy', 'isWriteBusy', 'hasPendingIO')) code = r''' #include +#include #define FRAME_QUEUE_SIZE 8 static int locks = 0; struct BluetoothLock { @@ -24,6 +149,7 @@ struct BluetoothLock { struct SerialBLEInterface { unsigned recv_queue_len = 0, send_queue_len = 0; bool _tx_pending = false; + mesh::BleDisconnectRecovery _tx_disconnect_recovery; bool isReadBusy() const; bool isWriteBusy() const; bool hasPendingIO() const; @@ -44,6 +170,11 @@ int main() { } } } + transport.recv_queue_len = transport.send_queue_len = 0; + transport._tx_pending = false; + transport._tx_disconnect_recovery.begin(); + assert(transport.isWriteBusy() && transport.hasPendingIO()); + assert(locks == 0); } '''.replace('@METHODS@', methods) for name in ('isReadBusy', 'isWriteBusy', 'hasPendingIO'): @@ -52,7 +183,8 @@ int main() { path = Path(directory) (path / 'test.cpp').write_text(code) subprocess.run(['g++', '-std=c++17', '-fsanitize=address,undefined', - '-fno-pie', '-no-pie', str(path / 'test.cpp'), '-o', str(path / 'test')], check=True) + '-fno-pie', '-no-pie', '-I' + str(ROOT / 'src'), + str(path / 'test.cpp'), '-o', str(path / 'test')], check=True) subprocess.run([str(path / 'test')], check=True)