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@xxxxxxxxx> + + 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 _______________________________________________ pulseaudio-discuss mailing list pulseaudio-discuss@xxxxxxxxxxxxxxxxxxxxx https://lists.freedesktop.org/mailman/listinfo/pulseaudio-discuss
