mirror of
https://git.kernel.org/pub/scm/bluetooth/bluez.git
synced 2024-11-17 09:14:32 +08:00
monitor: Add support for decoding link layer packets
This commit is contained in:
Marcel Holtmann
parent
5e3da6645b
commit
c631cd21da
@ -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
|
||||
|
||||
@ -24,6 +24,11 @@
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
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
|
||||
|
||||
376
monitor/ll.c
376
monitor/ll.c
@ -26,10 +26,384 @@
|
||||
#include <config.h>
|
||||
#endif
|
||||
|
||||
#include <bluetooth/bluetooth.h>
|
||||
|
||||
#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);
|
||||
}
|
||||
|
||||
@ -25,3 +25,4 @@
|
||||
#include <stdint.h>
|
||||
|
||||
void ll_packet(uint16_t frequency, const void *data, uint8_t size);
|
||||
void llcp_packet(const void *data, uint8_t size);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user