From: Pierre-Louis Bossart <pierre-louis.bossart@xxxxxxxxx>
Second version of patch to enable MP3 passthrough over A2DP, with
the decoding taking place on the headset to reduce power consumption.
This was only possible if PulseAudio was bypassed before with the
a2dpsink element in gstreamer.
This is a simpler version of an earlier patch. The MP3 parsing is
now supposed to happen in the application, and the BT sink
expects an IEC61937 format: 8byte header, mp3 frame and zero-padding
to reach 4608 bytes. This format is a pseudo-PCM stream, which
is compatible with the existing way of handling latency/timing.
this patch can be tested with following command:
gst-launch filesrc location=file.mp3 ! mp3parse ! mpegiec958 ! filesink location=file-be.iec
dd if=if file-be.iec of=file-le.iec conv=swab
paplay --raw --passthrough -v --format="s16le" --rate=44100 --device=bluez_sink.00_11_22_33_44_55 file-le.iec
or more simply
gst-launch filesrc location=file.mp3 ! mp3parse ! mpegiec958 ! pulsesink device=bluez_sink.00_11_22_33_44_55
patches for mpegiec958 and pulsesink will be posted separately.
TODO for later patches:
- add type for MP3/AC3
- expose sink capability to apps so that the pipeline can be reconfigured
- handle dynamic routing
- set sink sample rate to the rate of the stream
---
src/modules/bluetooth/bluetooth-util.h | 1 +
src/modules/bluetooth/module-bluetooth-device.c | 669 +++++++++++++++++++++--
src/modules/bluetooth/rtp.h | 14 +-
3 files changed, 640 insertions(+), 44 deletions(-)
diff --git a/src/modules/bluetooth/bluetooth-util.h b/src/modules/bluetooth/bluetooth-util.h
index 2752a69..7ca9cb2 100644
--- a/src/modules/bluetooth/bluetooth-util.h
+++ b/src/modules/bluetooth/bluetooth-util.h
@@ -58,6 +58,7 @@ struct pa_bluetooth_uuid {
enum profile {
PROFILE_A2DP,
PROFILE_A2DP_SOURCE,
+ PROFILE_A2DP_PASSTHROUGH,
PROFILE_HSP,
PROFILE_HFGW,
PROFILE_OFF
diff --git a/src/modules/bluetooth/module-bluetooth-device.c b/src/modules/bluetooth/module-bluetooth-device.c
index 7992e12..049b0a8 100644
--- a/src/modules/bluetooth/module-bluetooth-device.c
+++ b/src/modules/bluetooth/module-bluetooth-device.c
@@ -104,6 +104,9 @@ static const char* const valid_modargs[] = {
struct a2dp_info {
sbc_capabilities_t sbc_capabilities;
+ mpeg_capabilities_t mpeg_capabilities;
+ pa_bool_t has_mpeg;
+
sbc_t sbc; /* Codec data */
pa_bool_t sbc_initialized; /* Keep track if the encoder is initialized */
size_t codesize, frame_length; /* SBC Codesize, frame_length. We simply cache those values here */
@@ -184,6 +187,9 @@ struct userdata {
int service_write_type, service_read_type;
pa_bool_t filter_added;
+
+ /* required for PASSTHROUGH profile */
+ size_t leftover_bytes;
};
enum {
@@ -309,6 +315,64 @@ static ssize_t service_expect(struct userdata*u, bt_audio_msg_header_t *rsp, siz
}
/* Run from main thread */
+static int parse_mpeg_caps(struct userdata *u, uint8_t seid, const struct bt_get_capabilities_rsp *rsp) {
+ uint16_t bytes_left;
+ const codec_capabilities_t *codec;
+
+ pa_assert(u);
+ pa_assert(rsp);
+ pa_assert( u->profile == PROFILE_A2DP_PASSTHROUGH );
+
+ u->a2dp.has_mpeg = FALSE;
+
+ bytes_left = rsp->h.length - sizeof(*rsp);
+
+ if (bytes_left < sizeof(codec_capabilities_t)) {
+ pa_log_error("Packet too small to store codec information.");
+ return -1;
+ }
+
+ codec = (codec_capabilities_t *) rsp->data; /** ALIGNMENT? **/
+
+ pa_log_debug("Payload size is %lu %lu", (unsigned long) bytes_left, (unsigned long) sizeof(*codec));
+
+ if (codec->transport != BT_CAPABILITIES_TRANSPORT_A2DP) {
+ pa_log_error("Got capabilities for wrong codec.");
+ return -1;
+ }
+
+ while (bytes_left > 0) {
+ if ((codec->type == BT_A2DP_MPEG12_SINK) && (!codec->lock)) {
+ break;
+ }
+ bytes_left -= codec->length;
+ codec = (const codec_capabilities_t*) ((const uint8_t*) codec + codec->length);
+ }
+
+ if (bytes_left <= 0 || codec->length != sizeof(u->a2dp.mpeg_capabilities))
+ return -1;
+
+ pa_assert(codec->type == BT_A2DP_MPEG12_SINK);
+
+ if (codec->configured && seid == 0)
+ return codec->seid;
+
+ u->a2dp.has_mpeg = TRUE;
+ memcpy(&u->a2dp.mpeg_capabilities, codec, sizeof(u->a2dp.mpeg_capabilities));
+
+ pa_log_info("MPEG caps detected");
+ pa_log_info("channel_mode %d crc %d layer %d frequency %d mpf %d bitrate %d",
+ u->a2dp.mpeg_capabilities.channel_mode,
+ u->a2dp.mpeg_capabilities.crc,
+ u->a2dp.mpeg_capabilities.layer,
+ u->a2dp.mpeg_capabilities.frequency,
+ u->a2dp.mpeg_capabilities.mpf,
+ u->a2dp.mpeg_capabilities.bitrate);
+
+ return 0;
+}
+
+/* Run from main thread */
static int parse_caps(struct userdata *u, uint8_t seid, const struct bt_get_capabilities_rsp *rsp) {
uint16_t bytes_left;
const codec_capabilities_t *codec;
@@ -389,44 +453,89 @@ static int parse_caps(struct userdata *u, uint8_t seid, const struct bt_get_capa
return 0;
}
+typedef union {
+ struct bt_get_capabilities_req getcaps_req;
+ struct bt_get_capabilities_rsp getcaps_rsp;
+ bt_audio_error_t error;
+ uint8_t buf[BT_SUGGESTED_BUFFER_SIZE];
+} bt_getcaps_msg_t;
+
/* Run from main thread */
-static int get_caps(struct userdata *u, uint8_t seid) {
- union {
- struct bt_get_capabilities_req getcaps_req;
- struct bt_get_capabilities_rsp getcaps_rsp;
- bt_audio_error_t error;
- uint8_t buf[BT_SUGGESTED_BUFFER_SIZE];
- } msg;
- int ret;
+static int get_caps_msg(struct userdata *u, uint8_t seid, bt_getcaps_msg_t *msg) {
pa_assert(u);
- memset(&msg, 0, sizeof(msg));
- msg.getcaps_req.h.type = BT_REQUEST;
- msg.getcaps_req.h.name = BT_GET_CAPABILITIES;
- msg.getcaps_req.h.length = sizeof(msg.getcaps_req);
- msg.getcaps_req.seid = seid;
-
- pa_strlcpy(msg.getcaps_req.object, u->path, sizeof(msg.getcaps_req.object));
- if (u->profile == PROFILE_A2DP || u->profile == PROFILE_A2DP_SOURCE)
- msg.getcaps_req.transport = BT_CAPABILITIES_TRANSPORT_A2DP;
+ memset(msg, 0, sizeof(bt_getcaps_msg_t));
+ msg->getcaps_req.h.type = BT_REQUEST;
+ msg->getcaps_req.h.name = BT_GET_CAPABILITIES;
+ msg->getcaps_req.h.length = sizeof(struct bt_get_capabilities_req);
+ msg->getcaps_req.seid = seid;
+
+ pa_strlcpy(msg->getcaps_req.object, u->path, sizeof(msg->getcaps_req.object));
+ if (u->profile == PROFILE_A2DP || u->profile == PROFILE_A2DP_PASSTHROUGH ||
+ u->profile == PROFILE_A2DP_SOURCE)
+ msg->getcaps_req.transport = BT_CAPABILITIES_TRANSPORT_A2DP;
else {
pa_assert(u->profile == PROFILE_HSP || u->profile == PROFILE_HFGW);
- msg.getcaps_req.transport = BT_CAPABILITIES_TRANSPORT_SCO;
+ msg->getcaps_req.transport = BT_CAPABILITIES_TRANSPORT_SCO;
}
- msg.getcaps_req.flags = u->auto_connect ? BT_FLAG_AUTOCONNECT : 0;
+ msg->getcaps_req.flags = u->auto_connect ? BT_FLAG_AUTOCONNECT : 0;
- if (service_send(u, &msg.getcaps_req.h) < 0)
+ if (service_send(u, &msg->getcaps_req.h) < 0)
return -1;
- if (service_expect(u, &msg.getcaps_rsp.h, sizeof(msg), BT_GET_CAPABILITIES, 0) < 0)
+ if (service_expect(u, &msg->getcaps_rsp.h, sizeof(bt_getcaps_msg_t), BT_GET_CAPABILITIES, 0) < 0)
+ return -1;
+
+ return 0;
+}
+
+/* Run from main thread */
+static int get_caps(struct userdata *u, uint8_t seid) {
+ bt_getcaps_msg_t msg;
+ int ret;
+
+ pa_assert(u);
+
+ if (get_caps_msg(u, seid, &msg) < 0)
return -1;
ret = parse_caps(u, seid, &msg.getcaps_rsp);
- if (ret <= 0)
- return ret;
+ if (ret < 0)
+ return -1;
- return get_caps(u, ret);
+ if (ret > 0) {
+ /* refine seid caps */
+ if (get_caps_msg(u, ret, &msg) < 0)
+ return -1;
+
+ ret = parse_caps(u, ret, &msg.getcaps_rsp);
+ if (ret < 0)
+ return -1;
+ }
+
+ if (u->profile == PROFILE_A2DP_PASSTHROUGH) {
+ seid = 0;
+
+ /* try to find mpeg end-point */
+ if (get_caps_msg(u, seid, &msg) < 0)
+ return -1;
+
+ ret = parse_mpeg_caps(u, seid, &msg.getcaps_rsp);
+ if (ret < 0)
+ return -1;
+
+ if (ret > 0) {
+ /* refine seid caps */
+ if (get_caps_msg(u, ret, &msg) < 0)
+ return -1;
+
+ ret = parse_mpeg_caps(u, ret, &msg.getcaps_rsp);
+ if (ret < 0)
+ return -1;
+ }
+ }
+ return 0;
}
/* Run from main thread */
@@ -491,8 +600,54 @@ static int setup_a2dp(struct userdata *u) {
};
pa_assert(u);
- pa_assert(u->profile == PROFILE_A2DP || u->profile == PROFILE_A2DP_SOURCE);
+ pa_assert(u->profile == PROFILE_A2DP || u->profile == PROFILE_A2DP_PASSTHROUGH || u->profile == PROFILE_A2DP_SOURCE);
+
+ if (u->profile == PROFILE_A2DP_PASSTHROUGH) {
+ mpeg_capabilities_t *mcap;
+
+ if (u->a2dp.has_mpeg) {
+ int rate;
+
+ mcap = &u->a2dp.mpeg_capabilities;
+ rate = u->sample_spec.rate;
+
+ if (u->sample_spec.channels == 1)
+ mcap->channel_mode = BT_A2DP_CHANNEL_MODE_MONO;
+ else
+ mcap->channel_mode = BT_A2DP_CHANNEL_MODE_STEREO;
+
+ mcap->crc = 0; /* CRC is broken in some encoders */
+ mcap->layer = BT_MPEG_LAYER_3;
+
+ if (rate == 44100)
+ mcap->frequency = BT_MPEG_SAMPLING_FREQ_44100;
+ else if (rate == 48000)
+ mcap->frequency = BT_MPEG_SAMPLING_FREQ_48000;
+ else if (rate == 32000)
+ mcap->frequency = BT_MPEG_SAMPLING_FREQ_32000;
+ else if (rate == 24000)
+ mcap->frequency = BT_MPEG_SAMPLING_FREQ_24000;
+ else if (rate == 22050)
+ mcap->frequency = BT_MPEG_SAMPLING_FREQ_22050;
+ else if (rate == 16000)
+ mcap->frequency = BT_MPEG_SAMPLING_FREQ_16000;
+ else {
+ pa_log("unsupported sampling frequency");
+ return -1;
+ }
+
+ mcap->mpf = 0; /* don't use optional IETF payload, send raw frames */
+ mcap->bitrate = 0x8000; /* set for vbr, this covers all cases */
+ return 0;
+ }
+ else {
+ pa_log("setup_a2dp: Trying to set-up A2DP Passthrough configuration but no MPEG endpoint available");
+ return -1; /* return error, profile will not be loaded */
+ }
+ }
+
+ /* other profiles */
cap = &u->a2dp.sbc_capabilities;
/* Find the lowest freq that is at least as high as the requested
@@ -696,8 +851,17 @@ static int set_conf(struct userdata *u) {
msg.open_req.h.length = sizeof(msg.open_req);
pa_strlcpy(msg.open_req.object, u->path, sizeof(msg.open_req.object));
- msg.open_req.seid = (u->profile == PROFILE_A2DP || u->profile == PROFILE_A2DP_SOURCE) ? u->a2dp.sbc_capabilities.capability.seid : BT_A2DP_SEID_RANGE + 1;
- msg.open_req.lock = (u->profile == PROFILE_A2DP) ? BT_WRITE_LOCK : BT_READ_LOCK | BT_WRITE_LOCK;
+
+ if (u->profile == PROFILE_A2DP || u->profile == PROFILE_A2DP_SOURCE) {
+ msg.open_req.seid = u->a2dp.sbc_capabilities.capability.seid;
+ } else if (u->profile == PROFILE_A2DP_PASSTHROUGH) {
+ pa_assert(u->a2dp.has_mpeg);
+ msg.open_req.seid = u->a2dp.mpeg_capabilities.capability.seid;
+ } else {
+ msg.open_req.seid = BT_A2DP_SEID_RANGE + 1;
+ }
+
+ msg.open_req.lock = (u->profile == PROFILE_A2DP || u->profile == PROFILE_A2DP_PASSTHROUGH) ? BT_WRITE_LOCK : BT_READ_LOCK | BT_WRITE_LOCK;
if (service_send(u, &msg.open_req.h) < 0)
return -1;
@@ -705,7 +869,10 @@ static int set_conf(struct userdata *u) {
if (service_expect(u, &msg.open_rsp.h, sizeof(msg), BT_OPEN, sizeof(msg.open_rsp)) < 0)
return -1;
- if (u->profile == PROFILE_A2DP || u->profile == PROFILE_A2DP_SOURCE) {
+ if (u->profile == PROFILE_A2DP || u->profile == PROFILE_A2DP_PASSTHROUGH || u->profile == PROFILE_A2DP_SOURCE ) {
+
+ /* for passthrough we expect little-endian data to be compatible with the ALSA iec958
+ devices */
u->sample_spec.format = PA_SAMPLE_S16LE;
if (setup_a2dp(u) < 0)
@@ -725,6 +892,9 @@ static int set_conf(struct userdata *u) {
if (u->profile == PROFILE_A2DP || u->profile == PROFILE_A2DP_SOURCE) {
memcpy(&msg.setconf_req.codec, &u->a2dp.sbc_capabilities, sizeof(u->a2dp.sbc_capabilities));
+ } else if (u->profile == PROFILE_A2DP_PASSTHROUGH) {
+ pa_assert(u->a2dp.has_mpeg);
+ memcpy(&msg.setconf_req.codec, &u->a2dp.mpeg_capabilities, sizeof(u->a2dp.mpeg_capabilities));
} else {
msg.setconf_req.codec.transport = BT_CAPABILITIES_TRANSPORT_SCO;
msg.setconf_req.codec.seid = BT_A2DP_SEID_RANGE + 1;
@@ -745,12 +915,18 @@ static int set_conf(struct userdata *u) {
setup_sbc(&u->a2dp, u->profile);
u->block_size =
- ((u->link_mtu - sizeof(struct rtp_header) - sizeof(struct rtp_payload))
- / u->a2dp.frame_length
- * u->a2dp.codesize);
+ ((u->link_mtu - sizeof(struct rtp_header) - sizeof(struct sbc_rtp_payload))
+ / u->a2dp.frame_length
+ * u->a2dp.codesize);
pa_log_info("SBC parameters:\n\tallocation=%u\n\tsubbands=%u\n\tblocks=%u\n\tbitpool=%u\n",
u->a2dp.sbc.allocation, u->a2dp.sbc.subbands, u->a2dp.sbc.blocks, u->a2dp.sbc.bitpool);
+ } else if (u->profile == PROFILE_A2DP_PASSTHROUGH) {
+
+ /* available payload per packet */
+ u->block_size = 1152*4; /* this is the size of an IEC61937 frame for MPEG layer 3 */
+ u->leftover_bytes=0;
+
} else
u->block_size = u->link_mtu;
@@ -782,7 +958,7 @@ static void a2dp_set_bitpool(struct userdata *u, uint8_t bitpool)
pa_log_debug("Bitpool has changed to %u", a2dp->sbc.bitpool);
u->block_size =
- (u->link_mtu - sizeof(struct rtp_header) - sizeof(struct rtp_payload))
+ (u->link_mtu - sizeof(struct rtp_header) - sizeof(struct sbc_rtp_payload))
/ a2dp->frame_length * a2dp->codesize;
pa_sink_set_max_request_within_thread(u->sink, u->block_size);
@@ -1094,7 +1270,7 @@ static int source_process_msg(pa_msgobject *o, int code, void *data, int64_t off
if (!u->sink || u->sink->thread_info.state == PA_SINK_SUSPENDED) {
if (u->transport) {
if (bt_transport_acquire(u, TRUE) < 0)
- failed = TRUE;
+ failed = TRUE;
} else if (start_stream_fd(u) < 0)
failed = TRUE;
}
@@ -1313,7 +1489,7 @@ static void a2dp_prepare_buffer(struct userdata *u) {
static int a2dp_process_render(struct userdata *u) {
struct a2dp_info *a2dp;
struct rtp_header *header;
- struct rtp_payload *payload;
+ struct sbc_rtp_payload *payload;
size_t nbytes;
void *d;
const void *p;
@@ -1335,7 +1511,7 @@ static int a2dp_process_render(struct userdata *u) {
a2dp = &u->a2dp;
header = a2dp->buffer;
- payload = (struct rtp_payload*) ((uint8_t*) a2dp->buffer + sizeof(*header));
+ payload = (struct sbc_rtp_payload*) ((uint8_t*) a2dp->buffer + sizeof(*header));
frame_count = 0;
@@ -1443,6 +1619,387 @@ static int a2dp_process_render(struct userdata *u) {
return ret;
}
+
+static uint32_t uextract(uint32_t x, uint32_t position, uint32_t nbits)
+{
+ int mask;
+
+ pa_assert(position <= 31);
+ pa_assert(nbits <= 31);
+
+ x = x >> position;
+ mask = (1<<nbits)-1;
+ x = x & mask;
+
+ return x;
+}
+
+
+#define MPEG_LAYER_INDEX 4
+#define MPEG_BITRATE_INDEX 16
+#define MPEG_SAMPFREQ_INDEX 4
+#define MPEG_INDEX 2
+
+static unsigned short const mpeg_sampling_frequencies[MPEG_INDEX][MPEG_SAMPFREQ_INDEX] =
+ {
+ {22050, 24000, 16000, 0},
+ {44100, 48000, 32000, 0}
+ };
+
+static short const mpeg_layer_bitrates[MPEG_INDEX][MPEG_BITRATE_INDEX] =
+ {
+ { 0, 8, 16, 24, 32, 40, 48, 56, 64, 80, 96, 112, 128, 144, 160, -1},
+ { 0, 32, 40, 48, 56, 64, 80, 96, 112, 128, 160, 192, 224, 256, 320, -1}
+ };
+
+static short const mpeg_frame_length[MPEG_INDEX][MPEG_LAYER_INDEX] =
+ {
+ {0, 576, 1152, 384},
+ {0, 1152, 1152, 384}
+ };
+
+static pa_bool_t mp3_synclength(uint32_t hi, uint32_t *len, uint32_t *sample_len)
+{
+ unsigned int tmp;
+ unsigned int idex;
+ unsigned int id;
+ unsigned int bitrate;
+ unsigned int freq;
+ unsigned int bits;
+
+ pa_assert(len != NULL);
+ pa_assert(sample_len != NULL);
+
+ tmp = uextract(hi, 21, 11);
+ if (tmp == 0x7ff) { /* valid sync word */
+ tmp = uextract(hi, 19, 2);
+ if (tmp != 1) { /* valid IDex */
+
+ idex = tmp >> 1;
+ if (idex != 0) { /* MP3 2.5, not supported by A2DP */
+
+ id = tmp & 1;
+ tmp = uextract(hi, 17, 2);
+ if (tmp == 0x1) { /* layer 3 */
+
+ bitrate = uextract(hi, 12, 4);
+ if ((bitrate != 0) && /* not free format */
+ (bitrate != 0xf)) { /* not reserved */
+
+ freq = uextract(hi, 10, 2);
+ if (freq != 3) { /* valid sampling frequency */
+
+ tmp = uextract(hi, 9, 1);
+ bitrate = mpeg_layer_bitrates[id][bitrate] * 1000;
+ bits =
+ (unsigned int) ((bitrate * mpeg_frame_length[id][1]) /
+ mpeg_sampling_frequencies[id][freq]);
+
+ bits /= 8; /* # of bytes */
+ if (tmp) { /* padding */
+ bits += 1;
+ }
+
+ /* sanity check */
+ if (bits > 4 && bits <= 1728) { /* max frame length */
+ *len = bits;
+ *sample_len = mpeg_frame_length[id][1];
+ return TRUE;
+ }
+ }
+ }
+ }
+ }
+ }
+ } else {
+ if (hi!=0)
+ pa_log("no sync word found %x", hi);
+ }
+ return FALSE;
+}
+
+static pa_bool_t do_sync_iec958(const uint8_t **p_src, uint32_t *input_bytes)
+{
+ pa_bool_t sync_found = FALSE;
+ uint32_t preambles;
+ const uint8_t *src;
+
+ pa_assert(*p_src);
+ src = *p_src;
+
+ for (;;) {
+
+ if (*input_bytes <= 4)
+ break;
+
+ /* we need 4 bytes to detect the Pa,Pb preambles */
+ preambles = src[0]<<24 | src[1]<<16 | src[2]<<8 | src[3];
+#define IEC958_MAGIC_NUMBER 0x72F81F4E
+ if (preambles == IEC958_MAGIC_NUMBER) { /* little endian assumed */
+ sync_found = TRUE;
+ break;
+ } else {
+ /* try to find a new syncword */
+ src += 1;
+ *input_bytes -= 1;
+ }
+ }
+ *p_src=src;
+
+ return sync_found;
+}
+
+/* Run from IO thread */
+static int a2dp_passthrough_process_render(struct userdata *u) {
+ struct a2dp_info *a2dp;
+ struct rtp_header *header;
+ struct mpeg_rtp_payload *payload;
+ size_t nbytes;
+ uint8_t *d;
+ const uint8_t *p;
+ size_t bytes_remaining = 0; /* in the sink buffer */
+ pa_bool_t sync_found = FALSE;
+ pa_bool_t confirmed_sync;
+ uint32_t frame_length;
+ uint32_t frag_offset;
+ uint32_t max_bytes;
+ int ret = 0;
+
+ pa_assert(u);
+ pa_assert(u->profile == PROFILE_A2DP_PASSTHROUGH);
+ pa_assert(u->sink);
+
+ /* inits for output buffer */
+ a2dp_prepare_buffer(u);
+
+ /* allocate memory if needed */
+ if (!u->write_memchunk.memblock) {
+ pa_memchunk *c;
+
+ pa_assert(u->leftover_bytes == 0);
+ c = &u->write_memchunk;
+ c->memblock = pa_memblock_new(u->sink->core->mempool, u->block_size);
+ c->index = 0;
+ c->length = u->block_size;
+ }
+
+ /* if no sync was found, handle the remaining part of the previous buffer */
+ if (u->leftover_bytes) {
+ pa_memchunk *c;
+ pa_memblock *n;
+ void *tdata, *sdata;
+ size_t l;
+
+ c = &u->write_memchunk;
+ n = pa_memblock_new(u->sink->core->mempool, u->block_size);
+
+ sdata = pa_memblock_acquire(c->memblock);
+ tdata = pa_memblock_acquire(n);
+
+ l = c->length;
+ memcpy(tdata, (uint8_t*) sdata + c->index, l);
+
+ pa_memblock_release(c->memblock);
+ pa_memblock_release(n);
+
+ pa_memblock_unref(c->memblock);
+
+ c->memblock = n;
+ c->index = l;
+ c->length = u->block_size - l;
+ }
+
+ /* fill memchunck, previous leftover bytes have been copied into beginning of frame already */
+ pa_sink_render_into_full(u->sink, &u->write_memchunk);
+
+ bytes_remaining = u->block_size;
+
+ /* try to find an IEC958 pattern in the buffer */
+ p = (const uint8_t*) pa_memblock_acquire(u->write_memchunk.memblock);
+ sync_found = do_sync_iec958(&p, &bytes_remaining);
+ pa_memblock_release(u->write_memchunk.memblock);
+
+ /* update pointers */
+ u->write_memchunk.index = u->block_size-bytes_remaining;
+ u->write_memchunk.length = bytes_remaining;
+ u->leftover_bytes = bytes_remaining;
+
+ if (sync_found==TRUE && u->leftover_bytes!=u->block_size) {
+ /* only a partial frame found, handle in the next call */
+ sync_found = FALSE;
+ }
+
+ confirmed_sync = FALSE;
+ if (sync_found==TRUE) {
+ /* we are aligned on an IEC958 frame boundary */
+ p = (const uint8_t*) pa_memblock_acquire(u->write_memchunk.memblock);
+
+ if (p[4] == 0x05) { /* MPEG payload */
+ uint32_t frame_length2 = 0;
+ uint32_t sample_len = 0;
+
+ /* valid MPEG payload, now extract frame length */
+
+ frame_length = p[7]<<8 | p[6]; /* in bits */
+ frame_length >>= 3; /* in bytes */
+
+ if (mp3_synclength(p[9]<<24 | p[8]<<16 | p[11]<<8 | p[10], &frame_length2, &sample_len) == TRUE) {
+ if (frame_length == frame_length2) {
+ /* now we have verified that the IEC frame and MPEG frame are in
+ agreement, we should be good to go */
+ confirmed_sync = TRUE;
+ }
+ } else {
+ pa_log("IEC958 sync found but no MP3 sync found");
+ }
+ }
+ pa_memblock_release(u->write_memchunk.memblock);
+ }
+
+ /* no valid sync found, clean-up and return */
+ if (confirmed_sync == FALSE) {
+ int toflush;
+
+ toflush = u->block_size - bytes_remaining;
+ if (toflush == 0)
+ toflush = u->block_size;
+ u->write_index += (uint64_t)toflush;
+
+ /* clean-up and move on */
+ if (u->leftover_bytes == u->block_size) {
+ pa_memblock_unref(u->write_memchunk.memblock);
+ pa_memchunk_reset(&u->write_memchunk);
+ u->leftover_bytes = 0;
+ }
+
+ return 1;
+ }
+
+ /* now push the MP3 data into the A2DP link */
+
+ /* set pointers for RTP header and payload */
+ a2dp = &u->a2dp;
+ header = a2dp->buffer;
+ payload = (struct mpeg_rtp_payload*) ((uint8_t*) a2dp->buffer + sizeof(*header));
+
+ /* set pointer for MP3 frame */
+ p = (const uint8_t*) pa_memblock_acquire(u->write_memchunk.memblock);
+ p += 8;
+
+ max_bytes = u->link_mtu - sizeof(*header) - sizeof(*payload);
+ max_bytes >>= 1;
+ max_bytes <<= 1; /* make sure we handle byte pairs */
+
+ /* round to ceiling for frame length, required for byte swaps */
+ frame_length ++;
+ frame_length >>= 1;
+ frame_length <<= 1;
+
+ frag_offset = 0;
+
+ while (frame_length != 0) { /* several iterations if frame_length larger than mtu */
+ uint32_t length;
+ uint32_t i;
+
+ length = frame_length;
+ if (length > max_bytes)
+ length = max_bytes;
+ frame_length -= length;
+
+ d = (uint8_t*) a2dp->buffer + sizeof(*header) + sizeof(*payload);
+
+ /* swap bytes, input is assumed 16-bit little endian */
+ pa_assert(!(length&1));
+
+ for (i=0; i<length/2; i++) {
+ *(d+1) = *p;
+ *d = *(p+1);
+ d += 2;
+ p += 2;
+ }
+
+ nbytes = (uint8_t*) d - (uint8_t*) a2dp->buffer;
+ pa_assert(nbytes != 0);
+
+ /* fill RTP header and payload */
+ memset(a2dp->buffer, 0, sizeof(*header) + sizeof(*payload));
+ header->v = 2; /* rtp packet v2 */
+ header->pt = 14; /* MPA payload type */
+ header->timestamp =
+ htonl(u->write_index / pa_frame_size(&u->sample_spec)
+ * 90000
+ / u->sample_spec.rate); /* 90kHz timestamp */
+ header->m = 1; /* talk_spurt is always set */
+ header->sequence_number = htons(a2dp->seq_num);
+ header->ssrc = htonl(1);
+
+ payload->mbz = 0; /* always zero */
+ payload->frag_offset = frag_offset; /* non-zero only when frame is spread across multiple packets */
+
+ pa_assert(nbytes != 0);
+ pa_assert(nbytes <= u->link_mtu);
+
+ ret = 0;
+ for (;;) {
+ ssize_t l;
+
+ l = pa_write(u->stream_fd, a2dp->buffer, nbytes, &u->stream_write_type);
+
+ pa_assert(l != 0);
+
+ if (l < 0) {
+
+ if (errno == EINTR)
+ /* Retry right away if we got interrupted */
+ continue;
+
+ else if (errno == EAGAIN)
+ /* Hmm, apparently the socket was not writable, give up for now */
+ break;
+
+ pa_log_error("Failed to write data to socket: %s", pa_cstrerror(errno));
+ ret = -1;
+ break;
+ }
+
+ pa_assert((size_t) l <= nbytes);
+
+ if ((size_t) l != nbytes) {
+ pa_log_warn("Wrote memory block to socket only partially! %llu written, wanted to write %llu.",
+ (unsigned long long) l,
+ (unsigned long long) nbytes);
+ ret = -1;
+ break;
+ }
+
+ ret = 1;
+ break;
+ }
+
+ if (ret != 1) {
+ /* stop this loop */
+ frame_length = 0;
+ }
+
+ frag_offset += length;
+ }
+
+ pa_memblock_release(u->write_memchunk.memblock);
+
+ /* clean-up and move on */
+ if (u->leftover_bytes == u->block_size) {
+ pa_memblock_unref(u->write_memchunk.memblock);
+ pa_memchunk_reset(&u->write_memchunk);
+ u->leftover_bytes = 0;
+ }
+
+ /* update counters */
+ u->write_index += (uint64_t) u->block_size;
+ a2dp->seq_num++;
+
+ return ret;
+}
+
static int a2dp_process_push(struct userdata *u) {
int ret = 0;
pa_memchunk memchunk;
@@ -1460,7 +2017,7 @@ static int a2dp_process_push(struct userdata *u) {
pa_usec_t tstamp;
struct a2dp_info *a2dp;
struct rtp_header *header;
- struct rtp_payload *payload;
+ struct sbc_rtp_payload *payload;
const void *p;
void *d;
ssize_t l;
@@ -1471,7 +2028,7 @@ static int a2dp_process_push(struct userdata *u) {
a2dp = &u->a2dp;
header = a2dp->buffer;
- payload = (struct rtp_payload*) ((uint8_t*) a2dp->buffer + sizeof(*header));
+ payload = (struct sbc_rtp_payload*) ((uint8_t*) a2dp->buffer + sizeof(*header));
l = pa_read(u->stream_fd, a2dp->buffer, a2dp->buffer_size, &u->stream_write_type);
@@ -1691,6 +2248,9 @@ static void thread_func(void *userdata) {
if (u->profile == PROFILE_A2DP) {
if ((n_written = a2dp_process_render(u)) < 0)
goto fail;
+ } else if (u->profile == PROFILE_A2DP_PASSTHROUGH) {
+ if ((n_written = a2dp_passthrough_process_render(u)) < 0)
+ goto fail;
} else {
if ((n_written = hsp_process_render(u)) < 0)
goto fail;
@@ -2091,7 +2651,7 @@ static int add_sink(struct userdata *u) {
data.driver = __FILE__;
data.module = u->module;
pa_sink_new_data_set_sample_spec(&data, &u->sample_spec);
- pa_proplist_sets(data.proplist, "bluetooth.protocol", u->profile == PROFILE_A2DP ? "a2dp" : "sco");
+ pa_proplist_sets(data.proplist, "bluetooth.protocol", (u->profile == PROFILE_A2DP || u->profile == PROFILE_A2DP_PASSTHROUGH) ? "a2dp" : "sco");
if (u->profile == PROFILE_HSP)
pa_proplist_sets(data.proplist, PA_PROP_DEVICE_INTENDED_ROLES, "phone");
data.card = u->card;
@@ -2117,7 +2677,9 @@ static int add_sink(struct userdata *u) {
pa_sink_set_max_request(u->sink, u->block_size);
pa_sink_set_fixed_latency(u->sink,
- (u->profile == PROFILE_A2DP ? FIXED_LATENCY_PLAYBACK_A2DP : FIXED_LATENCY_PLAYBACK_HSP) +
+ (((u->profile == PROFILE_A2DP)||
+ (u->profile == PROFILE_A2DP_PASSTHROUGH)) ?
+ FIXED_LATENCY_PLAYBACK_A2DP : FIXED_LATENCY_PLAYBACK_HSP) +
pa_bytes_to_usec(u->block_size, &u->sample_spec));
}
@@ -2130,6 +2692,9 @@ static int add_sink(struct userdata *u) {
pa_xfree(k);
}
+ if (u->profile == PROFILE_A2DP_PASSTHROUGH)
+ u->sink->flags |= PA_SINK_PASSTHROUGH;
+
return 0;
}
@@ -2336,9 +2901,9 @@ static int bt_transport_config_a2dp(struct userdata *u) {
a2dp->frame_length = sbc_get_frame_length(&a2dp->sbc);
u->block_size =
- ((u->link_mtu - sizeof(struct rtp_header) - sizeof(struct rtp_payload))
- / a2dp->frame_length
- * a2dp->codesize);
+ ((u->link_mtu - sizeof(struct rtp_header) - sizeof(struct sbc_rtp_payload))
+ / a2dp->frame_length
+ * a2dp->codesize);
pa_log_info("SBC parameters:\n\tallocation=%u\n\tsubbands=%u\n\tblocks=%u\n\tbitpool=%u\n",
a2dp->sbc.allocation, a2dp->sbc.subbands, a2dp->sbc.blocks, a2dp->sbc.bitpool);
@@ -2442,6 +3007,7 @@ static int init_profile(struct userdata *u) {
return -1;
if (u->profile == PROFILE_A2DP ||
+ u->profile == PROFILE_A2DP_PASSTHROUGH ||
u->profile == PROFILE_HSP ||
u->profile == PROFILE_HFGW)
if (add_sink(u) < 0)
@@ -2627,6 +3193,10 @@ static int card_set_profile(pa_card *c, pa_card_profile *new_profile) {
pa_log_warn("A2DP is not connected, refused to switch profile");
return -PA_ERR_IO;
}
+ else if (device->audio_sink_state < PA_BT_AUDIO_STATE_CONNECTED && *d == PROFILE_A2DP_PASSTHROUGH) {
+ pa_log_warn("A2DP Passthrough is not connected, refused to switch profile");
+ return -PA_ERR_IO;
+ }
else if (device->hfgw_state < PA_BT_AUDIO_STATE_CONNECTED && *d == PROFILE_HFGW) {
pa_log_warn("HandsfreeGateway is not connected, refused to switch profile");
return -PA_ERR_IO;
@@ -2746,6 +3316,20 @@ static int add_card(struct userdata *u, const pa_bluetooth_device *device) {
*d = PROFILE_A2DP;
pa_hashmap_put(data.profiles, p->name, p);
+
+ /* add passthrough profile */
+ p = pa_card_profile_new("passthrough", _("MP3 passthrough (A2DP)"), sizeof(enum profile));
+ p->priority = 5;
+ p->n_sinks = 1;
+ p->n_sources = 0;
+ p->max_sink_channels = 2;
+ p->max_source_channels = 0;
+
+ d = PA_CARD_PROFILE_DATA(p);
+ *d = PROFILE_A2DP_PASSTHROUGH;
+
+ pa_hashmap_put(data.profiles, p->name, p);
+
}
if (pa_bluetooth_uuid_has(device->uuids, A2DP_SOURCE_UUID)) {
@@ -2820,6 +3404,7 @@ static int add_card(struct userdata *u, const pa_bluetooth_device *device) {
if ((device->headset_state < PA_BT_AUDIO_STATE_CONNECTED && *d == PROFILE_HSP) ||
(device->audio_sink_state < PA_BT_AUDIO_STATE_CONNECTED && *d == PROFILE_A2DP) ||
+ (device->audio_sink_state < PA_BT_AUDIO_STATE_CONNECTED && *d == PROFILE_A2DP_PASSTHROUGH) ||
(device->hfgw_state < PA_BT_AUDIO_STATE_CONNECTED && *d == PROFILE_HFGW)) {
pa_log_warn("Default profile not connected, selecting off profile");
u->card->active_profile = pa_hashmap_get(u->card->profiles, "off");
diff --git a/src/modules/bluetooth/rtp.h b/src/modules/bluetooth/rtp.h
index 45fddcf..e30ea1a 100644
--- a/src/modules/bluetooth/rtp.h
+++ b/src/modules/bluetooth/rtp.h
@@ -38,7 +38,7 @@ struct rtp_header {
uint32_t csrc[0];
} __attribute__ ((packed));
-struct rtp_payload {
+struct sbc_rtp_payload {
unsigned frame_count:4;
unsigned rfa0:1;
unsigned is_last_fragment:1;
@@ -46,6 +46,11 @@ struct rtp_payload {
unsigned is_fragmented:1;
} __attribute__ ((packed));
+struct mpeg_rtp_payload {
+ unsigned mbz:16;
+ unsigned frag_offset:16;
+} __attribute__ ((packed));
+
#elif __BYTE_ORDER == __BIG_ENDIAN
struct rtp_header {
@@ -63,7 +68,7 @@ struct rtp_header {
uint32_t csrc[0];
} __attribute__ ((packed));
-struct rtp_payload {
+struct sbc_rtp_payload {
unsigned is_fragmented:1;
unsigned is_first_fragment:1;
unsigned is_last_fragment:1;
@@ -71,6 +76,11 @@ struct rtp_payload {
unsigned frame_count:4;
} __attribute__ ((packed));
+struct mpeg_rtp_payload {
+ unsigned frag_offset:16;
+ unsigned mbz:16;
+} __attribute__ ((packed));
+
#else
#error "Unknown byte order"
#endif
--
1.7.5.4
