diff --git a/examples/companion_radio/main.cpp b/examples/companion_radio/main.cpp index ff0794ab9..85923a152 100644 --- a/examples/companion_radio/main.cpp +++ b/examples/companion_radio/main.cpp @@ -26,6 +26,10 @@ MultiSerialInterface interface_manager; // include nrf52 bluetooth interface #include SerialBLEInterface bluetooth_interface; + #elif defined(RP2040_PLATFORM) + // include rp2040 (Pico W / CYW43) bluetooth interface + #include + SerialBLEInterface bluetooth_interface; #else #error "SerialBLEInterface is not defined for this platform" #endif diff --git a/src/helpers/rp2040/SerialBLEInterface.cpp b/src/helpers/rp2040/SerialBLEInterface.cpp new file mode 100644 index 000000000..f03210039 --- /dev/null +++ b/src/helpers/rp2040/SerialBLEInterface.cpp @@ -0,0 +1,198 @@ +#include "SerialBLEInterface.h" +#include +#include +#include + +// Nordic UART 6E4000xx-B5A3-F393-E0A9-E50E24DCCA9E, as raw bytes: the core lib's string +// parser uses sscanf("%llx"), which newlib-nano on the RP2040 doesn't support (yields all zeros) +#define NUS_UUID(n) { 0x6E, 0x40, 0x00, n, 0xB5, 0xA3, 0xF3, 0x93, 0xE0, 0xA9, 0xE5, 0x0E, 0x24, 0xDC, 0xCA, 0x9E } +static const uint8_t SERVICE_UUID[16] = NUS_UUID(0x01); +static const uint8_t CHARACTERISTIC_UUID_RX[16] = NUS_UUID(0x02); +static const uint8_t CHARACTERISTIC_UUID_TX[16] = NUS_UUID(0x03); + +// The BLE core lib's own setValue()/notify path reallocs the value buffer on every call, +// so a second frame queued before the radio drained the first would corrupt it. We keep our +// own frame queue and drive att_server_notify() from the can-send-now callback instead. + +SerialBLEInterface::SerialBLEInterface() + : BLEService(BLEUUID(SERVICE_UUID)), + _rx(BLEUUID(CHARACTERISTIC_UUID_RX), BLEWrite, nullptr, ATT_SECURITY_AUTHENTICATED, ATT_SECURITY_AUTHENTICATED), + _tx(BLEUUID(CHARACTERISTIC_UUID_TX), BLERead | BLENotify, nullptr, ATT_SECURITY_AUTHENTICATED, ATT_SECURITY_AUTHENTICATED) +{ + _isEnabled = false; + _tx_pending = false; + send_queue_len = 0; + recv_queue_len = 0; + memset(&_can_send, 0, sizeof(_can_send)); + _rx.setCallbacks(this); + addCharacteristic(&_rx); + addCharacteristic(&_tx); +} + +void SerialBLEInterface::begin(const char* prefix, char* name, uint32_t pin_code) { + // JustWorks here only makes the server request pairing on connect; caps are overridden below + BLE.setSecurity(BLESecurityJustWorks); + // adv data carries the 128-bit service UUID, leaving room for only 8 name chars; + // the full name goes in the scan response + BLE.begin(prefix); + + if (strcmp(name, "@@MAC") == 0) { + bd_addr_t a; + gap_local_bd_addr(a); + sprintf(name, "%02X%02X%02X%02X%02X%02X", a[0], a[1], a[2], a[3], a[4], a[5]); // modify (IN-OUT param) + } + char dev_name[32+16]; + snprintf(dev_name, sizeof(dev_name), "%s%s", prefix, name); + + BLE.server()->setName(dev_name); // GAP device name characteristic + BLE.server()->addService(this); + BLE.server()->setCallbacks(this); + + // static passkey pairing with MITM protection, matching the esp32/nrf52 interfaces + sm_set_io_capabilities(IO_CAPABILITY_DISPLAY_ONLY); + sm_set_authentication_requirements(SM_AUTHREQ_MITM_PROTECTION | SM_AUTHREQ_BONDING); + sm_use_fixed_passkey_in_display_role(pin_code); + + size_t n = strlen(dev_name); + if (n > sizeof(_scan_rsp) - 2) n = sizeof(_scan_rsp) - 2; + _scan_rsp[0] = n + 1; + _scan_rsp[1] = BLUETOOTH_DATA_TYPE_COMPLETE_LOCAL_NAME; + memcpy(&_scan_rsp[2], dev_name, n); + gap_scan_response_set_data(n + 2, _scan_rsp); + + BLE_DEBUG_PRINTLN("begin: name=%s", dev_name); +} + +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 +} + +void SerialBLEInterface::onConnect(BLEServer* s) { + BLE_DEBUG_PRINTLN("connected handle=0x%04X", _tx.conHandle()); + clearBuffers(); +} + +void SerialBLEInterface::onDisconnect(BLEServer* s) { + BLE_DEBUG_PRINTLN("disconnected"); + clearBuffers(); // BTstack re-enables advertising on its own +} + +void SerialBLEInterface::onWrite(BLECharacteristic* c) { + if (c != &_rx) return; + size_t len = _rx.valueLen(); + if (len == 0 || len > MAX_FRAME_SIZE) { + BLE_DEBUG_PRINTLN("onWrite: bad frame len=%u", (unsigned)len); + return; + } + if (recv_queue_len >= FRAME_QUEUE_SIZE) { + BLE_DEBUG_PRINTLN("onWrite: recv queue full, dropping frame"); + return; + } + recv_queue[recv_queue_len].len = len; + memcpy(recv_queue[recv_queue_len].buf, _rx.valueData(), len); + recv_queue_len++; +} + +// caller holds the BT lock (or is in the BT context) +void SerialBLEInterface::kickSend() { + if (_tx_pending) 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) { + _tx_pending = false; + } +} + +// BT context: one notification per can-send-now, re-arm while frames remain +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; + } + 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]); + } + } + send_queue_len--; + memmove(&send_queue[0], &send_queue[1], send_queue_len * sizeof(Frame)); + if (send_queue_len > 0) kickSend(); +} + +void SerialBLEInterface::enable() { + if (_isEnabled) return; + _isEnabled = true; + clearBuffers(); + BLE.startAdvertising(true); +} + +void SerialBLEInterface::disconnect() { + uint16_t h = _tx.conHandle(); + if (h) gap_disconnect(h); +} + +void SerialBLEInterface::disable() { + _isEnabled = false; + BLE_DEBUG_PRINTLN("disable"); + disconnect(); + BLE.stopAdvertising(); +} + +bool SerialBLEInterface::isConnected() const { + // notifications can only be enabled once the link is authenticated (CCCD inherits the perms) + return _isEnabled && _tx.conHandle() != 0 && _tx.notifyEnabled(); +} + +bool SerialBLEInterface::isWriteBusy() const { + return send_queue_len >= (FRAME_QUEUE_SIZE * 2 / 3); +} + +size_t SerialBLEInterface::writeFrame(const uint8_t src[], size_t len) { + if (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) { + 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; + + size_t len = recv_queue[0].len; + memcpy(dest, recv_queue[0].buf, len); + recv_queue_len--; + memmove(&recv_queue[0], &recv_queue[1], recv_queue_len * sizeof(Frame)); + BLE_DEBUG_PRINTLN("readBytes: sz=%u, hdr=%u", (unsigned)len, dest[0]); + return len; +} diff --git a/src/helpers/rp2040/SerialBLEInterface.h b/src/helpers/rp2040/SerialBLEInterface.h new file mode 100644 index 000000000..2e085a6cc --- /dev/null +++ b/src/helpers/rp2040/SerialBLEInterface.h @@ -0,0 +1,75 @@ +#pragma once + +#include "../BaseSerialInterface.h" +#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. +class SerialBLEInterface : public BaseSerialInterface, BLEService, BLEServerCallbacks, BLECharacteristicCallbacks { + // subclass only to reach the protected connection/notify state + struct Characteristic : public BLECharacteristic { + using BLECharacteristic::BLECharacteristic; + uint16_t valueHandle() const { return _valueHandle; } + uint16_t conHandle() const { return con_handle; } + bool notifyEnabled() const { return _notificationEnabled; } + }; + + struct Frame { + uint8_t len; + uint8_t buf[MAX_FRAME_SIZE]; + }; + + #define FRAME_QUEUE_SIZE 8 + + Characteristic _rx; + Characteristic _tx; + bool _isEnabled; + bool _tx_pending; // a can-send-now callback is registered + btstack_context_callback_registration_t _can_send; + uint8_t _scan_rsp[31]; // complete local name; BTstack keeps the pointer + + uint8_t send_queue_len; + Frame send_queue[FRAME_QUEUE_SIZE]; + uint8_t recv_queue_len; + Frame recv_queue[FRAME_QUEUE_SIZE]; + + void clearBuffers(); + void kickSend(); + void sendNext(); + static void onCanSend(void* ctx) { ((SerialBLEInterface*)ctx)->sendNext(); } + + // BLE library callbacks (run in the BT context) + void onWrite(BLECharacteristic* c) override; + void onConnect(BLEServer* s) override; + void onDisconnect(BLEServer* s) override; + +public: + SerialBLEInterface(); + + /** + * init the BLE interface. + * @param prefix a prefix for the device name + * @param name IN/OUT - a name for the device (combined with prefix). If "@@MAC", is modified and returned + * @param pin_code the BLE security pin + */ + void begin(const char* prefix, char* name, uint32_t pin_code); + + void disconnect(); + void enable() override; + void disable() override; + bool isEnabled() const override { return _isEnabled; } + bool isConnected() const override; + bool isWriteBusy() const override; + size_t writeFrame(const uint8_t src[], size_t len) override; + size_t checkRecvFrame(uint8_t dest[]) override; +}; + +#if BLE_DEBUG_LOGGING && ARDUINO + #include + #define BLE_DEBUG_PRINT(F, ...) Serial.printf("BLE: " F, ##__VA_ARGS__) + #define BLE_DEBUG_PRINTLN(F, ...) Serial.printf("BLE: " F "\n", ##__VA_ARGS__) +#else + #define BLE_DEBUG_PRINT(...) {} + #define BLE_DEBUG_PRINTLN(...) {} +#endif diff --git a/variants/rpi_picow/platformio.ini b/variants/rpi_picow/platformio.ini index 32944a9a4..c41806505 100644 --- a/variants/rpi_picow/platformio.ini +++ b/variants/rpi_picow/platformio.ini @@ -67,19 +67,43 @@ lib_deps = ${rpi_picow.lib_deps} densaugeo/base64 @ ~1.4.0 lib_ignore = BLE -; [env:PicoW_companion_radio_ble] -; extends = rpi_picow -; build_flags = ${rpi_picow.build_flags} -; -D MAX_CONTACTS=100 -; -D MAX_GROUP_CHANNELS=8 -; -D BLE_PIN_CODE=123456 -; -D BLE_DEBUG_LOGGING=1 -; ; -D MESH_PACKET_LOGGING=1 -; ; -D MESH_DEBUG=1 -; build_src_filter = ${rpi_picow.build_src_filter} -; +<../examples/companion_radio/*.cpp> -; lib_deps = ${rpi_picow.lib_deps} -; densaugeo/base64 @ ~1.4.0 +[env:PicoW_companion_radio_ble] +extends = rpi_picow +build_flags = ${rpi_picow.build_flags} + -D MAX_CONTACTS=100 + -D MAX_GROUP_CHANNELS=8 + -D PIO_FRAMEWORK_ARDUINO_ENABLE_BLUETOOTH + -D BLE_PIN_CODE=123456 + -D BLE_DEBUG_LOGGING=1 +; -D MESH_PACKET_LOGGING=1 +; -D MESH_DEBUG=1 +build_src_filter = ${rpi_picow.build_src_filter} + + + +<../examples/companion_radio/*.cpp> +lib_deps = ${rpi_picow.lib_deps} + densaugeo/base64 @ ~1.4.0 + +; USB + WiFi + BLE together; the interface manager fans frames out to all of them +[env:PicoW_companion_radio_all] +extends = rpi_picow +build_flags = ${rpi_picow.build_flags} + -D MAX_CONTACTS=100 + -D MAX_GROUP_CHANNELS=8 + -D ENABLE_USB_INTERFACE + -D PIO_FRAMEWORK_ARDUINO_ENABLE_BLUETOOTH + -D BLE_PIN_CODE=123456 + -D BLE_DEBUG_LOGGING=1 + -D WIFI_DEBUG_LOGGING=1 + -D WIFI_SSID='"myssid"' + -D WIFI_PWD='"mypwd"' +; -D MESH_PACKET_LOGGING=1 +; -D MESH_DEBUG=1 +build_src_filter = ${rpi_picow.build_src_filter} + + + + + +<../examples/companion_radio/*.cpp> +lib_deps = ${rpi_picow.lib_deps} + densaugeo/base64 @ ~1.4.0 [env:PicoW_companion_radio_wifi] extends = rpi_picow