From d0296eea23b3be51538d9d926da4104af737dfb5 Mon Sep 17 00:00:00 2001
From: Marcel Holtmann <marcel@holtmann.org>
Date: Wed, 7 Oct 2015 18:15:34 +0200
Subject: [PATCH] monitor: Add support for vendor diagnostic packets

---
 monitor/packet.c     | 23 +++++++++++++++++++++++
 monitor/packet.h     |  3 +++
 src/shared/btsnoop.h |  1 +
 3 files changed, 27 insertions(+)

diff --git a/monitor/packet.c b/monitor/packet.c
index d40b69db1..687fcbd93 100644
--- a/monitor/packet.c
+++ b/monitor/packet.c
@@ -62,6 +62,7 @@
 #define COLOR_OPEN_INDEX		COLOR_GREEN
 #define COLOR_CLOSE_INDEX		COLOR_RED
 #define COLOR_INDEX_INFO		COLOR_GREEN
+#define COLOR_VENDOR_DIAG		COLOR_YELLOW
 
 #define COLOR_HCI_COMMAND		COLOR_BLUE
 #define COLOR_HCI_COMMAND_UNKNOWN	COLOR_WHITE_BG
@@ -3697,6 +3698,7 @@ void packet_monitor(struct timeval *tv, uint16_t index, uint16_t opcode,
 		if (index < MAX_INDEX) {
 			index_list[index].type = ni->type;
 			memcpy(index_list[index].bdaddr, ni->bdaddr, 6);
+			index_list[index].manufacturer = 0xffff;
 		}
 
 		addr2str(ni->bdaddr, str);
@@ -3756,6 +3758,14 @@ void packet_monitor(struct timeval *tv, uint16_t index, uint16_t opcode,
 		addr2str(ii->bdaddr, str);
 		packet_index_info(tv, index, str, manufacturer);
 		break;
+	case BTSNOOP_OPCODE_VENDOR_DIAG:
+		if (index < MAX_INDEX)
+			manufacturer = index_list[index].manufacturer;
+		else
+			manufacturer = 0xffff;
+
+		packet_vendor_diag(tv, index, manufacturer, data, size);
+		break;
 	default:
 		sprintf(extra_str, "(code %d len %d)", opcode, size);
 		print_packet(tv, index, '*', COLOR_ERROR,
@@ -8504,6 +8514,19 @@ void packet_index_info(struct timeval *tv, uint16_t index, const char *label,
 							label, details);
 }
 
+void packet_vendor_diag(struct timeval *tv, uint16_t index,
+					uint16_t manufacturer,
+					const void *data, uint16_t size)
+{
+	char extra_str[16];
+
+	sprintf(extra_str, "(len %d)", size);
+
+	print_packet(tv, index, '=', COLOR_VENDOR_DIAG, "Vendor Diagnostic",
+							NULL, extra_str);
+	packet_hexdump(data, size);
+}
+
 void packet_hci_command(struct timeval *tv, uint16_t index,
 					const void *data, uint16_t size)
 {
diff --git a/monitor/packet.h b/monitor/packet.h
index 43fabc07d..fb64cc61d 100644
--- a/monitor/packet.h
+++ b/monitor/packet.h
@@ -67,6 +67,9 @@ void packet_open_index(struct timeval *tv, uint16_t index, const char *label);
 void packet_close_index(struct timeval *tv, uint16_t index, const char *label);
 void packet_index_info(struct timeval *tv, uint16_t index, const char *label,
 							uint16_t manufacturer);
+void packet_vendor_diag(struct timeval *tv, uint16_t index,
+					uint16_t manufacturer,
+					const void *data, uint16_t size);
 
 void packet_hci_command(struct timeval *tv, uint16_t index,
 					const void *data, uint16_t size);
diff --git a/src/shared/btsnoop.h b/src/shared/btsnoop.h
index 91b592247..a0157ff64 100644
--- a/src/shared/btsnoop.h
+++ b/src/shared/btsnoop.h
@@ -46,6 +46,7 @@
 #define BTSNOOP_OPCODE_OPEN_INDEX	8
 #define BTSNOOP_OPCODE_CLOSE_INDEX	9
 #define BTSNOOP_OPCODE_INDEX_INFO	10
+#define BTSNOOP_OPCODE_VENDOR_DIAG	11
 
 #define BTSNOOP_MAX_PACKET_SIZE		(1486 + 4)
 
-- 
2.47.3