Return-Path: From: Brian Gix To: Marcel Holtmann , Luiz Augusto von Dentz , linux-bluetooth@vger.kernel.org Cc: Brian Gix Subject: [PATCH BlueZ v4 06/14] meshd: Upper and Lower mesh transport Date: Mon, 30 Apr 2018 14:03:11 -0700 Message-Id: <20180430210319.25137-7-brian.gix@intel.com> In-Reply-To: <20180430210319.25137-1-brian.gix@intel.com> References: <20180430210319.25137-1-brian.gix@intel.com> List-ID: --- meshd/src/net.c | 3639 +++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 3639 insertions(+) create mode 100644 meshd/src/net.c diff --git a/meshd/src/net.c b/meshd/src/net.c new file mode 100644 index 000000000..12f11b2b2 --- /dev/null +++ b/meshd/src/net.c @@ -0,0 +1,3639 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2018 Intel Corporation. All rights reserved. + * + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + */ + +#ifdef HAVE_CONFIG_H +#include +#endif + +#include +#include +#include +#include + +#include "meshd/common/mesh-defs.h" +#include "meshd/common/util.h" + +#include "meshd/src/display.h" +#include "meshd/src/crypto.h" +#include "meshd/src/mesh.h" +#include "meshd/src/node.h" +#include "meshd/src/net.h" +#include "meshd/src/mesh-io.h" +#include "meshd/src/friend.h" +#include "meshd/src/storage.h" +#include "meshd/src/model.h" +#include "meshd/src/appkey.h" +#include "meshd/src/prov.h" +#include "meshd/src/provision.h" + +#define abs_diff(a, b) ((a) > (b) ? (a) - (b) : (b) - (a)) + +#define IV_IDX_DIFF_RANGE 42 + +/* #define IV_IDX_UPD_MIN (60) 1 minute for Testing */ +#define IV_IDX_UPD_MIN (60 * 60 * 96) /* 96 Hours - per Spec */ +#define IV_IDX_UPD_HOLD (IV_IDX_UPD_MIN/2) +#define IV_IDX_UPD_MAX (IV_IDX_UPD_MIN + IV_IDX_UPD_HOLD) + +#define iv_is_updating(net) ((net)->iv_upd_state == IV_UPD_UPDATING) + +#define IV_UPDATE_SEQ_TRIGGER 0x800000 /* Half of Seq-Nums expended */ + +#define SEG_TO 2 +#define MSG_TO 60 + +#define DEFAULT_MIN_DELAY 0 +#define DEFAULT_MAX_DELAY 25 + +#define DEFAULT_TRANSMIT_COUNT 1 +#define DEFAULT_TRANSMIT_INTERVAL 100 + +#define BEACON_TYPE_SNB 0x01 + +#define BEACON_INTERVAL_MIN 10 +#define BEACON_INTERVAL_MAX 600 + +#define SAR_KEY(src, seq0) ((((uint32_t)(seq0)) << 16) | (src)) + +enum _iv_upd_state { + /* Allows acceptance of any iv_index secure net beacon */ + IV_UPD_INIT, + /* Normal, can transition, accept current or old */ + IV_UPD_NORMAL, + /* Updating proc running, we use old, accept old or new */ + IV_UPD_UPDATING, + /* Normal, can *not* transition, accept current or old iv_index */ + IV_UPD_NORMAL_HOLD, +}; + +struct net_key { + struct mesh_key_set key_set; + unsigned int beacon_id; + uint8_t key[16]; + uint8_t beacon_key[16]; + uint8_t network_id[8]; +}; + +struct mesh_beacon { + struct l_timeout *timeout; + uint32_t ts; + uint16_t observe_period; + uint16_t observed; + uint16_t expected; + uint8_t half_period; + uint8_t beacon[23]; +}; + +struct mesh_subnet { + struct mesh_net *net; + uint16_t idx; + struct net_key *tx; + struct net_key current; + struct net_key updated; + struct mesh_beacon snb; + uint8_t key_refresh; + uint8_t kr_phase; +}; + +struct mesh_net { + int ref_count; + struct mesh_io *io; + struct mesh_node *local_node; + struct mesh_prov *prov; + void *jconfig_local; + const char *cfg_file; + struct l_queue *app_keys; + unsigned int pkt_id; + unsigned int bea_id; + unsigned int beacon_id; + unsigned int key_id_next; + unsigned int sar_id_next; + + bool friend_enable; + bool beacon_enable; + bool proxy_enable; + bool provisioner; + bool provisioned; + bool friend_seq; + struct l_timeout *iv_update_timeout; + enum _iv_upd_state iv_upd_state; + + bool iv_update; + uint32_t instant; /* Controller Instant of recent Rx */ + uint32_t iv_index; + uint32_t seq_num; + uint32_t cached_seq_num; + uint16_t crpl; + uint16_t src_addr; + uint16_t last_addr; + uint16_t friend_addr; + uint16_t tx_interval; + uint16_t tx_cnt; + uint8_t chan; /* Channel of recent Rx */ + uint8_t default_ttl; + uint8_t tid; + uint8_t window_accuracy; + + struct { + bool enable; + uint16_t interval; + uint8_t count; + } relay; + + struct mesh_net_heartbeat heartbeat; + + struct l_queue *subnets; + struct l_queue *msg_cache; + struct l_queue *sar_in; + struct l_queue *sar_out; + struct l_queue *frnd_msgs; + struct l_queue *friends; + struct l_queue *destinations; + struct l_queue *fast_cache; + struct l_queue *key_sets; + + uint8_t prov_priv_key[32]; + + /* Unprovisioned Identity */ + char id_name[20]; + uint8_t id_uuid[16]; + + /* Provisioner: unicast address range */ + struct mesh_net_addr_range prov_uni_addr; + + /* Test Data */ + uint8_t prov_rand[16]; + uint8_t test_bd_addr[6]; + struct mesh_net_prov_caps prov_caps; + bool test_mode; +}; + +struct mesh_msg { + uint16_t src; + uint32_t seq; + uint32_t mic; +}; + +struct mesh_sar { + unsigned int id; + struct l_timeout *seg_timeout; + struct l_timeout *msg_timeout; + mesh_net_status_func_t status_func; + void *user_data; + uint32_t flags; + uint32_t last_nak; + uint32_t iv_index; + uint32_t seqAuth; + uint16_t seqZero; + uint16_t app_idx; + uint16_t src; + uint16_t remote; + uint16_t len; + bool szmic; + bool frnd; + bool frnd_cred; + uint8_t ttl; + uint8_t last_seg; + uint8_t key_id; + uint8_t buf[4]; /* Large enough for ACK-Flags and MIC */ +}; + +struct mesh_destination { + uint16_t dst; + uint16_t ref_cnt; +}; + +struct msg_rx { + const uint8_t *data; + uint32_t iv_index; + uint32_t seq; + uint16_t src; + uint16_t dst; + uint16_t size; + uint8_t tc; + bool done; + bool szmic; + union { + struct { + uint16_t app_idx; + uint8_t key_id; + } m; + struct { + uint16_t seq0; + } a; + struct { + uint8_t opcode; + } c; + } u; +}; + +struct net_decode { + struct mesh_net *net; + struct mesh_friend *frnd; + struct mesh_key_set *key_set; + uint8_t *packet; + uint32_t iv_index; + uint8_t size; + uint8_t nid; + bool proxy; +}; + +static inline struct mesh_subnet *get_primary_subnet(struct mesh_net *net) +{ + return l_queue_peek_head(net->subnets); +} + +static bool match_key_index(const void *a, const void *b) +{ + const struct mesh_subnet *subnet = a; + uint16_t idx = L_PTR_TO_UINT(b); + + return subnet->idx == idx; +} + +static bool match_key_set(const void *a, const void *b) +{ + const struct mesh_subnet *subnet = a; + const struct mesh_key_set *key_set = b; + + return (key_set == &subnet->current.key_set) || + (key_set == &subnet->updated.key_set); +} + +static bool match_network_id(const void *a, const void *b) +{ + const struct mesh_subnet *subnet = a; + const uint8_t *network_id = b; + + return ((memcmp(subnet->current.network_id, network_id, 8) == 0) || + (memcmp(subnet->updated.network_id, network_id, 8) == 0)); +} + +static void idle_mesh_heartbeat_send(void *net) +{ + mesh_net_heartbeat_send(net); +} + +static void trigger_heartbeat(struct mesh_net *net, uint16_t feature, + bool in_use) +{ + struct mesh_net_heartbeat *hb = &net->heartbeat; + + l_info("%s: %4.4x --> %d", __func__, feature, in_use); + if (in_use) { + if (net->heartbeat.features & feature) + return; /* no change */ + + hb->features |= feature; + } else { + if (!(hb->features & feature)) + return; /* no change */ + + hb->features &= ~feature; + } + + if (!(hb->pub_features & feature)) + return; /* not interested in this feature */ + + l_idle_oneshot(idle_mesh_heartbeat_send, net, NULL); + +} + +static bool match_by_friend(const void *a, const void *b) +{ + const struct mesh_friend *frnd = a; + uint16_t dst = L_PTR_TO_UINT(b); + + return frnd->dst == dst; +} + +static void free_friend_internals(struct mesh_friend *frnd) +{ + if (frnd->pkt_cache) + l_queue_destroy(frnd->pkt_cache, l_free); + + if (frnd->grp_list) + l_free(frnd->grp_list); + + frnd->pkt_cache = NULL; + frnd->grp_list = NULL; + mesh_net_remove_keyset(frnd->net, &frnd->key_set); + mesh_net_remove_keyset(frnd->net, &frnd->new_key_set); +} + +static void frnd_kr_phase1(void *a, void *b) +{ + struct mesh_friend *frnd = a; + const uint8_t *key = b; + uint8_t p[9] = {0x01}; + + l_put_be16(frnd->dst, p + 1); + l_put_be16(frnd->net->src_addr, p + 3); + l_put_be16(frnd->lp_cnt, p + 5); + l_put_be16(frnd->fn_cnt, p + 7); + + mesh_crypto_k2(key, p, sizeof(p), &frnd->new_key_set.nid, + frnd->new_key_set.enc_key, + frnd->new_key_set.privacy_key); + + mesh_net_add_keyset(frnd->net, &frnd->new_key_set); + l_info("Add New KeySet %2.2x for %4.4x", + frnd->new_key_set.nid, frnd->dst); + l_info("Outgoing with %2.2x", frnd->key_set.nid); +} + +static void frnd_kr_phase2(void *a, void *b) +{ + struct mesh_friend *frnd = a; + + /* + * I think that a Friend should use Old Key as long as possible + * Because a Friend Node will enter Phase 3 before it's LPN. + * Alternatively, the FN could keep the Old Friend Keys until it + * receives it's first Poll using the new keys (?) + */ + + l_info("Use Both KeySet %2.2x && %2.2x for %4.4x", + frnd->key_set.nid, frnd->new_key_set.nid, frnd->dst); +} + +static void frnd_kr_phase3(void *a, void *b) +{ + struct mesh_friend *frnd = a; + struct mesh_net *net = b; + + l_info("Replace KeySet %2.2x with %2.2x for %4.4x", + frnd->key_set.nid, frnd->new_key_set.nid, frnd->dst); + frnd->key_set = frnd->new_key_set; + mesh_net_remove_keyset(net, &frnd->new_key_set); + frnd->new_key_set.nid = 0xff; +} + +/* TODO: add net key idx? For now, use primary net key */ +struct mesh_friend *mesh_friend_new(struct mesh_net *net, uint16_t dst, + uint8_t ele_cnt, uint8_t frd, + uint8_t frw, uint32_t fpt, + uint16_t fn_cnt, uint16_t lp_cnt) +{ + struct mesh_subnet *subnet; + uint8_t p[9] = {0x01}; + struct mesh_friend *frnd = l_queue_find(net->friends, + match_by_friend, L_UINT_TO_PTR(dst)); + + if (frnd) { + /* Kill all timers and empty cache for this friend */ + free_friend_internals(frnd); + l_timeout_remove(frnd->timeout); + frnd->timeout = NULL; + } else { + frnd = l_new(struct mesh_friend, 1); + l_queue_push_head(net->friends, frnd); + } + + /* add _k2 */ + frnd->net = net; + frnd->dst = dst; + frnd->frd = frd; + frnd->frw = frw; + frnd->fn_cnt = fn_cnt; + frnd->lp_cnt = lp_cnt; + frnd->poll_timeout = fpt; + frnd->ele_cnt = ele_cnt; + frnd->pkt_cache = l_queue_new(); + frnd->new_key_set.nid = NET_NID_INVALID; + + l_put_be16(dst, p + 1); + l_put_be16(net->src_addr, p + 3); + l_put_be16(lp_cnt, p + 5); + l_put_be16(fn_cnt, p + 7); + + subnet = get_primary_subnet(net); + /* TODO: the primary key must be present, do we need to add check?. */ + + mesh_crypto_k2(subnet->current.key, p, sizeof(p), + &frnd->key_set.nid, + frnd->key_set.enc_key, + frnd->key_set.privacy_key); + + frnd->key_set.frnd = true; + mesh_net_add_keyset(net, &frnd->key_set); + + if (subnet->updated.key_set.nid == NET_NID_INVALID) + return frnd; + + mesh_crypto_k2(subnet->updated.key, p, sizeof(p), + &frnd->new_key_set.nid, + frnd->new_key_set.enc_key, + frnd->new_key_set.privacy_key); + frnd->new_key_set.frnd = true; + mesh_net_add_keyset(net, &frnd->new_key_set); + + return frnd; +} + +void mesh_friend_free(void *data) +{ + struct mesh_friend *frnd = data; + + free_friend_internals(frnd); + l_timeout_remove(frnd->timeout); + l_free(frnd); +} + +bool mesh_friend_clear(struct mesh_net *net, struct mesh_friend *frnd) +{ + bool removed = l_queue_remove(net->friends, frnd); + + free_friend_internals(frnd); + + return removed; +} + +bool mesh_net_add_keyset(struct mesh_net *net, struct mesh_key_set *key_set) +{ + if (!net) + return false; + + l_info("Add KEY_SET %2.2x (%d) %p", + key_set->nid, key_set->frnd, key_set); + return l_queue_push_tail(net->key_sets, key_set); +} + +bool mesh_net_remove_keyset(struct mesh_net *net, struct mesh_key_set *key_set) +{ + if (!net || !net->key_sets) + return false; + + l_info("DEL KEY_SET %2.2x (%d) %p", + key_set->nid, key_set->frnd, key_set); + return l_queue_remove(net->key_sets, key_set); +} + +void mesh_friend_sub_add(struct mesh_net *net, uint16_t lpn, uint8_t ele_cnt, + uint8_t grp_cnt, + const uint8_t *list) +{ + uint16_t *new_list; + uint16_t *grp_list; + struct mesh_friend *frnd = l_queue_find(net->friends, + match_by_friend, + L_UINT_TO_PTR(lpn)); + if (!frnd) + return; + + new_list = l_malloc((grp_cnt + frnd->grp_cnt) * sizeof(uint16_t)); + grp_list = frnd->grp_list; + + if (grp_list && frnd->grp_cnt) + memcpy(new_list, grp_list, frnd->grp_cnt * sizeof(uint16_t)); + + memcpy(&new_list[frnd->grp_cnt], list, grp_cnt * sizeof(uint16_t)); + l_free(grp_list); + frnd->ele_cnt = ele_cnt; + frnd->grp_list = new_list; + frnd->grp_cnt += grp_cnt; +} + +void mesh_friend_sub_del(struct mesh_net *net, uint16_t lpn, + uint8_t cnt, + const uint8_t *del_list) +{ + uint16_t *grp_list; + int16_t i, grp_cnt; + size_t cnt16 = cnt * sizeof(uint16_t); + struct mesh_friend *frnd = l_queue_find(net->friends, + match_by_friend, + L_UINT_TO_PTR(lpn)); + if (!frnd) + return; + + grp_cnt = frnd->grp_cnt; + grp_list = frnd->grp_list; + + while (cnt-- && grp_cnt) { + cnt16 -= sizeof(uint16_t); + for (i = grp_cnt - 1; i >= 0; i--) { + if (l_get_le16(del_list + cnt16) == grp_list[i]) { + grp_cnt--; + memcpy(&grp_list[i], &grp_list[i + 1], + (grp_cnt - i) * sizeof(uint16_t)); + break; + } + } + } + + frnd->grp_cnt = grp_cnt; + + if (!grp_cnt) { + l_free(frnd->grp_list); + frnd->grp_list = NULL; + } +} + +uint32_t mesh_net_next_seq_num(struct mesh_net *net) +{ + uint32_t seq = net->seq_num; + + net->seq_num++; + + /* Periodically store advanced sequence number */ + if (net->seq_num + MIN_SEQ_TRIGGER >= net->cached_seq_num) { + net->cached_seq_num = net->seq_num + + node_seq_cache(net->local_node); + node_set_sequence_number(net->local_node, net->cached_seq_num); + } + + return seq; +} + +static struct mesh_sar *mesh_sar_new(size_t len) +{ + size_t size = sizeof(struct mesh_sar) + len; + struct mesh_sar *sar; + + sar = l_malloc(size); + + memset(sar, 0, size); + + return sar; +} + +static void mesh_sar_free(void *data) +{ + struct mesh_sar *sar = data; + + if (!sar) + return; + + l_timeout_remove(sar->seg_timeout); + l_timeout_remove(sar->msg_timeout); + l_free(sar); +} + +static void mesh_msg_free(void *data) +{ + struct mesh_msg *msg = data; + + l_free(msg); +} + +static void lpn_process_beacon(void *user_data, const void *data, uint8_t size, + int8_t rssi); + +static struct mesh_subnet *subnet_new(struct mesh_net *net, uint16_t idx) +{ + struct mesh_subnet *subnet; + + subnet = l_new(struct mesh_subnet, 1); + if (!subnet) + return NULL; + + subnet->net = net; + subnet->idx = idx; + subnet->tx = &subnet->current; + subnet->updated.key_set.nid = NET_NID_INVALID; + subnet->snb.beacon[0] = MESH_AD_TYPE_BEACON; + return subnet; +} + +static bool create_keys(struct mesh_net *net, struct net_key *keys, + const uint8_t *net_key) +{ + uint8_t nid[1]; + uint8_t enc_key[16]; + uint8_t privacy_key[16]; + uint8_t network_id[8]; + uint8_t p[] = {0}; + + if (!mesh_crypto_k2(net_key, p, sizeof(p), + nid, enc_key, privacy_key)) + return false; + + if (!mesh_crypto_k3(net_key, network_id)) + return false; + + if (!mesh_crypto_nkbk(net_key, keys->beacon_key)) + return false; + + keys->key_set.frnd = false; + keys->key_set.nid = nid[0]; + memcpy(keys->key_set.enc_key, enc_key, 16); + memcpy(keys->key_set.privacy_key, privacy_key, 16); + memcpy(keys->network_id, network_id, 8); + memcpy(keys->key, net_key, 16); + return true; +} + +static bool create_secure_beacon(struct mesh_net *net, + struct mesh_subnet *subnet, + uint8_t *beacon_data, uint8_t size) +{ + uint64_t cmac; + + if (size < 22) + return false; + + beacon_data[0] = BEACON_TYPE_SNB; + beacon_data[1] = 0; + + if (subnet->key_refresh) + beacon_data[1] |= 0x01; + + if (iv_is_updating(net)) + beacon_data[1] |= 0x02; + + memcpy(beacon_data + 2, subnet->tx->network_id, 8); + l_put_be32(net->iv_index, beacon_data + 10); + + if (!mesh_crypto_beacon_cmac(subnet->tx->beacon_key, + subnet->tx->network_id, + net->iv_index, subnet->key_refresh, + iv_is_updating(net), &cmac)) + return false; + + l_put_be64(cmac, beacon_data + 14); + + return true; +} + +static void send_network_beacon(struct mesh_subnet *subnet, + struct mesh_net *net) +{ + struct mesh_io_send_info info = { + .type = MESH_IO_TIMING_TYPE_GENERAL, + .u.gen.interval = net->tx_interval, + .u.gen.cnt = 1, + .u.gen.min_delay = DEFAULT_MIN_DELAY, + .u.gen.max_delay = DEFAULT_MAX_DELAY + }; + + l_info("Send SNB on network %3.3x", subnet->idx); + mesh_io_send(net->io, &info, subnet->snb.beacon, + sizeof(subnet->snb.beacon)); +} + +static void network_beacon_timeout(struct l_timeout *timeout, void *user_data) +{ + struct mesh_subnet *subnet = user_data; + uint32_t interval; + + send_network_beacon(subnet, subnet->net); + + if (!subnet->snb.half_period) { + l_debug("beacon TO period %d, observed %d, expected %d", + subnet->snb.observe_period, + subnet->snb.observed, + subnet->snb.expected); + interval = subnet->snb.observe_period * + (subnet->snb.observed + 1) / subnet->snb.expected; + subnet->snb.observe_period = interval * 2; + subnet->snb.expected = subnet->snb.observe_period / 10; + subnet->snb.observed = 0; + } else + interval = subnet->snb.observe_period / 2; + + if (interval < BEACON_INTERVAL_MIN) + interval = BEACON_INTERVAL_MIN; + + if (interval > BEACON_INTERVAL_MAX) + interval = BEACON_INTERVAL_MAX; + + subnet->snb.ts = get_timestamp_secs(); + subnet->snb.half_period ^= 1; + l_timeout_modify(timeout, interval); +} + +static void start_network_beacon(void *a, void *b) +{ + struct mesh_subnet *subnet = a; + struct mesh_net *net = b; + + if (!net->beacon_enable) { + if (subnet->snb.timeout) + l_timeout_remove(subnet->snb.timeout); + subnet->snb.timeout = NULL; + return; + } + + /* If timeout is active, let it run it's course */ + if (subnet->snb.timeout) + return; + + send_network_beacon(subnet, subnet->net); + + subnet->snb.ts = get_timestamp_secs(); + subnet->snb.expected = 2; + subnet->snb.observed = 0; + subnet->snb.half_period = 1; + subnet->snb.observe_period = BEACON_INTERVAL_MIN * 2; + + subnet->snb.timeout = l_timeout_create(BEACON_INTERVAL_MIN, + network_beacon_timeout, subnet, NULL); +} + +struct mesh_net *mesh_net_new(uint16_t index) +{ + struct mesh_net *net; + + net = l_new(struct mesh_net, 1); + + if (!net) + return NULL; + + net->pkt_id = 0; + net->bea_id = 0; + net->key_id_next = 0; + + net->beacon_enable = true; + net->proxy_enable = false; + net->relay.enable = false; + + net->seq_num = 0x000000; + net->src_addr = 0x0000; + net->default_ttl = 0x00; + + net->provisioner = false; + + net->test_mode = false; + memset(&net->prov_caps, 0, sizeof(net->prov_caps)); + net->prov_caps.algorithms = 1; + + net->tx_cnt = DEFAULT_TRANSMIT_COUNT; + net->tx_interval = DEFAULT_TRANSMIT_INTERVAL; + + net->subnets = l_queue_new(); + net->key_sets = l_queue_new(); + net->fast_cache = l_queue_new(); + net->msg_cache = l_queue_new(); + net->sar_in = l_queue_new(); + net->sar_out = l_queue_new(); + net->frnd_msgs = l_queue_new(); + net->friends = l_queue_new(); + net->destinations = l_queue_new(); + net->app_keys = l_queue_new(); + + memset(&net->heartbeat, 0, sizeof(net->heartbeat)); + + return mesh_net_ref(net); +} + +struct mesh_net *mesh_net_ref(struct mesh_net *net) +{ + if (!net) + return NULL; + + __sync_fetch_and_add(&net->ref_count, 1); + + return net; +} + +void mesh_net_unref(struct mesh_net *net) +{ + if (!net) + return; + + if (__sync_sub_and_fetch(&net->ref_count, 1)) + return; + + /* key_sets are not allocated to this queue. Only Borrowed */ + l_queue_destroy(net->key_sets, NULL); + net->key_sets = NULL; + + l_queue_destroy(net->subnets, l_free); + l_queue_destroy(net->fast_cache, mesh_msg_free); + l_queue_destroy(net->msg_cache, mesh_msg_free); + l_queue_destroy(net->sar_in, mesh_sar_free); + l_queue_destroy(net->sar_out, mesh_sar_free); + l_queue_destroy(net->frnd_msgs, l_free); + l_queue_destroy(net->friends, mesh_friend_free); + l_queue_destroy(net->destinations, l_free); + l_queue_destroy(net->app_keys, appkey_key_free); + + l_free(net); +} + +int mesh_net_del_key(struct mesh_net *net, uint16_t idx) +{ + struct mesh_subnet *subnet; + + if (!net) + return MESH_STATUS_UNSPECIFIED_ERROR; + + /* Cannot remove primary key */ + if (l_queue_length(net->subnets) <= 1) + return MESH_STATUS_CANNOT_REMOVE; + + subnet = l_queue_find(net->subnets, match_key_index, + L_UINT_TO_PTR(idx)); + if (!subnet) + return MESH_STATUS_CANNOT_REMOVE; + + /* Delete associated app keys */ + appkey_delete_bound_keys(net, idx); + + /* Disable hearbeat publication on this subnet */ + if (idx == net->heartbeat.pub_net_idx) + net->heartbeat.pub_dst = UNASSIGNED_ADDRESS; + + mesh_net_remove_keyset(net, &subnet->current.key_set); + mesh_net_remove_keyset(net, &subnet->updated.key_set); + + /* TODO: cancel beacon_enable on this subnet */ + + l_queue_remove(net->subnets, subnet); + if (!storage_local_net_key_del(net, idx)) + return MESH_STATUS_STORAGE_FAIL; + + return MESH_STATUS_SUCCESS; +} + +int mesh_net_add_key(struct mesh_net *net, bool update, uint16_t idx, + const void *value) +{ + int status; + struct mesh_subnet *subnet; + + subnet = l_queue_find(net->subnets, match_key_index, + L_UINT_TO_PTR(idx)); + + if (update) { + if (subnet && subnet->kr_phase == KEY_REFRESH_PHASE_NONE) { + l_info("Start key refresh"); + status = mesh_net_kr_phase_one(net, idx, value); + if (status == MESH_STATUS_SUCCESS && + !storage_local_net_key_add(net, idx, + value, KEY_REFRESH_PHASE_ONE)) + return MESH_STATUS_STORAGE_FAIL; + } else + return MESH_STATUS_CANNOT_UPDATE; + } + + if (subnet) + return memcmp(subnet->current.key, value, 16) ? + MESH_STATUS_IDX_ALREADY_STORED : MESH_STATUS_SUCCESS; + + subnet = subnet_new(net, idx); + if (!subnet) + return MESH_STATUS_INSUFF_RESOURCES; + + if (!create_keys(net, &subnet->current, value) || + !mesh_net_add_keyset(net, &subnet->current.key_set)) { + l_free(subnet); + return MESH_STATUS_INSUFF_RESOURCES; + } + + if (!create_secure_beacon(net, subnet, &subnet->snb.beacon[1], 22) || + !l_queue_push_tail(net->subnets, subnet)) { + mesh_net_remove_keyset(net, &subnet->current.key_set); + l_free(subnet); + return MESH_STATUS_INSUFF_RESOURCES; + } + + if (!storage_local_net_key_add(net, idx, value, + KEY_REFRESH_PHASE_NONE)) { + l_queue_remove(net->subnets, subnet); + mesh_net_remove_keyset(net, &subnet->current.key_set); + l_free(subnet); + return MESH_STATUS_STORAGE_FAIL; + } + + start_network_beacon(subnet, net); + + return MESH_STATUS_SUCCESS; +} + +void mesh_net_flush_msg_queues(struct mesh_net *net) +{ + l_queue_clear(net->msg_cache, mesh_msg_free); + l_queue_clear(net->fast_cache, mesh_msg_free); +} + +static bool match_cache(const void *a, const void *b) +{ + const struct mesh_msg *msg = a; + const struct mesh_msg *tst = b; + + if (msg->seq != tst->seq || msg->mic != tst->mic || + msg->src != tst->src) + return false; + + return true; +} + +static bool msg_in_cache(struct mesh_net *net, uint16_t src, uint32_t seq, + uint32_t mic) +{ + struct mesh_msg *msg; + struct mesh_msg tst = { + .src = src, + .seq = seq, + .mic = mic, + }; + + msg = l_queue_remove_if(net->msg_cache, match_cache, &tst); + + if (msg) { + l_debug("Supressing duplicate %4.4x + %6.6x + %8.8x", + src, seq, mic); + l_queue_push_head(net->msg_cache, msg); + return true; + } + + msg = l_new(struct mesh_msg, 1); + *msg = tst; + l_queue_push_head(net->msg_cache, msg); + l_debug("Add %4.4x + %6.6x + %8.8x", src, seq, mic); + + if (l_queue_length(net->msg_cache) > MSG_CACHE_SIZE) { + msg = l_queue_peek_tail(net->msg_cache); + /* Remove Tail (oldest msg in cache) */ + l_debug("Remove %4.4x + %6.6x + %8.8x", + msg->src, msg->seq, msg->mic); + if (l_queue_remove(net->msg_cache, msg)) + l_free(msg); + } + + return false; +} + +static bool match_sar_seq0(const void *a, const void *b) +{ + const struct mesh_sar *sar = a; + uint16_t seqZero = L_PTR_TO_UINT(b); + + return sar->seqZero == seqZero; +} + +static bool match_sar_remote(const void *a, const void *b) +{ + const struct mesh_sar *sar = a; + uint16_t remote = L_PTR_TO_UINT(b); + + return sar->remote == remote; +} + +static bool match_msg_timeout(const void *a, const void *b) +{ + const struct mesh_sar *sar = a; + const struct l_timeout *msg_timeout = b; + + return sar->msg_timeout == msg_timeout; +} + +static bool match_sar_id(const void *a, const void *b) +{ + const struct mesh_sar *sar = a; + unsigned int id = L_PTR_TO_UINT(b); + + return sar->id == id; +} + +static bool match_seg_timeout(const void *a, const void *b) +{ + const struct mesh_sar *sar = a; + const struct l_timeout *seg_timeout = b; + + return sar->seg_timeout == seg_timeout; +} + +static bool match_dest_dst(const void *a, const void *b) +{ + const struct mesh_destination *dest = a; + uint16_t dst = L_PTR_TO_UINT(b); + + return dst == dest->dst; +} + +static bool match_frnd_dst(const void *a, const void *b) +{ + const struct mesh_friend *frnd = a; + uint16_t dst = L_PTR_TO_UINT(b); + int16_t i, grp_cnt = frnd->grp_cnt; + uint16_t *grp_list = frnd->grp_list; + + /* + * Determine if this message is for this friends unicast + * address, and/or one of it's group/virtual addresses + */ + if (dst >= frnd->dst && dst < (frnd->dst + frnd->ele_cnt)) + return true; + + if (!(dst & 0x8000)) + return false; + + for (i = 0; i < grp_cnt; i++) { + if (dst == grp_list[i]) + return true; + } + + return false; +} + +static bool is_lpn_friend(struct mesh_net *net, uint16_t addr, bool frnd) +{ + void *tst; + + if (!frnd) + return false; + + tst = l_queue_find(net->friends, match_frnd_dst, L_UINT_TO_PTR(addr)); + + return tst != NULL; +} + +static bool is_us(struct mesh_net *net, uint16_t addr, bool src) +{ + void *tst; + + if (IS_ALL_NODES(addr)) + return true; + + if (addr == FRIENDS_ADDRESS) + return net->friend_enable; + + if (addr == RELAYS_ADDRESS) + return net->relay.enable; + + if (addr == PROXIES_ADDRESS) + return net->proxy_enable; + + if (addr >= net->src_addr && addr <= net->last_addr) + return true; + + tst = l_queue_find(net->destinations, match_dest_dst, + L_UINT_TO_PTR(addr)); + + if (tst == NULL && !src) + tst = l_queue_find(net->friends, match_frnd_dst, + L_UINT_TO_PTR(addr)); + + return tst != NULL; +} + +static struct mesh_friend_msg *mesh_friend_msg_new(uint8_t seg_max) +{ + struct mesh_friend_msg *frnd_msg; + + if (seg_max) { + size_t size = sizeof(struct mesh_friend_msg) - + sizeof(struct mesh_friend_seg_one); + + size += (seg_max + 1) * sizeof(struct mesh_friend_seg_12); + frnd_msg = (struct mesh_friend_msg *) l_new(uint8_t, size); + } else + frnd_msg = l_new(struct mesh_friend_msg, 1); + + + return frnd_msg; +} + + +static bool match_ack(const void *a, const void *b) +{ + const struct mesh_friend_msg *old = a; + const struct mesh_friend_msg *rx = b; + uint32_t old_hdr; + uint32_t new_hdr; + + /* Determine if old pkt is ACK to same SAR message that new ACK is */ + if (!old->ctl || old->src != rx->src) + return false; + + /* Check the quickest items first before digging deeper */ + old_hdr = old->u.one[0].hdr & HDR_ACK_MASK; + new_hdr = rx->u.one[0].hdr & HDR_ACK_MASK; + + return old_hdr == new_hdr; +} + +static void enqueue_friend_pkt(void *a, void *b) +{ + struct mesh_friend *frnd = a; + struct mesh_friend_msg *pkt, *rx = b; + size_t size; + int16_t i; + + if (rx->done) + return; + + /* + * Determine if this message is for this friends unicast + * address, and/or one of it's group/virtual addresses + */ + if (rx->dst >= frnd->dst && (rx->dst - frnd->dst) < frnd->ele_cnt) { + rx->done = true; + goto enqueue; + } + + if (!(rx->dst & 0x8000)) + return; + + if (!IS_ALL_NODES(rx->dst)) { + for (i = 0; i < frnd->grp_cnt; i++) { + if (rx->dst == frnd->grp_list[i]) + goto enqueue; + } + return; + } + +enqueue: + /* Special handling for Seg Ack -- Only one per message queue */ + if (((rx->u.one[0].hdr >> OPCODE_HDR_SHIFT) & OPCODE_MASK) == + NET_OP_SEG_ACKNOWLEDGE) { + void *old_head = l_queue_peek_head(frnd->pkt_cache); + /* Suppress duplicate ACKs */ + do { + void *old = l_queue_remove_if(frnd->pkt_cache, + match_ack, rx); + + if (old) { + if (old_head == old) { + /* + * If we are discarding head for any + * reason, reset FRND SEQ + */ + frnd->last = frnd->seq; + } + + l_free(old); + } else + break; + + } while (true); + } + + l_debug("%s for %4.4x from %4.4x ttl: %2.2x (seq: %6.6x) (ctl: %d)", + __func__, frnd->dst, rx->src, rx->ttl, + rx->u.one[0].seq, rx->ctl); + + if (rx->cnt_in) { + size = sizeof(struct mesh_friend_msg) - + sizeof(struct mesh_friend_seg_one); + size += (rx->cnt_in + 1) * sizeof(struct mesh_friend_seg_12); + } else + size = sizeof(struct mesh_friend_msg); + + pkt = l_malloc(size); + memcpy(pkt, rx, size); + + l_queue_push_tail(frnd->pkt_cache, pkt); + + if (l_queue_length(frnd->pkt_cache) > FRND_CACHE_MAX) { + /* + * TODO: Guard against popping UPDATE packets + * (disallowed per spec) + */ + pkt = l_queue_pop_head(frnd->pkt_cache); + l_free(pkt); + frnd->last = frnd->seq; + } +} + +static void enqueue_update(void *a, void *b) +{ + struct mesh_friend *frnd = a; + struct mesh_friend_msg *pkt = b; + + pkt->dst = frnd->dst; + pkt->done = false; + enqueue_friend_pkt(frnd, pkt); +} + +static uint32_t seq_auth(uint32_t seq, uint16_t seqZero) +{ + uint32_t seqAuth = seqZero & SEQ_ZERO_MASK; + + seqAuth |= seq & (~SEQ_ZERO_MASK); + if (seqAuth > seq) + seqAuth -= (SEQ_ZERO_MASK + 1); + + return seqAuth; +} + +static bool friend_packet_queue(struct mesh_net *net, + uint32_t iv_index, + bool ctl, uint8_t ttl, + uint32_t seq, + uint16_t src, uint16_t dst, + uint32_t hdr, + const uint8_t *data, uint16_t size) +{ + struct mesh_friend_msg *frnd_msg; + uint8_t seg_max = SEG_TOTAL(hdr); + bool ret; + + if (seg_max && !IS_SEGMENTED(hdr)) + return false; + + frnd_msg = mesh_friend_msg_new(seg_max); + + if (IS_SEGMENTED(hdr)) { + uint32_t seqAuth = seq_auth(seq, hdr >> SEQ_ZERO_HDR_SHIFT); + uint8_t i; + + for (i = 0; i <= seg_max; i++) { + memcpy(frnd_msg->u.s12[i].data, data, 12); + frnd_msg->u.s12[i].hdr = hdr; + frnd_msg->u.s12[i].seq = seqAuth + i; + data += 12; + hdr += (1 << SEGO_HDR_SHIFT); + } + frnd_msg->u.s12[seg_max].seq = seq; + frnd_msg->cnt_in = seg_max; + frnd_msg->last_len = size % 12; + if (!frnd_msg->last_len) + frnd_msg->last_len = 12; + } else { + uint8_t opcode = hdr >> OPCODE_HDR_SHIFT; + + if (ctl && opcode != NET_OP_SEG_ACKNOWLEDGE) { + + /* Don't cache Friend Ctl opcodes */ + if (FRND_OPCODE(opcode)) { + l_free(frnd_msg); + return false; + } + + memcpy(frnd_msg->u.one[0].data + 1, data, size); + frnd_msg->last_len = size + 1; + frnd_msg->u.one[0].data[0] = opcode; + } else { + memcpy(frnd_msg->u.one[0].data, data, size); + frnd_msg->last_len = size; + } + frnd_msg->u.one[0].hdr = hdr; + frnd_msg->u.one[0].seq = seq; + } + + frnd_msg->iv_index = iv_index; + frnd_msg->src = src; + frnd_msg->dst = dst; + frnd_msg->ctl = ctl; + frnd_msg->ttl = ttl; + + /* Re-Package into Friend Delivery payload */ + l_queue_foreach(net->friends, enqueue_friend_pkt, frnd_msg); + ret = frnd_msg->done; + + /* TODO Optimization(?): Unicast messages keep this buffer */ + l_free(frnd_msg); + + return ret; +} + +static void friend_ack_rxed(struct mesh_net *net, uint32_t iv_index, + uint32_t seq, + uint16_t src, uint16_t dst, + const uint8_t *pkt) +{ + uint32_t hdr = l_get_be32(pkt) & + ((SEQ_ZERO_MASK << SEQ_ZERO_HDR_SHIFT) | /* Preserve SeqZero */ + (true << RELAY_HDR_SHIFT)); /* Preserve Relay bit */ + uint32_t flags = l_get_be32(pkt + 3); + struct mesh_friend_msg frnd_ack = { + .ctl = true, + .iv_index = iv_index, + .src = src, + .dst = dst, + .last_len = sizeof(flags), + .u.one[0].seq = seq, + .done = false, + }; + + hdr |= NET_OP_SEG_ACKNOWLEDGE << OPCODE_HDR_SHIFT; + frnd_ack.u.one[0].hdr = hdr; + l_put_be32(flags, frnd_ack.u.one[0].data); + l_queue_foreach(net->friends, enqueue_friend_pkt, &frnd_ack); +} + +static bool send_seg(struct mesh_net *net, struct mesh_sar *msg, uint8_t seg); + +static void send_frnd_ack(struct mesh_net *net, uint16_t src, uint16_t dst, + uint32_t hdr, uint32_t flags) +{ + uint32_t expected; + uint8_t msg[7]; + + /* We don't ACK from multicast destinations */ + if (src & 0x8000) + return; + + /* Calculate the "Full ACK" mask */ + expected = 0xffffffff >> (31 - SEG_TOTAL(hdr)); + + /* Clear Hdr bits that don't apply to Seg ACK */ + hdr &= ~((true << SEG_HDR_SHIFT) | + (OPCODE_MASK << OPCODE_HDR_SHIFT) | + (true << SZMIC_HDR_SHIFT) | + (SEG_MASK << SEGO_HDR_SHIFT) | + (SEG_MASK << SEGN_HDR_SHIFT)); + + hdr |= NET_OP_SEG_ACKNOWLEDGE << OPCODE_HDR_SHIFT; + hdr |= true << RELAY_HDR_SHIFT; + + /* Clear all unexpected bits */ + flags &= expected; + + l_put_be32(hdr, msg); + l_put_be32(flags, msg + 3); + + l_info("Send Friend ACK to Segs: %8.8x", flags); + + if (is_lpn_friend(net, dst, true)) { + /* If we are acking our LPN Friend, queue, don't send */ + friend_ack_rxed(net, mesh_net_get_iv_index(net), + mesh_net_next_seq_num(net), 0, dst, msg); + } else { + mesh_net_transport_send(net, NULL, false, + mesh_net_get_iv_index(net), DEFAULT_TTL, + 0, 0, dst, msg, sizeof(msg)); + } +} + +static void send_net_ack(struct mesh_net *net, struct mesh_sar *sar, + uint32_t flags) +{ + uint8_t msg[7]; + uint32_t hdr; + uint16_t src = sar->src; + uint16_t dst = sar->remote; + + /* We don't ACK from multicast destinations */ + if (src & 0x8000) + return; + + /* We don't ACK segments as a Low Power Node */ + if (net->friend_addr) + return; + + hdr = NET_OP_SEG_ACKNOWLEDGE << OPCODE_HDR_SHIFT; + hdr |= sar->seqZero << SEQ_ZERO_HDR_SHIFT; + + if (is_lpn_friend(net, src, true)) + hdr |= true << RELAY_HDR_SHIFT; + + l_put_be32(hdr, msg); + l_put_be32(flags, msg + 3); + l_info("Send%s ACK to Segs: %8.8x", sar->frnd ? " Friend" : "", flags); + + if (is_lpn_friend(net, dst, true)) { + /* If we are acking our LPN Friend, queue, don't send */ + friend_ack_rxed(net, mesh_net_get_iv_index(net), + mesh_net_next_seq_num(net), src, dst, msg); + return; + } + + mesh_net_transport_send(net, NULL, false, + mesh_net_get_iv_index(net), DEFAULT_TTL, + 0, src, dst, msg, sizeof(msg)); +} + +static void inseg_to(struct l_timeout *seg_timeout, void *user_data) +{ + struct mesh_net *net = user_data; + struct mesh_sar *sar = l_queue_find(net->sar_in, + match_seg_timeout, seg_timeout); + + l_timeout_remove(seg_timeout); + if (!sar) + return; + + /* Send NAK */ + l_info("Timeout %p %3.3x", sar, sar->app_idx); + send_net_ack(net, sar, sar->flags); + + sar->seg_timeout = l_timeout_create(SEG_TO, inseg_to, net, NULL); +} + +static void inmsg_to(struct l_timeout *msg_timeout, void *user_data) +{ + struct mesh_net *net = user_data; + struct mesh_sar *sar = l_queue_remove_if(net->sar_in, + match_msg_timeout, msg_timeout); + + l_timeout_remove(msg_timeout); + if (!sar) + return; + + sar->msg_timeout = NULL; + + /* print_packet("Incoming SAR Timeout", sar->buf, sar->len); */ + mesh_sar_free(sar); +} + +static void outmsg_to(struct l_timeout *msg_timeout, void *user_data) +{ + struct mesh_net *net = user_data; + struct mesh_sar *sar = l_queue_remove_if(net->sar_out, + match_msg_timeout, msg_timeout); + + l_timeout_remove(msg_timeout); + if (!sar) + return; + + sar->msg_timeout = NULL; + + if (sar->status_func) + sar->status_func(sar->remote, 1, + sar->buf, sar->len - 4, + sar->user_data); + + /* print_packet("Outgoing SAR Timeout", sar->buf, sar->len); */ + mesh_sar_free(sar); +} + +static void outseg_to(struct l_timeout *seg_timeout, void *user_data); +static void ack_received(struct mesh_net *net, bool timeout, + uint16_t src, uint16_t dst, + uint16_t seq0, uint32_t ack_flag) +{ + struct mesh_sar *outgoing; + uint32_t seg_flag = 0x00000001; + uint32_t ack_copy = ack_flag; + uint16_t i; + + l_info("ACK Rxed (%x) (to:%d): %8.8x", seq0, timeout, ack_flag); + + outgoing = l_queue_find(net->sar_out, match_sar_seq0, + L_UINT_TO_PTR(seq0)); + + if (!outgoing) { + l_info("Not Found: %4.4x", seq0); + return; + } + + /* + * TODO -- If we receive from different + * SRC than we are sending to, make sure the OBO flag is set + */ + + if ((!timeout && !ack_flag) || + (outgoing->flags & ack_flag) == outgoing->flags) { + l_debug("ob_sar_removal (%x)", outgoing->flags); + + /* Note: ack_flags == 0x00000000 is a remote Cancel request */ + if (outgoing->status_func) + outgoing->status_func(src, ack_flag ? 0 : 1, + outgoing->buf, + outgoing->len - 4, outgoing->user_data); + + l_queue_remove(net->sar_out, outgoing); + mesh_sar_free(outgoing); + + return; + } + + outgoing->last_nak |= ack_flag; + + ack_copy &= outgoing->flags; + + for (i = 0; i <= SEG_MAX(outgoing->len); i++, seg_flag <<= 1) { + if (seg_flag & ack_flag) { + l_debug("Skipping Seg %d of %d", + i, SEG_MAX(outgoing->len)); + continue; + } + + ack_copy |= seg_flag; + + l_info("Resend Seg %d net:%p dst:%x app_idx:%3.3x", + i, net, outgoing->remote, outgoing->app_idx); + + send_seg(net, outgoing, i); + } + + l_timeout_remove(outgoing->seg_timeout); + outgoing->seg_timeout = l_timeout_create(SEG_TO, outseg_to, net, NULL); +} + +static void outack_to(struct l_timeout *seg_timeout, void *user_data) +{ + struct mesh_net *net = user_data; + struct mesh_sar *sar = l_queue_find(net->sar_out, + match_seg_timeout, seg_timeout); + + l_timeout_remove(seg_timeout); + if (!sar) + return; + + sar->seg_timeout = NULL; + + /* Re-Send missing segments by faking NAK */ + ack_received(net, true, sar->remote, sar->src, + sar->seqZero, sar->last_nak); +} + +static void outseg_to(struct l_timeout *seg_timeout, void *user_data) +{ + struct mesh_net *net = user_data; + struct mesh_sar *sar = l_queue_find(net->sar_out, + match_seg_timeout, seg_timeout); + + l_timeout_remove(seg_timeout); + if (!sar) + return; + + sar->seg_timeout = NULL; + + if (net->friend_addr) { + /* We are LPN -- Poll for ACK */ + frnd_ack_poll(net); + sar->seg_timeout = l_timeout_create(SEG_TO, + outack_to, net, NULL); + } else { + /* Re-Send missing segments by faking NACK */ + ack_received(net, true, sar->remote, sar->src, + sar->seqZero, sar->last_nak); + } +} + +static bool msg_rxed(struct mesh_net *net, bool frnd, + uint32_t iv_index, + uint8_t ttl, + uint32_t seq, + uint16_t src, uint16_t dst, + uint8_t key_id, + bool szmic, uint16_t seqZero, + const uint8_t *data, uint16_t size) +{ + uint32_t seqAuth = seq_auth(seq, seqZero); + + /* Sanity check seqAuth */ + if (seqAuth > seq) + return false; + + /* Save un-decrypted messages for our friends */ + if (!frnd && l_queue_length(net->friends)) { + uint32_t hdr = key_id << KEY_HDR_SHIFT; + uint8_t frnd_ttl = ttl; + + /* If not from us, decrement for our hop */ + if (src < net->src_addr || src > net->last_addr) { + if (frnd_ttl > 1) + frnd_ttl--; + else + goto not_for_friend; + } + + if (szmic || size > 15) { + hdr |= true << SEG_HDR_SHIFT; + hdr |= szmic << SZMIC_HDR_SHIFT; + hdr |= (seqZero & SEQ_ZERO_MASK) << SEQ_ZERO_HDR_SHIFT; + hdr |= SEG_MAX(size) << SEGN_HDR_SHIFT; + } + + if (friend_packet_queue(net, iv_index, false, frnd_ttl, + seq, src, dst, + hdr, data, size)) + return true; + } + +not_for_friend: + return mesh_model_rx(net, szmic, seqAuth, seq, iv_index, + ttl, src, dst, key_id, data, size); +} + +static bool match_frnd_sar_dst(const void *a, const void *b) +{ + const struct mesh_friend_msg *frnd_msg = a; + uint16_t dst = L_PTR_TO_UINT(b); + + return frnd_msg->dst == dst; +} + +static void friend_seg_rxed(struct mesh_net *net, + uint32_t iv_index, + uint8_t ttl, uint32_t seq, + uint16_t src, uint16_t dst, uint32_t hdr, + const uint8_t *data, uint8_t size) +{ + struct mesh_friend *frnd = NULL; + struct mesh_friend_msg *frnd_msg = NULL; + uint8_t cnt; + uint8_t segN = hdr & 0x1f; + uint8_t segO = ((hdr >> 5) & 0x1f); + uint32_t expected = 0xffffffff >> (31 - segN); + uint32_t this_seg_flag = 0x00000001 << segO; + uint32_t largest = (0xffffffff << segO) & expected; + uint32_t hdr_key = hdr & HDR_KEY_MASK; + + frnd = l_queue_find(net->friends, match_frnd_dst, + L_UINT_TO_PTR(dst)); + if (!frnd) + return; + + if (frnd->last_hdr == hdr_key) { + /* We are no longer receiving this msg. Resend final ACK */ + send_frnd_ack(net, dst, src, frnd->last_hdr, 0xffffffff); + return; + } + + /* Check if we have a SAR-in-progress that matches incoming segment */ + frnd_msg = l_queue_find(net->frnd_msgs, match_frnd_sar_dst, + L_UINT_TO_PTR(dst)); + + if (frnd_msg) { + /* Flush if SZMICN or IV Index has changed */ + if (frnd_msg->iv_index != iv_index) + frnd_msg->u.s12[0].hdr = 0; + + /* Flush incomplete old SAR message if it doesn't match */ + if ((frnd_msg->u.s12[0].hdr & HDR_KEY_MASK) != hdr_key) { + l_queue_remove(net->frnd_msgs, frnd_msg); + l_free(frnd_msg); + frnd_msg = NULL; + } + } + + if (!frnd_msg) { + frnd_msg = mesh_friend_msg_new(segN); + frnd_msg->iv_index = iv_index; + frnd_msg->src = src; + frnd_msg->dst = dst; + frnd_msg->ttl = ttl; + l_queue_push_tail(net->frnd_msgs, frnd_msg); + } else if (frnd_msg->flags & this_seg_flag) /* Ignore dup segs */ + return; + + cnt = frnd_msg->cnt_in; + frnd_msg->flags |= this_seg_flag; + + frnd_msg->u.s12[cnt].hdr = hdr; + frnd_msg->u.s12[cnt].seq = seq; + memcpy(frnd_msg->u.s12[cnt].data, data, size); + + /* Last segment could be short */ + if (segN == segO) + frnd_msg->last_len = size; + + l_info("RXed Seg %d, Flags %8.8x (cnt: %d)", + segO, frnd_msg->flags, cnt); + + /* In reality, if one of these is true, then *both* must be true */ + if ((cnt == segN) || (frnd_msg->flags == expected)) { + l_info("Full ACK"); + send_frnd_ack(net, dst, src, hdr, frnd_msg->flags); + + if (frnd_msg->ttl > 1) { + frnd_msg->ttl--; + /* Add to friends cache */ + l_queue_foreach(net->friends, + enqueue_friend_pkt, frnd_msg); + } + + /* Remove from "in progress" queue */ + l_queue_remove(net->frnd_msgs, frnd_msg); + + /* TODO Optimization(?): Unicast messages keep this buffer */ + l_free(frnd_msg); + return; + } + + /* Always ACK if this is the largest outstanding segment */ + if ((largest & frnd_msg->flags) == largest) { + l_info("Partial ACK"); + send_frnd_ack(net, dst, src, hdr, frnd_msg->flags); + } + + frnd_msg->cnt_in++; +} + +static bool seg_rxed(struct mesh_net *net, bool frnd, + uint32_t iv_index, + uint8_t ttl, + uint32_t seq, + uint16_t src, uint16_t dst, + uint8_t key_id, + bool szmic, uint16_t seqZero, + uint8_t segO, uint8_t segN, + const uint8_t *data, uint8_t size) +{ + struct mesh_sar *sar_in = NULL; + uint16_t seg_off = 0; + uint32_t expected, this_seg_flag, largest, seqAuth; + bool reset_seg_to = true; + + /* + * DST could receive additional Segments after + * completing due to a lost ACK, so re-ACK and discard + */ + sar_in = l_queue_find(net->sar_in, match_sar_remote, + L_UINT_TO_PTR(src)); + + /* Discard *old* incoming-SAR-in-progress if this segment newer */ + seqAuth = seq_auth(seq, seqZero); + if (sar_in && (sar_in->seqAuth != seqAuth || + sar_in->iv_index != iv_index)) { + bool newer; + + if (iv_index > sar_in->iv_index) + newer = true; + else if (iv_index == sar_in->iv_index) + newer = seqAuth > sar_in->seqAuth; + else + newer = false; + + if (newer) { + /* Cancel Old, start New */ + l_queue_remove(net->sar_in, sar_in); + mesh_sar_free(sar_in); + sar_in = NULL; + } else + /* Ignore Old */ + return false; + } + + expected = 0xffffffff >> (31 - segN); + + if (sar_in) { + l_info("RXed (old: %04x %06x size:%d) %d of %d", + seqZero, seq, size, segO, segN); + /* Sanity Check--> certain things must match */ + if (SEG_MAX(sar_in->len) != segN || + sar_in->key_id != key_id) + return false; + + if (sar_in->flags == expected) { + /* Re-Send ACK for full msg */ + if (!net->friend_addr) + send_net_ack(net, sar_in, expected); + return true; + } + } else { + uint16_t len = MAX_SEG_TO_LEN(segN); + + l_info("RXed (new: %04x %06x size: %d len: %d) %d of %d", + seqZero, seq, size, len, segO, segN); + l_debug("Queue Size: %d", l_queue_length(net->sar_in)); + sar_in = mesh_sar_new(len); + sar_in->seqAuth = seqAuth; + sar_in->iv_index = iv_index; + sar_in->src = dst; + sar_in->remote = src; + sar_in->seqZero = seqZero; + sar_in->key_id = key_id; + sar_in->len = len; + sar_in->last_seg = 0xff; + if (!net->friend_addr) + sar_in->msg_timeout = l_timeout_create(MSG_TO, + inmsg_to, net, NULL); + + l_debug("First Seg %4.4x", sar_in->flags); + l_queue_push_head(net->sar_in, sar_in); + } + /* print_packet("Seg", data, size); */ + + seg_off = segO * MAX_SEG_LEN; + memcpy(sar_in->buf + seg_off, data, size); + this_seg_flag = 0x00000001 << segO; + + /* Don't reset Seg TO or NAK if we already have this seg */ + if (this_seg_flag & sar_in->flags) + reset_seg_to = false; + + sar_in->flags |= this_seg_flag; + sar_in->ttl = ttl; + + l_debug("Have Frags %4.4x", sar_in->flags); + + /* Msg length only definitive on last segment */ + if (segO == segN) + sar_in->len = segN * MAX_SEG_LEN + size; + + if (sar_in->flags == expected) { + /* Got it all */ + if (!net->friend_addr) + send_net_ack(net, sar_in, expected); + + msg_rxed(net, frnd, + iv_index, + ttl, + seq, + sar_in->remote, dst, + key_id, + szmic, sar_in->seqZero, + sar_in->buf, sar_in->len); + + /* Kill Inter-Seg timeout */ + l_timeout_remove(sar_in->seg_timeout); + sar_in->seg_timeout = NULL; + return true; + + } else if (reset_seg_to) { + /* Restart Inter-Seg Timeout */ + l_timeout_remove(sar_in->seg_timeout); + + /* if this is the largest outstanding segment, send NAK now */ + if (!net->friend_addr) { + largest = (0xffffffff << segO) & expected; + if ((largest & sar_in->flags) == largest) + send_net_ack(net, sar_in, sar_in->flags); + + sar_in->seg_timeout = l_timeout_create(SEG_TO, + inseg_to, net, NULL); + } + } + + l_debug("NAK: %d expected:%08x largest:%08x flags:%08x", + reset_seg_to, expected, largest, sar_in->flags); + return false; +} + +static bool ctl_received(struct mesh_net *net, bool frnd, uint32_t iv_index, + uint8_t ttl, + uint32_t seq, + uint16_t src, uint16_t dst, + uint8_t opcode, int8_t rssi, + const uint8_t *pkt, uint8_t len) +{ + uint8_t msg[12]; + uint8_t rsp_ttl = DEFAULT_TTL; + uint8_t n = 0; + + if (!frnd && ttl > 1) { + uint32_t hdr = opcode << OPCODE_HDR_SHIFT; + uint8_t frnd_ttl = ttl - 1; + + if (friend_packet_queue(net, iv_index, + true, frnd_ttl, + seq, + src, dst, + hdr, + pkt, len)) + return true; + } + + /* Don't process other peoples Unicast destinations */ + if (dst < 0x8000 && (dst < net->src_addr || dst > net->last_addr)) + return false; + + switch (opcode) { + default: + l_error("Unsupported Ctl Opcode: %2.2x", opcode); + break; + + case NET_OP_FRND_POLL: + if (len != 1 || ttl) + return false; + + print_packet("Rx-NET_OP_FRND_POLL", pkt, len); + friend_poll(net, src, !!(pkt[0]), + l_queue_find(net->friends, + match_by_friend, + L_UINT_TO_PTR(src))); + break; + + case NET_OP_FRND_UPDATE: + if (ttl) + return false; + + print_packet("Rx-NET_OP_FRND_UPDATE", pkt, len); + lpn_process_beacon(net, pkt, len, 0); + break; + + case NET_OP_FRND_REQUEST: + if (!net->friend_enable) + return false; + + if (!IS_ALL_NODES(dst) && dst != FRIENDS_ADDRESS) + return false; + + if (len != 10 || ttl) + return false; + + print_packet("Rx-NET_OP_FRND_REQUEST", pkt, len); + friend_request(net, src, pkt[0], pkt[1], + l_get_be32(pkt + 1) & 0xffffff, + l_get_be16(pkt + 5), pkt[7], + l_get_be16(pkt + 8), rssi); + break; + + case NET_OP_FRND_OFFER: + if (len != 6 || ttl) + return false; + + print_packet("Rx-NET_OP_FRND_OFFER", pkt, len); + frnd_offer(net, src, pkt[0], pkt[1], pkt[2], + (int8_t) pkt[3], rssi, l_get_be16(pkt + 4)); + break; + + case NET_OP_FRND_CLEAR_CONFIRM: + if (len != 4) + return false; + + print_packet("Rx-NET_OP_FRND_CLEAR_CONFIRM", pkt, len); + friend_clear_confirm(net, src, l_get_be16(pkt), + l_get_be16(pkt + 2)); + break; + + case NET_OP_FRND_CLEAR: + if (len != 4 || dst != net->src_addr) + return false; + + print_packet("Rx-NET_OP_FRND_CLEAR", pkt, len); + friend_clear(net, src, l_get_be16(pkt), l_get_be16(pkt + 2), + l_queue_find(net->friends, + match_by_friend, + L_UINT_TO_PTR(l_get_be16(pkt)))); + l_info("Remaining Friends: %d", l_queue_length(net->friends)); + break; + + case NET_OP_PROXY_SUB_ADD: + if (ttl) + return false; + + print_packet("Rx-NET_OP_PROXY_SUB_ADD", pkt, len); + friend_sub_add(net, l_queue_find(net->friends, + match_by_friend, L_UINT_TO_PTR(src)), + pkt, len); + break; + + case NET_OP_PROXY_SUB_REMOVE: + if (ttl) + return false; + + print_packet("Rx-NET_OP_PROXY_SUB_REMOVE", pkt, len); + friend_sub_del(net, l_queue_find(net->friends, + match_by_friend, L_UINT_TO_PTR(src)), + pkt, len); + break; + + case NET_OP_PROXY_SUB_CONFIRM: + if (ttl) + return false; + + print_packet("Rx-NET_OP_PROXY_SUB_CONFIRM", pkt, len); + break; + + case NET_OP_HEARTBEAT: + if (net->heartbeat.sub_enabled && + src == net->heartbeat.sub_src) { + uint8_t hops = pkt[0] - ttl + 1; + + print_packet("Rx-NET_OP_HEARTBEAT", pkt, len); + + if (net->heartbeat.sub_count != 0xffff) + net->heartbeat.sub_count++; + + if (net->heartbeat.sub_min_hops > hops) + net->heartbeat.sub_min_hops = hops; + + if (net->heartbeat.sub_max_hops < hops) + net->heartbeat.sub_max_hops = hops; + + l_info("HB: cnt:%4.4x min:%2.2x max:%2.2x", + net->heartbeat.sub_count, + net->heartbeat.sub_min_hops, + net->heartbeat.sub_max_hops); + } + break; + } + + if (n) { + mesh_net_transport_send(net, NULL, false, + mesh_net_get_iv_index(net), rsp_ttl, + 0, dst & 0x8000 ? 0 : dst, src, + msg, n); + } + + return true; +} + +static bool find_fast_hash(const void *a, const void *b) +{ + const uint64_t *entry = a; + const uint64_t *test = b; + + return *entry == *test; +} + +static void *check_fast_cache(struct mesh_net *net, uint64_t hash) +{ + void *found = l_queue_find(net->fast_cache, find_fast_hash, &hash); + uint64_t *new_hash; + + if (found) + return NULL; + + if (l_queue_length(net->fast_cache) >= 8) + new_hash = l_queue_pop_head(net->fast_cache); + else + new_hash = l_malloc(sizeof(hash)); + + *new_hash = hash; + l_queue_push_tail(net->fast_cache, new_hash); + + return new_hash; +} + +static bool match_keyset(const void *a, const void *b) +{ + const struct mesh_friend *frnd = a; + const struct mesh_key_set *key_set = b; + + return (key_set == &frnd->key_set) || (key_set == &frnd->new_key_set); +} + +static void try_decode(void *a, void *b) +{ + struct mesh_key_set *key_set = a; + struct net_decode *decode = b; + uint8_t tmp[29]; + bool status; + + if (decode->key_set || key_set->nid != decode->nid) + return; + + status = mesh_crypto_packet_decode(decode->packet, decode->size, + decode->proxy, tmp, decode->iv_index, + key_set->enc_key, key_set->privacy_key); + + if (!status) + return; + + memcpy(decode->packet, tmp, decode->size); + decode->key_set = key_set; + if (key_set->frnd) + decode->frnd = l_queue_find(decode->net->friends, + match_keyset, key_set); + else + decode->frnd = NULL; +} + +static struct mesh_key_set *net_packet_decode(struct mesh_net *net, + uint32_t iv_index, uint8_t nid, + struct mesh_friend **frnd, + bool proxy, + uint8_t *packet, uint8_t size) +{ + struct net_decode decode = { + .net = net, + .key_set = NULL, + .nid = nid, + .iv_index = iv_index, + .packet = packet, + .size = size, + .proxy = proxy, + }; + + l_queue_foreach(net->key_sets, try_decode, &decode); + + if (decode.key_set != NULL) { + *frnd = decode.frnd; + return decode.key_set; + } + return NULL; +} + +static bool match_key_nid(const void *a, const void *b) +{ + const struct mesh_key_set *key_set = a; + uint8_t nid = L_PTR_TO_UINT(b); + + return key_set->nid == nid; +} + +static bool match_by_dst(const void *a, const void *b) +{ + const struct mesh_destination *dest = a; + uint16_t dst = L_PTR_TO_UINT(b); + + return dest->dst == dst; +} + +static void send_relay_pkt(struct mesh_net *net, uint8_t *packet, uint8_t size) +{ + struct mesh_io *io = net->io; + struct mesh_io_send_info info = { + .type = MESH_IO_TIMING_TYPE_GENERAL, + .u.gen.interval = net->relay.interval, + .u.gen.cnt = net->relay.count, + .u.gen.min_delay = DEFAULT_MIN_DELAY, + .u.gen.max_delay = DEFAULT_MAX_DELAY + }; + + packet[0] = MESH_AD_TYPE_NETWORK; + + mesh_io_send(io, &info, packet, size); +} + +static void send_msg_pkt(struct mesh_net *net, uint8_t *packet, uint8_t size) +{ + struct mesh_io *io = net->io; + struct mesh_io_send_info info = { + .type = MESH_IO_TIMING_TYPE_GENERAL, + .u.gen.interval = net->tx_interval, + .u.gen.cnt = net->tx_cnt, + .u.gen.min_delay = DEFAULT_MIN_DELAY, + /* No extra randomization when sending regular mesh messages */ + .u.gen.max_delay = DEFAULT_MIN_DELAY + }; + + packet[0] = MESH_AD_TYPE_NETWORK; + + mesh_io_send(io, &info, packet, size); +} + +static void packet_received(void *user_data, const void *data, uint8_t size, + int8_t rssi) +{ + struct mesh_net *net = user_data; + uint32_t iv_index; + uint8_t iv_flag; + uint8_t nid; + const uint8_t *msg = data; + uint8_t app_msg_len; + uint8_t net_ttl, net_key_id, net_segO, net_segN, net_opcode; + uint32_t net_seq, cache_cookie; + uint16_t net_src, net_dst, net_seqZero; + uint8_t packet[31]; + bool net_ctl, net_segmented, net_szmic, net_relay; + struct mesh_friend *net_frnd; + bool drop = false; + uint64_t hash, *isNew = NULL; + struct mesh_key_set *keys; + + nid = msg[0] & 0x7f; + + /* Ignore unrecognized NIDs */ + if (!(l_queue_find(net->key_sets, match_key_nid, L_UINT_TO_PTR(nid)))) { + /* print_packet("Nope", data, size); */ + return; + } + + iv_flag = msg[0] >> 7; + iv_index = net->iv_index; + l_debug("%s iv_index %d NID: %2.2x", __func__, iv_index, nid); + + if (sizeof(uint16_t) <= sizeof(void *)) { + /* Add in additional cache to allow us to + * avoid decrypting duplicatesr + * Fast 64 bit Hash, Network MIC doesn't matter + * With 64 bit hash, false pos chance is 1 in 1.8 * 10^19 + */ + hash = l_get_le64(msg + 1) ^ l_get_le64(msg + 9); + isNew = check_fast_cache(net, hash); + if (!isNew) + return; + + l_debug("New"); + } + + memcpy(packet + 2, data, size); + + if (iv_index && (iv_index & 0x01) != iv_flag) + iv_index--; + + /* Tester--Drop 90% of packets */ + /* l_getrandom(&iv_flag, 1); */ + /* if (iv_flag%10<9) drop = true; */ + + if (!drop) + print_packet("RX: Network [enc] :", data, size); + + keys = net_packet_decode(net, iv_index, nid, &net_frnd, false, + packet + 2, size); + if (keys == NULL) { + l_debug("Failed to decode packet"); + /* Remove fast-cache-hash */ + l_queue_remove(net->fast_cache, isNew); + l_free(isNew); + return; + } + + if (!drop) + print_packet("RX: Network [clr] :", packet + 2, size); + + if (!mesh_crypto_packet_parse(packet + 2, size, + &net_ctl, &net_ttl, + &net_seq, + &net_src, &net_dst, + &cache_cookie, + &net_opcode, + &net_segmented, + &net_key_id, + &net_szmic, &net_relay, &net_seqZero, + &net_segO, &net_segN, + &msg, &app_msg_len)) { + l_error("Failed to parse packet content"); + return; + } + + /* Ignore incoming packets if we are LPN and frnd bit not set */ + if (net->friend_addr) { + struct mesh_subnet *subnet; + + subnet = l_queue_find(net->subnets, match_key_set, keys); + if (subnet) + return; + + /* If the queue is empty, stop polling */ + if (net_ctl && net_opcode == NET_OP_FRND_UPDATE && !msg[5]) + frnd_poll_cancel(net); + else + frnd_poll(net, false); + + } else if (net_dst == 0) { + l_error("illegal parms: DST: %4.4x Ctl: %d TTL: %2.2x", + net_dst, net_ctl, net_ttl); + return; + } + + /* Ignore if we originally sent this */ + if (is_us(net, net_src, true)) + return; + + if (drop) { + l_info("Dropping SEQ 0x%06x", net_seq); + return; + } + + l_debug("check %08x", cache_cookie); + + /* As a Relay, suppress repeats of last N packets that pass through */ + /* The "cache_cookie" should be unique part of App message */ + if (msg_in_cache(net, net_src, net_seq, cache_cookie)) + return; + + l_debug("RX: Network %04x -> %04x : TTL 0x%02x : IV : %8.8x SEQ 0x%06x", + net_src, net_dst, net_ttl, iv_index, net_seq); + + if (is_us(net, net_dst, false) || + is_lpn_friend(net, net_src, !!(net_frnd)) || + (net_ctl && net_opcode == NET_OP_HEARTBEAT)) { + + l_info("RX: App 0x%04x -> 0x%04x : TTL 0x%02x : SEQ 0x%06x", + net_src, net_dst, net_ttl, net_seq); + + l_debug("seq:%x seq0:%x", net_seq, net_seqZero); + if (net_ctl) { + l_debug("CTL - %4.4x RX", net_seqZero); + if (net_opcode == NET_OP_SEG_ACKNOWLEDGE) { + /* Illegal to send ACK to non-Unicast Addr */ + if (net_dst & 0x8000) + return; + + /* print_packet("Got ACK", msg, app_msg_len); */ + /* Pedantic check for correct size */ + if (app_msg_len != 7) + return; + + /* If this is an ACK to our friend queue-only */ + if (is_lpn_friend(net, net_dst, true)) + friend_ack_rxed(net, iv_index, net_seq, + net_src, net_dst, + msg); + else + ack_received(net, false, + net_src, net_dst, + net_seqZero, + l_get_be32(msg + 3)); + } else { + ctl_received(net, !!(net_frnd), iv_index, + net_ttl, net_seq, net_src, + net_dst, net_opcode, rssi, + msg, app_msg_len); + } + } else if (net_segmented) { + /* If we accept SAR packets to non-Unicast, then + * Friend Sar at least needs to be Unicast Only + */ + if (is_lpn_friend(net, net_dst, true) && + !(net_dst & 0x8000)) { + /* Check TTL >= 2 before accepting segments + * for Friends + */ + if (net_ttl >= 2) { + friend_seg_rxed(net, iv_index, + net_ttl, net_seq, + net_src, net_dst, + l_get_be32(packet + 2 + 9), + msg, app_msg_len); + } + } else { + seg_rxed(net, net_frnd, + iv_index, + net_ttl, + net_seq, + net_src, net_dst, + net_key_id, + net_szmic, net_seqZero, + net_segO, net_segN, + msg, app_msg_len); + } + + } else { + msg_rxed(net, net_frnd, + iv_index, + net_ttl, + net_seq, + net_src, net_dst, + net_key_id, + false, net_seq & SEQ_ZERO_MASK, + msg, app_msg_len); + } + + if (!!(net_frnd)) + l_info("Ask for more data!"); + + /* If this is one of our Unicast addresses, don't relay */ + if (net_dst <= 0x7fff) + return; + } + + if (!net->relay.enable || net_ttl < 0x02 || net_frnd) + return; + + packet[2 + 1] = (packet[2 + 1] & ~TTL_MASK) | (net_ttl - 1); + + if (!mesh_crypto_packet_encode(packet + 2, size, keys->enc_key, + iv_index, keys->privacy_key)) { + l_error("Failed to encode relay packet"); + return; + } + + if (net->relay.enable) + send_relay_pkt(net, packet, size + 1); +} + +static void net_msg_recv(void *user_data, struct mesh_io_recv_info *info, + const uint8_t *data, uint16_t len) +{ + struct mesh_net *net = user_data; + int8_t rssi = 0; + + if (len <= 2 || !net) + return; + + if (info) { + net->instant = info->instant; + net->chan = info->chan; + rssi = info->rssi; + } + + packet_received(user_data, data + 1, len - 1, rssi); +} + +static void set_network_beacon(void *a, void *b) +{ + struct mesh_subnet *subnet = a; + struct mesh_net *net = b; + uint8_t beacon_data[22]; + + if (!create_secure_beacon(net, subnet, beacon_data, + sizeof(beacon_data))) + return; + + if (memcmp(&subnet->snb.beacon[1], beacon_data, + sizeof(beacon_data)) == 0) + return; + + memcpy(&subnet->snb.beacon[1], beacon_data, sizeof(beacon_data)); + + if (net->beacon_enable && !net->friend_addr) { + print_packet("Set My Beacon to", + beacon_data, sizeof(beacon_data)); + start_network_beacon(subnet, net); + } + + if (l_queue_length(net->friends)) { + struct mesh_friend_msg update = { + .src = net->src_addr, + .iv_index = mesh_net_get_iv_index(net), + .last_len = 7, + .ctl = true, + }; + + update.u.one[0].hdr = NET_OP_FRND_UPDATE << OPCODE_HDR_SHIFT; + update.u.one[0].seq = mesh_net_next_seq_num(net); + update.u.one[0].data[0] = NET_OP_FRND_UPDATE; + update.u.one[0].data[1] = beacon_data[3]; + l_put_be32(net->iv_index, update.u.one[0].data + 2); + update.u.one[0].data[6] = 0x01; /* More Data */ + /* print_packet("Frnd-Beacon-SRC", + * beacon_data, sizeof(beacon_data)); + */ + /* print_packet("Frnd-Update", update.u.one[0].data, 6); */ + + l_queue_foreach(net->friends, enqueue_update, &update); + } +} + +static void iv_upd_to(struct l_timeout *upd_timeout, void *user_data) +{ + struct mesh_net *net = user_data; + + switch (net->iv_upd_state) { + case IV_UPD_UPDATING: + if (l_queue_length(net->sar_out)) { + l_info("don't leave IV Update until sar_out empty"); + l_timeout_modify(net->iv_update_timeout, 10); + break; + } + + l_info("iv_upd_state = IV_UPD_NORMAL_HOLD"); + net->iv_upd_state = IV_UPD_NORMAL_HOLD; + l_timeout_modify(net->iv_update_timeout, IV_IDX_UPD_MIN); + mesh_net_set_seq_num(net, 0); + l_queue_foreach(net->subnets, set_network_beacon, net); + mesh_net_flush_msg_queues(net); + break; + + case IV_UPD_INIT: + case IV_UPD_NORMAL_HOLD: + case IV_UPD_NORMAL: + l_timeout_remove(upd_timeout); + net->iv_update_timeout = NULL; + l_info("iv_upd_state = IV_UPD_NORMAL"); + net->iv_upd_state = IV_UPD_NORMAL; + if (net->seq_num > IV_UPDATE_SEQ_TRIGGER) + mesh_net_iv_index_update(net); + break; + } +} + +static void update_iv_kr_state(struct mesh_subnet *subnet, uint32_t iv_index, + bool iv_update, bool kr_transition, + bool rxed_key_refresh, bool lpn) +{ + struct mesh_net *net = subnet->net; + uint8_t local_kr; + uint32_t local_iv_index; + bool local_iv_update; + + /* Save original settings to avoid resetting same values, + * and secure beacon timer + */ + local_iv_index = net->iv_index; + local_kr = subnet->key_refresh; + local_iv_update = iv_is_updating(net); + + if (iv_index != local_iv_index || kr_transition) + l_info("SNB-RX: %8.8x - Key Refresh: %d IV Update: %d", + iv_index, rxed_key_refresh, iv_update); + + if (iv_update && (net->iv_upd_state > IV_UPD_UPDATING)) { + if (iv_index != net->iv_index) + l_error("Update attempted to0 soon (Normal < MIN)"); + + return; + } + + if (net->iv_upd_state == IV_UPD_INIT) { + if (iv_index > net->iv_index) + mesh_net_set_seq_num(net, 0); + net->iv_index = iv_index; + + if (iv_update) { + /* Other devices will be accepting old or new iv_index, + * but we don't know how far through update they are. + * Starting permissive state will allow us maximum + * (96 hours) to resync + */ + l_info("iv_upd_state = IV_UPD_UPDATING"); + net->iv_upd_state = IV_UPD_UPDATING; + net->iv_update_timeout = l_timeout_create( + IV_IDX_UPD_MIN, iv_upd_to, net, NULL); + } else { + l_info("iv_upd_state = IV_UPD_NORMAL"); + net->iv_upd_state = IV_UPD_NORMAL; + } + + storage_local_set_iv_index(net, iv_index, net->iv_upd_state); + + /* Figure out the key refresh phase */ + if (kr_transition) { + if (rxed_key_refresh) + mesh_net_key_refresh_phase_two(net, + subnet->idx); + else + mesh_net_key_refresh_finish(net, subnet->idx); + } + + if (!lpn) + set_network_beacon(subnet, net); + + return; + } + + if (iv_update && !iv_is_updating(net)) { + l_info("iv_upd_state = IV_UPD_UPDATING"); + net->iv_upd_state = IV_UPD_UPDATING; + net->iv_update_timeout = l_timeout_create(IV_IDX_UPD_MIN, + iv_upd_to, net, NULL); + storage_local_set_iv_index(net, iv_index, net->iv_upd_state); + } else if (iv_update && iv_index != net->iv_index) { + l_error("Update attempted too soon (iv idx already updated)"); + return; + } + + if (iv_index != local_iv_index || kr_transition) + l_info("IVindex 0x%8.8x / Key Refresh update received", + iv_index); + + if (iv_index > net->iv_index) { + l_queue_clear(net->msg_cache, mesh_msg_free); + net->iv_index = iv_index; + storage_local_set_iv_index(net, iv_index, net->iv_upd_state); + } + + /* Figure out the key refresh phase */ + if (kr_transition) { + if (rxed_key_refresh) + mesh_net_key_refresh_phase_two(net, subnet->idx); + else + mesh_net_key_refresh_finish(net, subnet->idx); + } + + if (!lpn) + return; + + if (local_kr != subnet->key_refresh || + local_iv_index != net->iv_index || + local_iv_update != iv_is_updating(net)) + set_network_beacon(subnet, net); +} + +static void process_beacon(void *user_data, const void *data, + uint8_t size, int8_t rssi) +{ + struct mesh_net *net = user_data; + const uint8_t *buf = data; + uint32_t iv_index; + uint64_t cmac; + bool iv_update, rxed_iv_update, rxed_key_refresh; + struct mesh_subnet *subnet; + struct net_key *keys; + bool kr_transition = false; + + if (size != 22 || buf[0] != 0x01) + return; + + /* print_packet("Secure Net Beacon RXed", data, size); */ + rxed_key_refresh = (buf[1] & 0x01) == 0x01; + rxed_iv_update = iv_update = (buf[1] & 0x02) == 0x02; + iv_index = l_get_be32(buf + 10); + + l_debug("KR: %d -- IVU: %d -- IV: %8.8x", + rxed_key_refresh, rxed_iv_update, iv_index); + + /* Inhibit recognizing iv_update true-->false + * if we have outbound SAR messages in flight + */ + if (l_queue_length(net->sar_out)) { + if (!iv_update && iv_update != iv_is_updating(net)) + iv_update = true; + } + + subnet = l_queue_find(net->subnets, match_network_id, buf + 2); + if (!subnet) + return; + + /* Check if Key Refresh flag value is different from + * the locally stored one + */ + if (rxed_key_refresh != subnet->key_refresh) + kr_transition = true; + + /* If the local node is a provisioner or there are no new keys, + * ignore KR beacon setting + */ + if (net->provisioner) + kr_transition = false; + else if (subnet->updated.key_set.nid == NET_NID_INVALID) + kr_transition = false; + /* If beacon's key refresh bit is not set and the beacon is encoded + * with the "new" network key, this signals transition from + * key refresh procedure to normal operation + */ + else if (!rxed_key_refresh && + !memcmp(subnet->updated.network_id, buf + 2, 8)) + kr_transition = true; + + if ((net->iv_index + IV_IDX_DIFF_RANGE < iv_index) || + (iv_index < net->iv_index)) { + l_info("iv index outside range"); + return; + } + + /* Don't bother going further if nothing has changed */ + if (!memcmp(&subnet->snb.beacon[1], buf, size)) { + subnet->snb.observed++; + return; + } + + if (!rxed_key_refresh && !subnet->key_refresh && !kr_transition) + keys = &subnet->current; + else if (subnet->updated.key_set.nid != NET_NID_INVALID) + keys = &subnet->updated; + else + return; + + if (memcmp(keys->network_id, buf + 2, 8)) + return; + + /* Any behavioral changes must pass CMAC test */ + if (!mesh_crypto_beacon_cmac(keys->beacon_key, keys->network_id, + iv_index, rxed_key_refresh, + rxed_iv_update, &cmac)) { + l_error("mesh_crypto_beacon_cmac failed"); + return; + } + + if (cmac != l_get_be64(buf + 14)) { + l_error("cmac compare failed %16.16lx != %16.16lx", + cmac, l_get_be64(buf + 14)); + return; + } + + if (iv_index == net->iv_index && + iv_is_updating(net) == iv_update && !kr_transition) { + l_info("No change: IV index %4.4x, rxed KR = %d ", + iv_index, rxed_key_refresh); + if (net->iv_upd_state == IV_UPD_INIT) { + l_info("iv_upd_state = IV_UPD_NORMAL"); + net->iv_upd_state = IV_UPD_NORMAL; + } + + subnet->snb.observed++; + return; + } + + subnet->snb.observed++; + + update_iv_kr_state(subnet, iv_index, iv_update, kr_transition, + rxed_key_refresh, false); +} + +static void lpn_process_beacon(void *user_data, const void *data, + uint8_t size, int8_t rssi) +{ + struct mesh_net *net = user_data; + const uint8_t *buf = data; + uint32_t iv_index; + bool iv_update, rxed_key_refresh; + struct mesh_subnet *subnet; + bool kr_transition = false; + + /* print_packet("lpn: Secure Net Beacon RXed", data, size); */ + rxed_key_refresh = (buf[0] & 0x01) == 0x01; + iv_index = l_get_be32(buf + 1); + + l_debug("KR: %d -- IVU: %d -- IV: %8.8x", + rxed_key_refresh, iv_index); + + /* Inhibit recognizing iv_update true-->false if we have outbound + * SAR messages in flight + */ + if (l_queue_length(net->sar_out)) { + if (!iv_update && iv_update != iv_is_updating(net)) + iv_update = true; + } + + /* TODO: figure out actual network index (i.e., friendship subnet) */ + subnet = get_primary_subnet(net); + if (!subnet) + return; + + /* Check if Key Refresh flag value is different from + * the locally stored one + */ + if (rxed_key_refresh != subnet->key_refresh) + kr_transition = true; + + /* If the local node is a provisioner or there are no new keys, + * ignore KR beacon setting + */ + if (subnet->updated.key_set.nid == NET_NID_INVALID) + kr_transition = false; + + if ((net->iv_index + IV_IDX_DIFF_RANGE < iv_index) || + (iv_index < net->iv_index)) { + l_info("iv index outside range"); + return; + } + + /* Don't bother going further if nothing has changed */ + if (!kr_transition && iv_index == net->iv_index && + iv_update == iv_is_updating(net) && + net->iv_upd_state != IV_UPD_INIT) + return; + + if (iv_index == net->iv_index && + iv_is_updating(net) == iv_update && !kr_transition) { + l_info("No change: IV index %4.4x, rxed KR = %d ", + iv_index, rxed_key_refresh); + if (net->iv_upd_state == IV_UPD_INIT) { + l_info("iv_upd_state = IV_UPD_NORMAL"); + net->iv_upd_state = IV_UPD_NORMAL; + } + return; + } + + update_iv_kr_state(subnet, iv_index, iv_update, kr_transition, + rxed_key_refresh, true); +} + +static void beacon_recv(void *user_data, struct mesh_io_recv_info *info, + const uint8_t *data, uint16_t len) +{ + struct mesh_net *net = user_data; + int8_t rssi = 0; + + if (len <= 2 || !net) + return; + + if (info) { + net->instant = info->instant; + net->chan = info->chan; + rssi = info->rssi; + } + + process_beacon(user_data, data + 1, len - 1, rssi); +} + +bool mesh_net_set_beacon_mode(struct mesh_net *net, bool enable) +{ + if (!net || !IS_UNASSIGNED(net->friend_addr)) + return false; + + if (net->beacon_enable != enable) { + uint8_t type = MESH_AD_TYPE_BEACON; + + net->beacon_enable = enable; + + if (!enable) + mesh_io_send_cancel(net->io, &type, 1); + + l_queue_foreach(net->subnets, start_network_beacon, net); + } + + return true; +} + + +bool mesh_net_attach(struct mesh_net *net, struct mesh_io *io) +{ + if (!net) + return false; + + net->io = io; + + return true; +} + +struct mesh_io *mesh_net_detach(struct mesh_net *net) +{ + struct mesh_io *io; + uint8_t type = 0; + + if (!net) + return NULL; + + io = net->io; + + mesh_io_send_cancel(net->io, &type, 1); + mesh_io_deregister_recv_cb(io, MESH_IO_FILTER_BEACON); + mesh_io_deregister_recv_cb(io, MESH_IO_FILTER_NET); + + net->io = NULL; + + return io; +} + +bool mesh_net_iv_index_update(struct mesh_net *net) +{ + if (net->iv_upd_state != IV_UPD_NORMAL) + return false; + + l_info("iv_upd_state = IV_UPD_UPDATING"); + mesh_net_flush_msg_queues(net); + net->iv_upd_state = IV_UPD_UPDATING; + net->iv_index++; + if (!storage_local_set_iv_index(net, net->iv_index, IV_UPD_UPDATING)) + return false; + + l_queue_foreach(net->subnets, set_network_beacon, net); + net->iv_update_timeout = l_timeout_create( + IV_IDX_UPD_MIN, + iv_upd_to, net, NULL); + + return true; +} + + + +void mesh_net_sub_list_add(struct mesh_net *net, uint16_t addr) +{ + uint8_t msg[11] = { PROXY_OP_FILTER_ADD }; + uint8_t n = 1; + + l_put_be16(addr, msg + n); + n += 2; + + mesh_net_transport_send(net, NULL, false, + mesh_net_get_iv_index(net), 0, + 0, 0, 0, msg, n); +} + +void mesh_net_sub_list_del(struct mesh_net *net, uint16_t addr) +{ + uint8_t msg[11] = { PROXY_OP_FILTER_DEL }; + uint8_t n = 1; + + l_put_be16(addr, msg + n); + n += 2; + + mesh_net_transport_send(net, NULL, false, + mesh_net_get_iv_index(net), 0, + 0, 0, 0, msg, n); +} + +bool mesh_net_dst_reg(struct mesh_net *net, uint16_t dst) +{ + struct mesh_destination *dest = l_queue_find(net->destinations, + match_by_dst, L_UINT_TO_PTR(dst)); + + if (IS_UNASSIGNED(dst) || IS_ALL_NODES(dst)) + return false; + + if (!dest) { + dest = l_new(struct mesh_destination, 1); + + if (dst < 0x8000) + l_queue_push_head(net->destinations, dest); + else + l_queue_push_tail(net->destinations, dest); + + /* If LPN, and Group/Virtual, add to Subscription List */ + if (net->friend_addr) { + /* TODO: Fix this garbage */ + uint32_t u32_dst[7] = {dst, 0xffffffff}; + + frnd_sub_add(net, u32_dst); + } + } + + dest->dst = dst; + dest->ref_cnt++; + + return true; +} + +bool mesh_net_dst_unreg(struct mesh_net *net, uint16_t dst) +{ + struct mesh_destination *dest = l_queue_find(net->destinations, + match_by_dst, L_UINT_TO_PTR(dst)); + + if (!dest) + return false; + + if (dest->ref_cnt) + dest->ref_cnt--; + + if (dest->ref_cnt) + return true; + + /* TODO: If LPN, and Group/Virtual, remove from Subscription List */ + if (net->friend_addr) { + /* TODO: Fix this garbage */ + uint32_t u32_dst[7] = {dst, 0xffffffff}; + + frnd_sub_del(net, u32_dst); + } + + l_queue_remove(net->destinations, dest); + + l_free(dest); + return true; +} + +bool mesh_net_flush(struct mesh_net *net) +{ + if (!net) + return false; + + /* TODO mesh-io Flush */ + return true; +} + +/* TODO: add net key index */ +static bool send_seg(struct mesh_net *net, struct mesh_sar *msg, uint8_t segO) +{ + uint8_t seg_len; + uint8_t gatt_data[30]; + uint8_t *packet = gatt_data; + uint8_t packet_len; + uint8_t segN = SEG_MAX(msg->len); + uint16_t seg_off = SEG_OFF(segO); + struct mesh_key_set *key_set = NULL; + uint32_t seq_num = mesh_net_next_seq_num(net); + + if (segN) { + if (msg->len - seg_off > SEG_OFF(1)) + seg_len = SEG_OFF(1); + else + seg_len = msg->len - seg_off; + } else { + seg_len = msg->len; + } + + /* Start IV Update procedure when we hit our trigger point */ + if (!msg->frnd && net->seq_num > IV_UPDATE_SEQ_TRIGGER) + mesh_net_iv_index_update(net); + + l_debug("segN %d segment %d seg_off %d", segN, segO, seg_off); + /* print_packet("Sending", msg->buf + seg_off, seg_len); */ + { + /* TODO: Are we RXing on an LPN's behalf? Then set RLY bit */ + + if (!mesh_crypto_packet_build(false, msg->ttl, + seq_num, + msg->src, msg->remote, + 0, + segN ? true : false, msg->key_id, + msg->szmic, false, msg->seqZero, + segO, segN, + msg->buf + seg_off, seg_len, + packet + 1, &packet_len)) { + l_error("Failed to build packet"); + return false; + } + } + print_packet("Clr-Net Tx", packet + 1, packet_len); + + if (msg->frnd_cred && net->friend_addr) + key_set = frnd_get_key(net); + + if (key_set == NULL) { + struct mesh_subnet *subnet = get_primary_subnet(net); + + key_set = &subnet->tx->key_set; + } + + if (!mesh_crypto_packet_encode(packet + 1, packet_len, key_set->enc_key, + msg->iv_index, key_set->privacy_key)) { + l_error("Failed to encode packet"); + return false; + } + + print_packet("Step 3", packet + 1, packet_len); + if (!mesh_crypto_packet_label(packet + 1, packet_len, + msg->iv_index, key_set->nid)) { + l_error("Failed to label packet"); + return false; + } + /* print_packet("Step 4", packet + 1, packet_len); */ + + { + char *str; + + send_msg_pkt(net, packet, packet_len + 1); + + str = l_util_hexstring(packet + 1, packet_len); + l_info("TX: Network %04x -> %04x : %s (%u) : TTL %d : SEQ %06x", + msg->src, msg->remote, str, + packet_len, msg->ttl, + msg->frnd ? msg->seqAuth + segO : seq_num); + l_free(str); + } + + msg->last_seg = segO; + + return true; +} + +void mesh_net_send_seg(struct mesh_net *net, struct mesh_key_set *key_set, + uint32_t iv_index, + uint8_t ttl, + uint32_t seq, + uint16_t src, uint16_t dst, + uint32_t hdr, + const void *seg, uint16_t seg_len) +{ + char *str; + uint8_t packet[30]; + uint8_t packet_len; + bool segmented = !!((hdr >> SEG_HDR_SHIFT) & true); + uint8_t key_id = (hdr >> KEY_HDR_SHIFT) & KEY_ID_MASK; + bool szmic = !!((hdr >> SZMIC_HDR_SHIFT) & true); + uint16_t seqZero = (hdr >> SEQ_ZERO_HDR_SHIFT) & SEQ_ZERO_MASK; + uint8_t segO = (hdr >> SEGO_HDR_SHIFT) & SEG_MASK; + uint8_t segN = (hdr >> SEGN_HDR_SHIFT) & SEG_MASK; + + /* TODO: Only used for current POLLed segments to LPNs */ + + l_debug("SEQ: %6.6x", seq + segO); + l_debug("SEQ0: %6.6x", seq); + l_debug("segO: %d", segO); + + if (!mesh_crypto_packet_build(false, ttl, + seq, + src, dst, + 0, + segmented, key_id, + szmic, false, seqZero, + segO, segN, + seg, seg_len, + packet + 1, &packet_len)) { + l_error("Failed to build packet"); + return; + } + + if (!mesh_crypto_packet_encode(packet + 1, packet_len, key_set->enc_key, + iv_index, key_set->privacy_key)) { + l_error("Failed to encode packet"); + return; + } + + /* print_packet("Step 3", packet + 0, packet_len); */ + if (!mesh_crypto_packet_label(packet + 1, packet_len, iv_index, + key_set->nid)) { + l_error("Failed to label packet"); + return; + } + /* print_packet("Step 4", packet + 1, packet_len); */ + + send_msg_pkt(net, packet, packet_len + 1); + + str = l_util_hexstring(packet + 1, packet_len); + l_info("TX: Friend Seg-%d %04x -> %04x : %s (%u) : TTL %d : SEQ %06x", + segO, src, dst, str, packet_len, ttl, seq); + l_free(str); +} + +unsigned int mesh_net_app_send(struct mesh_net *net, bool frnd_cred, + uint16_t src, uint16_t dst, + uint8_t key_id, uint8_t ttl, + uint32_t seq, uint32_t iv_index, + bool szmic, + const void *msg, uint16_t msg_len, + mesh_net_status_func_t status_func, + void *user_data) +{ + struct mesh_sar *payload = NULL; + uint8_t seg, seg_max; + unsigned int ret = 0; + bool result; + + if (!net || msg_len > 384) + return 0; + + if (!src) + src = net->src_addr; + + if (!src || !dst) + return 0; + + if (ttl == 0xff) + ttl = net->default_ttl; + + seg_max = SEG_MAX(msg_len); + + /* First enqueue to any Friends and internal models */ + result = msg_rxed(net, false, + iv_index, + ttl, + seq + seg_max, + src, dst, + key_id, + szmic, seq & SEQ_ZERO_MASK, + msg, msg_len); + + /* If successfully enqued or delivered + * to Unicast address, we are done + */ + if (result || src == dst || + (dst >= net->src_addr && dst <= net->last_addr)) { + /* Adjust our seq_num for "virtual" delivery */ + net->seq_num += seg_max; + mesh_net_next_seq_num(net); + return 0; + } + + /* If Segmented, Cancel any OB segmented message to same DST */ + if (seg_max) { + payload = l_queue_remove_if(net->sar_out, match_sar_remote, + L_UINT_TO_PTR(dst)); + mesh_sar_free(payload); + } + + /* Setup OTA Network send */ + payload = mesh_sar_new(msg_len); + memcpy(payload->buf, msg, msg_len); + payload->len = msg_len; + payload->src = src; + payload->remote = dst; + payload->ttl = ttl; + payload->szmic = szmic; + payload->frnd_cred = frnd_cred; + payload->key_id = key_id; + if (seg_max) { + payload->flags = 0xffffffff >> (31 - seg_max); + payload->seqZero = seq & SEQ_ZERO_MASK; + } + + payload->iv_index = mesh_net_get_iv_index(net); + payload->seqAuth = net->seq_num; + + result = true; + if (!IS_UNICAST(dst) && seg_max) { + for (int i = 0; i < 4; i++) { + for (seg = 0; seg <= seg_max && result; seg++) + result = send_seg(net, payload, seg); + } + } else { + for (seg = 0; seg <= seg_max && result; seg++) + result = send_seg(net, payload, seg); + } + + /* Reliable: Cache; Unreliable: Flush*/ + if (result && seg_max && IS_UNICAST(dst)) { + l_queue_push_head(net->sar_out, payload); + payload->seg_timeout = + l_timeout_create(SEG_TO, outseg_to, net, NULL); + payload->msg_timeout = + l_timeout_create(MSG_TO, outmsg_to, net, NULL); + payload->status_func = status_func; + payload->user_data = user_data; + ret = payload->id = ++net->sar_id_next; + } else + mesh_sar_free(payload); + + return ret; +} + +void mesh_net_app_send_cancel(struct mesh_net *net, unsigned int id) +{ + struct mesh_sar *sar = l_queue_remove_if(net->sar_out, match_sar_id, + L_UINT_TO_PTR(id)); + + if (sar) { + l_info("Canceling OB %d", id); + if (sar->status_func) + sar->status_func(sar->remote, 2, + sar->buf, sar->len - 4, + sar->user_data); + } + mesh_sar_free(sar); +} + +/* TODO: add net key index */ +void mesh_net_ack_send(struct mesh_net *net, struct mesh_key_set *key_set, + uint32_t iv_index, + uint8_t ttl, + uint32_t seq, + uint16_t src, uint16_t dst, + bool rly, uint16_t seqZero, + uint32_t ack_flags) +{ + uint32_t hdr; + uint8_t data[7]; + uint8_t pkt_len; + uint8_t pkt[30]; + char *str; + + hdr = NET_OP_SEG_ACKNOWLEDGE << OPCODE_HDR_SHIFT; + hdr |= rly << RELAY_HDR_SHIFT; + hdr |= (seqZero & SEQ_ZERO_MASK) << SEQ_ZERO_HDR_SHIFT; + l_put_be32(hdr, data); + l_put_be32(ack_flags, data + 3); + if (!mesh_crypto_packet_build(true, ttl, + seq, + src, dst, + NET_OP_SEG_ACKNOWLEDGE, + false, /* Not Segmented */ + 0, /* No Key ID associated */ + false, rly, seqZero, + 0, 0, /* no segO or segN */ + data + 1, 6, + pkt + 1, &pkt_len)) { + return; + } + + if (key_set == NULL) { + struct mesh_subnet *subnet = get_primary_subnet(net); + + key_set = &subnet->tx->key_set; + } + + if (!mesh_crypto_packet_encode(pkt + 1, pkt_len, key_set->enc_key, + iv_index, key_set->privacy_key)) { + l_error("Failed to encode packet"); + return; + } + + /* print_packet("Step 3", pkt, pkt_len); */ + if (!mesh_crypto_packet_label(pkt + 1, pkt_len, + iv_index, key_set->nid)) { + l_error("Failed to label packet"); + return; + } + + /* print_packet("Step 4", pkt, pkt_len); */ + send_msg_pkt(net, pkt, pkt_len + 1); + + str = l_util_hexstring(pkt + 1, pkt_len); + l_info("TX: Friend ACK %04x -> %04x : %s (%u) : TTL %d : SEQ %06x", + src, dst, str, pkt_len, + ttl, seq); + l_free(str); +} + +/* TODO: add net key index */ +void mesh_net_transport_send(struct mesh_net *net, struct mesh_key_set *key_set, + bool fast, uint32_t iv_index, uint8_t ttl, + uint32_t seq, uint16_t src, uint16_t dst, + const uint8_t *msg, uint16_t msg_len) +{ + uint32_t use_seq = seq; + uint8_t pkt_len; + uint8_t pkt[30]; + bool result = false; + + if (!net->src_addr) + return; + + if (!src) + src = net->src_addr; + + if (src == dst) + return; + + if (ttl == 0xff) + ttl = net->default_ttl; + + /* Range check the Opcode and msg length*/ + if (*msg & 0xc0 || (9 + msg_len + 8 > 29)) + return; + + /* Enqueue for Friend if forwardable and from us */ + if (!(key_set) && src >= net->src_addr && src <= net->last_addr) { + uint32_t hdr = msg[0] << OPCODE_HDR_SHIFT; + uint8_t frnd_ttl = ttl; + + if (friend_packet_queue(net, iv_index, + true, frnd_ttl, + mesh_net_next_seq_num(net), + src, dst, + hdr, + msg + 1, msg_len - 1)) { + return; + } + } + + /* Deliver to Local entities if applicable */ + if (!(dst & 0x8000) && src >= net->src_addr && src <= net->last_addr) { + result = ctl_received(net, !!(key_set), + iv_index, ttl, + mesh_net_next_seq_num(net), + src, dst, + msg[0], 0, msg + 1, msg_len - 1); + } + + if (key_set == NULL) { + struct mesh_subnet *subnet = get_primary_subnet(net); + + key_set = &subnet->tx->key_set; + use_seq = mesh_net_next_seq_num(net); + + if (result || (dst >= net->src_addr && dst <= net->last_addr)) + return; + } + + if (!mesh_crypto_packet_build(true, ttl, + use_seq, + src, dst, + msg[0], + false, 0, + false, false, 0, + 0, 0, + msg + 1, msg_len - 1, + pkt + 1, &pkt_len)) + return; + + /* print_packet("Step 2", pkt + 1, pkt_len); */ + + if (!mesh_crypto_packet_encode(pkt + 1, pkt_len, key_set->enc_key, + iv_index, key_set->privacy_key)) { + l_error("Failed to encode pkt"); + return; + } + + /* print_packet("Step 3", pkt + 1, pkt_len); */ + if (!mesh_crypto_packet_label(pkt, pkt_len, iv_index, + key_set->nid)) { + l_error("Failed to label pkt"); + return; + } + + /* print_packet("Step 4", pkt + 1, pkt_len); */ + + if (dst != 0) { + char *str; + + send_msg_pkt(net, pkt, pkt_len + 1); + + str = l_util_hexstring(pkt + 1, pkt_len); + l_info("TX: Network %04x -> %04x : %s (%u) : TTL %d : SEQ %06x", + src, dst, str, pkt_len, + ttl, use_seq); + l_free(str); + } +} + +uint8_t mesh_net_key_refresh_phase_set(struct mesh_net *net, uint16_t idx, + uint8_t transition) +{ + struct mesh_subnet *subnet; + + if (!net) + return MESH_STATUS_UNSPECIFIED_ERROR; + + subnet = l_queue_find(net->subnets, match_key_index, + L_UINT_TO_PTR(idx)); + if (!subnet) + return MESH_STATUS_INVALID_NETKEY; + + if (transition == subnet->kr_phase) + return MESH_STATUS_SUCCESS; + + if ((transition != 2 && transition != 3) || + transition < subnet->kr_phase) + return MESH_STATUS_CANNOT_SET; + + switch (transition) { + case 2: + if (mesh_net_key_refresh_phase_two(net, idx) + != MESH_STATUS_SUCCESS) + return MESH_STATUS_CANNOT_SET; + break; + case 3: + if (mesh_net_key_refresh_finish(net, idx) + != MESH_STATUS_SUCCESS) + return MESH_STATUS_CANNOT_SET; + break; + default: + return MESH_STATUS_CANNOT_SET; + } + return MESH_STATUS_SUCCESS; +} + +uint8_t mesh_net_key_refresh_phase_get(struct mesh_net *net, uint16_t idx, + uint8_t *phase) +{ + struct mesh_subnet *subnet; + + if (!net) + return MESH_STATUS_UNSPECIFIED_ERROR; + + subnet = l_queue_find(net->subnets, match_key_index, + L_UINT_TO_PTR(idx)); + if (!subnet) + return MESH_STATUS_INVALID_NETKEY; + + *phase = subnet->kr_phase; + return MESH_STATUS_SUCCESS; +} + +int mesh_net_kr_phase_one(struct mesh_net *net, uint16_t idx, + const uint8_t *value) +{ + struct mesh_subnet *subnet; + + if (!net) + return MESH_STATUS_UNSPECIFIED_ERROR; + + subnet = l_queue_find(net->subnets, match_key_index, + L_UINT_TO_PTR(idx)); + if (!subnet) + return MESH_STATUS_CANNOT_UPDATE; + + if (subnet->updated.key_set.nid != NET_NID_INVALID) + l_info("Warning: overwriting new keys"); + + /* Preserve starting data */ + subnet->updated = subnet->current; + + /* Generate new keys */ + if (!create_keys(net, &subnet->updated, value)) { + subnet->updated.key_set.nid = NET_NID_INVALID; + l_error("Failed to start key refresh phase one"); + return MESH_STATUS_CANNOT_UPDATE; + } + + /* If we are an LPN, generate our keys here */ + if (net->friend_addr) + frnd_key_refresh(net, 1); + else + /* If we are a Friend-Node, generate all our new keys */ + l_queue_foreach(net->friends, frnd_kr_phase1, (void *)value); + + l_info("key refresh phase 1: NID 0x%2x", subnet->updated.key_set.nid); + + mesh_net_add_keyset(net, &subnet->updated.key_set); + subnet->kr_phase = KEY_REFRESH_PHASE_ONE; + + return MESH_STATUS_SUCCESS; +} + +int mesh_net_key_refresh_phase_two(struct mesh_net *net, uint16_t idx) +{ + struct mesh_subnet *subnet; + + if (!net) + return MESH_STATUS_UNSPECIFIED_ERROR; + + subnet = l_queue_find(net->subnets, match_key_index, + L_UINT_TO_PTR(idx)); + + if (!subnet || subnet->updated.key_set.nid == NET_NID_INVALID) + return MESH_STATUS_INVALID_NETKEY; + + l_info("Key refresh procedure phase 2: start using new net TX keys"); + subnet->key_refresh = 1; + subnet->tx = &subnet->updated; + /* TODO: Provisioner may need to stay in phase three until + * it hears beacons from all the nodes + */ + subnet->kr_phase = KEY_REFRESH_PHASE_TWO; + set_network_beacon(subnet, net); + + if (net->friend_addr) + frnd_key_refresh(net, 2); + else + l_queue_foreach(net->friends, frnd_kr_phase2, net); + + return MESH_STATUS_SUCCESS; +} + +int mesh_net_key_refresh_finish(struct mesh_net *net, uint16_t idx) +{ + struct mesh_subnet *subnet; + + if (!net) + return MESH_STATUS_UNSPECIFIED_ERROR; + + subnet = l_queue_find(net->subnets, match_key_index, + L_UINT_TO_PTR(idx)); + + if (!subnet || subnet->updated.key_set.nid == NET_NID_INVALID) + return MESH_STATUS_INVALID_NETKEY; + + if (subnet->kr_phase == KEY_REFRESH_PHASE_NONE) + return MESH_STATUS_SUCCESS; + + l_info("Key refresh phase 3: use new keys only, discard old ones"); + + /* Switch to using new keys, discard old ones */ + subnet->current = subnet->updated; + subnet->tx = &subnet->current; + subnet->updated.key_set.nid = NET_NID_INVALID; + mesh_net_remove_keyset(net, &subnet->updated.key_set); + subnet->key_refresh = 0; + subnet->kr_phase = KEY_REFRESH_PHASE_NONE; + set_network_beacon(subnet, net); + + if (net->friend_addr) + frnd_key_refresh(net, 3); + else + l_queue_foreach(net->friends, frnd_kr_phase3, net); + + return MESH_STATUS_SUCCESS; +} + +void mesh_net_heartbeat_send(struct mesh_net *net) +{ + struct mesh_net_heartbeat *hb = &net->heartbeat; + uint8_t msg[4]; + int n = 0; + + if (hb->pub_dst == UNASSIGNED_ADDRESS) + return; + + msg[n++] = NET_OP_HEARTBEAT; + msg[n++] = hb->pub_ttl; + l_put_be16(hb->features, msg + n); + n += 2; + + mesh_net_transport_send(net, NULL, false, mesh_net_get_iv_index(net), + hb->pub_ttl, 0, 0, hb->pub_dst, msg, n); +} + +void mesh_net_heartbeat_init(struct mesh_net *net) +{ + struct mesh_net_heartbeat *hb = &net->heartbeat; + + memset(hb, 0, sizeof(struct mesh_net_heartbeat)); + hb->sub_min_hops = 0xff; + hb->features = mesh_net_get_features(net); +} + +uint32_t mesh_net_friend_timeout(struct mesh_net *net, uint16_t addr) +{ + struct mesh_friend *frnd = l_queue_find(net->friends, match_by_friend, + L_UINT_TO_PTR(addr)); + + if (!frnd) + return 0; + else + return frnd->poll_timeout; +} + +bool mesh_net_local_node_set(struct mesh_net *net, struct mesh_node *node, + bool provisioner) +{ + if (net->local_node) { + l_info("Local node already registered"); + return false; + } + + net->local_node = node; + net->provisioner = provisioner; + + return true; +} + +bool mesh_net_provisioned_new(struct mesh_net *net, uint8_t device_key[16], + uint16_t net_idx, uint8_t net_key[16], + uint16_t unicast, uint16_t snb_flags, + uint32_t iv_index, mesh_status_func_t cb, + void *user_data) +{ + if (net->provisioned || !net->local_node) + return false; + + if (!node_set_primary(net->local_node, unicast) || + !(mesh_net_register_unicast(net, unicast, + mesh_net_get_num_ele(net)))) + return false; + + if (!node_set_device_key(net->local_node, device_key)) + return false; + + net->iv_index = iv_index; + net->iv_update = ((snb_flags & 0x02) != 0); + if (!storage_local_set_iv_index(net, iv_index, net->iv_update)) + return false; + + if (mesh_net_add_key(net, false, net_idx, net_key) != + MESH_STATUS_SUCCESS) + return false; + + if ((snb_flags & 0x01) && + (mesh_net_add_key(net, true, net_idx, net_key) != + MESH_STATUS_SUCCESS)) { + l_queue_clear(net->subnets, l_free); + return false; + } + + return storage_save_new_config(net, net->cfg_file, cb, user_data); + +} + +void mesh_net_provisioned_set(struct mesh_net *net, bool provisioned) +{ + struct mesh_io *io; + + if (!net) + return; + + net->provisioned = provisioned; + io = net->io; + + if (provisioned) { + mesh_io_register_recv_cb(io, MESH_IO_FILTER_BEACON, + beacon_recv, net); + mesh_io_register_recv_cb(io, MESH_IO_FILTER_NET, + net_msg_recv, net); + } else { + uint8_t *uuid = node_uuid_get(net->local_node); + + if (!uuid) + return; + + mesh_io_deregister_recv_cb(io, MESH_IO_FILTER_BEACON); + mesh_io_deregister_recv_cb(io, MESH_IO_FILTER_NET); + + mesh_prov_listen(net, uuid, (uint8_t *) &net->prov_caps, + acceptor_prov_open, + acceptor_prov_close, + acceptor_prov_receive, net); + } +} -- 2.14.3