Introduce avinfo tool.

2 files changed, 573 insertions, 1 deletions

diff --git a/tools/Makefile.am b/tools/Makefile.am
index 5d677380..a529a6c5 100644
--- a/tools/Makefile.am
+++ b/tools/Makefile.am
@@ -51,7 +51,7 @@ sbin_PROGRAMS = hciattach hciconfig $(bccmd_programs) $(avctrl_programs) $(hid2h
 
 bin_PROGRAMS = $(tools_programs) $(dfutool_programs) $(dfubabel_programs)
 
-noinst_PROGRAMS = hcisecfilter ppporc
+noinst_PROGRAMS = hcisecfilter ppporc avinfo
 
 hciattach_SOURCES = hciattach.c hciattach_st.c
 hciattach_LDADD = @BLUEZ_LIBS@
@@ -68,6 +68,9 @@ l2ping_LDADD = @BLUEZ_LIBS@
 sdptool_LDADD = @BLUEZ_LIBS@ $(top_builddir)/common/libhelper.a
 
 ciptool_LDADD = @BLUEZ_LIBS@
+
+avinfo_SOURCES = avinfo.c
+avinfo_LDADD = @BLUEZ_LIBS@
 endif
 
 ppporc_LDADD = @BLUEZ_LIBS@
diff --git a/tools/avinfo.c b/tools/avinfo.c
new file mode 100644
index 00000000..f639181a
--- /dev/null
+++ b/tools/avinfo.c
@@ -0,0 +1,569 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2006-2007  Nokia Corporation
+ *  Copyright (C) 2004-2008  Marcel Holtmann <marcel@holtmann.org>
+ *
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program 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 General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; 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 <errno.h>
+#include <ctype.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <string.h>
+#include <getopt.h>
+#include <stdint.h>
+#include <sys/param.h>
+#include <sys/ioctl.h>
+#include <sys/socket.h>
+
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/hci.h>
+#include <bluetooth/hci_lib.h>
+#include <bluetooth/l2cap.h>
+
+#define AVDTP_PSM			25
+
+/* Commands */
+#define AVDTP_DISCOVER			0x01
+#define AVDTP_GET_CAPABILITIES		0x02
+
+#define AVDTP_PKT_TYPE_SINGLE		0x00
+
+#define AVDTP_MSG_TYPE_COMMAND		0x00
+
+/* SEP capability 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
+
+/* SEP types definitions */
+#define AVDTP_SEP_TYPE_SOURCE		0x00
+#define AVDTP_SEP_TYPE_SINK		0x01
+
+/* Media types definitions */
+#define AVDTP_MEDIA_TYPE_AUDIO		0x00
+#define AVDTP_MEDIA_TYPE_VIDEO		0x01
+#define AVDTP_MEDIA_TYPE_MULTIMEDIA	0x02
+
+#define A2DP_CODEC_SBC			0x00
+#define A2DP_CODEC_MPEG12		0x01
+#define A2DP_CODEC_MPEG24		0x02
+#define A2DP_CODEC_ATRAC		0x03
+
+#define SBC_SAMPLING_FREQ_16000		(1 << 3)
+#define SBC_SAMPLING_FREQ_32000		(1 << 2)
+#define SBC_SAMPLING_FREQ_44100		(1 << 1)
+#define SBC_SAMPLING_FREQ_48000		1
+
+#define SBC_CHANNEL_MODE_MONO		(1 << 3)
+#define SBC_CHANNEL_MODE_DUAL_CHANNEL	(1 << 2)
+#define SBC_CHANNEL_MODE_STEREO		(1 << 1)
+#define SBC_CHANNEL_MODE_JOINT_STEREO	1
+
+#define SBC_BLOCK_LENGTH_4		(1 << 3)
+#define SBC_BLOCK_LENGTH_8		(1 << 2)
+#define SBC_BLOCK_LENGTH_12		(1 << 1)
+#define SBC_BLOCK_LENGTH_16		1
+
+#define SBC_SUBBANDS_4			(1 << 1)
+#define SBC_SUBBANDS_8			1
+
+#define SBC_ALLOCATION_SNR		(1 << 1)
+#define SBC_ALLOCATION_LOUDNESS		1
+
+#define MPEG_CHANNEL_MODE_MONO		(1 << 3)
+#define MPEG_CHANNEL_MODE_DUAL_CHANNEL	(1 << 2)
+#define MPEG_CHANNEL_MODE_STEREO	(1 << 1)
+#define MPEG_CHANNEL_MODE_JOINT_STEREO	1
+
+#define MPEG_LAYER_MP1			(1 << 2)
+#define MPEG_LAYER_MP2			(1 << 1)
+#define MPEG_LAYER_MP3			1
+
+#define MPEG_SAMPLING_FREQ_16000	(1 << 5)
+#define MPEG_SAMPLING_FREQ_22050	(1 << 4)
+#define MPEG_SAMPLING_FREQ_24000	(1 << 3)
+#define MPEG_SAMPLING_FREQ_32000	(1 << 2)
+#define MPEG_SAMPLING_FREQ_44100	(1 << 1)
+#define MPEG_SAMPLING_FREQ_48000	1
+
+struct avdtp_service_capability {
+	uint8_t category;
+	uint8_t length;
+	uint8_t data[0];
+} __attribute__ ((packed));
+
+#if __BYTE_ORDER == __LITTLE_ENDIAN
+
+struct avdtp_header {
+	uint8_t message_type:2;
+	uint8_t packet_type:2;
+	uint8_t transaction:4;
+	uint8_t signal_id:6;
+	uint8_t rfa0:2;
+} __attribute__ ((packed));
+
+struct seid_info {
+	uint8_t rfa0:1;
+	uint8_t inuse:1;
+	uint8_t seid:6;
+	uint8_t rfa2:3;
+	uint8_t type:1;
+	uint8_t media_type:4;
+} __attribute__ ((packed));
+
+struct seid_req {
+	struct avdtp_header header;
+	uint8_t rfa0:2;
+	uint8_t acp_seid:6;
+} __attribute__ ((packed));
+
+struct avdtp_media_codec_capability {
+	uint8_t rfa0:4;
+	uint8_t media_type:4;
+	uint8_t media_codec_type;
+	uint8_t data[0];
+} __attribute__ ((packed));
+
+struct sbc_codec_cap {
+	struct avdtp_media_codec_capability cap;
+	uint8_t channel_mode:4;
+	uint8_t frequency:4;
+	uint8_t allocation_method:2;
+	uint8_t subbands:2;
+	uint8_t block_length:4;
+	uint8_t min_bitpool;
+	uint8_t max_bitpool;
+} __attribute__ ((packed));
+
+struct mpeg_codec_cap {
+	struct avdtp_media_codec_capability cap;
+	uint8_t channel_mode:4;
+	uint8_t crc:1;
+	uint8_t layer:3;
+	uint8_t frequency:6;
+	uint8_t mpf:1;
+	uint8_t rfa:1;
+	uint16_t bitrate;
+} __attribute__ ((packed));
+
+#elif __BYTE_ORDER == __BIG_ENDIAN
+
+struct avdtp_header {
+	uint8_t transaction:4;
+	uint8_t packet_type:2;
+	uint8_t message_type:2;
+	uint8_t rfa0:2;
+	uint8_t signal_id:6;
+} __attribute__ ((packed));
+
+struct seid_info {
+	uint8_t seid:6;
+	uint8_t inuse:1;
+	uint8_t rfa0:1;
+	uint8_t media_type:4;
+	uint8_t type:1;
+	uint8_t rfa2:3;
+} __attribute__ ((packed));
+
+struct seid_req {
+	struct avdtp_header header;
+	uint8_t acp_seid:6;
+	uint8_t rfa0:2;
+} __attribute__ ((packed));
+
+struct avdtp_media_codec_capability {
+	uint8_t media_type:4;
+	uint8_t rfa0:4;
+	uint8_t media_codec_type;
+	uint8_t data[0];
+} __attribute__ ((packed));
+
+struct sbc_codec_cap {
+	struct avdtp_media_codec_capability cap;
+	uint8_t frequency:4;
+	uint8_t channel_mode:4;
+	uint8_t block_length:4;
+	uint8_t subbands:2;
+	uint8_t allocation_method:2;
+	uint8_t min_bitpool;
+	uint8_t max_bitpool;
+} __attribute__ ((packed));
+
+struct mpeg_codec_cap {
+	struct avdtp_media_codec_capability cap;
+	uint8_t layer:3;
+	uint8_t crc:1;
+	uint8_t channel_mode:4;
+	uint8_t rfa:1;
+	uint8_t mpf:1;
+	uint8_t frequency:6;
+	uint16_t bitrate;
+} __attribute__ ((packed));
+
+#else
+#error "Unknown byte order"
+#endif
+
+struct discover_resp {
+	struct avdtp_header header;
+	struct seid_info seps[0];
+} __attribute__ ((packed));
+
+struct getcap_resp {
+	struct avdtp_header header;
+	uint8_t caps[0];
+} __attribute__ ((packed));
+
+
+static void print_mpeg12(struct mpeg_codec_cap *mpeg)
+{
+	printf("\tMedia Codec: MPEG12\n");
+}
+
+static void print_sbc(struct sbc_codec_cap *sbc)
+{
+	printf("\tMedia Codec: SBC\n\t\tChannel Modes: ");
+
+	if (sbc->channel_mode & SBC_CHANNEL_MODE_MONO)
+		printf("Mono ");
+	if (sbc->channel_mode & SBC_CHANNEL_MODE_DUAL_CHANNEL)
+		printf("DualChannel ");
+	if (sbc->channel_mode & SBC_CHANNEL_MODE_STEREO)
+		printf("Stereo ");
+	if (sbc->channel_mode & SBC_CHANNEL_MODE_JOINT_STEREO)
+		printf("JointStereo");
+
+	printf("\n\t\tFrequencies: ");
+	if (sbc->frequency & SBC_SAMPLING_FREQ_16000)
+		printf("16Khz ");
+	if (sbc->frequency & SBC_SAMPLING_FREQ_32000)
+		printf("32Khz ");
+	if (sbc->frequency & SBC_SAMPLING_FREQ_44100)
+		printf("44.1Khz ");
+	if (sbc->frequency & SBC_SAMPLING_FREQ_48000)
+		printf("48Khz ");
+
+	printf("\n\t\tSubbands: ");
+	if (sbc->allocation_method & SBC_SUBBANDS_4)
+		printf("4 ");
+	if (sbc->allocation_method & SBC_SUBBANDS_8)
+		printf("8");
+
+	printf("\n\t\tBlocks: ");
+	if (sbc->block_length & SBC_BLOCK_LENGTH_4)
+		printf("4 ");
+	if (sbc->block_length & SBC_BLOCK_LENGTH_8)
+		printf("8 ");
+	if (sbc->block_length & SBC_BLOCK_LENGTH_12)
+		printf("12 ");
+	if (sbc->block_length & SBC_BLOCK_LENGTH_16)
+		printf("16 ");
+
+	printf("\n\t\tBitpool Range: %d-%d\n", sbc->min_bitpool, sbc->max_bitpool);
+}
+
+static void print_media_codec(struct avdtp_media_codec_capability *cap)
+{
+	switch (cap->media_codec_type) {
+	case A2DP_CODEC_SBC:
+		print_sbc((void *) cap);
+		break;
+
+	case A2DP_CODEC_MPEG12:
+		print_mpeg12((void *) cap);
+		break;
+
+	default:
+		printf("\tMedia Codec: Unknown\n");
+	}
+}
+
+static void print_caps(void *data, int size)
+{
+	int processed;
+
+	for (processed = 0; processed + 2 < size;) {
+		struct avdtp_service_capability *cap;
+
+		cap = data;
+
+		if (processed + 2 + cap->length > size) {
+			printf("Invalid capability data in getcap resp\n");
+			break;
+		}
+
+		switch (cap->category) {
+		case AVDTP_MEDIA_TRANSPORT:
+		case AVDTP_REPORTING:
+		case AVDTP_RECOVERY:
+		case AVDTP_CONTENT_PROTECTION:
+		case AVDTP_MULTIPLEXING:
+			/* FIXME: Add proper functions */
+			break;
+		case AVDTP_MEDIA_CODEC:
+			print_media_codec((void *) cap->data);
+			break;
+		}
+		processed += 2 + cap->length;
+		data += 2 + cap->length;
+	}
+}
+
+static void init_request(struct avdtp_header *header, int request_id)
+{
+	static int transaction = 0;
+
+	header->packet_type = AVDTP_PKT_TYPE_SINGLE;
+	header->message_type = AVDTP_MSG_TYPE_COMMAND;
+	header->transaction = transaction;
+	header->signal_id = request_id;
+
+	/* clear rfa bits */
+	header->rfa0 = 0;
+
+	transaction = (transaction + 1) % 16;
+}
+
+static int avdtp_send(int sk, void *data, int len)
+{
+	int ret;
+
+	ret = send(sk, data, len, 0);
+
+	if (ret < 0)
+		ret = -errno;
+	else if (ret != len)
+		ret = -EIO;
+
+	if (ret < 0) {
+		printf("Unable to send message: %s (%d)\n", strerror(-ret), -ret);
+		return ret;
+	}
+
+	return ret;
+}
+
+static int avdtp_receive(int sk, void *data, int len)
+{
+	int ret;
+
+	ret = recv(sk, data, len, 0);
+
+	if (ret < 0) {
+		printf("Unable to receive message: %s (%d)\n", strerror(errno),
+			errno);
+		return -errno;
+	}
+
+	return ret;
+}
+
+int avdtp_get_caps(int sk, int seid)
+{
+	struct seid_req req;
+	char buffer[1024];
+	struct getcap_resp *caps = (void *) buffer;
+	int ret;
+
+	memset(&req, 0, sizeof(req));
+	init_request(&req.header, AVDTP_GET_CAPABILITIES);
+	req.acp_seid = seid;
+
+	ret = avdtp_send(sk, &req, sizeof(req));
+	if (ret < 0)
+		return ret;
+
+	memset(&buffer, 0, sizeof(buffer));
+	ret = avdtp_receive(sk, caps, sizeof(buffer));
+	if (ret < 0)
+		return ret;
+
+	if (ret < (sizeof(struct getcap_resp) + 4 +
+				sizeof(struct avdtp_media_codec_capability))) {
+		printf("Invalid capabilities\n");
+		return -1;
+	}
+
+	print_caps(caps, ret);
+
+	return 0;
+}
+
+int avdtp_discover(int sk)
+{
+	struct avdtp_header req;
+	char buffer[256];
+	struct discover_resp *discover = (void *) buffer;
+	int ret, seps, i;
+
+	memset(&req, 0, sizeof(req));
+	init_request(&req, AVDTP_DISCOVER);
+
+	ret = avdtp_send(sk, &req, sizeof(req));
+	if (ret < 0)
+		return ret;
+
+	memset(&buffer, 0, sizeof(buffer));
+	ret = avdtp_receive(sk, discover, sizeof(buffer));
+	if (ret < 0)
+		return ret;
+
+	seps = (ret - sizeof(struct avdtp_header)) / sizeof(struct seid_info);
+	for (i = 0; i < seps; i++) {
+		const char *type, *media;
+
+		switch (discover->seps[i].type) {
+		case AVDTP_SEP_TYPE_SOURCE:
+			type = "Source";
+			break;
+
+		case AVDTP_SEP_TYPE_SINK:
+			type = "Sink";
+			break;
+
+		default:
+			type = "Invalid";
+		}
+
+		switch (discover->seps[i].media_type) {
+		case AVDTP_MEDIA_TYPE_AUDIO:
+			media = "Audio";
+			break;
+
+		case AVDTP_MEDIA_TYPE_VIDEO:
+			media = "Audio";
+			break;
+
+		case AVDTP_MEDIA_TYPE_MULTIMEDIA:
+			media = "Audio";
+			break;
+
+		default:
+			media = "Invalid";
+		}
+
+		printf("Stream End-Point #%d: %s %s %s\n", discover->seps[i].seid, media, type,
+			discover->seps[i].inuse ? "*" : "");
+		avdtp_get_caps(sk, discover->seps[i].seid);
+	}
+
+	return 0;
+}
+
+static int l2cap_connect(bdaddr_t *src, bdaddr_t *dst)
+{
+	struct sockaddr_l2 l2a;
+	int sk;
+
+	memset(&l2a, 0, sizeof(l2a));
+	l2a.l2_family = AF_BLUETOOTH;
+	bacpy(&l2a.l2_bdaddr, src);
+
+	sk = socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
+	if (sk < 0) {
+		printf("Cannot create L2CAP socket. %s(%d)\n", strerror(errno),
+				errno);
+		return -errno;
+	}
+
+	if (bind(sk, (struct sockaddr *) &l2a, sizeof(l2a)) < 0) {
+		printf("Bind failed. %s (%d)\n", strerror(errno), errno);
+		return -errno;
+	}
+
+	memset(&l2a, 0, sizeof(l2a));
+	l2a.l2_family = AF_BLUETOOTH;
+	bacpy(&l2a.l2_bdaddr, dst);
+	l2a.l2_psm = htobs(AVDTP_PSM);
+
+	if (connect(sk, (struct sockaddr *) &l2a, sizeof(l2a)) < 0) {
+		printf("Connect failed. %s(%d)\n", strerror(errno), errno);
+		return -errno;
+	}
+
+	return sk;
+}
+
+static void usage()
+{
+	printf("avinfo - Audio/Video Info Tool ver %s\n", VERSION);
+	printf("Usage:\n"
+		"\tavinfo <remote address>\n");
+}
+
+static struct option main_options[] = {
+	{ "help",	0, 0, 'h' },
+	{ 0, 0, 0, 0 }
+};
+
+int main(int argc, char *argv[])
+{
+	bdaddr_t src, dst;
+	int opt, sk, dev_id;
+
+	if (argc < 2) {
+		usage();
+		exit(0);
+	}
+
+	while ((opt = getopt_long(argc, argv, "h", main_options, NULL)) != -1) {
+		switch (opt) {
+		case 'h':
+		default:
+			usage();
+			exit(0);
+		}
+	}
+
+	bacpy(&src, BDADDR_ANY);
+	dev_id = hci_get_route(&src);
+	if ((dev_id < 0) || (hci_devba(dev_id, &src) < 0)) {
+		printf("Cannot find any local adapter\n");
+		exit(-1);
+	}
+
+	printf("Connecting ... \n");
+
+	if (bachk(argv[1]) < 0) {
+		printf("Invalid argument\n");
+		exit(1);
+	}
+
+	str2ba(argv[1], &dst);
+	sk = l2cap_connect(&src, &dst);
+	if (sk < 0)
+		exit(1);
+
+	if (avdtp_discover(sk) < 0)
+		exit(1);
+
+	return 0;
+}