static bool filter_nettype_req(struct bnep_frame *bnep_frame, uint8_t indent)
{
struct l2cap_frame *frame = &bnep_frame->l2cap_frame;
uint16_t length, start_range, end_range;
int i;
if (!l2cap_frame_get_be16(frame, &length))
return false;
print_field("%*cLength: 0x%04x", indent, ' ', length);
for (i = 0; i < length / 4; i++) {
if (!l2cap_frame_get_be16(frame, &start_range))
return false;
if (!l2cap_frame_get_be16(frame, &end_range))
return false;
print_field("%*c0x%04x - 0x%04x", indent, ' ',
start_range, end_range);
}
return true;
}
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 bool setup_conn_req(struct bnep_frame *bnep_frame, uint8_t indent)
{
struct l2cap_frame *frame = &bnep_frame->l2cap_frame;
uint8_t uuid_size;
uint32_t src_uuid = 0, dst_uuid = 0;
if (!l2cap_frame_get_u8(frame, &uuid_size))
return false;
print_field("%*cSize: 0x%02x ", indent, ' ', uuid_size);
switch (uuid_size) {
case 2:
if (!l2cap_frame_get_be16(frame, (uint16_t *) &dst_uuid))
return false;
if (!l2cap_frame_get_be16(frame, (uint16_t *) &src_uuid))
return false;
break;
case 4:
if (!l2cap_frame_get_be32(frame, &dst_uuid))
return false;
if (!l2cap_frame_get_be32(frame, &src_uuid))
return false;
break;
case 16:
if (!l2cap_frame_get_be32(frame, &dst_uuid))
return false;
l2cap_frame_pull(frame, frame, 12);
if (!l2cap_frame_get_be32(frame, &src_uuid))
return false;
l2cap_frame_pull(frame, frame, 12);
break;
default:
l2cap_frame_pull(frame, frame, (uuid_size * 2));
return true;
}
print_field("%*cDst: 0x%x(%s)", indent, ' ', dst_uuid,
bt_uuid32_to_str(dst_uuid));
print_field("%*cSrc: 0x%x(%s)", indent, ' ', src_uuid,
bt_uuid32_to_str(src_uuid));
return true;
}
static bool filter_multaddr_req(struct bnep_frame *bnep_frame, uint8_t indent)
{
struct l2cap_frame *frame = &bnep_frame->l2cap_frame;
uint16_t length;
char start_addr[20], end_addr[20];
int i;
if (!l2cap_frame_get_be16(frame, &length))
return false;
print_field("%*cLength: 0x%04x", indent, ' ', length);
for (i = 0; i < length / 12; i++) {
if (!get_macaddr(bnep_frame, start_addr))
return false;
if (!get_macaddr(bnep_frame, end_addr))
return false;
print_field("%*c%s - %s", indent, ' ', start_addr, end_addr);
}
return true;
}
static bool avrcp_displayable_charset(struct avctp_frame *avctp_frame,
uint8_t ctype, uint8_t len,
uint8_t indent)
{
struct l2cap_frame *frame = &avctp_frame->l2cap_frame;
uint8_t num;
if (ctype > AVC_CTYPE_GENERAL_INQUIRY)
return true;
if (!l2cap_frame_get_u8(frame, &num))
return false;
print_field("%*cCharsetCount: 0x%02x", (indent - 8), ' ', num);
for (; num > 0; num--) {
uint16_t charset;
if (!l2cap_frame_get_be16(frame, &charset))
return false;
print_field("%*cCharsetID: 0x%04x (%s)", (indent - 8),
' ', charset, charset2str(charset));
}
return true;
}
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 bool avrcp_set_addressed_player(struct avctp_frame *avctp_frame,
uint8_t ctype, uint8_t len,
uint8_t indent)
{
struct l2cap_frame *frame = &avctp_frame->l2cap_frame;
uint16_t id;
uint8_t status;
if (ctype > AVC_CTYPE_GENERAL_INQUIRY)
goto response;
if (!l2cap_frame_get_be16(frame, &id))
return false;
print_field("%*cPlayerID: 0x%04x (%u)", (indent - 8), ' ', id, id);
return true;
response:
if (!l2cap_frame_get_u8(frame, &status))
return false;
print_field("%*cStatus: 0x%02x (%s)", (indent - 8), ' ',
status, error2str(status));
return true;
}
static bool bnep_compressed(struct bnep_frame *bnep_frame,
uint8_t indent, int hdr_len)
{
struct l2cap_frame *frame = &bnep_frame->l2cap_frame;
if (!l2cap_frame_get_be16(frame, &proto))
return false;
print_field("%*c[proto 0x%04x] ", indent, ' ', proto);
return true;
}
static bool print_rsp_msg(struct bnep_frame *bnep_frame, uint8_t indent)
{
struct l2cap_frame *frame = &bnep_frame->l2cap_frame;
uint16_t rsp_msg;
if (!l2cap_frame_get_be16(frame, &rsp_msg))
return false;
print_field("%*cRsp msg: %s(0x%04x) ", indent, ' ',
value2str(rsp_msg), rsp_msg);
return true;
}
static bool bnep_dst_only(struct bnep_frame *bnep_frame,
uint8_t indent, int hdr_len)
{
struct l2cap_frame *frame;
char dest_addr[20];
if (!get_macaddr(bnep_frame, dest_addr))
return false;
frame = &bnep_frame->l2cap_frame;
if (!l2cap_frame_get_be16(frame, &proto))
return false;
print_field("%*cdst %s [proto 0x%04x] ", indent,
' ', dest_addr, proto);
return true;
}
static bool avrcp_pdu_packet(struct avctp_frame *avctp_frame, uint8_t ctype,
uint8_t indent)
{
struct l2cap_frame *frame = &avctp_frame->l2cap_frame;
uint8_t pduid;
uint16_t len;
int i;
const struct avrcp_ctrl_pdu_data *ctrl_pdu_data = NULL;
if (!l2cap_frame_get_u8(frame, &pduid))
return false;
if (!l2cap_frame_get_u8(frame, &avctp_frame->pt))
return false;
if (!l2cap_frame_get_be16(frame, &len))
return false;
print_indent(indent, COLOR_OFF, "AVRCP: ", pdu2str(pduid), COLOR_OFF,
" pt %s len 0x%04x", pt2str(avctp_frame->pt), len);
if (frame->size != len)
return false;
if (ctype == 0xA)
return avrcp_rejected_packet(frame, indent + 2);
for (i = 0; avrcp_ctrl_pdu_table[i].func; i++) {
if (avrcp_ctrl_pdu_table[i].pduid == pduid) {
ctrl_pdu_data = &avrcp_ctrl_pdu_table[i];
break;
}
}
if (!ctrl_pdu_data || !ctrl_pdu_data->func) {
packet_hexdump(frame->data, frame->size);
return true;
}
return ctrl_pdu_data->func(avctp_frame, ctype, len, indent + 2);
}
static bool avrcp_add_to_now_playing(struct avctp_frame *avctp_frame,
uint8_t ctype, uint8_t len,
uint8_t indent)
{
struct l2cap_frame *frame = &avctp_frame->l2cap_frame;
uint64_t uid;
uint16_t uidcounter;
uint8_t scope, status;
if (ctype > AVC_CTYPE_GENERAL_INQUIRY)
goto response;
if (!l2cap_frame_get_u8(frame, &scope))
return false;
print_field("%*cScope: 0x%02x (%s)", (indent - 8), ' ',
scope, scope2str(scope));
if (!l2cap_frame_get_be64(frame, &uid))
return false;
print_field("%*cUID: 0x%16" PRIx64 " (%" PRIu64 ")", (indent - 8),
' ', uid, uid);
if (!l2cap_frame_get_be16(frame, &uidcounter))
return false;
print_field("%*cUIDCounter: 0x%04x (%u)", (indent - 8), ' ',
uidcounter, uidcounter);
return true;
response:
if (!l2cap_frame_get_u8(frame, &status))
return false;
print_field("%*cStatus: 0x%02x (%s)", (indent - 8), ' ', status,
error2str(status));
return true;
}
static bool avrcp_get_player_value_text(struct avctp_frame *avctp_frame,
uint8_t ctype, uint8_t len,
uint8_t indent)
{
struct l2cap_frame *frame = &avctp_frame->l2cap_frame;
static uint8_t attr = 0;
uint8_t num;
if (ctype > AVC_CTYPE_GENERAL_INQUIRY)
goto response;
if (!l2cap_frame_get_u8(frame, &attr))
return false;
print_field("%*cAttributeID: 0x%02x (%s)", (indent - 8), ' ',
attr, attr2str(attr));
if (!l2cap_frame_get_u8(frame, &num))
return false;
print_field("%*cValueCount: 0x%02x", (indent - 8), ' ', num);
for (; num > 0; num--) {
uint8_t value;
if (!l2cap_frame_get_u8(frame, &value))
return false;
print_field("%*cValueID: 0x%02x (%s)", (indent - 8),
' ', value, value2str(attr, value));
}
return true;
response:
if (!l2cap_frame_get_u8(frame, &num))
return false;
print_field("%*cValueCount: 0x%02x", (indent - 8), ' ', num);
for (; num > 0; num--) {
uint8_t value, len;
uint16_t charset;
if (!l2cap_frame_get_u8(frame, &value))
return false;
print_field("%*cValueID: 0x%02x (%s)", (indent - 8), ' ',
value, value2str(attr, value));
if (!l2cap_frame_get_be16(frame, &charset))
return false;
print_field("%*cCharsetIDID: 0x%02x (%s)", (indent - 8), ' ',
charset, charset2str(charset));
if (!l2cap_frame_get_u8(frame, &len))
return false;
print_field("%*cStringLength: 0x%02x", (indent - 8), ' ', len);
printf("String: ");
for (; len > 0; len--) {
uint8_t c;
if (!l2cap_frame_get_u8(frame, &c))
return false;
printf("%1c", isprint(c) ? c : '.');
}
printf("\n");
}
return true;
}
static bool avrcp_register_notification(struct avctp_frame *avctp_frame,
uint8_t ctype, uint8_t len,
uint8_t indent)
{
struct l2cap_frame *frame = &avctp_frame->l2cap_frame;
uint8_t event, status;
uint16_t uid;
uint32_t interval;
uint64_t id;
if (ctype > AVC_CTYPE_GENERAL_INQUIRY)
goto response;
if (!l2cap_frame_get_u8(frame, &event))
return false;
print_field("%*cEventID: 0x%02x (%s)", (indent - 8),
' ', event, event2str(event));
if (!l2cap_frame_get_be32(frame, &interval))
return false;
print_field("%*cInterval: 0x%08x (%u seconds)",
(indent - 8), ' ', interval, interval);
return true;
response:
if (!l2cap_frame_get_u8(frame, &event))
return false;
print_field("%*cEventID: 0x%02x (%s)", (indent - 8),
' ', event, event2str(event));
switch (event) {
case AVRCP_EVENT_PLAYBACK_STATUS_CHANGED:
if (!l2cap_frame_get_u8(frame, &status))
return false;
print_field("%*cPlayStatus: 0x%02x (%s)", (indent - 8),
' ', status, playstatus2str(status));
break;
case AVRCP_EVENT_TRACK_CHANGED:
if (!l2cap_frame_get_be64(frame, &id))
return false;
print_field("%*cIdentifier: 0x%16" PRIx64 " (%" PRIu64 ")",
(indent - 8), ' ', id, id);
break;
case AVRCP_EVENT_PLAYBACK_POS_CHANGED:
if (!l2cap_frame_get_be32(frame, &interval))
return false;
print_field("%*cPosition: 0x%08x (%u miliseconds)",
(indent - 8), ' ', interval, interval);
break;
case AVRCP_EVENT_BATT_STATUS_CHANGED:
if (!l2cap_frame_get_u8(frame, &status))
return false;
print_field("%*cBatteryStatus: 0x%02x (%s)", (indent - 8),
' ', status, status2str(status));
break;
case AVRCP_EVENT_SYSTEM_STATUS_CHANGED:
if (!l2cap_frame_get_u8(frame, &status))
return false;
print_field("%*cSystemStatus: 0x%02x ", (indent - 8),
' ', status);
switch (status) {
case 0x00:
printf("(POWER_ON)\n");
break;
case 0x01:
printf("(POWER_OFF)\n");
break;
case 0x02:
printf("(UNPLUGGED)\n");
break;
default:
printf("(UNKOWN)\n");
break;
}
break;
case AVRCP_EVENT_PLAYER_APPLICATION_SETTING_CHANGED:
if (!l2cap_frame_get_u8(frame, &status))
return false;
print_field("%*cAttributeCount: 0x%02x", (indent - 8),
' ', status);
for (; status > 0; status--) {
uint8_t attr, value;
if (!l2cap_frame_get_u8(frame, &attr))
return false;
print_field("%*cAttributeID: 0x%02x (%s)",
(indent - 8), ' ', attr, attr2str(attr));
if (!l2cap_frame_get_u8(frame, &value))
return false;
print_field("%*cValueID: 0x%02x (%s)", (indent - 8),
' ', value, value2str(attr, value));
}
break;
case AVRCP_EVENT_VOLUME_CHANGED:
if (!l2cap_frame_get_u8(frame, &status))
return false;
status &= 0x7F;
print_field("%*cVolume: %.2f%% (%d/127)", (indent - 8),
' ', status/1.27, status);
break;
case AVRCP_EVENT_ADDRESSED_PLAYER_CHANGED:
if (!l2cap_frame_get_be16(frame, &uid))
return false;
print_field("%*cPlayerID: 0x%04x (%u)", (indent - 8),
' ', uid, uid);
if (!l2cap_frame_get_be16(frame, &uid))
return false;
print_field("%*cUIDCounter: 0x%04x (%u)", (indent - 8),
' ', uid, uid);
break;
case AVRCP_EVENT_UIDS_CHANGED:
if (!l2cap_frame_get_be16(frame, &uid))
return false;
print_field("%*cUIDCounter: 0x%04x (%u)", (indent - 8),
' ', uid, uid);
break;
}
return true;
}
static bool avrcp_get_element_attributes(struct avctp_frame *avctp_frame,
uint8_t ctype, uint8_t len,
uint8_t indent)
{
struct l2cap_frame *frame = &avctp_frame->l2cap_frame;
uint64_t id;
uint8_t num;
if (ctype > AVC_CTYPE_GENERAL_INQUIRY)
goto response;
if (!l2cap_frame_get_be64(frame, &id))
return false;
print_field("%*cIdentifier: 0x%jx (%s)", (indent - 8), ' ',
id, id ? "Reserved" : "PLAYING");
if (!l2cap_frame_get_u8(frame, &num))
return false;
print_field("%*cAttributeCount: 0x%02x", (indent - 8), ' ', num);
for (; num > 0; num--) {
uint32_t attr;
if (!l2cap_frame_get_be32(frame, &attr))
return false;
print_field("%*cAttribute: 0x%08x (%s)", (indent - 8),
' ', attr, mediattr2str(attr));
}
return true;
response:
switch (avctp_frame->pt) {
case AVRCP_PACKET_TYPE_SINGLE:
case AVRCP_PACKET_TYPE_START:
if (!l2cap_frame_get_u8(frame, &num))
return false;
avrcp_continuing.num = num;
print_field("%*cAttributeCount: 0x%02x", (indent - 8),
' ', num);
len--;
break;
case AVRCP_PACKET_TYPE_CONTINUING:
case AVRCP_PACKET_TYPE_END:
num = avrcp_continuing.num;
if (avrcp_continuing.size > 0) {
uint16_t size;
if (avrcp_continuing.size > len) {
size = len;
avrcp_continuing.size -= len;
} else {
size = avrcp_continuing.size;
avrcp_continuing.size = 0;
}
printf("ContinuingAttributeValue: ");
for (; size > 0; size--) {
uint8_t c;
if (!l2cap_frame_get_u8(frame, &c))
goto failed;
printf("%1c", isprint(c) ? c : '.');
}
printf("\n");
len -= size;
}
break;
default:
goto failed;
}
while (num > 0 && len > 0) {
uint32_t attr;
uint16_t charset, attrlen;
if (!l2cap_frame_get_be32(frame, &attr))
goto failed;
print_field("%*cAttribute: 0x%08x (%s)", (indent - 8),
' ', attr, mediattr2str(attr));
if (!l2cap_frame_get_be16(frame, &charset))
goto failed;
print_field("%*cCharsetID: 0x%04x (%s)", (indent - 8),
' ', charset, charset2str(charset));
if (!l2cap_frame_get_be16(frame, &attrlen))
goto failed;
print_field("%*cAttributeValueLength: 0x%04x",
(indent - 8), ' ', attrlen);
len -= sizeof(attr) + sizeof(charset) + sizeof(attrlen);
num--;
print_field("%*cAttributeValue: ", (indent - 8), ' ');
for (; attrlen > 0 && len > 0; attrlen--, len--) {
uint8_t c;
if (!l2cap_frame_get_u8(frame, &c))
goto failed;
printf("%1c", isprint(c) ? c : '.');
}
if (attrlen > 0)
avrcp_continuing.size = attrlen;
}
avrcp_continuing.num = num;
return true;
failed:
avrcp_continuing.num = 0;
avrcp_continuing.size = 0;
return false;
}