host/mesh: Check the CID field before opcode compare

Bluetooth Mesh Vendor model hava company id field.

Accordin MeshPRFV1.0.1 3.7.3.1 Operation codes.

The 3-octet opcodes are used for manufacturer-specific opcodes.
The company identifiers are 16-bit values defined by the
Bluetooth SIG and are coded into the second and third octets of
the 3-octet opcodes.

Therefore, we can speed up the search process by checking whether
CID fields match, rather than comparing opcodes one by one.

This is port of cc0abee3db739413f2498d465386ab5a8bf7c1eb

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)

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;

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.

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