int
codec_sbc_decode(void *ctx, const void *input, int inputSizeBytes, void *output,
int *outputSizeBytes)
{
size_t size_in = (size_t) inputSizeBytes;
size_t size_out = SBC_BUFFER;
size_t len;
int framelen;
int xframe_pos = 0;
const guint8 *data_in = (const guint8 *) input;
guint8 *data_out = (guint8 *) output;
sbc_t *sbc = (sbc_t *) ctx;
guint8 *i_data;
guint8 tmp;
if (!output || !outputSizeBytes) {
return size_out;
}
sbc->endian = SBC_BE;
*outputSizeBytes = 0;
while (xframe_pos < inputSizeBytes) {
framelen = sbc_decode(sbc, data_in, size_in, data_out, size_out, &len);
xframe_pos += framelen;
data_in += framelen;
*outputSizeBytes += len;
for (i_data = data_out; i_data < data_out + len; i_data += 2) {
tmp = i_data[0];
i_data[0] = i_data[1];
i_data[1] = tmp;
}
data_out += len;
}
return *outputSizeBytes;
}
static void decode(char *filename, char *output, int tofile, bool msbc)
{
unsigned char buf[BUF_SIZE], *stream;
struct stat st;
sbc_t sbc;
int fd, ad, pos, streamlen, framelen, count;
size_t len;
int format = AFMT_S16_BE, frequency, channels;
ssize_t written;
if (stat(filename, &st) < 0) {
fprintf(stderr, "Can't get size of file %s: %s\n",
filename, strerror(errno));
return;
}
stream = malloc(st.st_size);
if (!stream) {
fprintf(stderr, "Can't allocate memory for %s: %s\n",
filename, strerror(errno));
return;
}
fd = open(filename, O_RDONLY);
if (fd < 0) {
fprintf(stderr, "Can't open file %s: %s\n",
filename, strerror(errno));
goto free;
}
if (read(fd, stream, st.st_size) != st.st_size) {
fprintf(stderr, "Can't read content of %s: %s\n",
filename, strerror(errno));
close(fd);
goto free;
}
close(fd);
pos = 0;
streamlen = st.st_size;
if (tofile)
ad = open(output, O_WRONLY | O_CREAT | O_TRUNC, 0644);
else
ad = open(output, O_WRONLY, 0);
if (ad < 0) {
fprintf(stderr, "Can't open output %s: %s\n",
output, strerror(errno));
goto free;
}
if (msbc)
sbc_init_msbc(&sbc, 0L);
else
sbc_init(&sbc, 0L);
sbc.endian = SBC_BE;
framelen = sbc_decode(&sbc, stream, streamlen, buf, sizeof(buf), &len);
channels = sbc.mode == SBC_MODE_MONO ? 1 : 2;
switch (sbc.frequency) {
case SBC_FREQ_16000:
frequency = 16000;
break;
case SBC_FREQ_32000:
frequency = 32000;
break;
case SBC_FREQ_44100:
frequency = 44100;
break;
case SBC_FREQ_48000:
frequency = 48000;
break;
default:
frequency = 0;
}
if (verbose) {
fprintf(stderr,"decoding %s with rate %d, %d subbands, "
"%d bits, allocation method %s and mode %s\n",
filename, frequency, sbc.subbands * 4 + 4, sbc.bitpool,
sbc.allocation == SBC_AM_SNR ? "SNR" : "LOUDNESS",
sbc.mode == SBC_MODE_MONO ? "MONO" :
sbc.mode == SBC_MODE_STEREO ?
"STEREO" : "JOINTSTEREO");
}
if (tofile) {
struct au_header au_hdr;
au_hdr.magic = AU_MAGIC;
au_hdr.hdr_size = BE_INT(24);
au_hdr.data_size = BE_INT(0);
au_hdr.encoding = BE_INT(AU_FMT_LIN16);
au_hdr.sample_rate = BE_INT(frequency);
au_hdr.channels = BE_INT(channels);
written = write(ad, &au_hdr, sizeof(au_hdr));
if (written < (ssize_t) sizeof(au_hdr)) {
fprintf(stderr, "Failed to write header\n");
goto close;
}
} else {
if (ioctl(ad, SNDCTL_DSP_SETFMT, &format) < 0) {
fprintf(stderr, "Can't set audio format on %s: %s\n",
output, strerror(errno));
goto close;
}
if (ioctl(ad, SNDCTL_DSP_CHANNELS, &channels) < 0) {
fprintf(stderr, "Can't set number of channels on %s: %s\n",
output, strerror(errno));
goto close;
}
if (ioctl(ad, SNDCTL_DSP_SPEED, &frequency) < 0) {
fprintf(stderr, "Can't set audio rate on %s: %s\n",
output, strerror(errno));
goto close;
}
}
count = len;
while (framelen > 0) {
/* we have completed an sbc_decode at this point sbc.len is the
* length of the frame we just decoded count is the number of
* decoded bytes yet to be written */
if (count + len >= BUF_SIZE) {
/* buffer is too full to stuff decoded audio in so it
* must be written to the device */
written = write(ad, buf, count);
if (written > 0)
count -= written;
}
/* sanity check */
if (count + len >= BUF_SIZE) {
fprintf(stderr,
"buffer size of %d is too small for decoded"
" data (%lu)\n", BUF_SIZE, (unsigned long) (len + count));
exit(1);
}
/* push the pointer in the file forward to the next bit to be
* decoded tell the decoder to decode up to the remaining
* length of the file (!) */
pos += framelen;
framelen = sbc_decode(&sbc, stream + pos, streamlen - pos,
buf + count, sizeof(buf) - count, &len);
/* increase the count */
count += len;
}
if (count > 0) {
written = write(ad, buf, count);
if (written > 0)
count -= written;
}
close:
sbc_finish(&sbc);
close(ad);
free:
free(stream);
}
static void decode(char *filename, char *audiodevice, int tofile)
{
unsigned char buf[BUF_SIZE], *stream;
struct stat st;
off_t filesize;
sbc_t sbc;
int fd, ad, pos, streamlen, framelen, count, format = AFMT_S16_BE;
if (stat(filename, &st) < 0) {
fprintf(stderr, "Can't get size of file %s: %s\n",
filename, strerror(errno));
return;
}
filesize = st.st_size;
stream = malloc(st.st_size);
if (!stream) {
fprintf(stderr, "Can't allocate memory for %s: %s\n",
filename, strerror(errno));
return;
}
fd = open(filename, O_RDONLY);
if (fd < 0) {
fprintf(stderr, "Can't open file %s: %s\n",
filename, strerror(errno));
goto free;
}
if (read(fd, stream, st.st_size) != st.st_size) {
fprintf(stderr, "Can't read content of %s: %s\n",
filename, strerror(errno));
close(fd);
goto free;
}
close(fd);
pos = 0;
streamlen = st.st_size;
ad = open(audiodevice, O_WRONLY | (tofile ? (O_CREAT | O_TRUNC) : 0), tofile ? 0644 : 0);
if (ad < 0) {
fprintf(stderr, "Can't open audio device %s: %s\n",
audiodevice, strerror(errno));
goto free;
}
sbc_init(&sbc, SBC_NULL);
framelen = sbc_decode(&sbc, stream, streamlen);
printf("%d Hz, %d channels\n", sbc.rate, sbc.channels);
if (!tofile) {
if (ioctl(ad, SNDCTL_DSP_SETFMT, &format) < 0) {
fprintf(stderr, "Can't set audio format on %s: %s\n",
audiodevice, strerror(errno));
goto close;
}
if (ioctl(ad, SNDCTL_DSP_CHANNELS, &sbc.channels) < 0) {
fprintf(stderr, "Can't set number of channels on %s: %s\n",
audiodevice, strerror(errno));
goto close;
}
if (ioctl(ad, SNDCTL_DSP_SPEED, &sbc.rate) < 0) {
fprintf(stderr, "Can't set audio rate on %s: %s\n",
audiodevice, strerror(errno));
goto close;
}
}
count = 0;
while (framelen > 0) {
// we have completed an sbc_decode at this point
// sbc.len is the length of the frame we just decoded
// count is the number of decoded bytes yet to be written
if (count + sbc.len > BUF_SIZE) {
// buffer is too full to stuff decoded audio in
// so it must be written to the device
write(ad, buf, count);
count = 0;
}
// sanity check
if(count + sbc.len > BUF_SIZE) {
fprintf(stderr, "buffer size of %d is too small for decoded data (%d)\n", BUF_SIZE, sbc.len + count);
exit(1);
}
// move the latest decoded data into buf and increase the count
memcpy(buf + count, sbc.data, sbc.len);
count += sbc.len;
// push the pointer in the file forward to the next bit to be decoded
// tell the decoder to decode up to the remaining length of the file (!)
pos += framelen;
framelen = sbc_decode(&sbc, stream + pos, streamlen - pos);
}
if (count > 0)
write(ad, buf, count);
close:
sbc_finish(&sbc);
close(ad);
free:
free(stream);
}
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_dec_handle_frame (GstAudioDecoder * audio_dec, GstBuffer * buf)
{
GstSbcDec *dec = GST_SBC_DEC (audio_dec);
GstBuffer *outbuf = NULL;
GstMapInfo out_map;
GstMapInfo in_map;
gsize output_size;
guint num_frames, i;
/* no fancy draining */
if (G_UNLIKELY (buf == NULL))
return GST_FLOW_OK;
if (G_UNLIKELY (dec->frame_len == 0))
return GST_FLOW_NOT_NEGOTIATED;
gst_buffer_map (buf, &in_map, GST_MAP_READ);
if (G_UNLIKELY (in_map.size == 0))
goto done;
/* we assume all frames are of the same size, this is implied by the
* input caps applying to the whole input buffer, and the parser should
* also have made sure of that */
if (G_UNLIKELY (in_map.size % dec->frame_len != 0))
goto mixed_frames;
num_frames = in_map.size / dec->frame_len;
output_size = num_frames * dec->samples_per_frame * sizeof (gint16);
outbuf = gst_audio_decoder_allocate_output_buffer (audio_dec, output_size);
if (outbuf == NULL)
goto no_buffer;
gst_buffer_map (outbuf, &out_map, GST_MAP_WRITE);
for (i = 0; i < num_frames; ++i) {
gssize ret;
gsize written;
ret = sbc_decode (&dec->sbc, in_map.data + (i * dec->frame_len),
dec->frame_len, out_map.data + (i * dec->samples_per_frame * 2),
dec->samples_per_frame * 2, &written);
if (ret <= 0 || written != (dec->samples_per_frame * 2)) {
GST_WARNING_OBJECT (dec, "decoding 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 * dec->samples_per_frame * 2);
else
gst_buffer_replace (&outbuf, NULL);
done:
gst_buffer_unmap (buf, &in_map);
return gst_audio_decoder_finish_frame (audio_dec, outbuf, 1);
/* ERRORS */
mixed_frames:
{
GST_WARNING_OBJECT (dec, "inconsistent input data/frames, skipping");
goto done;
}
no_buffer:
{
GST_ERROR_OBJECT (dec, "could not allocate output buffer");
goto done;
}
}