From e700d5526c9012a1feda11bdb3fdbdc210dfc0ce Mon Sep 17 00:00:00 2001 From: Luiz Augusto von Dentz Date: Wed, 9 Feb 2022 13:57:21 -0800 Subject: bthost: Fix not handling ACL fragmentation Large packets requires the support of ACL fragmentation in order to be properly processed. --- emulator/bthost.c | 121 ++++++++++++++++++++++++++++++++++++++++++++---------- 1 file changed, 100 insertions(+), 21 deletions(-) (limited to 'emulator') 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; -- cgit v1.2.1