diff --git a/nimble/host/include/host/ble_store.h b/nimble/host/include/host/ble_store.h index 24b208a09d..55cb17418e 100644 --- a/nimble/host/include/host/ble_store.h +++ b/nimble/host/include/host/ble_store.h @@ -31,6 +31,10 @@ extern "C" { #define BLE_STORE_OBJ_TYPE_PEER_SEC 2 #define BLE_STORE_OBJ_TYPE_CCCD 3 +#if MYNEWT_VAL(BLE_ENC_ADV_DATA) +#define BLE_STORE_OBJ_TYPE_ENC_ADV_DATA 4 +#endif + /** Failed to persist record; insufficient storage capacity. */ #define BLE_STORE_EVENT_OVERFLOW 1 @@ -112,6 +116,33 @@ struct ble_store_value_cccd { unsigned value_changed:1; }; +#if MYNEWT_VAL(BLE_ENC_ADV_DATA) +/** + * Used as a key for lookups of encrypted advertising data. This struct corresponds + * to the BLE_STORE_OBJ_TYPE_ENC_ADV_DATA store object type. + */ +struct ble_store_key_ead { + /** + * Key by peer identity address; + * peer_addr=BLE_ADDR_NONE means don't key off peer. + */ + ble_addr_t peer_addr; + + /** Number of results to skip; 0 means retrieve the first match. */ + uint8_t idx; +}; + +/** + * Represents a stored encrypted advertising data. This struct corresponds + * to the BLE_STORE_OBJ_TYPE_ENC_ADV_DATA store object type. + */ +struct ble_store_value_ead { + ble_addr_t peer_addr; + unsigned km_present : 1; + struct key_material *km; +}; +#endif + /** * Used as a key for store lookups. This union must be accompanied by an * object type code to indicate which field is valid. @@ -119,6 +150,10 @@ struct ble_store_value_cccd { union ble_store_key { struct ble_store_key_sec sec; struct ble_store_key_cccd cccd; +#if MYNEWT_VAL(BLE_ENC_ADV_DATA) + struct ble_store_key_ead ead; +#endif + }; /** @@ -128,6 +163,10 @@ union ble_store_key { union ble_store_value { struct ble_store_value_sec sec; struct ble_store_value_cccd cccd; +#if MYNEWT_VAL(BLE_ENC_ADV_DATA) + struct ble_store_value_ead ead; +#endif + }; struct ble_store_status_event { @@ -298,7 +337,14 @@ 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); - +#if MYNEWT_VAL(BLE_ENC_ADV_DATA) +int ble_store_read_ead(const struct ble_store_key_ead *key, + struct ble_store_value_ead *out_value); +int ble_store_write_ead(const struct ble_store_value_ead *value); +int ble_store_delete_ead(const struct ble_store_key_ead *key); +void ble_store_key_from_value_ead(struct ble_store_key_ead *out_key, + const struct ble_store_value_ead *value); +#endif 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_store.c b/nimble/host/src/ble_store.c index 79b2f7b95e..76dd635da7 100644 --- a/nimble/host/src/ble_store.c +++ b/nimble/host/src/ble_store.c @@ -302,6 +302,53 @@ ble_store_key_from_value_sec(struct ble_store_key_sec *out_key, out_key->idx = 0; } +#if MYNEWT_VAL(BLE_ENC_ADV_DATA) +int +ble_store_read_ead(const struct ble_store_key_ead *key, + struct ble_store_value_ead *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_ENC_ADV_DATA, store_key, store_value); + return rc; +} + +int +ble_store_write_ead(const struct ble_store_value_ead *value) +{ + union ble_store_value *store_value; + int rc; + + store_value = (void *)value; + rc = ble_store_write(BLE_STORE_OBJ_TYPE_ENC_ADV_DATA, store_value); + return rc; +} + +int +ble_store_delete_ead(const struct ble_store_key_ead *key) +{ + union ble_store_key *store_key; + int rc; + + store_key = (void *)key; + rc = ble_store_delete(BLE_STORE_OBJ_TYPE_ENC_ADV_DATA, store_key); + return rc; +} + +void +ble_store_key_from_value_ead(struct ble_store_key_ead *out_key, + const struct ble_store_value_ead *value) +{ + out_key->peer_addr = value->peer_addr; + out_key->idx = 0; +} +#endif + void ble_store_key_from_value(int obj_type, union ble_store_key *out_key, @@ -317,6 +364,12 @@ ble_store_key_from_value(int obj_type, ble_store_key_from_value_cccd(&out_key->cccd, &value->cccd); break; +#if MYNEWT_VAL(BLE_ENC_ADV_DATA) + case BLE_STORE_OBJ_TYPE_ENC_ADV_DATA: + ble_store_key_from_value_ead(&out_key->ead, &value->ead); + break; +#endif + default: BLE_HS_DBG_ASSERT(0); break; @@ -346,6 +399,13 @@ ble_store_iterate(int obj_type, key.cccd.peer_addr = *BLE_ADDR_ANY; pidx = &key.cccd.idx; break; +#if MYNEWT_VAL(BLE_ENC_ADV_DATA) + case BLE_STORE_OBJ_TYPE_ENC_ADV_DATA: + key.ead.peer_addr = *BLE_ADDR_ANY; + pidx = &key.ead.idx; + break; +#endif + default: BLE_HS_DBG_ASSERT(0); return BLE_HS_EINVAL; @@ -390,6 +450,9 @@ ble_store_clear(void) BLE_STORE_OBJ_TYPE_OUR_SEC, BLE_STORE_OBJ_TYPE_PEER_SEC, BLE_STORE_OBJ_TYPE_CCCD, +#if MYNEWT_VAL(BLE_ENC_ADV_DATA) + BLE_STORE_OBJ_TYPE_ENC_ADV_DATA, +#endif }; union ble_store_key key; int obj_type; diff --git a/nimble/host/src/ble_store_util.c b/nimble/host/src/ble_store_util.c index 508fa00db2..8509552bb3 100644 --- a/nimble/host/src/ble_store_util.c +++ b/nimble/host/src/ble_store_util.c @@ -132,6 +132,16 @@ ble_store_util_delete_peer(const ble_addr_t *peer_id_addr) return rc; } +#if MYNEWT_VAL(BLE_ENC_ADV_DATA) + memset(&key, 0, sizeof key); + key.ead.peer_addr = *peer_id_addr; + + rc = ble_store_util_delete_all(BLE_STORE_OBJ_TYPE_ENC_ADV_DATA, &key); + if (rc != 0) { + return rc; + } +#endif + return 0; } diff --git a/nimble/host/store/config/src/ble_store_config.c b/nimble/host/store/config/src/ble_store_config.c index 0220ba760e..62e82fb874 100644 --- a/nimble/host/store/config/src/ble_store_config.c +++ b/nimble/host/store/config/src/ble_store_config.c @@ -39,6 +39,12 @@ struct ble_store_value_cccd ble_store_config_cccds[MYNEWT_VAL(BLE_STORE_MAX_CCCDS)]; int ble_store_config_num_cccds; +#if MYNEWT_VAL(BLE_ENC_ADV_DATA) +struct ble_store_value_ead + ble_store_config_eads[MYNEWT_VAL(BLE_STORE_MAX_EADS)]; +int ble_store_config_num_eads; +#endif + /***************************************************************************** * $sec * *****************************************************************************/ @@ -394,6 +400,112 @@ ble_store_config_write_cccd(const struct ble_store_value_cccd *value_cccd) return 0; } +/***************************************************************************** +* $ead * +*****************************************************************************/ +#if MYNEWT_VAL(BLE_ENC_ADV_DATA) +static int +ble_store_config_find_ead(const struct ble_store_key_ead *key) +{ + struct ble_store_value_ead *ead; + int skipped; + int i; + + skipped = 0; + for (i = 0; i < ble_store_config_num_eads; i++) { + ead = ble_store_config_eads + i; + + if (ble_addr_cmp(&key->peer_addr, BLE_ADDR_ANY)) { + if (ble_addr_cmp(&ead->peer_addr, &key->peer_addr)) { + continue; + } + } + + if (key->idx > skipped) { + skipped++; + continue; + } + + return i; + } + + return -1; +} + +static int +ble_store_config_delete_ead(const struct ble_store_key_ead *key_ead) +{ + int idx; + int rc; + + idx = ble_store_config_find_ead(key_ead); + if (idx == -1) { + return BLE_HS_ENOENT; + } + + rc = ble_store_config_delete_obj(ble_store_config_eads, + sizeof *ble_store_config_eads, + idx, + &ble_store_config_num_eads); + if (rc != 0) { + return rc; + } + + rc = ble_store_config_persist_eads(); + if (rc != 0) { + return rc; + } + + return 0; +} + +static int +ble_store_config_read_ead(const struct ble_store_key_ead *key_ead, + struct ble_store_value_ead *value_ead) +{ + int idx; + + idx = ble_store_config_find_ead(key_ead); + if (idx == -1) { + return BLE_HS_ENOENT; + } + + *value_ead = ble_store_config_eads[idx]; + return 0; +} + +static int +ble_store_config_write_ead(const struct ble_store_value_ead *value_ead) +{ + struct ble_store_key_ead key_ead; + int idx; + int rc; + + ble_store_key_from_value_ead(&key_ead, value_ead); + idx = ble_store_config_find_ead(&key_ead); + + if (idx == -1) { + if (ble_store_config_num_eads >= MYNEWT_VAL(BLE_STORE_MAX_CCCDS)) { + BLE_HS_LOG(DEBUG, "error persisting ead; too many entries (%d)\n", + ble_store_config_num_eads); + return BLE_HS_ESTORE_CAP; + } + + idx = ble_store_config_num_eads; + ble_store_config_num_eads++; + } + + ble_store_config_eads[idx] = *value_ead; + + rc = ble_store_config_persist_eads(); + if (rc != 0) { + return rc; + } + + return 0; +} +#endif + /***************************************************************************** * $api * *****************************************************************************/ @@ -436,6 +548,12 @@ 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; +#if MYNEWT_VAL(BLE_ENC_ADV_DATA) + case BLE_STORE_OBJ_TYPE_ENC_ADV_DATA: + rc = ble_store_config_read_ead(&key->ead, &value->ead); + return rc; +#endif + default: return BLE_HS_ENOTSUP; } @@ -465,6 +583,12 @@ ble_store_config_write(int obj_type, const union ble_store_value *val) rc = ble_store_config_write_cccd(&val->cccd); return rc; +#if MYNEWT_VAL(BLE_ENC_ADV_DATA) + case BLE_STORE_OBJ_TYPE_ENC_ADV_DATA: + rc = ble_store_config_write_ead(&val->ead); + return rc; +#endif + default: return BLE_HS_ENOTSUP; } @@ -488,6 +612,12 @@ ble_store_config_delete(int obj_type, const union ble_store_key *key) rc = ble_store_config_delete_cccd(&key->cccd); return rc; +#if MYNEWT_VAL(BLE_ENC_ADV_DATA) + case BLE_STORE_OBJ_TYPE_ENC_ADV_DATA: + rc = ble_store_config_write_ead(&key->ead); + return rc; +#endif + default: return BLE_HS_ENOTSUP; } @@ -507,6 +637,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; +#if MYNEWT_VAL(BLE_ENC_ADV_DATA) + ble_store_config_num_eads = 0; +#endif 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 e74127ae93..aae2df135a 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,14 @@ 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) +#if MYNEWT_VAL(BLE_ENC_ADV_DATA) +#define BLE_STORE_CONFIG_EAD_ENCODE_SZ \ + BASE64_ENCODE_SIZE(sizeof (struct ble_store_value_ead)) + +#define BLE_STORE_CONFIG_EAD_SET_ENCODE_SZ \ + (MYNEWT_VAL(BLE_STORE_MAX_EADS) * BLE_STORE_CONFIG_EAD_ENCODE_SZ + 1) +#endif + static void ble_store_config_serialize_arr(const void *arr, int obj_sz, int num_objs, char *out_buf, int buf_sz) @@ -114,6 +122,16 @@ ble_store_config_conf_set(int argc, char **argv, char *val) &ble_store_config_num_cccds); return rc; } +#if MYNEWT_VAL(BLE_ENC_ADV_DATA) + else if (strcmp(argv[0], "ead") == 0) { + rc = ble_store_config_deserialize_arr( + val, + ble_store_config_eads, + sizeof *ble_store_config_eads, + &ble_store_config_num_eads); + return rc; + } +#endif } return OS_ENOENT; } @@ -148,6 +166,14 @@ ble_store_config_conf_export(void (*func)(char *name, char *val), sizeof buf.cccd); func("ble_hs/cccd", buf.cccd); +#if MYNEWT_VAL(BLE_ENC_ADV_DATA) + ble_store_config_serialize_arr(ble_store_config_eads, + sizeof *ble_store_config_eads, + ble_store_config_num_eads, + buf.ead, + sizeof buf.ead); + func("ble_hs/ead", buf.ead); +#endif return 0; } @@ -218,6 +244,25 @@ ble_store_config_persist_cccds(void) return 0; } +#if MYNEWT_VAL(BLE_ENC_ADV_DATA) +int +ble_store_config_persist_eads(void) +{ + char buf[BLE_STORE_CONFIG_CCCD_SET_ENCODE_SZ]; + int rc; + ble_store_config_serialize_arr(ble_store_config_eads, + sizeof *ble_store_config_eads, + ble_store_config_num_eads, + buf, + sizeof buf); + rc = conf_save_one("ble_hs/ead", buf); + if (rc != 0) { + return BLE_HS_ESTORE_FAIL; + } + return 0; +} +#endif + 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 bae90e9765..9b9be9bbf7 100644 --- a/nimble/host/store/config/src/ble_store_config_priv.h +++ b/nimble/host/store/config/src/ble_store_config_priv.h @@ -36,18 +36,30 @@ extern struct ble_store_value_cccd ble_store_config_cccds[MYNEWT_VAL(BLE_STORE_MAX_CCCDS)]; extern int ble_store_config_num_cccds; +#if MYNEWT_VAL(BLE_ENC_ADV_DATA) +extern struct ble_store_value_ead + ble_store_config_eads[MYNEWT_VAL(BLE_STORE_MAX_EADS)]; +extern int ble_store_config_num_eads; +#endif + #if MYNEWT_VAL(BLE_STORE_CONFIG_PERSIST) int ble_store_config_persist_our_secs(void); int ble_store_config_persist_peer_secs(void); int ble_store_config_persist_cccds(void); void ble_store_config_conf_init(void); +#if MYNEWT_VAL(BLE_ENC_ADV_DATA) +int ble_store_config_persist_eads(void); +#endif #else 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; } +#if MYNEWT_VAL(BLE_ENC_ADV_DATA) +static inline int ble_store_config_persist_eads(void) { return 0; } +#endif static inline void ble_store_config_conf_init(void) { } #endif /* MYNEWT_VAL(BLE_STORE_CONFIG_PERSIST) */ diff --git a/nimble/host/store/ram/src/ble_store_ram.c b/nimble/host/store/ram/src/ble_store_ram.c index 450be717d2..a33e8b4a2f 100644 --- a/nimble/host/store/ram/src/ble_store_ram.c +++ b/nimble/host/store/ram/src/ble_store_ram.c @@ -48,6 +48,12 @@ 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_ENC_ADV_DATA) +static struct ble_store_value_ead + ble_store_ram_eads[MYNEWT_VAL(BLE_STORE_MAX_EADS)]; +static int ble_store_ram_num_eads; +#endif + /***************************************************************************** * $sec * *****************************************************************************/ @@ -366,6 +372,99 @@ ble_store_ram_write_cccd(const struct ble_store_value_cccd *value_cccd) return 0; } + +/***************************************************************************** +* $ead * +*****************************************************************************/ +#if MYNEWT_VAL(BLE_ENC_ADV_DATA) +static int +ble_store_ram_find_ead(const struct ble_store_key_ead *key) +{ + struct ble_store_value_ead *ead; + int skipped; + int i; + + skipped = 0; + for (i = 0; i < ble_store_ram_num_eads; i++) { + ead = ble_store_ram_eads + i; + + if (ble_addr_cmp(&key->peer_addr, BLE_ADDR_ANY)) { + if (ble_addr_cmp(&ead->peer_addr, &key->peer_addr)) { + continue; + } + } + + if (key->idx > skipped) { + skipped++; + continue; + } + + return i; + } + + return -1; +} + +static int +ble_store_ram_delete_ead(const struct ble_store_key_ead *key_ead) +{ + int idx; + int rc; + + idx = ble_store_ram_find_ead(key_ead); + if (idx == -1) { + return BLE_HS_ENOENT; + } + rc = ble_store_ram_delete_obj(ble_store_ram_eads, + sizeof *ble_store_ram_eads, + idx, + &ble_store_ram_num_eads); + if (rc != 0) { + return rc; + } + + return 0; +} + +static int +ble_store_ram_read_ead(const struct ble_store_key_ead *key_ead, + struct ble_store_value_ead *value_ead) +{ + int idx; + + idx = ble_store_ram_find_ead(key_ead); + if (idx == -1) { + return BLE_HS_ENOENT; + } + + *value_ead = ble_store_ram_eads[idx]; + return 0; +} + +static int +ble_store_ram_write_ead(const struct ble_store_value_ead *value_ead) +{ + struct ble_store_key_ead key_ead; + int idx; + + ble_store_key_from_value_ead(&key_ead, value_ead); + idx = ble_store_ram_find_ead(&key_ead); + if (idx == -1) { + if (ble_store_ram_num_eads >= MYNEWT_VAL(BLE_STORE_MAX_EADS)) { + BLE_HS_LOG(DEBUG, "error persisting ead; too many entries (%d)\n", + ble_store_ram_num_eads); + return BLE_HS_ESTORE_CAP; + } + + idx = ble_store_ram_num_eads; + ble_store_ram_num_eads++; + } + + ble_store_ram_eads[idx] = *value_ead; + return 0; +} +#endif + /***************************************************************************** * $api * *****************************************************************************/ @@ -408,6 +507,12 @@ 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; +#if MYNEWT_VAL(BLE_ENC_ADV_DATA) + case BLE_STORE_OBJ_TYPE_ENC_ADV_DATA: + rc = ble_store_ram_read_ead(&key->ead, &value->ead); + return rc; +#endif + default: return BLE_HS_ENOTSUP; } @@ -437,6 +542,12 @@ ble_store_ram_write(int obj_type, const union ble_store_value *val) rc = ble_store_ram_write_cccd(&val->cccd); return rc; +#if MYNEWT_VAL(BLE_ENC_ADV_DATA) + case BLE_STORE_OBJ_TYPE_ENC_ADV_DATA: + rc = ble_store_ram_write_ead(&val->ead); + return rc; +#endif + default: return BLE_HS_ENOTSUP; } @@ -460,6 +571,12 @@ ble_store_ram_delete(int obj_type, const union ble_store_key *key) rc = ble_store_ram_delete_cccd(&key->cccd); return rc; +#if MYNEWT_VAL(BLE_ENC_ADV_DATA) + case BLE_STORE_OBJ_TYPE_ENC_ADV_DATA: + rc = ble_store_ram_delete_ead(&key->ead); + return rc; +#endif + default: return BLE_HS_ENOTSUP; } @@ -479,4 +596,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; +#if MYNEWT_VAL(BLE_ENC_ADV_DATA) + ble_store_ram_num_eads = 0; +#endif }