void avctp_packet(const struct l2cap_frame *frame)
{
struct l2cap_frame *l2cap_frame;
struct avctp_frame avctp_frame;
const char *pdu_color;
l2cap_frame_pull(&avctp_frame.l2cap_frame, frame, 0);
l2cap_frame = &avctp_frame.l2cap_frame;
if (!l2cap_frame_get_u8(l2cap_frame, &avctp_frame.hdr) ||
!l2cap_frame_get_be16(l2cap_frame, &avctp_frame.pid)) {
print_text(COLOR_ERROR, "frame too short");
packet_hexdump(frame->data, frame->size);
return;
}
if (frame->in)
pdu_color = COLOR_MAGENTA;
else
pdu_color = COLOR_BLUE;
print_indent(6, pdu_color, "AVCTP", "", COLOR_OFF,
" %s: %s: type 0x%02x label %d PID 0x%04x",
frame->psm == 23 ? "Control" : "Browsing",
avctp_frame.hdr & 0x02 ? "Response" : "Command",
avctp_frame.hdr & 0x0c, avctp_frame.hdr >> 4,
avctp_frame.pid);
if (avctp_frame.pid == 0x110e || avctp_frame.pid == 0x110c)
avrcp_packet(&avctp_frame);
else
packet_hexdump(frame->data, frame->size);
}
static void attr_req(const struct l2cap_frame *frame, struct tid_data *tid)
{
uint32_t attr_bytes;
if (frame->size < 6) {
print_text(COLOR_ERROR, "invalid size");
packet_hexdump(frame->data, frame->size);
return;
}
print_field("Record handle: 0x%4.4x", get_be32(frame->data));
print_field("Max attribute bytes: %d", get_be16(frame->data + 4));
attr_bytes = get_bytes(frame->data + 6, frame->size - 6);
print_field("Attribute list: [len %d]", attr_bytes);
if (attr_bytes + 6 > frame->size) {
print_text(COLOR_ERROR, "invalid attribute list length");
packet_hexdump(frame->data, frame->size);
return;
}
decode_data_elements(0, 2, frame->data + 6, attr_bytes, NULL);
store_continuation(tid, frame->data + 6 + attr_bytes,
frame->size - 6 - attr_bytes);
}
void sdp_packet(const struct l2cap_frame *frame)
{
uint8_t pdu;
uint16_t tid, plen;
struct l2cap_frame sdp_frame;
struct tid_data *tid_info;
const struct sdp_data *sdp_data = NULL;
const char *pdu_color, *pdu_str;
int i;
l2cap_frame_pull(&sdp_frame, frame, 0);
if (!l2cap_frame_get_u8(&sdp_frame, &pdu) ||
!l2cap_frame_get_be16(&sdp_frame, &tid) ||
!l2cap_frame_get_be16(&sdp_frame, &plen)) {
print_text(COLOR_ERROR, "frame too short");
packet_hexdump(frame->data, frame->size);
return;
}
if (sdp_frame.size != plen) {
print_text(COLOR_ERROR, "invalid frame size");
packet_hexdump(sdp_frame.data, sdp_frame.size);
return;
}
for (i = 0; sdp_table[i].str; i++) {
if (sdp_table[i].pdu == pdu) {
sdp_data = &sdp_table[i];
break;
}
}
if (sdp_data) {
if (sdp_data->func) {
if (frame->in)
pdu_color = COLOR_MAGENTA;
else
pdu_color = COLOR_BLUE;
} else
pdu_color = COLOR_WHITE_BG;
pdu_str = sdp_data->str;
} else {
pdu_color = COLOR_WHITE_BG;
pdu_str = "Unknown";
}
print_indent(6, pdu_color, "SDP: ", pdu_str, COLOR_OFF,
" (0x%2.2x) tid %d len %d", pdu, tid, plen);
tid_info = get_tid(tid, frame->chan);
if (!sdp_data || !sdp_data->func || !tid_info) {
packet_hexdump(sdp_frame.data, sdp_frame.size);
return;
}
sdp_data->func(&sdp_frame, tid_info);
}
static void print_continuation(const uint8_t *data, uint16_t size)
{
if (data[0] != size - 1) {
print_text(COLOR_ERROR, "invalid continuation state");
packet_hexdump(data, size);
return;
}
print_field("Continuation state: %d", data[0]);
packet_hexdump(data + 1, size - 1);
}
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;
}
}
static bool avrcp_control_packet(struct avctp_frame *avctp_frame)
{
struct l2cap_frame *frame = &avctp_frame->l2cap_frame;
uint8_t ctype, address, subunit, opcode, company[3], indent = 2;
if (!l2cap_frame_get_u8(frame, &ctype) ||
!l2cap_frame_get_u8(frame, &address) ||
!l2cap_frame_get_u8(frame, &opcode))
return false;
print_field("AV/C: %s: address 0x%02x opcode 0x%02x",
ctype2str(ctype), address, opcode);
subunit = address >> 3;
print_field("%*cSubunit: %s", indent, ' ', subunit2str(subunit));
print_field("%*cOpcode: %s", indent, ' ', opcode2str(opcode));
/* Skip non-panel subunit packets */
if (subunit != 0x09) {
packet_hexdump(frame->data, frame->size);
return true;
}
/* Not implemented should not contain any operand */
if (ctype == 0x8) {
packet_hexdump(frame->data, frame->size);
return true;
}
switch (opcode) {
case 0x7c:
return avrcp_passthrough_packet(avctp_frame);
case 0x00:
if (!l2cap_frame_get_u8(frame, &company[0]) ||
!l2cap_frame_get_u8(frame, &company[1]) ||
!l2cap_frame_get_u8(frame, &company[2]))
return false;
print_field("%*cCompany ID: 0x%02x%02x%02x", indent, ' ',
company[0], company[1], company[2]);
return avrcp_pdu_packet(avctp_frame, ctype, 10);
default:
packet_hexdump(frame->data, frame->size);
return true;
}
}
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);
}
static void service_rsp(const struct l2cap_frame *frame, struct tid_data *tid)
{
uint16_t count;
int i;
clear_tid(tid);
if (frame->size < 4) {
print_text(COLOR_ERROR, "invalid size");
packet_hexdump(frame->data, frame->size);
return;
}
count = get_be16(frame->data + 2);
if (count * 4 > frame->size) {
print_text(COLOR_ERROR, "invalid record count");
return;
}
print_field("Total record count: %d", get_be16(frame->data));
print_field("Current record count: %d", count);
for (i = 0; i < count; i++)
print_field("Record handle: 0x%4.4x",
get_be32(frame->data + 4 + (i * 4)));
print_continuation(frame->data + 4 + (count * 4),
frame->size - 4 - (count * 4));
}
static void print_uuid(uint8_t indent, const uint8_t *data, uint32_t size)
{
switch (size) {
case 2:
print_field("%*c%s (0x%4.4x)", indent, ' ',
bt_uuid16_to_str(get_be16(data)), get_be16(data));
break;
case 4:
print_field("%*c%s (0x%8.8x)", indent, ' ',
bt_uuid32_to_str(get_be32(data)), get_be32(data));
break;
case 16:
/* BASE_UUID = 00000000-0000-1000-8000-00805F9B34FB */
print_field("%*c%8.8x-%4.4x-%4.4x-%4.4x-%4.4x%8.4x",
indent, ' ',
get_be32(data), get_be16(data + 4),
get_be16(data + 6), get_be16(data + 8),
get_be16(data + 10), get_be32(data + 12));
if (get_be16(data + 4) == 0x0000 &&
get_be16(data + 6) == 0x1000 &&
get_be16(data + 8) == 0x8000 &&
get_be16(data + 10) == 0x0080 &&
get_be32(data + 12) == 0x5F9B34FB)
print_field("%*c%s", indent, ' ',
bt_uuid32_to_str(get_be32(data)));
break;
default:
packet_hexdump(data, size);
break;
}
}
static bool avrcp_passthrough_packet(struct avctp_frame *avctp_frame)
{
struct l2cap_frame *frame = &avctp_frame->l2cap_frame;
packet_hexdump(frame->data, frame->size);
return true;
}
void packet_hci_acldata(struct timeval *tv, uint16_t index, bool in,
const void *data, uint16_t size)
{
const hci_acl_hdr *hdr = data;
uint16_t handle = btohs(hdr->handle);
uint16_t dlen = btohs(hdr->dlen);
uint8_t flags = acl_flags(handle);
btsnoop_write(tv, index, in ? 0x01 : 0x00, data, size);
print_header(tv, index);
if (size < HCI_ACL_HDR_SIZE) {
printf("* Malformed ACL Data %s packet\n", in ? "RX" : "TX");
return;
}
printf("%c ACL Data: handle %d flags 0x%2.2x dlen %d\n",
in ? '>' : '<', acl_handle(handle), flags, dlen);
data += HCI_ACL_HDR_SIZE;
size -= HCI_ACL_HDR_SIZE;
if (filter_mask & PACKET_FILTER_SHOW_ACL_DATA)
packet_hexdump(data, size);
}
static void mgmt_device_disconnected(uint16_t len, const void *buf)
{
const struct mgmt_ev_device_disconnected *ev = buf;
char str[18];
uint8_t reason;
uint16_t consumed_len;
if (len < sizeof(struct mgmt_addr_info)) {
printf("* Malformed Device Disconnected control\n");
return;
}
if (len < sizeof(*ev)) {
reason = MGMT_DEV_DISCONN_UNKNOWN;
consumed_len = len;
} else {
reason = ev->reason;
consumed_len = sizeof(*ev);
}
ba2str(&ev->addr.bdaddr, str);
printf("@ Device Disconnected: %s (%d) reason %u\n", str, ev->addr.type,
reason);
buf += consumed_len;
len -= consumed_len;
packet_hexdump(buf, len);
}
static void search_attr_req(const struct l2cap_frame *frame,
struct tid_data *tid)
{
uint32_t search_bytes, attr_bytes;
search_bytes = get_bytes(frame->data, frame->size);
print_field("Search pattern: [len %d]", search_bytes);
if (search_bytes + 2 > frame->size) {
print_text(COLOR_ERROR, "invalid search list length");
packet_hexdump(frame->data, frame->size);
return;
}
decode_data_elements(0, 2, frame->data, search_bytes, NULL);
print_field("Max record count: %d",
get_be16(frame->data + search_bytes));
attr_bytes = get_bytes(frame->data + search_bytes + 2,
frame->size - search_bytes - 2);
print_field("Attribute list: [len %d]", attr_bytes);
if (search_bytes + attr_bytes > frame->size) {
print_text(COLOR_ERROR, "invalid attribute list length");
return;
}
decode_data_elements(0, 2, frame->data + search_bytes + 2,
attr_bytes, NULL);
store_continuation(tid, frame->data + search_bytes + 2 + attr_bytes,
frame->size - search_bytes - 2 - attr_bytes);
}
static inline bool mcc_msc(struct rfcomm_frame *rfcomm_frame, uint8_t indent)
{
struct l2cap_frame *frame = &rfcomm_frame->l2cap_frame;
struct rfcomm_lmsc msc;
if (!l2cap_frame_get_u8(frame, &msc.dlci))
return false;
print_field("%*cdlci %d ", indent, ' ', RFCOMM_GET_DLCI(msc.dlci));
if (!l2cap_frame_get_u8(frame, &msc.v24_sig))
return false;
/* v24 control signals */
print_field("%*cfc %d rtc %d rtr %d ic %d dv %d", indent, ' ',
GET_V24_FC(msc.v24_sig), GET_V24_RTC(msc.v24_sig),
GET_V24_RTR(msc.v24_sig), GET_V24_IC(msc.v24_sig),
GET_V24_DV(msc.v24_sig));
if (frame->size < 2)
goto done;
/*
* TODO: Implement the break signals decoding.
*/
packet_hexdump(frame->data, frame->size);
done:
return true;
}
static void mgmt_new_settings(uint16_t len, const void *buf)
{
uint32_t settings;
unsigned int i;
if (len < 4) {
printf("* Malformed New Settings control\n");
return;
}
settings = get_le32(buf);
printf("@ New Settings: 0x%4.4x\n", settings);
if (settings) {
printf("%-12c", ' ');
for (i = 0; i < NELEM(settings_str); i++) {
if (settings & (1 << i))
printf("%s ", settings_str[i]);
}
printf("\n");
}
buf += 4;
len -= 4;
packet_hexdump(buf, len);
}
static void mgmt_new_conn_param(uint16_t len, const void *buf)
{
const struct mgmt_ev_new_conn_param *ev = buf;
char addr[18];
uint16_t min, max, latency, timeout;
if (len < sizeof(*ev)) {
printf("* Malformed New Connection Parameter control\n");
return;
}
ba2str(&ev->addr.bdaddr, addr);
min = le16_to_cpu(ev->min_interval);
max = le16_to_cpu(ev->max_interval);
latency = le16_to_cpu(ev->latency);
timeout = le16_to_cpu(ev->timeout);
printf("@ New Conn Param: %s (%d) hint %d min 0x%4.4x max 0x%4.4x "
"latency 0x%4.4x timeout 0x%4.4x\n", addr, ev->addr.type,
ev->store_hint, min, max, latency, timeout);
buf += sizeof(*ev);
len -= sizeof(*ev);
packet_hexdump(buf, len);
}
void packet_hci_command(struct timeval *tv, uint16_t index,
const void *data, uint16_t size)
{
const hci_command_hdr *hdr = data;
uint16_t opcode = btohs(hdr->opcode);
uint16_t ogf = cmd_opcode_ogf(opcode);
uint16_t ocf = cmd_opcode_ocf(opcode);
btsnoop_write(tv, index, 0x02, data, size);
print_header(tv, index);
if (size < HCI_COMMAND_HDR_SIZE) {
printf("* Malformed HCI Command packet\n");
return;
}
printf("< HCI Command: %s (0x%2.2x|0x%4.4x) plen %d\n",
opcode2str(opcode), ogf, ocf, hdr->plen);
data += HCI_COMMAND_HDR_SIZE;
size -= HCI_COMMAND_HDR_SIZE;
packet_hexdump(data, size);
}
static void mgmt_new_config_options(uint16_t len, const void *buf)
{
uint32_t options;
unsigned int i;
if (len < 4) {
printf("* Malformed New Configuration Options control\n");
return;
}
options = get_le32(buf);
printf("@ New Configuration Options: 0x%4.4x\n", options);
if (options) {
printf("%-12c", ' ');
for (i = 0; i < NELEM(config_options_str); i++) {
if (options & (1 << i))
printf("%s ", config_options_str[i]);
}
printf("\n");
}
buf += 4;
len -= 4;
packet_hexdump(buf, len);
}
static void read_ram_rsp(const void *data, uint8_t size)
{
uint8_t status = get_u8(data);
print_status(status);
packet_hexdump(data + 1, size - 1);
}
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 set_sleepmode_param_cmd(const void *data, uint8_t size)
{
uint8_t mode = get_u8(data);
print_sleep_mode(mode);
packet_hexdump(data + 1, size - 1);
}
static void read_sleepmode_param_rsp(const void *data, uint8_t size)
{
uint8_t status = get_u8(data);
uint8_t mode = get_u8(data + 1);
print_status(status);
print_sleep_mode(mode);
packet_hexdump(data + 2, size - 2);
}
static bool avrcp_get_capabilities(struct avctp_frame *avctp_frame,
uint8_t ctype, uint8_t len,
uint8_t indent)
{
struct l2cap_frame *frame = &avctp_frame->l2cap_frame;
uint8_t cap, count;
int i;
if (!l2cap_frame_get_u8(frame, &cap))
return false;
print_field("%*cCapabilityID: 0x%02x (%s)", (indent - 8), ' ', cap,
cap2str(cap));
if (len == 1)
return true;
if (!l2cap_frame_get_u8(frame, &count))
return false;
print_field("%*cCapabilityCount: 0x%02x", (indent - 8), ' ', count);
switch (cap) {
case 0x2:
for (; count > 0; count--) {
uint8_t company[3];
if (!l2cap_frame_get_u8(frame, &company[0]) ||
!l2cap_frame_get_u8(frame, &company[1]) ||
!l2cap_frame_get_u8(frame, &company[2]))
return false;
print_field("%*c%s: 0x%02x%02x%02x", (indent - 8), ' ',
cap2str(cap), company[0], company[1],
company[2]);
}
break;
case 0x3:
for (i = 0; count > 0; count--, i++) {
uint8_t event;
if (!l2cap_frame_get_u8(frame, &event))
return false;
print_field("%*c%s: 0x%02x (%s)", (indent - 8), ' ',
cap2str(cap), event, event2str(event));
}
break;
default:
packet_hexdump(frame->data, frame->size);
}
return true;
}
static void avrcp_packet(struct avctp_frame *avctp_frame)
{
struct l2cap_frame *frame = &avctp_frame->l2cap_frame;
bool ret;
switch (frame->psm) {
case 0x17:
ret = avrcp_control_packet(avctp_frame);
break;
case 0x1B:
ret = avrcp_browsing_packet(avctp_frame);
break;
default:
packet_hexdump(frame->data, frame->size);
return;
}
if (!ret) {
print_text(COLOR_ERROR, "PDU malformed");
packet_hexdump(frame->data, frame->size);
}
}
static uint16_t common_rsp(const struct l2cap_frame *frame,
struct tid_data *tid)
{
uint16_t bytes;
if (frame->size < 2) {
print_text(COLOR_ERROR, "invalid size");
packet_hexdump(frame->data, frame->size);
return 0;
}
bytes = get_be16(frame->data);
print_field("Attribute bytes: %d", bytes);
if (bytes > frame->size - 2) {
print_text(COLOR_ERROR, "invalid attribute size");
packet_hexdump(frame->data + 2, frame->size - 2);
return 0;
}
return bytes;
}
static void mgmt_new_long_term_key(uint16_t len, const void *buf)
{
const struct mgmt_ev_new_long_term_key *ev = buf;
const char *type;
char str[18];
if (len < sizeof(*ev)) {
printf("* Malformed New Long Term Key control\n");
return;
}
/* LE SC keys are both for master and slave */
switch (ev->key.type) {
case 0x00:
if (ev->key.master)
type = "Master (Unauthenticated)";
else
type = "Slave (Unauthenticated)";
break;
case 0x01:
if (ev->key.master)
type = "Master (Authenticated)";
else
type = "Slave (Authenticated)";
break;
case 0x02:
type = "SC (Unauthenticated)";
break;
case 0x03:
type = "SC (Authenticated)";
break;
case 0x04:
type = "SC (Debug)";
break;
default:
type = "<unknown>";
break;
}
ba2str(&ev->key.addr.bdaddr, str);
printf("@ New Long Term Key: %s (%d) %s 0x%02x\n", str,
ev->key.addr.type, type, ev->key.type);
buf += sizeof(*ev);
len -= sizeof(*ev);
packet_hexdump(buf, len);
}
void packet_monitor(struct timeval *tv, uint16_t index, uint16_t opcode,
const void *data, uint16_t size)
{
const struct monitor_new_index *ni;
char str[18];
switch (opcode) {
case MONITOR_NEW_INDEX:
ni = data;
if (index < MAX_INDEX)
memcpy(&index_list[index], ni, MONITOR_NEW_INDEX_SIZE);
ba2str(&ni->bdaddr, str);
packet_new_index(tv, index, str, ni->type, ni->bus, ni->name);
break;
case MONITOR_DEL_INDEX:
if (index < MAX_INDEX)
ba2str(&index_list[index].bdaddr, str);
else
ba2str(BDADDR_ANY, str);
packet_del_index(tv, index, str);
break;
case MONITOR_COMMAND_PKT:
packet_hci_command(tv, index, data, size);
break;
case MONITOR_EVENT_PKT:
packet_hci_event(tv, index, data, size);
break;
case MONITOR_ACL_TX_PKT:
packet_hci_acldata(tv, index, false, data, size);
break;
case MONITOR_ACL_RX_PKT:
packet_hci_acldata(tv, index, true, data, size);
break;
case MONITOR_SCO_TX_PKT:
packet_hci_scodata(tv, index, false, data, size);
break;
case MONITOR_SCO_RX_PKT:
packet_hci_scodata(tv, index, true, data, size);
break;
default:
print_header(tv, index);
printf("* Unknown packet (code %d len %d)\n", opcode, size);
packet_hexdump(data, size);
break;
}
}
static void mgmt_local_name_changed(uint16_t len, const void *buf)
{
const struct mgmt_ev_local_name_changed *ev = buf;
if (len < sizeof(*ev)) {
printf("* Malformed Local Name Changed control\n");
return;
}
printf("@ Local Name Changed: %s (%s)\n", ev->name, ev->short_name);
buf += sizeof(*ev);
len -= sizeof(*ev);
packet_hexdump(buf, len);
}
static void mgmt_advertising_removed(uint16_t len, const void *buf)
{
const struct mgmt_ev_advertising_removed *ev = buf;
if (len < sizeof(*ev)) {
printf("* Malformed Advertising Removed control\n");
return;
}
printf("@ Advertising Removed: %u\n", ev->instance);
buf += sizeof(*ev);
len -= sizeof(*ev);
packet_hexdump(buf, len);
}
static void mgmt_discovering(uint16_t len, const void *buf)
{
const struct mgmt_ev_discovering *ev = buf;
if (len < sizeof(*ev)) {
printf("* Malformed Discovering control\n");
return;
}
printf("@ Discovering: 0x%2.2x (%d)\n", ev->discovering, ev->type);
buf += sizeof(*ev);
len -= sizeof(*ev);
packet_hexdump(buf, len);
}
