monitor: Add basic decoding for MSFT extension

1 files changed, 127 insertions, 3 deletions

diff --git a/monitor/msft.c b/monitor/msft.c
index e9a15d265..3357e0f4f 100644
--- a/monitor/msft.c
+++ b/monitor/msft.c
@@ -30,23 +30,147 @@
 #include <stdio.h>
 #include <inttypes.h>
 
+#include "src/shared/util.h"
 #include "display.h"
 #include "packet.h"
 #include "vendor.h"
 #include "msft.h"
 
+#define COLOR_COMMAND		COLOR_BLUE
+#define COLOR_COMMAND_UNKNOWN	COLOR_WHITE_BG
+
+static void null_cmd(const void *data, uint16_t size)
+{
+}
+
+static void null_rsp(const void *data, uint16_t size)
+{
+}
+
+static void read_supported_features_rsp(const void *data, uint16_t size)
+{
+	uint8_t evt_prefix_len = get_u8(data + 8);
+
+	packet_print_features_msft(data);
+	print_field("Event prefix length: %u", evt_prefix_len);
+	packet_hexdump(data + 9, size - 9);
+}
+
+static void set_adv_filter_enable_cmd(const void *data, uint16_t size)
+{
+	uint8_t enable = get_u8(data);
+	const char *str;
+
+	switch (enable) {
+	case 0x00:
+		str = "Current allow list";
+		break;
+	case 0x01:
+		str = "All filter conditions";
+		break;
+	default:
+		str = "Reserved";
+		break;
+	}
+
+	print_field("Enable: %s (0x%2.2x)", str, enable);
+}
+
+typedef void (*func_t) (const void *data, uint16_t size);
+
+static const struct {
+	uint8_t code;
+	const char *str;
+	func_t cmd_func;
+	func_t rsp_func;
+} cmd_table[] = {
+	{ 0x00, "Read Supported Features",
+			null_cmd,
+			read_supported_features_rsp },
+	{ 0x01, "Monitor RSSI" },
+	{ 0x02, "Cancel Monitor RSSI" },
+	{ 0x03, "LE Monitor Advertisement" },
+	{ 0x04, "LE Cancel Monitor Advertisement" },
+	{ 0x05, "LE Set Advertisement Filter Enable",
+			set_adv_filter_enable_cmd,
+			null_rsp },
+	{ 0x06, "Read Absolute RSSI" },
+	{ }
+};
+
 static void msft_cmd(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; cmd_table[i].str; i++) {
+		if (cmd_table[i].code == code) {
+			code_str = cmd_table[i].str;
+			code_func = cmd_table[i].cmd_func;
+			break;
+		}
+	}
+
+	if (code_str) {
+		if (code_func)
+			code_color = COLOR_COMMAND;
+		else
+			code_color = COLOR_COMMAND_UNKNOWN;
+	} else {
+		code_color = COLOR_COMMAND_UNKNOWN;
+		code_str = "Unknown";
+	}
+
+	print_indent(6, code_color, "", code_str, COLOR_OFF,
+						" (0x%2.2x)", code);
+
+	if (code_func)
+		code_func(data + 1, size - 1);
+	else
+		packet_hexdump(data + 1, size - 1);
 }
 
 static void msft_rsp(const void *data, uint8_t size)
 {
-	packet_hexdump(data, size);
+	uint8_t status = get_u8(data);
+	uint8_t code = get_u8(data + 1);
+	const char *code_color, *code_str = NULL;
+	func_t code_func = NULL;
+	int i;
+
+	for (i = 0; cmd_table[i].str; i++) {
+		if (cmd_table[i].code == code) {
+			code_str = cmd_table[i].str;
+			code_func = cmd_table[i].rsp_func;
+			break;
+		}
+	}
+
+	if (code_str) {
+		if (code_func)
+			code_color = COLOR_COMMAND;
+		else
+			code_color = COLOR_COMMAND_UNKNOWN;
+	} else {
+		code_color = COLOR_COMMAND_UNKNOWN;
+		code_str = "Unknown";
+	}
+
+	print_indent(6, code_color, "", code_str, COLOR_OFF,
+						" (0x%2.2x)", code);
+
+	packet_print_error("Status", status);
+
+	if (code_func)
+		code_func(data + 2, size - 2);
+	else
+		packet_hexdump(data + 2, size - 2);
 }
 
 static const struct vendor_ocf vendor_ocf_entry = {
-	0x000, "Extension", msft_cmd, 1, false, msft_rsp, 1, false
+	0x000, "Extension", msft_cmd, 1, false, msft_rsp, 2, false
 };
 
 const struct vendor_ocf *msft_vendor_ocf(void)