From a11313666371fb395d64c093efc63525323b706e Mon Sep 17 00:00:00 2001 From: Marcel Holtmann Date: Sun, 18 Oct 2015 21:45:39 +0200 Subject: [PATCH] monitor: Update handling for vendor commands and events --- monitor/broadcom.c | 146 +++++++++++++++++++++++++++++++++++++++++---- monitor/broadcom.h | 2 + monitor/intel.c | 21 ++++--- monitor/packet.c | 2 + src/shared/util.h | 5 ++ 5 files changed, 158 insertions(+), 18 deletions(-) diff --git a/monitor/broadcom.c b/monitor/broadcom.c index 7c76b97a9..a3c344398 100644 --- a/monitor/broadcom.c +++ b/monitor/broadcom.c @@ -36,17 +36,127 @@ #include "vendor.h" #include "broadcom.h" +static void print_status(uint8_t status) +{ + packet_print_error("Status", status); +} + +static void null_cmd(const void *data, uint8_t size) +{ +} + +static void status_rsp(const void *data, uint8_t size) +{ + uint8_t status = get_u8(data); + + print_status(status); +} + +static void write_bd_addr_cmd(const void *data, uint8_t size) +{ + packet_print_addr("Address", data, false); +} + +static void enable_usb_hid_emulation_cmd(const void *data, uint8_t size) +{ + uint8_t enable = get_u8(data); + const char *str; + + switch (enable) { + case 0x00: + str = "Bluetooth mode"; + break; + case 0x01: + str = "HID Mode"; + break; + default: + str = "Reserved"; + break; + } + + print_field("Enable: %s (0x%2.2x)", str, enable); +} + +static void write_ram_cmd(const void *data, uint8_t size) +{ + uint32_t addr = get_le32(data); + + print_field("Address: 0x%8.8x", addr); + + packet_hexdump(data + 4, size - 4); +} + +static void launch_ram_cmd(const void *data, uint8_t size) +{ + uint32_t addr = get_le32(data); + + print_field("Address: 0x%8.8x", addr); +} + +static void read_vid_pid_rsp(const void *data, uint8_t size) +{ + uint8_t status = get_u8(data); + uint16_t vid = get_le16(data + 1); + uint16_t pid = get_le16(data + 3); + + print_status(status); + print_field("Product: %4.4x:%4.4x", vid, pid); +} + +static void read_verbose_version_info_rsp(const void *data, uint8_t size) +{ + uint8_t status = get_u8(data); + uint8_t chip_id = get_u8(data + 1); + uint8_t target_id = get_u8(data + 2); + uint16_t build_base = get_le16(data + 3); + uint16_t build_num = get_le16(data + 5); + const char *str; + + print_status(status); + print_field("Chip ID: %u (0x%2.2x)", chip_id, chip_id); + + switch (target_id) { + case 254: + str = "Invalid"; + break; + case 255: + str = "Undefined"; + break; + default: + str = "Reserved"; + break; + } + + print_field("Build target: %s (%u)", str, target_id); + print_field("Build baseline: %u (0x%4.4x)", build_base, build_base); + print_field("Build number: %u (0x%4.4x)", build_num, build_num); +} + static const struct vendor_ocf vendor_ocf_table[] = { - { 0x001, "Write BD ADDR" }, - { 0x018, "Update UART Baud Rate" }, - { 0x027, "Set Sleepmode Param" }, - { 0x02e, "Download Minidriver" }, - { 0x03b, "Enable USB HID Emulation" }, - { 0x045, "Write UART Clock Setting" }, - { 0x04c, "Write RAM" }, - { 0x04e, "Launch RAM" }, - { 0x05a, "Read VID PID" }, - { 0x079, "Read Verbose Config Version Info" }, + { 0x001, "Write BD ADDR", + write_bd_addr_cmd, 6, true, + status_rsp, 1, true }, + { 0x018, "Update UART Baud Rate" }, + { 0x027, "Set Sleepmode Param" }, + { 0x02e, "Download Minidriver", + null_cmd, 0, true, + status_rsp, 1, true }, + { 0x03b, "Enable USB HID Emulation", + enable_usb_hid_emulation_cmd, 1, true, + status_rsp, 1, true }, + { 0x045, "Write UART Clock Setting" }, + { 0x04c, "Write RAM", + write_ram_cmd, 4, false, + status_rsp, 1, true }, + { 0x04e, "Launch RAM", + launch_ram_cmd, 4, true, + status_rsp, 1, true }, + { 0x05a, "Read VID PID", + null_cmd, 0, true, + read_vid_pid_rsp, 5, true }, + { 0x079, "Read Verbose Config Version Info", + null_cmd, 0, true, + read_verbose_version_info_rsp, 7, true }, { } }; @@ -123,3 +233,19 @@ void broadcom_lm_diag(const void *data, uint8_t size) break; } } + +static const struct vendor_evt vendor_evt_table[] = { + { } +}; + +const struct vendor_evt *broadcom_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/broadcom.h b/monitor/broadcom.h index f91f25b7d..ceda0e18b 100644 --- a/monitor/broadcom.h +++ b/monitor/broadcom.h @@ -25,6 +25,8 @@ #include struct vendor_ocf; +struct vendor_evt; const struct vendor_ocf *broadcom_vendor_ocf(uint16_t ocf); +const struct vendor_evt *broadcom_vendor_evt(uint8_t evt); void broadcom_lm_diag(const void *data, uint8_t size); diff --git a/monitor/intel.c b/monitor/intel.c index 6148be9ff..f5b42581b 100644 --- a/monitor/intel.c +++ b/monitor/intel.c @@ -36,10 +36,15 @@ #include "vendor.h" #include "intel.h" +static void null_cmd(const void *data, uint8_t size) +{ +} + static const struct vendor_ocf vendor_ocf_table[] = { { 0x001, "Reset" }, { 0x002, "No Operation" }, - { 0x005, "Read Version" }, + { 0x005, "Read Version", + null_cmd, 0, true }, { 0x006, "Set UART Baudrate" }, { 0x007, "Enable LPM" }, { 0x008, "PCM Write Configuration" }, @@ -87,7 +92,7 @@ const struct vendor_ocf *intel_vendor_ocf(uint16_t ocf) static void act_deact_traces_complete_evt(const void *data, uint8_t size) { - uint8_t status = *((const uint8_t *) data); + uint8_t status = get_u8(data); packet_print_error("Status", status); } @@ -99,8 +104,8 @@ static void lmp_pdu_trace_evt(const void *data, uint8_t size) uint32_t clock; const char *str; - type = *((uint8_t *) data); - handle = get_le16(data + 2); + type = get_u8(data); + handle = get_le16(data + 1); switch (type) { case 0x00: @@ -141,7 +146,7 @@ static void lmp_pdu_trace_evt(const void *data, uint8_t size) case 0x01: len = size - 9; clock = get_le32(data + 4 + len); - id = *((uint8_t *) (data + 4 + len + 4)); + id = get_u8(data + 4 + len + 4); packet_hexdump(data + 3, 1); lmp_packet(data + 4, len, false); @@ -150,7 +155,7 @@ static void lmp_pdu_trace_evt(const void *data, uint8_t size) break; case 0x02: clock = get_le32(data + 3); - id = *((uint8_t *) (data + 3 + 4)); + id = get_u8(data + 3 + 4); print_field("Clock: 0x%8.8x", clock); print_field("ID: 0x%2.2x", id); @@ -166,7 +171,7 @@ static void lmp_pdu_trace_evt(const void *data, uint8_t size) case 0x04: len = size - 8; count = get_le16(data + 3); - id = *((uint8_t *) (data + 3 + 2)); + id = get_u8(data + 3 + 2); print_field("Count: 0x%4.4x", count); print_field("ID: 0x%2.2x", id); @@ -175,7 +180,7 @@ static void lmp_pdu_trace_evt(const void *data, uint8_t size) break; case 0x05: count = get_le16(data + 3); - id = *((uint8_t *) (data + 3 + 2)); + id = get_u8(data + 3 + 2); print_field("Count: 0x%4.4x", count); print_field("ID: 0x%2.2x", id); diff --git a/monitor/packet.c b/monitor/packet.c index cfced1607..0dbc0e350 100644 --- a/monitor/packet.c +++ b/monitor/packet.c @@ -7307,6 +7307,8 @@ static const struct vendor_evt *current_vendor_evt(uint8_t evt) switch (manufacturer) { case 2: return intel_vendor_evt(evt); + case 15: + return broadcom_vendor_evt(evt); } return NULL; diff --git a/src/shared/util.h b/src/shared/util.h index 30b7d9230..65f5359a0 100644 --- a/src/shared/util.h +++ b/src/shared/util.h @@ -96,6 +96,11 @@ unsigned char util_get_dt(const char *parent, const char *name); uint8_t util_get_uid(unsigned int *bitmap, uint8_t max); void util_clear_uid(unsigned int *bitmap, uint8_t id); +static inline uint8_t get_u8(const void *ptr) +{ + return *((uint8_t *) ptr); +} + static inline uint16_t get_le16(const void *ptr) { return le16_to_cpu(get_unaligned((const uint16_t *) ptr)); -- 2.47.3