Properly set attribute handles and improve dynamic service changes

src/NimBLECharacteristic.cpp

@@ -165,21 +165,27 @@ void NimBLECharacteristic::removeDescriptor(NimBLEDescriptor* pDescriptor, bool
 /**
  * @brief Return the BLE Descriptor for the given UUID.
  * @param [in] uuid The UUID of the descriptor.
+ * @param [in] index The index of the descriptor to return (used when multiple descriptors have the same UUID).
  * @return A pointer to the descriptor object or nullptr if not found.
  */
-NimBLEDescriptor* NimBLECharacteristic::getDescriptorByUUID(const char* uuid) const {
-    return getDescriptorByUUID(NimBLEUUID(uuid));
+NimBLEDescriptor* NimBLECharacteristic::getDescriptorByUUID(const char* uuid, uint16_t index) const {
+    return getDescriptorByUUID(NimBLEUUID(uuid), index);
 } // getDescriptorByUUID
 
 /**
  * @brief Return the BLE Descriptor for the given UUID.
  * @param [in] uuid The UUID of the descriptor.
+ * @param [in] index The index of the descriptor to return (used when multiple descriptors have the same UUID).
  * @return A pointer to the descriptor object or nullptr if not found.
  */
-NimBLEDescriptor* NimBLECharacteristic::getDescriptorByUUID(const NimBLEUUID& uuid) const {
+NimBLEDescriptor* NimBLECharacteristic::getDescriptorByUUID(const NimBLEUUID& uuid, uint16_t index) const {
+    uint16_t position = 0;
     for (const auto& dsc : m_vDescriptors) {
         if (dsc->getUUID() == uuid) {
-            return dsc;
+            if (position == index) {
+                return dsc;
+            }
+            position++;
         }
     }
     return nullptr;

src/NimBLECharacteristic.h

@@ -69,8 +69,8 @@ class NimBLECharacteristic : public NimBLELocalValueAttribute {
                                        uint32_t          properties = NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::WRITE,
                                        uint16_t          maxLen     = BLE_ATT_ATTR_MAX_LEN);
     NimBLE2904*       create2904();
-    NimBLEDescriptor* getDescriptorByUUID(const char* uuid) const;
-    NimBLEDescriptor* getDescriptorByUUID(const NimBLEUUID& uuid) const;
+    NimBLEDescriptor* getDescriptorByUUID(const char* uuid, uint16_t index = 0) const;
+    NimBLEDescriptor* getDescriptorByUUID(const NimBLEUUID& uuid, uint16_t index = 0) const;
     NimBLEDescriptor* getDescriptorByHandle(uint16_t handle) const;
     NimBLEService*    getService() const;
 
@@ -113,7 +113,7 @@ class NimBLECharacteristic : public NimBLELocalValueAttribute {
     }
 
     /**
-         * @brief Template to send a notification with a value from a class that has a data() and size() method with value_type.
+     * @brief Template to send a notification with a value from a class that has a data() and size() method with value_type.
      * @param [in] v The value to send.
      * @param [in] connHandle Optional, a connection handle to send the notification to.
      * @details Correctly calculates byte size for containers with multi-byte element types.
@@ -125,11 +125,7 @@ class NimBLECharacteristic : public NimBLELocalValueAttribute {
     typename std::enable_if<Has_data_size<T>::value && Has_value_type<T>::value, bool>::type
 #  endif
     notify(const T& v, uint16_t connHandle = BLE_HS_CONN_HANDLE_NONE) const {
-        return notify(
-            reinterpret_cast<const uint8_t*>(v.data()),
-            v.size() * sizeof(typename T::value_type),
-            connHandle
-        );
+        return notify(reinterpret_cast<const uint8_t*>(v.data()), v.size() * sizeof(typename T::value_type), connHandle);
     }
 
     /**
@@ -193,11 +189,7 @@ class NimBLECharacteristic : public NimBLELocalValueAttribute {
     typename std::enable_if<Has_data_size<T>::value && Has_value_type<T>::value, bool>::type
 #  endif
     indicate(const T& v, uint16_t connHandle = BLE_HS_CONN_HANDLE_NONE) const {
-        return indicate(
-            reinterpret_cast<const uint8_t*>(v.data()),
-            v.size() * sizeof(typename T::value_type),
-            connHandle
-        );
+        return indicate(reinterpret_cast<const uint8_t*>(v.data()), v.size() * sizeof(typename T::value_type), connHandle);
     }
 
     /**
@@ -232,7 +224,9 @@ class NimBLECharacteristic : public NimBLELocalValueAttribute {
         const T& value, uint16_t connHandle = BLE_HS_CONN_HANDLE_NONE) const {
         if constexpr (Has_data_size<T>::value) {
             if constexpr (Has_value_type<T>::value) {
-                return notify(reinterpret_cast<const uint8_t*>(value.data()), value.size() * sizeof(typename T::value_type), connHandle);
+                return notify(reinterpret_cast<const uint8_t*>(value.data()),
+                              value.size() * sizeof(typename T::value_type),
+                              connHandle);
             } else {
                 return notify(reinterpret_cast<const uint8_t*>(value.data()), value.size(), connHandle);
             }
@@ -258,7 +252,9 @@ class NimBLECharacteristic : public NimBLELocalValueAttribute {
         const T& value, uint16_t connHandle = BLE_HS_CONN_HANDLE_NONE) const {
         if constexpr (Has_data_size<T>::value) {
             if constexpr (Has_value_type<T>::value) {
-                return indicate(reinterpret_cast<const uint8_t*>(value.data()), value.size() * sizeof(typename T::value_type), connHandle);
+                return indicate(reinterpret_cast<const uint8_t*>(value.data()),
+                                value.size() * sizeof(typename T::value_type),
+                                connHandle);
             } else {
                 return indicate(reinterpret_cast<const uint8_t*>(value.data()), value.size(), connHandle);
             }

src/NimBLEServer.cpp

@@ -39,6 +39,11 @@
 static const char*           LOG_TAG = "NimBLEServer";
 static NimBLEServerCallbacks defaultCallbacks;
 
+struct gattRegisterCallbackArgs {
+    NimBLEService*        pSvc{nullptr};
+    NimBLECharacteristic* pChar{nullptr};
+};
+
 /**
  * @brief Construct a BLE Server
  *
@@ -188,16 +193,96 @@ void NimBLEServer::serviceChanged() {
     }
 } // serviceChanged
 
+/**
+ * @brief Callback for GATT registration events,
+ * used to obtain the assigned handles for services, characteristics, and descriptors.
+ * @param [in] ctxt The context of the registration event.
+ * @param [in] arg Unused.
+ */
+void NimBLEServer::gattRegisterCallback(ble_gatt_register_ctxt* ctxt, void* arg) {
+    gattRegisterCallbackArgs* args = static_cast<gattRegisterCallbackArgs*>(arg);
+
+    if (ctxt->op == BLE_GATT_REGISTER_OP_SVC) {
+        NimBLEUUID uuid(ctxt->svc.svc_def->uuid);
+        for (auto i = 0; i < NimBLEDevice::getServer()->m_svcVec.size(); i++) {
+            auto pSvc = NimBLEDevice::getServer()->getServiceByUUID(uuid, i);
+            if (pSvc != nullptr && pSvc->m_handle == 0) {
+                pSvc->m_handle = ctxt->svc.handle;
+                NIMBLE_LOGD(LOG_TAG, "Service registered: %s, handle=%d", uuid.toString().c_str(), ctxt->svc.handle);
+                // Set the arg to the service so we know that the following
+                // characteristics and descriptors belong to this service
+                args->pSvc = pSvc;
+                break;
+            }
+        }
+
+        return;
+    }
+
+    if (args->pSvc == nullptr) {
+        // If the service is not found then this is likely a characteristic or descriptor that was registered as
+        // part of the GATT server setup and not found in the service vector
+        NIMBLE_LOGD(LOG_TAG, "Skipping characteristic or descriptor registered with unknown service");
+        return;
+    }
+
+    if (ctxt->op == BLE_GATT_REGISTER_OP_CHR) {
+        NimBLEUUID uuid(ctxt->chr.chr_def->uuid);
+        for (auto i = 0; i < args->pSvc->m_vChars.size(); i++) {
+            auto pChr = args->pSvc->getCharacteristic(uuid, i);
+            if (pChr != nullptr && pChr->m_handle == 0) {
+                pChr->m_handle = ctxt->chr.val_handle;
+                // Set the arg to the characteristic so we know that the following descriptors belong to this characteristic
+                args->pChar    = pChr;
+                NIMBLE_LOGD(LOG_TAG,
+                            "Characteristic registered: %s, def_handle=%d, val_handle=%d",
+                            uuid.toString().c_str(),
+                            ctxt->chr.def_handle,
+                            ctxt->chr.val_handle);
+                break;
+            }
+        }
+
+        return;
+    }
+
+    if (ctxt->op == BLE_GATT_REGISTER_OP_DSC) {
+        if (args->pChar == nullptr) {
+            NIMBLE_LOGE(LOG_TAG, "Descriptor registered with unknown characteristic, skipping");
+            return;
+        }
+
+        NimBLEUUID uuid(ctxt->dsc.dsc_def->uuid);
+        for (auto i = 0; i < args->pChar->m_vDescriptors.size(); i++) {
+            auto pDsc = args->pChar->getDescriptorByUUID(uuid, i);
+            if (pDsc != nullptr && pDsc->m_handle == 0) {
+                pDsc->m_handle = ctxt->dsc.handle;
+                NIMBLE_LOGD(LOG_TAG, "Descriptor registered: %s, handle=%d", uuid.toString().c_str(), ctxt->dsc.handle);
+                return;
+            }
+        }
+    }
+}
+
 /**
  * @brief Start the GATT server.
  * @details Required to be called after setup of all services and characteristics / descriptors
  * for the NimBLE host to register them.
  */
 void NimBLEServer::start() {
+    if (m_svcChanged && !getConnectedCount()) {
+        NIMBLE_LOGD(LOG_TAG, "Services have changed since last start, resetting GATT server");
+        resetGATT();
+    }
+
     if (m_gattsStarted) {
         return; // already started
     }
 
+    ble_hs_cfg.gatts_register_cb = NimBLEServer::gattRegisterCallback;
+    gattRegisterCallbackArgs args{};
+    ble_hs_cfg.gatts_register_arg = &args;
+
     int rc = ble_gatts_start();
     if (rc != 0) {
         NIMBLE_LOGE(LOG_TAG, "ble_gatts_start; rc=%d, %s", rc, NimBLEUtils::returnCodeToString(rc));
@@ -206,29 +291,20 @@ void NimBLEServer::start() {
 
 # if MYNEWT_VAL(NIMBLE_CPP_LOG_LEVEL) >= 4
     ble_gatts_show_local();
-# endif
 
-    // Get the assigned service handles and build a vector of characteristics
-    // with Notify / Indicate capabilities for event handling
+    // Check that all services were registered and log if any are missing.
     for (const auto& svc : m_svcVec) {
         if (svc->getRemoved() == 0) {
-            rc = ble_gatts_find_svc(svc->getUUID().getBase(), &svc->m_handle);
+            rc = ble_gatts_find_svc(svc->getUUID().getBase(), NULL);
             if (rc != 0) {
-                NIMBLE_LOGW(LOG_TAG,
+                NIMBLE_LOGD(LOG_TAG,
                             "GATT Server started without service: %s, Service %s",
                             svc->getUUID().toString().c_str(),
                             svc->isStarted() ? "missing" : "not started");
-                continue; // Skip this service as it was not started
-            }
-        }
-
-        // Set the descriptor handles now as the stack does not set these when the service is started
-        for (const auto& chr : svc->m_vChars) {
-            for (auto& desc : chr->m_vDescriptors) {
-                ble_gatts_find_dsc(svc->getUUID().getBase(), chr->getUUID().getBase(), desc->getUUID().getBase(), &desc->m_handle);
             }
         }
     }
+# endif
 
     // If the services have changed indicate it now
     if (m_svcChanged) {
@@ -427,10 +503,6 @@ int NimBLEServer::handleGapEvent(ble_gap_event* event, void* arg) {
             }
 # endif
 
-            if (pServer->m_svcChanged) {
-                pServer->resetGATT();
-            }
-
             peerInfo.m_desc = event->disconnect.conn;
             pServer->m_pServerCallbacks->onDisconnect(pServer, peerInfo, event->disconnect.reason);
 # if !MYNEWT_VAL(BLE_EXT_ADV)
@@ -784,32 +856,60 @@ void NimBLEServer::addService(NimBLEService* service) {
  * @brief Resets the GATT server, used when services are added/removed after initialization.
  */
 void NimBLEServer::resetGATT() {
-    if (getConnectedCount() > 0) {
-        return;
-    }
-
 # if MYNEWT_VAL(BLE_ROLE_BROADCASTER)
     NimBLEDevice::stopAdvertising();
 # endif
+
     ble_gatts_reset();
     ble_svc_gap_init();
     ble_svc_gatt_init();
 
-    for (auto it = m_svcVec.begin(); it != m_svcVec.end();) {
-        if ((*it)->getRemoved() > 0) {
-            if ((*it)->getRemoved() == NIMBLE_ATT_REMOVE_DELETE) {
-                delete *it;
-                it = m_svcVec.erase(it);
-            } else {
-                ++it;
-            }
+    // We clear the flag so the services can be started in the loop below
+    m_svcChanged = false;
+
+    for (auto svcIt = m_svcVec.begin(); svcIt != m_svcVec.end();) {
+        auto* pSvc = *svcIt;
+        if (pSvc->getRemoved() == NIMBLE_ATT_REMOVE_DELETE) {
+            delete pSvc;
+            svcIt = m_svcVec.erase(svcIt);
             continue;
         }
 
-        (*it)->start();
-        ++it;
+        for (auto chrIt = pSvc->m_vChars.begin(); chrIt != pSvc->m_vChars.end();) {
+            auto* pChr = *chrIt;
+            if (pChr->getRemoved() == NIMBLE_ATT_REMOVE_DELETE) {
+                delete pChr;
+                chrIt = pSvc->m_vChars.erase(chrIt);
+                continue;
+            }
+
+            for (auto dscIt = pChr->m_vDescriptors.begin(); dscIt != pChr->m_vDescriptors.end();) {
+                auto* pDsc = *dscIt;
+                if (pDsc->getRemoved() == NIMBLE_ATT_REMOVE_DELETE) {
+                    delete pDsc;
+                    dscIt = pChr->m_vDescriptors.erase(dscIt);
+                    continue;
+                }
+
+                pDsc->m_handle = 0;
+                ++dscIt;
+            }
+
+            pChr->m_handle = 0;
+            ++chrIt;
+        }
+
+        if (pSvc->getRemoved() == 0) {
+            pSvc->start();
+        }
+
+        pSvc->m_handle = 0;
+        ++svcIt;
     }
 
+    // Restore the flag to indicate the services have changed so that the
+    // GATT server start will send a service changed indication.
+    m_svcChanged   = true;
     m_gattsStarted = false;
 } // resetGATT
 

src/NimBLEServer.h

@@ -119,10 +119,11 @@ 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();
+    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);
+    static void gattRegisterCallback(struct ble_gatt_register_ctxt* ctxt, void* arg);
+    void        serviceChanged();
+    void        resetGATT();
 
     bool m_gattsStarted : 1;
     bool m_svcChanged : 1;

src/NimBLEService.cpp

@@ -89,7 +89,14 @@ void NimBLEService::dump() const {
  * @return bool success/failure .
  */
 bool NimBLEService::start() {
-    NIMBLE_LOGD(LOG_TAG, ">> start(): Starting service: %s", toString().c_str());
+    NIMBLE_LOGD(LOG_TAG, ">> start(): Starting service: UUID: %s", getUUID().toString().c_str());
+    // If the server has started before then we need to reset the GATT server
+    // to update the service/characteristic/descriptor definitions. If characteristics or descriptors
+    // have been added/removed since the last server start then this service will be started on gatt reset.
+    if (getServer()->m_svcChanged) {
+        NIMBLE_LOGW(LOG_TAG, "<< start(): GATT server already started, cannot start service");
+        return false;
+    }
 
     // If started previously and no characteristics have been added or removed,
     // then we can skip the service registration process.
@@ -113,7 +120,7 @@ bool NimBLEService::start() {
         ++numChrs;
     }
 
-    NIMBLE_LOGD(LOG_TAG, "Adding %d characteristics for service %s", numChrs, toString().c_str());
+    NIMBLE_LOGD(LOG_TAG, "Adding %d characteristics for service %s", numChrs, getUUID().toString().c_str());
     if (numChrs) {
         int i = 0;
 
@@ -160,7 +167,7 @@ bool NimBLEService::start() {
             pChrs[i].arg          = chr;
             pChrs[i].flags        = chr->getProperties();
             pChrs[i].min_key_size = 0;
-            pChrs[i].val_handle   = &chr->m_handle;
+            pChrs[i].val_handle   = nullptr;
             ++i;
         }
 

src/NimBLEUUID.cpp

@@ -38,6 +38,20 @@ static const uint8_t ble_base_uuid[] = {
  */
 NimBLEUUID::NimBLEUUID(const ble_uuid_any_t& uuid) : m_uuid{uuid} {}
 
+/**
+ * @brief Create a UUID from the native UUID pointer.
+ * @param [in] uuid The native UUID pointer.
+ */
+NimBLEUUID::NimBLEUUID(const ble_uuid_t* uuid) {
+    if (uuid == nullptr) {
+        NIMBLE_LOGE(LOG_TAG, "Invalid UUID pointer");
+        m_uuid.u.type = 0;
+        return;
+    }
+
+    ble_uuid_copy(&m_uuid, uuid);
+}
+
 /**
  * @brief Create a UUID from a string.
  *