From 2cc91e0717196e3abb98a9af645b549afa55e320 Mon Sep 17 00:00:00 2001 From: Andrzej Kaczmarek Date: Tue, 3 Oct 2023 15:12:44 +0200 Subject: [PATCH] nimble/ll: Add vs hci to set local IRK This adds vendor-specific HCI command to set local IRK in controller. IRK can be set for both public and static random addresses separately. Local IRK, if set, is used to generate local RPA in use cases where own address type was set to 0x02 or 0x03 but peer address is not added to resolving list. This for example allows to handle initiating connection to a new peer using RPA as our local address entirely in LL. Without that command it would be required for host to generate an RPA, set it as random address and connect using random address. This however doesn't work well with NimBLE host. If no IRK is set (or set to all-zero), the controller behaves as usual which makes it safe to enable as it won't break anything. --- .../include/controller/ble_ll_resolv.h | 11 ++ nimble/controller/src/ble_ll_adv.c | 4 +- nimble/controller/src/ble_ll_conn.c | 21 +-- nimble/controller/src/ble_ll_hci_vs.c | 28 ++++ nimble/controller/src/ble_ll_resolv.c | 126 +++++++++++++++--- nimble/controller/syscfg.yml | 12 ++ nimble/include/nimble/hci_common.h | 7 +- 7 files changed, 179 insertions(+), 30 deletions(-) diff --git a/nimble/controller/include/controller/ble_ll_resolv.h b/nimble/controller/include/controller/ble_ll_resolv.h index b9ca7fd387..f40de776ba 100644 --- a/nimble/controller/include/controller/ble_ll_resolv.h +++ b/nimble/controller/include/controller/ble_ll_resolv.h @@ -115,6 +115,17 @@ int ble_ll_resolv_peer_rpa_any(const uint8_t *rpa); /* Initialize resolv*/ void ble_ll_resolv_init(void); +#if MYNEWT_VAL(BLE_LL_HCI_VS_LOCAL_IRK) +int ble_ll_resolv_local_irk_set(uint8_t own_addr_type, const uint8_t *irk); +int ble_ll_resolv_local_rpa_get(uint8_t own_addr_type, uint8_t *rpa); +#else +static inline int +ble_ll_resolv_local_rpa_get(uint8_t *rpa) +{ + return -1; +} +#endif + #ifdef __cplusplus } #endif diff --git a/nimble/controller/src/ble_ll_adv.c b/nimble/controller/src/ble_ll_adv.c index 7056c8de93..fd8a13905d 100644 --- a/nimble/controller/src/ble_ll_adv.c +++ b/nimble/controller/src/ble_ll_adv.c @@ -332,7 +332,9 @@ static void ble_ll_adv_rpa_update(struct ble_ll_adv_sm *advsm) { if (ble_ll_resolv_gen_rpa(advsm->peer_addr, advsm->peer_addr_type, - advsm->adva, 1)) { + advsm->adva, 1) || + (ble_ll_resolv_local_rpa_get(advsm->own_addr_type & 1, + advsm->adva) == 0)) { ble_ll_adv_flags_set(advsm, BLE_LL_ADV_SM_FLAG_TX_ADD); } else { if (advsm->own_addr_type & 1) { diff --git a/nimble/controller/src/ble_ll_conn.c b/nimble/controller/src/ble_ll_conn.c index 3565e10424..63c9ee5961 100644 --- a/nimble/controller/src/ble_ll_conn.c +++ b/nimble/controller/src/ble_ll_conn.c @@ -3118,15 +3118,20 @@ ble_ll_conn_prepare_connect_ind(struct ble_ll_conn_sm *connsm, /* XXX: do this ahead of time? Calculate the local rpa I mean */ #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) - if ((connsm->own_addr_type > BLE_HCI_ADV_OWN_ADDR_RANDOM) && - (addrd->rpa_index >= 0)) { - /* We are using RPA and advertiser was on our resolving list, so - * we'll use RPA to reply (see Core 5.3, Vol 6, Part B, 6.4). - */ - rl = &g_ble_ll_resolv_list[addrd->rpa_index]; - if (rl->rl_has_local) { + if (connsm->own_addr_type > BLE_HCI_ADV_OWN_ADDR_RANDOM) { + if (addrd->rpa_index >= 0) { + /* We are using RPA and advertiser was on our resolving list, so + * we'll use RPA to reply (see Core 5.3, Vol 6, Part B, 6.4). + */ + rl = &g_ble_ll_resolv_list[addrd->rpa_index]; + if (rl->rl_has_local) { + hdr |= BLE_ADV_PDU_HDR_TXADD_RAND; + ble_ll_resolv_get_priv_addr(rl, 1, pdu_data->inita); + addr = NULL; + } + } else if (ble_ll_resolv_local_rpa_get(connsm->own_addr_type & 1, + pdu_data->inita) == 0) { hdr |= BLE_ADV_PDU_HDR_TXADD_RAND; - ble_ll_resolv_get_priv_addr(rl, 1, pdu_data->inita); addr = NULL; } } diff --git a/nimble/controller/src/ble_ll_hci_vs.c b/nimble/controller/src/ble_ll_hci_vs.c index 7fd9a6ea6b..f80db7f3f7 100644 --- a/nimble/controller/src/ble_ll_hci_vs.c +++ b/nimble/controller/src/ble_ll_hci_vs.c @@ -29,6 +29,7 @@ #include "controller/ble_fem.h" #include "ble_ll_conn_priv.h" #include "ble_ll_priv.h" +#include "controller/ble_ll_resolv.h" #if MYNEWT_VAL(BLE_LL_HCI_VS) @@ -328,6 +329,29 @@ ble_ll_hci_vs_set_antenna(uint16_t ocf, const uint8_t *cmdbuf, uint8_t cmdlen, } #endif +#if MYNEWT_VAL(BLE_LL_HCI_VS_LOCAL_IRK) +static int +ble_ll_hci_vs_set_local_irk(uint16_t ocf, const uint8_t *cmdbuf, uint8_t cmdlen, + uint8_t *rspbuf, uint8_t *rsplen) +{ + const struct ble_hci_vs_set_local_irk_cp *cmd = (const void *)cmdbuf; + int rc; + + if (cmdlen != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + rc = ble_ll_resolv_local_irk_set(cmd->own_addr_type, cmd->irk); + if (rc) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + *rsplen = 0; + + return 0; +} +#endif + static struct ble_ll_hci_vs_cmd g_ble_ll_hci_vs_cmds[] = { BLE_LL_HCI_VS_CMD(BLE_HCI_OCF_VS_RD_STATIC_ADDR, ble_ll_hci_vs_rd_static_addr), @@ -354,6 +378,10 @@ static struct ble_ll_hci_vs_cmd g_ble_ll_hci_vs_cmds[] = { #if MYNEWT_VAL(BLE_FEM_ANTENNA) BLE_LL_HCI_VS_CMD(BLE_HCI_OCF_VS_SET_ANTENNA, ble_ll_hci_vs_set_antenna), #endif +#if MYNEWT_VAL(BLE_LL_HCI_VS_LOCAL_IRK) + BLE_LL_HCI_VS_CMD(BLE_HCI_OCF_VS_SET_LOCAL_IRK, + ble_ll_hci_vs_set_local_irk), +#endif }; static struct ble_ll_hci_vs_cmd * diff --git a/nimble/controller/src/ble_ll_resolv.c b/nimble/controller/src/ble_ll_resolv.c index 8ec9a97d1b..3566876e09 100644 --- a/nimble/controller/src/ble_ll_resolv.c +++ b/nimble/controller/src/ble_ll_resolv.c @@ -48,6 +48,16 @@ struct ble_ll_resolv_data g_ble_ll_resolv_data; __attribute__((aligned(4))) struct ble_ll_resolv_entry g_ble_ll_resolv_list[MYNEWT_VAL(BLE_LL_RESOLV_LIST_SIZE)]; +#if MYNEWT_VAL(BLE_LL_HCI_VS_LOCAL_IRK) +struct local_irk_data { + uint8_t is_set; + uint8_t irk[16]; + uint8_t rpa[6]; +}; +/* 0 is for public, 1 is for static address */ +static struct local_irk_data g_local_irk[2]; +#endif + /** * Called to determine if a change is allowed to the resolving list at this * time. We are not allowed to modify the resolving list if address translation @@ -70,6 +80,30 @@ ble_ll_resolv_list_chg_allowed(void) return rc; } +static void +generate_rpa(const uint8_t *irk, uint8_t *rpa) +{ + uint8_t *prand; + struct ble_encryption_block ecb; + + /* Get prand */ + prand = rpa + 3; + ble_ll_rand_prand_get(prand); + + /* Calculate hash, hash = ah(local IRK, prand) */ + memcpy(ecb.key, irk, 16); + memset(ecb.plain_text, 0, 13); + ecb.plain_text[13] = prand[2]; + ecb.plain_text[14] = prand[1]; + ecb.plain_text[15] = prand[0]; + + /* Calculate hash */ + ble_hw_encrypt_block(&ecb); + + rpa[0] = ecb.cipher_text[15]; + rpa[1] = ecb.cipher_text[14]; + rpa[2] = ecb.cipher_text[13]; +} /** * Called to generate a resolvable private address in rl structure @@ -81,8 +115,6 @@ static void ble_ll_resolv_gen_priv_addr(struct ble_ll_resolv_entry *rl, int local) { uint8_t *irk; - uint8_t *prand; - struct ble_encryption_block ecb; uint8_t *addr; BLE_LL_ASSERT(rl != NULL); @@ -95,23 +127,7 @@ ble_ll_resolv_gen_priv_addr(struct ble_ll_resolv_entry *rl, int local) irk = rl->rl_peer_irk; } - /* Get prand */ - prand = addr + 3; - ble_ll_rand_prand_get(prand); - - /* Calculate hash, hash = ah(local IRK, prand) */ - memcpy(ecb.key, irk, 16); - memset(ecb.plain_text, 0, 13); - ecb.plain_text[13] = prand[2]; - ecb.plain_text[14] = prand[1]; - ecb.plain_text[15] = prand[0]; - - /* Calculate hash */ - ble_hw_encrypt_block(&ecb); - - addr[0] = ecb.cipher_text[15]; - addr[1] = ecb.cipher_text[14]; - addr[2] = ecb.cipher_text[13]; + generate_rpa(irk, addr); } /** @@ -124,6 +140,10 @@ ble_ll_resolv_rpa_timer_cb(struct ble_npl_event *ev) int i; os_sr_t sr; struct ble_ll_resolv_entry *rl; +#if MYNEWT_VAL(BLE_LL_HCI_VS_LOCAL_IRK) + struct local_irk_data *irk_data; + uint8_t rpa[6]; +#endif rl = &g_ble_ll_resolv_list[0]; for (i = 0; i < g_ble_ll_resolv_data.rl_cnt; ++i) { @@ -141,6 +161,18 @@ ble_ll_resolv_rpa_timer_cb(struct ble_npl_event *ev) ++rl; } +#if MYNEWT_VAL(BLE_LL_HCI_VS_LOCAL_IRK) + for (i = 0; i < ARRAY_SIZE(g_local_irk); i++) { + irk_data = &g_local_irk[i]; + if (irk_data->is_set) { + generate_rpa(irk_data->irk, rpa); + OS_ENTER_CRITICAL(sr); + memcpy(irk_data->rpa, rpa, 6); + OS_EXIT_CRITICAL(sr); + } + } +#endif + ble_npl_callout_reset(&g_ble_ll_resolv_data.rpa_timer, g_ble_ll_resolv_data.rpa_tmo); @@ -637,6 +669,58 @@ ble_ll_resolv_gen_rpa(uint8_t *addr, uint8_t addr_type, uint8_t *rpa, int local) return 0; } +#if MYNEWT_VAL(BLE_LL_HCI_VS_LOCAL_IRK) +int +ble_ll_resolv_local_irk_set(uint8_t own_addr_type, const uint8_t *irk) +{ + struct local_irk_data *irk_data; + int i; + + if (own_addr_type >= 2) { + return -1; + } + + irk_data = &g_local_irk[own_addr_type]; + + memcpy(irk_data->irk, irk, 16); + + irk_data->is_set = 0; + + for (i = 0; i < 16; i++) { + if (irk[i]) { + irk_data->is_set = 1; + break; + } + } + + if (irk_data->is_set) { + generate_rpa(irk_data->irk, irk_data->rpa); + } + + return 0; +} + +int +ble_ll_resolv_local_rpa_get(uint8_t own_addr_type, uint8_t *rpa) +{ + struct local_irk_data *irk_data; + + if (own_addr_type >= 2) { + return -1; + } + + irk_data = &g_local_irk[own_addr_type]; + + if (!irk_data->is_set) { + return -1; + } + + memcpy(rpa, irk_data->rpa, 6); + + return 0; +} +#endif + /** * Resolve a Resolvable Private Address * @@ -738,6 +822,10 @@ ble_ll_resolv_init(void) &g_ble_ll_data.ll_evq, ble_ll_resolv_rpa_timer_cb, NULL); + +#if MYNEWT_VAL(BLE_LL_HCI_VS_LOCAL_IRK) + memset(&g_local_irk, 0, sizeof(g_local_irk)); +#endif } #endif /* if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) */ diff --git a/nimble/controller/syscfg.yml b/nimble/controller/syscfg.yml index f02b04dfe4..66f4d8caee 100644 --- a/nimble/controller/syscfg.yml +++ b/nimble/controller/syscfg.yml @@ -417,6 +417,18 @@ syscfg.defs: value: 0 restrictions: - BLE_LL_HCI_VS if 1 + BLE_LL_HCI_VS_LOCAL_IRK: + description: > + Enables HCI command to set local IRK. + The local IRK is used by controller to generate RPA address in case + own address type 0x02 or 0x03 was requested by host but there is no + corresponding entry on resolving list. This allows to handle privacy + scenarios almost entirely in controller. If no local IRK is set, the + controller behaves as if feature is not enabled. + value: 0 + restrictions: + - BLE_LL_HCI_VS if 1 + BLE_LL_HCI_VS_EVENT_ON_ASSERT: description: > diff --git a/nimble/include/nimble/hci_common.h b/nimble/include/nimble/hci_common.h index 742824d877..ba1735baa0 100644 --- a/nimble/include/nimble/hci_common.h +++ b/nimble/include/nimble/hci_common.h @@ -1208,8 +1208,11 @@ struct ble_hci_vs_set_data_len_rp { struct ble_hci_vs_set_antenna_cp { uint8_t antenna; } __attribute__((packed)); - - +#define BLE_HCI_OCF_VS_SET_LOCAL_IRK (MYNEWT_VAL(BLE_HCI_VS_OCF_OFFSET) + (0x000A)) +struct ble_hci_vs_set_local_irk_cp { + uint8_t own_addr_type; + uint8_t irk[16]; +} __attribute__((packed)); /* Command Specific Definitions */ /* --- Set controller to host flow control (OGF 0x03, OCF 0x0031) --- */