static GstFlowReturn sbc_enc_chain(GstPad *pad, GstBuffer *buffer)
{
GstSbcEnc *enc = GST_SBC_ENC(gst_pad_get_parent(pad));
GstAdapter *adapter = enc->adapter;
GstFlowReturn res = GST_FLOW_OK;
gst_adapter_push(adapter, buffer);
while (gst_adapter_available(adapter) >= enc->codesize &&
res == GST_FLOW_OK) {
GstBuffer *output;
GstCaps *caps;
const guint8 *data;
gint consumed;
caps = GST_PAD_CAPS(enc->srcpad);
res = gst_pad_alloc_buffer_and_set_caps(enc->srcpad,
GST_BUFFER_OFFSET_NONE,
enc->frame_length, caps,
&output);
if (res != GST_FLOW_OK)
goto done;
data = gst_adapter_peek(adapter, enc->codesize);
consumed = sbc_encode(&enc->sbc, (gpointer) data,
enc->codesize,
GST_BUFFER_DATA(output),
GST_BUFFER_SIZE(output), NULL);
if (consumed <= 0) {
GST_DEBUG_OBJECT(enc, "comsumed < 0, codesize: %d",
enc->codesize);
break;
}
gst_adapter_flush(adapter, consumed);
GST_BUFFER_TIMESTAMP(output) = GST_BUFFER_TIMESTAMP(buffer);
/* we have only 1 frame */
GST_BUFFER_DURATION(output) = enc->frame_duration;
res = gst_pad_push(enc->srcpad, output);
if (res != GST_FLOW_OK)
goto done;
}
done:
gst_object_unref(enc);
return res;
}
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;
unsigned int 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) ||
(data->count + written >= BUFFER_SIZE)) {
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;
}
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;
}
// also works but sleeps between transfers
static snd_pcm_sframes_t a2dp_transfer2(snd_pcm_ioplug_t *io,
const snd_pcm_channel_area_t *areas,
snd_pcm_uframes_t offset, snd_pcm_uframes_t size)
{
snd_pcm_a2dp_t *a2dp = io->private_data;
char *buf;
int len;
struct media_packet_header packet_header;
struct media_payload_header payload_header;
int codesize,datatoread;
unsigned long sleeptime;
int written;
struct timeval dt;
codesize=a2dp->sbc.subbands*a2dp->sbc.blocks*a2dp->sbc.channels*2;
datatoread=min(codesize,size*a2dp->frame_bytes);
buf = (char *) areas->addr + (areas->first + areas->step * offset) / 8;
if(lenbufe<codesize){
memcpy(bufe+lenbufe,buf,datatoread);
lenbufe+=datatoread;
}
else{datatoread=0;}
if(lenbufe>=codesize){ //enough data to encode
change_endian(bufe,codesize); // changing the endianness
len = sbc_encode(&(a2dp->sbc), bufe, codesize); //encode
memmove(bufe, bufe + len, lenbufe - len); //shift the bufe
lenbufe-=len;
nbytes+=len;
sleeptime += a2dp->sbc.duration;
if (len <= 0)
return len;
if(a2dp->len + a2dp->sbc.len > 678) { // time to prepare and send the packet
dt.tv_sec=0;
dt.tv_usec=1000000*a2dp->sbc.subbands*a2dp->sbc.blocks*frame_count/io->rate;
memset(&payload_header, 0, sizeof(payload_header));
memset(&packet_header, 0, sizeof(packet_header));
payload_header.frame_count=frame_count;
packet_header.v = 2;
packet_header.pt = 1;
packet_header.sequence_number = htons(seq_num);
packet_header.timestamp = htonl(timestamp);
packet_header.ssrc = htonl(1);
timestamp += (a2dp->sbc.blocks + 1)*4 * (a2dp->sbc.subbands + 1)*4;
memcpy(a2dp->buf, &packet_header, sizeof(packet_header));
memcpy(a2dp->buf + sizeof(packet_header), &payload_header, sizeof(payload_header));
sleeptill(&tsend, &dt);
if((written = write(a2dp->sk,a2dp->buf,a2dp->len)) != a2dp->len) {
DBG("Wrote %d not %d bytes; errno %s(%d)", written, a2dp->len,
strerror(errno), errno);
}
a2dp->len = sizeof(packet_header)+sizeof(payload_header);
frame_count=0;
sleeptime=0;
seq_num++;
}
frame_count++;
memcpy(a2dp->buf + a2dp->len, a2dp->sbc.data, a2dp->sbc.len);
a2dp->len+=a2dp->sbc.len;
if (a2dp->state == BT_CONNECTED)
a2dp->num += len / a2dp->frame_bytes;
}
return datatoread / a2dp->frame_bytes;
}
// transfers around correct time postions
static snd_pcm_sframes_t a2dp_transfer(snd_pcm_ioplug_t *io,
const snd_pcm_channel_area_t *areas,
snd_pcm_uframes_t offset, snd_pcm_uframes_t size)
{
snd_pcm_a2dp_t *a2dp = io->private_data;
char *buf;
int len;
struct media_packet_header packet_header;
struct media_payload_header payload_header;
int codesize,datatoread;
unsigned long sleeptime;
struct timeval dt;
codesize=a2dp->sbc.subbands*a2dp->sbc.blocks*a2dp->sbc.channels*2; // size of data encoded by sbc_encode in one call
datatoread=min(codesize,size*a2dp->frame_bytes); // amount of data to read
buf = (char *) areas->addr + (areas->first + areas->step * offset) / 8;
if(lenbufe<codesize && lenbufe+datatoread<sizeof(bufe)){ // if not enough data in bufe to encode and there is space in bufe
memcpy(bufe+lenbufe,buf,datatoread);// we read data to bufe
lenbufe+=datatoread;
}
else{datatoread=0;}//nothing has been read
if(lenbufe>=codesize && a2dp->len + a2dp->sbc.len < 678){ // if enough data in bufe to encode and not enough frame to fill up mtu: encoding
change_endian(bufe,codesize); // changing the endianness
len = sbc_encode(&(a2dp->sbc), bufe, codesize); //encode
memmove(bufe, bufe + len, lenbufe - len); //shift the bufe
lenbufe-=len;
nbytes+=len;
sleeptime += a2dp->sbc.duration;
if (len <= 0)
return len;
frame_count++;
memcpy(a2dp->buf + a2dp->len, a2dp->sbc.data, a2dp->sbc.len); // copy encoded frames into a2dp->buf
a2dp->len+=a2dp->sbc.len;
if (a2dp->state == BT_CONNECTED)
a2dp->num += len / a2dp->frame_bytes; //update pointer
a2dp->num %=io->buffer_size;
}
if(a2dp->len + a2dp->sbc.len > 678){ // if packet is formed
dt.tv_usec=1000000*a2dp->sbc.subbands*a2dp->sbc.blocks*frame_count/io->rate; // time interval between transmitions
dt.tv_sec=0;
if(time_to_wait(&tsend, &dt)==0){ // time to send data
memset(&payload_header, 0, sizeof(payload_header)); // fill up the headers
memset(&packet_header, 0, sizeof(packet_header)); //---
payload_header.frame_count=frame_count;
packet_header.v = 2;
packet_header.pt = 1;
packet_header.sequence_number = htons(seq_num);
packet_header.timestamp = htonl(timestamp);
packet_header.ssrc = htonl(1);
timestamp += (a2dp->sbc.blocks + 1)*4 * (a2dp->sbc.subbands + 1)*4;
memcpy(a2dp->buf, &packet_header, sizeof(packet_header)); //copy the headers to buf
memcpy(a2dp->buf + sizeof(packet_header), &payload_header, sizeof(payload_header));//---
write(a2dp->sk,a2dp->buf,a2dp->len); // sending the packet
a2dp->len = sizeof(packet_header)+sizeof(payload_header); //inital position in buf, just after headers
frame_count=0;
sleeptime=0;
seq_num++;
}else{usleep(1);}
}
return datatoread / a2dp->frame_bytes;
}
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;
}
}
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);
}
