From 2f81942754847ed2c54447818888ed0974169ce2 Mon Sep 17 00:00:00 2001 From: Mariusz Skamra Date: Tue, 9 Jan 2018 14:25:16 +0100 Subject: [PATCH] Bluetooth: mesh: Fix model subscription groups cleanup Virtual address labels array entries were not updated on va deletion, so that STATUS_INSUFF_RESOURCES error was returned after few subsequent Config Model Subscription Virtual Address Add, Delete, Overwrite commands, even if there shall be free space available. Signed-off-by: Mariusz Skamra --- subsys/bluetooth/host/mesh/cfg_srv.c | 95 +++++++++++++++++++--------- 1 file changed, 65 insertions(+), 30 deletions(-) diff --git a/subsys/bluetooth/host/mesh/cfg_srv.c b/subsys/bluetooth/host/mesh/cfg_srv.c index 81e1e3e74fd..8e8d0928fcd 100644 --- a/subsys/bluetooth/host/mesh/cfg_srv.c +++ b/subsys/bluetooth/host/mesh/cfg_srv.c @@ -40,6 +40,7 @@ static struct bt_mesh_cfg_srv *conf; static struct label { + u16_t ref; u16_t addr; u8_t uuid[16]; } labels[CONFIG_BT_MESH_LABEL_COUNT]; @@ -1081,39 +1082,24 @@ send_status: } #if CONFIG_BT_MESH_LABEL_COUNT > 0 -static u16_t va_find(u8_t *label_uuid, struct label **free_slot) +static u8_t va_add(u8_t *label_uuid, u16_t *addr) { + struct label *free_slot = NULL; int i; - if (free_slot) { - *free_slot = NULL; - } - for (i = 0; i < ARRAY_SIZE(labels); i++) { - if (!BT_MESH_ADDR_IS_VIRTUAL(labels[i].addr)) { - if (free_slot) { - *free_slot = &labels[i]; - } + if (!labels[i].ref) { + free_slot = &labels[i]; continue; } if (!memcmp(labels[i].uuid, label_uuid, 16)) { - return labels[i].addr; + *addr = labels[i].addr; + labels[i].ref++; + return STATUS_SUCCESS; } } - return BT_MESH_ADDR_UNASSIGNED; -} - -static u8_t va_add(u8_t *label_uuid, u16_t *addr) -{ - struct label *free_slot; - - *addr = va_find(label_uuid, &free_slot); - if (*addr != BT_MESH_ADDR_UNASSIGNED) { - return STATUS_SUCCESS; - } - if (!free_slot) { return STATUS_INSUFF_RESOURCES; } @@ -1122,12 +1108,59 @@ static u8_t va_add(u8_t *label_uuid, u16_t *addr) return STATUS_UNSPECIFIED; } + free_slot->ref = 1; free_slot->addr = *addr; memcpy(free_slot->uuid, label_uuid, 16); return STATUS_SUCCESS; } +static u8_t va_del(u8_t *label_uuid, u16_t *addr) +{ + int i; + + for (i = 0; i < ARRAY_SIZE(labels); i++) { + if (!memcmp(labels[i].uuid, label_uuid, 16)) { + if (addr) { + *addr = labels[i].addr; + } + + labels[i].ref--; + return STATUS_SUCCESS; + } + } + + if (addr) { + *addr = BT_MESH_ADDR_UNASSIGNED; + } + + return STATUS_CANNOT_REMOVE; +} + +static void mod_sub_list_clear(struct bt_mesh_model *mod) +{ + u8_t *label_uuid; + int i; + + /* Unref stored labels related to this model */ + for (i = 0; i < ARRAY_SIZE(mod->groups); i++) { + if (!BT_MESH_ADDR_IS_VIRTUAL(mod->groups[i])) { + continue; + } + + label_uuid = bt_mesh_label_uuid_get(mod->groups[i]); + if (!label_uuid) { + BT_ERR("Label UUID not found"); + continue; + } + + va_del(label_uuid, NULL); + } + + /* Clear all subscriptions (0x0000 is the unassigned address) */ + memset(mod->groups, 0, sizeof(mod->groups)); +} + static void mod_pub_va_set(struct bt_mesh_model *model, struct bt_mesh_msg_ctx *ctx, struct net_buf_simple *buf) @@ -1192,6 +1225,12 @@ send_status: status, mod_id); } #else +static void mod_sub_list_clear(struct bt_mesh_model *mod) +{ + /* Clear all subscriptions (0x0000 is the unassigned address) */ + memset(mod->groups, 0, sizeof(mod->groups)); +} + static void mod_pub_va_set(struct bt_mesh_model *model, struct bt_mesh_msg_ctx *ctx, struct net_buf_simple *buf) @@ -1428,8 +1467,7 @@ static void mod_sub_overwrite(struct bt_mesh_model *model, bt_mesh_lpn_group_del(mod->groups, ARRAY_SIZE(mod->groups)); } - /* Clear all subscriptions (0x0000 is the unassigned address) */ - memset(mod->groups, 0, sizeof(mod->groups)); + mod_sub_list_clear(mod); if (ARRAY_SIZE(mod->groups) > 0) { mod->groups[0] = sub_addr; @@ -1483,8 +1521,7 @@ static void mod_sub_del_all(struct bt_mesh_model *model, bt_mesh_lpn_group_del(mod->groups, ARRAY_SIZE(mod->groups)); } - /* Clear all subscriptions (0x0000 is the unassigned address) */ - memset(mod->groups, 0, sizeof(mod->groups)); + mod_sub_list_clear(mod); status = STATUS_SUCCESS; @@ -1708,9 +1745,8 @@ static void mod_sub_va_del(struct bt_mesh_model *model, goto send_status; } - sub_addr = va_find(label_uuid, NULL); + status = va_del(label_uuid, &sub_addr); if (sub_addr == BT_MESH_ADDR_UNASSIGNED) { - status = STATUS_CANNOT_REMOVE; goto send_status; } @@ -1769,8 +1805,7 @@ static void mod_sub_va_overwrite(struct bt_mesh_model *model, bt_mesh_lpn_group_del(mod->groups, ARRAY_SIZE(mod->groups)); } - /* Clear all subscriptions (0x0000 is the unassigned address) */ - memset(mod->groups, 0, sizeof(mod->groups)); + mod_sub_list_clear(mod); if (ARRAY_SIZE(mod->groups) > 0) { status = va_add(label_uuid, &sub_addr);