diff --git a/meson.build b/meson.build index 6b86ba2bc..fd146a7e7 100644 --- a/meson.build +++ b/meson.build @@ -696,6 +696,7 @@ if avahi_dep.found() endif sbc_dep = dependency('sbc', version : '>= 1.0', required : false) + if get_option('bluez5') assert(dbus_dep.found(), 'BlueZ requires D-Bus support') assert(sbc_dep.found(), 'BlueZ requires SBC support') diff --git a/src/modules/bluetooth/a2dp-codec-util.c b/src/modules/bluetooth/a2dp-codec-util.c index 23860eafb..355e19ca7 100644 --- a/src/modules/bluetooth/a2dp-codec-util.c +++ b/src/modules/bluetooth/a2dp-codec-util.c @@ -29,12 +29,14 @@ #include "a2dp-codec-util.h" +extern const pa_a2dp_codec pa_bt_codec_msbc; extern const pa_a2dp_codec pa_bt_codec_cvsd; /* List of HSP/HFP codecs. */ static const pa_a2dp_codec *pa_hf_codecs[] = { &pa_bt_codec_cvsd, + &pa_bt_codec_msbc, }; extern const pa_a2dp_codec pa_a2dp_codec_sbc; diff --git a/src/modules/bluetooth/bt-codec-msbc.c b/src/modules/bluetooth/bt-codec-msbc.c new file mode 100644 index 000000000..af99a4774 --- /dev/null +++ b/src/modules/bluetooth/bt-codec-msbc.c @@ -0,0 +1,290 @@ +/*** + This file is part of PulseAudio. + + PulseAudio is free software; you can redistribute it and/or modify + it under the terms of the GNU Lesser General Public License as + published by the Free Software Foundation; either version 2.1 of the + License, or (at your option) any later version. + + PulseAudio is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with PulseAudio; if not, see . +***/ +#ifdef HAVE_CONFIG_H +#include +#endif + +#include "a2dp-codec-api.h" + +#include "bt-codec-msbc.h" +#include + +typedef struct sbc_info { + sbc_t sbc; /* Codec data */ + size_t codesize, frame_length; /* SBC Codesize, frame_length. We simply cache those values here */ + uint8_t msbc_seq:2; /* mSBC packet sequence number, 2 bits only */ + + uint16_t msbc_push_offset; + uint8_t input_buffer[MSBC_PACKET_SIZE]; /* Codec transfer buffer */ + + pa_sample_spec sample_spec; +} sbc_info_t; + +static void *init(bool for_encoding, bool for_backchannel, const uint8_t *config_buffer, uint8_t config_size, pa_sample_spec *sample_spec, pa_core *core) { + struct sbc_info *info; + int ret; + + info = pa_xnew0(struct sbc_info, 1); + + ret = sbc_init_msbc(&info->sbc, 0); + if (ret != 0) { + pa_xfree(info); + pa_log_error("mSBC initialization failed: %d", ret); + return NULL; + } + + info->sbc.endian = SBC_LE; + + info->codesize = sbc_get_codesize(&info->sbc); + info->frame_length = sbc_get_frame_length(&info->sbc); + pa_log_info("mSBC codesize=%d, frame_length=%d", + (int)info->codesize, + (int)info->frame_length); + + info->sample_spec.format = PA_SAMPLE_S16LE; + info->sample_spec.channels = 1; + info->sample_spec.rate = 16000; + + pa_assert(pa_frame_aligned(info->codesize, &info->sample_spec)); + + *sample_spec = info->sample_spec; + + return info; +} + +static void deinit(void *codec_info) { + struct sbc_info *sbc_info = (struct sbc_info *) codec_info; + + sbc_finish(&sbc_info->sbc); + pa_xfree(sbc_info); +} + +static int reset(void *codec_info) { + struct sbc_info *sbc_info = (struct sbc_info *) codec_info; + int ret; + + /* SBC library release 1.5 has a bug in sbc_reinit_msbc: + * it forgets to restore priv->msbc flag after clearing priv content. + * This causes decoder assertion on first call since codesize would be + * different from expected for mSBC configuration. + * + * Do not use sbc_reinit_msbc until it is fixed. + */ + + sbc_finish(&sbc_info->sbc); + ret = sbc_init_msbc(&sbc_info->sbc, 0); + if (ret != 0) { + pa_xfree(sbc_info); + pa_log_error("mSBC initialization failed: %d", ret); + return -1; + } + + sbc_info->sbc.endian = SBC_LE; + + sbc_info->msbc_seq = 0; + sbc_info->msbc_push_offset = 0; + + return 0; +} + +static size_t get_read_block_size(void *codec_info, size_t link_mtu) { + struct sbc_info *info = (struct sbc_info *) codec_info; + size_t block_size = info->codesize; + + /* this never happens as sbc_info->codesize is always frame-aligned */ + if (!pa_frame_aligned(block_size, &info->sample_spec)) { + pa_log_debug("Got invalid block size: %lu, rounding down", block_size); + block_size = pa_frame_align(block_size, &info->sample_spec); + } + + return block_size; +} + +static size_t get_write_block_size(void *codec_info, size_t link_mtu) { + struct sbc_info *info = (struct sbc_info *) codec_info; + return info->codesize; +} + +static size_t get_encoded_block_size(void *codec_info, size_t input_size) { + struct sbc_info *info = (struct sbc_info *) codec_info; + size_t encoded_size = MSBC_PACKET_SIZE; + + /* input size should be aligned to write block size */ + pa_assert_fp(input_size % info->codesize == 0); + + return encoded_size * (input_size / info->codesize); +} + +static size_t reduce_encoder_bitrate(void *codec_info, size_t write_link_mtu) { + return 0; +} + +static size_t increase_encoder_bitrate(void *codec_info, size_t write_link_mtu) { + return 0; +} + +static size_t encode_buffer(void *codec_info, uint32_t timestamp, const uint8_t *input_buffer, size_t input_size, uint8_t *output_buffer, size_t output_size, size_t *processed) { + struct sbc_info *sbc_info = (struct sbc_info *) codec_info; + struct msbc_frame *frame; + uint8_t seq; + ssize_t encoded; + ssize_t written; + + pa_assert(input_size == sbc_info->codesize); + + /* must be room to render packet */ + pa_assert(output_size >= MSBC_PACKET_SIZE); + + frame = (struct msbc_frame *)output_buffer; + seq = sbc_info->msbc_seq++; + frame->hdr.id0 = MSBC_H2_ID0; + frame->hdr.id1.s.id1 = MSBC_H2_ID1; + if (seq & 0x02) + frame->hdr.id1.s.sn1 = 3; + else + frame->hdr.id1.s.sn1 = 0; + if (seq & 0x01) + frame->hdr.id1.s.sn0 = 3; + else + frame->hdr.id1.s.sn0 = 0; + + encoded = sbc_encode(&sbc_info->sbc, + input_buffer, input_size, + frame->payload, MSBC_FRAME_SIZE, + &written); + + frame->padding = 0x00; + + if (PA_UNLIKELY(encoded <= 0)) { + pa_log_error("SBC encoding error (%li)", (long) encoded); + return -1; + } + + pa_assert_fp((size_t) encoded == sbc_info->codesize); + pa_assert_fp((size_t) written == sbc_info->frame_length); + + *processed = encoded; + + return MSBC_PACKET_SIZE; +} + +/* + * We build a msbc frame up in the sbc_info buffer until we have a whole one + */ +static struct msbc_frame *msbc_find_frame(struct sbc_info *si, ssize_t *len, + const uint8_t *buf, int *pseq) +{ + int i; + uint8_t *p = si->input_buffer; + + for (i = 0; i < *len; i++) { + union msbc_h2_id1 id1; + + if (si->msbc_push_offset == 0) { + if (buf[i] != MSBC_H2_ID0) + continue; + } else if (si->msbc_push_offset == 1) { + id1.b = buf[i]; + + if (id1.s.id1 != MSBC_H2_ID1) + goto error; + if (id1.s.sn0 != 3 && id1.s.sn0 != 0) + goto error; + if (id1.s.sn1 != 3 && id1.s.sn1 != 0) + goto error; + } else if (si->msbc_push_offset == 2) { + if (buf[i] != MSBC_SYNC_BYTE) + goto error; + } + p[si->msbc_push_offset++] = buf[i]; + + if (si->msbc_push_offset == MSBC_PACKET_SIZE) { + id1.b = p[1]; + *pseq = (id1.s.sn0 & 0x1) | (id1.s.sn1 & 0x2); + si->msbc_push_offset = 0; + *len = *len - i; + return (struct msbc_frame *)p; + } + continue; + + error: + si->msbc_push_offset = 0; + } + *len = 0; + return NULL; +} + +static size_t decode_buffer(void *codec_info, const uint8_t *input_buffer, size_t input_size, uint8_t *output_buffer, size_t output_size, size_t *processed) { + struct sbc_info *sbc_info = (struct sbc_info *) codec_info; + ssize_t remaining; + ssize_t decoded; + size_t written = 0; + struct msbc_frame *frame; + int seq; + + remaining = input_size; + frame = msbc_find_frame(sbc_info, &remaining, input_buffer, &seq); + + /* only process when we have a full frame */ + if (!frame) { + *processed = input_size - remaining; + return 0; + } + + uint8_t lost_packets = (4 + seq - sbc_info->msbc_seq++) % 4; + + if (lost_packets) { + pa_log_error("Lost %d input audio packet(s)", lost_packets); + sbc_info->msbc_seq = seq + 1; + } + + decoded = sbc_decode(&sbc_info->sbc, frame->payload, MSBC_FRAME_SIZE, output_buffer, output_size, &written); + + /* now we've consumed the sbc_info buffer, start a new one with + * the partial frame we have */ + if (remaining > 0) + msbc_find_frame(sbc_info, &remaining, input_buffer + input_size - remaining, &seq); + + pa_assert_fp(remaining == 0); + + if (PA_UNLIKELY(decoded <= 0)) { + pa_log_error("mSBC decoding error (%li)", (long) decoded); + *processed = 0; + return 0; + } + pa_assert_fp((size_t)decoded == sbc_info->frame_length); + pa_assert_fp((size_t)written == sbc_info->codesize); + + *processed = input_size - remaining; + return written; +} + +/* Modified SBC codec for HFP Wideband Speech*/ +const pa_a2dp_codec pa_bt_codec_msbc = { + .name = "mSBC", + .description = "mSBC", + .init = init, + .deinit = deinit, + .reset = reset, + .get_read_block_size = get_read_block_size, + .get_write_block_size = get_write_block_size, + .get_encoded_block_size = get_encoded_block_size, + .reduce_encoder_bitrate = reduce_encoder_bitrate, + .increase_encoder_bitrate = increase_encoder_bitrate, + .encode_buffer = encode_buffer, + .decode_buffer = decode_buffer, +}; diff --git a/src/modules/bluetooth/bt-codec-msbc.h b/src/modules/bluetooth/bt-codec-msbc.h new file mode 100644 index 000000000..3236ca1cc --- /dev/null +++ b/src/modules/bluetooth/bt-codec-msbc.h @@ -0,0 +1,35 @@ +#pragma once + +/* + * Parameters for use with mSBC over eSCO link + */ + +#define MSBC_H2_ID0 0x01 +#define MSBC_H2_ID1 0x08 +#define MSBC_FRAME_SIZE 57 + +#define MSBC_SYNC_BYTE 0xad + +struct msbc_h2_id1_s { + uint8_t id1:4; + uint8_t sn0:2; + uint8_t sn1:2; +} __attribute__ ((packed)); + +union msbc_h2_id1 { + struct msbc_h2_id1_s s; + uint8_t b; +}; + +struct msbc_h2_header { + uint8_t id0; + union msbc_h2_id1 id1; +} __attribute__ ((packed)); + +struct msbc_frame { + struct msbc_h2_header hdr; + uint8_t payload[MSBC_FRAME_SIZE]; + uint8_t padding; /* must be zero */ +} __attribute__ ((packed)); + +#define MSBC_PACKET_SIZE sizeof(struct msbc_frame) diff --git a/src/modules/bluetooth/meson.build b/src/modules/bluetooth/meson.build index 7f2e6db7a..99263bb5a 100644 --- a/src/modules/bluetooth/meson.build +++ b/src/modules/bluetooth/meson.build @@ -3,6 +3,7 @@ libbluez5_util_sources = [ 'a2dp-codec-util.c', 'bluez5-util.c', 'bt-codec-cvsd.c', + 'bt-codec-msbc.c', ] libbluez5_util_headers = [