nimble/mesh: Fix handling of connection handle value

Connection handle is not guaranteed to start from 0.

Author: sjanc

nimble/host/mesh/src/proxy_msg.c

@@ -61,7 +61,7 @@ static void proxy_sar_timeout(struct ble_npl_event *work)
 
 	BT_WARN("Proxy SAR timeout");
 
-	if (role->conn_handle) {
+	if (role->conn_handle != BLE_HS_CONN_HANDLE_NONE) {
 		rc = ble_gap_terminate(role->conn_handle,
 				       BLE_ERR_REM_USER_CONN_TERM);
 		assert(rc == 0);
@@ -194,7 +194,7 @@ static void proxy_msg_init(struct bt_mesh_proxy_role *role)
 
 	role->buf = NET_BUF_SIMPLE(CONFIG_BT_MESH_PROXY_MSG_LEN);
 	net_buf_simple_init_with_data(role->buf,
-				      &bufs[role->conn_handle *
+				      &bufs[role->index *
 				      CONFIG_BT_MESH_PROXY_MSG_LEN],
 				      CONFIG_BT_MESH_PROXY_MSG_LEN);
 
@@ -204,15 +204,42 @@ static void proxy_msg_init(struct bt_mesh_proxy_role *role)
 	k_work_add_arg_delayable(&role->sar_timer, role);
 }
 
+struct bt_mesh_proxy_role *bt_mesh_proxy_role_find_with_buf(const struct os_mbuf *buf)
+{
+    unsigned int i;
+
+    for (i = 0; i < CONFIG_BT_MAX_CONN; i++) {
+        if (roles[i].buf == buf) {
+            return &roles[i];
+        }
+    }
+
+    return NULL;
+}
+
+struct bt_mesh_proxy_role *get_role(uint16_t conn_handle)
+{
+    unsigned int i;
+
+    for (i = 0; i < CONFIG_BT_MAX_CONN; i++) {
+        if (roles[i].conn_handle == BLE_HS_CONN_HANDLE_NONE) {
+            roles[i].conn_handle = conn_handle;
+            return &roles[i];
+        }
+    }
+
+    return NULL;
+}
+
 struct bt_mesh_proxy_role *bt_mesh_proxy_role_setup(uint16_t conn_handle,
 	proxy_send_cb_t send,
 	proxy_recv_cb_t recv)
 {
 	struct bt_mesh_proxy_role *role;
 
-	role = &roles[conn_handle];
+	role = get_role(conn_handle);
+	assert(role);
 
-	role->conn_handle = conn_handle;
 	proxy_msg_init(role);
 
 	role->cb.recv = recv;
@@ -234,4 +261,14 @@ void bt_mesh_proxy_role_cleanup(struct bt_mesh_proxy_role *role)
 	bt_mesh_adv_update();
 }
 
+void bt_mesh_proxy_msg_init(void)
+{
+    unsigned int i;
+
+    for (i = 0; i < MYNEWT_VAL(BLE_MAX_CONNECTIONS); i++) {
+        roles[i].index = i;
+        roles[i].conn_handle = 0xffff;
+    }
+}
+
 #endif /* MYNEWT_VAL(BLE_MESH_PROXY) */

nimble/host/mesh/src/proxy_msg.h

@@ -31,6 +31,7 @@ typedef int (*proxy_send_cb_t)(uint16_t conn_handle,
 typedef void (*proxy_recv_cb_t)(struct bt_mesh_proxy_role *role);
 
 struct bt_mesh_proxy_role {
+    unsigned int index;
 	uint16_t conn_handle;
 	uint8_t msg_type;
 
@@ -58,10 +59,13 @@ struct bt_mesh_proxy_client {
 int bt_mesh_proxy_msg_recv(struct bt_mesh_proxy_role *role,
 	const void *buf, uint16_t len);
 int bt_mesh_proxy_msg_send(struct bt_mesh_proxy_role *role, uint8_t type, struct os_mbuf *msg);
-void bt_mesh_proxy_msg_init(struct bt_mesh_proxy_role *role);
 void bt_mesh_proxy_role_cleanup(struct bt_mesh_proxy_role *role);
 struct bt_mesh_proxy_role *bt_mesh_proxy_role_setup(uint16_t conn_handle,
 						    proxy_send_cb_t send,
 						    proxy_recv_cb_t recv);
 struct bt_mesh_proxy_client *find_client(uint16_t conn_handle);
+
+struct bt_mesh_proxy_role *bt_mesh_proxy_role_find_with_buf(const struct os_mbuf *buf);
+
+void bt_mesh_proxy_msg_init(void);
 #endif /* ZEPHYR_SUBSYS_BLUETOOTH_MESH_PROXY_MSG_H_ */

nimble/host/mesh/src/proxy_srv.c

@@ -720,10 +720,13 @@ int bt_mesh_proxy_gatt_disable(void)
 void bt_mesh_proxy_addr_add(struct os_mbuf *buf, uint16_t addr)
 {
 	struct bt_mesh_proxy_client *client;
-	struct bt_mesh_proxy_role *cli =
-		CONTAINER_OF(buf, struct bt_mesh_proxy_role, buf);
+	struct bt_mesh_proxy_role *cli;
+
+	cli = bt_mesh_proxy_role_find_with_buf(buf);
+	assert(cli);
 
 	client = find_client(cli->conn_handle);
+	assert(client);
 
 	BT_DBG("filter_type %u addr 0x%04x", client->filter_type, addr);
 
@@ -997,6 +1000,8 @@ int bt_mesh_proxy_init(void)
 		clients[i].conn_handle = 0xffff;
 	}
 
+	bt_mesh_proxy_msg_init();
+
 	resolve_svc_handles();
 
 	ble_gatts_svc_set_visibility(svc_handles.proxy_h, 0);