From 56917b28aa8e9249bd09b263e35ec7b62d7cde32 Mon Sep 17 00:00:00 2001 From: Darshan Dobariya Date: Fri, 5 Jan 2024 10:50:08 +0530 Subject: [PATCH] fix(nimble): Added support for address resolution during inquiry. --- nimble/host/src/ble_gap.c | 22 ++++++++++++++++++++-- nimble/host/src/ble_hs_hci_evt.c | 11 +++++++++++ nimble/host/src/ble_hs_resolv.c | 22 ++++++++++++++++++++-- nimble/host/src/ble_hs_resolv_priv.h | 4 ++++ 4 files changed, 55 insertions(+), 4 deletions(-) diff --git a/nimble/host/src/ble_gap.c b/nimble/host/src/ble_gap.c index b3fd1851d..ca2db9906 100644 --- a/nimble/host/src/ble_gap.c +++ b/nimble/host/src/ble_gap.c @@ -5750,6 +5750,7 @@ ble_gap_connect(uint8_t own_addr_type, const ble_addr_t *peer_addr, conn_params, NULL, NULL, cb, cb_arg); #else uint32_t duration_ticks; + ble_addr_t bhc_peer_addr; int rc; STATS_INC(ble_gap_stats, initiate); @@ -5846,6 +5847,19 @@ ble_gap_connect(uint8_t own_addr_type, const ble_addr_t *peer_addr, ble_gap_master.op = BLE_GAP_OP_M_CONN; + bhc_peer_addr.type = peer_addr->type; + memcpy(bhc_peer_addr.val, peer_addr->val, BLE_DEV_ADDR_LEN); + +#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) + struct ble_hs_resolv_entry *rl = NULL; + rl = ble_hs_resolv_list_find(bhc_peer_addr.val); + + if (rl != NULL) { + memcpy(bhc_peer_addr.val, rl->rl_peer_rpa, BLE_DEV_ADDR_LEN); + bhc_peer_addr.type = rl->rl_addr_type; + } +#endif + #if MYNEWT_VAL(BLE_ENABLE_CONN_REATTEMPT) /* ble_gap_connect_reattempt save the connection parameters */ if ((cb_arg != NULL) && conn_cookie_enabled) { @@ -5864,7 +5878,7 @@ ble_gap_connect(uint8_t own_addr_type, const ble_addr_t *peer_addr, } ble_conn_reattempt[reattempt_idx].own_addr_type = own_addr_type; - memcpy(&ble_conn_reattempt[reattempt_idx].peer_addr, peer_addr, + memcpy(&ble_conn_reattempt[reattempt_idx].peer_addr, &bhc_peer_addr, sizeof(ble_addr_t)); ble_conn_reattempt[reattempt_idx].duration_ms = duration_ms; memcpy(&ble_conn_reattempt[reattempt_idx].conn_params, @@ -5879,7 +5893,7 @@ ble_gap_connect(uint8_t own_addr_type, const ble_addr_t *peer_addr, reattempt_idx = (reattempt_idx + 1) % MYNEWT_VAL(BLE_MAX_CONNECTIONS); #endif - rc = ble_gap_conn_create_tx(own_addr_type, peer_addr, + rc = ble_gap_conn_create_tx(own_addr_type, &bhc_peer_addr, conn_params); if (rc != 0) { ble_gap_master_reset_state(); @@ -6707,6 +6721,10 @@ ble_gap_unpair(const ble_addr_t *peer_addr) ble_hs_unlock(); +#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) + ble_hs_pvcy_remove_entry(peer_addr->type, peer_addr->val); +#endif + memset(&key,0,sizeof(key)); key.rpa_rec.peer_rpa_addr = *peer_addr; diff --git a/nimble/host/src/ble_hs_hci_evt.c b/nimble/host/src/ble_hs_hci_evt.c index 909353260..b9088dc55 100644 --- a/nimble/host/src/ble_hs_hci_evt.c +++ b/nimble/host/src/ble_hs_hci_evt.c @@ -632,6 +632,17 @@ ble_hs_hci_evt_le_adv_rpt(uint8_t subevent, const void *data, unsigned int len) desc.event_type = rpt->type; desc.addr.type = rpt->addr_type; memcpy(desc.addr.val, rpt->addr, BLE_DEV_ADDR_LEN); + +#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) + struct ble_hs_resolv_entry *rl = NULL; + rl = ble_hs_resolv_rpa_addr(desc.addr.val, desc.addr.type); + + if (rl != NULL) { + memcpy(desc.addr.val, rl->rl_identity_addr, BLE_DEV_ADDR_LEN); + desc.addr.type = rl->rl_addr_type; + } +#endif + desc.length_data = rpt->data_len; desc.data = rpt->data; desc.rssi = rpt->data[rpt->data_len]; diff --git a/nimble/host/src/ble_hs_resolv.c b/nimble/host/src/ble_hs_resolv.c index c937c8793..f4b874619 100644 --- a/nimble/host/src/ble_hs_resolv.c +++ b/nimble/host/src/ble_hs_resolv.c @@ -500,8 +500,7 @@ ble_hs_is_on_resolv_list(uint8_t *addr, uint8_t addr_type) struct ble_hs_resolv_entry *rl = &g_ble_hs_resolv_list[1]; for (i = 1; i < g_ble_hs_resolv_data.rl_cnt; ++i) { - if ((rl->rl_addr_type == addr_type) && - (!memcmp(rl->rl_identity_addr, addr, BLE_DEV_ADDR_LEN))) { + if ((!memcmp(rl->rl_identity_addr, addr, BLE_DEV_ADDR_LEN)) || (!memcmp(rl->rl_peer_rpa, addr, BLE_DEV_ADDR_LEN))) { return i; } ++rl; @@ -723,6 +722,25 @@ ble_hs_resolv_set_rpa_tmo(uint16_t tmo_secs) return 0; } +struct ble_hs_resolv_entry * +ble_hs_resolv_rpa_addr(uint8_t *addr, uint8_t addr_type) { +#if MYNEWT_VAL(BLE_STORE_MAX_BONDS) + int i; + struct ble_hs_resolv_entry *rl = &g_ble_hs_resolv_list[1]; + + for (i = 1; i < g_ble_hs_resolv_data.rl_cnt; ++i) { + if(ble_hs_resolv_rpa(addr, rl->rl_peer_irk) == 0) { + memcpy(g_ble_hs_resolv_list[i].rl_peer_rpa, addr, BLE_DEV_ADDR_LEN); + g_ble_hs_resolv_list[i].rl_addr_type = addr_type; + return rl; + } + + ++rl; + } +#endif + return NULL; +} + /** * Resolve a Resolvable Private Address * diff --git a/nimble/host/src/ble_hs_resolv_priv.h b/nimble/host/src/ble_hs_resolv_priv.h index 7bc8a2fe6..5a74b9420 100644 --- a/nimble/host/src/ble_hs_resolv_priv.h +++ b/nimble/host/src/ble_hs_resolv_priv.h @@ -79,6 +79,10 @@ void ble_hs_resolv_nrpa_disable(void); struct ble_hs_resolv_entry * ble_hs_resolv_list_find(uint8_t *addr); +/* Resolve a Resolvable Private Address and maintain mapping of RPA */ +struct ble_hs_resolv_entry * +ble_hs_resolv_rpa_addr(uint8_t *addr, uint8_t addr_type); + /* Returns true if host based RPA (privacy) is enabled */ bool ble_host_rpa_enabled(void);