diff --git a/nimble/controller/include/controller/ble_ll_resolv.h b/nimble/controller/include/controller/ble_ll_resolv.h index b9ca7fd387..ff28e783ce 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 own_addr_type, 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 fad0bcc7df..52606f93e6 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 0221d6a916..cee6f976b2 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..1710e456fe 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,33 @@ 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; + } + + if (ble_ll_is_busy(BLE_LL_BUSY_EXCLUDE_CONNECTIONS)) { + return BLE_ERR_CMD_DISALLOWED; + } + + 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 +382,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 045cd50c5e..43f3682f70 100644 --- a/nimble/controller/syscfg.yml +++ b/nimble/controller/syscfg.yml @@ -402,6 +402,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) --- */