Return-Path: MIME-Version: 1.0 In-Reply-To: References: <20170814190119.8684-1-brian.gix@intel.com> <20170814190119.8684-5-brian.gix@intel.com> From: Luiz Augusto von Dentz Date: Wed, 16 Aug 2017 15:29:26 +0300 Message-ID: Subject: Re: [PATCH 4/5] mesh: Baseline Mesh implementation To: "Gix, Brian" Cc: "linux-bluetooth@vger.kernel.org" , Marcel Holtmann Content-Type: text/plain; charset="UTF-8" List-ID: Hi Brian, On Tue, Aug 15, 2017 at 6:36 PM, Gix, Brian wrote: > Hi Luiz, > > We actually use the agent.c code for Out-Of-Band data input during Provisioning. Which seems that was based on client/agent.c though there exists rl_prompt_input and rl_release_prompt which basically does the same thing now, but with introduction of input_request the handling becomes quite a bit different. I tried doing the conversion myself but it makes very no sense to me to name this agent, in fact much of what it does with types, HEXADECIMAL, etc could be done using different callbacks instead of a global variable. >> -----Original Message----- >> From: Luiz Augusto von Dentz [mailto:luiz.dentz@gmail.com] >> Sent: Tuesday, August 15, 2017 2:07 AM >> To: Gix, Brian >> Cc: linux-bluetooth@vger.kernel.org; Marcel Holtmann >> >> Subject: Re: [PATCH 4/5] mesh: Baseline Mesh implementation >> >> 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 -- Luiz Augusto von Dentz