Hi Sathish,
On Tue, Aug 14, 2018 at 5:15 PM Sathish Narasimman <nsathish41 at gmail.com> wrote:
>
> 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 | 371 ++++++++++++++++++++++++++-
> 2 files changed, 377 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..c9b88bd 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,27 @@ typedef struct sbc_info {
> size_t buffer_size; /* Size of the buffer */
> } sbc_info_t;
>
> +struct msbc_parser {
> + int len;
> + uint8_t buffer[60];
> +};
> +
> +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;
Have you though about putting this into libsbc?
> struct userdata {
> pa_module *module;
> pa_core *core;
> @@ -147,6 +171,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 +276,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 +552,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 +1630,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 +1817,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 +1894,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 +1958,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){
Was this the reason we were not in sync?
> bool have_written = false;
> pa_usec_t time_passed = 0;
> pa_usec_t audio_sent = 0;
> --
> 2.7.4
>
> _______________________________________________
> pulseaudio-discuss mailing list
> pulseaudio-discuss at lists.freedesktop.org
> https://lists.freedesktop.org/mailman/listinfo/pulseaudio-discuss
--
Luiz Augusto von Dentz
