From fc4497b786d2debc6e95d6251ba93ec0802e8446 Mon Sep 17 00:00:00 2001 From: Szymon Janc Date: Fri, 27 Apr 2018 14:29:48 +0200 Subject: [PATCH] nimble/ll: Refactor RPA handling in resolving code - peer and local are always generated when peer is added to resolved list - users get current RPA when needed - RPAs are re-generated only from RPA timer --- .../include/controller/ble_ll_resolv.h | 4 +- nimble/controller/src/ble_ll_conn.c | 2 +- nimble/controller/src/ble_ll_resolv.c | 176 ++++++++++-------- nimble/controller/src/ble_ll_scan.c | 2 +- 4 files changed, 104 insertions(+), 80 deletions(-) diff --git a/nimble/controller/include/controller/ble_ll_resolv.h b/nimble/controller/include/controller/ble_ll_resolv.h index 6c09bee7a..2f02e848a 100644 --- a/nimble/controller/include/controller/ble_ll_resolv.h +++ b/nimble/controller/include/controller/ble_ll_resolv.h @@ -33,13 +33,13 @@ extern "C" { struct ble_ll_resolv_entry { uint8_t rl_addr_type; - uint8_t rl_local_rpa_set; uint8_t rl_reserved; uint8_t rl_priv_mode; uint8_t rl_local_irk[16]; uint8_t rl_peer_irk[16]; uint8_t rl_identity_addr[BLE_DEV_ADDR_LEN]; uint8_t rl_local_rpa[BLE_DEV_ADDR_LEN]; + uint8_t rl_peer_rpa[BLE_DEV_ADDR_LEN]; }; extern struct ble_ll_resolv_entry g_ble_ll_resolv_list[]; @@ -76,7 +76,7 @@ uint8_t ble_ll_resolv_enabled(void); /* Reset private address resolution */ void ble_ll_resolv_list_reset(void); -void ble_ll_resolv_gen_priv_addr(struct ble_ll_resolv_entry *rl, int local, +void ble_ll_resolv_get_priv_addr(struct ble_ll_resolv_entry *rl, int local, uint8_t *addr); /* Generate a resolvable private address. */ diff --git a/nimble/controller/src/ble_ll_conn.c b/nimble/controller/src/ble_ll_conn.c index d33e9d319..e868d0fac 100644 --- a/nimble/controller/src/ble_ll_conn.c +++ b/nimble/controller/src/ble_ll_conn.c @@ -2753,7 +2753,7 @@ ble_ll_conn_req_pdu_update(struct os_mbuf *m, uint8_t *adva, uint8_t addr_type, */ if (rl) { hdr |= BLE_ADV_PDU_HDR_TXADD_RAND; - ble_ll_resolv_gen_priv_addr(rl, 1, dptr); + ble_ll_resolv_get_priv_addr(rl, 1, dptr); addr = NULL; } } diff --git a/nimble/controller/src/ble_ll_resolv.c b/nimble/controller/src/ble_ll_resolv.c index 52e1e33d8..d67ad56af 100644 --- a/nimble/controller/src/ble_ll_resolv.c +++ b/nimble/controller/src/ble_ll_resolv.c @@ -72,11 +72,68 @@ ble_ll_resolv_list_chg_allowed(void) return rc; } + +/** + * Called to generate a resolvable private address in rl structure + * + * @param rl + * @param local + */ +static void +ble_ll_resolv_gen_priv_addr(struct ble_ll_resolv_entry *rl, int local) +{ + uint8_t *irk; + uint8_t *prand; + uint32_t *irk32; + uint32_t *key32; + uint32_t *pt32; + struct ble_encryption_block ecb; + uint8_t *addr; + + assert(rl != NULL); + + if (local) { + addr = rl->rl_local_rpa; + irk = rl->rl_local_irk; + } else { + addr = rl->rl_peer_rpa; + irk = rl->rl_peer_irk; + } + + /* Get prand */ + prand = addr + 3; + ble_ll_rand_prand_get(prand); + + /* Calculate hash, hash = ah(local IRK, prand) */ + + irk32 = (uint32_t *)irk; + key32 = (uint32_t *)&ecb.key[0]; + key32[0] = irk32[0]; + key32[1] = irk32[1]; + key32[2] = irk32[2]; + key32[3] = irk32[3]; + pt32 = (uint32_t *)&ecb.plain_text[0]; + pt32[0] = 0; + pt32[1] = 0; + pt32[2] = 0; + ecb.plain_text[12] = 0; + 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]; +} + /** * Called when the Resolvable private address timer expires. This timer - * is used to regenerate local RPA's in the resolving list. + * is used to regenerate local and peers RPA's in the resolving list. */ -void +static void ble_ll_resolv_rpa_timer_cb(struct ble_npl_event *ev) { int i; @@ -86,9 +143,11 @@ ble_ll_resolv_rpa_timer_cb(struct ble_npl_event *ev) rl = &g_ble_ll_resolv_list[0]; for (i = 0; i < g_ble_ll_resolv_data.rl_cnt; ++i) { OS_ENTER_CRITICAL(sr); - rl->rl_local_rpa_set = 0; - ble_ll_resolv_gen_priv_addr(rl, 1, rl->rl_local_rpa); - rl->rl_local_rpa_set = 1; + ble_ll_resolv_gen_priv_addr(rl, 1); + OS_EXIT_CRITICAL(sr); + + OS_ENTER_CRITICAL(sr); + ble_ll_resolv_gen_priv_addr(rl, 0); OS_EXIT_CRITICAL(sr); ++rl; } @@ -237,30 +296,37 @@ ble_ll_resolv_list_add(uint8_t *cmdbuf) addr_type = cmdbuf[0]; ident_addr = cmdbuf + 1; - rc = BLE_ERR_SUCCESS; - if (!ble_ll_is_on_resolv_list(ident_addr, addr_type)) { - rl = &g_ble_ll_resolv_list[g_ble_ll_resolv_data.rl_cnt]; - rl->rl_addr_type = addr_type; - rl->rl_local_rpa_set = 0; - memcpy(&rl->rl_identity_addr[0], ident_addr, BLE_DEV_ADDR_LEN); - swap_buf(rl->rl_peer_irk, cmdbuf + 7, 16); - swap_buf(rl->rl_local_irk, cmdbuf + 23, 16); - - /* By default use ptivacy network mode */ - rl->rl_priv_mode = BLE_HCI_PRIVACY_NETWORK; - - /* - * Add peer IRK to HW resolving list. If we can add it, also - * generate a local RPA now to save time later. - */ - rc = ble_hw_resolv_list_add(rl->rl_peer_irk); - if (!rc) { - ble_ll_resolv_gen_priv_addr(rl, 1, rl->rl_local_rpa); - rl->rl_local_rpa_set = 1; - } - ++g_ble_ll_resolv_data.rl_cnt; + /* spec is not clear on how to handle this but make sure host is aware + * that new keys are not used in that case + */ + if (ble_ll_is_on_resolv_list(ident_addr, addr_type)) { + return BLE_ERR_INV_HCI_CMD_PARMS; } + rl = &g_ble_ll_resolv_list[g_ble_ll_resolv_data.rl_cnt]; + memset (rl, 0, sizeof(*rl)); + + rl->rl_addr_type = addr_type; + memcpy(&rl->rl_identity_addr[0], ident_addr, BLE_DEV_ADDR_LEN); + swap_buf(rl->rl_peer_irk, cmdbuf + 7, 16); + swap_buf(rl->rl_local_irk, cmdbuf + 23, 16); + + /* By default use privacy network mode */ + rl->rl_priv_mode = BLE_HCI_PRIVACY_NETWORK; + + /* Add peer IRK to HW resolving list. Should always succeed since we + * already checked if there is room for it. + */ + rc = ble_hw_resolv_list_add(rl->rl_peer_irk); + assert (rc == BLE_ERR_SUCCESS); + + /* generate a local and peer RPAs now, those will be updated by timer + * when resolution is enabled + */ + ble_ll_resolv_gen_priv_addr(rl, 1); + ble_ll_resolv_gen_priv_addr(rl, 0); + ++g_ble_ll_resolv_data.rl_cnt; + return rc; } @@ -421,65 +487,23 @@ ble_ll_resolv_get_rpa_tmo(void) return g_ble_ll_resolv_data.rpa_tmo; } -/** - * Called to generate a resolvable private address - * - * @param rl - * @param local - * @param addr Pointer to resolvable private address - */ void -ble_ll_resolv_gen_priv_addr(struct ble_ll_resolv_entry *rl, int local, +ble_ll_resolv_get_priv_addr(struct ble_ll_resolv_entry *rl, int local, uint8_t *addr) { - uint8_t *irk; - uint8_t *prand; - uint32_t *irk32; - uint32_t *key32; - uint32_t *pt32; - struct ble_encryption_block ecb; + os_sr_t sr; assert(rl != NULL); assert(addr != NULL); - /* If the local rpa has already been generated, just copy it */ - if (local && rl->rl_local_rpa_set) { - memcpy(addr, rl->rl_local_rpa, BLE_DEV_ADDR_LEN); - return; - } - - /* Get prand */ - prand = addr + 3; - ble_ll_rand_prand_get(prand); - - /* Calculate hash, hash = ah(local IRK, prand) */ + OS_ENTER_CRITICAL(sr); if (local) { - irk = rl->rl_local_irk; + memcpy(addr, rl->rl_local_rpa, BLE_DEV_ADDR_LEN); } else { - irk = rl->rl_peer_irk; + memcpy(addr, rl->rl_peer_rpa, BLE_DEV_ADDR_LEN); } - irk32 = (uint32_t *)irk; - key32 = (uint32_t *)&ecb.key[0]; - key32[0] = irk32[0]; - key32[1] = irk32[1]; - key32[2] = irk32[2]; - key32[3] = irk32[3]; - pt32 = (uint32_t *)&ecb.plain_text[0]; - pt32[0] = 0; - pt32[1] = 0; - pt32[2] = 0; - ecb.plain_text[12] = 0; - 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]; + OS_EXIT_CRITICAL(sr); } /** @@ -507,7 +531,7 @@ ble_ll_resolv_gen_rpa(uint8_t *addr, uint8_t addr_type, uint8_t *rpa, int local) irk = rl->rl_peer_irk; } if (ble_ll_resolv_irk_nonzero(irk)) { - ble_ll_resolv_gen_priv_addr(rl, local, rpa); + ble_ll_resolv_get_priv_addr(rl, local, rpa); rc = 1; } } diff --git a/nimble/controller/src/ble_ll_scan.c b/nimble/controller/src/ble_ll_scan.c index 843404d71..6b5ed5d95 100644 --- a/nimble/controller/src/ble_ll_scan.c +++ b/nimble/controller/src/ble_ll_scan.c @@ -344,7 +344,7 @@ ble_ll_scan_req_pdu_make(struct ble_ll_scan_sm *scansm, uint8_t *adv_addr, * section 6.3). */ if (rl) { - ble_ll_resolv_gen_priv_addr(rl, 1, rpa); + ble_ll_resolv_get_priv_addr(rl, 1, rpa); scana = rpa; } else { ble_ll_scan_refresh_nrpa(scansm);