From 12abeb4ec1306b84f76a462001a9323aee275d8b Mon Sep 17 00:00:00 2001
From: Marcel Holtmann <marcel@holtmann.org>
Date: Sat, 7 Nov 2015 10:27:01 +0100
Subject: [PATCH] monitor: Add support for system note packet type

---
 monitor/packet.c     | 11 ++++++++++
 src/shared/btsnoop.c | 50 ++++++++++++++++++++++++++------------------
 src/shared/btsnoop.h |  1 +
 3 files changed, 42 insertions(+), 20 deletions(-)

diff --git a/monitor/packet.c b/monitor/packet.c
index 678993e9c..926e651a1 100644
--- a/monitor/packet.c
+++ b/monitor/packet.c
@@ -66,6 +66,7 @@
 #define COLOR_CLOSE_INDEX		COLOR_RED
 #define COLOR_INDEX_INFO		COLOR_GREEN
 #define COLOR_VENDOR_DIAG		COLOR_YELLOW
+#define COLOR_SYSTEM_NOTE		COLOR_BLACK
 
 #define COLOR_HCI_COMMAND		COLOR_BLUE
 #define COLOR_HCI_COMMAND_UNKNOWN	COLOR_WHITE_BG
@@ -3683,6 +3684,13 @@ struct index_data {
 
 static struct index_data index_list[MAX_INDEX];
 
+static void packet_system_note(struct timeval *tv, struct ucred *cred,
+				uint16_t index, const void *data, uint16_t size)
+{
+	print_packet(tv, cred, index, '=', COLOR_SYSTEM_NOTE,
+						"Note", data, NULL);
+}
+
 void packet_monitor(struct timeval *tv, struct ucred *cred,
 					uint16_t index, uint16_t opcode,
 					const void *data, uint16_t size)
@@ -3775,6 +3783,9 @@ void packet_monitor(struct timeval *tv, struct ucred *cred,
 
 		packet_vendor_diag(tv, index, manufacturer, data, size);
 		break;
+	case BTSNOOP_OPCODE_SYSTEM_NOTE:
+		packet_system_note(tv, cred, index, data, size);
+		break;
 	default:
 		sprintf(extra_str, "(code %d len %d)", opcode, size);
 		print_packet(tv, cred, index, '*', COLOR_ERROR,
diff --git a/src/shared/btsnoop.c b/src/shared/btsnoop.c
index a2a169e0c..3613c331b 100644
--- a/src/shared/btsnoop.c
+++ b/src/shared/btsnoop.c
@@ -306,24 +306,6 @@ bool btsnoop_write_phy(struct btsnoop *btsnoop, struct timeval *tv,
 	return btsnoop_write(btsnoop, tv, flags, data, size);
 }
 
-static uint16_t get_opcode_from_pklg(uint8_t type)
-{
-	switch (type) {
-	case 0x00:
-		return BTSNOOP_OPCODE_COMMAND_PKT;
-	case 0x01:
-		return BTSNOOP_OPCODE_EVENT_PKT;
-	case 0x02:
-		return BTSNOOP_OPCODE_ACL_TX_PKT;
-	case 0x03:
-		return BTSNOOP_OPCODE_ACL_RX_PKT;
-	case 0x0b:
-		return BTSNOOP_OPCODE_VENDOR_DIAG;
-	}
-
-	return 0xffff;
-}
-
 static bool pklg_read_hci(struct btsnoop *btsnoop, struct timeval *tv,
 					uint16_t *index, uint16_t *opcode,
 					void *data, uint16_t *size)
@@ -356,8 +338,36 @@ static bool pklg_read_hci(struct btsnoop *btsnoop, struct timeval *tv,
 		tv->tv_usec = ts & 0xffffffff;
 	}
 
-	*index = 0;
-	*opcode = get_opcode_from_pklg(pkt.type);
+	switch (pkt.type) {
+	case 0x00:
+		*index = 0x0000;
+		*opcode = BTSNOOP_OPCODE_COMMAND_PKT;
+		break;
+	case 0x01:
+		*index = 0x0000;
+		*opcode = BTSNOOP_OPCODE_EVENT_PKT;
+		break;
+	case 0x02:
+		*index = 0x0000;
+		*opcode = BTSNOOP_OPCODE_ACL_TX_PKT;
+		break;
+	case 0x03:
+		*index = 0x0000;
+		*opcode = BTSNOOP_OPCODE_ACL_RX_PKT;
+		break;
+	case 0x0b:
+		*index = 0x0000;
+		*opcode = BTSNOOP_OPCODE_VENDOR_DIAG;
+		break;
+	case 0xfc:
+		*index = 0xffff;
+		*opcode = BTSNOOP_OPCODE_SYSTEM_NOTE;
+		break;
+	default:
+		*index = 0xffff;
+		*opcode = 0xffff;
+		break;
+	}
 
 	len = read(btsnoop->fd, data, toread);
 	if (len < 0) {
diff --git a/src/shared/btsnoop.h b/src/shared/btsnoop.h
index a0157ff64..fadc878b3 100644
--- a/src/shared/btsnoop.h
+++ b/src/shared/btsnoop.h
@@ -47,6 +47,7 @@
 #define BTSNOOP_OPCODE_CLOSE_INDEX	9
 #define BTSNOOP_OPCODE_INDEX_INFO	10
 #define BTSNOOP_OPCODE_VENDOR_DIAG	11
+#define BTSNOOP_OPCODE_SYSTEM_NOTE	12
 
 #define BTSNOOP_MAX_PACKET_SIZE		(1486 + 4)
 
-- 
2.47.3