diff --git a/nimble/host/include/host/ble_store.h b/nimble/host/include/host/ble_store.h index 732f4ff05..814bdcb89 100644 --- a/nimble/host/include/host/ble_store.h +++ b/nimble/host/include/host/ble_store.h @@ -38,6 +38,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 @@ -121,6 +122,29 @@ struct ble_store_value_cccd { unsigned value_changed:1; }; +/** + * 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 @@ -181,6 +205,7 @@ union ble_store_key { #endif struct ble_store_key_rpa_rec rpa_rec; struct ble_store_key_local_irk local_irk; + struct ble_store_key_csfc csfc; }; /** @@ -195,6 +220,7 @@ union ble_store_value { #endif struct ble_store_value_rpa_rec rpa_rec; struct ble_store_value_local_irk local_irk; + struct ble_store_value_csfc csfc; }; struct ble_store_status_event { @@ -361,10 +387,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 bb0a415bb..f6eb65920 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 941cdc146..a8326648d 100644 --- a/nimble/host/src/ble_gatt_priv.h +++ b/nimble/host/src/ble_gatt_priv.h @@ -92,7 +92,6 @@ 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 * @@ -121,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 84857b5b9..ace4a2497 100644 --- a/nimble/host/src/ble_gatts.c +++ b/nimble/host/src/ble_gatts.c @@ -2180,8 +2180,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, @@ -2196,7 +2196,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; @@ -2209,8 +2211,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) { @@ -2218,7 +2220,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)); } @@ -2233,7 +2235,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; @@ -2241,7 +2243,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(); @@ -2357,6 +2377,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; @@ -2401,6 +2422,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 @@ -2430,6 +2461,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; @@ -2529,6 +2562,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 d1cabfacf..a407cd8f7 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) @@ -436,6 +472,14 @@ ble_store_key_from_value_rpa_rec(struct ble_store_key_rpa_rec *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, @@ -456,12 +500,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: + case BLE_STORE_OBJ_TYPE_LOCAL_IRK: ble_store_key_from_value_local_irk(&out_key->local_irk, &value->local_irk); - break; + 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); @@ -506,6 +555,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; @@ -550,6 +603,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 b1227755b..837b8c8cf 100644 --- a/nimble/host/src/ble_store_util.c +++ b/nimble/host/src/ble_store_util.c @@ -154,6 +154,14 @@ ble_store_util_delete_peer(const ble_addr_t *peer_id_addr) return rc; } + 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); @@ -346,6 +354,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 770edfb01..3ee112eda 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_cccd int ble_store_config_num_cccds; +#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)]; @@ -896,6 +902,110 @@ ble_store_config_delete_rpa_rec(const struct ble_store_key_rpa_rec *key_rpa_rec) return 0; } +/***************************************************************************** + * $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 * *****************************************************************************/ @@ -938,6 +1048,10 @@ ble_store_config_read(int obj_type, const union ble_store_key *key, rc = ble_store_config_read_cccd(&key->cccd, &value->cccd); 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); @@ -979,6 +1093,10 @@ 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_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); @@ -1015,6 +1133,10 @@ 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_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); @@ -1047,6 +1169,7 @@ 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_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 1300bc160..30a20e9e6 100644 --- a/nimble/host/store/config/src/ble_store_config_conf.c +++ b/nimble/host/store/config/src/ble_store_config_conf.c @@ -57,6 +57,12 @@ static struct conf_handler ble_store_config_conf_handler = { #define BLE_STORE_CONFIG_CCCD_SET_ENCODE_SZ \ (MYNEWT_VAL(BLE_STORE_MAX_CCCDS) * BLE_STORE_CONFIG_CCCD_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)) @@ -127,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; } #if MYNEWT_VAL(ENC_ADV_DATA) else if (strcmp(argv[0], "ead") == 0) { @@ -181,6 +194,13 @@ ble_store_config_conf_export(void (*func)(char *name, char *val), sizeof buf.cccd); func("ble_hs/cccd", buf.cccd); + 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 +284,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 581cefdc9..a3dcc6aa4 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,10 @@ 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; + #if MYNEWT_VAL(ENC_ADV_DATA) extern struct ble_store_value_ead ble_store_config_eads[MYNEWT_VAL(BLE_STORE_MAX_EADS)]; @@ -56,6 +60,7 @@ 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); +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); @@ -72,6 +77,7 @@ void ble_store_config_conf_init(void); static inline int ble_store_config_persist_our_secs(void) { return 0; } 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_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 00339c29c..c76d496f6 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" @@ -74,8 +75,10 @@ get_nvs_key_string(int obj_type, int index, char *key_string) } else if (obj_type == BLE_STORE_OBJ_TYPE_PEER_ADDR){ sprintf(key_string, "%s_%d", NIMBLE_NVS_RPA_RECORDS_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); } } } @@ -110,6 +113,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); @@ -176,6 +181,9 @@ get_nvs_db_value(int obj_type, char *key_string, union ble_store_value *val) if (obj_type == BLE_STORE_OBJ_TYPE_CCCD) { err = nvs_get_blob(nimble_handle, key_string, &val->cccd, &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, @@ -255,6 +263,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, @@ -392,6 +403,9 @@ 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_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 @@ -489,6 +503,11 @@ populate_db_from_nvs(int obj_type, void *dst, int *db_num) memcpy(db_item, &cur.cccd, sizeof(struct ble_store_value_cccd)); db_item += sizeof(struct ble_store_value_cccd); (*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); @@ -581,6 +600,17 @@ ble_nvs_restore_sec_keys(void) ble_store_config_num_cccds); #endif +#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); @@ -667,6 +697,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 + #if MYNEWT_VAL(ENC_ADV_DATA) int ble_store_config_persist_eads(void) { diff --git a/nimble/host/store/ram/src/ble_store_ram.c b/nimble/host/store/ram/src/ble_store_ram.c index d587b8ac8..e00686f73 100644 --- a/nimble/host/store/ram/src/ble_store_ram.c +++ b/nimble/host/store/ram/src/ble_store_ram.c @@ -57,6 +57,13 @@ static struct ble_store_value_cccd 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)]; @@ -432,6 +439,111 @@ ble_store_ram_write_cccd(const struct ble_store_value_cccd *value_cccd) } +/***************************************************************************** + * $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 * @@ -567,6 +679,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); @@ -602,6 +718,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); @@ -631,6 +751,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); @@ -656,6 +780,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