fix(nimble): Fix Client and tasks

Author: lucasssvaz

libraries/BLE/src/BLEAdvertising.cpp

@@ -34,6 +34,7 @@
 #include "BLEAdvertising.h"
 #include <esp_err.h>
 #include "BLEUtils.h"
+#include "BLEDevice.h"
 #include "GeneralUtils.h"
 #include "esp32-hal-log.h"
 

libraries/BLE/src/BLEClient.cpp

@@ -98,6 +98,7 @@ BLEClient::BLEClient() {
   m_connectTimeout = 30000;
   m_pTaskData = nullptr;
   m_lastErr = 0;
+  m_terminateFailCount = 0;
 
   m_pConnParams.scan_itvl = 16;                                             // Scan interval in 0.625ms units (NimBLE Default)
   m_pConnParams.scan_window = 16;                                           // Scan window in 0.625ms units (NimBLE Default)
@@ -107,9 +108,6 @@ BLEClient::BLEClient() {
   m_pConnParams.supervision_timeout = BLE_GAP_INITIAL_SUPERVISION_TIMEOUT;  // timeout = 400*10ms = 4000ms
   m_pConnParams.min_ce_len = BLE_GAP_INITIAL_CONN_MIN_CE_LEN;               // Minimum length of connection event in 0.625ms units
   m_pConnParams.max_ce_len = BLE_GAP_INITIAL_CONN_MAX_CE_LEN;               // Maximum length of connection event in 0.625ms units
-
-  memset(&m_dcTimer, 0, sizeof(m_dcTimer));
-  ble_npl_callout_init(&m_dcTimer, nimble_port_get_dflt_eventq(), BLEClient::dcTimerCb, this);
 #endif
 }  // BLEClient
 
@@ -124,10 +122,6 @@ BLEClient::~BLEClient() {
   }
   m_servicesMap.clear();
   m_servicesMapByInstID.clear();
-
-#if defined(CONFIG_NIMBLE_ENABLED)
-  ble_npl_callout_deinit(&m_dcTimer);
-#endif
 }  // ~BLEClient
 
 /**
@@ -186,34 +180,27 @@ bool BLEClient::secureConnection() {
 #endif
 
 #if defined(CONFIG_NIMBLE_ENABLED)
-  TaskHandle_t cur_task = xTaskGetCurrentTaskHandle();
-  ble_task_data_t taskData = {this, cur_task, 0, nullptr};
-
   int retryCount = 1;
+  BLETaskData taskData(const_cast<BLEClient*>(this), BLE_HS_ENOTCONN);
+  m_pTaskData = &taskData;
 
   do {
-    m_pTaskData = &taskData;
-
-    int rc = BLESecurity::startSecurity(m_conn_id);
-    if (rc != 0) {
-      m_lastErr = rc;
-      m_pTaskData = nullptr;
-      return false;
+    if (BLESecurity::startSecurity(m_conn_id)) {
+      BLEUtils::taskWait(taskData, BLE_NPL_TIME_FOREVER);
     }
 
-#ifdef ulTaskNotifyValueClear
-    // Clear the task notification value to ensure we block
-    ulTaskNotifyValueClear(cur_task, ULONG_MAX);
-#endif
-    ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
-  } while (taskData.rc == (BLE_HS_ERR_HCI_BASE + BLE_ERR_PINKEY_MISSING) && retryCount--);
+  } while (taskData.m_flags == (BLE_HS_ERR_HCI_BASE + BLE_ERR_PINKEY_MISSING) && retryCount--);
 
-  if (taskData.rc != 0) {
-    m_lastErr = taskData.rc;
-    return false;
+  m_pTaskData = nullptr;
+
+  if (taskData.m_flags == 0) {
+    log_d("<< secureConnection: success");
+    return true;
   }
 
-  return true;
+  m_lastErr = taskData.m_flags;
+  log_e("secureConnection: failed rc=%d", taskData.m_flags);
+  return false;
 #endif
 }  // secureConnection
 
@@ -376,7 +363,7 @@ bool BLEClient::setMTU(uint16_t mtu) {
 #endif
 
 #if defined(CONFIG_NIMBLE_ENABLED)
-    err = ble_gattc_exchange_mtu(m_conn_id, nullptr, nullptr);
+    //err = ble_gattc_exchange_mtu(m_conn_id, nullptr, nullptr);
 #endif
 
     if (err != ESP_OK) {
@@ -799,9 +786,6 @@ bool BLEClient::connect(BLEAddress address, uint8_t type, uint32_t timeoutMs) {
   m_appId = BLEDevice::m_appId++;
   m_peerAddress = address;
 
-  TaskHandle_t cur_task = xTaskGetCurrentTaskHandle();
-  ble_task_data_t taskData = {this, cur_task, 0, nullptr};
-  m_pTaskData = &taskData;
   int rc = 0;
 
   /* Try to connect the the advertiser.  Allow 30 seconds (30000 ms) for
@@ -837,45 +821,34 @@ bool BLEClient::connect(BLEAddress address, uint8_t type, uint32_t timeoutMs) {
 
   } while (rc == BLE_HS_EBUSY);
 
-  m_lastErr = rc;
-
   if (rc != 0) {
-    m_pTaskData = nullptr;
+    m_lastErr = rc;
     return false;
   }
 
-#ifdef ulTaskNotifyValueClear
-  // Clear the task notification value to ensure we block
-  ulTaskNotifyValueClear(cur_task, ULONG_MAX);
-#endif
+  BLETaskData taskData(this);
+  m_pTaskData = &taskData;
 
   // Wait for the connect timeout time +1 second for the connection to complete
-  if (ulTaskNotifyTake(pdTRUE, pdMS_TO_TICKS(m_connectTimeout + 1000)) == pdFALSE) {
-    m_pTaskData = nullptr;
-    // If a connection was made but no response from MTU exchange; disconnect
+  if (!BLEUtils::taskWait(taskData, m_connectTimeout + 1000)) {
+    // If a connection was made but no response from MTU exchange proceed anyway
     if (isConnected()) {
-      log_e("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.
-      log_e("Connect timeout - canceling");
+      // workaround; if the controller doesn't cancel the connection at the timeout, cancel it here.
+      log_e("Connect timeout - cancelling");
       ble_gap_conn_cancel();
+      taskData.m_flags = BLE_HS_ETIMEOUT;
     }
+  }
 
-    return false;
+  m_pTaskData = nullptr;
+  rc = taskData.m_flags;
 
-  } else if (taskData.rc != 0) {
-    m_lastErr = taskData.rc;
-    log_e("Connection failed; status=%d %s", taskData.rc, BLEUtils::returnCodeToString(taskData.rc));
-    // If the failure was not a result of a disconnection
-    // make sure we disconnect now to avoid dangling connections
-    if (isConnected()) {
-      disconnect();
-    }
+  if (rc != 0) {
+    log_e("Connection failed; status=%d %s", rc, BLEUtils::returnCodeToString(rc));
+    m_lastErr = rc;
     return false;
-  } else {
-    log_i("Connection established");
   }
 
   m_isConnected = true;
@@ -895,15 +868,16 @@ bool BLEClient::connect(BLEAddress address, uint8_t type, uint32_t timeoutMs) {
  */
 int BLEClient::serviceDiscoveredCB(uint16_t conn_handle, const struct ble_gatt_error *error, const struct ble_gatt_svc *service, void *arg)
 {
-  if (error == nullptr || service == nullptr) {
-    log_e("serviceDiscoveredCB: error or service is nullptr");
-    return 0;
-  }
-
   log_d("Service Discovered >> status: %d handle: %d", error->status, (error->status == 0) ? service->start_handle : -1);
 
-  ble_task_data_t *pTaskData = (ble_task_data_t*)arg;
-  BLEClient *client = (BLEClient*)pTaskData->pATT;
+  BLETaskData *pTaskData = (BLETaskData*)arg;
+  BLEClient *client = (BLEClient*)pTaskData->m_pInstance;
+
+  if (error->status == BLE_HS_ENOTCONN) {
+    log_e("<< Service Discovered; Disconnected");
+    BLEUtils::taskRelease(*pTaskData, error->status);
+    return error->status;
+  }
 
   // Make sure the service discovery is for this device
   if(client->getConnId() != conn_handle){
@@ -918,53 +892,50 @@ int BLEClient::serviceDiscoveredCB(uint16_t conn_handle, const struct ble_gatt_e
     return 0;
   }
 
-  if(error->status == BLE_HS_EDONE) {
-    pTaskData->rc = 0;
-  } else {
-    log_e("serviceDiscoveredCB() rc=%d %s", error->status, BLEUtils::returnCodeToString(error->status));
-    pTaskData->rc = error->status;
-  }
-
-  xTaskNotifyGive(pTaskData->task);
-
+  BLEUtils::taskRelease(*pTaskData, error->status);
   log_d("<< Service Discovered");
   return error->status;
 }
 
 std::map<std::string, BLERemoteService *> *BLEClient::getServices() {
-  /*
- * Design
- * ------
- * We invoke esp_ble_gattc_search_service.  This will request a list of the service exposed by the
- * peer BLE partner to be returned as events.  Each event will be an an instance of ESP_GATTC_SEARCH_RES_EVT
- * and will culminate with an ESP_GATTC_SEARCH_CMPL_EVT when all have been received.
- */
   log_v(">> getServices");
-  // TODO implement retrieving services from cache
-  //m_semaphoreSearchCmplEvt.take("getServices");
+
   clearServices();  // Clear any services that may exist.
 
+  if (!isConnected()) {
+    log_e("Disconnected, could not retrieve services -aborting");
+    return &m_servicesMap;
+  }
+
   int errRc = 0;
-  TaskHandle_t cur_task = xTaskGetCurrentTaskHandle();
-  ble_task_data_t taskData = {this, cur_task, 0, nullptr};
+  BLETaskData taskData(this);
 
   errRc = ble_gattc_disc_all_svcs(m_conn_id, BLEClient::serviceDiscoveredCB, &taskData);
+
   if (errRc != 0) {
     log_e("ble_gattc_disc_all_svcs: rc=%d %s", errRc, BLEUtils::returnCodeToString(errRc));
     m_lastErr = errRc;
-    //m_semaphoreSearchCmplEvt.give();
     return &m_servicesMap;
   }
-  // If successful, remember that we now have services.
-  m_haveServices = m_servicesMap.size() > 0;
-  //m_semaphoreSearchCmplEvt.give();
-  log_v("<< getServices");
+
+  BLEUtils::taskWait(taskData, BLE_NPL_TIME_FOREVER);
+  errRc = taskData.m_flags;
+  if (errRc == 0 || errRc == BLE_HS_EDONE) {
+    // If successful, remember that we now have services.
+    m_haveServices = m_servicesMap.size() > 0;
+    log_v("<< getServices");
+    return &m_servicesMap;
+  }
+
+  m_lastErr = errRc;
+  log_e("Could not retrieve services, rc=%d %s", errRc, BLEUtils::returnCodeToString(errRc));
   return &m_servicesMap;
 }  // getServices
 
 int BLEClient::handleGAPEvent(struct ble_gap_event *event, void *arg) {
   BLEClient *client = (BLEClient *)arg;
-  int rc;
+  int rc = 0;
+  BLETaskData *pTaskData = client->m_pTaskData;
 
   log_d("BLEClient", "Got Client event %s", BLEUtils::gapEventToString(event->type));
 
@@ -979,7 +950,7 @@ int BLEClient::handleGAPEvent(struct ble_gap_event *event, void *arg) {
         case BLE_HS_ETIMEOUT_HCI:
         case BLE_HS_ENOTSYNCED:
         case BLE_HS_EOS:
-          log_d("BLEClient", "Disconnect - host reset, rc=%d", rc);
+          log_e("BLEClient", "Disconnect - host reset, rc=%d", rc);
           BLEDevice::onReset(rc);
           break;
         default:
@@ -990,14 +961,12 @@ int BLEClient::handleGAPEvent(struct ble_gap_event *event, void *arg) {
           break;
       }
 
-      // Stop the disconnect timer since we are now disconnected.
-      ble_npl_callout_stop(&client->m_dcTimer);
-
       // Remove the device from ignore list so we will scan it again
       // BLEDevice::removeIgnored(client->m_peerAddress);
 
       // No longer connected, clear the connection ID.
       client->m_conn_id = BLE_HS_CONN_HANDLE_NONE;
+      client->m_terminateFailCount = 0;
 
       // If we received a connected event but did not get established (no PDU)
       // then a disconnect event will be sent but we should not send it to the
@@ -1028,11 +997,11 @@ int BLEClient::handleGAPEvent(struct ble_gap_event *event, void *arg) {
 
       rc = event->connect.status;
       if (rc == 0) {
-        log_i("BLEClient", "Connected event");
+        log_i("BLEClient: Connected event. Handle: %d", event->connect.conn_handle);
 
         client->m_conn_id = event->connect.conn_handle;
 
-        rc = ble_gattc_exchange_mtu(client->m_conn_id, NULL, NULL);
+        rc = ble_gattc_exchange_mtu(client->m_conn_id, nullptr, nullptr);
         if (rc != 0) {
           log_e("BLEClient", "MTU exchange error; rc=%d %s", rc, BLEUtils::returnCodeToString(rc));
           break;
@@ -1049,6 +1018,20 @@ int BLEClient::handleGAPEvent(struct ble_gap_event *event, void *arg) {
       return 0;
     }  // BLE_GAP_EVENT_CONNECT
 
+    case BLE_GAP_EVENT_TERM_FAILURE: {
+      if (client->m_conn_id != event->term_failure.conn_handle) {
+        return 0;
+      }
+
+      log_e("Connection termination failure; rc=%d - retrying", event->term_failure.status);
+      if (++client->m_terminateFailCount > 2) {
+        ble_hs_sched_reset(BLE_HS_ECONTROLLER);
+      } else {
+        ble_gap_terminate(event->term_failure.conn_handle, BLE_ERR_REM_USER_CONN_TERM);
+      }
+      return 0;
+    } // BLE_GAP_EVENT_TERM_FAILURE
+
     case BLE_GAP_EVENT_NOTIFY_RX:
     {
       if (client->m_conn_id != event->notify_rx.conn_handle) {
@@ -1236,12 +1219,8 @@ int BLEClient::handleGAPEvent(struct ble_gap_event *event, void *arg) {
     }
   }  // Switch
 
-  if (client->m_pTaskData != nullptr) {
-    client->m_pTaskData->rc = rc;
-    if (client->m_pTaskData->task) {
-      xTaskNotifyGive(client->m_pTaskData->task);
-    }
-    client->m_pTaskData = nullptr;
+  if (pTaskData != nullptr) {
+    BLEUtils::taskRelease(*pTaskData, rc);
   }
 
   return 0;
@@ -1256,41 +1235,16 @@ int BLEClient::disconnect(uint8_t reason) {
   int rc = 0;
 
   if(isConnected()) {
-    // If the timer was already started, ignore this call.
-    if(ble_npl_callout_is_active(&m_dcTimer)) {
-      log_i("Already disconnecting, timer started");
-      return BLE_HS_EALREADY;
-    }
-
-    ble_gap_conn_desc desc;
-    if(ble_gap_conn_find(m_conn_id, &desc) != 0){
-      log_i("Connection ID not found");
-      return BLE_HS_EALREADY;
-    }
-
-    // We use a timer to detect a controller error in the event that it does
-    // not inform the stack when disconnection is complete.
-    // This is a common error in certain esp-idf versions.
-    // The disconnect timeout time is the supervison timeout time + 1 second.
-    // In the case that the event happenss shortly after the supervision timeout
-    // we don't want to prematurely reset the host.
-    ble_npl_time_t ticks;
-    ble_npl_time_ms_to_ticks((desc.supervision_timeout + 100) * 10, &ticks);
-    ble_npl_callout_reset(&m_dcTimer, ticks);
-
     rc = ble_gap_terminate(m_conn_id, reason);
-    if (rc != 0) {
-      if(rc != BLE_HS_EALREADY) {
-        ble_npl_callout_stop(&m_dcTimer);
-      }
+    if (rc != 0 && rc != BLE_HS_ENOTCONN && rc != BLE_HS_EALREADY) {
+      m_lastErr = rc;
       log_e("ble_gap_terminate failed: rc=%d %s", rc, BLEUtils::returnCodeToString(rc));
     }
   } else {
     log_d("Not connected to any peers");
   }
 
   log_d("<< disconnect()");
-  m_lastErr = rc;
   return rc;
 } // disconnect