[PATCH] bluetooth: Wideband speech implementaion

[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]

 



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


[Index of Archives]     [Linux Audio Users]     [AMD Graphics]     [Linux USB Devel]     [Linux Audio Users]     [Yosemite News]     [Linux Kernel]     [Linux SCSI]

  Powered by Linux