From 971397ef05c7ab5d5459b790780a846291f94925 Mon Sep 17 00:00:00 2001 From: anonymix007 <48598263+anonymix007@users.noreply.github.com> Date: Sun, 12 Nov 2023 16:40:00 +0300 Subject: [PATCH] Add support for LHDC V3 A2DP sink --- .github/spellcheck-wordlist.txt | 4 +- configure.ac | 9 + src/Makefile.am | 9 + src/a2dp-lhdc.c | 455 ++++++++++++++++++++++++++++++++ src/a2dp-lhdc.h | 30 +++ src/a2dp.c | 70 ++++- src/audio.c | 77 ++++++ src/audio.h | 4 + src/ba-transport.c | 8 + src/bluealsa-config.c | 10 + src/bluealsa-config.h | 4 + src/bluez.c | 8 + src/io.c | 4 + src/main.c | 39 +++ src/shared/a2dp-codecs.c | 6 +- src/shared/a2dp-codecs.h | 70 ++++- utils/a2dpconf.c | 8 +- 17 files changed, 795 insertions(+), 20 deletions(-) create mode 100644 src/a2dp-lhdc.c create mode 100644 src/a2dp-lhdc.h diff --git a/.github/spellcheck-wordlist.txt b/.github/spellcheck-wordlist.txt index 5380092c3..83f018fe9 100644 --- a/.github/spellcheck-wordlist.txt +++ b/.github/spellcheck-wordlist.txt @@ -2,6 +2,7 @@ AAC ABR ADDR +AOSP AVRCP BT CBR @@ -63,6 +64,7 @@ BlueZ Fraunhofer IWYU LDAC +LHDC MPD oFono PipeWire @@ -173,7 +175,7 @@ iter keyp LF LFE -LHDC +LHDCBT libasound libbsd libdbus diff --git a/configure.ac b/configure.ac index 200a3d132..07004e4f6 100644 --- a/configure.ac +++ b/configure.ac @@ -183,6 +183,15 @@ AM_COND_IF([ENABLE_LDAC], [ AC_DEFINE([ENABLE_LDAC], [1], [Define to 1 if LDAC is enabled.]) ]) +AC_ARG_ENABLE([lhdc], + [AS_HELP_STRING([--enable-lhdc], [enable LHDC support])]) +AM_CONDITIONAL([ENABLE_LHDC], [test "x$enable_lhdc" = "xyes"]) +AM_COND_IF([ENABLE_LHDC], [ + AC_DEFINE([ENABLE_LHDC], [1], [Define to 1 if LHDC is enabled.]) + PKG_CHECK_MODULES([LHDC_DEC], [ldhcBT-dec >= 4.0.2]) + PKG_CHECK_MODULES([LHDC_ENC], [lhdcBT-enc >= 4.0.6]) +]) + AC_ARG_ENABLE([mp3lame], [AS_HELP_STRING([--enable-mp3lame], [enable MP3 support])]) AM_CONDITIONAL([ENABLE_MP3LAME], [test "x$enable_mp3lame" = "xyes"]) diff --git a/src/Makefile.am b/src/Makefile.am index d3160170c..ef464813a 100644 --- a/src/Makefile.am +++ b/src/Makefile.am @@ -83,6 +83,11 @@ bluealsa_SOURCES += \ a2dp-ldac.c endif +if ENABLE_LHDC +bluealsa_SOURCES += \ + a2dp-lhdc.c +endif + if ENABLE_MPEG bluealsa_SOURCES += \ a2dp-mpeg.c @@ -114,6 +119,8 @@ AM_CFLAGS = \ @LDAC_ABR_CFLAGS@ \ @LDAC_DEC_CFLAGS@ \ @LDAC_ENC_CFLAGS@ \ + @LHDC_DEC_CFLAGS@ \ + @LHDC_ENC_CFLAGS@ \ @LIBBSD_CFLAGS@ \ @LIBUNWIND_CFLAGS@ \ @MPG123_CFLAGS@ \ @@ -131,6 +138,8 @@ LDADD = \ @LDAC_ABR_LIBS@ \ @LDAC_DEC_LIBS@ \ @LDAC_ENC_LIBS@ \ + @LHDC_DEC_LIBS@ \ + @LHDC_ENC_LIBS@ \ @LIBUNWIND_LIBS@ \ @MP3LAME_LIBS@ \ @MPG123_LIBS@ \ diff --git a/src/a2dp-lhdc.c b/src/a2dp-lhdc.c new file mode 100644 index 000000000..25d8760e3 --- /dev/null +++ b/src/a2dp-lhdc.c @@ -0,0 +1,455 @@ +/* + * BlueALSA - a2dp-lhdc.c + * Copyright (c) 2016-2023 Arkadiusz Bokowy + * Copyright (c) 2023 anonymix007 + * + * This file is a part of bluez-alsa. + * + * This project is licensed under the terms of the MIT license. + * + */ + +#include "a2dp-lhdc.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include "a2dp.h" +#include "ba-transport-pcm.h" +#include "bluealsa-config.h" +#include "io.h" +#include "rtp.h" +#include "utils.h" +#include "shared/a2dp-codecs.h" +#include "shared/defs.h" +#include "shared/ffb.h" +#include "shared/log.h" +#include "shared/rt.h" + +static const struct a2dp_channel_mode a2dp_lhdc_channels[] = { + { A2DP_CHM_STEREO, 2, LHDC_CHANNEL_MODE_STEREO }, +}; + +static const struct a2dp_sampling_freq a2dp_lhdc_samplings[] = { + { 44100, LHDC_SAMPLING_FREQ_44100 }, + { 48000, LHDC_SAMPLING_FREQ_48000 }, + { 96000, LHDC_SAMPLING_FREQ_96000 }, +}; + +struct a2dp_codec a2dp_lhdc_sink = { + .dir = A2DP_SINK, + .codec_id = A2DP_CODEC_VENDOR_LHDC_V3, + .capabilities.lhdc_v3 = { + .info = A2DP_SET_VENDOR_ID_CODEC_ID(LHDC_V3_VENDOR_ID, LHDC_V3_CODEC_ID), + .frequency = + LHDC_SAMPLING_FREQ_44100 | + LHDC_SAMPLING_FREQ_48000 | + LHDC_SAMPLING_FREQ_96000, + .bit_depth = + LHDC_BIT_DEPTH_16 | + LHDC_BIT_DEPTH_24, + .jas = 0, + .ar = 0, + .version = LHDC_VER3, + .max_bit_rate = LHDC_MAX_BIT_RATE_900K, + .low_latency = 0, + .llac = 1, + .ch_split_mode = LHDC_CH_SPLIT_MODE_NONE, + .meta = 0, + .min_bitrate = 0, + .larc = 0, + .lhdc_v4 = 1, + }, + .capabilities_size = sizeof(a2dp_lhdc_v3_t), + .channels[0] = a2dp_lhdc_channels, + .channels_size[0] = ARRAYSIZE(a2dp_lhdc_channels), + .samplings[0] = a2dp_lhdc_samplings, + .samplings_size[0] = ARRAYSIZE(a2dp_lhdc_samplings), +}; + +struct a2dp_codec a2dp_lhdc_source = { + .dir = A2DP_SOURCE, + .codec_id = A2DP_CODEC_VENDOR_LHDC_V3, + .capabilities.lhdc_v3 = { + .info = A2DP_SET_VENDOR_ID_CODEC_ID(LHDC_V3_VENDOR_ID, LHDC_V3_CODEC_ID), + .frequency = + LHDC_SAMPLING_FREQ_44100 | + LHDC_SAMPLING_FREQ_48000 | + LHDC_SAMPLING_FREQ_96000, + .bit_depth = + LHDC_BIT_DEPTH_16 | + LHDC_BIT_DEPTH_24, + .jas = 0, + .ar = 0, + .version = LHDC_VER3, + .max_bit_rate = LHDC_MAX_BIT_RATE_900K, + .low_latency = 0, + .llac = 0, // TODO: copy LLAC/V3/V4 logic from AOSP patches + .ch_split_mode = LHDC_CH_SPLIT_MODE_NONE, + .meta = 0, + .min_bitrate = 0, + .larc = 0, + .lhdc_v4 = 1, + }, + .capabilities_size = sizeof(a2dp_lhdc_v3_t), + .channels[0] = a2dp_lhdc_channels, + .channels_size[0] = ARRAYSIZE(a2dp_lhdc_channels), + .samplings[0] = a2dp_lhdc_samplings, + .samplings_size[0] = ARRAYSIZE(a2dp_lhdc_samplings), +}; + +void a2dp_lhdc_init(void) { +} + +void a2dp_lhdc_transport_init(struct ba_transport *t) { + + const struct a2dp_codec *codec = t->a2dp.codec; + + if (codec->dir == A2DP_SINK) { + t->a2dp.pcm.format = BA_TRANSPORT_PCM_FORMAT_S24_4LE; + } else { + if (t->a2dp.configuration.lhdc_v3.bit_depth == LHDC_BIT_DEPTH_16) { + t->a2dp.pcm.format = BA_TRANSPORT_PCM_FORMAT_S16_2LE; + } else { + t->a2dp.pcm.format = BA_TRANSPORT_PCM_FORMAT_S24_3LE; + } + } + + + + t->a2dp.pcm.channels = a2dp_codec_lookup_channels(codec, + LHDC_CHANNEL_MODE_STEREO, false); + t->a2dp.pcm.sampling = a2dp_codec_lookup_frequency(codec, + t->a2dp.configuration.lhdc_v3.frequency, false); + +} + +static LHDC_VERSION_SETUP get_version(const a2dp_lhdc_v3_t *configuration) { + if (configuration->llac) { + return LLAC; + } else if (configuration->lhdc_v4) { + return LHDC_V4; + } else { + return LHDC_V3; + } +} + +static int get_encoder_interval(const a2dp_lhdc_v3_t *configuration) { + if (configuration->low_latency) { + return 10; + } else { + return 20; + } +} + +static int get_bit_depth(const a2dp_lhdc_v3_t *configuration) { + if (configuration->bit_depth == LHDC_BIT_DEPTH_16) { + return 16; + } else { + return 24; + } +} + +static LHDCBT_QUALITY_T get_max_bitrate(const a2dp_lhdc_v3_t *configuration) { + if (configuration->max_bit_rate == LHDC_MAX_BIT_RATE_400K) { + return LHDCBT_QUALITY_LOW; + } else if (configuration->max_bit_rate == LHDC_MAX_BIT_RATE_500K) { + return LHDCBT_QUALITY_MID; + } else { + return LHDCBT_QUALITY_HIGH; + } +} + +void *a2dp_lhdc_enc_thread(struct ba_transport_pcm *t_pcm) { + + pthread_setcancelstate(PTHREAD_CANCEL_DISABLE, NULL); + pthread_cleanup_push(PTHREAD_CLEANUP(ba_transport_pcm_thread_cleanup), t_pcm); + + struct ba_transport *t = t_pcm->t; + struct ba_transport_thread *th = t_pcm->th; + struct io_poll io = { .timeout = -1 }; + + const a2dp_lhdc_v3_t *configuration = &t->a2dp.configuration.lhdc_v3; + + HANDLE_LHDC_BT handle; + if ((handle = lhdcBT_get_handle(get_version(configuration))) == NULL) { + error("Couldn't get LHDC handle: %s", strerror(errno)); + goto fail_open_lhdc; + } + + pthread_cleanup_push(PTHREAD_CLEANUP(lhdcBT_free_handle), handle); + + const size_t sample_size = BA_TRANSPORT_PCM_FORMAT_BYTES(t_pcm->format); + const unsigned int channels = t_pcm->channels; + const unsigned int samplerate = t_pcm->sampling; + const unsigned int bitdepth = get_bit_depth(configuration); + + lhdcBT_set_hasMinBitrateLimit(handle, configuration->min_bitrate); + lhdcBT_set_max_bitrate(handle, get_max_bitrate(configuration)); + + if (lhdcBT_init_encoder(handle, samplerate, bitdepth, config.lhdc_eqmid, + configuration->ch_split_mode > LHDC_CH_SPLIT_MODE_NONE, 0, t->mtu_write, + get_encoder_interval(configuration)) == -1) { + error("Couldn't initialize LHDC encoder"); + goto fail_init; + } + + const size_t lhdc_pcm_samples = lhdcBT_get_block_Size(handle); + + ffb_t bt = { 0 }; + ffb_t pcm = { 0 }; + pthread_cleanup_push(PTHREAD_CLEANUP(ffb_free), &bt); + pthread_cleanup_push(PTHREAD_CLEANUP(ffb_free), &pcm); + + if (ffb_init_int32_t(&pcm, lhdc_pcm_samples) == -1 || + ffb_init_uint8_t(&bt, t->mtu_write) == -1) { + error("Couldn't create data buffers: %s", strerror(errno)); + goto fail_ffb; + } + + rtp_header_t *rtp_header; + struct { + uint8_t seq_num; + uint8_t latency:2; + uint8_t frames:6; + } *lhdc_media_header; + /* initialize RTP headers and get anchor for payload */ + uint8_t *rtp_payload = rtp_a2dp_init(bt.data, &rtp_header, + (void **)&lhdc_media_header, sizeof(*lhdc_media_header)); + + struct rtp_state rtp = { .synced = false }; + /* RTP clock frequency equal to audio samplerate */ + rtp_state_init(&rtp, samplerate, samplerate); + + debug_transport_pcm_thread_loop(t_pcm, "START"); + + uint8_t seq_num = 0; + + for (ba_transport_thread_state_set_running(th);;) { + + ssize_t samples = ffb_len_in(&pcm); + switch (samples = io_poll_and_read_pcm(&io, t_pcm, pcm.tail, samples)) { + case -1: + if (errno == ESTALE) { + ffb_rewind(&pcm); + continue; + } + error("PCM poll and read error: %s", strerror(errno)); + /* fall-through */ + case 0: + ba_transport_stop_if_no_clients(t); + continue; + } + + ffb_seek(&pcm, samples); + samples = ffb_len_out(&pcm); + + uint8_t *input = pcm.data; + size_t input_len = samples; + + /* encode and transfer obtained data */ + while (input_len >= lhdc_pcm_samples) { + + /* anchor for RTP payload */ + bt.tail = rtp_payload; + + uint32_t encoded; + uint32_t frames; + + if (lhdcBT_encodeV3(handle, input, bt.tail, &encoded, &frames) < 0) { + error("LHDC encoding error"); + break; + } + + input += lhdc_pcm_samples * channels * sample_size; + input_len -= lhdc_pcm_samples; + ffb_seek(&bt, encoded); + + if (encoded > 0) { + + lhdc_media_header->seq_num = seq_num++; + lhdc_media_header->latency = 0; + lhdc_media_header->frames = frames; + + rtp_state_new_frame(&rtp, rtp_header); + + /* Try to get the number of bytes queued in the + * socket output buffer. */ + int queued_bytes = 0; + if (ioctl(t->bt_fd, TIOCOUTQ, &queued_bytes) != -1) + queued_bytes = abs(t->a2dp.bt_fd_coutq_init - queued_bytes); + + errno = 0; + + ssize_t len = ffb_blen_out(&bt); + if ((len = io_bt_write(th, bt.data, len)) <= 0) { + if (len == -1) + error("BT write error: %s", strerror(errno)); + goto fail; + } + + if (errno == EAGAIN) + /* The io_bt_write() call was blocking due to not enough + * space in the BT socket. Set the queued_bytes to some + * arbitrary big value. */ + queued_bytes = 1024 * 16; + + if (config.lhdc_eqmid == LHDCBT_QUALITY_AUTO) + lhdcBT_adjust_bitrate(handle, queued_bytes / t->mtu_write); + } + + unsigned int pcm_frames = lhdc_pcm_samples / channels; + /* keep data transfer at a constant bit rate */ + asrsync_sync(&io.asrs, pcm_frames); + /* move forward RTP timestamp clock */ + rtp_state_update(&rtp, pcm_frames); + + /* update busy delay (encoding overhead) */ + t_pcm->delay = asrsync_get_busy_usec(&io.asrs) / 100; + + } + + /* If the input buffer was not consumed (due to codesize limit), we + * have to append new data to the existing one. Since we do not use + * ring buffer, we will simply move unprocessed data to the front + * of our linear buffer. */ + ffb_shift(&pcm, samples - input_len); + + } + +fail: + debug_transport_pcm_thread_loop(t_pcm, "EXIT"); +fail_ffb: + pthread_cleanup_pop(1); + pthread_cleanup_pop(1); +fail_init: + pthread_cleanup_pop(1); +fail_open_lhdc: + pthread_cleanup_pop(1); + return NULL; +} + +static const int versions[5] = { + [LHDC_V2] = VERSION_2, + [LHDC_V3] = VERSION_3, + [LHDC_V4] = VERSION_4, + [LLAC] = VERSION_LLAC, +}; + +void *a2dp_lhdc_dec_thread(struct ba_transport_pcm *t_pcm) { + + pthread_setcancelstate(PTHREAD_CANCEL_DISABLE, NULL); + pthread_cleanup_push(PTHREAD_CLEANUP(ba_transport_pcm_thread_cleanup), t_pcm); + + struct ba_transport *t = t_pcm->t; + struct ba_transport_thread *th = t_pcm->th; + struct io_poll io = { .timeout = -1 }; + + const a2dp_lhdc_v3_t *configuration = &t->a2dp.configuration.lhdc_v3; + const size_t sample_size = BA_TRANSPORT_PCM_FORMAT_BYTES(t_pcm->format); + const unsigned int channels = t_pcm->channels; + const unsigned int samplerate = t_pcm->sampling; + const unsigned int bitdepth = get_bit_depth(configuration); + + tLHDCV3_DEC_CONFIG dec_config = { + .version = versions[get_version(configuration)], + .sample_rate = samplerate, + .bits_depth = bitdepth, + }; + + if (lhdcBT_dec_init_decoder(&dec_config) < 0) { + error("Couldn't initialise LHDC decoder: %s", strerror(errno)); + goto fail_open; + } + + pthread_cleanup_push(PTHREAD_CLEANUP(lhdcBT_dec_deinit_decoder), NULL); + + ffb_t bt = { 0 }; + ffb_t pcm = { 0 }; + pthread_cleanup_push(PTHREAD_CLEANUP(ffb_free), &bt); + pthread_cleanup_push(PTHREAD_CLEANUP(ffb_free), &pcm); + + if (ffb_init_int32_t(&pcm, 16 * 256 * channels) == -1 || + ffb_init_uint8_t(&bt, t->mtu_read) == -1) { + error("Couldn't create data buffers: %s", strerror(errno)); + goto fail_ffb; + } + + struct rtp_state rtp = { .synced = false }; + /* RTP clock frequency equal to audio samplerate */ + rtp_state_init(&rtp, samplerate, samplerate); + + debug_transport_pcm_thread_loop(t_pcm, "START"); + for (ba_transport_thread_state_set_running(th);;) { + + ssize_t len = ffb_blen_in(&bt); + if ((len = io_poll_and_read_bt(&io, th, bt.data, len)) <= 0) { + if (len == -1) + error("BT poll and read error: %s", strerror(errno)); + goto fail; + } + + const rtp_header_t *rtp_header = bt.data; + const void *lhdc_media_header; + if ((lhdc_media_header = rtp_a2dp_get_payload(rtp_header)) == NULL) + continue; + + int missing_rtp_frames = 0; + rtp_state_sync_stream(&rtp, rtp_header, &missing_rtp_frames, NULL); + + if (!ba_transport_pcm_is_active(t_pcm)) { + rtp.synced = false; + continue; + } + + const uint8_t *rtp_payload = (uint8_t *) lhdc_media_header; + size_t rtp_payload_len = len - (rtp_payload - (uint8_t *)bt.data); + + uint32_t decoded = 16 * 256 * sizeof(int32_t) * channels; + + lhdcBT_dec_decode(rtp_payload, rtp_payload_len, pcm.data, &decoded, 24); + + const size_t samples = decoded / sample_size; + io_pcm_scale(t_pcm, pcm.data, samples); + if (io_pcm_write(t_pcm, pcm.data, samples) == -1) + error("FIFO write error: %s", strerror(errno)); + + /* update local state with decoded PCM frames */ + rtp_state_update(&rtp, samples / channels); + + } + +fail: + debug_transport_pcm_thread_loop(t_pcm, "EXIT"); +fail_ffb: + pthread_cleanup_pop(1); + pthread_cleanup_pop(1); + pthread_cleanup_pop(1); +fail_open: + pthread_cleanup_pop(1); + return NULL; +} + +int a2dp_lhdc_transport_start(struct ba_transport *t) { + + if (t->profile & BA_TRANSPORT_PROFILE_A2DP_SOURCE) + return ba_transport_pcm_start(&t->a2dp.pcm, a2dp_lhdc_enc_thread, "ba-a2dp-lhdc", true); + + if (t->profile & BA_TRANSPORT_PROFILE_A2DP_SINK) + return ba_transport_pcm_start(&t->a2dp.pcm, a2dp_lhdc_dec_thread, "ba-a2dp-lhdc", true); + + g_assert_not_reached(); + return -1; +} diff --git a/src/a2dp-lhdc.h b/src/a2dp-lhdc.h new file mode 100644 index 000000000..bfd5eb6d5 --- /dev/null +++ b/src/a2dp-lhdc.h @@ -0,0 +1,30 @@ +/* + * BlueALSA - a2dp-ldac.h + * Copyright (c) 2016-2021 Arkadiusz Bokowy + * Copyright (c) 2023 anonymix007 + * + * This file is a part of bluez-alsa. + * + * This project is licensed under the terms of the MIT license. + * + */ + +#pragma once +#ifndef BLUEALSA_A2DPLHDC_H_ +#define BLUEALSA_A2DPLHDC_H_ + +#if HAVE_CONFIG_H +# include +#endif + +#include "a2dp.h" +#include "ba-transport.h" + +extern struct a2dp_codec a2dp_lhdc_sink; +extern struct a2dp_codec a2dp_lhdc_source; + +void a2dp_lhdc_init(void); +void a2dp_lhdc_transport_init(struct ba_transport *t); +int a2dp_lhdc_transport_start(struct ba_transport *t); + +#endif diff --git a/src/a2dp.c b/src/a2dp.c index b1baa6c7d..546311704 100644 --- a/src/a2dp.c +++ b/src/a2dp.c @@ -40,6 +40,9 @@ #if ENABLE_LDAC # include "a2dp-ldac.h" #endif +#if ENABLE_LHDC +# include "a2dp-lhdc.h" +#endif #if ENABLE_MPEG # include "a2dp-mpeg.h" #endif @@ -56,6 +59,10 @@ struct a2dp_codec * const a2dp_codecs[] = { &a2dp_lc3plus_source, &a2dp_lc3plus_sink, #endif +#if ENABLE_LHDC + &a2dp_lhdc_source, + &a2dp_lhdc_sink, +#endif #if ENABLE_LDAC &a2dp_ldac_source, # if HAVE_LDAC_DECODE @@ -119,6 +126,9 @@ int a2dp_codecs_init(void) { #endif #if ENABLE_LDAC a2dp_ldac_init(); +#endif +#if ENABLE_LHDC + a2dp_lhdc_init(); #endif return 0; } @@ -276,12 +286,16 @@ uint16_t a2dp_get_vendor_codec_id(const void *capabilities, size_t size) { } break; case BT_COMPID_SAVITECH: switch (codec_id) { - case LHDC_CODEC_ID: - return A2DP_CODEC_VENDOR_LHDC; - case LHDC_LL_CODEC_ID: - return A2DP_CODEC_VENDOR_LHDC_LL; case LHDC_V1_CODEC_ID: return A2DP_CODEC_VENDOR_LHDC_V1; + case LHDC_V2_CODEC_ID: + return A2DP_CODEC_VENDOR_LHDC_V2; + case LHDC_V3_CODEC_ID: + return A2DP_CODEC_VENDOR_LHDC_V3; + case LHDC_V5_CODEC_ID: + return A2DP_CODEC_VENDOR_LHDC_V5; + case LHDC_LL_CODEC_ID: + return A2DP_CODEC_VENDOR_LHDC_LL; } break; case BT_COMPID_FRAUNHOFER_IIS: switch (codec_id) { @@ -489,6 +503,15 @@ uint32_t a2dp_check_configuration( } #endif +#if ENABLE_LHDC + case A2DP_CODEC_VENDOR_LHDC_V3: { + const a2dp_lhdc_v3_t *cap = configuration; + cap_chm = LHDC_CHANNEL_MODE_STEREO; + cap_freq = cap->frequency; + break; + } +#endif + default: g_assert_not_reached(); } @@ -574,6 +597,10 @@ int a2dp_filter_capabilities( #if ENABLE_LDAC case A2DP_CODEC_VENDOR_LDAC: break; +#endif +#if ENABLE_LHDC + case A2DP_CODEC_VENDOR_LHDC_V3: + break; #endif default: g_assert_not_reached(); @@ -922,6 +949,32 @@ int a2dp_select_configuration( } #endif +#if ENABLE_LHDC + case A2DP_CODEC_VENDOR_LHDC_V3: { + warn("LHDC: LLAC/V3/V4 switch logic is not implemented"); + a2dp_lhdc_v3_t *cap = capabilities; + + const unsigned int cap_bps = cap->bit_depth; + + if (cap_bps & LHDC_BIT_DEPTH_24) { + cap->bit_depth = LHDC_BIT_DEPTH_24; + } else if (cap_bps & LHDC_BIT_DEPTH_16) { + cap->bit_depth = LHDC_BIT_DEPTH_16; + } else { + error("LHDC: No supported bit depths: %#x", tmp.lhdc_v3.bit_depth); + goto fail; + } + + const unsigned int cap_freq = cap->frequency; + if ((cap->frequency = a2dp_codec_select_sampling_freq(codec, cap_freq, false)) == 0) { + error("LHDC: No supported sampling frequencies: %#x", tmp.lhdc_v3.frequency); + goto fail; + } + + break; + } +#endif + default: g_assert_not_reached(); } @@ -974,6 +1027,11 @@ void a2dp_transport_init( case A2DP_CODEC_VENDOR_LDAC: a2dp_ldac_transport_init(t); break; +#endif +#if ENABLE_LHDC + case A2DP_CODEC_VENDOR_LHDC_V3: + a2dp_lhdc_transport_init(t); + break; #endif default: debug("Unsupported A2DP codec: %#x", codec_id); @@ -1014,6 +1072,10 @@ int a2dp_transport_start( #if ENABLE_LDAC case A2DP_CODEC_VENDOR_LDAC: return a2dp_ldac_transport_start(t); +#endif +#if ENABLE_LHDC + case A2DP_CODEC_VENDOR_LHDC_V3: + return a2dp_lhdc_transport_start(t); #endif default: debug("Unsupported A2DP codec: %#x", codec_id); diff --git a/src/audio.c b/src/audio.c index a9d238ec3..177c7e8b9 100644 --- a/src/audio.c +++ b/src/audio.c @@ -120,6 +120,52 @@ void audio_scale_s16_2le(int16_t *buffer, size_t frames, } } +static int le24toh(uint8_t data[3]) { + return (data[0] << 8 | (data[1] << 16) | (data[2] << 24)) >> 8; +} +static void htole24(int in, uint8_t out[3]) { + out[0] = in >> 0*8; + out[1] = in >> 1*8; + out[2] = in >> 2*8; +} +/** + * Scale S24_3LE PCM signal. + * + * Neutral value for scaling factor is 1.0. It is possible to increase + * signal gain by using scaling factor values greater than 1, however, + * clipping will most certainly occur. + * + * @param buffer Address to the buffer where the PCM signal is stored. + * @param frames The number of PCM frames in the buffer. + * @param channels The number of channels in the buffer. + * @param ch1 The scaling factor for 1st channel. + * @param ch1 The scaling factor for 2nd channel. */ +void audio_scale_s24_3le(uint8_t *buffer, size_t frames, + unsigned int channels, double ch1, double ch2) { + audio_silence_s24_3le(buffer, frames, channels, ch1 == 0, ch2 == 0); + switch (channels) { + case 1: + if (ch1 != 0 && ch1 != 1) + while (frames--) { + int s1 = le24toh(&buffer[frames*3]) * ch1; + htole24(s1, &buffer[3*frames]); + } + break; + case 2: + if ((ch1 != 0 && ch1 != 1) || (ch2 != 0 && ch2 != 1)) + while (frames--) { + int s1 = le24toh(&buffer[6 * frames]) * ch1; + htole24(s1, &buffer[6*frames]); + int s2 = le24toh(&buffer[6 * frames + 3]) * ch2; + htole24(s1, &buffer[6*frames + 3]); + } + break; + default: + g_assert_not_reached(); + } +} + + /** * Scale S32_4LE PCM signal. */ void audio_scale_s32_4le(int32_t *buffer, size_t frames, @@ -168,6 +214,37 @@ void audio_silence_s16_2le(int16_t *buffer, size_t frames, } } +/** + * Silence S24_3LE PCM signal. */ +void audio_silence_s24_3le(uint8_t *buffer, size_t frames, + unsigned int channels, bool ch1, bool ch2) { + switch (channels) { + case 1: + if (ch1) + memset(buffer, 0, frames * 3); + break; + case 2: + if (ch1) { + while (frames--) { + *buffer++ = 0; + *buffer++ = 0; + *buffer++ = 0; + buffer += 3; + } + } else if (ch2) { + while (frames--) { + buffer += 3; + *buffer++ = 0; + *buffer++ = 0; + *buffer++ = 0; + } + } + break; + default: + g_assert_not_reached(); + } +} + /** * Silence S32_4LE PCM signal. */ void audio_silence_s32_4le(int32_t *buffer, size_t frames, diff --git a/src/audio.h b/src/audio.h index e1d7b4cbb..ae93e44a3 100644 --- a/src/audio.h +++ b/src/audio.h @@ -37,12 +37,16 @@ void audio_deinterleave_s32_4le(const int32_t *src, size_t frames, void audio_scale_s16_2le(int16_t *buffer, size_t frames, unsigned int channels, double ch1, double ch2); +void audio_scale_s24_3le(uint8_t *buffer, size_t frames, + unsigned int channels, double ch1, double ch2); void audio_scale_s32_4le(int32_t *buffer, size_t frames, unsigned int channels, double ch1, double ch2); #define audio_scale_s24_4le audio_scale_s32_4le void audio_silence_s16_2le(int16_t *buffer, size_t frames, unsigned int channels, bool ch1, bool ch2); +void audio_silence_s24_3le(uint8_t *buffer, size_t frames, + unsigned int channels, bool ch1, bool ch2); void audio_silence_s32_4le(int32_t *buffer, size_t frames, unsigned int channels, bool ch1, bool ch2); #define audio_silence_s24_4le audio_silence_s32_4le diff --git a/src/ba-transport.c b/src/ba-transport.c index 19f498798..8052a96ec 100644 --- a/src/ba-transport.c +++ b/src/ba-transport.c @@ -901,6 +901,10 @@ const char *ba_transport_debug_name( #if ENABLE_LDAC case A2DP_CODEC_VENDOR_LDAC: return "A2DP Source (LDAC)"; +#endif +#if ENABLE_LHDC + case A2DP_CODEC_VENDOR_LHDC_V3: + return "A2DP Sink (LHDC V3)"; #endif } break; case BA_TRANSPORT_PROFILE_A2DP_SINK: @@ -934,6 +938,10 @@ const char *ba_transport_debug_name( #if ENABLE_LDAC case A2DP_CODEC_VENDOR_LDAC: return "A2DP Sink (LDAC)"; +#endif +#if ENABLE_LHDC + case A2DP_CODEC_VENDOR_LHDC_V3: + return "A2DP Sink (LHDC V3)"; #endif } break; case BA_TRANSPORT_PROFILE_HFP_HF: diff --git a/src/bluealsa-config.c b/src/bluealsa-config.c index 30dfa68e0..9fc0ba227 100644 --- a/src/bluealsa-config.c +++ b/src/bluealsa-config.c @@ -18,6 +18,11 @@ # include #endif +#if ENABLE_LHDC +# include +# include +#endif + #include "codec-sbc.h" #include "hfp.h" @@ -133,6 +138,11 @@ struct ba_config config = { .ldac_eqmid = LDACBT_EQMID_SQ, #endif +#if ENABLE_LHDC + /* Use ABR as a reasonable default. */ + .lhdc_eqmid = LHDCBT_QUALITY_AUTO, +#endif + }; int bluealsa_config_init(void) { diff --git a/src/bluealsa-config.h b/src/bluealsa-config.h index 60b569b76..afef6e215 100644 --- a/src/bluealsa-config.h +++ b/src/bluealsa-config.h @@ -156,6 +156,10 @@ struct ba_config { uint8_t ldac_eqmid; #endif +#if ENABLE_LHDC + uint8_t lhdc_eqmid; + // TODO: LLAC/V3/V4, bit depth, sample frequency, LLAC bitrate +#endif }; /* Global BlueALSA configuration. */ diff --git a/src/bluez.c b/src/bluez.c index 94f107c28..eada2e2d1 100644 --- a/src/bluez.c +++ b/src/bluez.c @@ -316,6 +316,10 @@ static const char *bluez_get_media_endpoint_object_path( #if ENABLE_LDAC case A2DP_CODEC_VENDOR_LDAC: return "/A2DP/LDAC/source"; +#endif +#if ENABLE_LHDC + case A2DP_CODEC_VENDOR_LHDC_V3: + return "/A2DP/LHDC_V3/source"; #endif default: error("Unsupported A2DP codec: %#x", codec_id); @@ -352,6 +356,10 @@ static const char *bluez_get_media_endpoint_object_path( #if ENABLE_LDAC case A2DP_CODEC_VENDOR_LDAC: return "/A2DP/LDAC/sink"; +#endif +#if ENABLE_LHDC + case A2DP_CODEC_VENDOR_LHDC_V3: + return "/A2DP/LHDC_V3/sink"; #endif default: error("Unsupported A2DP codec: %#x", codec_id); diff --git a/src/io.c b/src/io.c index 6e5c3a6c9..765237ce3 100644 --- a/src/io.c +++ b/src/io.c @@ -144,6 +144,10 @@ void io_pcm_scale( audio_scale_s16_2le(buffer, samples / channels, channels, pcm_volume_ch_scales[0], pcm_volume_ch_scales[1]); break; + case BA_TRANSPORT_PCM_FORMAT_S24_3LE: + audio_scale_s24_3le(buffer, samples / channels, channels, + pcm_volume_ch_scales[0], pcm_volume_ch_scales[1]); + break; case BA_TRANSPORT_PCM_FORMAT_S24_4LE: case BA_TRANSPORT_PCM_FORMAT_S32_4LE: audio_scale_s32_4le(buffer, samples / channels, channels, diff --git a/src/main.c b/src/main.c index 6da13f13f..9328086c7 100644 --- a/src/main.c +++ b/src/main.c @@ -31,6 +31,11 @@ # include #endif +#if ENABLE_LHDC +# include +# include +#endif + #include "a2dp.h" #include "a2dp-sbc.h" #include "audio.h" @@ -169,6 +174,10 @@ int main(int argc, char **argv) { { "aac-true-bps", no_argument, NULL, 18 }, { "aac-vbr", no_argument, NULL, 19 }, #endif +#if ENABLE_LHDC + // TODO: LLAC/V3/V4, bit depth, sample frequency, LLAC bitrate + { "lhdc-quality", required_argument, NULL, 22 }, +#endif #if ENABLE_LC3PLUS { "lc3plus-bitrate", required_argument, NULL, 20 }, #endif @@ -226,6 +235,9 @@ int main(int argc, char **argv) { " --ldac-abr\t\t\tenable LDAC adaptive bit rate\n" " --ldac-quality=MODE\t\tset LDAC encoder quality mode\n" #endif +#if ENABLE_LHDC + " --lhdc-quality=MODE\t\tset LHDC encoder quality mode\n" +#endif #if ENABLE_MP3LAME " --mp3-algorithm=TYPE\t\tselect LAME encoder algorithm type\n" " --mp3-vbr-quality=MODE\tset LAME encoder VBR quality mode\n" @@ -481,6 +493,33 @@ int main(int argc, char **argv) { } #endif +#if ENABLE_LHDC + case 22 /* --lhdc-quality=MODE */ : { + static const nv_entry_t values[] = { + { "low0", .v.ui = LHDCBT_QUALITY_LOW0 }, + { "low1", .v.ui = LHDCBT_QUALITY_LOW1 }, + { "low2", .v.ui = LHDCBT_QUALITY_LOW2 }, + { "low3", .v.ui = LHDCBT_QUALITY_LOW3 }, + { "low4", .v.ui = LHDCBT_QUALITY_LOW4 }, + { "low", .v.ui = LHDCBT_QUALITY_LOW }, + { "mid", .v.ui = LHDCBT_QUALITY_MID }, + { "high", .v.ui = LHDCBT_QUALITY_HIGH }, + { "auto", .v.ui = LHDCBT_QUALITY_AUTO }, + { 0 }, + }; + + const nv_entry_t *entry; + if ((entry = nv_find(values, optarg)) == NULL) { + error("Invalid LHDC encoder quality mode {%s}: %s", + nv_join_names(values), optarg); + return EXIT_FAILURE; + } + + config.lhdc_eqmid = entry->v.ui; + break; + } +#endif + #if ENABLE_MP3LAME case 12 /* --mp3-algorithm=TYPE */ : { diff --git a/src/shared/a2dp-codecs.c b/src/shared/a2dp-codecs.c index f1a73ce87..4b68c1f6c 100644 --- a/src/shared/a2dp-codecs.c +++ b/src/shared/a2dp-codecs.c @@ -32,9 +32,11 @@ static const struct { { A2DP_CODEC_VENDOR_FASTSTREAM, { "FastStream", "FS" } }, { A2DP_CODEC_VENDOR_LC3PLUS, { "LC3plus" } }, { A2DP_CODEC_VENDOR_LDAC, { "LDAC" } }, - { A2DP_CODEC_VENDOR_LHDC, { "LHDC" } }, - { A2DP_CODEC_VENDOR_LHDC_LL, { "LHDC-LL", "LLAC" } }, { A2DP_CODEC_VENDOR_LHDC_V1, { "LHDC-v1" } }, + { A2DP_CODEC_VENDOR_LHDC_V2, { "LHDC-V2" } }, + { A2DP_CODEC_VENDOR_LHDC_V3, { "LHDC-V3", "LHDC-V4", "LLAC" } }, + { A2DP_CODEC_VENDOR_LHDC_V5, { "LHDC-V5" } }, + { A2DP_CODEC_VENDOR_LHDC_LL, { "LHDC-LL"} }, { A2DP_CODEC_VENDOR_SAMSUNG_HD, { "samsung-HD" } }, { A2DP_CODEC_VENDOR_SAMSUNG_SC, { "samsung-SC" } }, }; diff --git a/src/shared/a2dp-codecs.h b/src/shared/a2dp-codecs.h index 0c8baccf0..5ace8b84b 100644 --- a/src/shared/a2dp-codecs.h +++ b/src/shared/a2dp-codecs.h @@ -52,7 +52,9 @@ #define A2DP_CODEC_VENDOR_FASTSTREAM 0xA1FF #define A2DP_CODEC_VENDOR_LC3PLUS 0xC3FF #define A2DP_CODEC_VENDOR_LDAC 0x2DFF -#define A2DP_CODEC_VENDOR_LHDC 0x4CFF +#define A2DP_CODEC_VENDOR_LHDC_V2 0x2CFF +#define A2DP_CODEC_VENDOR_LHDC_V3 0x3CFF +#define A2DP_CODEC_VENDOR_LHDC_V5 0x5CFF #define A2DP_CODEC_VENDOR_LHDC_LL 0x44FF #define A2DP_CODEC_VENDOR_LHDC_V1 0x48FF #define A2DP_CODEC_VENDOR_SAMSUNG_HD 0x52FF @@ -338,8 +340,14 @@ #define LDAC_CHANNEL_MODE_DUAL 0x02 #define LDAC_CHANNEL_MODE_STEREO 0x01 -#define LHDC_VENDOR_ID BT_COMPID_SAVITECH -#define LHDC_CODEC_ID 0x4C32 +#define LHDC_V2_VENDOR_ID BT_COMPID_SAVITECH +#define LHDC_V2_CODEC_ID 0x4C32 + +#define LHDC_V3_VENDOR_ID BT_COMPID_SAVITECH +#define LHDC_V3_CODEC_ID 0x4C33 + +#define LHDC_V5_VENDOR_ID BT_COMPID_SAVITECH +#define LHDC_V5_CODEC_ID 0x4C35 #define LHDC_LL_VENDOR_ID BT_COMPID_SAVITECH #define LHDC_LL_CODEC_ID 0x4C4C @@ -347,9 +355,13 @@ #define LHDC_V1_VENDOR_ID BT_COMPID_SAVITECH #define LHDC_V1_CODEC_ID 0x484C +#define LHDC_CHANNEL_MODE_STEREO 0x03 + #define LHDC_BIT_DEPTH_16 0x02 #define LHDC_BIT_DEPTH_24 0x01 +#define LHDC_VER3 0x01 + #define LHDC_SAMPLING_FREQ_44100 0x08 #define LHDC_SAMPLING_FREQ_48000 0x04 #define LHDC_SAMPLING_FREQ_88200 0x02 @@ -489,6 +501,14 @@ typedef struct { uint8_t rfa2:5; } __attribute__ ((packed)) a2dp_ldac_t; +typedef struct { + a2dp_vendor_codec_t info; + uint8_t frequency:4; + uint8_t bit_depth:2; + uint8_t ch_separation:1; + uint8_t rfa:1; +} __attribute__ ((packed)) a2dp_lhdc_v1_t; + typedef struct { a2dp_vendor_codec_t info; uint8_t frequency:4; @@ -499,15 +519,45 @@ typedef struct { uint8_t low_latency:1; uint8_t ch_split_mode:4; uint8_t rfa2:4; -} __attribute__ ((packed)) a2dp_lhdc_t; +} __attribute__ ((packed)) a2dp_lhdc_v2_t; typedef struct { a2dp_vendor_codec_t info; uint8_t frequency:4; uint8_t bit_depth:2; - uint8_t ch_separation:1; - uint8_t rfa:1; -} __attribute__ ((packed)) a2dp_lhdc_v1_t; + uint8_t jas:1; + uint8_t ar:1; + uint8_t version:4; + uint8_t max_bit_rate:2; + uint8_t low_latency:1; + uint8_t llac:1; + uint8_t ch_split_mode:4; + uint8_t meta:1; + uint8_t min_bitrate:1; + uint8_t larc:1; + uint8_t lhdc_v4:1; +} __attribute__ ((packed)) a2dp_lhdc_v3_t; + +typedef struct { + a2dp_vendor_codec_t info; + uint8_t frequency:5; + uint8_t rfa1:3; + uint8_t bit_depth:3; + uint8_t rfa2:1; + uint8_t max_bit_rate:2; + uint8_t min_bit_rate:2; + uint8_t version:4; + uint8_t frame_len_5ms:1; + uint8_t rfa3:3; + uint8_t ar:1; + uint8_t jas:1; + uint8_t meta:1; + uint8_t rfa4:3; + uint8_t low_latency:1; + uint8_t reserved:1; // lossless? + uint8_t ar_on:1; + uint8_t rfa5:7; +} __attribute__ ((packed)) a2dp_lhdc_v5_t; #elif defined(__BYTE_ORDER) && defined(__BIG_ENDIAN) && \ __BYTE_ORDER == __BIG_ENDIAN @@ -613,7 +663,7 @@ typedef struct { uint8_t version:4; uint8_t rfa2:4; uint8_t ch_split_mode:4; -} __attribute__ ((packed)) a2dp_lhdc_t; +} __attribute__ ((packed)) a2dp_lhdc_v2_t; typedef struct { a2dp_vendor_codec_t info; @@ -652,8 +702,10 @@ typedef union { a2dp_aptx_hd_t aptx_hd; a2dp_lc3plus_t lc3plus; a2dp_ldac_t ldac; - a2dp_lhdc_t lhdc; a2dp_lhdc_v1_t lhdc_v1; + a2dp_lhdc_v2_t lhdc_v2; + a2dp_lhdc_v3_t lhdc_v3; + a2dp_lhdc_v5_t lhdc_v5; } a2dp_t; uint16_t a2dp_codecs_codec_id_from_string(const char *alias); diff --git a/utils/a2dpconf.c b/utils/a2dpconf.c index d059d0b5d..513507693 100644 --- a/utils/a2dpconf.c +++ b/utils/a2dpconf.c @@ -371,7 +371,7 @@ static void dump_ldac(const void *blob, size_t size) { ldac->channel_mode & LDAC_CHANNEL_MODE_MONO ? " Mono" : ""); } -static int lhdc_get_max_bit_rate(const a2dp_lhdc_t *lhdc) { +static int lhdc_get_max_bit_rate(const a2dp_lhdc_v2_t *lhdc) { if (lhdc->max_bit_rate == LHDC_MAX_BIT_RATE_400K) return 400; if (lhdc->max_bit_rate == LHDC_MAX_BIT_RATE_500K) @@ -381,8 +381,8 @@ static int lhdc_get_max_bit_rate(const a2dp_lhdc_t *lhdc) { return -1; } -static void dump_lhdc(const void *blob, size_t size) { - const a2dp_lhdc_t *lhdc = blob; +static void dump_lhdc_v2(const void *blob, size_t size) { + const a2dp_lhdc_v2_t *lhdc = blob; if (check_blob_size(sizeof(*lhdc), size) == -1) return; printf("LHDC {\n", bintohex(blob, size)); @@ -450,7 +450,7 @@ static struct { { A2DP_CODEC_VENDOR_FASTSTREAM, sizeof(a2dp_faststream_t), dump_faststream }, { A2DP_CODEC_VENDOR_LC3PLUS, sizeof(a2dp_lc3plus_t), dump_lc3plus }, { A2DP_CODEC_VENDOR_LDAC, sizeof(a2dp_ldac_t), dump_ldac }, - { A2DP_CODEC_VENDOR_LHDC, sizeof(a2dp_lhdc_t), dump_lhdc }, + { A2DP_CODEC_VENDOR_LHDC_V2, sizeof(a2dp_lhdc_v2_t), dump_lhdc_v2 }, { A2DP_CODEC_VENDOR_LHDC_LL, -1, dump_vendor }, { A2DP_CODEC_VENDOR_LHDC_V1, sizeof(a2dp_lhdc_v1_t), dump_lhdc_v1 }, { A2DP_CODEC_VENDOR_SAMSUNG_HD, -1, dump_vendor },