Return-Path: From: Brian Gix To: Marcel Holtmann , Luiz Augusto von Dentz , linux-bluetooth@vger.kernel.org Cc: Inga Stotland Subject: [PATCH BlueZ v4 11/14] meshd: Mesh config server model Date: Mon, 30 Apr 2018 14:03:16 -0700 Message-Id: <20180430210319.25137-12-brian.gix@intel.com> In-Reply-To: <20180430210319.25137-1-brian.gix@intel.com> References: <20180430210319.25137-1-brian.gix@intel.com> List-ID: From: Inga Stotland --- meshd/src/cfgmod-server.c | 1194 +++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 1194 insertions(+) create mode 100644 meshd/src/cfgmod-server.c diff --git a/meshd/src/cfgmod-server.c b/meshd/src/cfgmod-server.c new file mode 100644 index 000000000..de7c6fcff --- /dev/null +++ b/meshd/src/cfgmod-server.c @@ -0,0 +1,1194 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2018 Intel Corporation. All rights reserved. + * + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * + */ + +#ifdef HAVE_CONFIG_H +#include +#endif + +#include +#include +#include +#include + +#include "meshd/common/mesh-defs.h" + +#include "meshd/src/mesh.h" +#include "meshd/src/node.h" +#include "meshd/src/net.h" +#include "meshd/src/appkey.h" +#include "meshd/src/model.h" +#include "meshd/src/storage.h" + +#include "meshd/src/cfgmod.h" + +#define CFG_MAX_MSG_LEN 380 + +static void send_pub_status(struct mesh_net *net, 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 ttl, uint8_t period, uint8_t retransmit) +{ + uint8_t msg[16]; + size_t n; + + n = mesh_model_opcode_set(OP_CONFIG_MODEL_PUB_STATUS, msg); + msg[n++] = status; + l_put_le16(ele_addr, msg + n); + n += 2; + l_put_le16(pub_addr, msg + n); + n += 2; + idx |= cred_flag ? CREDFLAG_MASK : 0; + l_put_le16(idx, msg + n); + n += 2; + msg[n++] = ttl; + msg[n++] = period; + msg[n++] = retransmit; + if (mod_id < 0x10000) { + l_put_le16(mod_id, msg + n); + n += 2; + } else { + l_put_le16(mod_id >> 16, msg + n); + n += 2; + l_put_le16(mod_id, msg + n); + n += 2; + } + + mesh_model_send(net, CONFIG_SRV_MODEL, + dst, src, + APP_IDX_DEV, DEFAULT_TTL, msg, n); +} + +static bool config_pub_get(struct mesh_net *net, uint16_t src, uint16_t dst, + const uint8_t *pkt, uint16_t size) +{ + uint32_t mod_id; + uint16_t ele_addr; + int ele_idx; + struct mesh_model_pub *pub; + int status; + + if (size == 4) + mod_id = l_get_le16(pkt + 2); + else if (size == 6) { + mod_id = l_get_le16(pkt + 2) << 16; + mod_id |= l_get_le16(pkt + 4); + } else + return false; + + ele_addr = l_get_le16(pkt); + ele_idx = node_get_element_idx(mesh_net_local_node_get(net), ele_addr); + + if (ele_idx >= 0) + pub = mesh_model_pub_get(net, ele_idx, mod_id, &status); + else + status = MESH_STATUS_INVALID_ADDRESS; + + if (pub && status == MESH_STATUS_SUCCESS) + send_pub_status(net, src, dst, status, ele_addr, pub->addr, + mod_id, pub->idx, pub->credential, pub->ttl, + pub->period, pub->retransmit); + else + send_pub_status(net, src, dst, status, ele_addr, 0, mod_id, + 0, 0, 0, 0, 0); + return true; +} + +static bool config_pub_set(struct mesh_net *net, uint16_t src, uint16_t dst, + const uint8_t *pkt, uint16_t size, + bool unreliable) +{ + uint32_t mod_id; + uint16_t ele_addr, idx, ota = 0; + const uint8_t *pub_addr; + uint16_t test_addr; + uint8_t ttl, period; + uint8_t retransmit; + int status; + bool cred_flag, b_virt = 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); + break; + + 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); + break; + + 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); + 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); + break; + } + 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; + + /* Get cred_flag */ + cred_flag = !!(CREDFLAG_MASK & idx); + + /* Ignore non-IDX bits */ + 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(net, ele_addr, mod_id, pub_addr, idx, + cred_flag, ttl, period, retransmit, + b_virt, &ota); + + l_info("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 >= 0 && !unreliable) + send_pub_status(net, src, dst, status, ele_addr, ota, + mod_id, idx, cred_flag, ttl, period, + retransmit); + return true; +} + +static void send_sub_status(struct mesh_net *net, uint16_t src, uint16_t dst, + uint8_t status, uint16_t ele_addr, + uint16_t addr, uint32_t mod) +{ + uint8_t msg[12]; + int n = mesh_model_opcode_set(OP_CONFIG_MODEL_SUB_STATUS, msg); + + msg[n++] = status; + l_put_le16(ele_addr, msg + n); + n += 2; + l_put_le16(addr, msg + n); + n += 2; + if (mod >= 0x10000) { + l_put_le16(mod >> 16, msg + n); + l_put_le16(mod, msg + n + 2); + n += 4; + } else { + l_put_le16(mod, msg + n); + n += 2; + } + + mesh_model_send(net, CONFIG_SRV_MODEL, dst, src, APP_IDX_DEV, + DEFAULT_TTL, msg, n); +} + +static bool config_sub_get(struct mesh_net *net, uint16_t src, uint16_t dst, + const uint8_t *pkt, uint16_t size) +{ + uint16_t ele_addr; + uint32_t mod_id; + uint16_t n = 0; + int ret = 0; + uint8_t *status; + uint16_t buf_size; + uint8_t msg[5 + sizeof(uint16_t) * MAX_GRP_PER_MOD]; + + /* Incoming message has already been size-checked */ + ele_addr = l_get_le16(pkt); + + switch (size) { + default: + l_debug("Bad Len Cfg_Pub_Set: %d", size); + return false; + + case 4: + mod_id = l_get_le16(pkt + 2); + n = mesh_model_opcode_set(OP_CONFIG_MODEL_SUB_LIST, msg); + status = msg + n; + msg[n++] = 0; + l_put_le16(ele_addr, msg + n); + n += 2; + l_put_le16(mod_id, msg + n); + n += 2; + break; + + case 6: + mod_id = l_get_le16(pkt + 2) << 16; + mod_id |= l_get_le16(pkt + 4); + n = mesh_model_opcode_set(OP_CONFIG_VEND_MODEL_SUB_LIST, msg); + status = msg + n; + msg[n++] = 0; + l_put_le16(ele_addr, msg + n); + n += 2; + l_put_le16(mod_id >> 16, msg + n); + n += 2; + l_put_le16(mod_id, msg + n); + n += 2; + break; + } + + buf_size = sizeof(uint16_t) * MAX_GRP_PER_MOD; + ret = mesh_model_sub_get(net, ele_addr, mod_id, msg + n, buf_size, + &size); + + if (!ret) + n += size; + else if (ret > 0) + *status = ret; + + mesh_model_send(net, CONFIG_SRV_MODEL, + dst, src, APP_IDX_DEV, + DEFAULT_TTL, msg, n); + return true; +} + +static void config_sub_set(struct mesh_net *net, uint16_t src, uint16_t dst, + const uint8_t *pkt, uint16_t size, + bool virt, uint32_t opcode) +{ + uint16_t grp, ele_addr; + bool unreliable = !!(opcode & OP_UNRELIABLE); + uint32_t mod_id, func; + const uint8_t *addr = NULL; + int status = 0; + + switch (size) { + default: + l_error("Bad Len Cfg_Sub_Set: %d", size); + return; + case 4: + if (opcode != OP_CONFIG_MODEL_SUB_DELETE_ALL) + return; + mod_id = l_get_le16(pkt + 2); + break; + case 6: + if (virt) + return; + if (opcode != OP_CONFIG_MODEL_SUB_DELETE_ALL) + mod_id = l_get_le16(pkt + 4); + else { + mod_id = l_get_le16(pkt + 2) << 16; + mod_id |= l_get_le16(pkt + 4); + } + break; + case 8: + if (virt) + return; + mod_id = l_get_le16(pkt + 4) << 16; + mod_id |= l_get_le16(pkt + 6); + break; + case 20: + if (!virt) + return; + mod_id = l_get_le16(pkt + 18); + break; + case 22: + if (!virt) + return; + mod_id = l_get_le16(pkt + 18) << 16; + mod_id |= l_get_le16(pkt + 20); + break; + } + ele_addr = l_get_le16(pkt); + + if (opcode != OP_CONFIG_MODEL_SUB_DELETE_ALL) { + addr = pkt + 2; + grp = l_get_le16(addr); + } else + grp = UNASSIGNED_ADDRESS; + + func = opcode & ~OP_UNRELIABLE; + switch (func) { + default: + l_info("Bad opcode: %x", func); + return; + + case OP_CONFIG_MODEL_SUB_DELETE_ALL: + status = mesh_model_sub_del_all(net, ele_addr, mod_id); + break; + + case OP_CONFIG_MODEL_SUB_VIRT_OVERWRITE: + grp = UNASSIGNED_ADDRESS; + /* Fall Through */ + case OP_CONFIG_MODEL_SUB_OVERWRITE: + status = mesh_model_sub_ovr(net, ele_addr, mod_id, + addr, virt, &grp); + break; + case OP_CONFIG_MODEL_SUB_VIRT_ADD: + grp = UNASSIGNED_ADDRESS; + /* Fall Through */ + case OP_CONFIG_MODEL_SUB_ADD: + status = mesh_model_sub_add(net, ele_addr, mod_id, + addr, virt, &grp); + break; + case OP_CONFIG_MODEL_SUB_VIRT_DELETE: + grp = UNASSIGNED_ADDRESS; + /* Fall Through */ + case OP_CONFIG_MODEL_SUB_DELETE: + status = mesh_model_sub_del(net, ele_addr, mod_id, + addr, virt, &grp); + break; + } + + if (!unreliable && status >= 0) + send_sub_status(net, src, dst, status, ele_addr, grp, mod_id); + +} + +static void send_model_app_status(struct mesh_net *net, uint16_t src, + uint16_t dst, uint8_t status, + uint16_t addr, uint32_t id, + uint16_t idx) +{ + uint8_t msg[12]; + size_t n = mesh_model_opcode_set(OP_MODEL_APP_STATUS, msg); + + msg[n++] = status; + l_put_le16(addr, msg + n); + n += 2; + l_put_le16(idx, msg + n); + n += 2; + if (id > 0xffff) { + l_put_le16(id >> 16, msg + n); + n += 2; + } + l_put_le16(id, msg + n); + n += 2; + + mesh_model_send(net, CONFIG_SRV_MODEL, dst, src, APP_IDX_DEV, + DEFAULT_TTL, msg, n); +} + +static void model_app_list(struct mesh_net *net, uint16_t src, uint16_t dst, + const uint8_t *pkt, uint16_t size) +{ + uint16_t ele_addr; + uint32_t mod_id = 0xffff; + uint8_t *msg = NULL; + uint8_t *status; + uint16_t n, buf_size; + int result; + + buf_size = MAX_BINDINGS * sizeof(uint16_t); + msg = l_malloc(7 + buf_size); + if (!msg) + return; + + ele_addr = l_get_le16(pkt); + + switch (size) { + default: + l_free(msg); + return; + case 4: + n = mesh_model_opcode_set(OP_MODEL_APP_LIST, msg); + status = msg + n; + mod_id = l_get_le16(pkt + 2); + l_put_le16(ele_addr, msg + 1 + n); + l_put_le16(mod_id, msg + 3 + n); + n += 5; + break; + case 6: + n = mesh_model_opcode_set(OP_VEND_MODEL_APP_LIST, msg); + status = msg + n; + mod_id = l_get_le16(pkt + 2) << 16; + mod_id |= l_get_le16(pkt + 4); + + l_put_le16(ele_addr, msg + 1 + n); + l_put_le16(mod_id >> 16, msg + 3 + n); + l_put_le16(mod_id, msg + 5 + n); + n += 7; + break; + } + + + result = mesh_model_get_bindings(net, ele_addr, mod_id, msg + n, + buf_size, &size); + n += size; + + if (result >= 0) { + *status = result; + mesh_model_send(net, CONFIG_SRV_MODEL, dst, src, APP_IDX_DEV, + DEFAULT_TTL, msg, n); + } + + l_free(msg); +} + +static bool model_app_bind(struct mesh_net *net, uint16_t src, uint16_t dst, + const uint8_t *pkt, uint16_t size, + bool unbind) +{ + uint16_t ele_addr; + uint32_t mod_id; + uint16_t idx; + int result; + + switch (size) { + default: + return false; + + case 6: + mod_id = l_get_le16(pkt + 4); + break; + case 8: + mod_id = l_get_le16(pkt + 4) << 16; + mod_id |= l_get_le16(pkt + 6); + break; + } + + ele_addr = l_get_le16(pkt); + idx = l_get_le16(pkt + 2); + + if (idx > 0xfff) + return false; + + if (unbind) + result = mesh_model_binding_del(net, ele_addr, mod_id, idx); + else + result = mesh_model_binding_add(net, ele_addr, mod_id, idx); + + send_model_app_status(net, src, dst, result, ele_addr, mod_id, idx); + + return true; +} + +static void hb_pub_timeout_func(struct l_timeout *timeout, void *user_data) +{ + struct mesh_net *net = user_data; + struct mesh_net_heartbeat *hb = mesh_net_heartbeat_get(net); + + mesh_net_heartbeat_send(net); + + if (hb->pub_count != 0xffff) + hb->pub_count--; + if (hb->pub_count > 0) + l_timeout_modify(hb->pub_timer, hb->pub_period); + else { + l_timeout_remove(hb->pub_timer); + hb->pub_timer = NULL; + } + l_debug("%d left", hb->pub_count); +} + +static void update_hb_pub_timer(struct mesh_net *net, + struct mesh_net_heartbeat *hb) +{ + if (IS_UNASSIGNED(hb->pub_dst) || hb->pub_count == 0) { + l_timeout_remove(hb->pub_timer); + hb->pub_timer = NULL; + return; + } + + if (!hb->pub_timer) + hb->pub_timer = l_timeout_create(hb->pub_period, + hb_pub_timeout_func, net, NULL); + else + l_timeout_modify(hb->pub_timer, hb->pub_period); +} + +static void hb_sub_timeout_func(struct l_timeout *timeout, void *user_data) +{ + struct mesh_net *net = user_data; + struct mesh_net_heartbeat *hb = mesh_net_heartbeat_get(net); + + l_info("HB Subscription Ended"); + l_timeout_remove(hb->sub_timer); + hb->sub_timer = NULL; + hb->sub_enabled = false; +} + +static uint8_t uint32_to_log(uint32_t value) +{ + uint32_t val = 1; + uint8_t ret = 1; + + if (!value) + return 0; + else if (value > 0x10000) + return 0xff; + + while (val < value) { + val <<= 1; + ret++; + } + + return ret; +} + +static uint32_t log_to_uint32(uint8_t log, uint8_t offset) +{ + if (!log) + return 0x0000; + else if (log > 0x11) + return 0xffff; + else + return (1 << (log - offset)); +} + + +static int hb_subscription_set(struct mesh_net *net, uint16_t src, + uint16_t dst, uint8_t period_log) +{ + struct mesh_net_heartbeat *hb = mesh_net_heartbeat_get(net); + struct timeval time_now; + + /* SRC must be Unicast, DST can be any legal address except Virtual */ + if ((!IS_UNASSIGNED(src) && !IS_UNICAST(src)) || IS_VIRTUAL(dst)) + return -1; + + /* Check if the subscription should be disabled */ + if (IS_UNASSIGNED(src) || IS_UNASSIGNED(dst)) { + if (IS_GROUP(hb->sub_dst)) + mesh_net_dst_unreg(net, hb->sub_dst); + + l_timeout_remove(hb->sub_timer); + hb->sub_timer = NULL; + hb->sub_enabled = false; + hb->sub_dst = UNASSIGNED_ADDRESS; + hb->sub_src = UNASSIGNED_ADDRESS; + hb->sub_count = 0; + hb->sub_period = 0; + hb->sub_min_hops = 0; + hb->sub_max_hops = 0; + return MESH_STATUS_SUCCESS; + } else if (!period_log && src == hb->sub_src && dst == hb->sub_dst) { + /* Preserve collected data, but disable */ + l_timeout_remove(hb->sub_timer); + hb->sub_timer = NULL; + hb->sub_enabled = false; + hb->sub_period = 0; + return MESH_STATUS_SUCCESS; + } + + if (hb->sub_dst != dst) { + if (IS_GROUP(hb->sub_dst)) + mesh_net_dst_unreg(net, hb->sub_dst); + if (IS_GROUP(dst)) + mesh_net_dst_reg(net, dst); + } + + hb->sub_enabled = !!period_log; + hb->sub_src = src; + hb->sub_dst = dst; + hb->sub_count = 0; + hb->sub_period = log_to_uint32(period_log, 1); + hb->sub_min_hops = 0x00; + hb->sub_max_hops = 0x00; + + gettimeofday(&time_now, NULL); + hb->sub_start = time_now.tv_sec; + + if (!hb->sub_enabled) { + l_timeout_remove(hb->sub_timer); + hb->sub_timer = NULL; + return MESH_STATUS_SUCCESS; + } + + hb->sub_min_hops = 0xff; + + if (!hb->sub_timer) + hb->sub_timer = l_timeout_create(hb->sub_period, + hb_sub_timeout_func, net, NULL); + else + l_timeout_modify(hb->sub_timer, hb->sub_period); + + return MESH_STATUS_SUCCESS; +} + +static void node_reset(struct l_timeout *timeout, void *user_data) +{ + l_info("Node Reset"); + l_timeout_remove(timeout); + l_main_quit(); +} + +static bool cfg_srv_pkt(uint16_t src, uint32_t dst, + uint16_t unicast, uint16_t idx, + const uint8_t *data, uint16_t size, + uint8_t ttl, const void *user_data) +{ + struct mesh_net *net = (struct mesh_net *) user_data; + const uint8_t *pkt = data; + struct timeval time_now; + uint32_t opcode, tmp32; + int b_res = MESH_STATUS_SUCCESS; + uint8_t msg[11]; + uint8_t *long_msg = NULL; + struct mesh_net_heartbeat *hb; + uint16_t net_idx, app_idx; + uint8_t state, status; + uint8_t phase; + bool virt = false; + uint8_t count; + uint16_t interval; + struct mesh_node *node; + uint16_t n; + + if (idx != APP_IDX_DEV) + return false; + + if (mesh_model_opcode_get(pkt, size, &opcode, &n)) { + size -= n; + pkt += n; + } else + return false; + + hb = mesh_net_heartbeat_get(net); + l_debug("CONFIG-SRV-opcode 0x%x size %u idx %3.3x", opcode, size, idx); + + node = mesh_net_local_node_get(net); + n = 0; + + switch (opcode) { + default: + return false; + + case OP_DEV_COMP_GET: + if (size != 1) + return false; + + /* Only page 0 is currently supported */ + if (pkt[0] != 0) { + l_info("Unsupported page number %d", pkt[0]); + l_info("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); + + break; + + case OP_CONFIG_DEFAULT_TTL_SET: + if (size != 1 || pkt[0] > TTL_MASK || pkt[0] == 1) + return true; + + if (pkt[0] <= TTL_MASK) + node_default_ttl_set(node, pkt[0]); + /* Fall Through */ + + case OP_CONFIG_DEFAULT_TTL_GET: + l_info("Get/Set Default TTL"); + + n = mesh_model_opcode_set(OP_CONFIG_DEFAULT_TTL_STATUS, msg); + msg[n++] = node_default_ttl_get(node); + break; + + case OP_CONFIG_MODEL_PUB_VIRT_SET: + if (size != 25 && size != 27) + return true; + + config_pub_set(net, src, unicast, pkt, size, + !!(opcode & OP_UNRELIABLE)); + break; + + case OP_CONFIG_MODEL_PUB_SET: + if (size != 11 && size != 13) + return true; + + config_pub_set(net, src, unicast, pkt, size, + !!(opcode & OP_UNRELIABLE)); + break; + + case OP_CONFIG_MODEL_PUB_GET: + config_pub_get(net, src, unicast, pkt, size); + break; + + case OP_CONFIG_VEND_MODEL_SUB_GET: + if (size != 6) + return true; + config_sub_get(net, src, unicast, pkt, size); + break; + + case OP_CONFIG_MODEL_SUB_GET: + if (size != 4) + return true; + config_sub_get(net, src, unicast, pkt, size); + break; + + case OP_CONFIG_MODEL_SUB_VIRT_OVERWRITE: + case OP_CONFIG_MODEL_SUB_VIRT_DELETE: + case OP_CONFIG_MODEL_SUB_VIRT_ADD: + virt = true; + /* Fall Through */ + case OP_CONFIG_MODEL_SUB_OVERWRITE: + case OP_CONFIG_MODEL_SUB_DELETE: + case OP_CONFIG_MODEL_SUB_ADD: + case OP_CONFIG_MODEL_SUB_DELETE_ALL: + config_sub_set(net, src, unicast, pkt, size, virt, opcode); + break; + + case OP_CONFIG_RELAY_SET: + if (size != 2 || pkt[0] > 0x01) + return true; + + count = (pkt[1] >> 5) + 1; + interval = ((pkt[1] & 0x1f) + 1) * 10; + node_relay_mode_set(node, !!pkt[0], pkt[1]>>5, + pkt[1] & 0x1f); + /* Fall Through */ + + case OP_CONFIG_RELAY_GET: + n = mesh_model_opcode_set(OP_CONFIG_RELAY_STATUS, msg); + + msg[n++] = node_relay_mode_get(node, &count, &interval); + msg[n++] = ((count - 1) << 5) + ((interval/10 - 1) & 0x1f); + + l_info("Get/Set Relay Config (%d)", msg[n-1]); + break; + + case OP_CONFIG_NETWORK_TRANSMIT_SET: + if (size != 1) + return true; + + count = (pkt[0] >> 5) + 1; + interval = ((pkt[0] & 0x1f) + 1) * 10; + if (storage_local_set_transmit_params(net, count, interval)) + mesh_net_transmit_params_set(net, count, interval); + /* Fall Through */ + + case OP_CONFIG_NETWORK_TRANSMIT_GET: + n = mesh_model_opcode_set(OP_CONFIG_NETWORK_TRANSMIT_STATUS, + msg); + mesh_net_transmit_params_get(net, &count, &interval); + msg[n++] = ((count - 1) << 5) + ((interval/10 - 1) & 0x1f); + + l_info("Get/Set Network Transmit Config"); + break; + + case OP_CONFIG_PROXY_SET: + if (size != 1 || pkt[0] > 0x01) + return true; + + node_proxy_mode_set(node, !!pkt[0]); + /* Fall Through */ + + case OP_CONFIG_PROXY_GET: + n = mesh_model_opcode_set(OP_CONFIG_PROXY_STATUS, msg); + + msg[n++] = node_proxy_mode_get(node); + l_info("Get/Set Config Proxy (%d)", msg[n-1]); + break; + + case OP_NODE_IDENTITY_SET: + if (size != 3 || pkt[2] > 0x01) + return true; + + net_idx = l_get_le16(pkt); + if (net_idx > 0xfff) + return true; + + /* + * Currently no support for proxy: node identity not supported + */ + + /* Fall Through */ + + case OP_NODE_IDENTITY_GET: + if (size < 2) + return true; + + net_idx = l_get_le16(pkt); + if (net_idx > 0xfff) + return true; + + n = mesh_model_opcode_set(OP_NODE_IDENTITY_STATUS, msg); + + status = mesh_net_get_identity_mode(net, net_idx, &state); + + msg[n++] = status; + + l_put_le16(net_idx, msg + n); + n += 2; + + msg[n++] = state; + l_info("Get/Set Config Identity (%d)", state); + break; + + case OP_CONFIG_BEACON_SET: + if (size != 1 || pkt[0] > 0x01) + return true; + + node_beacon_mode_set(node, !!pkt[0]); + /* Fall Through */ + + case OP_CONFIG_BEACON_GET: + n = mesh_model_opcode_set(OP_CONFIG_BEACON_STATUS, msg); + + msg[n++] = node_beacon_mode_get(node); + l_info("Get/Set Config Beacon (%d)", msg[n-1]); + break; + + case OP_CONFIG_FRIEND_SET: + if (size != 1 || pkt[0] > 0x01) + return true; + + node_friend_mode_set(node, !!pkt[0]); + /* Fall Through */ + + case OP_CONFIG_FRIEND_GET: + + n = mesh_model_opcode_set(OP_CONFIG_FRIEND_STATUS, msg); + + msg[n++] = node_friend_mode_get(node); + l_info("Get/Set Friend (%d)", msg[n-1]); + break; + + case OP_CONFIG_KEY_REFRESH_PHASE_SET: + if (size != 3 || pkt[2] > 0x03) + return true; + + b_res = mesh_net_key_refresh_phase_set(net, l_get_le16(pkt), + pkt[2]); + size = 2; + /* Fall Through */ + + case OP_CONFIG_KEY_REFRESH_PHASE_GET: + if (size != 2) + return true; + + net_idx = l_get_le16(pkt); + + n = mesh_model_opcode_set(OP_CONFIG_KEY_REFRESH_PHASE_STATUS, + msg); + + /* State: 0x00-0x03 phase of key refresh */ + status = mesh_net_key_refresh_phase_get(net, net_idx, + &phase); + if (status != MESH_STATUS_SUCCESS) { + b_res = status; + phase = KEY_REFRESH_PHASE_NONE; + } + + msg[n++] = b_res; + l_put_le16(net_idx, msg + n); + n += 2; + msg[n++] = phase; + + l_info("Get/Set Key Refresh State (%d)", msg[n-1]); + break; + + case OP_APPKEY_ADD: + case OP_APPKEY_UPDATE: + if (size != 19) + return true; + + net_idx = l_get_le16(pkt) & 0xfff; + app_idx = l_get_le16(pkt + 1) >> 4; + b_res = appkey_key_add(net, net_idx, app_idx, pkt + 3, + opcode == OP_APPKEY_UPDATE); + + l_info("Add/Update AppKey %s: Net_Idx %3.3x, App_Idx %3.3x", + (b_res == MESH_STATUS_SUCCESS) ? "success" : "fail", + net_idx, app_idx); + + + n = mesh_model_opcode_set(OP_APPKEY_STATUS, msg); + + msg[n++] = b_res; + msg[n++] = pkt[0]; + msg[n++] = pkt[1]; + msg[n++] = pkt[2]; + break; + + case OP_APPKEY_DELETE: + if (size != 3) + return + true; + + net_idx = l_get_le16(pkt) & 0xfff; + app_idx = l_get_le16(pkt + 1) >> 4; + b_res = appkey_key_delete(net, net_idx, app_idx); + if (b_res == MESH_STATUS_SUCCESS) + node_app_key_delete(net, dst, net_idx, app_idx); + l_info("Delete AppKey %s Net_Idx %3.3x to App_Idx %3.3x", + (b_res == MESH_STATUS_SUCCESS) ? "success" : "fail", + net_idx, app_idx); + + n = mesh_model_opcode_set(OP_APPKEY_STATUS, msg); + msg[n++] = b_res; + msg[n++] = pkt[0]; + msg[n++] = pkt[1]; + msg[n++] = pkt[2]; + break; + + case OP_APPKEY_GET: + if (size != 2) + return true; + net_idx = l_get_le16(pkt); + + long_msg = l_malloc(CFG_MAX_MSG_LEN); + n = mesh_model_opcode_set(OP_APPKEY_LIST, long_msg); + + status = appkey_list(net, net_idx, long_msg + n + 3, + CFG_MAX_MSG_LEN - n - 3, &size); + + long_msg[n] = status; + l_put_le16(net_idx, long_msg + n + 1); + n += (size + 3); + break; + + case OP_NETKEY_ADD: + case OP_NETKEY_UPDATE: + if (size != 18) + return true; + + b_res = mesh_net_add_key(net, opcode == OP_NETKEY_UPDATE, + l_get_le16(pkt), pkt + 2); + + l_info("NetKey Add/Update %s", + (b_res == MESH_STATUS_SUCCESS) ? "success" : "fail"); + + n = mesh_model_opcode_set(OP_NETKEY_STATUS, msg); + msg[n++] = b_res; + l_put_le16(l_get_le16(pkt), msg + n); + n += 2; + break; + + case OP_NETKEY_DELETE: + if (size != 2) + return true; + + b_res = mesh_net_del_key(net, l_get_le16(pkt)); + + l_info("NetKey delete %s", + (b_res == MESH_STATUS_SUCCESS) ? "success" : "fail"); + + n = mesh_model_opcode_set(OP_NETKEY_STATUS, msg); + msg[n++] = b_res; + l_put_le16(l_get_le16(pkt), msg + n); + n += 2; + break; + + case OP_NETKEY_GET: + long_msg = l_malloc(CFG_MAX_MSG_LEN); + n = mesh_model_opcode_set(OP_NETKEY_LIST, long_msg); + size = CFG_MAX_MSG_LEN - n; + + if (mesh_net_key_list_get(net, long_msg + n, &size)) + n += size; + else + n = 0; + break; + + case OP_MODEL_APP_BIND: + case OP_MODEL_APP_UNBIND: + model_app_bind(net, src, unicast, pkt, size, + opcode != OP_MODEL_APP_BIND); + break; + + case OP_VEND_MODEL_APP_GET: + if (size != 6) + return true; + model_app_list(net, src, unicast, pkt, size); + break; + + case OP_MODEL_APP_GET: + if (size != 4) + return true; + model_app_list(net, src, unicast, pkt, size); + break; + + case OP_CONFIG_HEARTBEAT_PUB_SET: + l_info("OP_CONFIG_HEARTBEAT_PUB_SET"); + if (size != 9) { + l_info("bad size %d", size); + return true; + } + if (pkt[2] > 0x11 || pkt[3] > 0x10 || pkt[4] > 0x7f) + return true; + else if (IS_VIRTUAL(l_get_le16(pkt))) + b_res = MESH_STATUS_INVALID_ADDRESS; + else if (l_get_le16(pkt + 7) != mesh_net_get_primary_idx(net)) + /* Future work: check for valid subnets */ + b_res = MESH_STATUS_INVALID_NETKEY; + + n = mesh_model_opcode_set(OP_CONFIG_HEARTBEAT_PUB_STATUS, + msg); + msg[n++] = b_res; + + memcpy(&msg[n], pkt, 9); + + /* Ignore RFU bits in features */ + l_put_le16(l_get_le16(pkt + 5) & 0xf, &msg[n + 5]); + + /* Add octet count to status */ + n += 9; + + if (b_res != MESH_STATUS_SUCCESS) + break; + + hb->pub_dst = l_get_le16(pkt); + if (hb->pub_dst == UNASSIGNED_ADDRESS || + pkt[2] == 0 || pkt[3] == 0) { + /* + * We might still have a pub_dst here in case + * we need it for State Change heartbeat + */ + hb->pub_count = 0; + hb->pub_period = 0; + } else { + hb->pub_count = (pkt[2] != 0xff) ? + log_to_uint32(pkt[2], 1) : 0xffff; + hb->pub_period = log_to_uint32(pkt[3], 1); + } + + hb->pub_ttl = pkt[4]; + hb->pub_features = l_get_le16(pkt + 5) & 0xf; + hb->pub_net_idx = l_get_le16(pkt + 7); + update_hb_pub_timer(net, hb); + + break; + + case OP_CONFIG_HEARTBEAT_PUB_GET: + n = mesh_model_opcode_set(OP_CONFIG_HEARTBEAT_PUB_STATUS, msg); + msg[n++] = b_res; + l_put_le16(hb->pub_dst, msg + n); + n += 2; + msg[n++] = uint32_to_log(hb->pub_count); + msg[n++] = uint32_to_log(hb->pub_period); + msg[n++] = hb->pub_ttl; + l_put_le16(hb->pub_features, msg + n); + n += 2; + l_put_le16(hb->pub_net_idx, msg + n); + n += 2; + break; + + case OP_CONFIG_HEARTBEAT_SUB_SET: + if (size != 5) + return true; + + l_info("Set Sub Period (Log %2.2x) %d sec", + pkt[4], log_to_uint32(pkt[4], 1)); + + b_res = hb_subscription_set(net, l_get_le16(pkt), + l_get_le16(pkt + 2), + pkt[4]); + if (b_res < 0) + return true; + + /* Fall through */ + + case OP_CONFIG_HEARTBEAT_SUB_GET: + gettimeofday(&time_now, NULL); + time_now.tv_sec -= hb->sub_start; + + if (time_now.tv_sec >= hb->sub_period) + time_now.tv_sec = 0; + else + time_now.tv_sec = hb->sub_period - time_now.tv_sec; + + l_info("Sub Period (Log %2.2x) %d sec", + uint32_to_log(time_now.tv_sec), + (int) time_now.tv_sec); + + n = mesh_model_opcode_set(OP_CONFIG_HEARTBEAT_SUB_STATUS, msg); + msg[n++] = b_res; + l_put_le16(hb->sub_src, msg + n); + n += 2; + l_put_le16(hb->sub_dst, msg + n); + n += 2; + msg[n++] = uint32_to_log(time_now.tv_sec); + msg[n++] = uint32_to_log(hb->sub_count); + msg[n++] = hb->sub_count ? hb->sub_min_hops : 0; + msg[n++] = hb->sub_max_hops; + break; + + case OP_CONFIG_POLL_TIMEOUT_LIST: + if (size != 2 || l_get_le16(pkt) == 0 || + l_get_le16(pkt) > 0x7fff) + return true; + + n = mesh_model_opcode_set(OP_CONFIG_POLL_TIMEOUT_STATUS, msg); + l_put_le16(l_get_le16(pkt), msg + n); + n += 2; + tmp32 = mesh_net_friend_timeout(net, l_get_le16(pkt)); + msg[n++] = tmp32; + msg[n++] = tmp32 >> 8; + msg[n++] = tmp32 >> 16; + break; + + case OP_NODE_RESET: + n = mesh_model_opcode_set(OP_NODE_RESET_STATUS, msg); + l_timeout_create(1, node_reset, net, NULL); + break; + } + + if (n) { + /* print_packet("App Tx", long_msg ? long_msg : msg, n); */ + mesh_model_send(net, CONFIG_SRV_MODEL, + unicast, src, + APP_IDX_DEV, DEFAULT_TTL, + long_msg ? long_msg : msg, n); + } + l_free(long_msg); + + return true; +} + +static void cfgmod_srv_unregister(void *user_data) +{ + struct mesh_net *net = user_data; + struct mesh_net_heartbeat *hb = mesh_net_heartbeat_get(net); + + l_timeout_remove(hb->pub_timer); + l_timeout_remove(hb->sub_timer); + hb->pub_timer = hb->sub_timer = NULL; +} + +static const struct mesh_model_ops ops = { + .unregister = cfgmod_srv_unregister, + .recv = cfg_srv_pkt, + .bind = NULL, + .sub = NULL, + .pub = NULL +}; + +void mesh_config_srv_init(struct mesh_net *net, uint8_t ele_idx) +{ + l_debug("%2.2x", ele_idx); + mesh_model_register(net, ele_idx, CONFIG_SRV_MODEL, &ops, net); +} -- 2.14.3