diff --git a/monitor/l2cap.c b/monitor/l2cap.c
index 355849a..999e770 100644
--- a/monitor/l2cap.c
+++ b/monitor/l2cap.c
#endif
#include <inttypes.h>
+#include <stdlib.h>
#include <bluetooth/bluetooth.h>
#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));
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 469d094..503baca 100644
--- a/monitor/l2cap.h
+++ b/monitor/l2cap.h
#include <stdint.h>
#include <stdbool.h>
-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 94539e6..f15390e 100644
--- a/monitor/packet.c
+++ b/monitor/packet.c
struct index_data {
bdaddr_t bdaddr;
- void *frag_buf;
- uint16_t frag_len;
};
static struct index_data index_list[MAX_INDEX];
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);
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,