Return-Path: From: Brian Gix To: Marcel Holtmann , Luiz Augusto von Dentz , linux-bluetooth@vger.kernel.org Cc: Brian Gix Subject: [PATCH BlueZ v4 04/14] meshd: Initial Mesh Friendship support Date: Mon, 30 Apr 2018 14:03:09 -0700 Message-Id: <20180430210319.25137-5-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/friend.c | 1116 ++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 1116 insertions(+) create mode 100644 meshd/src/friend.c diff --git a/meshd/src/friend.c b/meshd/src/friend.c new file mode 100644 index 000000000..a212f5523 --- /dev/null +++ b/meshd/src/friend.c @@ -0,0 +1,1116 @@ +/* + * + * 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/src/mesh.h" +#include "meshd/src/node.h" +#include "meshd/src/net.h" +#include "meshd/src/crypto.h" +#include "meshd/src/model.h" +#include "meshd/src/display.h" + +#include "meshd/src/friend.h" + +#define MAX_FRND_GROUPS 20 +#define FRND_RELAY_WINDOW 250 /* 250 ms */ +#define FRND_CACHE_SIZE 16 +#define FRND_SUB_LIST_SIZE 8 + +#define RESPONSE_DELAY (100 - 12) /* 100 ms - 12ms hw delay */ +#define MIN_RESP_DELAY 10 /* 10 ms */ +#define MAX_RESP_DELAY 255 /* 255 ms */ + +/* Absolute maximum time to wait for LPN to choose us. */ +#define RESPONSE_POLL_DELAY 1300 /* 1.300 s */ + +static uint8_t frnd_relay_window = FRND_RELAY_WINDOW; +static uint8_t frnd_cache_size = FRND_CACHE_SIZE; +static uint8_t frnd_sublist_size = FRND_SUB_LIST_SIZE; + +struct frnd_negotiation { + struct l_timeout *timeout; + struct mesh_net *net; + struct mesh_key_set key_set; + uint32_t poll_timeout; + uint16_t low_power_node; + uint16_t old_relay; + uint8_t num_ele; + uint8_t lp_cnt; + uint8_t fn_cnt; + uint8_t wrfrw; + uint8_t receive_delay; + int8_t rssi; + bool clearing; +}; + +static struct l_queue *frnd_negotiations; +static uint16_t counter; + +static void response_timeout(struct l_timeout *timeout, void *user_data) +{ + struct frnd_negotiation *neg = user_data; + + /* LPN did not choose us */ + l_info("Did not win negotiation for %4.4x", neg->low_power_node); + + mesh_net_remove_keyset(neg->net, &neg->key_set); + l_queue_remove(frnd_negotiations, neg); + l_timeout_remove(timeout); + l_free(neg); +} + +static void response_delay(struct l_timeout *timeout, void *user_data) +{ + struct frnd_negotiation *neg = user_data; + uint16_t net_idx = mesh_net_get_primary_idx(neg->net); + uint8_t key[16]; + uint8_t p[9] = { 1 }; + uint8_t msg[8]; + uint16_t n = 0; + bool res; + + l_timeout_remove(timeout); + + /* Create key Set for this offer */ + l_put_be16(neg->low_power_node, p + 1); + l_put_be16(mesh_net_get_address(neg->net), p + 3); + l_put_be16(neg->lp_cnt, p + 5); + l_put_be16(counter, p + 7); + res = mesh_net_get_key(neg->net, false, net_idx, key); + if (!res) + goto cleanup; + + print_packet("Friend Key P =", p, 9); + res = mesh_crypto_k2(key, p, sizeof(p), &neg->key_set.nid, + neg->key_set.enc_key, neg->key_set.privacy_key); + if (!res) + goto cleanup; + + print_packet("NID =", &neg->key_set.nid, 1); + print_packet("ENC_KEY =", neg->key_set.enc_key, 16); + print_packet("PRIV_KEY =", neg->key_set.privacy_key, 16); + + neg->fn_cnt = counter++; + neg->key_set.frnd = true; + mesh_net_add_keyset(neg->net, &neg->key_set); + + msg[n++] = NET_OP_FRND_OFFER; + msg[n++] = frnd_relay_window; + msg[n++] = frnd_cache_size; + msg[n++] = frnd_sublist_size; + msg[n++] = neg->rssi; + l_put_be16(neg->fn_cnt, msg + n); + n += 2; + print_packet("Tx-NET_OP_FRND_OFFER", msg, n); + mesh_net_transport_send(neg->net, NULL, true, + mesh_net_get_iv_index(neg->net), 0, + 0, 0, neg->low_power_node, + msg, n); + + /* Offer expires in 1.3 seconds, which is the max time for LPN to + * receive all offers, 1 second to make decision, and a little extra + */ + neg->timeout = l_timeout_create_ms(1000 + MAX_RESP_DELAY, + response_timeout, neg, NULL); + + return; + +cleanup: + mesh_net_remove_keyset(neg->net, &neg->key_set); + l_queue_remove(frnd_negotiations, neg); + l_free(neg); +} + +static uint8_t cache_size(uint8_t power) +{ + return 1 << power; +} + +static bool match_by_lpn(const void *a, const void *b) +{ + const struct frnd_negotiation *neg = a; + uint16_t lpn = L_PTR_TO_UINT(b); + + return neg->low_power_node == lpn; +} + +static bool match_by_dst(const void *a, const void *b) +{ + const struct mesh_friend *frnd = a; + uint16_t dst = L_PTR_TO_UINT(b); + + return frnd->dst == dst; +} + +/* Scaling factors in 1/10 ms */ +static const int32_t scaling[] = { + 10, + 15, + 20, + 15, +}; + +void friend_request(struct mesh_net *net, uint16_t src, + uint8_t minReq, uint8_t delay, uint32_t timeout, + uint16_t prev, uint8_t num_ele, uint16_t cntr, + int8_t rssi) +{ + struct frnd_negotiation *neg; + uint8_t rssiScale = (minReq >> 5) & 3; + uint8_t winScale = (minReq >> 3) & 3; + uint8_t minCache = (minReq >> 0) & 7; + int32_t rsp_delay; + + l_info("RSSI of Request: %d dbm", rssi); + l_info("Delay: %d ms", delay); + l_info("Poll Timeout of Request: %d ms", timeout * 100); + l_info("Previous Friend: %4.4x", prev); + l_info("Num Elem: %2.2x", num_ele); + l_info("Cache Requested: %d", cache_size(minCache)); + l_info("Cache to offer: %d", frnd_cache_size); + + /* Determine our own suitability before + * deciding to participate in negotiation + */ + if (minCache == 0 || num_ele == 0) + return; + + if (delay < 0x0A) + return; + + if (timeout < 0x00000A || timeout > 0x34BBFF) + return; + + if (cache_size(minCache) > frnd_cache_size) + return; + + if (frnd_negotiations == NULL) + frnd_negotiations = l_queue_new(); + + /* TODO: Check RSSI, and then start Negotiation if appropriate */ + + /* We are participating in this Negotiation */ + neg = l_new(struct frnd_negotiation, 1); + l_queue_push_head(frnd_negotiations, neg); + + neg->net = net; + neg->low_power_node = src; + neg->lp_cnt = cntr; + neg->rssi = rssi; + neg->receive_delay = delay; + neg->poll_timeout = timeout; + neg->old_relay = prev; + neg->num_ele = num_ele; + + /* RSSI (Negative Factor, larger values == less time) + * Scaling factor 0-3 == multiplier of 1.0 - 2.5 + * Minimum factor of 1. Bit 1 adds additional factor + * of 1, bit zero and additional 0.5 + */ + rsp_delay = -(rssi * scaling[rssiScale]); + l_info("RSSI Factor: %d ms", rsp_delay / 10); + + /* Relay Window (Positive Factor, larger values == more time) + * Scaling factor 0-3 == multiplier of 1.0 - 2.5 + * Minimum factor of 1. Bit 1 adds additional factor + * of 1, bit zero and additional 0.5 + */ + rsp_delay += frnd_relay_window * scaling[winScale]; + l_info("Win Size Factor: %d ms", + (frnd_relay_window * scaling[winScale]) / 10); + + /* Normalize to ms */ + rsp_delay /= 10; + + /* Range limits are 10-255 ms */ + if (rsp_delay < MIN_RESP_DELAY) + rsp_delay = MIN_RESP_DELAY; + else if (rsp_delay > MAX_RESP_DELAY) + rsp_delay = MAX_RESP_DELAY; + + l_info("Total Response Delay: %d ms", rsp_delay); + + /* Add in 100ms delay before start of "Offer Period" */ + rsp_delay += RESPONSE_DELAY; + + neg->timeout = l_timeout_create_ms(rsp_delay, + response_delay, neg, NULL); +} + +static struct l_queue *retired_lpns; + +void friend_clear_confirm(struct mesh_net *net, uint16_t src, + uint16_t lpn, uint16_t lpnCounter) +{ + struct frnd_negotiation *neg = l_queue_remove_if(frnd_negotiations, + match_by_lpn, L_UINT_TO_PTR(lpn)); + + l_info("Friend Clear confirmed %4.4x (cnt %4.4x)", lpn, lpnCounter); + + if (!neg) + return; + + l_timeout_remove(neg->timeout); + l_queue_remove(frnd_negotiations, neg); + l_free(neg); +} + +static void friend_poll_timeout(struct l_timeout *timeout, void *user_data) +{ + struct mesh_friend *frnd = user_data; + + if (mesh_friend_clear(frnd->net, frnd)) + l_info("Friend Poll Timeout %4.4x", frnd->dst); + + l_timeout_remove(frnd->timeout); + frnd->timeout = NULL; + + /* Friend may be in either Network or Retired list, so try both */ + l_queue_remove(retired_lpns, frnd); + mesh_friend_free(frnd); +} + +void friend_clear(struct mesh_net *net, uint16_t src, uint16_t lpn, + uint16_t lpnCounter, struct mesh_friend *frnd) +{ + uint8_t msg[5] = { NET_OP_FRND_CLEAR_CONFIRM }; + bool removed = false; + uint16_t lpnDelta; + + if (frnd) { + lpnDelta = lpnCounter - frnd->lp_cnt; + + /* Ignore old Friend Clear commands */ + if (lpnDelta > 0x100) + return; + + /* Move friend from Network list to Retired list */ + removed = mesh_friend_clear(net, frnd); + if (removed) { + struct mesh_friend *old; + struct frnd_negotiation *neg = l_queue_remove_if( + frnd_negotiations, + match_by_lpn, + L_UINT_TO_PTR(frnd->dst)); + + /* Cancel any negotiations or clears */ + if (neg) { + l_timeout_remove(neg->timeout); + l_free(neg); + } + + /* Create Retired LPN list if needed */ + if (retired_lpns == NULL) + retired_lpns = l_queue_new(); + + /* Find any duplicates */ + old = l_queue_find(retired_lpns, match_by_dst, + L_UINT_TO_PTR(lpn)); + + /* Force time-out of old friendship */ + if (old) + friend_poll_timeout(old->timeout, old); + + /* Retire this LPN (keeps timeout running) */ + l_queue_push_tail(retired_lpns, frnd); + } + } else { + frnd = l_queue_find(retired_lpns, match_by_dst, + L_UINT_TO_PTR(lpn)); + if (!frnd) + return; + + lpnDelta = lpnCounter - frnd->lp_cnt; + + /* Ignore old Friend Clear commands */ + if (!lpnDelta || (lpnDelta > 0x100)) + return; + } + + l_info("Friend Cleared %4.4x (%4.4x)", lpn, lpnCounter); + + l_put_be16(lpn, msg + 1); + l_put_be16(lpnCounter, msg + 3); + mesh_net_transport_send(net, NULL, false, + mesh_net_get_iv_index(net), DEFAULT_TTL, + 0, 0, src, + msg, sizeof(msg)); +} + +static void clear_retry(struct l_timeout *timeout, void *user_data) +{ + struct frnd_negotiation *neg = user_data; + uint8_t msg[5] = { NET_OP_FRND_CLEAR }; + uint32_t secs = 1 << neg->receive_delay; + + + l_put_be16(neg->low_power_node, msg + 1); + l_put_be16(neg->lp_cnt, msg + 3); + mesh_net_transport_send(neg->net, NULL, false, + mesh_net_get_iv_index(neg->net), DEFAULT_TTL, + 0, 0, neg->old_relay, + msg, sizeof(msg)); + + if (secs && ((secs << 1) < neg->poll_timeout/10)) { + neg->receive_delay++; + l_info("Try FRND_CLR again in %d seconds (total timeout %d)", + secs, neg->poll_timeout/10); + l_timeout_modify(neg->timeout, secs); + } else { + l_info("FRND_CLR timed out %d", secs); + l_timeout_remove(timeout); + l_queue_remove(frnd_negotiations, neg); + l_free(neg); + } +} + +static void friend_delay_rsp(struct l_timeout *timeout, void *user_data) +{ + struct mesh_friend *frnd = user_data; + struct mesh_friend_msg *pkt = frnd->pkt; + struct mesh_net *net = frnd->net; + uint32_t net_seq, iv_index; + uint8_t upd[7] = { NET_OP_FRND_UPDATE }; + + l_timeout_remove(timeout); + + if (pkt == NULL) + goto update; + + if (pkt->ctl) { + /* Make sure we don't change the bit-sense of MD, + * once it has been set because that would cause + * a "Dirty Nonce" security violation + */ + if (((pkt->u.one[0].hdr >> OPCODE_HDR_SHIFT) & OPCODE_MASK) == + NET_OP_SEG_ACKNOWLEDGE) { + bool rly = !!((pkt->u.one[0].hdr >> RELAY_HDR_SHIFT) & + true); + uint16_t seqZero = pkt->u.one[0].hdr >> + SEQ_ZERO_HDR_SHIFT; + + seqZero &= SEQ_ZERO_MASK; + + l_info("Fwd ACK pkt %6.6x-%8.8x", + pkt->u.one[0].seq, + pkt->iv_index); + + pkt->u.one[0].sent = true; + mesh_net_ack_send(net, &frnd->key_set, + pkt->iv_index, pkt->ttl, + pkt->u.one[0].seq, pkt->src, pkt->dst, + rly, seqZero, + l_get_be32(pkt->u.one[0].data)); + + + } else { + l_info("Fwd CTL pkt %6.6x-%8.8x", + pkt->u.one[0].seq, + pkt->iv_index); + + print_packet("Frnd-CTL", + pkt->u.one[0].data, pkt->last_len); + + pkt->u.one[0].sent = true; + mesh_net_transport_send(net, &frnd->key_set, false, + pkt->iv_index, pkt->ttl, + pkt->u.one[0].seq, pkt->src, pkt->dst, + pkt->u.one[0].data, pkt->last_len); + } + } else { + /* If segments after this one, then More Data must be TRUE */ + uint8_t len; + + if (pkt->cnt_out < pkt->cnt_in) + len = sizeof(pkt->u.s12[0].data); + else + len = pkt->last_len; + + l_info("Fwd FRND pkt %6.6x", + pkt->u.s12[pkt->cnt_out].seq); + + print_packet("Frnd-Msg", pkt->u.s12[pkt->cnt_out].data, len); + + pkt->u.s12[pkt->cnt_out].sent = true; + mesh_net_send_seg(net, &frnd->key_set, + pkt->iv_index, + pkt->ttl, + pkt->u.s12[pkt->cnt_out].seq, + pkt->src, pkt->dst, + pkt->u.s12[pkt->cnt_out].hdr, + pkt->u.s12[pkt->cnt_out].data, len); + } + + return; + +update: + // No More Data -- send Update message with md = false + net_seq = mesh_net_get_seq_num(net); + l_info("Fwd FRND UPDATE %6.6x with MD == 0", net_seq); + + frnd->last = frnd->seq; + mesh_net_get_snb_state(net, upd + 1, &iv_index); + l_put_be32(iv_index, upd + 2); + upd[6] = false; // Queue is Empty + print_packet("Update", upd, sizeof(upd)); + mesh_net_transport_send(net, &frnd->key_set, false, + mesh_net_get_iv_index(net), 0, + net_seq, 0, frnd->dst, + upd, sizeof(upd)); + mesh_net_next_seq_num(net); +} + + +void friend_poll(struct mesh_net *net, uint16_t src, bool seq, + struct mesh_friend *frnd) +{ + struct frnd_negotiation *neg; + struct mesh_friend_msg *pkt; + bool md; + + neg = l_queue_find(frnd_negotiations, match_by_lpn, L_UINT_TO_PTR(src)); + if (neg && !neg->clearing) { + uint8_t msg[5] = { NET_OP_FRND_CLEAR }; + + l_info("Won negotiation for %4.4x", neg->low_power_node); + + /* This call will clean-up and replace if already friends */ + frnd = mesh_friend_new(net, src, neg->num_ele, + neg->receive_delay, + neg->wrfrw, + neg->poll_timeout, + neg->fn_cnt, neg->lp_cnt); + + frnd->timeout = l_timeout_create_ms( + frnd->poll_timeout * 100, + friend_poll_timeout, frnd, NULL); + + l_timeout_remove(neg->timeout); + mesh_net_remove_keyset(neg->net, &neg->key_set); + + if (neg->old_relay == 0 || + neg->old_relay == mesh_net_get_address(net)) { + l_queue_remove(frnd_negotiations, neg); + l_free(neg); + } else { + neg->clearing = true; + l_put_be16(neg->low_power_node, msg + 1); + l_put_be16(neg->lp_cnt, msg + 3); + mesh_net_transport_send(net, NULL, false, + mesh_net_get_iv_index(net), DEFAULT_TTL, + 0, 0, neg->old_relay, + msg, sizeof(msg)); + + /* Reuse receive_delay as a shift counter to + * time-out FRIEND_CLEAR + */ + neg->receive_delay = 1; + neg->timeout = l_timeout_create(1, clear_retry, + neg, NULL); + } + } + + if (!frnd) + return; + + /* Reset Poll Timeout */ + l_timeout_modify_ms(frnd->timeout, frnd->poll_timeout * 100); + + if (!l_queue_length(frnd->pkt_cache)) + goto update; + + if (frnd->seq != frnd->last && frnd->seq != seq) { + pkt = l_queue_peek_head(frnd->pkt_cache); + if (pkt->cnt_out < pkt->cnt_in) { + pkt->cnt_out++; + } else { + pkt = l_queue_pop_head(frnd->pkt_cache); + l_free(pkt); + } + } + + pkt = l_queue_peek_head(frnd->pkt_cache); + + if (!pkt) + goto update; + + frnd->seq = seq; + frnd->last = !seq; + md = !!(l_queue_length(frnd->pkt_cache) > 1); + + if (pkt->ctl) { + /* Make sure we don't change the bit-sense of MD, + * once it has been set because that would cause + * a "Dirty Nonce" security violation + */ + if (!(pkt->u.one[0].sent)) + pkt->u.one[0].md = md; + } else { + /* If segments after this one, then More Data must be TRUE */ + if (pkt->cnt_out < pkt->cnt_in) + md = true; + + /* Make sure we don't change the bit-sense of MD, once + * it has been set because that would cause a + * "Dirty Nonce" security violation + */ + if (!(pkt->u.s12[pkt->cnt_out].sent)) + pkt->u.s12[pkt->cnt_out].md = md; + } + frnd->pkt = pkt; + l_timeout_create_ms(frnd->frd, friend_delay_rsp, frnd, NULL); + + return; + +update: + frnd->pkt = NULL; + l_timeout_create_ms(frnd->frd, friend_delay_rsp, frnd, NULL); +} + +void friend_sub_add(struct mesh_net *net, struct mesh_friend *frnd, + const uint8_t *pkt, uint8_t len) +{ + uint16_t *new_list; + uint32_t net_seq; + uint8_t plen = len; + uint8_t msg[] = { NET_OP_PROXY_SUB_CONFIRM, 0 }; + + if (!frnd || MAX_FRND_GROUPS < frnd->grp_cnt + (len/2)) + return; + + msg[1] = *pkt++; + plen--; + + /* Sanity Check Values, abort if any illegal */ + while (plen >= 2) { + plen -= 2; + if (l_get_be16(pkt + plen) < 0x8000) + return; + } + + new_list = l_malloc(frnd->grp_cnt * sizeof(uint16_t) + len); + if (frnd->grp_list) + memcpy(new_list, frnd->grp_list, + frnd->grp_cnt * sizeof(uint16_t)); + + while (len >= 2) { + new_list[frnd->grp_cnt++] = l_get_be16(pkt); + pkt += 2; + len -= 2; + } + + l_free(frnd->grp_list); + frnd->grp_list = new_list; + + print_packet("Tx-NET_OP_PROXY_SUB_CONFIRM", msg, sizeof(msg)); + net_seq = mesh_net_get_seq_num(net); + mesh_net_transport_send(net, &frnd->key_set, false, + mesh_net_get_iv_index(net), 0, + net_seq, 0, frnd->dst, + msg, sizeof(msg)); + mesh_net_next_seq_num(net); +} + +void friend_sub_del(struct mesh_net *net, struct mesh_friend *frnd, + const uint8_t *pkt, uint8_t len) +{ + uint32_t net_seq; + uint8_t msg[] = { NET_OP_PROXY_SUB_CONFIRM, 0 }; + int i; + + if (!frnd) + return; + + msg[1] = *pkt++; + len--; + + while (len >= 2) { + uint16_t grp = l_get_be16(pkt); + + for (i = frnd->grp_cnt - 1; i >= 0; i--) { + if (frnd->grp_list[i] == grp) { + frnd->grp_cnt--; + memcpy(&frnd->grp_list[i], + &frnd->grp_list[i + 1], + (frnd->grp_cnt - i) * 2); + break; + } + } + len -= 2; + pkt += 2; + } + + print_packet("Tx-NET_OP_PROXY_SUB_CONFIRM", msg, sizeof(msg)); + net_seq = mesh_net_get_seq_num(net); + mesh_net_transport_send(net, &frnd->key_set, false, + mesh_net_get_iv_index(net), 0, + net_seq, 0, frnd->dst, + msg, sizeof(msg)); + mesh_net_next_seq_num(net); +} + +/* Low-Power-Node role */ +struct frnd_offers { + uint16_t fn_cnt; + uint16_t src; + uint8_t window; + uint8_t cache; + uint8_t sub_list_size; + int8_t local_rssi; + int8_t remote_rssi; +}; + +#define MAX_POLL_RETRIES 5 +static bool quick_pick; +static uint8_t poll_cnt; +static struct l_queue *offers; +static uint16_t old_friend; +static uint16_t fn_cnt, cnt = 0xffff; +static uint32_t poll_period_ms; +static struct l_timeout *poll_retry_to; +static struct l_timeout *poll_period_to; +static struct mesh_key_set lpn_set; +static struct mesh_key_set new_lpn_set; + +void frnd_offer(struct mesh_net *net, uint16_t src, uint8_t window, + uint8_t cache, uint8_t sub_list_size, + int8_t r_rssi, int8_t l_rssi, uint16_t fn_cnt) +{ + struct frnd_offers *offer; + + l_info("RSSI of Offer: %d dbm", l_rssi); + + /* Ignore RFU window value 0 */ + if (window == 0) + return; + + if (mesh_net_get_friend(net)) + return; + + if (quick_pick) { + if (mesh_net_set_friend(net, src)) { + old_friend = src; + frnd_poll(net, false); + } + return; + } + + offer = l_new(struct frnd_offers, 1); + offer->src = src; + offer->window = window; + offer->cache = cache; + offer->sub_list_size = sub_list_size; + offer->local_rssi = l_rssi; + offer->remote_rssi = r_rssi; + offer->fn_cnt = fn_cnt; + + l_queue_push_tail(offers, offer); +} + +static void frnd_poll_timeout(struct l_timeout *timeout, void *user_data) +{ + struct mesh_net *net = user_data; + + frnd_poll(net, true); +} + +static void frnd_negotiated_to(struct l_timeout *timeout, void *user_data) +{ + struct mesh_net *net = user_data; + + l_info("frnd_negotiated_to"); + if (!mesh_net_get_friend(net)) { + l_timeout_remove(poll_period_to); + poll_period_to = NULL; + return; + } + + if (!poll_retry_to) + frnd_poll(net, false); +} + +void frnd_poll_cancel(struct mesh_net *net) +{ + l_timeout_remove(poll_retry_to); + poll_retry_to = NULL; +} + +void frnd_poll(struct mesh_net *net, bool retry) +{ + struct mesh_key_set *key_set = &lpn_set; + uint32_t net_seq; + uint8_t msg[2] = { NET_OP_FRND_POLL }; + bool seq = mesh_net_get_frnd_seq(net); + + /* Check if we are in Phase 2 of Key Refresh */ + if (new_lpn_set.nid != 0xff) { + uint8_t phase; + uint16_t net_idx = mesh_net_get_primary_idx(net); + uint8_t status = + mesh_net_key_refresh_phase_get(net, net_idx, &phase); + + if (status == MESH_STATUS_SUCCESS && + phase == KEY_REFRESH_PHASE_TWO) + key_set = &new_lpn_set; + } + + if (!retry) { + poll_cnt = MAX_POLL_RETRIES; + seq = !seq; + mesh_net_set_frnd_seq(net, seq); + } else if (!(poll_cnt--)) { + l_info("Lost Friendship with %4.4x", old_friend); + l_timeout_remove(poll_period_to); + poll_period_to = NULL; + frnd_poll_cancel(net); + mesh_net_remove_keyset(net, &lpn_set); + mesh_net_remove_keyset(net, &new_lpn_set); + mesh_net_set_friend(net, 0); + return; + } + + if (poll_retry_to) + l_timeout_remove(poll_retry_to); + + l_info("TX-FRIEND POLL %d", seq); + msg[1] = seq; + net_seq = mesh_net_get_seq_num(net); + mesh_net_transport_send(net, key_set, true, + mesh_net_get_iv_index(net), 0, + net_seq, 0, mesh_net_get_friend(net), + msg, sizeof(msg)); + mesh_net_next_seq_num(net); + poll_retry_to = l_timeout_create_ms(1000, frnd_poll_timeout, net, NULL); + + /* Reset Poll Period for next "Wake Up" */ + if (poll_period_to) + l_timeout_modify_ms(poll_period_to, poll_period_ms); + else + poll_period_to = l_timeout_create_ms(poll_period_ms, + frnd_negotiated_to, net, NULL); +} + +void frnd_ack_poll(struct mesh_net *net) +{ + /* Start new POLL, but only if not already Polling */ + if (poll_retry_to == NULL) + frnd_poll(net, false); +} + +static void req_timeout(struct l_timeout *timeout, void *user_data) +{ + struct mesh_net *net = user_data; + struct frnd_offers *best; + struct frnd_offers *offer = l_queue_pop_head(offers); + uint8_t p[9] = { 1 }; + uint8_t key[16]; + bool res; + + l_timeout_remove(timeout); + + best = offer; + while (offer) { + /* Screen out clearly inferior RSSI friends first */ + if (offer->local_rssi < -40 && offer->remote_rssi < -40) { + if (best->local_rssi + 20 < offer->local_rssi || + best->remote_rssi + 20 < offer->remote_rssi) { + + l_free(best); + best = offer; + offer = l_queue_pop_head(offers); + continue; + } + } + + /* Otherwise use best Windows, with Cache size as tie breaker */ + if (best->window > offer->window || + (best->window == offer->window && + best->cache < offer->cache)) { + l_free(best); + best = offer; + } else if (best != offer) + l_free(offer); + + offer = l_queue_pop_head(offers); + } + + mesh_net_remove_keyset(net, &lpn_set); + mesh_net_remove_keyset(net, &new_lpn_set); + if (mesh_net_get_friend(net)) { + l_free(best); + return; + } else if (!best) { + l_info("No Offers Received"); + return; + } + + fn_cnt = best->fn_cnt; + l_put_be16(mesh_net_get_address(net), p + 1); + l_put_be16(best->src, p + 3); + l_put_be16(cnt, p + 5); + l_put_be16(best->fn_cnt, p + 7); + print_packet("Friend Key P =", p, 9); + res = mesh_net_get_key(net, false, mesh_net_get_primary_idx(net), key); + if (!res) + return; + + res = mesh_crypto_k2(key, p, sizeof(p), &lpn_set.nid, + lpn_set.enc_key, lpn_set.privacy_key); + if (!res) + return; + + print_packet("Cur-NID", &lpn_set.nid, 1); + print_packet("Cur-ENC_KEY", lpn_set.enc_key, 16); + print_packet("Cur-PRIV_KEY", lpn_set.privacy_key, 16); + + mesh_net_add_keyset(net, &lpn_set); + + res = mesh_net_get_key(net, true, mesh_net_get_primary_idx(net), key); + + if (res) + res = mesh_crypto_k2(key, p, sizeof(p), &new_lpn_set.nid, + new_lpn_set.enc_key, new_lpn_set.privacy_key); + if (!res) { + new_lpn_set.nid = 0xff; + goto old_keys_only; + } + + print_packet("New-NID", &new_lpn_set.nid, 1); + print_packet("New-ENC_KEY", new_lpn_set.enc_key, 16); + print_packet("New-PRIV_KEY", new_lpn_set.privacy_key, 16); + + mesh_net_add_keyset(net, &new_lpn_set); + +old_keys_only: + + l_info("Winning offer %4.4x RSSI: %ddb Window: %dms Cache sz: %d", + best->src, best->local_rssi, + best->window, best->cache); + + if (mesh_net_set_friend(net, best->src)) { + old_friend = best->src; + mesh_net_set_frnd_seq(net, true); + frnd_poll(net, false); + } + + l_free(best); +} + +void frnd_clear(struct mesh_net *net) +{ + uint8_t msg[12]; + uint8_t n = 0; + uint16_t frnd_addr = mesh_net_get_friend(net); + uint16_t my_addr = mesh_net_get_address(net); + + msg[n++] = NET_OP_FRND_CLEAR; + l_put_be16(my_addr, msg + n); + n += 2; + l_put_be16(cnt, msg + n); + n += 2; + + mesh_net_remove_keyset(net, &lpn_set); + mesh_net_remove_keyset(net, &new_lpn_set); + mesh_net_set_friend(net, 0); + + mesh_net_transport_send(net, NULL, false, + mesh_net_get_iv_index(net), 0, + 0, 0, frnd_addr, + msg, n); +} + +void frnd_request_friend(struct mesh_net *net, uint8_t cache, + uint8_t offer_delay, uint8_t delay, uint32_t timeout) +{ + uint8_t msg[12]; + uint8_t n = 0; + + if (offers == NULL) + offers = l_queue_new(); + + msg[n++] = NET_OP_FRND_REQUEST; + msg[n] = cache & 0x07; // MinRequirements - Cache + msg[n++] |= (offer_delay & 0x0f) << 3; // Offer Delay + poll_period_ms = (timeout * 300) / 4; // 3/4 of the time in ms + l_put_be32(timeout, msg + n); // PollTimeout + msg[n++] = delay; // ReceiveDelay + n += 3; + l_put_be16(old_friend, msg + n); // PreviousAddress + n += 2; + msg[n++] = mesh_net_get_num_ele(net); // NumElements + l_put_be16(cnt + 1, msg + n); // Next counter + n += 2; + print_packet("Tx-NET_OP_FRND_REQUEST", msg, n); + mesh_net_transport_send(net, NULL, false, + mesh_net_get_iv_index(net), 0, + 0, 0, FRIENDS_ADDRESS, + msg, n); + l_timeout_create_ms(1000, req_timeout, net, NULL); // 1000 ms + mesh_net_set_friend(net, 0); + cnt++; +} + +static uint8_t trans_id; +void frnd_sub_add(struct mesh_net *net, uint32_t parms[7]) +{ + struct mesh_key_set *key_set = &lpn_set; + uint32_t net_seq; + uint8_t msg[15] = { NET_OP_PROXY_SUB_ADD }; + uint8_t i, n = 1; + + /* Check if we are in Phase 2 of Key Refresh */ + if (new_lpn_set.nid != 0xff) { + uint8_t phase; + uint16_t net_idx = mesh_net_get_primary_idx(net); + uint8_t status = mesh_net_key_refresh_phase_get(net, + net_idx, &phase); + + if (status == MESH_STATUS_SUCCESS && + phase == KEY_REFRESH_PHASE_TWO) + key_set = &new_lpn_set; + } + + msg[n++] = ++trans_id; + for (i = 0; i < 7; i++) { + if (parms[i] < 0x8000 || parms[i] > 0xffff) + break; + + l_put_be16(parms[i], msg + n); + n += 2; + } + + net_seq = mesh_net_get_seq_num(net); + print_packet("Friend Sub Add", msg, n); + mesh_net_transport_send(net, key_set, false, + mesh_net_get_iv_index(net), 0, + net_seq, 0, mesh_net_get_friend(net), + msg, n); + mesh_net_next_seq_num(net); +} + +void frnd_sub_del(struct mesh_net *net, uint32_t parms[7]) +{ + struct mesh_key_set *key_set = &lpn_set; + uint32_t net_seq; + uint8_t msg[15] = { NET_OP_PROXY_SUB_REMOVE }; + uint8_t i, n = 1; + + /* Check if we are in Phase 2 of Key Refresh */ + if (new_lpn_set.nid != 0xff) { + uint8_t phase; + uint16_t net_idx = mesh_net_get_primary_idx(net); + uint8_t status = mesh_net_key_refresh_phase_get(net, + net_idx, &phase); + + if (status == MESH_STATUS_SUCCESS && + phase == KEY_REFRESH_PHASE_TWO) + key_set = &new_lpn_set; + } + + msg[n++] = ++trans_id; + for (i = 0; i < 7; i++) { + if (parms[i] < 0x8000 || parms[i] > 0xffff) + break; + + l_put_be16(parms[i], msg + n); + n += 2; + } + + net_seq = mesh_net_get_seq_num(net); + print_packet("Friend Sub Del", msg, n); + mesh_net_transport_send(net, key_set, false, + mesh_net_get_iv_index(net), 0, + net_seq, 0, mesh_net_get_friend(net), + msg, n); + mesh_net_next_seq_num(net); +} + +void frnd_key_refresh(struct mesh_net *net, uint8_t phase) +{ + uint16_t net_idx = mesh_net_get_primary_idx(net); + uint8_t p[9] = { 1 }; + uint8_t key[16]; + + switch (phase) { + default: + case 0: + case 3: + if (new_lpn_set.nid != 0xff) { + l_info("LPN Retiring KeySet %2.2x", lpn_set.nid); + lpn_set = new_lpn_set; + new_lpn_set.nid = 0xff; + mesh_net_remove_keyset(net, &new_lpn_set); + } + return; + + case 1: + mesh_net_remove_keyset(net, &new_lpn_set); + if (!mesh_net_get_key(net, true, net_idx, key)) { + new_lpn_set.nid = 0xff; + return; + } + + l_put_be16(mesh_net_get_address(net), p + 1); + l_put_be16(mesh_net_get_friend(net), p + 3); + l_put_be16(cnt, p + 5); + l_put_be16(fn_cnt, p + 7); + print_packet("Friend Key P =", p, 9); + + if (!mesh_crypto_k2(key, p, sizeof(p), &new_lpn_set.nid, + new_lpn_set.enc_key, + new_lpn_set.privacy_key)) { + new_lpn_set.nid = 0xff; + return; + } + + print_packet("New-NID", &new_lpn_set.nid, 1); + print_packet("New-ENC_KEY", new_lpn_set.enc_key, 16); + print_packet("New-PRIV_KEY", new_lpn_set.privacy_key, 16); + + mesh_net_add_keyset(net, &new_lpn_set); + return; + + case 2: + /* Should we do anything here? Maybe not */ + return; + } +} + +struct mesh_key_set *frnd_get_key(struct mesh_net *net) +{ + uint8_t idx = mesh_net_get_primary_idx(net); + uint8_t phase = 0; + + mesh_net_key_refresh_phase_get(net, idx, &phase); + + if (phase == 2) + return &new_lpn_set; + else + return &lpn_set; +} -- 2.14.3