From e089fae11a001b47712549ee3ab29a1381547ddb Mon Sep 17 00:00:00 2001
From: Simon Hailes <simon@yellaumbrella.tv>
Date: Sun, 17 Jan 2021 17:05:10 +0000
Subject: [PATCH] update NimBLE to 5dc72ab10d9f928442a25ef3bdcf8a31a7e16301

---
 lib/libesp32/NimBLE-Arduino/CHANGELOG.md      |  69 ++++
 lib/libesp32/NimBLE-Arduino/README.md         |   4 +-
 .../docs/Command_line_config.md               |  93 +++++
 .../NimBLE_Secure_Client.ino                  |  91 +++++
 .../NimBLE_Secure_Server.ino                  |  37 ++
 .../BLE_notify/BLE_notify.ino                 |   6 +-
 .../BLE_server/BLE_server.ino                 |   7 +-
 .../BLE_server_multiconnect.ino               |   6 +-
 lib/libesp32/NimBLE-Arduino/src/FreeRTOS.cpp  |  10 +-
 lib/libesp32/NimBLE-Arduino/src/FreeRTOS.h    |   6 +-
 .../NimBLE-Arduino/src/NimBLE2904.cpp         |   2 +-
 .../NimBLE-Arduino/src/NimBLEAdvertising.cpp  | 289 ++++++++++++----
 .../NimBLE-Arduino/src/NimBLEAdvertising.h    |  11 +-
 .../src/NimBLECharacteristic.cpp              |   3 +-
 .../NimBLE-Arduino/src/NimBLEClient.cpp       | 318 ++++++++++++------
 .../NimBLE-Arduino/src/NimBLEClient.h         |  11 +-
 .../NimBLE-Arduino/src/NimBLEDevice.cpp       |  94 ++++--
 .../NimBLE-Arduino/src/NimBLEDevice.h         |   2 +
 .../NimBLE-Arduino/src/NimBLEHIDDevice.cpp    | 233 +++++++++++++
 .../NimBLE-Arduino/src/NimBLEHIDDevice.h      |  85 +++++
 .../src/NimBLERemoteCharacteristic.cpp        |  46 ++-
 .../src/NimBLERemoteDescriptor.cpp            |   4 +-
 .../src/NimBLERemoteService.cpp               |  34 +-
 .../NimBLE-Arduino/src/NimBLERemoteService.h  |   1 +
 .../NimBLE-Arduino/src/NimBLEScan.cpp         | 103 +++---
 lib/libesp32/NimBLE-Arduino/src/NimBLEScan.h  |   4 +-
 .../NimBLE-Arduino/src/NimBLEServer.cpp       |   5 +
 .../NimBLE-Arduino/src/NimBLEServer.h         |   9 +
 .../NimBLE-Arduino/src/NimBLEUUID.cpp         |  31 ++
 .../src/esp-hci/src/esp_nimble_hci.c          |  97 ++++--
 .../NimBLE-Arduino/src/esp_nimble_cfg.h       |   4 +
 .../src/nimble/host/src/ble_eddystone.c       |   2 +-
 .../src/nimble/host/src/ble_gap.c             |   6 +-
 .../src/nimble/host/src/ble_hs_conn.c         | 106 +++---
 .../host/store/config/src/ble_store_nvs.c     |  42 ++-
 lib/libesp32/NimBLE-Arduino/src/nimconfig.h   |  44 ++-
 36 files changed, 1537 insertions(+), 378 deletions(-)
 create mode 100644 lib/libesp32/NimBLE-Arduino/docs/Command_line_config.md
 create mode 100644 lib/libesp32/NimBLE-Arduino/examples/NimBLE_Secure_Client/NimBLE_Secure_Client.ino
 create mode 100644 lib/libesp32/NimBLE-Arduino/examples/NimBLE_Secure_Server/NimBLE_Secure_Server.ino
 create mode 100644 lib/libesp32/NimBLE-Arduino/src/NimBLEHIDDevice.cpp
 create mode 100644 lib/libesp32/NimBLE-Arduino/src/NimBLEHIDDevice.h

diff --git a/lib/libesp32/NimBLE-Arduino/CHANGELOG.md b/lib/libesp32/NimBLE-Arduino/CHANGELOG.md
index 8dfc5a141..128d3c93d 100644
--- a/lib/libesp32/NimBLE-Arduino/CHANGELOG.md
+++ b/lib/libesp32/NimBLE-Arduino/CHANGELOG.md
@@ -2,6 +2,75 @@
 
 All notable changes to this project will be documented in this file.  
 
+## [Unreleased]
+
+### Added
+- `NimBLEDevice::setOwnAddrType` added to enable the use of random and random-resolvable addresses, by asukiaaa  
+
+- New examples for securing and authenticating client/server connections, by mblasee.  
+
+- `NimBLEAdvertiseing::SetMinPreferred` and `NimBLEAdvertiseing::SetMinPreferred` re-added.  
+
+- Conditional checks added for command line config options in `nimconfig.h` to support custom configuration in platformio.  
+
+- `NimBLEClient::setValue` Now takes an extra bool parameter `response` to enable the use of write with response (default = false).  
+
+- `NimBLEClient::getCharacteristic(uint16_t handle)` Enabling the use of the characteristic handle to be used to find 
+the NimBLERemoteCharacteristic object.  
+
+- `NimBLEHIDDevice` class added by wakwak-koba.  
+
+- `NimBLEServerCallbacks::onDisconnect` overloaded callback added to provide a ble_gap_conn_desc parameter for the application  
+to obtain information about the disconnected client.  
+
+- Conditional checks in `nimconfig.h` for command line defined macros to support platformio config settings.  
+
+### Changed
+- `NimBLEAdvertising::start` now returns a bool value to indicate success/failure.  
+
+- Some asserts were removed in `NimBLEAdvertising::start` and replaced with better return code handling and logging.    
+
+- If a host reset event occurs, scanning and advertising will now only be restarted if their previous duration was indefinite.  
+
+- `NimBLERemoteCharacteristic::subscribe` and `NimBLERemoteCharacteristic::registerForNotify` will now set the callback
+regardless of the existance of the CCCD and return true unless the descriptor write operation failed.  
+
+- Advertising tx power level is now sent in the advertisement packet instead of scan response.  
+
+- `NimBLEScan` When the scan ends the scan stopped flag is now set before calling the scan complete callback (if used)  
+this allows the starting of a new scan from the callback function.  
+
+### Fixed
+- Sometimes `NimBLEClient::connect` would hang on the task block if no event arrived to unblock.  
+A time limit has been added to timeout appropriately.  
+
+- When getting descriptors for a characterisic the end handle of the service was used as a proxy for the characteristic end  
+handle. This would be rejected by some devices and has been changed to use the next characteristic handle as the end when possible.  
+
+- An exception could occur when deleting a client instance if a notification arrived while the attribute vectors were being  
+deleted. A flag has been added to prevent this.  
+  
+- An exception could occur after a host reset event when the host re-synced if the tasks that were stopped during the event did  
+not finish processing. A yield has been added after re-syncing to allow tasks to finish before proceeding.  
+  
+- Occasionally the controller would fail to send a disconnected event causing the client to indicate it is connected  
+and would be unable to reconnect. A timer has been added to reset the host/controller if it expires.  
+  
+- Occasionally the call to start scanning would get stuck in a loop on BLE_HS_EBUSY, this loop has been removed.  
+
+- 16bit and 32bit UUID's in some cases were not discovered or compared correctly if the device  
+advertised them as 16/32bit but resolved them to 128bits. Both are now checked.  
+  
+- `FreeRTOS` compile errors resolved in latest Ardruino core and IDF v3.3.  
+
+- Multiple instances of `time()` called inside critical sections caused sporadic crashes, these have been moved out of critical regions.  
+
+- Advertisement type now correctly set when using non-connectable (advertiser only) mode.  
+
+- Advertising payload length correction, now accounts for appearance.  
+
+- (Arduino) Ensure controller mode is set to BLE Only.  
+
 ## [1.0.2] - 2020-09-13
 
 ### Changed
diff --git a/lib/libesp32/NimBLE-Arduino/README.md b/lib/libesp32/NimBLE-Arduino/README.md
index 120b0c782..ea28b8811 100644
--- a/lib/libesp32/NimBLE-Arduino/README.md
+++ b/lib/libesp32/NimBLE-Arduino/README.md
@@ -68,9 +68,9 @@ such as increasing max connections, default is 3, absolute maximum connections i
 <br/>
 
 # Development Status
-This Library is tracking the esp-nimble repo, nimble-1.2.0-idf master branch, currently [@95bd864.](https://github.com/espressif/esp-nimble)  
+This Library is tracking the esp-nimble repo, nimble-1.2.0-idf master branch, currently [@f4ae049.](https://github.com/espressif/esp-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)  
+Also tracking the NimBLE related changes in ESP-IDF, master branch, currently [@3caa969.](https://github.com/espressif/esp-idf/tree/master/components/bt/host/nimble) 
 <br/>
 
 # Acknowledgments
diff --git a/lib/libesp32/NimBLE-Arduino/docs/Command_line_config.md b/lib/libesp32/NimBLE-Arduino/docs/Command_line_config.md
new file mode 100644
index 000000000..813156f76
--- /dev/null
+++ b/lib/libesp32/NimBLE-Arduino/docs/Command_line_config.md
@@ -0,0 +1,93 @@
+# Arduino command line and platformio config options  
+
+`CONFIG_BT_NIMBLE_ROLE_CENTRAL_DISABLED`  
+ 
+ If defined, NimBLE Client functions will not be included.  
+- Reduces flash size by approx. 7kB.  
+<br>
+
+`CONFIG_BT_NIMBLE_ROLE_OBSERVER_DISABLED`    
+
+If defined, NimBLE Scan functions will not be included.  
+- Reduces flash size by approx. 26kB.  
+<br>
+
+`CONFIG_BT_NIMBLE_ROLE_PERIPHERAL_DISABLED`   
+
+If defined NimBLE Server functions will not be included.  
+- Reduces flash size by approx. 16kB.  
+<br>
+
+`CONFIG_BT_NIMBLE_ROLE_BROADCASTER_DISABLED`    
+
+If defined, NimBLE Advertising functions will not be included.  
+- Reduces flash size by approx. 5kB.  
+<br>
+
+`CONFIG_BT_NIMBLE_DEBUG`   
+
+If defined, enables debug log messages from the NimBLE host  
+- Uses approx. 32kB of flash memory.  
+<br> 
+
+`CONFIG_NIMBLE_CPP_ENABLE_RETURN_CODE_TEXT`  
+
+If defined, NimBLE host return codes will be printed as text in debug log messages.  
+- Uses approx. 7kB of flash memory.  
+<br>  
+
+`CONFIG_NIMBLE_CPP_ENABLE_GAP_EVENT_CODE_TEXT`  
+
+If defined, GAP event codes will be printed as text in debug log messages.  
+- Uses approx. 1kB of flash memory.  
+<br>
+
+`CONFIG_NIMBLE_CPP_ENABLE_ADVERTISMENT_TYPE_TEXT`  
+
+If defined, advertisment types will be printed as text while scanning in debug log messages.  
+- Uses approx. 250 bytes of flash memory.  
+<br>  
+
+`CONFIG_BT_NIMBLE_PINNED_TO_CORE`  
+
+Sets the core the NimBLE host stack will run on   
+- Options: 0 or 1
+<br>
+
+`CONFIG_BT_NIMBLE_TASK_STACK_SIZE`  
+
+Set the task stack size for the NimBLE core.
+- Default is 4096  
+<br>
+
+
+`CONFIG_BT_NIMBLE_MEM_ALLOC_MODE_EXTERNAL`
+
+Sets the NimBLE stack to use external PSRAM will be loaded  
+- Must be defined with a value of 1; Default is CONFIG_BT_NIMBLE_MEM_ALLOC_MODE_INTERNAL 1  
+<br>
+
+`CONFIG_BT_NIMBLE_MAX_CONNECTIONS`
+
+Sets the number of simultaneous connections (esp controller max is 9)  
+- Default value is 3  
+<br>
+ 
+`CONFIG_BT_NIMBLE_MAX_BONDS`
+
+Sets the number of devices allowed to store/bond with  
+- Default value is 3  
+<br>
+
+`CONFIG_BT_NIMBLE_MAX_CCCDS` 
+
+Sets the maximum number of CCCD subscriptions to store  
+- Default value is 8  
+<br>
+ 
+`CONFIG_BT_NIMBLE_SVC_GAP_DEVICE_NAME`  
+
+Set the default device name  
+- Default value is "nimble"  
+<br>
+
diff --git a/lib/libesp32/NimBLE-Arduino/examples/NimBLE_Secure_Client/NimBLE_Secure_Client.ino b/lib/libesp32/NimBLE-Arduino/examples/NimBLE_Secure_Client/NimBLE_Secure_Client.ino
new file mode 100644
index 000000000..6f9af4f74
--- /dev/null
+++ b/lib/libesp32/NimBLE-Arduino/examples/NimBLE_Secure_Client/NimBLE_Secure_Client.ino
@@ -0,0 +1,91 @@
+/** NimBLE_Secure_Client Demo:
+ *  
+ * This example demonstrates the secure passkey protected conenction and communication between an esp32 server and an esp32 client.
+ * Please note that esp32 stores auth info in nvs memory. After a successful connection it is possible that a passkey change will be ineffective.
+ * To avoid this clear the memory of the esp32's between security testings. esptool.py is capable of this, example: esptool.py --port /dev/ttyUSB0 erase_flash.
+ *
+ *  Created: on Jan 08 2021
+ *      Author: mblasee
+ */
+ 
+#include <NimBLEDevice.h>
+
+class ClientCallbacks : public NimBLEClientCallbacks
+{
+  uint32_t onPassKeyRequest()
+  {
+    Serial.println("Client Passkey Request");
+    /** return the passkey to send to the server */
+    /** Change this to be different from NimBLE_Secure_Server if you want to test what happens on key mismatch */
+    return 123456;
+  };
+};
+static ClientCallbacks clientCB;
+
+void setup()
+{
+  Serial.begin(115200);
+  Serial.println("Starting NimBLE Client");
+
+  NimBLEDevice::init("");
+  NimBLEDevice::setPower(ESP_PWR_LVL_P9);
+  NimBLEDevice::setSecurityAuth(true, true, true);
+  NimBLEDevice::setSecurityIOCap(BLE_HS_IO_KEYBOARD_ONLY);
+  NimBLEScan *pScan = NimBLEDevice::getScan();
+  NimBLEScanResults results = pScan->start(5);
+
+  NimBLEUUID serviceUuid("ABCD");
+
+  for (int i = 0; i < results.getCount(); i++)
+  {
+    NimBLEAdvertisedDevice device = results.getDevice(i);
+    Serial.println(device.getName().c_str());
+
+    if (device.isAdvertisingService(serviceUuid))
+    {
+      NimBLEClient *pClient = NimBLEDevice::createClient();
+      pClient->setClientCallbacks(&clientCB, false);
+
+      if (pClient->connect(&device))
+      {
+        pClient->secureConnection();
+        NimBLERemoteService *pService = pClient->getService(serviceUuid);
+        if (pService != nullptr)
+        {
+          NimBLERemoteCharacteristic *pNonSecureCharacteristic = pService->getCharacteristic("1234");
+          
+          if (pNonSecureCharacteristic != nullptr)
+          {
+            // Testing to read a non secured characteristic, you should be able to read this even if you have mismatching passkeys.
+            std::string value = pNonSecureCharacteristic->readValue();
+            // print or do whatever you need with the value
+            Serial.println(value.c_str());
+          }
+
+          NimBLERemoteCharacteristic *pSecureCharacteristic = pService->getCharacteristic("1235");
+
+          if (pSecureCharacteristic != nullptr)
+          {
+            // Testing to read a secured characteristic, you should be able to read this only if you have matching passkeys, otherwise you should
+            // get an error like this. E NimBLERemoteCharacteristic: "<< readValue rc=261"
+            // This means you are trying to do something without the proper permissions. 
+            std::string value = pSecureCharacteristic->readValue();
+            // print or do whatever you need with the value
+            Serial.println(value.c_str());
+          }      
+        }
+      }
+      else
+      {
+        // failed to connect
+        Serial.println("failed to connect");
+      }
+
+      NimBLEDevice::deleteClient(pClient);
+    }
+  }
+}
+
+void loop()
+{
+}
diff --git a/lib/libesp32/NimBLE-Arduino/examples/NimBLE_Secure_Server/NimBLE_Secure_Server.ino b/lib/libesp32/NimBLE-Arduino/examples/NimBLE_Secure_Server/NimBLE_Secure_Server.ino
new file mode 100644
index 000000000..f63cbc162
--- /dev/null
+++ b/lib/libesp32/NimBLE-Arduino/examples/NimBLE_Secure_Server/NimBLE_Secure_Server.ino
@@ -0,0 +1,37 @@
+/** NimBLE_Secure_Server Demo:
+ *  
+ * This example demonstrates the secure passkey protected conenction and communication between an esp32 server and an esp32 client.
+ * Please note that esp32 stores auth info in nvs memory. After a successful connection it is possible that a passkey change will be ineffective.
+ * To avoid this clear the memory of the esp32's between security testings. esptool.py is capable of this, example: esptool.py --port /dev/ttyUSB0 erase_flash.
+ *
+ *  Created: on Jan 08 2021
+ *      Author: mblasee
+ */
+
+#include <NimBLEDevice.h>
+
+void setup() {
+  Serial.begin(115200);
+  Serial.println("Starting NimBLE Server");
+  NimBLEDevice::init("NimBLE");
+  NimBLEDevice::setPower(ESP_PWR_LVL_P9);
+
+  NimBLEDevice::setSecurityAuth(true, true, true);
+  NimBLEDevice::setSecurityPasskey(123456);
+  NimBLEDevice::setSecurityIOCap(BLE_HS_IO_DISPLAY_ONLY);
+  NimBLEServer *pServer = NimBLEDevice::createServer();
+  NimBLEService *pService = pServer->createService("ABCD");
+  NimBLECharacteristic *pNonSecureCharacteristic = pService->createCharacteristic("1234", NIMBLE_PROPERTY::READ );
+  NimBLECharacteristic *pSecureCharacteristic = pService->createCharacteristic("1235", NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::READ_ENC | NIMBLE_PROPERTY::READ_AUTHEN);
+
+  pService->start();
+  pNonSecureCharacteristic->setValue("Hello Non Secure BLE");
+  pSecureCharacteristic->setValue("Hello Secure BLE");
+
+  NimBLEAdvertising *pAdvertising = NimBLEDevice::getAdvertising();
+  pAdvertising->addServiceUUID("ABCD");
+  pAdvertising->start();
+}
+
+void loop() {
+}
diff --git a/lib/libesp32/NimBLE-Arduino/examples/Refactored_original_examples/BLE_notify/BLE_notify.ino b/lib/libesp32/NimBLE-Arduino/examples/Refactored_original_examples/BLE_notify/BLE_notify.ino
index 83f129b83..cb0488819 100644
--- a/lib/libesp32/NimBLE-Arduino/examples/Refactored_original_examples/BLE_notify/BLE_notify.ino
+++ b/lib/libesp32/NimBLE-Arduino/examples/Refactored_original_examples/BLE_notify/BLE_notify.ino
@@ -116,9 +116,9 @@ void setup() {
   BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
   pAdvertising->addServiceUUID(SERVICE_UUID);
   pAdvertising->setScanResponse(false);
-  /**This method is removed as it was no longer useful and consumed advertising space
-   * pAdvertising->setMinPreferred(0x0);  // set value to 0x00 to not advertise this parameter
-   */
+  /** Note, this could be left out as that is the default value */
+  pAdvertising->setMinPreferred(0x0);  // set value to 0x00 to not advertise this parameter
+
   BLEDevice::startAdvertising();
   Serial.println("Waiting a client connection to notify...");
 }
diff --git a/lib/libesp32/NimBLE-Arduino/examples/Refactored_original_examples/BLE_server/BLE_server.ino b/lib/libesp32/NimBLE-Arduino/examples/Refactored_original_examples/BLE_server/BLE_server.ino
index 652d77685..faa4d88ea 100644
--- a/lib/libesp32/NimBLE-Arduino/examples/Refactored_original_examples/BLE_server/BLE_server.ino
+++ b/lib/libesp32/NimBLE-Arduino/examples/Refactored_original_examples/BLE_server/BLE_server.ino
@@ -44,10 +44,9 @@ void setup() {
   BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
   pAdvertising->addServiceUUID(SERVICE_UUID);
   pAdvertising->setScanResponse(true);
-  /**These methods are removed as they are no longer useful and consumed advertising space
-  * pAdvertising->setMinPreferred(0x06);  // functions that help with iPhone connections issue
-  * pAdvertising->setMinPreferred(0x12);
-  */
+  pAdvertising->setMinPreferred(0x06);  // functions that help with iPhone connections issue
+  pAdvertising->setMaxPreferred(0x12);
+
   BLEDevice::startAdvertising();
   Serial.println("Characteristic defined! Now you can read it in your phone!");
 }
diff --git a/lib/libesp32/NimBLE-Arduino/examples/Refactored_original_examples/BLE_server_multiconnect/BLE_server_multiconnect.ino b/lib/libesp32/NimBLE-Arduino/examples/Refactored_original_examples/BLE_server_multiconnect/BLE_server_multiconnect.ino
index 2ec38c481..9ae3859c7 100644
--- a/lib/libesp32/NimBLE-Arduino/examples/Refactored_original_examples/BLE_server_multiconnect/BLE_server_multiconnect.ino
+++ b/lib/libesp32/NimBLE-Arduino/examples/Refactored_original_examples/BLE_server_multiconnect/BLE_server_multiconnect.ino
@@ -120,9 +120,9 @@ void setup() {
   BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
   pAdvertising->addServiceUUID(SERVICE_UUID);
   pAdvertising->setScanResponse(false);
-  /**This method is removed it was no longer useful and consumed advertising space
-   * pAdvertising->setMinPreferred(0x0);  // set value to 0x00 to not advertise this parameter
-   */
+  /** Note, this could be left out as that is the default value */
+  pAdvertising->setMinPreferred(0x0);  // set value to 0x00 to not advertise this parameter
+
   BLEDevice::startAdvertising();
   Serial.println("Waiting a client connection to notify...");
 }
diff --git a/lib/libesp32/NimBLE-Arduino/src/FreeRTOS.cpp b/lib/libesp32/NimBLE-Arduino/src/FreeRTOS.cpp
index 4ebab3958..1c398cbf3 100644
--- a/lib/libesp32/NimBLE-Arduino/src/FreeRTOS.cpp
+++ b/lib/libesp32/NimBLE-Arduino/src/FreeRTOS.cpp
@@ -264,10 +264,14 @@ void FreeRTOS::Semaphore::setName(std::string name) {
  * @param [in] length The amount of storage to allocate for the ring buffer.
  * @param [in] type The type of buffer.  One of RINGBUF_TYPE_NOSPLIT, RINGBUF_TYPE_ALLOWSPLIT, RINGBUF_TYPE_BYTEBUF.
  */
-#if defined(ESP_IDF_VERSION) && !defined(ESP_IDF_VERSION_VAL) //Quick hack to detect if using IDF version that replaced ringbuf_type_t, ESP_IDF_VERSION_VAL is for IDF>4.0.0
-Ringbuffer::Ringbuffer(size_t length, RingbufferType_t type) {
+#ifdef ESP_IDF_VERSION //Quick hack to detect if using IDF version that replaced ringbuf_type_t
+#if ESP_IDF_VERSION >= ESP_IDF_VERSION_VAL(4, 0, 0)
+    Ringbuffer::Ringbuffer(size_t length, RingbufferType_t type) {
 #else
-Ringbuffer::Ringbuffer(size_t length, ringbuf_type_t type) {
+    Ringbuffer::Ringbuffer(size_t length, ringbuf_type_t type) {
+#endif
+#else
+    Ringbuffer::Ringbuffer(size_t length, ringbuf_type_t type) {
 #endif
     m_handle = ::xRingbufferCreate(length, type);
 } // Ringbuffer
diff --git a/lib/libesp32/NimBLE-Arduino/src/FreeRTOS.h b/lib/libesp32/NimBLE-Arduino/src/FreeRTOS.h
index f93f0b1a0..fa33921fe 100644
--- a/lib/libesp32/NimBLE-Arduino/src/FreeRTOS.h
+++ b/lib/libesp32/NimBLE-Arduino/src/FreeRTOS.h
@@ -68,8 +68,12 @@ public:
  */
 class Ringbuffer {
 public:
-#if defined(ESP_IDF_VERSION) && !defined(ESP_IDF_VERSION_VAL) //Quick hack to detect if using IDF version that replaced ringbuf_type_t, ESP_IDF_VERSION_VAL is for IDF>4.0.0
+#ifdef ESP_IDF_VERSION //Quick hack to detect if using IDF version that replaced ringbuf_type_t
+#if ESP_IDF_VERSION >= ESP_IDF_VERSION_VAL(4, 0, 0)
     Ringbuffer(size_t length, RingbufferType_t type = RINGBUF_TYPE_NOSPLIT);
+#else
+    Ringbuffer(size_t length, ringbuf_type_t type = RINGBUF_TYPE_NOSPLIT);
+#endif
 #else
     Ringbuffer(size_t length, ringbuf_type_t type = RINGBUF_TYPE_NOSPLIT);
 #endif
diff --git a/lib/libesp32/NimBLE-Arduino/src/NimBLE2904.cpp b/lib/libesp32/NimBLE-Arduino/src/NimBLE2904.cpp
index d85cd87e4..80318b5b8 100644
--- a/lib/libesp32/NimBLE-Arduino/src/NimBLE2904.cpp
+++ b/lib/libesp32/NimBLE-Arduino/src/NimBLE2904.cpp
@@ -37,7 +37,7 @@ NimBLE2904::NimBLE2904(NimBLECharacteristic* pCharacterisitic)
     m_data.m_unit        = 0;
     m_data.m_description = 0;
     setValue((uint8_t*) &m_data, sizeof(m_data));
-} // BLE2902
+} // BLE2904
 
 
 /**
diff --git a/lib/libesp32/NimBLE-Arduino/src/NimBLEAdvertising.cpp b/lib/libesp32/NimBLE-Arduino/src/NimBLEAdvertising.cpp
index 36bdbf9e9..a10480410 100644
--- a/lib/libesp32/NimBLE-Arduino/src/NimBLEAdvertising.cpp
+++ b/lib/libesp32/NimBLE-Arduino/src/NimBLEAdvertising.cpp
@@ -32,7 +32,7 @@ static const char* LOG_TAG = "NimBLEAdvertising";
 /**
  * @brief Construct a default advertising object.
  */
-NimBLEAdvertising::NimBLEAdvertising() {
+NimBLEAdvertising::NimBLEAdvertising() : m_slaveItvl() {
     memset(&m_advData, 0, sizeof m_advData);
     memset(&m_scanData, 0, sizeof m_scanData);
     memset(&m_advParams, 0, sizeof m_advParams);
@@ -41,15 +41,20 @@ NimBLEAdvertising::NimBLEAdvertising() {
     m_advData.name                   = (uint8_t *)name;
     m_advData.name_len               = strlen(name);
     m_advData.name_is_complete       = 1;
-    m_scanData.tx_pwr_lvl_is_present = 1;
-    m_scanData.tx_pwr_lvl            = NimBLEDevice::getPower();
+    m_advData.tx_pwr_lvl_is_present  = 1;
+    m_advData.tx_pwr_lvl             = NimBLEDevice::getPower();
     m_advData.flags                  = (BLE_HS_ADV_F_DISC_GEN | BLE_HS_ADV_F_BREDR_UNSUP);
     m_advData.appearance             = 0;
     m_advData.appearance_is_present  = 0;
     m_advData.mfg_data_len           = 0;
     m_advData.mfg_data               = nullptr;
+    m_advData.slave_itvl_range       = nullptr;
 
+#if !defined(CONFIG_BT_NIMBLE_ROLE_PERIPHERAL)
+    m_advParams.conn_mode            = BLE_GAP_CONN_MODE_NON;
+#else
     m_advParams.conn_mode            = BLE_GAP_CONN_MODE_UND;
+#endif
     m_advParams.disc_mode            = BLE_GAP_DISC_MODE_GEN;
     m_advParams.itvl_min             = 0;
     m_advParams.itvl_max             = 0;
@@ -58,6 +63,8 @@ NimBLEAdvertising::NimBLEAdvertising() {
     m_customScanResponseData         = false;
     m_scanResp                       = true;
     m_advDataSet                     = false;
+    // Set this to non-zero to prevent auto start if host reset before started by app.
+    m_duration                       = BLE_HS_FOREVER;
 
 } // NimBLEAdvertising
 
@@ -86,7 +93,6 @@ void NimBLEAdvertising::addServiceUUID(const char* serviceUUID) {
  * @param [in] serviceUUID The UUID of the service to expose.
  */
 void NimBLEAdvertising::removeServiceUUID(const NimBLEUUID &serviceUUID) {
-    //m_serviceUUIDs.erase(std::remove_if(m_serviceUUIDs.begin(), m_serviceUUIDs.end(),[serviceUUID](const NimBLEUUID &s) {return serviceUUID == s;}), m_serviceUUIDs.end());
     for(auto it = m_serviceUUIDs.begin(); it != m_serviceUUIDs.end(); ++it) {
         if((*it) == serviceUUID) {
             m_serviceUUIDs.erase(it);
@@ -112,11 +118,9 @@ void NimBLEAdvertising::setAppearance(uint16_t appearance) {
 /**
  * @brief Set the type of advertisment to use.
  * @param [in] adv_type:
- * * BLE_HCI_ADV_TYPE_ADV_IND            (0) - indirect advertising
- * * BLE_HCI_ADV_TYPE_ADV_DIRECT_IND_HD  (1) - direct advertisng - high duty cycle
- * * BLE_HCI_ADV_TYPE_ADV_SCAN_IND       (2) - indirect scan response
- * * BLE_HCI_ADV_TYPE_ADV_NONCONN_IND    (3) - indirect advertisng - not connectable
- * * BLE_HCI_ADV_TYPE_ADV_DIRECT_IND_LD  (4) - direct advertising - low duty cycle
+ * * BLE_GAP_CONN_MODE_NON    (0) - not connectable advertising
+ * * BLE_GAP_CONN_MODE_DIR    (1) - directed connectable advertising
+ * * BLE_GAP_CONN_MODE_UND    (2) - undirected connectable advertising
  */
 void NimBLEAdvertising::setAdvertisementType(uint8_t adv_type){
     m_advParams.conn_mode = adv_type;
@@ -141,6 +145,64 @@ void NimBLEAdvertising::setMaxInterval(uint16_t maxinterval) {
 } // setMaxInterval
 
 
+/**
+ * @brief Set the advertised min connection interval preferred by this device.
+ * @param [in] mininterval the max interval value. Range = 0x0006 to 0x0C80.
+ * @details Values not within the range will cancel advertising of this data.\n
+ * Consumes 6 bytes of advertising space (combined with max interval).
+ */
+void NimBLEAdvertising::setMinPreferred(uint16_t mininterval) {
+    // invalid paramters, set the slave interval to null
+    if(mininterval < 0x0006 || mininterval > 0x0C80) {
+        m_advData.slave_itvl_range = nullptr;
+        return;
+    }
+
+    if(m_advData.slave_itvl_range == nullptr) {
+        m_advData.slave_itvl_range = m_slaveItvl;
+    }
+
+    m_slaveItvl[0] = mininterval;
+    m_slaveItvl[1] = mininterval >> 8;
+
+    uint16_t maxinterval = *(uint16_t*)(m_advData.slave_itvl_range+2);
+
+    // If mininterval is higher than the maxinterval make them the same
+    if(mininterval > maxinterval) {
+        m_slaveItvl[2] = m_slaveItvl[0];
+        m_slaveItvl[3] = m_slaveItvl[1];
+    }
+} // setMinPreferred
+
+
+/**
+ * @brief Set the advertised max connection interval preferred by this device.
+ * @param [in] maxinterval the max interval value. Range = 0x0006 to 0x0C80.
+ * @details Values not within the range will cancel advertising of this data.\n
+ * Consumes 6 bytes of advertising space (combined with min interval).
+ */
+void NimBLEAdvertising::setMaxPreferred(uint16_t maxinterval) {
+    // invalid paramters, set the slave interval to null
+    if(maxinterval < 0x0006 || maxinterval > 0x0C80) {
+        m_advData.slave_itvl_range = nullptr;
+        return;
+    }
+    if(m_advData.slave_itvl_range == nullptr) {
+        m_advData.slave_itvl_range = m_slaveItvl;
+    }
+    m_slaveItvl[2] = maxinterval;
+    m_slaveItvl[3] = maxinterval >> 8;
+
+    uint16_t mininterval = *(uint16_t*)(m_advData.slave_itvl_range);
+
+    // If mininterval is higher than the maxinterval make them the same
+    if(mininterval > maxinterval) {
+        m_slaveItvl[0] = m_slaveItvl[2];
+        m_slaveItvl[1] = m_slaveItvl[3];
+    }
+} // setMaxPreferred
+
+
 /**
  * @brief Set if scan response is available.
  * @param [in] set true = scan response available.
@@ -156,7 +218,8 @@ void NimBLEAdvertising::setScanResponse(bool set) {
  * @param [in] connectWhitelistOnly If true, only allow connections from those on the white list.
  */
 void NimBLEAdvertising::setScanFilter(bool scanRequestWhitelistOnly, bool connectWhitelistOnly) {
-    NIMBLE_LOGD(LOG_TAG, ">> setScanFilter: scanRequestWhitelistOnly: %d, connectWhitelistOnly: %d", scanRequestWhitelistOnly, connectWhitelistOnly);
+    NIMBLE_LOGD(LOG_TAG, ">> setScanFilter: scanRequestWhitelistOnly: %d, connectWhitelistOnly: %d",
+                scanRequestWhitelistOnly, connectWhitelistOnly);
     if (!scanRequestWhitelistOnly && !connectWhitelistOnly) {
         m_advParams.filter_policy = BLE_HCI_ADV_FILT_NONE;
         NIMBLE_LOGD(LOG_TAG, "<< setScanFilter");
@@ -194,7 +257,8 @@ void NimBLEAdvertising::setAdvertisementData(NimBLEAdvertisementData& advertisem
         (uint8_t*)advertisementData.getPayload().data(),
         advertisementData.getPayload().length());
     if (rc != 0) {
-        NIMBLE_LOGE(LOG_TAG, "ble_gap_adv_set_data: %d %s", rc, NimBLEUtils::returnCodeToString(rc));
+        NIMBLE_LOGE(LOG_TAG, "ble_gap_adv_set_data: %d %s",
+                    rc, NimBLEUtils::returnCodeToString(rc));
     }
     m_customAdvData = true;   // Set the flag that indicates we are using custom advertising data.
     NIMBLE_LOGD(LOG_TAG, "<< setAdvertisementData");
@@ -213,7 +277,8 @@ void NimBLEAdvertising::setScanResponseData(NimBLEAdvertisementData& advertiseme
         (uint8_t*)advertisementData.getPayload().data(),
         advertisementData.getPayload().length());
     if (rc != 0) {
-        NIMBLE_LOGE(LOG_TAG, "ble_gap_adv_rsp_set_data: %d %s", rc,  NimBLEUtils::returnCodeToString(rc));
+        NIMBLE_LOGE(LOG_TAG, "ble_gap_adv_rsp_set_data: %d %s",
+                    rc,  NimBLEUtils::returnCodeToString(rc));
     }
     m_customScanResponseData = true;   // Set the flag that indicates we are using custom scan response data.
     NIMBLE_LOGD(LOG_TAG, "<< setScanResponseData");
@@ -225,13 +290,14 @@ void NimBLEAdvertising::setScanResponseData(NimBLEAdvertisementData& advertiseme
  * @param [in] duration The duration, in seconds, to advertise, 0 == advertise forever.
  * @param [in] advCompleteCB A pointer to a callback to be invoked when advertising ends.
  */
-void NimBLEAdvertising::start(uint32_t duration, void (*advCompleteCB)(NimBLEAdvertising *pAdv)) {
-    NIMBLE_LOGD(LOG_TAG, ">> Advertising start: customAdvData: %d, customScanResponseData: %d", m_customAdvData, m_customScanResponseData);
+bool NimBLEAdvertising::start(uint32_t duration, void (*advCompleteCB)(NimBLEAdvertising *pAdv)) {
+    NIMBLE_LOGD(LOG_TAG, ">> Advertising start: customAdvData: %d, customScanResponseData: %d",
+                m_customAdvData, m_customScanResponseData);
 
     // If Host is not synced we cannot start advertising.
     if(!NimBLEDevice::m_synced) {
         NIMBLE_LOGE(LOG_TAG, "Host reset, wait for sync.");
-        return;
+        return false;
     }
 
 #if defined(CONFIG_BT_NIMBLE_ROLE_PERIPHERAL)
@@ -240,17 +306,21 @@ void NimBLEAdvertising::start(uint32_t duration, void (*advCompleteCB)(NimBLEAdv
         if(!pServer->m_gattsStarted){
             pServer->start();
         } else if(pServer->getConnectedCount() >= NIMBLE_MAX_CONNECTIONS) {
-            NIMBLE_LOGW(LOG_TAG, "Max connections reached - not advertising");
-            return;
+            NIMBLE_LOGE(LOG_TAG, "Max connections reached - not advertising");
+            return false;
         }
     }
 #endif
 
     // If already advertising just return
     if(ble_gap_adv_active()) {
-        return;
+        NIMBLE_LOGW(LOG_TAG, "Advertising already active");
+        return false;
     }
 
+    // Save the duration incase of host reset so we can restart with the same params
+    m_duration = duration;
+
     if(duration == 0){
         duration = BLE_HS_FOREVER;
     }
@@ -260,16 +330,31 @@ void NimBLEAdvertising::start(uint32_t duration, void (*advCompleteCB)(NimBLEAdv
 
     m_advCompCB = advCompleteCB;
 
+    m_advParams.disc_mode = BLE_GAP_DISC_MODE_GEN;
+    m_advData.flags = (BLE_HS_ADV_F_DISC_GEN | BLE_HS_ADV_F_BREDR_UNSUP);
+    if(m_advParams.conn_mode == BLE_GAP_CONN_MODE_NON) {
+        if(!m_scanResp) {
+            m_advParams.disc_mode = BLE_GAP_DISC_MODE_NON;
+            m_advData.flags = BLE_HS_ADV_F_BREDR_UNSUP;
+        }
+    }
+
     int rc = 0;
 
     if (!m_customAdvData && !m_advDataSet) {
         //start with 3 bytes for the flags data
-        uint8_t payloadLen = 3;
+        uint8_t payloadLen = (2 + 1);
+        if(m_advData.appearance_is_present)
+            payloadLen += (2 + BLE_HS_ADV_APPEARANCE_LEN);
+        if(m_advData.tx_pwr_lvl_is_present)
+            payloadLen += (2 + 1);
+        if(m_advData.slave_itvl_range != nullptr)
+            payloadLen += (2 + 4);
 
         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){
+                if((payloadLen + add) > BLE_HS_ADV_MAX_SZ){
                     m_advData.uuids16_is_complete = 0;
                     continue;
                 }
@@ -278,7 +363,7 @@ void NimBLEAdvertising::start(uint32_t duration, void (*advCompleteCB)(NimBLEAdv
                 if(nullptr == (m_advData.uuids16 = (ble_uuid16_t*)realloc(m_advData.uuids16,
                                                    (m_advData.num_uuids16 + 1) * sizeof(ble_uuid16_t))))
                 {
-                    NIMBLE_LOGE(LOG_TAG, "Error, no mem");
+                    NIMBLE_LOGC(LOG_TAG, "Error, no mem");
                     abort();
                 }
                 memcpy(&m_advData.uuids16[m_advData.num_uuids16].value,
@@ -290,7 +375,7 @@ void NimBLEAdvertising::start(uint32_t duration, void (*advCompleteCB)(NimBLEAdv
             }
             if(it.getNative()->u.type == BLE_UUID_TYPE_32) {
                 int add = (m_advData.num_uuids32 > 0) ? 4 : 6;
-                if((payloadLen + add) > 31){
+                if((payloadLen + add) > BLE_HS_ADV_MAX_SZ){
                     m_advData.uuids32_is_complete = 0;
                     continue;
                 }
@@ -299,7 +384,7 @@ void NimBLEAdvertising::start(uint32_t duration, void (*advCompleteCB)(NimBLEAdv
                 if(nullptr == (m_advData.uuids32 = (ble_uuid32_t*)realloc(m_advData.uuids32,
                                                    (m_advData.num_uuids32 + 1) * sizeof(ble_uuid32_t))))
                 {
-                    NIMBLE_LOGE(LOG_TAG, "Error, no mem");
+                    NIMBLE_LOGC(LOG_TAG, "Error, no mem");
                     abort();
                 }
                 memcpy(&m_advData.uuids32[m_advData.num_uuids32].value,
@@ -311,7 +396,7 @@ void NimBLEAdvertising::start(uint32_t duration, void (*advCompleteCB)(NimBLEAdv
             }
             if(it.getNative()->u.type == BLE_UUID_TYPE_128){
                 int add = (m_advData.num_uuids128 > 0) ? 16 : 18;
-                if((payloadLen + add) > 31){
+                if((payloadLen + add) > BLE_HS_ADV_MAX_SZ){
                     m_advData.uuids128_is_complete = 0;
                     continue;
                 }
@@ -320,7 +405,7 @@ void NimBLEAdvertising::start(uint32_t duration, void (*advCompleteCB)(NimBLEAdv
                 if(nullptr == (m_advData.uuids128 = (ble_uuid128_t*)realloc(m_advData.uuids128,
                               (m_advData.num_uuids128 + 1) * sizeof(ble_uuid128_t))))
                 {
-                    NIMBLE_LOGE(LOG_TAG, "Error, no mem");
+                    NIMBLE_LOGC(LOG_TAG, "Error, no mem");
                     abort();
                 }
                 memcpy(&m_advData.uuids128[m_advData.num_uuids128].value,
@@ -333,54 +418,74 @@ void NimBLEAdvertising::start(uint32_t duration, void (*advCompleteCB)(NimBLEAdv
         }
 
         // check if there is room for the name, if not put it in scan data
-        if((payloadLen + m_advData.name_len) > 29) {
+        if((payloadLen + (2 + m_advData.name_len)) > BLE_HS_ADV_MAX_SZ) {
             if(m_scanResp){
                 m_scanData.name = m_advData.name;
                 m_scanData.name_len = m_advData.name_len;
-                m_scanData.name_is_complete = m_advData.name_is_complete;
+                if(m_scanData.name_len > BLE_HS_ADV_MAX_SZ - 2) {
+                    m_scanData.name_len = BLE_HS_ADV_MAX_SZ - 2;
+                    m_scanData.name_is_complete = 0;
+                } else {
+                    m_scanData.name_is_complete = 1;
+                }
                 m_advData.name = nullptr;
                 m_advData.name_len = 0;
+                m_advData.name_is_complete = 0;
             } else {
+                if(m_advData.tx_pwr_lvl_is_present) {
+                    m_advData.tx_pwr_lvl = 0;
+                    m_advData.tx_pwr_lvl_is_present = 0;
+                    payloadLen -= (2 + 1);
+                }
                 // if not using scan response just cut the name down
                 // leaving 2 bytes for the data specifier.
-                m_advData.name_len = (29 - payloadLen);
+                if(m_advData.name_len > (BLE_HS_ADV_MAX_SZ - payloadLen - 2)) {
+                    m_advData.name_len = (BLE_HS_ADV_MAX_SZ - payloadLen - 2);
+                    m_advData.name_is_complete = 0;
+                }
             }
-            m_advData.name_is_complete = 0;
-        }
-
-        if(m_advData.name_len > 0) {
-            payloadLen += (m_advData.name_len + 2);
         }
 
         if(m_scanResp) {
-            // name length + type byte + length byte + tx power type + length + data
-            if((m_scanData.name_len + 5) > 31) {
-                // prioritize name data over tx power
-                m_scanData.tx_pwr_lvl_is_present = 0;
-                m_scanData.tx_pwr_lvl = 0;
-                // limit name to 29 to leave room for the data specifiers
-                if(m_scanData.name_len > 29) {
-                    m_scanData.name_len = 29;
-                    m_scanData.name_is_complete = false;
-                }
-            }
-
             rc = ble_gap_adv_rsp_set_fields(&m_scanData);
-            if (rc != 0) {
-                NIMBLE_LOGC(LOG_TAG, "error setting scan response data; rc=%d, %s", rc, NimBLEUtils::returnCodeToString(rc));
-                abort();
+            switch(rc) {
+                case 0:
+                    break;
+
+                case BLE_HS_EBUSY:
+                    NIMBLE_LOGE(LOG_TAG, "Already advertising");
+                    break;
+
+                case BLE_HS_EMSGSIZE:
+                    NIMBLE_LOGE(LOG_TAG, "Scan data too long");
+                    break;
+
+                default:
+                    NIMBLE_LOGE(LOG_TAG, "Error setting scan response data; rc=%d, %s",
+                                rc, NimBLEUtils::returnCodeToString(rc));
+                    break;
             }
-        // if not using scan response and there is room,
-        // 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();
         }
 
-        rc = ble_gap_adv_set_fields(&m_advData);
-        if (rc != 0) {
-            NIMBLE_LOGC(LOG_TAG, "error setting advertisement data; rc=%d, %s", rc, NimBLEUtils::returnCodeToString(rc));
-            abort();
+        if(rc == 0) {
+            rc = ble_gap_adv_set_fields(&m_advData);
+            switch(rc) {
+                case 0:
+                    break;
+
+                case BLE_HS_EBUSY:
+                    NIMBLE_LOGE(LOG_TAG, "Already advertising");
+                    break;
+
+                case BLE_HS_EMSGSIZE:
+                    NIMBLE_LOGE(LOG_TAG, "Advertisement data too long");
+                    break;
+
+                default:
+                    NIMBLE_LOGE(LOG_TAG, "Error setting advertisement data; rc=%d, %s",
+                                rc, NimBLEUtils::returnCodeToString(rc));
+                    break;
+            }
         }
 
         if(m_advData.num_uuids128 > 0) {
@@ -401,24 +506,54 @@ void NimBLEAdvertising::start(uint32_t duration, void (*advCompleteCB)(NimBLEAdv
             m_advData.num_uuids16 = 0;
         }
 
+        if(rc !=0) {
+            return false;
+        }
+
         m_advDataSet = true;
     }
 
 #if defined(CONFIG_BT_NIMBLE_ROLE_PERIPHERAL)
-    rc = ble_gap_adv_start(0, NULL, duration,
+    rc = ble_gap_adv_start(NimBLEDevice::m_own_addr_type, NULL, duration,
                            &m_advParams,
-                           (pServer != nullptr) ? NimBLEServer::handleGapEvent : NimBLEAdvertising::handleGapEvent,
+                           (pServer != nullptr) ? NimBLEServer::handleGapEvent :
+                                                  NimBLEAdvertising::handleGapEvent,
                            (pServer != nullptr) ? (void*)pServer : (void*)this);
 #else
-    rc = ble_gap_adv_start(0, NULL, duration,
+    rc = ble_gap_adv_start(NimBLEDevice::m_own_addr_type, NULL, duration,
                            &m_advParams, NimBLEAdvertising::handleGapEvent, this);
 #endif
-    if (rc != 0) {
-        NIMBLE_LOGC(LOG_TAG, "Error enabling advertising; rc=%d, %s", rc, NimBLEUtils::returnCodeToString(rc));
-        abort();
+    switch(rc) {
+        case 0:
+             break;
+
+        case BLE_HS_EINVAL:
+             NIMBLE_LOGE(LOG_TAG, "Unable to advertise - Duration too long");
+             break;
+
+        case BLE_HS_EPREEMPTED:
+             NIMBLE_LOGE(LOG_TAG, "Unable to advertise - busy");
+             break;
+
+        case BLE_HS_ETIMEOUT_HCI:
+        case BLE_HS_EOS:
+        case BLE_HS_ECONTROLLER:
+        case BLE_HS_ENOTSYNCED:
+             NIMBLE_LOGE(LOG_TAG, "Unable to advertise - Host Reset");
+             break;
+
+        default:
+            NIMBLE_LOGE(LOG_TAG, "Error enabling advertising; rc=%d, %s",
+                        rc, NimBLEUtils::returnCodeToString(rc));
+            break;
+    }
+
+    if(rc != 0) {
+        return false;
     }
 
     NIMBLE_LOGD(LOG_TAG, "<< Advertising start");
+    return true;
 } // start
 
 
@@ -427,9 +562,11 @@ void NimBLEAdvertising::start(uint32_t duration, void (*advCompleteCB)(NimBLEAdv
  */
 void NimBLEAdvertising::stop() {
     NIMBLE_LOGD(LOG_TAG, ">> stop");
+
     int rc = ble_gap_adv_stop();
     if (rc != 0 && rc != BLE_HS_EALREADY) {
-        NIMBLE_LOGE(LOG_TAG, "ble_gap_adv_stop rc=%d %s", rc, NimBLEUtils::returnCodeToString(rc));
+        NIMBLE_LOGE(LOG_TAG, "ble_gap_adv_stop rc=%d %s",
+                    rc, NimBLEUtils::returnCodeToString(rc));
         return;
     }
 
@@ -460,8 +597,17 @@ bool NimBLEAdvertising::isAdvertising() {
  * Host reset seems to clear advertising data,
  * we need clear the flag so it reloads it.
  */
-void NimBLEAdvertising::onHostReset() {
+void NimBLEAdvertising::onHostSync() {
+    NIMBLE_LOGD(LOG_TAG, "Host re-synced");
+
     m_advDataSet = false;
+    // If we were advertising forever, restart it now
+    if(m_duration == 0) {
+        start(m_duration, m_advCompCB);
+    } else {
+    // Otherwise we should tell the app that advertising stopped.
+        advCompleteCB();
+    }
 }
 
 
@@ -475,6 +621,19 @@ int NimBLEAdvertising::handleGapEvent(struct ble_gap_event *event, void *arg) {
     NimBLEAdvertising *pAdv = (NimBLEAdvertising*)arg;
 
     if(event->type == BLE_GAP_EVENT_ADV_COMPLETE) {
+        switch(event->adv_complete.reason) {
+            // Don't call the callback if host reset, we want to
+            // preserve the active flag until re-sync to restart advertising.
+            case BLE_HS_ETIMEOUT_HCI:
+            case BLE_HS_EOS:
+            case BLE_HS_ECONTROLLER:
+            case BLE_HS_ENOTSYNCED:
+                NIMBLE_LOGC(LOG_TAG, "host reset, rc=%d", event->adv_complete.reason);
+                NimBLEDevice::onReset(event->adv_complete.reason);
+                return 0;
+            default:
+                break;
+        }
         pAdv->advCompleteCB();
     }
     return 0;
diff --git a/lib/libesp32/NimBLE-Arduino/src/NimBLEAdvertising.h b/lib/libesp32/NimBLE-Arduino/src/NimBLEAdvertising.h
index 2fab71004..0d97ecbbc 100644
--- a/lib/libesp32/NimBLE-Arduino/src/NimBLEAdvertising.h
+++ b/lib/libesp32/NimBLE-Arduino/src/NimBLEAdvertising.h
@@ -77,7 +77,7 @@ public:
     void addServiceUUID(const NimBLEUUID &serviceUUID);
     void addServiceUUID(const char* serviceUUID);
     void removeServiceUUID(const NimBLEUUID &serviceUUID);
-    void start(uint32_t duration = 0, void (*advCompleteCB)(NimBLEAdvertising *pAdv) = nullptr);
+    bool start(uint32_t duration = 0, void (*advCompleteCB)(NimBLEAdvertising *pAdv) = nullptr);
     void stop();
     void setAppearance(uint16_t appearance);
     void setAdvertisementType(uint8_t adv_type);
@@ -87,13 +87,15 @@ public:
     void setScanFilter(bool scanRequestWhitelistOnly, bool connectWhitelistOnly);
     void setScanResponseData(NimBLEAdvertisementData& advertisementData);
     void setScanResponse(bool);
+    void setMinPreferred(uint16_t);
+    void setMaxPreferred(uint16_t);
     void advCompleteCB();
     bool isAdvertising();
 
 private:
     friend class NimBLEDevice;
 
-    void                    onHostReset();
+    void                    onHostSync();
     static int              handleGapEvent(struct ble_gap_event *event, void *arg);
 
     ble_hs_adv_fields       m_advData;
@@ -104,8 +106,9 @@ private:
     bool                    m_customScanResponseData;
     bool                    m_scanResp;
     bool                    m_advDataSet;
-    void                   (*m_advCompCB)(NimBLEAdvertising *pAdv);
-
+    void                    (*m_advCompCB)(NimBLEAdvertising *pAdv);
+    uint8_t                 m_slaveItvl[4];
+    uint32_t                m_duration;
 };
 
 #endif // #if defined(CONFIG_BT_NIMBLE_ROLE_PERIPHERAL)
diff --git a/lib/libesp32/NimBLE-Arduino/src/NimBLECharacteristic.cpp b/lib/libesp32/NimBLE-Arduino/src/NimBLECharacteristic.cpp
index 6f7d3d7e0..e9a5a49c9 100644
--- a/lib/libesp32/NimBLE-Arduino/src/NimBLECharacteristic.cpp
+++ b/lib/libesp32/NimBLE-Arduino/src/NimBLECharacteristic.cpp
@@ -473,9 +473,10 @@ void NimBLECharacteristic::setValue(const uint8_t* data, size_t length) {
         return;
     }
 
+    time_t t = time(nullptr);
     portENTER_CRITICAL(&m_valMux);
     m_value = std::string((char*)data, length);
-    m_timestamp = time(nullptr);
+    m_timestamp = t;
     portEXIT_CRITICAL(&m_valMux);
 
     NIMBLE_LOGD(LOG_TAG, "<< setValue");
diff --git a/lib/libesp32/NimBLE-Arduino/src/NimBLEClient.cpp b/lib/libesp32/NimBLE-Arduino/src/NimBLEClient.cpp
index 539a7a016..ddd3abecc 100644
--- a/lib/libesp32/NimBLE-Arduino/src/NimBLEClient.cpp
+++ b/lib/libesp32/NimBLE-Arduino/src/NimBLEClient.cpp
@@ -24,6 +24,9 @@
 #include <string>
 #include <unordered_set>
 
+#include "nimble/nimble_port.h"
+
+
 static const char* LOG_TAG = "NimBLEClient";
 static NimBLEClientCallbacks defaultCallbacks;
 
@@ -56,11 +59,10 @@ static NimBLEClientCallbacks defaultCallbacks;
 NimBLEClient::NimBLEClient(const NimBLEAddress &peerAddress) : m_peerAddress(peerAddress) {
     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_connEstablished  = false;
 
     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)
@@ -70,6 +72,9 @@ NimBLEClient::NimBLEClient(const NimBLEAddress &peerAddress) : m_peerAddress(pee
     m_pConnParams.supervision_timeout = BLE_GAP_INITIAL_SUPERVISION_TIMEOUT; // timeout = 400*10ms = 4000ms
     m_pConnParams.min_ce_len = BLE_GAP_INITIAL_CONN_MIN_CE_LEN; // Minimum length of connection event in 0.625ms units
     m_pConnParams.max_ce_len = BLE_GAP_INITIAL_CONN_MAX_CE_LEN; // Maximum length of connection event in 0.625ms units
+
+    ble_npl_callout_init(&m_dcTimer, nimble_port_get_dflt_eventq(),
+                         NimBLEClient::dcTimerCb, this);
 } // NimBLEClient
 
 
@@ -89,6 +94,19 @@ NimBLEClient::~NimBLEClient() {
 } // ~NimBLEClient
 
 
+/**
+ * @brief If we have asked to disconnect and the event does not
+ * occur within the supervision timeout + added delay, this will
+ * be called to reset the host in the case of a stalled controller.
+ */
+void NimBLEClient::dcTimerCb(ble_npl_event *event) {
+    NimBLEClient *pClient = (NimBLEClient*)event->arg;
+    NIMBLE_LOGC(LOG_TAG, "Timed out disconnecting from %s - resetting host",
+                std::string(pClient->getPeerAddress()).c_str());
+    ble_hs_sched_reset(BLE_HS_ECONTROLLER);
+}
+
+
 /**
  * @brief Delete all service objects created by this client and clear the vector.
  */
@@ -164,12 +182,9 @@ bool NimBLEClient::connect(const NimBLEAddress &address, bool deleteAttibutes) {
         return false;
     }
 
-    if(ble_gap_conn_active()) {
-        NIMBLE_LOGE(LOG_TAG, "Connection in progress - must wait.");
-        return false;
-    }
-
-    if(!NimBLEDevice::getScan()->stop()) {
+    if(isConnected() || m_pTaskData != nullptr) {
+        NIMBLE_LOGE(LOG_TAG, "Client busy, connected to %s, id=%d",
+                    std::string(m_peerAddress).c_str(), getConnId());
         return false;
     }
 
@@ -180,54 +195,97 @@ bool NimBLEClient::connect(const NimBLEAddress &address, bool deleteAttibutes) {
         m_peerAddress = address;
     }
 
-    ble_addr_t peerAddrt;
-    memcpy(&peerAddrt.val, m_peerAddress.getNative(),6);
-    peerAddrt.type = m_peerAddress.getType();
+    ble_addr_t peerAddr_t;
+    memcpy(&peerAddr_t.val, m_peerAddress.getNative(),6);
+    peerAddr_t.type = m_peerAddress.getType();
+
 
     ble_task_data_t taskData = {this, xTaskGetCurrentTaskHandle(), 0, nullptr};
-    m_pTaskData = &taskData;
-
     int rc = 0;
 
     /* Try to connect the the advertiser.  Allow 30 seconds (30000 ms) for
      *  timeout (default value of m_connectTimeout).
      *  Loop on BLE_HS_EBUSY if the scan hasn't stopped yet.
      */
-    do{
-        rc = ble_gap_connect(BLE_OWN_ADDR_PUBLIC, &peerAddrt, m_connectTimeout, &m_pConnParams,
-                            NimBLEClient::handleGapEvent, this);
-        if(rc == BLE_HS_EBUSY) {
-            vTaskDelay(1 / portTICK_PERIOD_MS);
+    do {
+        rc = ble_gap_connect(NimBLEDevice::m_own_addr_type, &peerAddr_t,
+                             m_connectTimeout, &m_pConnParams,
+                             NimBLEClient::handleGapEvent, this);
+        switch (rc) {
+            case 0:
+                m_pTaskData = &taskData;
+                break;
+
+            case BLE_HS_EBUSY:
+                // Scan was still running, stop it and try again
+                if (!NimBLEDevice::getScan()->stop()) {
+                    return false;
+                }
+                break;
+
+            case BLE_HS_EDONE:
+                // A connection to this device already exists, do not connect twice.
+                NIMBLE_LOGE(LOG_TAG, "Already connected to device; addr=%s",
+                            std::string(m_peerAddress).c_str());
+                return false;
+
+            case BLE_HS_EALREADY:
+                // Already attemting to connect to this device, cancel the previous
+                // attempt and report failure here so we don't get 2 connections.
+                NIMBLE_LOGE(LOG_TAG, "Already attempting to connect to %s - cancelling",
+                            std::string(m_peerAddress).c_str());
+                ble_gap_conn_cancel();
+                return false;
+
+            default:
+                NIMBLE_LOGE(LOG_TAG, "Failed to connect to %s, rc=%d; %s",
+                            std::string(m_peerAddress).c_str(),
+                            rc, NimBLEUtils::returnCodeToString(rc));
+                return false;
         }
-    }while(rc == BLE_HS_EBUSY);
 
-    if (rc != 0 && rc != BLE_HS_EDONE) {
-        NIMBLE_LOGE(LOG_TAG, "Error: Failed to connect to device; "
-                    "addr=%s, rc=%d; %s",
-                    std::string(m_peerAddress).c_str(),
-                    rc, NimBLEUtils::returnCodeToString(rc));
+    } while (rc == BLE_HS_EBUSY);
+
+    // Wait for the connect timeout time +1 second for the connection to complete
+    if(ulTaskNotifyTake(pdTRUE, pdMS_TO_TICKS(m_connectTimeout + 1000)) == pdFALSE) {
         m_pTaskData = nullptr;
-        m_waitingToConnect = false;
+        // If a connection was made but no response from MTU exchange; disconnect
+        if(isConnected()) {
+            NIMBLE_LOGE(LOG_TAG, "Connect timeout - no response");
+            disconnect();
+        } else {
+        // workaround; if the controller doesn't cancel the connection
+        // at the timeout cancel it here.
+            NIMBLE_LOGE(LOG_TAG, "Connect timeout - cancelling");
+            ble_gap_conn_cancel();
+        }
+
         return false;
-    }
 
-    m_waitingToConnect = true;
-
-    // Wait for the connection to complete.
-    ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
-
-    if(taskData.rc != 0){
+    } else if(taskData.rc != 0){
+        NIMBLE_LOGE(LOG_TAG, "Connection failed; status=%d %s",
+                    taskData.rc,
+                    NimBLEUtils::returnCodeToString(taskData.rc));
+        // If the failure was not a result of a disconnection
+        // make sure we disconnect now to avoid dangling connections
+        if(isConnected()) {
+            ble_gap_terminate(m_conn_id, BLE_ERR_REM_USER_CONN_TERM);
+        }
         return false;
+    } else {
+        NIMBLE_LOGI(LOG_TAG, "Connection established");
     }
 
     if(deleteAttibutes) {
         deleteServices();
     }
 
+    m_connEstablished = true;
     m_pClientCallbacks->onConnect(this);
 
     NIMBLE_LOGD(LOG_TAG, "<< connect()");
-    return true;
+    // Check if still connected before returning
+    return isConnected();
 } // connect
 
 
@@ -267,13 +325,31 @@ bool NimBLEClient::secureConnection() {
  */
 int NimBLEClient::disconnect(uint8_t reason) {
     NIMBLE_LOGD(LOG_TAG, ">> disconnect()");
+
     int rc = 0;
-    if(m_isConnected){
+    if(isConnected()){
         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));
+        if (rc == 0) {
+            ble_addr_t peerAddr_t;
+            memcpy(&peerAddr_t.val, m_peerAddress.getNative(),6);
+            peerAddr_t.type = m_peerAddress.getType();
+
+            // Set the disconnect timeout to the supervison timeout time + 1 second
+            // In case the event triggers shortly after the supervision timeout.
+            // We don't want to prematurely reset the host.
+            ble_gap_conn_desc desc;
+            if(ble_gap_conn_find_by_addr(&peerAddr_t, &desc) == 0){
+                ble_npl_time_t ticks;
+                ble_npl_time_ms_to_ticks((desc.supervision_timeout + 100) * 10, &ticks);
+                ble_npl_callout_reset(&m_dcTimer, ticks);
+                NIMBLE_LOGD(LOG_TAG, "DC TIMEOUT = %dms", (desc.supervision_timeout + 100) * 10);
+            }
+        } else if (rc != BLE_HS_EALREADY) {
+            NIMBLE_LOGE(LOG_TAG, "ble_gap_terminate failed: rc=%d %s",
+                        rc, NimBLEUtils::returnCodeToString(rc));
         }
+    } else {
+        NIMBLE_LOGD(LOG_TAG, "Not connected to any peers");
     }
 
     NIMBLE_LOGD(LOG_TAG, "<< disconnect()");
@@ -454,6 +530,16 @@ NimBLERemoteService* NimBLEClient::getService(const NimBLEUUID &uuid) {
         if(m_servicesVector.size() > prev_size) {
             return m_servicesVector.back();
         }
+
+        // If the request was successful but 16/32 bit service not found
+        // try again with the 128 bit uuid.
+        if(uuid.bitSize() == BLE_UUID_TYPE_16 ||
+           uuid.bitSize() == BLE_UUID_TYPE_32)
+        {
+            NimBLEUUID uuid128(uuid);
+            uuid128.to128();
+            return getService(uuid128);
+        }
     }
 
     NIMBLE_LOGD(LOG_TAG, "<< getService: not found");
@@ -510,7 +596,7 @@ bool NimBLEClient::retrieveServices(const NimBLEUUID *uuid_filter) {
 
     NIMBLE_LOGD(LOG_TAG, ">> retrieveServices");
 
-    if(!m_isConnected){
+    if(!isConnected()){
         NIMBLE_LOGE(LOG_TAG, "Disconnected, could not retrieve services -aborting");
         return false;
     }
@@ -618,10 +704,11 @@ std::string NimBLEClient::getValue(const NimBLEUUID &serviceUUID, const NimBLEUU
  * @param [in] serviceUUID The service that owns the characteristic.
  * @param [in] characteristicUUID The characteristic whose value we wish to write.
  * @param [in] value The value to write to the characteristic.
+ * @param [in] response If true, uses write with response operation.
  * @returns true if successful otherwise false
  */
 bool NimBLEClient::setValue(const NimBLEUUID &serviceUUID, const NimBLEUUID &characteristicUUID,
-                            const std::string &value)
+                            const std::string &value, bool response)
 {
     NIMBLE_LOGD(LOG_TAG, ">> setValue: serviceUUID: %s, characteristicUUID: %s",
                          serviceUUID.toString().c_str(), characteristicUUID.toString().c_str());
@@ -632,7 +719,7 @@ bool NimBLEClient::setValue(const NimBLEUUID &serviceUUID, const NimBLEUUID &cha
     if(pService != nullptr) {
         NimBLERemoteCharacteristic* pChar = pService->getCharacteristic(characteristicUUID);
         if(pChar != nullptr) {
-            ret = pChar->writeValue(value);
+            ret = pChar->writeValue(value, response);
         }
     }
 
@@ -641,6 +728,31 @@ bool NimBLEClient::setValue(const NimBLEUUID &serviceUUID, const NimBLEUUID &cha
 } // setValue
 
 
+/**
+ * @brief Get the remote characteristic with the specified handle.
+ * @param [in] handle The handle of the desired characteristic.
+ * @returns The matching remote characteristic, nullptr otherwise.
+ */
+NimBLERemoteCharacteristic* NimBLEClient::getCharacteristic(const uint16_t handle)
+{
+    NimBLERemoteService *pService = nullptr;
+    for(auto it = m_servicesVector.begin(); it != m_servicesVector.end(); ++it) {
+      if ((*it)->getStartHandle() <= handle && handle <= (*it)->getEndHandle()) {
+          pService = *it;
+          break;
+      }
+    }
+
+    if (pService != nullptr) {
+        for (auto it = pService->begin(); it != pService->end(); ++it) {
+            if ((*it)->getHandle() == handle) {
+                return *it;
+            }
+        }
+    }
+
+    return nullptr;
+}
 
 /**
  * @brief Get the current mtu of this connection.
@@ -656,7 +768,8 @@ uint16_t NimBLEClient::getMTU() {
  * @param [in] event The event structure sent by the NimBLE stack.
  * @param [in] arg A pointer to the client instance that registered for this callback.
  */
- /*STATIC*/ int NimBLEClient::handleGapEvent(struct ble_gap_event *event, void *arg) {
+ /*STATIC*/
+ int NimBLEClient::handleGapEvent(struct ble_gap_event *event, void *arg) {
     NimBLEClient* client = (NimBLEClient*)arg;
     int rc;
 
@@ -665,61 +778,66 @@ uint16_t NimBLEClient::getMTU() {
     switch(event->type) {
 
         case BLE_GAP_EVENT_DISCONNECT: {
-            if(!client->m_isConnected)
-                return 0;
-
-            if(client->m_conn_id != event->disconnect.conn.conn_handle)
-                return 0;
-
-            client->m_isConnected = false;
-            client->m_waitingToConnect=false;
-            // Remove the device from ignore list so we will scan it again
-            NimBLEDevice::removeIgnored(client->m_peerAddress);
-
-            NIMBLE_LOGI(LOG_TAG, "disconnect; reason=%d, %s", event->disconnect.reason,
-                                    NimBLEUtils::returnCodeToString(event->disconnect.reason));
-
+            rc = event->disconnect.reason;
             // If Host reset tell the device now before returning to prevent
             // any errors caused by calling host functions before resyncing.
-            switch(event->disconnect.reason) {
-                case BLE_HS_ETIMEOUT_HCI:
-                case BLE_HS_EOS:
+            switch(rc) {
                 case BLE_HS_ECONTROLLER:
+                case BLE_HS_ETIMEOUT_HCI:
                 case BLE_HS_ENOTSYNCED:
-                    NIMBLE_LOGC(LOG_TAG, "Disconnect - host reset, rc=%d", event->disconnect.reason);
-                    NimBLEDevice::onReset(event->disconnect.reason);
+                case BLE_HS_EOS:
+                    NIMBLE_LOGC(LOG_TAG, "Disconnect - host reset, rc=%d", rc);
+                    NimBLEDevice::onReset(rc);
                     break;
                 default:
+                    // Check that the event is for this client.
+                    if(client->m_conn_id != event->disconnect.conn.conn_handle) {
+                        return 0;
+                    }
                     break;
             }
 
-            //client->m_conn_id = BLE_HS_CONN_HANDLE_NONE;
+            client->m_conn_id = BLE_HS_CONN_HANDLE_NONE;
+
+            // Stop the disconnect timer since we are now disconnected.
+            ble_npl_callout_stop(&client->m_dcTimer);
+
+            // Remove the device from ignore list so we will scan it again
+            NimBLEDevice::removeIgnored(client->m_peerAddress);
+
+            // If we received a connected event but did not get established (no PDU)
+            // then a disconnect event will be sent but we should not send it to the
+            // app for processing. Instead we will ensure the task is released
+            // and report the error.
+            if(!client->m_connEstablished)
+                break;
+
+            NIMBLE_LOGI(LOG_TAG, "disconnect; reason=%d, %s",
+                        rc, NimBLEUtils::returnCodeToString(rc));
+
+            client->m_connEstablished = false;
             client->m_pClientCallbacks->onDisconnect(client);
-            rc = event->disconnect.reason;
             break;
         } // BLE_GAP_EVENT_DISCONNECT
 
         case BLE_GAP_EVENT_CONNECT: {
-
-            if(!client->m_waitingToConnect)
+            // If we aren't waiting for this connection response
+            // we should drop the connection immediately.
+            if(client->isConnected() || client->m_pTaskData == nullptr) {
+                ble_gap_terminate(event->connect.conn_handle, BLE_ERR_REM_USER_CONN_TERM);
                 return 0;
+            }
 
-            //if(client->m_conn_id != BLE_HS_CONN_HANDLE_NONE)
-            //  return 0;
-
-            client->m_waitingToConnect=false;
-
-            if (event->connect.status == 0) {
-                client->m_isConnected = true;
-
-                NIMBLE_LOGD(LOG_TAG, "Connection established");
+            rc = event->connect.status;
+            if (rc == 0) {
+                NIMBLE_LOGI(LOG_TAG, "Connected event");
 
                 client->m_conn_id = event->connect.conn_handle;
 
                 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));
+                    NIMBLE_LOGE(LOG_TAG, "MTU exchange error; rc=%d %s",
+                                rc, NimBLEUtils::returnCodeToString(rc));
                     break;
                 }
 
@@ -727,14 +845,10 @@ uint16_t NimBLEClient::getMTU() {
                 // scanning since we are already connected to it
                 NimBLEDevice::addIgnored(client->m_peerAddress);
             } else {
-                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;
+                client->m_conn_id = BLE_HS_CONN_HANDLE_NONE;
                 break;
             }
+
             return 0;
         } // BLE_GAP_EVENT_CONNECT
 
@@ -742,7 +856,14 @@ uint16_t NimBLEClient::getMTU() {
             if(client->m_conn_id != event->notify_rx.conn_handle)
                 return 0;
 
-            NIMBLE_LOGD(LOG_TAG, "Notify Recieved for handle: %d",event->notify_rx.attr_handle);
+            // If a notification comes before this flag is set we might
+            // access a vector while it is being cleared in connect()
+            if(!client->m_connEstablished) {
+                return 0;
+            }
+
+            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 its range
@@ -752,8 +873,8 @@ uint16_t NimBLEClient::getMTU() {
 
                 auto cVector = &it->m_characteristicVector;
                 NIMBLE_LOGD(LOG_TAG, "checking service %s for handle: %d",
-                                      it->getUUID().toString().c_str(),
-                                      event->notify_rx.attr_handle);
+                            it->getUUID().toString().c_str(),
+                            event->notify_rx.attr_handle);
 
                 auto characteristic = cVector->cbegin();
                 for(; characteristic != cVector->cend(); ++characteristic) {
@@ -762,16 +883,19 @@ uint16_t NimBLEClient::getMTU() {
                 }
 
                 if(characteristic != cVector->cend()) {
-                    NIMBLE_LOGD(LOG_TAG, "Got Notification for characteristic %s", (*characteristic)->toString().c_str());
+                    NIMBLE_LOGD(LOG_TAG, "Got Notification for characteristic %s",
+                                (*characteristic)->toString().c_str());
 
+                    time_t t = time(nullptr);
                     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);
+                    (*characteristic)->m_value = std::string((char *)event->notify_rx.om->om_data,
+                                                             event->notify_rx.om->om_len);
+                    (*characteristic)->m_timestamp = t;
                     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());
+                                    (*characteristic)->toString().c_str());
                         (*characteristic)->m_notifyCallback(*characteristic, event->notify_rx.om->om_data,
                                                             event->notify_rx.om->om_len,
                                                             !event->notify_rx.indication);
@@ -790,10 +914,10 @@ uint16_t NimBLEClient::getMTU() {
             }
             NIMBLE_LOGD(LOG_TAG, "Peer requesting to update connection parameters");
             NIMBLE_LOGD(LOG_TAG, "MinInterval: %d, MaxInterval: %d, Latency: %d, Timeout: %d",
-                                    event->conn_update_req.peer_params->itvl_min,
-                                    event->conn_update_req.peer_params->itvl_max,
-                                    event->conn_update_req.peer_params->latency,
-                                    event->conn_update_req.peer_params->supervision_timeout);
+                        event->conn_update_req.peer_params->itvl_min,
+                        event->conn_update_req.peer_params->itvl_max,
+                        event->conn_update_req.peer_params->latency,
+                        event->conn_update_req.peer_params->supervision_timeout);
 
             rc = client->m_pClientCallbacks->onConnParamsUpdateRequest(client,
                                     event->conn_update_req.peer_params) ? 0 : BLE_ERR_CONN_PARMS;
@@ -827,7 +951,9 @@ uint16_t NimBLEClient::getMTU() {
                 return 0;
             }
 
-            if(event->enc_change.status == 0 || event->enc_change.status == (BLE_HS_ERR_HCI_BASE + BLE_ERR_PINKEY_MISSING)) {
+            if(event->enc_change.status == 0 ||
+               event->enc_change.status == (BLE_HS_ERR_HCI_BASE + BLE_ERR_PINKEY_MISSING))
+            {
                 struct ble_gap_conn_desc desc;
                 rc = ble_gap_conn_find(event->enc_change.conn_handle, &desc);
                 assert(rc == 0);
@@ -922,7 +1048,9 @@ uint16_t NimBLEClient::getMTU() {
 
     if(client->m_pTaskData != nullptr) {
         client->m_pTaskData->rc = rc;
-        xTaskNotifyGive(client->m_pTaskData->task);
+        if(client->m_pTaskData->task) {
+            xTaskNotifyGive(client->m_pTaskData->task);
+        }
         client->m_pTaskData = nullptr;
     }
 
@@ -935,7 +1063,7 @@ uint16_t NimBLEClient::getMTU() {
  * @return True if we are connected and false if we are not connected.
  */
 bool NimBLEClient::isConnected() {
-    return m_isConnected;
+    return m_conn_id != BLE_HS_CONN_HANDLE_NONE;
 } // isConnected
 
 
diff --git a/lib/libesp32/NimBLE-Arduino/src/NimBLEClient.h b/lib/libesp32/NimBLE-Arduino/src/NimBLEClient.h
index ddeef3cc3..a4b847000 100644
--- a/lib/libesp32/NimBLE-Arduino/src/NimBLEClient.h
+++ b/lib/libesp32/NimBLE-Arduino/src/NimBLEClient.h
@@ -30,6 +30,7 @@
 #include <string>
 
 class NimBLERemoteService;
+class NimBLERemoteCharacteristic;
 class NimBLEClientCallbacks;
 class NimBLEAdvertisedDevice;
 
@@ -54,7 +55,8 @@ public:
     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);
+                                                         const std::string &value, bool response = false);
+    NimBLERemoteCharacteristic*                 getCharacteristic(const uint16_t handle);
     bool                                        isConnected();
     void                                        setClientCallbacks(NimBLEClientCallbacks *pClientCallbacks,
                                                                    bool deleteCallbacks = true);
@@ -82,16 +84,17 @@ private:
                                                 const struct ble_gatt_error *error,
                                                 const struct ble_gatt_svc *service,
                                                 void *arg);
+    static void             dcTimerCb(ble_npl_event *event);
     bool                    retrieveServices(const NimBLEUUID *uuid_filter = nullptr);
 
     NimBLEAddress           m_peerAddress;
     uint16_t                m_conn_id;
-    bool                    m_isConnected;
-    bool                    m_waitingToConnect;
+    bool                    m_connEstablished;
     bool                    m_deleteCallbacks;
     int32_t                 m_connectTimeout;
     NimBLEClientCallbacks*  m_pClientCallbacks;
-    ble_task_data_t         *m_pTaskData;
+    ble_task_data_t*        m_pTaskData;
+    ble_npl_callout         m_dcTimer;
 
     std::vector<NimBLERemoteService*> m_servicesVector;
 
diff --git a/lib/libesp32/NimBLE-Arduino/src/NimBLEDevice.cpp b/lib/libesp32/NimBLE-Arduino/src/NimBLEDevice.cpp
index fb36e6e57..f34a522f7 100644
--- a/lib/libesp32/NimBLE-Arduino/src/NimBLEDevice.cpp
+++ b/lib/libesp32/NimBLE-Arduino/src/NimBLEDevice.cpp
@@ -25,6 +25,7 @@
 #include "nimble/nimble_port.h"
 #include "nimble/nimble_port_freertos.h"
 #include "host/ble_hs.h"
+#include "host/ble_hs_pvcy.h"
 #include "host/util/util.h"
 #include "services/gap/ble_svc_gap.h"
 #include "services/gatt/ble_svc_gatt.h"
@@ -60,6 +61,7 @@ std::list <NimBLEClient*>   NimBLEDevice::m_cList;
 #endif
 std::list <NimBLEAddress>   NimBLEDevice::m_ignoreList;
 NimBLESecurityCallbacks*    NimBLEDevice::m_securityCallbacks = nullptr;
+uint8_t                     NimBLEDevice::m_own_addr_type = BLE_OWN_ADDR_PUBLIC;
 
 
 /**
@@ -144,8 +146,8 @@ void NimBLEDevice::stopAdvertising() {
 #if defined(CONFIG_BT_NIMBLE_ROLE_CENTRAL)
 /* STATIC */ NimBLEClient* NimBLEDevice::createClient(NimBLEAddress peerAddress) {
     if(m_cList.size() >= NIMBLE_MAX_CONNECTIONS) {
-        NIMBLE_LOGW("Number of clients exceeds Max connections. Max=(%d)",
-                                            NIMBLE_MAX_CONNECTIONS);
+        NIMBLE_LOGW(LOG_TAG,"Number of clients exceeds Max connections. Cur=%d Max=%d",
+                    m_cList.size(), NIMBLE_MAX_CONNECTIONS);
     }
 
     NimBLEClient* pClient = new NimBLEClient(peerAddress);
@@ -165,26 +167,31 @@ void NimBLEDevice::stopAdvertising() {
         return false;
     }
 
+    // Set the connection established flag to false to stop notifications
+    // from accessing the attribute vectors while they are being deleted.
+    pClient->m_connEstablished = false;
     int rc =0;
 
-    if(pClient->m_isConnected) {
+    if(pClient->isConnected()) {
         rc = pClient->disconnect();
         if (rc != 0 && rc != BLE_HS_EALREADY && rc != BLE_HS_ENOTCONN) {
             return false;
         }
 
-        while(pClient->m_isConnected) {
-            vTaskDelay(10);
+        while(pClient->isConnected()) {
+            taskYIELD();
         }
-    }
+        // Since we set the flag to false the app will not get a callback
+        // in the disconnect event so we call it here for good measure.
+        pClient->m_pClientCallbacks->onDisconnect(pClient);
 
-    if(pClient->m_waitingToConnect) {
+    } else if(pClient->m_pTaskData != nullptr) {
         rc = ble_gap_conn_cancel();
         if (rc != 0 && rc != BLE_HS_EALREADY) {
             return false;
         }
-        while(pClient->m_waitingToConnect) {
-            vTaskDelay(10);
+        while(pClient->m_pTaskData != nullptr) {
+            taskYIELD();
         }
     }
 
@@ -405,30 +412,16 @@ void NimBLEDevice::stopAdvertising() {
 
     m_synced = false;
 
-#if defined(CONFIG_BT_NIMBLE_ROLE_OBSERVER)
-    if(m_pScan != nullptr) {
-        m_pScan->onHostReset();
-    }
-#endif
-
-/*  Not needed
-    if(m_pServer != nullptr) {
-        m_pServer->onHostReset();
-    }
-
-    for(auto it = m_cList.cbegin(); it != m_cList.cend(); ++it) {
-        (*it)->onHostReset();
-    }
-*/
-
-#if defined(CONFIG_BT_NIMBLE_ROLE_BROADCASTER)
-    if(m_bleAdvertising != nullptr) {
-        m_bleAdvertising->onHostReset();
-    }
-#endif
-
     NIMBLE_LOGC(LOG_TAG, "Resetting state; reason=%d, %s", reason,
                         NimBLEUtils::returnCodeToString(reason));
+
+#if defined(CONFIG_BT_NIMBLE_ROLE_OBSERVER)
+    if(initialized) {
+        if(m_pScan != nullptr) {
+            m_pScan->onHostReset();
+        }
+    }
+#endif
 } // onReset
 
 
@@ -448,20 +441,22 @@ void NimBLEDevice::stopAdvertising() {
     int rc = ble_hs_util_ensure_addr(0);
     assert(rc == 0);
 
+    // Yield for houskeeping before returning to operations.
+    // Occasionally triggers exception without.
+    taskYIELD();
+
     m_synced = true;
 
     if(initialized) {
 #if defined(CONFIG_BT_NIMBLE_ROLE_OBSERVER)
         if(m_pScan != nullptr) {
-            // Restart scanning with the last values sent, allow to clear results.
-            m_pScan->start(m_pScan->m_duration, m_pScan->m_scanCompleteCB);
+            m_pScan->onHostSync();
         }
 #endif
 
 #if defined(CONFIG_BT_NIMBLE_ROLE_BROADCASTER)
         if(m_bleAdvertising != nullptr) {
-            // Restart advertisng, parameters should already be set.
-            m_bleAdvertising->start();
+            m_bleAdvertising->onHostSync();
         }
 #endif
     }
@@ -705,6 +700,35 @@ void NimBLEDevice::setSecurityCallbacks(NimBLESecurityCallbacks* callbacks) {
 } // setSecurityCallbacks
 
 
+/**
+ * @brief Set the own address type.
+ * @param [in] own_addr_type Own Bluetooth Device address type.\n
+ * The available bits are defined as:
+ * * 0x00: BLE_OWN_ADDR_PUBLIC
+ * * 0x01: BLE_OWN_ADDR_RANDOM
+ * * 0x02: BLE_OWN_ADDR_RPA_PUBLIC_DEFAULT
+ * * 0x03: BLE_OWN_ADDR_RPA_RANDOM_DEFAULT
+ * @param [in] useNRPA If true, and address type is random, uses a non-resolvable random address.
+ */
+void NimBLEDevice::setOwnAddrType(uint8_t own_addr_type, bool useNRPA) {
+    m_own_addr_type = own_addr_type;
+    switch (own_addr_type) {
+        case BLE_OWN_ADDR_PUBLIC:
+            ble_hs_pvcy_rpa_config(NIMBLE_HOST_DISABLE_PRIVACY);
+            break;
+        case BLE_OWN_ADDR_RANDOM:
+            setSecurityInitKey(BLE_SM_PAIR_KEY_DIST_ENC | BLE_SM_PAIR_KEY_DIST_ID);
+            ble_hs_pvcy_rpa_config(useNRPA ? NIMBLE_HOST_ENABLE_NRPA : NIMBLE_HOST_ENABLE_RPA);
+            break;
+        case BLE_OWN_ADDR_RPA_PUBLIC_DEFAULT:
+        case BLE_OWN_ADDR_RPA_RANDOM_DEFAULT:
+            setSecurityInitKey(BLE_SM_PAIR_KEY_DIST_ENC | BLE_SM_PAIR_KEY_DIST_ID);
+            ble_hs_pvcy_rpa_config(NIMBLE_HOST_ENABLE_RPA);
+            break;
+    }
+} // setOwnAddrType
+
+
 /**
  * @brief Start the connection securing and authorization for this connection.
  * @param conn_id The connection id of the peer device.
diff --git a/lib/libesp32/NimBLE-Arduino/src/NimBLEDevice.h b/lib/libesp32/NimBLE-Arduino/src/NimBLEDevice.h
index 252c52afd..2d586bb42 100644
--- a/lib/libesp32/NimBLE-Arduino/src/NimBLEDevice.h
+++ b/lib/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 void             setOwnAddrType(uint8_t own_addr_type, bool useNRPA=false);
     static int              startSecurity(uint16_t conn_id);
     static int              setMTU(uint16_t mtu);
     static uint16_t         getMTU();
@@ -182,6 +183,7 @@ private:
     static uint32_t                   m_passkey;
     static ble_gap_event_listener     m_listener;
     static gap_event_handler          m_customGapHandler;
+    static uint8_t                    m_own_addr_type;
 };
 
 
diff --git a/lib/libesp32/NimBLE-Arduino/src/NimBLEHIDDevice.cpp b/lib/libesp32/NimBLE-Arduino/src/NimBLEHIDDevice.cpp
new file mode 100644
index 000000000..39f07dede
--- /dev/null
+++ b/lib/libesp32/NimBLE-Arduino/src/NimBLEHIDDevice.cpp
@@ -0,0 +1,233 @@
+/*
+ * NimBLEHIDDevice.cpp
+ *
+ *  Created: on Oct 06 2020
+ *      Author wakwak-koba
+ *
+ * Originally:
+ *
+ * BLEHIDDevice.cpp
+ *
+ *  Created on: Jan 03, 2018
+ *      Author: chegewara
+ */
+#include "sdkconfig.h"
+#if defined(CONFIG_BT_ENABLED)
+
+#include "nimconfig.h"
+#if defined(CONFIG_BT_NIMBLE_ROLE_PERIPHERAL)
+
+#include "NimBLEHIDDevice.h"
+#include "NimBLE2904.h"
+
+NimBLEHIDDevice::NimBLEHIDDevice(NimBLEServer* server) {
+	/*
+	 * Here we create mandatory services described in bluetooth specification
+	 */
+	m_deviceInfoService = server->createService(NimBLEUUID((uint16_t) 0x180a));
+	m_hidService = server->createService(NimBLEUUID((uint16_t) 0x1812), 40);
+	m_batteryService = server->createService(NimBLEUUID((uint16_t) 0x180f));
+
+	/*
+	 * Mandatory characteristic for device info service
+	 */
+	m_pnpCharacteristic = m_deviceInfoService->createCharacteristic((uint16_t) 0x2a50, NIMBLE_PROPERTY::READ);
+
+	/*
+	 * Mandatory characteristics for HID service
+	 */
+	m_hidInfoCharacteristic = m_hidService->createCharacteristic((uint16_t) 0x2a4a, NIMBLE_PROPERTY::READ);
+	m_reportMapCharacteristic = m_hidService->createCharacteristic((uint16_t) 0x2a4b, NIMBLE_PROPERTY::READ);
+	m_hidControlCharacteristic = m_hidService->createCharacteristic((uint16_t) 0x2a4c, NIMBLE_PROPERTY::WRITE_NR);
+	m_protocolModeCharacteristic = m_hidService->createCharacteristic((uint16_t) 0x2a4e, NIMBLE_PROPERTY::WRITE_NR | NIMBLE_PROPERTY::READ);
+
+	/*
+	 * Mandatory battery level characteristic with notification and presence descriptor
+	 */
+	m_batteryLevelCharacteristic = m_batteryService->createCharacteristic((uint16_t) 0x2a19, NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::NOTIFY);
+	NimBLE2904* batteryLevelDescriptor = (NimBLE2904*)m_batteryLevelCharacteristic->createDescriptor((uint16_t) 0x2904);
+	batteryLevelDescriptor->setFormat(NimBLE2904::FORMAT_UINT8);
+	batteryLevelDescriptor->setNamespace(1);
+	batteryLevelDescriptor->setUnit(0x27ad);
+
+	/*
+	 * This value is setup here because its default value in most usage cases, its very rare to use boot mode
+	 * and we want to simplify library using as much as possible
+	 */
+	const uint8_t pMode[] = { 0x01 };
+	protocolMode()->setValue((uint8_t*) pMode, 1);
+}
+
+NimBLEHIDDevice::~NimBLEHIDDevice() {
+}
+
+/*
+ * @brief
+ */
+void NimBLEHIDDevice::reportMap(uint8_t* map, uint16_t size) {
+	m_reportMapCharacteristic->setValue(map, size);
+}
+
+/*
+ * @brief This function suppose to be called at the end, when we have created all characteristics we need to build HID service
+ */
+void NimBLEHIDDevice::startServices() {
+	m_deviceInfoService->start();
+	m_hidService->start();
+	m_batteryService->start();
+}
+
+/*
+ * @brief Create manufacturer characteristic (this characteristic is optional)
+ */
+NimBLECharacteristic* NimBLEHIDDevice::manufacturer() {
+	m_manufacturerCharacteristic = m_deviceInfoService->createCharacteristic((uint16_t) 0x2a29, NIMBLE_PROPERTY::READ);
+	return m_manufacturerCharacteristic;
+}
+
+/*
+ * @brief Set manufacturer name
+ * @param [in] name manufacturer name
+ */
+void NimBLEHIDDevice::manufacturer(std::string name) {
+	m_manufacturerCharacteristic->setValue(name);
+}
+
+/*
+ * @brief
+ */
+void NimBLEHIDDevice::pnp(uint8_t sig, uint16_t vid, uint16_t pid, uint16_t version) {
+	uint8_t pnp[] = { sig, (uint8_t) (vid >> 8), (uint8_t) vid, (uint8_t) (pid >> 8), (uint8_t) pid, (uint8_t) (version >> 8), (uint8_t) version };
+	m_pnpCharacteristic->setValue(pnp, sizeof(pnp));
+}
+
+/*
+ * @brief
+ */
+void NimBLEHIDDevice::hidInfo(uint8_t country, uint8_t flags) {
+	uint8_t info[] = { 0x11, 0x1, country, flags };
+	m_hidInfoCharacteristic->setValue(info, sizeof(info));
+}
+
+/*
+ * @brief Create input report characteristic that need to be saved as new characteristic object so can be further used
+ * @param [in] reportID input report ID, the same as in report map for input object related to created characteristic
+ * @return pointer to new input report characteristic
+ */
+NimBLECharacteristic* NimBLEHIDDevice::inputReport(uint8_t reportID) {
+	NimBLECharacteristic* inputReportCharacteristic = m_hidService->createCharacteristic((uint16_t) 0x2a4d, NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::NOTIFY);
+	NimBLEDescriptor* inputReportDescriptor = inputReportCharacteristic->createDescriptor((uint16_t) 0x2908);
+
+	uint8_t desc1_val[] = { reportID, 0x01 };
+	inputReportDescriptor->setValue((uint8_t*) desc1_val, 2);
+
+	return inputReportCharacteristic;
+}
+
+/*
+ * @brief Create output report characteristic that need to be saved as new characteristic object so can be further used
+ * @param [in] reportID Output report ID, the same as in report map for output object related to created characteristic
+ * @return Pointer to new output report characteristic
+ */
+NimBLECharacteristic* NimBLEHIDDevice::outputReport(uint8_t reportID) {
+	NimBLECharacteristic* outputReportCharacteristic = m_hidService->createCharacteristic((uint16_t) 0x2a4d, NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::WRITE | NIMBLE_PROPERTY::WRITE_NR | NIMBLE_PROPERTY::READ_ENC | NIMBLE_PROPERTY::WRITE_ENC);
+	NimBLEDescriptor* outputReportDescriptor = outputReportCharacteristic->createDescriptor((uint16_t) 0x2908);
+
+	uint8_t desc1_val[] = { reportID, 0x02 };
+	outputReportDescriptor->setValue((uint8_t*) desc1_val, 2);
+
+	return outputReportCharacteristic;
+}
+
+/*
+ * @brief Create feature report characteristic that need to be saved as new characteristic object so can be further used
+ * @param [in] reportID Feature report ID, the same as in report map for feature object related to created characteristic
+ * @return Pointer to new feature report characteristic
+ */
+NimBLECharacteristic* NimBLEHIDDevice::featureReport(uint8_t reportID) {
+	NimBLECharacteristic* featureReportCharacteristic = m_hidService->createCharacteristic((uint16_t) 0x2a4d, NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::WRITE | NIMBLE_PROPERTY::READ_ENC | NIMBLE_PROPERTY::WRITE_ENC);
+	NimBLEDescriptor* featureReportDescriptor = featureReportCharacteristic->createDescriptor((uint16_t) 0x2908);
+
+	uint8_t desc1_val[] = { reportID, 0x03 };
+	featureReportDescriptor->setValue((uint8_t*) desc1_val, 2);
+
+	return featureReportCharacteristic;
+}
+
+/*
+ * @brief
+ */
+NimBLECharacteristic* NimBLEHIDDevice::bootInput() {
+	return m_hidService->createCharacteristic((uint16_t) 0x2a22, NIMBLE_PROPERTY::NOTIFY);
+}
+
+/*
+ * @brief
+ */
+NimBLECharacteristic* NimBLEHIDDevice::bootOutput() {
+	return m_hidService->createCharacteristic((uint16_t) 0x2a32, NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::WRITE | NIMBLE_PROPERTY::WRITE_NR);
+}
+
+/*
+ * @brief
+ */
+NimBLECharacteristic* NimBLEHIDDevice::hidControl() {
+	return m_hidControlCharacteristic;
+}
+
+/*
+ * @brief
+ */
+NimBLECharacteristic* NimBLEHIDDevice::protocolMode() {
+	return m_protocolModeCharacteristic;
+}
+
+void NimBLEHIDDevice::setBatteryLevel(uint8_t level) {
+	m_batteryLevelCharacteristic->setValue(&level, 1);
+}
+/*
+ * @brief Returns battery level characteristic
+ * @ return battery level characteristic
+ *//*
+BLECharacteristic* BLEHIDDevice::batteryLevel() {
+	return m_batteryLevelCharacteristic;
+}
+
+
+
+BLECharacteristic*	 BLEHIDDevice::reportMap() {
+	return m_reportMapCharacteristic;
+}
+
+BLECharacteristic*	 BLEHIDDevice::pnp() {
+	return m_pnpCharacteristic;
+}
+
+
+BLECharacteristic*	BLEHIDDevice::hidInfo() {
+	return m_hidInfoCharacteristic;
+}
+*/
+/*
+ * @brief
+ */
+NimBLEService* NimBLEHIDDevice::deviceInfo() {
+	return m_deviceInfoService;
+}
+
+/*
+ * @brief
+ */
+NimBLEService* NimBLEHIDDevice::hidService() {
+	return m_hidService;
+}
+
+/*
+ * @brief
+ */
+NimBLEService* NimBLEHIDDevice::batteryService() {
+	return m_batteryService;
+}
+
+#endif // #if defined(CONFIG_BT_NIMBLE_ROLE_PERIPHERAL)
+#endif // #if defined(CONFIG_BT_ENABLED)
diff --git a/lib/libesp32/NimBLE-Arduino/src/NimBLEHIDDevice.h b/lib/libesp32/NimBLE-Arduino/src/NimBLEHIDDevice.h
new file mode 100644
index 000000000..3e7a6f759
--- /dev/null
+++ b/lib/libesp32/NimBLE-Arduino/src/NimBLEHIDDevice.h
@@ -0,0 +1,85 @@
+/*
+ * NimBLEHIDDevice.h
+ *
+ *  Created: on Oct 06 2020
+ *      Author wakwak-koba
+ *
+ * Originally:
+ *
+ * BLEHIDDevice.h
+ *
+ *  Created on: Jan 03, 2018
+ *      Author: chegewara
+ */
+
+#ifndef _BLEHIDDEVICE_H_
+#define _BLEHIDDEVICE_H_
+
+#include "sdkconfig.h"
+#if defined(CONFIG_BT_ENABLED)
+
+#include "nimconfig.h"
+#if defined(CONFIG_BT_NIMBLE_ROLE_BROADCASTER)
+
+#include "NimBLECharacteristic.h"
+#include "NimBLEService.h"
+#include "NimBLEDescriptor.h"
+#include "HIDTypes.h"
+
+#define GENERIC_HID		0x03C0
+#define HID_KEYBOARD	   0x03C1
+#define HID_MOUSE		  0x03C2
+#define HID_JOYSTICK	   0x03C3
+#define HID_GAMEPAD		0x03C4
+#define HID_TABLET		 0x03C5
+#define HID_CARD_READER	0x03C6
+#define HID_DIGITAL_PEN	0x03C7
+#define HID_BARCODE		0x03C8
+
+class NimBLEHIDDevice {
+public:
+	NimBLEHIDDevice(NimBLEServer*);
+	virtual ~NimBLEHIDDevice();
+
+	void reportMap(uint8_t* map, uint16_t);
+	void startServices();
+
+	NimBLEService* deviceInfo();
+	NimBLEService* hidService();
+	NimBLEService* batteryService();
+
+	NimBLECharacteristic* 	manufacturer();
+	void 	manufacturer(std::string name);
+	//NimBLECharacteristic* 	pnp();
+	void	pnp(uint8_t sig, uint16_t vid, uint16_t pid, uint16_t version);
+	//NimBLECharacteristic*	hidInfo();
+	void	hidInfo(uint8_t country, uint8_t flags);
+	//NimBLECharacteristic* 	batteryLevel();
+	void 	setBatteryLevel(uint8_t level);
+
+
+	//NimBLECharacteristic* 	reportMap();
+	NimBLECharacteristic* 	hidControl();
+	NimBLECharacteristic* 	inputReport(uint8_t reportID);
+	NimBLECharacteristic* 	outputReport(uint8_t reportID);
+	NimBLECharacteristic* 	featureReport(uint8_t reportID);
+	NimBLECharacteristic* 	protocolMode();
+	NimBLECharacteristic* 	bootInput();
+	NimBLECharacteristic* 	bootOutput();
+
+private:
+	NimBLEService*			m_deviceInfoService;			//0x180a
+	NimBLEService*			m_hidService;					//0x1812
+	NimBLEService*			m_batteryService = 0;			//0x180f
+
+	NimBLECharacteristic* 	m_manufacturerCharacteristic;	//0x2a29
+	NimBLECharacteristic* 	m_pnpCharacteristic;			//0x2a50
+	NimBLECharacteristic* 	m_hidInfoCharacteristic;		//0x2a4a
+	NimBLECharacteristic* 	m_reportMapCharacteristic;		//0x2a4b
+	NimBLECharacteristic* 	m_hidControlCharacteristic;		//0x2a4c
+	NimBLECharacteristic* 	m_protocolModeCharacteristic;	//0x2a4e
+	NimBLECharacteristic*	m_batteryLevelCharacteristic;	//0x2a19
+};
+#endif // CONFIG_BT_NIMBLE_ROLE_BROADCASTER
+#endif // CONFIG_BT_ENABLED
+#endif /* _BLEHIDDEVICE_H_ */
diff --git a/lib/libesp32/NimBLE-Arduino/src/NimBLERemoteCharacteristic.cpp b/lib/libesp32/NimBLE-Arduino/src/NimBLERemoteCharacteristic.cpp
index 154206c73..8bc5090b1 100644
--- a/lib/libesp32/NimBLE-Arduino/src/NimBLERemoteCharacteristic.cpp
+++ b/lib/libesp32/NimBLE-Arduino/src/NimBLERemoteCharacteristic.cpp
@@ -38,7 +38,7 @@ static const char* LOG_TAG = "NimBLERemoteCharacteristic";
  NimBLERemoteCharacteristic::NimBLERemoteCharacteristic(NimBLERemoteService *pRemoteService,
                                                         const struct ble_gatt_chr *chr)
 {
-
+    NIMBLE_LOGD(LOG_TAG, ">> NimBLERemoteCharacteristic()");
      switch (chr->uuid.u.type) {
         case BLE_UUID_TYPE_16:
             m_uuid = NimBLEUUID(chr->uuid.u16.value);
@@ -50,7 +50,6 @@ static const char* LOG_TAG = "NimBLERemoteCharacteristic";
             m_uuid = NimBLEUUID(const_cast<ble_uuid128_t*>(&chr->uuid.u128));
             break;
         default:
-            m_uuid = nullptr;
             break;
     }
 
@@ -61,6 +60,8 @@ static const char* LOG_TAG = "NimBLERemoteCharacteristic";
     m_notifyCallback     = nullptr;
     m_timestamp          = 0;
     m_valMux             = portMUX_INITIALIZER_UNLOCKED;
+
+    NIMBLE_LOGD(LOG_TAG, "<< NimBLERemoteCharacteristic(): %s", m_uuid.toString().c_str());
  } // NimBLERemoteCharacteristic
 
 
@@ -208,15 +209,21 @@ int NimBLERemoteCharacteristic::descriptorDiscCB(uint16_t conn_handle,
 bool NimBLERemoteCharacteristic::retrieveDescriptors(const NimBLEUUID *uuid_filter) {
     NIMBLE_LOGD(LOG_TAG, ">> retrieveDescriptors() for characteristic: %s", getUUID().toString().c_str());
 
+    uint16_t endHandle = getRemoteService()->getEndHandle(this);
+    if(m_handle >= endHandle) {
+        return false;
+    }
+
     int rc = 0;
     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,
-                                 getRemoteService()->getEndHandle(),
+                                 endHandle,
                                  NimBLERemoteCharacteristic::descriptorDiscCB,
                                  &filter);
+
     if (rc != 0) {
         NIMBLE_LOGE(LOG_TAG, "ble_gattc_disc_all_chrs: rc=%d %s", rc, NimBLEUtils::returnCodeToString(rc));
         return false;
@@ -225,12 +232,13 @@ bool NimBLERemoteCharacteristic::retrieveDescriptors(const NimBLEUUID *uuid_filt
     ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
 
     if(taskData.rc != 0) {
+        NIMBLE_LOGE(LOG_TAG, "ble_gattc_disc_all_chrs: startHandle:%d endHandle:%d taskData.rc=%d %s", m_handle, endHandle, taskData.rc, NimBLEUtils::returnCodeToString(0x0100+taskData.rc));
         return false;
     }
 
     return true;
     NIMBLE_LOGD(LOG_TAG, "<< retrieveDescriptors(): Found %d descriptors.", m_descriptorVector.size());
-} // getDescriptors
+} // retrieveDescriptors
 
 
 /**
@@ -243,7 +251,7 @@ NimBLERemoteDescriptor* NimBLERemoteCharacteristic::getDescriptor(const NimBLEUU
 
     for(auto &it: m_descriptorVector) {
         if(it->getUUID() == uuid) {
-            NIMBLE_LOGD(LOG_TAG, "<< getDescriptor: found");
+            NIMBLE_LOGD(LOG_TAG, "<< getDescriptor: found the descriptor with uuid: %s", uuid.toString().c_str());
             return it;
         }
     }
@@ -253,7 +261,18 @@ NimBLERemoteDescriptor* NimBLERemoteCharacteristic::getDescriptor(const NimBLEUU
         if(m_descriptorVector.size() > prev_size) {
             return m_descriptorVector.back();
         }
+
+        // If the request was successful but 16/32 bit descriptor not found
+        // try again with the 128 bit uuid.
+        if(uuid.bitSize() == BLE_UUID_TYPE_16 ||
+           uuid.bitSize() == BLE_UUID_TYPE_32)
+        {
+            NimBLEUUID uuid128(uuid);
+            uuid128.to128();
+            return getDescriptor(uuid128);
+        }
     }
+
     NIMBLE_LOGD(LOG_TAG, "<< getDescriptor: Not found");
     return nullptr;
 } // getDescriptor
@@ -447,9 +466,10 @@ std::string NimBLERemoteCharacteristic::readValue(time_t *timestamp) {
         }
     } while(rc != 0 && retryCount--);
 
+    time_t t = time(nullptr);
     portENTER_CRITICAL(&m_valMux);
     m_value = value;
-    m_timestamp = time(nullptr);
+    m_timestamp = t;
     if(timestamp != nullptr) {
         *timestamp = m_timestamp;
     }
@@ -506,19 +526,19 @@ int NimBLERemoteCharacteristic::onReadCB(uint16_t conn_handle,
  * @param [in] notifyCallback A callback to be invoked for a notification.
  * @param [in] response If write response required set this to true.
  * If NULL is provided then no callback is performed.
- * @return true if successful.
+ * @return false if writing to the descriptor failed.
  */
 bool NimBLERemoteCharacteristic::setNotify(uint16_t val, notify_callback notifyCallback, bool response) {
     NIMBLE_LOGD(LOG_TAG, ">> setNotify(): %s, %02x", toString().c_str(), val);
 
+    m_notifyCallback = notifyCallback;
+
     NimBLERemoteDescriptor* desc = getDescriptor(NimBLEUUID((uint16_t)0x2902));
     if(desc == nullptr) {
-        NIMBLE_LOGE(LOG_TAG, "<< setNotify(): Could not get descriptor");
-        return false;
+        NIMBLE_LOGW(LOG_TAG, "<< setNotify(): Callback set, CCCD not found");
+        return true;
     }
 
-    m_notifyCallback = notifyCallback;
-
     NIMBLE_LOGD(LOG_TAG, "<< setNotify()");
 
     return desc->writeValue((uint8_t *)&val, 2, response);
@@ -531,7 +551,7 @@ bool NimBLERemoteCharacteristic::setNotify(uint16_t val, notify_callback notifyC
  * @param [in] notifyCallback A callback to be invoked for a notification.
  * @param [in] response If true, require a write response from the descriptor write operation.
  * If NULL is provided then no callback is performed.
- * @return true if successful.
+ * @return false if writing to the descriptor failed.
  */
 bool NimBLERemoteCharacteristic::subscribe(bool notifications, notify_callback notifyCallback, bool response) {
     if(notifications) {
@@ -545,7 +565,7 @@ bool NimBLERemoteCharacteristic::subscribe(bool notifications, notify_callback n
 /**
  * @brief Unsubscribe for notifications or indications.
  * @param [in] response bool if true, require a write response from the descriptor write operation.
- * @return true if successful.
+ * @return false if writing to the descriptor failed.
  */
 bool NimBLERemoteCharacteristic::unsubscribe(bool response) {
     return setNotify(0x00, nullptr, response);
diff --git a/lib/libesp32/NimBLE-Arduino/src/NimBLERemoteDescriptor.cpp b/lib/libesp32/NimBLE-Arduino/src/NimBLERemoteDescriptor.cpp
index 9281e7df7..fc0f06b67 100644
--- a/lib/libesp32/NimBLE-Arduino/src/NimBLERemoteDescriptor.cpp
+++ b/lib/libesp32/NimBLE-Arduino/src/NimBLERemoteDescriptor.cpp
@@ -31,6 +31,7 @@ static const char* LOG_TAG = "NimBLERemoteDescriptor";
 NimBLERemoteDescriptor::NimBLERemoteDescriptor(NimBLERemoteCharacteristic* pRemoteCharacteristic,
                                                const struct ble_gatt_dsc *dsc)
 {
+    NIMBLE_LOGD(LOG_TAG, ">> NimBLERemoteDescriptor()");
     switch (dsc->uuid.u.type) {
         case BLE_UUID_TYPE_16:
             m_uuid = NimBLEUUID(dsc->uuid.u16.value);
@@ -42,12 +43,13 @@ NimBLERemoteDescriptor::NimBLERemoteDescriptor(NimBLERemoteCharacteristic* pRemo
             m_uuid = NimBLEUUID(const_cast<ble_uuid128_t*>(&dsc->uuid.u128));
             break;
         default:
-            m_uuid = nullptr;
             break;
     }
 
     m_handle                = dsc->handle;
     m_pRemoteCharacteristic = pRemoteCharacteristic;
+
+    NIMBLE_LOGD(LOG_TAG, "<< NimBLERemoteDescriptor(): %s", m_uuid.toString().c_str());
 }
 
 
diff --git a/lib/libesp32/NimBLE-Arduino/src/NimBLERemoteService.cpp b/lib/libesp32/NimBLE-Arduino/src/NimBLERemoteService.cpp
index 8901175dc..cb20e0b3f 100644
--- a/lib/libesp32/NimBLE-Arduino/src/NimBLERemoteService.cpp
+++ b/lib/libesp32/NimBLE-Arduino/src/NimBLERemoteService.cpp
@@ -44,12 +44,11 @@ NimBLERemoteService::NimBLERemoteService(NimBLEClient* pClient, const struct ble
             m_uuid = NimBLEUUID(const_cast<ble_uuid128_t*>(&service->uuid.u128));
             break;
         default:
-            m_uuid = nullptr;
             break;
     }
     m_startHandle = service->start_handle;
     m_endHandle = service->end_handle;
-    NIMBLE_LOGD(LOG_TAG, "<< NimBLERemoteService()");
+    NIMBLE_LOGD(LOG_TAG, "<< NimBLERemoteService(): %s", m_uuid.toString().c_str());
 }
 
 
@@ -95,8 +94,11 @@ NimBLERemoteCharacteristic* NimBLERemoteService::getCharacteristic(const char* u
  * @return A pointer to the characteristic object, or nullptr if not found.
  */
 NimBLERemoteCharacteristic* NimBLERemoteService::getCharacteristic(const NimBLEUUID &uuid) {
+    NIMBLE_LOGD(LOG_TAG, ">> getCharacteristic: uuid: %s", uuid.toString().c_str());
+
     for(auto &it: m_characteristicVector) {
         if(it->getUUID() == uuid) {
+            NIMBLE_LOGD(LOG_TAG, "<< getCharacteristic: found the characteristic with uuid: %s", uuid.toString().c_str());
             return it;
         }
     }
@@ -106,8 +108,19 @@ NimBLERemoteCharacteristic* NimBLERemoteService::getCharacteristic(const NimBLEU
         if(m_characteristicVector.size() > prev_size) {
             return m_characteristicVector.back();
         }
+
+        // If the request was successful but 16/32 bit characteristic not found
+        // try again with the 128 bit uuid.
+        if(uuid.bitSize() == BLE_UUID_TYPE_16 ||
+           uuid.bitSize() == BLE_UUID_TYPE_32)
+        {
+            NimBLEUUID uuid128(uuid);
+            uuid128.to128();
+            return getCharacteristic(uuid128);
+        }
     }
 
+    NIMBLE_LOGD(LOG_TAG, "<< getCharacteristic: not found");
     return nullptr;
 } // getCharacteristic
 
@@ -236,6 +249,23 @@ uint16_t NimBLERemoteService::getEndHandle() {
     return m_endHandle;
 } // getEndHandle
 
+/**
+ * @brief Get the end handle of specified NimBLERemoteCharacteristic.
+ */
+
+uint16_t NimBLERemoteService::getEndHandle(NimBLERemoteCharacteristic *pCharacteristic) {
+    uint16_t endHandle = m_endHandle;
+    
+    for(auto &it: m_characteristicVector) {
+        uint16_t defHandle = it->getDefHandle() - 1;
+        if(defHandle > pCharacteristic->getDefHandle() && endHandle > defHandle) {
+            endHandle = defHandle;
+        }
+    }
+
+    return endHandle;
+} // getEndHandle
+
 
 /**
  * @brief Get the service start handle.
diff --git a/lib/libesp32/NimBLE-Arduino/src/NimBLERemoteService.h b/lib/libesp32/NimBLE-Arduino/src/NimBLERemoteService.h
index 751c9effb..4920844e4 100644
--- a/lib/libesp32/NimBLE-Arduino/src/NimBLERemoteService.h
+++ b/lib/libesp32/NimBLE-Arduino/src/NimBLERemoteService.h
@@ -70,6 +70,7 @@ private:
 
     uint16_t            getStartHandle();
     uint16_t            getEndHandle();
+    uint16_t            getEndHandle(NimBLERemoteCharacteristic *pCharacteristic);
     void                releaseSemaphores();
 
     // Properties
diff --git a/lib/libesp32/NimBLE-Arduino/src/NimBLEScan.cpp b/lib/libesp32/NimBLE-Arduino/src/NimBLEScan.cpp
index dc82de549..122ff3332 100644
--- a/lib/libesp32/NimBLE-Arduino/src/NimBLEScan.cpp
+++ b/lib/libesp32/NimBLE-Arduino/src/NimBLEScan.cpp
@@ -30,7 +30,6 @@ static const char* LOG_TAG = "NimBLEScan";
  * @brief Scan constuctor.
  */
 NimBLEScan::NimBLEScan() {
-    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)
@@ -38,9 +37,10 @@ NimBLEScan::NimBLEScan() {
     m_scan_params.limited            = 0; // If set, only discover devices in limited discoverable mode.
     m_scan_params.filter_duplicates  = 0; // If set, the controller ignores all but the first advertisement from each device.
     m_pAdvertisedDeviceCallbacks     = nullptr;
-    m_stopped                        = true;
+    m_ignoreResults                  = false;
     m_wantDuplicates                 = false;
     m_pTaskData                      = nullptr;
+    m_duration                       = BLE_HS_FOREVER; // make sure this is non-zero in the event of a host reset
 }
 
 
@@ -63,8 +63,8 @@ NimBLEScan::~NimBLEScan() {
     switch(event->type) {
 
         case BLE_GAP_EVENT_DISC: {
-            if(pScan->m_stopped) {
-                NIMBLE_LOGE(LOG_TAG, "Scan stop called, ignoring results.");
+            if(pScan->m_ignoreResults) {
+                NIMBLE_LOGE(LOG_TAG, "Scan op in progress - ignoring results");
                 return 0;
             }
 
@@ -129,7 +129,6 @@ NimBLEScan::~NimBLEScan() {
                 pScan->m_scanCompleteCB(pScan->m_scanResults);
             }
 
-            pScan->m_stopped = true;
             if(pScan->m_pTaskData != nullptr) {
                 pScan->m_pTaskData->rc = event->disc_complete.reason;
                 xTaskNotifyGive(pScan->m_pTaskData->task);
@@ -238,7 +237,7 @@ void NimBLEScan::setWindow(uint16_t windowMSecs) {
  * @return true if scanning or scan starting.
  */
 bool NimBLEScan::isScanning() {
-    return !m_stopped;
+    return ble_gap_disc_active();
 }
 
 
@@ -252,25 +251,6 @@ bool NimBLEScan::isScanning() {
 bool NimBLEScan::start(uint32_t duration, void (*scanCompleteCB)(NimBLEScanResults), bool is_continue) {
     NIMBLE_LOGD(LOG_TAG, ">> start(duration=%d)", duration);
 
-    // If Host is not synced we cannot start scanning.
-    if(!NimBLEDevice::m_synced) {
-        NIMBLE_LOGC(LOG_TAG, "Host reset, wait for sync.");
-        return false;
-    }
-
-    if(ble_gap_conn_active()) {
-        NIMBLE_LOGE(LOG_TAG, "Connection in progress - must wait.");
-        return false;
-    }
-
-    // If we are already scanning don't start again or we will get stuck on the semaphore.
-    if(!m_stopped || ble_gap_disc_active()) { // double check - can cause host reset.
-        NIMBLE_LOGE(LOG_TAG, "Scan already in progress");
-        return false;
-    }
-
-    m_stopped = false;
-
     // Save the callback to be invoked when the scan completes.
     m_scanCompleteCB = scanCompleteCB;
     // Save the duration in the case that the host is reset so we can reuse it.
@@ -281,32 +261,51 @@ bool NimBLEScan::start(uint32_t duration, void (*scanCompleteCB)(NimBLEScanResul
         duration = BLE_HS_FOREVER;
     }
     else{
-        duration = duration*1000; // convert duration to milliseconds
+        // convert duration to milliseconds
+        duration = duration * 1000;
     }
 
-    //  if we are connecting to devices that are advertising even after being connected, multiconnecting peripherals
-    //  then we should not clear vector or we will connect the same device few times
+    // Set the flag to ignore the results while we are deleting the vector
     if(!is_continue) {
-        clearResults();
+        m_ignoreResults = true;
     }
 
-    int rc = 0;
-    do{
-        rc = ble_gap_disc(m_own_addr_type, duration, &m_scan_params,
-                                    NimBLEScan::handleGapEvent, this);
-        if(rc == BLE_HS_EBUSY) {
-            vTaskDelay(1 / portTICK_PERIOD_MS);
-        }
-    } while(rc == BLE_HS_EBUSY);
+    int rc = ble_gap_disc(NimBLEDevice::m_own_addr_type, duration, &m_scan_params,
+                          NimBLEScan::handleGapEvent, this);
 
-    if (rc != 0 && rc != BLE_HS_EDONE) {
-        NIMBLE_LOGE(LOG_TAG, "Error initiating GAP discovery procedure; rc=%d, %s",
-                                        rc, NimBLEUtils::returnCodeToString(rc));
-        m_stopped = true;
+    switch(rc) {
+        case 0:
+            if(!is_continue) {
+                clearResults();
+            }
+            break;
+
+        case BLE_HS_EALREADY:
+            break;
+
+        case BLE_HS_EBUSY:
+            NIMBLE_LOGE(LOG_TAG, "Unable to scan - connection in progress.");
+            break;
+
+        case BLE_HS_ETIMEOUT_HCI:
+        case BLE_HS_EOS:
+        case BLE_HS_ECONTROLLER:
+        case BLE_HS_ENOTSYNCED:
+            NIMBLE_LOGC(LOG_TAG, "Unable to scan - Host Reset");
+            break;
+
+        default:
+            NIMBLE_LOGE(LOG_TAG, "Error initiating GAP discovery procedure; rc=%d, %s",
+                        rc, NimBLEUtils::returnCodeToString(rc));
+            break;
+    }
+
+    m_ignoreResults = false;
+    NIMBLE_LOGD(LOG_TAG, "<< start()");
+
+    if(rc != 0 && rc != BLE_HS_EALREADY) {
         return false;
     }
-
-    NIMBLE_LOGD(LOG_TAG, "<< start()");
     return true;
 } // start
 
@@ -347,8 +346,6 @@ bool NimBLEScan::stop() {
         return false;
     }
 
-    m_stopped = true;
-
     if (rc != BLE_HS_EALREADY && m_scanCompleteCB != nullptr) {
         m_scanCompleteCB(m_scanResults);
     }
@@ -381,13 +378,25 @@ void NimBLEScan::erase(const NimBLEAddress &address) {
 
 
 /**
- * @brief If the host reset the scan will have stopped so we should set the flag as stopped.
+ * @brief Called when host reset, we set a flag to stop scanning until synced.
  */
 void NimBLEScan::onHostReset() {
-    m_stopped = true;
+    m_ignoreResults = true;
 }
 
 
+/**
+ * @brief If the host reset and re-synced this is called.
+ * If the application was scanning indefinitely with a callback, restart it.
+ */
+void NimBLEScan::onHostSync() {
+    m_ignoreResults = false;
+
+    if(m_duration == 0 && m_pAdvertisedDeviceCallbacks != nullptr) {
+        start(m_duration, m_scanCompleteCB);
+    }
+}
+
 /**
  * @brief Get the results of the scan.
  * @return NimBLEScanResults object.
diff --git a/lib/libesp32/NimBLE-Arduino/src/NimBLEScan.h b/lib/libesp32/NimBLE-Arduino/src/NimBLEScan.h
index 822629025..9007a7dd8 100644
--- a/lib/libesp32/NimBLE-Arduino/src/NimBLEScan.h
+++ b/lib/libesp32/NimBLE-Arduino/src/NimBLEScan.h
@@ -83,12 +83,12 @@ private:
     ~NimBLEScan();
     static int          handleGapEvent(ble_gap_event*  event, void* arg);
     void                onHostReset();
+    void                onHostSync();
 
     NimBLEAdvertisedDeviceCallbacks*    m_pAdvertisedDeviceCallbacks = nullptr;
     void                                (*m_scanCompleteCB)(NimBLEScanResults scanResults);
     ble_gap_disc_params                 m_scan_params;
-    uint8_t                             m_own_addr_type;
-    bool                                m_stopped;
+    bool                                m_ignoreResults;
     bool                                m_wantDuplicates;
     NimBLEScanResults                   m_scanResults;
     uint32_t                            m_duration;
diff --git a/lib/libesp32/NimBLE-Arduino/src/NimBLEServer.cpp b/lib/libesp32/NimBLE-Arduino/src/NimBLEServer.cpp
index 8c75192a9..655511aea 100644
--- a/lib/libesp32/NimBLE-Arduino/src/NimBLEServer.cpp
+++ b/lib/libesp32/NimBLE-Arduino/src/NimBLEServer.cpp
@@ -296,6 +296,7 @@ size_t NimBLEServer::getConnectedCount() {
             }
 
             server->m_pServerCallbacks->onDisconnect(server);
+            server->m_pServerCallbacks->onDisconnect(server, &event->disconnect.conn);
 
             if(server->m_advertiseOnDisconnect) {
                 server->startAdvertising();
@@ -658,6 +659,10 @@ void NimBLEServerCallbacks::onDisconnect(NimBLEServer* pServer) {
     NIMBLE_LOGD("NimBLEServerCallbacks", "onDisconnect(): Default");
 } // onDisconnect
 
+void NimBLEServerCallbacks::onDisconnect(NimBLEServer* pServer, ble_gap_conn_desc* desc) {
+    NIMBLE_LOGD("NimBLEServerCallbacks", "onDisconnect(): Default");
+} // onDisconnect
+
 uint32_t NimBLEServerCallbacks::onPassKeyRequest(){
     NIMBLE_LOGD("NimBLEServerCallbacks", "onPassKeyRequest: default: 123456");
     return 123456;
diff --git a/lib/libesp32/NimBLE-Arduino/src/NimBLEServer.h b/lib/libesp32/NimBLE-Arduino/src/NimBLEServer.h
index 1fa24b23c..bedf9cf58 100644
--- a/lib/libesp32/NimBLE-Arduino/src/NimBLEServer.h
+++ b/lib/libesp32/NimBLE-Arduino/src/NimBLEServer.h
@@ -114,6 +114,15 @@ public:
      */
     virtual void onDisconnect(NimBLEServer* pServer);
 
+     /**
+     * @brief Handle a client disconnection.
+     * This is called when a client discconnects.
+     * @param [in] pServer A pointer to the %BLE server that received the client disconnection.
+     * @param [in] desc A pointer to the connection description structure containig information
+     * about the connection.
+     */
+    virtual void onDisconnect(NimBLEServer* pServer, ble_gap_conn_desc* desc);
+
     /**
      * @brief Called when a client requests a passkey for pairing.
      * @return The passkey to be sent to the client.
diff --git a/lib/libesp32/NimBLE-Arduino/src/NimBLEUUID.cpp b/lib/libesp32/NimBLE-Arduino/src/NimBLEUUID.cpp
index 1b00a3237..21ff27047 100644
--- a/lib/libesp32/NimBLE-Arduino/src/NimBLEUUID.cpp
+++ b/lib/libesp32/NimBLE-Arduino/src/NimBLEUUID.cpp
@@ -264,6 +264,37 @@ std::string NimBLEUUID::toString() const {
  */
 bool NimBLEUUID::operator ==(const NimBLEUUID & rhs) const {
     if(m_valueSet && rhs.m_valueSet) {
+        NIMBLE_LOGD(LOG_TAG,"Comparing UUIDs; type %u to %u; UUID %s to %s",
+                            m_uuid.u.type, rhs.m_uuid.u.type,
+                            this->toString().c_str(), rhs.toString().c_str());
+
+        if(m_uuid.u.type != rhs.m_uuid.u.type) {
+            uint8_t uuidBase[16] = {
+                0xfb, 0x34, 0x9b, 0x5f, 0x80, 0x00, 0x00, 0x80,
+                0x00, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
+            };
+
+            if(m_uuid.u.type == BLE_UUID_TYPE_128){
+                if(rhs.m_uuid.u.type == BLE_UUID_TYPE_16){
+                    memcpy(uuidBase+12, &rhs.m_uuid.u16.value, 2);
+                } else if (rhs.m_uuid.u.type == BLE_UUID_TYPE_32){
+                    memcpy(uuidBase+12, &rhs.m_uuid.u32.value, 4);
+                }
+                return memcmp(m_uuid.u128.value,uuidBase,16) == 0;
+
+            } else if(rhs.m_uuid.u.type == BLE_UUID_TYPE_128) {
+                if(m_uuid.u.type == BLE_UUID_TYPE_16){
+                    memcpy(uuidBase+12, &m_uuid.u16.value, 2);
+                } else if (m_uuid.u.type == BLE_UUID_TYPE_32){
+                    memcpy(uuidBase+12, &m_uuid.u32.value, 4);
+                }
+                return memcmp(rhs.m_uuid.u128.value,uuidBase,16) == 0;
+
+            } else {
+                return false;
+            }
+        }
+
         return ble_uuid_cmp(&m_uuid.u, &rhs.m_uuid.u) == 0;
     }
 
diff --git a/lib/libesp32/NimBLE-Arduino/src/esp-hci/src/esp_nimble_hci.c b/lib/libesp32/NimBLE-Arduino/src/esp-hci/src/esp_nimble_hci.c
index 0ba15e9c3..98c133fa5 100644
--- a/lib/libesp32/NimBLE-Arduino/src/esp-hci/src/esp_nimble_hci.c
+++ b/lib/libesp32/NimBLE-Arduino/src/esp-hci/src/esp_nimble_hci.c
@@ -19,6 +19,13 @@
  * under the License.
  */
 
+/*
+ * This file has been modified by Ryan Powell, aka h2zero.
+ * The modifications are for the purpose of improving performance and support
+ * for Esprssif versions used by the ardruino-esp32 core that are less current
+ * than the esp-idf releases.
+ */
+
 #include <assert.h>
 #include "sysinit/sysinit.h"
 #include "nimble/hci_common.h"
@@ -30,8 +37,10 @@
 #include "esp_bt.h"
 #include "freertos/semphr.h"
 #include "esp_compiler.h"
+/* IPC is used to improve performance when calls come from a processor not running the NimBLE stack */
+/* but does not exist for solo */
 #ifndef CONFIG_FREERTOS_UNICORE
-#include "esp_ipc.h"
+  #include "esp_ipc.h"
 #endif
 
 #define NIMBLE_VHCI_TIMEOUT_MS  2000
@@ -81,31 +90,40 @@ void ble_hci_trans_cfg_hs(ble_hci_trans_rx_cmd_fn *cmd_cb,
     ble_hci_rx_acl_hs_arg = acl_arg;
 }
 
-void ble_hci_trans_hs_cmd_tx_on_core_0(void *arg)
+/* Added; Called from the core NimBLE is running on, not used for unicore */
+#ifndef CONFIG_FREERTOS_UNICORE
+void ble_hci_trans_hs_cmd_tx_on_core(void *arg)
 {
-    uint8_t *cmd = arg;
-    uint16_t len = BLE_HCI_CMD_HDR_LEN + cmd[3] + 1;
-    esp_vhci_host_send_packet(cmd, len);
+    // Ugly but necessary as the arduino core does not provide enough IPC stack for variables.
+    esp_vhci_host_send_packet((uint8_t*)arg, *((uint8_t*)arg + 3) + 1 + BLE_HCI_CMD_HDR_LEN);
 }
+#endif
 
+/* Modified to use ipc calls in arduino to correct performance issues */
 int ble_hci_trans_hs_cmd_tx(uint8_t *cmd)
 {
+    uint16_t len;
     uint8_t rc = 0;
 
     assert(cmd != NULL);
     *cmd = BLE_HCI_UART_H4_CMD;
+    len = BLE_HCI_CMD_HDR_LEN + cmd[3] + 1;
     if (!esp_vhci_host_check_send_available()) {
         ESP_LOGD(TAG, "Controller not ready to receive packets");
     }
 
     if (xSemaphoreTake(vhci_send_sem, NIMBLE_VHCI_TIMEOUT_MS / portTICK_PERIOD_MS) == pdTRUE) {
-        if (xPortGetCoreID() != 0) {
+/* esp_ipc_call_blocking does not exist for solo */
 #ifndef CONFIG_FREERTOS_UNICORE
-            esp_ipc_call_blocking(0, ble_hci_trans_hs_cmd_tx_on_core_0, cmd);
-#endif
+        if (xPortGetCoreID() != CONFIG_BT_NIMBLE_PINNED_TO_CORE) {
+            esp_ipc_call_blocking(CONFIG_BT_NIMBLE_PINNED_TO_CORE,
+                                  ble_hci_trans_hs_cmd_tx_on_core, cmd);
         } else {
-            ble_hci_trans_hs_cmd_tx_on_core_0(cmd);
+            esp_vhci_host_send_packet(cmd, len);
         }
+#else /* Unicore */
+        esp_vhci_host_send_packet(cmd, len);
+#endif
     } else {
         rc = BLE_HS_ETIMEOUT_HCI;
     }
@@ -124,21 +142,21 @@ int ble_hci_trans_ll_evt_tx(uint8_t *hci_ev)
     return rc;
 }
 
-void ble_hci_trans_hs_acl_tx_on_core_0(void *arg)
+/* Added; Called from the core NimBLE is running on, not used for unicore */
+#ifndef CONFIG_FREERTOS_UNICORE
+void ble_hci_trans_hs_acl_tx_on_core(void *arg)
 {
-    uint8_t data[MYNEWT_VAL(BLE_ACL_BUF_SIZE) + 1];
-    struct os_mbuf *om = arg;
-    uint16_t len = 1 + OS_MBUF_PKTLEN(om);
-
-    data[0] = BLE_HCI_UART_H4_ACL;
-    os_mbuf_copydata(om, 0, OS_MBUF_PKTLEN(om), &data[1]);
-
-    esp_vhci_host_send_packet(data, len);
+    // Ugly but necessary as the arduino core does not provide enough IPC stack for variables.
+    esp_vhci_host_send_packet((uint8_t*)arg + 2, *(uint16_t*)arg);
 }
+#endif
 
+/* Modified to use ipc calls in arduino to correct performance issues */
 int ble_hci_trans_hs_acl_tx(struct os_mbuf *om)
 {
-    uint8_t rc = 0;
+    uint16_t len = 0;
+    uint8_t data[MYNEWT_VAL(BLE_ACL_BUF_SIZE) + 3], rc = 0;
+    bool tx_using_nimble_core = 0;
     /* If this packet is zero length, just free it */
     if (OS_MBUF_PKTLEN(om) == 0) {
         os_mbuf_free_chain(om);
@@ -149,14 +167,36 @@ int ble_hci_trans_hs_acl_tx(struct os_mbuf *om)
         ESP_LOGD(TAG, "Controller not ready to receive packets");
     }
 
-    if (xSemaphoreTake(vhci_send_sem, NIMBLE_VHCI_TIMEOUT_MS / portTICK_PERIOD_MS) == pdTRUE) {
-        if (xPortGetCoreID() != 0) {
+    len = 1 + OS_MBUF_PKTLEN(om);
+/* Don't check core ID if unicore */
 #ifndef CONFIG_FREERTOS_UNICORE
-            esp_ipc_call_blocking(0, ble_hci_trans_hs_acl_tx_on_core_0, om);
+    tx_using_nimble_core = xPortGetCoreID() != CONFIG_BT_NIMBLE_PINNED_TO_CORE;
+    if (tx_using_nimble_core) {
+        data[0] = len;
+        data[1] = (len >> 8);
+        data[2] = BLE_HCI_UART_H4_ACL;
+        os_mbuf_copydata(om, 0, OS_MBUF_PKTLEN(om), &data[3]);
+    } else {
+        data[0] = BLE_HCI_UART_H4_ACL;
+        os_mbuf_copydata(om, 0, OS_MBUF_PKTLEN(om), &data[1]);
+    }
+#else /* Unicore */
+    data[0] = BLE_HCI_UART_H4_ACL;
+    os_mbuf_copydata(om, 0, OS_MBUF_PKTLEN(om), &data[1]);
 #endif
+
+    if (xSemaphoreTake(vhci_send_sem, NIMBLE_VHCI_TIMEOUT_MS / portTICK_PERIOD_MS) == pdTRUE) {
+/* esp_ipc_call_blocking does not exist for solo */
+#ifndef CONFIG_FREERTOS_UNICORE
+        if (tx_using_nimble_core) {
+            esp_ipc_call_blocking(CONFIG_BT_NIMBLE_PINNED_TO_CORE,
+                                  ble_hci_trans_hs_acl_tx_on_core, data);
         } else {
-            ble_hci_trans_hs_acl_tx_on_core_0(om);
+            esp_vhci_host_send_packet(data, len);
         }
+#else /* Unicore */
+        esp_vhci_host_send_packet(data, len);
+#endif
     } else {
         rc = BLE_HS_ETIMEOUT_HCI;
     }
@@ -367,6 +407,13 @@ static int host_rcv_pkt(uint8_t *data, uint16_t len)
         totlen = BLE_HCI_EVENT_HDR_LEN + data[2];
         assert(totlen <= UINT8_MAX + BLE_HCI_EVENT_HDR_LEN);
 
+        if (totlen > MYNEWT_VAL(BLE_HCI_EVT_BUF_SIZE)) {
+            ESP_LOGE(TAG, "Received HCI data length at host (%d) exceeds maximum configured HCI event buffer size (%d).",
+                     totlen, MYNEWT_VAL(BLE_HCI_EVT_BUF_SIZE));
+            ble_hs_sched_reset(BLE_HS_ECONTROLLER);
+            return 0;
+        }
+
         if (data[1] == BLE_HCI_EVCODE_HW_ERROR) {
             assert(0);
         }
@@ -476,6 +523,9 @@ esp_err_t esp_nimble_hci_and_controller_init(void)
     esp_bt_controller_mem_release(ESP_BT_MODE_CLASSIC_BT);
 
     esp_bt_controller_config_t bt_cfg = BT_CONTROLLER_INIT_CONFIG_DEFAULT();
+    /* Added to ensure BLE only mode */
+    bt_cfg.mode = ESP_BT_MODE_BLE;
+    /* Added to set max connections from nimconfig */
     bt_cfg.ble_max_conn = CONFIG_BT_NIMBLE_MAX_CONNECTIONS;
 
     if ((ret = esp_bt_controller_init(&bt_cfg)) != ESP_OK) {
@@ -485,6 +535,7 @@ esp_err_t esp_nimble_hci_and_controller_init(void)
     if ((ret = esp_bt_controller_enable(ESP_BT_MODE_BLE)) != ESP_OK) {
         return ret;
     }
+
     return esp_nimble_hci_init();
 }
 
diff --git a/lib/libesp32/NimBLE-Arduino/src/esp_nimble_cfg.h b/lib/libesp32/NimBLE-Arduino/src/esp_nimble_cfg.h
index 384ec4a56..bafbeac8f 100644
--- a/lib/libesp32/NimBLE-Arduino/src/esp_nimble_cfg.h
+++ b/lib/libesp32/NimBLE-Arduino/src/esp_nimble_cfg.h
@@ -483,6 +483,10 @@
 #define MYNEWT_VAL_BLE_L2CAP_COC_MAX_NUM CONFIG_BT_NIMBLE_L2CAP_COC_MAX_NUM
 #endif
 
+#ifndef MYNEWT_VAL_BLE_L2CAP_COC_MPS
+#define MYNEWT_VAL_BLE_L2CAP_COC_MPS (MYNEWT_VAL_MSYS_1_BLOCK_SIZE - 8)
+#endif
+
 #ifndef MYNEWT_VAL_BLE_L2CAP_JOIN_RX_FRAGS
 #define MYNEWT_VAL_BLE_L2CAP_JOIN_RX_FRAGS (1)
 #endif
diff --git a/lib/libesp32/NimBLE-Arduino/src/nimble/host/src/ble_eddystone.c b/lib/libesp32/NimBLE-Arduino/src/nimble/host/src/ble_eddystone.c
index 7d80d134d..eccb3e988 100644
--- a/lib/libesp32/NimBLE-Arduino/src/nimble/host/src/ble_eddystone.c
+++ b/lib/libesp32/NimBLE-Arduino/src/nimble/host/src/ble_eddystone.c
@@ -76,7 +76,7 @@ ble_eddystone_set_adv_data_gen(struct ble_hs_adv_fields *adv_fields,
     if (adv_fields->num_uuids16 > BLE_EDDYSTONE_MAX_UUIDS16) {
         return BLE_HS_EINVAL;
     }
-    if (svc_data_len > BLE_EDDYSTONE_MAX_SVC_DATA_LEN) {
+    if (svc_data_len > (BLE_EDDYSTONE_MAX_SVC_DATA_LEN - BLE_EDDYSTONE_SVC_DATA_BASE_SZ)) {
         return BLE_HS_EINVAL;
     }
     if (adv_fields->num_uuids16 > 0 && !adv_fields->uuids16_is_complete) {
diff --git a/lib/libesp32/NimBLE-Arduino/src/nimble/host/src/ble_gap.c b/lib/libesp32/NimBLE-Arduino/src/nimble/host/src/ble_gap.c
index 09ae27047..d77ff6a87 100644
--- a/lib/libesp32/NimBLE-Arduino/src/nimble/host/src/ble_gap.c
+++ b/lib/libesp32/NimBLE-Arduino/src/nimble/host/src/ble_gap.c
@@ -1017,7 +1017,7 @@ ble_gap_master_failed(int status)
 #endif
 
     default:
-        BLE_HS_DBG_ASSERT(0);
+        //BLE_HS_DBG_ASSERT(0);
         break;
     }
 }
@@ -1458,8 +1458,8 @@ ble_gap_rx_periodic_adv_rpt(struct hci_le_subev_periodic_adv_rpt *evt)
 {
     struct ble_hs_periodic_sync *psync;
     struct ble_gap_event event;
-    ble_gap_event_fn *cb;
-    void *cb_arg;
+    ble_gap_event_fn *cb = NULL;
+    void *cb_arg = NULL;
 
     ble_hs_lock();
     psync = ble_hs_periodic_sync_find_by_handle(evt->sync_handle);
diff --git a/lib/libesp32/NimBLE-Arduino/src/nimble/host/src/ble_hs_conn.c b/lib/libesp32/NimBLE-Arduino/src/nimble/host/src/ble_hs_conn.c
index eb65e3288..b45128e23 100644
--- a/lib/libesp32/NimBLE-Arduino/src/nimble/host/src/ble_hs_conn.c
+++ b/lib/libesp32/NimBLE-Arduino/src/nimble/host/src/ble_hs_conn.c
@@ -470,29 +470,52 @@ ble_hs_conn_timer(void)
     int32_t time_diff;
     uint16_t conn_handle;
 
-    conn_handle = BLE_HS_CONN_HANDLE_NONE;
-    next_exp_in = BLE_HS_FOREVER;
-    now = ble_npl_time_get();
+    for (;;) {
+        conn_handle = BLE_HS_CONN_HANDLE_NONE;
+        next_exp_in = BLE_HS_FOREVER;
+        now = ble_npl_time_get();
 
-    ble_hs_lock();
+        ble_hs_lock();
 
-    /* This loop performs one of two tasks:
-     * 1. Determine if any connections need to be terminated due to timeout.
-     *    If so, break out of the loop and terminate the connection.  This
-     *    function will need to be executed again.
-     * 2. Otherwise, determine when the next timeout will occur.
-     */
-    SLIST_FOREACH(conn, &ble_hs_conns, bhc_next) {
-        if (!(conn->bhc_flags & BLE_HS_CONN_F_TERMINATING)) {
+        /* This loop performs one of two tasks:
+         * 1. Determine if any connections need to be terminated due to timeout.
+         *    If so, break out of the loop and terminate the connection.  This
+         *    function will need to be executed again.
+         * 2. Otherwise, determine when the next timeout will occur.
+         */
+        SLIST_FOREACH(conn, &ble_hs_conns, bhc_next) {
+            if (!(conn->bhc_flags & BLE_HS_CONN_F_TERMINATING)) {
 
 #if MYNEWT_VAL(BLE_L2CAP_RX_FRAG_TIMEOUT) != 0
-            /* Check each connection's rx fragment timer.  If too much time
-             * passes after a partial packet is received, the connection is
-             * terminated.
-             */
-            if (conn->bhc_rx_chan != NULL) {
-                time_diff = conn->bhc_rx_timeout - now;
+                /* Check each connection's rx fragment timer.  If too much time
+                 * passes after a partial packet is received, the connection is
+                 * terminated.
+                 */
+                if (conn->bhc_rx_chan != NULL) {
+                    time_diff = conn->bhc_rx_timeout - now;
 
+                    if (time_diff <= 0) {
+                        /* ACL reassembly has timed out.  Remember the connection
+                         * handle so it can be terminated after the mutex is
+                         * unlocked.
+                         */
+                        conn_handle = conn->bhc_handle;
+                        break;
+                    }
+
+                    /* Determine if this connection is the soonest to time out. */
+                    if (time_diff < next_exp_in) {
+                        next_exp_in = time_diff;
+                    }
+                }
+#endif
+
+#if BLE_HS_ATT_SVR_QUEUED_WRITE_TMO
+                /* Check each connection's rx queued write timer.  If too much
+                 * time passes after a prep write is received, the queue is
+                 * cleared.
+                 */
+                time_diff = ble_att_svr_ticks_until_tmo(&conn->bhc_att_svr, now);
                 if (time_diff <= 0) {
                     /* ACL reassembly has timed out.  Remember the connection
                      * handle so it can be terminated after the mutex is
@@ -506,45 +529,22 @@ ble_hs_conn_timer(void)
                 if (time_diff < next_exp_in) {
                     next_exp_in = time_diff;
                 }
-            }
 #endif
-
-#if BLE_HS_ATT_SVR_QUEUED_WRITE_TMO
-            /* Check each connection's rx queued write timer.  If too much
-             * time passes after a prep write is received, the queue is
-             * cleared.
-             */
-            time_diff = ble_att_svr_ticks_until_tmo(&conn->bhc_att_svr, now);
-            if (time_diff <= 0) {
-                /* ACL reassembly has timed out.  Remember the connection
-                 * handle so it can be terminated after the mutex is
-                 * unlocked.
-                 */
-                conn_handle = conn->bhc_handle;
-                break;
             }
-
-            /* Determine if this connection is the soonest to time out. */
-            if (time_diff < next_exp_in) {
-                next_exp_in = time_diff;
-            }
-#endif
         }
+
+        ble_hs_unlock();
+
+        /* If a connection has timed out, terminate it.  We need to repeatedly
+         * call this function again to determine when the next timeout is.
+         */
+        if (conn_handle != BLE_HS_CONN_HANDLE_NONE) {
+            ble_gap_terminate(conn_handle, BLE_ERR_REM_USER_CONN_TERM);
+            continue;
+        }
+
+        return next_exp_in;
     }
-
-    ble_hs_unlock();
-
-    /* If a connection has timed out, terminate it.  We need to recursively
-     * call this function again to determine when the next timeout is.  This
-     * is a tail-recursive call, so it should be optimized to execute in the
-     * same stack frame.
-     */
-    if (conn_handle != BLE_HS_CONN_HANDLE_NONE) {
-        ble_gap_terminate(conn_handle, BLE_ERR_REM_USER_CONN_TERM);
-        return ble_hs_conn_timer();
-    }
-
-    return next_exp_in;
 }
 
 int
diff --git a/lib/libesp32/NimBLE-Arduino/src/nimble/host/store/config/src/ble_store_nvs.c b/lib/libesp32/NimBLE-Arduino/src/nimble/host/store/config/src/ble_store_nvs.c
index a6fea44c7..13dae8d2b 100644
--- a/lib/libesp32/NimBLE-Arduino/src/nimble/host/store/config/src/ble_store_nvs.c
+++ b/lib/libesp32/NimBLE-Arduino/src/nimble/host/store/config/src/ble_store_nvs.c
@@ -180,7 +180,9 @@ static int
 get_nvs_db_attribute(int obj_type, bool empty, void *value, int num_value)
 {
     union ble_store_value cur = {0};
+#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)
     struct ble_hs_dev_records p_dev_rec = {0};
+#endif
     esp_err_t err;
     int i, count = 0, max_limit = 0;
     char key_string[NIMBLE_NVS_STR_NAME_MAX_LEN];
@@ -190,11 +192,15 @@ get_nvs_db_attribute(int obj_type, bool empty, void *value, int num_value)
     for (i = 1; i <= max_limit; i++) {
         get_nvs_key_string(obj_type, i, key_string);
 
+#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)
         if (obj_type != BLE_STORE_OBJ_TYPE_PEER_DEV_REC) {
+#endif
             err = get_nvs_db_value(obj_type, key_string, &cur);
+#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)
         } else {
             err = get_nvs_peer_record(key_string, &p_dev_rec);
         }
+#endif
         /* Check if the user is searching for empty index to write to */
         if (err == ESP_ERR_NVS_NOT_FOUND) {
             if (empty) {
@@ -206,10 +212,13 @@ get_nvs_db_attribute(int obj_type, bool empty, void *value, int num_value)
             /* If user has provided value, then the purpose is to find
              * non-matching entry from NVS */
             if (value) {
+#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)
                 if (obj_type == BLE_STORE_OBJ_TYPE_PEER_DEV_REC) {
                     err = get_nvs_matching_index(&p_dev_rec, value, num_value,
                                                  sizeof(struct ble_hs_dev_records));
-                } else {
+                } else
+#endif
+                {
                     if (obj_type != BLE_STORE_OBJ_TYPE_CCCD) {
                         err = get_nvs_matching_index(&cur.sec, value, num_value,
                                                      sizeof(struct ble_store_value_sec));
@@ -376,7 +385,9 @@ populate_db_from_nvs(int obj_type, void *dst, int *db_num)
 {
     uint8_t *db_item = (uint8_t *)dst;
     union ble_store_value cur = {0};
+#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)
     struct ble_hs_dev_records p_dev_rec = {0};
+#endif
 
     esp_err_t err;
     int i;
@@ -385,8 +396,9 @@ populate_db_from_nvs(int obj_type, void *dst, int *db_num)
     for (i = 1; i <= get_nvs_max_obj_value(obj_type); i++) {
         get_nvs_key_string(obj_type, i, key_string);
 
+#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)
         if (obj_type != BLE_STORE_OBJ_TYPE_PEER_DEV_REC) {
-
+#endif
             err = get_nvs_db_value(obj_type, key_string, &cur);
             if (err == ESP_ERR_NVS_NOT_FOUND) {
                 continue;
@@ -394,6 +406,7 @@ populate_db_from_nvs(int obj_type, void *dst, int *db_num)
                 ESP_LOGE(TAG, "NVS read operation failed !!");
                 return -1;
             }
+#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)
         } else {
             err = get_nvs_peer_record(key_string, &p_dev_rec);
             if (err == ESP_ERR_NVS_NOT_FOUND) {
@@ -410,7 +423,9 @@ populate_db_from_nvs(int obj_type, void *dst, int *db_num)
             memcpy(db_item, &p_dev_rec, sizeof(struct ble_hs_dev_records));
             db_item += sizeof(struct ble_hs_dev_records);
             (*db_num)++;
-        } else {
+        } else
+#endif
+        {
             if (obj_type == BLE_STORE_OBJ_TYPE_CCCD) {
                 ESP_LOGD(TAG, "CCCD in RAM is filled up from NVS index = %d", i);
                 memcpy(db_item, &cur.cccd, sizeof(struct ble_store_value_cccd));
@@ -492,6 +507,11 @@ int ble_store_config_persist_cccds(void)
     union ble_store_value val;
 
     nvs_count = get_nvs_db_attribute(BLE_STORE_OBJ_TYPE_CCCD, 0, NULL, 0);
+    if (nvs_count == -1) {
+        ESP_LOGE(TAG, "NVS operation failed while persisting CCCD");
+        return BLE_HS_ESTORE_FAIL;
+    }
+
     if (nvs_count < ble_store_config_num_cccds) {
 
         /* NVS db count less than RAM count, write operation */
@@ -518,6 +538,11 @@ int ble_store_config_persist_peer_secs(void)
     union ble_store_value val;
 
     nvs_count = get_nvs_db_attribute(BLE_STORE_OBJ_TYPE_PEER_SEC, 0, NULL, 0);
+    if (nvs_count == -1) {
+        ESP_LOGE(TAG, "NVS operation failed while persisting peer sec");
+        return BLE_HS_ESTORE_FAIL;
+    }
+
     if (nvs_count < ble_store_config_num_peer_secs) {
 
         /* NVS db count less than RAM count, write operation */
@@ -544,6 +569,11 @@ int ble_store_config_persist_our_secs(void)
     union ble_store_value val;
 
     nvs_count = get_nvs_db_attribute(BLE_STORE_OBJ_TYPE_OUR_SEC, 0, NULL, 0);
+    if (nvs_count == -1) {
+        ESP_LOGE(TAG, "NVS operation failed while persisting our sec");
+        return BLE_HS_ESTORE_FAIL;
+    }
+
     if (nvs_count < ble_store_config_num_our_secs) {
 
         /* NVS db count less than RAM count, write operation */
@@ -573,7 +603,13 @@ int ble_store_persist_peer_records(void)
     struct ble_hs_dev_records *peer_dev_rec = ble_rpa_get_peer_dev_records();
 
     nvs_count = get_nvs_db_attribute(BLE_STORE_OBJ_TYPE_PEER_DEV_REC, 0, NULL, 0);
+    if (nvs_count == -1) {
+        ESP_LOGE(TAG, "NVS operation failed while persisting peer_dev_rec");
+        return BLE_HS_ESTORE_FAIL;
+    }
+
     if (nvs_count < ble_store_num_peer_dev_rec) {
+
         /* NVS db count less than RAM count, write operation */
         ESP_LOGD(TAG, "Persisting peer dev record to NVS...");
         peer_rec = peer_dev_rec[ble_store_num_peer_dev_rec - 1];
diff --git a/lib/libesp32/NimBLE-Arduino/src/nimconfig.h b/lib/libesp32/NimBLE-Arduino/src/nimconfig.h
index 2f10fa2fc..d90921fa1 100644
--- a/lib/libesp32/NimBLE-Arduino/src/nimconfig.h
+++ b/lib/libesp32/NimBLE-Arduino/src/nimconfig.h
@@ -15,8 +15,13 @@
  * This converts them to "CONFIG_BT_NIMBLE_" format used in the latest IDF.
  */
 
-/* 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)
+/* Detect if using ESP-IDF or Arduino (Arduino won't have these defines in sdkconfig) 
+ *
+ * Note: We do not use #ifdef CONFIG_BT_NIMBLE_ENABLED since we cannot enable NimBLE when using
+ * Arduino as a component and the esp-nimble-compnent, so we check if other config options are defined.
+ * We also need to use a config parameter that must be present and not likely defined in the command line.
+ */
+#if defined(CONFIG_BT_NIMBLE_GAP_DEVICE_NAME_MAX_LEN) || defined(CONFIG_NIMBLE_GAP_DEVICE_NAME_MAX_LEN)
 
 #if defined(CONFIG_NIMBLE_ENABLED) && !defined(CONFIG_BT_NIMBLE_ENABLED)
 #define CONFIG_BT_NIMBLE_ENABLED
@@ -51,22 +56,30 @@
 /** @brief Comment out if not using NimBLE Client functions \n
  *  Reduces flash size by approx. 7kB.
  */
+#ifndef CONFIG_BT_NIMBLE_ROLE_CENTRAL_DISABLED
 #define CONFIG_BT_NIMBLE_ROLE_CENTRAL
+#endif
 
 /** @brief Comment out if not using NimBLE Scan functions \n
  *  Reduces flash size by approx. 26kB.
  */
+#ifndef CONFIG_BT_NIMBLE_ROLE_OBSERVER_DISABLED
 #define CONFIG_BT_NIMBLE_ROLE_OBSERVER
+#endif
 
 /** @brief Comment out if not using NimBLE Server functions \n
  *  Reduces flash size by approx. 16kB.
  */
-// #define CONFIG_BT_NIMBLE_ROLE_PERIPHERAL
+#ifndef CONFIG_BT_NIMBLE_ROLE_PERIPHERAL_DISABLED
+#define CONFIG_BT_NIMBLE_ROLE_PERIPHERAL
+#endif
 
 /** @brief Comment out if not using NimBLE Advertising functions \n
  *  Reduces flash size by approx. 5kB.
  */
-// #define CONFIG_BT_NIMBLE_ROLE_BROADCASTER
+#ifndef CONFIG_BT_NIMBLE_ROLE_BROADCASTER_DISABLED
+#define CONFIG_BT_NIMBLE_ROLE_BROADCASTER
+#endif
 
 /*  Uncomment to see debug log messages from the NimBLE host
  *  Uses approx. 32kB of flash memory.
@@ -89,29 +102,46 @@
 // #define CONFIG_NIMBLE_CPP_ENABLE_ADVERTISMENT_TYPE_TEXT
 
 /** @brief Sets the core NimBLE host runs on */
+#ifndef CONFIG_BT_NIMBLE_PINNED_TO_CORE
 #define CONFIG_BT_NIMBLE_PINNED_TO_CORE 0
+#endif
 
 /** @brief Sets the stack size for the NimBLE host task */
+#ifndef CONFIG_BT_NIMBLE_TASK_STACK_SIZE
 #define CONFIG_BT_NIMBLE_TASK_STACK_SIZE 4096
+#endif
 
 /**
  * @brief Sets the memory pool where NimBLE will be loaded
  * @details By default NimBLE is loaded in internal ram.\n
  * To use external PSRAM you must change this to `#define CONFIG_BT_NIMBLE_MEM_ALLOC_MODE_EXTERNAL 1`
  */
+#ifndef CONFIG_BT_NIMBLE_MEM_ALLOC_MODE_EXTERNAL
 #define CONFIG_BT_NIMBLE_MEM_ALLOC_MODE_INTERNAL 1
+#endif
 
 /** @brief Sets the number of simultaneous connections (esp controller max is 9) */
+#ifndef CONFIG_BT_NIMBLE_MAX_CONNECTIONS
 #define CONFIG_BT_NIMBLE_MAX_CONNECTIONS 3
+#endif
 
 /** @brief Sets the number of devices allowed to store/bond with */
+#ifndef CONFIG_BT_NIMBLE_MAX_BONDS
 #define CONFIG_BT_NIMBLE_MAX_BONDS 3
+#endif
 
 /** @brief Sets the maximum number of CCCD subscriptions to store */
+#ifndef CONFIG_BT_NIMBLE_MAX_CCCDS
 #define CONFIG_BT_NIMBLE_MAX_CCCDS 8
+#endif
+
+/** @brief Default device name */
+#ifndef CONFIG_BT_NIMBLE_SVC_GAP_DEVICE_NAME
+#define CONFIG_BT_NIMBLE_SVC_GAP_DEVICE_NAME "nimble"
+#endif
 
 /** @brief Set if CCCD's and bond data should be stored in NVS */
-#define CONFIG_BT_NIMBLE_NVS_PERSIST 0
+#define CONFIG_BT_NIMBLE_NVS_PERSIST 1
 
 /** @brief Allow legacy paring */
 #define CONFIG_BT_NIMBLE_SM_LEGACY 1
@@ -119,9 +149,6 @@
 /** @brief Allow BLE secure connections */
 #define CONFIG_BT_NIMBLE_SM_SC 1
 
-/** @brief Default device name */
-#define CONFIG_BT_NIMBLE_SVC_GAP_DEVICE_NAME "nimble"
-
 /** @brief Max device name length (bytes) */
 #define CONFIG_BT_NIMBLE_GAP_DEVICE_NAME_MAX_LEN 31
 
@@ -154,7 +181,6 @@
  */
 #define CONFIG_BT_NIMBLE_MSYS1_BLOCK_COUNT 12
 
-
 /** @brief Random address refresh time in seconds */
 #define CONFIG_BT_NIMBLE_RPA_TIMEOUT 900