diff --git a/monitor/msft.c b/monitor/msft.c
index e9a15d2..3357e0f 100644
--- a/monitor/msft.c
+++ b/monitor/msft.c
#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
{ }
};
+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)
{
break;
}
break;
+ case 0xf0:
+ switch (page) {
+ case 0:
+ features_table = features_msft;
+ break;
+ }
+ break;
}
if (!features_table)
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
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);