korg/bluez
0
0
Fork 0
mirror of https://git.kernel.org/pub/scm/bluetooth/bluez.git synced 2024-12-16 15:34:53 +08:00
Code Issues Packages Projects Releases Wiki Activity
18ef642531
bluez/monitor/avdtp.c
Grzegorz Kolodziejczyk ffc0888857 monitor/avdtp: Correct bit mask for TSEP 
According to AVDP specification 1.3 (8.6.2 Stream End Point Discovery
Response), TSEP bit is on 3rd position of 3rd frame byte.
2017-06-10 21:18:52 +02:00

789 lines
17 KiB
C
Raw Blame History

/*
*
* BlueZ - Bluetooth protocol stack for Linux
*
* Copyright (C) 2015 Andrzej Kaczmarek <andrzej.kaczmarek@codecoup.pl>
*
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
#ifdef HAVE_CONFIG_H
#include <config.h>
#endif
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "lib/bluetooth.h"
#include "src/shared/util.h"
#include "bt.h"
#include "packet.h"
#include "display.h"
#include "l2cap.h"
#include "avdtp.h"
#include "a2dp.h"
/* Message Types */
#define AVDTP_MSG_TYPE_COMMAND 0x00
#define AVDTP_MSG_TYPE_GENERAL_REJECT 0x01
#define AVDTP_MSG_TYPE_RESPONSE_ACCEPT 0x02
#define AVDTP_MSG_TYPE_RESPONSE_REJECT 0x03
/* Signal Identifiers */
#define AVDTP_DISCOVER 0x01
#define AVDTP_GET_CAPABILITIES 0x02
#define AVDTP_SET_CONFIGURATION 0x03
#define AVDTP_GET_CONFIGURATION 0x04
#define AVDTP_RECONFIGURE 0x05
#define AVDTP_OPEN 0x06
#define AVDTP_START 0x07
#define AVDTP_CLOSE 0x08
#define AVDTP_SUSPEND 0x09
#define AVDTP_ABORT 0x0a
#define AVDTP_SECURITY_CONTROL 0x0b
#define AVDTP_GET_ALL_CAPABILITIES 0x0c
#define AVDTP_DELAYREPORT 0x0d
/* Service Categories */
#define AVDTP_MEDIA_TRANSPORT 0x01
#define AVDTP_REPORTING 0x02
#define AVDTP_RECOVERY 0x03
#define AVDTP_CONTENT_PROTECTION 0x04
#define AVDTP_HEADER_COMPRESSION 0x05
#define AVDTP_MULTIPLEXING 0x06
#define AVDTP_MEDIA_CODEC 0x07
#define AVDTP_DELAY_REPORTING 0x08
struct avdtp_frame {
uint8_t hdr;
uint8_t sig_id;
struct l2cap_frame l2cap_frame;
};
static inline bool is_configuration_sig_id(uint8_t sig_id)
{
return (sig_id == AVDTP_SET_CONFIGURATION) ||
(sig_id == AVDTP_GET_CONFIGURATION) ||
(sig_id == AVDTP_RECONFIGURE);
}
static const char *msgtype2str(uint8_t msgtype)
{
switch (msgtype) {
case 0:
return "Command";
case 1:
return "General Reject";
case 2:
return "Response Accept";
case 3:
return "Response Reject";
}
return "";
}
static const char *sigid2str(uint8_t sigid)
{
switch (sigid) {
case AVDTP_DISCOVER:
return "Discover";
case AVDTP_GET_CAPABILITIES:
return "Get Capabilities";
case AVDTP_SET_CONFIGURATION:
return "Set Configuration";
case AVDTP_GET_CONFIGURATION:
return "Get Configuration";
case AVDTP_RECONFIGURE:
return "Reconfigure";
case AVDTP_OPEN:
return "Open";
case AVDTP_START:
return "Start";
case AVDTP_CLOSE:
return "Close";
case AVDTP_SUSPEND:
return "Suspend";
case AVDTP_ABORT:
return "Abort";
case AVDTP_SECURITY_CONTROL:
return "Security Control";
case AVDTP_GET_ALL_CAPABILITIES:
return "Get All Capabilities";
case AVDTP_DELAYREPORT:
return "Delay Report";
default:
return "Reserved";
}
}
static const char *error2str(uint8_t error)
{
switch (error) {
case 0x01:
return "BAD_HEADER_FORMAT";
case 0x11:
return "BAD_LENGTH";
case 0x12:
return "BAD_ACP_SEID";
case 0x13:
return "SEP_IN_USE";
case 0x14:
return "SEP_NOT_IN_USER";
case 0x17:
return "BAD_SERV_CATEGORY";
case 0x18:
return "BAD_PAYLOAD_FORMAT";
case 0x19:
return "NOT_SUPPORTED_COMMAND";
case 0x1a:
return "INVALID_CAPABILITIES";
case 0x22:
return "BAD_RECOVERY_TYPE";
case 0x23:
return "BAD_MEDIA_TRANSPORT_FORMAT";
case 0x25:
return "BAD_RECOVERY_FORMAT";
case 0x26:
return "BAD_ROHC_FORMAT";
case 0x27:
return "BAD_CP_FORMAT";
case 0x28:
return "BAD_MULTIPLEXING_FORMAT";
case 0x29:
return "UNSUPPORTED_CONFIGURATION";
case 0x31:
return "BAD_STATE";
default:
return "Unknown";
}
}
static const char *mediatype2str(uint8_t media_type)
{
switch (media_type) {
case 0x00:
return "Audio";
case 0x01:
return "Video";
case 0x02:
return "Multimedia";
default:
return "Reserved";
}
}
static const char *mediacodec2str(uint8_t codec)
{
switch (codec) {
case 0x00:
return "SBC";
case 0x01:
return "MPEG-1,2 Audio";
case 0x02:
return "MPEG-2,4 AAC";
case 0x04:
return "ATRAC Family";
case 0xff:
return "Non-A2DP";
default:
return "Reserved";
}
}
static const char *cptype2str(uint8_t cp)
{
switch (cp) {
case 0x0001:
return "DTCP";
case 0x0002:
return "SCMS-T";
default:
return "Reserved";
}
}
static const char *servicecat2str(uint8_t service_cat)
{
switch (service_cat) {
case AVDTP_MEDIA_TRANSPORT:
return "Media Transport";
case AVDTP_REPORTING:
return "Reporting";
case AVDTP_RECOVERY:
return "Recovery";
case AVDTP_CONTENT_PROTECTION:
return "Content Protection";
case AVDTP_HEADER_COMPRESSION:
return "Header Compression";
case AVDTP_MULTIPLEXING:
return "Multiplexing";
case AVDTP_MEDIA_CODEC:
return "Media Codec";
case AVDTP_DELAY_REPORTING:
return "Delay Reporting";
default:
return "Reserved";
}
}
static bool avdtp_reject_common(struct avdtp_frame *avdtp_frame)
{
struct l2cap_frame *frame = &avdtp_frame->l2cap_frame;
uint8_t error;
if (!l2cap_frame_get_u8(frame, &error))
return false;
print_field("Error code: %s (0x%02x)", error2str(error), error);
return true;
}
static bool service_content_protection(struct avdtp_frame *avdtp_frame,
uint8_t losc)
{
struct l2cap_frame *frame = &avdtp_frame->l2cap_frame;
uint16_t type = 0;
if (losc < 2)
return false;
if (!l2cap_frame_get_le16(frame, &type))
return false;
losc -= 2;
print_field("%*cContent Protection Type: %s (0x%04x)", 2, ' ',
cptype2str(type), type);
/* TODO: decode protection specific information */
packet_hexdump(frame->data, losc);
l2cap_frame_pull(frame, frame, losc);
return true;
}
static bool service_media_codec(struct avdtp_frame *avdtp_frame, uint8_t losc)
{
struct l2cap_frame *frame = &avdtp_frame->l2cap_frame;
uint8_t type = 0;
uint8_t codec = 0;
if (losc < 2)
return false;
l2cap_frame_get_u8(frame, &type);
l2cap_frame_get_u8(frame, &codec);
losc -= 2;
print_field("%*cMedia Type: %s (0x%02x)", 2, ' ',
mediatype2str(type >> 4), type >> 4);
print_field("%*cMedia Codec: %s (0x%02x)", 2, ' ',
mediacodec2str(codec), codec);
if (is_configuration_sig_id(avdtp_frame->sig_id))
return a2dp_codec_cfg(codec, losc, frame);
else
return a2dp_codec_cap(codec, losc, frame);
}
static bool decode_capabilities(struct avdtp_frame *avdtp_frame)
{
struct l2cap_frame *frame = &avdtp_frame->l2cap_frame;
uint8_t service_cat;
uint8_t losc;
while (l2cap_frame_get_u8(frame, &service_cat)) {
print_field("Service Category: %s (0x%02x)",
servicecat2str(service_cat), service_cat);
if (!l2cap_frame_get_u8(frame, &losc))
return false;
if (frame->size < losc)
return false;
switch (service_cat) {
case AVDTP_CONTENT_PROTECTION:
if (!service_content_protection(avdtp_frame, losc))
return false;
break;
case AVDTP_MEDIA_CODEC:
if (!service_media_codec(avdtp_frame, losc))
return false;
break;
case AVDTP_MEDIA_TRANSPORT:
case AVDTP_REPORTING:
case AVDTP_RECOVERY:
case AVDTP_HEADER_COMPRESSION:
case AVDTP_MULTIPLEXING:
case AVDTP_DELAY_REPORTING:
default:
packet_hexdump(frame->data, losc);
l2cap_frame_pull(frame, frame, losc);
}
}
return true;
}
static bool avdtp_discover(struct avdtp_frame *avdtp_frame)
{
struct l2cap_frame *frame = &avdtp_frame->l2cap_frame;
uint8_t type = avdtp_frame->hdr & 0x03;
uint8_t seid;
uint8_t info;
switch (type) {
case AVDTP_MSG_TYPE_COMMAND:
return true;
case AVDTP_MSG_TYPE_RESPONSE_ACCEPT:
while (l2cap_frame_get_u8(frame, &seid)) {
print_field("ACP SEID: %d", seid >> 2);
if (!l2cap_frame_get_u8(frame, &info))
return false;
print_field("%*cMedia Type: %s (0x%02x)", 2, ' ',
mediatype2str(info >> 4), info >> 4);
print_field("%*cSEP Type: %s (0x%02x)", 2, ' ',
info & 0x08 ? "SNK" : "SRC",
(info >> 3) & 0x01);
print_field("%*cIn use: %s", 2, ' ',
seid & 0x02 ? "Yes" : "No");
}
return true;
case AVDTP_MSG_TYPE_RESPONSE_REJECT:
return avdtp_reject_common(avdtp_frame);
}
return false;
}
static bool avdtp_get_capabilities(struct avdtp_frame *avdtp_frame)
{
struct l2cap_frame *frame = &avdtp_frame->l2cap_frame;
uint8_t type = avdtp_frame->hdr & 0x03;
uint8_t seid;
switch (type) {
case AVDTP_MSG_TYPE_COMMAND:
if (!l2cap_frame_get_u8(frame, &seid))
return false;
print_field("ACP SEID: %d", seid >> 2);
return true;
case AVDTP_MSG_TYPE_RESPONSE_ACCEPT:
return decode_capabilities(avdtp_frame);
case AVDTP_MSG_TYPE_RESPONSE_REJECT:
return avdtp_reject_common(avdtp_frame);
}
return false;
}
static bool avdtp_set_configuration(struct avdtp_frame *avdtp_frame)
{
struct l2cap_frame *frame = &avdtp_frame->l2cap_frame;
uint8_t type = avdtp_frame->hdr & 0x03;
uint8_t acp_seid, int_seid;
uint8_t service_cat;
switch (type) {
case AVDTP_MSG_TYPE_COMMAND:
if (!l2cap_frame_get_u8(frame, &acp_seid))
return false;
print_field("ACP SEID: %d", acp_seid >> 2);
if (!l2cap_frame_get_u8(frame, &int_seid))
return false;
print_field("INT SEID: %d", int_seid >> 2);
return decode_capabilities(avdtp_frame);
case AVDTP_MSG_TYPE_RESPONSE_ACCEPT:
return true;
case AVDTP_MSG_TYPE_RESPONSE_REJECT:
if (!l2cap_frame_get_u8(frame, &service_cat))
return false;
print_field("Service Category: %s (0x%02x)",
servicecat2str(service_cat), service_cat);
return avdtp_reject_common(avdtp_frame);
}
return false;
}
static bool avdtp_get_configuration(struct avdtp_frame *avdtp_frame)
{
struct l2cap_frame *frame = &avdtp_frame->l2cap_frame;
uint8_t type = avdtp_frame->hdr & 0x03;
uint8_t seid;
switch (type) {
case AVDTP_MSG_TYPE_COMMAND:
if (!l2cap_frame_get_u8(frame, &seid))
return false;
print_field("ACP SEID: %d", seid >> 2);
return true;
case AVDTP_MSG_TYPE_RESPONSE_ACCEPT:
return decode_capabilities(avdtp_frame);
case AVDTP_MSG_TYPE_RESPONSE_REJECT:
return avdtp_reject_common(avdtp_frame);
}
return false;
}
static bool avdtp_reconfigure(struct avdtp_frame *avdtp_frame)
{
struct l2cap_frame *frame = &avdtp_frame->l2cap_frame;
uint8_t type = avdtp_frame->hdr & 0x03;
uint8_t seid;
uint8_t service_cat;
switch (type) {
case AVDTP_MSG_TYPE_COMMAND:
if (!l2cap_frame_get_u8(frame, &seid))
return false;
print_field("ACP SEID: %d", seid >> 2);
return decode_capabilities(avdtp_frame);
case AVDTP_MSG_TYPE_RESPONSE_ACCEPT:
return true;
case AVDTP_MSG_TYPE_RESPONSE_REJECT:
if (!l2cap_frame_get_u8(frame, &service_cat))
return false;
print_field("Service Category: %s (0x%02x)",
servicecat2str(service_cat), service_cat);
return avdtp_reject_common(avdtp_frame);
}
return false;
}
static bool avdtp_open(struct avdtp_frame *avdtp_frame)
{
struct l2cap_frame *frame = &avdtp_frame->l2cap_frame;
uint8_t type = avdtp_frame->hdr & 0x03;
uint8_t seid;
switch (type) {
case AVDTP_MSG_TYPE_COMMAND:
if (!l2cap_frame_get_u8(frame, &seid))
return false;
print_field("ACP SEID: %d", seid >> 2);
return true;
case AVDTP_MSG_TYPE_RESPONSE_ACCEPT:
return true;
case AVDTP_MSG_TYPE_RESPONSE_REJECT:
return avdtp_reject_common(avdtp_frame);
}
return false;
}
static bool avdtp_start(struct avdtp_frame *avdtp_frame)
{
struct l2cap_frame *frame = &avdtp_frame->l2cap_frame;
uint8_t type = avdtp_frame->hdr & 0x03;
uint8_t seid;
switch (type) {
case AVDTP_MSG_TYPE_COMMAND:
if (!l2cap_frame_get_u8(frame, &seid))
return false;
print_field("ACP SEID: %d", seid >> 2);
while (l2cap_frame_get_u8(frame, &seid))
print_field("ACP SEID: %d", seid >> 2);
return true;
case AVDTP_MSG_TYPE_RESPONSE_ACCEPT:
return true;
case AVDTP_MSG_TYPE_RESPONSE_REJECT:
if (!l2cap_frame_get_u8(frame, &seid))
return false;
print_field("ACP SEID: %d", seid >> 2);
return avdtp_reject_common(avdtp_frame);
}
return false;
}
static bool avdtp_close(struct avdtp_frame *avdtp_frame)
{
struct l2cap_frame *frame = &avdtp_frame->l2cap_frame;
uint8_t type = avdtp_frame->hdr & 0x03;
uint8_t seid;
switch (type) {
case AVDTP_MSG_TYPE_COMMAND:
if (!l2cap_frame_get_u8(frame, &seid))
return false;
print_field("ACP SEID: %d", seid >> 2);
return true;
case AVDTP_MSG_TYPE_RESPONSE_ACCEPT:
return true;
case AVDTP_MSG_TYPE_RESPONSE_REJECT:
return avdtp_reject_common(avdtp_frame);
}
return false;
}
static bool avdtp_suspend(struct avdtp_frame *avdtp_frame)
{
struct l2cap_frame *frame = &avdtp_frame->l2cap_frame;
uint8_t type = avdtp_frame->hdr & 0x03;
uint8_t seid;
switch (type) {
case AVDTP_MSG_TYPE_COMMAND:
if (!l2cap_frame_get_u8(frame, &seid))
return false;
print_field("ACP SEID: %d", seid >> 2);
while (l2cap_frame_get_u8(frame, &seid))
print_field("ACP SEID: %d", seid >> 2);
return true;
case AVDTP_MSG_TYPE_RESPONSE_ACCEPT:
return true;
case AVDTP_MSG_TYPE_RESPONSE_REJECT:
if (!l2cap_frame_get_u8(frame, &seid))
return false;
print_field("ACP SEID: %d", seid >> 2);
return avdtp_reject_common(avdtp_frame);
}
return false;
}
static bool avdtp_abort(struct avdtp_frame *avdtp_frame)
{
struct l2cap_frame *frame = &avdtp_frame->l2cap_frame;
uint8_t type = avdtp_frame->hdr & 0x03;
uint8_t seid;
switch (type) {
case AVDTP_MSG_TYPE_COMMAND:
if (!l2cap_frame_get_u8(frame, &seid))
return false;
print_field("ACP SEID: %d", seid >> 2);
return true;
case AVDTP_MSG_TYPE_RESPONSE_ACCEPT:
return true;
}
return false;
}
static bool avdtp_security_control(struct avdtp_frame *avdtp_frame)
{
struct l2cap_frame *frame = &avdtp_frame->l2cap_frame;
uint8_t type = avdtp_frame->hdr & 0x03;
uint8_t seid;
switch (type) {
case AVDTP_MSG_TYPE_COMMAND:
if (!l2cap_frame_get_u8(frame, &seid))
return false;
print_field("ACP SEID: %d", seid >> 2);
/* TODO: decode more information */
packet_hexdump(frame->data, frame->size);
return true;
case AVDTP_MSG_TYPE_RESPONSE_ACCEPT:
/* TODO: decode more information */
packet_hexdump(frame->data, frame->size);
return true;
case AVDTP_MSG_TYPE_RESPONSE_REJECT:
return avdtp_reject_common(avdtp_frame);
}
return false;
}
static bool avdtp_delayreport(struct avdtp_frame *avdtp_frame)
{
struct l2cap_frame *frame = &avdtp_frame->l2cap_frame;
uint8_t type = avdtp_frame->hdr & 0x03;
uint8_t seid;
uint16_t delay;
switch (type) {
case AVDTP_MSG_TYPE_COMMAND:
if (!l2cap_frame_get_u8(frame, &seid))
return false;
print_field("ACP SEID: %d", seid >> 2);
if (!l2cap_frame_get_be16(frame, &delay))
return false;
print_field("Delay: %d.%dms", delay / 10, delay % 10);
return true;
case AVDTP_MSG_TYPE_RESPONSE_ACCEPT:
return true;
case AVDTP_MSG_TYPE_RESPONSE_REJECT:
return avdtp_reject_common(avdtp_frame);
}
return false;
}
static bool avdtp_signalling_packet(struct avdtp_frame *avdtp_frame)
{
struct l2cap_frame *frame = &avdtp_frame->l2cap_frame;
const char *pdu_color;
uint8_t hdr;
uint8_t sig_id;
uint8_t nosp = 0;
if (frame->in)
pdu_color = COLOR_MAGENTA;
else
pdu_color = COLOR_BLUE;
if (!l2cap_frame_get_u8(frame, &hdr))
return false;
avdtp_frame->hdr = hdr;
/* Continue Packet || End Packet */
if (((hdr & 0x0c) == 0x08) || ((hdr & 0x0c) == 0x0c)) {
/* TODO: handle fragmentation */
packet_hexdump(frame->data, frame->size);
return true;
}
/* Start Packet */
if ((hdr & 0x0c) == 0x04) {
if (!l2cap_frame_get_u8(frame, &nosp))
return false;
}
if (!l2cap_frame_get_u8(frame, &sig_id))
return false;
sig_id &= 0x3f;
avdtp_frame->sig_id = sig_id;
print_indent(6, pdu_color, "AVDTP: ", sigid2str(sig_id), COLOR_OFF,
" (0x%02x) %s (0x%02x) type 0x%02x label %d nosp %d",
sig_id, msgtype2str(hdr & 0x03), hdr & 0x03,
hdr & 0x0c, hdr >> 4, nosp);
/* Start Packet */
if ((hdr & 0x0c) == 0x04) {
/* TODO: handle fragmentation */
packet_hexdump(frame->data, frame->size);
return true;
}
/* General Reject */
if ((hdr & 0x03) == 0x03)
return true;
switch (sig_id) {
case AVDTP_DISCOVER:
return avdtp_discover(avdtp_frame);
case AVDTP_GET_CAPABILITIES:
case AVDTP_GET_ALL_CAPABILITIES:
return avdtp_get_capabilities(avdtp_frame);
case AVDTP_SET_CONFIGURATION:
return avdtp_set_configuration(avdtp_frame);
case AVDTP_GET_CONFIGURATION:
return avdtp_get_configuration(avdtp_frame);
case AVDTP_RECONFIGURE:
return avdtp_reconfigure(avdtp_frame);
case AVDTP_OPEN:
return avdtp_open(avdtp_frame);
case AVDTP_START:
return avdtp_start(avdtp_frame);
case AVDTP_CLOSE:
return avdtp_close(avdtp_frame);
case AVDTP_SUSPEND:
return avdtp_suspend(avdtp_frame);
case AVDTP_ABORT:
return avdtp_abort(avdtp_frame);
case AVDTP_SECURITY_CONTROL:
return avdtp_security_control(avdtp_frame);
case AVDTP_DELAYREPORT:
return avdtp_delayreport(avdtp_frame);
}
packet_hexdump(frame->data, frame->size);
return true;
}
void avdtp_packet(const struct l2cap_frame *frame)
{
struct avdtp_frame avdtp_frame;
bool ret;
l2cap_frame_pull(&avdtp_frame.l2cap_frame, frame, 0);
switch (frame->seq_num) {
case 1:
ret = avdtp_signalling_packet(&avdtp_frame);
break;
default:
if (packet_has_filter(PACKET_FILTER_SHOW_A2DP_STREAM))
packet_hexdump(frame->data, frame->size);
return;
}
if (!ret) {
print_text(COLOR_ERROR, "PDU malformed");
packet_hexdump(frame->data, frame->size);
}
}
Reference in New Issue View Git Blame Copy Permalink