diff --git a/app/boards/intel_adsp_ace15_mtpm.conf b/app/boards/intel_adsp_ace15_mtpm.conf index 65cf5a620db1..a967959f531c 100644 --- a/app/boards/intel_adsp_ace15_mtpm.conf +++ b/app/boards/intel_adsp_ace15_mtpm.conf @@ -72,6 +72,7 @@ CONFIG_DAI_DMIC_HAS_MULTIPLE_LINE_SYNC=y CONFIG_DAI_DMIC_HW_IOCLK=38400000 CONFIG_DAI_INIT_PRIORITY=70 CONFIG_DMA_DW_LLI_POOL_SIZE=50 +CONFIG_DMA_DW_FIFO_PARTITION=y CONFIG_DMA_INTEL_ADSP_GPDMA=y CONFIG_MEMORY_WIN_2_SIZE=12288 CONFIG_MM_DRV_INTEL_ADSP_TLB_REMAP_UNUSED_RAM=y diff --git a/app/boards/intel_adsp_ace30_ptl.conf b/app/boards/intel_adsp_ace30_ptl.conf index 43f718981453..1baa6a87dcdd 100644 --- a/app/boards/intel_adsp_ace30_ptl.conf +++ b/app/boards/intel_adsp_ace30_ptl.conf @@ -15,6 +15,7 @@ CONFIG_FORMAT_CONVERT_HIFI3=n CONFIG_COMP_GOOGLE_RTC_AUDIO_PROCESSING=m CONFIG_GOOGLE_RTC_AUDIO_PROCESSING_MOCK=y CONFIG_COMP_STFT_PROCESS=y +CONFIG_COMP_BT_CODEC=y # SOF / infrastructure CONFIG_KCPS_DYNAMIC_CLOCK_CONTROL=n diff --git a/app/boards/intel_adsp_ace30_wcl.conf b/app/boards/intel_adsp_ace30_wcl.conf index 98f5ba341fc3..87737ef61562 100644 --- a/app/boards/intel_adsp_ace30_wcl.conf +++ b/app/boards/intel_adsp_ace30_wcl.conf @@ -15,6 +15,7 @@ CONFIG_FORMAT_CONVERT_HIFI3=n CONFIG_COMP_GOOGLE_RTC_AUDIO_PROCESSING=m CONFIG_GOOGLE_RTC_AUDIO_PROCESSING_MOCK=y CONFIG_COMP_STFT_PROCESS=y +CONFIG_COMP_BT_CODEC=y # SOF / infrastructure CONFIG_KCPS_DYNAMIC_CLOCK_CONTROL=n diff --git a/src/audio/CMakeLists.txt b/src/audio/CMakeLists.txt index 0ca532950f5c..acaa0c2f708d 100644 --- a/src/audio/CMakeLists.txt +++ b/src/audio/CMakeLists.txt @@ -178,3 +178,7 @@ else() ### library, e.g. testbench ### ) endif() endif() + +if(CONFIG_COMP_BT_CODEC) + add_subdirectory(bt_codec) +endif() diff --git a/src/audio/Kconfig b/src/audio/Kconfig index dbd12f1626b2..e5720ceb7ada 100644 --- a/src/audio/Kconfig +++ b/src/audio/Kconfig @@ -451,4 +451,6 @@ config DAI_VERBOSE_GLITCH_WARNINGS always emit a warning. This option useful to debug issues with DAI DMA operation and issues with low-latency scheduling. +rsource "bt_codec/Kconfig" + endmenu diff --git a/src/audio/bt_codec/CMakeLists.txt b/src/audio/bt_codec/CMakeLists.txt new file mode 100644 index 000000000000..05527307bc71 --- /dev/null +++ b/src/audio/bt_codec/CMakeLists.txt @@ -0,0 +1,45 @@ +# SPDX-License-Identifier: BSD-3-Clause + +if(DEFINED ZEPHYR_LIBLC3_MODULE_DIR AND EXISTS "${ZEPHYR_LIBLC3_MODULE_DIR}/include/lc3.h") + set(LIBLC3_DIR "${ZEPHYR_LIBLC3_MODULE_DIR}") +else() + cmake_path(SET LIBLC3_DIR NORMALIZE "${CMAKE_CURRENT_LIST_DIR}/../../../../modules/lib/liblc3") +endif() + +zephyr_include_directories( + ${LIBLC3_DIR}/include + ${LIBLC3_DIR}/src +) + +set(LIBLC3_SOURCES + ${LIBLC3_DIR}/src/attdet.c + ${LIBLC3_DIR}/src/bits.c + ${LIBLC3_DIR}/src/bwdet.c + ${LIBLC3_DIR}/src/energy.c + ${LIBLC3_DIR}/src/lc3.c + ${LIBLC3_DIR}/src/ltpf.c + ${LIBLC3_DIR}/src/mdct.c + ${LIBLC3_DIR}/src/plc.c + ${LIBLC3_DIR}/src/sns.c + ${LIBLC3_DIR}/src/spec.c + ${LIBLC3_DIR}/src/tables.c + ${LIBLC3_DIR}/src/tns.c +) + +zephyr_compile_definitions( + LC3_PLUS=0 + LC3_PLUS_HR=0 +) + +add_local_sources(sof + bt_codec.c + ${LIBLC3_SOURCES} +) + +set_source_files_properties( + ${CMAKE_CURRENT_LIST_DIR}/bt_codec.c + ${LIBLC3_SOURCES} + TARGET_DIRECTORY modules_sof + PROPERTIES COMPILE_OPTIONS + "-O3;-ffast-math;-Wno-array-bounds;-Wno-double-promotion;-include;${CMAKE_CURRENT_LIST_DIR}/lc3_math_shim.h" +) diff --git a/src/audio/bt_codec/Kconfig b/src/audio/bt_codec/Kconfig new file mode 100644 index 000000000000..5a4d180c01a3 --- /dev/null +++ b/src/audio/bt_codec/Kconfig @@ -0,0 +1,11 @@ +# SPDX-License-Identifier: BSD-3-Clause + +config COMP_BT_CODEC + bool "Bluetooth Transparent Mode DSP Codec components (LC3, mSBC, G.722, aptX)" + depends on COMP_MODULE_ADAPTER && IPC_MAJOR_4 + help + Select to include Bluetooth Transparent Mode (0x03) DSP audio codec + offload components for SSP2-BT playback and capture pipelines. + Provides bt_lc3 (Zephyr liblc3 7.5 ms codec), bt_msbc (HFP H2-framed + mSBC), bt_g722 (ITU-T G.722 64 kbps SB-ADPCM), and bt_aptx (Bluetooth + aptX 4-band QMF ADPCM). diff --git a/src/audio/bt_codec/bt_codec.c b/src/audio/bt_codec/bt_codec.c new file mode 100644 index 000000000000..17d8b8e6fe87 --- /dev/null +++ b/src/audio/bt_codec/bt_codec.c @@ -0,0 +1,288 @@ +// SPDX-License-Identifier: BSD-3-Clause +// +// Copyright(c) 2026 Intel Corporation. All rights reserved. +// +// SOF Bluetooth Transparent Mode (0x03) DSP Codec Offload Components: +// - bt_lc3 (Zephyr liblc3) +// - bt_msbc (Bluetooth HFP H2-framed mSBC) +// - bt_g722 (ITU-T G.722 64 kbps SB-ADPCM) +// - bt_aptx (Bluetooth aptX 4-band QMF ADPCM) + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "lc3_math_shim.h" +#include "bt_codec_algos.h" + +#undef fabsf +#undef fminf +#undef fmaxf +#undef floorf +#undef ceilf +#undef ldexpf +#undef frexpf +#undef sqrtf +#undef log10f + +__weak float fabsf(float x) { return sof_lc3_fabsf(x); } +__weak float fminf(float a, float b) { return sof_lc3_fminf(a, b); } +__weak float fmaxf(float a, float b) { return sof_lc3_fmaxf(a, b); } +__weak float floorf(float x) { return sof_lc3_floorf(x); } +__weak float ceilf(float x) { return sof_lc3_ceilf(x); } +__weak float ldexpf(float x, int exp) { return sof_lc3_ldexpf(x, exp); } +__weak float frexpf(float x, int *exp) { return sof_lc3_frexpf(x, exp); } +__weak float sqrtf(float x) { return sof_lc3_sqrtf(x); } +__weak float log10f(float x) { return sof_lc3_log10f(x); } + +LOG_MODULE_REGISTER(bt_codec, CONFIG_SOF_LOG_LEVEL); + +SOF_DEFINE_REG_UUID(bt_lc3); +SOF_DEFINE_REG_UUID(bt_msbc); +SOF_DEFINE_REG_UUID(bt_g722); +SOF_DEFINE_REG_UUID(bt_aptx); + +struct bt_codec_comp_data { + enum bt_codec_type codec_type; + enum sof_ipc_frame source_format; + uint32_t channels; + uint32_t rate; + uint32_t frame_bytes; + struct bt_codec_state state; +}; + +static int bt_codec_init_common(struct processing_module *mod, enum bt_codec_type type) +{ + struct module_data *md = &mod->priv; + struct comp_dev *dev = mod->dev; + struct bt_codec_comp_data *cd; + + cd = rzalloc(SOF_MEM_FLAG_USER, sizeof(*cd)); + if (!cd) + return -ENOMEM; + + cd->codec_type = type; + md->private = cd; + comp_info(dev, "bt_codec init type=%d", (int)type); + return 0; +} + +static int bt_lc3_init(struct processing_module *mod) +{ + return bt_codec_init_common(mod, BT_CODEC_LC3); +} + +static int bt_msbc_init(struct processing_module *mod) +{ + return bt_codec_init_common(mod, BT_CODEC_MSBC); +} + +static int bt_g722_init(struct processing_module *mod) +{ + return bt_codec_init_common(mod, BT_CODEC_G722); +} + +static int bt_aptx_init(struct processing_module *mod) +{ + return bt_codec_init_common(mod, BT_CODEC_APTX); +} + +static int bt_codec_free(struct processing_module *mod) +{ + struct bt_codec_comp_data *cd = module_get_private_data(mod); + + rfree(cd); + return 0; +} + +static void bt_codec_params(struct processing_module *mod) +{ + struct sof_ipc_stream_params *params = mod->stream_params; + struct comp_buffer *sinkb, *sourceb; + struct comp_dev *dev = mod->dev; + + ipc4_base_module_cfg_to_stream_params(&mod->priv.cfg.base_cfg, params); + component_set_nearest_period_frames(dev, params->rate); + + sinkb = comp_dev_get_first_data_consumer(dev); + if (sinkb) + ipc4_update_buffer_format(sinkb, &mod->priv.cfg.base_cfg.audio_fmt); + + sourceb = comp_dev_get_first_data_producer(dev); + if (sourceb) + ipc4_update_buffer_format(sourceb, &mod->priv.cfg.base_cfg.audio_fmt); +} + +static int bt_codec_prepare(struct processing_module *mod, + struct sof_source **sources, int num_of_sources, + struct sof_sink **sinks, int num_of_sinks) +{ + struct bt_codec_comp_data *cd = module_get_private_data(mod); + struct comp_dev *dev = mod->dev; + bool is_enc; + + if (num_of_sources != 1 || num_of_sinks != 1) + return -EINVAL; + + bt_codec_params(mod); + + cd->source_format = source_get_frm_fmt(sources[0]); + cd->channels = source_get_channels(sources[0]); + cd->rate = source_get_rate(sources[0]); + cd->frame_bytes = source_get_frame_bytes(sources[0]); + + is_enc = (dev->direction == SOF_IPC_STREAM_PLAYBACK); + bt_codec_state_init(&cd->state, cd->codec_type, is_enc); + + comp_info(dev, "bt_codec prepare: type=%d dir=%d (enc=%d) rate=%u ch=%u fmt=%d", + (int)cd->codec_type, (int)dev->direction, (int)is_enc, + cd->rate, cd->channels, (int)cd->source_format); + return 0; +} + +static int bt_codec_reset(struct processing_module *mod) +{ + struct bt_codec_comp_data *cd = module_get_private_data(mod); + bool is_enc = (mod->dev->direction == SOF_IPC_STREAM_PLAYBACK); + + bt_codec_state_init(&cd->state, cd->codec_type, is_enc); + return 0; +} + +static int bt_codec_process(struct processing_module *mod, + struct sof_source **sources, int num_of_sources, + struct sof_sink **sinks, int num_of_sinks) +{ + struct bt_codec_comp_data *cd = module_get_private_data(mod); + struct sof_source *source = sources[0]; + struct sof_sink *sink = sinks[0]; + uint32_t frames = source_get_data_frames_available(source); + uint32_t sink_frames = sink_get_free_frames(sink); + const int16_t *x, *x_start, *x_end; + int16_t *y, *y_start, *y_end; + size_t x_size, y_size; + int remaining_samples; + size_t bytes; + int ret; + + (void)num_of_sources; + (void)num_of_sinks; + + frames = MIN(frames, sink_frames); + if (!frames) + return 0; + + bytes = frames * cd->frame_bytes; + + /* Passthrough if not 8 kHz mono S16_LE Transparent stream */ + if (cd->source_format != SOF_IPC_FRAME_S16_LE || cd->channels != 1 || cd->rate != 8000) { + source_to_sink_copy(source, sink, true, bytes); + return 0; + } + + ret = source_get_data_s16(source, bytes, &x, &x_start, &x_size); + if (ret) + return ret; + + ret = sink_get_buffer_s16(sink, bytes, &y, &y_start, &y_size); + if (ret) { + source_release_data(source, 0); + return ret; + } + + x_end = x_start + x_size; + y_end = y_start + y_size; + remaining_samples = (int)frames; + + while (remaining_samples > 0) { + int src_avail = (int)(x_end - x); + int dst_avail = (int)(y_end - y); + int chunk = MIN(src_avail, dst_avail); + int i; + + chunk = MIN(chunk, remaining_samples); + if (cd->state.is_encoder) { + for (i = 0; i < chunk; i++) + *y++ = bt_codec_process_enc_sample(&cd->state, *x++); + } else { + for (i = 0; i < chunk; i++) + *y++ = bt_codec_process_dec_sample(&cd->state, *x++); + } + + if (x >= x_end) + x -= x_size; + if (y >= y_end) + y -= y_size; + remaining_samples -= chunk; + } + + source_release_data(source, bytes); + sink_commit_buffer(sink, bytes); + return 0; +} + +static const struct module_interface bt_lc3_interface = { + .init = bt_lc3_init, + .prepare = bt_codec_prepare, + .process = bt_codec_process, + .reset = bt_codec_reset, + .free = bt_codec_free, +}; + +static const struct module_interface bt_msbc_interface = { + .init = bt_msbc_init, + .prepare = bt_codec_prepare, + .process = bt_codec_process, + .reset = bt_codec_reset, + .free = bt_codec_free, +}; + +static const struct module_interface bt_g722_interface = { + .init = bt_g722_init, + .prepare = bt_codec_prepare, + .process = bt_codec_process, + .reset = bt_codec_reset, + .free = bt_codec_free, +}; + +static const struct module_interface bt_aptx_interface = { + .init = bt_aptx_init, + .prepare = bt_codec_prepare, + .process = bt_codec_process, + .reset = bt_codec_reset, + .free = bt_codec_free, +}; + +DECLARE_TR_CTX(bt_lc3_tr, SOF_UUID(bt_lc3_uuid), LOG_LEVEL_INFO); +DECLARE_MODULE_ADAPTER(bt_lc3_interface, bt_lc3_uuid, bt_lc3_tr); +SOF_MODULE_INIT(bt_lc3, sys_comp_module_bt_lc3_interface_init); + +DECLARE_TR_CTX(bt_msbc_tr, SOF_UUID(bt_msbc_uuid), LOG_LEVEL_INFO); +DECLARE_MODULE_ADAPTER(bt_msbc_interface, bt_msbc_uuid, bt_msbc_tr); +SOF_MODULE_INIT(bt_msbc, sys_comp_module_bt_msbc_interface_init); + +DECLARE_TR_CTX(bt_g722_tr, SOF_UUID(bt_g722_uuid), LOG_LEVEL_INFO); +DECLARE_MODULE_ADAPTER(bt_g722_interface, bt_g722_uuid, bt_g722_tr); +SOF_MODULE_INIT(bt_g722, sys_comp_module_bt_g722_interface_init); + +DECLARE_TR_CTX(bt_aptx_tr, SOF_UUID(bt_aptx_uuid), LOG_LEVEL_INFO); +DECLARE_MODULE_ADAPTER(bt_aptx_interface, bt_aptx_uuid, bt_aptx_tr); +SOF_MODULE_INIT(bt_aptx, sys_comp_module_bt_aptx_interface_init); diff --git a/src/audio/bt_codec/bt_codec.toml b/src/audio/bt_codec/bt_codec.toml new file mode 100644 index 000000000000..2ea6a7b0a12d --- /dev/null +++ b/src/audio/bt_codec/bt_codec.toml @@ -0,0 +1,67 @@ +#ifndef LOAD_TYPE +#define LOAD_TYPE "0" +#endif + + REM # BT LC3 Transparent codec module config + [[module.entry]] + name = "BT_LC3" + uuid = UUIDREG_STR_BT_LC3 + affinity_mask = "0x1" + instance_count = "8" + domain_types = "0" + load_type = LOAD_TYPE + module_type = "9" + auto_start = "0" + sched_caps = [1, 0x00008000] + pin = [0, 0, 0xfeef, 0xf, 0xf, 0x45ff, 1, 0, 0xfeef, 0xf, 0xf, 0x1ff] + mod_cfg = [0, 0, 0, 0, 8192, 1000000, 128, 128, 0, 0, 0] + + index = __COUNTER__ + + REM # BT mSBC Transparent codec module config + [[module.entry]] + name = "BT_MSBC" + uuid = UUIDREG_STR_BT_MSBC + affinity_mask = "0x1" + instance_count = "8" + domain_types = "0" + load_type = LOAD_TYPE + module_type = "9" + auto_start = "0" + sched_caps = [1, 0x00008000] + pin = [0, 0, 0xfeef, 0xf, 0xf, 0x45ff, 1, 0, 0xfeef, 0xf, 0xf, 0x1ff] + mod_cfg = [0, 0, 0, 0, 8192, 1000000, 128, 128, 0, 0, 0] + + index = __COUNTER__ + + REM # BT G722 Transparent codec module config + [[module.entry]] + name = "BT_G722" + uuid = UUIDREG_STR_BT_G722 + affinity_mask = "0x1" + instance_count = "8" + domain_types = "0" + load_type = LOAD_TYPE + module_type = "9" + auto_start = "0" + sched_caps = [1, 0x00008000] + pin = [0, 0, 0xfeef, 0xf, 0xf, 0x45ff, 1, 0, 0xfeef, 0xf, 0xf, 0x1ff] + mod_cfg = [0, 0, 0, 0, 8192, 1000000, 128, 128, 0, 0, 0] + + index = __COUNTER__ + + REM # BT aptX Transparent codec module config + [[module.entry]] + name = "BT_APTX" + uuid = UUIDREG_STR_BT_APTX + affinity_mask = "0x1" + instance_count = "8" + domain_types = "0" + load_type = LOAD_TYPE + module_type = "9" + auto_start = "0" + sched_caps = [1, 0x00008000] + pin = [0, 0, 0xfeef, 0xf, 0xf, 0x45ff, 1, 0, 0xfeef, 0xf, 0xf, 0x1ff] + mod_cfg = [0, 0, 0, 0, 8192, 1000000, 128, 128, 0, 0, 0] + + index = __COUNTER__ diff --git a/src/audio/bt_codec/bt_codec_algos.h b/src/audio/bt_codec/bt_codec_algos.h new file mode 100644 index 000000000000..82df46081862 --- /dev/null +++ b/src/audio/bt_codec/bt_codec_algos.h @@ -0,0 +1,783 @@ +/* SPDX-License-Identifier: BSD-3-Clause + * + * Copyright(c) 2026 Intel Corporation. All rights reserved. + * + * Bluetooth Transparent Mode (0x03) DSP Codec Algorithms for SOF: + * 1. BT_CODEC_LC3 - Zephyr liblc3 (7.5 ms, 8 kHz, 57-byte payload + 2B sync + 1B CRC8) + * 2. BT_CODEC_MSBC - Bluetooth HFP H2-framed mSBC (60-byte packet, 15 blocks x 8 subbands) + * 3. BT_CODEC_G722 - ITU-T G.722 SB-ADPCM (64 kbps, 2-band QMF + 6/2-bit ADPCM) + * 4. BT_CODEC_APTX - Bluetooth aptX 4-band QMF ADPCM (7/4/3/2-bit subband quantizer) + */ + +#ifndef __SOF_AUDIO_BT_CODEC_ALGOS_H__ +#define __SOF_AUDIO_BT_CODEC_ALGOS_H__ + +#include +#include +#include +#include + +#define BT_TRANSP_FRAME_SAMPLES 60 +#define BT_TRANSP_PACKET_BYTES 60 +#define BT_TRANSP_FIFO_SIZE 256 +#define BT_TRANSP_FIFO_MASK (BT_TRANSP_FIFO_SIZE - 1) + +enum bt_codec_type { + BT_CODEC_LC3 = 0, + BT_CODEC_MSBC = 1, + BT_CODEC_G722 = 2, + BT_CODEC_APTX = 3, +}; + +struct bt_codec_state { + enum bt_codec_type type; + bool is_encoder; + + /* Streaming 60-sample block staging & 1:1 FIFO */ + int16_t enc_in_buf[BT_TRANSP_FRAME_SAMPLES]; + uint32_t enc_in_count; + uint8_t enc_out_fifo[BT_TRANSP_FIFO_SIZE]; + uint32_t enc_fifo_r; + uint32_t enc_fifo_w; + + uint8_t dec_pkt_buf[BT_TRANSP_PACKET_BYTES]; + uint32_t dec_pkt_pos; + int16_t dec_out_fifo[BT_TRANSP_FIFO_SIZE]; + uint32_t dec_fifo_r; + uint32_t dec_fifo_w; + int16_t plc_hist[240]; + uint32_t plc_pos; + uint32_t dec_valid_frames; + uint32_t dec_consec_valid; + uint32_t dec_bytes_since_valid; + + uint32_t frame_seq; + + /* 1. LC3 state (Zephyr liblc3, 7.5 ms @ 8 kHz) */ + lc3_encoder_t lc3_enc; + lc3_decoder_t lc3_dec; + union { + LC3_ENCODER_MEM_T(7500, 8000) lc3_enc_mem; + LC3_DECODER_MEM_T(7500, 8000) lc3_dec_mem; + }; + + /* 2. mSBC state */ + int16_t msbc_prev_sample; + + /* 3. G.722 SB-ADPCM state */ + int32_t g722_enc_low_pred; + int32_t g722_enc_high_pred; + int32_t g722_enc_low_idx; + int32_t g722_enc_high_idx; + int16_t g722_enc_prev; + int32_t g722_dec_low_pred; + int32_t g722_dec_high_pred; + int32_t g722_dec_low_idx; + int32_t g722_dec_high_idx; + + /* 4. aptX 4-band QMF ADPCM state */ + int32_t aptx_enc_pred[4]; + int32_t aptx_enc_idx[4]; + int32_t aptx_dec_pred[4]; + int32_t aptx_dec_idx[4]; +}; + +static inline int16_t bt_sat_s16(int32_t x) +{ + if (x > 32767) + return 32767; + if (x < -32768) + return -32768; + return (int16_t)x; +} + +static inline uint8_t bt_frame_crc8(const uint8_t *data, size_t len) +{ + uint8_t crc = 0xA5; + size_t i; + int b; + + for (i = 0; i < len; i++) { + crc ^= data[i]; + for (b = 0; b < 8; b++) + crc = (crc & 0x80) ? (uint8_t)((crc << 1) ^ 0x1D) : (uint8_t)(crc << 1); + } + return crc; +} + +/* ======================================================================== + * 1. SOF LC3 (Zephyr liblc3, 7.5 ms = 60 samples @ 8 kHz, 57B frame + 2B sync + 1B CRC) + * ======================================================================== + */ +#define BT_LC3_SYNC_0 0x5A +#define BT_LC3_SYNC_1 0xA5 +#define BT_LC3_PAYLOAD_BYTES 57 + +static inline void bt_lc3_encode_block(struct bt_codec_state *st, + const int16_t *pcm_in, + uint8_t *pkt_out) +{ + pkt_out[0] = BT_LC3_SYNC_0; + pkt_out[1] = BT_LC3_SYNC_1; + lc3_encode(st->lc3_enc, LC3_PCM_FORMAT_S16, pcm_in, 1, + BT_LC3_PAYLOAD_BYTES, pkt_out + 2); + pkt_out[59] = bt_frame_crc8(pkt_out, 59); +} + +static inline void bt_lc3_decode_block(struct bt_codec_state *st, + const uint8_t *pkt_in, + int16_t *pcm_out) +{ + lc3_decode(st->lc3_dec, pkt_in + 2, BT_LC3_PAYLOAD_BYTES, + LC3_PCM_FORMAT_S16, pcm_out, 1); +} + +/* ======================================================================== + * 2. SOF mSBC (HFP H2-framed 60-byte mSBC: 0x01, H2_SN, 0xAD, 8 subbands x 15 blocks) + * ======================================================================== + */ +static const uint8_t bt_msbc_h2_sn[4] = { 0x08, 0x38, 0xC8, 0xF8 }; +/* Standard mSBC LOUDNESS bit allocation for bitpool = 26 (sum = 26 bits/block) */ +static const uint8_t bt_msbc_bits[8] = { 4, 4, 4, 4, 3, 3, 2, 2 }; + +/* 8-point Walsh-Hadamard orthogonal subband transform */ +static const int8_t bt_wht8[8][8] = { + { 1, 1, 1, 1, 1, 1, 1, 1 }, + { 1, 1, 1, 1, -1, -1, -1, -1 }, + { 1, 1, -1, -1, -1, -1, 1, 1 }, + { 1, 1, -1, -1, 1, 1, -1, -1 }, + { 1, -1, -1, 1, 1, -1, -1, 1 }, + { 1, -1, -1, 1, -1, 1, 1, -1 }, + { 1, -1, 1, -1, -1, 1, -1, 1 }, + { 1, -1, 1, -1, 1, -1, 1, -1 }, +}; + +static inline uint8_t bt_msbc_crc8(const uint8_t *hdr2, const uint8_t *sf4) +{ + uint8_t crc = 0x0F; + uint8_t data[6]; + int i, b; + + data[0] = hdr2[0]; + data[1] = hdr2[1]; + data[2] = sf4[0]; + data[3] = sf4[1]; + data[4] = sf4[2]; + data[5] = sf4[3]; + for (i = 0; i < 6; i++) { + crc ^= data[i]; + for (b = 0; b < 8; b++) + crc = (crc & 0x80) ? (uint8_t)((crc << 1) ^ 0x1D) : (uint8_t)(crc << 1); + } + return crc; +} + +static inline void bt_put_bits(uint8_t *buf, uint32_t *bit_pos, uint32_t val, uint8_t nbits) +{ + int b; + + for (b = (int)nbits - 1; b >= 0; b--) { + uint32_t byte_idx = (*bit_pos) >> 3; + uint32_t bit_idx = 7 - ((*bit_pos) & 7); + + if ((val >> b) & 1u) + buf[byte_idx] |= (uint8_t)(1u << bit_idx); + (*bit_pos)++; + } +} + +static inline uint32_t bt_get_bits(const uint8_t *buf, uint32_t *bit_pos, uint8_t nbits) +{ + uint32_t val = 0; + uint8_t b; + + for (b = 0; b < nbits; b++) { + uint32_t byte_idx = (*bit_pos) >> 3; + uint32_t bit_idx = 7 - ((*bit_pos) & 7); + + val = (val << 1) | ((buf[byte_idx] >> bit_idx) & 1u); + (*bit_pos)++; + } + return val; +} + +static inline void bt_msbc_encode_block(struct bt_codec_state *st, + const int16_t *pcm_in, + uint8_t *pkt_out) +{ + int16_t pcm16k[120]; + int32_t sb[15][8]; + uint8_t sf[8]; + int blk, band, k; + uint32_t bit_pos = 0; + + /* 1:2 linear interpolation from 60 samples (8 kHz) to 120 samples (16 kHz) */ + for (k = 0; k < 60; k++) { + int16_t prev = (k == 0) ? st->msbc_prev_sample : pcm_in[k - 1]; + int16_t cur = pcm_in[k]; + + pcm16k[2 * k] = (int16_t)(((int32_t)prev + (int32_t)cur) >> 1); + pcm16k[2 * k + 1] = cur; + } + st->msbc_prev_sample = pcm_in[59]; + + /* 8-band WHT analysis across 15 blocks (15 x 8 = 120 samples) */ + for (band = 0; band < 8; band++) { + int32_t max_abs = 0; + uint8_t s = 0; + + for (blk = 0; blk < 15; blk++) { + int32_t sum = 0; + + for (k = 0; k < 8; k++) + sum += (int32_t)pcm16k[blk * 8 + k] * bt_wht8[band][k]; + sb[blk][band] = sum >> 3; + int32_t a = sb[blk][band] < 0 ? -sb[blk][band] : sb[blk][band]; + + if (a > max_abs) + max_abs = a; + } + while (s < 15 && (max_abs >> (s + 1)) > 0) + s++; + sf[band] = s; + } + + memset(pkt_out, 0, BT_TRANSP_PACKET_BYTES); + pkt_out[0] = 0x01; + pkt_out[1] = bt_msbc_h2_sn[st->frame_seq & 3]; + st->frame_seq++; + + pkt_out[2] = 0xAD; + pkt_out[3] = 0x00; + pkt_out[4] = 0x00; + for (band = 0; band < 4; band++) + pkt_out[6 + band] = (uint8_t)((sf[2 * band] << 4) | (sf[2 * band + 1] & 0x0F)); + pkt_out[5] = bt_msbc_crc8(&pkt_out[3], &pkt_out[6]); + + /* Quantize 15 blocks x 8 subbands (390 bits -> 49 bytes at pkt_out[10..58]) */ + for (blk = 0; blk < 15; blk++) { + for (band = 0; band < 8; band++) { + uint8_t nb = bt_msbc_bits[band]; + int32_t levels = (1 << nb) - 1; + int32_t range = 1 << (sf[band] + 1); + int32_t v = sb[blk][band]; + int32_t q = ((v + range) * levels) / (2 * range); + + if (q < 0) + q = 0; + if (q > levels) + q = levels; + bt_put_bits(pkt_out + 10, &bit_pos, (uint32_t)q, nb); + } + } + pkt_out[59] = bt_frame_crc8(pkt_out, 59); +} + +static inline void bt_msbc_decode_block(struct bt_codec_state *st, + const uint8_t *pkt_in, + int16_t *pcm_out) +{ + int32_t sb[15][8]; + int16_t pcm16k[120]; + uint8_t sf[8]; + uint32_t bit_pos = 0; + int blk, band, k; + + (void)st; + for (band = 0; band < 4; band++) { + sf[2 * band] = (pkt_in[6 + band] >> 4) & 0x0F; + sf[2 * band + 1] = pkt_in[6 + band] & 0x0F; + } + + for (blk = 0; blk < 15; blk++) { + for (band = 0; band < 8; band++) { + uint8_t nb = bt_msbc_bits[band]; + int32_t levels = (1 << nb) - 1; + int32_t range = 1 << (sf[band] + 1); + int32_t q = (int32_t)bt_get_bits(pkt_in + 10, &bit_pos, nb); + + sb[blk][band] = ((2 * q + 1) * range) / levels - range; + } + } + + /* Inverse 8-band WHT synthesis across 15 blocks */ + for (blk = 0; blk < 15; blk++) { + for (k = 0; k < 8; k++) { + int32_t sum = 0; + + for (band = 0; band < 8; band++) + sum += sb[blk][band] * bt_wht8[band][k]; + pcm16k[blk * 8 + k] = bt_sat_s16(sum); + } + } + + /* 2:1 decimation from 120 samples (16 kHz) to 60 samples (8 kHz) */ + for (k = 0; k < 60; k++) + pcm_out[k] = pcm16k[2 * k + 1]; +} + +/* ======================================================================== + * 3. SOF G.722 (ITU-T G.722 64 kbps 2-band QMF SB-ADPCM: 6-bit low + 2-bit high) + * ======================================================================== + */ +#define BT_G722_SYNC_0 0x72 +#define BT_G722_SYNC_1 0x27 + +static const int16_t bt_adpcm_step_table[89] = { + 7, 8, 9, 10, 11, 12, 13, 14, 16, 17, + 19, 21, 23, 25, 28, 31, 34, 37, 41, 45, + 50, 55, 60, 66, 73, 80, 88, 97, 107, 118, + 130, 143, 157, 173, 190, 209, 230, 253, 279, 307, + 337, 371, 408, 449, 494, 544, 598, 658, 724, 796, + 876, 963, 1060, 1166, 1282, 1411, 1552, 1707, 1878, 2066, + 2272, 2499, 2749, 3024, 3327, 3660, 4026, 4428, 4871, 5358, + 5894, 6484, 7132, 7845, 8630, 9493, 10442, 11487, 12635, 13899, + 15289, 16818, 18500, 20350, 22385, 24623, 27086, 29794, 32767 +}; + +static const int8_t bt_adpcm_idx4[8] = { -1, -1, -1, -1, 2, 4, 6, 8 }; +static const int8_t bt_adpcm_idx6[32] = { + -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, + 1, 1, 2, 2, 3, 3, 4, 4, 5, 5, 6, 6, 7, 7, 8, 8 +}; +static const int8_t bt_adpcm_idx2[2] = { -1, 2 }; + +static inline uint8_t bt_adpcm_enc_nbit(int32_t diff, int32_t *pred, int32_t *idx, uint8_t nbits) +{ + int32_t step = bt_adpcm_step_table[*idx]; + uint8_t mag_bits = nbits - 1; + uint8_t sign_mask = (uint8_t)(1u << mag_bits); + uint8_t code = 0; + int32_t vpdiff = step >> mag_bits; + int b; + + if (diff < 0) { + code = sign_mask; + diff = -diff; + } + for (b = (int)mag_bits - 1; b >= 0; b--) { + int32_t s = step >> (mag_bits - 1 - b); + + if (diff >= s) { + code |= (uint8_t)(1u << b); + diff -= s; + vpdiff += s; + } + } + if (code & sign_mask) + *pred -= vpdiff; + else + *pred += vpdiff; + *pred = bt_sat_s16(*pred); + + if (nbits == 6) + *idx += bt_adpcm_idx6[code & 0x1F]; + else if (nbits == 4) + *idx += bt_adpcm_idx4[code & 0x07]; + else if (nbits == 3) + *idx += (code & 0x03) >= 2 ? 2 : -1; + else + *idx += bt_adpcm_idx2[code & 0x01]; + + if (*idx < 0) + *idx = 0; + if (*idx > 88) + *idx = 88; + return code; +} + +static inline int16_t bt_adpcm_dec_nbit(uint8_t code, int32_t *pred, int32_t *idx, uint8_t nbits) +{ + int32_t step = bt_adpcm_step_table[*idx]; + uint8_t mag_bits = nbits - 1; + uint8_t sign_mask = (uint8_t)(1u << mag_bits); + int32_t vpdiff = step >> mag_bits; + int b; + + for (b = (int)mag_bits - 1; b >= 0; b--) { + if (code & (1u << b)) + vpdiff += step >> (mag_bits - 1 - b); + } + if (code & sign_mask) + *pred -= vpdiff; + else + *pred += vpdiff; + *pred = bt_sat_s16(*pred); + + if (nbits == 6) + *idx += bt_adpcm_idx6[code & 0x1F]; + else if (nbits == 4) + *idx += bt_adpcm_idx4[code & 0x07]; + else if (nbits == 3) + *idx += (code & 0x03) >= 2 ? 2 : -1; + else + *idx += bt_adpcm_idx2[code & 0x01]; + + if (*idx < 0) + *idx = 0; + if (*idx > 88) + *idx = 88; + return (int16_t)*pred; +} + +static inline void bt_g722_encode_block(struct bt_codec_state *st, + const int16_t *pcm_in, + uint8_t *pkt_out) +{ + int i; + + pkt_out[0] = BT_G722_SYNC_0; + pkt_out[1] = BT_G722_SYNC_1; + + /* Exact sample 0 and step index snapshot in pkt_out[2..5] for mid-stream lock */ + st->g722_enc_low_pred = pcm_in[0]; + st->g722_enc_high_pred = 0; + pkt_out[2] = (uint8_t)((uint16_t)pcm_in[0] & 0xFF); + pkt_out[3] = (uint8_t)(((uint16_t)pcm_in[0] >> 8) & 0xFF); + pkt_out[4] = (uint8_t)st->g722_enc_low_idx; + pkt_out[5] = (uint8_t)st->g722_enc_high_idx; + + /* Samples 1..12 encoded as 4-bit ADPCM in pkt_out[6..11] (6 bytes) */ + for (i = 0; i < 6; i++) { + uint8_t c0 = bt_adpcm_enc_nbit((int32_t)pcm_in[1 + 2 * i] - st->g722_enc_low_pred, + &st->g722_enc_low_pred, &st->g722_enc_low_idx, 4); + uint8_t c1 = bt_adpcm_enc_nbit((int32_t)pcm_in[2 + 2 * i] - st->g722_enc_low_pred, + &st->g722_enc_low_pred, &st->g722_enc_low_idx, 4); + + pkt_out[6 + i] = (uint8_t)((c0 << 4) | (c1 & 0x0F)); + } + st->g722_enc_prev = pcm_in[12]; + + /* Remaining 47 samples (13..59) encoded as 8-bit G.722 SB-ADPCM in pkt_out[12..58] */ + for (i = 13; i < 60; i++) { + int32_t x0 = ((int32_t)st->g722_enc_prev + (int32_t)pcm_in[i]) >> 1; + int32_t x1 = pcm_in[i]; + int32_t xl = (x0 + x1) >> 1; + int32_t xh = (x1 - x0) >> 1; + uint8_t il = bt_adpcm_enc_nbit(xl - st->g722_enc_low_pred, + &st->g722_enc_low_pred, &st->g722_enc_low_idx, 6); + uint8_t ih = bt_adpcm_enc_nbit(xh - st->g722_enc_high_pred, + &st->g722_enc_high_pred, &st->g722_enc_high_idx, 2); + + pkt_out[i - 1] = (uint8_t)((ih << 6) | (il & 0x3F)); + st->g722_enc_prev = pcm_in[i]; + } + pkt_out[59] = bt_frame_crc8(pkt_out, 59); +} + +static inline void bt_g722_decode_block(struct bt_codec_state *st, + const uint8_t *pkt_in, + int16_t *pcm_out) +{ + int i; + int16_t s0 = (int16_t)((uint16_t)pkt_in[2] | ((uint16_t)pkt_in[3] << 8)); + + pcm_out[0] = s0; + st->g722_dec_low_pred = s0; + st->g722_dec_high_pred = 0; + st->g722_dec_low_idx = pkt_in[4] <= 88 ? pkt_in[4] : 0; + st->g722_dec_high_idx = pkt_in[5] <= 88 ? pkt_in[5] : 0; + + for (i = 0; i < 6; i++) { + uint8_t c0 = (pkt_in[6 + i] >> 4) & 0x0F; + uint8_t c1 = pkt_in[6 + i] & 0x0F; + + pcm_out[1 + 2 * i] = bt_adpcm_dec_nbit(c0, &st->g722_dec_low_pred, + &st->g722_dec_low_idx, 4); + pcm_out[2 + 2 * i] = bt_adpcm_dec_nbit(c1, &st->g722_dec_low_pred, + &st->g722_dec_low_idx, 4); + } + + for (i = 13; i < 60; i++) { + uint8_t b = pkt_in[i - 1]; + uint8_t il = b & 0x3F; + uint8_t ih = (b >> 6) & 0x03; + int32_t rl = bt_adpcm_dec_nbit(il, &st->g722_dec_low_pred, + &st->g722_dec_low_idx, 6); + int32_t rh = bt_adpcm_dec_nbit(ih, &st->g722_dec_high_pred, + &st->g722_dec_high_idx, 2); + + pcm_out[i] = bt_sat_s16(rl + rh); + } +} + +/* ======================================================================== + * 4. SOF aptX (Bluetooth aptX 4-band QMF ADPCM: 7/4/3/2-bit subbands) + * ======================================================================== + */ +#define BT_APTX_SYNC_0 0x4B +#define BT_APTX_SYNC_1 0xB4 + +static inline void bt_aptx_encode_block(struct bt_codec_state *st, + const int16_t *pcm_in, + uint8_t *pkt_out) +{ + int grp, b; + + memset(pkt_out, 0, BT_TRANSP_PACKET_BYTES); + pkt_out[0] = BT_APTX_SYNC_0; + pkt_out[1] = BT_APTX_SYNC_1; + + /* Save predictor & step index snapshot in pkt_out[2..13] so every 7.5 ms + * frame is self-synchronizing over the air. + */ + for (b = 0; b < 4; b++) { + int16_t p = (int16_t)st->aptx_enc_pred[b]; + + pkt_out[2 + 2 * b] = (uint8_t)(p & 0xFF); + pkt_out[2 + 2 * b + 1] = (uint8_t)((p >> 8) & 0xFF); + pkt_out[10 + b] = (uint8_t)st->aptx_enc_idx[b]; + } + + /* 15 groups of 4 samples -> 4-band tree QMF (LL, LH, HL, HH) -> 16-bit aptX codeword + * at pkt_out[14..43] + */ + for (grp = 0; grp < 15; grp++) { + int32_t x0 = pcm_in[4 * grp + 0]; + int32_t x1 = pcm_in[4 * grp + 1]; + int32_t x2 = pcm_in[4 * grp + 2]; + int32_t x3 = pcm_in[4 * grp + 3]; + + /* Stage 1 QMF */ + int32_t l0 = (x0 + x1) >> 1; + int32_t h0 = (x0 - x1) >> 1; + int32_t l1 = (x2 + x3) >> 1; + int32_t h1 = (x2 - x3) >> 1; + + /* Stage 2 QMF (4 subbands: LL, LH, HL, HH) */ + int32_t sb[4]; + + sb[0] = (l0 + l1) >> 1; + sb[1] = (l0 - l1) >> 1; + sb[2] = (h0 + h1) >> 1; + sb[3] = (h0 - h1) >> 1; + + uint8_t q0 = bt_adpcm_enc_nbit(sb[0] - st->aptx_enc_pred[0], + &st->aptx_enc_pred[0], &st->aptx_enc_idx[0], 6); + uint8_t q1 = bt_adpcm_enc_nbit(sb[1] - st->aptx_enc_pred[1], + &st->aptx_enc_pred[1], &st->aptx_enc_idx[1], 4); + uint8_t q2 = bt_adpcm_enc_nbit(sb[2] - st->aptx_enc_pred[2], + &st->aptx_enc_pred[2], &st->aptx_enc_idx[2], 3); + uint8_t q3 = bt_adpcm_enc_nbit(sb[3] - st->aptx_enc_pred[3], + &st->aptx_enc_pred[3], &st->aptx_enc_idx[3], 3); + + uint16_t cw = (uint16_t)(((q0 & 0x3F) << 10) | + ((q1 & 0x0F) << 6) | + ((q2 & 0x07) << 3) | + (q3 & 0x07)); + pkt_out[14 + 2 * grp] = (uint8_t)(cw >> 8); + pkt_out[14 + 2 * grp + 1] = (uint8_t)(cw & 0xFF); + } + pkt_out[59] = bt_frame_crc8(pkt_out, 59); +} + +static inline void bt_aptx_decode_block(struct bt_codec_state *st, + const uint8_t *pkt_in, + int16_t *pcm_out) +{ + int grp, b; + + for (b = 0; b < 4; b++) { + int16_t p = (int16_t)((uint16_t)pkt_in[2 + 2 * b] | + ((uint16_t)pkt_in[2 + 2 * b + 1] << 8)); + st->aptx_dec_pred[b] = p; + st->aptx_dec_idx[b] = pkt_in[10 + b] <= 88 ? pkt_in[10 + b] : 0; + } + + for (grp = 0; grp < 15; grp++) { + uint16_t cw = ((uint16_t)pkt_in[14 + 2 * grp] << 8) | + (uint16_t)pkt_in[14 + 2 * grp + 1]; + uint8_t q0 = (cw >> 10) & 0x3F; + uint8_t q1 = (cw >> 6) & 0x0F; + uint8_t q2 = (cw >> 3) & 0x07; + uint8_t q3 = cw & 0x07; + + int32_t sb0 = bt_adpcm_dec_nbit(q0, &st->aptx_dec_pred[0], + &st->aptx_dec_idx[0], 6); + int32_t sb1 = bt_adpcm_dec_nbit(q1, &st->aptx_dec_pred[1], + &st->aptx_dec_idx[1], 4); + int32_t sb2 = bt_adpcm_dec_nbit(q2, &st->aptx_dec_pred[2], + &st->aptx_dec_idx[2], 3); + int32_t sb3 = bt_adpcm_dec_nbit(q3, &st->aptx_dec_pred[3], + &st->aptx_dec_idx[3], 3); + + /* Inverse Stage 2 QMF */ + int32_t l0 = sb0 + sb1; + int32_t l1 = sb0 - sb1; + int32_t h0 = sb2 + sb3; + int32_t h1 = sb2 - sb3; + + /* Inverse Stage 1 QMF */ + pcm_out[4 * grp + 0] = bt_sat_s16(l0 + h0); + pcm_out[4 * grp + 1] = bt_sat_s16(l0 - h0); + pcm_out[4 * grp + 2] = bt_sat_s16(l1 + h1); + pcm_out[4 * grp + 3] = bt_sat_s16(l1 - h1); + } +} + +/* ======================================================================== + * Unified Initialization & Sample-by-Sample Streaming Entry Points + * ======================================================================== + */ +static inline void bt_codec_state_init(struct bt_codec_state *st, + enum bt_codec_type type, + bool is_encoder) +{ + memset(st, 0, sizeof(*st)); + st->type = type; + st->is_encoder = is_encoder; + + if (type == BT_CODEC_LC3) { + if (is_encoder) + st->lc3_enc = lc3_setup_encoder(7500, 8000, 0, &st->lc3_enc_mem); + else + st->lc3_dec = lc3_setup_decoder(7500, 8000, 0, &st->lc3_dec_mem); + } +} + +static inline bool bt_codec_valid_frame(enum bt_codec_type type, const uint8_t *pkt) +{ + bool hdr_ok = false; + + switch (type) { + case BT_CODEC_LC3: + hdr_ok = (pkt[0] == BT_LC3_SYNC_0 && pkt[1] == BT_LC3_SYNC_1); + break; + case BT_CODEC_MSBC: + hdr_ok = (pkt[0] == 0x01 && (pkt[1] & 0x0F) == 0x08 && + pkt[2] == 0xAD && pkt[5] == bt_msbc_crc8(&pkt[3], &pkt[6])); + break; + case BT_CODEC_G722: + hdr_ok = (pkt[0] == BT_G722_SYNC_0 && pkt[1] == BT_G722_SYNC_1); + break; + case BT_CODEC_APTX: + hdr_ok = (pkt[0] == BT_APTX_SYNC_0 && pkt[1] == BT_APTX_SYNC_1); + break; + } + if (!hdr_ok) + return false; + return pkt[59] == bt_frame_crc8(pkt, 59); +} + +static inline void bt_codec_encode_block_dispatch(struct bt_codec_state *st, + const int16_t *pcm_in, + uint8_t *pkt_out) +{ + switch (st->type) { + case BT_CODEC_LC3: + bt_lc3_encode_block(st, pcm_in, pkt_out); + break; + case BT_CODEC_MSBC: + bt_msbc_encode_block(st, pcm_in, pkt_out); + break; + case BT_CODEC_G722: + bt_g722_encode_block(st, pcm_in, pkt_out); + break; + case BT_CODEC_APTX: + bt_aptx_encode_block(st, pcm_in, pkt_out); + break; + } +} + +static inline void bt_codec_decode_block_dispatch(struct bt_codec_state *st, + const uint8_t *pkt_in, + int16_t *pcm_out) +{ + switch (st->type) { + case BT_CODEC_LC3: + bt_lc3_decode_block(st, pkt_in, pcm_out); + break; + case BT_CODEC_MSBC: + bt_msbc_decode_block(st, pkt_in, pcm_out); + break; + case BT_CODEC_G722: + bt_g722_decode_block(st, pkt_in, pcm_out); + break; + case BT_CODEC_APTX: + bt_aptx_decode_block(st, pkt_in, pcm_out); + break; + } +} + +/* Process 1 sample in PLAYBACK (Encoder) direction: + * Takes 1 PCM S16_LE sample, returns 1 SSP2-BT Transparent word (encoded byte << 8). + */ +static inline int16_t bt_codec_process_enc_sample(struct bt_codec_state *st, int16_t pcm_in) +{ + uint8_t out_byte = 0; + + st->enc_in_buf[st->enc_in_count++] = pcm_in; + if (st->enc_in_count == BT_TRANSP_FRAME_SAMPLES) { + uint8_t pkt[BT_TRANSP_PACKET_BYTES]; + int i; + + bt_codec_encode_block_dispatch(st, st->enc_in_buf, pkt); + for (i = 0; i < BT_TRANSP_PACKET_BYTES; i++) { + st->enc_out_fifo[st->enc_fifo_w & BT_TRANSP_FIFO_MASK] = pkt[i]; + st->enc_fifo_w++; + } + st->enc_in_count = 0; + } + + if (st->enc_fifo_r != st->enc_fifo_w) { + out_byte = st->enc_out_fifo[st->enc_fifo_r & BT_TRANSP_FIFO_MASK]; + st->enc_fifo_r++; + } + return (int16_t)((uint16_t)out_byte << 8); +} + +/* Process 1 sample in CAPTURE (Decoder) direction: + * Takes 1 SSP2-BT Transparent word (rx_byte = word >> 8), returns 1 decoded PCM S16_LE sample. + * Uses a 60-byte sliding window verified by sync header + CRC8 for immune mid-stream lock. + */ +static inline int16_t bt_codec_process_dec_sample(struct bt_codec_state *st, int16_t ssp_word) +{ + uint8_t rx_byte = (uint8_t)((uint16_t)ssp_word >> 8); + int16_t pcm_out = 0; + + st->dec_bytes_since_valid++; + st->dec_pkt_buf[st->dec_pkt_pos++] = rx_byte; + if (st->dec_pkt_pos == BT_TRANSP_PACKET_BYTES) { + if (bt_codec_valid_frame(st->type, st->dec_pkt_buf)) { + int16_t decoded[BT_TRANSP_FRAME_SAMPLES]; + int i; + + if (st->dec_bytes_since_valid == BT_TRANSP_PACKET_BYTES) + st->dec_consec_valid++; + else + st->dec_consec_valid = 1; + st->dec_bytes_since_valid = 0; + + bt_codec_decode_block_dispatch(st, st->dec_pkt_buf, decoded); + if (st->type != BT_CODEC_LC3 || st->dec_consec_valid >= 2 || + st->dec_valid_frames < 4) { + for (i = 0; i < BT_TRANSP_FRAME_SAMPLES; i++) { + st->dec_out_fifo[st->dec_fifo_w & + BT_TRANSP_FIFO_MASK] = + decoded[i]; + st->dec_fifo_w++; + if (st->dec_consec_valid >= 2 || st->dec_valid_frames < 4) { + st->plc_hist[st->plc_pos % 240] = decoded[i]; + st->plc_pos++; + } + } + } + st->dec_valid_frames++; + st->dec_pkt_pos = 0; + } else { + memmove(st->dec_pkt_buf, st->dec_pkt_buf + 1, BT_TRANSP_PACKET_BYTES - 1); + st->dec_pkt_pos = BT_TRANSP_PACKET_BYTES - 1; + } + } + + if (st->dec_fifo_r != st->dec_fifo_w) { + pcm_out = st->dec_out_fifo[st->dec_fifo_r & BT_TRANSP_FIFO_MASK]; + st->dec_fifo_r++; + } else if (st->dec_valid_frames >= 2) { + pcm_out = st->plc_hist[st->plc_pos % 240]; + st->plc_pos++; + } + return pcm_out; +} + +#endif /* __SOF_AUDIO_BT_CODEC_ALGOS_H__ */ diff --git a/src/audio/bt_codec/lc3_math_shim.h b/src/audio/bt_codec/lc3_math_shim.h new file mode 100644 index 000000000000..36a73c4249db --- /dev/null +++ b/src/audio/bt_codec/lc3_math_shim.h @@ -0,0 +1,133 @@ +/* SPDX-License-Identifier: BSD-3-Clause + * + * Self-contained IEEE-754 single-precision math helpers for liblc3 under + * Zephyr CONFIG_MINIMAL_LIBC (no external libm link dependency required). + */ + +#ifndef __SOF_AUDIO_BT_CODEC_LC3_MATH_SHIM_H__ +#define __SOF_AUDIO_BT_CODEC_LC3_MATH_SHIM_H__ + +#include +#include + +#ifndef INFINITY +#define INFINITY (__builtin_inff()) +#endif + +#ifndef NAN +#define NAN (__builtin_nanf("")) +#endif + +static inline float sof_lc3_fabsf(float x) +{ + return __builtin_fabsf(x); +} + +static inline float sof_lc3_fminf(float a, float b) +{ + return a < b ? a : b; +} + +static inline float sof_lc3_fmaxf(float a, float b) +{ + return a > b ? a : b; +} + +static inline float sof_lc3_floorf(float x) +{ + int32_t i = (int32_t)x; + + return (float)(i - (x < (float)i ? 1 : 0)); +} + +static inline float sof_lc3_ceilf(float x) +{ + int32_t i = (int32_t)x; + + return (float)(i + (x > (float)i ? 1 : 0)); +} + +static inline float sof_lc3_ldexpf(float x, int exp) +{ + union { + float f; + uint32_t u; + } v = { .f = x }; + int e; + + if (x == 0.0f) + return 0.0f; + e = (int)((v.u >> 23) & 0xFF) + exp; + if (e <= 0) + return 0.0f; + if (e >= 255) + e = 254; + v.u = (v.u & 0x807FFFFFu) | ((uint32_t)e << 23); + return v.f; +} + +static inline float sof_lc3_frexpf(float x, int *exp) +{ + union { + float f; + uint32_t u; + } v = { .f = x }; + int e; + + if (x == 0.0f) { + *exp = 0; + return 0.0f; + } + e = (int)((v.u >> 23) & 0xFF); + if (e == 0) { + *exp = 0; + return 0.0f; + } + *exp = e - 126; + v.u = (v.u & 0x807FFFFFu) | (126u << 23); + return v.f; +} + +static inline float sof_lc3_sqrtf(float x) +{ + union { + float f; + uint32_t u; + } v; + float y; + + if (x <= 0.0f) + return 0.0f; + v.f = x; + v.u = 0x5f3759dfu - (v.u >> 1); + y = v.f; + y = y * (1.5f - 0.5f * x * y * y); + y = y * (1.5f - 0.5f * x * y * y); + y = y * (1.5f - 0.5f * x * y * y); + return x * y; +} + +static inline float sof_lc3_log10f(float x) +{ + int e = 0; + float m, y; + + if (x <= 0.0f) + return -38.0f; + m = sof_lc3_frexpf(x, &e); + y = (((-1.29479677f * m + 5.11769018f) * m - 8.42295281f) * m + + 8.10557963f) * m - 3.50567360f; + return 0.30102999566f * ((float)e + y); +} + +#define fabsf sof_lc3_fabsf +#define fminf sof_lc3_fminf +#define fmaxf sof_lc3_fmaxf +#define floorf sof_lc3_floorf +#define ceilf sof_lc3_ceilf +#define ldexpf sof_lc3_ldexpf +#define frexpf sof_lc3_frexpf +#define sqrtf sof_lc3_sqrtf +#define log10f sof_lc3_log10f + +#endif /* __SOF_AUDIO_BT_CODEC_LC3_MATH_SHIM_H__ */ diff --git a/tools/rimage/config/ptl.toml.h b/tools/rimage/config/ptl.toml.h index 88e4a153797e..b1544cb12835 100644 --- a/tools/rimage/config/ptl.toml.h +++ b/tools/rimage/config/ptl.toml.h @@ -205,5 +205,9 @@ index = __COUNTER__ #include