bluez/mesh/friend.c

/*
 *
 *  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;
}