diff --git a/nimble/host/mesh/include/mesh/glue.h b/nimble/host/mesh/include/mesh/glue.h index 4edefd9f5..ec0e609a0 100644 --- a/nimble/host/mesh/include/mesh/glue.h +++ b/nimble/host/mesh/include/mesh/glue.h @@ -439,6 +439,7 @@ static inline unsigned int find_msb_set(uint32_t op) #define CONFIG_BT_MESH_RX_SEG_MAX MYNEWT_VAL(BLE_MESH_RX_SEG_MAX) #define CONFIG_BT_MESH_RX_SEG_MSG_COUNT MYNEWT_VAL(BLE_MESH_RX_SEG_MSG_COUNT) #define CONFIG_BT_MESH_LABEL_COUNT MYNEWT_VAL(BLE_MESH_LABEL_COUNT) +#define CONFIG_BT_MESH_MODEL_VND_MSG_CID_FORCE MYNEWT_VAL(BLE_MESH_MODEL_VND_MSG_CID_FORCE) #define CONFIG_BT_MESH_NODE_COUNT MYNEWT_VAL(BLE_MESH_CDB_NODE_COUNT) #define CONFIG_BT_GATT_PROXY_ENABLED MYNEWT_VAL(BLE_MESH_GATT_PROXY_ENABLED) #define CONFIG_BT_MESH_DEFAULT_TTL MYNEWT_VAL(BLE_MESH_DEFAULT_TTL) diff --git a/nimble/host/mesh/src/access.c b/nimble/host/mesh/src/access.c index 79f4be7b4..359d8c4e2 100644 --- a/nimble/host/mesh/src/access.c +++ b/nimble/host/mesh/src/access.c @@ -291,6 +291,30 @@ struct bt_mesh_model *bt_mesh_model_get(bool vnd, uint8_t elem_idx, uint8_t mod_ } } +#if defined(CONFIG_BT_MESH_MODEL_VND_MSG_CID_FORCE) +static int bt_mesh_vnd_mod_msg_cid_check(struct bt_mesh_model *mod) +{ + uint16_t cid; + const struct bt_mesh_model_op *op; + + for (op = mod->op; op->func; op++) { + cid = (uint16_t)(op->opcode & 0xffff); + + if (cid == mod->vnd.company) { + continue; + } + + BT_ERR("Invalid vendor model(company:0x%04x" + " id:0x%04x) message opcode 0x%08x", + mod->vnd.company, mod->vnd.id, op->opcode); + + return -EINVAL; + } + + return 0; +} +#endif + static void mod_init(struct bt_mesh_model *mod, struct bt_mesh_elem *elem, bool vnd, bool primary, void *user_data) { @@ -314,6 +338,13 @@ static void mod_init(struct bt_mesh_model *mod, struct bt_mesh_elem *elem, mod->elem_idx = elem - dev_comp->elem; if (vnd) { mod->mod_idx = mod - elem->vnd_models; + + if (CONFIG_BT_MESH_MODEL_VND_MSG_CID_FORCE) { + *err = bt_mesh_vnd_mod_msg_cid_check(mod); + if (*err) { + return; + } + } } else { mod->mod_idx = mod - elem->models; } @@ -509,15 +540,37 @@ static bool model_has_dst(struct bt_mesh_model *mod, uint16_t dst) return mod->elem_idx == 0; } -static const struct bt_mesh_model_op *find_op(struct bt_mesh_model *models, - uint8_t model_count, uint32_t opcode, - struct bt_mesh_model **model) +static const struct bt_mesh_model_op *find_op(struct bt_mesh_elem *elem, + uint32_t opcode, struct bt_mesh_model **model) { uint8_t i; + uint8_t count; + /* This value shall not be used in shipping end products. */ + uint32_t cid = UINT32_MAX; + struct bt_mesh_model *models; - for (i = 0; i < model_count; i++) { + /* SIG models cannot contain 3-byte (vendor) OpCodes, and + * vendor models cannot contain SIG (1- or 2-byte) OpCodes, so + * we only need to do the lookup in one of the model lists. + */ + if (BT_MESH_MODEL_OP_LEN(opcode) < 3) { + models = elem->models; + count = elem->model_count; + } else { + models = elem->vnd_models; + count = elem->vnd_model_count; + + cid = (uint16_t)(opcode & 0xffff); + } + + for (i = 0U; i < count; i++) { const struct bt_mesh_model_op *op; + if (CONFIG_BT_MESH_MODEL_VND_MSG_CID_FORCE && + cid != UINT32_MAX && + cid != models[i].vnd.company) { + continue; + } *model = &models[i]; for (op = (*model)->op; op->func; op++) { @@ -571,10 +624,9 @@ static int get_opcode(struct os_mbuf *buf, uint32_t *opcode) void bt_mesh_model_recv(struct bt_mesh_net_rx *rx, struct os_mbuf *buf) { - struct bt_mesh_model *models, *model; + struct bt_mesh_model *model; const struct bt_mesh_model_op *op; uint32_t opcode; - uint8_t count; int i; BT_DBG("app_idx 0x%04x src 0x%04x dst 0x%04x", rx->ctx.app_idx, @@ -589,22 +641,10 @@ void bt_mesh_model_recv(struct bt_mesh_net_rx *rx, struct os_mbuf *buf) BT_DBG("OpCode 0x%08x", (unsigned) opcode); for (i = 0; i < dev_comp->elem_count; i++) { - struct bt_mesh_elem *elem = &dev_comp->elem[i]; struct net_buf_simple_state state; - /* SIG models cannot contain 3-byte (vendor) OpCodes, and - * vendor models cannot contain SIG (1- or 2-byte) OpCodes, so - * we only need to do the lookup in one of the model lists. - */ - if (BT_MESH_MODEL_OP_LEN(opcode) < 3) { - models = elem->models; - count = elem->model_count; - } else { - models = elem->vnd_models; - count = elem->vnd_model_count; - } + op = find_op(&dev_comp->elem[i], opcode, &model); - op = find_op(models, count, opcode, &model); if (!op) { BT_DBG("No OpCode 0x%08x for elem %d", opcode, i); continue; diff --git a/nimble/host/mesh/syscfg.yml b/nimble/host/mesh/syscfg.yml index c0ebac5bd..dcde09444 100644 --- a/nimble/host/mesh/syscfg.yml +++ b/nimble/host/mesh/syscfg.yml @@ -171,6 +171,12 @@ syscfg.defs: at most be subscribed to. value: 1 + BLE_MESH_MODEL_VND_MSG_CID_FORCE: + description: > + This option forces vendor model to use messages for the + corresponding CID field. + value: 1 + BLE_MESH_LABEL_COUNT: description: > This option specifies how many Label UUIDs can be stored. diff --git a/porting/targets/linux_blemesh/syscfg.yml b/porting/targets/linux_blemesh/syscfg.yml index efc4150b4..8ef797218 100644 --- a/porting/targets/linux_blemesh/syscfg.yml +++ b/porting/targets/linux_blemesh/syscfg.yml @@ -34,6 +34,7 @@ syscfg.vals: BLE_MESH_LABEL_COUNT: 2 BLE_MESH_SUBNET_COUNT: 2 BLE_MESH_MODEL_GROUP_COUNT: 2 + BLE_MESH_MODEL_VND_MSG_CID_FORCE: 1 BLE_MESH_APP_KEY_COUNT: 4 BLE_MESH_IV_UPDATE_TEST: 1 BLE_MESH_TESTING: 1