Diff between 8e2d46d8b1cfa8169aa70eb50c9807add934e555 and 3d9ff9d52edf0b2ad4f7c878b6197dc369240d04

Changed Files

File Additions Deletions Status
monitor/msft.c +127 -3 modified
monitor/packet.c +22 -0 modified
monitor/packet.h +1 -0 modified

Full Patch

diff --git a/monitor/msft.c b/monitor/msft.c
index e9a15d2..3357e0f 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)
diff --git a/monitor/packet.c b/monitor/packet.c
index 0c98fd7..2fdea53 100644
--- a/monitor/packet.c
+++ b/monitor/packet.c
@@ -2638,6 +2638,16 @@ static const struct bitfield_data features_le[] = {
 	{ }
 };
 
+static const struct bitfield_data features_msft[] = {
+	{  0, "RSSI Monitoring feature for BR/EDR"		},
+	{  1, "RSSI Monitoring feature for LE connections"	},
+	{  2, "RSSI Monitoring of LE advertisements"		},
+	{  3, "Advertising Monitoring of LE advertisements"	},
+	{  4, "Verifying the validity of P-192 and P-256 keys"	},
+	{  5, "Continuous Advertising Monitoring"		},
+	{ }
+};
+
 static void print_features(uint8_t page, const uint8_t *features_array,
 								uint8_t type)
 {
@@ -2674,6 +2684,13 @@ static void print_features(uint8_t page, const uint8_t *features_array,
 			break;
 		}
 		break;
+	case 0xf0:
+		switch (page) {
+		case 0:
+			features_table = features_msft;
+			break;
+		}
+		break;
 	}
 
 	if (!features_table)
@@ -2695,6 +2712,11 @@ void packet_print_features_ll(const uint8_t *features)
 	print_features(0, features, 0x01);
 }
 
+void packet_print_features_msft(const uint8_t *features)
+{
+	print_features(0, features, 0xf0);
+}
+
 #define LE_STATE_SCAN_ADV		0x0001
 #define LE_STATE_CONN_ADV		0x0002
 #define LE_STATE_NONCONN_ADV		0x0004
diff --git a/monitor/packet.h b/monitor/packet.h
index 409bf0e..afbe10f 100644
--- a/monitor/packet.h
+++ b/monitor/packet.h
@@ -43,6 +43,7 @@ void packet_print_rssi(int8_t rssi);
 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);
+void packet_print_features_msft(const uint8_t *features);
 void packet_print_channel_map_lmp(const uint8_t *map);
 void packet_print_channel_map_ll(const uint8_t *map);
 void packet_print_io_capability(uint8_t capability);