Add BLE companion support for Pico W

2026-09-22 09:26:03 +00:00 · 2026-09-06 20:15:45 -05:00
parent d33fb4e9a7
commit 88cc014f55
4 changed files with 314 additions and 13 deletions
@@ -26,6 +26,10 @@ MultiSerialInterface interface_manager;
    // include nrf52 bluetooth interface
    #include <helpers/nrf52/SerialBLEInterface.h>
    SerialBLEInterface bluetooth_interface;
+  #elif defined(RP2040_PLATFORM)
+    // include rp2040 (Pico W / CYW43) bluetooth interface
+    #include <helpers/rp2040/SerialBLEInterface.h>
+    SerialBLEInterface bluetooth_interface;
  #else
    #error "SerialBLEInterface is not defined for this platform"
  #endif
@@ -0,0 +1,198 @@
+#include "SerialBLEInterface.h"
+#include <BluetoothLock.h>
+#include <stdio.h>
+#include <string.h>
+
+// 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;
+}
@@ -0,0 +1,75 @@
+#pragma once
+
+#include "../BaseSerialInterface.h"
+#include <BLE.h>
+#include <btstack.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.
+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 <Arduino.h>
+  #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
@@ -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}
+  +<helpers/rp2040/SerialBLEInterface.cpp>
+  +<../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}
+  +<helpers/rp2040/SerialBLEInterface.cpp>
+  +<helpers/wifi/*.cpp>
+  +<../examples/companion_radio/*.cpp>
+lib_deps = ${rpi_picow.lib_deps}
+  densaugeo/base64 @ ~1.4.0

 [env:PicoW_companion_radio_wifi]
 extends = rpi_picow