From b33ade97b244fc3193fea000abbb89d446adfa13 Mon Sep 17 00:00:00 2001 From: Marcel Holtmann Date: Sun, 18 Oct 2015 15:14:57 +0200 Subject: [PATCH] monitor: Add support for handling Intel vendor commands --- monitor/intel.c | 254 +++++++++++++++++++++++++++++------------------ monitor/intel.h | 6 +- monitor/packet.c | 156 ++++++++++++++++++----------- monitor/vendor.h | 8 ++ 4 files changed, 272 insertions(+), 152 deletions(-) diff --git a/monitor/intel.c b/monitor/intel.c index 10a4a1bb3..6148be9ff 100644 --- a/monitor/intel.c +++ b/monitor/intel.c @@ -33,109 +33,175 @@ #include "packet.h" #include "lmp.h" #include "ll.h" +#include "vendor.h" #include "intel.h" -void intel_vendor_event(const void *data, uint8_t size) +static const struct vendor_ocf vendor_ocf_table[] = { + { 0x001, "Reset" }, + { 0x002, "No Operation" }, + { 0x005, "Read Version" }, + { 0x006, "Set UART Baudrate" }, + { 0x007, "Enable LPM" }, + { 0x008, "PCM Write Configuration" }, + { 0x009, "Secure Send" }, + { 0x00d, "Read Secure Boot Params" }, + { 0x00e, "Write Secure Boot Params" }, + { 0x00f, "Unlock" }, + { 0x010, "Change UART Baudrate" }, + { 0x011, "Manufacturer Mode" }, + { 0x012, "Read Link RSSI" }, + { 0x022, "Get Exception Info" }, + { 0x024, "Clear Exception Info" }, + { 0x02f, "Write BD Data" }, + { 0x030, "Read BD Data" }, + { 0x031, "Write BD Address" }, + { 0x032, "Flow Specification" }, + { 0x034, "Read Secure ID" }, + { 0x038, "Set Synchronous USB Interface Type" }, + { 0x039, "Config Synchronous Interface" }, + { 0x03f, "SW RF Kill" }, + { 0x043, "Activate Deactivate Traces" }, + { 0x050, "Read HW Version" }, + { 0x052, "Set Event Mask" }, + { 0x053, "Config_Link_Controller" }, + { 0x089, "DDC Write" }, + { 0x08a, "DDC Read" }, + { 0x08b, "DDC Config Write" }, + { 0x08c, "DDC Config Read" }, + { 0x08d, "Memory Read" }, + { 0x08e, "Memory Write" }, + { } +}; + +const struct vendor_ocf *intel_vendor_ocf(uint16_t ocf) { - uint8_t evt, type, len, id; + int i; + + for (i = 0; vendor_ocf_table[i].str; i++) { + if (vendor_ocf_table[i].ocf == ocf) + return &vendor_ocf_table[i]; + } + + return NULL; +} + +static void act_deact_traces_complete_evt(const void *data, uint8_t size) +{ + uint8_t status = *((const uint8_t *) data); + + packet_print_error("Status", status); +} + +static void lmp_pdu_trace_evt(const void *data, uint8_t size) +{ + uint8_t type, len, id; uint16_t handle, count; uint32_t clock; const char *str; - evt = *((uint8_t *) data); - - print_field("Event: 0x%2.2x", evt); - - switch (evt) { - case 0x17: - type = *((uint8_t *) (data + 1)); - handle = get_le16(data + 2); - - switch (type) { - case 0x00: - str = "RX LMP"; - break; - case 0x01: - str = "TX LMP"; - break; - case 0x02: - str = "ACK LMP"; - break; - case 0x03: - str = "RX LL"; - break; - case 0x04: - str = "TX LL"; - break; - case 0x05: - str = "ACK LL"; - break; - default: - str = "Unknown"; - break; - } - - print_field("Type: %s (0x%2.2x)", str, type); - print_field("Handle: %u", handle); - - switch (type) { - case 0x00: - len = size - 9; - clock = get_le32(data + 5 + len); - - packet_hexdump(data + 4, 1); - lmp_packet(data + 5, len, false); - print_field("Clock: 0x%8.8x", clock); - break; - case 0x01: - len = size - 10; - clock = get_le32(data + 5 + len); - id = *((uint8_t *) (data + 5 + len + 4)); - - packet_hexdump(data + 4, 1); - lmp_packet(data + 5, len, false); - print_field("Clock: 0x%8.8x", clock); - print_field("ID: 0x%2.2x", id); - break; - case 0x02: - clock = get_le32(data + 4); - id = *((uint8_t *) (data + 4 + 4)); - - print_field("Clock: 0x%8.8x", clock); - print_field("ID: 0x%2.2x", id); - break; - case 0x03: - len = size - 9; - count = get_le16(data + 4); - - print_field("Count: 0x%4.4x", count); - packet_hexdump(data + 4 + 2 + 1, 2); - llcp_packet(data + 9, len, false); - break; - case 0x04: - len = size - 9; - count = get_le16(data + 4); - id = *((uint8_t *) (data + 4 + 2)); - - print_field("Count: 0x%4.4x", count); - print_field("ID: 0x%2.2x", id); - packet_hexdump(data + 4 + 2 + 1, 2); - llcp_packet(data + 9, len, false); - break; - case 0x05: - count = get_le16(data + 4); - id = *((uint8_t *) (data + 4 + 2)); - - print_field("Count: 0x%4.4x", count); - print_field("ID: 0x%2.2x", id); - break; - default: - packet_hexdump(data + 4, size - 4); - break; - } + type = *((uint8_t *) data); + handle = get_le16(data + 2); + + switch (type) { + case 0x00: + str = "RX LMP"; + break; + case 0x01: + str = "TX LMP"; + break; + case 0x02: + str = "ACK LMP"; + break; + case 0x03: + str = "RX LL"; + break; + case 0x04: + str = "TX LL"; + break; + case 0x05: + str = "ACK LL"; + break; + default: + str = "Unknown"; + break; + } + + print_field("Type: %s (0x%2.2x)", str, type); + print_field("Handle: %u", handle); + + switch (type) { + case 0x00: + len = size - 8; + clock = get_le32(data + 4 + len); + + packet_hexdump(data + 3, 1); + lmp_packet(data + 4, len, false); + print_field("Clock: 0x%8.8x", clock); + break; + case 0x01: + len = size - 9; + clock = get_le32(data + 4 + len); + id = *((uint8_t *) (data + 4 + len + 4)); + + packet_hexdump(data + 3, 1); + lmp_packet(data + 4, len, false); + print_field("Clock: 0x%8.8x", clock); + print_field("ID: 0x%2.2x", id); + break; + case 0x02: + clock = get_le32(data + 3); + id = *((uint8_t *) (data + 3 + 4)); + + print_field("Clock: 0x%8.8x", clock); + print_field("ID: 0x%2.2x", id); + break; + case 0x03: + len = size - 8; + count = get_le16(data + 3); + + print_field("Count: 0x%4.4x", count); + packet_hexdump(data + 3 + 2 + 1, 2); + llcp_packet(data + 8, len, false); + break; + case 0x04: + len = size - 8; + count = get_le16(data + 3); + id = *((uint8_t *) (data + 3 + 2)); + + print_field("Count: 0x%4.4x", count); + print_field("ID: 0x%2.2x", id); + packet_hexdump(data + 3 + 2 + 1, 2); + llcp_packet(data + 8, len, false); + break; + case 0x05: + count = get_le16(data + 3); + id = *((uint8_t *) (data + 3 + 2)); + + print_field("Count: 0x%4.4x", count); + print_field("ID: 0x%2.2x", id); break; default: - packet_hexdump(data + 1, size - 1); + packet_hexdump(data + 3, size - 3); break; } } + +static const struct vendor_evt vendor_evt_table[] = { + { 0x16, "Activate Deactivate Traces Complete", + act_deact_traces_complete_evt, 1, true }, + { 0x17, "LMP PDU Trace", + lmp_pdu_trace_evt, 3, false }, + { } +}; + +const struct vendor_evt *intel_vendor_evt(uint8_t evt) +{ + int i; + + for (i = 0; vendor_evt_table[i].str; i++) { + if (vendor_evt_table[i].evt == evt) + return &vendor_evt_table[i]; + } + + return NULL; +} diff --git a/monitor/intel.h b/monitor/intel.h index 828d855b8..573b23f3f 100644 --- a/monitor/intel.h +++ b/monitor/intel.h @@ -24,4 +24,8 @@ #include -void intel_vendor_event(const void *data, uint8_t size); +struct vendor_ocf; +struct vendor_evt; + +const struct vendor_ocf *intel_vendor_ocf(uint16_t ocf); +const struct vendor_evt *intel_vendor_evt(uint8_t evt); diff --git a/monitor/packet.c b/monitor/packet.c index a353193ef..3f2ad8d6d 100644 --- a/monitor/packet.c +++ b/monitor/packet.c @@ -7266,6 +7266,8 @@ static const struct vendor_ocf *current_vendor_ocf(uint16_t ocf) manufacturer = UNKNOWN_MANUFACTURER; switch (manufacturer) { + case 2: + return intel_vendor_ocf(ocf); case 15: return broadcom_vendor_ocf(ocf); } @@ -7273,6 +7275,23 @@ static const struct vendor_ocf *current_vendor_ocf(uint16_t ocf) return NULL; } +static const struct vendor_evt *current_vendor_evt(uint8_t evt) +{ + uint16_t manufacturer; + + if (index_current < MAX_INDEX) + manufacturer = index_list[index_current].manufacturer; + else + manufacturer = UNKNOWN_MANUFACTURER; + + switch (manufacturer) { + case 2: + return intel_vendor_evt(evt); + } + + return NULL; +} + static void inquiry_complete_evt(const void *data, uint8_t size) { const struct bt_hci_evt_inquiry_complete *evt = data; @@ -7445,11 +7464,16 @@ static void cmd_complete_evt(const void *data, uint8_t size) if (vnd) { vendor_data.str = vnd->str; - vendor_data.rsp_func = NULL; + vendor_data.rsp_func = vnd->rsp_func; + vendor_data.rsp_size = vnd->rsp_size; + vendor_data.rsp_fixed = vnd->rsp_fixed; opcode_data = &vendor_data; - opcode_color = COLOR_HCI_COMMAND; + if (opcode_data->rsp_func) + opcode_color = COLOR_HCI_COMMAND; + else + opcode_color = COLOR_HCI_COMMAND_UNKNOWN; opcode_str = opcode_data->str; } else { opcode_color = COLOR_HCI_COMMAND; @@ -8273,7 +8297,48 @@ struct subevent_data { bool fixed; }; -static const struct subevent_data subevent_table[] = { +static void print_subevent(const struct subevent_data *subevent_data, + const void *data, uint8_t size) +{ + const char *subevent_color, *subevent_str; + + if (subevent_data) { + if (subevent_data->func) + subevent_color = COLOR_HCI_EVENT; + else + subevent_color = COLOR_HCI_EVENT_UNKNOWN; + subevent_str = subevent_data->str; + } else { + subevent_color = COLOR_HCI_EVENT_UNKNOWN; + subevent_str = "Unknown"; + } + + print_indent(6, subevent_color, "", subevent_str, COLOR_OFF, + " (0x%2.2x)", subevent_data->subevent); + + if (!subevent_data || !subevent_data->func) { + packet_hexdump(data, size); + return; + } + + if (subevent_data->fixed) { + if (size != subevent_data->size) { + print_text(COLOR_ERROR, "invalid packet size"); + packet_hexdump(data, size); + return; + } + } else { + if (size < subevent_data->size) { + print_text(COLOR_ERROR, "too short packet"); + packet_hexdump(data, size); + return; + } + } + + subevent_data->func(data, size); +} + +static const struct subevent_data le_meta_event_table[] = { { 0x01, "LE Connection Complete", le_conn_complete_evt, 18, true }, { 0x02, "LE Advertising Report", @@ -8303,68 +8368,40 @@ static void le_meta_event_evt(const void *data, uint8_t size) { uint8_t subevent = *((const uint8_t *) data); const struct subevent_data *subevent_data = NULL; - const char *subevent_color, *subevent_str; int i; - for (i = 0; subevent_table[i].str; i++) { - if (subevent_table[i].subevent == subevent) { - subevent_data = &subevent_table[i]; + for (i = 0; le_meta_event_table[i].str; i++) { + if (le_meta_event_table[i].subevent == subevent) { + subevent_data = &le_meta_event_table[i]; break; } } - if (subevent_data) { - if (subevent_data->func) - subevent_color = COLOR_HCI_EVENT; - else - subevent_color = COLOR_HCI_EVENT_UNKNOWN; - subevent_str = subevent_data->str; - } else { - subevent_color = COLOR_HCI_EVENT_UNKNOWN; - subevent_str = "Unknown"; - } - - print_indent(6, subevent_color, "", subevent_str, COLOR_OFF, - " (0x%2.2x)", subevent); - - if (!subevent_data || !subevent_data->func) { - packet_hexdump(data + 1, size - 1); - return; - } - - if (subevent_data->fixed) { - if (size - 1 != subevent_data->size) { - print_text(COLOR_ERROR, "invalid packet size"); - packet_hexdump(data + 1, size - 1); - return; - } - } else { - if (size - 1 < subevent_data->size) { - print_text(COLOR_ERROR, "too short packet"); - packet_hexdump(data + 1, size - 1); - return; - } - } - - subevent_data->func(data + 1, size - 1); + print_subevent(subevent_data, data + 1, size - 1); } static void vendor_evt(const void *data, uint8_t size) { - uint16_t manufacturer; + uint8_t subevent = *((const uint8_t *) data); + struct subevent_data vendor_data; + const struct vendor_evt *vnd = current_vendor_evt(subevent); - if (index_current < MAX_INDEX) - manufacturer = index_list[index_current].manufacturer; - else - manufacturer = UNKNOWN_MANUFACTURER; + if (vnd) { + vendor_data.str = vnd->str; + vendor_data.func = vnd->evt_func; + vendor_data.size = vnd->evt_size; + vendor_data.fixed = vnd->evt_fixed; + + print_subevent(&vendor_data, data + 1, size - 1); + } else { + uint16_t manufacturer; + + if (index_current < MAX_INDEX) + manufacturer = index_list[index_current].manufacturer; + else + manufacturer = UNKNOWN_MANUFACTURER; - switch (manufacturer) { - case 2: - intel_vendor_event(data, size); - break; - default: vendor_event(manufacturer, data, size); - break; } } @@ -8636,11 +8673,16 @@ void packet_hci_command(struct timeval *tv, uint16_t index, if (vnd) { vendor_data.str = vnd->str; - vendor_data.cmd_func = NULL; + vendor_data.cmd_func = vnd->cmd_func; + vendor_data.cmd_size = vnd->cmd_size; + vendor_data.cmd_fixed = vnd->cmd_fixed; opcode_data = &vendor_data; - opcode_color = COLOR_HCI_COMMAND; + if (opcode_data->cmd_func) + opcode_color = COLOR_HCI_COMMAND; + else + opcode_color = COLOR_HCI_COMMAND_UNKNOWN; opcode_str = opcode_data->str; } else { opcode_color = COLOR_HCI_COMMAND; @@ -8866,10 +8908,10 @@ void packet_todo(void) printf("\t%s\n", event_table[i].str); } - for (i = 0; subevent_table[i].str; i++) { - if (subevent_table[i].func) + for (i = 0; le_meta_event_table[i].str; i++) { + if (le_meta_event_table[i].func) continue; - printf("\t%s\n", subevent_table[i].str); + printf("\t%s\n", le_meta_event_table[i].str); } } diff --git a/monitor/vendor.h b/monitor/vendor.h index be7baab81..f5792b3c5 100644 --- a/monitor/vendor.h +++ b/monitor/vendor.h @@ -35,4 +35,12 @@ struct vendor_ocf { bool rsp_fixed; }; +struct vendor_evt { + uint8_t evt; + const char *str; + void (*evt_func) (const void *data, uint8_t size); + uint8_t evt_size; + bool evt_fixed; +}; + void vendor_event(uint16_t manufacturer, const void *data, uint8_t size); -- 2.47.3