[PATCH BlueZ v3 05/14] meshd: Provisioning logic for mesh

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

---
 meshd/src/prov.c      |  722 ++++++++++++++++++++++++++++++
 meshd/src/provision.c | 1159 +++++++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 1881 insertions(+)
 create mode 100644 meshd/src/prov.c
 create mode 100644 meshd/src/provision.c

diff --git a/meshd/src/prov.c b/meshd/src/prov.c
new file mode 100644
index 000000000..d96f9f28b
--- /dev/null
+++ b/meshd/src/prov.c
@@ -0,0 +1,722 @@
+/*
+ *
+ *  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/time.h>
+#include <ell/ell.h>
+
+#include "meshd/common/mesh-defs.h"
+
+#include "meshd/src/mesh-io.h"
+#include "meshd/src/display.h"
+#include "meshd/src/crypto.h"
+#include "meshd/src/net.h"
+#include "meshd/src/prov.h"
+
+#define PB_ADV_MTU	24
+
+#define DEFAULT_CONN_ID	0x00000000
+#define DEFAULT_PROV_MSG_NUM	0x00
+#define DEFAULT_DEV_MSG_NUM	0x80
+
+#define TX_TIMEOUT	30
+
+struct mesh_prov *mesh_prov_new(struct mesh_net *net, uint16_t remote)
+{
+	struct mesh_prov *prov;
+
+	prov = l_new(struct mesh_prov, 1);
+
+	prov->remote = remote;
+	prov->net = net;
+
+	return mesh_prov_ref(prov);
+}
+
+struct mesh_prov *mesh_prov_ref(struct mesh_prov *prov)
+{
+	if (!prov)
+		return NULL;
+
+	__sync_fetch_and_add(&prov->ref_count, 1);
+
+	return prov;
+}
+
+void mesh_prov_unref(struct mesh_prov *prov)
+{
+	struct mesh_io *io;
+	uint8_t type;
+
+	if (!prov)
+		return;
+
+	if (__sync_sub_and_fetch(&prov->ref_count, 1))
+		return;
+
+	io = mesh_net_get_io(prov->net);
+	type = MESH_AD_TYPE_BEACON;
+	mesh_io_send_cancel(io, &type, 1);
+	type = MESH_AD_TYPE_PROVISION;
+	mesh_io_send_cancel(io, &type, 1);
+	mesh_io_deregister_recv_cb(io, MESH_IO_FILTER_PROV);
+	mesh_io_deregister_recv_cb(io, MESH_IO_FILTER_BEACON);
+	l_timeout_remove(prov->tx_timeout);
+
+
+	l_info("Freed Prov Data");
+	l_free(prov);
+}
+
+static void packet_received(struct mesh_prov *prov, const void *data,
+						uint16_t size, uint8_t fcs)
+{
+	if (prov->receive_callback)
+		prov->receive_callback(data, size, prov);
+}
+
+static void send_open_req(struct mesh_prov *prov)
+{
+	struct mesh_io *io;
+	uint8_t open_req[23] = { MESH_AD_TYPE_PROVISION };
+	struct mesh_io_send_info info = {
+		.type = MESH_IO_TIMING_TYPE_GENERAL,
+		.u.gen.interval = 50,
+		.u.gen.cnt = 1,
+		.u.gen.min_delay = 0,
+		.u.gen.max_delay = 0,
+	};
+
+	if (!prov)
+		return;
+
+	io = mesh_net_get_io(prov->net);
+	if (!io)
+		return;
+
+	l_put_be32(prov->conn_id, open_req + 1);
+	open_req[1 + 4] = prov->local_msg_num = 0;
+	open_req[1 + 4 + 1] = 0x03; /* OPEN_REQ */
+	memcpy(open_req + 1 + 4 + 1 + 1, prov->uuid, 16);
+
+	/* print_packet("PB-TX", open_req + 1, sizeof(open_req) - 1); */
+	mesh_io_send_cancel(io, open_req, 1);
+	mesh_io_send(io, &info, open_req, sizeof(open_req));
+}
+
+static void send_open_cfm(struct mesh_prov *prov)
+{
+	struct mesh_io *io;
+	uint8_t open_cfm[7] = { MESH_AD_TYPE_PROVISION };
+	struct mesh_io_send_info info = {
+		.type = MESH_IO_TIMING_TYPE_GENERAL,
+		.u.gen.interval = 50,
+		.u.gen.cnt = 1,
+		.u.gen.min_delay = 0,
+		.u.gen.max_delay = 0,
+	};
+
+	if (!prov)
+		return;
+
+	io = mesh_net_get_io(prov->net);
+	if (!io)
+		return;
+
+	l_put_be32(prov->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_io_send_cancel(io, open_cfm, 1);
+	mesh_io_send(io, &info, open_cfm, sizeof(open_cfm));
+}
+
+static void send_close_ind(struct mesh_prov *prov, uint8_t reason)
+{
+	uint8_t buf[8] = { MESH_AD_TYPE_PROVISION };
+	struct mesh_io *io;
+	struct mesh_io_send_info info = {
+		.type = MESH_IO_TIMING_TYPE_GENERAL,
+		.u.gen.interval = 50,
+		.u.gen.cnt = 3,
+		.u.gen.min_delay = 0,
+		.u.gen.max_delay = 0,
+	};
+
+	if (!prov)
+		return;
+
+	if (prov->bearer == MESH_BEARER_ADV) {
+		io = mesh_net_get_io(prov->net);
+		if (!io)
+			return;
+
+		l_put_be32(prov->conn_id, buf + 1);
+		buf[5] = 0;
+		buf[6] = 0x0B; /* CLOSE_IND */
+		buf[7] = reason;
+
+		/* print_packet("PB-TX", buf + 1, sizeof(buf) - 1); */
+
+		mesh_io_send_cancel(io, buf, 1);
+		mesh_io_send(io, &info, buf, sizeof(buf));
+	}
+
+	prov->bearer = MESH_BEARER_IDLE;
+}
+
+static void tx_timeout(struct l_timeout *timeout, void *user_data)
+{
+	struct mesh_prov *prov = user_data;
+	uint8_t cancel[] = { MESH_AD_TYPE_PROVISION };
+	struct mesh_io *io;
+
+	if (!prov)
+		return;
+
+	l_timeout_remove(prov->tx_timeout);
+	prov->tx_timeout = NULL;
+
+	io = mesh_net_get_io(prov->net);
+	if (!io)
+		return;
+
+	mesh_io_send_cancel(io, cancel, sizeof(cancel));
+
+	l_info("TX timeout");
+	mesh_prov_close(prov, 1);
+}
+
+static void send_adv_segs(struct mesh_prov *prov)
+{
+	struct mesh_io *io = mesh_net_get_io(prov->net);
+	struct mesh_io_send_info info = {
+		.type = MESH_IO_TIMING_TYPE_GENERAL,
+		.u.gen.interval = 50,
+		.u.gen.cnt = MESH_IO_TX_COUNT_UNLIMITED,
+		.u.gen.min_delay = 0,
+		.u.gen.max_delay = 0,
+	};
+	const void *data = prov->packet_buf;
+	uint16_t size = prov->packet_len;
+	uint16_t init_size;
+	uint8_t buf[1 + PB_ADV_MTU + 5] = { MESH_AD_TYPE_PROVISION };
+	uint8_t max_seg;
+	uint8_t consumed;
+	int i;
+
+	if (!size)
+		return;
+
+	mesh_io_send_cancel(io, buf, 1);
+
+	l_put_be32(prov->conn_id, buf + 1);
+	buf[1 + 4] = prov->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[1 + 4 + 1] = max_seg << 2;
+	l_put_be16(size, buf + 1 + 4 + 1 + 1);
+	buf[9] = mesh_crypto_compute_fcs(data, size);
+	memcpy(buf + 1 + 4 + 1 + 1 + 2 + 1, 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_io_send(io, &info, 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_io_send(io, &info, buf, seg_size + 7);
+
+		consumed += seg_size;
+	}
+}
+
+static void send_adv_msg(struct mesh_prov *prov, const void *data,
+								uint16_t size)
+{
+	l_timeout_remove(prov->tx_timeout);
+	prov->tx_timeout = l_timeout_create(TX_TIMEOUT, tx_timeout, prov, NULL);
+
+	memcpy(prov->packet_buf, data, size);
+	prov->packet_len = size;
+
+	send_adv_segs(prov);
+}
+
+static void send_ack(struct mesh_prov *prov)
+{
+	struct mesh_io *io = mesh_net_get_io(prov->net);
+	struct mesh_io_send_info info = {
+		.type = MESH_IO_TIMING_TYPE_GENERAL,
+		.u.gen.interval = 50,
+		.u.gen.cnt = 1,
+		.u.gen.min_delay = 0,
+		.u.gen.max_delay = 0,
+	};
+	uint8_t ack[7] = { MESH_AD_TYPE_PROVISION };
+
+	l_put_be32(prov->conn_id, ack + 1);
+	ack[1 + 4] = prov->last_peer_msg_num;
+	ack[1 + 4 + 1] = 0x01; /* ACK */
+
+	/* print_packet("ADV-ACK", ack + 1, sizeof(ack) - 1); */
+	mesh_io_send(io, &info, ack, sizeof(ack));
+}
+
+static void adv_data_pkt(uint8_t type, const void *pkt, uint8_t size,
+								void *user_data)
+{
+	const uint8_t *data = pkt;
+	struct mesh_prov *prov = user_data;
+	uint16_t offset = 0;
+
+	if ((type & 0x03) == 0x00) {
+		uint8_t last_seg = type >> 2;
+
+		prov->expected_len = l_get_be16(data);
+		prov->expected_fcs = l_get_u8(data + 2);
+
+		/* print_packet("Pkt", pkt, size); */
+		data += 3;
+		size -= 3;
+
+		prov->trans = MESH_TRANS_RX;
+
+		if (prov->expected_len > sizeof(prov->peer_buf)) {
+			l_info("Incoming pkt exceeds storage %d > %ld",
+				prov->expected_len, sizeof(prov->peer_buf));
+			return;
+		} else if (last_seg == 0)
+			prov->trans = MESH_TRANS_IDLE;
+
+		prov->expected_segs = 0xff >> (7 - last_seg);
+		prov->got_segs |= 1;
+		memcpy(prov->peer_buf, data, size);
+
+	} else if ((type & 0x03) == 0x02) {
+		offset = (PB_ADV_MTU - 4) + ((type >> 2) - 1) *
+							(PB_ADV_MTU - 1);
+
+		if (offset + size > prov->expected_len) {
+			l_info("Incoming pkt exceeds agreed len %d + %d > %d",
+					offset, size, prov->expected_len);
+			return;
+		}
+
+		prov->trans = MESH_TRANS_RX;
+
+		l_debug("Processing fragment %u", type & 0x3f);
+
+		prov->got_segs |= 1 << (type >> 2);
+		memcpy(prov->peer_buf + offset, data, size);
+
+	} else if (type == 0x01) {
+		if (prov->send_callback) {
+			void *data = prov->send_data;
+			mesh_prov_send_func_t cb = prov->send_callback;
+
+			prov->trans = MESH_TRANS_IDLE;
+			prov->send_callback = NULL;
+			prov->send_data = NULL;
+
+			cb(true, data);
+		}
+		return;
+	} else
+		return;
+
+	if (prov->got_segs != prov->expected_segs)
+		return;
+
+	/* Validate RXed packet and pass up to Provisioning */
+	if (!mesh_crypto_check_fcs(prov->peer_buf,
+				prov->expected_len,
+				prov->expected_fcs)) {
+		l_debug("Invalid FCS");
+		return;
+	}
+
+	prov->last_peer_msg_num = prov->peer_msg_num;
+	send_ack(prov);
+
+	prov->trans = MESH_TRANS_IDLE;
+
+	packet_received(prov, prov->peer_buf,
+			prov->expected_len, prov->expected_fcs);
+
+	/* Reset Re-Assembly for next packet */
+	prov->expected_len = sizeof(prov->peer_buf);
+	prov->expected_fcs = 0;
+	prov->expected_segs = 0;
+	prov->got_segs = 0;
+
+}
+
+static void adv_bearer_packet(void *user_data, struct mesh_io_recv_info *info,
+					const uint8_t *pkt, uint16_t len)
+{
+	struct mesh_prov *prov = user_data;
+	uint32_t conn_id;
+	uint8_t msg_num;
+	uint8_t type;
+
+	if (len < 6) {
+		l_info("  Too short packet");
+		return;
+	}
+
+	conn_id = l_get_be32(pkt + 1);
+	msg_num = l_get_u8(pkt + 1 + 4);
+	type = l_get_u8(pkt + 1 + 4 + 1);
+
+	/*if (prov->conn_id == conn_id) print_packet("ADV-RX", pkt, len); */
+
+	if (prov->conn_id != DEFAULT_CONN_ID) {
+		if (prov->conn_id != conn_id) {
+			l_debug("rxed unknown conn_id: %8.8x != %8.8x",
+							conn_id, prov->conn_id);
+			return;
+		}
+	} else if (type != 0x03)
+		return;
+
+	/* print_packet("PB-ADV-RX", pkt, len); */
+
+	/* Normalize pkt to start of PROV pkt payload */
+	pkt += 7;
+	len -= 7;
+
+	if (type == 0x07) { /* OPEN_CFM */
+		if (conn_id != prov->conn_id)
+			return;
+
+		if (msg_num != prov->local_msg_num)
+			return;
+
+		l_info("Link open confirmed");
+
+		prov->bearer = MESH_BEARER_ADV;
+		if (prov->open_callback)
+			prov->open_callback(prov->receive_data);
+	} else if (type == 0x01) {
+		if (conn_id != prov->conn_id)
+			return;
+
+		if (msg_num != prov->local_msg_num)
+			return;
+
+		l_debug("Got ACK %d", msg_num);
+		adv_data_pkt(type, pkt, len, user_data);
+	} else if (type == 0x03) {
+		/*
+		 * Ignore if:
+		 * 1. We are already provisioning
+		 * 2. We are not advertising that we are unprovisioned
+		 * 3. Open request not addressed to us
+		 */
+		if (prov->conn_id != DEFAULT_CONN_ID &&
+				prov->conn_id != conn_id)
+			return;
+
+		if (prov->local_msg_num != (DEFAULT_DEV_MSG_NUM - 1))
+			return;
+
+		if (memcmp(pkt, prov->uuid, 16))
+			return;
+
+		l_info("Link open request");
+
+		prov->last_peer_msg_num = 0xFF;
+		prov->bearer = MESH_BEARER_ADV;
+		if (prov->open_callback && prov->conn_id == DEFAULT_CONN_ID)
+			prov->open_callback(prov->receive_data);
+
+		prov->conn_id = conn_id;
+		prov->peer_msg_num = msg_num;
+		send_open_cfm(prov);
+	} else if (type == 0x0B) {
+		if (prov->conn_id != conn_id)
+			return;
+
+		prov->conn_id = DEFAULT_CONN_ID;
+		prov->local_msg_num = 0xFF;
+		prov->peer_msg_num = 0xFF;
+		prov->last_peer_msg_num = 0xFF;
+
+		l_timeout_remove(prov->tx_timeout);
+		prov->tx_timeout = NULL;
+
+		l_info("Link closed notification: %2.2x", pkt[0]);
+
+		if (prov->close_callback)
+			prov->close_callback(prov->receive_data, pkt[0]);
+	} else if ((type & 0x03) == 0x00) {
+		if (prov->conn_id != conn_id)
+			return;
+
+		if (msg_num == prov->last_peer_msg_num) {
+			send_ack(prov);
+			return;
+		}
+
+		prov->peer_msg_num = msg_num;
+
+		l_debug("Processing Data with %u fragments,%d octets",
+						type >> 2, l_get_be16(pkt));
+		adv_data_pkt(type, pkt, len, user_data);
+
+	} else if ((type & 0x03) == 0x02) {
+		if (prov->conn_id != conn_id)
+			return;
+
+		if (msg_num == prov->last_peer_msg_num) {
+			send_ack(prov);
+			return;
+		}
+
+		prov->peer_msg_num = msg_num;
+
+		l_debug("Processing fragment %u", type >> 2);
+		adv_data_pkt(type, pkt, len, user_data);
+	}
+}
+
+static void beacon_packet(void *user_data, struct mesh_io_recv_info *info,
+					const uint8_t *pkt, uint16_t len)
+{
+	struct mesh_prov *prov = user_data;
+	struct mesh_io *io;
+
+	pkt++;
+	len--;
+
+	if (len < 19)
+		return;
+
+	if (!pkt[0])
+		print_packet("UnProv-BEACON-RX", pkt, len);
+
+	/* Ignore devices not matching UUID */
+	if (pkt[0] || memcmp(pkt + 1, prov->uuid, 16))
+		return;
+
+	io = mesh_net_get_io(prov->net);
+	mesh_io_deregister_recv_cb(io, MESH_IO_FILTER_BEACON);
+
+	if ((prov->conn_id != DEFAULT_CONN_ID) ||
+			(prov->bearer != MESH_BEARER_IDLE)) {
+		l_info("PB-ADV: Already Provisioning");
+		return;
+	}
+
+	l_getrandom(&prov->conn_id, sizeof(prov->conn_id));
+	prov->bearer = MESH_BEARER_ADV;
+	send_open_req(prov);
+}
+
+static bool mesh_prov_enable(struct mesh_prov *prov, enum mesh_prov_mode mode,
+							uint8_t uuid[16])
+{
+	const uint8_t pb_adv_data[] = { MESH_AD_TYPE_BEACON, 0 };
+	uint8_t adv_data[62];
+	uint8_t adv_len, type;
+	struct mesh_io *io;
+	struct mesh_io_send_info tx_info = {
+		.type = MESH_IO_TIMING_TYPE_GENERAL,
+		.u.gen.interval = 1000,	/* ms */
+		.u.gen.cnt = 0,		/* 0 == Infinite */
+		.u.gen.min_delay = 0,	/* no delay */
+		.u.gen.max_delay = 0,	/* no delay */
+	};
+
+	if (!prov || !prov->net)
+		return false;
+
+
+	prov->mode = mode;
+	memcpy(prov->uuid, uuid, 16);
+	prov->conn_id = DEFAULT_CONN_ID;
+	io = mesh_net_get_io(prov->net);
+
+	switch (mode) {
+	case MESH_PROV_MODE_NONE:
+		break;
+	case MESH_PROV_MODE_INITIATOR:
+		print_packet("Searching for uuid", uuid, 16);
+		prov->local_msg_num = DEFAULT_PROV_MSG_NUM;
+		prov->peer_msg_num = DEFAULT_DEV_MSG_NUM;
+		mesh_io_register_recv_cb(io, MESH_IO_FILTER_PROV,
+						adv_bearer_packet, prov);
+		mesh_io_register_recv_cb(io, MESH_IO_FILTER_BEACON,
+						beacon_packet, prov);
+		break;
+
+	case MESH_PROV_MODE_ADV_ACCEPTOR:
+		prov->local_msg_num = DEFAULT_DEV_MSG_NUM - 1;
+		prov->peer_msg_num = DEFAULT_PROV_MSG_NUM;
+
+		print_packet("Beaconing as unProvisioned uuid", uuid, 16);
+		adv_len = sizeof(pb_adv_data);
+		memcpy(adv_data, pb_adv_data, adv_len);
+		memcpy(adv_data + adv_len, uuid, 16);
+		adv_len += 16;
+		adv_len += 2;
+		mesh_io_register_recv_cb(io, MESH_IO_FILTER_PROV,
+						adv_bearer_packet, prov);
+		type = MESH_AD_TYPE_BEACON;
+		mesh_io_send_cancel(io, &type, 1);
+		mesh_io_send(io, &tx_info, adv_data, adv_len);
+		break;
+
+	case MESH_PROV_MODE_GATT_CLIENT:
+	case MESH_PROV_MODE_MESH_GATT_CLIENT:
+	case MESH_PROV_MODE_GATT_ACCEPTOR:
+	case MESH_PROV_MODE_MESH_SERVER:
+	case MESH_PROV_MODE_MESH_CLIENT:
+	default:
+		l_error("Unimplemented Prov Mode: %d", mode);
+		break;
+	}
+
+	return true;
+}
+
+bool mesh_prov_listen(struct mesh_net *net, uint8_t uuid[16], uint8_t caps[12],
+					mesh_prov_open_func_t open_callback,
+					mesh_prov_close_func_t close_callback,
+					mesh_prov_receive_func_t recv_callback,
+					void *user_data)
+{
+	struct mesh_prov *prov = mesh_net_get_prov(net);
+
+	if (!prov) {
+		prov = mesh_prov_new(net, 0);
+		if (!prov)
+			return false;
+
+		mesh_net_set_prov(net, prov);
+	}
+
+	prov->open_callback = open_callback;
+	prov->close_callback = close_callback;
+	prov->receive_callback = recv_callback;
+	prov->receive_data = prov; /* TODO: retink the callback placement */
+	memcpy(prov->caps, caps, sizeof(prov->caps));
+
+	prov->trans = MESH_TRANS_IDLE;
+
+
+	return mesh_prov_enable(prov, MESH_PROV_MODE_ADV_ACCEPTOR, uuid);
+}
+
+unsigned int mesh_prov_send(struct mesh_prov *prov,
+					const void *ptr, uint16_t size,
+					mesh_prov_send_func_t send_callback,
+					void *user_data)
+{
+	const uint8_t *data = ptr;
+
+	if (!prov)
+		return 0;
+
+	if (prov->trans != MESH_TRANS_IDLE)
+		return 0;
+
+	if (prov->remote) {
+		/* TODO -- PB-Remote */
+	} else {
+		prov->send_callback = send_callback;
+		prov->send_data = user_data;
+		prov->trans = MESH_TRANS_TX;
+		prov->local_msg_num++;
+		send_adv_msg(prov, data, size);
+	}
+
+	return 1;
+}
+
+bool mesh_prov_close(struct mesh_prov *prov, uint8_t reason)
+{
+	if (!prov)
+		return false;
+
+	prov->local_msg_num = 0;
+	send_close_ind(prov, reason);
+
+	prov->conn_id = DEFAULT_CONN_ID;
+	prov->local_msg_num = 0xFF;
+	prov->peer_msg_num = 0xFF;
+	prov->last_peer_msg_num = 0xFF;
+
+	if (prov->tx_timeout) {
+		l_timeout_remove(prov->tx_timeout);
+
+		/* If timing out, give Close indication 1 second of
+		 * provisioning timing to get final Close indication out
+		 */
+		prov->tx_timeout = l_timeout_create(1, tx_timeout, prov, NULL);
+	}
+
+	if (prov->close_callback)
+		prov->close_callback(prov->receive_data, reason);
+
+	return false;
+}
+
+void mesh_prov_set_addr(struct mesh_prov *prov, uint16_t addr)
+{
+	prov->addr = addr;
+}
+
+uint16_t mesh_prov_get_idx(struct mesh_prov *prov)
+{
+	return prov->net_idx;
+}
diff --git a/meshd/src/provision.c b/meshd/src/provision.c
new file mode 100644
index 000000000..18e5b3398
--- /dev/null
+++ b/meshd/src/provision.c
@@ -0,0 +1,1159 @@
+/*
+ *
+ *  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 "meshd/common/mesh-defs.h"
+#include "src/shared/ecc.h"
+
+#include "meshd/src/display.h"
+#include "meshd/src/crypto.h"
+#include "meshd/src/net.h"
+#include "meshd/src/prov.h"
+#include "meshd/src/provision.h"
+#include "meshd/src/node.h"
+
+#define PROV_INVITE	0x00
+#define PROV_CAPS	0x01
+#define PROV_START	0x02
+#define PROV_PUB_KEY	0x03
+#define PROV_INP_CMPLT	0x04
+#define PROV_CONFIRM	0x05
+#define PROV_RANDOM	0x06
+#define PROV_DATA	0x07
+#define PROV_COMPLETE	0x08
+#define PROV_FAILED	0x09
+
+#define PROV_ERR_INVALID_PDU		0x01
+#define PROV_ERR_INVALID_FORMAT		0x02
+#define PROV_ERR_UNEXPECTED_PDU		0x03
+#define PROV_ERR_CONFIRM_FAILED		0x04
+#define PROV_ERR_INSUF_RESOURCE		0x05
+#define PROV_ERR_DECRYPT_FAILED		0x06
+#define PROV_ERR_UNEXPECTED_ERR		0x07
+#define PROV_ERR_CANT_ASSIGN_ADDR	0x08
+
+/* Expected Provisioning PDU sizes */
+static const uint16_t expected_pdu_size[] = {
+	1 + 1,					/* PROV_INVITE */
+	1 + 1 + 2 + 1 + 1 + 1 + 2 + 1 + 2,	/* PROV_CAPS */
+	1 + 1 + 1 + 1 + 1 + 1,			/* PROV_START */
+	1 + 64,					/* PROV_PUB_KEY */
+	1,					/* PROV_INP_CMPLT */
+	1 + 16,					/* PROV_CONFIRM */
+	1 + 16,					/* PROV_RANDOM */
+	1 + 16 + 2 + 1 + 4 + 2 + 8,		/* PROV_DATA */
+	1,					/* PROV_COMPLETE */
+	1 + 1,					/* PROV_FAILED */
+};
+
+static enum {
+	PUB_KEY_TYPE_ephemeral,
+	PUB_KEY_TYPE_available,
+} pub_key_type = PUB_KEY_TYPE_ephemeral;
+
+static enum {
+	AUTH_TYPE_3a,
+	AUTH_TYPE_3b,
+	AUTH_TYPE_3c,
+} prov_auth_type = AUTH_TYPE_3c;
+
+enum {
+	INT_PROV_IDLE,
+	INT_PROV_INVITE_SENT,
+	INT_PROV_INVITE_ACKED,
+	INT_PROV_START_SENT,
+	INT_PROV_START_ACKED,
+	INT_PROV_KEY_SENT,
+	INT_PROV_KEY_ACKED,
+	INT_PROV_CONF_SENT,
+	INT_PROV_CONF_ACKED,
+	INT_PROV_RAND_SENT,
+	INT_PROV_RAND_ACKED,
+	INT_PROV_DATA_SENT,
+	INT_PROV_DATA_ACKED,
+} int_prov_state = INT_PROV_IDLE;
+
+enum {
+	ACP_PROV_IDLE,
+	ACP_PROV_CAPS_SENT,
+	ACP_PROV_CAPS_ACKED,
+	ACP_PROV_KEY_SENT,
+	ACP_PROV_KEY_ACKED,
+	ACP_PROV_INP_CMPLT_SENT,
+	ACP_PROV_INP_CMPLT_ACKED,
+	ACP_PROV_CONF_SENT,
+	ACP_PROV_CONF_ACKED,
+	ACP_PROV_RAND_SENT,
+	ACP_PROV_RAND_ACKED,
+	ACP_PROV_CMPLT_SENT,
+	ACP_PROV_FAIL_SENT,
+} acp_prov_state = ACP_PROV_IDLE;
+
+static uint8_t prov_expected;
+static int8_t prov_last = -1;
+
+static void int_prov_send_cmplt(bool success, struct mesh_prov *prov);
+static void acp_prov_send_cmplt(bool success, struct mesh_prov *prov);
+
+static void swap_u256_bytes(uint8_t *u256)
+{
+	int i;
+
+	/* End-to-End byte reflection of 32 octet buffer */
+	for (i = 0; i < 16; i++) {
+		u256[i] ^= u256[31 - i];
+		u256[31 - i] ^= u256[i];
+		u256[i] ^= u256[31 - i];
+	}
+}
+
+static uint8_t u16_highest_bit(uint16_t mask)
+{
+	uint8_t cnt = 0;
+
+	if (!mask)
+		return 0xff;
+
+	while (mask & 0xfffe) {
+		cnt++;
+		mask >>= 1;
+	}
+
+	return cnt;
+}
+
+static void send_prov_start(struct mesh_prov *prov)
+{
+	struct mesh_net *net = prov->net;
+	struct mesh_net_prov_caps *caps = mesh_net_prov_caps_get(net);
+	uint8_t prov_start[6] = { PROV_START };
+
+	memset(prov_start + 1, 0, 1 + 1 + 1 + 1 + 1);
+	if (!(caps->algorithms & 0x0001)) {
+		/* We only support FIPS P-256 Elliptic Curve */
+		l_error("Unrecognized Algorithm %4.4x", caps->algorithms);
+		return;
+	}
+
+	if (caps->pub_type) {
+		/* Prov Step 2b: New device exposed PublicKey OOB */
+		prov_start[2] = 0x01;
+		pub_key_type = PUB_KEY_TYPE_available;
+	} else {
+		pub_key_type = PUB_KEY_TYPE_ephemeral;
+	}
+
+	if (caps->output_size &&
+			caps->output_action) {
+		/* Prov Step 3a: Output OOB used */
+		prov_start[3] = 0x02;
+		prov_start[4] = u16_highest_bit(caps->output_action);
+		prov_start[5] = caps->output_size > 8 ?
+			8 : caps->output_size;
+
+		prov_auth_type = AUTH_TYPE_3a;
+
+	} else if (caps->input_size &&
+			caps->input_action) {
+		/* Prov Step 3b: Input OOB used */
+		prov_start[3] = 0x03;
+		prov_start[4] = u16_highest_bit(caps->input_action);
+		prov_start[5] = caps->input_size > 8 ?
+			8 : caps->input_size;
+
+		prov_auth_type = AUTH_TYPE_3b;
+
+	} else  {
+		if (caps->static_type)
+			prov_start[3] = 0x01;
+
+		/* Prov Step 3c: Static OOB used (or no OOB available) */
+		prov_auth_type = AUTH_TYPE_3c;
+	}
+
+	memcpy(&prov->conf_inputs.start, prov_start + 1,
+					sizeof(prov->conf_inputs.start));
+
+	int_prov_state = INT_PROV_START_SENT;
+	if (pub_key_type == PUB_KEY_TYPE_ephemeral)
+		prov_expected = PROV_PUB_KEY;
+	else if (prov_auth_type == AUTH_TYPE_3b)
+		prov_expected = PROV_INP_CMPLT;
+	else
+		prov_expected = PROV_CONFIRM;
+
+	mesh_prov_send(prov, prov_start, 6,
+			int_prov_send_cmplt, prov);
+
+}
+
+static void calculate_secrets(struct mesh_prov *prov, bool initiator)
+{
+	struct mesh_net *net = prov->net;
+	uint8_t *priv_key = mesh_net_priv_key_get(net);
+	bool test_mode = mesh_net_test_mode(net);
+	uint8_t tmp[64];
+
+	if (initiator) {
+		memcpy(prov->conf_inputs.prv_pub_key,
+					prov->l_public, sizeof(prov->l_public));
+		memcpy(prov->conf_inputs.dev_pub_key,
+					prov->r_public, sizeof(prov->r_public));
+	} else {
+		memcpy(prov->conf_inputs.prv_pub_key,
+					prov->r_public, sizeof(prov->r_public));
+		memcpy(prov->conf_inputs.dev_pub_key,
+					prov->l_public, sizeof(prov->l_public));
+	}
+
+	/* Convert to Mesh byte order */
+	memcpy(tmp, prov->r_public, 64);
+	swap_u256_bytes(tmp);
+	swap_u256_bytes(tmp + 32);
+
+	ecdh_shared_secret(tmp, priv_key, prov->secret);
+
+	/* Convert to Mesh byte order */
+	swap_u256_bytes(prov->secret);
+
+	mesh_crypto_s1(&prov->conf_inputs,
+			sizeof(prov->conf_inputs), prov->conf_salt);
+
+
+	mesh_crypto_prov_conf_key(prov->secret, prov->conf_salt,
+							prov->conf_key);
+
+	if (test_mode) {
+		print_packet("PublicKeyRemote", prov->r_public, 64);
+		print_packet("PublicKeyLocal", prov->l_public, 64);
+		print_packet("PrivateKeyLocal", priv_key, 32);
+		print_packet("ConfirmationInputs", &prov->conf_inputs,
+						sizeof(prov->conf_inputs));
+		print_packet("ECDHSecret", prov->secret,
+						sizeof(prov->secret));
+		print_packet("ConfirmationSalt", prov->conf_salt, 16);
+		print_packet("ConfirmationKey", prov->conf_key,
+						sizeof(prov->conf_key));
+	}
+}
+
+static void send_prov_key(struct mesh_prov *prov,
+					mesh_prov_send_func_t send_callback)
+{
+	uint8_t send_pub_key[65] = { PROV_PUB_KEY };
+
+	memcpy(send_pub_key + 1, prov->l_public, 64);
+	mesh_prov_send(prov, send_pub_key, 65,
+			send_callback, prov);
+}
+
+static void send_prov_data(struct mesh_prov *prov)
+{
+	struct mesh_net *net = prov->net;
+	struct mesh_net_prov_caps *caps = mesh_net_prov_caps_get(net);
+	uint64_t mic;
+	uint32_t iv_index;
+	uint8_t snb_flags;
+	uint16_t net_idx = mesh_prov_get_idx(prov);
+	uint8_t prov_data[1 + 16 + 2 + 1 + 4 + 2 + sizeof(mic)] = { PROV_DATA };
+	uint16_t uni_addr = mesh_net_prov_uni(net, caps->num_ele);
+	bool test_mode = mesh_net_test_mode(net);
+
+	/* Calculate Provisioning Data */
+	prov_expected = PROV_COMPLETE;
+	mesh_net_get_snb_state(net, &snb_flags, &iv_index);
+
+	mesh_net_get_key(net, !!(snb_flags & 0x01), net_idx, prov_data + 1);
+	l_put_be16(net_idx, prov_data + 1 + 16);
+	l_put_u8(snb_flags, prov_data + 1 + 16 + 2);
+	l_put_be32(iv_index, prov_data + 1 + 16 + 2 + 1);
+	l_put_be16(uni_addr, prov_data + 1 + 16 + 2 + 1 + 4);
+
+	if (test_mode)
+		print_packet("Data", prov_data + 1, 16 + 2 + 1 + 4 + 2);
+
+	mesh_crypto_device_key(prov->secret, prov->prov_salt, prov->dev_key);
+	if (test_mode) {
+		print_packet("DevKey", prov->dev_key, 16);
+		print_packet("NetworkKey", prov_data + 1, 16);
+		print_packet("NetworkKey Index", prov_data + 1 + 16, 2);
+		print_packet("SNB Flags", prov_data + 1 + 16 + 2, 1);
+		print_packet("IVindex", prov_data + 1 + 16 + 2 + 1, 4);
+		print_packet("Unicast Addr", prov_data + 1 + 16 + 2 + 1 + 4, 2);
+	}
+
+	mesh_crypto_aes_ccm_encrypt(prov->s_nonce, prov->s_key,
+					NULL, 0,
+					&prov_data[1],
+					sizeof(prov_data) - 1 - sizeof(mic),
+					&prov_data[1],
+					&mic, sizeof(mic));
+	if (test_mode)
+		print_packet("DataEncrypted + mic", prov_data + 1,
+						sizeof(prov_data) - 1);
+
+	int_prov_state = INT_PROV_DATA_SENT;
+	mesh_prov_send(prov, prov_data, sizeof(prov_data),
+			int_prov_send_cmplt, prov);
+	mesh_prov_set_addr(prov, uni_addr);
+}
+
+static void send_prov_conf(struct mesh_prov *prov,
+			mesh_prov_send_func_t send_callback)
+{
+	struct mesh_net *net = prov->net;
+	uint8_t *test_rand = mesh_net_prov_rand(net);
+	uint8_t prov_conf[1 + sizeof(prov->conf)] = { PROV_CONFIRM };
+	bool test_mode = mesh_net_test_mode(net);
+
+	if (test_mode && test_rand[0])
+		memcpy(prov->rand_auth, test_rand, 16);
+	else
+		l_getrandom(prov->rand_auth, 16);
+
+	/* Calculate Confirmation */
+	mesh_crypto_aes_cmac(prov->conf_key, prov->rand_auth,
+				sizeof(prov->rand_auth), prov->conf);
+
+	/* Marshal Confirmation */
+	memcpy(prov_conf + 1, prov->conf, sizeof(prov->conf));
+
+	if (test_mode) {
+		print_packet("ConfirmationKey", prov->conf_key,
+						sizeof(prov->conf_key));
+		print_packet("RandomAuthValue", prov->rand_auth,
+						sizeof(prov->rand_auth));
+		print_packet("Sending Confirmation", prov->conf,
+						sizeof(prov->conf));
+	}
+
+	mesh_prov_send(prov, prov_conf, sizeof(prov_conf),
+					send_callback, prov);
+}
+
+static void send_prov_rand(struct mesh_prov *prov,
+			mesh_prov_send_func_t send_callback)
+{
+	struct mesh_net *net = prov->net;
+	uint8_t prov_rand[17] = { PROV_RANDOM };
+	bool test_mode = mesh_net_test_mode(net);
+
+	/* Marshal Random */
+	memcpy(prov_rand + 1, prov->rand_auth, 16);
+
+	if (test_mode)
+		print_packet("Sending Random", prov->rand_auth, 16);
+
+	mesh_prov_send(prov, prov_rand, sizeof(prov_rand),
+					send_callback, prov);
+}
+
+enum inputType {
+	INP_key,
+	INP_dec,
+	INP_text,
+};
+
+struct input_data {
+	struct mesh_prov *prov;
+	enum inputType type;
+	bool initiator;
+	void *dest;
+	void *user_data;
+	union {
+		struct {
+			uint8_t idx;
+			char data[129];
+		} key;
+		struct {
+			uint64_t value;
+		} dec;
+		struct {
+			uint8_t idx;
+			char str[16];
+		} text;
+	} u;
+};
+
+static void collectInput(struct mesh_prov *prov, char *prompt,
+					enum inputType type, bool initiator,
+					void *dest, void *user_data)
+{
+	struct input_data *inp = l_new(struct input_data, 1);
+
+	inp->prov = prov;
+	inp->type = type;
+	inp->dest = dest;
+	inp->initiator = initiator;
+	inp->user_data = user_data;
+
+	if (prompt)
+		l_info("%s", prompt);
+
+	/* TODO: Request agent get OOB data */
+}
+
+static uint32_t digit_mod(uint8_t power)
+{
+	uint32_t ret = 1;
+
+	while (power--)
+		ret *= 10;
+
+	return ret;
+}
+
+static char *key_type(uint8_t type)
+{
+	switch (type) {
+	case 0x01:
+		return "QR-Code";
+	case 0x02:
+		return "Barcode";
+	case 0x03:
+		return "NFC Tag";
+	case 0x04:
+		return "Printed Number";
+	default:
+		return "unknown Source";
+	}
+}
+
+static void int_prov_send_cmplt(bool success, struct mesh_prov *prov)
+{
+	struct mesh_net *net = prov->net;
+	struct mesh_net_prov_caps *caps = mesh_net_prov_caps_get(net);
+
+	l_debug("Provision sending complete");
+
+	switch (int_prov_state) {
+	case INT_PROV_INVITE_SENT:
+		int_prov_state = INT_PROV_INVITE_ACKED;
+		if (acp_prov_state == ACP_PROV_CAPS_SENT)
+			send_prov_start(prov);
+		break;
+	case INT_PROV_START_SENT:
+		int_prov_state = INT_PROV_START_ACKED;
+		if (pub_key_type == PUB_KEY_TYPE_ephemeral) {
+			int_prov_state = INT_PROV_KEY_SENT;
+			send_prov_key(prov, int_prov_send_cmplt);
+		} else {
+			collectInput(prov, NULL, INP_key, true,
+						prov->r_public, prov);
+			l_info("\n\nEnter key from %s:\n",
+				key_type(caps->pub_type));
+		}
+		break;
+	case INT_PROV_KEY_SENT:
+		int_prov_state = INT_PROV_KEY_ACKED;
+		if (pub_key_type == PUB_KEY_TYPE_ephemeral) {
+			prov_expected = PROV_PUB_KEY;
+			break;
+		}
+
+		/* Start Step 3 */
+		memset(prov->rand_auth + 16, 0, 16);
+		if (prov_auth_type == AUTH_TYPE_3a)
+			collectInput(prov,
+				"\n\nEnter prompted number from device:",
+				INP_dec, true,
+				prov->rand_auth + 32 - sizeof(uint32_t),
+				prov);
+
+		else if (prov_auth_type == AUTH_TYPE_3b) {
+			uint32_t oob_key;
+
+			l_getrandom(&oob_key, sizeof(uint32_t));
+			oob_key %= digit_mod(caps->input_size);
+			l_put_be32(oob_key,
+					prov->rand_auth + 32 -
+					sizeof(uint32_t));
+			l_info("\n\nEnter %d on Device\n", oob_key);
+			prov_expected = PROV_INP_CMPLT;
+
+		} else if (caps->static_type) {
+			collectInput(prov, NULL, INP_text, true,
+					prov->rand_auth + 16, prov);
+			l_info("\n\nstatic OOB str from %s:\n",
+				key_type(caps->static_type));
+
+		} else {
+			int_prov_state = INT_PROV_CONF_SENT;
+			send_prov_conf(prov, int_prov_send_cmplt);
+		}
+
+		break;
+	case INT_PROV_CONF_SENT:
+		int_prov_state = INT_PROV_CONF_ACKED;
+		if (acp_prov_state == ACP_PROV_CONF_SENT) {
+			int_prov_state = INT_PROV_RAND_SENT;
+			prov_expected = PROV_RANDOM;
+			send_prov_rand(prov, int_prov_send_cmplt);
+		}
+		break;
+	case INT_PROV_RAND_SENT:
+		int_prov_state = INT_PROV_RAND_ACKED;
+		if (acp_prov_state == ACP_PROV_RAND_SENT)
+			send_prov_data(prov);
+		break;
+	case INT_PROV_DATA_SENT:
+		int_prov_state = INT_PROV_DATA_ACKED;
+		break;
+	default:
+	case INT_PROV_INVITE_ACKED:
+	case INT_PROV_START_ACKED:
+	case INT_PROV_KEY_ACKED:
+	case INT_PROV_CONF_ACKED:
+	case INT_PROV_RAND_ACKED:
+	case INT_PROV_DATA_ACKED:
+	case INT_PROV_IDLE:
+		break;
+	}
+}
+
+void initiator_prov_open(struct mesh_prov *prov)
+{
+	uint8_t invite[] = { PROV_INVITE, 30 };
+	uint8_t *priv_key;
+
+	l_info("Provisioning link opened");
+
+	priv_key = mesh_net_priv_key_get(prov->net);
+	ecc_make_key(prov->l_public, priv_key);
+
+	int_prov_state = INT_PROV_INVITE_SENT;
+	prov_expected = PROV_CAPS;
+	prov_last = -1;
+	prov->conf_inputs.invite.attention = invite[1];
+	mesh_prov_send(prov, invite, sizeof(invite),
+					int_prov_send_cmplt, prov);
+}
+
+void initiator_prov_close(struct mesh_prov *prov, uint8_t reason)
+{
+	struct mesh_net *net = prov->net;
+	uint32_t iv_index;
+	uint8_t snb_flags;
+
+	l_info("Provisioning link closed");
+
+	/* Get the provisioned node's composition data*/
+	if (reason == 0) {
+		mesh_net_get_snb_state(net, &snb_flags, &iv_index);
+
+		l_info("Save provisioner's DB");
+	}
+}
+
+void initiator_prov_receive(const void *pkt, uint16_t size,
+							struct mesh_prov *prov)
+{
+	struct mesh_net *net = prov->net;
+	struct mesh_net_prov_caps *caps = mesh_net_prov_caps_get(net);
+	bool test_mode = mesh_net_test_mode(net);
+	const uint8_t *data = pkt;
+	uint8_t tmp[16];
+	uint8_t type = *data++;
+	uint8_t err = 0;
+
+
+	l_debug("Provisioning packet received type: %2.2x (%u octets)",
+								type, size);
+
+	if (type == prov_last) {
+		l_error("Ignore repeated %2.2x packet", type);
+		return;
+	} else if ((type > prov_expected || type < prov_last) &&
+						type != PROV_FAILED) {
+		l_error("Expected %2.2x, Got:%2.2x", prov_expected, type);
+		err = PROV_ERR_UNEXPECTED_PDU;
+		goto failure;
+	}
+
+	if (type >= L_ARRAY_SIZE(expected_pdu_size) ||
+					size != expected_pdu_size[type]) {
+		l_error("Expected PDU size %d, Got %d (type: %2.2x)",
+					expected_pdu_size[type], size, type);
+		err = PROV_ERR_INVALID_FORMAT;
+		goto failure;
+	}
+
+	prov_last = type;
+
+	switch (type) {
+	case PROV_CAPS: /* Capabilities */
+		int_prov_state = INT_PROV_INVITE_ACKED;
+		acp_prov_state = ACP_PROV_CAPS_SENT;
+		caps->num_ele = data[0];
+		if (test_mode)
+			l_info("Got Num Ele %d", data[0]);
+
+		caps->algorithms = l_get_be16(data + 1);
+		if (test_mode)
+			l_info("Got alg %d", caps->algorithms);
+
+		caps->pub_type = data[3];
+		if (test_mode)
+			l_info("Got pub_type %d", data[3]);
+
+		caps->static_type = data[4];
+		if (test_mode)
+			l_info("Got static_type %d", data[4]);
+
+		caps->output_size = data[5];
+		if (test_mode)
+			l_info("Got output_size %d", data[5]);
+
+		caps->output_action = l_get_be16(data + 6);
+		if (test_mode)
+			l_info("Got output_action %d", l_get_be16(data + 6));
+
+		caps->input_size = data[8];
+		if (test_mode)
+			l_info("Got input_size %d", data[8]);
+
+		caps->input_action = l_get_be16(data + 9);
+		if (test_mode)
+			l_info("Got input_action %d", l_get_be16(data + 9));
+
+		if (caps->algorithms != 0x0001) {
+			l_error("Unsupported Algorithm");
+			err = PROV_ERR_INVALID_FORMAT;
+			goto failure;
+		}
+
+		memcpy(&prov->conf_inputs.caps, data, 11);
+
+		if (int_prov_state == INT_PROV_INVITE_ACKED)
+			send_prov_start(prov);
+		break;
+
+	case PROV_PUB_KEY: /* Public Key */
+		int_prov_state = INT_PROV_KEY_ACKED;
+		acp_prov_state = ACP_PROV_KEY_SENT;
+		memcpy(prov->r_public, data, 64);
+		calculate_secrets(prov, true);
+		prov_expected = PROV_CONFIRM;
+
+		memset(prov->rand_auth + 16, 0, 16);
+		if (prov_auth_type == AUTH_TYPE_3a) {
+			collectInput(prov,
+				"\n\nEnter number from device:",
+				INP_dec, true,
+				prov->rand_auth + 32 - sizeof(uint32_t),
+				prov);
+
+		} else if (prov_auth_type == AUTH_TYPE_3b) {
+
+			uint32_t oob_key;
+
+			l_getrandom(&oob_key, sizeof(uint32_t));
+			oob_key %= digit_mod(caps->input_size);
+			l_put_be32(oob_key,
+					prov->rand_auth + 32 -
+					sizeof(uint32_t));
+			l_info("\n\nEnter %d on Device\n", oob_key);
+			prov_expected = PROV_INP_CMPLT;
+
+		} else if (caps->static_type) {
+			collectInput(prov, NULL, INP_dec, true,
+					prov->rand_auth + 16, prov);
+			l_info("\n\nstatic OOB str from %s:\n",
+				key_type(caps->static_type));
+
+		} else
+			send_prov_conf(prov, int_prov_send_cmplt);
+		break;
+
+	case PROV_INP_CMPLT: /* Provisioning Input Complete */
+		acp_prov_state = ACP_PROV_INP_CMPLT_SENT;
+		prov_expected = PROV_CONFIRM;
+		send_prov_conf(prov, int_prov_send_cmplt);
+		break;
+
+	case PROV_CONFIRM: /* Confirmation */
+		int_prov_state = INT_PROV_CONF_ACKED;
+		acp_prov_state = ACP_PROV_CONF_SENT;
+		/* RXed Device Confirmation */
+		memcpy(prov->conf, data, sizeof(prov->conf));
+		if (test_mode)
+			print_packet("ConfirmationDevice", prov->conf,
+							sizeof(prov->conf));
+
+		if (int_prov_state == INT_PROV_CONF_ACKED) {
+			prov_expected = PROV_RANDOM;
+			send_prov_rand(prov, int_prov_send_cmplt);
+		}
+		break;
+
+	case PROV_RANDOM: /* Random */
+		int_prov_state = INT_PROV_RAND_ACKED;
+		acp_prov_state = ACP_PROV_RAND_SENT;
+
+		/* Calculate SessionKey while the data is fresh */
+		mesh_crypto_prov_prov_salt(prov->conf_salt,
+						prov->rand_auth, data,
+						prov->prov_salt);
+		mesh_crypto_session_key(prov->secret, prov->prov_salt,
+							prov->s_key);
+		mesh_crypto_nonce(prov->secret, prov->prov_salt, prov->s_nonce);
+		if (test_mode) {
+			print_packet("SessionKey", prov->s_key,
+					sizeof(prov->s_key));
+			print_packet("Nonce", prov->s_nonce,
+					sizeof(prov->s_nonce));
+		}
+
+		/* RXed Device Confirmation */
+		memcpy(prov->rand_auth, data, sizeof(prov->conf));
+		if (test_mode)
+			print_packet("RandomDevice", prov->rand_auth, 16);
+
+		mesh_crypto_aes_cmac(prov->conf_key, prov->rand_auth,
+					sizeof(prov->rand_auth), tmp);
+
+		if (memcmp(tmp, prov->conf, sizeof(prov->conf))) {
+			l_error("Provisioning Failed-Confirm compare)");
+			err = PROV_ERR_CONFIRM_FAILED;
+			goto failure;
+		}
+
+		if (int_prov_state == INT_PROV_RAND_ACKED) {
+			prov_expected = PROV_COMPLETE;
+			send_prov_data(prov);
+		}
+		break;
+
+	case PROV_COMPLETE: /* Complete */
+		l_info("Provisioning Complete");
+		int_prov_state = INT_PROV_IDLE;
+		mesh_prov_close(prov, 0);
+		break;
+
+	case PROV_FAILED: /* Failed */
+		l_error("Provisioning Failed (reason: %d)", data[0]);
+		err = data[0];
+		goto failure;
+
+	default:
+		l_error("Unknown Pkt %2.2x", type);
+		err = PROV_ERR_UNEXPECTED_PDU;
+		goto failure;
+	}
+
+	return;
+
+failure:
+	int_prov_state = INT_PROV_IDLE;
+	mesh_prov_close(prov, err);
+}
+
+static void acp_prov_send_cmplt(bool success, struct mesh_prov *prov)
+{
+	l_debug("Provision sending complete");
+
+	switch (acp_prov_state) {
+	case ACP_PROV_CAPS_SENT:
+		acp_prov_state = ACP_PROV_CAPS_ACKED;
+		if (int_prov_state == INT_PROV_KEY_SENT) {
+			acp_prov_state = ACP_PROV_KEY_SENT;
+			prov_expected = PROV_CONFIRM;
+			send_prov_key(prov, acp_prov_send_cmplt);
+		}
+		break;
+	case ACP_PROV_KEY_SENT:
+		acp_prov_state = ACP_PROV_KEY_ACKED;
+		if (int_prov_state == INT_PROV_CONF_SENT) {
+			acp_prov_state = ACP_PROV_CONF_SENT;
+			prov_expected = PROV_RANDOM;
+			send_prov_conf(prov, acp_prov_send_cmplt);
+		}
+		break;
+	case ACP_PROV_INP_CMPLT_SENT:
+		acp_prov_state = ACP_PROV_INP_CMPLT_ACKED;
+		break;
+	case ACP_PROV_CONF_SENT:
+		acp_prov_state = ACP_PROV_CONF_ACKED;
+		if (int_prov_state == INT_PROV_RAND_SENT) {
+			acp_prov_state = ACP_PROV_RAND_SENT;
+			prov_expected = PROV_DATA;
+			send_prov_rand(prov, acp_prov_send_cmplt);
+		}
+		break;
+	case ACP_PROV_RAND_SENT:
+		acp_prov_state = ACP_PROV_RAND_ACKED;
+		break;
+	case ACP_PROV_CMPLT_SENT:
+		acp_prov_state = ACP_PROV_IDLE;
+		mesh_net_provisioned_set(prov->net, true);
+	default:
+	case ACP_PROV_IDLE:
+	case ACP_PROV_CAPS_ACKED:
+	case ACP_PROV_KEY_ACKED:
+	case ACP_PROV_INP_CMPLT_ACKED:
+	case ACP_PROV_CONF_ACKED:
+	case ACP_PROV_RAND_ACKED:
+	case ACP_PROV_FAIL_SENT:
+		break;
+	}
+}
+
+void acceptor_prov_open(struct mesh_prov *prov)
+{
+	uint8_t *priv_key;
+
+	l_info("Provisioning link opened");
+
+	priv_key = mesh_net_priv_key_get(prov->net);
+	ecc_make_key(prov->l_public, priv_key);
+
+	prov_expected = PROV_INVITE;
+	prov_last = -1;
+}
+
+void acceptor_prov_close(struct mesh_prov *prov, uint8_t reason)
+{
+	l_info("Provisioning link closed");
+	mesh_prov_unref(prov);
+}
+
+static void prov_store_cfm(void *user_data, bool result)
+{
+	struct mesh_prov *prov = user_data;
+	uint8_t out[2];
+
+	if (result) {
+		acp_prov_state = ACP_PROV_CMPLT_SENT;
+		out[0] = PROV_COMPLETE;
+		mesh_prov_send(prov, out, 1,
+				acp_prov_send_cmplt,
+				prov);
+	} else {
+		acp_prov_state = ACP_PROV_FAIL_SENT;
+		out[0] = PROV_FAILED;
+		out[1] = PROV_ERR_INSUF_RESOURCE;
+		mesh_prov_send(prov, out, 2, NULL, NULL);
+	}
+}
+
+void acceptor_prov_receive(const void *pkt, uint16_t size,
+							struct mesh_prov *prov)
+{
+	struct mesh_net *net = prov->net;
+	struct mesh_net_prov_caps *caps = mesh_net_prov_caps_get(net);
+	uint8_t *priv_key = mesh_net_priv_key_get(net);
+	bool test_mode = mesh_net_test_mode(net);
+	bool ret;
+	const uint8_t *data = pkt;
+	uint8_t type = *data++;
+	uint8_t out[129];
+	uint8_t tmp[16];
+	uint8_t rand_dev[16];
+	uint64_t rx_mic, decode_mic;
+
+	l_debug("Provisioning packet received type: %2.2x (%u octets)",
+								type, size);
+
+	if (type == prov_last) {
+		l_error("Ignore repeated %2.2x packet", type);
+		return;
+	} else if (type > prov_expected || type < prov_last) {
+		l_error("Expected %2.2x, Got:%2.2x", prov_expected, type);
+		out[1] = PROV_ERR_UNEXPECTED_PDU;
+		goto failure;
+	}
+
+	if (type >= L_ARRAY_SIZE(expected_pdu_size) ||
+					size != expected_pdu_size[type]) {
+		l_error("Expected PDU size %d, Got %d (type: %2.2x)",
+			size, expected_pdu_size[type], type);
+		out[1] = PROV_ERR_INVALID_FORMAT;
+		goto failure;
+	}
+
+	prov_last = type;
+
+	switch (type) {
+	case PROV_INVITE: /* Prov Invite */
+		int_prov_state = INT_PROV_INVITE_SENT;
+		/* Prov Capabilities */
+		out[0] = PROV_CAPS;
+		out[1] = caps->num_ele;
+		l_put_be16(caps->algorithms, out + 2);
+		out[4] = caps->pub_type;
+		out[5] = caps->static_type;
+		out[6] = caps->output_size;
+		l_put_be16(caps->output_action, out + 7);
+		out[9] = caps->input_size;
+		l_put_be16(caps->input_action, out + 10);
+
+		prov->conf_inputs.invite.attention = data[0];
+		memcpy(&prov->conf_inputs.caps, out + 1,
+				sizeof(prov->conf_inputs.caps));
+
+		acp_prov_state = ACP_PROV_CAPS_SENT;
+		prov_expected = PROV_START;
+		mesh_prov_send(prov, out, sizeof(*caps) + 1,
+				acp_prov_send_cmplt, prov);
+		break;
+
+	case PROV_START: /* Prov Start */
+		if (data[0]) {
+			/* Only Algorithm 0x00 supported */
+			l_error("Invalid Algorithm: %2.2x", data[0]);
+			out[1] = PROV_ERR_INVALID_FORMAT;
+			goto failure;
+		}
+
+		acp_prov_state = ACP_PROV_CAPS_ACKED;
+		int_prov_state = INT_PROV_START_SENT;
+		prov_expected = PROV_PUB_KEY;
+		memcpy(&prov->conf_inputs.start, data,
+				sizeof(prov->conf_inputs.start));
+		if (data[1] == 1 && caps->pub_type) {
+			pub_key_type = PUB_KEY_TYPE_available;
+			ecc_make_key(prov->l_public, priv_key);
+		} else if (data[1] == 0) {
+			pub_key_type = PUB_KEY_TYPE_ephemeral;
+			/* Use Ephemeral Key */
+			l_getrandom(priv_key, 32);
+			ecc_make_key(prov->l_public, priv_key);
+		} else {
+			out[1] = PROV_ERR_INVALID_FORMAT;
+			goto failure;
+		}
+
+		swap_u256_bytes(prov->l_public);
+		swap_u256_bytes(prov->l_public + 32);
+
+		switch (data[2]) {
+		default:
+			out[1] = PROV_ERR_INVALID_FORMAT;
+			goto failure;
+
+		case 0x00:
+		case 0x01:
+			prov_auth_type = AUTH_TYPE_3c;
+			break;
+
+		case 0x02:
+			prov_auth_type = AUTH_TYPE_3a;
+			caps->output_action = 1 << data[3];
+			caps->output_size = data[4];
+			break;
+
+		case 0x03:
+			prov_auth_type = AUTH_TYPE_3b;
+			caps->input_action = 1 << data[3];
+			caps->input_size = data[4];
+			break;
+		}
+		break;
+
+	case PROV_PUB_KEY: /* Public Key */
+		int_prov_state = INT_PROV_KEY_SENT;
+		prov_expected = PROV_CONFIRM;
+		/* Save Key */
+		memcpy(prov->r_public, data, 64);
+		calculate_secrets(prov, false);
+
+		if (pub_key_type == PUB_KEY_TYPE_ephemeral) {
+			acp_prov_state = ACP_PROV_KEY_SENT;
+			send_prov_key(prov, acp_prov_send_cmplt);
+		}
+
+		/* Start Step 3 */
+		memset(prov->rand_auth + 16, 0, 16);
+		if (prov_auth_type == AUTH_TYPE_3a) {
+			uint32_t oob_key;
+
+			l_getrandom(&oob_key, sizeof(uint32_t));
+			oob_key %= digit_mod(caps->output_size);
+			l_put_be32(oob_key,
+				prov->rand_auth + 32 - sizeof(uint32_t));
+			l_info("\n\nEnter %d on Provisioner\n",
+							oob_key);
+
+		} else if (prov_auth_type == AUTH_TYPE_3b) {
+			if (caps->input_action == (1 << 3)) {
+				/* TODO: Collect Text Input data */
+				;
+			} else {
+				/* TODO: Collect Decimal Input data */
+				;
+			}
+
+		} else {
+			if (caps->static_type) {
+				/* TODO: Collect Static Input data */
+				/* (If needed) */
+				;
+			}
+		}
+
+		break;
+
+	case PROV_CONFIRM: /* Confirmation */
+		int_prov_state = INT_PROV_CONF_SENT;
+		acp_prov_state = ACP_PROV_KEY_ACKED;
+		/* RXed Provision Confirmation */
+		memcpy(prov->r_conf, data, sizeof(prov->r_conf));
+		if (test_mode)
+			print_packet("ConfirmationProvisioner",
+					prov->r_conf,
+					sizeof(prov->r_conf));
+
+		if (acp_prov_state == ACP_PROV_KEY_ACKED) {
+			prov_expected = PROV_RANDOM;
+			send_prov_conf(prov, acp_prov_send_cmplt);
+		}
+		break;
+
+	case PROV_RANDOM: /* Random */
+		int_prov_state = INT_PROV_RAND_SENT;
+		acp_prov_state = ACP_PROV_CONF_ACKED;
+
+		/* Calculate Session key while the data is fresh */
+		mesh_crypto_prov_prov_salt(prov->conf_salt, data,
+						prov->rand_auth,
+						prov->prov_salt);
+		mesh_crypto_session_key(prov->secret, prov->prov_salt,
+							prov->s_key);
+		mesh_crypto_nonce(prov->secret, prov->prov_salt, prov->s_nonce);
+
+		if (test_mode) {
+			print_packet("SessionKey", prov->s_key,
+					sizeof(prov->s_key));
+			print_packet("Nonce", prov->s_nonce,
+					sizeof(prov->s_nonce));
+		}
+
+
+		/* Save Local Random data to send after verification */
+		memcpy(rand_dev, prov->rand_auth, 16);
+		/* RXed Provisioner Confirmation */
+		memcpy(prov->rand_auth, data, 16);
+		if (test_mode)
+			print_packet("RandomProvisioner", prov->rand_auth, 16);
+
+		mesh_crypto_aes_cmac(prov->conf_key, prov->rand_auth,
+					sizeof(prov->rand_auth), tmp);
+
+		if (memcmp(tmp, prov->r_conf,
+					sizeof(prov->r_conf))) {
+			l_error("Provisioning Failed-Confirm compare");
+			out[1] = PROV_ERR_CONFIRM_FAILED;
+			goto failure;
+		}
+
+
+		memcpy(prov->rand_auth, rand_dev, 16);
+		if (acp_prov_state == ACP_PROV_CONF_ACKED) {
+			prov_expected = PROV_DATA;
+			send_prov_rand(prov, acp_prov_send_cmplt);
+		}
+		break;
+
+	case PROV_DATA: /* Provisioning Data */
+		int_prov_state = INT_PROV_DATA_SENT;
+		acp_prov_state = ACP_PROV_RAND_ACKED;
+		if (test_mode) {
+			print_packet("DataEncrypted + mic", data, size - 1);
+			print_packet("Rxed-mic", data + 16 + 2 + 1 + 4 + 2, 8);
+		}
+
+		rx_mic = l_get_be64(data + 16 + 2 + 1 + 4 + 2);
+		mesh_crypto_aes_ccm_decrypt(prov->s_nonce, prov->s_key,
+				NULL, 0,
+				data, size - 1, out + 1,
+				&decode_mic, sizeof(decode_mic));
+
+		if (test_mode) {
+			print_packet("Data", out + 1, 16 + 2 + 1 + 4 + 2);
+			l_info("Calc-mic: %16.16lx", decode_mic);
+		}
+
+		if (rx_mic == decode_mic) {
+			mesh_crypto_device_key(prov->secret,
+						prov->prov_salt,
+						prov->dev_key);
+			if (test_mode) {
+				print_packet("DevKey", prov->dev_key, 16);
+				print_packet("NetworkKey", out + 1, 16);
+				print_packet("NetworkKey Index",
+					out + 1 + 16, 2);
+				print_packet("SNB Flags",
+					out + 1 + 16 + 2, 1);
+				print_packet("IVindex",
+					out + 1 + 16 + 2 + 1, 4);
+				print_packet("Unicast Addr",
+					out + 1 + 16 + 2 + 1 + 4, 2);
+			}
+
+			/* Set Provisioned Data */
+			ret = mesh_net_provisioned_new(prov->net,
+					prov->dev_key,
+					l_get_be16(out + 17),
+					out + 1,
+					l_get_be16(out + 24),
+					out[19],
+					l_get_be32(out + 20),
+					prov_store_cfm, prov);
+
+			if (!ret) {
+				out[1] = PROV_ERR_INSUF_RESOURCE;
+				goto failure;
+			}
+		} else {
+			l_error("Provisioning Failed-MIC compare");
+			out[1] = PROV_ERR_DECRYPT_FAILED;
+			goto failure;
+		}
+		break;
+
+	default:
+		l_error("Unknown Pkt %2.2x", type);
+		out[1] = PROV_ERR_UNEXPECTED_PDU;
+		goto failure;
+	}
+
+	return;
+
+failure:
+	acp_prov_state = ACP_PROV_FAIL_SENT;
+	out[0] = PROV_FAILED;
+	mesh_prov_send(prov, out, 2, acp_prov_send_cmplt, prov);
+}
-- 
2.14.3