/*
*
* BlueZ - Bluetooth protocol stack for Linux
*
* Copyright (C) 2006-2007 Nokia Corporation
* Copyright (C) 2004-2008 Marcel Holtmann <marcel@holtmann.org>
*
*
* 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
#include <stdint.h>
#include <sys/socket.h>
#include <sys/un.h>
#include <signal.h>
#include <limits.h>
#include <fcntl.h>
#include <unistd.h>
#include <pthread.h>
#include <netinet/in.h>
#include <sys/poll.h>
#include <sys/prctl.h>
#include <bluetooth/bluetooth.h>
#include <bluetooth/l2cap.h>
#include "ipc.h"
#include "sbc.h"
#include "rtp.h"
#include "liba2dp.h"
#define LOG_NDEBUG 0
#define LOG_TAG "A2DP"
#include <utils/Log.h>
#define ENABLE_DEBUG
/* #define ENABLE_VERBOSE */
/* #define ENABLE_TIMING */
#define BUFFER_SIZE 2048
#ifdef ENABLE_DEBUG
#define DBG ALOGD
#else
#define DBG(fmt, arg...)
#endif
#ifdef ENABLE_VERBOSE
#define VDBG ALOGV
#else
#define VDBG(fmt, arg...)
#endif
#ifndef MIN
# define MIN(x, y) ((x) < (y) ? (x) : (y))
#endif
#ifndef MAX
# define MAX(x, y) ((x) > (y) ? (x) : (y))
#endif
#define MAX_BITPOOL 64
#define MIN_BITPOOL 2
#define ERR ALOGE
/* Number of packets to buffer in the stream socket */
#define PACKET_BUFFER_COUNT 10
/* timeout in milliseconds to prevent poll() from hanging indefinitely */
#define POLL_TIMEOUT 1000
/* milliseconds of unsucessfull a2dp packets before we stop trying to catch up
* on write()'s and fall-back to metered writes */
#define CATCH_UP_TIMEOUT 200
/* timeout in milliseconds for a2dp_write */
#define WRITE_TIMEOUT 1000
/* timeout in seconds for command socket recv() */
#define RECV_TIMEOUT 5
typedef enum {
A2DP_STATE_NONE = 0,
A2DP_STATE_INITIALIZED,
A2DP_STATE_CONFIGURING,
A2DP_STATE_CONFIGURED,
A2DP_STATE_STARTING,
A2DP_STATE_STARTED,
A2DP_STATE_STOPPING,
} a2dp_state_t;
typedef enum {
A2DP_CMD_NONE = 0,
A2DP_CMD_INIT,
A2DP_CMD_CONFIGURE,
A2DP_CMD_START,
A2DP_CMD_STOP,
A2DP_CMD_QUIT,
} a2dp_command_t;
struct bluetooth_data {
unsigned int link_mtu; /* MTU for transport channel */
struct pollfd stream; /* Audio stream filedescriptor */
struct pollfd server; /* Audio daemon filedescriptor */
a2dp_state_t state; /* Current A2DP state */
a2dp_command_t command; /* Current command for a2dp_thread */
pthread_t thread;
pthread_mutex_t mutex;
int started;
pthread_cond_t thread_start;
pthread_cond_t thread_wait;
pthread_cond_t client_wait;
sbc_capabilities_t sbc_capabilities;
sbc_t sbc; /* Codec data */
int frame_duration; /* length of an SBC frame in microseconds */
int codesize; /* SBC codesize */
int samples; /* Number of encoded samples */
uint8_t buffer[BUFFER_SIZE]; /* Codec transfer buffer */
int count; /* Codec transfer buffer counter */
int nsamples; /* Cumulative number of codec samples */
uint16_t seq_num; /* Cumulative packet sequence */
int frame_count; /* Current frames in buffer*/
char address[20];
int rate;
int channels;
/* used for pacing our writes to the output socket */
uint64_t next_write;
};
static uint64_t get_microseconds()
{
struct timespec now;
clock_gettime(CLOCK_MONOTONIC, &now);
return (now.tv_sec * 1000000UL + now.tv_nsec / 1000UL);
}
#ifdef ENABLE_TIMING
static void print_time(const char* message, uint64_t then, uint64_t now)
{
DBG("%s: %lld us", message, now - then);
}
#endif
static int audioservice_send(struct bluetooth_data *data, const bt_audio_msg_header_t *msg);
static int audioservice_expect(struct bluetooth_data *data, bt_audio_msg_header_t *outmsg,
int expected_type);
static int bluetooth_a2dp_hw_params(struct bluetooth_data *data);
static void set_state(struct bluetooth_data *data, a2dp_state_t state);
static void bluetooth_close(struct bluetooth_data *data)
{
DBG("bluetooth_close");
if (data->server.fd >= 0) {
bt_audio_service_close(data->server.fd);
data->server.fd = -1;
}
if (data->stream.fd >= 0) {
close(data->stream.fd);
data->stream.fd = -1;
}
data->state = A2DP_STATE_NONE;
}
static int l2cap_set_flushable(int fd, int flushable)
{
int flags;
socklen_t len;
len = sizeof(flags);
if (getsockopt(fd, SOL_BLUETOOTH, BT_FLUSHABLE, &flags, &len) < 0)
return -errno;
if (flushable) {
if (flags == BT_FLUSHABLE_ON)
return 0;
flags = BT_FLUSHABLE_ON;
} else {
if (flags == BT_FLUSHABLE_OFF)
return 0;
flags = BT_FLUSHABLE_OFF;
}
if (setsockopt(fd, SOL_BLUETOOTH, L2CAP_LM, &flags, sizeof(flags)) < 0)
return -errno;
return 0;
}
static int bluetooth_start(struct bluetooth_data *data)
{
char c = 'w';
char buf[BT_SUGGESTED_BUFFER_SIZE];
struct bt_start_stream_req *start_req = (void*) buf;
struct bt_start_stream_rsp *start_rsp = (void*) buf;
struct bt_new_stream_ind *streamfd_ind = (void*) buf;
int opt_name, err, bytes;
DBG("bluetooth_start");
data->state = A2DP_STATE_STARTING;
/* send start */
memset(start_req, 0, BT_SUGGESTED_BUFFER_SIZE);
start_req->h.type = BT_REQUEST;
start_req->h.name = BT_START_STREAM;
start_req->h.length = sizeof(*start_req);
err = audioservice_send(data, &start_req->h);
if (err < 0)
goto error;
start_rsp->h.length = sizeof(*start_rsp);
err = audioservice_expect(data, &start_rsp->h, BT_START_STREAM);
if (err < 0)
goto error;
streamfd_ind->h.length = sizeof(*streamfd_ind);
err = audioservice_expect(data, &streamfd_ind->h, BT_NEW_STREAM);
if (err < 0)
goto error;
data->stream.fd = bt_audio_service_get_data_fd(data->server.fd);
if (data->stream.fd < 0) {
ERR("bt_audio_service_get_data_fd failed, errno: %d", errno);
err = -errno;
goto error;
}
l2cap_set_flushable(data->stream.fd, 1);
data->stream.events = POLLOUT;
/* set our socket buffer to the size of PACKET_BUFFER_COUNT packets */
bytes = data->link_mtu * PACKET_BUFFER_COUNT;
setsockopt(data->stream.fd, SOL_SOCKET, SO_SNDBUF, &bytes,
sizeof(bytes));
data->count = sizeof(struct rtp_header) + sizeof(struct rtp_payload);
data->frame_count = 0;
data->samples = 0;
data->nsamples = 0;
data->seq_num = 0;
data->frame_count = 0;
data->next_write = 0;
set_state(data, A2DP_STATE_STARTED);
return 0;
error:
/* close bluetooth connection to force reinit and reconfiguration */
if (data->state == A2DP_STATE_STARTING) {
bluetooth_close(data);
/* notify client that thread is ready for next command */
pthread_cond_signal(&data->client_wait);
}
return err;
}
static int bluetooth_stop(struct bluetooth_data *data)
{
char buf[BT_SUGGESTED_BUFFER_SIZE];
struct bt_stop_stream_req *stop_req = (void*) buf;
struct bt_stop_stream_rsp *stop_rsp = (void*) buf;
int err;
DBG("bluetooth_stop");
data->state = A2DP_STATE_STOPPING;
l2cap_set_flushable(data->stream.fd, 0);
if (data->stream.fd >= 0) {
close(data->stream.fd);
data->stream.fd = -1;
}
/* send stop request */
memset(stop_req, 0, BT_SUGGESTED_BUFFER_SIZE);
stop_req->h.type = BT_REQUEST;
stop_req->h.name = BT_STOP_STREAM;
stop_req->h.length = sizeof(*stop_req);
err = audioservice_send(data, &stop_req->h);
if (err < 0)
goto error;
stop_rsp->h.length = sizeof(*stop_rsp);
err = audioservice_expect(data, &stop_rsp->h, BT_STOP_STREAM);
if (err < 0)
goto error;
error:
if (data->state == A2DP_STATE_STOPPING)
set_state(data, A2DP_STATE_CONFIGURED);
return err;
}
static uint8_t default_bitpool(uint8_t freq, uint8_t mode)
{
switch (freq) {
case BT_SBC_SAMPLING_FREQ_16000:
case BT_SBC_SAMPLING_FREQ_32000:
return 53;
case BT_SBC_SAMPLING_FREQ_44100:
switch (mode) {
case BT_A2DP_CHANNEL_MODE_MONO:
case BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL:
return 31;
case BT_A2DP_CHANNEL_MODE_STEREO:
case BT_A2DP_CHANNEL_MODE_JOINT_STEREO:
return 53;
default:
ERR("Invalid channel mode %u", mode);
return 53;
}
case BT_SBC_SAMPLING_FREQ_48000:
switch (mode) {
case BT_A2DP_CHANNEL_MODE_MONO:
case BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL:
return 29;
case BT_A2DP_CHANNEL_MODE_STEREO:
case BT_A2DP_CHANNEL_MODE_JOINT_STEREO:
return 51;
default:
ERR("Invalid channel mode %u", mode);
return 51;
}
default:
ERR("Invalid sampling freq %u", freq);
return 53;
}
}
static int bluetooth_a2dp_init(struct bluetooth_data *data)
{
sbc_capabilities_t *cap = &data->sbc_capabilities;
unsigned int max_bitpool, min_bitpool;
int dir;
switch (data->rate) {
case 48000:
cap->frequency = BT_SBC_SAMPLING_FREQ_48000;
break;
case 44100:
cap->frequency = BT_SBC_SAMPLING_FREQ_44100;
break;
case 32000:
cap->frequency = BT_SBC_SAMPLING_FREQ_32000;
break;
case 16000:
cap->frequency = BT_SBC_SAMPLING_FREQ_16000;
break;
default:
ERR("Rate %d not supported", data->rate);
return -1;
}
if (data->channels == 2) {
if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_JOINT_STEREO)
cap->channel_mode = BT_A2DP_CHANNEL_MODE_JOINT_STEREO;
else if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_STEREO)
cap->channel_mode = BT_A2DP_CHANNEL_MODE_STEREO;
else if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL)
cap->channel_mode = BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL;
} else {
if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_MONO)
cap->channel_mode = BT_A2DP_CHANNEL_MODE_MONO;
}
if (!cap->channel_mode) {
ERR("No supported channel modes");
return -1;
}
if (cap->block_length & BT_A2DP_BLOCK_LENGTH_16)
cap->block_length = BT_A2DP_BLOCK_LENGTH_16;
else if (cap->block_length & BT_A2DP_BLOCK_LENGTH_12)
cap->block_length = BT_A2DP_BLOCK_LENGTH_12;
else if (cap->block_length & BT_A2DP_BLOCK_LENGTH_8)
cap->block_length = BT_A2DP_BLOCK_LENGTH_8;
else if (cap->block_length & BT_A2DP_BLOCK_LENGTH_4)
cap->block_length = BT_A2DP_BLOCK_LENGTH_4;
else {
ERR("No supported block lengths");
return -1;
}
if (cap->subbands & BT_A2DP_SUBBANDS_8)
cap->subbands = BT_A2DP_SUBBANDS_8;
else if (cap->subbands & BT_A2DP_SUBBANDS_4)
cap->subbands = BT_A2DP_SUBBANDS_4;
else {
ERR("No supported subbands");
return -1;
}
if (cap->allocation_method & BT_A2DP_ALLOCATION_LOUDNESS)
cap->allocation_method = BT_A2DP_ALLOCATION_LOUDNESS;
else if (cap->allocation_method & BT_A2DP_ALLOCATION_SNR)
cap->allocation_method = BT_A2DP_ALLOCATION_SNR;
min_bitpool = MAX(MIN_BITPOOL, cap->min_bitpool);
max_bitpool = MIN(default_bitpool(cap->frequency,
cap->channel_mode),
cap->max_bitpool);
cap->min_bitpool = min_bitpool;
cap->max_bitpool = max_bitpool;
return 0;
}
static void bluetooth_a2dp_setup(struct bluetooth_data *data)
{
sbc_capabilities_t active_capabilities = data->sbc_capabilities;
sbc_reinit(&data->sbc, 0);
if (active_capabilities.frequency & BT_SBC_SAMPLING_FREQ_16000)
data->sbc.frequency = SBC_FREQ_16000;
if (active_capabilities.frequency & BT_SBC_SAMPLING_FREQ_32000)
data->sbc.frequency = SBC_FREQ_32000;
if (active_capabilities.frequency & BT_SBC_SAMPLING_FREQ_44100)
data->sbc.frequency = SBC_FREQ_44100;
if (active_capabilities.frequency & BT_SBC_SAMPLING_FREQ_48000)
data->sbc.frequency = SBC_FREQ_48000;
if (active_capabilities.channel_mode & BT_A2DP_CHANNEL_MODE_MONO)
data->sbc.mode = SBC_MODE_MONO;
if (active_capabilities.channel_mode & BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL)
data->sbc.mode = SBC_MODE_DUAL_CHANNEL;
if (active_capabilities.channel_mode & BT_A2DP_CHANNEL_MODE_STEREO)
data->sbc.mode = SBC_MODE_STEREO;
if (active_capabilities.channel_mode & BT_A2DP_CHANNEL_MODE_JOINT_STEREO)
data->sbc.mode = SBC_MODE_JOINT_STEREO;
data->sbc.allocation = active_capabilities.allocation_method
== BT_A2DP_ALLOCATION_SNR ? SBC_AM_SNR
: SBC_AM_LOUDNESS;
switch (active_capabilities.subbands) {
case BT_A2DP_SUBBANDS_4:
data->sbc.subbands = SBC_SB_4;
break;
case BT_A2DP_SUBBANDS_8:
data->sbc.subbands = SBC_SB_8;
break;
}
switch (active_capabilities.block_length) {
case BT_A2DP_BLOCK_LENGTH_4:
data->sbc.blocks = SBC_BLK_4;
break;
case BT_A2DP_BLOCK_LENGTH_8:
data->sbc.blocks = SBC_BLK_8;
break;
case BT_A2DP_BLOCK_LENGTH_12:
data->sbc.blocks = SBC_BLK_12;
break;
case BT_A2DP_BLOCK_LENGTH_16:
data->sbc.blocks = SBC_BLK_16;
break;
}
data->sbc.bitpool = active_capabilities.max_bitpool;
data->codesize = sbc_get_codesize(&data->sbc);
data->frame_duration = sbc_get_frame_duration(&data->sbc);
DBG("frame_duration: %d us", data->frame_duration);
}
static int bluetooth_a2dp_hw_params(struct bluetooth_data *data)
{
char buf[BT_SUGGESTED_BUFFER_SIZE];
struct bt_open_req *open_req = (void *) buf;
struct bt_open_rsp *open_rsp = (void *) buf;
struct bt_set_configuration_req *setconf_req = (void*) buf;
struct bt_set_configuration_rsp *setconf_rsp = (void*) buf;
int err;
memset(open_req, 0, BT_SUGGESTED_BUFFER_SIZE);
open_req->h.type = BT_REQUEST;
open_req->h.name = BT_OPEN;
open_req->h.length = sizeof(*open_req);
strncpy(open_req->destination, data->address, 18);
open_req->seid = data->sbc_capabilities.capability.seid;
open_req->lock = BT_WRITE_LOCK;
err = audioservice_send(data, &open_req->h);
if (err < 0)
return err;
open_rsp->h.length = sizeof(*open_rsp);
err = audioservice_expect(data, &open_rsp->h, BT_OPEN);
if (err < 0)
return err;
err = bluetooth_a2dp_init(data);
if (err < 0)
return err;
memset(setconf_req, 0, BT_SUGGESTED_BUFFER_SIZE);
setconf_req->h.type = BT_REQUEST;
setconf_req->h.name = BT_SET_CONFIGURATION;
setconf_req->h.length = sizeof(*setconf_req);
memcpy(&setconf_req->codec, &data->sbc_capabilities,
sizeof(data->sbc_capabilities));
setconf_req->codec.transport = BT_CAPABILITIES_TRANSPORT_A2DP;
setconf_req->codec.length = sizeof(data->sbc_capabilities);
setconf_req->h.length += setconf_req->codec.length - sizeof(setconf_req->codec);
DBG("bluetooth_a2dp_hw_params sending configuration:\n");
switch (data->sbc_capabilities.channel_mode) {
case BT_A2DP_CHANNEL_MODE_MONO:
DBG("\tchannel_mode: MONO\n");
break;
case BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL:
DBG("\tchannel_mode: DUAL CHANNEL\n");
break;
case BT_A2DP_CHANNEL_MODE_STEREO:
DBG("\tchannel_mode: STEREO\n");
break;
case BT_A2DP_CHANNEL_MODE_JOINT_STEREO:
DBG("\tchannel_mode: JOINT STEREO\n");
break;
default:
DBG("\tchannel_mode: UNKNOWN (%d)\n",
data->sbc_capabilities.channel_mode);
}
switch (data->sbc_capabilities.frequency) {
case BT_SBC_SAMPLING_FREQ_16000:
DBG("\tfrequency: 16000\n");
break;
case BT_SBC_SAMPLING_FREQ_32000:
DBG("\tfrequency: 32000\n");
break;
case BT_SBC_SAMPLING_FREQ_44100:
DBG("\tfrequency: 44100\n");
break;
case BT_SBC_SAMPLING_FREQ_48000:
DBG("\tfrequency: 48000\n");
break;
default:
DBG("\tfrequency: UNKNOWN (%d)\n",
data->sbc_capabilities.frequency);
}
switch (data->sbc_capabilities.allocation_method) {
case BT_A2DP_ALLOCATION_SNR:
DBG("\tallocation_method: SNR\n");
break;
case BT_A2DP_ALLOCATION_LOUDNESS:
DBG("\tallocation_method: LOUDNESS\n");
break;
default:
DBG("\tallocation_method: UNKNOWN (%d)\n",
data->sbc_capabilities.allocation_method);
}
switch (data->sbc_capabilities.subbands) {
case BT_A2DP_SUBBANDS_4:
DBG("\tsubbands: 4\n");
break;
case BT_A2DP_SUBBANDS_8:
DBG("\tsubbands: 8\n");
break;
default:
DBG("\tsubbands: UNKNOWN (%d)\n",
data->sbc_capabilities.subbands);
}
switch (data->sbc_capabilities.block_length) {
case BT_A2DP_BLOCK_LENGTH_4:
DBG("\tblock_length: 4\n");
break;
case BT_A2DP_BLOCK_LENGTH_8:
DBG("\tblock_length: 8\n");
break;
case BT_A2DP_BLOCK_LENGTH_12:
DBG("\tblock_length: 12\n");
break;
case BT_A2DP_BLOCK_LENGTH_16:
DBG("\tblock_length: 16\n");
break;
default:
DBG("\tblock_length: UNKNOWN (%d)\n",
data->sbc_capabilities.block_length);
}
DBG("\tmin_bitpool: %d\n", data->sbc_capabilities.min_bitpool);
DBG("\tmax_bitpool: %d\n", data->sbc_capabilities.max_bitpool);
err = audioservice_send(data, &setconf_req->h);
if (err < 0)
return err;
err = audioservice_expect(data, &setconf_rsp->h, BT_SET_CONFIGURATION);
if (err < 0)
return err;
data->link_mtu = setconf_rsp->link_mtu;
DBG("MTU: %d", data->link_mtu);
/* Setup SBC encoder now we agree on parameters */
bluetooth_a2dp_setup(data);
DBG("\tallocation=%u\n\tsubbands=%u\n\tblocks=%u\n\tbitpool=%u\n",
data->sbc.allocation, data->sbc.subbands, data->sbc.blocks,
data->sbc.bitpool);
return 0;
}
static int avdtp_write(struct bluetooth_data *data)
{
int ret = 0;
struct rtp_header *header;
struct rtp_payload *payload;
uint64_t now;
long duration = data->frame_duration * data->frame_count;
#ifdef ENABLE_TIMING
uint64_t begin, end, begin2, end2;
begin = get_microseconds();
#endif
header = (struct rtp_header *)data->buffer;
payload = (struct rtp_payload *)(data->buffer + sizeof(*header));
memset(data->buffer, 0, sizeof(*header) + sizeof(*payload));
payload->frame_count = data->frame_count;
header->v = 2;
header->pt = 1;
header->sequence_number = htons(data->seq_num);
header->timestamp = htonl(data->nsamples);
header->ssrc = htonl(1);
data->stream.revents = 0;
#ifdef ENABLE_TIMING
begin2 = get_microseconds();
#endif
ret = poll(&data->stream, 1, POLL_TIMEOUT);
#ifdef ENABLE_TIMING
end2 = get_microseconds();
print_time("poll", begin2, end2);
#endif
if (ret == 1 && data->stream.revents == POLLOUT) {
long ahead = 0;
now = get_microseconds();
if (data->next_write) {
ahead = data->next_write - now;
#ifdef ENABLE_TIMING
DBG("duration: %ld, ahead: %ld", duration, ahead);
#endif
if (ahead > 0) {
/* too fast, need to throttle */
usleep(ahead);
}
} else {
data->next_write = now;
}
if (ahead <= -CATCH_UP_TIMEOUT * 1000) {
/* fallen too far behind, don't try to catch up */
VDBG("ahead < %d, reseting next_write timestamp", -CATCH_UP_TIMEOUT * 1000);
data->next_write = 0;
} else {
data->next_write += duration;
}
#ifdef ENABLE_TIMING
begin2 = get_microseconds();
#endif
ret = send(data->stream.fd, data->buffer, data->count, MSG_NOSIGNAL);
#ifdef ENABLE_TIMING
end2 = get_microseconds();
print_time("send", begin2, end2);
#endif
if (ret < 0) {
/* can happen during normal remote disconnect */
VDBG("send() failed: %d (errno %s)", ret, strerror(errno));
}
if (ret == -EPIPE) {
bluetooth_close(data);
}
} else {
/* can happen during normal remote disconnect */
VDBG("poll() failed: %d (revents = %d, errno %s)",
ret, data->stream.revents, strerror(errno));
data->next_write = 0;
}
/* Reset buffer of data to send */
data->count = sizeof(struct rtp_header) + sizeof(struct rtp_payload);
data->frame_count = 0;
data->samples = 0;
data->seq_num++;
#ifdef ENABLE_TIMING
end = get_microseconds();
print_time("avdtp_write", begin, end);
#endif
return 0; /* always return success */
}
static int audioservice_send(struct bluetooth_data *data,
const bt_audio_msg_header_t *msg)
{
int err;
uint16_t length;
length = msg->length ? msg->length : BT_SUGGESTED_BUFFER_SIZE;
VDBG("sending %s", bt_audio_strtype(msg->type));
if (send(data->server.fd, msg, length,
MSG_NOSIGNAL) > 0)
err = 0;
else {
err = -errno;
ERR("Error sending data to audio service: %s(%d)",
strerror(errno), errno);
if (err == -EPIPE)
bluetooth_close(data);
}
return err;
}
static int audioservice_recv(struct bluetooth_data *data,
bt_audio_msg_header_t *inmsg)
{
int err, ret;
const char *type, *name;
uint16_t length;
length = inmsg->length ? inmsg->length : BT_SUGGESTED_BUFFER_SIZE;
ret = recv(data->server.fd, inmsg, length, 0);
if (ret < 0) {
err = -errno;
ERR("Error receiving IPC data from bluetoothd: %s (%d)",
strerror(errno), errno);
if (err == -EPIPE)
bluetooth_close(data);
} else if ((size_t) ret < sizeof(bt_audio_msg_header_t)) {
ERR("Too short (%d bytes) IPC packet from bluetoothd", ret);
err = -EINVAL;
} else if (inmsg->type == BT_ERROR) {
bt_audio_error_t *error = (bt_audio_error_t *)inmsg;
ret = recv(data->server.fd, &error->posix_errno,
sizeof(error->posix_errno), 0);
if (ret < 0) {
err = -errno;
ERR("Error receiving error code for BT_ERROR: %s (%d)",
strerror(errno), errno);
if (err == -EPIPE)
bluetooth_close(data);
} else {
ERR("%s failed : %s(%d)",
bt_audio_strname(error->h.name),
strerror(error->posix_errno),
error->posix_errno);
err = -error->posix_errno;
}
} else {
type = bt_audio_strtype(inmsg->type);
name = bt_audio_strname(inmsg->name);
if (type && name) {
DBG("Received %s - %s", type, name);
err = 0;
} else {
err = -EINVAL;
ERR("Bogus message type %d - name %d"
" received from audio service",
inmsg->type, inmsg->name);
}
}
return err;
}
static int audioservice_expect(struct bluetooth_data *data,
bt_audio_msg_header_t *rsp_hdr, int expected_name)
{
int err = audioservice_recv(data, rsp_hdr);
if (err != 0)
return err;
if (rsp_hdr->name != expected_name) {
err = -EINVAL;
ERR("Bogus message %s received while %s was expected",
bt_audio_strname(rsp_hdr->name),
bt_audio_strname(expected_name));
}
return err;
}
static int bluetooth_init(struct bluetooth_data *data)
{
int sk, err;
struct timeval tv = {.tv_sec = RECV_TIMEOUT};
DBG("bluetooth_init");
sk = bt_audio_service_open();
if (sk < 0) {
ERR("bt_audio_service_open failed\n");
return -errno;
}
err = setsockopt(sk, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv));
if (err < 0) {
ERR("bluetooth_init setsockopt(SO_RCVTIMEO) failed %d", err);
return err;
}
data->server.fd = sk;
data->server.events = POLLIN;
data->state = A2DP_STATE_INITIALIZED;
return 0;
}
static int bluetooth_parse_capabilities(struct bluetooth_data *data,
struct bt_get_capabilities_rsp *rsp)
{
int bytes_left = rsp->h.length - sizeof(*rsp);
codec_capabilities_t *codec = (void *) rsp->data;
if (codec->transport != BT_CAPABILITIES_TRANSPORT_A2DP)
return -EINVAL;
while (bytes_left > 0) {
if ((codec->type == BT_A2DP_SBC_SINK) &&
!(codec->lock & BT_WRITE_LOCK))
break;
if (codec->length == 0) {
ERR("bluetooth_parse_capabilities() invalid codec capabilities length");
return -EINVAL;
}
bytes_left -= codec->length;
codec = (codec_capabilities_t *)((char *)codec + codec->length);
}
if (bytes_left <= 0 ||
codec->length != sizeof(data->sbc_capabilities))
return -EINVAL;
memcpy(&data->sbc_capabilities, codec, codec->length);
return 0;
}
static int bluetooth_configure(struct bluetooth_data *data)
{
char buf[BT_SUGGESTED_BUFFER_SIZE];
struct bt_get_capabilities_req *getcaps_req = (void*) buf;
struct bt_get_capabilities_rsp *getcaps_rsp = (void*) buf;
int err;
DBG("bluetooth_configure");
data->state = A2DP_STATE_CONFIGURING;
memset(getcaps_req, 0, BT_SUGGESTED_BUFFER_SIZE);
getcaps_req->h.type = BT_REQUEST;
getcaps_req->h.name = BT_GET_CAPABILITIES;
getcaps_req->flags = 0;
getcaps_req->flags |= BT_FLAG_AUTOCONNECT;
strncpy(getcaps_req->destination, data->address, 18);
getcaps_req->transport = BT_CAPABILITIES_TRANSPORT_A2DP;
getcaps_req->h.length = sizeof(*getcaps_req);
err = audioservice_send(data, &getcaps_req->h);
if (err < 0) {
ERR("audioservice_send failed for BT_GETCAPABILITIES_REQ\n");
goto error;
}
getcaps_rsp->h.length = 0;
err = audioservice_expect(data, &getcaps_rsp->h, BT_GET_CAPABILITIES);
if (err < 0) {
ERR("audioservice_expect failed for BT_GETCAPABILITIES_RSP\n");
goto error;
}
err = bluetooth_parse_capabilities(data, getcaps_rsp);
if (err < 0) {
ERR("bluetooth_parse_capabilities failed err: %d", err);
goto error;
}
err = bluetooth_a2dp_hw_params(data);
if (err < 0) {
ERR("bluetooth_a2dp_hw_params failed err: %d", err);
goto error;
}
set_state(data, A2DP_STATE_CONFIGURED);
return 0;
error:
if (data->state == A2DP_STATE_CONFIGURING) {
bluetooth_close(data);
/* notify client that thread is ready for next command */
pthread_cond_signal(&data->client_wait);
}
return err;
}
static void set_state(struct bluetooth_data *data, a2dp_state_t state)
{
data->state = state;
pthread_cond_signal(&data->client_wait);
}
static void __set_command(struct bluetooth_data *data, a2dp_command_t command)
{
VDBG("set_command %d\n", command);
data->command = command;
pthread_cond_signal(&data->thread_wait);
return;
}
static void set_command(struct bluetooth_data *data, a2dp_command_t command)
{
pthread_mutex_lock(&data->mutex);
__set_command(data, command);
pthread_mutex_unlock(&data->mutex);
}
/* timeout is in milliseconds */
static int wait_for_start(struct bluetooth_data *data, int timeout)
{
a2dp_state_t state = data->state;
struct timeval tv;
struct timespec ts;
int err = 0;
#ifdef ENABLE_TIMING
uint64_t begin, end;
begin = get_microseconds();
#endif
gettimeofday(&tv, (struct timezone *) NULL);
ts.tv_sec = tv.tv_sec + (timeout / 1000);
ts.tv_nsec = (tv.tv_usec + (timeout % 1000) * 1000L ) * 1000L;
pthread_mutex_lock(&data->mutex);
while (state != A2DP_STATE_STARTED) {
if (state == A2DP_STATE_NONE)
__set_command(data, A2DP_CMD_INIT);
else if (state == A2DP_STATE_INITIALIZED)
__set_command(data, A2DP_CMD_CONFIGURE);
else if (state == A2DP_STATE_CONFIGURED) {
__set_command(data, A2DP_CMD_START);
}
again:
err = pthread_cond_timedwait(&data->client_wait, &data->mutex, &ts);
if (err) {
/* don't timeout if we're done */
if (data->state == A2DP_STATE_STARTED) {
err = 0;
break;
}
if (err == ETIMEDOUT)
break;
goto again;
}
if (state == data->state)
goto again;
state = data->state;
if (state == A2DP_STATE_NONE) {
err = ENODEV;
break;
}
}
pthread_mutex_unlock(&data->mutex);
#ifdef ENABLE_TIMING
end = get_microseconds();
print_time("wait_for_start", begin, end);
#endif
/* pthread_cond_timedwait returns positive errors */
return -err;
}
static void a2dp_free(struct bluetooth_data *data)
{
pthread_cond_destroy(&data->client_wait);
pthread_cond_destroy(&data->thread_wait);
pthread_cond_destroy(&data->thread_start);
pthread_mutex_destroy(&data->mutex);
free(data);
return;
}
static void* a2dp_thread(void *d)
{
struct bluetooth_data* data = (struct bluetooth_data*)d;
a2dp_command_t command = A2DP_CMD_NONE;
int err = 0;
DBG("a2dp_thread started");
prctl(PR_SET_NAME, (int)"a2dp_thread", 0, 0, 0);
pthread_mutex_lock(&data->mutex);
data->started = 1;
pthread_cond_signal(&data->thread_start);
while (1)
{
while (1) {
pthread_cond_wait(&data->thread_wait, &data->mutex);
/* Initialization needed */
if (data->state == A2DP_STATE_NONE &&
data->command != A2DP_CMD_QUIT) {
err = bluetooth_init(data);
}
/* New state command signaled */
if (command != data->command) {
command = data->command;
break;
}
}
switch (command) {
case A2DP_CMD_CONFIGURE:
if (data->state != A2DP_STATE_INITIALIZED)
break;
err = bluetooth_configure(data);
break;
case A2DP_CMD_START:
if (data->state != A2DP_STATE_CONFIGURED)
break;
err = bluetooth_start(data);
break;
case A2DP_CMD_STOP:
if (data->state != A2DP_STATE_STARTED)
break;
err = bluetooth_stop(data);
break;
case A2DP_CMD_QUIT:
bluetooth_close(data);
sbc_finish(&data->sbc);
a2dp_free(data);
goto done;
case A2DP_CMD_INIT:
/* already called bluetooth_init() */
default:
break;
}
// reset last command in case of error to allow
// re-execution of the same command
if (err < 0) {
command = A2DP_CMD_NONE;
}
}
done:
pthread_mutex_unlock(&data->mutex);
DBG("a2dp_thread finished");
return NULL;
}
int a2dp_init(int rate, int channels, a2dpData* dataPtr)
{
struct bluetooth_data* data;
pthread_attr_t attr;
int err;
DBG("a2dp_init rate: %d channels: %d", rate, channels);
*dataPtr = NULL;
data = malloc(sizeof(struct bluetooth_data));
if (!data)
return -1;
memset(data, 0, sizeof(struct bluetooth_data));
data->server.fd = -1;
data->stream.fd = -1;
data->state = A2DP_STATE_NONE;
data->command = A2DP_CMD_NONE;
strncpy(data->address, "00:00:00:00:00:00", 18);
data->rate = rate;
data->channels = channels;
sbc_init(&data->sbc, 0);
pthread_mutex_init(&data->mutex, NULL);
pthread_cond_init(&data->thread_start, NULL);
pthread_cond_init(&data->thread_wait, NULL);
pthread_cond_init(&data->client_wait, NULL);
pthread_mutex_lock(&data->mutex);
data->started = 0;
pthread_attr_init(&attr);
pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);
err = pthread_create(&data->thread, &attr, a2dp_thread, data);
if (err) {
/* If the thread create fails we must not wait */
pthread_mutex_unlock(&data->mutex);
err = -err;
goto error;
}
/* Make sure the state machine is ready and waiting */
while (!data->started) {
pthread_cond_wait(&data->thread_start, &data->mutex);
}
/* Poke the state machine to get it going */
pthread_cond_signal(&data->thread_wait);
pthread_mutex_unlock(&data->mutex);
pthread_attr_destroy(&attr);
*dataPtr = data;
return 0;
error:
bluetooth_close(data);
sbc_finish(&data->sbc);
pthread_attr_destroy(&attr);
a2dp_free(data);
return err;
}
void a2dp_set_sink(a2dpData d, const char* address)
{
struct bluetooth_data* data = (struct bluetooth_data*)d;
if (strncmp(data->address, address, 18)) {
strncpy(data->address, address, 18);
set_command(data, A2DP_CMD_INIT);
}
}
int a2dp_write(a2dpData d, const void* buffer, int count)
{
struct bluetooth_data* data = (struct bluetooth_data*)d;
uint8_t* src = (uint8_t *)buffer;
int codesize;
int err, ret = 0;
long frames_left = count;
int encoded;
unsigned int written;
const char *buff;
int did_configure = 0;
#ifdef ENABLE_TIMING
uint64_t begin, end;
DBG("********** a2dp_write **********");
begin = get_microseconds();
#endif
err = wait_for_start(data, WRITE_TIMEOUT);
if (err < 0)
return err;
codesize = data->codesize;
while (frames_left >= codesize) {
/* Enough data to encode (sbc wants 512 byte blocks) */
encoded = sbc_encode(&(data->sbc), src, codesize,
data->buffer + data->count,
sizeof(data->buffer) - data->count,
&written);
if (encoded <= 0) {
ERR("Encoding error %d", encoded);
goto done;
}
VDBG("sbc_encode returned %d, codesize: %d, written: %d\n",
encoded, codesize, written);
src += encoded;
data->count += written;
data->frame_count++;
data->samples += encoded;
data->nsamples += encoded;
/* No space left for another frame then send */
if ((data->count + written >= data->link_mtu) ||
(data->count + written >= BUFFER_SIZE)) {
VDBG("sending packet %d, count %d, link_mtu %u",
data->seq_num, data->count,
data->link_mtu);
err = avdtp_write(data);
if (err < 0)
return err;
}
ret += encoded;
frames_left -= encoded;
}
if (frames_left > 0)
ERR("%ld bytes left at end of a2dp_write\n", frames_left);
done:
#ifdef ENABLE_TIMING
end = get_microseconds();
print_time("a2dp_write total", begin, end);
#endif
return ret;
}
int a2dp_stop(a2dpData d)
{
struct bluetooth_data* data = (struct bluetooth_data*)d;
DBG("a2dp_stop\n");
if (!data)
return 0;
set_command(data, A2DP_CMD_STOP);
return 0;
}
void a2dp_cleanup(a2dpData d)
{
struct bluetooth_data* data = (struct bluetooth_data*)d;
DBG("a2dp_cleanup\n");
set_command(data, A2DP_CMD_QUIT);
}