[PATCH BlueZ v7 04/14] mesh: Initial Mesh Friendship support

[Brian Gix <brian.gix@intel.com>]

---
 mesh/friend.c | 1116 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 1116 insertions(+)
 create mode 100644 mesh/friend.c

diff --git a/mesh/friend.c b/mesh/friend.c
new file mode 100644
index 000000000..5a4088ef1
--- /dev/null
+++ b/mesh/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 <config.h>
+#endif
+
+#include <unistd.h>
+#include <stdio.h>
+#include <sys/time.h>
+#include <ell/ell.h>
+
+#include "mesh/mesh-defs.h"
+
+#include "mesh/mesh.h"
+#include "mesh/node.h"
+#include "mesh/net.h"
+#include "mesh/crypto.h"
+#include "mesh/model.h"
+#include "mesh/display.h"
+
+#include "mesh/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.4