gboolean gst_sbc_enc_fill_sbc_params(GstSbcEnc *enc, GstCaps *caps)
{
if (!gst_caps_is_fixed(caps)) {
GST_DEBUG_OBJECT(enc, "didn't receive fixed caps, "
"returning false");
return FALSE;
}
if (!gst_sbc_util_fill_sbc_params(&enc->sbc, caps))
return FALSE;
if (enc->rate != 0 && gst_sbc_parse_rate_from_sbc(enc->sbc.frequency)
!= enc->rate)
goto fail;
if (enc->channels != 0 && gst_sbc_get_channel_number(enc->sbc.mode)
!= enc->channels)
goto fail;
if (enc->blocks != 0 && gst_sbc_parse_blocks_from_sbc(enc->sbc.blocks)
!= enc->blocks)
goto fail;
if (enc->subbands != 0 && gst_sbc_parse_subbands_from_sbc(
enc->sbc.subbands) != enc->subbands)
goto fail;
if (enc->mode != SBC_ENC_DEFAULT_MODE && enc->sbc.mode != enc->mode)
goto fail;
if (enc->allocation != SBC_AM_AUTO &&
enc->sbc.allocation != enc->allocation)
goto fail;
if (enc->bitpool != SBC_ENC_BITPOOL_AUTO &&
enc->sbc.bitpool != enc->bitpool)
goto fail;
enc->codesize = sbc_get_codesize(&enc->sbc);
enc->frame_length = sbc_get_frame_length(&enc->sbc);
enc->frame_duration = sbc_get_frame_duration(&enc->sbc);
GST_DEBUG_OBJECT(enc, "codesize: %d, frame_length: %d, frame_duration:"
" %d", enc->codesize, enc->frame_length,
enc->frame_duration);
return TRUE;
fail:
memset(&enc->sbc, 0, sizeof(sbc_t));
return FALSE;
}
void *io_thread_a2dp_source_sbc(void *arg) {
struct ba_transport *t = (struct ba_transport *)arg;
pthread_setcancelstate(PTHREAD_CANCEL_DISABLE, NULL);
pthread_cleanup_push(CANCEL_ROUTINE(io_thread_release), t);
sbc_t sbc;
if ((errno = -sbc_init_a2dp(&sbc, 0, t->a2dp.cconfig, t->a2dp.cconfig_size)) != 0) {
error("Couldn't initialize SBC codec: %s", strerror(errno));
goto fail_init;
}
const size_t sbc_codesize = sbc_get_codesize(&sbc);
const size_t sbc_frame_len = sbc_get_frame_length(&sbc);
const unsigned int channels = transport_get_channels(t);
/* Writing MTU should be big enough to contain RTP header, SBC payload
* header and at least one SBC frame. In general, there is no constraint
* for the MTU value, but the speed might suffer significantly. */
size_t mtu_write = t->mtu_write;
if (mtu_write < sizeof(rtp_header_t) + sizeof(rtp_payload_sbc_t) + sbc_frame_len) {
mtu_write = sizeof(rtp_header_t) + sizeof(rtp_payload_sbc_t) + sbc_frame_len;
warn("Writing MTU too small for one single SBC frame: %zu < %zu", t->mtu_write, mtu_write);
}
const size_t in_buffer_size = sbc_codesize * (mtu_write / sbc_frame_len);
const size_t out_buffer_size = mtu_write;
int16_t *in_buffer = malloc(in_buffer_size);
uint8_t *out_buffer = malloc(out_buffer_size);
pthread_cleanup_push(CANCEL_ROUTINE(sbc_finish), &sbc);
pthread_cleanup_push(CANCEL_ROUTINE(free), in_buffer);
pthread_cleanup_push(CANCEL_ROUTINE(free), out_buffer);
if (in_buffer == NULL || out_buffer == NULL) {
error("Couldn't create data buffers: %s", strerror(ENOMEM));
goto fail;
}
uint16_t seq_number = random();
uint32_t timestamp = random();
/* initialize RTP header (the constant part) */
rtp_header_t *rtp_header = (rtp_header_t *)out_buffer;
memset(rtp_header, 0, sizeof(*rtp_header));
rtp_header->version = 2;
rtp_header->paytype = 96;
rtp_payload_sbc_t *rtp_payload;
rtp_payload = (rtp_payload_sbc_t *)&rtp_header->csrc[rtp_header->cc];
memset(rtp_payload, 0, sizeof(*rtp_payload));
/* reading head position and available read length */
int16_t *in_buffer_head = in_buffer;
size_t in_samples = in_buffer_size / sizeof(int16_t);
struct pollfd pfds[] = {
{ t->event_fd, POLLIN, 0 },
{ -1, 0, 0 },
{ -1, 0, 0 },
};
struct io_sync io_sync = {
.sampling = transport_get_sampling(t),
};
debug("Starting IO loop: %s",
bluetooth_profile_to_string(t->profile, t->codec));
for (;;) {
pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL);
ssize_t samples;
int nr_shm_fds = libshm_nr_pollfd(t->a2dp.pcm.shm);
libshm_populate_pollfd(t->a2dp.pcm.shm, pfds + 1);
if (poll(pfds, 1 + nr_shm_fds, -1) == -1) {
error("Transport poll error: %s", strerror(errno));
goto fail;
}
if (pfds[0].revents & POLLIN) {
/* dispatch incoming event */
eventfd_t event;
eventfd_read(pfds[0].fd, &event);
io_sync.frames = 0;
continue;
}
if (libshm_poll(t->a2dp.pcm.shm, pfds + 1, nr_shm_fds) < 0) {
error("SHM poll failed");
goto fail;
}
/* read data from the FIFO - this function will block */
if ((samples = io_thread_read_pcm(&t->a2dp.pcm, in_buffer_head, in_samples)) <= 0) {
if (samples == -1)
error("FIFO read error: %s", strerror(errno));
goto fail;
}
pthread_setcancelstate(PTHREAD_CANCEL_DISABLE, NULL);
/* When the thread is created, there might be no data in the FIFO. In fact
* there might be no data for a long time - until client starts playback.
* In order to correctly calculate time drift, the zero time point has to
* be obtained after the stream has started. */
if (io_sync.frames == 0) {
gettimestamp(&io_sync.ts);
io_sync.ts0 = io_sync.ts;
}
if (!config.a2dp_volume || !t->a2dp.supports_dbus_volume)
/* scale volume or mute audio signal */
io_thread_scale_pcm(t, in_buffer_head, samples, channels);
/* overall input buffer size */
samples += in_buffer_head - in_buffer;
const uint8_t *input = (uint8_t *)in_buffer;
size_t input_len = samples * sizeof(int16_t);
/* encode and transfer obtained data */
while (input_len >= sbc_codesize) {
uint8_t *output = (uint8_t *)(rtp_payload + 1);
size_t output_len = out_buffer_size - (output - out_buffer);
size_t pcm_frames = 0;
size_t sbc_frames = 0;
/* Generate as many SBC frames as possible to fill the output buffer
* without overflowing it. The size of the output buffer is based on
* the socket MTU, so such a transfer should be most efficient. */
while (input_len >= sbc_codesize && output_len >= sbc_frame_len) {
ssize_t len;
ssize_t encoded;
if ((len = sbc_encode(&sbc, input, input_len, output, output_len, &encoded)) < 0) {
error("SBC encoding error: %s", strerror(-len));
break;
}
input += len;
input_len -= len;
output += encoded;
output_len -= encoded;
pcm_frames += len / channels / sizeof(int16_t);
sbc_frames++;
}
rtp_header->seq_number = htons(++seq_number);
rtp_header->timestamp = htonl(timestamp);
rtp_payload->frame_count = sbc_frames;
if (write(t->bt_fd, out_buffer, output - out_buffer) == -1) {
if (errno == ECONNRESET || errno == ENOTCONN) {
/* exit the thread upon BT socket disconnection */
debug("BT socket disconnected");
goto fail;
}
error("BT socket write error: %s", strerror(errno));
}
/* keep data transfer at a constant bit rate, also
* get a timestamp for the next RTP frame */
timestamp += io_thread_time_sync(&io_sync, pcm_frames);
t->delay = io_sync.delay;
}
/* convert bytes length to samples length */
samples = input_len / sizeof(int16_t);
/* If the input buffer was not consumed (due to codesize limit), we
* have to append new data to the existing one. Since we do not use
* ring buffer, we will simply move unprocessed data to the front
* of our linear buffer. */
if (samples > 0 && (uint8_t *)in_buffer != input)
memmove(in_buffer, input, samples * sizeof(int16_t));
/* reposition our reading head */
in_buffer_head = in_buffer + samples;
in_samples = in_buffer_size / sizeof(int16_t) - samples;
}
fail:
pthread_setcancelstate(PTHREAD_CANCEL_DISABLE, NULL);
pthread_cleanup_pop(1);
pthread_cleanup_pop(1);
pthread_cleanup_pop(1);
fail_init:
pthread_cleanup_pop(1);
return NULL;
}
void *io_thread_a2dp_sink_sbc(void *arg) {
struct ba_transport *t = (struct ba_transport *)arg;
/* Cancellation should be possible only in the carefully selected place
* in order to prevent memory leaks and resources not being released. */
pthread_setcancelstate(PTHREAD_CANCEL_DISABLE, NULL);
pthread_cleanup_push(CANCEL_ROUTINE(io_thread_release), t);
if (t->bt_fd == -1) {
error("Invalid BT socket: %d", t->bt_fd);
goto fail_init;
}
/* Check for invalid (e.g. not set) reading MTU. If buffer allocation does
* not return NULL (allocating zero bytes might return NULL), we will read
* zero bytes from the BT socket, which will be wrongly identified as a
* "connection closed" action. */
if (t->mtu_read <= 0) {
error("Invalid reading MTU: %zu", t->mtu_read);
goto fail_init;
}
sbc_t sbc;
if ((errno = -sbc_init_a2dp(&sbc, 0, t->a2dp.cconfig, t->a2dp.cconfig_size)) != 0) {
error("Couldn't initialize SBC codec: %s", strerror(errno));
goto fail_init;
}
const size_t sbc_codesize = sbc_get_codesize(&sbc);
const size_t sbc_frame_len = sbc_get_frame_length(&sbc);
const size_t in_buffer_size = t->mtu_read;
const size_t out_buffer_size = sbc_codesize * (in_buffer_size / sbc_frame_len + 1);
uint8_t *in_buffer = malloc(in_buffer_size);
int16_t *out_buffer = malloc(out_buffer_size);
pthread_cleanup_push(CANCEL_ROUTINE(sbc_finish), &sbc);
pthread_cleanup_push(CANCEL_ROUTINE(free), in_buffer);
pthread_cleanup_push(CANCEL_ROUTINE(free), out_buffer);
if (in_buffer == NULL || out_buffer == NULL) {
error("Couldn't create data buffers: %s", strerror(ENOMEM));
goto fail;
}
struct pollfd pfds[] = {
{ t->event_fd, POLLIN, 0 },
{ -1, POLLIN, 0 },
};
debug("Starting IO loop: %s",
bluetooth_profile_to_string(t->profile, t->codec));
for (;;) {
pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL);
ssize_t len;
/* add BT socket to the poll if transport is active */
pfds[1].fd = t->state == TRANSPORT_ACTIVE ? t->bt_fd : -1;
if (poll(pfds, sizeof(pfds) / sizeof(*pfds), -1) == -1) {
error("Transport poll error: %s", strerror(errno));
goto fail;
}
if (pfds[0].revents & POLLIN) {
/* dispatch incoming event */
eventfd_t event;
eventfd_read(pfds[0].fd, &event);
continue;
}
if ((len = read(pfds[1].fd, in_buffer, in_buffer_size)) == -1) {
debug("BT read error: %s", strerror(errno));
continue;
}
pthread_setcancelstate(PTHREAD_CANCEL_DISABLE, NULL);
/* it seems that zero is never returned... */
if (len == 0) {
debug("BT socket has been closed: %d", pfds[1].fd);
/* Prevent sending the release request to the BlueZ. If the socket has
* been closed, it means that BlueZ has already closed the connection. */
close(pfds[1].fd);
t->bt_fd = -1;
goto fail;
}
if (io_thread_open_pcm_write(&t->a2dp.pcm) == -1) {
if (errno != ENXIO)
error("Couldn't open FIFO: %s", strerror(errno));
continue;
}
const rtp_header_t *rtp_header = (rtp_header_t *)in_buffer;
const rtp_payload_sbc_t *rtp_payload = (rtp_payload_sbc_t *)&rtp_header->csrc[rtp_header->cc];
if (rtp_header->paytype != 96) {
warn("Unsupported RTP payload type: %u", rtp_header->paytype);
continue;
}
const uint8_t *input = (uint8_t *)(rtp_payload + 1);
int16_t *output = out_buffer;
size_t input_len = len - (input - in_buffer);
size_t output_len = out_buffer_size;
size_t frames = rtp_payload->frame_count;
/* decode retrieved SBC frames */
while (frames && input_len >= sbc_frame_len) {
ssize_t len;
size_t decoded;
if ((len = sbc_decode(&sbc, input, input_len, output, output_len, &decoded)) < 0) {
error("SBC decoding error: %s", strerror(-len));
break;
}
input += len;
input_len -= len;
output += decoded / sizeof(int16_t);
output_len -= decoded;
frames--;
}
const size_t size = output - out_buffer;
if (io_thread_write_pcm(&t->a2dp.pcm, out_buffer, size) == -1)
error("FIFO write error: %s", strerror(errno));
}
fail:
pthread_setcancelstate(PTHREAD_CANCEL_DISABLE, NULL);
pthread_cleanup_pop(1);
pthread_cleanup_pop(1);
pthread_cleanup_pop(1);
fail_init:
pthread_cleanup_pop(1);
return NULL;
}
static GstFlowReturn
gst_sbc_enc_handle_frame (GstAudioEncoder * audio_enc, GstBuffer * buffer)
{
GstSbcEnc *enc = GST_SBC_ENC (audio_enc);
GstMapInfo in_map, out_map;
GstBuffer *outbuf = NULL;
guint samples_per_frame, frames, i = 0;
/* no fancy draining */
if (buffer == NULL)
return GST_FLOW_OK;
if (G_UNLIKELY (enc->channels == 0 || enc->blocks == 0 || enc->subbands == 0))
return GST_FLOW_NOT_NEGOTIATED;
samples_per_frame = enc->channels * enc->blocks * enc->subbands;
if (!gst_buffer_map (buffer, &in_map, GST_MAP_READ))
goto map_failed;
frames = in_map.size / (samples_per_frame * sizeof (gint16));
GST_LOG_OBJECT (enc,
"encoding %" G_GSIZE_FORMAT " samples into %u SBC frames",
in_map.size / (enc->channels * sizeof (gint16)), frames);
if (frames > 0) {
gsize frame_len;
frame_len = sbc_get_frame_length (&enc->sbc);
outbuf = gst_audio_encoder_allocate_output_buffer (audio_enc,
frames * frame_len);
if (outbuf == NULL)
goto no_buffer;
gst_buffer_map (outbuf, &out_map, GST_MAP_WRITE);
for (i = 0; i < frames; ++i) {
gssize ret, written = 0;
ret = sbc_encode (&enc->sbc, in_map.data + (i * samples_per_frame * 2),
samples_per_frame * 2, out_map.data + (i * frame_len), frame_len,
&written);
if (ret < 0 || written != frame_len) {
GST_WARNING_OBJECT (enc, "encoding error, ret = %" G_GSSIZE_FORMAT ", "
"written = %" G_GSSIZE_FORMAT, ret, written);
break;
}
}
gst_buffer_unmap (outbuf, &out_map);
if (i > 0)
gst_buffer_set_size (outbuf, i * frame_len);
else
gst_buffer_replace (&outbuf, NULL);
}
done:
gst_buffer_unmap (buffer, &in_map);
return gst_audio_encoder_finish_frame (audio_enc, outbuf,
i * (samples_per_frame / enc->channels));
/* ERRORS */
no_buffer:
{
GST_ERROR_OBJECT (enc, "could not allocate output buffer");
goto done;
}
map_failed:
{
GST_ERROR_OBJECT (enc, "could not map input buffer");
goto done;
}
}
