Refactor notify/indicate

This refactors the handling of sending notifications and indications for greater efficiency.
* Adds client subscription state tracking to NimBLEServer 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,36 @@ 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);
-            }
+    const auto subs = NimBLEDevice::getServer()->getSubscribers(m_handle);
+    if (subs == nullptr) {
+        return true;
+    }
 
-            goto done;
+    // Notify all connected peers unless a specific handle is provided
+    int rc = 0;
+    for (auto ch : *subs) {
+        if (ch == BLE_HS_CONN_HANDLE_NONE || (connHandle != BLE_HS_CONN_HANDLE_NONE && ch != connHandle)) {
+            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.
-            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);
-            }
+        // Must re-create the data buffer on each iteration because it is freed by the calls bellow.
+        os_mbuf* om = ble_hs_mbuf_from_flat(value, length);
+        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 || connHandle != BLE_HS_CONN_HANDLE_NONE) {
+            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;

src/NimBLEServer.cpp

@@ -205,6 +205,20 @@ void NimBLEServer::start() {
                             svc->isStarted() ? "missing" : "not started");
                 continue; // Skip this service as it was not started
             }
+
+            for (const auto& chr : svc->m_vChars) {
+                if (chr->getRemoved() != 0) {
+                    continue;
+                }
+
+                if ((chr->getProperties() & BLE_GATT_CHR_F_INDICATE) || (chr->getProperties() & BLE_GATT_CHR_F_NOTIFY)) {
+                    NIMBLE_LOGD(LOG_TAG,
+                                "Adding Notify/Indicate characteristic: %s, handle=%d",
+                                chr->getUUID().toString().c_str(),
+                                chr->getHandle());
+                    m_subChars.emplace_back(chr->getHandle());
+                }
+            }
         }
 
         // Set the descriptor handles now as the stack does not set these when the service is started
@@ -360,9 +374,7 @@ int NimBLEServer::handleGapEvent(ble_gap_event* event, void* arg) {
             }
 
             if (rc != 0) {
-                NIMBLE_LOGE(LOG_TAG, "Connection failed rc = %d %s",
-                            rc,
-                            NimBLEUtils::returnCodeToString(rc));
+                NIMBLE_LOGE(LOG_TAG, "Connection failed rc = %d %s", rc, NimBLEUtils::returnCodeToString(rc));
 # if !MYNEWT_VAL(BLE_EXT_ADV) && MYNEWT_VAL(BLE_ROLE_BROADCASTER)
                 NimBLEDevice::startAdvertising();
 # endif
@@ -429,33 +441,72 @@ int NimBLEServer::handleGapEvent(ble_gap_event* event, void* arg) {
         } // BLE_GAP_EVENT_DISCONNECT
 
         case BLE_GAP_EVENT_SUBSCRIBE: {
-            NIMBLE_LOGI(LOG_TAG,
-                        "subscribe event; attr_handle=%d, subscribed: %s",
-                        event->subscribe.attr_handle,
-                        ((event->subscribe.cur_notify || event->subscribe.cur_indicate) ? "true" : "false"));
+            rc = ble_gap_conn_find(event->subscribe.conn_handle, &peerInfo.m_desc);
+            if (rc != 0) {
+                break;
+            }
+
+            uint8_t subVal = event->subscribe.cur_notify + (event->subscribe.cur_indicate << 1);
+            NIMBLE_LOGI(LOG_TAG, "subscribe event; attr_handle=%d, subscribed: %d", event->subscribe.attr_handle, subVal);
 
+            NimBLECharacteristic* pChar = nullptr;
             for (const auto& svc : pServer->m_svcVec) {
                 for (const auto& chr : svc->m_vChars) {
                     if (chr->getHandle() == event->subscribe.attr_handle) {
-                        rc = ble_gap_conn_find(event->subscribe.conn_handle, &peerInfo.m_desc);
-                        if (rc != 0) {
-                            break;
-                        }
-
-                        auto chrProps = chr->getProperties();
-                        if (!peerInfo.isEncrypted() &&
-                            (chrProps & BLE_GATT_CHR_F_READ_AUTHEN || chrProps & BLE_GATT_CHR_F_READ_AUTHOR ||
-                             chrProps & BLE_GATT_CHR_F_READ_ENC)) {
-                            NimBLEDevice::startSecurity(event->subscribe.conn_handle);
-                        }
-
-                        chr->m_pCallbacks->onSubscribe(chr,
-                                                       peerInfo,
-                                                       event->subscribe.cur_notify + (event->subscribe.cur_indicate << 1));
+                        pChar = chr;
+                        break;
                     }
                 }
+
+                if (pChar) {
+                    break;
+                }
+            }
+
+            if (!pChar) {
+                NIMBLE_LOGE(LOG_TAG, "subscribe event; attr_handle=%d, not found", event->subscribe.attr_handle);
+                break;
+            }
+
+            auto subs = pServer->getSubscribers(pChar->getHandle());
+            if (subs == nullptr) {
+                NIMBLE_LOGE(LOG_TAG, "No subscriber list for handle %d", pChar->getHandle());
+                break;
+            }
+
+            bool found     = false;
+            int  firstFree = -1;
+            for (auto i = 0; i < subs->size(); i++) {
+                if ((*subs)[i] == event->subscribe.conn_handle) {
+                    found = true;
+                    if (subVal == 0) { // remove subscriber
+                        (*subs)[i] = BLE_HS_CONN_HANDLE_NONE;
+                        break;
+                    }
+                }
+
+                if (firstFree < 0 && (*subs)[i] == BLE_HS_CONN_HANDLE_NONE) {
+                    firstFree = i;
+                }
+            }
+
+            if (!found) {
+                if (firstFree >= 0) {
+                    (*subs)[firstFree] = event->subscribe.conn_handle;
+                } else {
+                    // This should never happen
+                    NIMBLE_LOGE(LOG_TAG, "Unable to add subscriber for handle %d", pChar->getHandle());
+                    break;
+                }
+
+                auto chrProps = pChar->getProperties();
+                if (!peerInfo.isEncrypted() && (chrProps & BLE_GATT_CHR_F_READ_AUTHEN || chrProps & BLE_GATT_CHR_F_READ_AUTHOR ||
+                                                chrProps & BLE_GATT_CHR_F_READ_ENC)) {
+                    NimBLEDevice::startSecurity(event->subscribe.conn_handle);
+                }
             }
 
+            pChar->m_pCallbacks->onSubscribe(pChar, peerInfo, subVal);
             break;
         } // BLE_GAP_EVENT_SUBSCRIBE
 

src/NimBLEServer.h

@@ -118,26 +118,42 @@ class NimBLEServer {
 
     NimBLEServer();
     ~NimBLEServer();
+    static int handleGapEvent(struct ble_gap_event* event, void* arg);
+    static int handleGattEvent(uint16_t connHandle, uint16_t attrHandle, ble_gatt_access_ctxt* ctxt, void* arg);
+    void       serviceChanged();
+    void       resetGATT();
+
+    using ConnHandleArray = std::array<uint16_t, MYNEWT_VAL(BLE_MAX_CONNECTIONS)>;
+    struct AttSubChar {
+        AttSubChar() = delete;
+        AttSubChar(uint16_t attHandle) : m_attHandle(attHandle) { m_connHandles.fill(BLE_HS_CONN_HANDLE_NONE); }
+        uint16_t        m_attHandle;
+        ConnHandleArray m_connHandles;
+    };
+
+    ConnHandleArray* getSubscribers(uint16_t attHandle) {
+        for (auto& s : m_subChars) {
+            if (s.m_attHandle == attHandle) {
+                return &s.m_connHandles;
+            }
+        }
+        return nullptr;
+    }
 
     bool m_gattsStarted : 1;
     bool m_svcChanged : 1;
     bool m_deleteCallbacks : 1;
 # if !MYNEWT_VAL(BLE_EXT_ADV)
     bool m_advertiseOnDisconnect : 1;
 # endif
-    NimBLEServerCallbacks*                                 m_pServerCallbacks;
-    std::vector<NimBLEService*>                            m_svcVec;
-    std::array<uint16_t, MYNEWT_VAL(BLE_MAX_CONNECTIONS)> m_connectedPeers;
+    NimBLEServerCallbacks*      m_pServerCallbacks;
+    std::vector<NimBLEService*> m_svcVec;
+    std::vector<AttSubChar>     m_subChars;
+    ConnHandleArray             m_connectedPeers;
 
 # if MYNEWT_VAL(BLE_ROLE_CENTRAL)
     NimBLEClient* m_pClient{nullptr};
 # endif
-
-    static int handleGapEvent(struct ble_gap_event* event, void* arg);
-    static int handleGattEvent(uint16_t connHandle, uint16_t attrHandle, ble_gatt_access_ctxt* ctxt, void* arg);
-    void       serviceChanged();
-    void       resetGATT();
-
 }; // NimBLEServer
 
 /**