diff --git a/mesh/agent.c b/mesh/agent.c
new file mode 100644
index 0000000..0944862
--- /dev/null
+++ b/mesh/agent.c
+/*
+ *
+ * BlueZ - Bluetooth protocol stack for Linux
+ *
+ * Copyright (C) 2017 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.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <inttypes.h>
+#include <readline/readline.h>
+
+#include <glib.h>
+
+#include <lib/bluetooth.h>
+#include "client/display.h"
+#include "util.h"
+#include "agent.h"
+
+#define AGENT_PROMPT COLOR_RED "[agent]" COLOR_OFF " "
+
+static char *agent_saved_prompt = NULL;
+static int agent_saved_point = 0;
+
+struct input_request {
+ oob_type_t type;
+ uint16_t len;
+ agent_input_cb cb;
+ void *user_data;
+};
+
+static struct input_request pending_request = {NONE, 0, NULL, NULL};
+
+static void agent_prompt(const char *msg)
+{
+ char *prompt;
+
+ /* Normal use should not prompt for user input to the agent a second
+ * time before it releases the prompt, but we take a safe action. */
+ if (agent_saved_prompt)
+ return;
+
+ agent_saved_point = rl_point;
+ agent_saved_prompt = g_strdup(rl_prompt);
+
+ rl_set_prompt("");
+ rl_redisplay();
+
+ prompt = g_strdup_printf(AGENT_PROMPT "%s", msg);
+ rl_set_prompt(prompt);
+ g_free(prompt);
+
+ rl_replace_line("", 0);
+ rl_redisplay();
+}
+
+static void agent_release_prompt(void)
+{
+ if (!agent_saved_prompt)
+ return;
+
+ /* This will cause rl_expand_prompt to re-run over the last prompt, but
+ * our prompt doesn't expand anyway. */
+ rl_set_prompt(agent_saved_prompt);
+ rl_replace_line("", 0);
+ rl_point = agent_saved_point;
+ rl_redisplay();
+
+ g_free(agent_saved_prompt);
+ agent_saved_prompt = NULL;
+}
+
+bool agent_completion(void)
+{
+ if (pending_request.type == NONE)
+ return false;
+
+ return true;
+}
+
+static bool response_hexadecimal(const char *input)
+{
+ uint8_t buf[MAX_HEXADECIMAL_OOB_LEN];
+
+ if (!str2hex(input, strlen(input), buf, pending_request.len) ) {
+ rl_printf("Incorrect input: expecting %d hex octets\n",
+ pending_request.len);
+ return false;
+ }
+
+ if (pending_request.cb)
+ pending_request.cb(HEXADECIMAL, buf, pending_request.len,
+ pending_request.user_data);
+ return true;
+}
+
+static bool response_decimal(const char *input)
+{
+ uint8_t buf[DECIMAL_OOB_LEN];
+
+ if (strlen(input) > pending_request.len)
+ return false;
+
+ bt_put_be32(atoi(input), buf);
+
+ if (pending_request.cb)
+ pending_request.cb(DECIMAL, buf, DECIMAL_OOB_LEN,
+ pending_request.user_data);
+
+ return true;
+}
+
+static void response_ascii(const char *input)
+{
+ if (pending_request.cb)
+ pending_request.cb(ASCII, (uint8_t *) input, strlen(input),
+ pending_request.user_data);
+}
+
+bool agent_input(const char *input)
+{
+ bool repeat = false;
+
+ if (pending_request.type == NONE)
+ return false;
+
+ switch (pending_request.type) {
+ case HEXADECIMAL:
+ if (!response_hexadecimal(input))
+ repeat = true;
+ break;
+ case DECIMAL:
+ if (!response_decimal(input))
+ repeat = true;
+ break;
+ case ASCII:
+ response_ascii(input);
+ break;
+ case OUTPUT:
+ repeat = true;
+ case NONE:
+ default:
+ break;
+ };
+
+ if (!repeat) {
+ pending_request.type = NONE;
+ pending_request.len = 0;
+ pending_request.cb = NULL;
+ pending_request.user_data = NULL;
+
+ agent_release_prompt();
+ }
+
+ return true;
+}
+
+void agent_release(void)
+{
+ agent_release_prompt();
+}
+
+static bool request_hexadecimal(uint16_t len)
+{
+ if (len > MAX_HEXADECIMAL_OOB_LEN)
+ return false;
+
+ rl_printf("Request hexadecimal key (hex %d octets)\n", len);
+ agent_prompt("Enter key (hex number): ");
+
+ return true;
+}
+
+static uint32_t power_ten(uint8_t power)
+{
+ uint32_t ret = 1;
+
+ while (power--)
+ ret *= 10;
+
+ return ret;
+}
+
+static bool request_decimal(uint16_t len)
+{
+ rl_printf("Request decimal key (0 - %d)\n", power_ten(len) - 1);
+ agent_prompt("Enter Numeric key: ");
+
+ return true;
+}
+
+static bool request_ascii(uint16_t len)
+{
+ if (len != MAX_ASCII_OOB_LEN)
+ return false;
+
+ rl_printf("Request ASCII key (max characters %d)\n", len);
+ agent_prompt("Enter key (ascii string): ");
+
+ return true;
+}
+
+bool agent_input_request(oob_type_t type, uint16_t max_len, agent_input_cb cb,
+ void *user_data)
+{
+ bool result;
+
+ if (pending_request.type != NONE)
+ return FALSE;
+
+ switch (type) {
+ case HEXADECIMAL:
+ result = request_hexadecimal(max_len);
+ break;
+ case DECIMAL:
+ result = request_decimal(max_len);
+ break;
+ case ASCII:
+ result = request_ascii(max_len);
+ break;
+ case NONE:
+ case OUTPUT:
+ default:
+ return false;
+ };
+
+ if (result) {
+ pending_request.type = type;
+ pending_request.len = max_len;
+ pending_request.cb = cb;
+ pending_request.user_data = user_data;
+
+ return true;
+ }
+
+ return false;
+}
+
+bool agent_output_request(const char* str)
+{
+ if (pending_request.type != NONE)
+ return false;
+
+ pending_request.type = OUTPUT;
+ agent_prompt(str);
+ return true;
+}
+
+void agent_output_request_cancel(void)
+{
+ if (pending_request.type != OUTPUT)
+ return;
+ pending_request.type = NONE;
+ agent_release_prompt();
+}
diff --git a/mesh/config-client.c b/mesh/config-client.c
new file mode 100644
index 0000000..a0f6eee
--- /dev/null
+++ b/mesh/config-client.c
+/*
+ *
+ * BlueZ - Bluetooth protocol stack for Linux
+ *
+ * Copyright (C) 2017 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.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <errno.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <inttypes.h>
+#include <stdbool.h>
+#include <sys/uio.h>
+#include <wordexp.h>
+#include <readline/readline.h>
+#include <readline/history.h>
+#include <glib.h>
+
+#include "src/shared/util.h"
+#include "client/display.h"
+#include "mesh-net.h"
+#include "keys.h"
+#include "net.h"
+#include "node.h"
+#include "prov-db.h"
+#include "util.h"
+#include "config-model.h"
+
+#define MIN_COMPOSITION_LEN 16
+
+static bool client_msg_recvd(uint16_t src, uint8_t *data,
+ uint16_t len, void *user_data)
+{
+ uint32_t opcode;
+ struct mesh_node *node;
+ uint16_t app_idx, net_idx, addr;
+ uint32_t mod_id;
+ uint16_t primary;
+ uint16_t ele_addr;
+ uint8_t ele_idx;
+ struct mesh_publication pub;
+ int n;
+
+ if (mesh_opcode_get(data, len, &opcode, &n)) {
+ len -= n;
+ data += n;
+ } else
+ return false;
+
+ if (IS_UNICAST(src)) {
+ node = node_find_by_addr(src);
+ } else
+ node = NULL;
+
+ if (!node)
+ return false;
+
+ primary = node_get_primary(node);
+ if (primary != src)
+ return false;
+
+ switch (opcode & ~OP_UNRELIABLE) {
+ default:
+ return false;
+
+ case OP_DEV_COMP_STATUS:
+ if (len < MIN_COMPOSITION_LEN || !node)
+ break;
+ if (node_parse_composition(node, data, len)) {
+ if (!prov_db_add_node_composition(node, data, len))
+ break;
+ }
+
+ if (node_get_composition(node))
+ prov_db_print_node_composition(node);
+ break;
+
+ case OP_APPKEY_STATUS:
+ if (len != 4)
+ break;
+
+ rl_printf("Node %4.4x AppKey Status %s\n", src,
+ mesh_status_str(data[0]));
+ net_idx = get_le16(data + 1) & 0xfff;
+ app_idx = get_le16(data + 2) >> 4;
+
+ rl_printf("\tNetKey %3.3x, AppKey %3.3x\n", net_idx, app_idx);
+
+ if (data[0] != MESH_STATUS_SUCCESS &&
+ data[0] != MESH_STATUS_IDX_ALREADY_STORED &&
+ node_app_key_delete(node, net_idx, app_idx))
+ prov_db_node_keys(node, node_get_app_keys(node),
+ "appKeys");
+ break;
+
+ case OP_NETKEY_STATUS:
+ if (len != 3)
+ break;
+
+ rl_printf("Node %4.4x NetKey Status %s\n", src,
+ mesh_status_str(data[0]));
+ net_idx = get_le16(data + 1) & 0xfff;
+
+ rl_printf("\tNetKey %3.3x\n", net_idx);
+
+ if (data[0] != MESH_STATUS_SUCCESS &&
+ data[0] != MESH_STATUS_IDX_ALREADY_STORED &&
+ node_net_key_delete(node, net_idx))
+ prov_db_node_keys(node, node_get_net_keys(node),
+ "netKeys");
+ break;
+
+ case OP_MODEL_APP_STATUS:
+ if (len != 7 && len != 9)
+ break;
+
+ rl_printf("Node %4.4x Model App Status %s\n", src,
+ mesh_status_str(data[0]));
+ addr = get_le16(data + 1);
+ app_idx = get_le16(data + 3);
+
+ rl_printf("\tElement %4.4x AppIdx %3.3x\n ", addr, app_idx);
+
+ if (len == 7) {
+ mod_id = get_le16(data + 5);
+ rl_printf("ModelId %4.4x\n", mod_id);
+ mod_id = 0xffff0000 | mod_id;
+ } else {
+ mod_id = get_le16(data + 7);
+ rl_printf("ModelId %4.4x %4.4x\n", get_le16(data + 5),
+ mod_id);
+ mod_id = get_le16(data + 5) << 16 | mod_id;
+ }
+
+ if (data[0] == MESH_STATUS_SUCCESS &&
+ node_add_binding(node, addr - src, mod_id, app_idx))
+ prov_db_add_binding(node, addr - src, mod_id, app_idx);
+ break;
+
+ case OP_CONFIG_DEFAULT_TTL_STATUS:
+ if (len != 1)
+ return true;
+ rl_printf("Node %4.4x Default TTL %d\n", src, data[0]);
+ if (node_set_default_ttl (node, data[0]))
+ prov_db_node_set_ttl(node, data[0]);
+ break;
+
+ case OP_CONFIG_MODEL_PUB_STATUS:
+ if (len != 12 && len != 14)
+ return true;
+
+ rl_printf("\nSet publication for node %4.4x status: %s\n", src,
+ data[0] == MESH_STATUS_SUCCESS ? "Success" :
+ mesh_status_str(data[0]));
+
+ if (data[0] != MESH_STATUS_SUCCESS)
+ return true;
+
+ ele_addr = get_le16(data + 1);
+ mod_id = get_le16(data + 10);
+ if (len == 14)
+ mod_id = (mod_id << 16) | get_le16(data + 12);
+ else
+ mod_id |= 0xffff0000;
+
+ pub.u.addr16 = get_le16(data + 3);
+ pub.app_idx = get_le16(data + 5);
+ pub.ttl = data[7];
+ pub.period = data[8];
+ n = (data[8] & 0x3f);
+ switch (data[8] >> 6) {
+ case 0:
+ rl_printf("Period: %d ms\n", n * 100);
+ break;
+ case 2:
+ n *= 10;
+ /* fall through */
+ case 1:
+ rl_printf("Period: %d sec\n", n);
+ break;
+ case 3:
+ rl_printf("Period: %d min\n", n * 10);
+ break;
+ }
+
+ pub.retransmit = data[9];
+ rl_printf("Retransmit count: %d\n", data[9] >> 5);
+ rl_printf("Retransmit Interval Steps: %d\n", data[9] & 0x1f);
+
+ ele_idx = ele_addr - node_get_primary(node);
+
+ /* Local configuration is saved by server */
+ if (node == node_get_local_node())
+ break;
+
+ if (node_model_pub_set(node, ele_idx, mod_id, &pub))
+ prov_db_node_set_model_pub(node, ele_idx, mod_id,
+ node_model_pub_get(node, ele_idx, mod_id));
+ break;
+ }
+ return true;
+}
+
+static uint32_t target;
+static uint32_t parms[8];
+
+static uint32_t read_input_parameters(const char *args)
+{
+ uint32_t i;
+
+ if (!args)
+ return 0;
+
+ memset(parms, 0xff, sizeof(parms));
+
+ for (i = 0; i < sizeof(parms)/sizeof(parms[0]); i++) {
+ int n;
+
+ sscanf(args, "%x", &parms[i]);
+ if (parms[i] == 0xffffffff)
+ break;
+
+ n = strcspn(args, " \t");
+ args = args + n + strspn(args + n, " \t");
+ }
+
+ return i;
+}
+
+static void cmd_set_node(const char *args)
+{
+ uint32_t dst;
+ char *end;
+
+ dst = strtol(args, &end, 16);
+ if (end != (args + 4)) {
+ rl_printf("Bad unicast address %s: "
+ "expected format 4 digit hex\n", args);
+ target = UNASSIGNED_ADDRESS;
+ } else {
+ rl_printf("Configuring node %4.4x\n", dst);
+ target = dst;
+ set_menu_prompt("config", args);
+ }
+
+}
+
+static bool config_send(uint8_t *buf, uint16_t len)
+{
+ struct mesh_node *node = node_get_local_node();
+ uint16_t primary;
+
+ if(!node)
+ return false;
+
+ primary = node_get_primary(node);
+ if (target != primary)
+ return net_access_layer_send(DEFAULT_TTL, primary,
+ target, APP_IDX_DEV, buf, len);
+
+ node_local_data_handler(primary, target, node_get_iv_index(node),
+ node_get_sequence_number(node), APP_IDX_DEV,
+ buf, len);
+ return true;
+
+}
+
+static void cmd_get_composition(const char *args)
+{
+ uint16_t n;
+ uint8_t msg[32];
+ struct mesh_node *node;
+
+ if (IS_UNASSIGNED(target)) {
+ rl_printf("Destination not set\n");
+ return;
+ }
+
+ node = node_find_by_addr(target);
+
+ if (!node)
+ return;
+
+ n = mesh_opcode_set(OP_DEV_COMP_GET, msg);
+
+ /* By default, use page 0 */
+ msg[n++] = (read_input_parameters(args) == 1) ? parms[0] : 0;
+
+ if (!config_send(msg, n))
+ rl_printf("Failed to send \"GET NODE COMPOSITION\"\n");
+}
+
+static void cmd_net_key(const char *args, uint32_t opcode)
+{
+ uint16_t n;
+ uint8_t msg[32];
+ uint16_t net_idx;
+ uint8_t *key;
+ struct mesh_node *node;
+
+ if (IS_UNASSIGNED(target)) {
+ rl_printf("Destination not set\n");
+ return;
+ }
+
+ n = mesh_opcode_set(opcode, msg);
+
+ if (read_input_parameters(args) != 1) {
+ rl_printf("Bad arguments %s\n", args);
+ return;
+ }
+
+ node = node_find_by_addr(target);
+ if (!node) {
+ rl_printf("Node %4.4x\n not found", target);
+ return;
+ }
+
+ net_idx = parms[0];
+
+ if (opcode != OP_NETKEY_DELETE) {
+
+ key = keys_net_key_get(net_idx, true);
+ if (!key) {
+ rl_printf("Network key with index %4.4x not found\n",
+ net_idx);
+ return;
+ }
+
+ put_le16(net_idx, &msg[n]);
+ n += 2;
+
+ memcpy(msg + n, key, 16);
+ n += 16;
+ }
+
+ if (!config_send(msg, n)) {
+ rl_printf("Failed to send \"%s NET KEY\"\n",
+ opcode == OP_NETKEY_ADD ? "ADD" : "DEL");
+ return;
+ }
+
+ if (opcode != OP_NETKEY_DELETE) {
+ if (node_net_key_add(node, net_idx))
+ prov_db_node_keys(node, node_get_net_keys(node),
+ "netKeys");
+ } else {
+ if (node_net_key_delete(node, net_idx))
+ prov_db_node_keys(node, node_get_net_keys(node),
+ "netKeys");
+ }
+
+}
+
+static void cmd_add_net_key(const char *args)
+{
+ cmd_net_key(args, OP_NETKEY_ADD);
+}
+
+static void cmd_del_net_key(const char *args)
+{
+ cmd_net_key(args, OP_NETKEY_DELETE);
+}
+
+static void cmd_app_key(const char *args, uint32_t opcode)
+{
+ uint16_t n;
+ uint8_t msg[32];
+ uint16_t net_idx;
+ uint16_t app_idx;
+ uint8_t *key;
+ struct mesh_node *node;
+
+ if (IS_UNASSIGNED(target)) {
+ rl_printf("Destination not set\n");
+ return;
+ }
+
+ if (read_input_parameters(args) != 1) {
+ rl_printf("Bad arguments %s\n", args);
+ return;
+ }
+
+ node = node_find_by_addr(target);
+ if (!node) {
+ rl_printf("Node %4.4x\n not found", target);
+ return;
+ }
+
+ n = mesh_opcode_set(opcode, msg);
+
+ app_idx = parms[0];
+ net_idx = keys_app_key_get_bound(app_idx);
+ if (net_idx == NET_IDX_INVALID) {
+ rl_printf("App key with index %4.4x not found\n", app_idx);
+ return;
+ }
+
+ msg[n++] = net_idx & 0xf;
+ msg[n++] = ((net_idx >> 8) & 0xf) |
+ ((app_idx << 4) & 0xf0);
+ msg[n++] = app_idx >> 4;
+
+ if (opcode != OP_APPKEY_DELETE) {
+ key = keys_app_key_get(app_idx, true);
+ if (!key) {
+ rl_printf("App key %4.4x not found\n", net_idx);
+ return;
+ }
+
+ memcpy(msg + n, key, 16);
+ n += 16;
+ }
+
+ if (!config_send(msg, n)) {
+ rl_printf("Failed to send \"ADD %s KEY\"\n",
+ opcode == OP_APPKEY_ADD ? "ADD" : "DEL");
+ return;
+ }
+
+ if (opcode != OP_APPKEY_DELETE) {
+ if (node_app_key_add(node, app_idx))
+ prov_db_node_keys(node, node_get_app_keys(node),
+ "appKeys");
+ } else {
+ if (node_app_key_delete(node, net_idx, app_idx))
+ prov_db_node_keys(node, node_get_app_keys(node),
+ "appKeys");
+ }
+}
+
+static void cmd_add_app_key(const char *args)
+{
+ cmd_app_key(args, OP_APPKEY_ADD);
+}
+
+static void cmd_del_app_key(const char *args)
+{
+ cmd_app_key(args, OP_APPKEY_DELETE);
+}
+
+static void cmd_bind(const char *args)
+{
+ uint16_t n;
+ uint8_t msg[32];
+ int parm_cnt;
+
+ if (IS_UNASSIGNED(target)) {
+ rl_printf("Destination not set\n");
+ return;
+ }
+
+ parm_cnt = read_input_parameters(args);
+ if (parm_cnt != 3 && parm_cnt != 4) {
+ rl_printf("Bad arguments %s\n", args);
+ return;
+ }
+
+ n = mesh_opcode_set(OP_MODEL_APP_BIND, msg);
+
+ put_le16(target + parms[0], msg + n);
+ n += 2;
+ put_le16(parms[1], msg + n);
+ n += 2;
+ if (parm_cnt == 4) {
+ put_le16(parms[3], msg + n);
+ put_le16(parms[2], msg + n + 2);
+ n += 4;
+ } else {
+ put_le16(parms[2], msg + n);
+ n += 2;
+ }
+
+ if (!config_send(msg, n))
+ rl_printf("Failed to send \"MODEL APP BIND\"\n");
+}
+
+static void cmd_set_ttl(const char *args)
+{
+ uint16_t n;
+ uint8_t msg[32];
+ int parm_cnt;
+ uint8_t ttl;
+
+ if (IS_UNASSIGNED(target)) {
+ rl_printf("Destination not set\n");
+ return;
+ }
+
+ n = mesh_opcode_set(OP_CONFIG_DEFAULT_TTL_SET, msg);
+
+ parm_cnt = read_input_parameters(args);
+ if (parm_cnt) {
+ ttl = parms[0] & TTL_MASK;
+ } else
+ ttl = node_get_default_ttl(node_get_local_node());
+
+ msg[n++] = ttl;
+
+ if (!config_send(msg, n))
+ rl_printf("Failed to send \"SET_DEFAULT TTL\"\n");
+}
+
+static void cmd_set_pub(const char *args)
+{
+ uint16_t n;
+ uint8_t msg[32];
+ int parm_cnt;
+
+ if (IS_UNASSIGNED(target)) {
+ rl_printf("Destination not set\n");
+ return;
+ }
+
+ n = mesh_opcode_set(OP_CONFIG_MODEL_PUB_SET, msg);
+
+ parm_cnt = read_input_parameters(args);
+ if (parm_cnt != 5) {
+ rl_printf("Bad arguments: %s\n", args);
+ return;
+ }
+
+ put_le16(parms[0], msg + n);
+ n += 2;
+ /* Publish address */
+ put_le16(parms[1], msg + n);
+ n += 2;
+ /* App key index + credential (set to 0) */
+ put_le16(parms[2], msg + n);
+ n += 2;
+ /* TTL */
+ msg[n++] = DEFAULT_TTL;
+ /* Publish period step count and step resolution */
+ msg[n++] = parms[3];
+ /* Publish retransmit count & interval steps */
+ msg[n++] = (1 << 5) + 2;
+ /* Model Id */
+ if (parms[4] > 0xffff) {
+ put_le16(parms[4] >> 16, msg + n);
+ put_le16(parms[4], msg + n + 2);
+ n += 4;
+ } else {
+ put_le16(parms[4], msg + n);
+ n += 2;
+ }
+
+ if (!config_send(msg, n))
+ rl_printf("Failed to send \"SET MODEL PUBLICATION\"\n");
+}
+
+static void cmd_default(uint32_t opcode)
+{
+ uint16_t n;
+ uint8_t msg[32];
+
+ if (IS_UNASSIGNED(target)) {
+ rl_printf("Destination not set\n");
+ return;
+ }
+
+ n = mesh_opcode_set(opcode, msg);
+
+ if (!config_send(msg, n))
+ rl_printf("Failed to send command (opcode 0x%x)\n", opcode);
+}
+
+static void cmd_get_ttl(const char *args)
+{
+ cmd_default(OP_CONFIG_DEFAULT_TTL_GET);
+}
+
+static void cmd_back(const char *args)
+{
+ cmd_menu_main(false);
+}
+
+static void cmd_help(const char *args);
+
+static const struct menu_entry cfg_menu[] = {
+ {"target", "<unicast>", cmd_set_node,
+ "Set target node to configure"},
+ {"get-composition", "[<page_num>]", cmd_get_composition,
+ "Get Composition Data"},
+ {"add-netkey", "<net_idx>", cmd_add_net_key,
+ "Add network key"},
+ {"del-netkey", "<net_idx>", cmd_del_net_key,
+ "Delete network key"},
+ {"add-appkey", "<app_idx>", cmd_add_app_key,
+ "Add application key"},
+ {"del-appkey", "<app_idx>", cmd_del_app_key,
+ "Delete application key"},
+ {"bind", "<ele_idx> <app_idx> <mod_id> [cid]",
+ cmd_bind, "Bind app key to a model"},
+ {"set-ttl", "<ttl>", cmd_set_ttl,
+ "Set default TTL"},
+ {"get-ttl", NULL, cmd_get_ttl,
+ "Get default TTL"},
+ {"set-pub", "<ele_addr> <pub_addr> <app_idx> "
+ "<period (step|res)> <model>",
+ cmd_set_pub, "Set publication"},
+ {"back", NULL, cmd_back,
+ "Back to main menu"},
+ {"help", NULL, cmd_help,
+ "Config Commands"},
+ {}
+};
+
+static void cmd_help(const char *args)
+{
+ rl_printf("Client Configuration Menu\n");
+ print_cmd_menu(cfg_menu);
+}
+
+void config_set_node(const char *args)
+{
+ cmd_set_node(args);
+}
+
+void config_client_get_composition(uint32_t dst)
+{
+ uint32_t tmp = target;
+
+ target = dst;
+ cmd_get_composition("");
+ target = tmp;
+}
+
+static struct mesh_model_ops client_cbs = {
+ client_msg_recvd,
+ NULL,
+ NULL,
+ NULL
+};
+
+bool config_client_init(void)
+{
+ if (!node_local_model_register(PRIMARY_ELEMENT_IDX,
+ CONFIG_CLIENT_MODEL_ID,
+ &client_cbs, NULL))
+ return false;
+
+ add_cmd_menu("configure", cfg_menu);
+
+ return true;
+}
diff --git a/mesh/config-server.c b/mesh/config-server.c
new file mode 100644
index 0000000..14e5d7b
--- /dev/null
+++ b/mesh/config-server.c
+/*
+ *
+ * BlueZ - Bluetooth protocol stack for Linux
+ *
+ * Copyright (C) 2017 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.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <errno.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <inttypes.h>
+#include <stdbool.h>
+#include <sys/uio.h>
+#include <wordexp.h>
+#include <readline/readline.h>
+#include <readline/history.h>
+#include <glib.h>
+
+#include "src/shared/util.h"
+#include "client/display.h"
+#include "mesh-net.h"
+#include "keys.h"
+#include "net.h"
+#include "node.h"
+#include "prov-db.h"
+#include "util.h"
+#include "config-model.h"
+
+static bool server_msg_recvd(uint16_t src, uint8_t *data,
+ uint16_t len, void *user_data)
+{
+ uint32_t opcode;
+ uint8_t msg[32];
+ struct mesh_node *node;
+ uint16_t primary;
+ uint32_t mod_id;
+ uint16_t ele_addr;
+ uint8_t ele_idx;
+ struct mesh_publication pub;
+
+ int n;
+
+ if (mesh_opcode_get(data, len, &opcode, &n)) {
+ len -= n;
+ data += n;
+ } else
+ return false;
+
+ node = node_get_local_node();
+
+ if (!node)
+ return true;
+
+ switch (opcode & ~OP_UNRELIABLE) {
+ default:
+ return false;
+
+ case OP_CONFIG_DEFAULT_TTL_SET:
+ if (len != 1 || data[0] > TTL_MASK || data[0] == 1)
+ return true;
+
+ if (data[0] <= TTL_MASK) {
+ node_set_default_ttl(node, data[0]);
+ prov_db_node_set_ttl(node, data[0]);
+ }
+
+ /* Fall Through */
+
+ case OP_CONFIG_DEFAULT_TTL_GET:
+ n = mesh_opcode_set(OP_CONFIG_DEFAULT_TTL_STATUS, msg);
+ msg[n++] = node_get_default_ttl(node);
+ break;
+
+ case OP_CONFIG_MODEL_PUB_SET:
+ if (len != 11 && len != 13)
+ return true;
+
+ rl_printf("Set publication\n");
+
+ ele_addr = get_le16(data);
+ mod_id = get_le16(data + 9);
+ if (len == 14)
+ mod_id = (mod_id << 16) | get_le16(data + 11);
+ else
+ mod_id |= 0xffff0000;
+
+ pub.u.addr16 = get_le16(data + 2);
+ pub.app_idx = get_le16(data + 4);
+ pub.ttl = data[6];
+ pub.period = data[7];
+ n = (data[7] & 0x3f);
+ switch (data[7] >> 6) {
+ case 0:
+ rl_printf("Period: %d ms\n", n * 100);
+ break;
+ case 2:
+ n *= 10;
+ /* fall through */
+ case 1:
+ rl_printf("Period: %d sec\n", n);
+ break;
+ case 3:
+ rl_printf("Period: %d min\n", n * 10);
+ break;
+ }
+
+ pub.retransmit = data[8];
+ rl_printf("Retransmit count: %d\n", data[8] >> 5);
+ rl_printf("Retransmit Interval Steps: %d\n", data[8] & 0x1f);
+
+ ele_idx = ele_addr - node_get_primary(node);
+
+ if (node_model_pub_set(node, ele_idx, mod_id, &pub)) {
+ prov_db_node_set_model_pub(node, ele_idx, mod_id,
+ node_model_pub_get(node, ele_idx, mod_id));
+ }
+ break;
+ }
+
+ primary = node_get_primary(node);
+ if (src != primary)
+ net_access_layer_send(node_get_default_ttl(node), primary,
+ src, APP_IDX_DEV, msg, len);
+ return true;
+}
+
+
+static struct mesh_model_ops server_cbs = {
+ server_msg_recvd,
+ NULL,
+ NULL,
+ NULL
+};
+
+bool config_server_init(void)
+{
+ if (!node_local_model_register(PRIMARY_ELEMENT_IDX,
+ CONFIG_SERVER_MODEL_ID,
+ &server_cbs, NULL))
+ return false;
+
+ return true;
+}
diff --git a/mesh/crypto.c b/mesh/crypto.c
new file mode 100644
index 0000000..189624e
--- /dev/null
+++ b/mesh/crypto.c
+/*
+ *
+ * BlueZ - Bluetooth protocol stack for Linux
+ *
+ * Copyright (C) 2017 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.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <fcntl.h>
+#include <unistd.h>
+#include <string.h>
+#include <sys/socket.h>
+
+#include <linux/if_alg.h>
+
+#include <glib.h>
+
+#ifndef SOL_ALG
+#define SOL_ALG 279
+#endif
+
+#ifndef ALG_SET_AEAD_AUTHSIZE
+#define ALG_SET_AEAD_AUTHSIZE 5
+#endif
+
+#include "src/shared/util.h"
+#include "mesh-net.h"
+#include "crypto.h"
+
+static int alg_new(int fd, const void *keyval, socklen_t keylen,
+ size_t mic_size)
+{
+ if (setsockopt(fd, SOL_ALG, ALG_SET_KEY, keyval, keylen) < 0) {
+ g_printerr("key");
+ return -1;
+ }
+
+ if (mic_size &&
+ setsockopt(fd, SOL_ALG,
+ ALG_SET_AEAD_AUTHSIZE, NULL, mic_size) < 0) {
+ g_printerr("taglen");
+ return -1;
+ }
+
+ /* FIXME: This should use accept4() with SOCK_CLOEXEC */
+ return accept(fd, NULL, 0);
+}
+
+static bool alg_encrypt(int fd, const void *inbuf, size_t inlen,
+ void *outbuf, size_t outlen)
+{
+ __u32 alg_op = ALG_OP_ENCRYPT;
+ char cbuf[CMSG_SPACE(sizeof(alg_op))];
+ struct cmsghdr *cmsg;
+ struct msghdr msg;
+ struct iovec iov;
+ ssize_t len;
+
+ memset(cbuf, 0, sizeof(cbuf));
+ memset(&msg, 0, sizeof(msg));
+
+ msg.msg_control = cbuf;
+ msg.msg_controllen = sizeof(cbuf);
+
+ cmsg = CMSG_FIRSTHDR(&msg);
+ cmsg->cmsg_level = SOL_ALG;
+ cmsg->cmsg_type = ALG_SET_OP;
+ cmsg->cmsg_len = CMSG_LEN(sizeof(alg_op));
+ memcpy(CMSG_DATA(cmsg), &alg_op, sizeof(alg_op));
+
+ iov.iov_base = (void *) inbuf;
+ iov.iov_len = inlen;
+
+ msg.msg_iov = &iov;
+ msg.msg_iovlen = 1;
+
+ len = sendmsg(fd, &msg, 0);
+ if (len < 0)
+ return false;
+
+ len = read(fd, outbuf, outlen);
+ if (len < 0)
+ return false;
+
+ return true;
+}
+
+static int aes_ecb_setup(const uint8_t key[16])
+{
+ struct sockaddr_alg salg;
+ int fd, nfd;
+
+ fd = socket(PF_ALG, SOCK_SEQPACKET | SOCK_CLOEXEC, 0);
+ if (fd < 0)
+ return -1;
+
+ memset(&salg, 0, sizeof(salg));
+ salg.salg_family = AF_ALG;
+ strcpy((char *) salg.salg_type, "skcipher");
+ strcpy((char *) salg.salg_name, "ecb(aes)");
+
+ if (bind(fd, (struct sockaddr *) &salg, sizeof(salg)) < 0) {
+ close(fd);
+ return -1;
+ }
+
+ nfd = alg_new(fd, key, 16, 0);
+
+ close(fd);
+
+ return nfd;
+}
+
+static bool aes_ecb(int fd, const uint8_t plaintext[16], uint8_t encrypted[16])
+{
+ return alg_encrypt(fd, plaintext, 16, encrypted, 16);
+}
+
+static void aes_ecb_destroy(int fd)
+{
+ close(fd);
+}
+
+static bool aes_ecb_one(const uint8_t key[16],
+ const uint8_t plaintext[16], uint8_t encrypted[16])
+{
+ bool result;
+ int fd;
+
+ fd = aes_ecb_setup(key);
+ if (fd < 0)
+ return false;
+
+ result = aes_ecb(fd, plaintext, encrypted);
+
+ aes_ecb_destroy(fd);
+
+ return result;
+}
+
+/* Maximum message length that can be passed to aes_cmac */
+#define CMAC_MSG_MAX (64 + 64 + 17)
+
+static int aes_cmac_setup(const uint8_t key[16])
+{
+ struct sockaddr_alg salg;
+ int fd, nfd;
+
+ fd = socket(PF_ALG, SOCK_SEQPACKET | SOCK_CLOEXEC, 0);
+ if (fd < 0)
+ return -1;
+
+ memset(&salg, 0, sizeof(salg));
+ salg.salg_family = AF_ALG;
+ strcpy((char *) salg.salg_type, "hash");
+ strcpy((char *) salg.salg_name, "cmac(aes)");
+
+ if (bind(fd, (struct sockaddr *) &salg, sizeof(salg)) < 0) {
+ close(fd);
+ return -1;
+ }
+
+ nfd = alg_new(fd, key, 16, 0);
+
+ close(fd);
+
+ return nfd;
+}
+
+static bool aes_cmac(int fd, const uint8_t *msg,
+ size_t msg_len, uint8_t res[16])
+{
+ ssize_t len;
+
+ if (msg_len > CMAC_MSG_MAX)
+ return false;
+
+ len = send(fd, msg, msg_len, 0);
+ if (len < 0)
+ return false;
+
+ len = read(fd, res, 16);
+ if (len < 0)
+ return false;
+
+ return true;
+}
+
+static void aes_cmac_destroy(int fd)
+{
+ close(fd);
+}
+
+static int aes_cmac_N_start(const uint8_t N[16])
+{
+ int fd;
+
+ fd = aes_cmac_setup(N);
+ return fd;
+}
+
+static bool aes_cmac_one(const uint8_t key[16], const void *msg,
+ size_t msg_len, uint8_t res[16])
+{
+ bool result;
+ int fd;
+
+ fd = aes_cmac_setup(key);
+ if (fd < 0)
+ return false;
+
+ result = aes_cmac(fd, msg, msg_len, res);
+
+ aes_cmac_destroy(fd);
+
+ return result;
+}
+
+bool mesh_crypto_aes_cmac(const uint8_t key[16], const uint8_t *msg,
+ size_t msg_len, uint8_t res[16])
+{
+ return aes_cmac_one(key, msg, msg_len, res);
+}
+
+bool mesh_crypto_aes_ccm_encrypt(const uint8_t nonce[13], const uint8_t key[16],
+ const uint8_t *aad, uint16_t aad_len,
+ const uint8_t *msg, uint16_t msg_len,
+ uint8_t *out_msg, void *out_mic,
+ size_t mic_size)
+{
+ uint8_t pmsg[16], cmic[16], cmsg[16];
+ uint8_t mic[16], Xn[16];
+ uint16_t blk_cnt, last_blk;
+ bool result;
+ size_t i, j;
+ int fd;
+
+ if (aad_len >= 0xff00) {
+ g_printerr("Unsupported AAD size");
+ return false;
+ }
+
+ fd = aes_ecb_setup(key);
+ if (fd < 0)
+ return false;
+
+ /* C_mic = e(AppKey, 0x01 || nonce || 0x0000) */
+ pmsg[0] = 0x01;
+ memcpy(pmsg + 1, nonce, 13);
+ put_be16(0x0000, pmsg + 14);
+
+ result = aes_ecb(fd, pmsg, cmic);
+ if (!result)
+ goto done;
+
+ /* X_0 = e(AppKey, 0x09 || nonce || length) */
+ if (mic_size == sizeof(uint64_t))
+ pmsg[0] = 0x19 | (aad_len ? 0x40 : 0x00);
+ else
+ pmsg[0] = 0x09 | (aad_len ? 0x40 : 0x00);
+
+ memcpy(pmsg + 1, nonce, 13);
+ put_be16(msg_len, pmsg + 14);
+
+ result = aes_ecb(fd, pmsg, Xn);
+ if (!result)
+ goto done;
+
+ /* If AAD is being used to authenticate, include it here */
+ if (aad_len) {
+ put_be16(aad_len, pmsg);
+
+ for (i = 0; i < sizeof(uint16_t); i++)
+ pmsg[i] = Xn[i] ^ pmsg[i];
+
+ j = 0;
+ aad_len += sizeof(uint16_t);
+ while (aad_len > 16) {
+ do {
+ pmsg[i] = Xn[i] ^ aad[j];
+ i++, j++;
+ } while (i < 16);
+
+ aad_len -= 16;
+ i = 0;
+
+ result = aes_ecb(fd, pmsg, Xn);
+ if (!result)
+ goto done;
+ }
+
+ for (i = 0; i < aad_len; i++, j++)
+ pmsg[i] = Xn[i] ^ aad[j];
+
+ for (i = aad_len; i < 16; i++)
+ pmsg[i] = Xn[i];
+
+ result = aes_ecb(fd, pmsg, Xn);
+ if (!result)
+ goto done;
+ }
+
+ last_blk = msg_len % 16;
+ blk_cnt = (msg_len + 15) / 16;
+ if (!last_blk)
+ last_blk = 16;
+
+ for (j = 0; j < blk_cnt; j++) {
+ if (j + 1 == blk_cnt) {
+ /* X_1 = e(AppKey, X_0 ^ Payload[0-15]) */
+ for (i = 0; i < last_blk; i++)
+ pmsg[i] = Xn[i] ^ msg[(j * 16) + i];
+ for (i = last_blk; i < 16; i++)
+ pmsg[i] = Xn[i] ^ 0x00;
+
+ result = aes_ecb(fd, pmsg, Xn);
+ if (!result)
+ goto done;
+
+ /* MIC = C_mic ^ X_1 */
+ for (i = 0; i < sizeof(mic); i++)
+ mic[i] = cmic[i] ^ Xn[i];
+
+ /* C_1 = e(AppKey, 0x01 || nonce || 0x0001) */
+ pmsg[0] = 0x01;
+ memcpy(pmsg + 1, nonce, 13);
+ put_be16(j + 1, pmsg + 14);
+
+ result = aes_ecb(fd, pmsg, cmsg);
+ if (!result)
+ goto done;
+
+ if (out_msg) {
+ /* Encrypted = Payload[0-15] ^ C_1 */
+ for (i = 0; i < last_blk; i++)
+ out_msg[(j * 16) + i] =
+ msg[(j * 16) + i] ^ cmsg[i];
+
+ }
+ } else {
+ /* X_1 = e(AppKey, X_0 ^ Payload[0-15]) */
+ for (i = 0; i < 16; i++)
+ pmsg[i] = Xn[i] ^ msg[(j * 16) + i];
+
+ result = aes_ecb(fd, pmsg, Xn);
+ if (!result)
+ goto done;
+
+ /* C_1 = e(AppKey, 0x01 || nonce || 0x0001) */
+ pmsg[0] = 0x01;
+ memcpy(pmsg + 1, nonce, 13);
+ put_be16(j + 1, pmsg + 14);
+
+ result = aes_ecb(fd, pmsg, cmsg);
+ if (!result)
+ goto done;
+
+ if (out_msg) {
+ /* Encrypted = Payload[0-15] ^ C_N */
+ for (i = 0; i < 16; i++)
+ out_msg[(j * 16) + i] =
+ msg[(j * 16) + i] ^ cmsg[i];
+ }
+
+ }
+ }
+
+ if (out_msg)
+ memcpy(out_msg + msg_len, mic, mic_size);
+
+ if (out_mic) {
+ switch (mic_size) {
+ case sizeof(uint32_t):
+ *(uint32_t *)out_mic = get_be32(mic);
+ break;
+ case sizeof(uint64_t):
+ *(uint64_t *)out_mic = get_be64(mic);
+ break;
+ default:
+ g_printerr("Unsupported MIC size");
+ }
+ }
+
+done:
+ aes_ecb_destroy(fd);
+
+ return result;
+}
+
+bool mesh_crypto_aes_ccm_decrypt(const uint8_t nonce[13], const uint8_t key[16],
+ const uint8_t *aad, uint16_t aad_len,
+ const uint8_t *enc_msg, uint16_t enc_msg_len,
+ uint8_t *out_msg, void *out_mic,
+ size_t mic_size)
+{
+ uint8_t msg[16], pmsg[16], cmic[16], cmsg[16], Xn[16];
+ uint8_t mic[16];
+ uint16_t msg_len = enc_msg_len - mic_size;
+ uint16_t last_blk, blk_cnt;
+ bool result;
+ size_t i, j;
+ int fd;
+
+ if (enc_msg_len < 5 || aad_len >= 0xff00)
+ return false;
+
+ fd = aes_ecb_setup(key);
+ if (fd < 0)
+ return false;
+
+ /* C_mic = e(AppKey, 0x01 || nonce || 0x0000) */
+ pmsg[0] = 0x01;
+ memcpy(pmsg + 1, nonce, 13);
+ put_be16(0x0000, pmsg + 14);
+
+ result = aes_ecb(fd, pmsg, cmic);
+ if (!result)
+ goto done;
+
+ /* X_0 = e(AppKey, 0x09 || nonce || length) */
+ if (mic_size == sizeof(uint64_t))
+ pmsg[0] = 0x19 | (aad_len ? 0x40 : 0x00);
+ else
+ pmsg[0] = 0x09 | (aad_len ? 0x40 : 0x00);
+
+ memcpy(pmsg + 1, nonce, 13);
+ put_be16(msg_len, pmsg + 14);
+
+ result = aes_ecb(fd, pmsg, Xn);
+ if (!result)
+ goto done;
+
+ /* If AAD is being used to authenticate, include it here */
+ if (aad_len) {
+ put_be16(aad_len, pmsg);
+
+ for (i = 0; i < sizeof(uint16_t); i++)
+ pmsg[i] = Xn[i] ^ pmsg[i];
+
+ j = 0;
+ aad_len += sizeof(uint16_t);
+ while (aad_len > 16) {
+ do {
+ pmsg[i] = Xn[i] ^ aad[j];
+ i++, j++;
+ } while (i < 16);
+
+ aad_len -= 16;
+ i = 0;
+
+ result = aes_ecb(fd, pmsg, Xn);
+ if (!result)
+ goto done;
+ }
+
+ for (i = 0; i < aad_len; i++, j++)
+ pmsg[i] = Xn[i] ^ aad[j];
+
+ for (i = aad_len; i < 16; i++)
+ pmsg[i] = Xn[i];
+
+ result = aes_ecb(fd, pmsg, Xn);
+ if (!result)
+ goto done;
+ }
+
+ last_blk = msg_len % 16;
+ blk_cnt = (msg_len + 15) / 16;
+ if (!last_blk)
+ last_blk = 16;
+
+ for (j = 0; j < blk_cnt; j++) {
+ if (j + 1 == blk_cnt) {
+ /* C_1 = e(AppKey, 0x01 || nonce || 0x0001) */
+ pmsg[0] = 0x01;
+ memcpy(pmsg + 1, nonce, 13);
+ put_be16(j + 1, pmsg + 14);
+
+ result = aes_ecb(fd, pmsg, cmsg);
+ if (!result)
+ goto done;
+
+ /* Encrypted = Payload[0-15] ^ C_1 */
+ for (i = 0; i < last_blk; i++)
+ msg[i] = enc_msg[(j * 16) + i] ^ cmsg[i];
+
+ if (out_msg)
+ memcpy(out_msg + (j * 16), msg, last_blk);
+
+ /* X_1 = e(AppKey, X_0 ^ Payload[0-15]) */
+ for (i = 0; i < last_blk; i++)
+ pmsg[i] = Xn[i] ^ msg[i];
+ for (i = last_blk; i < 16; i++)
+ pmsg[i] = Xn[i] ^ 0x00;
+
+ result = aes_ecb(fd, pmsg, Xn);
+ if (!result)
+ goto done;
+
+ /* MIC = C_mic ^ X_1 */
+ for (i = 0; i < sizeof(mic); i++)
+ mic[i] = cmic[i] ^ Xn[i];
+ } else {
+ /* C_1 = e(AppKey, 0x01 || nonce || 0x0001) */
+ pmsg[0] = 0x01;
+ memcpy(pmsg + 1, nonce, 13);
+ put_be16(j + 1, pmsg + 14);
+
+ result = aes_ecb(fd, pmsg, cmsg);
+ if (!result)
+ goto done;
+
+ /* Encrypted = Payload[0-15] ^ C_1 */
+ for (i = 0; i < 16; i++)
+ msg[i] = enc_msg[(j * 16) + i] ^ cmsg[i];
+
+ if (out_msg)
+ memcpy(out_msg + (j * 16), msg, 16);
+
+ /* X_1 = e(AppKey, X_0 ^ Payload[0-15]) */
+ for (i = 0; i < 16; i++)
+ pmsg[i] = Xn[i] ^ msg[i];
+
+ result = aes_ecb(fd, pmsg, Xn);
+ if (!result)
+ goto done;
+ }
+ }
+
+ switch (mic_size) {
+ case sizeof(uint32_t):
+ if (out_mic)
+ *(uint32_t *)out_mic = get_be32(mic);
+ else if (get_be32(enc_msg + enc_msg_len - mic_size) !=
+ get_be32(mic))
+ result = false;
+ break;
+
+ case sizeof(uint64_t):
+ if (out_mic)
+ *(uint64_t *)out_mic = get_be64(mic);
+ else if (get_be64(enc_msg + enc_msg_len - mic_size) !=
+ get_be64(mic))
+ result = false;
+ break;
+
+ default:
+ g_printerr("Unsupported MIC size");
+ result = false;
+ }
+
+done:
+ aes_ecb_destroy(fd);
+
+ return result;
+}
+
+bool mesh_crypto_k1(const uint8_t ikm[16], const uint8_t salt[16],
+ const void *info, size_t info_len, uint8_t okm[16])
+{
+ uint8_t res[16];
+
+ if (!aes_cmac_one(salt, ikm, 16, res))
+ return false;
+
+ return aes_cmac_one(res, info, info_len, okm);
+}
+
+bool mesh_crypto_k2(const uint8_t n[16], const uint8_t *p, size_t p_len,
+ uint8_t net_id[1],
+ uint8_t enc_key[16],
+ uint8_t priv_key[16])
+{
+ int fd;
+ uint8_t output[16];
+ uint8_t t[16];
+ uint8_t *stage;
+ bool success = false;
+
+ stage = g_malloc(sizeof(output) + p_len + 1);
+ if (stage == NULL)
+ return false;
+
+ if (!mesh_crypto_s1("smk2", 4, stage))
+ goto fail;
+
+ if (!aes_cmac_one(stage, n, 16, t))
+ goto fail;
+
+ fd = aes_cmac_N_start(t);
+ if (fd < 0)
+ goto fail;
+
+ memcpy(stage, p, p_len);
+ stage[p_len] = 1;
+
+ if(!aes_cmac(fd, stage, p_len + 1, output))
+ goto done;
+
+ net_id[0] = output[15] & 0x7f;
+
+ memcpy(stage, output, 16);
+ memcpy(stage + 16, p, p_len);
+ stage[p_len + 16] = 2;
+
+ if(!aes_cmac(fd, stage, p_len + 16 + 1, output))
+ goto done;
+
+ memcpy(enc_key, output, 16);
+
+ memcpy(stage, output, 16);
+ memcpy(stage + 16, p, p_len);
+ stage[p_len + 16] = 3;
+
+ if(!aes_cmac(fd, stage, p_len + 16 + 1, output))
+ goto done;
+
+ memcpy(priv_key, output, 16);
+ success = true;
+
+done:
+ aes_cmac_destroy(fd);
+fail:
+ g_free(stage);
+
+ return success;
+}
+
+static bool crypto_128(const uint8_t n[16], const char *s, uint8_t out128[16])
+{
+ uint8_t id128[] = { 'i', 'd', '1', '2', '8', 0x01 };
+ uint8_t salt[16];
+
+ if (!mesh_crypto_s1(s, 4, salt))
+ return false;
+
+ return mesh_crypto_k1(n, salt, id128, sizeof(id128), out128);
+}
+
+bool mesh_crypto_nkik(const uint8_t n[16], uint8_t identity_key[16])
+{
+ return crypto_128(n, "nkik", identity_key);
+}
+
+static bool identity_calc(const uint8_t net_key[16], uint16_t addr,
+ bool check, uint8_t id[16])
+{
+ uint8_t id_key[16];
+ uint8_t tmp[16];
+
+ if (!mesh_crypto_nkik(net_key, id_key))
+ return false;
+
+ memset(tmp, 0, sizeof(tmp));
+ put_be16(addr, tmp + 14);
+
+ if (check) {
+ memcpy(tmp + 6, id + 8, 8);
+ } else {
+ mesh_get_random_bytes(tmp + 6, 8);
+ memcpy(id + 8, tmp + 6, 8);
+ }
+
+ if (!aes_ecb_one(id_key, tmp, tmp))
+ return false;
+
+ if (check)
+ return (memcmp(id, tmp + 8, 8) == 0);
+
+ memcpy(id, tmp + 8, 8);
+ return true;
+}
+
+bool mesh_crypto_identity(const uint8_t net_key[16], uint16_t addr,
+ uint8_t id[16])
+{
+ return identity_calc(net_key, addr, false, id);
+}
+
+bool mesh_crypto_identity_check(const uint8_t net_key[16], uint16_t addr,
+ uint8_t id[16])
+{
+ return identity_calc(net_key, addr, true, id);
+}
+
+bool mesh_crypto_nkbk(const uint8_t n[16], uint8_t beacon_key[16])
+{
+ return crypto_128(n, "nkbk", beacon_key);
+}
+
+bool mesh_crypto_k3(const uint8_t n[16], uint8_t out64[8])
+{
+ uint8_t tmp[16];
+ uint8_t t[16];
+ uint8_t id64[] = { 'i', 'd', '6', '4', 0x01 };
+
+ if (!mesh_crypto_s1("smk3", 4, tmp))
+ return false;
+
+ if (!aes_cmac_one(tmp, n, 16, t))
+ return false;
+
+ if (!aes_cmac_one(t, id64, sizeof(id64), tmp))
+ return false;
+
+ memcpy(out64, tmp + 8, 8);
+
+ return true;
+}
+
+bool mesh_crypto_k4(const uint8_t a[16], uint8_t out6[1])
+{
+ uint8_t tmp[16];
+ uint8_t t[16];
+ uint8_t id6[] = { 'i', 'd', '6', 0x01 };
+
+ if (!mesh_crypto_s1("smk4", 4, tmp))
+ return false;
+
+ if (!aes_cmac_one(tmp, a, 16, t))
+ return false;
+
+ if (!aes_cmac_one(t, id6, sizeof(id6), tmp))
+ return false;
+
+ out6[0] = tmp[15] & 0x3f;
+ return true;
+}
+
+bool mesh_crypto_beacon_cmac(const uint8_t encryption_key[16],
+ const uint8_t network_id[8],
+ uint32_t iv_index, bool kr, bool iu,
+ uint64_t *cmac)
+{
+ uint8_t msg[13], tmp[16];
+
+ if (!cmac)
+ return false;
+
+ msg[0] = kr ? 0x01 : 0x00;
+ msg[0] |= iu ? 0x02 : 0x00;
+ memcpy(msg + 1, network_id, 8);
+ put_be32(iv_index, msg + 9);
+
+ if (!aes_cmac_one(encryption_key, msg, 13, tmp))
+ return false;
+
+ *cmac = get_be64(tmp);
+
+ return true;
+}
+
+bool mesh_crypto_network_nonce(bool ctl, uint8_t ttl, uint32_t seq,
+ uint16_t src, uint32_t iv_index,
+ uint8_t nonce[13])
+{
+ nonce[0] = 0;
+ nonce[1] = (ttl & TTL_MASK) | (ctl ? CTL : 0x00);
+ nonce[2] = (seq >> 16) & 0xff;
+ nonce[3] = (seq >> 8) & 0xff;
+ nonce[4] = seq & 0xff;
+
+ /* SRC */
+ put_be16(src, nonce + 5);
+
+ put_be16(0, nonce + 7);
+
+ /* IV Index */
+ put_be32(iv_index, nonce + 9);
+
+ return true;
+}
+
+bool mesh_crypto_network_encrypt(bool ctl, uint8_t ttl,
+ uint32_t seq, uint16_t src,
+ uint32_t iv_index,
+ const uint8_t net_key[16],
+ const uint8_t *enc_msg, uint8_t enc_msg_len,
+ uint8_t *out, void *net_mic)
+{
+ uint8_t nonce[13];
+
+ if (!mesh_crypto_network_nonce(ctl, ttl, seq, src, iv_index, nonce))
+ return false;
+
+ return mesh_crypto_aes_ccm_encrypt(nonce, net_key,
+ NULL, 0, enc_msg,
+ enc_msg_len, out,
+ net_mic,
+ ctl ? sizeof(uint64_t) : sizeof(uint32_t));
+}
+
+bool mesh_crypto_network_decrypt(bool ctl, uint8_t ttl,
+ uint32_t seq, uint16_t src,
+ uint32_t iv_index,
+ const uint8_t net_key[16],
+ const uint8_t *enc_msg, uint8_t enc_msg_len,
+ uint8_t *out, void *net_mic, size_t mic_size)
+{
+ uint8_t nonce[13];
+
+ if (!mesh_crypto_network_nonce(ctl, ttl, seq, src, iv_index, nonce))
+ return false;
+
+ return mesh_crypto_aes_ccm_decrypt(nonce, net_key, NULL, 0,
+ enc_msg, enc_msg_len, out,
+ net_mic, mic_size);
+}
+
+bool mesh_crypto_application_nonce(uint32_t seq, uint16_t src,
+ uint16_t dst, uint32_t iv_index,
+ bool aszmic, uint8_t nonce[13])
+{
+ nonce[0] = 0x01;
+ nonce[1] = aszmic ? 0x80 : 0x00;
+ nonce[2] = (seq & 0x00ff0000) >> 16;
+ nonce[3] = (seq & 0x0000ff00) >> 8;
+ nonce[4] = (seq & 0x000000ff);
+ nonce[5] = (src & 0xff00) >> 8;
+ nonce[6] = (src & 0x00ff);
+ nonce[7] = (dst & 0xff00) >> 8;
+ nonce[8] = (dst & 0x00ff);
+ put_be32(iv_index, nonce + 9);
+
+ return true;
+}
+
+bool mesh_crypto_device_nonce(uint32_t seq, uint16_t src,
+ uint16_t dst, uint32_t iv_index,
+ bool aszmic, uint8_t nonce[13])
+{
+ nonce[0] = 0x02;
+ nonce[1] = aszmic ? 0x80 : 0x00;
+ nonce[2] = (seq & 0x00ff0000) >> 16;
+ nonce[3] = (seq & 0x0000ff00) >> 8;
+ nonce[4] = (seq & 0x000000ff);
+ nonce[5] = (src & 0xff00) >> 8;
+ nonce[6] = (src & 0x00ff);
+ nonce[7] = (dst & 0xff00) >> 8;
+ nonce[8] = (dst & 0x00ff);
+ put_be32(iv_index, nonce + 9);
+
+ return true;
+}
+
+bool mesh_crypto_application_encrypt(uint8_t key_id, uint32_t seq, uint16_t src,
+ uint16_t dst, uint32_t iv_index,
+ const uint8_t app_key[16],
+ const uint8_t *aad, uint8_t aad_len,
+ const uint8_t *msg, uint8_t msg_len,
+ uint8_t *out, void *app_mic,
+ size_t mic_size)
+{
+ uint8_t nonce[13];
+ bool aszmic = (mic_size == sizeof(uint64_t)) ? true : false;
+
+ if (!key_id && !mesh_crypto_device_nonce(seq, src, dst,
+ iv_index, aszmic, nonce))
+ return false;
+
+ if (key_id && !mesh_crypto_application_nonce(seq, src, dst,
+ iv_index, aszmic, nonce))
+ return false;
+
+ return mesh_crypto_aes_ccm_encrypt(nonce, app_key, aad, aad_len,
+ msg, msg_len,
+ out, app_mic, mic_size);
+}
+
+bool mesh_crypto_application_decrypt(uint8_t key_id, uint32_t seq, uint16_t src,
+ uint16_t dst, uint32_t iv_index,
+ const uint8_t app_key[16],
+ const uint8_t *aad, uint8_t aad_len,
+ const uint8_t *enc_msg, uint8_t enc_msg_len,
+ uint8_t *out, void *app_mic, size_t mic_size)
+{
+ uint8_t nonce[13];
+ bool aszmic = (mic_size == sizeof(uint64_t)) ? true : false;
+
+ if (!key_id && !mesh_crypto_device_nonce(seq, src, dst,
+ iv_index, aszmic, nonce))
+ return false;
+
+ if (key_id && !mesh_crypto_application_nonce(seq, src, dst,
+ iv_index, aszmic, nonce))
+ return false;
+
+ return mesh_crypto_aes_ccm_decrypt(nonce, app_key,
+ aad, aad_len, enc_msg,
+ enc_msg_len, out,
+ app_mic, mic_size);
+}
+
+bool mesh_crypto_session_key(const uint8_t secret[32],
+ const uint8_t salt[16],
+ uint8_t session_key[16])
+{
+ const uint8_t prsk[4] = "prsk";
+
+ if (!aes_cmac_one(salt, secret, 32, session_key))
+ return false;
+
+ return aes_cmac_one(session_key, prsk, 4, session_key);
+}
+
+bool mesh_crypto_nonce(const uint8_t secret[32],
+ const uint8_t salt[16],
+ uint8_t nonce[13])
+{
+ const uint8_t prsn[4] = "prsn";
+ uint8_t tmp[16];
+ bool result;
+
+ if (!aes_cmac_one(salt, secret, 32, tmp))
+ return false;
+
+ result = aes_cmac_one(tmp, prsn, 4, tmp);
+
+ if (result)
+ memcpy(nonce, tmp + 3, 13);
+
+ return result;
+}
+
+bool mesh_crypto_s1(const void *info, size_t len, uint8_t salt[16])
+{
+ const uint8_t zero[16] = {0};
+
+ return aes_cmac_one(zero, info, len, salt);
+}
+
+bool mesh_crypto_prov_prov_salt(const uint8_t conf_salt[16],
+ const uint8_t prov_rand[16],
+ const uint8_t dev_rand[16],
+ uint8_t prov_salt[16])
+{
+ const uint8_t zero[16] = {0};
+ uint8_t tmp[16 * 3];
+
+ memcpy(tmp, conf_salt, 16);
+ memcpy(tmp + 16, prov_rand, 16);
+ memcpy(tmp + 32, dev_rand, 16);
+
+ return aes_cmac_one(zero, tmp, sizeof(tmp), prov_salt);
+}
+
+bool mesh_crypto_prov_conf_key(const uint8_t secret[32],
+ const uint8_t salt[16],
+ uint8_t conf_key[16])
+{
+ const uint8_t prck[4] = "prck";
+
+ if (!aes_cmac_one(salt, secret, 32, conf_key))
+ return false;
+
+ return aes_cmac_one(conf_key, prck, 4, conf_key);
+}
+
+bool mesh_crypto_device_key(const uint8_t secret[32],
+ const uint8_t salt[16],
+ uint8_t device_key[16])
+{
+ const uint8_t prdk[4] = "prdk";
+
+ if (!aes_cmac_one(salt, secret, 32, device_key))
+ return false;
+
+ return aes_cmac_one(device_key, prdk, 4, device_key);
+}
+
+bool mesh_crypto_virtual_addr(const uint8_t virtual_label[16],
+ uint16_t *addr)
+{
+ uint8_t tmp[16];
+
+ if (!mesh_crypto_s1("vtad", 4, tmp))
+ return false;
+
+ if (!addr || !aes_cmac_one(tmp, virtual_label, 16, tmp))
+ return false;
+
+ *addr = (get_be16(tmp + 14) & 0x3fff) | 0x8000;
+
+ return true;
+}
+
+bool mesh_crypto_packet_encode(uint8_t *packet, uint8_t packet_len,
+ const uint8_t network_key[16],
+ uint32_t iv_index,
+ const uint8_t privacy_key[16])
+{
+ uint8_t network_nonce[13] = { 0x00, 0x00 };
+ uint8_t privacy_counter[16] = { 0x00, 0x00, 0x00, 0x00, 0x00, };
+ uint8_t tmp[16];
+ int i;
+
+ /* Detect Proxy packet by CTL == true && DST == 0x0000 */
+ if ((packet[1] & CTL) && get_be16(packet + 7) == 0)
+ network_nonce[0] = 0x03;
+ else
+ /* CTL + TTL */
+ network_nonce[1] = packet[1];
+
+ /* Seq Num */
+ network_nonce[2] = packet[2];
+ network_nonce[3] = packet[3];
+ network_nonce[4] = packet[4];
+
+ /* SRC */
+ network_nonce[5] = packet[5];
+ network_nonce[6] = packet[6];
+
+ /* DST not available */
+ network_nonce[7] = 0;
+ network_nonce[8] = 0;
+
+ /* IV Index */
+ put_be32(iv_index, network_nonce + 9);
+
+ /* Check for Long net-MIC */
+ if (packet[1] & CTL) {
+ if (!mesh_crypto_aes_ccm_encrypt(network_nonce, network_key,
+ NULL, 0,
+ packet + 7, packet_len - 7 - 8,
+ packet + 7, NULL, sizeof(uint64_t)))
+ return false;
+ } else {
+ if (!mesh_crypto_aes_ccm_encrypt(network_nonce, network_key,
+ NULL, 0,
+ packet + 7, packet_len - 7 - 4,
+ packet + 7, NULL, sizeof(uint32_t)))
+ return false;
+ }
+
+ put_be32(iv_index, privacy_counter + 5);
+ memcpy(privacy_counter + 9, packet + 7, 7);
+
+ if (!aes_ecb_one(privacy_key, privacy_counter, tmp))
+ return false;
+
+ for (i = 0; i < 6; i++)
+ packet[1 + i] ^= tmp[i];
+
+ return true;
+}
+
+bool mesh_crypto_packet_decode(const uint8_t *packet, uint8_t packet_len,
+ bool proxy, uint8_t *out, uint32_t iv_index,
+ const uint8_t network_key[16],
+ const uint8_t privacy_key[16])
+{
+ uint8_t privacy_counter[16] = { 0x00, 0x00, 0x00, 0x00, 0x00, };
+ uint8_t network_nonce[13] = { 0x00, 0x00, };
+ uint8_t tmp[16];
+ uint16_t src;
+ int i;
+
+ if (packet_len < 14)
+ return false;
+
+ put_be32(iv_index, privacy_counter + 5);
+ memcpy(privacy_counter + 9, packet + 7, 7);
+
+ if (!aes_ecb_one(privacy_key, privacy_counter, tmp))
+ return false;
+
+ memcpy(out, packet, packet_len);
+ for (i = 0; i < 6; i++)
+ out[1 + i] ^= tmp[i];
+
+ src = get_be16(out + 5);
+
+ /* Pre-check SRC address for illegal values */
+ if (!src || src >= 0x8000)
+ return false;
+
+ /* Detect Proxy packet by CTL == true && proxy == true */
+ if ((out[1] & CTL) && proxy)
+ network_nonce[0] = 0x03;
+ else
+ /* CTL + TTL */
+ network_nonce[1] = out[1];
+
+ /* Seq Num */
+ network_nonce[2] = out[2];
+ network_nonce[3] = out[3];
+ network_nonce[4] = out[4];
+
+ /* SRC */
+ network_nonce[5] = out[5];
+ network_nonce[6] = out[6];
+
+ /* DST not available */
+ network_nonce[7] = 0;
+ network_nonce[8] = 0;
+
+ /* IV Index */
+ put_be32(iv_index, network_nonce + 9);
+
+ /* Check for Long MIC */
+ if (out[1] & CTL) {
+ uint64_t mic;
+
+ if (!mesh_crypto_aes_ccm_decrypt(network_nonce, network_key,
+ NULL, 0, packet + 7, packet_len - 7,
+ out + 7, &mic, sizeof(mic)))
+ return false;
+
+ mic ^= get_be64(out + packet_len - 8);
+ put_be64(mic, out + packet_len - 8);
+
+ if (mic)
+ return false;
+ } else {
+ uint32_t mic;
+
+ if (!mesh_crypto_aes_ccm_decrypt(network_nonce, network_key,
+ NULL, 0, packet + 7, packet_len - 7,
+ out + 7, &mic, sizeof(mic)))
+ return false;
+
+ mic ^= get_be32(out + packet_len - 4);
+ put_be32(mic, out + packet_len - 4);
+
+ if (mic)
+ return false;
+ }
+
+ return true;
+}
+
+bool mesh_get_random_bytes(void *buf, size_t num_bytes)
+{
+ ssize_t len;
+ int fd;
+
+ fd = open("/dev/urandom", O_RDONLY);
+ if (fd < 0)
+ return false;
+
+ len = read(fd, buf, num_bytes);
+
+ close(fd);
+
+ if (len < 0)
+ return false;
+
+ return true;
+}
diff --git a/mesh/gatt.c b/mesh/gatt.c
new file mode 100644
index 0000000..b981054
--- /dev/null
+++ b/mesh/gatt.c
+/*
+ *
+ * BlueZ - Bluetooth protocol stack for Linux
+ *
+ * Copyright (C) 2017 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.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <errno.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <sys/uio.h>
+#include <wordexp.h>
+
+#include <readline/readline.h>
+#include <readline/history.h>
+#include <glib.h>
+
+#include "src/shared/io.h"
+#include "gdbus/gdbus.h"
+#include "lib/bluetooth.h"
+#include "lib/uuid.h"
+#include "client/display.h"
+#include "node.h"
+#include "util.h"
+#include "gatt.h"
+#include "prov.h"
+#include "net.h"
+
+#define MESH_PROV_DATA_OUT_UUID_STR "00002adc-0000-1000-8000-00805f9b34fb"
+#define MESH_PROXY_DATA_OUT_UUID_STR "00002ade-0000-1000-8000-00805f9b34fb"
+
+static struct io *write_io;
+static uint16_t write_mtu;
+
+static struct io *notify_io;
+static uint16_t notify_mtu;
+
+struct write_data {
+ GDBusProxy *proxy;
+ void *user_data;
+ struct iovec iov;
+ GDBusReturnFunction cb;
+ uint8_t *gatt_data;
+ uint8_t gatt_len;
+};
+
+struct notify_data {
+ GDBusProxy *proxy;
+ bool enable;
+ GDBusReturnFunction cb;
+ void *user_data;
+};
+
+bool mesh_gatt_is_child(GDBusProxy *proxy, GDBusProxy *parent,
+ const char *name)
+{
+ DBusMessageIter iter;
+ const char *parent_path;
+
+ if (!parent)
+ return FALSE;
+
+ if (g_dbus_proxy_get_property(proxy, name, &iter) == FALSE)
+ return FALSE;
+
+ dbus_message_iter_get_basic(&iter, &parent_path);
+
+ if (!strcmp(parent_path, g_dbus_proxy_get_path(parent)))
+ return TRUE;
+ else
+ return FALSE;
+}
+
+/* Refactor this once actual MTU is available */
+#define GATT_MTU 23
+
+static void write_data_free(void *user_data)
+{
+ struct write_data *data = user_data;
+
+ g_free(data->gatt_data);
+ free(data);
+}
+
+static void write_setup(DBusMessageIter *iter, void *user_data)
+{
+ struct write_data *data = user_data;
+ struct iovec *iov = &data->iov;
+ DBusMessageIter array, dict;
+
+ dbus_message_iter_open_container(iter, DBUS_TYPE_ARRAY, "y", &array);
+ dbus_message_iter_append_fixed_array(&array, DBUS_TYPE_BYTE,
+ &iov->iov_base, iov->iov_len);
+ dbus_message_iter_close_container(iter, &array);
+
+ dbus_message_iter_open_container(iter, DBUS_TYPE_ARRAY,
+ DBUS_DICT_ENTRY_BEGIN_CHAR_AS_STRING
+ DBUS_TYPE_STRING_AS_STRING
+ DBUS_TYPE_VARIANT_AS_STRING
+ DBUS_DICT_ENTRY_END_CHAR_AS_STRING,
+ &dict);
+
+ dbus_message_iter_close_container(iter, &dict);
+}
+
+uint16_t mesh_gatt_sar(uint8_t **pkt, uint16_t size)
+{
+ const uint8_t *data = *pkt;
+ uint8_t gatt_hdr = *data++;
+ uint8_t type = gatt_hdr & GATT_TYPE_MASK;
+ static uint8_t gatt_size;
+ static uint8_t gatt_pkt[67];
+
+ print_byte_array("GATT-RX:\t", *pkt, size);
+ if (size < 1) {
+ gatt_pkt[0] = GATT_TYPE_INVALID;
+ /* TODO: Disconnect GATT per last paragraph sec 6.6 */
+ return 0;
+ }
+
+ size--;
+
+ switch (gatt_hdr & GATT_SAR_MASK) {
+ case GATT_SAR_FIRST:
+ gatt_size = 1;
+ gatt_pkt[0] = type;
+ /* TODO: Start Proxy Timeout */
+ /* fall through */
+
+ case GATT_SAR_CONTINUE:
+ if (gatt_pkt[0] != type ||
+ gatt_size + size > MAX_GATT_SIZE) {
+
+ /* Invalidate packet and return failure */
+ gatt_pkt[0] = GATT_TYPE_INVALID;
+ /* TODO: Disconnect GATT per last paragraph sec 6.6 */
+ return 0;
+ }
+
+ memcpy(gatt_pkt + gatt_size, data, size);
+ gatt_size += size;
+
+ /* We are good to this point, but incomplete */
+ return 0;
+
+ default:
+ case GATT_SAR_COMPLETE:
+ gatt_size = 1;
+ gatt_pkt[0] = type;
+
+ /* fall through */
+
+ case GATT_SAR_LAST:
+ if (gatt_pkt[0] != type ||
+ gatt_size + size > MAX_GATT_SIZE) {
+
+ /* Invalidate packet and return failure */
+ gatt_pkt[0] = GATT_TYPE_INVALID;
+ /* Disconnect GATT per last paragraph sec 6.6 */
+ return 0;
+ }
+
+ memcpy(gatt_pkt + gatt_size, data, size);
+ gatt_size += size;
+ *pkt = gatt_pkt;
+ return gatt_size;
+ }
+}
+
+static bool pipe_write(struct io *io, void *user_data)
+{
+ struct write_data *data = user_data;
+ struct iovec iov[2];
+ uint8_t sar;
+ uint8_t max_len = write_mtu - 4;
+
+ if (data == NULL)
+ return true;
+
+ print_byte_array("GATT-TX:\t", data->gatt_data, data->gatt_len);
+
+ sar = data->gatt_data[0];
+
+ data->iov.iov_base = data->gatt_data + 1;
+ data->iov.iov_len--;
+
+ iov[0].iov_base = &sar;
+ iov[0].iov_len = sizeof(sar);
+
+ while (1) {
+ int err;
+
+ iov[1] = data->iov;
+
+ err = io_send(io, iov, 2);
+ if (err < 0) {
+ rl_printf("Failed to write: %s\n", strerror(-err));
+ write_data_free(data);
+ return false;
+ }
+
+ switch (sar & GATT_SAR_MASK) {
+ case GATT_SAR_FIRST:
+ case GATT_SAR_CONTINUE:
+ data->gatt_len -= max_len;
+ data->iov.iov_base = data->iov.iov_base + max_len;
+
+ sar &= GATT_TYPE_MASK;
+ if (max_len < data->gatt_len) {
+ data->iov.iov_len = max_len;
+ sar |= GATT_SAR_CONTINUE;
+ } else {
+ data->iov.iov_len = data->gatt_len;
+ sar |= GATT_SAR_LAST;
+ }
+
+ break;
+
+ default:
+ if(data->cb)
+ data->cb(NULL, data->user_data);
+ write_data_free(data);
+ return true;
+ }
+ }
+}
+
+static void write_reply(DBusMessage *message, void *user_data)
+{
+ struct write_data *data = user_data;
+ struct write_data *tmp;
+ uint8_t *dptr = data->gatt_data;
+ uint8_t max_len = GATT_MTU - 3;
+ uint8_t max_seg = GATT_MTU - 4;
+ DBusError error;
+
+ dbus_error_init(&error);
+
+ if (dbus_set_error_from_message(&error, message) == TRUE) {
+ rl_printf("Failed to write: %s\n", error.name);
+ dbus_error_free(&error);
+ return;
+ }
+
+ if (data == NULL)
+ return;
+
+ switch (data->gatt_data[0] & GATT_SAR_MASK) {
+ case GATT_SAR_FIRST:
+ case GATT_SAR_CONTINUE:
+ tmp = g_new0(struct write_data, 1);
+ if (!data)
+ return;
+
+ *tmp = *data;
+ tmp->gatt_data = g_malloc(data->gatt_len);
+
+ if (!tmp->gatt_data) {
+ g_free(tmp);
+ return;
+ }
+
+ tmp->gatt_data[0] = dptr[0];
+ data = tmp;
+ memcpy(data->gatt_data + 1, dptr + max_len,
+ data->gatt_len - max_seg);
+ data->gatt_len -= max_seg;
+ data->gatt_data[0] &= GATT_TYPE_MASK;
+ data->iov.iov_base = data->gatt_data;
+ if (max_len < data->gatt_len) {
+ data->iov.iov_len = max_len;
+ data->gatt_data[0] |= GATT_SAR_CONTINUE;
+ } else {
+ data->iov.iov_len = data->gatt_len;
+ data->gatt_data[0] |= GATT_SAR_LAST;
+ }
+
+ break;
+
+ default:
+ if(data->cb)
+ data->cb(message, data->user_data);
+ return;
+ }
+
+ if (g_dbus_proxy_method_call(data->proxy, "WriteValue", write_setup,
+ write_reply, data, write_data_free) == FALSE) {
+ rl_printf("Failed to write\n");
+ write_data_free(data);
+ return;
+ }
+
+}
+
+static void write_io_destroy(void)
+{
+ io_destroy(write_io);
+ write_io = NULL;
+ write_mtu = 0;
+}
+
+static void notify_io_destroy(void)
+{
+ io_destroy(notify_io);
+ notify_io = NULL;
+ notify_mtu = 0;
+}
+
+static bool pipe_hup(struct io *io, void *user_data)
+{
+ rl_printf("%s closed\n", io == notify_io ? "Notify" : "Write");
+
+ if (io == notify_io)
+ notify_io_destroy();
+ else
+ write_io_destroy();
+
+ return false;
+}
+
+static struct io *pipe_io_new(int fd)
+{
+ struct io *io;
+
+ io = io_new(fd);
+
+ io_set_close_on_destroy(io, true);
+
+ io_set_disconnect_handler(io, pipe_hup, NULL, NULL);
+
+ return io;
+}
+
+static void acquire_write_reply(DBusMessage *message, void *user_data)
+{
+ struct write_data *data = user_data;
+ DBusError error;
+ int fd;
+
+ dbus_error_init(&error);
+
+ if (dbus_set_error_from_message(&error, message) == TRUE) {
+ dbus_error_free(&error);
+ if (g_dbus_proxy_method_call(data->proxy, "WriteValue",
+ write_setup, write_reply, data,
+ write_data_free) == FALSE) {
+ rl_printf("Failed to write\n");
+ write_data_free(data);
+ }
+ return;
+ }
+
+ if ((dbus_message_get_args(message, NULL, DBUS_TYPE_UNIX_FD, &fd,
+ DBUS_TYPE_UINT16, &write_mtu,
+ DBUS_TYPE_INVALID) == false)) {
+ rl_printf("Invalid AcquireWrite response\n");
+ return;
+ }
+
+ rl_printf("AcquireWrite success: fd %d MTU %u\n", fd, write_mtu);
+
+ write_io = pipe_io_new(fd);
+
+ pipe_write(write_io, data);
+}
+
+bool mesh_gatt_write(GDBusProxy *proxy, uint8_t *buf, uint16_t len,
+ GDBusReturnFunction cb, void *user_data)
+{
+ struct write_data *data;
+ DBusMessageIter iter;
+ uint8_t max_len;
+
+ if (!buf || !len)
+ return false;
+
+ if (len > 69)
+ return false;
+
+ data = g_new0(struct write_data, 1);
+ if (!data)
+ return false;
+
+ max_len = write_mtu ? write_mtu - 3 : GATT_MTU - 3;
+
+ /* TODO: should keep in queue in case we need to cancel write? */
+
+ data->gatt_len = len;
+ data->gatt_data = g_memdup(buf, len);
+ data->gatt_data[0] &= GATT_TYPE_MASK;
+ if (max_len < len) {
+ len = max_len;
+ data->gatt_data[0] |= GATT_SAR_FIRST;
+ }
+ data->iov.iov_base = data->gatt_data;
+ data->iov.iov_len = len;
+ data->proxy = proxy;
+ data->user_data = user_data;
+ data->cb = cb;
+
+ if (write_io)
+ return pipe_write(write_io, data);
+
+ if (g_dbus_proxy_get_property(proxy, "WriteAcquired", &iter)) {
+ if (g_dbus_proxy_method_call(proxy, "AcquireWrite", NULL,
+ acquire_write_reply, data, NULL) == FALSE) {
+ rl_printf("Failed to AcquireWrite\n");
+ write_data_free(data);
+ return false;
+ }
+ } else {
+ if (g_dbus_proxy_method_call(data->proxy, "WriteValue",
+ write_setup, write_reply, data,
+ write_data_free) == FALSE) {
+ rl_printf("Failed to write\n");
+ write_data_free(data);
+ return false;
+ }
+ print_byte_array("GATT-TX: ", buf, len);
+ rl_printf("Attempting to write %s\n",
+ g_dbus_proxy_get_path(proxy));
+ }
+
+ return true;
+}
+
+static void notify_reply(DBusMessage *message, void *user_data)
+{
+ struct notify_data *data = user_data;
+ DBusError error;
+
+ dbus_error_init(&error);
+
+ if (dbus_set_error_from_message(&error, message) == TRUE) {
+ rl_printf("Failed to %s notify: %s\n",
+ data->enable ? "start" : "stop", error.name);
+ dbus_error_free(&error);
+ goto done;
+ }
+
+ rl_printf("Notify %s\n", data->enable ? "started" : "stopped");
+
+done:
+ if (data->cb)
+ data->cb(message, data->user_data);
+
+ g_free(data);
+}
+
+static bool pipe_read(struct io *io, bool prov, void *user_data)
+{
+ struct mesh_node *node = user_data;
+ uint8_t buf[512];
+ uint8_t *res;
+ int fd = io_get_fd(io);
+ ssize_t len;
+
+ if (io != notify_io)
+ return true;
+
+ while ((len = read(fd, buf, sizeof(buf)))) {
+ if (len <= 0)
+ break;
+
+ res = buf;
+ mesh_gatt_sar(&res, len);
+
+ if (prov)
+ prov_data_ready(node, res, len);
+ else
+ net_data_ready(res, len);
+ }
+
+ return true;
+}
+
+static bool pipe_read_prov(struct io *io, void *user_data)
+{
+ return pipe_read(io, true, user_data);
+}
+
+static bool pipe_read_proxy(struct io *io, void *user_data)
+{
+ return pipe_read(io, false, user_data);
+}
+
+static void acquire_notify_reply(DBusMessage *message, void *user_data)
+{
+ struct notify_data *data = user_data;
+ DBusMessageIter iter;
+ DBusError error;
+ int fd;
+ const char *uuid;
+
+ dbus_error_init(&error);
+
+ if (dbus_set_error_from_message(&error, message) == TRUE) {
+ dbus_error_free(&error);
+ if (g_dbus_proxy_method_call(data->proxy, "StartNotify", NULL,
+ notify_reply, data, NULL) == FALSE) {
+ rl_printf("Failed to StartNotify\n");
+ g_free(data);
+ }
+ return;
+ }
+
+ if (notify_io) {
+ io_destroy(notify_io);
+ notify_io = NULL;
+ }
+
+ notify_mtu = 0;
+
+ if ((dbus_message_get_args(message, NULL, DBUS_TYPE_UNIX_FD, &fd,
+ DBUS_TYPE_UINT16, ¬ify_mtu,
+ DBUS_TYPE_INVALID) == false)) {
+ if (g_dbus_proxy_method_call(data->proxy, "StartNotify", NULL,
+ notify_reply, data, NULL) == FALSE) {
+ rl_printf("Failed to StartNotify\n");
+ g_free(data);
+ }
+ return;
+ }
+
+ rl_printf("AcquireNotify success: fd %d MTU %u\n", fd, notify_mtu);
+
+ if (g_dbus_proxy_get_property(data->proxy, "UUID", &iter) == FALSE)
+ goto done;
+
+ notify_io = pipe_io_new(fd);
+
+ dbus_message_iter_get_basic(&iter, &uuid);
+
+ if (!bt_uuid_strcmp(uuid, MESH_PROV_DATA_OUT_UUID_STR))
+ io_set_read_handler(notify_io, pipe_read_prov, data->user_data,
+ NULL);
+ else if (!bt_uuid_strcmp(uuid, MESH_PROXY_DATA_OUT_UUID_STR))
+ io_set_read_handler(notify_io, pipe_read_proxy, data->user_data,
+ NULL);
+
+done:
+ if (data->cb)
+ data->cb(message, data->user_data);
+
+ g_free(data);
+}
+
+bool mesh_gatt_notify(GDBusProxy *proxy, bool enable, GDBusReturnFunction cb,
+ void *user_data)
+{
+ struct notify_data *data;
+ DBusMessageIter iter;
+ const char *method;
+
+ data = g_new0(struct notify_data, 1);
+ data->proxy = proxy;
+ data->enable = enable;
+ data->cb = cb;
+ data->user_data = user_data;
+
+ if (enable == TRUE) {
+ if (g_dbus_proxy_get_property(proxy, "NotifyAcquired", &iter)) {
+ method = "AcquireNotify";
+ cb = acquire_notify_reply;
+ } else {
+ method = "StartNotify";
+ cb = notify_reply;
+ }
+ } else {
+ if (notify_io) {
+ notify_io_destroy();
+ if (cb)
+ cb(NULL, user_data);
+ return true;
+ } else {
+ method = "StopNotify";
+ cb = notify_reply;
+ }
+ }
+
+ if (g_dbus_proxy_method_call(proxy, method, NULL, cb,
+ data, NULL) == FALSE) {
+ rl_printf("Failed to %s\n", method);
+ return false;
+ }
+ return true;
+}
diff --git a/mesh/main.c b/mesh/main.c
new file mode 100644
index 0000000..a347484
--- /dev/null
+++ b/mesh/main.c
+/*
+ *
+ * BlueZ - Bluetooth protocol stack for Linux
+ *
+ * Copyright (C) 2017 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.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <errno.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <signal.h>
+#include <sys/signalfd.h>
+#include <wordexp.h>
+
+#include <inttypes.h>
+#include <ctype.h>
+#include <sys/file.h>
+#include <sys/ioctl.h>
+#include <sys/stat.h>
+#include "bluetooth/bluetooth.h"
+
+#include <readline/readline.h>
+#include <readline/history.h>
+#include <glib.h>
+
+#include "lib/bluetooth.h"
+#include "lib/uuid.h"
+#include "src/shared/util.h"
+#include "gdbus/gdbus.h"
+#include "monitor/uuid.h"
+#include "client/display.h"
+#include "mesh-net.h"
+#include "gatt.h"
+#include "crypto.h"
+#include "node.h"
+#include "net.h"
+#include "keys.h"
+#include "prov.h"
+#include "util.h"
+#include "agent.h"
+#include "prov-db.h"
+#include "config-model.h"
+#include "onoff-model.h"
+
+/* String display constants */
+#define COLORED_NEW COLOR_GREEN "NEW" COLOR_OFF
+#define COLORED_CHG COLOR_YELLOW "CHG" COLOR_OFF
+#define COLORED_DEL COLOR_RED "DEL" COLOR_OFF
+
+#define PROMPT_ON COLOR_BLUE "[meshctl]" COLOR_OFF "# "
+#define PROMPT_OFF "Waiting to connect to bluetoothd..."
+
+#define MESH_PROV_DATA_IN_UUID_STR "00002adb-0000-1000-8000-00805f9b34fb"
+#define MESH_PROV_DATA_OUT_UUID_STR "00002adc-0000-1000-8000-00805f9b34fb"
+#define MESH_PROXY_DATA_IN_UUID_STR "00002add-0000-1000-8000-00805f9b34fb"
+#define MESH_PROXY_DATA_OUT_UUID_STR "00002ade-0000-1000-8000-00805f9b34fb"
+
+static GMainLoop *main_loop;
+static DBusConnection *dbus_conn;
+
+struct adapter {
+GDBusProxy *proxy;
+ GList *mesh_devices;
+};
+
+struct mesh_device {
+ GDBusProxy *proxy;
+ uint8_t dev_uuid[16];
+ gboolean hide;
+};
+
+GList *service_list;
+GList *char_list;
+
+static GList *ctrl_list;
+static struct adapter *default_ctrl;
+
+static char *mesh_prov_db_filename;
+static char *mesh_local_config_filename;
+
+static bool discovering = false;
+static bool discover_mesh;
+static uint16_t prov_net_key_index = NET_IDX_PRIMARY;
+
+static guint input = 0;
+
+#define CONN_TYPE_NETWORK 0x00
+#define CONN_TYPE_IDENTITY 0x01
+#define CONN_TYPE_PROVISION 0x02
+#define CONN_TYPE_INVALID 0xff
+
+#define NET_IDX_INVALID 0xffff
+
+struct {
+ GDBusProxy *device;
+ GDBusProxy *service;
+ GDBusProxy *data_in;
+ GDBusProxy *data_out;
+ bool session_open;
+ uint16_t unicast;
+ uint16_t net_idx;
+ uint8_t dev_uuid[16];
+ uint8_t type;
+} connection;
+
+static bool service_is_mesh(GDBusProxy *proxy, const char *target_uuid)
+{
+ DBusMessageIter iter;
+ const char *uuid;
+
+ if (g_dbus_proxy_get_property(proxy, "UUID", &iter) == FALSE)
+ return false;
+
+ dbus_message_iter_get_basic(&iter, &uuid);
+
+ if (target_uuid)
+ return (!bt_uuid_strcmp(uuid, target_uuid));
+ else if (bt_uuid_strcmp(uuid, MESH_PROV_SVC_UUID) ||
+ bt_uuid_strcmp(uuid, MESH_PROXY_SVC_UUID))
+ return true;
+ else
+ return false;
+}
+
+static bool char_is_mesh(GDBusProxy *proxy, const char *target_uuid)
+{
+ DBusMessageIter iter;
+ const char *uuid;
+
+ if (g_dbus_proxy_get_property(proxy, "UUID", &iter) == FALSE)
+ return false;
+
+ dbus_message_iter_get_basic(&iter, &uuid);
+
+ if (target_uuid)
+ return (!bt_uuid_strcmp(uuid, target_uuid));
+
+ if (!bt_uuid_strcmp(uuid, MESH_PROV_DATA_IN_UUID_STR))
+ return true;
+
+ if (!bt_uuid_strcmp(uuid, MESH_PROV_DATA_OUT_UUID_STR))
+ return true;
+
+ if (!bt_uuid_strcmp(uuid, MESH_PROXY_DATA_IN_UUID_STR))
+ return true;
+
+ if (!bt_uuid_strcmp(uuid, MESH_PROXY_DATA_OUT_UUID_STR))
+ return true;
+
+ return false;
+}
+
+static gboolean check_default_ctrl(void)
+{
+ if (!default_ctrl) {
+ rl_printf("No default controller available\n");
+ return FALSE;
+ }
+
+ return TRUE;
+}
+
+static void proxy_leak(gpointer data)
+{
+ rl_printf("Leaking proxy %p\n", data);
+}
+
+static gboolean input_handler(GIOChannel *channel, GIOCondition condition,
+ gpointer user_data)
+{
+ if (condition & G_IO_IN) {
+ rl_callback_read_char();
+ return TRUE;
+ }
+
+ if (condition & (G_IO_HUP | G_IO_ERR | G_IO_NVAL)) {
+ g_main_loop_quit(main_loop);
+ return FALSE;
+ }
+
+ return TRUE;
+}
+
+static guint setup_standard_input(void)
+{
+ GIOChannel *channel;
+ guint source;
+
+ channel = g_io_channel_unix_new(fileno(stdin));
+
+ source = g_io_add_watch(channel,
+ G_IO_IN | G_IO_HUP | G_IO_ERR | G_IO_NVAL,
+ input_handler, NULL);
+
+ g_io_channel_unref(channel);
+
+ return source;
+}
+
+static void connect_handler(DBusConnection *connection, void *user_data)
+{
+ rl_set_prompt(PROMPT_ON);
+ rl_printf("\r");
+ rl_on_new_line();
+ rl_redisplay();
+}
+
+static void disconnect_handler(DBusConnection *connection, void *user_data)
+{
+ if (input > 0) {
+ g_source_remove(input);
+ input = 0;
+ }
+
+ rl_set_prompt(PROMPT_OFF);
+ rl_printf("\r");
+ rl_on_new_line();
+ rl_redisplay();
+
+ g_list_free_full(ctrl_list, proxy_leak);
+ ctrl_list = NULL;
+
+ default_ctrl = NULL;
+}
+
+static void print_adapter(GDBusProxy *proxy, const char *description)
+{
+ DBusMessageIter iter;
+ const char *address, *name;
+
+ if (g_dbus_proxy_get_property(proxy, "Address", &iter) == FALSE)
+ return;
+
+ dbus_message_iter_get_basic(&iter, &address);
+
+ if (g_dbus_proxy_get_property(proxy, "Alias", &iter) == TRUE)
+ dbus_message_iter_get_basic(&iter, &name);
+ else
+ name = "<unknown>";
+
+ rl_printf("%s%s%sController %s %s %s\n",
+ description ? "[" : "",
+ description ? : "",
+ description ? "] " : "",
+ address, name,
+ default_ctrl &&
+ default_ctrl->proxy == proxy ?
+ "[default]" : "");
+
+}
+
+static void print_device(GDBusProxy *proxy, const char *description)
+{
+ DBusMessageIter iter;
+ const char *address, *name;
+
+ if (g_dbus_proxy_get_property(proxy, "Address", &iter) == FALSE)
+ return;
+
+ dbus_message_iter_get_basic(&iter, &address);
+
+ if (g_dbus_proxy_get_property(proxy, "Alias", &iter) == TRUE)
+ dbus_message_iter_get_basic(&iter, &name);
+ else
+ name = "<unknown>";
+
+ rl_printf("%s%s%sDevice %s %s\n",
+ description ? "[" : "",
+ description ? : "",
+ description ? "] " : "",
+ address, name);
+}
+
+static void print_iter(const char *label, const char *name,
+ DBusMessageIter *iter)
+{
+ dbus_bool_t valbool;
+ dbus_uint32_t valu32;
+ dbus_uint16_t valu16;
+ dbus_int16_t vals16;
+ unsigned char byte;
+ const char *valstr;
+ DBusMessageIter subiter;
+ char *entry;
+
+ if (iter == NULL) {
+ rl_printf("%s%s is nil\n", label, name);
+ return;
+ }
+
+ switch (dbus_message_iter_get_arg_type(iter)) {
+ case DBUS_TYPE_INVALID:
+ rl_printf("%s%s is invalid\n", label, name);
+ break;
+ case DBUS_TYPE_STRING:
+ case DBUS_TYPE_OBJECT_PATH:
+ dbus_message_iter_get_basic(iter, &valstr);
+ rl_printf("%s%s: %s\n", label, name, valstr);
+ break;
+ case DBUS_TYPE_BOOLEAN:
+ dbus_message_iter_get_basic(iter, &valbool);
+ rl_printf("%s%s: %s\n", label, name,
+ valbool == TRUE ? "yes" : "no");
+ break;
+ case DBUS_TYPE_UINT32:
+ dbus_message_iter_get_basic(iter, &valu32);
+ rl_printf("%s%s: 0x%06x\n", label, name, valu32);
+ break;
+ case DBUS_TYPE_UINT16:
+ dbus_message_iter_get_basic(iter, &valu16);
+ rl_printf("%s%s: 0x%04x\n", label, name, valu16);
+ break;
+ case DBUS_TYPE_INT16:
+ dbus_message_iter_get_basic(iter, &vals16);
+ rl_printf("%s%s: %d\n", label, name, vals16);
+ break;
+ case DBUS_TYPE_BYTE:
+ dbus_message_iter_get_basic(iter, &byte);
+ rl_printf("%s%s: 0x%02x\n", label, name, byte);
+ break;
+ case DBUS_TYPE_VARIANT:
+ dbus_message_iter_recurse(iter, &subiter);
+ print_iter(label, name, &subiter);
+ break;
+ case DBUS_TYPE_ARRAY:
+ dbus_message_iter_recurse(iter, &subiter);
+ while (dbus_message_iter_get_arg_type(&subiter) !=
+ DBUS_TYPE_INVALID) {
+ print_iter(label, name, &subiter);
+ dbus_message_iter_next(&subiter);
+ }
+ break;
+ case DBUS_TYPE_DICT_ENTRY:
+ dbus_message_iter_recurse(iter, &subiter);
+ entry = g_strconcat(name, "Key", NULL);
+ print_iter(label, entry, &subiter);
+ g_free(entry);
+
+ entry = g_strconcat(name, " Value", NULL);
+ dbus_message_iter_next(&subiter);
+ print_iter(label, entry, &subiter);
+ g_free(entry);
+ break;
+ default:
+ rl_printf("%s%s has unsupported type\n", label, name);
+ break;
+ }
+}
+
+static void print_property(GDBusProxy *proxy, const char *name)
+{
+ DBusMessageIter iter;
+
+ if (g_dbus_proxy_get_property(proxy, name, &iter) == FALSE)
+ return;
+
+ print_iter("\t", name, &iter);
+}
+
+static void forget_mesh_devices()
+{
+ g_list_free_full(default_ctrl->mesh_devices, g_free);
+ default_ctrl->mesh_devices = NULL;
+}
+
+static struct mesh_device *find_device_by_uuid(GList *source, uint8_t uuid[16])
+{
+ GList *list;
+
+ for (list = g_list_first(source); list; list = g_list_next(list)) {
+ struct mesh_device *dev = list->data;
+
+ if (!memcmp(dev->dev_uuid, uuid, 16))
+ return dev;
+ }
+
+ return NULL;
+}
+
+static void print_prov_service(struct prov_svc_data *prov_data)
+{
+ const char *prefix = "\t\t";
+ char txt_uuid[16 * 2 + 1];
+ int i;
+
+ rl_printf("%sMesh Provisioning Service (%s)\n", prefix,
+ MESH_PROV_SVC_UUID);
+ for (i = 0; i < 16; ++i) {
+ sprintf(txt_uuid + (i * 2), "%2.2x", prov_data->dev_uuid[i]);
+ }
+
+ rl_printf("%s\tDevice UUID: %s\n", prefix, txt_uuid);
+ rl_printf("%s\tOOB: %4.4x\n", prefix, prov_data->oob);
+
+}
+
+static bool parse_prov_service_data(const char *uuid, uint8_t *data, int len,
+ void *data_out)
+{
+ struct prov_svc_data *prov_data = data_out;
+ int i;
+
+ if (len < 18)
+ return false;
+
+ for (i = 0; i < 16; ++i) {
+ prov_data->dev_uuid[i] = data[i];
+ }
+
+ prov_data->oob = get_be16(&data[16]);
+
+ return true;
+}
+
+static bool parse_mesh_service_data(const char *uuid, uint8_t *data, int len,
+ void *data_out)
+{
+ const char *prefix = "\t\t";
+
+ if (!(len == 9 && data[0] == 0x00) && !(len == 17 && data[0] == 0x01)) {
+ rl_printf("Unexpected mesh proxy service data length %d\n",
+ len);
+ return false;
+ }
+
+ if (data[0] != connection.type)
+ return false;
+
+ if (data[0] == CONN_TYPE_IDENTITY) {
+ uint8_t *key;
+
+ if (IS_UNASSIGNED(connection.unicast)) {
+ /* This would be a bug */
+ rl_printf("Error: Searching identity with "
+ "unicast 0000\n");
+ return false;
+ }
+
+ key = keys_net_key_get(prov_net_key_index, true);
+ if (!key)
+ return false;
+
+ if (!mesh_crypto_identity_check(key, connection.unicast,
+ &data[1]))
+ return false;
+
+ if (discovering) {
+ rl_printf("\n%sMesh Proxy Service (%s)\n", prefix,
+ uuid);
+ rl_printf("%sIdentity for node %4.4x\n", prefix,
+ connection.unicast);
+ }
+
+ } else if (data[0] == CONN_TYPE_NETWORK) {
+ uint16_t net_idx = net_validate_proxy_beacon(data + 1);
+
+ if (net_idx == NET_IDX_INVALID || net_idx != connection.net_idx)
+ return false;
+
+ if (discovering) {
+ rl_printf("\n%sMesh Proxy Service (%s)\n", prefix,
+ uuid);
+ rl_printf("%sNetwork Beacon for net index %4.4x\n",
+ prefix, net_idx);
+ }
+ }
+
+ return true;
+}
+
+static bool parse_service_data(GDBusProxy *proxy, const char *target_uuid,
+ void *data_out)
+{
+ DBusMessageIter iter, entries;
+ bool mesh_prov = false;
+ bool mesh_proxy = false;
+
+ if (target_uuid) {
+ mesh_prov = !strcmp(target_uuid, MESH_PROV_SVC_UUID);
+ mesh_proxy = !strcmp(target_uuid, MESH_PROXY_SVC_UUID);
+ }
+
+ if (!g_dbus_proxy_get_property(proxy, "ServiceData", &iter))
+ return true;
+
+ if (dbus_message_iter_get_arg_type(&iter) != DBUS_TYPE_ARRAY)
+ return false;
+
+ dbus_message_iter_recurse(&iter, &entries);
+
+ while (dbus_message_iter_get_arg_type(&entries)
+ == DBUS_TYPE_DICT_ENTRY) {
+ DBusMessageIter value, entry, array;
+ const char *uuid_str;
+ bt_uuid_t uuid;
+ uint8_t *service_data;
+ int len;
+
+ dbus_message_iter_recurse(&entries, &entry);
+ dbus_message_iter_get_basic(&entry, &uuid_str);
+
+ if (bt_string_to_uuid(&uuid, uuid_str) < 0)
+ goto fail;
+
+ dbus_message_iter_next(&entry);
+
+ if (dbus_message_iter_get_arg_type(&entry) != DBUS_TYPE_VARIANT)
+ goto fail;
+
+ dbus_message_iter_recurse(&entry, &value);
+
+ if (dbus_message_iter_get_arg_type(&value) != DBUS_TYPE_ARRAY)
+ goto fail;
+
+ dbus_message_iter_recurse(&value, &array);
+
+ if (dbus_message_iter_get_arg_type(&array) != DBUS_TYPE_BYTE)
+ goto fail;
+
+ dbus_message_iter_get_fixed_array(&array, &service_data, &len);
+
+ if (mesh_prov && !strcmp(uuid_str, MESH_PROV_SVC_UUID)) {
+ return parse_prov_service_data(uuid_str, service_data,
+ len, data_out);
+ } else if (mesh_proxy &&
+ !strcmp(uuid_str, MESH_PROXY_SVC_UUID)) {
+ return parse_mesh_service_data(uuid_str, service_data,
+ len, data_out);
+ }
+
+ dbus_message_iter_next(&entries);
+ }
+
+ if (!target_uuid)
+ return true;
+fail:
+ return false;
+}
+
+static void print_uuids(GDBusProxy *proxy)
+{
+ DBusMessageIter iter, value;
+
+ if (g_dbus_proxy_get_property(proxy, "UUIDs", &iter) == FALSE)
+ return;
+
+ dbus_message_iter_recurse(&iter, &value);
+
+ while (dbus_message_iter_get_arg_type(&value) == DBUS_TYPE_STRING) {
+ const char *uuid, *text;
+
+ dbus_message_iter_get_basic(&value, &uuid);
+
+ text = uuidstr_to_str(uuid);
+ if (text) {
+ char str[26];
+ unsigned int n;
+
+ str[sizeof(str) - 1] = '\0';
+
+ n = snprintf(str, sizeof(str), "%s", text);
+ if (n > sizeof(str) - 1) {
+ str[sizeof(str) - 2] = '.';
+ str[sizeof(str) - 3] = '.';
+ if (str[sizeof(str) - 4] == ' ')
+ str[sizeof(str) - 4] = '.';
+
+ n = sizeof(str) - 1;
+ }
+
+ rl_printf("\tUUID: %s%*c(%s)\n",
+ str, 26 - n, ' ', uuid);
+ } else
+ rl_printf("\tUUID: %*c(%s)\n", 26, ' ', uuid);
+
+ dbus_message_iter_next(&value);
+ }
+}
+
+static gboolean device_is_child(GDBusProxy *device, GDBusProxy *master)
+{
+ DBusMessageIter iter;
+ const char *adapter, *path;
+
+ if (!master)
+ return FALSE;
+
+ if (g_dbus_proxy_get_property(device, "Adapter", &iter) == FALSE)
+ return FALSE;
+
+ dbus_message_iter_get_basic(&iter, &adapter);
+ path = g_dbus_proxy_get_path(master);
+
+ if (!strcmp(path, adapter))
+ return TRUE;
+
+ return FALSE;
+}
+
+static struct adapter *find_parent(GDBusProxy *device)
+{
+ GList *list;
+
+ for (list = g_list_first(ctrl_list); list; list = g_list_next(list)) {
+ struct adapter *adapter = list->data;
+
+ if (device_is_child(device, adapter->proxy) == TRUE)
+ return adapter;
+ }
+ return NULL;
+}
+
+static void set_connected_device(GDBusProxy *proxy)
+{
+ char *desc = NULL;
+ DBusMessageIter iter;
+ char buf[10];
+ bool mesh;
+
+ connection.device = proxy;
+
+ if (proxy == NULL) {
+ memset(&connection, 0, sizeof(connection));
+ connection.type = CONN_TYPE_INVALID;
+ goto done;
+ }
+
+ if (connection.type == CONN_TYPE_IDENTITY) {
+ mesh = true;
+ snprintf(buf, 10, "Node-%4.4x", connection.unicast);
+ } else if (connection.type == CONN_TYPE_NETWORK) {
+ mesh = true;
+ snprintf(buf, 9, "Net-%4.4x", connection.net_idx);
+ } else {
+ mesh = false;
+ }
+
+ if (!g_dbus_proxy_get_property(proxy, "Alias", &iter) && !mesh)
+ goto done;
+
+ dbus_message_iter_get_basic(&iter, &desc);
+ desc = g_strdup_printf(COLOR_BLUE "[%s%s%s]" COLOR_OFF "# ", desc,
+ (desc && mesh) ? "-" : "",
+ mesh ? buf : "");
+
+done:
+ rl_set_prompt(desc ? desc : PROMPT_ON);
+ rl_printf("\r");
+ rl_on_new_line();
+ g_free(desc);
+
+ /* If disconnected, return to main menu */
+ if (proxy == NULL)
+ cmd_menu_main(true);
+}
+
+static void connect_reply(DBusMessage *message, void *user_data)
+{
+ GDBusProxy *proxy = user_data;
+ DBusError error;
+
+ dbus_error_init(&error);
+
+ if (dbus_set_error_from_message(&error, message) == TRUE) {
+ rl_printf("Failed to connect: %s\n", error.name);
+ dbus_error_free(&error);
+ set_connected_device(NULL);
+ return;
+ }
+
+ rl_printf("Connection successful\n");
+
+ set_connected_device(proxy);
+}
+
+static void update_device_info(GDBusProxy *proxy)
+{
+ struct adapter *adapter = find_parent(proxy);
+ DBusMessageIter iter;
+ struct prov_svc_data prov_data;
+
+ if (!adapter) {
+ /* TODO: Error */
+ return;
+ }
+
+ if (adapter != default_ctrl)
+ return;
+
+ if (!g_dbus_proxy_get_property(proxy, "Address", &iter))
+ return;
+
+ if (parse_service_data(proxy, MESH_PROV_SVC_UUID, &prov_data)) {
+ struct mesh_device *dev;
+
+ dev = find_device_by_uuid(adapter->mesh_devices,
+ prov_data.dev_uuid);
+
+ /* Display provisioning service once per sicovery session */
+ if (discovering && (!dev || !dev->hide))
+ print_prov_service(&prov_data);
+
+ if (dev) {
+ dev->proxy = proxy;
+ dev->hide = discovering;
+ return;
+ }
+
+ dev = g_malloc0(sizeof(struct mesh_device));
+ if (!dev)
+ return;
+
+ dev->proxy = proxy;
+ dev->hide = discovering;
+
+ memcpy(dev->dev_uuid, prov_data.dev_uuid, 16);
+
+ adapter->mesh_devices = g_list_append(adapter->mesh_devices,
+ dev);
+ print_device(proxy, COLORED_NEW);
+
+ node_create_new(&prov_data);
+
+ } else if (parse_service_data(proxy, MESH_PROXY_SVC_UUID, NULL) &&
+ discover_mesh) {
+ bool res;
+
+ g_dbus_proxy_method_call(default_ctrl->proxy, "StopDiscovery",
+ NULL, NULL, NULL, NULL);
+ discover_mesh = false;
+
+ forget_mesh_devices();
+
+ res = g_dbus_proxy_method_call(proxy, "Connect", NULL,
+ connect_reply, proxy, NULL);
+
+ if (!res)
+ rl_printf("Failed to connect to mesh\n");
+
+ else
+ rl_printf("Trying to connect to mesh\n");
+
+ }
+}
+
+static void adapter_added(GDBusProxy *proxy)
+{
+ struct adapter *adapter = g_malloc0(sizeof(struct adapter));
+
+ adapter->proxy = proxy;
+ ctrl_list = g_list_append(ctrl_list, adapter);
+
+ if (!default_ctrl)
+ default_ctrl = adapter;
+
+ print_adapter(proxy, COLORED_NEW);
+}
+
+static void data_out_notify(GDBusProxy *proxy, bool enable,
+ GDBusReturnFunction cb)
+{
+ struct mesh_node *node;
+
+ node = node_find_by_uuid(connection.dev_uuid);
+
+ if (!mesh_gatt_notify(proxy, enable, cb, node))
+ rl_printf("Failed to %s notification on %s\n", enable ?
+ "start" : "stop", g_dbus_proxy_get_path(proxy));
+ else
+ rl_printf("%s notification on %s\n", enable ?
+ "Start" : "Stop", g_dbus_proxy_get_path(proxy));
+}
+
+struct disconnect_data {
+ GDBusReturnFunction cb;
+ void *data;
+};
+
+static void disconnect(GDBusReturnFunction cb, void *user_data)
+{
+ GDBusProxy *proxy;
+ DBusMessageIter iter;
+ const char *addr;
+
+ proxy = connection.device;
+ if (!proxy)
+ return;
+
+ if (g_dbus_proxy_method_call(proxy, "Disconnect", NULL, cb, user_data,
+ NULL) == FALSE) {
+ rl_printf("Failed to disconnect\n");
+ return;
+ }
+
+ if (g_dbus_proxy_get_property(proxy, "Address", &iter) == TRUE)
+ dbus_message_iter_get_basic(&iter, &addr);
+
+ rl_printf("Attempting to disconnect from %s\n", addr);
+}
+
+static void disc_notify_cb(DBusMessage *message, void *user_data)
+{
+ struct disconnect_data *disc_data = user_data;
+
+ disconnect(disc_data->cb, disc_data->data);
+
+ g_free(user_data);
+}
+
+static void disconnect_device(GDBusReturnFunction cb, void *user_data)
+{
+ DBusMessageIter iter;
+
+ net_session_close(connection.data_in);
+
+ /* Stop notificiation on prov_out or proxy out characteristics */
+ if (connection.data_out) {
+ if (g_dbus_proxy_get_property(connection.data_out, "Notifying",
+ &iter) == TRUE) {
+ struct disconnect_data *disc_data;
+ disc_data = g_malloc(sizeof(struct disconnect_data));
+ disc_data->cb = cb;
+ disc_data->data = user_data;
+
+ if (mesh_gatt_notify(connection.data_out, false,
+ disc_notify_cb, disc_data))
+ return;
+ }
+ }
+
+ disconnect(cb, user_data);
+}
+
+static void mesh_prov_done(void *user_data, int status);
+
+static void notify_prov_out_cb(DBusMessage *message, void *user_data)
+{
+ struct mesh_node *node = user_data;
+ DBusError error;
+
+ dbus_error_init(&error);
+
+ if (dbus_set_error_from_message(&error, message) == TRUE) {
+ rl_printf("Failed to start notify: %s\n", error.name);
+ dbus_error_free(&error);
+ return;
+ }
+
+ rl_printf("Notify for Mesh Provisioning Out Data started\n");
+
+ if (connection.type != CONN_TYPE_PROVISION) {
+ rl_printf("Error: wrong connection type %d (expected %d)\n",
+ connection.type, CONN_TYPE_PROVISION);
+ return;
+ }
+
+ if (!connection.data_in) {
+ rl_printf("Error: don't have mesh provisioning data in\n");
+ return;
+ }
+
+ if (!node) {
+ rl_printf("Error: provisioning node not present\n");
+ return;
+ }
+
+ if(!prov_open(node, connection.data_in, prov_net_key_index,
+ mesh_prov_done, node))
+ {
+ rl_printf("Failed to start provisioning\n");
+ node_free(node);
+ disconnect_device(NULL, NULL);
+ } else
+ rl_printf("Initiated provisioning\n");
+
+}
+
+static void session_open_cb (int status)
+{
+ if (status) {
+ rl_printf("Failed to open Mesh session\n");
+ disconnect_device(NULL, NULL);
+ return;
+ }
+
+ rl_printf("Mesh session is open\n");
+
+ /* Get composition data for a newly provisioned node */
+ if (connection.type == CONN_TYPE_IDENTITY)
+ config_client_get_composition(connection.unicast);
+}
+
+static void notify_proxy_out_cb(DBusMessage *message, void *user_data)
+{
+ DBusError error;
+
+ dbus_error_init(&error);
+
+ if (dbus_set_error_from_message(&error, message) == TRUE) {
+ rl_printf("Failed to start notify: %s\n", error.name);
+ dbus_error_free(&error);
+ return;
+ }
+
+ rl_printf("Notify for Mesh Proxy Out Data started\n");
+
+ if (connection.type != CONN_TYPE_IDENTITY &&
+ connection.type != CONN_TYPE_NETWORK) {
+ rl_printf("Error: wrong connection type %d "
+ "(expected %d or %d)\n", connection.type,
+ CONN_TYPE_IDENTITY, CONN_TYPE_NETWORK);
+ return;
+ }
+
+ if (!connection.data_in) {
+ rl_printf("Error: don't have mesh proxy data in\n");
+ return;
+ }
+
+ rl_printf("Trying to open mesh session\n");
+ net_session_open(connection.data_in, true, session_open_cb);
+ connection.session_open = true;
+}
+
+static GDBusProxy *get_characteristic(GDBusProxy *device, const char *char_uuid)
+{
+ GList *l;
+ GDBusProxy *service;
+ const char *svc_uuid;
+
+ if (connection.type == CONN_TYPE_PROVISION) {
+ svc_uuid = MESH_PROV_SVC_UUID;
+ } else {
+ svc_uuid = MESH_PROXY_SVC_UUID;
+ }
+ for (l = service_list; l; l = l->next) {
+ if (mesh_gatt_is_child(l->data, device, "Device") &&
+ service_is_mesh(l->data, svc_uuid))
+ break;
+ }
+
+ if (l)
+ service = l->data;
+ else {
+ rl_printf("Mesh service not found\n");
+ return NULL;
+ }
+
+ for (l = char_list; l; l = l->next) {
+ if (mesh_gatt_is_child(l->data, service, "Service") &&
+ char_is_mesh(l->data, char_uuid)) {
+ rl_printf("Found matching char: path %s, uuid %s\n",
+ g_dbus_proxy_get_path(l->data), char_uuid);
+ return l->data;
+ }
+ }
+ return NULL;
+}
+
+static void mesh_session_setup(GDBusProxy *proxy)
+{
+ if (connection.type == CONN_TYPE_PROVISION) {
+ connection.data_in = get_characteristic(proxy,
+ MESH_PROV_DATA_IN_UUID_STR);
+ if (!connection.data_in)
+ goto fail;
+
+ connection.data_out = get_characteristic(proxy,
+ MESH_PROV_DATA_OUT_UUID_STR);
+ if (!connection.data_out)
+ goto fail;
+
+ data_out_notify(connection.data_out, true, notify_prov_out_cb);
+
+ } else if (connection.type != CONN_TYPE_INVALID){
+
+ connection.data_in = get_characteristic(proxy,
+ MESH_PROXY_DATA_IN_UUID_STR);
+ if (!connection.data_in)
+ goto fail;
+
+ connection.data_out = get_characteristic(proxy,
+ MESH_PROXY_DATA_OUT_UUID_STR);
+ if (!connection.data_out)
+ goto fail;
+
+ data_out_notify(connection.data_out, true, notify_proxy_out_cb);
+ }
+
+ return;
+
+fail:
+
+ rl_printf("Services resolved, mesh characteristics not found\n");
+}
+
+static void proxy_added(GDBusProxy *proxy, void *user_data)
+{
+ const char *interface;
+
+ interface = g_dbus_proxy_get_interface(proxy);
+
+ if (!strcmp(interface, "org.bluez.Device1")) {
+ update_device_info(proxy);
+
+ } else if (!strcmp(interface, "org.bluez.Adapter1")) {
+
+ adapter_added(proxy);
+
+ } else if (!strcmp(interface, "org.bluez.GattService1") &&
+ service_is_mesh(proxy, NULL)) {
+
+ rl_printf("Service added %s\n", g_dbus_proxy_get_path(proxy));
+ service_list = g_list_append(service_list, proxy);
+
+ } else if (!strcmp(interface, "org.bluez.GattCharacteristic1") &&
+ char_is_mesh(proxy, NULL)) {
+
+ rl_printf("Char added %s:\n", g_dbus_proxy_get_path(proxy));
+
+ char_list = g_list_append(char_list, proxy);
+ }
+}
+
+static void start_discovery_reply(DBusMessage *message, void *user_data)
+{
+ dbus_bool_t enable = GPOINTER_TO_UINT(user_data);
+ DBusError error;
+
+ dbus_error_init(&error);
+
+ if (dbus_set_error_from_message(&error, message) == TRUE) {
+ rl_printf("Failed to %s discovery: %s\n",
+ enable == TRUE ? "start" : "stop", error.name);
+ dbus_error_free(&error);
+ return;
+ }
+
+ rl_printf("Discovery %s\n", enable == TRUE ? "started" : "stopped");
+}
+
+static struct mesh_device *find_device_by_proxy(GList *source,
+ GDBusProxy *proxy)
+{
+ GList *list;
+
+ for (list = g_list_first(source); list; list = g_list_next(list)) {
+ struct mesh_device *dev = list->data;
+ GDBusProxy *proxy = dev->proxy;
+
+ if (dev->proxy == proxy)
+ return dev;
+ }
+
+ return NULL;
+}
+
+static void device_removed(GDBusProxy *proxy)
+{
+ struct adapter *adapter = find_parent(proxy);
+ struct mesh_device *dev;
+
+ if (!adapter) {
+ /* TODO: Error */
+ return;
+ }
+
+ dev = find_device_by_proxy(adapter->mesh_devices, proxy);
+ if (dev)
+ adapter->mesh_devices = g_list_remove(adapter->mesh_devices,
+ dev);
+
+ print_device(proxy, COLORED_DEL);
+
+ if (connection.device == proxy)
+ set_connected_device(NULL);
+
+}
+
+static void adapter_removed(GDBusProxy *proxy)
+{
+ GList *ll;
+ for (ll = g_list_first(ctrl_list); ll; ll = g_list_next(ll)) {
+ struct adapter *adapter = ll->data;
+
+ if (adapter->proxy == proxy) {
+ print_adapter(proxy, COLORED_DEL);
+
+ if (default_ctrl && default_ctrl->proxy == proxy) {
+ default_ctrl = NULL;
+ set_connected_device(NULL);
+ }
+
+ ctrl_list = g_list_remove_link(ctrl_list, ll);
+
+ g_list_free_full(adapter->mesh_devices, g_free);
+ g_free(adapter);
+ g_list_free(ll);
+ return;
+ }
+ }
+}
+
+static void proxy_removed(GDBusProxy *proxy, void *user_data)
+{
+ const char *interface;
+
+ interface = g_dbus_proxy_get_interface(proxy);
+
+ if (!strcmp(interface, "org.bluez.Device1")) {
+ device_removed(proxy);
+ } else if (!strcmp(interface, "org.bluez.Adapter1")) {
+ adapter_removed(proxy);
+ } else if (!strcmp(interface, "org.bluez.GattService1")) {
+ if (proxy == connection.service) {
+ if (service_is_mesh(proxy, MESH_PROXY_SVC_UUID)) {
+ data_out_notify(connection.data_out,
+ false, NULL);
+ net_session_close(connection.data_in);
+ }
+ connection.service = NULL;
+ connection.data_in = NULL;
+ connection.data_out = NULL;
+ }
+
+ service_list = g_list_remove(service_list, proxy);
+
+ } else if (!strcmp(interface, "org.bluez.GattCharacteristic1")) {
+ char_list = g_list_remove(char_list, proxy);
+ }
+}
+
+static int get_characteristic_value(DBusMessageIter *value, uint8_t *buf)
+{
+ DBusMessageIter array;
+ uint8_t *data;
+ int len;
+
+ if (dbus_message_iter_get_arg_type(value) != DBUS_TYPE_ARRAY)
+ return 0;
+
+ dbus_message_iter_recurse(value, &array);
+
+ if (dbus_message_iter_get_arg_type(&array) != DBUS_TYPE_BYTE)
+ return 0;
+
+ dbus_message_iter_get_fixed_array(&array, &data, &len);
+ memcpy(buf, data, len);
+
+ return len;
+}
+
+static bool process_mesh_characteristic(GDBusProxy *proxy)
+{
+ DBusMessageIter iter;
+ const char *uuid;
+ uint8_t *res;
+ uint8_t buf[256];
+ bool is_prov;
+
+ if (g_dbus_proxy_get_property(proxy, "UUID", &iter) == FALSE)
+ return false;
+
+ dbus_message_iter_get_basic(&iter, &uuid);
+
+ if (g_dbus_proxy_get_property(proxy, "Value", &iter) == FALSE)
+ return false;
+
+ is_prov = !bt_uuid_strcmp(uuid, MESH_PROV_DATA_OUT_UUID_STR);
+
+ if (is_prov || !bt_uuid_strcmp(uuid, MESH_PROXY_DATA_OUT_UUID_STR))
+ {
+ struct mesh_node *node;
+ uint16_t len;
+
+ len = get_characteristic_value(&iter, buf);
+
+ if (!len || len > 69)
+ return false;
+
+ res = buf;
+ len = mesh_gatt_sar(&res, len);
+
+ if (!len)
+ return false;
+
+ if (is_prov) {
+ node = node_find_by_uuid(connection.dev_uuid);
+
+ if (!node) {
+ rl_printf("Node not found?\n");
+ return false;
+ }
+
+ return prov_data_ready(node, res, len);
+ }
+
+ return net_data_ready(res, len);
+ }
+
+ return false;
+}
+
+
+static void property_changed(GDBusProxy *proxy, const char *name,
+ DBusMessageIter *iter, void *user_data)
+{
+ const char *interface;
+
+ interface = g_dbus_proxy_get_interface(proxy);
+
+ if (!strcmp(interface, "org.bluez.Device1")) {
+
+ if (default_ctrl && device_is_child(proxy,
+ default_ctrl->proxy) == TRUE) {
+
+ if (strcmp(name, "Connected") == 0) {
+ dbus_bool_t connected;
+ dbus_message_iter_get_basic(iter, &connected);
+
+ if (connected && connection.device == NULL)
+ set_connected_device(proxy);
+ else if (!connected &&
+ connection.device == proxy)
+ set_connected_device(NULL);
+ } else if ((strcmp(name, "Alias") == 0) &&
+ connection.device == proxy) {
+ /* Re-generate prompt */
+ set_connected_device(proxy);
+ } else if (!strcmp(name, "ServiceData")) {
+ update_device_info(proxy);
+ } else if (!strcmp(name, "ServicesResolved")) {
+ gboolean resolved;
+
+ dbus_message_iter_get_basic(iter, &resolved);
+
+ rl_printf("Services resolved %s\n", resolved ?
+ "yes" : "no");
+
+ if (resolved)
+ mesh_session_setup(connection.device);
+ }
+
+ }
+ } else if (!strcmp(interface, "org.bluez.Adapter1")) {
+ DBusMessageIter addr_iter;
+ char *str;
+
+ rl_printf("Adapter property changed \n");
+ if (g_dbus_proxy_get_property(proxy, "Address",
+ &addr_iter) == TRUE) {
+ const char *address;
+
+ dbus_message_iter_get_basic(&addr_iter, &address);
+ str = g_strdup_printf("[" COLORED_CHG
+ "] Controller %s ", address);
+ } else
+ str = g_strdup("");
+
+ if (strcmp(name, "Discovering") == 0) {
+ int temp;
+
+ dbus_message_iter_get_basic(iter, &temp);
+ discovering = !!temp;
+ }
+
+ print_iter(str, name, iter);
+ g_free(str);
+ } else if (!strcmp(interface, "org.bluez.GattService1")) {
+ rl_printf("Service property changed %s\n",
+ g_dbus_proxy_get_path(proxy));
+ } else if (!strcmp(interface, "org.bluez.GattCharacteristic1")) {
+ rl_printf("Characteristic property changed %s\n",
+ g_dbus_proxy_get_path(proxy));
+
+ if ((connection.type == CONN_TYPE_PROVISION) ||
+ connection.session_open)
+ process_mesh_characteristic(proxy);
+ }
+}
+
+static void message_handler(DBusConnection *connection,
+ DBusMessage *message, void *user_data)
+{
+ rl_printf("[SIGNAL] %s.%s\n", dbus_message_get_interface(message),
+ dbus_message_get_member(message));
+}
+
+static struct adapter *find_ctrl_by_address(GList *source, const char *address)
+{
+ GList *list;
+
+ for (list = g_list_first(source); list; list = g_list_next(list)) {
+ struct adapter *adapter = list->data;
+ DBusMessageIter iter;
+ const char *str;
+
+ if (g_dbus_proxy_get_property(adapter->proxy,
+ "Address", &iter) == FALSE)
+ continue;
+
+ dbus_message_iter_get_basic(&iter, &str);
+
+ if (!strcmp(str, address))
+ return adapter;
+ }
+
+ return NULL;
+}
+
+static gboolean parse_argument_on_off(const char *arg, dbus_bool_t *value)
+{
+ if (!arg || !strlen(arg)) {
+ rl_printf("Missing on/off argument\n");
+ return FALSE;
+ }
+
+ if (!strcmp(arg, "on") || !strcmp(arg, "yes")) {
+ *value = TRUE;
+ return TRUE;
+ }
+
+ if (!strcmp(arg, "off") || !strcmp(arg, "no")) {
+ *value = FALSE;
+ return TRUE;
+ }
+
+ rl_printf("Invalid argument %s\n", arg);
+ return FALSE;
+}
+
+static void cmd_list(const char *arg)
+{
+ GList *list;
+
+ for (list = g_list_first(ctrl_list); list; list = g_list_next(list)) {
+ struct adapter *adapter = list->data;
+ print_adapter(adapter->proxy, NULL);
+ }
+}
+
+static void cmd_show(const char *arg)
+{
+ struct adapter *adapter;
+ GDBusProxy *proxy;
+ DBusMessageIter iter;
+ const char *address;
+
+
+ if (!arg || !strlen(arg)) {
+ if (check_default_ctrl() == FALSE)
+ return;
+
+ proxy = default_ctrl->proxy;
+ } else {
+ adapter = find_ctrl_by_address(ctrl_list, arg);
+ if (!adapter) {
+ rl_printf("Controller %s not available\n", arg);
+ return;
+ }
+ proxy = adapter->proxy;
+ }
+
+ if (g_dbus_proxy_get_property(proxy, "Address", &iter) == FALSE)
+ return;
+
+ dbus_message_iter_get_basic(&iter, &address);
+ rl_printf("Controller %s\n", address);
+
+ print_property(proxy, "Name");
+ print_property(proxy, "Alias");
+ print_property(proxy, "Class");
+ print_property(proxy, "Powered");
+ print_property(proxy, "Discoverable");
+ print_uuids(proxy);
+ print_property(proxy, "Modalias");
+ print_property(proxy, "Discovering");
+}
+
+static void cmd_select(const char *arg)
+{
+ struct adapter *adapter;
+
+ if (!arg || !strlen(arg)) {
+ rl_printf("Missing controller address argument\n");
+ return;
+ }
+
+ adapter = find_ctrl_by_address(ctrl_list, arg);
+ if (!adapter) {
+ rl_printf("Controller %s not available\n", arg);
+ return;
+ }
+
+ if (default_ctrl && default_ctrl->proxy == adapter->proxy)
+ return;
+
+ forget_mesh_devices();
+
+ default_ctrl = adapter;
+ print_adapter(adapter->proxy, NULL);
+}
+
+static void generic_callback(const DBusError *error, void *user_data)
+{
+ char *str = user_data;
+
+ if (dbus_error_is_set(error))
+ rl_printf("Failed to set %s: %s\n", str, error->name);
+ else
+ rl_printf("Changing %s succeeded\n", str);
+}
+
+static void cmd_power(const char *arg)
+{
+ dbus_bool_t powered;
+ char *str;
+
+ if (parse_argument_on_off(arg, &powered) == FALSE)
+ return;
+
+ if (check_default_ctrl() == FALSE)
+ return;
+
+ str = g_strdup_printf("power %s", powered == TRUE ? "on" : "off");
+
+ if (g_dbus_proxy_set_property_basic(default_ctrl->proxy, "Powered",
+ DBUS_TYPE_BOOLEAN, &powered,
+ generic_callback, str, g_free) == TRUE)
+ return;
+
+ g_free(str);
+}
+
+static void cmd_scan(const char *arg)
+{
+ dbus_bool_t enable;
+ const char *method;
+
+ if (parse_argument_on_off(arg, &enable) == FALSE)
+ return;
+
+ if (check_default_ctrl() == FALSE)
+ return;
+
+ if (enable == TRUE) {
+ method = "StartDiscovery";
+ } else {
+ method = "StopDiscovery";
+ }
+
+ if (g_dbus_proxy_method_call(default_ctrl->proxy, method,
+ NULL, start_discovery_reply,
+ GUINT_TO_POINTER(enable), NULL) == FALSE) {
+ rl_printf("Failed to %s discovery\n",
+ enable == TRUE ? "start" : "stop");
+ return;
+ }
+}
+
+static void append_variant(DBusMessageIter *iter, int type, void *val)
+{
+ DBusMessageIter value;
+ char sig[2] = { type, '\0' };
+
+ dbus_message_iter_open_container(iter, DBUS_TYPE_VARIANT, sig, &value);
+
+ dbus_message_iter_append_basic(&value, type, val);
+
+ dbus_message_iter_close_container(iter, &value);
+}
+
+static void append_array_variant(DBusMessageIter *iter, int type, void *val,
+ int n_elements)
+{
+ DBusMessageIter variant, array;
+ char type_sig[2] = { type, '\0' };
+ char array_sig[3] = { DBUS_TYPE_ARRAY, type, '\0' };
+
+ dbus_message_iter_open_container(iter, DBUS_TYPE_VARIANT,
+ array_sig, &variant);
+
+ dbus_message_iter_open_container(&variant, DBUS_TYPE_ARRAY,
+ type_sig, &array);
+
+ if (dbus_type_is_fixed(type) == TRUE) {
+ dbus_message_iter_append_fixed_array(&array, type, val,
+ n_elements);
+ } else if (type == DBUS_TYPE_STRING || type == DBUS_TYPE_OBJECT_PATH) {
+ const char ***str_array = val;
+ int i;
+
+ for (i = 0; i < n_elements; i++)
+ dbus_message_iter_append_basic(&array, type,
+ &((*str_array)[i]));
+ }
+
+ dbus_message_iter_close_container(&variant, &array);
+
+ dbus_message_iter_close_container(iter, &variant);
+}
+
+static void dict_append_entry(DBusMessageIter *dict, const char *key,
+ int type, void *val)
+{
+ DBusMessageIter entry;
+
+ if (type == DBUS_TYPE_STRING) {
+ const char *str = *((const char **) val);
+
+ if (str == NULL)
+ return;
+ }
+
+ dbus_message_iter_open_container(dict, DBUS_TYPE_DICT_ENTRY,
+ NULL, &entry);
+
+ dbus_message_iter_append_basic(&entry, DBUS_TYPE_STRING, &key);
+
+ append_variant(&entry, type, val);
+
+ dbus_message_iter_close_container(dict, &entry);
+}
+
+static void dict_append_basic_array(DBusMessageIter *dict, int key_type,
+ const void *key, int type, void *val,
+ int n_elements)
+{
+ DBusMessageIter entry;
+
+ dbus_message_iter_open_container(dict, DBUS_TYPE_DICT_ENTRY,
+ NULL, &entry);
+
+ dbus_message_iter_append_basic(&entry, key_type, key);
+
+ append_array_variant(&entry, type, val, n_elements);
+
+ dbus_message_iter_close_container(dict, &entry);
+}
+
+static void dict_append_array(DBusMessageIter *dict, const char *key, int type,
+ void *val, int n_elements)
+{
+ dict_append_basic_array(dict, DBUS_TYPE_STRING, &key, type, val,
+ n_elements);
+}
+
+#define DISTANCE_VAL_INVALID 0x7FFF
+
+struct set_discovery_filter_args {
+ char *transport;
+ dbus_uint16_t rssi;
+ dbus_int16_t pathloss;
+ char **uuids;
+ size_t uuids_len;
+ dbus_bool_t reset;
+};
+
+static void set_discovery_filter_setup(DBusMessageIter *iter, void *user_data)
+{
+ struct set_discovery_filter_args *args = user_data;
+ DBusMessageIter dict;
+
+ dbus_message_iter_open_container(iter, DBUS_TYPE_ARRAY,
+ DBUS_DICT_ENTRY_BEGIN_CHAR_AS_STRING
+ DBUS_TYPE_STRING_AS_STRING
+ DBUS_TYPE_VARIANT_AS_STRING
+ DBUS_DICT_ENTRY_END_CHAR_AS_STRING, &dict);
+
+ dict_append_array(&dict, "UUIDs", DBUS_TYPE_STRING, &args->uuids,
+ args->uuids_len);
+
+ if (args->pathloss != DISTANCE_VAL_INVALID)
+ dict_append_entry(&dict, "Pathloss", DBUS_TYPE_UINT16,
+ &args->pathloss);
+
+ if (args->rssi != DISTANCE_VAL_INVALID)
+ dict_append_entry(&dict, "RSSI", DBUS_TYPE_INT16, &args->rssi);
+
+ if (args->transport != NULL)
+ dict_append_entry(&dict, "Transport", DBUS_TYPE_STRING,
+ &args->transport);
+ if (args->reset)
+ dict_append_entry(&dict, "ResetData", DBUS_TYPE_BOOLEAN,
+ &args->reset);
+
+ dbus_message_iter_close_container(iter, &dict);
+}
+
+
+static void set_discovery_filter_reply(DBusMessage *message, void *user_data)
+{
+ DBusError error;
+
+ dbus_error_init(&error);
+ if (dbus_set_error_from_message(&error, message) == TRUE) {
+ rl_printf("SetDiscoveryFilter failed: %s\n", error.name);
+ dbus_error_free(&error);
+ return;
+ }
+
+ rl_printf("SetDiscoveryFilter success\n");
+}
+
+static gint filtered_scan_rssi = DISTANCE_VAL_INVALID;
+static gint filtered_scan_pathloss = DISTANCE_VAL_INVALID;
+static char **filtered_scan_uuids;
+static size_t filtered_scan_uuids_len;
+static char *filtered_scan_transport = "le";
+
+static void set_scan_filter_commit(void)
+{
+ struct set_discovery_filter_args args;
+
+ args.pathloss = filtered_scan_pathloss;
+ args.rssi = filtered_scan_rssi;
+ args.transport = filtered_scan_transport;
+ args.uuids = filtered_scan_uuids;
+ args.uuids_len = filtered_scan_uuids_len;
+ args.reset = TRUE;
+
+ if (check_default_ctrl() == FALSE)
+ return;
+
+ if (g_dbus_proxy_method_call(default_ctrl->proxy, "SetDiscoveryFilter",
+ set_discovery_filter_setup, set_discovery_filter_reply,
+ &args, NULL) == FALSE) {
+ rl_printf("Failed to set discovery filter\n");
+ return;
+ }
+}
+
+static void set_scan_filter_uuids(const char *arg)
+{
+ g_strfreev(filtered_scan_uuids);
+ filtered_scan_uuids = NULL;
+ filtered_scan_uuids_len = 0;
+
+ if (!arg || !strlen(arg))
+ goto commit;
+
+ rl_printf("set_scan_filter_uuids %s\n", arg);
+ filtered_scan_uuids = g_strsplit(arg, " ", -1);
+ if (!filtered_scan_uuids) {
+ rl_printf("Failed to parse input\n");
+ return;
+ }
+
+ filtered_scan_uuids_len = g_strv_length(filtered_scan_uuids);
+
+commit:
+ set_scan_filter_commit();
+}
+
+static void cmd_scan_unprovisioned_devices(const char *arg)
+{
+ dbus_bool_t enable;
+
+ if (parse_argument_on_off(arg, &enable) == FALSE)
+ return;
+
+ if (enable == TRUE) {
+ discover_mesh = false;
+ set_scan_filter_uuids(MESH_PROV_SVC_UUID);
+ }
+ cmd_scan(arg);
+}
+
+static void cmd_info(const char *arg)
+{
+ GDBusProxy *proxy;
+ DBusMessageIter iter;
+ const char *address;
+
+ proxy = connection.device;
+ if (!proxy)
+ return;
+
+ if (g_dbus_proxy_get_property(proxy, "Address", &iter) == FALSE)
+ return;
+
+ dbus_message_iter_get_basic(&iter, &address);
+ rl_printf("Device %s\n", address);
+
+ print_property(proxy, "Name");
+ print_property(proxy, "Alias");
+ print_property(proxy, "Class");
+ print_property(proxy, "Appearance");
+ print_property(proxy, "Icon");
+ print_property(proxy, "Trusted");
+ print_property(proxy, "Blocked");
+ print_property(proxy, "Connected");
+ print_uuids(proxy);
+ print_property(proxy, "Modalias");
+ print_property(proxy, "ManufacturerData");
+ print_property(proxy, "ServiceData");
+ print_property(proxy, "RSSI");
+ print_property(proxy, "TxPower");
+}
+
+static void cmd_connect(const char *arg)
+{
+ if (check_default_ctrl() == FALSE)
+ return;
+
+ memset(&connection, 0, sizeof(connection));
+
+ if (!arg || !strlen(arg)) {
+ connection.net_idx = NET_IDX_PRIMARY;
+ } else {
+ char *end;
+ connection.net_idx = strtol(arg, &end, 16);
+ if (end == arg) {
+ connection.net_idx = NET_IDX_INVALID;
+ rl_printf("Invalid network index %s\n", arg);
+ return;
+ }
+ }
+
+ if (discovering)
+ g_dbus_proxy_method_call(default_ctrl->proxy, "StopDiscovery",
+ NULL, NULL, NULL, NULL);
+
+ set_scan_filter_uuids(MESH_PROXY_SVC_UUID);
+ discover_mesh = true;
+
+ connection.type = CONN_TYPE_NETWORK;
+
+
+ rl_printf("Looking for mesh network with net index %4.4x\n",
+ connection.net_idx);
+
+ if (g_dbus_proxy_method_call(default_ctrl->proxy,
+ "StartDiscovery", NULL, start_discovery_reply,
+ GUINT_TO_POINTER(TRUE), NULL) == FALSE)
+ rl_printf("Failed to start mesh proxy discovery\n");
+
+ g_dbus_proxy_method_call(default_ctrl->proxy, "StartDiscovery",
+ NULL, NULL, NULL, NULL);
+
+}
+
+static void prov_disconn_reply(DBusMessage *message, void *user_data)
+{
+ struct mesh_node *node = user_data;
+ DBusError error;
+
+ dbus_error_init(&error);
+
+ if (dbus_set_error_from_message(&error, message) == TRUE) {
+ rl_printf("Failed to disconnect: %s\n", error.name);
+ dbus_error_free(&error);
+ return;
+ }
+
+ set_connected_device(NULL);
+
+ set_scan_filter_uuids(MESH_PROXY_SVC_UUID);
+ discover_mesh = true;
+
+ connection.type = CONN_TYPE_IDENTITY;
+ connection.data_in = NULL;
+ connection.data_out = NULL;
+ connection.unicast = node_get_primary(node);
+
+ if (g_dbus_proxy_method_call(default_ctrl->proxy,
+ "StartDiscovery", NULL, start_discovery_reply,
+ GUINT_TO_POINTER(TRUE), NULL) == FALSE)
+ rl_printf("Failed to start mesh proxy discovery\n");
+
+}
+
+static void disconn_reply(DBusMessage *message, void *user_data)
+{
+ GDBusProxy *proxy = user_data;
+ DBusError error;
+
+ dbus_error_init(&error);
+
+ if (dbus_set_error_from_message(&error, message) == TRUE) {
+ rl_printf("Failed to disconnect: %s\n", error.name);
+ dbus_error_free(&error);
+ return;
+ }
+
+ rl_printf("Successfully disconnected\n");
+
+ if (proxy != connection.device)
+ return;
+
+ set_connected_device(NULL);
+}
+
+static void cmd_disconn(const char *arg)
+{
+ if (connection.type == CONN_TYPE_PROVISION) {
+ struct mesh_node *node = node_find_by_uuid(connection.dev_uuid);
+ if (node)
+ node_free(node);
+ }
+
+ disconnect_device(disconn_reply, connection.device);
+}
+
+static void mesh_prov_done(void *user_data, int status)
+{
+ struct mesh_node *node = user_data;
+
+ if (status){
+ rl_printf("Provisioning failed\n");
+ node_free(node);
+ disconnect_device(NULL, NULL);
+ return;
+ }
+
+ rl_printf("Provision success. Assigned Primary Unicast %4.4x\n",
+ node_get_primary(node));
+
+ if (!prov_db_add_new_node(node))
+ rl_printf("Failed to add node to provisioning DB\n");
+
+ disconnect_device(prov_disconn_reply, node);
+}
+
+static void cmd_start_prov(const char *arg)
+{
+ GDBusProxy *proxy;
+ struct mesh_device *dev;
+ struct mesh_node *node;
+ int len;
+
+ if (!arg) {
+ rl_printf("Mesh Device UUID is required\n");
+ return;
+ }
+
+ len = strlen(arg);
+ if ( len > 32 || len % 2) {
+ rl_printf("Incorrect UUID size %d\n", len);
+ }
+
+ disconnect_device(NULL, NULL);
+
+ memset(connection.dev_uuid, 0, 16);
+ str2hex(arg, len, connection.dev_uuid, len/2);
+
+ node = node_find_by_uuid(connection.dev_uuid);
+ if (!node) {
+ rl_printf("Device with UUID %s not found.\n", arg);
+ rl_printf("Stale services? Remove device and re-discover\n");
+ return;
+ }
+
+ /* TODO: add command to remove a node from mesh, i.e., "unprovision" */
+ if (node_is_provisioned(node)) {
+ rl_printf("Already provisioned with unicast %4.4x\n",
+ node_get_primary(node));
+ return;
+ }
+
+ dev = find_device_by_uuid(default_ctrl->mesh_devices,
+ connection.dev_uuid);
+ if (!dev || !dev->proxy) {
+ rl_printf("Could not find device proxy\n");
+ memset(connection.dev_uuid, 0, 16);
+ return;
+ }
+
+ proxy = dev->proxy;
+ if (discovering)
+ g_dbus_proxy_method_call(default_ctrl->proxy, "StopDiscovery",
+ NULL, NULL, NULL, NULL);
+ forget_mesh_devices();
+
+ connection.type = CONN_TYPE_PROVISION;
+
+ if (g_dbus_proxy_method_call(proxy, "Connect", NULL, connect_reply,
+ proxy, NULL) == FALSE) {
+ rl_printf("Failed to connect ");
+ print_device(proxy, NULL);
+ return;
+ } else {
+ rl_printf("Trying to connect ");
+ print_device(proxy, NULL);
+ }
+
+}
+
+static void cmd_config(const char *arg)
+{
+ rl_printf("Switching to Mesh Client configuration menu\n");
+
+ if (!switch_cmd_menu("configure"))
+ return;
+
+ set_menu_prompt("config", NULL);
+
+ if (arg && strlen(arg))
+ config_set_node(arg);
+}
+
+static void cmd_onoff_cli(const char *arg)
+{
+ rl_printf("Switching to Mesh Generic ON OFF Client menu\n");
+
+ if (!switch_cmd_menu("onoff"))
+ return;
+
+ set_menu_prompt("on/off", NULL);
+
+ if (arg && strlen(arg))
+ onoff_set_node(arg);
+}
+
+static void cmd_print_mesh(const char *arg)
+{
+ if (!prov_db_show(mesh_prov_db_filename))
+ rl_printf("Unavailable\n");
+
+}
+
+ static void cmd_print_local(const char *arg)
+{
+ if (!prov_db_show(mesh_local_config_filename))
+ rl_printf("Unavailable\n");
+}
+
+static void disc_quit_cb(DBusMessage *message, void *user_data)
+{
+ g_main_loop_quit(main_loop);
+}
+
+static void cmd_quit(const char *arg)
+{
+ if (connection.device) {
+ disconnect_device(disc_quit_cb, NULL);
+ return;
+ }
+
+ g_main_loop_quit(main_loop);
+}
+
+static const struct menu_entry meshctl_cmd_table[] = {
+ { "list", NULL, cmd_list, "List available controllers"},
+ { "show", "[ctrl]", cmd_show, "Controller information"},
+ { "select", "<ctrl>", cmd_select, "Select default controller"},
+ { "info", "[dev]", cmd_info, "Device information"},
+ { "connect", "[net_idx]",cmd_connect, "Connect to mesh network"},
+ { "discover-unprovisioned", "<on/off>", cmd_scan_unprovisioned_devices,
+ "Look for devices to provision" },
+ { "provision", "<uuid>", cmd_start_prov, "Initiate provisioning"},
+ { "power", "<on/off>", cmd_power, "Set controller power" },
+ { "disconnect", "[dev]", cmd_disconn, "Disconnect device"},
+ { "mesh-info", NULL, cmd_print_mesh,
+ "Mesh networkinfo (provisioner)" },
+ { "local-info", NULL, cmd_print_local, "Local mesh node info" },
+ { "configure", "[dst]", cmd_config, "Config client model menu"},
+ { "onoff", "[dst]", cmd_onoff_cli,
+ "Generic On/Off model menu"},
+ { "quit", NULL, cmd_quit, "Quit program" },
+ { "exit", NULL, cmd_quit },
+ { "help" },
+ { }
+};
+
+static void rl_handler(char *input)
+{
+ char *cmd, *arg;
+
+ if (!input) {
+ rl_insert_text("quit");
+ rl_redisplay();
+ rl_crlf();
+ g_main_loop_quit(main_loop);
+ return;
+ }
+
+ if (!strlen(input))
+ goto done;
+ else if (!strcmp(input, "q") || !strcmp(input, "quit")
+ || !strcmp(input, "exit")) {
+ cmd_quit(NULL);
+ goto done;
+ }
+
+ if (agent_input(input) == TRUE)
+ goto done;
+
+ add_history(input);
+
+ cmd = strtok_r(input, " \t\r\n", &arg);
+ if (!cmd)
+ goto done;
+
+ process_menu_cmd(cmd, arg);
+
+done:
+ free(input);
+}
+
+static gboolean signal_handler(GIOChannel *channel, GIOCondition condition,
+ gpointer user_data)
+{
+ static bool terminated = false;
+ struct signalfd_siginfo si;
+ ssize_t result;
+ int fd;
+
+ if (condition & (G_IO_NVAL | G_IO_ERR | G_IO_HUP)) {
+ g_main_loop_quit(main_loop);
+ return FALSE;
+ }
+
+ fd = g_io_channel_unix_get_fd(channel);
+
+ result = read(fd, &si, sizeof(si));
+ if (result != sizeof(si))
+ return FALSE;
+
+ switch (si.ssi_signo) {
+ case SIGINT:
+ if (input) {
+ rl_replace_line("", 0);
+ rl_crlf();
+ rl_on_new_line();
+ rl_redisplay();
+ break;
+ }
+
+ /*
+ * If input was not yet setup up that means signal was received
+ * while daemon was not yet running. Since user is not able
+ * to terminate client by CTRL-D or typing exit treat this as
+ * exit and fall through.
+ */
+
+ /* fall through */
+ case SIGTERM:
+ if (!terminated) {
+ rl_replace_line("", 0);
+ rl_crlf();
+ g_main_loop_quit(main_loop);
+ }
+
+ terminated = true;
+ break;
+ }
+
+ return TRUE;
+}
+
+static guint setup_signalfd(void)
+{
+ GIOChannel *channel;
+ guint source;
+ sigset_t mask;
+ int fd;
+
+ sigemptyset(&mask);
+ sigaddset(&mask, SIGINT);
+ sigaddset(&mask, SIGTERM);
+
+ if (sigprocmask(SIG_BLOCK, &mask, NULL) < 0) {
+ perror("Failed to set signal mask");
+ return 0;
+ }
+
+ fd = signalfd(-1, &mask, 0);
+ if (fd < 0) {
+ perror("Failed to create signal descriptor");
+ return 0;
+ }
+
+ channel = g_io_channel_unix_new(fd);
+
+ g_io_channel_set_close_on_unref(channel, TRUE);
+ g_io_channel_set_encoding(channel, NULL, NULL);
+ g_io_channel_set_buffered(channel, FALSE);
+
+ source = g_io_add_watch(channel,
+ G_IO_IN | G_IO_HUP | G_IO_ERR | G_IO_NVAL,
+ signal_handler, NULL);
+
+ g_io_channel_unref(channel);
+
+ return source;
+}
+
+static gboolean option_version = FALSE;
+static const char *mesh_config_dir;
+
+static GOptionEntry options[] = {
+ { "config", 'c', 0, G_OPTION_ARG_STRING, &mesh_config_dir,
+ "Read local mesh config JSON files from <directory>" },
+ { "version", 'v', 0, G_OPTION_ARG_NONE, &option_version,
+ "Show version information and exit" },
+ { NULL },
+};
+
+static void client_ready(GDBusClient *client, void *user_data)
+{
+ if (!input)
+ input = setup_standard_input();
+}
+
+int main(int argc, char *argv[])
+{
+ GOptionContext *context;
+ GError *error = NULL;
+ GDBusClient *client;
+ guint signal;
+ int len;
+ int extra;
+
+ context = g_option_context_new(NULL);
+ g_option_context_add_main_entries(context, options, NULL);
+
+ if (g_option_context_parse(context, &argc, &argv, &error) == FALSE) {
+ if (error != NULL) {
+ g_printerr("%s\n", error->message);
+ g_error_free(error);
+ } else
+ g_printerr("An unknown error occurred\n");
+ exit(1);
+ }
+
+ g_option_context_free(context);
+
+ if (option_version == TRUE) {
+ rl_printf("%s\n", VERSION);
+ exit(0);
+ }
+
+ if (!mesh_config_dir) {
+ rl_printf("Local config directory not provided.\n");
+ mesh_config_dir = "";
+ } else {
+ rl_printf("Reading prov_db.json and local_node.json from %s\n",
+ mesh_config_dir);
+ }
+
+ len = strlen(mesh_config_dir);
+ if (len && mesh_config_dir[len - 1] != '/') {
+ extra = 1;
+ rl_printf("mesh_config_dir[%d] %s\n", len,
+ &mesh_config_dir[len - 1]);
+ } else {
+ extra = 0;
+ }
+ mesh_local_config_filename = g_malloc(len + strlen("local_node.json")
+ + 2);
+ if (!mesh_local_config_filename)
+ exit(1);
+
+ mesh_prov_db_filename = g_malloc(len + strlen("prov_db.json") + 2);
+ if (!mesh_prov_db_filename) {
+ exit(1);
+ }
+
+ sprintf(mesh_local_config_filename, "%s", mesh_config_dir);
+
+ if (extra)
+ sprintf(mesh_local_config_filename + len , "%c", '/');
+
+ sprintf(mesh_local_config_filename + len + extra, "%s",
+ "local_node.json");
+ len = len + extra + strlen("local_node.json");
+ sprintf(mesh_local_config_filename + len, "%c", '\0');
+
+ if (!prov_db_read_local_node(mesh_local_config_filename, true)) {
+ g_printerr("Failed to parse local node configuration file %s\n",
+ mesh_local_config_filename);
+ exit(1);
+ }
+
+ sprintf(mesh_prov_db_filename, "%s", mesh_config_dir);
+ len = strlen(mesh_config_dir);
+ if (extra)
+ sprintf(mesh_prov_db_filename + len , "%c", '/');
+
+ sprintf(mesh_prov_db_filename + len + extra, "%s", "prov_db.json");
+ sprintf(mesh_prov_db_filename + len + extra + strlen("prov_db.json"),
+ "%c", '\0');
+
+ if (!prov_db_read(mesh_prov_db_filename)) {
+ g_printerr("Failed to parse provisioning database file %s\n",
+ mesh_prov_db_filename);
+ exit(1);
+ }
+
+ main_loop = g_main_loop_new(NULL, FALSE);
+ dbus_conn = g_dbus_setup_bus(DBUS_BUS_SYSTEM, NULL, NULL);
+
+ setlinebuf(stdout);
+
+ rl_erase_empty_line = 1;
+ rl_callback_handler_install(NULL, rl_handler);
+
+ rl_set_prompt(PROMPT_OFF);
+ rl_redisplay();
+
+ signal = setup_signalfd();
+ client = g_dbus_client_new(dbus_conn, "org.bluez", "/org/bluez");
+
+ g_dbus_client_set_connect_watch(client, connect_handler, NULL);
+ g_dbus_client_set_disconnect_watch(client, disconnect_handler, NULL);
+ g_dbus_client_set_signal_watch(client, message_handler, NULL);
+
+ g_dbus_client_set_proxy_handlers(client, proxy_added, proxy_removed,
+ property_changed, NULL);
+
+ g_dbus_client_set_ready_watch(client, client_ready, NULL);
+
+ cmd_menu_init(meshctl_cmd_table);
+
+ if (!config_client_init())
+ g_printerr("Failed to initialize mesh configuration client\n");
+
+ if (!config_server_init())
+ g_printerr("Failed to initialize mesh configuration server\n");
+
+ if (!onoff_client_init(PRIMARY_ELEMENT_IDX))
+ g_printerr("Failed to initialize mesh generic On/Off client\n");
+
+ g_main_loop_run(main_loop);
+
+ g_dbus_client_unref(client);
+ g_source_remove(signal);
+ if (input > 0)
+ g_source_remove(input);
+
+ rl_message("");
+ rl_callback_handler_remove();
+
+ dbus_connection_unref(dbus_conn);
+ g_main_loop_unref(main_loop);
+
+ node_cleanup();
+
+ g_list_free(char_list);
+ g_list_free(service_list);
+ g_list_free_full(ctrl_list, proxy_leak);
+
+ agent_release();
+
+ return 0;
+}
diff --git a/mesh/net.c b/mesh/net.c
new file mode 100644
index 0000000..fb2d200
--- /dev/null
+++ b/mesh/net.c
+/*
+ *
+ * BlueZ - Bluetooth protocol stack for Linux
+ *
+ * Copyright (C) 2017 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.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <inttypes.h>
+#include <ctype.h>
+#include <stdbool.h>
+#include <stdio.h>
+#include <string.h>
+#include <glib.h>
+
+#include "src/shared/util.h"
+#include "client/display.h"
+
+#include "crypto.h"
+#include "gatt.h"
+#include "mesh-net.h"
+#include "util.h"
+#include "keys.h"
+#include "node.h"
+#include "prov-db.h"
+#include "net.h"
+
+struct address_range
+{
+ uint16_t min;
+ uint16_t max;
+};
+
+struct mesh_net {
+ uint32_t iv_index;
+ uint32_t seq_num;
+ uint32_t seq_num_reserved;
+ uint16_t primary_addr;
+ uint8_t iv_upd_state;
+ uint8_t num_elements;
+ uint8_t default_ttl;
+ bool iv_update;
+ bool provisioner;
+ bool blacklist;
+ guint iv_update_timeout;
+ GDBusProxy *proxy_in;
+ GList *address_pool;
+ GList *dest; /* List of valid local destinations for Whitelist */
+ GList *sar_in; /* Incoming segmented messages in progress */
+ GList *msg_out; /* Pre-Network encoded, might be multi-segment */
+ GList *pkt_out; /* Fully encoded packets awaiting Tx in order */
+ net_mesh_session_open_callback open_cb;
+};
+
+struct generic_key {
+ uint16_t idx;
+};
+
+struct net_key_parts {
+ uint8_t nid;
+ uint8_t enc_key[16];
+ uint8_t privacy_key[16];
+ uint8_t net_key[16];
+ uint8_t beacon_key[16];
+ uint8_t net_id[8];
+};
+
+struct mesh_net_key {
+ struct generic_key generic;
+ uint8_t phase;
+ struct net_key_parts current;
+ struct net_key_parts new;
+};
+
+struct app_key_parts {
+ uint8_t key[16];
+ uint8_t akf_aid;
+};
+
+struct mesh_app_key {
+ struct generic_key generic;
+ uint16_t net_idx;
+ struct app_key_parts current;
+ struct app_key_parts new;
+};
+
+struct mesh_virt_addr {
+ uint16_t va16;
+ uint32_t va32;
+ uint8_t va128[16];
+};
+
+struct mesh_pkt {
+ uint8_t data[30];
+ uint8_t len;
+};
+
+struct mesh_sar_msg {
+ guint ack_to;
+ guint msg_to;
+ uint32_t iv_index;
+ uint32_t seqAuth;
+ uint32_t ack;
+ uint32_t dst;
+ uint16_t src;
+ uint16_t net_idx;
+ uint16_t len;
+ uint8_t akf_aid;
+ uint8_t ttl;
+ uint8_t segN;
+ uint8_t activity_cnt;
+ bool ctl;
+ bool segmented;
+ bool szmic;
+ bool proxy;
+ uint8_t data[20]; /* Open ended, min 20 */
+};
+
+struct mesh_destination {
+ uint16_t cnt;
+ uint16_t dst;
+};
+
+/* Network Packet Layer based Offsets */
+#define AKF_BIT 0x40
+
+#define PKT_IVI(p) !!((p)[0] & 0x80)
+#define SET_PKT_IVI(p,v) do {(p)[0] &= 0x7f; \
+ (p)[0] |= ((v) ? 0x80 : 0);} while(0)
+#define PKT_NID(p) ((p)[0] & 0x7f)
+#define SET_PKT_NID(p,v) do {(p)[0] &= 0x80; (p)[0] |= (v);} while(0)
+#define PKT_CTL(p) (!!((p)[1] & 0x80))
+#define SET_PKT_CTL(p,v) do {(p)[1] &= 0x7f; \
+ (p)[1] |= ((v) ? 0x80 : 0);} while(0)
+#define PKT_TTL(p) ((p)[1] & 0x7f)
+#define SET_PKT_TTL(p,v) do {(p)[1] &= 0x80; (p)[1] |= (v);} while(0)
+#define PKT_SEQ(p) (get_be32((p) + 1) & 0xffffff)
+#define SET_PKT_SEQ(p,v) put_be32(((p)[1] << 24) + ((v) & 0xffffff), \
+ (p) + 1)
+#define PKT_SRC(p) get_be16((p) + 5)
+#define SET_PKT_SRC(p,v) put_be16(v, (p) + 5)
+#define PKT_DST(p) get_be16((p) + 7)
+#define SET_PKT_DST(p,v) put_be16(v, (p) + 7)
+#define PKT_TRANS(p) ((p) + 9)
+#define PKT_TRANS_LEN(l) ((l) - 9)
+
+#define PKT_SEGMENTED(p) (!!((p)[9] & 0x80))
+#define SET_PKT_SEGMENTED(p,v) do {(p)[9] &= 0x7f; \
+ (p)[9] |= ((v) ? 0x80 : 0);} while(0)
+#define PKT_AKF_AID(p) ((p)[9] & 0x7f)
+#define SET_PKT_AKF_AID(p,v) do {(p)[9] &= 0x80; (p)[9] |= (v);} while(0)
+#define PKT_OPCODE(p) ((p)[9] & 0x7f)
+#define SET_PKT_OPCODE(p,v) do {(p)[9] &= 0x80; (p)[9] |= (v);} while(0)
+#define PKT_OBO(p) (!!((p)[10] & 0x80))
+#define PKT_SZMIC(p) (!!(PKT_SEGMENTED(p) ? ((p)[10] & 0x40) : 0))
+#define SET_PKT_SZMIC(p,v) do {(p)[10] &= 0x7f; \
+ (p)[10] |= ((v) ? 0x80 : 0);} while(0)
+#define PKT_SEQ0(p) ((get_be16((p) + 10) >> 2) & 0x1fff)
+#define SET_PKT_SEQ0(p,v) do {put_be16((get_be16((p) + 10) & 0x8003) \
+ | (((v) & 0x1fff) << 2), \
+ (p) + 10);} while(0)
+#define SET_PKT_SEGO(p,v) do {put_be16((get_be16( \
+ (p) + 11) & 0xfc1f) | ((v) << 5), \
+ (p) + 11);} while(0)
+#define SET_PKT_SEGN(p,v) do {(p)[12] = ((p)[12] & 0xe0) | (v);} while(0)
+#define PKT_ACK(p) (get_be32((p) + 12))
+#define SET_PKT_ACK(p,v) (put_be32((v)(p) + 12))
+
+/* Transport Layer based offsets */
+#define TRANS_SEGMENTED(t) (!!((t)[0] & 0x80))
+#define SET_TRANS_SEGMENTD(t,v) do {(t)[0] &= 0x7f; \
+ (t)[0] |= ((v) ? 0x80 : 0);} while(0)
+#define TRANS_OPCODE(t) ((t)[0] & 0x7f)
+#define SET_TRANS_OPCODE(t,v) do {(t)[0] &= 0x80; (t)[0] |= (v);} while(0)
+#define TRANS_AKF_AID(t) ((t)[0] & 0x7f)
+#define SET_TRANS_AKF_AID(t,v) do {(t)[0] &= 0xc0; (t)[0] |= (v);} while(0)
+#define TRANS_AKF(t) (!!((t)[0] & AKF_BIT))
+#define TRANS_SZMIC(t) (!!(TRANS_SEGMENTED(t) ? ((t)[1] & 0x80) : 0))
+#define TRANS_SEQ0(t) ((get_be16((t) + 1) >> 2) & 0x1fff)
+#define SET_TRANS_SEQ0(t,v) do {put_be16((get_be16((t) + 1) & 0x8003) \
+ | (((v) & 0x1fff) << 2), \
+ (t) + 1);} while(0)
+#define SET_TRANS_ACK(t,v) put_be32((v), (t) + 3)
+#define TRANS_SEGO(t) ((get_be16((t) + 2) >> 5) & 0x1f)
+#define TRANS_SEGN(t) ((t)[3] & 0x1f)
+
+#define TRANS_PAYLOAD(t) ((t) + (TRANS_SEGMENTED(t) ? 4 : 1))
+#define TRANS_LEN(t,l) ((l) -(TRANS_SEGMENTED(t) ? 4 : 1))
+
+/* Proxy Config Opcodes */
+#define FILTER_SETUP 0x00
+#define FILTER_ADD 0x01
+#define FILTER_DEL 0x02
+#define FILTER_STATUS 0x03
+
+/* Proxy Filter Types */
+#define WHITELIST_FILTER 0x00
+#define BLACKLIST_FILTER 0x01
+
+/* IV Updating states for timing enforcement */
+#define IV_UPD_INIT 0
+#define IV_UPD_NORMAL 1
+#define IV_UPD_UPDATING 2
+#define IV_UPD_NORMAL_HOLD 3
+
+#define IV_IDX_DIFF_RANGE 42
+
+static struct mesh_net net;
+static GList *virt_addrs = NULL;
+static GList *net_keys = NULL;
+static GList *app_keys = NULL;
+
+/* Forward static declarations */
+static void resend_segs(struct mesh_sar_msg *sar);
+
+static int match_net_id(const void *a, const void *net_id)
+{
+ const struct mesh_net_key *net_key = a;
+
+ if (net_key->current.nid != 0xff &&
+ !memcmp(net_key->current.net_id, net_id, 8))
+ return 0;
+
+ if (net_key->new.nid != 0xff &&
+ !memcmp(net_key->new.net_id, net_id, 8))
+ return 0;
+
+ return -1;
+}
+
+static struct mesh_net_key *find_net_key_by_id(const uint8_t *net_id)
+{
+ GList *l;
+
+ l = g_list_find_custom(net_keys, net_id, match_net_id);
+
+ if (!l)
+ return NULL;
+
+ return l->data;
+}
+
+uint16_t net_validate_proxy_beacon(const uint8_t *proxy_beacon)
+{
+ struct mesh_net_key *net_key = find_net_key_by_id(proxy_beacon);
+
+ if (net_key == NULL)
+ return NET_IDX_INVALID;
+
+ return net_key->generic.idx;
+}
+
+static int match_sar_dst(const void *a, const void *b)
+{
+ const struct mesh_sar_msg *sar = a;
+ uint16_t dst = GPOINTER_TO_UINT(b);
+
+ return (sar->dst == dst) ? 0 : -1;
+}
+
+static struct mesh_sar_msg *find_sar_out_by_dst(uint16_t dst)
+{
+ GList *l;
+
+ l = g_list_find_custom(net.msg_out, GUINT_TO_POINTER(dst),
+ match_sar_dst);
+
+ if (!l)
+ return NULL;
+
+ return l->data;
+}
+
+static int match_sar_src(const void *a, const void *b)
+{
+ const struct mesh_sar_msg *sar = a;
+ uint16_t src = GPOINTER_TO_UINT(b);
+
+ return (sar->src == src) ? 0 : -1;
+}
+
+static struct mesh_sar_msg *find_sar_in_by_src(uint16_t src)
+{
+ GList *l;
+
+ l = g_list_find_custom(net.sar_in, GUINT_TO_POINTER(src),
+ match_sar_src);
+
+ if (!l)
+ return NULL;
+
+ return l->data;
+}
+
+static int match_key_index(const void *a, const void *b)
+{
+ const struct generic_key *generic = a;
+ uint16_t index = GPOINTER_TO_UINT(b);
+
+ return (generic->idx == index) ? 0 : -1;
+}
+
+static bool delete_key(GList **list, uint16_t index)
+{
+ GList *l;
+
+ l = g_list_find_custom(*list, GUINT_TO_POINTER(index),
+ match_key_index);
+
+ if (!l)
+ return false;
+
+ *list = g_list_delete_link(*list, l);
+
+ return true;
+
+}
+
+static uint8_t *get_key(GList *list, uint16_t index)
+{
+ GList *l;
+ struct mesh_app_key *app_key;
+ struct mesh_net_key *net_key;
+
+ l = g_list_find_custom(list, GUINT_TO_POINTER(index),
+ match_key_index);
+
+ if (!l) return NULL;
+
+ if (list == app_keys) {
+ app_key = l->data;
+
+ /* All App Keys must belong to a valid Net Key */
+ l = g_list_find_custom(net_keys,
+ GUINT_TO_POINTER(app_key->net_idx),
+ match_key_index);
+
+ if (!l) return NULL;
+
+ net_key = l->data;
+
+ if (net_key->phase == 2 && app_key->new.akf_aid != 0xff)
+ return app_key->new.key;
+
+ if (app_key->current.akf_aid != 0xff)
+ return app_key->current.key;
+
+ return NULL;
+ }
+
+ net_key = l->data;
+
+ if (net_key->phase == 2 && net_key->new.nid != 0xff)
+ return net_key->new.net_key;
+
+ if (net_key->current.nid != 0xff)
+ return net_key->current.net_key;
+
+ return NULL;
+}
+
+bool keys_app_key_add(uint16_t net_idx, uint16_t app_idx, uint8_t *key,
+ bool update)
+{
+ struct mesh_app_key *app_key = NULL;
+ uint8_t akf_aid;
+ GList *l = g_list_find_custom(app_keys, GUINT_TO_POINTER(app_idx),
+ match_key_index);
+
+ if (!mesh_crypto_k4(key, &akf_aid))
+ return false;
+
+ akf_aid |= AKF_BIT;
+
+ if (l && update) {
+
+ app_key = l->data;
+
+ if (app_key->net_idx != net_idx)
+ return false;
+
+ memcpy(app_key->new.key, key, 16);
+ app_key->new.akf_aid = akf_aid;
+
+ } else if (l) {
+
+ app_key = l->data;
+
+ if (memcmp(app_key->current.key, key, 16) ||
+ app_key->net_idx != net_idx)
+ return false;
+
+ } else {
+
+ app_key = g_new(struct mesh_app_key, 1);
+ memcpy(app_key->current.key, key, 16);
+ app_key->net_idx = net_idx;
+ app_key->generic.idx = app_idx;
+ app_key->current.akf_aid = akf_aid;
+
+ /* Invalidate "New" version */
+ app_key->new.akf_aid = 0xff;
+
+ app_keys = g_list_append(app_keys, app_key);
+
+ }
+
+ return true;
+}
+
+bool keys_net_key_add(uint16_t net_idx, uint8_t *key, bool update)
+{
+ struct mesh_net_key *net_key = NULL;
+ uint8_t p = 0;
+ GList *l = g_list_find_custom(net_keys, GUINT_TO_POINTER(net_idx),
+ match_key_index);
+
+ if (l && update) {
+ bool result;
+
+ net_key = l->data;
+
+ memcpy(net_key->new.net_key, key, 16);
+
+ /* Calculate the many component parts */
+ result = mesh_crypto_nkbk(key, net_key->new.beacon_key);
+ if (!result)
+ return false;
+
+ result = mesh_crypto_k3(key, net_key->new.net_id);
+ if (!result)
+ return false;
+
+ result = mesh_crypto_k2(key, &p, 1,
+ &net_key->new.nid,
+ net_key->new.enc_key,
+ net_key->new.privacy_key);
+ if (!result)
+ net_key->new.nid = 0xff;
+
+ return result;
+
+ } else if (l) {
+ net_key = l->data;
+
+ if (memcmp(net_key->current.net_key, key, 16))
+ return false;
+ } else {
+ bool result;
+
+ net_key = g_new(struct mesh_net_key, 1);
+ memcpy(net_key->current.net_key, key, 16);
+ net_key->generic.idx = net_idx;
+
+ /* Invalidate "New" version */
+ net_key->new.nid = 0xff;
+
+ /* Calculate the many component parts */
+ result = mesh_crypto_nkbk(key, net_key->current.beacon_key);
+ if (!result) {
+ g_free(net_key);
+ return false;
+ }
+
+ result = mesh_crypto_k3(key, net_key->current.net_id);
+ if (!result) {
+ g_free(net_key);
+ return false;
+ }
+
+ result = mesh_crypto_k2(key, &p, 1,
+ &net_key->current.nid,
+ net_key->current.enc_key,
+ net_key->current.privacy_key);
+
+ if (!result) {
+ g_free(net_key);
+ return false;
+ }
+
+ net_keys = g_list_append(net_keys, net_key);
+ }
+
+ return true;
+}
+
+static struct mesh_app_key *find_app_key_by_idx(uint16_t app_idx)
+{
+ GList *l;
+
+ l = g_list_find_custom(app_keys, GUINT_TO_POINTER(app_idx),
+ match_key_index);
+
+ if (!l) return NULL;
+
+ return l->data;
+}
+
+static struct mesh_net_key *find_net_key_by_idx(uint16_t net_idx)
+{
+ GList *l;
+
+ l = g_list_find_custom(net_keys, GUINT_TO_POINTER(net_idx),
+ match_key_index);
+
+ if (!l) return NULL;
+
+ return l->data;
+}
+
+static int match_virt_dst(const void *a, const void *b)
+{
+ const struct mesh_virt_addr *virt = a;
+ uint32_t dst = GPOINTER_TO_UINT(b);
+
+ if (dst < 0x10000 && dst == virt->va16)
+ return 0;
+
+ if (dst == virt->va32)
+ return 0;
+
+ return -1;
+}
+
+static struct mesh_virt_addr *find_virt_by_dst(uint32_t dst)
+{
+ GList *l;
+
+ l = g_list_find_custom(virt_addrs, GUINT_TO_POINTER(dst),
+ match_virt_dst);
+
+ if (!l) return NULL;
+
+ return l->data;
+}
+
+uint8_t *keys_net_key_get(uint16_t net_idx, bool current)
+{
+ GList *l;
+
+
+ l = g_list_find_custom(net_keys, GUINT_TO_POINTER(net_idx),
+ match_key_index);
+ if (!l) {
+ return NULL;
+ } else {
+ struct mesh_net_key *key = l->data;
+ if (current)
+ return key->current.net_key;
+ else
+ return key->new.net_key;
+ }
+}
+
+bool keys_app_key_delete(uint16_t app_idx)
+{
+ /* TODO: remove all associated bindings */
+ return delete_key(&app_keys, app_idx);
+}
+
+bool keys_net_key_delete(uint16_t net_idx)
+{
+ /* TODO: remove all associated app keys and bindings */
+ return delete_key(&net_keys, net_idx);
+}
+
+uint8_t keys_get_kr_phase(uint16_t net_idx)
+{
+ GList *l;
+ struct mesh_net_key *key;
+
+ l = g_list_find_custom(net_keys, GUINT_TO_POINTER(net_idx),
+ match_key_index);
+
+ if (!l)
+ return KR_PHASE_INVALID;
+
+ key = l->data;
+
+ return key->phase;
+}
+
+bool keys_set_kr_phase(uint16_t index, uint8_t phase)
+{
+ GList *l;
+ struct mesh_net_key *net_key;
+
+ l = g_list_find_custom(net_keys, GUINT_TO_POINTER(index),
+ match_key_index);
+
+ if (!l)
+ return false;
+
+ net_key = l->data;
+ net_key->phase = phase;
+
+ return true;
+}
+
+uint16_t keys_app_key_get_bound(uint16_t app_idx)
+{
+ GList *l;
+
+ l = g_list_find_custom(app_keys, GUINT_TO_POINTER(app_idx),
+ match_key_index);
+ if (!l)
+ return NET_IDX_INVALID;
+ else {
+ struct mesh_app_key *key = l->data;
+ return key->net_idx;
+ }
+}
+
+uint8_t *keys_app_key_get(uint16_t app_idx, bool current)
+{
+ GList *l;
+
+
+ l = g_list_find_custom(app_keys, GUINT_TO_POINTER(app_idx),
+ match_key_index);
+ if (!l) {
+ return NULL;
+ } else {
+ struct mesh_app_key *key = l->data;
+ if (current)
+ return key->current.key;
+ else
+ return key->new.key;
+ }
+}
+
+void keys_cleanup_all(void)
+{
+ g_list_free_full(app_keys, g_free);
+ g_list_free_full(net_keys, g_free);
+ app_keys = net_keys = NULL;
+}
+
+bool net_get_key(uint16_t net_idx, uint8_t *key)
+{
+ uint8_t *buf;
+
+ buf = get_key(net_keys, net_idx);
+
+ if (!buf)
+ return false;
+
+ memcpy(key, buf, 16);
+ return true;
+}
+
+bool net_get_flags(uint16_t net_idx, uint8_t *out_flags)
+{
+ uint8_t phase;
+
+ phase = keys_get_kr_phase(net_idx);
+
+ if (phase == KR_PHASE_INVALID || !out_flags)
+ return false;
+
+ if (phase != KR_PHASE_NONE)
+ *out_flags = 0x01;
+ else
+ *out_flags = 0x00;
+
+ if (net.iv_update)
+ *out_flags |= 0x02;
+
+ return true;
+}
+
+uint32_t net_get_iv_index(bool *update)
+{
+ if (update)
+ *update = net.iv_update;
+
+ return net.iv_index;
+}
+
+void net_set_iv_index(uint32_t iv_index, bool update)
+{
+ net.iv_index = iv_index;
+ net.iv_update = update;
+}
+
+void set_sequence_number(uint32_t seq_num)
+{
+ net.seq_num = seq_num;
+}
+
+uint32_t get_sequence_number(void)
+{
+ return net.seq_num;
+}
+
+bool net_add_address_pool(uint16_t min, uint16_t max)
+{
+ uint32_t range;
+ if (max < min)
+ return false;
+ range = min + (max << 16);
+ net.address_pool = g_list_append(net.address_pool,
+ GUINT_TO_POINTER(range));
+ return true;
+}
+
+static int match_address_range(const void *a, const void *b)
+{
+ uint32_t range = GPOINTER_TO_UINT(a);
+ uint8_t num_elements = (uint8_t) (GPOINTER_TO_UINT(b));
+ uint16_t max = range >> 16;
+ uint16_t min = range & 0xffff;
+
+ return ((max - min) >= (num_elements - 1)) ? 0 : -1;
+
+}
+
+uint16_t net_obtain_address(uint8_t num_eles)
+{
+ uint16_t addr;
+ GList *l;
+
+ l = g_list_find_custom(net.address_pool, GUINT_TO_POINTER(num_eles),
+ match_address_range);
+ if (l) {
+ uint32_t range = GPOINTER_TO_UINT(l->data);
+ uint16_t max = range >> 16;
+ uint16_t min = range & 0xffff;
+
+ addr = min;
+ min += num_eles;
+
+ if (min > max)
+ net.address_pool = g_list_delete_link(net.address_pool,
+ l);
+ else {
+ range = min + (max << 16);
+ l->data = GUINT_TO_POINTER(range);
+ }
+ return addr;
+ }
+
+ return UNASSIGNED_ADDRESS;
+}
+
+static int range_cmp(const void *a, const void *b)
+{
+ uint32_t range1 = GPOINTER_TO_UINT(a);
+ uint32_t range2 = GPOINTER_TO_UINT(b);
+
+ return range2 - range1;
+}
+
+void net_release_address(uint16_t addr, uint8_t num_elements)
+{
+ GList *l;
+ uint32_t range;
+
+ for (l = net.address_pool; l != NULL; l = l->next)
+ {
+ uint16_t max;
+ uint16_t min;
+
+ range = GPOINTER_TO_UINT(l->data);
+
+ max = range >> 16;
+ min = range & 0xffff;
+
+ if (min == (addr + num_elements + 1))
+ min = addr;
+ else if (addr && max == (addr - 1))
+ max = addr + num_elements + 1;
+ else
+ continue;
+
+ range = min + (max << 16);
+ l->data = GUINT_TO_POINTER(range);
+ return;
+ }
+
+ range = addr + ((addr + num_elements - 1) << 16);
+ net.address_pool = g_list_insert_sorted(net.address_pool,
+ GUINT_TO_POINTER(range),
+ range_cmp);
+}
+
+bool net_reserve_address_range(uint16_t base, uint8_t num_elements)
+{
+ GList *l;
+ uint32_t range;
+ uint16_t max;
+ uint16_t min;
+ bool shrink;
+
+ for (l = net.address_pool; l != NULL; l = l->next) {
+
+ range = GPOINTER_TO_UINT(l->data);
+
+ max = range >> 16;
+ min = range & 0xffff;
+
+ if (base >= min && (base + num_elements - 1) <= max)
+ break;
+ }
+
+ if (!l)
+ return false;
+
+ net.address_pool = g_list_delete_link(net.address_pool, l);
+
+ shrink = false;
+
+ if (base == min) {
+ shrink = true;
+ min = base + num_elements;
+ }
+
+ if (max == base + num_elements - 1) {
+ shrink = true;
+ max -= num_elements;
+ }
+
+ if (min > max)
+ return true;
+
+ if (shrink)
+ range = min + (max << 16);
+ else
+ range = min + ((base - 1) << 16);
+
+ net.address_pool = g_list_insert_sorted(net.address_pool,
+ GUINT_TO_POINTER(range),
+ range_cmp);
+
+ if (shrink)
+ return true;
+
+ range = (base + num_elements) + (max << 16);
+ net.address_pool = g_list_insert_sorted(net.address_pool,
+ GUINT_TO_POINTER(range),
+ range_cmp);
+
+ return true;
+}
+
+static int match_destination(const void *a, const void *b)
+{
+ const struct mesh_destination *dest = a;
+ uint16_t dst = GPOINTER_TO_UINT(b);
+
+ return (dest->dst == dst) ? 0 : -1;
+}
+
+void net_dest_ref(uint16_t dst)
+{
+ struct mesh_destination *dest;
+ GList *l;
+
+ if (!dst) return;
+
+ l = g_list_find_custom(net.dest, GUINT_TO_POINTER(dst),
+ match_destination);
+
+ if (l) {
+ dest = l->data;
+ dest->cnt++;
+ return;
+ }
+
+ dest = g_new0(struct mesh_destination, 1);
+ dest->dst = dst;
+ dest->cnt++;
+ net.dest = g_list_append(net.dest, dest);
+}
+
+void net_dest_unref(uint16_t dst)
+{
+ struct mesh_destination *dest;
+ GList *l;
+
+ l = g_list_find_custom(net.dest, GUINT_TO_POINTER(dst),
+ match_destination);
+
+ if (!l)
+ return;
+
+ dest = l->data;
+ dest->cnt--;
+
+ if (dest->cnt == 0) {
+ net.dest = g_list_remove(net.dest, dest);
+ g_free(dest);
+ }
+}
+
+struct build_whitelist {
+ uint8_t len;
+ uint8_t data[12];
+};
+
+static void whitefilter_add(gpointer data, gpointer user_data)
+{
+ struct mesh_destination *dest = data;
+ struct build_whitelist *white = user_data;
+
+ if (white->len == 0)
+ white->data[white->len++] = FILTER_ADD;
+
+ put_be16(dest->dst, white->data + white->len);
+ white->len += 2;
+
+ if (white->len > (sizeof(white->data) - sizeof(uint16_t))) {
+ net_ctl_msg_send(0, 0, 0, white->data, white->len);
+ white->len = 0;
+ }
+}
+
+static void setup_whitelist()
+{
+ struct build_whitelist white;
+
+ white.len = 0;
+
+ /* Enable (and Clear) Proxy Whitelist */
+ white.data[white.len++] = FILTER_SETUP;
+ white.data[white.len++] = WHITELIST_FILTER;
+
+ net_ctl_msg_send(0, 0, 0, white.data, white.len);
+
+ white.len = 0;
+ g_list_foreach(net.dest, whitefilter_add, &white);
+
+ if (white.len)
+ net_ctl_msg_send(0, 0, 0, white.data, white.len);
+}
+
+static void beacon_update(bool first, bool iv_update, uint32_t iv_index)
+{
+
+ /* Enforcement of 96 hour and 192 hour IVU time windows */
+ if (iv_update && !net.iv_update) {
+ rl_printf("iv_upd_state = IV_UPD_UPDATING\n");
+ net.iv_upd_state = IV_UPD_UPDATING;
+ /* TODO: Start timer to enforce IV Update parameters */
+ } else if (first) {
+ if (iv_update)
+ net.iv_upd_state = IV_UPD_UPDATING;
+ else
+ net.iv_upd_state = IV_UPD_NORMAL;
+
+ rl_printf("iv_upd_state = IV_UPD_%s\n",
+ iv_update ? "UPDATING" : "NORMAL");
+
+ } else if (iv_update && iv_index != net.iv_index) {
+ rl_printf("IV Update too soon -- Rejecting\n");
+ return;
+ }
+
+ if (iv_index > net.iv_index ||
+ iv_update != net.iv_update) {
+
+ /* Don't reset our seq_num unless
+ * we start using new iv_index */
+ if (!(iv_update && (net.iv_index + 1 == iv_index))) {
+ net.seq_num = 0;
+ net.seq_num_reserved = 100;
+ }
+ }
+
+ if (!net.seq_num || net.iv_index != iv_index ||
+ net.iv_update != iv_update) {
+
+ if (net.seq_num_reserved <= net.seq_num)
+ net.seq_num_reserved = net.seq_num + 100;
+
+ prov_db_local_set_iv_index(iv_index, iv_update,
+ net.provisioner);
+ prov_db_local_set_seq_num(net.seq_num_reserved);
+ }
+
+ net.iv_index = iv_index;
+ net.iv_update = iv_update;
+
+ if (first) {
+ /* Must be done once per Proxy Connection after Beacon RXed */
+ setup_whitelist();
+ if (net.open_cb)
+ net.open_cb(0);
+ }
+}
+
+static bool process_beacon(uint8_t *data, uint8_t size)
+{
+ struct mesh_net_key *net_key;
+ struct net_key_parts *key_part;
+ bool rxed_iv_update, rxed_key_refresh, iv_update;
+ bool my_krf;
+ uint32_t rxed_iv_index, iv_index;
+ uint64_t cmac;
+
+ if (size != 22)
+ return false;
+
+ rxed_key_refresh = (data[1] & 0x01) == 0x01;
+ iv_update = rxed_iv_update = (data[1] & 0x02) == 0x02;
+ iv_index = rxed_iv_index = get_be32(data + 10);
+
+ /* Inhibit recognizing iv_update true-->false
+ * if we have outbound SAR messages in flight */
+ if (net.msg_out != NULL) {
+ if (net.iv_update && !rxed_iv_update)
+ iv_update = true;
+ }
+
+ /* Don't bother going further if nothing has changed */
+ if (iv_index == net.iv_index && iv_update == net.iv_update &&
+ net.iv_upd_state != IV_UPD_INIT)
+ return true;
+
+ /* Find key we are using for SNBs */
+ net_key = find_net_key_by_id(data + 2);
+
+ if (net_key == NULL)
+ return false;
+
+ /* We are Provisioner, and control the key_refresh flag */
+ if (rxed_key_refresh != !!(net_key->phase == 2))
+ return false;
+
+ if (net_key->phase != 2) {
+ my_krf = false;
+ key_part = &net_key->current;
+ } else {
+ my_krf = true;
+ key_part = &net_key->new;
+ }
+
+ /* Ignore for incorrect KR state */
+ if (memcmp(key_part->net_id, data + 2, 8))
+ return false;
+
+ if ((net.iv_index + IV_IDX_DIFF_RANGE < iv_index) ||
+ (iv_index < net.iv_index)) {
+ rl_printf("iv index outside range\n");
+ return false;
+ }
+
+ /* Any behavioral changes must pass CMAC test */
+ if (!mesh_crypto_beacon_cmac(key_part->beacon_key, key_part->net_id,
+ rxed_iv_index, my_krf,
+ rxed_iv_update, &cmac)) {
+ return false;
+ }
+
+ if (cmac != get_be64(data + 14))
+ return false;
+
+ if (iv_update && (net.iv_upd_state > IV_UPD_UPDATING)) {
+ if (iv_index != net.iv_index) {
+ rl_printf("Update too soon -- Rejecting\n");
+ }
+ /* Silently ignore old beacons */
+ return true;
+ }
+
+ beacon_update(net.iv_upd_state == IV_UPD_INIT, iv_update, iv_index);
+
+ return true;
+}
+
+struct decode_params {
+ struct mesh_net_key *net_key;
+ uint8_t *packet;
+ uint32_t iv_index;
+ uint8_t size;
+ bool proxy;
+};
+
+static void try_decode(gpointer data, gpointer user_data)
+{
+ struct mesh_net_key *net_key = data;
+ struct decode_params *decode = user_data;
+ uint8_t nid = decode->packet[0] & 0x7f;
+ uint8_t tmp[29];
+ bool status = false;
+
+ if (decode->net_key)
+ return;
+
+ if (net_key->current.nid == nid)
+ status = mesh_crypto_packet_decode(decode->packet,
+ decode->size, decode->proxy, tmp,
+ decode->iv_index,
+ net_key->current.enc_key,
+ net_key->current.privacy_key);
+
+ if (!status && net_key->new.nid == nid)
+ status = mesh_crypto_packet_decode(decode->packet,
+ decode->size, decode->proxy, tmp,
+ decode->iv_index,
+ net_key->new.enc_key,
+ net_key->new.privacy_key);
+
+ if (status) {
+ decode->net_key = net_key;
+ memcpy(decode->packet, tmp, decode->size);
+ return;
+ }
+}
+
+static struct mesh_net_key *net_packet_decode(bool proxy, uint32_t iv_index,
+ uint8_t *packet, uint8_t size)
+{
+ struct decode_params decode = {
+ .proxy = proxy,
+ .iv_index = iv_index,
+ .packet = packet,
+ .size = size,
+ .net_key = NULL,
+ };
+
+ g_list_foreach(net_keys, try_decode, &decode);
+
+ return decode.net_key;
+}
+
+static void flush_sar(GList **list, struct mesh_sar_msg *sar)
+{
+ *list = g_list_remove(*list, sar);
+
+ if (sar->msg_to)
+ g_source_remove(sar->msg_to);
+
+ if (sar->ack_to)
+ g_source_remove(sar->ack_to);
+
+ g_free(sar);
+}
+
+static void flush_sar_list(GList **list)
+{
+ struct mesh_sar_msg *sar;
+ GList *l = g_list_first(*list);
+
+ while (l) {
+ sar = l->data;
+ flush_sar(list, sar);
+ l = g_list_first(*list);
+ }
+}
+
+static void flush_pkt_list(GList **list)
+{
+ struct mesh_pkt *pkt;
+ GList *l = g_list_first(*list);
+
+ while (l) {
+ pkt = l->data;
+ *list = g_list_remove(*list, pkt);
+ g_free(pkt);
+ }
+}
+
+static void resend_unacked_segs(gpointer data, gpointer user_data)
+{
+ struct mesh_sar_msg *sar = data;
+
+ if (sar->activity_cnt)
+ resend_segs(sar);
+}
+
+static void send_pkt_cmplt(DBusMessage *message, void *user_data)
+{
+ struct mesh_pkt *pkt = user_data;
+ GList *l = g_list_first(net.pkt_out);
+
+ if (l && user_data == l->data) {
+ net.pkt_out = g_list_delete_link(net.pkt_out, l);
+ g_free(pkt);
+ } else {
+ /* This is a serious error, and probable memory leak */
+ rl_printf("ERR: send_pkt_cmplt %p not head of queue\n", pkt);
+ }
+
+ l = g_list_first(net.pkt_out);
+
+ if (l == NULL) {
+ /* If queue is newly empty, resend all SAR outbound packets */
+ g_list_foreach(net.msg_out, resend_unacked_segs, NULL);
+ return;
+ }
+
+ pkt = l->data;
+
+ mesh_gatt_write(net.proxy_in, pkt->data, pkt->len,
+ send_pkt_cmplt, pkt);
+}
+
+static void send_mesh_pkt(struct mesh_pkt *pkt)
+{
+ bool queued = !!(net.pkt_out);
+
+ net.pkt_out = g_list_append(net.pkt_out, pkt);
+
+ if (queued)
+ return;
+
+ mesh_gatt_write(net.proxy_in, pkt->data, pkt->len,
+ send_pkt_cmplt, pkt);
+}
+
+static uint32_t get_next_seq()
+{
+ uint32_t this_seq = net.seq_num++;
+
+ if (net.seq_num + 32 >= net.seq_num_reserved) {
+ net.seq_num_reserved = net.seq_num + 100;
+ prov_db_local_set_seq_num(net.seq_num_reserved);
+ }
+
+ return this_seq;
+}
+
+static void send_seg(struct mesh_sar_msg *sar, uint8_t seg)
+{
+ struct mesh_net_key *net_key;
+ struct net_key_parts *part;
+ struct mesh_pkt *pkt;
+ uint8_t *data;
+
+ net_key = find_net_key_by_idx(sar->net_idx);
+
+ if (net_key == NULL)
+ return;
+
+ /* Choose which components to use to secure pkt */
+ if (net_key->phase == 2 && net_key->new.nid != 0xff)
+ part = &net_key->new;
+ else
+ part = &net_key->current;
+
+ pkt = g_new0(struct mesh_pkt, 1);
+
+ if (pkt == NULL)
+ return;
+
+ /* leave extra byte at start for GATT Proxy type */
+ data = pkt->data + 1;
+
+ SET_PKT_NID(data, part->nid);
+ SET_PKT_IVI(data, sar->iv_index & 1);
+ SET_PKT_CTL(data, sar->ctl);
+ SET_PKT_TTL(data, sar->ttl);
+ SET_PKT_SEQ(data, get_next_seq());
+ SET_PKT_SRC(data, sar->src);
+ SET_PKT_DST(data, sar->dst);
+ SET_PKT_SEGMENTED(data, sar->segmented);
+
+ if (sar->ctl)
+ SET_PKT_OPCODE(data, sar->data[0]);
+ else
+ SET_PKT_AKF_AID(data, sar->akf_aid);
+
+ if (sar->segmented) {
+
+ if (!sar->ctl)
+ SET_PKT_SZMIC(data, sar->szmic);
+
+ SET_PKT_SEQ0(data, sar->seqAuth);
+ SET_PKT_SEGO(data, seg);
+ SET_PKT_SEGN(data, sar->segN);
+
+ memcpy(PKT_TRANS(data) + 4,
+ sar->data + sar->ctl + (seg * 12), 12);
+
+ pkt->len = 9 + 4;
+
+ if (sar->segN == seg)
+ pkt->len += (sar->len - sar->ctl) % 12;
+
+ if (pkt->len == (9 + 4))
+ pkt->len += 12;
+
+ } else {
+ memcpy(PKT_TRANS(data) + 1,
+ sar->data + sar->ctl, 15);
+
+ pkt->len = 9 + 1 + sar->len - sar->ctl;
+ }
+
+ pkt->len += (sar->ctl ? 8 : 4);
+ mesh_crypto_packet_encode(data, pkt->len,
+ part->enc_key,
+ sar->iv_index,
+ part->privacy_key);
+
+
+ /* Prepend GATT_Proxy packet type */
+ if (sar->proxy)
+ pkt->data[0] = PROXY_CONFIG_PDU;
+ else
+ pkt->data[0] = PROXY_NETWORK_PDU;
+
+ pkt->len++;
+
+ send_mesh_pkt(pkt);
+}
+
+static void resend_segs(struct mesh_sar_msg *sar)
+{
+ uint32_t ack = 1;
+ uint8_t i;
+
+ sar->activity_cnt = 0;
+
+ for (i = 0; i <= sar->segN; i++, ack <<= 1) {
+ if (!(ack & sar->ack))
+ send_seg(sar, i);
+ }
+}
+
+static bool ack_rxed(bool to, uint16_t src, uint16_t dst, bool obo,
+ uint16_t seq0, uint32_t ack_flags)
+{
+ struct mesh_sar_msg *sar = find_sar_out_by_dst(src);
+ uint32_t full_ack;
+
+ /* Silently ignore unknown (stale?) ACKs */
+ if (sar == NULL)
+ return true;
+
+ full_ack = 0xffffffff >> (31 - sar->segN);
+
+ sar->ack |= (ack_flags & full_ack);
+
+ if (sar->ack == full_ack) {
+ /* Outbound message 100% received by remote node */
+ flush_sar(&net.msg_out, sar);
+ return true;
+ }
+
+ /* Because we are GATT, and slow, only resend PKTs if it is
+ * time *and* our outbound PKT queue is empty. */
+ sar->activity_cnt++;
+
+ if (net.pkt_out == NULL)
+ resend_segs(sar);
+
+ return true;
+}
+
+static bool proxy_ctl_rxed(uint16_t net_idx, uint32_t iv_index,
+ uint8_t ttl, uint32_t seq_num, uint16_t src, uint16_t dst,
+ uint8_t *trans, uint16_t len)
+{
+ if (len < 1)
+ return false;
+
+ switch(trans[0]) {
+ case FILTER_STATUS:
+ if (len != 4)
+ return false;
+
+ net.blacklist = !!(trans[1] == BLACKLIST_FILTER);
+ rl_printf("Proxy %slist filter length: %d\n",
+ net.blacklist ? "Black" : "White",
+ get_be16(trans + 2));
+
+ return true;
+
+ default:
+ return false;
+ }
+
+ return false;
+}
+
+static bool ctl_rxed(uint16_t net_idx, uint32_t iv_index,
+ uint8_t ttl, uint32_t seq_num, uint16_t src, uint16_t dst,
+ uint8_t *trans, uint16_t len)
+{
+ /* TODO: Handle control messages */
+ return false;
+}
+
+struct decrypt_params {
+ uint8_t *nonce;
+ uint8_t *aad;
+ uint8_t *out_msg;
+ uint8_t *trans;
+ uint32_t iv_index;
+ uint32_t seq_num;
+ uint16_t src;
+ uint16_t dst;
+ uint16_t len;
+ uint16_t net_idx;
+ uint16_t app_idx;
+ uint8_t akf_aid;
+ bool szmic;
+};
+
+
+static void try_decrypt(gpointer data, gpointer user_data)
+{
+ struct mesh_app_key *app_key = data;
+ struct decrypt_params *decrypt = user_data;
+ size_t mic_size = decrypt->szmic ? sizeof(uint64_t) : sizeof(uint32_t);
+ bool status = false;
+
+ /* Already done... Nothing to do */
+ if (decrypt->app_idx != APP_IDX_INVALID)
+ return;
+
+ /* Don't decrypt on Appkeys not owned by this NetKey */
+ if (app_key->net_idx != decrypt->net_idx)
+ return;
+
+ /* Test and decrypt against current key copy */
+ if (app_key->current.akf_aid == decrypt->akf_aid)
+ status = mesh_crypto_aes_ccm_decrypt(decrypt->nonce,
+ app_key->current.key,
+ decrypt->aad, decrypt->aad ? 16 : 0,
+ decrypt->trans, decrypt->len,
+ decrypt->out_msg, NULL, mic_size);
+
+ /* Test and decrypt against new key copy */
+ if (!status && app_key->new.akf_aid == decrypt->akf_aid)
+ status = mesh_crypto_aes_ccm_decrypt(decrypt->nonce,
+ app_key->new.key,
+ decrypt->aad, decrypt->aad ? 16 : 0,
+ decrypt->trans, decrypt->len,
+ decrypt->out_msg, NULL, mic_size);
+
+ /* If successful, terminate with successful App IDX */
+ if (status)
+ decrypt->app_idx = app_key->generic.idx;
+}
+
+static uint16_t access_pkt_decrypt(uint8_t *nonce, uint8_t *aad,
+ uint16_t net_idx, uint8_t akf_aid, bool szmic,
+ uint8_t *trans, uint16_t len)
+{
+ uint8_t *out_msg;
+ struct decrypt_params decrypt = {
+ .nonce = nonce,
+ .aad = aad,
+ .net_idx = net_idx,
+ .akf_aid = akf_aid,
+ .szmic = szmic,
+ .trans = trans,
+ .len = len,
+ .app_idx = APP_IDX_INVALID,
+ };
+
+ out_msg = g_malloc(len);
+
+ if (out_msg == NULL)
+ return false;
+
+ decrypt.out_msg = out_msg;
+
+ g_list_foreach(app_keys, try_decrypt, &decrypt);
+
+ if (decrypt.app_idx != APP_IDX_INVALID)
+ memcpy(trans, out_msg, len);
+
+ g_free(out_msg);
+
+ return decrypt.app_idx;
+}
+
+static bool access_rxed(uint8_t *nonce, uint16_t net_idx,
+ uint32_t iv_index, uint32_t seq_num,
+ uint16_t src, uint16_t dst,
+ uint8_t akf_aid, bool szmic, uint8_t *trans, uint16_t len)
+{
+ uint16_t app_idx = access_pkt_decrypt(nonce, NULL,
+ net_idx, akf_aid, szmic, trans, len);
+
+ if (app_idx != APP_IDX_INVALID) {
+ len -= szmic ? sizeof(uint64_t) : sizeof(uint32_t);
+
+ node_local_data_handler(src, dst, iv_index, seq_num,
+ app_idx, trans, len);
+ return true;
+ }
+
+ return false;
+}
+
+static void try_virt_decrypt(gpointer data, gpointer user_data)
+{
+ struct mesh_virt_addr *virt = data;
+ struct decrypt_params *decrypt = user_data;
+
+ if (decrypt->app_idx != APP_IDX_INVALID || decrypt->dst != virt->va16)
+ return;
+
+ decrypt->app_idx = access_pkt_decrypt(decrypt->nonce,
+ virt->va128,
+ decrypt->net_idx, decrypt->akf_aid,
+ decrypt->szmic, decrypt->trans, decrypt->len);
+
+ if (decrypt->app_idx != APP_IDX_INVALID) {
+ uint16_t len = decrypt->len;
+
+ len -= decrypt->szmic ? sizeof(uint64_t) : sizeof(uint32_t);
+
+ node_local_data_handler(decrypt->src, virt->va32,
+ decrypt->iv_index, decrypt->seq_num,
+ decrypt->app_idx, decrypt->trans, len);
+ }
+}
+
+static bool virtual_rxed(uint8_t *nonce, uint16_t net_idx,
+ uint32_t iv_index, uint32_t seq_num,
+ uint16_t src, uint16_t dst,
+ uint8_t akf_aid, bool szmic, uint8_t *trans, uint16_t len)
+{
+ struct decrypt_params decrypt = {
+ .nonce = nonce,
+ .net_idx = net_idx,
+ .iv_index = iv_index,
+ .seq_num = seq_num,
+ .src = dst,
+ .dst = dst,
+ .akf_aid = akf_aid,
+ .szmic = szmic,
+ .trans = trans,
+ .len = len,
+ .app_idx = APP_IDX_INVALID,
+ };
+
+ /* Cycle through known virtual addresses */
+ g_list_foreach(virt_addrs, try_virt_decrypt, &decrypt);
+
+ if (decrypt.app_idx != APP_IDX_INVALID)
+ return true;
+
+ return false;
+}
+
+static bool msg_rxed(uint16_t net_idx, uint32_t iv_index, bool szmic,
+ uint8_t ttl, uint32_t seq_num, uint32_t seq_auth,
+ uint16_t src, uint16_t dst,
+ uint8_t *trans, uint16_t len)
+{
+ uint8_t akf_aid = TRANS_AKF_AID(trans);
+ bool result;
+ size_t mic_size = szmic ? sizeof(uint64_t) : sizeof(uint32_t);
+ uint8_t nonce[13];
+ uint8_t *dev_key;
+ uint8_t *out = NULL;
+
+ if (!TRANS_AKF(trans)) {
+ /* Compose Nonce */
+ result = mesh_crypto_device_nonce(seq_auth, src, dst,
+ iv_index, szmic, nonce);
+
+ if (!result) return false;
+
+ out = g_malloc0(TRANS_LEN(trans, len));
+ if (out == NULL) return false;
+
+ /* If we are provisioner, we probably RXed on remote Dev Key */
+ if (net.provisioner) {
+ dev_key = node_get_device_key(node_find_by_addr(src));
+
+ if (dev_key == NULL)
+ goto local_dev_key;
+ } else
+ goto local_dev_key;
+
+ result = mesh_crypto_aes_ccm_decrypt(nonce, dev_key,
+ NULL, 0,
+ TRANS_PAYLOAD(trans), TRANS_LEN(trans, len),
+ out, NULL, mic_size);
+
+ if (result) {
+ node_local_data_handler(src, dst,
+ iv_index, seq_num, APP_IDX_DEV,
+ out, TRANS_LEN(trans, len) - mic_size);
+ goto done;
+ }
+
+local_dev_key:
+ /* Always fallback to the local Dev Key */
+ dev_key = node_get_device_key(node_get_local_node());
+
+ if (dev_key == NULL)
+ goto done;
+
+ result = mesh_crypto_aes_ccm_decrypt(nonce, dev_key,
+ NULL, 0,
+ TRANS_PAYLOAD(trans), TRANS_LEN(trans, len),
+ out, NULL, mic_size);
+
+ if (result) {
+ node_local_data_handler(src, dst,
+ iv_index, seq_num, APP_IDX_DEV,
+ out, TRANS_LEN(trans, len) - mic_size);
+ goto done;
+ }
+
+ goto done;
+ }
+
+ result = mesh_crypto_application_nonce(seq_auth, src, dst,
+ iv_index, szmic, nonce);
+
+ if (!result) goto done;
+
+ /* If Virtual destination wrap the Access decoder with Virtual */
+ if (IS_VIRTUAL(dst)) {
+ result = virtual_rxed(nonce, net_idx, iv_index, seq_num,
+ src, dst, akf_aid, szmic,
+ TRANS_PAYLOAD(trans), TRANS_LEN(trans, len));
+ goto done;
+ }
+
+ /* Try all matching App Keys until success or exhaustion */
+ result = access_rxed(nonce, net_idx, iv_index, seq_num,
+ src, dst, akf_aid, szmic,
+ TRANS_PAYLOAD(trans), TRANS_LEN(trans, len));
+
+done:
+ if (out != NULL)
+ g_free(out);
+
+ return result;
+}
+
+static void send_sar_ack(struct mesh_sar_msg *sar)
+{
+ uint8_t ack[7];
+
+ sar->activity_cnt = 0;
+
+ memset(ack, 0, sizeof(ack));
+ SET_TRANS_OPCODE(ack, NET_OP_SEG_ACKNOWLEDGE);
+ SET_TRANS_SEQ0(ack, sar->seqAuth);
+ SET_TRANS_ACK(ack, sar->ack);
+
+ net_ctl_msg_send(0xff, sar->dst, sar->src, ack, sizeof(ack));
+}
+
+static gboolean sar_out_ack_timeout(void *user_data)
+{
+ struct mesh_sar_msg *sar = user_data;
+
+ sar->activity_cnt++;
+
+ /* Because we are GATT, and slow, only resend PKTs if it is
+ * time *and* our outbound PKT queue is empty. */
+ if (net.pkt_out == NULL)
+ resend_segs(sar);
+
+ /* Only add resent SAR pkts to empty queue */
+ return true;
+}
+
+static gboolean sar_out_msg_timeout(void *user_data)
+{
+ struct mesh_sar_msg *sar = user_data;
+
+ /* msg_to will expire when we return false */
+ sar->msg_to = 0;
+
+ flush_sar(&net.msg_out, sar);
+
+ return false;
+}
+
+static gboolean sar_in_ack_timeout(void *user_data)
+{
+ struct mesh_sar_msg *sar = user_data;
+ uint32_t full_ack = 0xffffffff >> (31 - sar->segN);
+
+ if (sar->activity_cnt || sar->ack != full_ack)
+ send_sar_ack(sar);
+
+ return true;
+}
+
+static gboolean sar_in_msg_timeout(void *user_data)
+{
+ struct mesh_sar_msg *sar = user_data;
+
+ /* msg_to will expire when we return false */
+ sar->msg_to = 0;
+
+ flush_sar(&net.sar_in, sar);
+
+ return false;
+}
+
+static uint32_t calc_seqAuth(uint32_t seq_num, uint8_t *trans)
+{
+ uint32_t seqAuth = seq_num & ~0x1fff;
+
+ seqAuth |= TRANS_SEQ0(trans);
+
+ return seqAuth;
+}
+
+static bool seg_rxed(uint16_t net_idx, uint32_t iv_index, bool ctl,
+ uint8_t ttl, uint32_t seq_num, uint16_t src, uint16_t dst,
+ uint8_t *trans, uint16_t len)
+{
+ struct mesh_sar_msg *sar;
+ uint32_t seqAuth = calc_seqAuth(seq_num, trans);
+ uint8_t segN, segO;
+ uint32_t old_ack, full_ack, last_ack_mask;
+ bool send_ack, result = false;
+
+ segN = TRANS_SEGN(trans);
+ segO = TRANS_SEGO(trans);
+
+ /* Only support single incoming SAR'd message per SRC */
+ sar = find_sar_in_by_src(src);
+
+ /* Reuse existing SAR structure if appropriate */
+ if (sar) {
+ uint64_t iv_seqAuth = (uint64_t)iv_index << 32 | seqAuth;
+ uint64_t old_iv_seqAuth = (uint64_t)sar->iv_index << 32 |
+ sar->seqAuth;
+ if (old_iv_seqAuth < iv_seqAuth) {
+
+ flush_sar(&net.sar_in, sar);
+ sar = NULL;
+
+ } else if (old_iv_seqAuth > iv_seqAuth) {
+
+ /* New segment is Stale. Silently ignore */
+ return false;
+
+ } else if (segN != sar->segN) {
+
+ /* Remote side sent conflicting data: abandon */
+ flush_sar(&net.sar_in, sar);
+ sar = NULL;
+
+ }
+ }
+
+ if (sar == NULL) {
+ sar = g_malloc0(sizeof(*sar) + (12 * segN));
+
+ if (sar == NULL)
+ return false;
+
+ sar->net_idx = net_idx;
+ sar->iv_index = iv_index;
+ sar->ctl = ctl;
+ sar->ttl = ttl;
+ sar->seqAuth = seqAuth;
+ sar->src = src;
+ sar->dst = dst;
+ sar->segmented = true;
+ sar->szmic = TRANS_SZMIC(trans);
+ sar->segN = segN;
+
+ /* In all cases, the reassembled packet should begin with the
+ * same first octet of all segments, minus the SEGMENTED flag */
+ sar->data[0] = trans[0] & 0x7f;
+
+ net.sar_in = g_list_append(net.sar_in, sar);
+
+ /* Setup expiration timers */
+ if (IS_UNICAST(dst))
+ sar->ack_to = g_timeout_add(5000,
+ sar_in_ack_timeout, sar);
+
+ sar->msg_to = g_timeout_add(60000, sar_in_msg_timeout, sar);
+ }
+
+ /* If last segment, calculate full msg size */
+ if (segN == segO)
+ sar->len = (segN * 12) + len - 3;
+
+ /* Copy to correct offset */
+ memcpy(sar->data + 1 + (12 * segO), trans + 4, 12);
+
+ full_ack = 0xffffffff >> (31 - segN);
+ last_ack_mask = 0xffffffff << segO;
+ old_ack = sar->ack;
+ sar->ack |= 1 << segO;
+ send_ack = false;
+
+ /* Determine if we should forward message */
+ if (sar->ack == full_ack && old_ack != full_ack) {
+
+ /* First time we have seen this complete message */
+ send_ack = true;
+
+ if (ctl)
+ result = ctl_rxed(sar->net_idx, sar->iv_index,
+ sar->ttl, sar->seqAuth, sar->src,
+ sar->dst, sar->data, sar->len);
+ else
+ result = msg_rxed(sar->net_idx, sar->iv_index,
+ sar->szmic, sar->ttl,
+ seq_num, sar->seqAuth, sar->src,
+ sar->dst, sar->data, sar->len);
+ }
+
+ /* Never Ack Group addressed SAR messages */
+ if (!IS_UNICAST(dst))
+ return result;
+
+ /* Tickle the ACK system so it knows we are still RXing segments */
+ sar->activity_cnt++;
+
+ /* Determine if we should ACK */
+ if (old_ack == sar->ack)
+ /* Let the timer generate repeat ACKs as needed */
+ send_ack = false;
+ else if ((last_ack_mask & sar->ack) == (last_ack_mask & full_ack))
+ /* If this was largest segO outstanding segment, we ACK */
+ send_ack = true;
+
+ if (send_ack)
+ send_sar_ack(sar);
+
+ return result;
+}
+
+bool net_data_ready(uint8_t *msg, uint8_t len)
+{
+ uint8_t type = *msg++;
+ uint32_t iv_index = net.iv_index;
+ struct mesh_net_key *net_key;
+
+ if (len-- < 10) return false;
+
+ if (type == PROXY_MESH_BEACON)
+ return process_beacon(msg, len);
+ else if (type > PROXY_CONFIG_PDU)
+ return false;
+
+ /* RXed iv_index must be equal or 1 less than local iv_index */
+ /* With the clue being high-order bit of first octet */
+ if (!!(iv_index & 0x01) != !!(msg[0] & 0x80)) {
+ if (iv_index)
+ iv_index--;
+ else
+ return false;
+ }
+
+ net_key = net_packet_decode(type == PROXY_CONFIG_PDU,
+ iv_index, msg, len);
+
+ if (net_key == NULL)
+ return false;
+
+ /* CTL packets have 64 bit network MIC, otherwise 32 bit MIC */
+ len -= PKT_CTL(msg) ? sizeof(uint64_t) : sizeof(uint32_t);
+
+ if (type == PROXY_CONFIG_PDU) {
+
+ /* Proxy Configuration DST messages must be 0x0000 */
+ if (PKT_DST(msg))
+ return false;
+
+ return proxy_ctl_rxed(net_key->generic.idx,
+ iv_index, PKT_TTL(msg), PKT_SEQ(msg),
+ PKT_SRC(msg), PKT_DST(msg),
+ PKT_TRANS(msg), PKT_TRANS_LEN(len));
+
+ } if (PKT_CTL(msg) && PKT_OPCODE(msg) == NET_OP_SEG_ACKNOWLEDGE) {
+
+ return ack_rxed(false, PKT_SRC(msg), PKT_DST(msg),
+ PKT_OBO(msg), PKT_SEQ0(msg), PKT_ACK(msg));
+
+ } else if (PKT_SEGMENTED(msg)) {
+
+ return seg_rxed(net_key->generic.idx, iv_index, PKT_CTL(msg),
+ PKT_TTL(msg), PKT_SEQ(msg),
+ PKT_SRC(msg), PKT_DST(msg),
+ PKT_TRANS(msg), PKT_TRANS_LEN(len));
+
+ } else if (!PKT_CTL(msg)){
+
+ return msg_rxed(net_key->generic.idx,
+ iv_index, false, PKT_TTL(msg), PKT_SEQ(msg),
+ PKT_SEQ(msg), PKT_SRC(msg), PKT_DST(msg),
+ PKT_TRANS(msg), PKT_TRANS_LEN(len));
+ } else {
+
+ return ctl_rxed(net_key->generic.idx,
+ iv_index, PKT_TTL(msg), PKT_SEQ(msg),
+ PKT_SRC(msg), PKT_DST(msg),
+ PKT_TRANS(msg), PKT_TRANS_LEN(len));
+
+ }
+
+ return false;
+}
+
+bool net_session_open(GDBusProxy *data_in, bool provisioner,
+ net_mesh_session_open_callback cb)
+{
+ if (net.proxy_in)
+ return false;
+
+ net.proxy_in = data_in;
+ net.iv_upd_state = IV_UPD_INIT;
+ net.blacklist = false;
+ net.provisioner = provisioner;
+ net.open_cb = cb;
+ flush_pkt_list(&net.pkt_out);
+ return true;
+}
+
+void net_session_close(GDBusProxy *data_in)
+{
+ if (net.proxy_in == data_in)
+ net.proxy_in = NULL;
+
+ flush_sar_list(&net.sar_in);
+ flush_sar_list(&net.msg_out);
+ flush_pkt_list(&net.pkt_out);
+}
+
+bool net_register_unicast(uint16_t unicast, uint8_t count)
+{
+ /* TODO */
+ return true;
+}
+
+bool net_register_group(uint16_t group_addr)
+{
+ /* TODO */
+ return true;
+}
+
+uint32_t net_register_virtual(uint8_t buf[16])
+{
+ /* TODO */
+ return 0;
+}
+
+static bool get_enc_keys(uint16_t app_idx, uint16_t dst,
+ uint8_t *akf_aid, uint8_t **app_enc_key,
+ uint16_t *net_idx)
+{
+ if (app_idx == APP_IDX_DEV) {
+ struct mesh_node *node;
+ uint8_t *enc_key = NULL;
+
+ if (net.provisioner) {
+ /* Default to Remote Device Key when Provisioner */
+ node = node_find_by_addr(dst);
+ enc_key = node_get_device_key(node);
+ }
+
+ if (enc_key == NULL) {
+ /* Use Local node Device Key */
+ node = node_get_local_node();
+ enc_key = node_get_device_key(node);
+ }
+
+ if (enc_key == NULL || node == NULL)
+ return false;
+
+ if (akf_aid) *akf_aid = 0;
+ if (app_enc_key) *app_enc_key = enc_key;
+ if (net_idx) *net_idx = node_get_primary_net_idx(node);
+
+ } else {
+ struct mesh_app_key *app_key = find_app_key_by_idx(app_idx);
+ struct mesh_net_key *net_key;
+ bool phase_two;
+
+
+ if (app_key == NULL)
+ return false;
+
+ net_key = find_net_key_by_idx(app_key->net_idx);
+
+ if (net_key == NULL)
+ return false;
+
+ if (net_idx) *net_idx = app_key->net_idx;
+
+ phase_two = !!(net_key->phase == 2);
+
+ if (phase_two && app_key->new.akf_aid != 0xff) {
+ if (app_enc_key) *app_enc_key = app_key->new.key;
+ if (akf_aid) *akf_aid = app_key->new.akf_aid;
+ } else {
+ if (app_enc_key) *app_enc_key = app_key->current.key;
+ if (akf_aid) *akf_aid = app_key->current.akf_aid;
+ }
+ }
+
+ return true;
+}
+
+bool net_ctl_msg_send(uint8_t ttl, uint16_t src, uint16_t dst,
+ uint8_t *buf, uint16_t len)
+{
+ struct mesh_node *node = node_get_local_node();
+ struct mesh_sar_msg sar_ctl;
+
+ /* For simplicity, we will reject segmented OB CTL messages */
+ if (len > 12 || node == NULL || buf == NULL || buf[0] & 0x80)
+ return false;
+
+ if (!src) {
+ src = node_get_primary(node);
+
+ if (!src)
+ return false;
+ }
+
+ if (ttl == 0xff)
+ ttl = net.default_ttl;
+
+ memset(&sar_ctl, 0, sizeof(sar_ctl));
+
+ if (!dst)
+ sar_ctl.proxy = true;
+
+ /* Get the default net_idx for remote device (or local) */
+ get_enc_keys(APP_IDX_DEV, dst, NULL, NULL, &sar_ctl.net_idx);
+ sar_ctl.ctl = true;
+ sar_ctl.iv_index = net.iv_index - net.iv_update;
+ sar_ctl.ttl = ttl;
+ sar_ctl.src = src;
+ sar_ctl.dst = dst;
+ sar_ctl.len = len;
+ memcpy(sar_ctl.data, buf, len);
+ send_seg(&sar_ctl, 0);
+
+ return true;
+}
+
+bool net_access_layer_send(uint8_t ttl, uint16_t src, uint32_t dst,
+ uint16_t app_idx, uint8_t *buf, uint16_t len)
+{
+ struct mesh_node *node = node_get_local_node();
+ struct mesh_sar_msg *sar;
+ uint8_t *app_enc_key = NULL;
+ uint8_t *aad = NULL;
+ uint32_t mic32;
+ uint8_t aad_len = 0;
+ uint8_t i, j, ackless_retries = 0;
+ uint8_t segN, akf_aid;
+ uint16_t net_idx;
+ bool result;
+
+ if (len > 384 || node == NULL)
+ return false;
+
+ if (!src)
+ src = node_get_primary(node);
+
+ if (!src || !dst)
+ return false;
+
+ if (ttl == 0xff)
+ ttl = net.default_ttl;
+
+ if (IS_VIRTUAL(dst)) {
+ struct mesh_virt_addr *virt = find_virt_by_dst(dst);
+
+ if (virt == NULL)
+ return false;
+
+ dst = virt->va16;
+ aad = virt->va128;
+ aad_len = sizeof(virt->va128);
+ }
+
+ result = get_enc_keys(app_idx, dst,
+ &akf_aid, &app_enc_key, &net_idx);
+
+ if (!result)
+ return false;
+
+ segN = SEG_MAX(len);
+
+ /* Only one ACK required SAR message per destination at a time */
+ if (segN && IS_UNICAST(dst)) {
+ sar = find_sar_out_by_dst(dst);
+
+ if (sar)
+ flush_sar(&net.msg_out, sar);
+ }
+
+ sar = g_malloc0(sizeof(struct mesh_sar_msg) + (segN * 12));
+
+ if (sar == NULL)
+ return false;
+
+ if (segN)
+ sar->segmented = true;
+
+ sar->ttl = ttl;
+ sar->segN = segN;
+ sar->seqAuth = net.seq_num;
+ sar->iv_index = net.iv_index - net.iv_update;
+ sar->net_idx = net_idx;
+ sar->src = src;
+ sar->dst = dst;
+ sar->akf_aid = akf_aid;
+ sar->len = len + sizeof(uint32_t);
+
+ mesh_crypto_application_encrypt(akf_aid,
+ sar->seqAuth, src,
+ dst, sar->iv_index,
+ app_enc_key,
+ aad, aad_len,
+ buf, len,
+ sar->data, &mic32,
+ sizeof(uint32_t));
+
+ /* If sending as a segmented message to a non-Unicast (thus non-ACKing)
+ * destination, send each segments multiple times. */
+ if (!IS_UNICAST(dst) && segN)
+ ackless_retries = 4;
+
+ for (j = 0; j <= ackless_retries; j++) {
+ for (i = 0; i <= segN; i++)
+ send_seg(sar, i);
+ }
+
+ if (IS_UNICAST(dst) && segN) {
+ net.msg_out = g_list_append(net.msg_out, sar);
+ sar->ack_to = g_timeout_add(2000, sar_out_ack_timeout, sar);
+ sar->msg_to = g_timeout_add(60000, sar_out_msg_timeout, sar);
+ } else
+ g_free(sar);
+
+ return true;
+}
+
+bool net_set_default_ttl(uint8_t ttl)
+{
+ if (ttl > 0x7f)
+ return false;
+
+ net.default_ttl = ttl;
+ return true;
+}
+
+uint8_t net_get_default_ttl()
+{
+ return net.default_ttl;
+}
+
+bool net_set_seq_num(uint32_t seq_num)
+{
+ if (seq_num > 0xffffff)
+ return false;
+
+ net.seq_num = seq_num;
+ return true;
+}
+
+uint32_t net_get_seq_num()
+{
+ return net.seq_num;
+}
diff --git a/mesh/node.c b/mesh/node.c
new file mode 100644
index 0000000..ba8d4b6
--- /dev/null
+++ b/mesh/node.c
+/*
+ *
+ * BlueZ - Bluetooth protocol stack for Linux
+ *
+ * Copyright (C) 2017 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.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <errno.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <sys/uio.h>
+#include <wordexp.h>
+
+#include <readline/readline.h>
+#include <readline/history.h>
+#include <glib.h>
+
+#include "client/display.h"
+#include "src/shared/util.h"
+#include "gdbus/gdbus.h"
+#include "monitor/uuid.h"
+#include "mesh-net.h"
+#include "config-model.h"
+#include "node.h"
+#include "keys.h"
+#include "gatt.h"
+#include "net.h"
+#include "prov-db.h"
+#include "util.h"
+
+struct mesh_model {
+ struct mesh_model_ops cbs;
+ void *user_data;
+ GList *bindings;
+ GList *subscriptions;
+ uint32_t id;
+ struct mesh_publication *pub;
+};
+
+struct mesh_element {
+ GList *models;
+ uint16_t loc;
+ uint8_t index;
+};
+
+struct mesh_node {
+ const char *name;
+ GList *net_keys;
+ GList *app_keys;
+ void *prov;
+ GList *elements;
+ uint32_t iv_index;
+ uint32_t seq_number;
+ uint16_t primary_net_idx;
+ uint16_t primary;
+ uint16_t oob;
+ uint16_t features;
+ uint8_t gatt_pkt[MAX_GATT_SIZE];
+ uint8_t dev_uuid[16];
+ uint8_t dev_key[16];
+ uint8_t num_ele;
+ uint8_t ttl;
+ uint8_t gatt_size;
+ bool provisioner;
+ struct mesh_node_composition *comp;
+};
+
+static GList *nodes;
+
+static struct mesh_node *local_node;
+
+static int match_node_unicast(const void *a, const void *b)
+{
+ const struct mesh_node *node = a;
+ uint16_t dst = GPOINTER_TO_UINT(b);
+
+ if (dst >= node->primary &&
+ dst <= (node->primary + node->num_ele - 1))
+ return 0;
+
+ return -1;
+}
+
+static int match_device_uuid(const void *a, const void *b)
+{
+ const struct mesh_node *node = a;
+ const uint8_t *uuid = b;
+
+ return memcmp(node->dev_uuid, uuid, 16);
+}
+
+static int match_element_idx(const void *a, const void *b)
+{
+ const struct mesh_element *element = a;
+ uint32_t index = GPOINTER_TO_UINT(b);
+
+ return (element->index == index) ? 0 : -1;
+}
+
+static int match_model_id(const void *a, const void *b)
+{
+ const struct mesh_model *model = a;
+ uint32_t id = GPOINTER_TO_UINT(b);
+
+ return (model->id == id) ? 0 : -1;
+}
+
+struct mesh_node *node_find_by_addr(uint16_t addr)
+{
+ GList *l;
+
+ if (!IS_UNICAST(addr))
+ return NULL;
+
+ l = g_list_find_custom(nodes, GUINT_TO_POINTER(addr),
+ match_node_unicast);
+
+ if (l)
+ return l->data;
+ else
+ return NULL;
+}
+
+struct mesh_node *node_find_by_uuid(uint8_t uuid[16])
+{
+ GList *l;
+
+ l = g_list_find_custom(nodes, uuid, match_device_uuid);
+
+ if (l)
+ return l->data;
+ else
+ return NULL;
+}
+
+struct mesh_node *node_create_new(struct prov_svc_data *prov)
+{
+ struct mesh_node *node;
+
+ if (node_find_by_uuid(prov->dev_uuid))
+ return NULL;
+
+ node = g_malloc0(sizeof(struct mesh_node));
+ if (!node)
+ return NULL;
+
+ memcpy(node->dev_uuid, prov->dev_uuid, 16);
+ node->oob = prov->oob;
+ nodes = g_list_append(nodes, node);
+
+ return node;
+}
+
+struct mesh_node *node_new(void)
+{
+ struct mesh_node *node;
+
+ node = g_malloc0(sizeof(struct mesh_node));
+ if (!node)
+ return NULL;
+
+ nodes = g_list_append(nodes, node);
+
+ return node;
+}
+
+static void model_free(void *data)
+{
+ struct mesh_model *model = data;
+
+ g_list_free(model->bindings);
+ g_list_free(model->subscriptions);
+ g_free(model->pub);
+ g_free(model);
+}
+
+static void element_free(void *data)
+{
+ struct mesh_element *element = data;
+
+ g_list_free_full(element->models, model_free);
+ g_free(element);
+}
+
+static void free_node_resources(void *data)
+{
+ struct mesh_node *node = data;
+ g_list_free(node->net_keys);
+ g_list_free(node->app_keys);
+
+ g_list_free_full(node->elements, element_free);
+
+ if(node->comp)
+ g_free(node->comp);
+
+ g_free(node);
+}
+
+void node_free(struct mesh_node *node)
+{
+ if (!node)
+ return;
+ nodes = g_list_remove(nodes, node);
+ free_node_resources(node);
+}
+
+void node_cleanup(void)
+{
+ g_list_free_full(nodes, free_node_resources);
+ local_node = NULL;
+}
+
+bool node_is_provisioned(struct mesh_node *node)
+{
+ return (!IS_UNASSIGNED(node->primary));
+}
+
+void *node_get_prov(struct mesh_node *node)
+{
+ return node->prov;
+}
+
+void node_set_prov(struct mesh_node *node, void *prov)
+{
+ node->prov = prov;
+}
+
+bool node_app_key_add(struct mesh_node *node, uint16_t idx)
+{
+ uint32_t index;
+ uint16_t net_idx;
+
+ if (!node)
+ return false;
+
+ net_idx = keys_app_key_get_bound(idx);
+ if (net_idx == NET_IDX_INVALID)
+ return false;
+
+ if (!g_list_find(node->net_keys, GUINT_TO_POINTER(net_idx)))
+ return false;
+
+ index = (net_idx << 16) + idx;
+
+ if (g_list_find(node->app_keys, GUINT_TO_POINTER(index)))
+ return false;
+
+ node->app_keys = g_list_append(node->app_keys, GUINT_TO_POINTER(index));
+
+ return true;
+}
+
+bool node_net_key_add(struct mesh_node *node, uint16_t index)
+{
+ if(!node)
+ return false;
+
+ if (g_list_find(node->net_keys, GUINT_TO_POINTER(index)))
+ return false;
+
+ node->net_keys = g_list_append(node->net_keys, GUINT_TO_POINTER(index));
+ return true;
+}
+
+bool node_net_key_delete(struct mesh_node *node, uint16_t index)
+{
+ GList *l;
+
+ if(!node)
+ return false;
+
+ l = g_list_find(node->net_keys, GUINT_TO_POINTER(index));
+ if (!l)
+ return false;
+
+ node->net_keys = g_list_remove(node->net_keys,
+ GUINT_TO_POINTER(index));
+ /* TODO: remove all associated app keys and bindings */
+ return true;
+}
+
+bool node_app_key_delete(struct mesh_node *node, uint16_t net_idx,
+ uint16_t idx)
+{
+ GList *l;
+ uint32_t index;
+
+ if(!node)
+ return false;
+
+ index = (net_idx << 16) + idx;
+
+ l = g_list_find(node->app_keys, GUINT_TO_POINTER(index));
+ if (!l)
+ return false;
+
+ node->app_keys = g_list_remove(node->app_keys,
+ GUINT_TO_POINTER(index));
+ /* TODO: remove all associated bindings */
+ return true;
+}
+
+void node_set_primary(struct mesh_node *node, uint16_t unicast)
+{
+ node->primary = unicast;
+}
+
+uint16_t node_get_primary(struct mesh_node *node)
+{
+ if (!node)
+ return UNASSIGNED_ADDRESS;
+ else
+ return node->primary;
+}
+
+void node_set_device_key(struct mesh_node *node, uint8_t *key)
+
+{
+ if (!node || !key)
+ return;
+
+ memcpy(node->dev_key, key, 16);
+}
+
+uint8_t *node_get_device_key(struct mesh_node *node)
+{
+ if (!node)
+ return NULL;
+ else
+ return node->dev_key;
+}
+
+void node_set_num_elements(struct mesh_node *node, uint8_t num_ele)
+{
+ node->num_ele = num_ele;
+}
+
+uint8_t node_get_num_elements(struct mesh_node *node)
+{
+ return node->num_ele;
+}
+
+GList *node_get_net_keys(struct mesh_node *node)
+{
+ if (!node)
+ return NULL;
+ else
+ return node->net_keys;
+}
+
+GList *node_get_app_keys(struct mesh_node *node)
+{
+ if (!node)
+ return NULL;
+ else
+ return node->app_keys;
+}
+
+bool node_parse_composition(struct mesh_node *node, uint8_t *data, uint16_t len)
+{
+ struct mesh_node_composition *comp;
+ uint16_t features;
+ int i;
+
+ comp = g_malloc0(sizeof(struct mesh_node_composition));
+ if (!comp)
+ return false;
+
+ /* skip page -- We only support Page Zero */
+ data++;
+ len--;
+
+ comp->cid = get_le16(&data[0]);
+ comp->pid = get_le16(&data[2]);
+ comp->vid = get_le16(&data[4]);
+ comp->crpl = get_le16(&data[6]);
+ features = get_le16(&data[8]);
+ data += 10;
+ len -= 10;
+
+ comp->relay = !!(features & MESH_FEATURE_RELAY);
+ comp->proxy = !!(features & MESH_FEATURE_PROXY);
+ comp->friend = !!(features & MESH_FEATURE_FRIEND);
+ comp->lpn = !!(features & MESH_FEATURE_LPN);
+
+ for (i = 0; i< node->num_ele; i++) {
+ uint8_t m, v;
+ uint32_t mod_id;
+ uint16_t vendor_id;
+ struct mesh_element *ele;
+ ele = g_malloc0(sizeof(struct mesh_element));
+ if (!ele)
+ return false;
+
+ ele->index = i;
+ ele->loc = get_le16(data);
+ data += 2;
+ node->elements = g_list_append(node->elements, ele);
+
+ m = *data++;
+ v = *data++;
+ len -= 4;
+
+ while (len >= 2 && m--) {
+ mod_id = get_le16(data);
+ /* initialize uppper 16 bits to 0xffff for SIG models */
+ mod_id |= 0xffff0000;
+ if (!node_set_model(node, ele->index, mod_id))
+ return false;
+ data += 2;
+ len -= 2;
+ }
+ while (len >= 4 && v--) {
+ mod_id = get_le16(data);
+ vendor_id = get_le16(data);
+ mod_id |= (vendor_id << 16);
+ if (!node_set_model(node, ele->index, mod_id))
+ return false;
+ data += 4;
+ len -= 4;
+ }
+
+ }
+
+ node->comp = comp;
+ return true;
+}
+
+bool node_set_local_node(struct mesh_node *node)
+{
+ if (local_node) {
+ rl_printf("Local node already registered\n");
+ return false;
+ }
+ net_register_unicast(node->primary, node->num_ele);
+
+ local_node = node;
+ local_node->provisioner = true;
+
+ return true;
+}
+
+struct mesh_node *node_get_local_node()
+{
+ return local_node;
+}
+
+uint16_t node_get_primary_net_idx(struct mesh_node *node)
+{
+ if (node == NULL)
+ return NET_IDX_INVALID;
+
+ return node->primary_net_idx;
+}
+
+static bool deliver_model_data(struct mesh_element* element, uint16_t src,
+ uint16_t app_idx, uint8_t *data, uint16_t len)
+{
+ GList *l;
+
+ for(l = element->models; l; l = l->next) {
+ struct mesh_model *model = l->data;
+
+ if (!g_list_find(model->bindings, GUINT_TO_POINTER(app_idx)))
+ continue;
+
+ if (model->cbs.recv &&
+ model->cbs.recv(src, data, len, model->user_data))
+ return true;
+ }
+
+ return false;
+}
+
+void node_local_data_handler(uint16_t src, uint32_t dst,
+ uint32_t iv_index, uint32_t seq_num,
+ uint16_t app_idx, uint8_t *data, uint16_t len)
+{
+ GList *l;
+ bool res;
+ uint64_t iv_seq;
+ uint64_t iv_seq_remote;
+ uint8_t ele_idx;
+ struct mesh_element *element;
+ struct mesh_node *remote;
+ bool loopback;
+
+ if (!local_node || seq_num > 0xffffff)
+ return;
+
+ iv_seq = iv_index << 24;
+ iv_seq |= seq_num;
+
+ remote = node_find_by_addr(src);
+
+ if (!remote) {
+ if (local_node->provisioner) {
+ rl_printf("Remote node unknown (%4.4x)\n", src);
+ return;
+ }
+
+ remote = g_new0(struct mesh_node, 1);
+ if (!remote)
+ return;
+
+ /* Not Provisioner; Assume all SRC elements stand alone */
+ remote->primary = src;
+ remote->num_ele = 1;
+ nodes = g_list_append(nodes, remote);
+ }
+
+ loopback = (src < (local_node->primary + local_node->num_ele) &&
+ src >= local_node->primary);
+
+ if (!loopback) {
+ iv_seq_remote = remote->iv_index << 24;
+ iv_seq |= remote->seq_number;
+
+ if (iv_seq_remote >= iv_seq) {
+ rl_printf("Replayed message detected "
+ "(%14lx >= %14lx)\n",
+ iv_seq_remote, iv_seq);
+ return;
+ }
+ }
+
+ if (IS_GROUP(dst) || IS_VIRTUAL(dst)) {
+ /* TODO: if subscription address, deliver to subscribers */
+ return;
+ }
+
+ if (IS_ALL_NODES(dst)) {
+ ele_idx = 0;
+ } else {
+ if (dst >= (local_node->primary + local_node->num_ele) ||
+ dst < local_node->primary)
+ return;
+
+ ele_idx = dst - local_node->primary;
+ }
+
+ l = g_list_find_custom(local_node->elements,
+ GUINT_TO_POINTER(ele_idx), match_element_idx);
+
+ /* This should not happen */
+ if (!l)
+ return;
+
+ element = l->data;
+ res = deliver_model_data(element, src, app_idx, data, len);
+
+ if (res && !loopback) {
+ /* TODO: Save remote in Replay Protection db */
+ remote->iv_index = iv_index;
+ remote->seq_number = seq_num;
+ prov_db_node_set_iv_seq(remote, iv_index, seq_num);
+ }
+}
+
+static gboolean restore_model_state(gpointer data)
+{
+ struct mesh_model *model = data;
+ GList *l;
+ struct mesh_model_ops *ops;
+
+ ops = &model->cbs;
+
+ if (model->bindings && ops->bind) {
+ for (l = model->bindings; l; l = l->next) {
+ if (ops->bind(GPOINTER_TO_UINT(l->data), ACTION_ADD) !=
+ MESH_STATUS_SUCCESS)
+ break;
+ }
+ }
+
+ if (model->pub && ops->pub)
+ ops->pub(model->pub);
+
+ g_idle_remove_by_data(data);
+
+ return true;
+
+}
+
+bool node_local_model_register(uint8_t ele_idx, uint16_t model_id,
+ struct mesh_model_ops *ops, void *user_data)
+{
+ uint32_t id = 0xffff0000 | model_id;
+
+ return node_local_vendor_model_register(ele_idx, id, ops, user_data);
+}
+
+bool node_local_vendor_model_register(uint8_t ele_idx, uint32_t model_id,
+ struct mesh_model_ops *ops, void *user_data)
+{
+ struct mesh_element *ele;
+ struct mesh_model *model;
+ GList *l;
+
+ if (!local_node)
+ return false;
+
+ l = g_list_find_custom(local_node->elements, GUINT_TO_POINTER(ele_idx),
+ match_element_idx);
+ if (!l)
+ return false;
+
+ ele = l->data;
+
+ l = g_list_find_custom(ele->models, GUINT_TO_POINTER(model_id),
+ match_model_id);
+ if (!l)
+ return false;
+
+ model = l->data;
+ model->cbs = *ops;
+ model->user_data = user_data;
+
+ if (model_id >= 0xffff0000)
+ model_id = model_id & 0xffff;
+
+ /* Silently assign device key binding to configuration models */
+ if (model_id == CONFIG_SERVER_MODEL_ID ||
+ model_id == CONFIG_CLIENT_MODEL_ID) {
+ model->bindings = g_list_append(model->bindings,
+ GUINT_TO_POINTER(APP_IDX_DEV));
+ } else {
+ g_idle_add(restore_model_state, model);
+ }
+
+ return true;
+}
+
+bool node_set_element(struct mesh_node *node, uint8_t ele_idx)
+{
+ struct mesh_element *ele;
+ GList *l;
+
+ if (!node)
+ return false;
+
+ l = g_list_find_custom(node->elements, GUINT_TO_POINTER(ele_idx),
+ match_element_idx);
+ if (l)
+ return false;
+
+ ele = g_malloc0(sizeof(struct mesh_element));
+ if (!ele)
+ return false;
+
+ ele->index = ele_idx;
+ node->elements = g_list_append(node->elements, ele);
+
+ return true;
+}
+
+bool node_set_model(struct mesh_node *node, uint8_t ele_idx, uint32_t id)
+{
+ struct mesh_element *ele;
+ struct mesh_model *model;
+ GList *l;
+
+ if (!node)
+ return false;
+
+ l = g_list_find_custom(node->elements, GUINT_TO_POINTER(ele_idx),
+ match_element_idx);
+ if (!l)
+ return false;
+
+ ele = l->data;
+
+ l = g_list_find_custom(ele->models, GUINT_TO_POINTER(id),
+ match_model_id);
+ if (l)
+ return false;
+
+ model = g_malloc0(sizeof(struct mesh_model));
+ if (!model)
+ return false;
+
+ model->id = id;
+ ele->models = g_list_append(ele->models, model);
+
+ return true;
+}
+
+bool node_set_composition(struct mesh_node *node,
+ struct mesh_node_composition *comp)
+{
+ if (!node || !comp || node->comp)
+ return false;
+
+ node->comp = g_malloc0(sizeof(struct mesh_node_composition));
+ if (!node->comp)
+ return false;
+
+ *(node->comp) = *comp;
+ return true;
+}
+
+struct mesh_node_composition *node_get_composition(struct mesh_node *node)
+{
+ if (!node)
+ return NULL;
+
+ return node->comp;
+}
+
+static struct mesh_model *get_model(struct mesh_node *node, uint8_t ele_idx,
+ uint32_t model_id)
+{
+ struct mesh_element *ele;
+ GList *l;
+
+ if (!node)
+ return NULL;
+
+ l = g_list_find_custom(node->elements, GUINT_TO_POINTER(ele_idx),
+ match_element_idx);
+ if (!l)
+ return NULL;
+
+ ele = l->data;
+
+ l = g_list_find_custom(ele->models, GUINT_TO_POINTER(model_id),
+ match_model_id);
+ if (!l)
+ return NULL;
+
+ return l->data;
+
+}
+
+bool node_add_binding(struct mesh_node *node, uint8_t ele_idx,
+ uint32_t model_id, uint16_t app_idx)
+{
+ struct mesh_model *model;
+ GList *l;
+
+ model = get_model(node, ele_idx, model_id);
+ if(!model)
+ return false;
+
+ l = g_list_find(model->bindings, GUINT_TO_POINTER(app_idx));
+ if (l)
+ return false;
+
+ if ((node == local_node) && model->cbs.bind) {
+ if (!model->cbs.bind(app_idx, ACTION_ADD))
+ return false;
+ }
+
+ model->bindings = g_list_append(model->bindings,
+ GUINT_TO_POINTER(app_idx));
+
+ return true;
+}
+
+uint8_t node_get_default_ttl(struct mesh_node *node)
+{
+ if (!node)
+ return DEFAULT_TTL;
+ else if (node == local_node)
+ return net_get_default_ttl();
+ else
+ return node->ttl;
+}
+
+bool node_set_default_ttl(struct mesh_node *node, uint8_t ttl)
+{
+ if (!node)
+ return false;
+
+ node->ttl = ttl;
+
+ if (node == local_node || local_node == NULL)
+ return net_set_default_ttl(ttl);
+
+ return true;
+}
+
+bool node_set_sequence_number(struct mesh_node *node, uint32_t seq)
+{
+ if (!node)
+ return false;
+
+ node->seq_number = seq;
+
+ if (node == local_node || local_node == NULL)
+ return net_set_seq_num(seq);
+
+ return true;
+}
+
+uint32_t node_get_sequence_number(struct mesh_node *node)
+{
+ if (!node)
+ return 0xffffffff;
+ else if (node == local_node)
+ return net_get_seq_num();
+
+ return node->seq_number;
+}
+
+bool node_set_iv_index(struct mesh_node *node, uint32_t iv_index)
+{
+ if (!node)
+ return false;
+
+ node->iv_index = iv_index;
+ return true;
+}
+
+uint32_t node_get_iv_index(struct mesh_node *node)
+{
+ bool update;
+
+ if (!node)
+ return 0xffffffff;
+ else if (node == local_node)
+ return net_get_iv_index(&update);
+ return node->iv_index;
+}
+
+bool node_model_pub_set(struct mesh_node *node, uint8_t ele, uint32_t model_id,
+ struct mesh_publication *pub)
+{
+ struct mesh_model *model;
+
+ model = get_model(node, ele, model_id);
+ if(!model)
+ return false;
+
+ if (!model->pub)
+ model->pub = g_malloc0(sizeof(struct mesh_publication));
+ if (!model)
+ return false;
+
+ memcpy(model->pub, pub, (sizeof(struct mesh_publication)));
+
+ if((node == local_node) && model->cbs.pub)
+ model->cbs.pub(pub);
+ return true;
+}
+
+struct mesh_publication *node_model_pub_get(struct mesh_node *node, uint8_t ele,
+ uint32_t model_id)
+{
+ struct mesh_model *model;
+
+ model = get_model(node, ele, model_id);
+ if(!model)
+ return NULL;
+ else
+ return model->pub;
+}
diff --git a/mesh/onoff-model.c b/mesh/onoff-model.c
new file mode 100644
index 0000000..61c6ed6
--- /dev/null
+++ b/mesh/onoff-model.c
+/*
+ *
+ * BlueZ - Bluetooth protocol stack for Linux
+ *
+ * Copyright (C) 2017 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.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <errno.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <inttypes.h>
+#include <stdbool.h>
+#include <sys/uio.h>
+#include <wordexp.h>
+#include <readline/readline.h>
+#include <readline/history.h>
+#include <glib.h>
+
+#include "client/display.h"
+#include "src/shared/util.h"
+#include "mesh-net.h"
+#include "keys.h"
+#include "net.h"
+#include "node.h"
+#include "prov-db.h"
+#include "util.h"
+#include "onoff-model.h"
+
+static uint8_t trans_id;
+static uint16_t onoff_app_idx = APP_IDX_INVALID;
+
+static int client_bind(uint16_t app_idx, int action)
+{
+ if (action == ACTION_ADD) {
+ if (onoff_app_idx != APP_IDX_INVALID) {
+ return MESH_STATUS_INSUFF_RESOURCES;
+ } else {
+ onoff_app_idx = app_idx;
+ rl_printf("On/Off client model: new binding %4.4x\n",
+ app_idx);
+ }
+ } else {
+ if (onoff_app_idx == app_idx)
+ onoff_app_idx = APP_IDX_INVALID;
+ }
+ return MESH_STATUS_SUCCESS;
+}
+
+static void print_remaining_time(uint8_t remaining_time)
+{
+ uint8_t step = (remaining_time & 0xc0) >> 6;
+ uint8_t count = remaining_time & 0x3f;
+ int secs = 0, msecs = 0, minutes = 0, hours = 0;
+
+ switch (step) {
+ case 0:
+ msecs = 100 * count;
+ secs = msecs / 60;
+ msecs -= (secs * 60);
+ break;
+ case 1:
+ secs = 1 * count;
+ minutes = secs / 60;
+ secs -= (minutes * 60);
+ break;
+
+ case 2:
+ secs = 10 * count;
+ minutes = secs / 60;
+ secs -= (minutes * 60);
+ break;
+ case 3:
+ minutes = 10 * count;
+ hours = minutes / 60;
+ minutes -= (hours * 60);
+ break;
+
+ default:
+ break;
+ }
+
+ rl_printf("\n\t\tRemaining time: %d hrs %d mins %d secs %d msecs\n",
+ hours, minutes, secs, msecs);
+
+}
+
+static bool client_msg_recvd(uint16_t src, uint8_t *data,
+ uint16_t len, void *user_data)
+{
+ uint32_t opcode;
+ int n;
+
+ if (mesh_opcode_get(data, len, &opcode, &n)) {
+ len -= n;
+ data += n;
+ } else
+ return false;
+
+ rl_printf("On Off Model Message received (%d) opcode %x\n",
+ len, opcode);
+ print_byte_array("\t",data, len);
+
+ switch (opcode & ~OP_UNRELIABLE) {
+ default:
+ return false;
+
+ case OP_GENERIC_ONOFF_STATUS:
+ if (len != 1 && len != 3)
+ break;
+
+ rl_printf("Node %4.4x: Off Status present = %s",
+ src, data[0] ? "ON" : "OFF");
+
+ if (len == 3) {
+ rl_printf(", target = %s", data[1] ? "ON" : "OFF");
+ print_remaining_time(data[2]);
+ } else
+ rl_printf("\n");
+ break;
+ }
+
+ return true;
+}
+
+
+static uint32_t target;
+static uint32_t parms[8];
+
+static uint32_t read_input_parameters(const char *args)
+{
+ uint32_t i;
+
+ if (!args)
+ return 0;
+
+ memset(parms, 0xff, sizeof(parms));
+
+ for (i = 0; i < sizeof(parms)/sizeof(parms[0]); i++) {
+ int n;
+
+ sscanf(args, "%x", &parms[i]);
+ if (parms[i] == 0xffffffff)
+ break;
+
+ n = strcspn(args, " \t");
+ args = args + n + strspn(args + n, " \t");
+ }
+
+ return i;
+}
+
+static void cmd_set_node(const char *args)
+{
+ uint32_t dst;
+ char *end;
+
+ dst = strtol(args, &end, 16);
+ if (end != (args + 4)) {
+ rl_printf("Bad unicast address %s: "
+ "expected format 4 digit hex\n",
+ args);
+ target = UNASSIGNED_ADDRESS;
+ } else {
+ rl_printf("Controlling ON/OFF for node %4.4x\n", dst);
+ target = dst;
+ set_menu_prompt("on/off", args);
+ }
+}
+
+static bool send_cmd(uint8_t *buf, uint16_t len)
+{
+ struct mesh_node *node = node_get_local_node();
+ uint8_t ttl;
+
+ if(!node)
+ return false;
+
+ ttl = node_get_default_ttl(node);
+
+ return net_access_layer_send(ttl, node_get_primary(node),
+ target, onoff_app_idx, buf, len);
+}
+
+static void cmd_get_status(const char *args)
+{
+ uint16_t n;
+ uint8_t msg[32];
+ struct mesh_node *node;
+
+ if (IS_UNASSIGNED(target)) {
+ rl_printf("Destination not set\n");
+ return;
+ }
+
+ node = node_find_by_addr(target);
+
+ if (!node)
+ return;
+
+ n = mesh_opcode_set(OP_GENERIC_ONOFF_GET, msg);
+
+ if (!send_cmd(msg, n))
+ rl_printf("Failed to send \"GENERIC ON/OFF GET\"\n");
+}
+
+static void cmd_set(const char *args)
+{
+ uint16_t n;
+ uint8_t msg[32];
+ struct mesh_node *node;
+
+ if (IS_UNASSIGNED(target)) {
+ rl_printf("Destination not set\n");
+ return;
+ }
+
+ node = node_find_by_addr(target);
+
+ if (!node)
+ return;
+
+ if ((read_input_parameters(args) != 1) &&
+ parms[0] != 0 && parms[0] != 1) {
+ rl_printf("Bad arguments %s. Expecting \"0\" or \"1\"\n", args);
+ return;
+ }
+
+ n = mesh_opcode_set(OP_GENERIC_ONOFF_SET, msg);
+ msg[n++] = parms[0];
+ msg[n++] = trans_id++;
+
+ if (!send_cmd(msg, n))
+ rl_printf("Failed to send \"GENERIC ON/OFF SET\"\n");
+
+}
+
+static void cmd_back(const char *args)
+{
+ cmd_menu_main(false);
+}
+
+static void cmd_help(const char *args);
+
+static const struct menu_entry cfg_menu[] = {
+ {"target", "<unicast>", cmd_set_node,
+ "Set node to configure"},
+ {"get", NULL, cmd_get_status,
+ "Get ON/OFF status"},
+ {"onoff", "<0/1>", cmd_set,
+ "Send \"SET ON/OFF\" command"},
+ {"back", NULL, cmd_back,
+ "Back to main menu"},
+ {"help", NULL, cmd_help,
+ "Config Commands"},
+ {}
+};
+
+static void cmd_help(const char *args)
+{
+ rl_printf("Client Configuration Menu\n");
+ print_cmd_menu(cfg_menu);
+}
+
+void onoff_set_node(const char *args) {
+ cmd_set_node(args);
+}
+
+static struct mesh_model_ops client_cbs = {
+ client_msg_recvd,
+ client_bind,
+ NULL,
+ NULL
+};
+
+bool onoff_client_init(uint8_t ele)
+{
+ if (!node_local_model_register(ele, GENERIC_ONOFF_CLIENT_MODEL_ID,
+ &client_cbs, NULL))
+ return false;
+
+ add_cmd_menu("onoff", cfg_menu);
+
+ return true;
+}
diff --git a/mesh/prov-db.c b/mesh/prov-db.c
new file mode 100644
index 0000000..aad6145
--- /dev/null
+++ b/mesh/prov-db.c
+/*
+ *
+ * BlueZ - Bluetooth protocol stack for Linux
+ *
+ * Copyright (C) 2017 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.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <errno.h>
+#include <fcntl.h>
+#include <glib.h>
+#include <stdbool.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+
+#include <json-c/json.h>
+#include <sys/stat.h>
+
+#include <readline/readline.h>
+#include <glib.h>
+
+#include "src/shared/util.h"
+#include "client/display.h"
+
+#include "mesh-net.h"
+#include "crypto.h"
+#include "keys.h"
+#include "net.h"
+#include "node.h"
+#include "util.h"
+#include "prov-db.h"
+
+#define CHECK_KEY_IDX_RANGE(x) (((x) >= 0) && ((x) <= 4095))
+
+static const char *prov_filename;
+static const char *local_filename;
+
+static char* prov_file_read(const char *filename)
+{
+ int fd;
+ char *str;
+ struct stat st;
+ ssize_t sz;
+
+ if (!filename)
+ return NULL;
+
+ fd = open(filename,O_RDONLY);
+ if (!fd)
+ return NULL;
+
+ if (fstat(fd, &st) == -1) {
+ close(fd);
+ return NULL;
+ }
+
+ str = (char *) g_malloc0(st.st_size + 1);
+ if (!str) {
+ close(fd);
+ return NULL;
+ }
+
+ sz = read(fd, str, st.st_size);
+ if (sz != st.st_size)
+ rl_printf("Incomplete read: %d vs %d\n", (int)sz,
+ (int)(st.st_size));
+
+ close(fd);
+
+ return str;
+}
+
+static void prov_file_write(json_object *jmain, bool local)
+{
+ FILE *outfile;
+ const char *out_str;
+ const char *out_filename;
+
+ if (local)
+ out_filename = local_filename;
+ else
+ out_filename = prov_filename;
+
+ outfile = fopen(out_filename, "wr");
+ if (!outfile) {
+ rl_printf("Failed to open file %s for writing\n", out_filename);
+ return;
+ }
+
+ out_str = json_object_to_json_string_ext(jmain,
+ JSON_C_TO_STRING_PRETTY);
+
+ fwrite(out_str, sizeof(char), strlen(out_str), outfile);
+ fclose(outfile);
+}
+
+static void put_uint16(json_object *jobject, const char *desc, uint16_t value)
+{
+ json_object *jstring;
+ char buf[5];
+
+ snprintf(buf, 5, "%4.4x", value);
+ jstring = json_object_new_string(buf);
+ json_object_object_add(jobject, desc, jstring);
+}
+
+static void put_uint32(json_object *jobject, const char *desc, uint32_t value)
+{
+ json_object *jstring;
+ char buf[9];
+
+ snprintf(buf, 9, "%8.8x", value);
+ jstring = json_object_new_string(buf);
+ json_object_object_add(jobject, desc, jstring);
+}
+
+static void put_uint16_array_entry(json_object *jarray, uint16_t value)
+{
+ json_object *jstring;
+ char buf[5];
+
+ snprintf(buf, 5, "%4.4x", value);
+ jstring = json_object_new_string(buf);
+ json_object_array_add(jarray, jstring);
+}
+
+static void put_uint32_array_entry(json_object *jarray, uint32_t value)
+{
+ json_object *jstring;
+ char buf[9];
+
+ snprintf(buf, 9, "%8.8x", value);
+ jstring = json_object_new_string(buf);
+ json_object_array_add(jarray, jstring);
+}
+
+static void put_uint16_list(json_object *jarray, GList *list)
+{
+ GList *l;
+
+ if (!list)
+ return;
+
+ for (l = list; l; l = l->next) {
+ uint32_t ivalue = GPOINTER_TO_UINT(l->data);
+ put_uint16_array_entry(jarray, ivalue);
+ }
+}
+
+static void add_node_idxs(json_object *jnode, const char *desc,
+ GList *idxs)
+{
+ json_object *jarray;
+
+ jarray = json_object_new_array();
+
+ put_uint16_list(jarray, idxs);
+
+ json_object_object_add(jnode, desc, jarray);
+}
+
+static bool parse_unicast_range(json_object *jobject)
+{
+ int cnt;
+ int i;
+
+ cnt = json_object_array_length(jobject);
+
+ for (i = 0; i < cnt; ++i) {
+ json_object *jrange;
+ json_object *jvalue;
+ uint16_t low, high;
+ char *str;
+
+ jrange = json_object_array_get_idx(jobject, i);
+ json_object_object_get_ex(jrange, "lowAddress", &jvalue);
+ str = (char *)json_object_get_string(jvalue);
+ if (sscanf(str, "%04hx", &low) != 1)
+ return false;
+
+ json_object_object_get_ex(jrange, "highAddress", &jvalue);
+ str = (char *)json_object_get_string(jvalue);
+ if (sscanf(str, "%04hx", &high) != 1)
+ return false;
+
+ if(high < low)
+ return false;
+
+ net_add_address_pool(low, high);
+ }
+ return true;
+}
+
+static int parse_node_keys(struct mesh_node *node, json_object *jidxs,
+ bool is_app_key)
+{
+ int idx_cnt;
+ int i;
+
+ idx_cnt = json_object_array_length(jidxs);
+ for (i = 0; i < idx_cnt; ++i) {
+ int idx;
+ json_object *jvalue;
+
+ jvalue = json_object_array_get_idx(jidxs, i);
+ if (!jvalue)
+ break;
+ idx = json_object_get_int(jvalue);
+ if (!CHECK_KEY_IDX_RANGE(idx))
+ break;
+
+ if (is_app_key)
+ node_app_key_add(node, idx);
+ else
+ node_net_key_add(node, idx);
+ }
+
+ return i;
+}
+
+static bool parse_composition_models(struct mesh_node *node, int index,
+ json_object *jmodels)
+{
+ int model_cnt;
+ int i;
+
+ model_cnt = json_object_array_length(jmodels);
+
+ for (i = 0; i < model_cnt; ++i) {
+ json_object *jmodel;
+ char *str;
+ uint32_t model_id;
+ int len;
+
+ jmodel = json_object_array_get_idx(jmodels, i);
+ str = (char *)json_object_get_string(jmodel);
+ len = strlen(str);
+
+ if (len != 4 && len != 8)
+ return false;
+
+ if (sscanf(str, "%08x", &model_id) != 1)
+ return false;
+ if (len == 4)
+ model_id += 0xffff0000;
+
+ node_set_model(node, index, model_id);
+ }
+
+ return true;
+}
+
+static bool parse_composition_elements(struct mesh_node *node,
+ json_object *jelements)
+{
+ int el_cnt;
+ int i;
+
+ el_cnt = json_object_array_length(jelements);
+ node_set_num_elements(node, el_cnt);
+
+ for (i = 0; i < el_cnt; ++i) {
+ json_object *jelement;
+ json_object *jmodels;
+ json_object *jvalue;
+ int index;
+
+ jelement = json_object_array_get_idx(jelements, i);
+ json_object_object_get_ex(jelement, "elementIndex", &jvalue);
+ if (jvalue) {
+ index = json_object_get_int(jvalue);
+ if (index >= el_cnt) {
+ return false;
+ }
+ } else
+ return false;
+
+ if (!node_set_element(node, index))
+ return false;
+
+ json_object_object_get_ex(jelement, "models", &jmodels);
+ if (!jmodels)
+ continue;
+
+ if(!parse_composition_models(node, index, jmodels))
+ return false;
+ }
+ return true;
+}
+
+static bool parse_model_pub(struct mesh_node *node, int ele_idx,
+ uint32_t model_id, json_object *jpub)
+{
+ json_object *jvalue;
+ struct mesh_publication pub;
+ char *str;
+
+ memset(&pub, 0, sizeof(struct mesh_publication));
+
+ /* Read only required fields */
+ json_object_object_get_ex(jpub, "address", &jvalue);
+ if (!jvalue)
+ return false;
+
+ str = (char *)json_object_get_string(jvalue);
+ if (sscanf(str, "%04hx", &pub.u.addr16) != 1)
+ return false;
+
+ json_object_object_get_ex(jpub, "index", &jvalue);
+ if (!jvalue)
+ return false;
+
+ str = (char *)json_object_get_string(jvalue);
+ if (sscanf(str, "%04hx", &pub.app_idx) != 1)
+ return false;
+
+
+ json_object_object_get_ex(jpub, "ttl", &jvalue);
+ pub.ttl = json_object_get_int(jvalue);
+
+ if (!node_model_pub_set(node, ele_idx, model_id, &pub))
+ return false;
+
+ return true;
+}
+
+static bool parse_bindings(struct mesh_node *node, int ele_idx,
+ uint32_t model_id, json_object *jbindings)
+{
+ int cnt;
+ int i;
+
+ cnt = json_object_array_length(jbindings);
+
+ for (i = 0; i < cnt; ++i) {
+ int key_idx;
+ json_object *jvalue;
+
+ jvalue = json_object_array_get_idx(jbindings, i);
+ if (!jvalue)
+ return true;
+
+ key_idx = json_object_get_int(jvalue);
+ if (!CHECK_KEY_IDX_RANGE(key_idx))
+ return false;
+
+ if (!node_add_binding(node, ele_idx, model_id, key_idx))
+ return false;
+ }
+
+ return true;
+}
+
+static bool parse_configuration_models(struct mesh_node *node, int ele_idx,
+ json_object *jmodels, uint32_t target_id, json_object **jtarget)
+{
+ int model_cnt;
+ int i;
+
+ if (jtarget)
+ *jtarget = NULL;
+
+ model_cnt = json_object_array_length(jmodels);
+
+ for (i = 0; i < model_cnt; ++i) {
+ json_object *jmodel;
+ json_object *jvalue;
+ json_object *jarray;
+ char *str;
+ int len;
+ uint32_t model_id;
+
+ jmodel = json_object_array_get_idx(jmodels, i);
+
+ json_object_object_get_ex(jmodel, "modelId", &jvalue);
+ str = (char *)json_object_get_string(jvalue);
+
+ len = strlen(str);
+
+ if (len != 4 && len != 8)
+ return false;
+
+ if (sscanf(str, "%08x", &model_id) != 1)
+ return false;
+ if (len == 4)
+ model_id += 0xffff0000;
+
+ if (jtarget && model_id == target_id) {
+ *jtarget = jmodel;
+ return true;
+ }
+
+ json_object_object_get_ex(jmodel, "bind", &jarray);
+ if (jarray && !parse_bindings(node, ele_idx, model_id, jarray))
+ return false;
+
+ json_object_object_get_ex(jmodel, "publish", &jvalue);
+
+ if (jvalue && !parse_model_pub(node, ele_idx, model_id, jvalue))
+ return false;
+ }
+
+ return true;
+}
+
+static bool parse_configuration_elements(struct mesh_node *node,
+ json_object *jelements, bool local)
+{
+ int el_cnt;
+ int i;
+
+ el_cnt = json_object_array_length(jelements);
+ node_set_num_elements(node, el_cnt);
+
+ for (i = 0; i < el_cnt; ++i) {
+ json_object *jelement;
+ json_object *jmodels;
+ json_object *jvalue;
+ int index;
+ uint16_t addr;
+
+ jelement = json_object_array_get_idx(jelements, i);
+ json_object_object_get_ex(jelement, "elementIndex", &jvalue);
+ if (jvalue) {
+ index = json_object_get_int(jvalue);
+ if (index >= el_cnt) {
+ return false;
+ }
+ } else
+ return false;
+
+ if (index == 0) {
+ char *str;
+
+ json_object_object_get_ex(jelement, "unicastAddress",
+ &jvalue);
+ str = (char *)json_object_get_string(jvalue);
+ if (sscanf(str, "%04hx", &addr) != 1)
+ return false;
+
+ if (!local && !net_reserve_address_range(addr, el_cnt))
+ return false;
+
+ node_set_primary(node, addr);
+ }
+
+ json_object_object_get_ex(jelement, "models", &jmodels);
+ if (!jmodels)
+ continue;
+
+ if(!parse_configuration_models(node, index, jmodels, 0, NULL))
+ return false;
+ }
+ return true;
+}
+
+static void add_key(json_object *jobject, const char *desc, uint8_t* key)
+{
+ json_object *jstring;
+ char hexstr[33];
+
+ hex2str(key, 16, hexstr, 33);
+ jstring = json_object_new_string(hexstr);
+ json_object_object_add(jobject, desc, jstring);
+}
+
+static json_object *find_node_by_primary(json_object *jmain, uint16_t primary)
+{
+ json_object *jarray;
+ int i, len;
+
+ json_object_object_get_ex(jmain, "nodes", &jarray);
+
+ if (!jarray)
+ return NULL;
+ len = json_object_array_length(jarray);
+
+ for (i = 0; i < len; ++i) {
+ json_object *jnode;
+ json_object *jconfig;
+ json_object *jelements;
+ json_object *jelement;
+ json_object *jvalue;
+ char *str;
+ uint16_t addr;
+
+ jnode = json_object_array_get_idx(jarray, i);
+ if (!jnode)
+ return NULL;
+
+ json_object_object_get_ex(jnode, "configuration", &jconfig);
+ if (!jconfig)
+ return NULL;
+
+ json_object_object_get_ex(jconfig, "elements", &jelements);
+ if (!jelements)
+ return NULL;
+
+ jelement = json_object_array_get_idx(jelements, 0);
+ if (!jelement)
+ return NULL;
+
+ json_object_object_get_ex(jelement, "unicastAddress",
+ &jvalue);
+ str = (char *)json_object_get_string(jvalue);
+ if (sscanf(str, "%04hx", &addr) != 1)
+ return NULL;
+
+ if (addr == primary)
+ return jnode;
+ }
+
+ return NULL;
+
+}
+
+void prov_db_print_node_composition(struct mesh_node *node)
+{
+ char *in_str;
+ const char *comp_str;
+ json_object *jmain;
+ json_object *jnode;
+ json_object *jcomp;
+ uint16_t primary = node_get_primary(node);
+ const char *filename;
+ bool res = false;
+
+ if (!node || !node_get_composition(node))
+ return;
+
+ if (node == node_get_local_node())
+ filename = local_filename;
+ else
+ filename = prov_filename;
+
+ in_str = prov_file_read(filename);
+ if (!in_str)
+ return;
+
+ jmain = json_tokener_parse(in_str);
+ if (!jmain)
+ goto done;
+
+ jnode = find_node_by_primary(jmain, primary);
+ if (!jnode)
+ goto done;
+
+ json_object_object_get_ex(jnode, "composition", &jcomp);
+ if (!jcomp)
+ goto done;
+
+ comp_str = json_object_to_json_string_ext(jcomp,
+ JSON_C_TO_STRING_PRETTY);
+
+ res = true;
+
+done:
+ if (res)
+ rl_printf("\tComposition data for node %4.4x %s\n",
+ primary, comp_str);
+ else
+ rl_printf("\tComposition data for node %4.4x not present\n",
+ primary);
+ g_free(in_str);
+
+ if (jmain)
+ json_object_put(jmain);
+}
+
+bool prov_db_add_node_composition(struct mesh_node *node, uint8_t *data,
+ uint16_t len)
+{
+ char *in_str;
+ json_object *jmain;
+ json_object *jnode;
+ json_object *jcomp;
+ json_object *jbool;
+ json_object *jfeatures;
+ json_object *jelements;
+ struct mesh_node_composition *comp;
+ uint8_t num_ele;
+ int i;
+ uint16_t primary = node_get_primary(node);
+ bool res = NULL;
+
+ comp = node_get_composition(node);
+ if (!comp)
+ return false;
+
+ in_str = prov_file_read(prov_filename);
+ if (!in_str)
+ return false;
+
+ jmain = json_tokener_parse(in_str);
+ if (!jmain)
+ goto done;
+
+ jnode = find_node_by_primary(jmain, primary);
+ if (!jnode)
+ goto done;
+
+ jcomp = json_object_new_object();
+
+ put_uint16(jcomp, "cid", comp->cid);
+ put_uint16(jcomp, "pid", comp->pid);
+ put_uint16(jcomp, "vid", comp->pid);
+ put_uint16(jcomp, "crpl", comp->crpl);
+
+ jfeatures = json_object_new_object();
+ jbool = json_object_new_boolean(comp->relay);
+ json_object_object_add(jfeatures, "relay", jbool);
+ jbool = json_object_new_boolean(comp->proxy);
+ json_object_object_add(jfeatures, "proxy", jbool);
+ jbool = json_object_new_boolean(comp->friend);
+ json_object_object_add(jfeatures, "friend", jbool);
+ jbool = json_object_new_boolean(comp->lpn);
+ json_object_object_add(jfeatures, "lpn", jbool);
+ json_object_object_add(jcomp, "features", jfeatures);
+
+ data += 11;
+ len -= 11;
+
+ num_ele = node_get_num_elements(node);
+
+ jelements = json_object_new_array();
+
+ for (i = 0; i < num_ele; ++i) {
+ json_object *jelement;
+ json_object *jmodels;
+ json_object *jint;
+ uint32_t mod_id;
+ uint16_t vendor_id;
+ uint8_t m, v;
+
+ jelement = json_object_new_object();
+
+ /* Element Index */
+ jint = json_object_new_int(i);
+ json_object_object_add(jelement, "elementIndex", jint);
+
+ /* Location */
+ put_uint16(jelement, "location", get_le16(data));
+ data += 2;
+ m = *data++;
+ v = *data++;
+ len -= 4;
+
+ /* Models */
+ jmodels = json_object_new_array();
+ while (len >= 2 && m--) {
+ mod_id = get_le16(data);
+ data += 2;
+ len -= 2;
+ put_uint16_array_entry(jmodels, (uint16_t) mod_id);
+ }
+
+ while (len >= 4 && v--) {
+ mod_id = get_le16(data);
+ vendor_id = get_le16(data);
+ mod_id |= (vendor_id << 16);
+ data += 4;
+ len -= 4;
+ put_uint32_array_entry(jmodels, mod_id);
+ }
+
+ json_object_object_add(jelement, "models", jmodels);
+ json_object_array_add(jelements, jelement);
+ }
+
+ json_object_object_add(jcomp, "elements", jelements);
+
+ json_object_object_add(jnode, "composition", jcomp);
+
+ prov_file_write(jmain, false);
+
+ res = true;;
+done:
+
+ g_free(in_str);
+
+ if(jmain)
+ json_object_put(jmain);
+
+ return res;
+}
+
+bool prov_db_node_set_ttl(struct mesh_node *node, uint8_t ttl)
+{
+ char *in_str;
+ json_object *jmain;
+ json_object *jnode;
+ json_object *jconfig;
+ json_object *jvalue;
+ uint16_t primary = node_get_primary(node);
+ const char *filename;
+ bool local = node == node_get_local_node();
+ bool res = false;
+
+ if (local)
+ filename = local_filename;
+ else
+ filename = prov_filename;
+
+ in_str = prov_file_read(filename);
+ if (!in_str)
+ return false;
+
+ jmain = json_tokener_parse(in_str);
+ if (!jmain)
+ goto done;
+
+ if (local)
+ json_object_object_get_ex(jmain, "node", &jnode);
+ else
+ jnode = find_node_by_primary(jmain, primary);
+
+ if (!jnode)
+ goto done;
+
+ json_object_object_get_ex(jnode, "configuration", &jconfig);
+ if (!jconfig)
+ goto done;
+
+ json_object_object_del(jconfig, "defaultTTL");
+
+ jvalue = json_object_new_int(ttl);
+ json_object_object_add(jconfig, "defaultTTL", jvalue);
+
+ prov_file_write(jmain, local);
+
+ res = true;
+done:
+
+ g_free(in_str);
+
+ if(jmain)
+ json_object_put(jmain);
+
+ return res;
+
+}
+
+static void set_local_iv_index(json_object *jobj, uint32_t idx, bool update)
+{
+ json_object *jvalue;
+
+ json_object_object_del(jobj, "IVindex");
+ jvalue = json_object_new_int(idx);
+ json_object_object_add(jobj, "IVindex", jvalue);
+
+ json_object_object_del(jobj, "IVupdate");
+ jvalue = json_object_new_int((update) ? 1 : 0);
+ json_object_object_add(jobj, "IVupdate", jvalue);
+
+}
+
+bool prov_db_local_set_iv_index(uint32_t iv_index, bool update, bool prov)
+{
+ char *in_str;
+ json_object *jmain;
+ json_object *jnode;
+ bool res = false;
+
+ in_str = prov_file_read(local_filename);
+ if (!in_str)
+ return false;
+
+ jmain = json_tokener_parse(in_str);
+ if (!jmain)
+ goto done;
+
+ json_object_object_get_ex(jmain, "node", &jnode);
+ set_local_iv_index(jnode, iv_index, update);
+ prov_file_write(jmain, true);
+
+ g_free(in_str);
+ json_object_put(jmain);
+
+ /* If provisioner, save to global DB as well */
+ if (prov) {
+ in_str = prov_file_read(prov_filename);
+ if (!in_str)
+ return false;
+
+ jmain = json_tokener_parse(in_str);
+ if (!jmain)
+ goto done;
+
+ set_local_iv_index(jmain, iv_index, update);
+ prov_file_write(jmain, false);
+ }
+
+ res = true;
+done:
+
+ g_free(in_str);
+
+ if(jmain)
+ json_object_put(jmain);
+
+ return res;
+
+}
+
+bool prov_db_local_set_seq_num(uint32_t seq_num)
+{
+ char *in_str;
+ json_object *jmain;
+ json_object *jnode;
+ json_object *jvalue;
+ bool res = false;
+
+ in_str = prov_file_read(local_filename);
+ if (!in_str)
+ return false;
+
+ jmain = json_tokener_parse(in_str);
+ if (!jmain)
+ goto done;
+
+ json_object_object_get_ex(jmain, "node", &jnode);
+
+ json_object_object_del(jnode, "sequenceNumber");
+ jvalue = json_object_new_int(seq_num);
+ json_object_object_add(jnode, "sequenceNumber", jvalue);
+
+ prov_file_write(jmain, true);
+
+ res = true;
+done:
+
+ g_free(in_str);
+
+ if(jmain)
+ json_object_put(jmain);
+
+ return res;
+}
+
+bool prov_db_node_set_iv_seq(struct mesh_node *node, uint32_t iv, uint32_t seq)
+{
+ char *in_str;
+ json_object *jmain;
+ json_object *jnode;
+ json_object *jvalue;
+ uint16_t primary = node_get_primary(node);
+ bool res = false;
+
+ in_str = prov_file_read(prov_filename);
+ if (!in_str)
+ return false;
+
+ jmain = json_tokener_parse(in_str);
+ if (!jmain)
+ goto done;
+
+ jnode = find_node_by_primary(jmain, primary);
+ if (!jnode)
+ goto done;
+
+ json_object_object_del(jnode, "IVindex");
+
+ jvalue = json_object_new_int(iv);
+ json_object_object_add(jnode, "IVindex", jvalue);
+
+ json_object_object_del(jnode, "sequenceNumber");
+
+ jvalue = json_object_new_int(seq);
+ json_object_object_add(jnode, "sequenceNumber", jvalue);
+
+ prov_file_write(jmain, false);
+
+ res = true;
+done:
+
+ g_free(in_str);
+
+ if(jmain)
+ json_object_put(jmain);
+
+ return res;
+
+}
+
+bool prov_db_node_keys(struct mesh_node *node, GList *idxs, const char *desc)
+{
+ char *in_str;
+ json_object *jmain;
+ json_object *jnode;
+ json_object *jconfig;
+ json_object *jidxs;
+ uint16_t primary = node_get_primary(node);
+ const char *filename;
+ bool local = (node == node_get_local_node());
+ bool res = false;
+
+ if (local)
+ filename = local_filename;
+ else
+ filename = prov_filename;
+
+ in_str = prov_file_read(filename);
+ if (!in_str)
+ return false;
+
+ jmain = json_tokener_parse(in_str);
+ if (!jmain)
+ goto done;
+
+ jnode = find_node_by_primary(jmain, primary);
+ if (!jnode)
+ goto done;
+
+ json_object_object_get_ex(jnode, "configuration", &jconfig);
+ if (!jconfig)
+ goto done;
+
+ json_object_object_del(jconfig, desc);
+
+ if (idxs) {
+ jidxs = json_object_new_array();
+ put_uint16_list(jidxs, idxs);
+ json_object_object_add(jconfig, desc, jidxs);
+ }
+
+ prov_file_write(jmain, local);
+
+ res = true;
+done:
+
+ g_free(in_str);
+
+ if(jmain)
+ json_object_put(jmain);
+
+ return res;
+
+}
+
+static json_object *get_jmodel_obj(struct mesh_node *node, uint8_t ele_idx,
+ uint32_t model_id, json_object **jmain)
+{
+ char *in_str;
+ json_object *jnode;
+ json_object *jconfig;
+ json_object *jelements, *jelement;
+ json_object *jmodels, *jmodel = NULL;
+ uint16_t primary = node_get_primary(node);
+ const char *filename;
+ bool local = (node == node_get_local_node());
+
+ if (local)
+ filename = local_filename;
+ else
+ filename = prov_filename;
+
+ in_str = prov_file_read(filename);
+ if (!in_str)
+ return NULL;
+
+ *jmain = json_tokener_parse(in_str);
+ if (!(*jmain))
+ goto done;
+
+ if (local)
+ json_object_object_get_ex(*jmain, "node", &jnode);
+ else
+ jnode = find_node_by_primary(*jmain, primary);
+
+ if (!jnode)
+ goto done;
+
+ /* Configuration is mandatory for nodes in provisioning database */
+ json_object_object_get_ex(jnode, "configuration", &jconfig);
+ if (!jconfig)
+ goto done;
+
+ json_object_object_get_ex(jconfig, "elements", &jelements);
+ if (!jelements) {
+ goto done;
+ }
+
+ jelement = json_object_array_get_idx(jelements, ele_idx);
+ if (!jelement) {
+ goto done;
+ }
+
+ json_object_object_get_ex(jelement, "models", &jmodels);
+
+ if (!jmodels) {
+ jmodels = json_object_new_array();
+ json_object_object_add(jelement, "models", jmodels);
+ } else {
+ parse_configuration_models(node, ele_idx, jmodels,
+ model_id, &jmodel);
+ }
+
+ if (!jmodel) {
+ jmodel = json_object_new_object();
+
+ if ((model_id & 0xffff0000) == 0xffff0000)
+ put_uint16(jmodel, "modelId", model_id & 0xffff);
+ else
+ put_uint32(jmodel, "modelId", model_id);
+
+ json_object_array_add(jmodels, jmodel);
+ }
+
+done:
+
+ g_free(in_str);
+
+ if(!jmodel && *jmain)
+ json_object_put(*jmain);
+
+ return jmodel;
+
+}
+
+bool prov_db_add_binding(struct mesh_node *node, uint8_t ele_idx,
+ uint32_t model_id, uint16_t app_idx)
+{
+ json_object *jmain;
+ json_object *jmodel;
+ json_object *jvalue;
+ json_object *jbindings = NULL;
+ bool local = (node == node_get_local_node());
+
+ jmodel = get_jmodel_obj(node, ele_idx, model_id, &jmain);
+
+ if (!jmodel)
+ return false;
+
+ json_object_object_get_ex(jmodel, "bind", &jbindings);
+
+ if (!jbindings) {
+ jbindings = json_object_new_array();
+ json_object_object_add(jmodel, "bind", jbindings);
+ }
+
+ jvalue = json_object_new_int(app_idx);
+ json_object_array_add(jbindings, jvalue);
+
+ prov_file_write(jmain, local);
+
+ json_object_put(jmain);
+
+ return true;
+}
+
+bool prov_db_node_set_model_pub(struct mesh_node *node, uint8_t ele_idx,
+ uint32_t model_id,
+ struct mesh_publication *pub)
+{
+ json_object *jmain;
+ json_object *jmodel;
+ json_object *jpub;
+ json_object *jvalue;
+ bool local = (node == node_get_local_node());
+
+ jmodel = get_jmodel_obj(node, ele_idx, model_id, &jmain);
+
+ if (!jmodel)
+ return false;
+
+ json_object_object_del(jmodel, "publish");
+ if (!pub)
+ goto done;
+
+ jpub = json_object_new_object();
+
+ /* Save only required fields */
+ put_uint16(jpub, "address", pub->u.addr16);
+ put_uint16(jpub, "index", pub->app_idx);
+ jvalue = json_object_new_int(pub->ttl);
+ json_object_object_add(jpub, "ttl", jvalue);
+
+ json_object_object_add(jmodel, "publish", jpub);
+
+done:
+ prov_file_write(jmain, local);
+
+ json_object_put(jmain);
+
+ return true;
+}
+
+bool prov_db_add_new_node(struct mesh_node *node)
+{
+ char *in_str;
+ json_object *jmain;
+ json_object *jarray;
+ json_object *jnode;
+ json_object *jconfig;
+ json_object *jelements;
+ uint8_t num_ele;
+ uint16_t primary;
+ int i;
+ bool first_node;
+ bool res = false;
+
+ in_str = prov_file_read(prov_filename);
+ if (!in_str)
+ return false;
+
+ jmain = json_tokener_parse(in_str);
+ if (!jmain)
+ goto done;
+ json_object_object_get_ex(jmain, "nodes", &jarray);
+
+ if (!jarray) {
+ jarray = json_object_new_array();
+ first_node = true;
+ } else
+ first_node = false;
+
+ jnode = json_object_new_object();
+
+ /* Device key */
+ add_key(jnode, "deviceKey", node_get_device_key(node));
+
+ /* Net key */
+ jconfig = json_object_new_object();
+ add_node_idxs(jconfig, "netKeys", node_get_net_keys(node));
+
+ num_ele = node_get_num_elements(node);
+ if (num_ele == 0)
+ goto done;
+
+ jelements = json_object_new_array();
+
+ primary = node_get_primary(node);
+ if (IS_UNASSIGNED(primary))
+ goto done;
+
+ for (i = 0; i < num_ele; ++i) {
+ json_object *jelement;
+ json_object *jint;
+
+ jelement = json_object_new_object();
+
+ /* Element Index */
+ jint = json_object_new_int(i);
+ json_object_object_add(jelement, "elementIndex", jint);
+
+ /* Unicast */
+ put_uint16(jelement, "unicastAddress", primary + i);
+
+ json_object_array_add(jelements, jelement);
+ }
+
+ json_object_object_add(jconfig, "elements", jelements);
+
+ json_object_object_add(jnode, "configuration", jconfig);
+
+ json_object_array_add(jarray, jnode);
+
+ if (first_node)
+ json_object_object_add(jmain, "nodes", jarray);
+
+ prov_file_write(jmain, false);
+
+ res = true;
+done:
+
+ g_free(in_str);
+
+ if (jmain)
+ json_object_put(jmain);
+
+ return res;
+}
+
+static bool parse_node_composition(struct mesh_node *node, json_object *jcomp)
+{
+ json_object *jvalue;
+ json_object *jelements;
+ json_bool enable;
+ char *str;
+ struct mesh_node_composition comp;
+
+ json_object_object_get_ex(jcomp, "cid", &jvalue);
+ if (!jvalue)
+ return false;
+
+ str = (char *)json_object_get_string(jvalue);
+
+ if (sscanf(str, "%04hx", &comp.cid) != 1)
+ return false;
+
+ json_object_object_get_ex(jcomp, "pid", &jvalue);
+ if (!jvalue)
+ return false;
+
+ str = (char *)json_object_get_string(jvalue);
+
+ if (sscanf(str, "%04hx", &comp.vid) != 1)
+ return false;
+
+ json_object_object_get_ex(jcomp, "vid", &jvalue);
+ if (!jvalue)
+ return false;
+
+ str = (char *)json_object_get_string(jvalue);
+
+ if (sscanf(str, "%04hx", &comp.vid) != 1)
+ return false;
+
+ json_object_object_get_ex(jcomp, "crpl", &jvalue);
+ if (!jvalue)
+ return false;
+
+ str = (char *)json_object_get_string(jvalue);
+
+ if (sscanf(str, "%04hx", &comp.crpl) != 1)
+ return false;
+
+ /* Extract features */
+ json_object_object_get_ex(jcomp, "relay", &jvalue);
+ enable = json_object_get_boolean(jvalue);
+ comp.relay = (enable) ? true : false;
+
+ json_object_object_get_ex(jcomp, "proxy", &jvalue);
+ enable = json_object_get_boolean(jvalue);
+ comp.proxy = (enable) ? true : false;
+
+ json_object_object_get_ex(jcomp, "friend", &jvalue);
+ enable = json_object_get_boolean(jvalue);
+ comp.friend = (enable) ? true : false;
+
+ json_object_object_get_ex(jcomp, "lowPower", &jvalue);
+ enable = json_object_get_boolean(jvalue);
+ comp.lpn = (enable) ? true : false;
+
+ if (!node_set_composition(node, &comp))
+ return false;
+
+ json_object_object_get_ex(jcomp, "elements", &jelements);
+ if (!jelements)
+ return false;
+
+ return parse_composition_elements(node, jelements);
+}
+
+static bool parse_node(json_object *jnode, bool local)
+{
+ json_object *jconfig;
+ json_object *jelements;
+ json_object *jidxs;
+ json_object *jvalue;
+ json_object *jint;
+ uint8_t key[16];
+ char *value_str;
+ uint32_t idx;
+ struct mesh_node *node;
+
+ /* Device key */
+ if (!json_object_object_get_ex(jnode, "deviceKey", &jvalue) ||
+ !jvalue) {
+ if (!mesh_get_random_bytes(key, 16))
+ return false;
+
+ add_key(jnode, "deviceKey", key);
+ } else {
+ value_str = (char *)json_object_get_string(jvalue);
+ if (!str2hex(value_str, strlen(value_str), key, 16))
+ return false;;
+ }
+
+ node = node_new();
+
+ if (!node)
+ return false;
+
+ node_set_device_key(node, key);
+
+ json_object_object_get_ex(jnode, "IVindex", &jint);
+ if (jint)
+ idx = json_object_get_int(jint);
+ else
+ idx = 0;
+
+ node_set_iv_index(node, idx);
+ if (local) {
+ bool update = false;
+ json_object_object_get_ex(jnode, "IVupdate", &jint);
+ if (jint)
+ update = json_object_get_int(jint) ? true : false;
+ net_set_iv_index(idx, update);
+ }
+
+ if (json_object_object_get_ex(jnode, "sequenceNumber", &jint) &&
+ jint) {
+ int seq = json_object_get_int(jint);
+ node_set_sequence_number(node, seq);
+ }
+
+ /* Composition is mandatory for local node */
+ json_object_object_get_ex(jnode, "composition", &jconfig);
+ if ((jconfig && !parse_node_composition(node, jconfig)) ||
+ (!jconfig && local)) {
+ node_free(node);
+ return false;
+ }
+
+ /* Configuration is mandatory for nodes in provisioning database */
+ json_object_object_get_ex(jnode, "configuration", &jconfig);
+ if (!jconfig) {
+ if (local) {
+ /* This is an unprovisioned local device */
+ goto done;
+ } else {
+ node_free(node);
+ return false;
+ }
+ }
+
+ json_object_object_get_ex(jconfig, "elements", &jelements);
+ if (!jelements) {
+ node_free(node);
+ return false;
+ }
+
+ if (!parse_configuration_elements(node, jelements, local)) {
+ node_free(node);
+ return false;;
+ }
+
+ json_object_object_get_ex(jconfig, "netKeys", &jidxs);
+ if (!jidxs || (parse_node_keys(node, jidxs, false) == 0)) {
+ node_free(node);
+ return false;
+ }
+
+ json_object_object_get_ex(jconfig, "appKeys", &jidxs);
+ if (jidxs)
+ parse_node_keys(node, jidxs, true);
+
+ json_object_object_get_ex(jconfig, "defaultTTL", &jvalue);
+ if (jvalue) {
+ int ttl = json_object_get_int(jvalue);
+ node_set_default_ttl(node, ttl &TTL_MASK);
+ }
+
+done:
+ if (local && !node_set_local_node(node)) {
+ node_free(node);
+ return false;
+ }
+
+ return true;
+}
+
+bool prov_db_show(const char *filename)
+{
+ char *str;
+
+ str = prov_file_read(filename);
+ if (!str)
+ return false;
+
+ rl_printf("%s\n", str);
+ g_free(str);
+ return true;
+}
+
+static bool read_json_db(const char *filename, bool provisioner, bool local)
+{
+ char *str;
+ json_object *jmain;
+ json_object *jarray;
+ json_object *jprov;
+ json_object *jvalue;
+ json_object *jtemp;
+ uint8_t key[16];
+ int value_int;
+ char *value_str;
+ int len;
+ int i;
+ uint32_t index;
+ bool refresh = false;
+ bool res = false;
+
+ str = prov_file_read(filename);
+ if (!str) return false;
+
+ jmain = json_tokener_parse(str);
+ if (!jmain)
+ goto done;
+
+ if (local) {
+ json_object *jnode;
+ bool result;
+
+ json_object_object_get_ex(jmain, "node", &jnode);
+ if (!jnode) {
+ rl_printf("Cannot find \"node\" object");
+ goto done;
+ } else
+ result = parse_node(jnode, true);
+
+ /*
+ * If local node is provisioner, the rest of mesh settings
+ * are read from provisioning database.
+ */
+ if (provisioner) {
+ res = result;
+ goto done;
+ }
+ }
+
+ /* IV index */
+ json_object_object_get_ex(jmain, "IVindex", &jvalue);
+ if (!jvalue)
+ goto done;
+
+ index = json_object_get_int(jvalue);
+
+ json_object_object_get_ex(jmain, "IVupdate", &jvalue);
+ if (!jvalue)
+ goto done;
+
+ value_int = json_object_get_int(jvalue);
+
+ net_set_iv_index(index, value_int);
+
+ /* Network key(s) */
+ json_object_object_get_ex(jmain, "netKeys", &jarray);
+ if (!jarray)
+ goto done;
+
+ len = json_object_array_length(jarray);
+ rl_printf("# netkeys = %d\n", len);
+
+ for (i = 0; i < len; ++i) {
+ uint32_t idx;
+
+ jtemp = json_object_array_get_idx(jarray, i);
+ json_object_object_get_ex(jtemp, "index", &jvalue);
+ if (!jvalue)
+ goto done;
+ idx = json_object_get_int(jvalue);
+
+ json_object_object_get_ex(jtemp, "key", &jvalue);
+ if (!jvalue) {
+ if (!mesh_get_random_bytes(key, 16))
+ goto done;
+ add_key(jtemp, "key", key);
+ refresh = true;
+ } else {
+ value_str = (char *)json_object_get_string(jvalue);
+ if (!str2hex(value_str, strlen(value_str), key, 16)) {
+ goto done;
+ }
+ }
+
+ if (!keys_net_key_add(idx, key, false))
+ goto done;
+
+ json_object_object_get_ex(jtemp, "keyRefresh", &jvalue);
+ if (!jvalue)
+ goto done;
+
+ keys_set_kr_phase(idx, (uint8_t) json_object_get_int(jvalue));
+ }
+
+ /* App keys */
+ json_object_object_get_ex(jmain, "appKeys", &jarray);
+ if (jarray) {
+ len = json_object_array_length(jarray);
+ rl_printf("# appkeys = %d\n", len);
+
+ for (i = 0; i < len; ++i) {
+ int app_idx;
+ int net_idx;
+
+ jtemp = json_object_array_get_idx(jarray, i);
+ json_object_object_get_ex(jtemp, "index",
+ &jvalue);
+ if (!jvalue)
+ goto done;
+
+ app_idx = json_object_get_int(jvalue);
+ if (!CHECK_KEY_IDX_RANGE(app_idx))
+ goto done;
+
+ json_object_object_get_ex(jtemp, "key", &jvalue);
+ if (!jvalue) {
+ if (!mesh_get_random_bytes(key, 16))
+ goto done;
+ add_key(jtemp, "key", key);
+ refresh = true;
+ } else {
+ value_str =
+ (char *)json_object_get_string(jvalue);
+ str2hex(value_str, strlen(value_str), key, 16);
+ }
+
+ json_object_object_get_ex(jtemp, "boundNetKey",
+ &jvalue);
+ if (!jvalue)
+ goto done;
+
+ net_idx = json_object_get_int(jvalue);
+ if (!CHECK_KEY_IDX_RANGE(net_idx))
+ goto done;
+
+ keys_app_key_add(net_idx, app_idx, key, false);
+ }
+ }
+
+ /* Provisioner info */
+ json_object_object_get_ex(jmain, "provisioners", &jarray);
+ if (!jarray)
+ goto done;
+
+ len = json_object_array_length(jarray);
+ rl_printf("# provisioners = %d\n", len);
+
+ for (i = 0; i < len; ++i) {
+
+ jprov = json_object_array_get_idx(jarray, i);
+
+ /* Allocated unicast range */
+ json_object_object_get_ex(jprov, "allocatedUnicastRange",
+ &jtemp);
+ if (!jtemp) {
+ goto done;
+ }
+
+ if (!parse_unicast_range(jtemp)) {
+ rl_printf("Doneed to parse unicast range\n");
+ goto done;
+ }
+ }
+
+ json_object_object_get_ex(jmain, "nodes", &jarray);
+ if (!jarray) {
+ res = true;
+ goto done;
+ }
+
+ len = json_object_array_length(jarray);
+
+ rl_printf("# provisioned nodes = %d\n", len);
+ for (i = 0; i < len; ++i) {
+ json_object *jnode;
+ jnode = json_object_array_get_idx(jarray, i);
+
+ if (!jnode || !parse_node(jnode, false))
+ goto done;
+ }
+
+ res = true;
+done:
+
+ g_free(str);
+
+ if (res && refresh)
+ prov_file_write(jmain, false);
+
+ if (jmain)
+ json_object_put(jmain);
+
+ return res;
+}
+
+bool prov_db_read(const char *filename)
+{
+ prov_filename = filename;
+ return read_json_db(filename, true, false);
+}
+
+bool prov_db_read_local_node(const char *filename, bool provisioner)
+{
+ local_filename = filename;
+ return read_json_db(filename, provisioner, true);
+}
diff --git a/mesh/prov.c b/mesh/prov.c
new file mode 100644
index 0000000..89fc884
--- /dev/null
+++ b/mesh/prov.c
+/*
+ *
+ * BlueZ - Bluetooth protocol stack for Linux
+ *
+ * Copyright (C) 2017 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.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ */
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <errno.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <sys/uio.h>
+#include <wordexp.h>
+
+#include <readline/readline.h>
+#include <readline/history.h>
+#include <glib.h>
+
+#include "src/shared/util.h"
+#include "src/shared/ecc.h"
+
+#include "gdbus/gdbus.h"
+#include "monitor/uuid.h"
+#include "client/display.h"
+#include "node.h"
+#include "gatt.h"
+#include "crypto.h"
+#include "mesh-net.h"
+#include "util.h"
+#include "agent.h"
+#include "prov.h"
+#include "net.h"
+
+/* Provisioning Security Levels */
+#define MESH_PROV_SEC_HIGH 2
+#define MESH_PROV_SEC_MED 1
+#define MESH_PROV_SEC_LOW 0
+
+/* For Deployment, Security levels below HIGH are *not* recomended */
+#define mesh_gatt_prov_security() MESH_PROV_SEC_MED
+
+#define PROV_INVITE 0x00
+#define PROV_CAPS 0x01
+#define PROV_START 0x02
+#define PROV_PUB_KEY 0x03
+#define PROV_INP_CMPLT 0x04
+#define PROV_CONFIRM 0x05
+#define PROV_RANDOM 0x06
+#define PROV_DATA 0x07
+#define PROV_COMPLETE 0x08
+#define PROV_FAILED 0x09
+
+#define PROV_NO_OOB 0
+#define PROV_STATIC_OOB 1
+#define PROV_OUTPUT_OOB 2
+#define PROV_INPUT_OOB 3
+
+#define PROV_ERR_INVALID_PDU 0x01
+#define PROV_ERR_INVALID_FORMAT 0x02
+#define PROV_ERR_UNEXPECTED_PDU 0x03
+#define PROV_ERR_CONFIRM_FAILED 0x04
+#define PROV_ERR_INSUF_RESOURCE 0x05
+#define PROV_ERR_DECRYPT_FAILED 0x06
+#define PROV_ERR_UNEXPECTED_ERR 0x07
+#define PROV_ERR_CANT_ASSIGN_ADDR 0x08
+
+/* Expected Provisioning PDU sizes */
+static const uint16_t expected_pdu_size[] = {
+ 1 + 1, /* PROV_INVITE */
+ 1 + 1 + 2 + 1 + 1 + 1 + 2 + 1 + 2, /* PROV_CAPS */
+ 1 + 1 + 1 + 1 + 1 + 1, /* PROV_START */
+ 1 + 64, /* PROV_PUB_KEY */
+ 1, /* PROV_INP_CMPLT */
+ 1 + 16, /* PROV_CONFIRM */
+ 1 + 16, /* PROV_RANDOM */
+ 1 + 16 + 2 + 1 + 4 + 2 + 8, /* PROV_DATA */
+ 1, /* PROV_COMPLETE */
+ 1 + 1, /* PROV_FAILED */
+};
+
+typedef struct __packed {
+ uint8_t attention;
+} __attribute__ ((packed)) prov_invite;
+
+typedef struct {
+ uint8_t num_ele;
+ uint16_t algorithms;
+ uint8_t pub_type;
+ uint8_t static_type;
+ uint8_t output_size;
+ uint16_t output_action;
+ uint8_t input_size;
+ uint16_t input_action;
+} __attribute__ ((packed)) prov_caps;
+
+typedef struct {
+ uint8_t algorithm;
+ uint8_t pub_key;
+ uint8_t auth_method;
+ uint8_t auth_action;
+ uint8_t auth_size;
+} __attribute__ ((packed)) prov_start;
+
+typedef struct {
+ prov_invite invite;
+ prov_caps caps;
+ prov_start start;
+ uint8_t prv_pub_key[64];
+ uint8_t dev_pub_key[64];
+} __attribute__ ((packed)) conf_input;
+
+struct prov_data {
+ GDBusProxy *prov_in;
+ provision_done_cb prov_done;
+ void *user_data;
+ uint16_t net_idx;
+ uint16_t new_addr;
+ uint8_t state;
+ uint8_t eph_priv_key[32];
+ uint8_t ecdh_secret[32];
+ conf_input conf_in;
+ uint8_t rand_auth[32];
+ uint8_t salt[16];
+ uint8_t conf_key[16];
+ uint8_t mesh_conf[16];
+ uint8_t dev_key[16];
+};
+
+static uint8_t u16_highest_bit(uint16_t mask)
+{
+ uint8_t cnt = 0;
+
+ if (!mask) return 0xff;
+
+ while (mask & 0xfffe) {
+ cnt++;
+ mask >>= 1;
+ }
+
+ return cnt;
+}
+
+bool prov_open(struct mesh_node *node, GDBusProxy *prov_in, uint16_t net_idx,
+ provision_done_cb cb, void *user_data)
+{
+ uint8_t invite[] = { PROXY_PROVISIONING_PDU, PROV_INVITE, 0x10 };
+ struct prov_data *prov = node_get_prov(node);
+
+ if (prov) return false;
+
+ prov = g_new0(struct prov_data, 1);
+ prov->prov_in = prov_in;
+ prov->net_idx = net_idx;
+ prov->prov_done = cb;
+ prov->user_data = user_data;
+ node_set_prov(node, prov);
+ prov->conf_in.invite.attention = invite[2];
+ prov->state = PROV_INVITE;
+
+ rl_printf("Open-Node: %p\n", node);
+ rl_printf("Open-Prov: %p\n", prov);
+ rl_printf("Open-Prov: proxy %p\n", prov_in);
+
+ return mesh_gatt_write(prov_in, invite, sizeof(invite), NULL, node);
+}
+
+static bool prov_send_prov_data(void *node)
+{
+ struct prov_data *prov = node_get_prov(node);
+ uint8_t out[35] = { PROXY_PROVISIONING_PDU, PROV_DATA };
+ uint8_t key[16];
+ uint8_t nonce[13];
+ uint64_t mic;
+
+ if (prov == NULL) return false;
+
+ mesh_crypto_session_key(prov->ecdh_secret, prov->salt, key);
+ mesh_crypto_nonce(prov->ecdh_secret, prov->salt, nonce);
+ mesh_crypto_device_key(prov->ecdh_secret, prov->salt, prov->dev_key);
+
+ print_byte_array("S-Key\t", key, sizeof(key));
+ print_byte_array("S-Nonce\t", nonce, sizeof(nonce));
+ print_byte_array("DevKey\t", prov->dev_key, sizeof(prov->dev_key));
+
+ if (!net_get_key(prov->net_idx, out + 2))
+ return false;
+
+ put_be16(prov->net_idx, out + 2 + 16);
+ net_get_flags(prov->net_idx, out + 2 + 16 + 2);
+ put_be32(net_get_iv_index(NULL), out + 2 + 16 + 2 + 1);
+ put_be16(prov->new_addr, out + 2 + 16 + 2 + 1 + 4);
+
+ print_byte_array("Data\t", out + 2, 16 + 2 + 1 + 4 + 2);
+
+ mesh_crypto_aes_ccm_encrypt(nonce, key,
+ NULL, 0,
+ out + 2,
+ sizeof(out) - 2 - sizeof(mic),
+ out + 2,
+ &mic, sizeof(mic));
+
+ print_byte_array("DataEncrypted + mic\t", out + 2, sizeof(out) - 2);
+
+ prov->state = PROV_DATA;
+ return mesh_gatt_write(prov->prov_in, out, sizeof(out), NULL, node);
+}
+
+static bool prov_send_confirm(void *node)
+{
+ struct prov_data *prov = node_get_prov(node);
+ uint8_t out[18] = { PROXY_PROVISIONING_PDU, PROV_CONFIRM };
+
+ if (prov == NULL) return false;
+
+ mesh_get_random_bytes(prov->rand_auth, 16);
+
+ mesh_crypto_aes_cmac(prov->conf_key, prov->rand_auth,
+ sizeof(prov->rand_auth), out + 2);
+
+ prov->state = PROV_CONFIRM;
+ return mesh_gatt_write(prov->prov_in, out, sizeof(out), NULL, node);
+}
+
+static void prov_out_oob_done(oob_type_t type, void *buf, uint16_t len,
+ void *node)
+{
+ struct prov_data *prov = node_get_prov(node);
+
+ if (prov == NULL) return;
+
+ switch (type) {
+ default:
+ case NONE:
+ case OUTPUT:
+ prov_complete(node, PROV_ERR_INVALID_PDU);
+ return;
+
+ case ASCII:
+ case HEXADECIMAL:
+ if (len > 16)
+ prov_complete(node, PROV_ERR_INVALID_PDU);
+
+ memcpy(prov->rand_auth + 16, buf, len);
+ break;
+
+ case DECIMAL:
+ if (len != 4)
+ prov_complete(node, PROV_ERR_INVALID_PDU);
+
+ memcpy(prov->rand_auth +
+ sizeof(prov->rand_auth) -
+ sizeof(uint32_t),
+ buf, len);
+ break;
+ }
+
+ prov_send_confirm(node);
+}
+
+static uint32_t power_ten(uint8_t power)
+{
+ uint32_t ret = 1;
+
+ while (power--)
+ ret *= 10;
+
+ return ret;
+}
+
+char *in_action[3] = {
+ "Push",
+ "Twist",
+ "Enter"
+};
+
+static void prov_calc_ecdh(DBusMessage *message, void *node)
+{
+ struct prov_data *prov = node_get_prov(node);
+ uint8_t action = prov->conf_in.start.auth_action;
+ uint8_t size = prov->conf_in.start.auth_size;
+ char in_oob_display[100];
+ uint8_t *tmp = (void *) in_oob_display;
+ uint32_t in_oob;
+
+ if (prov == NULL) return;
+
+ /* Convert to Mesh byte order */
+ memcpy(tmp, prov->conf_in.dev_pub_key, 64);
+ swap_u256_bytes(tmp);
+ swap_u256_bytes(tmp + 32);
+
+ ecdh_shared_secret(tmp, prov->eph_priv_key, prov->ecdh_secret);
+
+ /* Convert to Mesh byte order */
+ swap_u256_bytes(prov->ecdh_secret);
+
+ mesh_crypto_s1(&prov->conf_in,
+ sizeof(prov->conf_in), prov->salt);
+
+ mesh_crypto_prov_conf_key(prov->ecdh_secret,
+ prov->salt, prov->conf_key);
+
+ switch (prov->conf_in.start.auth_method) {
+ default:
+ prov_complete(node, PROV_ERR_INVALID_PDU);
+ break;
+
+ case 0: /* No OOB */
+ prov_send_confirm(node);
+ break;
+
+ case 1: /* Static OOB */
+ agent_input_request(HEXADECIMAL,
+ 16,
+ prov_out_oob_done, node);
+ break;
+
+ case 2: /* Output OOB */
+ if (action <= 3)
+ agent_input_request(DECIMAL,
+ size,
+ prov_out_oob_done, node);
+ else
+ agent_input_request(ASCII,
+ size,
+ prov_out_oob_done, node);
+ break;
+
+ case 3: /* Input OOB */
+
+ if (action <= 2) {
+ mesh_get_random_bytes(&in_oob, sizeof(in_oob));
+ in_oob %= power_ten(size);
+ sprintf(in_oob_display, "%s %d on device\n",
+ in_action[action], in_oob);
+ put_be32(in_oob,
+ prov->rand_auth +
+ sizeof(prov->rand_auth) -
+ sizeof(uint32_t));
+ } else {
+ uint8_t in_ascii[9];
+ int i = size;
+
+ mesh_get_random_bytes(in_ascii, i);
+
+ while (i--) {
+ in_ascii[i] =
+ in_ascii[i] % ((26 * 2) + 10);
+ if (in_ascii[i] >= 10 + 26)
+ in_ascii[i] += 'a' - (10 + 26);
+ else if (in_ascii[i] >= 10)
+ in_ascii[i] += 'A' - 10;
+ else
+ in_ascii[i] += '0';
+ }
+ in_ascii[size] = '\0';
+ memcpy(prov->rand_auth + 16, in_ascii, size);
+ sprintf(in_oob_display,
+ "Enter %s on device\n",
+ in_ascii);
+ }
+ rl_printf("Agent String: %s\n", in_oob_display);
+ agent_output_request(in_oob_display);
+ break;
+ }
+}
+
+static void prov_send_pub_key(struct mesh_node *node)
+{
+ struct prov_data *prov = node_get_prov(node);
+ uint8_t out[66] = { PROXY_PROVISIONING_PDU, PROV_PUB_KEY };
+ GDBusReturnFunction cb = NULL;
+
+ if (prov == NULL) return;
+
+ if (prov->conf_in.start.pub_key)
+ cb = prov_calc_ecdh;
+
+ memcpy(out + 2, prov->conf_in.prv_pub_key, 64);
+ prov->state = PROV_PUB_KEY;
+ mesh_gatt_write(prov->prov_in, out, 66, cb, node);
+}
+
+static void prov_oob_pub_key(oob_type_t type, void *buf, uint16_t len,
+ void *node)
+{
+ struct prov_data *prov = node_get_prov(node);
+
+ if (prov == NULL) return;
+
+ memcpy(prov->conf_in.dev_pub_key, buf, 64);
+ prov_send_pub_key(node);
+}
+
+static void prov_start_cmplt(DBusMessage *message, void *node)
+{
+ struct prov_data *prov = node_get_prov(node);
+
+ if (prov == NULL) return;
+
+ if (prov->conf_in.start.pub_key)
+ agent_input_request(HEXADECIMAL, 64, prov_oob_pub_key, node);
+ else
+ prov_send_pub_key(node);
+}
+
+bool prov_data_ready(struct mesh_node *node, uint8_t *buf, uint8_t len)
+{
+ struct prov_data *prov = node_get_prov(node);
+ uint8_t sec_level = MESH_PROV_SEC_HIGH;
+ uint8_t out[35] = { PROXY_PROVISIONING_PDU };
+
+ if (prov == NULL || len < 2) return false;
+
+ buf++;
+ len--;
+
+ rl_printf("Got provisioning data (%d bytes)\n", len);
+
+ if (buf[0] > PROV_FAILED || expected_pdu_size[buf[0]] != len)
+ return prov_complete(node, PROV_ERR_INVALID_PDU);
+
+ print_byte_array("\t", buf, len);
+
+ if (buf[0] == PROV_FAILED)
+ return prov_complete(node, buf[1]);
+
+ /* Check provisioning state */
+ switch (prov->state) {
+ default:
+ return prov_complete(node, PROV_ERR_INVALID_PDU);
+
+ case PROV_INVITE:
+
+ if (buf[0] != PROV_CAPS)
+ return prov_complete(node,
+ PROV_ERR_INVALID_PDU);
+
+ /* Normalize to beginning of packed Param struct */
+ buf++;
+ len--;
+
+ /* Save Capability values */
+ memcpy(&prov->conf_in.caps, buf, len);
+
+ sec_level = mesh_gatt_prov_security();
+
+ if (sec_level == MESH_PROV_SEC_HIGH) {
+
+ /* Enforce High Security */
+ if (prov->conf_in.caps.pub_type != 1 &&
+ prov->conf_in.caps.static_type != 1)
+ return prov_complete(node,
+ PROV_ERR_INVALID_PDU);
+
+ } else if (sec_level == MESH_PROV_SEC_MED) {
+
+ /* Enforce Medium Security */
+ if (prov->conf_in.caps.pub_type != 1 &&
+ prov->conf_in.caps.static_type != 1 &&
+ prov->conf_in.caps.input_size == 0 &&
+ prov->conf_in.caps.output_size == 0)
+ return prov_complete(node,
+ PROV_ERR_INVALID_PDU);
+
+ }
+
+ /* Num Elements cannot be Zero */
+ if (prov->conf_in.caps.num_ele == 0)
+ return prov_complete(node,
+ PROV_ERR_INVALID_PDU);
+
+ /* All nodes must support Algorithm 0x0001 */
+ if (!(get_be16(buf + 1) & 0x0001))
+ return prov_complete(node,
+ PROV_ERR_INVALID_PDU);
+
+ /* Pub Key and Static type may not be > 1 */
+ if (prov->conf_in.caps.pub_type > 0x01 ||
+ prov->conf_in.caps.static_type > 0x01)
+ return prov_complete(node,
+ PROV_ERR_INVALID_PDU);
+
+ prov->new_addr =
+ net_obtain_address(prov->conf_in.caps.num_ele);
+
+ if (!prov->new_addr)
+ return prov_complete(node,
+ PROV_ERR_INVALID_PDU);
+
+ out[1] = PROV_START;
+ prov->conf_in.start.algorithm = 0;
+ prov->conf_in.start.pub_key =
+ prov->conf_in.caps.pub_type;
+
+ /* Compose START based on most secure values */
+ if (prov->conf_in.caps.static_type) {
+
+ prov->conf_in.start.auth_method =
+ PROV_STATIC_OOB;
+
+ } else if (prov->conf_in.caps.output_size >
+ prov->conf_in.caps.input_size) {
+
+ prov->conf_in.start.auth_method =
+ PROV_OUTPUT_OOB;
+ prov->conf_in.start.auth_action =
+ u16_highest_bit(get_be16(buf + 6));
+ prov->conf_in.start.auth_size =
+ prov->conf_in.caps.output_size;
+
+ } else if (prov->conf_in.caps.input_size > 0) {
+
+ prov->conf_in.start.auth_method =
+ PROV_INPUT_OOB;
+ prov->conf_in.start.auth_action =
+ u16_highest_bit(get_be16(buf + 9));
+ prov->conf_in.start.auth_size =
+ prov->conf_in.caps.input_size;
+ }
+
+ /* Range Check START values */
+ if (prov->conf_in.start.auth_size > 8)
+ return prov_complete(node,
+ PROV_ERR_INVALID_PDU);
+
+ prov->state = PROV_START;
+
+ memcpy(out + 2, &prov->conf_in.start, 5);
+
+ ecc_make_key(prov->conf_in.prv_pub_key,
+ prov->eph_priv_key);
+
+ /* Swap public key to share into Mesh byte ordering */
+ swap_u256_bytes(prov->conf_in.prv_pub_key);
+ swap_u256_bytes(prov->conf_in.prv_pub_key + 32);
+
+ return mesh_gatt_write(prov->prov_in, out, 7,
+ prov_start_cmplt, node);
+
+
+ case PROV_PUB_KEY:
+ if (buf[0] == PROV_PUB_KEY &&
+ !prov->conf_in.start.pub_key) {
+
+ memcpy(prov->conf_in.dev_pub_key, buf + 1, 64);
+ prov_calc_ecdh(NULL, node);
+ return true;
+
+ } else if (buf[0] == PROV_INP_CMPLT) {
+ agent_output_request_cancel();
+ return prov_send_confirm(node);
+ } else
+ return prov_complete(node,
+ PROV_ERR_INVALID_PDU);
+
+ case PROV_CONFIRM:
+ if (buf[0] != PROV_CONFIRM)
+ return prov_complete(node,
+ PROV_ERR_INVALID_PDU);
+
+ memcpy(prov->mesh_conf, buf + 1, 16);
+
+ out[1] = PROV_RANDOM;
+ memcpy(out + 2, prov->rand_auth, 16);
+
+ prov->state = PROV_RANDOM;
+ return mesh_gatt_write(prov->prov_in, out, 18,
+ NULL, node);
+
+ case PROV_RANDOM:
+ if (buf[0] != PROV_RANDOM)
+ return prov_complete(node,
+ PROV_ERR_INVALID_PDU);
+
+ /* Calculate New Salt while we still have
+ * both random values */
+ mesh_crypto_prov_prov_salt(prov->salt,
+ prov->rand_auth,
+ buf + 1,
+ prov->salt);
+
+ /* Calculate meshs Conf Value */
+ memcpy(prov->rand_auth, buf + 1, 16);
+ mesh_crypto_aes_cmac(prov->conf_key, prov->rand_auth,
+ sizeof(prov->rand_auth), out + 1);
+
+ /* Validate Mesh confirmation */
+ if (memcmp(out + 1, prov->mesh_conf, 16) != 0)
+ return prov_complete(node,
+ PROV_ERR_INVALID_PDU);
+
+ rl_printf("Confirmation Validated\n");
+
+ prov_send_prov_data(node);
+
+ return true;
+
+ case PROV_DATA:
+ if (buf[0] != PROV_COMPLETE)
+ return prov_complete(node,
+ PROV_ERR_INVALID_PDU);
+
+ return prov_complete(node, 0);
+ }
+
+
+
+ /* Compose appropriate reply for the prov state message */
+ /* Send reply via mesh_gatt_write() */
+ /* If done, call prov_done calllback and free prov housekeeping data */
+ rl_printf("Got provisioning data (%d bytes)\n", len);
+ print_byte_array("\t", buf, len);
+
+ return true;
+}
+
+bool prov_complete(struct mesh_node *node, uint8_t status)
+{
+ struct prov_data *prov = node_get_prov(node);
+ void *user_data;
+ provision_done_cb cb;
+
+ if (prov == NULL) return false;
+
+ if (status && prov->new_addr && prov->conf_in.caps.num_ele) {
+ net_release_address(prov->new_addr, prov->conf_in.caps.num_ele);
+ }
+
+ if (!status) {
+ node_set_num_elements(node, prov->conf_in.caps.num_ele);
+ node_set_primary(node, prov->new_addr);
+ node_set_device_key(node, prov->dev_key);
+ node_net_key_add(node, prov->net_idx);
+ }
+
+ user_data = prov->user_data;
+ cb = prov->prov_done;
+ g_free(prov);
+ node_set_prov(node, NULL);
+ if (cb) cb(user_data, status);
+
+ return true;
+}
diff --git a/mesh/util.c b/mesh/util.c
new file mode 100644
index 0000000..cb241b3
--- /dev/null
+++ b/mesh/util.c
+/*
+ *
+ * BlueZ - Bluetooth protocol stack for Linux
+ *
+ * Copyright (C) 2017 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.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <stdbool.h>
+#include <inttypes.h>
+#include <readline/readline.h>
+#include <glib.h>
+
+#include "client/display.h"
+#include "src/shared/util.h"
+#include "mesh-net.h"
+#include "util.h"
+
+struct cmd_menu {
+ const char *name;
+ const struct menu_entry *table;
+};
+
+static struct menu_entry *main_cmd_table;
+static struct menu_entry *current_cmd_table;
+static GList *menu_list;
+
+static char *main_menu_prompt;
+static int main_menu_point;
+
+static int match_menu_name(const void *a, const void *b)
+{
+ const struct cmd_menu *menu = a;
+ const char *name = b;
+
+ return strcasecmp(menu->name, name);
+}
+
+bool cmd_menu_init(const struct menu_entry *cmd_table)
+{
+ struct cmd_menu *menu;
+
+ if (main_cmd_table) {
+ rl_printf("Main menu already registered\n");
+ return false;
+ }
+
+ menu = g_malloc(sizeof(struct cmd_menu));
+ if (!menu)
+ return false;
+
+ menu->name = "meshctl";
+ menu->table = cmd_table;
+ menu_list = g_list_append(menu_list, menu);
+ main_cmd_table = (struct menu_entry *) cmd_table;
+ current_cmd_table = (struct menu_entry *) main_cmd_table;
+
+ return true;
+}
+
+void cmd_menu_main(bool forced)
+{
+ current_cmd_table = main_cmd_table;
+
+ if (!forced) {
+ rl_set_prompt(main_menu_prompt);
+ rl_replace_line("", 0);
+ rl_point = main_menu_point;
+ rl_redisplay();
+ }
+
+ g_free(main_menu_prompt);
+ main_menu_prompt = NULL;
+}
+
+bool add_cmd_menu(const char *name, const struct menu_entry *cmd_table)
+{
+ struct cmd_menu *menu;
+ GList *l;
+
+ l = g_list_find_custom(menu_list, name, match_menu_name);
+ if (l) {
+ menu = l->data;
+ rl_printf("menu \"%s\" already registered\n", menu->name);
+ return false;
+ }
+
+ menu = g_malloc(sizeof(struct cmd_menu));
+ if (!menu)
+ return false;
+
+ menu->name = name;
+ menu->table = cmd_table;
+ menu_list = g_list_append(menu_list, menu);
+
+ return true;
+}
+
+void set_menu_prompt(const char *name, const char *id)
+{
+ char *prompt;
+
+ prompt = g_strdup_printf(COLOR_BLUE "[%s%s%s]" COLOR_OFF "# ", name,
+ id ? ": Target = " : "", id ? id : "");
+ rl_set_prompt(prompt);
+ g_free(prompt);
+ rl_on_new_line();
+}
+
+bool switch_cmd_menu(const char *name)
+{
+ GList *l;
+ struct cmd_menu *menu;
+
+ l = g_list_find_custom(menu_list, name, match_menu_name);
+ if(!l)
+ return false;
+
+ menu = l->data;
+ current_cmd_table = (struct menu_entry *) menu->table;
+
+ main_menu_point = rl_point;
+ main_menu_prompt = g_strdup(rl_prompt);
+
+ return true;
+}
+
+void process_menu_cmd(const char *cmd, const char *arg)
+{
+ int i;
+ int len;
+ struct menu_entry *cmd_table = current_cmd_table;
+
+ if (!current_cmd_table)
+ return;
+
+ len = strlen(cmd);
+
+ for (i = 0; cmd_table[i].cmd; i++) {
+ if (strncmp(cmd, cmd_table[i].cmd, len))
+ continue;
+
+ if (cmd_table[i].func) {
+ cmd_table[i].func(arg);
+ return;
+ }
+ }
+
+ if (strncmp(cmd, "help", len)) {
+ rl_printf("Invalid command\n");
+ return;
+ }
+
+ print_cmd_menu(cmd_table);
+}
+
+void print_cmd_menu(const struct menu_entry *cmd_table)
+{
+ int i;
+
+ rl_printf("Available commands:\n");
+
+ for (i = 0; cmd_table[i].cmd; i++) {
+ if (cmd_table[i].desc)
+ rl_printf(" %s %-*s %s\n", cmd_table[i].cmd,
+ (int)(40 - strlen(cmd_table[i].cmd)),
+ cmd_table[i].arg ? : "",
+ cmd_table[i].desc ? : "");
+ }
+
+}
+
+void cmd_menu_cleanup(void)
+{
+ main_cmd_table = NULL;
+ current_cmd_table = NULL;
+
+ g_list_free_full(menu_list, g_free);
+}
+
+void print_byte_array(const char *prefix, const void *ptr, int len)
+{
+ const uint8_t *data = ptr;
+ char *line, *bytes;
+ int i;
+
+ line = g_malloc(strlen(prefix) + (16 * 3) + 2);
+ sprintf(line, "%s ", prefix);
+ bytes = line + strlen(prefix) + 1;
+
+ for (i = 0; i < len; ++i) {
+ sprintf(bytes, "%2.2x ", data[i]);
+ if ((i + 1) % 16) {
+ bytes += 3;
+ } else {
+ rl_printf("\r%s\n", line);
+ bytes = line + strlen(prefix) + 1;
+ }
+ }
+
+ if (i % 16)
+ rl_printf("\r%s\n", line);
+
+ g_free(line);
+}
+
+bool str2hex(const char *str, uint16_t in_len, uint8_t *out,
+ uint16_t out_len)
+{
+ uint16_t i;
+
+ if (in_len < out_len * 2)
+ return false;
+
+ for (i = 0; i < out_len; i++) {
+ if (sscanf(&str[i * 2], "%02hhx", &out[i]) != 1)
+ return false;
+ }
+
+ return true;
+}
+
+size_t hex2str(uint8_t *in, size_t in_len, char *out,
+ size_t out_len)
+{
+ static const char hexdigits[] = "0123456789abcdef";
+ size_t i;
+
+ if(in_len * 2 > out_len - 1)
+ return 0;
+
+ for (i = 0; i < in_len; i++) {
+ out[i * 2] = hexdigits[in[i] >> 4];
+ out[i * 2 + 1] = hexdigits[in[i] & 0xf];
+ }
+
+ out[in_len * 2] = '\0';
+ return i;
+}
+
+uint16_t mesh_opcode_set(uint32_t opcode, uint8_t *buf)
+{
+ if (opcode <= 0x7e) {
+ buf[0] = opcode;
+ return 1;
+ } else if (opcode >= 0x8000 && opcode <= 0xbfff) {
+ put_be16(opcode, buf);
+ return 2;
+ } else if (opcode >= 0xc00000 && opcode <= 0xffffff) {
+ buf[0] = (opcode >> 16) & 0xff;
+ put_be16(opcode, buf + 1);
+ return 3;
+ } else {
+ rl_printf("Illegal Opcode %x", opcode);
+ return 0;
+ }
+}
+
+bool mesh_opcode_get(const uint8_t *buf, uint16_t sz, uint32_t *opcode, int *n)
+{
+ if (!n || !opcode || sz < 1) return false;
+
+ switch (buf[0] & 0xc0) {
+ case 0x00:
+ case 0x40:
+ /* RFU */
+ if (buf[0] == 0x7f)
+ return false;
+
+ *n = 1;
+ *opcode = buf[0];
+ break;
+
+ case 0x80:
+ if (sz < 2)
+ return false;
+
+ *n = 2;
+ *opcode = get_be16(buf);
+ break;
+
+ case 0xc0:
+ if (sz < 3)
+ return false;
+
+ *n = 3;
+ *opcode = get_be16(buf + 1);
+ *opcode |= buf[0] << 16;
+ break;
+
+ default:
+ rl_printf("Bad Packet:\n");
+ print_byte_array("\t", (void *) buf, sz);
+ return false;
+ }
+
+ return true;
+}
+
+const char *mesh_status_str(uint8_t status)
+{
+ switch (status) {
+ case MESH_STATUS_SUCCESS: return "Success";
+ case MESH_STATUS_INVALID_ADDRESS: return "Invalid Address";
+ case MESH_STATUS_INVALID_MODEL: return "Invalid Model";
+ case MESH_STATUS_INVALID_APPKEY: return "Invalid AppKey";
+ case MESH_STATUS_INVALID_NETKEY: return "Invalid NetKey";
+ case MESH_STATUS_INSUFF_RESOURCES: return "Insufficient Resources";
+ case MESH_STATUS_IDX_ALREADY_STORED: return "Key Idx Already Stored";
+ case MESH_STATUS_INVALID_PUB_PARAM: return "Invalid Publish Parameters";
+ case MESH_STATUS_NOT_SUB_MOD: return "Not a Subscribe Model";
+ case MESH_STATUS_STORAGE_FAIL: return "Storage Failure";
+ case MESH_STATUS_FEAT_NOT_SUP: return "Feature Not Supported";
+ case MESH_STATUS_CANNOT_UPDATE: return "Cannot Update";
+ case MESH_STATUS_CANNOT_REMOVE: return "Cannot Remove";
+ case MESH_STATUS_CANNOT_BIND: return "Cannot bind";
+ case MESH_STATUS_UNABLE_CHANGE_STATE: return "Unable to change state";
+ case MESH_STATUS_CANNOT_SET: return "Cannot set";
+ case MESH_STATUS_UNSPECIFIED_ERROR: return "Unspecified error";
+ case MESH_STATUS_INVALID_BINDING: return "Invalid Binding";
+
+ default: return "Unknown";
+ }
+}
+
+void print_model_pub(uint16_t ele_addr, uint32_t mod_id,
+ struct mesh_publication *pub)
+{
+ rl_printf("\tElement: %4.4x\n", ele_addr);
+ rl_printf("\tPub Addr: %4.4x", pub->u.addr16);
+ if (mod_id > 0xffff0000)
+ rl_printf("\tModel: %8.8x \n", mod_id);
+ else
+ rl_printf("\tModel: %4.4x \n", (uint16_t) (mod_id & 0xffff));
+ rl_printf("\tApp Key Idx: %4.4x", pub->app_idx);
+ rl_printf("\tTTL: %2.2x", pub->ttl);
+}
+
+void swap_u256_bytes(uint8_t *u256)
+{
+ int i;
+
+ /* End-to-End byte reflection of 32 octet buffer */
+ for (i = 0; i < 16; i++) {
+ u256[i] ^= u256[31 - i];
+ u256[31 - i] ^= u256[i];
+ u256[i] ^= u256[31 - i];
+ }
+}