a2dpd_output_a2dp.c

linux蓝牙剖面实现

/*
*
*  BlueZ - Bluetooth protocol stack for Linux
*
*  Copyright (C) 2004-2005  Marcel Holtmann <[e-mail]>
*
*
*  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 FASTTIMEOUTS 1

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <errno.h>
#include <malloc.h>
#include <signal.h>
#include <time.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <sys/time.h>
#include <sys/select.h>
#include <sys/poll.h>

#include <bluetooth/bluetooth.h>
#include <bluetooth/rfcomm.h>
#include <bluetooth/l2cap.h>
#include <bluetooth/sdp.h>
#include <bluetooth/sdp_lib.h>

#include <netinet/in.h>

#include "a2dpd_output_a2dp.h"
#include "a2dpd_tools.h"
#include "a2dpd_protocol.h"
#include "a2dpd_sdp.h"
#include "a2dpd_timer.h"
#include "a2dpd_ipc.h"
#include "sbc.h"
#include "../a2dp.h"

#define A2DPD_FLAGS_NONSPECAUDIO    1
#define NBSDPRETRIESMAX             0
#define BUFS                        2048
#define MAXANSWERCOUNT              2000

//FIXME how should I choose the int_seid??
// The SEID used is the one that is advertised
#define A2DPD_SEID                  1

enum { A2DPD_NOROLE, A2DPD_INITIATOR, A2DPD_ACCEPTOR };

typedef enum {
	AVDTP_STATE_DISCONNECTED,
	AVDTP_STATEX_DISCONNECTING,
	AVDTP_STATEX_SDP_CONNECTING,
	AVDTP_STATEX_SDP_CONNECTING_WAIT,
	AVDTP_STATEX_CTL_CONNECTING,
	AVDTP_STATEX_CTL_CONNECTING_WAIT,
	AVDTP_STATEX_IDLE_CONNECTING,
	AVDTP_STATE_IDLE,
	AVDTP_STATEX_DISCOVERING,
	AVDTP_STATEX_DISCOVERING_RESP,
	AVDTP_STATEX_GETTING_CAPABILITIES,
	AVDTP_STATEX_GETTING_CAPABILITIES_RESP,
	AVDTP_STATEX_SETTING_CONFIGURATION,
	AVDTP_STATEX_SETTING_CONFIGURATION_RESP,
	AVDTP_STATEX_STREAM_OPENING,
	AVDTP_STATEX_STREAM_OPENING_RESP,
	AVDTP_STATE_CONFIGURED,
	AVDTP_STATEX_STREAM_CONNECTING,
	AVDTP_STATEX_STREAM_CONNECTING_WAIT,
	AVDTP_STATE_OPEN_START_STREAMING,
	AVDTP_STATE_OPEN,
	AVDTP_STATEX_STREAM_STARTING,
	AVDTP_STATEX_STREAM_STARTING_WAIT,
	AVDTP_STATEX_STREAM_SUSPENDING,
	AVDTP_STATEX_STREAM_SUSPENDING_WAIT,
	AVDTP_STATEX_STREAM_CLOSING,
	AVDTP_STATEX_STREAM_CLOSING_WAIT,
	AVDTP_STATE_STREAMING,
	AVDTP_STATE_CLOSING,
	AVDTP_STATE_ABORTING,
} AVDTP_STATE;

typedef struct {
	int nb_seid;
	int curr_seid;
	int curr_seid_acp_seid;
	struct sepd_resp resp;
	struct getcap_resp cap_resp;
} discover_state_info;

typedef struct {
	int answer_count;
	discover_state_info discover;
} STATE_INFO;

typedef struct snd_pcm_a2dp {
	A2DPSETTINGS settings;
	bdaddr_t src;
	bdaddr_t dst;
	char bdaddr[18];
	sdp_session_t *sdp_session;
	int role;
	int sk_sdp;
	int sk;
	int control_sk;
	sbc_t sbc;
	int flags;
	int user_flags;
	unsigned char buf[1024];	// contain sbc encoded data, incrementally filled
	unsigned int len;		// number of valid bytes in buf
	unsigned int frame_bytes;	// fixed when initializing

	AVDTP_STATE state;		// State of a2dp
	STATE_INFO state_info;
	char bufe[BUFS];		// temporary encoding buffer
	int lenbufe;			//=0;

	time_t timestamp;		//=0;
	uint16_t seq_num;		//=1;
	int frame_count;		//=0; // Number of sbc frames in one AVDTP packet

	int mtu;			//=A2DPMAXIMUMTRANSFERUNITSIZE
	uint8_t seid;
	unsigned short psm_stream;
	unsigned short psm_cmd;

	// Bluetooth bandwith used
	int bandwithcount;
	struct timeval bandwithtimestamp;

	// Used to control stream from headset
	int pause_writing;	// = 0;
} snd_pcm_a2dp_t;

// In fact sbc blocks are 76 bytes long, so a group of them is either 608 or 684 bytes
// So 650 or 678 makes no differences!
// However some devices may have longer transfer unit up to I saw omtu=733?

#define a2dp_set_state(new_state) do { DBG("State %s", #new_state); a2dp->state = new_state; } while (0)
#define a2dp_filter_state(new_state) do { DBG("State %s", #new_state); a2dp->state = new_state; } while (0)

#define FREQUENCY_16000     (1<<3)
#define FREQUENCY_32000     (1<<2)
#define FREQUENCY_44100     (1<<1)
#define FREQUENCY_48000     (1<<0)
unsigned int frequency_table [] = { 16000, 32000, 44100, 48000 };

#define BLOCK_LENGTH_4      (1<<3)
#define BLOCK_LENGTH_8      (1<<2)
#define BLOCK_LENGTH_12     (1<<1)
#define BLOCK_LENGTH_16     (1<<0)
unsigned int block_length_table [] = { 4, 8, 12, 16 };

#define CHANNEL_MODE_MONO   (1<<3)
#define CHANNEL_MODE_DUAL   (1<<2)
#define CHANNEL_MODE_STEREO (1<<1)
#define CHANNEL_MODE_JOINT  (1<<0)
enum { MONO, DUAL_CHANNEL, STEREO, JOINT_STEREO };
unsigned int channel_mode_table [] = { MONO, DUAL_CHANNEL, STEREO, JOINT_STEREO };

#define ALLOCATION_SNR      (1<<1)
#define ALLOCATION_LOUDNESS (1<<0)
enum { LOUDNESS, SNR };
unsigned int allocation_method_table [] = { LOUDNESS, SNR };

#define SUBBANDS_4          (1<<1)
#define SUBBANDS_8          (1<<0)
unsigned int subbands_table [] = { 4, 8 };

char* get_msgtype(int sigval)
{
	char*sigtxt = "Unknown";
	if(sigval==MESSAGE_TYPE_COMMAND)
		sigtxt="MESSAGE_TYPE_COMMAND";
	if(sigval==MESSAGE_TYPE_ACCEPT)
		sigtxt="MESSAGE_TYPE_ACCEPT";
	if(sigval==MESSAGE_TYPE_REJECT)
		sigtxt="MESSAGE_TYPE_REJECT";
	return sigtxt;
}

char* get_signal(int sigval)
{
	char*sigtxt = "Unknown";
	if(sigval==AVDTP_DISCOVER)
		sigtxt="AVDTP_DISCOVER";
	if(sigval==AVDTP_GET_CAPABILITIES)
		sigtxt="AVDTP_GET_CAPABILITIES";
	if(sigval==AVDTP_SET_CONFIGURATION)
		sigtxt="AVDTP_SET_CONFIGURATION";
	if(sigval==AVDTP_GET_CONFIGURATION)
		sigtxt="AVDTP_GET_CONFIGURATION";
	if(sigval==AVDTP_RECONFIGURE)
		sigtxt="AVDTP_RECONFIGURE";
	if(sigval==AVDTP_OPEN)
		sigtxt="AVDTP_OPEN";
	if(sigval==AVDTP_START)
		sigtxt="AVDTP_START";
	if(sigval==AVDTP_CLOSE)
		sigtxt="AVDTP_CLOSE";
	if(sigval==AVDTP_SUSPEND)
		sigtxt="AVDTP_SUSPEND";
	if(sigval==AVDTP_ABORT)
		sigtxt="AVDTP_ABORT";
	if(sigval==AVDTP_SECURITY_CONTROL)
		sigtxt="AVDTP_SECURITY_CONTROL";
	return sigtxt;
}


void memcpy_changeendian(void *dst, const void *src, int size)
{
	int i;
	const uint16_t *ptrsrc = src;
	uint16_t *ptrdst = dst;
	for (i = 0; i < size / 2; i++) {
		*ptrdst++ = htons(*ptrsrc++);
	}
}

// Prepare packet headers
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;

	// clear rfa bits
	header->rfa0 = 0;

	transaction = (transaction + 1) & 0xf;
}

uint16_t get_socket_omtu(int sockfd)
{
	// Get mtu size
	uint16_t mtu = A2DPMAXIMUMTRANSFERUNITSIZE;
	struct l2cap_options opts;
	unsigned int iOptSize = sizeof(opts);
	if(getsockopt(sockfd, SOL_L2CAP, L2CAP_OPTIONS, &opts, &iOptSize) >= 0) {
		if (opts.omtu)
			mtu = opts.omtu;
		if (!(mtu))
			mtu = A2DPMAXIMUMTRANSFERUNITSIZE;

		DBG("Socket %d imtu=%d, omtu=%d, flush_to=%d", sockfd, opts.imtu, opts.omtu, opts.flush_to);
	}
	return mtu;
}

/*
// Analyse the SEIDs the sink has sent to us
int process_seid(int s, struct acp_seid_info *get_seid_resp, unsigned short *psm, sbc_t * sbc)
{
	int v, size;
	int seid = get_seid_resp->acp_seid;
	struct getcap_req put_req;
	struct getcap_resp cap_resp;
	struct set_config s_config;
	struct set_config_resp s_resp;
	struct stream_cmd open_stream;
	struct open_stream_rsp open_resp;

	memset(&put_req, 0, sizeof(put_req));
	init_request(&put_req.header, AVDTP_GET_CAPABILITIES);
	put_req.acp_seid = seid;

	if (write(s, &put_req, sizeof(put_req)) != sizeof(put_req)) {
		DBG("Couldn't request capabilities for SEID = %d", seid);
		return (-1);
	}

	if (read(s, &cap_resp, sizeof(cap_resp)) < sizeof(cap_resp) ||
	    cap_resp.header.message_type == MESSAGE_TYPE_REJECT || cap_resp.media_type != AUDIO_MEDIA_TYPE || cap_resp.media_codec_type != SBC_MEDIA_CODEC_TYPE) {
		DBG("Didn't receive SBC codec parameters (first) for SEID = %d", seid);
		return (-1);
	}

	DBG("Got capabilities response for seid %d", seid);
	DBG("servcap_cap=%d, servcap_len=%d,", cap_resp.serv_cap, cap_resp.serv_cap_len);
	DBG("cap_type=%d, length=%d", cap_resp.cap_type, cap_resp.length);
	DBG("media_type=%d, codec=%d", cap_resp.media_type, cap_resp.media_codec_type);

	memset(&s_config, 0, sizeof(s_config));
	init_request(&s_config.header, AVDTP_SET_CONFIGURATION);
	s_config.serv_cap = MEDIA_TRANSPORT_CATEGORY;
	s_config.acp_seid = seid;
	s_config.int_seid = A2DPD_SEID;
	s_config.cap_type = MEDIA_CODEC;
	s_config.length = 6;
	s_config.media_type = AUDIO_MEDIA_TYPE;
	s_config.media_codec_type = SBC_MEDIA_CODEC_TYPE;

	switch (sbc->channels) {
	case 1:
		v = 8;
		break;
	case 2:
	default:
		v = 2;
		break;
	}
	s_config.codec_elements.sbc_elements.channel_mode = v;

	switch (sbc->rate) {
	case 16000:
		v = 8;
		break;
	case 32000:
		v = 4;
		break;
	case 48000:
		v = 1;
		break;
	case 44100:
	default:
		v = 2;
		break;
	}
	s_config.codec_elements.sbc_elements.frequency = v;
	s_config.codec_elements.sbc_elements.allocation_method = 1 << 1;

	switch (sbc->subbands) {
	case 4:
		v = 2;
		break;
	case 8:
	default:
		v = 1;
		break;
	}
	s_config.codec_elements.sbc_elements.subbands = v;

	switch (sbc->blocks) {
	case 4:
		v = 8;
		break;
	case 8:
		v = 4;
		break;
	case 12:
		v = 2;
		break;
	case 16:
	default:
		v = 1;
		break;
	}
	s_config.codec_elements.sbc_elements.block_length = v;
	s_config.codec_elements.sbc_elements.min_bitpool = cap_resp.codec_elements.sbc_elements.min_bitpool;
	s_config.codec_elements.sbc_elements.max_bitpool = cap_resp.codec_elements.sbc_elements.max_bitpool;

	if (!(cap_resp.codec_elements.sbc_elements.channel_mode & s_config.codec_elements.sbc_elements.channel_mode)) {
		DBG("headset does not support this channel mode");
	}

	if (!(cap_resp.codec_elements.sbc_elements.frequency & s_config.codec_elements.sbc_elements.frequency)) {
		DBG("headset does not support this frequency");
	}

	if (!(cap_resp.codec_elements.sbc_elements.allocation_method & s_config.codec_elements.sbc_elements.allocation_method)) {
		DBG("headset does not support this allocation_method");
	}

	if (!(cap_resp.codec_elements.sbc_elements.subbands & s_config.codec_elements.sbc_elements.subbands)) {
		DBG("headset does not support this subbands setting");
	}

	if (write(s, &s_config, sizeof(s_config)) != sizeof(s_config))
		RETURNERROR("couldn't set config seid = %d", seid);

	size = read(s, &s_resp, sizeof(s_resp));
	DBG("Got Set Configurations Response");

	memset(&open_stream, 0, sizeof(open_stream));
	init_request(&open_stream.header, AVDTP_OPEN);
	open_stream.acp_seid = seid;

	if (write(s, &open_stream, sizeof(open_stream)) != sizeof(open_stream))
		RETURNERROR("Couldn't open stream SEID = %d", seid);

	size = read(s, &open_resp, sizeof(open_resp));
	if(size < sizeof(open_resp) - 1)
		RETURNERROR("Didn't receive open response confirm for SEID = %d", seid);

	if(open_resp.header.message_type == MESSAGE_TYPE_REJECT)
		RETURNERROR("Open Response Rejected for SEID = %d", seid);

	DBG("Got Open Stream Response");

	*psm = 25;
	return 0;
}

// Connecting on PSM 25
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;
	unsigned int opt;
	int tries;

	sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
	if (sk < 0) {
		DBG("Can't create socket.");
		return -1;
	}

	// Set socket to non blocking
	//fcntl(sk, F_SETFL, fcntl(sk, F_GETFL) | O_NONBLOCK);

#ifdef FASTTIMEOUTS
	// Set connection timeout
	struct timeval t = { 3, 0 };