From dbcd38dd910e8e817491d7017654c841e0b751bf Mon Sep 17 00:00:00 2001 From: Sumeet Singh Date: Tue, 29 Oct 2024 11:45:06 +0530 Subject: [PATCH] feat(nimble): Added support for persisting csf characteristic for bonded devices --- nimble/host/include/host/ble_store.h | 34 +++++ .../gatt/include/services/gatt/ble_svc_gatt.h | 6 + nimble/host/src/ble_gatt_priv.h | 8 +- nimble/host/src/ble_gatts.c | 57 ++++++-- nimble/host/src/ble_store.c | 63 ++++++++- nimble/host/src/ble_store_util.c | 9 ++ .../host/store/config/src/ble_store_config.c | 127 +++++++++++++++++- .../store/config/src/ble_store_config_conf.c | 41 ++++++ .../store/config/src/ble_store_config_priv.h | 5 + nimble/host/store/config/src/ble_store_nvs.c | 73 +++++++++- nimble/host/store/ram/src/ble_store_ram.c | 125 +++++++++++++++++ 11 files changed, 527 insertions(+), 21 deletions(-) diff --git a/nimble/host/include/host/ble_store.h b/nimble/host/include/host/ble_store.h index dbc4ddbc0..f66dddfbb 100644 --- a/nimble/host/include/host/ble_store.h +++ b/nimble/host/include/host/ble_store.h @@ -37,6 +37,7 @@ extern "C" { #define BLE_STORE_OBJ_TYPE_PEER_ADDR 6 #define BLE_STORE_OBJ_TYPE_LOCAL_IRK 7 +#define BLE_STORE_OBJ_TYPE_CSFC 8 /** Failed to persist record; insufficient storage capacity. */ #define BLE_STORE_EVENT_OVERFLOW 1 @@ -148,6 +149,29 @@ struct ble_store_value_local_irk { }; +/** + * Used as a key for lookups of stored client supported features characteristic (CSFC). + * This struct corresponds to the BLE_STORE_OBJ_TYPE_CSFC store object type. + */ +struct ble_store_key_csfc { + /** + * Key by peer identity address; + */ + ble_addr_t peer_addr; + + /** Number of results to skip; 0 means retrieve the first match. */ + uint8_t idx; +}; + +/** + * Represents a stored client supported features characteristic (CSFC). + * This struct corresponds to the BLE_STORE_OBJ_TYPE_CSFC store object type. + */ +struct ble_store_value_csfc { + ble_addr_t peer_addr; + uint8_t csfc[MYNEWT_VAL(BLE_GATT_CSFC_SIZE)]; +}; + #if MYNEWT_VAL(ENC_ADV_DATA) /** * Used as a key for lookups of encrypted advertising data. This struct corresponds @@ -187,6 +211,7 @@ union ble_store_key { #if MYNEWT_VAL(ENC_ADV_DATA) struct ble_store_key_ead ead; #endif + struct ble_store_key_csfc csfc; }; /** @@ -201,6 +226,7 @@ union ble_store_value { #if MYNEWT_VAL(ENC_ADV_DATA) struct ble_store_value_ead ead; #endif + struct ble_store_value_csfc csfc; }; struct ble_store_status_event { @@ -331,10 +357,18 @@ int ble_store_read_cccd(const struct ble_store_key_cccd *key, int ble_store_write_cccd(const struct ble_store_value_cccd *value); int ble_store_delete_cccd(const struct ble_store_key_cccd *key); +int ble_store_read_csfc(const struct ble_store_key_csfc *key, + struct ble_store_value_csfc *out_value); +int ble_store_write_csfc(const struct ble_store_value_csfc *value); +int ble_store_delete_csfc(const struct ble_store_key_csfc *key); + void ble_store_key_from_value_sec(struct ble_store_key_sec *out_key, const struct ble_store_value_sec *value); void ble_store_key_from_value_cccd(struct ble_store_key_cccd *out_key, const struct ble_store_value_cccd *value); +void ble_store_key_from_value_csfc(struct ble_store_key_csfc *out_key, + const struct ble_store_value_csfc *value); + #if MYNEWT_VAL(ENC_ADV_DATA) int ble_store_read_ead(const struct ble_store_key_ead *key, struct ble_store_value_ead *out_value); diff --git a/nimble/host/services/gatt/include/services/gatt/ble_svc_gatt.h b/nimble/host/services/gatt/include/services/gatt/ble_svc_gatt.h index 0a0280fc2..85e4eaa61 100644 --- a/nimble/host/services/gatt/include/services/gatt/ble_svc_gatt.h +++ b/nimble/host/services/gatt/include/services/gatt/ble_svc_gatt.h @@ -33,6 +33,12 @@ struct ble_hs_cfg; #define BLE_SVC_GATT_CHR_SERVER_SUPPORTED_FEAT_UUID16 0x2b3a #define BLE_SVC_GATT_CHR_CLIENT_SUPPORTED_FEAT_UUID16 0x2b29 +#define BLE_SVR_GATT_CHR_SVR_SUP_FEAT_EATT_FLAG 0x01 + +#define BLE_SVR_GATT_CHR_CLT_SUP_FEAT_ROBUST_CACHING_FLAG 0x01 +#define BLE_SVR_GATT_CHR_CLT_SUP_FEAT_EATT_FLAG 0x02 +#define BLE_SVR_GATT_CHR_CLT_SUP_FEAT_MULTI_NOTIF_FLAG 0x04 + #if MYNEWT_VAL(BLE_GATT_CACHING) #define BLE_SVC_GATT_CHR_DATABASE_HASH_UUID16 0x2b2a diff --git a/nimble/host/src/ble_gatt_priv.h b/nimble/host/src/ble_gatt_priv.h index fd40368d0..a8326648d 100644 --- a/nimble/host/src/ble_gatt_priv.h +++ b/nimble/host/src/ble_gatt_priv.h @@ -92,7 +92,11 @@ extern STATS_SECT_DECL(ble_gatts_stats) ble_gatts_stats; #define BLE_GATT_CHR_DECL_SZ_16 5 #define BLE_GATT_CHR_DECL_SZ_128 19 -#define BLE_GATT_CHR_CLI_SUP_FEAT_SZ 1 +/** + * For now only 3 bits in first octet are defined + * + */ +#define BLE_GATT_CHR_CLI_SUP_FEAT_MASK 7 typedef uint8_t ble_gatts_conn_flags; @@ -116,7 +120,7 @@ struct ble_gatts_conn { * future proof if more octets might be used. * (Vol. 3, Part G, 7.2) */ - uint8_t peer_cl_sup_feat[BLE_GATT_CHR_CLI_SUP_FEAT_SZ]; + uint8_t peer_cl_sup_feat[MYNEWT_VAL(BLE_GATT_CSFC_SIZE)]; }; /*** @client. */ diff --git a/nimble/host/src/ble_gatts.c b/nimble/host/src/ble_gatts.c index 39a025dda..eb270aff2 100644 --- a/nimble/host/src/ble_gatts.c +++ b/nimble/host/src/ble_gatts.c @@ -2169,8 +2169,8 @@ ble_gatts_peer_cl_sup_feat_get(uint16_t conn_handle, uint8_t *out_supported_feat goto done; } - if (BLE_GATT_CHR_CLI_SUP_FEAT_SZ < len) { - len = BLE_GATT_CHR_CLI_SUP_FEAT_SZ; + if (MYNEWT_VAL(BLE_GATT_CSFC_SIZE) < len) { + len = MYNEWT_VAL(BLE_GATT_CSFC_SIZE); } memcpy(out_supported_feat, conn->bhc_gatt_svr.peer_cl_sup_feat, @@ -2185,7 +2185,9 @@ int ble_gatts_peer_cl_sup_feat_update(uint16_t conn_handle, struct os_mbuf *om) { struct ble_hs_conn *conn; - uint8_t feat[BLE_GATT_CHR_CLI_SUP_FEAT_SZ] = {}; + struct ble_store_value_csfc value_csfc; + struct ble_store_key_csfc key_csfc; + uint8_t feat[MYNEWT_VAL(BLE_GATT_CSFC_SIZE)] = {}; uint16_t len; int rc = 0; int i; @@ -2199,8 +2201,8 @@ ble_gatts_peer_cl_sup_feat_update(uint16_t conn_handle, struct os_mbuf *om) /* RFU bits are ignored so we can skip any bytes larger than supported */ len = os_mbuf_len(om); - if (len > BLE_GATT_CHR_CLI_SUP_FEAT_SZ) { - len = BLE_GATT_CHR_CLI_SUP_FEAT_SZ; + if (len > MYNEWT_VAL(BLE_GATT_CSFC_SIZE)) { + len = MYNEWT_VAL(BLE_GATT_CSFC_SIZE); } if (os_mbuf_copydata(om, 0, len, feat) < 0) { @@ -2208,7 +2210,7 @@ ble_gatts_peer_cl_sup_feat_update(uint16_t conn_handle, struct os_mbuf *om) } /* clear RFU bits */ - for (i = 0; i < BLE_GATT_CHR_CLI_SUP_FEAT_SZ; i++) { + for (i = 0; i < MYNEWT_VAL(BLE_GATT_CSFC_SIZE); i++) { feat[i] &= (BLE_GATT_CHR_CLI_SUP_FEAT_MASK >> (8 * i)); } @@ -2223,7 +2225,7 @@ ble_gatts_peer_cl_sup_feat_update(uint16_t conn_handle, struct os_mbuf *om) * Disabling already enabled features is not permitted * (Vol. 3, Part F, 3.3.3) */ - for (i = 0; i < BLE_GATT_CHR_CLI_SUP_FEAT_SZ; i++) { + for (i = 0; i < MYNEWT_VAL(BLE_GATT_CSFC_SIZE); i++) { if ((conn->bhc_gatt_svr.peer_cl_sup_feat[i] & feat[i]) != conn->bhc_gatt_svr.peer_cl_sup_feat[i]) { rc = BLE_ATT_ERR_VALUE_NOT_ALLOWED; @@ -2231,7 +2233,25 @@ ble_gatts_peer_cl_sup_feat_update(uint16_t conn_handle, struct os_mbuf *om) } } - memcpy(conn->bhc_gatt_svr.peer_cl_sup_feat, feat, BLE_GATT_CHR_CLI_SUP_FEAT_SZ); + memcpy(conn->bhc_gatt_svr.peer_cl_sup_feat, feat, MYNEWT_VAL(BLE_GATT_CSFC_SIZE)); + + if (conn->bhc_sec_state.bonded) { + memset(&key_csfc, 0, sizeof key_csfc); + key_csfc.peer_addr = conn->bhc_peer_addr; + + rc = ble_store_delete_csfc(&key_csfc); + if (rc != 0) { + goto done; + } + + value_csfc.peer_addr = conn->bhc_peer_addr; + memcpy(value_csfc.csfc, feat, MYNEWT_VAL(BLE_GATT_CSFC_SIZE)); + + rc = ble_store_write_csfc(&value_csfc); + if (rc != 0) { + goto done; + } + } done: ble_hs_unlock(); @@ -2347,6 +2367,7 @@ void ble_gatts_bonding_established(uint16_t conn_handle) { struct ble_store_value_cccd cccd_value; + struct ble_store_value_csfc csfc; struct ble_gatts_clt_cfg *clt_cfg; struct ble_gatts_conn *gatt_srv; struct ble_hs_conn *conn; @@ -2391,6 +2412,16 @@ ble_gatts_bonding_established(uint16_t conn_handle) } } + csfc.peer_addr = conn->bhc_peer_addr; + memcpy(csfc.csfc, conn->bhc_gatt_svr.peer_cl_sup_feat, MYNEWT_VAL(BLE_GATT_CSFC_SIZE)); + + ble_hs_unlock(); + ble_store_write_csfc(&csfc); + ble_hs_lock(); + + conn = ble_hs_conn_find(conn_handle); + BLE_HS_DBG_ASSERT(conn != NULL); + #if MYNEWT_VAL(BLE_GATT_CACHING) /* store the bonded peer aware_state if space not available delete the @@ -2420,6 +2451,8 @@ ble_gatts_bonding_restored(uint16_t conn_handle) { struct ble_store_value_cccd cccd_value; struct ble_store_key_cccd cccd_key; + struct ble_store_value_csfc csfc_value; + struct ble_store_key_csfc csfc_key; struct ble_gatts_clt_cfg *clt_cfg; struct ble_hs_conn *conn; uint8_t att_op; @@ -2519,6 +2552,14 @@ ble_gatts_bonding_restored(uint16_t conn_handle) cccd_key.idx++; } + + memset(&csfc_key, 0, sizeof csfc_key); + csfc_key.peer_addr = conn->bhc_peer_addr; + rc = ble_store_read_csfc(&csfc_key, &csfc_value); + if (rc != 0) { + return; + } + memcpy(conn->bhc_gatt_svr.peer_cl_sup_feat, csfc_value.csfc, MYNEWT_VAL(BLE_GATT_CSFC_SIZE)); } #if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) diff --git a/nimble/host/src/ble_store.c b/nimble/host/src/ble_store.c index 8657f09a6..7c1e1b6f5 100644 --- a/nimble/host/src/ble_store.c +++ b/nimble/host/src/ble_store.c @@ -285,6 +285,42 @@ ble_store_delete_cccd(const struct ble_store_key_cccd *key) return rc; } +int +ble_store_read_csfc(const struct ble_store_key_csfc *key, + struct ble_store_value_csfc *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_CSFC, store_key, store_value); + return rc; +} + +int +ble_store_write_csfc(const struct ble_store_value_csfc *value) +{ + union ble_store_value *store_value; + int rc; + + store_value = (void *)value; + rc = ble_store_write(BLE_STORE_OBJ_TYPE_CSFC, store_value); + return rc; +} + +int +ble_store_delete_csfc(const struct ble_store_key_csfc *key) +{ + union ble_store_key *store_key; + int rc; + + store_key = (void *)key; + rc = ble_store_delete(BLE_STORE_OBJ_TYPE_CSFC, store_key); + return rc; +} + void ble_store_key_from_value_cccd(struct ble_store_key_cccd *out_key, const struct ble_store_value_cccd *value) @@ -440,6 +476,14 @@ ble_store_key_from_value_local_irk(struct ble_store_key_local_irk *out_key, out_key->idx = 0; } +void +ble_store_key_from_value_csfc(struct ble_store_key_csfc *out_key, + const struct ble_store_value_csfc *value) +{ + out_key->peer_addr = value->peer_addr; + out_key->idx = 0; +} + void ble_store_key_from_value(int obj_type, union ble_store_key *out_key, @@ -460,11 +504,17 @@ ble_store_key_from_value(int obj_type, break; #endif case BLE_STORE_OBJ_TYPE_PEER_ADDR: - ble_store_key_from_value_rpa_rec(&out_key->rpa_rec, &value->rpa_rec); - break; + ble_store_key_from_value_rpa_rec(&out_key->rpa_rec, &value->rpa_rec); + break; + case BLE_STORE_OBJ_TYPE_LOCAL_IRK: - ble_store_key_from_value_local_irk(&out_key->local_irk, &value->local_irk); - break; + ble_store_key_from_value_local_irk(&out_key->local_irk, &value->local_irk); + break; + + case BLE_STORE_OBJ_TYPE_CSFC: + ble_store_key_from_value_csfc(&out_key->csfc, &value->csfc); + break; + default: BLE_HS_DBG_ASSERT(0); break; @@ -508,6 +558,10 @@ ble_store_iterate(int obj_type, key.local_irk.addr = *BLE_ADDR_ANY; pidx = &key.local_irk.idx; break; + case BLE_STORE_OBJ_TYPE_CSFC: + key.csfc.peer_addr = *BLE_ADDR_ANY; + pidx = &key.csfc.idx; + break; default: BLE_HS_DBG_ASSERT(0); return BLE_HS_EINVAL; @@ -552,6 +606,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_CSFC, BLE_STORE_OBJ_TYPE_PEER_ADDR, BLE_STORE_OBJ_TYPE_LOCAL_IRK, #if MYNEWT_VAL(ENC_ADV_DATA) diff --git a/nimble/host/src/ble_store_util.c b/nimble/host/src/ble_store_util.c index eba5a82bb..a568dc1ff 100644 --- a/nimble/host/src/ble_store_util.c +++ b/nimble/host/src/ble_store_util.c @@ -157,6 +157,14 @@ ble_store_util_delete_peer(const ble_addr_t *peer_id_addr) } #endif + memset(&key, 0, sizeof key); + key.csfc.peer_addr = *peer_id_addr; + + rc = ble_store_util_delete_all(BLE_STORE_OBJ_TYPE_CSFC, &key); + if (rc != 0) { + 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); @@ -348,6 +356,7 @@ ble_store_util_status_rr(struct ble_store_status_event *event, void *arg) case BLE_STORE_OBJ_TYPE_PEER_ADDR: return ble_gap_unpair_oldest_peer(); case BLE_STORE_OBJ_TYPE_CCCD: + case BLE_STORE_OBJ_TYPE_CSFC: /* Try unpairing oldest peer except current peer */ return ble_gap_unpair_oldest_except(&event->overflow.value->cccd.peer_addr); #if MYNEWT_VAL(ENC_ADV_DATA) diff --git a/nimble/host/store/config/src/ble_store_config.c b/nimble/host/store/config/src/ble_store_config.c index a7eaf6886..4900fe960 100644 --- a/nimble/host/store/config/src/ble_store_config.c +++ b/nimble/host/store/config/src/ble_store_config.c @@ -49,6 +49,12 @@ struct ble_store_value_local_irk ble_store_config_local_irks[MYNEWT_VAL(BLE_STORE_MAX_BONDS)]; int ble_store_config_num_local_irks; +#if MYNEWT_VAL(BLE_STORE_MAX_CSFCS) +struct ble_store_value_csfc + ble_store_config_csfcs[MYNEWT_VAL(BLE_STORE_MAX_CSFCS)]; +int ble_store_config_num_csfcs; +#endif + #if MYNEWT_VAL(ENC_ADV_DATA) struct ble_store_value_ead ble_store_config_eads[MYNEWT_VAL(BLE_STORE_MAX_EADS)]; @@ -862,6 +868,110 @@ ble_store_config_write_local_irk(const struct ble_store_value_local_irk *value_i } +/***************************************************************************** + * $csfc * + *****************************************************************************/ + +static int +ble_store_config_find_csfc(const struct ble_store_key_csfc *key) +{ + struct ble_store_value_csfc *csfc; + int skipped; + int i; + + skipped = 0; + for (i = 0; i < ble_store_config_num_csfcs; i++) { + csfc = ble_store_config_csfcs + i; + + if (ble_addr_cmp(&key->peer_addr, BLE_ADDR_ANY)) { + if (ble_addr_cmp(&csfc->peer_addr, &key->peer_addr)) { + continue; + } + } + + if (key->idx > skipped) { + skipped++; + continue; + } + + return i; + } + + return -1; +} + +static int +ble_store_config_delete_csfc(const struct ble_store_key_csfc *key_csfc) +{ + int idx; + int rc; + + idx = ble_store_config_find_csfc(key_csfc); + if (idx == -1) { + return BLE_HS_ENOENT; + } + + rc = ble_store_config_delete_obj(ble_store_config_csfcs, + sizeof *ble_store_config_csfcs, + idx, &ble_store_config_num_csfcs); + + if (rc != 0) { + return rc; + } + + rc = ble_store_config_persist_csfcs(); + if (rc != 0) { + return rc; + } + + return 0; +} + +static int +ble_store_config_read_csfc(const struct ble_store_key_csfc *key_csfc, + struct ble_store_value_csfc *value_csfc) +{ + int idx; + + idx = ble_store_config_find_csfc(key_csfc); + if (idx == -1) { + return BLE_HS_ENOENT; + } + + *value_csfc = ble_store_config_csfcs[idx]; + return 0; +} + +static int +ble_store_config_write_csfc(const struct ble_store_value_csfc *value_csfc) +{ + struct ble_store_key_csfc key_csfc; + int idx; + int rc; + + ble_store_key_from_value_csfc(&key_csfc, value_csfc); + idx = ble_store_config_find_csfc(&key_csfc); + if (idx == -1) { + if (ble_store_config_num_csfcs >= MYNEWT_VAL(BLE_STORE_MAX_CSFCS)) { + BLE_HS_LOG(DEBUG, "error persisting csfc; too many entries (%d)\n", + ble_store_config_num_csfcs); + return BLE_HS_ESTORE_CAP; + } + + idx = ble_store_config_num_csfcs; + ble_store_config_num_csfcs++; + } + + ble_store_config_csfcs[idx] = *value_csfc; + + rc = ble_store_config_persist_csfcs(); + if (rc != 0) { + return rc; + } + + return 0; +} + /***************************************************************************** * $api * *****************************************************************************/ @@ -912,6 +1022,10 @@ ble_store_config_read(int obj_type, const union ble_store_key *key, rc = ble_store_config_read_local_irk(&key->local_irk, &value->local_irk); return rc; + case BLE_STORE_OBJ_TYPE_CSFC: + rc = ble_store_config_read_csfc(&key->csfc, &value->csfc); + return rc; + #if MYNEWT_VAL(ENC_ADV_DATA) case BLE_STORE_OBJ_TYPE_ENC_ADV_DATA: rc = ble_store_config_read_ead(&key->ead, &value->ead); @@ -947,7 +1061,6 @@ ble_store_config_write(int obj_type, const union ble_store_value *val) rc = ble_store_config_write_cccd(&val->cccd); return rc; - case BLE_STORE_OBJ_TYPE_PEER_ADDR: rc = ble_store_config_write_rpa_rec(&val->rpa_rec); return rc; @@ -955,6 +1068,11 @@ ble_store_config_write(int obj_type, const union ble_store_value *val) case BLE_STORE_OBJ_TYPE_LOCAL_IRK: rc = ble_store_config_write_local_irk(&val->local_irk); return rc; + + case BLE_STORE_OBJ_TYPE_CSFC: + rc = ble_store_config_write_csfc(&val->csfc); + return rc; + #if MYNEWT_VAL(ENC_ADV_DATA) case BLE_STORE_OBJ_TYPE_ENC_ADV_DATA: rc = ble_store_config_write_ead(&val->ead); @@ -984,7 +1102,6 @@ ble_store_config_delete(int obj_type, const union ble_store_key *key) rc = ble_store_config_delete_cccd(&key->cccd); return rc; - case BLE_STORE_OBJ_TYPE_PEER_ADDR: rc = ble_store_config_delete_rpa_rec(&key->rpa_rec); return rc; @@ -993,6 +1110,10 @@ ble_store_config_delete(int obj_type, const union ble_store_key *key) rc = ble_store_config_delete_local_irk(&key->local_irk); return rc; + case BLE_STORE_OBJ_TYPE_CSFC: + rc = ble_store_config_delete_csfc(&key->csfc); + return rc; + #if MYNEWT_VAL(ENC_ADV_DATA) case BLE_STORE_OBJ_TYPE_ENC_ADV_DATA: rc = ble_store_config_delete_ead(&key->ead); @@ -1019,9 +1140,9 @@ ble_store_config_init(void) ble_store_config_num_our_secs = 0; ble_store_config_num_peer_secs = 0; ble_store_config_num_cccds = 0; - ble_store_config_num_rpa_recs = 0; ble_store_config_num_local_irks=0; + ble_store_config_num_csfcs = 0; #if MYNEWT_VAL(ENC_ADV_DATA) ble_store_config_num_eads = 0; #endif 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 fb33553bf..93d36f618 100644 --- a/nimble/host/store/config/src/ble_store_config_conf.c +++ b/nimble/host/store/config/src/ble_store_config_conf.c @@ -62,6 +62,13 @@ static struct conf_handler ble_store_config_conf_handler = { #define BLE_STORE_CONFIG_RPA_REC_SET_ENCODE_SZ \ (MYNEWT_VAL(BLE_STORE_MAX_BONDS) * BLE_STORE_CONFIG_RPA_REC_ENCODE_SZ + 1) + +#define BLE_STORE_CONFIG_CSFC_ENCODE_SZ \ + BASE64_ENCODE_SIZE(sizeof (struct ble_store_value_csfc)) + +#define BLE_STORE_CONFIG_CSFC_ENCODE_SZ \ + BASE64_ENCODE_SIZE(sizeof (struct ble_store_value_csfc)) + #if MYNEWT_VAL(ENC_ADV_DATA) #define BLE_STORE_CONFIG_EAD_ENCODE_SZ \ BASE64_ENCODE_SIZE(sizeof (struct ble_store_value_ead)) @@ -126,6 +133,13 @@ ble_store_config_conf_set(int argc, char **argv, char *val) sizeof *ble_store_config_cccds, &ble_store_config_num_cccds); return rc; + } else if (strcmp(argv[0], "csfc") == 0) { + rc = ble_store_config_deserialize_arr( + val, + ble_store_config_csfcs, + sizeof *ble_store_config_csfcs, + &&ble_store_config_num_csfcs); + return rc; } else if (strcmp(argv[0],"rpa_rec") == 0){ rc = ble_store_config_deserialize_arr( @@ -186,6 +200,14 @@ ble_store_config_conf_export(void (*func)(char *name, char *val), buf.rpa_rec, sizeof buf.rpa_rec); func("ble_hs/rpa_rec", buf.rpa_rec); + + ble_store_config_serialize_arr(ble_store_config_csfcs, + sizeof *ble_store_config_csfcs, + ble_store_config_num_csfcs, + buf.csfc, + sizeof buf.csfc); + func("ble_hs/csfc", buf.csfc); + #if MYNEWT_VAL(ENC_ADV_DATA) ble_store_config_serialize_arr(ble_store_config_eads, sizeof *ble_store_config_eads, @@ -264,6 +286,25 @@ ble_store_config_persist_cccds(void) return 0; } +int +ble_store_config_persist_csfcs(void) +{ + char buf[BLE_STORE_CONFIG_CSFC_SET_ENCODE_SZ]; + int rc; + + ble_store_config_serialize_arr(ble_store_config_csfcs, + sizeof *ble_store_config_csfcs, + ble_store_config_num_csfcs, + buf, + sizeof buf); + rc = conf_save_one("ble_hs/csfc", buf); + if (rc != 0) { + return BLE_HS_ESTORE_FAIL; + } + + return 0; +} + #if MYNEWT_VAL(ENC_ADV_DATA) int ble_store_config_persist_eads(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 5380e137e..4d84632ac 100644 --- a/nimble/host/store/config/src/ble_store_config_priv.h +++ b/nimble/host/store/config/src/ble_store_config_priv.h @@ -36,6 +36,9 @@ extern struct ble_store_value_cccd ble_store_config_cccds[MYNEWT_VAL(BLE_STORE_MAX_CCCDS)]; extern int ble_store_config_num_cccds; +extern struct ble_store_value_csfc + ble_store_config_csfcs[MYNEWT_VAL(BLE_STORE_MAX_CSFCS)]; +extern int ble_store_config_num_csfcs; extern struct ble_store_value_rpa_rec ble_store_config_rpa_recs[MYNEWT_VAL(BLE_STORE_MAX_BONDS)]; @@ -56,6 +59,7 @@ extern int ble_store_config_num_eads; int ble_store_config_persist_our_secs(void); int ble_store_config_persist_peer_secs(void); int ble_store_config_persist_cccds(void); +int ble_store_config_persist_csfcs(void); #if MYNEWT_VAL(BLE_STORE_MAX_BONDS) int ble_restore_our_sec_nvs(void); int ble_restore_peer_sec_nvs(void); @@ -74,6 +78,7 @@ 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 int ble_store_config_persist_rpa_recs(void) { return 0; } static inline int ble_store_config_persist_local_irk(void) { return 0; } +static inline int ble_store_config_persist_csfcs(void) { return 0; } #if MYNEWT_VAL(ENC_ADV_DATA) static inline int ble_store_config_persist_eads(void) { return 0; } #endif diff --git a/nimble/host/store/config/src/ble_store_nvs.c b/nimble/host/store/config/src/ble_store_nvs.c index 3acc11f73..28e645621 100644 --- a/nimble/host/store/config/src/ble_store_nvs.c +++ b/nimble/host/store/config/src/ble_store_nvs.c @@ -39,6 +39,7 @@ #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_CSFC_SEC_KEY "csfc_sec" #define NIMBLE_NVS_PEER_RECORDS_KEY "p_dev_rec" #define NIMBLE_NVS_NAMESPACE "nimble_bond" #define NIMBLE_NVS_RPA_RECORDS_KEY "rpa_rec" @@ -68,12 +69,14 @@ get_nvs_key_string(int obj_type, int index, char *key_string) } else if (obj_type == BLE_STORE_OBJ_TYPE_ENC_ADV_DATA) { sprintf(key_string, "%s_%d", NIMBLE_NVS_EAD_SEC_KEY, index); #endif - } else if (obj_type == BLE_STORE_OBJ_TYPE_PEER_ADDR){ + } else if (obj_type == BLE_STORE_OBJ_TYPE_PEER_ADDR){ sprintf(key_string, "%s_%d", NIMBLE_NVS_RPA_RECORDS_KEY, index); - } else if (obj_type == BLE_STORE_OBJ_TYPE_LOCAL_IRK) { + } else if (obj_type == BLE_STORE_OBJ_TYPE_LOCAL_IRK) { sprintf(key_string, "%s_%d", NIMBLE_NVS_LOCAL_IRK_KEY, index); - } else { + } else if (obj_type == BLE_STORE_OBJ_TYPE_CCCD) { sprintf(key_string, "%s_%d", NIMBLE_NVS_CCCD_SEC_KEY, index); + } else { + sprintf(key_string, "%s_%d", NIMBLE_NVS_CSFC_SEC_KEY, index); } } } @@ -108,6 +111,8 @@ get_nvs_max_obj_value(int obj_type) } else { if (obj_type == BLE_STORE_OBJ_TYPE_CCCD) { return MYNEWT_VAL(BLE_STORE_MAX_CCCDS); + } else if (obj_type == BLE_STORE_OBJ_TYPE_CSFC) { + return MYNEWT_VAL(BLE_STORE_MAX_CSFCS); #if MYNEWT_VAL(ENC_ADV_DATA) } else if (obj_type == BLE_STORE_OBJ_TYPE_ENC_ADV_DATA) { return MYNEWT_VAL(BLE_STORE_MAX_EADS); @@ -177,10 +182,12 @@ get_nvs_db_value(int obj_type, char *key_string, union ble_store_value *val) } else if (obj_type == BLE_STORE_OBJ_TYPE_PEER_ADDR) { err = nvs_get_blob(nimble_handle, key_string, &val->rpa_rec, &required_size); - } else if (obj_type == BLE_STORE_OBJ_TYPE_LOCAL_IRK) { err = nvs_get_blob (nimble_handle, key_string, &val->local_irk, &required_size); + } else if (obj_type == BLE_STORE_OBJ_TYPE_CSFC) { + err = nvs_get_blob(nimble_handle, key_string, &val->csfc, + &required_size); #if MYNEWT_VAL(ENC_ADV_DATA) } else if (obj_type == BLE_STORE_OBJ_TYPE_ENC_ADV_DATA) { err = nvs_get_blob(nimble_handle, key_string, &val->ead, @@ -252,6 +259,9 @@ get_nvs_db_attribute(int obj_type, bool empty, void *value, int num_value) if (obj_type == BLE_STORE_OBJ_TYPE_CCCD) { err = get_nvs_matching_index(&cur.sec, value, num_value, sizeof(struct ble_store_value_cccd)); + } else if (obj_type == BLE_STORE_OBJ_TYPE_CSFC) { + err = get_nvs_matching_index(&cur.csfc, value, num_value, + sizeof(struct ble_store_value_csfc)); #if MYNEWT_VAL(ENC_ADV_DATA) } else if (obj_type == BLE_STORE_OBJ_TYPE_ENC_ADV_DATA) { err = get_nvs_matching_index(&cur.sec, value, num_value, @@ -389,6 +399,7 @@ ble_store_nvs_write(int obj_type, const union ble_store_value *val) if (obj_type == BLE_STORE_OBJ_TYPE_CCCD) { return ble_nvs_write_key_value(key_string, &val->cccd, sizeof(struct ble_store_value_cccd)); + } 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)); @@ -396,6 +407,11 @@ ble_store_nvs_write(int obj_type, const union ble_store_value *val) } else if (obj_type == BLE_STORE_OBJ_TYPE_LOCAL_IRK) { return ble_nvs_write_key_value(key_string, &val->local_irk, sizeof(struct ble_store_value_local_irk)); + + } else if (obj_type == BLE_STORE_OBJ_TYPE_CSFC) { + return ble_nvs_write_key_value(key_string, &val->csfc, sizeof(struct + ble_store_value_csfc)); + #if MYNEWT_VAL(ENC_ADV_DATA) } else if (obj_type == BLE_STORE_OBJ_TYPE_ENC_ADV_DATA) { return ble_nvs_write_key_value(key_string, &val->ead, sizeof(struct @@ -498,6 +514,12 @@ populate_db_from_nvs(int obj_type, void *dst, int *db_num) db_item += sizeof(struct ble_store_value_local_irk); (*db_num)++; + } else if (obj_type == BLE_STORE_OBJ_TYPE_CSFC) { + ESP_LOGD(TAG, "CSFC in RAM is filled up from NVS index = %d", i); + memcpy(db_item, &cur.csfc, sizeof(struct ble_store_value_csfc)); + db_item += sizeof(struct ble_store_value_csfc); + (*db_num)++; + #if MYNEWT_VAL(ENC_ADV_DATA) } if (obj_type == BLE_STORE_OBJ_TYPE_ENC_ADV_DATA) { ESP_LOGD(TAG, "EAD in RAM is filled up from NVS index = %d", i); @@ -597,6 +619,17 @@ ble_nvs_restore_sec_keys(void) ESP_LOGD(TAG, "ble_store_config_local_irks restored %d irks", ble_store_config_num_local_irks); +#if MYNEWT_VAL(BLE_STORE_MAX_CSFCS) + err = populate_db_from_nvs(BLE_STORE_OBJ_TYPE_CSFC, ble_store_config_csfcs, + &ble_store_config_num_csfcs); + if (err != ESP_OK) { + ESP_LOGE(TAG, "NVS operation failed for 'CSFC'"); + return err; + } + ESP_LOGD(TAG, "ble_store_config_csfcs restored %d bonds", + ble_store_config_num_csfcs); +#endif + #if MYNEWT_VAL(ENC_ADV_DATA) err = populate_db_from_nvs(BLE_STORE_OBJ_TYPE_ENC_ADV_DATA, ble_store_config_eads, &ble_store_config_num_eads); @@ -665,6 +698,38 @@ int ble_store_config_persist_cccds(void) } #endif +#if MYNEWT_VAL(BLE_STORE_MAX_CSFCS) +int ble_store_config_persist_csfcs(void) +{ + int nvs_count, nvs_idx; + union ble_store_value val; + + nvs_count = get_nvs_db_attribute(BLE_STORE_OBJ_TYPE_CSFC, 0, NULL, 0); + if (nvs_count == -1) { + ESP_LOGE(TAG, "NVS operation failed while persisting CSFC"); + return BLE_HS_ESTORE_FAIL; + } + + if (nvs_count < ble_store_config_num_csfcs) { + /* NVS db count less than RAM count, write operation */ + ESP_LOGD(TAG, "Persisting CSFC value in NVS..."); + val.csfc = ble_store_config_csfcs[ble_store_config_num_csfcs - 1]; + return ble_store_nvs_write(BLE_STORE_OBJ_TYPE_CSFC, &val); + } else if (nvs_count > ble_store_config_num_csfcs) { + /* NVS db count more than RAM count, delete operation */ + nvs_idx = get_nvs_db_attribute(BLE_STORE_OBJ_TYPE_CSFC, 0, + ble_store_config_csfcs, ble_store_config_num_csfcs); + if (nvs_idx == -1) { + ESP_LOGE(TAG, "NVS delete operation failed for CSFC"); + return BLE_HS_ESTORE_FAIL; + } + ESP_LOGD(TAG, "Deleting CSFC, nvs idx = %d", nvs_idx); + return ble_nvs_delete_value(BLE_STORE_OBJ_TYPE_CSFC, nvs_idx); + } + return 0; +} +#endif + int ble_store_config_persist_rpa_recs(void) { int nvs_count, nvs_idx; diff --git a/nimble/host/store/ram/src/ble_store_ram.c b/nimble/host/store/ram/src/ble_store_ram.c index 2b93a4f25..e9fb7ab63 100644 --- a/nimble/host/store/ram/src/ble_store_ram.c +++ b/nimble/host/store/ram/src/ble_store_ram.c @@ -48,6 +48,13 @@ static struct ble_store_value_cccd ble_store_ram_cccds[MYNEWT_VAL(BLE_STORE_MAX_CCCDS)]; static int ble_store_ram_num_cccds; +#if MYNEWT_VAL(BLE_STORE_MAX_CSFCS) +static struct ble_store_value_csfc + ble_store_ram_csfcs[MYNEWT_VAL(BLE_STORE_MAX_CSFCS)]; +#endif + +static int ble_store_ram_num_csfcs; + #if MYNEWT_VAL(ENC_ADV_DATA) static struct ble_store_value_ead ble_store_ram_eads[MYNEWT_VAL(BLE_STORE_MAX_EADS)]; @@ -392,6 +399,111 @@ ble_store_ram_write_cccd(const struct ble_store_value_cccd *value_cccd) return 0; } +/***************************************************************************** + * $csfc * + *****************************************************************************/ + +#if MYNEWT_VAL(BLE_STORE_MAX_CSFCS) +static int +ble_store_ram_find_csfc(const struct ble_store_key_csfc *key) +{ + struct ble_store_value_csfc *csfc; + int skipped; + int i; + + skipped = 0; + for (i = 0; i < ble_store_ram_num_csfcs; i++) { + csfc = ble_store_ram_csfcs + i; + + if (ble_addr_cmp(&key->peer_addr, BLE_ADDR_ANY)) { + if (ble_addr_cmp(&csfc->peer_addr, &key->peer_addr)) { + continue; + } + } + + if (key->idx > skipped) { + skipped++; + continue; + } + + return i; + } + + return -1; +} +#endif + +static int +ble_store_ram_delete_csfc(const struct ble_store_key_csfc *key_csfc) +{ +#if MYNEWT_VAL(BLE_STORE_MAX_CSFCS) + int idx; + int rc; + + idx = ble_store_ram_find_csfc(key_csfc); + if (idx == -1) { + return BLE_HS_ENOENT; + } + + rc = ble_store_ram_delete_obj(ble_store_ram_csfcs, + sizeof *ble_store_ram_csfcs, + idx, + &ble_store_ram_num_csfcs); + if (rc != 0) { + return rc; + } + return 0; +#else + return BLE_HS_ENOENT; +#endif +} + +static int +ble_store_ram_read_csfc(const struct ble_store_key_csfc *key_csfc, + struct ble_store_value_csfc *value_csfc) +{ +#if MYNEWT_VAL(BLE_STORE_MAX_CSFCS) + int idx; + + idx = ble_store_ram_find_csfc(key_csfc); + if (idx == -1) { + return BLE_HS_ENOENT; + } + + *value_csfc = ble_store_ram_csfcs[idx]; + + return 0; +#else + return BLE_HS_ENOENT; +#endif +} + +static int +ble_store_ram_write_csfc(const struct ble_store_value_csfc *value_csfc) +{ +#if MYNEWT_VAL(BLE_STORE_MAX_CSFCS) + struct ble_store_key_csfc key_csfc; + int idx; + + ble_store_key_from_value_csfc(&key_csfc, value_csfc); + idx = ble_store_ram_find_csfc(&key_csfc); + if (idx == -1) { + if (ble_store_ram_num_csfcs >= MYNEWT_VAL(BLE_STORE_MAX_CSFCS)) { + BLE_HS_LOG(DEBUG, "error persisting csfc; too many entries (%d)\n", + ble_store_ram_num_csfcs); + return BLE_HS_ESTORE_CAP; + } + + idx = ble_store_ram_num_csfcs; + ble_store_ram_num_csfcs++; + } + + ble_store_ram_csfcs[idx] = *value_csfc; + return 0; +#else + return BLE_HS_ENOENT; +#endif +} /***************************************************************************** * $ead * @@ -527,6 +639,10 @@ ble_store_ram_read(int obj_type, const union ble_store_key *key, rc = ble_store_ram_read_cccd(&key->cccd, &value->cccd); return rc; + case BLE_STORE_OBJ_TYPE_CSFC: + rc = ble_store_ram_read_csfc(&key->csfc, &value->csfc); + return rc; + #if MYNEWT_VAL(ENC_ADV_DATA) case BLE_STORE_OBJ_TYPE_ENC_ADV_DATA: rc = ble_store_ram_read_ead(&key->ead, &value->ead); @@ -562,6 +678,10 @@ ble_store_ram_write(int obj_type, const union ble_store_value *val) rc = ble_store_ram_write_cccd(&val->cccd); return rc; + case BLE_STORE_OBJ_TYPE_CSFC: + rc = ble_store_ram_write_csfc(&val->csfc); + return rc; + #if MYNEWT_VAL(ENC_ADV_DATA) case BLE_STORE_OBJ_TYPE_ENC_ADV_DATA: rc = ble_store_ram_write_ead(&val->ead); @@ -591,6 +711,10 @@ ble_store_ram_delete(int obj_type, const union ble_store_key *key) rc = ble_store_ram_delete_cccd(&key->cccd); return rc; + case BLE_STORE_OBJ_TYPE_CSFC: + rc = ble_store_ram_delete_csfc(&key->csfc); + return rc; + #if MYNEWT_VAL(ENC_ADV_DATA) case BLE_STORE_OBJ_TYPE_ENC_ADV_DATA: rc = ble_store_ram_delete_ead(&key->ead); @@ -616,6 +740,7 @@ ble_store_ram_init(void) ble_store_ram_num_our_secs = 0; ble_store_ram_num_peer_secs = 0; ble_store_ram_num_cccds = 0; + ble_store_ram_num_csfcs = 0; #if MYNEWT_VAL(ENC_ADV_DATA) ble_store_ram_num_eads = 0; #endif