bthost: Fix not handling ACL fragmentation

Large packets requires the support of ACL fragmentation in order to be
properly processed.

1 files changed, 100 insertions, 21 deletions

diff --git a/emulator/bthost.c b/emulator/bthost.c
index 66e337f34..ec7a7eb0d 100644
--- a/emulator/bthost.c
+++ b/emulator/bthost.c
@@ -203,6 +203,9 @@ 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;
@@ -491,6 +494,7 @@ void bthost_destroy(struct bthost *bthost)
 
 	queue_destroy(bthost->le_ext_adv, le_ext_adv_free);
 
+	free(bthost->acl_data);
 	free(bthost);
 }
 
@@ -2426,24 +2430,15 @@ static void process_rfcomm(struct bthost *bthost, struct btconn *conn,
 	}
 }
 
-static void process_acl(struct bthost *bthost, const void *data, uint16_t len)
+static void process_l2cap(struct bthost *bthost, uint16_t handle,
+					const void *data, uint16_t len)
 {
-	const struct bt_hci_acl_hdr *acl_hdr = data;
-	const struct bt_l2cap_hdr *l2_hdr = data + sizeof(*acl_hdr);
-	uint16_t handle, cid, acl_len, l2_len;
+	const struct bt_l2cap_hdr *l2_hdr = data;
 	struct cid_hook *hook;
 	struct btconn *conn;
 	struct l2conn *l2conn;
-	const void *l2_data;
-
-	if (len < sizeof(*acl_hdr) + sizeof(*l2_hdr))
-		return;
-
-	acl_len = le16_to_cpu(acl_hdr->dlen);
-	if (len != sizeof(*acl_hdr) + acl_len)
-		return;
+	uint16_t cid, l2_len;
 
-	handle = acl_handle(acl_hdr->handle);
 	conn = bthost_find_conn(bthost, handle);
 	if (!conn) {
 		bthost_debug(bthost, "ACL data for unknown handle 0x%04x",
@@ -2452,36 +2447,40 @@ static void process_acl(struct bthost *bthost, const void *data, uint16_t len)
 	}
 
 	l2_len = le16_to_cpu(l2_hdr->len);
-	if (len - sizeof(*acl_hdr) != sizeof(*l2_hdr) + l2_len)
+	if (len != sizeof(*l2_hdr) + l2_len) {
+		bthost_debug(bthost, "L2CAP invalid length: %u != %zu",
+					len, sizeof(*l2_hdr) + l2_len);
 		return;
+	}
 
-	l2_data = data + sizeof(*acl_hdr) + sizeof(*l2_hdr);
+	bthost_debug(bthost, "L2CAP data: %u bytes", l2_len);
 
 	cid = le16_to_cpu(l2_hdr->cid);
 
 	hook = find_cid_hook(conn, cid);
 	if (hook) {
-		hook->func(l2_data, l2_len, hook->user_data);
+		hook->func(l2_hdr->data, l2_len, hook->user_data);
 		return;
 	}
 
 	switch (cid) {
 	case 0x0001:
-		l2cap_sig(bthost, conn, l2_data, l2_len);
+		l2cap_sig(bthost, conn, l2_hdr->data, l2_len);
 		break;
 	case 0x0005:
-		l2cap_le_sig(bthost, conn, l2_data, l2_len);
+		l2cap_le_sig(bthost, conn, l2_hdr->data, l2_len);
 		break;
 	case 0x0006:
-		smp_data(conn->smp_data, l2_data, l2_len);
+		smp_data(conn->smp_data, l2_hdr->data, l2_len);
 		break;
 	case 0x0007:
-		smp_bredr_data(conn->smp_data, l2_data, l2_len);
+		smp_bredr_data(conn->smp_data, l2_hdr->data, l2_len);
 		break;
 	default:
 		l2conn = btconn_find_l2cap_conn_by_scid(conn, cid);
 		if (l2conn && l2conn->psm == 0x0003)
-			process_rfcomm(bthost, conn, l2conn, l2_data, l2_len);
+			process_rfcomm(bthost, conn, l2conn, l2_hdr->data,
+								l2_len);
 		else
 			bthost_debug(bthost,
 					"Packet for unknown CID 0x%04x (%u)",
@@ -2490,6 +2489,86 @@ static void process_acl(struct bthost *bthost, const void *data, uint16_t len)
 	}
 }
 
+static void append_acl_data(struct bthost *bthost, uint16_t handle,
+				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);
+		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);
+		return;
+	}
+
+	memcpy(bthost->acl_data + bthost->acl_len, data, len);
+	bthost->acl_len += len;
+
+	bthost_debug(bthost, "ACL data: %u/%u bytes", bthost->acl_len,
+							bthost->l2_len);
+
+	if (bthost->acl_len < bthost->l2_len)
+		return;
+
+	process_l2cap(bthost, handle, bthost->acl_data, bthost->acl_len);
+
+	free(bthost->acl_data);
+	bthost->acl_data = NULL;
+	bthost->acl_len = 0;
+	bthost->l2_len = 0;
+}
+
+static void process_acl(struct bthost *bthost, const void *data, uint16_t len)
+{
+	const struct bt_hci_acl_hdr *acl_hdr = data;
+	const struct bt_l2cap_hdr *l2_hdr = (void *) acl_hdr->data;
+	uint16_t handle, acl_len, l2_len;
+	uint8_t flags;
+
+	acl_len = le16_to_cpu(acl_hdr->dlen);
+	if (len != sizeof(*acl_hdr) + acl_len)
+		return;
+
+	handle = acl_handle(acl_hdr->handle);
+	flags = acl_flags(acl_hdr->handle);
+
+	switch (flags) {
+	case 0x00:	/* start of a non-automatically-flushable PDU */
+	case 0x02:	/* start of an automatically-flushable PDU */
+		if (bthost->acl_data) {
+			bthost_debug(bthost, "Unexpected ACL start frame");
+			free(bthost->acl_data);
+			bthost->acl_data = NULL;
+			bthost->acl_len = 0;
+		}
+
+		l2_len = le16_to_cpu(l2_hdr->len) + sizeof(*l2_hdr);
+
+		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);
+			break;
+		}
+
+		bthost->acl_data = malloc(l2_len);
+		bthost->acl_len = 0;
+		bthost->l2_len = l2_len;
+		/* fall through */
+	case 0x01:	/* continuing fragment */
+		append_acl_data(bthost, handle, flags, acl_hdr->data, acl_len);
+		break;
+	case 0x03:	/* complete automatically-flushable PDU */
+		process_l2cap(bthost, handle, acl_hdr->data, acl_len);
+		break;
+	default:
+		bthost_debug(bthost, "Invalid ACL frame flags 0x%2.2x", flags);
+	}
+}
+
 void bthost_receive_h4(struct bthost *bthost, const void *data, uint16_t len)
 {
 	uint8_t pkt_type;