From 4003a333dc4484dd68671ffecda894f57149ff07 Mon Sep 17 00:00:00 2001
From: Johan Hedberg <johan.hedberg@intel.com>
Date: Wed, 7 Jan 2015 12:16:11 +0200
Subject: [PATCH] emulator/bthost: Add support for triggering SMP over BR/EDR

---
 emulator/bthost.c | 13 +++++++++++++
 emulator/bthost.h |  2 ++
 emulator/smp.c    | 37 +++++++++++++++++++++++++++++++++++++
 3 files changed, 52 insertions(+)

diff --git a/emulator/bthost.c b/emulator/bthost.c
index 7792a71f8..6c616e8f4 100644
--- a/emulator/bthost.c
+++ b/emulator/bthost.c
@@ -1522,6 +1522,8 @@ static bool l2cap_info_rsp(struct bthost *bthost, struct btconn *conn,
 		if (len < sizeof(*rsp) + 8)
 			return false;
 		conn->fixed_chan = get_le64(rsp->data);
+		if (conn->smp_data && conn->encr_mode)
+			smp_conn_encrypted(conn->smp_data, conn->encr_mode);
 		break;
 	default:
 		break;
@@ -2318,6 +2320,17 @@ void bthost_le_start_encrypt(struct bthost *bthost, uint16_t handle,
 	send_command(bthost, BT_HCI_CMD_LE_START_ENCRYPT, &cmd, sizeof(cmd));
 }
 
+uint64_t bthost_conn_get_fixed_chan(struct bthost *bthost, uint16_t handle)
+{
+	struct btconn *conn;
+
+	conn = bthost_find_conn(bthost, handle);
+	if (!conn)
+		return 0;
+
+	return conn->fixed_chan;
+}
+
 void bthost_add_l2cap_server(struct bthost *bthost, uint16_t psm,
 				bthost_l2cap_connect_cb func, void *user_data)
 {
diff --git a/emulator/bthost.h b/emulator/bthost.h
index 207ce77fb..6beb1f487 100644
--- a/emulator/bthost.h
+++ b/emulator/bthost.h
@@ -107,6 +107,8 @@ uint8_t bthost_get_auth_req(struct bthost *bthost);
 void bthost_set_reject_user_confirm(struct bthost *bthost, bool reject);
 bool bthost_get_reject_user_confirm(struct bthost *bthost);
 
+uint64_t bthost_conn_get_fixed_chan(struct bthost *bthost, uint16_t handle);
+
 typedef void (*bthost_rfcomm_connect_cb) (uint16_t handle, uint16_t cid,
 						void *user_data, bool status);
 
diff --git a/emulator/smp.c b/emulator/smp.c
index 5d338b7e0..4f527e990 100644
--- a/emulator/smp.c
+++ b/emulator/smp.c
@@ -47,6 +47,8 @@
 #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
@@ -462,6 +464,13 @@ static void pairing_rsp(struct smp_conn *conn, const void *data, uint16_t len)
 	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;
@@ -788,6 +797,29 @@ int smp_get_ltk(void *smp_data, uint64_t rand, uint16_t ediv, uint8_t *ltk)
 	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;
+
+	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;
@@ -795,6 +827,11 @@ void smp_conn_encrypted(void *conn_data, uint8_t encrypt)
 	if (!encrypt)
 		return;
 
+	if (conn->addr_type == BDADDR_BREDR) {
+		smp_conn_bredr(conn, encrypt);
+		return;
+	}
+
 	if (conn->out && conn->remote_key_dist)
 		return;
 
-- 
2.47.3