static gboolean l2cap_listen_cb(GIOChannel *io, GIOCondition cond,
gpointer user_data)
{
struct test_data *data = tester_get_data();
const struct l2cap_data *l2data = data->test_data;
int sk, new_sk;
data->io_id = 0;
sk = g_io_channel_unix_get_fd(io);
new_sk = accept(sk, NULL, NULL);
if (new_sk < 0) {
tester_warn("accept failed: %s (%u)", strerror(errno), errno);
tester_test_failed();
return FALSE;
}
if (l2data->read_data) {
struct bthost *bthost;
GIOChannel *new_io;
new_io = g_io_channel_unix_new(new_sk);
g_io_channel_set_close_on_unref(new_io, TRUE);
bthost = hciemu_client_get_host(data->hciemu);
g_io_add_watch(new_io, G_IO_IN, server_received_data, NULL);
bthost_send_cid(bthost, data->handle, data->dcid,
l2data->read_data, l2data->data_len);
g_io_channel_unref(new_io);
return FALSE;
} else if (l2data->write_data) {
struct bthost *bthost;
ssize_t ret;
bthost = hciemu_client_get_host(data->hciemu);
bthost_add_cid_hook(bthost, data->handle, data->scid,
server_bthost_received_data, NULL);
ret = write(new_sk, l2data->write_data, l2data->data_len);
close(new_sk);
if (ret != l2data->data_len) {
tester_warn("Unable to write all data");
tester_test_failed();
}
return FALSE;
}
tester_print("Successfully connected");
close(new_sk);
tester_test_passed();
return FALSE;
}
static void smp_new_conn(uint16_t handle, void *user_data)
{
struct test_data *data = user_data;
const struct smp_data *smp = data->test_data;
struct bthost *bthost = hciemu_client_get_host(data->hciemu);
const struct smp_req_rsp *req;
const void *pdu;
tester_print("New SMP client connection with handle 0x%04x", handle);
data->handle = handle;
bthost_add_cid_hook(bthost, handle, SMP_CID, smp_server, data);
if (smp->req_count == data->counter)
return;
req = &smp->req[data->counter];
if (!req->send)
return;
tester_print("Sending SMP PDU");
pdu = get_pdu(req->send);
bthost_send_cid(bthost, handle, SMP_CID, pdu, req->send_len);
if (!req->expect)
test_condition_complete(data);
}
static gboolean l2cap_connect_cb(GIOChannel *io, GIOCondition cond,
gpointer user_data)
{
struct test_data *data = tester_get_data();
const struct l2cap_data *l2data = data->test_data;
int err, sk_err, sk;
socklen_t len = sizeof(sk_err);
data->io_id = 0;
sk = g_io_channel_unix_get_fd(io);
if (getsockopt(sk, SOL_SOCKET, SO_ERROR, &sk_err, &len) < 0)
err = -errno;
else
err = -sk_err;
if (err < 0)
tester_warn("Connect failed: %s (%d)", strerror(-err), -err);
else
tester_print("Successfully connected");
if (l2data->read_data) {
struct bthost *bthost;
bthost = hciemu_client_get_host(data->hciemu);
g_io_add_watch(io, G_IO_IN, client_received_data, NULL);
bthost_send_cid(bthost, data->handle, data->dcid,
l2data->read_data, l2data->data_len);
return FALSE;
} else if (l2data->write_data) {
struct bthost *bthost;
ssize_t ret;
bthost = hciemu_client_get_host(data->hciemu);
bthost_add_cid_hook(bthost, data->handle, data->dcid,
bthost_received_data, NULL);
ret = write(sk, l2data->write_data, l2data->data_len);
if (ret != l2data->data_len) {
tester_warn("Unable to write all data");
tester_test_failed();
}
return FALSE;
}
if (-err != l2data->expect_err)
tester_test_failed();
else
tester_test_passed();
return FALSE;
}
static void distribute_keys(struct smp_conn *conn)
{
uint8_t buf[17];
struct bthost *bthost = conn->smp->bthost;
if (conn->local_key_dist & DIST_ENC_KEY) {
memset(buf, 0, sizeof(buf));
buf[0] = BT_L2CAP_SMP_ENCRYPT_INFO;
bthost_send_cid(bthost, conn->handle, SMP_CID, buf, 17);
buf[0] = BT_L2CAP_SMP_MASTER_IDENT;
bthost_send_cid(bthost, conn->handle, SMP_CID, buf, 11);
}
if (conn->local_key_dist & DIST_ID_KEY) {
memset(buf, 0, sizeof(buf));
buf[0] = BT_L2CAP_SMP_IDENT_ADDR_INFO;
if (conn->out) {
buf[1] = conn->ia_type;
memcpy(&buf[2], conn->ia, 6);
} else {
buf[1] = conn->ra_type;
memcpy(&buf[2], conn->ra, 6);
}
bthost_send_cid(bthost, conn->handle, SMP_CID, buf, 8);
buf[0] = BT_L2CAP_SMP_IDENT_INFO;
memset(&buf[1], 0, 16);
bthost_send_cid(bthost, conn->handle, SMP_CID, buf, 17);
}
if (conn->local_key_dist & DIST_SIGN) {
memset(buf, 0, sizeof(buf));
buf[0] = BT_L2CAP_SMP_SIGNING_INFO;
bthost_send_cid(bthost, conn->handle, SMP_CID, buf, 17);
}
}
void smp_pair(void *conn_data, uint8_t io_cap, uint8_t auth_req)
{
struct smp_conn *conn = conn_data;
struct bthost *bthost = conn->smp->bthost;
const uint8_t req[] = { BT_L2CAP_SMP_PAIRING_REQUEST,
io_cap, /* IO Capability */
0x00, /* OOB Flag */
auth_req, /* Auth requirement */
0x10, /* Max key size */
KEY_DIST, /* Init. key dist. */
KEY_DIST, /* Rsp. key dist. */
};
memcpy(conn->preq, req, sizeof(req));
bthost_send_cid(bthost, conn->handle, SMP_CID, req, sizeof(req));
}
static void pairing_rsp(struct smp_conn *conn, const void *data, uint16_t len)
{
struct smp *smp = conn->smp;
uint8_t cfm[17];
memcpy(conn->prsp, data, sizeof(conn->prsp));
conn->local_key_dist = conn->prsp[5];
conn->remote_key_dist = conn->prsp[6];
cfm[0] = BT_L2CAP_SMP_PAIRING_CONFIRM;
bt_crypto_c1(smp->crypto, conn->tk, conn->prnd, conn->prsp,
conn->preq, conn->ia_type, conn->ia,
conn->ra_type, conn->ra, &cfm[1]);
bthost_send_cid(smp->bthost, conn->handle, SMP_CID, cfm, sizeof(cfm));
}
static void pairing_rnd(struct smp_conn *conn, const void *data, uint16_t len)
{
struct bthost *bthost = conn->smp->bthost;
uint8_t rsp[17];
memcpy(conn->rrnd, data + 1, 16);
if (!verify_random(conn, data + 1))
return;
if (conn->out)
return;
rsp[0] = BT_L2CAP_SMP_PAIRING_RANDOM;
memset(&rsp[1], 0, 16);
bthost_send_cid(bthost, conn->handle, SMP_CID, rsp, sizeof(rsp));
}
static void pairing_cfm(struct smp_conn *conn, const void *data, uint16_t len)
{
struct bthost *bthost = conn->smp->bthost;
uint8_t rsp[17];
memcpy(conn->pcnf, data + 1, 16);
if (conn->out) {
rsp[0] = BT_L2CAP_SMP_PAIRING_RANDOM;
memset(&rsp[1], 0, 16);
} else {
rsp[0] = BT_L2CAP_SMP_PAIRING_CONFIRM;
bt_crypto_c1(conn->smp->crypto, conn->tk, conn->prnd,
conn->prsp, conn->preq, conn->ia_type,
conn->ia, conn->ra_type, conn->ra, &rsp[1]);
}
bthost_send_cid(bthost, conn->handle, SMP_CID, rsp, sizeof(rsp));
}
static void pairing_req(struct smp_conn *conn, const void *data, uint16_t len)
{
struct bthost *bthost = conn->smp->bthost;
uint8_t rsp[7];
memcpy(conn->preq, data, sizeof(conn->preq));
rsp[0] = BT_L2CAP_SMP_PAIRING_RESPONSE;
rsp[1] = bthost_get_io_capability(bthost);
rsp[2] = 0x00; /* OOB Flag */
rsp[3] = bthost_get_auth_req(bthost);
rsp[4] = 0x10; /* Max key size */
rsp[5] = conn->preq[5] & KEY_DIST; /* Init. key dist. */
rsp[6] = conn->preq[6] & KEY_DIST; /* Rsp. key dist. */
memcpy(conn->prsp, rsp, sizeof(rsp));
conn->local_key_dist = rsp[6];
conn->remote_key_dist = rsp[5];
bthost_send_cid(bthost, conn->handle, SMP_CID, rsp, sizeof(rsp));
}
static void bthost_send_rsp(const void *buf, uint16_t len, void *user_data)
{
struct test_data *data = tester_get_data();
const struct l2cap_data *l2data = data->test_data;
struct bthost *bthost;
if (l2data->expect_cmd_len && len != l2data->expect_cmd_len) {
tester_test_failed();
return;
}
if (l2data->expect_cmd && memcmp(buf, l2data->expect_cmd,
l2data->expect_cmd_len)) {
tester_test_failed();
return;
}
if (!l2data->send_cmd)
return;
bthost = hciemu_client_get_host(data->hciemu);
bthost_send_cid(bthost, data->handle, data->dcid,
l2data->send_cmd, l2data->send_cmd_len);
}
static void smp_server(const void *data, uint16_t len, void *user_data)
{
struct test_data *test_data = user_data;
struct bthost *bthost = hciemu_client_get_host(test_data->hciemu);
const struct smp_data *smp = test_data->test_data;
const struct smp_req_rsp *req;
const void *pdu;
uint8_t opcode;
if (len < 1) {
tester_warn("Received too small SMP PDU");
goto failed;
}
opcode = *((const uint8_t *) data);
tester_print("Received SMP opcode 0x%02x", opcode);
if (test_data->counter >= smp->req_count) {
test_condition_complete(test_data);
return;
}
req = &smp->req[test_data->counter++];
if (!req->expect)
goto next;
if (req->expect_len != len) {
tester_warn("Unexpected SMP PDU length (%u != %u)",
len, req->expect_len);
goto failed;
}
switch (opcode) {
case 0x01: /* Pairing Request */
memcpy(test_data->preq, data, sizeof(test_data->preq));
break;
case 0x02: /* Pairing Response */
memcpy(test_data->prsp, data, sizeof(test_data->prsp));
break;
case 0x03: /* Pairing Confirm */
memcpy(test_data->pcnf, data + 1, 16);
goto next;
case 0x04: /* Pairing Random */
memcpy(test_data->rrnd, data + 1, 16);
if (!verify_random(data + 1))
goto failed;
goto next;
default:
break;
}
if (memcmp(req->expect, data, len) != 0) {
tester_warn("Unexpected SMP PDU");
goto failed;
}
next:
if (smp->req_count == test_data->counter) {
test_condition_complete(test_data);
return;
}
req = &smp->req[test_data->counter];
pdu = get_pdu(req->send);
bthost_send_cid(bthost, test_data->handle, SMP_CID, pdu,
req->send_len);
if (!req->expect)
test_condition_complete(test_data);
return;
failed:
tester_test_failed();
}