[Server] Implement remove Characteristics/Descriptors. (#54)

This allows for adding and removing of characteristics and descriptors after initial setup of the server.
When used it will send a service changed notification to all clients.
The changes will not take effect until all clients have disconnected and advertising restarted.

src/NimBLECharacteristic.cpp

@@ -53,6 +53,7 @@ NimBLECharacteristic::NimBLECharacteristic(const NimBLEUUID &uuid, uint16_t prop
     m_value       = "";
     m_valMux      = portMUX_INITIALIZER_UNLOCKED;
     m_timestamp   = 0;
+    m_removed     = 0;
 } // NimBLECharacteristic
 
 /**
@@ -95,20 +96,62 @@ NimBLEDescriptor* NimBLECharacteristic::createDescriptor(const NimBLEUUID &uuid,
     }
 
     addDescriptor(pDescriptor);
-
     return pDescriptor;
 } // createDescriptor
 
 
 /**
  * @brief Add a descriptor to the characteristic.
- * @param [in] A pointer to the descriptor to add.
+ * @param [in] pDescriptor A pointer to the descriptor to add.
  */
 void NimBLECharacteristic::addDescriptor(NimBLEDescriptor *pDescriptor) {
-    pDescriptor->setCharacteristic(this);
+    bool foundRemoved = false;
+
+    if(pDescriptor->m_removed > 0) {
+        for(auto& it : m_dscVec) {
+            if(it == pDescriptor) {
+                foundRemoved = true;
+                pDescriptor->m_removed = 0;
+            }
+        }
+    }
+
+    if(!foundRemoved) {
         m_dscVec.push_back(pDescriptor);
     }
 
+    pDescriptor->setCharacteristic(this);
+    NimBLEDevice::getServer()->serviceChanged();
+}
+
+
+/**
+ * @brief Remove a descriptor from the characterisitc.
+ * @param[in] pDescriptor A pointer to the descriptor instance to remove from the characterisitc.
+ * @param[in] deleteDsc If true it will delete the descriptor instance and free it's resources.
+ */
+void NimBLECharacteristic::removeDescriptor(NimBLEDescriptor *pDescriptor, bool deleteDsc) {
+    // Check if the descriptor was already removed and if so, check if this
+    // is being called to delete the object and do so if requested.
+    // Otherwise, ignore the call and return.
+    if(pDescriptor->m_removed > 0) {
+        if(deleteDsc) {
+            for(auto it = m_dscVec.begin(); it != m_dscVec.end(); ++it) {
+                if ((*it) == pDescriptor) {
+                    delete *it;
+                    m_dscVec.erase(it);
+                    break;
+                }
+            }
+        }
+
+        return;
+    }
+
+    pDescriptor->m_removed = deleteDsc ? NIMBLE_ATT_REMOVE_DELETE : NIMBLE_ATT_REMOVE_HIDE;
+    NimBLEDevice::getServer()->serviceChanged();
+} // removeDescriptor
+
 
 /**
  * @brief Return the BLE Descriptor for the given UUID.

src/NimBLECharacteristic.h

@@ -99,6 +99,7 @@ public:
     NimBLEDescriptor* getDescriptorByUUID(const char* uuid);
     NimBLEDescriptor* getDescriptorByUUID(const NimBLEUUID &uuid);
     NimBLEDescriptor* getDescriptorByHandle(uint16_t handle);
+    void              removeDescriptor(NimBLEDescriptor *pDescriptor, bool deleteDsc = false);
 
     std::string       getValue(time_t *timestamp = nullptr);
     size_t            getDataLength();
@@ -152,6 +153,7 @@ private:
     std::vector<NimBLEDescriptor*> m_dscVec;
     portMUX_TYPE                   m_valMux;
     time_t                         m_timestamp;
+    uint8_t                        m_removed;
 
     std::vector<std::pair<uint16_t, uint16_t>>  m_subscribedVec;
 }; // NimBLECharacteristic

src/NimBLEDescriptor.cpp

@@ -53,6 +53,7 @@ NimBLEDescriptor::NimBLEDescriptor(NimBLEUUID uuid, uint16_t properties, uint16_
     m_value.attr_value   = (uint8_t*) calloc(max_len,1);  // Allocate storage for the value.
     m_valMux             = portMUX_INITIALIZER_UNLOCKED;
     m_properties         = 0;
+    m_removed            = 0;
 
     if (properties & BLE_GATT_CHR_F_READ) {             // convert uint16_t properties to uint8_t
         m_properties |= BLE_ATT_F_READ;

src/NimBLEDescriptor.h

@@ -93,6 +93,7 @@ private:
     uint8_t                    m_properties;
     attr_value_t               m_value;
     portMUX_TYPE               m_valMux;
+    uint8_t                    m_removed;
 }; // NimBLEDescriptor
 
 

src/NimBLEServer.cpp

@@ -91,12 +91,7 @@ NimBLEService* NimBLEServer::createService(const NimBLEUUID &uuid, uint32_t numH
 
     NimBLEService* pService = new NimBLEService(uuid, numHandles, this);
     m_svcVec.push_back(pService); // Save a reference to this service being on this server.
-
+    serviceChanged();
-    if(m_gattsStarted) {
-        ble_svc_gatt_changed(0x0001, 0xffff);
-        m_svcChanged = true;
-        resetGATT();
-    }
 
     NIMBLE_LOGD(LOG_TAG, "<< createService");
     return pService;
@@ -157,6 +152,18 @@ NimBLEAdvertising* NimBLEServer::getAdvertising() {
 } // getAdvertising
 
 
+/**
+ * @brief Sends a service changed notification and resets the GATT server.
+ */
+void NimBLEServer::serviceChanged() {
+    if(m_gattsStarted) {
+        m_svcChanged = true;
+        ble_svc_gatt_changed(0x0001, 0xffff);
+        resetGATT();
+    }
+}
+
+
 /**
  * @brief Start the GATT server. Required to be called after setup of all
  * services and characteristics / descriptors for the NimBLE host to register them.
@@ -630,7 +637,7 @@ void NimBLEServer::removeService(NimBLEService* service, bool deleteSvc) {
     if(service->m_removed > 0) {
         if(deleteSvc) {
             for(auto it = m_svcVec.begin(); it != m_svcVec.end(); ++it) {
-                if ((*it)->getUUID() == service->getUUID()) {
+                if ((*it) == service) {
                     delete *it;
                     m_svcVec.erase(it);
                     break;
@@ -646,11 +653,8 @@ void NimBLEServer::removeService(NimBLEService* service, bool deleteSvc) {
         return;
     }
 
-    service->m_removed = deleteSvc ? 2 : 1;
+    service->m_removed = deleteSvc ? NIMBLE_ATT_REMOVE_DELETE : NIMBLE_ATT_REMOVE_HIDE;
-    m_svcChanged = true;
+    serviceChanged();
-
-    ble_svc_gatt_changed(0x0001, 0xffff);
-    resetGATT();
     NimBLEDevice::getAdvertising()->removeServiceUUID(service->getUUID());
 }
 
@@ -677,10 +681,7 @@ void NimBLEServer::addService(NimBLEService* service) {
     }
 
     service->m_removed = 0;
-    m_svcChanged = true;
+    serviceChanged();
-
-    ble_svc_gatt_changed(0x0001, 0xffff);
-    resetGATT();
 }
 
 
@@ -699,7 +700,7 @@ void NimBLEServer::resetGATT() {
 
     for(auto it = m_svcVec.begin(); it != m_svcVec.end(); ) {
         if ((*it)->m_removed > 0) {
-            if ((*it)->m_removed == 2) {
+            if ((*it)->m_removed == NIMBLE_ATT_REMOVE_DELETE) {
                 delete *it;
                 it = m_svcVec.erase(it);
             } else {

src/NimBLEServer.h

@@ -20,6 +20,9 @@
 #include "nimconfig.h"
 #if defined(CONFIG_BT_NIMBLE_ROLE_PERIPHERAL)
 
+#define NIMBLE_ATT_REMOVE_HIDE 1
+#define NIMBLE_ATT_REMOVE_DELETE 2
+
 #include "NimBLEUtils.h"
 #include "NimBLEAddress.h"
 #include "NimBLEAdvertising.h"
@@ -69,6 +72,7 @@ private:
     NimBLEServer();
     ~NimBLEServer();
     friend class           NimBLECharacteristic;
+    friend class           NimBLEService;
     friend class           NimBLEDevice;
     friend class           NimBLEAdvertising;
 
@@ -86,6 +90,7 @@ private:
     std::vector<NimBLECharacteristic*> m_notifyChrVec;
 
     static int             handleGapEvent(struct ble_gap_event *event, void *arg);
+    void                   serviceChanged();
     void                   resetGATT();
     bool                   setIndicateWait(uint16_t conn_handle);
     void                   clearIndicateWait(uint16_t conn_handle);

src/NimBLEService.cpp

@@ -35,7 +35,7 @@ static const char* LOG_TAG = "NimBLEService"; // Tag for logging.
  * @brief Construct an instance of the NimBLEService
  * @param [in] uuid The UUID of the service.
  * @param [in] numHandles The maximum number of handles associated with the service.
- * @param [in] a pointer to the server instance that this service belongs to.
+ * @param [in] pServer A pointer to the server instance that this service belongs to.
  */
 NimBLEService::NimBLEService(const char* uuid, uint16_t numHandles, NimBLEServer* pServer)
 : NimBLEService(NimBLEUUID(uuid), numHandles, pServer) {
@@ -46,7 +46,7 @@ NimBLEService::NimBLEService(const char* uuid, uint16_t numHandles, NimBLEServer
  * @brief Construct an instance of the BLEService
  * @param [in] uuid The UUID of the service.
  * @param [in] numHandles The maximum number of handles associated with the service.
- * @param [in] a pointer to the server instance that this service belongs to.
+ * @param [in] pServer A pointer to the server instance that this service belongs to.
  */
 NimBLEService::NimBLEService(const NimBLEUUID &uuid, uint16_t numHandles, NimBLEServer* pServer) {
     m_uuid         = uuid;
@@ -118,7 +118,12 @@ NimBLEUUID NimBLEService::getUUID() {
  */
 bool NimBLEService::start() {
     NIMBLE_LOGD(LOG_TAG, ">> start(): Starting service: %s", toString().c_str());
-    int rc = 0;
+
+    // Rebuild the service definition if the server attributes have changed.
+    if(getServer()->m_svcChanged && m_pSvcDef != nullptr) {
+        delete(m_pSvcDef);
+        m_pSvcDef = nullptr;
+    }
 
     if(m_pSvcDef == nullptr) {
         // Nimble requires an array of services to be sent to the api
@@ -132,8 +137,23 @@ bool NimBLEService::start() {
         svc[0].uuid = &m_uuid.getNative()->u;
         svc[0].includes = NULL;
 
-        size_t numChrs = m_chrVec.size();
+        int removedCount = 0;
+        for(auto it = m_chrVec.begin(); it != m_chrVec.end(); ) {
+            if ((*it)->m_removed > 0) {
+                if ((*it)->m_removed == NIMBLE_ATT_REMOVE_DELETE) {
+                    delete *it;
+                    it = m_chrVec.erase(it);
+                } else {
+                    ++removedCount;
+                    ++it;
+                }
+                continue;
+            }
 
+            ++it;
+        }
+
+        size_t numChrs = m_chrVec.size() - removedCount;
         NIMBLE_LOGD(LOG_TAG,"Adding %d characteristics for service %s", numChrs, toString().c_str());
 
         if(!numChrs){
@@ -143,39 +163,59 @@ bool NimBLEService::start() {
             // of the characteristics for the service. We create 1 extra and set it to null
             // for this purpose.
             pChr_a = new ble_gatt_chr_def[numChrs + 1];
-            NimBLECharacteristic* pCharacteristic = *m_chrVec.begin();
+            uint8_t i = 0;
+            for(auto chr_it = m_chrVec.begin(); chr_it != m_chrVec.end(); ++chr_it) {
+                if((*chr_it)->m_removed > 0) {
+                    continue;
+                }
 
-            for(uint8_t i=0; i < numChrs;) {
+                removedCount = 0;
-                uint8_t numDscs = pCharacteristic->m_dscVec.size();
+                for(auto it = (*chr_it)->m_dscVec.begin(); it != (*chr_it)->m_dscVec.end(); ) {
+                    if ((*it)->m_removed > 0) {
+                        if ((*it)->m_removed == NIMBLE_ATT_REMOVE_DELETE) {
+                            delete *it;
+                            it = (*chr_it)->m_dscVec.erase(it);
+                        } else {
+                            ++removedCount;
+                            ++it;
+                        }
+                        continue;
+                    }
+
+                    ++it;
+                }
+
+                size_t numDscs = (*chr_it)->m_dscVec.size() - removedCount;
 
                 if(!numDscs){
                     pChr_a[i].descriptors = NULL;
                 } else {
                     // Must have last descriptor uuid = 0 so we have to create 1 extra
                     pDsc_a = new ble_gatt_dsc_def[numDscs+1];
-                    NimBLEDescriptor* pDescriptor = *pCharacteristic->m_dscVec.begin();
+                    uint8_t d = 0;
-                    for(uint8_t d=0; d < numDscs;) {
+                    for(auto dsc_it = (*chr_it)->m_dscVec.begin(); dsc_it != (*chr_it)->m_dscVec.end(); ++dsc_it ) {
-                        pDsc_a[d].uuid = &pDescriptor->m_uuid.getNative()->u;
+                        if((*dsc_it)->m_removed > 0) {
-                        pDsc_a[d].att_flags = pDescriptor->m_properties;
+                            continue;
+                        }
+                        pDsc_a[d].uuid = &(*dsc_it)->m_uuid.getNative()->u;
+                        pDsc_a[d].att_flags = (*dsc_it)->m_properties;
                         pDsc_a[d].min_key_size = 0;
                         pDsc_a[d].access_cb = NimBLEDescriptor::handleGapEvent;
-                        pDsc_a[d].arg = pDescriptor;
+                        pDsc_a[d].arg = (*dsc_it);
-                        d++;
+                        ++d;
-                        pDescriptor = *(pCharacteristic->m_dscVec.begin() + d);
                     }
 
                     pDsc_a[numDscs].uuid = NULL;
                     pChr_a[i].descriptors = pDsc_a;
                 }
 
-                pChr_a[i].uuid = &pCharacteristic->m_uuid.getNative()->u;
+                pChr_a[i].uuid = &(*chr_it)->m_uuid.getNative()->u;
                 pChr_a[i].access_cb = NimBLECharacteristic::handleGapEvent;
-                pChr_a[i].arg = pCharacteristic;
+                pChr_a[i].arg = (*chr_it);
-                pChr_a[i].flags = pCharacteristic->m_properties;
+                pChr_a[i].flags = (*chr_it)->m_properties;
                 pChr_a[i].min_key_size = 0;
-                pChr_a[i].val_handle = &pCharacteristic->m_handle;
+                pChr_a[i].val_handle = &(*chr_it)->m_handle;
-                i++;
+                ++i;
-                pCharacteristic = *(m_chrVec.begin() + i);
             }
 
             pChr_a[numChrs].uuid = NULL;
@@ -187,7 +227,7 @@ bool NimBLEService::start() {
         m_pSvcDef = svc;
     }
 
-    rc = ble_gatts_count_cfg((const ble_gatt_svc_def*)m_pSvcDef);
+    int rc = ble_gatts_count_cfg((const ble_gatt_svc_def*)m_pSvcDef);
     if (rc != 0) {
         NIMBLE_LOGE(LOG_TAG, "ble_gatts_count_cfg failed, rc= %d, %s", rc, NimBLEUtils::returnCodeToString(rc));
         return false;
@@ -239,18 +279,64 @@ NimBLECharacteristic* NimBLEService::createCharacteristic(const NimBLEUUID &uuid
                              std::string(uuid).c_str());
     }
 
-    // Remember this characteristic in our vector of characteristics.
+    addCharacteristic(pCharacteristic);
-    m_chrVec.push_back(pCharacteristic);
-
     return pCharacteristic;
 } // createCharacteristic
 
 
+/**
+ * @brief Add a characteristic to the service.
+ * @param[in] pCharacteristic A pointer to the characteristic instance to add to the service.
+ */
 void NimBLEService::addCharacteristic(NimBLECharacteristic* pCharacteristic) {
-    pCharacteristic->setService(this);
+    bool foundRemoved = false;
+
+    if(pCharacteristic->m_removed > 0) {
+        for(auto& it : m_chrVec) {
+            if(it == pCharacteristic) {
+                foundRemoved = true;
+                pCharacteristic->m_removed = 0;
+            }
+        }
+    }
+
+    if(!foundRemoved) {
         m_chrVec.push_back(pCharacteristic);
     }
 
+    pCharacteristic->setService(this);
+    getServer()->serviceChanged();
+} // addCharacteristic
+
+
+/**
+ * @brief Remove a characteristic from the service.
+ * @param[in] pCharacteristic A pointer to the characteristic instance to remove from the service.
+ * @param[in] deleteChr If true it will delete the characteristic instance and free it's resources.
+ */
+void NimBLEService::removeCharacteristic(NimBLECharacteristic* pCharacteristic, bool deleteChr) {
+    // Check if the characteristic was already removed and if so, check if this
+    // is being called to delete the object and do so if requested.
+    // Otherwise, ignore the call and return.
+    if(pCharacteristic->m_removed > 0) {
+        if(deleteChr) {
+            for(auto it = m_chrVec.begin(); it != m_chrVec.end(); ++it) {
+                if ((*it) == pCharacteristic) {
+                    m_chrVec.erase(it);
+                    delete *it;
+                    break;
+                }
+            }
+        }
+
+        return;
+    }
+
+    pCharacteristic->m_removed = deleteChr ? NIMBLE_ATT_REMOVE_DELETE : NIMBLE_ATT_REMOVE_HIDE;
+    getServer()->serviceChanged();
+} // removeCharacteristic
+
+
 /**
  * @brief Get a pointer to the characteristic object with the specified UUID.
  * @param [in] uuid The UUID of the characteristic.

src/NimBLEService.h

@@ -60,6 +60,7 @@ public:
                                                NIMBLE_PROPERTY::WRITE);
 
     void                  addCharacteristic(NimBLECharacteristic* pCharacteristic);
+    void                  removeCharacteristic(NimBLECharacteristic* pCharacteristic, bool deleteChr = false);
     NimBLECharacteristic* getCharacteristic(const char* uuid, uint16_t instanceId = 0);
     NimBLECharacteristic* getCharacteristic(const NimBLEUUID &uuid, uint16_t instanceId = 0);
     NimBLECharacteristic* getCharacteristicByHandle(uint16_t handle);