Return-Path: MIME-Version: 1.0 In-Reply-To: <20170814190119.8684-5-brian.gix@intel.com> References: <20170814190119.8684-1-brian.gix@intel.com> <20170814190119.8684-5-brian.gix@intel.com> From: Luiz Augusto von Dentz Date: Tue, 15 Aug 2017 12:07:16 +0300 Message-ID: Subject: Re: [PATCH 4/5] mesh: Baseline Mesh implementation To: Brian Gix Cc: "linux-bluetooth@vger.kernel.org" , Marcel Holtmann Content-Type: text/plain; charset="UTF-8" List-ID: Hi Brian, On Mon, Aug 14, 2017 at 10:01 PM, Brian Gix wrote: > --- > mesh/agent.c | 276 ++++++ > mesh/config-client.c | 667 +++++++++++++++ > mesh/config-server.c | 165 ++++ > mesh/crypto.c | 1168 ++++++++++++++++++++++++++ > mesh/gatt.c | 609 ++++++++++++++ > mesh/main.c | 2269 ++++++++++++++++++++++++++++++++++++++++++++++++++ > mesh/net.c | 2184 ++++++++++++++++++++++++++++++++++++++++++++++++ > mesh/node.c | 879 +++++++++++++++++++ > mesh/onoff-model.c | 306 +++++++ > mesh/prov-db.c | 1599 +++++++++++++++++++++++++++++++++++ > mesh/prov.c | 664 +++++++++++++++ > mesh/util.c | 369 ++++++++ > 12 files changed, 11155 insertions(+) > create mode 100644 mesh/agent.c > create mode 100644 mesh/config-client.c > create mode 100644 mesh/config-server.c > create mode 100644 mesh/crypto.c > create mode 100644 mesh/gatt.c > create mode 100644 mesh/main.c > create mode 100644 mesh/net.c > create mode 100644 mesh/node.c > create mode 100644 mesh/onoff-model.c > create mode 100644 mesh/prov-db.c > create mode 100644 mesh/prov.c > create mode 100644 mesh/util.c > > diff --git a/mesh/agent.c b/mesh/agent.c > new file mode 100644 > index 0000000..0944862 > --- /dev/null > +++ b/mesh/agent.c > @@ -0,0 +1,276 @@ > +/* > + * > + * 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 > +#endif > + > +#include > +#include > +#include > +#include > +#include > + > +#include > + > +#include > +#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(); > +} The whole agent.c can be removed as well. > 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 > @@ -0,0 +1,667 @@ > +/* > + * > + * 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 > +#endif > + > +#include > +#include > +#include > +#include > +#include > +#include > +#include > +#include > +#include > +#include > +#include > +#include > + > +#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", "", cmd_set_node, > + "Set target node to configure"}, > + {"get-composition", "[]", cmd_get_composition, > + "Get Composition Data"}, > + {"add-netkey", "", cmd_add_net_key, > + "Add network key"}, > + {"del-netkey", "", cmd_del_net_key, > + "Delete network key"}, > + {"add-appkey", "", cmd_add_app_key, > + "Add application key"}, > + {"del-appkey", "", cmd_del_app_key, > + "Delete application key"}, > + {"bind", " [cid]", > + cmd_bind, "Bind app key to a model"}, > + {"set-ttl", "", cmd_set_ttl, > + "Set default TTL"}, > + {"get-ttl", NULL, cmd_get_ttl, > + "Get default TTL"}, > + {"set-pub", " " > + " ", > + 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 > @@ -0,0 +1,165 @@ > +/* > + * > + * 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 > +#endif > + > +#include > +#include > +#include > +#include > +#include > +#include > +#include > +#include > +#include > +#include > +#include > +#include > + > +#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 > @@ -0,0 +1,1168 @@ > +/* > + * > + * 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 > +#endif > + > +#include > +#include > +#include > +#include > + > +#include > + > +#include > + > +#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 > @@ -0,0 +1,609 @@ > +/* > + * > + * 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 > +#endif > + > +#include > +#include > +#include > +#include > +#include > +#include > +#include > + > +#include > +#include > +#include > + > +#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 > @@ -0,0 +1,2269 @@ > +/* > + * > + * 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 > +#endif > + > +#include > +#include > +#include > +#include > +#include > +#include > +#include > +#include > + > +#include > +#include > +#include > +#include > +#include > +#include "bluetooth/bluetooth.h" > + > +#include > +#include > +#include > + > +#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 = ""; > + > + 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 = ""; > + > + 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", "", cmd_select, "Select default controller"}, > + { "info", "[dev]", cmd_info, "Device information"}, > + { "connect", "[net_idx]",cmd_connect, "Connect to mesh network"}, > + { "discover-unprovisioned", "", cmd_scan_unprovisioned_devices, > + "Look for devices to provision" }, > + { "provision", "", cmd_start_prov, "Initiate provisioning"}, > + { "power", "", 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 " }, > + { "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 > @@ -0,0 +1,2184 @@ > +/* > + * > + * 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 > +#endif > + > +#include > +#include > +#include > +#include > +#include > +#include > + > +#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 > @@ -0,0 +1,879 @@ > +/* > + * > + * 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 > +#endif > + > +#include > +#include > +#include > +#include > +#include > +#include > +#include > + > +#include > +#include > +#include > + > +#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 > @@ -0,0 +1,306 @@ > +/* > + * > + * 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 > +#endif > + > +#include > +#include > +#include > +#include > +#include > +#include > +#include > +#include > +#include > +#include > +#include > +#include > + > +#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", "", 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 > @@ -0,0 +1,1599 @@ > +/* > + * > + * 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 > +#endif > + > +#include > +#include > +#include > +#include > +#include > +#include > +#include > +#include > + > +#include > +#include > + > +#include > +#include > + > +#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 > @@ -0,0 +1,664 @@ > +/* > + * > + * 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 > +#endif > + > +#include > +#include > +#include > +#include > +#include > +#include > +#include > + > +#include > +#include > +#include > + > +#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 > @@ -0,0 +1,369 @@ > +/* > + * > + * 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 > +#endif > + > +#include > +#include > +#include > +#include > +#include > + > +#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]; > + } > +} > -- > 2.9.5 > -- Luiz Augusto von Dentz