diff --git a/nimble/host/include/host/ble_store.h b/nimble/host/include/host/ble_store.h index 0dbd099b2..732f4ff05 100644 --- a/nimble/host/include/host/ble_store.h +++ b/nimble/host/include/host/ble_store.h @@ -69,6 +69,7 @@ struct ble_store_key_sec { */ struct ble_store_value_sec { ble_addr_t peer_addr; + uint16_t bond_count; uint8_t key_size; uint16_t ediv; diff --git a/nimble/host/store/config/src/ble_store_config.c b/nimble/host/store/config/src/ble_store_config.c index 00ad791b8..770edfb01 100644 --- a/nimble/host/store/config/src/ble_store_config.c +++ b/nimble/host/store/config/src/ble_store_config.c @@ -32,6 +32,9 @@ struct ble_store_value_sec #endif int ble_store_config_num_our_secs; +uint16_t ble_store_config_our_bond_count; +uint16_t ble_store_config_peer_bond_count; + #if MYNEWT_VAL(BLE_STORE_MAX_BONDS) struct ble_store_value_sec ble_store_config_peer_secs[MYNEWT_VAL(BLE_STORE_MAX_BONDS)]; @@ -66,6 +69,107 @@ int ble_store_config_num_local_irks; * $sec * *****************************************************************************/ +int ble_store_config_compare_bond_count(const void *a, const void *b) { + const struct ble_store_value_sec *sec_a = (const struct ble_store_value_sec *)a; + const struct ble_store_value_sec *sec_b = (const struct ble_store_value_sec *)b; + + return sec_a->bond_count - sec_b->bond_count; +} + +/* This function gets the stored device records of OUR_SEC object type, arranges them in order of their bond count, + * and then updates them with new counts so they're in sequence. + */ +#if MYNEWT_VAL(BLE_STORE_MAX_BONDS) +int ble_restore_our_sec_nvs(void) +{ + esp_err_t err; + extern uint16_t ble_store_config_our_bond_count; + struct ble_store_value_sec temp_our_secs[MYNEWT_VAL(BLE_STORE_MAX_BONDS)]; + int temp_count = 0; + + ble_store_config_our_bond_count = 0; + + memcpy(temp_our_secs, ble_store_config_our_secs, ble_store_config_num_our_secs * sizeof(struct ble_store_value_sec)); + temp_count = ble_store_config_num_our_secs; + + qsort(temp_our_secs, temp_count, sizeof(struct ble_store_value_sec), ble_store_config_compare_bond_count); + + for (int i = 0; i < temp_count; i++) { + + union ble_store_key key; + ble_store_key_from_value_sec(&key.sec, &temp_our_secs[i]); + + err = ble_store_config_delete(BLE_STORE_OBJ_TYPE_OUR_SEC, &key); + + if (err != ESP_OK) { + BLE_HS_LOG(DEBUG, "Error deleting from nvs"); + return err; + } + } + + for (int i = 0; i < temp_count; i++) { + + union ble_store_value val; + val.sec = temp_our_secs[i]; + + err = ble_store_config_write(BLE_STORE_OBJ_TYPE_OUR_SEC, &val); + + if (err != ESP_OK) { + BLE_HS_LOG(DEBUG, "Error writing record to NVS"); + return err; + } + } + + return 0; +} + +/* This function gets the stored device records of PEER_SEC object type, arranges them in order of their bond count, + * and then updates them with new counts so they're in sequence. + */ +int ble_restore_peer_sec_nvs(void) +{ + esp_err_t err; + extern uint16_t ble_store_config_peer_bond_count; + struct ble_store_value_sec temp_peer_secs[MYNEWT_VAL(BLE_STORE_MAX_BONDS)]; + int temp_count = 0; + + ble_store_config_peer_bond_count = 0; + + memcpy(temp_peer_secs, ble_store_config_peer_secs, ble_store_config_num_peer_secs * sizeof(struct ble_store_value_sec)); + temp_count = ble_store_config_num_peer_secs; + + qsort(temp_peer_secs, temp_count, sizeof(struct ble_store_value_sec), ble_store_config_compare_bond_count); + + for (int i = 0; i < temp_count; i++) { + + union ble_store_key key; + ble_store_key_from_value_sec(&key.sec, &temp_peer_secs[i]); + + err = ble_store_config_delete(BLE_STORE_OBJ_TYPE_PEER_SEC, &key); + + if (err != ESP_OK) { + BLE_HS_LOG(DEBUG, "Error deleting from nvs"); + return err; + } + } + + for (int i = 0; i < temp_count; i++) { + + union ble_store_value val; + val.sec = temp_peer_secs[i]; + + err = ble_store_config_write(BLE_STORE_OBJ_TYPE_PEER_SEC, &val); + + if (err != ESP_OK) { + BLE_HS_LOG(DEBUG, "Error writing record to NVS"); + return err; + } + } + + return 0; +} +#endif + #if MYNEWT_VAL(BLE_STORE_MAX_BONDS) static void ble_store_config_print_value_sec(const struct ble_store_value_sec *sec) @@ -177,11 +281,20 @@ ble_store_config_write_our_sec(const struct ble_store_value_sec *value_sec) ble_store_config_our_secs[idx] = *value_sec; + ble_store_config_our_secs[idx].bond_count = ++ble_store_config_our_bond_count; + rc = ble_store_config_persist_our_secs(); if (rc != 0) { return rc; } + if (ble_store_config_our_bond_count > (UINT16_MAX - 5)) { + rc = ble_restore_our_sec_nvs(); + if (rc != 0) { + return rc; + } + } + return 0; #else return BLE_HS_ENOENT; @@ -327,10 +440,20 @@ ble_store_config_write_peer_sec(const struct ble_store_value_sec *value_sec) ble_store_config_peer_secs[idx] = *value_sec; + ble_store_config_peer_secs[idx].bond_count = ++ble_store_config_peer_bond_count; + rc = ble_store_config_persist_peer_secs(); if (rc != 0) { return rc; } + + if (ble_store_config_peer_bond_count > (UINT16_MAX - 5)) { + rc = ble_restore_peer_sec_nvs(); + if (rc != 0) { + return rc; + } + } + return 0; #else return BLE_HS_ENOENT; 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 cdc676e1e..581cefdc9 100644 --- a/nimble/host/store/config/src/ble_store_config_priv.h +++ b/nimble/host/store/config/src/ble_store_config_priv.h @@ -56,6 +56,10 @@ extern int ble_store_config_num_local_irks; int ble_store_config_persist_our_secs(void); int ble_store_config_persist_peer_secs(void); int ble_store_config_persist_cccds(void); +#if MYNEWT_VAL(BLE_STORE_MAX_BONDS) +int ble_restore_our_sec_nvs(void); +int ble_restore_peer_sec_nvs(void); +#endif #if MYNEWT_VAL(ENC_ADV_DATA) int ble_store_config_persist_eads(void); #endif diff --git a/nimble/host/store/config/src/ble_store_nvs.c b/nimble/host/store/config/src/ble_store_nvs.c index c6ad1aa0c..33a9349e4 100644 --- a/nimble/host/store/config/src/ble_store_nvs.c +++ b/nimble/host/store/config/src/ble_store_nvs.c @@ -525,6 +525,10 @@ static int ble_nvs_restore_sec_keys(void) { esp_err_t err; + int flag = 0; + extern uint16_t ble_store_config_our_bond_count; + extern uint16_t ble_store_config_peer_bond_count; + extern int ble_store_config_compare_bond_count(const void *a, const void *b); err = populate_db_from_nvs(BLE_STORE_OBJ_TYPE_OUR_SEC, ble_store_config_our_secs, &ble_store_config_num_our_secs); @@ -540,6 +544,27 @@ ble_nvs_restore_sec_keys(void) ESP_LOGE(TAG, "NVS operation failed for 'peer sec'"); return err; } + + for (int i = 0; i < MYNEWT_VAL(BLE_STORE_MAX_BONDS) - 1; i++) { + if ((ble_store_config_our_secs[i].bond_count > ble_store_config_our_secs[i+1].bond_count) + || (ble_store_config_peer_secs[i].bond_count > ble_store_config_peer_secs[i+1].bond_count)) { + flag = 1; + break; + } + } + + if (flag) { + + qsort(ble_store_config_our_secs, ble_store_config_num_our_secs, + sizeof(struct ble_store_value_sec), ble_store_config_compare_bond_count); + + qsort(ble_store_config_peer_secs, ble_store_config_num_peer_secs, + sizeof(struct ble_store_value_sec), ble_store_config_compare_bond_count); + } + + ble_store_config_our_bond_count = ble_store_config_our_secs[ble_store_config_num_our_secs - 1].bond_count; + ble_store_config_peer_bond_count = ble_store_config_peer_secs[ble_store_config_num_peer_secs - 1].bond_count; + ESP_LOGD(TAG, "ble_store_config_peer_secs restored %d bonds", ble_store_config_num_peer_secs);