bluez/monitor/avdtp.c

/*
 *
 *  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

#define _GNU_SOURCE
#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);
	}
}