From 5c79268c74110adeedb985366cfa83886b9c8102 Mon Sep 17 00:00:00 2001 From: Prasad Alatkar Date: Thu, 5 Mar 2020 20:18:40 +0800 Subject: [PATCH] NimBLE: Add Host based privacy (RPA) support --- nimble/host/include/host/ble_hs_pvcy.h | 40 + nimble/host/include/host/ble_store.h | 7 +- nimble/host/src/ble_gap.c | 30 + nimble/host/src/ble_hs_conn.c | 30 + nimble/host/src/ble_hs_hci_evt.c | 39 + nimble/host/src/ble_hs_id.c | 64 ++ nimble/host/src/ble_hs_id_priv.h | 4 + nimble/host/src/ble_hs_priv.h | 5 + nimble/host/src/ble_hs_pvcy.c | 88 ++- nimble/host/src/ble_hs_pvcy_priv.h | 3 + nimble/host/src/ble_hs_resolv.c | 711 ++++++++++++++++++ nimble/host/src/ble_hs_resolv_priv.h | 106 +++ nimble/host/src/ble_sm.c | 38 + nimble/host/src/ble_sm_alg.c | 2 +- nimble/host/src/ble_store_util.c | 20 + .../store/config/src/ble_store_config_priv.h | 3 + nimble/host/store/config/src/ble_store_nvs.c | 230 +++++- 17 files changed, 1375 insertions(+), 45 deletions(-) create mode 100644 nimble/host/include/host/ble_hs_pvcy.h create mode 100644 nimble/host/src/ble_hs_resolv.c create mode 100644 nimble/host/src/ble_hs_resolv_priv.h diff --git a/nimble/host/include/host/ble_hs_pvcy.h b/nimble/host/include/host/ble_hs_pvcy.h new file mode 100644 index 000000000..0ff32b80b --- /dev/null +++ b/nimble/host/include/host/ble_hs_pvcy.h @@ -0,0 +1,40 @@ +/* + * Copyright 2020 Espressif Systems (Shanghai) PTE LTD + * + * Licensed to the Apache Software Foundation (ASF) under one + * or more contributor license agreements. See the NOTICE file + * distributed with this work for additional information + * regarding copyright ownership. The ASF licenses this file + * to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance + * with the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY + * KIND, either express or implied. See the License for the + * specific language governing permissions and limitations + * under the License. + */ + +#include "host/ble_hs.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) +/* Called to configure local(own) privacy (RPA) when using host based privacy. In + * Host based privacy as controller is not aware of RPA, we do it via + * 'BLE_ADDR_RANDOM' addr_type route. + * + * @param enable RPA when enable is not 0 + * disable RPA otherwise + * + * @return return 0 when successful. + * return appropriate error code otherwise + */ +int ble_hs_pvcy_rpa_config(uint8_t enable); +#endif diff --git a/nimble/host/include/host/ble_store.h b/nimble/host/include/host/ble_store.h index 24b208a09..ac32ad81a 100644 --- a/nimble/host/include/host/ble_store.h +++ b/nimble/host/include/host/ble_store.h @@ -27,9 +27,10 @@ extern "C" { #endif -#define BLE_STORE_OBJ_TYPE_OUR_SEC 1 -#define BLE_STORE_OBJ_TYPE_PEER_SEC 2 -#define BLE_STORE_OBJ_TYPE_CCCD 3 +#define BLE_STORE_OBJ_TYPE_OUR_SEC 1 +#define BLE_STORE_OBJ_TYPE_PEER_SEC 2 +#define BLE_STORE_OBJ_TYPE_CCCD 3 +#define BLE_STORE_OBJ_TYPE_PEER_DEV_REC 4 /** Failed to persist record; insufficient storage capacity. */ #define BLE_STORE_EVENT_OVERFLOW 1 diff --git a/nimble/host/src/ble_gap.c b/nimble/host/src/ble_gap.c index 3720cafd6..7d7dfd144 100644 --- a/nimble/host/src/ble_gap.c +++ b/nimble/host/src/ble_gap.c @@ -26,6 +26,7 @@ #include "host/ble_hs_hci.h" #include "ble_hs_priv.h" #include "ble_gap_priv.h" +#include "ble_hs_resolv_priv.h" #ifndef min #define min(a, b) ((a) < (b) ? (a) : (b)) @@ -409,6 +410,23 @@ ble_gap_fill_conn_desc(struct ble_hs_conn *conn, ble_hs_conn_addrs(conn, &addrs); desc->our_id_addr = addrs.our_id_addr; +#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) + /* Check if the privacy is enabled, change the address type accordingly + * */ + if (ble_host_rpa_enabled()) + { + uint8_t *local_id = NULL; + struct ble_hs_resolv_entry *rl = NULL; + rl = ble_hs_resolv_list_find(conn->bhc_peer_addr.val); + + if (rl != NULL) { + /* Get public ID address here */ + ble_hs_id_addr(BLE_ADDR_PUBLIC, (const uint8_t **) &local_id, NULL); + memcpy(desc->our_id_addr.val, local_id, BLE_DEV_ADDR_LEN); + desc->our_id_addr.type = BLE_ADDR_PUBLIC; + } + } +#endif desc->peer_id_addr = addrs.peer_id_addr; desc->our_ota_addr = addrs.our_ota_addr; desc->peer_ota_addr = addrs.peer_ota_addr; @@ -2309,6 +2327,12 @@ ble_gap_timer(void) static int ble_gap_wl_busy(void) { + +#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) + if (ble_host_rpa_enabled()) { + return BLE_HS_ENOTSUP; + } +#endif /* Check if an auto or selective connection establishment procedure is in * progress. */ @@ -2345,6 +2369,12 @@ ble_gap_wl_tx_clear(void) int ble_gap_wl_set(const ble_addr_t *addrs, uint8_t white_list_count) { +#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) + if (ble_host_rpa_enabled()) { + return BLE_HS_ENOTSUP; + } +#endif + #if MYNEWT_VAL(BLE_WHITELIST) int rc; int i; diff --git a/nimble/host/src/ble_hs_conn.c b/nimble/host/src/ble_hs_conn.c index ea89460cc..a3303cfd4 100644 --- a/nimble/host/src/ble_hs_conn.c +++ b/nimble/host/src/ble_hs_conn.c @@ -23,6 +23,7 @@ #include "os/os.h" #include "host/ble_hs_id.h" #include "ble_hs_priv.h" +#include "ble_hs_resolv_priv.h" /** At least three channels required per connection (sig, att, sm). */ #define BLE_HS_CONN_MIN_CHANS 3 @@ -444,6 +445,35 @@ ble_hs_conn_addrs(const struct ble_hs_conn *conn, /* Determine peer address information. */ addrs->peer_id_addr = conn->bhc_peer_addr; addrs->peer_ota_addr = conn->bhc_peer_addr; + +#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) + /* RPA: Override peer address information. */ + struct ble_hs_resolv_entry *rl = NULL; + + ble_addr_t bhc_peer_addr; + bhc_peer_addr.type = conn->bhc_peer_addr.type; + memcpy(bhc_peer_addr.val, conn->bhc_peer_addr.val, BLE_DEV_ADDR_LEN); + if (ble_host_rpa_enabled()) { + + uint8_t *local_id = NULL; + ble_hs_id_addr(BLE_ADDR_PUBLIC, (const uint8_t **) &local_id, NULL); + + rl = ble_hs_resolv_list_find(bhc_peer_addr.val); + if (rl != NULL) { + memcpy(addrs->peer_ota_addr.val, addrs->peer_id_addr.val, BLE_DEV_ADDR_LEN); + memcpy(addrs->peer_id_addr.val, rl->rl_identity_addr, BLE_DEV_ADDR_LEN); + + addrs->peer_id_addr.type = rl->rl_addr_type; + + /* RL is present: populate our id addr with public ID */ + memcpy(addrs->our_id_addr.val, local_id, BLE_DEV_ADDR_LEN); + addrs->our_id_addr.type = BLE_ADDR_PUBLIC; + BLE_HS_LOG(DEBUG, "Revised our id addr:\n"); + ble_hs_log_flat_buf(our_id_addr_val, BLE_DEV_ADDR_LEN); + BLE_HS_LOG(DEBUG, "\n"); + } + } +#endif switch (conn->bhc_peer_addr.type) { case BLE_ADDR_PUBLIC: case BLE_ADDR_RANDOM: diff --git a/nimble/host/src/ble_hs_hci_evt.c b/nimble/host/src/ble_hs_hci_evt.c index e7af2243b..f7d45a516 100644 --- a/nimble/host/src/ble_hs_hci_evt.c +++ b/nimble/host/src/ble_hs_hci_evt.c @@ -24,6 +24,7 @@ #include "nimble/hci_common.h" #include "host/ble_gap.h" #include "ble_hs_priv.h" +#include "ble_hs_resolv_priv.h" _Static_assert(sizeof (struct hci_data_hdr) == BLE_HCI_DATA_HDR_SZ, "struct hci_data_hdr must be 4 bytes"); @@ -347,6 +348,19 @@ ble_hs_hci_evt_le_enh_conn_complete(uint8_t subevent, const void *data, memcpy(evt.peer_addr, ev->peer_addr, BLE_DEV_ADDR_LEN); memcpy(evt.local_rpa, ev->local_rpa, BLE_DEV_ADDR_LEN); memcpy(evt.peer_rpa,ev->peer_rpa, BLE_DEV_ADDR_LEN); + +#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) + /* RPA needs to be resolved here, as controller is not aware of the + * address is RPA in Host based RPA */ + if (ble_host_rpa_enabled()) { + uint8_t *local_id_rpa = ble_hs_get_rpa_local(); + memcpy(evt.local_rpa, local_id_rpa, BLE_DEV_ADDR_LEN); + } + + struct ble_hs_resolv_entry *rl = NULL; + ble_rpa_replace_peer_params_with_rl(evt.peer_addr, + &evt.peer_addr_type, &rl); +#endif evt.conn_itvl = le16toh(ev->conn_itvl); evt.conn_latency = le16toh(ev->conn_latency); evt.supervision_timeout = le16toh(ev->supervision_timeout); @@ -389,6 +403,23 @@ ble_hs_hci_evt_le_conn_complete(uint8_t subevent, const void *data, evt.peer_addr_type = ev->peer_addr_type; memcpy(evt.peer_addr, ev->peer_addr, BLE_DEV_ADDR_LEN); +#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) + /* RPA needs to be resolved here, as controller is not aware of the + * address is RPA in Host based RPA */ + if (ble_host_rpa_enabled()) { + uint8_t *local_id_rpa = ble_hs_get_rpa_local(); + memcpy(evt.local_rpa, local_id_rpa, BLE_DEV_ADDR_LEN); + + struct ble_hs_resolv_entry *rl = NULL; + ble_rpa_replace_peer_params_with_rl(evt.peer_addr, + &evt.peer_addr_type, &rl); + if (rl == NULL) { + if (ble_rpa_resolv_add_peer_rec(evt.peer_addr) != 0) { + BLE_HS_LOG(DEBUG, "Memory unavailable for new peer record\n"); + } + } + } +#endif evt.conn_itvl = le16toh(ev->conn_itvl); evt.conn_latency = le16toh(ev->conn_latency); evt.supervision_timeout = le16toh(ev->supervision_timeout); @@ -478,6 +509,14 @@ ble_hs_hci_evt_le_adv_rpt(uint8_t subevent, const void *data, unsigned int len) for (i = 0; i < ev->num_reports; i++) { rpt = data; +#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) + if (ble_host_rpa_enabled()) { + /* Now RPA to be resolved here, since controller is unaware of the + * address is RPA */ + ble_rpa_replace_peer_params_with_rl(desc.addr.val, + &desc.addr.type, NULL); + } +#endif data += sizeof(rpt) + rpt->data_len + 1; diff --git a/nimble/host/src/ble_hs_id.c b/nimble/host/src/ble_hs_id.c index 2dd21aa78..fc35e93cd 100644 --- a/nimble/host/src/ble_hs_id.c +++ b/nimble/host/src/ble_hs_id.c @@ -20,12 +20,25 @@ #include #include "host/ble_hs_id.h" #include "ble_hs_priv.h" +#include "ble_hs_resolv_priv.h" static uint8_t ble_hs_id_pub[6]; static uint8_t ble_hs_id_rnd[6]; static const uint8_t ble_hs_misc_null_addr[6]; +bool +ble_hs_is_rpa(uint8_t *addr, uint8_t addr_type) +{ + bool rc = 0; + /* According to spec v4.2, Vol 6, Part B, section 1.3.2.2, the two most + * significant bits of RPA shall be equal to 0 and 1 */ + if (addr_type && ((addr[5] & 0xc0) == 0x40)) { + rc = 1; + } + return rc; +} + void ble_hs_id_set_pub(const uint8_t *pub_addr) { @@ -55,6 +68,57 @@ ble_hs_id_gen_rnd(int nrpa, ble_addr_t *out_addr) return 0; } +#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) +/** + * Sets the device's pseudo RPA address when 'Host based privacy' is in use. + * The address type (RPA) is inferred from the most-significant bits. The + * address is specified in host byte order (little-endian!). + * + * @param rnd_addr The RPA address to set. + * + * @return 0 on success; + * BLE_HS_EINVAL if the specified address is not a + * resolvable private address. + * Other nonzero on error. + */ +int +ble_hs_id_set_pseudo_rnd(const uint8_t *rnd_addr) +{ + uint8_t addr_type_byte; + int rc; + int i, rnd_part_sum = 0; + + ble_hs_lock(); + + /* Make sure all bits of rnd_addr are neither one nor zero (3rd, 4th and + * 5th bytes of rnd_addr(RPA) are prand) Vol 6, Part B, section 1.3.2.2 + * The two most significant bits of RPA shall be equal to 0 and 1 */ + addr_type_byte = rnd_addr[5] & 0xc0; + for (i = 3; i < BLE_DEV_ADDR_LEN; i++) { + rnd_part_sum += *(rnd_addr + i); + } + rnd_part_sum -= addr_type_byte; + + /* All ones in random part: 3*(0xFF) - 0x40 = 0x2BD */ + if ((addr_type_byte != 0x40) || + (rnd_part_sum == 0) || (rnd_part_sum == 0x2BD)) { + rc = BLE_HS_EINVAL; + goto done; + } + /* set the RPA address as pseudo random address in controller */ + rc = ble_hs_hci_util_set_random_addr(rnd_addr); + if (rc != 0) { + goto done; + } + + memcpy(ble_hs_id_rnd, rnd_addr, BLE_DEV_ADDR_LEN); + +done: + ble_hs_unlock(); + return rc; +} +#endif + int ble_hs_id_set_rnd(const uint8_t *rnd_addr) { diff --git a/nimble/host/src/ble_hs_id_priv.h b/nimble/host/src/ble_hs_id_priv.h index aa2827d41..c031b9511 100644 --- a/nimble/host/src/ble_hs_id_priv.h +++ b/nimble/host/src/ble_hs_id_priv.h @@ -33,6 +33,10 @@ int ble_hs_id_use_addr(uint8_t addr_type); void ble_hs_id_reset(void); void ble_hs_id_rnd_reset(void); +#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) +bool ble_hs_is_rpa(uint8_t *addr, uint8_t addr_type); +int ble_hs_id_set_pseudo_rnd(const uint8_t *); +#endif #ifdef __cplusplus } #endif diff --git a/nimble/host/src/ble_hs_priv.h b/nimble/host/src/ble_hs_priv.h index d8949cbc9..89e678ff2 100644 --- a/nimble/host/src/ble_hs_priv.h +++ b/nimble/host/src/ble_hs_priv.h @@ -129,6 +129,11 @@ void ble_hs_notifications_sched(void); struct ble_npl_eventq *ble_hs_evq_get(void); void ble_hs_stop_init(void); +int +ble_sm_alg_encrypt(const uint8_t *key, const uint8_t *plaintext, + uint8_t *enc_data); + + struct ble_mqueue { STAILQ_HEAD(, os_mbuf_pkthdr) head; struct ble_npl_event ev; diff --git a/nimble/host/src/ble_hs_pvcy.c b/nimble/host/src/ble_hs_pvcy.c index dc09b51ba..f6af3df2c 100644 --- a/nimble/host/src/ble_hs_pvcy.c +++ b/nimble/host/src/ble_hs_pvcy.c @@ -21,6 +21,7 @@ #include #include "stats/stats.h" #include "ble_hs_priv.h" +#include "ble_hs_resolv_priv.h" static uint8_t ble_hs_pvcy_started; static uint8_t ble_hs_pvcy_irk[16]; @@ -36,6 +37,10 @@ ble_hs_pvcy_set_addr_timeout(uint16_t timeout) { struct ble_hci_le_set_rpa_tmo_cp cmd; +#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) + return ble_hs_resolv_set_rpa_tmo(timeout); +#endif + if (timeout == 0 || timeout > 0xA1B8) { return BLE_ERR_INV_HCI_CMD_PARMS; } @@ -47,6 +52,7 @@ ble_hs_pvcy_set_addr_timeout(uint16_t timeout) &cmd, sizeof(cmd), NULL, 0); } +#if (!MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)) static int ble_hs_pvcy_set_resolve_enabled(int enable) { @@ -58,11 +64,13 @@ ble_hs_pvcy_set_resolve_enabled(int enable) BLE_HCI_OCF_LE_SET_ADDR_RES_EN), &cmd, sizeof(cmd), NULL, 0); } +#endif int ble_hs_pvcy_remove_entry(uint8_t addr_type, const uint8_t *addr) { struct ble_hci_le_rmv_resolve_list_cp cmd; + int rc; if (addr_type > BLE_ADDR_RANDOM) { addr_type = addr_type % 2; @@ -70,12 +78,17 @@ ble_hs_pvcy_remove_entry(uint8_t addr_type, const uint8_t *addr) cmd.peer_addr_type = addr_type; memcpy(cmd.peer_id_addr, addr, BLE_DEV_ADDR_LEN); - - return ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, - BLE_HCI_OCF_LE_RMV_RESOLV_LIST), - &cmd, sizeof(cmd), NULL, 0); +#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) + rc = ble_hs_resolv_list_rmv(addr_type, &cmd.peer_id_addr[0]); +#else + rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, + BLE_HCI_OCF_LE_RMV_RESOLV_LIST), + &cmd, sizeof(cmd), NULL, 0); +#endif + return rc; } +#if (!MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)) static int ble_hs_pvcy_clear_entries(void) { @@ -83,13 +96,13 @@ ble_hs_pvcy_clear_entries(void) BLE_HCI_OCF_LE_CLR_RESOLV_LIST), NULL, 0, NULL, 0); } +#endif static int ble_hs_pvcy_add_entry_hci(const uint8_t *addr, uint8_t addr_type, const uint8_t *irk) { struct ble_hci_le_add_resolv_list_cp cmd; - ble_addr_t peer_addr; int rc; if (addr_type > BLE_ADDR_RANDOM) { @@ -101,6 +114,15 @@ ble_hs_pvcy_add_entry_hci(const uint8_t *addr, uint8_t addr_type, memcpy(cmd.local_irk, ble_hs_pvcy_irk, 16); memcpy(cmd.peer_irk, irk, 16); +#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) + rc = ble_hs_resolv_list_add((uint8_t *) &cmd); + if (rc != 0) { + return rc; + } + +#else + ble_addr_t peer_addr; + rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_ADD_RESOLV_LIST), &cmd, sizeof(cmd), NULL, 0); @@ -119,6 +141,7 @@ ble_hs_pvcy_add_entry_hci(const uint8_t *addr, uint8_t addr_type, if (rc != 0) { return rc; } +#endif return 0; } @@ -135,6 +158,9 @@ ble_hs_pvcy_add_entry(const uint8_t *addr, uint8_t addr_type, * list (Vol 2, Part E, 7.8.38). Stop all GAP procedures and temporarily * prevent any new ones from being started. */ +#if (MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)) + rc = ble_hs_pvcy_add_entry_hci(addr, addr_type, irk); +#else ble_gap_preempt(); /* Try to add the entry now that GAP is halted. */ @@ -143,6 +169,7 @@ ble_hs_pvcy_add_entry(const uint8_t *addr, uint8_t addr_type, /* Allow GAP procedures to be started again. */ ble_gap_preempt_done(); +#endif if (rc != 0) { STATS_INC(ble_hs_stats, pvcy_add_entry_fail); } @@ -159,6 +186,11 @@ ble_hs_pvcy_ensure_started(void) return 0; } +#if (MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)) + /*This is to be called only once*/ + ble_hs_resolv_init(); +#endif + /* Set up the periodic change of our RPA. */ rc = ble_hs_pvcy_set_addr_timeout(MYNEWT_VAL(BLE_RPA_TIMEOUT)); if (rc != 0) { @@ -187,6 +219,21 @@ ble_hs_pvcy_set_our_irk(const uint8_t *irk) if (memcmp(ble_hs_pvcy_irk, new_irk, 16) != 0) { memcpy(ble_hs_pvcy_irk, new_irk, 16); +#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) + if (irk != NULL) { + bool rpa_state = false; + + if ((rpa_state = ble_host_rpa_enabled()) == true) { + ble_hs_resolv_enable(0); + } + + ble_hs_resolv_list_clear_all(); + + if (rpa_state) { + ble_hs_resolv_enable(1); + } + } +#else rc = ble_hs_pvcy_set_resolve_enabled(0); if (rc != 0) { return rc; @@ -202,6 +249,7 @@ ble_hs_pvcy_set_our_irk(const uint8_t *irk) return rc; } +#endif /* * Add local IRK entry with 00:00:00:00:00:00 address. This entry will * be used to generate RPA for non-directed advertising if own_addr_type @@ -246,3 +294,33 @@ ble_hs_pvcy_set_mode(const ble_addr_t *addr, uint8_t priv_mode) BLE_HCI_OCF_LE_SET_PRIVACY_MODE), &cmd, sizeof(cmd), NULL, 0); } + +bool +ble_hs_pvcy_enabled(void) +{ + return ble_hs_pvcy_started; +} + +#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) +int +ble_hs_pvcy_rpa_config(uint8_t enable) +{ + int rc = 0; + + if (enable != 0) { + rc = ble_hs_pvcy_ensure_started(); + if (rc != 0) { + return rc; + } + + ble_hs_resolv_enable(true); + + /* Generate local RPA address and set it in controller */ + rc = ble_hs_gen_own_rpa_random(); + } else { + ble_hs_resolv_enable(false); + } + + return rc; +} +#endif diff --git a/nimble/host/src/ble_hs_pvcy_priv.h b/nimble/host/src/ble_hs_pvcy_priv.h index 7f0aa4b90..86157da0f 100644 --- a/nimble/host/src/ble_hs_pvcy_priv.h +++ b/nimble/host/src/ble_hs_pvcy_priv.h @@ -35,6 +35,9 @@ int ble_hs_pvcy_add_entry(const uint8_t *addr, uint8_t addrtype, const uint8_t *irk); int ble_hs_pvcy_ensure_started(void); int ble_hs_pvcy_set_mode(const ble_addr_t *addr, uint8_t priv_mode); +#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) +bool ble_hs_pvcy_enabled(void); +#endif #ifdef __cplusplus } diff --git a/nimble/host/src/ble_hs_resolv.c b/nimble/host/src/ble_hs_resolv.c new file mode 100644 index 000000000..7f2413898 --- /dev/null +++ b/nimble/host/src/ble_hs_resolv.c @@ -0,0 +1,711 @@ +/* + * Copyright 2020 Espressif Systems (Shanghai) PTE LTD + * + * Licensed to the Apache Software Foundation (ASF) under one + * or more contributor license agreements. See the NOTICE file + * distributed with this work for additional information + * regarding copyright ownership. The ASF licenses this file + * to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance + * with the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY + * KIND, either express or implied. See the License for the + * specific language governing permissions and limitations + * under the License. + */ + +#include "syscfg/syscfg.h" +#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) + +#include +#include +#include "ble_hs_priv.h" +#include "host/ble_hs_id.h" +#include "nimble/ble.h" +#include "nimble/nimble_opt.h" +#include "ble_hs_resolv_priv.h" +#include "store/config/ble_store_config.h" +#include "../store/config/src/ble_store_config_priv.h" + +/* Resolve list size, additional space to save local device's configuration */ +#define BLE_RESOLV_LIST_SIZE (MYNEWT_VAL(BLE_STORE_MAX_BONDS) + 1) +#define BLE_MAX_RPA_TIMEOUT_VAL 0xA1B8 + +static struct ble_hs_resolv_data g_ble_hs_resolv_data; +static struct ble_hs_resolv_entry g_ble_hs_resolv_list[BLE_RESOLV_LIST_SIZE]; +/* Allocate one extra space for peer_records than no. of Bonds, it will take + * care of storage overflow */ +static struct ble_hs_dev_records peer_dev_rec[BLE_RESOLV_LIST_SIZE]; +static int ble_store_num_peer_dev_rec; + +struct ble_hs_resolv_data { + uint8_t addr_res_enabled; + uint8_t rl_cnt; + uint32_t rpa_tmo; + struct ble_npl_callout rpa_timer; +}; + +/*** APIs for Peer Device Records. + * + * These Peer records are necessary to take care of Peers with RPA address when + * they are not yet added to Resolving list ***/ + +struct ble_hs_dev_records * +ble_rpa_get_peer_dev_records(void) +{ + return &peer_dev_rec[0]; +} + +int +ble_rpa_get_num_peer_dev_records(void) +{ + return ble_store_num_peer_dev_rec; +} + +void +ble_rpa_set_num_peer_dev_records(int num_rec) +{ + ble_store_num_peer_dev_rec = num_rec; +} + +int +ble_rpa_remove_peer_dev_rec(struct ble_hs_dev_records *p_dev_rec) +{ + int i; + + for (i = 0; i < ble_store_num_peer_dev_rec; i++) { + if (!(memcmp(p_dev_rec, &peer_dev_rec[i], sizeof(struct + ble_hs_dev_records)))) { + memset(&peer_dev_rec[i], 0, sizeof(struct ble_hs_dev_records)); + break; + } + } + + if (i >= ble_store_num_peer_dev_rec) { + return BLE_HS_EUNKNOWN; + } + + ble_store_num_peer_dev_rec--; + if ((i != ble_store_num_peer_dev_rec) && (ble_store_num_peer_dev_rec != 0)) { + memmove(&peer_dev_rec[i], &peer_dev_rec[i + 1], + (ble_store_num_peer_dev_rec - i + 1) * sizeof(struct ble_hs_dev_records )); + } + + BLE_HS_LOG(DEBUG, " RPA: removed device at index = %d, no. of peer records" + " = %d\n", i, ble_store_num_peer_dev_rec); + + ble_store_persist_peer_records(); + return 0; +} + +static void +ble_rpa_peer_dev_rec_clear_all(void) +{ + uint8_t i; + + /* As NVS record need to be deleted one by one, we loop through + * peer_records */ + for (i = 0; i < ble_store_num_peer_dev_rec; i++) { + ble_store_num_peer_dev_rec--; + + if ((i != ble_store_num_peer_dev_rec) && (ble_store_num_peer_dev_rec != 0)) { + memmove(&peer_dev_rec[i], &peer_dev_rec[i + 1], + (ble_store_num_peer_dev_rec - i + 1) * sizeof(struct ble_hs_dev_records )); + } + + ble_store_persist_peer_records(); + } + return; +} + +/* Find peer device record with the address value. + * + * @return peer_dev_record if the addr matches with either of rand_addr, + * pseudo_addr or identity_addr + * NULL otherwise + */ +struct ble_hs_dev_records * +ble_rpa_find_peer_dev_rec(uint8_t *addr) +{ + struct ble_hs_dev_records *p_dev_rec = &peer_dev_rec[0]; + uint8_t i; + + for (i = 0; i < ble_store_num_peer_dev_rec; i++, p_dev_rec++) { + if (p_dev_rec->rec_used) { + if (!(memcmp(p_dev_rec->rand_addr, addr, BLE_DEV_ADDR_LEN)) || + !(memcmp(p_dev_rec->pseudo_addr, addr, BLE_DEV_ADDR_LEN)) || + !(memcmp(p_dev_rec->identity_addr, addr, BLE_DEV_ADDR_LEN))) { + return p_dev_rec; + } + } + } + return NULL; +} + +/* Find peer device record with the IRK */ +static struct ble_hs_dev_records * +ble_rpa_find_peer_dev_by_irk(uint8_t *irk) +{ + struct ble_hs_dev_records *p_dev_rec = &peer_dev_rec[0]; + uint8_t i; + + for (i = 0; i < ble_store_num_peer_dev_rec; i++, p_dev_rec++) { + if ((p_dev_rec->rec_used) && + (!memcmp(irk, p_dev_rec->peer_sec.irk, 16))) { + return p_dev_rec; + } + } + return NULL; +} + +/* Find out if the peer record at perticular index resolves received peer + * address. + * + * @return true if the RPA is resolvable, false otherwise */ +static bool +is_rpa_resolvable_by_peer_rec(struct ble_hs_dev_records *p_dev_rec, uint8_t *peer_add) +{ + if (p_dev_rec->peer_sec.irk_present) { + if (ble_hs_resolv_rpa(peer_add, p_dev_rec->peer_sec.irk) == 0) { + return true; + } + } + return false; +} + +/* Add peer to peer device records. + * + * @return 0 if added successfully, + * BLE_HS_ESTORE_CAP if the no. of peer device records are exceeding + * maximum allowed value (No. of Bonds + 1) */ +int +ble_rpa_resolv_add_peer_rec(uint8_t *peer_addr) +{ + struct ble_hs_dev_records *p_dev_rec = + &peer_dev_rec[ble_store_num_peer_dev_rec]; + uint8_t idx = 0; + + while (p_dev_rec->rec_used) { + p_dev_rec++; + idx++; + if (idx > MYNEWT_VAL(BLE_STORE_MAX_BONDS)) { + return BLE_HS_ESTORE_CAP; + } + } + + p_dev_rec->rec_used = 1; + memcpy(p_dev_rec->pseudo_addr, peer_addr, BLE_DEV_ADDR_LEN); + memcpy(p_dev_rec->rand_addr, peer_addr, BLE_DEV_ADDR_LEN); + memcpy(p_dev_rec->identity_addr, peer_addr, BLE_DEV_ADDR_LEN); + ble_store_num_peer_dev_rec++; + + return 0; +} + +static struct ble_hs_resolv_entry * +ble_rpa_find_rl_from_peer_records(uint8_t *peer_addr, uint8_t *peer_addr_type) +{ + struct ble_hs_dev_records *p_dev_rec = NULL; + struct ble_hs_resolv_entry *rl = NULL; + int i; + int rc = 0; + + for (i = (ble_store_num_peer_dev_rec - 1); i >= 0; i--) { + p_dev_rec = &peer_dev_rec[i]; + /* If the record is not used, skip */ + if (!(p_dev_rec->rec_used)) { + continue; + } + + if (is_rpa_resolvable_by_peer_rec(p_dev_rec, peer_addr)) { + memcpy(p_dev_rec->rand_addr, peer_addr, BLE_DEV_ADDR_LEN); + rl = ble_hs_resolv_list_find(p_dev_rec->identity_addr); + if (rl) { + memcpy(peer_addr, p_dev_rec->identity_addr, BLE_DEV_ADDR_LEN); + *peer_addr_type = p_dev_rec->peer_sec.peer_addr.type; + } else if ((rl = ble_hs_resolv_list_find(p_dev_rec->pseudo_addr)) + != NULL) { + memcpy(peer_addr, p_dev_rec->identity_addr, BLE_DEV_ADDR_LEN); + *peer_addr_type = p_dev_rec->peer_sec.peer_addr.type; + } else { + /* Peer record exists, but RL does not + * exist, remove peer record */ + BLE_HS_LOG(DEBUG, "RPA resolvable but RL doesn't exist; remove" + " peer rec at index = %d \n", i); + rc = ble_rpa_remove_peer_dev_rec(p_dev_rec); + if (rc != 0) { + BLE_HS_LOG(ERROR, "Unexpected error; index exceeds max capacity\n"); + } + } + /* Break loop, unique entry */ + break; + } else { + /* As peer_dev_rec is added as soon as peer RPA + * is spotted, we might encounter duplications, + * remove peer_dev_rec here itself */ + if ((!memcmp(p_dev_rec->rand_addr, peer_addr, BLE_DEV_ADDR_LEN)) || + (!memcmp(p_dev_rec->pseudo_addr, peer_addr, BLE_DEV_ADDR_LEN)) || + (!memcmp(p_dev_rec->identity_addr, peer_addr, BLE_DEV_ADDR_LEN))) { + BLE_HS_LOG(DEBUG, "RPA NOT resolvable; remove" + " peer rec at index = %d \n", i); + rc = ble_rpa_remove_peer_dev_rec(p_dev_rec); + if (rc != 0) { + BLE_HS_LOG(ERROR, "Unexpected error; index exceeds max capacity\n"); + break; + } + } + } + } + + return rl; +} + +void +ble_rpa_replace_peer_params_with_rl(uint8_t *peer_addr, uint8_t *addr_type, + struct ble_hs_resolv_entry **rl) +{ + struct ble_hs_resolv_entry *rl_tmp = NULL; + bool is_rpa = ble_hs_is_rpa(peer_addr, *addr_type); + + if (is_rpa) { + ble_hs_log_flat_buf(peer_addr, BLE_DEV_ADDR_LEN); + rl_tmp = ble_hs_resolv_list_find(peer_addr); + + /* Try to find from your peer_device records, if RL doesn't + * exist */ + if (rl_tmp == NULL) { + rl_tmp = ble_rpa_find_rl_from_peer_records(peer_addr, addr_type); + } + } + + if (rl != NULL) { + *rl = rl_tmp; + } +} + +/**** APIs for Resolving List: ****/ + +/** + * Returns whether or not address resolution is enabled. + * + * @return uint8_t + */ +static uint8_t +is_ble_hs_resolv_enabled(void) +{ + return g_ble_hs_resolv_data.addr_res_enabled; +} + +/* Check if the Host based RPA is enabled */ +bool +ble_host_rpa_enabled(void) +{ + if (is_ble_hs_resolv_enabled() && ble_hs_pvcy_enabled()) { + return true; + } + return false; +} + +static void +ble_hs_rand_prand_get(uint8_t *prand) +{ + uint16_t sum; + uint8_t rc; + + while (1) { + /* Get 24 bits of random data */ + rc = ble_hs_hci_util_rand(prand, 3); + if (rc != 0) { + return; + } + + /* Prand cannot be all zeros or 1's. */ + sum = prand[0] + prand[1] + prand[2]; + if ((sum != 0) && (sum != (3 * 0xff))) { + break; + } + } + + /* Upper two bits must be 01 */ + prand[2] &= ~0xc0; + prand[2] |= 0x40; +} + +/** + * Called to generate a resolvable private address in rl structure + * + * @param rl + * @param local + */ +static void +ble_hs_resolv_gen_priv_addr(struct ble_hs_resolv_entry *rl, int local) +{ + uint8_t *irk = NULL; + uint8_t *prand = NULL; + struct ble_encryption_block ecb = {0}; + uint8_t *addr = 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_hs_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]; + + swap_in_place(ecb.key, 16); + swap_in_place(ecb.plain_text, 16); + + /* Calculate hash */ + if (ble_sm_alg_encrypt(ecb.key, ecb.plain_text, ecb.cipher_text) != 0) { + /* We can't do much here if the encryption fails */ + return; + } + swap_in_place(ecb.cipher_text, 16); + + addr[0] = ecb.cipher_text[15]; + addr[1] = ecb.cipher_text[14]; + addr[2] = ecb.cipher_text[13]; +} + +/* Called to generate RPA address and this address is set in controller as + * Random address. This is necessary in Host based privacy because controller is unaware of RPA + * address is being used */ +int +ble_hs_gen_own_rpa_random(void) +{ + struct ble_hs_resolv_entry *rl = &g_ble_hs_resolv_list[0]; + + ble_hs_resolv_gen_priv_addr(rl, 1); + return ble_hs_id_set_pseudo_rnd(rl->rl_local_rpa); +} + +/* Called to fetch local RPA address */ +uint8_t * +ble_hs_get_rpa_local(void) +{ + struct ble_hs_resolv_entry *rl = &g_ble_hs_resolv_list[0]; + return rl->rl_local_rpa; +} + +/** + * Called when the Resolvable private address timer expires. This timer + * is used to regenerate local and peers RPA's in the resolving list. + */ +static void +ble_hs_resolv_rpa_timer_cb(struct ble_npl_event *ev) +{ + if (ble_host_rpa_enabled()) { + BLE_HS_LOG(DEBUG, "RPA Timeout; start active adv & scan with new RPA\n"); + + ble_gap_preempt(); + /* Generate local RPA */ + ble_hs_gen_own_rpa_random(); + ble_npl_callout_reset(&g_ble_hs_resolv_data.rpa_timer, + (int32_t)g_ble_hs_resolv_data.rpa_tmo); + ble_gap_preempt_done(); + } + + return; +} + +static bool +is_irk_nonzero(uint8_t *irk) +{ + int i; + bool rc = false; + + for (i = 0; i < 16; ++i) { + if (*irk != 0) { + rc = true; + break; + } + ++irk; + } + + return rc; +} + +/** + * Used to determine if the device is on the resolving list. + * + * @param addr + * @param addr_type Public address (0) or random address (1) + * + * @return int 0: device is not on resolving list; otherwise the return value + * is the 'position' of the device in the resolving list (the index of the + * element). + */ +static int +ble_hs_is_on_resolv_list(uint8_t *addr, uint8_t addr_type) +{ + 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 ((rl->rl_addr_type == addr_type) && + (!memcmp(rl->rl_identity_addr, addr, BLE_DEV_ADDR_LEN))) { + return i; + } + ++rl; + } + + return 0; +} + +/** + * Used to find Resolving list entry with the provided address field. + * + * @param addr + * + * @return Pointer to resolving list entry or NULL if no entry found. + */ +struct ble_hs_resolv_entry * +ble_hs_resolv_list_find(uint8_t *addr) +{ + 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 (memcmp(rl->rl_identity_addr, addr, BLE_DEV_ADDR_LEN) == 0) { + return rl; + } + + if (memcmp(rl->rl_pseudo_id, addr, BLE_DEV_ADDR_LEN) == 0) { + return rl; + } + + if (memcmp(rl->rl_peer_rpa, addr, BLE_DEV_ADDR_LEN) == 0) { + return rl; + } + ++rl; + } + return NULL; +} + +/** + * Add a device to the resolving list + * + * @return int + */ +int +ble_hs_resolv_list_add(uint8_t *cmdbuf) +{ + uint8_t addr_type; + uint8_t *ident_addr; + struct ble_hs_resolv_entry *rl; + struct ble_hs_dev_records *p_dev_rec = NULL; + + /* Check if we have any open entries */ + if (g_ble_hs_resolv_data.rl_cnt >= BLE_RESOLV_LIST_SIZE) { + return BLE_HS_ENOMEM; + } + + addr_type = cmdbuf[0]; + ident_addr = cmdbuf + 1; + + if (ble_hs_is_on_resolv_list(ident_addr, addr_type)) { + return BLE_HS_EINVAL; + } + + rl = &g_ble_hs_resolv_list[g_ble_hs_resolv_data.rl_cnt]; + memset(rl, 0, sizeof(*rl)); + + rl->rl_addr_type = addr_type; + memcpy(rl->rl_identity_addr, ident_addr, BLE_DEV_ADDR_LEN); + swap_buf(rl->rl_peer_irk, cmdbuf + 7, 16); + swap_buf(rl->rl_local_irk, cmdbuf + 23, 16); + + if (is_irk_nonzero(rl->rl_peer_irk)) { + p_dev_rec = ble_rpa_find_peer_dev_by_irk(rl->rl_peer_irk); + + if (p_dev_rec != NULL) { + memcpy(p_dev_rec->identity_addr, ident_addr, 6); + memcpy(rl->rl_pseudo_id, p_dev_rec->pseudo_addr, 6); + } + } + + /* generate a local and peer RPAs now, those will be updated by timer + * when resolution is enabled + */ + ble_hs_resolv_gen_priv_addr(rl, 1); + ble_hs_resolv_gen_priv_addr(rl, 0); + ++(g_ble_hs_resolv_data.rl_cnt); + BLE_HS_LOG(DEBUG, "Device added to RL, Resolving list count = %d\n", g_ble_hs_resolv_data.rl_cnt); + + return 0; +} + +/** + * Remove a device from the resolving list + * + * @return int 0: success, BLE Host error code otherwise + */ +int +ble_hs_resolv_list_rmv(uint8_t addr_type, uint8_t *ident_addr) +{ + int position; + + /* Remove from IRK records */ + position = ble_hs_is_on_resolv_list(ident_addr, addr_type); + if (position) { + + memmove(&g_ble_hs_resolv_list[position], + &g_ble_hs_resolv_list[position + 1], + (g_ble_hs_resolv_data.rl_cnt - position) * sizeof (struct + ble_hs_resolv_entry)); + --g_ble_hs_resolv_data.rl_cnt; + + return 0; + } + + return BLE_HS_ENOENT; +} + +/** + * Clear the resolving list + */ +void +ble_hs_resolv_list_clear_all(void) +{ + g_ble_hs_resolv_data.rl_cnt = 0; + memset(g_ble_hs_resolv_list, 0, BLE_RESOLV_LIST_SIZE * sizeof(struct + ble_hs_resolv_entry)); + + /* Now delete peer device records as well */ + ble_rpa_peer_dev_rec_clear_all(); + + return; +} + +/** + * Called to enable or disable address resolution in the host + * + * @param bool + * + * @return int + */ +void +ble_hs_resolv_enable(bool enable) +{ + int32_t tmo; + + /* If we change state, we need to disable/enable the RPA timer */ + if (enable ^ g_ble_hs_resolv_data.addr_res_enabled) { + if (enable) { + tmo = (int32_t)g_ble_hs_resolv_data.rpa_tmo; + ble_npl_callout_reset(&g_ble_hs_resolv_data.rpa_timer, tmo); + } else { + ble_npl_callout_stop(&g_ble_hs_resolv_data.rpa_timer); + } + g_ble_hs_resolv_data.addr_res_enabled = enable; + } + + return; +} + +/** + * Set the resolvable private address timeout. + * + * @param cmdbuf + * + * @return int + */ +int +ble_hs_resolv_set_rpa_tmo(uint16_t tmo_secs) +{ + /* Though the check validates smaller timeout values, it is recommended to + * set it to enough bigger value (~15 minutes). There is no point in + * changing RPA address aggressively and ending up sacrificing normal BLE + * operations. Max RPA_TIMEOUT is ~11.5HRS (Spec v4.2, Vol 2, Part E, + * section 7.8.45) */ + if (!((tmo_secs > 0) && (tmo_secs <= BLE_MAX_RPA_TIMEOUT_VAL))) { + return BLE_HS_EINVAL; + } + + g_ble_hs_resolv_data.rpa_tmo = ble_npl_time_ms_to_ticks32(tmo_secs * 1000); + + /* If resolving is not enabled, we are done here. */ + if (!is_ble_hs_resolv_enabled()) { + return 0; + } + + /* Reset timeout if resolving is enabled */ + ble_npl_callout_reset(&g_ble_hs_resolv_data.rpa_timer, + (int32_t)g_ble_hs_resolv_data.rpa_tmo); + + return 0; +} + +/** + * Resolve a Resolvable Private Address + * + * @param rpa + * @param index + * + * @return int + */ +int +ble_hs_resolv_rpa(uint8_t *rpa, uint8_t *irk) +{ + int rc; + struct ble_encryption_block ecb = {0}; + + if (!(is_irk_nonzero(irk))) { + return BLE_HS_EINVAL; + } + + swap_buf(ecb.key, irk, 16); + memset(ecb.plain_text, 0, 16); + + ecb.plain_text[15] = rpa[3]; + ecb.plain_text[14] = rpa[4]; + ecb.plain_text[13] = rpa[5]; + + swap_in_place(ecb.plain_text, 16); + + /* Send the data to ble_sm_alg_encrypt in little-endian style */ + rc = ble_sm_alg_encrypt(ecb.key, ecb.plain_text, ecb.cipher_text); + if (rc != 0) { + return rc; + } + swap_in_place(ecb.cipher_text, 16); + + if ((ecb.cipher_text[15] == rpa[0]) && (ecb.cipher_text[14] == rpa[1]) && + (ecb.cipher_text[13] == rpa[2])) { + rc = 0; + } else { + rc = BLE_HS_ENOENT; + } + + return rc; +} + +void ble_hs_resolv_init(void) +{ + g_ble_hs_resolv_data.rpa_tmo = ble_npl_time_ms_to_ticks32(MYNEWT_VAL(BLE_RPA_TIMEOUT) * 1000); + + ble_npl_callout_init(&g_ble_hs_resolv_data.rpa_timer, + ble_hs_evq_get(), + ble_hs_resolv_rpa_timer_cb, + NULL); +} + +#endif /* if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) */ diff --git a/nimble/host/src/ble_hs_resolv_priv.h b/nimble/host/src/ble_hs_resolv_priv.h new file mode 100644 index 000000000..65e0f6e95 --- /dev/null +++ b/nimble/host/src/ble_hs_resolv_priv.h @@ -0,0 +1,106 @@ +/* + * Copyright 2020 Espressif Systems (Shanghai) PTE LTD + * + * Licensed to the Apache Software Foundation (ASF) under one + * or more contributor license agreements. See the NOTICE file + * distributed with this work for additional information + * regarding copyright ownership. The ASF licenses this file + * to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance + * with the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY + * KIND, either express or implied. See the License for the + * specific language governing permissions and limitations + * under the License. + */ + +#ifdef __cplusplus +extern "C" { +#endif + +#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) +/* + * An entry in the resolving list. + */ +struct ble_hs_resolv_entry { + uint8_t rl_addr_type; + uint8_t rl_local_irk[16]; + uint8_t rl_peer_irk[16]; + uint8_t rl_identity_addr[BLE_DEV_ADDR_LEN]; + uint8_t rl_pseudo_id[BLE_DEV_ADDR_LEN]; + uint8_t rl_local_rpa[BLE_DEV_ADDR_LEN]; + uint8_t rl_peer_rpa[BLE_DEV_ADDR_LEN]; +}; + +/* Persist peer records in NVS. XXX Need to handle this in `store` module */ +int ble_store_persist_peer_records(void); + +struct ble_hs_peer_sec { + ble_addr_t peer_addr; + uint8_t irk[16]; + uint8_t irk_present: 1; +}; +/* + * BLE host peer device record, this helps in storing peer RPA before bond is + * created and IRKs are exchanged. + */ +struct ble_hs_dev_records { + bool rec_used; + uint8_t pseudo_addr[BLE_DEV_ADDR_LEN]; + uint8_t rand_addr[BLE_DEV_ADDR_LEN]; + uint8_t identity_addr[BLE_DEV_ADDR_LEN]; + struct ble_hs_peer_sec peer_sec; +}; + +/* Add a device to the resolving list */ +int ble_hs_resolv_list_add(uint8_t *cmdbuf); +int ble_hs_gen_own_rpa_random(void); +uint8_t *ble_hs_get_rpa_local(void); + +/* Remove a device from the resolving list */ +int ble_hs_resolv_list_rmv(uint8_t, uint8_t *); +/* Clear the resolving list and peer dev record */ +void ble_hs_resolv_list_clear_all(void); + +/* Address resolution enable command */ +void ble_hs_resolv_enable(bool); + +/* Finds 'addr' in resolving list. Doesnt check if address resolution enabled */ +struct ble_hs_resolv_entry * +ble_hs_resolv_list_find(uint8_t *addr); + +/* Returns true if host based RPA (privacy) is enabled */ +bool ble_host_rpa_enabled(void); + +/* Searches peer device records (RPA) and fetches matching RL, peer_address + * into input parameters if RL is found */ +void +ble_rpa_replace_peer_params_with_rl(uint8_t *, uint8_t *, struct ble_hs_resolv_entry **); + +int ble_rpa_resolv_add_peer_rec(uint8_t *); + +struct ble_hs_dev_records *ble_rpa_get_peer_dev_records(void); +int ble_rpa_get_num_peer_dev_records(void); +void ble_rpa_set_num_peer_dev_records(int); +int ble_rpa_remove_peer_dev_rec(struct ble_hs_dev_records *); +struct ble_hs_dev_records *ble_rpa_find_peer_dev_rec(uint8_t *); + +/* Set the resolvable private address timeout */ +int ble_hs_resolv_set_rpa_tmo(uint16_t); + +/* Resolve a resolvable private address */ +int ble_hs_resolv_rpa(uint8_t *rpa, uint8_t *irk); + +/* Initialize resolv*/ +void ble_hs_resolv_init(void); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/nimble/host/src/ble_sm.c b/nimble/host/src/ble_sm.c index 831cf0fcb..5f0194dbb 100644 --- a/nimble/host/src/ble_sm.c +++ b/nimble/host/src/ble_sm.c @@ -47,6 +47,8 @@ #include "nimble/nimble_opt.h" #include "host/ble_sm.h" #include "ble_hs_priv.h" +#include "ble_hs_resolv_priv.h" +#include "../store/config/src/ble_store_config_priv.h" #if NIMBLE_BLE_CONNECT @@ -563,6 +565,29 @@ ble_sm_persist_keys(struct ble_sm_proc *proc) } identity_ev = 1; +#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) + if (ble_host_rpa_enabled()) + { + struct ble_hs_dev_records *p_dev_rec = + ble_rpa_find_peer_dev_rec(conn->bhc_peer_rpa_addr.val); + if (p_dev_rec == NULL) { + if (!ble_rpa_resolv_add_peer_rec(conn->bhc_peer_rpa_addr.val)) { + p_dev_rec = ble_rpa_find_peer_dev_rec(conn->bhc_peer_rpa_addr.val); + } + } + + if (p_dev_rec != NULL) { + /* Once bonded, copy the peer device records */ + swap_buf(p_dev_rec->peer_sec.irk, proc->peer_keys.irk, 16); + p_dev_rec->peer_sec.irk_present = proc->peer_keys.irk_valid; + memcpy(p_dev_rec->peer_sec.peer_addr.val, + proc->peer_keys.addr, 6); + p_dev_rec->peer_sec.peer_addr.type = proc->peer_keys.addr_type; + + ble_store_persist_peer_records(); + } + } +#endif } } else { peer_addr = conn->bhc_peer_addr; @@ -2218,6 +2243,19 @@ ble_sm_key_exch_exec(struct ble_sm_proc *proc, struct ble_sm_result *res, conn = ble_hs_conn_find_assert(proc->conn_handle); ble_hs_conn_addrs(conn, &addrs); +#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) + /* Check if the privacy is enabled, change the address accordingly + * Overriding the addrs field to get the ID address remain unique + * (public address) */ + if (ble_host_rpa_enabled()) + { + uint8_t *local_id_rpa = NULL; + + ble_hs_id_addr(BLE_ADDR_PUBLIC, (void *) &local_id_rpa, NULL); + memcpy(addrs.our_id_addr.val, local_id_rpa, 6); + addrs.our_id_addr.type = BLE_ADDR_PUBLIC; + } +#endif addr_info->addr_type = addrs.our_id_addr.type; memcpy(addr_info->bd_addr, addrs.our_id_addr.val, 6); diff --git a/nimble/host/src/ble_sm_alg.c b/nimble/host/src/ble_sm_alg.c index 2476d0f3c..94349955a 100644 --- a/nimble/host/src/ble_sm_alg.c +++ b/nimble/host/src/ble_sm_alg.c @@ -76,7 +76,7 @@ ble_sm_alg_xor_128(const uint8_t *p, const uint8_t *q, uint8_t *r) } } -static int +int ble_sm_alg_encrypt(const uint8_t *key, const uint8_t *plaintext, uint8_t *enc_data) { diff --git a/nimble/host/src/ble_store_util.c b/nimble/host/src/ble_store_util.c index 508fa00db..44affa543 100644 --- a/nimble/host/src/ble_store_util.c +++ b/nimble/host/src/ble_store_util.c @@ -19,6 +19,7 @@ #include "host/ble_store.h" #include "ble_hs_priv.h" +#include "ble_hs_resolv_priv.h" struct ble_store_util_peer_set { ble_addr_t *peer_id_addrs; @@ -132,6 +133,25 @@ ble_store_util_delete_peer(const ble_addr_t *peer_id_addr) return rc; } +#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) + struct ble_hs_dev_records *peer_rec = + ble_rpa_find_peer_dev_rec(key.sec.peer_addr.val); + + if (peer_rec != NULL) { + rc = ble_hs_resolv_list_rmv(peer_rec->peer_sec.peer_addr.type, + peer_rec->peer_sec.peer_addr.val); + if (rc != 0) { + /* We can't do anything much here, continue with removing from peer_record */ + BLE_HS_LOG(DEBUG, "Peer Device was not removed from RL \n"); + } + + rc = ble_rpa_remove_peer_dev_rec(peer_rec); + if (rc != 0) { + return rc; + } + } +#endif + return 0; } diff --git a/nimble/host/store/config/src/ble_store_config_priv.h b/nimble/host/store/config/src/ble_store_config_priv.h index bae90e976..61b43c53c 100644 --- a/nimble/host/store/config/src/ble_store_config_priv.h +++ b/nimble/host/store/config/src/ble_store_config_priv.h @@ -50,6 +50,9 @@ static inline int ble_store_config_persist_peer_secs(void) { return 0; } static inline int ble_store_config_persist_cccds(void) { return 0; } static inline void ble_store_config_conf_init(void) { } +#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) +static inline int ble_store_persist_peer_records(void) { return 0; } +#endif #endif /* MYNEWT_VAL(BLE_STORE_CONFIG_PERSIST) */ #ifdef __cplusplus diff --git a/nimble/host/store/config/src/ble_store_nvs.c b/nimble/host/store/config/src/ble_store_nvs.c index b38e4cfd7..8cbb231b6 100644 --- a/nimble/host/store/config/src/ble_store_nvs.c +++ b/nimble/host/store/config/src/ble_store_nvs.c @@ -32,11 +32,14 @@ #include "ble_store_config_priv.h" #include "esp_log.h" #include "nvs.h" +#include "../../../src/ble_hs_resolv_priv.h" + #define NIMBLE_NVS_STR_NAME_MAX_LEN 16 #define NIMBLE_NVS_PEER_SEC_KEY "peer_sec" #define NIMBLE_NVS_OUR_SEC_KEY "our_sec" #define NIMBLE_NVS_CCCD_SEC_KEY "cccd_sec" +#define NIMBLE_NVS_PEER_RECORDS_KEY "p_dev_rec" #define NIMBLE_NVS_NAMESPACE "nimble_bond" static const char *TAG = "NIMBLE_NVS"; @@ -48,12 +51,16 @@ static const char *TAG = "NIMBLE_NVS"; static void get_nvs_key_string(int obj_type, int index, char *key_string) { - if (obj_type == BLE_STORE_OBJ_TYPE_PEER_SEC) { - sprintf(key_string, "%s_%d", NIMBLE_NVS_PEER_SEC_KEY, index); - } else if (obj_type == BLE_STORE_OBJ_TYPE_OUR_SEC) { - sprintf(key_string, "%s_%d", NIMBLE_NVS_OUR_SEC_KEY, index); + if (obj_type == BLE_STORE_OBJ_TYPE_PEER_DEV_REC) { + sprintf(key_string, "%s_%d", NIMBLE_NVS_PEER_RECORDS_KEY, index); } else { - sprintf(key_string, "%s_%d", NIMBLE_NVS_CCCD_SEC_KEY, index); + if (obj_type == BLE_STORE_OBJ_TYPE_PEER_SEC) { + sprintf(key_string, "%s_%d", NIMBLE_NVS_PEER_SEC_KEY, index); + } else if (obj_type == BLE_STORE_OBJ_TYPE_OUR_SEC) { + sprintf(key_string, "%s_%d", NIMBLE_NVS_OUR_SEC_KEY, index); + } else { + sprintf(key_string, "%s_%d", NIMBLE_NVS_CCCD_SEC_KEY, index); + } } } @@ -79,18 +86,53 @@ get_nvs_matching_index(void *nvs_val, void *db_list, int db_num, size_t } static int -get_nvs_max_bonds(int obj_type) +get_nvs_max_obj_value(int obj_type) { - if (obj_type == BLE_STORE_OBJ_TYPE_CCCD) { - return MYNEWT_VAL(BLE_STORE_MAX_CCCDS); + /* If host based privacy is enabled */ + if (obj_type == BLE_STORE_OBJ_TYPE_PEER_DEV_REC) { + return (MYNEWT_VAL(BLE_STORE_MAX_BONDS) + 1); } else { - return MYNEWT_VAL(BLE_STORE_MAX_BONDS); + if (obj_type == BLE_STORE_OBJ_TYPE_CCCD) { + return MYNEWT_VAL(BLE_STORE_MAX_CCCDS); + } else { + return MYNEWT_VAL(BLE_STORE_MAX_BONDS); + } } } /***************************************************************************** * $ NVS * *****************************************************************************/ +#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) +static int +get_nvs_peer_record(char *key_string, struct ble_hs_dev_records *p_dev_rec) +{ + esp_err_t err; + size_t required_size = 0; + nvs_handle_t nimble_handle; + + err = nvs_open(NIMBLE_NVS_NAMESPACE, NVS_READWRITE, &nimble_handle); + if (err != ESP_OK) { + ESP_LOGE(TAG, "NVS open operation failed"); + return BLE_HS_ESTORE_FAIL; + } + + err = nvs_get_blob(nimble_handle, key_string, NULL, &required_size); + + /* if Address pointer for value is NULL, filling of value not needed */ + if (err != ESP_OK || p_dev_rec == NULL) { + goto end; + } + + err = nvs_get_blob(nimble_handle, key_string, p_dev_rec, + &required_size); + +end: + nvs_close(nimble_handle); + return err; +} +#endif + static int get_nvs_db_value(int obj_type, char *key_string, union ble_store_value *val) { @@ -136,16 +178,21 @@ static int get_nvs_db_attribute(int obj_type, bool empty, void *value, int num_value) { union ble_store_value cur = {0}; + struct ble_hs_dev_records p_dev_rec = {0}; esp_err_t err; int i, count = 0, max_limit = 0; char key_string[NIMBLE_NVS_STR_NAME_MAX_LEN]; - max_limit = get_nvs_max_bonds(obj_type); + max_limit = get_nvs_max_obj_value(obj_type); for (i = 1; i <= max_limit; i++) { get_nvs_key_string(obj_type, i, key_string); - err = get_nvs_db_value(obj_type, key_string, &cur); + if (obj_type != BLE_STORE_OBJ_TYPE_PEER_DEV_REC) { + err = get_nvs_db_value(obj_type, key_string, &cur); + } else { + err = get_nvs_peer_record(key_string, &p_dev_rec); + } /* Check if the user is searching for empty index to write to */ if (err == ESP_ERR_NVS_NOT_FOUND) { if (empty) { @@ -157,12 +204,17 @@ get_nvs_db_attribute(int obj_type, bool empty, void *value, int num_value) /* If user has provided value, then the purpose is to find * non-matching entry from NVS */ if (value) { - if (obj_type == BLE_STORE_OBJ_TYPE_CCCD) { - err = get_nvs_matching_index(&cur.sec, value, num_value, - sizeof(struct ble_store_value_sec)); + if (obj_type == BLE_STORE_OBJ_TYPE_PEER_DEV_REC) { + err = get_nvs_matching_index(&p_dev_rec, value, num_value, + sizeof(struct ble_hs_dev_records)); } else { - err = get_nvs_matching_index(&cur.sec, value, num_value, - sizeof(struct ble_store_value_cccd)); + if (obj_type == BLE_STORE_OBJ_TYPE_CCCD) { + err = get_nvs_matching_index(&cur.sec, value, num_value, + sizeof(struct ble_store_value_sec)); + } else { + err = get_nvs_matching_index(&cur.sec, value, num_value, + sizeof(struct ble_store_value_cccd)); + } } /* If found non-matching/odd entry of NVS with entries in the * internal database, return NVS index so can be deleted */ @@ -194,8 +246,8 @@ ble_nvs_delete_value(int obj_type, int8_t index) nvs_handle_t nimble_handle; char key_string[NIMBLE_NVS_STR_NAME_MAX_LEN]; - if (index > get_nvs_max_bonds(obj_type)) { - ESP_LOGD(TAG, "Invalid index provided to delete"); + if (index > get_nvs_max_obj_value(obj_type)) { + ESP_LOGE(TAG, "Invalid index provided to delete"); return BLE_HS_EUNKNOWN; } @@ -271,7 +323,7 @@ ble_store_nvs_write(int obj_type, const union ble_store_value *val) if (write_key_index == -1) { ESP_LOGE(TAG, "NVS operation failed !!"); return BLE_HS_ESTORE_FAIL; - } else if (write_key_index > get_nvs_max_bonds(obj_type)) { + } else if (write_key_index > get_nvs_max_obj_value(obj_type)) { /* bare-bone config code will take care of capacity overflow event, * however another check added for consistency */ @@ -290,36 +342,84 @@ ble_store_nvs_write(int obj_type, const union ble_store_value *val) } } +#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) +/* If Host based privacy is enabled */ +static int +ble_store_nvs_peer_records(int obj_type, const struct ble_hs_dev_records *p_dev_rec) +{ + char key_string[NIMBLE_NVS_STR_NAME_MAX_LEN]; + int8_t write_key_index = 0; + + write_key_index = get_nvs_db_attribute(obj_type, 1, NULL, 0); + if (write_key_index == -1) { + ESP_LOGE(TAG, "NVS operation failed !!"); + return BLE_HS_ESTORE_FAIL; + } else if (write_key_index > get_nvs_max_obj_value(obj_type)) { + + /* bare-bone config code will take care of capacity overflow event, + * however another check added for consistency */ + ESP_LOGD(TAG, "NVS size overflow."); + return BLE_HS_ESTORE_CAP; + } + + get_nvs_key_string(obj_type, write_key_index, key_string); + + return ble_nvs_write_key_value(key_string, p_dev_rec, sizeof(struct + ble_hs_dev_records)); +} +#endif + static int populate_db_from_nvs(int obj_type, void *dst, int *db_num) { uint8_t *db_item = (uint8_t *)dst; union ble_store_value cur = {0}; + struct ble_hs_dev_records p_dev_rec = {0}; + esp_err_t err; int i; char key_string[NIMBLE_NVS_STR_NAME_MAX_LEN]; - for (i = 1; i <= get_nvs_max_bonds(obj_type); i++) { + for (i = 1; i <= get_nvs_max_obj_value(obj_type); i++) { get_nvs_key_string(obj_type, i, key_string); - err = get_nvs_db_value(obj_type, key_string, &cur); - if (err == ESP_ERR_NVS_NOT_FOUND) { - continue; - } else if (err != ESP_OK) { - ESP_LOGE(TAG, "NVS read operation failed !!"); - return -1; + if (obj_type != BLE_STORE_OBJ_TYPE_PEER_DEV_REC) { + + err = get_nvs_db_value(obj_type, key_string, &cur); + if (err == ESP_ERR_NVS_NOT_FOUND) { + continue; + } else if (err != ESP_OK) { + ESP_LOGE(TAG, "NVS read operation failed !!"); + return -1; + } + } else { + err = get_nvs_peer_record(key_string, &p_dev_rec); + if (err == ESP_ERR_NVS_NOT_FOUND) { + continue; + } else if (err != ESP_OK) { + ESP_LOGE(TAG, "NVS read operation failed !!"); + return -1; + } } + /* NVS index has data, fill up the ram db with it */ - if (obj_type == BLE_STORE_OBJ_TYPE_CCCD) { - ESP_LOGD(TAG, "CCCD in RAM is filled up from NVS index = %d", i); - memcpy(db_item, &cur.cccd, sizeof(struct ble_store_value_cccd)); - db_item += sizeof(struct ble_store_value_cccd); + if (obj_type == BLE_STORE_OBJ_TYPE_PEER_DEV_REC) { + ESP_LOGD(TAG, "Peer dev records filled from NVS index = %d", i); + memcpy(db_item, &p_dev_rec, sizeof(struct ble_hs_dev_records)); + db_item += sizeof(struct ble_hs_dev_records); (*db_num)++; } else { - ESP_LOGD(TAG, "KEY in RAM is filled up from NVS index = %d", i); - memcpy(db_item, &cur.sec, sizeof(struct ble_store_value_sec)); - db_item += sizeof(struct ble_store_value_sec); - (*db_num)++; + if (obj_type == BLE_STORE_OBJ_TYPE_CCCD) { + ESP_LOGD(TAG, "CCCD in RAM is filled up from NVS index = %d", i); + memcpy(db_item, &cur.cccd, sizeof(struct ble_store_value_cccd)); + db_item += sizeof(struct ble_store_value_cccd); + (*db_num)++; + } else { + ESP_LOGD(TAG, "KEY in RAM is filled up from NVS index = %d", i); + memcpy(db_item, &cur.sec, sizeof(struct ble_store_value_sec)); + db_item += sizeof(struct ble_store_value_sec); + (*db_num)++; + } } } return 0; @@ -362,6 +462,28 @@ ble_nvs_restore_sec_keys(void) return 0; } +#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) +static int +ble_nvs_restore_peer_records(void) +{ + esp_err_t err; + int ble_store_num_peer_dev_rec = 0; + struct ble_hs_dev_records *peer_dev_rec = ble_rpa_get_peer_dev_records(); + + err = populate_db_from_nvs(BLE_STORE_OBJ_TYPE_PEER_DEV_REC, peer_dev_rec, + &ble_store_num_peer_dev_rec); + if (err != ESP_OK) { + ESP_LOGE(TAG, "NVS operation failed fetching 'Peer Dev Records'"); + return err; + } + + ble_rpa_set_num_peer_dev_records(ble_store_num_peer_dev_rec); + ESP_LOGD(TAG, "peer_dev_rec restored %d records", ble_store_num_peer_dev_rec); + + return 0; +} +#endif + int ble_store_config_persist_cccds(void) { int nvs_count, nvs_idx; @@ -440,14 +562,50 @@ int ble_store_config_persist_our_secs(void) return 0; } +#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) +int ble_store_persist_peer_records(void) +{ + int nvs_count, nvs_idx; + struct ble_hs_dev_records peer_rec; + int ble_store_num_peer_dev_rec = ble_rpa_get_num_peer_dev_records(); + struct ble_hs_dev_records *peer_dev_rec = ble_rpa_get_peer_dev_records(); + + nvs_count = get_nvs_db_attribute(BLE_STORE_OBJ_TYPE_PEER_DEV_REC, 0, NULL, 0); + if (nvs_count < ble_store_num_peer_dev_rec) { + /* NVS db count less than RAM count, write operation */ + ESP_LOGD(TAG, "Persisting peer dev record to NVS..."); + peer_rec = peer_dev_rec[ble_store_num_peer_dev_rec - 1]; + return ble_store_nvs_peer_records(BLE_STORE_OBJ_TYPE_PEER_DEV_REC, &peer_rec); + } else if (nvs_count > ble_store_num_peer_dev_rec) { + /* NVS db count more than RAM count, delete operation */ + nvs_idx = get_nvs_db_attribute(BLE_STORE_OBJ_TYPE_PEER_DEV_REC, 0, + peer_dev_rec, + ble_store_num_peer_dev_rec); + if (nvs_idx == -1) { + ESP_LOGE(TAG, "NVS delete operation failed for peer records"); + return BLE_HS_ESTORE_FAIL; + } + ESP_LOGD(TAG, "Deleting peer record, nvs idx = %d", nvs_idx); + return ble_nvs_delete_value(BLE_STORE_OBJ_TYPE_PEER_DEV_REC, nvs_idx); + } + return 0; +} +#endif + void ble_store_config_conf_init(void) { - esp_err_t err; + int err; err = ble_nvs_restore_sec_keys(); - if (err != ESP_OK) { + if (err != 0) { ESP_LOGE(TAG, "NVS operation failed, can't retrieve the bonding info"); } +#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) + err = ble_nvs_restore_peer_records(); + if (err != 0) { + ESP_LOGE(TAG, "NVS operation failed, can't retrieve the peer records"); + } +#endif } /***************************************************************************************/