From 05f2ba73c7bb9477b60168f270adbff9e234f229 Mon Sep 17 00:00:00 2001 From: Luiz Augusto von Dentz Date: Wed, 9 Feb 2022 13:57:21 -0800 Subject: [PATCH] bthost: Fix not handling ACL fragmentation Large packets requires the support of ACL fragmentation in order to be properly processed. Signed-off-by: Manika Shrivastava Signed-off-by: Ayush Garg --- 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 db4d143..7d9cacb 100755 --- a/emulator/bthost.c +++ b/emulator/bthost.c @@ -202,6 +202,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; @@ -490,6 +493,7 @@ void bthost_destroy(struct bthost *bthost) queue_destroy(bthost->le_ext_adv, le_ext_adv_free); + free(bthost->acl_data); free(bthost); } @@ -2425,24 +2429,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", @@ -2451,36 +2446,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)", @@ -2489,6 +2488,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 3e6b4df..7a61d26 100755 --- 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 { @@ -3662,6 +3663,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.7.4