M7350/external/bluetooth/bluez/emulator/smp.c

/*
 *
 *  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 "src/shared/crypto.h"
#include "monitor/bt.h"
#include "bthost.h"

#define SMP_CID 0x0006

struct smp {
	struct bthost *bthost;
	struct smp_conn *conn;
	struct bt_crypto *crypto;
};

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 bool verify_random(struct smp_conn *conn, const uint8_t rnd[16])
{
	uint8_t confirm[16];

	if (!bt_crypto_c1(conn->smp->crypto, conn->tk, conn->rrnd, conn->prsp,
				conn->preq, conn->ia_type, conn->ia,
				conn->ra_type, conn->ra, confirm))
		return false;

	if (memcmp(conn->pcnf, confirm, sizeof(conn->pcnf) != 0)) {
		printf("Confirmation values don't match\n");
		return false;
	}

	if (conn->out) {
		bt_crypto_s1(conn->smp->crypto, conn->tk, conn->rrnd,
							conn->prnd, conn->ltk);
		bthost_le_start_encrypt(conn->smp->bthost, conn->handle,
								conn->ltk);
	} else {
		bt_crypto_s1(conn->smp->crypto, conn->tk, conn->prnd,
							conn->rrnd, 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];

	memcpy(conn->pcnf, data + 1, 16);

	rsp[0] = cfm[0];
	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_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];

	memcpy(conn->rrnd, data + 1, 16);

	if (!verify_random(conn, data + 1))
		return;

	rsp[0] = rnd[0];
	memcpy(&rsp[1], conn->prnd, 16);

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

int smp_get_ltk(void *smp_data, uint64_t rand, uint16_t ediv, uint8_t *ltk)
{
	struct smp_conn *conn = smp_data;
	static const uint8_t no_ltk[16] = { 0 };

	if (!memcmp(conn->ltk, no_ltk, 16))
		return -ENOENT;

	memcpy(ltk, conn->ltk, 16);

	return 0;
}

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->crypto = bt_crypto_new();
	if (!smp->crypto) {
		free(smp);
		return NULL;
	}

	smp->bthost = bthost;

	return smp;
}

void smp_stop(void *smp_data)
{
	struct smp *smp = smp_data;

	bt_crypto_unref(smp->crypto);

	free(smp);
}