diff --git a/nimble/host/include/host/ble_store.h b/nimble/host/include/host/ble_store.h index 0586f67bc..6ee0f33a0 100644 --- a/nimble/host/include/host/ble_store.h +++ b/nimble/host/include/host/ble_store.h @@ -35,6 +35,7 @@ extern "C" { #if MYNEWT_VAL(ENC_ADV_DATA) #define BLE_STORE_OBJ_TYPE_ENC_ADV_DATA 5 #endif +#define BLE_STORE_OBJ_TYPE_PEER_ADDR 6 /** Failed to persist record; insufficient storage capacity. */ #define BLE_STORE_EVENT_OVERFLOW 1 @@ -144,6 +145,15 @@ struct ble_store_value_ead { }; #endif +struct ble_store_key_rpa_rec{ + ble_addr_t peer_rpa_addr; + uint8_t idx; +}; +struct ble_store_value_rpa_rec{ + ble_addr_t peer_rpa_addr; + ble_addr_t peer_addr; +}; + /** * Used as a key for store lookups. This union must be accompanied by an * object type code to indicate which field is valid. @@ -154,6 +164,7 @@ union ble_store_key { #if MYNEWT_VAL(ENC_ADV_DATA) struct ble_store_key_ead ead; #endif + struct ble_store_key_rpa_rec rpa_rec; }; /** @@ -166,6 +177,7 @@ union ble_store_value { #if MYNEWT_VAL(ENC_ADV_DATA) struct ble_store_value_ead ead; #endif + struct ble_store_value_rpa_rec rpa_rec; }; struct ble_store_status_event { @@ -345,6 +357,14 @@ void ble_store_key_from_value_ead(struct ble_store_key_ead *out_key, const struct ble_store_value_ead *value); #endif +/* rpa mapping*/ +int ble_store_read_rpa_rec(const struct ble_store_key_rpa_rec *key, + struct ble_store_value_rpa_rec *out_value); +int ble_store_write_rpa_rec(const struct ble_store_value_rpa_rec *value); +int ble_store_delete_rpa_rec(const struct ble_store_key_rpa_rec *key); +void ble_store_key_from_value_rpa_rec(struct ble_store_key_rpa_rec *out_key, + const struct ble_store_value_rpa_rec *value); +/* rpa mapping*/ void ble_store_key_from_value(int obj_type, union ble_store_key *out_key, const union ble_store_value *value); diff --git a/nimble/host/src/ble_gap.c b/nimble/host/src/ble_gap.c index 5ec7eb923..309eca097 100644 --- a/nimble/host/src/ble_gap.c +++ b/nimble/host/src/ble_gap.c @@ -6600,8 +6600,12 @@ ble_gap_unpair(const ble_addr_t *peer_addr) { #if NIMBLE_BLE_SM int rc; + int ltk_rc = 0; + int irk_rc = 0; struct ble_hs_conn *conn; - + union ble_store_value value; + union ble_store_key key; + ble_addr_t *new_addr = (ble_addr_t *) peer_addr; if (!ble_hs_is_enabled()) { return BLE_HS_EDISABLED; } @@ -6619,13 +6623,45 @@ ble_gap_unpair(const ble_addr_t *peer_addr) ble_hs_unlock(); - rc = ble_hs_pvcy_remove_entry(peer_addr->type, - peer_addr->val); - if (rc != 0) { - return rc; + memset(&key,0,sizeof(key)); + key.rpa_rec.peer_rpa_addr = *peer_addr; + + rc = ble_store_read(BLE_STORE_OBJ_TYPE_PEER_ADDR, &key, &value); + + if (!rc) { + new_addr = &(value.rpa_rec.peer_addr); } - return ble_store_util_delete_peer(peer_addr); + memset(&key, 0, sizeof(key)); + key.sec.peer_addr = *new_addr; + + rc = ble_store_read(BLE_STORE_OBJ_TYPE_PEER_SEC, &key, &value); + + // Checking if the device is in ble_store + if (!rc) { + if (value.sec.irk_present) { + // Delete the IRK as it is Distributed + irk_rc = ble_hs_pvcy_remove_entry(key.sec.peer_addr.type, + key.sec.peer_addr.val); + if (irk_rc != 0) { + BLE_HS_LOG(ERROR, "Error while removing IRK\n"); + } + } + + if (value.sec.ltk_present) { + // Delete the Peer record from store as LTK is present + ltk_rc = ble_store_util_delete_peer(&key.sec.peer_addr); + if (ltk_rc != 0) { + BLE_HS_LOG(ERROR, "Error while removing LTK\n"); + } + } + } + else { + BLE_HS_LOG(ERROR,"No record found for the given address in ble store"); + return rc; + } + + return 0; #else return BLE_HS_ENOTSUP; #endif diff --git a/nimble/host/src/ble_sm.c b/nimble/host/src/ble_sm.c index cfdeb8797..e82181531 100644 --- a/nimble/host/src/ble_sm.c +++ b/nimble/host/src/ble_sm.c @@ -527,6 +527,7 @@ static void ble_sm_persist_keys(struct ble_sm_proc *proc) { struct ble_store_value_sec value_sec; + struct ble_store_value_rpa_rec value_rpa_rec; struct ble_hs_conn *conn; ble_addr_t peer_addr; int authenticated; @@ -614,6 +615,9 @@ ble_sm_persist_keys(struct ble_sm_proc *proc) ble_sm_fill_store_value(&peer_addr, authenticated, sc, &proc->peer_keys, &value_sec); ble_store_write_peer_sec(&value_sec); + value_rpa_rec.peer_addr=peer_addr; + value_rpa_rec.peer_rpa_addr=conn->bhc_peer_rpa_addr; + ble_store_write_rpa_rec(&value_rpa_rec); } static int diff --git a/nimble/host/src/ble_store.c b/nimble/host/src/ble_store.c index c14109c15..032d0d917 100644 --- a/nimble/host/src/ble_store.c +++ b/nimble/host/src/ble_store.c @@ -348,6 +348,49 @@ ble_store_key_from_value_ead(struct ble_store_key_ead *out_key, } #endif +int +ble_store_read_rpa_rec(const struct ble_store_key_rpa_rec *key, + struct ble_store_value_rpa_rec *out_value) +{ + union ble_store_value *store_value; + union ble_store_key *store_key; + int rc; + + store_key = (void *)key; + store_value = (void *)out_value; + rc = ble_store_read(BLE_STORE_OBJ_TYPE_PEER_ADDR, store_key, store_value); + return rc; +} + +int +ble_store_write_rpa_rec(const struct ble_store_value_rpa_rec *value) +{ + union ble_store_value *store_value; + int rc; + + store_value = (void *)value; + rc = ble_store_write(BLE_STORE_OBJ_TYPE_PEER_ADDR, store_value); + return rc; +} + +int +ble_store_delete_rpa_rec(const struct ble_store_key_rpa_rec *key) +{ + union ble_store_key *store_key; + int rc; + + store_key = (void *)key; + rc = ble_store_delete(BLE_STORE_OBJ_TYPE_PEER_ADDR, store_key); + return rc; +} +void +ble_store_key_from_value_rpa_rec(struct ble_store_key_rpa_rec *out_key, + const struct ble_store_value_rpa_rec *value) +{ + out_key->peer_rpa_addr = value->peer_rpa_addr; + out_key->idx = 0; +} + void ble_store_key_from_value(int obj_type, union ble_store_key *out_key, @@ -367,6 +410,9 @@ ble_store_key_from_value(int obj_type, ble_store_key_from_value_ead(&out_key->ead, &value->ead); break; #endif + case BLE_STORE_OBJ_TYPE_PEER_ADDR: + ble_store_key_from_value_rpa_rec(&out_key->rpa_rec, &value->rpa_rec); + break; default: BLE_HS_DBG_ASSERT(0); break; @@ -402,6 +448,10 @@ ble_store_iterate(int obj_type, pidx = &key.ead.idx; break; #endif + case BLE_STORE_OBJ_TYPE_PEER_ADDR: + key.rpa_rec.peer_rpa_addr = *BLE_ADDR_ANY; + pidx = &key.rpa_rec.idx; + break; default: BLE_HS_DBG_ASSERT(0); return BLE_HS_EINVAL; @@ -446,6 +496,7 @@ ble_store_clear(void) BLE_STORE_OBJ_TYPE_OUR_SEC, BLE_STORE_OBJ_TYPE_PEER_SEC, BLE_STORE_OBJ_TYPE_CCCD, + BLE_STORE_OBJ_TYPE_PEER_ADDR, #if MYNEWT_VAL(ENC_ADV_DATA) BLE_STORE_OBJ_TYPE_ENC_ADV_DATA, #endif diff --git a/nimble/host/store/config/src/ble_store_config.c b/nimble/host/store/config/src/ble_store_config.c index 2df66ce84..ebad22548 100644 --- a/nimble/host/store/config/src/ble_store_config.c +++ b/nimble/host/store/config/src/ble_store_config.c @@ -52,6 +52,12 @@ struct ble_store_value_ead int ble_store_config_num_eads; #endif +#if MYNEWT_VAL(BLE_STORE_MAX_BONDS) +struct ble_store_value_rpa_rec + ble_store_config_rpa_recs[MYNEWT_VAL(BLE_STORE_MAX_BONDS)]; +#endif +int ble_store_config_num_rpa_recs; + /***************************************************************************** * $sec * *****************************************************************************/ @@ -555,6 +561,109 @@ ble_store_config_write_ead(const struct ble_store_value_ead *value_ead) } #endif +/***************************************************************************** + * $rpa-map * + *****************************************************************************/ +#if MYNEWT_VAL(BLE_STORE_MAX_BONDS) +static int +ble_store_config_find_rpa_rec(const struct ble_store_key_rpa_rec *key) +{ + struct ble_store_value_rpa_rec *rpa_rec; + int skipped = 0; + int i = 0; + + for(i = 0; i < ble_store_config_num_rpa_recs; i++){ + rpa_rec = ble_store_config_rpa_recs + i; + + if (ble_addr_cmp(&rpa_rec->peer_rpa_addr, &key->peer_rpa_addr)) { + continue; + } + if (key->idx > skipped) { + skipped++; + continue; + } + return i; + } + return -1; +} +#endif + +static int +ble_store_config_read_rpa_rec(const struct ble_store_key_rpa_rec *key_rpa_rec,struct ble_store_value_rpa_rec *value_rpa_rec) +{ +#if MYNEWT_VAL(BLE_STORE_MAX_BONDS) + int idx; + + idx = ble_store_config_find_rpa_rec(key_rpa_rec); + if (idx == -1) { + return BLE_HS_ENOENT; + } + *value_rpa_rec = ble_store_config_rpa_recs[idx]; + return 0; +#else + return BLE_HS_ENOENT; +#endif +} + +static int +ble_store_config_write_rpa_rec(const struct ble_store_value_rpa_rec *value_rpa_rec){ + +#if MYNEWT_VAL(BLE_STORE_MAX_BONDS) + struct ble_store_key_rpa_rec key_rpa_rec; + int idx; + int rc; + ble_store_key_from_value_rpa_rec(&key_rpa_rec, value_rpa_rec); + idx = ble_store_config_find_rpa_rec(&key_rpa_rec); + + if (idx == -1) { + if (ble_store_config_num_rpa_recs >= MYNEWT_VAL(BLE_STORE_MAX_BONDS)) { + BLE_HS_LOG(DEBUG, "error persisting peer addrr; too many entries (%d)\n", + ble_store_config_num_rpa_recs); + return BLE_HS_ESTORE_CAP; + } + + idx = ble_store_config_num_rpa_recs; + ble_store_config_num_rpa_recs++; + } + ble_store_config_rpa_recs[idx] = *value_rpa_rec; + + rc = ble_store_config_persist_rpa_recs(); + if (rc != 0) { + return rc; + } + return 0; +#else + return BLE_HS_ENOENT; +#endif +} + +static int +ble_store_config_delete_rpa_rec(const struct ble_store_key_rpa_rec *key_rpa_rec) +{ + int idx; + int rc; + + idx = ble_store_config_find_rpa_rec(key_rpa_rec); + if (idx == -1) { + return BLE_HS_ENOENT; + } + + rc = ble_store_config_delete_obj(ble_store_config_rpa_recs, + sizeof *ble_store_config_rpa_recs, + idx, + &ble_store_config_num_rpa_recs); + if (rc != 0) { + return rc; + } + + rc = ble_store_config_persist_rpa_recs(); + if (rc != 0) { + return rc; + } + + return 0; +} + /***************************************************************************** * $api * *****************************************************************************/ @@ -603,6 +712,10 @@ ble_store_config_read(int obj_type, const union ble_store_key *key, return rc; #endif + case BLE_STORE_OBJ_TYPE_PEER_ADDR: + rc = ble_store_config_read_rpa_rec(&key->rpa_rec, &value->rpa_rec); + return rc; + default: return BLE_HS_ENOTSUP; } @@ -638,6 +751,10 @@ ble_store_config_write(int obj_type, const union ble_store_value *val) return rc; #endif + case BLE_STORE_OBJ_TYPE_PEER_ADDR: + rc = ble_store_config_write_rpa_rec(&val->rpa_rec); + return rc; + default: return BLE_HS_ENOTSUP; } @@ -667,6 +784,9 @@ ble_store_config_delete(int obj_type, const union ble_store_key *key) return rc; #endif + case BLE_STORE_OBJ_TYPE_PEER_ADDR: + rc = ble_store_config_delete_rpa_rec(&key->rpa_rec); + default: return BLE_HS_ENOTSUP; } @@ -689,6 +809,6 @@ ble_store_config_init(void) #if MYNEWT_VAL(ENC_ADV_DATA) ble_store_config_num_eads = 0; #endif - + ble_store_config_num_rpa_recs = 0; ble_store_config_conf_init(); } diff --git a/nimble/host/store/config/src/ble_store_config_conf.c b/nimble/host/store/config/src/ble_store_config_conf.c index 7113b1e0d..1300bc160 100644 --- a/nimble/host/store/config/src/ble_store_config_conf.c +++ b/nimble/host/store/config/src/ble_store_config_conf.c @@ -65,6 +65,12 @@ static struct conf_handler ble_store_config_conf_handler = { (MYNEWT_VAL(BLE_STORE_MAX_EADS) * BLE_STORE_CONFIG_EAD_ENCODE_SZ + 1) #endif +#define BLE_STORE_CONFIG_RPA_REC_ENCODE_SZ \ + BASE64_ENCODE_SIZE(sizeof (struct ble_store_value_rpa_rec)) + +#define BLE_STORE_CONFIG_RPA_REC_SET_ENCODE_SZ \ + (MYNEWT_VAL(BLE_STORE_MAX_BONDS) * BLE_STORE_CONFIG_RPA_REC_ENCODE_SZ + 1) + static void ble_store_config_serialize_arr(const void *arr, int obj_sz, int num_objs, char *out_buf, int buf_sz) @@ -132,6 +138,14 @@ ble_store_config_conf_set(int argc, char **argv, char *val) return rc; } #endif + else if (strcmp(argv[0],"rpa_rec") == 0){ + rc = ble_store_config_deserialize_arr( + val, + ble_store_config_rpa_recs, + sizeof *ble_store_config_rpa_recs, + &ble_store_config_num_rpa_recs); + return rc; + } } return OS_ENOENT; } @@ -143,6 +157,7 @@ ble_store_config_conf_export(void (*func)(char *name, char *val), union { char sec[BLE_STORE_CONFIG_SEC_SET_ENCODE_SZ]; char cccd[BLE_STORE_CONFIG_CCCD_SET_ENCODE_SZ]; + char rpa_rec[BLE_STORE_CONFIG_RPA_REC_SET_ENCODE_SZ]; } buf; ble_store_config_serialize_arr(ble_store_config_our_secs, @@ -174,6 +189,11 @@ ble_store_config_conf_export(void (*func)(char *name, char *val), sizeof buf.ead); func("ble_hs/ead", buf.ead); #endif + ble_store_config_serialize_arr(ble_store_config_rpa_recs, + sizeof *ble_store_config_rpa_recs, + ble_store_config_num_rpa_recs, + buf.rpa_rec, + sizeof buf.rpa_rec); return 0; } @@ -262,7 +282,22 @@ ble_store_config_persist_eads(void) return 0; } #endif - +int +ble_store_config_persist_rpa_recs(void) +{ + char buf[BLE_STORE_CONFIG_RPA_REC_SET_ENCODE_SZ]; + int rc; + ble_store_config_serialize_arr(ble_store_config_rpa_recs, + sizeof *ble_store_config_rpa_recs, + ble_store_config_num_rpa_recs, + buf, + sizeof buf); + rc = conf_save_one("ble_hs/rpa_rec", buf); + if (rc != 0) { + return BLE_HS_ESTORE_FAIL; + } + return 0; +} void ble_store_config_conf_init(void) { 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 9b8a8c6c2..c2531bd1d 100644 --- a/nimble/host/store/config/src/ble_store_config_priv.h +++ b/nimble/host/store/config/src/ble_store_config_priv.h @@ -42,6 +42,11 @@ extern struct ble_store_value_ead extern int ble_store_config_num_eads; #endif +extern struct ble_store_value_rpa_rec + ble_store_config_rpa_recs[MYNEWT_VAL(BLE_STORE_MAX_BONDS)]; +extern int ble_store_config_num_rpa_recs; + + #if MYNEWT_VAL(BLE_STORE_CONFIG_PERSIST) int ble_store_config_persist_our_secs(void); @@ -50,6 +55,7 @@ int ble_store_config_persist_cccds(void); #if MYNEWT_VAL(ENC_ADV_DATA) int ble_store_config_persist_eads(void); #endif +int ble_store_config_persist_rpa_recs(void); void ble_store_config_conf_init(void); #else @@ -60,6 +66,7 @@ static inline int ble_store_config_persist_cccds(void) { return 0; } #if MYNEWT_VAL(ENC_ADV_DATA) static inline int ble_store_config_persist_eads(void) { return 0; } #endif +static inline int ble_store_config_persist_rpa_recs(void) { return 0; } static inline void ble_store_config_conf_init(void) { } #if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY) diff --git a/nimble/host/store/config/src/ble_store_nvs.c b/nimble/host/store/config/src/ble_store_nvs.c index 478527024..dc936ba1a 100644 --- a/nimble/host/store/config/src/ble_store_nvs.c +++ b/nimble/host/store/config/src/ble_store_nvs.c @@ -46,6 +46,8 @@ #define NIMBLE_NVS_EAD_SEC_KEY "ead_sec" #endif +#define NIMBLE_NVS_RPA_RECORDS_KEY "rpa_rec" + static const char *TAG = "NIMBLE_NVS"; /***************************************************************************** @@ -66,7 +68,9 @@ get_nvs_key_string(int obj_type, int index, char *key_string) } else if (obj_type == NIMBLE_NVS_EAD_SEC_KEY) { sprintf(key_string, "%s_%d", NIMBLE_NVS_EAD_SEC_KEY, index); #endif - } else { + } else if (obj_type == BLE_STORE_OBJ_TYPE_PEER_ADDR){ + sprintf(key_string, "%s_%d", NIMBLE_NVS_RPA_RECORDS_KEY, index); + }else { sprintf(key_string, "%s_%d", NIMBLE_NVS_CCCD_SEC_KEY, index); } } @@ -173,6 +177,10 @@ get_nvs_db_value(int obj_type, char *key_string, union ble_store_value *val) err = nvs_get_blob(nimble_handle, key_string, &val->ead, &required_size); #endif + } else if(obj_type == BLE_STORE_OBJ_TYPE_PEER_ADDR){ + err = nvs_get_blob(nimble_handle, key_string, &val->rpa_rec, + &required_size); + } else { err = nvs_get_blob(nimble_handle, key_string, &val->sec, &required_size); @@ -244,6 +252,9 @@ get_nvs_db_attribute(int obj_type, bool empty, void *value, int num_value) err = get_nvs_matching_index(&cur.sec, value, num_value, sizeof(struct ble_store_value_ead)); #endif + } else if (obj_type == BLE_STORE_OBJ_TYPE_PEER_ADDR){ + err = get_nvs_matching_index(&cur.rpa_rec,value,num_value, + sizeof(struct ble_store_value_rpa_rec)); } else { err = get_nvs_matching_index(&cur.cccd, value, num_value, sizeof(struct ble_store_value_sec)); @@ -374,6 +385,10 @@ ble_store_nvs_write(int obj_type, const union ble_store_value *val) return ble_nvs_write_key_value(key_string, &val->ead, sizeof(struct ble_store_value_ead)); #endif + } else if (obj_type == BLE_STORE_OBJ_TYPE_PEER_ADDR) { + return ble_nvs_write_key_value(key_string, &val->rpa_rec, sizeof(struct + ble_store_value_rpa_rec)); + } else { return ble_nvs_write_key_value(key_string, &val->sec, sizeof(struct ble_store_value_sec)); @@ -465,6 +480,11 @@ populate_db_from_nvs(int obj_type, void *dst, int *db_num) db_item += sizeof(struct ble_store_value_ead); (*db_num)++; #endif + } else if(obj_type == BLE_STORE_OBJ_TYPE_PEER_ADDR) { + ESP_LOGD(TAG, "RPA_REC in RAM is filled up from NVS index = %d", i); + memcpy(db_item, &cur.rpa_rec, sizeof(struct ble_store_value_rpa_rec)); + db_item += sizeof(struct ble_store_value_rpa_rec); + (*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)); @@ -520,6 +540,15 @@ ble_nvs_restore_sec_keys(void) ESP_LOGD(TAG, "ble_store_config_eads restored %d bonds", ble_store_config_num_eads); #endif + err = populate_db_from_nvs(BLE_STORE_OBJ_TYPE_PEER_ADDR, ble_store_config_rpa_recs, + &ble_store_config_num_rpa_recs); + if (err != ESP_OK) { + ESP_LOGE(TAG, "NVS operation failed for 'RPA_REC'"); + return err; + } + ESP_LOGD(TAG, "ble_store_config_rpa_recs restored %d bonds", + ble_store_config_num_rpa_recs); + return 0; } @@ -595,7 +624,7 @@ int ble_store_config_persist_eads(void) return ble_store_nvs_write(BLE_STORE_OBJ_TYPE_ENC_ADV_DATA, &val); } else if (nvs_count > ble_store_config_num_eads) { /* NVS db count more than RAM count, delete operation */ - vvs_idx = get_nvs_db_attribute(BLE_STORE_OBJ_TYPE_ENC_ADV_DATA, 0, + nvs_idx = get_nvs_db_attribute(BLE_STORE_OBJ_TYPE_ENC_ADV_DATA, 0, ble_store_config_eads, ble_store_config_num_eads); if (nvs_idx == -1) { ESP_LOGE(TAG, "NVS delete operation failed for EAD"); @@ -608,6 +637,35 @@ int ble_store_config_persist_eads(void) } #endif +int ble_store_config_persist_rpa_recs(void) +{ + int nvs_count, nvs_idx; + union ble_store_value val; + nvs_count = get_nvs_db_attribute(BLE_STORE_OBJ_TYPE_PEER_ADDR, 0, NULL, 0); + if (nvs_count == -1) { + ESP_LOGE(TAG, "NVS operation failed while persisting RPA_RECS"); + return BLE_HS_ESTORE_FAIL; + } + if (nvs_count < ble_store_config_num_rpa_recs) { + /* NVS db count less than RAM count, write operation */ + ESP_LOGD(TAG, "Persisting RPA_RECS value in NVS..."); + val.rpa_rec = ble_store_config_rpa_recs[ble_store_config_num_rpa_recs - 1]; + return ble_store_nvs_write(BLE_STORE_OBJ_TYPE_PEER_ADDR, &val); + } else if (nvs_count > ble_store_config_num_rpa_recs) { + /* NVS db count more than RAM count, delete operation */ + nvs_idx = get_nvs_db_attribute(BLE_STORE_OBJ_TYPE_PEER_ADDR, 0, + ble_store_config_rpa_recs, ble_store_config_num_rpa_recs); + if (nvs_idx == -1) { + ESP_LOGE(TAG, "NVS delete operation failed for RPA_REC"); + return BLE_HS_ESTORE_FAIL; + } + ESP_LOGD(TAG, "Deleting RPA_REC, nvs idx = %d", nvs_idx); + return ble_nvs_delete_value(BLE_STORE_OBJ_TYPE_PEER_ADDR, nvs_idx); + } + return 0; + +} + int ble_store_config_persist_peer_secs(void) { int nvs_count, nvs_idx;