From 8651e6486c478e143d00de02b602ec468f509789 Mon Sep 17 00:00:00 2001
From: Marcel Holtmann <marcel@holtmann.org>
Date: Sun, 18 Oct 2015 00:22:06 +0200
Subject: [PATCH] monitor: Add support for handling Broadcom vendor commands

---
 monitor/broadcom.c | 27 ++++++++++++++++++++++++
 monitor/broadcom.h |  3 +++
 monitor/packet.c   | 51 ++++++++++++++++++++++++++++++++++++++++++----
 monitor/vendor.h   | 11 ++++++++++
 4 files changed, 88 insertions(+), 4 deletions(-)

diff --git a/monitor/broadcom.c b/monitor/broadcom.c
index b9c70b4bb..7c76b97a9 100644
--- a/monitor/broadcom.c
+++ b/monitor/broadcom.c
@@ -33,8 +33,35 @@
 #include "packet.h"
 #include "lmp.h"
 #include "ll.h"
+#include "vendor.h"
 #include "broadcom.h"
 
+static const struct vendor_ocf vendor_ocf_table[] = {
+	{ 0x001, "Write BD ADDR"			},
+	{ 0x018, "Update UART Baud Rate"		},
+	{ 0x027, "Set Sleepmode Param"			},
+	{ 0x02e, "Download Minidriver"			},
+	{ 0x03b, "Enable USB HID Emulation"		},
+	{ 0x045, "Write UART Clock Setting"		},
+	{ 0x04c, "Write RAM"				},
+	{ 0x04e, "Launch RAM"				},
+	{ 0x05a, "Read VID PID"				},
+	{ 0x079, "Read Verbose Config Version Info"	},
+	{ }
+};
+
+const struct vendor_ocf *broadcom_vendor_ocf(uint16_t ocf)
+{
+	int i;
+
+	for (i = 0; vendor_ocf_table[i].str; i++) {
+		if (vendor_ocf_table[i].ocf == ocf)
+			return &vendor_ocf_table[i];
+	}
+
+	return NULL;
+}
+
 void broadcom_lm_diag(const void *data, uint8_t size)
 {
 	uint8_t type;
diff --git a/monitor/broadcom.h b/monitor/broadcom.h
index 2aec9128f..f91f25b7d 100644
--- a/monitor/broadcom.h
+++ b/monitor/broadcom.h
@@ -24,4 +24,7 @@
 
 #include <stdint.h>
 
+struct vendor_ocf;
+
+const struct vendor_ocf *broadcom_vendor_ocf(uint16_t ocf);
 void broadcom_lm_diag(const void *data, uint8_t size);
diff --git a/monitor/packet.c b/monitor/packet.c
index 439b5dc59..a353193ef 100644
--- a/monitor/packet.c
+++ b/monitor/packet.c
@@ -7256,6 +7256,23 @@ static const char *get_supported_command(int bit)
 	return NULL;
 }
 
+static const struct vendor_ocf *current_vendor_ocf(uint16_t ocf)
+{
+	uint16_t manufacturer;
+
+	if (index_current < MAX_INDEX)
+		manufacturer = index_list[index_current].manufacturer;
+	else
+		manufacturer = UNKNOWN_MANUFACTURER;
+
+	switch (manufacturer) {
+	case 15:
+		return broadcom_vendor_ocf(ocf);
+	}
+
+	return NULL;
+}
+
 static void inquiry_complete_evt(const void *data, uint8_t size)
 {
 	const struct bt_hci_evt_inquiry_complete *evt = data;
@@ -7404,6 +7421,7 @@ static void cmd_complete_evt(const void *data, uint8_t size)
 	uint16_t opcode = le16_to_cpu(evt->opcode);
 	uint16_t ogf = cmd_opcode_ogf(opcode);
 	uint16_t ocf = cmd_opcode_ocf(opcode);
+	struct opcode_data vendor_data;
 	const struct opcode_data *opcode_data = NULL;
 	const char *opcode_color, *opcode_str;
 	int i;
@@ -7423,8 +7441,20 @@ static void cmd_complete_evt(const void *data, uint8_t size)
 		opcode_str = opcode_data->str;
 	} else {
 		if (ogf == 0x3f) {
-			opcode_color = COLOR_HCI_COMMAND;
-			opcode_str = "Vendor";
+			const struct vendor_ocf *vnd = current_vendor_ocf(ocf);
+
+			if (vnd) {
+				vendor_data.str = vnd->str;
+				vendor_data.rsp_func = NULL;
+
+				opcode_data = &vendor_data;
+
+				opcode_color = COLOR_HCI_COMMAND;
+				opcode_str = opcode_data->str;
+			} else {
+				opcode_color = COLOR_HCI_COMMAND;
+				opcode_str = "Vendor";
+			}
 		} else {
 			opcode_color = COLOR_HCI_COMMAND_UNKNOWN;
 			opcode_str = "Unknown";
@@ -8570,6 +8600,7 @@ void packet_hci_command(struct timeval *tv, uint16_t index,
 	uint16_t opcode = le16_to_cpu(hdr->opcode);
 	uint16_t ogf = cmd_opcode_ogf(opcode);
 	uint16_t ocf = cmd_opcode_ocf(opcode);
+	struct opcode_data vendor_data;
 	const struct opcode_data *opcode_data = NULL;
 	const char *opcode_color, *opcode_str;
 	char extra_str[25];
@@ -8601,8 +8632,20 @@ void packet_hci_command(struct timeval *tv, uint16_t index,
 		opcode_str = opcode_data->str;
 	} else {
 		if (ogf == 0x3f) {
-			opcode_color = COLOR_HCI_COMMAND;
-			opcode_str = "Vendor";
+			const struct vendor_ocf *vnd = current_vendor_ocf(ocf);
+
+			if (vnd) {
+				vendor_data.str = vnd->str;
+				vendor_data.cmd_func = NULL;
+
+				opcode_data = &vendor_data;
+
+				opcode_color = COLOR_HCI_COMMAND;
+				opcode_str = opcode_data->str;
+			} else {
+				opcode_color = COLOR_HCI_COMMAND;
+				opcode_str = "Vendor";
+			}
 		} else {
 			opcode_color = COLOR_HCI_COMMAND_UNKNOWN;
 			opcode_str = "Unknown";
diff --git a/monitor/vendor.h b/monitor/vendor.h
index 9ac9a4bf2..be7baab81 100644
--- a/monitor/vendor.h
+++ b/monitor/vendor.h
@@ -24,4 +24,15 @@
 
 #include <stdint.h>
 
+struct vendor_ocf {
+	uint16_t ocf;
+	const char *str;
+	void (*cmd_func) (const void *data, uint8_t size);
+	uint8_t cmd_size;
+	bool cmd_fixed;
+	void (*rsp_func) (const void *data, uint8_t size);
+	uint8_t rsp_size;
+	bool rsp_fixed;
+};
+
 void vendor_event(uint16_t manufacturer, const void *data, uint8_t size);
-- 
2.47.3