Refactor notify/indicate

This refactors the handling of sending notifications and indications for greater efficiency.
* Adds client subscription state tracking to NimBLECharacteristic rather than relying on the stack.
* Notifications/indications are now sent directly, no longer calling the callback to read the values.
  This avoids delays and flash writes in the stack, allowing for greater throughput.

Author: h2zero

src/NimBLECharacteristic.cpp

@@ -15,15 +15,15 @@
  * limitations under the License.
  */
 
-# include "NimBLECharacteristic.h"
+#include "NimBLECharacteristic.h"
 #if CONFIG_BT_NIMBLE_ENABLED && MYNEWT_VAL(BLE_ROLE_PERIPHERAL)
 
-#if defined(CONFIG_NIMBLE_CPP_IDF)
-# if !defined(ESP_IDF_VERSION_MAJOR) || ESP_IDF_VERSION_MAJOR < 5
-#  define ble_gatts_notify_custom ble_gattc_notify_custom
-#  define ble_gatts_indicate_custom ble_gattc_indicate_custom
+# if defined(CONFIG_NIMBLE_CPP_IDF)
+#  if !defined(ESP_IDF_VERSION_MAJOR) || ESP_IDF_VERSION_MAJOR < 5
+#   define ble_gatts_notify_custom   ble_gattc_notify_custom
+#   define ble_gatts_indicate_custom ble_gattc_indicate_custom
+#  endif
 # endif
-#endif
 
 # include "NimBLE2904.h"
 # include "NimBLEDevice.h"
@@ -225,7 +225,8 @@ void NimBLECharacteristic::setService(NimBLEService* pService) {
  * @return True if the indication was sent successfully, false otherwise.
  */
 bool NimBLECharacteristic::indicate(uint16_t connHandle) const {
-    return sendValue(nullptr, 0, false, connHandle);
+    auto value{m_value}; // make a copy to avoid issues if the value is changed while indicating
+    return sendValue(value.data(), value.size(), false, connHandle);
 } // indicate
 
 /**
@@ -247,7 +248,8 @@ bool NimBLECharacteristic::indicate(const uint8_t* value, size_t length, uint16_
  * @return True if the notification was sent successfully, false otherwise.
  */
 bool NimBLECharacteristic::notify(uint16_t connHandle) const {
-    return sendValue(nullptr, 0, true, connHandle);
+    auto value{m_value}; // make a copy to avoid issues if the value is changed while notifying
+    return sendValue(value.data(), value.size(), true, connHandle);
 } // notify
 
 /**
@@ -271,55 +273,50 @@ bool NimBLECharacteristic::notify(const uint8_t* value, size_t length, uint16_t
  * @return True if the value was sent successfully, false otherwise.
  */
 bool NimBLECharacteristic::sendValue(const uint8_t* value, size_t length, bool isNotification, uint16_t connHandle) const {
-    int rc = 0;
-
-    if (value != nullptr && length > 0) { // custom notification value
-        os_mbuf* om = nullptr;
-
-        if (connHandle != BLE_HS_CONN_HANDLE_NONE) { // only sending to specific peer
-            om = ble_hs_mbuf_from_flat(value, length);
-            if (!om) {
-                rc = BLE_HS_ENOMEM;
-                goto done;
-            }
-
-            // Null buffer will read the value from the characteristic
-            if (isNotification) {
-                rc = ble_gatts_notify_custom(connHandle, m_handle, om);
-            } else {
-                rc = ble_gatts_indicate_custom(connHandle, m_handle, om);
-            }
+    ble_npl_hw_enter_critical();
+    const auto subs = getSubscribers(); // make a copy to avoid issues if subscribers change while sending
+    ble_npl_hw_exit_critical(0);
+
+    bool chSpecified = connHandle != BLE_HS_CONN_HANDLE_NONE;
+    bool requireSecure = m_properties & (BLE_GATT_CHR_F_READ_ENC | BLE_GATT_CHR_F_READ_AUTHEN | BLE_GATT_CHR_F_READ_AUTHOR);
+    int rc = chSpecified ? BLE_HS_ENOENT : 0; // if handle specified, assume not found until sent
+
+    // Notify all connected peers unless a specific handle is provided
+    for (const auto& entry : subs) {
+        uint16_t ch = entry.getConnHandle();
+        if (ch == BLE_HS_CONN_HANDLE_NONE || (chSpecified && ch != connHandle)) {
+            continue;
+        }
 
-            goto done;
+        if (requireSecure && !entry.isSecured()) {
+            NIMBLE_LOGW(LOG_TAG, "skipping notify/indicate to connHandle=%d, link not secured", entry.getConnHandle());
+            continue;
         }
 
-        // Notify all connected peers unless a specific handle is provided
-        for (const auto& ch : NimBLEDevice::getServer()->getPeerDevices()) {
-            // Must re-create the data buffer on each iteration because it is freed by the calls bellow.
+        // Must re-create the data buffer on each iteration because it is freed by the calls below.
+        uint8_t  retries = 10; // wait up to 10ms for a free buffer
+        os_mbuf* om      = ble_hs_mbuf_from_flat(value, length);
+        while (!om && --retries) {
+            ble_npl_time_delay(ble_npl_time_ms_to_ticks32(1));
             om = ble_hs_mbuf_from_flat(value, length);
-            if (!om) {
-                rc = BLE_HS_ENOMEM;
-                goto done;
-            }
+        }
 
-            if (isNotification) {
-                rc = ble_gatts_notify_custom(ch, m_handle, om);
-            } else {
-                rc = ble_gatts_indicate_custom(ch, m_handle, om);
-            }
+        if (!om) {
+            rc = BLE_HS_ENOMEM;
+            break;
         }
-    } else if (connHandle != BLE_HS_CONN_HANDLE_NONE) {
-        // Null buffer will read the value from the characteristic
+
         if (isNotification) {
-            rc = ble_gatts_notify_custom(connHandle, m_handle, nullptr);
+            rc = ble_gatts_notify_custom(ch, m_handle, om);
         } else {
-            rc = ble_gatts_indicate_custom(connHandle, m_handle, nullptr);
+            rc = ble_gatts_indicate_custom(ch, m_handle, om);
+        }
+
+        if (rc != 0 || chSpecified) {
+            break;
         }
-    } else { // Notify or indicate to all connected peers the characteristic value
-        ble_gatts_chr_updated(m_handle);
     }
 
-done:
     if (rc != 0) {
         NIMBLE_LOGE(LOG_TAG, "failed to send value, rc=%d %s", rc, NimBLEUtils::returnCodeToString(rc));
         return false;
@@ -328,10 +325,108 @@ bool NimBLECharacteristic::sendValue(const uint8_t* value, size_t length, bool i
     return true;
 } // sendValue
 
+/**
+ * @brief Process a subscription or unsubscription request from a peer.
+ * @param[in] connInfo A reference to the connection info of the peer.
+ * @param[in] subVal The subscription value (bitmask).
+ */
+void NimBLECharacteristic::processSubRequest(NimBLEConnInfo& connInfo, uint8_t subVal) const {
+    // Only allocate subscribers for characteristics that support notify or indicate.
+    const uint16_t props = getProperties();
+    if (!(props & (BLE_GATT_CHR_F_NOTIFY | BLE_GATT_CHR_F_INDICATE))) {
+        return;
+    }
+
+    auto found     = false;
+    auto firstFree = -1;
+    for (auto& entry : m_subPeers) {
+        if (entry.getConnHandle() == connInfo.getConnHandle()) {
+            found = true;
+            if (!subVal) {
+                ble_npl_hw_enter_critical();
+                entry = SubPeerEntry{}; // unsubscribed, reset entry
+                ble_npl_hw_exit_critical(0);
+            }
+            break;
+        }
+
+        if (firstFree == -1 && entry.getConnHandle() == BLE_HS_CONN_HANDLE_NONE) {
+            firstFree = &entry - &m_subPeers[0];
+        }
+    }
+
+    if (!found && subVal) {
+        if (firstFree >= 0) {
+            ble_npl_hw_enter_critical();
+            m_subPeers[firstFree].setConnHandle(connInfo.getConnHandle());
+            m_subPeers[firstFree].setSubNotify(subVal & 0x1);
+            m_subPeers[firstFree].setSubIndicate(subVal & 0x2);
+            m_subPeers[firstFree].setSecured(connInfo.isEncrypted() || connInfo.isAuthenticated() || connInfo.isBonded());
+            if (m_properties & (BLE_GATT_CHR_F_READ_AUTHEN | BLE_GATT_CHR_F_READ_AUTHOR | BLE_GATT_CHR_F_READ_ENC)) {
+                // characteristic requires security/authorization
+                if (!m_subPeers[firstFree].isSecured()) {
+                    m_subPeers[firstFree].setAwaitingSecure(true);
+                    ble_npl_hw_exit_critical(0);
+                    NimBLEDevice::startSecurity(connInfo.getConnHandle());
+                    NIMBLE_LOGD(LOG_TAG,
+                                "Subscription deferred until link is secured for connHandle=%d",
+                                connInfo.getConnHandle());
+                    return;
+                }
+            }
+            ble_npl_hw_exit_critical(0);
+        } else {
+            // should never happen, but log just in case
+            NIMBLE_LOGE(LOG_TAG, "No free subscription slots");
+            return;
+        }
+    }
+
+    m_pCallbacks->onSubscribe(const_cast<NimBLECharacteristic*>(this), const_cast<NimBLEConnInfo&>(connInfo), subVal);
+}
+
+/**
+ * @brief Update the security status of a subscribed peer.
+ * @param[in] connHandle The connection handle of the peer.
+ * @param[in] peerInfo A reference to the connection info of the peer.
+ */
+void NimBLECharacteristic::updatePeerStatus(const NimBLEConnInfo& peerInfo) const {
+    if (!(getProperties() & (NIMBLE_PROPERTY::NOTIFY | NIMBLE_PROPERTY::INDICATE))) {
+        return;
+    }
+
+    ble_npl_hw_enter_critical();
+    for (auto& entry : m_subPeers) {
+        if (entry.getConnHandle() == peerInfo.getConnHandle()) {
+            entry.setSecured(peerInfo.isEncrypted() || peerInfo.isAuthenticated() || peerInfo.isBonded());
+            if (entry.isAwaitingSecure()) {
+                entry.setAwaitingSecure(false);
+                ble_npl_hw_exit_critical(0);
+                m_pCallbacks->onSubscribe(const_cast<NimBLECharacteristic*>(this),
+                                          const_cast<NimBLEConnInfo&>(peerInfo),
+                                          entry.isSubNotify() | (entry.isSubIndicate() << 1));
+                return;
+            }
+            break;
+        }
+    }
+    ble_npl_hw_exit_critical(0);
+}
+
+/**
+ * @brief Handle a read event from a client.
+ * @param [in] connInfo A reference to a NimBLEConnInfo instance containing the peer info.
+ */
 void NimBLECharacteristic::readEvent(NimBLEConnInfo& connInfo) {
     m_pCallbacks->onRead(this, connInfo);
 } // readEvent
 
+/**
+ * @brief Handle a write event from a client.
+ * @param [in] val A pointer to the data written by the client.
+ * @param [in] len The length of the data written by the client.
+ * @param [in] connInfo A reference to a NimBLEConnInfo instance containing the peer info.
+ */
 void NimBLECharacteristic::writeEvent(const uint8_t* val, uint16_t len, NimBLEConnInfo& connInfo) {
     setValue(val, len);
     m_pCallbacks->onWrite(this, connInfo);

src/NimBLECharacteristic.h

@@ -31,6 +31,7 @@ class NimBLE2904;
 
 # include <string>
 # include <vector>
+# include <array>
 
 /**
  * @brief The model of a BLE Characteristic.
@@ -233,9 +234,34 @@ class NimBLECharacteristic : public NimBLELocalValueAttribute {
                    bool           is_notification = true,
                    uint16_t       connHandle      = BLE_HS_CONN_HANDLE_NONE) const;
 
+    struct SubPeerEntry {
+        enum : uint8_t { AWAITING_SECURE = 1 << 0, SECURE = 1 << 1, SUB_NOTIFY = 1 << 2, SUB_INDICATE = 1 << 3 };
+        void     setConnHandle(uint16_t connHandle) { m_connHandle = connHandle; }
+        uint16_t getConnHandle() const { return m_connHandle; }
+        void setAwaitingSecure(bool awaiting) { awaiting ? m_flags |= AWAITING_SECURE : m_flags &= ~AWAITING_SECURE; }
+        void setSecured(bool secure) { secure ? m_flags |= SECURE : m_flags &= ~SECURE; }
+        void setSubNotify(bool notify) { notify ? m_flags |= SUB_NOTIFY : m_flags &= ~SUB_NOTIFY; }
+        void setSubIndicate(bool indicate) { indicate ? m_flags |= SUB_INDICATE : m_flags &= ~SUB_INDICATE; }
+        bool isSubNotify() const { return m_flags & SUB_NOTIFY; }
+        bool isSubIndicate() const { return m_flags & SUB_INDICATE; }
+        bool isSecured() const { return m_flags & SECURE; }
+        bool isAwaitingSecure() const { return m_flags & AWAITING_SECURE; }
+
+      private:
+        uint16_t m_connHandle{BLE_HS_CONN_HANDLE_NONE};
+        uint8_t  m_flags{0};
+    };
+    __attribute__((packed));
+
+    using SubPeerArray = std::array<SubPeerEntry, MYNEWT_VAL(BLE_MAX_CONNECTIONS)>;
+    SubPeerArray getSubscribers() const { return m_subPeers; }
+    void         processSubRequest(NimBLEConnInfo& connInfo, uint8_t subVal) const;
+    void         updatePeerStatus(const NimBLEConnInfo& peerInfo) const;
+
     NimBLECharacteristicCallbacks* m_pCallbacks{nullptr};
     NimBLEService*                 m_pService{nullptr};
     std::vector<NimBLEDescriptor*> m_vDescriptors{};
+    mutable SubPeerArray           m_subPeers{};
 }; // NimBLECharacteristic
 
 /**