From 28717c300a4d355b64dd6b98a6384e987b472fcd Mon Sep 17 00:00:00 2001 From: h2zero Date: Tue, 12 Jan 2021 13:50:08 -0700 Subject: [PATCH] Fix client connect return code handling, add disconnect timer. * Handle the return codes from ble_gap_connect to take proper actions for different codes. * Improve client event handling to accomodate delayed PDU responses. * Use connection ID as a replacement for the isConnected flag. Also check if a task is waiting for results instead of the waitingToConnect flag. * Adds a disconnect timer so that calling disconnect will start a timer for the connection supervision timeout time + a small delay. When this expires it will reset the host if a disconnect event does not occur in this time. This is added as a workaround for the occasional situation when the controller does not send an event after the disconnect command. --- src/NimBLEClient.cpp | 176 +++++++++++++++++++++++++++---------------- src/NimBLEClient.h | 6 +- src/NimBLEDevice.cpp | 18 ++--- 3 files changed, 123 insertions(+), 77 deletions(-) diff --git a/src/NimBLEClient.cpp b/src/NimBLEClient.cpp index 04520a4..2a545e8 100644 --- a/src/NimBLEClient.cpp +++ b/src/NimBLEClient.cpp @@ -24,6 +24,9 @@ #include #include +#include "nimble/nimble_port.h" + + static const char* LOG_TAG = "NimBLEClient"; static NimBLEClientCallbacks defaultCallbacks; @@ -56,8 +59,6 @@ static NimBLEClientCallbacks defaultCallbacks; NimBLEClient::NimBLEClient(const NimBLEAddress &peerAddress) : m_peerAddress(peerAddress) { m_pClientCallbacks = &defaultCallbacks; m_conn_id = BLE_HS_CONN_HANDLE_NONE; - m_isConnected = false; - m_waitingToConnect = false; m_connectTimeout = 30000; m_deleteCallbacks = false; m_pTaskData = nullptr; @@ -70,6 +71,9 @@ NimBLEClient::NimBLEClient(const NimBLEAddress &peerAddress) : m_peerAddress(pee m_pConnParams.supervision_timeout = BLE_GAP_INITIAL_SUPERVISION_TIMEOUT; // timeout = 400*10ms = 4000ms m_pConnParams.min_ce_len = BLE_GAP_INITIAL_CONN_MIN_CE_LEN; // Minimum length of connection event in 0.625ms units m_pConnParams.max_ce_len = BLE_GAP_INITIAL_CONN_MAX_CE_LEN; // Maximum length of connection event in 0.625ms units + + ble_npl_callout_init(&m_dcTimer, nimble_port_get_dflt_eventq(), + NimBLEClient::dcTimerCb, this); } // NimBLEClient @@ -89,6 +93,19 @@ NimBLEClient::~NimBLEClient() { } // ~NimBLEClient +/** + * @brief If we have asked to disconnect and the event does not + * occur within the supervision timeout + added delay, this will + * be called to reset the host in the case of a stalled controller. + */ +void NimBLEClient::dcTimerCb(ble_npl_event *event) { + NimBLEClient *pClient = (NimBLEClient*)event->arg; + NIMBLE_LOGC(LOG_TAG, "Timed out disconnecting from %s - resetting host", + std::string(pClient->getPeerAddress()).c_str()); + ble_hs_sched_reset(BLE_HS_ECONTROLLER); +} + + /** * @brief Delete all service objects created by this client and clear the vector. */ @@ -164,12 +181,9 @@ bool NimBLEClient::connect(const NimBLEAddress &address, bool deleteAttibutes) { return false; } - if(ble_gap_conn_active()) { - NIMBLE_LOGE(LOG_TAG, "Connection in progress - must wait."); - return false; - } - - if(!NimBLEDevice::getScan()->stop()) { + if(isConnected() || m_pTaskData != nullptr) { + NIMBLE_LOGE(LOG_TAG, "Client busy, connected to %s, id=%d", + std::string(m_peerAddress).c_str(), getConnId()); return false; } @@ -180,42 +194,54 @@ bool NimBLEClient::connect(const NimBLEAddress &address, bool deleteAttibutes) { m_peerAddress = address; } - ble_addr_t peerAddrt; - memcpy(&peerAddrt.val, m_peerAddress.getNative(),6); - peerAddrt.type = m_peerAddress.getType(); + ble_addr_t peerAddr_t; + memcpy(&peerAddr_t.val, m_peerAddress.getNative(),6); + peerAddr_t.type = m_peerAddress.getType(); + ble_task_data_t taskData = {this, xTaskGetCurrentTaskHandle(), 0, nullptr}; - m_pTaskData = &taskData; - int rc = 0; /* Try to connect the the advertiser. Allow 30 seconds (30000 ms) for * timeout (default value of m_connectTimeout). * Loop on BLE_HS_EBUSY if the scan hasn't stopped yet. */ - do{ - rc = ble_gap_connect(BLE_OWN_ADDR_PUBLIC, &peerAddrt, m_connectTimeout, &m_pConnParams, - NimBLEClient::handleGapEvent, this); - if(rc == BLE_HS_EBUSY) { - vTaskDelay(1 / portTICK_PERIOD_MS); + do { + rc = ble_gap_connect(BLE_OWN_ADDR_PUBLIC, &peerAddr_t, + m_connectTimeout, &m_pConnParams, + NimBLEClient::handleGapEvent, this); + switch (rc) { + case 0: + m_pTaskData = &taskData; + break; + + case BLE_HS_EBUSY: + if (!NimBLEDevice::getScan()->stop()) { + return false; + } + vTaskDelay(1 / portTICK_PERIOD_MS); + break; + + case BLE_HS_EDONE: + NIMBLE_LOGE(LOG_TAG, "Already connected to device; addr=%s", + std::string(m_peerAddress).c_str()); + return false; + + case BLE_HS_EALREADY: + ble_gap_conn_cancel(); + return false; + + default: + NIMBLE_LOGE(LOG_TAG, "Failed to connect to %s, rc=%d; %s", + std::string(m_peerAddress).c_str(), + rc, NimBLEUtils::returnCodeToString(rc)); + return false; } - }while(rc == BLE_HS_EBUSY); - if (rc != 0 && rc != BLE_HS_EDONE) { - NIMBLE_LOGE(LOG_TAG, "Error: Failed to connect to device; " - "addr=%s, rc=%d; %s", - std::string(m_peerAddress).c_str(), - rc, NimBLEUtils::returnCodeToString(rc)); - m_pTaskData = nullptr; - m_waitingToConnect = false; - return false; - } - - m_waitingToConnect = true; + } while (rc == BLE_HS_EBUSY); // Wait for the connection to complete. ulTaskNotifyTake(pdTRUE, portMAX_DELAY); - if(taskData.rc != 0){ return false; } @@ -267,13 +293,31 @@ bool NimBLEClient::secureConnection() { */ int NimBLEClient::disconnect(uint8_t reason) { NIMBLE_LOGD(LOG_TAG, ">> disconnect()"); + int rc = 0; - if(m_isConnected){ + if(isConnected()){ rc = ble_gap_terminate(m_conn_id, reason); - if(rc != 0){ - NIMBLE_LOGE(LOG_TAG, "ble_gap_terminate failed: rc=%d %s", rc, - NimBLEUtils::returnCodeToString(rc)); + if (rc == 0) { + ble_addr_t peerAddr_t; + memcpy(&peerAddr_t.val, m_peerAddress.getNative(),6); + peerAddr_t.type = m_peerAddress.getType(); + + // Set the disconnect timeout to the supervison timeout time + 1 second + // In case the event triggers shortly after the supervision timeout. + // We don't want to prematurely reset the host. + ble_gap_conn_desc desc; + if(ble_gap_conn_find_by_addr(&peerAddr_t, &desc) == 0){ + ble_npl_time_t ticks; + ble_npl_time_ms_to_ticks((desc.supervision_timeout + 100) * 10, &ticks); + ble_npl_callout_reset(&m_dcTimer, ticks); + NIMBLE_LOGD(LOG_TAG, "DC TIMEOUT = %dms", (desc.supervision_timeout + 100) * 10); + } + } else if (rc != BLE_HS_EALREADY) { + NIMBLE_LOGE(LOG_TAG, "ble_gap_terminate failed: rc=%d %s", + rc, NimBLEUtils::returnCodeToString(rc)); } + } else { + NIMBLE_LOGD(LOG_TAG, "Not connected to any peers"); } NIMBLE_LOGD(LOG_TAG, "<< disconnect()"); @@ -520,7 +564,7 @@ bool NimBLEClient::retrieveServices(const NimBLEUUID *uuid_filter) { NIMBLE_LOGD(LOG_TAG, ">> retrieveServices"); - if(!m_isConnected){ + if(!isConnected()){ NIMBLE_LOGE(LOG_TAG, "Disconnected, could not retrieve services -aborting"); return false; } @@ -700,19 +744,24 @@ uint16_t NimBLEClient::getMTU() { switch(event->type) { case BLE_GAP_EVENT_DISCONNECT: { - if(!client->m_isConnected) + // If the connection id is invalid ignore this event + if(event->disconnect.conn.conn_handle == BLE_HS_CONN_HANDLE_NONE) return 0; + // Check that the event is for this connection. if(client->m_conn_id != event->disconnect.conn.conn_handle) return 0; - client->m_isConnected = false; - client->m_waitingToConnect=false; + rc = event->disconnect.reason; + + // Stop the disconnect timer since we are now disconnected. + ble_npl_callout_stop(&client->m_dcTimer); + // Remove the device from ignore list so we will scan it again NimBLEDevice::removeIgnored(client->m_peerAddress); - NIMBLE_LOGI(LOG_TAG, "disconnect; reason=%d, %s", event->disconnect.reason, - NimBLEUtils::returnCodeToString(event->disconnect.reason)); + NIMBLE_LOGI(LOG_TAG, "disconnect; reason=%d, %s", + rc, NimBLEUtils::returnCodeToString(rc)); // If Host reset tell the device now before returning to prevent // any errors caused by calling host functions before resyncing. @@ -721,40 +770,37 @@ uint16_t NimBLEClient::getMTU() { case BLE_HS_EOS: case BLE_HS_ECONTROLLER: case BLE_HS_ENOTSYNCED: - NIMBLE_LOGC(LOG_TAG, "Disconnect - host reset, rc=%d", event->disconnect.reason); - NimBLEDevice::onReset(event->disconnect.reason); + NIMBLE_LOGC(LOG_TAG, "Disconnect - host reset, rc=%d", rc); + NimBLEDevice::onReset(rc); break; default: break; } - //client->m_conn_id = BLE_HS_CONN_HANDLE_NONE; + client->m_conn_id = BLE_HS_CONN_HANDLE_NONE; client->m_pClientCallbacks->onDisconnect(client); - rc = event->disconnect.reason; + break; } // BLE_GAP_EVENT_DISCONNECT case BLE_GAP_EVENT_CONNECT: { - - if(!client->m_waitingToConnect) + // If we aren't waiting for this connection response, this is a delayed packet + // We should drop the connection immediately. + if(client->isConnected() || client->m_pTaskData == nullptr) { + ble_gap_terminate(event->connect.conn_handle, BLE_ERR_REM_USER_CONN_TERM); return 0; + } - //if(client->m_conn_id != BLE_HS_CONN_HANDLE_NONE) - // return 0; - - client->m_waitingToConnect=false; - - if (event->connect.status == 0) { - client->m_isConnected = true; - + rc = event->connect.status; + if (rc == 0) { NIMBLE_LOGD(LOG_TAG, "Connection established"); client->m_conn_id = event->connect.conn_handle; rc = ble_gattc_exchange_mtu(client->m_conn_id, NULL,NULL); if(rc != 0) { - NIMBLE_LOGE(LOG_TAG, "ble_gattc_exchange_mtu: rc=%d %s",rc, - NimBLEUtils::returnCodeToString(rc)); + NIMBLE_LOGE(LOG_TAG, "ble_gattc_exchange_mtu: rc=%d %s", + rc, NimBLEUtils::returnCodeToString(rc)); break; } @@ -762,14 +808,13 @@ uint16_t NimBLEClient::getMTU() { // scanning since we are already connected to it NimBLEDevice::addIgnored(client->m_peerAddress); } else { - NIMBLE_LOGE(LOG_TAG, "Error: Connection failed; status=%d %s", - event->connect.status, - NimBLEUtils::returnCodeToString(event->connect.status)); + NIMBLE_LOGE(LOG_TAG, "Connection failed; status=%d %s", + rc, NimBLEUtils::returnCodeToString(rc)); - client->m_isConnected = false; - rc = event->connect.status; + client->m_conn_id = BLE_HS_CONN_HANDLE_NONE; break; } + return 0; } // BLE_GAP_EVENT_CONNECT @@ -863,7 +908,9 @@ uint16_t NimBLEClient::getMTU() { return 0; } - if(event->enc_change.status == 0 || event->enc_change.status == (BLE_HS_ERR_HCI_BASE + BLE_ERR_PINKEY_MISSING)) { + if(event->enc_change.status == 0 || + event->enc_change.status == (BLE_HS_ERR_HCI_BASE + BLE_ERR_PINKEY_MISSING)) + { struct ble_gap_conn_desc desc; rc = ble_gap_conn_find(event->enc_change.conn_handle, &desc); assert(rc == 0); @@ -957,6 +1004,7 @@ uint16_t NimBLEClient::getMTU() { } // Switch if(client->m_pTaskData != nullptr) { + //NIMBLE_LOGC(LOG_TAG, "TASK GIVE rc = %d %s", rc, NimBLEUtils::gapEventToString(event->type)); client->m_pTaskData->rc = rc; xTaskNotifyGive(client->m_pTaskData->task); client->m_pTaskData = nullptr; @@ -971,7 +1019,7 @@ uint16_t NimBLEClient::getMTU() { * @return True if we are connected and false if we are not connected. */ bool NimBLEClient::isConnected() { - return m_isConnected; + return m_conn_id != BLE_HS_CONN_HANDLE_NONE; } // isConnected diff --git a/src/NimBLEClient.h b/src/NimBLEClient.h index f13490f..098bcc1 100644 --- a/src/NimBLEClient.h +++ b/src/NimBLEClient.h @@ -84,16 +84,16 @@ private: const struct ble_gatt_error *error, const struct ble_gatt_svc *service, void *arg); + static void dcTimerCb(ble_npl_event *event); bool retrieveServices(const NimBLEUUID *uuid_filter = nullptr); NimBLEAddress m_peerAddress; uint16_t m_conn_id; - bool m_isConnected; - bool m_waitingToConnect; bool m_deleteCallbacks; int32_t m_connectTimeout; NimBLEClientCallbacks* m_pClientCallbacks; - ble_task_data_t *m_pTaskData; + ble_task_data_t* m_pTaskData; + ble_npl_callout m_dcTimer; std::vector m_servicesVector; diff --git a/src/NimBLEDevice.cpp b/src/NimBLEDevice.cpp index fb36e6e..833104c 100644 --- a/src/NimBLEDevice.cpp +++ b/src/NimBLEDevice.cpp @@ -144,8 +144,8 @@ void NimBLEDevice::stopAdvertising() { #if defined(CONFIG_BT_NIMBLE_ROLE_CENTRAL) /* STATIC */ NimBLEClient* NimBLEDevice::createClient(NimBLEAddress peerAddress) { if(m_cList.size() >= NIMBLE_MAX_CONNECTIONS) { - NIMBLE_LOGW("Number of clients exceeds Max connections. Max=(%d)", - NIMBLE_MAX_CONNECTIONS); + NIMBLE_LOGW(LOG_TAG,"Number of clients exceeds Max connections. Cur=%d Max=%d", + m_cList.size(), NIMBLE_MAX_CONNECTIONS); } NimBLEClient* pClient = new NimBLEClient(peerAddress); @@ -167,24 +167,22 @@ void NimBLEDevice::stopAdvertising() { int rc =0; - if(pClient->m_isConnected) { + if(pClient->isConnected()) { rc = pClient->disconnect(); if (rc != 0 && rc != BLE_HS_EALREADY && rc != BLE_HS_ENOTCONN) { return false; } - while(pClient->m_isConnected) { - vTaskDelay(10); + while(pClient->isConnected()) { + vTaskDelay(10 / portTICK_PERIOD_MS); } - } - - if(pClient->m_waitingToConnect) { + } else if(pClient->m_pTaskData != nullptr) { rc = ble_gap_conn_cancel(); if (rc != 0 && rc != BLE_HS_EALREADY) { return false; } - while(pClient->m_waitingToConnect) { - vTaskDelay(10); + while(pClient->m_pTaskData != nullptr) { + vTaskDelay(10 / portTICK_PERIOD_MS); } }