Hi,
Any comment over this patch?
may I know the blocking?
On Thu, Aug 16, 2018 at 2:37 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 | 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
>
>
-------------- next part --------------
An HTML attachment was scrubbed...
URL: <https://lists.freedesktop.org/archives/pulseaudio-discuss/attachments/20180820/770cc0a0/attachment-0001.html>
