Diff between 49a1a03f369a90373368b57ca38773f2087d54dd and 1cbe46134fc2a5ef1deefc33b43716f6710dd27f

Changed Files

File Additions Deletions Status
Makefile.tools +2 -1 modified
monitor/control.c +92 -9 modified
monitor/tty.h +41 -0 added

Full Patch

diff --git a/Makefile.tools b/Makefile.tools
index e79b53b..0d5f143 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 048add2..d4ec191 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 0000000..f0ba0c5
--- /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 <stdint.h>
+
+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