diff --git a/Kconfig b/Kconfig
index db717983..2a603c6b 100644
--- a/Kconfig
+++ b/Kconfig
@@ -76,6 +76,13 @@ config NIMBLE_CPP_DEBUG_ASSERT_ENABLED
         Enabling this option will add debug asserts to the NimBLE CPP library.
         This will use approximately 1kB of flash memory.
 
+config NIMBLE_CPP_FREERTOS_TASK_BLOCK_BIT
+    int "FreeRTOS task block bit"
+    default 31
+    help
+        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.
+        
 #
 # BT config
 #
diff --git a/src/NimBLEClient.cpp b/src/NimBLEClient.cpp
index da4b58f6..5e5f7dbf 100644
--- a/src/NimBLEClient.cpp
+++ b/src/NimBLEClient.cpp
@@ -204,10 +204,6 @@ bool NimBLEClient::connect(const NimBLEAddress& address, bool deleteAttributes,
     int rc         = 0;
     m_asyncConnect = asyncConnect;
     m_exchangeMTU  = exchangeMTU;
-    NimBLETaskData taskData(this);
-    if (!asyncConnect) {
-        m_pTaskData = &taskData;
-    }
 
     // Set the connection in progress flag to prevent a scan from starting while connecting.
     NimBLEDevice::setConnectionInProgress(true);
@@ -263,10 +259,8 @@ bool NimBLEClient::connect(const NimBLEAddress& address, bool deleteAttributes,
 
     } while (rc == BLE_HS_EBUSY);
 
-    m_lastErr = rc;
     if (rc != 0) {
-        m_lastErr   = rc;
-        m_pTaskData = nullptr;
+        m_lastErr = rc;
         NimBLEDevice::setConnectionInProgress(false);
         return false;
     }
@@ -275,29 +269,31 @@ bool NimBLEClient::connect(const NimBLEAddress& address, bool deleteAttributes,
         return true;
     }
 
+    NimBLETaskData taskData(this);
+    m_pTaskData = &taskData;
+
     // Wait for the connect timeout time +1 second for the connection to complete
-    if (!NimBLEUtils::taskWait(*m_pTaskData, m_connectTimeout + 1000)) {
-        m_pTaskData = nullptr;
-        // If a connection was made but no response from MTU exchange; disconnect
+    if (!NimBLEUtils::taskWait(taskData, m_connectTimeout + 1000)) {
+        // If a connection was made but no response from MTU exchange proceed anyway
         if (isConnected()) {
-            NIMBLE_LOGE(LOG_TAG, "Connect timeout - no response");
-            disconnect();
+            taskData.m_flags = 0;
         } else {
-            // workaround; if the controller doesn't cancel the connection
-            // at the timeout, cancel it here.
+            // workaround; if the controller doesn't cancel the connection at the timeout, cancel it here.
             NIMBLE_LOGE(LOG_TAG, "Connect timeout - cancelling");
             ble_gap_conn_cancel();
+            taskData.m_flags = BLE_HS_ETIMEOUT;
         }
+    }
 
-        return false;
-
-    } else if (taskData.m_flags != 0) {
-        m_lastErr = taskData.m_flags;
-        NIMBLE_LOGE(LOG_TAG, "Connection failed; status=%d %s", m_lastErr, NimBLEUtils::returnCodeToString(m_lastErr));
+    m_pTaskData = nullptr;
+    rc          = taskData.m_flags;
+    if (rc != 0) {
+        NIMBLE_LOGE(LOG_TAG, "Connection failed; status=%d %s", rc, NimBLEUtils::returnCodeToString(rc));
         // If the failure was not a result of a disconnection, make sure we disconnect now to avoid dangling connections
         if (isConnected()) {
             disconnect();
         }
+        m_lastErr = rc;
         return false;
     } else {
         NIMBLE_LOGI(LOG_TAG, "Connection established");
@@ -319,29 +315,27 @@ bool NimBLEClient::connect(const NimBLEAddress& address, bool deleteAttributes,
  */
 bool NimBLEClient::secureConnection() const {
     NIMBLE_LOGD(LOG_TAG, ">> secureConnection()");
-    NimBLETaskData taskData(const_cast<NimBLEClient*>(this));
-    int            retryCount = 1;
 
+    NimBLETaskData taskData(const_cast<NimBLEClient*>(this), BLE_HS_ENOTCONN);
+    m_pTaskData    = &taskData;
+    int retryCount = 1;
     do {
-        m_pTaskData = &taskData;
-
-        if (!NimBLEDevice::startSecurity(m_connHandle)) {
-            m_lastErr   = BLE_HS_ENOTCONN;
-            m_pTaskData = nullptr;
-            return false;
+        if (NimBLEDevice::startSecurity(m_connHandle)) {
+            NimBLEUtils::taskWait(taskData, BLE_NPL_TIME_FOREVER);
         }
-
-        NimBLEUtils::taskWait(*m_pTaskData, BLE_NPL_TIME_FOREVER);
     } while (taskData.m_flags == (BLE_HS_ERR_HCI_BASE + BLE_ERR_PINKEY_MISSING) && retryCount--);
 
-    if (taskData.m_flags != 0) {
-        m_lastErr = taskData.m_flags;
-        NIMBLE_LOGE(LOG_TAG, "secureConnection: failed rc=%d", taskData.m_flags);
-        return false;
+    m_pTaskData = nullptr;
+
+    if (taskData.m_flags == 0) {
+        NIMBLE_LOGD(LOG_TAG, "<< secureConnection: success");
+        return true;
     }
 
-    NIMBLE_LOGD(LOG_TAG, "<< secureConnection: success");
-    return true;
+    m_lastErr = taskData.m_flags;
+    NIMBLE_LOGE(LOG_TAG, "secureConnection: failed rc=%d", taskData.m_flags);
+    return false;
+
 } // secureConnection
 
 /**
@@ -988,7 +982,7 @@ int NimBLEClient::handleGapEvent(struct ble_gap_event* event, void* arg) {
                 pClient->m_connEstablished = rc == 0;
                 pClient->m_pClientCallbacks->onConnect(pClient);
             } else if (!pClient->m_exchangeMTU) {
-                break; // not wating for MTU exchange so release the task now.
+                break; // not waiting for MTU exchange so release the task now.
             }
 
             return 0;
@@ -1177,7 +1171,6 @@ int NimBLEClient::handleGapEvent(struct ble_gap_event* event, void* arg) {
 
     if (pClient->m_pTaskData != nullptr) {
         NimBLEUtils::taskRelease(*pClient->m_pTaskData, rc);
-        pClient->m_pTaskData = nullptr;
     }
 
     return 0;
diff --git a/src/NimBLEUtils.cpp b/src/NimBLEUtils.cpp
index cd3f4fe5..bafcadfa 100644
--- a/src/NimBLEUtils.cpp
+++ b/src/NimBLEUtils.cpp
@@ -27,8 +27,51 @@
 # include <stdlib.h>
 # include <climits>
 
+# if defined INC_FREERTOS_H
+constexpr uint32_t TASK_BLOCK_BIT = (1 << CONFIG_NIMBLE_CPP_FREERTOS_TASK_BLOCK_BIT);
+# endif
+
 static const char* LOG_TAG = "NimBLEUtils";
 
+/**
+ * @brief Construct a NimBLETaskData instance.
+ * @param [in] pInstance An instance of the class that will be waiting.
+ * @param [in] flags General purpose flags for the caller.
+ * @param [in] buf A buffer for data.
+ */
+NimBLETaskData::NimBLETaskData(void* pInstance, int flags, void* buf)
+    : m_pInstance{pInstance},
+      m_flags{flags},
+      m_pBuf{buf}
+# if defined INC_FREERTOS_H
+      ,
+      m_pHandle{xTaskGetCurrentTaskHandle()} {
+}
+# else
+{
+    ble_npl_sem* sem = new ble_npl_sem;
+    if (ble_npl_sem_init(sem, 0) != BLE_NPL_OK) {
+        NIMBLE_LOGE(LOG_TAG, "Failed to init semaphore");
+        delete sem;
+        m_pHandle = nullptr;
+    } else {
+        m_pHandle = sem;
+    }
+}
+# endif
+
+/**
+ * @brief Destructor.
+ */
+NimBLETaskData::~NimBLETaskData() {
+# if !defined INC_FREERTOS_H
+    if (m_pHandle != nullptr) {
+        ble_npl_sem_deinit(static_cast<ble_npl_sem*>(m_pHandle));
+        delete static_cast<ble_npl_sem*>(m_pHandle);
+    }
+# endif
+}
+
 /**
  * @brief Blocks the calling task until released or timeout.
  * @param [in] taskData A pointer to the task data structure.
@@ -44,26 +87,16 @@ bool NimBLEUtils::taskWait(const NimBLETaskData& taskData, uint32_t timeout) {
     }
 
 # if defined INC_FREERTOS_H
-    taskData.m_pHandle = xTaskGetCurrentTaskHandle();
-#  ifdef ulTaskNotifyValueClear
-    // Clear the task notification value to ensure we block
-    ulTaskNotifyValueClear(static_cast<TaskHandle_t>(taskData.m_pHandle), ULONG_MAX);
-#  endif
-    return ulTaskNotifyTake(pdTRUE, ticks) == pdTRUE;
-
-# else
-    ble_npl_sem     sem;
-    ble_npl_error_t err = ble_npl_sem_init(&sem, 0);
-    if (err != BLE_NPL_OK) {
-        NIMBLE_LOGE(LOG_TAG, "Failed to create semaphore");
-        return false;
+    uint32_t notificationValue;
+    xTaskNotifyWait(0, TASK_BLOCK_BIT, &notificationValue, 0);
+    if (notificationValue & TASK_BLOCK_BIT) {
+        return true;
     }
 
-    taskData.m_pHandle = &sem;
-    err                = ble_npl_sem_pend(&sem, ticks);
-    ble_npl_sem_deinit(&sem);
-    taskData.m_pHandle = nullptr;
-    return err == BLE_NPL_OK;
+    return xTaskNotifyWait(0, TASK_BLOCK_BIT, nullptr, ticks) == pdTRUE;
+
+# else
+    return ble_npl_sem_pend(static_cast<ble_npl_sem*>(taskData.m_pHandle), ticks) == BLE_NPL_OK;
 # endif
 } // taskWait
 
@@ -73,10 +106,10 @@ bool NimBLEUtils::taskWait(const NimBLETaskData& taskData, uint32_t timeout) {
  * @param [in] flags A return value to set in the task data structure.
  */
 void NimBLEUtils::taskRelease(const NimBLETaskData& taskData, int flags) {
+    taskData.m_flags = flags;
     if (taskData.m_pHandle != nullptr) {
-        taskData.m_flags = flags;
 # if defined INC_FREERTOS_H
-        xTaskNotifyGive(static_cast<TaskHandle_t>(taskData.m_pHandle));
+        xTaskNotify(static_cast<TaskHandle_t>(taskData.m_pHandle), TASK_BLOCK_BIT, eSetBits);
 # else
         ble_npl_sem_release(static_cast<ble_npl_sem*>(taskData.m_pHandle));
 # endif
diff --git a/src/NimBLEUtils.h b/src/NimBLEUtils.h
index 1c80ba54..4819d397 100644
--- a/src/NimBLEUtils.h
+++ b/src/NimBLEUtils.h
@@ -21,14 +21,8 @@ class NimBLEAddress;
  * All items are optional, the m_pHandle will be set in taskWait().
  */
 struct NimBLETaskData {
-    /**
-     * @brief Constructor.
-     * @param [in] pInstance An instance of the class that is waiting.
-     * @param [in] flags General purpose flags for the caller.
-     * @param [in] buf A buffer for data.
-     */
-    NimBLETaskData(void* pInstance = nullptr, int flags = 0, void* buf = nullptr)
-        : m_pInstance(pInstance), m_flags(flags), m_pBuf(buf) {}
+    NimBLETaskData(void* pInstance = nullptr, int flags = 0, void* buf = nullptr);
+    ~NimBLETaskData();
     void*       m_pInstance{nullptr};
     mutable int m_flags{0};
     void*       m_pBuf{nullptr};