From: Inga Stotland <inga.stotland@xxxxxxxxx>
---
mesh/cfgmod-server.c | 1194 ++++++++++++++++++++++++++++++++++++++++++++++++++
1 file changed, 1194 insertions(+)
create mode 100644 mesh/cfgmod-server.c
diff --git a/mesh/cfgmod-server.c b/mesh/cfgmod-server.c
new file mode 100644
index 000000000..e982c79b3
--- /dev/null
+++ b/mesh/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 <config.h>
+#endif
+
+#include <unistd.h>
+#include <stdio.h>
+#include <sys/time.h>
+#include <ell/ell.h>
+
+#include "mesh/mesh-defs.h"
+
+#include "mesh/mesh.h"
+#include "mesh/node.h"
+#include "mesh/net.h"
+#include "mesh/appkey.h"
+#include "mesh/model.h"
+#include "mesh/storage.h"
+
+#include "mesh/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.4
--
To unsubscribe from this list: send the line "unsubscribe linux-bluetooth" in
the body of a message to majordomo@xxxxxxxxxxxxxxx
More majordomo info at http://vger.kernel.org/majordomo-info.html
