/*
 *
 *  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 "lib/bluetooth.h"
#include "lib/hci.h"

#include "src/shared/util.h"
#include "src/shared/crypto.h"
#include "src/shared/ecc.h"
#include "monitor/bt.h"
#include "bthost.h"

#define SMP_CID		0x0006
#define SMP_BREDR_CID	0x0007

#define L2CAP_FC_SMP_BREDR	0x80

#define SMP_PASSKEY_ENTRY_FAILED	0x01
#define SMP_OOB_NOT_AVAIL		0x02
#define SMP_AUTH_REQUIREMENTS		0x03
#define SMP_CONFIRM_FAILED		0x04
#define SMP_PAIRING_NOTSUPP		0x05
#define SMP_ENC_KEY_SIZE		0x06
#define SMP_CMD_NOTSUPP			0x07
#define SMP_UNSPECIFIED			0x08
#define SMP_REPEATED_ATTEMPTS		0x09
#define SMP_INVALID_PARAMS		0x0a
#define SMP_DHKEY_CHECK_FAILED		0x0b
#define SMP_NUMERIC_COMP_FAILED		0x0c
#define SMP_BREDR_PAIRING_IN_PROGRESS	0x0d

#define DIST_ENC_KEY	0x01
#define DIST_ID_KEY	0x02
#define DIST_SIGN	0x04
#define DIST_LINK_KEY	0x08

#define KEY_DIST	(DIST_ENC_KEY | DIST_ID_KEY | DIST_SIGN)

#define SC_NO_DIST	(DIST_ENC_KEY | DIST_LINK_KEY)

#define MAX_IO_CAP	0x04

#define SMP_AUTH_NONE		0x00
#define SMP_AUTH_BONDING	0x01
#define SMP_AUTH_MITM		0x04
#define SMP_AUTH_SC		0x08
#define SMP_AUTH_KEYPRESS	0x10

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

struct smp_conn {
	struct smp *smp;
	uint16_t handle;
	uint8_t addr_type;
	bool out;
	bool sc;
	bool initiator;
	uint8_t method;
	uint8_t local_key_dist;
	uint8_t remote_key_dist;
	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];

	uint8_t local_sk[32];
	uint8_t local_pk[64];
	uint8_t remote_pk[64];
	uint8_t dhkey[32];
	uint8_t mackey[16];

	uint8_t passkey_notify;
	uint8_t passkey_round;
};

enum {
	JUST_WORKS,
	JUST_CFM,
	REQ_PASSKEY,
	CFM_PASSKEY,
	REQ_OOB,
	DSP_PASSKEY,
	OVERLAP,
};

static const uint8_t gen_method[5][5] = {
	{ JUST_WORKS,  JUST_CFM,    REQ_PASSKEY, JUST_WORKS, REQ_PASSKEY },
	{ JUST_WORKS,  JUST_CFM,    REQ_PASSKEY, JUST_WORKS, REQ_PASSKEY },
	{ CFM_PASSKEY, CFM_PASSKEY, REQ_PASSKEY, JUST_WORKS, CFM_PASSKEY },
	{ JUST_WORKS,  JUST_CFM,    JUST_WORKS,  JUST_WORKS, JUST_CFM    },
	{ CFM_PASSKEY, CFM_PASSKEY, REQ_PASSKEY, JUST_WORKS, OVERLAP     },
};

static const uint8_t sc_method[5][5] = {
	{ JUST_WORKS,  JUST_CFM,    REQ_PASSKEY, JUST_WORKS, REQ_PASSKEY },
	{ JUST_WORKS,  CFM_PASSKEY, REQ_PASSKEY, JUST_WORKS, CFM_PASSKEY },
	{ DSP_PASSKEY, DSP_PASSKEY, REQ_PASSKEY, JUST_WORKS, DSP_PASSKEY },
	{ JUST_WORKS,  JUST_CFM,    JUST_WORKS,  JUST_WORKS, JUST_CFM    },
	{ DSP_PASSKEY, CFM_PASSKEY, REQ_PASSKEY, JUST_WORKS, CFM_PASSKEY },
};

static uint8_t get_auth_method(struct smp_conn *conn, uint8_t local_io,
							uint8_t remote_io)
{
	/* If either side has unknown io_caps, use JUST_CFM (which gets
	 * converted later to JUST_WORKS if we're initiators.
	 */
	if (local_io > MAX_IO_CAP || remote_io > MAX_IO_CAP)
		return JUST_CFM;

	if (conn->sc)
		return sc_method[remote_io][local_io];

	return gen_method[remote_io][local_io];
}

static uint8_t sc_select_method(struct smp_conn *conn)
{
	struct bt_l2cap_smp_pairing_request *local, *remote;
	uint8_t local_mitm, remote_mitm, local_io, remote_io, method;

	if (conn->out) {
		local = (void *) &conn->preq[1];
		remote = (void *) &conn->prsp[1];
	} else {
		local = (void *) &conn->prsp[1];
		remote = (void *) &conn->preq[1];
	}

	local_io = local->io_capa;
	remote_io = remote->io_capa;

	local_mitm = (local->auth_req & SMP_AUTH_MITM);
	remote_mitm = (remote->auth_req & SMP_AUTH_MITM);

	/* If either side wants MITM, look up the method from the table,
	 * otherwise use JUST WORKS.
	 */
	if (local_mitm || remote_mitm)
		method = get_auth_method(conn, local_io, remote_io);
	else
		method = JUST_WORKS;

	/* Don't confirm locally initiated pairing attempts */
	if (method == JUST_CFM && conn->initiator)
		method = JUST_WORKS;

	return method;
}

static void smp_send(struct smp_conn *conn, uint8_t smp_cmd, const void *data,
								uint8_t len)
{
	struct iovec iov[2];
	uint16_t cid;

	iov[0].iov_base = &smp_cmd;
	iov[0].iov_len = 1;

	iov[1].iov_base = (void *) data;
	iov[1].iov_len = len;

	if (conn->addr_type == BDADDR_BREDR)
		cid = SMP_BREDR_CID;
	else
		cid = SMP_CID;

	bthost_send_cid_v(conn->smp->bthost, conn->handle, cid, iov, 2);
}

static bool send_public_key(struct smp_conn *conn)
{
	if (!ecc_make_key(conn->local_pk, conn->local_sk))
		return false;

	smp_send(conn, BT_L2CAP_SMP_PUBLIC_KEY, conn->local_pk, 64);

	return true;
}

static void sc_dhkey_check(struct smp_conn *conn)
{
	uint8_t io_cap[3], r[16], a[7], b[7], *local_addr, *remote_addr;
	struct bt_l2cap_smp_dhkey_check check;

	memcpy(a, conn->ia, 6);
	memcpy(b, conn->ra, 6);
	a[6] = conn->ia_type;
	b[6] = conn->ra_type;

	if (conn->out) {
		local_addr = a;
		remote_addr = b;
		memcpy(io_cap, &conn->preq[1], 3);
	} else {
		local_addr = b;
		remote_addr = a;
		memcpy(io_cap, &conn->prsp[1], 3);
	}

	memset(r, 0, sizeof(r));

	bt_crypto_f6(conn->smp->crypto, conn->mackey, conn->prnd, conn->rrnd,
				r, io_cap, local_addr, remote_addr, check.e);

	smp_send(conn, BT_L2CAP_SMP_DHKEY_CHECK, &check, sizeof(check));
}

static void sc_mackey_and_ltk(struct smp_conn *conn)
{
	uint8_t *na, *nb, a[7], b[7];

	if (conn->out) {
		na = conn->prnd;
		nb = conn->rrnd;
	} else {
		na = conn->rrnd;
		nb = conn->prnd;
	}

	memcpy(a, conn->ia, 6);
	memcpy(b, conn->ra, 6);
	a[6] = conn->ia_type;
	b[6] = conn->ra_type;

	bt_crypto_f5(conn->smp->crypto, conn->dhkey, na, nb, a, b,
						conn->mackey, conn->ltk);
}

static uint8_t sc_passkey_send_confirm(struct smp_conn *conn)
{
	struct bt_l2cap_smp_pairing_confirm cfm;
	uint8_t r;

	r = ((conn->passkey_notify >> conn->passkey_round) & 0x01);
	r |= 0x80;

	if (!bt_crypto_f4(conn->smp->crypto, conn->local_pk, conn->remote_pk,
					conn->prnd, r, cfm.value))
		return SMP_UNSPECIFIED;

	smp_send(conn, BT_L2CAP_SMP_PAIRING_CONFIRM, &cfm, sizeof(cfm));

	return 0;
}

static uint8_t sc_passkey_round(struct smp_conn *conn, uint8_t smp_op)
{
	uint8_t cfm[16], r;

	/* Ignore the PDU if we've already done 20 rounds (0 - 19) */
	if (conn->passkey_round >= 20)
		return 0;

	switch (smp_op) {
	case BT_L2CAP_SMP_PAIRING_RANDOM:
		r = ((conn->passkey_notify >> conn->passkey_round) & 0x01);
		r |= 0x80;

		if (!bt_crypto_f4(conn->smp->crypto, conn->remote_pk,
					conn->local_pk, conn->rrnd, r, cfm))
			return SMP_UNSPECIFIED;

		if (memcmp(conn->pcnf, cfm, 16))
			return SMP_CONFIRM_FAILED;

		conn->passkey_round++;

		if (conn->passkey_round == 20) {
			/* Generate MacKey and LTK */
			sc_mackey_and_ltk(conn);
		}

		/* The round is only complete when the initiator
		 * receives pairing random.
		 */
		if (!conn->out) {
			smp_send(conn, BT_L2CAP_SMP_PAIRING_RANDOM,
					conn->prnd, sizeof(conn->prnd));
			return 0;
		}

		/* Start the next round */
		if (conn->passkey_round != 20)
			return sc_passkey_round(conn, 0);

		/* Passkey rounds are complete - start DHKey Check */
		sc_dhkey_check(conn);

		break;

	case BT_L2CAP_SMP_PAIRING_CONFIRM:
		if (conn->out) {
			smp_send(conn, BT_L2CAP_SMP_PAIRING_RANDOM,
					conn->prnd, sizeof(conn->prnd));
			return 0;
		}

		return sc_passkey_send_confirm(conn);

	case BT_L2CAP_SMP_PUBLIC_KEY:
	default:
		/* Initiating device starts the round */
		if (!conn->out)
			return 0;

		return sc_passkey_send_confirm(conn);
	}

	return 0;
}

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 distribute_keys(struct smp_conn *conn)
{
	uint8_t buf[16];

	if (conn->local_key_dist & DIST_ENC_KEY) {
		memset(buf, 0, sizeof(buf));
		smp_send(conn, BT_L2CAP_SMP_ENCRYPT_INFO, buf, sizeof(buf));
		smp_send(conn, BT_L2CAP_SMP_MASTER_IDENT, buf, 10);
	}

	if (conn->local_key_dist & DIST_ID_KEY) {
		memset(buf, 0, sizeof(buf));
		smp_send(conn, BT_L2CAP_SMP_IDENT_INFO, buf, sizeof(buf));

		memset(buf, 0, sizeof(buf));

		if (conn->out) {
			buf[0] = conn->ia_type;
			memcpy(&buf[1], conn->ia, 6);
		} else {
			buf[0] = conn->ra_type;
			memcpy(&buf[1], conn->ra, 6);
		}

		smp_send(conn, BT_L2CAP_SMP_IDENT_ADDR_INFO, buf, 7);
	}

	if (conn->local_key_dist & DIST_SIGN) {
		memset(buf, 0, sizeof(buf));
		smp_send(conn, BT_L2CAP_SMP_SIGNING_INFO, buf, sizeof(buf));
	}
}

static void pairing_req(struct smp_conn *conn, const void *data, uint16_t len)
{
	struct bthost *bthost = conn->smp->bthost;
	struct bt_l2cap_smp_pairing_response rsp;

	memcpy(conn->preq, data, sizeof(conn->preq));

	if (conn->addr_type == BDADDR_BREDR) {
		rsp.io_capa	= 0x00;
		rsp.oob_data	= 0x00;
		rsp.auth_req	= 0x00;
	} else {
		rsp.io_capa	= bthost_get_io_capability(bthost);
		rsp.oob_data	= 0x00;
		rsp.auth_req	= bthost_get_auth_req(bthost);
	}

	rsp.max_key_size	= 0x10;
	rsp.init_key_dist	= conn->preq[5] & KEY_DIST;
	rsp.resp_key_dist	= conn->preq[6] & KEY_DIST;

	conn->prsp[0] = BT_L2CAP_SMP_PAIRING_RESPONSE;
	memcpy(&conn->prsp[1], &rsp, sizeof(rsp));

	conn->local_key_dist	= rsp.resp_key_dist;
	conn->remote_key_dist	= rsp.init_key_dist;

	if (((conn->prsp[3] & 0x08) && (conn->preq[3] & 0x08)) ||
					conn->addr_type == BDADDR_BREDR) {
		conn->sc = true;
		conn->local_key_dist &= ~SC_NO_DIST;
		conn->remote_key_dist &= ~SC_NO_DIST;
	}

	smp_send(conn, BT_L2CAP_SMP_PAIRING_RESPONSE, &rsp, sizeof(rsp));

	if (conn->addr_type == BDADDR_BREDR)
		distribute_keys(conn);
}

static void pairing_rsp(struct smp_conn *conn, const void *data, uint16_t len)
{
	struct smp *smp = conn->smp;
	uint8_t cfm[16];

	memcpy(conn->prsp, data, sizeof(conn->prsp));

	conn->local_key_dist = conn->prsp[5];
	conn->remote_key_dist = conn->prsp[6];

	if (conn->addr_type == BDADDR_BREDR) {
		conn->local_key_dist &= ~SC_NO_DIST;
		conn->remote_key_dist &= ~SC_NO_DIST;
		distribute_keys(conn);
		return;
	}

	if (((conn->prsp[3] & 0x08) && (conn->preq[3] & 0x08)) ||
					conn->addr_type == BDADDR_BREDR) {
		conn->sc = true;
		conn->local_key_dist &= ~SC_NO_DIST;
		conn->remote_key_dist &= ~SC_NO_DIST;
		if (conn->addr_type == BDADDR_BREDR)
			distribute_keys(conn);
		else
			send_public_key(conn);
		return;
	}

	bt_crypto_c1(smp->crypto, conn->tk, conn->prnd, conn->prsp,
			conn->preq, conn->ia_type, conn->ia,
			conn->ra_type, conn->ra, cfm);

	smp_send(conn, BT_L2CAP_SMP_PAIRING_CONFIRM, cfm, sizeof(cfm));
}
static void sc_check_confirm(struct smp_conn *conn)
{
	if (conn->method == REQ_PASSKEY || conn->method == DSP_PASSKEY) {
		sc_passkey_round(conn, BT_L2CAP_SMP_PAIRING_CONFIRM);
		return;
	}

	if (conn->out)
		smp_send(conn, BT_L2CAP_SMP_PAIRING_RANDOM, conn->prnd,
							sizeof(conn->prnd));
}

static void pairing_cfm(struct smp_conn *conn, const void *data, uint16_t len)
{
	uint8_t rsp[16];

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

	if (conn->sc) {
		sc_check_confirm(conn);
		return;
	}

	if (conn->out) {
		memset(rsp, 0, sizeof(rsp));
		smp_send(conn, BT_L2CAP_SMP_PAIRING_RANDOM, rsp, sizeof(rsp));
	} else {
		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);
		smp_send(conn, BT_L2CAP_SMP_PAIRING_CONFIRM, rsp, sizeof(rsp));
	}
}

static uint8_t sc_random(struct smp_conn *conn)
{
	/* Passkey entry has special treatment */
	if (conn->method == REQ_PASSKEY || conn->method == DSP_PASSKEY)
		return sc_passkey_round(conn, BT_L2CAP_SMP_PAIRING_RANDOM);

	if (conn->out) {
		uint8_t cfm[16];

		bt_crypto_f4(conn->smp->crypto, conn->remote_pk,
					conn->local_pk, conn->rrnd, 0, cfm);

		if (memcmp(conn->pcnf, cfm, 16))
			return 0x04; /* Confirm Value Failed */
	} else {
		smp_send(conn, BT_L2CAP_SMP_PAIRING_RANDOM, conn->prnd, 16);
	}

	sc_mackey_and_ltk(conn);

	if (conn->out)
		sc_dhkey_check(conn);

	return 0;
}

static void pairing_rnd(struct smp_conn *conn, const void *data, uint16_t len)
{
	uint8_t rsp[16];

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

	if (conn->sc) {
		uint8_t reason = sc_random(conn);
		if (reason)
			smp_send(conn, BT_L2CAP_SMP_PAIRING_FAILED, &reason,
							sizeof(reason));
		return;
	}

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

	if (conn->out)
		return;

	memset(rsp, 0, sizeof(rsp));
	smp_send(conn, BT_L2CAP_SMP_PAIRING_RANDOM, rsp, sizeof(rsp));
}

static void encrypt_info(struct smp_conn *conn, const void *data, uint16_t len)
{
}

static void master_ident(struct smp_conn *conn, const void *data, uint16_t len)
{
	conn->remote_key_dist &= ~DIST_ENC_KEY;

	if (conn->out && !conn->remote_key_dist)
		distribute_keys(conn);
}

static void ident_addr_info(struct smp_conn *conn, const void *data,
								uint16_t len)
{
}

static void ident_info(struct smp_conn *conn, const void *data, uint16_t len)
{
	conn->remote_key_dist &= ~DIST_ID_KEY;

	if (conn->out && !conn->remote_key_dist)
		distribute_keys(conn);
}

static void signing_info(struct smp_conn *conn, const void *data, uint16_t len)
{
	conn->remote_key_dist &= ~DIST_SIGN;

	if (conn->out && !conn->remote_key_dist)
		distribute_keys(conn);
}

static void public_key(struct smp_conn *conn, const void *data, uint16_t len)
{
	struct smp *smp = conn->smp;
	uint8_t buf[16];

	memcpy(conn->remote_pk, data + 1, 64);

	if (!conn->out) {
		if (!send_public_key(conn))
			return;
	}

	if (!ecdh_shared_secret(conn->remote_pk, conn->local_sk, conn->dhkey))
		return;

	conn->method = sc_select_method(conn);

	if (conn->method == DSP_PASSKEY || conn->method == REQ_PASSKEY) {
		sc_passkey_round(conn, BT_L2CAP_SMP_PUBLIC_KEY);
		return;
	}

	if (conn->out)
		return;

	if (!bt_crypto_f4(smp->crypto, conn->local_pk, conn->remote_pk,
							conn->prnd, 0, buf))
		return;

	smp_send(conn, BT_L2CAP_SMP_PAIRING_CONFIRM, buf, sizeof(buf));
}

static void dhkey_check(struct smp_conn *conn, const void *data, uint16_t len)
{
	const struct bt_l2cap_smp_dhkey_check *cmd = data + 1;
	uint8_t a[7], b[7], *local_addr, *remote_addr;
	uint8_t io_cap[3], r[16], e[16];

	memcpy(a, &conn->ia, 6);
	memcpy(b, &conn->ra, 6);
	a[6] = conn->ia_type;
	b[6] = conn->ra_type;

	if (conn->out) {
		local_addr = a;
		remote_addr = b;
		memcpy(io_cap, &conn->prsp[1], 3);
	} else {
		local_addr = b;
		remote_addr = a;
		memcpy(io_cap, &conn->preq[1], 3);
	}

	memset(r, 0, sizeof(r));

	if (conn->method == REQ_PASSKEY || conn->method == DSP_PASSKEY)
		put_le32(conn->passkey_notify, r);

	if (!bt_crypto_f6(conn->smp->crypto, conn->mackey, conn->rrnd,
			conn->prnd, r, io_cap, remote_addr, local_addr, e))
		return;

	if (memcmp(cmd->e, e, 16)) {
		uint8_t reason = 0x0b; /* DHKey Check Failed */
		smp_send(conn, BT_L2CAP_SMP_PAIRING_FAILED, &reason,
							sizeof(reason));
	}

	if (conn->out)
		bthost_le_start_encrypt(conn->smp->bthost, conn->handle,
								conn->ltk);
	else
		sc_dhkey_check(conn);
}

void smp_pair(void *conn_data, uint8_t io_cap, uint8_t auth_req)
{
	struct smp_conn *conn = conn_data;
	struct bt_l2cap_smp_pairing_request req;

	req.io_capa		= io_cap;
	req.oob_data		= 0x00;
	req.auth_req		= auth_req;
	req.max_key_size	= 0x10;
	req.init_key_dist	= KEY_DIST;
	req.resp_key_dist	= KEY_DIST;

	conn->preq[0] = BT_L2CAP_SMP_PAIRING_REQUEST;
	memcpy(&conn->preq[1], &req, sizeof(req));

	smp_send(conn, BT_L2CAP_SMP_PAIRING_REQUEST, &req, sizeof(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;
	}

	if (conn->addr_type == BDADDR_BREDR) {
		printf("Received BR/EDR SMP data on LE link\n");
		return;
	}

	opcode = *((const uint8_t *) data);

	switch (opcode) {
	case BT_L2CAP_SMP_PAIRING_REQUEST:
		pairing_req(conn, data, len);
		break;
	case BT_L2CAP_SMP_PAIRING_RESPONSE:
		pairing_rsp(conn, data, len);
		break;
	case BT_L2CAP_SMP_PAIRING_CONFIRM:
		pairing_cfm(conn, data, len);
		break;
	case BT_L2CAP_SMP_PAIRING_RANDOM:
		pairing_rnd(conn, data, len);
		break;
	case BT_L2CAP_SMP_ENCRYPT_INFO:
		encrypt_info(conn, data, len);
		break;
	case BT_L2CAP_SMP_MASTER_IDENT:
		master_ident(conn, data, len);
		break;
	case BT_L2CAP_SMP_IDENT_ADDR_INFO:
		ident_addr_info(conn, data, len);
		break;
	case BT_L2CAP_SMP_IDENT_INFO:
		ident_info(conn, data, len);
		break;
	case BT_L2CAP_SMP_SIGNING_INFO:
		signing_info(conn, data, len);
		break;
	case BT_L2CAP_SMP_PUBLIC_KEY:
		public_key(conn, data, len);
		break;
	case BT_L2CAP_SMP_DHKEY_CHECK:
		dhkey_check(conn, data, len);
		break;
	default:
		break;
	}
}

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

	if (conn->addr_type != BDADDR_BREDR) {
		printf("Received LE SMP data on BR/EDR link\n");
		return;
	}

	opcode = *((const uint8_t *) data);

	switch (opcode) {
	case BT_L2CAP_SMP_PAIRING_REQUEST:
		pairing_req(conn, data, len);
		break;
	case BT_L2CAP_SMP_PAIRING_RESPONSE:
		pairing_rsp(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;
}

static void smp_conn_bredr(struct smp_conn *conn, uint8_t encrypt)
{
	struct smp *smp = conn->smp;
	struct bt_l2cap_smp_pairing_request req;
	uint64_t fixed_chan;

	if (encrypt != 0x02)
		return;

	conn->sc = true;

	if (!conn->out)
		return;

	fixed_chan = bthost_conn_get_fixed_chan(smp->bthost, conn->handle);
	if (!(fixed_chan & L2CAP_FC_SMP_BREDR))
		return;

	memset(&req, 0, sizeof(req));
	req.max_key_size = 0x10;
	req.init_key_dist = KEY_DIST;
	req.resp_key_dist = KEY_DIST;

	smp_send(conn, BT_L2CAP_SMP_PAIRING_REQUEST, &req, sizeof(req));
}

void smp_conn_encrypted(void *conn_data, uint8_t encrypt)
{
	struct smp_conn *conn = conn_data;

	if (!encrypt)
		return;

	if (conn->addr_type == BDADDR_BREDR) {
		smp_conn_bredr(conn, encrypt);
		return;
	}

	if (conn->out && conn->remote_key_dist)
		return;

	distribute_keys(conn);
}

void *smp_conn_add(void *smp_data, uint16_t handle, const uint8_t *ia,
			const uint8_t *ra, uint8_t addr_type, 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->addr_type = addr_type;
	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);
}