Release 2.5.0

- Add NimBLEL2CAPChannel::disconnect() and getConnHandle().
- Fix CoC TX mbuf ownership handling in writeFragment():
  treat BLE_HS_ENOMEM / BLE_HS_EAGAIN as consumed buffer,
  only free local tx mbuf on BLE_HS_EBUSY.
- Refresh L2CAP client/server examples for stress testing and
  runtime stats.
- Pin example dependency mickeyl/esp-hpl to tag 1.1.0.
- Clean trailing whitespace in updated example sources.

Closes #391

.github/workflows/build.yml

@@ -12,13 +12,14 @@ jobs:
     name: Build with ESP-IDF ${{ matrix.idf_ver }} for ${{ matrix.idf_target }}
     runs-on: ubuntu-latest
     strategy:
+      fail-fast: false
       matrix:
         # The version names here correspond to the versions of espressif/idf Docker image.
         # See https://hub.docker.com/r/espressif/idf/tags and
         # https://docs.espressif.com/projects/esp-idf/en/latest/esp32/api-guides/tools/idf-docker-image.html
         # for details.
-        idf_ver: ["release-v4.4", "release-v5.4", "release-v5.5"]
-        idf_target: ["esp32", "esp32s3", "esp32c2", "esp32c3", "esp32c5", "esp32c6", "esp32h2", "esp32p4"]
+        idf_ver: ["release-v5.4", "release-v5.5"]
+        idf_target: ["esp32", "esp32s3", "esp32c2", "esp32c3", "esp32c5", "esp32c6", "esp32c61", "esp32h2", "esp32p4"]
         example:
           - NimBLE_Client
           - NimBLE_Server
@@ -35,6 +36,10 @@ jobs:
             idf_target: "esp32c5"
           - idf_ver: release-v4.4
             idf_target: "esp32c6"
+          - idf_ver: release-v4.4
+            idf_target: "esp32c61"
+          - idf_ver: release-v5.4
+            idf_target: "esp32c61"
           - idf_ver: release-v4.4
             idf_target: "esp32h2"
           - idf_ver: release-v4.4
@@ -45,7 +50,7 @@ jobs:
     container: espressif/idf:${{ matrix.idf_ver }}
     steps:
       - name: Checkout
-        uses: actions/checkout@v4
+        uses: actions/checkout@v5
         with:
           path: components/esp-nimble-cpp
       - name: Build examples
@@ -60,7 +65,7 @@ jobs:
   build_docs:
     runs-on: ubuntu-latest
     steps:
-      - uses: actions/checkout@v4
+      - uses: actions/checkout@v5
       - name: Doxygen Action
         uses: mattnotmitt/doxygen-action@v1.9.8
         with:

.github/workflows/release.yml

@@ -8,7 +8,7 @@ jobs:
   build_docs:
     runs-on: ubuntu-latest
     steps:
-      - uses: actions/checkout@v4
+      - uses: actions/checkout@v5
       - name: Doxygen Action
         uses: mattnotmitt/doxygen-action@v1.9.8
         with:
@@ -17,4 +17,27 @@ jobs:
         uses: peaceiris/actions-gh-pages@v4
         with:
           github_token: ${{ secrets.GITHUB_TOKEN }}
-          publish_dir: ./docs/doxydocs/html
+          publish_dir: ./docs/doxydocs/html
+
+  upload_to_component_registry:
+    permissions:
+      id-token: write
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v5
+        with:
+          submodules: "recursive"
+      - name: Upload component to the component registry
+        uses: espressif/upload-components-ci-action@v2
+        with:
+          components: "esp-nimble-cpp: ."
+          namespace: "h2zero"
+
+  upload_to_platformio_registry:
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v5
+      - name: Upload component to PlatformIO Registry
+        uses: bojit/platformio-publish@main
+        with:
+          token: ${{ secrets.PLATFORMIO_AUTH_TOKEN }}

.gitignore

@@ -1 +1,4 @@
 docs/doxydocs
+dist
+.development
+_codeql_detected_source_root

CHANGELOG.md

@@ -1,6 +1,75 @@
 # Changelog
 All notable changes to this project will be documented in this file.
 
+## [2.5.0] 2026-04-01
+
+## Fixed
+- `NimBLEClient` connection state tracking.
+- Calling disconnect will no longer return false if the HCI response is "Unknown ID".
+- Remote descriptors not found when characteristic vector handles out of order.
+- `setValue` with char inputs now calculates the data length correctly.
+
+## Added
+- `NimBLEServer::sendServiceChangedIndication` Sends the service changed indication to peers so they refresh their database.
+- `NimBLEScan` user configuarable scan response timer added to prevent unreported devices on long duration scans.
+- `NimBLEClient` Connection retry on connection establishment failure, retry count configurable by app, default 2.
+- ANCS Example
+- `l2Cap` Disconnect API
+
+
+## [2.4.0] 2026-03-20
+
+## Fixed
+- GATT attribute handles are now assigned from the registration callback so duplicate UUID attributes are identified correctly.
+- Dynamic service changes now properly remove characteristics/descriptors and reset the GATT database when advertising starts.
+- Missing notification/indication payload data when the value spans multiple mbufs, such as values larger than 255 bytes with small ACL buffers.
+- `NimBLEDevice::createServer` will longer crash when called before the stack is initialized.
+- Re-pairing after deleting all bonds now works by unpairing each stored bond instead of only deleting NVS data.
+- Whitelist bounds checks.
+- `NimBLEDevice::getBondedAddress` index bounds validation.
+- Compiler warnings when bonds are disabled.
+- kconfig warnings, redefined macros.
+
+## Added
+- `NimBLEStream`, `NimBLEStreamClient`, and `NimBLEStreamServer` classes and examples.
+- `NimBLECppVersion.h` with compile-time version macros.
+- `NimBLEDevice::getVersion` runtime version string helper.
+- Matching passkey callbacks for both roles: `NimBLEServerCallbacks::onPassKeyEntry` and `NimBLEClientCallbacks::onPassKeyDisplay`.
+- Bond migration helpers to convert bond storage between v1 and current formats while preserving existing bonds.
+- `NimBLEUUID` constructor overload for `ble_uuid_t*`.
+- Optional `index` parameter for `NimBLECharacteristic::getDescriptorByUUID` to access multiple descriptors with the same UUID.
+- `NimBLEConnInfo::toString` method to get a string representation of the connection info.
+
+## Changed
+- `NimBLEService::start` is deprecated; services are now added when the server starts.
+- `NimBLEHIDDevice::startServices()` is deprecated; services are now added when the server starts.
+
+## [2.3.4] 2025-12-27
+
+## Fixed
+- Disconnect event not finding the client by address in some cases, use the connection handle instead.
+- Notification/Indications will now be sent to server created clients.
+- Attribute values will now consider type size when using a container.
+- Descriptor searching will now correctly stop at the end handle of the characteristic.
+
+## Changed
+- Build conditionals now use `MYNEWT_VAL_` macros instead of `CONFIG_` macros where applicable.
+- Sending of indications/notifications has been refactored for greater efficiency and tracks client subscription state locally instead uof relying on the stack.
+
+## Added
+- Support for esp32c61.
+- `NimBLECharacteristicCallbacks::onStatus` overload that takes a `NimBLEConnInfo&` parameter to indicate which client the status is for.
+- `NimBLEDevice::setCustomGapHandler` now takes a `void*` parameter to allow for passing user data to the custom handler.
+
+## [2.3.3] 2025-09-05
+
+## Fixed
+- NimBLEAdvertisedDevice::isConnectable returning incorrect result.
+- Extended advertisements not reporting full data.
+
+## Added
+- Support up to 1650 bytes of advertisement with extended advertising
+
 ## [2.3.2] 2025-09-02
 
 ## Fixed

CMakeLists.txt

@@ -37,6 +37,7 @@ idf_component_register(
     "esp32c3"
     "esp32c5"
     "esp32c6"
+    "esp32c61"
     "esp32h2"
     "esp32p4"
   INCLUDE_DIRS
@@ -65,6 +66,7 @@ idf_component_register(
     "src/NimBLEScan.cpp"
     "src/NimBLEServer.cpp"
     "src/NimBLEService.cpp"
+    "src/NimBLEStream.cpp"
     "src/NimBLEUtils.cpp"
     "src/NimBLEUUID.cpp"
   REQUIRES

Kconfig

@@ -204,55 +204,4 @@ config NIMBLE_CPP_FREERTOS_TASK_BLOCK_BIT
         Configure the bit to set in the task notification value when a task is blocked waiting for an event.
         This should be set to a bit that is not used by other notifications in the system.
 
-config NIMBLE_CPP_IDF
-    bool
-    default BT_NIMBLE_ENABLED
-
-#
-# BT config
-#
-config BT_ENABLED
-    bool "Bluetooth"
-    default "y"
-    help
-        Select this option to enable Bluetooth and show the submenu with Bluetooth configuration choices.
-
-
-config BT_NIMBLE_ENABLED
-    bool "NimBLE - BLE only"
-    default "y"
-    help
-        This option is recommended for BLE only usecases to save on memory
-
-if IDF_TARGET_ESP32P4
-
-    config BT_NIMBLE_TRANSPORT_UART
-        bool "Enable Uart Transport"
-        default "n"
-
-    #
-    # Enable ESP Hosted BT
-    # Used as VHCI transport between BT Host and Controller
-    #
-    config ESP_ENABLE_BT
-        bool "Enable Hosted Bluetooth support"
-        default "y"
-        help
-            Enable Bluetooth Support via Hosted
-
-    choice ESP_WIFI_REMOTE_LIBRARY
-        prompt "Choose WiFi-remote implementation"
-        default ESP_WIFI_REMOTE_LIBRARY_HOSTED
-        help
-            Select type of WiFi Remote implementation
-
-            ESP-HOSTED is the default and most versatile option.
-            It's also possible to use EPPP, which uses PPPoS link between micros and NAPT, so it's slower
-            and less universal.
-
-        config ESP_WIFI_REMOTE_LIBRARY_HOSTED
-            bool "ESP-HOSTED"
-    endchoice
-endif
-
 endmenu

README.md

@@ -9,6 +9,8 @@
 
 NimBLE CPP library for use with ESP32 that attempts to maintain compatibility with the [nkolban cpp_utils BLE API](https://github.com/nkolban/esp32-snippets/tree/master/cpp_utils).
 
+The library is threadsafe, characteristics can be set from any thread.
+
 **An Arduino version of this library, including NimBLE, can be [found here.](https://github.com/h2zero/NimBLE-Arduino)**
 
 This library **significantly** reduces resource usage and improves performance for ESP32 BLE applications as compared    

docs/1.x_to2.x_migration_guide.md

@@ -55,7 +55,7 @@ The default address type is public `0`, whereas many devices use a random `1` ad
 If you experience this issue please create your address instances with the address type specified, i.e. `NimBLEAddress address("11:22:33:44:55:66", TYPE_HERE)`
 
 `NimBLEAddress::getNative` has been removed and replaced with `NimBLEAddress::getBase`.
-This returns a pointer to `const ble_addr_t` instead of a pointer to the address value that `getNative` did. The value can be accessed through this struct via `ble_addr_t.value` and type is in `ble_addr_t.type`.  
+This returns a pointer to `const ble_addr_t` instead of a pointer to the address value that `getNative` did. The value can be accessed through this struct via `ble_addr_t.val` and type is in `ble_addr_t.type`.  
 <br/>
 
 ## BLE UUID's

docs/Doxyfile

@@ -48,7 +48,7 @@ PROJECT_NAME           = esp-nimble-cpp
 # could be handy for archiving the generated documentation or if some version
 # control system is used.
 
-PROJECT_NUMBER         = 2.3.2
+PROJECT_NUMBER         = 2.5.0
 # Using the PROJECT_BRIEF tag one can provide an optional one line description
 # for a project that appears at the top of each page and should give viewer a
 # quick idea about the purpose of the project. Keep the description short.

docs/Usage_tips.md

@@ -1,11 +1,8 @@
 # Usage Tips
 
-## Put BLE functions in a task running on the NimBLE stack core
+## Threadsafety
 
-When commands are sent to the stack from a different core they can experience delays in execution.  
-This library detects this and invokes the esp32 IPC to reroute these commands through the correct core but this also increases overhead.  
-Therefore it is highly recommended to create tasks for BLE to run on the same core, the macro `CONFIG_BT_NIMBLE_PINNED_TO_CORE` can be used to set the core.
-<br/>  
+This library is threadsafe. Attribues can be manipulated freely.
 
 ## Do not delete client instances unless necessary or unused
 
@@ -32,6 +29,41 @@ or nullptr such as when  calling `NimBLEClient::getService`. The latter being a
 Most of the functions in this library return something that should be checked before proceeding.  
 <br/>  
 
+## Persisted bonds can be lost due to low MAX_CCCDS
+
+The symptom: CONFIG_BT_NIMBLE_MAX_BONDS is set to N, but a smaller number of bonds is preserved, perhaps even a single bond. The limitation of persisted bonds can be observed via NimBLEDevice::getNumBonds(). The number may not reach CONFIG_BT_NIMBLE_MAX_BONDS.
+
+Cause: For each bond, NimBLE persists each of the CCCD (client characteristic configuration descriptor) values that have been subscribed
+to by the client. If CONFIG_BT_NIMBLE_MAX_CCCDS is too low, the older CCCD values are overwritten by the newer ones. The loss of the older
+CCCDs values results in those bonds being lost.
+
+Fix: Increase CONFIG_BT_NIMBLE_MAX_CCCDS. These take approximately 40 bytes in NVS, 2 bytes for the CCCD value and the NVS metadata overhead. The value of
+CONFIG_BT_NIMBLE_MAX_CCCDS should conservatively be no less than (CONFIG_BT_NIMBLE_MAX_BONDS * {maximum number of characteristics that can be subscribed to}).
+
+## Device 'Local Name'
+
+'Local name' refers to how the device is seen and displayed.
+
+A devices 'Local name' can be thought of as coming from two places, the <i>Advertising "Local name"</i> and the <i>the GATT Device Name</i>.
+
+### Advertising "Local name"
+
+Field found in the advertising data payload. Value is set via NimBLEAdvertising::setName().
+
+### GATT Device Name
+
+Characteristic UUID 0x2A00 in the Generic Access service. Set via NimBLEDevice::init() or NimBLEDevice::setDeviceName().
+
+This characteristic is read <b>after</b> connecting to the device.
+
+### Important considerations
+
+* OSes cache the <i>'GATT Device Name'</i>.
+* OSes update the device name based on the <i>'GATT Device Name'</i> after connecting to a device. This means that if you set the <i>Advertising 'Local name'</i> to "ABCD" but the <i>'GATT Device Name'</i> to "12345", the device will be seen as "ABCD" until connecting to the device, at which time the devices name will change to "12345".
+* If no <i>'Advertising "Local name"'</i> is set, OSes, such as iOS, may display the devices name as 'Unnamed' until the device is connected to, at which time the <i>'GATT Device Name'</i> is read and used instead.
+
+It is recommended that both <i>'Advertising "Local name"'</i> <b>and</b> <i>'GATT Device Name'</i> be set appropriately, after considering the above described behavior.
+
 ## There will be bugs - please report them
 
 No code is bug free and unit testing will not find them all on it's own. If you encounter a bug, please report it along with any logs and decoded backtrace if applicable.  

examples/ANCS/CMakeLists.txt

@@ -0,0 +1,6 @@
+# The following lines of boilerplate have to be in your project's
+# CMakeLists in this exact order for cmake to work correctly
+cmake_minimum_required(VERSION 3.5)
+
+include($ENV{IDF_PATH}/tools/cmake/project.cmake)
+project(ANCS)

examples/ANCS/main/CMakeLists.txt

@@ -0,0 +1,4 @@
+set(COMPONENT_SRCS "main.cpp")
+set(COMPONENT_ADD_INCLUDEDIRS ".")
+
+register_component()

examples/ANCS/main/main.cpp

@@ -0,0 +1,253 @@
+// Original: https://github.com/mathcampbell/ANCS
+#include "NimBLEDevice.h"
+#include "driver/uart.h"
+
+static NimBLEUUID ancsServiceUUID("7905F431-B5CE-4E99-A40F-4B1E122D00D0");
+static NimBLEUUID notificationSourceCharacteristicUUID("9FBF120D-6301-42D9-8C58-25E699A21DBD");
+static NimBLEUUID controlPointCharacteristicUUID("69D1D8F3-45E1-49A8-9821-9BBDFDAAD9D9");
+static NimBLEUUID dataSourceCharacteristicUUID("22EAC6E9-24D6-4BB5-BE44-B36ACE7C7BFB");
+
+static NimBLEClient *pClient;
+
+uint8_t latestMessageID[4];
+bool pendingNotification = false;
+bool incomingCall = false;
+uint8_t acceptCall = 0;
+
+static void initUart()
+{
+    uart_config_t uartConfig{};
+    uartConfig.baud_rate = 115200;
+    uartConfig.data_bits = UART_DATA_8_BITS;
+    uartConfig.parity = UART_PARITY_DISABLE;
+    uartConfig.stop_bits = UART_STOP_BITS_1;
+    uartConfig.flow_ctrl = UART_HW_FLOWCTRL_DISABLE;
+    uartConfig.source_clk = UART_SCLK_DEFAULT;
+
+    uart_driver_install(UART_NUM_0, 256, 0, 0, nullptr, 0);
+    uart_param_config(UART_NUM_0, &uartConfig);
+}
+
+static void dataSourceNotifyCallback(NimBLERemoteCharacteristic *pDataSourceCharacteristic,
+                                     uint8_t *pData,
+                                     size_t length,
+                                     bool isNotify)
+{
+    for (int i = 0; i < length; i++)
+    {
+        if (i > 7)
+        {
+            printf("%c", pData[i]);
+        }
+        else
+        {
+            printf("%02X ", pData[i]);
+        }
+    }
+    printf("\n");
+}
+
+static void NotificationSourceNotifyCallback(NimBLERemoteCharacteristic *pNotificationSourceCharacteristic,
+                                             uint8_t *pData,
+                                             size_t length,
+                                             bool isNotify)
+{
+    if (pData[0] == 0)
+    {
+        printf("New notification!\n");
+        latestMessageID[0] = pData[4];
+        latestMessageID[1] = pData[5];
+        latestMessageID[2] = pData[6];
+        latestMessageID[3] = pData[7];
+
+        switch (pData[2])
+        {
+        case 0:
+            printf("Category: Other\n");
+            break;
+        case 1:
+            incomingCall = true;
+            printf("Category: Incoming call\n");
+            break;
+        case 2:
+            printf("Category: Missed call\n");
+            break;
+        case 3:
+            printf("Category: Voicemail\n");
+            break;
+        case 4:
+            printf("Category: Social\n");
+            break;
+        case 5:
+            printf("Category: Schedule\n");
+            break;
+        case 6:
+            printf("Category: Email\n");
+            break;
+        case 7:
+            printf("Category: News\n");
+            break;
+        case 8:
+            printf("Category: Health\n");
+            break;
+        case 9:
+            printf("Category: Business\n");
+            break;
+        case 10:
+            printf("Category: Location\n");
+            break;
+        case 11:
+            printf("Category: Entertainment\n");
+            break;
+        default:
+            break;
+        }
+    }
+    else if (pData[0] == 1)
+    {
+        printf("Notification Modified!\n");
+        if (pData[2] == 1)
+        {
+            printf("Call Changed!\n");
+        }
+    }
+    else if (pData[0] == 2)
+    {
+        printf("Notification Removed!\n");
+        if (pData[2] == 1)
+        {
+            printf("Call Gone!\n");
+        }
+    }
+    pendingNotification = true;
+}
+
+class ServerCallbacks : public NimBLEServerCallbacks
+{
+    void onConnect(NimBLEServer *pServer, NimBLEConnInfo &connInfo)
+    {
+        printf("Client connected: %s\n", connInfo.getAddress().toString().c_str());
+        pClient = pServer->getClient(connInfo);
+        printf("Client connected!\n");
+    }
+
+    void onDisconnect(NimBLEServer *pServer, NimBLEConnInfo &connInfo, int reason)
+    {
+        printf("Client disconnected: %s, reason: %d\n", connInfo.getAddress().toString().c_str(), reason);
+    }
+} serverCallbacks;
+
+extern "C" void app_main()
+{
+    initUart();
+    printf("Starting setup...\n");
+
+    NimBLEDevice::init("ANCS");
+    NimBLEDevice::setSecurityAuth(true, true, true);
+    NimBLEDevice::setSecurityIOCap(BLE_HS_IO_DISPLAY_YESNO);
+    NimBLEDevice::setPower(9);
+
+    NimBLEServer *pServer = NimBLEDevice::createServer();
+    pServer->setCallbacks(&serverCallbacks);
+    pServer->advertiseOnDisconnect(true);
+
+    NimBLEAdvertising *pAdvertising = pServer->getAdvertising();
+    NimBLEAdvertisementData advData{};
+    advData.setFlags(0x06);
+    advData.addServiceUUID(ancsServiceUUID);
+    pAdvertising->setAdvertisementData(advData);
+    pAdvertising->start();
+
+    printf("Advertising started!\n");
+
+    while (1)
+    {
+        if (pClient != nullptr && pClient->isConnected())
+        {
+            auto pAncsService = pClient->getService(ancsServiceUUID);
+            if (pAncsService == nullptr)
+            {
+                printf("Failed to find our service UUID: %s\n", ancsServiceUUID.toString().c_str());
+                continue;
+            }
+            // Obtain a reference to the characteristic in the service of the remote BLE server.
+            auto pNotificationSourceCharacteristic = pAncsService->getCharacteristic(notificationSourceCharacteristicUUID);
+            if (pNotificationSourceCharacteristic == nullptr)
+            {
+                printf("Failed to find our characteristic UUID: %s\n",
+                       notificationSourceCharacteristicUUID.toString().c_str());
+                continue;
+            }
+            // Obtain a reference to the characteristic in the service of the remote BLE server.
+            auto pControlPointCharacteristic = pAncsService->getCharacteristic(controlPointCharacteristicUUID);
+            if (pControlPointCharacteristic == nullptr)
+            {
+                printf("Failed to find our characteristic UUID: %s\n",
+                       controlPointCharacteristicUUID.toString().c_str());
+                continue;
+            }
+            // Obtain a reference to the characteristic in the service of the remote BLE server.
+            auto pDataSourceCharacteristic = pAncsService->getCharacteristic(dataSourceCharacteristicUUID);
+            if (pDataSourceCharacteristic == nullptr)
+            {
+                printf("Failed to find our characteristic UUID: %s\n", dataSourceCharacteristicUUID.toString().c_str());
+                continue;
+            }
+            pDataSourceCharacteristic->subscribe(true, dataSourceNotifyCallback);
+            pNotificationSourceCharacteristic->subscribe(true, NotificationSourceNotifyCallback);
+
+            while (1)
+            {
+                if (pendingNotification || incomingCall)
+                {
+                    // CommandID: CommandIDGetNotificationAttributes
+                    // 32bit uid
+                    // AttributeID
+                    printf("Requesting details...\n");
+                    uint8_t val[8] =
+                        {0x0, latestMessageID[0], latestMessageID[1], latestMessageID[2], latestMessageID[3], 0x0, 0x0, 0x10};
+                    pControlPointCharacteristic->writeValue(val, 6, true); // Identifier
+                    val[5] = 0x1;
+                    pControlPointCharacteristic->writeValue(val, 8, true); // Title
+                    val[5] = 0x3;
+                    pControlPointCharacteristic->writeValue(val, 8, true); // Message
+                    val[5] = 0x5;
+                    pControlPointCharacteristic->writeValue(val, 6, true); // Date
+
+                    while (incomingCall)
+                    {
+                        int bytesRead = uart_read_bytes(UART_NUM_0, &acceptCall, 1, 0);
+                        if (bytesRead > 0)
+                        {
+                            printf("%c\n", (char)acceptCall);
+                        }
+
+                        if (acceptCall == 49)
+                        { // call accepted , get number 1 from serial
+                            const uint8_t vResponse[] =
+                                {0x02, latestMessageID[0], latestMessageID[1], latestMessageID[2], latestMessageID[3], 0x00};
+                            pControlPointCharacteristic->writeValue((uint8_t *)vResponse, 6, true);
+
+                            acceptCall = 0;
+                            // incomingCall = false;
+                        }
+                        else if (acceptCall == 48)
+                        { // call rejected , get number 0 from serial
+                            const uint8_t vResponse[] =
+                                {0x02, latestMessageID[0], latestMessageID[1], latestMessageID[2], latestMessageID[3], 0x01};
+                            pControlPointCharacteristic->writeValue((uint8_t *)vResponse, 6, true);
+
+                            acceptCall = 0;
+                            incomingCall = false;
+                        }
+                        vTaskDelay(10 / portTICK_PERIOD_MS);
+                    }
+
+                    pendingNotification = false;
+                }
+                vTaskDelay(10 / portTICK_PERIOD_MS);
+            }
+        }
+        vTaskDelay(10 / portTICK_PERIOD_MS);
+    }
+}

examples/ANCS/sdkconfig.defaults

@@ -0,0 +1,13 @@
+# Override some defaults so BT stack is enabled
+# in this example
+
+#
+# BT config
+#
+CONFIG_BT_ENABLED=y
+CONFIG_BTDM_CTRL_MODE_BLE_ONLY=y
+CONFIG_BTDM_CTRL_MODE_BR_EDR_ONLY=n
+CONFIG_BTDM_CTRL_MODE_BTDM=n
+CONFIG_BT_BLUEDROID_ENABLED=n
+CONFIG_BT_NIMBLE_ENABLED=y
+CONFIG_BT_NIMBLE_NVS_PERSIST=y

examples/Bluetooth_5/NimBLE_extended_server/main/main.cpp

@@ -36,7 +36,7 @@ static uint8_t secondaryPhy = BLE_HCI_LE_PHY_1M;
 /** Handler class for server events */
 class ServerCallbacks : public NimBLEServerCallbacks {
     void onConnect(NimBLEServer* pServer, NimBLEConnInfo& connInfo) override {
-        printf("Client connected:: %s\n", connInfo.getAddress().toString().c_str());
+        printf("Client connected:\n%s", connInfo.toString().c_str());
     }
 
     void onDisconnect(NimBLEServer* pServer, NimBLEConnInfo& connInfo, int reason) override {
@@ -80,9 +80,6 @@ extern "C" void app_main(void) {
 
     pCharacteristic->setValue("Hello World");
 
-    /** Start the service */
-    pService->start();
-
     /**
      *  Create an extended advertisement with the instance ID 0 and set the PHY's.
      *  Multiple instances can be added as long as the instance ID is incremented.

examples/Bluetooth_5/NimBLE_multi_advertiser/main/main.cpp

@@ -36,7 +36,7 @@ static uint8_t secondaryPhy = BLE_HCI_LE_PHY_1M;
 /** Handler class for server events */
 class ServerCallbacks : public NimBLEServerCallbacks {
     void onConnect(NimBLEServer* pServer, NimBLEConnInfo& connInfo) override {
-        printf("Client connected: %s\n", connInfo.getAddress().toString().c_str());
+        printf("Client connected:\n%s", connInfo.toString().c_str());
     }
 
     void onDisconnect(NimBLEServer* pServer, NimBLEConnInfo& connInfo, int reason) override {
@@ -98,9 +98,6 @@ extern "C" void app_main(void) {
 
     pCharacteristic->setValue("Hello World");
 
-    /** Start the service */
-    pService->start();
-
     /** Create our multi advertising instances */
 
     /** extended scannable instance advertising on coded and 1m PHY's. */

examples/L2CAP/L2CAP_Client/main/idf_component.yml

@@ -1,3 +1,6 @@
 dependencies:
   local/esp-nimble-cpp:
     path: ../../../../../esp-nimble-cpp/
+  mickeyl/esp-hpl:
+    git: https://github.com/mickeyl/esp-hpl.git
+    version: "1.1.0"

examples/L2CAP/L2CAP_Client/main/main.cpp

@@ -1,15 +1,12 @@
 #include <NimBLEDevice.h>
+#include <esp_hpl.hpp>
+#include <esp_timer.h>
 
-// See the following for generating UUIDs:
-// https://www.uuidgenerator.net/
-
-// The remote service we wish to connect to.
-static BLEUUID serviceUUID("dcbc7255-1e9e-49a0-a360-b0430b6c6905");
-// The characteristic of the remote service we are interested in.
-static BLEUUID    charUUID("371a55c8-f251-4ad2-90b3-c7c195b049be");
-
-#define L2CAP_CHANNEL        150
+#define L2CAP_PSM            192
 #define L2CAP_MTU            5000
+#define INITIAL_PAYLOAD_SIZE 64
+#define BLOCKS_BEFORE_DOUBLE 50
+#define MAX_PAYLOAD_SIZE     4900
 
 const BLEAdvertisedDevice* theDevice = NULL;
 BLEClient* theClient = NULL;
@@ -17,6 +14,15 @@ BLEL2CAPChannel* theChannel = NULL;
 
 size_t bytesSent = 0;
 size_t bytesReceived = 0;
+size_t currentPayloadSize = INITIAL_PAYLOAD_SIZE;
+uint32_t blocksSent = 0;
+uint64_t startTime = 0;
+
+// Heap monitoring
+size_t initialHeap = 0;
+size_t lastHeap = 0;
+size_t heapDecreaseCount = 0;
+const size_t HEAP_LEAK_THRESHOLD = 10;  // Warn after 10 consecutive decreases
 
 class L2CAPChannelCallbacks: public BLEL2CAPChannelCallbacks {
 
@@ -43,7 +49,7 @@ class MyClientCallbacks: public BLEClientCallbacks {
         printf("GAP connected\n");
         pClient->setDataLen(251);
 
-        theChannel = BLEL2CAPChannel::connect(pClient, L2CAP_CHANNEL, L2CAP_MTU, new L2CAPChannelCallbacks());
+        theChannel = BLEL2CAPChannel::connect(pClient, L2CAP_PSM, L2CAP_MTU, new L2CAPChannelCallbacks());
     }
 
     void onDisconnect(BLEClient* pClient, int reason) {
@@ -61,23 +67,72 @@ class MyAdvertisedDeviceCallbacks: public BLEAdvertisedDeviceCallbacks {
         if (theDevice) { return; }
         printf("BLE Advertised Device found: %s\n", advertisedDevice->toString().c_str());
 
-        if (!advertisedDevice->haveServiceUUID()) { return; }
-        if (!advertisedDevice->isAdvertisingService(serviceUUID)) { return; }
-
-        printf("Found the device we're interested in!\n");
-        BLEDevice::getScan()->stop();
-
-        // Hand over the device to the other task
-        theDevice = advertisedDevice;
+        // Look for device named "l2cap"
+        if (advertisedDevice->haveName() && advertisedDevice->getName() == "l2cap") {
+            printf("Found l2cap device!\n");
+            BLEDevice::getScan()->stop();
+            theDevice = advertisedDevice;
+        }
     }
 };
 
+void statusTask(void *pvParameters) {
+    while (true) {
+        vTaskDelay(1000 / portTICK_PERIOD_MS);
+
+        if (startTime > 0 && blocksSent > 0) {
+            uint64_t currentTime = esp_timer_get_time();
+            double elapsedSeconds = (currentTime - startTime) / 1000000.0;
+            double bytesPerSecond = 0.0;
+            double kbPerSecond = 0.0;
+            if (elapsedSeconds > 0.0) {
+                bytesPerSecond = bytesSent / elapsedSeconds;
+                kbPerSecond = bytesPerSecond / 1024.0;
+            }
+
+            // Heap monitoring
+            size_t currentHeap = esp_get_free_heap_size();
+            size_t minHeap = esp_get_minimum_free_heap_size();
+
+            // Track heap for leak detection
+            if (initialHeap == 0) {
+                initialHeap = currentHeap;
+                lastHeap = currentHeap;
+            }
+
+            // Check for consistent heap decrease
+            if (currentHeap < lastHeap) {
+                heapDecreaseCount++;
+                if (heapDecreaseCount >= HEAP_LEAK_THRESHOLD) {
+                    printf("\n⚠️  WARNING: POSSIBLE MEMORY LEAK DETECTED! ⚠️\n");
+                    printf("Heap has decreased %zu times in a row\n", heapDecreaseCount);
+                    printf("Initial heap: %zu, Current heap: %zu, Lost: %zu bytes\n",
+                           initialHeap, currentHeap, initialHeap - currentHeap);
+                }
+            } else if (currentHeap >= lastHeap) {
+                heapDecreaseCount = 0;  // Reset counter if heap stabilizes or increases
+            }
+            lastHeap = currentHeap;
+
+            printf("\n=== STATUS UPDATE ===\n");
+            printf("Blocks sent: %lu\n", (unsigned long)blocksSent);
+            printf("Total bytes sent: %zu\n", bytesSent);
+            printf("Current payload size: %zu bytes\n", currentPayloadSize);
+            printf("Elapsed time: %.1f seconds\n", elapsedSeconds);
+            printf("Bandwidth: %.2f KB/s (%.2f Mbps)\n", kbPerSecond, (bytesPerSecond * 8) / 1000000.0);
+            printf("Heap: %zu free (min: %zu), Used since start: %zu\n",
+                   currentHeap, minHeap, initialHeap > 0 ? initialHeap - currentHeap : 0);
+            printf("==================\n\n");
+        }
+    }
+}
+
 void connectTask(void *pvParameters) {
 
     uint8_t sequenceNumber = 0;
 
     while (true) {
-        
+
         if (!theDevice) {
             vTaskDelay(1000 / portTICK_PERIOD_MS);
             continue;
@@ -96,7 +151,7 @@ void connectTask(void *pvParameters) {
                 break;
             }
             vTaskDelay(2000 / portTICK_PERIOD_MS);
-            continue;            
+            continue;
         }
 
         if (!theChannel) {
@@ -112,22 +167,58 @@ void connectTask(void *pvParameters) {
         }
 
         while (theChannel->isConnected()) {
+            // Create framed packet: [seqno 8bit] [16bit payload length] [payload]
+            std::vector<uint8_t> packet;
+            packet.reserve(3 + currentPayloadSize);
 
-        /*
-            static auto initialDelay = true;
-            if (initialDelay) {
-                printf("Waiting gracefully 3 seconds before sending data\n");
-                vTaskDelay(3000 / portTICK_PERIOD_MS);
-                initialDelay = false;
-            };
-*/
-            std::vector<uint8_t> data(5000, sequenceNumber++);
-            if (theChannel->write(data)) {
-                bytesSent += data.size();
+            // Add sequence number (8 bits)
+            packet.push_back(sequenceNumber);
+
+            // Add payload length (16 bits, big endian - network byte order)
+            uint16_t payloadLen = currentPayloadSize;
+            packet.push_back((payloadLen >> 8) & 0xFF);  // High byte first
+            packet.push_back(payloadLen & 0xFF);         // Low byte second
+
+            // Add payload
+            for (size_t i = 0; i < currentPayloadSize; i++) {
+                packet.push_back(i & 0xFF);
+            }
+
+            if (theChannel->write(packet)) {
+                if (startTime == 0) {
+                    startTime = esp_timer_get_time();
+                }
+                bytesSent += packet.size();
+                blocksSent++;
+
+                // Print every block since we're sending slowly now
+                printf("Sent block %lu (seq=%d, payload=%zu bytes, frame_size=%zu)\n",
+                       (unsigned long)blocksSent, sequenceNumber, currentPayloadSize, packet.size());
+
+                sequenceNumber++;
+
+                // After every 50 blocks, double payload size
+                if (blocksSent % BLOCKS_BEFORE_DOUBLE == 0) {
+                    size_t newSize = currentPayloadSize * 2;
+
+                    // Cap at maximum safe payload size
+                    if (newSize > MAX_PAYLOAD_SIZE) {
+                        if (currentPayloadSize < MAX_PAYLOAD_SIZE) {
+                            currentPayloadSize = MAX_PAYLOAD_SIZE;
+                            printf("\n=== Reached maximum payload size of %zu bytes after %lu blocks ===\n", currentPayloadSize, (unsigned long)blocksSent);
+                        }
+                        // Already at max, don't increase further
+                    } else {
+                        currentPayloadSize = newSize;
+                        printf("\n=== Doubling payload size to %zu bytes after %lu blocks ===\n", currentPayloadSize, (unsigned long)blocksSent);
+                    }
+                }
             } else {
                 printf("failed to send!\n");
-                abort();                
+                abort();
             }
+
+            // No delay - send as fast as possible
         }
 
         vTaskDelay(1000 / portTICK_PERIOD_MS);
@@ -136,9 +227,13 @@ void connectTask(void *pvParameters) {
 
 extern "C"
 void app_main(void) {
+    // Install high performance logging before any output
+    esp_hpl::HighPerformanceLogger::init();
+
     printf("Starting L2CAP client example\n");
 
     xTaskCreate(connectTask, "connectTask", 5000, NULL, 1, NULL);
+    xTaskCreate(statusTask, "statusTask", 3000, NULL, 1, NULL);
 
     BLEDevice::init("L2CAP-Client");
     BLEDevice::setMTU(BLE_ATT_MTU_MAX);
@@ -151,15 +246,8 @@ void app_main(void) {
     scan->setActiveScan(true);
     scan->start(25 * 1000, false);
 
-    int numberOfSeconds = 0;
-
-    while (bytesSent == 0) {
-        vTaskDelay(10 / portTICK_PERIOD_MS);
-    }
-
+    // Main task just waits
     while (true) {
         vTaskDelay(1000 / portTICK_PERIOD_MS);
-        int bytesSentPerSeconds = bytesSent / ++numberOfSeconds;
-        printf("Bandwidth: %d b/sec = %d KB/sec\n", bytesSentPerSeconds, bytesSentPerSeconds / 1024);
     }
 }

examples/L2CAP/L2CAP_Server/main/idf_component.yml

@@ -1,3 +1,6 @@
 dependencies:
   local/esp-nimble-cpp:
     path: ../../../../../esp-nimble-cpp/
+  mickeyl/esp-hpl:
+    git: https://github.com/mickeyl/esp-hpl.git
+    version: "1.1.0"

examples/L2CAP/L2CAP_Server/main/main.cpp

@@ -1,13 +1,16 @@
 #include <NimBLEDevice.h>
+#include <esp_hpl.hpp>
+#include <esp_timer.h>
 
-// See the following for generating UUIDs:
-// https://www.uuidgenerator.net/
-
-#define SERVICE_UUID        "dcbc7255-1e9e-49a0-a360-b0430b6c6905"
-#define CHARACTERISTIC_UUID "371a55c8-f251-4ad2-90b3-c7c195b049be"
-#define L2CAP_CHANNEL        150
+#define L2CAP_PSM            192
 #define L2CAP_MTU            5000
 
+// Heap monitoring
+size_t initialHeap = 0;
+size_t lastHeap = 0;
+size_t heapDecreaseCount = 0;
+const size_t HEAP_LEAK_THRESHOLD = 10;  // Warn after 10 consecutive decreases
+
 class GATTCallbacks: public BLEServerCallbacks {
 
 public:
@@ -23,68 +26,179 @@ class L2CAPChannelCallbacks: public BLEL2CAPChannelCallbacks {
 
 public:
     bool connected = false;
-    size_t numberOfReceivedBytes;
-    uint8_t nextSequenceNumber;
+    size_t totalBytesReceived = 0;
+    size_t totalFramesReceived = 0;
+    size_t totalPayloadBytes = 0;
+    uint8_t expectedSequenceNumber = 0;
+    size_t sequenceErrors = 0;
+    size_t frameErrors = 0;
+    uint64_t startTime = 0;
+    std::vector<uint8_t> buffer;  // Buffer for incomplete frames
 
 public:
     void onConnect(NimBLEL2CAPChannel* channel) {
-        printf("L2CAP connection established\n");
+        printf("L2CAP connection established on PSM %d\n", L2CAP_PSM);
         connected = true;
-        numberOfReceivedBytes = nextSequenceNumber = 0;
+        totalBytesReceived = 0;
+        totalFramesReceived = 0;
+        totalPayloadBytes = 0;
+        expectedSequenceNumber = 0;
+        sequenceErrors = 0;
+        frameErrors = 0;
+        startTime = esp_timer_get_time();
+        buffer.clear();
     }
 
     void onRead(NimBLEL2CAPChannel* channel, std::vector<uint8_t>& data) {
-        numberOfReceivedBytes += data.size();
-        size_t sequenceNumber = data[0];
-        printf("L2CAP read %d bytes w/ sequence number %d", data.size(), sequenceNumber);
-        if (sequenceNumber != nextSequenceNumber) {
-            printf("(wrong sequence number %d, expected %d)\n", sequenceNumber, nextSequenceNumber);
-        } else {
-            printf("\n");
-            nextSequenceNumber++;
+        // Append new data to buffer
+        buffer.insert(buffer.end(), data.begin(), data.end());
+        totalBytesReceived += data.size();
+        if (startTime == 0) {
+            startTime = esp_timer_get_time();  // start measuring once data flows
+        }
+
+        // Process complete frames from buffer
+        while (buffer.size() >= 3) {  // Minimum frame size: seqno(1) + len(2)
+            // Parse frame header
+            uint8_t seqno = buffer[0];
+            uint16_t payloadLen = (buffer[1] << 8) | buffer[2];  // Big-endian
+
+            size_t frameSize = 3 + payloadLen;
+
+            // Check if we have complete frame
+            if (buffer.size() < frameSize) {
+                break;  // Wait for more data
+            }
+
+            // Validate and process frame
+            totalFramesReceived++;
+            totalPayloadBytes += payloadLen;
+
+            // Check sequence number
+            if (seqno != expectedSequenceNumber) {
+                sequenceErrors++;
+                printf("Frame %zu: Sequence error - got %d, expected %d (payload=%d bytes)\n",
+                       totalFramesReceived, seqno, expectedSequenceNumber, payloadLen);
+            }
+
+            // Update expected sequence number (wraps at 256)
+            expectedSequenceNumber = (seqno + 1) & 0xFF;
+
+            // Remove processed frame from buffer
+            buffer.erase(buffer.begin(), buffer.begin() + frameSize);
+
+            // Print progress every 100 frames
+            if (totalFramesReceived % 100 == 0) {
+                double elapsedSeconds = (esp_timer_get_time() - startTime) / 1000000.0;
+                double bytesPerSecond = elapsedSeconds > 0 ? totalBytesReceived / elapsedSeconds : 0.0;
+                printf("Received %zu frames (%zu payload bytes) - Bandwidth: %.2f KB/s (%.2f Mbps)\n",
+                       totalFramesReceived, totalPayloadBytes,
+                       bytesPerSecond / 1024.0, (bytesPerSecond * 8) / 1000000.0);
+            }
         }
     }
+
     void onDisconnect(NimBLEL2CAPChannel* channel) {
-        printf("L2CAP disconnected\n");
+        printf("\nL2CAP disconnected\n");
+        double elapsedSeconds = startTime > 0 ? (esp_timer_get_time() - startTime) / 1000000.0 : 0.0;
+        double bytesPerSecond = elapsedSeconds > 0 ? totalBytesReceived / elapsedSeconds : 0.0;
+
+        printf("Final statistics:\n");
+        printf("  Total frames: %zu\n", totalFramesReceived);
+        printf("  Total bytes: %zu\n", totalBytesReceived);
+        printf("  Payload bytes: %zu\n", totalPayloadBytes);
+        printf("  Sequence errors: %zu\n", sequenceErrors);
+        printf("  Frame errors: %zu\n", frameErrors);
+        printf("  Bandwidth: %.2f KB/s (%.2f Mbps)\n", bytesPerSecond / 1024.0, (bytesPerSecond * 8) / 1000000.0);
+
+        // Reset state for the next connection
+        buffer.clear();
+        totalBytesReceived = 0;
+        totalFramesReceived = 0;
+        totalPayloadBytes = 0;
+        expectedSequenceNumber = 0;
+        sequenceErrors = 0;
+        frameErrors = 0;
+        startTime = 0;
         connected = false;
+
+        // Restart advertising so another client can connect
+        BLEDevice::startAdvertising();
     }
 };
 
 extern "C"
 void app_main(void) {
+    // Install high performance logging before any other output
+    esp_hpl::HighPerformanceLogger::init();
+
     printf("Starting L2CAP server example [%lu free] [%lu min]\n", esp_get_free_heap_size(), esp_get_minimum_free_heap_size());
 
-    BLEDevice::init("L2CAP-Server");
+    BLEDevice::init("l2cap");  // Match the name the client is looking for
     BLEDevice::setMTU(BLE_ATT_MTU_MAX);
 
     auto cocServer = BLEDevice::createL2CAPServer();
     auto l2capChannelCallbacks = new L2CAPChannelCallbacks();
-    auto channel = cocServer->createService(L2CAP_CHANNEL, L2CAP_MTU, l2capChannelCallbacks);
-    
+    auto channel = cocServer->createService(L2CAP_PSM, L2CAP_MTU, l2capChannelCallbacks);
+    (void)channel;  // prevent unused warning
+
     auto server = BLEDevice::createServer();
     server->setCallbacks(new GATTCallbacks());
-    auto service = server->createService(SERVICE_UUID);
-    auto characteristic = service->createCharacteristic(CHARACTERISTIC_UUID, NIMBLE_PROPERTY::READ);
-    characteristic->setValue(L2CAP_CHANNEL);
-    service->start();
+
     auto advertising = BLEDevice::getAdvertising();
-    advertising->addServiceUUID(SERVICE_UUID);
-    advertising->enableScanResponse(true);
+    NimBLEAdvertisementData scanData;
+    scanData.setName("l2cap");
+    advertising->setScanResponseData(scanData);
 
     BLEDevice::startAdvertising();
     printf("Server waiting for connection requests [%lu free] [%lu min]\n", esp_get_free_heap_size(), esp_get_minimum_free_heap_size());
 
-    // Wait until transfer actually starts...
-    while (!l2capChannelCallbacks->numberOfReceivedBytes) {
-        vTaskDelay(10 / portTICK_PERIOD_MS);
-    }
-    printf("\n\n\n");
-    int numberOfSeconds = 0;
-
+    // Status reporting loop
     while (true) {
         vTaskDelay(1000 / portTICK_PERIOD_MS);
-        if (!l2capChannelCallbacks->connected) { continue; }
-        int bps = l2capChannelCallbacks->numberOfReceivedBytes / ++numberOfSeconds;
-        printf("Bandwidth: %d b/sec = %d KB/sec [%lu free] [%lu min]\n", bps, bps / 1024, esp_get_free_heap_size(), esp_get_minimum_free_heap_size());
+
+        if (l2capChannelCallbacks->connected && l2capChannelCallbacks->totalBytesReceived > 0) {
+            uint64_t currentTime = esp_timer_get_time();
+            double elapsedSeconds = (currentTime - l2capChannelCallbacks->startTime) / 1000000.0;
+
+            if (elapsedSeconds > 0) {
+                double bytesPerSecond = l2capChannelCallbacks->totalBytesReceived / elapsedSeconds;
+                double framesPerSecond = l2capChannelCallbacks->totalFramesReceived / elapsedSeconds;
+
+                // Heap monitoring
+                size_t currentHeap = esp_get_free_heap_size();
+                size_t minHeap = esp_get_minimum_free_heap_size();
+
+                // Track heap for leak detection
+                if (initialHeap == 0) {
+                    initialHeap = currentHeap;
+                    lastHeap = currentHeap;
+                }
+
+                // Check for consistent heap decrease
+                if (currentHeap < lastHeap) {
+                    heapDecreaseCount++;
+                    if (heapDecreaseCount >= HEAP_LEAK_THRESHOLD) {
+                        printf("\n⚠️  WARNING: POSSIBLE MEMORY LEAK DETECTED! ⚠️\n");
+                        printf("Heap has decreased %zu times in a row\n", heapDecreaseCount);
+                        printf("Initial heap: %zu, Current heap: %zu, Lost: %zu bytes\n",
+                               initialHeap, currentHeap, initialHeap - currentHeap);
+                    }
+                } else if (currentHeap >= lastHeap) {
+                    heapDecreaseCount = 0;  // Reset counter if heap stabilizes or increases
+                }
+                lastHeap = currentHeap;
+
+                printf("\n=== STATUS UPDATE ===\n");
+                printf("Frames received: %zu (%.1f fps)\n", l2capChannelCallbacks->totalFramesReceived, framesPerSecond);
+                printf("Total bytes: %zu\n", l2capChannelCallbacks->totalBytesReceived);
+                printf("Payload bytes: %zu\n", l2capChannelCallbacks->totalPayloadBytes);
+                printf("Bandwidth: %.2f KB/s (%.2f Mbps)\n", bytesPerSecond / 1024.0, (bytesPerSecond * 8) / 1000000.0);
+                printf("Sequence errors: %zu\n", l2capChannelCallbacks->sequenceErrors);
+                printf("Heap: %zu free (min: %zu), Used since start: %zu\n",
+                       currentHeap, minHeap, initialHeap > 0 ? initialHeap - currentHeap : 0);
+                printf("==================\n");
+            }
+        }
     }
 }

examples/NimBLE_Server/main/main.cpp

@@ -16,7 +16,7 @@ static NimBLEServer* pServer;
  **                       Remove as you see fit for your needs                        */
 class ServerCallbacks : public NimBLEServerCallbacks {
     void onConnect(NimBLEServer* pServer, NimBLEConnInfo& connInfo) override {
-        printf("Client address: %s\n", connInfo.getAddress().toString().c_str());
+        printf("Client connected:\n%s", connInfo.toString().c_str());
 
         /**
          *  We can use the connection handle here to ask for different connection parameters.
@@ -184,10 +184,6 @@ extern "C" void app_main(void) {
     pC01Ddsc->setValue("Send it back!");
     pC01Ddsc->setCallbacks(&dscCallbacks);
 
-    /** Start the services when finished creating all Characteristics and Descriptors */
-    pDeadService->start();
-    pBaadService->start();
-
     /** Create an advertising instance and add the services to the advertised data */
     NimBLEAdvertising* pAdvertising = NimBLEDevice::getAdvertising();
     pAdvertising->setName("NimBLE-Server");

examples/NimBLE_Stream_Client/CMakeLists.txt

@@ -0,0 +1,6 @@
+# The following lines of boilerplate have to be in your project's
+# CMakeLists in this exact order for cmake to work correctly
+cmake_minimum_required(VERSION 3.5)
+
+include($ENV{IDF_PATH}/tools/cmake/project.cmake)
+project(NimBLE_Stream_Client)

examples/NimBLE_Stream_Client/README.md

@@ -0,0 +1,53 @@
+# NimBLE Stream Client Example
+
+This example demonstrates how to use the `NimBLEStreamClient` class to connect to a BLE GATT server and communicate using the familiar Arduino Stream interface.
+
+## Features
+
+- Uses Arduino Stream interface (print, println, read, available, etc.)
+- Automatic server discovery and connection
+- Bidirectional communication
+- Buffered TX/RX using ring buffers
+- Automatic reconnection on disconnect
+- Similar usage to Serial communication
+
+## How it Works
+
+1. Scans for BLE devices advertising the target service UUID
+2. Connects to the server and discovers the stream characteristic
+3. Initializes `NimBLEStreamClient` with the remote characteristic
+4. Subscribes to notifications to receive data in the RX buffer
+5. Uses familiar Stream methods like `print()`, `println()`, `read()`, and `available()`
+
+## Usage
+
+1. Build and flash the NimBLE_Stream_Server example to one ESP32 using ESP-IDF (`idf.py build flash monitor`)
+2. Build and flash this client example to another ESP32 using ESP-IDF
+3. The client will automatically:
+   - Scan for the server
+   - Connect when found
+   - Set up the stream interface
+   - Begin bidirectional communication
+4. Open `idf.py monitor` on each board to observe stream traffic
+
+## Service UUIDs
+
+Must match the server:
+- Service: `6E400001-B5A3-F393-E0A9-E50E24DCCA9E`
+- Characteristic: `6E400002-B5A3-F393-E0A9-E50E24DCCA9E`
+
+## Monitor Output
+
+The example displays:
+- Server discovery progress
+- Connection status
+- All data received from the server
+- Confirmation of data sent to the server
+
+## Testing
+
+Run with NimBLE_Stream_Server to see bidirectional communication:
+- Server sends periodic status messages
+- Client sends periodic uptime messages
+- Both echo data received from each other
+- You can send data from either `idf.py monitor` session

examples/NimBLE_Stream_Client/main/CMakeLists.txt

@@ -0,0 +1,4 @@
+set(COMPONENT_SRCS "main.cpp")
+set(COMPONENT_ADD_INCLUDEDIRS ".")
+
+register_component()

examples/NimBLE_Stream_Client/main/main.cpp

@@ -0,0 +1,217 @@
+/**
+ *  NimBLE_Stream_Client Example:
+ *
+ *  Demonstrates using NimBLEStreamClient to connect to a BLE GATT server
+ *  and communicate using the Stream-like interface.
+ *
+ *  This example connects to the NimBLE_Stream_Server example.
+ */
+
+#include <inttypes.h>
+#include <stdint.h>
+#include <stdio.h>
+
+#include "esp_timer.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+
+#include <NimBLEDevice.h>
+
+// Service and Characteristic UUIDs (must match the server)
+#define SERVICE_UUID        "6E400001-B5A3-F393-E0A9-E50E24DCCA9E"
+#define CHARACTERISTIC_UUID "6E400002-B5A3-F393-E0A9-E50E24DCCA9E"
+
+// Create the stream client instance
+NimBLEStreamClient bleStream;
+
+struct RxOverflowStats {
+    uint32_t droppedOld{0};
+    uint32_t droppedNew{0};
+};
+
+RxOverflowStats g_rxOverflowStats;
+uint32_t        scanTime = 5000; // Scan duration in milliseconds
+
+NimBLEStream::RxOverflowAction onRxOverflow(const uint8_t* data, size_t len, void* userArg) {
+    auto* stats = static_cast<RxOverflowStats*>(userArg);
+    if (stats) {
+        stats->droppedOld++;
+    }
+
+    // For status/telemetry streams, prioritize newest packets.
+    (void)data;
+    (void)len;
+    return NimBLEStream::DROP_OLDER_DATA;
+}
+
+static uint64_t millis() {
+    return esp_timer_get_time() / 1000ULL;
+}
+
+// Connection state variables
+static bool                          doConnect     = false;
+static bool                          connected     = false;
+static const NimBLEAdvertisedDevice* pServerDevice = nullptr;
+static NimBLEClient*                 pClient       = nullptr;
+
+/** Scan callbacks to find the server */
+class ScanCallbacks : public NimBLEScanCallbacks {
+    void onResult(const NimBLEAdvertisedDevice* advertisedDevice) override {
+        printf("Advertised Device: %s\n", advertisedDevice->toString().c_str());
+
+        // Check if this device advertises our service.
+        if (advertisedDevice->isAdvertisingService(NimBLEUUID(SERVICE_UUID))) {
+            printf("Found our stream server!\n");
+            pServerDevice = advertisedDevice;
+            NimBLEDevice::getScan()->stop();
+            doConnect = true;
+        }
+    }
+
+    void onScanEnd(const NimBLEScanResults& results, int reason) override {
+        (void)results;
+        (void)reason;
+        printf("Scan ended\n");
+        if (!doConnect && !connected) {
+            printf("Server not found, restarting scan...\n");
+            NimBLEDevice::getScan()->start(scanTime, false, true);
+        }
+    }
+} scanCallbacks;
+
+/** Client callbacks for connection/disconnection events */
+class ClientCallbacks : public NimBLEClientCallbacks {
+    void onConnect(NimBLEClient* pClient) override {
+        printf("Connected to server\n");
+        // Update connection parameters for better throughput.
+        pClient->updateConnParams(12, 24, 0, 200);
+    }
+
+    void onDisconnect(NimBLEClient* pClient, int reason) override {
+        (void)pClient;
+        printf("Disconnected from server, reason: %d\n", reason);
+        connected = false;
+        bleStream.end();
+
+        // Restart scanning.
+        printf("Restarting scan...\n");
+        NimBLEDevice::getScan()->start(scanTime, false, true);
+    }
+} clientCallbacks;
+
+/** Connect to the BLE Server and set up the stream */
+bool connectToServer() {
+    printf("Connecting to: %s\n", pServerDevice->getAddress().toString().c_str());
+
+    // Create or reuse a client.
+    pClient = NimBLEDevice::getClientByPeerAddress(pServerDevice->getAddress());
+    if (!pClient) {
+        pClient = NimBLEDevice::createClient();
+        if (!pClient) {
+            printf("Failed to create client\n");
+            return false;
+        }
+        pClient->setClientCallbacks(&clientCallbacks, false);
+        pClient->setConnectionParams(12, 24, 0, 200);
+        pClient->setConnectTimeout(5000);
+    }
+
+    // Connect to the remote BLE Server.
+    if (!pClient->connect(pServerDevice)) {
+        printf("Failed to connect to server\n");
+        return false;
+    }
+
+    printf("Connected! Discovering services...\n");
+
+    // Get the service and characteristic.
+    NimBLERemoteService* pRemoteService = pClient->getService(SERVICE_UUID);
+    if (!pRemoteService) {
+        printf("Failed to find our service UUID\n");
+        pClient->disconnect();
+        return false;
+    }
+    printf("Found the stream service\n");
+
+    NimBLERemoteCharacteristic* pRemoteCharacteristic = pRemoteService->getCharacteristic(CHARACTERISTIC_UUID);
+    if (!pRemoteCharacteristic) {
+        printf("Failed to find our characteristic UUID\n");
+        pClient->disconnect();
+        return false;
+    }
+    printf("Found the stream characteristic\n");
+
+    // subscribeNotify=true means notifications are stored in the RX buffer.
+    if (!bleStream.begin(pRemoteCharacteristic, true)) {
+        printf("Failed to initialize BLE stream!\n");
+        pClient->disconnect();
+        return false;
+    }
+
+    bleStream.setRxOverflowCallback(onRxOverflow, &g_rxOverflowStats);
+
+    printf("BLE Stream initialized successfully!\n");
+    connected = true;
+    return true;
+}
+
+extern "C" void app_main(void) {
+    printf("Starting NimBLE Stream Client\n");
+
+    /** Initialize NimBLE */
+    NimBLEDevice::init("NimBLE-StreamClient");
+
+    // Create the BLE scan instance and set callbacks.
+    NimBLEScan* pScan = NimBLEDevice::getScan();
+    pScan->setScanCallbacks(&scanCallbacks, false);
+    pScan->setActiveScan(true);
+
+    // Start scanning for the server.
+    printf("Scanning for BLE Stream Server...\n");
+    pScan->start(scanTime, false, true);
+
+    uint32_t lastDroppedOld = 0;
+    uint32_t lastDroppedNew = 0;
+    uint64_t lastSend       = 0;
+
+    for (;;) {
+        if (g_rxOverflowStats.droppedOld != lastDroppedOld || g_rxOverflowStats.droppedNew != lastDroppedNew) {
+            lastDroppedOld = g_rxOverflowStats.droppedOld;
+            lastDroppedNew = g_rxOverflowStats.droppedNew;
+            printf("RX overflow handled (drop-old=%" PRIu32 ", drop-new=%" PRIu32 ")\n", lastDroppedOld, lastDroppedNew);
+        }
+
+        // If we found a server, try to connect.
+        if (doConnect) {
+            doConnect = false;
+            if (connectToServer()) {
+                printf("Stream ready for communication!\n");
+            } else {
+                printf("Failed to connect to server, restarting scan...\n");
+                pServerDevice = nullptr;
+                NimBLEDevice::getScan()->start(scanTime, false, true);
+            }
+        }
+
+        // If connected, demonstrate stream communication.
+        if (connected && bleStream) {
+            if (bleStream.available()) {
+                printf("Received from server: ");
+                while (bleStream.available()) {
+                    char c = bleStream.read();
+                    putchar(c);
+                }
+                printf("\n");
+            }
+
+            uint64_t now = millis();
+            if (now - lastSend > 5000) {
+                lastSend = now;
+                bleStream.printf("Hello from client! Uptime: %" PRIu64 " seconds\n", now / 1000);
+                printf("Sent data to server via BLE stream\n");
+            }
+        }
+
+        vTaskDelay(pdMS_TO_TICKS(10));
+    }
+}

examples/NimBLE_Stream_Client/sdkconfig.defaults

@@ -0,0 +1,12 @@
+# Override some defaults so BT stack is enabled
+# in this example
+
+#
+# BT config
+#
+CONFIG_BT_ENABLED=y
+CONFIG_BTDM_CTRL_MODE_BLE_ONLY=y
+CONFIG_BTDM_CTRL_MODE_BR_EDR_ONLY=n
+CONFIG_BTDM_CTRL_MODE_BTDM=n
+CONFIG_BT_BLUEDROID_ENABLED=n
+CONFIG_BT_NIMBLE_ENABLED=y

examples/NimBLE_Stream_Echo/CMakeLists.txt

@@ -0,0 +1,6 @@
+# The following lines of boilerplate have to be in your project's
+# CMakeLists in this exact order for cmake to work correctly
+cmake_minimum_required(VERSION 3.5)
+
+include($ENV{IDF_PATH}/tools/cmake/project.cmake)
+project(NimBLE_Stream_Echo)

examples/NimBLE_Stream_Echo/README.md

@@ -0,0 +1,39 @@
+# NimBLE Stream Echo Example
+
+This is the simplest example demonstrating `NimBLEStreamServer`. It echoes back any data received from BLE clients.
+
+## Features
+
+- Minimal code showing essential NimBLE Stream usage
+- Echoes all received data back to the client
+- Uses default service and characteristic UUIDs
+- Perfect starting point for learning the Stream interface
+
+## How it Works
+
+1. Initializes BLE with minimal configuration
+2. Creates a stream server with default UUIDs
+3. Waits for client connection and data
+4. Echoes received data back to the client
+5. Displays received data in the ESP-IDF monitor output
+
+## Default UUIDs
+
+- Service: `0xc0de`
+- Characteristic: `0xfeed`
+
+## Usage
+
+1. Build and flash this example to your ESP32 using ESP-IDF (`idf.py build flash monitor`)
+2. Connect with a BLE client app (nRF Connect, Serial Bluetooth Terminal, etc.)
+3. Find the service `0xc0de` and characteristic `0xfeed`
+4. Subscribe to notifications
+5. Write data to the characteristic
+6. The data will be echoed back and displayed in `idf.py monitor`
+
+## Good For
+
+- Learning the basic NimBLE Stream API
+- Testing BLE connectivity
+- Starting point for custom applications
+- Understanding Stream read/write operations

examples/NimBLE_Stream_Echo/main/CMakeLists.txt

@@ -0,0 +1,4 @@
+set(COMPONENT_SRCS "main.cpp")
+set(COMPONENT_ADD_INCLUDEDIRS ".")
+
+register_component()

examples/NimBLE_Stream_Echo/main/main.cpp

@@ -0,0 +1,83 @@
+/**
+ *  NimBLE_Stream_Echo Example:
+ *
+ *  A minimal example demonstrating NimBLEStreamServer.
+ *  Echoes back any data received from BLE clients.
+ */
+
+#include <inttypes.h>
+#include <stdint.h>
+#include <stdio.h>
+
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+
+#include <NimBLEDevice.h>
+
+NimBLEStreamServer bleStream;
+
+struct RxOverflowStats {
+    uint32_t droppedOld{0};
+    uint32_t droppedNew{0};
+};
+
+RxOverflowStats g_rxOverflowStats;
+
+NimBLEStream::RxOverflowAction onRxOverflow(const uint8_t* data, size_t len, void* userArg) {
+    auto* stats = static_cast<RxOverflowStats*>(userArg);
+    if (stats) {
+        stats->droppedOld++;
+    }
+
+    // Echo mode prefers the latest incoming bytes.
+    (void)data;
+    (void)len;
+    return NimBLEStream::DROP_OLDER_DATA;
+}
+
+extern "C" void app_main(void) {
+    printf("NimBLE Stream Echo Server\n");
+
+    // Initialize BLE.
+    NimBLEDevice::init("BLE-Echo");
+    auto pServer = NimBLEDevice::createServer();
+    pServer->advertiseOnDisconnect(true); // Keep advertising after disconnects.
+
+    if (!bleStream.begin(NimBLEUUID(uint16_t(0xc0de)),
+                         NimBLEUUID(uint16_t(0xfeed)),
+                         1024,
+                         1024,
+                         false)) {
+        printf("Failed to initialize BLE stream\n");
+        return;
+    }
+
+    bleStream.setRxOverflowCallback(onRxOverflow, &g_rxOverflowStats);
+
+    // Start advertising.
+    NimBLEDevice::getAdvertising()->start();
+    printf("Ready! Connect with a BLE client and send data.\n");
+
+    uint32_t lastDroppedOld = 0;
+    uint32_t lastDroppedNew = 0;
+
+    for (;;) {
+        if (g_rxOverflowStats.droppedOld != lastDroppedOld || g_rxOverflowStats.droppedNew != lastDroppedNew) {
+            lastDroppedOld = g_rxOverflowStats.droppedOld;
+            lastDroppedNew = g_rxOverflowStats.droppedNew;
+            printf("RX overflow handled (drop-old=%" PRIu32 ", drop-new=%" PRIu32 ")\n", lastDroppedOld, lastDroppedNew);
+        }
+
+        // Echo any received data back to the client.
+        if (bleStream.ready() && bleStream.available()) {
+            printf("Echo: ");
+            while (bleStream.available()) {
+                char c = bleStream.read();
+                putchar(c);
+                bleStream.write(c);
+            }
+            printf("\n");
+        }
+        vTaskDelay(pdMS_TO_TICKS(10));
+    }
+}

examples/NimBLE_Stream_Echo/sdkconfig.defaults

@@ -0,0 +1,12 @@
+# Override some defaults so BT stack is enabled
+# in this example
+
+#
+# BT config
+#
+CONFIG_BT_ENABLED=y
+CONFIG_BTDM_CTRL_MODE_BLE_ONLY=y
+CONFIG_BTDM_CTRL_MODE_BR_EDR_ONLY=n
+CONFIG_BTDM_CTRL_MODE_BTDM=n
+CONFIG_BT_BLUEDROID_ENABLED=n
+CONFIG_BT_NIMBLE_ENABLED=y

examples/NimBLE_Stream_Server/CMakeLists.txt

@@ -0,0 +1,6 @@
+# The following lines of boilerplate have to be in your project's
+# CMakeLists in this exact order for cmake to work correctly
+cmake_minimum_required(VERSION 3.5)
+
+include($ENV{IDF_PATH}/tools/cmake/project.cmake)
+project(NimBLE_Stream_Server)

examples/NimBLE_Stream_Server/README.md

@@ -0,0 +1,42 @@
+# NimBLE Stream Server Example
+
+This example demonstrates how to use the `NimBLEStreamServer` class to create a BLE GATT server that behaves like a serial port using the familiar Arduino Stream interface.
+
+## Features
+
+- Uses Arduino Stream interface (print, println, read, available, etc.)
+- Automatic connection management
+- Bidirectional communication
+- Buffered TX/RX using ring buffers
+- Similar usage to Serial communication
+
+## How it Works
+
+1. Creates a BLE GATT server with a custom service and characteristic
+2. Initializes `NimBLEStreamServer` with the characteristic configured for notifications and writes
+3. Uses familiar Stream methods like `print()`, `println()`, `read()`, and `available()`
+4. Automatically handles connection state and MTU negotiation
+
+## Usage
+
+1. Build and flash this example to your ESP32 using ESP-IDF (`idf.py build flash monitor`)
+2. The device will advertise as "NimBLE-Stream"
+3. Connect with a BLE client (such as the NimBLE_Stream_Client example or a mobile app)
+4. Once connected, the server will:
+   - Send periodic messages to the client
+   - Echo back any data received from the client
+   - Display all communication in `idf.py monitor`
+
+## Service UUIDs
+
+- Service: `6E400001-B5A3-F393-E0A9-E50E24DCCA9E`
+- Characteristic: `6E400002-B5A3-F393-E0A9-E50E24DCCA9E`
+
+These are based on the Nordic UART Service (NUS) UUIDs for compatibility with many BLE terminal apps.
+
+## Compatible With
+
+- NimBLE_Stream_Client example
+- nRF Connect mobile app
+- Serial Bluetooth Terminal apps
+- Any BLE client that supports characteristic notifications and writes

examples/NimBLE_Stream_Server/main/CMakeLists.txt

@@ -0,0 +1,4 @@
+set(COMPONENT_SRCS "main.cpp")
+set(COMPONENT_ADD_INCLUDEDIRS ".")
+
+register_component()

examples/NimBLE_Stream_Server/main/main.cpp

@@ -0,0 +1,146 @@
+/**
+ *  NimBLE_Stream_Server Example:
+ *
+ *  Demonstrates using NimBLEStreamServer to create a BLE GATT server
+ *  that behaves like a serial port using the Stream-like interface.
+ */
+
+#include <inttypes.h>
+#include <stdint.h>
+#include <stdio.h>
+
+#include "esp_heap_caps.h"
+#include "esp_timer.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+
+#include <NimBLEDevice.h>
+
+// Create the stream server instance
+NimBLEStreamServer bleStream;
+
+struct RxOverflowStats {
+    uint32_t droppedOld{0};
+    uint32_t droppedNew{0};
+};
+
+RxOverflowStats g_rxOverflowStats;
+
+NimBLEStream::RxOverflowAction onRxOverflow(const uint8_t* data, size_t len, void* userArg) {
+    auto* stats = static_cast<RxOverflowStats*>(userArg);
+    if (stats) {
+        stats->droppedOld++;
+    }
+
+    // Keep the newest bytes for command/stream style traffic.
+    (void)data;
+    (void)len;
+    return NimBLEStream::DROP_OLDER_DATA;
+}
+
+static uint64_t millis() {
+    return esp_timer_get_time() / 1000ULL;
+}
+
+// Service and Characteristic UUIDs for the stream.
+#define SERVICE_UUID        "6E400001-B5A3-F393-E0A9-E50E24DCCA9E"
+#define CHARACTERISTIC_UUID "6E400002-B5A3-F393-E0A9-E50E24DCCA9E"
+
+/** Server callbacks to handle connection/disconnection events */
+class ServerCallbacks : public NimBLEServerCallbacks {
+    void onConnect(NimBLEServer* pServer, NimBLEConnInfo& connInfo) override {
+        printf("Client connected: %s\n", connInfo.getAddress().toString().c_str());
+        // Optionally update connection parameters for better throughput.
+        pServer->updateConnParams(connInfo.getConnHandle(), 12, 24, 0, 200);
+    }
+
+    void onDisconnect(NimBLEServer* pServer, NimBLEConnInfo& connInfo, int reason) override {
+        (void)pServer;
+        (void)connInfo;
+        printf("Client disconnected - reason: %d, restarting advertising\n", reason);
+        NimBLEDevice::startAdvertising();
+    }
+
+    void onMTUChange(uint16_t MTU, NimBLEConnInfo& connInfo) override {
+        printf("MTU updated: %u for connection ID: %u\n", MTU, connInfo.getConnHandle());
+    }
+} serverCallbacks;
+
+extern "C" void app_main(void) {
+    printf("Starting NimBLE Stream Server\n");
+
+    /** Initialize NimBLE and set the device name */
+    NimBLEDevice::init("NimBLE-Stream");
+
+    /**
+     * Create the BLE server and set callbacks.
+     * Note: The stream will create its own service and characteristic.
+     */
+    NimBLEServer* pServer = NimBLEDevice::createServer();
+    pServer->setCallbacks(&serverCallbacks);
+
+    /**
+     * Initialize the stream server with:
+     * - Service UUID
+     * - Characteristic UUID
+     * - txBufSize: 1024 bytes for outgoing data (notifications)
+     * - rxBufSize: 1024 bytes for incoming data (writes)
+     * - secure: false (no encryption required - set true for secure connections)
+     */
+    if (!bleStream.begin(NimBLEUUID(SERVICE_UUID),
+                         NimBLEUUID(CHARACTERISTIC_UUID),
+                         1024,
+                         1024,
+                         false)) {
+        printf("Failed to initialize BLE stream!\n");
+        return;
+    }
+
+    bleStream.setRxOverflowCallback(onRxOverflow, &g_rxOverflowStats);
+
+    // Make the stream service discoverable.
+    NimBLEAdvertising* pAdvertising = NimBLEDevice::getAdvertising();
+    pAdvertising->addServiceUUID(SERVICE_UUID);
+    pAdvertising->setName("NimBLE-Stream");
+    pAdvertising->enableScanResponse(true);
+    pAdvertising->start();
+
+    printf("BLE Stream Server ready!\n");
+    printf("Waiting for client connection...\n");
+
+    uint32_t lastDroppedOld = 0;
+    uint32_t lastDroppedNew = 0;
+    uint64_t lastSend       = 0;
+
+    for (;;) {
+        if (g_rxOverflowStats.droppedOld != lastDroppedOld || g_rxOverflowStats.droppedNew != lastDroppedNew) {
+            lastDroppedOld = g_rxOverflowStats.droppedOld;
+            lastDroppedNew = g_rxOverflowStats.droppedNew;
+            printf("RX overflow handled (drop-old=%" PRIu32 ", drop-new=%" PRIu32 ")\n", lastDroppedOld, lastDroppedNew);
+        }
+
+        if (bleStream.ready()) {
+            uint64_t now = millis();
+            if (now - lastSend > 2000) {
+                lastSend = now;
+                bleStream.printf("Hello from server! Uptime: %" PRIu64 " seconds\n", now / 1000);
+                bleStream.printf("Free heap: %" PRIu32 " bytes\n", esp_get_free_heap_size());
+                printf("Sent data to client via BLE stream\n");
+            }
+
+            if (bleStream.available()) {
+                printf("Received from client: ");
+                while (bleStream.available()) {
+                    char c = bleStream.read();
+                    putchar(c);
+                    bleStream.write(c); // Echo back to BLE client.
+                }
+                printf("\n");
+            }
+        } else {
+            vTaskDelay(pdMS_TO_TICKS(100));
+        }
+
+        vTaskDelay(pdMS_TO_TICKS(10));
+    }
+}

examples/NimBLE_Stream_Server/sdkconfig.defaults

@@ -0,0 +1,12 @@
+# Override some defaults so BT stack is enabled
+# in this example
+
+#
+# BT config
+#
+CONFIG_BT_ENABLED=y
+CONFIG_BTDM_CTRL_MODE_BLE_ONLY=y
+CONFIG_BTDM_CTRL_MODE_BR_EDR_ONLY=n
+CONFIG_BTDM_CTRL_MODE_BTDM=n
+CONFIG_BT_BLUEDROID_ENABLED=n
+CONFIG_BT_NIMBLE_ENABLED=y

idf_component.yml

@@ -1,5 +1,5 @@
 ## IDF Component Manager Manifest File
-version: "2.3.2"
+version: "2.5.0"
 license: "Apache-2.0"
 description: "C++ wrapper for the NimBLE BLE stack"
 url: "https://github.com/h2zero/esp-nimble-cpp"

library.json

@@ -1,6 +1,6 @@
 {
   "name": "esp-nimble-cpp",
-  "version": "2.3.2",
+  "version": "2.5.0",
   "description": "C++ wrapper for the NimBLE BLE stack",
   "keywords": [
     "BLE",

src/NimBLEAddress.cpp

@@ -22,13 +22,29 @@
 
 # include <algorithm>
 
-# ifdef CONFIG_NIMBLE_CPP_ADDR_FMT_EXCLUDE_DELIMITER
+# ifndef MYNEWT_VAL_NIMBLE_CPP_ADDR_FMT_EXCLUDE_DELIMITER
+#  ifndef CONFIG_NIMBLE_CPP_ADDR_FMT_EXCLUDE_DELIMITER
+#   define MYNEWT_VAL_NIMBLE_CPP_ADDR_FMT_EXCLUDE_DELIMITER (0)
+#  else
+#   define MYNEWT_VAL_NIMBLE_CPP_ADDR_FMT_EXCLUDE_DELIMITER CONFIG_NIMBLE_CPP_ADDR_FMT_EXCLUDE_DELIMITER
+#  endif
+# endif
+
+# ifndef MYNEWT_VAL_NIMBLE_CPP_ADDR_FMT_UPPERCASE
+#  ifndef CONFIG_NIMBLE_CPP_ADDR_FMT_UPPERCASE
+#   define MYNEWT_VAL_NIMBLE_CPP_ADDR_FMT_UPPERCASE (1)
+#  else
+#   define MYNEWT_VAL_NIMBLE_CPP_ADDR_FMT_UPPERCASE CONFIG_NIMBLE_CPP_ADDR_FMT_UPPERCASE
+#  endif
+# endif
+
+# if MYNEWT_VAL(NIMBLE_CPP_ADDR_FMT_EXCLUDE_DELIMITER)
 #  define NIMBLE_CPP_ADDR_DELIMITER ""
 # else
 #  define NIMBLE_CPP_ADDR_DELIMITER ":"
 # endif
 
-# ifdef CONFIG_NIMBLE_CPP_ADDR_FMT_UPPERCASE
+# if MYNEWT_VAL(NIMBLE_CPP_ADDR_FMT_UPPERCASE)
 #  define NIMBLE_CPP_ADDR_FMT "%02X%s%02X%s%02X%s%02X%s%02X%s%02X"
 # else
 #  define NIMBLE_CPP_ADDR_FMT "%02x%s%02x%s%02x%s%02x%s%02x%s%02x"

src/NimBLEAddress.h

@@ -21,10 +21,10 @@
 #include "syscfg/syscfg.h"
 #if CONFIG_BT_NIMBLE_ENABLED
 
-# if defined(CONFIG_NIMBLE_CPP_IDF)
-#  include "nimble/ble.h"
-# else
+# ifdef USING_NIMBLE_ARDUINO_HEADERS
 #  include "nimble/nimble/include/nimble/ble.h"
+# else
+#  include "nimble/ble.h"
 # endif
 
 /****  FIX COMPILATION ****/
@@ -63,8 +63,8 @@ class NimBLEAddress : private ble_addr_t {
     const NimBLEAddress& reverseByteOrder();
     bool                 operator==(const NimBLEAddress& rhs) const;
     bool                 operator!=(const NimBLEAddress& rhs) const;
-    operator std::string() const;
-    operator uint64_t() const;
+                         operator std::string() const;
+                         operator uint64_t() const;
 };
 
 #endif // CONFIG_BT_NIMBLE_ENABLED

src/NimBLEAdvertisedDevice.cpp

@@ -38,6 +38,7 @@ NimBLEAdvertisedDevice::NimBLEAdvertisedDevice(const ble_gap_event* event, uint8
       m_callbackSent{0},
       m_advLength{event->ext_disc.length_data},
       m_isLegacyAdv{!!(event->ext_disc.props & BLE_HCI_ADV_LEGACY_MASK)},
+      m_dataStatus{event->ext_disc.data_status},
       m_sid{event->ext_disc.sid},
       m_primPhy{event->ext_disc.prim_phy},
       m_secPhy{event->ext_disc.sec_phy},
@@ -51,6 +52,7 @@ NimBLEAdvertisedDevice::NimBLEAdvertisedDevice(const ble_gap_event* event, uint8
       m_advLength{event->disc.length_data},
       m_payload(event->disc.data, event->disc.data + event->disc.length_data) {
 # endif
+    m_pNextWaiting = this; // initialize sentinel: self-pointer means "not in list"
 } // NimBLEAdvertisedDevice
 
 /**
@@ -60,7 +62,16 @@ NimBLEAdvertisedDevice::NimBLEAdvertisedDevice(const ble_gap_event* event, uint8
 void NimBLEAdvertisedDevice::update(const ble_gap_event* event, uint8_t eventType) {
 # if MYNEWT_VAL(BLE_EXT_ADV)
     const auto& disc = event->ext_disc;
-    m_isLegacyAdv    = disc.props & BLE_HCI_ADV_LEGACY_MASK;
+    if (m_dataStatus == BLE_GAP_EXT_ADV_DATA_STATUS_INCOMPLETE) {
+        m_payload.reserve(m_advLength + disc.length_data);
+        m_payload.insert(m_payload.end(), disc.data, disc.data + disc.length_data);
+        m_dataStatus = disc.data_status;
+        m_advLength  = m_payload.size();
+        return;
+    }
+
+    m_dataStatus  = disc.data_status;
+    m_isLegacyAdv = disc.props & BLE_HCI_ADV_LEGACY_MASK;
 # else
     const auto& disc = event->disc;
 # endif
@@ -86,11 +97,11 @@ const NimBLEAddress& NimBLEAdvertisedDevice::getAddress() const {
 /**
  * @brief Get the advertisement type.
  * @return The advertising type the device is reporting:
- * * BLE_HCI_ADV_TYPE_ADV_IND            (0) - indirect advertising
- * * BLE_HCI_ADV_TYPE_ADV_DIRECT_IND_HD  (1) - direct advertising - high duty cycle
- * * BLE_HCI_ADV_TYPE_ADV_SCAN_IND       (2) - indirect scan response
- * * BLE_HCI_ADV_TYPE_ADV_NONCONN_IND    (3) - indirect advertising - not connectable
- * * BLE_HCI_ADV_TYPE_ADV_DIRECT_IND_LD  (4) - direct advertising - low duty cycle
+ * * BLE_HCI_ADV_RPT_EVTYPE_ADV_IND      (0) - indirect advertising - connectable and scannable
+ * * BLE_HCI_ADV_RPT_EVTYPE_DIR_IND      (1) - direct advertising - connectable
+ * * BLE_HCI_ADV_RPT_EVTYPE_SCAN_IND     (2) - indirect scan response - not connectable - scannable
+ * * BLE_HCI_ADV_RPT_EVTYPE_NONCONN_IND  (3) - beacon only - not connectable - not scannable
+ * * BLE_HCI_ADV_RPT_EVTYPE_SCAN_RSP     (4) - scan response
  */
 uint8_t NimBLEAdvertisedDevice::getAdvType() const {
     return m_advType;
@@ -617,6 +628,18 @@ uint8_t NimBLEAdvertisedDevice::getSecondaryPhy() const {
 uint16_t NimBLEAdvertisedDevice::getPeriodicInterval() const {
     return m_periodicItvl;
 } // getPeriodicInterval
+
+/**
+ * @brief Get the advertisement data status.
+ * @return The advertisement data status.
+ * One of:
+ * * BLE_GAP_EXT_ADV_DATA_STATUS_COMPLETE (0) - Complete extended advertising data
+ * * BLE_GAP_EXT_ADV_DATA_STATUS_INCOMPLETE (1) - Incomplete extended advertising data, more to come
+ * * BLE_GAP_EXT_ADV_DATA_STATUS_TRUNCATED (2) - Incomplete extended advertising data, no more to come
+ */
+uint8_t NimBLEAdvertisedDevice::getDataStatus() const {
+    return m_dataStatus;
+} // getDataStatus
 # endif
 
 uint8_t NimBLEAdvertisedDevice::findAdvField(uint8_t type, uint8_t index, size_t* data_loc) const {
@@ -730,7 +753,7 @@ std::string NimBLEAdvertisedDevice::toString() const {
  * @brief Get the length of the advertisement data in the payload.
  * @return The number of bytes in the payload that is from the advertisement.
  */
-uint8_t NimBLEAdvertisedDevice::getAdvLength() const {
+uint16_t NimBLEAdvertisedDevice::getAdvLength() const {
     return m_advLength;
 }
 
@@ -752,11 +775,12 @@ uint8_t NimBLEAdvertisedDevice::getAddressType() const {
  */
 bool NimBLEAdvertisedDevice::isConnectable() const {
 # if MYNEWT_VAL(BLE_EXT_ADV)
-    if (m_isLegacyAdv) {
-        return m_advType == BLE_HCI_ADV_RPT_EVTYPE_ADV_IND || m_advType == BLE_HCI_ADV_RPT_EVTYPE_DIR_IND;
+    if (!m_isLegacyAdv) {
+        return (m_advType & BLE_HCI_ADV_CONN_MASK) || (m_advType & BLE_HCI_ADV_DIRECT_MASK);
     }
 # endif
-    return (m_advType & BLE_HCI_ADV_CONN_MASK) || (m_advType & BLE_HCI_ADV_DIRECT_MASK);
+
+    return m_advType == BLE_HCI_ADV_RPT_EVTYPE_ADV_IND || m_advType == BLE_HCI_ADV_RPT_EVTYPE_DIR_IND;
 } // isConnectable
 
 /**

src/NimBLEAdvertisedDevice.h

@@ -25,12 +25,12 @@
 # include "NimBLEScan.h"
 # include "NimBLEUUID.h"
 
-# if defined(CONFIG_NIMBLE_CPP_IDF)
-#  include "host/ble_hs_adv.h"
-#  include "host/ble_gap.h"
-# else
+# ifdef USING_NIMBLE_ARDUINO_HEADERS
 #  include "nimble/nimble/host/include/host/ble_hs_adv.h"
 #  include "nimble/nimble/host/include/host/ble_gap.h"
+# else
+#  include "host/ble_hs_adv.h"
+#  include "host/ble_gap.h"
 # endif
 
 # include <vector>
@@ -69,7 +69,7 @@ class NimBLEAdvertisedDevice {
     NimBLEAddress        getTargetAddress(uint8_t index = 0) const;
     uint8_t              getTargetAddressCount() const;
     int8_t               getTXPower() const;
-    uint8_t              getAdvLength() const;
+    uint16_t             getAdvLength() const;
     uint8_t              getAddressType() const;
     bool                 isAdvertisingService(const NimBLEUUID& uuid) const;
     bool                 haveAppearance() const;
@@ -92,6 +92,7 @@ class NimBLEAdvertisedDevice {
     uint8_t  getPrimaryPhy() const;
     uint8_t  getSecondaryPhy() const;
     uint16_t getPeriodicInterval() const;
+    uint8_t  getDataStatus() const;
 # endif
     operator NimBLEAddress() const;
 
@@ -157,14 +158,17 @@ class NimBLEAdvertisedDevice {
     uint8_t findAdvField(uint8_t type, uint8_t index = 0, size_t* data_loc = nullptr) const;
     size_t  findServiceData(uint8_t index, uint8_t* bytes) const;
 
-    NimBLEAddress m_address{};
-    uint8_t       m_advType{};
-    int8_t        m_rssi{};
-    uint8_t       m_callbackSent{};
-    uint8_t       m_advLength{};
+    NimBLEAddress           m_address{};
+    uint8_t                 m_advType{};
+    int8_t                  m_rssi{};
+    uint8_t                 m_callbackSent{};
+    uint16_t                m_advLength{};
+    ble_npl_time_t          m_time{};
+    NimBLEAdvertisedDevice* m_pNextWaiting{}; // intrusive list node; self-pointer means "not in list", set in ctor
 
 # if MYNEWT_VAL(BLE_EXT_ADV)
     bool     m_isLegacyAdv{};
+    uint8_t  m_dataStatus{};
     uint8_t  m_sid{};
     uint8_t  m_primPhy{};
     uint8_t  m_secPhy{};

src/NimBLEAdvertisementData.cpp

@@ -23,10 +23,10 @@
 # include "NimBLEUUID.h"
 # include "NimBLELog.h"
 
-# if defined(CONFIG_NIMBLE_CPP_IDF)
-#  include "host/ble_hs_adv.h"
-# else
+#ifdef USING_NIMBLE_ARDUINO_HEADERS
 #  include "nimble/nimble/host/include/host/ble_hs_adv.h"
+# else
+#  include "host/ble_hs_adv.h"
 # endif
 
 static const char* LOG_TAG = "NimBLEAdvertisementData";

src/NimBLEAdvertising.cpp

@@ -18,11 +18,12 @@
 #include "NimBLEAdvertising.h"
 #if (CONFIG_BT_NIMBLE_ENABLED && MYNEWT_VAL(BLE_ROLE_BROADCASTER) && !MYNEWT_VAL(BLE_EXT_ADV)) || defined(_DOXYGEN_)
 
-# if defined(CONFIG_NIMBLE_CPP_IDF)
-#  include "services/gap/ble_svc_gap.h"
-# else
+#ifdef USING_NIMBLE_ARDUINO_HEADERS
 #  include "nimble/nimble/host/services/gap/include/services/gap/ble_svc_gap.h"
+# else
+#  include "services/gap/ble_svc_gap.h"
 # endif
+
 # include "NimBLEDevice.h"
 # include "NimBLEServer.h"
 # include "NimBLEUtils.h"
@@ -197,8 +198,9 @@ bool NimBLEAdvertising::start(uint32_t duration, const NimBLEAddress* dirAddr) {
 
 # if MYNEWT_VAL(BLE_ROLE_PERIPHERAL)
     NimBLEServer* pServer = NimBLEDevice::getServer();
-    if (pServer != nullptr) {
-        pServer->start(); // make sure the GATT server is ready before advertising
+    if (pServer != nullptr && !pServer->start()) { // make sure the GATT server is ready before advertising
+        NIMBLE_LOGE(LOG_TAG, "Failed to start GATT server");
+        return false;
     }
 # endif
 

src/NimBLEAdvertising.h

@@ -21,10 +21,10 @@
 #include "syscfg/syscfg.h"
 #if (CONFIG_BT_NIMBLE_ENABLED && MYNEWT_VAL(BLE_ROLE_BROADCASTER) && !MYNEWT_VAL(BLE_EXT_ADV)) || defined(_DOXYGEN_)
 
-# if defined(CONFIG_NIMBLE_CPP_IDF)
-#  include "host/ble_gap.h"
-# else
+#ifdef USING_NIMBLE_ARDUINO_HEADERS
 #  include "nimble/nimble/host/include/host/ble_gap.h"
+# else
+#  include "host/ble_gap.h"
 # endif
 
 /****  FIX COMPILATION ****/

src/NimBLEAttValue.cpp

@@ -18,10 +18,10 @@
 #include "NimBLEAttValue.h"
 #if CONFIG_BT_NIMBLE_ENABLED
 
-# if defined(CONFIG_NIMBLE_CPP_IDF)
-#  include "nimble/nimble_npl.h"
-# else
+# ifdef USING_NIMBLE_ARDUINO_HEADERS
 #  include "nimble/nimble/include/nimble/nimble_npl.h"
+# else
+#  include "nimble/nimble_npl.h"
 # endif
 
 # include "NimBLEUtils.h"
@@ -35,7 +35,7 @@ NimBLEAttValue::NimBLEAttValue(uint16_t init_len, uint16_t max_len)
       m_attr_max_len{std::min<uint16_t>(BLE_ATT_ATTR_MAX_LEN, max_len)},
       m_attr_len{},
       m_capacity{init_len}
-# if CONFIG_NIMBLE_CPP_ATT_VALUE_TIMESTAMP_ENABLED
+# if MYNEWT_VAL(NIMBLE_CPP_ATT_VALUE_TIMESTAMP_ENABLED)
       ,
       m_timestamp{}
 # endif
@@ -134,7 +134,7 @@ NimBLEAttValue& NimBLEAttValue::append(const uint8_t* value, uint16_t len) {
         return *this;
     }
 
-# if CONFIG_NIMBLE_CPP_ATT_VALUE_TIMESTAMP_ENABLED
+# if MYNEWT_VAL(NIMBLE_CPP_ATT_VALUE_TIMESTAMP_ENABLED)
     time_t t = time(nullptr);
 # else
     time_t t = 0;

src/NimBLEAttValue.h

@@ -21,8 +21,13 @@
 #include "syscfg/syscfg.h"
 #if CONFIG_BT_NIMBLE_ENABLED
 
-# ifdef NIMBLE_CPP_ARDUINO_STRING_AVAILABLE
-#  include <Arduino.h>
+/* Enables the use of Arduino String class for attribute values */
+# ifndef NIMBLE_CPP_ARDUINO_STRING_AVAILABLE
+#  define NIMBLE_CPP_ARDUINO_STRING_AVAILABLE (__has_include(<Arduino.h>))
+# endif
+
+# if NIMBLE_CPP_ARDUINO_STRING_AVAILABLE
+#  include <WString.h>
 # endif
 
 # include <string>
@@ -31,20 +36,30 @@
 # include <cstring>
 # include <cstdint>
 
-# ifndef CONFIG_NIMBLE_CPP_ATT_VALUE_TIMESTAMP_ENABLED
-#  define CONFIG_NIMBLE_CPP_ATT_VALUE_TIMESTAMP_ENABLED 0
+# ifndef MYNEWT_VAL_NIMBLE_CPP_ATT_VALUE_TIMESTAMP_ENABLED
+#  ifndef CONFIG_NIMBLE_CPP_ATT_VALUE_TIMESTAMP_ENABLED
+#   define MYNEWT_VAL_NIMBLE_CPP_ATT_VALUE_TIMESTAMP_ENABLED 0
+#  else
+#   define MYNEWT_VAL_NIMBLE_CPP_ATT_VALUE_TIMESTAMP_ENABLED CONFIG_NIMBLE_CPP_ATT_VALUE_TIMESTAMP_ENABLED
+#  endif
 # endif
 
 # ifndef BLE_ATT_ATTR_MAX_LEN
 #  define BLE_ATT_ATTR_MAX_LEN 512
 # endif
 
-# if !defined(CONFIG_NIMBLE_CPP_ATT_VALUE_INIT_LENGTH)
-#  define CONFIG_NIMBLE_CPP_ATT_VALUE_INIT_LENGTH 20
-# elif CONFIG_NIMBLE_CPP_ATT_VALUE_INIT_LENGTH > BLE_ATT_ATTR_MAX_LEN
-#  error CONFIG_NIMBLE_CPP_ATT_VALUE_INIT_LENGTH cannot be larger than 512 (BLE_ATT_ATTR_MAX_LEN)
-# elif CONFIG_NIMBLE_CPP_ATT_VALUE_INIT_LENGTH < 1
-#  error CONFIG_NIMBLE_CPP_ATT_VALUE_INIT_LENGTH cannot be less than 1; Range = 1 : 512
+# ifndef MYNEWT_VAL_NIMBLE_CPP_ATT_VALUE_INIT_LENGTH
+#  ifndef CONFIG_NIMBLE_CPP_ATT_VALUE_INIT_LENGTH
+#   define MYNEWT_VAL_NIMBLE_CPP_ATT_VALUE_INIT_LENGTH 20
+#  else
+#   define MYNEWT_VAL_NIMBLE_CPP_ATT_VALUE_INIT_LENGTH CONFIG_NIMBLE_CPP_ATT_VALUE_INIT_LENGTH
+#  endif
+# endif
+
+# if MYNEWT_VAL(NIMBLE_CPP_ATT_VALUE_INIT_LENGTH) > BLE_ATT_ATTR_MAX_LEN
+#  error NIMBLE_CPP_ATT_VALUE_INIT_LENGTH cannot be larger than 512 (BLE_ATT_ATTR_MAX_LEN)
+# elif MYNEWT_VAL(NIMBLE_CPP_ATT_VALUE_INIT_LENGTH) < 1
+#  error NIMBLE_CPP_ATT_VALUE_INIT_LENGTH cannot be less than 1; Range = 1 : 512
 # endif
 
 /* Used to determine if the type passed to a template has a data() and size() method. */
@@ -63,6 +78,14 @@ template <typename T>
 struct Has_c_str_length<T, decltype(void(std::declval<T&>().c_str())), decltype(void(std::declval<T&>().length()))>
     : std::true_type {};
 
+/* Used to determine if the type passed to a template has a value_type member (std::vector, std::array, std::string, etc.). */
+template <typename T, typename = void>
+struct Has_value_type : std::false_type {};
+
+template <typename T>
+struct Has_value_type<T, decltype(void(sizeof(typename T::value_type)))>
+    : std::true_type {};
+
 /**
  * @brief A specialized container class to hold BLE attribute values.
  * @details This class is designed to be more memory efficient than using\n
@@ -74,7 +97,7 @@ class NimBLEAttValue {
     uint16_t m_attr_max_len{};
     uint16_t m_attr_len{};
     uint16_t m_capacity{};
-# if CONFIG_NIMBLE_CPP_ATT_VALUE_TIMESTAMP_ENABLED
+# if MYNEWT_VAL(NIMBLE_CPP_ATT_VALUE_TIMESTAMP_ENABLED)
     time_t m_timestamp{};
 # endif
     void deepCopy(const NimBLEAttValue& source);
@@ -85,7 +108,7 @@ class NimBLEAttValue {
      * @param[in] init_len The initial size in bytes.
      * @param[in] max_len The max size in bytes that the value can be.
      */
-    NimBLEAttValue(uint16_t init_len = CONFIG_NIMBLE_CPP_ATT_VALUE_INIT_LENGTH, uint16_t max_len = BLE_ATT_ATTR_MAX_LEN);
+    NimBLEAttValue(uint16_t init_len = MYNEWT_VAL(NIMBLE_CPP_ATT_VALUE_INIT_LENGTH), uint16_t max_len = BLE_ATT_ATTR_MAX_LEN);
 
     /**
      * @brief Construct with an initial value from a buffer.
@@ -127,7 +150,7 @@ class NimBLEAttValue {
     NimBLEAttValue(const std::vector<uint8_t> vec, uint16_t max_len = BLE_ATT_ATTR_MAX_LEN)
         : NimBLEAttValue(&vec[0], vec.size(), max_len) {}
 
-# ifdef NIMBLE_CPP_ARDUINO_STRING_AVAILABLE
+# if NIMBLE_CPP_ARDUINO_STRING_AVAILABLE
     /**
      * @brief Construct with an initial value from an Arduino String.
      * @param str An Arduino String containing to the initial value to set.
@@ -170,7 +193,7 @@ class NimBLEAttValue {
     /** @brief Iterator end */
     const uint8_t* end() const { return m_attr_value + m_attr_len; }
 
-# if CONFIG_NIMBLE_CPP_ATT_VALUE_TIMESTAMP_ENABLED
+# if MYNEWT_VAL(NIMBLE_CPP_ATT_VALUE_TIMESTAMP_ENABLED)
     /** @brief Returns a timestamp of when the value was last updated */
     time_t getTimeStamp() const { return m_timestamp; }
 
@@ -210,7 +233,7 @@ class NimBLEAttValue {
 
     const NimBLEAttValue& getValue(time_t* timestamp = nullptr) const {
         if (timestamp != nullptr) {
-# if CONFIG_NIMBLE_CPP_ATT_VALUE_TIMESTAMP_ENABLED
+# if MYNEWT_VAL(NIMBLE_CPP_ATT_VALUE_TIMESTAMP_ENABLED)
             *timestamp = m_timestamp;
 # else
             *timestamp = 0;
@@ -230,6 +253,23 @@ class NimBLEAttValue {
     /*********************** Template Functions ************************/
 
 # if __cplusplus < 201703L
+    /**
+     * @brief Template to set value to the value of a char array using strnlen.
+     * @param [in] s A reference to a char array.
+     * @details Only used for char array types to correctly determine length via strnlen.
+     */
+    template <typename T>
+#  ifdef _DOXYGEN_
+    bool
+#  else
+    typename std::enable_if<std::is_array<T>::value &&
+                                std::is_same<typename std::remove_extent<T>::type, char>::value,
+                            bool>::type
+#  endif
+    setValue(const T& s) {
+        return setValue(reinterpret_cast<const uint8_t*>(s), strnlen(s, sizeof(T)));
+    }
+
     /**
      * @brief Template to set value to the value of <type\>val.
      * @param [in] v The <type\>value to set.
@@ -240,7 +280,10 @@ class NimBLEAttValue {
 #  ifdef _DOXYGEN_
     bool
 #  else
-    typename std::enable_if<!std::is_pointer<T>::value && !Has_c_str_length<T>::value && !Has_data_size<T>::value, bool>::type
+    typename std::enable_if<!std::is_pointer<T>::value && !Has_c_str_length<T>::value && !Has_data_size<T>::value &&
+                                !(std::is_array<T>::value &&
+                                  std::is_same<typename std::remove_extent<T>::type, char>::value),
+                            bool>::type
 #  endif
     setValue(const T& v) {
         return setValue(reinterpret_cast<const uint8_t*>(&v), sizeof(T));
@@ -264,13 +307,32 @@ class NimBLEAttValue {
     /**
      * @brief Template to set value to the value of <type\>val.
      * @param [in] v The <type\>value to set.
-     * @details Only used if the <type\> has a `data()` and `size()` method.
+     * @details Only used if the <type\> has a `data()` and `size()` method with `value_type`.
+     * Correctly calculates byte size for containers with multi-byte element types.
      */
     template <typename T>
 #  ifdef _DOXYGEN_
     bool
 #  else
-    typename std::enable_if<Has_data_size<T>::value, bool>::type
+    typename std::enable_if<Has_data_size<T>::value && Has_value_type<T>::value, bool>::type
+#  endif
+    setValue(const T& v) {
+        return setValue(
+            reinterpret_cast<const uint8_t*>(v.data()),
+            v.size() * sizeof(typename T::value_type)
+        );
+    }
+
+    /**
+     * @brief Template to set value to the value of <type\>val.
+     * @param [in] v The <type\>value to set.
+     * @details Only used if the <type\> has a `data()` and `size()` method without `value_type`.
+     */
+    template <typename T>
+#  ifdef _DOXYGEN_
+    bool
+#  else
+    typename std::enable_if<Has_data_size<T>::value && !Has_value_type<T>::value, bool>::type
 #  endif
     setValue(const T& v) {
         return setValue(reinterpret_cast<const uint8_t*>(v.data()), v.size());
@@ -285,9 +347,16 @@ class NimBLEAttValue {
     template <typename T>
     typename std::enable_if<!std::is_pointer<T>::value, bool>::type setValue(const T& s) {
         if constexpr (Has_data_size<T>::value) {
-            return setValue(reinterpret_cast<const uint8_t*>(s.data()), s.size());
+            if constexpr (Has_value_type<T>::value) {
+                return setValue(reinterpret_cast<const uint8_t*>(s.data()), s.size() * sizeof(typename T::value_type));
+            } else {
+                return setValue(reinterpret_cast<const uint8_t*>(s.data()), s.size());
+            }
         } else if constexpr (Has_c_str_length<T>::value) {
             return setValue(reinterpret_cast<const uint8_t*>(s.c_str()), s.length());
+        } else if constexpr (std::is_array<T>::value &&
+                             std::is_same<typename std::remove_extent<T>::type, char>::value) {
+            return setValue(reinterpret_cast<const uint8_t*>(s), strnlen(s, sizeof(s)));
         } else {
             return setValue(reinterpret_cast<const uint8_t*>(&s), sizeof(s));
         }
@@ -307,7 +376,7 @@ class NimBLEAttValue {
     template <typename T>
     T getValue(time_t* timestamp = nullptr, bool skipSizeCheck = false) const {
         if (timestamp != nullptr) {
-# if CONFIG_NIMBLE_CPP_ATT_VALUE_TIMESTAMP_ENABLED
+# if MYNEWT_VAL(NIMBLE_CPP_ATT_VALUE_TIMESTAMP_ENABLED)
             *timestamp = m_timestamp;
 # else
             *timestamp = 0;
@@ -357,7 +426,7 @@ class NimBLEAttValue {
     /** @brief Inequality operator */
     bool operator!=(const NimBLEAttValue& source) const { return !(*this == source); }
 
-# ifdef NIMBLE_CPP_ARDUINO_STRING_AVAILABLE
+# if NIMBLE_CPP_ARDUINO_STRING_AVAILABLE
     /** @brief Operator; Get the value as an Arduino String value. */
     operator String() const { return String(reinterpret_cast<char*>(m_attr_value)); }
 # endif

src/NimBLECharacteristic.cpp

@@ -15,15 +15,15 @@
  * limitations under the License.
  */
 
-# include "NimBLECharacteristic.h"
+#include "NimBLECharacteristic.h"
 #if CONFIG_BT_NIMBLE_ENABLED && MYNEWT_VAL(BLE_ROLE_PERIPHERAL)
 
-#if defined(CONFIG_NIMBLE_CPP_IDF)
-# if !defined(ESP_IDF_VERSION_MAJOR) || ESP_IDF_VERSION_MAJOR < 5
-#  define ble_gatts_notify_custom ble_gattc_notify_custom
-#  define ble_gatts_indicate_custom ble_gattc_indicate_custom
+# ifndef USING_NIMBLE_ARDUINO_HEADERS
+#  if !defined(ESP_IDF_VERSION_MAJOR) || ESP_IDF_VERSION_MAJOR < 5
+#   define ble_gatts_notify_custom   ble_gattc_notify_custom
+#   define ble_gatts_indicate_custom ble_gattc_indicate_custom
+#  endif
 # endif
-#endif
 
 # include "NimBLE2904.h"
 # include "NimBLEDevice.h"
@@ -132,7 +132,7 @@ void NimBLECharacteristic::addDescriptor(NimBLEDescriptor* pDescriptor) {
     }
 
     pDescriptor->setCharacteristic(this);
-    NimBLEDevice::getServer()->serviceChanged();
+    NimBLEDevice::getServer()->setServiceChanged();
 }
 
 /**
@@ -159,27 +159,33 @@ void NimBLECharacteristic::removeDescriptor(NimBLEDescriptor* pDescriptor, bool
     }
 
     pDescriptor->setRemoved(deleteDsc ? NIMBLE_ATT_REMOVE_DELETE : NIMBLE_ATT_REMOVE_HIDE);
-    NimBLEDevice::getServer()->serviceChanged();
+    NimBLEDevice::getServer()->setServiceChanged();
 } // removeDescriptor
 
 /**
  * @brief Return the BLE Descriptor for the given UUID.
  * @param [in] uuid The UUID of the descriptor.
+ * @param [in] index The index of the descriptor to return (used when multiple descriptors have the same UUID).
  * @return A pointer to the descriptor object or nullptr if not found.
  */
-NimBLEDescriptor* NimBLECharacteristic::getDescriptorByUUID(const char* uuid) const {
-    return getDescriptorByUUID(NimBLEUUID(uuid));
+NimBLEDescriptor* NimBLECharacteristic::getDescriptorByUUID(const char* uuid, uint16_t index) const {
+    return getDescriptorByUUID(NimBLEUUID(uuid), index);
 } // getDescriptorByUUID
 
 /**
  * @brief Return the BLE Descriptor for the given UUID.
  * @param [in] uuid The UUID of the descriptor.
+ * @param [in] index The index of the descriptor to return (used when multiple descriptors have the same UUID).
  * @return A pointer to the descriptor object or nullptr if not found.
  */
-NimBLEDescriptor* NimBLECharacteristic::getDescriptorByUUID(const NimBLEUUID& uuid) const {
+NimBLEDescriptor* NimBLECharacteristic::getDescriptorByUUID(const NimBLEUUID& uuid, uint16_t index) const {
+    uint16_t position = 0;
     for (const auto& dsc : m_vDescriptors) {
         if (dsc->getUUID() == uuid) {
-            return dsc;
+            if (position == index) {
+                return dsc;
+            }
+            position++;
         }
     }
     return nullptr;
@@ -225,7 +231,8 @@ void NimBLECharacteristic::setService(NimBLEService* pService) {
  * @return True if the indication was sent successfully, false otherwise.
  */
 bool NimBLECharacteristic::indicate(uint16_t connHandle) const {
-    return sendValue(nullptr, 0, false, connHandle);
+    auto value{m_value}; // make a copy to avoid issues if the value is changed while indicating
+    return sendValue(value.data(), value.size(), false, connHandle);
 } // indicate
 
 /**
@@ -247,7 +254,8 @@ bool NimBLECharacteristic::indicate(const uint8_t* value, size_t length, uint16_
  * @return True if the notification was sent successfully, false otherwise.
  */
 bool NimBLECharacteristic::notify(uint16_t connHandle) const {
-    return sendValue(nullptr, 0, true, connHandle);
+    auto value{m_value}; // make a copy to avoid issues if the value is changed while notifying
+    return sendValue(value.data(), value.size(), true, connHandle);
 } // notify
 
 /**
@@ -271,55 +279,50 @@ bool NimBLECharacteristic::notify(const uint8_t* value, size_t length, uint16_t
  * @return True if the value was sent successfully, false otherwise.
  */
 bool NimBLECharacteristic::sendValue(const uint8_t* value, size_t length, bool isNotification, uint16_t connHandle) const {
-    int rc = 0;
+    ble_npl_hw_enter_critical();
+    const auto subs = getSubscribers(); // make a copy to avoid issues if subscribers change while sending
+    ble_npl_hw_exit_critical(0);
 
-    if (value != nullptr && length > 0) { // custom notification value
-        os_mbuf* om = nullptr;
+    bool chSpecified = connHandle != BLE_HS_CONN_HANDLE_NONE;
+    bool requireSecure = m_properties & (BLE_GATT_CHR_F_READ_ENC | BLE_GATT_CHR_F_READ_AUTHEN | BLE_GATT_CHR_F_READ_AUTHOR);
+    int rc = chSpecified ? BLE_HS_ENOENT : 0; // if handle specified, assume not found until sent
 
-        if (connHandle != BLE_HS_CONN_HANDLE_NONE) { // only sending to specific peer
-            om = ble_hs_mbuf_from_flat(value, length);
-            if (!om) {
-                rc = BLE_HS_ENOMEM;
-                goto done;
-            }
-
-            // Null buffer will read the value from the characteristic
-            if (isNotification) {
-                rc = ble_gatts_notify_custom(connHandle, m_handle, om);
-            } else {
-                rc = ble_gatts_indicate_custom(connHandle, m_handle, om);
-            }
-
-            goto done;
+    // Notify all connected peers unless a specific handle is provided
+    for (const auto& entry : subs) {
+        uint16_t ch = entry.getConnHandle();
+        if (ch == BLE_HS_CONN_HANDLE_NONE || (chSpecified && ch != connHandle)) {
+            continue;
         }
 
-        // Notify all connected peers unless a specific handle is provided
-        for (const auto& ch : NimBLEDevice::getServer()->getPeerDevices()) {
-            // Must re-create the data buffer on each iteration because it is freed by the calls bellow.
-            om = ble_hs_mbuf_from_flat(value, length);
-            if (!om) {
-                rc = BLE_HS_ENOMEM;
-                goto done;
-            }
-
-            if (isNotification) {
-                rc = ble_gatts_notify_custom(ch, m_handle, om);
-            } else {
-                rc = ble_gatts_indicate_custom(ch, m_handle, om);
-            }
+        if (requireSecure && !entry.isSecured()) {
+            NIMBLE_LOGW(LOG_TAG, "skipping notify/indicate to connHandle=%d, link not secured", entry.getConnHandle());
+            continue;
         }
-    } else if (connHandle != BLE_HS_CONN_HANDLE_NONE) {
-        // Null buffer will read the value from the characteristic
+
+        // Must re-create the data buffer on each iteration because it is freed by the calls below.
+        uint8_t  retries = 10; // wait up to 10ms for a free buffer
+        os_mbuf* om      = ble_hs_mbuf_from_flat(value, length);
+        while (!om && --retries) {
+            ble_npl_time_delay(ble_npl_time_ms_to_ticks32(1));
+            om = ble_hs_mbuf_from_flat(value, length);
+        }
+
+        if (!om) {
+            rc = BLE_HS_ENOMEM;
+            break;
+        }
+
         if (isNotification) {
-            rc = ble_gatts_notify_custom(connHandle, m_handle, nullptr);
+            rc = ble_gatts_notify_custom(ch, m_handle, om);
         } else {
-            rc = ble_gatts_indicate_custom(connHandle, m_handle, nullptr);
+            rc = ble_gatts_indicate_custom(ch, m_handle, om);
+        }
+
+        if (rc != 0 || chSpecified) {
+            break;
         }
-    } else { // Notify or indicate to all connected peers the characteristic value
-        ble_gatts_chr_updated(m_handle);
     }
 
-done:
     if (rc != 0) {
         NIMBLE_LOGE(LOG_TAG, "failed to send value, rc=%d %s", rc, NimBLEUtils::returnCodeToString(rc));
         return false;
@@ -328,10 +331,107 @@ done:
     return true;
 } // sendValue
 
+/**
+ * @brief Process a subscription or unsubscription request from a peer.
+ * @param[in] connInfo A reference to the connection info of the peer.
+ * @param[in] subVal The subscription value (bitmask).
+ */
+void NimBLECharacteristic::processSubRequest(NimBLEConnInfo& connInfo, uint8_t subVal) const {
+    // Only allocate subscribers for characteristics that support notify or indicate.
+    const uint16_t props = getProperties();
+    if (!(props & (BLE_GATT_CHR_F_NOTIFY | BLE_GATT_CHR_F_INDICATE))) {
+        return;
+    }
+
+    auto found     = false;
+    auto firstFree = -1;
+    for (auto& entry : m_subPeers) {
+        if (entry.getConnHandle() == connInfo.getConnHandle()) {
+            found = true;
+            if (!subVal) {
+                ble_npl_hw_enter_critical();
+                entry = SubPeerEntry{}; // unsubscribed, reset entry
+                ble_npl_hw_exit_critical(0);
+            }
+            break;
+        }
+
+        if (firstFree == -1 && entry.getConnHandle() == BLE_HS_CONN_HANDLE_NONE) {
+            firstFree = &entry - &m_subPeers[0];
+        }
+    }
+
+    if (!found && subVal) {
+        if (firstFree >= 0) {
+            ble_npl_hw_enter_critical();
+            m_subPeers[firstFree].setConnHandle(connInfo.getConnHandle());
+            m_subPeers[firstFree].setSubNotify(subVal & 0x1);
+            m_subPeers[firstFree].setSubIndicate(subVal & 0x2);
+            m_subPeers[firstFree].setSecured(connInfo.isEncrypted() || connInfo.isAuthenticated() || connInfo.isBonded());
+            if (m_properties & (BLE_GATT_CHR_F_READ_AUTHEN | BLE_GATT_CHR_F_READ_AUTHOR | BLE_GATT_CHR_F_READ_ENC)) {
+                // characteristic requires security/authorization
+                if (!m_subPeers[firstFree].isSecured()) {
+                    m_subPeers[firstFree].setAwaitingSecure(true);
+                    ble_npl_hw_exit_critical(0);
+                    NimBLEDevice::startSecurity(connInfo.getConnHandle());
+                    NIMBLE_LOGD(LOG_TAG,
+                                "Subscription deferred until link is secured for connHandle=%d",
+                                connInfo.getConnHandle());
+                    return;
+                }
+            }
+            ble_npl_hw_exit_critical(0);
+        } else {
+            // should never happen, but log just in case
+            NIMBLE_LOGE(LOG_TAG, "No free subscription slots");
+            return;
+        }
+    }
+
+    m_pCallbacks->onSubscribe(const_cast<NimBLECharacteristic*>(this), const_cast<NimBLEConnInfo&>(connInfo), subVal);
+}
+
+/**
+ * @brief Update the security status of a subscribed peer.
+ * @param[in] peerInfo A reference to the connection info of the peer.
+ */
+void NimBLECharacteristic::updatePeerStatus(const NimBLEConnInfo& peerInfo) const {
+    if (!(getProperties() & (NIMBLE_PROPERTY::NOTIFY | NIMBLE_PROPERTY::INDICATE))) {
+        return;
+    }
+
+    ble_npl_hw_enter_critical();
+    for (auto& entry : m_subPeers) {
+        if (entry.getConnHandle() == peerInfo.getConnHandle()) {
+            entry.setSecured(peerInfo.isEncrypted() || peerInfo.isAuthenticated() || peerInfo.isBonded());
+            if (entry.isAwaitingSecure()) {
+                entry.setAwaitingSecure(false);
+                ble_npl_hw_exit_critical(0);
+                m_pCallbacks->onSubscribe(const_cast<NimBLECharacteristic*>(this),
+                                          const_cast<NimBLEConnInfo&>(peerInfo),
+                                          entry.isSubNotify() | (entry.isSubIndicate() << 1));
+                return;
+            }
+            break;
+        }
+    }
+    ble_npl_hw_exit_critical(0);
+}
+
+/**
+ * @brief Handle a read event from a client.
+ * @param [in] connInfo A reference to a NimBLEConnInfo instance containing the peer info.
+ */
 void NimBLECharacteristic::readEvent(NimBLEConnInfo& connInfo) {
     m_pCallbacks->onRead(this, connInfo);
 } // readEvent
 
+/**
+ * @brief Handle a write event from a client.
+ * @param [in] val A pointer to the data written by the client.
+ * @param [in] len The length of the data written by the client.
+ * @param [in] connInfo A reference to a NimBLEConnInfo instance containing the peer info.
+ */
 void NimBLECharacteristic::writeEvent(const uint8_t* val, uint16_t len, NimBLEConnInfo& connInfo) {
     setValue(val, len);
     m_pCallbacks->onWrite(this, connInfo);
@@ -405,6 +505,18 @@ void NimBLECharacteristicCallbacks::onStatus(NimBLECharacteristic* pCharacterist
     NIMBLE_LOGD("NimBLECharacteristicCallbacks", "onStatus: default");
 } // onStatus
 
+/**
+ * @brief Callback function to support a Notify/Indicate Status report.
+ * @param [in] pCharacteristic The characteristic that is the source of the event.
+ * @param [in] connInfo A reference to a NimBLEConnInfo instance containing the peer info.
+ * @param [in] code Status return code from the NimBLE stack.
+ * @details The status code for success is 0 for notifications and BLE_HS_EDONE for indications,
+ * any other value is an error.
+ */
+void NimBLECharacteristicCallbacks::onStatus(NimBLECharacteristic* pCharacteristic, NimBLEConnInfo& connInfo, int code) {
+    NIMBLE_LOGD("NimBLECharacteristicCallbacks", "onStatus: default");
+} // onStatus
+
 /**
  * @brief Callback function called when a client changes subscription status.
  * @param [in] pCharacteristic The characteristic that is the source of the event.

src/NimBLECharacteristic.h

@@ -31,6 +31,7 @@ class NimBLE2904;
 
 # include <string>
 # include <vector>
+# include <array>
 
 /**
  * @brief The model of a BLE Characteristic.
@@ -68,8 +69,8 @@ class NimBLECharacteristic : public NimBLELocalValueAttribute {
                                        uint32_t          properties = NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::WRITE,
                                        uint16_t          maxLen     = BLE_ATT_ATTR_MAX_LEN);
     NimBLE2904*       create2904();
-    NimBLEDescriptor* getDescriptorByUUID(const char* uuid) const;
-    NimBLEDescriptor* getDescriptorByUUID(const NimBLEUUID& uuid) const;
+    NimBLEDescriptor* getDescriptorByUUID(const char* uuid, uint16_t index = 0) const;
+    NimBLEDescriptor* getDescriptorByUUID(const NimBLEUUID& uuid, uint16_t index = 0) const;
     NimBLEDescriptor* getDescriptorByHandle(uint16_t handle) const;
     NimBLEService*    getService() const;
 
@@ -112,7 +113,23 @@ class NimBLECharacteristic : public NimBLELocalValueAttribute {
     }
 
     /**
-     * @brief Template to send a notification with a value from a class that has a data() and size() method.
+     * @brief Template to send a notification with a value from a class that has a data() and size() method with value_type.
+     * @param [in] v The value to send.
+     * @param [in] connHandle Optional, a connection handle to send the notification to.
+     * @details Correctly calculates byte size for containers with multi-byte element types.
+     */
+    template <typename T>
+#  ifdef _DOXYGEN_
+    bool
+#  else
+    typename std::enable_if<Has_data_size<T>::value && Has_value_type<T>::value, bool>::type
+#  endif
+    notify(const T& v, uint16_t connHandle = BLE_HS_CONN_HANDLE_NONE) const {
+        return notify(reinterpret_cast<const uint8_t*>(v.data()), v.size() * sizeof(typename T::value_type), connHandle);
+    }
+
+    /**
+     * @brief Template to send a notification with a value from a class that has a data() and size() method without value_type.
      * @param [in] v The value to send.
      * @param [in] connHandle Optional, a connection handle to send the notification to.
      */
@@ -120,7 +137,7 @@ class NimBLECharacteristic : public NimBLELocalValueAttribute {
 #  ifdef _DOXYGEN_
     bool
 #  else
-    typename std::enable_if<Has_data_size<T>::value, bool>::type
+    typename std::enable_if<Has_data_size<T>::value && !Has_value_type<T>::value, bool>::type
 #  endif
     notify(const T& v, uint16_t connHandle = BLE_HS_CONN_HANDLE_NONE) const {
         return notify(reinterpret_cast<const uint8_t*>(v.data()), v.size(), connHandle);
@@ -160,7 +177,23 @@ class NimBLECharacteristic : public NimBLELocalValueAttribute {
     }
 
     /**
-     * @brief Template to send a indication with a value from a class that has a data() and size() method.
+     * @brief Template to send a indication with a value from a class that has a data() and size() method with value_type.
+     * @param [in] v The value to send.
+     * @param [in] connHandle Optional, a connection handle to send the notification to.
+     * @details Correctly calculates byte size for containers with multi-byte element types.
+     */
+    template <typename T>
+#  ifdef _DOXYGEN_
+    bool
+#  else
+    typename std::enable_if<Has_data_size<T>::value && Has_value_type<T>::value, bool>::type
+#  endif
+    indicate(const T& v, uint16_t connHandle = BLE_HS_CONN_HANDLE_NONE) const {
+        return indicate(reinterpret_cast<const uint8_t*>(v.data()), v.size() * sizeof(typename T::value_type), connHandle);
+    }
+
+    /**
+     * @brief Template to send a indication with a value from a class that has a data() and size() method without value_type.
      * @param [in] v The value to send.
      * @param [in] connHandle Optional, a connection handle to send the notification to.
      */
@@ -168,7 +201,7 @@ class NimBLECharacteristic : public NimBLELocalValueAttribute {
 #  ifdef _DOXYGEN_
     bool
 #  else
-    typename std::enable_if<Has_data_size<T>::value, bool>::type
+    typename std::enable_if<Has_data_size<T>::value && !Has_value_type<T>::value, bool>::type
 #  endif
     indicate(const T& v, uint16_t connHandle = BLE_HS_CONN_HANDLE_NONE) const {
         return indicate(reinterpret_cast<const uint8_t*>(v.data()), v.size(), connHandle);
@@ -190,7 +223,13 @@ class NimBLECharacteristic : public NimBLELocalValueAttribute {
     typename std::enable_if<!std::is_pointer<T>::value && !std::is_array<T>::value, bool>::type notify(
         const T& value, uint16_t connHandle = BLE_HS_CONN_HANDLE_NONE) const {
         if constexpr (Has_data_size<T>::value) {
-            return notify(reinterpret_cast<const uint8_t*>(value.data()), value.size(), connHandle);
+            if constexpr (Has_value_type<T>::value) {
+                return notify(reinterpret_cast<const uint8_t*>(value.data()),
+                              value.size() * sizeof(typename T::value_type),
+                              connHandle);
+            } else {
+                return notify(reinterpret_cast<const uint8_t*>(value.data()), value.size(), connHandle);
+            }
         } else if constexpr (Has_c_str_length<T>::value) {
             return notify(reinterpret_cast<const uint8_t*>(value.c_str()), value.length(), connHandle);
         } else {
@@ -212,7 +251,13 @@ class NimBLECharacteristic : public NimBLELocalValueAttribute {
     typename std::enable_if<!std::is_pointer<T>::value && !std::is_array<T>::value, bool>::type indicate(
         const T& value, uint16_t connHandle = BLE_HS_CONN_HANDLE_NONE) const {
         if constexpr (Has_data_size<T>::value) {
-            return indicate(reinterpret_cast<const uint8_t*>(value.data()), value.size(), connHandle);
+            if constexpr (Has_value_type<T>::value) {
+                return indicate(reinterpret_cast<const uint8_t*>(value.data()),
+                                value.size() * sizeof(typename T::value_type),
+                                connHandle);
+            } else {
+                return indicate(reinterpret_cast<const uint8_t*>(value.data()), value.size(), connHandle);
+            }
         } else if constexpr (Has_c_str_length<T>::value) {
             return indicate(reinterpret_cast<const uint8_t*>(value.c_str()), value.length(), connHandle);
         } else {
@@ -233,9 +278,33 @@ class NimBLECharacteristic : public NimBLELocalValueAttribute {
                    bool           is_notification = true,
                    uint16_t       connHandle      = BLE_HS_CONN_HANDLE_NONE) const;
 
+    struct SubPeerEntry {
+        enum : uint8_t { AWAITING_SECURE = 1 << 0, SECURE = 1 << 1, SUB_NOTIFY = 1 << 2, SUB_INDICATE = 1 << 3 };
+        void     setConnHandle(uint16_t connHandle) { m_connHandle = connHandle; }
+        uint16_t getConnHandle() const { return m_connHandle; }
+        void setAwaitingSecure(bool awaiting) { awaiting ? m_flags |= AWAITING_SECURE : m_flags &= ~AWAITING_SECURE; }
+        void setSecured(bool secure) { secure ? m_flags |= SECURE : m_flags &= ~SECURE; }
+        void setSubNotify(bool notify) { notify ? m_flags |= SUB_NOTIFY : m_flags &= ~SUB_NOTIFY; }
+        void setSubIndicate(bool indicate) { indicate ? m_flags |= SUB_INDICATE : m_flags &= ~SUB_INDICATE; }
+        bool isSubNotify() const { return m_flags & SUB_NOTIFY; }
+        bool isSubIndicate() const { return m_flags & SUB_INDICATE; }
+        bool isSecured() const { return m_flags & SECURE; }
+        bool isAwaitingSecure() const { return m_flags & AWAITING_SECURE; }
+
+      private:
+        uint16_t m_connHandle{BLE_HS_CONN_HANDLE_NONE};
+        uint8_t  m_flags{0};
+    } __attribute__((packed));
+
+    using SubPeerArray = std::array<SubPeerEntry, MYNEWT_VAL(BLE_MAX_CONNECTIONS)>;
+    SubPeerArray getSubscribers() const { return m_subPeers; }
+    void         processSubRequest(NimBLEConnInfo& connInfo, uint8_t subVal) const;
+    void         updatePeerStatus(const NimBLEConnInfo& peerInfo) const;
+
     NimBLECharacteristicCallbacks* m_pCallbacks{nullptr};
     NimBLEService*                 m_pService{nullptr};
     std::vector<NimBLEDescriptor*> m_vDescriptors{};
+    mutable SubPeerArray           m_subPeers{};
 }; // NimBLECharacteristic
 
 /**
@@ -250,7 +319,8 @@ class NimBLECharacteristicCallbacks {
     virtual ~NimBLECharacteristicCallbacks() {}
     virtual void onRead(NimBLECharacteristic* pCharacteristic, NimBLEConnInfo& connInfo);
     virtual void onWrite(NimBLECharacteristic* pCharacteristic, NimBLEConnInfo& connInfo);
-    virtual void onStatus(NimBLECharacteristic* pCharacteristic, int code);
+    virtual void onStatus(NimBLECharacteristic* pCharacteristic, int code); // deprecated
+    virtual void onStatus(NimBLECharacteristic* pCharacteristic, NimBLEConnInfo& connInfo, int code);
     virtual void onSubscribe(NimBLECharacteristic* pCharacteristic, NimBLEConnInfo& connInfo, uint16_t subValue);
 };
 

src/NimBLEClient.cpp

@@ -23,10 +23,10 @@
 # include "NimBLEDevice.h"
 # include "NimBLELog.h"
 
-# if defined(CONFIG_NIMBLE_CPP_IDF)
-#  include "nimble/nimble_port.h"
-# else
+# ifdef USING_NIMBLE_ARDUINO_HEADERS
 #  include "nimble/porting/nimble/include/nimble/nimble_port.h"
+# else
+#  include "nimble/nimble_port.h"
 # endif
 
 # include <climits>
@@ -34,6 +34,12 @@
 static const char*           LOG_TAG = "NimBLEClient";
 static NimBLEClientCallbacks defaultCallbacks;
 
+namespace {
+constexpr inline uint32_t connIntervalToMs(uint16_t interval) {
+    return (static_cast<uint32_t>(interval) * 5U) / 4U;
+} // connIntervalToMs
+} // namespace
+
 /*
  * Design
  * ------
@@ -67,7 +73,9 @@ NimBLEClient::NimBLEClient(const NimBLEAddress& peerAddress)
       m_connHandle{BLE_HS_CONN_HANDLE_NONE},
       m_terminateFailCount{0},
       m_asyncSecureAttempt{0},
-      m_config{},
+      m_connStatus{DISCONNECTED},
+      m_connectCallbackPending{false},
+      m_connectFailRetryCount{0},
 # if MYNEWT_VAL(BLE_EXT_ADV)
       m_phyMask{BLE_GAP_LE_PHY_1M_MASK | BLE_GAP_LE_PHY_2M_MASK | BLE_GAP_LE_PHY_CODED_MASK},
 # endif
@@ -79,6 +87,10 @@ NimBLEClient::NimBLEClient(const NimBLEAddress& peerAddress)
                    BLE_GAP_INITIAL_SUPERVISION_TIMEOUT,
                    BLE_GAP_INITIAL_CONN_MIN_CE_LEN,
                    BLE_GAP_INITIAL_CONN_MAX_CE_LEN} {
+    ble_npl_callout_init(&m_connectEstablishedTimer,
+                         nimble_port_get_dflt_eventq(),
+                         NimBLEClient::connectEstablishedTimerCb,
+                         this);
 } // NimBLEClient
 
 /**
@@ -86,6 +98,9 @@ NimBLEClient::NimBLEClient(const NimBLEAddress& peerAddress)
  * to ensure proper disconnect and removal from device list.
  */
 NimBLEClient::~NimBLEClient() {
+    ble_npl_callout_stop(&m_connectEstablishedTimer);
+    ble_npl_callout_deinit(&m_connectEstablishedTimer);
+
     // We may have allocated service references associated with this client.
     // Before we are finished with the client, we must release resources.
     deleteServices();
@@ -158,50 +173,8 @@ bool NimBLEClient::connect(bool deleteAttributes, bool asyncConnect, bool exchan
     return connect(m_peerAddress, deleteAttributes, asyncConnect, exchangeMTU);
 } // connect
 
-/**
- * @brief Connect to a BLE Server by address.
- * @param [in] address The address of the server.
- * @param [in] deleteAttributes If true this will delete any attribute objects this client may already\n
- * have created when last connected.
- * @param [in] asyncConnect If true, the connection will be made asynchronously and this function will return immediately.\n
- * If false, this function will block until the connection is established or the connection attempt times out.
- * @param [in] exchangeMTU If true, the client will attempt to exchange MTU with the server after connection.\n
- * If false, the client will use the default MTU size and the application will need to call exchangeMTU() later.
- * @return true on success.
- */
-bool NimBLEClient::connect(const NimBLEAddress& address, bool deleteAttributes, bool asyncConnect, bool exchangeMTU) {
-    NIMBLE_LOGD(LOG_TAG, ">> connect(%s)", address.toString().c_str());
-
-    if (!NimBLEDevice::m_synced) {
-        NIMBLE_LOGE(LOG_TAG, "Host reset, wait for sync.");
-        return false;
-    }
-
-    if (isConnected()) {
-        NIMBLE_LOGE(LOG_TAG, "Client already connected");
-        return false;
-    }
-
-    const ble_addr_t* peerAddr = address.getBase();
-    if (ble_gap_conn_find_by_addr(peerAddr, NULL) == 0) {
-        NIMBLE_LOGE(LOG_TAG, "A connection to %s already exists", address.toString().c_str());
-        return false;
-    }
-
-    if (address.isNull()) {
-        NIMBLE_LOGE(LOG_TAG, "Invalid peer address; (NULL)");
-        return false;
-    } else {
-        m_peerAddress = address;
-    }
-
-    if (deleteAttributes) {
-        deleteServices();
-    }
-
-    int rc                = 0;
-    m_config.asyncConnect = asyncConnect;
-    m_config.exchangeMTU  = exchangeMTU;
+int NimBLEClient::startConnectionAttempt(const ble_addr_t* peerAddr) {
+    int rc = 0;
 
     do {
 # if MYNEWT_VAL(BLE_EXT_ADV)
@@ -259,25 +232,73 @@ bool NimBLEClient::connect(const NimBLEAddress& address, bool deleteAttributes,
 
     } while (rc == BLE_HS_EBUSY);
 
+    return rc;
+}
+
+/**
+ * @brief Connect to a BLE Server by address.
+ * @param [in] address The address of the server.
+ * @param [in] deleteAttributes If true this will delete any attribute objects this client may already\n
+ * have created when last connected.
+ * @param [in] asyncConnect If true, the connection will be made asynchronously and this function will return immediately.\n
+ * If false, this function will block until the connection is established or the connection attempt times out.
+ * @param [in] exchangeMTU If true, the client will attempt to exchange MTU with the server after connection.\n
+ * If false, the client will use the default MTU size and the application will need to call exchangeMTU() later.
+ * @return true on success.
+ */
+bool NimBLEClient::connect(const NimBLEAddress& address, bool deleteAttributes, bool asyncConnect, bool exchangeMTU) {
+    NIMBLE_LOGD(LOG_TAG, ">> connect(%s)", address.toString().c_str());
+    NimBLETaskData    taskData(this);
+    const ble_addr_t* peerAddr = address.getBase();
+    int               rc       = 0;
+
+    if (!NimBLEDevice::m_synced) {
+        NIMBLE_LOGE(LOG_TAG, "Host not synced with controller.");
+        rc = BLE_HS_ENOTSYNCED;
+        goto error;
+    }
+
+    if (m_connStatus != DISCONNECTED) {
+        NIMBLE_LOGE(LOG_TAG, "Client not disconnected, cannot connect");
+        rc = BLE_HS_EREJECT;
+        goto error;
+    }
+
+    if (address.isNull()) {
+        NIMBLE_LOGE(LOG_TAG, "Invalid peer address; (NULL)");
+        rc = BLE_HS_EINVAL;
+        goto error;
+    }
+
+    m_connStatus             = CONNECTING;
+    m_peerAddress            = address;
+    m_config.asyncConnect    = asyncConnect;
+    m_config.exchangeMTU     = exchangeMTU;
+    m_connectCallbackPending = false;
+    m_connectFailRetryCount  = 0;
+
+    rc = startConnectionAttempt(peerAddr);
+
+    if (deleteAttributes) {
+        deleteServices();
+    }
+
     if (rc != 0) {
-        m_lastErr = rc;
-        return false;
+        goto error;
     }
 
     if (m_config.asyncConnect) {
         return true;
     }
 
-    NimBLETaskData taskData(this);
     m_pTaskData = &taskData;
 
-    // Wait for the connect timeout time +1 second for the connection to complete
-    if (!NimBLEUtils::taskWait(taskData, m_connectTimeout + 1000)) {
-        // If a connection was made but no response from MTU exchange proceed anyway
-        if (isConnected()) {
-            taskData.m_flags = 0;
-        } else {
-            // workaround; if the controller doesn't cancel the connection at the timeout, cancel it here.
+    // Wait for the connect timeout time +retry time * retries for the connection to complete
+    if (!NimBLEUtils::taskWait(
+            taskData,
+            (m_connectTimeout + connIntervalToMs(m_connParams.itvl_max) * 7) * (m_config.connectFailRetries + 1U))) {
+        if (m_connStatus != CONNECTED) {
+            // 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();
             taskData.m_flags = BLE_HS_ETIMEOUT;
@@ -288,17 +309,19 @@ bool NimBLEClient::connect(const NimBLEAddress& address, bool deleteAttributes,
     rc          = taskData.m_flags;
     if (rc != 0) {
         NIMBLE_LOGE(LOG_TAG, "Connection failed; status=%d %s", rc, NimBLEUtils::returnCodeToString(rc));
-        m_lastErr = rc;
-        if (m_config.deleteOnConnectFail) {
-            NimBLEDevice::deleteClient(this);
-        }
-        return false;
+        goto error;
     }
 
-    m_pClientCallbacks->onConnect(this);
     NIMBLE_LOGD(LOG_TAG, "<< connect()");
-    // Check if still connected before returning
-    return isConnected();
+    return true;
+
+error:
+    m_connStatus = DISCONNECTED;
+    m_lastErr    = rc;
+    if (m_config.deleteOnConnectFail) {
+        NimBLEDevice::deleteClient(this);
+    }
+    return false;
 } // connect
 
 /**
@@ -331,7 +354,7 @@ bool NimBLEClient::secureConnection(bool async) const {
         if (NimBLEDevice::startSecurity(m_connHandle)) {
             NimBLEUtils::taskWait(taskData, BLE_NPL_TIME_FOREVER);
         }
-    } while (taskData.m_flags == (BLE_HS_ERR_HCI_BASE + BLE_ERR_PINKEY_MISSING) && retryCount--);
+    } while (taskData.m_flags == BLE_HS_HCI_ERR(BLE_ERR_PINKEY_MISSING) && retryCount--);
 
     m_pTaskData = nullptr;
 
@@ -341,7 +364,10 @@ bool NimBLEClient::secureConnection(bool async) const {
     }
 
     m_lastErr = taskData.m_flags;
-    NIMBLE_LOGE(LOG_TAG, "secureConnection: failed rc=%d", taskData.m_flags);
+    NIMBLE_LOGE(LOG_TAG,
+                "secureConnection: failed rc=%d %s",
+                taskData.m_flags,
+                NimBLEUtils::returnCodeToString(taskData.m_flags));
     return false;
 
 } // secureConnection
@@ -352,13 +378,19 @@ bool NimBLEClient::secureConnection(bool async) const {
  */
 bool NimBLEClient::disconnect(uint8_t reason) {
     int rc = ble_gap_terminate(m_connHandle, reason);
-    if (rc != 0 && rc != BLE_HS_ENOTCONN && rc != BLE_HS_EALREADY) {
-        NIMBLE_LOGE(LOG_TAG, "ble_gap_terminate failed: rc=%d %s", rc, NimBLEUtils::returnCodeToString(rc));
-        m_lastErr = rc;
-        return false;
+    switch (rc) {
+        case 0:
+            m_connStatus = DISCONNECTING;
+            return true;
+        case BLE_HS_ENOTCONN:
+        case BLE_HS_EALREADY:
+        case BLE_HS_HCI_ERR(BLE_ERR_UNK_CONN_ID): // should not happen but just in case
+            return true;
     }
 
-    return true;
+    NIMBLE_LOGE(LOG_TAG, "ble_gap_terminate failed: rc=%d %s", rc, NimBLEUtils::returnCodeToString(rc));
+    m_lastErr = rc;
+    return false;
 } // disconnect
 
 /**
@@ -518,7 +550,7 @@ bool NimBLEClient::updateConnParams(uint16_t minInterval, uint16_t maxInterval,
  * @param [in] txOctets The preferred number of payload octets to use (Range 0x001B-0x00FB).
  */
 bool NimBLEClient::setDataLen(uint16_t txOctets) {
-# if defined(CONFIG_NIMBLE_CPP_IDF) && !defined(ESP_IDF_VERSION) || \
+# if !defined(USING_NIMBLE_ARDUINO_HEADERS) && !defined(ESP_IDF_VERSION) || \
      (ESP_IDF_VERSION_MAJOR * 100 + ESP_IDF_VERSION_MINOR * 10 + ESP_IDF_VERSION_PATCH) < 432
     return false;
 # else
@@ -575,8 +607,8 @@ NimBLEAddress NimBLEClient::getPeerAddress() const {
  * @return True if successful.
  */
 bool NimBLEClient::setPeerAddress(const NimBLEAddress& address) {
-    if (isConnected()) {
-        NIMBLE_LOGE(LOG_TAG, "Cannot set peer address while connected");
+    if (m_connStatus == CONNECTED || m_connStatus == CONNECTING) {
+        NIMBLE_LOGE(LOG_TAG, "Cannot set peer address while connected/connecting");
         return false;
     }
 
@@ -589,7 +621,7 @@ bool NimBLEClient::setPeerAddress(const NimBLEAddress& address) {
  * @return The RSSI value or 0 if there was an error.
  */
 int NimBLEClient::getRssi() const {
-    if (!isConnected()) {
+    if (m_connStatus != CONNECTED) {
         NIMBLE_LOGE(LOG_TAG, "getRssi(): Not connected");
         return 0;
     }
@@ -732,7 +764,7 @@ bool NimBLEClient::discoverAttributes() {
  * @return true on success otherwise false if an error occurred
  */
 bool NimBLEClient::retrieveServices(const NimBLEUUID* uuidFilter) {
-    if (!isConnected()) {
+    if (m_connStatus != CONNECTED) {
         NIMBLE_LOGE(LOG_TAG, "Disconnected, could not retrieve services -aborting");
         return false;
     }
@@ -910,7 +942,7 @@ int NimBLEClient::exchangeMTUCb(uint16_t conn_handle, const ble_gatt_error* erro
  */
 bool NimBLEClient::exchangeMTU() {
     int rc = ble_gattc_exchange_mtu(m_connHandle, NimBLEClient::exchangeMTUCb, this);
-    if (rc != 0) {
+    if (rc != 0 && rc != BLE_HS_EALREADY) {
         NIMBLE_LOGE(LOG_TAG, "MTU exchange error; rc=%d %s", rc, NimBLEUtils::returnCodeToString(rc));
         m_lastErr = rc;
         return false;
@@ -919,6 +951,59 @@ bool NimBLEClient::exchangeMTU() {
     return true;
 } // exchangeMTU
 
+void NimBLEClient::startConnectEstablishedTimer(uint16_t connInterval) {
+    // As per Bluetooth spec, the connection is only established after receiving a PDU
+    // within 6 connections events, so we wait for 7 connection events for a margin.
+    uint32_t waitMs = connIntervalToMs(connInterval) * 7;
+    if (waitMs == 0) {
+        waitMs = 1;
+    }
+
+    ble_npl_time_t waitTicks = 1;
+    ble_npl_time_ms_to_ticks(waitMs, &waitTicks);
+    if (waitTicks == 0) {
+        waitTicks = 1;
+    }
+
+    ble_npl_callout_reset(&m_connectEstablishedTimer, waitTicks);
+} // startConnectEstablishedTimer
+
+bool NimBLEClient::completeConnectEstablished() {
+    if (!m_connectCallbackPending) {
+        return false;
+    }
+
+    m_connectCallbackPending = false;
+    ble_npl_callout_stop(&m_connectEstablishedTimer);
+    auto pTaskData = m_pTaskData; // save a copy in case something in the callback changes it
+    m_pTaskData    = nullptr;     // clear before callback to prevent other handlers from releasing
+    m_pClientCallbacks->onConnect(this);
+
+    if (pTaskData != nullptr) {
+        NimBLEUtils::taskRelease(*pTaskData, 0);
+    }
+
+    return true;
+} // completeConnectEstablished
+
+void NimBLEClient::connectEstablishedTimerCb(struct ble_npl_event* event) {
+    auto* pClient = static_cast<NimBLEClient*>(ble_npl_event_get_arg(event));
+    if (pClient == nullptr || pClient->m_connStatus != CONNECTED) {
+        return;
+    }
+
+    pClient->completeConnectEstablished();
+} // connectEstablishedTimerCb
+
+/**
+ * @brief Set the number of times to retry connecting after a connection establishment error (0x3e).
+ * @param [in] numRetries The number of retries to attempt before giving up and reporting the failure.
+ * @details Max is 7, Default is 2.
+ */
+void NimBLEClient::setConnectRetries(uint8_t numRetries) {
+    m_config.connectFailRetries = std::min<uint8_t>(numRetries, 7U);
+} // setConnectRetries
+
 /**
  * @brief Handle a received GAP event.
  * @param [in] event The event structure sent by the NimBLE stack.
@@ -933,19 +1018,24 @@ int NimBLEClient::handleGapEvent(struct ble_gap_event* event, void* arg) {
 
     switch (event->type) {
         case BLE_GAP_EVENT_DISCONNECT: {
-
             // workaround for bug in NimBLE stack where disconnect event argument is not passed correctly
-            pClient = NimBLEDevice::getClientByPeerAddress(event->disconnect.conn.peer_ota_addr);
+            pClient = NimBLEDevice::getClientByHandle(event->disconnect.conn.conn_handle);
+            if (pClient == nullptr) {
+                pClient = NimBLEDevice::getClientByPeerAddress(event->disconnect.conn.peer_ota_addr);
+            }
+
             if (pClient == nullptr) {
                 pClient = NimBLEDevice::getClientByPeerAddress(event->disconnect.conn.peer_id_addr);
             }
 
             if (pClient == nullptr) {
-                NIMBLE_LOGE(LOG_TAG, "Disconnected client not found, conn_handle=%d",
-                            event->disconnect.conn.conn_handle);
+                NIMBLE_LOGE(LOG_TAG, "Disconnected client not found, conn_handle=%d", event->disconnect.conn.conn_handle);
                 return 0;
             }
 
+            pClient->m_connectCallbackPending = false;
+            ble_npl_callout_stop(&pClient->m_connectEstablishedTimer);
+
             rc = event->disconnect.reason;
             // If Host reset tell the device now before returning to prevent
             // any errors caused by calling host functions before re-syncing.
@@ -966,20 +1056,42 @@ int NimBLEClient::handleGapEvent(struct ble_gap_event* event, void* arg) {
             pClient->m_terminateFailCount = 0;
             pClient->m_asyncSecureAttempt = 0;
 
-            // Don't call the disconnect callback if we are waiting for a connection to complete and it fails
-            if (rc == (BLE_HS_ERR_HCI_BASE + BLE_ERR_CONN_ESTABLISHMENT) && pClient->m_config.asyncConnect) {
+            // set this incase the client instance was changed due to incorrect event arg bug above
+            pTaskData = pClient->m_pTaskData;
+
+            const int connEstablishFailReason = BLE_HS_HCI_ERR(BLE_ERR_CONN_ESTABLISHMENT);
+            if (rc == connEstablishFailReason && pClient->m_connectFailRetryCount < pClient->m_config.connectFailRetries) {
+                pClient->m_connHandle = BLE_HS_CONN_HANDLE_NONE;
+                ++pClient->m_connectFailRetryCount;
+                pClient->m_connStatus = CONNECTING;
+                NIMBLE_LOGW(LOG_TAG,
+                            "Connection establishment failed (0x3e), retry %u/%u",
+                            pClient->m_connectFailRetryCount,
+                            pClient->m_config.connectFailRetries);
+
+                const int retryRc = pClient->startConnectionAttempt(pClient->m_peerAddress.getBase());
+                if (retryRc == 0) {
+                    // A retry attempt is in progress; suppress user callbacks until final outcome.
+                    return 0;
+                }
+
+                NIMBLE_LOGE(LOG_TAG, "Retry connect start failed, rc=%d %s", retryRc, NimBLEUtils::returnCodeToString(retryRc));
+            }
+
+            if (rc == connEstablishFailReason) {
                 pClient->m_pClientCallbacks->onConnectFail(pClient, rc);
             } else {
                 pClient->m_pClientCallbacks->onDisconnect(pClient, rc);
             }
 
             pClient->m_connHandle = BLE_HS_CONN_HANDLE_NONE;
+            pClient->m_connStatus = DISCONNECTED;
 
-            if (pClient->m_config.deleteOnDisconnect) {
+            if (pClient->m_config.deleteOnDisconnect ||
+                (rc == connEstablishFailReason && pClient->m_config.deleteOnConnectFail)) {
                 // If we are set to self delete on disconnect but we have a task waiting on the connection
                 // completion we will set the flag to delete on connect fail instead of deleting here
-                // to prevent segmentation faults or double deleting
-                if (pTaskData != nullptr && rc == (BLE_HS_ERR_HCI_BASE + BLE_ERR_CONN_ESTABLISHMENT)) {
+                if (pTaskData != nullptr && rc == connEstablishFailReason) {
                     pClient->m_config.deleteOnConnectFail = true;
                     break;
                 }
@@ -991,7 +1103,7 @@ int NimBLEClient::handleGapEvent(struct ble_gap_event* event, void* arg) {
 
         case BLE_GAP_EVENT_CONNECT: {
             // If we aren't waiting for this connection response we should drop the connection immediately.
-            if (pClient->isConnected() || (!pClient->m_config.asyncConnect && pClient->m_pTaskData == nullptr)) {
+            if (pClient->m_connStatus != CONNECTING) {
                 ble_gap_terminate(event->connect.conn_handle, BLE_ERR_REM_USER_CONN_TERM);
                 return 0;
             }
@@ -1002,22 +1114,28 @@ int NimBLEClient::handleGapEvent(struct ble_gap_event* event, void* arg) {
             }
 
             if (rc == 0) {
-                pClient->m_connHandle = event->connect.conn_handle;
+                pClient->m_connStatus             = CONNECTED;
+                pClient->m_connHandle             = event->connect.conn_handle;
+                pClient->m_connectCallbackPending = true;
 
-                if (pClient->m_config.asyncConnect) {
-                    pClient->m_pClientCallbacks->onConnect(pClient);
+                ble_gap_conn_desc desc;
+                if (ble_gap_conn_find(event->connect.conn_handle, &desc) == 0) {
+                    pClient->startConnectEstablishedTimer(desc.conn_itvl);
+                } else {
+                    pClient->startConnectEstablishedTimer(pClient->m_connParams.itvl_max);
                 }
 
                 if (pClient->m_config.exchangeMTU) {
-                    if (!pClient->exchangeMTU()) {
-                        rc = pClient->m_lastErr; // sets the error in the task data
-                        break;
-                    }
-
-                    return 0; // return as we may have a task waiting for the MTU before releasing it.
+                    pClient->exchangeMTU();
                 }
+                // return as we may have a task waiting on the connection completion
+                // and will release it in the timer callback after the connection is fully established.
+                return 0;
             } else {
-                pClient->m_connHandle = BLE_HS_CONN_HANDLE_NONE;
+                pClient->m_connStatus             = DISCONNECTED;
+                pClient->m_connHandle             = BLE_HS_CONN_HANDLE_NONE;
+                pClient->m_connectCallbackPending = false;
+                ble_npl_callout_stop(&pClient->m_connectEstablishedTimer);
 
                 if (pClient->m_config.asyncConnect) {
                     pClient->m_pClientCallbacks->onConnectFail(pClient, rc);
@@ -1045,33 +1163,50 @@ int NimBLEClient::handleGapEvent(struct ble_gap_event* event, void* arg) {
         } // BLE_GAP_EVENT_TERM_FAILURE
 
         case BLE_GAP_EVENT_NOTIFY_RX: {
-            if (pClient->m_connHandle != event->notify_rx.conn_handle) return 0;
+            if (pClient->m_connHandle != event->notify_rx.conn_handle) {
+                return 0;
+            }
+
+            if (pClient->completeConnectEstablished()) {
+                pTaskData = nullptr;
+            }
+
             NIMBLE_LOGD(LOG_TAG, "Notify Received for handle: %d", event->notify_rx.attr_handle);
 
-            for (const auto& svc : pClient->m_svcVec) {
-                // Dont waste cycles searching services without this handle in its range
-                if (svc->getEndHandle() < event->notify_rx.attr_handle) {
-                    continue;
-                }
+            NimBLERemoteCharacteristic* pChr = pClient->getCharacteristic(event->notify_rx.attr_handle);
+            if (pChr == nullptr) {
+                NIMBLE_LOGW(LOG_TAG, "unknown handle: %d", event->notify_rx.attr_handle);
+                return BLE_ATT_ERR_INVALID_HANDLE;
+            }
 
-                NIMBLE_LOGD(LOG_TAG,
-                            "checking service %s for handle: %d",
-                            svc->getUUID().toString().c_str(),
-                            event->notify_rx.attr_handle);
-
-                for (const auto& chr : svc->m_vChars) {
-                    if (chr->getHandle() == event->notify_rx.attr_handle) {
-                        NIMBLE_LOGD(LOG_TAG, "Got Notification for characteristic %s", chr->toString().c_str());
-
-                        uint32_t data_len = OS_MBUF_PKTLEN(event->notify_rx.om);
-                        chr->m_value.setValue(event->notify_rx.om->om_data, data_len);
-
-                        if (chr->m_notifyCallback != nullptr) {
-                            chr->m_notifyCallback(chr, event->notify_rx.om->om_data, data_len, !event->notify_rx.indication);
-                        }
+            auto len = event->notify_rx.om->om_len;
+            if (pChr->m_value.setValue(event->notify_rx.om->om_data, len)) {
+                os_mbuf* next;
+                next = SLIST_NEXT(event->notify_rx.om, om_next);
+                while (next != NULL) {
+                    pChr->m_value.append(next->om_data, next->om_len);
+                    if (pChr->m_value.length() != len + next->om_len) {
+                        rc = BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
                         break;
                     }
+                    len  += next->om_len;
+                    next  = SLIST_NEXT(next, om_next);
                 }
+            } else {
+                rc = BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
+            }
+
+            if (rc != 0) { // This should never happen
+                NIMBLE_LOGE(LOG_TAG, "notification value error; exceeds limit");
+                return rc;
+            }
+
+            if (pChr->m_notifyCallback != nullptr) {
+                // TODO: change this callback to use the NimBLEAttValue class instead of raw data and length
+                pChr->m_notifyCallback(pChr,
+                                       const_cast<uint8_t*>(pChr->m_value.getValue().data()),
+                                       pChr->m_value.length(),
+                                       !event->notify_rx.indication);
             }
 
             return 0;
@@ -1082,6 +1217,11 @@ int NimBLEClient::handleGapEvent(struct ble_gap_event* event, void* arg) {
             if (pClient->m_connHandle != event->conn_update_req.conn_handle) {
                 return 0;
             }
+
+            if (pClient->completeConnectEstablished()) {
+                pTaskData = nullptr;
+            }
+
             NIMBLE_LOGD(LOG_TAG, "Peer requesting to update connection parameters");
             NIMBLE_LOGD(LOG_TAG,
                         "MinInterval: %d, MaxInterval: %d, Latency: %d, Timeout: %d",
@@ -1109,6 +1249,11 @@ int NimBLEClient::handleGapEvent(struct ble_gap_event* event, void* arg) {
             if (pClient->m_connHandle != event->conn_update.conn_handle) {
                 return 0;
             }
+
+            if (pClient->completeConnectEstablished()) {
+                pTaskData = nullptr;
+            }
+
             if (event->conn_update.status == 0) {
                 NIMBLE_LOGI(LOG_TAG, "Connection parameters updated.");
             } else {
@@ -1122,8 +1267,11 @@ int NimBLEClient::handleGapEvent(struct ble_gap_event* event, void* arg) {
                 return 0;
             }
 
-            if (event->enc_change.status == 0 ||
-                event->enc_change.status == (BLE_HS_ERR_HCI_BASE + BLE_ERR_PINKEY_MISSING)) {
+            if (pClient->completeConnectEstablished()) {
+                pTaskData = nullptr;
+            }
+
+            if (event->enc_change.status == 0 || event->enc_change.status == BLE_HS_HCI_ERR(BLE_ERR_PINKEY_MISSING)) {
                 NimBLEConnInfo peerInfo;
                 rc = ble_gap_conn_find(event->enc_change.conn_handle, &peerInfo.m_desc);
                 if (rc != 0) {
@@ -1131,7 +1279,7 @@ int NimBLEClient::handleGapEvent(struct ble_gap_event* event, void* arg) {
                     break;
                 }
 
-                if (event->enc_change.status == (BLE_HS_ERR_HCI_BASE + BLE_ERR_PINKEY_MISSING)) {
+                if (event->enc_change.status == BLE_HS_HCI_ERR(BLE_ERR_PINKEY_MISSING)) {
                     // Key is missing, try deleting.
                     ble_store_util_delete_peer(&peerInfo.m_desc.peer_id_addr);
                     // Attempt a retry if async secure failed.
@@ -1149,6 +1297,14 @@ int NimBLEClient::handleGapEvent(struct ble_gap_event* event, void* arg) {
         } // BLE_GAP_EVENT_ENC_CHANGE
 
         case BLE_GAP_EVENT_IDENTITY_RESOLVED: {
+            if (pClient->m_connHandle != event->identity_resolved.conn_handle) {
+                return 0;
+            }
+
+            if (pClient->completeConnectEstablished()) {
+                pTaskData = nullptr;
+            }
+
             NimBLEConnInfo peerInfo;
             rc = ble_gap_conn_find(event->identity_resolved.conn_handle, &peerInfo.m_desc);
             if (rc != 0) {
@@ -1161,6 +1317,14 @@ int NimBLEClient::handleGapEvent(struct ble_gap_event* event, void* arg) {
         } // BLE_GAP_EVENT_IDENTITY_RESOLVED
 
         case BLE_GAP_EVENT_PHY_UPDATE_COMPLETE: {
+            if (pClient->m_connHandle != event->phy_updated.conn_handle) {
+                return 0;
+            }
+
+            if (pClient->completeConnectEstablished()) {
+                pTaskData = nullptr;
+            }
+
             NimBLEConnInfo peerInfo;
             rc = ble_gap_conn_find(event->phy_updated.conn_handle, &peerInfo.m_desc);
             if (rc != 0) {
@@ -1176,6 +1340,10 @@ int NimBLEClient::handleGapEvent(struct ble_gap_event* event, void* arg) {
                 return 0;
             }
 
+            if (pClient->completeConnectEstablished()) {
+                pTaskData = nullptr;
+            }
+
             NIMBLE_LOGI(LOG_TAG, "mtu update: mtu=%d", event->mtu.value);
             pClient->m_pClientCallbacks->onMTUChange(pClient, event->mtu.value);
             rc = 0;
@@ -1187,6 +1355,10 @@ int NimBLEClient::handleGapEvent(struct ble_gap_event* event, void* arg) {
                 return 0;
             }
 
+            if (pClient->completeConnectEstablished()) {
+                pTaskData = nullptr;
+            }
+
             NimBLEConnInfo peerInfo;
             rc = ble_gap_conn_find(event->passkey.conn_handle, &peerInfo.m_desc);
             if (rc != 0) {
@@ -1194,7 +1366,16 @@ int NimBLEClient::handleGapEvent(struct ble_gap_event* event, void* arg) {
                 break;
             }
 
-            if (event->passkey.params.action == BLE_SM_IOACT_NUMCMP) {
+            if (event->passkey.params.action == BLE_SM_IOACT_DISP) {
+                struct ble_sm_io pkey = {0, 0};
+                pkey.action           = event->passkey.params.action;
+                pkey.passkey          = NimBLEDevice::getSecurityPasskey();
+                if (pkey.passkey == 123456) {
+                    pkey.passkey = pClient->m_pClientCallbacks->onPassKeyDisplay(peerInfo);
+                }
+                rc = ble_sm_inject_io(event->passkey.conn_handle, &pkey);
+                NIMBLE_LOGD(LOG_TAG, "BLE_SM_IOACT_DISP; ble_sm_inject_io result: %d", rc);
+            } else if (event->passkey.params.action == BLE_SM_IOACT_NUMCMP) {
                 NIMBLE_LOGD(LOG_TAG, "Passkey on device's display: %" PRIu32, event->passkey.params.numcmp);
                 pClient->m_pClientCallbacks->onConfirmPasskey(peerInfo, event->passkey.params.numcmp);
             } else if (event->passkey.params.action == BLE_SM_IOACT_OOB) {
@@ -1233,7 +1414,7 @@ int NimBLEClient::handleGapEvent(struct ble_gap_event* event, void* arg) {
  * @return True if we are connected and false if we are not connected.
  */
 bool NimBLEClient::isConnected() const {
-    return m_connHandle != BLE_HS_CONN_HANDLE_NONE;
+    return m_connStatus == CONNECTED;
 } // isConnected
 
 /**
@@ -1298,6 +1479,11 @@ void NimBLEClientCallbacks::onPassKeyEntry(NimBLEConnInfo& connInfo) {
     NimBLEDevice::injectPassKey(connInfo, 123456);
 } // onPassKeyEntry
 
+uint32_t NimBLEClientCallbacks::onPassKeyDisplay(NimBLEConnInfo& connInfo) {
+    NIMBLE_LOGD(CB_TAG, "onPassKeyDisplay: default");
+    return NimBLEDevice::getSecurityPasskey();
+} // onPassKeyDisplay
+
 void NimBLEClientCallbacks::onAuthenticationComplete(NimBLEConnInfo& connInfo) {
     NIMBLE_LOGD(CB_TAG, "onAuthenticationComplete: default");
 } // onAuthenticationComplete
@@ -1318,6 +1504,5 @@ void NimBLEClientCallbacks::onMTUChange(NimBLEClient* pClient, uint16_t mtu) {
 void NimBLEClientCallbacks::onPhyUpdate(NimBLEClient* pClient, uint8_t txPhy, uint8_t rxPhy) {
     NIMBLE_LOGD(CB_TAG, "onPhyUpdate: default, txPhy: %d, rxPhy: %d", txPhy, rxPhy);
 } // onPhyUpdate
-#
 
 #endif // CONFIG_BT_NIMBLE_ENABLED && MYNEWT_VAL(BLE_ROLE_CENTRAL)

src/NimBLEClient.h

@@ -21,10 +21,10 @@
 #include "syscfg/syscfg.h"
 #if CONFIG_BT_NIMBLE_ENABLED && MYNEWT_VAL(BLE_ROLE_CENTRAL)
 
-# if defined(CONFIG_NIMBLE_CPP_IDF)
-#  include "host/ble_gap.h"
-# else
+# ifdef USING_NIMBLE_ARDUINO_HEADERS
 #  include "nimble/nimble/host/include/host/ble_gap.h"
+# else
+#  include "host/ble_gap.h"
 # endif
 
 # include "NimBLEAddress.h"
@@ -58,6 +58,7 @@ class NimBLEClient {
     bool           connect(bool deleteAttributes = true, bool asyncConnect = false, bool exchangeMTU = true);
     bool           disconnect(uint8_t reason = BLE_ERR_REM_USER_CONN_TERM);
     bool           cancelConnect() const;
+    void           setConnectRetries(uint8_t numRetries);
     void           setSelfDelete(bool deleteOnDisconnect, bool deleteOnConnectFail);
     NimBLEAddress  getPeerAddress() const;
     bool           setPeerAddress(const NimBLEAddress& address);
@@ -107,24 +108,49 @@ class NimBLEClient {
         uint8_t deleteOnConnectFail : 1; // Delete the client when a connection attempt fails.
         uint8_t asyncConnect : 1;        // Connect asynchronously.
         uint8_t exchangeMTU : 1;         // Exchange MTU after connection.
+        uint8_t connectFailRetries : 3;  // Number of retries for 0x3e (connection establishment) failures.
+
+        /**
+         * @brief Construct a new Config object with default values.
+         * @details Default values are:
+         * - deleteCallbacks: false
+         * - deleteOnDisconnect: false
+         * - deleteOnConnectFail: false
+         * - asyncConnect: false
+         * - exchangeMTU: true
+         * - connectFailRetries: 2
+         */
+        Config()
+            : deleteCallbacks(0),
+              deleteOnDisconnect(0),
+              deleteOnConnectFail(0),
+              asyncConnect(0),
+              exchangeMTU(1),
+              connectFailRetries(2) {}
     };
 
     Config getConfig() const;
     void   setConfig(Config config);
 
   private:
+    enum ConnStatus : uint8_t { CONNECTED, DISCONNECTED, CONNECTING, DISCONNECTING };
+
     NimBLEClient(const NimBLEAddress& peerAddress);
     ~NimBLEClient();
     NimBLEClient(const NimBLEClient&)            = delete;
     NimBLEClient& operator=(const NimBLEClient&) = delete;
 
-    bool       retrieveServices(const NimBLEUUID* uuidFilter = nullptr);
-    static int handleGapEvent(struct ble_gap_event* event, void* arg);
-    static int exchangeMTUCb(uint16_t conn_handle, const ble_gatt_error* error, uint16_t mtu, void* arg);
-    static int serviceDiscoveredCB(uint16_t                     connHandle,
-                                   const struct ble_gatt_error* error,
-                                   const struct ble_gatt_svc*   service,
-                                   void*                        arg);
+    bool        retrieveServices(const NimBLEUUID* uuidFilter = nullptr);
+    int         startConnectionAttempt(const ble_addr_t* peerAddr);
+    static int  handleGapEvent(struct ble_gap_event* event, void* arg);
+    static void connectEstablishedTimerCb(struct ble_npl_event* event);
+    void        startConnectEstablishedTimer(uint16_t connInterval);
+    bool        completeConnectEstablished();
+    static int  exchangeMTUCb(uint16_t conn_handle, const ble_gatt_error* error, uint16_t mtu, void* arg);
+    static int  serviceDiscoveredCB(uint16_t                     connHandle,
+                                    const struct ble_gatt_error* error,
+                                    const struct ble_gatt_svc*   service,
+                                    void*                        arg);
 
     NimBLEAddress                     m_peerAddress;
     mutable int                       m_lastErr;
@@ -136,6 +162,10 @@ class NimBLEClient {
     uint8_t                           m_terminateFailCount;
     mutable uint8_t                   m_asyncSecureAttempt;
     Config                            m_config;
+    ConnStatus                        m_connStatus;
+    ble_npl_callout                   m_connectEstablishedTimer{};
+    bool                              m_connectCallbackPending;
+    uint8_t                           m_connectFailRetryCount;
 
 # if MYNEWT_VAL(BLE_EXT_ADV)
     uint8_t m_phyMask;
@@ -187,6 +217,13 @@ class NimBLEClientCallbacks {
      */
     virtual void onPassKeyEntry(NimBLEConnInfo& connInfo);
 
+    /**
+     * @brief Called when using passkey entry pairing and the passkey should be displayed.
+     * @param [in] connInfo A reference to a NimBLEConnInfo instance containing the peer info.
+     * @return The passkey to display to the user. The peer device must enter this passkey to complete the pairing.
+     */
+    virtual uint32_t onPassKeyDisplay(NimBLEConnInfo& connInfo);
+
     /**
      * @brief Called when the pairing procedure is complete.
      * @param [in] connInfo A reference to a NimBLEConnInfo instance containing the peer info.\n

src/NimBLEConnInfo.h

@@ -18,13 +18,14 @@
 #ifndef NIMBLE_CPP_CONNINFO_H_
 #define NIMBLE_CPP_CONNINFO_H_
 
-#if defined(CONFIG_NIMBLE_CPP_IDF)
-# include "host/ble_gap.h"
-#else
+#ifdef USING_NIMBLE_ARDUINO_HEADERS
 # include "nimble/nimble/host/include/host/ble_gap.h"
+#else
+# include "host/ble_gap.h"
 #endif
 
 #include "NimBLEAddress.h"
+#include <cstdio>
 
 /**
  * @brief Connection information.
@@ -70,6 +71,41 @@ class NimBLEConnInfo {
     /** @brief Gets the key size used to encrypt the connection */
     uint8_t getSecKeySize() const { return m_desc.sec_state.key_size; }
 
+    /** @brief Get a string representation of the connection info, useful for debugging */
+    std::string toString() const {
+        std::string str;
+        // 294 chars max expected from all labels + worst-case values, round up to 300.
+        str.resize(300);
+
+        snprintf(&str[0],
+                 str.size(),
+                 "  Address: %s\n"
+                 "  ID Address: %s\n"
+                 "  Connection Handle: %u\n"
+                 "  Connection Interval: %.1f ms\n"
+                 "  Connection Timeout: %u ms\n"
+                 "  Connection Latency: %u\n"
+                 "  MTU: %u bytes\n"
+                 "  Role: %s\n"
+                 "  Bonded: %s\n"
+                 "  Encrypted: %s\n"
+                 "  Authenticated: %s\n"
+                 "  Security Key Size: %u\n",
+                 getAddress().toString().c_str(),
+                 getIdAddress().toString().c_str(),
+                 getConnHandle(),
+                 getConnInterval() * 1.25f,
+                 getConnTimeout() * 10,
+                 getConnLatency(),
+                 getMTU(),
+                 isMaster() ? "Master" : "Slave",
+                 isBonded() ? "Yes" : "No",
+                 isEncrypted() ? "Yes" : "No",
+                 isAuthenticated() ? "Yes" : "No",
+                 getSecKeySize());
+        return str;
+    }
+
   private:
     friend class NimBLEServer;
     friend class NimBLEClient;

src/NimBLECppVersion.h

@@ -0,0 +1,80 @@
+/*
+ * Copyright 2020-2025 Ryan Powell <ryan@nable-embedded.io> and
+ * esp-nimble-cpp, NimBLE-Arduino contributors.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef NIMBLE_CPP_VERSION_H_
+#define NIMBLE_CPP_VERSION_H_
+
+/** @brief NimBLE-Arduino library major version number. */
+#define NIMBLE_CPP_VERSION_MAJOR 2
+
+/** @brief NimBLE-Arduino library minor version number. */
+#define NIMBLE_CPP_VERSION_MINOR 5
+
+/** @brief NimBLE-Arduino library patch version number. */
+#define NIMBLE_CPP_VERSION_PATCH 0
+
+/**
+ * @brief Macro to create a version number for comparison.
+ * @param major Major version number.
+ * @param minor Minor version number.
+ * @param patch Patch version number.
+ * @details Example usage:
+ * @code{.cpp}
+ * #if NIMBLE_CPP_VERSION >= NIMBLE_CPP_VERSION_VAL(2, 0, 0)
+ *   // Using NimBLE-Arduino v2 or later
+ * #endif
+ * @endcode
+ */
+#define NIMBLE_CPP_VERSION_VAL(major, minor, patch) (((major) << 16) | ((minor) << 8) | (patch))
+
+/**
+ * @brief The library version as a single integer for compile-time comparison.
+ * @details Format: (major << 16) | (minor << 8) | patch
+ */
+#define NIMBLE_CPP_VERSION \
+    NIMBLE_CPP_VERSION_VAL(NIMBLE_CPP_VERSION_MAJOR, NIMBLE_CPP_VERSION_MINOR, NIMBLE_CPP_VERSION_PATCH)
+
+/** @cond NIMBLE_CPP_INTERNAL */
+#define NIMBLE_CPP_VERSION_STRINGIFY_IMPL(x) #x
+#define NIMBLE_CPP_VERSION_STRINGIFY(x)      NIMBLE_CPP_VERSION_STRINGIFY_IMPL(x)
+/** @endcond */
+
+/**
+ * @brief Optional Semantic Versioning prerelease suffix.
+ * @details Include the leading '-' when defined, for example: "-beta.1"
+ */
+#ifndef NIMBLE_CPP_VERSION_PRERELEASE
+# define NIMBLE_CPP_VERSION_PRERELEASE ""
+#endif
+
+/**
+ * @brief Optional Semantic Versioning build metadata suffix.
+ * @details Include the leading '+' when defined, for example: "+sha.abcd1234"
+ */
+#ifndef NIMBLE_CPP_VERSION_BUILD_METADATA
+# define NIMBLE_CPP_VERSION_BUILD_METADATA ""
+#endif
+
+/** @brief library version as a prefixed Semantic Versioning string. */
+#define NIMBLE_CPP_VERSION_STR                         \
+    "NimBLE-CPP " \
+    NIMBLE_CPP_VERSION_STRINGIFY(NIMBLE_CPP_VERSION_MAJOR) "." \
+    NIMBLE_CPP_VERSION_STRINGIFY(NIMBLE_CPP_VERSION_MINOR) "." \
+    NIMBLE_CPP_VERSION_STRINGIFY(NIMBLE_CPP_VERSION_PATCH) \
+    NIMBLE_CPP_VERSION_PRERELEASE NIMBLE_CPP_VERSION_BUILD_METADATA
+
+#endif // NIMBLE_CPP_VERSION_H_

src/NimBLEDevice.cpp

@@ -24,7 +24,7 @@
 #   include "esp_bt.h"
 #  endif
 #  include "nvs_flash.h"
-#  if defined(CONFIG_NIMBLE_CPP_IDF)
+#  ifndef USING_NIMBLE_ARDUINO_HEADERS
 #   if (ESP_IDF_VERSION < ESP_IDF_VERSION_VAL(5, 0, 0) || CONFIG_BT_NIMBLE_LEGACY_VHCI_ENABLE)
 #    include "esp_nimble_hci.h"
 #   endif
@@ -35,14 +35,14 @@
 #   include "host/util/util.h"
 #   include "services/gap/ble_svc_gap.h"
 #   include "services/gatt/ble_svc_gatt.h"
-#  else
+#  else // USING_NIMBLE_ARDUINO_HEADERS
 #   include "nimble/esp_port/esp-hci/include/esp_nimble_hci.h"
 #  endif
 # else
 #  include "nimble/nimble/controller/include/controller/ble_phy.h"
 # endif
 
-# ifndef CONFIG_NIMBLE_CPP_IDF
+# ifdef USING_NIMBLE_ARDUINO_HEADERS
 #  include "nimble/porting/nimble/include/nimble/nimble_port.h"
 #  include "nimble/porting/npl/freertos/include/nimble/nimble_port_freertos.h"
 #  include "nimble/nimble/host/include/host/ble_hs.h"
@@ -98,15 +98,11 @@ ble_gap_event_listener     NimBLEDevice::m_listener{};
 std::vector<NimBLEAddress> NimBLEDevice::m_whiteList{};
 uint8_t                    NimBLEDevice::m_ownAddrType{BLE_OWN_ADDR_PUBLIC};
 
-# ifdef ESP_PLATFORM
-#  if CONFIG_BTDM_BLE_SCAN_DUPL
-uint16_t NimBLEDevice::m_scanDuplicateSize{CONFIG_BTDM_SCAN_DUPL_CACHE_SIZE};
-uint8_t  NimBLEDevice::m_scanFilterMode{CONFIG_BTDM_SCAN_DUPL_TYPE};
+# if NIMBLE_CPP_SCAN_DUPL_ENABLED
+uint16_t NimBLEDevice::m_scanDuplicateSize{100};
+uint8_t  NimBLEDevice::m_scanFilterMode{0};
 uint16_t NimBLEDevice::m_scanDuplicateResetTime{0};
-#  elif CONFIG_BT_LE_SCAN_DUPL
-uint16_t       NimBLEDevice::m_scanDuplicateSize{CONFIG_BT_LE_LL_DUP_SCAN_LIST_COUNT};
-uint8_t        NimBLEDevice::m_scanFilterMode{CONFIG_BT_LE_SCAN_DUPL_TYPE};
-uint16_t       NimBLEDevice::m_scanDuplicateResetTime{0};
+#  if SOC_ESP_NIMBLE_CONTROLLER
 extern "C" int ble_vhci_disc_duplicate_set_max_cache_size(int max_cache_size);
 extern "C" int ble_vhci_disc_duplicate_set_period_refresh_time(int refresh_period_time);
 extern "C" int ble_vhci_disc_duplicate_mode_disable(int mode);
@@ -126,9 +122,6 @@ extern "C" int ble_vhci_disc_duplicate_mode_enable(int mode);
 NimBLEServer* NimBLEDevice::createServer() {
     if (NimBLEDevice::m_pServer == nullptr) {
         NimBLEDevice::m_pServer = new NimBLEServer();
-        ble_gatts_reset();
-        ble_svc_gap_init();
-        ble_svc_gatt_init();
     }
 
     return m_pServer;
@@ -366,12 +359,12 @@ bool NimBLEDevice::deleteClient(NimBLEClient* pClient) {
 
     for (auto& clt : m_pClients) {
         if (clt == pClient) {
-            if (clt->isConnected()) {
+            if (clt->m_connStatus == NimBLEClient::CONNECTED || clt->m_connStatus == NimBLEClient::DISCONNECTING) {
                 clt->m_config.deleteOnDisconnect = true;
                 if (!clt->disconnect()) {
                     break;
                 }
-            } else if (pClient->m_pTaskData != nullptr) {
+            } else if (pClient->m_connStatus == NimBLEClient::CONNECTING) {
                 clt->m_config.deleteOnConnectFail = true;
                 if (!clt->cancelConnect()) {
                     break;
@@ -439,7 +432,7 @@ NimBLEClient* NimBLEDevice::getClientByPeerAddress(const NimBLEAddress& addr) {
  */
 NimBLEClient* NimBLEDevice::getDisconnectedClient() {
     for (const auto clt : m_pClients) {
-        if (clt != nullptr && !clt->isConnected()) {
+        if (clt != nullptr && clt->m_connStatus == NimBLEClient::DISCONNECTED) {
             return clt;
         }
     }
@@ -618,6 +611,7 @@ uint16_t NimBLEDevice::getMTU() {
  * @brief Gets the number of bonded peers stored
  */
 int NimBLEDevice::getNumBonds() {
+#  if MYNEWT_VAL(BLE_STORE_MAX_BONDS)
     ble_addr_t peer_id_addrs[MYNEWT_VAL(BLE_STORE_MAX_BONDS)];
     int        num_peers, rc;
     rc = ble_store_util_bonded_peers(&peer_id_addrs[0], &num_peers, MYNEWT_VAL(BLE_STORE_MAX_BONDS));
@@ -626,6 +620,9 @@ int NimBLEDevice::getNumBonds() {
     }
 
     return num_peers;
+#  else
+    return 0;
+#  endif
 }
 
 /**
@@ -633,10 +630,13 @@ int NimBLEDevice::getNumBonds() {
  * @returns True on success.
  */
 bool NimBLEDevice::deleteAllBonds() {
-    int rc = ble_store_clear();
-    if (rc != 0) {
-        NIMBLE_LOGE(LOG_TAG, "Failed to delete all bonds; rc=%d", rc);
-        return false;
+    int numBonds = NimBLEDevice::getNumBonds();
+    for (int i = numBonds - 1; i >= 0; i--) {
+        auto addr = NimBLEDevice::getBondedAddress(i);
+        if (!NimBLEDevice::deleteBond(addr)) {
+            NIMBLE_LOGE(LOG_TAG, "Failed to delete bond for address: %s", addr.toString().c_str());
+            return false;
+        }
     }
     return true;
 }
@@ -656,6 +656,7 @@ bool NimBLEDevice::deleteBond(const NimBLEAddress& address) {
  * @returns True if bonded.
  */
 bool NimBLEDevice::isBonded(const NimBLEAddress& address) {
+#  if MYNEWT_VAL(BLE_STORE_MAX_BONDS)
     ble_addr_t peer_id_addrs[MYNEWT_VAL(BLE_STORE_MAX_BONDS)];
     int        num_peers, rc;
 
@@ -670,7 +671,8 @@ bool NimBLEDevice::isBonded(const NimBLEAddress& address) {
             return true;
         }
     }
-
+#  endif
+    (void)address; // unused
     return false;
 }
 
@@ -680,14 +682,19 @@ bool NimBLEDevice::isBonded(const NimBLEAddress& address) {
  * @returns NimBLEAddress of the found bonded peer or null address if not found.
  */
 NimBLEAddress NimBLEDevice::getBondedAddress(int index) {
+#  if MYNEWT_VAL(BLE_STORE_MAX_BONDS)
     ble_addr_t peer_id_addrs[MYNEWT_VAL(BLE_STORE_MAX_BONDS)];
     int        num_peers, rc;
     rc = ble_store_util_bonded_peers(&peer_id_addrs[0], &num_peers, MYNEWT_VAL(BLE_STORE_MAX_BONDS));
-    if (rc != 0 || index > num_peers || index < 0) {
+    if (rc != 0 || index >= num_peers || index < 0) {
         return NimBLEAddress{};
     }
 
     return NimBLEAddress(peer_id_addrs[index]);
+#  else
+    (void)index; // unused
+    return NimBLEAddress{};
+#  endif
 }
 # endif
 
@@ -766,7 +773,7 @@ size_t NimBLEDevice::getWhiteListCount() {
  * @returns The NimBLEAddress at the whitelist index or null address if not found.
  */
 NimBLEAddress NimBLEDevice::getWhiteListAddress(size_t index) {
-    if (index > m_whiteList.size()) {
+    if (index >= m_whiteList.size()) {
         NIMBLE_LOGE(LOG_TAG, "Invalid index; %u", index);
         return NimBLEAddress{};
     }
@@ -868,7 +875,7 @@ void NimBLEDevice::onSync(void) {
  * @brief The main host task.
  */
 void NimBLEDevice::host_task(void* param) {
-    NIMBLE_LOGI(LOG_TAG, "BLE Host Task Started");
+    NIMBLE_LOGI(LOG_TAG, "NimBLE Started!");
     nimble_port_run(); // This function will return only when nimble_port_stop() is executed
     nimble_port_freertos_deinit();
 } // host_task
@@ -879,6 +886,7 @@ void NimBLEDevice::host_task(void* param) {
  */
 bool NimBLEDevice::init(const std::string& deviceName) {
     if (!m_initialized) {
+        NIMBLE_LOGD(LOG_TAG, "Starting %s", getVersion());
 # ifdef ESP_PLATFORM
 
 #  if defined(CONFIG_ENABLE_ARDUINO_DEPENDS) && SOC_BT_SUPPORTED
@@ -903,34 +911,37 @@ bool NimBLEDevice::init(const std::string& deviceName) {
         esp_bt_controller_mem_release(ESP_BT_MODE_CLASSIC_BT);
 #  endif
 
-#  if ESP_IDF_VERSION < ESP_IDF_VERSION_VAL(5, 0, 0) || !defined(CONFIG_NIMBLE_CPP_IDF)
+#  if ESP_IDF_VERSION < ESP_IDF_VERSION_VAL(5, 0, 0) || defined(USING_NIMBLE_ARDUINO_HEADERS)
         esp_bt_controller_config_t bt_cfg = BT_CONTROLLER_INIT_CONFIG_DEFAULT();
 #   if defined(CONFIG_IDF_TARGET_ESP32)
         bt_cfg.mode         = ESP_BT_MODE_BLE;
         bt_cfg.ble_max_conn = MYNEWT_VAL(BLE_MAX_CONNECTIONS);
 #   elif defined(CONFIG_IDF_TARGET_ESP32C3) || defined(CONFIG_IDF_TARGET_ESP32S3)
-        bt_cfg.ble_max_act = MYNEWT_VAL(BLE_MAX_CONNECTIONS) + MYNEWT_VAL(BLE_ROLE_BROADCASTER) + MYNEWT_VAL(BLE_ROLE_OBSERVER);
+        bt_cfg.ble_max_act =
+            MYNEWT_VAL(BLE_MAX_CONNECTIONS) + MYNEWT_VAL(BLE_ROLE_BROADCASTER) + MYNEWT_VAL(BLE_ROLE_OBSERVER);
 #   else
         bt_cfg.nimble_max_connections = MYNEWT_VAL(BLE_MAX_CONNECTIONS);
 #   endif
 
-#   if CONFIG_BTDM_BLE_SCAN_DUPL
+#   if NIMBLE_CPP_SCAN_DUPL_ENABLED
+#    if !SOC_ESP_NIMBLE_CONTROLLER
         bt_cfg.normal_adv_size     = m_scanDuplicateSize;
         bt_cfg.scan_duplicate_type = m_scanFilterMode;
-#    if ESP_IDF_VERSION >= ESP_IDF_VERSION_VAL(5, 0, 0)
+        bt_cfg.scan_duplicate_mode = 0; // Ensure normal filter mode, could be set to mesh in default config
+#     if ESP_IDF_VERSION >= ESP_IDF_VERSION_VAL(5, 0, 0)
         bt_cfg.dup_list_refresh_period = m_scanDuplicateResetTime;
-#    endif
-#   elif CONFIG_BT_LE_SCAN_DUPL
+#     endif
+#    else  // SOC_ESP_NIMBLE_CONTROLLER
         bt_cfg.ble_ll_rsp_dup_list_count = m_scanDuplicateSize;
         bt_cfg.ble_ll_adv_dup_list_count = m_scanDuplicateSize;
-#   endif
+#    endif // SOC_ESP_NIMBLE_CONTROLLER
         err = esp_bt_controller_init(&bt_cfg);
         if (err != ESP_OK) {
             NIMBLE_LOGE(LOG_TAG, "esp_bt_controller_init() failed; err=%d", err);
             return false;
         }
 
-#   if CONFIG_BT_LE_SCAN_DUPL
+#    if SOC_ESP_NIMBLE_CONTROLLER
         int mode = (1UL << 4); // FILTER_DUPLICATE_EXCEPTION_FOR_MESH
         switch (m_scanFilterMode) {
             case 1:
@@ -940,14 +951,15 @@ bool NimBLEDevice::init(const std::string& deviceName) {
                 mode |= ((1UL << 2) | (1UL << 3)); // FILTER_DUPLICATE_ADDRESS | FILTER_DUPLICATE_ADVDATA
                 break;
             default:
-                mode |= (1UL << 0) | (1UL << 2); // FILTER_DUPLICATE_PDUTYPE | FILTER_DUPLICATE_ADDRESS
+                mode |= ((1UL << 0) | (1UL << 2)); // FILTER_DUPLICATE_PDUTYPE | FILTER_DUPLICATE_ADDRESS
         }
 
         ble_vhci_disc_duplicate_mode_disable(0xFFFFFFFF);
         ble_vhci_disc_duplicate_mode_enable(mode);
         ble_vhci_disc_duplicate_set_max_cache_size(m_scanDuplicateSize);
         ble_vhci_disc_duplicate_set_period_refresh_time(m_scanDuplicateResetTime);
-#   endif
+#    endif // SOC_ESP_NIMBLE_CONTROLLER
+#   endif  // NIMBLE_CPP_SCAN_DUPL_ENABLED
 
         err = esp_bt_controller_enable(ESP_BT_MODE_BLE);
         if (err != ESP_OK) {
@@ -996,6 +1008,7 @@ bool NimBLEDevice::init(const std::string& deviceName) {
     }
 
     m_initialized = true; // Set the initialization flag to ensure we are only initialized once.
+    NIMBLE_LOGD(LOG_TAG, "Initialized");
     return true;
 } // init
 
@@ -1012,7 +1025,7 @@ bool NimBLEDevice::deinit(bool clearAll) {
         rc = nimble_port_stop();
         if (rc == 0) {
             nimble_port_deinit();
-# ifdef CONFIG_NIMBLE_CPP_IDF
+# ifndef USING_NIMBLE_ARDUINO_HEADERS
 #  if ESP_IDF_VERSION < ESP_IDF_VERSION_VAL(5, 0, 0)
             rc = esp_nimble_hci_and_controller_deinit();
             if (rc != ESP_OK) {
@@ -1262,10 +1275,17 @@ bool NimBLEDevice::startSecurity(uint16_t connHandle, int* rcPtr) {
  * @return true if the passkey was injected successfully.
  */
 bool NimBLEDevice::injectPassKey(const NimBLEConnInfo& peerInfo, uint32_t passkey) {
+#  if MYNEWT_VAL(BLE_SM_LEGACY)
     ble_sm_io pkey{.action = BLE_SM_IOACT_INPUT, .passkey = passkey};
     int       rc = ble_sm_inject_io(peerInfo.getConnHandle(), &pkey);
     NIMBLE_LOGD(LOG_TAG, "BLE_SM_IOACT_INPUT; ble_sm_inject_io result: %d", rc);
     return rc == 0;
+#  else
+    (void)peerInfo;
+    (void)passkey;
+    NIMBLE_LOGE(LOG_TAG, "Passkey entry not supported with current security settings");
+    return false;
+#  endif
 }
 
 /**
@@ -1274,10 +1294,17 @@ bool NimBLEDevice::injectPassKey(const NimBLEConnInfo& peerInfo, uint32_t passke
  * @param [in] accept Whether the user confirmed or declined the comparison.
  */
 bool NimBLEDevice::injectConfirmPasskey(const NimBLEConnInfo& peerInfo, bool accept) {
+#  if MYNEWT_VAL(BLE_SM_SC)
     ble_sm_io pkey{.action = BLE_SM_IOACT_NUMCMP, .numcmp_accept = accept};
     int       rc = ble_sm_inject_io(peerInfo.getConnHandle(), &pkey);
     NIMBLE_LOGD(LOG_TAG, "BLE_SM_IOACT_NUMCMP; ble_sm_inject_io result: %d", rc);
     return rc == 0;
+#  else
+    (void)peerInfo;
+    (void)accept;
+    NIMBLE_LOGE(LOG_TAG, "Numeric comparison not supported with current security settings");
+    return false;
+#  endif
 }
 # endif // MYNEWT_VAL(BLE_ROLE_CENTRAL) || MYNEWT_VAL(BLE_ROLE_PERIPHERAL)
 
@@ -1290,23 +1317,24 @@ bool NimBLEDevice::injectConfirmPasskey(const NimBLEConnInfo& peerInfo, bool acc
  * @param [in] deviceName The name to set.
  */
 bool NimBLEDevice::setDeviceName(const std::string& deviceName) {
-#if !defined(MYNEWT_VAL_BLE_GATTS) || MYNEWT_VAL(BLE_GATTS) > 0
+# if !defined(MYNEWT_VAL_BLE_GATTS) || MYNEWT_VAL(BLE_GATTS) > 0
     int rc = ble_svc_gap_device_name_set(deviceName.c_str());
     if (rc != 0) {
         NIMBLE_LOGE(LOG_TAG, "Device name not set - too long");
         return false;
     }
-#endif
+# endif
     return true;
 } // setDeviceName
 
 /**
  * @brief Set a custom callback for gap events.
  * @param [in] handler The function to call when gap events occur.
+ * @param [in] arg Argument to pass to the handler.
  * @returns
  */
-bool NimBLEDevice::setCustomGapHandler(gap_event_handler handler) {
-    int rc = ble_gap_event_listener_register(&m_listener, handler, NULL);
+bool NimBLEDevice::setCustomGapHandler(gap_event_handler handler, void* arg) {
+    int rc = ble_gap_event_listener_register(&m_listener, handler, arg);
     if (rc == BLE_HS_EALREADY) {
         NIMBLE_LOGI(LOG_TAG, "Already listening to GAP events.");
         return true;
@@ -1325,7 +1353,15 @@ std::string NimBLEDevice::toString() {
     return getAddress().toString();
 } // toString
 
-# if CONFIG_NIMBLE_CPP_DEBUG_ASSERT_ENABLED || __DOXYGEN__
+/**
+ * @brief Return the library version as a string.
+ * @return A const char* containing library version information.
+ */
+const char* NimBLEDevice::getVersion() {
+    return NIMBLE_CPP_VERSION_STR;
+} // getVersion
+
+# if MYNEWT_VAL(NIMBLE_CPP_DEBUG_ASSERT_ENABLED) || __DOXYGEN__
 /**
  * @brief Debug assert - weak function.
  * @param [in] file The file where the assert occurred.
@@ -1336,7 +1372,7 @@ void nimble_cpp_assert(const char* file, unsigned line) {
     ble_npl_time_delay(10);
     abort();
 }
-# endif // CONFIG_NIMBLE_CPP_DEBUG_ASSERT_ENABLED
+# endif // MYNEWT_VAL(NIMBLE_CPP_DEBUG_ASSERT_ENABLED)
 
 void NimBLEDevice::setDeviceCallbacks(NimBLEDeviceCallbacks* cb) {
     m_pDeviceCallbacks = cb ? cb : &defaultDeviceCallbacks;

src/NimBLEDevice.h

@@ -18,18 +18,21 @@
 #ifndef NIMBLE_CPP_DEVICE_H_
 #define NIMBLE_CPP_DEVICE_H_
 
+#include "NimBLECppVersion.h"
 #include "syscfg/syscfg.h"
 #if CONFIG_BT_NIMBLE_ENABLED
 # ifdef ESP_PLATFORM
 #  ifndef CONFIG_IDF_TARGET_ESP32P4
 #   include <esp_bt.h>
 #  endif
+#  define NIMBLE_CPP_SCAN_DUPL_ENABLED \
+      (CONFIG_BTDM_BLE_SCAN_DUPL || CONFIG_BT_LE_SCAN_DUPL || CONFIG_BT_CTRL_BLE_SCAN_DUPL)
 # endif
 
-# if defined(CONFIG_NIMBLE_CPP_IDF)
-#  include <host/ble_gap.h>
+# ifdef USING_NIMBLE_ARDUINO_HEADERS
+#  include "nimble/nimble/host/include/host/ble_gap.h"
 # else
-#  include <nimble/nimble/host/include/host/ble_gap.h>
+#  include "host/ble_gap.h"
 # endif
 
 /****  FIX COMPILATION ****/
@@ -121,6 +124,7 @@ class NimBLEDevice {
     static bool          isInitialized();
     static NimBLEAddress getAddress();
     static std::string   toString();
+    static const char*   getVersion();
     static bool          whiteListAdd(const NimBLEAddress& address);
     static bool          whiteListRemove(const NimBLEAddress& address);
     static bool          onWhiteList(const NimBLEAddress& address);
@@ -133,7 +137,7 @@ class NimBLEDevice {
     static void          setScanDuplicateCacheSize(uint16_t cacheSize);
     static void          setScanFilterMode(uint8_t type);
     static void          setScanDuplicateCacheResetTime(uint16_t time);
-    static bool          setCustomGapHandler(gap_event_handler handler);
+    static bool          setCustomGapHandler(gap_event_handler handler, void* arg = nullptr);
     static void          setSecurityAuth(bool bonding, bool mitm, bool sc);
     static void          setSecurityAuth(uint8_t auth);
     static void          setSecurityIOCap(uint8_t iocap);
@@ -243,7 +247,7 @@ class NimBLEDevice {
 # endif
 
 # ifdef ESP_PLATFORM
-#  if CONFIG_BTDM_BLE_SCAN_DUPL || CONFIG_BT_LE_SCAN_DUPL
+#  if NIMBLE_CPP_SCAN_DUPL_ENABLED
     static uint16_t m_scanDuplicateSize;
     static uint8_t  m_scanFilterMode;
     static uint16_t m_scanDuplicateResetTime;
@@ -304,6 +308,7 @@ class NimBLEDevice {
 
 # if MYNEWT_VAL(BLE_ROLE_CENTRAL) || MYNEWT_VAL(BLE_ROLE_PERIPHERAL)
 #  include "NimBLEConnInfo.h"
+#  include "NimBLEStream.h"
 # endif
 
 # include "NimBLEAddress.h"

src/NimBLEExtAdvertising.cpp

@@ -18,10 +18,10 @@
 #include "NimBLEExtAdvertising.h"
 #if CONFIG_BT_NIMBLE_ENABLED && MYNEWT_VAL(BLE_ROLE_BROADCASTER) && MYNEWT_VAL(BLE_EXT_ADV)
 
-# if defined(CONFIG_NIMBLE_CPP_IDF)
-#  include "services/gap/ble_svc_gap.h"
-# else
+#ifdef USING_NIMBLE_ARDUINO_HEADERS
 #  include "nimble/nimble/host/services/gap/include/services/gap/ble_svc_gap.h"
+#else
+#  include "services/gap/ble_svc_gap.h"
 # endif
 
 # include "NimBLEDevice.h"
@@ -616,6 +616,11 @@ bool NimBLEExtAdvertisement::setFlags(uint8_t flag) {
  * @return True if successful.
  */
 bool NimBLEExtAdvertisement::setManufacturerData(const uint8_t* data, size_t length) {
+    if (length > 0xFF - 1) {
+        NIMBLE_LOGE(LOG_TAG, "Manufacturer data too long!");
+        return false;
+    }
+
     uint8_t header[2];
     header[0] = length + 1;
     header[1] = BLE_HS_ADV_TYPE_MFG_DATA;
@@ -652,6 +657,11 @@ bool NimBLEExtAdvertisement::setManufacturerData(const std::vector<uint8_t>& dat
  * @return True if successful.
  */
 bool NimBLEExtAdvertisement::setURI(const std::string& uri) {
+    if (uri.length() > 0xFF - 1) {
+        NIMBLE_LOGE(LOG_TAG, "URI too long!");
+        return false;
+    }
+
     uint8_t header[2];
     header[0] = uri.length() + 1;
     header[1] = BLE_HS_ADV_TYPE_URI;
@@ -670,6 +680,11 @@ bool NimBLEExtAdvertisement::setURI(const std::string& uri) {
  * @return True if successful.
  */
 bool NimBLEExtAdvertisement::setName(const std::string& name, bool isComplete) {
+    if (name.length() > 0xFF - 1) {
+        NIMBLE_LOGE(LOG_TAG, "Name too long!");
+        return false;
+    }
+
     uint8_t header[2];
     header[0] = name.length() + 1;
     header[1] = isComplete ? BLE_HS_ADV_TYPE_COMP_NAME : BLE_HS_ADV_TYPE_INCOMP_NAME;
@@ -917,8 +932,12 @@ bool NimBLEExtAdvertisement::setServices(bool complete, uint8_t size, const std:
  */
 bool NimBLEExtAdvertisement::setServiceData(const NimBLEUUID& uuid, const uint8_t* data, size_t length) {
     uint8_t uuidBytes = uuid.bitSize() / 8;
-    uint8_t sDataLen  = 2 + uuidBytes + length;
+    if (length + uuidBytes + 2 > 0xFF) {
+        NIMBLE_LOGE(LOG_TAG, "Service data too long!");
+        return false;
+    }
 
+    uint8_t sDataLen = 2 + uuidBytes + length;
     if (m_payload.size() + sDataLen > MYNEWT_VAL(BLE_EXT_ADV_MAX_SIZE)) {
         return false;
     }

src/NimBLEExtAdvertising.h

@@ -21,10 +21,10 @@
 #include "syscfg/syscfg.h"
 #if CONFIG_BT_NIMBLE_ENABLED && MYNEWT_VAL(BLE_ROLE_BROADCASTER) && MYNEWT_VAL(BLE_EXT_ADV)
 
-# if defined(CONFIG_NIMBLE_CPP_IDF)
-#  include "host/ble_gap.h"
-# else
+# ifdef USING_NIMBLE_ARDUINO_HEADERS
 #  include "nimble/nimble/host/include/host/ble_gap.h"
+# else
+#  include "host/ble_gap.h"
 # endif
 
 /****  FIX COMPILATION ****/

src/NimBLEHIDDevice.cpp

@@ -84,9 +84,7 @@ void NimBLEHIDDevice::setReportMap(uint8_t* map, uint16_t size) {
  * This function called when all the services have been created.
  */
 void NimBLEHIDDevice::startServices() {
-    m_deviceInfoSvc->start();
-    m_hidSvc->start();
-    m_batterySvc->start();
+    // no-op now, services started by server start.
 } // startServices
 
 /**

src/NimBLEHIDDevice.h

@@ -49,7 +49,8 @@ class NimBLEHIDDevice {
     NimBLEHIDDevice(NimBLEServer* server);
 
     void                  setReportMap(uint8_t* map, uint16_t);
-    void                  startServices();
+    void                  startServices() __attribute__((deprecated("Services are now started by the server when start() is called, " 
+                                                         "this function is no longer needed and will be removed in a future release.")));
     bool                  setManufacturer(const std::string& name);
     void                  setPnp(uint8_t sig, uint16_t vid, uint16_t pid, uint16_t version);
     void                  setHidInfo(uint8_t country, uint8_t flags);

src/NimBLEL2CAPChannel.cpp

@@ -9,10 +9,10 @@
 # include "NimBLELog.h"
 # include "NimBLEUtils.h"
 
-# if defined(CONFIG_NIMBLE_CPP_IDF)
-#  include "host/ble_gap.h"
-# else
+# ifdef USING_NIMBLE_ARDUINO_HEADERS
 #  include "nimble/nimble/host/include/host/ble_gap.h"
+# else
+#  include "host/ble_gap.h"
 # endif
 
 // L2CAP buffer block size
@@ -134,8 +134,14 @@ int NimBLEL2CAPChannel::writeFragment(std::vector<uint8_t>::const_iterator begin
 
             case BLE_HS_ENOMEM:
             case BLE_HS_EAGAIN:
+                /* ble_l2cap_send already consumed and freed txd on these errors */
+                NIMBLE_LOGD(LOG_TAG, "ble_l2cap_send returned %d (consumed buffer). Retrying shortly...", res);
+                ble_npl_time_delay(ble_npl_time_ms_to_ticks32(RetryTimeout));
+                continue;
+
             case BLE_HS_EBUSY:
-                NIMBLE_LOGD(LOG_TAG, "ble_l2cap_send returned %d. Retrying shortly...", res);
+                /* Channel busy; txd not consumed */
+                NIMBLE_LOGD(LOG_TAG, "ble_l2cap_send returned %d (busy). Retrying shortly...", res);
                 os_mbuf_free_chain(txd);
                 ble_npl_time_delay(ble_npl_time_ms_to_ticks32(RetryTimeout));
                 continue;
@@ -197,6 +203,28 @@ bool NimBLEL2CAPChannel::write(const std::vector<uint8_t>& bytes) {
     return true;
 }
 
+bool NimBLEL2CAPChannel::disconnect() {
+    if (!this->channel) {
+        NIMBLE_LOGW(LOG_TAG, "L2CAP Channel not open");
+        return false;
+    }
+
+    int rc = ble_l2cap_disconnect(this->channel);
+    if (rc != 0 && rc != BLE_HS_ENOTCONN && rc != BLE_HS_EALREADY) {
+        NIMBLE_LOGE(LOG_TAG, "ble_l2cap_disconnect failed: rc=%d %s", rc, NimBLEUtils::returnCodeToString(rc));
+        return false;
+    }
+
+    return true;
+}
+
+uint16_t NimBLEL2CAPChannel::getConnHandle() const {
+    if (!this->channel) {
+        return BLE_HS_CONN_HANDLE_NONE;
+    }
+    return ble_l2cap_get_conn_handle(this->channel);
+}
+
 // private
 int NimBLEL2CAPChannel::handleConnectionEvent(struct ble_l2cap_event* event) {
     channel = event->connect.chan;

src/NimBLEL2CAPChannel.h

@@ -9,12 +9,12 @@
 #if CONFIG_BT_NIMBLE_ENABLED && MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM)
 
 # include "inttypes.h"
-# if defined(CONFIG_NIMBLE_CPP_IDF)
-#  include "host/ble_l2cap.h"
-#  include "os/os_mbuf.h"
-# else
+# ifdef USING_NIMBLE_ARDUINO_HEADERS
 #  include "nimble/nimble/host/include/host/ble_l2cap.h"
 #  include "nimble/porting/nimble/include/os/os_mbuf.h"
+# else
+#  include "host/ble_l2cap.h"
+#  include "os/os_mbuf.h"
 # endif
 
 /****  FIX COMPILATION ****/
@@ -56,6 +56,14 @@ class NimBLEL2CAPChannel {
     /// NOTE: This function will block until the data has been sent or an error occurred.
     bool write(const std::vector<uint8_t>& bytes);
 
+    /// @brief Disconnect this L2CAP channel.
+    /// @return true on success, false on failure.
+    bool disconnect();
+
+    /// @brief Get the connection handle associated with this channel.
+    /// @return Connection handle, or BLE_HS_CONN_HANDLE_NONE if not connected.
+    uint16_t getConnHandle() const;
+
     /// @return True, if the channel is connected. False, otherwise.
     bool isConnected() const { return !!channel; }
 

src/NimBLELocalValueAttribute.h

@@ -21,10 +21,10 @@
 #include "syscfg/syscfg.h"
 #if CONFIG_BT_NIMBLE_ENABLED && MYNEWT_VAL(BLE_ROLE_PERIPHERAL)
 
-# if defined(CONFIG_NIMBLE_CPP_IDF)
-#  include "host/ble_hs.h"
-# else
+#ifdef USING_NIMBLE_ARDUINO_HEADERS
 #  include "nimble/nimble/host/include/host/ble_hs.h"
+# else
+#  include "host/ble_hs.h"
 # endif
 
 /****  FIX COMPILATION ****/
@@ -101,7 +101,7 @@ class NimBLELocalValueAttribute : public NimBLELocalAttribute, public NimBLEValu
     NimBLELocalValueAttribute(const NimBLEUUID& uuid,
                               uint16_t          handle,
                               uint16_t          maxLen,
-                              uint16_t          initLen = CONFIG_NIMBLE_CPP_ATT_VALUE_INIT_LENGTH)
+                              uint16_t          initLen = MYNEWT_VAL(NIMBLE_CPP_ATT_VALUE_INIT_LENGTH))
         : NimBLELocalAttribute(uuid, handle), NimBLEValueAttribute(maxLen, initLen) {}
     /**
      * @brief Destroy the NimBLELocalValueAttribute object.

src/NimBLELog.h

@@ -21,12 +21,21 @@
 #include "syscfg/syscfg.h"
 #if CONFIG_BT_NIMBLE_ENABLED
 
-# if defined(CONFIG_NIMBLE_CPP_IDF)
+# ifndef MYNEWT_VAL_NIMBLE_CPP_LOG_LEVEL
+#  ifndef CONFIG_NIMBLE_CPP_LOG_LEVEL
+#   if defined(ARDUINO_ARCH_ESP32) && defined(CORE_DEBUG_LEVEL)
+#    define MYNEWT_VAL_NIMBLE_CPP_LOG_LEVEL CORE_DEBUG_LEVEL
+#   else
+#    define MYNEWT_VAL_NIMBLE_CPP_LOG_LEVEL 0
+#   endif
+#  else
+#   define MYNEWT_VAL_NIMBLE_CPP_LOG_LEVEL CONFIG_NIMBLE_CPP_LOG_LEVEL
+#  endif
+# endif
+
+# ifndef USING_NIMBLE_ARDUINO_HEADERS
 #  include "esp_log.h"
 #  include "console/console.h"
-#  ifndef CONFIG_NIMBLE_CPP_LOG_LEVEL
-#   define CONFIG_NIMBLE_CPP_LOG_LEVEL 0
-#  endif
 
 #  if defined(CONFIG_NIMBLE_CPP_LOG_OVERRIDE_COLOR)
 #   if CONFIG_LOG_COLORS
@@ -120,13 +129,13 @@
 
 #   define NIMBLE_CPP_LOG_PRINT(level, tag, format, ...)                                                            \
       do {                                                                                                          \
-        if (CONFIG_NIMBLE_CPP_LOG_LEVEL >= level) NIMBLE_CPP_LOG_LEVEL_LOCAL(level, tag, format, ##__VA_ARGS__);    \
+        if (MYNEWT_VAL(NIMBLE_CPP_LOG_LEVEL) >= level) NIMBLE_CPP_LOG_LEVEL_LOCAL(level, tag, format, ##__VA_ARGS__);    \
       } while (0)
 
 #  else
 #   define NIMBLE_CPP_LOG_PRINT(level, tag, format, ...)                                                    \
       do {                                                                                                  \
-        if (CONFIG_NIMBLE_CPP_LOG_LEVEL >= level) ESP_LOG_LEVEL_LOCAL(level, tag, format, ##__VA_ARGS__);   \
+        if (MYNEWT_VAL(NIMBLE_CPP_LOG_LEVEL) >= level) ESP_LOG_LEVEL_LOCAL(level, tag, format, ##__VA_ARGS__);   \
       } while (0)
 
 #  endif /* CONFIG_NIMBLE_CPP_LOG_OVERRIDE_COLOR */
@@ -137,41 +146,33 @@
 #  define NIMBLE_LOGE(tag, format, ...) NIMBLE_CPP_LOG_PRINT(ESP_LOG_ERROR, tag, format, ##__VA_ARGS__)
 
 # else
-#  include "syscfg/syscfg.h"
 #  include "nimble/console/console.h"
-#  ifndef CONFIG_NIMBLE_CPP_LOG_LEVEL
-#   if defined(ARDUINO_ARCH_ESP32) && defined(CORE_DEBUG_LEVEL)
-#    define CONFIG_NIMBLE_CPP_LOG_LEVEL CORE_DEBUG_LEVEL
-#   else
-#    define CONFIG_NIMBLE_CPP_LOG_LEVEL 0
-#   endif
-#  endif
 
-#  if CONFIG_NIMBLE_CPP_LOG_LEVEL >= 4
+#  if MYNEWT_VAL(NIMBLE_CPP_LOG_LEVEL) >= 4
 #   define NIMBLE_LOGD(tag, format, ...) console_printf("D %s: " format "\n", tag, ##__VA_ARGS__)
 #  else
 #   define NIMBLE_LOGD(tag, format, ...) (void)tag
 #  endif
 
-#  if CONFIG_NIMBLE_CPP_LOG_LEVEL >= 3
+#  if MYNEWT_VAL(NIMBLE_CPP_LOG_LEVEL) >= 3
 #   define NIMBLE_LOGI(tag, format, ...) console_printf("I %s: " format "\n", tag, ##__VA_ARGS__)
 #  else
 #   define NIMBLE_LOGI(tag, format, ...) (void)tag
 #  endif
 
-#  if CONFIG_NIMBLE_CPP_LOG_LEVEL >= 2
+#  if MYNEWT_VAL(NIMBLE_CPP_LOG_LEVEL) >= 2
 #   define NIMBLE_LOGW(tag, format, ...) console_printf("W %s: " format "\n", tag, ##__VA_ARGS__)
 #  else
 #   define NIMBLE_LOGW(tag, format, ...) (void)tag
 #  endif
 
-#  if CONFIG_NIMBLE_CPP_LOG_LEVEL >= 1
+#  if MYNEWT_VAL(NIMBLE_CPP_LOG_LEVEL) >= 1
 #   define NIMBLE_LOGE(tag, format, ...) console_printf("E %s: " format "\n", tag, ##__VA_ARGS__)
 #  else
 #   define NIMBLE_LOGE(tag, format, ...) (void)tag
 #  endif
 
-# endif  /* CONFIG_NIMBLE_CPP_IDF */
+# endif  /* !USING_NIMBLE_ARDUINO_HEADERS */
 
 #  define NIMBLE_LOGD_IF(cond, tag, format, ...) { if (cond) { NIMBLE_LOGD(tag, format, ##__VA_ARGS__); }}
 #  define NIMBLE_LOGI_IF(cond, tag, format, ...) { if (cond) { NIMBLE_LOGI(tag, format, ##__VA_ARGS__); }}

src/NimBLERemoteCharacteristic.cpp

@@ -94,8 +94,24 @@ int NimBLERemoteCharacteristic::descriptorDiscCB(
 bool NimBLERemoteCharacteristic::retrieveDescriptors(NimBLEDescriptorFilter* pFilter) const {
     NIMBLE_LOGD(LOG_TAG, ">> retrieveDescriptors() for characteristic: %s", getUUID().toString().c_str());
 
+    const auto pSvc      = getRemoteService();
+    uint16_t   endHandle = pSvc->getEndHandle();
+
+    // Find the handle of the next characteristic to limit the descriptor search range.
+    const auto& chars = pSvc->getCharacteristics(false);
+    for (auto it = chars.begin(); it != chars.end(); ++it) {
+        if ((*it)->getHandle() == this->getHandle()) {
+            auto next_it = std::next(it);
+            if (next_it != chars.end()) {
+                endHandle = (*next_it)->getHandle() - 1;
+                NIMBLE_LOGD(LOG_TAG, "Search range limited to handle 0x%04X", endHandle);
+            }
+            break;
+        }
+    }
+
     // If this is the last handle then there are no descriptors
-    if (getHandle() == getRemoteService()->getEndHandle()) {
+    if (getHandle() == endHandle) {
         NIMBLE_LOGD(LOG_TAG, "<< retrieveDescriptors(): found 0 descriptors.");
         return true;
     }
@@ -108,7 +124,7 @@ bool NimBLERemoteCharacteristic::retrieveDescriptors(NimBLEDescriptorFilter* pFi
 
     int rc = ble_gattc_disc_all_dscs(getClient()->getConnHandle(),
                                      getHandle(),
-                                     getRemoteService()->getEndHandle(),
+                                     endHandle,
                                      NimBLERemoteCharacteristic::descriptorDiscCB,
                                      pFilter);
     if (rc != 0) {

src/NimBLERemoteService.cpp

@@ -76,31 +76,22 @@ NimBLERemoteCharacteristic* NimBLERemoteService::getCharacteristic(const char* u
 NimBLERemoteCharacteristic* NimBLERemoteService::getCharacteristic(const NimBLEUUID& uuid) const {
     NIMBLE_LOGD(LOG_TAG, ">> getCharacteristic: uuid: %s", uuid.toString().c_str());
     NimBLERemoteCharacteristic* pChar     = nullptr;
-    size_t                      prev_size = m_vChars.size();
 
     for (const auto& it : m_vChars) {
         if (it->getUUID() == uuid) {
             pChar = it;
-            goto Done;
+            NIMBLE_LOGD(LOG_TAG, "<< getCharacteristic: found in cache");
+            return pChar;
         }
     }
 
-    if (retrieveCharacteristics(&uuid)) {
-        if (m_vChars.size() > prev_size) {
-            pChar = m_vChars.back();
-            goto Done;
-        }
-
+    if (retrieveCharacteristics(&uuid, &pChar) && pChar == nullptr) {
         // If the request was successful but 16/32 bit uuid 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();
-            if (retrieveCharacteristics(&uuid128)) {
-                if (m_vChars.size() > prev_size) {
-                    pChar = m_vChars.back();
-                }
-            }
+            retrieveCharacteristics(&uuid128, &pChar);
         } else {
             // If the request was successful but the 128 bit uuid not found
             // try again with the 16 bit uuid.
@@ -108,16 +99,11 @@ NimBLERemoteCharacteristic* NimBLERemoteService::getCharacteristic(const NimBLEU
             uuid16.to16();
             // if the uuid was 128 bit but not of the BLE base type this check will fail
             if (uuid16.bitSize() == BLE_UUID_TYPE_16) {
-                if (retrieveCharacteristics(&uuid16)) {
-                    if (m_vChars.size() > prev_size) {
-                        pChar = m_vChars.back();
-                    }
-                }
+                retrieveCharacteristics(&uuid16, &pChar);
             }
         }
     }
 
-Done:
     NIMBLE_LOGD(LOG_TAG, "<< Characteristic %sfound", pChar ? "" : "not ");
     return pChar;
 } // getCharacteristic
@@ -165,7 +151,18 @@ int NimBLERemoteService::characteristicDiscCB(uint16_t              conn_handle,
     }
 
     if (error->status == 0) {
-        pSvc->m_vChars.push_back(new NimBLERemoteCharacteristic(pSvc, chr));
+        // insert in handle order
+        auto pNewChar = new NimBLERemoteCharacteristic(pSvc, chr);
+        for (auto it = pSvc->m_vChars.begin(); it != pSvc->m_vChars.end(); ++it) {
+            if ((*it)->getHandle() > chr->def_handle) {
+                pSvc->m_vChars.insert(it, pNewChar);
+                pTaskData->m_pBuf = pNewChar;
+                return 0;
+            }
+        }
+
+        pSvc->m_vChars.push_back(pNewChar);
+        pTaskData->m_pBuf = pNewChar;
         return 0;
     }
 
@@ -179,7 +176,7 @@ int NimBLERemoteService::characteristicDiscCB(uint16_t              conn_handle,
  * This function will not return until we have all the characteristics.
  * @return True if successful.
  */
-bool NimBLERemoteService::retrieveCharacteristics(const NimBLEUUID* uuidFilter) const {
+bool NimBLERemoteService::retrieveCharacteristics(const NimBLEUUID* uuidFilter, NimBLERemoteCharacteristic** ppChar) const {
     NIMBLE_LOGD(LOG_TAG, ">> retrieveCharacteristics()");
     int            rc = 0;
     NimBLETaskData taskData(const_cast<NimBLERemoteService*>(this));
@@ -207,6 +204,9 @@ bool NimBLERemoteService::retrieveCharacteristics(const NimBLEUUID* uuidFilter)
     NimBLEUtils::taskWait(taskData, BLE_NPL_TIME_FOREVER);
     rc = taskData.m_flags;
     if (rc == 0 || rc == BLE_HS_EDONE) {
+        if (ppChar != nullptr) {
+            *ppChar = static_cast<NimBLERemoteCharacteristic*>(taskData.m_pBuf);
+        }
         NIMBLE_LOGD(LOG_TAG, "<< retrieveCharacteristics()");
         return true;
     }

src/NimBLERemoteService.h

@@ -53,7 +53,7 @@ class NimBLERemoteService : public NimBLEAttribute {
 
     NimBLERemoteService(NimBLEClient* pClient, const struct ble_gatt_svc* service);
     ~NimBLERemoteService();
-    bool       retrieveCharacteristics(const NimBLEUUID* uuidFilter = nullptr) const;
+    bool retrieveCharacteristics(const NimBLEUUID* uuidFilter = nullptr, NimBLERemoteCharacteristic** ppChar = nullptr) const;
     static int characteristicDiscCB(uint16_t                     conn_handle,
                                     const struct ble_gatt_error* error,
                                     const struct ble_gatt_chr*   chr,

src/NimBLERemoteValueAttribute.h

@@ -21,10 +21,10 @@
 #include "syscfg/syscfg.h"
 #if CONFIG_BT_NIMBLE_ENABLED && MYNEWT_VAL(BLE_ROLE_CENTRAL)
 
-# if defined(CONFIG_NIMBLE_CPP_IDF)
-#  include <host/ble_gatt.h>
+#ifdef USING_NIMBLE_ARDUINO_HEADERS
+#  include "nimble/nimble/host/include/host/ble_gatt.h"
 # else
-#  include <nimble/nimble/host/include/host/ble_gatt.h>
+#  include "host/ble_gatt.h"
 # endif
 
 /****  FIX COMPILATION ****/
@@ -109,13 +109,34 @@ class NimBLERemoteValueAttribute : public NimBLEValueAttribute, public NimBLEAtt
      * @brief Template to set the remote characteristic value to <type\>val.
      * @param [in] v The value to write.
      * @param [in] response True == request write response.
-     * @details Only used if the <type\> has a `data()` and `size()` method.
+     * @details Only used if the <type\> has a `data()` and `size()` method with `value_type`.
+     * Correctly calculates byte size for containers with multi-byte element types.
      */
     template <typename T>
 #  ifdef _DOXYGEN_
     bool
 #  else
-    typename std::enable_if<Has_data_size<T>::value, bool>::type
+    typename std::enable_if<Has_data_size<T>::value && Has_value_type<T>::value, bool>::type
+#  endif
+    writeValue(const T& v, bool response = false) const {
+        return writeValue(
+            reinterpret_cast<const uint8_t*>(v.data()),
+            v.size() * sizeof(typename T::value_type),
+            response
+        );
+    }
+
+    /**
+     * @brief Template to set the remote characteristic value to <type\>val.
+     * @param [in] v The value to write.
+     * @param [in] response True == request write response.
+     * @details Only used if the <type\> has a `data()` and `size()` method without `value_type`.
+     */
+    template <typename T>
+#  ifdef _DOXYGEN_
+    bool
+#  else
+    typename std::enable_if<Has_data_size<T>::value && !Has_value_type<T>::value, bool>::type
 #  endif
     writeValue(const T& v, bool response = false) const {
         return writeValue(reinterpret_cast<const uint8_t*>(v.data()), v.size(), response);
@@ -131,7 +152,11 @@ class NimBLERemoteValueAttribute : public NimBLEValueAttribute, public NimBLEAtt
     template <typename T>
     typename std::enable_if<!std::is_pointer<T>::value, bool>::type writeValue(const T& v, bool response = false) const {
         if constexpr (Has_data_size<T>::value) {
-            return writeValue(reinterpret_cast<const uint8_t*>(v.data()), v.size(), response);
+            if constexpr (Has_value_type<T>::value) {
+                return writeValue(reinterpret_cast<const uint8_t*>(v.data()), v.size() * sizeof(typename T::value_type), response);
+            } else {
+                return writeValue(reinterpret_cast<const uint8_t*>(v.data()), v.size(), response);
+            }
         } else if constexpr (Has_c_str_length<T>::value) {
             return writeValue(reinterpret_cast<const uint8_t*>(v.c_str()), v.length(), response);
         } else {

src/NimBLEScan.cpp

@@ -20,32 +20,192 @@
 
 # include "NimBLEDevice.h"
 # include "NimBLELog.h"
+# ifdef USING_NIMBLE_ARDUINO_HEADERS
+#  include "nimble/porting/nimble/include/nimble/nimble_port.h"
+# else
+#  include "nimble/nimble_port.h"
+# endif
 
 # include <string>
 # include <climits>
 
+# define DEFAULT_SCAN_RESP_TIMEOUT_MS 10240 // max advertising interval (10.24s)
+
 static const char*         LOG_TAG = "NimBLEScan";
 static NimBLEScanCallbacks defaultScanCallbacks;
 
+/**
+ * @brief This handles an event run in the host task when the scan response timeout for the head of
+ * the waiting list is triggered and directly invokes the onResult callback with the current device.
+ */
+void NimBLEScan::srTimerCb(ble_npl_event* event) {
+    auto pScan = NimBLEDevice::getScan();
+    auto pDev  = pScan->m_pWaitingListHead;
+
+    if (pDev == nullptr) {
+        ble_npl_callout_stop(&pScan->m_srTimer);
+        return;
+    }
+
+    if (ble_npl_time_get() - pDev->m_time < pScan->m_srTimeoutTicks) {
+        // This can happen if a scan response was received and the device was removed from the waiting list
+        // after this was put in the queue. In this case, just reset the timer for this device.
+        pScan->resetWaitingTimer();
+        return;
+    }
+
+    NIMBLE_LOGI(LOG_TAG, "Scan response timeout for: %s", pDev->getAddress().toString().c_str());
+    pScan->m_stats.incMissedSrCount();
+    pScan->removeWaitingDevice(pDev);
+    pDev->m_callbackSent = 2;
+    pScan->m_pScanCallbacks->onResult(pDev);
+    if (pScan->m_maxResults == 0) {
+        pScan->erase(pDev);
+    }
+}
+
 /**
  * @brief Scan constructor.
  */
 NimBLEScan::NimBLEScan()
     : m_pScanCallbacks{&defaultScanCallbacks},
-      // default interval + window, no whitelist scan filter,not limited scan, no scan response, filter_duplicates
-      m_scanParams{0, 0, BLE_HCI_SCAN_FILT_NO_WL, 0, 1, 1},
+      m_scanParams{
+          .itvl              = 0,                       // default interval
+          .window            = 0,                       // default window
+          .filter_policy     = BLE_HCI_SCAN_FILT_NO_WL, // no whitelist scan filter
+          .limited           = 0,                       // no limited scan
+          .passive           = 1,                       // no scan response
+          .filter_duplicates = 1,                       // filter duplicates
+# if defined(ESP_PLATFORM) && !defined(CONFIG_USING_NIMBLE_COMPONENT)
+#  if ESP_IDF_VERSION >= ESP_IDF_VERSION_VAL(5, 4, 2)
+          .disable_observer_mode = 0, // observer role enabled
+#  endif
+# endif
+      },
       m_pTaskData{nullptr},
-      m_maxResults{0xFF} {}
+      m_maxResults{0xFF} {
+    ble_npl_callout_init(&m_srTimer, nimble_port_get_dflt_eventq(), NimBLEScan::srTimerCb, nullptr);
+    ble_npl_time_ms_to_ticks(DEFAULT_SCAN_RESP_TIMEOUT_MS, &m_srTimeoutTicks);
+} // NimBLEScan::NimBLEScan
 
 /**
  * @brief Scan destructor, release any allocated resources.
  */
 NimBLEScan::~NimBLEScan() {
+    ble_npl_callout_deinit(&m_srTimer);
+
     for (const auto& dev : m_scanResults.m_deviceVec) {
         delete dev;
     }
 }
 
+/**
+ * @brief Add a device to the waiting list for scan responses.
+ * @param [in] pDev The device to add to the list.
+ */
+void NimBLEScan::addWaitingDevice(NimBLEAdvertisedDevice* pDev) {
+    if (pDev == nullptr) {
+        return;
+    }
+
+    ble_npl_hw_enter_critical();
+
+    // Self-pointer is the "not in list" sentinel; anything else means already in list.
+    if (pDev->m_pNextWaiting != pDev) {
+        ble_npl_hw_exit_critical(0);
+        return;
+    }
+
+    // Initialize link field before inserting into the list.
+    pDev->m_pNextWaiting = nullptr;
+    if (m_pWaitingListTail == nullptr) {
+        m_pWaitingListHead = pDev;
+        m_pWaitingListTail = pDev;
+        ble_npl_hw_exit_critical(0);
+        return;
+    }
+
+    m_pWaitingListTail->m_pNextWaiting = pDev;
+    m_pWaitingListTail                 = pDev;
+    ble_npl_hw_exit_critical(0);
+}
+
+/**
+ * @brief Remove a device from the waiting list.
+ * @param [in] pDev The device to remove from the list.
+ */
+void NimBLEScan::removeWaitingDevice(NimBLEAdvertisedDevice* pDev) {
+    if (pDev == nullptr) {
+        return;
+    }
+
+    if (pDev->m_pNextWaiting == pDev) {
+        return; // Not in the list
+    }
+
+    bool resetTimer = false;
+    ble_npl_hw_enter_critical();
+    if (m_pWaitingListHead == pDev) {
+        m_pWaitingListHead = pDev->m_pNextWaiting;
+        if (m_pWaitingListHead == nullptr) {
+            m_pWaitingListTail = nullptr;
+        } else {
+            resetTimer = true;
+        }
+    } else {
+        NimBLEAdvertisedDevice* current = m_pWaitingListHead;
+        while (current != nullptr) {
+            if (current->m_pNextWaiting == pDev) {
+                current->m_pNextWaiting = pDev->m_pNextWaiting;
+                if (m_pWaitingListTail == pDev) {
+                    m_pWaitingListTail = current;
+                }
+                break;
+            }
+            current = current->m_pNextWaiting;
+        }
+    }
+    ble_npl_hw_exit_critical(0);
+    pDev->m_pNextWaiting = pDev; // Restore sentinel: self-pointer means "not in list"
+    if (resetTimer) {
+        resetWaitingTimer();
+    }
+}
+
+/**
+ * @brief Clear all devices from the waiting list.
+ */
+void NimBLEScan::clearWaitingList() {
+    // Stop the timer and remove any pending timeout events since we're clearing
+    // the list and won't be processing any more timeouts for these devices
+    ble_npl_callout_stop(&m_srTimer);
+    ble_npl_hw_enter_critical();
+    NimBLEAdvertisedDevice* current = m_pWaitingListHead;
+    while (current != nullptr) {
+        NimBLEAdvertisedDevice* next = current->m_pNextWaiting;
+        current->m_pNextWaiting      = current; // Restore sentinel
+        current                      = next;
+    }
+    m_pWaitingListHead = nullptr;
+    m_pWaitingListTail = nullptr;
+    ble_npl_hw_exit_critical(0);
+}
+
+/**
+ * @brief Reset the timer for the next waiting device at the head of the FIFO list.
+ */
+void NimBLEScan::resetWaitingTimer() {
+    if (m_srTimeoutTicks == 0 || m_pWaitingListHead == nullptr) {
+        ble_npl_callout_stop(&m_srTimer);
+        return;
+    }
+
+    ble_npl_time_t now      = ble_npl_time_get();
+    ble_npl_time_t elapsed  = now - m_pWaitingListHead->m_time;
+    ble_npl_time_t nextTime = elapsed >= m_srTimeoutTicks ? 1 : m_srTimeoutTicks - elapsed;
+    ble_npl_callout_reset(&m_srTimer, nextTime);
+}
+
 /**
  * @brief Handle GAP events related to scans.
  * @param [in] event The event type for this event.
@@ -101,6 +261,8 @@ int NimBLEScan::handleGapEvent(ble_gap_event* event, void* arg) {
             // If we haven't seen this device before; create a new instance and insert it in the vector.
             // Otherwise just update the relevant parameters of the already known device.
             if (advertisedDevice == nullptr) {
+                pScan->m_stats.incDevCount();
+
                 // Check if we have reach the scan results limit, ignore this one if so.
                 // We still need to store each device when maxResults is 0 to be able to append the scan results
                 if (pScan->m_maxResults > 0 && pScan->m_maxResults < 0xFF &&
@@ -109,21 +271,50 @@ int NimBLEScan::handleGapEvent(ble_gap_event* event, void* arg) {
                 }
 
                 if (isLegacyAdv && event_type == BLE_HCI_ADV_RPT_EVTYPE_SCAN_RSP) {
+                    pScan->m_stats.incOrphanedSrCount();
                     NIMBLE_LOGI(LOG_TAG, "Scan response without advertisement: %s", advertisedAddress.toString().c_str());
                 }
 
                 advertisedDevice = new NimBLEAdvertisedDevice(event, event_type);
                 pScan->m_scanResults.m_deviceVec.push_back(advertisedDevice);
+                advertisedDevice->m_time = ble_npl_time_get();
                 NIMBLE_LOGI(LOG_TAG, "New advertiser: %s", advertisedAddress.toString().c_str());
             } else {
                 advertisedDevice->update(event, event_type);
-                if (isLegacyAdv && event_type == BLE_HCI_ADV_RPT_EVTYPE_SCAN_RSP) {
-                    NIMBLE_LOGI(LOG_TAG, "Scan response from: %s", advertisedAddress.toString().c_str());
-                } else {
-                    NIMBLE_LOGI(LOG_TAG, "Duplicate; updated: %s", advertisedAddress.toString().c_str());
+                if (isLegacyAdv) {
+                    if (event_type == BLE_HCI_ADV_RPT_EVTYPE_SCAN_RSP) {
+                        pScan->m_stats.recordSrTime(ble_npl_time_get() - advertisedDevice->m_time);
+                        NIMBLE_LOGI(LOG_TAG, "Scan response from: %s", advertisedAddress.toString().c_str());
+                        // Remove device from waiting list since we got the response
+                        pScan->removeWaitingDevice(advertisedDevice);
+                    } else {
+                        pScan->m_stats.incDupCount();
+                        NIMBLE_LOGI(LOG_TAG, "Duplicate; updated: %s", advertisedAddress.toString().c_str());
+                        // Restart scan-response timeout when we see a new non-scan-response
+                        // legacy advertisement during active scanning for a scannable device.
+                        advertisedDevice->m_time = ble_npl_time_get();
+                        // Re-add to the tail so FIFO timeout order matches advertisement order.
+                        if (advertisedDevice->isScannable()) {
+                            pScan->removeWaitingDevice(advertisedDevice);
+                            pScan->addWaitingDevice(advertisedDevice);
+                        }
+
+                        // If we're not filtering duplicates, we need to reset the callbackSent count
+                        // so that callbacks will be triggered again for this device
+                        if (!pScan->m_scanParams.filter_duplicates) {
+                            advertisedDevice->m_callbackSent = 0;
+                        }
+                    }
                 }
             }
 
+# if MYNEWT_VAL(BLE_EXT_ADV)
+            if (advertisedDevice->getDataStatus() == BLE_GAP_EXT_ADV_DATA_STATUS_INCOMPLETE) {
+                NIMBLE_LOGD(LOG_TAG, "EXT ADV data incomplete, waiting for more");
+                return 0;
+            }
+# endif
+
             if (!advertisedDevice->m_callbackSent) {
                 advertisedDevice->m_callbackSent++;
                 pScan->m_pScanCallbacks->onDiscovered(advertisedDevice);
@@ -138,6 +329,12 @@ int NimBLEScan::handleGapEvent(ble_gap_event* event, void* arg) {
                 advertisedDevice->m_callbackSent++;
                 // got the scan response report the full data.
                 pScan->m_pScanCallbacks->onResult(advertisedDevice);
+            } else if (isLegacyAdv && advertisedDevice->isScannable()) {
+                // Add to waiting list for scan response and start the timer
+                pScan->addWaitingDevice(advertisedDevice);
+                if (pScan->m_pWaitingListHead == advertisedDevice) {
+                    pScan->resetWaitingTimer();
+                }
             }
 
             // If not storing results and we have invoked the callback, delete the device.
@@ -149,12 +346,26 @@ int NimBLEScan::handleGapEvent(ble_gap_event* event, void* arg) {
         }
 
         case BLE_GAP_EVENT_DISC_COMPLETE: {
-            NIMBLE_LOGD(LOG_TAG, "discovery complete; reason=%d", event->disc_complete.reason);
+            ble_npl_callout_stop(&pScan->m_srTimer);
+
+            // If we have any scannable devices that haven't received a scan response,
+            // we should trigger the callback with whatever data we have since the scan is complete
+            // and we won't be getting any more updates for these devices.
+            while (pScan->m_pWaitingListHead != nullptr) {
+                auto pDev = pScan->m_pWaitingListHead;
+                pScan->m_stats.incMissedSrCount();
+                pScan->removeWaitingDevice(pDev);
+                pDev->m_callbackSent = 2;
+                pScan->m_pScanCallbacks->onResult(pDev);
+            }
 
             if (pScan->m_maxResults == 0) {
                 pScan->clearResults();
             }
 
+            NIMBLE_LOGD(LOG_TAG, "discovery complete; reason=%d", event->disc_complete.reason);
+            NIMBLE_LOGD(LOG_TAG, "%s", pScan->getStatsString().c_str());
+
             pScan->m_pScanCallbacks->onScanEnd(pScan->m_scanResults, event->disc_complete.reason);
 
             if (pScan->m_pTaskData != nullptr) {
@@ -169,6 +380,27 @@ int NimBLEScan::handleGapEvent(ble_gap_event* event, void* arg) {
     }
 } // handleGapEvent
 
+/**
+ * @brief Set the scan response timeout.
+ * @param [in] timeoutMs The timeout in milliseconds to wait for a scan response, default: max advertising interval (10.24s)
+ * @details If a scan response is not received within the timeout period,
+ * the pending device will be reported to the scan result callback with whatever
+ * data was present in the advertisement; no synthetic scan-response event is generated.
+ * If set to 0, the scan result callback will only be triggered when a scan response
+ * is received from the advertiser or when the scan completes, at which point any
+ * pending scannable devices will be reported with the advertisement data only.
+ */
+void NimBLEScan::setScanResponseTimeout(uint32_t timeoutMs) {
+    if (timeoutMs == 0) {
+        ble_npl_callout_stop(&m_srTimer);
+        m_srTimeoutTicks = 0;
+        return;
+    }
+
+    ble_npl_time_ms_to_ticks(timeoutMs, &m_srTimeoutTicks);
+    resetWaitingTimer();
+} // setScanResponseTimeout
+
 /**
  * @brief Should we perform an active or passive scan?
  * The default is a passive scan. An active scan means that we will request a scan response.
@@ -199,7 +431,7 @@ void NimBLEScan::setDuplicateFilter(uint8_t enabled) {
  */
 void NimBLEScan::setLimitedOnly(bool enabled) {
     m_scanParams.limited = enabled;
-} // setLimited
+} // setLimitedOnly
 
 /**
  * @brief Sets the scan filter policy.
@@ -314,11 +546,13 @@ bool NimBLEScan::start(uint32_t duration, bool isContinue, bool restart) {
 
             if (!isContinue) {
                 clearResults();
+                m_stats.reset();
             }
         }
     } else { // Don't clear results while scanning is active
         if (!isContinue) {
             clearResults();
+            m_stats.reset();
         }
     }
 
@@ -330,15 +564,15 @@ bool NimBLEScan::start(uint32_t duration, bool isContinue, bool restart) {
     scan_params.itvl    = m_scanParams.itvl;
     scan_params.window  = m_scanParams.window;
     int rc              = ble_gap_ext_disc(NimBLEDevice::m_ownAddrType,
-                              duration / 10, // 10ms units
-                              m_period,
-                              m_scanParams.filter_duplicates,
-                              m_scanParams.filter_policy,
-                              m_scanParams.limited,
-                              m_phy & SCAN_1M ? &scan_params : NULL,
-                              m_phy & SCAN_CODED ? &scan_params : NULL,
-                              NimBLEScan::handleGapEvent,
-                              NULL);
+                                           duration / 10, // 10ms units
+                                           m_period,
+                                           m_scanParams.filter_duplicates,
+                                           m_scanParams.filter_policy,
+                                           m_scanParams.limited,
+                                           m_phy & SCAN_1M ? &scan_params : NULL,
+                                           m_phy & SCAN_CODED ? &scan_params : NULL,
+                                           NimBLEScan::handleGapEvent,
+                                           NULL);
 # else
     int rc = ble_gap_disc(NimBLEDevice::m_ownAddrType,
                           duration ? duration : BLE_HS_FOREVER,
@@ -385,6 +619,8 @@ bool NimBLEScan::stop() {
         return false;
     }
 
+    clearWaitingList();
+
     if (m_maxResults == 0) {
         clearResults();
     }
@@ -405,6 +641,7 @@ void NimBLEScan::erase(const NimBLEAddress& address) {
     NIMBLE_LOGD(LOG_TAG, "erase device: %s", address.toString().c_str());
     for (auto it = m_scanResults.m_deviceVec.begin(); it != m_scanResults.m_deviceVec.end(); ++it) {
         if ((*it)->getAddress() == address) {
+            removeWaitingDevice(*it);
             delete *it;
             m_scanResults.m_deviceVec.erase(it);
             break;
@@ -420,6 +657,7 @@ void NimBLEScan::erase(const NimBLEAdvertisedDevice* device) {
     NIMBLE_LOGD(LOG_TAG, "erase device: %s", device->getAddress().toString().c_str());
     for (auto it = m_scanResults.m_deviceVec.begin(); it != m_scanResults.m_deviceVec.end(); ++it) {
         if ((*it) == device) {
+            removeWaitingDevice(*it);
             delete *it;
             m_scanResults.m_deviceVec.erase(it);
             break;
@@ -474,6 +712,12 @@ NimBLEScanResults NimBLEScan::getResults() {
  * @brief Clear the stored results of the scan.
  */
 void NimBLEScan::clearResults() {
+    if (isScanning()) {
+        NIMBLE_LOGW(LOG_TAG, "Cannot clear results while scan is active");
+        return;
+    }
+
+    clearWaitingList();
     if (m_scanResults.m_deviceVec.size()) {
         std::vector<NimBLEAdvertisedDevice*> vSwap{};
         ble_npl_hw_enter_critical();
@@ -489,7 +733,7 @@ void NimBLEScan::clearResults() {
  * @brief Dump the scan results to the log.
  */
 void NimBLEScanResults::dump() const {
-# if CONFIG_NIMBLE_CPP_LOG_LEVEL >= 3
+# if MYNEWT_VAL(NIMBLE_CPP_LOG_LEVEL) >= 3
     for (const auto& dev : m_deviceVec) {
         NIMBLE_LOGI(LOG_TAG, "- %s", dev->toString().c_str());
     }

src/NimBLEScan.h

@@ -24,13 +24,15 @@
 # include "NimBLEAdvertisedDevice.h"
 # include "NimBLEUtils.h"
 
-# if defined(CONFIG_NIMBLE_CPP_IDF)
-#  include "host/ble_gap.h"
-# else
+# ifdef USING_NIMBLE_ARDUINO_HEADERS
 #  include "nimble/nimble/host/include/host/ble_gap.h"
+# else
+#  include "host/ble_gap.h"
 # endif
 
 # include <vector>
+# include <cinttypes>
+# include <cstdio>
 
 class NimBLEDevice;
 class NimBLEScan;
@@ -82,6 +84,8 @@ class NimBLEScan {
     void              setMaxResults(uint8_t maxResults);
     void              erase(const NimBLEAddress& address);
     void              erase(const NimBLEAdvertisedDevice* device);
+    void              setScanResponseTimeout(uint32_t timeoutMs);
+    std::string       getStatsString() const { return m_stats.toString(); }
 
 # if MYNEWT_VAL(BLE_EXT_ADV)
     enum Phy { SCAN_1M = 0x01, SCAN_CODED = 0x02, SCAN_ALL = 0x03 };
@@ -92,16 +96,103 @@ class NimBLEScan {
   private:
     friend class NimBLEDevice;
 
+    struct stats {
+# if MYNEWT_VAL(NIMBLE_CPP_LOG_LEVEL) >= 4
+        uint32_t devCount        = 0; // unique devices seen for the first time
+        uint32_t dupCount        = 0; // repeat advertisements from already-known devices
+        uint32_t srMinMs         = UINT32_MAX;
+        uint32_t srMaxMs         = 0;
+        uint64_t srTotalMs       = 0; // uint64 to avoid overflow on long/busy scans
+        uint32_t srCount         = 0; // matched scan responses (advertisement + SR pair)
+        uint32_t orphanedSrCount = 0; // scan responses received with no prior advertisement
+        uint32_t missedSrCount   = 0; // scannable devices for which no SR ever arrived
+
+        void reset() {
+            devCount        = 0;
+            dupCount        = 0;
+            srMinMs         = UINT32_MAX;
+            srMaxMs         = 0;
+            srTotalMs       = 0;
+            srCount         = 0;
+            orphanedSrCount = 0;
+            missedSrCount   = 0;
+        }
+
+        void incDevCount() { devCount++; }
+        void incDupCount() { dupCount++; }
+        void incMissedSrCount() { missedSrCount++; }
+        void incOrphanedSrCount() { orphanedSrCount++; }
+
+        std::string toString() const {
+            std::string out;
+            out.resize(400); // should be more than enough for the stats string
+            snprintf(&out[0],
+                     out.size(),
+                     "Scan stats:\n"
+                     "  Devices seen      : %" PRIu32 "\n"
+                     "  Duplicate advs    : %" PRIu32 "\n"
+                     "  Scan responses    : %" PRIu32 "\n"
+                     "  SR timing (ms)    : min=%" PRIu32 ", max=%" PRIu32 ", avg=%" PRIu64 "\n"
+                     "  Orphaned SR       : %" PRIu32 "\n"
+                     "  Missed SR         : %" PRIu32 "\n",
+                     devCount,
+                     dupCount,
+                     srCount,
+                     srCount ? srMinMs : 0,
+                     srCount ? srMaxMs : 0,
+                     srCount ? srTotalMs / srCount : 0,
+                     orphanedSrCount,
+                     missedSrCount);
+            return out;
+        }
+
+        // Records scan-response round-trip time.
+        void recordSrTime(uint32_t ticks) {
+            uint32_t ms;
+            ble_npl_time_ticks_to_ms(ticks, &ms);
+
+            if (ms < srMinMs) {
+                srMinMs = ms;
+            }
+            if (ms > srMaxMs) {
+                srMaxMs = ms;
+            }
+            srTotalMs += ms;
+            srCount++;
+            return;
+        }
+# else
+        void        reset() {}
+        void        incDevCount() {}
+        void        incDupCount() {}
+        void        incMissedSrCount() {}
+        void        incOrphanedSrCount() {}
+        std::string toString() const { return ""; }
+        void        recordSrTime(uint32_t ticks) {}
+# endif
+    } m_stats;
+
     NimBLEScan();
     ~NimBLEScan();
-    static int handleGapEvent(ble_gap_event* event, void* arg);
-    void       onHostSync();
+    static int  handleGapEvent(ble_gap_event* event, void* arg);
+    void        onHostSync();
+    static void srTimerCb(ble_npl_event* event);
 
-    NimBLEScanCallbacks* m_pScanCallbacks;
-    ble_gap_disc_params  m_scanParams;
-    NimBLEScanResults    m_scanResults;
-    NimBLETaskData*      m_pTaskData;
-    uint8_t              m_maxResults;
+    // Linked list helpers for devices awaiting scan responses
+    void addWaitingDevice(NimBLEAdvertisedDevice* pDev);
+    void removeWaitingDevice(NimBLEAdvertisedDevice* pDev);
+    void clearWaitingList();
+    void resetWaitingTimer();
+
+    NimBLEScanCallbacks*    m_pScanCallbacks;
+    ble_gap_disc_params     m_scanParams;
+    NimBLEScanResults       m_scanResults;
+    NimBLETaskData*         m_pTaskData;
+    ble_npl_callout         m_srTimer{};
+    ble_npl_time_t          m_srTimeoutTicks{};
+    uint8_t                 m_maxResults;
+    NimBLEAdvertisedDevice* m_pWaitingListHead{}; // head of linked list for devices awaiting scan responses
+    NimBLEAdvertisedDevice* m_pWaitingListTail{}; // tail of linked list for FIFO ordering
 
 # if MYNEWT_VAL(BLE_EXT_ADV)
     uint8_t  m_phy{SCAN_ALL};

src/NimBLEServer.cpp

@@ -25,12 +25,12 @@
 #  include "NimBLEClient.h"
 # endif
 
-# if defined(CONFIG_NIMBLE_CPP_IDF)
-#  include "services/gap/ble_svc_gap.h"
-#  include "services/gatt/ble_svc_gatt.h"
-# else
+# ifdef USING_NIMBLE_ARDUINO_HEADERS
 #  include "nimble/nimble/host/services/gap/include/services/gap/ble_svc_gap.h"
 #  include "nimble/nimble/host/services/gatt/include/services/gatt/ble_svc_gatt.h"
+# else
+#  include "services/gap/ble_svc_gap.h"
+#  include "services/gatt/ble_svc_gatt.h"
 # endif
 
 # define NIMBLE_SERVER_GET_PEER_NAME_ON_CONNECT_CB 0
@@ -39,6 +39,11 @@
 static const char*           LOG_TAG = "NimBLEServer";
 static NimBLEServerCallbacks defaultCallbacks;
 
+struct gattRegisterCallbackArgs {
+    NimBLEService*        pSvc{nullptr};
+    NimBLECharacteristic* pChar{nullptr};
+};
+
 /**
  * @brief Construct a BLE Server
  *
@@ -93,8 +98,7 @@ NimBLEService* NimBLEServer::createService(const char* uuid) {
 NimBLEService* NimBLEServer::createService(const NimBLEUUID& uuid) {
     NimBLEService* pService = new NimBLEService(uuid);
     m_svcVec.push_back(pService);
-    serviceChanged();
-
+    setServiceChanged();
     return pService;
 } // createService
 
@@ -143,10 +147,25 @@ NimBLEService* NimBLEServer::getServiceByHandle(uint16_t handle) const {
     return nullptr;
 }
 
+/**
+ * @brief Get a BLE Characteristic by its handle
+ * @param handle The handle of the characteristic.
+ * @return A pointer to the characteristic object or nullptr if not found.
+ */
+NimBLECharacteristic* NimBLEServer::getCharacteristicByHandle(uint16_t handle) const {
+    for (const auto& svc : m_svcVec) {
+        NimBLECharacteristic* pChr = svc->getCharacteristicByHandle(handle);
+        if (pChr != nullptr) {
+            return pChr;
+        }
+    }
+    return nullptr;
+} // getCharacteristicByHandle
+
 # if MYNEWT_VAL(BLE_EXT_ADV)
 /**
  * @brief Retrieve the advertising object that can be used to advertise the existence of the server.
- * @return A pinter to an advertising object.
+ * @return A pointer to an advertising object.
  */
 NimBLEExtAdvertising* NimBLEServer::getAdvertising() const {
     return NimBLEDevice::getAdvertising();
@@ -167,61 +186,145 @@ NimBLEAdvertising* NimBLEServer::getAdvertising() const {
  * @brief Called when the services are added/removed and sets a flag to indicate they should be reloaded.
  * @details This has no effect if the GATT server was not already started.
  */
-void NimBLEServer::serviceChanged() {
+void NimBLEServer::setServiceChanged() {
     if (m_gattsStarted) {
         m_svcChanged = true;
     }
 } // serviceChanged
 
+/**
+ * @brief Send a service changed indication to all clients.
+ * @details This should be called when services are added, removed or modified after the server has been started.
+ */
+void NimBLEServer::sendServiceChangedIndication() const {
+    ble_svc_gatt_changed(0x0001, 0xffff);
+}
+
+/**
+ * @brief Callback for GATT registration events,
+ * used to obtain the assigned handles for services, characteristics, and descriptors.
+ * @param [in] ctxt The context of the registration event.
+ * @param [in] arg A pointer to the gattRegisterCallbackArgs struct used to track the
+ * service and characteristic being registered.
+ */
+void NimBLEServer::gattRegisterCallback(ble_gatt_register_ctxt* ctxt, void* arg) {
+    gattRegisterCallbackArgs* args = static_cast<gattRegisterCallbackArgs*>(arg);
+
+    if (ctxt->op == BLE_GATT_REGISTER_OP_SVC) {
+        NimBLEUUID uuid(ctxt->svc.svc_def->uuid);
+        args->pSvc = nullptr;
+        for (auto pSvc : NimBLEDevice::getServer()->m_svcVec) {
+            if (!pSvc->getRemoved() && pSvc->m_handle == 0 && pSvc->getUUID() == uuid) {
+                pSvc->m_handle = ctxt->svc.handle;
+                NIMBLE_LOGD(LOG_TAG, "Service registered: %s, handle=%d", uuid.toString().c_str(), ctxt->svc.handle);
+                // Set the arg to the service so we know that the following
+                // characteristics and descriptors belong to this service
+                args->pSvc = pSvc;
+                break;
+            }
+        }
+
+        return;
+    }
+
+    if (args->pSvc == nullptr) {
+        // If the service is not found then this is likely a characteristic or descriptor that was registered as
+        // part of the GATT server setup and not found in the service vector
+        NIMBLE_LOGD(LOG_TAG, "Skipping characteristic or descriptor registered with unknown service");
+        return;
+    }
+
+    if (ctxt->op == BLE_GATT_REGISTER_OP_CHR) {
+        NimBLEUUID uuid(ctxt->chr.chr_def->uuid);
+        args->pChar = nullptr;
+        for (auto pChr : args->pSvc->m_vChars) {
+            if (!pChr->getRemoved() && pChr->m_handle == 0 && pChr->getUUID() == uuid) {
+                pChr->m_handle = ctxt->chr.val_handle;
+                // Set the arg to the characteristic so we know that the following descriptors belong to this characteristic
+                args->pChar    = pChr;
+                NIMBLE_LOGD(LOG_TAG,
+                            "Characteristic registered: %s, def_handle=%d, val_handle=%d",
+                            uuid.toString().c_str(),
+                            ctxt->chr.def_handle,
+                            ctxt->chr.val_handle);
+                break;
+            }
+        }
+
+        return;
+    }
+
+    if (ctxt->op == BLE_GATT_REGISTER_OP_DSC) {
+        if (args->pChar == nullptr) {
+            NIMBLE_LOGE(LOG_TAG, "Descriptor registered with unknown characteristic, skipping");
+            return;
+        }
+
+        NimBLEUUID uuid(ctxt->dsc.dsc_def->uuid);
+        for (auto pDsc : args->pChar->m_vDescriptors) {
+            if (!pDsc->getRemoved() && pDsc->m_handle == 0 && pDsc->getUUID() == uuid) {
+                pDsc->m_handle = ctxt->dsc.handle;
+                NIMBLE_LOGD(LOG_TAG, "Descriptor registered: %s, handle=%d", uuid.toString().c_str(), ctxt->dsc.handle);
+                return;
+            }
+        }
+    }
+}
+
 /**
  * @brief Start the GATT server.
  * @details Required to be called after setup of all services and characteristics / descriptors
  * for the NimBLE host to register them.
  */
-void NimBLEServer::start() {
-    if (m_gattsStarted) {
-        return; // already started
+bool NimBLEServer::start() {
+    if (m_svcChanged && !getConnectedCount()) {
+        NIMBLE_LOGD(LOG_TAG, "Services have changed since last start, resetting GATT server");
+        m_gattsStarted = false;
     }
 
+    if (m_gattsStarted) {
+        return true; // already started
+    }
+
+    if (!resetGATT()) {
+        return false;
+    }
+
+    ble_hs_cfg.gatts_register_cb = NimBLEServer::gattRegisterCallback;
+    gattRegisterCallbackArgs args{};
+    ble_hs_cfg.gatts_register_arg = &args;
+
     int rc = ble_gatts_start();
     if (rc != 0) {
         NIMBLE_LOGE(LOG_TAG, "ble_gatts_start; rc=%d, %s", rc, NimBLEUtils::returnCodeToString(rc));
-        return;
+        return false;
     }
 
-# if CONFIG_NIMBLE_CPP_LOG_LEVEL >= 4
+# if MYNEWT_VAL(NIMBLE_CPP_LOG_LEVEL) >= 4
     ble_gatts_show_local();
-# endif
 
-    // Get the assigned service handles and build a vector of characteristics
-    // with Notify / Indicate capabilities for event handling
+    // Check that all services were registered and log if any are missing.
     for (const auto& svc : m_svcVec) {
         if (svc->getRemoved() == 0) {
-            rc = ble_gatts_find_svc(svc->getUUID().getBase(), &svc->m_handle);
+            rc = ble_gatts_find_svc(svc->getUUID().getBase(), NULL);
             if (rc != 0) {
-                NIMBLE_LOGW(LOG_TAG,
+                NIMBLE_LOGD(LOG_TAG,
                             "GATT Server started without service: %s, Service %s",
                             svc->getUUID().toString().c_str(),
                             svc->isStarted() ? "missing" : "not started");
-                continue; // Skip this service as it was not started
-            }
-        }
-
-        // Set the descriptor handles now as the stack does not set these when the service is started
-        for (const auto& chr : svc->m_vChars) {
-            for (auto& desc : chr->m_vDescriptors) {
-                ble_gatts_find_dsc(svc->getUUID().getBase(), chr->getUUID().getBase(), desc->getUUID().getBase(), &desc->m_handle);
             }
         }
     }
+# endif
 
     // If the services have changed indicate it now
     if (m_svcChanged) {
         m_svcChanged = false;
-        ble_svc_gatt_changed(0x0001, 0xffff);
+        sendServiceChangedIndication();
     }
 
     m_gattsStarted = true;
+    return true;
 } // start
 
 /**
@@ -232,19 +335,22 @@ void NimBLEServer::start() {
  */
 bool NimBLEServer::disconnect(uint16_t connHandle, uint8_t reason) const {
     int rc = ble_gap_terminate(connHandle, reason);
-    if (rc != 0 && rc != BLE_HS_ENOTCONN && rc != BLE_HS_EALREADY) {
-        NIMBLE_LOGE(LOG_TAG, "ble_gap_terminate failed: rc=%d %s", rc, NimBLEUtils::returnCodeToString(rc));
-        return false;
+    switch (rc) {
+        case 0:
+        case BLE_HS_ENOTCONN:
+        case BLE_HS_EALREADY:
+        case BLE_HS_HCI_ERR(BLE_ERR_UNK_CONN_ID):
+            return true;
     }
-
-    return true;
+    NIMBLE_LOGE(LOG_TAG, "ble_gap_terminate failed: rc=%d %s", rc, NimBLEUtils::returnCodeToString(rc));
+    return false;
 } // disconnect
 
 /**
  * @brief Disconnect the specified client with optional reason.
  * @param [in] connInfo Connection of the client to disconnect.
  * @param [in] reason code for disconnecting.
- * @return NimBLE host return code.
+ * @return True if successful.
  */
 bool NimBLEServer::disconnect(const NimBLEConnInfo& connInfo, uint8_t reason) const {
     return disconnect(connInfo.getConnHandle(), reason);
@@ -305,7 +411,7 @@ NimBLEConnInfo NimBLEServer::getPeerInfo(uint8_t index) const {
     for (const auto& peer : m_connectedPeers) {
         if (peer != BLE_HS_CONN_HANDLE_NONE) {
             if (count == index) {
-                return getPeerInfoByHandle(m_connectedPeers[count]);
+                return getPeerInfoByHandle(peer);
             }
             count++;
         }
@@ -360,9 +466,7 @@ int NimBLEServer::handleGapEvent(ble_gap_event* event, void* arg) {
             }
 
             if (rc != 0) {
-                NIMBLE_LOGE(LOG_TAG, "Connection failed rc = %d %s",
-                            rc,
-                            NimBLEUtils::returnCodeToString(rc));
+                NIMBLE_LOGE(LOG_TAG, "Connection failed rc = %d %s", rc, NimBLEUtils::returnCodeToString(rc));
 # if !MYNEWT_VAL(BLE_EXT_ADV) && MYNEWT_VAL(BLE_ROLE_BROADCASTER)
                 NimBLEDevice::startAdvertising();
 # endif
@@ -414,13 +518,9 @@ int NimBLEServer::handleGapEvent(ble_gap_event* event, void* arg) {
             }
 # endif
 
-            if (pServer->m_svcChanged) {
-                pServer->resetGATT();
-            }
-
             peerInfo.m_desc = event->disconnect.conn;
             pServer->m_pServerCallbacks->onDisconnect(pServer, peerInfo, event->disconnect.reason);
-# if !MYNEWT_VAL(BLE_EXT_ADV)
+# if !MYNEWT_VAL(BLE_EXT_ADV) && MYNEWT_VAL(BLE_ROLE_BROADCASTER)
             if (pServer->m_advertiseOnDisconnect) {
                 pServer->startAdvertising();
             }
@@ -429,33 +529,21 @@ int NimBLEServer::handleGapEvent(ble_gap_event* event, void* arg) {
         } // BLE_GAP_EVENT_DISCONNECT
 
         case BLE_GAP_EVENT_SUBSCRIBE: {
-            NIMBLE_LOGI(LOG_TAG,
-                        "subscribe event; attr_handle=%d, subscribed: %s",
-                        event->subscribe.attr_handle,
-                        ((event->subscribe.cur_notify || event->subscribe.cur_indicate) ? "true" : "false"));
-
-            for (const auto& svc : pServer->m_svcVec) {
-                for (const auto& chr : svc->m_vChars) {
-                    if (chr->getHandle() == event->subscribe.attr_handle) {
-                        rc = ble_gap_conn_find(event->subscribe.conn_handle, &peerInfo.m_desc);
-                        if (rc != 0) {
-                            break;
-                        }
-
-                        auto chrProps = chr->getProperties();
-                        if (!peerInfo.isEncrypted() &&
-                            (chrProps & BLE_GATT_CHR_F_READ_AUTHEN || chrProps & BLE_GATT_CHR_F_READ_AUTHOR ||
-                             chrProps & BLE_GATT_CHR_F_READ_ENC)) {
-                            NimBLEDevice::startSecurity(event->subscribe.conn_handle);
-                        }
-
-                        chr->m_pCallbacks->onSubscribe(chr,
-                                                       peerInfo,
-                                                       event->subscribe.cur_notify + (event->subscribe.cur_indicate << 1));
-                    }
-                }
+            rc = ble_gap_conn_find(event->subscribe.conn_handle, &peerInfo.m_desc);
+            if (rc != 0) {
+                break;
             }
 
+            uint8_t subVal = event->subscribe.cur_notify + (event->subscribe.cur_indicate << 1);
+            NIMBLE_LOGI(LOG_TAG, "subscribe event; attr_handle=%d, subscribed: %d", event->subscribe.attr_handle, subVal);
+
+            auto pChar = pServer->getCharacteristicByHandle(event->subscribe.attr_handle);
+            if (!pChar) {
+                NIMBLE_LOGE(LOG_TAG, "subscribe event; attr_handle=%d, not found", event->subscribe.attr_handle);
+                break;
+            }
+
+            pChar->processSubRequest(peerInfo, subVal);
             break;
         } // BLE_GAP_EVENT_SUBSCRIBE
 
@@ -469,18 +557,14 @@ int NimBLEServer::handleGapEvent(ble_gap_event* event, void* arg) {
         } // BLE_GAP_EVENT_MTU
 
         case BLE_GAP_EVENT_NOTIFY_TX: {
-            NimBLECharacteristic* pChar = nullptr;
-
-            for (const auto& svc : pServer->m_svcVec) {
-                for (auto& chr : svc->m_vChars) {
-                    if (chr->getHandle() == event->notify_tx.attr_handle) {
-                        pChar = chr;
-                    }
-                }
+            rc = ble_gap_conn_find(event->notify_tx.conn_handle, &peerInfo.m_desc);
+            if (rc != 0) {
+                break;
             }
 
+            auto pChar = pServer->getCharacteristicByHandle(event->notify_tx.attr_handle);
             if (pChar == nullptr) {
-                return 0;
+                break;
             }
 
             if (event->notify_tx.indication) {
@@ -490,9 +574,19 @@ int NimBLEServer::handleGapEvent(ble_gap_event* event, void* arg) {
             }
 
             pChar->m_pCallbacks->onStatus(pChar, event->notify_tx.status);
+            pChar->m_pCallbacks->onStatus(pChar, peerInfo, event->notify_tx.status);
             break;
         } // BLE_GAP_EVENT_NOTIFY_TX
 
+# if MYNEWT_VAL(BLE_ROLE_CENTRAL)
+        case BLE_GAP_EVENT_NOTIFY_RX: {
+            if (pServer->m_pClient && pServer->m_pClient->m_connHandle == event->notify_rx.conn_handle) {
+                NimBLEClient::handleGapEvent(event, pServer->m_pClient);
+            }
+            break;
+        } // BLE_GAP_EVENT_NOTIFY_RX
+# endif
+
         case BLE_GAP_EVENT_ADV_COMPLETE: {
 # if MYNEWT_VAL(BLE_EXT_ADV) && MYNEWT_VAL(BLE_ROLE_BROADCASTER)
             case BLE_GAP_EVENT_SCAN_REQ_RCVD:
@@ -542,6 +636,13 @@ int NimBLEServer::handleGapEvent(ble_gap_event* event, void* arg) {
                 NimBLEClient::handleGapEvent(event, pServer->m_pClient);
             }
 # endif
+            // update the secured status of the peer in each characteristic's subscribed peers list
+            for (const auto& svc : pServer->m_svcVec) {
+                for (const auto& chr : svc->m_vChars) {
+                    chr->updatePeerStatus(peerInfo);
+                }
+            }
+
             break;
         } // BLE_GAP_EVENT_ENC_CHANGE
 
@@ -598,6 +699,15 @@ int NimBLEServer::handleGapEvent(ble_gap_event* event, void* arg) {
                 // }
                 // rc = ble_sm_inject_io(event->passkey.conn_handle, &pkey);
                 // NIMBLE_LOGD(LOG_TAG, "BLE_SM_IOACT_OOB; ble_sm_inject_io result: %d", rc);
+            } else if (event->passkey.params.action == BLE_SM_IOACT_INPUT) {
+                NIMBLE_LOGD(LOG_TAG, "Enter the passkey");
+
+                rc = ble_gap_conn_find(event->passkey.conn_handle, &peerInfo.m_desc);
+                if (rc != 0) {
+                    return BLE_ATT_ERR_INVALID_HANDLE;
+                }
+
+                pServer->m_pServerCallbacks->onPassKeyEntry(peerInfo);
             } else if (event->passkey.params.action == BLE_SM_IOACT_NONE) {
                 NIMBLE_LOGD(LOG_TAG, "No passkey action required");
             }
@@ -736,7 +846,7 @@ void NimBLEServer::removeService(NimBLEService* service, bool deleteSvc) {
     }
 
     service->setRemoved(deleteSvc ? NIMBLE_ATT_REMOVE_DELETE : NIMBLE_ATT_REMOVE_HIDE);
-    serviceChanged();
+    setServiceChanged();
 # if !MYNEWT_VAL(BLE_EXT_ADV) && MYNEWT_VAL(BLE_ROLE_BROADCASTER)
     NimBLEDevice::getAdvertising()->removeServiceUUID(service->getUUID());
 # endif
@@ -763,40 +873,70 @@ void NimBLEServer::addService(NimBLEService* service) {
     }
 
     service->setRemoved(0);
-    serviceChanged();
+    setServiceChanged();
 } // addService
 
 /**
  * @brief Resets the GATT server, used when services are added/removed after initialization.
+ * @return True if successful.
+ * @details This will reset the GATT server and re-register all services, characteristics, and
+ * descriptors that have not been removed. Services, characteristics, and descriptors that have been
+ * removed but not deleted will be skipped and have their handles cleared, and those that have been
+ * deleted will be removed from the server's service vector.
  */
-void NimBLEServer::resetGATT() {
-    if (getConnectedCount() > 0) {
-        return;
-    }
-
+bool NimBLEServer::resetGATT() {
 # if MYNEWT_VAL(BLE_ROLE_BROADCASTER)
     NimBLEDevice::stopAdvertising();
 # endif
+
     ble_gatts_reset();
     ble_svc_gap_init();
     ble_svc_gatt_init();
 
-    for (auto it = m_svcVec.begin(); it != m_svcVec.end();) {
-        if ((*it)->getRemoved() > 0) {
-            if ((*it)->getRemoved() == NIMBLE_ATT_REMOVE_DELETE) {
-                delete *it;
-                it = m_svcVec.erase(it);
-            } else {
-                ++it;
-            }
+    for (auto svcIt = m_svcVec.begin(); svcIt != m_svcVec.end();) {
+        auto* pSvc = *svcIt;
+        if (pSvc->getRemoved() == NIMBLE_ATT_REMOVE_DELETE) {
+            delete pSvc;
+            svcIt = m_svcVec.erase(svcIt);
             continue;
         }
 
-        (*it)->start();
-        ++it;
+        for (auto chrIt = pSvc->m_vChars.begin(); chrIt != pSvc->m_vChars.end();) {
+            auto* pChr = *chrIt;
+            if (pChr->getRemoved() == NIMBLE_ATT_REMOVE_DELETE) {
+                delete pChr;
+                chrIt = pSvc->m_vChars.erase(chrIt);
+                continue;
+            }
+
+            for (auto dscIt = pChr->m_vDescriptors.begin(); dscIt != pChr->m_vDescriptors.end();) {
+                auto* pDsc = *dscIt;
+                if (pDsc->getRemoved() == NIMBLE_ATT_REMOVE_DELETE) {
+                    delete pDsc;
+                    dscIt = pChr->m_vDescriptors.erase(dscIt);
+                    continue;
+                }
+
+                pDsc->m_handle = 0;
+                ++dscIt;
+            }
+
+            pChr->m_handle = 0;
+            ++chrIt;
+        }
+
+        if (pSvc->getRemoved() == 0) {
+            if (!pSvc->start_internal()) {
+                NIMBLE_LOGE(LOG_TAG, "Failed to start service: %s", pSvc->getUUID().toString().c_str());
+                return false;
+            }
+        }
+
+        pSvc->m_handle = 0;
+        ++svcIt;
     }
 
-    m_gattsStarted = false;
+    return true;
 } // resetGATT
 
 /**
@@ -932,7 +1072,7 @@ void NimBLEServer::updateConnParams(
  * @param [in] octets The preferred number of payload octets to use (Range 0x001B-0x00FB).
  */
 void NimBLEServer::setDataLen(uint16_t connHandle, uint16_t octets) const {
-# if defined(CONFIG_NIMBLE_CPP_IDF) && !defined(ESP_IDF_VERSION) || \
+# if !defined(USING_NIMBLE_ARDUINO_HEADERS) && !defined(ESP_IDF_VERSION) || \
      (ESP_IDF_VERSION_MAJOR * 100 + ESP_IDF_VERSION_MINOR * 10 + ESP_IDF_VERSION_PATCH) < 432
     return;
 # else
@@ -979,6 +1119,7 @@ NimBLEClient* NimBLEServer::getClient(const NimBLEConnInfo& connInfo) {
     m_pClient->deleteServices(); // Changed peer connection delete the database.
     m_pClient->m_peerAddress = connInfo.getAddress();
     m_pClient->m_connHandle  = connInfo.getConnHandle();
+    m_pClient->m_connStatus  = NimBLEClient::CONNECTED;
     return m_pClient;
 } // getClient
 
@@ -1011,6 +1152,11 @@ uint32_t NimBLEServerCallbacks::onPassKeyDisplay() {
     return 123456;
 } // onPassKeyDisplay
 
+void NimBLEServerCallbacks::onPassKeyEntry(NimBLEConnInfo& connInfo) {
+    NIMBLE_LOGD("NimBLEServerCallbacks", "onPassKeyEntry: default: 123456");
+    NimBLEDevice::injectPassKey(connInfo, 123456);
+} // onPassKeyEntry
+
 void NimBLEServerCallbacks::onConfirmPassKey(NimBLEConnInfo& connInfo, uint32_t pin) {
     NIMBLE_LOGD("NimBLEServerCallbacks", "onConfirmPasskey: default: true");
     NimBLEDevice::injectConfirmPasskey(connInfo, true);

src/NimBLEServer.h

@@ -21,10 +21,10 @@
 #include "syscfg/syscfg.h"
 #if CONFIG_BT_NIMBLE_ENABLED && MYNEWT_VAL(BLE_ROLE_PERIPHERAL)
 
-# if defined(CONFIG_NIMBLE_CPP_IDF)
-#  include "host/ble_gap.h"
-# else
+# ifdef USING_NIMBLE_ARDUINO_HEADERS
 #  include "nimble/nimble/host/include/host/ble_gap.h"
+# else
+#  include "host/ble_gap.h"
 # endif
 
 /****  FIX COMPILATION ****/
@@ -61,7 +61,7 @@ class NimBLEClient;
  */
 class NimBLEServer {
   public:
-    void    start();
+    bool    start();
     uint8_t getConnectedCount() const;
     bool    disconnect(uint16_t connHandle, uint8_t reason = BLE_ERR_REM_USER_CONN_TERM) const;
     bool    disconnect(const NimBLEConnInfo& connInfo, uint8_t reason = BLE_ERR_REM_USER_CONN_TERM) const;
@@ -72,6 +72,7 @@ class NimBLEServer {
     NimBLEService*        getServiceByUUID(const char* uuid, uint16_t instanceId = 0) const;
     NimBLEService*        getServiceByUUID(const NimBLEUUID& uuid, uint16_t instanceId = 0) const;
     NimBLEService*        getServiceByHandle(uint16_t handle) const;
+    NimBLECharacteristic* getCharacteristicByHandle(uint16_t handle) const;
     void                  removeService(NimBLEService* service, bool deleteSvc = false);
     void                  addService(NimBLEService* service);
     uint16_t              getPeerMTU(uint16_t connHandle) const;
@@ -83,6 +84,7 @@ class NimBLEServer {
     void                  setDataLen(uint16_t connHandle, uint16_t tx_octets) const;
     bool                  updatePhy(uint16_t connHandle, uint8_t txPhysMask, uint8_t rxPhysMask, uint16_t phyOptions);
     bool                  getPhy(uint16_t connHandle, uint8_t* txPhy, uint8_t* rxPhy);
+    void                  sendServiceChangedIndication() const;
 
 # if MYNEWT_VAL(BLE_ROLE_CENTRAL)
     NimBLEClient* getClient(uint16_t connHandle);
@@ -118,26 +120,25 @@ class NimBLEServer {
 
     NimBLEServer();
     ~NimBLEServer();
+    static int  handleGapEvent(struct ble_gap_event* event, void* arg);
+    static int  handleGattEvent(uint16_t connHandle, uint16_t attrHandle, ble_gatt_access_ctxt* ctxt, void* arg);
+    static void gattRegisterCallback(struct ble_gatt_register_ctxt* ctxt, void* arg);
+    void        setServiceChanged();
+    bool        resetGATT();
 
     bool m_gattsStarted : 1;
     bool m_svcChanged : 1;
     bool m_deleteCallbacks : 1;
-# if !MYNEWT_VAL(BLE_EXT_ADV)
+# if !MYNEWT_VAL(BLE_EXT_ADV) && MYNEWT_VAL(BLE_ROLE_BROADCASTER)
     bool m_advertiseOnDisconnect : 1;
 # endif
-    NimBLEServerCallbacks*                                 m_pServerCallbacks;
-    std::vector<NimBLEService*>                            m_svcVec;
+    NimBLEServerCallbacks*                                m_pServerCallbacks;
+    std::vector<NimBLEService*>                           m_svcVec;
     std::array<uint16_t, MYNEWT_VAL(BLE_MAX_CONNECTIONS)> m_connectedPeers;
 
 # if MYNEWT_VAL(BLE_ROLE_CENTRAL)
     NimBLEClient* m_pClient{nullptr};
 # endif
-
-    static int handleGapEvent(struct ble_gap_event* event, void* arg);
-    static int handleGattEvent(uint16_t connHandle, uint16_t attrHandle, ble_gatt_access_ctxt* ctxt, void* arg);
-    void       serviceChanged();
-    void       resetGATT();
-
 }; // NimBLEServer
 
 /**
@@ -180,6 +181,15 @@ class NimBLEServerCallbacks {
      */
     virtual uint32_t onPassKeyDisplay();
 
+    /**
+     * @brief Called when using passkey entry pairing and the peer requires the passkey to be entered.
+     * @param [in] connInfo A reference to a NimBLEConnInfo instance with information
+     * about the peer connection parameters.
+     * @details The application should call NimBLEDevice::injectPassKey with the passkey
+     * displayed on the peer device to complete the pairing process.
+     */
+    virtual void onPassKeyEntry(NimBLEConnInfo& connInfo);
+
     /**
      * @brief Called when using numeric comparision for pairing.
      * @param [in] connInfo A reference to a NimBLEConnInfo instance with information

src/NimBLEService.cpp

@@ -48,12 +48,7 @@ NimBLEService::NimBLEService(const NimBLEUUID& uuid)
  * @brief Destructor, make sure we release the resources allocated for the service.
  */
 NimBLEService::~NimBLEService() {
-    if (m_pSvcDef->characteristics) {
-        if (m_pSvcDef->characteristics->descriptors) {
-            delete[] m_pSvcDef->characteristics->descriptors;
-        }
-        delete[] m_pSvcDef->characteristics;
-    }
+    clearServiceDefinitions();
 
     for (const auto& it : m_vChars) {
         delete it;
@@ -88,22 +83,10 @@ void NimBLEService::dump() const {
  * and registers it with the NimBLE stack.
  * @return bool success/failure .
  */
-bool NimBLEService::start() {
-    NIMBLE_LOGD(LOG_TAG, ">> start(): Starting service: %s", toString().c_str());
-
-    // If started previously and no characteristics have been added or removed,
-    // then we can skip the service registration process.
-    if (m_pSvcDef->characteristics && !getServer()->m_svcChanged) {
-        return true;
-    }
-
-    // If started previously, clear everything and start over
-    if (m_pSvcDef->characteristics) {
-        if (m_pSvcDef->characteristics->descriptors) {
-            delete[] m_pSvcDef->characteristics->descriptors;
-        }
-        delete[] m_pSvcDef->characteristics;
-    }
+bool NimBLEService::start_internal() {
+    NIMBLE_LOGD(LOG_TAG, ">> start(): Starting service: UUID: %s", getUUID().toString().c_str());
+    // Make sure the definitions are cleared first
+    clearServiceDefinitions();
 
     size_t numChrs = 0;
     for (const auto& chr : m_vChars) {
@@ -113,7 +96,7 @@ bool NimBLEService::start() {
         ++numChrs;
     }
 
-    NIMBLE_LOGD(LOG_TAG, "Adding %d characteristics for service %s", numChrs, toString().c_str());
+    NIMBLE_LOGD(LOG_TAG, "Adding %zu characteristics for service %s", numChrs, getUUID().toString().c_str());
     if (numChrs) {
         int i = 0;
 
@@ -160,7 +143,7 @@ bool NimBLEService::start() {
             pChrs[i].arg          = chr;
             pChrs[i].flags        = chr->getProperties();
             pChrs[i].min_key_size = 0;
-            pChrs[i].val_handle   = &chr->m_handle;
+            pChrs[i].val_handle   = nullptr;
             ++i;
         }
 
@@ -168,15 +151,18 @@ bool NimBLEService::start() {
     }
 
     m_pSvcDef->type = BLE_GATT_SVC_TYPE_PRIMARY;
-    int rc          = ble_gatts_count_cfg(m_pSvcDef);
+
+    int rc = ble_gatts_count_cfg(m_pSvcDef);
     if (rc != 0) {
         NIMBLE_LOGE(LOG_TAG, "ble_gatts_count_cfg failed, rc= %d, %s", rc, NimBLEUtils::returnCodeToString(rc));
+        clearServiceDefinitions(); // Clear the definitions to free memory and reset the service for re-registration.
         return false;
     }
 
     rc = ble_gatts_add_svcs(m_pSvcDef);
     if (rc != 0) {
         NIMBLE_LOGE(LOG_TAG, "ble_gatts_add_svcs, rc= %d, %s", rc, NimBLEUtils::returnCodeToString(rc));
+        clearServiceDefinitions(); // Clear the definitions to free memory and reset the service for re-registration.
         return false;
     }
 
@@ -184,6 +170,25 @@ bool NimBLEService::start() {
     return true;
 } // start
 
+/**
+ * @brief Clear the service definitions to free memory and reset the service for re-registration.
+ */
+void NimBLEService::clearServiceDefinitions() {
+    if (m_pSvcDef->characteristics) {
+        const ble_gatt_chr_def* chrDef = m_pSvcDef->characteristics;
+        while (chrDef->uuid != nullptr) {
+            if (chrDef->descriptors) {
+                delete[] chrDef->descriptors;
+            }
+            ++chrDef;
+        }
+        delete[] m_pSvcDef->characteristics;
+        m_pSvcDef->characteristics = nullptr;
+    }
+
+    m_pSvcDef->type = 0; // Clear the type to indicate the service is not started/registered.
+} // clearServiceDefinitions
+
 /**
  * @brief Create a new BLE Characteristic associated with this service.
  * @param [in] uuid - The UUID of the characteristic.
@@ -240,7 +245,7 @@ void NimBLEService::addCharacteristic(NimBLECharacteristic* pChar) {
     }
 
     pChar->setService(this);
-    getServer()->serviceChanged();
+    getServer()->setServiceChanged();
 } // addCharacteristic
 
 /**
@@ -267,7 +272,7 @@ void NimBLEService::removeCharacteristic(NimBLECharacteristic* pChar, bool delet
     }
 
     pChar->setRemoved(deleteChr ? NIMBLE_ATT_REMOVE_DELETE : NIMBLE_ATT_REMOVE_HIDE);
-    getServer()->serviceChanged();
+    getServer()->setServiceChanged();
 } // removeCharacteristic
 
 /**

src/NimBLEService.h

@@ -37,11 +37,19 @@ class NimBLEService : public NimBLELocalAttribute {
     NimBLEService(const NimBLEUUID& uuid);
     ~NimBLEService();
 
-    NimBLEServer*         getServer() const;
-    std::string           toString() const;
-    void                  dump() const;
-    bool                  isStarted() const;
-    bool                  start();
+    NimBLEServer* getServer() const;
+    std::string   toString() const;
+    void          dump() const;
+    bool          isStarted() const;
+
+    /**
+     * @brief Dummy function to start the service. Services are started when the server is started.
+     * This will be removed in a future release. Use `NimBLEServer::start()` to start the server and all associated services.
+     */
+    __attribute__((deprecated("NimBLEService::start() has no effect. "
+                   "Services are started when the server is started.")))
+    bool start() { return true; }
+
     NimBLECharacteristic* createCharacteristic(const char* uuid,
                                                uint32_t    properties = NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::WRITE,
                                                uint16_t    max_len    = BLE_ATT_ATTR_MAX_LEN);
@@ -61,6 +69,8 @@ class NimBLEService : public NimBLELocalAttribute {
 
   private:
     friend class NimBLEServer;
+    bool start_internal();
+    void clearServiceDefinitions();
 
     std::vector<NimBLECharacteristic*> m_vChars{};
     // Nimble requires an array of services to be sent to the api

src/NimBLEStream.h

@@ -0,0 +1,231 @@
+/*
+ * Copyright 2020-2025 Ryan Powell <ryan@nable-embedded.io> and
+ * esp-nimble-cpp, NimBLE-Arduino contributors.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef NIMBLE_CPP_STREAM_H
+#define NIMBLE_CPP_STREAM_H
+
+#include "syscfg/syscfg.h"
+#if CONFIG_BT_NIMBLE_ENABLED && (MYNEWT_VAL(BLE_ROLE_PERIPHERAL) || MYNEWT_VAL(BLE_ROLE_CENTRAL))
+
+# ifdef USING_NIMBLE_ARDUINO_HEADERS
+#  include "nimble/nimble/include/nimble/nimble_npl.h"
+# else
+#  include "nimble/nimble_npl.h"
+# endif
+
+# include <functional>
+# include <type_traits>
+# include <cstdarg>
+
+# ifndef NIMBLE_CPP_ARDUINO_STRING_AVAILABLE
+#  define NIMBLE_CPP_ARDUINO_STRING_AVAILABLE (__has_include(<Arduino.h>))
+# endif
+
+# if NIMBLE_CPP_ARDUINO_STRING_AVAILABLE
+#  include <Stream.h>
+# else
+
+// Minimal Stream/Print stubs when Arduino not available
+class Print {
+  public:
+    virtual ~Print() {}
+    virtual size_t write(uint8_t)                            = 0;
+    virtual size_t write(const uint8_t* buffer, size_t size) = 0;
+    size_t         print(const char* s);
+    size_t         println(const char* s);
+    size_t         printf(const char* format, ...) __attribute__((format(printf, 2, 3)));
+};
+
+class Stream : public Print {
+  public:
+    virtual int   available() = 0;
+    virtual int   read()      = 0;
+    virtual int   peek()      = 0;
+    virtual void  flush() {}
+    void          setTimeout(unsigned long timeout) { m_timeout = timeout; }
+    unsigned long getTimeout() const { return m_timeout; }
+
+  protected:
+    unsigned long m_timeout{0};
+};
+# endif
+
+class NimBLEStream : public Stream {
+  public:
+    enum RxOverflowAction {
+        DROP_OLDER_DATA, // Drop older buffered data to make room for new data
+        DROP_NEW_DATA    // Drop new incoming data when buffer is full
+    };
+
+    using RxOverflowCallback = std::function<RxOverflowAction(const uint8_t* data, size_t len, void* userArg)>;
+
+    NimBLEStream() = default;
+    virtual ~NimBLEStream() { end(); }
+
+    // Print/Stream TX methods
+    virtual size_t write(const uint8_t* data, size_t len) override;
+    virtual size_t write(uint8_t data) override { return write(&data, 1); }
+
+    // Template for other integral types (char, int, long, etc.)
+    template <typename T>
+    typename std::enable_if<std::is_integral<T>::value && !std::is_same<T, uint8_t>::value, size_t>::type write(T data) {
+        return write(static_cast<uint8_t>(data));
+    }
+
+    size_t availableForWrite() const;
+
+    // Read up to len bytes into buffer (non-blocking)
+    size_t read(uint8_t* buffer, size_t len);
+
+    // Stream RX methods
+    virtual int  available() override;
+    virtual int  read() override;
+    virtual int  peek() override;
+    virtual bool ready() const = 0;
+
+    /**
+     * @brief Set a callback to be invoked when incoming data exceeds RX buffer capacity.
+     * @param cb The callback function, which should return DROP_OLDER_DATA to drop older buffered data and
+     * make room for the new data, or DROP_NEW_DATA to drop the new data instead.
+     */
+    void setRxOverflowCallback(RxOverflowCallback cb, void* userArg = nullptr) {
+        m_rxOverflowCallback = cb;
+        m_rxOverflowUserArg  = userArg;
+    }
+
+    operator bool() const { return ready(); }
+
+    using Print::write;
+
+    struct ByteRingBuffer;
+
+  protected:
+    bool         begin();
+    void         drainTx();
+    size_t       pushRx(const uint8_t* data, size_t len);
+    virtual void end();
+    virtual bool send() = 0;
+    static void  txDrainEventCb(struct ble_npl_event* ev);
+    static void  txDrainCalloutCb(struct ble_npl_event* ev);
+
+    ByteRingBuffer*    m_txBuf{nullptr};
+    ByteRingBuffer*    m_rxBuf{nullptr};
+    uint8_t            m_txChunkBuf[MYNEWT_VAL(BLE_ATT_PREFERRED_MTU)];
+    uint32_t           m_txBufSize{1024};
+    uint32_t           m_rxBufSize{1024};
+    ble_npl_event      m_txDrainEvent{};
+    ble_npl_callout    m_txDrainCallout{};
+    RxOverflowCallback m_rxOverflowCallback{nullptr};
+    void*              m_rxOverflowUserArg{nullptr};
+    bool               m_coInitialized{false};
+    bool               m_eventInitialized{false};
+};
+
+# if MYNEWT_VAL(BLE_ROLE_PERIPHERAL)
+#  include "NimBLECharacteristic.h"
+
+class NimBLEStreamServer : public NimBLEStream {
+  public:
+    NimBLEStreamServer() : m_charCallbacks(this) {}
+    ~NimBLEStreamServer() override { end(); }
+
+    // non-copyable
+    NimBLEStreamServer(const NimBLEStreamServer&)            = delete;
+    NimBLEStreamServer& operator=(const NimBLEStreamServer&) = delete;
+
+    bool begin(NimBLECharacteristic* chr, uint32_t txBufSize = 1024, uint32_t rxBufSize = 1024);
+
+    // Convenience overload to create service/characteristic internally; service will be deleted on end()
+    bool begin(const NimBLEUUID& svcUuid,
+               const NimBLEUUID& chrUuid,
+               uint32_t          txBufSize = 1024,
+               uint32_t          rxBufSize = 1024,
+               bool              secure    = false);
+
+    void     end() override;
+    size_t   write(const uint8_t* data, size_t len) override;
+    uint16_t getPeerHandle() const { return m_charCallbacks.m_peerHandle; }
+    void     setCallbacks(NimBLECharacteristicCallbacks* pCallbacks) { m_charCallbacks.m_userCallbacks = pCallbacks; }
+    bool     ready() const override;
+    virtual void flush() override;
+
+    using NimBLEStream::write; // Inherit template write overloads
+
+  protected:
+    bool send() override;
+
+    struct ChrCallbacks : public NimBLECharacteristicCallbacks {
+        ChrCallbacks(NimBLEStreamServer* parent)
+            : m_parent(parent), m_userCallbacks(nullptr), m_peerHandle(BLE_HS_CONN_HANDLE_NONE) {}
+        void onWrite(NimBLECharacteristic* pCharacteristic, NimBLEConnInfo& connInfo) override;
+        void onSubscribe(NimBLECharacteristic* pCharacteristic, NimBLEConnInfo& connInfo, uint16_t subValue) override;
+        void onStatus(NimBLECharacteristic* pCharacteristic, NimBLEConnInfo& connInfo, int code) override;
+        // override this to avoid recursion when debug logs are enabled
+        void onStatus(NimBLECharacteristic* pCharacteristic, int code) override {
+            if (m_userCallbacks != nullptr) {
+                m_userCallbacks->onStatus(pCharacteristic, code);
+            }
+        }
+
+        NimBLEStreamServer*            m_parent;
+        NimBLECharacteristicCallbacks* m_userCallbacks;
+        uint16_t                       m_peerHandle;
+    } m_charCallbacks;
+
+    NimBLECharacteristic* m_pChr{nullptr};
+    int                   m_rc{0};
+    // Whether to delete the BLE service when end() is called; set to false if service is managed externally
+    bool                  m_deleteSvcOnEnd{false};
+};
+# endif // BLE_ROLE_PERIPHERAL
+
+# if MYNEWT_VAL(BLE_ROLE_CENTRAL)
+#  include "NimBLERemoteCharacteristic.h"
+
+class NimBLEStreamClient : public NimBLEStream {
+  public:
+    NimBLEStreamClient() = default;
+    ~NimBLEStreamClient() override { end(); }
+
+    // non-copyable
+    NimBLEStreamClient(const NimBLEStreamClient&)            = delete;
+    NimBLEStreamClient& operator=(const NimBLEStreamClient&) = delete;
+
+    // Attach a discovered remote characteristic; app owns discovery/connection.
+    // Set subscribeNotify=true to receive notifications into RX buffer.
+    bool         begin(NimBLERemoteCharacteristic* pChr,
+                       bool                        subscribeNotify = false,
+                       uint32_t                    txBufSize       = 1024,
+                       uint32_t                    rxBufSize       = 1024);
+    void         end() override;
+    void         setNotifyCallback(NimBLERemoteCharacteristic::notify_callback cb) { m_userNotifyCallback = cb; }
+    bool         ready() const override;
+    virtual void flush() override;
+
+    using NimBLEStream::write; // Inherit template write overloads
+
+  protected:
+    bool send() override;
+    void notifyCallback(NimBLERemoteCharacteristic* pChar, uint8_t* pData, size_t len, bool isNotify);
+
+    NimBLERemoteCharacteristic*                 m_pChr{nullptr};
+    NimBLERemoteCharacteristic::notify_callback m_userNotifyCallback{nullptr};
+};
+# endif // BLE_ROLE_CENTRAL
+
+#endif // CONFIG_BT_NIMBLE_ENABLED && (MYNEWT_VAL(BLE_ROLE_PERIPHERAL) || MYNEWT_VAL(BLE_ROLE_CENTRAL))
+#endif // NIMBLE_CPP_STREAM_H

src/NimBLEUUID.cpp

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

src/NimBLEUUID.h

@@ -21,10 +21,10 @@
 #include "syscfg/syscfg.h"
 #if CONFIG_BT_NIMBLE_ENABLED
 
-# if defined(CONFIG_NIMBLE_CPP_IDF)
-#  include "host/ble_uuid.h"
-# else
+# ifdef USING_NIMBLE_ARDUINO_HEADERS
 #  include "nimble/nimble/host/include/host/ble_uuid.h"
+# else
+#  include "host/ble_uuid.h"
 # endif
 
 /****  FIX COMPILATION ****/
@@ -45,6 +45,7 @@ class NimBLEUUID {
      */
     NimBLEUUID() = default;
     NimBLEUUID(const ble_uuid_any_t& uuid);
+    NimBLEUUID(const ble_uuid_t* uuid);
     NimBLEUUID(const std::string& uuid);
     NimBLEUUID(uint16_t uuid);
     NimBLEUUID(uint32_t uuid);
@@ -64,7 +65,7 @@ class NimBLEUUID {
 
     bool operator==(const NimBLEUUID& rhs) const;
     bool operator!=(const NimBLEUUID& rhs) const;
-    operator std::string() const;
+         operator std::string() const;
 
   private:
     ble_uuid_any_t m_uuid{};

src/NimBLEUtils.cpp

@@ -21,10 +21,10 @@
 # include "NimBLEAddress.h"
 # include "NimBLELog.h"
 
-# if defined(CONFIG_NIMBLE_CPP_IDF)
-#  include "host/ble_hs.h"
-# else
+#ifdef USING_NIMBLE_ARDUINO_HEADERS
 #  include "nimble/nimble/host/include/host/ble_hs.h"
+#else
+#  include "host/ble_hs.h"
 # endif
 
 /****  FIX COMPILATION ****/
@@ -35,14 +35,42 @@
 # include <stdlib.h>
 # include <climits>
 
-# if defined INC_FREERTOS_H
-#  ifndef CONFIG_NIMBLE_CPP_FREERTOS_TASK_BLOCK_BIT
-#   define CONFIG_NIMBLE_CPP_FREERTOS_TASK_BLOCK_BIT 31
+# ifndef MYNEWT_VAL_NIMBLE_CPP_ENABLE_RETURN_CODE_TEXT
+#  ifdef CONFIG_NIMBLE_CPP_ENABLE_RETURN_CODE_TEXT
+#   define MYNEWT_VAL_NIMBLE_CPP_ENABLE_RETURN_CODE_TEXT CONFIG_NIMBLE_CPP_ENABLE_RETURN_CODE_TEXT
+#  else
+#   define MYNEWT_VAL_NIMBLE_CPP_ENABLE_RETURN_CODE_TEXT (0)
 #  endif
-constexpr uint32_t TASK_BLOCK_BIT = (1 << CONFIG_NIMBLE_CPP_FREERTOS_TASK_BLOCK_BIT);
 # endif
 
-static const char* LOG_TAG = "NimBLEUtils";
+# ifndef MYNEWT_VAL_NIMBLE_CPP_ENABLE_GAP_EVENT_CODE_TEXT
+#  ifdef CONFIG_NIMBLE_CPP_ENABLE_GAP_EVENT_CODE_TEXT
+#   define MYNEWT_VAL_NIMBLE_CPP_ENABLE_GAP_EVENT_CODE_TEXT CONFIG_NIMBLE_CPP_ENABLE_GAP_EVENT_CODE_TEXT
+#  else
+#   define MYNEWT_VAL_NIMBLE_CPP_ENABLE_GAP_EVENT_CODE_TEXT (0)
+#  endif
+# endif
+
+# ifndef MYNEWT_VAL_NIMBLE_CPP_ENABLE_ADVERTISEMENT_TYPE_TEXT
+#  ifdef CONFIG_NIMBLE_CPP_ENABLE_ADVERTISEMENT_TYPE_TEXT
+#   define MYNEWT_VAL_NIMBLE_CPP_ENABLE_ADVERTISEMENT_TYPE_TEXT CONFIG_NIMBLE_CPP_ENABLE_ADVERTISEMENT_TYPE_TEXT
+#  else
+#   define MYNEWT_VAL_NIMBLE_CPP_ENABLE_ADVERTISEMENT_TYPE_TEXT (0)
+#  endif
+# endif
+
+# if defined INC_FREERTOS_H
+#  ifndef MYNEWT_VAL_NIMBLE_CPP_FREERTOS_TASK_BLOCK_BIT
+#   ifndef CONFIG_NIMBLE_CPP_FREERTOS_TASK_BLOCK_BIT
+#    define MYNEWT_VAL_NIMBLE_CPP_FREERTOS_TASK_BLOCK_BIT 31
+#   else
+#    define MYNEWT_VAL_NIMBLE_CPP_FREERTOS_TASK_BLOCK_BIT CONFIG_NIMBLE_CPP_FREERTOS_TASK_BLOCK_BIT
+#   endif
+#  endif
+# endif
+
+constexpr uint32_t TASK_BLOCK_BIT = (1 << MYNEWT_VAL(NIMBLE_CPP_FREERTOS_TASK_BLOCK_BIT));
+static const char* LOG_TAG        = "NimBLEUtils";
 
 /**
  * @brief Construct a NimBLETaskData instance.
@@ -54,7 +82,7 @@ NimBLETaskData::NimBLETaskData(void* pInstance, int flags, void* buf)
     : m_pInstance{pInstance},
       m_flags{flags},
       m_pBuf{buf}
-# if defined INC_FREERTOS_H
+# ifdef INC_FREERTOS_H
       ,
       m_pHandle{xTaskGetCurrentTaskHandle()} {
 }
@@ -75,7 +103,7 @@ NimBLETaskData::NimBLETaskData(void* pInstance, int flags, void* buf)
  * @brief Destructor.
  */
 NimBLETaskData::~NimBLETaskData() {
-# if !defined INC_FREERTOS_H
+# ifndef INC_FREERTOS_H
     if (m_pHandle != nullptr) {
         ble_npl_sem_deinit(static_cast<ble_npl_sem*>(m_pHandle));
         delete static_cast<ble_npl_sem*>(m_pHandle);
@@ -97,7 +125,7 @@ bool NimBLEUtils::taskWait(const NimBLETaskData& taskData, uint32_t timeout) {
         ble_npl_time_ms_to_ticks(timeout, &ticks);
     }
 
-# if defined INC_FREERTOS_H
+# ifdef INC_FREERTOS_H
     uint32_t notificationValue;
     xTaskNotifyWait(0, TASK_BLOCK_BIT, &notificationValue, 0);
     if (notificationValue & TASK_BLOCK_BIT) {
@@ -119,7 +147,7 @@ bool NimBLEUtils::taskWait(const NimBLETaskData& taskData, uint32_t timeout) {
 void NimBLEUtils::taskRelease(const NimBLETaskData& taskData, int flags) {
     taskData.m_flags = flags;
     if (taskData.m_pHandle != nullptr) {
-# if defined INC_FREERTOS_H
+# ifdef INC_FREERTOS_H
         xTaskNotify(static_cast<TaskHandle_t>(taskData.m_pHandle), TASK_BLOCK_BIT, eSetBits);
 # else
         ble_npl_sem_release(static_cast<ble_npl_sem*>(taskData.m_pHandle));
@@ -133,7 +161,7 @@ void NimBLEUtils::taskRelease(const NimBLETaskData& taskData, int flags) {
  * @return A string representation of the return code.
  */
 const char* NimBLEUtils::returnCodeToString(int rc) {
-# if defined(CONFIG_NIMBLE_CPP_ENABLE_RETURN_CODE_TEXT)
+# if MYNEWT_VAL(NIMBLE_CPP_ENABLE_RETURN_CODE_TEXT)
     switch (rc) {
         case 0:
             return "SUCCESS";
@@ -416,10 +444,10 @@ const char* NimBLEUtils::returnCodeToString(int rc) {
         default:
             return "Unknown";
     }
-# else  // #if defined(CONFIG_NIMBLE_CPP_ENABLE_RETURN_CODE_TEXT)
+# else  // MYNEWT_VAL(NIMBLE_CPP_ENABLE_RETURN_CODE_TEXT)
     (void)rc;
     return "";
-# endif // #if defined(CONFIG_NIMBLE_CPP_ENABLE_RETURN_CODE_TEXT)
+# endif // MYNEWT_VAL(NIMBLE_CPP_ENABLE_RETURN_CODE_TEXT)
 }
 
 /**
@@ -428,7 +456,7 @@ const char* NimBLEUtils::returnCodeToString(int rc) {
  * @return A string representation of the advertising flags.
  */
 const char* NimBLEUtils::advTypeToString(uint8_t advType) {
-# if defined(CONFIG_NIMBLE_CPP_ENABLE_ADVERTISEMENT_TYPE_TEXT)
+# if MYNEWT_VAL(NIMBLE_CPP_ENABLE_ADVERTISEMENT_TYPE_TEXT)
     switch (advType) {
         case BLE_HCI_ADV_TYPE_ADV_IND: // 0
             return "Undirected - Connectable / Scannable";
@@ -443,10 +471,10 @@ const char* NimBLEUtils::advTypeToString(uint8_t advType) {
         default:
             return "Unknown flag";
     }
-# else  // #if defined(CONFIG_NIMBLE_CPP_ENABLE_ADVERTISEMENT_TYPE_TEXT)
+# else  // MYNEWT_VAL(NIMBLE_CPP_ENABLE_ADVERTISEMENT_TYPE_TEXT)
     (void)advType;
     return "";
-# endif // #if defined(CONFIG_NIMBLE_CPP_ENABLE_ADVERTISEMENT_TYPE_TEXT)
+# endif // MYNEWT_VAL(NIMBLE_CPP_ENABLE_ADVERTISEMENT_TYPE_TEXT)
 } // adFlagsToString
 
 /**
@@ -455,7 +483,7 @@ const char* NimBLEUtils::advTypeToString(uint8_t advType) {
  * @return A string representation of the event type.
  */
 const char* NimBLEUtils::gapEventToString(uint8_t eventType) {
-# if defined(CONFIG_NIMBLE_CPP_ENABLE_GAP_EVENT_CODE_TEXT)
+# if MYNEWT_VAL(NIMBLE_CPP_ENABLE_GAP_EVENT_CODE_TEXT)
     switch (eventType) {
         case BLE_GAP_EVENT_CONNECT: // 0
             return "BLE_GAP_EVENT_CONNECT ";
@@ -535,10 +563,10 @@ const char* NimBLEUtils::gapEventToString(uint8_t eventType) {
             NIMBLE_LOGD(LOG_TAG, "Unknown event type %d 0x%.2x", eventType, eventType);
             return "Unknown event type";
     }
-# else  // #if defined(CONFIG_NIMBLE_CPP_ENABLE_GAP_EVENT_CODE_TEXT)
+# else // MYNEWT_VAL(NIMBLE_CPP_ENABLE_GAP_EVENT_CODE_TEXT)
     (void)eventType;
     return "";
-# endif // #if defined(CONFIG_NIMBLE_CPP_ENABLE_GAP_EVENT_CODE_TEXT)
+# endif // MYNEWT_VAL(NIMBLE_CPP_ENABLE_GAP_EVENT_CODE_TEXT)
 } // gapEventToString
 
 /**

src/NimBLEUtils.h

@@ -21,7 +21,15 @@
 #include "syscfg/syscfg.h"
 #if CONFIG_BT_NIMBLE_ENABLED
 
-#if CONFIG_NIMBLE_CPP_DEBUG_ASSERT_ENABLED && !defined NDEBUG
+# ifndef MYNEWT_VAL_NIMBLE_CPP_DEBUG_ASSERT_ENABLED
+#  if defined(CONFIG_NIMBLE_CPP_DEBUG_ASSERT_ENABLED)
+#   define MYNEWT_VAL_NIMBLE_CPP_DEBUG_ASSERT_ENABLED CONFIG_NIMBLE_CPP_DEBUG_ASSERT_ENABLED
+#  else
+#   define MYNEWT_VAL_NIMBLE_CPP_DEBUG_ASSERT_ENABLED (0)
+#  endif
+# endif
+
+#if MYNEWT_VAL(NIMBLE_CPP_DEBUG_ASSERT_ENABLED) && !defined NDEBUG
 void nimble_cpp_assert(const char *file, unsigned line) __attribute((weak, noreturn));
 # define NIMBLE_ATT_VAL_FILE  (__builtin_strrchr(__FILE__, '/') ? \
                             __builtin_strrchr (__FILE__, '/') + 1 : __FILE__)

src/NimBLEValueAttribute.h

@@ -26,7 +26,7 @@
 
 class NimBLEValueAttribute {
   public:
-    NimBLEValueAttribute(uint16_t maxLen = BLE_ATT_ATTR_MAX_LEN, uint16_t initLen = CONFIG_NIMBLE_CPP_ATT_VALUE_INIT_LENGTH)
+    NimBLEValueAttribute(uint16_t maxLen = BLE_ATT_ATTR_MAX_LEN, uint16_t initLen = MYNEWT_VAL(NIMBLE_CPP_ATT_VALUE_INIT_LENGTH))
         : m_value(initLen, maxLen) {}
 
     /**