From c9d51f571af62c1299160f4ca331699d3cd307d1 Mon Sep 17 00:00:00 2001 From: Marcel Holtmann Date: Sat, 3 Nov 2012 10:25:03 +0100 Subject: [PATCH] monitor: Add simple L2CAP packet framing --- Makefile.tools | 3 ++- monitor/l2cap.c | 56 ++++++++++++++++++++++++++++++++++++++++++++++++ monitor/l2cap.h | 32 +++++++++++++++++++++++++++ monitor/main.c | 1 - monitor/packet.c | 55 +++++++++++++++++++++++++++++++++++++---------- 5 files changed, 134 insertions(+), 13 deletions(-) create mode 100644 monitor/l2cap.c create mode 100644 monitor/l2cap.h diff --git a/Makefile.tools b/Makefile.tools index ebda7c584..07eeeacfc 100644 --- a/Makefile.tools +++ b/Makefile.tools @@ -52,7 +52,8 @@ monitor_btmon_SOURCES = monitor/main.c monitor/bt.h \ monitor/hcidump.h monitor/hcidump.c \ monitor/btsnoop.h monitor/btsnoop.c \ monitor/control.h monitor/control.c \ - monitor/packet.h monitor/packet.c + monitor/packet.h monitor/packet.c \ + monitor/l2cap.h monitor/l2cap.c monitor_btmon_LDADD = lib/libbluetooth-private.la emulator_btvirt_SOURCES = emulator/main.c monitor/bt.h \ diff --git a/monitor/l2cap.c b/monitor/l2cap.c new file mode 100644 index 000000000..13ed4f7f3 --- /dev/null +++ b/monitor/l2cap.c @@ -0,0 +1,56 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2011-2012 Intel Corporation + * Copyright (C) 2004-2010 Marcel Holtmann + * + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + +#ifdef HAVE_CONFIG_H +#include +#endif + +#include + +#include "packet.h" +#include "l2cap.h" + +#define print_field(fmt, args...) printf("%-12c" fmt "\n", ' ', ## args) + +void l2cap_packet(const void *data, uint16_t size) +{ + const struct bt_l2cap_hdr *hdr = data; + + if (size < sizeof(*hdr)) { + print_field("malformed packet"); + packet_hexdump(data, size); + return; + } + + print_field("Length: %d", btohs(hdr->len)); + print_field("Channel: %d", btohs(hdr->cid)); + + if (btohs(hdr->len) != size - sizeof(*hdr)) { + print_field("invalid packet size"); + packet_hexdump(data + sizeof(*hdr), size - sizeof(*hdr)); + return; + } + + packet_hexdump(data + sizeof(*hdr), size - sizeof(*hdr)); +} diff --git a/monitor/l2cap.h b/monitor/l2cap.h new file mode 100644 index 000000000..72ac8b738 --- /dev/null +++ b/monitor/l2cap.h @@ -0,0 +1,32 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2011-2012 Intel Corporation + * Copyright (C) 2004-2010 Marcel Holtmann + * + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + +#include + +struct bt_l2cap_hdr { + uint16_t len; + uint16_t cid; +} __attribute__ ((packed)); + +void l2cap_packet(const void *data, uint16_t size); diff --git a/monitor/main.c b/monitor/main.c index 4cb281dc6..7ea7773e9 100644 --- a/monitor/main.c +++ b/monitor/main.c @@ -111,7 +111,6 @@ int main(int argc, char *argv[]) filter_mask |= PACKET_FILTER_SHOW_INDEX; filter_mask |= PACKET_FILTER_SHOW_TIME; - filter_mask |= PACKET_FILTER_SHOW_ACL_DATA; packet_set_filter(filter_mask); diff --git a/monitor/packet.c b/monitor/packet.c index 1489f548c..dde80503e 100644 --- a/monitor/packet.c +++ b/monitor/packet.c @@ -42,6 +42,7 @@ #include "pager.h" #include "bt.h" +#include "l2cap.h" #include "control.h" #include "packet.h" @@ -1066,7 +1067,13 @@ struct monitor_new_index { #define MAX_INDEX 16 -static struct monitor_new_index index_list[MAX_INDEX]; +struct index_data { + bdaddr_t bdaddr; + void *frag_buf; + uint16_t frag_len; +}; + +static struct index_data index_list[MAX_INDEX]; uint32_t packet_get_flags(uint16_t opcode) { @@ -1135,16 +1142,20 @@ void packet_monitor(struct timeval *tv, uint16_t index, uint16_t opcode, case MONITOR_NEW_INDEX: ni = data; - if (index < MAX_INDEX) - memcpy(&index_list[index], ni, MONITOR_NEW_INDEX_SIZE); + 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); - else + free(index_list[index].frag_buf); + } else ba2str(BDADDR_ANY, str); packet_del_index(tv, index, str); @@ -3700,15 +3711,32 @@ void packet_hci_acldata(struct timeval *tv, uint16_t index, bool in, return; } + data += HCI_ACL_HDR_SIZE; + size -= HCI_ACL_HDR_SIZE; + + if (size != dlen) { + print_text(COLOR_ERROR, "* Invalid ACL Data packet size\n"); + return; + } + print_text(COLOR_HCI_ACLDATA, "%c ACL Data: handle %d", in ? '>' : '<', acl_handle(handle)); print_text(COLOR_OFF, " flags 0x%2.2x dlen %d\n", flags, dlen); - data += HCI_ACL_HDR_SIZE; - size -= HCI_ACL_HDR_SIZE; - if (filter_mask & PACKET_FILTER_SHOW_ACL_DATA) packet_hexdump(data, size); + + if (index > MAX_INDEX - 1) + return; + + switch (flags) { + case 0x00: + case 0x02: + if (index_list[index].frag_len == 0) + l2cap_packet(data, size); + index_list[index].frag_len = 0; + break; + } } void packet_hci_scodata(struct timeval *tv, uint16_t index, bool in, @@ -3726,13 +3754,18 @@ void packet_hci_scodata(struct timeval *tv, uint16_t index, bool in, return; } + data += HCI_SCO_HDR_SIZE; + size -= HCI_SCO_HDR_SIZE; + + if (size != hdr->dlen) { + print_text(COLOR_ERROR, "* Invalid SCO Data packet size\n"); + return; + } + print_text(COLOR_HCI_SCODATA, "%c SCO Data: handle %d", in ? '>' : '<', acl_handle(handle)); print_text(COLOR_OFF, " flags 0x%2.2x dlen %d\n", flags, hdr->dlen); - data += HCI_SCO_HDR_SIZE; - size -= HCI_SCO_HDR_SIZE; - if (filter_mask & PACKET_FILTER_SHOW_SCO_DATA) packet_hexdump(data, size); } -- 2.47.3