diff --git a/monitor/intel.c b/monitor/intel.c
index 10a4a1b..6148be9 100644
--- a/monitor/intel.c
+++ b/monitor/intel.c
#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 828d855..573b23f 100644
--- a/monitor/intel.h
+++ b/monitor/intel.h
#include <stdint.h>
-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 a353193..3f2ad8d 100644
--- a/monitor/packet.c
+++ b/monitor/packet.c
manufacturer = UNKNOWN_MANUFACTURER;
switch (manufacturer) {
+ case 2:
+ return intel_vendor_ocf(ocf);
case 15:
return broadcom_vendor_ocf(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;
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;
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",
{
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;
}
}
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;
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 be7baab..f5792b3 100644
--- a/monitor/vendor.h
+++ b/monitor/vendor.h
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);