[pulseaudio-discuss] [PATCH v3 7/7] bluetooth: Add A2DP FastStream codec support
Pali Rohár
pali.rohar at gmail.com
Sat Jan 12 15:21:56 UTC 2019
Now only main channel, voice backchannel is not implemented yet.
---
src/Makefile.am | 2 +
src/modules/bluetooth/a2dp-codec-faststream.c | 401 ++++++++++++++++++++++++++
src/modules/bluetooth/a2dp-codec-util.c | 2 +
3 files changed, 405 insertions(+)
create mode 100644 src/modules/bluetooth/a2dp-codec-faststream.c
diff --git a/src/Makefile.am b/src/Makefile.am
index 7c7f1b564..26c0e0794 100644
--- a/src/Makefile.am
+++ b/src/Makefile.am
@@ -2146,6 +2146,8 @@ libbluez5_util_la_SOURCES += modules/bluetooth/a2dp-codec-sbc.c
libbluez5_util_la_LIBADD += $(SBC_LIBS)
libbluez5_util_la_CFLAGS += $(SBC_CFLAGS)
+libbluez5_util_la_SOURCES += modules/bluetooth/a2dp-codec-faststream.c
+
if HAVE_OPENAPTX
libbluez5_util_la_SOURCES += modules/bluetooth/a2dp-codec-aptx.c
libbluez5_util_la_CPPFLAGS += $(OPENAPTX_CPPFLAGS)
diff --git a/src/modules/bluetooth/a2dp-codec-faststream.c b/src/modules/bluetooth/a2dp-codec-faststream.c
new file mode 100644
index 000000000..8c910ea8b
--- /dev/null
+++ b/src/modules/bluetooth/a2dp-codec-faststream.c
@@ -0,0 +1,401 @@
+/***
+ This file is part of PulseAudio.
+
+ Copyright 2018-2019 Pali Rohár <pali.rohar at gmail.com>
+
+ PulseAudio is free software; you can redistribute it and/or modify
+ it under the terms of the GNU Lesser General Public License as
+ published by the Free Software Foundation; either version 2.1 of the
+ License, or (at your option) any later version.
+
+ PulseAudio is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with PulseAudio; if not, see <http://www.gnu.org/licenses/>.
+***/
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <pulsecore/core-util.h>
+#include <pulsecore/log.h>
+#include <pulsecore/macro.h>
+#include <pulsecore/once.h>
+#include <pulse/sample.h>
+#include <pulse/xmalloc.h>
+
+#include <sbc/sbc.h>
+
+#include "a2dp-codecs.h"
+#include "a2dp-codec-api.h"
+
+#define SBC_BITPOOL_DEC_LIMIT 32
+#define SBC_BITPOOL_DEC_STEP 5
+
+struct faststream_info {
+ sbc_t sbc; /* Codec data */
+ size_t codesize, frame_length; /* SBC Codesize, frame_length. We simply cache those values here */
+};
+
+static bool accept_capabilities(const uint8_t *capabilities_buffer, uint8_t capabilities_size, bool for_encoding) {
+ const a2dp_faststream_t *capabilities = (const a2dp_faststream_t *) capabilities_buffer;
+
+ if (A2DP_GET_VENDOR_ID(capabilities->info) != FASTSTREAM_VENDOR_ID || A2DP_GET_CODEC_ID(capabilities->info) != FASTSTREAM_CODEC_ID)
+ return false;
+
+ if (!(capabilities->direction & (FASTSTREAM_DIRECTION_SINK | FASTSTREAM_DIRECTION_SOURCE)))
+ return false;
+
+ if (capabilities->direction & FASTSTREAM_DIRECTION_SINK) {
+ if (!(capabilities->sink_frequency & (FASTSTREAM_SINK_SAMPLING_FREQ_44100 | FASTSTREAM_SINK_SAMPLING_FREQ_48000)))
+ return false;
+ } else {
+ if (capabilities->sink_frequency != 0)
+ return false;
+ }
+
+ if (capabilities->direction & FASTSTREAM_DIRECTION_SOURCE) {
+ if (!(capabilities->source_frequency & FASTSTREAM_SOURCE_SAMPLING_FREQ_16000))
+ return false;
+ } else {
+ if (capabilities->source_frequency != 0)
+ return false;
+ }
+
+ return true;
+}
+
+static const char *choose_capabilities(const pa_hashmap *capabilities_hashmap, bool for_encoding) {
+ const pa_a2dp_codec_capabilities *a2dp_capabilities;
+ const char *key;
+ void *state;
+
+ /* There is no preference, just choose random valid entry */
+ PA_HASHMAP_FOREACH_KV(key, a2dp_capabilities, capabilities_hashmap, state) {
+ if (accept_capabilities(a2dp_capabilities->buffer, a2dp_capabilities->size, for_encoding))
+ return key;
+ }
+
+ return NULL;
+}
+
+static uint8_t fill_capabilities(uint8_t capabilities_buffer[254]) {
+ a2dp_faststream_t *capabilities = (a2dp_faststream_t *) capabilities_buffer;
+
+ pa_zero(*capabilities);
+
+ capabilities->info = A2DP_SET_VENDOR_ID_CODEC_ID(FASTSTREAM_VENDOR_ID, FASTSTREAM_CODEC_ID);
+ capabilities->direction = FASTSTREAM_DIRECTION_SINK | FASTSTREAM_DIRECTION_SOURCE;
+ capabilities->sink_frequency = FASTSTREAM_SINK_SAMPLING_FREQ_44100 | FASTSTREAM_SINK_SAMPLING_FREQ_48000;
+ capabilities->source_frequency = FASTSTREAM_SOURCE_SAMPLING_FREQ_16000;
+
+ return sizeof(*capabilities);
+}
+
+static bool validate_configuration(const uint8_t *config_buffer, uint8_t config_size) {
+ const a2dp_faststream_t *config = (const a2dp_faststream_t *) config_buffer;
+
+ if (config_size != sizeof(*config)) {
+ pa_log_error("Invalid size of config buffer");
+ return false;
+ }
+
+ if (A2DP_GET_VENDOR_ID(config->info) != FASTSTREAM_VENDOR_ID || A2DP_GET_CODEC_ID(config->info) != FASTSTREAM_CODEC_ID) {
+ pa_log_error("Invalid vendor codec information in configuration");
+ return false;
+ }
+
+ if (config->direction & ~(FASTSTREAM_DIRECTION_SINK | FASTSTREAM_DIRECTION_SOURCE)) {
+ pa_log_error("Invalid direction in configuration");
+ return false;
+ }
+
+ if (config->direction & FASTSTREAM_DIRECTION_SINK) {
+ if (config->sink_frequency != FASTSTREAM_SINK_SAMPLING_FREQ_44100 && config->sink_frequency != FASTSTREAM_SINK_SAMPLING_FREQ_48000) {
+ pa_log_error("Invalid sampling sink frequency in configuration");
+ return false;
+ }
+ } else {
+ if (config->sink_frequency != 0) {
+ pa_log_error("Invalid sampling sink frequency in configuration");
+ return false;
+ }
+ }
+
+ if (config->direction & FASTSTREAM_DIRECTION_SOURCE) {
+ if (config->source_frequency != FASTSTREAM_SOURCE_SAMPLING_FREQ_16000) {
+ pa_log_error("Invalid sampling source frequency in configuration");
+ return false;
+ }
+ } else {
+ if (config->source_frequency != 0) {
+ pa_log_error("Invalid sampling source frequency in configuration");
+ return false;
+ }
+ }
+
+ return true;
+}
+
+static uint8_t fill_preferred_configuration(const pa_sample_spec *sample_spec, const uint8_t *capabilities_buffer, uint8_t capabilities_size, uint8_t config_buffer[254]) {
+ a2dp_faststream_t *config = (a2dp_faststream_t *) config_buffer;
+ const a2dp_faststream_t *capabilities = (const a2dp_faststream_t *) capabilities_buffer;
+ int i;
+
+ static const struct {
+ uint32_t rate;
+ uint8_t cap;
+ } freq_table[] = {
+ { 44100U, FASTSTREAM_SINK_SAMPLING_FREQ_44100 },
+ { 48000U, FASTSTREAM_SINK_SAMPLING_FREQ_48000 }
+ };
+
+ if (capabilities_size != sizeof(*capabilities)) {
+ pa_log_error("Invalid size of capabilities buffer");
+ return 0;
+ }
+
+ pa_zero(*config);
+
+ if (A2DP_GET_VENDOR_ID(capabilities->info) != FASTSTREAM_VENDOR_ID || A2DP_GET_CODEC_ID(capabilities->info) != FASTSTREAM_CODEC_ID) {
+ pa_log_error("No supported vendor codec information");
+ return 0;
+ }
+
+ config->info = A2DP_SET_VENDOR_ID_CODEC_ID(FASTSTREAM_VENDOR_ID, FASTSTREAM_CODEC_ID);
+
+ /* Find the lowest freq that is at least as high as the requested sampling rate */
+ for (i = 0; (unsigned) i < PA_ELEMENTSOF(freq_table); i++)
+ if (freq_table[i].rate >= sample_spec->rate && (capabilities->sink_frequency & freq_table[i].cap)) {
+ config->sink_frequency = freq_table[i].cap;
+ break;
+ }
+
+ if ((unsigned) i == PA_ELEMENTSOF(freq_table)) {
+ for (--i; i >= 0; i--) {
+ if (capabilities->sink_frequency & freq_table[i].cap) {
+ config->sink_frequency = freq_table[i].cap;
+ break;
+ }
+ }
+
+ if (i < 0) {
+ pa_log_error("Not suitable sample rate");
+ return 0;
+ }
+ }
+
+ pa_assert((unsigned) i < PA_ELEMENTSOF(freq_table));
+
+ if (!(capabilities->direction & FASTSTREAM_DIRECTION_SINK)) {
+ pa_log_error("No sink support");
+ return 0;
+ }
+
+ config->direction = capabilities->direction;
+ config->source_frequency = capabilities->source_frequency;
+
+ return sizeof(*config);
+}
+
+static void *init_codec(bool for_encoding, bool for_backchannel, const uint8_t *config_buffer, uint8_t config_size, pa_sample_spec *sample_spec) {
+ struct faststream_info *faststream_info;
+ const a2dp_faststream_t *config = (const a2dp_faststream_t *) config_buffer;
+ int ret;
+
+ pa_assert(config_size == sizeof(*config));
+
+ faststream_info = pa_xnew0(struct faststream_info, 1);
+
+ ret = sbc_init(&faststream_info->sbc, 0);
+ if (ret != 0) {
+ pa_xfree(faststream_info);
+ pa_log_error("SBC initialization failed: %d", ret);
+ return NULL;
+ }
+
+ sample_spec->format = PA_SAMPLE_S16LE;
+
+ switch (config->sink_frequency) {
+ case FASTSTREAM_SINK_SAMPLING_FREQ_44100:
+ faststream_info->sbc.frequency = SBC_FREQ_44100;
+ sample_spec->rate = 44100U;
+ break;
+ case FASTSTREAM_SINK_SAMPLING_FREQ_48000:
+ faststream_info->sbc.frequency = SBC_FREQ_48000;
+ sample_spec->rate = 48000U;
+ break;
+ default:
+ pa_assert_not_reached();
+ }
+
+ sample_spec->format = PA_SAMPLE_S16LE;
+ sample_spec->channels = 2;
+
+ faststream_info->sbc.mode = SBC_MODE_JOINT_STEREO;
+ faststream_info->sbc.allocation = SBC_AM_LOUDNESS;
+ faststream_info->sbc.subbands = SBC_SB_8;
+ faststream_info->sbc.blocks = SBC_BLK_16;
+ faststream_info->sbc.bitpool = 29;
+ faststream_info->codesize = sbc_get_codesize(&faststream_info->sbc);
+ faststream_info->frame_length = sbc_get_frame_length(&faststream_info->sbc);
+
+ /* Frame length in FastStream is 71 bytes, but rounded to 72 */
+ pa_assert(faststream_info->frame_length == 71);
+
+ PA_ONCE_BEGIN {
+ pa_log_debug("Using FastStream codec with SBC codec implementation: %s", pa_strnull(sbc_get_implementation_info(&faststream_info->sbc)));
+ } PA_ONCE_END;
+
+ pa_log_info("SBC parameters: allocation=%u, subbands=%u, blocks=%u, bitpool=%u codesize=%zu frame_length=%zu",
+ faststream_info->sbc.allocation, faststream_info->sbc.subbands ? 8 : 4, faststream_info->sbc.blocks, faststream_info->sbc.bitpool, faststream_info->codesize, faststream_info->frame_length);
+
+ return faststream_info;
+}
+
+static void finish_codec(void *codec_info) {
+ struct faststream_info *faststream_info = (struct faststream_info *) codec_info;
+
+ sbc_finish(&faststream_info->sbc);
+ pa_xfree(faststream_info);
+}
+
+static void reset_codec(void *codec_info) {
+ struct faststream_info *faststream_info = (struct faststream_info *) codec_info;
+ int ret;
+
+ ret = sbc_reinit(&faststream_info->sbc, 0);
+ if (ret != 0)
+ pa_log_error("SBC reinitialization failed: %d", ret);
+}
+
+static void fill_blocksize(void *codec_info, size_t read_link_mtu, size_t write_link_mtu, size_t *read_block_size, size_t *write_block_size) {
+ struct faststream_info *faststream_info = (struct faststream_info *) codec_info;
+
+ /* Packet size is 220 (= (71+1)*3 + 4) therefore 3 samples */
+ *read_block_size = 3 * faststream_info->codesize;
+ *write_block_size = 3 * faststream_info->codesize;
+}
+
+static bool reduce_encoder_bitrate(void *codec_info, size_t write_link_mtu, size_t *write_block_size) {
+ return false;
+}
+
+static size_t encode_buffer(void *codec_info, uint32_t timestamp, const uint8_t *input_buffer, size_t input_size, uint8_t *output_buffer, size_t output_size, size_t *processed) {
+ struct faststream_info *faststream_info = (struct faststream_info *) codec_info;
+ uint8_t *d;
+ const uint8_t *p;
+ size_t to_write, to_encode;
+
+ p = input_buffer;
+ to_encode = input_size;
+
+ d = output_buffer;
+ to_write = output_size;
+
+ while (PA_LIKELY(to_encode > 0 && to_write > 0)) {
+ ssize_t written;
+ ssize_t encoded;
+
+ encoded = sbc_encode(&faststream_info->sbc,
+ p, to_encode,
+ d, to_write,
+ &written);
+
+ if (PA_UNLIKELY(encoded <= 0)) {
+ pa_log_error("SBC encoding error (%li)", (long) encoded);
+ *processed = p - input_buffer;
+ return 0;
+ }
+
+ pa_assert_fp((size_t) encoded <= to_encode);
+ pa_assert_fp((size_t) encoded == faststream_info->codesize);
+
+ pa_assert_fp((size_t) written <= to_write);
+ pa_assert_fp((size_t) written == faststream_info->frame_length);
+
+ p += encoded;
+ to_encode -= encoded;
+
+ d += written;
+ to_write -= written;
+
+ /* frame length is 71 and it is rounded to 72, so put nul byte */
+ *(d++) = 0;
+ to_write--;
+ }
+
+ *processed = p - input_buffer;
+ return d - output_buffer;
+}
+
+static size_t decode_buffer(void *codec_info, const uint8_t *input_buffer, size_t input_size, uint8_t *output_buffer, size_t output_size, size_t *processed) {
+ struct faststream_info *faststream_info = (struct faststream_info *) codec_info;
+
+ const uint8_t *p;
+ uint8_t *d;
+ size_t to_write, to_decode;
+
+ p = input_buffer;
+ to_decode = input_size;
+
+ d = output_buffer;
+ to_write = output_size;
+
+ while (PA_LIKELY(to_decode > 0)) {
+ size_t written;
+ ssize_t decoded;
+
+ decoded = sbc_decode(&faststream_info->sbc,
+ p, to_decode,
+ d, to_write,
+ &written);
+
+ if (PA_UNLIKELY(decoded <= 0)) {
+ pa_log_error("SBC decoding error (%li)", (long) decoded);
+ *processed = p - input_buffer;
+ return 0;
+ }
+
+ pa_assert_fp((size_t) decoded <= to_decode);
+ pa_assert_fp((size_t) decoded == faststream_info->frame_length-1);
+
+ pa_assert_fp((size_t) written == faststream_info->codesize);
+
+ p += decoded;
+ to_decode -= decoded;
+
+ d += written;
+ to_write -= written;
+
+ /* frame length is 71 and it is rounded to 72, so skip one byte */
+ p++;
+ to_decode--;
+ }
+
+ *processed = p - input_buffer;
+ return d - output_buffer;
+}
+
+const pa_a2dp_codec pa_a2dp_codec_faststream = {
+ .codec_name = "faststream",
+ .codec_description = "FastStream",
+ .codec_id = { A2DP_CODEC_VENDOR, FASTSTREAM_VENDOR_ID, FASTSTREAM_CODEC_ID },
+ .support_backchannel = false, /* TODO: Implement support for backchannel */
+ .accept_capabilities = accept_capabilities,
+ .choose_capabilities = choose_capabilities,
+ .fill_capabilities = fill_capabilities,
+ .validate_configuration = validate_configuration,
+ .fill_preferred_configuration = fill_preferred_configuration,
+ .init_codec = init_codec,
+ .finish_codec = finish_codec,
+ .reset_codec = reset_codec,
+ .fill_blocksize = fill_blocksize,
+ .reduce_encoder_bitrate = reduce_encoder_bitrate,
+ .encode_buffer = encode_buffer,
+ .decode_buffer = decode_buffer,
+};
diff --git a/src/modules/bluetooth/a2dp-codec-util.c b/src/modules/bluetooth/a2dp-codec-util.c
index 1f685f2d3..ab3b341a5 100644
--- a/src/modules/bluetooth/a2dp-codec-util.c
+++ b/src/modules/bluetooth/a2dp-codec-util.c
@@ -26,6 +26,7 @@
#include "a2dp-codec-util.h"
+extern const pa_a2dp_codec pa_a2dp_codec_faststream;
extern const pa_a2dp_codec pa_a2dp_codec_sbc;
#ifdef HAVE_OPENAPTX
extern const pa_a2dp_codec pa_a2dp_codec_aptx;
@@ -34,6 +35,7 @@ extern const pa_a2dp_codec pa_a2dp_codec_aptx;
/* This is list of supported codecs. Their order is important.
* Codec with higher index has higher priority. */
const pa_a2dp_codec *pa_a2dp_codecs[] = {
+ &pa_a2dp_codec_faststream,
&pa_a2dp_codec_sbc,
#ifdef HAVE_OPENAPTX
&pa_a2dp_codec_aptx,
--
2.11.0
More information about the pulseaudio-discuss
mailing list