From patchwork Fri Feb 21 02:18:08 2020 Content-Type: text/plain; charset="utf-8" MIME-Version: 1.0 Content-Transfer-Encoding: 7bit X-Patchwork-Submitter: "Stotland, Inga" X-Patchwork-Id: 11395227 X-Patchwork-Delegate: brian.gix@gmail.com Return-Path: Received: from mail.kernel.org (pdx-korg-mail-1.web.codeaurora.org [172.30.200.123]) by pdx-korg-patchwork-2.web.codeaurora.org (Postfix) with ESMTP id C934413A4 for ; Fri, 21 Feb 2020 02:18:15 +0000 (UTC) Received: from vger.kernel.org (vger.kernel.org [209.132.180.67]) by mail.kernel.org (Postfix) with ESMTP id AC04924653 for ; Fri, 21 Feb 2020 02:18:15 +0000 (UTC) Received: (majordomo@vger.kernel.org) by vger.kernel.org via listexpand id S1729497AbgBUCSP (ORCPT ); Thu, 20 Feb 2020 21:18:15 -0500 Received: from mga04.intel.com ([192.55.52.120]:20563 "EHLO mga04.intel.com" rhost-flags-OK-OK-OK-OK) by vger.kernel.org with ESMTP id S1728992AbgBUCSP (ORCPT ); Thu, 20 Feb 2020 21:18:15 -0500 X-Amp-Result: SKIPPED(no attachment in message) X-Amp-File-Uploaded: False Received: from fmsmga008.fm.intel.com ([10.253.24.58]) by fmsmga104.fm.intel.com with ESMTP/TLS/DHE-RSA-AES256-GCM-SHA384; 20 Feb 2020 18:18:15 -0800 X-ExtLoop1: 1 X-IronPort-AV: E=Sophos;i="5.70,466,1574150400"; d="scan'208";a="230313308" Received: from ingas-nuc1.sea.intel.com ([10.254.187.96]) by fmsmga008.fm.intel.com with ESMTP; 20 Feb 2020 18:18:14 -0800 From: Inga Stotland To: linux-bluetooth@vger.kernel.org Cc: brian.gix@intel.com, Inga Stotland Subject: [PATCH BlueZ 1/4] tools/mesh-cfgclient: Add support for virtual labels Date: Thu, 20 Feb 2020 18:18:08 -0800 Message-Id: <20200221021811.30408-2-inga.stotland@intel.com> X-Mailer: git-send-email 2.21.1 In-Reply-To: <20200221021811.30408-1-inga.stotland@intel.com> References: <20200221021811.30408-1-inga.stotland@intel.com> MIME-Version: 1.0 Sender: linux-bluetooth-owner@vger.kernel.org Precedence: bulk List-ID: X-Mailing-List: linux-bluetooth@vger.kernel.org This adds commands to generate and use virtual addresses for configuring remote node's publication and subscription. New commands: virt-add - generate a new label and calculate virtual address group-list - displays group addresses that are in use and available virtual labels with corresponding virtual addresses --- Makefile.tools | 3 +- tools/mesh/cfgcli.c | 203 ++++++++++++++++++++++++++++++++++++++++---- 2 files changed, 188 insertions(+), 18 deletions(-) diff --git a/Makefile.tools b/Makefile.tools index 006554cf7..f43764adc 100644 --- a/Makefile.tools +++ b/Makefile.tools @@ -336,7 +336,8 @@ tools_mesh_cfgclient_SOURCES = tools/mesh-cfgclient.c \ tools/mesh/agent.h tools/mesh/agent.c \ tools/mesh/mesh-db.h tools/mesh/mesh-db.c \ mesh/util.h mesh/util.c \ - mesh/mesh-config.h mesh/mesh-config-json.c + mesh/mesh-config.h mesh/mesh-config-json.c \ + mesh/crypto.h mesh/crypto.c tools_mesh_cfgclient_LDADD = lib/libbluetooth-internal.la src/libshared-ell.la \ $(ell_ldadd) -ljson-c -lreadline diff --git a/tools/mesh/cfgcli.c b/tools/mesh/cfgcli.c index cfa573de3..4930c8b7b 100644 --- a/tools/mesh/cfgcli.c +++ b/tools/mesh/cfgcli.c @@ -31,6 +31,8 @@ #include "src/shared/util.h" #include "mesh/mesh-defs.h" +#include "mesh/util.h" +#include "mesh/crypto.h" #include "tools/mesh/util.h" #include "tools/mesh/model.h" @@ -58,7 +60,13 @@ struct pending_req { uint16_t addr; }; +struct mesh_group { + uint16_t addr; + uint8_t label[16]; +}; + static struct l_queue *requests; +static struct l_queue *groups; static void *send_data; static model_send_msg_func_t send_msg; @@ -764,6 +772,53 @@ static uint32_t read_input_parameters(int argc, char *argv[]) return i; } +static bool match_group_addr(const void *a, const void *b) +{ + const struct mesh_group *grp = a; + uint16_t addr = L_PTR_TO_UINT(b); + + return grp->addr == addr; +} + +static int compare_group_addr(const void *a, const void *b, void *user_data) +{ + const struct mesh_group *grp0 = a; + const struct mesh_group *grp1 = b; + + if (grp0->addr < grp1->addr) + return -1; + + if (grp0->addr > grp1->addr) + return 1; + + return 0; +} + +static void print_virtual_not_found(uint16_t addr) +{ + bt_shell_printf("Virtual group with hash %4.4x not found\n", addr); + bt_shell_printf("To see available, use \"group-list\"\n"); + bt_shell_printf("To create new, use \"label-add\"\n"); +} + +static struct mesh_group *add_group(uint16_t addr) +{ + struct mesh_group *grp; + + if (!IS_GROUP(addr)) + return NULL; + + grp = l_queue_find(groups, match_group_addr, L_UINT_TO_PTR(addr)); + if (grp) + return grp; + + grp = l_new(struct mesh_group, 1); + grp->addr = addr; + l_queue_insert(groups, grp, compare_group_addr, NULL); + + return grp; +} + static void cmd_timeout_set(int argc, char *argv[]) { if (read_input_parameters(argc, argv) != 1) @@ -1196,22 +1251,47 @@ static void cmd_ttl_set(int argc, char *argv[]) static void cmd_pub_set(int argc, char *argv[]) { uint16_t n; - uint8_t msg[32]; + uint8_t msg[48]; int parm_cnt; - - n = mesh_opcode_set(OP_CONFIG_MODEL_PUB_SET, msg); + struct mesh_group *grp; + uint32_t opcode; + uint16_t pub_addr; parm_cnt = read_input_parameters(argc, argv); + if (parm_cnt != 6 && parm_cnt != 7) { bt_shell_printf("Bad arguments\n"); return bt_shell_noninteractive_quit(EXIT_FAILURE); } + pub_addr = parms[1]; + + grp = l_queue_find(groups, match_group_addr, L_UINT_TO_PTR(pub_addr)); + if (!grp) + grp = add_group(pub_addr); + + if (!grp && IS_VIRTUAL(pub_addr)) { + print_virtual_not_found(pub_addr); + return bt_shell_noninteractive_quit(EXIT_FAILURE); + } + + opcode = (!IS_VIRTUAL(pub_addr)) ? OP_CONFIG_MODEL_PUB_SET : + OP_CONFIG_MODEL_PUB_VIRT_SET; + + n = mesh_opcode_set(opcode, msg); + put_le16(parms[0], msg + n); n += 2; + /* Publish address */ - put_le16(parms[1], msg + n); - n += 2; + if (!IS_VIRTUAL(pub_addr)) { + put_le16(pub_addr, msg + n); + n += 2; + } else { + memcpy(msg + n, grp->label, 16); + n += 16; + } + /* AppKey index + credential (set to 0) */ put_le16(parms[2], msg + n); n += 2; @@ -1225,10 +1305,10 @@ static void cmd_pub_set(int argc, char *argv[]) /* Model Id */ n += put_model_id(msg + n, &parms[5], parm_cnt == 7); - if (!config_send(msg, n, OP_CONFIG_MODEL_PUB_SET)) + if (!config_send(msg, n, opcode)) return bt_shell_noninteractive_quit(EXIT_FAILURE); - return bt_shell_noninteractive_quit(EXIT_SUCCESS); + bt_shell_noninteractive_quit(EXIT_SUCCESS); } static void cmd_pub_get(int argc, char *argv[]) @@ -1263,8 +1343,8 @@ static void subscription_cmd(int argc, char *argv[], uint32_t opcode) uint16_t n; uint8_t msg[32]; int parm_cnt; - - n = mesh_opcode_set(opcode, msg); + struct mesh_group *grp; + uint16_t sub_addr; parm_cnt = read_input_parameters(argc, argv); if (parm_cnt != 3 && parm_cnt != 4) { @@ -1272,12 +1352,42 @@ static void subscription_cmd(int argc, char *argv[], uint32_t opcode) return bt_shell_noninteractive_quit(EXIT_FAILURE); } + sub_addr = parms[1]; + + grp = l_queue_find(groups, match_group_addr, L_UINT_TO_PTR(sub_addr)); + + if (!grp && opcode != OP_CONFIG_MODEL_SUB_DELETE) { + grp = add_group(sub_addr); + + if (!grp && IS_VIRTUAL(sub_addr)) { + print_virtual_not_found(sub_addr); + return bt_shell_noninteractive_quit(EXIT_FAILURE); + } + } + + if (IS_VIRTUAL(sub_addr)) { + if (opcode == OP_CONFIG_MODEL_SUB_ADD) + opcode = OP_CONFIG_MODEL_SUB_VIRT_ADD; + else if (opcode == OP_CONFIG_MODEL_SUB_DELETE) + opcode = OP_CONFIG_MODEL_SUB_VIRT_DELETE; + else if (opcode == OP_CONFIG_MODEL_SUB_OVERWRITE) + opcode = OP_CONFIG_MODEL_SUB_VIRT_OVERWRITE; + } + + n = mesh_opcode_set(opcode, msg); + /* Element Address */ put_le16(parms[0], msg + n); n += 2; + /* Subscription Address */ - put_le16(parms[1], msg + n); - n += 2; + if (!IS_VIRTUAL(sub_addr)) { + put_le16(sub_addr, msg + n); + n += 2; + } else { + memcpy(msg + n, grp->label, 16); + n += 16; + } /* Model ID */ n += put_model_id(msg + n, &parms[2], parm_cnt == 4); @@ -1399,6 +1509,9 @@ static void cmd_hb_pub_set(int argc, char *argv[]) n = mesh_opcode_set(OP_CONFIG_HEARTBEAT_PUB_SET, msg); + if (!l_queue_find(groups, match_group_addr, L_UINT_TO_PTR(parms[1]))) + add_group(parms[1]); + parm_cnt = read_input_parameters(argc, argv); if (parm_cnt != 6) { bt_shell_printf("Bad arguments: %s\n", argv[1]); @@ -1447,6 +1560,9 @@ static void cmd_hb_sub_set(int argc, char *argv[]) return bt_shell_noninteractive_quit(EXIT_FAILURE); } + if (!l_queue_find(groups, match_group_addr, L_UINT_TO_PTR(parms[1]))) + add_group(parms[1]); + /* Per Mesh Profile 4.3.2.65 */ /* Source address */ put_le16(parms[0], msg + n); @@ -1537,6 +1653,54 @@ static void cmd_netkey_get(int argc, char *argv[]) cmd_default(OP_NETKEY_GET); } +static void print_group(void *a, void *b) +{ + struct mesh_group *grp = a; + char buf[33]; + + if (!IS_VIRTUAL(grp->addr)) { + bt_shell_printf("\tGroup addr: %4.4x\n", grp->addr); + return; + } + + hex2str(grp->label, 16, buf, sizeof(buf)); + bt_shell_printf("\tVirtual addr: %4.4x, label: %s\n", grp->addr, buf); +} + +static void cmd_add_virt(int argc, char *argv[]) +{ + struct mesh_group *grp, *tmp; + uint8_t max_tries = 3; + + grp = l_new(struct mesh_group, 1); + +retry: + l_getrandom(grp->label, 16); + mesh_crypto_virtual_addr(grp->label, &grp->addr); + + /* For simplicity sake, avoid labels that map to the same hash */ + tmp = l_queue_find(groups, match_group_addr, L_UINT_TO_PTR(grp->addr)); + if (!tmp) { + l_queue_insert(groups, grp, compare_group_addr, NULL); + print_group(grp, NULL); + return bt_shell_noninteractive_quit(EXIT_SUCCESS); + } + + max_tries--; + if (max_tries) + goto retry; + + l_free(grp); + bt_shell_printf("Failed to generate unique label. Try again."); + bt_shell_noninteractive_quit(EXIT_FAILURE); +} + +static void cmd_list_groups(int argc, char *argv[]) +{ + l_queue_foreach(groups, print_group, NULL); + return bt_shell_noninteractive_quit(EXIT_FAILURE); +} + static bool tx_setup(model_send_msg_func_t send_func, void *user_data) { if (!send_func) @@ -1625,12 +1789,15 @@ static const struct bt_shell_menu cfg_menu = { "Set heartbeat subscribe"}, {"hb-sub-get", NULL, cmd_hb_sub_get, "Get heartbeat subscribe"}, - {"sub-add", " [vendor]", cmd_sub_add, - "Add subscription"}, - {"sub-del", " [vendor]", cmd_sub_del, - "Delete subscription"}, - {"sub-wrt", " [vendor]", cmd_sub_ovwrt, - "Overwrite subscription"}, + {"virt-add", NULL, cmd_add_virt, "Generate and add a virtual label"}, + {"group-list", NULL, cmd_list_groups, + "Display existing group addresses and virtual labels"}, + {"sub-add", " [vendor]", + cmd_sub_add, "Add subscription"}, + {"sub-del", " [vendor]", + cmd_sub_del, "Delete subscription"}, + {"sub-wrt", " [vendor]", + cmd_sub_ovwrt, "Overwrite subscription"}, {"sub-del-all", " [vendor]", cmd_sub_del_all, "Delete subscription"}, {"sub-get", " [vendor]", cmd_sub_get, @@ -1660,6 +1827,7 @@ struct model_info *cfgcli_init(key_send_func_t key_send, void *user_data) send_key_msg = key_send; key_data = user_data; requests = l_queue_new(); + groups = l_queue_new(); bt_shell_add_submenu(&cfg_menu); @@ -1669,4 +1837,5 @@ struct model_info *cfgcli_init(key_send_func_t key_send, void *user_data) void cfgcli_cleanup(void) { l_queue_destroy(requests, free_request); + l_queue_destroy(groups, l_free); } From patchwork Fri Feb 21 02:18:09 2020 Content-Type: text/plain; charset="utf-8" MIME-Version: 1.0 Content-Transfer-Encoding: 7bit X-Patchwork-Submitter: "Stotland, Inga" X-Patchwork-Id: 11395229 X-Patchwork-Delegate: brian.gix@gmail.com Return-Path: Received: from mail.kernel.org (pdx-korg-mail-1.web.codeaurora.org [172.30.200.123]) by pdx-korg-patchwork-2.web.codeaurora.org (Postfix) with ESMTP id 5FAED14BC for ; Fri, 21 Feb 2020 02:18:17 +0000 (UTC) Received: from vger.kernel.org (vger.kernel.org [209.132.180.67]) by mail.kernel.org (Postfix) with ESMTP id 3F34A20722 for ; Fri, 21 Feb 2020 02:18:17 +0000 (UTC) Received: (majordomo@vger.kernel.org) by vger.kernel.org via listexpand id S1729529AbgBUCSQ (ORCPT ); Thu, 20 Feb 2020 21:18:16 -0500 Received: from mga04.intel.com ([192.55.52.120]:20563 "EHLO mga04.intel.com" rhost-flags-OK-OK-OK-OK) by vger.kernel.org with ESMTP id S1728992AbgBUCSQ (ORCPT ); Thu, 20 Feb 2020 21:18:16 -0500 X-Amp-Result: SKIPPED(no attachment in message) X-Amp-File-Uploaded: False Received: from fmsmga008.fm.intel.com ([10.253.24.58]) by fmsmga104.fm.intel.com with ESMTP/TLS/DHE-RSA-AES256-GCM-SHA384; 20 Feb 2020 18:18:16 -0800 X-ExtLoop1: 1 X-IronPort-AV: E=Sophos;i="5.70,466,1574150400"; d="scan'208";a="230313320" Received: from ingas-nuc1.sea.intel.com ([10.254.187.96]) by fmsmga008.fm.intel.com with ESMTP; 20 Feb 2020 18:18:15 -0800 From: Inga Stotland To: linux-bluetooth@vger.kernel.org Cc: brian.gix@intel.com, Inga Stotland Subject: [PATCH BlueZ 2/4] tools/mesh-cfgclient: Save and restore group addresses Date: Thu, 20 Feb 2020 18:18:09 -0800 Message-Id: <20200221021811.30408-3-inga.stotland@intel.com> X-Mailer: git-send-email 2.21.1 In-Reply-To: <20200221021811.30408-1-inga.stotland@intel.com> References: <20200221021811.30408-1-inga.stotland@intel.com> MIME-Version: 1.0 Sender: linux-bluetooth-owner@vger.kernel.org Precedence: bulk List-ID: X-Mailing-List: linux-bluetooth@vger.kernel.org This allows to save created virtual labels and group addresses in configuration file. The stored values can be restored upon the tool start up. --- tools/mesh/cfgcli.c | 15 +++--- tools/mesh/cfgcli.h | 6 +++ tools/mesh/mesh-db.c | 123 +++++++++++++++++++++++++++++++++++++++++++ tools/mesh/mesh-db.h | 3 ++ 4 files changed, 140 insertions(+), 7 deletions(-) diff --git a/tools/mesh/cfgcli.c b/tools/mesh/cfgcli.c index 4930c8b7b..60fce17cf 100644 --- a/tools/mesh/cfgcli.c +++ b/tools/mesh/cfgcli.c @@ -60,11 +60,6 @@ struct pending_req { uint16_t addr; }; -struct mesh_group { - uint16_t addr; - uint8_t label[16]; -}; - static struct l_queue *requests; static struct l_queue *groups; @@ -816,6 +811,8 @@ static struct mesh_group *add_group(uint16_t addr) grp->addr = addr; l_queue_insert(groups, grp, compare_group_addr, NULL); + mesh_db_add_group(grp); + return grp; } @@ -1683,6 +1680,7 @@ retry: if (!tmp) { l_queue_insert(groups, grp, compare_group_addr, NULL); print_group(grp, NULL); + mesh_db_add_group(grp); return bt_shell_noninteractive_quit(EXIT_SUCCESS); } @@ -1819,6 +1817,11 @@ static struct model_info cli_info = { .vendor_id = VENDOR_ID_INVALID }; +void cfgcli_restore_groups(struct l_queue *saved_groups) +{ + groups = saved_groups; +} + struct model_info *cfgcli_init(key_send_func_t key_send, void *user_data) { if (!key_send) @@ -1827,8 +1830,6 @@ struct model_info *cfgcli_init(key_send_func_t key_send, void *user_data) send_key_msg = key_send; key_data = user_data; requests = l_queue_new(); - groups = l_queue_new(); - bt_shell_add_submenu(&cfg_menu); return &cli_info; diff --git a/tools/mesh/cfgcli.h b/tools/mesh/cfgcli.h index 16d2e0a61..c35cb1ca8 100644 --- a/tools/mesh/cfgcli.h +++ b/tools/mesh/cfgcli.h @@ -18,8 +18,14 @@ * */ +struct mesh_group { + uint16_t addr; + uint8_t label[16]; +}; + typedef bool (*key_send_func_t) (void *user_data, uint16_t dst, uint16_t idx, bool is_appkey, bool update); struct model_info *cfgcli_init(key_send_func_t key_func, void *user_data); +void cfgcli_restore_groups(struct l_queue *groups); void cfgcli_cleanup(void); diff --git a/tools/mesh/mesh-db.c b/tools/mesh/mesh-db.c index 5dbb91440..1c8f92e20 100644 --- a/tools/mesh/mesh-db.c +++ b/tools/mesh/mesh-db.c @@ -41,6 +41,7 @@ #include "tools/mesh/keys.h" #include "tools/mesh/remote.h" +#include "tools/mesh/cfgcli.h" #include "tools/mesh/mesh-db.h" #define KEY_IDX_INVALID NET_IDX_INVALID @@ -254,6 +255,81 @@ static uint16_t node_parse_key(json_object *jarray, int i) return idx; } +static int compare_group_addr(const void *a, const void *b, void *user_data) +{ + const struct mesh_group *grp0 = a; + const struct mesh_group *grp1 = b; + + if (grp0->addr < grp1->addr) + return -1; + + if (grp0->addr > grp1->addr) + return 1; + + return 0; +} + +static void load_groups(json_object *jcfg) +{ + json_object *jgroups; + struct l_queue *groups; + int i, sz; + + json_object_object_get_ex(jcfg, "groups", &jgroups); + + if (!jgroups || json_object_get_type(jgroups) != json_type_array) + return; + + groups = l_queue_new(); + + sz = json_object_array_length(jgroups); + + for (i = 0; i < sz; ++i) { + json_object *jgroup, *jval; + struct mesh_group *grp; + uint16_t addr, addr_len; + const char *str; + + jgroup = json_object_array_get_idx(jgroups, i); + if (!jgroup) + continue; + + if (!json_object_object_get_ex(jgroup, "name", &jval)) + continue; + + str = json_object_get_string(jval); + if (strlen(str) != 10) + continue; + + if (sscanf(str + 6, "%04hx", &addr) != 1) + continue; + + if (!json_object_object_get_ex(jgroup, "address", &jval)) + continue; + + str = json_object_get_string(jval); + addr_len = strlen(str); + if (addr_len != 4 && addr_len != 32) + continue; + + if (addr_len == 32 && !IS_VIRTUAL(addr)) + continue; + + grp = l_new(struct mesh_group, 1); + + if (addr_len == 4) + sscanf(str, "%04hx", &grp->addr); + else { + str2hex(str, 32, grp->label, 16); + grp->addr = addr; + } + + l_queue_insert(groups, grp, compare_group_addr, NULL); + } + + cfgcli_restore_groups(groups); +} + static void load_remotes(json_object *jcfg) { json_object *jnodes; @@ -632,6 +708,45 @@ bool mesh_db_app_key_del(uint16_t app_idx) return delete_key(cfg->jcfg, "appKeys", app_idx); } +bool mesh_db_add_group(struct mesh_group *grp) +{ + json_object *jgroup, *jgroups, *jval; + char buf[16]; + + if (!cfg || !cfg->jcfg) + return false; + + if (!json_object_object_get_ex(cfg->jcfg, "groups", &jgroups)) + return false; + + jgroup = json_object_new_object(); + if (!jgroup) + return false; + + snprintf(buf, 11, "Group_%4.4x", grp->addr); + jval = json_object_new_string(buf); + json_object_object_add(jgroup, "name", jval); + + if (IS_VIRTUAL(grp->addr)) { + if (!add_u8_16(jgroup, grp->label, "address")) + goto fail; + } else { + snprintf(buf, 5, "%4.4x", grp->addr); + jval = json_object_new_string(buf); + if (!jval) + goto fail; + json_object_object_add(jgroup, "address", jval); + } + + json_object_array_add(jgroups, jgroup); + + return mesh_config_save((struct mesh_config *) cfg, true, NULL, NULL); + +fail: + json_object_put(jgroup); + return false; +} + bool mesh_db_add_node(uint8_t uuid[16], uint8_t num_els, uint16_t unicast, uint16_t net_idx) { @@ -803,6 +918,13 @@ bool mesh_db_create(const char *fname, const uint8_t token[8], json_object_object_add(jcfg, "appKeys", jarray); + jarray = json_object_new_array(); + if (!jarray) + goto fail; + + json_object_object_add(jcfg, "groups", jarray); + + if (!mesh_config_save((struct mesh_config *) cfg, true, NULL, NULL)) goto fail; @@ -866,6 +988,7 @@ bool mesh_db_load(const char *fname) goto fail; load_remotes(jcfg); + load_groups(jcfg); return true; fail: diff --git a/tools/mesh/mesh-db.h b/tools/mesh/mesh-db.h index 4a7b16ab4..172c2b09b 100644 --- a/tools/mesh/mesh-db.h +++ b/tools/mesh/mesh-db.h @@ -19,6 +19,8 @@ #include "mesh/mesh-config.h" +struct mesh_group; + bool mesh_db_create(const char *fname, const uint8_t token[8], const char *name); bool mesh_db_load(const char *fname); @@ -52,3 +54,4 @@ bool mesh_db_node_model_binding_add(uint16_t unicast, uint8_t ele, bool vendor, uint32_t mod_id, uint16_t app_idx); bool mesh_db_node_model_binding_del(uint16_t unicast, uint8_t ele, bool vendor, uint32_t mod_id, uint16_t app_idx); +bool mesh_db_add_group(struct mesh_group *grp); From patchwork Fri Feb 21 02:18:10 2020 Content-Type: text/plain; charset="utf-8" MIME-Version: 1.0 Content-Transfer-Encoding: 7bit X-Patchwork-Submitter: "Stotland, Inga" X-Patchwork-Id: 11395231 X-Patchwork-Delegate: brian.gix@gmail.com Return-Path: Received: from mail.kernel.org (pdx-korg-mail-1.web.codeaurora.org [172.30.200.123]) by pdx-korg-patchwork-2.web.codeaurora.org (Postfix) with ESMTP id EF2DA14BC for ; Fri, 21 Feb 2020 02:18:18 +0000 (UTC) Received: from vger.kernel.org (vger.kernel.org [209.132.180.67]) by mail.kernel.org (Postfix) with ESMTP id C5BC6207FD for ; Fri, 21 Feb 2020 02:18:18 +0000 (UTC) Received: (majordomo@vger.kernel.org) by vger.kernel.org via listexpand id S1729537AbgBUCSS (ORCPT ); Thu, 20 Feb 2020 21:18:18 -0500 Received: from mga04.intel.com ([192.55.52.120]:20563 "EHLO mga04.intel.com" rhost-flags-OK-OK-OK-OK) by vger.kernel.org with ESMTP id S1728992AbgBUCSS (ORCPT ); Thu, 20 Feb 2020 21:18:18 -0500 X-Amp-Result: SKIPPED(no attachment in message) X-Amp-File-Uploaded: False Received: from fmsmga008.fm.intel.com ([10.253.24.58]) by fmsmga104.fm.intel.com with ESMTP/TLS/DHE-RSA-AES256-GCM-SHA384; 20 Feb 2020 18:18:17 -0800 X-ExtLoop1: 1 X-IronPort-AV: E=Sophos;i="5.70,466,1574150400"; d="scan'208";a="230313324" Received: from ingas-nuc1.sea.intel.com ([10.254.187.96]) by fmsmga008.fm.intel.com with ESMTP; 20 Feb 2020 18:18:17 -0800 From: Inga Stotland To: linux-bluetooth@vger.kernel.org Cc: brian.gix@intel.com, Inga Stotland Subject: [PATCH BlueZ 3/4] mesh: Simplify model virtual pub/sub logic Date: Thu, 20 Feb 2020 18:18:10 -0800 Message-Id: <20200221021811.30408-4-inga.stotland@intel.com> X-Mailer: git-send-email 2.21.1 In-Reply-To: <20200221021811.30408-1-inga.stotland@intel.com> References: <20200221021811.30408-1-inga.stotland@intel.com> MIME-Version: 1.0 Sender: linux-bluetooth-owner@vger.kernel.org Precedence: bulk List-ID: X-Mailing-List: linux-bluetooth@vger.kernel.org This reorganizes the part of the code that handles model publishing and subscribitng to virtual labels. --- mesh/model.c | 258 ++++++++++++++++++++------------------------------- mesh/model.h | 9 +- 2 files changed, 105 insertions(+), 162 deletions(-) diff --git a/mesh/model.c b/mesh/model.c index 4e5856292..55eeb84a1 100644 --- a/mesh/model.c +++ b/mesh/model.c @@ -42,6 +42,8 @@ /* Divide and round to ceiling (up) to calculate segment count */ #define CEILDIV(val, div) (((val) + (div) - 1) / (div)) +#define VIRTUAL_BASE 0x10000 + struct mesh_model { const struct mesh_model_ops *cbs; void *user_data; @@ -54,7 +56,6 @@ struct mesh_model { }; struct mesh_virtual { - uint32_t id; /* Internal ID of a stored virtual addr, min val 0x10000 */ uint16_t ref_cnt; uint16_t addr; /* 16-bit virtual address, used in messages */ uint8_t label[16]; /* 128 bit label UUID */ @@ -79,7 +80,6 @@ struct mod_forward { static struct l_queue *mesh_virtuals; -static uint32_t virt_id_next = VIRTUAL_BASE; static struct timeval tx_start; static bool is_internal(uint32_t id) @@ -120,14 +120,6 @@ static bool has_binding(struct l_queue *bindings, uint16_t idx) return false; } -static bool find_virt_by_id(const void *a, const void *b) -{ - const struct mesh_virtual *virt = a; - uint32_t id = L_PTR_TO_UINT(b); - - return virt->id == id; -} - static bool find_virt_by_label(const void *a, const void *b) { const struct mesh_virtual *virt = a; @@ -307,7 +299,7 @@ static void append_dict_subs_array(struct l_dbus_message_builder *builder, l_dbus_message_builder_enter_variant(builder, "av"); l_dbus_message_builder_enter_array(builder, "v"); - if (!subs) + if (l_queue_isempty(subs)) goto virts; for (entry = l_queue_get_entries(subs); entry; entry = entry->next) { @@ -319,7 +311,7 @@ static void append_dict_subs_array(struct l_dbus_message_builder *builder, } virts: - if (!virts) + if (l_queue_isempty(virts)) goto done; for (entry = l_queue_get_entries(virts); entry; entry = entry->next) { @@ -364,7 +356,7 @@ static void forward_model(void *a, void *b) struct mesh_model *mod = a; struct mod_forward *fwd = b; struct mesh_virtual *virt; - uint32_t dst; + uint16_t dst; bool result; l_debug("model %8.8x with idx %3.3x", mod->id, fwd->app_idx); @@ -379,20 +371,9 @@ static void forward_model(void *a, void *b) fwd->has_dst = true; else if (fwd->virt) { virt = l_queue_find(mod->virtuals, simple_match, fwd->virt); - - /* Check that this is not own publication */ - if (mod->pub && (virt && virt->id == mod->pub->addr)) - return; - if (virt) { - /* - * Map Virtual addresses to a usable namespace that - * prevents us for forwarding a false positive - * (multiple Virtual Addresses that map to the same - * 16-bit virtual address identifier) - */ fwd->has_dst = true; - dst = virt->id; + dst = virt->addr; } } else { if (l_queue_find(mod->subs, simple_match, L_UINT_TO_PTR(dst))) @@ -627,8 +608,13 @@ done: static void remove_pub(struct mesh_node *node, struct mesh_model *mod) { - l_free(mod->pub); - mod->pub = NULL; + if (mod->pub) { + if (mod->pub->virt) + unref_virt(mod->pub->virt); + + l_free(mod->pub); + mod->pub = NULL; + } if (!mod->cbs) /* External models */ @@ -650,11 +636,9 @@ static void model_unbind_idx(struct mesh_node *node, struct mesh_model *mod, /* Internal model */ mod->cbs->bind(idx, ACTION_DELETE); - if (mod->pub && idx != mod->pub->idx) - return; - /* Remove model publication if the publication key is unbound */ - remove_pub(node, mod); + if (mod->pub && idx == mod->pub->idx) + remove_pub(node, mod); } static void model_bind_idx(struct mesh_node *node, struct mesh_model *mod, @@ -746,65 +730,64 @@ static struct mesh_virtual *add_virtual(const uint8_t *v) memcpy(virt->label, v, 16); virt->ref_cnt = 1; - virt->id = virt_id_next++; l_queue_push_head(mesh_virtuals, virt); return virt; } -static int set_pub(struct mesh_model *mod, const uint8_t *pub_addr, +static int set_pub(struct mesh_model *mod, uint16_t pub_addr, uint16_t idx, bool cred_flag, uint8_t ttl, - uint8_t period, uint8_t retransmit, bool b_virt, - uint16_t *dst) + uint8_t period, uint8_t retransmit) { - struct mesh_virtual *virt = NULL; - uint16_t grp; + if (!mod->pub) + mod->pub = l_new(struct mesh_model_pub, 1); - if (dst) { - if (b_virt) - *dst = 0; - else - *dst = l_get_le16(pub_addr); - } + mod->pub->addr = pub_addr; + mod->pub->credential = cred_flag; + mod->pub->idx = idx; + mod->pub->ttl = ttl; + mod->pub->period = period; + mod->pub->retransmit = retransmit; - if (b_virt) { - virt = add_virtual(pub_addr); - if (!virt) - return MESH_STATUS_STORAGE_FAIL; + return MESH_STATUS_SUCCESS; +} - } +static int set_virt_pub(struct mesh_model *mod, const uint8_t *label, + uint16_t idx, bool cred_flag, uint8_t ttl, + uint8_t period, uint8_t retransmit) +{ + struct mesh_virtual *virt = NULL; - /* If the old publication address is virtual, remove it from lists */ - if (mod->pub && mod->pub->addr >= VIRTUAL_BASE) { - struct mesh_virtual *old_virt; + virt = add_virtual(label); + if (!virt) + return MESH_STATUS_STORAGE_FAIL; - old_virt = l_queue_find(mod->virtuals, find_virt_by_id, - L_UINT_TO_PTR(mod->pub->addr)); - if (old_virt) { - l_queue_remove(mod->virtuals, old_virt); - unref_virt(old_virt); - } - } + if (!mod->pub) + mod->pub = l_new(struct mesh_model_pub, 1); + + mod->pub->virt = virt; + return set_pub(mod, virt->addr, idx, cred_flag, ttl, period, + retransmit); +} - mod->pub = l_new(struct mesh_model_pub, 1); +static int add_virt_sub(struct mesh_net *net, struct mesh_model *mod, + const uint8_t *label, uint16_t *dst) +{ + struct mesh_virtual *virt = l_queue_find(mod->virtuals, + find_virt_by_label, label); + + if (!virt) { + virt = add_virtual(label); + if (!virt) + return MESH_STATUS_STORAGE_FAIL; - if (b_virt) { l_queue_push_head(mod->virtuals, virt); - grp = virt->addr; - mod->pub->addr = virt->id; - } else { - grp = l_get_le16(pub_addr); - mod->pub->addr = grp; + mesh_net_dst_reg(net, virt->addr); + l_debug("Added virtual sub addr %4.4x", virt->addr); } if (dst) - *dst = grp; - - mod->pub->credential = cred_flag; - mod->pub->idx = idx; - mod->pub->ttl = ttl; - mod->pub->period = period; - mod->pub->retransmit = retransmit; + *dst = virt->addr; return MESH_STATUS_SUCCESS; } @@ -812,42 +795,25 @@ static int set_pub(struct mesh_model *mod, const uint8_t *pub_addr, static int add_sub(struct mesh_net *net, struct mesh_model *mod, const uint8_t *group, bool b_virt, uint16_t *dst) { - struct mesh_virtual *virt = NULL; uint16_t grp; - if (b_virt) { - virt = add_virtual(group); - if (!virt) - return MESH_STATUS_STORAGE_FAIL; - - grp = virt->addr; - } else { - grp = l_get_le16(group); - } + if (b_virt) + return add_virt_sub(net, mod, group, dst); + grp = l_get_le16(group); if (dst) *dst = grp; - if (!mod->subs) - mod->subs = l_queue_new(); + if (!l_queue_find(mod->subs, simple_match, L_UINT_TO_PTR(grp))) { - /* Check if this group already exists */ - if (l_queue_find(mod->subs, simple_match, L_UINT_TO_PTR(grp))) { - if (b_virt) - unref_virt(virt); + if (!mod->subs) + mod->subs = l_queue_new(); - return MESH_STATUS_SUCCESS; + l_queue_push_tail(mod->subs, L_UINT_TO_PTR(grp)); + mesh_net_dst_reg(net, grp); + l_debug("Added group subscription %4.4x", grp); } - if (b_virt) - l_queue_push_head(mod->virtuals, virt); - - l_queue_push_tail(mod->subs, L_UINT_TO_PTR(grp)); - - l_debug("Added %4.4x", grp); - - mesh_net_dst_reg(net, grp); - return MESH_STATUS_SUCCESS; } @@ -1096,9 +1062,7 @@ int mesh_model_publish(struct mesh_node *node, uint32_t mod_id, { struct mesh_net *net = node_get_net(node); struct mesh_model *mod; - uint32_t target; uint8_t *label = NULL; - uint16_t dst; uint16_t net_idx; bool result; int status; @@ -1125,35 +1089,21 @@ int mesh_model_publish(struct mesh_node *node, uint32_t mod_id, gettimeofday(&tx_start, NULL); - target = mod->pub->addr; - - if (IS_UNASSIGNED(target)) + if (IS_UNASSIGNED(mod->pub->addr)) return MESH_ERROR_DOES_NOT_EXIST; - if (target >= VIRTUAL_BASE) { - struct mesh_virtual *virt; + if (mod->pub->virt) + label = mod->pub->virt->label; - virt = l_queue_find(mesh_virtuals, find_virt_by_id, - L_UINT_TO_PTR(target)); - if (!virt) - return MESH_ERROR_NOT_FOUND; - - label = virt->label; - dst = virt->addr; - } else { - dst = target; - } - - l_debug("publish dst=%x", dst); + l_debug("publish dst=%x", mod->pub->addr); net_idx = appkey_net_idx(net, mod->pub->idx); result = msg_send(node, mod->pub->credential != 0, src, - dst, mod->pub->idx, net_idx, label, ttl, - msg, msg_len); + mod->pub->addr, mod->pub->idx, net_idx, + label, ttl, msg, msg_len); return result ? MESH_ERROR_NONE : MESH_ERROR_FAILED; - } bool mesh_model_send(struct mesh_node *node, uint16_t src, uint16_t dst, @@ -1179,7 +1129,7 @@ bool mesh_model_send(struct mesh_node *node, uint16_t src, uint16_t dst, int mesh_model_pub_set(struct mesh_node *node, uint16_t addr, uint32_t id, const uint8_t *pub_addr, uint16_t idx, bool cred_flag, uint8_t ttl, uint8_t period, uint8_t retransmit, - bool b_virt, uint16_t *dst) + bool is_virt, uint16_t *dst) { struct mesh_model *mod; int status; @@ -1198,13 +1148,25 @@ int mesh_model_pub_set(struct mesh_node *node, uint16_t addr, uint32_t id, * If the publication address is set to unassigned address value, * remove the publication */ - if (!b_virt && IS_UNASSIGNED(l_get_le16(pub_addr))) { + if (!is_virt && IS_UNASSIGNED(l_get_le16(pub_addr))) { remove_pub(node, mod); return MESH_STATUS_SUCCESS; } - status = set_pub(mod, pub_addr, idx, cred_flag, ttl, period, retransmit, - b_virt, dst); + /* Check if the old publication destination is a virtual label */ + if (mod->pub && mod->pub->virt) { + unref_virt(mod->pub->virt); + mod->pub->virt = NULL; + } + + if (!is_virt) { + status = set_pub(mod, l_get_le16(pub_addr), idx, cred_flag, + ttl, period, retransmit); + } else + status = set_virt_pub(mod, pub_addr, idx, cred_flag, ttl, + period, retransmit); + + *dst = mod->pub->addr; if (status != MESH_STATUS_SUCCESS) return status; @@ -1417,7 +1379,7 @@ int mesh_model_sub_get(struct mesh_node *node, uint16_t addr, uint32_t id, } int mesh_model_sub_add(struct mesh_node *node, uint16_t addr, uint32_t id, - const uint8_t *group, bool b_virt, uint16_t *dst) + const uint8_t *group, bool is_virt, uint16_t *dst) { int status; struct mesh_model *mod; @@ -1426,7 +1388,7 @@ int mesh_model_sub_add(struct mesh_node *node, uint16_t addr, uint32_t id, if (!mod) return status; - status = add_sub(node_get_net(node), mod, group, b_virt, dst); + status = add_sub(node_get_net(node), mod, group, is_virt, dst); if (status != MESH_STATUS_SUCCESS) return status; @@ -1439,11 +1401,10 @@ int mesh_model_sub_add(struct mesh_node *node, uint16_t addr, uint32_t id, } int mesh_model_sub_ovr(struct mesh_node *node, uint16_t addr, uint32_t id, - const uint8_t *group, bool b_virt, uint16_t *dst) + const uint8_t *group, bool is_virt, uint16_t *dst) { int status; struct l_queue *virtuals, *subs; - struct mesh_virtual *virt; struct mesh_model *mod; mod = find_model(node, addr, id, &status); @@ -1458,20 +1419,7 @@ int mesh_model_sub_ovr(struct mesh_node *node, uint16_t addr, uint32_t id, if (!mod->subs || !mod->virtuals) return MESH_STATUS_INSUFF_RESOURCES; - /* - * When overwriting the Subscription List, - * make sure any virtual Publication address is preserved - */ - if (mod->pub && mod->pub->addr >= VIRTUAL_BASE) { - virt = l_queue_find(virtuals, find_virt_by_id, - L_UINT_TO_PTR(mod->pub->addr)); - if (virt) { - virt->ref_cnt++; - l_queue_push_head(mod->virtuals, virt); - } - } - - status = add_sub(node_get_net(node), mod, group, b_virt, dst); + status = add_sub(node_get_net(node), mod, group, is_virt, dst); if (status != MESH_STATUS_SUCCESS) { /* Adding new group failed, so revert to old lists */ @@ -1502,7 +1450,7 @@ int mesh_model_sub_ovr(struct mesh_node *node, uint16_t addr, uint32_t id, } int mesh_model_sub_del(struct mesh_node *node, uint16_t addr, uint32_t id, - const uint8_t *group, bool b_virt, uint16_t *dst) + const uint8_t *group, bool is_virt, uint16_t *dst) { int status; uint16_t grp; @@ -1512,7 +1460,7 @@ int mesh_model_sub_del(struct mesh_node *node, uint16_t addr, uint32_t id, if (!mod) return status; - if (b_virt) { + if (is_virt) { struct mesh_virtual *virt; virt = l_queue_find(mod->virtuals, find_virt_by_label, group); @@ -1611,22 +1559,16 @@ struct mesh_model *mesh_model_setup(struct mesh_node *node, uint8_t ele_idx, } /* Add publication if present */ - if (pub && (pub->virt || !(IS_UNASSIGNED(pub->addr)))) { - uint8_t mod_addr[2]; - uint8_t *pub_addr; + if (pub) { uint8_t retransmit = pub->count + ((pub->interval / 50 - 1) << 3); - - /* Add publication */ - l_put_le16(pub->addr, &mod_addr); - pub_addr = pub->virt ? pub->virt_addr : mod_addr; - - if (set_pub(mod, pub_addr, pub->idx, pub->credential, pub->ttl, - pub->period, retransmit, pub->virt, NULL) != - MESH_STATUS_SUCCESS) { - mesh_model_free(mod); - return NULL; - } + if (pub->virt) + set_virt_pub(mod, pub->virt_addr, pub->idx, + pub->credential, pub->ttl, + pub->period, retransmit); + else if (!IS_UNASSIGNED(pub->addr)) + set_pub(mod, pub->addr, pub->idx, pub->credential, + pub->ttl, pub->period, retransmit); } /* Add subscriptions if present */ diff --git a/mesh/model.h b/mesh/model.h index 9c7ce9334..0613c9cca 100644 --- a/mesh/model.h +++ b/mesh/model.h @@ -24,8 +24,6 @@ struct mesh_model; #define MAX_BINDINGS 10 #define MAX_GRP_PER_MOD 10 -#define VIRTUAL_BASE 0x10000 - #define OP_MODEL_TEST 0x8000fffe #define OP_MODEL_INVALID 0x8000ffff @@ -35,8 +33,11 @@ struct mesh_model; #define ACTION_UPDATE 2 #define ACTION_DELETE 3 +struct mesh_virtual; + struct mesh_model_pub { - uint32_t addr; + struct mesh_virtual *virt; + uint16_t addr; uint16_t idx; uint8_t ttl; uint8_t credential; @@ -75,7 +76,7 @@ struct mesh_model_pub *mesh_model_pub_get(struct mesh_node *node, int mesh_model_pub_set(struct mesh_node *node, uint16_t addr, uint32_t id, const uint8_t *pub_addr, uint16_t idx, bool cred_flag, uint8_t ttl, uint8_t period, uint8_t retransmit, - bool b_virt, uint16_t *dst); + bool is_virt, uint16_t *dst); int mesh_model_binding_add(struct mesh_node *node, uint16_t addr, uint32_t id, uint16_t idx); From patchwork Fri Feb 21 02:18:11 2020 Content-Type: text/plain; charset="utf-8" MIME-Version: 1.0 Content-Transfer-Encoding: 7bit X-Patchwork-Submitter: "Stotland, Inga" X-Patchwork-Id: 11395233 X-Patchwork-Delegate: brian.gix@gmail.com Return-Path: Received: from mail.kernel.org (pdx-korg-mail-1.web.codeaurora.org [172.30.200.123]) by pdx-korg-patchwork-2.web.codeaurora.org (Postfix) with ESMTP id B03B813A4 for ; Fri, 21 Feb 2020 02:18:19 +0000 (UTC) Received: from vger.kernel.org (vger.kernel.org [209.132.180.67]) by mail.kernel.org (Postfix) with ESMTP id 8FEBF206EF for ; Fri, 21 Feb 2020 02:18:19 +0000 (UTC) Received: (majordomo@vger.kernel.org) by vger.kernel.org via listexpand id S1729541AbgBUCST (ORCPT ); Thu, 20 Feb 2020 21:18:19 -0500 Received: from mga04.intel.com ([192.55.52.120]:20563 "EHLO mga04.intel.com" rhost-flags-OK-OK-OK-OK) by vger.kernel.org with ESMTP id S1728992AbgBUCST (ORCPT ); Thu, 20 Feb 2020 21:18:19 -0500 X-Amp-Result: SKIPPED(no attachment in message) X-Amp-File-Uploaded: False Received: from fmsmga008.fm.intel.com ([10.253.24.58]) by fmsmga104.fm.intel.com with ESMTP/TLS/DHE-RSA-AES256-GCM-SHA384; 20 Feb 2020 18:18:18 -0800 X-ExtLoop1: 1 X-IronPort-AV: E=Sophos;i="5.70,466,1574150400"; d="scan'208";a="230313331" Received: from ingas-nuc1.sea.intel.com ([10.254.187.96]) by fmsmga008.fm.intel.com with ESMTP; 20 Feb 2020 18:18:17 -0800 From: Inga Stotland To: linux-bluetooth@vger.kernel.org Cc: brian.gix@intel.com, Inga Stotland Subject: [PATCH BlueZ 4/4] mesh: Clean up handling config model publication message Date: Thu, 20 Feb 2020 18:18:11 -0800 Message-Id: <20200221021811.30408-5-inga.stotland@intel.com> X-Mailer: git-send-email 2.21.1 In-Reply-To: <20200221021811.30408-1-inga.stotland@intel.com> References: <20200221021811.30408-1-inga.stotland@intel.com> MIME-Version: 1.0 Sender: linux-bluetooth-owner@vger.kernel.org Precedence: bulk List-ID: X-Mailing-List: linux-bluetooth@vger.kernel.org This tightens up the Config Server code that handles the processing of Config Model Publication Set and Config Model Publication Get messages. --- mesh/cfgmod-server.c | 131 +++++++++++++++---------------------------- 1 file changed, 44 insertions(+), 87 deletions(-) diff --git a/mesh/cfgmod-server.c b/mesh/cfgmod-server.c index 7111411c7..3eb7316fc 100644 --- a/mesh/cfgmod-server.c +++ b/mesh/cfgmod-server.c @@ -36,8 +36,8 @@ static void send_pub_status(struct mesh_node *node, uint16_t net_idx, uint16_t src, uint16_t dst, - uint8_t status, uint16_t ele_addr, uint16_t pub_addr, - uint32_t mod_id, uint16_t idx, bool cred_flag, + uint8_t status, uint16_t ele_addr, uint32_t mod_id, + uint16_t pub_addr, uint16_t idx, bool cred_flag, uint8_t ttl, uint8_t period, uint8_t retransmit) { uint8_t msg[16]; @@ -56,7 +56,7 @@ static void send_pub_status(struct mesh_node *node, uint16_t net_idx, msg[n++] = period; msg[n++] = retransmit; - if (mod_id < 0x10000 || mod_id > VENDOR_ID_MASK) { + if (mod_id >= VENDOR_ID_MASK) { l_put_le16(mod_id, msg + n); n += 2; } else { @@ -76,8 +76,7 @@ static bool config_pub_get(struct mesh_node *node, uint16_t net_idx, { uint32_t mod_id; uint16_t ele_addr; - int ele_idx; - struct mesh_model_pub *pub = NULL; + struct mesh_model_pub *pub; int status; if (size == 4) { @@ -90,27 +89,22 @@ static bool config_pub_get(struct mesh_node *node, uint16_t net_idx, return false; ele_addr = l_get_le16(pkt); - ele_idx = node_get_element_idx(node, ele_addr); - - if (ele_idx >= 0) - pub = mesh_model_pub_get(node, ele_addr, mod_id, &status); - else - status = MESH_STATUS_INVALID_ADDRESS; + pub = mesh_model_pub_get(node, ele_addr, mod_id, &status); if (pub && status == MESH_STATUS_SUCCESS) send_pub_status(node, net_idx, src, dst, status, ele_addr, - pub->addr, mod_id, pub->idx, pub->credential, + mod_id, pub->addr, pub->idx, pub->credential, pub->ttl, pub->period, pub->retransmit); else - send_pub_status(node, net_idx, src, dst, status, ele_addr, 0, - mod_id, 0, 0, 0, 0, 0); + send_pub_status(node, net_idx, src, dst, status, ele_addr, + mod_id, 0, 0, 0, 0, 0, 0); return true; } -static bool config_pub_set(struct mesh_node *node, uint16_t net_idx, +static void config_pub_set(struct mesh_node *node, uint16_t net_idx, uint16_t src, uint16_t dst, - const uint8_t *pkt, uint16_t size, - bool unreliable) + const uint8_t *pkt, uint8_t virt_offset, + bool vendor, bool unreliable) { uint32_t mod_id; uint16_t ele_addr, idx, ota = 0; @@ -119,93 +113,58 @@ static bool config_pub_set(struct mesh_node *node, uint16_t net_idx, uint8_t ttl, period; uint8_t retransmit; int status; - bool cred_flag, b_virt = false; - bool vendor = false; - - switch (size) { - default: - return false; - - case 11: - idx = l_get_le16(pkt + 4); - ttl = pkt[6]; - period = pkt[7]; - retransmit = pkt[8]; - mod_id = l_get_le16(pkt + 9); - mod_id |= VENDOR_ID_MASK; - break; + bool cred_flag; - case 13: - idx = l_get_le16(pkt + 4); - ttl = pkt[6]; - period = pkt[7]; - retransmit = pkt[8]; - mod_id = l_get_le16(pkt + 9) << 16; - mod_id |= l_get_le16(pkt + 11); - vendor = true; - break; + idx = l_get_le16(pkt + 4 + virt_offset); + ttl = pkt[6 + virt_offset]; + period = pkt[7 + virt_offset]; + retransmit = pkt[8 + virt_offset]; + mod_id = l_get_le16(pkt + 9 + virt_offset); - case 25: - b_virt = true; - idx = l_get_le16(pkt + 18); - ttl = pkt[20]; - period = pkt[21]; - retransmit = pkt[22]; - mod_id = l_get_le16(pkt + 23); + if (!vendor) mod_id |= VENDOR_ID_MASK; - break; - - case 27: - b_virt = true; - idx = l_get_le16(pkt + 18); - ttl = pkt[20]; - period = pkt[21]; - retransmit = pkt[22]; - mod_id = l_get_le16(pkt + 23) << 16; - mod_id |= l_get_le16(pkt + 25); - vendor = true; - break; - } + else + mod_id |= l_get_le16(pkt + 11 + virt_offset); ele_addr = l_get_le16(pkt); pub_addr = pkt + 2; - /* Doesn't accept out-of-range TTLs */ - if (ttl > TTL_MASK && ttl != DEFAULT_TTL) - return false; + /* Doesn't accept virtual seeming addresses */ + test_addr = l_get_le16(pub_addr); + if (!virt_offset && IS_VIRTUAL(test_addr)) + return; /* Get cred_flag */ cred_flag = !!(CREDFLAG_MASK & idx); - /* Ignore non-IDX bits */ + /* Get AppKey index */ idx &= APP_IDX_MASK; - /* Doesn't accept virtual seeming addresses */ - test_addr = l_get_le16(pub_addr); - if (!b_virt && test_addr > 0x7fff && test_addr < 0xc000) - return false; - status = mesh_model_pub_set(node, ele_addr, mod_id, pub_addr, idx, cred_flag, ttl, period, retransmit, - b_virt, &ota); + virt_offset != 0, &ota); l_debug("pub_set: status %d, ea %4.4x, ota: %4.4x, mod: %x, idx: %3.3x", status, ele_addr, ota, mod_id, idx); - if (IS_UNASSIGNED(ota) && !b_virt) { - ttl = period = idx = 0; + if (status != MESH_STATUS_SUCCESS) { + if (!unreliable) + send_pub_status(node, net_idx, src, dst, status, + ele_addr, mod_id, 0, 0, 0, 0, 0, 0); - /* Remove model publication from config file */ - if (status == MESH_STATUS_SUCCESS) - mesh_config_model_pub_del(node_config_get(node), - ele_addr, vendor ? mod_id : mod_id & 0x0000ffff, - vendor); - goto done; + return; } - if (status == MESH_STATUS_SUCCESS) { + if (IS_UNASSIGNED(ota) && !virt_offset) { + ttl = period = idx = 0; + + /* Remove model publication from config file */ + mesh_config_model_pub_del(node_config_get(node), ele_addr, + vendor ? mod_id : mod_id & ~VENDOR_ID_MASK, + vendor); + } else { struct mesh_config_pub db_pub = { - .virt = b_virt, + .virt = (virt_offset != 0), .addr = ota, .idx = idx, .ttl = ttl, @@ -215,21 +174,19 @@ static bool config_pub_set(struct mesh_node *node, uint16_t net_idx, .interval = ((retransmit >> 3) + 1) * 50 }; - if (b_virt) + if (virt_offset) memcpy(db_pub.virt_addr, pub_addr, 16); /* Save model publication to config file */ if (!mesh_config_model_pub_add(node_config_get(node), ele_addr, - vendor ? mod_id : mod_id & 0x0000ffff, + vendor ? mod_id : mod_id & ~VENDOR_ID_MASK, vendor, &db_pub)) status = MESH_STATUS_STORAGE_FAIL; } -done: if (!unreliable) send_pub_status(node, net_idx, src, dst, status, ele_addr, ota, mod_id, idx, cred_flag, ttl, period, retransmit); - return true; } static void send_sub_status(struct mesh_node *node, uint16_t net_idx, @@ -825,7 +782,7 @@ static bool cfg_srv_pkt(uint16_t src, uint32_t dst, uint16_t unicast, if (size != 25 && size != 27) return true; - config_pub_set(node, net_idx, src, unicast, pkt, size, + config_pub_set(node, net_idx, src, unicast, pkt, 14, size == 27, !!(opcode & OP_UNRELIABLE)); break; @@ -833,7 +790,7 @@ static bool cfg_srv_pkt(uint16_t src, uint32_t dst, uint16_t unicast, if (size != 11 && size != 13) return true; - config_pub_set(node, net_idx, src, unicast, pkt, size, + config_pub_set(node, net_idx, src, unicast, pkt, 0, size == 13, !!(opcode & OP_UNRELIABLE)); break;