---
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 <config.h>
+#endif
+
+#include <unistd.h>
+#include <stdio.h>
+#include <sys/time.h>
+#include <ell/ell.h>
+
+#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
--
To unsubscribe from this list: send the line "unsubscribe linux-bluetooth" in
the body of a message to majordomo@xxxxxxxxxxxxxxx
More majordomo info at http://vger.kernel.org/majordomo-info.html
