From 3d9ff9d52edf0b2ad4f7c878b6197dc369240d04 Mon Sep 17 00:00:00 2001 From: Marcel Holtmann Date: Fri, 19 Mar 2021 20:29:51 +0100 Subject: [PATCH] monitor: Add basic decoding for MSFT extension --- monitor/msft.c | 130 +++++++++++++++++++++++++++++++++++++++++++++-- monitor/packet.c | 22 ++++++++ monitor/packet.h | 1 + 3 files changed, 150 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 #include +#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 0c98fd766..2fdea532d 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 409bf0e54..afbe10f5b 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); -- 2.47.3