diff --git a/libesp32/NimBLE-Arduino/API_DIFFERENCES.md b/libesp32/NimBLE-Arduino/API_DIFFERENCES.md
index 32bf4a2f0..91b0fd78f 100644
--- a/libesp32/NimBLE-Arduino/API_DIFFERENCES.md
+++ b/libesp32/NimBLE-Arduino/API_DIFFERENCES.md
@@ -122,21 +122,20 @@ Has been removed from the API as it is no longer maintained in the library.
 
 The last two above changes reduce the heap usage significantly with minimal application code adjustments.   
 
-**NEW** on May 23, 2020
+**UPDATED** on June 21, 2020
 > ```
 > NimBLEClient::getServices(bool refresh = false)   
 > NimBLERemoteService::getCharacteristics(bool refresh = false)   
 > NimBLERemoteCharacteristic::getDecriptors(bool refresh = false)
 >```
-> These methods now take an optional (bool) parameter.   
+These methods now take an optional (bool) parameter.   
 If true it will clear the respective vector and retrieve all the respective attributes from the peripheral.   
-If false it will retrieve the attributes only if the vector is empty, otherwise the vector is returned   
-with the currently stored attributes.   
+If false(default) it will return the respective vector empty or otherwise with the currently stored attributes.   
 
-> Removed the automatic discovery of all peripheral attributes as they consumed time and resources for data   
+**Removed:** the automatic discovery of all peripheral attributes as they consumed time and resources for data   
 the user may not be interested in.   
    
-> Added `NimBLEClient::discoverAtrributes()` for the user to discover all the peripheral attributes   
+**Added:** `NimBLEClient::discoverAtrributes()` for the user to discover all the peripheral attributes   
 to replace the the former functionality.
    
    
@@ -145,12 +144,11 @@ to replace the the former functionality.
 >getCharacteristic(NimBLEUUID)   
 >getDescriptor(NimBLEUUID)
 >```
->These methods will now check the respective vectors for the attribute object and, if not found, will retrieve (only)   
+These methods will now check the respective vectors for the attribute object and, if not found, will retrieve (only)   
 the specified attribute from the peripheral.
 
-> These changes allow more control for the user to manage the resources used for the attributes.   
-
-
+These changes allow more control for the user to manage the resources used for the attributes.   
+***
 #### Client Security:
 The client will automatically initiate security when the peripheral responds that it's required.    
 The default configuration will use "just-works" pairing with no bonding, if you wish to enable bonding see below.
diff --git a/libesp32/NimBLE-Arduino/README.md b/libesp32/NimBLE-Arduino/README.md
index 514416e29..04c3a80c5 100644
--- a/libesp32/NimBLE-Arduino/README.md
+++ b/libesp32/NimBLE-Arduino/README.md
@@ -1,5 +1,17 @@
-# *** UPDATE ***
-**Breaking change:** Client and scan now use `std::vector` instead of `std::map` for storing the remote attribute database.   
+# *** UPDATES ***
+**Breaking changes:**   
+**NEW** on June 21, 2020
+> ```
+> NimBLEClient::getServices(bool refresh = false)   
+> NimBLERemoteService::getCharacteristics(bool refresh = false)   
+> NimBLERemoteCharacteristic::getDecriptors(bool refresh = false)
+>```
+These methods now take an optional (bool) parameter.   
+If true it will clear the respective vector and retrieve all the respective attributes from the peripheral.   
+If false(default) it will return the respective vector empty or otherwise with the currently stored attributes. 
+
+**NEW** on May 23, 2020   
+Client and scan now use `std::vector` instead of `std::map` for storing the remote attribute database.   
    
 This change will affect your application code if you use `NimBLEClient::getServices()` or `NimBLERemoteService::getCharacteristics()`   
 in your application as they now return a pointer to `std::vector` of the respective attributes.   
@@ -13,30 +25,33 @@ It is expected that there will be minimal impact on most applications, if you ne
 # NimBLE-Arduino
 A fork of the NimBLE stack restructured for compilation in the Ardruino IDE with a CPP library for use with ESP32.
 
-Why? Because the Bluedroid library is too bulky. 
+This library **significantly** reduces resource usage and improves performance for ESP32 BLE applications as compared    
+with the bluedroid based library. The goal is to maintain, as much as reasonable, compatibility with the original   
+library but refactored to use the NimBLE stack. In addition, this library will be more actively developed and maintained   
+to provide improved capabilites and stability over the original.
 
-Initial client code testing has resulted in code size reduction of ~115k and reduced ram consumption of ~37k.
+## Resource use improvement:
 
-Server code testing results from @beegee-toyo [from the project here](https://github.com/beegee-tokyo/ESP32WiFiBLE-NimBLE):
+### (Original) BLE_client example comparison (Debug):
+#### Arduino BLE Library   
+Sketch uses **1216377** bytes (58%) of program storage space.   
+Memory after connection: Free Heap: **171548**  
 
-
-### Memory usage (compilation output)
-#### Arduino BLE library
-```log
-RAM:   [==        ]  17.7% (used 58156 bytes from 327680 bytes)    
-Flash: [========  ]  76.0% (used 1345630 bytes from 1769472 bytes)    
-```
 #### NimBLE-Arduino library
-```log
-RAM:   [=         ]  14.5% (used 47476 bytes from 327680 bytes)    
-Flash: [=======   ]  69.5% (used 911378 bytes from 1310720 bytes)    
-```
-### Memory usage after **`setup()`** function
-#### Arduino BLE library
-**`Internal Total heap 259104, internal Free Heap 91660`**    
-#### NimBLE-Arduino library
-**`Internal Total heap 290288, internal Free Heap 182344`** 
-  
+Sketch uses **617256** bytes (29%) of program storage space.   
+Memory after connection: Free Heap: **270336**   
+***
+### (Original) BLE_notify example comparison (Debug):   
+#### Arduino BLE Library
+Sketch uses **1208409** bytes (57%) of program storage space.   
+Memory after connection: Free Heap: **173300**   
+
+#### NimBLE-Arduino library   
+Sketch uses **603432** bytes (28%) of program storage space.   
+Memory after connection: Free Heap: **269792**  
+
+**As shown: there is nearly a 50% reduction in flash use and approx. 100kB less ram consumed!**
+ 
   
 # Installation:
 
@@ -62,20 +77,21 @@ Change the settings in the `nimconfig.h` file to customize NimBLE to your projec
 
 # Continuing development:
 
-This Library is tracking the esp-nimble repo, nimble-1.2.0-idf master branch, currently [@fead24e.](https://github.com/espressif/esp-nimble)
+This Library is tracking the esp-nimble repo, nimble-1.2.0-idf master branch, currently [@46c1d9f.](https://github.com/espressif/esp-nimble)
 
-Also tracking the NimBLE related changes in esp-idf, master branch, currently [@2bc28bb.](https://github.com/espressif/esp-idf/tree/master/components/bt/host/nimble)
+Also tracking the NimBLE related changes in esp-idf, master branch, currently [@2ef4890.](https://github.com/espressif/esp-idf/tree/master/components/bt/host/nimble)
 
 # Acknowledgments:
 
 * @nkolban and @chegewara for the [original esp32 BLE library](https://github.com/nkolban/esp32-snippets) this project was derived from.
 * @beegee-tokyo for contributing your time to test/debug and contributing the beacon examples.
+* @Jeroen88 for the amazing help debugging and improving the client code.
 
 
 # Todo:
 
-1. Code cleanup.
-2. Create documentation.
+1. Create documentation.
+2. Add BLE Mesh code.
 3. Expose more NimBLE features.
-4. Add BLE Mesh code.
+
 
diff --git a/libesp32/NimBLE-Arduino/src/NimBLE2902.h b/libesp32/NimBLE-Arduino/src/NimBLE2902.h
index f7fc55569..2d84b73dc 100644
--- a/libesp32/NimBLE-Arduino/src/NimBLE2902.h
+++ b/libesp32/NimBLE-Arduino/src/NimBLE2902.h
@@ -22,11 +22,16 @@
 
 #include "NimBLEDescriptor.h"
 
-#include <map>
+#include <vector>
 
 #define NIMBLE_DESC_FLAG_NOTIFY   0x0001
 #define NIMBLE_DESC_FLAG_INDICATE 0x0002
 
+typedef struct {
+    uint16_t conn_id;
+    uint16_t sub_val;
+} chr_sub_status_t;
+
 
 /**
  * @brief Descriptor for Client Characteristic Configuration.
@@ -45,7 +50,7 @@ public:
 private:
     NimBLE2902(NimBLECharacteristic* pCharacterisitic);
     friend class NimBLECharacteristic;
-    std::map<uint16_t, uint16_t> m_subscribedMap;
+    std::vector<chr_sub_status_t> m_subscribedVec;
 
 }; // NimBLE2902
 
diff --git a/libesp32/NimBLE-Arduino/src/NimBLE2904.h b/libesp32/NimBLE-Arduino/src/NimBLE2904.h
index dd665a2bb..0a6d036a4 100644
--- a/libesp32/NimBLE-Arduino/src/NimBLE2904.h
+++ b/libesp32/NimBLE-Arduino/src/NimBLE2904.h
@@ -31,6 +31,7 @@ struct BLE2904_Data {
 
 } __attribute__((packed));
 
+
 /**
  * @brief Descriptor for Characteristic Presentation Format.
  *
diff --git a/libesp32/NimBLE-Arduino/src/NimBLEAddress.cpp b/libesp32/NimBLE-Arduino/src/NimBLEAddress.cpp
index b6bc9526b..8c2c68f79 100644
--- a/libesp32/NimBLE-Arduino/src/NimBLEAddress.cpp
+++ b/libesp32/NimBLE-Arduino/src/NimBLEAddress.cpp
@@ -142,4 +142,10 @@ NimBLEAddress::operator std::string() const {
     return std::string(buffer);
 }
 
+NimBLEAddress::operator uint64_t() const {
+    uint64_t address = 0;
+    memcpy(&address, m_address, sizeof m_address);
+    return address;
+}
+
 #endif
diff --git a/libesp32/NimBLE-Arduino/src/NimBLEAddress.h b/libesp32/NimBLE-Arduino/src/NimBLEAddress.h
index e81a2bdf3..778ff8644 100644
--- a/libesp32/NimBLE-Arduino/src/NimBLEAddress.h
+++ b/libesp32/NimBLE-Arduino/src/NimBLEAddress.h
@@ -44,6 +44,7 @@ public:
     bool operator   ==(const NimBLEAddress & rhs) const;
     bool operator   !=(const NimBLEAddress & rhs) const;
     operator        std::string() const;
+    operator        uint64_t() const;
 
 private:
     uint8_t        m_address[6];
diff --git a/libesp32/NimBLE-Arduino/src/NimBLEAdvertisedDevice.cpp b/libesp32/NimBLE-Arduino/src/NimBLEAdvertisedDevice.cpp
index 3634bac8a..c53bb688b 100644
--- a/libesp32/NimBLE-Arduino/src/NimBLEAdvertisedDevice.cpp
+++ b/libesp32/NimBLE-Arduino/src/NimBLEAdvertisedDevice.cpp
@@ -37,6 +37,8 @@ NimBLEAdvertisedDevice::NimBLEAdvertisedDevice() {
     m_serviceData      = "";
     m_txPower          = 0;
     m_pScan            = nullptr;
+    m_payloadLength    = 0;
+    m_payload          = nullptr;
 
     m_haveAppearance       = false;
     m_haveManufacturerData = false;
@@ -45,6 +47,7 @@ NimBLEAdvertisedDevice::NimBLEAdvertisedDevice() {
     m_haveServiceData      = false;
     m_haveServiceUUID      = false;
     m_haveTXPower          = false;
+    m_callbackSent         = false;
 
 } // NimBLEAdvertisedDevice
 
@@ -62,6 +65,16 @@ NimBLEAddress NimBLEAdvertisedDevice::getAddress() {
 } // getAddress
 
 
+/**
+ * @brief Get the advertised type.
+ *
+ * @return The advertised type of the advertised device.
+ */
+uint8_t NimBLEAdvertisedDevice::getAdvType() {
+    return m_advType;
+} // getAddress
+
+
 /**
  * @brief Get the appearance.
  *
@@ -536,6 +549,11 @@ uint8_t NimBLEAdvertisedDevice::getAddressType() {
 }
 
 
+time_t NimBLEAdvertisedDevice::getTimestamp() {
+    return m_timestamp;
+}
+
+
 void NimBLEAdvertisedDevice::setAddressType(uint8_t type) {
     m_addressType = type;
 }
diff --git a/libesp32/NimBLE-Arduino/src/NimBLEAdvertisedDevice.h b/libesp32/NimBLE-Arduino/src/NimBLEAdvertisedDevice.h
index 7a4115f28..c38d7001d 100644
--- a/libesp32/NimBLE-Arduino/src/NimBLEAdvertisedDevice.h
+++ b/libesp32/NimBLE-Arduino/src/NimBLEAdvertisedDevice.h
@@ -42,19 +42,39 @@ public:
     NimBLEAdvertisedDevice();
 
     NimBLEAddress   getAddress();
+    uint8_t         getAdvType();
     uint16_t        getAppearance();
     std::string     getManufacturerData();
+
+    template<typename T>
+    T               getManufacturerData(bool skipSizeCheck = false) {
+        std::string data = getManufacturerData();
+        if(!skipSizeCheck && data.size() < sizeof(T)) return T();
+        const char *pData = data.data();
+        return *((T *)pData);
+    }
+
     std::string     getName();
     int             getRSSI();
     NimBLEScan*     getScan();
     std::string     getServiceData();
+
+    template<typename T>
+    T               getServiceData(bool skipSizeCheck = false) {
+        std::string data = getServiceData();
+        if(!skipSizeCheck && data.size() < sizeof(T)) return T();
+        const char *pData = data.data();
+        return *((T *)pData);
+    }
+
     NimBLEUUID      getServiceDataUUID();
     NimBLEUUID      getServiceUUID();
     int8_t          getTXPower();
     uint8_t*        getPayload();
     size_t          getPayloadLength();
     uint8_t         getAddressType();
-    void setAddressType(uint8_t type);
+    time_t          getTimestamp();
+    void            setAddressType(uint8_t type);
 
 
     bool        isAdvertisingService(const NimBLEUUID &uuid);
@@ -95,7 +115,7 @@ private:
     bool m_haveTXPower;
 
 
-    NimBLEAddress  m_address = NimBLEAddress("");
+    NimBLEAddress   m_address = NimBLEAddress("");
     uint8_t         m_advType;
     uint16_t        m_appearance;
     int             m_deviceType;
@@ -108,8 +128,10 @@ private:
     std::string     m_serviceData;
     NimBLEUUID      m_serviceDataUUID;
     uint8_t*        m_payload;
-    size_t          m_payloadLength = 0;
+    size_t          m_payloadLength;
     uint8_t         m_addressType;
+    time_t          m_timestamp;
+    bool            m_callbackSent;
 };
 
 /**
diff --git a/libesp32/NimBLE-Arduino/src/NimBLEAdvertising.cpp b/libesp32/NimBLE-Arduino/src/NimBLEAdvertising.cpp
index 7c930bfc6..7e1e07ae3 100644
--- a/libesp32/NimBLE-Arduino/src/NimBLEAdvertising.cpp
+++ b/libesp32/NimBLE-Arduino/src/NimBLEAdvertising.cpp
@@ -100,7 +100,8 @@ void NimBLEAdvertising::setMaxInterval(uint16_t maxinterval) {
     m_advParams.itvl_max = maxinterval;
 } // setMaxInterval
 
-// These are dummy functions for now for compatibility
+
+/* These are dummy functions for now for compatibility */
 void NimBLEAdvertising::setMinPreferred(uint16_t mininterval) {
     //m_advData.min_interval = mininterval;
 } //
@@ -108,7 +109,8 @@ void NimBLEAdvertising::setMinPreferred(uint16_t mininterval) {
 void NimBLEAdvertising::setMaxPreferred(uint16_t maxinterval) {
     //m_advData.max_interval = maxinterval;
 } //
-//////////////////////////////////////////////////////////
+/*******************************************************/
+
 
 void NimBLEAdvertising::setScanResponse(bool set) {
     m_scanResp = set;
@@ -204,19 +206,19 @@ void NimBLEAdvertising::start() {
     }
 #endif
 
-    int numServices = m_serviceUUIDs.size();
-    int rc = 0;
-    uint8_t addressType;
-    uint8_t payloadLen = 3; //start with 3 bytes for the flags data
-
     // If already advertising just return
     if(ble_gap_adv_active()) {
         return;
     }
 
-    if (!m_customAdvData && !m_advSvcsSet && numServices > 0) {
-        for (int i = 0; i < numServices; i++) {
-            if(m_serviceUUIDs[i].getNative()->u.type == BLE_UUID_TYPE_16) {
+    int rc = 0;
+
+    if (!m_customAdvData && !m_advDataSet) {
+        //start with 3 bytes for the flags data
+        uint8_t payloadLen = 3;
+
+        for(auto &it : m_serviceUUIDs) {
+            if(it.getNative()->u.type == BLE_UUID_TYPE_16) {
                 int add = (m_advData.num_uuids16 > 0) ? 2 : 4;
                 if((payloadLen + add) > 31){
                     m_advData.uuids16_is_complete = 0;
@@ -225,19 +227,19 @@ void NimBLEAdvertising::start() {
                 payloadLen += add;
 
                 if(nullptr == (m_advData.uuids16 = (ble_uuid16_t*)realloc(m_advData.uuids16,
-                                    (m_advData.num_uuids16 + 1) * sizeof(ble_uuid16_t))))
+                                                   (m_advData.num_uuids16 + 1) * sizeof(ble_uuid16_t))))
                 {
                     NIMBLE_LOGE(LOG_TAG, "Error, no mem");
                     abort();
                 }
                 memcpy(&m_advData.uuids16[m_advData.num_uuids16].value,
-                            &m_serviceUUIDs[i].getNative()->u16.value, sizeof(uint16_t));
+                       &it.getNative()->u16.value, 2);
 
                 m_advData.uuids16[m_advData.num_uuids16].u.type = BLE_UUID_TYPE_16;
                 m_advData.uuids16_is_complete = 1;
                 m_advData.num_uuids16++;
             }
-            if(m_serviceUUIDs[i].getNative()->u.type == BLE_UUID_TYPE_32) {
+            if(it.getNative()->u.type == BLE_UUID_TYPE_32) {
                 int add = (m_advData.num_uuids32 > 0) ? 4 : 6;
                 if((payloadLen + add) > 31){
                     m_advData.uuids32_is_complete = 0;
@@ -246,19 +248,19 @@ void NimBLEAdvertising::start() {
                 payloadLen += add;
 
                 if(nullptr == (m_advData.uuids32 = (ble_uuid32_t*)realloc(m_advData.uuids32,
-                                    (m_advData.num_uuids32 + 1) * sizeof(ble_uuid32_t))))
+                                                   (m_advData.num_uuids32 + 1) * sizeof(ble_uuid32_t))))
                 {
                     NIMBLE_LOGE(LOG_TAG, "Error, no mem");
                     abort();
                 }
                 memcpy(&m_advData.uuids32[m_advData.num_uuids32].value,
-                            &m_serviceUUIDs[i].getNative()->u32.value, sizeof(uint32_t));
+                       &it.getNative()->u32.value, 4);
 
                 m_advData.uuids32[m_advData.num_uuids32].u.type = BLE_UUID_TYPE_32;
                 m_advData.uuids32_is_complete = 1;
                 m_advData.num_uuids32++;
             }
-            if(m_serviceUUIDs[i].getNative()->u.type == BLE_UUID_TYPE_128){
+            if(it.getNative()->u.type == BLE_UUID_TYPE_128){
                 int add = (m_advData.num_uuids128 > 0) ? 16 : 18;
                 if((payloadLen + add) > 31){
                     m_advData.uuids128_is_complete = 0;
@@ -267,12 +269,13 @@ void NimBLEAdvertising::start() {
                 payloadLen += add;
 
                 if(nullptr == (m_advData.uuids128 = (ble_uuid128_t*)realloc(m_advData.uuids128,
-                                    (m_advData.num_uuids128 + 1) * sizeof(ble_uuid128_t)))) {
+                              (m_advData.num_uuids128 + 1) * sizeof(ble_uuid128_t))))
+                {
                     NIMBLE_LOGE(LOG_TAG, "Error, no mem");
                     abort();
                 }
                 memcpy(&m_advData.uuids128[m_advData.num_uuids128].value,
-                            &m_serviceUUIDs[i].getNative()->u128.value, 16);
+                       &it.getNative()->u128.value, 16);
 
                 m_advData.uuids128[m_advData.num_uuids128].u.type = BLE_UUID_TYPE_128;
                 m_advData.uuids128_is_complete = 1;
@@ -315,11 +318,11 @@ void NimBLEAdvertising::start() {
 
             rc = ble_gap_adv_rsp_set_fields(&m_scanData);
             if (rc != 0) {
-                NIMBLE_LOGE(LOG_TAG, "error setting scan response data; rc=%d, %s", rc, NimBLEUtils::returnCodeToString(rc));
+                NIMBLE_LOGC(LOG_TAG, "error setting scan response data; rc=%d, %s", rc, NimBLEUtils::returnCodeToString(rc));
                 abort();
             }
         // if not using scan response and there is room,
-        // throw the tx power data into the advertisment
+        // put the tx power data into the advertisment
         } else if (payloadLen < 29) {
             m_advData.tx_pwr_lvl_is_present = 1;
             m_advData.tx_pwr_lvl = NimBLEDevice::getPower();
@@ -327,7 +330,7 @@ void NimBLEAdvertising::start() {
 
         rc = ble_gap_adv_set_fields(&m_advData);
         if (rc != 0) {
-            NIMBLE_LOGE(LOG_TAG, "error setting advertisement data; rc=%d, %s", rc, NimBLEUtils::returnCodeToString(rc));
+            NIMBLE_LOGC(LOG_TAG, "error setting advertisement data; rc=%d, %s", rc, NimBLEUtils::returnCodeToString(rc));
             abort();
         }
 
@@ -349,22 +352,16 @@ void NimBLEAdvertising::start() {
             m_advData.num_uuids16 = 0;
         }
 
-        m_advSvcsSet = true;
-    }
-
-    rc = ble_hs_id_infer_auto(0, &addressType);
-    if (rc != 0) {
-        NIMBLE_LOGC(LOG_TAG, "Error determining address type; rc=%d, %s", rc, NimBLEUtils::returnCodeToString(rc));
-        abort();
+        m_advDataSet = true;
     }
 
 #if defined(CONFIG_BT_NIMBLE_ROLE_PERIPHERAL)
-    rc = ble_gap_adv_start(addressType, NULL, BLE_HS_FOREVER,
+    rc = ble_gap_adv_start(0, NULL, BLE_HS_FOREVER,
                            &m_advParams,
                            (pServer != nullptr) ? NimBLEServer::handleGapEvent : NULL,
                            pServer);
 #else
-    rc = ble_gap_adv_start(addressType, NULL, BLE_HS_FOREVER,
+    rc = ble_gap_adv_start(0, NULL, BLE_HS_FOREVER,
                            &m_advParams, NULL,NULL);
 #endif
     if (rc != 0) {
@@ -398,7 +395,7 @@ void NimBLEAdvertising::stop() {
  * we need clear the flag so it reloads it.
  */
 void NimBLEAdvertising::onHostReset() {
-    m_advSvcsSet = false;
+    m_advDataSet = false;
 }
 
 
diff --git a/libesp32/NimBLE-Arduino/src/NimBLEAdvertising.h b/libesp32/NimBLE-Arduino/src/NimBLEAdvertising.h
index f30b1581c..ad76d7eec 100644
--- a/libesp32/NimBLE-Arduino/src/NimBLEAdvertising.h
+++ b/libesp32/NimBLE-Arduino/src/NimBLEAdvertising.h
@@ -27,7 +27,6 @@
 /**************************/
 
 #include "NimBLEUUID.h"
-#include "FreeRTOS.h"
 
 #include <vector>
 
@@ -103,7 +102,7 @@ private:
     bool                 m_customAdvData = false;  // Are we using custom advertising data?
     bool                 m_customScanResponseData = false;  // Are we using custom scan response data?
     bool                 m_scanResp = true;
-    bool                 m_advSvcsSet = false;
+    bool                 m_advDataSet = false;
 
 };
 
diff --git a/libesp32/NimBLE-Arduino/src/NimBLECharacteristic.cpp b/libesp32/NimBLE-Arduino/src/NimBLECharacteristic.cpp
index dd1be3bf0..0bd11f6ba 100644
--- a/libesp32/NimBLE-Arduino/src/NimBLECharacteristic.cpp
+++ b/libesp32/NimBLE-Arduino/src/NimBLECharacteristic.cpp
@@ -19,21 +19,18 @@
 #include "NimBLE2902.h"
 #include "NimBLE2904.h"
 #include "NimBLEDevice.h"
-#include "NimBLEUtils.h"
 #include "NimBLELog.h"
 
-#include <string>
-
 #define NULL_HANDLE (0xffff)
 
 static NimBLECharacteristicCallbacks defaultCallback;
-
 static const char* LOG_TAG = "NimBLECharacteristic";
 
 /**
  * @brief Construct a characteristic
  * @param [in] uuid - UUID (const char*) for the characteristic.
  * @param [in] properties - Properties for the characteristic.
+ * @param [in] pService - pointer to the service instance this characteristic belongs to.
  */
 NimBLECharacteristic::NimBLECharacteristic(const char* uuid, uint16_t properties, NimBLEService* pService)
 : NimBLECharacteristic(NimBLEUUID(uuid), properties, pService) {
@@ -43,6 +40,7 @@ NimBLECharacteristic::NimBLECharacteristic(const char* uuid, uint16_t properties
  * @brief Construct a characteristic
  * @param [in] uuid - UUID for the characteristic.
  * @param [in] properties - Properties for the characteristic.
+ * @param [in] pService - pointer to the service instance this characteristic belongs to.
  */
 NimBLECharacteristic::NimBLECharacteristic(const NimBLEUUID &uuid, uint16_t properties, NimBLEService* pService) {
     m_uuid       = uuid;
@@ -50,15 +48,10 @@ NimBLECharacteristic::NimBLECharacteristic(const NimBLEUUID &uuid, uint16_t prop
     m_properties = properties;
     m_pCallbacks = &defaultCallback;
     m_pService   = pService;
-//  Backward Compatibility - to be removed
-/*  setBroadcastProperty((properties & PROPERTY_BROADCAST) != 0);
-    setReadProperty((properties & PROPERTY_READ) != 0);
-    setWriteProperty((properties & PROPERTY_WRITE) != 0);
-    setNotifyProperty((properties & PROPERTY_NOTIFY) != 0);
-    setIndicateProperty((properties & PROPERTY_INDICATE) != 0);
-    setWriteNoResponseProperty((properties & PROPERTY_WRITE_NR) != 0);
-*/
-///////////////////////////////////////////
+    m_value      = "";
+    m_valMux     = portMUX_INITIALIZER_UNLOCKED;
+    m_pTaskData  = nullptr;
+    m_timestamp  = 0;
 } // NimBLECharacteristic
 
 /**
@@ -68,23 +61,6 @@ NimBLECharacteristic::~NimBLECharacteristic() {
 } // ~NimBLECharacteristic
 
 
-/**
- * @brief Associate a descriptor with this characteristic.
- * @param [in] pDescriptor
- * @return N/A.
- */
-void NimBLECharacteristic::addDescriptor(NimBLEDescriptor* pDescriptor) {
-    NIMBLE_LOGD(LOG_TAG, ">> addDescriptor(): Adding %s to %s", pDescriptor->toString().c_str(), toString().c_str());
-    // Check that we don't add the same descriptor twice.
-    if (m_descriptorMap.getByUUID(pDescriptor->getUUID()) != nullptr) {
-        NIMBLE_LOGW(LOG_TAG, "<< Adding a new descriptor with the same UUID as a previous one");
-        //return;
-    }
-    m_descriptorMap.setByUUID(pDescriptor->getUUID(), pDescriptor);
-    NIMBLE_LOGD(LOG_TAG, "<< addDescriptor()");
-} // addDescriptor
-
-
 /**
  * @brief Create a new BLE Descriptor associated with this characteristic.
  * @param [in] uuid - The UUID of the descriptor.
@@ -104,25 +80,26 @@ NimBLEDescriptor* NimBLECharacteristic::createDescriptor(const char* uuid, uint3
  */
 NimBLEDescriptor* NimBLECharacteristic::createDescriptor(const NimBLEUUID &uuid, uint32_t properties, uint16_t max_len) {
     NimBLEDescriptor* pDescriptor = nullptr;
-    if(uuid.equals(NimBLEUUID((uint16_t)0x2902))) {
+    if(uuid == NimBLEUUID(uint16_t(0x2902))) {
         if(!(m_properties & BLE_GATT_CHR_F_NOTIFY) && !(m_properties & BLE_GATT_CHR_F_INDICATE)) {
             assert(0 && "Cannot create 2902 descriptior without characteristic notification or indication property set");
         }
         // We cannot have more than one 2902 descriptor, if it's already been created just return a pointer to it.
-        pDescriptor = m_descriptorMap.getByUUID(uuid);
+        pDescriptor = getDescriptorByUUID(uuid);
         if(pDescriptor == nullptr) {
             pDescriptor = new NimBLE2902(this);
         } else {
             return pDescriptor;
         }
 
-    } else if (uuid.equals(NimBLEUUID((uint16_t)0x2904))) {
+    } else if (uuid == NimBLEUUID(uint16_t(0x2904))) {
         pDescriptor = new NimBLE2904(this);
 
     } else {
         pDescriptor = new NimBLEDescriptor(uuid, properties, max_len, this);
     }
-    addDescriptor(pDescriptor);
+
+    m_dscVec.push_back(pDescriptor);
     return pDescriptor;
 } // createCharacteristic
 
@@ -132,8 +109,8 @@ NimBLEDescriptor* NimBLECharacteristic::createDescriptor(const NimBLEUUID &uuid,
  * @param [in] descriptorUUID The UUID of the descriptor that we wish to retrieve.
  * @return The BLE Descriptor.  If no such descriptor is associated with the characteristic, nullptr is returned.
  */
-NimBLEDescriptor* NimBLECharacteristic::getDescriptorByUUID(const char* descriptorUUID) {
-    return m_descriptorMap.getByUUID(NimBLEUUID(descriptorUUID));
+NimBLEDescriptor* NimBLECharacteristic::getDescriptorByUUID(const char* uuid) {
+    return getDescriptorByUUID(NimBLEUUID(uuid));
 } // getDescriptorByUUID
 
 
@@ -142,8 +119,13 @@ NimBLEDescriptor* NimBLECharacteristic::getDescriptorByUUID(const char* descript
  * @param [in] descriptorUUID The UUID of the descriptor that we wish to retrieve.
  * @return The BLE Descriptor.  If no such descriptor is associated with the characteristic, nullptr is returned.
  */
-NimBLEDescriptor* NimBLECharacteristic::getDescriptorByUUID(const NimBLEUUID &descriptorUUID) {
-    return m_descriptorMap.getByUUID(descriptorUUID);
+NimBLEDescriptor* NimBLECharacteristic::getDescriptorByUUID(const NimBLEUUID &uuid) {
+    for (auto &it : m_dscVec) {
+        if (it->getUUID() == uuid) {
+            return it;
+        }
+    }
+    return nullptr;
 } // getDescriptorByUUID
 
 
@@ -155,13 +137,8 @@ uint16_t NimBLECharacteristic::getHandle() {
     return m_handle;
 } // getHandle
 
-/*
-void NimBLECharacteristic::setAccessPermissions(uint16_t perm) {
-    m_permissions = perm;
-}
-*/
 
-uint8_t NimBLECharacteristic::getProperties() {
+uint16_t NimBLECharacteristic::getProperties() {
     return m_properties;
 } // getProperties
 
@@ -187,26 +164,28 @@ NimBLEUUID NimBLECharacteristic::getUUID() {
  * @brief Retrieve the current value of the characteristic.
  * @return A pointer to storage containing the current characteristic value.
  */
-std::string NimBLECharacteristic::getValue() {
-    return m_value.getValue();
+std::string NimBLECharacteristic::getValue(time_t *timestamp) {
+    portENTER_CRITICAL(&m_valMux);
+    std::string retVal = m_value;
+    if(timestamp != nullptr) {
+        *timestamp = m_timestamp;
+    }
+    portEXIT_CRITICAL(&m_valMux);
+
+    return retVal;
 } // getValue
 
 
-/**
- * @brief Retrieve the current raw data of the characteristic.
- * @return A pointer to storage containing the current characteristic data.
- */
-uint8_t* NimBLECharacteristic::getData() {
-    return m_value.getData();
-} // getData
-
-
 /**
  * @brief Retrieve the the current data length of the characteristic.
  * @return The length of the current characteristic data.
  */
-size_t NimBLECharacteristic:: getDataLength() {
-    return m_value.getLength();
+size_t NimBLECharacteristic::getDataLength() {
+    portENTER_CRITICAL(&m_valMux);
+    size_t len = m_value.length();
+    portEXIT_CRITICAL(&m_valMux);
+
+    return len;
 }
 
 
@@ -225,32 +204,41 @@ int NimBLECharacteristic::handleGapEvent(uint16_t conn_handle, uint16_t attr_han
     if(ble_uuid_cmp(uuid, &pCharacteristic->getUUID().getNative()->u) == 0){
         switch(ctxt->op) {
             case BLE_GATT_ACCESS_OP_READ_CHR: {
-                //NIMBLE_LOGD(LOG_TAG, "read char pkthdr len:%d flags:%d", ctxt->om->om_pkthdr_len, ctxt->om->om_flags);
                 // If the packet header is only 8 bytes this is a follow up of a long read
                 // so we don't want to call the onRead() callback again.
                 if(ctxt->om->om_pkthdr_len > 8) {
                     pCharacteristic->m_pCallbacks->onRead(pCharacteristic);
                 }
-                rc = os_mbuf_append(ctxt->om, pCharacteristic->getData(), pCharacteristic->m_value.getLength());
+
+                portENTER_CRITICAL(&pCharacteristic->m_valMux);
+                rc = os_mbuf_append(ctxt->om, (uint8_t*)pCharacteristic->m_value.data(),
+                                    pCharacteristic->m_value.length());
+                portEXIT_CRITICAL(&pCharacteristic->m_valMux);
+
                 return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
             }
 
             case BLE_GATT_ACCESS_OP_WRITE_CHR: {
-                //NIMBLE_LOGD(LOG_TAG, "write char pkthdr len:%d datalen:%d", ctxt->om->om_pkthdr_len, ctxt->om->om_len);
                 if (ctxt->om->om_len > BLE_ATT_ATTR_MAX_LEN) {
                     return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
                 }
 
-                //pCharacteristic->setValue(ctxt->om->om_data, ctxt->om->om_len);
-                pCharacteristic->m_value.addPart(ctxt->om->om_data, ctxt->om->om_len);
+                uint8_t buf[BLE_ATT_ATTR_MAX_LEN];
+                size_t len = ctxt->om->om_len;
+                memcpy(buf, ctxt->om->om_data,len);
+
                 os_mbuf *next;
                 next = SLIST_NEXT(ctxt->om, om_next);
                 while(next != NULL){
-                    //NIMBLE_LOGD(LOG_TAG, "Found long write data, len:%d", next->om_len);
-                    pCharacteristic->m_value.addPart(next->om_data, next->om_len);
+                    if((len + next->om_len) > BLE_ATT_ATTR_MAX_LEN) {
+                        return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
+                    }
+                    memcpy(&buf[len-1], next->om_data, next->om_len);
+                    len += next->om_len;
                     next = SLIST_NEXT(next, om_next);
                 }
-                pCharacteristic->m_value.commit();
+
+                pCharacteristic->setValue(buf, len);
                 pCharacteristic->m_pCallbacks->onWrite(pCharacteristic);
 
                 return 0;
@@ -278,14 +266,19 @@ void NimBLECharacteristic::setSubscribe(struct ble_gap_event *event) {
         subVal |= NIMBLE_DESC_FLAG_INDICATE;
     }
 
-    m_semaphoreConfEvt.give((subVal | NIMBLE_DESC_FLAG_INDICATE) ? 0 :
-            NimBLECharacteristicCallbacks::Status::ERROR_INDICATE_DISABLED);
+    if(m_pTaskData != nullptr) {
+        m_pTaskData->rc = (subVal & NIMBLE_DESC_FLAG_INDICATE) ? 0 :
+                          NimBLECharacteristicCallbacks::Status::ERROR_INDICATE_DISABLED;
+        xTaskNotifyGive(m_pTaskData->task);
+    }
 
-    NIMBLE_LOGI(LOG_TAG, "New subscribe value for conn: %d val: %d", event->subscribe.conn_handle, subVal);
+    NIMBLE_LOGI(LOG_TAG, "New subscribe value for conn: %d val: %d",
+                         event->subscribe.conn_handle, subVal);
 
-    NimBLE2902* p2902 = (NimBLE2902*)getDescriptorByUUID((uint16_t)0x2902);
+    NimBLE2902* p2902 = (NimBLE2902*)getDescriptorByUUID(uint16_t(0x2902));
     if(p2902 == nullptr){
-        ESP_LOGE(LOG_TAG, "No 2902 descriptor found for %s", getUUID().toString().c_str());
+        ESP_LOGE(LOG_TAG, "No 2902 descriptor found for %s",
+        std::string(getUUID()).c_str());
         return;
     }
 
@@ -294,21 +287,28 @@ void NimBLECharacteristic::setSubscribe(struct ble_gap_event *event) {
     p2902->m_pCallbacks->onWrite(p2902);
 
 
-    auto it = p2902->m_subscribedMap.find(event->subscribe.conn_handle);
-    if(subVal > 0 && it == p2902->m_subscribedMap.cend()) {
-        p2902->m_subscribedMap.insert(std::pair<uint16_t, uint16_t>(event->subscribe.conn_handle, subVal));
-        return;
-    } else if(it != p2902->m_subscribedMap.cend()) {
-        p2902->m_subscribedMap.erase(event->subscribe.conn_handle);
-        return;
+    auto it = p2902->m_subscribedVec.begin();
+    for(;it != p2902->m_subscribedVec.end(); ++it) {
+        if((*it).conn_id == event->subscribe.conn_handle) {
+            break;
+        }
     }
-/*
-    if(event->subscribe.reason == BLE_GAP_SUBSCRIBE_REASON_TERM) {
-        p2902->m_subscribedMap.erase(event->subscribe.conn_handle);
-        return;
+
+    if(subVal > 0) {
+        if(it == p2902->m_subscribedVec.end()) {
+            chr_sub_status_t client_sub;
+            client_sub.conn_id = event->subscribe.conn_handle;
+            client_sub.sub_val = subVal;
+            p2902->m_subscribedVec.push_back(client_sub);
+            return;
+        }
+
+        (*it).sub_val = subVal;
+
+    } else if(it != p2902->m_subscribedVec.end()) {
+        p2902->m_subscribedVec.erase(it);
+        p2902->m_subscribedVec.shrink_to_fit();
     }
-*/
-    (*it).second = subVal;
 }
 
 
@@ -319,7 +319,7 @@ void NimBLECharacteristic::setSubscribe(struct ble_gap_event *event) {
  * @return N/A
  */
 void NimBLECharacteristic::indicate() {
-    NIMBLE_LOGD(LOG_TAG, ">> indicate: length: %d", m_value.getValue().length());
+    NIMBLE_LOGD(LOG_TAG, ">> indicate: length: %d", getDataLength());
     notify(false);
     NIMBLE_LOGD(LOG_TAG, "<< indicate");
 } // indicate
@@ -331,91 +331,102 @@ void NimBLECharacteristic::indicate() {
  * @return N/A.
  */
 void NimBLECharacteristic::notify(bool is_notification) {
-    NIMBLE_LOGD(LOG_TAG, ">> notify: length: %d", m_value.getValue().length());
+    NIMBLE_LOGD(LOG_TAG, ">> notify: length: %d", getDataLength());
 
-    assert(getService() != nullptr);
-    assert(getService()->getServer() != nullptr);
+    NimBLE2902* p2902 = (NimBLE2902*)getDescriptorByUUID(uint16_t(0x2902));
 
+    if(p2902 == nullptr) {
+        NIMBLE_LOGE(LOG_TAG,
+                    "<< notify-Error; Notify/indicate not enabled for characterisitc: %s",
+                    std::string(getUUID()).c_str());
+    }
 
-    if (getService()->getServer()->getConnectedCount() == 0) {
-        NIMBLE_LOGD(LOG_TAG, "<< notify: No connected clients.");
+    if (p2902->m_subscribedVec.size() == 0) {
+        NIMBLE_LOGD(LOG_TAG, "<< notify: No clients subscribed.");
         return;
     }
 
     m_pCallbacks->onNotify(this);
 
+    std::string value = getValue();
+    size_t length = value.length();
+    bool reqSec = (m_properties & BLE_GATT_CHR_F_READ_AUTHEN) ||
+                  (m_properties & BLE_GATT_CHR_F_READ_AUTHOR) ||
+                  (m_properties & BLE_GATT_CHR_F_READ_ENC);
     int rc = 0;
-    NimBLE2902* p2902 = (NimBLE2902*)getDescriptorByUUID((uint16_t)0x2902);
 
-    for (auto it = p2902->m_subscribedMap.cbegin(); it != p2902->m_subscribedMap.cend(); ++it) {
-        uint16_t _mtu = getService()->getServer()->getPeerMTU((*it).first);
-        // Must rebuild the data on each loop iteration as NimBLE will release it.
-        size_t length = m_value.getValue().length();
-        uint8_t* data = (uint8_t*)m_value.getValue().data();
-        os_mbuf *om;
+    for (auto &it : p2902->m_subscribedVec) {
+        uint16_t _mtu = getService()->getServer()->getPeerMTU(it.conn_id);
 
-        if(_mtu == 0) {
-            //NIMBLE_LOGD(LOG_TAG, "peer not connected, removing from map");
-            p2902->m_subscribedMap.erase((*it).first);
-            it = p2902->m_subscribedMap.cbegin();
-            if(it == p2902->m_subscribedMap.cend()) {
-                return;
-            }
+        // check if connected and subscribed
+        if(_mtu == 0 || it.sub_val == 0) {
             continue;
         }
 
+        // check if security requirements are satisfied
+        if(reqSec) {
+            struct ble_gap_conn_desc desc;
+            rc = ble_gap_conn_find(it.conn_id, &desc);
+            if(rc != 0 || !desc.sec_state.encrypted) {
+                continue;
+            }
+        }
+
         if (length > _mtu - 3) {
             NIMBLE_LOGW(LOG_TAG, "- Truncating to %d bytes (maximum notify size)", _mtu - 3);
         }
 
-        if((*it).second == 0) {
-            //NIMBLE_LOGI(LOG_TAG, "Skipping unsubscribed client");
-            continue;
-        }
-
-        if(is_notification && (!((*it).second & NIMBLE_DESC_FLAG_NOTIFY))) {
+        if(is_notification && (!(it.sub_val & NIMBLE_DESC_FLAG_NOTIFY))) {
             NIMBLE_LOGW(LOG_TAG,
             "Sending notification to client subscribed to indications, sending indication instead");
             is_notification = false;
         }
 
-        if(!is_notification && (!((*it).second & NIMBLE_DESC_FLAG_INDICATE))) {
+        if(!is_notification && (!(it.sub_val & NIMBLE_DESC_FLAG_INDICATE))) {
             NIMBLE_LOGW(LOG_TAG,
-            "Sending indication to client subscribed to notifications, sending notifications instead");
+            "Sending indication to client subscribed to notification, sending notification instead");
             is_notification = true;
         }
 
         // don't create the m_buf until we are sure to send the data or else
         // we could be allocating a buffer that doesn't get released.
         // We also must create it in each loop iteration because it is consumed with each host call.
-        om = ble_hs_mbuf_from_flat(data, length);
+        os_mbuf *om = ble_hs_mbuf_from_flat((uint8_t*)value.data(), length);
 
-        if(!is_notification) {
-            m_semaphoreConfEvt.take("indicate");
-            rc = ble_gattc_indicate_custom((*it).first, m_handle, om);
+        NimBLECharacteristicCallbacks::Status statusRC;
+
+        if(!is_notification && (m_properties & NIMBLE_PROPERTY::INDICATE)) {
+            ble_task_data_t taskData = {nullptr, xTaskGetCurrentTaskHandle(),0, nullptr};
+            m_pTaskData = &taskData;
+
+            rc = ble_gattc_indicate_custom(it.conn_id, m_handle, om);
             if(rc != 0){
-                m_semaphoreConfEvt.give();
-                m_pCallbacks->onStatus(this, NimBLECharacteristicCallbacks::Status::ERROR_GATT, rc);
-                return;
+                statusRC = NimBLECharacteristicCallbacks::Status::ERROR_GATT;
+            } else {
+                ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
+                rc = m_pTaskData->rc;
             }
 
-            rc = m_semaphoreConfEvt.wait();
+            m_pTaskData = nullptr;
 
-            if(rc == BLE_HS_ETIMEOUT) {
-                m_pCallbacks->onStatus(this, NimBLECharacteristicCallbacks::Status::ERROR_INDICATE_TIMEOUT, rc);
-            } else if(rc == BLE_HS_EDONE) {
-                m_pCallbacks->onStatus(this, NimBLECharacteristicCallbacks::Status::SUCCESS_INDICATE, rc);
+            if(rc == BLE_HS_EDONE) {
+                rc = 0;
+                statusRC = NimBLECharacteristicCallbacks::Status::SUCCESS_INDICATE;
+            } else if(rc == BLE_HS_ETIMEOUT) {
+                statusRC = NimBLECharacteristicCallbacks::Status::ERROR_INDICATE_TIMEOUT;
             } else {
-                m_pCallbacks->onStatus(this, NimBLECharacteristicCallbacks::Status::ERROR_INDICATE_FAILURE, rc);
+                statusRC = NimBLECharacteristicCallbacks::Status::ERROR_INDICATE_FAILURE;
             }
         } else {
-            rc = ble_gattc_notify_custom((*it).first, m_handle, om);
+            rc = ble_gattc_notify_custom(it.conn_id, m_handle, om);
             if(rc == 0) {
-                m_pCallbacks->onStatus(this, NimBLECharacteristicCallbacks::Status::SUCCESS_NOTIFY, 0);
+                statusRC = NimBLECharacteristicCallbacks::Status::SUCCESS_NOTIFY;
             } else {
-                m_pCallbacks->onStatus(this, NimBLECharacteristicCallbacks::Status::ERROR_GATT, rc);
+                statusRC = NimBLECharacteristicCallbacks::Status::ERROR_GATT;
             }
         }
+
+        m_pCallbacks->onStatus(this, statusRC, rc);
     }
 
     NIMBLE_LOGD(LOG_TAG, "<< notify");
@@ -434,87 +445,6 @@ void NimBLECharacteristic::setCallbacks(NimBLECharacteristicCallbacks* pCallback
     }
 } // setCallbacks
 
-// Backward compatibility - to be removed ////////////////////////////////
-/**
- * @brief Set the permission to broadcast.
- * A characteristics has properties associated with it which define what it is capable of doing.
- * One of these is the broadcast flag.
- * @param [in] value The flag value of the property.
- * @return N/A
- */
-void NimBLECharacteristic::setBroadcastProperty(bool value) {
-    if (value) {
-        m_properties = (m_properties | BLE_GATT_CHR_F_BROADCAST);
-    } else {
-        m_properties = (m_properties & ~BLE_GATT_CHR_F_BROADCAST);
-    }
-} // setBroadcastProperty
-
-
-/**
- * @brief Set the Indicate property value.
- * @param [in] value Set to true if we are to allow indicate messages.
- */
-void NimBLECharacteristic::setIndicateProperty(bool value) {
-    if (value) {
-        m_properties = (m_properties | BLE_GATT_CHR_F_INDICATE);
-    } else {
-        m_properties = (m_properties & ~BLE_GATT_CHR_F_INDICATE);
-    }
-} // setIndicateProperty
-
-
-/**
- * @brief Set the Notify property value.
- * @param [in] value Set to true if we are to allow notification messages.
- */
-void NimBLECharacteristic::setNotifyProperty(bool value) {
-    if (value) {
-        m_properties = (m_properties | BLE_GATT_CHR_F_NOTIFY);
-    } else {
-        m_properties = (m_properties & ~BLE_GATT_CHR_F_NOTIFY);
-    }
-} // setNotifyProperty
-
-
-/**
- * @brief Set the Read property value.
- * @param [in] value Set to true if we are to allow reads.
- */
-void NimBLECharacteristic::setReadProperty(bool value) {
-    if (value) {
-        m_properties = (m_properties | BLE_GATT_CHR_F_READ);
-    } else {
-        m_properties = (m_properties & ~BLE_GATT_CHR_F_READ);
-    }
-} // setReadProperty
-
-
-/**
- * @brief Set the Write No Response property value.
- * @param [in] value Set to true if we are to allow writes with no response.
- */
-void NimBLECharacteristic::setWriteNoResponseProperty(bool value) {
-    if (value) {
-        m_properties = (m_properties | BLE_GATT_CHR_F_WRITE_NO_RSP);
-    } else {
-        m_properties = (m_properties & ~BLE_GATT_CHR_F_WRITE_NO_RSP);
-    }
-} // setWriteNoResponseProperty
-
-
-/**
- * @brief Set the Write property value.
- * @param [in] value Set to true if we are to allow writes.
- */
-void NimBLECharacteristic::setWriteProperty(bool value) {
-    if (value) {
-        m_properties = (m_properties | BLE_GATT_CHR_F_WRITE );
-    } else {
-        m_properties = (m_properties & ~BLE_GATT_CHR_F_WRITE );
-    }
-} // setWriteProperty
-//////////////////////////////////////////////////////////////////////////////////
 
 /**
  * @brief Set the value of the characteristic.
@@ -522,21 +452,21 @@ void NimBLECharacteristic::setWriteProperty(bool value) {
  * @param [in] length The length of the data in bytes.
  */
 void NimBLECharacteristic::setValue(const uint8_t* data, size_t length) {
+#if CONFIG_LOG_DEFAULT_LEVEL > 3 || (ARDUINO_ARCH_ESP32 && CORE_DEBUG_LEVEL >= 4)
     char* pHex = NimBLEUtils::buildHexData(nullptr, data, length);
     NIMBLE_LOGD(LOG_TAG, ">> setValue: length=%d, data=%s, characteristic UUID=%s", length, pHex, getUUID().toString().c_str());
     free(pHex);
+#endif
 
     if (length > BLE_ATT_ATTR_MAX_LEN) {
         NIMBLE_LOGE(LOG_TAG, "Size %d too large, must be no bigger than %d", length, BLE_ATT_ATTR_MAX_LEN);
         return;
     }
 
-    m_value.setValue(data, length);
-
-  //  if(m_handle != NULL_HANDLE) {
-        //ble_gatts_chr_updated(m_handle);
-       // ble_gattc_notify(getService()->getServer()->m_connId, m_handle);
-  //  }
+    portENTER_CRITICAL(&m_valMux);
+    m_value = std::string((char*)data, length);
+    m_timestamp = time(nullptr);
+    portEXIT_CRITICAL(&m_valMux);
 
     NIMBLE_LOGD(LOG_TAG, "<< setValue");
 } // setValue
@@ -553,41 +483,6 @@ void NimBLECharacteristic::setValue(const std::string &value) {
     setValue((uint8_t*)(value.data()), value.length());
 } // setValue
 
-void NimBLECharacteristic::setValue(uint16_t& data16) {
-    uint8_t temp[2];
-    temp[0] = data16;
-    temp[1] = data16 >> 8;
-    setValue(temp, 2);
-} // setValue
-
-void NimBLECharacteristic::setValue(uint32_t& data32) {
-    uint8_t temp[4];
-    temp[0] = data32;
-    temp[1] = data32 >> 8;
-    temp[2] = data32 >> 16;
-    temp[3] = data32 >> 24;
-    setValue(temp, 4);
-} // setValue
-
-void NimBLECharacteristic::setValue(int& data32) {
-    uint8_t temp[4];
-    temp[0] = data32;
-    temp[1] = data32 >> 8;
-    temp[2] = data32 >> 16;
-    temp[3] = data32 >> 24;
-    setValue(temp, 4);
-} // setValue
-
-void NimBLECharacteristic::setValue(float& data32) {
-    float temp = data32;
-    setValue((uint8_t*)&temp, 4);
-} // setValue
-
-void NimBLECharacteristic::setValue(double& data64) {
-    double temp = data64;
-    setValue((uint8_t*)&temp, 8);
-} // setValue
-
 
 /**
  * @brief Return a string representation of the characteristic.
diff --git a/libesp32/NimBLE-Arduino/src/NimBLECharacteristic.h b/libesp32/NimBLE-Arduino/src/NimBLECharacteristic.h
index 25e1ec7f7..4474e5f29 100644
--- a/libesp32/NimBLE-Arduino/src/NimBLECharacteristic.h
+++ b/libesp32/NimBLE-Arduino/src/NimBLECharacteristic.h
@@ -42,42 +42,15 @@ typedef enum {
 
 #include "NimBLEService.h"
 #include "NimBLEDescriptor.h"
-#include "NimBLEUUID.h"
-#include "NimBLEValue.h"
-#include "FreeRTOS.h"
 
 #include <string>
-#include <map>
-
+#include <vector>
 
 class NimBLEService;
 class NimBLEDescriptor;
 class NimBLECharacteristicCallbacks;
 
 
-/**
- * @brief A management structure for %BLE descriptors.
- */
-class NimBLEDescriptorMap {
-public:
-    void setByUUID(const char* uuid, NimBLEDescriptor* pDescriptor);
-    void setByUUID(const NimBLEUUID &uuid, NimBLEDescriptor* pDescriptor);
-//  void setByHandle(uint16_t handle, NimBLEDescriptor* pDescriptor);
-    NimBLEDescriptor* getByUUID(const char* uuid);
-    NimBLEDescriptor* getByUUID(const NimBLEUUID &uuid);
-//  NimBLEDescriptor* getByHandle(uint16_t handle);
-    std::string toString();
-    NimBLEDescriptor* getFirst();
-    NimBLEDescriptor* getNext();
-    uint8_t           getSize();
-
-private:
-    std::map<NimBLEDescriptor*, std::string> m_uuidMap;
-//  std::map<uint16_t, BLEDescriptor*> m_handleMap;
-    std::map<NimBLEDescriptor*, std::string>::iterator m_iterator;
-};
-
-
 /**
  * @brief The model of a %BLE Characteristic.
  *
@@ -87,84 +60,78 @@ private:
 class NimBLECharacteristic {
 public:
     NimBLEDescriptor* createDescriptor(const char* uuid,
-                        uint32_t properties = NIMBLE_PROPERTY::READ |
-                                              NIMBLE_PROPERTY::WRITE,
-                                     uint16_t max_len = 100);
+                                       uint32_t properties =
+                                       NIMBLE_PROPERTY::READ |
+                                       NIMBLE_PROPERTY::WRITE,
+                                       uint16_t max_len = 100);
     NimBLEDescriptor* createDescriptor(const NimBLEUUID &uuid,
-                        uint32_t properties = NIMBLE_PROPERTY::READ |
-                                              NIMBLE_PROPERTY::WRITE,
-                                     uint16_t max_len = 100);
+                                       uint32_t properties =
+                                       NIMBLE_PROPERTY::READ |
+                                       NIMBLE_PROPERTY::WRITE,
+                                       uint16_t max_len = 100);
 
-    NimBLEDescriptor* getDescriptorByUUID(const char* descriptorUUID);
-    NimBLEDescriptor* getDescriptorByUUID(const NimBLEUUID &descriptorUUID);
+    NimBLEDescriptor* getDescriptorByUUID(const char* uuid);
+    NimBLEDescriptor* getDescriptorByUUID(const NimBLEUUID &uuid);
     NimBLEUUID        getUUID();
-    std::string       getValue();
-    uint8_t*          getData();
+    std::string       getValue(time_t *timestamp = nullptr);
+
+    template<typename T>
+    T                 getValue(time_t *timestamp = nullptr, bool skipSizeCheck = false) {
+        std::string value = getValue();
+        if(!skipSizeCheck && value.size() < sizeof(T)) return T();
+        const char *pData = value.data();
+        return *((T *)pData);
+    }
+
     size_t            getDataLength();
+    void              indicate();
+    void              notify(bool is_notification = true);
+    void              setCallbacks(NimBLECharacteristicCallbacks* pCallbacks);
+    void              setValue(const uint8_t* data, size_t size);
+    void              setValue(const std::string &value);
 
-    void indicate();
-    void notify(bool is_notification = true);
-    void setCallbacks(NimBLECharacteristicCallbacks* pCallbacks);
-//  Backward Compatibility - to be removed
-    void setBroadcastProperty(bool value);
-    void setIndicateProperty(bool value);
-    void setNotifyProperty(bool value);
-    void setReadProperty(bool value);
-    void setWriteProperty(bool value);
-    void setWriteNoResponseProperty(bool value);
-//////////////////////////////////////////////////////
-    void setValue(const uint8_t* data, size_t size);
-    void setValue(const std::string &value);
-    void setValue(uint16_t& data16);
-    void setValue(uint32_t& data32);
-    void setValue(int& data32);
-    void setValue(float& data32);
-    void setValue(double& data64);
+    template<typename T>
+    void setValue(const T &s) {
+        setValue((uint8_t*)&s, sizeof(T));
+    }
 
-    std::string toString();
-    uint16_t getHandle();
-//  void setAccessPermissions(uint16_t perm);
-
-//  Backward Compatibility - to be removed
-/*  static const uint32_t PROPERTY_READ      = 1<<0;
-    static const uint32_t PROPERTY_WRITE     = 1<<1;
-    static const uint32_t PROPERTY_NOTIFY    = 1<<2;
-    static const uint32_t PROPERTY_BROADCAST = 1<<3;
-    static const uint32_t PROPERTY_INDICATE  = 1<<4;
-    static const uint32_t PROPERTY_WRITE_NR  = 1<<5;
-*/
-//////////////////////////////////////////////////////
+    std::string       toString();
+    uint16_t          getHandle();
 
 private:
 
-    friend class NimBLEServer;
-    friend class NimBLEService;
-//  friend class NimBLEDescriptor;
-//  friend class NimBLECharacteristicMap;
+    friend class      NimBLEServer;
+    friend class      NimBLEService;
 
-    NimBLECharacteristic(const char* uuid, uint16_t properties = NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::WRITE,
-                                                        NimBLEService* pService = nullptr);
-    NimBLECharacteristic(const NimBLEUUID &uuid, uint16_t properties = NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::WRITE,
-                                                        NimBLEService* pService = nullptr);
-    virtual ~NimBLECharacteristic();
+    NimBLECharacteristic(const char* uuid,
+                         uint16_t properties =
+                         NIMBLE_PROPERTY::READ |
+                         NIMBLE_PROPERTY::WRITE,
+                         NimBLEService* pService = nullptr);
+    NimBLECharacteristic(const NimBLEUUID &uuid,
+                         uint16_t properties =
+                         NIMBLE_PROPERTY::READ |
+                         NIMBLE_PROPERTY::WRITE,
+                         NimBLEService* pService = nullptr);
+
+    ~NimBLECharacteristic();
+
+    NimBLEService*  getService();
+    uint16_t        getProperties();
+    void            setSubscribe(struct ble_gap_event *event);
+    static int      handleGapEvent(uint16_t conn_handle, uint16_t attr_handle,
+                                   struct ble_gatt_access_ctxt *ctxt, void *arg);
 
     NimBLEUUID                     m_uuid;
-    NimBLEDescriptorMap            m_descriptorMap;
     uint16_t                       m_handle;
     uint16_t                       m_properties;
     NimBLECharacteristicCallbacks* m_pCallbacks;
     NimBLEService*                 m_pService;
-    NimBLEValue                    m_value;
-//  uint16_t                       m_permissions;
-
-    void            addDescriptor(NimBLEDescriptor* pDescriptor);
-    NimBLEService*  getService();
-    uint8_t         getProperties();
-    void            setSubscribe(struct ble_gap_event *event);
-    static int      handleGapEvent(uint16_t conn_handle, uint16_t attr_handle,
-                                struct ble_gatt_access_ctxt *ctxt, void *arg);
-
-    FreeRTOS::Semaphore m_semaphoreConfEvt   = FreeRTOS::Semaphore("ConfEvt");
+    std::string                    m_value;
+    std::vector<NimBLEDescriptor*> m_dscVec;
+    ble_task_data_t                *m_pTaskData;
+    portMUX_TYPE                   m_valMux;
+    time_t                         m_timestamp;
 }; // NimBLECharacteristic
 
 
diff --git a/libesp32/NimBLE-Arduino/src/NimBLECharacteristicMap.cpp b/libesp32/NimBLE-Arduino/src/NimBLECharacteristicMap.cpp
deleted file mode 100644
index b3cdcf9c6..000000000
--- a/libesp32/NimBLE-Arduino/src/NimBLECharacteristicMap.cpp
+++ /dev/null
@@ -1,132 +0,0 @@
-/*
- * NimBLECharacteristicMap.cpp
- *
- *  Created: on March 3, 2020
- *      Author H2zero
- *
- * BLECharacteristicMap.cpp
- *
- *  Created on: Jun 22, 2017
- *      Author: kolban
- */
-#include "sdkconfig.h"
-#if defined(CONFIG_BT_ENABLED)
-
-#include "nimconfig.h"
-#if defined(CONFIG_BT_NIMBLE_ROLE_PERIPHERAL)
-
-#include "NimBLEService.h"
-#include "NimBLELog.h"
-
-
-/**
- * @brief Return the characteristic by handle.
- * @param [in] handle The handle to look up the characteristic.
- * @return The characteristic.
- */
-NimBLECharacteristic* NimBLECharacteristicMap::getByHandle(uint16_t handle) {
-    return m_handleMap.at(handle);
-} // getByHandle
-
-
-/**
- * @brief Return the characteristic by UUID.
- * @param [in] UUID The UUID to look up the characteristic.
- * @return The characteristic.
- */
-NimBLECharacteristic* NimBLECharacteristicMap::getByUUID(const char* uuid) {
-    return getByUUID(NimBLEUUID(uuid));
-}
-
-
-/**
- * @brief Return the characteristic by UUID.
- * @param [in] UUID The UUID to look up the characteristic.
- * @return The characteristic.
- */
-NimBLECharacteristic* NimBLECharacteristicMap::getByUUID(const NimBLEUUID &uuid) {
-    for (auto &myPair : m_uuidMap) {
-        if (myPair.first->getUUID().equals(uuid)) {
-            return myPair.first;
-        }
-    }
-
-    return nullptr;
-} // getByUUID
-
-/**
- * @brief Get the number of characteristics in the map.
- */
-uint8_t NimBLECharacteristicMap::getSize() {
-    return (uint8_t)m_uuidMap.size();
-} // getSize
-
-/**
- * @brief Get the first characteristic in the map.
- * @return The first characteristic in the map.
- */
-NimBLECharacteristic* NimBLECharacteristicMap::getFirst() {
-    m_iterator = m_uuidMap.begin();
-    if (m_iterator == m_uuidMap.end()) return nullptr;
-    NimBLECharacteristic* pRet = m_iterator->first;
-    m_iterator++;
-    return pRet;
-} // getFirst
-
-
-/**
- * @brief Get the next characteristic in the map.
- * @return The next characteristic in the map.
- */
-NimBLECharacteristic* NimBLECharacteristicMap::getNext() {
-    if (m_iterator == m_uuidMap.end()) return nullptr;
-    NimBLECharacteristic* pRet = m_iterator->first;
-    m_iterator++;
-    return pRet;
-} // getNext
-
-
-/**
- * @brief Set the characteristic by handle.
- * @param [in] handle The handle of the characteristic.
- * @param [in] characteristic The characteristic to cache.
- * @return N/A.
- */
-void NimBLECharacteristicMap::setByHandle(uint16_t handle, NimBLECharacteristic* characteristic) {
-    m_handleMap.insert(std::pair<uint16_t, NimBLECharacteristic*>(handle, characteristic));
-} // setByHandle
-
-
-/**
- * @brief Set the characteristic by UUID.
- * @param [in] uuid The uuid of the characteristic.
- * @param [in] characteristic The characteristic to cache.
- * @return N/A.
- */
-void NimBLECharacteristicMap::setByUUID(NimBLECharacteristic* pCharacteristic, const NimBLEUUID &uuid) {
-    m_uuidMap.insert(std::pair<NimBLECharacteristic*, std::string>(pCharacteristic, uuid.toString()));
-} // setByUUID
-
-
-/**
- * @brief Return a string representation of the characteristic map.
- * @return A string representation of the characteristic map.
- */
-std::string NimBLECharacteristicMap::toString() {
-    std::string res;
-    int count = 0;
-    char hex[5];
-    for (auto &myPair: m_uuidMap) {
-        if (count > 0) {res += "\n";}
-        snprintf(hex, sizeof(hex), "%04x", myPair.first->getHandle());
-        count++;
-        res += "handle: 0x";
-        res += hex;
-        res += ", uuid: " + myPair.first->getUUID().toString();
-    }
-    return res;
-} // toString
-
-
-#endif // #if defined(CONFIG_BT_NIMBLE_ROLE_PERIPHERAL)
-#endif /* CONFIG_BT_ENABLED */
diff --git a/libesp32/NimBLE-Arduino/src/NimBLEClient.cpp b/libesp32/NimBLE-Arduino/src/NimBLEClient.cpp
index a385126d2..71f1c9afc 100644
--- a/libesp32/NimBLE-Arduino/src/NimBLEClient.cpp
+++ b/libesp32/NimBLE-Arduino/src/NimBLEClient.cpp
@@ -18,7 +18,6 @@
 #if defined( CONFIG_BT_NIMBLE_ROLE_CENTRAL)
 
 #include "NimBLEClient.h"
-#include "NimBLEUtils.h"
 #include "NimBLEDevice.h"
 #include "NimBLELog.h"
 
@@ -54,7 +53,10 @@ NimBLEClient::NimBLEClient()
     m_pClientCallbacks = &defaultCallbacks;
     m_conn_id          = BLE_HS_CONN_HANDLE_NONE;
     m_isConnected      = false;
+    m_waitingToConnect = false;
     m_connectTimeout   = 30000;
+    m_deleteCallbacks  = false;
+    m_pTaskData        = nullptr;
 
     m_pConnParams.scan_itvl = 16;          // Scan interval in 0.625ms units (NimBLE Default)
     m_pConnParams.scan_window = 16;        // Scan window in 0.625ms units (NimBLE Default)
@@ -74,7 +76,7 @@ NimBLEClient::NimBLEClient()
 NimBLEClient::~NimBLEClient() {
     // We may have allocated service references associated with this client.
     // Before we are finished with the client, we must release resources.
-    clearServices();
+    deleteServices();
 
     if(m_deleteCallbacks && m_pClientCallbacks != &defaultCallbacks) {
         delete m_pClientCallbacks;
@@ -84,18 +86,40 @@ NimBLEClient::~NimBLEClient() {
 
 
 /**
- * @brief Clear any existing services.
+ * @brief Delete any existing services.
  */
-void NimBLEClient::clearServices() {
-    NIMBLE_LOGD(LOG_TAG, ">> clearServices");
+void NimBLEClient::deleteServices() {
+    NIMBLE_LOGD(LOG_TAG, ">> deleteServices");
     // Delete all the services.
     for(auto &it: m_servicesVector) {
         delete it;
     }
     m_servicesVector.clear();
 
-    NIMBLE_LOGD(LOG_TAG, "<< clearServices");
-} // clearServices
+    NIMBLE_LOGD(LOG_TAG, "<< deleteServices");
+} // deleteServices
+
+
+/**
+ * @brief Delete service by UUID
+ * @param [in] uuid The UUID of the service to be deleted from the local database.
+ * @return Number of services left.
+ */
+size_t NimBLEClient::deleteService(const NimBLEUUID &uuid) {
+    NIMBLE_LOGD(LOG_TAG, ">> deleteService");
+    // Delete the requested service.
+    for(auto it = m_servicesVector.begin(); it != m_servicesVector.end(); ++it) {
+        if((*it)->getUUID() == uuid) {
+            delete *it;
+            m_servicesVector.erase(it);
+            break;
+        }
+    }
+
+    NIMBLE_LOGD(LOG_TAG, "<< deleteService");
+
+    return m_servicesVector.size();
+} // deleteServices
 
 
 /**
@@ -135,6 +159,10 @@ bool NimBLEClient::connect(const NimBLEAddress &address, uint8_t type, bool refr
         return false;
     }
 
+    if(!NimBLEDevice::getScan()->stop()) {
+        return false;
+    }
+
     int rc = 0;
     m_peerAddress = address;
 
@@ -142,7 +170,8 @@ bool NimBLEClient::connect(const NimBLEAddress &address, uint8_t type, bool refr
     memcpy(&peerAddrt.val, address.getNative(),6);
     peerAddrt.type = type;
 
-    m_semaphoreOpenEvt.take("connect");
+    ble_task_data_t taskData = {this, xTaskGetCurrentTaskHandle(), 0, nullptr};
+    m_pTaskData = &taskData;
 
     /** Try to connect the the advertiser.  Allow 30 seconds (30000 ms) for
      *  timeout (default value of m_connectTimeout).
@@ -152,7 +181,7 @@ bool NimBLEClient::connect(const NimBLEAddress &address, uint8_t type, bool refr
         rc = ble_gap_connect(BLE_OWN_ADDR_PUBLIC, &peerAddrt, m_connectTimeout, &m_pConnParams,
                             NimBLEClient::handleGapEvent, this);
         if(rc == BLE_HS_EBUSY) {
-            vTaskDelay(1);
+            vTaskDelay(1 / portTICK_PERIOD_MS);
         }
     }while(rc == BLE_HS_EBUSY);
 
@@ -162,23 +191,23 @@ bool NimBLEClient::connect(const NimBLEAddress &address, uint8_t type, bool refr
                     type,
                     m_peerAddress.toString().c_str(),
                     rc, NimBLEUtils::returnCodeToString(rc));
-
-        m_semaphoreOpenEvt.give();
+        m_pTaskData = nullptr;
         m_waitingToConnect = false;
         return false;
     }
 
     m_waitingToConnect = true;
 
-    rc = m_semaphoreOpenEvt.wait("connect");   // Wait for the connection to complete.
+    // Wait for the connection to complete.
+    ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
 
-    if(rc != 0){
+    if(taskData.rc != 0){
         return false;
     }
 
     if(refreshServices) {
         NIMBLE_LOGD(LOG_TAG, "Refreshing Services for: (%s)", address.toString().c_str());
-        clearServices();
+        deleteServices();
     }
 
     m_pClientCallbacks->onConnect(this);
@@ -194,17 +223,18 @@ bool NimBLEClient::connect(const NimBLEAddress &address, uint8_t type, bool refr
  * @return True on success.
  */
 bool NimBLEClient::secureConnection() {
-
-    m_semeaphoreSecEvt.take("secureConnection");
+    ble_task_data_t taskData = {this, xTaskGetCurrentTaskHandle(), 0, nullptr};
+    m_pTaskData = &taskData;
 
     int rc = NimBLEDevice::startSecurity(m_conn_id);
     if(rc != 0){
-        m_semeaphoreSecEvt.give();
+        m_pTaskData = nullptr;
         return false;
     }
 
-    rc = m_semeaphoreSecEvt.wait("secureConnection");
-    if(rc != 0){
+    ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
+
+    if(taskData.rc != 0){
         return false;
     }
 
@@ -220,15 +250,11 @@ int NimBLEClient::disconnect(uint8_t reason) {
     NIMBLE_LOGD(LOG_TAG, ">> disconnect()");
     int rc = 0;
     if(m_isConnected){
-        m_isConnected = false; // flag the disconnect now so no calls are performed after
         rc = ble_gap_terminate(m_conn_id, reason);
         if(rc != 0){
             NIMBLE_LOGE(LOG_TAG, "ble_gap_terminate failed: rc=%d %s", rc,
                                     NimBLEUtils::returnCodeToString(rc));
         }
-        // Sometimes a disconnect event is not sent so we need to make sure
-        // the device can be found again.
-        NimBLEDevice::removeIgnored(m_peerAddress);
     }
 
     NIMBLE_LOGD(LOG_TAG, "<< disconnect()");
@@ -394,15 +420,13 @@ NimBLERemoteService* NimBLEClient::getService(const NimBLEUUID &uuid) {
  * @param [in] bool value to indicate if the current vector should be cleared and
  * subsequently all services retrieved from the peripheral.
  * If false the vector will be returned with the currently stored services,
- * if vector is empty it will retrieve all services from the peripheral.
+ * If true it will retrieve all services from the peripheral and return the vector with all services
  * @return a pointer to the vector of available services.
  */
 std::vector<NimBLERemoteService*>* NimBLEClient::getServices(bool refresh) {
     if(refresh) {
-        clearServices();
-    }
+        deleteServices();
 
-    if(m_servicesVector.empty()) {
         if (!retrieveServices()) {
             NIMBLE_LOGE(LOG_TAG, "Error: Failed to get services");
         }
@@ -442,30 +466,31 @@ bool NimBLEClient::retrieveServices(const NimBLEUUID *uuid_filter) {
  */
 
     NIMBLE_LOGD(LOG_TAG, ">> retrieveServices");
-    int rc = 0;
 
     if(!m_isConnected){
         NIMBLE_LOGE(LOG_TAG, "Disconnected, could not retrieve services -aborting");
         return false;
     }
 
-    m_semaphoreSearchCmplEvt.take("retrieveServices");
+    int rc = 0;
+    ble_task_data_t taskData = {this, xTaskGetCurrentTaskHandle(), 0, nullptr};
 
     if(uuid_filter == nullptr) {
-        rc = ble_gattc_disc_all_svcs(m_conn_id, NimBLEClient::serviceDiscoveredCB, this);
+        rc = ble_gattc_disc_all_svcs(m_conn_id, NimBLEClient::serviceDiscoveredCB, &taskData);
     } else {
         rc = ble_gattc_disc_svc_by_uuid(m_conn_id, &uuid_filter->getNative()->u,
-                                        NimBLEClient::serviceDiscoveredCB, this);
+                                        NimBLEClient::serviceDiscoveredCB, &taskData);
     }
 
     if (rc != 0) {
         NIMBLE_LOGE(LOG_TAG, "ble_gattc_disc_all_svcs: rc=%d %s", rc, NimBLEUtils::returnCodeToString(rc));
-        m_semaphoreSearchCmplEvt.give();
         return false;
     }
 
     // wait until we have all the services
-    if(m_semaphoreSearchCmplEvt.wait("retrieveServices") == 0){
+    ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
+
+    if(taskData.rc == 0){
         NIMBLE_LOGD(LOG_TAG, "<< retrieveServices");
         return true;
     }
@@ -489,41 +514,34 @@ int NimBLEClient::serviceDiscoveredCB(
     NIMBLE_LOGD(LOG_TAG,"Service Discovered >> status: %d handle: %d",
                         error->status, (error->status == 0) ? service->start_handle : -1);
 
-    NimBLEClient *peer = (NimBLEClient*)arg;
-    int rc=0;
+    ble_task_data_t *pTaskData = (ble_task_data_t*)arg;
+    NimBLEClient *client = (NimBLEClient*)pTaskData->pATT;
 
     // Make sure the service discovery is for this device
-    if(peer->getConnId() != conn_handle){
+    if(client->getConnId() != conn_handle){
         return 0;
     }
 
-    switch (error->status) {
-        case 0: {
-            // Found a service - add it to the vector
-            NimBLERemoteService* pRemoteService = new NimBLERemoteService(peer, service);
-            peer->m_servicesVector.push_back(pRemoteService);
-            break;
-        }
-        case BLE_HS_EDONE:{
-            // All services discovered; start discovering characteristics.
-
-            //NIMBLE_LOGD(LOG_TAG,"Giving search semaphore - completed");
-            peer->m_semaphoreSearchCmplEvt.give(0);
-            rc = 0;
-            break;
-        }
-        default:
-            // Error; abort discovery.
-            rc = error->status;
-            break;
+    if(error->status == 0) {
+        // Found a service - add it to the vector
+        NimBLERemoteService* pRemoteService = new NimBLERemoteService(client, service);
+        client->m_servicesVector.push_back(pRemoteService);
+        return 0;
     }
 
-    if (rc != 0) {
-        // pass non-zero to semaphore on error to indicate an error finding services
-        peer->m_semaphoreSearchCmplEvt.give(1);
+    if(error->status == BLE_HS_EDONE) {
+        pTaskData->rc = 0;
+    } else {
+        NIMBLE_LOGE(LOG_TAG, "characteristicDiscCB() rc=%d %s",
+                             error->status,
+                             NimBLEUtils::returnCodeToString(error->status));
+        pTaskData->rc = error->status;
     }
-    NIMBLE_LOGD(LOG_TAG,"<< Service Discovered. status: %d", rc);
-    return rc;
+
+    xTaskNotifyGive(pTaskData->task);
+
+    NIMBLE_LOGD(LOG_TAG,"<< << Service Discovered");
+    return error->status;
 }
 
 
@@ -595,15 +613,11 @@ uint16_t NimBLEClient::getMTU() {
  * @param [in] arg = pointer to the client instance
  */
  /*STATIC*/ int NimBLEClient::handleGapEvent(struct ble_gap_event *event, void *arg) {
-
     NimBLEClient* client = (NimBLEClient*)arg;
-    //struct ble_gap_conn_desc desc;
-    //struct ble_hs_adv_fields fields;
     int rc;
 
     NIMBLE_LOGD(LOG_TAG, "Got Client event %s", NimBLEUtils::gapEventToString(event->type));
 
-    // Execute handler code based on the type of event received.
     switch(event->type) {
 
         case BLE_GAP_EVENT_DISCONNECT: {
@@ -620,9 +634,6 @@ uint16_t NimBLEClient::getMTU() {
 
             NIMBLE_LOGI(LOG_TAG, "disconnect; reason=%d, %s", event->disconnect.reason,
                                     NimBLEUtils::returnCodeToString(event->disconnect.reason));
-            //print_conn_desc(&event->disconnect.conn);
-            //MODLOG_DFLT(INFO, "\n");
-
 
             // If Host reset tell the device now before returning to prevent
             // any errors caused by calling host functions before resyncing.
@@ -639,15 +650,9 @@ uint16_t NimBLEClient::getMTU() {
             }
 
             //client->m_conn_id = BLE_HS_CONN_HANDLE_NONE;
-
-            // Indicate a non-success return value to any semaphores waiting
-            client->m_semaphoreOpenEvt.give(1);
-            client->m_semaphoreSearchCmplEvt.give(1);
-            client->m_semeaphoreSecEvt.give(1);
-
             client->m_pClientCallbacks->onDisconnect(client);
-
-            return 0;
+            rc = event->disconnect.reason;
+            break;
         } // BLE_GAP_EVENT_DISCONNECT
 
         case BLE_GAP_EVENT_CONNECT: {
@@ -667,30 +672,25 @@ uint16_t NimBLEClient::getMTU() {
 
                 client->m_conn_id = event->connect.conn_handle;
 
-            //  rc = ble_gap_conn_find(event->connect.conn_handle, &desc);
-            //  assert(rc == 0);
-            //  print_conn_desc(&desc);
-            //  MODLOG_DFLT(INFO, "\n");
-
-
-                // In the case of a multiconnecting device we ignore this device when
-                // scanning since we are already connected to it
-                NimBLEDevice::addIgnored(client->m_peerAddress);
-
                 rc = ble_gattc_exchange_mtu(client->m_conn_id, NULL,NULL);
                 if(rc != 0) {
                     NIMBLE_LOGE(LOG_TAG, "ble_gattc_exchange_mtu: rc=%d %s",rc,
                                             NimBLEUtils::returnCodeToString(rc));
-                    // if error getting mtu indicate a connection error.
-                    client->m_semaphoreOpenEvt.give(rc);
+                    break;
                 }
+
+                // In the case of a multiconnecting device we ignore this device when
+                // scanning since we are already connected to it
+                NimBLEDevice::addIgnored(client->m_peerAddress);
             } else {
-                // Connection attempt failed
                 NIMBLE_LOGE(LOG_TAG, "Error: Connection failed; status=%d %s",
                             event->connect.status,
                             NimBLEUtils::returnCodeToString(event->connect.status));
+
+                client->m_isConnected = false;
+                rc = event->connect.status;
+                break;
             }
-            client->m_semaphoreOpenEvt.give(event->connect.status);
             return 0;
         } // BLE_GAP_EVENT_CONNECT
 
@@ -701,7 +701,7 @@ uint16_t NimBLEClient::getMTU() {
             NIMBLE_LOGD(LOG_TAG, "Notify Recieved for handle: %d",event->notify_rx.attr_handle);
 
             for(auto &it: client->m_servicesVector) {
-                // Dont waste cycles searching services without this handle in their range
+                // Dont waste cycles searching services without this handle in its range
                 if(it->getEndHandle() < event->notify_rx.attr_handle) {
                     continue;
                 }
@@ -720,6 +720,11 @@ uint16_t NimBLEClient::getMTU() {
                 if(characteristic != cVector->cend()) {
                     NIMBLE_LOGD(LOG_TAG, "Got Notification for characteristic %s", (*characteristic)->toString().c_str());
 
+                    portENTER_CRITICAL(&(*characteristic)->m_valMux);
+                    (*characteristic)->m_value = std::string((char *)event->notify_rx.om->om_data, event->notify_rx.om->om_len);
+                    (*characteristic)->m_timestamp = time(nullptr);
+                    portEXIT_CRITICAL(&(*characteristic)->m_valMux);
+
                     if ((*characteristic)->m_notifyCallback != nullptr) {
                         NIMBLE_LOGD(LOG_TAG, "Invoking callback for notification on characteristic %s",
                                              (*characteristic)->toString().c_str());
@@ -727,7 +732,6 @@ uint16_t NimBLEClient::getMTU() {
                                                             event->notify_rx.om->om_len,
                                                             !event->notify_rx.indication);
                     }
-
                     break;
                 }
             }
@@ -738,7 +742,7 @@ uint16_t NimBLEClient::getMTU() {
         case BLE_GAP_EVENT_CONN_UPDATE_REQ:
         case BLE_GAP_EVENT_L2CAP_UPDATE_REQ: {
             if(client->m_conn_id != event->conn_update_req.conn_handle){
-                return 0; //BLE_HS_ENOTCONN BLE_ATT_ERR_INVALID_HANDLE
+                return 0;
             }
             NIMBLE_LOGD(LOG_TAG, "Peer requesting to update connection parameters");
             NIMBLE_LOGD(LOG_TAG, "MinInterval: %d, MaxInterval: %d, Latency: %d, Timeout: %d",
@@ -764,7 +768,7 @@ uint16_t NimBLEClient::getMTU() {
 
         case BLE_GAP_EVENT_CONN_UPDATE: {
             if(client->m_conn_id != event->conn_update.conn_handle){
-                return 0; //BLE_HS_ENOTCONN BLE_ATT_ERR_INVALID_HANDLE
+                return 0;
             }
             if(event->conn_update.status == 0) {
                 NIMBLE_LOGI(LOG_TAG, "Connection parameters updated.");
@@ -776,7 +780,7 @@ uint16_t NimBLEClient::getMTU() {
 
         case BLE_GAP_EVENT_ENC_CHANGE: {
             if(client->m_conn_id != event->enc_change.conn_handle){
-                return 0; //BLE_HS_ENOTCONN BLE_ATT_ERR_INVALID_HANDLE
+                return 0;
             }
 
             if(event->enc_change.status == 0) {
@@ -791,23 +795,23 @@ uint16_t NimBLEClient::getMTU() {
                 }
             }
 
-            client->m_semeaphoreSecEvt.give(event->enc_change.status);
-            return 0;
+            rc = event->enc_change.status;
+            break;
         } //BLE_GAP_EVENT_ENC_CHANGE
 
         case BLE_GAP_EVENT_MTU: {
             if(client->m_conn_id != event->mtu.conn_handle){
-                return 0; //BLE_HS_ENOTCONN BLE_ATT_ERR_INVALID_HANDLE
+                return 0;
             }
             NIMBLE_LOGI(LOG_TAG, "mtu update event; conn_handle=%d mtu=%d",
                         event->mtu.conn_handle,
                         event->mtu.value);
-            client->m_semaphoreOpenEvt.give(0);
-            return 0;
+            rc = 0;
+            break;
         } // BLE_GAP_EVENT_MTU
 
         case BLE_GAP_EVENT_PASSKEY_ACTION: {
-            struct ble_sm_io pkey = {0};
+            struct ble_sm_io pkey = {0,0};
 
             if(client->m_conn_id != event->passkey.conn_handle)
                 return 0;
@@ -868,6 +872,14 @@ uint16_t NimBLEClient::getMTU() {
             return 0;
         }
     } // Switch
+
+    if(client->m_pTaskData != nullptr) {
+        client->m_pTaskData->rc = rc;
+        xTaskNotifyGive(client->m_pTaskData->task);
+        client->m_pTaskData = nullptr;
+    }
+
+    return 0;
 } // handleGapEvent
 
 
diff --git a/libesp32/NimBLE-Arduino/src/NimBLEClient.h b/libesp32/NimBLE-Arduino/src/NimBLEClient.h
index 4bf2c33d2..888b42d02 100644
--- a/libesp32/NimBLE-Arduino/src/NimBLEClient.h
+++ b/libesp32/NimBLE-Arduino/src/NimBLEClient.h
@@ -21,17 +21,14 @@
 #if defined(CONFIG_BT_NIMBLE_ROLE_CENTRAL)
 
 #include "NimBLEAddress.h"
+#include "NimBLEUUID.h"
+#include "NimBLEUtils.h"
 #include "NimBLEAdvertisedDevice.h"
 #include "NimBLERemoteService.h"
 
 #include <vector>
 #include <string>
 
-typedef struct {
-    const NimBLEUUID *uuid;
-    const void *attribute;
-} disc_filter_t;
-
 class NimBLERemoteService;
 class NimBLEClientCallbacks;
 class NimBLEAdvertisedDevice;
@@ -52,6 +49,8 @@ public:
     std::vector<NimBLERemoteService*>::iterator end();
     NimBLERemoteService*                        getService(const char* uuid);
     NimBLERemoteService*                        getService(const NimBLEUUID &uuid);
+    void                                        deleteServices();
+    size_t                                      deleteService(const NimBLEUUID &uuid);
     std::string                                 getValue(const NimBLEUUID &serviceUUID, const NimBLEUUID &characteristicUUID);
     bool                                        setValue(const NimBLEUUID &serviceUUID, const NimBLEUUID &characteristicUUID,
                                                          const std::string &value);
@@ -82,19 +81,16 @@ private:
                                                 const struct ble_gatt_error *error,
                                                 const struct ble_gatt_svc *service,
                                                 void *arg);
-    void                    clearServices();
     bool                    retrieveServices(const NimBLEUUID *uuid_filter = nullptr);
 
     NimBLEAddress           m_peerAddress = NimBLEAddress("");
     uint16_t                m_conn_id;
-    bool                    m_isConnected = false;
-    bool                    m_waitingToConnect =false;
-    bool                    m_deleteCallbacks = true;
+    bool                    m_isConnected;
+    bool                    m_waitingToConnect;
+    bool                    m_deleteCallbacks;
     int32_t                 m_connectTimeout;
-    NimBLEClientCallbacks*  m_pClientCallbacks = nullptr;
-    FreeRTOS::Semaphore     m_semaphoreOpenEvt       = FreeRTOS::Semaphore("OpenEvt");
-    FreeRTOS::Semaphore     m_semaphoreSearchCmplEvt = FreeRTOS::Semaphore("SearchCmplEvt");
-    FreeRTOS::Semaphore     m_semeaphoreSecEvt       = FreeRTOS::Semaphore("Security");
+    NimBLEClientCallbacks*  m_pClientCallbacks;
+    ble_task_data_t         *m_pTaskData;
 
     std::vector<NimBLERemoteService*> m_servicesVector;
 
diff --git a/libesp32/NimBLE-Arduino/src/NimBLEDescriptor.cpp b/libesp32/NimBLE-Arduino/src/NimBLEDescriptor.cpp
index 0d414d316..e8f7f9f8b 100644
--- a/libesp32/NimBLE-Arduino/src/NimBLEDescriptor.cpp
+++ b/libesp32/NimBLE-Arduino/src/NimBLEDescriptor.cpp
@@ -50,6 +50,7 @@ NimBLEDescriptor::NimBLEDescriptor(NimBLEUUID uuid, uint16_t properties, uint16_
     m_pCharacteristic    = nullptr;                     // No initial characteristic.
     m_pCallbacks         = &defaultCallbacks;           // No initial callback.
     m_value.attr_value   = (uint8_t*) calloc(max_len,1);  // Allocate storage for the value.
+    m_valMux             = portMUX_INITIALIZER_UNLOCKED;
     m_properties         = 0;
 
     if (properties & BLE_GATT_CHR_F_READ) {             // convert uint16_t properties to uint8_t
@@ -137,17 +138,37 @@ int NimBLEDescriptor::handleGapEvent(uint16_t conn_handle, uint16_t attr_handle,
     if(ble_uuid_cmp(uuid, &pDescriptor->getUUID().getNative()->u) == 0){
         switch(ctxt->op) {
             case BLE_GATT_ACCESS_OP_READ_DSC: {
-                pDescriptor->m_pCallbacks->onRead(pDescriptor);
+                // If the packet header is only 8 bytes this is a follow up of a long read
+                // so we don't want to call the onRead() callback again.
+                if(ctxt->om->om_pkthdr_len > 8) {
+                    pDescriptor->m_pCallbacks->onRead(pDescriptor);
+                }
+                portENTER_CRITICAL(&pDescriptor->m_valMux);
                 rc = os_mbuf_append(ctxt->om, pDescriptor->getValue(), pDescriptor->getLength());
+                portEXIT_CRITICAL(&pDescriptor->m_valMux);
                 return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
             }
 
             case BLE_GATT_ACCESS_OP_WRITE_DSC: {
-                if (ctxt->om->om_len > BLE_ATT_ATTR_MAX_LEN) {
+                if (ctxt->om->om_len > pDescriptor->m_value.attr_max_len) {
                     return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
                 }
 
-                pDescriptor->setValue(ctxt->om->om_data, ctxt->om->om_len);
+                uint8_t buf[pDescriptor->m_value.attr_max_len];
+                size_t len = ctxt->om->om_len;
+                memcpy(buf, ctxt->om->om_data,len);
+                os_mbuf *next;
+                next = SLIST_NEXT(ctxt->om, om_next);
+                while(next != NULL){
+                    if((len + next->om_len) > pDescriptor->m_value.attr_max_len) {
+                        return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
+                    }
+                    memcpy(&buf[len-1], next->om_data, next->om_len);
+                    len += next->om_len;
+                    next = SLIST_NEXT(next, om_next);
+                }
+
+                pDescriptor->setValue(buf, len);
                 pDescriptor->m_pCallbacks->onWrite(pDescriptor);
                 return 0;
             }
@@ -191,12 +212,14 @@ void NimBLEDescriptor::setHandle(uint16_t handle) {
  * @param [in] length The length of the data in bytes.
  */
 void NimBLEDescriptor::setValue(const uint8_t* data, size_t length) {
-    if (length > BLE_ATT_ATTR_MAX_LEN) {
-        NIMBLE_LOGE(LOG_TAG, "Size %d too large, must be no bigger than %d", length, BLE_ATT_ATTR_MAX_LEN);
+    if (length > m_value.attr_max_len) {
+        NIMBLE_LOGE(LOG_TAG, "Size %d too large, must be no bigger than %d", length, m_value.attr_max_len);
         return;
     }
+    portENTER_CRITICAL(&m_valMux);
     m_value.attr_len = length;
     memcpy(m_value.attr_value, data, length);
+    portEXIT_CRITICAL(&m_valMux);
 } // setValue
 
 
@@ -209,13 +232,6 @@ void NimBLEDescriptor::setValue(const std::string &value) {
 } // setValue
 
 
-/*
-void NimBLEDescriptor::setAccessPermissions(uint8_t perm) {
-    m_permissions = perm;
-}
-*/
-
-
 /**
  * @brief Return a string representation of the descriptor.
  * @return A string representation of the descriptor.
diff --git a/libesp32/NimBLE-Arduino/src/NimBLEDescriptor.h b/libesp32/NimBLE-Arduino/src/NimBLEDescriptor.h
index 8af3560d3..f4978f570 100644
--- a/libesp32/NimBLE-Arduino/src/NimBLEDescriptor.h
+++ b/libesp32/NimBLE-Arduino/src/NimBLEDescriptor.h
@@ -22,20 +22,17 @@
 
 #include "NimBLECharacteristic.h"
 #include "NimBLEUUID.h"
-#include "FreeRTOS.h"
 
 #include <string>
 
 
 typedef struct
 {
-    uint16_t attr_max_len;                                  /*!<  attribute max value length */
-    uint16_t attr_len;                                      /*!<  attribute current value length */
-    uint8_t  *attr_value;                                   /*!<  the pointer to attribute value */
+    uint16_t attr_max_len;  /*!<  attribute max value length */
+    uint16_t attr_len;      /*!<  attribute current value length */
+    uint8_t  *attr_value;    /*!<  the pointer to attribute value */
 } attr_value_t;
 
-typedef attr_value_t esp_attr_value_t; /*!< compatibility for esp32 */
-
 class NimBLEService;
 class NimBLECharacteristic;
 class NimBLEDescriptorCallbacks;
@@ -46,32 +43,39 @@ class NimBLEDescriptorCallbacks;
  */
 class NimBLEDescriptor {
 public:
-    virtual ~NimBLEDescriptor();
-    uint16_t getHandle();                                   // Get the handle of the descriptor.
-    size_t   getLength();                                   // Get the length of the value of the descriptor.
-    NimBLEUUID  getUUID();                                     // Get the UUID of the descriptor.
-    uint8_t* getValue();                                    // Get a pointer to the value of the descriptor.
-//  void setAccessPermissions(uint8_t perm);          // Set the permissions of the descriptor.
-    void setCallbacks(NimBLEDescriptorCallbacks* pCallbacks);  // Set callbacks to be invoked for the descriptor.
-    void setValue(const uint8_t* data, size_t size);              // Set the value of the descriptor as a pointer to data.
-    void setValue(const std::string &value);                       // Set the value of the descriptor as a data buffer.
+    uint16_t     getHandle();
+    size_t       getLength();
+    NimBLEUUID   getUUID();
+    uint8_t*     getValue();
+    void         setCallbacks(NimBLEDescriptorCallbacks* pCallbacks);
+    void         setValue(const uint8_t* data, size_t size);
+    void         setValue(const std::string &value);
+    std::string  toString();
 
-    std::string toString();                                 // Convert the descriptor to a string representation.
+    template<typename T>
+    void setValue(const T &s) {
+        setValue((uint8_t*)&s, sizeof(T));
+    }
 
 private:
-    friend class NimBLEDescriptorMap;
     friend class NimBLECharacteristic;
     friend class NimBLEService;
     friend class NimBLE2902;
     friend class NimBLE2904;
 
     NimBLEDescriptor(const char* uuid, uint16_t properties,
-                            uint16_t max_len,
-                            NimBLECharacteristic* pCharacteristic);
+                     uint16_t max_len,
+                     NimBLECharacteristic* pCharacteristic);
 
     NimBLEDescriptor(NimBLEUUID uuid, uint16_t properties,
-                            uint16_t max_len,
-                            NimBLECharacteristic* pCharacteristic);
+                     uint16_t max_len,
+                     NimBLECharacteristic* pCharacteristic);
+
+    ~NimBLEDescriptor();
+
+    static int handleGapEvent(uint16_t conn_handle, uint16_t attr_handle,
+                              struct ble_gatt_access_ctxt *ctxt, void *arg);
+    void       setHandle(uint16_t handle);
 
     NimBLEUUID                 m_uuid;
     uint16_t                   m_handle;
@@ -79,12 +83,8 @@ private:
     NimBLECharacteristic*      m_pCharacteristic;
     uint8_t                    m_properties;
     attr_value_t               m_value;
-
-    static int handleGapEvent(uint16_t conn_handle, uint16_t attr_handle,
-                           struct ble_gatt_access_ctxt *ctxt, void *arg);
-
-    void setHandle(uint16_t handle);
-}; // BLEDescriptor
+    portMUX_TYPE               m_valMux;
+}; // NimBLEDescriptor
 
 
 /**
diff --git a/libesp32/NimBLE-Arduino/src/NimBLEDescriptorMap.cpp b/libesp32/NimBLE-Arduino/src/NimBLEDescriptorMap.cpp
deleted file mode 100644
index 0ca1cbeeb..000000000
--- a/libesp32/NimBLE-Arduino/src/NimBLEDescriptorMap.cpp
+++ /dev/null
@@ -1,149 +0,0 @@
-/*
- * NimBLEDescriptorMap.cpp
- *
- *  Created: on March 10, 2020
- *      Author H2zero
- *
- * Originally:
- *
- * BLEDescriptorMap.cpp
- *
- *  Created on: Jun 22, 2017
- *      Author: kolban
- */
-#include "sdkconfig.h"
-#if defined(CONFIG_BT_ENABLED)
-
-#include "nimconfig.h"
-#if defined(CONFIG_BT_NIMBLE_ROLE_PERIPHERAL)
-
-#include "NimBLECharacteristic.h"
-#include "NimBLEDescriptor.h"
-
-
-/**
- * @brief Return the descriptor by UUID.
- * @param [in] UUID The UUID to look up the descriptor.
- * @return The descriptor.  If not present, then nullptr is returned.
- */
-NimBLEDescriptor* NimBLEDescriptorMap::getByUUID(const char* uuid) {
-    return getByUUID(NimBLEUUID(uuid));
-}
-
-
-/**
- * @brief Return the descriptor by UUID.
- * @param [in] UUID The UUID to look up the descriptor.
- * @return The descriptor.  If not present, then nullptr is returned.
- */
-NimBLEDescriptor* NimBLEDescriptorMap::getByUUID(const NimBLEUUID &uuid) {
-    for (auto &myPair : m_uuidMap) {
-        if (myPair.first->getUUID().equals(uuid)) {
-            return myPair.first;
-        }
-    }
-    return nullptr;
-} // getByUUID
-
-
-/**
- * @brief Return the descriptor by handle.
- * @param [in] handle The handle to look up the descriptor.
- * @return The descriptor.
- */
- /*
-NimBLEDescriptor* NimBLEDescriptorMap::getByHandle(uint16_t handle) {
-    return m_handleMap.at(handle);
-} // getByHandle
-*/
-
-/**
- * @brief Set the descriptor by UUID.
- * @param [in] uuid The uuid of the descriptor.
- * @param [in] characteristic The descriptor to cache.
- * @return N/A.
- */
-void NimBLEDescriptorMap::setByUUID(const char* uuid, NimBLEDescriptor* pDescriptor){
-    m_uuidMap.insert(std::pair<NimBLEDescriptor*, std::string>(pDescriptor, uuid));
-} // setByUUID
-
-
-
-/**
- * @brief Set the descriptor by UUID.
- * @param [in] uuid The uuid of the descriptor.
- * @param [in] characteristic The descriptor to cache.
- * @return N/A.
- */
-void NimBLEDescriptorMap::setByUUID(const NimBLEUUID &uuid, NimBLEDescriptor* pDescriptor) {
-    m_uuidMap.insert(std::pair<NimBLEDescriptor*, std::string>(pDescriptor, uuid.toString()));
-} // setByUUID
-
-
-/**
- * @brief Set the descriptor by handle.
- * @param [in] handle The handle of the descriptor.
- * @param [in] descriptor The descriptor to cache.
- * @return N/A.
- */
- /*
-void NimBLEDescriptorMap::setByHandle(uint16_t handle, NimBLEDescriptor* pDescriptor) {
-    m_handleMap.insert(std::pair<uint16_t, NimBLEDescriptor*>(handle, pDescriptor));
-} // setByHandle
-*/
-
-
-/**
- * @brief Get the number of descriptors in the map.
- */
-uint8_t NimBLEDescriptorMap::getSize() {
-    return (uint8_t)m_uuidMap.size();
-} // getSize
-
-
-/**
- * @brief Return a string representation of the descriptor map.
- * @return A string representation of the descriptor map.
- */
-std::string NimBLEDescriptorMap::toString() {
-    std::string res;
-    char hex[5];
-    int count = 0;
-    for (auto &myPair : m_uuidMap) {
-        if (count > 0) {res += "\n";}
-        snprintf(hex, sizeof(hex), "%04x", myPair.first->getHandle());
-        count++;
-        res += "handle: 0x";
-        res += hex;
-        res += ", uuid: " + myPair.first->getUUID().toString();
-    }
-    return res;
-} // toString
-
-
-/**
- * @brief Get the first descriptor in the map.
- * @return The first descriptor in the map.
- */
-NimBLEDescriptor* NimBLEDescriptorMap::getFirst() {
-    m_iterator = m_uuidMap.begin();
-    if (m_iterator == m_uuidMap.end()) return nullptr;
-    NimBLEDescriptor* pRet = m_iterator->first;
-    m_iterator++;
-    return pRet;
-} // getFirst
-
-
-/**
- * @brief Get the next descriptor in the map.
- * @return The next descriptor in the map.
- */
-NimBLEDescriptor* NimBLEDescriptorMap::getNext() {
-    if (m_iterator == m_uuidMap.end()) return nullptr;
-    NimBLEDescriptor* pRet = m_iterator->first;
-    m_iterator++;
-    return pRet;
-} // getNext
-
-#endif // #if defined(CONFIG_BT_NIMBLE_ROLE_PERIPHERAL)
-#endif /* CONFIG_BT_ENABLED */
diff --git a/libesp32/NimBLE-Arduino/src/NimBLEDevice.cpp b/libesp32/NimBLE-Arduino/src/NimBLEDevice.cpp
index 1695a5177..b3d882ba3 100644
--- a/libesp32/NimBLE-Arduino/src/NimBLEDevice.cpp
+++ b/libesp32/NimBLE-Arduino/src/NimBLEDevice.cpp
@@ -152,21 +152,26 @@ void NimBLEDevice::stopAdvertising() {
         return false;
     }
 
+    int rc =0;
+
     if(pClient->m_isConnected) {
-        if (pClient->disconnect() != 0) {
+        rc = pClient->disconnect();
+        if (rc != 0 && rc != BLE_HS_EALREADY && rc != BLE_HS_ENOTCONN) {
             return false;
         }
+
         while(pClient->m_isConnected) {
-            vTaskDelay(1);
+            vTaskDelay(10);
         }
     }
 
     if(pClient->m_waitingToConnect) {
-        if(ble_gap_conn_cancel() != 0){
+        rc = ble_gap_conn_cancel();
+        if (rc != 0 && rc != BLE_HS_EALREADY) {
             return false;
         }
         while(pClient->m_waitingToConnect) {
-            vTaskDelay(1);
+            vTaskDelay(10);
         }
     }
 
diff --git a/libesp32/NimBLE-Arduino/src/NimBLEDevice.h b/libesp32/NimBLE-Arduino/src/NimBLEDevice.h
index c4a6d0417..412647987 100644
--- a/libesp32/NimBLE-Arduino/src/NimBLEDevice.h
+++ b/libesp32/NimBLE-Arduino/src/NimBLEDevice.h
@@ -116,6 +116,7 @@ public:
     static void             setSecurityPasskey(uint32_t pin);
     static uint32_t         getSecurityPasskey();
     static void             setSecurityCallbacks(NimBLESecurityCallbacks* pCallbacks);
+    static int              startSecurity(uint16_t conn_id);
     static int              setMTU(uint16_t mtu);
     static uint16_t         getMTU();
     static bool             isIgnored(const NimBLEAddress &address);
@@ -159,7 +160,6 @@ private:
     static void        onReset(int reason);
     static void        onSync(void);
     static void        host_task(void *param);
-    static int         startSecurity(uint16_t conn_id);
     static bool        m_synced;
 
 #if defined(CONFIG_BT_NIMBLE_ROLE_OBSERVER)
diff --git a/libesp32/NimBLE-Arduino/src/NimBLELog.h b/libesp32/NimBLE-Arduino/src/NimBLELog.h
index b519c0faa..6e9ed97f3 100644
--- a/libesp32/NimBLE-Arduino/src/NimBLELog.h
+++ b/libesp32/NimBLE-Arduino/src/NimBLELog.h
@@ -25,25 +25,25 @@
 #if CORE_DEBUG_LEVEL >= 4
 #define NIMBLE_LOGD( tag, format, ... ) MODLOG_DFLT(ERROR,      "D %s: "#format"\n",tag,##__VA_ARGS__)
 #else
-#define NIMBLE_LOGD( tag, format, ... )
+#define NIMBLE_LOGD( tag, format, ... ) (void)tag
 #endif
 
 #if CORE_DEBUG_LEVEL >= 3
 #define NIMBLE_LOGI( tag, format, ... ) MODLOG_DFLT(ERROR,      "I %s: "#format"\n",tag,##__VA_ARGS__)
 #else
-#define NIMBLE_LOGI( tag, format, ... )
+#define NIMBLE_LOGI( tag, format, ... ) (void)tag
 #endif
 
 #if CORE_DEBUG_LEVEL >= 2
 #define NIMBLE_LOGW( tag, format, ... ) MODLOG_DFLT(ERROR,      "W %s: "#format"\n",tag,##__VA_ARGS__)
 #else
-#define NIMBLE_LOGW( tag, format, ... )
+#define NIMBLE_LOGW( tag, format, ... ) (void)tag
 #endif
 
 #if CORE_DEBUG_LEVEL >= 1
 #define NIMBLE_LOGE( tag, format, ... ) MODLOG_DFLT(ERROR,      "E %s: "#format"\n",tag,##__VA_ARGS__)
 #else
-#define NIMBLE_LOGE( tag, format, ... )
+#define NIMBLE_LOGE( tag, format, ... ) (void)tag
 #endif
 
 #define NIMBLE_LOGC( tag, format, ... ) MODLOG_DFLT(CRITICAL,   "CRIT %s: "#format"\n",tag,##__VA_ARGS__)
diff --git a/libesp32/NimBLE-Arduino/src/NimBLERemoteCharacteristic.cpp b/libesp32/NimBLE-Arduino/src/NimBLERemoteCharacteristic.cpp
index 85760b433..23089ca77 100644
--- a/libesp32/NimBLE-Arduino/src/NimBLERemoteCharacteristic.cpp
+++ b/libesp32/NimBLE-Arduino/src/NimBLERemoteCharacteristic.cpp
@@ -53,13 +53,14 @@ static const char* LOG_TAG = "NimBLERemoteCharacteristic";
             m_uuid = nullptr;
             break;
     }
+
     m_handle             = chr->val_handle;
     m_defHandle          = chr->def_handle;
     m_charProp           = chr->properties;
     m_pRemoteService     = pRemoteService;
     m_notifyCallback     = nullptr;
-    m_rawData            = nullptr;
-    m_dataLen            = 0;
+    m_timestamp          = 0;
+    m_valMux             = portMUX_INITIALIZER_UNLOCKED;
  } // NimBLERemoteCharacteristic
 
 
@@ -67,10 +68,7 @@ static const char* LOG_TAG = "NimBLERemoteCharacteristic";
  *@brief Destructor.
  */
 NimBLERemoteCharacteristic::~NimBLERemoteCharacteristic() {
-    removeDescriptors();   // Release resources for any descriptor information we may have allocated.
-    if(m_rawData != nullptr) {
-        free(m_rawData);
-    }
+    deleteDescriptors();
 } // ~NimBLERemoteCharacteristic
 
 /*
@@ -150,12 +148,12 @@ int NimBLERemoteCharacteristic::descriptorDiscCB(uint16_t conn_handle,
     NIMBLE_LOGD(LOG_TAG,"Descriptor Discovered >> status: %d handle: %d",
                         error->status, (error->status == 0) ? dsc->handle : -1);
 
-    disc_filter_t *filter = (disc_filter_t*)arg;
-    NimBLEUUID *uuid_filter = (NimBLEUUID*)filter->uuid;
-    NimBLERemoteCharacteristic *characteristic = (NimBLERemoteCharacteristic*)filter->attribute;
+    desc_filter_t *filter = (desc_filter_t*)arg;
+    const NimBLEUUID *uuid_filter = filter->uuid;
+    ble_task_data_t *pTaskData = (ble_task_data_t*)filter->task_data;
+    NimBLERemoteCharacteristic *characteristic = (NimBLERemoteCharacteristic*)pTaskData->pATT;
     int rc=0;
 
-    // Make sure the discovery is for this device
     if(characteristic->getRemoteService()->getClient()->getConnId() != conn_handle){
         return 0;
     }
@@ -175,7 +173,7 @@ int NimBLERemoteCharacteristic::descriptorDiscCB(uint16_t conn_handle,
                     rc = BLE_HS_EDONE;
                 }
             }
-            // Found a descriptor - add it to the vector
+
             NimBLERemoteDescriptor* pNewRemoteDescriptor = new NimBLERemoteDescriptor(characteristic, dsc);
             characteristic->m_descriptorVector.push_back(pNewRemoteDescriptor);
             break;
@@ -185,18 +183,20 @@ int NimBLERemoteCharacteristic::descriptorDiscCB(uint16_t conn_handle,
             break;
     }
 
-    /** If rc == BLE_HS_EDONE, release the semaphore with a success error code and stop the discovery process.
+    /** If rc == BLE_HS_EDONE, resume the task with a success error code and stop the discovery process.
      *  Else if rc == 0, just return 0 to continue the discovery until we get BLE_HS_EDONE.
-     *  If we get any other error code tell the application to abort by returning non-zero in the semaphore rc.
+     *  If we get any other error code tell the application to abort by returning non-zero in the rc.
      */
     if (rc == BLE_HS_EDONE) {
-        characteristic->m_semaphoreGetDescEvt.give(0);
+        pTaskData->rc = 0;
+        xTaskNotifyGive(pTaskData->task);
     } else if(rc != 0) {
-        /* Error; abort discovery. */
-        // pass error code to semaphore waiting
-        characteristic->m_semaphoreGetDescEvt.give(rc);
+        // Error; abort discovery.
+        pTaskData->rc = rc;
+        xTaskNotifyGive(pTaskData->task);
     }
-    NIMBLE_LOGD(LOG_TAG,"<< Descriptor Discovered. status: %d", rc);
+
+    NIMBLE_LOGD(LOG_TAG,"<< Descriptor Discovered. status: %d", pTaskData->rc);
     return rc;
 }
 
@@ -209,11 +209,8 @@ bool NimBLERemoteCharacteristic::retrieveDescriptors(const NimBLEUUID *uuid_filt
     NIMBLE_LOGD(LOG_TAG, ">> retrieveDescriptors() for characteristic: %s", getUUID().toString().c_str());
 
     int rc = 0;
-    disc_filter_t filter;
-    filter.uuid = uuid_filter;
-    filter.attribute = this;
-
-    m_semaphoreGetDescEvt.take("retrieveDescriptors");
+    ble_task_data_t taskData = {this, xTaskGetCurrentTaskHandle(), 0, nullptr};
+    desc_filter_t filter = {uuid_filter, &taskData};
 
     rc = ble_gattc_disc_all_dscs(getRemoteService()->getClient()->getConnId(),
                                  m_handle,
@@ -222,11 +219,12 @@ bool NimBLERemoteCharacteristic::retrieveDescriptors(const NimBLEUUID *uuid_filt
                                  &filter);
     if (rc != 0) {
         NIMBLE_LOGE(LOG_TAG, "ble_gattc_disc_all_chrs: rc=%d %s", rc, NimBLEUtils::returnCodeToString(rc));
-        m_semaphoreGetDescEvt.give();
         return false;
     }
 
-    if(m_semaphoreGetDescEvt.wait("retrieveDescriptors") != 0) {
+    ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
+
+    if(taskData.rc != 0) {
         return false;
     }
 
@@ -272,10 +270,8 @@ NimBLERemoteDescriptor* NimBLERemoteCharacteristic::getDescriptor(const NimBLEUU
  */
 std::vector<NimBLERemoteDescriptor*>* NimBLERemoteCharacteristic::getDescriptors(bool refresh) {
     if(refresh) {
-        removeDescriptors();
-    }
+        deleteDescriptors();
 
-    if(m_descriptorVector.empty()) {
         if (!retrieveDescriptors()) {
             NIMBLE_LOGE(LOG_TAG, "Error: Failed to get descriptors");
         }
@@ -340,16 +336,28 @@ NimBLEUUID NimBLERemoteCharacteristic::getUUID() {
 } // getUUID
 
 
+/**
+ * @brief Get the value of the remote characteristic.
+ * @return The value of the remote characteristic.
+ */
+std::string NimBLERemoteCharacteristic::getValue(time_t *timestamp) {
+    portENTER_CRITICAL(&m_valMux);
+    std::string value = m_value;
+    if(timestamp != nullptr) {
+        *timestamp = m_timestamp;
+    }
+    portEXIT_CRITICAL(&m_valMux);
+
+    return value;
+}
+
+
 /**
  * @brief Read an unsigned 16 bit value
  * @return The unsigned 16 bit value.
  */
 uint16_t NimBLERemoteCharacteristic::readUInt16() {
-    std::string value = readValue();
-    if (value.length() >= 2) {
-        return *(uint16_t*)(value.data());
-    }
-    return 0;
+    return readValue<uint16_t>();
 } // readUInt16
 
 
@@ -358,11 +366,7 @@ uint16_t NimBLERemoteCharacteristic::readUInt16() {
  * @return the unsigned 32 bit value.
  */
 uint32_t NimBLERemoteCharacteristic::readUInt32() {
-    std::string value = readValue();
-    if (value.length() >= 4) {
-        return *(uint32_t*)(value.data());
-    }
-    return 0;
+    return readValue<uint32_t>();
 } // readUInt32
 
 
@@ -371,49 +375,52 @@ uint32_t NimBLERemoteCharacteristic::readUInt32() {
  * @return The value as a byte
  */
 uint8_t NimBLERemoteCharacteristic::readUInt8() {
-    std::string value = readValue();
-    if (value.length() >= 1) {
-        return (uint8_t)value[0];
-    }
-    return 0;
+    return readValue<uint8_t>();
 } // readUInt8
 
 
+/**
+ * @brief Read a float value.
+ * @return the float value.
+ */
+float NimBLERemoteCharacteristic::readFloat() {
+	return readValue<float>();
+} // readFloat
+
+
 /**
  * @brief Read the value of the remote characteristic.
  * @return The value of the remote characteristic.
  */
-std::string NimBLERemoteCharacteristic::readValue() {
+std::string NimBLERemoteCharacteristic::readValue(time_t *timestamp) {
     NIMBLE_LOGD(LOG_TAG, ">> readValue(): uuid: %s, handle: %d 0x%.2x",
                          getUUID().toString().c_str(), getHandle(), getHandle());
 
-    int rc = 0;
-    int retryCount = 1;
-    // Clear the value before reading.
-    m_value = "";
-
     NimBLEClient* pClient = getRemoteService()->getClient();
+    std::string value;
 
-    // Check to see that we are connected.
     if (!pClient->isConnected()) {
         NIMBLE_LOGE(LOG_TAG, "Disconnected");
-        return "";
+        return value;
     }
 
-    do {
-        m_semaphoreReadCharEvt.take("readValue");
+    int rc = 0;
+    int retryCount = 1;
+    ble_task_data_t taskData = {this, xTaskGetCurrentTaskHandle(),0, &value};
 
+    do {
         rc = ble_gattc_read_long(pClient->getConnId(), m_handle, 0,
                                  NimBLERemoteCharacteristic::onReadCB,
-                                 this);
+                                 &taskData);
         if (rc != 0) {
             NIMBLE_LOGE(LOG_TAG, "Error: Failed to read characteristic; rc=%d, %s",
                                   rc, NimBLEUtils::returnCodeToString(rc));
-            m_semaphoreReadCharEvt.give(0);
-            return "";
+            return value;
         }
 
-        rc = m_semaphoreReadCharEvt.wait("readValue");
+        ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
+        rc = taskData.rc;
+
         switch(rc){
             case 0:
             case BLE_HS_EDONE:
@@ -431,12 +438,21 @@ std::string NimBLERemoteCharacteristic::readValue() {
                     break;
             /* Else falls through. */
             default:
-                return "";
+                NIMBLE_LOGE(LOG_TAG, "<< readValue rc=%d", rc);
+                return value;
         }
     } while(rc != 0 && retryCount--);
 
-    NIMBLE_LOGD(LOG_TAG, "<< readValue(): length: %d", m_value.length());
-    return m_value;
+    portENTER_CRITICAL(&m_valMux);
+    m_value = value;
+    m_timestamp = time(nullptr);
+    if(timestamp != nullptr) {
+        *timestamp = m_timestamp;
+    }
+    portEXIT_CRITICAL(&m_valMux);
+
+    NIMBLE_LOGD(LOG_TAG, "<< readValue length: %d rc=%d", value.length(), rc);
+    return value;
 } // readValue
 
 
@@ -448,31 +464,91 @@ int NimBLERemoteCharacteristic::onReadCB(uint16_t conn_handle,
                 const struct ble_gatt_error *error,
                 struct ble_gatt_attr *attr, void *arg)
 {
-    NimBLERemoteCharacteristic* characteristic = (NimBLERemoteCharacteristic*)arg;
+    ble_task_data_t *pTaskData = (ble_task_data_t*)arg;
+    NimBLERemoteCharacteristic *characteristic = (NimBLERemoteCharacteristic*)pTaskData->pATT;
     uint16_t conn_id = characteristic->getRemoteService()->getClient()->getConnId();
 
-    // Make sure the read is for this client
     if(conn_id != conn_handle) {
         return 0;
     }
+
     NIMBLE_LOGI(LOG_TAG, "Read complete; status=%d conn_handle=%d", error->status, conn_handle);
 
-    if(error->status == 0) {
-        if(attr) {
-            NIMBLE_LOGD(LOG_TAG, "Got %d bytes", attr->om->om_len);
+    std::string *strBuf = (std::string*)pTaskData->buf;
+    int rc = error->status;
 
-            characteristic->m_value += std::string((char*) attr->om->om_data, attr->om->om_len);
-            return 0;
+    if(rc == 0) {
+        if(attr) {
+            if(((*strBuf).length() + attr->om->om_len) > BLE_ATT_ATTR_MAX_LEN) {
+                rc = BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
+            } else {
+                NIMBLE_LOGD(LOG_TAG, "Got %d bytes", attr->om->om_len);
+                (*strBuf) += std::string((char*) attr->om->om_data, attr->om->om_len);
+                return 0;
+            }
         }
     }
-    // Read complete release semaphore and let the app can continue.
-    characteristic->m_semaphoreReadCharEvt.give(error->status);
-    return 0;
+
+    pTaskData->rc = rc;
+    xTaskNotifyGive(pTaskData->task);
+
+    return rc;
 }
 
 
 /**
- * @brief Register for notifications.
+ * @brief Subscribe or unsubscribe for notifications or indications.
+ * @param [in] uint16_t val 0x00 to unsubscribe, 0x01 for notifications, 0x02 for indications.
+ * @param [in] notifyCallback A callback to be invoked for a notification.  If NULL is provided then no callback
+ * is performed for notifications.
+ * @return true if successful.
+ */
+bool NimBLERemoteCharacteristic::setNotify(uint16_t val, bool response, notify_callback notifyCallback) {
+    NIMBLE_LOGD(LOG_TAG, ">> setNotify(): %s, %02x", toString().c_str(), val);
+
+    NimBLERemoteDescriptor* desc = getDescriptor(NimBLEUUID((uint16_t)0x2902));
+    if(desc == nullptr) {
+        NIMBLE_LOGE(LOG_TAG, "<< setNotify(): Could not get descriptor");
+        return false;
+    }
+
+    m_notifyCallback = notifyCallback;
+
+    NIMBLE_LOGD(LOG_TAG, "<< setNotify()");
+
+    return desc->writeValue((uint8_t *)&val, 2, response);
+} // setNotify
+
+
+/**
+ * @brief Subscribe for notifications or indications.
+ * @param [in] bool if true, subscribe for notifications, false subscribe for indications.
+ * @param [in] bool if true, require a write response from the descriptor write operation.
+ * @param [in] notifyCallback A callback to be invoked for a notification.  If NULL is provided then no callback
+ * is performed for notifications.
+ * @return true if successful.
+ */
+bool NimBLERemoteCharacteristic::subscribe(bool notifications, bool response, notify_callback notifyCallback) {
+    if(notifications) {
+        return setNotify(0x01, response, notifyCallback);
+    } else {
+        return setNotify(0x02, response, notifyCallback);
+    }
+} // subscribe
+
+
+/**
+ * @brief Unsubscribe for notifications or indications.
+ * @param [in] bool if true, require a write response from the descriptor write operation.
+ * @return true if successful.
+ */
+bool NimBLERemoteCharacteristic::unsubscribe(bool response) {
+    return setNotify(0x00, response);
+} // unsubscribe
+
+
+ /**
+ * @brief backward-compatibility method for subscribe/unsubscribe notifications/indications
  * @param [in] notifyCallback A callback to be invoked for a notification.  If NULL is provided then we are
  * unregistering for notifications.
  * @param [in] bool if true, register for notifications, false register for indications.
@@ -480,46 +556,54 @@ int NimBLERemoteCharacteristic::onReadCB(uint16_t conn_handle,
  * @return true if successful.
  */
 bool NimBLERemoteCharacteristic::registerForNotify(notify_callback notifyCallback, bool notifications, bool response) {
-    NIMBLE_LOGD(LOG_TAG, ">> registerForNotify(): %s", toString().c_str());
-
-    m_notifyCallback = notifyCallback;   // Save the notification callback.
-
-    uint8_t val[] = {0x01, 0x00};
-
-    NimBLERemoteDescriptor* desc = getDescriptor(NimBLEUUID((uint16_t)0x2902));
-    if(desc == nullptr)
-        return false;
-
-    if(notifyCallback != nullptr){
-        if(!notifications){
-            val[0] = 0x02;
-        }
+    bool success;
+    if(notifyCallback != nullptr) {
+        success = subscribe(notifications, response, notifyCallback);
+    } else {
+        success = unsubscribe(response);
     }
-
-    else if (notifyCallback == nullptr){
-        val[0] = 0x00;
-    }
-
-    NIMBLE_LOGD(LOG_TAG, "<< registerForNotify()");
-
-    return desc->writeValue(val, 2, response);
+    return success;
 } // registerForNotify
 
 
 /**
  * @brief Delete the descriptors in the descriptor vector.
  * We maintain a vector called m_descriptorVector that contains pointers to BLERemoteDescriptors
- * object references. Since we allocated these in this class, we are also responsible for deleteing
+ * object references. Since we allocated these in this class, we are also responsible for deleting
  * them. This method does just that.
  * @return N/A.
  */
-void NimBLERemoteCharacteristic::removeDescriptors() {
-    // Iterate through all the descriptors releasing their storage and erasing them from the vector.
+void NimBLERemoteCharacteristic::deleteDescriptors() {
+    NIMBLE_LOGD(LOG_TAG, ">> deleteDescriptors");
+
     for(auto &it: m_descriptorVector) {
         delete it;
     }
     m_descriptorVector.clear();
-} // removeCharacteristics
+    NIMBLE_LOGD(LOG_TAG, "<< deleteDescriptors");
+} // deleteDescriptors
+
+
+/**
+ * @brief Delete descriptor by UUID
+ * @param [in] uuid The UUID of the descriptor to be deleted.
+ * @return Number of services left.
+ */
+size_t NimBLERemoteCharacteristic::deleteDescriptor(const NimBLEUUID &uuid) {
+    NIMBLE_LOGD(LOG_TAG, ">> deleteDescriptor");
+
+    for(auto it = m_descriptorVector.begin(); it != m_descriptorVector.end(); ++it) {
+        if((*it)->getUUID() == uuid) {
+            delete *it;
+            m_descriptorVector.erase(it);
+            break;
+        }
+    }
+
+    NIMBLE_LOGD(LOG_TAG, "<< deleteDescriptor");
+
+    return m_descriptorVector.size();
+} // deleteDescriptor
 
 
 /**
@@ -559,19 +643,6 @@ bool NimBLERemoteCharacteristic::writeValue(const std::string &newValue, bool re
 } // writeValue
 
 
-/**
- * @brief Write the new value for the characteristic.
- *
- * This is a convenience function.  Many BLE characteristics are a single byte of data.
- * @param [in] newValue The new byte value to write.
- * @param [in] response Whether we require a response from the write.
- * @return false if not connected or cant perform write for some reason.
- */
-bool NimBLERemoteCharacteristic::writeValue(uint8_t newValue, bool response) {
-    return writeValue(&newValue, 1, response);
-} // writeValue
-
-
 /**
  * @brief Write the new value for the characteristic from a data buffer.
  * @param [in] data A pointer to a data buffer.
@@ -584,47 +655,45 @@ bool NimBLERemoteCharacteristic::writeValue(const uint8_t* data, size_t length,
     NIMBLE_LOGD(LOG_TAG, ">> writeValue(), length: %d", length);
 
     NimBLEClient* pClient = getRemoteService()->getClient();
-    int rc = 0;
-    int retryCount = 1;
-    uint16_t mtu;
 
-    // Check to see that we are connected.
     if (!pClient->isConnected()) {
         NIMBLE_LOGE(LOG_TAG, "Disconnected");
         return false;
     }
 
-    mtu = ble_att_mtu(pClient->getConnId()) - 3;
+    int rc = 0;
+    int retryCount = 1;
+    uint16_t mtu = ble_att_mtu(pClient->getConnId()) - 3;
 
-    // Check if the data length is longer than we can write in 1 connection event.
+    // Check if the data length is longer than we can write in one connection event.
     // If so we must do a long write which requires a response.
     if(length <= mtu && !response) {
         rc =  ble_gattc_write_no_rsp_flat(pClient->getConnId(), m_handle, data, length);
         return (rc==0);
     }
 
-    do {
-        m_semaphoreWriteCharEvt.take("writeValue");
+    ble_task_data_t taskData = {this, xTaskGetCurrentTaskHandle(), 0, nullptr};
 
+    do {
         if(length > mtu) {
             NIMBLE_LOGI(LOG_TAG,"long write %d bytes", length);
             os_mbuf *om = ble_hs_mbuf_from_flat(data, length);
             rc = ble_gattc_write_long(pClient->getConnId(), m_handle, 0, om,
                                       NimBLERemoteCharacteristic::onWriteCB,
-                                      this);
+                                      &taskData);
         } else {
             rc = ble_gattc_write_flat(pClient->getConnId(), m_handle,
                                       data, length,
                                       NimBLERemoteCharacteristic::onWriteCB,
-                                      this);
+                                      &taskData);
         }
         if (rc != 0) {
             NIMBLE_LOGE(LOG_TAG, "Error: Failed to write characteristic; rc=%d", rc);
-            m_semaphoreWriteCharEvt.give();
             return false;
         }
 
-        rc = m_semaphoreWriteCharEvt.wait("writeValue");
+        ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
+        rc = taskData.rc;
 
         switch(rc){
             case 0:
@@ -644,11 +713,12 @@ bool NimBLERemoteCharacteristic::writeValue(const uint8_t* data, size_t length,
                     break;
             /* Else falls through. */
             default:
+                NIMBLE_LOGE(LOG_TAG, "<< writeValue, rc: %d", rc);
                 return false;
         }
     } while(rc != 0 && retryCount--);
 
-    NIMBLE_LOGD(LOG_TAG, "<< writeValue, rc: %d",rc);
+    NIMBLE_LOGD(LOG_TAG, "<< writeValue, rc: %d", rc);
     return (rc == 0);
 } // writeValue
 
@@ -661,59 +731,21 @@ int NimBLERemoteCharacteristic::onWriteCB(uint16_t conn_handle,
                 const struct ble_gatt_error *error,
                 struct ble_gatt_attr *attr, void *arg)
 {
-    NimBLERemoteCharacteristic* characteristic = (NimBLERemoteCharacteristic*)arg;
+    ble_task_data_t *pTaskData = (ble_task_data_t*)arg;
+    NimBLERemoteCharacteristic *characteristic = (NimBLERemoteCharacteristic*)pTaskData->pATT;
 
-    // Make sure the discovery is for this device
     if(characteristic->getRemoteService()->getClient()->getConnId() != conn_handle){
         return 0;
     }
 
     NIMBLE_LOGI(LOG_TAG, "Write complete; status=%d conn_handle=%d", error->status, conn_handle);
 
-    characteristic->m_semaphoreWriteCharEvt.give(error->status);
+    pTaskData->rc = error->status;
+    xTaskNotifyGive(pTaskData->task);
 
     return 0;
 }
 
 
-/**
- * @brief Read raw data from remote characteristic as hex bytes
- * @return uint8_t pointer to the data read.
- */
-uint8_t* NimBLERemoteCharacteristic::readRawData() {
-    if(m_rawData != nullptr) {
-        free(m_rawData);
-        m_rawData = nullptr;
-    }
-
-    m_dataLen = m_value.length();
-    // If we have data copy it to rawData
-    if(m_dataLen) {
-        m_rawData = (uint8_t*) calloc(m_dataLen, sizeof(uint8_t));
-        memcpy(m_rawData, m_value.data(), m_dataLen);
-    }
-
-    return m_rawData;
-}
-
-
-/**
- * @brief Get the length of the data read from the remote characteristic.
- * @return size_t length of the data in bytes.
- */
-size_t NimBLERemoteCharacteristic::getDataLength() {
-    return m_value.length();
-}
-
-
-void NimBLERemoteCharacteristic::releaseSemaphores() {
-    for (auto &it: m_descriptorVector) {
-        it->releaseSemaphores();
-    }
-    m_semaphoreWriteCharEvt.give(1);
-    m_semaphoreGetDescEvt.give(1);
-    m_semaphoreReadCharEvt.give(1);
-}
-
 #endif // #if defined( CONFIG_BT_NIMBLE_ROLE_CENTRAL)
 #endif /* CONFIG_BT_ENABLED */
diff --git a/libesp32/NimBLE-Arduino/src/NimBLERemoteCharacteristic.h b/libesp32/NimBLE-Arduino/src/NimBLERemoteCharacteristic.h
index 0b7d490d6..364efb59d 100644
--- a/libesp32/NimBLE-Arduino/src/NimBLERemoteCharacteristic.h
+++ b/libesp32/NimBLE-Arduino/src/NimBLERemoteCharacteristic.h
@@ -32,6 +32,12 @@ class NimBLERemoteDescriptor;
 typedef void (*notify_callback)(NimBLERemoteCharacteristic* pBLERemoteCharacteristic,
                                 uint8_t* pData, size_t length, bool isNotify);
 
+typedef struct {
+    const NimBLEUUID *uuid;
+    void *task_data;
+} desc_filter_t;
+
+
 /**
  * @brief A model of a remote %BLE characteristic.
  */
@@ -50,26 +56,54 @@ public:
     std::vector<NimBLERemoteDescriptor*>::iterator end();
     NimBLERemoteDescriptor*                        getDescriptor(const NimBLEUUID &uuid);
     std::vector<NimBLERemoteDescriptor*>*          getDescriptors(bool refresh = false);
+    void                                           deleteDescriptors();
+    size_t                                         deleteDescriptor(const NimBLEUUID &uuid);
     uint16_t                                       getHandle();
     uint16_t                                       getDefHandle();
     NimBLEUUID                                     getUUID();
-    std::string                                    readValue();
-    uint8_t                                        readUInt8();
-    uint16_t                                       readUInt16();
-    uint32_t                                       readUInt32();
-    bool                                           registerForNotify(notify_callback _callback,
+    std::string                                    readValue(time_t *timestamp = nullptr);
+
+    template<typename T>
+    T                                              readValue(time_t *timestamp = nullptr, bool skipSizeCheck = false) {
+        std::string value = readValue(timestamp);
+        if(!skipSizeCheck && value.size() < sizeof(T)) return T();
+        const char *pData = value.data();
+        return *((T *)pData);
+    }
+
+    uint8_t                                        readUInt8()  __attribute__ ((deprecated("Use template readValue<uint8_t>()")));
+    uint16_t                                       readUInt16() __attribute__ ((deprecated("Use template readValue<uint16_t>()")));
+    uint32_t                                       readUInt32() __attribute__ ((deprecated("Use template readValue<uint32_t>()")));
+    float                                          readFloat()  __attribute__ ((deprecated("Use template readValue<float>()")));
+    std::string                                    getValue(time_t *timestamp = nullptr);
+
+    template<typename T>
+    T                                              getValue(time_t *timestamp = nullptr, bool skipSizeCheck = false) {
+        std::string value = getValue(timestamp);
+        if(!skipSizeCheck && value.size() < sizeof(T)) return T();
+        const char *pData = value.data();
+        return *((T *)pData);
+    }
+
+    bool                                           subscribe(bool notifications = true,
+                                                             bool response = true,
+                                                             notify_callback notifyCallback = nullptr);
+    bool                                           unsubscribe(bool response = true);
+    bool                                           registerForNotify(notify_callback notifyCallback,
                                                                      bool notifications = true,
-                                                                     bool response = true);
+                                                                     bool response = true)
+                                                                     __attribute__ ((deprecated("Use subscribe()/unsubscribe()")));
     bool                                           writeValue(const uint8_t* data,
                                                               size_t length,
                                                               bool response = false);
     bool                                           writeValue(const std::string &newValue,
                                                               bool response = false);
-    bool                                           writeValue(uint8_t newValue,
-                                                              bool response = false);
+    template<typename T>
+    bool writeValue(const T &s, bool response = false) {
+        return writeValue((uint8_t*)&s, sizeof(T), response);
+    }
+
     std::string                                    toString();
-    uint8_t*                                       readRawData();
-    size_t                                         getDataLength();
     NimBLERemoteService*                           getRemoteService();
 
 private:
@@ -81,13 +115,12 @@ private:
     friend class      NimBLERemoteDescriptor;
 
     // Private member functions
-    void              removeDescriptors();
+    bool              setNotify(uint16_t val, bool response = true, notify_callback notifyCallback = nullptr);
     bool              retrieveDescriptors(const NimBLEUUID *uuid_filter = nullptr);
     static int        onReadCB(uint16_t conn_handle, const struct ble_gatt_error *error,
                                struct ble_gatt_attr *attr, void *arg);
     static int        onWriteCB(uint16_t conn_handle, const struct ble_gatt_error *error,
                                 struct ble_gatt_attr *attr, void *arg);
-    void              releaseSemaphores();
     static int        descriptorDiscCB(uint16_t conn_handle, const struct ble_gatt_error *error,
                                        uint16_t chr_val_handle, const struct ble_gatt_dsc *dsc,
                                        void *arg);
@@ -98,13 +131,10 @@ private:
     uint16_t                m_handle;
     uint16_t                m_defHandle;
     NimBLERemoteService*    m_pRemoteService;
-    FreeRTOS::Semaphore     m_semaphoreGetDescEvt       = FreeRTOS::Semaphore("GetDescEvt");
-    FreeRTOS::Semaphore     m_semaphoreReadCharEvt      = FreeRTOS::Semaphore("ReadCharEvt");
-    FreeRTOS::Semaphore     m_semaphoreWriteCharEvt     = FreeRTOS::Semaphore("WriteCharEvt");
     std::string             m_value;
-    uint8_t*                m_rawData;
-    size_t                  m_dataLen;
     notify_callback         m_notifyCallback;
+    time_t                  m_timestamp;
+    portMUX_TYPE            m_valMux;
 
     // We maintain a vector of descriptors owned by this characteristic.
     std::vector<NimBLERemoteDescriptor*> m_descriptorVector;
diff --git a/libesp32/NimBLE-Arduino/src/NimBLERemoteDescriptor.cpp b/libesp32/NimBLE-Arduino/src/NimBLERemoteDescriptor.cpp
index 057a68a1c..d17ae6a0e 100644
--- a/libesp32/NimBLE-Arduino/src/NimBLERemoteDescriptor.cpp
+++ b/libesp32/NimBLE-Arduino/src/NimBLERemoteDescriptor.cpp
@@ -45,9 +45,9 @@ NimBLERemoteDescriptor::NimBLERemoteDescriptor(NimBLERemoteCharacteristic* pRemo
             m_uuid = nullptr;
             break;
     }
+
     m_handle                = dsc->handle;
     m_pRemoteCharacteristic = pRemoteCharacteristic;
-
 }
 
 
@@ -78,96 +78,6 @@ NimBLEUUID NimBLERemoteDescriptor::getUUID() {
 } // getUUID
 
 
-/**
- * @brief Callback for Descriptor read operation.
- * @return 0 or error code.
- */
-int NimBLERemoteDescriptor::onReadCB(uint16_t conn_handle,
-                const struct ble_gatt_error *error,
-                struct ble_gatt_attr *attr, void *arg)
-{
-    NimBLERemoteDescriptor* desc = (NimBLERemoteDescriptor*)arg;
-    uint16_t conn_id = desc->getRemoteCharacteristic()->getRemoteService()->getClient()->getConnId();
-
-        // Make sure the discovery is for this device
-    if(conn_id != conn_handle){
-        return 0;
-    }
-
-    NIMBLE_LOGD(LOG_TAG, "Read complete; status=%d conn_handle=%d", error->status, conn_handle);
-
-    if(error->status == 0){
-        if(attr){
-            NIMBLE_LOGD(LOG_TAG, "Got %d bytes", attr->om->om_len);
-
-            desc->m_value += std::string((char*) attr->om->om_data, attr->om->om_len);
-            return 0;
-        }
-    }
-
-    // Read complete release semaphore and let the app can continue.
-    desc->m_semaphoreReadDescrEvt.give(error->status);
-    return 0;
-}
-
-
-std::string NimBLERemoteDescriptor::readValue() {
-    NIMBLE_LOGD(LOG_TAG, ">> Descriptor readValue: %s", toString().c_str());
-
-    int rc = 0;
-    int retryCount = 1;
-    // Clear the value before reading.
-    m_value = "";
-
-    NimBLEClient* pClient = getRemoteCharacteristic()->getRemoteService()->getClient();
-
-    // Check to see that we are connected.
-    if (!pClient->isConnected()) {
-        NIMBLE_LOGE(LOG_TAG, "Disconnected");
-        return "";
-    }
-
-    do {
-        m_semaphoreReadDescrEvt.take("ReadDescriptor");
-
-        rc = ble_gattc_read_long(pClient->getConnId(), m_handle, 0,
-                                 NimBLERemoteDescriptor::onReadCB,
-                                 this);
-        if (rc != 0) {
-            NIMBLE_LOGE(LOG_TAG, "Error: Failed to read descriptor; rc=%d, %s",
-                                  rc, NimBLEUtils::returnCodeToString(rc));
-            m_semaphoreReadDescrEvt.give(0);
-            return "";
-        }
-
-        rc = m_semaphoreReadDescrEvt.wait("ReadDescriptor");
-
-        switch(rc){
-            case 0:
-            case BLE_HS_EDONE:
-                rc = 0;
-                break;
-            // Descriptor is not long-readable, return with what we have.
-            case BLE_HS_ATT_ERR(BLE_ATT_ERR_ATTR_NOT_LONG):
-                NIMBLE_LOGI(LOG_TAG, "Attribute not long");
-                rc = 0;
-                break;
-            case BLE_HS_ATT_ERR(BLE_ATT_ERR_INSUFFICIENT_AUTHEN):
-            case BLE_HS_ATT_ERR(BLE_ATT_ERR_INSUFFICIENT_AUTHOR):
-            case BLE_HS_ATT_ERR(BLE_ATT_ERR_INSUFFICIENT_ENC):
-                if (retryCount && pClient->secureConnection())
-                    break;
-            /* Else falls through. */
-            default:
-                return "";
-        }
-    } while(rc != 0 && retryCount--);
-
-    NIMBLE_LOGD(LOG_TAG, "<< Descriptor readValue(): length: %d", m_value.length());
-    return m_value;
-} // readValue
-
-
 uint8_t NimBLERemoteDescriptor::readUInt8() {
     std::string value = readValue();
     if (value.length() >= 1) {
@@ -195,6 +105,100 @@ uint32_t NimBLERemoteDescriptor::readUInt32() {
 } // readUInt32
 
 
+std::string NimBLERemoteDescriptor::readValue() {
+    NIMBLE_LOGD(LOG_TAG, ">> Descriptor readValue: %s", toString().c_str());
+
+    NimBLEClient* pClient = getRemoteCharacteristic()->getRemoteService()->getClient();
+    std::string value;
+
+    if (!pClient->isConnected()) {
+        NIMBLE_LOGE(LOG_TAG, "Disconnected");
+        return value;
+    }
+
+    int rc = 0;
+    int retryCount = 1;
+    ble_task_data_t taskData = {this, xTaskGetCurrentTaskHandle(),0, &value};
+
+    do {
+        rc = ble_gattc_read_long(pClient->getConnId(), m_handle, 0,
+                                 NimBLERemoteDescriptor::onReadCB,
+                                 &taskData);
+        if (rc != 0) {
+            NIMBLE_LOGE(LOG_TAG, "Error: Failed to read descriptor; rc=%d, %s",
+                                  rc, NimBLEUtils::returnCodeToString(rc));
+            return value;
+        }
+
+        ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
+        rc = taskData.rc;
+
+        switch(rc){
+            case 0:
+            case BLE_HS_EDONE:
+                rc = 0;
+                break;
+            // Descriptor is not long-readable, return with what we have.
+            case BLE_HS_ATT_ERR(BLE_ATT_ERR_ATTR_NOT_LONG):
+                NIMBLE_LOGI(LOG_TAG, "Attribute not long");
+                rc = 0;
+                break;
+            case BLE_HS_ATT_ERR(BLE_ATT_ERR_INSUFFICIENT_AUTHEN):
+            case BLE_HS_ATT_ERR(BLE_ATT_ERR_INSUFFICIENT_AUTHOR):
+            case BLE_HS_ATT_ERR(BLE_ATT_ERR_INSUFFICIENT_ENC):
+                if (retryCount && pClient->secureConnection())
+                    break;
+            /* Else falls through. */
+            default:
+                return value;
+        }
+    } while(rc != 0 && retryCount--);
+
+    NIMBLE_LOGD(LOG_TAG, "<< Descriptor readValue(): length: %d rc=%d", value.length(), rc);
+    return value;
+} // readValue
+
+
+/**
+ * @brief Callback for Descriptor read operation.
+ * @return 0 or error code.
+ */
+int NimBLERemoteDescriptor::onReadCB(uint16_t conn_handle,
+                const struct ble_gatt_error *error,
+                struct ble_gatt_attr *attr, void *arg)
+{
+    ble_task_data_t *pTaskData = (ble_task_data_t*)arg;
+    NimBLERemoteDescriptor* desc = (NimBLERemoteDescriptor*)pTaskData->pATT;
+    uint16_t conn_id = desc->getRemoteCharacteristic()->getRemoteService()->getClient()->getConnId();
+
+    if(conn_id != conn_handle){
+        return 0;
+    }
+
+    NIMBLE_LOGD(LOG_TAG, "Read complete; status=%d conn_handle=%d", error->status, conn_handle);
+
+    std::string *strBuf = (std::string*)pTaskData->buf;
+    int rc = error->status;
+
+    if(rc == 0) {
+        if(attr) {
+            if(((*strBuf).length() + attr->om->om_len) > BLE_ATT_ATTR_MAX_LEN) {
+                rc = BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
+            } else {
+                NIMBLE_LOGD(LOG_TAG, "Got %d bytes", attr->om->om_len);
+                (*strBuf) += std::string((char*) attr->om->om_data, attr->om->om_len);
+                return 0;
+            }
+        }
+    }
+
+    pTaskData->rc = rc;
+    xTaskNotifyGive(pTaskData->task);
+
+    return rc;
+}
+
+
 /**
  * @brief Return a string representation of this BLE Remote Descriptor.
  * @retun A string representation of this BLE Remote Descriptor.
@@ -218,16 +222,17 @@ int NimBLERemoteDescriptor::onWriteCB(uint16_t conn_handle,
                 const struct ble_gatt_error *error,
                 struct ble_gatt_attr *attr, void *arg)
 {
-    NimBLERemoteDescriptor* descriptor = (NimBLERemoteDescriptor*)arg;
+    ble_task_data_t *pTaskData = (ble_task_data_t*)arg;
+    NimBLERemoteDescriptor* descriptor = (NimBLERemoteDescriptor*)pTaskData->pATT;
 
-    // Make sure the discovery is for this device
     if(descriptor->getRemoteCharacteristic()->getRemoteService()->getClient()->getConnId() != conn_handle){
         return 0;
     }
 
-    NIMBLE_LOGD(LOG_TAG, "Write complete; status=%d conn_handle=%d", error->status, conn_handle);
+    NIMBLE_LOGI(LOG_TAG, "Write complete; status=%d conn_handle=%d", error->status, conn_handle);
 
-    descriptor->m_semaphoreDescWrite.give(error->status);
+    pTaskData->rc = error->status;
+    xTaskNotifyGive(pTaskData->task);
 
     return 0;
 }
@@ -245,17 +250,15 @@ bool NimBLERemoteDescriptor::writeValue(const uint8_t* data, size_t length, bool
 
     NimBLEClient* pClient = getRemoteCharacteristic()->getRemoteService()->getClient();
 
-    int rc = 0;
-    int retryCount = 1;
-    uint16_t mtu;
-
     // Check to see that we are connected.
     if (!pClient->isConnected()) {
         NIMBLE_LOGE(LOG_TAG, "Disconnected");
         return false;
     }
 
-    mtu = ble_att_mtu(pClient->getConnId()) - 3;
+    int rc = 0;
+    int retryCount = 1;
+    uint16_t mtu = ble_att_mtu(pClient->getConnId()) - 3;
 
     // Check if the data length is longer than we can write in 1 connection event.
     // If so we must do a long write which requires a response.
@@ -264,31 +267,31 @@ bool NimBLERemoteDescriptor::writeValue(const uint8_t* data, size_t length, bool
         return (rc == 0);
     }
 
-    do {
-        m_semaphoreDescWrite.take("WriteDescriptor");
+    ble_task_data_t taskData = {this, xTaskGetCurrentTaskHandle(), 0, nullptr};
 
+    do {
         if(length > mtu) {
             NIMBLE_LOGI(LOG_TAG,"long write %d bytes", length);
             os_mbuf *om = ble_hs_mbuf_from_flat(data, length);
             rc = ble_gattc_write_long(pClient->getConnId(), m_handle, 0, om,
                                       NimBLERemoteDescriptor::onWriteCB,
-                                      this);
+                                      &taskData);
         } else {
             rc = ble_gattc_write_flat(pClient->getConnId(), m_handle,
                                       data, length,
                                       NimBLERemoteDescriptor::onWriteCB,
-                                      this);
+                                      &taskData);
         }
 
         if (rc != 0) {
             NIMBLE_LOGE(LOG_TAG, "Error: Failed to write descriptor; rc=%d", rc);
-            m_semaphoreDescWrite.give();
             return false;
         }
 
-        rc = m_semaphoreDescWrite.wait("WriteDescriptor");
+        ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
+        rc = taskData.rc;
 
-        switch(rc){
+        switch(rc) {
             case 0:
             case BLE_HS_EDONE:
                 rc = 0;
@@ -325,23 +328,5 @@ bool NimBLERemoteDescriptor::writeValue(const std::string &newValue, bool respon
 } // writeValue
 
 
-/**
- * @brief Write a byte value to the Descriptor.
- * @param [in] The single byte to write.
- * @param [in] True if we expect a response.
- */
-bool NimBLERemoteDescriptor::writeValue(uint8_t newValue, bool response) {
-    return writeValue(&newValue, 1, response);
-} // writeValue
-
-
-/**
- * @brief In the event of an error this is called to make sure we don't block.
- */
-void NimBLERemoteDescriptor::releaseSemaphores() {
-    m_semaphoreDescWrite.give(1);
-    m_semaphoreReadDescrEvt.give(1);
-}
-
 #endif // #if defined( CONFIG_BT_NIMBLE_ROLE_CENTRAL)
 #endif /* CONFIG_BT_ENABLED */
diff --git a/libesp32/NimBLE-Arduino/src/NimBLERemoteDescriptor.h b/libesp32/NimBLE-Arduino/src/NimBLERemoteDescriptor.h
index 23fe13f6b..554e9dc16 100644
--- a/libesp32/NimBLE-Arduino/src/NimBLERemoteDescriptor.h
+++ b/libesp32/NimBLE-Arduino/src/NimBLERemoteDescriptor.h
@@ -31,15 +31,26 @@ public:
     uint16_t                    getHandle();
     NimBLERemoteCharacteristic* getRemoteCharacteristic();
     NimBLEUUID                  getUUID();
-    std::string                 readValue(void);
-    uint8_t                     readUInt8(void);
-    uint16_t                    readUInt16(void);
-    uint32_t                    readUInt32(void);
+    std::string                 readValue();
+
+    template<typename T>
+    T                           readValue(bool skipSizeCheck = false) {
+        std::string value = readValue();
+        if(!skipSizeCheck && value.size() < sizeof(T)) return T();
+        const char *pData = value.data();
+        return *((T *)pData);
+    }
+
+    uint8_t                     readUInt8()  __attribute__ ((deprecated));
+    uint16_t                    readUInt16() __attribute__ ((deprecated));
+    uint32_t                    readUInt32() __attribute__ ((deprecated));
     std::string                 toString(void);
     bool                        writeValue(const uint8_t* data, size_t length, bool response = false);
     bool                        writeValue(const std::string &newValue, bool response = false);
-    bool                        writeValue(uint8_t newValue, bool response = false);
-
+    template<typename T>
+    bool writeValue(const T &s, bool response = false) {
+        return writeValue((uint8_t*)&s, sizeof(T), response);
+    }
 
 private:
     friend class                NimBLERemoteCharacteristic;
@@ -50,16 +61,10 @@ private:
                                           struct ble_gatt_attr *attr, void *arg);
     static int                  onReadCB(uint16_t conn_handle, const struct ble_gatt_error *error,
                                          struct ble_gatt_attr *attr, void *arg);
-    void                        releaseSemaphores();
 
     uint16_t                    m_handle;
     NimBLEUUID                  m_uuid;
-    std::string                 m_value;
     NimBLERemoteCharacteristic* m_pRemoteCharacteristic;
-    FreeRTOS::Semaphore         m_semaphoreReadDescrEvt  = FreeRTOS::Semaphore("ReadDescrEvt");
-    FreeRTOS::Semaphore         m_semaphoreDescWrite     = FreeRTOS::Semaphore("WriteDescEvt");
-
-
 };
 
 #endif // #if defined( CONFIG_BT_NIMBLE_ROLE_CENTRAL)
diff --git a/libesp32/NimBLE-Arduino/src/NimBLERemoteService.cpp b/libesp32/NimBLE-Arduino/src/NimBLERemoteService.cpp
index 193a40b1a..db9eabca1 100644
--- a/libesp32/NimBLE-Arduino/src/NimBLERemoteService.cpp
+++ b/libesp32/NimBLE-Arduino/src/NimBLERemoteService.cpp
@@ -58,7 +58,7 @@ NimBLERemoteService::NimBLERemoteService(NimBLEClient* pClient, const struct ble
  * Also release any semaphores they may be holding.
  */
 NimBLERemoteService::~NimBLERemoteService() {
-    removeCharacteristics();
+    deleteCharacteristics();
 }
 
 
@@ -118,17 +118,15 @@ NimBLERemoteCharacteristic* NimBLERemoteService::getCharacteristic(const NimBLEU
  * @param [in] bool value to indicate if the current vector should be cleared and
  * subsequently all characteristics for this service retrieved from the peripheral.
  * If false the vector will be returned with the currently stored characteristics,
- * if the vector is empty it will retrieve all characteristics of this service
- * from the peripheral.
+ * If true it will retrieve all characteristics of this service from the peripheral 
+ * and return the vector with all characteristics for this service.
  * @return a pointer to the vector of descriptors for this characteristic.
  */
 
 std::vector<NimBLERemoteCharacteristic*>* NimBLERemoteService::getCharacteristics(bool refresh) {
     if(refresh) {
-        removeCharacteristics();
-    }
+        deleteCharacteristics();
 
-    if(m_characteristicVector.empty()) {
         if (!retrieveCharacteristics()) {
             NIMBLE_LOGE(LOG_TAG, "Error: Failed to get characteristics");
         }
@@ -150,43 +148,34 @@ int NimBLERemoteService::characteristicDiscCB(uint16_t conn_handle,
     NIMBLE_LOGD(LOG_TAG,"Characteristic Discovered >> status: %d handle: %d",
                         error->status, (error->status == 0) ? chr->val_handle : -1);
 
-    NimBLERemoteService *service = (NimBLERemoteService*)arg;
-    int rc=0;
+    ble_task_data_t *pTaskData = (ble_task_data_t*)arg;
+    NimBLERemoteService *service = (NimBLERemoteService*)pTaskData->pATT;
 
     // Make sure the discovery is for this device
     if(service->getClient()->getConnId() != conn_handle){
         return 0;
     }
 
-    switch (error->status) {
-        case 0: {
-            // Found a service - add it to the vector
-            NimBLERemoteCharacteristic* pRemoteCharacteristic = new NimBLERemoteCharacteristic(service, chr);
-            service->m_characteristicVector.push_back(pRemoteCharacteristic);
-            break;
-        }
-        case BLE_HS_EDONE:{
-            /** All characteristics in this service discovered; start discovering
-             *  characteristics in the next service.
-             */
-            service->m_semaphoreGetCharEvt.give(0);
-            rc = 0;
-            break;
-        }
-        default:
-            rc = error->status;
-            break;
+    if(error->status == 0) {
+        // Found a service - add it to the vector
+        NimBLERemoteCharacteristic* pRemoteCharacteristic = new NimBLERemoteCharacteristic(service, chr);
+        service->m_characteristicVector.push_back(pRemoteCharacteristic);
+        return 0;
     }
-    if (rc != 0) {
-        /* Error; abort discovery. */
-        // pass non-zero to semaphore on error to indicate an error finding characteristics
-        // release memory from any characteristics we created
-        //service->removeCharacteristics(); --this will now be done when we clear services on returning with error
-        NIMBLE_LOGE(LOG_TAG, "characteristicDiscCB() rc=%d %s", rc, NimBLEUtils::returnCodeToString(rc));
-        service->m_semaphoreGetCharEvt.give(1);
+
+    if(error->status == BLE_HS_EDONE) {
+        pTaskData->rc = 0;
+    } else {
+        NIMBLE_LOGE(LOG_TAG, "characteristicDiscCB() rc=%d %s",
+                             error->status,
+                             NimBLEUtils::returnCodeToString(error->status));
+        pTaskData->rc = error->status;
     }
-    NIMBLE_LOGD(LOG_TAG,"<< Characteristic Discovered. status: %d", rc);
-    return rc;
+
+    xTaskNotifyGive(pTaskData->task);
+
+    NIMBLE_LOGD(LOG_TAG,"<< Characteristic Discovered");
+    return error->status;
 }
 
 
@@ -199,32 +188,31 @@ bool NimBLERemoteService::retrieveCharacteristics(const NimBLEUUID *uuid_filter)
     NIMBLE_LOGD(LOG_TAG, ">> retrieveCharacteristics() for service: %s", getUUID().toString().c_str());
 
     int rc = 0;
-    //removeCharacteristics(); // Forget any previous characteristics.
-
-    m_semaphoreGetCharEvt.take("retrieveCharacteristics");
+    ble_task_data_t taskData = {this, xTaskGetCurrentTaskHandle(), 0, nullptr};
 
     if(uuid_filter == nullptr) {
         rc = ble_gattc_disc_all_chrs(m_pClient->getConnId(),
                              m_startHandle,
                              m_endHandle,
                              NimBLERemoteService::characteristicDiscCB,
-                             this);
+                             &taskData);
     } else {
         rc = ble_gattc_disc_chrs_by_uuid(m_pClient->getConnId(),
                              m_startHandle,
                              m_endHandle,
                              &uuid_filter->getNative()->u,
                              NimBLERemoteService::characteristicDiscCB,
-                             this);
+                             &taskData);
     }
 
     if (rc != 0) {
         NIMBLE_LOGE(LOG_TAG, "ble_gattc_disc_all_chrs: rc=%d %s", rc, NimBLEUtils::returnCodeToString(rc));
-        m_semaphoreGetCharEvt.give();
         return false;
     }
 
-    if(m_semaphoreGetCharEvt.wait("retrieveCharacteristics") == 0){
+    ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
+
+    if(taskData.rc == 0){
         NIMBLE_LOGD(LOG_TAG, "<< retrieveCharacteristics()");
         return true;
     }
@@ -316,12 +304,36 @@ bool NimBLERemoteService::setValue(const NimBLEUUID &characteristicUuid, const s
  * them. This method does just that.
  * @return N/A.
  */
-void NimBLERemoteService::removeCharacteristics() {
+void NimBLERemoteService::deleteCharacteristics() {
+    NIMBLE_LOGD(LOG_TAG, ">> deleteCharacteristics");
     for(auto &it: m_characteristicVector) {
         delete it;
     }
-    m_characteristicVector.clear();   // Clear the vector
-} // removeCharacteristics
+    m_characteristicVector.clear();
+    NIMBLE_LOGD(LOG_TAG, "<< deleteCharacteristics");
+} // deleteCharacteristics
+
+
+/**
+ * @brief Delete characteristic by UUID
+ * @param [in] uuid The UUID of the characteristic to be cleared.
+ * @return Number of characteristics left.
+ */
+size_t NimBLERemoteService::deleteCharacteristic(const NimBLEUUID &uuid) {
+    NIMBLE_LOGD(LOG_TAG, ">> deleteCharacteristic");
+
+    for(auto it = m_characteristicVector.begin(); it != m_characteristicVector.end(); ++it) {
+        if((*it)->getUUID() == uuid) {
+            delete *it;
+            m_characteristicVector.erase(it);
+            break;
+        }
+    }
+
+    NIMBLE_LOGD(LOG_TAG, "<< deleteCharacteristic");
+
+    return m_characteristicVector.size();
+} // deleteCharacteristic
 
 
 /**
@@ -352,16 +364,5 @@ std::string NimBLERemoteService::toString() {
 } // toString
 
 
-/**
- * @brief called when an error occurrs and we need to release the semaphores to resume operations.
- * Will release all characteristic and subsequently all descriptor semaphores for this service.
- */
-void NimBLERemoteService::releaseSemaphores() {
-    for(auto &it: m_characteristicVector) {
-       it->releaseSemaphores();
-    }
-    m_semaphoreGetCharEvt.give(1);
-}
-
 #endif // #if defined( CONFIG_BT_NIMBLE_ROLE_CENTRAL)
 #endif /* CONFIG_BT_ENABLED */
diff --git a/libesp32/NimBLE-Arduino/src/NimBLERemoteService.h b/libesp32/NimBLE-Arduino/src/NimBLERemoteService.h
index 3803498d0..2d63c5d70 100644
--- a/libesp32/NimBLE-Arduino/src/NimBLERemoteService.h
+++ b/libesp32/NimBLE-Arduino/src/NimBLERemoteService.h
@@ -22,7 +22,6 @@
 
 #include "NimBLEClient.h"
 #include "NimBLEUUID.h"
-#include "FreeRTOS.h"
 #include "NimBLERemoteCharacteristic.h"
 
 #include <vector>
@@ -43,6 +42,8 @@ public:
     std::vector<NimBLERemoteCharacteristic*>::iterator end();
     NimBLERemoteCharacteristic*               getCharacteristic(const char* uuid);
     NimBLERemoteCharacteristic*               getCharacteristic(const NimBLEUUID &uuid);
+    void                                      deleteCharacteristics();
+    size_t                                    deleteCharacteristic(const NimBLEUUID &uuid);
     NimBLEClient*                             getClient(void);
     uint16_t                                  getHandle();
     NimBLEUUID                                getUUID(void);
@@ -70,7 +71,6 @@ private:
     uint16_t            getStartHandle();
     uint16_t            getEndHandle();
     void                releaseSemaphores();
-    void                removeCharacteristics();
 
     // Properties
 
@@ -78,7 +78,6 @@ private:
     std::vector<NimBLERemoteCharacteristic*> m_characteristicVector;
 
     NimBLEClient*       m_pClient;
-    FreeRTOS::Semaphore m_semaphoreGetCharEvt = FreeRTOS::Semaphore("GetCharEvt");
     NimBLEUUID          m_uuid;
     uint16_t            m_startHandle;
     uint16_t            m_endHandle;
diff --git a/libesp32/NimBLE-Arduino/src/NimBLEScan.cpp b/libesp32/NimBLE-Arduino/src/NimBLEScan.cpp
index 6d738f951..dbf88741d 100644
--- a/libesp32/NimBLE-Arduino/src/NimBLEScan.cpp
+++ b/libesp32/NimBLE-Arduino/src/NimBLEScan.cpp
@@ -18,7 +18,6 @@
 #if defined(CONFIG_BT_NIMBLE_ROLE_OBSERVER)
 
 #include "NimBLEScan.h"
-#include "NimBLEUtils.h"
 #include "NimBLEDevice.h"
 #include "NimBLELog.h"
 
@@ -26,41 +25,12 @@
 
 static const char* LOG_TAG = "NimBLEScan";
 
-/*
- * Scanning filter policy
- *  NO_WL:
- *      Scanner processes all advertising packets (white list not used) except
- *      directed, connectable advertising packets not sent to the scanner.
- *  USE_WL:
- *      Scanner processes advertisements from white list only. A connectable,
- *      directed advertisment is ignored unless it contains scanners address.
- *  NO_WL_INITA:
- *      Scanner process all advertising packets (white list not used). A
- *      connectable, directed advertisement shall not be ignored if the InitA
- *      is a resolvable private address.
- *  USE_WL_INITA:
- *      Scanner process advertisements from white list only. A connectable,
- *      directed advertisement shall not be ignored if the InitA is a
- *      resolvable private address.
- */
-
-//#define BLE_HCI_SCAN_FILT_NO_WL             (0)
-//#define BLE_HCI_SCAN_FILT_USE_WL            (1)
-//#define BLE_HCI_SCAN_FILT_NO_WL_INITA       (2)
-//#define BLE_HCI_SCAN_FILT_USE_WL_INITA      (3)
-//#define BLE_HCI_SCAN_FILT_MAX               (3)
-
 
 /**
  * @brief Scan constuctor.
  */
 NimBLEScan::NimBLEScan() {
-    uint8_t own_addr_type;
-    if(ble_hs_id_infer_auto(0, &own_addr_type) !=0){
-        NIMBLE_LOGE(LOG_TAG, "error determining address type\n");
-        return;
-    }
-    m_own_addr_type                  = own_addr_type;
+    m_own_addr_type                  = 0;
     m_scan_params.filter_policy      = BLE_HCI_SCAN_FILT_NO_WL;
     m_scan_params.passive            = 1; // If set, don’t send scan requests to advertisers (i.e., don’t request additional advertising data).
     m_scan_params.itvl               = 0; // This is defined as the time interval from when the Controller started its last LE scan until it begins the subsequent LE scan. (units=0.625 msec)
@@ -70,6 +40,7 @@ NimBLEScan::NimBLEScan() {
     m_pAdvertisedDeviceCallbacks     = nullptr;
     m_stopped                        = true;
     m_wantDuplicates                 = false;
+    m_pTaskData                      = nullptr;
 }
 
 
@@ -101,14 +72,6 @@ NimBLEScan::NimBLEScan() {
 
             NimBLEAddress advertisedAddress(event->disc.addr);
 
-            // Print advertisement data
-    //        print_adv_fields(&fields);
-
-            // If we are not scanning, nothing to do with the extra results.
-            if (pScan->m_stopped) {
-                return 0;
-            }
-
             // Examine our list of ignored addresses and stop processing if we don't want to see it or are already connected
             if(NimBLEDevice::isIgnored(advertisedAddress)) {
                 NIMBLE_LOGI(LOG_TAG, "Ignoring device: address: %s", advertisedAddress.toString().c_str());
@@ -131,7 +94,6 @@ NimBLEScan::NimBLEScan() {
                 advertisedDevice = new NimBLEAdvertisedDevice();
                 advertisedDevice->setAddressType(event->disc.addr.type);
                 advertisedDevice->setAddress(advertisedAddress);
-                //NIMBLE_LOGE(LOG_TAG, "advertisement type: %d, %s",event->disc.event_type, NimBLEUtils::advTypeToString(event->disc.event_type));
                 advertisedDevice->setAdvType(event->disc.event_type);
                 pScan->m_scanResults.m_advertisedDevicesVector.push_back(advertisedDevice);
                 NIMBLE_LOGI(LOG_TAG, "NEW DEVICE FOUND: %s", advertisedAddress.toString().c_str());
@@ -143,16 +105,21 @@ NimBLEScan::NimBLEScan() {
             advertisedDevice->parseAdvertisement(&fields);
             advertisedDevice->setScan(pScan);
             advertisedDevice->setAdvertisementResult(event->disc.data, event->disc.length_data);
+            advertisedDevice->m_timestamp = time(nullptr);
 
             if (pScan->m_pAdvertisedDeviceCallbacks) {
-                // If not active scanning report the result to the listener.
-                if(pScan->m_scan_params.passive || event->disc.event_type == BLE_HCI_ADV_TYPE_ADV_NONCONN_IND) {
-                    pScan->m_pAdvertisedDeviceCallbacks->onResult(advertisedDevice);
-                // Otherwise wait for the scan response so we can report all of the data at once.
-                } else if (event->disc.event_type == BLE_HCI_ADV_RPT_EVTYPE_SCAN_RSP) {
-                    pScan->m_pAdvertisedDeviceCallbacks->onResult(advertisedDevice);
+                if(pScan->m_wantDuplicates || !advertisedDevice->m_callbackSent) {
+                    // If not active scanning report the result to the listener.
+                    if(pScan->m_scan_params.passive || event->disc.event_type == BLE_HCI_ADV_TYPE_ADV_NONCONN_IND) {
+                        advertisedDevice->m_callbackSent = true;
+                        pScan->m_pAdvertisedDeviceCallbacks->onResult(advertisedDevice);
+
+                    // Otherwise wait for the scan response so we can report all of the data at once.
+                    } else if (event->disc.event_type == BLE_HCI_ADV_RPT_EVTYPE_SCAN_RSP) {
+                        advertisedDevice->m_callbackSent = true;
+                        pScan->m_pAdvertisedDeviceCallbacks->onResult(advertisedDevice);
+                    }
                 }
-                //m_pAdvertisedDeviceCallbacks->onResult(*advertisedDevice);
             }
 
             return 0;
@@ -166,7 +133,11 @@ NimBLEScan::NimBLEScan() {
             }
 
             pScan->m_stopped = true;
-            pScan->m_semaphoreScanEnd.give();
+            if(pScan->m_pTaskData != nullptr) {
+                pScan->m_pTaskData->rc = event->disc_complete.reason;
+                xTaskNotifyGive(pScan->m_pTaskData->task);
+            }
+
             return 0;
         }
 
@@ -191,13 +162,59 @@ void NimBLEScan::setActiveScan(bool active) {
 } // setActiveScan
 
 
+/**
+ * @brief Set whether or not the BLE controller should only report results
+ * from devices it has not already seen.
+ * @param [in] active If true, scanned devices will only be reported once.
+ * @details The controller has a limited buffer and will start reporting
+ * dupicate devices once the limit is reached.
+ */
+void NimBLEScan::setDuplicateFilter(bool active) {
+    m_scan_params.filter_duplicates = active;
+} // setDuplicateFilter
+
+
+/**
+ * @brief Set whether or not the BLE controller only report scan results
+ * from devices advertising in limited discovery mode, i.e. directed advertising.
+ * @param [in] active If true, only limited discovery devices will be in scan results.
+ */
+void NimBLEScan::setLimitedOnly(bool active) {
+    m_scan_params.limited = active;
+} // setLimited
+
+
+/**
+ * @brief Sets the scan filter policy.
+ * @param [in] filter Can be one of:
+ *  BLE_HCI_SCAN_FILT_NO_WL             (0)
+ *      Scanner processes all advertising packets (white list not used) except
+ *      directed, connectable advertising packets not sent to the scanner.
+ *  BLE_HCI_SCAN_FILT_USE_WL            (1)
+ *      Scanner processes advertisements from white list only. A connectable,
+ *      directed advertisment is ignored unless it contains scanners address.
+ *  BLE_HCI_SCAN_FILT_NO_WL_INITA       (2)
+ *      Scanner process all advertising packets (white list not used). A
+ *      connectable, directed advertisement shall not be ignored if the InitA
+ *      is a resolvable private address.
+ *  BLE_HCI_SCAN_FILT_USE_WL_INITA      (3)
+ *      Scanner process advertisements from white list only. A connectable,
+ *      directed advertisement shall not be ignored if the InitA is a
+ *      resolvable private address.
+ */
+void NimBLEScan::setFilterPolicy(uint8_t filter) {
+    m_scan_params.filter_policy = filter;
+} // setFilterPolicy
+
+
 /**
  * @brief Set the call backs to be invoked.
  * @param [in] pAdvertisedDeviceCallbacks Call backs to be invoked.
  * @param [in] wantDuplicates  True if we wish to be called back with duplicates.  Default is false.
  */
-void NimBLEScan::setAdvertisedDeviceCallbacks(NimBLEAdvertisedDeviceCallbacks* pAdvertisedDeviceCallbacks/*, bool wantDuplicates*/) {
-    //m_wantDuplicates = wantDuplicates;
+void NimBLEScan::setAdvertisedDeviceCallbacks(NimBLEAdvertisedDeviceCallbacks* pAdvertisedDeviceCallbacks,
+                                              bool wantDuplicates) {
+    m_wantDuplicates = wantDuplicates;
     m_pAdvertisedDeviceCallbacks = pAdvertisedDeviceCallbacks;
 } // setAdvertisedDeviceCallbacks
 
@@ -248,7 +265,6 @@ bool NimBLEScan::start(uint32_t duration, void (*scanCompleteCB)(NimBLEScanResul
     }
 
     m_stopped = false;
-    m_semaphoreScanEnd.take("start");
 
     // Save the callback to be invoked when the scan completes.
     m_scanCompleteCB = scanCompleteCB;
@@ -274,7 +290,7 @@ bool NimBLEScan::start(uint32_t duration, void (*scanCompleteCB)(NimBLEScanResul
         rc = ble_gap_disc(m_own_addr_type, duration, &m_scan_params,
                                     NimBLEScan::handleGapEvent, this);
         if(rc == BLE_HS_EBUSY) {
-            vTaskDelay(2);
+            vTaskDelay(1 / portTICK_PERIOD_MS);
         }
     } while(rc == BLE_HS_EBUSY);
 
@@ -282,7 +298,6 @@ bool NimBLEScan::start(uint32_t duration, void (*scanCompleteCB)(NimBLEScanResul
         NIMBLE_LOGE(LOG_TAG, "Error initiating GAP discovery procedure; rc=%d, %s",
                                         rc, NimBLEUtils::returnCodeToString(rc));
         m_stopped = true;
-        m_semaphoreScanEnd.give();
         return false;
     }
 
@@ -297,9 +312,18 @@ bool NimBLEScan::start(uint32_t duration, void (*scanCompleteCB)(NimBLEScanResul
  * @return The BLEScanResults.
  */
 NimBLEScanResults NimBLEScan::start(uint32_t duration, bool is_continue) {
-    if(start(duration, nullptr, is_continue)) {
-        m_semaphoreScanEnd.wait("start");   // Wait for the semaphore to release.
+    if(duration == 0) {
+        NIMBLE_LOGW(LOG_TAG, "Blocking scan called with duration = forever");
     }
+
+    ble_task_data_t taskData = {nullptr, xTaskGetCurrentTaskHandle(),0, nullptr};
+    m_pTaskData = &taskData;
+
+    if(start(duration, nullptr, is_continue)) {
+        ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
+    }
+
+    m_pTaskData = nullptr;
     return m_scanResults;
 } // start
 
@@ -308,13 +332,13 @@ NimBLEScanResults NimBLEScan::start(uint32_t duration, bool is_continue) {
  * @brief Stop an in progress scan.
  * @return N/A.
  */
-void NimBLEScan::stop() {
+bool NimBLEScan::stop() {
     NIMBLE_LOGD(LOG_TAG, ">> stop()");
 
     int rc = ble_gap_disc_cancel();
     if (rc != 0 && rc != BLE_HS_EALREADY) {
         NIMBLE_LOGE(LOG_TAG, "Failed to cancel scan; rc=%d\n", rc);
-        return;
+        return false;
     }
 
     m_stopped = true;
@@ -323,9 +347,12 @@ void NimBLEScan::stop() {
         m_scanCompleteCB(m_scanResults);
     }
 
-    m_semaphoreScanEnd.give();
+    if(m_pTaskData != nullptr) {
+        xTaskNotifyGive(m_pTaskData->task);
+    }
 
     NIMBLE_LOGD(LOG_TAG, "<< stop()");
+    return true;
 } // stop
 
 
@@ -349,7 +376,6 @@ void NimBLEScan::erase(const NimBLEAddress &address) {
  */
 void NimBLEScan::onHostReset() {
     m_stopped = true;
-    m_semaphoreScanEnd.give();
 }
 
 
diff --git a/libesp32/NimBLE-Arduino/src/NimBLEScan.h b/libesp32/NimBLE-Arduino/src/NimBLEScan.h
index 3cab25784..5bc7bcf35 100644
--- a/libesp32/NimBLE-Arduino/src/NimBLEScan.h
+++ b/libesp32/NimBLE-Arduino/src/NimBLEScan.h
@@ -20,7 +20,7 @@
 #if defined(CONFIG_BT_NIMBLE_ROLE_OBSERVER)
 
 #include "NimBLEAdvertisedDevice.h"
-#include "FreeRTOS.h"
+#include "NimBLEUtils.h"
 
 #include "host/ble_gap.h"
 
@@ -62,11 +62,14 @@ class NimBLEScan {
 public:
     bool                start(uint32_t duration, void (*scanCompleteCB)(NimBLEScanResults), bool is_continue = false);
     NimBLEScanResults   start(uint32_t duration, bool is_continue = false);
-    void                setAdvertisedDeviceCallbacks(NimBLEAdvertisedDeviceCallbacks* pAdvertisedDeviceCallbacks/*, bool wantDuplicates = false*/);
+    void                setAdvertisedDeviceCallbacks(NimBLEAdvertisedDeviceCallbacks* pAdvertisedDeviceCallbacks, bool wantDuplicates = false);
     void                setActiveScan(bool active);
     void                setInterval(uint16_t intervalMSecs);
     void                setWindow(uint16_t windowMSecs);
-    void                stop();
+    void                setDuplicateFilter(bool active);
+    void                setLimitedOnly(bool active);
+    void                setFilterPolicy(uint8_t filter);
+    bool                stop();
     void                clearResults();
     NimBLEScanResults   getResults();
     void                erase(const NimBLEAddress &address);
@@ -85,8 +88,8 @@ private:
     bool                                m_stopped;
     bool                                m_wantDuplicates;
     NimBLEScanResults                   m_scanResults;
-    FreeRTOS::Semaphore                 m_semaphoreScanEnd = FreeRTOS::Semaphore("ScanEnd");
     uint32_t                            m_duration;
+    ble_task_data_t                     *m_pTaskData;
 };
 
 #endif // #if defined(CONFIG_BT_NIMBLE_ROLE_OBSERVER)
diff --git a/libesp32/NimBLE-Arduino/src/NimBLEServer.cpp b/libesp32/NimBLE-Arduino/src/NimBLEServer.cpp
index d731bad5b..376b02144 100644
--- a/libesp32/NimBLE-Arduino/src/NimBLEServer.cpp
+++ b/libesp32/NimBLE-Arduino/src/NimBLEServer.cpp
@@ -19,8 +19,6 @@
 #if defined(CONFIG_BT_NIMBLE_ROLE_PERIPHERAL)
 
 #include "NimBLEServer.h"
-#include "NimBLE2902.h"
-#include "NimBLEUtils.h"
 #include "NimBLEDevice.h"
 #include "NimBLELog.h"
 
@@ -32,51 +30,46 @@ static NimBLEServerCallbacks defaultCallbacks;
  * @brief Construct a %BLE Server
  *
  * This class is not designed to be individually instantiated.  Instead one should create a server by asking
- * the BLEDevice class.
+ * the NimBLEDevice class.
  */
 NimBLEServer::NimBLEServer() {
-    m_connId           = BLE_HS_CONN_HANDLE_NONE;
-    m_svcChgChrHdl     = 0xffff;
-    m_pServerCallbacks = &defaultCallbacks;
-    m_gattsStarted     = false;
-} // BLEServer
+//    m_svcChgChrHdl          = 0xffff; // Future Use
+    m_pServerCallbacks      = &defaultCallbacks;
+    m_gattsStarted          = false;
+    m_advertiseOnDisconnect = true;
+} // NimBLEServer
 
 
 /**
  * @brief Create a %BLE Service.
- *
- * With a %BLE server, we can host one or more services.  Invoking this function causes the creation of a definition
- * of a new service.  Every service must have a unique UUID.
  * @param [in] uuid The UUID of the new service.
  * @return A reference to the new service object.
  */
 NimBLEService* NimBLEServer::createService(const char* uuid) {
     return createService(NimBLEUUID(uuid));
-}
+} // createService
 
 
 /**
  * @brief Create a %BLE Service.
- *
- * With a %BLE server, we can host one or more services.  Invoking this function causes the creation of a definition
- * of a new service.  Every service must have a unique UUID.
  * @param [in] uuid The UUID of the new service.
  * @param [in] numHandles The maximum number of handles associated with this service.
- * @param [in] inst_id With multiple services with the same UUID we need to provide inst_id value different for each service.
+ * @param [in] inst_id if we have multiple services with the same UUID we need
+ *             to provide inst_id value different for each service.
  * @return A reference to the new service object.
  */
 NimBLEService* NimBLEServer::createService(const NimBLEUUID &uuid, uint32_t numHandles, uint8_t inst_id) {
     NIMBLE_LOGD(LOG_TAG, ">> createService - %s", uuid.toString().c_str());
-
+    // TODO: add functionality to use inst_id for multiple services with same uuid
+    (void)inst_id;
     // Check that a service with the supplied UUID does not already exist.
-    if (m_serviceMap.getByUUID(uuid) != nullptr) {
-        NIMBLE_LOGW(LOG_TAG, "<< Attempt to create a new service with uuid %s but a service with that UUID already exists.",
-            uuid.toString().c_str());
+    if(getServiceByUUID(uuid) != nullptr) {
+        NIMBLE_LOGW(LOG_TAG, "Warning creating a duplicate service UUID: %s",
+                             std::string(uuid).c_str());
     }
 
     NimBLEService* pService = new NimBLEService(uuid, numHandles, this);
-    pService->m_instId = inst_id;
-    m_serviceMap.setByUUID(uuid, pService); // Save a reference to this service being on this server.
+    m_svcVec.push_back(pService); // Save a reference to this service being on this server.
 
     NIMBLE_LOGD(LOG_TAG, "<< createService");
     return pService;
@@ -89,8 +82,8 @@ NimBLEService* NimBLEServer::createService(const NimBLEUUID &uuid, uint32_t numH
  * @return A reference to the service object.
  */
 NimBLEService* NimBLEServer::getServiceByUUID(const char* uuid) {
-    return m_serviceMap.getByUUID(uuid);
-}
+    return getServiceByUUID(NimBLEUUID(uuid));
+} // getServiceByUUID
 
 
 /**
@@ -99,8 +92,13 @@ NimBLEService* NimBLEServer::getServiceByUUID(const char* uuid) {
  * @return A reference to the service object.
  */
 NimBLEService* NimBLEServer::getServiceByUUID(const NimBLEUUID &uuid) {
-    return m_serviceMap.getByUUID(uuid);
-}
+    for (auto &it : m_svcVec) {
+        if (it->getUUID() == uuid) {
+            return it;
+        }
+    }
+    return nullptr;
+} // getServiceByUUID
 
 
 /**
@@ -109,18 +107,8 @@ NimBLEService* NimBLEServer::getServiceByUUID(const NimBLEUUID &uuid) {
  * @return An advertising object.
  */
 NimBLEAdvertising* NimBLEServer::getAdvertising() {
-    return BLEDevice::getAdvertising();
-}
-
-
-/**
- * @brief Retrieve the connection id of the last connected client.
- * @todo Not very useful, should refactor or remove.
- * @return Client connection id.
- */
-uint16_t NimBLEServer::getConnId() {
-    return m_connId;
-}
+    return NimBLEDevice::getAdvertising();
+} // getAdvertising
 
 
 /**
@@ -143,6 +131,8 @@ void NimBLEServer::start() {
 #if CONFIG_LOG_DEFAULT_LEVEL > 3 || (ARDUINO_ARCH_ESP32 && CORE_DEBUG_LEVEL >= 4)
     ble_gatts_show_local();
 #endif
+/*** Future use ***
+  * TODO: implement service changed handling
 
     ble_uuid16_t svc = {BLE_UUID_TYPE_16, 0x1801};
     ble_uuid16_t chr = {BLE_UUID_TYPE_16, 0x2a05};
@@ -155,36 +145,25 @@ 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
+    for(auto &svc : m_svcVec) {
+        for(auto &chr : svc->m_chrVec) {
+            // if Notify / Indicate is enabled but we didn't create the descriptor
+            // we do it now.
+            if((chr->m_properties & BLE_GATT_CHR_F_INDICATE) ||
+                (chr->m_properties & BLE_GATT_CHR_F_NOTIFY)) {
 
-    // Build a map of characteristics with Notify / Indicate capabilities for event handling
-    uint8_t numSvcs = m_serviceMap.getRegisteredServiceCount();
-    NimBLEService* pService = m_serviceMap.getFirst();
-
-    for(int i = 0; i < numSvcs; i++) {
-        uint8_t numChrs = pService->m_characteristicMap.getSize();
-        NimBLECharacteristic* pChr = pService->m_characteristicMap.getFirst();
-
-        if(pChr != nullptr) {
-            for( int d = 0; d < numChrs; d++) {
-                // if Notify / Indicate is enabled but we didn't create the descriptor
-                // we do it now.
-                if((pChr->m_properties & BLE_GATT_CHR_F_INDICATE) ||
-                    (pChr->m_properties & BLE_GATT_CHR_F_NOTIFY)) {
-
-                    if(nullptr == pChr->getDescriptorByUUID("2902")) {
-                        pChr->createDescriptor("2902");
-                    }
-                    m_notifyChrMap.insert(std::pair<uint16_t, NimBLECharacteristic*>
-                                                    (pChr->getHandle(), pChr));
+                if(nullptr == chr->getDescriptorByUUID(uint16_t(0x2902))) {
+                    chr->createDescriptor(uint16_t(0x2902));
                 }
-                pChr = pService->m_characteristicMap.getNext();
+                m_notifyChrVec.push_back(chr);
             }
         }
-        pService = m_serviceMap.getNext();
     }
 
     m_gattsStarted = true;
-}
+} // start
 
 
 /**
@@ -202,17 +181,26 @@ int NimBLEServer::disconnect(uint16_t connId, uint8_t reason) {
                                     NimBLEUtils::returnCodeToString(rc));
     }
 
-    return rc;
     NIMBLE_LOGD(LOG_TAG, "<< disconnect()");
-}
+    return rc;
+} // disconnect
+
+
+/**
+ * @brief Set the server to automatically start advertising when a client disconnects.
+ * @param [in] bool true == advertise, false == don't advertise.
+ */
+void NimBLEServer::advertiseOnDisconnect(bool aod) {
+    m_advertiseOnDisconnect = aod;
+} // advertiseOnDisconnect
 
 
 /**
  * @brief Return the number of connected clients.
  * @return The number of connected clients.
  */
-uint32_t NimBLEServer::getConnectedCount() {
-    return m_connectedServersMap.size();
+size_t NimBLEServer::getConnectedCount() {
+    return m_connectedPeersVec.size();
 } // getConnectedCount
 
 
@@ -227,7 +215,7 @@ uint32_t NimBLEServer::getConnectedCount() {
 /*STATIC*/int NimBLEServer::handleGapEvent(struct ble_gap_event *event, void *arg) {
     NimBLEServer* server = (NimBLEServer*)arg;
     NIMBLE_LOGD(LOG_TAG, ">> handleGapEvent: %s",
-        NimBLEUtils::gapEventToString(event->type));
+                         NimBLEUtils::gapEventToString(event->type));
     int rc = 0;
     struct ble_gap_conn_desc desc;
 
@@ -236,15 +224,12 @@ uint32_t NimBLEServer::getConnectedCount() {
         case BLE_GAP_EVENT_CONNECT: {
             if (event->connect.status != 0) {
                 /* Connection failed; resume advertising */
-                NIMBLE_LOGC(LOG_TAG, "Connection failed");
+                NIMBLE_LOGE(LOG_TAG, "Connection failed");
                 NimBLEDevice::startAdvertising();
-                server->m_connId = BLE_HS_CONN_HANDLE_NONE;
             }
             else {
-                server->m_connId = event->connect.conn_handle;
-                server->addPeerDevice((void*)server, false, server->m_connId);
+                server->m_connectedPeersVec.push_back(event->connect.conn_handle);
 
-                ble_gap_conn_desc desc;
                 rc = ble_gap_conn_find(event->connect.conn_handle, &desc);
                 assert(rc == 0);
 
@@ -271,10 +256,15 @@ uint32_t NimBLEServer::getConnectedCount() {
                     break;
             }
 
-            server->removePeerDevice(event->disconnect.conn.conn_handle, false);
-            server->m_connId = BLE_HS_CONN_HANDLE_NONE;
+            server->m_connectedPeersVec.erase(std::remove(server->m_connectedPeersVec.begin(),
+                                                          server->m_connectedPeersVec.end(),
+                                                          event->disconnect.conn.conn_handle),
+                                                          server->m_connectedPeersVec.end());
             server->m_pServerCallbacks->onDisconnect(server);
 
+            if(server->m_advertiseOnDisconnect) {
+                server->startAdvertising();
+            }
             return 0;
         } // BLE_GAP_EVENT_DISCONNECT
 
@@ -283,9 +273,23 @@ uint32_t NimBLEServer::getConnectedCount() {
                               "val_handle=%d\n",
                         event->subscribe.cur_notify, event->subscribe.attr_handle);
 
-            auto it = server->m_notifyChrMap.find(event->subscribe.attr_handle);
-            if(it != server->m_notifyChrMap.cend()) {
-                (*it).second->setSubscribe(event);
+            for(auto &it : server->m_notifyChrVec) {
+                if(it->getHandle() == event->subscribe.attr_handle) {
+                    if((it->getProperties() & BLE_GATT_CHR_F_READ_AUTHEN) ||
+                       (it->getProperties() & BLE_GATT_CHR_F_READ_AUTHOR) ||
+                       (it->getProperties() & BLE_GATT_CHR_F_READ_ENC))
+                    {
+                        rc = ble_gap_conn_find(event->subscribe.conn_handle, &desc);
+                        assert(rc == 0);
+
+                        if(!desc.sec_state.encrypted) {
+                            NimBLEDevice::startSecurity(event->subscribe.conn_handle);
+                        }
+                    }
+
+                    it->setSubscribe(event);
+                    break;
+                }
             }
 
             return 0;
@@ -295,15 +299,19 @@ uint32_t NimBLEServer::getConnectedCount() {
             NIMBLE_LOGI(LOG_TAG, "mtu update event; conn_handle=%d mtu=%d",
                         event->mtu.conn_handle,
                         event->mtu.value);
-            server->updatePeerMTU(event->mtu.conn_handle, event->mtu.value);
             return 0;
         } // BLE_GAP_EVENT_MTU
 
         case BLE_GAP_EVENT_NOTIFY_TX: {
             if(event->notify_tx.indication && event->notify_tx.status != 0) {
-                auto it = server->m_notifyChrMap.find(event->notify_tx.attr_handle);
-                if(it != server->m_notifyChrMap.cend()) {
-                    (*it).second->m_semaphoreConfEvt.give(event->notify_tx.status);
+                for(auto &it : server->m_notifyChrVec) {
+                    if(it->getHandle() == event->notify_tx.attr_handle) {
+                        if(it->m_pTaskData != nullptr) {
+                            it->m_pTaskData->rc = event->notify_tx.status;
+                            xTaskNotifyGive(it->m_pTaskData->task);
+                        }
+                        break;
+                    }
                 }
             }
 
@@ -333,7 +341,7 @@ uint32_t NimBLEServer::getConnectedCount() {
         } // BLE_GAP_EVENT_REPEAT_PAIRING
 
         case BLE_GAP_EVENT_ENC_CHANGE: {
-            rc = ble_gap_conn_find(event->conn_update.conn_handle, &desc);
+            rc = ble_gap_conn_find(event->enc_change.conn_handle, &desc);
             if(rc != 0) {
                 return BLE_ATT_ERR_INVALID_HANDLE;
             }
@@ -349,7 +357,7 @@ uint32_t NimBLEServer::getConnectedCount() {
         } // BLE_GAP_EVENT_ENC_CHANGE
 
         case BLE_GAP_EVENT_PASSKEY_ACTION: {
-            struct ble_sm_io pkey = {0};
+            struct ble_sm_io pkey = {0,0};
 
             if (event->passkey.params.action == BLE_SM_IOACT_DISP) {
                 pkey.action = event->passkey.params.action;
@@ -416,7 +424,7 @@ uint32_t NimBLEServer::getConnectedCount() {
 
     NIMBLE_LOGD(LOG_TAG, "<< handleGATTServerEvent");
     return 0;
-} // handleGATTServerEvent
+} // handleGapEvent
 
 
 /**
@@ -437,18 +445,6 @@ void NimBLEServer::setCallbacks(NimBLEServerCallbacks* pCallbacks) {
 } // setCallbacks
 
 
-/*
- * Remove service
- */
-/*
-void BLEServer::removeService(BLEService* service) {
-    service->stop();
-    service->executeDelete();
-    m_serviceMap.removeService(service);
-}
-*/
-
-
 /**
  * @brief Start advertising.
  *
@@ -456,9 +452,7 @@ void BLEServer::removeService(BLEService* service) {
  * retrieving the advertising object and invoking start upon it.
  */
 void NimBLEServer::startAdvertising() {
-    NIMBLE_LOGD(LOG_TAG, ">> startAdvertising");
     NimBLEDevice::startAdvertising();
-    NIMBLE_LOGD(LOG_TAG, "<< startAdvertising");
 } // startAdvertising
 
 
@@ -466,38 +460,43 @@ void NimBLEServer::startAdvertising() {
  * @brief Stop advertising.
  */
 void NimBLEServer::stopAdvertising() {
-    NIMBLE_LOGD(LOG_TAG, ">> stopAdvertising");
     NimBLEDevice::stopAdvertising();
-    NIMBLE_LOGD(LOG_TAG, "<< stopAdvertising");
 } // startAdvertising
 
 
 /**
- * Allow to connect GATT server to peer device
- * Probably can be used in ANCS for iPhone
+ * @brief Get the MTU of the client.
+ * @returns The client MTU or 0 if not found/connected.
  */
- /*
-bool BLEServer::connect(BLEAddress address) {
-    esp_bd_addr_t addr;
-    memcpy(&addr, address.getNative(), 6);
-    // Perform the open connection request against the target BLE Server.
-    m_semaphoreOpenEvt.take("connect");
-    esp_err_t errRc = ::esp_ble_gatts_open(
-        getGattsIf(),
-        addr, // address
-        1                              // direct connection
-    );
-    if (errRc != ESP_OK) {
-        ESP_LOGE(LOG_TAG, "esp_ble_gattc_open: rc=%d %s", errRc, GeneralUtils::errorToString(errRc));
-        return false;
+uint16_t NimBLEServer::getPeerMTU(uint16_t conn_id) {
+    return ble_att_mtu(conn_id);
+} //getPeerMTU
+
+
+/**
+ * Update connection parameters can be called only after connection has been established
+ */
+void NimBLEServer::updateConnParams(uint16_t conn_handle,
+                            uint16_t minInterval, uint16_t maxInterval,
+                            uint16_t latency, uint16_t timeout)
+{
+    ble_gap_upd_params params;
+
+    params.latency  = latency;
+    params.itvl_max = maxInterval;    // max_int = 0x20*1.25ms = 40ms
+    params.itvl_min = minInterval;    // min_int = 0x10*1.25ms = 20ms
+    params.supervision_timeout = timeout;    // timeout = 400*10ms = 4000ms
+    params.min_ce_len = BLE_GAP_INITIAL_CONN_MIN_CE_LEN;  // Minimum length of connection event in 0.625ms units
+    params.max_ce_len = BLE_GAP_INITIAL_CONN_MAX_CE_LEN;  // Maximum length of connection event in 0.625ms units
+
+    int rc = ble_gap_update_params(conn_handle, &params);
+    if(rc != 0) {
+        NIMBLE_LOGE(LOG_TAG, "Update params error: %d, %s", rc, NimBLEUtils::returnCodeToString(rc));
     }
+} // updateConnParams
 
-    uint32_t rc = m_semaphoreOpenEvt.wait("connect");   // Wait for the connection to complete.
-    ESP_LOGD(LOG_TAG, "<< connect(), rc=%d", rc==ESP_GATT_OK);
-    return rc == ESP_GATT_OK;
-} // connect
-*/
 
+/** Default callback handlers */
 
 void NimBLEServerCallbacks::onConnect(NimBLEServer* pServer) {
     NIMBLE_LOGD("NimBLEServerCallbacks", "onConnect(): Default");
@@ -534,80 +533,6 @@ bool NimBLEServerCallbacks::onConfirmPIN(uint32_t pin){
     return true;
 }
 
-/* multi connect support */
-void NimBLEServer::updatePeerMTU(uint16_t conn_id, uint16_t mtu) {
-    const std::map<uint16_t, conn_status_t>::iterator it = m_connectedServersMap.find(conn_id);
-    if (it != m_connectedServersMap.end()) {
-        it->second.mtu = mtu;
-    }
-}
-
-std::map<uint16_t, conn_status_t> NimBLEServer::getPeerDevices() {
-    return m_connectedServersMap;
-}
-
-
-/**
- * @brief Get the MTU of the client.
- * @returns The client MTU or 0 if not found/connected.
- */
-uint16_t NimBLEServer::getPeerMTU(uint16_t conn_id) {
-    auto it = m_connectedServersMap.find(conn_id);
-    if(it != m_connectedServersMap.cend()) {
-        return (*it).second.mtu;
-    } else {
-        return 0;
-    }
-}
-
-void NimBLEServer::addPeerDevice(void* peer, bool _client, uint16_t conn_id) {
-    conn_status_t status = {
-        .peer_device = peer,
-        .connected = true,
-        .mtu = 23
-    };
-
-    m_connectedServersMap.insert(std::pair<uint16_t, conn_status_t>(conn_id, status));
-}
-
-void NimBLEServer::removePeerDevice(uint16_t conn_id, bool _client) {
-    m_connectedServersMap.erase(conn_id);
-}
-/* multi connect support */
-
-
-/**
- * Update connection parameters can be called only after connection has been established
- */
-void NimBLEServer::updateConnParams(uint16_t conn_handle,
-                            uint16_t minInterval, uint16_t maxInterval,
-                            uint16_t latency, uint16_t timeout,
-                            uint16_t minConnTime, uint16_t maxConnTime)
-{
-    ble_gap_upd_params params;
-
-    params.latency  = latency;
-    params.itvl_max = maxInterval;    // max_int = 0x20*1.25ms = 40ms
-    params.itvl_min = minInterval;    // min_int = 0x10*1.25ms = 20ms
-    params.supervision_timeout = timeout;    // timeout = 400*10ms = 4000ms
-    params.min_ce_len = minConnTime;  // Minimum length of connection event in 0.625ms units
-    params.max_ce_len = maxConnTime;  // Maximum length of connection event in 0.625ms units
-
-    int rc = ble_gap_update_params(conn_handle, &params);
-    if(rc != 0) {
-        NIMBLE_LOGE(LOG_TAG, "Update params error: %d, %s", rc, NimBLEUtils::returnCodeToString(rc));
-    }
-}
-
-/* Don't think this is needed
-
-void NimBLEServer::onHostReset() {
-    for(auto it = m_notifyChrMap.cbegin(); it != m_notifyChrMap.cend(); ++it) {
-        (*it).second->m_semaphoreConfEvt.give(0);
-    }
-
-}
-*/
 
 #endif // #if defined(CONFIG_BT_NIMBLE_ROLE_PERIPHERAL)
 #endif // CONFIG_BT_ENABLED
diff --git a/libesp32/NimBLE-Arduino/src/NimBLEServer.h b/libesp32/NimBLE-Arduino/src/NimBLEServer.h
index 903eb2392..cfaa8acc7 100644
--- a/libesp32/NimBLE-Arduino/src/NimBLEServer.h
+++ b/libesp32/NimBLE-Arduino/src/NimBLEServer.h
@@ -20,103 +20,60 @@
 #include "nimconfig.h"
 #if defined(CONFIG_BT_NIMBLE_ROLE_PERIPHERAL)
 
+#include "NimBLEUtils.h"
 #include "NimBLEAddress.h"
-#include "NimBLEUUID.h"
 #include "NimBLEAdvertising.h"
 #include "NimBLEService.h"
 #include "NimBLESecurity.h"
-#include "FreeRTOS.h"
 
-#include <map>
 
 class NimBLEService;
 class NimBLECharacteristic;
 class NimBLEServerCallbacks;
 
-/* TODO possibly refactor this struct */
-typedef struct {
-    void *peer_device;      // peer device BLEClient or BLEServer - maybe its better to have 2 structures or union here
-    bool connected;         // do we need it?
-    uint16_t mtu;           // every peer device negotiate own mtu
-} conn_status_t;
-
-
-/**
- * @brief A data structure that manages the %BLE servers owned by a BLE server.
- */
-class NimBLEServiceMap {
-public:
-//    NimBLEService* getByHandle(uint16_t handle);
-    NimBLEService* getByUUID(const char* uuid);
-    NimBLEService* getByUUID(const NimBLEUUID &uuid, uint8_t inst_id = 0);
-//    void           setByHandle(uint16_t handle, NimBLEService* service);
-    void           setByUUID(const char* uuid, NimBLEService* service);
-    void           setByUUID(const NimBLEUUID &uuid, NimBLEService* service);
-    std::string    toString();
-    NimBLEService* getFirst();
-    NimBLEService* getNext();
-    void           removeService(NimBLEService *service);
-    int            getRegisteredServiceCount();
-
-private:
-//    std::map<uint16_t, NimBLEService*>    m_handleMap;
-    std::map<NimBLEService*, std::string> m_uuidMap;
-    std::map<NimBLEService*, std::string>::iterator m_iterator;
-};
-
 
 /**
  * @brief The model of a %BLE server.
  */
 class NimBLEServer {
 public:
-    uint32_t              getConnectedCount();
-    NimBLEService*        createService(const char* uuid);
-    NimBLEService*        createService(const NimBLEUUID &uuid, uint32_t numHandles=15, uint8_t inst_id=0);
-    NimBLEAdvertising*    getAdvertising();
-    void                  setCallbacks(NimBLEServerCallbacks* pCallbacks);
-    void                  startAdvertising();
-    void                  stopAdvertising();
-    void                  start();
-//    void                  removeService(BLEService* service);
-    NimBLEService*        getServiceByUUID(const char* uuid);
-    NimBLEService*        getServiceByUUID(const NimBLEUUID &uuid);
-    int                   disconnect(uint16_t connID, uint8_t reason = BLE_ERR_REM_USER_CONN_TERM);
-//    bool                connect(BLEAddress address);
-    void                  updateConnParams(uint16_t conn_handle,
-                                    uint16_t minInterval, uint16_t maxInterval,
-                                    uint16_t latency, uint16_t timeout,
-                                    uint16_t minConnTime=0, uint16_t maxConnTime=0);
-
-    /* multi connection support */
-    std::map<uint16_t, conn_status_t> getPeerDevices();
-    void          addPeerDevice(void* peer, bool is_client, uint16_t conn_id);
-    void          removePeerDevice(uint16_t conn_id, bool client);
-    NimBLEServer* getServerByConnId(uint16_t conn_id);
-    void          updatePeerMTU(uint16_t connId, uint16_t mtu);
-    uint16_t      getPeerMTU(uint16_t conn_id);
-    uint16_t      getConnId();
-
+    size_t                 getConnectedCount();
+    NimBLEService*         createService(const char* uuid);
+    NimBLEService*         createService(const NimBLEUUID &uuid, uint32_t numHandles=15,
+                                         uint8_t inst_id=0);
+    NimBLEAdvertising*     getAdvertising();
+    void                   setCallbacks(NimBLEServerCallbacks* pCallbacks);
+    void                   startAdvertising();
+    void                   stopAdvertising();
+    void                   start();
+    NimBLEService*         getServiceByUUID(const char* uuid);
+    NimBLEService*         getServiceByUUID(const NimBLEUUID &uuid);
+    int                    disconnect(uint16_t connID,
+                                      uint8_t reason = BLE_ERR_REM_USER_CONN_TERM);
+    void                   updateConnParams(uint16_t conn_handle,
+                                            uint16_t minInterval, uint16_t maxInterval,
+                                            uint16_t latency, uint16_t timeout);
+    uint16_t               getPeerMTU(uint16_t conn_id);
+    std::vector<uint16_t>  getPeerDevices();
+    void                   advertiseOnDisconnect(bool);
 
 private:
     NimBLEServer();
-    //friend class BLEService;
-    friend class NimBLECharacteristic;
-    friend class NimBLEDevice;
-    friend class NimBLEAdvertising;
- //   void         onHostReset();
-    // BLEAdvertising      m_bleAdvertising;
-    uint16_t               m_connId;
-    uint16_t               m_svcChgChrHdl;
+    friend class           NimBLECharacteristic;
+    friend class           NimBLEDevice;
+    friend class           NimBLEAdvertising;
+
     bool                   m_gattsStarted;
-
-    std::map<uint16_t, conn_status_t> m_connectedServersMap;
-    std::map<uint16_t, NimBLECharacteristic*> m_notifyChrMap;
-
-    NimBLEServiceMap       m_serviceMap;
+    bool                   m_advertiseOnDisconnect;
     NimBLEServerCallbacks* m_pServerCallbacks;
+    std::vector<uint16_t>  m_connectedPeersVec;
 
-    static int      handleGapEvent(struct ble_gap_event *event, void *arg);
+//    uint16_t               m_svcChgChrHdl; // Future use
+
+    std::vector<NimBLEService*> m_svcVec;
+    std::vector<NimBLECharacteristic*> m_notifyChrVec;
+
+    static int             handleGapEvent(struct ble_gap_event *event, void *arg);
 }; // NimBLEServer
 
 
@@ -149,7 +106,7 @@ public:
     virtual bool onSecurityRequest(); //{return true;}
     virtual void onAuthenticationComplete(ble_gap_conn_desc* desc);//{};
     virtual bool onConfirmPIN(uint32_t pin);//{return true;}
-}; // BLEServerCallbacks
+}; // NimBLEServerCallbacks
 
 
 #endif // #if defined(CONFIG_BT_NIMBLE_ROLE_PERIPHERAL)
diff --git a/libesp32/NimBLE-Arduino/src/NimBLEService.cpp b/libesp32/NimBLE-Arduino/src/NimBLEService.cpp
index 6036a38ad..c2631ab36 100644
--- a/libesp32/NimBLE-Arduino/src/NimBLEService.cpp
+++ b/libesp32/NimBLE-Arduino/src/NimBLEService.cpp
@@ -32,9 +32,10 @@ static const char* LOG_TAG = "NimBLEService"; // Tag for logging.
 
 
 /**
- * @brief Construct an instance of the BLEService
+ * @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.
  */
 NimBLEService::NimBLEService(const char* uuid, uint16_t numHandles, NimBLEServer* pServer)
 : NimBLEService(NimBLEUUID(uuid), numHandles, pServer) {
@@ -45,11 +46,12 @@ 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.
  */
 NimBLEService::NimBLEService(const NimBLEUUID &uuid, uint16_t numHandles, NimBLEServer* pServer) {
-    m_uuid      = uuid;
-    m_handle    = NULL_HANDLE;
-    m_pServer   = pServer;
+    m_uuid       = uuid;
+    m_handle     = NULL_HANDLE;
+    m_pServer    = pServer;
     m_numHandles = numHandles;
 } // NimBLEService
 
@@ -59,10 +61,22 @@ NimBLEService::NimBLEService(const NimBLEUUID &uuid, uint16_t numHandles, NimBLE
  * @return N/A.
  */
 void NimBLEService::dump() {
-    NIMBLE_LOGD(LOG_TAG, "Service: uuid:%s, handle: 0x%.2x",
+    NIMBLE_LOGD(LOG_TAG, "Service: uuid:%s, handle: 0x%2x",
         m_uuid.toString().c_str(),
         m_handle);
-    NIMBLE_LOGD(LOG_TAG, "Characteristics:\n%s", m_characteristicMap.toString().c_str());
+
+    std::string res;
+    int count = 0;
+    char hex[5];
+    for (auto &it: m_chrVec) {
+        if (count > 0) {res += "\n";}
+        snprintf(hex, sizeof(hex), "%04x", it->getHandle());
+        count++;
+        res += "handle: 0x";
+        res += hex;
+        res += ", uuid: " + std::string(it->getUUID());
+    }
+    NIMBLE_LOGD(LOG_TAG, "Characteristics:\n%s", res.c_str());
 } // dump
 
 
@@ -76,10 +90,9 @@ NimBLEUUID NimBLEService::getUUID() {
 
 
 /**
- * @brief Start the service.
- * Here we wish to start the service which means that we will respond to partner requests about it.
- * Starting a service also means that we can create the corresponding characteristics.
- * @return Start the service.
+ * @brief Builds the database of characteristics/descriptors for the service
+ * and registers it with the NimBLE stack.
+ * @return bool success/failure .
  */
 
 bool NimBLEService::start() {
@@ -96,7 +109,7 @@ bool NimBLEService::start() {
     svc[0].uuid = &m_uuid.getNative()->u;
     svc[0].includes = NULL;
 
-    uint8_t numChrs = m_characteristicMap.getSize();
+    size_t numChrs = m_chrVec.size();
 
     NIMBLE_LOGD(LOG_TAG,"Adding %d characteristics for service %s", numChrs, toString().c_str());
 
@@ -107,16 +120,17 @@ 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_characteristicMap.getFirst();
+        NimBLECharacteristic* pCharacteristic = *m_chrVec.begin();
 
-        for(uint8_t i=0; i < numChrs; i++) {
-            uint8_t numDscs = pCharacteristic->m_descriptorMap.getSize();
+        for(uint8_t i=0; i < numChrs;) {
+            uint8_t numDscs = pCharacteristic->m_dscVec.size();
             if(numDscs) {
                 // skip 2902 as it's automatically created by NimBLE
                 // if Indicate or Notify flags are set
                 if(((pCharacteristic->m_properties & BLE_GATT_CHR_F_INDICATE) ||
                     (pCharacteristic->m_properties & BLE_GATT_CHR_F_NOTIFY))  &&
-                     pCharacteristic->getDescriptorByUUID("2902") != nullptr) {
+                     pCharacteristic->getDescriptorByUUID("2902") != nullptr)
+                {
                     numDscs--;
                 }
             }
@@ -127,12 +141,12 @@ bool NimBLEService::start() {
                 // Must have last descriptor uuid = 0 so we have to create 1 extra
                 //NIMBLE_LOGD(LOG_TAG, "Adding %d descriptors", numDscs);
                 pDsc_a = new ble_gatt_dsc_def[numDscs+1];
-                NimBLEDescriptor* pDescriptor = pCharacteristic->m_descriptorMap.getFirst();
+                NimBLEDescriptor* pDescriptor = *pCharacteristic->m_dscVec.begin();
                 for(uint8_t d=0; d < numDscs;) {
                     // skip 2902
-                    if(pDescriptor->m_uuid.equals(NimBLEUUID((uint16_t)0x2902))) {
+                    if(pDescriptor->m_uuid == NimBLEUUID(uint16_t(0x2902))) {
                         //NIMBLE_LOGD(LOG_TAG, "Skipped 0x2902");
-                        pDescriptor = pCharacteristic->m_descriptorMap.getNext();
+                        pDescriptor = *(pCharacteristic->m_dscVec.begin()+d+1);
                         continue;
                     }
                     pDsc_a[d].uuid = &pDescriptor->m_uuid.getNative()->u;
@@ -140,8 +154,8 @@ bool NimBLEService::start() {
                     pDsc_a[d].min_key_size = 0;
                     pDsc_a[d].access_cb = NimBLEDescriptor::handleGapEvent;
                     pDsc_a[d].arg = pDescriptor;
-                    pDescriptor = pCharacteristic->m_descriptorMap.getNext();
                     d++;
+                    pDescriptor = *(pCharacteristic->m_dscVec.begin() + d);
                 }
 
                 pDsc_a[numDscs].uuid = NULL;
@@ -154,7 +168,8 @@ bool NimBLEService::start() {
             pChr_a[i].flags = pCharacteristic->m_properties;
             pChr_a[i].min_key_size = 0;
             pChr_a[i].val_handle = &pCharacteristic->m_handle;
-            pCharacteristic = m_characteristicMap.getNext();
+            i++;
+            pCharacteristic = *(m_chrVec.begin() + i);
         }
 
         pChr_a[numChrs].uuid = NULL;
@@ -182,21 +197,6 @@ bool NimBLEService::start() {
 } // start
 
 
-/**
- * @brief Set the handle associated with this service.
- * @param [in] handle The handle associated with the service.
- */
-void NimBLEService::setHandle(uint16_t handle) {
-    NIMBLE_LOGD(LOG_TAG, ">> setHandle - Handle=0x%.2x, service UUID=%s)", handle, getUUID().toString().c_str());
-    if (m_handle != NULL_HANDLE) {
-        NIMBLE_LOGE(LOG_TAG, "!!! Handle is already set %.2x", m_handle);
-        return;
-    }
-    m_handle = handle;
-    NIMBLE_LOGD(LOG_TAG, "<< setHandle");
-} // setHandle
-
-
 /**
  * @brief Get the handle associated with this service.
  * @return The handle associated with this service.
@@ -206,34 +206,6 @@ uint16_t NimBLEService::getHandle() {
 } // getHandle
 
 
-/**
- * @brief Add a characteristic to the service.
- * @param [in] pCharacteristic A pointer to the characteristic to be added.
- */
-void NimBLEService::addCharacteristic(NimBLECharacteristic* pCharacteristic) {
-    // We maintain a mapping of characteristics owned by this service.  These are managed by the
-    // BLECharacteristicMap class instance found in m_characteristicMap.  We add the characteristic
-    // to the map and then ask the service to add the characteristic at the BLE level (ESP-IDF).
-
-    NIMBLE_LOGD(LOG_TAG, ">> addCharacteristic()");
-    NIMBLE_LOGD(LOG_TAG, "Adding characteristic: uuid=%s to service: %s",
-        pCharacteristic->getUUID().toString().c_str(),
-        toString().c_str());
-
-    // Check that we don't add the same characteristic twice.
-    if (m_characteristicMap.getByUUID(pCharacteristic->getUUID()) != nullptr) {
-        NIMBLE_LOGW(LOG_TAG, "<< Adding a new characteristic with the same UUID as a previous one");
-        //return;
-    }
-
-    // Remember this characteristic in our map of characteristics.  At this point, we can lookup by UUID
-    // but not by handle.  The handle is allocated to us on the ESP_GATTS_ADD_CHAR_EVT.
-    m_characteristicMap.setByUUID(pCharacteristic, pCharacteristic->getUUID());
-
-    NIMBLE_LOGD(LOG_TAG, "<< addCharacteristic()");
-} // addCharacteristic
-
-
 /**
  * @brief Create a new BLE Characteristic associated with this service.
  * @param [in] uuid - The UUID of the characteristic.
@@ -253,8 +225,15 @@ NimBLECharacteristic* NimBLEService::createCharacteristic(const char* uuid, uint
  */
 NimBLECharacteristic* NimBLEService::createCharacteristic(const NimBLEUUID &uuid, uint32_t properties) {
     NimBLECharacteristic* pCharacteristic = new NimBLECharacteristic(uuid, properties, this);
-    addCharacteristic(pCharacteristic);
-    //pCharacteristic->executeCreate(this);
+    // Check that we don't add the same characteristic twice.
+    if (getCharacteristic(uuid) != nullptr) {
+        NIMBLE_LOGW(LOG_TAG, "<< Adding a duplicate characteristic with UUID: %s",
+                             std::string(uuid).c_str());
+    }
+
+    // Remember this characteristic in our vector of characteristics.
+    m_chrVec.push_back(pCharacteristic);
+
     return pCharacteristic;
 } // createCharacteristic
 
@@ -265,7 +244,13 @@ NimBLECharacteristic* NimBLEService::getCharacteristic(const char* uuid) {
 
 
 NimBLECharacteristic* NimBLEService::getCharacteristic(const NimBLEUUID &uuid) {
-    return m_characteristicMap.getByUUID(uuid);
+    for (auto &it : m_chrVec) {
+        if (it->getUUID() == uuid) {
+            return it;
+        }
+    }
+
+    return nullptr;
 }
 
 
diff --git a/libesp32/NimBLE-Arduino/src/NimBLEService.h b/libesp32/NimBLE-Arduino/src/NimBLEService.h
index 1cb0f6153..4c1fa2adf 100644
--- a/libesp32/NimBLE-Arduino/src/NimBLEService.h
+++ b/libesp32/NimBLE-Arduino/src/NimBLEService.h
@@ -20,37 +20,14 @@
 #include "nimconfig.h"
 #if defined(CONFIG_BT_NIMBLE_ROLE_PERIPHERAL)
 
-#include "NimBLECharacteristic.h"
 #include "NimBLEServer.h"
+#include "NimBLECharacteristic.h"
 #include "NimBLEUUID.h"
-#include "FreeRTOS.h"
 
 
 class NimBLEServer;
 class NimBLECharacteristic;
 
-/**
- * @brief A data mapping used to manage the set of %BLE characteristics known to the server.
- */
-class NimBLECharacteristicMap {
-public:
-    void setByUUID(NimBLECharacteristic* pCharacteristic, const char* uuid);
-    void setByUUID(NimBLECharacteristic* pCharacteristic, const NimBLEUUID &uuid);
-    void setByHandle(uint16_t handle, NimBLECharacteristic* pCharacteristic);
-    NimBLECharacteristic* getByUUID(const char* uuid);
-    NimBLECharacteristic* getByUUID(const NimBLEUUID &uuid);
-    NimBLECharacteristic* getByHandle(uint16_t handle);
-    NimBLECharacteristic* getFirst();
-    NimBLECharacteristic* getNext();
-    uint8_t getSize();
-    std::string toString();
-
-private:
-    std::map<NimBLECharacteristic*, std::string> m_uuidMap;
-    std::map<uint16_t, NimBLECharacteristic*> m_handleMap;
-    std::map<NimBLECharacteristic*, std::string>::iterator m_iterator;
-};
-
 
 /**
  * @brief The model of a %BLE service.
@@ -59,40 +36,39 @@ private:
 class NimBLEService {
 public:
     NimBLECharacteristic* createCharacteristic(const char* uuid,
-                        uint32_t properties = NIMBLE_PROPERTY::READ |
+                                              uint32_t properties =
+                                              NIMBLE_PROPERTY::READ |
                                               NIMBLE_PROPERTY::WRITE);
 
     NimBLECharacteristic* createCharacteristic(const NimBLEUUID &uuid,
-                        uint32_t properties = NIMBLE_PROPERTY::READ |
-                                              NIMBLE_PROPERTY::WRITE);
+                                               uint32_t properties =
+                                               NIMBLE_PROPERTY::READ |
+                                               NIMBLE_PROPERTY::WRITE);
 
-    void               dump();
+    void                  dump();
     NimBLECharacteristic* getCharacteristic(const char* uuid);
     NimBLECharacteristic* getCharacteristic(const NimBLEUUID &uuid);
     NimBLEUUID            getUUID();
     NimBLEServer*         getServer();
-    bool               start();
-//  void               stop();
-    std::string        toString();
-    uint16_t           getHandle();
-    uint8_t            m_instId = 0;
+    bool                  start();
+    std::string           toString();
+    uint16_t              getHandle();
 
 private:
     NimBLEService(const char* uuid, uint16_t numHandles, NimBLEServer* pServer);
     NimBLEService(const NimBLEUUID &uuid, uint16_t numHandles, NimBLEServer* pServer);
-    friend class NimBLEServer;
-    friend class NimBLEDevice;
 
-    void               addCharacteristic(NimBLECharacteristic* pCharacteristic);
+    friend class          NimBLEServer;
+    friend class          NimBLEDevice;
 
-    NimBLECharacteristicMap m_characteristicMap;
-    uint16_t             m_handle;
-    NimBLEServer*           m_pServer = nullptr;
-    NimBLEUUID              m_uuid;
+    uint16_t              m_handle;
+    NimBLEServer*         m_pServer;
+    NimBLEUUID            m_uuid;
+    uint16_t              m_numHandles;
 
-    uint16_t             m_numHandles;
-    void               setHandle(uint16_t handle);
-}; // BLEService
+    std::vector<NimBLECharacteristic*> m_chrVec;
+
+}; // NimBLEService
 
 
 #endif // #if defined(CONFIG_BT_NIMBLE_ROLE_PERIPHERAL)
diff --git a/libesp32/NimBLE-Arduino/src/NimBLEServiceMap.cpp b/libesp32/NimBLE-Arduino/src/NimBLEServiceMap.cpp
deleted file mode 100644
index e5b96e676..000000000
--- a/libesp32/NimBLE-Arduino/src/NimBLEServiceMap.cpp
+++ /dev/null
@@ -1,145 +0,0 @@
-/*
- * NimBLEService.cpp
- *
- *  Created: on March 7, 2020
- *      Author H2zero
- *
- * Originally:
- *
- * BLEServiceMap.cpp
- *
- *  Created on: Jun 22, 2017
- *      Author: kolban
- */
-#include "sdkconfig.h"
-#if defined(CONFIG_BT_ENABLED)
-
-#include "nimconfig.h"
-#if defined(CONFIG_BT_NIMBLE_ROLE_PERIPHERAL)
-
-#include "NimBLEService.h"
-
-
-/**
- * @brief Return the service by UUID.
- * @param [in] UUID The UUID to look up the service.
- * @return The characteristic.
- */
-NimBLEService* NimBLEServiceMap::getByUUID(const char* uuid) {
-    return getByUUID(NimBLEUUID(uuid));
-}
-
-/**
- * @brief Return the service by UUID.
- * @param [in] UUID The UUID to look up the service.
- * @return The characteristic.
- */
-NimBLEService* NimBLEServiceMap::getByUUID(const NimBLEUUID &uuid, uint8_t inst_id) {
-    for (auto &myPair : m_uuidMap) {
-        if (myPair.first->getUUID().equals(uuid)) {
-            return myPair.first;
-        }
-    }
-    //return m_uuidMap.at(uuid.toString());
-    return nullptr;
-} // getByUUID
-
-
-/**
- * @brief Return the service by handle.
- * @param [in] handle The handle to look up the service.
- * @return The service.
- */
-/*
-NimBLEService* NimBLEServiceMap::getByHandle(uint16_t handle) {
-    return m_handleMap.at(handle);
-} // getByHandle
-*/
-
-/**
- * @brief Set the service by UUID.
- * @param [in] uuid The uuid of the service.
- * @param [in] characteristic The service to cache.
- * @return N/A.
- */
-void NimBLEServiceMap::setByUUID(const NimBLEUUID &uuid, NimBLEService* service) {
-    m_uuidMap.insert(std::pair<NimBLEService*, std::string>(service, uuid.toString()));
-} // setByUUID
-
-
-/**
- * @brief Set the service by handle.
- * @param [in] handle The handle of the service.
- * @param [in] service The service to cache.
- * @return N/A.
- */
- /*
-void NimBLEServiceMap::setByHandle(uint16_t handle, NimBLEService* service) {
-    m_handleMap.insert(std::pair<uint16_t, NimBLEService*>(handle, service));
-} // setByHandle
-*/
-
-/**
- * @brief Return a string representation of the service map.
- * @return A string representation of the service map.
- */
-std::string NimBLEServiceMap::toString() {
-    std::string res;
-    //char hex[5];
-    for (auto &myPair: m_uuidMap) {
-    //  res += "handle: 0x";
-    //  snprintf(hex, sizeof(hex), "%04x", myPair.first);
-    //  res += hex;
-        res += ", uuid: " + myPair.second + "\n";
-    }
-    return res;
-} // toString
-
-
-/**
- * @brief Get the first service in the map.
- * @return The first service in the map.
- */
-NimBLEService* NimBLEServiceMap::getFirst() {
-    m_iterator = m_uuidMap.begin();
-    if (m_iterator == m_uuidMap.end()) return nullptr;
-    NimBLEService* pRet = m_iterator->first;
-    m_iterator++;
-    return pRet;
-} // getFirst
-
-
-/**
- * @brief Get the next service in the map.
- * @return The next service in the map.
- */
-NimBLEService* NimBLEServiceMap::getNext() {
-    if (m_iterator == m_uuidMap.end()) return nullptr;
-    NimBLEService* pRet = m_iterator->first;
-    m_iterator++;
-    return pRet;
-} // getNext
-
-
-/**
- * @brief Removes service from maps.
- * @return N/A.
- */
-void NimBLEServiceMap::removeService(NimBLEService* service) {
-    //m_handleMap.erase(service->getHandle());
-    m_uuidMap.erase(service);
-} // removeService
-
-
-/**
- * @brief Returns the amount of registered services
- * @return amount of registered services
- */
-int NimBLEServiceMap::getRegisteredServiceCount(){
-    //return m_handleMap.size();
-    return m_uuidMap.size();
-}
-
-
-#endif // #if defined(CONFIG_BT_NIMBLE_ROLE_PERIPHERAL)
-#endif /* CONFIG_BT_ENABLED */
diff --git a/libesp32/NimBLE-Arduino/src/NimBLEUtils.cpp b/libesp32/NimBLE-Arduino/src/NimBLEUtils.cpp
index 9036d4d30..1f1f22c25 100644
--- a/libesp32/NimBLE-Arduino/src/NimBLEUtils.cpp
+++ b/libesp32/NimBLE-Arduino/src/NimBLEUtils.cpp
@@ -502,8 +502,13 @@ void print_bytes(const uint8_t *bytes, int len)
     int i;
 
     for (i = 0; i < len; i++) {
-        MODLOG_DFLT(DEBUG, "%s0x%02x", i != 0 ? ":" : "", bytes[i]);
+        MODLOG_DFLT(ERROR, "%s0x%02x", i != 0 ? ":" : "", bytes[i]);
+        if(i % 30 == 0){
+            MODLOG_DFLT(ERROR, "\n");
+        }
     }
+
+    MODLOG_DFLT(ERROR, "\n");
 }
 
 void print_mbuf(const struct os_mbuf *om)
diff --git a/libesp32/NimBLE-Arduino/src/NimBLEUtils.h b/libesp32/NimBLE-Arduino/src/NimBLEUtils.h
index b26d41a51..891f83596 100644
--- a/libesp32/NimBLE-Arduino/src/NimBLEUtils.h
+++ b/libesp32/NimBLE-Arduino/src/NimBLEUtils.h
@@ -13,6 +13,20 @@
 
 #include "host/ble_gap.h"
 
+/****  FIX COMPILATION ****/
+#undef min
+#undef max
+/**************************/
+
+#include <string>
+
+typedef struct {
+    void *pATT;
+    TaskHandle_t task;
+    int rc;
+    std::string *buf;
+} ble_task_data_t;
+
 extern "C"{
 char *addr_str(const void *addr);
 void print_conn_desc(const struct ble_gap_conn_desc *desc);
diff --git a/libesp32/NimBLE-Arduino/src/NimBLEValue.cpp b/libesp32/NimBLE-Arduino/src/NimBLEValue.cpp
deleted file mode 100644
index aa437c60c..000000000
--- a/libesp32/NimBLE-Arduino/src/NimBLEValue.cpp
+++ /dev/null
@@ -1,143 +0,0 @@
-/*
- * NimNimBLEValue.cpp
- *
- *  Created: on March 6, 2020
- *      Author H2zero
- *
- * Originally:
- *
- * BLEValue.cpp
- *
- *  Created on: Jul 17, 2017
- *      Author: kolban
- */
-#include "sdkconfig.h"
-#if defined(CONFIG_BT_ENABLED)
-
-#include "nimconfig.h"
-#if defined(CONFIG_BT_NIMBLE_ROLE_PERIPHERAL)
-
-#include "NimBLEValue.h"
-#include "NimBLELog.h"
-
-static const char* LOG_TAG="NimBLEValue";
-
-NimBLEValue::NimBLEValue() {
-    m_accumulation = "";
-    m_value        = "";
-    m_readOffset   = 0;
-} // NimBLEValue
-
-
-/**
- * @brief Add a message part to the accumulation.
- * The accumulation is a growing set of data that is added to until a commit or cancel.
- * @param [in] part A message part being added.
- */
-void NimBLEValue::addPart(const std::string &part) {
-    NIMBLE_LOGD(LOG_TAG, ">> addPart: length=%d", part.length());
-    m_accumulation += part;
-} // addPart
-
-
-/**
- * @brief Add a message part to the accumulation.
- * The accumulation is a growing set of data that is added to until a commit or cancel.
- * @param [in] pData A message part being added.
- * @param [in] length The number of bytes being added.
- */
-void NimBLEValue::addPart(const uint8_t* pData, size_t length) {
-    NIMBLE_LOGD(LOG_TAG, ">> addPart: length=%d", length);
-    m_accumulation += std::string((char*) pData, length);
-} // addPart
-
-
-/**
- * @brief Cancel the current accumulation.
- */
-void NimBLEValue::cancel() {
-    NIMBLE_LOGD(LOG_TAG, ">> cancel");
-    m_accumulation = "";
-    m_readOffset   = 0;
-} // cancel
-
-
-/**
- * @brief Commit the current accumulation.
- * When writing a value, we may find that we write it in "parts" meaning that the writes come in in pieces
- * of the overall message.  After the last part has been received, we may perform a commit which means that
- * we now have the complete message and commit the change as a unit.
- */
-void NimBLEValue::commit() {
-    NIMBLE_LOGD(LOG_TAG, ">> commit");
-    // If there is nothing to commit, do nothing.
-    if (m_accumulation.length() == 0) return;
-    setValue(m_accumulation);
-    m_accumulation = "";
-    m_readOffset   = 0;
-} // commit
-
-
-/**
- * @brief Get a pointer to the data.
- * @return A pointer to the data.
- */
-uint8_t* NimBLEValue::getData() {
-    return (uint8_t*) m_value.data();
-}
-
-
-/**
- * @brief Get the length of the data in bytes.
- * @return The length of the data in bytes.
- */
-size_t NimBLEValue::getLength() {
-    return m_value.length();
-} // getLength
-
-
-/**
- * @brief Get the read offset.
- * @return The read offset into the read.
- */
-uint16_t NimBLEValue::getReadOffset() {
-    return m_readOffset;
-} // getReadOffset
-
-
-/**
- * @brief Get the current value.
- */
-std::string NimBLEValue::getValue() {
-    return m_value;
-} // getValue
-
-
-/**
- * @brief Set the read offset
- * @param [in] readOffset The offset into the read.
- */
-void NimBLEValue::setReadOffset(uint16_t readOffset) {
-    m_readOffset = readOffset;
-} // setReadOffset
-
-
-/**
- * @brief Set the current value.
- */
-void NimBLEValue::setValue(const std::string &value) {
-    m_value = value;
-} // setValue
-
-
-/**
- * @brief Set the current value.
- * @param [in] pData The data for the current value.
- * @param [in] The length of the new current value.
- */
-void NimBLEValue::setValue(const uint8_t* pData, size_t length) {
-    m_value = std::string((char*) pData, length);
-} // setValue
-
-#endif // #if defined(CONFIG_BT_NIMBLE_ROLE_PERIPHERAL)
-#endif // CONFIG_BT_ENABLED
diff --git a/libesp32/NimBLE-Arduino/src/NimBLEValue.h b/libesp32/NimBLE-Arduino/src/NimBLEValue.h
deleted file mode 100644
index 4fdeb9bc4..000000000
--- a/libesp32/NimBLE-Arduino/src/NimBLEValue.h
+++ /dev/null
@@ -1,52 +0,0 @@
-/*
- * NimBLEValue.h
- *
- *  Created: on March 6, 2020
- *      Author H2zero
- *
- * Originally:
- *
- * BLEValue.h
- *
- *  Created on: Jul 17, 2017
- *      Author: kolban
- */
-
-#ifndef MAIN_BLEVALUE_H_
-#define MAIN_BLEVALUE_H_
-#include "sdkconfig.h"
-#if defined(CONFIG_BT_ENABLED)
-
-#include "nimconfig.h"
-#if defined(CONFIG_BT_NIMBLE_ROLE_PERIPHERAL)
-
-#include <string>
-
-/**
- * @brief The model of a %BLE value.
- */
-class NimBLEValue {
-public:
-    NimBLEValue();
-    void        addPart(const std::string &part);
-    void        addPart(const uint8_t* pData, size_t length);
-    void        cancel();
-    void        commit();
-    uint8_t*    getData();
-    size_t      getLength();
-    uint16_t    getReadOffset();
-    std::string getValue();
-    void        setReadOffset(uint16_t readOffset);
-    void        setValue(const std::string &value);
-    void        setValue(const uint8_t* pData, size_t length);
-
-private:
-    std::string m_accumulation;
-    uint16_t    m_readOffset;
-    std::string m_value;
-
-};
-
-#endif // #if defined(CONFIG_BT_NIMBLE_ROLE_PERIPHERAL)
-#endif // CONFIG_BT_ENABLED
-#endif /* MAIN_BLEVALUE_H_ */
diff --git a/libesp32/NimBLE-Arduino/src/host/ble_hs_pvcy.h b/libesp32/NimBLE-Arduino/src/host/ble_hs_pvcy.h
index 0ff32b80b..19087f220 100644
--- a/libesp32/NimBLE-Arduino/src/host/ble_hs_pvcy.h
+++ b/libesp32/NimBLE-Arduino/src/host/ble_hs_pvcy.h
@@ -19,6 +19,9 @@
  * under the License.
  */
 
+#ifndef H_BLE_HS_PVCY_
+#define H_BLE_HS_PVCY_
+
 #include "host/ble_hs.h"
 
 #ifdef __cplusplus
@@ -26,15 +29,45 @@ extern "C" {
 #endif
 
 #if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)
-/* Called to configure local(own) privacy (RPA) when using host based privacy. In
- * Host based privacy as controller is not aware of RPA, we do it via
- * 'BLE_ADDR_RANDOM' addr_type route.
+
+#define NIMBLE_HOST_DISABLE_PRIVACY            0x00
+#define NIMBLE_HOST_ENABLE_RPA                 0x01
+#define NIMBLE_HOST_ENABLE_NRPA                0x02
+
+/* Called to configure local(own) privacy (RPA/NRPA) when using Host based privacy.
+ * In Host based privacy, as controller is not aware of RPA/NRPA address is in use,
+ * we do it through 'BLE_ADDR_RANDOM (0x01)' addr_type route. This is necessary
+ * so as to set the private address as random address in controller.
+ * Remember to configure `BLE_SM_PAIR_KEY_DIST_ID` in our & their
+ * key distributions for using RPA. For NRPA part of privacy it is not
+ * necessary to configure key distributions in host, as anyway NRPA is non-resolvable.
+ * Please call this API once host-controller are synced as we set the private
+ * (RPA/NRPA) address using host-controller HCI commands.
  *
- * @param                enable RPA when enable is not 0
- *                       disable RPA otherwise
+ * To give brief information on how to use this feature,
+ * please refer to following steps while using RPA feature:
+ *
+ * 1. Include "host/ble_hs_pvcy.h".
+ * 2. Set own_addr_type to `BLE_OWN_ADDR_RANDOM`.
+ * 3. Add `BLE_SM_PAIR_KEY_DIST_ID` to key distribution in
+ *    `ble_hs_cfg.sm_our_key_dist` & `ble_hs_cfg.sm_their_key_dist`.
+ * 4. Call `ble_hs_pvcy_rpa_config(1)` in Host-Controller sync callback.
+ *
+ * In case of NRPA, steps 1, 2 and calling ble_hs_pvcy_rpa_config(2) will
+ * suffice.
+ *
+ * @param                enable RPA when param      = 1 (NIMBLE_HOST_ENABLE_RPA)
+ *                       enable NRPA when param     = 2 (NIMBLE_HOST_ENABLE_NRPA)
+ *                       disable privacy when param = 0 (NIMBLE_HOST_DISABLE_PRIVACY)
  *
  * @return               return 0 when successful.
  *                       return appropriate error code otherwise
  */
 int ble_hs_pvcy_rpa_config(uint8_t enable);
 #endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/libesp32/NimBLE-Arduino/src/mesh/glue.h b/libesp32/NimBLE-Arduino/src/mesh/glue.h
index 08e81fa41..d6aee21f9 100644
--- a/libesp32/NimBLE-Arduino/src/mesh/glue.h
+++ b/libesp32/NimBLE-Arduino/src/mesh/glue.h
@@ -332,6 +332,8 @@ static inline void net_buf_simple_restore(struct os_mbuf *buf,
 
 static inline void sys_memcpy_swap(void *dst, const void *src, size_t length)
 {
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wpointer-arith"
     __ASSERT(((src < dst && (src + length) <= dst) ||
           (src > dst && (dst + length) <= src)),
          "Source and destination buffers must not overlap");
@@ -341,6 +343,7 @@ static inline void sys_memcpy_swap(void *dst, const void *src, size_t length)
     for (; length > 0; length--) {
         *((u8_t *)dst++) = *((u8_t *)src--);
     }
+#pragma GCC diagnostic pop
 }
 
 #define popcount(x) __builtin_popcount(x)
diff --git a/libesp32/NimBLE-Arduino/src/nimble/host/mesh/pkg.yml b/libesp32/NimBLE-Arduino/src/nimble/host/mesh/pkg.yml
deleted file mode 100644
index 44cc0c732..000000000
--- a/libesp32/NimBLE-Arduino/src/nimble/host/mesh/pkg.yml
+++ /dev/null
@@ -1,49 +0,0 @@
-#
-# Licensed to the Apache Software Foundation (ASF) under one
-# or more contributor license agreements.  See the NOTICE file
-# distributed with this work for additional information
-# regarding copyright ownership.  The ASF licenses this file
-# to you under the Apache License, Version 2.0 (the
-# "License"); you may not use this file except in compliance
-# with the License.  You may obtain a copy of the License at
-#
-#  http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing,
-# software distributed under the License is distributed on an
-# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
-# KIND, either express or implied.  See the License for the
-# specific language governing permissions and limitations
-# under the License.
-#
-
-pkg.name: nimble/host/mesh
-pkg.description: Bluetooth Mesh
-pkg.author: "Apache Mynewt <dev@mynewt.apache.org>"
-pkg.homepage: "http://mynewt.apache.org/"
-pkg.keywords:
-    - ble
-    - bluetooth
-    - mesh
-
-pkg.deps:
-    - "@apache-mynewt-core/kernel/os"
-    - "@apache-mynewt-core/util/mem"
-    - "@apache-mynewt-core/crypto/tinycrypt"
-    - nimble
-    - nimble/host
-
-pkg.deps.BLE_MESH_SHELL:
-    - "@apache-mynewt-core/sys/shell"
-
-pkg.deps.BLE_MESH_SETTINGS:
-    - "@apache-mynewt-core/encoding/base64"
-    - "@apache-mynewt-core/sys/config"
-
-pkg.req_apis:
-    - log
-    - stats
-
-pkg.init:
-    bt_mesh_register_gatt: 'MYNEWT_VAL(BLE_MESH_SYSINIT_STAGE)'
-    ble_mesh_shell_init: 'MYNEWT_VAL(BLE_MESH_SYSINIT_STAGE_SHELL)'
diff --git a/libesp32/NimBLE-Arduino/src/nimble/host/mesh/src/src/ble_gap_priv.h b/libesp32/NimBLE-Arduino/src/nimble/host/mesh/src/src/ble_gap_priv.h
index c80872898..90c32e22a 100644
--- a/libesp32/NimBLE-Arduino/src/nimble/host/mesh/src/src/ble_gap_priv.h
+++ b/libesp32/NimBLE-Arduino/src/nimble/host/mesh/src/src/ble_gap_priv.h
@@ -99,7 +99,7 @@ int ble_gap_rx_l2cap_update_req(uint16_t conn_handle,
                                 struct ble_gap_upd_params *params);
 void ble_gap_rx_phy_update_complete(struct hci_le_phy_upd_complete *evt);
 void ble_gap_enc_event(uint16_t conn_handle, int status,
-                       int security_restored);
+                       int security_restored, int bonded);
 void ble_gap_passkey_event(uint16_t conn_handle,
                            struct ble_gap_passkey_params *passkey_params);
 void ble_gap_notify_rx_event(uint16_t conn_handle, uint16_t attr_handle,
diff --git a/libesp32/NimBLE-Arduino/src/nimble/host/mesh/src/src/ble_hs_id_priv.h b/libesp32/NimBLE-Arduino/src/nimble/host/mesh/src/src/ble_hs_id_priv.h
index c031b9511..85260ec48 100644
--- a/libesp32/NimBLE-Arduino/src/nimble/host/mesh/src/src/ble_hs_id_priv.h
+++ b/libesp32/NimBLE-Arduino/src/nimble/host/mesh/src/src/ble_hs_id_priv.h
@@ -36,6 +36,7 @@ void ble_hs_id_rnd_reset(void);
 #if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)
 bool ble_hs_is_rpa(uint8_t *addr, uint8_t addr_type);
 int ble_hs_id_set_pseudo_rnd(const uint8_t *);
+int ble_hs_id_set_nrpa_rnd(void);
 #endif
 #ifdef __cplusplus
 }
diff --git a/libesp32/NimBLE-Arduino/src/nimble/host/mesh/src/src/ble_hs_priv.h b/libesp32/NimBLE-Arduino/src/nimble/host/mesh/src/src/ble_hs_priv.h
index 27bf904b7..49269546c 100644
--- a/libesp32/NimBLE-Arduino/src/nimble/host/mesh/src/src/ble_hs_priv.h
+++ b/libesp32/NimBLE-Arduino/src/nimble/host/mesh/src/src/ble_hs_priv.h
@@ -115,9 +115,9 @@ int ble_hs_hci_evt_acl_process(struct os_mbuf *om);
 int ble_hs_misc_conn_chan_find(uint16_t conn_handle, uint16_t cid,
                                struct ble_hs_conn **out_conn,
                                struct ble_l2cap_chan **out_chan);
-void ble_hs_misc_conn_chan_find_reqd(uint16_t conn_handle, uint16_t cid,
-                                     struct ble_hs_conn **out_conn,
-                                     struct ble_l2cap_chan **out_chan);
+int ble_hs_misc_conn_chan_find_reqd(uint16_t conn_handle, uint16_t cid,
+                                    struct ble_hs_conn **out_conn,
+                                    struct ble_l2cap_chan **out_chan);
 uint8_t ble_hs_misc_addr_type_to_id(uint8_t addr_type);
 int ble_hs_misc_restore_irks(void);
 
diff --git a/libesp32/NimBLE-Arduino/src/nimble/host/mesh/src/src/ble_hs_resolv_priv.h b/libesp32/NimBLE-Arduino/src/nimble/host/mesh/src/src/ble_hs_resolv_priv.h
index 568aa89ab..e76a26a55 100644
--- a/libesp32/NimBLE-Arduino/src/nimble/host/mesh/src/src/ble_hs_resolv_priv.h
+++ b/libesp32/NimBLE-Arduino/src/nimble/host/mesh/src/src/ble_hs_resolv_priv.h
@@ -61,7 +61,7 @@ struct ble_hs_dev_records {
 
 /* Add a device to the resolving list */
 int ble_hs_resolv_list_add(uint8_t *cmdbuf);
-int ble_hs_gen_own_rpa_random(void);
+int ble_hs_gen_own_private_rnd(void);
 uint8_t *ble_hs_get_rpa_local(void);
 
 /* Remove a device from the resolving list */
@@ -71,6 +71,8 @@ void ble_hs_resolv_list_clear_all(void);
 
 /* Address resolution enable command */
 void ble_hs_resolv_enable(bool);
+void ble_hs_resolv_nrpa_enable(void);
+void ble_hs_resolv_nrpa_disable(void);
 
 /* Finds 'addr' in resolving list. Doesnt check if address resolution enabled */
 struct ble_hs_resolv_entry *
diff --git a/libesp32/NimBLE-Arduino/src/nimble/host/mesh/src/src/ble_sm_priv.h b/libesp32/NimBLE-Arduino/src/nimble/host/mesh/src/src/ble_sm_priv.h
index 74205bd0c..bbb4b03ae 100644
--- a/libesp32/NimBLE-Arduino/src/nimble/host/mesh/src/src/ble_sm_priv.h
+++ b/libesp32/NimBLE-Arduino/src/nimble/host/mesh/src/src/ble_sm_priv.h
@@ -277,10 +277,10 @@ struct ble_sm_result {
     uint8_t sm_err;
     struct ble_gap_passkey_params passkey_params;
     void *state_arg;
-    unsigned execute:1;
-    unsigned enc_cb:1;
-    unsigned persist_keys:1;
-    unsigned restore:1;
+    unsigned execute : 1;
+    unsigned enc_cb : 1;
+    unsigned bonded : 1;
+    unsigned restore : 1;
 };
 
 #if MYNEWT_VAL(BLE_HS_DEBUG)
diff --git a/libesp32/NimBLE-Arduino/src/nimble/host/mesh/syscfg.yml b/libesp32/NimBLE-Arduino/src/nimble/host/mesh/syscfg.yml
deleted file mode 100644
index 7d5a36a71..000000000
--- a/libesp32/NimBLE-Arduino/src/nimble/host/mesh/syscfg.yml
+++ /dev/null
@@ -1,540 +0,0 @@
-# Licensed to the Apache Software Foundation (ASF) under one
-# or more contributor license agreements.  See the NOTICE file
-# distributed with this work for additional information
-# regarding copyright ownership.  The ASF licenses this file
-# to you under the Apache License, Version 2.0 (the
-# "License"); you may not use this file except in compliance
-# with the License.  You may obtain a copy of the License at
-#
-#  http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing,
-# software distributed under the License is distributed on an
-# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
-# KIND, either express or implied.  See the License for the
-# specific language governing permissions and limitations
-# under the License.
-#
-
-syscfg.defs:
-    BLE_MESH_PROV:
-        description: >
-            Enable provisioning. It is automatically enabled whenever
-            BLE_MESH_PB_ADV or BLE_MESH_PB_GATT is set.
-        value: 1
-
-    BLE_MESH_PB_ADV:
-        description: >
-           Enable this option to allow the device to be provisioned over
-           the advertising bearer.
-        value: 1
-
-    BLE_MESH_PROXY:
-        description: >
-           Enable proxy. This is automatically set whenever BLE_MESH_PB_GATT or
-           BLE_MESH_GATT_PROXY is set.
-        value: 0
-
-    BLE_MESH_PB_GATT:
-        description: >
-           Enable this option to allow the device to be provisioned over
-           the GATT bearer.
-        value: 1
-
-    BLE_MESH_GATT_PROXY:
-        description: >
-            This option enables support for the Mesh GATT Proxy Service,
-            i.e. the ability to act as a proxy between a Mesh GATT Client
-            and a Mesh network.
-        value: 1
-
-    BLE_MESH_NODE_ID_TIMEOUT:
-        description: >
-            This option determines for how long the local node advertises
-            using Node Identity. The given value is in seconds. The
-            specification limits this to 60 seconds, and implies that to
-            be the appropriate value as well, so just leaving this as the
-            default is the safest option.
-        value: 60
-
-    BLE_MESH_PROXY_FILTER_SIZE:
-        descryption: >
-            This option specifies how many Proxy Filter entries the local
-            node supports.
-        value: 1
-
-    BLE_MESH_SUBNET_COUNT:
-        description: >
-            This option specifies how many subnets a Mesh network can
-            participate in at the same time.
-        value: 1
-
-    BLE_MESH_APP_KEY_COUNT:
-        description: >
-            This option specifies how many application keys the device can
-            store per network.
-        value: 1
-
-    BLE_MESH_MODEL_KEY_COUNT:
-        description: >
-             This option specifies how many application keys each model can
-             at most be bound to.
-        value: 1
-
-    BLE_MESH_MODEL_GROUP_COUNT:
-        description: >
-            This option specifies how many group addresses each model can
-            at most be subscribed to.
-        value: 1
-
-    BLE_MESH_LABEL_COUNT:
-        description: >
-            This option specifies how many Label UUIDs can be stored.
-        value: 1
-
-    BLE_MESH_CRPL:
-        description: >
-            This options specifies the maximum capacity of the replay
-            protection list. This option is similar to the network message
-            cache size, but has a different purpose.
-        value: 10
-
-    BLE_MESH_ADV_TASK_PRIO:
-        description: >
-            Advertising task prio (FIXME)
-        type: task_priority
-        value: 9
-
-    BLE_MESH_MSG_CACHE_SIZE:
-        description: >
-            Number of messages that are cached for the network. This description
-            prevent unnecessary decryption operations and unnecessary
-            relays. This option is similar to the replay protection list,
-            but has a different purpose.
-        value: 10
-
-    BLE_MESH_ADV_BUF_COUNT:
-        description: >
-            Number of advertising buffers available. This should be chosen
-            based on what kind of features the local node shoule have. E.g.
-            a relay will perform better the more buffers it has. Another
-            thing to consider is outgoing segmented messages. There must
-            be at least three more advertising buffers than the maximum
-            supported outgoing segment count (BT_MESH_TX_SEG_MAX).
-        value: 6
-
-    BLE_MESH_IVU_DIVIDER:
-        description: >
-            When the IV Update state enters Normal operation or IV Update
-            in Progress, we need to keep track of how many hours has passed
-            in the state, since the specification requires us to remain in
-            the state at least for 96 hours (Update in Progress has an
-            additional upper limit of 144 hours).
-
-            In order to fulfil the above requirement, even if the node might
-            be powered off once in a while, we need to store persistently
-            how many hours the node has been in the state. This doesn't
-            necessarily need to happen every hour (thanks to the flexible
-            duration range). The exact cadence will depend a lot on the
-            ways that the node will be used and what kind of power source it
-            has.
-
-            Since there is no single optimal answer, this configuration
-            option allows specifying a divider, i.e. how many intervals
-            the 96 hour minimum gets split into. After each interval the
-            duration that the node has been in the current state gets
-            stored to flash. E.g. the default value of 4 means that the
-            state is saved every 24 hours (96 / 4).
-        value: 4
-
-    BLE_MESH_TX_SEG_MSG_COUNT:
-        description: >
-            Maximum number of simultaneous outgoing multi-segment and/or
-            reliable messages.
-        value: 4
-
-    BLE_MESH_RX_SEG_MSG_COUNT:
-        description: >
-            Maximum number of simultaneous incoming multi-segment and/or
-            reliable messages.
-        value: 2
-
-    BLE_MESH_RX_SDU_MAX:
-        description: >
-            Maximum incoming Upper Transport Access PDU length. This
-            determines also how many segments incoming segmented messages
-            can have. Each segment can contain 12 bytes, so this value should
-            be set to a multiple of 12 to avoid wasted memory. The minimum
-            requirement is 2 segments (24 bytes) whereas the maximum supported
-            by the Mesh specification is 32 segments (384 bytes).
-        value: 72
-
-    BLE_MESH_TX_SEG_MAX:
-        description: >
-            Maximum number of segments supported for outgoing messages.
-            This value should typically be fine-tuned based on what
-            models the local node supports, i.e. what's the largest
-            message payload that the node needs to be able to send.
-            This value affects memory and call stack consumption, which
-            is why the default is lower than the maximum that the
-            specification would allow (32 segments).
-
-            The maximum outgoing SDU size is 12 times this number (out of
-            which 4 or 8 bytes is used for the Transport Layer MIC). For
-            example, 5 segments means the maximum SDU size is 60 bytes,
-            which leaves 56 bytes for application layer data using a
-            4-byte MIC and 52 bytes using an 8-byte MIC.
-
-            Be sure to specify a sufficient number of advertising buffers
-            when setting this option to a higher value. There must be at
-            least three more advertising buffers (BT_MESH_ADV_BUF_COUNT)
-            as there are outgoing segments.
-        value: 3
-
-    BLE_MESH_RELAY:
-        description: >
-            Support for acting as a Mesh Relay Node.
-        value: 0
-
-    BLE_MESH_LOW_POWER:
-        description: >
-           Enable this option to be able to act as a Low Power Node.
-        value: 0
-
-    BLE_MESH_LPN_ESTABLISHMENT:
-        description: >
-           Perform the Friendship establishment using low power, with
-           the help of a reduced scan duty cycle. The downside of this
-           is that the node may miss out on messages intended for it
-           until it has successfully set up Friendship with a Friend
-           node.
-        value: 1
-
-    BLE_MESH_LPN_AUTO:
-        description: >
-           Automatically enable LPN functionality once provisioned and start
-           looking for Friend nodes. If this option is disabled LPN mode
-           needs to be manually enabled by calling bt_mesh_lpn_set(true).
-           node.
-        value: 1
-
-    BLE_MESH_LPN_AUTO_TIMEOUT:
-        description: >
-           Time in seconds from the last received message, that the node
-           will wait before starting to look for Friend nodes.
-        value: 15
-
-    BLE_MESH_LPN_RETRY_TIMEOUT:
-        description: >
-           Time in seconds between Friend Requests, if a previous Friend
-           Request did not receive any acceptable Friend Offers.
-        value: 8
-
-    BLE_MESH_LPN_RSSI_FACTOR:
-        description: >
-            The contribution of the RSSI measured by the Friend node used
-            in Friend Offer Delay calculations. 0 = 1, 1 = 1.5, 2 = 2, 3 = 2.5.
-        value: 0
-
-    BLE_MESH_LPN_RECV_WIN_FACTOR:
-        description: >
-           The contribution of the supported Receive Window used in
-           Friend Offer Delay calculations. 0 = 1, 1 = 1.5, 2 = 2, 3 = 2.5.
-        value: 0
-
-    BLE_MESH_LPN_MIN_QUEUE_SIZE:
-        description: >
-           The MinQueueSizeLog field is defined as log_2(N), where N is
-           the minimum number of maximum size Lower Transport PDUs that
-           the Friend node can store in its Friend Queue. As an example,
-           MinQueueSizeLog value 1 gives N = 2, and value 7 gives N = 128.
-        value: 1
-
-    BLE_MESH_LPN_RECV_DELAY:
-        description: >
-            The ReceiveDelay is the time between the Low Power node
-            sending a request and listening for a response. This delay
-            allows the Friend node time to prepare the response. The value
-            is in units of milliseconds.
-        value: 100
-
-    BLE_MESH_LPN_POLL_TIMEOUT:
-        description: >
-            PollTimeout timer is used to measure time between two
-            consecutive requests sent by the Low Power node. If no
-            requests are received by the Friend node before the
-            PollTimeout timer expires, then the friendship is considered
-            terminated. The value is in units of 100 milliseconds, so e.g.
-            a value of 300 means 30 seconds.
-        value: 300
-
-    BLE_MESH_LPN_INIT_POLL_TIMEOUT:
-        description: >
-            The initial value of the PollTimeout timer when Friendship
-            gets established for the first time. After this the timeout
-            will gradually grow toward the actual PollTimeout, doubling
-            in value for each iteration. The value is in units of 100
-            milliseconds, so e.g. a value of 300 means 3 seconds.
-        value: MYNEWT_VAL_BLE_MESH_LPN_POLL_TIMEOUT
-
-    BLE_MESH_LPN_SCAN_LATENCY:
-        description: >
-            Latency in milliseconds that it takes to enable scanning. This
-            is in practice how much time in advance before the Receive Window
-            that scanning is requested to be enabled.
-        value: 10
-
-    BLE_MESH_LPN_GROUPS:
-        description: >
-            Maximum number of groups that the LPN can subscribe to.
-        value: 10
-
-    BLE_MESH_FRIEND:
-        description: >
-            Enable this option to be able to act as a Friend Node.
-        value: 0
-
-    BLE_MESH_FRIEND_RECV_WIN:
-        description: >
-            Receive Window in milliseconds supported by the Friend node.
-        value: 255
-
-    BLE_MESH_FRIEND_QUEUE_SIZE:
-        description: >
-            Minimum number of buffers available to be stored for each
-            local Friend Queue.
-        value: 16
-
-    BLE_MESH_FRIEND_SUB_LIST_SIZE:
-        description: >
-            Size of the Subscription List that can be supported by a
-            Friend node for a Low Power node.
-        value: 3
-
-    BLE_MESH_FRIEND_LPN_COUNT:
-        description: >
-            Number of Low Power Nodes the Friend can have a Friendship
-            with simultaneously.
-        value: 2
-
-    BLE_MESH_FRIEND_SEG_RX:
-        description: >
-            Number of incomplete segment lists that we track for each LPN
-            that we are Friends for. In other words, this determines how
-            many elements we can simultaneously be receiving segmented
-            messages from when the messages are going into the Friend queue.
-        value: 1
-
-    BLE_MESH_CFG_CLI:
-        description: >
-            Enable support for the configuration client model.
-        value: 0
-
-    BLE_MESH_HEALTH_CLI:
-        description: >
-            Enable support for the health client model.
-        value: 0
-
-    BLE_MESH_SHELL:
-        description: >
-            Activate shell module that provides Bluetooth Mesh commands to
-            the console.
-        value: 0
-
-    BLE_MESH_DEBUG:
-        description: >
-            Use this option to enable debug logs for the Bluetooth
-            Mesh functionality.
-        value: 0
-
-    BLE_MESH_DEBUG_NET:
-        description: >
-            Use this option to enable Network layer debug logs for the
-            Bluetooth Mesh functionality.
-        value: 0
-
-    BLE_MESH_DEBUG_TRANS:
-        description: >
-            Use this option to enable Transport layer debug logs for the
-            Bluetooth Mesh functionality.
-        value: 0
-
-    BLE_MESH_DEBUG_BEACON:
-        description: >
-            Use this option to enable Beacon-related debug logs for the
-            Bluetooth Mesh functionality.
-        value: 0
-
-    BLE_MESH_DEBUG_CRYPTO:
-        description: >
-            Use this option to enable cryptographic debug logs for the
-            Bluetooth Mesh functionality.
-        value: 0
-
-    BLE_MESH_DEBUG_PROV:
-        description: >
-            Use this option to enable Provisioning debug logs for the
-            Bluetooth Mesh functionality.
-        value: 0
-
-    BLE_MESH_DEBUG_ACCESS:
-        description: >
-            Use this option to enable Access layer and device composition
-            related debug logs for Bluetooth Mesh.
-        value: 0
-
-    BLE_MESH_DEBUG_MODEL:
-        description: >
-            Use this option to enable debug logs for the Foundation
-            Models.
-        value: 0
-
-    BLE_MESH_DEBUG_ADV:
-        description: >
-            Use this option to enable advertising debug logs for
-            the Bluetooth Mesh functionality.
-        value: 0
-
-    BLE_MESH_DEBUG_LOW_POWER:
-        description: >
-            Use this option to enable Low Power debug logs for the
-            Bluetooth Mesh functionality.
-        value: 0
-
-    BLE_MESH_DEBUG_FRIEND:
-        description: >
-            Use this option to enable Friend debug logs for the
-            Bluetooth Mesh functionality.
-        value: 0
-
-    BLE_MESH_DEBUG_PROXY:
-        description: >
-            Use this option to enable Proxy protocol debug logs.
-        value: 0
-
-    BLE_MESH_DEBUG_SETTINGS:
-        description: >
-            Use this option to enable persistent settings debug logs.
-        value: 1
-
-    BLE_MESH_IV_UPDATE_TEST:
-        description: >
-            This option removes the 96 hour limit of the IV Update
-            Procedure and lets the state be changed at any time.
-        value: 0
-
-    BLE_MESH_TESTING:
-        description: >
-            This option enables testing API.
-        value: 0
-
-    BLE_MESH_DEV_UUID:
-        description: >
-            Device UUID
-        value: ((uint8_t[16]){0x11, 0x22, 0})
-
-    BLE_MESH_SHELL_MODELS:
-        description: >
-            Include implementation of some demo models.
-        value: 0
-
-    BLE_MESH_OOB_OUTPUT_ACTIONS:
-        description: >
-            Supported Output OOB Actions
-            BT_MESH_NO_OUTPUT       = 0,
-            BT_MESH_BLINK           = BIT(0)
-            BT_MESH_BEEP            = BIT(1)
-            BT_MESH_VIBRATE         = BIT(2)
-            BT_MESH_DISPLAY_NUMBER  = BIT(3)
-            BT_MESH_DISPLAY_STRING  = BIT(4)
-        value: ((BT_MESH_DISPLAY_NUMBER))
-
-    BLE_MESH_OOB_OUTPUT_SIZE:
-        description: >
-            Output OOB size
-        value: 4
-
-    BLE_MESH_OOB_INPUT_ACTIONS:
-        description: >
-            Supported Input OOB Actions
-            BT_MESH_NO_INPUT      = 0,
-            BT_MESH_PUSH          = BIT(0)
-            BT_MESH_TWIST         = BIT(1)
-            BT_MESH_ENTER_NUMBER  = BIT(2)
-            BT_MESH_ENTER_STRING  = BIT(3)
-        value: ((BT_MESH_NO_INPUT))
-
-    BLE_MESH_OOB_INPUT_SIZE:
-        description: >
-            Input OOB size
-        value: 4
-
-    BLE_MESH_SETTINGS:
-        description: >
-            This option enables Mesh settings storage.
-        value: 1
-
-    BLE_MESH_STORE_TIMEOUT:
-        description: >
-            This value defines in seconds how soon any pending changes
-            are actually written into persistent storage (flash) after
-            a change occurs.
-        value: 2
-
-    BLE_MESH_SEQ_STORE_RATE:
-        description: >
-            This value defines how often the local sequence number gets
-            updated in persistent storage (i.e. flash). E.g. a value of 100
-            means that the sequence number will be stored to flash on every
-            100th increment. If the node sends messages very frequently a
-            higher value makes more sense, whereas if the node sends
-            infrequently a value as low as 0 (update storage for every
-            increment) can make sense. When the stack gets initialized it
-            will add this number to the last stored one, so that it starts
-            off with a value that's guaranteed to be larger than the last
-            one used before power off.
-        value: 128
-
-    BLE_MESH_RPL_STORE_TIMEOUT:
-        description: >
-            This value defines in seconds how soon the RPL gets written to
-            persistent storage after a change occurs. If the node receives
-            messages frequently it may make sense to have this set to a
-            large value, whereas if the RPL gets updated infrequently a
-            value as low as 0 (write immediately) may make sense. Note that
-            if the node operates a security sensitive use case, and there's
-            a risk of sudden power loss, it may be a security vulnerability
-            to set this value to anything else than 0 (a power loss before
-            writing to storage exposes the node to potential message
-            replay attacks).
-        value: 5
-
-    BLE_MESH_DEVICE_NAME:
-        description: >
-            This value defines BLE Mesh device/node name.
-        value: '"nimble-mesh-node"'
-
-    BLE_MESH_SYSINIT_STAGE:
-        description: >
-            Primary sysinit stage for BLE mesh functionality.
-        value: 500
-
-    BLE_MESH_SYSINIT_STAGE_SHELL:
-        description: >
-            Secondary sysinit stage for BLE mesh functionality.
-        value: 1000
-
-syscfg.vals.BLE_MESH_SHELL:
-    BLE_MESH_CFG_CLI: 1
-    BLE_MESH_HEALTH_CLI: 1
-    BLE_MESH_IV_UPDATE_TEST: 1
-
-syscfg.vals.BLE_MESH_GATT_PROXY:
-    BLE_MESH_PROXY: 1
-
-syscfg.vals.BLE_MESH_PB_GATT:
-    BLE_MESH_PROXY: 1
-    BLE_MESH_PROV: 1
-
-syscfg.vals.BLE_MESH_PB_ADV:
-    BLE_MESH_PROV: 1
diff --git a/libesp32/NimBLE-Arduino/src/nimble/host/services/ans/pkg.yml b/libesp32/NimBLE-Arduino/src/nimble/host/services/ans/pkg.yml
deleted file mode 100644
index 691e566e1..000000000
--- a/libesp32/NimBLE-Arduino/src/nimble/host/services/ans/pkg.yml
+++ /dev/null
@@ -1,34 +0,0 @@
-#
-# Licensed to the Apache Software Foundation (ASF) under one
-# or more contributor license agreements.  See the NOTICE file
-# distributed with this work for additional information
-# regarding copyright ownership.  The ASF licenses this file
-# to you under the Apache License, Version 2.0 (the
-# "License"); you may not use this file except in compliance
-# with the License.  You may obtain a copy of the License at
-#
-#  http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing,
-# software distributed under the License is distributed on an
-# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
-# KIND, either express or implied.  See the License for the
-# specific language governing permissions and limitations
-# under the License.
-#
-
-pkg.name: nimble/host/services/ans
-pkg.description: Alert Notification Service Server.
-pkg.author: "Apache Mynewt <dev@mynewt.apache.org>"
-pkg.homepage: "http://mynewt.apache.org/"
-pkg.keywords:
-    - ble
-    - bluetooth
-    - ans
-    - nimble
-
-pkg.deps:
-    - nimble/host
-
-pkg.init:
-    ble_svc_ans_init: 'MYNEWT_VAL(BLE_SVC_ANS_SYSINIT_STAGE)'
diff --git a/libesp32/NimBLE-Arduino/src/nimble/host/services/ans/syscfg.yml b/libesp32/NimBLE-Arduino/src/nimble/host/services/ans/syscfg.yml
deleted file mode 100644
index 74de8d963..000000000
--- a/libesp32/NimBLE-Arduino/src/nimble/host/services/ans/syscfg.yml
+++ /dev/null
@@ -1,30 +0,0 @@
-# Licensed to the Apache Software Foundation (ASF) under one
-# or more contributor license agreements.  See the NOTICE file
-# distributed with this work for additional information
-# regarding copyright ownership.  The ASF licenses this file
-# to you under the Apache License, Version 2.0 (the
-# "License"); you may not use this file except in compliance
-# with the License.  You may obtain a copy of the License at
-#
-#  http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing,
-# software distributed under the License is distributed on an
-# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
-# KIND, either express or implied.  See the License for the
-# specific language governing permissions and limitations
-# under the License.
-
-syscfg.defs:
-    BLE_SVC_ANS_NEW_ALERT_CAT:
-        description: "Initial supported new alert category bitmask."
-        value: 0
-
-    BLE_SVC_ANS_UNR_ALERT_CAT:
-        description: "Initial supported unread alert category bitmask."
-        value: 0
-
-    BLE_SVC_ANS_SYSINIT_STAGE:
-        description: >
-            Sysinit stage for the alert notification service.
-        value: 303
diff --git a/libesp32/NimBLE-Arduino/src/nimble/host/services/bas/pkg.yml b/libesp32/NimBLE-Arduino/src/nimble/host/services/bas/pkg.yml
deleted file mode 100644
index afdc69421..000000000
--- a/libesp32/NimBLE-Arduino/src/nimble/host/services/bas/pkg.yml
+++ /dev/null
@@ -1,34 +0,0 @@
-#
-# Licensed to the Apache Software Foundation (ASF) under one
-# or more contributor license agreements.  See the NOTICE file
-# distributed with this work for additional information
-# regarding copyright ownership.  The ASF licenses this file
-# to you under the Apache License, Version 2.0 (the
-# "License"); you may not use this file except in compliance
-# with the License.  You may obtain a copy of the License at
-#
-#  http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing,
-# software distributed under the License is distributed on an
-# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
-# KIND, either express or implied.  See the License for the
-# specific language governing permissions and limitations
-# under the License.
-#
-
-pkg.name: nimble/host/services/bas
-pkg.description: Battery Service
-pkg.author: "Apache Mynewt <dev@mynewt.apache.org>"
-pkg.homepage: "http://mynewt.apache.org/"
-pkg.keywords:
-    - ble
-    - bluetooth
-    - bas
-    - nimble
-
-pkg.deps:
-    - nimble/host
-
-pkg.init:
-    ble_svc_bas_init: 'MYNEWT_VAL(BLE_SVC_BAS_SYSINIT_STAGE)'
diff --git a/libesp32/NimBLE-Arduino/src/nimble/host/services/bas/syscfg.yml b/libesp32/NimBLE-Arduino/src/nimble/host/services/bas/syscfg.yml
deleted file mode 100644
index 279930f14..000000000
--- a/libesp32/NimBLE-Arduino/src/nimble/host/services/bas/syscfg.yml
+++ /dev/null
@@ -1,34 +0,0 @@
-# Licensed to the Apache Software Foundation (ASF) under one
-# or more contributor license agreements.  See the NOTICE file
-# distributed with this work for additional information
-# regarding copyright ownership.  The ASF licenses this file
-# to you under the Apache License, Version 2.0 (the
-# "License"); you may not use this file except in compliance
-# with the License.  You may obtain a copy of the License at
-#
-#  http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing,
-# software distributed under the License is distributed on an
-# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
-# KIND, either express or implied.  See the License for the
-# specific language governing permissions and limitations
-# under the License.
-
-syscfg.defs:
-    BLE_SVC_BAS_BATTERY_LEVEL_READ_PERM:
-        description: >
-            Defines permissions for reading "Battery Level" characteristics. Can
-            be zero to allow read without extra permissions or combination of:
-                BLE_GATT_CHR_F_READ_ENC
-                BLE_GATT_CHR_F_READ_AUTHEN
-                BLE_GATT_CHR_F_READ_AUTHOR
-        value: 0
-    BLE_SVC_BAS_BATTERY_LEVEL_NOTIFY_ENABLE:
-        description: >
-            Set to 1 to support notification or 0 to disable it.
-        value: 1
-    BLE_SVC_BAS_SYSINIT_STAGE:
-        description: >
-            Sysinit stage for the battery level service.
-        value: 303
diff --git a/libesp32/NimBLE-Arduino/src/nimble/host/services/gap/pkg.yml b/libesp32/NimBLE-Arduino/src/nimble/host/services/gap/pkg.yml
deleted file mode 100644
index a2ef756e3..000000000
--- a/libesp32/NimBLE-Arduino/src/nimble/host/services/gap/pkg.yml
+++ /dev/null
@@ -1,34 +0,0 @@
-#
-# Licensed to the Apache Software Foundation (ASF) under one
-# or more contributor license agreements.  See the NOTICE file
-# distributed with this work for additional information
-# regarding copyright ownership.  The ASF licenses this file
-# to you under the Apache License, Version 2.0 (the
-# "License"); you may not use this file except in compliance
-# with the License.  You may obtain a copy of the License at
-#
-#  http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing,
-# software distributed under the License is distributed on an
-# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
-# KIND, either express or implied.  See the License for the
-# specific language governing permissions and limitations
-# under the License.
-#
-
-pkg.name: nimble/host/services/gap
-pkg.description: Implements the GAP Service.
-pkg.author: "Apache Mynewt <dev@mynewt.apache.org>"
-pkg.homepage: "http://mynewt.apache.org/"
-pkg.keywords:
-    - ble
-    - bluetooth
-    - nimble
-    - gap
-
-pkg.deps:
-    - nimble/host
-
-pkg.init:
-    ble_svc_gap_init: 'MYNEWT_VAL(BLE_SVC_GAP_SYSINIT_STAGE)'
diff --git a/libesp32/NimBLE-Arduino/src/nimble/host/services/gap/syscfg.yml b/libesp32/NimBLE-Arduino/src/nimble/host/services/gap/syscfg.yml
deleted file mode 100644
index ad6aa7ef3..000000000
--- a/libesp32/NimBLE-Arduino/src/nimble/host/services/gap/syscfg.yml
+++ /dev/null
@@ -1,83 +0,0 @@
-# Licensed to the Apache Software Foundation (ASF) under one
-# or more contributor license agreements.  See the NOTICE file
-# distributed with this work for additional information
-# regarding copyright ownership.  The ASF licenses this file
-# to you under the Apache License, Version 2.0 (the
-# "License"); you may not use this file except in compliance
-# with the License.  You may obtain a copy of the License at
-#
-#  http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing,
-# software distributed under the License is distributed on an
-# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
-# KIND, either express or implied.  See the License for the
-# specific language governing permissions and limitations
-# under the License.
-#
-
-syscfg.defs:
-    BLE_SVC_GAP_DEVICE_NAME:
-        description: >
-            Default value for "Device Name" characteristics, unless overwritten
-            by application.
-        value: '"nimble"'
-    BLE_SVC_GAP_DEVICE_NAME_WRITE_PERM:
-        description: >
-            Defines permissions for writing "Device Name" characteristics. Can
-            be zero to allow write without extra permissions or combination of:
-                BLE_GATT_CHR_F_WRITE_ENC
-                BLE_GATT_CHR_F_WRITE_AUTHEN
-                BLE_GATT_CHR_F_WRITE_AUTHOR
-            Set to '-1' to make characteristic read only.
-        value: -1
-    BLE_SVC_GAP_DEVICE_NAME_MAX_LENGTH:
-        description: Maximum length for "Device Name" characteristics
-        value: 31
-    BLE_SVC_GAP_APPEARANCE:
-        description: 'Device appearance'
-        value: 0
-    BLE_SVC_GAP_APPEARANCE_WRITE_PERM:
-        description: >
-            Defines permissions for writing "Appearance" characteristics. Can
-            be zero to allow write without extra permissions or combination of:
-                BLE_GATT_CHR_F_WRITE_ENC
-                BLE_GATT_CHR_F_WRITE_AUTHEN
-                BLE_GATT_CHR_F_WRITE_AUTHOR
-            Set to '-1' to make characteristic read only.
-        value: -1
-
-    # Setting all values for PPCP to '0' will disable characteristic!
-    BLE_SVC_GAP_PPCP_MIN_CONN_INTERVAL:
-        description: >
-            Value of "minimum connection interval" of PPCP characteristic as
-            defined by Core specification 5.0, Vol 3, Part C, section 12.3.
-        value: 0
-    BLE_SVC_GAP_PPCP_MAX_CONN_INTERVAL:
-        description: >
-            Value of "maximum connection interval" of PPCP characteristic as
-            defined by Core specification 5.0, Vol 3, Part C, section 12.3.
-        value: 0
-    BLE_SVC_GAP_PPCP_SLAVE_LATENCY:
-        description: >
-            Value of "slave latency" of PPCP characteristic as  defined by Core
-            specification 5.0, Vol 3, Part C, section 12.3.
-        value: 0
-    BLE_SVC_GAP_PPCP_SUPERVISION_TMO:
-        description: >
-            Value of "connection supervision timeout multiplier" of PPCP
-            characteristic as defined by Core specification 5.0, Vol 3, Part C,
-            section 12.3.
-        value: 0
-
-    BLE_SVC_GAP_CENTRAL_ADDRESS_RESOLUTION:
-        description: >
-            Value of "Central Address Resolution" characteristics, as defined
-            by Core specification 5.0, Vol 3, Part C, section 12.
-            Set to '-1' to disable.
-        value: -1
-
-    BLE_SVC_GAP_SYSINIT_STAGE:
-        description: >
-            Sysinit stage for the GAP BLE service.
-        value: 301
diff --git a/libesp32/NimBLE-Arduino/src/nimble/host/services/gatt/pkg.yml b/libesp32/NimBLE-Arduino/src/nimble/host/services/gatt/pkg.yml
deleted file mode 100644
index e3704bc18..000000000
--- a/libesp32/NimBLE-Arduino/src/nimble/host/services/gatt/pkg.yml
+++ /dev/null
@@ -1,34 +0,0 @@
-#
-# Licensed to the Apache Software Foundation (ASF) under one
-# or more contributor license agreements.  See the NOTICE file
-# distributed with this work for additional information
-# regarding copyright ownership.  The ASF licenses this file
-# to you under the Apache License, Version 2.0 (the
-# "License"); you may not use this file except in compliance
-# with the License.  You may obtain a copy of the License at
-#
-#  http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing,
-# software distributed under the License is distributed on an
-# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
-# KIND, either express or implied.  See the License for the
-# specific language governing permissions and limitations
-# under the License.
-#
-
-pkg.name: nimble/host/services/gatt
-pkg.description: Implements the GATT service.
-pkg.author: "Apache Mynewt <dev@mynewt.apache.org>"
-pkg.homepage: "http://mynewt.apache.org/"
-pkg.keywords:
-    - ble
-    - bluetooth
-    - nimble
-    - gatt
-
-pkg.deps:
-    - nimble/host
-
-pkg.init:
-    ble_svc_gatt_init: 'MYNEWT_VAL(BLE_SVC_GATT_SYSINIT_STAGE)'
diff --git a/libesp32/NimBLE-Arduino/src/nimble/host/services/gatt/syscfg.yml b/libesp32/NimBLE-Arduino/src/nimble/host/services/gatt/syscfg.yml
deleted file mode 100644
index 6ba1b333e..000000000
--- a/libesp32/NimBLE-Arduino/src/nimble/host/services/gatt/syscfg.yml
+++ /dev/null
@@ -1,24 +0,0 @@
-#
-# Licensed to the Apache Software Foundation (ASF) under one
-# or more contributor license agreements.  See the NOTICE file
-# distributed with this work for additional information
-# regarding copyright ownership.  The ASF licenses this file
-# to you under the Apache License, Version 2.0 (the
-# "License"); you may not use this file except in compliance
-# with the License.  You may obtain a copy of the License at
-#
-#  http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing,
-# software distributed under the License is distributed on an
-# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
-# KIND, either express or implied.  See the License for the
-# specific language governing permissions and limitations
-# under the License.
-#
-
-syscfg.defs:
-    BLE_SVC_GATT_SYSINIT_STAGE:
-        description: >
-            Sysinit stage for the GATT BLE service
-        value: 302
diff --git a/libesp32/NimBLE-Arduino/src/nimble/host/services/ias/pkg.yml b/libesp32/NimBLE-Arduino/src/nimble/host/services/ias/pkg.yml
deleted file mode 100644
index 3b0ca0745..000000000
--- a/libesp32/NimBLE-Arduino/src/nimble/host/services/ias/pkg.yml
+++ /dev/null
@@ -1,34 +0,0 @@
-
-# Licensed to the Apache Software Foundation (ASF) under one
-# or more contributor license agreements.  See the NOTICE file
-# distributed with this work for additional information
-# regarding copyright ownership.  The ASF licenses this file
-# to you under the Apache License, Version 2.0 (the
-# "License"); you may not use this file except in compliance
-# with the License.  You may obtain a copy of the License at
-#
-#  http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing,
-# software distributed under the License is distributed on an
-# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
-# KIND, either express or implied.  See the License for the
-# specific language governing permissions and limitations
-# under the License.
-#
-
-pkg.name: nimble/host/services/ias
-pkg.description: Immediate Alert Service Implementation.
-pkg.author: "Apache Mynewt <dev@mynewt.apache.org>"
-pkg.homepage: "http://mynewt.apache.org/"
-pkg.keywords:
-    - ble
-    - bluetooth
-    - ias
-    - nimble
-
-pkg.deps:
-    - nimble/host
-
-pkg.init:
-    ble_svc_ias_init: 'MYNEWT_VAL(BLE_SVC_IAS_SYSINIT_STAGE)'
diff --git a/libesp32/NimBLE-Arduino/src/nimble/host/services/ias/syscfg.yml b/libesp32/NimBLE-Arduino/src/nimble/host/services/ias/syscfg.yml
deleted file mode 100644
index 2cbed3ab4..000000000
--- a/libesp32/NimBLE-Arduino/src/nimble/host/services/ias/syscfg.yml
+++ /dev/null
@@ -1,23 +0,0 @@
-# Licensed to the Apache Software Foundation (ASF) under one
-# or more contributor license agreements.  See the NOTICE file
-# distributed with this work for additional information
-# regarding copyright ownership.  The ASF licenses this file
-# to you under the Apache License, Version 2.0 (the
-# "License"); you may not use this file except in compliance
-# with the License.  You may obtain a copy of the License at
-#
-#  http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing,
-# software distributed under the License is distributed on an
-# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
-# KIND, either express or implied.  See the License for the
-# specific language governing permissions and limitations
-# under the License.
-#
-
-syscfg.defs:
-    BLE_SVC_IAS_SYSINIT_STAGE:
-        description: >
-            Sysinit stage for the immediate alert BLE service.
-        value: 303
diff --git a/libesp32/NimBLE-Arduino/src/nimble/host/services/ipss/src/ble_svc_ipss.c b/libesp32/NimBLE-Arduino/src/nimble/host/services/ipss/src/ble_svc_ipss.c
new file mode 100644
index 000000000..f42ca1e9e
--- /dev/null
+++ b/libesp32/NimBLE-Arduino/src/nimble/host/services/ipss/src/ble_svc_ipss.c
@@ -0,0 +1,51 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <assert.h>
+
+#include "sysinit/sysinit.h"
+#include "host/ble_hs.h"
+#include "services/ipss/ble_svc_ipss.h"
+
+static const struct ble_gatt_svc_def ble_svc_ipss_defs[] = {
+    {
+        /*** Service: GATT */
+        .type = BLE_GATT_SVC_TYPE_PRIMARY,
+        .uuid = BLE_UUID16_DECLARE(BLE_SVC_IPSS_UUID16),
+        .characteristics = NULL,
+    },
+    {
+        0, /* No more services. */
+    },
+};
+
+void
+ble_svc_ipss_init(void)
+{
+    int rc;
+
+    /* Ensure this function only gets called by sysinit. */
+    SYSINIT_ASSERT_ACTIVE();
+
+    rc = ble_gatts_count_cfg(ble_svc_ipss_defs);
+    SYSINIT_PANIC_ASSERT(rc == 0);
+
+    rc = ble_gatts_add_svcs(ble_svc_ipss_defs);
+    SYSINIT_PANIC_ASSERT(rc == 0);
+}
diff --git a/libesp32/NimBLE-Arduino/src/nimble/host/services/lls/pkg.yml b/libesp32/NimBLE-Arduino/src/nimble/host/services/lls/pkg.yml
deleted file mode 100644
index 6160f020e..000000000
--- a/libesp32/NimBLE-Arduino/src/nimble/host/services/lls/pkg.yml
+++ /dev/null
@@ -1,34 +0,0 @@
-#
-# Licensed to the Apache Software Foundation (ASF) under one
-# or more contributor license agreements.  See the NOTICE file
-# distributed with this work for additional information
-# regarding copyright ownership.  The ASF licenses this file
-# to you under the Apache License, Version 2.0 (the
-# "License"); you may not use this file except in compliance
-# with the License.  You may obtain a copy of the License at
-#
-#  http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing,
-# software distributed under the License is distributed on an
-# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
-# KIND, either express or implied.  See the License for the
-# specific language governing permissions and limitations
-# under the License.
-#
-
-pkg.name: nimble/host/services/lls
-pkg.description: Link Loss Service Implementation.
-pkg.author: "Apache Mynewt <dev@mynewt.apache.org>"
-pkg.homepage: "http://mynewt.apache.org/"
-pkg.keywords:
-    - ble
-    - bluetooth
-    - lls
-    - nimble
-
-pkg.deps:
-    - nimble/host
-
-pkg.init:
-    ble_svc_lls_init: 'MYNEWT_VAL(BLE_SVC_LLS_SYSINIT_STAGE)'
diff --git a/libesp32/NimBLE-Arduino/src/nimble/host/services/lls/syscfg.yml b/libesp32/NimBLE-Arduino/src/nimble/host/services/lls/syscfg.yml
deleted file mode 100644
index 312b08a2f..000000000
--- a/libesp32/NimBLE-Arduino/src/nimble/host/services/lls/syscfg.yml
+++ /dev/null
@@ -1,22 +0,0 @@
-# Licensed to the Apache Software Foundation (ASF) under one
-# or more contributor license agreements.  See the NOTICE file
-# distributed with this work for additional information
-# regarding copyright ownership.  The ASF licenses this file
-# to you under the Apache License, Version 2.0 (the
-# "License"); you may not use this file except in compliance
-# with the License.  You may obtain a copy of the License at
-#
-#  http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing,
-# software distributed under the License is distributed on an
-# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
-# KIND, either express or implied.  See the License for the
-# specific language governing permissions and limitations
-# under the License.
-
-syscfg.defs:
-    BLE_SVC_LLS_SYSINIT_STAGE:
-        description: >
-            Sysinit stage for the link loss BLE service.
-        value: 303
diff --git a/libesp32/NimBLE-Arduino/src/nimble/host/services/tps/pkg.yml b/libesp32/NimBLE-Arduino/src/nimble/host/services/tps/pkg.yml
deleted file mode 100644
index 3d4c5e98e..000000000
--- a/libesp32/NimBLE-Arduino/src/nimble/host/services/tps/pkg.yml
+++ /dev/null
@@ -1,34 +0,0 @@
-
-# Licensed to the Apache Software Foundation (ASF) under one
-# or more contributor license agreements.  See the NOTICE file
-# distributed with this work for additional information
-# regarding copyright ownership.  The ASF licenses this file
-# to you under the Apache License, Version 2.0 (the
-# "License"); you may not use this file except in compliance
-# with the License.  You may obtain a copy of the License at
-#
-#  http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing,
-# software distributed under the License is distributed on an
-# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
-# KIND, either express or implied.  See the License for the
-# specific language governing permissions and limitations
-# under the License.
-#
-
-pkg.name: nimble/host/services/tps
-pkg.description: Tx Power Service adopted specification.
-pkg.author: "Apache Mynewt <dev@mynewt.apache.org>"
-pkg.homepage: "http://mynewt.apache.org/"
-pkg.keywords:
-    - ble
-    - bluetooth
-    - tps
-    - nimble
-
-pkg.deps:
-    - nimble/host
-
-pkg.init:
-    ble_svc_tps_init: 'MYNEWT_VAL(BLE_SVC_TPS_SYSINIT_STAGE)'
diff --git a/libesp32/NimBLE-Arduino/src/nimble/host/services/tps/syscfg.yml b/libesp32/NimBLE-Arduino/src/nimble/host/services/tps/syscfg.yml
deleted file mode 100644
index 0391e8b1d..000000000
--- a/libesp32/NimBLE-Arduino/src/nimble/host/services/tps/syscfg.yml
+++ /dev/null
@@ -1,23 +0,0 @@
-# Licensed to the Apache Software Foundation (ASF) under one
-# or more contributor license agreements.  See the NOTICE file
-# distributed with this work for additional information
-# regarding copyright ownership.  The ASF licenses this file
-# to you under the Apache License, Version 2.0 (the
-# "License"); you may not use this file except in compliance
-# with the License.  You may obtain a copy of the License at
-#
-#  http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing,
-# software distributed under the License is distributed on an
-# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
-# KIND, either express or implied.  See the License for the
-# specific language governing permissions and limitations
-# under the License.
-
-syscfg.defs:
-    BLE_SVC_TPS_SYSINIT_STAGE:
-        description: >
-            Sysinit stage for the transmit power BLE service.
-        value: 303
-
diff --git a/libesp32/NimBLE-Arduino/src/nimble/host/src/ble_hs_id.c b/libesp32/NimBLE-Arduino/src/nimble/host/src/ble_hs_id.c
index 43380d1f6..db32f90df 100644
--- a/libesp32/NimBLE-Arduino/src/nimble/host/src/ble_hs_id.c
+++ b/libesp32/NimBLE-Arduino/src/nimble/host/src/ble_hs_id.c
@@ -67,6 +67,37 @@ ble_hs_id_gen_rnd(int nrpa, ble_addr_t *out_addr)
 }
 
 #if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)
+/**
+ * Sets the device's pseudo Non Resolvable Private Address when 'Host based
+ * privacy' is in use.
+ *
+ * @return                      0 on success;
+ *                              Appropriate error code if failure.
+ */
+int
+ble_hs_id_set_nrpa_rnd()
+{
+
+    ble_addr_t nrpa_addr;
+    int rc;
+
+    ble_hs_id_gen_rnd(1, &nrpa_addr);
+
+    ble_hs_lock();
+
+    /* set the NRPA address as pseudo random address in controller */
+    rc = ble_hs_hci_util_set_random_addr(nrpa_addr.val);
+    if (rc != 0) {
+        goto done;
+    }
+
+    memcpy(ble_hs_id_rnd, nrpa_addr.val, BLE_DEV_ADDR_LEN);
+
+done:
+    ble_hs_unlock();
+    return rc;
+}
+
 /**
  * Sets the device's pseudo RPA address when 'Host based privacy' is in use.
  * The address type (RPA) is inferred from the most-significant bits. The
diff --git a/libesp32/NimBLE-Arduino/src/nimble/host/src/ble_hs_id_priv.h b/libesp32/NimBLE-Arduino/src/nimble/host/src/ble_hs_id_priv.h
index c031b9511..85260ec48 100644
--- a/libesp32/NimBLE-Arduino/src/nimble/host/src/ble_hs_id_priv.h
+++ b/libesp32/NimBLE-Arduino/src/nimble/host/src/ble_hs_id_priv.h
@@ -36,6 +36,7 @@ void ble_hs_id_rnd_reset(void);
 #if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)
 bool ble_hs_is_rpa(uint8_t *addr, uint8_t addr_type);
 int ble_hs_id_set_pseudo_rnd(const uint8_t *);
+int ble_hs_id_set_nrpa_rnd(void);
 #endif
 #ifdef __cplusplus
 }
diff --git a/libesp32/NimBLE-Arduino/src/nimble/host/src/ble_hs_pvcy.c b/libesp32/NimBLE-Arduino/src/nimble/host/src/ble_hs_pvcy.c
index 32f15974b..69f3da543 100644
--- a/libesp32/NimBLE-Arduino/src/nimble/host/src/ble_hs_pvcy.c
+++ b/libesp32/NimBLE-Arduino/src/nimble/host/src/ble_hs_pvcy.c
@@ -22,6 +22,7 @@
 #include "stats/stats.h"
 #include "ble_hs_priv.h"
 #include "ble_hs_resolv_priv.h"
+#include "host/ble_hs_pvcy.h"
 
 static uint8_t ble_hs_pvcy_started;
 static uint8_t ble_hs_pvcy_irk[16];
@@ -329,7 +330,7 @@ ble_hs_pvcy_rpa_config(uint8_t enable)
 {
     int rc = 0;
 
-    if (enable != 0) {
+    if (enable != NIMBLE_HOST_DISABLE_PRIVACY) {
         rc = ble_hs_pvcy_ensure_started();
         if (rc != 0) {
             return rc;
@@ -337,8 +338,15 @@ ble_hs_pvcy_rpa_config(uint8_t enable)
 
         ble_hs_resolv_enable(true);
 
+        /* Configure NRPA address related flags according to input parameter */
+        if (enable == NIMBLE_HOST_ENABLE_NRPA) {
+            ble_hs_resolv_nrpa_enable();
+        } else {
+            ble_hs_resolv_nrpa_disable();
+        }
+
         /* Generate local RPA address and set it in controller */
-        rc = ble_hs_gen_own_rpa_random();
+        rc = ble_hs_gen_own_private_rnd();
     } else {
         ble_hs_resolv_enable(false);
     }
diff --git a/libesp32/NimBLE-Arduino/src/nimble/host/src/ble_hs_resolv.c b/libesp32/NimBLE-Arduino/src/nimble/host/src/ble_hs_resolv.c
index 7f2413898..60c77c9b8 100644
--- a/libesp32/NimBLE-Arduino/src/nimble/host/src/ble_hs_resolv.c
+++ b/libesp32/NimBLE-Arduino/src/nimble/host/src/ble_hs_resolv.c
@@ -50,6 +50,9 @@ struct ble_hs_resolv_data {
     struct ble_npl_callout rpa_timer;
 };
 
+/* NRPA bit: Enables NRPA as private address. */
+static bool nrpa_pvcy;
+
 /*** APIs for Peer Device Records.
  *
  * These Peer records are necessary to take care of Peers with RPA address when
@@ -305,6 +308,10 @@ is_ble_hs_resolv_enabled(void)
 bool
 ble_host_rpa_enabled(void)
 {
+    if (nrpa_pvcy) {
+        return false;
+    }
+
     if (is_ble_hs_resolv_enabled() && ble_hs_pvcy_enabled()) {
         return true;
     }
@@ -385,12 +392,16 @@ ble_hs_resolv_gen_priv_addr(struct ble_hs_resolv_entry *rl, int local)
     addr[2] = ecb.cipher_text[13];
 }
 
-/* Called to generate RPA address and this address is set in controller as
- * Random address. This is necessary in Host based privacy because controller is unaware of RPA
- * address is being used */
+/* Called to generate private (RPA/NRPA) address and this address is set in controller as
+ * Random address. This is necessary in Host based privacy because controller
+ * is unaware of private address is being used */
 int
-ble_hs_gen_own_rpa_random(void)
+ble_hs_gen_own_private_rnd(void)
 {
+    if (nrpa_pvcy) {
+        return ble_hs_id_set_nrpa_rnd();
+    }
+
     struct ble_hs_resolv_entry *rl = &g_ble_hs_resolv_list[0];
 
     ble_hs_resolv_gen_priv_addr(rl, 1);
@@ -412,12 +423,11 @@ ble_hs_get_rpa_local(void)
 static void
 ble_hs_resolv_rpa_timer_cb(struct ble_npl_event *ev)
 {
-    if (ble_host_rpa_enabled()) {
-        BLE_HS_LOG(DEBUG, "RPA Timeout; start active adv & scan with new RPA\n");
-
+    if (ble_host_rpa_enabled() || (nrpa_pvcy)) {
+        BLE_HS_LOG(DEBUG, "RPA/NRPA Timeout; start active adv & scan with new Private address \n");
         ble_gap_preempt();
-        /* Generate local RPA */
-        ble_hs_gen_own_rpa_random();
+        /* Generate local private address */
+        ble_hs_gen_own_private_rnd();
         ble_npl_callout_reset(&g_ble_hs_resolv_data.rpa_timer,
                               (int32_t)g_ble_hs_resolv_data.rpa_tmo);
         ble_gap_preempt_done();
@@ -595,6 +605,23 @@ ble_hs_resolv_list_clear_all(void)
     return;
 }
 
+/**
+* Called by host stack to enable NRPA privacy flag for future reference
+*/
+void
+ble_hs_resolv_nrpa_enable(void)
+{
+    nrpa_pvcy = true;
+}
+
+/**
+* Called by host stack to disable NRPA privacy flag
+*/
+void
+ble_hs_resolv_nrpa_disable(void)
+{
+    nrpa_pvcy = false;
+}
 /**
  * Called to enable or disable address resolution in the host
  *
diff --git a/libesp32/NimBLE-Arduino/src/nimble/host/src/ble_hs_resolv_priv.h b/libesp32/NimBLE-Arduino/src/nimble/host/src/ble_hs_resolv_priv.h
index 568aa89ab..e76a26a55 100644
--- a/libesp32/NimBLE-Arduino/src/nimble/host/src/ble_hs_resolv_priv.h
+++ b/libesp32/NimBLE-Arduino/src/nimble/host/src/ble_hs_resolv_priv.h
@@ -61,7 +61,7 @@ struct ble_hs_dev_records {
 
 /* Add a device to the resolving list */
 int ble_hs_resolv_list_add(uint8_t *cmdbuf);
-int ble_hs_gen_own_rpa_random(void);
+int ble_hs_gen_own_private_rnd(void);
 uint8_t *ble_hs_get_rpa_local(void);
 
 /* Remove a device from the resolving list */
@@ -71,6 +71,8 @@ void ble_hs_resolv_list_clear_all(void);
 
 /* Address resolution enable command */
 void ble_hs_resolv_enable(bool);
+void ble_hs_resolv_nrpa_enable(void);
+void ble_hs_resolv_nrpa_disable(void);
 
 /* Finds 'addr' in resolving list. Doesnt check if address resolution enabled */
 struct ble_hs_resolv_entry *
diff --git a/libesp32/NimBLE-Arduino/src/nimble/host/src/ble_uuid.c b/libesp32/NimBLE-Arduino/src/nimble/host/src/ble_uuid.c
index 16352cf6e..3a1642480 100644
--- a/libesp32/NimBLE-Arduino/src/nimble/host/src/ble_uuid.c
+++ b/libesp32/NimBLE-Arduino/src/nimble/host/src/ble_uuid.c
@@ -239,7 +239,10 @@ ble_uuid_flat(const ble_uuid_t *uuid, void *dst)
         break;
     case BLE_UUID_TYPE_32:
         memcpy(dst, ble_uuid_base, 16);
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wpointer-arith"
         put_le32(dst + 12, BLE_UUID32(uuid)->value);
+#pragma GCC diagnostic pop
         break;
     case BLE_UUID_TYPE_128:
         memcpy(dst, BLE_UUID128(uuid)->value, 16);
diff --git a/libesp32/NimBLE-Arduino/src/nimble/host/store/config/pkg.yml b/libesp32/NimBLE-Arduino/src/nimble/host/store/config/pkg.yml
deleted file mode 100644
index db80d1df8..000000000
--- a/libesp32/NimBLE-Arduino/src/nimble/host/store/config/pkg.yml
+++ /dev/null
@@ -1,38 +0,0 @@
-#
-# Licensed to the Apache Software Foundation (ASF) under one
-# or more contributor license agreements.  See the NOTICE file
-# distributed with this work for additional information
-# regarding copyright ownership.  The ASF licenses this file
-# to you under the Apache License, Version 2.0 (the
-# "License"); you may not use this file except in compliance
-# with the License.  You may obtain a copy of the License at
-#
-#  http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing,
-# software distributed under the License is distributed on an
-# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
-# KIND, either express or implied.  See the License for the
-# specific language governing permissions and limitations
-# under the License.
-#
-
-pkg.name: nimble/host/store/config
-pkg.description: sys/config-based persistence layer for the NimBLE host.
-pkg.author: "Apache Mynewt <dev@mynewt.apache.org>"
-pkg.homepage: "http://mynewt.apache.org/"
-pkg.keywords:
-    - ble
-    - bluetooth
-    - nimble
-    - persistence
-
-pkg.deps:
-    - "@apache-mynewt-core/encoding/base64"
-    - nimble/host
-
-pkg.deps.BLE_STORE_CONFIG_PERSIST:
-    - "@apache-mynewt-core/sys/config"
-
-pkg.init:
-    ble_store_config_init: 'MYNEWT_VAL(BLE_STORE_SYSINIT_STAGE)'
diff --git a/libesp32/NimBLE-Arduino/src/nimble/host/store/config/src/ble_store_config_conf.unused b/libesp32/NimBLE-Arduino/src/nimble/host/store/config/src/ble_store_config_conf.unused
deleted file mode 100644
index e74127ae9..000000000
--- a/libesp32/NimBLE-Arduino/src/nimble/host/store/config/src/ble_store_config_conf.unused
+++ /dev/null
@@ -1,231 +0,0 @@
-/*
- * Licensed to the Apache Software Foundation (ASF) under one
- * or more contributor license agreements.  See the NOTICE file
- * distributed with this work for additional information
- * regarding copyright ownership.  The ASF licenses this file
- * to you under the Apache License, Version 2.0 (the
- * "License"); you may not use this file except in compliance
- * with the License.  You may obtain a copy of the License at
- *
- *  http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing,
- * software distributed under the License is distributed on an
- * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
- * KIND, either express or implied.  See the License for the
- * specific language governing permissions and limitations
- * under the License.
- */
-
-#include "syscfg/syscfg.h"
-
-#if MYNEWT_VAL(BLE_STORE_CONFIG_PERSIST)
-
-#include <inttypes.h>
-#include <string.h>
-
-#include "sysinit/sysinit.h"
-#include "host/ble_hs.h"
-#include "config/config.h"
-#include "base64/base64.h"
-#include "store/config/ble_store_config.h"
-#include "ble_store_config_priv.h"
-
-static int
-ble_store_config_conf_set(int argc, char **argv, char *val);
-static int
-ble_store_config_conf_export(void (*func)(char *name, char *val),
-                             enum conf_export_tgt tgt);
-
-static struct conf_handler ble_store_config_conf_handler = {
-    .ch_name = "ble_hs",
-    .ch_get = NULL,
-    .ch_set = ble_store_config_conf_set,
-    .ch_commit = NULL,
-    .ch_export = ble_store_config_conf_export
-};
-
-#define BLE_STORE_CONFIG_SEC_ENCODE_SZ      \
-    BASE64_ENCODE_SIZE(sizeof (struct ble_store_value_sec))
-
-#define BLE_STORE_CONFIG_SEC_SET_ENCODE_SZ  \
-    (MYNEWT_VAL(BLE_STORE_MAX_BONDS) * BLE_STORE_CONFIG_SEC_ENCODE_SZ + 1)
-
-#define BLE_STORE_CONFIG_CCCD_ENCODE_SZ     \
-    BASE64_ENCODE_SIZE(sizeof (struct ble_store_value_cccd))
-
-#define BLE_STORE_CONFIG_CCCD_SET_ENCODE_SZ \
-    (MYNEWT_VAL(BLE_STORE_MAX_CCCDS) * BLE_STORE_CONFIG_CCCD_ENCODE_SZ + 1)
-
-static void
-ble_store_config_serialize_arr(const void *arr, int obj_sz, int num_objs,
-                               char *out_buf, int buf_sz)
-{
-    int arr_size;
-
-    arr_size = obj_sz * num_objs;
-    assert(arr_size <= buf_sz);
-
-    base64_encode(arr, arr_size, out_buf, 1);
-}
-
-static int
-ble_store_config_deserialize_arr(const char *enc,
-                                 void *out_arr,
-                                 int obj_sz,
-                                 int *out_num_objs)
-{
-    int len;
-
-    len = base64_decode(enc, out_arr);
-    if (len < 0) {
-        return OS_EINVAL;
-    }
-
-    *out_num_objs = len / obj_sz;
-    return 0;
-}
-
-static int
-ble_store_config_conf_set(int argc, char **argv, char *val)
-{
-    int rc;
-
-    if (argc == 1) {
-        if (strcmp(argv[0], "our_sec") == 0) {
-            rc = ble_store_config_deserialize_arr(
-                    val,
-                    ble_store_config_our_secs,
-                    sizeof *ble_store_config_our_secs,
-                    &ble_store_config_num_our_secs);
-            return rc;
-        } else if (strcmp(argv[0], "peer_sec") == 0) {
-            rc = ble_store_config_deserialize_arr(
-                    val,
-                    ble_store_config_peer_secs,
-                    sizeof *ble_store_config_peer_secs,
-                    &ble_store_config_num_peer_secs);
-            return rc;
-        } else if (strcmp(argv[0], "cccd") == 0) {
-            rc = ble_store_config_deserialize_arr(
-                    val,
-                    ble_store_config_cccds,
-                    sizeof *ble_store_config_cccds,
-                    &ble_store_config_num_cccds);
-            return rc;
-        }
-    }
-    return OS_ENOENT;
-}
-
-static int
-ble_store_config_conf_export(void (*func)(char *name, char *val),
-                             enum conf_export_tgt tgt)
-{
-    union {
-        char sec[BLE_STORE_CONFIG_SEC_SET_ENCODE_SZ];
-        char cccd[BLE_STORE_CONFIG_CCCD_SET_ENCODE_SZ];
-    } buf;
-
-    ble_store_config_serialize_arr(ble_store_config_our_secs,
-                                   sizeof *ble_store_config_our_secs,
-                                   ble_store_config_num_our_secs,
-                                   buf.sec,
-                                   sizeof buf.sec);
-    func("ble_hs/our_sec", buf.sec);
-
-    ble_store_config_serialize_arr(ble_store_config_peer_secs,
-                                   sizeof *ble_store_config_peer_secs,
-                                   ble_store_config_num_peer_secs,
-                                   buf.sec,
-                                   sizeof buf.sec);
-    func("ble_hs/peer_sec", buf.sec);
-
-    ble_store_config_serialize_arr(ble_store_config_cccds,
-                                   sizeof *ble_store_config_cccds,
-                                   ble_store_config_num_cccds,
-                                   buf.cccd,
-                                   sizeof buf.cccd);
-    func("ble_hs/cccd", buf.cccd);
-
-    return 0;
-}
-
-static int
-ble_store_config_persist_sec_set(const char *setting_name,
-                                 const struct ble_store_value_sec *secs,
-                                 int num_secs)
-{
-    char buf[BLE_STORE_CONFIG_SEC_SET_ENCODE_SZ];
-    int rc;
-
-    ble_store_config_serialize_arr(secs, sizeof *secs, num_secs,
-                                   buf, sizeof buf);
-    rc = conf_save_one(setting_name, buf);
-    if (rc != 0) {
-        return BLE_HS_ESTORE_FAIL;
-    }
-
-    return 0;
-}
-
-int
-ble_store_config_persist_our_secs(void)
-{
-    int rc;
-
-    rc = ble_store_config_persist_sec_set("ble_hs/our_sec",
-                                          ble_store_config_our_secs,
-                                          ble_store_config_num_our_secs);
-    if (rc != 0) {
-        return rc;
-    }
-
-    return 0;
-}
-
-int
-ble_store_config_persist_peer_secs(void)
-{
-    int rc;
-
-    rc = ble_store_config_persist_sec_set("ble_hs/peer_sec",
-                                          ble_store_config_peer_secs,
-                                          ble_store_config_num_peer_secs);
-    if (rc != 0) {
-        return rc;
-    }
-
-    return 0;
-}
-
-int
-ble_store_config_persist_cccds(void)
-{
-    char buf[BLE_STORE_CONFIG_CCCD_SET_ENCODE_SZ];
-    int rc;
-
-    ble_store_config_serialize_arr(ble_store_config_cccds,
-                                   sizeof *ble_store_config_cccds,
-                                   ble_store_config_num_cccds,
-                                   buf,
-                                   sizeof buf);
-    rc = conf_save_one("ble_hs/cccd", buf);
-    if (rc != 0) {
-        return BLE_HS_ESTORE_FAIL;
-    }
-
-    return 0;
-}
-
-void
-ble_store_config_conf_init(void)
-{
-    int rc;
-
-    rc = conf_register(&ble_store_config_conf_handler);
-    SYSINIT_PANIC_ASSERT_MSG(rc == 0,
-                             "Failed to register ble_store_config conf");
-}
-
-#endif /* MYNEWT_VAL(BLE_STORE_CONFIG_PERSIST) */
diff --git a/libesp32/NimBLE-Arduino/src/nimble/host/store/config/syscfg.yml b/libesp32/NimBLE-Arduino/src/nimble/host/store/config/syscfg.yml
deleted file mode 100644
index ff0689c6d..000000000
--- a/libesp32/NimBLE-Arduino/src/nimble/host/store/config/syscfg.yml
+++ /dev/null
@@ -1,27 +0,0 @@
-# Licensed to the Apache Software Foundation (ASF) under one
-# or more contributor license agreements.  See the NOTICE file
-# distributed with this work for additional information
-# regarding copyright ownership.  The ASF licenses this file
-# to you under the Apache License, Version 2.0 (the
-# "License"); you may not use this file except in compliance
-# with the License.  You may obtain a copy of the License at
-#
-#  http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing,
-# software distributed under the License is distributed on an
-# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
-# KIND, either express or implied.  See the License for the
-# specific language governing permissions and limitations
-# under the License.
-#
-
-syscfg.defs:
-    BLE_STORE_CONFIG_PERSIST:
-        description: >
-            Whether to save data to sys/config, or just keep it in RAM.
-        value: 1
-    BLE_STORE_SYSINIT_STAGE:
-        description: >
-            Sysinit stage for BLE host store.
-        value: 500
diff --git a/libesp32/NimBLE-Arduino/src/nimble/host/store/ram/pkg.yml b/libesp32/NimBLE-Arduino/src/nimble/host/store/ram/pkg.yml
deleted file mode 100644
index 68765fddf..000000000
--- a/libesp32/NimBLE-Arduino/src/nimble/host/store/ram/pkg.yml
+++ /dev/null
@@ -1,37 +0,0 @@
-#
-# Licensed to the Apache Software Foundation (ASF) under one
-# or more contributor license agreements.  See the NOTICE file
-# distributed with this work for additional information
-# regarding copyright ownership.  The ASF licenses this file
-# to you under the Apache License, Version 2.0 (the
-# "License"); you may not use this file except in compliance
-# with the License.  You may obtain a copy of the License at
-#
-#  http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing,
-# software distributed under the License is distributed on an
-# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
-# KIND, either express or implied.  See the License for the
-# specific language governing permissions and limitations
-# under the License.
-#
-
-pkg.name: nimble/host/store/ram
-pkg.description: >
-    DEPRECATED; for a RAM-only BLE store, use store/config and set
-    BLE_STORE_CONFIG_PERSIST to 0.  RAM-based persistence layer for the NimBLE
-    host.
-pkg.author: "Apache Mynewt <dev@mynewt.apache.org>"
-pkg.homepage: "http://mynewt.apache.org/"
-pkg.keywords:
-    - ble
-    - bluetooth
-    - nimble
-    - persistence
-
-pkg.deps:
-    - nimble/host
-
-pkg.init:
-    ble_store_ram_init: 'MYNEWT_VAL(BLE_STORE_RAM_SYSINIT_STAGE)'
diff --git a/libesp32/NimBLE-Arduino/src/nimble/host/store/ram/syscfg.yml b/libesp32/NimBLE-Arduino/src/nimble/host/store/ram/syscfg.yml
deleted file mode 100644
index 442211ddd..000000000
--- a/libesp32/NimBLE-Arduino/src/nimble/host/store/ram/syscfg.yml
+++ /dev/null
@@ -1,23 +0,0 @@
-# Licensed to the Apache Software Foundation (ASF) under one
-# or more contributor license agreements.  See the NOTICE file
-# distributed with this work for additional information
-# regarding copyright ownership.  The ASF licenses this file
-# to you under the Apache License, Version 2.0 (the
-# "License"); you may not use this file except in compliance
-# with the License.  You may obtain a copy of the License at
-#
-#  http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing,
-# software distributed under the License is distributed on an
-# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
-# KIND, either express or implied.  See the License for the
-# specific language governing permissions and limitations
-# under the License.
-
-syscfg.defs:
-    BLE_STORE_RAM_SYSINIT_STAGE:
-        description: >
-            Sysinit stage for the RAM BLE store.
-        value: 500
-
diff --git a/libesp32/NimBLE-Arduino/src/nimble/host/util/pkg.yml b/libesp32/NimBLE-Arduino/src/nimble/host/util/pkg.yml
deleted file mode 100644
index 0f5f3a5de..000000000
--- a/libesp32/NimBLE-Arduino/src/nimble/host/util/pkg.yml
+++ /dev/null
@@ -1,29 +0,0 @@
-#
-# Licensed to the Apache Software Foundation (ASF) under one
-# or more contributor license agreements.  See the NOTICE file
-# distributed with this work for additional information
-# regarding copyright ownership.  The ASF licenses this file
-# to you under the Apache License, Version 2.0 (the
-# "License"); you may not use this file except in compliance
-# with the License.  You may obtain a copy of the License at
-#
-#  http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing,
-# software distributed under the License is distributed on an
-# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
-# KIND, either express or implied.  See the License for the
-# specific language governing permissions and limitations
-# under the License.
-#
-
-pkg.name: nimble/host/util
-pkg.description: Supplementary utilities for the NimBLE host
-pkg.author: "Apache Mynewt <dev@mynewt.apache.org>"
-pkg.homepage: "http://mynewt.apache.org/"
-pkg.keywords:
-    - ble
-    - bluetooth
-
-pkg.deps:
-    - nimble/host
diff --git a/libesp32/NimBLE-Arduino/src/nimble/host/util/syscfg.yml b/libesp32/NimBLE-Arduino/src/nimble/host/util/syscfg.yml
deleted file mode 100644
index 2cdd57468..000000000
--- a/libesp32/NimBLE-Arduino/src/nimble/host/util/syscfg.yml
+++ /dev/null
@@ -1,19 +0,0 @@
-# Licensed to the Apache Software Foundation (ASF) under one
-# or more contributor license agreements.  See the NOTICE file
-# distributed with this work for additional information
-# regarding copyright ownership.  The ASF licenses this file
-# to you under the Apache License, Version 2.0 (the
-# "License"); you may not use this file except in compliance
-# with the License.  You may obtain a copy of the License at
-#
-#  http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing,
-# software distributed under the License is distributed on an
-# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
-# KIND, either express or implied.  See the License for the
-# specific language governing permissions and limitations
-# under the License.
-#
-
-syscfg.defs:
diff --git a/libesp32/NimBLE-Arduino/src/nimconfig.h b/libesp32/NimBLE-Arduino/src/nimconfig.h
index 0be27c198..907e6bc26 100644
--- a/libesp32/NimBLE-Arduino/src/nimconfig.h
+++ b/libesp32/NimBLE-Arduino/src/nimconfig.h
@@ -9,27 +9,27 @@
 /* Detect if using ESP-IDF or Arduino (Arduino won't have these defines in sdkconfig)*/
 #if defined(CONFIG_BT_NIMBLE_TASK_STACK_SIZE) || defined(CONFIG_NIMBLE_TASK_STACK_SIZE)
 
-#if defined(CONFIG_NIMBLE_ENABLED)
+#if defined(CONFIG_NIMBLE_ENABLED) && !defined(CONFIG_BT_NIMBLE_ENABLED)
 #define CONFIG_BT_NIMBLE_ENABLED
 #endif
 
-#if defined(CONFIG_NIMBLE_ROLE_OBSERVER)
+#if defined(CONFIG_NIMBLE_ROLE_OBSERVER) && !defined(CONFIG_BT_NIMBLE_ROLE_OBSERVER)
 #define CONFIG_BT_NIMBLE_ROLE_OBSERVER
 #endif
 
-#if defined(CONFIG_NIMBLE_ROLE_BROADCASTER)
+#if defined(CONFIG_NIMBLE_ROLE_BROADCASTER) && !defined(CONFIG_BT_NIMBLE_ROLE_BROADCASTER)
 #define CONFIG_BT_NIMBLE_ROLE_BROADCASTER
 #endif
 
-#if defined(CONFIG_NIMBLE_ROLE_CENTRAL)
+#if defined(CONFIG_NIMBLE_ROLE_CENTRAL) && !defined(CONFIG_BT_NIMBLE_ROLE_CENTRAL)
 #define CONFIG_BT_NIMBLE_ROLE_CENTRAL
 #endif
 
-#if defined(CONFIG_NIMBLE_ROLE_PERIPHERAL)
+#if defined(CONFIG_NIMBLE_ROLE_PERIPHERAL) && !defined(CONFIG_BT_NIMBLE_ROLE_PERIPHERAL)
 #define CONFIG_BT_NIMBLE_ROLE_PERIPHERAL
 #endif
 
-#if defined(CONFIG_NIMBLE_DEBUG)
+#if defined(CONFIG_NIMBLE_DEBUG) && !defined(CONFIG_BT_NIMBLE_DEBUG)
 #define CONFIG_BT_NIMBLE_DEBUG
 #endif
 
@@ -52,12 +52,12 @@
 /** Comment out if not using NimBLE Server functions
  *  Reduces flash size by approx. 16kB.
  */
-// #define CONFIG_BT_NIMBLE_ROLE_PERIPHERAL // Tasmota
+// #define CONFIG_BT_NIMBLE_ROLE_PERIPHERAL
 
 /** Comment out if not using NimBLE Advertising functions
  *  Reduces flash size by approx. 5kB.
  */
-// #define CONFIG_BT_NIMBLE_ROLE_BROADCASTER // Tasmota
+// #define CONFIG_BT_NIMBLE_ROLE_BROADCASTER
 
 /** Uncomment to see debug log messages from the NimBLE host
  *  Uses approx. 32kB of flash memory.
@@ -89,12 +89,12 @@
 #define CONFIG_BT_NIMBLE_MAX_CONNECTIONS 3
 
 /** Sets the number of devices allowed to store/bond with */
-#define CONFIG_BT_NIMBLE_MAX_BONDS 3 // Tasmota
+#define CONFIG_BT_NIMBLE_MAX_BONDS 3
 
 /** Sets the number of CCCD's to store per bonded device */
-#define CONFIG_BT_NIMBLE_MAX_CCCDS 3 // Tasmota
+#define CONFIG_BT_NIMBLE_MAX_CCCDS 8
 
-#define CONFIG_BT_NIMBLE_NVS_PERSIST 0 // Tasmota
+#define CONFIG_BT_NIMBLE_NVS_PERSIST 0
 #define CONFIG_BT_NIMBLE_SM_LEGACY 1
 #define CONFIG_BT_NIMBLE_SM_SC 1
 #define CONFIG_BT_NIMBLE_SVC_GAP_DEVICE_NAME "nimble"
diff --git a/libesp32/NimBLE-Arduino/src/porting/nimble/Makefile.controller b/libesp32/NimBLE-Arduino/src/porting/nimble/Makefile.controller
deleted file mode 100644
index 8479ac1c5..000000000
--- a/libesp32/NimBLE-Arduino/src/porting/nimble/Makefile.controller
+++ /dev/null
@@ -1,32 +0,0 @@
-#
-# Licensed to the Apache Software Foundation (ASF) under one
-# or more contributor license agreements.  See the NOTICE file
-# distributed with this work for additional information
-# regarding copyright ownership.  The ASF licenses this file
-# to you under the Apache License, Version 2.0 (the
-# "License"); you may not use this file except in compliance
-# with the License.  You may obtain a copy of the License at
-#  *  http://www.apache.org/licenses/LICENSE-2.0
-#  * Unless required by applicable law or agreed to in writing,
-#  software distributed under the License is distributed on an
-# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
-# KIND, either express or implied.  See the License for the
-# specific language governing permissions and limitations
-# under the License.
-#
-
-NIMBLE_CFLAGS += \
-	-DNIMBLE_CFG_CONTROLLER=1 \
-
-NIMBLE_INCLUDE += \
-	$(NIMBLE_ROOT)/nimble/transport/ram/include \
-	$(NIMBLE_ROOT)/nimble/controller/include \
-	$(NIMBLE_ROOT)/nimble/drivers/nrf52/include \
-	$(NULL)
-
-NIMBLE_SRC += \
-	$(filter-out $(NIMBLE_IGNORE), $(wildcard $(NIMBLE_ROOT)/nimble/transport/ram/src/*.c)) \
-	$(filter-out $(NIMBLE_IGNORE), $(wildcard $(NIMBLE_ROOT)/nimble/controller/src/*.c)) \
-	$(filter-out $(NIMBLE_IGNORE), $(wildcard $(NIMBLE_ROOT)/nimble/drivers/nrf52/src/*.c)) \
-	$(filter-out $(NIMBLE_IGNORE), $(wildcard $(NIMBLE_ROOT)/porting/nimble/controller/src/*.c)) \
-	$(NULL)
diff --git a/libesp32/NimBLE-Arduino/src/porting/nimble/Makefile.defs b/libesp32/NimBLE-Arduino/src/porting/nimble/Makefile.defs
deleted file mode 100644
index ffb531fb6..000000000
--- a/libesp32/NimBLE-Arduino/src/porting/nimble/Makefile.defs
+++ /dev/null
@@ -1,68 +0,0 @@
-#
-# Licensed to the Apache Software Foundation (ASF) under one
-# or more contributor license agreements.  See the NOTICE file
-# distributed with this work for additional information
-# regarding copyright ownership.  The ASF licenses this file
-# to you under the Apache License, Version 2.0 (the
-# "License"); you may not use this file except in compliance
-# with the License.  You may obtain a copy of the License at
-#  *  http://www.apache.org/licenses/LICENSE-2.0
-#  * Unless required by applicable law or agreed to in writing,
-#  software distributed under the License is distributed on an
-# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
-# KIND, either express or implied.  See the License for the
-# specific language governing permissions and limitations
-# under the License.
-#
-
-ifeq (,$(NIMBLE_ROOT))
-$(error NIMBLE_ROOT shall be defined)
-endif
-
-NIMBLE_CFLAGS :=
-
-NIMBLE_INCLUDE := \
-	$(NIMBLE_ROOT)/nimble/include \
-	$(NIMBLE_ROOT)/nimble/host/include \
-	$(NIMBLE_ROOT)/nimble/host/services/ans/include \
-	$(NIMBLE_ROOT)/nimble/host/services/bas/include \
-	$(NIMBLE_ROOT)/nimble/host/services/bleuart/include \
-	$(NIMBLE_ROOT)/nimble/host/services/gap/include \
-	$(NIMBLE_ROOT)/nimble/host/services/gatt/include \
-	$(NIMBLE_ROOT)/nimble/host/services/ias/include \
-	$(NIMBLE_ROOT)/nimble/host/services/lls/include \
-	$(NIMBLE_ROOT)/nimble/host/services/tps/include \
-	$(NIMBLE_ROOT)/nimble/host/store/ram/include \
-	$(NIMBLE_ROOT)/nimble/host/util/include \
-	$(NIMBLE_ROOT)/porting/nimble/include \
-	$(NULL)
-
-NIMBLE_SRC := \
-	$(filter-out $(NIMBLE_IGNORE), $(wildcard $(NIMBLE_ROOT)/porting/nimble/src/*.c)) \
-	$(filter-out $(NIMBLE_IGNORE), $(wildcard $(NIMBLE_ROOT)/nimble/src/*.c)) \
-	$(filter-out $(NIMBLE_IGNORE), $(wildcard $(NIMBLE_ROOT)/nimble/host/src/*.c)) \
-	$(filter-out $(NIMBLE_IGNORE), $(wildcard $(NIMBLE_ROOT)/nimble/host/util/src/*.c)) \
-	$(filter-out $(NIMBLE_IGNORE), $(wildcard $(NIMBLE_ROOT)/nimble/host/services/ans/src/*.c)) \
-	$(filter-out $(NIMBLE_IGNORE), $(wildcard $(NIMBLE_ROOT)/nimble/host/services/bas/src/*.c)) \
-	$(filter-out $(NIMBLE_IGNORE), $(wildcard $(NIMBLE_ROOT)/nimble/host/services/gap/src/*.c)) \
-	$(filter-out $(NIMBLE_IGNORE), $(wildcard $(NIMBLE_ROOT)/nimble/host/services/gatt/src/*.c)) \
-	$(filter-out $(NIMBLE_IGNORE), $(wildcard $(NIMBLE_ROOT)/nimble/host/services/ias/src/*.c)) \
-	$(filter-out $(NIMBLE_IGNORE), $(wildcard $(NIMBLE_ROOT)/nimble/host/services/lls/src/*.c)) \
-	$(filter-out $(NIMBLE_IGNORE), $(wildcard $(NIMBLE_ROOT)/nimble/host/services/tps/src/*.c)) \
-	$(filter-out $(NIMBLE_IGNORE), $(wildcard $(NIMBLE_ROOT)/nimble/host/store/ram/src/*.c)) \
-	$(NULL)
-
-ifneq (,$(NIMBLE_CFG_CONTROLLER))
-include $(NIMBLE_ROOT)/porting/nimble/Makefile.controller
-endif
-
-# TinyCrypt (for SM)
-ifneq (,$(NIMBLE_CFG_TINYCRYPT))
-include $(NIMBLE_ROOT)/porting/nimble/Makefile.tinycrypt
-endif
-
-ifneq (,$(NIMBLE_CFG_MESH))
-include $(NIMBLE_ROOT)/porting/nimble/Makefile.mesh
-endif
-
-NIMBLE_OBJ := $(NIMBLE_SRC:.c=.o)
diff --git a/libesp32/NimBLE-Arduino/src/porting/nimble/Makefile.mesh b/libesp32/NimBLE-Arduino/src/porting/nimble/Makefile.mesh
deleted file mode 100644
index e7f6ccec6..000000000
--- a/libesp32/NimBLE-Arduino/src/porting/nimble/Makefile.mesh
+++ /dev/null
@@ -1,24 +0,0 @@
-#
-# Licensed to the Apache Software Foundation (ASF) under one
-# or more contributor license agreements.  See the NOTICE file
-# distributed with this work for additional information
-# regarding copyright ownership.  The ASF licenses this file
-# to you under the Apache License, Version 2.0 (the
-# "License"); you may not use this file except in compliance
-# with the License.  You may obtain a copy of the License at
-#  *  http://www.apache.org/licenses/LICENSE-2.0
-#  * Unless required by applicable law or agreed to in writing,
-#  software distributed under the License is distributed on an
-# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
-# KIND, either express or implied.  See the License for the
-# specific language governing permissions and limitations
-# under the License.
-#
-
-NIMBLE_INCLUDE += \
-	$(NIMBLE_ROOT)/nimble/host/mesh/include \
-	$(NULL)
-
-NIMBLE_SRC += \
-	$(filter-out $(NIMBLE_IGNORE), $(wildcard $(NIMBLE_ROOT)/nimble/host/mesh/src/*.c)) \
-	$(NULL)
diff --git a/libesp32/NimBLE-Arduino/src/porting/nimble/Makefile.tinycrypt b/libesp32/NimBLE-Arduino/src/porting/nimble/Makefile.tinycrypt
deleted file mode 100644
index 2333e6188..000000000
--- a/libesp32/NimBLE-Arduino/src/porting/nimble/Makefile.tinycrypt
+++ /dev/null
@@ -1,26 +0,0 @@
-#
-# Licensed to the Apache Software Foundation (ASF) under one
-# or more contributor license agreements.  See the NOTICE file
-# distributed with this work for additional information
-# regarding copyright ownership.  The ASF licenses this file
-# to you under the Apache License, Version 2.0 (the
-# "License"); you may not use this file except in compliance
-# with the License.  You may obtain a copy of the License at
-#  *  http://www.apache.org/licenses/LICENSE-2.0
-#  * Unless required by applicable law or agreed to in writing,
-#  software distributed under the License is distributed on an
-# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
-# KIND, either express or implied.  See the License for the
-# specific language governing permissions and limitations
-# under the License.
-#
-
-TINYCRYPT_CFLAGS := -std=c99
-
-TINYCRYPT_INCLUDE := \
-	$(NIMBLE_ROOT)/ext/tinycrypt/include \
-	$(NULL)
-
-TINYCRYPT_SRC := \
-	$(filter-out $(NIMBLE_IGNORE), $(wildcard $(NIMBLE_ROOT)/ext/tinycrypt/src/*.c)) \
-	$(NULL)
diff --git a/libesp32/NimBLE-Arduino/src/porting/nimble/pkg.yml b/libesp32/NimBLE-Arduino/src/porting/nimble/pkg.yml
deleted file mode 100644
index d56c90dbe..000000000
--- a/libesp32/NimBLE-Arduino/src/porting/nimble/pkg.yml
+++ /dev/null
@@ -1,43 +0,0 @@
-#
-# Licensed to the Apache Software Foundation (ASF) under one
-# or more contributor license agreements.  See the NOTICE file
-# distributed with this work for additional information
-# regarding copyright ownership.  The ASF licenses this file
-# to you under the Apache License, Version 2.0 (the
-# "License"); you may not use this file except in compliance
-# with the License.  You may obtain a copy of the License at
-# 
-#  http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing,
-# software distributed under the License is distributed on an
-# "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
-# KIND, either express or implied.  See the License for the
-# specific language governing permissions and limitations
-# under the License.
-#
-
-pkg.name: porting/nimble
-pkg.type: app
-pkg.description: Stub for NimBLE porting
-pkg.author: "Apache Mynewt <dev@mynewt.incubator.apache.org>"
-pkg.homepage: "http://mynewt.apache.org/"
-pkg.keywords:
-
-pkg.deps:
-    - nimble/host
-    - nimble/host/services/ans
-    - nimble/host/services/bas
-    - nimble/host/services/gap
-    - nimble/host/services/gatt
-    - nimble/host/services/ias
-    - nimble/host/services/lls
-    - nimble/host/services/tps
-    - nimble/transport
-    - "@apache-mynewt-core/sys/console/stub"
-    - "@apache-mynewt-core/sys/log/stub"
-    - "@apache-mynewt-core/sys/stats/stub"
-
-# No need to build files from this package
-pkg.ign_files:
-    - ".*\\.c"
\ No newline at end of file
diff --git a/libesp32/NimBLE-Arduino/src/porting/nimble/src/os_mbuf.c b/libesp32/NimBLE-Arduino/src/porting/nimble/src/os_mbuf.c
index 4ac6bbb7c..4da8b9625 100644
--- a/libesp32/NimBLE-Arduino/src/porting/nimble/src/os_mbuf.c
+++ b/libesp32/NimBLE-Arduino/src/porting/nimble/src/os_mbuf.c
@@ -385,7 +385,10 @@ os_mbuf_append(struct os_mbuf *om, const void *data,  uint16_t len)
         memcpy(OS_MBUF_DATA(last, uint8_t *) + last->om_len , data, space);
 
         last->om_len += space;
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wpointer-arith"
         data += space;
+#pragma GCC diagnostic pop
         remainder -= space;
     }
 
@@ -400,7 +403,10 @@ os_mbuf_append(struct os_mbuf *om, const void *data,  uint16_t len)
 
         new->om_len = min(omp->omp_databuf_len, remainder);
         memcpy(OS_MBUF_DATA(new, void *), data, new->om_len);
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wpointer-arith"
         data += new->om_len;
+#pragma GCC diagnostic pop
         remainder -= new->om_len;
         SLIST_NEXT(last, om_next) = new;
         last = new;
@@ -651,7 +657,10 @@ os_mbuf_cmpf(const struct os_mbuf *om, int off, const void *data, int len)
 
         chunk_sz = min(om->om_len - om_off, len - data_off);
         if (chunk_sz > 0) {
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wpointer-arith"
             rc = memcmp(om->om_data + om_off, data + data_off, chunk_sz);
+#pragma GCC diagnostic pop
             if (rc != 0) {
                 return rc;
             }
diff --git a/libesp32/NimBLE-Arduino/src/services/ipss/ble_svc_ipss.h b/libesp32/NimBLE-Arduino/src/services/ipss/ble_svc_ipss.h
new file mode 100644
index 000000000..d894732c9
--- /dev/null
+++ b/libesp32/NimBLE-Arduino/src/services/ipss/ble_svc_ipss.h
@@ -0,0 +1,38 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef H_BLE_SVC_IPSS_
+#define H_BLE_SVC_IPSS_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define BLE_SVC_IPSS_UUID16         0x1820
+
+/**
+ * @brief   Initialize the IPSS service
+ */
+void ble_svc_ipss_init(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/libesp32/NimBLE-Arduino/src/src/ble_sm_priv.h b/libesp32/NimBLE-Arduino/src/src/ble_sm_priv.h
index 3f64b7786..bbb4b03ae 100644
--- a/libesp32/NimBLE-Arduino/src/src/ble_sm_priv.h
+++ b/libesp32/NimBLE-Arduino/src/src/ble_sm_priv.h
@@ -277,10 +277,10 @@ struct ble_sm_result {
     uint8_t sm_err;
     struct ble_gap_passkey_params passkey_params;
     void *state_arg;
-    unsigned execute:1;
-    unsigned enc_cb:1;
-    unsigned persist_keys:1;
-    unsigned restore:1;
+    unsigned execute : 1;
+    unsigned enc_cb : 1;
+    unsigned bonded : 1;
+    unsigned restore : 1;
 };
 
 #if MYNEWT_VAL(BLE_HS_DEBUG)
@@ -399,6 +399,7 @@ int ble_sm_slave_initiate(uint16_t conn_handle);
 int ble_sm_enc_initiate(uint16_t conn_handle, uint8_t key_size,
                         const uint8_t *ltk, uint16_t ediv,
                         uint64_t rand_val, int auth);
+int ble_sm_alg_encrypt(uint8_t *key, uint8_t *plaintext, uint8_t *enc_data);
 int ble_sm_init(void);
 
 #define BLE_SM_LOG_CMD(is_tx, cmd_name, conn_handle, log_cb, cmd) \
@@ -419,6 +420,9 @@ int ble_sm_init(void);
 
 #define ble_sm_init() 0
 
+#define ble_sm_alg_encrypt(key, plaintext, enc_data) \
+        BLE_HS_ENOTSUP
+
 #endif
 
 struct ble_l2cap_chan *ble_sm_create_chan(uint16_t handle);