From 4e63dba9c358fe3ed8ee70446d6668921cea8f5b Mon Sep 17 00:00:00 2001 From: Rahul Tank Date: Wed, 8 May 2024 17:00:28 +0530 Subject: [PATCH] fix(nimble) Start advertising if disconnect due to 0x3E in slave --- nimble/host/src/ble_gap.c | 126 +++++++++++++++++++++++++++++++ nimble/host/src/ble_hs_hci_evt.c | 90 ++++++++++++---------- 2 files changed, 178 insertions(+), 38 deletions(-) diff --git a/nimble/host/src/ble_gap.c b/nimble/host/src/ble_gap.c index f9b7373ac..e4e852bf0 100644 --- a/nimble/host/src/ble_gap.c +++ b/nimble/host/src/ble_gap.c @@ -131,6 +131,30 @@ struct ble_gap_connect_reattempt_ctxt { ble_gap_event_fn *cb; void *cb_arg; }ble_conn_reattempt; + +struct ble_gap_adv_reattempt_ctxt { + uint8_t type; + + struct ble_hs_adv_fields fields; + uint8_t own_addr_type; + ble_addr_t direct_addr; + uint8_t direct_addr_present:1 ; + int32_t duration_ms; + struct ble_gap_adv_params adv_params; + +#if MYNEWT_VAL(BLE_EXT_ADV) + uint8_t instance; + struct ble_gap_ext_adv_params params; + int8_t selected_tx_power; + uint8_t selected_tx_power_present:1; + struct os_mbuf *data; + int duration; + int max_events; +#endif + + ble_gap_event_fn *cb; + void *cb_arg; +}ble_adv_reattempt; #endif @@ -1110,6 +1134,58 @@ void ble_gap_reattempt_count(uint16_t conn_handle, uint8_t count) ble_gap_event_listener_call(&event); ble_gap_call_conn_event_cb(&event, handle); } + +int ble_gap_slave_adv_reattempt(void) +{ + int rc = 0; + + switch (ble_adv_reattempt.type) { + case 0: + ble_gap_adv_stop(); + + rc = ble_gap_adv_set_fields(&ble_adv_reattempt.fields); + if (rc != 0) { + return rc; + } + + rc = ble_gap_adv_start(ble_adv_reattempt.own_addr_type, + (ble_adv_reattempt.direct_addr_present == 1 ? &ble_adv_reattempt.direct_addr: NULL), + ble_adv_reattempt.duration_ms, &ble_adv_reattempt.adv_params, + ble_adv_reattempt.cb, ble_adv_reattempt.cb_arg); + if (rc != 0) { + return rc; + } + break; + + case 1: +#if MYNEWT_VAL(BLE_EXT_ADV) + ble_gap_ext_adv_stop(ble_adv_reattempt.instance); + + rc = ble_gap_ext_adv_configure(ble_adv_reattempt.instance, &ble_adv_reattempt.params, + (ble_adv_reattempt.selected_tx_power_present == 1 ? &ble_adv_reattempt.selected_tx_power : NULL), + ble_adv_reattempt.cb, ble_adv_reattempt.cb_arg); + if (rc != 0) { + return rc; + } + rc = ble_gap_ext_adv_set_data(ble_adv_reattempt.instance, ble_adv_reattempt.data); + + if (rc != 0) + return rc; + + rc = ble_gap_ext_adv_start(ble_adv_reattempt.instance, ble_adv_reattempt.duration, + ble_adv_reattempt.max_events); + if (rc != 0) + return rc; +#endif + break; + + default: + break; + } + + return rc; + +} #endif #endif @@ -2936,6 +3012,23 @@ ble_gap_adv_start(uint8_t own_addr_type, const ble_addr_t *direct_addr, return BLE_HS_EDISABLED; } +#if MYNEWT_VAL(BLE_ENABLE_CONN_REATTEMPT) + ble_adv_reattempt.type = 0; + ble_adv_reattempt.own_addr_type = own_addr_type; + + if (direct_addr) { + memcpy(&ble_adv_reattempt.direct_addr, direct_addr, sizeof(ble_addr_t)); + ble_adv_reattempt.direct_addr_present = 1; + } else { + ble_adv_reattempt.direct_addr_present = 0; + } + + ble_adv_reattempt.duration_ms = duration_ms; + memcpy(&ble_adv_reattempt.adv_params , adv_params, sizeof(struct ble_gap_adv_params)); + ble_adv_reattempt.cb = cb; + ble_adv_reattempt.cb_arg = cb_arg; +#endif + ble_hs_lock(); rc = ble_gap_adv_validate(own_addr_type, direct_addr, adv_params); @@ -3076,6 +3169,9 @@ ble_gap_adv_set_fields(const struct ble_hs_adv_fields *adv_fields) if (!ble_hs_is_enabled()) { return BLE_HS_EDISABLED; } +#if MYNEWT_VAL(BLE_ENABLE_CONN_REATTEMPT) + memcpy(&ble_adv_reattempt.fields, adv_fields, sizeof( struct ble_hs_adv_fields)); +#endif rc = ble_hs_adv_set_fields(adv_fields, buf, &buf_sz, sizeof buf); if (rc != 0) { @@ -3308,6 +3404,23 @@ ble_gap_ext_adv_configure(uint8_t instance, return rc; } +#if MYNEWT_VAL(BLE_ENABLE_CONN_REATTEMPT) + ble_adv_reattempt.instance = instance; + + memcpy(&ble_adv_reattempt.params, params, sizeof(struct ble_gap_ext_adv_params)); + + if (selected_tx_power) { + ble_adv_reattempt.selected_tx_power = *selected_tx_power; + ble_adv_reattempt.selected_tx_power_present = 1 ; + } else { + ble_adv_reattempt.selected_tx_power_present = 0 ; + } + + ble_adv_reattempt.cb = cb; + + ble_adv_reattempt.cb_arg = cb_arg; +#endif + ble_hs_lock(); if (ble_gap_adv_active_instance(instance)) { @@ -3425,6 +3538,13 @@ ble_gap_ext_adv_start(uint8_t instance, int duration, int max_events) return BLE_HS_EDISABLED; } +#if MYNEWT_VAL(BLE_ENABLE_CONN_REATTEMPT) + ble_adv_reattempt.type = 1; + ble_adv_reattempt.instance = instance; + ble_adv_reattempt.duration = duration; + ble_adv_reattempt.max_events = max_events; +#endif + ble_hs_lock(); if (!ble_gap_slave[instance].configured) { ble_hs_unlock(); @@ -3711,6 +3831,12 @@ ble_gap_ext_adv_set_data(uint8_t instance, struct os_mbuf *data) goto done; } +#if MYNEWT_VAL(BLE_ENABLE_CONN_REATTEMPT) + ble_adv_reattempt.type = 1; + ble_adv_reattempt.instance = instance; + ble_adv_reattempt.data = data; +#endif + ble_hs_lock(); rc = ble_gap_ext_adv_set_data_validate(instance, data); if (rc != 0) { diff --git a/nimble/host/src/ble_hs_hci_evt.c b/nimble/host/src/ble_hs_hci_evt.c index 2e8115847..9b9386436 100644 --- a/nimble/host/src/ble_hs_hci_evt.c +++ b/nimble/host/src/ble_hs_hci_evt.c @@ -34,6 +34,7 @@ struct ble_gap_reattempt_ctxt { }reattempt_conn; extern int ble_gap_master_connect_reattempt(uint16_t conn_handle); +extern int ble_gap_slave_adv_reattempt(void); #ifdef CONFIG_BT_NIMBLE_MAX_CONN_REATTEMPT #define MAX_REATTEMPT_ALLOWED CONFIG_BT_NIMBLE_MAX_CONN_REATTEMPT @@ -41,13 +42,13 @@ extern int ble_gap_master_connect_reattempt(uint16_t conn_handle); #define MAX_REATTEMPT_ALLOWED 0 #endif #endif - + #if MYNEWT_VAL(BLE_QUEUE_CONG_CHECK) -static struct ble_npl_mutex adv_list_lock; +static struct ble_npl_mutex adv_list_lock; static uint16_t ble_adv_list_count; -#define BLE_ADV_LIST_MAX_LENGTH 50 +#define BLE_ADV_LIST_MAX_LENGTH 50 #define BLE_ADV_LIST_MAX_COUNT 200 -#endif +#endif _Static_assert(sizeof (struct hci_data_hdr) == BLE_HCI_DATA_HDR_SZ, "struct hci_data_hdr must be 4 bytes"); @@ -232,30 +233,43 @@ ble_hs_hci_evt_disconn_complete(uint8_t event_code, const void *data, if (conn && ev->reason == BLE_ERR_CONN_ESTABLISHMENT) { uint16_t handle; int rc; - if (reattempt_conn.count < MAX_REATTEMPT_ALLOWED ) { - /* Got for connection */ - BLE_HS_LOG(INFO, "Reattempt connection; reason = 0x%x, status = %d," - "reattempt count = %d ", ev->reason, ev->status, - reattempt_conn.count); - if (conn->bhc_flags & BLE_HS_CONN_F_MASTER) { - reattempt_conn.count += 1; - handle = le16toh(ev->conn_handle); - /* Post event to interested application */ - ble_gap_reattempt_count(handle, reattempt_conn.count); + if (!(conn->bhc_flags & BLE_HS_CONN_F_MASTER)) { //slave + BLE_HS_LOG(INFO, "Reattempt advertising; reason: 0x%x, status = %x", + ev->reason, ev->status); - rc = ble_gap_master_connect_reattempt(ev->conn_handle); - if (rc != 0) { - BLE_HS_LOG(INFO, "Master reconnect attempt failed; rc = %d", rc); - } + rc = ble_gap_slave_adv_reattempt(); + if (rc != 0) { + BLE_HS_LOG(INFO, "Adv reattempt failed; rc= %d ", rc); + } + + return 0; // Restart advertising, so don't post disconnect event + + } else { // master + if (reattempt_conn.count < MAX_REATTEMPT_ALLOWED ) { + /* Got for connection */ + BLE_HS_LOG(INFO, "Reattempt connection; reason = 0x%x, status = %d," + "reattempt count = %d ", ev->reason, ev->status, + reattempt_conn.count); + reattempt_conn.count += 1; + + handle = le16toh(ev->conn_handle); + /* Post event to interested application */ + ble_gap_reattempt_count(handle, reattempt_conn.count); + + rc = ble_gap_master_connect_reattempt(ev->conn_handle); + if (rc != 0) { + BLE_HS_LOG(INFO, "Master reconnect attempt failed; rc = %d", rc); } - } else { - /* Exhausted attempts */ - memset(&reattempt_conn, 0x0, sizeof (struct ble_gap_reattempt_ctxt)); + } else { + /* Exhausted attempts */ + memset(&reattempt_conn, 0x0, sizeof (struct ble_gap_reattempt_ctxt)); + } } - } else { - /* Normal disconnect. Reset the structure */ - memset(&reattempt_conn, 0x0, sizeof (struct ble_gap_reattempt_ctxt)); + } + else { + /* Normal disconnect. Reset the structure */ + memset(&reattempt_conn, 0x0, sizeof (struct ble_gap_reattempt_ctxt)); } #endif @@ -592,9 +606,9 @@ ble_hs_hci_evt_le_adv_rpt(uint8_t subevent, const void *data, unsigned int len) data += sizeof(*ev); desc.direct_addr = *BLE_ADDR_ANY; - + /* BLE Queue Congestion check*/ -#if MYNEWT_VAL(BLE_QUEUE_CONG_CHECK) +#if MYNEWT_VAL(BLE_QUEUE_CONG_CHECK) if (ble_get_adv_list_length() > BLE_ADV_LIST_MAX_LENGTH || ble_adv_list_count > BLE_ADV_LIST_MAX_COUNT) { ble_adv_list_refresh(); } @@ -603,8 +617,8 @@ ble_hs_hci_evt_le_adv_rpt(uint8_t subevent, const void *data, unsigned int len) for (i = 0; i < ev->num_reports; i++) { - /* Avoiding further processing, if the adv report is from the same device*/ -#if MYNEWT_VAL(BLE_QUEUE_CONG_CHECK) + /* Avoiding further processing, if the adv report is from the same device*/ +#if MYNEWT_VAL(BLE_QUEUE_CONG_CHECK) if (ble_check_adv_list(ev->reports[i].addr, ev->reports[i].addr_type) == true) { continue; } @@ -1207,8 +1221,8 @@ void ble_adv_list_init(void) ble_adv_list_count = 0; } -void ble_adv_list_deinit(void) -{ +void ble_adv_list_deinit(void) +{ struct ble_addr_list_entry *device; struct ble_addr_list_entry *temp; @@ -1226,16 +1240,16 @@ void ble_adv_list_deinit(void) void ble_adv_list_add_packet(void *data) { - struct ble_addr_list_entry *device; + struct ble_addr_list_entry *device; if (!data) { BLE_HS_LOG(ERROR, "%s data is NULL", __func__); return; } - + ble_npl_mutex_pend(&adv_list_lock, BLE_NPL_TIME_FOREVER); - device = (struct ble_addr_list_entry *)data; + device = (struct ble_addr_list_entry *)data; SLIST_INSERT_HEAD(&ble_adv_list, device, next); ble_npl_mutex_release(&adv_list_lock); @@ -1245,7 +1259,7 @@ uint32_t ble_get_adv_list_length(void) { uint32_t length = 0; struct ble_addr_list_entry *device; - + SLIST_FOREACH(device, &ble_adv_list, next) { length++; } @@ -1265,7 +1279,7 @@ void ble_adv_list_refresh(void) } ble_npl_mutex_pend(&adv_list_lock, BLE_NPL_TIME_FOREVER); - + SLIST_FOREACH_SAFE(device, &ble_adv_list, next, temp) { SLIST_REMOVE(&ble_adv_list, device, ble_addr_list_entry, next); free(device); @@ -1277,16 +1291,16 @@ void ble_adv_list_refresh(void) bool ble_check_adv_list(const uint8_t *addr, uint8_t addr_type) { struct ble_addr_list_entry *device; - struct ble_addr_list_entry *adv_packet; + struct ble_addr_list_entry *adv_packet; bool found = false; - + if (!addr) { BLE_HS_LOG(ERROR, "%s addr is NULL", __func__); return found; } ble_npl_mutex_pend(&adv_list_lock, BLE_NPL_TIME_FOREVER); - + SLIST_FOREACH(device, &ble_adv_list, next) { if (!memcmp(addr, device->addr.val, BLE_DEV_ADDR_LEN) && device->addr.type == addr_type) { found = true;