From 1cbe46134fc2a5ef1deefc33b43716f6710dd27f Mon Sep 17 00:00:00 2001 From: Johan Hedberg Date: Tue, 3 May 2016 09:11:04 +0300 Subject: [PATCH] monitor: Introduce support for extended TTY header Add support for packet drops and 32-bit timestamps. --- Makefile.tools | 3 +- monitor/control.c | 101 +++++++++++++++++++++++++++++++++++++++++----- monitor/tty.h | 41 +++++++++++++++++++ 3 files changed, 135 insertions(+), 10 deletions(-) create mode 100644 monitor/tty.h diff --git a/Makefile.tools b/Makefile.tools index e79b53ba3..0d5f1431e 100644 --- a/Makefile.tools +++ b/Makefile.tools @@ -36,7 +36,8 @@ monitor_btmon_SOURCES = monitor/main.c monitor/bt.h \ monitor/keys.h monitor/keys.c \ monitor/analyze.h monitor/analyze.c \ monitor/intel.h monitor/intel.c \ - monitor/broadcom.h monitor/broadcom.c + monitor/broadcom.h monitor/broadcom.c \ + monitor/tty.h monitor_btmon_LDADD = lib/libbluetooth-internal.la \ src/libshared-mainloop.la @UDEV_LIBS@ endif diff --git a/monitor/control.c b/monitor/control.c index 048add25e..d4ec191a2 100644 --- a/monitor/control.c +++ b/monitor/control.c @@ -52,6 +52,7 @@ #include "packet.h" #include "hcidump.h" #include "ellisys.h" +#include "tty.h" #include "control.h" static struct btsnoop *btsnoop_file = NULL; @@ -1163,12 +1164,90 @@ void control_server(const char *path) server_fd = fd; } -struct tty_hdr { - uint16_t data_len; - uint16_t opcode; - uint8_t flags; - uint8_t hdr_len; -} __attribute__ ((packed)); +static bool parse_drops(uint8_t **data, uint8_t *len, uint8_t *drops, + uint32_t *total) +{ + if (*len < 1) + return false; + + *drops = **data; + *total += *drops; + (*data)++; + (*len)--; + + return true; +} + +static bool tty_parse_header(uint8_t *hdr, uint8_t len, struct timeval **tv, + struct timeval *ctv) +{ + uint8_t cmd = 0; + uint8_t evt = 0; + uint8_t acl_tx = 0; + uint8_t acl_rx = 0; + uint8_t sco_tx = 0; + uint8_t sco_rx = 0; + uint8_t other = 0; + uint32_t total = 0; + uint32_t ts32; + + while (len) { + uint8_t type = hdr[0]; + + hdr++; len--; + + switch (type) { + case TTY_EXTHDR_COMMAND_DROPS: + if (!parse_drops(&hdr, &len, &cmd, &total)) + return false; + break; + case TTY_EXTHDR_EVENT_DROPS: + if (!parse_drops(&hdr, &len, &evt, &total)) + return false; + break; + case TTY_EXTHDR_ACL_TX_DROPS: + if (!parse_drops(&hdr, &len, &acl_tx, &total)) + return false; + break; + case TTY_EXTHDR_ACL_RX_DROPS: + if (!parse_drops(&hdr, &len, &acl_rx, &total)) + return false; + break; + case TTY_EXTHDR_SCO_TX_DROPS: + if (!parse_drops(&hdr, &len, &sco_tx, &total)) + return false; + break; + case TTY_EXTHDR_SCO_RX_DROPS: + if (!parse_drops(&hdr, &len, &sco_rx, &total)) + return false; + break; + case TTY_EXTHDR_OTHER_DROPS: + if (!parse_drops(&hdr, &len, &other, &total)) + return false; + break; + case TTY_EXTHDR_TS32: + if (len < sizeof(ts32)) + return false; + ts32 = get_le32(hdr); + hdr += sizeof(ts32); len -= sizeof(ts32); + /* ts32 is in units of 1/10th of a millisecond */ + ctv->tv_sec = ts32 / 10000; + ctv->tv_usec = (ts32 % 10000) * 100; + *tv = ctv; + break; + default: + printf("Unknown extended header type %u\n", type); + return false; + } + } + + if (total) + printf("* Drops: cmd %u evt %u acl_tx %u acl_rx %u sco_tx %u " + "sco_rx %u other %u\n", cmd, evt, acl_tx, acl_rx, + sco_tx, sco_rx, other); + + return true; +} static void tty_callback(int fd, uint32_t events, void *user_data) { @@ -1190,6 +1269,8 @@ static void tty_callback(int fd, uint32_t events, void *user_data) while (data->offset >= sizeof(struct tty_hdr)) { struct tty_hdr *hdr = (struct tty_hdr *) data->buf; uint16_t pktlen, opcode, data_len; + struct timeval *tv = NULL; + struct timeval ctv; data_len = le16_to_cpu(hdr->data_len); @@ -1203,12 +1284,14 @@ static void tty_callback(int fd, uint32_t events, void *user_data) return; } + if (!tty_parse_header(hdr->ext_hdr, hdr->hdr_len, &tv, &ctv)) + fprintf(stderr, "Unable to parse extended header\n"); + opcode = le16_to_cpu(hdr->opcode); pktlen = data_len - 4 - hdr->hdr_len; - packet_monitor(NULL, NULL, 0, opcode, - data->buf + sizeof(*hdr) + hdr->hdr_len, - pktlen); + packet_monitor(tv, NULL, 0, opcode, + hdr->ext_hdr + hdr->hdr_len, pktlen); data->offset -= 2 + data_len; diff --git a/monitor/tty.h b/monitor/tty.h new file mode 100644 index 000000000..f0ba0c5c0 --- /dev/null +++ b/monitor/tty.h @@ -0,0 +1,41 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2016 Intel Corporation + * + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library 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 + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + +#include + +struct tty_hdr { + uint16_t data_len; + uint16_t opcode; + uint8_t flags; + uint8_t hdr_len; + uint8_t ext_hdr[0]; +} __attribute__ ((packed)); + +#define TTY_EXTHDR_COMMAND_DROPS 1 +#define TTY_EXTHDR_EVENT_DROPS 2 +#define TTY_EXTHDR_ACL_TX_DROPS 3 +#define TTY_EXTHDR_ACL_RX_DROPS 4 +#define TTY_EXTHDR_SCO_TX_DROPS 5 +#define TTY_EXTHDR_SCO_RX_DROPS 6 +#define TTY_EXTHDR_OTHER_DROPS 7 +#define TTY_EXTHDR_TS32 8 -- 2.47.3