a2play.c

蓝牙blue tooth sco协议栈

/*
 * a2play.c
 * experimenting with sending a2dp audio to a headset
 * Brad Midgley
 * 
 *   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., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
 *
 */
 
#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include <stdio.h>
#include <errno.h>
#include <fcntl.h>
#include <unistd.h>
#include <stdlib.h>
#include <getopt.h>
#include <signal.h>
#include <string.h>
#include <sys/socket.h>

#include <bluetooth/bluetooth.h>
#include <bluetooth/hci.h>
#include <bluetooth/hci_lib.h>
#include <bluetooth/l2cap.h>
#include <bluetooth/sdp.h>
#include <bluetooth/sdp_lib.h>

#include <netinet/in.h>

/* AVDTP structures */

/* packet components */

struct avdtp_header {
	uint8_t packet_type:2;
	uint8_t message_type:2;
	uint8_t transaction_label:4;
	uint8_t signal_id:6;
	uint8_t rfa0:2;
} __attribute__ ((packed));

struct acp_seid_info {
	uint8_t rfa0:1;
	uint8_t inuse0:1;
	uint8_t acp_seid:6;
	uint8_t rfa2:3;
	uint8_t tsep:1;
	uint8_t media_type:4;
} __attribute__ ((packed));

struct sbc_codec_specific_elements {
	// a2dp p. 20
	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));

#define MAX_ADDITIONAL_CODEC 4
#define MAX_ADDITIONAL_CODEC_OCTETS (MAX_ADDITIONAL_CODEC*sizeof(struct acp_seid_info))

/* packets */

struct sepd_req {
	struct avdtp_header header;
} __attribute__ ((packed));

struct sepd_resp {
	struct avdtp_header header;
	struct acp_seid_info infos[1 + MAX_ADDITIONAL_CODEC];
} __attribute__ ((packed));

struct getcap_req {
	struct avdtp_header header;
	uint8_t rfa1:2;
	uint8_t acp_seid:6;
} __attribute__ ((packed));

struct getcap_resp {
	struct avdtp_header header;

	uint8_t serv_cap;
	uint8_t serv_cap_len;

	uint8_t cap_type;
	uint8_t length;
	uint8_t media_type;
	uint8_t media_codec_type;

	struct sbc_codec_specific_elements sbc_elements;

} __attribute__ ((packed));

struct set_config {
	struct avdtp_header header;

	uint8_t rfa0:2;
	uint8_t acp_seid:6;
	uint8_t rfa1:2;
	uint8_t int_seid:6;

	uint8_t serv_cap;
	uint8_t serv_cap_len;

	uint8_t cap_type;
	uint8_t length;
	uint8_t media_type;
	uint8_t media_codec_type;

	struct sbc_codec_specific_elements sbc_elements;

} __attribute__ ((packed));

struct set_config_resp {
	struct avdtp_header header;

	// only present for an error

	uint8_t serv_cat;
	uint8_t error_code;
} __attribute__ ((packed));

struct open_stream_cmd {
	struct avdtp_header header;
	uint8_t rfa0:2;
	uint8_t acp_seid:6;
} __attribute__ ((packed));

struct open_stream_rsp {
	struct avdtp_header header;

	// only present for an error

	uint8_t error;
} __attribute__ ((packed));

struct start_stream_cmd {
	struct avdtp_header header;
	uint8_t rfa0:2;
	uint8_t acp_seid:6;
} __attribute__ ((packed));

struct start_stream_rsp {
	struct avdtp_header header;

	// only present for an error

	uint8_t rfa0:2;
	uint8_t acp_seid:6;
	uint8_t error;
} __attribute__ ((packed));

struct close_stream_cmd {
	struct avdtp_header header;
	uint8_t rfa0:2;
	uint8_t acp_seid:6;
} __attribute__ ((packed));

struct close_stream_rsp {
	struct avdtp_header header;

	// only present for an error

	uint8_t error;
} __attribute__ ((packed));

// this is an rtp, not bluetooth header, so values are big endian
struct media_packet_header {
	uint8_t cc:4;
	uint8_t x:1;
	uint8_t p:1;
	uint8_t v:2;

	uint8_t pt:7;
	uint8_t m:1;

	uint16_t sequence_number;
	uint32_t timestamp;
	uint32_t ssrc;
	uint32_t csrc[0];
} __attribute__ ((packed));

struct media_payload_header {
	uint8_t frame_count:4;
	uint8_t rfa0:1;
	uint8_t is_last_fragment:1;
	uint8_t is_first_fragment:1;
	uint8_t is_fragmented:1;
} __attribute__ ((packed));

// SBC file format header

struct sbc_frame_header {
	uint8_t syncword:8;	/* Sync word */
	uint8_t subbands:1;	/* Subbands */
	uint8_t allocation_method:1;	/* Allocation method */
	uint8_t channel_mode:2;	/* Channel mode */
	uint8_t blocks:2;	/* Blocks */
	uint8_t sampling_frequency:2;	/* Sampling frequency */
	uint8_t bitpool:8;	/* Bitpool */
	uint8_t crc_check:8;	/* CRC check */
} __attribute__ ((packed));

#define AVDTP_DISCOVER 1
#define AVDTP_GET_CAPABILITIES 2
#define AVDTP_SET_CONFIGURATION 3
#define AVDTP_OPEN 6
#define AVDTP_START 7
#define AVDTP_CLOSE 8

#define MEDIA_TRANSPORT_CATEGORY 1
#define MEDIA_CODEC 7

#define SBC_MEDIA_CODEC_TYPE 0
#define AUDIO_MEDIA_TYPE 0

#define PACKET_TYPE_SINGLE 0
#define PACKET_TYPE_START 1
#define PACKET_TYPE_CONTINUE 2
#define PACKET_TYPE_END 3

#define MESSAGE_TYPE_COMMAND 0
#define MESSAGE_TYPE_ACCEPT 2
#define MESSAGE_TYPE_REJECT 3

#define BUFS 1024

#define MEDIA_PACKET_HEADER_LENGTH 14

#define NONSPECAUDIO 1

static volatile int terminate = 0;
static int cmdfd;
static struct sbc_frame_header sbc_info;

static void sig_term(int sig)
{
	terminate = 1;
}

static void usage()
{
	fprintf(stderr, "use: a2play <bdaddr> <filename>\n");
}

static int detect_a2dp(bdaddr_t *src, bdaddr_t *dst, unsigned short *psm, unsigned long *flags)
{
	sdp_session_t *sess;
	sdp_list_t *attrid, *search, *seq, *next;
	sdp_data_t *pdlist;
	uuid_t group;
	uint32_t range = 0x0000ffff;
	int err;

	sess = sdp_connect(src, dst, SDP_RETRY_IF_BUSY);
	if (!sess) {
		printf("Failed to connect to SDP server: %s\n", strerror(errno));
		return -1;
	}

	/* 0x1108->all? 0x1101->rf sink 0x111e->handsfree 0x1108->headset */
	sdp_uuid16_create(&group, 0x110d);
	search = sdp_list_append(0, &group);
	attrid = sdp_list_append(0, &range);
	err = sdp_service_search_attr_req(sess, search,
					SDP_ATTR_REQ_RANGE, attrid, &seq);
	sdp_list_free(search, 0);
	sdp_list_free(attrid, 0);

	if (err) {
		printf("Service Search failed: %s\n", strerror(errno));
		sdp_close(sess);
		return -1;
	}

	for (; seq; seq = next) {
		sdp_record_t *rec = (sdp_record_t *) seq->data;

		printf("Found A2DP Sink\n");
		if (psm)
			*psm = 25;

		next = seq->next;
		free(seq);
		sdp_record_free(rec);
	}

	sdp_uuid16_create(&group, PNP_INFO_SVCLASS_ID);
	search = sdp_list_append(0, &group);
	attrid = sdp_list_append(0, &range);
	err = sdp_service_search_attr_req(sess, search,
					SDP_ATTR_REQ_RANGE, attrid, &seq);
	sdp_list_free(search, 0);
	sdp_list_free(attrid, 0);

	if (err)
		goto done;

	if (flags)
		*flags = 0;

	for (; seq; seq = next) {
		sdp_record_t *rec = (sdp_record_t *) seq->data;
		uint16_t vendor, product, version;

		pdlist = sdp_data_get(rec, 0x0201);
		vendor = pdlist ? pdlist->val.uint16 : 0x0000;

		pdlist = sdp_data_get(rec, 0x0202);
		product = pdlist ? pdlist->val.uint16 : 0x0000;

		pdlist = sdp_data_get(rec, 0x0203);
		version = pdlist ? pdlist->val.uint16 : 0x0000;

		printf("Product ID %04x:%04x:%04x\n", vendor, product, version);

		if (vendor == 0x1310 && product == 0x0100 && version == 0x0104) {
			printf("Enabling GCT media payload workaround\n");
			if (flags)
				*flags |= NONSPECAUDIO;
		}

		next = seq->next;
		free(seq);
		sdp_record_free(rec);
	}

done:
	sdp_close(sess);
	return 0;
}

static int do_connect(bdaddr_t *src, bdaddr_t *dst, unsigned short psm, uint16_t *mtu)
{
	struct sockaddr_l2 addr;
	struct l2cap_options opts;
	int sk, opt;

	sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
	if (sk < 0) {
		fprintf(stderr, "Can't create socket. %s(%d)\n",
			strerror(errno), errno);
		return -1;
	}

	memset(&addr, 0, sizeof(addr));
	addr.l2_family = AF_BLUETOOTH;
	bacpy(&addr.l2_bdaddr, src);
	if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
		fprintf(stderr, "Can't bind socket. %s(%d)\n",
						strerror(errno), errno);
		return -1;
	}

	/* Get default options */
	opt = sizeof(opts);
	if (getsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &opts, &opt) < 0) {
		fprintf(stderr, "Can't get default L2CAP options. %s(%d)\n",
						strerror(errno), errno);
		return -1;
	}

	/* Set new options */
	//opts.omtu = 48;
	//opts.imtu = imtu;
	if (setsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &opts, opt) < 0) {
		fprintf(stderr, "Can't set L2CAP options. %s(%d)\n",
						strerror(errno), errno);
		return -1;
	}

	memset(&addr, 0, sizeof(addr));
	addr.l2_family = AF_BLUETOOTH;
	bacpy(&addr.l2_bdaddr, dst);
	addr.l2_psm = htobs(psm);
	if (connect(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
		fprintf(stderr, "Can't connect to %s. %s(%d)\n",
			batostr(&addr.l2_bdaddr), strerror(errno), errno);
		close(sk);
		return -1;
	}

	opt = sizeof(opts);
	if (getsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &opts, &opt) < 0) {
		fprintf(stderr, "Can't get L2CAP options. %s(%d)\n",
						strerror(errno), errno);
		close(sk);
		return -1;
	}

	fprintf(stderr, "Connected [imtu %d, omtu %d, flush_to %d]\n",
					opts.imtu, opts.omtu, opts.flush_to);

	if (mtu)
		*mtu = opts.omtu;

	return sk;
}

#if 0
static void dump_packet(void *p, int size)
{
	uint8_t *c = (uint8_t *) p;
	while (size-- > 0)
		printf(" %02x\n", *c++);
	printf("\n");
}
#endif

static void init_request(struct avdtp_header * header, int request_id)
{
	static int transaction = 0;

	header->packet_type = PACKET_TYPE_SINGLE;
	header->message_type = MESSAGE_TYPE_COMMAND;
	header->transaction_label = transaction;
	header->signal_id = request_id;