[PATCH BlueZ v5 08/30] mesh: Direction agnostic PB-Adv implementation

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

---
 mesh/pb-adv.c | 444 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 mesh/pb-adv.h |  23 +++
 2 files changed, 467 insertions(+)
 create mode 100644 mesh/pb-adv.c
 create mode 100644 mesh/pb-adv.h

diff --git a/mesh/pb-adv.c b/mesh/pb-adv.c
new file mode 100644
index 000000000..57647e184
--- /dev/null
+++ b/mesh/pb-adv.c
@@ -0,0 +1,444 @@
+/*
+ *
+ *  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 <sys/select.h>
+#include <sys/time.h>
+#include <sys/types.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <termios.h>
+
+#include <ctype.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <getopt.h>
+#include <time.h>
+#include <ell/ell.h>
+
+#include "mesh/mesh-defs.h"
+#include "src/shared/ecc.h"
+
+#include "mesh/util.h"
+#include "mesh/net_keys.h"
+#include "mesh/crypto.h"
+#include "mesh/net.h"
+#include "mesh/mesh-io.h"
+#include "mesh/mesh.h"
+#include "mesh/prov.h"
+#include "mesh/provision.h"
+#include "mesh/pb-adv.h"
+
+
+struct pb_adv_session {
+	mesh_prov_open_func_t open_cb;
+	mesh_prov_close_func_t close_cb;
+	mesh_prov_receive_func_t rx_cb;
+	mesh_prov_ack_func_t ack_cb;
+	struct l_timeout *tx_timeout;
+	uint32_t conn_id;
+	uint16_t exp_len;
+	uint8_t exp_fcs;
+	uint8_t exp_segs;
+	uint8_t got_segs;
+	uint8_t msg_num;
+	uint8_t local_acked;
+	uint8_t local_msg_num;
+	uint8_t peer_msg_num;
+	uint8_t last_peer_msg_num;
+	uint8_t sar[80];
+	uint8_t uuid[16];
+	bool initiator;
+	bool opened;
+	void *user_data;
+};
+
+#define PB_ADV_ACK 0x01
+#define PB_ADV_OPEN_REQ 0x03
+#define PB_ADV_OPEN_CFM 0x07
+#define PB_ADV_CLOSE 0x0B
+
+#define PB_ADV_MTU	24
+
+static struct pb_adv_session *pb_session = NULL;
+
+static void send_adv_segs(struct pb_adv_session *session, const uint8_t *data,
+							uint16_t size)
+{
+	uint16_t init_size;
+	uint8_t buf[PB_ADV_MTU + 6] = { MESH_AD_TYPE_PROVISION };
+	uint8_t max_seg;
+	uint8_t consumed;
+	int i;
+
+	if (!size)
+		return;
+
+	mesh_send_cancel(buf, 1);
+
+	l_put_be32(session->conn_id, buf + 1);
+	buf[1 + 4] = ++session->local_msg_num;
+
+	if (size > PB_ADV_MTU - 4) {
+		max_seg = 1 +
+			(((size - (PB_ADV_MTU - 4)) - 1) / (PB_ADV_MTU - 1));
+		init_size = PB_ADV_MTU - 4;
+	} else {
+		max_seg = 0;
+		init_size = size;
+	}
+
+	/* print_packet("FULL-TX", data, size); */
+
+	l_debug("Sending %u fragments for %u octets", max_seg + 1, size);
+
+	buf[6] = max_seg << 2;
+	l_put_be16(size, buf + 7);
+	buf[9] = mesh_crypto_compute_fcs(data, size);
+	memcpy(buf + 10, data, init_size);
+
+	l_debug("max_seg: %2.2x", max_seg);
+	l_debug("size: %2.2x, CRC: %2.2x", size, buf[9]);
+	/* print_packet("PB-TX", buf + 1, init_size + 9); */
+	mesh_send_pkt(MESH_IO_TX_COUNT_UNLIMITED, 50, buf, init_size + 10);
+
+	consumed = init_size;
+
+	for (i = 1; i <= max_seg; i++) {
+		uint8_t seg_size; /* Amount of payload data being sent */
+
+		if (size - consumed > PB_ADV_MTU - 1)
+			seg_size = PB_ADV_MTU - 1;
+		else
+			seg_size = size - consumed;
+
+		buf[6] = (i << 2) | 0x02;
+		memcpy(buf + 7, data + consumed, seg_size);
+
+		/* print_packet("PB-TX", buf + 1, seg_size + 6); */
+
+		mesh_send_pkt(MESH_IO_TX_COUNT_UNLIMITED, 50,
+							buf, seg_size + 7);
+
+		consumed += seg_size;
+	}
+}
+
+static void tx_timeout(struct l_timeout *timeout, void *user_data)
+{
+	struct pb_adv_session *session = user_data;
+	uint8_t cancel[] = { MESH_AD_TYPE_PROVISION };
+	mesh_prov_close_func_t cb;
+
+	if (!session || pb_session != session)
+		return;
+
+	l_timeout_remove(session->tx_timeout);
+	session->tx_timeout = NULL;
+
+	mesh_send_cancel(cancel, sizeof(cancel));
+
+	l_info("TX timeout");
+	cb = pb_session->close_cb;
+	user_data = pb_session->user_data;
+	l_free(pb_session);
+	pb_session = NULL;
+	cb(user_data, 1);
+}
+
+static void pb_adv_tx(void *user_data, uint8_t *data, uint16_t len)
+{
+	struct pb_adv_session *session = user_data;
+
+	if (!session || pb_session != session)
+		return;
+
+	l_timeout_remove(session->tx_timeout);
+	session->tx_timeout = l_timeout_create(30, tx_timeout, session, NULL);
+
+	send_adv_segs(session, data, len);
+}
+
+static void send_open_cfm(struct pb_adv_session *session)
+{
+	uint8_t open_cfm[7] = { MESH_AD_TYPE_PROVISION };
+
+	l_put_be32(session->conn_id, open_cfm + 1);
+	open_cfm[1 + 4] = 0;
+	open_cfm[1 + 4 + 1] = 0x07; /* OPEN_CFM */
+
+	/* print_packet("PB-TX", open_cfm + 1, sizeof(open_cfm) - 1); */
+
+	mesh_send_cancel(open_cfm, 1);
+	mesh_send_pkt(5, 100, open_cfm, sizeof(open_cfm));
+}
+
+static void send_ack(struct pb_adv_session *session, uint8_t msg_num)
+{
+	uint8_t ack[7] = { MESH_AD_TYPE_PROVISION };
+
+	l_put_be32(session->conn_id, ack + 1);
+	ack[1 + 4] = msg_num;
+	ack[1 + 4 + 1] = 0x01; /* ACK */
+
+	/* print_packet("ADV-ACK", ack + 1, sizeof(ack) - 1); */
+	mesh_send_pkt(1, 100, ack, sizeof(ack));
+}
+
+static void send_close_ind(struct pb_adv_session *session, uint8_t reason)
+{
+	uint8_t close_ind[8] = { MESH_AD_TYPE_PROVISION };
+
+	if (!pb_session || pb_session->user_data != session)
+		return;
+
+	l_put_be32(session->conn_id, close_ind + 1);
+	close_ind[5] = 0;
+	close_ind[6] = PB_ADV_CLOSE;		/* CLOSE_IND */
+	close_ind[7] = reason;
+
+	mesh_send_cancel(close_ind, 1);
+	mesh_send_pkt(5, 100, close_ind, sizeof(close_ind));
+}
+
+static void pb_adv_packet(void *user_data, const uint8_t *pkt, uint16_t len)
+{
+	struct pb_adv_session *session = user_data;
+	uint32_t conn_id;
+	size_t offset;
+	uint8_t msg_num;
+	uint8_t type;
+	bool first;
+
+	if (!session || pb_session != session)
+		return;
+
+	conn_id = l_get_be32(pkt + 1);
+	type = l_get_u8(pkt + 6);
+
+	/* Validate new or existing Connection ID */
+	if (session->conn_id) {
+		if (session->conn_id != conn_id)
+			return;
+	} else if (type != 0x03)
+		return;
+	else if (!conn_id)
+		return;
+
+	msg_num = l_get_u8(pkt + 5);
+	pkt += 7;
+	len -= 7;
+
+	switch (type) {
+	case PB_ADV_OPEN_CFM:
+		/*
+		 * Ignore if:
+		 * 1. We are acceptor
+		 * 2. We are already provisioning on different conn_id
+		 */
+
+		if (!session->initiator)
+			return;
+
+		first = !session->opened;
+		session->opened = true;
+
+		/* Only call Open callback once */
+		if (first) {
+			l_debug("PB-ADV open confirmed");
+			session->open_cb(session->user_data, pb_adv_tx,
+							session, PB_ADV);
+		}
+		return;
+
+	case PB_ADV_OPEN_REQ:
+		/*
+		 * Ignore if:
+		 * 1. We are initiator
+		 * 2. Open request not addressed to us
+		 * 3. We are already provisioning on different conn_id
+		 */
+
+		if (session->initiator)
+			return;
+
+		if (memcmp(pkt, session->uuid, 16))
+			return;
+
+		first = !session->conn_id;
+		session->conn_id = conn_id;
+		session->last_peer_msg_num = 0xFF;
+		session->local_acked = 0xFF;
+		session->peer_msg_num = 0x00;
+		session->local_msg_num = 0x7F;
+		session->opened = true;
+
+		/* Only call Open callback once */
+		if (first) {
+			l_debug("PB-ADV open requested");
+			session->open_cb(session->user_data, pb_adv_tx,
+							session, PB_ADV);
+		}
+
+		/* Send CFM once per received request */
+		send_open_cfm(session);
+		break;
+
+	case PB_ADV_CLOSE:
+		l_timeout_remove(session->tx_timeout);
+		l_debug("Link closed notification: %2.2x", pkt[0]);
+		/* Wrap callback for pre-cleaning */
+		if (true) {
+			mesh_prov_close_func_t cb = session->close_cb;
+			void *user_data = session->user_data;
+
+			l_free(session);
+			pb_session = NULL;
+			cb(user_data, pkt[0]);
+		}
+		break;
+
+	case PB_ADV_ACK:
+		if (!session->opened)
+			return;
+
+		if (msg_num != session->local_msg_num)
+			return;
+
+		if (session->local_acked > msg_num)
+			return;
+
+		l_debug("Got ACK %d", msg_num);
+		session->local_acked = msg_num;
+		session->ack_cb(session->user_data, msg_num);
+		break;
+
+	default: /* DATA SEGMENT */
+		if (!session->opened)
+			return;
+
+		if (msg_num == session->last_peer_msg_num) {
+			send_ack(session, msg_num);
+			return;
+		}
+
+		switch(type & 0x03) {
+		case 0x00:
+			session->peer_msg_num = msg_num;
+			session->exp_len = l_get_be16(pkt);
+
+			l_debug("PB-ADV start with %u fragments, %d octets",
+						type >> 2, session->exp_len);
+
+			if (session->exp_len > sizeof(session->sar)) {
+				l_debug("Incoming length exceeded: %d",
+							session->exp_len);
+				return;
+			}
+
+			session->exp_fcs = l_get_u8(pkt + 2);
+			session->exp_segs = 0xff >> (7 - (type >> 2));
+
+			/* Save first segment */
+			memcpy(session->sar, pkt + 3, len - 3);
+			session->got_segs |= 1;
+			break;
+
+		case 0x02:
+			session->peer_msg_num = msg_num;
+			offset = 20 + (((type >> 2) - 1) * 23);
+
+			if (offset + len - 3 > sizeof(session->sar)) {
+				l_debug("Length exceeded: %d",
+							session->exp_len);
+				return;
+			}
+
+			l_debug("Processing fragment %u", type >> 2);
+			memcpy(session->sar + offset, pkt, len);
+			session->got_segs |= 1 << (type >> 2);
+			break;
+
+		default:
+			/* Malformed or unrecognized */
+			return;
+		}
+
+		if (session->got_segs != session->exp_segs)
+			return;
+
+		/* Validate RXed packet and pass up to Provisioning */
+		if (!mesh_crypto_check_fcs(session->sar,
+					session->exp_len,
+					session->exp_fcs)) {
+
+			/* This can be a false negative if first
+			 * segment missed, and can almost always
+			 * be ignored.
+			 */
+
+			l_debug("Invalid FCS");
+			return;
+		}
+
+		if (session->last_peer_msg_num != session->peer_msg_num) {
+			session->got_segs = 0;
+			session->rx_cb(session->user_data, session->sar,
+							session->exp_len);
+		}
+
+		session->last_peer_msg_num = session->peer_msg_num;
+		send_ack(session, session->last_peer_msg_num);
+	}
+}
+
+bool pb_adv_reg(mesh_prov_open_func_t open_cb, mesh_prov_close_func_t close_cb,
+		mesh_prov_receive_func_t rx_cb, mesh_prov_ack_func_t ack_cb,
+		uint8_t uuid[16], void *user_data)
+{
+	if (pb_session)
+		return false;
+
+	pb_session = l_new(struct pb_adv_session, 1);
+	pb_session->open_cb = open_cb;
+	pb_session->close_cb = close_cb;
+	pb_session->rx_cb = rx_cb;
+	pb_session->ack_cb = ack_cb;
+	pb_session->user_data = user_data;
+	memcpy(pb_session->uuid, uuid, 16);
+
+	/* TODO: register PB AD type and Start Beaconing ? */
+	mesh_reg_prov_rx(pb_adv_packet, pb_session);
+
+	return true;
+}
+
+void pb_adv_unreg(void *user_data)
+{
+	if (!pb_session || pb_session->user_data != user_data)
+		return;
+
+	l_timeout_remove(pb_session->tx_timeout);
+	send_close_ind(pb_session, 0);
+	l_free(pb_session);
+	pb_session = NULL;
+}
diff --git a/mesh/pb-adv.h b/mesh/pb-adv.h
new file mode 100644
index 000000000..a5870d5a2
--- /dev/null
+++ b/mesh/pb-adv.h
@@ -0,0 +1,23 @@
+/*
+ *
+ *  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.
+ *
+ */
+
+bool pb_adv_reg(mesh_prov_open_func_t open_cb, mesh_prov_close_func_t close_cb,
+		mesh_prov_receive_func_t rx_cb, mesh_prov_ack_func_t ack_cb,
+		uint8_t uuid[16], void *user_data);
+void pb_adv_unreg(void *user_data);
-- 
2.14.5