diff --git a/nimble/host/include/host/ble_store.h b/nimble/host/include/host/ble_store.h index 30a5666cfe..1749bf2fe6 100644 --- a/nimble/host/include/host/ble_store.h +++ b/nimble/host/include/host/ble_store.h @@ -290,6 +290,10 @@ int ble_store_clear(void); int ble_store_util_bonded_peers(ble_addr_t *out_peer_id_addrs, int *out_num_peers, int max_peers); +int ble_store_util_subscribed_cccds(ble_addr_t *out_peer_id_addrs, + int *out_num_peers, + int max_peers, + ble_addr_t *curr_peer_addrs); int ble_store_util_delete_all(int type, const union ble_store_key *key); int ble_store_util_delete_peer(const ble_addr_t *peer_id_addr); int ble_store_util_delete_oldest_peer(void); diff --git a/nimble/host/src/ble_store_util.c b/nimble/host/src/ble_store_util.c index 444cc55d72..bf0b3a0279 100644 --- a/nimble/host/src/ble_store_util.c +++ b/nimble/host/src/ble_store_util.c @@ -27,6 +27,11 @@ struct ble_store_util_peer_set { int status; }; +struct ble_store_util_peer_cccd_set { + struct ble_store_util_peer_set peer_set; + ble_addr_t *curr_peer_addrs; +}; + static int ble_store_util_iter_unique_peer(int obj_type, union ble_store_value *val, @@ -59,6 +64,41 @@ ble_store_util_iter_unique_peer(int obj_type, return 0; } +static int +ble_store_util_iter_peer_cccd(int obj_type, + union ble_store_value *val, + void *arg) +{ + struct ble_store_util_peer_cccd_set *set; + int i; + + set = arg; + + /* Do nothing if this peer is a duplicate or current peer */ + for (i = 0; i < set->peer_set.num_peers; i++) { + if (ble_addr_cmp(set->peer_set.peer_id_addrs + i, &val->sec.peer_addr) == 0) { + return 0; + } + + if (set->curr_peer_addrs != NULL) { + if (ble_addr_cmp(set->curr_peer_addrs, &val->sec.peer_addr) == 0) { + return 0; + } + } + } + + if (set->peer_set.num_peers >= set->peer_set.max_peers) { + /* Overflow; abort the iterate procedure. */ + set->peer_set.status = BLE_HS_ENOMEM; + return 1; + } + + set->peer_set.peer_id_addrs[set->peer_set.num_peers] = val->sec.peer_addr; + set->peer_set.num_peers++; + + return 0; +} + /** * Retrieves the set of peer addresses for which a bond has been established. * @@ -99,6 +139,51 @@ ble_store_util_bonded_peers(ble_addr_t *out_peer_id_addrs, int *out_num_peers, return 0; } +/** + * Retrieves the set of peer addresses for which CCCDs are subscribed. + * + * @param out_peer_id_addrs On success, the set of peer addresses + * gets written here. + * @param out_num_peers On success, the number of peer addresses gets written + * here. + * @param max_peers The capacity of the destination buffer. + * + * @param curr_peer_addrs Current peer's address, ignore if NULL + * + * @return 0 on success; + * BLE_HS_ENOMEM if the destination buffer is too + * small; + * Other nonzero on error. + */ +int +ble_store_util_subscribed_cccds(ble_addr_t *out_peer_id_addrs, int *out_num_peers, + int max_peers, ble_addr_t *curr_peer_addrs) +{ + struct ble_store_util_peer_cccd_set set = { + .peer_set = { + .peer_id_addrs = out_peer_id_addrs, + .num_peers = 0, + .max_peers = max_peers, + .status = 0, + }, + .curr_peer_addrs = curr_peer_addrs, + }; + int rc; + + rc = ble_store_iterate(BLE_STORE_OBJ_TYPE_CCCD, + ble_store_util_iter_peer_cccd, + &set); + if (rc != 0) { + return rc; + } + if (set.peer_set.status != 0) { + return set.peer_set.status; + } + + *out_num_peers = set.peer_set.num_peers; + return 0; +} + /** * Deletes all entries from the store that are attached to the specified peer * address. This function deletes security entries and CCCD records. @@ -230,16 +315,43 @@ ble_store_util_delete_oldest_peer(void) int ble_store_util_status_rr(struct ble_store_status_event *event, void *arg) { + int rc = BLE_HS_EUNKNOWN; + ble_addr_t peer_id_addr; + int num_peers; + switch (event->event_code) { case BLE_STORE_EVENT_OVERFLOW: switch (event->overflow.obj_type) { - case BLE_STORE_OBJ_TYPE_OUR_SEC: - case BLE_STORE_OBJ_TYPE_PEER_SEC: - case BLE_STORE_OBJ_TYPE_CCCD: - return ble_gap_unpair_oldest_peer(); - - default: - return BLE_HS_EUNKNOWN; + case BLE_STORE_OBJ_TYPE_OUR_SEC: + case BLE_STORE_OBJ_TYPE_PEER_SEC: + return ble_gap_unpair_oldest_peer(); + case BLE_STORE_OBJ_TYPE_CCCD: + if ((rc = ble_gap_unpair_oldest_peer()) == BLE_HS_ENOENT) { + /* No peer to unpair in overflow event. Here bonds do not + * exist but CCCDs are stored. Need to delete CCCDs of + * other peer address to make space.*/ + rc = ble_store_util_subscribed_cccds(&peer_id_addr, + &num_peers, 1, + (void *) &event->overflow.value->cccd.peer_addr); + if (rc != 0) { + return rc; + } else if (num_peers == 0) { + return BLE_HS_ENOMEM; + } + + union ble_store_key key = {0}; + key.cccd.peer_addr = peer_id_addr; + + rc = ble_store_util_delete_all(BLE_STORE_OBJ_TYPE_CCCD, &key); + if (rc != 0) { + return rc; + } + } + + return rc; + + default: + return BLE_HS_EUNKNOWN; } case BLE_STORE_EVENT_FULL: