From d81794bf4004c13459e557c8c00daaf6fe8d060e Mon Sep 17 00:00:00 2001
From: Marcel Holtmann <marcel@holtmann.org>
Date: Sat, 16 Jun 2018 22:17:40 +0200
Subject: [PATCH] monitor: Add support for decoding Broadcom high priority
 connection

---
 monitor/broadcom.c | 32 +++++++++++++++++++++++++++++++-
 monitor/packet.c   | 12 +++++++++++-
 monitor/packet.h   |  1 +
 3 files changed, 43 insertions(+), 2 deletions(-)

diff --git a/monitor/broadcom.c b/monitor/broadcom.c
index 901483b98..d2304e244 100644
--- a/monitor/broadcom.c
+++ b/monitor/broadcom.c
@@ -44,6 +44,11 @@ static void print_status(uint8_t status)
 	packet_print_error("Status", status);
 }
 
+static void print_handle(uint16_t handle)
+{
+	packet_print_handle(handle);
+}
+
 static void print_sco_routing(uint8_t routing)
 {
 	const char *str;
@@ -403,6 +408,29 @@ static void read_vid_pid_rsp(const void *data, uint8_t size)
 	print_field("Product: %4.4x:%4.4x", vid, pid);
 }
 
+static void write_high_priority_connection_cmd(const void *data, uint8_t size)
+{
+	uint16_t handle = get_le16(data);
+	uint8_t priority = get_u8(data + 2);
+	const char *str;
+
+	print_handle(handle);
+
+	switch (priority) {
+	case 0x00:
+		str = "Low";
+		break;
+	case 0x01:
+		str = "High";
+		break;
+	default:
+		str = "Reserved";
+		break;
+	}
+
+	print_field("Priority: %s (0x%2.2x)", str, priority);
+}
+
 static const struct {
 	uint8_t bit;
 	const char *str;
@@ -524,7 +552,9 @@ static const struct vendor_ocf vendor_ocf_table[] = {
 	{ 0x05a, "Read VID PID",
 			null_cmd, 0, true,
 			read_vid_pid_rsp, 5, true },
-	{ 0x057, "Write High Priority Connection" },
+	{ 0x057, "Write High Priority Connection",
+			write_high_priority_connection_cmd, 3, true,
+			status_rsp, 1, true },
 	{ 0x06d, "Write I2SPCM Interface Param" },
 	{ 0x06e, "Read Controller Features",
 			null_cmd, 0, true,
diff --git a/monitor/packet.c b/monitor/packet.c
index cba740791..942470fd3 100644
--- a/monitor/packet.c
+++ b/monitor/packet.c
@@ -734,9 +734,14 @@ static void print_lt_addr(uint8_t lt_addr)
 	print_field("LT address: %d", lt_addr);
 }
 
+static void print_handle_native(uint16_t handle)
+{
+	print_field("Handle: %d", handle);
+}
+
 static void print_handle(uint16_t handle)
 {
-	print_field("Handle: %d", le16_to_cpu(handle));
+	print_handle_native(le16_to_cpu(handle));
 }
 
 static void print_phy_handle(uint8_t phy_handle)
@@ -3871,6 +3876,11 @@ void packet_print_addr(const char *label, const void *data, bool random)
 	print_addr(label ? : "Address", data, random ? 0x01 : 0x00);
 }
 
+void packet_print_handle(uint16_t handle)
+{
+	print_handle_native(handle);
+}
+
 void packet_print_ad(const void *data, uint8_t size)
 {
 	print_eir(data, size, true);
diff --git a/monitor/packet.h b/monitor/packet.h
index 79733deec..e95738401 100644
--- a/monitor/packet.h
+++ b/monitor/packet.h
@@ -50,6 +50,7 @@ void packet_print_version(const char *label, uint8_t version,
 				const char *sublabel, uint16_t subversion);
 void packet_print_company(const char *label, uint16_t company);
 void packet_print_addr(const char *label, const void *data, bool random);
+void packet_print_handle(uint16_t handle);
 void packet_print_ad(const void *data, uint8_t size);
 void packet_print_features_lmp(const uint8_t *features, uint8_t page);
 void packet_print_features_ll(const uint8_t *features);
-- 
2.47.3