mirror of
https://git.kernel.org/pub/scm/bluetooth/bluez.git
synced 2024-12-13 05:54:34 +08:00
ad7db1fa1f
This allows one App using the mesh daemon to provision another.
576 lines
13 KiB
C
576 lines
13 KiB
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;
|
|
|
|
l_timeout_remove(session->tx_timeout);
|
|
session->tx_timeout = NULL;
|
|
|
|
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_timeout_remove(session->tx_timeout);
|
|
l_debug("Link closed notification: %2.2x", pkt[0]);
|
|
/* Wrap callback for pre-cleaning */
|
|
if (true) {
|
|
mesh_prov_close_func_t cb = session->close_cb;
|
|
void *user_data = session->user_data;
|
|
|
|
l_queue_remove(pb_sessions, session);
|
|
l_free(session);
|
|
cb(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);
|
|
}
|