[PATCH v2 1/3] btdev: Add support for multiple connections

[PATCH v2 1/3] btdev: Add support for multiple connections

[Luiz Augusto von Dentz]

From: Luiz Augusto von Dentz <luiz.von.dentz@intel.com>

This adds support for assigning different handles for connection
instead of always using fixed handles.
---
 emulator/btdev.c | 930 +++++++++++++++++++++++++++++++----------------
 lib/hci.h        |   1 +
 2 files changed, 610 insertions(+), 321 deletions(-)

diff --git a/emulator/btdev.c b/emulator/btdev.c
index a26d73865..ae382d9b9 100644
--- a/emulator/btdev.c
+++ b/emulator/btdev.c
@@ -29,6 +29,7 @@
 #include "src/shared/timeout.h"
 #include "src/shared/crypto.h"
 #include "src/shared/ecc.h"
+#include "src/shared/queue.h"
 #include "monitor/bt.h"
 #include "btdev.h"
 
@@ -36,7 +37,7 @@
 #define has_le(btdev)		(!!((btdev)->features[4] & 0x40))
 
 #define ACL_HANDLE 42
-#define ISO_HANDLE 44
+#define ISO_HANDLE 257
 #define SCO_HANDLE 257
 
 struct hook {
@@ -48,10 +49,17 @@ struct hook {
 
 #define MAX_HOOK_ENTRIES 16
 
+struct btdev_conn {
+	uint16_t handle;
+	uint8_t  type;
+	struct btdev *dev;
+	struct btdev_conn *link;
+};
+
 struct btdev {
 	enum btdev_type type;
 
-	struct btdev *conn;
+	struct queue *conns;
 
 	bool auth_init;
 	uint8_t link_key[16];
@@ -520,36 +528,70 @@ static void send_event(struct btdev *btdev, uint8_t event,
 		send_packet(btdev, iov, len > 0 ? 3 : 2);
 }
 
+static bool match_handle(const void *data, const void *match_data)
+{
+	const struct btdev_conn *conn = data;
+	uint16_t handle = PTR_TO_UINT(match_data);
+
+	return conn->handle == handle;
+}
+
+static void conn_unlink(struct btdev_conn *conn1, struct btdev_conn *conn2)
+{
+	conn1->link = NULL;
+	conn2->link = NULL;
+}
+
+static void conn_remove(struct btdev_conn *conn)
+{
+	if (conn->link) {
+		struct btdev_conn *link = conn->link;
+
+		conn_unlink(conn, conn->link);
+		conn_remove(link);
+	}
+
+	queue_remove(conn->dev->conns, conn);
+
+	free(conn);
+}
+
+static void disconnect_complete(struct btdev *dev, uint16_t handle,
+					uint8_t status, uint8_t reason)
+{
+	struct bt_hci_evt_disconnect_complete rsp;
+
+	memset(&rsp, 0, sizeof(rsp));
+
+	rsp.status = status;
+	rsp.handle = cpu_to_le16(handle);
+	rsp.reason = reason;
+
+	send_event(dev, BT_HCI_EVT_DISCONNECT_COMPLETE, &rsp, sizeof(rsp));
+}
+
 static int cmd_disconnect_complete(struct btdev *dev, const void *data,
 						uint8_t len)
 {
 	const struct bt_hci_cmd_disconnect *cmd = data;
 	struct bt_hci_evt_disconnect_complete rsp;
-	struct btdev *remote = dev->conn;
+	struct btdev_conn *conn;
 
 	memset(&rsp, 0, sizeof(rsp));
 
-	if (!remote) {
-		rsp.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
-		rsp.handle = cpu_to_le16(cmd->handle);
-		rsp.reason = 0x00;
-
-		send_event(dev, BT_HCI_EVT_DISCONNECT_COMPLETE, &rsp,
-						sizeof(rsp));
+	conn = queue_remove_if(dev->conns, match_handle,
+				UINT_TO_PTR(cpu_to_le16(cmd->handle)));
+	if (!conn) {
+		disconnect_complete(dev, 0x0000, BT_HCI_ERR_UNKNOWN_CONN_ID,
+								0x00);
 		return 0;
 	}
 
-	rsp.status = BT_HCI_ERR_SUCCESS;
-	rsp.handle = cpu_to_le16(cmd->handle);
-	rsp.reason = cmd->reason;
+	disconnect_complete(dev, conn->handle, BT_HCI_ERR_SUCCESS, cmd->reason);
+	disconnect_complete(conn->link->dev, conn->link->handle,
+				BT_HCI_ERR_SUCCESS, cmd->reason);
 
-	if (rsp.handle == ACL_HANDLE) {
-		dev->conn = NULL;
-		remote->conn = NULL;
-	}
-
-	send_event(dev, BT_HCI_EVT_DISCONNECT_COMPLETE, &rsp, sizeof(rsp));
-	send_event(remote, BT_HCI_EVT_DISCONNECT_COMPLETE, &rsp, sizeof(rsp));
+	conn_remove(conn);
 
 	return 0;
 }
@@ -566,15 +608,18 @@ static int cmd_remote_version_complete(struct btdev *dev, const void *data,
 {
 	const struct bt_hci_cmd_read_remote_version *cmd = data;
 	struct bt_hci_evt_remote_version_complete ev;
+	struct btdev_conn *conn;
 
 	memset(&ev, 0, sizeof(ev));
 
-	if (dev->conn) {
+	conn = queue_find(dev->conns, match_handle,
+				UINT_TO_PTR(cpu_to_le16(cmd->handle)));
+	if (conn) {
 		ev.status = BT_HCI_ERR_SUCCESS;
 		ev.handle = cpu_to_le16(cmd->handle);
-		ev.lmp_ver = dev->conn->version;
-		ev.manufacturer = cpu_to_le16(dev->conn->manufacturer);
-		ev.lmp_subver = cpu_to_le16(dev->conn->revision);
+		ev.lmp_ver = conn->link->dev->version;
+		ev.manufacturer = cpu_to_le16(conn->link->dev->manufacturer);
+		ev.lmp_subver = cpu_to_le16(conn->link->dev->revision);
 	} else {
 		ev.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
 		ev.handle = cpu_to_le16(cmd->handle);
@@ -860,27 +905,105 @@ static int cmd_create_conn(struct btdev *dev, const void *data, uint8_t len)
 	return 0;
 }
 
+static struct btdev_conn *conn_new(struct btdev *dev, uint16_t handle,
+							uint8_t type)
+{
+	struct btdev_conn *conn;
+
+	while ((conn = queue_find(dev->conns, match_handle,
+					UINT_TO_PTR(handle))))
+		handle++;
+
+	conn = new0(struct btdev_conn, 1);
+	conn->handle = handle;
+	conn->type = type;
+	conn->dev = dev;
+
+	if (!queue_push_tail(dev->conns, conn)) {
+		free(conn);
+		return NULL;
+	}
+
+	return conn;
+}
+
+static struct btdev_conn *conn_link(struct btdev *dev, struct btdev *remote,
+					uint16_t handle, uint8_t type)
+{
+	struct btdev_conn *conn1, *conn2;
+
+	conn1 = conn_new(dev, handle, type);
+	if (!conn1)
+		return NULL;
+
+	conn2 = conn_new(remote, handle, type);
+	if (!conn2) {
+		free(conn1);
+		return NULL;
+	}
+
+	conn1->link = conn2;
+	conn2->link = conn1;
+
+	util_debug(dev->debug_callback, dev->debug_data,
+				"conn1 %p handle 0x%04x conn2 %p handle 0x%04x",
+				conn1, conn1->handle, conn2, conn2->handle);
+
+	return conn1;
+}
+
+static struct btdev_conn *conn_add(struct btdev *dev,
+				const uint8_t *bdaddr, uint8_t bdaddr_type,
+				uint16_t handle, uint8_t type)
+{
+	struct btdev *remote;
+
+	remote = find_btdev_by_bdaddr_type(bdaddr, bdaddr_type);
+	if (!remote)
+		return NULL;
+
+	return conn_link(dev, remote, handle, type);
+}
+
+static struct btdev_conn *conn_add_acl(struct btdev *dev,
+				const uint8_t *bdaddr, uint8_t bdaddr_type)
+{
+	return conn_add(dev, bdaddr, bdaddr_type, ACL_HANDLE, HCI_ACLDATA_PKT);
+}
+
+static struct btdev_conn *conn_add_sco(struct btdev_conn *acl)
+{
+	return conn_link(acl->dev, acl->link->dev, SCO_HANDLE, HCI_SCODATA_PKT);
+}
+
+static struct btdev_conn *conn_add_iso(struct btdev_conn *acl, uint16_t handle)
+{
+	return conn_link(acl->dev, acl->link->dev, handle, HCI_ISODATA_PKT);
+}
+
 static void conn_complete(struct btdev *btdev,
 					const uint8_t *bdaddr, uint8_t status)
 {
 	struct bt_hci_evt_conn_complete cc;
 
 	if (!status) {
-		struct btdev *remote = find_btdev_by_bdaddr(bdaddr);
+		struct btdev_conn *conn;
 
-		btdev->conn = remote;
-		remote->conn = btdev;
+		conn = conn_add_acl(btdev, bdaddr, BDADDR_BREDR);
+		if (!conn)
+			return;
 
 		cc.status = status;
 		memcpy(cc.bdaddr, btdev->bdaddr, 6);
 		cc.encr_mode = 0x00;
 
-		cc.handle = cpu_to_le16(ACL_HANDLE);
+		cc.handle = cpu_to_le16(conn->link->handle);
 		cc.link_type = 0x01;
 
-		send_event(remote, BT_HCI_EVT_CONN_COMPLETE, &cc, sizeof(cc));
+		send_event(conn->link->dev, BT_HCI_EVT_CONN_COMPLETE, &cc,
+						sizeof(cc));
 
-		cc.handle = cpu_to_le16(ACL_HANDLE);
+		cc.handle = cpu_to_le16(conn->handle);
 		cc.link_type = 0x01;
 	} else {
 		cc.handle = cpu_to_le16(0x0000);
@@ -917,17 +1040,32 @@ static int cmd_create_conn_complete(struct btdev *dev, const void *data,
 
 static int cmd_add_sco_conn(struct btdev *dev, const void *data, uint8_t len)
 {
+	const struct bt_hci_cmd_add_sco_conn *cmd = data;
 	struct bt_hci_evt_conn_complete cc;
+	struct btdev_conn *conn;
 
-	if (!dev->conn)
-		return 0;
+	memset(&cc, 0, sizeof(cc));
+
+	conn = queue_find(dev->conns, match_handle,
+				UINT_TO_PTR(cpu_to_le16(cmd->handle)));
+	if (!conn) {
+		cc.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
+		goto done;
+	}
+
+	conn = conn_add_sco(conn);
+	if (!conn) {
+		cc.status = BT_HCI_ERR_MEM_CAPACITY_EXCEEDED;
+		goto done;
+	}
 
 	cc.status = BT_HCI_ERR_SUCCESS;
-	memcpy(cc.bdaddr, dev->conn->bdaddr, 6);
-	cc.handle = cpu_to_le16(SCO_HANDLE);
+	memcpy(cc.bdaddr, conn->link->dev->bdaddr, 6);
+	cc.handle = cpu_to_le16(conn->handle);
 	cc.link_type = 0x00;
 	cc.encr_mode = 0x00;
 
+done:
 	send_event(dev, BT_HCI_EVT_CONN_COMPLETE, &cc, sizeof(cc));
 
 	return 0;
@@ -1004,35 +1142,57 @@ static int cmd_link_key_reply(struct btdev *dev, const void *data, uint8_t len)
 	return 0;
 }
 
+static bool match_bdaddr(const void *data, const void *match_data)
+{
+	const struct btdev_conn *conn = data;
+	const uint8_t *bdaddr = match_data;
+
+	return !memcmp(conn->link->dev->bdaddr, bdaddr, 6);
+}
+
+static void auth_complete(struct btdev_conn *conn, uint8_t status)
+{
+	struct bt_hci_evt_auth_complete ev;
+
+	memset(&ev, 0, sizeof(ev));
+
+	ev.handle = conn ? cpu_to_le16(conn->handle) : 0x0000;
+	ev.status = status;
+
+	send_event(conn->dev, BT_HCI_EVT_AUTH_COMPLETE, &ev, sizeof(ev));
+}
+
 static int cmd_link_key_reply_complete(struct btdev *dev, const void *data,
 						uint8_t len)
 {
 	const struct bt_hci_cmd_link_key_request_reply *cmd = data;
-	struct btdev *remote = dev->conn;
-	struct bt_hci_evt_auth_complete ev;
-
-	memcpy(dev->link_key, cmd->link_key, 16);
+	struct btdev_conn *conn;
+	uint8_t status;
 
-	if (!remote) {
-		remote = find_btdev_by_bdaddr(cmd->bdaddr);
-		if (!remote)
-			return 0;
+	conn = queue_find(dev->conns, match_bdaddr, cmd->bdaddr);
+	if (!conn) {
+		status = BT_HCI_ERR_INVALID_PARAMETERS;
+		goto done;
 	}
 
-	if (!memcmp(remote->link_key, LINK_KEY_NONE, 16)) {
-		send_event(remote, BT_HCI_EVT_LINK_KEY_REQUEST, dev->bdaddr, 6);
+	memcpy(dev->link_key, cmd->link_key, 16);
+
+	if (!memcmp(conn->link->dev->link_key, LINK_KEY_NONE, 16)) {
+		send_event(conn->link->dev, BT_HCI_EVT_LINK_KEY_REQUEST,
+					dev->bdaddr, 6);
 		return 0;
 	}
 
-	ev.handle = cpu_to_le16(ACL_HANDLE);
-
-	if (!memcmp(dev->link_key, remote->link_key, 16))
-		ev.status = BT_HCI_ERR_SUCCESS;
+	if (!memcmp(dev->link_key, conn->link->dev->link_key, 16))
+		status = BT_HCI_ERR_SUCCESS;
 	else
-		ev.status = BT_HCI_ERR_AUTH_FAILURE;
+		status = BT_HCI_ERR_AUTH_FAILURE;
+
+done:
+	auth_complete(conn, status);
 
-	send_event(dev, BT_HCI_EVT_AUTH_COMPLETE, &ev, sizeof(ev));
-	send_event(remote, BT_HCI_EVT_AUTH_COMPLETE, &ev, sizeof(ev));
+	if (conn)
+		auth_complete(conn->link, status);
 
 	return 0;
 }
@@ -1063,13 +1223,11 @@ static int cmd_link_key_neg_reply_complete(struct btdev *dev, const void *data,
 							uint8_t len)
 {
 	const struct bt_hci_cmd_link_key_request_neg_reply *cmd = data;
-	struct btdev *remote = dev->conn;
+	struct btdev *remote;
 
-	if (!remote) {
-		remote = find_btdev_by_bdaddr(cmd->bdaddr);
-		if (!remote)
-			return 0;
-	}
+	remote = find_btdev_by_bdaddr(cmd->bdaddr);
+	if (!remote)
+		return 0;
 
 	if (use_ssp(dev, remote)) {
 		struct bt_hci_evt_io_capability_request io_req;
@@ -1100,21 +1258,23 @@ static int cmd_pin_code_reply(struct btdev *dev, const void *data, uint8_t len)
 	return 0;
 }
 
-static uint8_t get_link_key_type(struct btdev *btdev)
+static uint8_t get_link_key_type(struct btdev *btdev, const uint8_t *bdaddr)
 {
-	struct btdev *remote = btdev->conn;
+	struct btdev_conn *conn;
 	uint8_t auth, unauth;
 
-	if (!remote)
+	conn = queue_find(btdev->conns, match_bdaddr, bdaddr);
+	if (!conn)
 		return 0x00;
 
 	if (!btdev->simple_pairing_mode)
 		return 0x00;
 
-	if (btdev->ssp_debug_mode || remote->ssp_debug_mode)
+	if (btdev->ssp_debug_mode || conn->link->dev->ssp_debug_mode)
 		return 0x03;
 
-	if (btdev->secure_conn_support && remote->secure_conn_support) {
+	if (btdev->secure_conn_support &&
+			conn->link->dev->secure_conn_support) {
 		unauth = 0x07;
 		auth = 0x08;
 	} else {
@@ -1122,18 +1282,18 @@ static uint8_t get_link_key_type(struct btdev *btdev)
 		auth = 0x05;
 	}
 
-	if (btdev->io_cap == 0x03 || remote->io_cap == 0x03)
+	if (btdev->io_cap == 0x03 || conn->link->dev->io_cap == 0x03)
 		return unauth;
 
-	if (!(btdev->auth_req & 0x01) && !(remote->auth_req & 0x01))
+	if (!(btdev->auth_req & 0x01) && !(conn->link->dev->auth_req & 0x01))
 		return unauth;
 
 	/* DisplayOnly only produces authenticated with KeyboardOnly */
-	if (btdev->io_cap == 0x00 && remote->io_cap != 0x02)
+	if (btdev->io_cap == 0x00 && conn->link->dev->io_cap != 0x02)
 		return unauth;
 
 	/* DisplayOnly only produces authenticated with KeyboardOnly */
-	if (remote->io_cap == 0x00 && btdev->io_cap != 0x02)
+	if (conn->link->dev->io_cap == 0x00 && btdev->io_cap != 0x02)
 		return unauth;
 
 	return auth;
@@ -1148,7 +1308,7 @@ static void link_key_notify(struct btdev *btdev, const uint8_t *bdaddr,
 
 	memcpy(ev.bdaddr, bdaddr, 6);
 	memcpy(ev.link_key, key, 16);
-	ev.key_type = get_link_key_type(btdev);
+	ev.key_type = get_link_key_type(btdev, bdaddr);
 
 	send_event(btdev, BT_HCI_EVT_LINK_KEY_NOTIFY, &ev, sizeof(ev));
 }
@@ -1157,14 +1317,17 @@ static int cmd_pin_code_reply_complete(struct btdev *dev, const void *data,
 							uint8_t len)
 {
 	const struct bt_hci_cmd_pin_code_request_reply *cmd = data;
-	struct bt_hci_evt_auth_complete ev;
-	struct btdev *remote = dev->conn;
+	struct btdev *remote;
+	struct btdev_conn *conn;
+	uint8_t status;
 
-	if (!remote) {
+	conn = queue_find(dev->conns, match_bdaddr, cmd->bdaddr);
+	if (!conn) {
 		remote = find_btdev_by_bdaddr(cmd->bdaddr);
 		if (!remote)
 			return 0;
-	}
+	} else
+		remote = conn->link->dev;
 
 	memcpy(dev->pin, cmd->pin_code, cmd->pin_len);
 	dev->pin_len = cmd->pin_len;
@@ -1173,8 +1336,8 @@ static int cmd_pin_code_reply_complete(struct btdev *dev, const void *data,
 		struct bt_hci_evt_pin_code_request pin_req;
 
 		memcpy(pin_req.bdaddr, dev->bdaddr, 6);
-		send_event(remote, BT_HCI_EVT_PIN_CODE_REQUEST, &pin_req,
-							sizeof(pin_req));
+		send_event(remote, BT_HCI_EVT_PIN_CODE_REQUEST,
+					&pin_req, sizeof(pin_req));
 		return 0;
 	}
 
@@ -1182,17 +1345,15 @@ static int cmd_pin_code_reply_complete(struct btdev *dev, const void *data,
 			!memcmp(dev->pin, remote->pin, dev->pin_len)) {
 		link_key_notify(dev, remote->bdaddr, LINK_KEY_DUMMY);
 		link_key_notify(remote, dev->bdaddr, LINK_KEY_DUMMY);
-		ev.status = BT_HCI_ERR_SUCCESS;
+		status = BT_HCI_ERR_SUCCESS;
 	} else {
-		ev.status = BT_HCI_ERR_AUTH_FAILURE;
+		status = BT_HCI_ERR_AUTH_FAILURE;
 	}
 
-	if (remote->conn) {
-		ev.handle = cpu_to_le16(ACL_HANDLE);
-		send_event(remote, BT_HCI_EVT_AUTH_COMPLETE, &ev, sizeof(ev));
-	} else {
-		conn_complete(remote, dev->bdaddr, ev.status);
-	}
+	if (conn)
+		auth_complete(conn->link, status);
+	else
+		conn_complete(remote, dev->bdaddr, status);
 
 	dev->pin_len = 0;
 	remote->pin_len = 0;
@@ -1218,27 +1379,25 @@ static int cmd_pin_code_neg_reply_complete(struct btdev *dev, const void *data,
 							uint8_t len)
 {
 	const struct bt_hci_cmd_pin_code_request_neg_reply *cmd = data;
-	struct bt_hci_evt_auth_complete ev;
-	struct btdev *remote = dev->conn;
+	struct btdev *remote;
+	struct btdev_conn *conn;
+	uint8_t status;
 
-	if (!remote) {
-		remote = find_btdev_by_bdaddr(cmd->bdaddr);
-		if (!remote)
-			return 0;
-	}
+	remote = find_btdev_by_bdaddr(cmd->bdaddr);
+	if (!remote)
+		return 0;
 
-	ev.status = BT_HCI_ERR_PIN_OR_KEY_MISSING;
-	ev.handle = cpu_to_le16(ACL_HANDLE);
+	status = BT_HCI_ERR_PIN_OR_KEY_MISSING;
 
-	if (dev->conn)
-		send_event(dev, BT_HCI_EVT_AUTH_COMPLETE, &ev, sizeof(ev));
+	conn = queue_find(dev->conns, match_bdaddr, cmd->bdaddr);
+	if (conn)
+		auth_complete(conn, status);
 	else
 		conn_complete(dev, cmd->bdaddr, BT_HCI_ERR_PIN_OR_KEY_MISSING);
 
-	if (remote->conn) {
+	if (conn) {
 		if (remote->pin_len)
-			send_event(remote, BT_HCI_EVT_AUTH_COMPLETE, &ev,
-								sizeof(ev));
+			auth_complete(conn->link, status);
 	} else {
 		conn_complete(remote, dev->bdaddr,
 					BT_HCI_ERR_PIN_OR_KEY_MISSING);
@@ -1258,9 +1417,11 @@ static int cmd_auth_requested_complete(struct btdev *dev, const void *data,
 						uint8_t len)
 {
 	const struct bt_hci_cmd_auth_requested *cmd = data;
-	struct btdev *remote = dev->conn;
+	struct btdev_conn *conn;
 
-	if (!remote) {
+	conn = queue_find(dev->conns, match_handle,
+				UINT_TO_PTR(le16_to_cpu(cmd->handle)));
+	if (!conn) {
 		struct bt_hci_evt_auth_complete ev;
 
 		ev.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
@@ -1273,7 +1434,8 @@ static int cmd_auth_requested_complete(struct btdev *dev, const void *data,
 
 	dev->auth_init = true;
 
-	send_event(dev, BT_HCI_EVT_LINK_KEY_REQUEST, remote->bdaddr, 6);
+	send_event(dev, BT_HCI_EVT_LINK_KEY_REQUEST, conn->link->dev->bdaddr,
+					sizeof(conn->link->dev->bdaddr));
 
 	return 0;
 }
@@ -1286,35 +1448,45 @@ static int cmd_set_conn_encrypt(struct btdev *dev, const void *data,
 	return 0;
 }
 
-static void encrypt_change(struct btdev *btdev, uint8_t mode, uint8_t status)
+static void encrypt_change(struct btdev_conn *conn, uint8_t mode,
+					uint8_t status)
 {
 	struct bt_hci_evt_encrypt_change ev;
 
+	if (!conn)
+		return;
+
+	memset(&ev, 0, sizeof(ev));
+
 	ev.status = status;
-	ev.handle = cpu_to_le16(ACL_HANDLE);
+	ev.handle = cpu_to_le16(conn->handle);
 	ev.encr_mode = mode;
 
-	send_event(btdev, BT_HCI_EVT_ENCRYPT_CHANGE, &ev, sizeof(ev));
+	send_event(conn->dev, BT_HCI_EVT_ENCRYPT_CHANGE, &ev, sizeof(ev));
 }
 
 static int cmd_set_conn_encrypt_complete(struct btdev *dev, const void *data,
 							uint8_t len)
 {
 	const struct bt_hci_cmd_set_conn_encrypt *cmd = data;
+	struct btdev_conn *conn;
 	uint8_t mode;
 
-	if (!dev->conn)
+	conn = queue_find(dev->conns, match_handle,
+				UINT_TO_PTR(le16_to_cpu(cmd->handle)));
+	if (!conn)
 		return 0;
 
 	if (!cmd->encr_mode)
 		mode = 0x00;
-	else if (dev->secure_conn_support && dev->conn->secure_conn_support)
+	else if (dev->secure_conn_support &&
+				conn->link->dev->secure_conn_support)
 		mode = 0x02;
 	else
 		mode = 0x01;
 
-	encrypt_change(dev, mode, BT_HCI_ERR_SUCCESS);
-	encrypt_change(dev->conn, mode, BT_HCI_ERR_SUCCESS);
+	encrypt_change(conn, mode, BT_HCI_ERR_SUCCESS);
+	encrypt_change(conn->link, mode, BT_HCI_ERR_SUCCESS);
 
 	return 0;
 }
@@ -1396,11 +1568,14 @@ static int cmd_read_remote_features_complete(struct btdev *dev,
 {
 	const struct bt_hci_cmd_read_remote_features *cmd = data;
 	struct bt_hci_evt_remote_features_complete rfc;
+	struct btdev_conn *conn;
 
-	if (dev->conn) {
+	conn = queue_find(dev->conns, match_handle,
+				UINT_TO_PTR(le16_to_cpu(cmd->handle)));
+	if (conn) {
 		rfc.status = BT_HCI_ERR_SUCCESS;
 		rfc.handle = cpu_to_le16(cmd->handle);
-		memcpy(rfc.features, dev->conn->features, 8);
+		memcpy(rfc.features, conn->link->dev->features, 8);
 	} else {
 		rfc.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
 		rfc.handle = cpu_to_le16(cmd->handle);
@@ -1439,10 +1614,13 @@ static int cmd_read_remote_ext_features_compl(struct btdev *dev,
 {
 	const struct bt_hci_cmd_read_remote_ext_features *cmd = data;
 	struct bt_hci_evt_remote_ext_features_complete ev;
+	struct btdev_conn *conn;
 
 	memset(&ev, 0, sizeof(ev));
 
-	if (dev->conn && cmd->page < 0x02) {
+	conn = queue_find(dev->conns, match_handle,
+				UINT_TO_PTR(le16_to_cpu(cmd->handle)));
+	if (conn && cmd->page < 0x02) {
 		ev.handle = cpu_to_le16(cmd->handle);
 		ev.page = cmd->page;
 		ev.max_page = 0x01;
@@ -1450,11 +1628,11 @@ static int cmd_read_remote_ext_features_compl(struct btdev *dev,
 		switch (cmd->page) {
 		case 0x00:
 			ev.status = BT_HCI_ERR_SUCCESS;
-			memcpy(ev.features, dev->conn->features, 8);
+			memcpy(ev.features, conn->link->dev->features, 8);
 			break;
 		case 0x01:
 			ev.status = BT_HCI_ERR_SUCCESS;
-			btdev_get_host_features(dev->conn, ev.features);
+			btdev_get_host_features(conn->link->dev, ev.features);
 			break;
 		default:
 			ev.status = BT_HCI_ERR_INVALID_PARAMETERS;
@@ -1488,10 +1666,13 @@ static int cmd_read_clock_offset_complete(struct btdev *dev, const void *data,
 {
 	const struct bt_hci_cmd_read_clock_offset *cmd = data;
 	struct bt_hci_evt_clock_offset_complete ev;
+	struct btdev_conn *conn;
 
 	memset(&ev, 0, sizeof(ev));
 
-	if (dev->conn) {
+	conn = queue_find(dev->conns, match_handle,
+				UINT_TO_PTR(le16_to_cpu(cmd->handle)));
+	if (conn) {
 		ev.status = BT_HCI_ERR_SUCCESS;
 		ev.handle = cpu_to_le16(cmd->handle);
 		ev.clock_offset = 0;
@@ -2142,20 +2323,38 @@ static void set_common_commands_bredr20(struct btdev *btdev)
 }
 
 static int cmd_setup_sync_conn(struct btdev *dev, const void *data, uint8_t len)
+{
+	cmd_status(dev, BT_HCI_ERR_SUCCESS, BT_HCI_EVT_SYNC_CONN_COMPLETE);
+
+	return 0;
+}
+
+static int cmd_setup_sync_conn_complete(struct btdev *dev, const void *data,
+							uint8_t len)
 {
 	const struct bt_hci_cmd_setup_sync_conn *cmd = data;
 	struct bt_hci_evt_sync_conn_complete cc;
-	uint8_t status = BT_HCI_ERR_SUCCESS;
+	struct btdev_conn *conn;
 
-	cmd_status(dev, status, BT_HCI_EVT_SYNC_CONN_COMPLETE);
+	memset(&cc, 0, sizeof(cc));
 
-	if (!dev->conn)
-		return 0;
+	conn = queue_find(dev->conns, match_handle,
+				UINT_TO_PTR(le16_to_cpu(cmd->handle)));
+	if (!conn) {
+		cc.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
+		goto done;
+	}
 
-	cc.status = status;
-	memcpy(cc.bdaddr, dev->conn->bdaddr, 6);
+	conn = conn_add_sco(conn);
+	if (!conn) {
+		cc.status = BT_HCI_ERR_MEM_CAPACITY_EXCEEDED;
+		goto done;
+	}
 
-	cc.handle = cpu_to_le16(status == BT_HCI_ERR_SUCCESS ? SCO_HANDLE : 0);
+	cc.status = BT_HCI_ERR_SUCCESS;
+	memcpy(cc.bdaddr, conn->link->dev->bdaddr, 6);
+
+	cc.handle = cpu_to_le16(conn->handle);
 	cc.link_type = 0x02;
 	cc.tx_interval = 0x000c;
 	cc.retrans_window = 0x06;
@@ -2163,6 +2362,7 @@ static int cmd_setup_sync_conn(struct btdev *dev, const void *data, uint8_t len)
 	cc.tx_pkt_len = 60;
 	cc.air_mode = (cmd->voice_setting == 0x0060) ? 0x02 : 0x03;
 
+done:
 	send_event(dev, BT_HCI_EVT_SYNC_CONN_COMPLETE, &cc, sizeof(cc));
 
 	return 0;
@@ -2255,13 +2455,14 @@ static int cmd_write_inquiry_tx_power(struct btdev *dev, const void *data,
 
 static int cmd_io_cap_reply(struct btdev *dev, const void *data, uint8_t len)
 {
-	struct btdev *remote = dev->conn;
 	const struct bt_hci_cmd_io_capability_request_reply *cmd = data;
 	struct bt_hci_evt_io_capability_response ev;
 	struct bt_hci_rsp_io_capability_request_reply rsp;
+	struct btdev_conn *conn;
 	uint8_t status;
 
-	if (!remote) {
+	conn = queue_find(dev->conns, match_bdaddr, cmd->bdaddr);
+	if (!conn) {
 		status = BT_HCI_ERR_UNKNOWN_CONN_ID;
 		goto done;
 	}
@@ -2276,22 +2477,23 @@ static int cmd_io_cap_reply(struct btdev *dev, const void *data, uint8_t len)
 	ev.oob_data = cmd->oob_data;
 	ev.authentication = cmd->authentication;
 
-	send_event(remote, BT_HCI_EVT_IO_CAPABILITY_RESPONSE, &ev, sizeof(ev));
+	send_event(conn->link->dev, BT_HCI_EVT_IO_CAPABILITY_RESPONSE, &ev,
+					sizeof(ev));
 
-	if (remote->io_cap) {
+	if (conn->link->dev->io_cap) {
 		struct bt_hci_evt_user_confirm_request cfm;
 
 		memcpy(cfm.bdaddr, dev->bdaddr, 6);
 		cfm.passkey = 0;
 
-		send_event(remote, BT_HCI_EVT_USER_CONFIRM_REQUEST,
+		send_event(conn->link->dev, BT_HCI_EVT_USER_CONFIRM_REQUEST,
 							&cfm, sizeof(cfm));
 
 		memcpy(cfm.bdaddr, cmd->bdaddr, 6);
 		send_event(dev, BT_HCI_EVT_USER_CONFIRM_REQUEST,
 							&cfm, sizeof(cfm));
 	} else {
-		send_event(remote, BT_HCI_EVT_IO_CAPABILITY_REQUEST,
+		send_event(conn->link->dev, BT_HCI_EVT_IO_CAPABILITY_REQUEST,
 							dev->bdaddr, 6);
 	}
 
@@ -2308,51 +2510,49 @@ static void ssp_complete(struct btdev *btdev, const uint8_t *bdaddr,
 						uint8_t status, bool wait)
 {
 	struct bt_hci_evt_simple_pairing_complete iev, aev;
-	struct bt_hci_evt_auth_complete auth;
-	struct btdev *remote = btdev->conn;
-	struct btdev *init, *accp;
+	struct btdev_conn *conn;
+	struct btdev_conn *init, *accp;
 
-	if (!remote)
+	conn = queue_find(btdev->conns, match_bdaddr, bdaddr);
+	if (!conn)
 		return;
 
 	btdev->ssp_status = status;
 	btdev->ssp_auth_complete = true;
 
-	if (!remote->ssp_auth_complete && wait)
+	if (!conn->link->dev->ssp_auth_complete && wait)
 		return;
 
 	if (status == BT_HCI_ERR_SUCCESS &&
-				remote->ssp_status != BT_HCI_ERR_SUCCESS)
-		status = remote->ssp_status;
+			conn->link->dev->ssp_status != BT_HCI_ERR_SUCCESS)
+		status = conn->link->dev->ssp_status;
 
 	iev.status = status;
 	aev.status = status;
 
 	if (btdev->auth_init) {
-		init = btdev;
-		accp = remote;
+		init = conn;
+		accp = conn->link;
 		memcpy(iev.bdaddr, bdaddr, 6);
 		memcpy(aev.bdaddr, btdev->bdaddr, 6);
 	} else {
-		init = remote;
-		accp = btdev;
+		init = conn->link;
+		accp = conn;
 		memcpy(iev.bdaddr, btdev->bdaddr, 6);
 		memcpy(aev.bdaddr, bdaddr, 6);
 	}
 
-	send_event(init, BT_HCI_EVT_SIMPLE_PAIRING_COMPLETE, &iev,
-								sizeof(iev));
-	send_event(accp, BT_HCI_EVT_SIMPLE_PAIRING_COMPLETE, &aev,
-								sizeof(aev));
+	send_event(init->dev, BT_HCI_EVT_SIMPLE_PAIRING_COMPLETE, &iev,
+							sizeof(iev));
+	send_event(accp->dev, BT_HCI_EVT_SIMPLE_PAIRING_COMPLETE, &aev,
+							sizeof(aev));
 
 	if (status == BT_HCI_ERR_SUCCESS) {
-		link_key_notify(init, iev.bdaddr, LINK_KEY_DUMMY);
-		link_key_notify(accp, aev.bdaddr, LINK_KEY_DUMMY);
+		link_key_notify(init->dev, iev.bdaddr, LINK_KEY_DUMMY);
+		link_key_notify(accp->dev, aev.bdaddr, LINK_KEY_DUMMY);
 	}
 
-	auth.status = status;
-	auth.handle = cpu_to_le16(ACL_HANDLE);
-	send_event(init, BT_HCI_EVT_AUTH_COMPLETE, &auth, sizeof(auth));
+	auth_complete(init, status);
 }
 
 static int cmd_user_confirm_reply(struct btdev *dev, const void *data,
@@ -2419,10 +2619,15 @@ static int cmd_read_encrypt_key_size(struct btdev *dev, const void *data,
 {
 	const struct bt_hci_cmd_read_encrypt_key_size *cmd = data;
 	struct bt_hci_rsp_read_encrypt_key_size rsp;
+	struct btdev_conn *conn;
+
+	memset(&rsp, 0, sizeof(rsp));
 
 	rsp.handle = cmd->handle;
 
-	if (dev->conn) {
+	conn = queue_find(dev->conns, match_handle,
+					UINT_TO_PTR(le16_to_cpu(cmd->handle)));
+	if (conn) {
 		rsp.status = BT_HCI_ERR_SUCCESS;
 		rsp.key_size = 16;
 	} else {
@@ -2485,7 +2690,8 @@ static int cmd_get_mws_transport_config(struct btdev *dev, const void *data,
 }
 
 #define CMD_BREDR \
-	CMD(BT_HCI_CMD_SETUP_SYNC_CONN, cmd_setup_sync_conn, NULL), \
+	CMD(BT_HCI_CMD_SETUP_SYNC_CONN, cmd_setup_sync_conn, \
+					cmd_setup_sync_conn_complete), \
 	CMD(BT_HCI_CMD_READ_EXT_INQUIRY_RESPONSE, cmd_read_ext_inquiry, NULL), \
 	CMD(BT_HCI_CMD_WRITE_EXT_INQUIRY_RESPONSE, cmd_write_ext_inquiry, \
 					NULL), \
@@ -2986,50 +3192,67 @@ static bool adv_connectable(struct btdev *btdev)
 	return btdev->le_adv_type != 0x03;
 }
 
+static void le_meta_event(struct btdev *btdev, uint8_t event,
+						void *data, uint8_t len)
+{
+	void *pkt_data;
+
+	util_debug(btdev->debug_callback, btdev->debug_data,
+				"meta event 0x%02x", event);
+
+	pkt_data = alloca(1 + len);
+	if (!pkt_data)
+		return;
+
+	((uint8_t *) pkt_data)[0] = event;
+
+	if (len > 0)
+		memcpy(pkt_data + 1, data, len);
+
+	send_event(btdev, BT_HCI_EVT_LE_META_EVENT, pkt_data, 1 + len);
+}
+
 static void le_conn_complete(struct btdev *btdev,
 				const struct bt_hci_cmd_le_create_conn *lecc,
 				uint8_t status)
 {
-	char buf[1 + sizeof(struct bt_hci_evt_le_conn_complete)];
-	struct bt_hci_evt_le_conn_complete *cc = (void *) &buf[1];
+	struct bt_hci_evt_le_conn_complete cc;
 
-	memset(buf, 0, sizeof(buf));
-
-	buf[0] = BT_HCI_EVT_LE_CONN_COMPLETE;
+	memset(&cc, 0, sizeof(cc));
 
 	if (!status) {
-		struct btdev *remote;
+		struct btdev_conn *conn;
 
-		remote = find_btdev_by_bdaddr_type(lecc->peer_addr,
-							lecc->peer_addr_type);
+		conn = conn_add_acl(btdev, lecc->peer_addr,
+						lecc->peer_addr_type);
+		if (!conn)
+			return;
 
-		btdev->conn = remote;
 		btdev->le_adv_enable = 0;
-		remote->conn = btdev;
-		remote->le_adv_enable = 0;
+		conn->link->dev->le_adv_enable = 0;
 
-		cc->status = status;
-		cc->peer_addr_type = btdev->le_scan_own_addr_type;
-		if (cc->peer_addr_type == 0x01)
-			memcpy(cc->peer_addr, btdev->random_addr, 6);
+		cc.status = status;
+		cc.peer_addr_type = btdev->le_scan_own_addr_type;
+		if (cc.peer_addr_type == 0x01)
+			memcpy(cc.peer_addr, btdev->random_addr, 6);
 		else
-			memcpy(cc->peer_addr, btdev->bdaddr, 6);
-
-		cc->role = 0x01;
-		cc->handle = cpu_to_le16(ACL_HANDLE);
-		cc->interval = lecc->max_interval;
-		cc->latency = lecc->latency;
-		cc->supv_timeout = lecc->supv_timeout;
-
-		send_event(remote, BT_HCI_EVT_LE_META_EVENT, buf, sizeof(buf));
+			memcpy(cc.peer_addr, btdev->bdaddr, 6);
+
+		cc.role = 0x01;
+		cc.handle = cpu_to_le16(conn->handle);
+		cc.interval = lecc->max_interval;
+		cc.latency = lecc->latency;
+		cc.supv_timeout = lecc->supv_timeout;
+		le_meta_event(conn->link->dev, BT_HCI_EVT_LE_CONN_COMPLETE,
+					&cc, sizeof(cc));
 	}
 
-	cc->status = status;
-	cc->peer_addr_type = lecc->peer_addr_type;
-	memcpy(cc->peer_addr, lecc->peer_addr, 6);
-	cc->role = 0x00;
+	cc.status = status;
+	cc.peer_addr_type = lecc->peer_addr_type;
+	memcpy(cc.peer_addr, lecc->peer_addr, 6);
+	cc.role = 0x00;
 
-	send_event(btdev, BT_HCI_EVT_LE_META_EVENT, buf, sizeof(buf));
+	le_meta_event(btdev, BT_HCI_EVT_LE_CONN_COMPLETE, &cc, sizeof(cc));
 }
 
 static int cmd_le_create_conn(struct btdev *dev, const void *data, uint8_t len)
@@ -3092,27 +3315,29 @@ static void le_conn_update(struct btdev *btdev, uint16_t handle,
 				uint16_t latency, uint16_t supv_timeout,
 				uint16_t min_length, uint16_t max_length)
 {
-	struct btdev *remote = btdev->conn;
-	struct __packed {
-		uint8_t subevent;
-		struct bt_hci_evt_le_conn_update_complete ev;
-	} ev;
+	struct bt_hci_evt_le_conn_update_complete ev;
+	struct btdev_conn *conn;
+
+	memset(&ev, 0, sizeof(ev));
 
-	ev.subevent = BT_HCI_EVT_LE_CONN_UPDATE_COMPLETE;
-	ev.ev.handle = cpu_to_le16(handle);
-	ev.ev.interval = cpu_to_le16(min_interval);
-	ev.ev.latency = cpu_to_le16(latency);
-	ev.ev.supv_timeout = cpu_to_le16(supv_timeout);
+	ev.handle = cpu_to_le16(handle);
+	ev.interval = cpu_to_le16(min_interval);
+	ev.latency = cpu_to_le16(latency);
+	ev.supv_timeout = cpu_to_le16(supv_timeout);
 
-	if (remote)
-		ev.ev.status = BT_HCI_ERR_SUCCESS;
+	conn = queue_find(btdev->conns, match_handle, UINT_TO_PTR(handle));
+	if (conn)
+		ev.status = BT_HCI_ERR_SUCCESS;
 	else
-		ev.ev.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
+		ev.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
 
-	send_event(btdev, BT_HCI_EVT_LE_META_EVENT, &ev, sizeof(ev));
+	le_meta_event(btdev, BT_HCI_EVT_LE_CONN_UPDATE_COMPLETE, &ev,
+					sizeof(ev));
 
-	if (remote)
-		send_event(remote, BT_HCI_EVT_LE_META_EVENT, &ev, sizeof(ev));
+	if (conn)
+		le_meta_event(conn->link->dev,
+					BT_HCI_EVT_LE_CONN_UPDATE_COMPLETE,
+					&ev, sizeof(ev));
 }
 
 static void le_conn_param_req(struct btdev *btdev, uint16_t handle,
@@ -3120,23 +3345,23 @@ static void le_conn_param_req(struct btdev *btdev, uint16_t handle,
 				uint16_t latency, uint16_t supv_timeout,
 				uint16_t min_length, uint16_t max_length)
 {
-	struct btdev *remote = btdev->conn;
-	struct __packed {
-		uint8_t subevent;
-		struct bt_hci_evt_le_conn_param_request ev;
-	} ev;
+	struct bt_hci_evt_le_conn_param_request ev;
+	struct btdev_conn *conn;
 
-	if (!remote)
+	conn = queue_find(btdev->conns, match_handle, UINT_TO_PTR(handle));
+	if (!conn)
 		return;
 
-	ev.subevent = BT_HCI_EVT_LE_CONN_PARAM_REQUEST;
-	ev.ev.handle = cpu_to_le16(handle);
-	ev.ev.min_interval = cpu_to_le16(min_interval);
-	ev.ev.max_interval = cpu_to_le16(max_interval);
-	ev.ev.latency = cpu_to_le16(latency);
-	ev.ev.supv_timeout = cpu_to_le16(supv_timeout);
+	memset(&ev, 0, sizeof(ev));
+
+	ev.handle = cpu_to_le16(handle);
+	ev.min_interval = cpu_to_le16(min_interval);
+	ev.max_interval = cpu_to_le16(max_interval);
+	ev.latency = cpu_to_le16(latency);
+	ev.supv_timeout = cpu_to_le16(supv_timeout);
 
-	send_event(remote, BT_HCI_EVT_LE_META_EVENT, &ev, sizeof(ev));
+	le_meta_event(conn->link->dev, BT_HCI_EVT_LE_CONN_PARAM_REQUEST, &ev,
+					sizeof(ev));
 }
 
 static int cmd_conn_update_complete(struct btdev *dev, const void *data,
@@ -3164,44 +3389,28 @@ static int cmd_conn_update_complete(struct btdev *dev, const void *data,
 	return 0;
 }
 
-static void le_meta_event(struct btdev *btdev, uint8_t event,
-						void *data, uint8_t len)
-{
-	void *pkt_data;
-
-	util_debug(btdev->debug_callback, btdev->debug_data,
-				"meta event 0x%02x", event);
-
-	pkt_data = alloca(1 + len);
-	if (!pkt_data)
-		return;
-
-	((uint8_t *) pkt_data)[0] = event;
-
-	if (len > 0)
-		memcpy(pkt_data + 1, data, len);
-
-	send_event(btdev, BT_HCI_EVT_LE_META_EVENT, pkt_data, 1 + len);
-}
-
 static int cmd_le_read_remote_features(struct btdev *dev, const void *data,
 							uint8_t len)
 {
+	const struct bt_hci_cmd_read_remote_features *cmd = data;
 	struct bt_hci_evt_le_remote_features_complete ev;
-	struct btdev *remote = dev->conn;
+	struct btdev_conn *conn;
+	uint8_t status = BT_HCI_ERR_SUCCESS;
 
-	if (!remote) {
-		cmd_status(dev, BT_HCI_ERR_UNKNOWN_CONN_ID,
-					BT_HCI_CMD_LE_READ_REMOTE_FEATURES);
-		return 0;
-	}
+	conn = queue_find(dev->conns, match_handle,
+				UINT_TO_PTR(le16_to_cpu(cmd->handle)));
+	if (!conn)
+		status = BT_HCI_ERR_UNKNOWN_CONN_ID;
 
-	cmd_status(dev, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_LE_READ_REMOTE_FEATURES);
+	cmd_status(dev, status, BT_HCI_CMD_LE_READ_REMOTE_FEATURES);
+
+	if (status)
+		return 0;
 
 	memset(&ev, 0, sizeof(ev));
 	ev.status = BT_HCI_ERR_SUCCESS;
-	ev.handle = cpu_to_le16(ACL_HANDLE);
-	memcpy(ev.features, remote->le_features, 8);
+	ev.handle = cpu_to_le16(conn->handle);
+	memcpy(ev.features, conn->link->dev->le_features, 8);
 
 	le_meta_event(dev, BT_HCI_EVT_LE_REMOTE_FEATURES_COMPLETE, &ev,
 						sizeof(ev));
@@ -3246,27 +3455,27 @@ static int cmd_rand(struct btdev *dev, const void *data, uint8_t len)
 static int cmd_start_encrypt(struct btdev *dev, const void *data, uint8_t len)
 {
 	const struct bt_hci_cmd_le_start_encrypt *cmd = data;
-	char buf[1 + sizeof(struct bt_hci_evt_le_long_term_key_request)];
-	struct bt_hci_evt_le_long_term_key_request *ev = (void *) &buf[1];
-	struct btdev *remote = dev->conn;
+	struct bt_hci_evt_le_long_term_key_request ev;
+	struct btdev_conn *conn;
 
-	if (!remote) {
+	conn = queue_find(dev->conns, match_handle,
+				UINT_TO_PTR(le16_to_cpu(cmd->handle)));
+	if (!conn) {
 		cmd_status(dev, BT_HCI_ERR_UNKNOWN_CONN_ID,
-						BT_HCI_CMD_LE_START_ENCRYPT);
+				BT_HCI_CMD_LE_START_ENCRYPT);
 		return 0;
 	}
 
-	memcpy(dev->le_ltk, cmd->ltk, 16);
-
 	cmd_status(dev, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_LE_START_ENCRYPT);
 
-	memset(buf, 0, sizeof(buf));
-	buf[0] = BT_HCI_EVT_LE_LONG_TERM_KEY_REQUEST;
-	ev->handle = cpu_to_le16(ACL_HANDLE);
-	ev->ediv = cmd->ediv;
-	ev->rand = cmd->rand;
+	memcpy(dev->le_ltk, cmd->ltk, 16);
+
+	ev.handle = cpu_to_le16(conn->handle);
+	ev.ediv = cmd->ediv;
+	ev.rand = cmd->rand;
 
-	send_event(remote, BT_HCI_EVT_LE_META_EVENT, buf, sizeof(buf));
+	le_meta_event(conn->link->dev, BT_HCI_EVT_LE_LONG_TERM_KEY_REQUEST, &ev,
+					sizeof(ev));
 
 	return 0;
 }
@@ -3274,11 +3483,13 @@ static int cmd_start_encrypt(struct btdev *dev, const void *data, uint8_t len)
 static int cmd_ltk_reply(struct btdev *dev, const void *data, uint8_t len)
 {
 	const struct bt_hci_cmd_le_ltk_req_reply *cmd = data;
-	struct bt_hci_evt_encrypt_change ev;
 	struct bt_hci_rsp_le_ltk_req_reply rp;
-	struct btdev *remote = dev->conn;
+	struct btdev_conn *conn;
+	uint8_t mode, status;
 
-	if (!remote) {
+	conn = queue_find(dev->conns, match_handle,
+				UINT_TO_PTR(le16_to_cpu(cmd->handle)));
+	if (!conn) {
 		rp.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
 		cmd_complete(dev, BT_HCI_CMD_LE_LTK_REQ_REPLY, &rp, sizeof(rp));
 		return 0;
@@ -3287,36 +3498,34 @@ static int cmd_ltk_reply(struct btdev *dev, const void *data, uint8_t len)
 	memcpy(dev->le_ltk, cmd->ltk, 16);
 
 	memset(&rp, 0, sizeof(rp));
-	rp.handle = cpu_to_le16(ACL_HANDLE);
+	rp.handle = cpu_to_le16(conn->handle);
 
 	rp.status = BT_HCI_ERR_SUCCESS;
 	cmd_complete(dev, BT_HCI_CMD_LE_LTK_REQ_REPLY, &rp, sizeof(rp));
 
-	memset(&ev, 0, sizeof(ev));
-
-	if (memcmp(dev->le_ltk, remote->le_ltk, 16)) {
-		ev.status = BT_HCI_ERR_AUTH_FAILURE;
-		ev.encr_mode = 0x00;
+	if (memcmp(dev->le_ltk, conn->link->dev->le_ltk, 16)) {
+		status = BT_HCI_ERR_AUTH_FAILURE;
+		mode = 0x00;
 	} else {
-		ev.status = BT_HCI_ERR_SUCCESS;
-		ev.encr_mode = 0x01;
+		status = BT_HCI_ERR_SUCCESS;
+		mode = 0x01;
 	}
 
-	ev.handle = cpu_to_le16(ACL_HANDLE);
-
-	send_event(dev, BT_HCI_EVT_ENCRYPT_CHANGE, &ev, sizeof(ev));
-	send_event(remote, BT_HCI_EVT_ENCRYPT_CHANGE, &ev, sizeof(ev));
+	encrypt_change(conn, mode, status);
+	encrypt_change(conn->link, mode, status);
 
 	return 0;
 }
 
 static int cmd_ltk_neg_reply(struct btdev *dev, const void *data, uint8_t len)
 {
+	const struct bt_hci_cmd_le_ltk_req_neg_reply *cmd = data;
 	struct bt_hci_rsp_le_ltk_req_neg_reply rp;
-	struct bt_hci_evt_encrypt_change ev;
-	struct btdev *remote = dev->conn;
+	struct btdev_conn *conn;
 
-	if (!remote) {
+	conn = queue_find(dev->conns, match_handle,
+				UINT_TO_PTR(le16_to_cpu(cmd->handle)));
+	if (!conn) {
 		rp.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
 		cmd_complete(dev, BT_HCI_CMD_LE_LTK_REQ_NEG_REPLY, &rp,
 							sizeof(rp));
@@ -3324,15 +3533,11 @@ static int cmd_ltk_neg_reply(struct btdev *dev, const void *data, uint8_t len)
 	}
 
 	memset(&rp, 0, sizeof(rp));
-	rp.handle = cpu_to_le16(ACL_HANDLE);
+	rp.handle = cpu_to_le16(conn->handle);
 	rp.status = BT_HCI_ERR_SUCCESS;
 	cmd_complete(dev, BT_HCI_CMD_LE_LTK_REQ_NEG_REPLY, &rp, sizeof(rp));
 
-	memset(&ev, 0, sizeof(ev));
-	ev.status = BT_HCI_ERR_PIN_OR_KEY_MISSING;
-	ev.handle = cpu_to_le16(ACL_HANDLE);
-
-	send_event(remote, BT_HCI_EVT_ENCRYPT_CHANGE, &ev, sizeof(ev));
+	encrypt_change(conn->link, 0x00, BT_HCI_ERR_PIN_OR_KEY_MISSING);
 
 	return 0;
 }
@@ -3413,10 +3618,12 @@ static int cmd_conn_param_neg_reply_complete(struct btdev *dev,
 						const void *data, uint8_t len)
 {
 	const struct bt_hci_cmd_le_conn_param_req_neg_reply *cmd = data;
-	struct btdev *remote = dev->conn;
+	struct btdev_conn *conn;
 	struct bt_hci_evt_le_conn_update_complete ev;
 
-	if (!remote)
+	conn = queue_find(dev->conns, match_handle,
+				UINT_TO_PTR(le16_to_cpu(cmd->handle)));
+	if (!conn)
 		return 0;
 
 	memset(&ev, 0, sizeof(ev));
@@ -3424,7 +3631,7 @@ static int cmd_conn_param_neg_reply_complete(struct btdev *dev,
 	ev.handle = cpu_to_le16(cmd->handle);
 	ev.status = cpu_to_le16(cmd->reason);
 
-	le_meta_event(remote, BT_HCI_EVT_LE_CONN_UPDATE_COMPLETE, &ev,
+	le_meta_event(conn->link->dev, BT_HCI_EVT_LE_CONN_UPDATE_COMPLETE, &ev,
 						sizeof(ev));
 
 	return 0;
@@ -3861,15 +4068,14 @@ static void le_ext_conn_complete(struct btdev *btdev,
 	memset(&ev, 0, sizeof(ev));
 
 	if (!status) {
-		struct btdev *remote;
+		struct btdev_conn *conn;
 
-		remote = find_btdev_by_bdaddr_type(cmd->peer_addr,
-							cmd->peer_addr_type);
+		conn = conn_add_acl(btdev, cmd->peer_addr, cmd->peer_addr_type);
+		if (!conn)
+			return;
 
-		btdev->conn = remote;
 		btdev->le_adv_enable = 0;
-		remote->conn = btdev;
-		remote->le_adv_enable = 0;
+		conn->link->dev->le_adv_enable = 0;
 
 		ev.status = status;
 		ev.peer_addr_type = btdev->le_scan_own_addr_type;
@@ -3879,13 +4085,14 @@ static void le_ext_conn_complete(struct btdev *btdev,
 			memcpy(ev.peer_addr, btdev->bdaddr, 6);
 
 		ev.role = 0x01;
-		ev.handle = cpu_to_le16(ACL_HANDLE);
+		ev.handle = cpu_to_le16(conn->handle);
 		ev.interval = lecc->max_interval;
 		ev.latency = lecc->latency;
 		ev.supv_timeout = lecc->supv_timeout;
 
-		le_meta_event(remote, BT_HCI_EVT_LE_ENHANCED_CONN_COMPLETE, &ev,
-						sizeof(ev));
+		le_meta_event(conn->link->dev,
+				BT_HCI_EVT_LE_ENHANCED_CONN_COMPLETE, &ev,
+				sizeof(ev));
 	}
 
 	ev.status = status;
@@ -4066,17 +4273,30 @@ static int cmd_set_cig_params(struct btdev *dev, const void *data,
 	const struct bt_hci_cmd_le_set_cig_params *cmd = data;
 	struct lescp {
 		struct bt_hci_rsp_le_set_cig_params params;
-		uint16_t handle;
+		uint16_t handle[3];
 	} __attribute__ ((packed)) rsp;
+	int i = 0;
 
 	memset(&rsp, 0, sizeof(rsp));
 
 	memcpy(&dev->le_cig, data, len);
+
+	if (cmd->num_cis > ARRAY_SIZE(rsp.handle)) {
+		rsp.params.status = BT_HCI_ERR_MEM_CAPACITY_EXCEEDED;
+		goto done;
+	}
+
 	rsp.params.status = BT_HCI_ERR_SUCCESS;
 	rsp.params.cig_id = cmd->cig_id;
-	rsp.params.num_handles = 1;
-	rsp.handle = cpu_to_le16(ISO_HANDLE);
-	cmd_complete(dev, BT_HCI_CMD_LE_SET_CIG_PARAMS, &rsp, sizeof(rsp));
+
+	for (i = 0; i < cmd->num_cis; i++) {
+		rsp.params.num_handles++;
+		rsp.handle[i] = cpu_to_le16(ISO_HANDLE + i);
+	}
+
+done:
+	cmd_complete(dev, BT_HCI_CMD_LE_SET_CIG_PARAMS, &rsp,
+				sizeof(rsp.params) + (i * sizeof(uint16_t)));
 
 	return 0;
 }
@@ -4095,7 +4315,8 @@ static int cmd_create_cis(struct btdev *dev, const void *data, uint8_t len)
 	return 0;
 }
 
-static void le_cis_estabilished(struct btdev *dev, uint8_t status)
+static void le_cis_estabilished(struct btdev *dev, struct btdev_conn *conn,
+						uint8_t status)
 {
 	struct bt_hci_evt_le_cis_established evt;
 
@@ -4103,11 +4324,8 @@ static void le_cis_estabilished(struct btdev *dev, uint8_t status)
 
 	evt.status = status;
 
-	if (dev->conn)
-		dev = dev->conn;
-
 	if (!evt.status) {
-		evt.conn_handle = cpu_to_le16(ISO_HANDLE);
+		evt.conn_handle = cpu_to_le16(conn->handle);
 		/* TODO: Figure out if these values makes sense */
 		memcpy(evt.cig_sync_delay, dev->le_cig.params.m_interval,
 				sizeof(dev->le_cig.params.m_interval));
@@ -4131,27 +4349,49 @@ static void le_cis_estabilished(struct btdev *dev, uint8_t status)
 
 	le_meta_event(dev, BT_HCI_EVT_LE_CIS_ESTABLISHED, &evt, sizeof(evt));
 
-	if (dev->conn)
-		le_meta_event(dev->conn, BT_HCI_EVT_LE_CIS_ESTABLISHED, &evt,
-							sizeof(evt));
+	if (conn)
+		le_meta_event(conn->link->dev, BT_HCI_EVT_LE_CIS_ESTABLISHED,
+						&evt, sizeof(evt));
 }
 
 static int cmd_create_cis_complete(struct btdev *dev, const void *data,
 							uint8_t len)
 {
-	struct btdev *remote = dev->conn;
+	const struct bt_hci_cmd_le_create_cis *cmd = data;
+	int i;
 
-	if (remote) {
+	for (i = 0; i < cmd->num_cis; i++) {
+		const struct bt_hci_cis *cis = &cmd->cis[i];
+		struct btdev_conn *acl;
+		struct btdev_conn *iso;
 		struct bt_hci_evt_le_cis_req evt;
 
-		evt.acl_handle = cpu_to_le16(ACL_HANDLE);
-		evt.cis_handle = cpu_to_le16(ISO_HANDLE);
+		acl = queue_find(dev->conns, match_handle,
+				UINT_TO_PTR(cpu_to_le16(cis->acl_handle)));
+		if (!acl) {
+			le_cis_estabilished(dev, NULL,
+						BT_HCI_ERR_UNKNOWN_CONN_ID);
+			break;
+		}
+
+		iso = queue_find(dev->conns, match_handle,
+				UINT_TO_PTR(cpu_to_le16(cis->cis_handle)));
+		if (!iso) {
+			iso = conn_add_iso(acl, cpu_to_le16(cis->cis_handle));
+			if (!iso) {
+				le_cis_estabilished(dev, NULL,
+						BT_HCI_ERR_UNKNOWN_CONN_ID);
+				break;
+			}
+		}
+
+		evt.acl_handle = cpu_to_le16(acl->handle);
+		evt.cis_handle = cpu_to_le16(iso->handle);
 		evt.cig_id = 0x00;
 		evt.cis_id = 0x00;
 
-		le_meta_event(remote, BT_HCI_EVT_LE_CIS_REQ, &evt, sizeof(evt));
-	} else {
-		le_cis_estabilished(dev, BT_HCI_ERR_UNKNOWN_CONN_ID);
+		le_meta_event(iso->dev, BT_HCI_EVT_LE_CIS_REQ, &evt,
+					sizeof(evt));
 	}
 
 	return 0;
@@ -4173,8 +4413,19 @@ static int cmd_remove_cig(struct btdev *dev, const void *data, uint8_t len)
 
 static int cmd_accept_cis(struct btdev *dev, const void *data, uint8_t len)
 {
+	const struct bt_hci_cmd_le_accept_cis *cmd = data;
+	struct btdev_conn *conn;
+
+	conn = queue_find(dev->conns, match_handle,
+				UINT_TO_PTR(cpu_to_le16(cmd->handle)));
+	if (!conn) {
+		cmd_status(dev, BT_HCI_ERR_UNKNOWN_CONN_ID,
+					BT_HCI_CMD_LE_ACCEPT_CIS);
+		return 0;
+	}
+
 	cmd_status(dev, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_LE_ACCEPT_CIS);
-	le_cis_estabilished(dev, BT_HCI_ERR_SUCCESS);
+	le_cis_estabilished(dev, conn, BT_HCI_ERR_SUCCESS);
 
 	return 0;
 }
@@ -4182,9 +4433,18 @@ static int cmd_accept_cis(struct btdev *dev, const void *data, uint8_t len)
 static int cmd_reject_cis(struct btdev *dev, const void *data, uint8_t len)
 {
 	const struct bt_hci_cmd_le_reject_cis *cmd = data;
+	struct btdev_conn *conn;
 
-	cmd_status(dev, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_LE_ACCEPT_CIS);
-	le_cis_estabilished(dev, cmd->reason);
+	conn = queue_find(dev->conns, match_handle,
+				UINT_TO_PTR(cpu_to_le16(cmd->handle)));
+	if (!conn) {
+		cmd_status(dev, BT_HCI_ERR_UNKNOWN_CONN_ID,
+					BT_HCI_CMD_LE_REJECT_CIS);
+		return 0;
+	}
+
+	cmd_status(dev, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_LE_REJECT_CIS);
+	le_cis_estabilished(dev, conn, cmd->reason);
 
 	return 0;
 }
@@ -4229,10 +4489,13 @@ static int cmd_setup_iso_path(struct btdev *dev, const void *data, uint8_t len)
 {
 	const struct bt_hci_cmd_le_setup_iso_path *cmd = data;
 	struct bt_hci_rsp_le_setup_iso_path rsp;
+	struct btdev_conn *conn;
 
 	memset(&rsp, 0, sizeof(rsp));
 
-	if (le16_to_cpu(cmd->handle) != ISO_HANDLE) {
+	conn = queue_find(dev->conns, match_handle,
+				UINT_TO_PTR(cpu_to_le16(cmd->handle)));
+	if (!conn) {
 		rsp.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
 		goto done;
 	}
@@ -4246,11 +4509,11 @@ static int cmd_setup_iso_path(struct btdev *dev, const void *data, uint8_t len)
 	switch (cmd->direction) {
 	case 0x00:
 		dev->le_iso_path[0] = cmd->path;
-		rsp.handle = cpu_to_le16(ISO_HANDLE);
+		rsp.handle = cpu_to_le16(conn->handle);
 		break;
 	case 0x01:
 		dev->le_iso_path[1] = cmd->path;
-		rsp.handle = cpu_to_le16(ISO_HANDLE);
+		rsp.handle = cpu_to_le16(conn->handle);
 		break;
 	default:
 		rsp.status = BT_HCI_ERR_INVALID_PARAMETERS;
@@ -4266,9 +4529,14 @@ static int cmd_remove_iso_path(struct btdev *dev, const void *data, uint8_t len)
 {
 	const struct bt_hci_cmd_le_remove_iso_path *cmd = data;
 	uint8_t status = BT_HCI_ERR_SUCCESS;
+	struct btdev_conn *conn;
 
-	if (!dev->conn || le16_to_cpu(cmd->handle) != ISO_HANDLE)
+	conn = queue_find(dev->conns, match_handle,
+				UINT_TO_PTR(cpu_to_le16(cmd->handle)));
+	if (!conn) {
 		status = BT_HCI_ERR_UNKNOWN_CONN_ID;
+		goto done;
+	}
 
 	switch (cmd->direction) {
 	case 0x00:
@@ -4281,6 +4549,7 @@ static int cmd_remove_iso_path(struct btdev *dev, const void *data, uint8_t len)
 		status = BT_HCI_ERR_INVALID_PARAMETERS;
 	}
 
+done:
 	cmd_complete(dev, BT_HCI_CMD_LE_REMOVE_ISO_PATH, &status,
 							sizeof(status));
 
@@ -4797,6 +5066,8 @@ struct btdev *btdev_create(enum btdev_type type, uint16_t id)
 
 	get_bdaddr(id, index, btdev->bdaddr);
 
+	btdev->conns = queue_new();
+
 	return btdev;
 }
 
@@ -4878,7 +5149,10 @@ void btdev_set_send_handler(struct btdev *btdev, btdev_send_func handler,
 
 static void num_completed_packets(struct btdev *btdev, uint16_t handle)
 {
-	if (btdev->conn) {
+	struct btdev_conn *conn;
+
+	conn = queue_find(btdev->conns, match_handle, UINT_TO_PTR(handle));
+	if (conn) {
 		struct bt_hci_evt_num_completed_packets ncp;
 
 		ncp.num_handles = 1;
@@ -5019,10 +5293,11 @@ static void process_cmd(struct btdev *btdev, const void *data, uint16_t len)
 	}
 }
 
-static void send_acl(struct btdev *conn, const void *data, uint16_t len)
+static void send_acl(struct btdev *dev, const void *data, uint16_t len)
 {
 	struct bt_hci_acl_hdr hdr;
 	struct iovec iov[3];
+	struct btdev_conn *conn;
 
 	/* Packet type */
 	iov[0].iov_base = (void *) data;
@@ -5032,8 +5307,16 @@ static void send_acl(struct btdev *conn, const void *data, uint16_t len)
 	 * From controller to host this should be converted to ACL_START.
 	 */
 	memcpy(&hdr, data + 1, sizeof(hdr));
+
+	conn = queue_find(dev->conns, match_handle,
+					UINT_TO_PTR(acl_handle(hdr.handle)));
+	if (!conn)
+		return;
+
+	num_completed_packets(dev, conn->handle);
+
 	if (acl_flags(hdr.handle) == ACL_START_NO_FLUSH)
-		hdr.handle = acl_handle_pack(acl_handle(hdr.handle), ACL_START);
+		hdr.handle = acl_handle_pack(conn->handle, ACL_START);
 
 	iov[1].iov_base = &hdr;
 	iov[1].iov_len = sizeof(hdr);
@@ -5041,17 +5324,26 @@ static void send_acl(struct btdev *conn, const void *data, uint16_t len)
 	iov[2].iov_base = (void *) (data + 1 + sizeof(hdr));
 	iov[2].iov_len = len - 1 - sizeof(hdr);
 
-	send_packet(conn, iov, 3);
+	send_packet(conn->link->dev, iov, 3);
 }
 
-static void send_iso(struct btdev *conn, const void *data, uint16_t len)
+static void send_iso(struct btdev *dev, const void *data, uint16_t len)
 {
+	struct bt_hci_acl_hdr *hdr;
 	struct iovec iov;
+	struct btdev_conn *conn;
 
-	iov.iov_base = (void *) (data);
+	iov.iov_base = hdr = (void *) (data);
 	iov.iov_len = len;
 
-	send_packet(conn, &iov, 1);
+	conn = queue_find(dev->conns, match_handle,
+					UINT_TO_PTR(acl_handle(hdr->handle)));
+	if (!conn)
+		return;
+
+	num_completed_packets(dev, conn->handle);
+
+	send_packet(conn->link->dev, &iov, 1);
 }
 
 void btdev_receive_h4(struct btdev *btdev, const void *data, uint16_t len)
@@ -5074,14 +5366,10 @@ void btdev_receive_h4(struct btdev *btdev, const void *data, uint16_t len)
 		process_cmd(btdev, data + 1, len - 1);
 		break;
 	case BT_H4_ACL_PKT:
-		if (btdev->conn)
-			send_acl(btdev->conn, data, len);
-		num_completed_packets(btdev, ACL_HANDLE);
+		send_acl(btdev, data, len);
 		break;
 	case BT_H4_ISO_PKT:
-		num_completed_packets(btdev, ISO_HANDLE);
-		if (btdev->conn)
-			send_iso(btdev->conn, data, len);
+		send_iso(btdev, data, len);
 		break;
 	default:
 		util_debug(btdev->debug_callback, btdev->debug_data,
diff --git a/lib/hci.h b/lib/hci.h
index 0ca20421a..3382b87bf 100644
--- a/lib/hci.h
+++ b/lib/hci.h
@@ -107,6 +107,7 @@ enum {
 #define HCI_ACLDATA_PKT		0x02
 #define HCI_SCODATA_PKT		0x03
 #define HCI_EVENT_PKT		0x04
+#define HCI_ISODATA_PKT		0x05
 #define HCI_VENDOR_PKT		0xff
 
 /* HCI Packet types */
-- 
2.26.2

[PATCH v2 2/3] hciemu: Add support for multiple clients

[Luiz Augusto von Dentz]

From: Luiz Augusto von Dentz <luiz.von.dentz@intel.com>

This adds support for creating multiple clients (bthost).
---
 emulator/hciemu.c | 234 +++++++++++++++++++++++++++++++---------------
 emulator/hciemu.h |   6 ++
 2 files changed, 163 insertions(+), 77 deletions(-)

diff --git a/emulator/hciemu.c b/emulator/hciemu.c
index 961de6359..a3ec44243 100644
--- a/emulator/hciemu.c
+++ b/emulator/hciemu.c
@@ -34,16 +34,20 @@
 #include "src/shared/queue.h"
 #include "emulator/hciemu.h"
 
+struct hciemu_client {
+	struct bthost *host;
+	struct btdev *dev;
+	guint start_source;
+	guint host_source;
+	guint source;
+};
+
 struct hciemu {
 	int ref_count;
 	enum btdev_type btdev_type;
-	struct bthost *host_stack;
-	struct btdev *master_dev;
-	struct btdev *client_dev;
-	guint host_source;
-	guint master_source;
-	guint client_source;
-	guint start_source;
+	struct btdev *dev;
+	struct queue *clients;
+	guint source;
 	struct queue *post_command_hooks;
 	char bdaddr_str[18];
 
@@ -245,69 +249,123 @@ static bool create_vhci(struct hciemu *hciemu)
 		return false;
 	}
 
-	hciemu->master_dev = btdev;
+	hciemu->dev = btdev;
 
-	hciemu->master_source = create_source_btdev(fd, btdev);
+	hciemu->source = create_source_btdev(fd, btdev);
 
 	return true;
 }
 
+struct hciemu_client *hciemu_get_client(struct hciemu *hciemu, int num)
+{
+	const struct queue_entry *entry;
+
+	if (!hciemu)
+		return NULL;
+
+	for (entry = queue_get_entries(hciemu->clients); entry;
+					entry = entry->next, num--) {
+		if (!num)
+			return entry->data;
+	}
+
+	return NULL;
+}
+
+struct bthost *hciemu_client_host(struct hciemu_client *client)
+{
+	if (!client)
+		return NULL;
+
+	return client->host;
+}
+
 struct bthost *hciemu_client_get_host(struct hciemu *hciemu)
 {
+	struct hciemu_client *client;
+
 	if (!hciemu)
 		return NULL;
 
-	return hciemu->host_stack;
+	client = hciemu_get_client(hciemu, 0);
+
+	return hciemu_client_host(client);
 }
 
-static bool create_stack(struct hciemu *hciemu)
+static gboolean start_host(gpointer user_data)
 {
-	struct btdev *btdev;
-	struct bthost *bthost;
-	int sv[2];
+	struct hciemu_client *client = user_data;
 
-	btdev = btdev_create(hciemu->btdev_type, 0x00);
-	if (!btdev)
-		return false;
+	client->start_source = 0;
 
-	bthost = bthost_create();
-	if (!bthost) {
-		btdev_destroy(btdev);
-		return false;
-	}
+	bthost_start(client->host);
+
+	return FALSE;
+}
 
-	btdev_set_command_handler(btdev, client_command_callback, hciemu);
+static void hciemu_client_destroy(void *data)
+{
+	struct hciemu_client *client = data;
 
-	if (socketpair(AF_UNIX, SOCK_SEQPACKET | SOCK_NONBLOCK | SOCK_CLOEXEC,
-								0, sv) < 0) {
-		bthost_destroy(bthost);
-		btdev_destroy(btdev);
-		return false;
-	}
+	if (client->start_source)
+		g_source_remove(client->start_source);
 
-	hciemu->client_dev = btdev;
-	hciemu->host_stack = bthost;
+	g_source_remove(client->host_source);
+	g_source_remove(client->source);
 
-	hciemu->client_source = create_source_btdev(sv[0], btdev);
-	hciemu->host_source = create_source_bthost(sv[1], bthost);
+	bthost_destroy(client->host);
+	btdev_destroy(client->dev);
 
-	return true;
+	free(client);
 }
 
-static gboolean start_stack(gpointer user_data)
+static struct hciemu_client *hciemu_client_new(struct hciemu *hciemu,
+							uint8_t id)
 {
-	struct hciemu *hciemu = user_data;
+	struct hciemu_client *client;
+	int sv[2];
 
-	hciemu->start_source = 0;
+	client = new0(struct hciemu_client, 1);
+	if (!client)
+		return NULL;
 
-	bthost_start(hciemu->host_stack);
+	client->dev = btdev_create(hciemu->btdev_type, id++);
+	if (!client->dev) {
+		free(client);
+		return NULL;
+	}
 
-	return FALSE;
+	client->host = bthost_create();
+	if (!client->host) {
+		btdev_destroy(client->dev);
+		free(client);
+		return NULL;
+	}
+
+	btdev_set_command_handler(client->dev, client_command_callback, client);
+
+	if (socketpair(AF_UNIX, SOCK_SEQPACKET | SOCK_NONBLOCK | SOCK_CLOEXEC,
+								0, sv) < 0) {
+		bthost_destroy(client->host);
+		btdev_destroy(client->dev);
+		return NULL;
+	}
+
+	client->source = create_source_btdev(sv[0], client->dev);
+	client->host_source = create_source_bthost(sv[1], client->host);
+	client->start_source = g_idle_add(start_host, client);
+
+	return client;
 }
 
-struct hciemu *hciemu_new(enum hciemu_type type)
+struct hciemu *hciemu_new_num(enum hciemu_type type, uint8_t num)
 {
+
 	struct hciemu *hciemu;
+	int i;
+
+	if (!num)
+		return NULL;
 
 	hciemu = new0(struct hciemu, 1);
 	if (!hciemu)
@@ -348,19 +406,27 @@ struct hciemu *hciemu_new(enum hciemu_type type)
 		return NULL;
 	}
 
-	if (!create_stack(hciemu)) {
-		g_source_remove(hciemu->master_source);
-		btdev_destroy(hciemu->master_dev);
-		queue_destroy(hciemu->post_command_hooks, NULL);
-		free(hciemu);
-		return NULL;
-	}
+	hciemu->clients = queue_new();
+
+	for (i = 0; i < num; i++) {
+		struct hciemu_client *client = hciemu_client_new(hciemu, i);
 
-	hciemu->start_source = g_idle_add(start_stack, hciemu);
+		if (!client) {
+			queue_destroy(hciemu->clients, hciemu_client_destroy);
+			break;
+		}
+
+		queue_push_tail(hciemu->clients, client);
+	}
 
 	return hciemu_ref(hciemu);
 }
 
+struct hciemu *hciemu_new(enum hciemu_type type)
+{
+	return hciemu_new_num(type, 1);
+}
+
 struct hciemu *hciemu_ref(struct hciemu *hciemu)
 {
 	if (!hciemu)
@@ -380,17 +446,10 @@ void hciemu_unref(struct hciemu *hciemu)
 		return;
 
 	queue_destroy(hciemu->post_command_hooks, destroy_command_hook);
+	queue_destroy(hciemu->clients, hciemu_client_destroy);
 
-	if (hciemu->start_source)
-		g_source_remove(hciemu->start_source);
-
-	g_source_remove(hciemu->host_source);
-	g_source_remove(hciemu->client_source);
-	g_source_remove(hciemu->master_source);
-
-	bthost_destroy(hciemu->host_stack);
-	btdev_destroy(hciemu->client_dev);
-	btdev_destroy(hciemu->master_dev);
+	g_source_remove(hciemu->source);
+	btdev_destroy(hciemu->dev);
 
 	free(hciemu);
 }
@@ -419,6 +478,15 @@ static void btdev_client_debug(const char *str, void *user_data)
 					"btdev[bthost]: %s", str);
 }
 
+static void hciemu_client_set_debug(void *data, void *user_data)
+{
+	struct hciemu_client *client = data;
+	struct hciemu *hciemu = user_data;
+
+	btdev_set_debug(client->dev, btdev_client_debug, hciemu, NULL);
+	bthost_set_debug(client->host, bthost_debug, hciemu, NULL);
+}
+
 bool hciemu_set_debug(struct hciemu *hciemu, hciemu_debug_func_t callback,
 			void *user_data, hciemu_destroy_func_t destroy)
 {
@@ -432,9 +500,9 @@ bool hciemu_set_debug(struct hciemu *hciemu, hciemu_debug_func_t callback,
 	hciemu->debug_destroy = destroy;
 	hciemu->debug_data = user_data;
 
-	btdev_set_debug(hciemu->master_dev, btdev_master_debug, hciemu, NULL);
-	btdev_set_debug(hciemu->client_dev, btdev_client_debug, hciemu, NULL);
-	bthost_set_debug(hciemu->host_stack, bthost_debug, hciemu, NULL);
+	btdev_set_debug(hciemu->dev, btdev_master_debug, hciemu, NULL);
+
+	queue_foreach(hciemu->clients, hciemu_client_set_debug, hciemu);
 
 	return true;
 }
@@ -443,10 +511,10 @@ const char *hciemu_get_address(struct hciemu *hciemu)
 {
 	const uint8_t *addr;
 
-	if (!hciemu || !hciemu->master_dev)
+	if (!hciemu || !hciemu->dev)
 		return NULL;
 
-	addr = btdev_get_bdaddr(hciemu->master_dev);
+	addr = btdev_get_bdaddr(hciemu->dev);
 	sprintf(hciemu->bdaddr_str, "%2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X",
 			addr[5], addr[4], addr[3], addr[2], addr[1], addr[0]);
 	return hciemu->bdaddr_str;
@@ -454,50 +522,62 @@ const char *hciemu_get_address(struct hciemu *hciemu)
 
 uint8_t *hciemu_get_features(struct hciemu *hciemu)
 {
-	if (!hciemu || !hciemu->master_dev)
+	if (!hciemu || !hciemu->dev)
 		return NULL;
 
-	return btdev_get_features(hciemu->master_dev);
+	return btdev_get_features(hciemu->dev);
 }
 
 const uint8_t *hciemu_get_master_bdaddr(struct hciemu *hciemu)
 {
-	if (!hciemu || !hciemu->master_dev)
+	if (!hciemu || !hciemu->dev)
 		return NULL;
 
-	return btdev_get_bdaddr(hciemu->master_dev);
+	return btdev_get_bdaddr(hciemu->dev);
+}
+
+const uint8_t *hciemu_client_bdaddr(struct hciemu_client *client)
+{
+	if (!client)
+		return NULL;
+
+	return btdev_get_bdaddr(client->dev);
 }
 
 const uint8_t *hciemu_get_client_bdaddr(struct hciemu *hciemu)
 {
-	if (!hciemu || !hciemu->client_dev)
+	struct hciemu_client *client;
+
+	if (!hciemu)
 		return NULL;
 
-	return btdev_get_bdaddr(hciemu->client_dev);
+	client = hciemu_get_client(hciemu, 0);
+
+	return hciemu_client_bdaddr(client);
 }
 
 uint8_t hciemu_get_master_scan_enable(struct hciemu *hciemu)
 {
-	if (!hciemu || !hciemu->master_dev)
+	if (!hciemu || !hciemu->dev)
 		return 0;
 
-	return btdev_get_scan_enable(hciemu->master_dev);
+	return btdev_get_scan_enable(hciemu->dev);
 }
 
 uint8_t hciemu_get_master_le_scan_enable(struct hciemu *hciemu)
 {
-	if (!hciemu || !hciemu->master_dev)
+	if (!hciemu || !hciemu->dev)
 		return 0;
 
-	return btdev_get_le_scan_enable(hciemu->master_dev);
+	return btdev_get_le_scan_enable(hciemu->dev);
 }
 
 void hciemu_set_master_le_states(struct hciemu *hciemu, const uint8_t *le_states)
 {
-	if (!hciemu || !hciemu->master_dev)
+	if (!hciemu || !hciemu->dev)
 		return;
 
-	btdev_set_le_states(hciemu->master_dev, le_states);
+	btdev_set_le_states(hciemu->dev, le_states);
 }
 
 bool hciemu_add_master_post_command_hook(struct hciemu *hciemu,
@@ -559,7 +639,7 @@ int hciemu_add_hook(struct hciemu *hciemu, enum hciemu_hook_type type,
 		return -1;
 	}
 
-	return btdev_add_hook(hciemu->master_dev, hook_type, opcode, function,
+	return btdev_add_hook(hciemu->dev, hook_type, opcode, function,
 								user_data);
 }
 
@@ -588,5 +668,5 @@ bool hciemu_del_hook(struct hciemu *hciemu, enum hciemu_hook_type type,
 		return false;
 	}
 
-	return btdev_del_hook(hciemu->master_dev, hook_type, opcode);
+	return btdev_del_hook(hciemu->dev, hook_type, opcode);
 }
diff --git a/emulator/hciemu.h b/emulator/hciemu.h
index d0708277d..8bf2d070e 100644
--- a/emulator/hciemu.h
+++ b/emulator/hciemu.h
@@ -12,6 +12,7 @@
 #include <stdint.h>
 
 struct hciemu;
+struct hciemu_client;
 
 enum hciemu_type {
 	HCIEMU_TYPE_BREDRLE,
@@ -30,10 +31,15 @@ enum hciemu_hook_type {
 };
 
 struct hciemu *hciemu_new(enum hciemu_type type);
+struct hciemu *hciemu_new_num(enum hciemu_type type, uint8_t num);
 
 struct hciemu *hciemu_ref(struct hciemu *hciemu);
 void hciemu_unref(struct hciemu *hciemu);
 
+struct hciemu_client *hciemu_get_client(struct hciemu *hciemu, int num);
+struct bthost *hciemu_client_host(struct hciemu_client *client);
+const uint8_t *hciemu_client_bdaddr(struct hciemu_client *client);
+
 typedef void (*hciemu_debug_func_t)(const char *str, void *user_data);
 typedef void (*hciemu_destroy_func_t)(void *user_data);
 bool hciemu_set_debug(struct hciemu *hciemu, hciemu_debug_func_t callback,
-- 
2.26.2

[PATCH v2 3/3] bthost: Add support for address types other then public

[Luiz Augusto von Dentz]

From: Luiz Augusto von Dentz <luiz.von.dentz@intel.com>

This properly initialize the address type according to the connection
address.
---
 emulator/bthost.c |  9 +++++++--
 emulator/bthost.h |  5 +++--
 emulator/smp.c    | 24 +++++++++++++++++++-----
 3 files changed, 29 insertions(+), 9 deletions(-)

diff --git a/emulator/bthost.c b/emulator/bthost.c
index 3b42c0664..16934d35a 100644
--- a/emulator/bthost.c
+++ b/emulator/bthost.c
@@ -923,6 +923,7 @@ static void init_conn(struct bthost *bthost, uint16_t handle,
 {
 	struct btconn *conn;
 	const uint8_t *ia, *ra;
+	uint8_t ia_type, ra_type;
 
 	conn = malloc(sizeof(*conn));
 	if (!conn)
@@ -939,14 +940,18 @@ static void init_conn(struct bthost *bthost, uint16_t handle,
 
 	if (bthost->conn_init) {
 		ia = bthost->bdaddr;
+		ia_type = addr_type;
 		ra = conn->bdaddr;
+		ra_type = conn->addr_type;
 	} else {
 		ia = conn->bdaddr;
+		ia_type = conn->addr_type;
 		ra = bthost->bdaddr;
+		ra_type = addr_type;
 	}
 
-	conn->smp_data = smp_conn_add(bthost->smp_data, handle, ia, ra,
-						addr_type, bthost->conn_init);
+	conn->smp_data = smp_conn_add(bthost->smp_data, handle, ia, ia_type,
+					ra, ra_type, bthost->conn_init);
 
 	if (bthost->new_conn_cb)
 		bthost->new_conn_cb(conn->handle, bthost->new_conn_data);
diff --git a/emulator/bthost.h b/emulator/bthost.h
index 77f17fd69..7e5286df3 100644
--- a/emulator/bthost.h
+++ b/emulator/bthost.h
@@ -142,8 +142,9 @@ void bthost_start(struct bthost *bthost);
 
 void *smp_start(struct bthost *bthost);
 void smp_stop(void *smp_data);
-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);
+void *smp_conn_add(void *smp_data, uint16_t handle,
+			const uint8_t *ia, uint8_t ia_type,
+			const uint8_t *ra, uint8_t ra_type, bool conn_init);
 void smp_conn_del(void *conn_data);
 void smp_conn_encrypted(void *conn_data, uint8_t encrypt);
 void smp_data(void *conn_data, const void *data, uint16_t len);
diff --git a/emulator/smp.c b/emulator/smp.c
index db0065d7d..ec1baea04 100644
--- a/emulator/smp.c
+++ b/emulator/smp.c
@@ -834,8 +834,22 @@ void smp_conn_encrypted(void *conn_data, uint8_t encrypt)
 	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)
+static uint8_t type2hci(uint8_t addr_type)
+{
+	switch (addr_type) {
+	case BDADDR_BREDR:
+	case BDADDR_LE_PUBLIC:
+		return LE_PUBLIC_ADDRESS;
+	case BDADDR_LE_RANDOM:
+		return LE_RANDOM_ADDRESS;
+	}
+
+	return 0x00;
+}
+
+void *smp_conn_add(void *smp_data, uint16_t handle,
+			const uint8_t *ia, uint8_t ia_type,
+			const uint8_t *ra, uint8_t ra_type, bool conn_init)
 {
 	struct smp *smp = smp_data;
 	struct smp_conn *conn;
@@ -848,11 +862,11 @@ void *smp_conn_add(void *smp_data, uint16_t handle, const uint8_t *ia,
 
 	conn->smp = smp;
 	conn->handle = handle;
-	conn->addr_type = addr_type;
 	conn->out = conn_init;
+	conn->addr_type = conn_init ? ia_type : ra_type;
 
-	conn->ia_type = LE_PUBLIC_ADDRESS;
-	conn->ra_type = LE_PUBLIC_ADDRESS;
+	conn->ia_type = type2hci(ia_type);
+	conn->ra_type = type2hci(ra_type);
 	memcpy(conn->ia, ia, 6);
 	memcpy(conn->ra, ra, 6);
 
-- 
2.26.2

RE: [v2,1/3] btdev: Add support for multiple connections

[bluez.test.bot]

[-- Attachment #1: Type: text/plain, Size: 557 bytes --]

This is automated email and please do not reply to this email!

Dear submitter,

Thank you for submitting the patches to the linux bluetooth mailing list.
This is a CI test results with your patch series:
PW Link:https://patchwork.kernel.org/project/bluetooth/list/?series=403877

---Test result---

##############################
Test: CheckPatch - PASS

##############################
Test: CheckGitLint - PASS

##############################
Test: CheckBuild - PASS

##############################
Test: MakeCheck - PASS



---
Regards,
Linux Bluetooth

Re: [v2,1/3] btdev: Add support for multiple connections

[Luiz Augusto von Dentz]

Hi,

On Thu, Dec 17, 2020 at 5:20 PM <bluez.test.bot@gmail.com> wrote:
>
> This is automated email and please do not reply to this email!
>
> Dear submitter,
>
> Thank you for submitting the patches to the linux bluetooth mailing list.
> This is a CI test results with your patch series:
> PW Link:https://patchwork.kernel.org/project/bluetooth/list/?series=403877
>
> ---Test result---
>
> ##############################
> Test: CheckPatch - PASS
>
> ##############################
> Test: CheckGitLint - PASS
>
> ##############################
> Test: CheckBuild - PASS
>
> ##############################
> Test: MakeCheck - PASS
>
>
>
> ---
> Regards,
> Linux Bluetooth

Pushed.

-- 
Luiz Augusto von Dentz