espressif/esp-nimble
1
0
Fork 0
mirror of https://github.com/espressif/esp-nimble.git synced 2026-11-10 06:20:34 +00:00
Code Issues Packages Projects Releases Wiki Activity

NimBLE: Add Host based privacy (RPA) support

Browse Source
This commit is contained in:
Prasad Alatkar
2024-02-14 14:51:37 +05:30
committed by Abhinav Kudnar
parent 3b44ea024d
commit 5c79268c74
17 changed files with 1375 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files
nimble/host/include/host/ble_hs_pvcy.h
+40
View File
@@ -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
nimble/host/include/host/ble_store.h
+4 -3
View File
@@ -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
nimble/host/src/ble_gap.c
+30
View File
@@ -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;
nimble/host/src/ble_hs_conn.c
+30
View File
@@ -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:
nimble/host/src/ble_hs_hci_evt.c
+39
View File
@@ -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;
nimble/host/src/ble_hs_id.c
+64
View File
@@ -20,12 +20,25 @@
#include <string.h>
#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)
{
nimble/host/src/ble_hs_id_priv.h
+4
View File
@@ -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
nimble/host/src/ble_hs_priv.h
+5
View File
@@ -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;
nimble/host/src/ble_hs_pvcy.c
+83 -5
View File
@@ -21,6 +21,7 @@
#include <string.h>
#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
nimble/host/src/ble_hs_pvcy_priv.h
+3
View File
@@ -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
}
nimble/host/src/ble_hs_resolv.c
+711
View File
@@ -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 <stdint.h>
#include <string.h>
#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) */
nimble/host/src/ble_hs_resolv_priv.h
+106
View File
@@ -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
nimble/host/src/ble_sm.c
+38
View File
@@ -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);
nimble/host/src/ble_sm_alg.c
+1 -1
View File
@@ -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)
{
nimble/host/src/ble_store_util.c
+20
View File
@@ -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;
}
nimble/host/store/config/src/ble_store_config_priv.h
+3
View File
@@ -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
nimble/host/store/config/src/ble_store_nvs.c
+194 -36
View File
@@ -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
}
/***************************************************************************************/