monitor: Add basic packet processing

1 files changed, 290 insertions, 6 deletions

diff --git a/monitor/main.c b/monitor/main.c
index 883934cda..102f1b8a2 100644
--- a/monitor/main.c
+++ b/monitor/main.c
@@ -27,9 +27,12 @@
 
 #include <stdio.h>
 #include <errno.h>
+#include <ctype.h>
 #include <unistd.h>
 #include <stdlib.h>
 #include <string.h>
+#include <stdbool.h>
+#include <time.h>
 #include <sys/time.h>
 #include <sys/epoll.h>
 
@@ -48,6 +51,234 @@ struct monitor_hdr {
 
 #define MONITOR_HDR_SIZE 6
 
+#define MONITOR_NEW_INDEX	0
+#define MONITOR_DEL_INDEX	1
+#define MONITOR_COMMAND_PKT	2
+#define MONITOR_EVENT_PKT	3
+#define MONITOR_ACL_TX_PKT	4
+#define MONITOR_ACL_RX_PKT	5
+#define MONITOR_SCO_TX_PKT	6
+#define MONITOR_SCO_RX_PKT	7
+
+struct monitor_new_index {
+	uint8_t  type;
+	uint8_t  bus;
+	bdaddr_t bdaddr;
+	char     name[8];
+} __attribute__((packed));
+
+#define MONITOR_NEW_INDEX_SIZE 16
+
+#define MONITOR_DEL_INDEX_SIZE 0
+
+static unsigned long filter_mask = 0;
+
+#define FILTER_SHOW_INDEX	(1 << 0)
+#define FILTER_SHOW_DATE	(1 << 1)
+#define FILTER_SHOW_TIME	(1 << 2)
+#define FILTER_SHOW_ACL_DATA	(1 << 3)
+#define FILTER_SHOW_SCO_DATA	(1 << 4)
+
+#define MAX_INDEX 16
+
+static struct monitor_new_index index_list[MAX_INDEX];
+
+static const char *devtype2str(uint8_t type)
+{
+	switch (type) {
+	case 0:
+		return "BR/EDR";
+	case 1:
+		return "AMP";
+	}
+
+	return "UNKNOWN";
+}
+
+static const char *devbus2str(uint8_t bus)
+{
+	switch (bus) {
+	case 0:
+		return "VIRTUAL";
+	case 1:
+		return "USB";
+	case 2:
+		return "PCCARD";
+	case 3:
+		return "UART";
+	}
+
+	return "UNKNOWN";
+}
+
+static const char *opcode2str(uint16_t opcode)
+{
+	return "Unknown";
+}
+
+static const char *event2str(uint8_t event)
+{
+	return "Unknown";
+}
+
+static void hexdump(const unsigned char *buf, uint16_t len)
+{
+	static const char hexdigits[] = "0123456789abcdef";
+	char str[68];
+	uint16_t i;
+
+	if (!len)
+		return;
+
+	for (i = 0; i < len; i++) {
+		str[((i % 16) * 3) + 0] = hexdigits[buf[i] >> 4];
+		str[((i % 16) * 3) + 1] = hexdigits[buf[i] & 0xf];
+		str[((i % 16) * 3) + 2] = ' ';
+		str[(i % 16) + 49] = isprint(buf[i]) ? buf[i] : '.';
+
+		if ((i + 1) % 16 == 0) {
+			str[47] = ' ';
+			str[48] = ' ';
+			str[65] = '\0';
+			printf("%-12c%s\n", ' ', str);
+			str[0] = ' ';
+		}
+	}
+
+	if (i % 16 > 0) {
+		uint16_t j;
+		for (j = (i % 16); j < 16; j++) {
+			str[(j * 3) + 0] = ' ';
+			str[(j * 3) + 1] = ' ';
+			str[(j * 3) + 2] = ' ';
+			str[j + 49] = ' ';
+		}
+		str[47] = ' ';
+		str[48] = ' ';
+		str[65] = '\0';
+		printf("%-12c%s\n", ' ', str);
+	}
+}
+
+static void process_new_index(uint16_t index, uint16_t len, void *buf)
+{
+	struct monitor_new_index *ni = buf;
+	char str[18];
+
+	if (len != MONITOR_NEW_INDEX_SIZE) {
+		printf("* Malformed New Index packet\n");
+		return;
+	}
+
+	ba2str(&ni->bdaddr, str);
+
+	printf("= New Index: %s (%s,%s,%s)\n", str,
+					devtype2str(ni->type),
+					devbus2str(ni->bus), ni->name);
+
+	if (index < MAX_INDEX)
+		memcpy(&index_list[index], ni, MONITOR_NEW_INDEX_SIZE);
+}
+
+static void process_del_index(uint16_t index, uint16_t len)
+{
+	char str[18];
+
+	if (len != MONITOR_DEL_INDEX_SIZE) {
+		printf("* Malformed Delete Index packet\n");
+		return;
+	}
+
+	if (index < MAX_INDEX)
+		ba2str(&index_list[index].bdaddr, str);
+	else
+		ba2str(BDADDR_ANY, str);
+
+	printf("= Delete Index: %s\n", str);
+}
+
+static void process_command_pkt(uint16_t len, void *buf)
+{
+	hci_command_hdr *hdr = buf;
+	uint16_t opcode = btohs(hdr->opcode);
+	uint16_t ogf = cmd_opcode_ogf(opcode);
+	uint16_t ocf = cmd_opcode_ocf(opcode);
+
+	if (len < HCI_COMMAND_HDR_SIZE) {
+		printf("* Malformed HCI Command packet\n");
+		return;
+	}
+
+	printf("< HCI Command: %s (0x%2.2x|0x%4.4x) plen %d\n",
+				opcode2str(opcode), ogf, ocf, hdr->plen);
+
+	buf += HCI_COMMAND_HDR_SIZE;
+	len -= HCI_COMMAND_HDR_SIZE;
+
+	hexdump(buf, len);
+}
+
+static void process_event_pkt(uint16_t len, void *buf)
+{
+	hci_event_hdr *hdr = buf;
+
+	if (len < HCI_EVENT_HDR_SIZE) {
+		printf("* Malformed HCI Event packet\n");
+		return;
+	}
+
+	printf("> HCI Event: %s (0x%2.2x) plen %d\n",
+				event2str(hdr->evt), hdr->evt, hdr->plen);
+
+	buf += HCI_EVENT_HDR_SIZE;
+	len -= HCI_EVENT_HDR_SIZE;
+
+	hexdump(buf, len);
+}
+
+static void process_acldata_pkt(bool in, uint16_t len, void *buf)
+{
+	hci_acl_hdr *hdr = buf;
+	uint16_t handle = btohs(hdr->handle);
+	uint16_t dlen = btohs(hdr->dlen);
+	uint8_t flags = acl_flags(handle);
+
+	if (len < HCI_ACL_HDR_SIZE) {
+		printf("* Malformed ACL Data %s packet\n", in ? "RX" : "TX");
+		return;
+	}
+
+	printf("%c ACL Data: handle %d flags 0x%2.2x dlen %d\n",
+			in ? '>' : '<', acl_handle(handle), flags, dlen);
+
+	buf += HCI_ACL_HDR_SIZE;
+	len -= HCI_ACL_HDR_SIZE;
+
+	if (filter_mask & FILTER_SHOW_ACL_DATA)
+		hexdump(buf, len);
+}
+
+static void process_scodata_pkt(bool in, uint16_t len, void *buf)
+{
+	hci_sco_hdr *hdr = buf;
+	uint16_t handle = btohs(hdr->handle);
+	uint8_t flags = acl_flags(handle);
+
+	if (len < HCI_SCO_HDR_SIZE) {
+		printf("* Malformed SCO Data %s packet\n", in ? "RX" : "TX");
+		return;
+	}
+
+	printf("%c SCO Data: handle %d flags 0x%2.2x dlen %d\n",
+			in ? '>' : '<',	acl_handle(handle), flags, hdr->dlen);
+
+	buf += HCI_SCO_HDR_SIZE;
+	len -= HCI_SCO_HDR_SIZE;
+
+	if (filter_mask & FILTER_SHOW_SCO_DATA)
+		hexdump(buf, len);
+}
+
 static void process_monitor(int fd)
 {
 	unsigned char buf[4096];
@@ -56,6 +287,8 @@ static void process_monitor(int fd)
 	struct msghdr msg;
 	struct iovec iov[2];
 	struct cmsghdr *cmsg;
+	struct timeval *tv = NULL;
+	uint16_t opcode, index, pktlen;
 	ssize_t len;
 
 	iov[0].iov_base = &hdr;
@@ -81,15 +314,62 @@ static void process_monitor(int fd)
 		if (cmsg->cmsg_level != SOL_SOCKET)
 			continue;
 
-		if (cmsg->cmsg_type == SCM_TIMESTAMP) {
-			struct timeval *tv = (void *) CMSG_DATA(cmsg);
+		if (cmsg->cmsg_type == SCM_TIMESTAMP)
+			tv = (void *) CMSG_DATA(cmsg);
+	}
 
-			printf("[%jd.%03jd] ", tv->tv_sec, tv->tv_usec);
-		}
+	opcode = btohs(hdr.opcode);
+	index  = btohs(hdr.index);
+	pktlen = btohs(hdr.len);
+
+	if (filter_mask & FILTER_SHOW_INDEX)
+		printf("[hci%d] ", index);
+
+	if (tv) {
+		time_t t = tv->tv_sec;
+		struct tm tm;
+
+		localtime_r(&t, &tm);
+
+		if (filter_mask & FILTER_SHOW_DATE)
+			printf("%04d-%02d-%02d ",
+				tm.tm_year + 1900, tm.tm_mon + 1, tm.tm_mday);
+
+		if (filter_mask & FILTER_SHOW_TIME)
+			printf("%02d:%02d:%02d.%06lu ",
+				tm.tm_hour, tm.tm_min, tm.tm_sec, tv->tv_usec);
 	}
 
-	printf("{opcode=%d,index=%d,len=%d}\n",
-				hdr.opcode, hdr.index, hdr.len);
+	switch (opcode) {
+	case MONITOR_NEW_INDEX:
+		process_new_index(index, pktlen, buf);
+		break;
+	case MONITOR_DEL_INDEX:
+		process_del_index(index, pktlen);
+		break;
+	case MONITOR_COMMAND_PKT:
+		process_command_pkt(pktlen, buf);
+		break;
+	case MONITOR_EVENT_PKT:
+		process_event_pkt(pktlen, buf);
+		break;
+	case MONITOR_ACL_TX_PKT:
+		process_acldata_pkt(false, pktlen, buf);
+		break;
+	case MONITOR_ACL_RX_PKT:
+		process_acldata_pkt(true, pktlen, buf);
+		break;
+	case MONITOR_SCO_TX_PKT:
+		process_scodata_pkt(false, pktlen, buf);
+		break;
+	case MONITOR_SCO_RX_PKT:
+		process_scodata_pkt(true, pktlen, buf);
+		break;
+	default:
+		printf("* Unknown packet (code %d len %d)\n", opcode, pktlen);
+		hexdump(buf, pktlen);
+		break;
+	}
 }
 
 static int open_monitor(void)
@@ -131,6 +411,10 @@ int main(int argc, char *argv[])
 	struct epoll_event mon_event;
 	int mon_fd, epoll_fd;
 
+	filter_mask |= FILTER_SHOW_INDEX;
+	filter_mask |= FILTER_SHOW_TIME;
+	filter_mask |= FILTER_SHOW_ACL_DATA;
+
 	mon_fd = open_monitor();
 	if (mon_fd < 0)
 		return exitcode;