Add mSBC support into the SCO IO thread

Add support for the mSBC codec for HFP into the SCO IO thread.
This support is optional, and is controlled by the --enable-msbc
configuration flag.

The receiving part of this mSBC support has been tested with Jabra
MOVE v2.3.0 headset and seems to work flawlessly. However, playback
does not work... Maybe it will work with some other BT device.

Note:
This commit is a rework of a pull request submitted by Juha Kuikka.

Fixes #29 and closes #37

.travis.yml

@@ -26,5 +26,5 @@ script:
   - cat test/*.log
   - ../configure --enable-test --enable-debug && make && make check
   - cat test/*.log
-  - ../configure --enable-test --enable-aac && make && make check
+  - ../configure --enable-test --enable-aac --enable-msbc && make && make check
   - cat test/*.log

configure.ac

@@ -86,6 +86,13 @@ AM_COND_IF([ENABLE_LDAC], [
 	AC_DEFINE([ENABLE_LDAC], [1], [Define to 1 if LDAC is enabled.])
 ])
 
+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], [
+	AC_DEFINE([ENABLE_MSBC], [1], [Define to 1 if mSBC is enabled.])
+])
+
 AC_ARG_ENABLE([ofono],
 	AS_HELP_STRING([--enable-ofono], [enable HFP over oFono]))
 AM_CONDITIONAL([ENABLE_OFONO], [test "x$enable_ofono" = "xyes"])

src/Makefile.am

@@ -22,6 +22,11 @@ bluealsa_SOURCES = \
 	utils.c \
 	main.c
 
+if ENABLE_MSBC
+bluealsa_SOURCES += \
+	msbc.c
+endif
+
 if ENABLE_OFONO
 bluealsa_SOURCES += \
 	ofono.c \

src/ba-transport.c

@@ -784,6 +784,9 @@ static int transport_acquire_bt_sco(struct ba_transport *t) {
 	t->mtu_read = 48;
 	t->mtu_write = 48;
 
+	if (t->type.codec == HFP_CODEC_MSBC)
+		t->mtu_read = t->mtu_write = 24;
+
 	debug("New SCO link: %d (MTU: R:%zu W:%zu)", t->bt_fd, t->mtu_read, t->mtu_write);
 
 	return t->bt_fd;

src/bluealsa.c

@@ -36,20 +36,36 @@ struct ba_config config = {
 
 	.hfp.features_sdp_hf =
 		SDP_HFP_HF_FEAT_CLI |
-		SDP_HFP_HF_FEAT_VOLUME,
-	.hfp.features_sdp_ag = 0,
+		SDP_HFP_HF_FEAT_VOLUME |
+#if ENABLE_MSBC
+		SDP_HFP_HF_FEAT_WBAND |
+#endif
+		0,
+	.hfp.features_sdp_ag =
+#if ENABLE_MSBC
+		SDP_HFP_AG_FEAT_WBAND |
+#endif
+		0,
 	.hfp.features_rfcomm_hf =
 		HFP_HF_FEAT_CLI |
 		HFP_HF_FEAT_VOLUME |
 		HFP_HF_FEAT_ECS |
 		HFP_HF_FEAT_ECC |
-		HFP_HF_FEAT_CODEC,
+#if ENABLE_MSBC
+		HFP_HF_FEAT_CODEC |
+		HFP_HF_FEAT_ESOC |
+#endif
+		0,
 	.hfp.features_rfcomm_ag =
 		HFP_AG_FEAT_REJECT |
 		HFP_AG_FEAT_ECS |
 		HFP_AG_FEAT_ECC |
 		HFP_AG_FEAT_EERC |
-		HFP_AG_FEAT_CODEC,
+#if ENABLE_MSBC
+		HFP_AG_FEAT_CODEC |
+		HFP_AG_FEAT_ESOC |
+#endif
+		0,
 
 	.a2dp.volume = false,
 	.a2dp.force_mono = false,

src/io.c

@@ -43,6 +43,7 @@
 #include "ba-transport.h"
 #include "bluealsa.h"
 #include "hfp.h"
+#include "msbc.h"
 #include "utils.h"
 #include "shared/defs.h"
 #include "shared/ffb.h"
@@ -1548,6 +1549,11 @@ void *io_thread_sco(void *arg) {
 	pthread_cleanup_push(PTHREAD_CLEANUP(ffb_uint8_free), &bt_in);
 	pthread_cleanup_push(PTHREAD_CLEANUP(ffb_uint8_free), &bt_out);
 
+#if ENABLE_MSBC
+	struct esco_msbc msbc = { .init = false };
+	pthread_cleanup_push(PTHREAD_CLEANUP(msbc_finish), &msbc);
+#endif
+
 	/* these buffers shall be bigger than the SCO MTU */
 	if (ffb_init(&bt_in, 128) == NULL ||
 			ffb_init(&bt_out, 128) == NULL) {
@@ -1576,6 +1582,20 @@ void *io_thread_sco(void *arg) {
 		pfds[3].fd = pfds[4].fd = -1;
 
 		switch (t->type.codec) {
+#if ENABLE_MSBC
+		case HFP_CODEC_MSBC:
+			msbc_encode(&msbc);
+			msbc_decode(&msbc);
+			if (t->mtu_read > 0 && ffb_blen_in(&msbc.dec_data) >= t->mtu_read)
+				pfds[1].fd = t->bt_fd;
+			if (t->mtu_write > 0 && ffb_blen_out(&msbc.enc_data) >= t->mtu_write)
+				pfds[2].fd = t->bt_fd;
+			if (t->mtu_write > 0 && ffb_blen_in(&msbc.enc_pcm) >= t->mtu_write)
+				pfds[3].fd = t->sco.spk_pcm.fd;
+			if (ffb_blen_out(&msbc.dec_pcm) > 0)
+				pfds[4].fd = t->sco.mic_pcm.fd;
+			break;
+#endif
 		case HFP_CODEC_CVSD:
 		default:
 			if (t->mtu_read > 0 && ffb_len_in(&bt_in) >= t->mtu_read)
@@ -1663,8 +1683,16 @@ void *io_thread_sco(void *arg) {
 				t->release(t);
 				asrs.frames = 0;
 			}
-			else
+			else {
 				t->acquire(t);
+#if ENABLE_MSBC
+				/* this can be called again, make sure it is idempotent */
+				if (t->type.codec == HFP_CODEC_MSBC && msbc_init(&msbc) != 0) {
+					error("Couldn't initialize mSBC codec: %s", strerror(errno));
+					goto fail;
+				}
+#endif
+			}
 
 			continue;
 		}
@@ -1680,6 +1708,12 @@ void *io_thread_sco(void *arg) {
 			ssize_t len;
 
 			switch (t->type.codec) {
+#if ENABLE_MSBC
+			case HFP_CODEC_MSBC:
+				buffer = msbc.dec_data.tail;
+				buffer_len = ffb_len_in(&msbc.dec_data);
+				break;
+#endif
 			case HFP_CODEC_CVSD:
 			default:
 				if (t->sco.mic_pcm.fd == -1)
@@ -1705,6 +1739,11 @@ void *io_thread_sco(void *arg) {
 				}
 
 			switch (t->type.codec) {
+#if ENABLE_MSBC
+			case HFP_CODEC_MSBC:
+				ffb_seek(&msbc.dec_data, len);
+				break;
+#endif
 			case HFP_CODEC_CVSD:
 			default:
 				ffb_seek(&bt_in, len);
@@ -1724,6 +1763,12 @@ void *io_thread_sco(void *arg) {
 			ssize_t len;
 
 			switch (t->type.codec) {
+#if ENABLE_MSBC
+			case HFP_CODEC_MSBC:
+				buffer = msbc.enc_data.data;
+				buffer_len = t->mtu_write;
+				break;
+#endif
 			case HFP_CODEC_CVSD:
 			default:
 				buffer = bt_out.data;
@@ -1747,6 +1792,11 @@ void *io_thread_sco(void *arg) {
 				}
 
 			switch (t->type.codec) {
+#if ENABLE_MSBC
+			case HFP_CODEC_MSBC:
+				ffb_shift(&msbc.enc_data, len);
+				break;
+#endif
 			case HFP_CODEC_CVSD:
 			default:
 				ffb_shift(&bt_out, len);
@@ -1761,6 +1811,12 @@ void *io_thread_sco(void *arg) {
 			ssize_t samples;
 
 			switch (t->type.codec) {
+#if ENABLE_MSBC
+			case HFP_CODEC_MSBC:
+				buffer = msbc.enc_pcm.tail;
+				samples = ffb_len_in(&msbc.enc_pcm);
+				break;
+#endif
 			case HFP_CODEC_CVSD:
 			default:
 				buffer = (int16_t *)bt_out.tail;
@@ -1779,6 +1835,11 @@ void *io_thread_sco(void *arg) {
 				snd_pcm_scale_s16le(buffer, samples, 1, 0, 0);
 
 			switch (t->type.codec) {
+#if ENABLE_MSBC
+			case HFP_CODEC_MSBC:
+				ffb_seek(&msbc.enc_pcm, samples);
+				break;
+#endif
 			case HFP_CODEC_CVSD:
 			default:
 				ffb_seek(&bt_out, samples * sizeof(int16_t));
@@ -1798,6 +1859,12 @@ void *io_thread_sco(void *arg) {
 			ssize_t samples;
 
 			switch (t->type.codec) {
+#if ENABLE_MSBC
+			case HFP_CODEC_MSBC:
+				buffer = msbc.dec_pcm.data;
+				samples = ffb_len_out(&msbc.dec_pcm);
+				break;
+#endif
 			case HFP_CODEC_CVSD:
 			default:
 				buffer = (int16_t *)bt_in.data;
@@ -1815,6 +1882,11 @@ void *io_thread_sco(void *arg) {
 			}
 
 			switch (t->type.codec) {
+#if ENABLE_MSBC
+			case HFP_CODEC_MSBC:
+				ffb_shift(&msbc.dec_pcm, samples);
+				break;
+#endif
 			case HFP_CODEC_CVSD:
 			default:
 				ffb_shift(&bt_in, samples * sizeof(int16_t));
@@ -1823,7 +1895,7 @@ void *io_thread_sco(void *arg) {
 		}
 
 		/* keep data transfer at a constant bit rate */
-		asrsync_sync(&asrs, 48 / 2);
+		asrsync_sync(&asrs, t->mtu_write / 2);
 		/* update busy delay (encoding overhead) */
 		t->delay = asrsync_get_busy_usec(&asrs) / 100;
 
@@ -1832,6 +1904,9 @@ void *io_thread_sco(void *arg) {
 fail:
 	pthread_setcancelstate(PTHREAD_CANCEL_DISABLE, NULL);
 fail_ffb:
+#if ENABLE_MSBC
+	pthread_cleanup_pop(1);
+#endif
 	pthread_cleanup_pop(1);
 	pthread_cleanup_pop(1);
 	pthread_cleanup_pop(1);