diff --git a/subsys/bluetooth/host/gatt.c b/subsys/bluetooth/host/gatt.c index e4e510d8d96f58..4e89b62876f0bb 100644 --- a/subsys/bluetooth/host/gatt.c +++ b/subsys/bluetooth/host/gatt.c @@ -998,12 +998,18 @@ struct bt_gatt_attr *bt_gatt_attr_next(const struct bt_gatt_attr *attr) return next; } -static struct bt_gatt_ccc_cfg *find_ccc_cfg(struct bt_conn *conn, - struct _bt_gatt_ccc *ccc) +static void clear_ccc_cfg(struct bt_gatt_ccc_cfg *cfg) { - size_t i; + bt_addr_le_copy(&cfg->peer, BT_ADDR_LE_ANY); + cfg->id = 0U; + cfg->value = 0U; + memset(cfg->data, 0, sizeof(cfg->data)); +} - for (i = 0; i < ccc->cfg_len; i++) { +static struct bt_gatt_ccc_cfg *find_ccc_cfg(const struct bt_conn *conn, + const struct _bt_gatt_ccc *ccc) +{ + for (size_t i = 0; i < ccc->cfg_len; i++) { if (conn) { if (conn->id == ccc->cfg[i].id && !bt_conn_addr_le_cmp(conn, &ccc->cfg[i].peer)) { @@ -1094,6 +1100,7 @@ ssize_t bt_gatt_attr_write_ccc(struct bt_conn *conn, } bt_addr_le_copy(&cfg->peer, &conn->le.dst); + cfg->id = conn->id; } /* Confirm write if cfg is managed by application */ @@ -1125,8 +1132,7 @@ ssize_t bt_gatt_attr_write_ccc(struct bt_conn *conn, /* Disabled CCC is the same as no configured CCC, so clear the entry */ if (!value) { - bt_addr_le_copy(&cfg->peer, BT_ADDR_LE_ANY); - cfg->value = 0U; + clear_ccc_cfg(cfg); } return len; @@ -1552,8 +1558,7 @@ static u8_t disconnected_cb(const struct bt_gatt_attr *attr, void *user_data) } else { /* Clear value if not paired */ if (!bt_addr_le_is_bonded(conn->id, &conn->le.dst)) { - bt_addr_le_copy(&cfg->peer, BT_ADDR_LE_ANY); - cfg->value = 0U; + clear_ccc_cfg(cfg); } else { /* Update address in case it has changed */ bt_addr_le_copy(&cfg->peer, &conn->le.dst); @@ -3008,13 +3013,13 @@ void bt_gatt_disconnected(struct bt_conn *conn) #define CCC_STORE_MAX 48 -static struct bt_gatt_ccc_cfg *ccc_find_cfg(struct _bt_gatt_ccc *ccc, - const bt_addr_le_t *addr) +static struct bt_gatt_ccc_cfg *ccc_find_cfg(const struct _bt_gatt_ccc *ccc, + const bt_addr_le_t *addr, + u8_t id) { - int i; - - for (i = 0; i < ccc->cfg_len; i++) { - if (!bt_addr_le_cmp(&ccc->cfg[i].peer, addr)) { + for (size_t i = 0; i < ccc->cfg_len; i++) { + if (id == ccc->cfg[i].id && + !bt_addr_le_cmp(&ccc->cfg[i].peer, addr)) { return &ccc->cfg[i]; } } @@ -3022,8 +3027,13 @@ static struct bt_gatt_ccc_cfg *ccc_find_cfg(struct _bt_gatt_ccc *ccc, return NULL; } -struct ccc_save { +struct addr_with_id { const bt_addr_le_t *addr; + u8_t id; +}; + +struct ccc_save { + struct addr_with_id addr_with_id; struct ccc_store store[CCC_STORE_MAX]; size_t count; }; @@ -3042,7 +3052,7 @@ static u8_t ccc_save(const struct bt_gatt_attr *attr, void *user_data) ccc = attr->user_data; /* Check if there is a cfg for the peer */ - cfg = ccc_find_cfg(ccc, save->addr); + cfg = ccc_find_cfg(ccc, save->addr_with_id.addr, save->addr_with_id.id); if (!cfg) { return BT_GATT_ITER_CONTINUE; } @@ -3065,7 +3075,8 @@ int bt_gatt_store_ccc(u8_t id, const bt_addr_le_t *addr) char *str; int err; - save.addr = addr; + save.addr_with_id.addr = addr; + save.addr_with_id.id = id; save.count = 0; bt_gatt_foreach_attr(0x0001, 0xffff, ccc_save, &save); @@ -3110,9 +3121,9 @@ int bt_gatt_store_ccc(u8_t id, const bt_addr_le_t *addr) } static u8_t remove_peer_from_attr(const struct bt_gatt_attr *attr, - void *user_data) + void *user_data) { - const bt_addr_le_t *peer = user_data; + const struct addr_with_id *addr_with_id = user_data; struct _bt_gatt_ccc *ccc; struct bt_gatt_ccc_cfg *cfg; @@ -3124,7 +3135,7 @@ static u8_t remove_peer_from_attr(const struct bt_gatt_attr *attr, ccc = attr->user_data; /* Check if there is a cfg for the peer */ - cfg = ccc_find_cfg(ccc, peer); + cfg = ccc_find_cfg(ccc, addr_with_id->addr, addr_with_id->id); if (cfg) { memset(cfg, 0, sizeof(*cfg)); } @@ -3135,6 +3146,10 @@ static u8_t remove_peer_from_attr(const struct bt_gatt_attr *attr, static int bt_gatt_clear_ccc(u8_t id, const bt_addr_le_t *addr) { char key[BT_SETTINGS_KEY_MAX]; + struct addr_with_id addr_with_id = { + .addr = addr, + .id = id, + }; if (id) { char id_str[4]; @@ -3148,7 +3163,7 @@ static int bt_gatt_clear_ccc(u8_t id, const bt_addr_le_t *addr) } bt_gatt_foreach_attr(0x0001, 0xffff, remove_peer_from_attr, - (void *)addr); + &addr_with_id); return settings_delete(key); } @@ -3207,23 +3222,23 @@ int bt_gatt_clear(u8_t id, const bt_addr_le_t *addr) return bt_gatt_clear_cf(id, addr); } -static void ccc_clear(struct _bt_gatt_ccc *ccc, bt_addr_le_t *addr) +static void ccc_clear(struct _bt_gatt_ccc *ccc, + const bt_addr_le_t *addr, + u8_t id) { struct bt_gatt_ccc_cfg *cfg; - cfg = ccc_find_cfg(ccc, addr); + cfg = ccc_find_cfg(ccc, addr, id); if (!cfg) { BT_DBG("Unable to clear CCC: cfg not found"); return; } - bt_addr_le_copy(&cfg->peer, BT_ADDR_LE_ANY); - cfg->value = 0U; + clear_ccc_cfg(cfg); } struct ccc_load { - u8_t id; - bt_addr_le_t addr; + struct addr_with_id addr_with_id; struct ccc_store *entry; size_t count; }; @@ -3241,9 +3256,9 @@ static u8_t ccc_load(const struct bt_gatt_attr *attr, void *user_data) ccc = attr->user_data; - /* Clear if value was invalidade */ + /* Clear if value was invalidated */ if (!load->entry) { - ccc_clear(ccc, &load->addr); + ccc_clear(ccc, load->addr_with_id.addr, load->addr_with_id.id); return BT_GATT_ITER_CONTINUE; } else if (!load->count) { return BT_GATT_ITER_STOP; @@ -3266,14 +3281,15 @@ static u8_t ccc_load(const struct bt_gatt_attr *attr, void *user_data) BT_DBG("Restoring CCC: handle 0x%04x value 0x%04x", load->entry->handle, load->entry->value); - cfg = ccc_find_cfg(ccc, &load->addr); + cfg = ccc_find_cfg(ccc, load->addr_with_id.addr, load->addr_with_id.id); if (!cfg) { - cfg = ccc_find_cfg(ccc, BT_ADDR_LE_ANY); + cfg = ccc_find_cfg(ccc, BT_ADDR_LE_ANY, 0); if (!cfg) { BT_DBG("Unable to restore CCC: no cfg left"); goto next; } - bt_addr_le_copy(&cfg->peer, &load->addr); + bt_addr_le_copy(&cfg->peer, load->addr_with_id.addr); + cfg->id = load->addr_with_id.id; } cfg->value = load->entry->value; @@ -3289,23 +3305,26 @@ static int ccc_set(int argc, char **argv, void *val_ctx) { struct ccc_store ccc_store[CCC_STORE_MAX]; struct ccc_load load; + bt_addr_le_t addr; int len, err; if (argc < 1) { BT_ERR("Insufficient number of arguments"); return -EINVAL; } else if (argc == 1) { - load.id = BT_ID_DEFAULT; + load.addr_with_id.id = BT_ID_DEFAULT; } else { - load.id = strtol(argv[1], NULL, 10); + load.addr_with_id.id = strtol(argv[1], NULL, 10); } - err = bt_settings_decode_key(argv[0], &load.addr); + err = bt_settings_decode_key(argv[0], &addr); if (err) { BT_ERR("Unable to decode address %s", argv[0]); return -EINVAL; } + load.addr_with_id.addr = &addr; + if (settings_val_get_len_cb(val_ctx)) { len = settings_val_read_cb(val_ctx, ccc_store, sizeof(ccc_store)); @@ -3329,7 +3348,8 @@ static int ccc_set(int argc, char **argv, void *val_ctx) bt_gatt_foreach_attr(0x0001, 0xffff, ccc_load, &load); - BT_DBG("Restored CCC for %s", bt_addr_le_str(&load.addr)); + BT_DBG("Restored CCC for id:%" PRIu8 " addr:%s", load.addr_with_id.id, + bt_addr_le_str(load.addr_with_id.addr)); return 0; } @@ -3373,8 +3393,7 @@ static int cf_set(int argc, char **argv, void *val_ctx) BT_DBG("Read CF: len %d", len); } else { - bt_addr_le_copy(&cfg->peer, BT_ADDR_LE_ANY); - memset(cfg->data, 0, sizeof(cfg->data)); + clear_cf_cfg(cfg); } BT_DBG("Restored CF for %s", bt_addr_le_str(&addr));