bluez5: add FastStream codec

A2DP codec that uses SBC.  Has a duplex channel, which sends an SBC
stream back.

spa/plugins/bluez5/a2dp-codec-faststream.c

@@ -0,0 +1,612 @@
+/* Spa A2DP FastStream codec
+ *
+ * Copyright © 2020 Wim Taymans
+ * Copyright © 2021 Pauli Virtanen
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ */
+
+#include <unistd.h>
+#include <stddef.h>
+#include <errno.h>
+#include <arpa/inet.h>
+#if __BYTE_ORDER != __LITTLE_ENDIAN
+#include <byteswap.h>
+#endif
+
+#include <spa/param/audio/format.h>
+#include <spa/param/audio/format-utils.h>
+
+#include <sbc/sbc.h>
+
+#include "defs.h"
+#include "a2dp-codecs.h"
+
+struct impl {
+	sbc_t sbc;
+
+	size_t mtu;
+	int codesize;
+	int frame_count;
+	int max_frames;
+};
+
+struct duplex_impl {
+	sbc_t sbc;
+};
+
+static int codec_fill_caps(const struct a2dp_codec *codec, uint32_t flags,
+		uint8_t caps[A2DP_MAX_CAPS_SIZE])
+{
+	const a2dp_faststream_t a2dp_faststream = {
+		.info = codec->vendor,
+		.direction = FASTSTREAM_DIRECTION_SINK |
+			(codec->duplex_codec ? FASTSTREAM_DIRECTION_SOURCE : 0),
+		.sink_frequency =
+			FASTSTREAM_SINK_SAMPLING_FREQ_44100 |
+			FASTSTREAM_SINK_SAMPLING_FREQ_48000,
+		.source_frequency =
+			FASTSTREAM_SOURCE_SAMPLING_FREQ_16000,
+	};
+
+	memcpy(caps, &a2dp_faststream, sizeof(a2dp_faststream));
+	return sizeof(a2dp_faststream);
+}
+
+static const struct a2dp_codec_config
+frequencies[] = {
+	{ FASTSTREAM_SINK_SAMPLING_FREQ_48000, 48000, 1 },
+	{ FASTSTREAM_SINK_SAMPLING_FREQ_44100, 44100, 0 },
+};
+
+static const struct a2dp_codec_config
+duplex_frequencies[] = {
+	{ FASTSTREAM_SOURCE_SAMPLING_FREQ_16000, 16000, 0 },
+};
+
+static int codec_select_config(const struct a2dp_codec *codec, uint32_t flags,
+		const void *caps, size_t caps_size,
+		const struct a2dp_codec_audio_info *info,
+		const struct spa_dict *settings, uint8_t config[A2DP_MAX_CAPS_SIZE])
+{
+	a2dp_faststream_t conf;
+	int i;
+
+	if (caps_size < sizeof(conf))
+		return -EINVAL;
+
+	memcpy(&conf, caps, sizeof(conf));
+
+	if (codec->vendor.vendor_id != conf.info.vendor_id ||
+	    codec->vendor.codec_id != conf.info.codec_id)
+		return -ENOTSUP;
+
+	if (codec->duplex_codec && !(conf.direction & FASTSTREAM_DIRECTION_SOURCE))
+		return -ENOTSUP;
+
+	if (!(conf.direction & FASTSTREAM_DIRECTION_SINK))
+		return -ENOTSUP;
+
+	conf.direction = FASTSTREAM_DIRECTION_SINK;
+
+	if ((i = a2dp_codec_select_config(frequencies,
+			SPA_N_ELEMENTS(frequencies),
+			conf.sink_frequency,
+			info ? info->rate : A2DP_CODEC_DEFAULT_RATE
+			)) < 0)
+		return -ENOTSUP;
+	conf.sink_frequency = frequencies[i].config;
+
+	if ((i = a2dp_codec_select_config(duplex_frequencies,
+			SPA_N_ELEMENTS(duplex_frequencies),
+			conf.source_frequency,
+			16000
+			)) < 0)
+		return -ENOTSUP;
+	conf.source_frequency = duplex_frequencies[i].config;
+
+	memcpy(config, &conf, sizeof(conf));
+
+	return sizeof(conf);
+}
+
+static int codec_enum_config(const struct a2dp_codec *codec,
+		const void *caps, size_t caps_size, uint32_t id, uint32_t idx,
+		struct spa_pod_builder *b, struct spa_pod **param)
+{
+	a2dp_faststream_t conf;
+        struct spa_pod_frame f[2];
+	struct spa_pod_choice *choice;
+	uint32_t position[SPA_AUDIO_MAX_CHANNELS];
+	uint32_t i = 0;
+
+	if (caps_size < sizeof(conf))
+		return -EINVAL;
+
+	memcpy(&conf, caps, sizeof(conf));
+
+	if (idx > 0)
+		return 0;
+
+	spa_pod_builder_push_object(b, &f[0], SPA_TYPE_OBJECT_Format, id);
+	spa_pod_builder_add(b,
+			SPA_FORMAT_mediaType,      SPA_POD_Id(SPA_MEDIA_TYPE_audio),
+			SPA_FORMAT_mediaSubtype,   SPA_POD_Id(SPA_MEDIA_SUBTYPE_raw),
+			SPA_FORMAT_AUDIO_format,   SPA_POD_Id(SPA_AUDIO_FORMAT_S16),
+			0);
+	spa_pod_builder_prop(b, SPA_FORMAT_AUDIO_rate, 0);
+
+	spa_pod_builder_push_choice(b, &f[1], SPA_CHOICE_None, 0);
+	choice = (struct spa_pod_choice*)spa_pod_builder_frame(b, &f[1]);
+	i = 0;
+	if (conf.sink_frequency & FASTSTREAM_SINK_SAMPLING_FREQ_48000) {
+		if (i++ == 0)
+			spa_pod_builder_int(b, 48000);
+		spa_pod_builder_int(b, 48000);
+	}
+	if (conf.sink_frequency & FASTSTREAM_SINK_SAMPLING_FREQ_44100) {
+		if (i++ == 0)
+			spa_pod_builder_int(b, 44100);
+		spa_pod_builder_int(b, 44100);
+	}
+	if (i == 0)
+		return -EINVAL;
+	if (i > 1)
+		choice->body.type = SPA_CHOICE_Enum;
+	spa_pod_builder_pop(b, &f[1]);
+
+	position[0] = SPA_AUDIO_CHANNEL_FL;
+	position[1] = SPA_AUDIO_CHANNEL_FR;
+	spa_pod_builder_add(b,
+			SPA_FORMAT_AUDIO_channels, SPA_POD_Int(2),
+			SPA_FORMAT_AUDIO_position, SPA_POD_Array(sizeof(uint32_t),
+					SPA_TYPE_Id, 2, position),
+			0);
+
+	*param = spa_pod_builder_pop(b, &f[0]);
+	return *param == NULL ? -EIO : 1;
+}
+
+static int codec_reduce_bitpool(void *data)
+{
+	return -ENOTSUP;
+}
+
+static int codec_increase_bitpool(void *data)
+{
+	return -ENOTSUP;
+}
+
+static int codec_get_block_size(void *data)
+{
+	struct impl *this = data;
+	return this->codesize;
+}
+
+static size_t ceil2(size_t v)
+{
+	if (v % 2 != 0 && v < SIZE_MAX)
+		v += 1;
+	return v;
+}
+
+static void *codec_init(const struct a2dp_codec *codec, uint32_t flags,
+		void *config, size_t config_len, const struct spa_audio_info *info,
+		void *props, size_t mtu)
+{
+	a2dp_faststream_t *conf = config;
+	struct impl *this;
+	bool sbc_initialized = false;
+	int res;
+
+	if ((this = calloc(1, sizeof(struct impl))) == NULL)
+		goto error_errno;
+
+	if ((res = sbc_init(&this->sbc, 0)) < 0)
+		goto error;
+
+	sbc_initialized = true;
+	this->sbc.endian = SBC_LE;
+	this->mtu = mtu;
+
+	if (info->media_type != SPA_MEDIA_TYPE_audio ||
+	    info->media_subtype != SPA_MEDIA_SUBTYPE_raw ||
+	    info->info.raw.format != SPA_AUDIO_FORMAT_S16) {
+		res = -EINVAL;
+		goto error;
+	}
+
+	switch (conf->sink_frequency) {
+	case FASTSTREAM_SINK_SAMPLING_FREQ_44100:
+		this->sbc.frequency = SBC_FREQ_44100;
+		break;
+	case FASTSTREAM_SINK_SAMPLING_FREQ_48000:
+		this->sbc.frequency = SBC_FREQ_48000;
+		break;
+	default:
+		res = -EINVAL;
+                goto error;
+        }
+
+	this->sbc.mode = SBC_MODE_JOINT_STEREO;
+	this->sbc.subbands = SBC_SB_8;
+	this->sbc.allocation = SBC_AM_LOUDNESS;
+	this->sbc.blocks = SBC_BLK_16;
+	this->sbc.bitpool = 29;
+
+	this->codesize = sbc_get_codesize(&this->sbc);
+
+	this->max_frames = 3;
+	if (this->mtu < this->max_frames * ceil2(sbc_get_frame_length(&this->sbc))) {
+		res = -EINVAL;
+		goto error;
+	}
+
+	return this;
+
+error_errno:
+	res = -errno;
+	goto error;
+
+error:
+	if (sbc_initialized)
+		sbc_finish(&this->sbc);
+	free(this);
+	errno = -res;
+	return NULL;
+}
+
+static void codec_deinit(void *data)
+{
+	struct impl *this = data;
+	sbc_finish(&this->sbc);
+	free(this);
+}
+
+static int codec_abr_process (void *data, size_t unsent)
+{
+	return -ENOTSUP;
+}
+
+static int codec_start_encode (void *data,
+		void *dst, size_t dst_size, uint16_t seqnum, uint32_t timestamp)
+{
+	struct impl *this = data;
+	this->frame_count = 0;
+	return 0;
+}
+
+static int codec_encode(void *data,
+		const void *src, size_t src_size,
+		void *dst, size_t dst_size,
+		size_t *dst_out, int *need_flush)
+{
+	struct impl *this = data;
+	int res;
+
+	res = sbc_encode(&this->sbc, src, src_size,
+			dst, dst_size, (ssize_t*)dst_out);
+	if (SPA_UNLIKELY(res < 0))
+		return -EINVAL;
+	spa_assert(res == this->codesize);
+
+	if (*dst_out % 2 != 0 && *dst_out < dst_size) {
+		/* Pad similarly as in input stream */
+		*((uint8_t *)dst + *dst_out) = 0;
+		++*dst_out;
+	}
+
+	this->frame_count += res / this->codesize;
+	*need_flush = this->frame_count >= this->max_frames;
+	return res;
+}
+
+static int codec_start_decode (void *data,
+		const void *src, size_t src_size, uint16_t *seqnum, uint32_t *timestamp)
+{
+	return 0;
+}
+
+static int do_decode(sbc_t *sbc,
+		const void *src, size_t src_size,
+		void *dst, size_t dst_size,
+		size_t *dst_out)
+{
+	size_t processed = 0;
+	int res;
+
+	*dst_out = 0;
+
+	/* Scan for SBC syncword.
+	 * We could probably assume 1-byte paddings instead,
+	 * which devices seem to be sending.
+	 */
+	while (src_size >= 1) {
+		if (*(uint8_t*)src == 0x9C)
+			break;
+		src = (uint8_t*)src + 1;
+		--src_size;
+		++processed;
+	}
+
+	res = sbc_decode(sbc, src, src_size,
+			dst, dst_size, dst_out);
+	if (res <= 0)
+		res = SPA_MIN((size_t)1, src_size);    /* skip bad payload */
+
+	processed += res;
+	return processed;
+}
+
+static int codec_decode(void *data,
+		const void *src, size_t src_size,
+		void *dst, size_t dst_size,
+		size_t *dst_out)
+{
+	struct impl *this = data;
+	return do_decode(&this->sbc, src, src_size, dst, dst_size, dst_out);
+}
+
+/*
+ * Duplex codec
+ *
+ * When connected as SRC to SNK, FastStream sink may send back SBC data.
+ */
+
+static int duplex_enum_config(const struct a2dp_codec *codec,
+		const void *caps, size_t caps_size, uint32_t id, uint32_t idx,
+		struct spa_pod_builder *b, struct spa_pod **param)
+{
+	a2dp_faststream_t conf;
+	struct spa_audio_info_raw info = { 0, };
+
+	if (caps_size < sizeof(conf))
+		return -EINVAL;
+
+	memcpy(&conf, caps, sizeof(conf));
+
+	if (idx > 0)
+		return 0;
+
+	switch (conf.source_frequency) {
+	case FASTSTREAM_SOURCE_SAMPLING_FREQ_16000:
+		info.rate = 16000;
+		break;
+	default:
+		return -EINVAL;
+        }
+
+	/*
+	 * Some headsets send mono stream, others stereo.  This information
+	 * is contained in the SBC headers, and becomes known only when
+	 * stream arrives. To be able to work in both cases, we will
+	 * produce 2-channel output, and will double the channels
+	 * in the decoding step if mono stream was received.
+	 */
+	info.format = SPA_AUDIO_FORMAT_S16_LE;
+	info.channels = 2;
+	info.position[0] = SPA_AUDIO_CHANNEL_FL;
+	info.position[1] = SPA_AUDIO_CHANNEL_FR;
+
+	*param = spa_format_audio_raw_build(b, id, &info);
+	return *param == NULL ? -EIO : 1;
+}
+
+static int duplex_reduce_bitpool(void *data)
+{
+	return -ENOTSUP;
+}
+
+static int duplex_increase_bitpool(void *data)
+{
+	return -ENOTSUP;
+}
+
+static int duplex_get_block_size(void *data)
+{
+	return 0;
+}
+
+static void *duplex_init(const struct a2dp_codec *codec, uint32_t flags,
+		void *config, size_t config_len, const struct spa_audio_info *info,
+		void *props, size_t mtu)
+{
+	a2dp_faststream_t *conf = config;
+	struct duplex_impl *this = NULL;
+	int res;
+
+	if (info->media_type != SPA_MEDIA_TYPE_audio ||
+	    info->media_subtype != SPA_MEDIA_SUBTYPE_raw ||
+	    info->info.raw.format != SPA_AUDIO_FORMAT_S16_LE) {
+		res = -EINVAL;
+		goto error;
+	}
+
+	if ((this = calloc(1, sizeof(struct duplex_impl))) == NULL)
+		goto error_errno;
+
+	if ((res = sbc_init(&this->sbc, 0)) < 0)
+		goto error;
+
+	switch (conf->source_frequency) {
+	case FASTSTREAM_SOURCE_SAMPLING_FREQ_16000:
+		this->sbc.frequency = SBC_FREQ_16000;
+		break;
+	default:
+		res = -EINVAL;
+                goto error;
+        }
+
+	this->sbc.endian = SBC_LE;
+	this->sbc.mode = SBC_MODE_MONO;
+	this->sbc.subbands = SBC_SB_8;
+	this->sbc.allocation = SBC_AM_LOUDNESS;
+	this->sbc.blocks = SBC_BLK_16;
+	this->sbc.bitpool = 32;
+
+	return this;
+
+error_errno:
+	res = -errno;
+	goto error;
+error:
+	free(this);
+	errno = -res;
+	return NULL;
+}
+
+static void duplex_deinit(void *data)
+{
+	struct duplex_impl *this = data;
+	sbc_finish(&this->sbc);
+	free(this);
+}
+
+static int duplex_abr_process (void *data, size_t unsent)
+{
+	return -ENOTSUP;
+}
+
+static int duplex_start_encode (void *data,
+		void *dst, size_t dst_size, uint16_t seqnum, uint32_t timestamp)
+{
+	return -ENOTSUP;
+}
+
+static int duplex_encode(void *data,
+		const void *src, size_t src_size,
+		void *dst, size_t dst_size,
+		size_t *dst_out, int *need_flush)
+{
+	return -ENOTSUP;
+}
+
+static int duplex_start_decode (void *data,
+		const void *src, size_t src_size, uint16_t *seqnum, uint32_t *timestamp)
+{
+	return 0;
+}
+
+/** Convert S16LE stereo -> S16LE mono, in-place (only for testing purposes) */
+static SPA_UNUSED size_t convert_s16le_c2_to_c1(int16_t *data, size_t size, size_t max_size)
+{
+	size_t i;
+	for (i = 0; i < size / 2; ++i)
+#if __BYTE_ORDER == __LITTLE_ENDIAN
+		data[i] = data[2*i]/2 + data[2*i+1]/2;
+#else
+		data[i] = bswap_16(bswap_16(data[2*i])/2 + bswap_16(data[2*i+1])/2);
+#endif
+	return size / 2;
+}
+
+/** Convert S16LE mono -> S16LE stereo, in-place */
+static size_t convert_s16le_c1_to_c2(uint8_t *data, size_t size, size_t max_size)
+{
+	size_t pos;
+
+	pos = 2 * SPA_MIN(size / 2, max_size / 4);
+	size = 2 * pos;
+
+	/* We'll trust the compiler to optimize this */
+	while (pos >= 2) {
+		pos -= 2;
+		data[2*pos+3] = data[pos+1];
+		data[2*pos+2] = data[pos];
+		data[2*pos+1] = data[pos+1];
+		data[2*pos] = data[pos];
+	}
+
+	return size;
+}
+
+static int duplex_decode(void *data,
+		const void *src, size_t src_size,
+		void *dst, size_t dst_size,
+		size_t *dst_out)
+{
+	struct duplex_impl *this = data;
+	int res;
+
+	*dst_out = 0;
+	res = do_decode(&this->sbc, src, src_size, dst, dst_size, dst_out);
+
+	/*
+	 * Depending on headers of first frame, libsbc may output either
+	 * 1 or 2 channels. This function should always produce 2 channels,
+	 * so we'll just double the channels here.
+	 */
+	if (this->sbc.mode == SBC_MODE_MONO)
+		*dst_out = convert_s16le_c1_to_c2(dst, *dst_out, dst_size);
+
+	return res;
+}
+
+/* Voice channel SBC, not a real A2DP codec */
+static const struct a2dp_codec duplex_codec = {
+	.codec_id = A2DP_CODEC_VENDOR,
+	.name = "faststream_sbc",
+	.description = "FastStream duplex SBC",
+	.fill_caps = codec_fill_caps,
+	.select_config = codec_select_config,
+	.enum_config = duplex_enum_config,
+	.init = duplex_init,
+	.deinit = duplex_deinit,
+	.get_block_size = duplex_get_block_size,
+	.abr_process = duplex_abr_process,
+	.start_encode = duplex_start_encode,
+	.encode = duplex_encode,
+	.start_decode = duplex_start_decode,
+	.decode = duplex_decode,
+	.reduce_bitpool = duplex_reduce_bitpool,
+	.increase_bitpool = duplex_increase_bitpool,
+};
+
+#define FASTSTREAM_COMMON_DEFS				\
+	.codec_id = A2DP_CODEC_VENDOR,			\
+	.vendor = { .vendor_id = FASTSTREAM_VENDOR_ID,	\
+		.codec_id = FASTSTREAM_CODEC_ID },	\
+	.description = "FastStream",			\
+	.fill_caps = codec_fill_caps,			\
+	.select_config = codec_select_config,		\
+	.enum_config = codec_enum_config,		\
+	.init = codec_init,				\
+	.deinit = codec_deinit,				\
+	.get_block_size = codec_get_block_size,		\
+	.abr_process = codec_abr_process,		\
+	.start_encode = codec_start_encode,		\
+	.encode = codec_encode,				\
+	.start_decode = codec_start_decode,		\
+	.decode = codec_decode,				\
+	.reduce_bitpool = codec_reduce_bitpool,		\
+	.increase_bitpool = codec_increase_bitpool
+
+const struct a2dp_codec a2dp_codec_faststream = {
+	FASTSTREAM_COMMON_DEFS,
+	.id = SPA_BLUETOOTH_AUDIO_CODEC_FASTSTREAM,
+	.name = "faststream",
+};
+
+const struct a2dp_codec a2dp_codec_faststream_duplex = {
+	FASTSTREAM_COMMON_DEFS,
+	.id = SPA_BLUETOOTH_AUDIO_CODEC_FASTSTREAM_DUPLEX,
+	.name = "faststream_duplex",
+	.duplex_codec = &duplex_codec,
+};

spa/plugins/bluez5/a2dp-codecs.c

@@ -120,6 +120,8 @@ const a2dp_mpeg_t bluez_a2dp_mpeg = {
 
 extern struct a2dp_codec a2dp_codec_sbc;
 extern struct a2dp_codec a2dp_codec_sbc_xq;
+extern struct a2dp_codec a2dp_codec_faststream;
+extern struct a2dp_codec a2dp_codec_faststream_duplex;
 #if ENABLE_LDAC
 extern struct a2dp_codec a2dp_codec_ldac;
 #endif
@@ -154,6 +156,8 @@ static const struct a2dp_codec * const a2dp_codec_list[] = {
 #endif
 	&a2dp_codec_sbc,
 	&a2dp_codec_sbc_xq,
+	&a2dp_codec_faststream,
+	&a2dp_codec_faststream_duplex,
 #if ENABLE_APTX
 	&a2dp_codec_aptx_ll_0,
 	&a2dp_codec_aptx_ll_1,

spa/plugins/bluez5/bluez5-dbus.c

@@ -1947,6 +1947,8 @@ int64_t spa_bt_transport_get_delay_nsec(struct spa_bt_transport *t)
 		return 175 * SPA_NSEC_PER_MSEC;
 	case SPA_BLUETOOTH_AUDIO_CODEC_APTX_LL:
 	case SPA_BLUETOOTH_AUDIO_CODEC_APTX_LL_DUPLEX:
+	case SPA_BLUETOOTH_AUDIO_CODEC_FASTSTREAM:
+	case SPA_BLUETOOTH_AUDIO_CODEC_FASTSTREAM_DUPLEX:
 		return 40 * SPA_NSEC_PER_MSEC;
 	default:
 		break;

spa/plugins/bluez5/meson.build

@@ -23,6 +23,7 @@ endif
 bluez5_sources = ['plugin.c',
 		  'a2dp-codecs.c',
 		  'a2dp-codec-sbc.c',
+		  'a2dp-codec-faststream.c',
 		  'a2dp-sink.c',
 		  'a2dp-source.c',
 		  'sco-sink.c',