---
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
--
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
