From a6fa17f90dcbadc3fa294bc6a6a5755eb9dcd436 Mon Sep 17 00:00:00 2001 From: Marcel Holtmann Date: Wed, 14 Nov 2012 00:46:03 +0900 Subject: [PATCH] monitor: Support proper ACL packet fragmentation --- monitor/l2cap.c | 185 +++++++++++++++++++++++++++++++++++++++-------- monitor/l2cap.h | 3 +- monitor/packet.c | 26 +------ 3 files changed, 158 insertions(+), 56 deletions(-) diff --git a/monitor/l2cap.c b/monitor/l2cap.c index 355849ac4..999e7708f 100644 --- a/monitor/l2cap.c +++ b/monitor/l2cap.c @@ -27,6 +27,7 @@ #endif #include +#include #include @@ -35,6 +36,25 @@ #include "display.h" #include "l2cap.h" +#define MAX_INDEX 16 + +struct index_data { + void *frag_buf; + uint16_t frag_pos; + uint16_t frag_len; + uint16_t frag_cid; +}; + +static struct index_data index_list[MAX_INDEX]; + +static void clear_fragment_buffer(uint16_t index) +{ + free(index_list[index].frag_buf); + index_list[index].frag_buf = NULL; + index_list[index].frag_pos = 0; + index_list[index].frag_len = 0; +} + static void print_psm(uint16_t psm) { print_field("PSM: %d (0x%4.4x)", btohs(psm), btohs(psm)); @@ -783,52 +803,153 @@ static void smp_packet(const void *data, uint16_t size) packet_hexdump(data + 1, size - 1); } -void l2cap_packet(uint16_t handle, bool in, const void *data, uint16_t size) +static void l2cap_frame(uint16_t index, bool in, uint16_t handle, + uint16_t cid, const void *data, uint16_t size) { - const struct bt_l2cap_hdr *hdr = data; - uint16_t len, cid; - - if (size < sizeof(*hdr)) { - print_text(COLOR_ERROR, "malformed packet"); - packet_hexdump(data, size); - return; - } - - len = btohs(hdr->len); - cid = btohs(hdr->cid); - - data += sizeof(*hdr); - size -= sizeof(*hdr); - - if (len != size) { - print_text(COLOR_ERROR, "invalid packet size"); - packet_hexdump(data, size); - return; - } - - switch (btohs(hdr->cid)) { + switch (cid) { case 0x0001: case 0x0005: - sig_packet(in, data, len); + sig_packet(in, data, size); break; case 0x0003: - amp_packet(data, len); + amp_packet(data, size); break; case 0x0004: - att_packet(data, len); + att_packet(data, size); break; case 0x0006: - smp_packet(data, len); + smp_packet(data, size); break; default: print_indent(6, COLOR_CYAN, "Channel:", "", COLOR_OFF, - " %d len %d", cid, len); - packet_hexdump(data, len); + " %d len %d", cid, size); + packet_hexdump(data, size); break; } +} + +void l2cap_packet(uint16_t index, bool in, uint16_t handle, uint8_t flags, + const void *data, uint16_t size) +{ + const struct bt_l2cap_hdr *hdr = data; + uint16_t len, cid; + + if (index > MAX_INDEX - 1) { + print_text(COLOR_ERROR, "controller index too large"); + packet_hexdump(data, size); + return; + } + + switch (flags) { + case 0x00: /* start of a non-automatically-flushable PDU */ + case 0x02: /* start of an automatically-flushable PDU */ + if (index_list[index].frag_len) { + print_text(COLOR_ERROR, "unexpected start frame"); + packet_hexdump(data, size); + clear_fragment_buffer(index); + return; + } - data += len; - size -= len; + if (size < sizeof(*hdr)) { + print_text(COLOR_ERROR, "frame too short"); + packet_hexdump(data, size); + return; + } - packet_hexdump(data, size); + len = btohs(hdr->len); + cid = btohs(hdr->cid); + + data += sizeof(*hdr); + size -= sizeof(*hdr); + + if (len == size) { + /* complete frame */ + l2cap_frame(index, in, handle, cid, data, len); + return; + } + + if (size > len) { + print_text(COLOR_ERROR, "frame too long"); + packet_hexdump(data, size); + return; + } + + index_list[index].frag_buf = malloc(len); + if (!index_list[index].frag_buf) { + print_text(COLOR_ERROR, "failed buffer allocation"); + packet_hexdump(data, size); + return; + } + + memcpy(index_list[index].frag_buf, data, size); + index_list[index].frag_pos = size; + index_list[index].frag_len = len - size; + index_list[index].frag_cid = cid; + break; + + case 0x01: /* continuing fragment */ + if (!index_list[index].frag_len) { + print_text(COLOR_ERROR, "unexpected continuation"); + packet_hexdump(data, size); + return; + } + + if (size > index_list[index].frag_len) { + print_text(COLOR_ERROR, "fragment too long"); + packet_hexdump(data, size); + clear_fragment_buffer(index); + return; + } + + memcpy(index_list[index].frag_buf + + index_list[index].frag_pos, data, size); + index_list[index].frag_pos += size; + index_list[index].frag_len -= size; + + if (!index_list[index].frag_len) { + /* complete frame */ + l2cap_frame(index, in, handle, + index_list[index].frag_cid, + data, index_list[index].frag_pos); + clear_fragment_buffer(index); + return; + } + break; + + case 0x03: /* complete automatically-flushable PDU */ + if (index_list[index].frag_len) { + print_text(COLOR_ERROR, "unexpected complete frame"); + packet_hexdump(data, size); + clear_fragment_buffer(index); + return; + } + + if (size < sizeof(*hdr)) { + print_text(COLOR_ERROR, "frame too short"); + packet_hexdump(data, size); + return; + } + + len = btohs(hdr->len); + cid = btohs(hdr->cid); + + data += sizeof(*hdr); + size -= sizeof(*hdr); + + if (len != size) { + print_text(COLOR_ERROR, "wrong frame size"); + packet_hexdump(data, size); + return; + } + + /* complete frame */ + l2cap_frame(index, in, handle, cid, data, len); + break; + + default: + print_text(COLOR_ERROR, "invalid packet flags (0x%2.2x)", + flags); + packet_hexdump(data, size); + return; + } } diff --git a/monitor/l2cap.h b/monitor/l2cap.h index 469d094e7..503bacaf9 100644 --- a/monitor/l2cap.h +++ b/monitor/l2cap.h @@ -25,4 +25,5 @@ #include #include -void l2cap_packet(uint16_t handle, bool in, const void *data, uint16_t size); +void l2cap_packet(uint16_t index, bool in, uint16_t handle, uint8_t flags, + const void *data, uint16_t size); diff --git a/monitor/packet.c b/monitor/packet.c index 94539e65a..f15390e37 100644 --- a/monitor/packet.c +++ b/monitor/packet.c @@ -1491,8 +1491,6 @@ struct monitor_new_index { struct index_data { bdaddr_t bdaddr; - void *frag_buf; - uint16_t frag_len; }; static struct index_data index_list[MAX_INDEX]; @@ -1572,18 +1570,15 @@ void packet_monitor(struct timeval *tv, uint16_t index, uint16_t opcode, if (index < MAX_INDEX) { bacpy(&index_list[index].bdaddr, &ni->bdaddr); - index_list[index].frag_buf = NULL; - index_list[index].frag_len = 0; } ba2str(&ni->bdaddr, str); packet_new_index(tv, index, str, ni->type, ni->bus, ni->name); break; case MONITOR_DEL_INDEX: - if (index < MAX_INDEX) { + if (index < MAX_INDEX) ba2str(&index_list[index].bdaddr, str); - free(index_list[index].frag_buf); - } else + else ba2str(BDADDR_ANY, str); packet_del_index(tv, index, str); @@ -4548,22 +4543,7 @@ void packet_hci_acldata(struct timeval *tv, uint16_t index, bool in, if (filter_mask & PACKET_FILTER_SHOW_ACL_DATA) packet_hexdump(data, size); - if (index > MAX_INDEX - 1) - return; - - switch (flags) { - case 0x00: /* start of a non-automatically-flushable PDU */ - case 0x02: /* start of an automatically-flushable PDU */ - if (index_list[index].frag_len == 0) - l2cap_packet(acl_handle(handle), in, data, size); - index_list[index].frag_len = 0; - break; - default: - print_text(COLOR_ERROR, "invalid packet flags (0x%2.2x)", - flags); - packet_hexdump(data, size); - break; - } + l2cap_packet(index, in, acl_handle(handle), flags, data, size); } void packet_hci_scodata(struct timeval *tv, uint16_t index, bool in, -- 2.47.3