mSBC-encoded streams for HFP. The wideband speec encoding and decoding
is implemeted with this patch. This patch was refered from original
patch of Joao Paula Rechi Vita and was verified with the supported
bluetooth controller.
Signed-off-by: Sathish Narasimman <sathish.narasimman at intel.com>
---
src/modules/bluetooth/backend-ofono.c | 20 +-
src/modules/bluetooth/module-bluez5-device.c | 372 ++++++++++++++++++++++++++-
2 files changed, 378 insertions(+), 14 deletions(-)
diff --git a/src/modules/bluetooth/backend-ofono.c b/src/modules/bluetooth/backend-ofono.c
index 1f0efe9..a836779 100644
--- a/src/modules/bluetooth/backend-ofono.c
+++ b/src/modules/bluetooth/backend-ofono.c
@@ -164,7 +164,7 @@ static int card_acquire(struct hf_audio_card *card) {
DBUS_TYPE_BYTE, &codec,
DBUS_TYPE_INVALID) == true)) {
dbus_message_unref(r);
- if (codec != HFP_AUDIO_CODEC_CVSD) {
+ if (codec != HFP_AUDIO_CODEC_CVSD && codec != HFP_AUDIO_CODEC_MSBC) {
pa_log_error("Invalid codec: %u", codec);
/* shutdown to make sure connection is dropped immediately */
shutdown(fd, SHUT_RDWR);
@@ -250,10 +250,17 @@ static int hf_audio_agent_transport_acquire(pa_bluetooth_transport *t, bool opti
* value from the Isoc USB endpoint in use by btusb and should be
* made available to userspace by the Bluetooth kernel subsystem.
* Meanwhile the empiric value 48 will be used. */
- if (imtu)
- *imtu = 48;
- if (omtu)
- *omtu = 48;
+ if (t->codec == HFP_AUDIO_CODEC_MSBC) {
+ if (imtu)
+ *imtu = 60;
+ if (omtu)
+ *omtu = 60;
+ } else {
+ if (imtu)
+ *imtu = 48;
+ if (omtu)
+ *omtu = 48;
+ }
err = socket_accept(card->fd);
if (err < 0) {
@@ -464,6 +471,7 @@ static void hf_audio_agent_register(pa_bluetooth_backend *hf) {
pa_assert_se(m = dbus_message_new_method_call(OFONO_SERVICE, "/", HF_AUDIO_MANAGER_INTERFACE, "Register"));
codecs[ncodecs++] = HFP_AUDIO_CODEC_CVSD;
+ codecs[ncodecs++] = HFP_AUDIO_CODEC_MSBC;
pa_assert_se(dbus_message_append_args(m, DBUS_TYPE_OBJECT_PATH, &path, DBUS_TYPE_ARRAY, DBUS_TYPE_BYTE, &pcodecs, ncodecs,
DBUS_TYPE_INVALID));
@@ -611,7 +619,7 @@ static DBusMessage *hf_audio_agent_new_connection(DBusConnection *c, DBusMessage
card->connecting = false;
- if (!card || codec != HFP_AUDIO_CODEC_CVSD || card->fd >= 0) {
+ if (!card || (codec != HFP_AUDIO_CODEC_CVSD && codec != HFP_AUDIO_CODEC_MSBC) || card->fd >= 0) {
pa_log_warn("New audio connection invalid arguments (path=%s fd=%d, codec=%d)", path, fd, codec);
pa_assert_se(r = dbus_message_new_error(m, "org.ofono.Error.InvalidArguments", "Invalid arguments in method call"));
shutdown(fd, SHUT_RDWR);
diff --git a/src/modules/bluetooth/module-bluez5-device.c b/src/modules/bluetooth/module-bluez5-device.c
index 9dbdca3..a404709 100644
--- a/src/modules/bluetooth/module-bluez5-device.c
+++ b/src/modules/bluetooth/module-bluez5-device.c
@@ -57,6 +57,9 @@ PA_MODULE_LOAD_ONCE(false);
PA_MODULE_USAGE("path=<device object path>"
"autodetect_mtu=<boolean>");
+#define HFP_AUDIO_CODEC_CVSD 0x01
+#define HFP_AUDIO_CODEC_MSBC 0x02
+
#define FIXED_LATENCY_PLAYBACK_A2DP (25 * PA_USEC_PER_MSEC)
#define FIXED_LATENCY_PLAYBACK_SCO (25 * PA_USEC_PER_MSEC)
#define FIXED_LATENCY_RECORD_A2DP (25 * PA_USEC_PER_MSEC)
@@ -106,6 +109,28 @@ typedef struct sbc_info {
size_t buffer_size; /* Size of the buffer */
} sbc_info_t;
+struct msbc_parser {
+ int len;
+ uint8_t buffer[60];
+};
+
+/* This structure to be moved to libsbc in the future */
+typedef struct msbc_info {
+ bool msbc_initialized; /* Keep track if the encoder is initialized */
+ sbc_t sbcenc; /* Encoder data */
+ uint8_t *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;
+
+} msbc_info_t;
+
struct userdata {
pa_module *module;
pa_core *core;
@@ -147,6 +172,7 @@ struct userdata {
pa_memchunk write_memchunk;
pa_sample_spec sample_spec;
struct sbc_info sbc_info;
+ struct msbc_info msbc_info;
};
typedef enum pa_bluetooth_form_factor {
@@ -251,6 +277,215 @@ static void connect_ports(struct userdata *u, void *new_data, pa_direction_t dir
}
/* Run from IO thread */
+static void msbc_parser_reset(struct msbc_parser *p) {
+ p->len = 0;
+}
+
+/* Run from IO thread */
+static int msbc_state_machine(struct msbc_parser *p, uint8_t byte) {
+ pa_assert(p->len < 60);
+
+ switch (p->len) {
+ 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;
+ }
+
+ p->len = 0;
+ return 0;
+copy:
+ p->buffer[p->len] = byte;
+ p->len ++;
+
+ return p->len;
+}
+
+/* Run from IO thread */
+static size_t msbc_parse(sbc_t *sbcdec, struct msbc_parser *p, uint8_t *data, int len, uint8_t *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 = 0;
+
+ /* Decode the recived data from the socket */
+ decoded = sbc_decode(sbcdec,
+ p->buffer + 2, p->len - 2 - 1,
+ out, outlen,
+ &written);
+
+ if (decoded > 0) {
+ totalwritten += written;
+ *bytes += decoded;
+ } else {
+ pa_log_debug("Error while decoding: %d\n", decoded);
+ }
+ msbc_parser_reset(p);
+ }
+ }
+ return totalwritten;
+}
+
+/* Run from IO thread */
+static void hsp_prepare_buffer(struct userdata *u) {
+
+ pa_assert(u);
+
+ /* Initialize sbc codec if not already done */
+ if (!u->msbc_info.msbc_initialized) {
+ sbc_init_msbc(&u->msbc_info.sbcenc, 0);
+ sbc_init_msbc(&u->msbc_info.sbcdec, 0);
+ u->msbc_info.msbc_frame_size = 2 + sbc_get_frame_length(&u->msbc_info.sbcenc) + 1;
+ u->msbc_info.decoded_frame_size = sbc_get_codesize(&u->msbc_info.sbcenc);
+ u->msbc_info.msbc_initialized = 1;
+ msbc_parser_reset(&u->msbc_info.parser);
+ }
+
+ /* Allocate a buffer for encoding, and a tmp buffer for sending */
+ if (u->msbc_info.ebuffer_size < u->msbc_info.msbc_frame_size) {
+ pa_xfree(u->msbc_info.ebuffer);
+ u->msbc_info.ebuffer_size = u->msbc_info.msbc_frame_size * 4; /* 5 * 48 = 10 * 24 = 4 * 60 */
+ u->msbc_info.ebuffer = pa_xmalloc(u->msbc_info.ebuffer_size);
+ u->msbc_info.ebuffer_start = 0;
+ u->msbc_info.ebuffer_end = 0;
+ }
+}
+
+static const char sntable[4] = { 0x08, 0x38, 0xC8, 0xF8 };
+
+/* Run from IO thread */
+static int sco_process_render_msbc(struct userdata *u) {
+
+ int ret = 0;
+ size_t to_write, to_encode;
+
+ pa_assert(u);
+ pa_assert(u->profile == PA_BLUETOOTH_PROFILE_HEADSET_HEAD_UNIT || u->profile == PA_BLUETOOTH_PROFILE_HEADSET_AUDIO_GATEWAY);
+ pa_assert(u->sink);
+
+ hsp_prepare_buffer(u);
+
+ /* First, render some data */
+ if (!u->write_memchunk.memblock)
+ pa_sink_render_full(u->sink, u->msbc_info.decoded_frame_size, &u->write_memchunk);
+
+ for (;;) {
+ int l = 0;
+ bool wrote = false;
+ const void *p;
+ void *d;
+ ssize_t written = 0;
+ ssize_t encoded;
+ uint8_t *h2 = u->msbc_info.ebuffer + u->msbc_info.ebuffer_end;
+ static int sn = 0;
+
+ /* Now write that data to the socket. The socket is of type
+ * SEQPACKET, and we generated the data of the MTU size, so this
+ * should just work. */
+
+ p = (const uint8_t *) pa_memblock_acquire_chunk(&u->write_memchunk);
+ to_encode = u->write_memchunk.length;
+
+ d = ((uint8_t *)u->msbc_info.ebuffer) + u->msbc_info.ebuffer_end + 2;
+ to_write = u->msbc_info.ebuffer_size - u->msbc_info.ebuffer_end - 2;
+
+ h2[0] = 0x01;
+ h2[1] = sntable[sn];
+ h2[59] = 0x00;
+ sn = (sn + 1) % 4;
+
+ pa_assert(u->msbc_info.ebuffer_end + u->msbc_info.msbc_frame_size <= u->msbc_info.ebuffer_size);
+
+ encoded = sbc_encode(&u->msbc_info.sbcenc, p, to_encode, d, to_write, &written);
+
+ if (PA_UNLIKELY(encoded <= 0)) {
+ pa_log_error("MSBC encoding error (%li)", (long) encoded);
+ pa_memblock_release(u->write_memchunk.memblock);
+ return -1;
+ }
+
+ written += 2 /* H2 */ + 1 /* 0x00 */;
+ pa_assert((size_t)written == u->msbc_info.msbc_frame_size);
+ u->msbc_info.ebuffer_end += written;
+
+ /* Send MTU bytes of it, if there is more it will send later */
+ while (u->msbc_info.ebuffer_start + u->write_link_mtu <= u->msbc_info.ebuffer_end) {
+ l = pa_write(u->stream_fd,
+ u->msbc_info.ebuffer + u->msbc_info.ebuffer_start,
+ u->write_link_mtu,
+ &u->stream_write_type);
+
+ wrote = true;
+ if (l <= 0) {
+ pa_log_debug("Error while writing: l %d, errno %d", l, errno);
+ break;
+ }
+
+ u->msbc_info.ebuffer_start += l;
+ if (u->msbc_info.ebuffer_start >= u->msbc_info.ebuffer_end)
+ u->msbc_info.ebuffer_start = u->msbc_info.ebuffer_end = 0;
+ }
+
+ pa_memblock_release(u->write_memchunk.memblock);
+
+ if (wrote && l < 0) {
+
+ if (errno == EINTR)
+ /* Retry right away if we got interrupted */
+ continue;
+
+ else if (errno == EAGAIN)
+ /* Hmm, apparently the socket was not writable, give up for now */
+ break;
+
+ pa_log_error("Failed to write data to SCO socket: %s", pa_cstrerror(errno));
+ ret = -1;
+ break;
+ }
+
+ if ((size_t) l != (size_t)u->write_link_mtu) {
+ pa_log_error("Wrote memory block to socket only partially! %llu written, wanted to write %llu.",
+ (unsigned long long) l,
+ (unsigned long long) u->write_link_mtu);
+ ret = -1;
+ break;
+ }
+
+ u->write_index += (uint64_t) u->write_memchunk.length;
+
+ pa_memblock_unref(u->write_memchunk.memblock);
+ pa_memchunk_reset(&u->write_memchunk);
+
+ ret = 1;
+ break;
+ }
+
+ return ret;
+}
+
+/* Run from IO thread */
static int sco_process_render(struct userdata *u) {
ssize_t l;
pa_memchunk memchunk;
@@ -318,6 +553,108 @@ static int sco_process_render(struct userdata *u) {
}
/* Run from IO thread */
+static int sco_process_push_msbc(struct userdata *u) {
+
+ int ret = 0;
+ pa_memchunk memchunk;
+
+ pa_assert(u);
+ pa_assert(u->profile == PA_BLUETOOTH_PROFILE_HEADSET_HEAD_UNIT ||
+ u->profile == PA_BLUETOOTH_PROFILE_HEADSET_AUDIO_GATEWAY);
+ pa_assert(u->source);
+ pa_assert(u->read_smoother);
+
+ /*Prepare the buffer before allocating memory*/
+ hsp_prepare_buffer(u);
+
+ u->read_block_size = u->msbc_info.decoded_frame_size;
+ memchunk.memblock = pa_memblock_new(u->core->mempool, u->read_block_size);
+ memchunk.index = memchunk.length = 0;
+
+ for (;;) {
+ ssize_t l;
+ void *p;
+ struct msghdr m;
+ struct cmsghdr *cm;
+ uint8_t aux[1024];
+ struct iovec iov;
+ bool found_tstamp = false;
+ pa_usec_t tstamp;
+ int decoded = 0;
+ size_t written;
+ uint8_t *tmpbuf = pa_xmalloc(u->read_link_mtu);
+
+ pa_zero(m);
+ pa_zero(aux);
+ pa_zero(iov);
+
+ m.msg_iov = &iov;
+ m.msg_iovlen = 1;
+ m.msg_control = aux;
+ m.msg_controllen = sizeof(aux);
+ iov.iov_base = tmpbuf;
+ iov.iov_len = u->read_link_mtu;
+
+ /* Receive data from the socket */
+ l = recvmsg(u->stream_fd, &m, 0);
+
+ if (l <= 0) {
+
+ if (l < 0 && errno == EINTR)
+ /* Retry right away if we got interrupted */
+ continue;
+
+ else if (l < 0 && errno == EAGAIN)
+ /* Hmm, apparently the socket was not readable, give up for now. */
+ break;
+
+ pa_log_error("Failed to read data from SCO socket: %s", l < 0 ? pa_cstrerror(errno) : "EOF");
+ ret = -1;
+ break;
+ }
+
+ p = pa_memblock_acquire(memchunk.memblock);
+ written = msbc_parse(&u->msbc_info.sbcdec, &u->msbc_info.parser, tmpbuf, l, p, pa_memblock_get_length(memchunk.memblock), &decoded);
+ pa_memblock_release(memchunk.memblock);
+
+ pa_xfree(tmpbuf);
+
+ memchunk.length = (size_t) written;
+
+ u->read_index += (uint64_t) decoded;
+
+ for (cm = CMSG_FIRSTHDR(&m); cm; cm = CMSG_NXTHDR(&m, cm)) {
+ if (cm->cmsg_level == SOL_SOCKET && cm->cmsg_type == SO_TIMESTAMP) {
+ struct timeval *tv = (struct timeval*) CMSG_DATA(cm);
+ pa_rtclock_from_wallclock(tv);
+ tstamp = pa_timeval_load(tv);
+ found_tstamp = true;
+ break;
+ }
+ }
+
+ if (!found_tstamp) {
+ pa_log_warn("Couldn't find SO_TIMESTAMP data in auxiliary recvmsg() data!");
+ tstamp = pa_rtclock_now();
+ }
+
+ pa_smoother_put(u->read_smoother, tstamp, pa_bytes_to_usec(u->read_index, &u->sample_spec));
+ pa_smoother_resume(u->read_smoother, tstamp, true);
+
+ if (memchunk.length > 0) {
+ pa_source_post(u->source, &memchunk);
+ }
+
+ ret = decoded;
+ break;
+ }
+
+ pa_memblock_unref(memchunk.memblock);
+
+ return ret;
+}
+
+/* Run from IO thread */
static int sco_process_push(struct userdata *u) {
ssize_t l;
pa_memchunk memchunk;
@@ -1294,7 +1631,7 @@ static void transport_config(struct userdata *u) {
if (u->profile == PA_BLUETOOTH_PROFILE_HEADSET_HEAD_UNIT || u->profile == PA_BLUETOOTH_PROFILE_HEADSET_AUDIO_GATEWAY) {
u->sample_spec.format = PA_SAMPLE_S16LE;
u->sample_spec.channels = 1;
- u->sample_spec.rate = 8000;
+ u->sample_spec.rate = (u->transport->codec == HFP_AUDIO_CODEC_CVSD) ? 8000 : 16000;
} else {
sbc_info_t *sbc_info = &u->sbc_info;
a2dp_sbc_t *config;
@@ -1481,8 +1818,16 @@ static int write_block(struct userdata *u) {
if ((n_written = a2dp_process_render(u)) < 0)
return -1;
} else {
- if ((n_written = sco_process_render(u)) < 0)
- return -1;
+ if (u->transport->codec == HFP_AUDIO_CODEC_CVSD) {
+ if ((n_written = sco_process_render(u)) < 0)
+ return -1;
+ } else if (u->transport->codec == HFP_AUDIO_CODEC_MSBC) {
+ if ((n_written = sco_process_render_msbc(u)) < 0)
+ return -1;
+ } else {
+ n_written = -1;
+ pa_log("Invalid codec for encoding: %d", u->transport->codec);
+ }
}
return n_written;
@@ -1550,11 +1895,21 @@ static void thread_func(void *userdata) {
/* If we got woken up by POLLIN let's do some reading */
if (pollfd->revents & POLLIN) {
int n_read;
-
if (u->profile == PA_BLUETOOTH_PROFILE_A2DP_SOURCE)
n_read = a2dp_process_push(u);
- else
- n_read = sco_process_push(u);
+ else {
+ switch(u->transport->codec) {
+ case HFP_AUDIO_CODEC_CVSD:
+ n_read = sco_process_push(u);
+ break;
+ case HFP_AUDIO_CODEC_MSBC:
+ n_read = sco_process_push_msbc(u);
+ break;
+ default:
+ pa_log_error("Invalid codec for encoding %d", u->transport->codec);
+ n_read = -1;
+ }
+ }
if (n_read < 0)
goto fail;
@@ -1604,9 +1959,10 @@ static void thread_func(void *userdata) {
if (blocks_to_write > 0)
writable = false;
}
+ }
- /* There is no source, we have to use the system clock for timing */
- } else {
+ /* There is no source or audio codec was MSBC, we have to use the system clock for timing */
+ if ((u->transport->codec == HFP_AUDIO_CODEC_MSBC) || !have_source){
bool have_written = false;
pa_usec_t time_passed = 0;
pa_usec_t audio_sent = 0;
--
2.7.4
