Packet loss concealment for HFP with mSBC codec

.github/workflows/build-and-test.yaml

@@ -33,6 +33,7 @@ jobs:
           libncurses5-dev \
           libreadline-dev \
           libsbc-dev \
+          libspandsp-dev \
           python-docutils
     - uses: actions/checkout@v2
     - name: Create Build Environment

.github/workflows/codeql-analysis.yaml

@@ -29,7 +29,8 @@ jobs:
           libmpg123-dev \
           libncurses5-dev \
           libreadline-dev \
-          libsbc-dev
+          libsbc-dev \
+          libspandsp-dev
     - uses: actions/checkout@v2
     - name: Initialize CodeQL
       uses: github/codeql-action/init@v1

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;

src/sco.c

@@ -369,41 +369,42 @@ static void *sco_msbc_enc_thread(struct ba_transport_thread *th) {
 
 		ffb_seek(&msbc.pcm, samples);
 
-		int err;
-		if ((err = msbc_encode(&msbc)) < 0) {
-			warn("Couldn't encode mSBC: %s", sbc_strerror(err));
-			ffb_rewind(&msbc.pcm);
-		}
+		while (ffb_len_out(&msbc.pcm) >= MSBC_CODESAMPLES) {
 
-		if (msbc.frames == 0)
-			continue;
+			int err;
+			if ((err = msbc_encode(&msbc)) < 0) {
+				error("mSBC encoding error: %s", sbc_strerror(err));
+				break;
+			}
 
-		uint8_t *data = msbc.data.data;
-		size_t data_len = ffb_blen_out(&msbc.data);
+			uint8_t *data = msbc.data.data;
+			size_t data_len = ffb_blen_out(&msbc.data);
 
-		while (data_len >= mtu_write) {
+			while (data_len >= mtu_write) {
 
-			ssize_t len;
-			if ((len = io_bt_write(th, data, mtu_write)) <= 0) {
-				if (len == -1)
-					error("BT write error: %s", strerror(errno));
-				goto exit;
-			}
+				ssize_t len;
+				if ((len = io_bt_write(th, data, mtu_write)) <= 0) {
+					if (len == -1)
+						error("BT write error: %s", strerror(errno));
+					goto exit;
+				}
 
-			data += len;
-			data_len -= len;
+				data += len;
+				data_len -= len;
 
-		}
+			}
 
-		/* keep data transfer at a constant bit rate */
-		asrsync_sync(&io.asrs, msbc.frames * MSBC_CODESAMPLES);
-		/* update busy delay (encoding overhead) */
-		pcm->delay = asrsync_get_busy_usec(&io.asrs) / 100;
+			/* keep data transfer at a constant bit rate */
+			asrsync_sync(&io.asrs, msbc.frames * MSBC_CODESAMPLES);
+			/* update busy delay (encoding overhead) */
+			pcm->delay = asrsync_get_busy_usec(&io.asrs) / 100;
+
+			/* Move unprocessed data to the front of our linear
+			* buffer and clear the mSBC frame counter. */
+			ffb_shift(&msbc.data, ffb_blen_out(&msbc.data) - data_len);
+			msbc.frames = 0;
 
-		/* Move unprocessed data to the front of our linear
-		 * buffer and clear the mSBC frame counter. */
-		ffb_shift(&msbc.data, ffb_blen_out(&msbc.data) - data_len);
-		msbc.frames = 0;
+		}
 
 	}
 
@@ -452,8 +453,8 @@ static void *sco_msbc_dec_thread(struct ba_transport_thread *th) {
 
 		int err;
 		if ((err = msbc_decode(&msbc)) < 0) {
-			warn("Couldn't decode mSBC: %s", sbc_strerror(err));
-			ffb_rewind(&msbc.data);
+			error("mSBC decoding error: %s", sbc_strerror(err));
+			break;
 		}
 
 		ssize_t samples;

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@