Packet loss concealment for HFP with mSBC codec

NEWS

@@ -3,6 +3,7 @@ unreleased
 
 - optional support for A2DP FastStream codec (music & voice)
 - optional support for A2DP LC3plus codec (music & voice)
+- packet loss concealment (PLC) for HFP with mSBC codec
 - enable/disable BT profiles/codecs via command line options
 - allow to select BT transport codec with ALSA configuration
 - allow to set PCM volume properties with ALSA configuration

README.md

@@ -60,6 +60,7 @@ Dependencies:
 - [mpg123](https://www.mpg123.org/) (when MPEG decoding support is enabled with `--enable-mpg123`)
 - [openaptx](https://github.com/Arkq/openaptx) (when apt-X support is enabled with
   `--enable-aptx` and/or `--enable-aptx-hd`)
+- [spandsp](https://www.soft-switch.org) (when mSBC support is enabled with `--enable-msbc`)
 
 Dependencies for client applications (e.g. `bluealsa-aplay` or `bluealsa-cli`):
 

configure.ac

@@ -201,6 +201,7 @@ AC_ARG_ENABLE([msbc],
 	[AS_HELP_STRING([--enable-msbc], [enable mSBC support])])
 AM_CONDITIONAL([ENABLE_MSBC], [test "x$enable_msbc" = "xyes"])
 AM_COND_IF([ENABLE_MSBC], [
+	PKG_CHECK_MODULES([SPANDSP], [spandsp >= 0.0.6])
 	AC_DEFINE([ENABLE_MSBC], [1], [Define to 1 if mSBC is enabled.])
 ])
 

src/Makefile.am

@@ -107,7 +107,8 @@ AM_CFLAGS = \
 	@LIBBSD_CFLAGS@ \
 	@LIBUNWIND_CFLAGS@ \
 	@MPG123_CFLAGS@ \
-	@SBC_CFLAGS@
+	@SBC_CFLAGS@ \
+	@SPANDSP_CFLAGS@
 
 LDADD = \
 	@AAC_LIBS@ \
@@ -123,4 +124,5 @@ LDADD = \
 	@LIBUNWIND_LIBS@ \
 	@MP3LAME_LIBS@ \
 	@MPG123_LIBS@ \
-	@SBC_LIBS@
+	@SBC_LIBS@ \
+	@SPANDSP_LIBS@

src/codec-msbc.c

@@ -1,6 +1,6 @@
 /*
  * BlueALSA - codec-msbc.c
- * Copyright (c) 2016-2021 Arkadiusz Bokowy
+ * Copyright (c) 2016-2022 Arkadiusz Bokowy
  *               2017 Juha Kuikka
  *
  * This file is a part of bluez-alsa.
@@ -14,8 +14,11 @@
 #include <endian.h>
 #include <errno.h>
 #include <stdbool.h>
+#include <stdint.h>
 #include <string.h>
 
+#include <spandsp.h>
+
 #include "codec-sbc.h"
 #include "shared/log.h"
 
@@ -72,7 +75,9 @@ int msbc_init(struct esco_msbc *msbc) {
 			goto fail;
 		if (ffb_init_uint8_t(&msbc->data, sizeof(esco_msbc_frame_t) * 3) == -1)
 			goto fail;
-		if (ffb_init_int16_t(&msbc->pcm, MSBC_CODESAMPLES * 2) == -1)
+		/* Allocate buffer for 1 decoded frame, optional 3 PLC frames and
+		 * some extra frames to account for async PCM samples reading. */
+		if (ffb_init_int16_t(&msbc->pcm, MSBC_CODESAMPLES * 6) == -1)
 			goto fail;
 	}
 
@@ -100,6 +105,11 @@ int msbc_init(struct esco_msbc *msbc) {
 	msbc->seq_number = 0;
 	msbc->frames = 0;
 
+	/* Initialize PLC context. When calling with non-NULL parameter,
+	 * this function does not allocate anything - there is no need
+	 * to call plc_free(). */
+	plc_init(&msbc->plc);
+
 	msbc->initialized = true;
 	return 0;
 
@@ -131,7 +141,6 @@ ssize_t msbc_decode(struct esco_msbc *msbc) {
 
 	const uint8_t *input = msbc->data.data;
 	size_t input_len = ffb_blen_out(&msbc->data);
-	int16_t *output = msbc->pcm.tail;
 	size_t output_len = ffb_blen_in(&msbc->pcm);
 	ssize_t rv = 0;
 
@@ -140,9 +149,10 @@ ssize_t msbc_decode(struct esco_msbc *msbc) {
 	input += tmp - input_len;
 
 	/* Skip decoding if there is not enough input data or the output
-	 * buffer is not big enough to hold decoded PCM samples.*/
+	 * buffer is not big enough to hold decoded PCM samples and PCM
+	 * samples reconstructed with PLC (up to 3 mSBC frames). */
 	if (input_len < sizeof(*frame) ||
-			output_len < MSBC_CODESIZE)
+			output_len < MSBC_CODESIZE * (1 + 3))
 		goto final;
 
 	esco_h2_header_t h2;
@@ -155,22 +165,35 @@ ssize_t msbc_decode(struct esco_msbc *msbc) {
 		msbc->seq_number = _seq;
 	}
 	else if (_seq != ++msbc->seq_number) {
-		warn("Missing mSBC packet: %u != %u", _seq, msbc->seq_number);
+
+		/* In case of missing mSBC frames (we can detect up to 3 consecutive
+		 * missing frames) use PLC for PCM samples reconstruction. */
+
+		uint8_t missing = (_seq + ESCO_H2_SN_MAX - msbc->seq_number) % ESCO_H2_SN_MAX;
+		warn("Missing mSBC packets (%u != %u): %u", _seq, msbc->seq_number, missing);
+
 		msbc->seq_number = _seq;
-		/* TODO: Implement PLC. */
+
+		plc_fillin(&msbc->plc, msbc->pcm.tail, missing * MSBC_CODESAMPLES);
+		ffb_seek(&msbc->pcm, missing * MSBC_CODESAMPLES);
+		rv += missing * MSBC_CODESAMPLES;
+
 	}
 
 	ssize_t len;
 	if ((len = sbc_decode(&msbc->sbc, frame->payload, sizeof(frame->payload),
-					output, output_len, NULL)) < 0) {
+					msbc->pcm.tail, output_len, NULL)) < 0) {
 		input += 1;
 		rv = len;
 		goto final;
 	}
 
+	/* record PCM history and blend new data after PLC */
+	plc_rx(&msbc->plc, msbc->pcm.tail, MSBC_CODESAMPLES);
+
 	ffb_seek(&msbc->pcm, MSBC_CODESAMPLES);
 	input += sizeof(*frame);
-	rv = MSBC_CODESAMPLES;
+	rv += MSBC_CODESAMPLES;
 
 final:
 	/* Reshuffle remaining data to the beginning of the buffer. */

src/codec-msbc.h

@@ -1,6 +1,6 @@
 /*
  * BlueALSA - codec-msbc.h
- * Copyright (c) 2016-2021 Arkadiusz Bokowy
+ * Copyright (c) 2016-2022 Arkadiusz Bokowy
  *
  * This file is a part of bluez-alsa.
  *
@@ -22,6 +22,7 @@
 #include <sys/types.h>
 
 #include <sbc/sbc.h>
+#include <spandsp.h>
 
 #include "shared/ffb.h"
 
@@ -32,6 +33,7 @@
 #define MSBC_FRAMELEN    57
 
 #define ESCO_H2_SYNCWORD 0x801
+#define ESCO_H2_SN_MAX   0x4
 #define ESCO_H2_GET_SYNCWORD(h2) ((h2) & 0xFFF)
 #define ESCO_H2_GET_SN0(h2)      (((h2) >> 12) & 0x3)
 #define ESCO_H2_GET_SN1(h2)      (((h2) >> 14) & 0x3)
@@ -62,6 +64,9 @@ struct esco_msbc {
 	/* number of processed frames */
 	size_t frames;
 
+	/* packet loss concealment */
+	plc_state_t plc;
+
 	/* Determine whether structure has been initialized. This field is
 	 * used for reinitialization - it makes msbc_init() idempotent. */
 	bool initialized;

test/Makefile.am

@@ -224,7 +224,8 @@ AM_CFLAGS = \
 	@LIBUNWIND_CFLAGS@ \
 	@MPG123_CFLAGS@ \
 	@SBC_CFLAGS@ \
-	@SNDFILE_CFLAGS@
+	@SNDFILE_CFLAGS@ \
+	@SPANDSP_CFLAGS@
 
 LDADD = \
 	@AAC_LIBS@ \
@@ -243,4 +244,5 @@ LDADD = \
 	@MP3LAME_LIBS@ \
 	@MPG123_LIBS@ \
 	@SBC_LIBS@ \
-	@SNDFILE_LIBS@
+	@SNDFILE_LIBS@ \
+	@SPANDSP_LIBS@

test/test-msbc.c

@@ -1,6 +1,6 @@
 /*
  * test-msbc.c
- * Copyright (c) 2016-2021 Arkadiusz Bokowy
+ * Copyright (c) 2016-2022 Arkadiusz Bokowy
  *
  * This file is a part of bluez-alsa.
  *
@@ -10,6 +10,7 @@
 
 #include <stdbool.h>
 #include <stdint.h>
+#include <stdio.h>
 #include <string.h>
 
 #include <check.h>
@@ -86,17 +87,17 @@ START_TEST(test_msbc_find_h2_header) {
 
 START_TEST(test_msbc_encode_decode) {
 
-	struct esco_msbc msbc = { 0 };
-	int16_t sine[1024];
-	size_t len;
-	size_t i;
-	int rv;
-
+	int16_t sine[8 * MSBC_CODESAMPLES];
 	snd_pcm_sine_s16_2le(sine, ARRAYSIZE(sine), 1, 0, 1.0 / 128);
 
 	uint8_t data[sizeof(sine)];
 	uint8_t *data_tail = data;
 
+	struct esco_msbc msbc = { 0 };
+	size_t len;
+	size_t i;
+	int rv;
+
 	msbc.initialized = false;
 	ck_assert_int_eq(msbc_init(&msbc), 0);
 	for (rv = 1, i = 0; rv > 0;) {
@@ -110,7 +111,7 @@ START_TEST(test_msbc_encode_decode) {
 
 		len = ffb_blen_out(&msbc.data);
 		memcpy(data_tail, msbc.data.data, len);
-		ffb_shift(&msbc.data, len);
+		ffb_rewind(&msbc.data);
 		data_tail += len;
 
 	}
@@ -135,7 +136,7 @@ START_TEST(test_msbc_encode_decode) {
 
 		len = ffb_len_out(&msbc.pcm);
 		memcpy(pcm_tail, msbc.pcm.data, len * msbc.pcm.size);
-		ffb_shift(&msbc.pcm, len);
+		ffb_rewind(&msbc.pcm);
 		pcm_tail += len;
 
 	}
@@ -146,6 +147,75 @@ START_TEST(test_msbc_encode_decode) {
 
 } END_TEST
 
+START_TEST(test_msbc_decode_plc) {
+
+	int16_t sine[18 * MSBC_CODESAMPLES];
+	snd_pcm_sine_s16_2le(sine, ARRAYSIZE(sine), 1, 0, 1.0 / 128);
+
+	struct esco_msbc msbc = { .initialized = false };
+	ck_assert_int_eq(msbc_init(&msbc), 0);
+
+	uint8_t data[sizeof(sine)];
+	uint8_t *data_tail = data;
+
+	debug("Simulating mSBC packet loss events");
+
+	int rv;
+	size_t counter, i;
+	for (rv = 1, counter = i = 0; rv > 0; counter++) {
+
+		size_t len = MIN(ARRAYSIZE(sine) - i, ffb_len_in(&msbc.pcm));
+		memcpy(msbc.pcm.tail, &sine[i], len * msbc.pcm.size);
+		ffb_seek(&msbc.pcm, len);
+		i += len;
+
+		rv = msbc_encode(&msbc);
+
+		len = ffb_blen_out(&msbc.data);
+		memcpy(data_tail, msbc.data.data, len);
+		ffb_rewind(&msbc.data);
+
+		/* simulate packet loss */
+		if (counter == 2 ||
+				(6 <= counter && counter <= 8) ||
+				/* 4 packets (undetectable) */
+				(12 <= counter && counter <= 15)) {
+			fprintf(stderr, "_");
+			continue;
+		}
+
+		fprintf(stderr, "x");
+		data_tail += len;
+
+	}
+
+	fprintf(stderr, "\n");
+
+	/* reinitialize encoder/decoder handler */
+	ck_assert_int_eq(msbc_init(&msbc), 0);
+
+	size_t samples = 0;
+	for (rv = 1, i = 0; rv > 0; ) {
+
+		size_t len = MIN((data_tail - data) - i, ffb_blen_in(&msbc.data));
+		memcpy(msbc.data.tail, &data[i], len);
+		ffb_seek(&msbc.data, len);
+		i += len;
+
+		rv = msbc_decode(&msbc);
+
+		samples += ffb_len_out(&msbc.pcm);
+		ffb_rewind(&msbc.pcm);
+
+	}
+
+	/* we should recover all except consecutive 4 frames */
+	ck_assert_int_eq(samples, (18 - 4) * MSBC_CODESAMPLES);
+
+	msbc_finish(&msbc);
+
+} END_TEST
+
 int main(void) {
 
 	Suite *s = suite_create(__FILE__);
@@ -157,6 +227,7 @@ int main(void) {
 	tcase_add_test(tc, test_msbc_init);
 	tcase_add_test(tc, test_msbc_find_h2_header);
 	tcase_add_test(tc, test_msbc_encode_decode);
+	tcase_add_test(tc, test_msbc_decode_plc);
 
 	srunner_run_all(sr, CK_ENV);
 	int nf = srunner_ntests_failed(sr);