Home | History | Annotate | Download | only in audio
      1 /*
      2  *
      3  *  BlueZ - Bluetooth protocol stack for Linux
      4  *
      5  *  Copyright (C) 2006-2007  Nokia Corporation
      6  *  Copyright (C) 2004-2008  Marcel Holtmann <marcel (at) holtmann.org>
      7  *
      8  *
      9  *  This library is free software; you can redistribute it and/or
     10  *  modify it under the terms of the GNU Lesser General Public
     11  *  License as published by the Free Software Foundation; either
     12  *  version 2.1 of the License, or (at your option) any later version.
     13  *
     14  *  This library is distributed in the hope that it will be useful,
     15  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
     16  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
     17  *  Lesser General Public License for more details.
     18  *
     19  *  You should have received a copy of the GNU Lesser General Public
     20  *  License along with this library; if not, write to the Free Software
     21  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
     22  *
     23  */
     24 
     25 #ifdef HAVE_CONFIG_H
     26 #include <config.h>
     27 #endif
     28 
     29 #include <stdint.h>
     30 #include <sys/socket.h>
     31 #include <sys/un.h>
     32 #include <signal.h>
     33 #include <limits.h>
     34 #include <fcntl.h>
     35 #include <unistd.h>
     36 #include <pthread.h>
     37 
     38 #include <netinet/in.h>
     39 #include <sys/poll.h>
     40 #include <sys/prctl.h>
     41 
     42 #include <bluetooth/bluetooth.h>
     43 #include <bluetooth/l2cap.h>
     44 
     45 #include "ipc.h"
     46 #include "sbc.h"
     47 #include "rtp.h"
     48 #include "liba2dp.h"
     49 
     50 #define LOG_NDEBUG 0
     51 #define LOG_TAG "A2DP"
     52 #include <utils/Log.h>
     53 
     54 #define ENABLE_DEBUG
     55 /* #define ENABLE_VERBOSE */
     56 /* #define ENABLE_TIMING */
     57 
     58 #define BUFFER_SIZE 2048
     59 
     60 #ifdef ENABLE_DEBUG
     61 #define DBG ALOGD
     62 #else
     63 #define DBG(fmt, arg...)
     64 #endif
     65 
     66 #ifdef ENABLE_VERBOSE
     67 #define VDBG ALOGV
     68 #else
     69 #define VDBG(fmt, arg...)
     70 #endif
     71 
     72 #ifndef MIN
     73 # define MIN(x, y) ((x) < (y) ? (x) : (y))
     74 #endif
     75 
     76 #ifndef MAX
     77 # define MAX(x, y) ((x) > (y) ? (x) : (y))
     78 #endif
     79 
     80 #define MAX_BITPOOL 64
     81 #define MIN_BITPOOL 2
     82 
     83 #define ERR ALOGE
     84 
     85 /* Number of packets to buffer in the stream socket */
     86 #define PACKET_BUFFER_COUNT		10
     87 
     88 /* timeout in milliseconds to prevent poll() from hanging indefinitely */
     89 #define POLL_TIMEOUT			1000
     90 
     91 /* milliseconds of unsucessfull a2dp packets before we stop trying to catch up
     92  * on write()'s and fall-back to metered writes */
     93 #define CATCH_UP_TIMEOUT		200
     94 
     95 /* timeout in milliseconds for a2dp_write */
     96 #define WRITE_TIMEOUT			1000
     97 
     98 /* timeout in seconds for command socket recv() */
     99 #define RECV_TIMEOUT			5
    100 
    101 
    102 typedef enum {
    103 	A2DP_STATE_NONE = 0,
    104 	A2DP_STATE_INITIALIZED,
    105 	A2DP_STATE_CONFIGURING,
    106 	A2DP_STATE_CONFIGURED,
    107 	A2DP_STATE_STARTING,
    108 	A2DP_STATE_STARTED,
    109 	A2DP_STATE_STOPPING,
    110 } a2dp_state_t;
    111 
    112 typedef enum {
    113 	A2DP_CMD_NONE = 0,
    114 	A2DP_CMD_INIT,
    115 	A2DP_CMD_CONFIGURE,
    116 	A2DP_CMD_START,
    117 	A2DP_CMD_STOP,
    118 	A2DP_CMD_QUIT,
    119 } a2dp_command_t;
    120 
    121 struct bluetooth_data {
    122 	unsigned int link_mtu;			/* MTU for transport channel */
    123 	struct pollfd stream;			/* Audio stream filedescriptor */
    124 	struct pollfd server;			/* Audio daemon filedescriptor */
    125 	a2dp_state_t state;				/* Current A2DP state */
    126 	a2dp_command_t command;			/* Current command for a2dp_thread */
    127 	pthread_t thread;
    128 	pthread_mutex_t mutex;
    129 	int started;
    130 	pthread_cond_t thread_start;
    131 	pthread_cond_t thread_wait;
    132 	pthread_cond_t client_wait;
    133 
    134 	sbc_capabilities_t sbc_capabilities;
    135 	sbc_t sbc;				/* Codec data */
    136 	int	frame_duration;			/* length of an SBC frame in microseconds */
    137 	int codesize;				/* SBC codesize */
    138 	int samples;				/* Number of encoded samples */
    139 	uint8_t buffer[BUFFER_SIZE];		/* Codec transfer buffer */
    140 	int count;				/* Codec transfer buffer counter */
    141 
    142 	int nsamples;				/* Cumulative number of codec samples */
    143 	uint16_t seq_num;			/* Cumulative packet sequence */
    144 	int frame_count;			/* Current frames in buffer*/
    145 
    146 	char	address[20];
    147 	int	rate;
    148 	int	channels;
    149 
    150 	/* used for pacing our writes to the output socket */
    151 	uint64_t	next_write;
    152 };
    153 
    154 static uint64_t get_microseconds()
    155 {
    156 	struct timespec now;
    157 	clock_gettime(CLOCK_MONOTONIC, &now);
    158 	return (now.tv_sec * 1000000UL + now.tv_nsec / 1000UL);
    159 }
    160 
    161 #ifdef ENABLE_TIMING
    162 static void print_time(const char* message, uint64_t then, uint64_t now)
    163 {
    164 	DBG("%s: %lld us", message, now - then);
    165 }
    166 #endif
    167 
    168 static int audioservice_send(struct bluetooth_data *data, const bt_audio_msg_header_t *msg);
    169 static int audioservice_expect(struct bluetooth_data *data, bt_audio_msg_header_t *outmsg,
    170 				int expected_type);
    171 static int bluetooth_a2dp_hw_params(struct bluetooth_data *data);
    172 static void set_state(struct bluetooth_data *data, a2dp_state_t state);
    173 
    174 
    175 static void bluetooth_close(struct bluetooth_data *data)
    176 {
    177 	DBG("bluetooth_close");
    178 	if (data->server.fd >= 0) {
    179 		bt_audio_service_close(data->server.fd);
    180 		data->server.fd = -1;
    181 	}
    182 
    183 	if (data->stream.fd >= 0) {
    184 		close(data->stream.fd);
    185 		data->stream.fd = -1;
    186 	}
    187 
    188 	data->state = A2DP_STATE_NONE;
    189 }
    190 
    191 static int l2cap_set_flushable(int fd, int flushable)
    192 {
    193 	int flags;
    194 	socklen_t len;
    195 
    196 	len = sizeof(flags);
    197 	if (getsockopt(fd, SOL_BLUETOOTH, BT_FLUSHABLE, &flags, &len) < 0)
    198 		return -errno;
    199 
    200 	if (flushable) {
    201 		if (flags == BT_FLUSHABLE_ON)
    202 			return 0;
    203 		flags = BT_FLUSHABLE_ON;
    204 	} else {
    205 		if (flags == BT_FLUSHABLE_OFF)
    206 			return 0;
    207 		flags = BT_FLUSHABLE_OFF;
    208 	}
    209 
    210 	if (setsockopt(fd, SOL_BLUETOOTH, L2CAP_LM, &flags, sizeof(flags)) < 0)
    211 		return -errno;
    212 
    213 	return 0;
    214 }
    215 
    216 static int bluetooth_start(struct bluetooth_data *data)
    217 {
    218 	char c = 'w';
    219 	char buf[BT_SUGGESTED_BUFFER_SIZE];
    220 	struct bt_start_stream_req *start_req = (void*) buf;
    221 	struct bt_start_stream_rsp *start_rsp = (void*) buf;
    222 	struct bt_new_stream_ind *streamfd_ind = (void*) buf;
    223 	int opt_name, err, bytes;
    224 
    225 	DBG("bluetooth_start");
    226 	data->state = A2DP_STATE_STARTING;
    227 	/* send start */
    228 	memset(start_req, 0, BT_SUGGESTED_BUFFER_SIZE);
    229 	start_req->h.type = BT_REQUEST;
    230 	start_req->h.name = BT_START_STREAM;
    231 	start_req->h.length = sizeof(*start_req);
    232 
    233 
    234 	err = audioservice_send(data, &start_req->h);
    235 	if (err < 0)
    236 		goto error;
    237 
    238 	start_rsp->h.length = sizeof(*start_rsp);
    239 	err = audioservice_expect(data, &start_rsp->h, BT_START_STREAM);
    240 	if (err < 0)
    241 		goto error;
    242 
    243 	streamfd_ind->h.length = sizeof(*streamfd_ind);
    244 	err = audioservice_expect(data, &streamfd_ind->h, BT_NEW_STREAM);
    245 	if (err < 0)
    246 		goto error;
    247 
    248 	data->stream.fd = bt_audio_service_get_data_fd(data->server.fd);
    249 	if (data->stream.fd < 0) {
    250 		ERR("bt_audio_service_get_data_fd failed, errno: %d", errno);
    251 		err = -errno;
    252 		goto error;
    253 	}
    254 	l2cap_set_flushable(data->stream.fd, 1);
    255 	data->stream.events = POLLOUT;
    256 
    257 	/* set our socket buffer to the size of PACKET_BUFFER_COUNT packets */
    258 	bytes = data->link_mtu * PACKET_BUFFER_COUNT;
    259 	setsockopt(data->stream.fd, SOL_SOCKET, SO_SNDBUF, &bytes,
    260 			sizeof(bytes));
    261 
    262 	data->count = sizeof(struct rtp_header) + sizeof(struct rtp_payload);
    263 	data->frame_count = 0;
    264 	data->samples = 0;
    265 	data->nsamples = 0;
    266 	data->seq_num = 0;
    267 	data->frame_count = 0;
    268 	data->next_write = 0;
    269 
    270 	set_state(data, A2DP_STATE_STARTED);
    271 	return 0;
    272 
    273 error:
    274 	/* close bluetooth connection to force reinit and reconfiguration */
    275 	if (data->state == A2DP_STATE_STARTING) {
    276 		bluetooth_close(data);
    277 		/* notify client that thread is ready for next command */
    278 		pthread_cond_signal(&data->client_wait);
    279         }
    280 	return err;
    281 }
    282 
    283 static int bluetooth_stop(struct bluetooth_data *data)
    284 {
    285 	char buf[BT_SUGGESTED_BUFFER_SIZE];
    286 	struct bt_stop_stream_req *stop_req = (void*) buf;
    287 	struct bt_stop_stream_rsp *stop_rsp = (void*) buf;
    288 	int err;
    289 
    290 	DBG("bluetooth_stop");
    291 
    292 	data->state = A2DP_STATE_STOPPING;
    293 	l2cap_set_flushable(data->stream.fd, 0);
    294 	if (data->stream.fd >= 0) {
    295 		close(data->stream.fd);
    296 		data->stream.fd = -1;
    297 	}
    298 
    299 	/* send stop request */
    300 	memset(stop_req, 0, BT_SUGGESTED_BUFFER_SIZE);
    301 	stop_req->h.type = BT_REQUEST;
    302 	stop_req->h.name = BT_STOP_STREAM;
    303 	stop_req->h.length = sizeof(*stop_req);
    304 
    305 	err = audioservice_send(data, &stop_req->h);
    306 	if (err < 0)
    307 		goto error;
    308 
    309 	stop_rsp->h.length = sizeof(*stop_rsp);
    310 	err = audioservice_expect(data, &stop_rsp->h, BT_STOP_STREAM);
    311 	if (err < 0)
    312 		goto error;
    313 
    314 error:
    315 	if (data->state == A2DP_STATE_STOPPING)
    316 		set_state(data, A2DP_STATE_CONFIGURED);
    317 	return err;
    318 }
    319 
    320 static uint8_t default_bitpool(uint8_t freq, uint8_t mode)
    321 {
    322 	switch (freq) {
    323 	case BT_SBC_SAMPLING_FREQ_16000:
    324 	case BT_SBC_SAMPLING_FREQ_32000:
    325 		return 53;
    326 	case BT_SBC_SAMPLING_FREQ_44100:
    327 		switch (mode) {
    328 		case BT_A2DP_CHANNEL_MODE_MONO:
    329 		case BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL:
    330 			return 31;
    331 		case BT_A2DP_CHANNEL_MODE_STEREO:
    332 		case BT_A2DP_CHANNEL_MODE_JOINT_STEREO:
    333 			return 53;
    334 		default:
    335 			ERR("Invalid channel mode %u", mode);
    336 			return 53;
    337 		}
    338 	case BT_SBC_SAMPLING_FREQ_48000:
    339 		switch (mode) {
    340 		case BT_A2DP_CHANNEL_MODE_MONO:
    341 		case BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL:
    342 			return 29;
    343 		case BT_A2DP_CHANNEL_MODE_STEREO:
    344 		case BT_A2DP_CHANNEL_MODE_JOINT_STEREO:
    345 			return 51;
    346 		default:
    347 			ERR("Invalid channel mode %u", mode);
    348 			return 51;
    349 		}
    350 	default:
    351 		ERR("Invalid sampling freq %u", freq);
    352 		return 53;
    353 	}
    354 }
    355 
    356 static int bluetooth_a2dp_init(struct bluetooth_data *data)
    357 {
    358 	sbc_capabilities_t *cap = &data->sbc_capabilities;
    359 	unsigned int max_bitpool, min_bitpool;
    360 	int dir;
    361 
    362 	switch (data->rate) {
    363 	case 48000:
    364 		cap->frequency = BT_SBC_SAMPLING_FREQ_48000;
    365 		break;
    366 	case 44100:
    367 		cap->frequency = BT_SBC_SAMPLING_FREQ_44100;
    368 		break;
    369 	case 32000:
    370 		cap->frequency = BT_SBC_SAMPLING_FREQ_32000;
    371 		break;
    372 	case 16000:
    373 		cap->frequency = BT_SBC_SAMPLING_FREQ_16000;
    374 		break;
    375 	default:
    376 		ERR("Rate %d not supported", data->rate);
    377 		return -1;
    378 	}
    379 
    380 	if (data->channels == 2) {
    381 		if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_JOINT_STEREO)
    382 			cap->channel_mode = BT_A2DP_CHANNEL_MODE_JOINT_STEREO;
    383 		else if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_STEREO)
    384 			cap->channel_mode = BT_A2DP_CHANNEL_MODE_STEREO;
    385 		else if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL)
    386 			cap->channel_mode = BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL;
    387 	} else {
    388 		if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_MONO)
    389 			cap->channel_mode = BT_A2DP_CHANNEL_MODE_MONO;
    390 	}
    391 
    392 	if (!cap->channel_mode) {
    393 		ERR("No supported channel modes");
    394 		return -1;
    395 	}
    396 
    397 	if (cap->block_length & BT_A2DP_BLOCK_LENGTH_16)
    398 		cap->block_length = BT_A2DP_BLOCK_LENGTH_16;
    399 	else if (cap->block_length & BT_A2DP_BLOCK_LENGTH_12)
    400 		cap->block_length = BT_A2DP_BLOCK_LENGTH_12;
    401 	else if (cap->block_length & BT_A2DP_BLOCK_LENGTH_8)
    402 		cap->block_length = BT_A2DP_BLOCK_LENGTH_8;
    403 	else if (cap->block_length & BT_A2DP_BLOCK_LENGTH_4)
    404 		cap->block_length = BT_A2DP_BLOCK_LENGTH_4;
    405 	else {
    406 		ERR("No supported block lengths");
    407 		return -1;
    408 	}
    409 
    410 	if (cap->subbands & BT_A2DP_SUBBANDS_8)
    411 		cap->subbands = BT_A2DP_SUBBANDS_8;
    412 	else if (cap->subbands & BT_A2DP_SUBBANDS_4)
    413 		cap->subbands = BT_A2DP_SUBBANDS_4;
    414 	else {
    415 		ERR("No supported subbands");
    416 		return -1;
    417 	}
    418 
    419 	if (cap->allocation_method & BT_A2DP_ALLOCATION_LOUDNESS)
    420 		cap->allocation_method = BT_A2DP_ALLOCATION_LOUDNESS;
    421 	else if (cap->allocation_method & BT_A2DP_ALLOCATION_SNR)
    422 		cap->allocation_method = BT_A2DP_ALLOCATION_SNR;
    423 
    424 		min_bitpool = MAX(MIN_BITPOOL, cap->min_bitpool);
    425 		max_bitpool = MIN(default_bitpool(cap->frequency,
    426 					cap->channel_mode),
    427 					cap->max_bitpool);
    428 
    429 	cap->min_bitpool = min_bitpool;
    430 	cap->max_bitpool = max_bitpool;
    431 
    432 	return 0;
    433 }
    434 
    435 static void bluetooth_a2dp_setup(struct bluetooth_data *data)
    436 {
    437 	sbc_capabilities_t active_capabilities = data->sbc_capabilities;
    438 
    439 	sbc_reinit(&data->sbc, 0);
    440 
    441 	if (active_capabilities.frequency & BT_SBC_SAMPLING_FREQ_16000)
    442 		data->sbc.frequency = SBC_FREQ_16000;
    443 
    444 	if (active_capabilities.frequency & BT_SBC_SAMPLING_FREQ_32000)
    445 		data->sbc.frequency = SBC_FREQ_32000;
    446 
    447 	if (active_capabilities.frequency & BT_SBC_SAMPLING_FREQ_44100)
    448 		data->sbc.frequency = SBC_FREQ_44100;
    449 
    450 	if (active_capabilities.frequency & BT_SBC_SAMPLING_FREQ_48000)
    451 		data->sbc.frequency = SBC_FREQ_48000;
    452 
    453 	if (active_capabilities.channel_mode & BT_A2DP_CHANNEL_MODE_MONO)
    454 		data->sbc.mode = SBC_MODE_MONO;
    455 
    456 	if (active_capabilities.channel_mode & BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL)
    457 		data->sbc.mode = SBC_MODE_DUAL_CHANNEL;
    458 
    459 	if (active_capabilities.channel_mode & BT_A2DP_CHANNEL_MODE_STEREO)
    460 		data->sbc.mode = SBC_MODE_STEREO;
    461 
    462 	if (active_capabilities.channel_mode & BT_A2DP_CHANNEL_MODE_JOINT_STEREO)
    463 		data->sbc.mode = SBC_MODE_JOINT_STEREO;
    464 
    465 	data->sbc.allocation = active_capabilities.allocation_method
    466 				== BT_A2DP_ALLOCATION_SNR ? SBC_AM_SNR
    467 				: SBC_AM_LOUDNESS;
    468 
    469 	switch (active_capabilities.subbands) {
    470 	case BT_A2DP_SUBBANDS_4:
    471 		data->sbc.subbands = SBC_SB_4;
    472 		break;
    473 	case BT_A2DP_SUBBANDS_8:
    474 		data->sbc.subbands = SBC_SB_8;
    475 		break;
    476 	}
    477 
    478 	switch (active_capabilities.block_length) {
    479 	case BT_A2DP_BLOCK_LENGTH_4:
    480 		data->sbc.blocks = SBC_BLK_4;
    481 		break;
    482 	case BT_A2DP_BLOCK_LENGTH_8:
    483 		data->sbc.blocks = SBC_BLK_8;
    484 		break;
    485 	case BT_A2DP_BLOCK_LENGTH_12:
    486 		data->sbc.blocks = SBC_BLK_12;
    487 		break;
    488 	case BT_A2DP_BLOCK_LENGTH_16:
    489 		data->sbc.blocks = SBC_BLK_16;
    490 		break;
    491 	}
    492 
    493 	data->sbc.bitpool = active_capabilities.max_bitpool;
    494 	data->codesize = sbc_get_codesize(&data->sbc);
    495 	data->frame_duration = sbc_get_frame_duration(&data->sbc);
    496 	DBG("frame_duration: %d us", data->frame_duration);
    497 }
    498 
    499 static int bluetooth_a2dp_hw_params(struct bluetooth_data *data)
    500 {
    501 	char buf[BT_SUGGESTED_BUFFER_SIZE];
    502 	struct bt_open_req *open_req = (void *) buf;
    503 	struct bt_open_rsp *open_rsp = (void *) buf;
    504 	struct bt_set_configuration_req *setconf_req = (void*) buf;
    505 	struct bt_set_configuration_rsp *setconf_rsp = (void*) buf;
    506 	int err;
    507 
    508 	memset(open_req, 0, BT_SUGGESTED_BUFFER_SIZE);
    509 	open_req->h.type = BT_REQUEST;
    510 	open_req->h.name = BT_OPEN;
    511 	open_req->h.length = sizeof(*open_req);
    512 	strncpy(open_req->destination, data->address, 18);
    513 	open_req->seid = data->sbc_capabilities.capability.seid;
    514 	open_req->lock = BT_WRITE_LOCK;
    515 
    516 	err = audioservice_send(data, &open_req->h);
    517 	if (err < 0)
    518 		return err;
    519 
    520 	open_rsp->h.length = sizeof(*open_rsp);
    521 	err = audioservice_expect(data, &open_rsp->h, BT_OPEN);
    522 	if (err < 0)
    523 		return err;
    524 
    525 	err = bluetooth_a2dp_init(data);
    526 	if (err < 0)
    527 		return err;
    528 
    529 
    530 	memset(setconf_req, 0, BT_SUGGESTED_BUFFER_SIZE);
    531 	setconf_req->h.type = BT_REQUEST;
    532 	setconf_req->h.name = BT_SET_CONFIGURATION;
    533 	setconf_req->h.length = sizeof(*setconf_req);
    534 	memcpy(&setconf_req->codec, &data->sbc_capabilities,
    535 						sizeof(data->sbc_capabilities));
    536 
    537 	setconf_req->codec.transport = BT_CAPABILITIES_TRANSPORT_A2DP;
    538 	setconf_req->codec.length = sizeof(data->sbc_capabilities);
    539 	setconf_req->h.length += setconf_req->codec.length - sizeof(setconf_req->codec);
    540 
    541 	DBG("bluetooth_a2dp_hw_params sending configuration:\n");
    542 	switch (data->sbc_capabilities.channel_mode) {
    543 		case BT_A2DP_CHANNEL_MODE_MONO:
    544 			DBG("\tchannel_mode: MONO\n");
    545 			break;
    546 		case BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL:
    547 			DBG("\tchannel_mode: DUAL CHANNEL\n");
    548 			break;
    549 		case BT_A2DP_CHANNEL_MODE_STEREO:
    550 			DBG("\tchannel_mode: STEREO\n");
    551 			break;
    552 		case BT_A2DP_CHANNEL_MODE_JOINT_STEREO:
    553 			DBG("\tchannel_mode: JOINT STEREO\n");
    554 			break;
    555 		default:
    556 			DBG("\tchannel_mode: UNKNOWN (%d)\n",
    557 				data->sbc_capabilities.channel_mode);
    558 	}
    559 	switch (data->sbc_capabilities.frequency) {
    560 		case BT_SBC_SAMPLING_FREQ_16000:
    561 			DBG("\tfrequency: 16000\n");
    562 			break;
    563 		case BT_SBC_SAMPLING_FREQ_32000:
    564 			DBG("\tfrequency: 32000\n");
    565 			break;
    566 		case BT_SBC_SAMPLING_FREQ_44100:
    567 			DBG("\tfrequency: 44100\n");
    568 			break;
    569 		case BT_SBC_SAMPLING_FREQ_48000:
    570 			DBG("\tfrequency: 48000\n");
    571 			break;
    572 		default:
    573 			DBG("\tfrequency: UNKNOWN (%d)\n",
    574 				data->sbc_capabilities.frequency);
    575 	}
    576 	switch (data->sbc_capabilities.allocation_method) {
    577 		case BT_A2DP_ALLOCATION_SNR:
    578 			DBG("\tallocation_method: SNR\n");
    579 			break;
    580 		case BT_A2DP_ALLOCATION_LOUDNESS:
    581 			DBG("\tallocation_method: LOUDNESS\n");
    582 			break;
    583 		default:
    584 			DBG("\tallocation_method: UNKNOWN (%d)\n",
    585 				data->sbc_capabilities.allocation_method);
    586 	}
    587 	switch (data->sbc_capabilities.subbands) {
    588 		case BT_A2DP_SUBBANDS_4:
    589 			DBG("\tsubbands: 4\n");
    590 			break;
    591 		case BT_A2DP_SUBBANDS_8:
    592 			DBG("\tsubbands: 8\n");
    593 			break;
    594 		default:
    595 			DBG("\tsubbands: UNKNOWN (%d)\n",
    596 				data->sbc_capabilities.subbands);
    597 	}
    598 	switch (data->sbc_capabilities.block_length) {
    599 		case BT_A2DP_BLOCK_LENGTH_4:
    600 			DBG("\tblock_length: 4\n");
    601 			break;
    602 		case BT_A2DP_BLOCK_LENGTH_8:
    603 			DBG("\tblock_length: 8\n");
    604 			break;
    605 		case BT_A2DP_BLOCK_LENGTH_12:
    606 			DBG("\tblock_length: 12\n");
    607 			break;
    608 		case BT_A2DP_BLOCK_LENGTH_16:
    609 			DBG("\tblock_length: 16\n");
    610 			break;
    611 		default:
    612 			DBG("\tblock_length: UNKNOWN (%d)\n",
    613 				data->sbc_capabilities.block_length);
    614 	}
    615 	DBG("\tmin_bitpool: %d\n", data->sbc_capabilities.min_bitpool);
    616 	DBG("\tmax_bitpool: %d\n", data->sbc_capabilities.max_bitpool);
    617 
    618 	err = audioservice_send(data, &setconf_req->h);
    619 	if (err < 0)
    620 		return err;
    621 
    622 	err = audioservice_expect(data, &setconf_rsp->h, BT_SET_CONFIGURATION);
    623 	if (err < 0)
    624 		return err;
    625 
    626 	data->link_mtu = setconf_rsp->link_mtu;
    627 	DBG("MTU: %d", data->link_mtu);
    628 
    629 	/* Setup SBC encoder now we agree on parameters */
    630 	bluetooth_a2dp_setup(data);
    631 
    632 	DBG("\tallocation=%u\n\tsubbands=%u\n\tblocks=%u\n\tbitpool=%u\n",
    633 		data->sbc.allocation, data->sbc.subbands, data->sbc.blocks,
    634 		data->sbc.bitpool);
    635 
    636 	return 0;
    637 }
    638 
    639 static int avdtp_write(struct bluetooth_data *data)
    640 {
    641 	int ret = 0;
    642 	struct rtp_header *header;
    643 	struct rtp_payload *payload;
    644 
    645 	uint64_t now;
    646 	long duration = data->frame_duration * data->frame_count;
    647 #ifdef ENABLE_TIMING
    648 	uint64_t begin, end, begin2, end2;
    649 	begin = get_microseconds();
    650 #endif
    651 
    652 	header = (struct rtp_header *)data->buffer;
    653 	payload = (struct rtp_payload *)(data->buffer + sizeof(*header));
    654 
    655 	memset(data->buffer, 0, sizeof(*header) + sizeof(*payload));
    656 
    657 	payload->frame_count = data->frame_count;
    658 	header->v = 2;
    659 	header->pt = 1;
    660 	header->sequence_number = htons(data->seq_num);
    661 	header->timestamp = htonl(data->nsamples);
    662 	header->ssrc = htonl(1);
    663 
    664 	data->stream.revents = 0;
    665 #ifdef ENABLE_TIMING
    666 	begin2 = get_microseconds();
    667 #endif
    668 	ret = poll(&data->stream, 1, POLL_TIMEOUT);
    669 #ifdef ENABLE_TIMING
    670 	end2 = get_microseconds();
    671 	print_time("poll", begin2, end2);
    672 #endif
    673 	if (ret == 1 && data->stream.revents == POLLOUT) {
    674 		long ahead = 0;
    675 		now = get_microseconds();
    676 
    677 		if (data->next_write) {
    678 			ahead = data->next_write - now;
    679 #ifdef ENABLE_TIMING
    680 			DBG("duration: %ld, ahead: %ld", duration, ahead);
    681 #endif
    682 			if (ahead > 0) {
    683 				/* too fast, need to throttle */
    684 				usleep(ahead);
    685 			}
    686 		} else {
    687 			data->next_write = now;
    688 		}
    689 		if (ahead <= -CATCH_UP_TIMEOUT * 1000) {
    690 			/* fallen too far behind, don't try to catch up */
    691 			VDBG("ahead < %d, reseting next_write timestamp", -CATCH_UP_TIMEOUT * 1000);
    692 			data->next_write = 0;
    693 		} else {
    694 			data->next_write += duration;
    695 		}
    696 
    697 #ifdef ENABLE_TIMING
    698 		begin2 = get_microseconds();
    699 #endif
    700 		ret = send(data->stream.fd, data->buffer, data->count, MSG_NOSIGNAL);
    701 #ifdef ENABLE_TIMING
    702 		end2 = get_microseconds();
    703 		print_time("send", begin2, end2);
    704 #endif
    705 		if (ret < 0) {
    706 			/* can happen during normal remote disconnect */
    707 			VDBG("send() failed: %d (errno %s)", ret, strerror(errno));
    708 		}
    709 		if (ret == -EPIPE) {
    710 			bluetooth_close(data);
    711 		}
    712 	} else {
    713 		/* can happen during normal remote disconnect */
    714 		VDBG("poll() failed: %d (revents = %d, errno %s)",
    715 				ret, data->stream.revents, strerror(errno));
    716 		data->next_write = 0;
    717 	}
    718 
    719 	/* Reset buffer of data to send */
    720 	data->count = sizeof(struct rtp_header) + sizeof(struct rtp_payload);
    721 	data->frame_count = 0;
    722 	data->samples = 0;
    723 	data->seq_num++;
    724 
    725 #ifdef ENABLE_TIMING
    726 	end = get_microseconds();
    727 	print_time("avdtp_write", begin, end);
    728 #endif
    729 	return 0; /* always return success */
    730 }
    731 
    732 static int audioservice_send(struct bluetooth_data *data,
    733 		const bt_audio_msg_header_t *msg)
    734 {
    735 	int err;
    736 	uint16_t length;
    737 
    738 	length = msg->length ? msg->length : BT_SUGGESTED_BUFFER_SIZE;
    739 
    740 	VDBG("sending %s", bt_audio_strtype(msg->type));
    741 	if (send(data->server.fd, msg, length,
    742 			MSG_NOSIGNAL) > 0)
    743 		err = 0;
    744 	else {
    745 		err = -errno;
    746 		ERR("Error sending data to audio service: %s(%d)",
    747 			strerror(errno), errno);
    748 		if (err == -EPIPE)
    749 			bluetooth_close(data);
    750 	}
    751 
    752 	return err;
    753 }
    754 
    755 static int audioservice_recv(struct bluetooth_data *data,
    756 		bt_audio_msg_header_t *inmsg)
    757 {
    758 	int err, ret;
    759 	const char *type, *name;
    760 	uint16_t length;
    761 
    762 	length = inmsg->length ? inmsg->length : BT_SUGGESTED_BUFFER_SIZE;
    763 
    764 	ret = recv(data->server.fd, inmsg, length, 0);
    765 	if (ret < 0) {
    766 		err = -errno;
    767 		ERR("Error receiving IPC data from bluetoothd: %s (%d)",
    768 						strerror(errno), errno);
    769 		if (err == -EPIPE)
    770 			bluetooth_close(data);
    771 	} else if ((size_t) ret < sizeof(bt_audio_msg_header_t)) {
    772 		ERR("Too short (%d bytes) IPC packet from bluetoothd", ret);
    773 		err = -EINVAL;
    774 	} else if (inmsg->type == BT_ERROR) {
    775 		bt_audio_error_t *error = (bt_audio_error_t *)inmsg;
    776 		ret = recv(data->server.fd, &error->posix_errno,
    777 				sizeof(error->posix_errno), 0);
    778 		if (ret < 0) {
    779 			err = -errno;
    780 			ERR("Error receiving error code for BT_ERROR: %s (%d)",
    781 						strerror(errno), errno);
    782 			if (err == -EPIPE)
    783 				bluetooth_close(data);
    784 		} else {
    785 			ERR("%s failed : %s(%d)",
    786 					bt_audio_strname(error->h.name),
    787 					strerror(error->posix_errno),
    788 					error->posix_errno);
    789 			err = -error->posix_errno;
    790 		}
    791 	} else {
    792 		type = bt_audio_strtype(inmsg->type);
    793 		name = bt_audio_strname(inmsg->name);
    794 		if (type && name) {
    795 			DBG("Received %s - %s", type, name);
    796 			err = 0;
    797 		} else {
    798 			err = -EINVAL;
    799 			ERR("Bogus message type %d - name %d"
    800 					" received from audio service",
    801 					inmsg->type, inmsg->name);
    802 		}
    803 
    804 	}
    805 	return err;
    806 }
    807 
    808 static int audioservice_expect(struct bluetooth_data *data,
    809 		bt_audio_msg_header_t *rsp_hdr, int expected_name)
    810 {
    811 	int err = audioservice_recv(data, rsp_hdr);
    812 
    813 	if (err != 0)
    814 		return err;
    815 
    816 	if (rsp_hdr->name != expected_name) {
    817 		err = -EINVAL;
    818 		ERR("Bogus message %s received while %s was expected",
    819 				bt_audio_strname(rsp_hdr->name),
    820 				bt_audio_strname(expected_name));
    821 	}
    822 	return err;
    823 
    824 }
    825 
    826 static int bluetooth_init(struct bluetooth_data *data)
    827 {
    828 	int sk, err;
    829 	struct timeval tv = {.tv_sec = RECV_TIMEOUT};
    830 
    831 	DBG("bluetooth_init");
    832 
    833 	sk = bt_audio_service_open();
    834 	if (sk < 0) {
    835 		ERR("bt_audio_service_open failed\n");
    836 		return -errno;
    837 	}
    838 
    839 	err = setsockopt(sk, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv));
    840 	if (err < 0) {
    841 		ERR("bluetooth_init setsockopt(SO_RCVTIMEO) failed %d", err);
    842 		return err;
    843 	}
    844 
    845 	data->server.fd = sk;
    846 	data->server.events = POLLIN;
    847 	data->state = A2DP_STATE_INITIALIZED;
    848 
    849 	return 0;
    850 }
    851 
    852 static int bluetooth_parse_capabilities(struct bluetooth_data *data,
    853 					struct bt_get_capabilities_rsp *rsp)
    854 {
    855 	int bytes_left = rsp->h.length - sizeof(*rsp);
    856 	codec_capabilities_t *codec = (void *) rsp->data;
    857 
    858 	if (codec->transport != BT_CAPABILITIES_TRANSPORT_A2DP)
    859 		return -EINVAL;
    860 
    861 	while (bytes_left > 0) {
    862 		if ((codec->type == BT_A2DP_SBC_SINK) &&
    863 			!(codec->lock & BT_WRITE_LOCK))
    864 			break;
    865 
    866 		if (codec->length == 0) {
    867 			ERR("bluetooth_parse_capabilities() invalid codec capabilities length");
    868 			return -EINVAL;
    869 		}
    870 		bytes_left -= codec->length;
    871 		codec = (codec_capabilities_t *)((char *)codec + codec->length);
    872 	}
    873 
    874 	if (bytes_left <= 0 ||
    875 			codec->length != sizeof(data->sbc_capabilities))
    876 		return -EINVAL;
    877 
    878 	memcpy(&data->sbc_capabilities, codec, codec->length);
    879 	return 0;
    880 }
    881 
    882 
    883 static int bluetooth_configure(struct bluetooth_data *data)
    884 {
    885 	char buf[BT_SUGGESTED_BUFFER_SIZE];
    886 	struct bt_get_capabilities_req *getcaps_req = (void*) buf;
    887 	struct bt_get_capabilities_rsp *getcaps_rsp = (void*) buf;
    888 	int err;
    889 
    890 	DBG("bluetooth_configure");
    891 
    892 	data->state = A2DP_STATE_CONFIGURING;
    893 	memset(getcaps_req, 0, BT_SUGGESTED_BUFFER_SIZE);
    894 	getcaps_req->h.type = BT_REQUEST;
    895 	getcaps_req->h.name = BT_GET_CAPABILITIES;
    896 
    897 	getcaps_req->flags = 0;
    898 	getcaps_req->flags |= BT_FLAG_AUTOCONNECT;
    899 	strncpy(getcaps_req->destination, data->address, 18);
    900 	getcaps_req->transport = BT_CAPABILITIES_TRANSPORT_A2DP;
    901 	getcaps_req->h.length = sizeof(*getcaps_req);
    902 
    903 	err = audioservice_send(data, &getcaps_req->h);
    904 	if (err < 0) {
    905 		ERR("audioservice_send failed for BT_GETCAPABILITIES_REQ\n");
    906 		goto error;
    907 	}
    908 
    909 	getcaps_rsp->h.length = 0;
    910 	err = audioservice_expect(data, &getcaps_rsp->h, BT_GET_CAPABILITIES);
    911 	if (err < 0) {
    912 		ERR("audioservice_expect failed for BT_GETCAPABILITIES_RSP\n");
    913 		goto error;
    914 	}
    915 
    916 	err = bluetooth_parse_capabilities(data, getcaps_rsp);
    917 	if (err < 0) {
    918 		ERR("bluetooth_parse_capabilities failed err: %d", err);
    919 		goto error;
    920 	}
    921 
    922 	err = bluetooth_a2dp_hw_params(data);
    923 	if (err < 0) {
    924 		ERR("bluetooth_a2dp_hw_params failed err: %d", err);
    925 		goto error;
    926 	}
    927 
    928 
    929 	set_state(data, A2DP_STATE_CONFIGURED);
    930 	return 0;
    931 
    932 error:
    933 
    934 	if (data->state == A2DP_STATE_CONFIGURING) {
    935 		bluetooth_close(data);
    936 		/* notify client that thread is ready for next command */
    937 		pthread_cond_signal(&data->client_wait);
    938         }
    939 	return err;
    940 }
    941 
    942 static void set_state(struct bluetooth_data *data, a2dp_state_t state)
    943 {
    944 	data->state = state;
    945 	pthread_cond_signal(&data->client_wait);
    946 }
    947 
    948 static void __set_command(struct bluetooth_data *data, a2dp_command_t command)
    949 {
    950 	VDBG("set_command %d\n", command);
    951 	data->command = command;
    952 	pthread_cond_signal(&data->thread_wait);
    953 	return;
    954 }
    955 
    956 static void set_command(struct bluetooth_data *data, a2dp_command_t command)
    957 {
    958 	pthread_mutex_lock(&data->mutex);
    959 	__set_command(data, command);
    960 	pthread_mutex_unlock(&data->mutex);
    961 }
    962 
    963 /* timeout is in milliseconds */
    964 static int wait_for_start(struct bluetooth_data *data, int timeout)
    965 {
    966 	a2dp_state_t state = data->state;
    967 	struct timeval tv;
    968 	struct timespec ts;
    969 	int err = 0;
    970 
    971 #ifdef ENABLE_TIMING
    972 	uint64_t begin, end;
    973 	begin = get_microseconds();
    974 #endif
    975 
    976 	gettimeofday(&tv, (struct timezone *) NULL);
    977 	ts.tv_sec = tv.tv_sec + (timeout / 1000);
    978 	ts.tv_nsec = (tv.tv_usec + (timeout % 1000) * 1000L ) * 1000L;
    979 
    980 	pthread_mutex_lock(&data->mutex);
    981 	while (state != A2DP_STATE_STARTED) {
    982 		if (state == A2DP_STATE_NONE)
    983 			__set_command(data, A2DP_CMD_INIT);
    984 		else if (state == A2DP_STATE_INITIALIZED)
    985 			__set_command(data, A2DP_CMD_CONFIGURE);
    986 		else if (state == A2DP_STATE_CONFIGURED) {
    987 			__set_command(data, A2DP_CMD_START);
    988 		}
    989 again:
    990 		err = pthread_cond_timedwait(&data->client_wait, &data->mutex, &ts);
    991 		if (err) {
    992 			/* don't timeout if we're done */
    993 			if (data->state == A2DP_STATE_STARTED) {
    994 				err = 0;
    995 				break;
    996 			}
    997 			if (err == ETIMEDOUT)
    998 				break;
    999 			goto again;
   1000 		}
   1001 
   1002 		if (state == data->state)
   1003 			goto again;
   1004 
   1005 		state = data->state;
   1006 
   1007 		if (state == A2DP_STATE_NONE) {
   1008 			err = ENODEV;
   1009 			break;
   1010 		}
   1011 	}
   1012 	pthread_mutex_unlock(&data->mutex);
   1013 
   1014 #ifdef ENABLE_TIMING
   1015 	end = get_microseconds();
   1016 	print_time("wait_for_start", begin, end);
   1017 #endif
   1018 
   1019 	/* pthread_cond_timedwait returns positive errors */
   1020 	return -err;
   1021 }
   1022 
   1023 static void a2dp_free(struct bluetooth_data *data)
   1024 {
   1025 	pthread_cond_destroy(&data->client_wait);
   1026 	pthread_cond_destroy(&data->thread_wait);
   1027 	pthread_cond_destroy(&data->thread_start);
   1028 	pthread_mutex_destroy(&data->mutex);
   1029 	free(data);
   1030 	return;
   1031 }
   1032 
   1033 static void* a2dp_thread(void *d)
   1034 {
   1035 	struct bluetooth_data* data = (struct bluetooth_data*)d;
   1036 	a2dp_command_t command = A2DP_CMD_NONE;
   1037 	int err = 0;
   1038 
   1039 	DBG("a2dp_thread started");
   1040 	prctl(PR_SET_NAME, (int)"a2dp_thread", 0, 0, 0);
   1041 
   1042 	pthread_mutex_lock(&data->mutex);
   1043 
   1044 	data->started = 1;
   1045 	pthread_cond_signal(&data->thread_start);
   1046 
   1047 	while (1)
   1048 	{
   1049 		while (1) {
   1050 			pthread_cond_wait(&data->thread_wait, &data->mutex);
   1051 
   1052 			/* Initialization needed */
   1053 			if (data->state == A2DP_STATE_NONE &&
   1054 				data->command != A2DP_CMD_QUIT) {
   1055 				err = bluetooth_init(data);
   1056 			}
   1057 
   1058 			/* New state command signaled */
   1059 			if (command != data->command) {
   1060 				command = data->command;
   1061 				break;
   1062 			}
   1063 		}
   1064 
   1065 		switch (command) {
   1066 			case A2DP_CMD_CONFIGURE:
   1067 				if (data->state != A2DP_STATE_INITIALIZED)
   1068 					break;
   1069 				err = bluetooth_configure(data);
   1070 				break;
   1071 
   1072 			case A2DP_CMD_START:
   1073 				if (data->state != A2DP_STATE_CONFIGURED)
   1074 					break;
   1075 				err = bluetooth_start(data);
   1076 				break;
   1077 
   1078 			case A2DP_CMD_STOP:
   1079 				if (data->state != A2DP_STATE_STARTED)
   1080 					break;
   1081 				err = bluetooth_stop(data);
   1082 				break;
   1083 
   1084 			case A2DP_CMD_QUIT:
   1085 				bluetooth_close(data);
   1086 				sbc_finish(&data->sbc);
   1087 				a2dp_free(data);
   1088 				goto done;
   1089 
   1090 			case A2DP_CMD_INIT:
   1091 				/* already called bluetooth_init() */
   1092 			default:
   1093 				break;
   1094 		}
   1095 		// reset last command in case of error to allow
   1096 		// re-execution of the same command
   1097 		if (err < 0) {
   1098 			command = A2DP_CMD_NONE;
   1099 		}
   1100 	}
   1101 
   1102 done:
   1103 	pthread_mutex_unlock(&data->mutex);
   1104 	DBG("a2dp_thread finished");
   1105 	return NULL;
   1106 }
   1107 
   1108 int a2dp_init(int rate, int channels, a2dpData* dataPtr)
   1109 {
   1110 	struct bluetooth_data* data;
   1111 	pthread_attr_t attr;
   1112 	int err;
   1113 
   1114 	DBG("a2dp_init rate: %d channels: %d", rate, channels);
   1115 	*dataPtr = NULL;
   1116 	data = malloc(sizeof(struct bluetooth_data));
   1117 	if (!data)
   1118 		return -1;
   1119 
   1120 	memset(data, 0, sizeof(struct bluetooth_data));
   1121 	data->server.fd = -1;
   1122 	data->stream.fd = -1;
   1123 	data->state = A2DP_STATE_NONE;
   1124 	data->command = A2DP_CMD_NONE;
   1125 
   1126 	strncpy(data->address, "00:00:00:00:00:00", 18);
   1127 	data->rate = rate;
   1128 	data->channels = channels;
   1129 
   1130 	sbc_init(&data->sbc, 0);
   1131 
   1132 	pthread_mutex_init(&data->mutex, NULL);
   1133 	pthread_cond_init(&data->thread_start, NULL);
   1134 	pthread_cond_init(&data->thread_wait, NULL);
   1135 	pthread_cond_init(&data->client_wait, NULL);
   1136 
   1137 	pthread_mutex_lock(&data->mutex);
   1138 	data->started = 0;
   1139 
   1140 	pthread_attr_init(&attr);
   1141 	pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);
   1142 
   1143 	err = pthread_create(&data->thread, &attr, a2dp_thread, data);
   1144 	if (err) {
   1145 		/* If the thread create fails we must not wait */
   1146 		pthread_mutex_unlock(&data->mutex);
   1147 		err = -err;
   1148 		goto error;
   1149 	}
   1150 
   1151 	/* Make sure the state machine is ready and waiting */
   1152 	while (!data->started) {
   1153 		pthread_cond_wait(&data->thread_start, &data->mutex);
   1154 	}
   1155 
   1156 	/* Poke the state machine to get it going */
   1157 	pthread_cond_signal(&data->thread_wait);
   1158 
   1159 	pthread_mutex_unlock(&data->mutex);
   1160 	pthread_attr_destroy(&attr);
   1161 
   1162 	*dataPtr = data;
   1163 	return 0;
   1164 error:
   1165 	bluetooth_close(data);
   1166 	sbc_finish(&data->sbc);
   1167 	pthread_attr_destroy(&attr);
   1168 	a2dp_free(data);
   1169 
   1170 	return err;
   1171 }
   1172 
   1173 void a2dp_set_sink(a2dpData d, const char* address)
   1174 {
   1175 	struct bluetooth_data* data = (struct bluetooth_data*)d;
   1176 	if (strncmp(data->address, address, 18)) {
   1177 		strncpy(data->address, address, 18);
   1178 		set_command(data, A2DP_CMD_INIT);
   1179 	}
   1180 }
   1181 
   1182 int a2dp_write(a2dpData d, const void* buffer, int count)
   1183 {
   1184 	struct bluetooth_data* data = (struct bluetooth_data*)d;
   1185 	uint8_t* src = (uint8_t *)buffer;
   1186 	int codesize;
   1187 	int err, ret = 0;
   1188 	long frames_left = count;
   1189 	int encoded;
   1190 	unsigned int written;
   1191 	const char *buff;
   1192 	int did_configure = 0;
   1193 #ifdef ENABLE_TIMING
   1194 	uint64_t begin, end;
   1195 	DBG("********** a2dp_write **********");
   1196 	begin = get_microseconds();
   1197 #endif
   1198 
   1199 	err = wait_for_start(data, WRITE_TIMEOUT);
   1200 	if (err < 0)
   1201 		return err;
   1202 
   1203 	codesize = data->codesize;
   1204 
   1205 	while (frames_left >= codesize) {
   1206 		/* Enough data to encode (sbc wants 512 byte blocks) */
   1207 		encoded = sbc_encode(&(data->sbc), src, codesize,
   1208 					data->buffer + data->count,
   1209 					sizeof(data->buffer) - data->count,
   1210 					&written);
   1211 		if (encoded <= 0) {
   1212 			ERR("Encoding error %d", encoded);
   1213 			goto done;
   1214 		}
   1215 		VDBG("sbc_encode returned %d, codesize: %d, written: %d\n",
   1216 			encoded, codesize, written);
   1217 
   1218 		src += encoded;
   1219 		data->count += written;
   1220 		data->frame_count++;
   1221 		data->samples += encoded;
   1222 		data->nsamples += encoded;
   1223 
   1224 		/* No space left for another frame then send */
   1225 		if ((data->count + written >= data->link_mtu) ||
   1226 				(data->count + written >= BUFFER_SIZE)) {
   1227 			VDBG("sending packet %d, count %d, link_mtu %u",
   1228 					data->seq_num, data->count,
   1229 					data->link_mtu);
   1230 			err = avdtp_write(data);
   1231 			if (err < 0)
   1232 				return err;
   1233 		}
   1234 
   1235 		ret += encoded;
   1236 		frames_left -= encoded;
   1237 	}
   1238 
   1239 	if (frames_left > 0)
   1240 		ERR("%ld bytes left at end of a2dp_write\n", frames_left);
   1241 
   1242 done:
   1243 #ifdef ENABLE_TIMING
   1244 	end = get_microseconds();
   1245 	print_time("a2dp_write total", begin, end);
   1246 #endif
   1247 	return ret;
   1248 }
   1249 
   1250 int a2dp_stop(a2dpData d)
   1251 {
   1252 	struct bluetooth_data* data = (struct bluetooth_data*)d;
   1253 	DBG("a2dp_stop\n");
   1254 	if (!data)
   1255 		return 0;
   1256 
   1257 	set_command(data, A2DP_CMD_STOP);
   1258 	return 0;
   1259 }
   1260 
   1261 void a2dp_cleanup(a2dpData d)
   1262 {
   1263 	struct bluetooth_data* data = (struct bluetooth_data*)d;
   1264 	DBG("a2dp_cleanup\n");
   1265 	set_command(data, A2DP_CMD_QUIT);
   1266 }
   1267