Add utility for basic AVDTP testing

1 files changed, 270 insertions, 0 deletions

diff --git a/test/avtest.c b/test/avtest.c
index 6ec6035b0..996aa167d 100644
--- a/test/avtest.c
+++ b/test/avtest.c
@@ -25,7 +25,277 @@
 #include <config.h>
 #endif
 
+#include <stdio.h>
+#include <errno.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <string.h>
+#include <getopt.h>
+#include <sys/socket.h>
+
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/hci.h>
+#include <bluetooth/hci_lib.h>
+#include <bluetooth/l2cap.h>
+
+#define AVDTP_PKT_TYPE_SINGLE		0x00
+
+#define AVDTP_MSG_TYPE_COMMAND		0x00
+#define AVDTP_MSG_TYPE_ACCEPT		0x02
+#define AVDTP_MSG_TYPE_REJECT		0x03
+
+#define AVDTP_DISCOVER			0x01
+#define AVDTP_GET_CAPABILITIES		0x02
+#define AVDTP_SET_CONFIGURATION		0x03
+#define AVDTP_GET_CONFIGURATION		0x04
+
+#define AVDTP_SEP_TYPE_SOURCE		0x00
+#define AVDTP_SEP_TYPE_SINK		0x01
+
+#define AVDTP_MEDIA_TYPE_AUDIO		0x00
+#define AVDTP_MEDIA_TYPE_VIDEO		0x01
+#define AVDTP_MEDIA_TYPE_MULTIMEDIA	0x02
+
+#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));
+
+#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));
+
+#else
+#error "Unknown byte order"
+#endif
+
+static void dump_header(struct avdtp_header *hdr)
+{
+	printf("TL %d PT %d MT %d SI %d\n", hdr->transaction,
+			hdr->packet_type, hdr->message_type, hdr->signal_id);
+}
+
+static void dump_buffer(const unsigned char *buf, int len)
+{
+	int i;
+
+	for (i = 0; i < len; i++)
+		printf("%02x ", buf[i]);
+	printf("\n");
+}
+
+static void process_sigchan(int sk, unsigned char reject)
+{
+	unsigned char buf[672];
+	ssize_t len;
+
+	while (1) {
+		struct avdtp_header *hdr = (void *) buf;
+
+		len = read(sk, buf, sizeof(buf));
+		if (len <= 0) {
+			perror("Read failed");
+			break;
+		}
+
+		dump_buffer(buf, len);
+		dump_header(hdr);
+
+		if (hdr->packet_type != AVDTP_PKT_TYPE_SINGLE) {
+			fprintf(stderr, "Only single packets are supported\n");
+			break;
+		}
+
+		if (hdr->message_type != AVDTP_MSG_TYPE_COMMAND) {
+			fprintf(stderr, "Ignoring non-command messages\n");
+			continue;
+		}
+
+		switch (hdr->signal_id) {
+		case AVDTP_DISCOVER:
+			if (reject == AVDTP_DISCOVER) {
+				hdr->message_type = AVDTP_MSG_TYPE_REJECT;
+				buf[2] = 0x29; /* Unsupported configuration */
+				printf("Rejecting discover command\n");
+				len = write(sk, buf, 3);
+			} else {
+				struct seid_info *sei = (void *) (buf + 2);
+				hdr->message_type = AVDTP_MSG_TYPE_ACCEPT;
+				buf[2] = 0x00;
+				buf[3] = 0x00;
+				sei->seid = 0x01;
+				sei->type = AVDTP_SEP_TYPE_SINK;
+				sei->media_type = AVDTP_MEDIA_TYPE_AUDIO;
+				printf("Accepting discover command\n");
+				len = write(sk, buf, 4);
+			}
+			break;
+
+		case AVDTP_GET_CAPABILITIES:
+			if (reject == AVDTP_GET_CAPABILITIES) {
+				hdr->message_type = AVDTP_MSG_TYPE_REJECT;
+				buf[2] = 0x29; /* Unsupported configuration */
+				printf("Rejecting get capabilties command\n");
+				len = write(sk, buf, 3);
+			} else {
+				hdr->message_type = AVDTP_MSG_TYPE_ACCEPT;
+				buf[2] = 0x01;	/* Media transport category */
+				buf[3] = 0x00;
+				buf[4] = 0x07;	/* Media codec category */
+				buf[5] = 0x06;
+				buf[6] = 0x00;	/* Media type audio */
+				buf[7] = 0x00;	/* Codec SBC */
+				buf[8] = 0x22;	/* 44.1 kHz, stereo */
+				buf[9] = 0x15;	/* 16 blocks, 8 subbands */
+				buf[10] = 0x02;
+				buf[11] = 0x33;
+				printf("Accepting get capabilities command\n");
+				len = write(sk, buf, 12);
+			}
+			break;
+
+		default:
+			buf[1] = 0x00;
+			printf("Unknown command\n");
+			len = write(sk, buf, 2);
+			break;
+		}
+	}
+}
+
+static void do_listen(const bdaddr_t *src, unsigned char reject)
+{
+	struct sockaddr_l2 addr;
+	socklen_t optlen;
+	int sk, nsk;
+
+	sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
+	if (sk < 0) {
+		perror("Can't create socket");
+		return;
+	}
+
+	memset(&addr, 0, sizeof(addr));
+	addr.l2_family = AF_BLUETOOTH;
+	bacpy(&addr.l2_bdaddr, src);
+	addr.l2_psm = htobs(25);
+
+	if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
+		perror("Can't bind socket");
+		goto error;
+	}
+
+	if (listen(sk, 10)) {
+		perror("Can't listen on the socket");
+		goto error;
+	}
+
+	while (1) {
+		memset(&addr, 0, sizeof(addr));
+		optlen = sizeof(addr);
+
+		nsk = accept(sk, (struct sockaddr *) &addr, &optlen);
+		if (nsk < 0) {
+			perror("Accept failed");
+			continue;
+		}
+
+		process_sigchan(nsk, reject);
+
+		close(nsk);
+	}
+
+error:
+	close(sk);
+}
+
+static void usage()
+{
+	printf("avtest - Audio/Video testing ver %s\n", VERSION);
+	printf("Usage:\n"
+		"\tavtest [options]\n");
+	printf("Options:\n"
+		"\t--reject <command>\tReject command\n");
+}
+
+static struct option main_options[] = {
+	{ "help",	0, 0, 'h' },
+	{ "device",	1, 0, 'i' },
+	{ "reject",	1, 0, 'r' },
+	{ 0, 0, 0, 0 }
+};
+
 int main(int argc, char *argv[])
 {
+	unsigned char reject = 0x00;
+	bdaddr_t src, dst;
+	int opt;
+
+	bacpy(&src, BDADDR_ANY);
+	bacpy(&dst, BDADDR_ANY);
+
+	while ((opt = getopt_long(argc, argv, "+i:r:h",
+						main_options, NULL)) != EOF) {
+		switch (opt) {
+		case 'i':
+			if (!strncmp(optarg, "hci", 3))
+				hci_devba(atoi(optarg + 3), &src);
+			else
+				str2ba(optarg, &src);
+			break;
+
+		case 'r':
+			if (!strncmp(optarg, "discov", 6))
+				reject = AVDTP_DISCOVER;
+			else if (!strncmp(optarg, "capa", 4))
+				reject = AVDTP_GET_CAPABILITIES;
+			else if (!strncmp(optarg, "getcapa", 7))
+				reject = AVDTP_GET_CAPABILITIES;
+			else if (!strncmp(optarg, "setconf", 7))
+				reject = AVDTP_SET_CONFIGURATION;
+			else if (!strncmp(optarg, "getconf", 7))
+				reject = AVDTP_GET_CONFIGURATION;
+			else
+				reject = atoi(optarg);
+			break;
+
+		case 'h':
+		default:
+			usage();
+			exit(0);
+		}
+	}
+
+	do_listen(&src, reject);
+
 	return 0;
 }