Applied
On Tue, 2020-07-07 at 11:16 -0700, Brian Gix wrote:
> Mesh supports multiple Composition Pages, although only one is defined
> in the current specification. This change allows saving and retrieval of
> any pages, numbered 0-255.
> ---
> mesh/cfgmod-server.c | 42 +++++++++--
> mesh/mesh-config-json.c | 162 ++++++++++++++++++++++++++++++++++++++++
> mesh/mesh-config.h | 12 +++
> mesh/node.c | 130 ++++++++++++++++++++++++++------
> mesh/node.h | 6 +-
> 5 files changed, 320 insertions(+), 32 deletions(-)
>
> diff --git a/mesh/cfgmod-server.c b/mesh/cfgmod-server.c
> index c525d9d24..6194fc7d4 100644
> --- a/mesh/cfgmod-server.c
> +++ b/mesh/cfgmod-server.c
> @@ -34,6 +34,12 @@
>
> #define CFG_MAX_MSG_LEN 380
>
> +/* Supported composition pages, sorted high to low */
> +/* Only page 0 is currently supported */
> +static const uint8_t supported_pages[] = {
> + 0
> +};
> +
> 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, uint32_t mod_id,
> @@ -701,6 +707,33 @@ static void node_reset(void *user_data)
> node_remove(node);
> }
>
> +static uint16_t get_composition(struct mesh_node *node, uint8_t page,
> + uint8_t *buf)
> +{
> + const uint8_t *comp;
> + uint16_t len = 0;
> + size_t i;
> +
> + for (i = 0; i < sizeof(supported_pages); i++) {
> + if (page < supported_pages[i])
> + continue;
> +
> + page = supported_pages[i];
> + comp = node_get_comp(node, page, &len);
> +
> + if (!page || len)
> + break;
> + }
> +
> + if (!len)
> + return 0;
> +
> + *buf++ = page;
> + memcpy(buf, comp, len);
> +
> + return len + 1;
> +}
> +
> static bool cfg_srv_pkt(uint16_t src, uint16_t dst, uint16_t app_idx,
> uint16_t net_idx, const uint8_t *data,
> uint16_t size, const void *user_data)
> @@ -746,16 +779,9 @@ static bool cfg_srv_pkt(uint16_t src, uint16_t dst, uint16_t app_idx,
> if (size != 1)
> return false;
>
> - /* Only page 0 is currently supported */
> - if (pkt[0] != 0) {
> - l_debug("Unsupported page number %d", pkt[0]);
> - l_debug("Returning page number 0");
> - }
> long_msg = l_malloc(CFG_MAX_MSG_LEN);
> n = mesh_model_opcode_set(OP_DEV_COMP_STATUS, long_msg);
> - long_msg[n++] = 0;
> - n += node_generate_comp(node, long_msg + n,
> - CFG_MAX_MSG_LEN - n);
> + n += get_composition(node, pkt[0], long_msg + n);
>
> break;
>
> diff --git a/mesh/mesh-config-json.c b/mesh/mesh-config-json.c
> index 661775f95..88f715fc1 100644
> --- a/mesh/mesh-config-json.c
> +++ b/mesh/mesh-config-json.c
> @@ -430,6 +430,54 @@ static bool read_device_key(json_object *jobj, uint8_t key_buf[16])
> return true;
> }
>
> +static bool read_comp_pages(json_object *jobj, struct mesh_config_node *node)
> +{
> + json_object *jarray, *jentry;
> + struct mesh_config_comp_page *page;
> + int len;
> + int i;
> +
> + if (!json_object_object_get_ex(jobj, "pages", &jarray))
> + return true;
> +
> + if (json_object_get_type(jarray) != json_type_array)
> + return false;
> +
> + len = json_object_array_length(jarray);
> +
> + for (i = 0; i < len; i++) {
> + size_t clen;
> + char *str;
> +
> + jentry = json_object_array_get_idx(jarray, i);
> + str = (char *)json_object_get_string(jentry);
> + clen = strlen(str);
> +
> + if (clen < ((MIN_COMP_SIZE * 2) + 1))
> + continue;
> +
> + clen = (clen / 2) - 1;
> +
> + page = l_malloc(sizeof(struct mesh_config_comp_page) + clen);
> +
> + if (!str2hex(str + 2, clen * 2, page->data, clen))
> + goto parse_fail;
> +
> + if (sscanf(str, "%02hhx", &page->page_num) != 1)
> + goto parse_fail;
> +
> + page->len = clen;
> +
> + l_queue_push_tail(node->pages, page);
> + }
> +
> + return true;
> +
> +parse_fail:
> + l_free(page);
> + return false;
> +}
> +
> static bool read_app_keys(json_object *jobj, struct mesh_config_node *node)
> {
> json_object *jarray;
> @@ -1384,6 +1432,11 @@ static bool read_node(json_object *jnode, struct mesh_config_node *node)
> return false;
> }
>
> + if (!read_comp_pages(jnode, node)) {
> + l_info("Failed to read Composition Pages");
> + return false;
> + }
> +
> if (!parse_elements(jvalue, node)) {
> l_info("Failed to parse elements");
> return false;
> @@ -1889,6 +1942,113 @@ bool mesh_config_model_pub_del(struct mesh_config *cfg, uint16_t addr,
> return save_config(cfg->jnode, cfg->node_dir_path);
> }
>
> +static void del_page(json_object *jarray, uint8_t page)
> +{
> + char buf[3];
> + int i, len;
> +
> + if (!jarray)
> + return;
> +
> + snprintf(buf, 3, "%2.2x", page);
> +
> + len = json_object_array_length(jarray);
> +
> + for (i = 0; i < len; i++) {
> + json_object *jentry;
> + char *str;
> +
> + jentry = json_object_array_get_idx(jarray, i);
> + str = (char *)json_object_get_string(jentry);
> +
> + /* Delete matching page(s) */
> + if (!memcmp(str, buf, 2))
> + json_object_array_del_idx(jarray, i, 1);
> + }
> +}
> +
> +bool mesh_config_comp_page_add(struct mesh_config *cfg, uint8_t page,
> + uint8_t *data, uint16_t size)
> +{
> + json_object *jnode, *jstring, *jarray = NULL;
> + char *buf;
> + int len;
> +
> + if (!cfg)
> + return false;
> +
> + jnode = cfg->jnode;
> +
> + json_object_object_get_ex(jnode, "pages", &jarray);
> +
> + len = (size * 2) + 3;
> + buf = l_malloc(len);
> + snprintf(buf, len, "%2.2x", page);
> + hex2str(data, size, buf + 2, len - 2);
> +
> + if (jarray && jarray_has_string(jarray, buf, len)) {
> + l_free(buf);
> + return true;
> + } else if (!jarray) {
> + jarray = json_object_new_array();
> + json_object_object_add(jnode, "pages", jarray);
> + } else
> + del_page(jarray, page);
> +
> + jstring = json_object_new_string(buf);
> + json_object_array_add(jarray, jstring);
> + l_free(buf);
> +
> + return save_config(jnode, cfg->node_dir_path);
> +}
> +
> +bool mesh_config_comp_page_mv(struct mesh_config *cfg, uint8_t old, uint8_t nw)
> +{
> + json_object *jnode, *jarray = NULL;
> + uint8_t *data;
> + char *str;
> + char old_buf[3];
> + int i, len, dlen = 0;
> + bool status = true;
> +
> + if (!cfg || old == nw)
> + return false;
> +
> + jnode = cfg->jnode;
> +
> + json_object_object_get_ex(jnode, "pages", &jarray);
> +
> + if (!jarray)
> + return false;
> +
> + snprintf(old_buf, 3, "%2.2x", old);
> + data = l_malloc(MAX_MSG_LEN);
> +
> + len = json_object_array_length(jarray);
> +
> + for (i = 0; i < len; i++) {
> + json_object *jentry;
> +
> + jentry = json_object_array_get_idx(jarray, i);
> + str = (char *)json_object_get_string(jentry);
> +
> + /* Delete matching page(s) but save data*/
> + if (!memcmp(str, old_buf, 2)) {
> + dlen = strlen(str + 2);
> + str2hex(str + 2, dlen, data, MAX_MSG_LEN);
> + dlen /= 2;
> + json_object_array_del_idx(jarray, i, 1);
> + }
> + }
> +
> + if (dlen)
> + status = mesh_config_comp_page_add(cfg, nw, data, dlen);
> +
> + l_free(data);
> +
> + return status;
> +}
> +
> bool mesh_config_model_sub_add(struct mesh_config *cfg, uint16_t ele_addr,
> uint32_t mod_id, bool vendor,
> struct mesh_config_sub *sub)
> @@ -2212,6 +2372,7 @@ static bool load_node(const char *fname, const uint8_t uuid[16],
> node.elements = l_queue_new();
> node.netkeys = l_queue_new();
> node.appkeys = l_queue_new();
> + node.pages = l_queue_new();
>
> result = read_node(jnode, &node);
>
> @@ -2238,6 +2399,7 @@ static bool load_node(const char *fname, const uint8_t uuid[16],
> l_free(node.net_transmit);
> l_queue_destroy(node.netkeys, l_free);
> l_queue_destroy(node.appkeys, l_free);
> + l_queue_destroy(node.pages, l_free);
> l_queue_destroy(node.elements, free_element);
>
> if (!result)
> diff --git a/mesh/mesh-config.h b/mesh/mesh-config.h
> index 9f30e693b..7dfa9f20c 100644
> --- a/mesh/mesh-config.h
> +++ b/mesh/mesh-config.h
> @@ -17,6 +17,8 @@
> *
> */
>
> +#define MIN_COMP_SIZE 14
> +
> struct mesh_config;
>
> struct mesh_config_sub {
> @@ -88,10 +90,17 @@ struct mesh_config_transmit {
> uint8_t count;
> };
>
> +struct mesh_config_comp_page {
> + uint16_t len;
> + uint8_t page_num;
> + uint8_t data[];
> +};
> +
> struct mesh_config_node {
> struct l_queue *elements;
> struct l_queue *netkeys;
> struct l_queue *appkeys;
> + struct l_queue *pages;
> uint32_t seq_number;
> uint32_t iv_index;
> bool iv_update;
> @@ -139,6 +148,9 @@ bool mesh_config_write_relay_mode(struct mesh_config *cfg, uint8_t mode,
> bool mesh_config_write_ttl(struct mesh_config *cfg, uint8_t ttl);
> bool mesh_config_write_mode(struct mesh_config *cfg, const char *keyword,
> int value);
> +bool mesh_config_comp_page_add(struct mesh_config *cfg, uint8_t page,
> + uint8_t *data, uint16_t size);
> +bool mesh_config_comp_page_mv(struct mesh_config *cfg, uint8_t old, uint8_t nw);
> bool mesh_config_model_binding_add(struct mesh_config *cfg, uint16_t ele_addr,
> bool vendor, uint32_t mod_id,
> uint16_t app_idx);
> diff --git a/mesh/node.c b/mesh/node.c
> index 3e888ce61..c61167bda 100644
> --- a/mesh/node.c
> +++ b/mesh/node.c
> @@ -46,8 +46,6 @@
> #include "mesh/manager.h"
> #include "mesh/node.h"
>
> -#define MIN_COMP_SIZE 14
> -
> #define MESH_NODE_PATH_PREFIX "/node"
>
> /* Default values for a new locally created node */
> @@ -81,6 +79,7 @@ struct node_composition {
> struct mesh_node {
> struct mesh_net *net;
> struct l_queue *elements;
> + struct l_queue *pages;
> char *app_path;
> char *owner;
> char *obj_path;
> @@ -266,6 +265,7 @@ static struct mesh_node *node_new(const uint8_t uuid[16])
> node = l_new(struct mesh_node, 1);
> node->net = mesh_net_new(node);
> node->elements = l_queue_new();
> + node->pages = l_queue_new();
> memcpy(node->uuid, uuid, sizeof(node->uuid));
> set_defaults(node);
>
> @@ -335,6 +335,7 @@ static void free_node_resources(void *data)
> /* Free dynamic resources */
> free_node_dbus_resources(node);
> l_queue_destroy(node->elements, element_free);
> + l_queue_destroy(node->pages, l_free);
> mesh_agent_remove(node->agent);
> mesh_config_release(node->cfg);
> mesh_net_free(node->net);
> @@ -557,8 +558,15 @@ static bool init_from_storage(struct mesh_config_node *db_node,
>
> l_queue_foreach(db_node->netkeys, set_net_key, node);
>
> - if (db_node->appkeys)
> - l_queue_foreach(db_node->appkeys, set_appkey, node);
> + l_queue_foreach(db_node->appkeys, set_appkey, node);
> +
> + while (l_queue_length(db_node->pages)) {
> + struct mesh_config_comp_page *page;
> +
> + /* Move the composition pages to the node struct */
> + page = l_queue_pop_head(db_node->pages);
> + l_queue_push_tail(node->pages, page);
> + }
>
> mesh_net_set_seq_num(node->net, node->seq_number);
> mesh_net_set_default_ttl(node->net, node->ttl);
> @@ -877,7 +885,8 @@ uint8_t node_friend_mode_get(struct mesh_node *node)
> return node->friend;
> }
>
> -uint16_t node_generate_comp(struct mesh_node *node, uint8_t *buf, uint16_t sz)
> +static uint16_t node_generate_comp(struct mesh_node *node, uint8_t *buf,
> + uint16_t sz)
> {
> uint16_t n, features;
> uint16_t num_ele = 0;
> @@ -991,6 +1000,78 @@ element_done:
> return n;
> }
>
> +static bool match_page(const void *a, const void *b)
> +{
> + const struct mesh_config_comp_page *page = a;
> + uint8_t page_num = L_PTR_TO_UINT(b);
> +
> + return page->page_num == page_num;
> +}
> +
> +bool node_set_comp(struct mesh_node *node, uint8_t page_num,
> + const uint8_t *data, uint16_t len)
> +{
> + struct mesh_config_comp_page *page;
> +
> + if (!node || len < MIN_COMP_SIZE)
> + return false;
> +
> + page = l_queue_remove_if(node->pages, match_page,
> + L_UINT_TO_PTR(page_num));
> +
> + l_free(page);
> +
> + page = l_malloc(sizeof(struct mesh_config_comp_page) + len);
> + page->len = len;
> + page->page_num = page_num;
> + memcpy(page->data, data, len);
> + l_queue_push_tail(node->pages, page);
> +
> + mesh_config_comp_page_add(node->cfg, page_num, page->data, len);
> +
> + return true;
> +}
> +
> +const uint8_t *node_get_comp(struct mesh_node *node, uint8_t page_num,
> + uint16_t *len)
> +{
> + struct mesh_config_comp_page *page = NULL;
> +
> + if (node)
> + page = l_queue_find(node->pages, match_page,
> + L_UINT_TO_PTR(page_num));
> +
> + if (!page) {
> + *len = 0;
> + return NULL;
> + }
> +
> + *len = page->len;
> + return page->data;
> +}
> +
> +bool node_replace_comp(struct mesh_node *node, uint8_t retire, uint8_t with)
> +{
> + struct mesh_config_comp_page *old_page, *keep;
> +
> + if (!node)
> + return false;
> +
> + keep = l_queue_find(node->pages, match_page, L_UINT_TO_PTR(with));
> +
> + if (!keep)
> + return false;
> +
> + old_page = l_queue_remove_if(node->pages, match_page,
> + L_UINT_TO_PTR(retire));
> +
> + l_free(old_page);
> + keep->page_num = retire;
> + mesh_config_comp_page_mv(node->cfg, with, retire);
> +
> + return true;
> +}
> +
> static void attach_io(void *a, void *b)
> {
> struct mesh_node *node = a;
> @@ -1486,27 +1567,30 @@ static void update_model_options(struct mesh_node *node,
>
> static bool check_req_node(struct managed_obj_request *req)
> {
> - uint8_t node_comp[MAX_MSG_LEN - 2];
> - uint8_t attach_comp[MAX_MSG_LEN - 2];
> - uint16_t offset = 10;
> - uint16_t node_len = node_generate_comp(req->node, node_comp,
> - sizeof(node_comp));
> + struct mesh_node *node;
> + const int offset = 8;
> + uint16_t node_len, len;
> + uint8_t comp[MAX_MSG_LEN - 2];
> + const uint8_t *node_comp;
>
> - if (!node_len)
> - return false;
> + if (req->type == REQUEST_TYPE_ATTACH)
> + node = req->attach;
> + else
> + node = req->node;
>
> - if (req->type == REQUEST_TYPE_ATTACH) {
> - uint16_t attach_len = node_generate_comp(req->attach,
> - attach_comp, sizeof(attach_comp));
> + node_comp = node_get_comp(node, 0, &node_len);
> + len = node_generate_comp(node, comp, sizeof(comp));
>
> - /* Verify only element/models composition */
> - if (node_len != attach_len ||
> - memcmp(&node_comp[offset], &attach_comp[offset],
> - node_len - offset)) {
> - l_debug("Failed to verify app's composition data");
> - return false;
> - }
> - }
> + /* If no page 0 exists, save it and return */
> + if (req->type != REQUEST_TYPE_ATTACH || !node_len || !node_comp)
> + return node_set_comp(node, 0, comp, len);
> +
> + if (node_len != len || memcmp(&node_comp[offset], &comp[offset],
> + node_len - offset))
> + return false;
> +
> + else if (memcmp(node_comp, comp, node_len))
> + return node_set_comp(node, 0, comp, len);
>
> return true;
> }
> diff --git a/mesh/node.h b/mesh/node.h
> index 6c4542a78..df058458a 100644
> --- a/mesh/node.h
> +++ b/mesh/node.h
> @@ -63,7 +63,11 @@ struct l_queue *node_get_element_models(struct mesh_node *node, uint8_t ele_idx,
> uint16_t node_get_crpl(struct mesh_node *node);
> bool node_init_from_storage(struct mesh_node *node, const uint8_t uuid[16],
> struct mesh_config_node *db_node);
> -uint16_t node_generate_comp(struct mesh_node *node, uint8_t *buf, uint16_t sz);
> +const uint8_t *node_get_comp(struct mesh_node *node, uint8_t page_num,
> + uint16_t *len);
> +bool node_set_comp(struct mesh_node *node, uint8_t page_num,
> + const uint8_t *data, uint16_t len);
> +bool node_replace_comp(struct mesh_node *node, uint8_t retire, uint8_t with);
> uint8_t node_lpn_mode_get(struct mesh_node *node);
> bool node_relay_mode_set(struct mesh_node *node, bool enable, uint8_t cnt,
> uint16_t interval);
