From 5551b4cfadc060e49be78a46cf80723300f12236 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anders=20Storr=C3=B8?= Date: Tue, 10 Oct 2023 10:09:35 +0200 Subject: [PATCH] Bluetooth: Mesh: Priv net id parse on proxy cli MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Adds parsing of Private Network Identity parsing in the proxy client, allowing the client to use these messages to establish a GATT connection. Signed-off-by: Anders Storrø --- subsys/bluetooth/mesh/proxy_cli.c | 124 ++++++++++++++++++++++++------ 1 file changed, 101 insertions(+), 23 deletions(-) diff --git a/subsys/bluetooth/mesh/proxy_cli.c b/subsys/bluetooth/mesh/proxy_cli.c index 54cbf9ab2e9c4b..4b95e894c9c6b0 100644 --- a/subsys/bluetooth/mesh/proxy_cli.c +++ b/subsys/bluetooth/mesh/proxy_cli.c @@ -27,6 +27,7 @@ #include "proxy.h" #include "gatt_cli.h" #include "proxy_msg.h" +#include "crypto.h" #define LOG_LEVEL CONFIG_BT_MESH_PROXY_LOG_LEVEL #include @@ -157,14 +158,9 @@ static const struct bt_mesh_gatt_cli proxy = { .disconnected = proxy_disconnected }; -struct find_net_id { - const uint8_t *net_id; - struct bt_mesh_proxy_server *srv; -}; - -static bool has_net_id(struct bt_mesh_subnet *sub, void *user_data) +static bool proxy_srv_check_and_get(struct bt_mesh_subnet *sub, const uint8_t *net_id, + struct bt_mesh_proxy_server **p_srv) { - struct find_net_id *res = user_data; struct bt_mesh_proxy_server *srv; srv = find_proxy_srv(sub->net_idx, true, true); @@ -183,43 +179,125 @@ static bool has_net_id(struct bt_mesh_subnet *sub, void *user_data) } } - if (!memcmp(sub->keys[0].net_id, res->net_id, 8) || - (bt_mesh_subnet_has_new_key(sub) && - !memcmp(sub->keys[1].net_id, res->net_id, 8))) { - res->srv = srv; + /* If net_id is NULL we already know that the networks match */ + if (!net_id || !memcmp(sub->keys[0].net_id, net_id, 8) || + (bt_mesh_subnet_has_new_key(sub) && !memcmp(sub->keys[1].net_id, net_id, 8))) { + + *p_srv = srv; return true; } return false; } -void bt_mesh_proxy_cli_adv_recv(const struct bt_le_scan_recv_info *info, - struct net_buf_simple *buf) -{ +struct find_net_id { uint8_t type; + + union { + const uint8_t *net_id; + struct { + const uint8_t *hash; + const uint8_t *rand; + } priv; + } data; + + struct bt_mesh_proxy_server *srv; +}; + +static bool is_hash_equal(struct bt_mesh_subnet *sub, struct find_net_id *res, uint8_t idx) +{ + int err; + uint8_t in[16], out[16]; + + memcpy(&in[0], sub->keys[idx].net_id, 8); + memcpy(&in[8], res->data.priv.rand, 8); + err = bt_mesh_encrypt(&sub->keys[idx].identity, in, out); + if (err) { + LOG_ERR("Failed to generate hash (err: %d)", err); + return false; + } + + if (memcmp(&out[8], res->data.priv.hash, 8)) { + return false; + } + + return true; +} + +static bool has_net_id(struct bt_mesh_subnet *sub, void *user_data) +{ + struct find_net_id *res = user_data; + uint8_t *net_id = NULL; + + if (res->type == BT_MESH_ID_TYPE_NET) { + net_id = (uint8_t *)res->data.net_id; + goto end; + } + + /* Additional handling for BT_MESH_ID_TYPE_PRIV_NET msg type */ + if (!(is_hash_equal(sub, res, 0) || + (bt_mesh_subnet_has_new_key(sub) && is_hash_equal(sub, res, 1)))) { + return false; + } +end: + return proxy_srv_check_and_get(sub, net_id, &res->srv); +} + +static void handle_net_id(uint8_t type, const struct bt_le_scan_recv_info *info, + struct net_buf_simple *buf) +{ + int err; struct find_net_id res; struct bt_mesh_subnet *sub; - type = net_buf_simple_pull_u8(buf); - switch (type) { - case BT_MESH_ID_TYPE_NET: + res.type = type; + res.srv = NULL; + + if (type == BT_MESH_ID_TYPE_NET) { if (buf->len != 8) { - break; + return; } + res.data.net_id = net_buf_simple_pull_mem(buf, 8); - res.net_id = net_buf_simple_pull_mem(buf, 8); - res.srv = NULL; + } else { + if (buf->len != 16) { + return; + } + + res.data.priv.hash = net_buf_simple_pull_mem(buf, 8); + res.data.priv.rand = net_buf_simple_pull_mem(buf, 8); + } - sub = bt_mesh_subnet_find(has_net_id, (void *)&res); - if (sub && res.srv) { - (void)bt_mesh_gatt_cli_connect(info->addr, &proxy, res.srv); + sub = bt_mesh_subnet_find(has_net_id, (void *)&res); + if (sub && res.srv) { + err = bt_mesh_gatt_cli_connect(info->addr, &proxy, res.srv); + if (err) { + LOG_DBG("Failed to connect over GATT (err:%d)", err); } + } +} + +void bt_mesh_proxy_cli_adv_recv(const struct bt_le_scan_recv_info *info, + struct net_buf_simple *buf) +{ + uint8_t type; + type = net_buf_simple_pull_u8(buf); + switch (type) { + case BT_MESH_ID_TYPE_NET: + /* Fallthrough */ + case BT_MESH_ID_TYPE_PRIV_NET: { + handle_net_id(type, info, buf); break; + } case BT_MESH_ID_TYPE_NODE: { /* TODO */ break; } + case BT_MESH_ID_TYPE_PRIV_NODE: { + /* TODO */ + break; + } default: return; }