Serialize nRF52 BLE receive frames across link resets

src/helpers/nrf52/BleRxFrameQueue.h

@@ -0,0 +1,120 @@
+#pragma once
+
+#include <stddef.h>
+#include <stdint.h>
+#include <string.h>
+
+namespace mesh_nrf52 {
+
+// Guard serializes task access on the single-core nRF52. Keep every guarded
+// operation bounded to metadata or one frame copy; UART FIFO operations must
+// run outside the guard because Bluefruit takes a FreeRTOS mutex there.
+template <typename Frame, size_t Capacity, typename Guard>
+class BleRxFrameQueue {
+  static_assert(Capacity > 0 && Capacity <= 255, "invalid BLE RX capacity");
+  static constexpr uint16_t INVALID_HANDLE = 0xffff;
+
+  Frame _frames[Capacity];
+  uint32_t _generation = 0;
+  uint16_t _handle = INVALID_HANDLE;
+  uint16_t _resets_in_progress = 0;
+  uint8_t _head = 0;
+  uint8_t _count = 0;
+  bool _enabled = false;
+  bool _secured = false;
+
+  void invalidate() {
+    ++_generation;
+    _head = _count = 0;
+  }
+
+  bool accepts(uint16_t handle, uint32_t generation) const {
+    return _enabled && _secured && !_resets_in_progress
+        && handle == _handle && generation == _generation;
+  }
+
+public:
+  enum class PushResult { Accepted, Full, Stale };
+
+  void setEnabled(bool enabled) {
+    Guard guard;
+    _enabled = enabled;
+    _handle = INVALID_HANDLE;
+    _secured = false;
+    invalidate();
+  }
+
+  void connect(uint16_t handle) {
+    Guard guard;
+    _handle = _enabled ? handle : INVALID_HANDLE;
+    _secured = false;
+    invalidate();
+  }
+
+  void secure(uint16_t handle) {
+    Guard guard;
+    if (_enabled && _handle == handle && handle != INVALID_HANDLE) {
+      _secured = true;
+    }
+  }
+
+  void disconnect(uint16_t handle) {
+    Guard guard;
+    if (_handle != handle) return;
+    _handle = INVALID_HANDLE;
+    _secured = false;
+    invalidate();
+  }
+
+  template <typename Flush>
+  void reset(Flush flush) {
+    {
+      Guard guard;
+      ++_resets_in_progress;
+      invalidate();
+    }
+    flush();
+    {
+      Guard guard;
+      --_resets_in_progress;
+    }
+  }
+
+  bool snapshot(uint16_t handle, uint32_t& generation) const {
+    Guard guard;
+    generation = _generation;
+    return accepts(handle, generation);
+  }
+
+  bool current(uint16_t handle, uint32_t generation) const {
+    Guard guard;
+    return accepts(handle, generation);
+  }
+
+  PushResult push(uint16_t handle, uint32_t generation, const Frame& frame) {
+    Guard guard;
+    if (!accepts(handle, generation)) return PushResult::Stale;
+    if (_count == Capacity) return PushResult::Full;
+    _frames[(_head + _count) % Capacity] = frame;
+    ++_count;
+    return PushResult::Accepted;
+  }
+
+  size_t pop(uint8_t* destination) {
+    Guard guard;
+    if (!_count) return 0;
+    const Frame& frame = _frames[_head];
+    const size_t length = frame.len;
+    memcpy(destination, frame.buf, length);
+    _head = (_head + 1) % Capacity;
+    --_count;
+    return length;
+  }
+
+  uint8_t size() const {
+    Guard guard;
+    return _count;
+  }
+};
+
+}  // namespace mesh_nrf52

src/helpers/nrf52/SerialBLEInterface.cpp

@@ -32,9 +32,6 @@ __attribute__((used, externally_visible)) MeshBleTraceBuffer mesh_ble_trace;
 #define BLE_ADV_INTERVAL_MAX       244    // 152.5ms (units: 0.625ms)
 #define BLE_ADV_FAST_TIMEOUT       30     // seconds
 
-// RX drain buffer size for overflow protection
-#define BLE_RX_DRAIN_BUF_SIZE      32
-
 static SerialBLEInterface* instance = nullptr;
 
 #if defined(MESH_NRF52_BLE_TRACE) && MESH_NRF52_BLE_TRACE
@@ -45,7 +42,7 @@ void SerialBLEInterface::formatDebugState(char* reply, size_t capacity) {
       (unsigned)_isDeviceConnected, (unsigned)Bluefruit.Advertising.isRunning(),
       (unsigned)Bluefruit.connected(),
       (unsigned)_companionDataSeen.load(std::memory_order_acquire),
-      (unsigned)send_queue_len, (unsigned)recv_queue_len,
+      (unsigned)send_queue_len, (unsigned)recv_queue.size(),
       (unsigned)_tx_disconnect_recovery.pending(),
       (unsigned)(_conn_handle != BLE_CONN_HANDLE_INVALID && bleuart.notifyEnabled(_conn_handle)),
       (unsigned)_debug_mtu.load());
@@ -273,6 +270,7 @@ void SerialBLEInterface::onSecured(uint16_t connection_handle) {
       }
 
       instance->_isDeviceConnected = true;
+      instance->recv_queue.secure(connection_handle);
 #if defined(MESH_NRF52_BLE_TRACE) && MESH_NRF52_BLE_TRACE
       mesh_ble_trace.secured.fetch_add(1);
       meshBleTrace(0xf006, connection_handle, conn->bonded(),
@@ -431,6 +429,10 @@ void SerialBLEInterface::onBLEEvent(ble_evt_t* evt) {
   if (evt->header.evt_id == BLE_GAP_EVT_CONNECTED) {
     ble_gap_evt_connected_t const* connected = &evt->evt.gap_evt.params.connected;
     if (connected->role == BLE_GAP_ROLE_PERIPH) {
+      // Observe the physical session in the BLE worker before its deferred
+      // onConnect callback. A disable/re-enable between the event and that
+      // callback must not allow the old link to reopen the RX queue.
+      instance->recv_queue.connect(evt->evt.gap_evt.conn_handle);
       instance->_peer_address = connected->peer_addr;
       instance->_peer_address_valid = true;
       instance->_bond_removed_for_connection = false;
@@ -480,6 +482,7 @@ void SerialBLEInterface::onBLEEvent(ble_evt_t* evt) {
   } else if (evt->header.evt_id == BLE_GATTS_EVT_SYS_ATTR_MISSING) {
     BLE_DEBUG_PRINTLN("CCCD RAM system attributes missing");
   } else if (evt->header.evt_id == BLE_GAP_EVT_DISCONNECTED) {
+    instance->recv_queue.disconnect(evt->evt.gap_evt.conn_handle);
     ble_gap_evt_disconnected_t const* disconnected = &evt->evt.gap_evt.params.disconnected;
     if (isBondAuthenticationFailure(disconnected->reason)) {
       instance->removeStoredBondForPeer("authentication disconnect");
@@ -986,11 +989,10 @@ void SerialBLEInterface::cancelStealthPairingTransition() {
 
 void SerialBLEInterface::clearBuffers() {
   send_queue_len = 0;
-  recv_queue_len = 0;
   _last_retry_attempt = 0;
   _tx_stall_watchdog.reset();
   _tx_disconnect_recovery.complete();
-  bleuart.flush();
+  recv_queue.reset([this]() { bleuart.flush(); });
 }
 
 void SerialBLEInterface::shiftSendQueueLeft() {
@@ -1002,15 +1004,6 @@ void SerialBLEInterface::shiftSendQueueLeft() {
   }
 }
 
-void SerialBLEInterface::shiftRecvQueueLeft() {
-  if (recv_queue_len > 0) {
-    recv_queue_len--;
-    for (uint8_t i = 0; i < recv_queue_len; i++) {
-      recv_queue[i] = recv_queue[i + 1];
-    }
-  }
-}
-
 size_t SerialBLEInterface::writeBleUartFrame(const Frame& frame) {
   BLEConnection* conn = Bluefruit.Connection(_conn_handle);
   if (conn == nullptr || !conn->connected() ||
@@ -1071,7 +1064,7 @@ void SerialBLEInterface::serviceTxRecovery(uint32_t now) {
 }
 
 void SerialBLEInterface::recoverStalledTx(const char* cause) {
-  meshBleTrace(0xf020, _conn_handle, send_queue_len, recv_queue_len);
+  meshBleTrace(0xf020, _conn_handle, send_queue_len, recv_queue.size());
   if (_tx_disconnect_recovery.pending()) return;
 
   BLE_DEBUG_PRINTLN("SerialBLEInterface: %s; forcing reconnect", cause);
@@ -1080,11 +1073,11 @@ void SerialBLEInterface::recoverStalledTx(const char* cause) {
   // disconnection, but make isConnected() false through the recovery state so
   // no more companion frames enter this damaged stream.
   send_queue_len = 0;
-  recv_queue_len = 0;
+  recv_queue.disconnect(_conn_handle);
   _last_retry_attempt = 0;
   _tx_stall_watchdog.reset();
   _companion_start_timer.cancel();
-  bleuart.flush();
+  recv_queue.reset([this]() { bleuart.flush(); });
 #if COMPANION_FEATURE_BLE_MOTA_SOURCE
   setMotaStreamActive(false);
 #endif
@@ -1113,6 +1106,7 @@ bool SerialBLEInterface::isAdvertising() const {
 void SerialBLEInterface::enable() {
   if (_isEnabled) return;
 
+  recv_queue.setEnabled(true);
   _pairingRequestPending.store(false, std::memory_order_release);
   _isEnabled = true;
   clearBuffers();
@@ -1126,6 +1120,7 @@ void SerialBLEInterface::enable() {
 
 void SerialBLEInterface::disconnect() {
   if (_conn_handle != BLE_CONN_HANDLE_INVALID) {
+    recv_queue.disconnect(_conn_handle);
     const uint32_t result = sd_ble_gap_disconnect(_conn_handle, BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION);
     meshBleTrace(0xf013, _conn_handle, result, 0);
     (void)result;
@@ -1133,6 +1128,7 @@ void SerialBLEInterface::disconnect() {
 }
 
 void SerialBLEInterface::disable() {
+  recv_queue.setEnabled(false);
   _isEnabled = false;
   _pairingRequestPending.store(false, std::memory_order_release);
   BLE_DEBUG_PRINTLN("SerialBLEInterface: disable");
@@ -1298,13 +1294,11 @@ size_t SerialBLEInterface::checkRecvFrame(uint8_t dest[]) {
   
   // TX/recovery above must run even when full. Retain RX until a required
   // command reply can be admitted instead of silently losing that reply.
-  if (recv_queue_len > 0 && hasRequiredReplyCapacity()) {
-    size_t len = recv_queue[0].len;
-    memcpy(dest, recv_queue[0].buf, len);
-    
-    BLE_DEBUG_PRINTLN("readBytes: sz=%u, hdr=%u", (unsigned)len, (unsigned)dest[0]);
-    
-    shiftRecvQueueLeft();
+  if (hasRequiredReplyCapacity()) {
+    const size_t len = recv_queue.pop(dest);
+    if (len) {
+      BLE_DEBUG_PRINTLN("readBytes: sz=%u, hdr=%u", (unsigned)len, (unsigned)dest[0]);
+    }
     return len;
   }
   
@@ -1312,50 +1306,49 @@ size_t SerialBLEInterface::checkRecvFrame(uint8_t dest[]) {
 }
 
 void SerialBLEInterface::onBleUartRX(uint16_t conn_handle) {
-  if (!instance) {
-    return;
-  }
-  
-  meshBleTrace(0xf030, conn_handle, instance->bleuart.available(),
-               instance->isConnected());
-  if (instance->_conn_handle != conn_handle || !instance->isConnected()) {
-    while (instance->bleuart.available() > 0) {
-      instance->bleuart.read();
-    }
-    return;
-  }
-  
-  while (instance->bleuart.available() > 0) {
-    if (instance->recv_queue_len >= FRAME_QUEUE_SIZE) {
-      while (instance->bleuart.available() > 0) {
-        instance->bleuart.read();
-      }
-      meshBleTrace(0xf034, conn_handle, instance->recv_queue_len);
-      BLE_DEBUG_PRINTLN("onBleUartRX: recv queue full, dropping data");
-      break;
-    }
-    
-    int avail = instance->bleuart.available();
-    
+  SerialBLEInterface* self = instance;
+  if (!self) return;
+
+  uint32_t generation;
+  if (!self->recv_queue.snapshot(conn_handle, generation)
+      || !self->isConnected()) return;
+  meshBleTrace(0xf030, conn_handle, self->bleuart.available(), true);
+
+  while (self->recv_queue.current(conn_handle, generation)) {
+    const int avail = self->bleuart.available();
+    if (avail <= 0) return;
     if (avail > MAX_FRAME_SIZE) {
       BLE_DEBUG_PRINTLN("onBleUartRX: WARN: BLE RX overflow, avail=%d, draining all", avail);
-      uint8_t drain_buf[BLE_RX_DRAIN_BUF_SIZE];
-      while (instance->bleuart.available() > 0) {
-        int chunk = instance->bleuart.available() > BLE_RX_DRAIN_BUF_SIZE ? BLE_RX_DRAIN_BUF_SIZE : instance->bleuart.available();
-        instance->bleuart.readBytes(drain_buf, chunk);
-      }
-      continue;
+      self->bleuart.flush();
+      return;
+    }
+
+    // BLEUart::read() is an immediate FIFO read. Stream::readBytes() may wait
+    // for bytes flushed by a simultaneous reset. Stage outside the critical
+    // section, then reject the frame if its connection generation changed.
+    Frame frame = {};
+    const int read_len = self->bleuart.read(frame.buf, avail);
+    if (read_len != avail) return;
+    frame.len = read_len;
+    RxQueue::PushResult result;
+    {
+      RxQueueGuard guard;
+      result = self->recv_queue.push(conn_handle, generation, frame);
+      if (result == RxQueue::PushResult::Accepted) {
+        self->_companionDataSeen.store(true, std::memory_order_release);
+        self->_companion_start_timer.cancel();
+      }
+    }
+    if (result == RxQueue::PushResult::Stale) return;
+    if (result == RxQueue::PushResult::Full) {
+      self->bleuart.flush();
+      meshBleTrace(0xf034, conn_handle, FRAME_QUEUE_SIZE);
+      BLE_DEBUG_PRINTLN("onBleUartRX: recv queue full, dropping data");
+      return;
     }
-    
-    int read_len = avail;
-    instance->recv_queue[instance->recv_queue_len].len = read_len;
-    instance->bleuart.readBytes(instance->recv_queue[instance->recv_queue_len].buf, read_len);
-    instance->recv_queue_len++;
 #if defined(MESH_NRF52_BLE_TRACE) && MESH_NRF52_BLE_TRACE
     mesh_ble_trace.rx.fetch_add(1);
 #endif
-    instance->_companionDataSeen.store(true, std::memory_order_release);
-    instance->_companion_start_timer.cancel();
   }
 }
 
@@ -1393,7 +1386,7 @@ uint16_t SerialBLEInterface::motaResponseCapacity() {
 #endif
 
 bool SerialBLEInterface::isReadBusy() const {
-  return (recv_queue_len > 0);
+  return recv_queue.size() > 0;
 }
 
 bool SerialBLEInterface::isWriteBusy() const {
@@ -1401,5 +1394,5 @@ bool SerialBLEInterface::isWriteBusy() const {
 }
 
 bool SerialBLEInterface::hasPendingIO() const {
-  return recv_queue_len > 0 || send_queue_len > 0;
+  return recv_queue.size() > 0 || send_queue_len > 0;
 }

src/helpers/nrf52/SerialBLEInterface.h

@@ -5,6 +5,7 @@
 #include "../BleTxStallWatchdog.h"
 #include "../UsbLogging.h"
 #include "SecuritySessionTimer.h"
+#include "BleRxFrameQueue.h"
 #include <bluefruit.h>
 #include <atomic>
 #if COMPANION_FEATURE_BLE_MOTA_SOURCE
@@ -105,13 +106,17 @@ class SerialBLEInterface : public BaseSerialInterface {
   uint8_t send_queue_len;
   Frame send_queue[FRAME_QUEUE_SIZE];
   
-  uint8_t recv_queue_len;
-  Frame recv_queue[FRAME_QUEUE_SIZE];
+  struct RxQueueGuard {
+    RxQueueGuard() { taskENTER_CRITICAL(); }
+    ~RxQueueGuard() { taskEXIT_CRITICAL(); }
+  };
+  using RxQueue = mesh_nrf52::BleRxFrameQueue<Frame, FRAME_QUEUE_SIZE,
+                                             RxQueueGuard>;
+  RxQueue recv_queue;
 
   void clearBuffers();
   bool hasRequiredReplyCapacity() const;
   void shiftSendQueueLeft();
-  void shiftRecvQueueLeft();
   size_t writeBleUartFrame(const Frame& frame);
   void recoverStalledTx(const char* cause);
   void serviceTxRecovery(uint32_t now);
@@ -156,7 +161,6 @@ public:
     _bonded_only = false;
     _bonded_only_configure_pending = false;
     send_queue_len = 0;
-    recv_queue_len = 0;
   }
 
   /**