From 7b95755f07b1ee042f2a1f71168e9c8cd52e226b Mon Sep 17 00:00:00 2001
From: Jan Schmidt <jan@centricular.com>
Date: Mon, 15 Nov 2021 00:06:18 +1100
Subject: [PATCH] d/wmr: Implement bluetooth debug message handling

Handle the debug message packet from the bluetooth
interface on the HP G2. There might be other packet
types, but that's the only one I've seen so far.
---
 src/xrt/drivers/wmr/wmr_hmd.c      | 27 +++++++++++++++++++++++++++
 src/xrt/drivers/wmr/wmr_protocol.h |  6 ++++--
 2 files changed, 31 insertions(+), 2 deletions(-)

diff --git a/src/xrt/drivers/wmr/wmr_hmd.c b/src/xrt/drivers/wmr/wmr_hmd.c
index 8d018acf2..1d0a9b4db 100644
--- a/src/xrt/drivers/wmr/wmr_hmd.c
+++ b/src/xrt/drivers/wmr/wmr_hmd.c
@@ -78,6 +78,33 @@ hololens_unknown_17_decode_packet(struct wmr_hmd *wh, const unsigned char *buffe
 static void
 hololens_handle_bt_iface_packet(struct wmr_hmd *wh, const unsigned char *buffer, int size)
 {
+	int pkt_type;
+
+	if (size < 2)
+		return;
+
+	if (size < 6) {
+		WMR_DEBUG(wh, "Short Bluetooth interface packet (%d) type 0x%02x", size, buffer[1]);
+		return;
+	}
+
+	pkt_type = buffer[1];
+	if (pkt_type != WMR_BT_IFACE_MSG_DEBUG) {
+		WMR_DEBUG(wh, "Unknown Bluetooth interface packet (%d) type 0x%02x", size, pkt_type);
+		return;
+	}
+	buffer += 2;
+
+	uint16_t tag = read16(&buffer);
+	uint16_t msg_len = read16(&buffer);
+
+	if (size < msg_len + 6) {
+		WMR_DEBUG(wh, "Bluetooth interface debug packet (%d) too short. tag 0x%x msg len %u", size, tag,
+		          msg_len);
+		return;
+	}
+
+	WMR_DEBUG(wh, "BT debug: tag %d: %.*s", tag, msg_len, buffer);
 }
 
 static void
diff --git a/src/xrt/drivers/wmr/wmr_protocol.h b/src/xrt/drivers/wmr/wmr_protocol.h
index 8de3902e0..4db34fab1 100644
--- a/src/xrt/drivers/wmr/wmr_protocol.h
+++ b/src/xrt/drivers/wmr/wmr_protocol.h
@@ -34,8 +34,8 @@ extern "C" {
 #define WMR_MS_HOLOLENS_MSG_SENSORS 0x01
 #define WMR_MS_HOLOLENS_MSG_CONTROL 0x02 // Integrated motion controller messages?
 #define WMR_MS_HOLOLENS_MSG_DEBUG 0x03
-#define WMR_MS_HOLOLENS_MSG_BT_IFACE 0x05 /* Bluetooth interface */
-#define WMR_MS_HOLOLENS_MSG_LEFT_CONTROLLER 0x06 /* Left controller */
+#define WMR_MS_HOLOLENS_MSG_BT_IFACE 0x05         /* Bluetooth interface */
+#define WMR_MS_HOLOLENS_MSG_LEFT_CONTROLLER 0x06  /* Left controller */
 #define WMR_MS_HOLOLENS_MSG_RIGHT_CONTROLLER 0x0E /* Right controller */
 #define WMR_MS_HOLOLENS_MSG_UNKNOWN_17 0x17
 
@@ -43,6 +43,8 @@ extern "C" {
 #define WMR_CONTROL_MSG_IPD_VALUE 0x01
 #define WMR_CONTROL_MSG_UNKNOWN_05 0x05
 
+#define WMR_BT_IFACE_MSG_DEBUG 0x19
+
 #define STR_TO_U32(s) ((uint32_t)(((s)[0]) | ((s)[1] << 8) | ((s)[2] << 16) | ((s)[3] << 24)))
 #define WMR_MAGIC STR_TO_U32("Dlo+")