[PATCH v5 6/7] handsfree-audio: mSBC support

[=?unknown-8bit?q?Fr=C3=A9d=C3=A9ric?= Dalleau <frederic.dalleau@linux.intel.com>]

[-- Attachment #1: Type: text/plain, Size: 6470 bytes --]

---
 tools/handsfree-audio.c |  204 +++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 204 insertions(+)

diff --git a/tools/handsfree-audio.c b/tools/handsfree-audio.c
index f4699f8..1201a62 100644
--- a/tools/handsfree-audio.c
+++ b/tools/handsfree-audio.c
@@ -40,6 +40,8 @@
 #include <glib.h>
 
 #include <alsa/asoundlib.h>
+#include <sbc/sbc.h>
+#include "bluetooth.h"
 
 #define OFONO_SERVICE			"org.ofono"
 #define HFP_AUDIO_MANAGER_PATH		"/"
@@ -62,6 +64,28 @@ static GSList *threads = NULL;
 static gboolean option_nocvsd = FALSE;
 static gboolean option_nomsbc = FALSE;
 
+static const char sntable[4] = { 0x08, 0x38, 0xC8, 0xF8 };
+static const int audio_rates[3] = { 0, 8000, 16000 };
+
+struct msbc_parser {
+	uint8_t buffer[60];
+	int parsed;
+};
+
+struct msbc_codec {
+	sbc_t sbcenc; /* Coder data */
+	char *ebuffer; /* Codec transfer buffer */
+	size_t ebuffer_size; /* Size of the buffer */
+	size_t ebuffer_start; /* start of encoding data */
+	size_t ebuffer_end; /* end of encoding data */
+
+	struct msbc_parser parser; /* mSBC parser for concatenating frames */
+	sbc_t sbcdec; /* Decoder data */
+
+	size_t msbc_frame_size;
+	size_t decoded_frame_size;
+};
+
 struct hfp_thread {
 	int fd;
 	int running;
@@ -78,8 +102,78 @@ struct hfp_thread {
 	int (*encode)(struct hfp_thread *, char *data, int len);
 	int (*decode)(struct hfp_thread *, char *data, int len, char *out,
 								int outlen);
+	struct msbc_codec msbc;
 };
 
+static void msbc_parser_reset(struct msbc_parser *p)
+{
+	p->parsed = 0;
+}
+
+static int msbc_state_machine(struct msbc_parser *p, uint8_t byte)
+{
+	switch (p->parsed) {
+	case 0:
+		if (byte == 0x01)
+			goto copy;
+		return 0;
+	case 1:
+		if (byte == 0x08 || byte == 0x38 || byte == 0xC8
+				|| byte == 0xF8)
+			goto copy;
+		break;
+	case 2:
+		if (byte == 0xAD)
+			goto copy;
+		break;
+	case 3:
+		if (byte == 0x00)
+			goto copy;
+		break;
+	case 4:
+		if (byte == 0x00)
+			goto copy;
+		break;
+	default:
+		goto copy;
+	}
+
+	msbc_parser_reset(p);
+	return 0;
+
+copy:
+	p->buffer[p->parsed] = byte;
+	p->parsed++;
+
+	return p->parsed;
+}
+
+static size_t msbc_parse(sbc_t *sbcdec, struct msbc_parser *p, char *data,
+		int len, char *out, int outlen, int *bytes)
+{
+	size_t totalwritten = 0;
+	size_t written = 0;
+	int i;
+	*bytes = 0;
+
+	for (i = 0; i < len; i++) {
+		if (msbc_state_machine(p, data[i]) == 60) {
+			int decoded;
+			decoded = sbc_decode(sbcdec, p->buffer + 2,
+					p->parsed - 2 - 1, out, outlen,
+					&written);
+			if (decoded > 0) {
+				totalwritten += written;
+				*bytes += decoded;
+			} else {
+				DBG("Error while decoding: %d", decoded);
+			}
+			msbc_parser_reset(p);
+		}
+	}
+
+	return totalwritten;
+}
 
 static int hfp_audio_cvsd_init(struct hfp_thread *thread)
 {
@@ -125,6 +219,109 @@ static int hfp_audio_cvsd_decode(struct hfp_thread *thread, char *data,
 	return size;
 }
 
+/* Run from IO thread */
+static int hfp_audio_msbc_init(struct hfp_thread *thread)
+{
+	struct msbc_codec *codec = &thread->msbc;
+	struct bt_voice voice;
+
+	thread->rate = 16000;
+	thread->capture_size = 240; /* decoded mSBC frame */
+
+	memset(&voice, 0, sizeof(voice));
+	voice.setting = BT_VOICE_TRANSPARENT;
+	if (setsockopt(thread->fd, SOL_BLUETOOTH, BT_VOICE,
+					&voice, sizeof(voice)) < 0) {
+		DBG("Can't set transparent mode: %s (%d)",
+				strerror(errno), errno);
+		return -EOPNOTSUPP;
+	}
+
+	sbc_init_msbc(&codec->sbcenc, 0);
+	sbc_init_msbc(&codec->sbcdec, 0);
+
+	codec->msbc_frame_size = 2 + sbc_get_frame_length(&codec->sbcenc) + 1;
+	codec->decoded_frame_size = sbc_get_codesize(&codec->sbcenc);
+	msbc_parser_reset(&codec->parser);
+
+	/* 5 * 48 == 10 * 24 == 4 * 60 */
+	codec->ebuffer_size = codec->msbc_frame_size * 4;
+	codec->ebuffer = g_try_malloc(codec->ebuffer_size);
+	codec->ebuffer_start = 0;
+	codec->ebuffer_end = 0;
+
+	DBG("codec->msbc_frame_size %d", (int) codec->msbc_frame_size);
+	DBG("codec->ebuffer_size %d", (int) codec->ebuffer_size);
+	DBG("codec->decoded_frame_size %d", (int) codec->decoded_frame_size);
+
+	return 0;
+}
+
+/* Run from IO thread */
+static int hfp_audio_msbc_free(struct hfp_thread *thread)
+{
+	struct msbc_codec *codec = &thread->msbc;
+
+	g_free(codec->ebuffer);
+	sbc_finish(&codec->sbcenc);
+	sbc_finish(&codec->sbcdec);
+
+	return 0;
+}
+
+/* Run from IO thread */
+static int hfp_audio_msbc_encode(struct hfp_thread *thread, char *data, int len)
+{
+	struct msbc_codec *codec = &thread->msbc;
+	char *h2 = codec->ebuffer + codec->ebuffer_end;
+	static int sn = 0;
+	int written = 0;
+	char *qbuf;
+
+	h2[0] = 0x01;
+	h2[1] = sntable[sn];
+	h2[59] = 0xff;
+	sn = (sn + 1) % 4;
+
+	sbc_encode(&codec->sbcenc, data, len,
+			codec->ebuffer + codec->ebuffer_end + 2,
+			codec->ebuffer_size - codec->ebuffer_end - 2,
+			(ssize_t *) &written);
+
+	written += 2 /* H2 */ + 1 /* 0xff */;
+	codec->ebuffer_end += written;
+
+	/* Split into MTU sized chunks */
+	while (codec->ebuffer_start + thread->mtu <= codec->ebuffer_end) {
+		qbuf = g_try_malloc(thread->mtu);
+		if (!qbuf)
+			return -ENOMEM;
+
+		memcpy(qbuf, codec->ebuffer + codec->ebuffer_start,
+								thread->mtu);
+
+		thread->outq = g_slist_insert(thread->outq, qbuf, -1);
+
+		codec->ebuffer_start += thread->mtu;
+		if (codec->ebuffer_start >= codec->ebuffer_end)
+			codec->ebuffer_start = codec->ebuffer_end = 0;
+	}
+
+	return 0;
+}
+
+/* Run from IO thread */
+static int hfp_audio_msbc_decode(struct hfp_thread *thread, char *data,
+						int len, char *out, int outlen)
+{
+	struct msbc_codec *codec = &thread->msbc;
+	int written, decoded;
+
+	written = msbc_parse(&codec->sbcdec, &codec->parser, data, len, out,
+			outlen, &decoded);
+
+	return written;
+}
 
 static snd_pcm_t *hfp_audio_pcm_init(snd_pcm_stream_t stream, int rate)
 {
@@ -360,6 +557,13 @@ static int new_connection(int fd, int codec)
 		thread->decode = hfp_audio_cvsd_decode;
 		thread->encode = hfp_audio_cvsd_encode;
 		break;
+	case HFP_AUDIO_MSBC:
+		thread->rate = 16000;
+		thread->init = hfp_audio_msbc_init;
+		thread->free = hfp_audio_msbc_free;
+		thread->decode = hfp_audio_msbc_decode;
+		thread->encode = hfp_audio_msbc_encode;
+		break;
 	default:
 		return -EINVAL;
 	}
-- 
1.7.9.5