static void bnep_eval_extension(int level, struct frame *frm)
{
uint8_t type = p_get_u8(frm);
uint8_t length = p_get_u8(frm);
int extension = type & 0x80;
p_indent(level, frm);
switch (type & 0x7f) {
case BNEP_EXTENSION_CONTROL:
printf("Ext Control(0x%02x|%s) len 0x%02x\n",
type & 0x7f, extension ? "1" : "0", length);
bnep_control(level, frm, length);
break;
default:
printf("Ext Unknown(0x%02x|%s) len 0x%02x\n",
type & 0x7f, extension ? "1" : "0", length);
raw_ndump(level + 1, frm, length);
frm->ptr += length;
frm->len -= length;
}
if (extension)
bnep_eval_extension(level, frm);
}
void cmtp_dump(int level, struct frame *frm)
{
struct frame *msg;
uint8_t hdr, bid;
uint16_t len;
while (frm->len > 0) {
hdr = p_get_u8(frm);
bid = (hdr & 0x3c) >> 2;
switch ((hdr & 0xc0) >> 6) {
case 0x01:
len = p_get_u8(frm);
break;
case 0x02:
len = htons(p_get_u16(frm));
break;
default:
len = 0;
break;
}
p_indent(level, frm);
printf("CMTP: %s: id %d len %d\n", bst2str(hdr & 0x03), bid, len);
switch (hdr & 0x03) {
case 0x00:
add_segment(bid, frm, len);
msg = get_segment(bid, frm);
if (!msg)
break;
if (!p_filter(FILT_CAPI))
capi_dump(level + 1, msg);
else
raw_dump(level, msg);
free_segment(bid, frm);
break;
case 0x01:
add_segment(bid, frm, len);
break;
default:
free_segment(bid, frm);
break;
}
frm->ptr += len;
frm->len -= len;
}
}
static void att_error_dump(int level, struct frame *frm)
{
uint8_t op = p_get_u8(frm);
uint16_t handle = btohs(htons(p_get_u16(frm)));
uint8_t err = p_get_u8(frm);
p_indent(level, frm);
printf("Error: %s (%d)\n", atterror2str(err), err);
p_indent(level, frm);
printf("%s (0x%.2x) on handle 0x%4.4x\n", attop2str(op), op, handle);
}
void hidp_dump(int level, struct frame *frm)
{
uint8_t hdr;
char *param;
hdr = p_get_u8(frm);
switch (hdr & 0xf0) {
case 0x00:
param = result2str(hdr);
break;
case 0x10:
param = operation2str(hdr);
break;
case 0x60:
case 0x70:
param = protocol2str(hdr);
break;
case 0x40:
case 0x50:
case 0xa0:
case 0xb0:
param = report2str(hdr);
break;
default:
param = "";
break;
}
p_indent(level, frm);
printf("HIDP: %s: %s\n", type2str(hdr), param);
raw_dump(level, frm);
}
static void att_find_info_resp_dump(int level, struct frame *frm)
{
uint8_t fmt = p_get_u8(frm);
p_indent(level, frm);
if (fmt == 0x01) {
printf("format: uuid-16\n");
while (frm->len > 0) {
uint16_t handle = btohs(htons(p_get_u16(frm)));
uint16_t uuid = btohs(htons(p_get_u16(frm)));
p_indent(level + 1, frm);
printf("handle 0x%4.4x, uuid 0x%4.4x (%s)\n", handle, uuid,
uuid2str(uuid));
}
} else {
printf("format: uuid-128\n");
while (frm->len > 0) {
uint16_t handle = btohs(htons(p_get_u16(frm)));
p_indent(level + 1, frm);
printf("handle 0x%4.4x, uuid ", handle);
print_uuid128(frm);
printf("\n");
}
}
}
static void smp_cmd_security_req_dump(int level, struct frame *frm)
{
uint8_t auth = p_get_u8(frm);
p_indent(level, frm);
printf("auth req 0x%2.2x\n", auth);
}
static void att_signed_write_dump(int level, struct frame *frm)
{
uint16_t handle = btohs(htons(p_get_u16(frm)));
int value_len = frm->len - 12; /* handle:2 already accounted, sig: 12 */
p_indent(level, frm);
printf("handle 0x%4.4x value ", handle);
while (value_len--)
printf(" 0x%2.2x", p_get_u8(frm));
printf("\n");
p_indent(level, frm);
printf("auth signature ");
while (frm->len > 0)
printf(" 0x%2.2x", p_get_u8(frm));
printf("\n");
}
static void att_read_multi_resp_dump(int level, struct frame *frm)
{
p_indent(level, frm);
printf("values");
while (frm->len > 0)
printf(" 0x%2.2x", p_get_u8(frm));
printf("\n");
}
static void smp_cmd_ident_addr_info_dump(int level, struct frame *frm)
{
uint8_t type = p_get_u8(frm);
char addr[18];
p_indent(level, frm);
p_ba2str((bdaddr_t *) frm, addr);
printf("bdaddr %s (%s)\n", addr, type == 0x00 ? "Public" : "Random");
}
static void smp_cmd_pairing_failed_dump(int level, struct frame *frm)
{
uint8_t reason = p_get_u8(frm);
p_indent(level, frm);
printf("reason 0x%2.2x\n", reason);
p_indent(level, frm);
printf("Reason %s\n", smpreason2str(reason));
}
static void smp_cmd_sign_info_dump(int level, struct frame *frm)
{
int i;
p_indent(level, frm);
printf("CSRK ");
for (i = 0; i < 16; i++)
printf("%2.2x", p_get_u8(frm));
printf("\n");
}
static void smp_cmd_pairing_random_dump(int level, struct frame *frm)
{
int i;
p_indent(level, frm);
printf("random ");
for (i = 0; i < 16; i++)
printf("%2.2x", p_get_u8(frm));
printf("\n");
}
static void att_write_req_dump(int level, struct frame *frm)
{
uint16_t handle = btohs(htons(p_get_u16(frm)));
p_indent(level, frm);
printf("handle 0x%4.4x value ", handle);
while (frm->len > 0)
printf(" 0x%2.2x", p_get_u8(frm));
printf("\n");
}
static void att_read_by_type_resp_dump(int level, struct frame *frm)
{
uint8_t length = p_get_u8(frm);
p_indent(level, frm);
printf("length: %d\n", length);
while (frm->len > 0) {
uint16_t handle = btohs(htons(p_get_u16(frm)));
int val_len = length - 2;
int i;
p_indent(level + 1, frm);
printf("handle 0x%4.4x, value ", handle);
for (i = 0; i < val_len; i++) {
printf("0x%.2x ", p_get_u8(frm));
}
printf("\n");
}
}
void bpa_dump(int level, struct frame *frm)
{
uint8_t id, status, channel;
uint16_t num, len;
uint32_t time;
id = p_get_u8(frm);
num = p_get_u16(frm);
len = BPA_U16(frm);
status = p_get_u8(frm);
time = p_get_u32(frm);
channel = p_get_u8(frm);
p_indent(level, frm);
printf("BPA: id %d num %d len %u status 0x%02x time %d channel %d\n",
id, num, len, status, time, channel);
raw_dump(level, frm);
}
static void att_exec_write_req_dump(int level, struct frame *frm)
{
uint8_t flags = p_get_u8(frm);
p_indent(level, frm);
if (flags == 0x00)
printf("cancel all prepared writes ");
else
printf("immediatelly write all pending prepared values ");
printf("(0x%2.2x)\n", flags);
}
static void att_read_by_group_resp_dump(int level, struct frame *frm)
{
uint8_t length = p_get_u8(frm);
while (frm->len > 0) {
uint16_t attr_handle = btohs(htons(p_get_u16(frm)));
uint16_t end_grp_handle = btohs(htons(p_get_u16(frm)));
uint8_t remaining = length - 4;
p_indent(level, frm);
printf("attr handle 0x%4.4x, end group handle 0x%4.4x\n",
attr_handle, end_grp_handle);
p_indent(level, frm);
printf("value");
while (remaining > 0) {
printf(" 0x%2.2x", p_get_u8(frm));
remaining--;
}
printf("\n");
}
}
static void smp_cmd_master_ident_dump(int level, struct frame *frm)
{
uint16_t ediv = btohs(htons(p_get_u16(frm)));
int i;
p_indent(level, frm);
printf("EDIV 0x%4.4x ", ediv);
printf("Rand 0x");
for (i = 0; i < 8; i++)
printf("%2.2x", p_get_u8(frm));
printf("\n");
}
static void print_uuid128(struct frame *frm)
{
uint8_t uuid[16];
int i;
for (i = 0; i < 16; i++)
uuid[15 - i] = p_get_u8(frm);
for (i = 0; i < 16; i++) {
printf("%02x", uuid[i]);
if (i == 3 || i == 5 || i == 7 || i == 9)
printf("-");
}
}
static void att_find_by_type_req_dump(int level, struct frame *frm)
{
uint16_t start = btohs(htons(p_get_u16(frm)));
uint16_t end = btohs(htons(p_get_u16(frm)));
uint16_t uuid = btohs(htons(p_get_u16(frm)));
p_indent(level, frm);
printf("start 0x%4.4x, end 0x%4.4x, uuid 0x%4.4x\n", start, end, uuid);
p_indent(level, frm);
printf("value");
while (frm->len > 0)
printf(" 0x%2.2x", p_get_u8(frm));
printf("\n");
}
static void att_prep_write_dump(int level, struct frame *frm)
{
uint16_t handle = btohs(htons(p_get_u16(frm)));
uint16_t val_offset = btohs(htons(p_get_u16(frm)));
p_indent(level, frm);
printf("attr handle 0x%4.4x, value offset 0x%4.4x\n", handle,
val_offset);
p_indent(level, frm);
printf("part attr value ");
while (frm->len > 0)
printf(" 0x%2.2x", p_get_u8(frm));
printf("\n");
}
void smp_dump(int level, struct frame *frm)
{
uint8_t cmd;
cmd = p_get_u8(frm);
p_indent(level, frm);
printf("SMP: %s (0x%.2x)\n", smpcmd2str(cmd), cmd);
switch (cmd) {
case SMP_CMD_PAIRING_REQ:
smp_cmd_pairing_dump(level + 1, frm);
break;
case SMP_CMD_PAIRING_RESP:
smp_cmd_pairing_dump(level + 1, frm);
break;
case SMP_CMD_PAIRING_CONFIRM:
smp_cmd_pairing_confirm_dump(level + 1, frm);
break;
case SMP_CMD_PAIRING_RANDOM:
smp_cmd_pairing_random_dump(level + 1, frm);
break;
case SMP_CMD_PAIRING_FAILED:
smp_cmd_pairing_failed_dump(level + 1, frm);
break;
case SMP_CMD_ENCRYPT_INFO:
smp_cmd_encrypt_info_dump(level + 1, frm);
break;
case SMP_CMD_MASTER_IDENT:
smp_cmd_master_ident_dump(level + 1, frm);
break;
case SMP_CMD_IDENT_INFO:
smp_cmd_ident_info_dump(level + 1, frm);
break;
case SMP_CMD_IDENT_ADDR_INFO:
smp_cmd_ident_addr_info_dump(level + 1, frm);
break;
case SMP_CMD_SIGN_INFO:
smp_cmd_sign_info_dump(level + 1, frm);
break;
case SMP_CMD_SECURITY_REQ:
smp_cmd_security_req_dump(level + 1, frm);
break;
default:
raw_dump(level, frm);
}
}
static void smp_cmd_pairing_dump(int level, struct frame *frm)
{
uint8_t cap = p_get_u8(frm);
uint8_t oob = p_get_u8(frm);
uint8_t auth = p_get_u8(frm);
uint8_t key_size = p_get_u8(frm);
uint8_t int_dist = p_get_u8(frm);
uint8_t resp_dist = p_get_u8(frm);
p_indent(level, frm);
printf("capability 0x%2.2x oob 0x%2.2x auth req 0x%2.2x\n", cap, oob,
auth);
p_indent(level , frm);
printf("max key size 0x%2.2x init key dist 0x%2.2x "
"resp key dist 0x%2.2x\n", key_size, int_dist, resp_dist);
p_indent(level , frm);
printf("Capability: %s (OOB data %s)\n", smpio2str(cap),
oob == 0x00 ? "not present" : "available");
p_indent(level , frm);
printf("Authentication: %s (%s)\n",
auth & SMP_AUTH_BONDING ? "Bonding" : "No Bonding",
auth & SMP_AUTH_MITM ? "MITM Protection" :
"No MITM Protection");
p_indent(level , frm);
printf("Initiator Key Distribution: %s %s %s\n",
int_dist & SMP_DIST_ENC_KEY ? "LTK" : "",
int_dist & SMP_DIST_ID_KEY ? "IRK" : "",
int_dist & SMP_DIST_SIGN ? "CSRK" : "");
p_indent(level , frm);
printf("Responder Key Distribution: %s %s %s\n",
resp_dist & SMP_DIST_ENC_KEY ? "LTK" : "",
resp_dist & SMP_DIST_ID_KEY ? "IRK" : "",
resp_dist & SMP_DIST_SIGN ? "CSRK" : "");
}
void bnep_dump(int level, struct frame *frm)
{
uint8_t type = p_get_u8(frm);
uint16_t proto = 0x0000;
int extension = type & 0x80;
p_indent(level, frm);
switch (type & 0x7f) {
case BNEP_CONTROL:
printf("BNEP: Control(0x%02x|%s)\n",
type & 0x7f, extension ? "1" : "0");
bnep_control(level, frm, -1);
break;
case BNEP_COMPRESSED_ETHERNET:
printf("BNEP: Compressed(0x%02x|%s)\n",
type & 0x7f, extension ? "1" : "0");
p_indent(++level, frm);
proto = p_get_u16(frm);
printf("[proto 0x%04x]\n", proto);
break;
case BNEP_GENERAL_ETHERNET:
printf("BNEP: General ethernet(0x%02x|%s)\n",
type & 0x7f, extension ? "1" : "0");
p_indent(++level, frm);
printf("dst %s ", get_macaddr(frm));
printf("src %s ", get_macaddr(frm));
proto = p_get_u16(frm);
printf("[proto 0x%04x]\n", proto);
break;
case BNEP_COMPRESSED_ETHERNET_DEST_ONLY:
printf("BNEP: Compressed DestOnly(0x%02x|%s)\n",
type & 0x7f, extension ? "1" : "0");
p_indent(++level, frm);
printf("dst %s ", get_macaddr(frm));
proto = p_get_u16(frm);
printf("[proto 0x%04x]\n", proto);
break;
case BNEP_COMPRESSED_ETHERNET_SOURCE_ONLY:
printf("BNEP: Compressed SrcOnly(0x%02x|%s)\n",
type & 0x7f, extension ? "1" : "0");
p_indent(++level, frm);
printf("src %s ", get_macaddr(frm));
proto = p_get_u16(frm);
printf("[proto 0x%04x]\n", proto);
break;
default:
printf("(Unknown packet type)\n");
return;
}
/* Extension info */
if (extension)
bnep_eval_extension(++level, frm);
/* Control packet => No payload info */
if ((type & 0x7f) == BNEP_CONTROL)
return;
/* 802.1p header */
if (proto == 0x8100) {
p_indent(level, frm);
printf("802.1p Header: 0x%04x ", p_get_u16(frm));
proto = p_get_u16(frm);
printf("[proto 0x%04x]\n", proto);
}
if (!(parser.flags & DUMP_VERBOSE)) {
raw_dump(level, frm);
return;
}
switch (proto) {
case ETHERTYPE_ARP:
p_indent(++level, frm);
printf("ARP: ");
arp_dump(level, frm);
break;
case ETHERTYPE_REVARP:
p_indent(++level, frm);
printf("RARP: ");
arp_dump(level, frm);
break;
case ETHERTYPE_IP:
p_indent(++level, frm);
printf("IP: ");
ip_dump(level, frm);
break;
case ETHERTYPE_IPV6:
p_indent(++level, frm);
printf("IPV6: ");
ip_dump(level, frm);
break;
default:
raw_dump(level, frm);
break;
}
}
static void bnep_control(int level, struct frame *frm, int header_length)
{
uint8_t uuid_size;
int i, length;
char *s;
uint32_t uuid = 0;
uint8_t type = p_get_u8(frm);
p_indent(++level, frm);
switch (type) {
case BNEP_CONTROL_COMMAND_NOT_UNDERSTOOD:
printf("Not Understood(0x%02x) type 0x%02x\n", type, p_get_u8(frm));
break;
case BNEP_SETUP_CONNECTION_REQUEST_MSG:
uuid_size = p_get_u8(frm);
printf("Setup Req(0x%02x) size 0x%02x ", type, uuid_size);
switch (uuid_size) {
case 2:
uuid = p_get_u16(frm);
printf("dst 0x%x", uuid);
if ((s = get_uuid_name(uuid)) != 0)
printf("(%s)", s);
uuid = p_get_u16(frm);
printf(" src 0x%x", uuid);
if ((s = get_uuid_name(uuid)) != 0)
printf("(%s)", s);
printf("\n");
break;
case 4:
uuid = p_get_u32(frm);
printf("dst 0x%x", uuid);
if ((s = get_uuid_name(uuid)) != 0)
printf("(%s)", s);
uuid = p_get_u32(frm);
printf(" src 0x%x", uuid);
if ((s = get_uuid_name(uuid)) != 0)
printf("(%s)", s);
printf("\n");
break;
case 16:
uuid = p_get_u32(frm);
printf("dst 0x%x", uuid);
if ((s = get_uuid_name(uuid)) != 0)
printf("(%s)", s);
frm->ptr += 12;
frm->len -= 12;
uuid = p_get_u32(frm);
printf(" src 0x%x", uuid);
if ((s = get_uuid_name(uuid)) != 0)
printf("(%s)", s);
printf("\n");
frm->ptr += 12;
frm->len -= 12;
break;
default:
frm->ptr += (uuid_size * 2);
frm->len -= (uuid_size * 2);
break;
}
break;
case BNEP_SETUP_CONNECTION_RESPONSE_MSG:
printf("Setup Rsp(0x%02x) res 0x%04x\n",
type, p_get_u16(frm));
break;
case BNEP_FILTER_NET_TYPE_SET_MSG:
length = p_get_u16(frm);
printf("Filter NetType Set(0x%02x) len 0x%04x\n",
type, length);
for (i = 0; i < length / 4; i++) {
p_indent(level + 1, frm);
printf("0x%04x - ", p_get_u16(frm));
printf("0x%04x\n", p_get_u16(frm));
}
break;
case BNEP_FILTER_NET_TYPE_RESPONSE_MSG:
printf("Filter NetType Rsp(0x%02x) res 0x%04x\n",
type, p_get_u16(frm));
break;
case BNEP_FILTER_MULT_ADDR_SET_MSG:
length = p_get_u16(frm);
printf("Filter MultAddr Set(0x%02x) len 0x%04x\n",
type, length);
for (i = 0; i < length / 12; i++) {
p_indent(level + 1, frm);
printf("%s - ", get_macaddr(frm));
printf("%s\n", get_macaddr(frm));
}
break;
case BNEP_FILTER_MULT_ADDR_RESPONSE_MSG:
printf("Filter MultAddr Rsp(0x%02x) res 0x%04x\n",
type, p_get_u16(frm));
break;
default:
printf("Unknown control type(0x%02x)\n", type);
raw_ndump(level + 1, frm, header_length - 1);
frm->ptr += header_length - 1;
frm->len -= header_length - 1;
return;
}
}
void att_dump(int level, struct frame *frm)
{
uint8_t op;
op = p_get_u8(frm);
p_indent(level, frm);
printf("ATT: %s (0x%.2x)\n", attop2str(op), op);
switch (op) {
case ATT_OP_ERROR:
att_error_dump(level + 1, frm);
break;
case ATT_OP_MTU_REQ:
att_mtu_req_dump(level + 1, frm);
break;
case ATT_OP_MTU_RESP:
att_mtu_resp_dump(level + 1, frm);
break;
case ATT_OP_FIND_INFO_REQ:
att_find_info_req_dump(level + 1, frm);
break;
case ATT_OP_FIND_INFO_RESP:
att_find_info_resp_dump(level + 1, frm);
break;
case ATT_OP_FIND_BY_TYPE_REQ:
att_find_by_type_req_dump(level + 1, frm);
break;
case ATT_OP_FIND_BY_TYPE_RESP:
att_find_by_type_resp_dump(level + 1, frm);
break;
case ATT_OP_READ_BY_TYPE_REQ:
case ATT_OP_READ_BY_GROUP_REQ: /* exact same parsing */
att_read_by_type_req_dump(level + 1, frm);
break;
case ATT_OP_READ_BY_TYPE_RESP:
att_read_by_type_resp_dump(level + 1, frm);
break;
case ATT_OP_READ_REQ:
att_read_req_dump(level + 1, frm);
break;
case ATT_OP_READ_RESP:
raw_dump(level + 1, frm);
break;
case ATT_OP_READ_BLOB_REQ:
att_read_blob_req_dump(level + 1, frm);
break;
case ATT_OP_READ_BLOB_RESP:
att_read_blob_resp_dump(level + 1, frm);
break;
case ATT_OP_READ_MULTI_REQ:
att_read_multi_req_dump(level + 1, frm);
break;
case ATT_OP_READ_MULTI_RESP:
att_read_multi_resp_dump(level + 1, frm);
break;
case ATT_OP_READ_BY_GROUP_RESP:
att_read_by_group_resp_dump(level + 1, frm);
break;
case ATT_OP_WRITE_REQ:
case ATT_OP_WRITE_CMD:
att_write_req_dump(level + 1, frm);
break;
case ATT_OP_SIGNED_WRITE_CMD:
att_signed_write_dump(level + 1, frm);
break;
case ATT_OP_PREP_WRITE_REQ:
case ATT_OP_PREP_WRITE_RESP:
att_prep_write_dump(level + 1, frm);
break;
case ATT_OP_EXEC_WRITE_REQ:
att_exec_write_req_dump(level + 1, frm);
break;
case ATT_OP_HANDLE_NOTIFY:
att_handle_notify_dump(level + 1, frm);
break;
default:
raw_dump(level, frm);
break;
}
}
