bluez/mesh/pb-adv.c

/*
 *
 *  BlueZ - Bluetooth protocol stack for Linux
 *
 *  Copyright (C) 2018-2019  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 <ell/ell.h>

#include "mesh/mesh-defs.h"
#include "mesh/crypto.h"
#include "mesh/net.h"
#include "mesh/mesh-io.h"
#include "mesh/mesh.h"
#include "mesh/prov.h"
#include "mesh/provision.h"
#include "mesh/pb-adv.h"


struct pb_adv_session {
	mesh_prov_open_func_t open_cb;
	mesh_prov_close_func_t close_cb;
	mesh_prov_receive_func_t rx_cb;
	mesh_prov_ack_func_t ack_cb;
	struct l_timeout *tx_timeout;
	struct pb_adv_session *loop;
	uint32_t link_id;
	uint16_t exp_len;
	uint8_t exp_fcs;
	uint8_t exp_segs;
	uint8_t got_segs;
	uint8_t trans_num;
	uint8_t local_acked;
	uint8_t local_trans_num;
	uint8_t peer_trans_num;
	uint8_t last_peer_trans_num;
	uint8_t sar[80];
	uint8_t uuid[16];
	bool initiator;
	bool opened;
	void *user_data;
};

#define PB_ADV_ACK 0x01
#define PB_ADV_OPEN_REQ 0x03
#define PB_ADV_OPEN_CFM 0x07
#define PB_ADV_CLOSE 0x0B

#define PB_ADV_MTU	24

struct pb_ack {
	uint8_t ad_type;
	uint32_t link_id;
	uint8_t trans_num;
	uint8_t opcode;
} __packed;

struct pb_open_req{
	uint8_t ad_type;
	uint32_t link_id;
	uint8_t trans_num;
	uint8_t opcode;
	uint8_t uuid[16];
} __packed;

struct pb_open_cfm{
	uint8_t ad_type;
	uint32_t link_id;
	uint8_t trans_num;
	uint8_t opcode;
} __packed;

struct pb_close_ind {
	uint8_t ad_type;
	uint32_t link_id;
	uint8_t trans_num;
	uint8_t opcode;
	uint8_t reason;
} __packed;

struct idle_rx {
	struct pb_adv_session *session;
	uint16_t len;
	uint8_t data[PB_ADV_MTU + 6];
};

static struct l_queue *pb_sessions = NULL;

static const uint8_t filter[1] = { MESH_AD_TYPE_PROVISION };

static void pb_adv_packet(void *user_data, const uint8_t *pkt, uint16_t len);

static void idle_rx_adv(void *user_data)
{
	struct idle_rx *rx = user_data;

	pb_adv_packet(rx->session, rx->data, rx->len);
	l_free(rx);
}

static void pb_adv_send(struct pb_adv_session *session,
					uint8_t count, uint16_t interval,
					void *data, uint16_t len)
{
	struct idle_rx *rx;

	if (session->loop) {
		rx = l_new(struct idle_rx, 1);
		rx->session = session->loop;
		rx->len = len;
		memcpy(rx->data, data, len);

		l_idle_oneshot(idle_rx_adv, rx, NULL);
	} else
		mesh_send_pkt(count, interval, data, len);
}

static void send_adv_segs(struct pb_adv_session *session, const uint8_t *data,
							uint16_t size)
{
	uint16_t init_size;
	uint8_t buf[PB_ADV_MTU + 6] = { MESH_AD_TYPE_PROVISION };
	uint8_t max_seg;
	uint8_t consumed;
	int i;

	if (!size)
		return;

	mesh_send_cancel(filter, sizeof(filter));

	l_put_be32(session->link_id, buf + 1);
	buf[1 + 4] = ++session->local_trans_num;

	if (size > PB_ADV_MTU - 4) {
		max_seg = 1 +
			(((size - (PB_ADV_MTU - 4)) - 1) / (PB_ADV_MTU - 1));
		init_size = PB_ADV_MTU - 4;
	} else {
		max_seg = 0;
		init_size = size;
	}

	/* print_packet("FULL-TX", data, size); */

	l_debug("Sending %u fragments for %u octets", max_seg + 1, size);

	buf[6] = max_seg << 2;
	l_put_be16(size, buf + 7);
	buf[9] = mesh_crypto_compute_fcs(data, size);
	memcpy(buf + 10, data, init_size);

	l_debug("max_seg: %2.2x", max_seg);
	l_debug("size: %2.2x, CRC: %2.2x", size, buf[9]);
	/* print_packet("PB-TX", buf + 1, init_size + 9); */

	pb_adv_send(session, MESH_IO_TX_COUNT_UNLIMITED, 200,
							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); */

		pb_adv_send(session, MESH_IO_TX_COUNT_UNLIMITED, 200,
							buf, seg_size + 7);

		consumed += seg_size;
	}
}

static bool session_match (const void *a, const void *b)
{
	return a == b;
}

static bool uuid_match (const void *a, const void *b)
{
	const struct pb_adv_session *session = a;
	const uint8_t *uuid = b;

	return !memcmp(session->uuid, uuid, sizeof(session->uuid));
}

static bool user_match (const void *a, const void *b)
{
	const struct pb_adv_session *session = a;

	return session->user_data == b;
}

static void tx_timeout(struct l_timeout *timeout, void *user_data)
{
	struct pb_adv_session *session = user_data;
	mesh_prov_close_func_t cb;

	if (!l_queue_find(pb_sessions, session_match, session))
		return;

	mesh_send_cancel(filter, sizeof(filter));

	l_info("TX timeout");
	cb = session->close_cb;
	user_data = session->user_data;
	cb(user_data, 1);
}

static void pb_adv_tx(void *user_data, void *data, uint16_t len)
{
	struct pb_adv_session *session = user_data;

	if (!l_queue_find(pb_sessions, session_match, session))
		return;

	l_timeout_remove(session->tx_timeout);
	session->tx_timeout = l_timeout_create(30, tx_timeout, session, NULL);

	send_adv_segs(session, data, len);
}

static void send_open_req(struct pb_adv_session *session)
{
	struct pb_open_req open_req = { MESH_AD_TYPE_PROVISION };

	l_put_be32(session->link_id, &open_req.link_id);
	open_req.trans_num = 0;
	open_req.opcode = PB_ADV_OPEN_REQ;
	memcpy(open_req.uuid, session->uuid, 16);

	mesh_send_cancel(filter, sizeof(filter));

	pb_adv_send(session, MESH_IO_TX_COUNT_UNLIMITED, 500, &open_req,
							sizeof(open_req));
}

static void send_open_cfm(struct pb_adv_session *session)
{
	struct pb_open_cfm open_cfm = { MESH_AD_TYPE_PROVISION };

	l_put_be32(session->link_id, &open_cfm.link_id);
	open_cfm.trans_num = 0;
	open_cfm.opcode = PB_ADV_OPEN_CFM;

	mesh_send_cancel(filter, sizeof(filter));

	pb_adv_send(session, MESH_IO_TX_COUNT_UNLIMITED, 500, &open_cfm,
							sizeof(open_cfm));
}

static void send_ack(struct pb_adv_session *session, uint8_t trans_num)
{
	struct pb_ack ack = { MESH_AD_TYPE_PROVISION };

	if (!l_queue_find(pb_sessions, session_match, session))
		return;

	l_put_be32(session->link_id, &ack.link_id);
	ack.trans_num = trans_num;
	ack.opcode = PB_ADV_ACK;

	pb_adv_send(session, 1, 100, &ack, sizeof(ack));
}

static void send_close_ind(struct pb_adv_session *session, uint8_t reason)
{
	struct pb_close_ind close_ind = { MESH_AD_TYPE_PROVISION };

	if (!l_queue_find(pb_sessions, session_match, session))
		return;

	l_put_be32(session->link_id, &close_ind.link_id);
	close_ind.trans_num = 0;
	close_ind.opcode = PB_ADV_CLOSE;
	close_ind.reason = reason;

	mesh_send_cancel(filter, sizeof(filter));

	pb_adv_send(session, 10, 100, &close_ind, sizeof(close_ind));
}

static void pb_adv_packet(void *user_data, const uint8_t *pkt, uint16_t len)
{
	struct pb_adv_session *session = user_data;
	uint32_t link_id;
	size_t offset;
	uint8_t trans_num;
	uint8_t type;
	bool first;

	if (!l_queue_find(pb_sessions, session_match, session))
		return;

	link_id = l_get_be32(pkt + 1);
	type = l_get_u8(pkt + 6);

	/* Validate new or existing Connection ID */
	if (session->link_id) {
		if (session->link_id != link_id)
			return;
	} else if (type != 0x03)
		return;
	else if (!link_id)
		return;

	trans_num = l_get_u8(pkt + 5);
	pkt += 7;
	len -= 7;

	switch (type) {
	case PB_ADV_OPEN_CFM:
		/*
		 * Ignore if:
		 * 1. We are acceptor
		 * 2. We are already provisioning on different link_id
		 */

		if (!session->initiator)
			return;

		first = !session->opened;
		session->opened = true;

		/* Only call Open callback once */
		if (first) {
			l_debug("PB-ADV open confirmed");
			session->local_trans_num = 0xFF;
			session->open_cb(session->user_data, pb_adv_tx,
							session, PB_ADV);
		}
		return;

	case PB_ADV_OPEN_REQ:
		/*
		 * Ignore if:
		 * 1. We are initiator
		 * 2. Open request not addressed to us
		 * 3. We are already provisioning on different link_id
		 */

		if (session->initiator)
			return;

		if (memcmp(pkt, session->uuid, 16))
			return;

		first = !session->link_id;
		session->link_id = link_id;
		session->last_peer_trans_num = 0xFF;
		session->local_acked = 0xFF;
		session->peer_trans_num = 0x00;
		session->local_trans_num = 0x7F;
		session->opened = true;

		/* Only call Open callback once */
		if (first) {
			l_debug("PB-ADV open requested");
			session->open_cb(session->user_data, pb_adv_tx,
							session, PB_ADV);
		}

		/* Send CFM once per received request */
		send_open_cfm(session);
		break;

	case PB_ADV_CLOSE:
		l_debug("Link closed notification: %2.2x", pkt[0]);
		session->close_cb(session->user_data, pkt[0]);
		break;

	case PB_ADV_ACK:
		if (!session->opened)
			return;

		if (trans_num != session->local_trans_num)
			return;

		if (session->local_acked > trans_num)
			return;

		mesh_send_cancel(filter, sizeof(filter));
		session->local_acked = trans_num;
		session->ack_cb(session->user_data, trans_num);
		break;

	default: /* DATA SEGMENT */
		if (!session->opened)
			return;

		if (trans_num == session->last_peer_trans_num) {
			send_ack(session, trans_num);
			return;
		}

		switch(type & 0x03) {
		case 0x00:
			session->peer_trans_num = trans_num;
			session->exp_len = l_get_be16(pkt);

			l_debug("PB-ADV start with %u fragments, %d octets",
						type >> 2, session->exp_len);

			if (session->exp_len > sizeof(session->sar)) {
				l_debug("Incoming length exceeded: %d",
							session->exp_len);
				return;
			}

			session->exp_fcs = l_get_u8(pkt + 2);
			session->exp_segs = 0xff >> (7 - (type >> 2));

			/* Save first segment */
			memcpy(session->sar, pkt + 3, len - 3);
			session->got_segs |= 1;
			break;

		case 0x02:
			session->peer_trans_num = trans_num;
			offset = 20 + (((type >> 2) - 1) * 23);

			if (offset + len - 3 > sizeof(session->sar)) {
				l_debug("Length exceeded: %d",
							session->exp_len);
				return;
			}

			l_debug("Processing fragment %u", type >> 2);
			memcpy(session->sar + offset, pkt, len);
			session->got_segs |= 1 << (type >> 2);
			break;

		default:
			/* Malformed or unrecognized */
			return;
		}

		if (session->got_segs != session->exp_segs)
			return;

		/* Validate RXed packet and pass up to Provisioning */
		if (!mesh_crypto_check_fcs(session->sar,
					session->exp_len,
					session->exp_fcs)) {

			/* This can be a false negative if first
			 * segment missed, and can almost always
			 * be ignored.
			 */

			l_debug("Invalid FCS");
			return;
		}

		send_ack(session, session->peer_trans_num);

		if (session->last_peer_trans_num != session->peer_trans_num) {
			session->got_segs = 0;
			session->last_peer_trans_num = session->peer_trans_num;
			session->rx_cb(session->user_data, session->sar,
							session->exp_len);
		}
	}
}

bool pb_adv_reg(bool initiator, mesh_prov_open_func_t open_cb,
		mesh_prov_close_func_t close_cb,
		mesh_prov_receive_func_t rx_cb, mesh_prov_ack_func_t ack_cb,
		uint8_t uuid[16], void *user_data)
{
	struct pb_adv_session *session, *old_session;

	if (!pb_sessions)
		pb_sessions = l_queue_new();

	old_session = l_queue_find(pb_sessions, uuid_match, uuid);

	/* Reject 2nd session if not looping back */
	if (l_queue_length(pb_sessions) && !old_session)
		return false;

	/* Reject looping to more than one session or with same role*/
	if (old_session && (old_session->loop ||
					old_session->initiator == initiator))
		return false;

	session = l_new(struct pb_adv_session, 1);
	session->open_cb = open_cb;
	session->close_cb = close_cb;
	session->rx_cb = rx_cb;
	session->ack_cb = ack_cb;
	session->user_data = user_data;
	session->initiator = initiator;
	memcpy(session->uuid, uuid, 16);

	l_queue_push_head(pb_sessions, session);

	if (initiator) {
		l_getrandom(&session->link_id, sizeof(session->link_id));
		session->tx_timeout = l_timeout_create(60, tx_timeout,
							session, NULL);
	}

	/* Setup Loop-back if complementary session with same UUID */
	if (old_session) {
		session->loop = old_session;
		old_session->loop = session;
		mesh_unreg_prov_rx(pb_adv_packet);

		if (initiator)
			send_open_req(session);
		else
			send_open_req(old_session);

		return true;
	}

	mesh_reg_prov_rx(pb_adv_packet, session);

	if (initiator)
		send_open_req(session);

	return true;
}

void pb_adv_unreg(void *user_data)
{
	struct pb_adv_session *session = l_queue_find(pb_sessions,
						user_match, user_data);

	if (!session)
		return;

	l_timeout_remove(session->tx_timeout);
	session->tx_timeout = NULL;
	send_close_ind(session, 0);
	l_queue_remove(pb_sessions, session);
	l_free(session);
}