[PATCH bluez RFC 3/3] monitor: Add generic support for vendor HCI frames

[Zijun Hu]

To show vendor HCI frames.
---
 doc/btsnoop-protocol.rst |  8 ++++++++
 monitor/analyze.c        | 23 +++++++++++++++++++++++
 monitor/packet.c         | 26 ++++++++++++++++++++++++++
 monitor/packet.h         |  2 ++
 src/shared/btsnoop.h     |  2 ++
 5 files changed, 61 insertions(+)

diff --git a/doc/btsnoop-protocol.rst b/doc/btsnoop-protocol.rst
index a875db63a9dd..81cd8fdc71ae 100644
--- a/doc/btsnoop-protocol.rst
+++ b/doc/btsnoop-protocol.rst
@@ -116,6 +116,14 @@ values match the definitions in ``src/shared/btsnoop.h``.
      - 19
      - 0x0013
      - Incoming ISO packet
+   * - BTSNOOP_OPCODE_VENDOR_TX_PKT
+     - 22
+     - 0x0016
+     - Outgoing vendor HCI packet
+   * - BTSNOOP_OPCODE_VENDOR_RX_PKT
+     - 23
+     - 0x0017
+     - Incoming vendor HCI packet
 
 New Index
 ---------
diff --git a/monitor/analyze.c b/monitor/analyze.c
index de9c23603a21..3fe96aaeddb0 100644
--- a/monitor/analyze.c
+++ b/monitor/analyze.c
@@ -45,6 +45,7 @@ struct hci_dev {
 	unsigned long num_sco;
 	unsigned long num_iso;
 	unsigned long vendor_diag;
+	unsigned long vendor_hci;
 	unsigned long system_note;
 	unsigned long user_log;
 	unsigned long ctrl_msg;
@@ -466,6 +467,7 @@ static void dev_destroy(void *data)
 	printf("  %lu SCO packets\n", dev->num_sco);
 	printf("  %lu ISO packets\n", dev->num_iso);
 	printf("  %lu vendor diagnostics\n", dev->vendor_diag);
+	printf("  %lu vendor HCI frames\n", dev->vendor_hci);
 	printf("  %lu system notes\n", dev->system_note);
 	printf("  %lu user logs\n", dev->user_log);
 	printf("  %lu control messages \n", dev->ctrl_msg);
@@ -1296,6 +1298,19 @@ static void vendor_diag(struct timeval *tv, uint16_t index,
 	dev->vendor_diag++;
 }
 
+static void vendor_hci_pkt(struct timeval *tv, uint16_t index, bool in,
+					const void *data, uint16_t size)
+{
+	struct hci_dev *dev;
+
+	dev = dev_lookup(index);
+	if (!dev)
+		return;
+
+	dev->num_hci++;
+	dev->vendor_hci++;
+}
+
 static void system_note(struct timeval *tv, uint16_t index,
 					const void *data, uint16_t size)
 {
@@ -1468,6 +1483,14 @@ void analyze_trace(const char *path)
 			num_frames++;
 			iso_pkt(&tv, index, false, buf, pktlen);
 			break;
+		case BTSNOOP_OPCODE_VENDOR_TX_PKT:
+			num_frames++;
+			vendor_hci_pkt(&tv, index, false, buf, pktlen);
+			break;
+		case BTSNOOP_OPCODE_VENDOR_RX_PKT:
+			num_frames++;
+			vendor_hci_pkt(&tv, index, true, buf, pktlen);
+			break;
 		default:
 			unknown_opcode(&tv, index, buf, pktlen);
 			break;
diff --git a/monitor/packet.c b/monitor/packet.c
index 82afef6011d2..1fd8453e2666 100644
--- a/monitor/packet.c
+++ b/monitor/packet.c
@@ -72,6 +72,7 @@
 #define COLOR_HCI_ACLDATA		COLOR_CYAN
 #define COLOR_HCI_SCODATA		COLOR_YELLOW
 #define COLOR_HCI_ISODATA		COLOR_YELLOW
+#define COLOR_VENDOR_HCI		COLOR_GREEN
 
 #define COLOR_UNKNOWN_ERROR		COLOR_WHITE_BG
 #define COLOR_UNKNOWN_FEATURE_BIT	COLOR_WHITE_BG
@@ -4528,6 +4529,12 @@ void packet_monitor(struct timeval *tv, struct ucred *cred,
 	case BTSNOOP_OPCODE_ISO_RX_PKT:
 		packet_hci_isodata(tv, cred, index, true, data, size);
 		break;
+	case BTSNOOP_OPCODE_VENDOR_TX_PKT:
+		packet_vendor_hci(tv, cred, index, false, data, size);
+		break;
+	case BTSNOOP_OPCODE_VENDOR_RX_PKT:
+		packet_vendor_hci(tv, cred, index, true, data, size);
+		break;
 	case BTSNOOP_OPCODE_OPEN_INDEX:
 		if (index < MAX_INDEX)
 			addr2str(index_list[index].bdaddr, str);
@@ -14655,6 +14662,25 @@ malformed:
 	packet_hexdump(data, size);
 }
 
+void packet_vendor_hci(struct timeval *tv, struct ucred *cred, uint16_t index,
+				bool in, const void *data, uint16_t size)
+{
+	char extra_str[16];
+
+	if (index >= MAX_INDEX) {
+		print_field("Invalid index (%d).", index);
+		return;
+	}
+
+	index_list[index].frame++;
+
+	sprintf(extra_str, "(len %d)", size);
+	print_packet(tv, cred, in ? '>' : '<', index, NULL, COLOR_VENDOR_HCI,
+				"Vendor HCI Frame", NULL, extra_str);
+
+	packet_hexdump(data, size);
+}
+
 void packet_ctrl_open(struct timeval *tv, struct ucred *cred, uint16_t index,
 					const void *data, uint16_t size)
 {
diff --git a/monitor/packet.h b/monitor/packet.h
index 73a86f64b242..57debcb94bce 100644
--- a/monitor/packet.h
+++ b/monitor/packet.h
@@ -135,6 +135,8 @@ void packet_hci_scodata(struct timeval *tv, struct ucred *cred, uint16_t index,
 				bool in, const void *data, uint16_t size);
 void packet_hci_isodata(struct timeval *tv, struct ucred *cred, uint16_t index,
 				bool in, const void *data, uint16_t size);
+void packet_vendor_hci(struct timeval *tv, struct ucred *cred, uint16_t index,
+				bool in, const void *data, uint16_t size);
 
 void packet_ctrl_open(struct timeval *tv, struct ucred *cred, uint16_t index,
 					const void *data, uint16_t size);
diff --git a/src/shared/btsnoop.h b/src/shared/btsnoop.h
index c24755d56729..efa87a8f2971 100644
--- a/src/shared/btsnoop.h
+++ b/src/shared/btsnoop.h
@@ -42,6 +42,8 @@
 #define BTSNOOP_OPCODE_CTRL_EVENT	17
 #define BTSNOOP_OPCODE_ISO_TX_PKT	18
 #define BTSNOOP_OPCODE_ISO_RX_PKT	19
+#define BTSNOOP_OPCODE_VENDOR_TX_PKT	22
+#define BTSNOOP_OPCODE_VENDOR_RX_PKT	23
 
 #define BTSNOOP_MAX_PACKET_SIZE		(1486 + 4)
 

-- 
2.34.1