emulator/bthost: Add SMP support

1 files changed, 469 insertions, 0 deletions

diff --git a/emulator/smp.c b/emulator/smp.c
new file mode 100644
index 000000000..33799b237
--- /dev/null
+++ b/emulator/smp.c
@@ -0,0 +1,469 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2013-2014  Intel Corporation
+ *
+ *
+ *  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.
+ *
+ *  You should have received a copy of the GNU Lesser General Public
+ *  License along with this library; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <ctype.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <string.h>
+#include <errno.h>
+#include <endian.h>
+#include <stdbool.h>
+#include <sys/socket.h>
+
+#include "bluetooth/bluetooth.h"
+#include "bluetooth/hci.h"
+
+#include "monitor/bt.h"
+#include "bthost.h"
+
+#ifndef SOL_ALG
+#define SOL_ALG 279
+#endif
+
+#ifndef AF_ALG
+#define AF_ALG  38
+#define PF_ALG  AF_ALG
+
+#include <linux/types.h>
+
+struct sockaddr_alg {
+	__u16   salg_family;
+	__u8    salg_type[14];
+	__u32   salg_feat;
+	__u32   salg_mask;
+	__u8    salg_name[64];
+};
+
+struct af_alg_iv {
+	__u32   ivlen;
+	__u8    iv[0];
+};
+
+#define ALG_SET_KEY                     1
+#define ALG_SET_IV                      2
+#define ALG_SET_OP                      3
+
+#define ALG_OP_DECRYPT                  0
+#define ALG_OP_ENCRYPT                  1
+
+#else
+#include <linux/if_alg.h>
+#endif
+
+#define SMP_CID 0x0006
+
+struct smp {
+	struct bthost *bthost;
+	struct smp_conn *conn;
+	int alg_sk;
+};
+
+struct smp_conn {
+	struct smp *smp;
+	uint16_t handle;
+	bool out;
+	uint8_t ia[6];
+	uint8_t ia_type;
+	uint8_t ra[6];
+	uint8_t ra_type;
+	uint8_t tk[16];
+	uint8_t prnd[16];
+	uint8_t rrnd[16];
+	uint8_t pcnf[16];
+	uint8_t preq[7];
+	uint8_t prsp[7];
+	uint8_t ltk[16];
+};
+
+static int alg_setup(void)
+{
+	struct sockaddr_alg salg;
+	int sk;
+
+	sk = socket(PF_ALG, SOCK_SEQPACKET | SOCK_CLOEXEC, 0);
+	if (sk < 0) {
+		printf("socket(AF_ALG): %s\n", strerror(errno));
+		return -1;
+	}
+
+	memset(&salg, 0, sizeof(salg));
+	salg.salg_family = AF_ALG;
+	strcpy((char *) salg.salg_type, "skcipher");
+	strcpy((char *) salg.salg_name, "ecb(aes)");
+
+	if (bind(sk, (struct sockaddr *) &salg, sizeof(salg)) < 0) {
+		printf("bind(AF_ALG): %s\n", strerror(errno));
+		close(sk);
+		return -1;
+	}
+
+	return sk;
+}
+
+static int alg_new(int alg_sk, const uint8_t *key)
+{
+	int sk;
+
+	if (setsockopt(alg_sk, SOL_ALG, ALG_SET_KEY, key, 16) < 0) {
+		printf("setsockopt(ALG_SET_KEY): %s\n", strerror(errno));
+		return -1;
+	}
+
+	sk = accept4(alg_sk, NULL, 0, SOCK_CLOEXEC);
+	if (sk < 0) {
+		printf("accept4(AF_ALG): %s\n", strerror(errno));
+		return -1;
+	}
+
+	return sk;
+}
+
+static int alg_encrypt(int sk, uint8_t in[16], uint8_t out[16])
+{
+	__u32 alg_op = ALG_OP_ENCRYPT;
+	char cbuf[CMSG_SPACE(sizeof(alg_op))];
+	struct cmsghdr *cmsg;
+	struct msghdr msg;
+	struct iovec iov;
+	int ret;
+
+	memset(cbuf, 0, sizeof(cbuf));
+	memset(&msg, 0, sizeof(msg));
+
+	msg.msg_control = cbuf;
+	msg.msg_controllen = sizeof(cbuf);
+
+	cmsg = CMSG_FIRSTHDR(&msg);
+	cmsg->cmsg_level = SOL_ALG;
+	cmsg->cmsg_type = ALG_SET_OP;
+	cmsg->cmsg_len = CMSG_LEN(sizeof(alg_op));
+	memcpy(CMSG_DATA(cmsg), &alg_op, sizeof(alg_op));
+
+	iov.iov_base = in;
+	iov.iov_len = 16;
+
+	msg.msg_iov = &iov;
+	msg.msg_iovlen = 1;
+
+	ret = sendmsg(sk, &msg, 0);
+	if (ret < 0) {
+		printf("sendmsg(AF_ALG): %s\n", strerror(errno));
+		return ret;
+	}
+
+	ret = read(sk, out, 16);
+	if (ret < 0)
+		printf("read(AF_ALG): %s\n", strerror(errno));
+
+	return 0;
+}
+
+static int smp_e(int alg_sk, uint8_t key[16], uint8_t in[16], uint8_t out[16])
+{
+	int sk, err;
+
+	sk = alg_new(alg_sk, key);
+	if (sk < 0)
+		return sk;
+
+	err = alg_encrypt(sk, in, out);
+
+	close(sk);
+
+	return err;
+}
+
+static inline void swap128(const uint8_t src[16], uint8_t dst[16])
+{
+	int i;
+	for (i = 0; i < 16; i++)
+		dst[15 - i] = src[i];
+}
+
+static inline void swap56(const uint8_t src[7], uint8_t dst[7])
+{
+	int i;
+	for (i = 0; i < 7; i++)
+		dst[6 - i] = src[i];
+}
+
+typedef struct {
+	uint64_t a, b;
+} u128;
+
+static inline void u128_xor(void *r, const void *p, const void *q)
+{
+	const u128 pp = bt_get_unaligned((const u128 *) p);
+	const u128 qq = bt_get_unaligned((const u128 *) q);
+	u128 rr;
+
+	rr.a = pp.a ^ qq.a;
+	rr.b = pp.b ^ qq.b;
+
+	bt_put_unaligned(rr, (u128 *) r);
+}
+
+static int smp_c1(struct smp_conn *conn, uint8_t rnd[16], uint8_t res[16])
+{
+	uint8_t p1[16], p2[16];
+	int err;
+
+	memset(p1, 0, 16);
+
+	/* p1 = pres || preq || _rat || _iat */
+	swap56(conn->prsp, p1);
+	swap56(conn->preq, p1 + 7);
+	p1[14] = conn->ra_type;
+	p1[15] = conn->ia_type;
+
+	memset(p2, 0, 16);
+
+	/* p2 = padding || ia || ra */
+	baswap((bdaddr_t *) (p2 + 4), (bdaddr_t *) conn->ia);
+	baswap((bdaddr_t *) (p2 + 10), (bdaddr_t *) conn->ra);
+
+	/* res = r XOR p1 */
+	u128_xor((u128 *) res, (u128 *) rnd, (u128 *) p1);
+
+	/* res = e(k, res) */
+	err = smp_e(conn->smp->alg_sk, conn->tk, res, res);
+	if (err)
+		return err;
+
+	/* res = res XOR p2 */
+	u128_xor((u128 *) res, (u128 *) res, (u128 *) p2);
+
+	/* res = e(k, res) */
+	return smp_e(conn->smp->alg_sk, conn->tk, res, res);
+}
+
+static int smp_s1(struct smp_conn *conn, uint8_t r1[16], uint8_t r2[16],
+							uint8_t res[16])
+{
+	memcpy(res, r1 + 8, 8);
+	memcpy(res + 8, r2 + 8, 8);
+
+	return smp_e(conn->smp->alg_sk, conn->tk, res, res);
+}
+
+static bool verify_random(struct smp_conn *conn, const uint8_t rnd[16])
+{
+	uint8_t confirm[16], res[16], key[16];
+	int err;
+
+	err = smp_c1(conn, conn->rrnd, res);
+	if (err < 0)
+		return false;
+
+	swap128(res, confirm);
+
+	if (memcmp(conn->pcnf, confirm, sizeof(conn->pcnf) != 0)) {
+		printf("Confirmation values don't match\n");
+		return false;
+	}
+
+	if (conn->out) {
+		smp_s1(conn, conn->rrnd, conn->prnd, key);
+		swap128(key, conn->ltk);
+		bthost_le_start_encrypt(conn->smp->bthost, conn->handle,
+								conn->ltk);
+	} else {
+		smp_s1(conn, conn->prnd, conn->rrnd, key);
+		swap128(key, conn->ltk);
+	}
+
+	return true;
+}
+
+static void pairing_req(struct smp_conn *conn, const void *data, uint16_t len)
+{
+	struct bthost *bthost = conn->smp->bthost;
+	static const uint8_t rsp[] = {	0x02,	/* Pairing Response */
+					0x03,	/* NoInputNoOutput */
+					0x00,	/* OOB Flag */
+					0x01,	/* Bonding - no MITM */
+					0x10,	/* Max key size */
+					0x00,	/* Init. key dist. */
+					0x01,	/* Rsp. key dist. */
+				};
+
+	memcpy(conn->preq, data, sizeof(conn->preq));
+	memcpy(conn->prsp, rsp, sizeof(rsp));
+
+	bthost_send_cid(bthost, conn->handle, SMP_CID, rsp, sizeof(rsp));
+}
+
+static void pairing_rsp(struct smp_conn *conn, const void *data, uint16_t len)
+{
+	memcpy(conn->prsp, data, sizeof(conn->prsp));
+
+	/*bthost_send_cid(bthost, handle, SMP_CID, pdu, req->send_len);*/
+}
+
+static void pairing_cfm(struct smp_conn *conn, const void *data, uint16_t len)
+{
+	struct bthost *bthost = conn->smp->bthost;
+	const uint8_t *cfm = data;
+	uint8_t rsp[17];
+	uint8_t res[16];
+
+	memcpy(conn->pcnf, data + 1, 16);
+
+	rsp[0] = cfm[0];
+	smp_c1(conn, conn->prnd, res);
+	swap128(res, &rsp[1]);
+
+	bthost_send_cid(bthost, conn->handle, SMP_CID, rsp, sizeof(rsp));
+}
+
+static void pairing_rnd(struct smp_conn *conn, const void *data, uint16_t len)
+{
+	struct bthost *bthost = conn->smp->bthost;
+	const uint8_t *rnd = data;
+	uint8_t rsp[17];
+
+	swap128(data + 1, conn->rrnd);
+
+	if (!verify_random(conn, data + 1))
+		return;
+
+	rsp[0] = rnd[0];
+	swap128(conn->prnd, &rsp[1]);
+
+	bthost_send_cid(bthost, conn->handle, SMP_CID, rsp, sizeof(rsp));
+}
+
+void smp_pair(void *conn_data)
+{
+	struct smp_conn *conn = conn_data;
+	struct bthost *bthost = conn->smp->bthost;
+	const uint8_t smp_pair_req[] = {	0x01,	/* Pairing Request */
+						0x03,	/* NoInputNoOutput */
+						0x00,	/* OOB Flag */
+						0x01,	/* Bonding - no MITM */
+						0x10,	/* Max key size */
+						0x00,	/* Init. key dist. */
+						0x01,	/* Rsp. key dist. */
+					};
+
+	memcpy(conn->preq, smp_pair_req, sizeof(smp_pair_req));
+
+	bthost_send_cid(bthost, conn->handle, SMP_CID, smp_pair_req,
+							sizeof(smp_pair_req));
+}
+
+void smp_data(void *conn_data, const void *data, uint16_t len)
+{
+	struct smp_conn *conn = conn_data;
+	uint8_t opcode;
+
+	if (len < 1) {
+		printf("Received too small SMP PDU\n");
+		return;
+	}
+
+	opcode = *((const uint8_t *) data);
+
+	switch (opcode) {
+	case 0x01: /* Pairing Request */
+		pairing_req(conn, data, len);
+		break;
+	case 0x02: /* Pairing Response */
+		pairing_rsp(conn, data, len);
+		break;
+	case 0x03: /* Pairing Confirm */
+		pairing_cfm(conn, data, len);
+		break;
+	case 0x04: /* Pairing Random */
+		pairing_rnd(conn, data, len);
+		break;
+	default:
+		break;
+	}
+}
+
+void *smp_conn_add(void *smp_data, uint16_t handle, const uint8_t *ia,
+					const uint8_t *ra, bool conn_init)
+{
+	struct smp *smp = smp_data;
+	struct smp_conn *conn;
+
+	conn = malloc(sizeof(struct smp_conn));
+	if (!conn)
+		return NULL;
+
+	memset(conn, 0, sizeof(*conn));
+
+	conn->smp = smp;
+	conn->handle = handle;
+	conn->out = conn_init;
+
+	conn->ia_type = LE_PUBLIC_ADDRESS;
+	conn->ra_type = LE_PUBLIC_ADDRESS;
+	memcpy(conn->ia, ia, 6);
+	memcpy(conn->ra, ra, 6);
+
+	return conn;
+}
+
+void smp_conn_del(void *conn_data)
+{
+	struct smp_conn *conn = conn_data;
+
+	free(conn);
+}
+
+void *smp_start(struct bthost *bthost)
+{
+	struct smp *smp;
+
+	smp = malloc(sizeof(struct smp));
+	if (!smp)
+		return NULL;
+
+	memset(smp, 0, sizeof(*smp));
+
+	smp->alg_sk = alg_setup();
+	if (smp->alg_sk < 0) {
+		free(smp);
+		return NULL;
+	}
+
+	smp->bthost = bthost;
+
+	return smp;
+}
+
+void smp_stop(void *smp_data)
+{
+	struct smp *smp = smp_data;
+
+	close(smp->alg_sk);
+
+	free(smp);
+}