bthost: Add support for BT_H4_ISO_PKT

This adds sending and receiving BT_H4_ISO_PKT packets.

3 files changed, 326 insertions, 42 deletions

diff --git a/emulator/bthost.c b/emulator/bthost.c
index d751e7dd4..54a2728e0 100644
--- a/emulator/bthost.c
+++ b/emulator/bthost.c
@@ -37,6 +37,9 @@
 #define acl_handle(h)		(h & 0x0fff)
 #define acl_flags(h)		(h >> 12)
 
+#define iso_flags_pb(f)		(f & 0x0003)
+#define iso_data_len_pack(h, f)	((uint16_t) ((h) | ((f) << 14)))
+
 #define L2CAP_FEAT_FIXED_CHAN	0x00000080
 #define L2CAP_FC_SIG_BREDR	0x02
 #define L2CAP_FC_SMP_BREDR	0x80
@@ -131,6 +134,11 @@ struct rfcomm_chan_hook {
 	struct rfcomm_chan_hook *next;
 };
 
+struct iso_hook {
+	bthost_cid_hook_func_t func;
+	void *user_data;
+};
+
 struct btconn {
 	uint16_t handle;
 	uint8_t bdaddr[6];
@@ -142,8 +150,12 @@ struct btconn {
 	struct rcconn *rcconns;
 	struct cid_hook *cid_hooks;
 	struct rfcomm_chan_hook *rfcomm_chan_hooks;
+	struct iso_hook *iso_hook;
 	struct btconn *next;
 	void *smp_data;
+	uint16_t recv_len;
+	uint16_t data_len;
+	void *recv_data;
 };
 
 struct l2conn {
@@ -203,9 +215,6 @@ struct bthost {
 	uint8_t bdaddr[6];
 	uint8_t features[8];
 	bthost_send_func send_handler;
-	uint16_t acl_len;
-	uint16_t l2_len;
-	void *acl_data;
 	void *send_data;
 	struct cmd_queue cmd_q;
 	uint8_t ncmd;
@@ -214,6 +223,8 @@ struct bthost {
 	void *cmd_complete_data;
 	bthost_new_conn_cb new_conn_cb;
 	void *new_conn_data;
+	bthost_new_conn_cb new_iso_cb;
+	void *new_iso_data;
 	struct rfcomm_connection_data *rfcomm_conn_data;
 	struct l2cap_conn_cb_data *new_l2cap_conn_data;
 	struct rfcomm_conn_cb_data *new_rfcomm_conn_data;
@@ -295,6 +306,8 @@ static void btconn_free(struct btconn *conn)
 		free(hook);
 	}
 
+	free(conn->iso_hook);
+	free(conn->recv_data);
 	free(conn);
 }
 
@@ -494,7 +507,6 @@ void bthost_destroy(struct bthost *bthost)
 
 	queue_destroy(bthost->le_ext_adv, le_ext_adv_free);
 
-	free(bthost->acl_data);
 	free(bthost);
 }
 
@@ -663,6 +675,28 @@ void bthost_add_cid_hook(struct bthost *bthost, uint16_t handle, uint16_t cid,
 	conn->cid_hooks = hook;
 }
 
+void bthost_add_iso_hook(struct bthost *bthost, uint16_t handle,
+				bthost_cid_hook_func_t func, void *user_data)
+{
+	struct iso_hook *hook;
+	struct btconn *conn;
+
+	conn = bthost_find_conn(bthost, handle);
+	if (!conn || conn->iso_hook)
+		return;
+
+	hook = malloc(sizeof(*hook));
+	if (!hook)
+		return;
+
+	memset(hook, 0, sizeof(*hook));
+
+	hook->func = func;
+	hook->user_data = user_data;
+
+	conn->iso_hook = hook;
+}
+
 void bthost_send_cid(struct bthost *bthost, uint16_t handle, uint16_t cid,
 					const void *data, uint16_t len)
 {
@@ -687,6 +721,52 @@ void bthost_send_cid_v(struct bthost *bthost, uint16_t handle, uint16_t cid,
 	send_iov(bthost, handle, cid, iov, iovcnt);
 }
 
+static void send_iso(struct bthost *bthost, uint16_t handle,
+					const struct iovec *iov, int iovcnt)
+{
+	struct bt_hci_iso_hdr iso_hdr;
+	struct bt_hci_iso_data_start data_hdr;
+	uint8_t pkt = BT_H4_ISO_PKT;
+	struct iovec pdu[3 + iovcnt];
+	int i, len = 0;
+	static uint16_t sn;
+
+	for (i = 0; i < iovcnt; i++) {
+		pdu[3 + i].iov_base = iov[i].iov_base;
+		pdu[3 + i].iov_len = iov[i].iov_len;
+		len += iov[i].iov_len;
+	}
+
+	pdu[0].iov_base = &pkt;
+	pdu[0].iov_len = sizeof(pkt);
+
+	iso_hdr.handle = acl_handle_pack(handle, 0x02);
+	iso_hdr.dlen = cpu_to_le16(len + sizeof(data_hdr));
+
+	pdu[1].iov_base = &iso_hdr;
+	pdu[1].iov_len = sizeof(iso_hdr);
+
+	data_hdr.sn = cpu_to_le16(sn++);
+	data_hdr.slen = cpu_to_le16(iso_data_len_pack(len, 0));
+
+	pdu[2].iov_base = &data_hdr;
+	pdu[2].iov_len = sizeof(data_hdr);
+
+	send_packet(bthost, pdu, 3 + iovcnt);
+}
+
+void bthost_send_iso(struct bthost *bthost, uint16_t handle,
+					const struct iovec *iov, int iovcnt)
+{
+	struct btconn *conn;
+
+	conn = bthost_find_conn(bthost, handle);
+	if (!conn)
+		return;
+
+	send_iso(bthost, handle, iov, iovcnt);
+}
+
 bool bthost_l2cap_req(struct bthost *bthost, uint16_t handle, uint8_t code,
 				const void *data, uint16_t len,
 				bthost_l2cap_rsp_cb cb, void *user_data)
@@ -1337,6 +1417,40 @@ static void evt_le_ltk_request(struct bthost *bthost, const void *data,
 								sizeof(cp));
 }
 
+static void init_iso(struct bthost *bthost, uint16_t handle,
+				const uint8_t *bdaddr, uint8_t addr_type)
+{
+	struct btconn *conn;
+
+	bthost_debug(bthost, "ISO handle 0x%4.4x", handle);
+
+	conn = malloc(sizeof(*conn));
+	if (!conn)
+		return;
+
+	memset(conn, 0, sizeof(*conn));
+	conn->handle = handle;
+	memcpy(conn->bdaddr, bdaddr, 6);
+	conn->addr_type = addr_type;
+
+	conn->next = bthost->conns;
+	bthost->conns = conn;
+
+	if (bthost->new_iso_cb)
+		bthost->new_iso_cb(handle, bthost->new_iso_data);
+}
+
+static void evt_le_cis_established(struct bthost *bthost, const void *data,
+							uint8_t size)
+{
+	const struct bt_hci_evt_le_cis_established *ev = data;
+
+	if (ev->status)
+		return;
+
+	init_iso(bthost, ev->conn_handle, BDADDR_ANY->b, BDADDR_LE_PUBLIC);
+}
+
 static void evt_le_cis_req(struct bthost *bthost, const void *data, uint8_t len)
 {
 	const struct bt_hci_evt_le_cis_req *ev = data;
@@ -1384,6 +1498,38 @@ static void evt_le_ext_adv_report(struct bthost *bthost, const void *data,
 	}
 }
 
+static void evt_le_big_complete(struct bthost *bthost, const void *data,
+							uint8_t size)
+{
+	const struct bt_hci_evt_le_big_complete *ev = data;
+	int i;
+
+	if (ev->status)
+		return;
+
+	for (i = 0; i < ev->num_bis; i++) {
+		uint16_t handle = le16_to_cpu(ev->bis_handle[i]);
+
+		init_iso(bthost, handle, BDADDR_ANY->b, BDADDR_LE_PUBLIC);
+	}
+}
+
+static void evt_le_big_sync_established(struct bthost *bthost,
+						const void *data, uint8_t size)
+{
+	const struct bt_hci_evt_le_big_sync_estabilished *ev = data;
+	int i;
+
+	if (ev->status)
+		return;
+
+	for (i = 0; i < ev->num_bis; i++) {
+		uint16_t handle = le16_to_cpu(ev->bis[i]);
+
+		init_iso(bthost, handle, BDADDR_ANY->b, BDADDR_LE_PUBLIC);
+	}
+}
+
 static void evt_le_meta_event(struct bthost *bthost, const void *data,
 								uint8_t len)
 {
@@ -1393,7 +1539,7 @@ static void evt_le_meta_event(struct bthost *bthost, const void *data,
 	if (len < 1)
 		return;
 
-	bthost_debug(bthost, "event 0x%02x", *event);
+	bthost_debug(bthost, "meta event 0x%02x", *event);
 
 	switch (*event) {
 	case BT_HCI_EVT_LE_CONN_COMPLETE:
@@ -1414,9 +1560,18 @@ static void evt_le_meta_event(struct bthost *bthost, const void *data,
 	case BT_HCI_EVT_LE_EXT_ADV_REPORT:
 		evt_le_ext_adv_report(bthost, evt_data, len - 1);
 		break;
+	case BT_HCI_EVT_LE_CIS_ESTABLISHED:
+		evt_le_cis_established(bthost, evt_data, len - 1);
+		break;
 	case BT_HCI_EVT_LE_CIS_REQ:
 		evt_le_cis_req(bthost, evt_data, len - 1);
 		break;
+	case BT_HCI_EVT_LE_BIG_COMPLETE:
+		evt_le_big_complete(bthost, evt_data, len - 1);
+		break;
+	case BT_HCI_EVT_LE_BIG_SYNC_ESTABILISHED:
+		evt_le_big_sync_established(bthost, evt_data, len - 1);
+		break;
 	default:
 		bthost_debug(bthost, "Unsupported LE Meta event 0x%2.2x",
 								*event);
@@ -2437,22 +2592,14 @@ static void process_rfcomm(struct bthost *bthost, struct btconn *conn,
 	}
 }
 
-static void process_l2cap(struct bthost *bthost, uint16_t handle,
+static void process_l2cap(struct bthost *bthost, struct btconn *conn,
 					const void *data, uint16_t len)
 {
 	const struct bt_l2cap_hdr *l2_hdr = data;
 	struct cid_hook *hook;
-	struct btconn *conn;
 	struct l2conn *l2conn;
 	uint16_t cid, l2_len;
 
-	conn = bthost_find_conn(bthost, handle);
-	if (!conn) {
-		bthost_debug(bthost, "ACL data for unknown handle 0x%04x",
-								handle);
-		return;
-	}
-
 	l2_len = le16_to_cpu(l2_hdr->len);
 	if (len != sizeof(*l2_hdr) + l2_len) {
 		bthost_debug(bthost, "L2CAP invalid length: %u != %zu",
@@ -2496,36 +2643,55 @@ static void process_l2cap(struct bthost *bthost, uint16_t handle,
 	}
 }
 
-static void append_acl_data(struct bthost *bthost, uint16_t handle,
-				uint8_t flags, const void *data, uint16_t len)
+static void append_recv_data(struct bthost *bthost, struct btconn *conn,
+				const char *type, uint8_t flags,
+				const void *data, uint16_t len)
 {
-	if (!bthost->acl_data) {
-		bthost_debug(bthost, "Unexpected ACL frame: handle 0x%4.4x "
-				"flags 0x%2.2x", handle, flags);
+	if (!conn->recv_data) {
+		bthost_debug(bthost, "Unexpected %s frame: handle 0x%4.4x "
+				"flags 0x%2.2x", type, conn->handle, flags);
 		return;
 	}
 
-	if (bthost->acl_len + len > bthost->l2_len) {
-		bthost_debug(bthost, "Unexpected ACL frame: handle 0x%4.4x "
-				"flags 0x%2.2x", handle, flags);
+	if (conn->recv_len + len > conn->data_len) {
+		bthost_debug(bthost, "Unexpected %s frame: handle 0x%4.4x "
+				"flags 0x%2.2x", type, conn->handle, flags);
 		return;
 	}
 
-	memcpy(bthost->acl_data + bthost->acl_len, data, len);
-	bthost->acl_len += len;
+	memcpy(conn->recv_data + conn->recv_len, data, len);
+	conn->recv_len += len;
 
-	bthost_debug(bthost, "ACL data: %u/%u bytes", bthost->acl_len,
-							bthost->l2_len);
+	bthost_debug(bthost, "%s data: %u/%u bytes", type, conn->recv_len,
+							conn->data_len);
+}
 
-	if (bthost->acl_len < bthost->l2_len)
+static void free_recv_data(struct btconn *conn)
+{
+	free(conn->recv_data);
+	conn->recv_data = NULL;
+	conn->recv_len = 0;
+	conn->data_len = 0;
+}
+
+static void append_acl_data(struct bthost *bthost, struct btconn *conn,
+				uint8_t flags, const void *data, uint16_t len)
+{
+	append_recv_data(bthost, conn, "ACL", flags, data, len);
+
+	if (conn->recv_len < conn->data_len)
 		return;
 
-	process_l2cap(bthost, handle, bthost->acl_data, bthost->acl_len);
+	process_l2cap(bthost, conn, conn->recv_data, conn->recv_len);
 
-	free(bthost->acl_data);
-	bthost->acl_data = NULL;
-	bthost->acl_len = 0;
-	bthost->l2_len = 0;
+	free_recv_data(conn);
+}
+
+static void new_recv_data(struct btconn *conn, uint16_t len)
+{
+	conn->recv_data = malloc(len);
+	conn->recv_len = 0;
+	conn->data_len = len;
 }
 
 static void process_acl(struct bthost *bthost, const void *data, uint16_t len)
@@ -2534,6 +2700,7 @@ static void process_acl(struct bthost *bthost, const void *data, uint16_t len)
 	const struct bt_l2cap_hdr *l2_hdr = (void *) acl_hdr->data;
 	uint16_t handle, acl_len, l2_len;
 	uint8_t flags;
+	struct btconn *conn;
 
 	acl_len = le16_to_cpu(acl_hdr->dlen);
 	if (len != sizeof(*acl_hdr) + acl_len)
@@ -2542,14 +2709,18 @@ static void process_acl(struct bthost *bthost, const void *data, uint16_t len)
 	handle = acl_handle(acl_hdr->handle);
 	flags = acl_flags(acl_hdr->handle);
 
+	conn = bthost_find_conn(bthost, handle);
+	if (!conn) {
+		bthost_debug(bthost, "Unknown handle: 0x%4.4x", handle);
+		return;
+	}
+
 	switch (flags) {
 	case 0x00:	/* start of a non-automatically-flushable PDU */
 	case 0x02:	/* start of an automatically-flushable PDU */
-		if (bthost->acl_data) {
+		if (conn->recv_data) {
 			bthost_debug(bthost, "Unexpected ACL start frame");
-			free(bthost->acl_data);
-			bthost->acl_data = NULL;
-			bthost->acl_len = 0;
+			free_recv_data(conn);
 		}
 
 		l2_len = le16_to_cpu(l2_hdr->len) + sizeof(*l2_hdr);
@@ -2557,25 +2728,116 @@ static void process_acl(struct bthost *bthost, const void *data, uint16_t len)
 		bthost_debug(bthost, "acl_len %u l2_len %u", acl_len, l2_len);
 
 		if (acl_len == l2_len) {
-			process_l2cap(bthost, handle, acl_hdr->data, acl_len);
+			process_l2cap(bthost, conn, acl_hdr->data, acl_len);
 			break;
 		}
 
-		bthost->acl_data = malloc(l2_len);
-		bthost->acl_len = 0;
-		bthost->l2_len = l2_len;
+		new_recv_data(conn, l2_len);
 		/* fall through */
 	case 0x01:	/* continuing fragment */
-		append_acl_data(bthost, handle, flags, acl_hdr->data, acl_len);
+		append_acl_data(bthost, conn, flags, acl_hdr->data, acl_len);
 		break;
 	case 0x03:	/* complete automatically-flushable PDU */
-		process_l2cap(bthost, handle, acl_hdr->data, acl_len);
+		process_l2cap(bthost, conn, acl_hdr->data, acl_len);
 		break;
 	default:
 		bthost_debug(bthost, "Invalid ACL frame flags 0x%2.2x", flags);
 	}
 }
 
+static void process_iso_data(struct bthost *bthost, struct btconn *conn,
+					const void *data, uint16_t len)
+{
+	const struct bt_hci_iso_data_start *data_hdr = data;
+	uint16_t data_len;
+	struct iso_hook *hook;
+
+	data_len = le16_to_cpu(data_hdr->slen);
+	if (len != sizeof(*data_hdr) + data_len) {
+		bthost_debug(bthost, "ISO invalid length: %u != %zu",
+					len, sizeof(*data_hdr) + data_len);
+		return;
+	}
+
+	bthost_debug(bthost, "ISO data: %u bytes (%u)", data_len, data_hdr->sn);
+
+	hook = conn->iso_hook;
+	if (!hook)
+		return;
+
+	hook->func(data_hdr->data, data_len, hook->user_data);
+}
+
+static void append_iso_data(struct bthost *bthost, struct btconn *conn,
+				uint8_t flags, const void *data, uint16_t len)
+{
+	append_recv_data(bthost, conn, "ISO", flags, data, len);
+
+	if (conn->recv_len < conn->data_len) {
+		if (flags == 0x03) {
+			bthost_debug(bthost, "Unexpected ISO end frame");
+			free_recv_data(conn);
+		}
+		return;
+	}
+
+	process_iso_data(bthost, conn, conn->recv_data, conn->recv_len);
+
+	free_recv_data(conn);
+}
+
+static void process_iso(struct bthost *bthost, const void *data, uint16_t len)
+{
+	const struct bt_hci_iso_hdr *iso_hdr = data;
+	const struct bt_hci_iso_data_start *data_hdr;
+	uint16_t handle, iso_len, data_len;
+	uint8_t flags;
+	struct btconn *conn;
+
+	iso_len = le16_to_cpu(iso_hdr->dlen);
+	if (len != sizeof(*iso_hdr) + iso_len)
+		return;
+
+	handle = acl_handle(iso_hdr->handle);
+	flags = iso_flags_pb(acl_flags(iso_hdr->handle));
+
+	conn = bthost_find_conn(bthost, handle);
+	if (!conn) {
+		bthost_debug(bthost, "Unknown handle: 0x%4.4x", handle);
+		return;
+	}
+
+	data_hdr = (void *) data + sizeof(*iso_hdr);
+
+	switch (flags) {
+	case 0x00:
+	case 0x02:
+		if (conn->recv_data) {
+			bthost_debug(bthost, "Unexpected ISO start frame");
+			free_recv_data(conn);
+		}
+
+		data_len = le16_to_cpu(data_hdr->slen) + sizeof(*data_hdr);
+
+		bthost_debug(bthost, "iso_len %u data_len %u", iso_len,
+								data_len);
+
+		if (iso_len == data_len) {
+			process_iso_data(bthost, conn, iso_hdr->data, iso_len);
+			break;
+		}
+
+		new_recv_data(conn, data_len);
+		/* fall through */
+	case 0x01:
+	case 0x03:
+		append_iso_data(bthost, conn, flags, iso_hdr->data, iso_len);
+		break;
+	default:
+		bthost_debug(bthost, "Invalid ISO frame flags 0x%2.2x", flags);
+	}
+}
+
 void bthost_receive_h4(struct bthost *bthost, const void *data, uint16_t len)
 {
 	uint8_t pkt_type;
@@ -2598,6 +2860,9 @@ void bthost_receive_h4(struct bthost *bthost, const void *data, uint16_t len)
 	case BT_H4_ACL_PKT:
 		process_acl(bthost, data + 1, len - 1);
 		break;
+	case BT_H4_ISO_PKT:
+		process_iso(bthost, data + 1, len - 1);
+		break;
 	default:
 		bthost_debug(bthost, "Unsupported packet 0x%2.2x", pkt_type);
 		break;
@@ -2618,6 +2883,13 @@ void bthost_set_connect_cb(struct bthost *bthost, bthost_new_conn_cb cb,
 	bthost->new_conn_data = user_data;
 }
 
+void bthost_set_iso_cb(struct bthost *bthost, bthost_new_conn_cb cb,
+							void *user_data)
+{
+	bthost->new_iso_cb = cb;
+	bthost->new_iso_data = user_data;
+}
+
 void bthost_hci_connect(struct bthost *bthost, const uint8_t *bdaddr,
 							uint8_t addr_type)
 {
diff --git a/emulator/bthost.h b/emulator/bthost.h
index 868af5469..f597d7596 100644
--- a/emulator/bthost.h
+++ b/emulator/bthost.h
@@ -47,6 +47,9 @@ typedef void (*bthost_new_conn_cb) (uint16_t handle, void *user_data);
 void bthost_set_connect_cb(struct bthost *bthost, bthost_new_conn_cb cb,
 							void *user_data);
 
+void bthost_set_iso_cb(struct bthost *bthost, bthost_new_conn_cb cb,
+							void *user_data);
+
 void bthost_hci_connect(struct bthost *bthost, const uint8_t *bdaddr,
 							uint8_t addr_type);
 
@@ -62,10 +65,18 @@ typedef void (*bthost_cid_hook_func_t)(const void *data, uint16_t len,
 void bthost_add_cid_hook(struct bthost *bthost, uint16_t handle, uint16_t cid,
 				bthost_cid_hook_func_t func, void *user_data);
 
+typedef void (*bthost_iso_hook_func_t)(const void *data, uint16_t len,
+							void *user_data);
+
+void bthost_add_iso_hook(struct bthost *bthost, uint16_t handle,
+				bthost_iso_hook_func_t func, void *user_data);
+
 void bthost_send_cid(struct bthost *bthost, uint16_t handle, uint16_t cid,
 					const void *data, uint16_t len);
 void bthost_send_cid_v(struct bthost *bthost, uint16_t handle, uint16_t cid,
 					const struct iovec *iov, int iovcnt);
+void bthost_send_iso(struct bthost *bthost, uint16_t handle,
+					const struct iovec *iov, int iovcnt);
 
 typedef void (*bthost_l2cap_rsp_cb) (uint8_t code, const void *data,
 						uint16_t len, void *user_data);
diff --git a/emulator/hciemu.c b/emulator/hciemu.c
index 057f76ff3..dcfed9be7 100644
--- a/emulator/hciemu.c
+++ b/emulator/hciemu.c
@@ -190,6 +190,7 @@ static gboolean receive_btdev(GIOChannel *channel, GIOCondition condition,
 	case BT_H4_CMD_PKT:
 	case BT_H4_ACL_PKT:
 	case BT_H4_SCO_PKT:
+	case BT_H4_ISO_PKT:
 		btdev_receive_h4(btdev, buf, len);
 		break;
 	}