Add feature: remove service (#17)

* Get service handles on server start

* remove service + indicate service changed

* Reset gatt services when no connections active and services changed.

* NimBLEServer::createService can now be used any time and will send service changed
indication if server was already active.

* Add ability to remove advertised serviceUUIDS

* Adds addService() method to server to be allow user to re-add a service previously removed

* Add destructior to NimBLEServer, NimBLEService and NimBLECharacteristic to release allocated resources.

src/NimBLEServer.cpp

@@ -22,6 +22,10 @@
 #include "NimBLEDevice.h"
 #include "NimBLELog.h"
 
+#include "services/gap/ble_svc_gap.h"
+#include "services/gatt/ble_svc_gatt.h"
+
+
 static const char* LOG_TAG = "NimBLEServer";
 static NimBLEServerCallbacks defaultCallbacks;
 
@@ -37,9 +41,20 @@ NimBLEServer::NimBLEServer() {
     m_pServerCallbacks      = &defaultCallbacks;
     m_gattsStarted          = false;
     m_advertiseOnDisconnect = true;
+    m_svcChanged            = false;
 } // NimBLEServer
 
 
+/**
+ * @brief Destructor: frees all resources / attributes created.
+ */
+NimBLEServer::~NimBLEServer() {
+    for(auto &it : m_svcVec) {
+        delete it;
+    }
+}
+
+
 /**
  * @brief Create a %BLE Service.
  * @param [in] uuid The UUID of the new service.
@@ -71,6 +86,12 @@ 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.
 
+    if(m_gattsStarted) {
+        ble_svc_gatt_changed(0x0001, 0xffff);
+        m_svcChanged = true;
+        resetGATT();
+    }
+
     NIMBLE_LOGD(LOG_TAG, "<< createService");
     return pService;
 } // createService
@@ -146,8 +167,16 @@ void NimBLEServer::start() {
 
     NIMBLE_LOGI(LOG_TAG, "Service changed characterisic handle: %d", m_svcChgChrHdl);
 */
-    // Build a vector of characteristics with Notify / Indicate capabilities for event handling
+    // Get the assigned service handles and build a vector of characteristics
+    // with Notify / Indicate capabilities for event handling
     for(auto &svc : m_svcVec) {
+        if(svc->m_removed == 0) {
+            rc = ble_gatts_find_svc(&svc->getUUID().getNative()->u, &svc->m_handle);
+            if(rc != 0) {
+                abort();
+            }
+        }
+
         for(auto &chr : svc->m_chrVec) {
             // if Notify / Indicate is enabled but we didn't create the descriptor
             // we do it now.
@@ -260,6 +289,11 @@ size_t NimBLEServer::getConnectedCount() {
                                                           server->m_connectedPeersVec.end(),
                                                           event->disconnect.conn.conn_handle),
                                                           server->m_connectedPeersVec.end());
+
+            if(server->m_svcChanged) {
+                server->resetGATT();
+            }
+
             server->m_pServerCallbacks->onDisconnect(server);
 
             if(server->m_advertiseOnDisconnect) {
@@ -445,6 +479,111 @@ void NimBLEServer::setCallbacks(NimBLEServerCallbacks* pCallbacks) {
 } // setCallbacks
 
 
+/**
+ * @brief Remove a service from the server.
+ *
+ * @details Immediately removes access to the service by clients, sends a service changed indication,
+ * and removes the service (if applicable) from the advertisments.
+ * The service is not deleted unless the deleteSvc parameter is true, otherwise the service remains
+ * available and can be re-added in the future. If desired a removed but not deleted service can
+ * be deleted later by calling this method with deleteSvc set to true.
+ *
+ * @note The service will not be removed from the database until all open connections are closed
+ * as it requires resetting the GATT server. In the interim the service will have it's visibility disabled.
+ *
+ * @note Advertising will need to be restarted by the user after calling this as we must stop
+ * advertising in order to remove the service.
+ *
+ * @param [in] service The service object to remove.
+ * @param [in] deleteSvc true if the service should be deleted.
+ */
+void NimBLEServer::removeService(NimBLEService* service, bool deleteSvc) {
+    // Check if the service 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(service->m_removed > 0) {
+        if(deleteSvc) {
+            for(auto it = m_svcVec.begin(); it != m_svcVec.end(); ++it) {
+                if ((*it)->getUUID() == service->getUUID()) {
+                    delete *it;
+                    m_svcVec.erase(it);
+                    break;
+                }
+            }
+        }
+
+        return;
+    }
+
+    int rc = ble_gatts_svc_set_visibility(service->getHandle(), 0);
+    if(rc !=0) {
+        return;
+    }
+
+    service->m_removed = deleteSvc ? 2 : 1;
+    m_svcChanged = true;
+
+    ble_svc_gatt_changed(0x0001, 0xffff);
+    resetGATT();
+    NimBLEDevice::getAdvertising()->removeServiceUUID(service->getUUID());
+}
+
+
+/**
+ * @brief Adds a service which was already created, but removed from availability.
+ *
+ * @note If it is desired to advertise the service it must be added by
+ * calling NimBLEAdvertising::addServiceUUID.
+ *
+ * @param [in} service The service object to add.
+ */
+void NimBLEServer::addService(NimBLEService* service) {
+    // If adding a service that was not removed just return.
+    if(service->m_removed == 0) {
+        return;
+    }
+
+    service->m_removed = 0;
+    m_svcChanged = true;
+
+    ble_svc_gatt_changed(0x0001, 0xffff);
+    resetGATT();
+}
+
+
+/**
+ * @brief Resets the GATT server, used when services are added/removed after initialization.
+ */
+void NimBLEServer::resetGATT() {
+    if(getConnectedCount() > 0) {
+        return;
+    }
+
+    NimBLEDevice::stopAdvertising();
+    ble_gatts_reset();
+    ble_svc_gap_init();
+    ble_svc_gatt_init();
+
+    for(auto it = m_svcVec.begin(); it != m_svcVec.end(); ) {
+        if ((*it)->m_removed > 0) {
+            if ((*it)->m_removed == 2) {
+                delete *it;
+                it = m_svcVec.erase(it);
+            } else {
+                ++it;
+            }
+            continue;
+        }
+
+        (*it)->start();
+        ++it;
+    }
+
+    m_svcChanged = false;
+    m_gattsStarted = false;
+}
+
+
 /**
  * @brief Start advertising.
  *