/* * * BlueZ - Bluetooth protocol stack for Linux * * Copyright (C) 2006-2007 Nokia Corporation * Copyright (C) 2004-2008 Marcel Holtmann * * * 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 #endif #include #include #include #include #include #include #include #include #include #include #include #include "ipc.h" #include "sbc.h" #include "rtp.h" #include "liba2dp.h" #define LOG_NDEBUG 0 #define LOG_TAG "A2DP" #include #define ENABLE_DEBUG /* #define ENABLE_VERBOSE */ /* #define ENABLE_TIMING */ #define BUFFER_SIZE 2048 #ifdef ENABLE_DEBUG #define DBG LOGD #else #define DBG(fmt, arg...) #endif #ifdef ENABLE_VERBOSE #define VDBG LOGV #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 LOGE /* 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 /* timeout in milliseconds for a2dp_write */ #define WRITE_TIMEOUT 500 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_CONFIGURE, A2DP_CMD_START, A2DP_CMD_STOP, A2DP_CMD_QUIT, } a2dp_command_t; struct bluetooth_data { 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; 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 timeval now; gettimeofday(&now, NULL); return (now.tv_sec * 1000000UL + now.tv_usec); } #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 bluetooth_start(struct bluetooth_data *data) { char c = 'w'; char buf[BT_AUDIO_IPC_PACKET_SIZE]; struct bt_streamstart_req *start_req = (void*) buf; bt_audio_rsp_msg_header_t *rsp_hdr = (void*) buf; struct bt_streamfd_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_AUDIO_IPC_PACKET_SIZE); start_req->h.msg_type = BT_STREAMSTART_REQ; err = audioservice_send(data, &start_req->h); if (err < 0) goto error; err = audioservice_expect(data, &rsp_hdr->msg_h, BT_STREAMSTART_RSP); if (err < 0) goto error; if (rsp_hdr->posix_errno != 0) { ERR("BT_START failed : %s(%d)", strerror(rsp_hdr->posix_errno), rsp_hdr->posix_errno); err = -rsp_hdr->posix_errno; goto error; } err = audioservice_expect(data, &streamfd_ind->h, BT_STREAMFD_IND); 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; } 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: /* set state to A2DP_STATE_INITIALIZED to force reconfiguration */ if (data->state == A2DP_STATE_STARTING) set_state(data, A2DP_STATE_INITIALIZED); return err; } static int bluetooth_stop(struct bluetooth_data *data) { char buf[BT_AUDIO_IPC_PACKET_SIZE]; struct bt_streamstop_req *stop_req = (void*) buf; bt_audio_rsp_msg_header_t *rsp_hdr = (void*) buf; int err; DBG("bluetooth_stop"); data->state = A2DP_STATE_STOPPING; if (data->stream.fd >= 0) { close(data->stream.fd); data->stream.fd = -1; } /* send stop request */ memset(stop_req, 0, BT_AUDIO_IPC_PACKET_SIZE); stop_req->h.msg_type = BT_STREAMSTOP_REQ; err = audioservice_send(data, &stop_req->h); if (err < 0) goto error; err = audioservice_expect(data, &rsp_hdr->msg_h, BT_STREAMSTOP_RSP); if (err < 0) goto error; if (rsp_hdr->posix_errno != 0) { ERR("BT_STREAMSTOP failed : %s(%d)", strerror(rsp_hdr->posix_errno), rsp_hdr->posix_errno); err = -rsp_hdr->posix_errno; 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_AUDIO_IPC_PACKET_SIZE]; bt_audio_rsp_msg_header_t *rsp_hdr = (void*) buf; struct bt_setconfiguration_req *setconf_req = (void*) buf; struct bt_setconfiguration_rsp *setconf_rsp = (void*) buf; int err; err = bluetooth_a2dp_init(data); if (err < 0) return err; memset(setconf_req, 0, BT_AUDIO_IPC_PACKET_SIZE); setconf_req->h.msg_type = BT_SETCONFIGURATION_REQ; strncpy(setconf_req->device, data->address, 18); setconf_req->transport = BT_CAPABILITIES_TRANSPORT_A2DP; setconf_req->sbc_capabilities = data->sbc_capabilities; setconf_req->access_mode = BT_CAPABILITIES_ACCESS_MODE_WRITE; 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, &rsp_hdr->msg_h, BT_SETCONFIGURATION_RSP); if (err < 0) return err; if (rsp_hdr->posix_errno != 0) { ERR("BT_SETCONFIGURATION failed : %s(%d)", strerror(rsp_hdr->posix_errno), rsp_hdr->posix_errno); return -rsp_hdr->posix_errno; } 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 < 0) { /* fallen too far behind, don't try to catch up */ VDBG("ahead < 0, resetting next_write"); data->next_write = 0; } else { /* advance next_write by duration */ 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)); } } else { /* can happen during normal remote disconnect */ VDBG("poll() failed: %d (revents = %d, errno %s)", ret, data->stream.revents, strerror(errno)); } /* 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; VDBG("sending %s", bt_audio_strmsg(msg->msg_type)); if (send(data->server.fd, msg, BT_AUDIO_IPC_PACKET_SIZE, 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; const char *type; err = recv(data->server.fd, inmsg, BT_AUDIO_IPC_PACKET_SIZE, 0); if (err > 0) { type = bt_audio_strmsg(inmsg->msg_type); if (type) { VDBG("Received %s", type); err = 0; } else { err = -EINVAL; ERR("Bogus message type %d " "received from audio service", inmsg->msg_type); } } else { err = -errno; ERR("Error receiving data from audio service: %s(%d)", strerror(errno), errno); if (err == -EPIPE) bluetooth_close(data); } return err; } static int audioservice_expect(struct bluetooth_data *data, bt_audio_msg_header_t *rsp_hdr, int expected_type) { int err = audioservice_recv(data, rsp_hdr); if (err == 0) { if (rsp_hdr->msg_type != expected_type) { err = -EINVAL; ERR("Bogus message %s received while " "%s was expected", bt_audio_strmsg(rsp_hdr->msg_type), bt_audio_strmsg(expected_type)); } } return err; } static int bluetooth_init(struct bluetooth_data *data) { int sk, err; DBG("bluetooth_init"); sk = bt_audio_service_open(); if (sk <= 0) { ERR("bt_audio_service_open failed\n"); return -errno; } data->server.fd = sk; data->server.events = POLLIN; data->state = A2DP_STATE_INITIALIZED; return 0; } static int bluetooth_configure(struct bluetooth_data *data) { char buf[BT_AUDIO_IPC_PACKET_SIZE]; bt_audio_rsp_msg_header_t *rsp_hdr = (void*) buf; struct bt_getcapabilities_req *getcaps_req = (void*) buf; struct bt_getcapabilities_rsp *getcaps_rsp = (void*) buf; int err; DBG("bluetooth_configure"); data->state = A2DP_STATE_CONFIGURING; memset(getcaps_req, 0, BT_AUDIO_IPC_PACKET_SIZE); getcaps_req->h.msg_type = BT_GETCAPABILITIES_REQ; getcaps_req->flags = 0; getcaps_req->flags |= BT_FLAG_AUTOCONNECT; strncpy(getcaps_req->device, data->address, 18); getcaps_req->transport = BT_CAPABILITIES_TRANSPORT_A2DP; err = audioservice_send(data, &getcaps_req->h); if (err < 0) { ERR("audioservice_send failed for BT_GETCAPABILITIES_REQ\n"); goto error; } err = audioservice_expect(data, &rsp_hdr->msg_h, BT_GETCAPABILITIES_RSP); if (err < 0) { ERR("audioservice_expect failed for BT_GETCAPABILITIES_RSP\n"); goto error; } if (rsp_hdr->posix_errno != 0) { ERR("BT_GETCAPABILITIES failed : %s(%d)", strerror(rsp_hdr->posix_errno), rsp_hdr->posix_errno); err = -rsp_hdr->posix_errno; goto error; } if (getcaps_rsp->transport == BT_CAPABILITIES_TRANSPORT_A2DP) data->sbc_capabilities = getcaps_rsp->sbc_capabilities; 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) set_state(data, A2DP_STATE_INITIALIZED); return err; } static void set_state(struct bluetooth_data *data, a2dp_state_t state) { pthread_mutex_lock(&data->mutex); data->state = state; pthread_cond_signal(&data->client_wait); pthread_mutex_unlock(&data->mutex); } static void set_command(struct bluetooth_data *data, a2dp_command_t command) { VDBG("set_command %d\n", command); pthread_mutex_lock(&data->mutex); data->command = command; pthread_cond_signal(&data->thread_wait); 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; while (state != A2DP_STATE_STARTED && !err) { if (state == A2DP_STATE_NONE) return -ENODEV; else if (state == A2DP_STATE_INITIALIZED) set_command(data, A2DP_CMD_CONFIGURE); else if (state == A2DP_STATE_CONFIGURED) set_command(data, A2DP_CMD_START); pthread_mutex_lock(&data->mutex); while ((err = pthread_cond_timedwait(&data->client_wait, &data->mutex, &ts)) == EINTR) ; state = data->state; 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_thread(void *d) { struct bluetooth_data* data = (struct bluetooth_data*)d; DBG("a2dp_thread started"); prctl(PR_SET_NAME, "a2dp_thread", 0, 0, 0); while (1) { a2dp_command_t command; pthread_mutex_lock(&data->mutex); pthread_cond_wait(&data->thread_wait, &data->mutex); command = data->command; pthread_mutex_unlock(&data->mutex); if (data->state == A2DP_STATE_NONE && command != A2DP_CMD_QUIT) bluetooth_init(data); switch (command) { case A2DP_CMD_CONFIGURE: if (data->state != A2DP_STATE_INITIALIZED) break; bluetooth_configure(data); break; case A2DP_CMD_START: if (data->state != A2DP_STATE_CONFIGURED) break; bluetooth_start(data); break; case A2DP_CMD_STOP: if (data->state != A2DP_STATE_STARTED) break; bluetooth_stop(data); break; case A2DP_CMD_QUIT: bluetooth_close(data); sbc_finish(&data->sbc); free(data); goto done; default: break; } } done: 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; 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_wait, NULL); pthread_cond_init(&data->client_wait, NULL); pthread_attr_init(&attr); pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED); pthread_create(&data->thread, &attr, a2dp_thread, data); *dataPtr = data; return 0; error: bluetooth_close(data); 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); // force reconfiguration set_command(data, A2DP_CMD_CONFIGURE); } } 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, 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) { 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); }