[PATCH BlueZ] monitor: Add decoding for Microsoft defined event

[Archie Pusaka <apusaka@google.com>]

From: Archie Pusaka <apusaka@chromium.org>

This adds decoders to MSFT LE monitor device event
---

 monitor/msft.c   | 78 +++++++++++++++++++++++++++++++++++++++++++++++-
 monitor/msft.h   |  1 +
 monitor/packet.c | 44 ++++++++++++++++++++++-----
 3 files changed, 115 insertions(+), 8 deletions(-)

diff --git a/monitor/msft.c b/monitor/msft.c
index 054f34006..d41f10d68 100644
--- a/monitor/msft.c
+++ b/monitor/msft.c
@@ -41,6 +41,8 @@
 
 #define COLOR_COMMAND		COLOR_BLUE
 #define COLOR_COMMAND_UNKNOWN	COLOR_WHITE_BG
+#define COLOR_EVENT		COLOR_MAGENTA
+#define COLOR_EVENT_UNKNOWN	COLOR_WHITE_BG
 
 static void null_cmd(const void *data, uint16_t size)
 {
@@ -299,10 +301,72 @@ const struct vendor_ocf *msft_vendor_ocf(void)
 	return &vendor_ocf_entry;
 }
 
+static void monitor_device_evt(const void *data, uint16_t size)
+{
+	const struct msft_evt_monitor_device *evt = data;
+	const char *str_state;
+
+	packet_print_addr(NULL, evt->addr, evt->addr_type);
+	print_field("Monitor handle: %u", evt->handle);
+
+	switch (evt->state) {
+	case 0x00:
+		str_state = "Stop monitoring";
+		break;
+	case 0x01:
+		str_state = "Start monitoring";
+		break;
+	default:
+		str_state = "Reserved";
+		break;
+	}
+
+	print_field("State: %s (0x%2.2x)", str_state, evt->state);
+}
+
+static const struct {
+	uint8_t code;
+	const char *str;
+	func_t evt_func;
+} evt_table[] = {
+	{ 0x01, "RSSI Event" },
+	{ 0x02, "LE Monitor Device Event", monitor_device_evt },
+	{ }
+};
+
 static void msft_evt(struct timeval *tv, uint16_t index,
 			const void *data, uint8_t size)
 {
-	packet_hexdump(data, size);
+	uint8_t code = get_u8(data);
+	const char *code_color, *code_str = NULL;
+	func_t code_func = NULL;
+	int i;
+
+	for (i = 0; evt_table[i].str; i++) {
+		if (evt_table[i].code == code) {
+			code_str = evt_table[i].str;
+			code_func = evt_table[i].evt_func;
+			break;
+		}
+	}
+
+	if (code_str) {
+		if (code_func)
+			code_color = COLOR_EVENT;
+		else
+			code_color = COLOR_EVENT_UNKNOWN;
+	} else {
+		code_color = COLOR_EVENT_UNKNOWN;
+		code_str = "Unknown";
+	}
+
+	print_indent(6, code_color, "", code_str, COLOR_OFF,
+						" (0x%2.2x)", code);
+
+	if (code_func)
+		code_func(data, size);
+	else
+		packet_hexdump(data + 1, size - 1);
 }
 
 static const struct vendor_evt vendor_evt_entry = {
@@ -313,3 +377,15 @@ const struct vendor_evt *msft_vendor_evt(void)
 {
 	return &vendor_evt_entry;
 }
+
+bool msft_event_code_valid(uint8_t code)
+{
+	int i;
+
+	for (i = 0; evt_table[i].str; i++) {
+		if (evt_table[i].code == code)
+			return true;
+	}
+
+	return false;
+}
diff --git a/monitor/msft.h b/monitor/msft.h
index e6e3019be..ff6c28011 100644
--- a/monitor/msft.h
+++ b/monitor/msft.h
@@ -187,3 +187,4 @@ struct vendor_evt;
 
 const struct vendor_ocf *msft_vendor_ocf(void);
 const struct vendor_evt *msft_vendor_evt(void);
+bool msft_event_code_valid(uint8_t code);
diff --git a/monitor/packet.c b/monitor/packet.c
index ff0b1cac2..02f69dfca 100644
--- a/monitor/packet.c
+++ b/monitor/packet.c
@@ -450,8 +450,10 @@ void packet_set_fallback_manufacturer(uint16_t manufacturer)
 
 void packet_set_msft_evt_prefix(const uint8_t *prefix, uint8_t len)
 {
-	if (index_current < MAX_INDEX && len < 8)
+	if (index_current < MAX_INDEX && len < 8) {
 		memcpy(index_list[index_current].msft_evt_prefix, prefix, len);
+		index_list[index_current].msft_evt_len = len;
+	}
 }
 
 static void cred_pid(struct ucred *cred, char *str, size_t len)
@@ -4359,7 +4361,8 @@ static int addr2str(const uint8_t *addr, char *str)
 			addr[5], addr[4], addr[3], addr[2], addr[1], addr[0]);
 }
 
-static int get_msft_opcode(uint16_t manufacturer) {
+static int get_msft_opcode(uint16_t manufacturer)
+{
 	switch (manufacturer) {
 	case COMPANY_ID_INTEL:
 		return 0xFC1E;
@@ -4374,7 +4377,23 @@ static int get_msft_opcode(uint16_t manufacturer) {
 	default:
 		return BT_HCI_CMD_NOP;
 	}
+}
+
+static bool msft_event_prefix_match(const void *data, int size)
+{
+	const void *prefix = index_list[index_current].msft_evt_prefix;
+	int prefix_len = index_list[index_current].msft_evt_len;
+
+	/*
+	 * MSFT event has one byte of event code following the MSFT prefix.
+	 * We need to check the event code is valid, as it's possible for
+	 * a vendor to use the same MSFT prefix but for other events.
+	 */
+	if (size >= prefix_len + 1 && !memcmp(prefix, data, prefix_len))
+		return msft_event_code_valid(
+				((const uint8_t *) data)[prefix_len]);
 
+	return false;
 }
 
 void packet_monitor(struct timeval *tv, struct ucred *cred,
@@ -10925,11 +10944,16 @@ static const struct vendor_ocf *current_vendor_ocf(uint16_t ocf)
 }
 
 static const struct vendor_evt *current_vendor_evt(const void *data,
-							int *consumed_size)
+					uint8_t size, int *consumed_size)
 {
 	uint16_t manufacturer;
 	uint8_t evt = *((const uint8_t *) data);
 
+	if (msft_event_prefix_match(data, size)) {
+		*consumed_size = index_list[index_current].msft_evt_len;
+		return msft_vendor_evt();
+	}
+
 	/* A regular vendor event consumes 1 byte. */
 	*consumed_size = 1;
 
@@ -10948,10 +10972,13 @@ static const struct vendor_evt *current_vendor_evt(const void *data,
 	return NULL;
 }
 
-static const char *current_vendor_evt_str(void)
+static const char *current_vendor_evt_str(const void *data, uint8_t size)
 {
 	uint16_t manufacturer;
 
+	if (msft_event_prefix_match(data, size))
+		return "Microsoft";
+
 	if (index_current < MAX_INDEX)
 		manufacturer = index_list[index_current].manufacturer;
 	else
@@ -13547,17 +13574,20 @@ static void vendor_evt(struct timeval *tv, uint16_t index,
 	struct subevent_data vendor_data;
 	char vendor_str[150];
 	int consumed_size;
-	const struct vendor_evt *vnd = current_vendor_evt(data, &consumed_size);
+	const struct vendor_evt *vnd = current_vendor_evt(data, size,
+								&consumed_size);
 
 	if (vnd) {
-		const char *str = current_vendor_evt_str();
+		const char *str = current_vendor_evt_str(data, size);
 
 		if (str) {
 			snprintf(vendor_str, sizeof(vendor_str),
 						"%s %s", str, vnd->str);
 			vendor_data.str = vendor_str;
-		} else
+		} else {
 			vendor_data.str = vnd->str;
+		}
+
 		vendor_data.subevent = vnd->evt;
 		vendor_data.func = vnd->evt_func;
 		vendor_data.size = vnd->evt_size;
-- 
2.54.0.rc0.605.g598a273b03-goog