LHDC recode

test/Makefile.am

@@ -58,6 +58,7 @@ endif
 
 if HAVE_SNDFILE
 check_PROGRAMS += sndalign
+check_PROGRAMS += lhdcrecode
 endif
 
 check_LTLIBRARIES = \

test/lhdcrecode.c

@@ -0,0 +1,226 @@
+// lhdcrecode.c
+
+#include <errno.h>
+#include <math.h>
+#include <endian.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <sys/param.h>
+
+#include <lhdcBT.h>
+#include <lhdcBT_dec.h>
+#include <sndfile.h>
+
+#include "../src/shared/a2dp-codecs.h"
+
+void audio_deinterleave_s16_2le(int16_t **dest, const int16_t *src,
+		unsigned int channels, size_t frames) {
+	for (size_t f = 0; f < frames; f++)
+		for (size_t c = 0; c < channels; c++)
+			dest[c][f] = *src++;
+}
+
+typedef struct rtp_lhdc_media_header {
+	uint8_t seq_number;
+#if __BYTE_ORDER == __LITTLE_ENDIAN
+	uint8_t latency:2;
+	uint8_t frame_count:6;
+#else
+	uint8_t frame_count:6;
+	uint8_t latency:2;
+#endif
+} __attribute__ ((packed)) rtp_lhdc_media_header_t;
+
+static const a2dp_lhdc_v3_t config_lhdc_44100_stereo = {
+	.info = A2DP_VENDOR_INFO_INIT(LHDC_V3_VENDOR_ID, LHDC_V3_CODEC_ID),
+	.sampling_freq = LHDC_SAMPLING_FREQ_44100,
+	.bit_depth = LHDC_BIT_DEPTH_16,
+};
+
+static LHDC_VERSION_SETUP get_version(const a2dp_lhdc_v3_t *configuration) {
+	if (configuration->llac)
+		return LLAC;
+	if (configuration->lhdc_v4)
+		return LHDC_V4;
+	return LHDC_V3;
+}
+
+static lhdc_ver_t get_decoder_version(const a2dp_lhdc_v3_t *configuration) {
+
+	static const lhdc_ver_t versions[] = {
+		[LHDC_V3] = VERSION_3,
+		[LHDC_V4] = VERSION_4,
+		[LLAC]	= VERSION_LLAC,
+	};
+
+	return versions[get_version(configuration)];
+}
+
+static int get_interval(const a2dp_lhdc_v3_t *configuration) {
+	return configuration->low_latency ? 10 : 20;
+}
+
+static int get_bit_depth(const a2dp_lhdc_v3_t *configuration) {
+	return configuration->bit_depth == LHDC_BIT_DEPTH_16 ? 16 : 24;
+}
+
+static LHDCBT_QUALITY_T get_max_bitrate(const a2dp_lhdc_v3_t *configuration) {
+	switch (configuration->max_bitrate) {
+	case LHDC_MAX_BITRATE_400K:
+		return LHDCBT_QUALITY_LOW;
+	case LHDC_MAX_BITRATE_500K:
+		return LHDCBT_QUALITY_MID;
+	case LHDC_MAX_BITRATE_900K:
+	default:
+		return LHDCBT_QUALITY_HIGH;
+	}
+}
+
+int main(int argc, char *argv[]) {
+
+	if (argc != 2) {
+		fprintf(stderr, "Usage: %s <file>\n", argv[0]);
+		return 1;
+	}
+
+	SNDFILE *sf;
+	SF_INFO sf_info = { 0 };
+	if ((sf = sf_open(argv[1], SFM_READ, &sf_info)) == NULL) {
+		fprintf(stderr, "Couldn't open audio file: %s: %s\n", argv[1], sf_strerror(NULL));
+		return EXIT_FAILURE;
+	}
+
+	printf("Source: %s\n", argv[1]);
+	printf("  Frames: %ld\n", sf_info.frames);
+	printf("  Rate: %d\n", sf_info.samplerate);
+	printf("  Channels: %d\n", sf_info.channels);
+
+	short *sf_data = malloc(sf_info.frames * sf_info.channels * sizeof(short));
+	if (sf_readf_short(sf, sf_data, sf_info.frames) != sf_info.frames) {
+		fprintf(stderr, "Couldn't read audio data: %s\n", sf_strerror(sf));
+		return EXIT_FAILURE;
+	}
+
+	// Output
+
+	SNDFILE *sf_out;
+	SF_INFO sf_out_info = {
+		.channels = sf_info.channels,
+		.samplerate = sf_info.samplerate,
+		.format = SF_FORMAT_WAV | SF_FORMAT_PCM_16 };
+	if ((sf_out = sf_open("recoded.wav", SFM_WRITE, &sf_out_info)) == NULL) {
+		fprintf(stderr, "Couldn't open audio file: %s: %s\n", "recoded.wav", sf_strerror(NULL));
+		return EXIT_FAILURE;
+	}
+
+	// LHDC re-encode
+
+	const a2dp_lhdc_v3_t *configuration = &config_lhdc_44100_stereo;
+	const unsigned int bit_depth = get_bit_depth(configuration);
+	const unsigned int channels = 2;
+	const unsigned int rate = 44100;
+	printf("LHDC config: %u bit, %u channel, %u Hz\n", bit_depth, channels, rate);
+
+	if (sf_info.channels != channels) {
+		fprintf(stderr, "Only stereo audio is supported\n");
+		return EXIT_FAILURE;
+	}
+	if (sf_info.samplerate != rate)
+		fprintf(stderr, "warning: encoding as 44100 Hz audio\n");
+
+	// Encoder
+
+	HANDLE_LHDC_BT handle;
+	if ((handle = lhdcBT_get_handle(get_version(configuration))) == NULL) {
+		fprintf(stderr, "Couldn't get LHDC handle: %s\n", strerror(errno));
+		return EXIT_FAILURE;
+	}
+
+	lhdcBT_set_hasMinBitrateLimit(handle, configuration->min_bitrate);
+	lhdcBT_set_max_bitrate(handle, get_max_bitrate(configuration));
+
+	if (lhdcBT_init_encoder(handle, rate, bit_depth, LHDCBT_QUALITY_AUTO,
+				configuration->ch_split_mode > LHDC_CH_SPLIT_MODE_NONE,
+				0 /* need padding */,
+				700 /* mtu */,
+				get_interval(configuration)) == -1) {
+		fprintf(stderr, "Couldn't initialize LHDC encoder\n");
+		return EXIT_FAILURE;
+	}
+
+	const size_t lhdc_ch_samples = lhdcBT_get_block_Size(handle);
+	const size_t lhdc_pcm_samples = lhdc_ch_samples * channels;
+	fprintf(stderr, "LHDC block size: %zu\n", lhdc_ch_samples);
+
+	// Decoder
+
+	tLHDCV3_DEC_CONFIG dec_config = {
+		.version = get_decoder_version(configuration),
+		.sample_rate = rate,
+		.bits_depth = bit_depth,
+	};
+
+	if (lhdcBT_dec_init_decoder(&dec_config) < 0) {
+		fprintf(stderr, "Couldn't get LHDC handle: %s\n", strerror(errno));
+		return EXIT_FAILURE;
+	}
+
+
+	int16_t *input = sf_data;
+	size_t input_frames = sf_info.frames;
+
+	struct {
+		rtp_lhdc_media_header_t header;
+		uint8_t data[1024];
+	} encoded_data = { 0 };
+
+	while (input_frames >= lhdc_ch_samples) {
+
+		int16_t pcm_ch1[lhdc_ch_samples];
+		int16_t pcm_ch2[lhdc_ch_samples];
+		int16_t *pcm_ch_buffers[2] = { pcm_ch1, pcm_ch2 };
+		audio_deinterleave_s16_2le(pcm_ch_buffers, input, channels, lhdc_ch_samples);
+
+		uint32_t encoded = 0;
+		uint32_t frames = 0;
+		if (lhdcBT_encode_stereo(handle, pcm_ch1, pcm_ch2, encoded_data.data, &encoded, &frames) < 0) {
+			fprintf(stderr, "LHDC encoding error\n");
+			return EXIT_FAILURE;
+		}
+
+		input += lhdc_pcm_samples;
+		input_frames -= lhdc_ch_samples;
+		fprintf(stderr, "Encoded: %u bytes, %u frames\n", encoded, frames);
+
+		if (encoded == 0)
+			continue;
+
+		// Setup LHDC RTP header
+
+		encoded_data.header.seq_number++;
+		encoded_data.header.latency = 0;
+		encoded_data.header.frame_count = frames;
+
+		// Decode
+
+		int16_t decoded_pcm[lhdc_pcm_samples];
+		uint32_t decoded_len = sizeof(decoded_pcm);
+		size_t encoded_data_len = sizeof(encoded_data.header) + encoded;
+		lhdcBT_dec_decode(&encoded_data, encoded_data_len, decoded_pcm, &decoded_len, bit_depth);
+
+		// Write to output
+
+		const sf_count_t out_frames = decoded_len / 2 / channels;
+		if (sf_writef_short(sf_out, decoded_pcm, out_frames) != out_frames) {
+			fprintf(stderr, "Couldn't write audio data: %s\n", sf_strerror(sf_out));
+			return EXIT_FAILURE;
+		}
+
+	}
+
+	sf_close(sf);
+	sf_close(sf_out);
+	free(sf_data);
+
+	return 0;
+}