From: Luiz Augusto von Dentz <luiz.von.dentz@xxxxxxxxx>
Large packets requires the support of ACL fragmentation in order to be
properly processed.
---
emulator/bthost.c | 121 ++++++++++++++++++++++++++++++++++++++--------
monitor/bt.h | 2 +
2 files changed, 102 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;
diff --git a/monitor/bt.h b/monitor/bt.h
index dc4e11c3c..66ed3ef5a 100644
--- a/monitor/bt.h
+++ b/monitor/bt.h
@@ -488,6 +488,7 @@ struct bt_hci_cmd_hdr {
struct bt_hci_acl_hdr {
uint16_t handle;
uint16_t dlen;
+ uint8_t data[];
} __attribute__ ((packed));
struct bt_hci_sco_hdr {
@@ -3663,6 +3664,7 @@ struct bt_hci_evt_le_req_peer_sca_complete {
struct bt_l2cap_hdr {
uint16_t len;
uint16_t cid;
+ uint8_t data[];
} __attribute__ ((packed));
struct bt_l2cap_hdr_sig {
--
2.34.1
