From: Fr?d?ric Dalleau <frederic.dalleau@xxxxxxxxxxxxxxx>
---
src/modules/bluetooth/bluetooth-util.c | 1 +
src/modules/bluetooth/bluetooth-util.h | 1 +
src/modules/bluetooth/module-bluetooth-device.c | 327 +++++++++++++++++++++++-
3 files changed, 324 insertions(+), 5 deletions(-)
diff --git a/src/modules/bluetooth/bluetooth-util.c b/src/modules/bluetooth/bluetooth-util.c
index 0fd4c8e..572edcf 100644
--- a/src/modules/bluetooth/bluetooth-util.c
+++ b/src/modules/bluetooth/bluetooth-util.c
@@ -1345,6 +1345,7 @@ static void ofono_init(pa_bluetooth_discovery *y) {
pa_assert_se(m = dbus_message_new_method_call("org.ofono", "/", "org.ofono.HandsfreeAudioManager", "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,
diff --git a/src/modules/bluetooth/bluetooth-util.h b/src/modules/bluetooth/bluetooth-util.h
index d9968cf..b33e97f 100644
--- a/src/modules/bluetooth/bluetooth-util.h
+++ b/src/modules/bluetooth/bluetooth-util.h
@@ -39,6 +39,7 @@
#define HFP_AG_UUID "0000111f-0000-1000-8000-00805f9b34fb"
#define HFP_AUDIO_CODEC_CVSD 0x01
+#define HFP_AUDIO_CODEC_MSBC 0x02
#define ADVANCED_AUDIO_UUID "0000110d-0000-1000-8000-00805f9b34fb"
diff --git a/src/modules/bluetooth/module-bluetooth-device.c b/src/modules/bluetooth/module-bluetooth-device.c
index 6664c10..a94fa4e 100644
--- a/src/modules/bluetooth/module-bluetooth-device.c
+++ b/src/modules/bluetooth/module-bluetooth-device.c
@@ -115,11 +115,30 @@ struct a2dp_info {
uint8_t max_bitpool;
};
+struct msbc_parser {
+ int len;
+ uint8_t buffer[60];
+};
+
struct hsp_info {
pa_sink *sco_sink;
void (*sco_sink_set_volume)(pa_sink *s);
pa_source *sco_source;
void (*sco_source_set_volume)(pa_source *s);
+
+ pa_bool_t sbc_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;
+
};
struct bluetooth_msg {
@@ -563,6 +582,97 @@ static int device_process_msg(pa_msgobject *obj, int code, void *data, int64_t o
return 0;
}
+static void msbc_parser_reset(struct msbc_parser *p) {
+ p->len = 0;
+}
+
+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;
+}
+
+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;
+ 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->hsp.sbc_initialized) {
+ sbc_init_msbc(&u->hsp.sbcenc, 0);
+ sbc_init_msbc(&u->hsp.sbcdec, 0);
+ u->hsp.msbc_frame_size = 2 + sbc_get_frame_length(&u->hsp.sbcenc) + 1;
+ u->hsp.decoded_frame_size = sbc_get_codesize(&u->hsp.sbcenc);
+ u->hsp.sbc_initialized = TRUE;
+ msbc_parser_reset(&u->hsp.parser);
+ }
+
+ /* Allocate a buffer for encoding, and a tmp buffer for sending */
+ if (u->hsp.ebuffer_size < u->hsp.msbc_frame_size) {
+ pa_xfree(u->hsp.ebuffer);
+ u->hsp.ebuffer_size = u->hsp.msbc_frame_size * 4; /* 5 * 48 = 10 * 24 = 4 * 60 */
+ u->hsp.ebuffer = pa_xmalloc(u->hsp.ebuffer_size);
+ u->hsp.ebuffer_start = 0;
+ u->hsp.ebuffer_end = 0;
+ }
+}
+
+static const char sntable[4] = { 0x08, 0x38, 0xC8, 0xF8 };
+
/* Run from IO thread */
static int hsp_process_render(struct userdata *u) {
int ret = 0;
@@ -628,6 +738,94 @@ static int hsp_process_render(struct userdata *u) {
}
/* Run from IO thread */
+static int hsp_process_render_msbc(struct userdata *u) {
+ int ret = 0;
+
+ pa_assert(u);
+ pa_assert(u->profile == PROFILE_HSP || u->profile == PROFILE_HFGW);
+ pa_assert(u->sink);
+
+ hsp_prepare_buffer(u);
+
+ /* First, render some data */
+ if (!u->write_memchunk.memblock)
+ pa_sink_render_full(u->sink, u->hsp.decoded_frame_size, &u->write_memchunk);
+
+ for (;;) {
+ int l = 0;
+ pa_bool_t wrote = false;
+ const uint8_t *p;
+ size_t written = 0;
+ uint8_t *h2 = u->hsp.ebuffer + u->hsp.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);
+ h2[0] = 0x01;
+ h2[1] = sntable[sn];
+ h2[59] = 0xff;
+ sn = (sn + 1) % 4;
+
+ pa_assert(u->hsp.ebuffer_end + u->hsp.msbc_frame_size <= u->hsp.ebuffer_size);
+ sbc_encode(&u->hsp.sbcenc, p, u->write_memchunk.length, ((uint8_t *)u->hsp.ebuffer)+u->hsp.ebuffer_end+2, u->hsp.ebuffer_size-u->hsp.ebuffer_end-2, (ssize_t *)&written);
+
+ written += 2 /* H2 */ + 1 /* 0xff */;
+ pa_assert(written == u->hsp.msbc_frame_size);
+ u->hsp.ebuffer_end += written;
+
+ /* Send MTU bytes of it, if there is more it will send later */
+ while (u->hsp.ebuffer_start + u->write_link_mtu <= u->hsp.ebuffer_end) {
+ l = pa_write(u->stream_fd, u->hsp.ebuffer + u->hsp.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->hsp.ebuffer_start += l;
+ if (u->hsp.ebuffer_start >= u->hsp.ebuffer_end)
+ u->hsp.ebuffer_start = u->hsp.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 hsp_process_push(struct userdata *u) {
int ret = 0;
pa_memchunk memchunk;
@@ -725,6 +923,102 @@ static int hsp_process_push(struct userdata *u) {
}
/* Run from IO thread */
+static int hsp_process_push_msbc(struct userdata *u) {
+ int ret = 0;
+ pa_memchunk memchunk;
+
+ pa_assert(u);
+ pa_assert(u->profile == PROFILE_HSP || u->profile == PROFILE_HFGW);
+ pa_assert(u->source);
+ pa_assert(u->read_smoother);
+
+ u->read_block_size = u->hsp.decoded_frame_size;
+ memchunk.memblock = pa_memblock_new(u->core->mempool, u->read_block_size);
+ memchunk.index = memchunk.length = 0;
+
+ hsp_prepare_buffer(u);
+
+ 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);
+
+ memset(&m, 0, sizeof(m));
+ memset(&aux, 0, sizeof(aux));
+ memset(&iov, 0, sizeof(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;
+ 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->hsp.sbcdec, &u->hsp.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 void a2dp_prepare_buffer(struct userdata *u) {
size_t min_buffer_size = PA_MAX(u->read_link_mtu, u->write_link_mtu);
@@ -1046,7 +1340,18 @@ static void thread_func(void *userdata) {
int n_read;
if (u->profile == PROFILE_HSP || u->profile == PROFILE_HFGW)
- n_read = hsp_process_push(u);
+ switch(u->transport->codec) {
+ case HFP_AUDIO_CODEC_CVSD:
+ n_read = hsp_process_push(u);
+ break;
+ case HFP_AUDIO_CODEC_MSBC:
+ n_read = hsp_process_push_msbc(u);
+ break;
+ default:
+ pa_log_error("Invalid codec for encoding %d", u->transport->codec);
+ n_read = -1;
+ }
+
else
n_read = a2dp_process_push(u);
@@ -1069,7 +1374,11 @@ static void thread_func(void *userdata) {
if (pollfd->revents & POLLOUT)
writable = true;
- if ((!u->source || !PA_SOURCE_IS_LINKED(u->source->thread_info.state)) && do_write <= 0 && writable) {
+ /* Force time based scheduling for outgoing packets */
+ /* This is necessary for mSBC because one pushed PCM frame != one sent mSBC frame */
+ if (u->transport->codec == 2 ||
+ ((!u->source || !PA_SOURCE_IS_LINKED(u->source->thread_info.state)) && do_write <= 0 && writable)
+ ) {
pa_usec_t time_passed;
pa_usec_t audio_sent;
@@ -1121,8 +1430,16 @@ static void thread_func(void *userdata) {
if ((n_written = a2dp_process_render(u)) < 0)
goto io_fail;
} else {
- if ((n_written = hsp_process_render(u)) < 0)
- goto io_fail;
+ if (u->transport->codec == 1) {
+ if ((n_written = hsp_process_render(u)) < 0)
+ goto io_fail;
+ } else if (u->transport->codec == 2) {
+ if ((n_written = hsp_process_render_msbc(u)) < 0)
+ goto io_fail;
+ } else {
+ n_written = -1;
+ pa_log_debug("Invalid codec for encoding %d", u->transport->codec);
+ }
}
if (n_written == 0)
@@ -1824,7 +2141,7 @@ static void bt_transport_config(struct userdata *u) {
if (u->profile == PROFILE_HSP || u->profile == PROFILE_HFGW) {
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
bt_transport_config_a2dp(u);
}
--
1.7.11.7
