From: Marcel Holtmann Date: Sat, 24 Aug 2013 03:35:15 +0000 (-0700) Subject: monitor: Add support for decoding link layer packets X-Git-Tag: android-x86-4.4-r3~7735 X-Git-Url: http://git.osdn.net/view?a=commitdiff_plain;h=c631cd21da5cd170b38731a6dbc6d5b473d43f75;p=android-x86%2Fexternal-bluetooth-bluez.git monitor: Add support for decoding link layer packets --- diff --git a/Makefile.tools b/Makefile.tools index 816cdc01b..b2c272b1b 100644 --- a/Makefile.tools +++ b/Makefile.tools @@ -24,6 +24,7 @@ monitor_btmon_SOURCES = monitor/main.c monitor/bt.h \ monitor/l2cap.h monitor/l2cap.c \ monitor/uuid.h monitor/uuid.c \ monitor/sdp.h monitor/sdp.c \ + monitor/crc.h monitor/crc.c \ monitor/ll.h monitor/ll.c monitor_btmon_LDADD = lib/libbluetooth-internal.la endif diff --git a/monitor/bt.h b/monitor/bt.h index 269d76dc9..c2d7921f9 100644 --- a/monitor/bt.h +++ b/monitor/bt.h @@ -24,6 +24,11 @@ #include +struct bt_ll_hdr { + uint8_t preamble; + uint32_t access_addr; +} __attribute__ ((packed)); + #define BT_H4_CMD_PKT 0x01 #define BT_H4_ACL_PKT 0x02 #define BT_H4_SCO_PKT 0x03 diff --git a/monitor/ll.c b/monitor/ll.c index 95fed726c..41bba1aca 100644 --- a/monitor/ll.c +++ b/monitor/ll.c @@ -26,10 +26,384 @@ #include #endif +#include + +#include "display.h" #include "packet.h" +#include "crc.h" +#include "bt.h" #include "ll.h" +#define COLOR_OPCODE COLOR_MAGENTA +#define COLOR_OPCODE_UNKNOWN COLOR_WHITE_BG + +#define MAX_CHANNEL 16 + +struct channel_data { + uint32_t access_addr; + uint32_t crc_init; +}; + +static struct channel_data channel_list[MAX_CHANNEL]; + +static void set_crc_init(uint32_t access_addr, uint32_t crc_init) +{ + int i; + + for (i = 0; i < MAX_CHANNEL; i++) { + if (channel_list[i].access_addr == 0x00000000) { + channel_list[i].access_addr = access_addr; + channel_list[i].crc_init = crc_init; + break; + } + } +} + +static uint32_t get_crc_init(uint32_t access_addr) +{ + int i; + + for (i = 0; i < MAX_CHANNEL; i++) { + if (channel_list[i].access_addr == access_addr) + return channel_list[i].crc_init; + } + + return 0x00000000; +} + +static void advertising_packet(const void *data, uint8_t size) +{ + const uint8_t *ptr = data; + uint8_t pdu_type, length, win_size; + bool tx_add, rx_add; + uint32_t access_addr, crc_init; + uint16_t win_offset, interval, latency, timeout; + const char *str; + + if (size < 2) { + print_text(COLOR_ERROR, "packet too short"); + packet_hexdump(data, size); + return; + } + + pdu_type = ptr[0] & 0x0f; + tx_add = !!(ptr[0] & 0x40); + rx_add = !!(ptr[0] & 0x80); + length = ptr[1] & 0x3f; + + switch (pdu_type) { + case 0x00: + str = "ADV_IND"; + break; + case 0x01: + str = "ADV_DIRECT_IND"; + break; + case 0x02: + str = "ADV_NONCONN_IND"; + break; + case 0x03: + str = "SCAN_REQ"; + break; + case 0x04: + str = "SCAN_RSP"; + break; + case 0x05: + str = "CONNECT_REQ"; + break; + case 0x06: + str = "ADV_SCAN_IND"; + break; + default: + str = "Reserved"; + break; + } + + print_field("Type: %s (0x%2.2x)", str, pdu_type); + print_field("TxAdd: %u", tx_add); + print_field("RxAdd: %u", rx_add); + print_field("Length: %u", length); + + if (length != size - 2) { + print_text(COLOR_ERROR, "packet size mismatch"); + packet_hexdump(data + 2, size - 2); + return; + } + + switch (pdu_type) { + case 0x00: /* ADV_IND */ + case 0x02: /* AVD_NONCONN_IND */ + case 0x06: /* ADV_SCAN_IND */ + case 0x04: /* SCAN_RSP */ + if (length < 6) { + print_text(COLOR_ERROR, "payload too short"); + packet_hexdump(data + 2, length); + return; + } + + packet_print_addr("Advertiser address", data + 2, tx_add); + packet_print_ad(data + 8, length - 6); + break; + + case 0x01: /* ADV_DIRECT_IND */ + if (length < 12) { + print_text(COLOR_ERROR, "payload too short"); + packet_hexdump(data + 2, length); + return; + } + + packet_print_addr("Advertiser address", data + 2, tx_add); + packet_print_addr("Inititator address", data + 8, rx_add); + break; + + case 0x03: /* SCAN_REQ */ + if (length < 12) { + print_text(COLOR_ERROR, "payload too short"); + packet_hexdump(data + 2, length); + return; + } + + packet_print_addr("Scanner address", data + 2, tx_add); + packet_print_addr("Advertiser address", data + 8, rx_add); + break; + + case 0x05: /* CONNECT_REQ */ + if (length < 34) { + print_text(COLOR_ERROR, "payload too short"); + packet_hexdump(data + 2, length); + return; + } + + packet_print_addr("Inititator address", data + 2, tx_add); + packet_print_addr("Advertiser address", data + 8, rx_add); + + access_addr = ptr[14] | ptr[15] << 8 | + ptr[16] << 16 | ptr[17] << 24; + crc_init = ptr[18] | ptr[19] << 8 | ptr[20] << 16; + + print_field("Access address: 0x%8.8x", access_addr); + print_field("CRC init: 0x%6.6x", crc_init); + + set_crc_init(access_addr, crc24_bit_reverse(crc_init)); + + win_size = ptr[21]; + win_offset = ptr[22] | ptr[23] << 8; + interval = ptr[24] | ptr[25] << 8; + latency = ptr[26] | ptr[27] << 8; + timeout = ptr[28] | ptr[29] << 8; + + print_field("Transmit window size: %u", win_size); + print_field("Transmit window offset: %u", win_offset); + print_field("Connection interval: %u", interval); + print_field("Connection slave latency: %u", latency); + print_field("Connection supervision timeout: %u", timeout); + + packet_hexdump(data + 30, length - 28); + break; + + default: + packet_hexdump(data + 2, length); + break; + } +} + +static void data_packet(const void *data, uint8_t size) +{ + const uint8_t *ptr = data; + uint8_t llid, length; + bool nesn, sn, md; + const char *str; + + if (size < 2) { + print_text(COLOR_ERROR, "packet too short"); + packet_hexdump(data, size); + return; + } + + llid = ptr[0] & 0x03; + nesn = !!(ptr[0] & 0x04); + sn = !!(ptr[0] & 0x08); + md = !!(ptr[0] & 0x10); + length = ptr[1] & 0x1f; + + switch (llid) { + case 0x01: + if (length > 0) + str = "Continuation fragement of L2CAP message"; + else + str = "Empty message"; + break; + case 0x02: + str = "Start of L2CAP message"; + break; + case 0x03: + str = "Control"; + break; + default: + str = "Reserved"; + break; + } + + print_field("LLID: %s (0x%2.2x)", str, llid); + print_field("Next expected sequence number: %u", nesn); + print_field("Sequence number: %u", sn); + print_field("More data: %u", md); + print_field("Length: %u", length); + + switch (llid) { + case 0x03: + llcp_packet(data + 2, size - 2); + break; + + default: + packet_hexdump(data + 2, size - 2); + break; + } +} + void ll_packet(uint16_t frequency, const void *data, uint8_t size) { - packet_hexdump(data, size); + const struct bt_ll_hdr *hdr = data; + uint8_t channel = (frequency - 2402) / 2; + uint32_t access_addr; + char access_str[12]; + const char *channel_label, *channel_color; + const uint8_t *pdu_data; + uint8_t pdu_len; + uint32_t pdu_crc, crc, crc_init; + + if (size < sizeof(*hdr)) { + print_text(COLOR_ERROR, "packet missing header"); + packet_hexdump(data, size); + return; + } + + if (size < sizeof(*hdr) + 3) { + print_text(COLOR_ERROR, "packet missing checksum"); + packet_hexdump(data, size); + return; + } + + if (hdr->preamble != 0xaa && hdr->preamble != 0x55) { + print_text(COLOR_ERROR, "invalid preamble"); + packet_hexdump(data, size); + return; + } + + access_addr = btohl(hdr->access_addr); + + pdu_data = data + sizeof(*hdr); + pdu_len = size - sizeof(*hdr) - 3; + + pdu_crc = pdu_data[pdu_len + 0] | (pdu_data[pdu_len + 1] << 8) | + (pdu_data[pdu_len + 2] << 16); + + if (access_addr == 0x8e89bed6) { + channel_label = "Advertising channel: "; + channel_color = COLOR_MAGENTA; + } else { + channel_label = "Data channel: "; + channel_color = COLOR_CYAN; + } + + sprintf(access_str, "0x%8.8x", access_addr); + + print_indent(6, channel_color, channel_label, access_str, COLOR_OFF, + " (channel %d) len %d crc 0x%6.6x", channel, pdu_len, pdu_crc); + + if (access_addr == 0x8e89bed6) + crc_init = 0xaaaaaa; + else + crc_init = get_crc_init(access_addr); + + if (crc_init) { + crc = crc24_calculate(crc_init, pdu_data, pdu_len); + + if (crc != pdu_crc) { + print_text(COLOR_ERROR, "invalid checksum"); + packet_hexdump(pdu_data, pdu_len); + return; + } + } else + print_text(COLOR_ERROR, "unknown access address"); + + if (access_addr == 0x8e89bed6) + advertising_packet(pdu_data, pdu_len); + else + data_packet(pdu_data, pdu_len); +} + +struct llcp_data { + uint8_t opcode; + const char *str; + void (*func) (const void *data, uint8_t size); + uint8_t size; + bool fixed; +}; + +static const struct llcp_data llcp_table[] = { + { 0x00, "LL_CONNECTION_UPDATE_REQ" }, + { 0x01, "LL_CHANNEL_MAP_REQ" }, + { 0x02, "LL_TERMINATE_IND" }, + { 0x03, "LL_ENC_REQ" }, + { 0x04, "LL_ENC_RSP" }, + { 0x05, "LL_START_ENC_REQ" }, + { 0x06, "LL_START_ENC_RSP" }, + { 0x07, "LL_UNKNOWN_RSP" }, + { 0x08, "LL_FEATURE_REQ" }, + { 0x09, "LL_FEATURE_RSP" }, + { 0x0a, "LL_PAUSE_ENC_REQ" }, + { 0x0b, "LL_PAUSE_ENC_RSP" }, + { 0x0c, "LL_VERSION_IND" }, + { 0x0d, "LL_REJECT_IND" }, + { } +}; + +void llcp_packet(const void *data, uint8_t size) +{ + uint8_t opcode = ((const uint8_t *) data)[0]; + const struct llcp_data *llcp_data = NULL; + const char *opcode_color, *opcode_str; + int i; + + for (i = 0; llcp_table[i].str; i++) { + if (llcp_table[i].opcode == opcode) { + llcp_data = &llcp_table[i]; + break; + } + } + + if (llcp_data) { + if (llcp_data->func) + opcode_color = COLOR_OPCODE; + else + opcode_color = COLOR_OPCODE_UNKNOWN; + opcode_str = llcp_data->str; + } else { + opcode_color = COLOR_OPCODE_UNKNOWN; + opcode_str = "Unknown"; + } + + print_indent(6, opcode_color, "", opcode_str, COLOR_OFF, + " (0x%2.2x)", opcode); + + if (!llcp_data || !llcp_data->func) { + packet_hexdump(data + 1, size - 1); + return; + } + + if (llcp_data->fixed) { + if (size - 1 != llcp_data->size) { + print_text(COLOR_ERROR, "invalid packet size"); + packet_hexdump(data + 1, size - 1); + return; + } + } else { + if (size - 1 < llcp_data->size) { + print_text(COLOR_ERROR, "too short packet"); + packet_hexdump(data + 1, size - 1); + return; + } + } + + llcp_data->func(data + 1, size - 1); } diff --git a/monitor/ll.h b/monitor/ll.h index 496bc22f6..17d934ba0 100644 --- a/monitor/ll.h +++ b/monitor/ll.h @@ -25,3 +25,4 @@ #include void ll_packet(uint16_t frequency, const void *data, uint8_t size); +void llcp_packet(const void *data, uint8_t size);