bluez5: add LHDC V3 A2DP decoder

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

@@ -5,6 +5,7 @@
 
 #include <assert.h>
 #include <unistd.h>
+#include <stdbool.h>
 #include <stddef.h>
 #include <errno.h>
 #include <arpa/inet.h>
@@ -16,6 +17,7 @@
 #include <spa/param/audio/format.h>
 
 #include <lhdcBT.h>
+#include <lhdcBT_dec.h>
 
 #include "rtp.h"
 #include "media-codecs.h"
@@ -37,6 +39,8 @@ static_assert(sizeof(struct rtp_lhdc_payload) == sizeof(uint16_t), "LHDC payload
 struct impl_v3 {
 	HANDLE_LHDC_BT lhdc;
 
+	bool dec_initialized;
+
 	struct rtp_header *header;
 	struct rtp_lhdc_payload *payload;
 
@@ -404,6 +408,16 @@ static LHDCBT_QUALITY_T get_max_bitrate_v3(const a2dp_lhdc_v3_t *configuration)
 	}
 }
 
+static int get_version_setup_v3(const a2dp_lhdc_v3_t *configuration) {
+	if (configuration->llac) {
+		return VERSION_LLAC;
+	} else if (configuration->lhdc_v4) {
+		return VERSION_4;
+	} else {
+		return VERSION_3;
+	}
+}
+
 static void *codec_init_v3(const struct media_codec *codec, uint32_t flags,
 		void *config, size_t config_len, const struct spa_audio_info *info,
 		void *props, size_t mtu)
@@ -445,6 +459,19 @@ static void *codec_init_v3(const struct media_codec *codec, uint32_t flags,
 	if (res < 0)
 		goto error;
 
+	tLHDCV3_DEC_CONFIG dec_config = {
+		.version = get_version_setup_v3(conf),
+		.sample_rate = this->frequency,
+		.bits_depth = this->bit_depth,
+	};
+
+	this->dec_initialized = false;
+
+	if (lhdcBT_dec_init_decoder(&dec_config) < 0)
+		goto error;
+
+	this->dec_initialized = true;
+
 	this->block_size = lhdcBT_get_block_Size(this->lhdc);
 	this->codesize = info->info.raw.channels * lhdcBT_get_block_Size(this->lhdc);
 
@@ -474,6 +501,8 @@ static void codec_deinit_v3(void *data)
 	struct impl_v3 *this = data;
 	if (this->lhdc)
 		lhdcBT_free_handle(this->lhdc);
+	if (this->dec_initialized)
+		lhdcBT_dec_deinit_decoder();
 	free(this);
 }
 
@@ -519,6 +548,8 @@ static int codec_start_encode_v3(void *data,
 	return sizeof(struct rtp_header) + sizeof(struct rtp_lhdc_payload);
 }
 
+
+
 static void deinterleave_32_c2(int32_t * SPA_RESTRICT * SPA_RESTRICT dst, const int32_t * SPA_RESTRICT src, size_t n_samples)
 {
 	/* We'll trust the compiler to optimize this */
@@ -560,6 +591,62 @@ static int codec_encode_v3(void *data,
 	return src_used;
 }
 
+static int codec_start_decode(void *data,
+		const void *src, size_t src_size, uint16_t *seqnum, uint32_t *timestamp)
+{
+	const struct rtp_header *header = src;
+	size_t header_size = sizeof(struct rtp_header);
+
+	spa_return_val_if_fail (src_size > header_size, -EINVAL);
+
+	if (seqnum)
+		*seqnum = ntohs(header->sequence_number);
+	if (timestamp)
+		*timestamp = ntohl(header->timestamp);
+
+	return header_size;
+}
+
+static const char *dec_errors[] = {
+	[-LHDCBT_DEC_FUNC_SUCCEED] = "OK",
+	[-LHDCBT_DEC_FUNC_FAIL] = "General error",
+	[-LHDCBT_DEC_FUNC_INPUT_NOT_ENOUGH] = "Not enough input data",
+	[-LHDCBT_DEC_FUNC_OUTPUT_NOT_ENOUGH] = "Not enough output space",
+	[-LHDCBT_DEC_FUNC_INVALID_SEQ_NO] = "Invalid sequence number",
+};
+
+static int codec_decode_v3(void *data,
+		const void *src, size_t src_size,
+		void *dst, size_t dst_size,
+		size_t *dst_out)
+{
+	uint32_t decoded = dst_size;
+	uint32_t consumed = 0;
+
+	int err = 0;
+
+	if ((err = lhdcBT_dec_check_frame_data_enough(src, src_size, &consumed)) < 0)
+		goto error;
+
+	consumed += sizeof(struct rtp_lhdc_payload);
+
+	if ((err = lhdcBT_dec_decode(src, consumed, dst, &decoded, 24)) < 0)
+		goto error;
+
+	int32_t *samples = dst;
+	for (size_t i = 0; i < decoded / 4; i++)
+		samples[i] *= (1 << 8);
+
+	if (dst_out)
+		*dst_out = decoded;
+
+	return consumed;
+
+error:
+	spa_log_error(log, "lhdcBT_dec_decode: %s (%d)!", dec_errors[-err], err);
+	return -1;
+}
+
 static int codec_reduce_bitpool(void *data)
 {
 	return -ENOTSUP;
@@ -597,8 +684,11 @@ const struct media_codec a2dp_codec_lhdc_v3 = {
 	.abr_process = codec_abr_process_v3,
 	.start_encode = codec_start_encode_v3,
 	.encode = codec_encode_v3,
+	.start_decode = codec_start_decode,
+	.decode = codec_decode_v3,
 	.reduce_bitpool = codec_reduce_bitpool,
-	.increase_bitpool = codec_increase_bitpool
+	.increase_bitpool = codec_increase_bitpool,
+	.set_log = codec_set_log,
 };
 
 MEDIA_CODEC_EXPORT_DEF(

spa/plugins/bluez5/meson.build

@@ -139,12 +139,12 @@ if ldac_dep.found()
 endif
 
 if lhdc_enc_dep.found()
-  lhdc_enc_args = codec_args
+  lhdc_args = codec_args
   bluez_codec_ldac = shared_library('spa-codec-bluez5-lhdc',
     [ 'a2dp-codec-lhdc.c', 'media-codecs.c' ],
     include_directories : [ configinc ],
-    c_args : lhdc_enc_args,
-    dependencies : [ spa_dep, lhdc_enc_dep ],
+    c_args : lhdc_args,
+    dependencies : [ spa_dep, lhdc_enc_dep, lhdc_dec_dep ],
     install : true,
     install_dir : spa_plugindir / 'bluez5')
 endif