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;
}
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);
}
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 void bluetooth_a2dp_setup(struct bluetooth_a2dp *a2dp)
{
sbc_capabilities_t active_capabilities = a2dp->sbc_capabilities;
if (a2dp->sbc_initialized)
sbc_reinit(&a2dp->sbc, 0);
else
sbc_init(&a2dp->sbc, 0);
a2dp->sbc_initialized = 1;
if (active_capabilities.frequency & BT_SBC_SAMPLING_FREQ_16000)
a2dp->sbc.frequency = SBC_FREQ_16000;
if (active_capabilities.frequency & BT_SBC_SAMPLING_FREQ_32000)
a2dp->sbc.frequency = SBC_FREQ_32000;
if (active_capabilities.frequency & BT_SBC_SAMPLING_FREQ_44100)
a2dp->sbc.frequency = SBC_FREQ_44100;
if (active_capabilities.frequency & BT_SBC_SAMPLING_FREQ_48000)
a2dp->sbc.frequency = SBC_FREQ_48000;
if (active_capabilities.channel_mode & BT_A2DP_CHANNEL_MODE_MONO)
a2dp->sbc.mode = SBC_MODE_MONO;
if (active_capabilities.channel_mode & BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL)
a2dp->sbc.mode = SBC_MODE_DUAL_CHANNEL;
if (active_capabilities.channel_mode & BT_A2DP_CHANNEL_MODE_STEREO)
a2dp->sbc.mode = SBC_MODE_STEREO;
if (active_capabilities.channel_mode & BT_A2DP_CHANNEL_MODE_JOINT_STEREO)
a2dp->sbc.mode = SBC_MODE_JOINT_STEREO;
a2dp->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:
a2dp->sbc.subbands = SBC_SB_4;
break;
case BT_A2DP_SUBBANDS_8:
a2dp->sbc.subbands = SBC_SB_8;
break;
}
switch (active_capabilities.block_length) {
case BT_A2DP_BLOCK_LENGTH_4:
a2dp->sbc.blocks = SBC_BLK_4;
break;
case BT_A2DP_BLOCK_LENGTH_8:
a2dp->sbc.blocks = SBC_BLK_8;
break;
case BT_A2DP_BLOCK_LENGTH_12:
a2dp->sbc.blocks = SBC_BLK_12;
break;
case BT_A2DP_BLOCK_LENGTH_16:
a2dp->sbc.blocks = SBC_BLK_16;
break;
}
a2dp->sbc.bitpool = active_capabilities.max_bitpool;
a2dp->codesize = sbc_get_codesize(&a2dp->sbc);
a2dp->count = sizeof(struct rtp_header) + sizeof(struct rtp_payload);
}
static void encode(char *filename, int subbands, int bitpool, int joint,
int dualchannel, int snr, int blocks)
{
struct au_header au_hdr;
sbc_t sbc;
int fd, size, srate, codesize, nframes;
size_t encoded;
ssize_t len;
if (sizeof(au_hdr) != 24) {
/* Sanity check just in case */
fprintf(stderr, "FIXME: sizeof(au_hdr) != 24\n");
return;
}
if (strcmp(filename, "-")) {
fd = open(filename, O_RDONLY);
if (fd < 0) {
fprintf(stderr, "Can't open file %s: %s\n",
filename, strerror(errno));
return;
}
} else
fd = fileno(stdin);
len = read(fd, &au_hdr, sizeof(au_hdr));
if (len < (ssize_t) sizeof(au_hdr)) {
if (fd > fileno(stderr))
fprintf(stderr, "Can't read header from file %s: %s\n",
filename, strerror(errno));
else
perror("Can't read audio header");
goto done;
}
if (au_hdr.magic != AU_MAGIC ||
BE_INT(au_hdr.hdr_size) > 128 ||
BE_INT(au_hdr.hdr_size) < sizeof(au_hdr) ||
BE_INT(au_hdr.encoding) != AU_FMT_LIN16) {
fprintf(stderr, "Not in Sun/NeXT audio S16_BE format\n");
goto done;
}
sbc_init(&sbc, 0L);
switch (BE_INT(au_hdr.sample_rate)) {
case 16000:
sbc.frequency = SBC_FREQ_16000;
break;
case 32000:
sbc.frequency = SBC_FREQ_32000;
break;
case 44100:
sbc.frequency = SBC_FREQ_44100;
break;
case 48000:
sbc.frequency = SBC_FREQ_48000;
break;
}
srate = BE_INT(au_hdr.sample_rate);
sbc.subbands = subbands == 4 ? SBC_SB_4 : SBC_SB_8;
if (BE_INT(au_hdr.channels) == 1) {
sbc.mode = SBC_MODE_MONO;
if (joint || dualchannel) {
fprintf(stderr, "Audio is mono but joint or "
"dualchannel mode has been specified\n");
goto done;
}
} else if (joint && !dualchannel)
sbc.mode = SBC_MODE_JOINT_STEREO;
else if (!joint && dualchannel)
sbc.mode = SBC_MODE_DUAL_CHANNEL;
else if (!joint && !dualchannel)
sbc.mode = SBC_MODE_STEREO;
else {
fprintf(stderr, "Both joint and dualchannel mode have been "
"specified\n");
goto done;
}
sbc.endian = SBC_BE;
/* Skip extra bytes of the header if any */
if (read(fd, input, BE_INT(au_hdr.hdr_size) - len) < 0)
goto done;
sbc.bitpool = bitpool;
sbc.allocation = snr ? SBC_AM_SNR : SBC_AM_LOUDNESS;
sbc.blocks = blocks == 4 ? SBC_BLK_4 :
blocks == 8 ? SBC_BLK_8 :
blocks == 12 ? SBC_BLK_12 : SBC_BLK_16;
if (verbose) {
fprintf(stderr, "encoding %s with rate %d, %d blocks, "
"%d subbands, %d bits, allocation method %s, "
"and mode %s\n",
filename, srate, blocks, subbands, bitpool,
sbc.allocation == SBC_AM_SNR ? "SNR" : "LOUDNESS",
sbc.mode == SBC_MODE_MONO ? "MONO" :
sbc.mode == SBC_MODE_STEREO ?
"STEREO" : "JOINTSTEREO");
}
codesize = sbc_get_codesize(&sbc);
nframes = sizeof(input) / codesize;
while (1) {
unsigned char *inp, *outp;
/* read data for up to 'nframes' frames of input data */
size = read(fd, input, codesize * nframes);
if (size < 0) {
/* Something really bad happened */
perror("Can't read audio data");
break;
}
if (size < codesize) {
/* Not enough data for encoding even a single frame */
break;
}
/* encode all the data from the input buffer in a loop */
inp = input;
outp = output;
while (size >= codesize) {
len = sbc_encode(&sbc, inp, codesize,
outp, sizeof(output) - (outp - output),
&encoded);
if (len != codesize || encoded <= 0) {
fprintf(stderr,
"sbc_encode fail, len=%zd, encoded=%lu\n",
len, (unsigned long) encoded);
break;
}
size -= len;
inp += len;
outp += encoded;
}
len = write(fileno(stdout), output, outp - output);
if (len != outp - output) {
perror("Can't write SBC output");
break;
}
if (size != 0) {
/*
* sbc_encode failure has been detected earlier or end
* of file reached (have trailing partial data which is
* insufficient to encode SBC frame)
*/
break;
}
}
sbc_finish(&sbc);
done:
if (fd > fileno(stderr))
close(fd);
}
