Add callback for RX buffer overflow

Author: h2zero

src/NimBLEStream.cpp

@@ -485,9 +485,30 @@ size_t NimBLEStream::pushRx(const uint8_t* data, size_t len) {
         return 0;
     }
 
-    if (m_rxBuf->freeSize() < len) {
-        NIMBLE_LOGE(LOG_TAG, "RX buffer full, dropping data");
-        return 0;
+    size_t freeSize = m_rxBuf->freeSize();
+    if (len > freeSize) {
+        const bool dropOlderData = m_rxOverflowCallback && m_rxOverflowCallback(data, len);
+        if (!dropOlderData) {
+            NIMBLE_LOGE(LOG_TAG, "RX buffer overflow, dropping current data");
+            return 0;
+        }
+
+        if (len >= m_rxBuf->capacity()) {
+            m_rxBuf->drop(m_rxBuf->size());
+            const uint8_t* tail    = data + (len - m_rxBuf->capacity());
+            size_t         written = m_rxBuf->write(tail, m_rxBuf->capacity());
+            if (written != m_rxBuf->capacity()) {
+                NIMBLE_LOGE(LOG_TAG, "RX buffer overflow, %zu bytes dropped", m_rxBuf->capacity() - written);
+            }
+            return written;
+        }
+
+        const size_t requiredSpace = len - freeSize;
+        size_t       dropped       = m_rxBuf->drop(requiredSpace);
+        if (dropped < requiredSpace) {
+            NIMBLE_LOGE(LOG_TAG, "RX buffer overflow, failed to drop enough buffered data");
+            return 0;
+        }
     }
 
     return m_rxBuf->write(data, len);
@@ -522,8 +543,8 @@ bool NimBLEStreamServer::begin(NimBLECharacteristic* pChr, uint32_t txBufSize, u
         return false;
     }
 
-    auto props     = pChr->getProperties();
-    bool canWrite  = (props & NIMBLE_PROPERTY::WRITE) || (props & NIMBLE_PROPERTY::WRITE_NR);
+    auto props    = pChr->getProperties();
+    bool canWrite = (props & NIMBLE_PROPERTY::WRITE) || (props & NIMBLE_PROPERTY::WRITE_NR);
     if (!canWrite && rxBufSize > 0) {
         NIMBLE_LOGW(LOG_TAG, "Characteristic does not support WRITE, ignoring RX buffer size");
     }

src/NimBLEStream.h

@@ -27,6 +27,7 @@
 #  include "nimble/nimble/include/nimble/nimble_npl.h"
 # endif
 
+# include <functional>
 # include <type_traits>
 # include <cstdarg>
 
@@ -61,6 +62,8 @@ class Stream : public Print {
 
 class NimBLEStream : public Stream {
   public:
+    typedef std::function<bool(const uint8_t* data, size_t len)> rx_overflow_callback_t;
+
     NimBLEStream() = default;
     virtual ~NimBLEStream() { end(); }
 
@@ -87,6 +90,13 @@ class NimBLEStream : public Stream {
     virtual int  peek() override;
     virtual bool ready() const = 0;
 
+    /**
+     * @brief Set a callback to be invoked when incoming data exceeds RX buffer capacity.
+     * @param cb The callback function, which should return true to drop older buffered data and
+     * make room for the new data, or false to drop the new data instead.
+     */
+    void setRxOverflowCallback(rx_overflow_callback_t cb) { m_rxOverflowCallback = cb; }
+
     operator bool() const { return ready(); }
 
     using Print::write;
@@ -102,15 +112,16 @@ class NimBLEStream : public Stream {
     static void  txDrainEventCb(struct ble_npl_event* ev);
     static void  txDrainCalloutCb(struct ble_npl_event* ev);
 
-    ByteRingBuffer* m_txBuf{nullptr};
-    ByteRingBuffer* m_rxBuf{nullptr};
-    uint8_t         m_txChunkBuf[MYNEWT_VAL(BLE_ATT_PREFERRED_MTU)];
-    uint32_t        m_txBufSize{1024};
-    uint32_t        m_rxBufSize{1024};
-    ble_npl_event   m_txDrainEvent{};
-    ble_npl_callout m_txDrainCallout{};
-    bool            m_coInitialized{false};
-    bool            m_eventInitialized{false};
+    ByteRingBuffer*        m_txBuf{nullptr};
+    ByteRingBuffer*        m_rxBuf{nullptr};
+    uint8_t                m_txChunkBuf[MYNEWT_VAL(BLE_ATT_PREFERRED_MTU)];
+    uint32_t               m_txBufSize{1024};
+    uint32_t               m_rxBufSize{1024};
+    ble_npl_event          m_txDrainEvent{};
+    ble_npl_callout        m_txDrainCallout{};
+    bool                   m_coInitialized{false};
+    bool                   m_eventInitialized{false};
+    rx_overflow_callback_t m_rxOverflowCallback{nullptr};
 };
 
 # if MYNEWT_VAL(BLE_ROLE_PERIPHERAL)