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 || err == -ECONNRESET) {
data->server.fd = -1;
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_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);
//SW3-CONN-DL-TT466_ramdump--[
//if (err == -EPIPE)
//SW3-CONN-DL-TT466_ramdump--]
//SW3-CONN-DL-TT466_ramdump++[
if (err == -EPIPE || err == -ECONNRESET) {
data->server.fd = -1;
//SW3-CONN-DL-TT466_ramdump++]
bluetooth_close(data);
//SW3-CONN-DL-TT466_ramdump++[
}
//SW3-CONN-DL-TT466_ramdump++]
}
return err;
}
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 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;
}
void device_close(struct gn_statemachine *state)
{
dprintf("Serial device: closing device\n");
switch (state->device.type) {
case GN_CT_DKU2:
case GN_CT_Serial:
case GN_CT_Infrared:
serial_close(state->device.fd, state);
break;
case GN_CT_Irda:
irda_close(state->device.fd, state);
break;
case GN_CT_Bluetooth:
bluetooth_close(state->device.fd, state);
break;
case GN_CT_Tekram:
tekram_close(state->device.fd, state);
break;
case GN_CT_TCP:
tcp_close(state->device.fd, state);
break;
case GN_CT_DKU2LIBUSB:
fbusdku2usb_close(state);
break;
case GN_CT_SOCKETPHONET:
socketphonet_close(state);
break;
default:
break;
}
if (state->device.device_instance) {
free(state->device.device_instance);
state->device.device_instance = NULL;
}
}
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;
#ifdef TN_JPN_NTT_BT_SCMS-T
payload = (struct rtp_payload *)(data->buffer + sizeof(*header) + data->sizeof_scms_t);
memset(data->buffer, 0, sizeof(*header) + sizeof(*payload) + data->sizeof_scms_t);
if (data->sizeof_scms_t) {
/*
* In theory, this should be setable on the fly to 0x01 to "protect"
* the stream and 0x00 to allow capture by remote device. In practice,
* all I've ever seem is "protect", or 0x01.
*/
data->buffer[sizeof(*header)] = SCMS_T_COPY_NOT_ALLOWED;
}
#else
payload = (struct rtp_payload *)(data->buffer + sizeof(*header));
memset(data->buffer, 0, sizeof(*header) + sizeof(*payload));
#endif
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 */
#ifdef TN_JPN_NTT_BT_SCMS-T
data->count = sizeof(struct rtp_header) + sizeof(struct rtp_payload) + data->sizeof_scms_t;
#else
data->count = sizeof(struct rtp_header) + sizeof(struct rtp_payload);
#endif
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 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));
#ifdef TN_JPN_NTT_BT_SCMS-T
data->count = sizeof(struct rtp_header) + sizeof(struct rtp_payload) + data->sizeof_scms_t;
#else
data->count = sizeof(struct rtp_header) + sizeof(struct rtp_payload);
#endif
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;
}
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;
}
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;
}
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 Time Taken", 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;
}
//DBG("duration: %ld, ahead: %ld", duration, ahead);
//DBG("decrease_bitpool %d",decrease_bitpool);
if(ahead <= -30*1000){
decrease_bitpool++;
if(decrease_bitpool > 2 ) {
DBG("duration: %ld, ahead: %ld", duration, ahead);
VDBG("calibrating Bitpool Decrease");
calibrate_bitpool(data,0);
decrease_bitpool = 0;
avdtp_good_counter=0;
}
} else if(ahead >= 10000){
if(data->sbc.bitpool != data->sbc_capabilities.max_bitpool){
avdtp_good_counter++;
if(avdtp_good_counter > 1000){//about 10 secs
DBG("duration: %ld, ahead: %ld avdtp_good_counter: %ld", duration, ahead, avdtp_good_counter);
calibrate_bitpool(data,1);
skip_avdtp_flow_check=0;
}
}
decrease_bitpool = 0;
}
if (ahead <= -CATCH_UP_TIMEOUT * 1000) {
/* fallen too far behind, don't try to catch up */
DBG("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
pthread_mutex_lock(&data->lock);
if (data->stream.fd == -1) {
pthread_mutex_unlock(&data->lock);
return EBADF;
} else {
ret = send(data->stream.fd, data->buffer, data->count, MSG_NOSIGNAL);
}
pthread_mutex_unlock(&data->lock);
#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 */
}
