diff --git a/nimble/controller/include/controller/ble_ll_resolv.h b/nimble/controller/include/controller/ble_ll_resolv.h index b9ca7fd387..16c8808b2a 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(const uint8_t *irk); +int ble_ll_resolv_local_rpa_get(uint8_t *rpa); +#else +static inline int +ble_ll_resolv_local_rpa_get(uint8_t *rpa) +{ + return -ENOENT; +} +#endif + #ifdef __cplusplus } #endif diff --git a/nimble/controller/src/ble_ll_conn.c b/nimble/controller/src/ble_ll_conn.c index 3565e10424..9328a6ea50 100644 --- a/nimble/controller/src/ble_ll_conn.c +++ b/nimble/controller/src/ble_ll_conn.c @@ -3118,15 +3118,18 @@ 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) { - hdr |= BLE_ADV_PDU_HDR_TXADD_RAND; - ble_ll_resolv_get_priv_addr(rl, 1, pdu_data->inita); + 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(pdu_data->inita) == 0) { addr = NULL; } } diff --git a/nimble/controller/src/ble_ll_hci_vs.c b/nimble/controller/src/ble_ll_hci_vs.c index 7fd9a6ea6b..1ecb49c22e 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->irk); + if (rc) { + return rc; + } + + *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..832749dc58 100644 --- a/nimble/controller/src/ble_ll_resolv.c +++ b/nimble/controller/src/ble_ll_resolv.c @@ -19,6 +19,7 @@ #include #include #include +#include #include "syscfg/syscfg.h" #include "os/os.h" #include "nimble/ble.h" @@ -48,6 +49,15 @@ 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]; +}; +static struct local_irk_data g_local_irk; +#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); } /** @@ -141,6 +157,14 @@ ble_ll_resolv_rpa_timer_cb(struct ble_npl_event *ev) ++rl; } +#if MYNEWT_VAL(BLE_LL_HCI_VS_LOCAL_IRK) + OS_ENTER_CRITICAL(sr); + if (g_local_irk.is_set) { + generate_rpa(g_local_irk.irk, g_local_irk.rpa); + } + 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 +661,41 @@ 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(const uint8_t *irk) +{ + int i; + + memcpy(g_local_irk.irk, irk, 16); + + g_local_irk.is_set = 0; + + for (i = 0; i < 16; i++) { + if (irk[i]) { + g_local_irk.is_set = 1; + break; + } + } + + if (g_local_irk.is_set) { + generate_rpa(g_local_irk.irk, g_local_irk.rpa); + } + + return 0; +} + +int +ble_ll_resolv_local_rpa_get(uint8_t *rpa) +{ + if (!g_local_irk.is_set) { + return -ENOENT; + } + + return 0; +} +#endif + /** * Resolve a Resolvable Private Address * @@ -738,6 +797,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..e0c9e0f6b5 100644 --- a/nimble/include/nimble/hci_common.h +++ b/nimble/include/nimble/hci_common.h @@ -1208,8 +1208,10 @@ 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 irk[16]; +} __attribute__((packed)); /* Command Specific Definitions */ /* --- Set controller to host flow control (OGF 0x03, OCF 0x0031) --- */