Release 2.5.0

feat(L2CAP): add disconnect API and harden CoC send/error handling
- 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
2026-04-13 05:05:51 +02:00 · 2026-04-01 14:18:02 -06:00 · 2026-04-01 07:13:59 -06:00 · 2026-04-01 07:13:56 -06:00 · 2026-04-01 07:13:51 -06:00 · 2026-04-01 07:13:46 -06:00
46 changed files with 1456 additions and 382 deletions
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -1,6 +1,22 @@
 # 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
--- a/4
+++ b/4
@@ -204,8 +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
-
 endmenu
--- a/docs/Doxyfile
+++ b/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.4.0
+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.
--- a/examples/ANCS/CMakeLists.txt
+++ b/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)
--- a/examples/ANCS/main/CMakeLists.txt
+++ b/examples/ANCS/main/CMakeLists.txt
@@ -0,0 +1,4 @@
+set(COMPONENT_SRCS "main.cpp")
+set(COMPONENT_ADD_INCLUDEDIRS ".")
+
+register_component()
--- a/examples/ANCS/main/main.cpp
+++ b/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);
+    }
+}
--- a/examples/ANCS/sdkconfig.defaults
+++ b/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
--- a/examples/L2CAP/L2CAP_Client/main/idf_component.yml
+++ b/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"
--- a/examples/L2CAP/L2CAP_Client/main/main.cpp
+++ b/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);
    }
 }
--- a/examples/L2CAP/L2CAP_Server/main/idf_component.yml
+++ b/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"
--- a/examples/L2CAP/L2CAP_Server/main/main.cpp
+++ b/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");
+            }
+        }
    }
 }
--- a/idf_component.yml
+++ b/idf_component.yml
@@ -1,5 +1,5 @@
 ## IDF Component Manager Manifest File
-version: "2.4.0"
+version: "2.5.0"
 license: "Apache-2.0"
 description: "C++ wrapper for the NimBLE BLE stack"
 url: "https://github.com/h2zero/esp-nimble-cpp"
--- a/library.json
+++ b/library.json
@@ -1,6 +1,6 @@
 {
  "name": "esp-nimble-cpp",
-  "version": "2.4.0",
+  "version": "2.5.0",
  "description": "C++ wrapper for the NimBLE BLE stack",
  "keywords": [
    "BLE",
@@ -19,10 +19,5 @@
    "email": "ryan@nable-embedded.io",
    "url": "https://github.com/h2zero/esp-nimble-cpp",
    "maintainer": true
-  },
-  "build": {
-    "flags": [
-      "-DCONFIG_NIMBLE_CPP_IDF=1"
-    ]
  }
 }
--- a/src/NimBLEAddress.h
+++ b/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
--- a/src/NimBLEAdvertisedDevice.cpp
+++ b/src/NimBLEAdvertisedDevice.cpp
@@ -52,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

 /**
--- a/src/NimBLEAdvertisedDevice.h
+++ b/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>
@@ -158,11 +158,13 @@ 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{};
-    uint16_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{};
--- a/src/NimBLEAdvertisementData.cpp
+++ b/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";
--- a/src/NimBLEAdvertising.cpp
+++ b/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"
--- a/src/NimBLEAdvertising.h
+++ b/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 ****/
--- a/src/NimBLEAttValue.cpp
+++ b/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"
--- a/src/NimBLEAttValue.h
+++ b/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>
@@ -145,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.
@@ -248,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.
@@ -258,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));
@@ -329,6 +354,9 @@ class NimBLEAttValue {
            }
        } 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));
        }
@@ -398,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
--- a/src/NimBLECharacteristic.cpp
+++ b/src/NimBLECharacteristic.cpp
@@ -18,7 +18,7 @@
 #include "NimBLECharacteristic.h"
 #if CONFIG_BT_NIMBLE_ENABLED && MYNEWT_VAL(BLE_ROLE_PERIPHERAL)

-# if defined(CONFIG_NIMBLE_CPP_IDF)
+# 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
@@ -132,7 +132,7 @@ void NimBLECharacteristic::addDescriptor(NimBLEDescriptor* pDescriptor) {
    }

    pDescriptor->setCharacteristic(this);
-    NimBLEDevice::getServer()->serviceChanged();
+    NimBLEDevice::getServer()->setServiceChanged();
 }

 /**
@@ -159,7 +159,7 @@ void NimBLECharacteristic::removeDescriptor(NimBLEDescriptor* pDescriptor, bool
    }

    pDescriptor->setRemoved(deleteDsc ? NIMBLE_ATT_REMOVE_DELETE : NIMBLE_ATT_REMOVE_HIDE);
-    NimBLEDevice::getServer()->serviceChanged();
+    NimBLEDevice::getServer()->setServiceChanged();
 } // removeDescriptor

 /**
--- a/src/NimBLEClient.cpp
+++ b/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.
@@ -934,14 +1019,13 @@ 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_id_addr);
+                pClient = NimBLEDevice::getClientByPeerAddress(event->disconnect.conn.peer_ota_addr);
            }

-            // try by connection handle
            if (pClient == nullptr) {
-                pClient = NimBLEDevice::getClientByHandle(event->disconnect.conn.conn_handle);
+                pClient = NimBLEDevice::getClientByPeerAddress(event->disconnect.conn.peer_id_addr);
            }

            if (pClient == nullptr) {
@@ -949,6 +1033,9 @@ int NimBLEClient::handleGapEvent(struct ble_gap_event* event, void* arg) {
                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.
@@ -969,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;
                }
@@ -994,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;
            }
@@ -1005,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);
@@ -1052,6 +1167,10 @@ int NimBLEClient::handleGapEvent(struct ble_gap_event* event, void* arg) {
                return 0;
            }

+            if (pClient->completeConnectEstablished()) {
+                pTaskData = nullptr;
+            }
+
            NIMBLE_LOGD(LOG_TAG, "Notify Received for handle: %d", event->notify_rx.attr_handle);

            NimBLERemoteCharacteristic* pChr = pClient->getCharacteristic(event->notify_rx.attr_handle);
@@ -1098,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",
@@ -1125,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 {
@@ -1138,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) {
@@ -1147,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.
@@ -1165,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) {
@@ -1177,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) {
@@ -1192,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;
@@ -1203,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) {
@@ -1258,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

 /**
@@ -1348,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)
--- a/src/NimBLEClient.h
+++ b/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;
--- a/src/NimBLEConnInfo.h
+++ b/src/NimBLEConnInfo.h
@@ -18,10 +18,10 @@
 #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"
--- a/src/NimBLECppVersion.h
+++ b/src/NimBLECppVersion.h
@@ -22,7 +22,7 @@
 #define NIMBLE_CPP_VERSION_MAJOR 2

 /** @brief NimBLE-Arduino library minor version number. */
-#define NIMBLE_CPP_VERSION_MINOR 4
+#define NIMBLE_CPP_VERSION_MINOR 5

 /** @brief NimBLE-Arduino library patch version number. */
 #define NIMBLE_CPP_VERSION_PATCH 0
--- a/src/NimBLEDevice.cpp
+++ b/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"
@@ -359,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;
@@ -432,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;
        }
    }
@@ -682,7 +682,7 @@ 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)
+#  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));
@@ -691,10 +691,10 @@ NimBLEAddress NimBLEDevice::getBondedAddress(int index) {
    }

    return NimBLEAddress(peer_id_addrs[index]);
-# else
+#  else
    (void)index; // unused
    return NimBLEAddress{};
-# endif
+#  endif
 }
 # endif

@@ -911,7 +911,7 @@ 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;
@@ -1025,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) {
@@ -1275,17 +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)
+#  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
+#  else
    (void)peerInfo;
    (void)passkey;
    NIMBLE_LOGE(LOG_TAG, "Passkey entry not supported with current security settings");
    return false;
-#endif
+#  endif
 }

 /**
@@ -1294,17 +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)
+#  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
+#  else
    (void)peerInfo;
    (void)accept;
    NIMBLE_LOGE(LOG_TAG, "Numeric comparison not supported with current security settings");
    return false;
-#endif
+#  endif
 }
 # endif // MYNEWT_VAL(BLE_ROLE_CENTRAL) || MYNEWT_VAL(BLE_ROLE_PERIPHERAL)

--- a/src/NimBLEDevice.h
+++ b/src/NimBLEDevice.h
@@ -29,10 +29,10 @@
      (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 ****/
--- a/src/NimBLEExtAdvertising.cpp
+++ b/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"
--- a/src/NimBLEExtAdvertising.h
+++ b/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 ****/
--- a/src/NimBLEL2CAPChannel.cpp
+++ b/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;
--- a/src/NimBLEL2CAPChannel.h
+++ b/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; }

--- a/src/NimBLELocalValueAttribute.h
+++ b/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 ****/
--- a/src/NimBLELog.h
+++ b/src/NimBLELog.h
@@ -33,7 +33,7 @@
 #  endif
 # endif

-# if defined(CONFIG_NIMBLE_CPP_IDF)
+# ifndef USING_NIMBLE_ARDUINO_HEADERS
 #  include "esp_log.h"
 #  include "console/console.h"

@@ -172,7 +172,7 @@
 #   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__); }}
--- a/src/NimBLERemoteService.cpp
+++ b/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;
    }
--- a/src/NimBLERemoteService.h
+++ b/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,
--- a/src/NimBLERemoteValueAttribute.h
+++ b/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 ****/
--- a/src/NimBLEScan.cpp
+++ b/src/NimBLEScan.cpp
@@ -20,13 +20,50 @@

 # 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.
 */
@@ -47,17 +84,128 @@ NimBLEScan::NimBLEScan()
      },
      m_pTaskData{nullptr},
      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.
@@ -113,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 &&
@@ -121,19 +271,39 @@ 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) {
                    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;
+                        }
                    }
                }
            }
@@ -159,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.
@@ -170,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) {
@@ -190,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.
@@ -220,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.
@@ -335,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();
        }
    }

@@ -351,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,
@@ -406,6 +619,8 @@ bool NimBLEScan::stop() {
        return false;
    }

+    clearWaitingList();
+
    if (m_maxResults == 0) {
        clearResults();
    }
@@ -426,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;
@@ -441,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;
@@ -495,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();
--- a/src/NimBLEScan.h
+++ b/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};
--- a/src/NimBLEServer.cpp
+++ b/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
@@ -98,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

@@ -187,12 +186,20 @@ 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.
@@ -313,7 +320,7 @@ bool NimBLEServer::start() {
    // If the services have changed indicate it now
    if (m_svcChanged) {
        m_svcChanged = false;
-        ble_svc_gatt_changed(0x0001, 0xffff);
+        sendServiceChangedIndication();
    }

    m_gattsStarted = true;
@@ -328,12 +335,15 @@ bool 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

 /**
@@ -836,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
@@ -863,7 +873,7 @@ void NimBLEServer::addService(NimBLEService* service) {
    }

    service->setRemoved(0);
-    serviceChanged();
+    setServiceChanged();
 } // addService

 /**
@@ -1062,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
@@ -1109,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

--- a/src/NimBLEServer.h
+++ b/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 ****/
@@ -84,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);
@@ -122,7 +123,7 @@ class 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        serviceChanged();
+    void        setServiceChanged();
    bool        resetGATT();

    bool m_gattsStarted : 1;
--- a/src/NimBLEService.cpp
+++ b/src/NimBLEService.cpp
@@ -245,7 +245,7 @@ void NimBLEService::addCharacteristic(NimBLECharacteristic* pChar) {
    }

    pChar->setService(this);
-    getServer()->serviceChanged();
+    getServer()->setServiceChanged();
 } // addCharacteristic

 /**
@@ -272,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

 /**
--- a/src/NimBLEStream.cpp
+++ b/src/NimBLEStream.cpp
@@ -20,12 +20,12 @@

 # include "NimBLEDevice.h"
 # include "NimBLELog.h"
-# if defined(CONFIG_NIMBLE_CPP_IDF)
-#  include "os/os_mbuf.h"
-#  include "nimble/nimble_port.h"
-# else
+# ifdef USING_NIMBLE_ARDUINO_HEADERS
 #  include "nimble/porting/nimble/include/os/os_mbuf.h"
 #  include "nimble/porting/nimble/include/nimble/nimble_port.h"
+# else
+#  include "os/os_mbuf.h"
+#  include "nimble/nimble_port.h"
 # endif
 # include <algorithm>
 # include <cstdio>
--- a/src/NimBLEStream.h
+++ b/src/NimBLEStream.h
@@ -21,16 +21,20 @@
 #include "syscfg/syscfg.h"
 #if CONFIG_BT_NIMBLE_ENABLED && (MYNEWT_VAL(BLE_ROLE_PERIPHERAL) || MYNEWT_VAL(BLE_ROLE_CENTRAL))

-# 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 <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
@@ -100,7 +104,7 @@ class NimBLEStream : public Stream {
     */
    void setRxOverflowCallback(RxOverflowCallback cb, void* userArg = nullptr) {
        m_rxOverflowCallback = cb;
-        m_rxOverflowUserArg = userArg;
+        m_rxOverflowUserArg  = userArg;
    }

    operator bool() const { return ready(); }
@@ -118,17 +122,17 @@ class NimBLEStream : public Stream {
    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{};
+    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};
+    void*              m_rxOverflowUserArg{nullptr};
+    bool               m_coInitialized{false};
+    bool               m_eventInitialized{false};
 };

 # if MYNEWT_VAL(BLE_ROLE_PERIPHERAL)
@@ -203,13 +207,13 @@ class NimBLEStreamClient : public NimBLEStream {

    // 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;
+    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
--- a/src/NimBLEUUID.h
+++ b/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 ****/
@@ -65,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{};
--- a/src/NimBLEUtils.cpp
+++ b/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 ****/
Author	SHA1	Message	Date
h2zero	e26b502297	Release 2.5.0	2026-04-01 14:18:02 -06:00
Dr. Michael Lauer	aede439dab	feat(L2CAP): add disconnect API and harden CoC send/error handling - 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	2026-04-01 07:13:59 -06:00
h2zero	1d39b8fd05	Add a flag to to use arduino header paths - fixes PIO builds	2026-04-01 07:13:56 -06:00
copilot-swe-agent[bot]	c9c3e05b2d	Fix setValue() using sizeof instead of strlen for char array arguments Agent-Logs-Url: https://github.com/h2zero/NimBLE-Arduino/sessions/42ace199-2049-4c37-8f0a-30443045b8b2 Co-authored-by: h2zero <32826625+h2zero@users.noreply.github.com>	2026-04-01 07:13:51 -06:00
h2zero	958e9bc0d0	Fix backward compatability issue with remote charactertistic sorting	2026-04-01 07:13:46 -06:00
h2zero	31801cf91d	Add ANCS example	2026-04-01 07:13:39 -06:00
h2zero	d15dff0ad8	[Bugfix] get remote characteristic by uuid returning the wrong instance. This fixes a bug introduced when sorting the characteristic vector where the last item on in the vector was returned but the characteristc wasn't at the end anymore. This reverts the previous sorting of the vector after retrieval and instead sorts as the characteristics are found and also provides a pointer to the characteristic from the retrieval callback.	2026-04-01 07:13:35 -06:00
h2zero	017f7ce581	[Bugfix] Remote descriptor not found when char handles out of order This resolves the issue of retrieving characteristics out of handle order and then trying to subscribe or do some other fetch of a descriptor belonging to that characteristic and the search handle was limited to the handle of the next characteristic in the vector. This ensures that the vector is sorted in ascending order based on handle values so the next characteristic in the vector will always have a higher handle value and the desciptor serach will function as intended.	2026-04-01 07:13:25 -06:00
h2zero	055cc2ba83	[Bugfix] Set server created client connection status on creation.	2026-04-01 07:13:22 -06:00
h2zero	a76d579501	[Bugfix] HCI response to disconnect of unknown ID should return success. * Update macro use to use conversion macros.	2026-04-01 07:13:17 -06:00
h2zero	3a603185a7	[Feat] Wait for connections to establish before callback, retry on fail * Delay onConnect until connection is likely established using a timer (7x conn interval), with early completion on peer activity. * Suppress onDisconnect for 0x3e and route terminal 0x3e failures to onConnectFail. * Add configurable 0x3e reconnect attempts via `setConnectRetries` or client config (default 2). * Retry logic suppresses callbacks while retrying; callbacks fire only on final outcome. * Refactor connect-start path into a shared helper used by initial connect and retries.	2026-04-01 07:13:09 -06:00
h2zero	0c221c56c4	[Bugfix] Arduino detection for headers.	2026-04-01 07:13:04 -06:00
h2zero	2eb47cb96b	[Bugfix] Incorrect Client connection state tracking and self delete This adds better client state tracking so that functions like NimBLEDevice::getDisconnectedClient get a more accurate state and will not return a connecting client. This also fixes the client self delete on connection error where function call errors did not delete the client	2026-04-01 07:12:47 -06:00
h2zero	94939cd98d	Add NimBLEServer::sendServiceChangedIndication This reverts changing NimBLEServer::serviceChanged from private to public and adds a new function that is more user firendly to indicate to clients that the services should be re-discovered. * Renames serviceChanged to setServiceChanged to better indicate its function.	2026-04-01 07:12:44 -06:00
h2zero	ced2be5e7d	Add a timer for scan responses This ensures that the callback will be called within the configured time (in ms) when devices fail to respond to a scan response request within that time. * Adds stats when debug logging to help tune the scan response timeout/scan parameters.	2026-04-01 07:12:41 -06:00
Chris Morgan	6f692697df	NimBLEServer::serviceChanged() should be public to enable it being called outside of the library to indicate GATT changes	2026-04-01 07:12:39 -06:00