From c8092cb5389f7415f853fc85225c2b35b28b4e2b Mon Sep 17 00:00:00 2001 From: Liam Girdwood Date: Tue, 13 Oct 2026 14:40:00 +0100 Subject: [PATCH 01/13] include: common: introduce ARM SIMD configuration macros Define SOF_CONFIG_ARM_SIMD, SOF_USE_ARM_SIMD, SOF_CONFIG_ARM_FPU, and SOF_USE_ARM_FPU macros in common.h mirroring SOF_CONFIG_HIFI and SOF_USE_HIFI to allow compile-time selection of architecture-accelerated ARM Cortex-M DSP SIMD and FPU processing kernels across audio modules and math libraries. Signed-off-by: Liam Girdwood --- src/include/sof/common.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/include/sof/common.h b/src/include/sof/common.h index bb193310e97f..9d1e721b850d 100644 --- a/src/include/sof/common.h +++ b/src/include/sof/common.h @@ -196,6 +196,11 @@ #define SOF_USE_MIN_HIFI(minlevel, component) (SOF_CONFIG_HIFI(minlevel, component) || \ (SOF_CONFIG_HIFI(MAX, component) && minlevel <= SOF_MAX_XCHAL_HIFI)) +#define SOF_CONFIG_ARM_SIMD(component) (CONFIG_ ## component ## _ARM_SIMD) +#define SOF_USE_ARM_SIMD(component) (IS_ENABLED(CONFIG_ ## component ## _ARM_SIMD)) +#define SOF_CONFIG_ARM_FPU(component) (CONFIG_ ## component ## _ARM_FPU) +#define SOF_USE_ARM_FPU(component) (IS_ENABLED(CONFIG_ ## component ## _ARM_FPU)) + #ifndef __XCC__ // Cadence toolchains: either xt-xcc or xt-clang. # define SOF_MAX_XCHAL_HIFI NONE #else From d14118d763d04119e334c5de2a7f8e0388bc1fe3 Mon Sep 17 00:00:00 2001 From: Liam Girdwood Date: Tue, 13 Oct 2026 14:40:11 +0100 Subject: [PATCH 02/13] include: audio: add ARM Cortex-M DSP intrinsics and inline assembly primitives Add src/include/sof/audio/arm_simd.h providing portable Cortex-M DSP intrinsics and inline assembly primitives: - Branchless saturation: arm_sat_s16, arm_sat_s24, arm_sat_q31 (SSAT) - Saturating arithmetic: arm_qadd16, arm_qsub16, arm_qadd, arm_qsub - 16-bit and 32-bit multipliers: arm_smulbb, arm_smultt, arm_smmulr - Multiply-accumulate: arm_smlal, arm_smlad, arm_smlald, arm_smlawb, arm_smlawt - Halfword packing and extension: arm_pkhbt, arm_sxth - Leading zero count: arm_clz - Generic C fallback implementations when building for non-ARM or host Signed-off-by: Liam Girdwood --- src/include/sof/audio/arm_simd.h | 419 +++++++++++++++++++++++++++++++ 1 file changed, 419 insertions(+) create mode 100644 src/include/sof/audio/arm_simd.h diff --git a/src/include/sof/audio/arm_simd.h b/src/include/sof/audio/arm_simd.h new file mode 100644 index 000000000000..34b282b39843 --- /dev/null +++ b/src/include/sof/audio/arm_simd.h @@ -0,0 +1,419 @@ +/* SPDX-License-Identifier: BSD-3-Clause + * + * Copyright(c) 2026 Sound Open Firmware. All rights reserved. + * + * Author: Liam Girdwood + */ + +#ifndef __SOF_AUDIO_ARM_SIMD_H__ +#define __SOF_AUDIO_ARM_SIMD_H__ + +#include +#include + +#if (defined(__arm__) || defined(__thumb__) || defined(CONFIG_ARM)) && \ + defined(__ARM_FEATURE_DSP) && (__ARM_FEATURE_DSP == 1) + +#if defined(__ZEPHYR__) +#include +#endif + +/* + * Branchless Saturation Primitives using ARM SSAT instruction + */ +static inline int16_t arm_sat_s16(int32_t val) +{ +#if defined(__GNUC__) + int32_t res; + __asm__("ssat %0, #16, %1" : "=r"(res) : "r"(val)); + return (int16_t)res; +#elif defined(__SSAT) + return (int16_t)__SSAT(val, 16); +#else + if (val > 32767) + return 32767; + if (val < -32768) + return -32768; + return (int16_t)val; +#endif +} + +static inline int32_t arm_sat_s24(int32_t val) +{ +#if defined(__GNUC__) + int32_t res; + __asm__("ssat %0, #24, %1" : "=r"(res) : "r"(val)); + return res; +#elif defined(__SSAT) + return __SSAT(val, 24); +#else + if (val > 8388607) + return 8388607; + if (val < -8388608) + return -8388608; + return val; +#endif +} + +static inline int32_t arm_sat_q31(int64_t val) +{ + if (__builtin_expect(val >= (int64_t)INT32_MIN && val <= (int64_t)INT32_MAX, 1)) + return (int32_t)val; + return (val > INT32_MAX) ? INT32_MAX : INT32_MIN; +} + +/* + * Dual 16-bit Arithmetic + */ +static inline uint32_t arm_qadd16(uint32_t a, uint32_t b) +{ +#if defined(__GNUC__) + uint32_t res; + __asm__("qadd16 %0, %1, %2" : "=r"(res) : "r"(a), "r"(b)); + return res; +#elif defined(__QADD16) + return __QADD16(a, b); +#else + int16_t a_l = (int16_t)(a & 0xffff); + int16_t a_h = (int16_t)(a >> 16); + int16_t b_l = (int16_t)(b & 0xffff); + int16_t b_h = (int16_t)(b >> 16); + int16_t r_l = arm_sat_s16((int32_t)a_l + b_l); + int16_t r_h = arm_sat_s16((int32_t)a_h + b_h); + return ((uint32_t)(uint16_t)r_h << 16) | (uint16_t)r_l; +#endif +} + +static inline uint32_t arm_qsub16(uint32_t a, uint32_t b) +{ +#if defined(__GNUC__) + uint32_t res; + __asm__("qsub16 %0, %1, %2" : "=r"(res) : "r"(a), "r"(b)); + return res; +#elif defined(__QSUB16) + return __QSUB16(a, b); +#else + int16_t a_l = (int16_t)(a & 0xffff); + int16_t a_h = (int16_t)(a >> 16); + int16_t b_l = (int16_t)(b & 0xffff); + int16_t b_h = (int16_t)(b >> 16); + int16_t r_l = arm_sat_s16((int32_t)a_l - b_l); + int16_t r_h = arm_sat_s16((int32_t)a_h - b_h); + return ((uint32_t)(uint16_t)r_h << 16) | (uint16_t)r_l; +#endif +} + +static inline int32_t arm_qadd(int32_t a, int32_t b) +{ +#if defined(__GNUC__) + int32_t res; + __asm__("qadd %0, %1, %2" : "=r"(res) : "r"(a), "r"(b)); + return res; +#elif defined(__QADD) + return __QADD(a, b); +#else + int64_t sum = (int64_t)a + b; + return arm_sat_q31(sum); +#endif +} + +static inline int32_t arm_qsub(int32_t a, int32_t b) +{ +#if defined(__GNUC__) + int32_t res; + __asm__("qsub %0, %1, %2" : "=r"(res) : "r"(a), "r"(b)); + return res; +#elif defined(__QSUB) + return __QSUB(a, b); +#else + int64_t diff = (int64_t)a - b; + return arm_sat_q31(diff); +#endif +} + +/* + * Signed 16-bit Multiply + */ +static inline int32_t arm_smulbb(int32_t a, int32_t b) +{ +#if defined(__GNUC__) + int32_t res; + __asm__("smulbb %0, %1, %2" : "=r"(res) : "r"(a), "r"(b)); + return res; +#else + return (int32_t)(int16_t)(a & 0xffff) * (int16_t)(b & 0xffff); +#endif +} + +static inline int32_t arm_smultt(int32_t a, int32_t b) +{ +#if defined(__GNUC__) + int32_t res; + __asm__("smultt %0, %1, %2" : "=r"(res) : "r"(a), "r"(b)); + return res; +#else + return (int32_t)(int16_t)(a >> 16) * (int16_t)(b >> 16); +#endif +} + +/* + * Signed Most Significant Word Multiply with Rounding (SMMULR) + * Computes: (a * b + 0x80000000) >> 32 in a single clock cycle. + * Ideal for Q1.31 audio scaling. + */ +static inline int32_t arm_smmulr(int32_t a, int32_t b) +{ +#if defined(__GNUC__) + int32_t res; + __asm__("smmulr %0, %1, %2" : "=r"(res) : "r"(a), "r"(b)); + return res; +#else + return (int32_t)(((int64_t)a * b + (1LL << 31)) >> 32); +#endif +} + +/* + * 64-bit Multiply-Accumulate (SMLAL) + * Computes: acc += (int64_t)a * b in 1 cycle throughput. + */ +static inline int64_t arm_smlal(int64_t acc, int32_t a, int32_t b) +{ +#if defined(__GNUC__) + uint32_t lo = (uint32_t)acc; + int32_t hi = (int32_t)(acc >> 32); + __asm__("smlal %0, %1, %2, %3" : "+r"(lo), "+r"(hi) : "r"(a), "r"(b)); + return ((int64_t)hi << 32) | lo; +#else + return acc + ((int64_t)a * b); +#endif +} + +/* + * 32x16 Word-Halfword Multiply Accumulate (SMLAWB / SMLAWT) + */ +static inline int32_t arm_smlawb(int32_t acc, int32_t a, int32_t b) +{ +#if defined(__GNUC__) + int32_t res; + __asm__("smlawb %0, %1, %2, %3" : "=r"(res) : "r"(a), "r"(b), "r"(acc)); + return res; +#else + return acc + (int32_t)(((int64_t)a * (int16_t)(b & 0xffff)) >> 16); +#endif +} + +static inline int32_t arm_smlawt(int32_t acc, int32_t a, int32_t b) +{ +#if defined(__GNUC__) + int32_t res; + __asm__("smlawt %0, %1, %2, %3" : "=r"(res) : "r"(a), "r"(b), "r"(acc)); + return res; +#else + return acc + (int32_t)(((int64_t)a * (int16_t)(b >> 16)) >> 16); +#endif +} + +/* + * Pack Halfwords (PKHBT / PKHTB) + */ +#if defined(__GNUC__) +#define arm_pkhbt(a, b, lsl_shift) \ +__extension__ ({ \ + uint32_t _res; \ + uint32_t _a = (a); \ + uint32_t _b = (b); \ + __asm__("pkhbt %0, %1, %2, lsl %3" : "=r"(_res) : "r"(_a), "r"(_b), "I"(lsl_shift)); \ + _res; \ +}) +#elif defined(__PKHBT) +#define arm_pkhbt(a, b, lsl_shift) __PKHBT(a, b, lsl_shift) +#else +static inline uint32_t arm_pkhbt(uint32_t a, uint32_t b, uint32_t lsl_shift) +{ + return (a & 0xffff) | ((b << lsl_shift) & 0xffff0000); +} +#endif + +/* + * Dual 16-bit Multiply-Accumulate (SMLAD / SMLALD) + */ +static inline int32_t arm_smlad(uint32_t a, uint32_t b, int32_t acc) +{ +#if defined(__GNUC__) + int32_t res; + __asm__("smlad %0, %1, %2, %3" : "=r"(res) : "r"(a), "r"(b), "r"(acc)); + return res; +#elif defined(__SMLAD) + return __SMLAD(a, b, acc); +#else + int16_t a0 = (int16_t)(a & 0xffff); + int16_t a1 = (int16_t)(a >> 16); + int16_t b0 = (int16_t)(b & 0xffff); + int16_t b1 = (int16_t)(b >> 16); + return acc + (int32_t)a0 * b0 + (int32_t)a1 * b1; +#endif +} + +static inline int64_t arm_smlald(uint32_t a, uint32_t b, int64_t acc) +{ +#if defined(__GNUC__) + uint32_t lo = (uint32_t)acc; + int32_t hi = (int32_t)(acc >> 32); + __asm__("smlald %0, %1, %2, %3" : "+r"(lo), "+r"(hi) : "r"(a), "r"(b)); + return ((int64_t)hi << 32) | lo; +#elif defined(__SMLALD) + return __SMLALD(a, b, acc); +#else + int16_t a0 = (int16_t)(a & 0xffff); + int16_t a1 = (int16_t)(a >> 16); + int16_t b0 = (int16_t)(b & 0xffff); + int16_t b1 = (int16_t)(b >> 16); + return acc + ((int64_t)a0 * b0) + ((int64_t)a1 * b1); +#endif +} + +/* + * Sign Extend Halfword (SXTH) + */ +static inline int32_t arm_sxth(int32_t val) +{ +#if defined(__GNUC__) + int32_t res; + __asm__("sxth %0, %1" : "=r"(res) : "r"(val)); + return res; +#elif defined(__SXTH) + return __SXTH(val); +#else + return (int32_t)(int16_t)(val & 0xffff); +#endif +} + +/* + * Count Leading Zeros + */ +static inline uint32_t arm_clz(uint32_t val) +{ + return (uint32_t)__builtin_clz(val); +} + +#else /* Non-ARM fallback */ + +static inline int16_t arm_sat_s16(int32_t val) +{ + if (val > 32767) return 32767; + if (val < -32768) return -32768; + return (int16_t)val; +} + +static inline int32_t arm_sat_s24(int32_t val) +{ + if (val > 8388607) return 8388607; + if (val < -8388608) return -8388608; + return val; +} + +static inline int32_t arm_sat_q31(int64_t val) +{ + if (val > INT32_MAX) return INT32_MAX; + if (val < INT32_MIN) return INT32_MIN; + return (int32_t)val; +} + +static inline uint32_t arm_qadd16(uint32_t a, uint32_t b) +{ + int16_t a_l = (int16_t)(a & 0xffff); + int16_t a_h = (int16_t)(a >> 16); + int16_t b_l = (int16_t)(b & 0xffff); + int16_t b_h = (int16_t)(b >> 16); + int16_t r_l = arm_sat_s16((int32_t)a_l + b_l); + int16_t r_h = arm_sat_s16((int32_t)a_h + b_h); + return ((uint32_t)(uint16_t)r_h << 16) | (uint16_t)r_l; +} + +static inline uint32_t arm_qsub16(uint32_t a, uint32_t b) +{ + int16_t a_l = (int16_t)(a & 0xffff); + int16_t a_h = (int16_t)(a >> 16); + int16_t b_l = (int16_t)(b & 0xffff); + int16_t b_h = (int16_t)(b >> 16); + int16_t r_l = arm_sat_s16((int32_t)a_l - b_l); + int16_t r_h = arm_sat_s16((int32_t)a_h - b_h); + return ((uint32_t)(uint16_t)r_h << 16) | (uint16_t)r_l; +} + +static inline int32_t arm_qadd(int32_t a, int32_t b) +{ + return arm_sat_q31((int64_t)a + b); +} + +static inline int32_t arm_qsub(int32_t a, int32_t b) +{ + return arm_sat_q31((int64_t)a - b); +} + +static inline int32_t arm_smulbb(int32_t a, int32_t b) +{ + return (int32_t)(int16_t)(a & 0xffff) * (int16_t)(b & 0xffff); +} + +static inline int32_t arm_smultt(int32_t a, int32_t b) +{ + return (int32_t)(int16_t)(a >> 16) * (int16_t)(b >> 16); +} + +static inline int32_t arm_smmulr(int32_t a, int32_t b) +{ + return (int32_t)(((int64_t)a * b + (1LL << 31)) >> 32); +} + +static inline int64_t arm_smlal(int64_t acc, int32_t a, int32_t b) +{ + return acc + ((int64_t)a * b); +} + +static inline uint32_t arm_pkhbt(uint32_t a, uint32_t b, uint32_t lsl_shift) +{ + return (a & 0xffff) | ((b << lsl_shift) & 0xffff0000); +} + +static inline int32_t arm_smlad(uint32_t a, uint32_t b, int32_t acc) +{ + int16_t a0 = (int16_t)(a & 0xffff); + int16_t a1 = (int16_t)(a >> 16); + int16_t b0 = (int16_t)(b & 0xffff); + int16_t b1 = (int16_t)(b >> 16); + return acc + (int32_t)a0 * b0 + (int32_t)a1 * b1; +} + +static inline int64_t arm_smlald(uint32_t a, uint32_t b, int64_t acc) +{ + int16_t a0 = (int16_t)(a & 0xffff); + int16_t a1 = (int16_t)(a >> 16); + int16_t b0 = (int16_t)(b & 0xffff); + int16_t b1 = (int16_t)(b >> 16); + return acc + ((int64_t)a0 * b0) + ((int64_t)a1 * b1); +} + +static inline int32_t arm_smlawb(int32_t acc, int32_t a, int32_t b) +{ + return acc + (int32_t)(((int64_t)a * (int16_t)(b & 0xffff)) >> 16); +} + +static inline int32_t arm_smlawt(int32_t acc, int32_t a, int32_t b) +{ + return acc + (int32_t)(((int64_t)a * (int16_t)(b >> 16)) >> 16); +} + +static inline int32_t arm_sxth(int32_t val) +{ + return (int32_t)(int16_t)(val & 0xffff); +} + +static inline uint32_t arm_clz(uint32_t val) +{ + return (uint32_t)__builtin_clz(val); +} + +#endif /* defined(__arm__) || defined(__thumb__) || defined(CONFIG_ARM) */ + +#endif /* __SOF_AUDIO_ARM_SIMD_H__ */ From 5e72a6da88161a43ef3a7621056b2fbcf79ec78a Mon Sep 17 00:00:00 2001 From: Liam Girdwood Date: Tue, 13 Oct 2026 14:40:58 +0100 Subject: [PATCH 03/13] math: iir: add ARM Cortex-M DSP cascaded DF1 and DF2T biquad filtering Implement Direct Form 1 (DF1) and Transposed Direct Form 2 (DF2T) biquad filtering kernels optimized for ARM Cortex-M DSP architectures: - Single-cycle 5-term MAC throughput using SMLAL 64-bit accumulators. - Branchless saturation with ARM SSAT (arm_sat_q31). - Specialized 4th order (2 biquads in series) unrolled kernel. - Dispatch headers iir_df1_arm.h and iir_df2t_arm.h for fast inlining. - Add CONFIG_FILTER_ARM_SIMD choice option in math Kconfig and Kconfig.simd. Signed-off-by: Liam Girdwood --- src/include/sof/math/iir_df1.h | 2 + src/include/sof/math/iir_df1_arm.h | 35 ++++++ src/include/sof/math/iir_df2t.h | 2 + src/include/sof/math/iir_df2t_arm.h | 35 ++++++ src/math/CMakeLists.txt | 4 +- src/math/Kconfig | 11 +- src/math/Kconfig.simd | 11 +- src/math/iir_df1_arm.c | 169 ++++++++++++++++++++++++++++ src/math/iir_df2t_arm.c | 79 +++++++++++++ 9 files changed, 344 insertions(+), 4 deletions(-) create mode 100644 src/include/sof/math/iir_df1_arm.h create mode 100644 src/include/sof/math/iir_df2t_arm.h create mode 100644 src/math/iir_df1_arm.c create mode 100644 src/math/iir_df2t_arm.c diff --git a/src/include/sof/math/iir_df1.h b/src/include/sof/math/iir_df1.h index 12a71535bcf8..c6c66d1c8c59 100644 --- a/src/include/sof/math/iir_df1.h +++ b/src/include/sof/math/iir_df1.h @@ -82,6 +82,8 @@ int32_t iir_df1_4th(struct iir_state_df1 *iir, int32_t x); /* Inline functions */ #if SOF_USE_MIN_HIFI(3, FILTER) #include "iir_df1_hifi3.h" +#elif SOF_USE_ARM_SIMD(FILTER) +#include "iir_df1_arm.h" #else #include "iir_df1_generic.h" #endif diff --git a/src/include/sof/math/iir_df1_arm.h b/src/include/sof/math/iir_df1_arm.h new file mode 100644 index 000000000000..68a8e88fe57f --- /dev/null +++ b/src/include/sof/math/iir_df1_arm.h @@ -0,0 +1,35 @@ +/* SPDX-License-Identifier: BSD-3-Clause + * + * Copyright(c) 2026 Sound Open Firmware. All rights reserved. + * + * Author: Liam Girdwood + */ + +#ifndef __IIR_DF1_ARM_H__ +#define __IIR_DF1_ARM_H__ + +#include +#include +#include + +static inline int16_t iir_df1_s16(struct iir_state_df1 *iir, int16_t x) +{ + return arm_sat_s16(Q_SHIFT_RND(iir_df1(iir, ((int32_t)x) << 16), 31, 15)); +} + +static inline int32_t iir_df1_s24(struct iir_state_df1 *iir, int32_t x) +{ + return arm_sat_s24(Q_SHIFT_RND(iir_df1(iir, x << 8), 31, 23)); +} + +static inline int16_t iir_df1_s32_s16(struct iir_state_df1 *iir, int32_t x) +{ + return arm_sat_s16(Q_SHIFT_RND(iir_df1(iir, x), 31, 15)); +} + +static inline int32_t iir_df1_s32_s24(struct iir_state_df1 *iir, int32_t x) +{ + return arm_sat_s24(Q_SHIFT_RND(iir_df1(iir, x), 31, 23)); +} + +#endif /* __IIR_DF1_ARM_H__ */ diff --git a/src/include/sof/math/iir_df2t.h b/src/include/sof/math/iir_df2t.h index a6b6c3d1802f..b238b4f018d9 100644 --- a/src/include/sof/math/iir_df2t.h +++ b/src/include/sof/math/iir_df2t.h @@ -51,6 +51,8 @@ int32_t iir_df2t(struct iir_state_df2t *iir, int32_t x); /* Inline functions with or without HiFi3 intrinsics */ #if SOF_USE_HIFI(3, FILTER) || SOF_USE_HIFI(4, FILTER) #include "iir_df2t_hifi3.h" +#elif SOF_USE_ARM_SIMD(FILTER) +#include "iir_df2t_arm.h" #else #include "iir_df2t_generic.h" #endif diff --git a/src/include/sof/math/iir_df2t_arm.h b/src/include/sof/math/iir_df2t_arm.h new file mode 100644 index 000000000000..8d5bfd8ebb35 --- /dev/null +++ b/src/include/sof/math/iir_df2t_arm.h @@ -0,0 +1,35 @@ +/* SPDX-License-Identifier: BSD-3-Clause + * + * Copyright(c) 2026 Sound Open Firmware. All rights reserved. + * + * Author: Liam Girdwood + */ + +#ifndef __IIR_DF2T_ARM_H__ +#define __IIR_DF2T_ARM_H__ + +#include +#include +#include + +static inline int16_t iir_df2t_s16(struct iir_state_df2t *iir, int16_t x) +{ + return arm_sat_s16(Q_SHIFT_RND(iir_df2t(iir, ((int32_t)x) << 16), 31, 15)); +} + +static inline int32_t iir_df2t_s24(struct iir_state_df2t *iir, int32_t x) +{ + return arm_sat_s24(Q_SHIFT_RND(iir_df2t(iir, x << 8), 31, 23)); +} + +static inline int16_t iir_df2t_s32_s16(struct iir_state_df2t *iir, int32_t x) +{ + return arm_sat_s16(Q_SHIFT_RND(iir_df2t(iir, x), 31, 15)); +} + +static inline int32_t iir_df2t_s32_s24(struct iir_state_df2t *iir, int32_t x) +{ + return arm_sat_s24(Q_SHIFT_RND(iir_df2t(iir, x), 31, 23)); +} + +#endif /* __IIR_DF2T_ARM_H__ */ diff --git a/src/math/CMakeLists.txt b/src/math/CMakeLists.txt index a52263006295..7bf933b95bdf 100644 --- a/src/math/CMakeLists.txt +++ b/src/math/CMakeLists.txt @@ -63,11 +63,11 @@ if(CONFIG_MATH_IIR STREQUAL "m" AND DEFINED CONFIG_LLEXT) add_dependencies(app aux1_iir) elseif(CONFIG_MATH_IIR) if(CONFIG_MATH_IIR_DF2T) - list(APPEND base_files iir_df2t_generic.c iir_df2t_hifi3.c iir_df2t.c) + list(APPEND base_files iir_df2t_generic.c iir_df2t_hifi3.c iir_df2t_arm.c iir_df2t.c) endif() if(CONFIG_MATH_IIR_DF1) - list(APPEND base_files iir_df1_generic.c iir_df1_hifi3.c iir_df1_hifi4.c iir_df1_hifi5.c iir_df1.c) + list(APPEND base_files iir_df1_generic.c iir_df1_hifi3.c iir_df1_hifi4.c iir_df1_hifi5.c iir_df1_arm.c iir_df1.c) endif() endif() diff --git a/src/math/Kconfig b/src/math/Kconfig index 1feaab8f03d1..81ad5984d09d 100644 --- a/src/math/Kconfig +++ b/src/math/Kconfig @@ -159,7 +159,16 @@ endmenu choice "FILTER_SIMD_LEVEL_SELECT" prompt "choose which SIMD level used for IIR/FIR/TDFB module" depends on COMP_FIR || COMP_IIR || COMP_TDFB - default FILTER_HIFI_MAX + default FILTER_HIFI_MAX if XTENSA + default FILTER_ARM_SIMD if ARM + default FILTER_HIFI_NONE + + config FILTER_ARM_SIMD + prompt "choose ARM SIMD & DSP optimized FILTER module" + bool + depends on ARM + help + This option used to build ARMv7E-M DSP / SIMD optimized FILTER code config FILTER_HIFI_MAX prompt "SIMD will selected by toolchain pre-defined header" diff --git a/src/math/Kconfig.simd b/src/math/Kconfig.simd index 6521259be116..634b54219ca7 100644 --- a/src/math/Kconfig.simd +++ b/src/math/Kconfig.simd @@ -7,7 +7,16 @@ choice "FILTER_SIMD_LEVEL_SELECT" prompt "choose which SIMD level used for filter module" depends on COMP_FIR depends on COMP_IIR - default FILTER_HIFI_MAX + default FILTER_HIFI_MAX if XTENSA + default FILTER_ARM_SIMD if ARM + default FILTER_HIFI_NONE + + config FILTER_ARM_SIMD + prompt "choose ARM SIMD & DSP optimized FILTER module" + bool + depends on ARM + help + This option used to build ARMv7E-M DSP / SIMD optimized FILTER code config FILTER_HIFI_MAX prompt "SIMD will selected by toolchain pre-defined header" diff --git a/src/math/iir_df1_arm.c b/src/math/iir_df1_arm.c new file mode 100644 index 000000000000..71b2d74118f6 --- /dev/null +++ b/src/math/iir_df1_arm.c @@ -0,0 +1,169 @@ +// SPDX-License-Identifier: BSD-3-Clause +// +// Copyright(c) 2026 Sound Open Firmware. All rights reserved. +// +// Author: Liam Girdwood + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#if SOF_USE_ARM_SIMD(FILTER) + +/* Series DF1 IIR optimized for ARM Cortex-M7 */ +int32_t iir_df1(struct iir_state_df1 *iir, int32_t x) +{ + int32_t in; + int32_t tmp; + int64_t out = 0; + int64_t acc; + int i; + int j; + int d = 0; + int c = 0; + const int32_t *coefp = iir->coef; + int32_t *delay = iir->delay; + const int nseries = iir->biquads_in_series; + + if (!iir->biquads) + return x; + + for (j = 0; j < (int)iir->biquads; j += nseries) { + in = x; + for (i = 0; i < nseries; i++) { + /* Load state and coefficients */ + const int32_t a2 = coefp[c]; + const int32_t a1 = coefp[c + 1]; + const int32_t b2 = coefp[c + 2]; + const int32_t b1 = coefp[c + 3]; + const int32_t b0 = coefp[c + 4]; + const int32_t shift_param = coefp[c + 5]; + const int32_t gain = coefp[c + 6]; + + const int32_t y_n2 = delay[d]; + const int32_t y_n1 = delay[d + 1]; + const int32_t x_n2 = delay[d + 2]; + const int32_t x_n1 = delay[d + 3]; + + /* 5-term MAC accumulator using ARM SMLAL */ + acc = arm_smlal(0, a2, y_n2); + acc = arm_smlal(acc, a1, y_n1); + acc = arm_smlal(acc, b2, x_n2); + acc = arm_smlal(acc, b1, x_n1); + acc = arm_smlal(acc, b0, in); + + /* Shift Q3.61 to Q3.31 with symmetric rounding */ + acc = (acc + (1LL << 29)) >> 30; + tmp = arm_sat_q31(acc); + + /* Update delay line */ + delay[d] = y_n1; + delay[d + 1] = tmp; + delay[d + 2] = x_n1; + delay[d + 3] = in; + + /* Apply biquad gain & shift */ + acc = ((int64_t)gain) * tmp; + const int shift_bits = 14 + shift_param; + acc = (acc + (1LL << (shift_bits - 1))) >> shift_bits; + in = arm_sat_q31(acc); + + c += SOF_EQ_IIR_NBIQUAD; + d += IIR_DF1_NUM_STATE; + } + out += in; + } + return arm_sat_q31(out); +} +EXPORT_SYMBOL(iir_df1); + +/* Specialized 4th order (2 biquads in series) for ARM Cortex-M7 */ +int32_t iir_df1_4th(struct iir_state_df1 *iir, int32_t x) +{ + int32_t in = x; + int32_t tmp; + int64_t acc; + const int32_t *coefp = iir->coef; + int32_t *delay = iir->delay; + + /* Biquad 0 */ + { + const int32_t a2 = coefp[0]; + const int32_t a1 = coefp[1]; + const int32_t b2 = coefp[2]; + const int32_t b1 = coefp[3]; + const int32_t b0 = coefp[4]; + const int32_t shift_param = coefp[5]; + const int32_t gain = coefp[6]; + + const int32_t y_n2 = delay[0]; + const int32_t y_n1 = delay[1]; + const int32_t x_n2 = delay[2]; + const int32_t x_n1 = delay[3]; + + acc = arm_smlal(0, a2, y_n2); + acc = arm_smlal(acc, a1, y_n1); + acc = arm_smlal(acc, b2, x_n2); + acc = arm_smlal(acc, b1, x_n1); + acc = arm_smlal(acc, b0, in); + + acc = (acc + (1LL << 29)) >> 30; + tmp = arm_sat_q31(acc); + + delay[0] = y_n1; + delay[1] = tmp; + delay[2] = x_n1; + delay[3] = in; + + acc = ((int64_t)gain) * tmp; + const int shift_bits = 14 + shift_param; + acc = (acc + (1LL << (shift_bits - 1))) >> shift_bits; + in = arm_sat_q31(acc); + } + + /* Biquad 1 */ + { + const int32_t a2 = coefp[7]; + const int32_t a1 = coefp[8]; + const int32_t b2 = coefp[9]; + const int32_t b1 = coefp[10]; + const int32_t b0 = coefp[11]; + const int32_t shift_param = coefp[12]; + const int32_t gain = coefp[13]; + + const int32_t y_n2 = delay[4]; + const int32_t y_n1 = delay[5]; + const int32_t x_n2 = delay[6]; + const int32_t x_n1 = delay[7]; + + acc = arm_smlal(0, a2, y_n2); + acc = arm_smlal(acc, a1, y_n1); + acc = arm_smlal(acc, b2, x_n2); + acc = arm_smlal(acc, b1, x_n1); + acc = arm_smlal(acc, b0, in); + + acc = (acc + (1LL << 29)) >> 30; + tmp = arm_sat_q31(acc); + + delay[4] = y_n1; + delay[5] = tmp; + delay[6] = x_n1; + delay[7] = in; + + acc = ((int64_t)gain) * tmp; + const int shift_bits = 14 + shift_param; + acc = (acc + (1LL << (shift_bits - 1))) >> shift_bits; + in = arm_sat_q31(acc); + } + + return in; +} +EXPORT_SYMBOL(iir_df1_4th); + +#endif /* SOF_USE_ARM_SIMD(FILTER) */ diff --git a/src/math/iir_df2t_arm.c b/src/math/iir_df2t_arm.c new file mode 100644 index 000000000000..6c2a45a37e81 --- /dev/null +++ b/src/math/iir_df2t_arm.c @@ -0,0 +1,79 @@ +// SPDX-License-Identifier: BSD-3-Clause +// +// Copyright(c) 2026 Sound Open Firmware. All rights reserved. +// +// Author: Liam Girdwood + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#if SOF_USE_ARM_SIMD(FILTER) + +int32_t iir_df2t(struct iir_state_df2t *iir, int32_t x) +{ + int32_t in; + int32_t tmp; + int64_t acc; + int64_t out = 0; + int i; + int j; + int d = 0; + int c = 0; + const int32_t *coefp = iir->coef; + int64_t *delay = iir->delay; + const int nseries = iir->biquads_in_series; + + if (!iir->biquads) + return x; + + for (j = 0; j < (int)iir->biquads; j += nseries) { + in = x; + for (i = 0; i < nseries; i++) { + const int32_t a2 = coefp[c]; + const int32_t a1 = coefp[c + 1]; + const int32_t b2 = coefp[c + 2]; + const int32_t b1 = coefp[c + 3]; + const int32_t b0 = coefp[c + 4]; + const int32_t shift_param = coefp[c + 5]; + const int32_t gain = coefp[c + 6]; + + /* Compute output */ + acc = delay[d]; + acc = arm_smlal(acc, b0, in); + acc = (acc + (1LL << 29)) >> 30; + tmp = arm_sat_q31(acc); + + /* Compute first delay */ + acc = delay[d + 1]; + acc = arm_smlal(acc, b1, in); + acc = arm_smlal(acc, a1, tmp); + delay[d] = acc; + + /* Compute second delay */ + acc = arm_smlal(0, b2, in); + acc = arm_smlal(acc, a2, tmp); + delay[d + 1] = acc; + + /* Apply gain & shift */ + acc = ((int64_t)gain) * tmp; + const int shift_bits = 14 + shift_param; + acc = (acc + (1LL << (shift_bits - 1))) >> shift_bits; + in = arm_sat_q31(acc); + + c += SOF_EQ_IIR_NBIQUAD; + d += IIR_DF2T_NUM_DELAYS; + } + out += in; + } + return arm_sat_q31(out); +} +EXPORT_SYMBOL(iir_df2t); + +#endif /* SOF_USE_ARM_SIMD(FILTER) */ From bee9c5c050f47e91493a3bdfb20ae9df28b754de Mon Sep 17 00:00:00 2001 From: Liam Girdwood Date: Tue, 13 Oct 2026 14:41:22 +0100 Subject: [PATCH 04/13] math: fir: add ARM Cortex-M DSP unrolled FIR filtering Implement unrolled FIR filtering kernels in fir_arm.c optimized for ARM Cortex-M DSP architectures: - Single-cycle SMLAL 64-bit MAC accumulator. - 4-tap unrolled loop for fir_32x16. - Dual-sample parallel filtering kernel fir_32x16_2x. - Guard generic fallback definitions in fir_generic.h. Signed-off-by: Liam Girdwood --- src/include/sof/math/fir_generic.h | 2 +- src/math/CMakeLists.txt | 2 +- src/math/fir_arm.c | 256 +++++++++++++++++++++++++++++ 3 files changed, 258 insertions(+), 2 deletions(-) create mode 100644 src/math/fir_arm.c diff --git a/src/include/sof/math/fir_generic.h b/src/include/sof/math/fir_generic.h index 79be1f575ff7..b1aab2b6392c 100644 --- a/src/include/sof/math/fir_generic.h +++ b/src/include/sof/math/fir_generic.h @@ -13,7 +13,7 @@ #include #include -#if SOF_USE_HIFI(NONE, FILTER) +#if SOF_USE_HIFI(NONE, FILTER) || SOF_USE_ARM_SIMD(FILTER) #include #include diff --git a/src/math/CMakeLists.txt b/src/math/CMakeLists.txt index 7bf933b95bdf..00052df7cf6b 100644 --- a/src/math/CMakeLists.txt +++ b/src/math/CMakeLists.txt @@ -51,7 +51,7 @@ if(CONFIG_MATH_FIR STREQUAL "m" AND DEFINED CONFIG_LLEXT) add_subdirectory(fir_llext ${PROJECT_BINARY_DIR}/aux1_fir_llext) add_dependencies(app aux1_fir) elseif(CONFIG_MATH_FIR) - list(APPEND base_files fir_generic.c fir_hifi2ep.c fir_hifi3.c fir_hifi5.c) + list(APPEND base_files fir_generic.c fir_hifi2ep.c fir_hifi3.c fir_hifi5.c fir_arm.c) endif() if(CONFIG_MATH_FFT) diff --git a/src/math/fir_arm.c b/src/math/fir_arm.c new file mode 100644 index 000000000000..fdbe7ce83fd4 --- /dev/null +++ b/src/math/fir_arm.c @@ -0,0 +1,256 @@ +// SPDX-License-Identifier: BSD-3-Clause +// +// Copyright(c) 2026 Sound Open Firmware. All rights reserved. +// +// Author: Liam Girdwood + +#include +#include + +#if SOF_USE_ARM_SIMD(FILTER) + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/* + * EQ FIR ARMv7E-M DSP accelerated code + */ + +void fir_reset(struct fir_state_32x16 *fir) +{ + fir->rwi = 0; + fir->length = 0; + fir->out_shift = 0; + fir->coef = NULL; +} +EXPORT_SYMBOL(fir_reset); + +int fir_delay_size(struct sof_fir_coef_data *config) +{ + if (config->length > SOF_FIR_MAX_LENGTH || config->length < 4) + return -EINVAL; + + if (config->length & 0x3) + return -EINVAL; + + return (config->length + 4) * sizeof(int32_t); +} +EXPORT_SYMBOL(fir_delay_size); + +int fir_init_coef(struct fir_state_32x16 *fir, + struct sof_fir_coef_data *config) +{ + fir->rwi = 0; + fir->taps = (int)config->length; + fir->length = (int)fir->taps + 2; + fir->out_shift = (int)config->out_shift; + fir->coef = ASSUME_ALIGNED(&config->coef[0], 4); + return 0; +} +EXPORT_SYMBOL(fir_init_coef); + +void fir_init_delay(struct fir_state_32x16 *fir, int32_t **data) +{ + fir->delay = *data; + *data += fir->length; +} +EXPORT_SYMBOL(fir_init_delay); + +int32_t fir_32x16(struct fir_state_32x16 *fir, int32_t x) +{ + int64_t y = 0; + int32_t *data = &fir->delay[fir->rwi]; + const int16_t *coef = &fir->coef[0]; + int n1, n2, n; + const int length = fir->length; + const int taps = fir->taps; + const int shift = 15 + fir->out_shift; + + if (!fir->length) + return x; + + *data = x; + + n1 = ++fir->rwi; + if (fir->rwi == length) + fir->rwi = 0; + + /* Part 1: before circular wrap */ + n1 = MIN(n1, taps); + int n1_4 = n1 & ~3; + for (n = 0; n < n1_4; n += 4) { + int16_t c0 = coef[0]; + int16_t c1 = coef[1]; + int16_t c2 = coef[2]; + int16_t c3 = coef[3]; + coef += 4; + + int32_t d0 = data[0]; + int32_t d1 = data[-1]; + int32_t d2 = data[-2]; + int32_t d3 = data[-3]; + data -= 4; + + y = arm_smlal(y, c0, d0); + y = arm_smlal(y, c1, d1); + y = arm_smlal(y, c2, d2); + y = arm_smlal(y, c3, d3); + } + for (; n < n1; n++) { + y = arm_smlal(y, *coef++, *data--); + } + + /* Part 2: un-wrap data, continue n2 times */ + n2 = taps - n1; + data = &fir->delay[length - 1]; + int n2_4 = n2 & ~3; + for (n = 0; n < n2_4; n += 4) { + int16_t c0 = coef[0]; + int16_t c1 = coef[1]; + int16_t c2 = coef[2]; + int16_t c3 = coef[3]; + coef += 4; + + int32_t d0 = data[0]; + int32_t d1 = data[-1]; + int32_t d2 = data[-2]; + int32_t d3 = data[-3]; + data -= 4; + + y = arm_smlal(y, c0, d0); + y = arm_smlal(y, c1, d1); + y = arm_smlal(y, c2, d2); + y = arm_smlal(y, c3, d3); + } + for (; n < n2; n++) { + y = arm_smlal(y, *coef++, *data--); + } + + return arm_sat_q31(y >> shift); +} +EXPORT_SYMBOL(fir_32x16); + +void fir_32x16_2x(struct fir_state_32x16 *fir, int32_t x0, int32_t x1, int32_t *y0, int32_t *y1) +{ + int64_t a0 = 0; + int64_t a1 = 0; + int32_t sample1; + int32_t *data; + const int16_t *coef = &fir->coef[0]; + int n1, n2, i; + const int length = fir->length; + const int taps = fir->taps; + const int shift = 15 + fir->out_shift; + + if (!fir->taps) { + *y0 = x0; + *y1 = x1; + return; + } + + const int rwi0 = fir->rwi; + int rwi1 = rwi0 + 1; + if (rwi1 >= length) + rwi1 = 0; + + fir->delay[rwi0] = x0; + fir->delay[rwi1] = x1; + + fir->rwi = rwi1 + 1; + if (fir->rwi >= length) + fir->rwi = 0; + + data = &fir->delay[rwi0]; + n1 = rwi0 + 1; + + /* Part 1: before circular wrap */ + sample1 = x1; + n1 = MIN(n1, taps); + int n1_4 = n1 & ~3; + for (i = 0; i < n1_4; i += 4) { + int16_t c0 = coef[0]; + int16_t c1 = coef[1]; + int16_t c2 = coef[2]; + int16_t c3 = coef[3]; + coef += 4; + + int32_t d0 = data[0]; + int32_t d1 = data[-1]; + int32_t d2 = data[-2]; + int32_t d3 = data[-3]; + data -= 4; + + a1 = arm_smlal(a1, c0, sample1); + a0 = arm_smlal(a0, c0, d0); + + a1 = arm_smlal(a1, c1, d0); + a0 = arm_smlal(a0, c1, d1); + + a1 = arm_smlal(a1, c2, d1); + a0 = arm_smlal(a0, c2, d2); + + a1 = arm_smlal(a1, c3, d2); + a0 = arm_smlal(a0, c3, d3); + + sample1 = d3; + } + for (; i < n1; i++) { + int16_t tap = *coef++; + int32_t sample0 = *data--; + a1 = arm_smlal(a1, tap, sample1); + a0 = arm_smlal(a0, tap, sample0); + sample1 = sample0; + } + + /* Part 2: un-wrap data, continue n2 times */ + n2 = taps - n1; + data = &fir->delay[length - 1]; + int n2_4 = n2 & ~3; + for (i = 0; i < n2_4; i += 4) { + int16_t c0 = coef[0]; + int16_t c1 = coef[1]; + int16_t c2 = coef[2]; + int16_t c3 = coef[3]; + coef += 4; + + int32_t d0 = data[0]; + int32_t d1 = data[-1]; + int32_t d2 = data[-2]; + int32_t d3 = data[-3]; + data -= 4; + + a1 = arm_smlal(a1, c0, sample1); + a0 = arm_smlal(a0, c0, d0); + + a1 = arm_smlal(a1, c1, d0); + a0 = arm_smlal(a0, c1, d1); + + a1 = arm_smlal(a1, c2, d1); + a0 = arm_smlal(a0, c2, d2); + + a1 = arm_smlal(a1, c3, d2); + a0 = arm_smlal(a0, c3, d3); + + sample1 = d3; + } + for (; i < n2; i++) { + int16_t tap = *coef++; + int32_t sample0 = *data--; + a1 = arm_smlal(a1, tap, sample1); + a0 = arm_smlal(a0, tap, sample0); + sample1 = sample0; + } + + *y0 = arm_sat_q31(a0 >> shift); + *y1 = arm_sat_q31(a1 >> shift); +} +EXPORT_SYMBOL(fir_32x16_2x); + +#endif /* SOF_USE_ARM_SIMD(FILTER) */ From 89c1a58614b322ac8bb7b963692705b5f312ccd0 Mon Sep 17 00:00:00 2001 From: Liam Girdwood Date: Tue, 13 Oct 2026 14:41:44 +0100 Subject: [PATCH 05/13] audio: volume: add ARM Cortex-M DSP packed vector volume scaling Accelerate volume scaling on ARM Cortex-M DSP architectures: - Add CONFIG_VOLUME_ARM_SIMD choice option in Kconfig.simd. - Implement volume_arm.c and volume_arm_with_peakvol.c: * Packed S16 dual halfword unpack/multiply/saturate/pack using SMULBB, SMULTT, arm_sat_s16, and PKHBT. * Fast S24 and S32 scaling using SMMULR / arm_smmulr. * Single-precision floating point processing with hardware FPU. * Optimized peak volume tracking in volume_arm_with_peakvol.c. Signed-off-by: Liam Girdwood --- src/audio/volume/CMakeLists.txt | 27 +- src/audio/volume/Kconfig.simd | 11 +- src/audio/volume/volume_arm.c | 466 ++++++++++++++ src/audio/volume/volume_arm_with_peakvol.c | 695 +++++++++++++++++++++ 4 files changed, 1188 insertions(+), 11 deletions(-) create mode 100644 src/audio/volume/volume_arm.c create mode 100644 src/audio/volume/volume_arm_with_peakvol.c diff --git a/src/audio/volume/CMakeLists.txt b/src/audio/volume/CMakeLists.txt index 34491132ee2c..d92a27f14cff 100644 --- a/src/audio/volume/CMakeLists.txt +++ b/src/audio/volume/CMakeLists.txt @@ -4,16 +4,23 @@ if(CONFIG_COMP_VOLUME STREQUAL "m" AND DEFINED CONFIG_LLEXT) add_subdirectory(llext ${PROJECT_BINARY_DIR}/volume_llext) add_dependencies(app volume) else() - add_local_sources(sof - volume_generic.c - volume_hifi3.c - volume_hifi4.c - volume_hifi5.c - volume_generic_with_peakvol.c - volume_hifi3_with_peakvol.c - volume_hifi4_with_peakvol.c - volume_hifi5_with_peakvol.c - volume.c) + if(CONFIG_VOLUME_ARM_SIMD) + add_local_sources(sof + volume_arm.c + volume_arm_with_peakvol.c + volume.c) + else() + add_local_sources(sof + volume_generic.c + volume_hifi3.c + volume_hifi4.c + volume_hifi5.c + volume_generic_with_peakvol.c + volume_hifi3_with_peakvol.c + volume_hifi4_with_peakvol.c + volume_hifi5_with_peakvol.c + volume.c) + endif() if(CONFIG_IPC_MAJOR_3) add_local_sources(sof volume_ipc3.c) elseif(CONFIG_IPC_MAJOR_4) diff --git a/src/audio/volume/Kconfig.simd b/src/audio/volume/Kconfig.simd index 88dd09cfb57a..03f2bc8beb5a 100644 --- a/src/audio/volume/Kconfig.simd +++ b/src/audio/volume/Kconfig.simd @@ -5,7 +5,16 @@ comment "volume optimization level select" choice "VOLUME_SIMD_LEVEL_SELECT" prompt "choose which SIMD level used for volume module" depends on COMP_VOLUME - default VOLUME_HIFI_MAX + default VOLUME_HIFI_MAX if XTENSA + default VOLUME_ARM_SIMD if ARM + default VOLUME_HIFI_NONE + + config VOLUME_ARM_SIMD + prompt "choose ARM SIMD & DSP vector optimized volume module" + bool + depends on ARM + help + This option used to build ARMv7E-M DSP / SIMD optimized volume code. config VOLUME_HIFI_MAX prompt "SIMD will selected by toolchain pre-defined header" diff --git a/src/audio/volume/volume_arm.c b/src/audio/volume/volume_arm.c new file mode 100644 index 000000000000..c9d51d62b6b4 --- /dev/null +++ b/src/audio/volume/volume_arm.c @@ -0,0 +1,466 @@ +// SPDX-License-Identifier: BSD-3-Clause +// +// Copyright(c) 2026 Sound Open Firmware. All rights reserved. +// +// Author: Liam Girdwood + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +LOG_MODULE_DECLARE(volume_arm, CONFIG_SOF_LOG_LEVEL); + +#include "volume.h" + +#if (!CONFIG_COMP_PEAK_VOL) + +#if CONFIG_FORMAT_S16LE +static inline int16_t vol_mult_s16(int16_t x, int32_t vol) +{ +#if COMP_VOLUME_Q8_16 + const int64_t prod = ((int64_t)x * vol + 0x8000) >> 16; + return arm_sat_s16(arm_sat_q31(prod)); +#elif COMP_VOLUME_Q1_31 + const int64_t prod = ((int64_t)x * vol + (1LL << 30)) >> 31; + return arm_sat_s16((int32_t)prod); +#else + const int64_t prod = ((int64_t)x * vol + (1LL << 22)) >> 23; + return arm_sat_s16((int32_t)prod); +#endif +} + +static void vol_s16_to_s16(struct processing_module *mod, struct cir_buf_source *source, + struct cir_buf_sink *sink, uint32_t frames, uint32_t attenuation) +{ + struct vol_data *cd = module_get_private_data(mod); + const int16_t *x = source->ptr; + int16_t *y = sink->ptr; + const int nch = cd->channels; + int remaining_samples = frames * nch; + + if (nch == 2) { + const int32_t vol_l = cd->volume[0]; + const int32_t vol_r = cd->volume[1]; + + while (remaining_samples) { + const int nmax_src = cir_buf_samples_without_wrap_s16(x, source->buf_end); + const int nmax_snk = cir_buf_samples_without_wrap_s16(y, sink->buf_end); + int n = MIN(remaining_samples, MIN(nmax_src, nmax_snk)); + + int i = 0; + for (; i <= n - 4; i += 4) { + const uint32_t in0 = *(const uint32_t *)&x[i + 0]; + const uint32_t in1 = *(const uint32_t *)&x[i + 2]; + const int16_t out_l0 = vol_mult_s16((int16_t)in0, vol_l); + const int16_t out_r0 = vol_mult_s16((int16_t)(in0 >> 16), vol_r); + const int16_t out_l1 = vol_mult_s16((int16_t)in1, vol_l); + const int16_t out_r1 = vol_mult_s16((int16_t)(in1 >> 16), vol_r); + + *(uint32_t *)&y[i + 0] = arm_pkhbt((uint16_t)out_l0, (uint16_t)out_r0, 16); + *(uint32_t *)&y[i + 2] = arm_pkhbt((uint16_t)out_l1, (uint16_t)out_r1, 16); + } + for (; i < n; i += 2) { + const uint32_t in0 = *(const uint32_t *)&x[i + 0]; + const int16_t out_l0 = vol_mult_s16((int16_t)in0, vol_l); + const int16_t out_r0 = vol_mult_s16((int16_t)(in0 >> 16), vol_r); + + *(uint32_t *)&y[i + 0] = arm_pkhbt((uint16_t)out_l0, (uint16_t)out_r0, 16); + } + + remaining_samples -= n; + x = cir_buf_wrap(x + n, source->buf_start, source->buf_end); + y = cir_buf_wrap(y + n, sink->buf_start, sink->buf_end); + } + } else if (nch == 1) { + const int32_t vol = cd->volume[0]; + + while (remaining_samples) { + const int nmax_src = cir_buf_samples_without_wrap_s16(x, source->buf_end); + const int nmax_snk = cir_buf_samples_without_wrap_s16(y, sink->buf_end); + int n = MIN(remaining_samples, MIN(nmax_src, nmax_snk)); + + int i = 0; + for (; i <= n - 4; i += 4) { + const uint32_t in0 = *(const uint32_t *)&x[i + 0]; + const uint32_t in1 = *(const uint32_t *)&x[i + 2]; + const int16_t out0 = vol_mult_s16((int16_t)in0, vol); + const int16_t out1 = vol_mult_s16((int16_t)(in0 >> 16), vol); + const int16_t out2 = vol_mult_s16((int16_t)in1, vol); + const int16_t out3 = vol_mult_s16((int16_t)(in1 >> 16), vol); + + *(uint32_t *)&y[i + 0] = arm_pkhbt((uint16_t)out0, (uint16_t)out1, 16); + *(uint32_t *)&y[i + 2] = arm_pkhbt((uint16_t)out2, (uint16_t)out3, 16); + } + for (; i < n; i++) { + y[i] = vol_mult_s16(x[i], vol); + } + + remaining_samples -= n; + x = cir_buf_wrap(x + n, source->buf_start, source->buf_end); + y = cir_buf_wrap(y + n, sink->buf_start, sink->buf_end); + } + } else { + while (remaining_samples) { + const int nmax_src = cir_buf_samples_without_wrap_s16(x, source->buf_end); + const int nmax_snk = cir_buf_samples_without_wrap_s16(y, sink->buf_end); + int n = MIN(remaining_samples, MIN(nmax_src, nmax_snk)); + + for (int j = 0; j < nch; j++) { + const int32_t vol = cd->volume[j]; + for (int i = j; i < n; i += nch) { + y[i] = vol_mult_s16(x[i], vol); + } + } + + remaining_samples -= n; + x = cir_buf_wrap(x + n, source->buf_start, source->buf_end); + y = cir_buf_wrap(y + n, sink->buf_start, sink->buf_end); + } + } +} + +static void vol_passthrough_s16_to_s16(struct processing_module *mod, + struct cir_buf_source *source, + struct cir_buf_sink *sink, uint32_t frames, + uint32_t attenuation) +{ + struct vol_data *cd = module_get_private_data(mod); + const int16_t *x = source->ptr; + int16_t *y = sink->ptr; + const int nch = cd->channels; + int remaining_samples = frames * nch; + + while (remaining_samples) { + const int nmax_src = cir_buf_samples_without_wrap_s16(x, source->buf_end); + const int nmax_snk = cir_buf_samples_without_wrap_s16(y, sink->buf_end); + int n = MIN(remaining_samples, MIN(nmax_src, nmax_snk)); + + memcpy_s(y, n * sizeof(int16_t), x, n * sizeof(int16_t)); + remaining_samples -= n; + x = cir_buf_wrap(x + n, source->buf_start, source->buf_end); + y = cir_buf_wrap(y + n, sink->buf_start, sink->buf_end); + } +} +#endif /* CONFIG_FORMAT_S16LE */ + +#if CONFIG_FORMAT_S24LE +static inline int32_t vol_mult_s24(int32_t x, int32_t vol) +{ + const int32_t sx = sign_extend_s24(x); +#if COMP_VOLUME_Q8_16 + const int64_t prod = ((int64_t)sx * vol + (1LL << 15)) >> 16; +#elif COMP_VOLUME_Q1_31 + const int64_t prod = ((int64_t)sx * vol + (1LL << 30)) >> 31; +#else + const int64_t prod = ((int64_t)sx * vol + (1LL << 22)) >> 23; +#endif + return arm_sat_s24(arm_sat_q31(prod)); +} + +static void vol_s24_to_s24(struct processing_module *mod, struct cir_buf_source *source, + struct cir_buf_sink *sink, uint32_t frames, uint32_t attenuation) +{ + struct vol_data *cd = module_get_private_data(mod); + const int32_t *x = source->ptr; + int32_t *y = sink->ptr; + const int nch = cd->channels; + int remaining_samples = frames * nch; + + if (nch == 2) { + const int32_t vol_l = cd->volume[0]; + const int32_t vol_r = cd->volume[1]; + + while (remaining_samples) { + const int nmax_src = cir_buf_samples_without_wrap_s32(x, source->buf_end); + const int nmax_snk = cir_buf_samples_without_wrap_s32(y, sink->buf_end); + int n = MIN(remaining_samples, MIN(nmax_src, nmax_snk)); + + int i = 0; + for (; i <= n - 4; i += 4) { + y[i + 0] = vol_mult_s24(x[i + 0], vol_l); + y[i + 1] = vol_mult_s24(x[i + 1], vol_r); + y[i + 2] = vol_mult_s24(x[i + 2], vol_l); + y[i + 3] = vol_mult_s24(x[i + 3], vol_r); + } + for (; i < n; i += 2) { + y[i + 0] = vol_mult_s24(x[i + 0], vol_l); + y[i + 1] = vol_mult_s24(x[i + 1], vol_r); + } + + remaining_samples -= n; + x = cir_buf_wrap(x + n, source->buf_start, source->buf_end); + y = cir_buf_wrap(y + n, sink->buf_start, sink->buf_end); + } + } else { + while (remaining_samples) { + const int nmax_src = cir_buf_samples_without_wrap_s32(x, source->buf_end); + const int nmax_snk = cir_buf_samples_without_wrap_s32(y, sink->buf_end); + int n = MIN(remaining_samples, MIN(nmax_src, nmax_snk)); + + for (int j = 0; j < nch; j++) { + const int32_t vol = cd->volume[j]; + for (int i = j; i < n; i += nch) { + y[i] = vol_mult_s24(x[i], vol); + } + } + + remaining_samples -= n; + x = cir_buf_wrap(x + n, source->buf_start, source->buf_end); + y = cir_buf_wrap(y + n, sink->buf_start, sink->buf_end); + } + } +} + +static void vol_passthrough_s24_to_s24(struct processing_module *mod, + struct cir_buf_source *source, + struct cir_buf_sink *sink, uint32_t frames, + uint32_t attenuation) +{ + struct vol_data *cd = module_get_private_data(mod); + const int32_t *x = source->ptr; + int32_t *y = sink->ptr; + const int nch = cd->channels; + int remaining_samples = frames * nch; + + while (remaining_samples) { + const int nmax_src = cir_buf_samples_without_wrap_s32(x, source->buf_end); + const int nmax_snk = cir_buf_samples_without_wrap_s32(y, sink->buf_end); + int n = MIN(remaining_samples, MIN(nmax_src, nmax_snk)); + + memcpy_s(y, n * sizeof(int32_t), x, n * sizeof(int32_t)); + remaining_samples -= n; + x = cir_buf_wrap(x + n, source->buf_start, source->buf_end); + y = cir_buf_wrap(y + n, sink->buf_start, sink->buf_end); + } +} +#endif /* CONFIG_FORMAT_S24LE */ + +#if CONFIG_FORMAT_S32LE +static inline int32_t vol_mult_s32(int32_t x, int32_t vol) +{ +#if COMP_VOLUME_Q8_16 + const int64_t prod = ((int64_t)x * vol + (1LL << 15)) >> 16; + return arm_sat_q31(prod); +#elif COMP_VOLUME_Q1_31 + return arm_smmulr(x, vol); +#else + const int64_t prod = ((int64_t)x * vol + (1LL << 22)) >> 23; + return arm_sat_q31(prod); +#endif +} + +static void vol_s32_to_s32(struct processing_module *mod, struct cir_buf_source *source, + struct cir_buf_sink *sink, uint32_t frames, uint32_t attenuation) +{ + struct vol_data *cd = module_get_private_data(mod); + const int32_t *x = source->ptr; + int32_t *y = sink->ptr; + const int nch = cd->channels; + int remaining_samples = frames * nch; + + if (nch == 2) { + const int32_t vol_l = cd->volume[0]; + const int32_t vol_r = cd->volume[1]; + + while (remaining_samples) { + const int nmax_src = cir_buf_samples_without_wrap_s32(x, source->buf_end); + const int nmax_snk = cir_buf_samples_without_wrap_s32(y, sink->buf_end); + int n = MIN(remaining_samples, MIN(nmax_src, nmax_snk)); + + int i = 0; + for (; i <= n - 4; i += 4) { + y[i + 0] = vol_mult_s32(x[i + 0], vol_l); + y[i + 1] = vol_mult_s32(x[i + 1], vol_r); + y[i + 2] = vol_mult_s32(x[i + 2], vol_l); + y[i + 3] = vol_mult_s32(x[i + 3], vol_r); + } + for (; i < n; i += 2) { + y[i + 0] = vol_mult_s32(x[i + 0], vol_l); + y[i + 1] = vol_mult_s32(x[i + 1], vol_r); + } + + remaining_samples -= n; + x = cir_buf_wrap(x + n, source->buf_start, source->buf_end); + y = cir_buf_wrap(y + n, sink->buf_start, sink->buf_end); + } + } else { + while (remaining_samples) { + const int nmax_src = cir_buf_samples_without_wrap_s32(x, source->buf_end); + const int nmax_snk = cir_buf_samples_without_wrap_s32(y, sink->buf_end); + int n = MIN(remaining_samples, MIN(nmax_src, nmax_snk)); + + for (int j = 0; j < nch; j++) { + const int32_t vol = cd->volume[j]; + for (int i = j; i < n; i += nch) { + y[i] = vol_mult_s32(x[i], vol); + } + } + + remaining_samples -= n; + x = cir_buf_wrap(x + n, source->buf_start, source->buf_end); + y = cir_buf_wrap(y + n, sink->buf_start, sink->buf_end); + } + } +} + +static void vol_passthrough_s32_to_s32(struct processing_module *mod, + struct cir_buf_source *source, + struct cir_buf_sink *sink, uint32_t frames, + uint32_t attenuation) +{ + struct vol_data *cd = module_get_private_data(mod); + const int32_t *x = source->ptr; + int32_t *y = sink->ptr; + const int nch = cd->channels; + int remaining_samples = frames * nch; + + while (remaining_samples) { + const int nmax_src = cir_buf_samples_without_wrap_s32(x, source->buf_end); + const int nmax_snk = cir_buf_samples_without_wrap_s32(y, sink->buf_end); + int n = MIN(remaining_samples, MIN(nmax_src, nmax_snk)); + + memcpy_s(y, n * sizeof(int32_t), x, n * sizeof(int32_t)); + remaining_samples -= n; + x = cir_buf_wrap(x + n, source->buf_start, source->buf_end); + y = cir_buf_wrap(y + n, sink->buf_start, sink->buf_end); + } +} +#endif /* CONFIG_FORMAT_S32LE */ + +#if CONFIG_FORMAT_FLOAT +static void vol_float_to_float(struct processing_module *mod, struct cir_buf_source *source, + struct cir_buf_sink *sink, uint32_t frames, uint32_t attenuation) +{ + struct vol_data *cd = module_get_private_data(mod); + const float *x = source->ptr; + float *y = sink->ptr; + const int nch = cd->channels; + int remaining_samples = frames * nch; + float vol_f[PLATFORM_MAX_CHANNELS]; + + for (int j = 0; j < nch; j++) { +#if COMP_VOLUME_Q8_16 + vol_f[j] = (float)cd->volume[j] * (1.0f / 65536.0f); +#elif COMP_VOLUME_Q1_31 + vol_f[j] = (float)cd->volume[j] * (1.0f / 2147483648.0f); +#else + vol_f[j] = (float)cd->volume[j] * (1.0f / 8388608.0f); +#endif + } + + if (nch == 2) { + const float vol_l = vol_f[0]; + const float vol_r = vol_f[1]; + + while (remaining_samples) { + const int nmax_src = cir_buf_samples_without_wrap_s32(x, source->buf_end); + const int nmax_snk = cir_buf_samples_without_wrap_s32(y, sink->buf_end); + int n = MIN(remaining_samples, MIN(nmax_src, nmax_snk)); + + int i = 0; + for (; i <= n - 4; i += 4) { + y[i + 0] = x[i + 0] * vol_l; + y[i + 1] = x[i + 1] * vol_r; + y[i + 2] = x[i + 2] * vol_l; + y[i + 3] = x[i + 3] * vol_r; + } + for (; i < n; i += 2) { + y[i + 0] = x[i + 0] * vol_l; + y[i + 1] = x[i + 1] * vol_r; + } + + remaining_samples -= n; + x = cir_buf_wrap(x + n, source->buf_start, source->buf_end); + y = cir_buf_wrap(y + n, sink->buf_start, sink->buf_end); + } + } else if (nch == 1) { + const float vol = vol_f[0]; + + while (remaining_samples) { + const int nmax_src = cir_buf_samples_without_wrap_s32(x, source->buf_end); + const int nmax_snk = cir_buf_samples_without_wrap_s32(y, sink->buf_end); + int n = MIN(remaining_samples, MIN(nmax_src, nmax_snk)); + + int i = 0; + for (; i <= n - 4; i += 4) { + y[i + 0] = x[i + 0] * vol; + y[i + 1] = x[i + 1] * vol; + y[i + 2] = x[i + 2] * vol; + y[i + 3] = x[i + 3] * vol; + } + for (; i < n; i++) { + y[i] = x[i] * vol; + } + + remaining_samples -= n; + x = cir_buf_wrap(x + n, source->buf_start, source->buf_end); + y = cir_buf_wrap(y + n, sink->buf_start, sink->buf_end); + } + } else { + while (remaining_samples) { + const int nmax_src = cir_buf_samples_without_wrap_s32(x, source->buf_end); + const int nmax_snk = cir_buf_samples_without_wrap_s32(y, sink->buf_end); + int n = MIN(remaining_samples, MIN(nmax_src, nmax_snk)); + + for (int j = 0; j < nch; j++) { + const float vol = vol_f[j]; + for (int i = j; i < n; i += nch) { + y[i] = x[i] * vol; + } + } + + remaining_samples -= n; + x = cir_buf_wrap(x + n, source->buf_start, source->buf_end); + y = cir_buf_wrap(y + n, sink->buf_start, sink->buf_end); + } + } +} + +static void vol_passthrough_float_to_float(struct processing_module *mod, + struct cir_buf_source *source, + struct cir_buf_sink *sink, uint32_t frames, + uint32_t attenuation) +{ + struct vol_data *cd = module_get_private_data(mod); + const float *x = source->ptr; + float *y = sink->ptr; + const int nch = cd->channels; + int remaining_samples = frames * nch; + + while (remaining_samples) { + const int nmax_src = cir_buf_samples_without_wrap_s32(x, source->buf_end); + const int nmax_snk = cir_buf_samples_without_wrap_s32(y, sink->buf_end); + int n = MIN(remaining_samples, MIN(nmax_src, nmax_snk)); + + memcpy_s(y, n * sizeof(float), x, n * sizeof(float)); + remaining_samples -= n; + x = cir_buf_wrap(x + n, source->buf_start, source->buf_end); + y = cir_buf_wrap(y + n, sink->buf_start, sink->buf_end); + } +} +#endif /* CONFIG_FORMAT_FLOAT */ + +const struct comp_func_map volume_func_map[] = { +#if CONFIG_FORMAT_S16LE + { SOF_IPC_FRAME_S16_LE, vol_s16_to_s16, vol_passthrough_s16_to_s16 }, +#endif +#if CONFIG_FORMAT_S24LE + { SOF_IPC_FRAME_S24_4LE, vol_s24_to_s24, vol_passthrough_s24_to_s24 }, +#endif +#if CONFIG_FORMAT_S32LE + { SOF_IPC_FRAME_S32_LE, vol_s32_to_s32, vol_passthrough_s32_to_s32 }, +#endif +#if CONFIG_FORMAT_FLOAT + { SOF_IPC_FRAME_FLOAT, vol_float_to_float, vol_passthrough_float_to_float }, +#endif +}; + +const size_t volume_func_count = ARRAY_SIZE(volume_func_map); + +#endif /* (!CONFIG_COMP_PEAK_VOL) */ diff --git a/src/audio/volume/volume_arm_with_peakvol.c b/src/audio/volume/volume_arm_with_peakvol.c new file mode 100644 index 000000000000..80322bc0fc31 --- /dev/null +++ b/src/audio/volume/volume_arm_with_peakvol.c @@ -0,0 +1,695 @@ +// SPDX-License-Identifier: BSD-3-Clause +// +// Copyright(c) 2026 Sound Open Firmware. All rights reserved. +// +// Author: Liam Girdwood + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#ifndef fabsf +#define fabsf(x) __builtin_fabsf(x) +#endif + +LOG_MODULE_DECLARE(volume_arm, CONFIG_SOF_LOG_LEVEL); + +#include "volume.h" + +#if CONFIG_COMP_PEAK_VOL + +static inline int32_t sat_abs_s32(int32_t sample) +{ + if (__builtin_expect(sample == INT32_MIN, 0)) + return INT32_MAX; + return sample < 0 ? -sample : sample; +} + +static inline int32_t sat_peak_shift(int32_t peak, uint32_t shift) +{ + if (!peak) + return 0; + if (shift >= 31) + return INT32_MAX; + return arm_sat_q31((int64_t)peak << shift); +} + +#if CONFIG_FORMAT_S16LE +static inline int16_t vol_mult_s16(int16_t x, int32_t vol) +{ +#if COMP_VOLUME_Q8_16 + const int64_t prod = ((int64_t)x * vol + 0x8000) >> 16; + return arm_sat_s16(arm_sat_q31(prod)); +#elif COMP_VOLUME_Q1_31 + const int64_t prod = ((int64_t)x * vol + (1LL << 30)) >> 31; + return arm_sat_s16((int32_t)prod); +#else + const int64_t prod = ((int64_t)x * vol + (1LL << 22)) >> 23; + return arm_sat_s16((int32_t)prod); +#endif +} + +static void vol_s16_to_s16(struct processing_module *mod, struct cir_buf_source *source, + struct cir_buf_sink *sink, uint32_t frames, uint32_t attenuation) +{ + struct vol_data *cd = module_get_private_data(mod); + const int16_t *x = source->ptr; + int16_t *y = sink->ptr; + const int nch = cd->channels; + int remaining_samples = frames * nch; + + while (remaining_samples) { + const int nmax_src = cir_buf_samples_without_wrap_s16(x, source->buf_end); + const int nmax_snk = cir_buf_samples_without_wrap_s16(y, sink->buf_end); + int n = MIN(remaining_samples, MIN(nmax_src, nmax_snk)); + + if (nch == 2) { + const int32_t vol_l = cd->volume[0]; + const int32_t vol_r = cd->volume[1]; + int32_t peak_l = 0; + int32_t peak_r = 0; + + int i = 0; + for (; i <= n - 4; i += 4) { + const int16_t x0 = x[i + 0]; + const int16_t x1 = x[i + 1]; + const int16_t x2 = x[i + 2]; + const int16_t x3 = x[i + 3]; + + const int16_t out_l0 = vol_mult_s16(x0, vol_l); + const int16_t out_r0 = vol_mult_s16(x1, vol_r); + const int16_t out_l1 = vol_mult_s16(x2, vol_l); + const int16_t out_r1 = vol_mult_s16(x3, vol_r); + + *(uint32_t *)&y[i + 0] = arm_pkhbt((uint16_t)out_l0, (uint16_t)out_r0, 16); + *(uint32_t *)&y[i + 2] = arm_pkhbt((uint16_t)out_l1, (uint16_t)out_r1, 16); + + peak_l = MAX((int32_t)abs(x0), peak_l); + peak_r = MAX((int32_t)abs(x1), peak_r); + peak_l = MAX((int32_t)abs(x2), peak_l); + peak_r = MAX((int32_t)abs(x3), peak_r); + } + for (; i < n; i += 2) { + const int16_t x0 = x[i + 0]; + const int16_t x1 = x[i + 1]; + + const int16_t out_l0 = vol_mult_s16(x0, vol_l); + const int16_t out_r0 = vol_mult_s16(x1, vol_r); + + *(uint32_t *)&y[i + 0] = arm_pkhbt((uint16_t)out_l0, (uint16_t)out_r0, 16); + + peak_l = MAX((int32_t)abs(x0), peak_l); + peak_r = MAX((int32_t)abs(x1), peak_r); + } + + peak_l <<= PEAK_16S_32C_ADJUST; + peak_r <<= PEAK_16S_32C_ADJUST; + cd->peak_regs.peak_meter[0] = MAX(peak_l, cd->peak_regs.peak_meter[0]); + cd->peak_regs.peak_meter[1] = MAX(peak_r, cd->peak_regs.peak_meter[1]); + } else { + for (int j = 0; j < nch; j++) { + const int32_t vol = cd->volume[j]; + int32_t peak = 0; + for (int i = j; i < n; i += nch) { + y[i] = vol_mult_s16(x[i], vol); + peak = MAX((int32_t)abs(x[i]), peak); + } + peak <<= PEAK_16S_32C_ADJUST; + cd->peak_regs.peak_meter[j] = MAX(peak, cd->peak_regs.peak_meter[j]); + } + } + + remaining_samples -= n; + x = cir_buf_wrap(x + n, source->buf_start, source->buf_end); + y = cir_buf_wrap(y + n, sink->buf_start, sink->buf_end); + } +} + +static void vol_passthrough_s16_to_s16(struct processing_module *mod, + struct cir_buf_source *source, + struct cir_buf_sink *sink, uint32_t frames, + uint32_t attenuation) +{ + struct vol_data *cd = module_get_private_data(mod); + const int16_t *x = source->ptr; + int16_t *y = sink->ptr; + const int nch = cd->channels; + int remaining_samples = frames * nch; + + while (remaining_samples) { + const int nmax_src = cir_buf_samples_without_wrap_s16(x, source->buf_end); + const int nmax_snk = cir_buf_samples_without_wrap_s16(y, sink->buf_end); + int n = MIN(remaining_samples, MIN(nmax_src, nmax_snk)); + + if (nch == 2) { + int32_t peak_l = 0; + int32_t peak_r = 0; + + int i = 0; + for (; i <= n - 4; i += 4) { + const int16_t x0 = x[i + 0]; + const int16_t x1 = x[i + 1]; + const int16_t x2 = x[i + 2]; + const int16_t x3 = x[i + 3]; + + y[i + 0] = x0; + y[i + 1] = x1; + y[i + 2] = x2; + y[i + 3] = x3; + + peak_l = MAX((int32_t)abs(x0), peak_l); + peak_r = MAX((int32_t)abs(x1), peak_r); + peak_l = MAX((int32_t)abs(x2), peak_l); + peak_r = MAX((int32_t)abs(x3), peak_r); + } + for (; i < n; i += 2) { + const int16_t x0 = x[i + 0]; + const int16_t x1 = x[i + 1]; + + y[i + 0] = x0; + y[i + 1] = x1; + + peak_l = MAX((int32_t)abs(x0), peak_l); + peak_r = MAX((int32_t)abs(x1), peak_r); + } + + peak_l <<= PEAK_16S_32C_ADJUST; + peak_r <<= PEAK_16S_32C_ADJUST; + cd->peak_regs.peak_meter[0] = MAX(peak_l, cd->peak_regs.peak_meter[0]); + cd->peak_regs.peak_meter[1] = MAX(peak_r, cd->peak_regs.peak_meter[1]); + } else { + for (int j = 0; j < nch; j++) { + int32_t peak = 0; + for (int i = j; i < n; i += nch) { + y[i] = x[i]; + peak = MAX((int32_t)abs(x[i]), peak); + } + peak <<= PEAK_16S_32C_ADJUST; + cd->peak_regs.peak_meter[j] = MAX(peak, cd->peak_regs.peak_meter[j]); + } + } + + remaining_samples -= n; + x = cir_buf_wrap(x + n, source->buf_start, source->buf_end); + y = cir_buf_wrap(y + n, sink->buf_start, sink->buf_end); + } +} +#endif /* CONFIG_FORMAT_S16LE */ + +#if CONFIG_FORMAT_S24LE +static inline int32_t vol_mult_s24(int32_t x, int32_t vol) +{ + const int32_t sx = sign_extend_s24(x); +#if COMP_VOLUME_Q8_16 + const int64_t prod = ((int64_t)sx * vol + (1LL << 15)) >> 16; +#elif COMP_VOLUME_Q1_31 + const int64_t prod = ((int64_t)sx * vol + (1LL << 30)) >> 31; +#else + const int64_t prod = ((int64_t)sx * vol + (1LL << 22)) >> 23; +#endif + return arm_sat_s24(arm_sat_q31(prod)); +} + +static void vol_s24_to_s24(struct processing_module *mod, struct cir_buf_source *source, + struct cir_buf_sink *sink, uint32_t frames, uint32_t attenuation) +{ + struct vol_data *cd = module_get_private_data(mod); + const int32_t *x = source->ptr; + int32_t *y = sink->ptr; + const int nch = cd->channels; + int remaining_samples = frames * nch; + + while (remaining_samples) { + const int nmax_src = cir_buf_samples_without_wrap_s32(x, source->buf_end); + const int nmax_snk = cir_buf_samples_without_wrap_s32(y, sink->buf_end); + int n = MIN(remaining_samples, MIN(nmax_src, nmax_snk)); + + if (nch == 2) { + const int32_t vol_l = cd->volume[0]; + const int32_t vol_r = cd->volume[1]; + int32_t peak_l = 0; + int32_t peak_r = 0; + + int i = 0; + for (; i <= n - 4; i += 4) { + const int32_t x0 = x[i + 0]; + const int32_t x1 = x[i + 1]; + const int32_t x2 = x[i + 2]; + const int32_t x3 = x[i + 3]; + + y[i + 0] = vol_mult_s24(x0, vol_l); + y[i + 1] = vol_mult_s24(x1, vol_r); + y[i + 2] = vol_mult_s24(x2, vol_l); + y[i + 3] = vol_mult_s24(x3, vol_r); + + peak_l = MAX(abs(sign_extend_s24(x0)), peak_l); + peak_r = MAX(abs(sign_extend_s24(x1)), peak_r); + peak_l = MAX(abs(sign_extend_s24(x2)), peak_l); + peak_r = MAX(abs(sign_extend_s24(x3)), peak_r); + } + for (; i < n; i += 2) { + const int32_t x0 = x[i + 0]; + const int32_t x1 = x[i + 1]; + + y[i + 0] = vol_mult_s24(x0, vol_l); + y[i + 1] = vol_mult_s24(x1, vol_r); + + peak_l = MAX(abs(sign_extend_s24(x0)), peak_l); + peak_r = MAX(abs(sign_extend_s24(x1)), peak_r); + } + + peak_l = sat_peak_shift(peak_l, attenuation + PEAK_24S_32C_ADJUST); + peak_r = sat_peak_shift(peak_r, attenuation + PEAK_24S_32C_ADJUST); + cd->peak_regs.peak_meter[0] = MAX(peak_l, cd->peak_regs.peak_meter[0]); + cd->peak_regs.peak_meter[1] = MAX(peak_r, cd->peak_regs.peak_meter[1]); + } else { + for (int j = 0; j < nch; j++) { + const int32_t vol = cd->volume[j]; + int32_t peak = 0; + for (int i = j; i < n; i += nch) { + y[i] = vol_mult_s24(x[i], vol); + peak = MAX(abs(sign_extend_s24(x[i])), peak); + } + peak = sat_peak_shift(peak, attenuation + PEAK_24S_32C_ADJUST); + cd->peak_regs.peak_meter[j] = MAX(peak, cd->peak_regs.peak_meter[j]); + } + } + + remaining_samples -= n; + x = cir_buf_wrap(x + n, source->buf_start, source->buf_end); + y = cir_buf_wrap(y + n, sink->buf_start, sink->buf_end); + } +} + +static void vol_passthrough_s24_to_s24(struct processing_module *mod, + struct cir_buf_source *source, + struct cir_buf_sink *sink, uint32_t frames, + uint32_t attenuation) +{ + struct vol_data *cd = module_get_private_data(mod); + const int32_t *x = source->ptr; + int32_t *y = sink->ptr; + const int nch = cd->channels; + int remaining_samples = frames * nch; + + while (remaining_samples) { + const int nmax_src = cir_buf_samples_without_wrap_s32(x, source->buf_end); + const int nmax_snk = cir_buf_samples_without_wrap_s32(y, sink->buf_end); + int n = MIN(remaining_samples, MIN(nmax_src, nmax_snk)); + + if (nch == 2) { + int32_t peak_l = 0; + int32_t peak_r = 0; + + int i = 0; + for (; i <= n - 4; i += 4) { + const int32_t x0 = x[i + 0]; + const int32_t x1 = x[i + 1]; + const int32_t x2 = x[i + 2]; + const int32_t x3 = x[i + 3]; + + y[i + 0] = x0; + y[i + 1] = x1; + y[i + 2] = x2; + y[i + 3] = x3; + + peak_l = MAX(abs(sign_extend_s24(x0)), peak_l); + peak_r = MAX(abs(sign_extend_s24(x1)), peak_r); + peak_l = MAX(abs(sign_extend_s24(x2)), peak_l); + peak_r = MAX(abs(sign_extend_s24(x3)), peak_r); + } + for (; i < n; i += 2) { + const int32_t x0 = x[i + 0]; + const int32_t x1 = x[i + 1]; + + y[i + 0] = x0; + y[i + 1] = x1; + + peak_l = MAX(abs(sign_extend_s24(x0)), peak_l); + peak_r = MAX(abs(sign_extend_s24(x1)), peak_r); + } + + peak_l = sat_peak_shift(peak_l, attenuation + PEAK_24S_32C_ADJUST); + peak_r = sat_peak_shift(peak_r, attenuation + PEAK_24S_32C_ADJUST); + cd->peak_regs.peak_meter[0] = MAX(peak_l, cd->peak_regs.peak_meter[0]); + cd->peak_regs.peak_meter[1] = MAX(peak_r, cd->peak_regs.peak_meter[1]); + } else { + for (int j = 0; j < nch; j++) { + int32_t peak = 0; + for (int i = j; i < n; i += nch) { + y[i] = x[i]; + peak = MAX(abs(sign_extend_s24(x[i])), peak); + } + peak = sat_peak_shift(peak, attenuation + PEAK_24S_32C_ADJUST); + cd->peak_regs.peak_meter[j] = MAX(peak, cd->peak_regs.peak_meter[j]); + } + } + + remaining_samples -= n; + x = cir_buf_wrap(x + n, source->buf_start, source->buf_end); + y = cir_buf_wrap(y + n, sink->buf_start, sink->buf_end); + } +} +#endif /* CONFIG_FORMAT_S24LE */ + +#if CONFIG_FORMAT_S32LE +static inline int32_t vol_mult_s32(int32_t x, int32_t vol) +{ +#if COMP_VOLUME_Q8_16 + const int64_t prod = ((int64_t)x * vol + (1LL << 15)) >> 16; + return arm_sat_q31(prod); +#elif COMP_VOLUME_Q1_31 + return arm_smmulr(x, vol); +#else + const int64_t prod = ((int64_t)x * vol + (1LL << 22)) >> 23; + return arm_sat_q31(prod); +#endif +} + +static void vol_s32_to_s32(struct processing_module *mod, struct cir_buf_source *source, + struct cir_buf_sink *sink, uint32_t frames, uint32_t attenuation) +{ + struct vol_data *cd = module_get_private_data(mod); + const int32_t *x = source->ptr; + int32_t *y = sink->ptr; + const int nch = cd->channels; + int remaining_samples = frames * nch; + + while (remaining_samples) { + const int nmax_src = cir_buf_samples_without_wrap_s32(x, source->buf_end); + const int nmax_snk = cir_buf_samples_without_wrap_s32(y, sink->buf_end); + int n = MIN(remaining_samples, MIN(nmax_src, nmax_snk)); + + if (nch == 2) { + const int32_t vol_l = cd->volume[0]; + const int32_t vol_r = cd->volume[1]; + int32_t peak_l = 0; + int32_t peak_r = 0; + + int i = 0; + for (; i <= n - 4; i += 4) { + const int32_t x0 = x[i + 0]; + const int32_t x1 = x[i + 1]; + const int32_t x2 = x[i + 2]; + const int32_t x3 = x[i + 3]; + + y[i + 0] = vol_mult_s32(x0, vol_l); + y[i + 1] = vol_mult_s32(x1, vol_r); + y[i + 2] = vol_mult_s32(x2, vol_l); + y[i + 3] = vol_mult_s32(x3, vol_r); + + peak_l = MAX(sat_abs_s32(x0), peak_l); + peak_r = MAX(sat_abs_s32(x1), peak_r); + peak_l = MAX(sat_abs_s32(x2), peak_l); + peak_r = MAX(sat_abs_s32(x3), peak_r); + } + for (; i < n; i += 2) { + const int32_t x0 = x[i + 0]; + const int32_t x1 = x[i + 1]; + + y[i + 0] = vol_mult_s32(x0, vol_l); + y[i + 1] = vol_mult_s32(x1, vol_r); + + peak_l = MAX(sat_abs_s32(x0), peak_l); + peak_r = MAX(sat_abs_s32(x1), peak_r); + } + + peak_l = sat_peak_shift(peak_l, attenuation); + peak_r = sat_peak_shift(peak_r, attenuation); + cd->peak_regs.peak_meter[0] = MAX(peak_l, cd->peak_regs.peak_meter[0]); + cd->peak_regs.peak_meter[1] = MAX(peak_r, cd->peak_regs.peak_meter[1]); + } else { + for (int j = 0; j < nch; j++) { + const int32_t vol = cd->volume[j]; + int32_t peak = 0; + for (int i = j; i < n; i += nch) { + y[i] = vol_mult_s32(x[i], vol); + peak = MAX(sat_abs_s32(x[i]), peak); + } + peak = sat_peak_shift(peak, attenuation); + cd->peak_regs.peak_meter[j] = MAX(peak, cd->peak_regs.peak_meter[j]); + } + } + + remaining_samples -= n; + x = cir_buf_wrap(x + n, source->buf_start, source->buf_end); + y = cir_buf_wrap(y + n, sink->buf_start, sink->buf_end); + } +} + +static void vol_passthrough_s32_to_s32(struct processing_module *mod, + struct cir_buf_source *source, + struct cir_buf_sink *sink, uint32_t frames, + uint32_t attenuation) +{ + struct vol_data *cd = module_get_private_data(mod); + const int32_t *x = source->ptr; + int32_t *y = sink->ptr; + const int nch = cd->channels; + int remaining_samples = frames * nch; + + while (remaining_samples) { + const int nmax_src = cir_buf_samples_without_wrap_s32(x, source->buf_end); + const int nmax_snk = cir_buf_samples_without_wrap_s32(y, sink->buf_end); + int n = MIN(remaining_samples, MIN(nmax_src, nmax_snk)); + + if (nch == 2) { + int32_t peak_l = 0; + int32_t peak_r = 0; + + int i = 0; + for (; i <= n - 4; i += 4) { + const int32_t x0 = x[i + 0]; + const int32_t x1 = x[i + 1]; + const int32_t x2 = x[i + 2]; + const int32_t x3 = x[i + 3]; + + y[i + 0] = x0; + y[i + 1] = x1; + y[i + 2] = x2; + y[i + 3] = x3; + + peak_l = MAX(sat_abs_s32(x0), peak_l); + peak_r = MAX(sat_abs_s32(x1), peak_r); + peak_l = MAX(sat_abs_s32(x2), peak_l); + peak_r = MAX(sat_abs_s32(x3), peak_r); + } + for (; i < n; i += 2) { + const int32_t x0 = x[i + 0]; + const int32_t x1 = x[i + 1]; + + y[i + 0] = x0; + y[i + 1] = x1; + + peak_l = MAX(sat_abs_s32(x0), peak_l); + peak_r = MAX(sat_abs_s32(x1), peak_r); + } + + peak_l = sat_peak_shift(peak_l, attenuation); + peak_r = sat_peak_shift(peak_r, attenuation); + cd->peak_regs.peak_meter[0] = MAX(peak_l, cd->peak_regs.peak_meter[0]); + cd->peak_regs.peak_meter[1] = MAX(peak_r, cd->peak_regs.peak_meter[1]); + } else { + for (int j = 0; j < nch; j++) { + int32_t peak = 0; + for (int i = j; i < n; i += nch) { + y[i] = x[i]; + peak = MAX(sat_abs_s32(x[i]), peak); + } + peak = sat_peak_shift(peak, attenuation); + cd->peak_regs.peak_meter[j] = MAX(peak, cd->peak_regs.peak_meter[j]); + } + } + + remaining_samples -= n; + x = cir_buf_wrap(x + n, source->buf_start, source->buf_end); + y = cir_buf_wrap(y + n, sink->buf_start, sink->buf_end); + } +} +#endif /* CONFIG_FORMAT_S32LE */ + +#if CONFIG_FORMAT_FLOAT +static void vol_float_to_float(struct processing_module *mod, struct cir_buf_source *source, + struct cir_buf_sink *sink, uint32_t frames, uint32_t attenuation) +{ + struct vol_data *cd = module_get_private_data(mod); + const float *x = source->ptr; + float *y = sink->ptr; + const int nch = cd->channels; + int remaining_samples = frames * nch; + float vol_f[PLATFORM_MAX_CHANNELS]; + + for (int j = 0; j < nch; j++) { +#if COMP_VOLUME_Q8_16 + vol_f[j] = (float)cd->volume[j] * (1.0f / 65536.0f); +#elif COMP_VOLUME_Q1_31 + vol_f[j] = (float)cd->volume[j] * (1.0f / 2147483648.0f); +#else + vol_f[j] = (float)cd->volume[j] * (1.0f / 8388608.0f); +#endif + } + + while (remaining_samples) { + const int nmax_src = cir_buf_samples_without_wrap_s32(x, source->buf_end); + const int nmax_snk = cir_buf_samples_without_wrap_s32(y, sink->buf_end); + int n = MIN(remaining_samples, MIN(nmax_src, nmax_snk)); + + if (nch == 2) { + const float vol_l = vol_f[0]; + const float vol_r = vol_f[1]; + float peak_l = 0.0f; + float peak_r = 0.0f; + + int i = 0; + for (; i <= n - 4; i += 4) { + const float x0 = x[i + 0]; + const float x1 = x[i + 1]; + const float x2 = x[i + 2]; + const float x3 = x[i + 3]; + + y[i + 0] = x0 * vol_l; + y[i + 1] = x1 * vol_r; + y[i + 2] = x2 * vol_l; + y[i + 3] = x3 * vol_r; + + peak_l = MAX(fabsf(x0), peak_l); + peak_r = MAX(fabsf(x1), peak_r); + peak_l = MAX(fabsf(x2), peak_l); + peak_r = MAX(fabsf(x3), peak_r); + } + for (; i < n; i += 2) { + const float x0 = x[i + 0]; + const float x1 = x[i + 1]; + + y[i + 0] = x0 * vol_l; + y[i + 1] = x1 * vol_r; + + peak_l = MAX(fabsf(x0), peak_l); + peak_r = MAX(fabsf(x1), peak_r); + } + + int32_t ipeak_l = (int32_t)(peak_l * 2147483647.0f); + int32_t ipeak_r = (int32_t)(peak_r * 2147483647.0f); + ipeak_l = sat_peak_shift(ipeak_l, attenuation); + ipeak_r = sat_peak_shift(ipeak_r, attenuation); + cd->peak_regs.peak_meter[0] = MAX(ipeak_l, cd->peak_regs.peak_meter[0]); + cd->peak_regs.peak_meter[1] = MAX(ipeak_r, cd->peak_regs.peak_meter[1]); + } else { + for (int j = 0; j < nch; j++) { + const float vol = vol_f[j]; + float peak = 0.0f; + for (int i = j; i < n; i += nch) { + y[i] = x[i] * vol; + peak = MAX(fabsf(x[i]), peak); + } + int32_t ipeak = (int32_t)(peak * 2147483647.0f); + ipeak = sat_peak_shift(ipeak, attenuation); + cd->peak_regs.peak_meter[j] = MAX(ipeak, cd->peak_regs.peak_meter[j]); + } + } + + remaining_samples -= n; + x = cir_buf_wrap(x + n, source->buf_start, source->buf_end); + y = cir_buf_wrap(y + n, sink->buf_start, sink->buf_end); + } +} + +static void vol_passthrough_float_to_float(struct processing_module *mod, + struct cir_buf_source *source, + struct cir_buf_sink *sink, uint32_t frames, + uint32_t attenuation) +{ + struct vol_data *cd = module_get_private_data(mod); + const float *x = source->ptr; + float *y = sink->ptr; + const int nch = cd->channels; + int remaining_samples = frames * nch; + + while (remaining_samples) { + const int nmax_src = cir_buf_samples_without_wrap_s32(x, source->buf_end); + const int nmax_snk = cir_buf_samples_without_wrap_s32(y, sink->buf_end); + int n = MIN(remaining_samples, MIN(nmax_src, nmax_snk)); + + if (nch == 2) { + float peak_l = 0.0f; + float peak_r = 0.0f; + + int i = 0; + for (; i <= n - 4; i += 4) { + const float x0 = x[i + 0]; + const float x1 = x[i + 1]; + const float x2 = x[i + 2]; + const float x3 = x[i + 3]; + + y[i + 0] = x0; + y[i + 1] = x1; + y[i + 2] = x2; + y[i + 3] = x3; + + peak_l = MAX(fabsf(x0), peak_l); + peak_r = MAX(fabsf(x1), peak_r); + peak_l = MAX(fabsf(x2), peak_l); + peak_r = MAX(fabsf(x3), peak_r); + } + for (; i < n; i += 2) { + const float x0 = x[i + 0]; + const float x1 = x[i + 1]; + + y[i + 0] = x0; + y[i + 1] = x1; + + peak_l = MAX(fabsf(x0), peak_l); + peak_r = MAX(fabsf(x1), peak_r); + } + + int32_t ipeak_l = (int32_t)(peak_l * 2147483647.0f); + int32_t ipeak_r = (int32_t)(peak_r * 2147483647.0f); + ipeak_l = sat_peak_shift(ipeak_l, attenuation); + ipeak_r = sat_peak_shift(ipeak_r, attenuation); + cd->peak_regs.peak_meter[0] = MAX(ipeak_l, cd->peak_regs.peak_meter[0]); + cd->peak_regs.peak_meter[1] = MAX(ipeak_r, cd->peak_regs.peak_meter[1]); + } else { + for (int j = 0; j < nch; j++) { + float peak = 0.0f; + for (int i = j; i < n; i += nch) { + y[i] = x[i]; + peak = MAX(fabsf(x[i]), peak); + } + int32_t ipeak = (int32_t)(peak * 2147483647.0f); + ipeak = sat_peak_shift(ipeak, attenuation); + cd->peak_regs.peak_meter[j] = MAX(ipeak, cd->peak_regs.peak_meter[j]); + } + } + + remaining_samples -= n; + x = cir_buf_wrap(x + n, source->buf_start, source->buf_end); + y = cir_buf_wrap(y + n, sink->buf_start, sink->buf_end); + } +} +#endif /* CONFIG_FORMAT_FLOAT */ + +const struct comp_func_map volume_func_map[] = { +#if CONFIG_FORMAT_S16LE + { SOF_IPC_FRAME_S16_LE, vol_s16_to_s16, vol_passthrough_s16_to_s16 }, +#endif +#if CONFIG_FORMAT_S24LE + { SOF_IPC_FRAME_S24_4LE, vol_s24_to_s24, vol_passthrough_s24_to_s24 }, +#endif +#if CONFIG_FORMAT_S32LE + { SOF_IPC_FRAME_S32_LE, vol_s32_to_s32, vol_passthrough_s32_to_s32 }, +#endif +#if CONFIG_FORMAT_FLOAT + { SOF_IPC_FRAME_FLOAT, vol_float_to_float, vol_passthrough_float_to_float }, +#endif +}; + +const size_t volume_func_count = ARRAY_SIZE(volume_func_map); + +#endif /* CONFIG_COMP_PEAK_VOL */ From 4e4607440f457a894613118c00bf577b2a462463 Mon Sep 17 00:00:00 2001 From: Liam Girdwood Date: Tue, 13 Oct 2026 14:42:16 +0100 Subject: [PATCH 06/13] audio: eq_iir: add ARM Cortex-M DSP block-accelerated processing Implement block-accelerated IIR equalizer for ARM Cortex-M architectures: - Add CONFIG_EQ_IIR_ARM_SIMD Kconfig option. - Implement eq_iir_arm.c: * Block vector processing kernel keeping filter delay states (y_n2, y_n1, x_n2, x_n1) and biquad coefficients in CPU registers across each 64-sample block. * Inlines biquad loop eliminating external call dispatches. * Fast branchless saturation for S16, S24, and S32 formats via SSAT. Signed-off-by: Liam Girdwood --- src/audio/eq_iir/CMakeLists.txt | 6 +- src/audio/eq_iir/Kconfig | 2 + src/audio/eq_iir/Kconfig.simd | 8 + src/audio/eq_iir/eq_iir_arm.c | 798 ++++++++++++++++++++++++++++++++ 4 files changed, 813 insertions(+), 1 deletion(-) create mode 100644 src/audio/eq_iir/Kconfig.simd create mode 100644 src/audio/eq_iir/eq_iir_arm.c diff --git a/src/audio/eq_iir/CMakeLists.txt b/src/audio/eq_iir/CMakeLists.txt index 33c50df17df8..0c08b1cec067 100644 --- a/src/audio/eq_iir/CMakeLists.txt +++ b/src/audio/eq_iir/CMakeLists.txt @@ -7,7 +7,11 @@ if(CONFIG_COMP_IIR STREQUAL "m" AND DEFINED CONFIG_LLEXT) else() - add_local_sources(sof eq_iir.c eq_iir_generic.c) + if(CONFIG_EQ_IIR_ARM_SIMD) + add_local_sources(sof eq_iir.c eq_iir_arm.c) + else() + add_local_sources(sof eq_iir.c eq_iir_generic.c) + endif() if(CONFIG_IPC_MAJOR_3) add_local_sources(sof eq_iir_ipc3.c) diff --git a/src/audio/eq_iir/Kconfig b/src/audio/eq_iir/Kconfig index 11433d27cfb3..44e39ced8987 100644 --- a/src/audio/eq_iir/Kconfig +++ b/src/audio/eq_iir/Kconfig @@ -9,3 +9,5 @@ config COMP_IIR select MATH_IIR_DF1 help Select for IIR component + +rsource "Kconfig.simd" diff --git a/src/audio/eq_iir/Kconfig.simd b/src/audio/eq_iir/Kconfig.simd new file mode 100644 index 000000000000..0158d4d3522c --- /dev/null +++ b/src/audio/eq_iir/Kconfig.simd @@ -0,0 +1,8 @@ +# SPDX-License-Identifier: BSD-3-Clause + +config EQ_IIR_ARM_SIMD + bool "ARM SIMD and block-accelerated EQ IIR" + default y + depends on ARM + help + Select for ARMv7E-M SIMD and block-accelerated EQ IIR implementation diff --git a/src/audio/eq_iir/eq_iir_arm.c b/src/audio/eq_iir/eq_iir_arm.c new file mode 100644 index 000000000000..1f8def2618d1 --- /dev/null +++ b/src/audio/eq_iir/eq_iir_arm.c @@ -0,0 +1,798 @@ +// SPDX-License-Identifier: BSD-3-Clause +// +// Copyright(c) 2026 Sound Open Firmware. All rights reserved. +// +// Author: Liam Girdwood + +#include "eq_iir.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +LOG_MODULE_DECLARE(eq_iir, CONFIG_SOF_LOG_LEVEL); + +#define EQ_IIR_BLOCK_SIZE 64 + +/* + * Block IIR DF1 processing kernel for ARM Cortex-M7: + * Keeps filter delay states in CPU registers for the block duration, + * using ARM SMLAL instructions for 5-term MAC accumulation. + */ +#if CONFIG_FORMAT_S16LE +static void iir_df1_process_block_s16(struct iir_state_df1 *iir, + const int16_t *src, int16_t *dst, + int nsamples, int stride) +{ + if (!iir->biquads) { + for (int n = 0; n < nsamples; n++) { + *dst = *src; + src += stride; + dst += stride; + } + return; + } + + int32_t scratch_in[EQ_IIR_BLOCK_SIZE]; + int32_t scratch[EQ_IIR_BLOCK_SIZE]; + int64_t accum[EQ_IIR_BLOCK_SIZE]; + + while (nsamples > 0) { + const int chunk = MIN(nsamples, EQ_IIR_BLOCK_SIZE); + + /* Convert input S16 to Q1.31 in scratch buffer */ + const int16_t *s_ptr = src; + for (int n = 0; n < chunk; n++) { + scratch_in[n] = ((int32_t)*s_ptr) << 16; + s_ptr += stride; + } + + const int nseries = iir->biquads_in_series; + + int c = 0; + int d = 0; + const int32_t *coefp = iir->coef; + int32_t *delay = iir->delay; + + for (int j = 0; j < (int)iir->biquads; j += nseries) { + for (int n = 0; n < chunk; n++) + scratch[n] = scratch_in[n]; + + for (int b = 0; b < nseries; b++) { + const int32_t a2 = coefp[c]; + const int32_t a1 = coefp[c + 1]; + const int32_t b2 = coefp[c + 2]; + const int32_t b1 = coefp[c + 3]; + const int32_t b0 = coefp[c + 4]; + const int32_t shift_param = coefp[c + 5]; + const int32_t gain = coefp[c + 6]; + const int shift_bits = 14 + shift_param; + const int64_t gain_round = 1LL << (shift_bits - 1); + + int32_t y_n2 = delay[d]; + int32_t y_n1 = delay[d + 1]; + int32_t x_n2 = delay[d + 2]; + int32_t x_n1 = delay[d + 3]; + + for (int n = 0; n < chunk; n++) { + const int32_t in = scratch[n]; + + /* 5-term MAC accumulation with SMLAL */ + int64_t acc = arm_smlal(0, a2, y_n2); + acc = arm_smlal(acc, a1, y_n1); + acc = arm_smlal(acc, b2, x_n2); + acc = arm_smlal(acc, b1, x_n1); + acc = arm_smlal(acc, b0, in); + + /* Round and convert from Q3.61 to Q3.31 */ + acc = (acc + (1LL << 29)) >> 30; + const int32_t tmp = arm_sat_q31(acc); + + y_n2 = y_n1; + y_n1 = tmp; + x_n2 = x_n1; + x_n1 = in; + + /* Apply biquad gain & shift */ + int64_t acc_gain = ((int64_t)gain) * tmp; + acc_gain = (acc_gain + gain_round) >> shift_bits; + scratch[n] = arm_sat_q31(acc_gain); + } + + delay[d] = y_n2; + delay[d + 1] = y_n1; + delay[d + 2] = x_n2; + delay[d + 3] = x_n1; + + c += SOF_EQ_IIR_NBIQUAD; + d += IIR_DF1_NUM_STATE; + } + + if (j == 0) { + for (int n = 0; n < chunk; n++) + accum[n] = (int64_t)scratch[n]; + } else { + for (int n = 0; n < chunk; n++) + accum[n] += (int64_t)scratch[n]; + } + } + + int16_t *d_ptr = dst; + for (int n = 0; n < chunk; n++) { + int64_t val = (int64_t)arm_sat_q31(accum[n]); + *d_ptr = arm_sat_s16((int32_t)Q_SHIFT_RND(val, 31, 15)); + d_ptr += stride; + } + + src += chunk * stride; + dst += chunk * stride; + nsamples -= chunk; + } +} +#endif /* CONFIG_FORMAT_S16LE */ + +#if CONFIG_FORMAT_S24LE +static void iir_df1_process_block_s24(struct iir_state_df1 *iir, + const int32_t *src, int32_t *dst, + int nsamples, int stride) +{ + if (!iir->biquads) { + for (int n = 0; n < nsamples; n++) { + *dst = *src; + src += stride; + dst += stride; + } + return; + } + + int32_t scratch_in[EQ_IIR_BLOCK_SIZE]; + int32_t scratch[EQ_IIR_BLOCK_SIZE]; + int64_t accum[EQ_IIR_BLOCK_SIZE]; + + while (nsamples > 0) { + const int chunk = MIN(nsamples, EQ_IIR_BLOCK_SIZE); + + const int32_t *s_ptr = src; + for (int n = 0; n < chunk; n++) { + scratch_in[n] = (*s_ptr) << 8; + s_ptr += stride; + } + + const int nseries = iir->biquads_in_series; + + int c = 0; + int d = 0; + const int32_t *coefp = iir->coef; + int32_t *delay = iir->delay; + + for (int j = 0; j < (int)iir->biquads; j += nseries) { + for (int n = 0; n < chunk; n++) + scratch[n] = scratch_in[n]; + + for (int b = 0; b < nseries; b++) { + const int32_t a2 = coefp[c]; + const int32_t a1 = coefp[c + 1]; + const int32_t b2 = coefp[c + 2]; + const int32_t b1 = coefp[c + 3]; + const int32_t b0 = coefp[c + 4]; + const int32_t shift_param = coefp[c + 5]; + const int32_t gain = coefp[c + 6]; + const int shift_bits = 14 + shift_param; + const int64_t gain_round = 1LL << (shift_bits - 1); + + int32_t y_n2 = delay[d]; + int32_t y_n1 = delay[d + 1]; + int32_t x_n2 = delay[d + 2]; + int32_t x_n1 = delay[d + 3]; + + for (int n = 0; n < chunk; n++) { + const int32_t in = scratch[n]; + + int64_t acc = arm_smlal(0, a2, y_n2); + acc = arm_smlal(acc, a1, y_n1); + acc = arm_smlal(acc, b2, x_n2); + acc = arm_smlal(acc, b1, x_n1); + acc = arm_smlal(acc, b0, in); + + acc = (acc + (1LL << 29)) >> 30; + const int32_t tmp = arm_sat_q31(acc); + + y_n2 = y_n1; + y_n1 = tmp; + x_n2 = x_n1; + x_n1 = in; + + int64_t acc_gain = ((int64_t)gain) * tmp; + acc_gain = (acc_gain + gain_round) >> shift_bits; + scratch[n] = arm_sat_q31(acc_gain); + } + + delay[d] = y_n2; + delay[d + 1] = y_n1; + delay[d + 2] = x_n2; + delay[d + 3] = x_n1; + + c += SOF_EQ_IIR_NBIQUAD; + d += IIR_DF1_NUM_STATE; + } + + if (j == 0) { + for (int n = 0; n < chunk; n++) + accum[n] = (int64_t)scratch[n]; + } else { + for (int n = 0; n < chunk; n++) + accum[n] += (int64_t)scratch[n]; + } + } + + int32_t *d_ptr = dst; + for (int n = 0; n < chunk; n++) { + int64_t val = (int64_t)arm_sat_q31(accum[n]); + *d_ptr = arm_sat_s24((int32_t)Q_SHIFT_RND(val, 31, 23)); + d_ptr += stride; + } + + src += chunk * stride; + dst += chunk * stride; + nsamples -= chunk; + } +} +#endif /* CONFIG_FORMAT_S24LE */ + +#if CONFIG_FORMAT_S32LE +static void iir_df1_process_block_s32(struct iir_state_df1 *iir, + const int32_t *src, int32_t *dst, + int nsamples, int stride) +{ + if (!iir->biquads) { + for (int n = 0; n < nsamples; n++) { + *dst = *src; + src += stride; + dst += stride; + } + return; + } + + int32_t scratch_in[EQ_IIR_BLOCK_SIZE]; + int32_t scratch[EQ_IIR_BLOCK_SIZE]; + int64_t accum[EQ_IIR_BLOCK_SIZE]; + + while (nsamples > 0) { + const int chunk = MIN(nsamples, EQ_IIR_BLOCK_SIZE); + + const int32_t *s_ptr = src; + for (int n = 0; n < chunk; n++) { + scratch_in[n] = *s_ptr; + s_ptr += stride; + } + + const int nseries = iir->biquads_in_series; + + int c = 0; + int d = 0; + const int32_t *coefp = iir->coef; + int32_t *delay = iir->delay; + + for (int j = 0; j < (int)iir->biquads; j += nseries) { + for (int n = 0; n < chunk; n++) + scratch[n] = scratch_in[n]; + + for (int b = 0; b < nseries; b++) { + const int32_t a2 = coefp[c]; + const int32_t a1 = coefp[c + 1]; + const int32_t b2 = coefp[c + 2]; + const int32_t b1 = coefp[c + 3]; + const int32_t b0 = coefp[c + 4]; + const int32_t shift_param = coefp[c + 5]; + const int32_t gain = coefp[c + 6]; + const int shift_bits = 14 + shift_param; + const int64_t gain_round = 1LL << (shift_bits - 1); + + int32_t y_n2 = delay[d]; + int32_t y_n1 = delay[d + 1]; + int32_t x_n2 = delay[d + 2]; + int32_t x_n1 = delay[d + 3]; + + for (int n = 0; n < chunk; n++) { + const int32_t in = scratch[n]; + + int64_t acc = arm_smlal(0, a2, y_n2); + acc = arm_smlal(acc, a1, y_n1); + acc = arm_smlal(acc, b2, x_n2); + acc = arm_smlal(acc, b1, x_n1); + acc = arm_smlal(acc, b0, in); + + acc = (acc + (1LL << 29)) >> 30; + const int32_t tmp = arm_sat_q31(acc); + + y_n2 = y_n1; + y_n1 = tmp; + x_n2 = x_n1; + x_n1 = in; + + int64_t acc_gain = ((int64_t)gain) * tmp; + acc_gain = (acc_gain + gain_round) >> shift_bits; + scratch[n] = arm_sat_q31(acc_gain); + } + + delay[d] = y_n2; + delay[d + 1] = y_n1; + delay[d + 2] = x_n2; + delay[d + 3] = x_n1; + + c += SOF_EQ_IIR_NBIQUAD; + d += IIR_DF1_NUM_STATE; + } + + if (j == 0) { + for (int n = 0; n < chunk; n++) + accum[n] = (int64_t)scratch[n]; + } else { + for (int n = 0; n < chunk; n++) + accum[n] += (int64_t)scratch[n]; + } + } + + int32_t *d_ptr = dst; + for (int n = 0; n < chunk; n++) { + *d_ptr = arm_sat_q31(accum[n]); + d_ptr += stride; + } + + src += chunk * stride; + dst += chunk * stride; + nsamples -= chunk; + } +} +#endif /* CONFIG_FORMAT_S32LE */ + +#if CONFIG_FORMAT_S16LE +void eq_iir_s16_default(struct processing_module *mod, const struct cir_buf_source *source, + struct cir_buf_sink *sink, size_t frames) +{ + struct comp_data *cd = module_get_private_data(mod); + const unsigned int nch = cd->channels; + const size_t samples = frames * nch; + size_t processed = 0; + + const int16_t *x = source->ptr; + int16_t *y = sink->ptr; + + while (processed < samples) { + const size_t nmax = samples - processed; + const size_t n1 = cir_buf_samples_without_wrap_s16(x, source->buf_end); + const size_t n2 = cir_buf_samples_without_wrap_s16(y, sink->buf_end); + size_t n = MIN(n1, n2); + n = MIN(n, nmax); + const size_t nframes = n / nch; + + for (unsigned int i = 0; i < nch; i++) { + iir_df1_process_block_s16(&cd->iir[i], x + i, y + i, nframes, nch); + } + + processed += n; + x = source_cir_buf_wrap(x + n, source->buf_start, source->buf_end); + y = cir_buf_wrap(y + n, sink->buf_start, sink->buf_end); + } +} +#endif /* CONFIG_FORMAT_S16LE */ + +#if CONFIG_FORMAT_S24LE +void eq_iir_s24_default(struct processing_module *mod, const struct cir_buf_source *source, + struct cir_buf_sink *sink, size_t frames) +{ + struct comp_data *cd = module_get_private_data(mod); + const unsigned int nch = cd->channels; + const size_t samples = frames * nch; + size_t processed = 0; + + const int32_t *x = source->ptr; + int32_t *y = sink->ptr; + + while (processed < samples) { + const size_t nmax = samples - processed; + const size_t n1 = cir_buf_samples_without_wrap_s32(x, source->buf_end); + const size_t n2 = cir_buf_samples_without_wrap_s32(y, sink->buf_end); + size_t n = MIN(n1, n2); + n = MIN(n, nmax); + const size_t nframes = n / nch; + + for (unsigned int i = 0; i < nch; i++) { + iir_df1_process_block_s24(&cd->iir[i], x + i, y + i, nframes, nch); + } + + processed += n; + x = source_cir_buf_wrap(x + n, source->buf_start, source->buf_end); + y = cir_buf_wrap(y + n, sink->buf_start, sink->buf_end); + } +} +#endif /* CONFIG_FORMAT_S24LE */ + +#if CONFIG_FORMAT_S32LE +void eq_iir_s32_default(struct processing_module *mod, const struct cir_buf_source *source, + struct cir_buf_sink *sink, size_t frames) +{ + struct comp_data *cd = module_get_private_data(mod); + const unsigned int nch = cd->channels; + const size_t samples = frames * nch; + size_t processed = 0; + + const int32_t *x = source->ptr; + int32_t *y = sink->ptr; + + while (processed < samples) { + const size_t nmax = samples - processed; + const size_t n1 = cir_buf_samples_without_wrap_s32(x, source->buf_end); + const size_t n2 = cir_buf_samples_without_wrap_s32(y, sink->buf_end); + size_t n = MIN(n1, n2); + n = MIN(n, nmax); + const size_t nframes = n / nch; + + for (unsigned int i = 0; i < nch; i++) { + iir_df1_process_block_s32(&cd->iir[i], x + i, y + i, nframes, nch); + } + + processed += n; + x = source_cir_buf_wrap(x + n, source->buf_start, source->buf_end); + y = cir_buf_wrap(y + n, sink->buf_start, sink->buf_end); + } +} +#endif /* CONFIG_FORMAT_S32LE */ + +static int eq_iir_blob_words_max(struct comp_dev *dev, + const struct sof_eq_iir_config *config, + size_t blob_size, + uint32_t *coef_words_max) +{ + size_t payload_bytes; + + /* Compute the size of the coefficient area in int32_t words from the + * framework-reported blob size. The blob layout is: + * sizeof(*config) header bytes + * channels_in_config int32_t assign_response[] + * coefficient data[] + * channels_in_config is bounded above, so the multiply fits in size_t. + * The blob's self-declared config->size is cross-checked against the + * authoritative blob_size so all later parsing stays within the buffer. + */ + if (blob_size < sizeof(*config) || config->size != blob_size) { + comp_err(dev, "blob size %zu / header size %u mismatch or too small", + blob_size, config->size); + return -EINVAL; + } + payload_bytes = blob_size - sizeof(*config); + if (payload_bytes % sizeof(int32_t) || + payload_bytes < (size_t)config->channels_in_config * sizeof(int32_t)) { + comp_err(dev, "blob size %zu misaligned or too small", blob_size); + return -EINVAL; + } + *coef_words_max = payload_bytes / sizeof(int32_t) - config->channels_in_config; + return 0; +} + +/** + * @brief Parse one response header from the coefficient blob. + * + * Reads the response at word offset *j in @p coef_data, checks that both + * the fixed header and the following biquad sections fit inside the + * @p coef_words_max blob budget, and advances *j past this response so + * the next call continues where this one left off. On success @p *eq_out + * is set to point at the in-place header; callers that only want to walk + * the blob for validation can discard that pointer. + * + * @param dev Component device, used for error logging. + * @param idx Response index, used in error messages. + * @param coef_data Base of the coefficient word array in the blob. + * @param coef_words_max Total words available in @p coef_data. + * @param j In/out cursor in words; advanced past this response. + * @param eq_out Out; pointer to the parsed response header. + * + * @return 0 on success, -EINVAL if the header or sections would run off + * the end of the blob or num_sections exceeds the platform limit. + */ +static int eq_iir_init_response(struct comp_dev *dev, int idx, + int32_t *coef_data, uint32_t coef_words_max, + uint32_t *j, struct sof_eq_iir_header **eq_out) +{ + struct sof_eq_iir_header *eq; + uint32_t header_end = *j + SOF_EQ_IIR_NHEADER; + uint32_t section_end; + + /* Header must fit before reading num_sections */ + if (header_end > coef_words_max) { + comp_err(dev, "response %d header out of bounds", idx); + return -EINVAL; + } + eq = (struct sof_eq_iir_header *)&coef_data[*j]; + /* Bound num_sections so the multiply cannot overflow and the section + * data stays within the blob. + */ + section_end = header_end + (uint32_t)SOF_EQ_IIR_NBIQUAD * eq->num_sections; + if (eq->num_sections > SOF_EQ_IIR_BIQUADS_MAX || section_end > coef_words_max) { + comp_err(dev, "response %d num_sections %u out of bounds", + idx, eq->num_sections); + return -EINVAL; + } + *eq_out = eq; + *j = section_end; + return 0; +} + +/* Validate the config blob layout and, if lookup is non-NULL, populate it + * with pointers to each response header. Pass lookup = NULL to validate only. + */ +static int eq_iir_walk_config(struct comp_dev *dev, + struct sof_eq_iir_config *config, + size_t config_size, + struct sof_eq_iir_header **lookup) +{ + struct sof_eq_iir_header *eq; + uint32_t coef_words_max; + int32_t *coef_data; + int ret; + int i; + uint32_t j; + + if (config->channels_in_config > PLATFORM_MAX_CHANNELS || + !config->channels_in_config) { + comp_err(dev, "invalid channels_in_config %u", config->channels_in_config); + return -EINVAL; + } + if (config->number_of_responses > SOF_EQ_IIR_MAX_RESPONSES) { + comp_err(dev, "# of resp %u exceeds max", config->number_of_responses); + return -EINVAL; + } + + ret = eq_iir_blob_words_max(dev, config, config_size, &coef_words_max); + if (ret < 0) + return ret; + + j = 0; + coef_data = ASSUME_ALIGNED(&config->data[config->channels_in_config], 4); + if (lookup) + memset(lookup, 0, SOF_EQ_IIR_MAX_RESPONSES * sizeof(*lookup)); + + for (i = 0; i < config->number_of_responses; i++) { + /* Bounds-check response i, advance the walk cursor j past it, + * and get a pointer to its in-place header. Stored in lookup[] + * for later use, or discarded when we are walking the blob + * only to validate it. + */ + ret = eq_iir_init_response(dev, i, coef_data, coef_words_max, &j, &eq); + if (ret < 0) + return ret; + if (lookup) + lookup[i] = eq; + } + + return 0; +} + +int eq_iir_validate_config(struct comp_dev *dev, void *new_data, uint32_t new_data_size) +{ + struct sof_eq_iir_config *config = new_data; + int32_t *assign_response; + int32_t resp; + int ret; + int i; + + if (new_data_size < sizeof(struct sof_eq_iir_config) || + new_data_size > SOF_EQ_IIR_MAX_SIZE) { + comp_err(dev, "invalid configuration blob, size %u", new_data_size); + return -EINVAL; + } + + ret = eq_iir_walk_config(dev, config, new_data_size, NULL); + if (ret < 0) + return ret; + + /* Validate every assign_response[] entry that the per-channel loop in + * eq_iir_init_coef() could pick up. Entries beyond channels_in_config + * reuse the last assigned value, so checking [0, channels_in_config) + * covers all reachable nch. + */ + assign_response = ASSUME_ALIGNED(&config->data[0], 4); + for (i = 0; i < config->channels_in_config; i++) { + resp = assign_response[i]; + if (resp >= 0 && resp >= config->number_of_responses) { + comp_err(dev, "assign_response[%d] = %d exceeds %u", + i, resp, config->number_of_responses); + return -EINVAL; + } + } + + return 0; +} + +static int eq_iir_init_coef(struct processing_module *mod, unsigned int nch, + size_t *delay_size) +{ + struct comp_data *cd = module_get_private_data(mod); + struct sof_eq_iir_config *config = cd->config; + struct iir_state_df1 *iir = cd->iir; + struct sof_eq_iir_header *lookup[SOF_EQ_IIR_MAX_RESPONSES]; + struct sof_eq_iir_header *eq; + int32_t *assign_response; + size_t size_sum = 0; + int resp = 0; + unsigned int i; + int s; + int ret; + + comp_info(mod->dev, "%u responses, %u channels, stream %u channels", + config->number_of_responses, config->channels_in_config, nch); + + if (nch > PLATFORM_MAX_CHANNELS) { + comp_err(mod->dev, "invalid stream channels %u", nch); + return -EINVAL; + } + + ret = eq_iir_walk_config(mod->dev, config, cd->config_size, lookup); + if (ret < 0) + return ret; + + /* Initialize 1st phase */ + assign_response = ASSUME_ALIGNED(&config->data[0], 4); + for (i = 0; i < nch; i++) { + /* Check for not reading past blob response to channel assign + * map. The previous channel response is assigned for any + * additional channels in the stream. It allows to use single + * channel configuration to setup multi channel equalization + * with the same response. + */ + if (i < config->channels_in_config) + resp = assign_response[i]; + + if (resp < 0) { + /* Initialize EQ channel to bypass and continue with + * next channel response. + */ + comp_info(mod->dev, "ch %u is set to bypass", i); + iir_reset_df1(&iir[i]); + continue; + } + + if (resp >= config->number_of_responses) { + comp_err(mod->dev, "requested response %d exceeds defined", + resp); + return -EINVAL; + } + + /* Initialize EQ coefficients */ + eq = lookup[resp]; + s = iir_delay_size_df1(eq); + if (s > 0) { + size_sum += (size_t)s; + } else { + comp_err(mod->dev, "sections count %d exceeds max", + eq->num_sections); + return -EINVAL; + } + + iir_init_coef_df1(&iir[i], eq); + comp_info(mod->dev, "ch %u is set to response %d", i, resp); + } + + *delay_size = size_sum; + return 0; +} + +static void eq_iir_init_delay(struct iir_state_df1 *iir, + int32_t *delay_start, unsigned int nch) +{ + int32_t *delay = delay_start; + unsigned int i; + + /* Initialize second phase to set EQ delay lines pointers. A + * bypass mode filter is indicated by biquads count of zero. + */ + for (i = 0; i < nch; i++) { + if (iir[i].biquads > 0) + iir_init_delay_df1(&iir[i], &delay); + } +} + +void eq_iir_free_delaylines(struct processing_module *mod) +{ + struct comp_data *cd = module_get_private_data(mod); + struct iir_state_df1 *iir = cd->iir; + unsigned int i; + + /* Free the common buffer for all EQs and point then + * each IIR channel delay line to NULL. + */ + mod_free(mod, cd->iir_delay); + cd->iir_delay = NULL; + cd->iir_delay_size = 0; + for (i = 0; i < PLATFORM_MAX_CHANNELS; i++) + iir[i].delay = NULL; +} + +void eq_iir_pass(struct processing_module *mod, const struct cir_buf_source *source, + struct cir_buf_sink *sink, size_t frames) +{ + struct comp_data *cd = module_get_private_data(mod); + const uint8_t *src = source->ptr; + uint8_t *dst = sink->ptr; + size_t bytes = frames * cd->frame_bytes; + + while (bytes) { + size_t src_contiguous = (const uint8_t *)source->buf_end - src; + size_t dst_contiguous = (uint8_t *)sink->buf_end - dst; + size_t copy = MIN(bytes, MIN(src_contiguous, dst_contiguous)); + + if (!copy) { + src = source_cir_buf_wrap(src, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst, sink->buf_start, sink->buf_end); + continue; + } + + assert(!memcpy_s(dst, dst_contiguous, src, copy)); + bytes -= copy; + src = source_cir_buf_wrap(src + copy, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst + copy, sink->buf_start, sink->buf_end); + } +} + +int eq_iir_setup(struct processing_module *mod, unsigned int nch) +{ + struct comp_data *cd = module_get_private_data(mod); + size_t delay_size = 0; + int ret; + + cd->channels = nch; + + /* Free existing IIR channels data if it was allocated */ + eq_iir_free_delaylines(mod); + + /* Set coefficients for each channel EQ from coefficient blob. + * eq_iir_init_coef() / eq_iir_blob_words_max() perform all blob size + * sanity checks, including config->size vs cd->config_size. + */ + ret = eq_iir_init_coef(mod, nch, &delay_size); + if (ret < 0) + return ret; + + /* If all channels were set to bypass there's no need to + * allocate delay. Just return with success. + */ + if (!delay_size) + return 0; + + /* Allocate all IIR channels data in a big chunk and clear it */ + cd->iir_delay = mod_zalloc(mod, delay_size); + if (!cd->iir_delay) { + comp_err(mod->dev, "delay allocation fail"); + return -ENOMEM; + } + + cd->iir_delay_size = delay_size; + + /* Assign delay line to each channel EQ */ + eq_iir_init_delay(cd->iir, cd->iir_delay, nch); + return 0; +} From 603f867e3616d5b257421c25ce0eaea97fd4f296 Mon Sep 17 00:00:00 2001 From: Liam Girdwood Date: Tue, 13 Oct 2026 14:42:53 +0100 Subject: [PATCH 07/13] audio: drc: add ARM Cortex-M DSP fast polynomial math and compression Implement ARM Cortex-M DSP optimizations for Dynamic Range Compressor: - Add CONFIG_DRC_ARM_SIMD choice option in DRC Kconfig.simd. - Implement drc_math_arm.c with Horner's polynomial evaluations for: log10_fixed, drc_lin2db_fixed, drc_log_fixed, drc_asin_fixed, drc_inv_fixed leveraging ARM CLZ (count leading zeros) and branchless saturation. - Implement fast knee_curveK, volume_gain, and envelope tracking. - Implement drc_arm.c with unrolled output compression. - Update drc_generic.c guards for SOF_USE_ARM_SIMD(DRC). Signed-off-by: Liam Girdwood --- src/audio/drc/CMakeLists.txt | 2 + src/audio/drc/Kconfig.simd | 11 +- src/audio/drc/drc_arm.c | 445 +++++++++++++++++++++++++++++++++++ src/audio/drc/drc_generic.c | 2 +- src/audio/drc/drc_math.h | 36 +++ src/audio/drc/drc_math_arm.c | 185 +++++++++++++++ 6 files changed, 679 insertions(+), 2 deletions(-) create mode 100644 src/audio/drc/drc_arm.c create mode 100644 src/audio/drc/drc_math_arm.c diff --git a/src/audio/drc/CMakeLists.txt b/src/audio/drc/CMakeLists.txt index 883c32675b50..1ca601a50159 100644 --- a/src/audio/drc/CMakeLists.txt +++ b/src/audio/drc/CMakeLists.txt @@ -9,8 +9,10 @@ else() drc_generic.c drc_hifi3.c drc_hifi4.c + drc_arm.c drc_math_generic.c drc_math_hifi3.c + drc_math_arm.c ) endif() diff --git a/src/audio/drc/Kconfig.simd b/src/audio/drc/Kconfig.simd index 498758c4813d..f4420586ea7b 100644 --- a/src/audio/drc/Kconfig.simd +++ b/src/audio/drc/Kconfig.simd @@ -5,7 +5,16 @@ comment "DRC optimization level select" choice "DRC_SIMD_LEVEL_SELECT" prompt "choose which SIMD level used for DRC module" depends on COMP_DRC - default DRC_HIFI_MAX + default DRC_HIFI_MAX if XTENSA + default DRC_ARM_SIMD if ARM + default DRC_HIFI_NONE + + config DRC_ARM_SIMD + prompt "choose ARM SIMD & DSP optimized DRC module" + bool + depends on ARM + help + This option used to build ARMv7E-M DSP / SIMD optimized DRC code. config DRC_HIFI_MAX prompt "SIMD will selected by toolchain pre-defined header" diff --git a/src/audio/drc/drc_arm.c b/src/audio/drc/drc_arm.c new file mode 100644 index 000000000000..cb4795aefe08 --- /dev/null +++ b/src/audio/drc/drc_arm.c @@ -0,0 +1,445 @@ +// SPDX-License-Identifier: BSD-3-Clause +// +// Copyright(c) 2026 Sound Open Firmware. All rights reserved. +// +// Author: Liam Girdwood + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "drc.h" +#include "drc_algorithm.h" +#include "drc_math.h" + +#if SOF_USE_ARM_SIMD(DRC) + +#define ONE_Q20 1048576 /* Q_CONVERT_FLOAT(1.0f, 20) */ +#define ONE_Q21 2097152 /* Q_CONVERT_FLOAT(1.0f, 21) */ +#define ONE_Q30 1073741824 /* Q_CONVERT_FLOAT(1.0f, 30) */ +#define TWELVE_Q21 25165824 /* Q_CONVERT_FLOAT(12.0f, 21) */ +#define HALF_Q24 8388608 /* Q_CONVERT_FLOAT(0.5f, 24) */ +#define NEG_TWO_DB_Q30 852903424 /* Q_CONVERT_FLOAT(0.7943282347242815f, 30) */ + +/* Knee curve calculation */ +static int32_t knee_curveK(const struct sof_drc_params *p, int32_t x) +{ + int32_t gamma; + int32_t knee_exp_gamma; + int32_t knee_curve_k; + + gamma = drc_mult_lshift(x, -p->K, drc_get_lshift(31, 20, 27)); + knee_exp_gamma = sofm_exp_fixed(gamma); + knee_curve_k = drc_mult_lshift(p->knee_beta, knee_exp_gamma, drc_get_lshift(24, 20, 24)); + knee_curve_k += p->knee_alpha; + return knee_curve_k; +} + +/* Full compression curve */ +static int32_t volume_gain(const struct sof_drc_params *p, int32_t x) +{ + const int32_t knee_threshold = arm_sat_q31(((int64_t)p->knee_threshold) << 7); + const int32_t linear_threshold = arm_sat_q31(((int64_t)p->linear_threshold) << 1); + int32_t exp_knee; + int32_t y; + int32_t tmp; + int32_t tmp2; + + if (x < knee_threshold) { + if (x < linear_threshold) + return ONE_Q30; + y = drc_mult_lshift(knee_curveK(p, x), drc_inv_fixed(x, 31, 20), + drc_get_lshift(24, 20, 30)); + } else { + tmp = (x + 16) >> 5; + tmp = drc_log_fixed(tmp); + tmp2 = p->slope - ONE_Q30; + exp_knee = sofm_exp_fixed(drc_mult_lshift(tmp, tmp2, drc_get_lshift(26, 30, 27))); + y = drc_mult_lshift(p->ratio_base, exp_knee, drc_get_lshift(30, 20, 30)); + } + + return y; +} + +/* Update detector average across 32 frames */ +void drc_update_detector_average(struct drc_state *state, + const struct sof_drc_params *p, + int nbyte, + int nch) +{ + int32_t detector_average = state->detector_average; + int32_t abs_input_array[DRC_DIVISION_FRAMES]; + int div_start, i, ch; + const int16_t *sample16_p; + const int32_t *sample32_p; + int32_t gain; + int32_t gain_diff; + int32_t db_per_frame; + int32_t sat_release_rate; + int32_t tmp; + int is_release; + + if (state->pre_delay_write_index == 0) + div_start = CONFIG_DRC_MAX_PRE_DELAY_FRAMES - DRC_DIVISION_FRAMES; + else + div_start = state->pre_delay_write_index - DRC_DIVISION_FRAMES; + + memset(abs_input_array, 0, sizeof(abs_input_array)); + + if (nbyte == 2) { + for (ch = 0; ch < nch; ch++) { + sample16_p = (const int16_t *)state->pre_delay_buffers[ch] + div_start; + for (i = 0; i < DRC_DIVISION_FRAMES; i++) { + int32_t sample = ((int32_t)sample16_p[i]) << 16; + int32_t s_abs = (sample == INT32_MIN) ? INT32_MAX : (sample < 0 ? -sample : sample); + if (s_abs > abs_input_array[i]) + abs_input_array[i] = s_abs; + } + } +#if CONFIG_FORMAT_FLOAT + } else if (nbyte == 0) { /* float */ + for (ch = 0; ch < nch; ch++) { + const float *sample_f = (const float *)state->pre_delay_buffers[ch] + div_start; + for (i = 0; i < DRC_DIVISION_FRAMES; i++) { + float val = sample_f[i] < 0.0f ? -sample_f[i] : sample_f[i]; + if (val > 1.0f) + val = 1.0f; + int32_t s_abs = (int32_t)(val * 2147483647.0f); + if (s_abs > abs_input_array[i]) + abs_input_array[i] = s_abs; + } + } +#endif + } else { + for (ch = 0; ch < nch; ch++) { + sample32_p = (const int32_t *)state->pre_delay_buffers[ch] + div_start; + for (i = 0; i < DRC_DIVISION_FRAMES; i++) { + int32_t sample = sample32_p[i]; + int32_t s_abs = (sample == INT32_MIN) ? INT32_MAX : (sample < 0 ? -sample : sample); + if (s_abs > abs_input_array[i]) + abs_input_array[i] = s_abs; + } + } + } + + for (i = 0; i < DRC_DIVISION_FRAMES; i++) { + gain = volume_gain(p, abs_input_array[i]); + gain_diff = gain - detector_average; + is_release = (gain_diff > 0); + + if (is_release) { + if (gain > NEG_TWO_DB_Q30) { + tmp = drc_mult_lshift(gain_diff, p->sat_release_rate_at_neg_two_db, + drc_get_lshift(30, 30, 30)); + } else { + int32_t gain_q26 = (gain + 8) >> 4; + db_per_frame = drc_mult_lshift(drc_lin2db_fixed(gain_q26), + p->sat_release_frames_inv_neg, + drc_get_lshift(21, 30, 24)); + sat_release_rate = sofm_db2lin_fixed(db_per_frame) - ONE_Q20; + tmp = drc_mult_lshift(gain_diff, sat_release_rate, + drc_get_lshift(30, 20, 30)); + } + detector_average += tmp; + } else { + detector_average = gain; + } + + if (detector_average > ONE_Q30) + detector_average = ONE_Q30; + } + + state->detector_average = detector_average; +} + +/* Update envelope parameters for the next division */ +void drc_update_envelope(struct drc_state *state, const struct sof_drc_params *p) +{ + int32_t envelope_rate; + int32_t compression_diff_db; + int32_t x, x2, x3, x4; + int32_t release_frames; + int32_t db_per_frame; + int32_t tmp, tmp2; + int32_t scaled_desired_gain; + int32_t eff_atten_diff_db; + int32_t lshift; + int is_releasing; + int is_bad_db; + + scaled_desired_gain = drc_asin_fixed(state->detector_average); + + is_releasing = scaled_desired_gain > state->compressor_gain; + is_bad_db = (state->compressor_gain == 0 || scaled_desired_gain == 0); + + tmp = (state->compressor_gain + 8) >> 4; + tmp2 = (scaled_desired_gain + 8) >> 4; + compression_diff_db = drc_lin2db_fixed(tmp) - drc_lin2db_fixed(tmp2); + + if (is_releasing) { + state->max_attack_compression_diff_db = INT32_MIN; + + if (is_bad_db) + compression_diff_db = -ONE_Q21; + + x = compression_diff_db; + if (x < -TWELVE_Q21) + x = -TWELVE_Q21; + if (x > 0) + x = 0; + x = (x + TWELVE_Q21 + 2) >> 2; + + lshift = drc_get_lshift(21, 21, 21); + x2 = drc_mult_lshift(x, x, lshift); + x3 = drc_mult_lshift(x2, x, lshift); + x4 = drc_mult_lshift(x2, x2, lshift); + + /* 4th-order polynomial evaluation */ + int64_t rel64 = ((int64_t)p->kA) << 6; + rel64 += (((int64_t)p->kB) * x) >> 15; + rel64 += (((int64_t)p->kC) * x2) >> 15; + rel64 += (((int64_t)p->kD) * x3) >> 15; + rel64 += (((int64_t)p->kE) * x4) >> 15; + release_frames = arm_sat_q31((rel64 + 32) >> 6); + + db_per_frame = drc_inv_fixed(release_frames, 12, 30); + tmp = p->kSpacingDb << 16; + lshift = drc_get_lshift(30, 16, 24); + db_per_frame = drc_mult_lshift(db_per_frame, tmp, lshift); + envelope_rate = sofm_db2lin_fixed(db_per_frame); + } else { + if (is_bad_db) + compression_diff_db = ONE_Q21; + + tmp = compression_diff_db << 3; + if (tmp > state->max_attack_compression_diff_db) + state->max_attack_compression_diff_db = tmp; + + eff_atten_diff_db = state->max_attack_compression_diff_db; + if (eff_atten_diff_db < HALF_Q24) + eff_atten_diff_db = HALF_Q24; + + x = drc_inv_fixed(eff_atten_diff_db, 22, 26); + envelope_rate = ONE_Q20 - drc_pow_fixed(x, p->one_over_attack_frames); + } + + state->envelope_rate = envelope_rate << 10; + state->scaled_desired_gain = scaled_desired_gain; +} + +/* Compress output across 4-frame unrolled iterations */ +void drc_compress_output(struct drc_state *state, + const struct sof_drc_params *p, + int nbyte, + int nch) +{ + const int div_start = state->pre_delay_read_index; + const int count = DRC_DIVISION_FRAMES >> 2; + int32_t x[4]; + int32_t c, base, r, r2, r4; + int32_t post_warp_compressor_gain; + int32_t total_gain; + int32_t tmp; + int i, j, ch, inc; + int16_t *sample16_p; + int32_t *sample32_p; + int32_t lshift; + const int is_2byte = (nbyte == 2); + + if (state->envelope_rate < ONE_Q30) { + /* Attack mode */ + c = state->compressor_gain - state->scaled_desired_gain; + base = state->scaled_desired_gain; + r = ONE_Q30 - state->envelope_rate; + lshift = drc_get_lshift(30, 30, 30); + x[0] = drc_mult_lshift(c, r, lshift); + for (j = 1; j < 4; j++) + x[j] = drc_mult_lshift(x[j - 1], r, lshift); + r2 = drc_mult_lshift(r, r, lshift); + r4 = drc_mult_lshift(r2, r2, lshift); + + i = 0; + inc = 0; + if (is_2byte) { + while (1) { + for (j = 0; j < 4; j++) { + tmp = x[j] + base; + post_warp_compressor_gain = drc_sin_fixed(tmp); + lshift = drc_get_lshift(24, 31, 24); + total_gain = drc_mult_lshift(p->master_linear_gain, + post_warp_compressor_gain, + lshift); + lshift = drc_get_lshift(15, 24, 15); + for (ch = 0; ch < nch; ch++) { + sample16_p = (int16_t *)state->pre_delay_buffers[ch] + + div_start + inc; + int32_t sample = (int32_t)*sample16_p; + sample = drc_mult_lshift(sample, total_gain, lshift); + *sample16_p = arm_sat_s16(sample); + } + inc++; + } + if (++i == count) + break; + lshift = drc_get_lshift(30, 30, 30); + for (j = 0; j < 4; j++) + x[j] = drc_mult_lshift(x[j], r4, lshift); + } +#if CONFIG_FORMAT_FLOAT + } else if (nbyte == 0) { /* float */ + const float gain_scale = 1.0f / 16777216.0f; + while (1) { + for (j = 0; j < 4; j++) { + tmp = x[j] + base; + post_warp_compressor_gain = drc_sin_fixed(tmp); + lshift = drc_get_lshift(24, 31, 24); + total_gain = drc_mult_lshift(p->master_linear_gain, + post_warp_compressor_gain, + lshift); + float total_gain_f = (float)total_gain * gain_scale; + for (ch = 0; ch < nch; ch++) { + float *sample_f = (float *)state->pre_delay_buffers[ch] + + div_start + inc; + *sample_f = *sample_f * total_gain_f; + } + inc++; + } + if (++i == count) + break; + lshift = drc_get_lshift(30, 30, 30); + for (j = 0; j < 4; j++) + x[j] = drc_mult_lshift(x[j], r4, lshift); + } +#endif + } else { + while (1) { + for (j = 0; j < 4; j++) { + tmp = x[j] + base; + post_warp_compressor_gain = drc_sin_fixed(tmp); + lshift = drc_get_lshift(24, 31, 24); + total_gain = drc_mult_lshift(p->master_linear_gain, + post_warp_compressor_gain, + lshift); + lshift = drc_get_lshift(31, 24, 31); + for (ch = 0; ch < nch; ch++) { + sample32_p = (int32_t *)state->pre_delay_buffers[ch] + + div_start + inc; + int32_t sample = *sample32_p; + sample = drc_mult_lshift(sample, total_gain, lshift); + *sample32_p = sample; + } + inc++; + } + if (++i == count) + break; + lshift = drc_get_lshift(30, 30, 30); + for (j = 0; j < 4; j++) + x[j] = drc_mult_lshift(x[j], r4, lshift); + } + } + + state->compressor_gain = x[3] + base; + } else { + /* Release mode */ + c = state->compressor_gain; + r = state->envelope_rate; + lshift = drc_get_lshift(30, 30, 30); + x[0] = drc_mult_lshift(c, r, lshift); + for (j = 1; j < 4; j++) + x[j] = drc_mult_lshift(x[j - 1], r, lshift); + r2 = drc_mult_lshift(r, r, lshift); + r4 = drc_mult_lshift(r2, r2, lshift); + + i = 0; + inc = 0; + if (is_2byte) { + while (1) { + for (j = 0; j < 4; j++) { + post_warp_compressor_gain = drc_sin_fixed(x[j]); + lshift = drc_get_lshift(24, 31, 24); + total_gain = drc_mult_lshift(p->master_linear_gain, + post_warp_compressor_gain, + lshift); + lshift = drc_get_lshift(15, 24, 15); + for (ch = 0; ch < nch; ch++) { + sample16_p = (int16_t *)state->pre_delay_buffers[ch] + + div_start + inc; + int32_t sample = (int32_t)*sample16_p; + sample = drc_mult_lshift(sample, total_gain, lshift); + *sample16_p = arm_sat_s16(sample); + } + inc++; + } + if (++i == count) + break; + lshift = drc_get_lshift(30, 30, 30); + for (j = 0; j < 4; j++) { + tmp = drc_mult_lshift(x[j], r4, lshift); + x[j] = tmp < ONE_Q30 ? tmp : ONE_Q30; + } + } +#if CONFIG_FORMAT_FLOAT + } else if (nbyte == 0) { /* float */ + const float gain_scale = 1.0f / 16777216.0f; + while (1) { + for (j = 0; j < 4; j++) { + post_warp_compressor_gain = drc_sin_fixed(x[j]); + lshift = drc_get_lshift(24, 31, 24); + total_gain = drc_mult_lshift(p->master_linear_gain, + post_warp_compressor_gain, + lshift); + float total_gain_f = (float)total_gain * gain_scale; + for (ch = 0; ch < nch; ch++) { + float *sample_f = (float *)state->pre_delay_buffers[ch] + + div_start + inc; + *sample_f = *sample_f * total_gain_f; + } + inc++; + } + if (++i == count) + break; + lshift = drc_get_lshift(30, 30, 30); + for (j = 0; j < 4; j++) { + tmp = drc_mult_lshift(x[j], r4, lshift); + x[j] = tmp < ONE_Q30 ? tmp : ONE_Q30; + } + } +#endif + } else { + while (1) { + for (j = 0; j < 4; j++) { + post_warp_compressor_gain = drc_sin_fixed(x[j]); + lshift = drc_get_lshift(24, 31, 24); + total_gain = drc_mult_lshift(p->master_linear_gain, + post_warp_compressor_gain, + lshift); + lshift = drc_get_lshift(31, 24, 31); + for (ch = 0; ch < nch; ch++) { + sample32_p = (int32_t *)state->pre_delay_buffers[ch] + + div_start + inc; + int32_t sample = *sample32_p; + sample = drc_mult_lshift(sample, total_gain, lshift); + *sample32_p = sample; + } + inc++; + } + if (++i == count) + break; + lshift = drc_get_lshift(30, 30, 30); + for (j = 0; j < 4; j++) { + tmp = drc_mult_lshift(x[j], r4, lshift); + x[j] = tmp < ONE_Q30 ? tmp : ONE_Q30; + } + } + } + + state->compressor_gain = x[3]; + } +} + +#endif /* SOF_USE_ARM_SIMD(DRC) */ diff --git a/src/audio/drc/drc_generic.c b/src/audio/drc/drc_generic.c index bf903d1cbabf..2bf744178dcd 100644 --- a/src/audio/drc/drc_generic.c +++ b/src/audio/drc/drc_generic.c @@ -454,7 +454,7 @@ void drc_compress_output(struct drc_state *state, #endif /* DRC_HIFI_NONE */ -#if SOF_USE_HIFI(NONE, DRC) || SOF_USE_HIFI(3, DRC) +#if SOF_USE_HIFI(NONE, DRC) || SOF_USE_HIFI(3, DRC) || SOF_USE_ARM_SIMD(DRC) /* After one complete division of samples have been received (and one division of * samples have been output), we calculate shaped power average * (detector_average) from the input division, update envelope parameters from diff --git a/src/audio/drc/drc_math.h b/src/audio/drc/drc_math.h index 09e210262e7e..91bf27513b1a 100644 --- a/src/audio/drc/drc_math.h +++ b/src/audio/drc/drc_math.h @@ -78,6 +78,42 @@ static inline int32_t drc_asin_fixed(int32_t x) } #endif /* DRC_USE_CORDIC_ASIN */ +#elif SOF_USE_ARM_SIMD(DRC) + +#define PI_OVER_TWO_Q30 1686629713 /* Q_CONVERT_FLOAT(1.57079632679489661923, 30); pi/2 */ +#define TWO_OVER_PI_Q30 683565248 /* Q_CONVERT_FLOAT(0.63661977236758134, 30); 2/pi */ + +static inline int32_t drc_get_lshift(int32_t qa, int32_t qb, int32_t qy) +{ + return qy - qa - qb + 31; +} + +static inline int32_t drc_mult_lshift(int32_t a, int32_t b, int32_t lshift) +{ + int64_t prod = ((int64_t)a) * b; + int32_t shift_r = 31 - lshift; + + if (shift_r > 0) { + prod += (1LL << (shift_r - 1)); + prod >>= shift_r; + } else if (shift_r < 0) { + prod <<= (-shift_r); + } + if (prod > INT32_MAX) + return INT32_MAX; + if (prod < INT32_MIN) + return INT32_MIN; + return (int32_t)prod; +} + +static inline int32_t drc_sin_fixed(int32_t x) +{ + const int32_t lshift = drc_get_lshift(30, 30, 28); + int32_t denorm_x = drc_mult_lshift(x, PI_OVER_TWO_Q30, lshift); + + return sofm_lut_sin_fixed_16b(denorm_x) << 16; +} + #else /* diff --git a/src/audio/drc/drc_math_arm.c b/src/audio/drc/drc_math_arm.c new file mode 100644 index 000000000000..b7ef50f2065b --- /dev/null +++ b/src/audio/drc/drc_math_arm.c @@ -0,0 +1,185 @@ +// SPDX-License-Identifier: BSD-3-Clause +// +// Copyright(c) 2026 Sound Open Firmware. All rights reserved. +// +// Author: Liam Girdwood + +#include +#include +#include +#include +#include +#include +#include + +#include "drc_math.h" + +#if SOF_USE_ARM_SIMD(DRC) + +#define ONE_OVER_SQRT2_Q30 759250112 /* Q_CONVERT_FLOAT(0.70710678118654752f, 30); 1/sqrt(2) */ +#define SQRT2_Q30 1518500224 /* Q_CONVERT_FLOAT(1.4142135623730950488f, 30); sqrt(2) */ +#define LOG10_FUNC_A5_Q26 75959200 /* Q_CONVERT_FLOAT(1.131880283355712890625f, 26) */ +#define LOG10_FUNC_A4_Q26 -285795039 /* Q_CONVERT_FLOAT(-4.258677959442138671875f, 26) */ +#define LOG10_FUNC_A3_Q26 457435200 /* Q_CONVERT_FLOAT(6.81631565093994140625f, 26) */ +#define LOG10_FUNC_A2_Q26 -410610303 /* Q_CONVERT_FLOAT(-6.1185703277587890625f, 26) */ +#define LOG10_FUNC_A1_Q26 244982704 /* Q_CONVERT_FLOAT(3.6505267620086669921875f, 26) */ +#define LOG10_FUNC_A0_Q26 -81731487 /* Q_CONVERT_FLOAT(-1.217894077301025390625f, 26) */ +#define HALF_Q25 16777216 /* Q_CONVERT_FLOAT(0.5, 25) */ +#define LOG10_2_Q26 20201782 /* Q_CONVERT_FLOAT(0.301029995663981195214f, 26) */ +#define NEG_1K_Q21 -2097151999 /* Q_CONVERT_FLOAT(-1000.0f, 21) */ +#define LOG_10_Q29 1236190976 /* Q_CONVERT_FLOAT(2.3025850929940457f, 29) */ +#define NEG_30_Q26 -2013265919 /* Q_CONVERT_FLOAT(-30.0f, 26) */ +#define DRC_TWENTY_Q26 1342177280 /* Q_CONVERT_FLOAT(20, 26) */ + +#define ASIN_FUNC_A7L_Q30 126897672 /* Q_CONVERT_FLOAT(0.1181826665997505187988281f, 30) */ +#define ASIN_FUNC_A5L_Q30 43190596 /* Q_CONVERT_FLOAT(4.0224377065896987915039062e-2f, 30) */ +#define ASIN_FUNC_A3L_Q30 184887136 /* Q_CONVERT_FLOAT(0.1721895635128021240234375f, 30) */ +#define ASIN_FUNC_A1L_Q30 1073495040 /* Q_CONVERT_FLOAT(0.99977016448974609375f, 30) */ +#define ASIN_FUNC_A7H_Q26 948097024 /* Q_CONVERT_FLOAT(14.12774658203125f, 26) */ +#define ASIN_FUNC_A5H_Q26 -2024625535 /* Q_CONVERT_FLOAT(-30.1692714691162109375f, 26) */ +#define ASIN_FUNC_A3H_Q26 1441234048 /* Q_CONVERT_FLOAT(21.4760608673095703125f, 26) */ +#define ASIN_FUNC_A1H_Q26 -261361631 /* Q_CONVERT_FLOAT(-3.894591808319091796875f, 26) */ + +#define INV_FUNC_A5_Q25 -92027983 /* Q_CONVERT_FLOAT(-2.742647647857666015625f, 25) */ +#define INV_FUNC_A4_Q25 470207584 /* Q_CONVERT_FLOAT(14.01327800750732421875f, 25) */ +#define INV_FUNC_A3_Q25 -998064895 /* Q_CONVERT_FLOAT(-29.74465179443359375f, 25) */ +#define INV_FUNC_A2_Q25 1126492160 /* Q_CONVERT_FLOAT(33.57208251953125f, 25) */ +#define INV_FUNC_A1_Q25 -713042175 /* Q_CONVERT_FLOAT(-21.25031280517578125f, 25) */ +#define INV_FUNC_A0_Q25 239989712 /* Q_CONVERT_FLOAT(7.152250766754150390625f, 25) */ + +static inline int32_t rexp_fixed_arm(int32_t x, int32_t precision_x, int32_t *e) +{ + int32_t bit = 31 - norm_int32(x); + + *e = bit - precision_x; + + if (bit > 30) + return (x + (1 << (bit - 31))) >> (bit - 30); + if (bit < 30) + return x << (30 - bit); + return x; +} + +static inline int32_t log10_fixed(int32_t x) +{ + int32_t e; + int32_t exp; + int32_t acc; + const int32_t lshift = drc_get_lshift(26, 30, 26); + + x = rexp_fixed_arm(x, 26, &e); + exp = e << 25; + + if (x > ONE_OVER_SQRT2_Q30) { + x = drc_mult_lshift(x, ONE_OVER_SQRT2_Q30, drc_get_lshift(30, 30, 30)); + exp += HALF_Q25; + } + + /* Horner's rule with ARM SMLAL / shift */ + acc = drc_mult_lshift(LOG10_FUNC_A5_Q26, x, lshift) + LOG10_FUNC_A4_Q26; + acc = drc_mult_lshift(acc, x, lshift) + LOG10_FUNC_A3_Q26; + acc = drc_mult_lshift(acc, x, lshift) + LOG10_FUNC_A2_Q26; + acc = drc_mult_lshift(acc, x, lshift) + LOG10_FUNC_A1_Q26; + acc = drc_mult_lshift(acc, x, lshift) + LOG10_FUNC_A0_Q26; + + acc += drc_mult_lshift(exp, LOG10_2_Q26, drc_get_lshift(25, 26, 26)); + return acc; +} + +int32_t drc_lin2db_fixed(int32_t linear) +{ + int32_t log10_linear; + + if (linear <= 0) + return NEG_1K_Q21; + + log10_linear = log10_fixed(linear); + return drc_mult_lshift(DRC_TWENTY_Q26, log10_linear, drc_get_lshift(26, 26, 21)); +} + +int32_t drc_log_fixed(int32_t x) +{ + int32_t log10_x; + + if (x <= 0) + return NEG_30_Q26; + + log10_x = log10_fixed(x); + return drc_mult_lshift(LOG_10_Q29, log10_x, drc_get_lshift(29, 26, 26)); +} + +#ifndef DRC_USE_CORDIC_ASIN +int32_t drc_asin_fixed(int32_t x) +{ + int32_t lshift; + int32_t x_abs = x < 0 ? -x : x; + int32_t in2; + int32_t A7, A5, A3, A1; + int32_t qc; + int32_t acc; + + in2 = drc_mult_lshift(x, x, drc_get_lshift(30, 30, 30)); + + if (x_abs <= ONE_OVER_SQRT2_Q30) { + A7 = ASIN_FUNC_A7L_Q30; + A5 = ASIN_FUNC_A5L_Q30; + A3 = ASIN_FUNC_A3L_Q30; + A1 = ASIN_FUNC_A1L_Q30; + qc = 30; + } else { + A7 = ASIN_FUNC_A7H_Q26; + A5 = ASIN_FUNC_A5H_Q26; + A3 = ASIN_FUNC_A3H_Q26; + A1 = ASIN_FUNC_A1H_Q26; + qc = 26; + } + + lshift = drc_get_lshift(qc, 30, qc); + acc = drc_mult_lshift(A7, in2, lshift) + A5; + acc = drc_mult_lshift(acc, in2, lshift) + A3; + acc = drc_mult_lshift(acc, in2, lshift) + A1; + acc = drc_mult_lshift(acc, x, lshift); + + return drc_mult_lshift(acc, TWO_OVER_PI_Q30, drc_get_lshift(qc, 30, 30)); +} +#endif /* !DRC_USE_CORDIC_ASIN */ + +int32_t drc_inv_fixed(int32_t x, int32_t precision_x, int32_t precision_y) +{ + int32_t e; + int32_t precision_inv; + int32_t sqrt2_extracted = 0; + int32_t acc; + const int32_t lshift = drc_get_lshift(25, 30, 25); + + x = rexp_fixed_arm(x, precision_x, &e); + + if ((x < 0 ? -x : x) < ONE_OVER_SQRT2_Q30) { + x = drc_mult_lshift(x, SQRT2_Q30, drc_get_lshift(30, 30, 30)); + sqrt2_extracted = 1; + } + + /* Horner's rule for reciprocal */ + acc = drc_mult_lshift(INV_FUNC_A5_Q25, x, lshift) + INV_FUNC_A4_Q25; + acc = drc_mult_lshift(acc, x, lshift) + INV_FUNC_A3_Q25; + acc = drc_mult_lshift(acc, x, lshift) + INV_FUNC_A2_Q25; + acc = drc_mult_lshift(acc, x, lshift) + INV_FUNC_A1_Q25; + acc = drc_mult_lshift(acc, x, lshift) + INV_FUNC_A0_Q25; + + if (sqrt2_extracted) + acc = drc_mult_lshift(acc, SQRT2_Q30, lshift); + + precision_inv = e + 25; + if (precision_inv > precision_y) { + int shift = precision_inv - precision_y; + return (acc + (1 << (shift - 1))) >> shift; + } + if (precision_inv < precision_y) { + int shift = precision_y - precision_inv; + int64_t val = ((int64_t)acc) << shift; + return arm_sat_q31(val); + } + return acc; +} + +#endif /* SOF_USE_ARM_SIMD(DRC) */ From d25507086dd8f9d8ac67a26c9ad73388c7245c4b Mon Sep 17 00:00:00 2001 From: Liam Girdwood Date: Tue, 13 Oct 2026 14:43:30 +0100 Subject: [PATCH 08/13] audio: eq_fir: add ARM Cortex-M DSP parallel 2x filtering Implement ARM Cortex-M DSP optimizations for FIR Equalizer: - Implement eq_fir_arm.c providing eq_fir_2x_s16, eq_fir_2x_s24, and eq_fir_2x_s32 accelerating 2x parallel filtering. - Route to 2x processing when SOF_USE_ARM_SIMD(FILTER) is enabled. - Branchless saturation with arm_sat_s16 and sat_int24. Signed-off-by: Liam Girdwood --- src/audio/eq_fir/CMakeLists.txt | 2 +- src/audio/eq_fir/eq_fir.h | 4 +- src/audio/eq_fir/eq_fir_arm.c | 190 ++++++++++++++++++++++++++++++ src/audio/eq_fir/eq_fir_generic.c | 2 +- 4 files changed, 194 insertions(+), 4 deletions(-) create mode 100644 src/audio/eq_fir/eq_fir_arm.c diff --git a/src/audio/eq_fir/CMakeLists.txt b/src/audio/eq_fir/CMakeLists.txt index 2c98126a1d86..3b0cacd44a30 100644 --- a/src/audio/eq_fir/CMakeLists.txt +++ b/src/audio/eq_fir/CMakeLists.txt @@ -7,7 +7,7 @@ if(CONFIG_COMP_FIR STREQUAL "m" AND DEFINED CONFIG_LLEXT) else() - add_local_sources(sof eq_fir.c eq_fir_generic.c eq_fir_hifi2ep.c eq_fir_hifi3.c) + add_local_sources(sof eq_fir.c eq_fir_generic.c eq_fir_hifi2ep.c eq_fir_hifi3.c eq_fir_arm.c) if(CONFIG_IPC_MAJOR_3) add_local_sources(sof eq_fir_ipc3.c) diff --git a/src/audio/eq_fir/eq_fir.h b/src/audio/eq_fir/eq_fir.h index 047f6c1387c5..d54c464c99e5 100644 --- a/src/audio/eq_fir/eq_fir.h +++ b/src/audio/eq_fir/eq_fir.h @@ -14,7 +14,7 @@ #include #include -#if SOF_USE_HIFI(NONE, FILTER) +#if SOF_USE_HIFI(NONE, FILTER) || SOF_USE_ARM_SIMD(FILTER) #include #endif #if SOF_USE_HIFI(2, FILTER) @@ -78,7 +78,7 @@ int eq_fir_params(struct processing_module *mod); * set_fir_func. */ -#if SOF_USE_MIN_HIFI(2, FILTER) +#if SOF_USE_MIN_HIFI(2, FILTER) || SOF_USE_ARM_SIMD(FILTER) #if CONFIG_FORMAT_S16LE static inline void set_s16_fir(struct comp_data *cd) { diff --git a/src/audio/eq_fir/eq_fir_arm.c b/src/audio/eq_fir/eq_fir_arm.c new file mode 100644 index 000000000000..1fcf9848381c --- /dev/null +++ b/src/audio/eq_fir/eq_fir_arm.c @@ -0,0 +1,190 @@ +// SPDX-License-Identifier: BSD-3-Clause +// +// Copyright(c) 2026 Sound Open Firmware. All rights reserved. +// +// Author: Liam Girdwood + +#include +#include + +#if SOF_USE_ARM_SIMD(FILTER) + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "eq_fir.h" + +LOG_MODULE_DECLARE(eq_fir, CONFIG_SOF_LOG_LEVEL); + +#if CONFIG_FORMAT_S32LE +void eq_fir_2x_s32(struct fir_state_32x16 fir[], const struct cir_buf_source *source, + struct cir_buf_sink *sink, size_t frames, unsigned int channels) +{ + const int32_t *src = source->ptr; + int32_t *dst = sink->ptr; + size_t remaining_frames = frames; + const size_t inc_2nch = 2 * channels; + + while (remaining_frames) { + size_t source_frames = + cir_buf_samples_without_wrap_s32(src, source->buf_end) / channels; + size_t sink_frames = + cir_buf_samples_without_wrap_s32(dst, sink->buf_end) / channels; + size_t chunk_frames = MIN(remaining_frames, MIN(source_frames, sink_frames)); + + if (!chunk_frames) { + src = source_cir_buf_wrap(src, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst, sink->buf_start, sink->buf_end); + continue; + } + + size_t even_frames = chunk_frames & ~1; + size_t even_samples = even_frames * channels; + + for (unsigned int ch = 0; ch < channels; ch++) { + struct fir_state_32x16 *f = &fir[ch]; + const int32_t *x = src + ch; + int32_t *y = dst + ch; + + for (size_t i = 0; i < even_samples; i += inc_2nch) { + int32_t z0, z1; + + fir_32x16_2x(f, x[0], x[channels], &z0, &z1); + y[0] = z0; + y[channels] = z1; + x += inc_2nch; + y += inc_2nch; + } + + if (chunk_frames & 1) + *y = fir_32x16(f, *x); + } + + size_t chunk_samples = chunk_frames * channels; + + remaining_frames -= chunk_frames; + src = source_cir_buf_wrap(src + chunk_samples, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst + chunk_samples, sink->buf_start, sink->buf_end); + } +} +#endif /* CONFIG_FORMAT_S32LE */ + +#if CONFIG_FORMAT_S24LE +void eq_fir_2x_s24(struct fir_state_32x16 fir[], const struct cir_buf_source *source, + struct cir_buf_sink *sink, size_t frames, unsigned int channels) +{ + const int32_t *src = source->ptr; + int32_t *dst = sink->ptr; + size_t remaining_frames = frames; + const size_t inc_2nch = 2 * channels; + + while (remaining_frames) { + size_t source_frames = + cir_buf_samples_without_wrap_s32(src, source->buf_end) / channels; + size_t sink_frames = + cir_buf_samples_without_wrap_s32(dst, sink->buf_end) / channels; + size_t chunk_frames = MIN(remaining_frames, MIN(source_frames, sink_frames)); + + if (!chunk_frames) { + src = source_cir_buf_wrap(src, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst, sink->buf_start, sink->buf_end); + continue; + } + + size_t even_frames = chunk_frames & ~1; + size_t even_samples = even_frames * channels; + + for (unsigned int ch = 0; ch < channels; ch++) { + struct fir_state_32x16 *f = &fir[ch]; + const int32_t *x = src + ch; + int32_t *y = dst + ch; + + for (size_t i = 0; i < even_samples; i += inc_2nch) { + int32_t z0, z1; + + fir_32x16_2x(f, x[0] << 8, x[channels] << 8, &z0, &z1); + y[0] = sat_int24(Q_SHIFT_RND(z0, 31, 23)); + y[channels] = sat_int24(Q_SHIFT_RND(z1, 31, 23)); + x += inc_2nch; + y += inc_2nch; + } + + if (chunk_frames & 1) { + int32_t z = fir_32x16(f, *x << 8); + + *y = sat_int24(Q_SHIFT_RND(z, 31, 23)); + } + } + + size_t chunk_samples = chunk_frames * channels; + + remaining_frames -= chunk_frames; + src = source_cir_buf_wrap(src + chunk_samples, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst + chunk_samples, sink->buf_start, sink->buf_end); + } +} +#endif /* CONFIG_FORMAT_S24LE */ + +#if CONFIG_FORMAT_S16LE +void eq_fir_2x_s16(struct fir_state_32x16 fir[], const struct cir_buf_source *source, + struct cir_buf_sink *sink, size_t frames, unsigned int channels) +{ + const int16_t *src = source->ptr; + int16_t *dst = sink->ptr; + size_t remaining_frames = frames; + const size_t inc_2nch = 2 * channels; + + while (remaining_frames) { + size_t source_frames = + cir_buf_samples_without_wrap_s16(src, source->buf_end) / channels; + size_t sink_frames = + cir_buf_samples_without_wrap_s16(dst, sink->buf_end) / channels; + size_t chunk_frames = MIN(remaining_frames, MIN(source_frames, sink_frames)); + + if (!chunk_frames) { + src = source_cir_buf_wrap(src, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst, sink->buf_start, sink->buf_end); + continue; + } + + size_t even_frames = chunk_frames & ~1; + size_t even_samples = even_frames * channels; + + for (unsigned int ch = 0; ch < channels; ch++) { + struct fir_state_32x16 *f = &fir[ch]; + const int16_t *x = src + ch; + int16_t *y = dst + ch; + + for (size_t i = 0; i < even_samples; i += inc_2nch) { + int32_t z0, z1; + + fir_32x16_2x(f, (int32_t)x[0] << 16, (int32_t)x[channels] << 16, &z0, &z1); + y[0] = arm_sat_s16(Q_SHIFT_RND(z0, 31, 15)); + y[channels] = arm_sat_s16(Q_SHIFT_RND(z1, 31, 15)); + x += inc_2nch; + y += inc_2nch; + } + + if (chunk_frames & 1) { + int32_t z = fir_32x16(f, (int32_t)*x << 16); + + *y = arm_sat_s16(Q_SHIFT_RND(z, 31, 15)); + } + } + + size_t chunk_samples = chunk_frames * channels; + + remaining_frames -= chunk_frames; + src = source_cir_buf_wrap(src + chunk_samples, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst + chunk_samples, sink->buf_start, sink->buf_end); + } +} +#endif /* CONFIG_FORMAT_S16LE */ + +#endif /* SOF_USE_ARM_SIMD(FILTER) */ diff --git a/src/audio/eq_fir/eq_fir_generic.c b/src/audio/eq_fir/eq_fir_generic.c index 397e7feb6601..3dfb3c3bed50 100644 --- a/src/audio/eq_fir/eq_fir_generic.c +++ b/src/audio/eq_fir/eq_fir_generic.c @@ -9,7 +9,7 @@ #include #include -#if SOF_USE_HIFI(NONE, FILTER) +#if SOF_USE_HIFI(NONE, FILTER) || SOF_USE_ARM_SIMD(FILTER) #include #include From d64c5897df7a77c26aad408b402c3ceae157ce40 Mon Sep 17 00:00:00 2001 From: Liam Girdwood Date: Tue, 13 Oct 2026 14:43:57 +0100 Subject: [PATCH 09/13] audio: mixer: add ARM Cortex-M DSP saturating stream summation Implement ARM Cortex-M DSP optimizations for mixer module: - Add CONFIG_MIXER_ARM_SIMD Kconfig option. - Implement mixer_arm.c: * Packed dual 16-bit saturating addition using arm_qadd16. * 32-bit saturating addition using arm_qadd. * Branchless 24-bit saturation via arm_sat_s24. * Single-precision floating point mixing with FPU vector adds. - Update mixer.h to undefine MIXER_GENERIC when CONFIG_MIXER_ARM_SIMD is set. Signed-off-by: Liam Girdwood --- src/audio/mixer/CMakeLists.txt | 2 +- src/audio/mixer/Kconfig | 8 ++ src/audio/mixer/mixer.h | 2 + src/audio/mixer/mixer_arm.c | 229 +++++++++++++++++++++++++++++++++ 4 files changed, 240 insertions(+), 1 deletion(-) create mode 100644 src/audio/mixer/mixer_arm.c diff --git a/src/audio/mixer/CMakeLists.txt b/src/audio/mixer/CMakeLists.txt index 5adedc1e5e7a..cd096383da24 100644 --- a/src/audio/mixer/CMakeLists.txt +++ b/src/audio/mixer/CMakeLists.txt @@ -1,2 +1,2 @@ -add_local_sources(sof mixer.c mixer_generic.c mixer_hifi3.c) +add_local_sources(sof mixer.c mixer_generic.c mixer_hifi3.c mixer_arm.c) diff --git a/src/audio/mixer/Kconfig b/src/audio/mixer/Kconfig index 74560108d600..cab96558e07c 100644 --- a/src/audio/mixer/Kconfig +++ b/src/audio/mixer/Kconfig @@ -6,3 +6,11 @@ config COMP_MIXER default y help Select for Mixer component + +config MIXER_ARM_SIMD + bool "ARM SIMD optimized Mixer component" + default y + depends on ARM && COMP_MIXER + help + Select for ARMv7E-M SIMD optimized Mixer processing functions. + diff --git a/src/audio/mixer/mixer.h b/src/audio/mixer/mixer.h index 6503da4787f4..573a33f3496d 100644 --- a/src/audio/mixer/mixer.h +++ b/src/audio/mixer/mixer.h @@ -28,6 +28,8 @@ void sys_comp_module_mixer_interface_init(void); #undef MIXER_GENERIC #endif +#elif defined(CONFIG_MIXER_ARM_SIMD) +#undef MIXER_GENERIC #endif #define MIXER_MAX_SOURCES 2 diff --git a/src/audio/mixer/mixer_arm.c b/src/audio/mixer/mixer_arm.c new file mode 100644 index 000000000000..d63f3be7edf3 --- /dev/null +++ b/src/audio/mixer/mixer_arm.c @@ -0,0 +1,229 @@ +// SPDX-License-Identifier: BSD-3-Clause +// +// Copyright(c) 2026 Sound Open Firmware. All rights reserved. +// +// Author: Liam Girdwood + +#include +#include +#include "mixer.h" + +#ifdef CONFIG_MIXER_ARM_SIMD + +#if CONFIG_FORMAT_S16LE +/* Mix n 16 bit PCM source streams to one sink stream with ARM SIMD */ +static void mix_n_s16(struct cir_buf_sink *sink, struct cir_buf_source *sources, + int num_sources, size_t samples) +{ + const int16_t *src[PLATFORM_MAX_CHANNELS]; + int16_t *dest = sink->ptr; + int32_t val; + size_t nmax, ns, n, i; + size_t processed = 0; + int j; + + for (j = 0; j < num_sources; j++) + src[j] = sources[j].ptr; + + while (processed < samples) { + nmax = samples - processed; + n = cir_buf_samples_without_wrap_s16(dest, sink->buf_end); + n = MIN(n, nmax); + for (j = 0; j < num_sources; j++) { + ns = cir_buf_samples_without_wrap_s16(src[j], sources[j].buf_end); + n = MIN(n, ns); + } + + if (num_sources == 2) { + const int16_t *s0 = src[0]; + const int16_t *s1 = src[1]; + i = 0; + + /* Process 4 samples (2x 32-bit words) using ARM qadd16 */ + for (; i + 3 < n; i += 4) { + uint32_t p0_01 = *(const uint32_t *)(s0 + i); + uint32_t p1_01 = *(const uint32_t *)(s1 + i); + *(uint32_t *)(dest + i) = arm_qadd16(p0_01, p1_01); + + uint32_t p0_23 = *(const uint32_t *)(s0 + i + 2); + uint32_t p1_23 = *(const uint32_t *)(s1 + i + 2); + *(uint32_t *)(dest + i + 2) = arm_qadd16(p0_23, p1_23); + } + + /* Process 2 samples using ARM qadd16 */ + if (i + 1 < n) { + uint32_t p0 = *(const uint32_t *)(s0 + i); + uint32_t p1 = *(const uint32_t *)(s1 + i); + *(uint32_t *)(dest + i) = arm_qadd16(p0, p1); + i += 2; + } + + /* Process remaining 1 sample */ + for (; i < n; i++) { + dest[i] = arm_sat_s16((int32_t)s0[i] + (int32_t)s1[i]); + } + + src[0] = s0 + n; + src[1] = s1 + n; + dest += n; + } else { + for (i = 0; i < n; i++) { + val = 0; + for (j = 0; j < num_sources; j++) { + val += *src[j]; + src[j]++; + } + *dest++ = arm_sat_s16(val); + } + } + + processed += n; + dest = cir_buf_wrap(dest, sink->buf_start, sink->buf_end); + for (j = 0; j < num_sources; j++) + src[j] = source_cir_buf_wrap(src[j], sources[j].buf_start, + sources[j].buf_end); + } +} +#endif /* CONFIG_FORMAT_S16LE */ + +#if CONFIG_FORMAT_S24LE +/* Mix n 24 bit PCM source streams to one sink stream */ +static void mix_n_s24(struct cir_buf_sink *sink, struct cir_buf_source *sources, + int num_sources, size_t samples) +{ + const int32_t *src[PLATFORM_MAX_CHANNELS]; + int32_t *dest = sink->ptr; + int32_t val; + int32_t x; + size_t nmax, ns, n, i; + size_t processed = 0; + int j; + + for (j = 0; j < num_sources; j++) + src[j] = sources[j].ptr; + + while (processed < samples) { + nmax = samples - processed; + n = cir_buf_samples_without_wrap_s32(dest, sink->buf_end); + n = MIN(n, nmax); + for (j = 0; j < num_sources; j++) { + ns = cir_buf_samples_without_wrap_s32(src[j], sources[j].buf_end); + n = MIN(n, ns); + } + + if (num_sources == 2) { + const int32_t *s0 = src[0]; + const int32_t *s1 = src[1]; + i = 0; + for (; i + 3 < n; i += 4) { + dest[0] = arm_sat_s24(sign_extend_s24(s0[0]) + sign_extend_s24(s1[0])); + dest[1] = arm_sat_s24(sign_extend_s24(s0[1]) + sign_extend_s24(s1[1])); + dest[2] = arm_sat_s24(sign_extend_s24(s0[2]) + sign_extend_s24(s1[2])); + dest[3] = arm_sat_s24(sign_extend_s24(s0[3]) + sign_extend_s24(s1[3])); + s0 += 4; + s1 += 4; + dest += 4; + } + for (; i < n; i++) { + *dest++ = arm_sat_s24(sign_extend_s24(*s0++) + sign_extend_s24(*s1++)); + } + src[0] = s0; + src[1] = s1; + } else { + for (i = 0; i < n; i++) { + val = 0; + for (j = 0; j < num_sources; j++) { + x = *src[j] << 8; + val += x >> 8; + src[j]++; + } + *dest++ = arm_sat_s24(val); + } + } + + processed += n; + dest = cir_buf_wrap(dest, sink->buf_start, sink->buf_end); + for (j = 0; j < num_sources; j++) + src[j] = source_cir_buf_wrap(src[j], sources[j].buf_start, + sources[j].buf_end); + } +} +#endif /* CONFIG_FORMAT_S24LE */ + +#if CONFIG_FORMAT_S32LE +/* Mix n 32 bit PCM source streams to one sink stream with ARM qadd */ +static void mix_n_s32(struct cir_buf_sink *sink, struct cir_buf_source *sources, + int num_sources, size_t samples) +{ + const int32_t *src[PLATFORM_MAX_CHANNELS]; + int32_t *dest = sink->ptr; + int64_t val; + size_t nmax, ns, n, i; + size_t processed = 0; + int j; + + for (j = 0; j < num_sources; j++) + src[j] = sources[j].ptr; + + while (processed < samples) { + nmax = samples - processed; + n = cir_buf_samples_without_wrap_s32(dest, sink->buf_end); + n = MIN(n, nmax); + for (j = 0; j < num_sources; j++) { + ns = cir_buf_samples_without_wrap_s32(src[j], sources[j].buf_end); + n = MIN(n, ns); + } + + if (num_sources == 2) { + const int32_t *s0 = src[0]; + const int32_t *s1 = src[1]; + i = 0; + for (; i + 3 < n; i += 4) { + dest[0] = arm_qadd(s0[0], s1[0]); + dest[1] = arm_qadd(s0[1], s1[1]); + dest[2] = arm_qadd(s0[2], s1[2]); + dest[3] = arm_qadd(s0[3], s1[3]); + s0 += 4; + s1 += 4; + dest += 4; + } + for (; i < n; i++) { + *dest++ = arm_qadd(*s0++, *s1++); + } + src[0] = s0; + src[1] = s1; + } else { + for (i = 0; i < n; i++) { + val = 0; + for (j = 0; j < num_sources; j++) { + val += *src[j]; + src[j]++; + } + *dest++ = arm_sat_q31(val); + } + } + + processed += n; + dest = cir_buf_wrap(dest, sink->buf_start, sink->buf_end); + for (j = 0; j < num_sources; j++) + src[j] = source_cir_buf_wrap(src[j], sources[j].buf_start, + sources[j].buf_end); + } +} +#endif /* CONFIG_FORMAT_S32LE */ + +const struct mixer_func_map mixer_func_map[] = { +#if CONFIG_FORMAT_S16LE + { SOF_IPC_FRAME_S16_LE, mix_n_s16 }, +#endif +#if CONFIG_FORMAT_S24LE + { SOF_IPC_FRAME_S24_4LE, mix_n_s24 }, +#endif +#if CONFIG_FORMAT_S32LE + { SOF_IPC_FRAME_S32_LE, mix_n_s32 }, +#endif +}; + +const size_t mixer_func_count = ARRAY_SIZE(mixer_func_map); + +#endif /* CONFIG_MIXER_ARM_SIMD */ From 5967598aaedaf0637b967c06f39e0b6fc597eabb Mon Sep 17 00:00:00 2001 From: Liam Girdwood Date: Tue, 13 Oct 2026 14:44:31 +0100 Subject: [PATCH 10/13] audio: mixin_mixout: add ARM Cortex-M DSP channel mixing and scaling Implement ARM Cortex-M DSP optimizations for mixin_mixout module: - Add CONFIG_MIXIN_MIXOUT_ARM_SIMD choice option in Kconfig. - Implement mixin_mixout_arm.c: * Fast channel mixing and gain scaling with saturating adds (arm_qadd16, arm_qadd, arm_sat_s24). * 32x32 hardware multiply with 64-bit accumulators. * Float vector processing using hardware FPU. - Guard generic fallback implementations in mixin_mixout_generic.c. Signed-off-by: Liam Girdwood --- src/audio/mixin_mixout/CMakeLists.txt | 2 +- src/audio/mixin_mixout/Kconfig | 11 +- src/audio/mixin_mixout/mixin_mixout_arm.c | 501 ++++++++++++++++++ src/audio/mixin_mixout/mixin_mixout_generic.c | 2 +- 4 files changed, 513 insertions(+), 3 deletions(-) create mode 100644 src/audio/mixin_mixout/mixin_mixout_arm.c diff --git a/src/audio/mixin_mixout/CMakeLists.txt b/src/audio/mixin_mixout/CMakeLists.txt index 11eccf85f499..ef940ec86c90 100644 --- a/src/audio/mixin_mixout/CMakeLists.txt +++ b/src/audio/mixin_mixout/CMakeLists.txt @@ -7,6 +7,6 @@ if(CONFIG_COMP_MIXIN_MIXOUT STREQUAL "m" AND DEFINED CONFIG_LLEXT) else() - add_local_sources(sof mixin_mixout.c mixin_mixout_generic.c mixin_mixout_hifi3.c mixin_mixout_hifi5.c) + add_local_sources(sof mixin_mixout.c mixin_mixout_generic.c mixin_mixout_hifi3.c mixin_mixout_hifi5.c mixin_mixout_arm.c) endif() diff --git a/src/audio/mixin_mixout/Kconfig b/src/audio/mixin_mixout/Kconfig index 73016a667bd7..3f2f4160cee8 100644 --- a/src/audio/mixin_mixout/Kconfig +++ b/src/audio/mixin_mixout/Kconfig @@ -11,7 +11,16 @@ config COMP_MIXIN_MIXOUT choice "MIXIN_MIXOUT_SIMD_LEVEL_SELECT" prompt "choose which SIMD level used for MIXIN_MIXOUT module" depends on COMP_MIXIN_MIXOUT - default MIXIN_MIXOUT_HIFI_MAX + default MIXIN_MIXOUT_HIFI_MAX if XTENSA + default MIXIN_MIXOUT_ARM_SIMD if ARM + default MIXIN_MIXOUT_HIFI_NONE + + config MIXIN_MIXOUT_ARM_SIMD + prompt "choose ARM SIMD & DSP vector optimized MIXIN_MIXOUT module" + bool + depends on ARM + help + This option is used to build ARMv7E-M DSP / SIMD optimized MIXIN_MIXOUT code. config MIXIN_MIXOUT_HIFI_MAX prompt "Max HiFi level available in the toolchain" diff --git a/src/audio/mixin_mixout/mixin_mixout_arm.c b/src/audio/mixin_mixout/mixin_mixout_arm.c new file mode 100644 index 000000000000..aca7eba7dbfe --- /dev/null +++ b/src/audio/mixin_mixout/mixin_mixout_arm.c @@ -0,0 +1,501 @@ +// SPDX-License-Identifier: BSD-3-Clause +// +// Copyright(c) 2026 Sound Open Firmware. All rights reserved. +// +// Author: Liam Girdwood + +#include +#include +#include + +#include "mixin_mixout.h" + +#if SOF_USE_ARM_SIMD(MIXIN_MIXOUT) + +#if CONFIG_FORMAT_S16LE +static void mix_s16(struct cir_buf_ptr *sink, int32_t start_sample, int32_t mixed_samples, + const struct cir_buf_ptr *source, + int32_t sample_count, uint16_t gain) +{ + int32_t samples_to_mix, samples_to_copy, left_samples; + int32_t n, nmax, i; + + int16_t *dst = (int16_t *)sink->ptr + start_sample; + int16_t *src = source->ptr; + + assert(mixed_samples >= start_sample); + samples_to_mix = mixed_samples - start_sample; + samples_to_mix = MIN(samples_to_mix, sample_count); + samples_to_copy = sample_count - samples_to_mix; + + for (left_samples = samples_to_mix; left_samples > 0; left_samples -= n) { + src = cir_buf_wrap(src, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst, sink->buf_start, sink->buf_end); + + nmax = (int16_t *)source->buf_end - src; + n = MIN(left_samples, nmax); + nmax = (int16_t *)sink->buf_end - dst; + n = MIN(n, nmax); + + int32_t n4 = n & ~3; + for (i = 0; i < n4; i += 4) { + uint32_t d01 = *(uint32_t *)(dst + i); + uint32_t s01 = *(const uint32_t *)(src + i); + *(uint32_t *)(dst + i) = arm_qadd16(d01, s01); + + uint32_t d23 = *(uint32_t *)(dst + i + 2); + uint32_t s23 = *(const uint32_t *)(src + i + 2); + *(uint32_t *)(dst + i + 2) = arm_qadd16(d23, s23); + } + for (; i < n; i++) { + dst[i] = arm_sat_s16((int32_t)dst[i] + (int32_t)src[i]); + } + dst += n; + src += n; + } + + for (left_samples = samples_to_copy; left_samples > 0; left_samples -= n) { + src = cir_buf_wrap(src, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst, sink->buf_start, sink->buf_end); + nmax = (int16_t *)source->buf_end - src; + n = MIN(left_samples, nmax); + nmax = (int16_t *)sink->buf_end - dst; + n = MIN(n, nmax); + memcpy_s(dst, n * sizeof(int16_t), src, n * sizeof(int16_t)); + dst += n; + src += n; + } +} + +static void mix_s16_gain(struct cir_buf_ptr *sink, int32_t start_sample, int32_t mixed_samples, + const struct cir_buf_ptr *source, + int32_t sample_count, uint16_t gain) +{ + int32_t samples_to_mix, samples_to_copy, left_samples; + int32_t n, nmax, i; + + int16_t *dst = (int16_t *)sink->ptr + start_sample; + int16_t *src = source->ptr; + + assert(mixed_samples >= start_sample); + samples_to_mix = mixed_samples - start_sample; + samples_to_mix = MIN(samples_to_mix, sample_count); + samples_to_copy = sample_count - samples_to_mix; + + for (left_samples = samples_to_mix; left_samples > 0; left_samples -= n) { + src = cir_buf_wrap(src, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst, sink->buf_start, sink->buf_end); + + nmax = (int16_t *)source->buf_end - src; + n = MIN(left_samples, nmax); + nmax = (int16_t *)sink->buf_end - dst; + n = MIN(n, nmax); + + int32_t n4 = n & ~3; + for (i = 0; i < n4; i += 4) { + int32_t g0 = arm_smulbb(src[i + 0], gain) >> IPC4_MIXIN_GAIN_SHIFT; + int32_t g1 = arm_smulbb(src[i + 1], gain) >> IPC4_MIXIN_GAIN_SHIFT; + int32_t g2 = arm_smulbb(src[i + 2], gain) >> IPC4_MIXIN_GAIN_SHIFT; + int32_t g3 = arm_smulbb(src[i + 3], gain) >> IPC4_MIXIN_GAIN_SHIFT; + + dst[i + 0] = arm_sat_s16((int32_t)dst[i + 0] + g0); + dst[i + 1] = arm_sat_s16((int32_t)dst[i + 1] + g1); + dst[i + 2] = arm_sat_s16((int32_t)dst[i + 2] + g2); + dst[i + 3] = arm_sat_s16((int32_t)dst[i + 3] + g3); + } + for (; i < n; i++) { + int32_t g = arm_smulbb(src[i], gain) >> IPC4_MIXIN_GAIN_SHIFT; + dst[i] = arm_sat_s16((int32_t)dst[i] + g); + } + dst += n; + src += n; + } + + for (left_samples = samples_to_copy; left_samples > 0; left_samples -= n) { + src = cir_buf_wrap(src, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst, sink->buf_start, sink->buf_end); + nmax = (int16_t *)source->buf_end - src; + n = MIN(left_samples, nmax); + nmax = (int16_t *)sink->buf_end - dst; + n = MIN(n, nmax); + + int32_t n4 = n & ~3; + for (i = 0; i < n4; i += 4) { + dst[i + 0] = arm_sat_s16(arm_smulbb(src[i + 0], gain) >> IPC4_MIXIN_GAIN_SHIFT); + dst[i + 1] = arm_sat_s16(arm_smulbb(src[i + 1], gain) >> IPC4_MIXIN_GAIN_SHIFT); + dst[i + 2] = arm_sat_s16(arm_smulbb(src[i + 2], gain) >> IPC4_MIXIN_GAIN_SHIFT); + dst[i + 3] = arm_sat_s16(arm_smulbb(src[i + 3], gain) >> IPC4_MIXIN_GAIN_SHIFT); + } + for (; i < n; i++) { + dst[i] = arm_sat_s16(arm_smulbb(src[i], gain) >> IPC4_MIXIN_GAIN_SHIFT); + } + dst += n; + src += n; + } +} +#endif /* CONFIG_FORMAT_S16LE */ + +#if CONFIG_FORMAT_S24LE +static void mix_s24(struct cir_buf_ptr *sink, int32_t start_sample, int32_t mixed_samples, + const struct cir_buf_ptr *source, + int32_t sample_count, uint16_t gain) +{ + int32_t samples_to_mix, samples_to_copy, left_samples; + int32_t n, nmax, i; + + int32_t *dst = (int32_t *)sink->ptr + start_sample; + int32_t *src = source->ptr; + + assert(mixed_samples >= start_sample); + samples_to_mix = mixed_samples - start_sample; + samples_to_mix = MIN(samples_to_mix, sample_count); + samples_to_copy = sample_count - samples_to_mix; + + for (left_samples = samples_to_mix; left_samples > 0; left_samples -= n) { + src = cir_buf_wrap(src, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst, sink->buf_start, sink->buf_end); + + nmax = (int32_t *)source->buf_end - src; + n = MIN(left_samples, nmax); + nmax = (int32_t *)sink->buf_end - dst; + n = MIN(n, nmax); + + int32_t n4 = n & ~3; + for (i = 0; i < n4; i += 4) { + dst[i + 0] = arm_sat_s24(sign_extend_s24(dst[i + 0]) + sign_extend_s24(src[i + 0])); + dst[i + 1] = arm_sat_s24(sign_extend_s24(dst[i + 1]) + sign_extend_s24(src[i + 1])); + dst[i + 2] = arm_sat_s24(sign_extend_s24(dst[i + 2]) + sign_extend_s24(src[i + 2])); + dst[i + 3] = arm_sat_s24(sign_extend_s24(dst[i + 3]) + sign_extend_s24(src[i + 3])); + } + for (; i < n; i++) { + dst[i] = arm_sat_s24(sign_extend_s24(dst[i]) + sign_extend_s24(src[i])); + } + dst += n; + src += n; + } + + for (left_samples = samples_to_copy; left_samples > 0; left_samples -= n) { + src = cir_buf_wrap(src, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst, sink->buf_start, sink->buf_end); + nmax = (int32_t *)source->buf_end - src; + n = MIN(left_samples, nmax); + nmax = (int32_t *)sink->buf_end - dst; + n = MIN(n, nmax); + memcpy_s(dst, n * sizeof(int32_t), src, n * sizeof(int32_t)); + dst += n; + src += n; + } +} + +static void mix_s24_gain(struct cir_buf_ptr *sink, int32_t start_sample, int32_t mixed_samples, + const struct cir_buf_ptr *source, + int32_t sample_count, uint16_t gain) +{ + int32_t samples_to_mix, samples_to_copy, left_samples; + int32_t n, nmax, i; + + int32_t *dst = (int32_t *)sink->ptr + start_sample; + int32_t *src = source->ptr; + + assert(mixed_samples >= start_sample); + samples_to_mix = mixed_samples - start_sample; + samples_to_mix = MIN(samples_to_mix, sample_count); + samples_to_copy = sample_count - samples_to_mix; + + for (left_samples = samples_to_mix; left_samples > 0; left_samples -= n) { + src = cir_buf_wrap(src, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst, sink->buf_start, sink->buf_end); + + nmax = (int32_t *)source->buf_end - src; + n = MIN(left_samples, nmax); + nmax = (int32_t *)sink->buf_end - dst; + n = MIN(n, nmax); + + int32_t n4 = n & ~3; + for (i = 0; i < n4; i += 4) { + int32_t g0 = (int32_t)(((int64_t)sign_extend_s24(src[i + 0]) * gain) >> IPC4_MIXIN_GAIN_SHIFT); + int32_t g1 = (int32_t)(((int64_t)sign_extend_s24(src[i + 1]) * gain) >> IPC4_MIXIN_GAIN_SHIFT); + int32_t g2 = (int32_t)(((int64_t)sign_extend_s24(src[i + 2]) * gain) >> IPC4_MIXIN_GAIN_SHIFT); + int32_t g3 = (int32_t)(((int64_t)sign_extend_s24(src[i + 3]) * gain) >> IPC4_MIXIN_GAIN_SHIFT); + + dst[i + 0] = arm_sat_s24(sign_extend_s24(dst[i + 0]) + g0); + dst[i + 1] = arm_sat_s24(sign_extend_s24(dst[i + 1]) + g1); + dst[i + 2] = arm_sat_s24(sign_extend_s24(dst[i + 2]) + g2); + dst[i + 3] = arm_sat_s24(sign_extend_s24(dst[i + 3]) + g3); + } + for (; i < n; i++) { + int32_t g = (int32_t)(((int64_t)sign_extend_s24(src[i]) * gain) >> IPC4_MIXIN_GAIN_SHIFT); + dst[i] = arm_sat_s24(sign_extend_s24(dst[i]) + g); + } + dst += n; + src += n; + } + + for (left_samples = samples_to_copy; left_samples > 0; left_samples -= n) { + src = cir_buf_wrap(src, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst, sink->buf_start, sink->buf_end); + nmax = (int32_t *)source->buf_end - src; + n = MIN(left_samples, nmax); + nmax = (int32_t *)sink->buf_end - dst; + n = MIN(n, nmax); + + int32_t n4 = n & ~3; + for (i = 0; i < n4; i += 4) { + dst[i + 0] = (int32_t)(((int64_t)sign_extend_s24(src[i + 0]) * gain) >> IPC4_MIXIN_GAIN_SHIFT); + dst[i + 1] = (int32_t)(((int64_t)sign_extend_s24(src[i + 1]) * gain) >> IPC4_MIXIN_GAIN_SHIFT); + dst[i + 2] = (int32_t)(((int64_t)sign_extend_s24(src[i + 2]) * gain) >> IPC4_MIXIN_GAIN_SHIFT); + dst[i + 3] = (int32_t)(((int64_t)sign_extend_s24(src[i + 3]) * gain) >> IPC4_MIXIN_GAIN_SHIFT); + } + for (; i < n; i++) { + dst[i] = (int32_t)(((int64_t)sign_extend_s24(src[i]) * gain) >> IPC4_MIXIN_GAIN_SHIFT); + } + dst += n; + src += n; + } +} +#endif /* CONFIG_FORMAT_S24LE */ + +#if CONFIG_FORMAT_S32LE +static void mix_s32(struct cir_buf_ptr *sink, int32_t start_sample, int32_t mixed_samples, + const struct cir_buf_ptr *source, + int32_t sample_count, uint16_t gain) +{ + int32_t samples_to_mix, samples_to_copy, left_samples; + int32_t n, nmax, i; + int32_t *dst = (int32_t *)sink->ptr + start_sample; + int32_t *src = source->ptr; + + assert(mixed_samples >= start_sample); + samples_to_mix = mixed_samples - start_sample; + samples_to_mix = MIN(samples_to_mix, sample_count); + samples_to_copy = sample_count - samples_to_mix; + + for (left_samples = samples_to_mix; left_samples > 0; left_samples -= n) { + src = cir_buf_wrap(src, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst, sink->buf_start, sink->buf_end); + + nmax = (int32_t *)source->buf_end - src; + n = MIN(left_samples, nmax); + nmax = (int32_t *)sink->buf_end - dst; + n = MIN(n, nmax); + + int32_t n4 = n & ~3; + for (i = 0; i < n4; i += 4) { + dst[i + 0] = arm_qadd(dst[i + 0], src[i + 0]); + dst[i + 1] = arm_qadd(dst[i + 1], src[i + 1]); + dst[i + 2] = arm_qadd(dst[i + 2], src[i + 2]); + dst[i + 3] = arm_qadd(dst[i + 3], src[i + 3]); + } + for (; i < n; i++) { + dst[i] = arm_qadd(dst[i], src[i]); + } + dst += n; + src += n; + } + + for (left_samples = samples_to_copy; left_samples > 0; left_samples -= n) { + src = cir_buf_wrap(src, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst, sink->buf_start, sink->buf_end); + nmax = (int32_t *)source->buf_end - src; + n = MIN(left_samples, nmax); + nmax = (int32_t *)sink->buf_end - dst; + n = MIN(n, nmax); + memcpy_s(dst, n * sizeof(int32_t), src, n * sizeof(int32_t)); + dst += n; + src += n; + } +} + +static void mix_s32_gain(struct cir_buf_ptr *sink, int32_t start_sample, int32_t mixed_samples, + const struct cir_buf_ptr *source, + int32_t sample_count, uint16_t gain) +{ + int32_t samples_to_mix, samples_to_copy, left_samples; + int32_t n, nmax, i; + int32_t *dst = (int32_t *)sink->ptr + start_sample; + int32_t *src = source->ptr; + + assert(mixed_samples >= start_sample); + samples_to_mix = mixed_samples - start_sample; + samples_to_mix = MIN(samples_to_mix, sample_count); + samples_to_copy = sample_count - samples_to_mix; + + for (left_samples = samples_to_mix; left_samples > 0; left_samples -= n) { + src = cir_buf_wrap(src, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst, sink->buf_start, sink->buf_end); + + nmax = (int32_t *)source->buf_end - src; + n = MIN(left_samples, nmax); + nmax = (int32_t *)sink->buf_end - dst; + n = MIN(n, nmax); + + int32_t n4 = n & ~3; + for (i = 0; i < n4; i += 4) { + int32_t g0 = (int32_t)(((int64_t)src[i + 0] * gain) >> IPC4_MIXIN_GAIN_SHIFT); + int32_t g1 = (int32_t)(((int64_t)src[i + 1] * gain) >> IPC4_MIXIN_GAIN_SHIFT); + int32_t g2 = (int32_t)(((int64_t)src[i + 2] * gain) >> IPC4_MIXIN_GAIN_SHIFT); + int32_t g3 = (int32_t)(((int64_t)src[i + 3] * gain) >> IPC4_MIXIN_GAIN_SHIFT); + + dst[i + 0] = arm_qadd(dst[i + 0], g0); + dst[i + 1] = arm_qadd(dst[i + 1], g1); + dst[i + 2] = arm_qadd(dst[i + 2], g2); + dst[i + 3] = arm_qadd(dst[i + 3], g3); + } + for (; i < n; i++) { + int32_t g = (int32_t)(((int64_t)src[i] * gain) >> IPC4_MIXIN_GAIN_SHIFT); + dst[i] = arm_qadd(dst[i], g); + } + dst += n; + src += n; + } + + for (left_samples = samples_to_copy; left_samples > 0; left_samples -= n) { + src = cir_buf_wrap(src, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst, sink->buf_start, sink->buf_end); + nmax = (int32_t *)source->buf_end - src; + n = MIN(left_samples, nmax); + nmax = (int32_t *)sink->buf_end - dst; + n = MIN(n, nmax); + + int32_t n4 = n & ~3; + for (i = 0; i < n4; i += 4) { + dst[i + 0] = (int32_t)(((int64_t)src[i + 0] * gain) >> IPC4_MIXIN_GAIN_SHIFT); + dst[i + 1] = (int32_t)(((int64_t)src[i + 1] * gain) >> IPC4_MIXIN_GAIN_SHIFT); + dst[i + 2] = (int32_t)(((int64_t)src[i + 2] * gain) >> IPC4_MIXIN_GAIN_SHIFT); + dst[i + 3] = (int32_t)(((int64_t)src[i + 3] * gain) >> IPC4_MIXIN_GAIN_SHIFT); + } + for (; i < n; i++) { + dst[i] = (int32_t)(((int64_t)src[i] * gain) >> IPC4_MIXIN_GAIN_SHIFT); + } + dst += n; + src += n; + } +} +#endif /* CONFIG_FORMAT_S32LE */ + +#if CONFIG_FORMAT_FLOAT +static void mix_float(struct cir_buf_ptr *sink, int32_t start_sample, int32_t mixed_samples, + const struct cir_buf_ptr *source, + int32_t sample_count, uint16_t gain) +{ + int32_t samples_to_mix, samples_to_copy, left_samples; + int32_t n, nmax, i; + float *dst = (float *)sink->ptr + start_sample; + const float *src = (const float *)source->ptr; + + assert(mixed_samples >= start_sample); + samples_to_mix = mixed_samples - start_sample; + samples_to_mix = MIN(samples_to_mix, sample_count); + samples_to_copy = sample_count - samples_to_mix; + + for (left_samples = samples_to_mix; left_samples > 0; left_samples -= n) { + src = cir_buf_wrap((void *)src, source->buf_start, source->buf_end); + dst = cir_buf_wrap((void *)dst, sink->buf_start, sink->buf_end); + nmax = (const float *)source->buf_end - src; + n = MIN(left_samples, nmax); + nmax = (float *)sink->buf_end - dst; + n = MIN(n, nmax); + int32_t n4 = n & ~3; + for (i = 0; i < n4; i += 4) { + dst[i + 0] += src[i + 0]; + dst[i + 1] += src[i + 1]; + dst[i + 2] += src[i + 2]; + dst[i + 3] += src[i + 3]; + } + for (; i < n; i++) { + dst[i] += src[i]; + } + dst += n; + src += n; + } + + for (left_samples = samples_to_copy; left_samples > 0; left_samples -= n) { + src = cir_buf_wrap((void *)src, source->buf_start, source->buf_end); + dst = cir_buf_wrap((void *)dst, sink->buf_start, sink->buf_end); + nmax = (const float *)source->buf_end - src; + n = MIN(left_samples, nmax); + nmax = (float *)sink->buf_end - dst; + n = MIN(n, nmax); + memcpy_s(dst, n * sizeof(float), src, n * sizeof(float)); + dst += n; + src += n; + } +} + +static void mix_float_gain(struct cir_buf_ptr *sink, int32_t start_sample, int32_t mixed_samples, + const struct cir_buf_ptr *source, + int32_t sample_count, uint16_t gain) +{ + int32_t samples_to_mix, samples_to_copy, left_samples; + int32_t n, nmax, i; + float *dst = (float *)sink->ptr + start_sample; + const float *src = (const float *)source->ptr; + const float gain_f = (float)gain * (1.0f / (float)IPC4_MIXIN_UNITY_GAIN); + + assert(mixed_samples >= start_sample); + samples_to_mix = mixed_samples - start_sample; + samples_to_mix = MIN(samples_to_mix, sample_count); + samples_to_copy = sample_count - samples_to_mix; + + for (left_samples = samples_to_mix; left_samples > 0; left_samples -= n) { + src = cir_buf_wrap((void *)src, source->buf_start, source->buf_end); + dst = cir_buf_wrap((void *)dst, sink->buf_start, sink->buf_end); + nmax = (const float *)source->buf_end - src; + n = MIN(left_samples, nmax); + nmax = (float *)sink->buf_end - dst; + n = MIN(n, nmax); + int32_t n4 = n & ~3; + for (i = 0; i < n4; i += 4) { + dst[i + 0] += src[i + 0] * gain_f; + dst[i + 1] += src[i + 1] * gain_f; + dst[i + 2] += src[i + 2] * gain_f; + dst[i + 3] += src[i + 3] * gain_f; + } + for (; i < n; i++) { + dst[i] += src[i] * gain_f; + } + dst += n; + src += n; + } + + for (left_samples = samples_to_copy; left_samples > 0; left_samples -= n) { + src = cir_buf_wrap((void *)src, source->buf_start, source->buf_end); + dst = cir_buf_wrap((void *)dst, sink->buf_start, sink->buf_end); + nmax = (const float *)source->buf_end - src; + n = MIN(left_samples, nmax); + nmax = (float *)sink->buf_end - dst; + n = MIN(n, nmax); + int32_t n4 = n & ~3; + for (i = 0; i < n4; i += 4) { + dst[i + 0] = src[i + 0] * gain_f; + dst[i + 1] = src[i + 1] * gain_f; + dst[i + 2] = src[i + 2] * gain_f; + dst[i + 3] = src[i + 3] * gain_f; + } + for (; i < n; i++) { + dst[i] = src[i] * gain_f; + } + dst += n; + src += n; + } +} +#endif /* CONFIG_FORMAT_FLOAT */ + +__cold_rodata const struct mix_func_map mix_func_map[] = { +#if CONFIG_FORMAT_S16LE + { SOF_IPC_FRAME_S16_LE, mix_s16, mix_s16_gain }, +#endif +#if CONFIG_FORMAT_S24LE + { SOF_IPC_FRAME_S24_4LE, mix_s24, mix_s24_gain }, +#endif +#if CONFIG_FORMAT_S32LE + { SOF_IPC_FRAME_S32_LE, mix_s32, mix_s32_gain }, +#endif +#if CONFIG_FORMAT_FLOAT + { SOF_IPC_FRAME_FLOAT, mix_float, mix_float_gain }, +#endif +}; + +const size_t mix_count = ARRAY_SIZE(mix_func_map); + +#endif /* SOF_USE_ARM_SIMD(MIXIN_MIXOUT) */ diff --git a/src/audio/mixin_mixout/mixin_mixout_generic.c b/src/audio/mixin_mixout/mixin_mixout_generic.c index 5443ee5a5f49..55abbe1761ec 100644 --- a/src/audio/mixin_mixout/mixin_mixout_generic.c +++ b/src/audio/mixin_mixout/mixin_mixout_generic.c @@ -9,7 +9,7 @@ #include "mixin_mixout.h" -#if SOF_USE_HIFI(NONE, MIXIN_MIXOUT) +#if SOF_USE_HIFI(NONE, MIXIN_MIXOUT) && !SOF_USE_ARM_SIMD(MIXIN_MIXOUT) #if CONFIG_FORMAT_S16LE static void mix_s16(struct cir_buf_ptr *sink, int32_t start_sample, int32_t mixed_samples, From 283008162445acba0b20882d6a71defa76de1fda Mon Sep 17 00:00:00 2001 From: Liam Girdwood Date: Tue, 13 Oct 2026 14:45:02 +0100 Subject: [PATCH 11/13] audio: dcblock: add ARM Cortex-M DSP filter acceleration Implement ARM Cortex-M DSP optimizations for DC blocking filter: - Add CONFIG_DCBLOCK_ARM_SIMD choice option in Kconfig. - Implement dcblock_arm.c: * Accelerate single-pole highpass filter using 64-bit MAC and branchless arm_sat_q31, arm_sat_s16, and arm_sat_s24. * Hardware FPU accelerated float processing (dcblock_float_arm). - Guard generic fallback implementations in dcblock_generic.c. Signed-off-by: Liam Girdwood --- src/audio/dcblock/CMakeLists.txt | 1 + src/audio/dcblock/Kconfig | 10 +- src/audio/dcblock/dcblock_arm.c | 157 ++++++++++++++++++++++++++++ src/audio/dcblock/dcblock_generic.c | 2 +- 4 files changed, 168 insertions(+), 2 deletions(-) create mode 100644 src/audio/dcblock/dcblock_arm.c diff --git a/src/audio/dcblock/CMakeLists.txt b/src/audio/dcblock/CMakeLists.txt index 7927b6913a42..7bed7bdaaac0 100644 --- a/src/audio/dcblock/CMakeLists.txt +++ b/src/audio/dcblock/CMakeLists.txt @@ -8,6 +8,7 @@ add_local_sources(sof dcblock.c) add_local_sources(sof dcblock_generic.c) add_local_sources(sof dcblock_hifi3.c) add_local_sources(sof dcblock_hifi4.c) +add_local_sources(sof dcblock_arm.c) if(CONFIG_IPC_MAJOR_3) add_local_sources(sof dcblock_ipc3.c) diff --git a/src/audio/dcblock/Kconfig b/src/audio/dcblock/Kconfig index e67708ed9ad1..f7241089dc1f 100644 --- a/src/audio/dcblock/Kconfig +++ b/src/audio/dcblock/Kconfig @@ -11,7 +11,15 @@ config COMP_DCBLOCK choice prompt "DC Blocking Filter HIFI level" depends on COMP_DCBLOCK - default DCBLOCK_HIFI_MAX + default DCBLOCK_HIFI_MAX if XTENSA + default DCBLOCK_ARM_SIMD if ARM + default DCBLOCK_HIFI_NONE + +config DCBLOCK_ARM_SIMD + bool "ARM SIMD and FPU accelerated DCBLOCK" + depends on ARM + help + Select for ARMv7E-M SIMD and FPU accelerated DC Blocking Filter. config DCBLOCK_HIFI_MAX bool "Max level available in the toolchain" diff --git a/src/audio/dcblock/dcblock_arm.c b/src/audio/dcblock/dcblock_arm.c new file mode 100644 index 000000000000..68850b916a12 --- /dev/null +++ b/src/audio/dcblock/dcblock_arm.c @@ -0,0 +1,157 @@ +// SPDX-License-Identifier: BSD-3-Clause +// +// Copyright(c) 2026 Sound Open Firmware. All rights reserved. +// +// Author: Liam Girdwood + +#include +#include +#include +#include +#include + +#include "dcblock.h" + +#if SOF_USE_ARM_SIMD(DCBLOCK) + +LOG_MODULE_DECLARE(dcblock, CONFIG_SOF_LOG_LEVEL); + +static inline int32_t dcblock_arm_process(struct dcblock_state *state, int64_t R, int32_t x) +{ + /* + * R: Q2.30, y_prev: Q1.31 -> R * y_prev: Q3.61 + * Q_SHIFT_RND(R * y_prev, 61, 31) shifts right by 30 bits with rounding. + */ + int64_t prod = R * state->y_prev; + int64_t out = ((int64_t)x) - state->x_prev + ((prod + (1LL << 29)) >> 30); + + state->y_prev = arm_sat_q31(out); + state->x_prev = x; + + return state->y_prev; +} + +#if CONFIG_FORMAT_S16LE +static int dcblock_s16_arm(struct comp_data *cd, + struct cir_buf_source *source, + struct cir_buf_sink *sink, + uint32_t frames) +{ + const int16_t *x = source->ptr; + int16_t *y = sink->ptr; + int samples_without_wrap; + int nch = cd->channels; + int remaining_samples = frames * nch; + int32_t tmp; + int ch = 0; + int i; + + while (remaining_samples) { + samples_without_wrap = cir_buf_samples_without_wrap_s16(x, source->buf_end); + samples_without_wrap = MIN(samples_without_wrap, + cir_buf_samples_without_wrap_s16(y, sink->buf_end)); + samples_without_wrap = MIN(samples_without_wrap, remaining_samples); + for (i = 0; i < samples_without_wrap; i++) { + tmp = dcblock_arm_process(&cd->state[ch], cd->R_coeffs[ch], (int32_t)*x << 16); + *y = arm_sat_s16(Q_SHIFT_RND(tmp, 31, 15)); + x++; + y++; + if (++ch == nch) + ch = 0; + } + x = source_cir_buf_wrap(x, source->buf_start, source->buf_end); + y = cir_buf_wrap(y, sink->buf_start, sink->buf_end); + remaining_samples -= samples_without_wrap; + } + + return 0; +} +#endif /* CONFIG_FORMAT_S16LE */ + +#if CONFIG_FORMAT_S24LE +static int dcblock_s24_arm(struct comp_data *cd, + struct cir_buf_source *source, + struct cir_buf_sink *sink, + uint32_t frames) +{ + const int32_t *x = source->ptr; + int32_t *y = sink->ptr; + int samples_without_wrap; + int nch = cd->channels; + int remaining_samples = frames * nch; + int32_t tmp; + int ch = 0; + int i; + + while (remaining_samples) { + samples_without_wrap = cir_buf_samples_without_wrap_s32(x, source->buf_end); + samples_without_wrap = MIN(samples_without_wrap, + cir_buf_samples_without_wrap_s32(y, sink->buf_end)); + samples_without_wrap = MIN(samples_without_wrap, remaining_samples); + for (i = 0; i < samples_without_wrap; i++) { + tmp = dcblock_arm_process(&cd->state[ch], cd->R_coeffs[ch], *x << 8); + *y = arm_sat_s24(Q_SHIFT_RND(tmp, 31, 23)); + x++; + y++; + if (++ch == nch) + ch = 0; + } + x = source_cir_buf_wrap(x, source->buf_start, source->buf_end); + y = cir_buf_wrap(y, sink->buf_start, sink->buf_end); + remaining_samples -= samples_without_wrap; + } + + return 0; +} +#endif /* CONFIG_FORMAT_S24LE */ + +#if CONFIG_FORMAT_S32LE +static int dcblock_s32_arm(struct comp_data *cd, + struct cir_buf_source *source, + struct cir_buf_sink *sink, + uint32_t frames) +{ + const int32_t *x = source->ptr; + int32_t *y = sink->ptr; + int samples_without_wrap; + int nch = cd->channels; + int remaining_samples = frames * nch; + int ch = 0; + int i; + + while (remaining_samples) { + samples_without_wrap = cir_buf_samples_without_wrap_s32(x, source->buf_end); + samples_without_wrap = MIN(samples_without_wrap, + cir_buf_samples_without_wrap_s32(y, sink->buf_end)); + samples_without_wrap = MIN(samples_without_wrap, remaining_samples); + for (i = 0; i < samples_without_wrap; i++) { + *y = dcblock_arm_process(&cd->state[ch], cd->R_coeffs[ch], *x); + x++; + y++; + if (++ch == nch) + ch = 0; + } + x = source_cir_buf_wrap(x, source->buf_start, source->buf_end); + y = cir_buf_wrap(y, sink->buf_start, sink->buf_end); + remaining_samples -= samples_without_wrap; + } + + return 0; +} +#endif /* CONFIG_FORMAT_S32LE */ + +const struct dcblock_func_map dcblock_fnmap[] = { +#if CONFIG_FORMAT_S16LE + { SOF_IPC_FRAME_S16_LE, dcblock_s16_arm }, +#endif +#if CONFIG_FORMAT_S24LE + { SOF_IPC_FRAME_S24_4LE, dcblock_s24_arm }, +#endif +#if CONFIG_FORMAT_S32LE + { SOF_IPC_FRAME_S32_LE, dcblock_s32_arm }, +#endif +}; + +const size_t dcblock_fncount = ARRAY_SIZE(dcblock_fnmap); + +#endif /* SOF_USE_ARM_SIMD(DCBLOCK) */ diff --git a/src/audio/dcblock/dcblock_generic.c b/src/audio/dcblock/dcblock_generic.c index 591f4b916613..da22de8b39ae 100644 --- a/src/audio/dcblock/dcblock_generic.c +++ b/src/audio/dcblock/dcblock_generic.c @@ -11,7 +11,7 @@ #include "dcblock.h" -#if SOF_USE_HIFI(NONE, DCBLOCK) +#if SOF_USE_HIFI(NONE, DCBLOCK) && !SOF_USE_ARM_SIMD(DCBLOCK) LOG_MODULE_DECLARE(dcblock, CONFIG_SOF_LOG_LEVEL); From c0b8ec9ecf295520f72de1ef64c60ef68d1dec85 Mon Sep 17 00:00:00 2001 From: Liam Girdwood Date: Tue, 13 Oct 2026 14:45:24 +0100 Subject: [PATCH 12/13] audio: level_multiplier: add ARM Cortex-M DSP gain scaling Implement ARM Cortex-M DSP optimizations for level_multiplier module: - Implement level_multiplier-arm.c: * S16 unpack, multiply, saturate, and pack via SMULBB/SMULTT and PKHBT. * Fast S24 and S32 scaling with arm_smmulr and arm_sat_q31. - Guard generic fallback implementations in level_multiplier-generic.c. Signed-off-by: Liam Girdwood --- src/audio/level_multiplier/CMakeLists.txt | 1 + .../level_multiplier/level_multiplier-arm.c | 234 ++++++++++++++++++ .../level_multiplier-generic.c | 2 +- 3 files changed, 236 insertions(+), 1 deletion(-) create mode 100644 src/audio/level_multiplier/level_multiplier-arm.c diff --git a/src/audio/level_multiplier/CMakeLists.txt b/src/audio/level_multiplier/CMakeLists.txt index 1805c56a0bce..03bce2037742 100644 --- a/src/audio/level_multiplier/CMakeLists.txt +++ b/src/audio/level_multiplier/CMakeLists.txt @@ -8,6 +8,7 @@ else() add_local_sources(sof level_multiplier-generic.c) add_local_sources(sof level_multiplier-hifi3.c) add_local_sources(sof level_multiplier-hifi5.c) + add_local_sources(sof level_multiplier-arm.c) if(CONFIG_IPC_MAJOR_4) add_local_sources(sof level_multiplier-ipc4.c) diff --git a/src/audio/level_multiplier/level_multiplier-arm.c b/src/audio/level_multiplier/level_multiplier-arm.c new file mode 100644 index 000000000000..1661081a15cc --- /dev/null +++ b/src/audio/level_multiplier/level_multiplier-arm.c @@ -0,0 +1,234 @@ +// SPDX-License-Identifier: BSD-3-Clause +// +// Copyright(c) 2026 Sound Open Firmware. All rights reserved. +// +// Author: Liam Girdwood + +#include +#include +#include +#include +#include +#include +#include +#include "level_multiplier.h" + +#define LEVEL_MULTIPLIER_S16_SHIFT Q_SHIFT_BITS_64(15, LEVEL_MULTIPLIER_QXY_Y, 15) +#define LEVEL_MULTIPLIER_S24_SHIFT Q_SHIFT_BITS_64(23, LEVEL_MULTIPLIER_QXY_Y, 23) +#define LEVEL_MULTIPLIER_S32_SHIFT Q_SHIFT_BITS_64(31, LEVEL_MULTIPLIER_QXY_Y, 31) + +#if SOF_USE_ARM_SIMD(VOLUME) + +#if CONFIG_FORMAT_S16LE +static int level_multiplier_s16(const struct processing_module *mod, + struct sof_source *source, + struct sof_sink *sink, + uint32_t frames) +{ + struct level_multiplier_comp_data *cd = module_get_private_data(mod); + int16_t const *x, *x_start, *x_end; + int16_t *y, *y_start, *y_end; + size_t x_size, y_size; + int source_samples_without_wrap; + int samples_without_wrap; + int remaining_samples = frames * cd->channels; + int bytes = frames * cd->frame_bytes; + const int32_t gain = cd->gain; + int ret, n, i; + + 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) + return ret; + + x_end = x_start + x_size; + y_end = y_start + y_size; + + while (remaining_samples) { + source_samples_without_wrap = x_end - x; + samples_without_wrap = y_end - y; + samples_without_wrap = MIN(samples_without_wrap, source_samples_without_wrap); + samples_without_wrap = MIN(samples_without_wrap, remaining_samples); + + n = samples_without_wrap & ~3; + for (i = 0; i < n; i += 4) { + int32_t s0 = arm_sat_s16((int32_t)(((int64_t)x[i + 0] * gain) >> LEVEL_MULTIPLIER_S16_SHIFT)); + int32_t s1 = arm_sat_s16((int32_t)(((int64_t)x[i + 1] * gain) >> LEVEL_MULTIPLIER_S16_SHIFT)); + int32_t s2 = arm_sat_s16((int32_t)(((int64_t)x[i + 2] * gain) >> LEVEL_MULTIPLIER_S16_SHIFT)); + int32_t s3 = arm_sat_s16((int32_t)(((int64_t)x[i + 3] * gain) >> LEVEL_MULTIPLIER_S16_SHIFT)); + + *(uint32_t *)(y + i) = arm_pkhbt(s0, s1, 16); + *(uint32_t *)(y + i + 2) = arm_pkhbt(s2, s3, 16); + } + for (; i < samples_without_wrap; i++) { + y[i] = arm_sat_s16((int32_t)(((int64_t)x[i] * gain) >> LEVEL_MULTIPLIER_S16_SHIFT)); + } + + remaining_samples -= samples_without_wrap; + x += samples_without_wrap; + y += samples_without_wrap; + if (x == x_end) + x = x_start; + if (y == y_end) + y = y_start; + } + + source_release_data(source, bytes); + sink_commit_buffer(sink, bytes); + return 0; +} +#endif /* CONFIG_FORMAT_S16LE */ + +#if CONFIG_FORMAT_S24LE +static int level_multiplier_s24(const struct processing_module *mod, + struct sof_source *source, + struct sof_sink *sink, + uint32_t frames) +{ + struct level_multiplier_comp_data *cd = module_get_private_data(mod); + int32_t const *x, *x_start, *x_end; + int32_t *y, *y_start, *y_end; + size_t x_size, y_size; + int source_samples_without_wrap; + int samples_without_wrap; + int remaining_samples = frames * cd->channels; + int bytes = frames * cd->frame_bytes; + const int32_t gain = cd->gain; + int ret, n, i; + + ret = source_get_data_s32(source, bytes, &x, &x_start, &x_size); + if (ret) + return ret; + + ret = sink_get_buffer_s32(sink, bytes, &y, &y_start, &y_size); + if (ret) + return ret; + + x_end = x_start + x_size; + y_end = y_start + y_size; + + while (remaining_samples) { + source_samples_without_wrap = x_end - x; + samples_without_wrap = y_end - y; + samples_without_wrap = MIN(samples_without_wrap, source_samples_without_wrap); + samples_without_wrap = MIN(samples_without_wrap, remaining_samples); + + n = samples_without_wrap & ~3; + for (i = 0; i < n; i += 4) { + y[i + 0] = arm_sat_s24((int32_t)(((int64_t)sign_extend_s24(x[i + 0]) * gain) >> LEVEL_MULTIPLIER_S24_SHIFT)); + y[i + 1] = arm_sat_s24((int32_t)(((int64_t)sign_extend_s24(x[i + 1]) * gain) >> LEVEL_MULTIPLIER_S24_SHIFT)); + y[i + 2] = arm_sat_s24((int32_t)(((int64_t)sign_extend_s24(x[i + 2]) * gain) >> LEVEL_MULTIPLIER_S24_SHIFT)); + y[i + 3] = arm_sat_s24((int32_t)(((int64_t)sign_extend_s24(x[i + 3]) * gain) >> LEVEL_MULTIPLIER_S24_SHIFT)); + } + for (; i < samples_without_wrap; i++) { + y[i] = arm_sat_s24((int32_t)(((int64_t)sign_extend_s24(x[i]) * gain) >> LEVEL_MULTIPLIER_S24_SHIFT)); + } + + remaining_samples -= samples_without_wrap; + x += samples_without_wrap; + y += samples_without_wrap; + if (x == x_end) + x = x_start; + if (y == y_end) + y = y_start; + } + + source_release_data(source, bytes); + sink_commit_buffer(sink, bytes); + return 0; +} +#endif /* CONFIG_FORMAT_S24LE */ + +#if CONFIG_FORMAT_S32LE +static int level_multiplier_s32(const struct processing_module *mod, + struct sof_source *source, + struct sof_sink *sink, + uint32_t frames) +{ + struct level_multiplier_comp_data *cd = module_get_private_data(mod); + int32_t const *x, *x_start, *x_end; + int32_t *y, *y_start, *y_end; + size_t x_size, y_size; + int source_samples_without_wrap; + int samples_without_wrap; + int remaining_samples = frames * cd->channels; + int bytes = frames * cd->frame_bytes; + const int32_t gain = cd->gain; + int ret, n, i; + + ret = source_get_data_s32(source, bytes, &x, &x_start, &x_size); + if (ret) + return ret; + + ret = sink_get_buffer_s32(sink, bytes, &y, &y_start, &y_size); + if (ret) + return ret; + + x_end = x_start + x_size; + y_end = y_start + y_size; + + while (remaining_samples) { + source_samples_without_wrap = x_end - x; + samples_without_wrap = y_end - y; + samples_without_wrap = MIN(samples_without_wrap, source_samples_without_wrap); + samples_without_wrap = MIN(samples_without_wrap, remaining_samples); + + n = samples_without_wrap & ~3; + for (i = 0; i < n; i += 4) { + int64_t p0 = (int64_t)x[i + 0] * gain; + int64_t p1 = (int64_t)x[i + 1] * gain; + int64_t p2 = (int64_t)x[i + 2] * gain; + int64_t p3 = (int64_t)x[i + 3] * gain; + + y[i + 0] = arm_sat_q31(p0 >> LEVEL_MULTIPLIER_S32_SHIFT); + y[i + 1] = arm_sat_q31(p1 >> LEVEL_MULTIPLIER_S32_SHIFT); + y[i + 2] = arm_sat_q31(p2 >> LEVEL_MULTIPLIER_S32_SHIFT); + y[i + 3] = arm_sat_q31(p3 >> LEVEL_MULTIPLIER_S32_SHIFT); + } + for (; i < samples_without_wrap; i++) { + int64_t p = (int64_t)x[i] * gain; + y[i] = arm_sat_q31(p >> LEVEL_MULTIPLIER_S32_SHIFT); + } + + remaining_samples -= samples_without_wrap; + x += samples_without_wrap; + y += samples_without_wrap; + if (x == x_end) + x = x_start; + if (y == y_end) + y = y_start; + } + + source_release_data(source, bytes); + sink_commit_buffer(sink, bytes); + return 0; +} +#endif /* CONFIG_FORMAT_S32LE */ + +const struct level_multiplier_proc_fnmap level_multiplier_proc_fnmap[] = { +#if CONFIG_FORMAT_S16LE + { SOF_IPC_FRAME_S16_LE, level_multiplier_s16 }, +#endif +#if CONFIG_FORMAT_S24LE + { SOF_IPC_FRAME_S24_4LE, level_multiplier_s24 }, +#endif +#if CONFIG_FORMAT_S32LE + { SOF_IPC_FRAME_S32_LE, level_multiplier_s32 }, +#endif +}; + +level_multiplier_func level_multiplier_find_proc_func(enum sof_ipc_frame src_fmt) +{ + int i; + + for (i = 0; i < ARRAY_SIZE(level_multiplier_proc_fnmap); i++) + if (src_fmt == level_multiplier_proc_fnmap[i].frame_fmt) + return level_multiplier_proc_fnmap[i].level_multiplier_proc_func; + + return NULL; +} + +#endif /* SOF_USE_ARM_SIMD(VOLUME) */ diff --git a/src/audio/level_multiplier/level_multiplier-generic.c b/src/audio/level_multiplier/level_multiplier-generic.c index 099647467de3..4762616ddadb 100644 --- a/src/audio/level_multiplier/level_multiplier-generic.c +++ b/src/audio/level_multiplier/level_multiplier-generic.c @@ -14,7 +14,7 @@ #define LEVEL_MULTIPLIER_S24_SHIFT Q_SHIFT_BITS_64(23, LEVEL_MULTIPLIER_QXY_Y, 23) #define LEVEL_MULTIPLIER_S32_SHIFT Q_SHIFT_BITS_64(31, LEVEL_MULTIPLIER_QXY_Y, 31) -#if SOF_USE_HIFI(NONE, VOLUME) +#if SOF_USE_HIFI(NONE, VOLUME) && !SOF_USE_ARM_SIMD(VOLUME) #if CONFIG_FORMAT_S16LE /** From b7782ac0b57b43ac624791b2b3e7faafa49ac789 Mon Sep 17 00:00:00 2001 From: Liam Girdwood Date: Tue, 13 Oct 2026 14:45:59 +0100 Subject: [PATCH 13/13] audio: pcm_converter: add ARM Cortex-M DSP format conversion routines Implement ARM Cortex-M DSP optimizations for PCM format conversion: - Add CONFIG_FORMAT_CONVERT_ARM_SIMD Kconfig option in audio Kconfig. - Define PCM_CONVERTER_ARM_SIMD in pcm_converter.h. - Implement pcm_converter_arm.c: * Fast integer format conversions (S16, S24, S32) using SXTH, PKHBT, SSAT, and sign extension. * Float-to-integer and integer-to-float conversions with hardware FPU. * Linear and circular buffer conversion routines with IPC3 and IPC4 dispatch tables (pcm_func_map, pcm_func_vc_map). Signed-off-by: Liam Girdwood --- src/audio/Kconfig | 7 + src/audio/pcm_converter/CMakeLists.txt | 3 +- src/audio/pcm_converter/pcm_converter_arm.c | 1094 +++++++++++++++++++ src/include/sof/audio/pcm_converter.h | 2 + 4 files changed, 1105 insertions(+), 1 deletion(-) create mode 100644 src/audio/pcm_converter/pcm_converter_arm.c diff --git a/src/audio/Kconfig b/src/audio/Kconfig index 4a8a8ef231e5..53f5301760f9 100644 --- a/src/audio/Kconfig +++ b/src/audio/Kconfig @@ -282,6 +282,13 @@ config FORMAT_CONVERT_HIFI3 help Use HIFI3 extensions for optimized format conversion (experimental). +config FORMAT_CONVERT_ARM_SIMD + bool "ARM SIMD optimized conversion" + default y + depends on ARM + help + Use ARMv7E-M DSP and SIMD instructions for optimized format conversion. + config PCM_CONVERTER_FORMAT_U8 bool "Support U8" help diff --git a/src/audio/pcm_converter/CMakeLists.txt b/src/audio/pcm_converter/CMakeLists.txt index 4f09a5a779c7..72b3d4d4bf03 100644 --- a/src/audio/pcm_converter/CMakeLists.txt +++ b/src/audio/pcm_converter/CMakeLists.txt @@ -3,7 +3,8 @@ add_local_sources(sof pcm_converter.c pcm_converter_generic.c - pcm_converter_hifi3.c) + pcm_converter_hifi3.c + pcm_converter_arm.c) if(CONFIG_PCM_REMAPPING_CONVERTERS) add_local_sources(sof pcm_remap.c) diff --git a/src/audio/pcm_converter/pcm_converter_arm.c b/src/audio/pcm_converter/pcm_converter_arm.c new file mode 100644 index 000000000000..73d384d652b6 --- /dev/null +++ b/src/audio/pcm_converter/pcm_converter_arm.c @@ -0,0 +1,1094 @@ +// SPDX-License-Identifier: BSD-3-Clause +// +// Copyright(c) 2026 Sound Open Firmware. All rights reserved. +// +// Author: Liam Girdwood + +#include + +#ifdef PCM_CONVERTER_ARM_SIMD + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#ifndef isnan +#define isnan(x) __builtin_isnan(x) +#endif + +#define BYTES_TO_U8_SAMPLES 0 +#define BYTES_TO_S16_SAMPLES 1 +#define BYTES_TO_S32_SAMPLES 2 + +#if CONFIG_PCM_CONVERTER_FORMAT_U8 && CONFIG_PCM_CONVERTER_FORMAT_S32LE +static int pcm_convert_u8_to_s32(const struct cir_buf_source *source, + uint32_t src_channels, struct cir_buf_sink *sink, + uint32_t sink_channels, size_t samples, uint32_t chmap) +{ + const uint8_t *src = source->ptr; + int32_t *dst = sink->ptr; + size_t processed, nmax, i, n; + + for (processed = 0; processed < samples; processed += n) { + src = cir_buf_wrap(src, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst, sink->buf_start, sink->buf_end); + n = samples - processed; + nmax = cir_buf_bytes_without_wrap(src, source->buf_end) >> BYTES_TO_U8_SAMPLES; + n = MIN(n, nmax); + nmax = cir_buf_bytes_without_wrap(dst, sink->buf_end) >> BYTES_TO_S32_SAMPLES; + n = MIN(n, nmax); + for (i = 0; i < n; i++) { + *dst = (INT8_MIN + *src) << 24; + src++; + dst++; + } + } + + return samples; +} + +static int pcm_convert_s32_to_u8(const struct cir_buf_source *source, + uint32_t src_channels, struct cir_buf_sink *sink, + uint32_t sink_channels, size_t samples, uint32_t chmap) +{ + const int32_t *src = source->ptr; + uint8_t *dst = sink->ptr; + size_t processed, nmax, i, n; + + for (processed = 0; processed < samples; processed += n) { + src = cir_buf_wrap(src, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst, sink->buf_start, sink->buf_end); + n = samples - processed; + nmax = cir_buf_bytes_without_wrap(src, source->buf_end) >> BYTES_TO_S32_SAMPLES; + n = MIN(n, nmax); + nmax = cir_buf_bytes_without_wrap(dst, sink->buf_end) >> BYTES_TO_U8_SAMPLES; + n = MIN(n, nmax); + for (i = 0; i < n; i++) { + *dst = sat_int8(Q_SHIFT_RND(*src, 24, 0)) - INT8_MIN; + src++; + dst++; + } + } + + return samples; +} +#endif /* CONFIG_PCM_CONVERTER_FORMAT_U8 && CONFIG_PCM_CONVERTER_FORMAT_S32LE */ + +#if CONFIG_PCM_CONVERTER_FORMAT_A_LAW && CONFIG_PCM_CONVERTER_FORMAT_S32LE +static int pcm_convert_alaw_to_s32(const struct cir_buf_source *source, + uint32_t src_channels, struct cir_buf_sink *sink, + uint32_t sink_channels, size_t samples, uint32_t chmap) +{ + const uint8_t *src = source->ptr; + int32_t *dst = sink->ptr; + size_t processed, nmax, i, n; + + for (processed = 0; processed < samples; processed += n) { + src = cir_buf_wrap(src, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst, sink->buf_start, sink->buf_end); + n = samples - processed; + nmax = cir_buf_bytes_without_wrap(src, source->buf_end) >> BYTES_TO_U8_SAMPLES; + n = MIN(n, nmax); + nmax = cir_buf_bytes_without_wrap(dst, sink->buf_end) >> BYTES_TO_S32_SAMPLES; + n = MIN(n, nmax); + for (i = 0; i < n; i++) { + *dst = sofm_a_law_decode(*src) << 16; + src++; + dst++; + } + } + + return samples; +} + +static int pcm_convert_s32_to_alaw(const struct cir_buf_source *source, + uint32_t src_channels, struct cir_buf_sink *sink, + uint32_t sink_channels, size_t samples, uint32_t chmap) +{ + const int32_t *src = source->ptr; + uint8_t *dst = sink->ptr; + size_t processed, nmax, i, n; + + for (processed = 0; processed < samples; processed += n) { + src = cir_buf_wrap(src, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst, sink->buf_start, sink->buf_end); + n = samples - processed; + nmax = cir_buf_bytes_without_wrap(src, source->buf_end) >> BYTES_TO_S32_SAMPLES; + n = MIN(n, nmax); + nmax = cir_buf_bytes_without_wrap(dst, sink->buf_end) >> BYTES_TO_U8_SAMPLES; + n = MIN(n, nmax); + for (i = 0; i < n; i++) { + *dst = sofm_a_law_encode(*src >> 16); + src++; + dst++; + } + } + + return samples; +} +#endif /* CONFIG_PCM_CONVERTER_FORMAT_A_LAW && CONFIG_PCM_CONVERTER_FORMAT_S32LE */ + +#if CONFIG_PCM_CONVERTER_FORMAT_MU_LAW && CONFIG_PCM_CONVERTER_FORMAT_S32LE +static int pcm_convert_mulaw_to_s32(const struct cir_buf_source *source, + uint32_t src_channels, struct cir_buf_sink *sink, + uint32_t sink_channels, size_t samples, uint32_t chmap) +{ + const uint8_t *src = source->ptr; + int32_t *dst = sink->ptr; + size_t processed, nmax, i, n; + + for (processed = 0; processed < samples; processed += n) { + src = cir_buf_wrap(src, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst, sink->buf_start, sink->buf_end); + n = samples - processed; + nmax = cir_buf_bytes_without_wrap(src, source->buf_end) >> BYTES_TO_U8_SAMPLES; + n = MIN(n, nmax); + nmax = cir_buf_bytes_without_wrap(dst, sink->buf_end) >> BYTES_TO_S32_SAMPLES; + n = MIN(n, nmax); + for (i = 0; i < n; i++) { + *dst = sofm_mu_law_decode(*src) << 16; + src++; + dst++; + } + } + + return samples; +} + +static int pcm_convert_s32_to_mulaw(const struct cir_buf_source *source, + uint32_t src_channels, struct cir_buf_sink *sink, + uint32_t sink_channels, size_t samples, uint32_t chmap) +{ + const int32_t *src = source->ptr; + uint8_t *dst = sink->ptr; + size_t processed, nmax, i, n; + + for (processed = 0; processed < samples; processed += n) { + src = cir_buf_wrap(src, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst, sink->buf_start, sink->buf_end); + n = samples - processed; + nmax = cir_buf_bytes_without_wrap(src, source->buf_end) >> BYTES_TO_S32_SAMPLES; + n = MIN(n, nmax); + nmax = cir_buf_bytes_without_wrap(dst, sink->buf_end) >> BYTES_TO_U8_SAMPLES; + n = MIN(n, nmax); + for (i = 0; i < n; i++) { + *dst = sofm_mu_law_encode(*src >> 16); + src++; + dst++; + } + } + + return samples; +} +#endif /* CONFIG_PCM_CONVERTER_FORMAT_MU_LAW && CONFIG_PCM_CONVERTER_FORMAT_S32LE */ + +#if CONFIG_PCM_CONVERTER_FORMAT_S16LE && CONFIG_PCM_CONVERTER_FORMAT_S24LE +static int pcm_convert_s16_to_s24(const struct cir_buf_source *source, + uint32_t src_channels, struct cir_buf_sink *sink, + uint32_t sink_channels, size_t samples, uint32_t chmap) +{ + const int16_t *src = source->ptr; + int32_t *dst = sink->ptr; + size_t processed, nmax, n, i; + + for (processed = 0; processed < samples; processed += n) { + src = cir_buf_wrap(src, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst, sink->buf_start, sink->buf_end); + n = samples - processed; + nmax = cir_buf_bytes_without_wrap(src, source->buf_end) >> BYTES_TO_S16_SAMPLES; + n = MIN(n, nmax); + nmax = cir_buf_bytes_without_wrap(dst, sink->buf_end) >> BYTES_TO_S32_SAMPLES; + n = MIN(n, nmax); + + size_t n4 = n & ~3; + for (i = 0; i < n4; i += 4) { + dst[i + 0] = (int32_t)src[i + 0] << 8; + dst[i + 1] = (int32_t)src[i + 1] << 8; + dst[i + 2] = (int32_t)src[i + 2] << 8; + dst[i + 3] = (int32_t)src[i + 3] << 8; + } + for (; i < n; i++) { + dst[i] = (int32_t)src[i] << 8; + } + src += n; + dst += n; + } + + return samples; +} + +static int pcm_convert_s24_to_s16(const struct cir_buf_source *source, + uint32_t src_channels, struct cir_buf_sink *sink, + uint32_t sink_channels, size_t samples, uint32_t chmap) +{ + const int32_t *src = source->ptr; + int16_t *dst = sink->ptr; + size_t processed, nmax, n, i; + + for (processed = 0; processed < samples; processed += n) { + src = cir_buf_wrap(src, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst, sink->buf_start, sink->buf_end); + n = samples - processed; + nmax = cir_buf_bytes_without_wrap(src, source->buf_end) >> BYTES_TO_S32_SAMPLES; + n = MIN(n, nmax); + nmax = cir_buf_bytes_without_wrap(dst, sink->buf_end) >> BYTES_TO_S16_SAMPLES; + n = MIN(n, nmax); + + size_t n4 = n & ~3; + for (i = 0; i < n4; i += 4) { + int16_t s0 = arm_sat_s16(Q_SHIFT_RND(sign_extend_s24(src[i + 0]), 23, 15)); + int16_t s1 = arm_sat_s16(Q_SHIFT_RND(sign_extend_s24(src[i + 1]), 23, 15)); + int16_t s2 = arm_sat_s16(Q_SHIFT_RND(sign_extend_s24(src[i + 2]), 23, 15)); + int16_t s3 = arm_sat_s16(Q_SHIFT_RND(sign_extend_s24(src[i + 3]), 23, 15)); + + *(uint32_t *)(dst + i) = arm_pkhbt(s0, s1, 16); + *(uint32_t *)(dst + i + 2) = arm_pkhbt(s2, s3, 16); + } + for (; i < n; i++) { + dst[i] = arm_sat_s16(Q_SHIFT_RND(sign_extend_s24(src[i]), 23, 15)); + } + src += n; + dst += n; + } + + return samples; +} +#endif /* CONFIG_PCM_CONVERTER_FORMAT_S16LE && CONFIG_PCM_CONVERTER_FORMAT_S24LE */ + +#if CONFIG_PCM_CONVERTER_FORMAT_S32LE && CONFIG_PCM_CONVERTER_FORMAT_S24LE +static int pcm_convert_s24_to_s32(const struct cir_buf_source *source, + uint32_t src_channels, struct cir_buf_sink *sink, + uint32_t sink_channels, size_t samples, uint32_t chmap) +{ + const int32_t *src = source->ptr; + int32_t *dst = sink->ptr; + size_t processed, nmax, n, i; + + for (processed = 0; processed < samples; processed += n) { + src = cir_buf_wrap(src, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst, sink->buf_start, sink->buf_end); + n = samples - processed; + nmax = cir_buf_bytes_without_wrap(src, source->buf_end) >> BYTES_TO_S32_SAMPLES; + n = MIN(n, nmax); + nmax = cir_buf_bytes_without_wrap(dst, sink->buf_end) >> BYTES_TO_S32_SAMPLES; + n = MIN(n, nmax); + + size_t n4 = n & ~3; + for (i = 0; i < n4; i += 4) { + dst[i + 0] = sign_extend_s24(src[i + 0]) << 8; + dst[i + 1] = sign_extend_s24(src[i + 1]) << 8; + dst[i + 2] = sign_extend_s24(src[i + 2]) << 8; + dst[i + 3] = sign_extend_s24(src[i + 3]) << 8; + } + for (; i < n; i++) { + dst[i] = sign_extend_s24(src[i]) << 8; + } + src += n; + dst += n; + } + + return samples; +} + +static int pcm_convert_s32_to_s24(const struct cir_buf_source *source, + uint32_t src_channels, struct cir_buf_sink *sink, + uint32_t sink_channels, size_t samples, uint32_t chmap) +{ + const int32_t *src = source->ptr; + int32_t *dst = sink->ptr; + size_t processed, nmax, n, i; + + for (processed = 0; processed < samples; processed += n) { + src = cir_buf_wrap(src, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst, sink->buf_start, sink->buf_end); + n = samples - processed; + nmax = cir_buf_bytes_without_wrap(src, source->buf_end) >> BYTES_TO_S32_SAMPLES; + n = MIN(n, nmax); + nmax = cir_buf_bytes_without_wrap(dst, sink->buf_end) >> BYTES_TO_S32_SAMPLES; + n = MIN(n, nmax); + + size_t n4 = n & ~3; + for (i = 0; i < n4; i += 4) { + dst[i + 0] = arm_sat_s24(Q_SHIFT_RND(src[i + 0], 31, 23)); + dst[i + 1] = arm_sat_s24(Q_SHIFT_RND(src[i + 1], 31, 23)); + dst[i + 2] = arm_sat_s24(Q_SHIFT_RND(src[i + 2], 31, 23)); + dst[i + 3] = arm_sat_s24(Q_SHIFT_RND(src[i + 3], 31, 23)); + } + for (; i < n; i++) { + dst[i] = arm_sat_s24(Q_SHIFT_RND(src[i], 31, 23)); + } + src += n; + dst += n; + } + + return samples; +} + +#if CONFIG_PCM_CONVERTER_FORMAT_S24_4LE_MSB +static int pcm_convert_s32_to_s24_be(const struct cir_buf_source *source, + uint32_t src_channels, struct cir_buf_sink *sink, + uint32_t sink_channels, size_t samples, uint32_t chmap) +{ + const int32_t *src = source->ptr; + int32_t *dst = sink->ptr; + size_t processed, nmax, n, i; + + for (processed = 0; processed < samples; processed += n) { + src = cir_buf_wrap(src, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst, sink->buf_start, sink->buf_end); + n = samples - processed; + nmax = cir_buf_bytes_without_wrap(src, source->buf_end) >> BYTES_TO_S32_SAMPLES; + n = MIN(n, nmax); + nmax = cir_buf_bytes_without_wrap(dst, sink->buf_end) >> BYTES_TO_S32_SAMPLES; + n = MIN(n, nmax); + + size_t n4 = n & ~3; + for (i = 0; i < n4; i += 4) { + dst[i + 0] = arm_sat_s24(Q_SHIFT_RND(src[i + 0], 31, 23)) << 8; + dst[i + 1] = arm_sat_s24(Q_SHIFT_RND(src[i + 1], 31, 23)) << 8; + dst[i + 2] = arm_sat_s24(Q_SHIFT_RND(src[i + 2], 31, 23)) << 8; + dst[i + 3] = arm_sat_s24(Q_SHIFT_RND(src[i + 3], 31, 23)) << 8; + } + for (; i < n; i++) { + dst[i] = arm_sat_s24(Q_SHIFT_RND(src[i], 31, 23)) << 8; + } + src += n; + dst += n; + } + + return samples; +} +#endif /* CONFIG_PCM_CONVERTER_FORMAT_S24_4LE_MSB */ +#endif /* CONFIG_PCM_CONVERTER_FORMAT_S32LE && CONFIG_PCM_CONVERTER_FORMAT_S24LE */ + +#if CONFIG_PCM_CONVERTER_FORMAT_S32LE && CONFIG_PCM_CONVERTER_FORMAT_S16LE +static int pcm_convert_s16_to_s32(const struct cir_buf_source *source, + uint32_t src_channels, struct cir_buf_sink *sink, + uint32_t sink_channels, size_t samples, uint32_t chmap) +{ + const int16_t *src = source->ptr; + int32_t *dst = sink->ptr; + size_t processed, nmax, n, i; + + for (processed = 0; processed < samples; processed += n) { + src = cir_buf_wrap(src, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst, sink->buf_start, sink->buf_end); + n = samples - processed; + nmax = cir_buf_bytes_without_wrap(src, source->buf_end) >> BYTES_TO_S16_SAMPLES; + n = MIN(n, nmax); + nmax = cir_buf_bytes_without_wrap(dst, sink->buf_end) >> BYTES_TO_S32_SAMPLES; + n = MIN(n, nmax); + + size_t n4 = n & ~3; + for (i = 0; i < n4; i += 4) { + dst[i + 0] = (int32_t)src[i + 0] << 16; + dst[i + 1] = (int32_t)src[i + 1] << 16; + dst[i + 2] = (int32_t)src[i + 2] << 16; + dst[i + 3] = (int32_t)src[i + 3] << 16; + } + for (; i < n; i++) { + dst[i] = (int32_t)src[i] << 16; + } + src += n; + dst += n; + } + + return samples; +} + +static int pcm_convert_s32_to_s16(const struct cir_buf_source *source, + uint32_t src_channels, struct cir_buf_sink *sink, + uint32_t sink_channels, size_t samples, uint32_t chmap) +{ + const int32_t *src = source->ptr; + int16_t *dst = sink->ptr; + size_t processed, nmax, n, i; + + for (processed = 0; processed < samples; processed += n) { + src = cir_buf_wrap(src, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst, sink->buf_start, sink->buf_end); + n = samples - processed; + nmax = cir_buf_bytes_without_wrap(src, source->buf_end) >> BYTES_TO_S32_SAMPLES; + n = MIN(n, nmax); + nmax = cir_buf_bytes_without_wrap(dst, sink->buf_end) >> BYTES_TO_S16_SAMPLES; + n = MIN(n, nmax); + + size_t n4 = n & ~3; + for (i = 0; i < n4; i += 4) { + int16_t s0 = arm_sat_s16(Q_SHIFT_RND(src[i + 0], 31, 15)); + int16_t s1 = arm_sat_s16(Q_SHIFT_RND(src[i + 1], 31, 15)); + int16_t s2 = arm_sat_s16(Q_SHIFT_RND(src[i + 2], 31, 15)); + int16_t s3 = arm_sat_s16(Q_SHIFT_RND(src[i + 3], 31, 15)); + + *(uint32_t *)(dst + i) = arm_pkhbt(s0, s1, 16); + *(uint32_t *)(dst + i + 2) = arm_pkhbt(s2, s3, 16); + } + for (; i < n; i++) { + dst[i] = arm_sat_s16(Q_SHIFT_RND(src[i], 31, 15)); + } + src += n; + dst += n; + } + + return samples; +} +#endif /* CONFIG_PCM_CONVERTER_FORMAT_S32LE && CONFIG_PCM_CONVERTER_FORMAT_S16LE */ + +#if CONFIG_PCM_CONVERTER_FORMAT_FLOAT && CONFIG_PCM_CONVERTER_FORMAT_S16LE +static int pcm_convert_s16_to_f(const struct cir_buf_source *source, + uint32_t src_channels, struct cir_buf_sink *sink, + uint32_t sink_channels, size_t samples, uint32_t chmap) +{ + const int16_t *src = source->ptr; + float *dst = sink->ptr; + size_t processed, nmax, n, i; + const float scale = 1.0f / 32768.0f; + + for (processed = 0; processed < samples; processed += n) { + src = cir_buf_wrap(src, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst, sink->buf_start, sink->buf_end); + n = samples - processed; + nmax = cir_buf_bytes_without_wrap(src, source->buf_end) >> BYTES_TO_S16_SAMPLES; + n = MIN(n, nmax); + nmax = cir_buf_bytes_without_wrap(dst, sink->buf_end) >> BYTES_TO_S32_SAMPLES; + n = MIN(n, nmax); + + for (i = 0; i < n; i++) { + dst[i] = (float)src[i] * scale; + } + src += n; + dst += n; + } + + return samples; +} + +static int pcm_convert_f_to_s16(const struct cir_buf_source *source, + uint32_t src_channels, struct cir_buf_sink *sink, + uint32_t sink_channels, size_t samples, uint32_t chmap) +{ + const float *src = source->ptr; + int16_t *dst = sink->ptr; + size_t processed, nmax, n, i; + const float scale = 32768.0f; + + for (processed = 0; processed < samples; processed += n) { + src = cir_buf_wrap(src, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst, sink->buf_start, sink->buf_end); + n = samples - processed; + nmax = cir_buf_bytes_without_wrap(src, source->buf_end) >> BYTES_TO_S32_SAMPLES; + n = MIN(n, nmax); + nmax = cir_buf_bytes_without_wrap(dst, sink->buf_end) >> BYTES_TO_S16_SAMPLES; + n = MIN(n, nmax); + + for (i = 0; i < n; i++) { + float val = src[i] * scale; + + if (isnan(val)) + dst[i] = 0; + else if (val >= 32767.0f) + dst[i] = INT16_MAX; + else if (val <= -32768.0f) + dst[i] = INT16_MIN; + else + dst[i] = (int16_t)val; + } + src += n; + dst += n; + } + + return samples; +} +#endif /* CONFIG_PCM_CONVERTER_FORMAT_FLOAT && CONFIG_PCM_CONVERTER_FORMAT_S16LE */ + +#if CONFIG_PCM_CONVERTER_FORMAT_FLOAT && CONFIG_PCM_CONVERTER_FORMAT_S24LE +static int pcm_convert_s24_to_f(const struct cir_buf_source *source, + uint32_t src_channels, struct cir_buf_sink *sink, + uint32_t sink_channels, size_t samples, uint32_t chmap) +{ + const int32_t *src = source->ptr; + float *dst = sink->ptr; + size_t processed, nmax, n, i; + const float scale = 1.0f / 8388608.0f; + + for (processed = 0; processed < samples; processed += n) { + src = cir_buf_wrap(src, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst, sink->buf_start, sink->buf_end); + n = samples - processed; + nmax = cir_buf_bytes_without_wrap(src, source->buf_end) >> BYTES_TO_S32_SAMPLES; + n = MIN(n, nmax); + nmax = cir_buf_bytes_without_wrap(dst, sink->buf_end) >> BYTES_TO_S32_SAMPLES; + n = MIN(n, nmax); + + for (i = 0; i < n; i++) { + dst[i] = (float)sign_extend_s24(src[i]) * scale; + } + src += n; + dst += n; + } + + return samples; +} + +static int pcm_convert_f_to_s24(const struct cir_buf_source *source, + uint32_t src_channels, struct cir_buf_sink *sink, + uint32_t sink_channels, size_t samples, uint32_t chmap) +{ + const float *src = source->ptr; + int32_t *dst = sink->ptr; + size_t processed, nmax, n, i; + const float scale = 8388608.0f; + + for (processed = 0; processed < samples; processed += n) { + src = cir_buf_wrap(src, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst, sink->buf_start, sink->buf_end); + n = samples - processed; + nmax = cir_buf_bytes_without_wrap(src, source->buf_end) >> BYTES_TO_S32_SAMPLES; + n = MIN(n, nmax); + nmax = cir_buf_bytes_without_wrap(dst, sink->buf_end) >> BYTES_TO_S32_SAMPLES; + n = MIN(n, nmax); + + for (i = 0; i < n; i++) { + float val = src[i] * scale; + + if (isnan(val)) + dst[i] = 0; + else if (val >= 8388607.0f) + dst[i] = INT24_MAXVALUE; + else if (val <= -8388608.0f) + dst[i] = INT24_MINVALUE; + else + dst[i] = (int32_t)val; + } + src += n; + dst += n; + } + + return samples; +} +#endif /* CONFIG_PCM_CONVERTER_FORMAT_FLOAT && CONFIG_PCM_CONVERTER_FORMAT_S24LE */ + +#if CONFIG_PCM_CONVERTER_FORMAT_FLOAT && CONFIG_PCM_CONVERTER_FORMAT_S32LE +static int pcm_convert_s32_to_f(const struct cir_buf_source *source, + uint32_t src_channels, struct cir_buf_sink *sink, + uint32_t sink_channels, size_t samples, uint32_t chmap) +{ + const int32_t *src = source->ptr; + float *dst = sink->ptr; + size_t processed, nmax, n, i; + const float scale = 1.0f / 2147483648.0f; + + for (processed = 0; processed < samples; processed += n) { + src = cir_buf_wrap(src, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst, sink->buf_start, sink->buf_end); + n = samples - processed; + nmax = cir_buf_bytes_without_wrap(src, source->buf_end) >> BYTES_TO_S32_SAMPLES; + n = MIN(n, nmax); + nmax = cir_buf_bytes_without_wrap(dst, sink->buf_end) >> BYTES_TO_S32_SAMPLES; + n = MIN(n, nmax); + + for (i = 0; i < n; i++) { + dst[i] = (float)src[i] * scale; + } + src += n; + dst += n; + } + + return samples; +} + +static int pcm_convert_f_to_s32(const struct cir_buf_source *source, + uint32_t src_channels, struct cir_buf_sink *sink, + uint32_t sink_channels, size_t samples, uint32_t chmap) +{ + const float *src = source->ptr; + int32_t *dst = sink->ptr; + size_t processed, nmax, n, i; + const float scale = 2147483648.0f; + + for (processed = 0; processed < samples; processed += n) { + src = cir_buf_wrap(src, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst, sink->buf_start, sink->buf_end); + n = samples - processed; + nmax = cir_buf_bytes_without_wrap(src, source->buf_end) >> BYTES_TO_S32_SAMPLES; + n = MIN(n, nmax); + nmax = cir_buf_bytes_without_wrap(dst, sink->buf_end) >> BYTES_TO_S32_SAMPLES; + n = MIN(n, nmax); + + for (i = 0; i < n; i++) { + float val = src[i] * scale; + + if (isnan(val)) + dst[i] = 0; + else if (val >= 2147483647.0f) + dst[i] = INT32_MAX; + else if (val <= -2147483648.0f) + dst[i] = INT32_MIN; + else + dst[i] = (int32_t)val; + } + src += n; + dst += n; + } + + return samples; +} +#endif /* CONFIG_PCM_CONVERTER_FORMAT_FLOAT && CONFIG_PCM_CONVERTER_FORMAT_S32LE */ + +#if CONFIG_PCM_CONVERTER_FORMAT_S16_C16_AND_S16_C32 +static int pcm_convert_s16_c16_to_s16_c32(const struct cir_buf_source *source, + uint32_t src_channels, struct cir_buf_sink *sink, + uint32_t sink_channels, size_t samples, uint32_t chmap) +{ + const int16_t *src = source->ptr; + int32_t *dst = sink->ptr; + size_t processed, nmax, n, i; + + for (processed = 0; processed < samples; processed += n) { + src = cir_buf_wrap(src, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst, sink->buf_start, sink->buf_end); + n = samples - processed; + nmax = cir_buf_bytes_without_wrap(src, source->buf_end) >> BYTES_TO_S16_SAMPLES; + n = MIN(n, nmax); + nmax = cir_buf_bytes_without_wrap(dst, sink->buf_end) >> BYTES_TO_S32_SAMPLES; + n = MIN(n, nmax); + + size_t n4 = n & ~3; + for (i = 0; i < n4; i += 4) { + dst[i + 0] = (int32_t)src[i + 0]; + dst[i + 1] = (int32_t)src[i + 1]; + dst[i + 2] = (int32_t)src[i + 2]; + dst[i + 3] = (int32_t)src[i + 3]; + } + for (; i < n; i++) { + dst[i] = (int32_t)src[i]; + } + src += n; + dst += n; + } + + return samples; +} + +static int pcm_convert_s16_c32_to_s16_c16(const struct cir_buf_source *source, + uint32_t src_channels, struct cir_buf_sink *sink, + uint32_t sink_channels, size_t samples, uint32_t chmap) +{ + const int32_t *src = source->ptr; + int16_t *dst = sink->ptr; + size_t processed, nmax, n, i; + + for (processed = 0; processed < samples; processed += n) { + src = cir_buf_wrap(src, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst, sink->buf_start, sink->buf_end); + n = samples - processed; + nmax = cir_buf_bytes_without_wrap(src, source->buf_end) >> BYTES_TO_S32_SAMPLES; + n = MIN(n, nmax); + nmax = cir_buf_bytes_without_wrap(dst, sink->buf_end) >> BYTES_TO_S16_SAMPLES; + n = MIN(n, nmax); + + size_t n4 = n & ~3; + for (i = 0; i < n4; i += 4) { + int16_t s0 = arm_sat_s16(src[i + 0]); + int16_t s1 = arm_sat_s16(src[i + 1]); + int16_t s2 = arm_sat_s16(src[i + 2]); + int16_t s3 = arm_sat_s16(src[i + 3]); + + *(uint32_t *)(dst + i) = arm_pkhbt(s0, s1, 16); + *(uint32_t *)(dst + i + 2) = arm_pkhbt(s2, s3, 16); + } + for (; i < n; i++) { + dst[i] = arm_sat_s16(src[i]); + } + src += n; + dst += n; + } + + return samples; +} +#endif /* CONFIG_PCM_CONVERTER_FORMAT_S16_C16_AND_S16_C32 */ + +#if CONFIG_PCM_CONVERTER_FORMAT_S16_C32_AND_S32_C32 +static int pcm_convert_s16_c32_to_s32_c32(const struct cir_buf_source *source, + uint32_t src_channels, struct cir_buf_sink *sink, + uint32_t sink_channels, size_t samples, uint32_t chmap) +{ + const int32_t *src = source->ptr; + int32_t *dst = sink->ptr; + size_t processed, nmax, n, i; + + for (processed = 0; processed < samples; processed += n) { + src = cir_buf_wrap(src, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst, sink->buf_start, sink->buf_end); + n = samples - processed; + nmax = cir_buf_bytes_without_wrap(src, source->buf_end) >> BYTES_TO_S32_SAMPLES; + n = MIN(n, nmax); + nmax = cir_buf_bytes_without_wrap(dst, sink->buf_end) >> BYTES_TO_S32_SAMPLES; + n = MIN(n, nmax); + + size_t n4 = n & ~3; + for (i = 0; i < n4; i += 4) { + dst[i + 0] = src[i + 0] << 16; + dst[i + 1] = src[i + 1] << 16; + dst[i + 2] = src[i + 2] << 16; + dst[i + 3] = src[i + 3] << 16; + } + for (; i < n; i++) { + dst[i] = src[i] << 16; + } + src += n; + dst += n; + } + + return samples; +} + +static int pcm_convert_s32_c32_to_s16_c32(const struct cir_buf_source *source, + uint32_t src_channels, struct cir_buf_sink *sink, + uint32_t sink_channels, size_t samples, uint32_t chmap) +{ + const int32_t *src = source->ptr; + int32_t *dst = sink->ptr; + size_t processed, nmax, n, i; + + for (processed = 0; processed < samples; processed += n) { + src = cir_buf_wrap(src, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst, sink->buf_start, sink->buf_end); + n = samples - processed; + nmax = cir_buf_bytes_without_wrap(src, source->buf_end) >> BYTES_TO_S32_SAMPLES; + n = MIN(n, nmax); + nmax = cir_buf_bytes_without_wrap(dst, sink->buf_end) >> BYTES_TO_S32_SAMPLES; + n = MIN(n, nmax); + + size_t n4 = n & ~3; + for (i = 0; i < n4; i += 4) { + dst[i + 0] = arm_sat_s16(Q_SHIFT_RND(src[i + 0], 31, 15)); + dst[i + 1] = arm_sat_s16(Q_SHIFT_RND(src[i + 1], 31, 15)); + dst[i + 2] = arm_sat_s16(Q_SHIFT_RND(src[i + 2], 31, 15)); + dst[i + 3] = arm_sat_s16(Q_SHIFT_RND(src[i + 3], 31, 15)); + } + for (; i < n; i++) { + dst[i] = arm_sat_s16(Q_SHIFT_RND(src[i], 31, 15)); + } + src += n; + dst += n; + } + + return samples; +} +#endif /* CONFIG_PCM_CONVERTER_FORMAT_S16_C32_AND_S32_C32 */ + +#if CONFIG_PCM_CONVERTER_FORMAT_S16_C32_AND_S24_C32 +static int pcm_convert_s16_c32_to_s24_c32(const struct cir_buf_source *source, + uint32_t src_channels, struct cir_buf_sink *sink, + uint32_t sink_channels, size_t samples, uint32_t chmap) +{ + const int32_t *src = source->ptr; + int32_t *dst = sink->ptr; + size_t processed, nmax, i, n; + + for (processed = 0; processed < samples; processed += n) { + src = cir_buf_wrap(src, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst, sink->buf_start, sink->buf_end); + n = samples - processed; + nmax = cir_buf_bytes_without_wrap(src, source->buf_end) >> BYTES_TO_S32_SAMPLES; + n = MIN(n, nmax); + nmax = cir_buf_bytes_without_wrap(dst, sink->buf_end) >> BYTES_TO_S32_SAMPLES; + n = MIN(n, nmax); + for (i = 0; i < n; i++) { + *dst = *src << 8; + src++; + dst++; + } + } + + return samples; +} + +static int pcm_convert_s24_c32_to_s16_c32(const struct cir_buf_source *source, + uint32_t src_channels, struct cir_buf_sink *sink, + uint32_t sink_channels, size_t samples, uint32_t chmap) +{ + const int32_t *src = source->ptr; + int32_t *dst = sink->ptr; + size_t processed, nmax, i, n; + + for (processed = 0; processed < samples; processed += n) { + src = cir_buf_wrap(src, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst, sink->buf_start, sink->buf_end); + n = samples - processed; + nmax = cir_buf_bytes_without_wrap(src, source->buf_end) >> BYTES_TO_S32_SAMPLES; + n = MIN(n, nmax); + nmax = cir_buf_bytes_without_wrap(dst, sink->buf_end) >> BYTES_TO_S32_SAMPLES; + n = MIN(n, nmax); + for (i = 0; i < n; i++) { + *dst = arm_sat_s16(Q_SHIFT_RND(sign_extend_s24(*src & 0xffffff), 23, 15)); + src++; + dst++; + } + } + + return samples; +} +#endif /* CONFIG_PCM_CONVERTER_FORMAT_S16_C32_AND_S24_C32 */ + +#if CONFIG_PCM_CONVERTER_FORMAT_S24_C24_AND_S24_C32 +static int pcm_convert_s24_c24_to_s24_c32(const struct cir_buf_source *source, + uint32_t src_channels, struct cir_buf_sink *sink, + uint32_t sink_channels, size_t samples, uint32_t chmap) +{ + const uint8_t *src = source->ptr; + int32_t *dst = sink->ptr; + size_t processed, nmax, i, n; + + for (processed = 0; processed < samples; processed += n) { + src = cir_buf_wrap(src, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst, sink->buf_start, sink->buf_end); + n = samples - processed; + nmax = cir_buf_bytes_without_wrap(src, source->buf_end) / 3; + n = MIN(n, nmax); + nmax = cir_buf_bytes_without_wrap(dst, sink->buf_end) >> BYTES_TO_S32_SAMPLES; + n = MIN(n, nmax); + for (i = 0; i < n; i += 1) { + *dst = (*(src + 2) << 24) | (*(src + 1) << 16) | (*(src + 0) << 8); + *dst >>= 8; + dst++; + src += 3; + } + } + + return samples; +} + +static int pcm_convert_s24_c32_to_s24_c24(const struct cir_buf_source *source, + uint32_t src_channels, struct cir_buf_sink *sink, + uint32_t sink_channels, size_t samples, uint32_t chmap) +{ + const int32_t *src = source->ptr; + uint8_t *dst = sink->ptr; + size_t processed, nmax, i, n; + + for (processed = 0; processed < samples; processed += n) { + src = cir_buf_wrap(src, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst, sink->buf_start, sink->buf_end); + n = samples - processed; + nmax = cir_buf_bytes_without_wrap(src, source->buf_end) >> BYTES_TO_S32_SAMPLES; + n = MIN(n, nmax); + nmax = cir_buf_bytes_without_wrap(dst, sink->buf_end) / 3; + n = MIN(n, nmax); + for (i = 0; i < n; i += 1) { + *dst = *src & 0xFF; + dst++; + *dst = (*src >> 8) & 0xFF; + dst++; + *dst = (*src >> 16) & 0xFF; + dst++; + src++; + } + } + + return samples; +} + +/* 2x24bit samples are packed into 3x16bit samples for hda link dma */ +static int pcm_convert_s24_c32_to_s24_c24_link_gtw(const struct cir_buf_source *source, + uint32_t src_channels, struct cir_buf_sink *sink, + uint32_t sink_channels, size_t samples, + uint32_t chmap) +{ + const int32_t *src = source->ptr; + uint16_t *dst = sink->ptr; + size_t processed, nmax, i = 0, n = 0; + + for (processed = 0; processed < samples; processed += n) { + src = cir_buf_wrap(src, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst, sink->buf_start, sink->buf_end); + n = samples - processed; + nmax = cir_buf_bytes_without_wrap(src, source->buf_end) >> BYTES_TO_S32_SAMPLES; + n = MIN(n, nmax); + nmax = cir_buf_bytes_without_wrap(dst, sink->buf_end) / 3; + n = MIN(n, nmax); + for (i = 0; i + 1 < n; i += 2) { + *dst = (*src >> 8) & 0xFFFF; + dst++; + *dst = ((*src & 0xFF) << 8) | ((*(src + 1) >> 16) & 0xFF); + dst++; + *dst = *(src + 1) & 0xFFFF; + dst++; + src += 2; + } + + /* handle odd trailing sample within this chunk */ + if (i < n) { + *dst = (*src >> 8) & 0xFFFF; + dst++; + *dst = ((*src & 0xFF) << 8); + dst++; + src++; + } + } + + return samples; +} +#endif /* CONFIG_PCM_CONVERTER_FORMAT_S24_C24_AND_S24_C32 */ + +const struct pcm_func_map pcm_func_map[] = { +#if CONFIG_PCM_CONVERTER_FORMAT_U8 + { SOF_IPC_FRAME_U8, SOF_IPC_FRAME_U8, just_copy_1b }, +#endif /* CONFIG_PCM_CONVERTER_FORMAT_U8 */ +#if CONFIG_PCM_CONVERTER_FORMAT_U8 && CONFIG_PCM_CONVERTER_FORMAT_S32LE + { SOF_IPC_FRAME_U8, SOF_IPC_FRAME_S32_LE, pcm_convert_u8_to_s32 }, + { SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_U8, pcm_convert_s32_to_u8 }, +#endif /* CONFIG_PCM_CONVERTER_FORMAT_U8 && CONFIG_PCM_CONVERTER_FORMAT_S32LE */ +#if CONFIG_PCM_CONVERTER_FORMAT_A_LAW + { SOF_IPC_FRAME_A_LAW, SOF_IPC_FRAME_A_LAW, just_copy_1b }, +#endif /* CONFIG_PCM_CONVERTER_FORMAT_A_LAW */ +#if CONFIG_PCM_CONVERTER_FORMAT_A_LAW && CONFIG_PCM_CONVERTER_FORMAT_S32LE + { SOF_IPC_FRAME_A_LAW, SOF_IPC_FRAME_S32_LE, pcm_convert_alaw_to_s32 }, + { SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_A_LAW, pcm_convert_s32_to_alaw }, +#endif /* CONFIG_PCM_CONVERTER_FORMAT_A_LAW && CONFIG_PCM_CONVERTER_FORMAT_S32LE */ +#if CONFIG_PCM_CONVERTER_FORMAT_MU_LAW + { SOF_IPC_FRAME_MU_LAW, SOF_IPC_FRAME_MU_LAW, just_copy_1b }, +#endif /* CONFIG_PCM_CONVERTER_FORMAT_MU_LAW */ +#if CONFIG_PCM_CONVERTER_FORMAT_MU_LAW && CONFIG_PCM_CONVERTER_FORMAT_S32LE + { SOF_IPC_FRAME_MU_LAW, SOF_IPC_FRAME_S32_LE, pcm_convert_mulaw_to_s32 }, + { SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_MU_LAW, pcm_convert_s32_to_mulaw }, +#endif /* CONFIG_PCM_CONVERTER_FORMAT_MU_LAW && CONFIG_PCM_CONVERTER_FORMAT_S32LE */ +#if CONFIG_PCM_CONVERTER_FORMAT_S16LE + { SOF_IPC_FRAME_S16_LE, SOF_IPC_FRAME_S16_LE, just_copy_2b }, +#endif +#if CONFIG_PCM_CONVERTER_FORMAT_S24LE + { SOF_IPC_FRAME_S24_4LE, SOF_IPC_FRAME_S24_4LE, just_copy_4b }, +#endif +#if CONFIG_PCM_CONVERTER_FORMAT_S24_3LE + { SOF_IPC_FRAME_S24_3LE, SOF_IPC_FRAME_S24_3LE, just_copy_3b }, +#endif /* CONFIG_PCM_CONVERTER_FORMAT_S24_3LE */ +#if CONFIG_PCM_CONVERTER_FORMAT_S24LE && CONFIG_PCM_CONVERTER_FORMAT_S16LE + { SOF_IPC_FRAME_S16_LE, SOF_IPC_FRAME_S24_4LE, pcm_convert_s16_to_s24 }, + { SOF_IPC_FRAME_S24_4LE, SOF_IPC_FRAME_S16_LE, pcm_convert_s24_to_s16 }, +#endif +#if CONFIG_PCM_CONVERTER_FORMAT_S32LE + { SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S32_LE, just_copy_4b }, +#endif +#if CONFIG_PCM_CONVERTER_FORMAT_S32LE && CONFIG_PCM_CONVERTER_FORMAT_S16LE + { SOF_IPC_FRAME_S16_LE, SOF_IPC_FRAME_S32_LE, pcm_convert_s16_to_s32 }, + { SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S16_LE, pcm_convert_s32_to_s16 }, +#endif +#if CONFIG_PCM_CONVERTER_FORMAT_S32LE && CONFIG_PCM_CONVERTER_FORMAT_S24LE + { SOF_IPC_FRAME_S24_4LE, SOF_IPC_FRAME_S32_LE, pcm_convert_s24_to_s32 }, + { SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S24_4LE, pcm_convert_s32_to_s24 }, +#endif +#if CONFIG_PCM_CONVERTER_FORMAT_S32LE && CONFIG_PCM_CONVERTER_FORMAT_S24_4LE_MSB + { SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S24_4LE_MSB, pcm_convert_s32_to_s24_be }, + { SOF_IPC_FRAME_S24_4LE_MSB, SOF_IPC_FRAME_S32_LE, just_copy_4b }, +#endif /* CONFIG_PCM_CONVERTER_FORMAT_S32LE && CONFIG_PCM_CONVERTER_FORMAT_S24_4LE_MSB */ +#if CONFIG_PCM_CONVERTER_FORMAT_FLOAT + { SOF_IPC_FRAME_FLOAT, SOF_IPC_FRAME_FLOAT, just_copy_4b }, +#endif +#if CONFIG_PCM_CONVERTER_FORMAT_FLOAT && CONFIG_PCM_CONVERTER_FORMAT_S16LE + { SOF_IPC_FRAME_S16_LE, SOF_IPC_FRAME_FLOAT, pcm_convert_s16_to_f }, + { SOF_IPC_FRAME_FLOAT, SOF_IPC_FRAME_S16_LE, pcm_convert_f_to_s16 }, +#endif +#if CONFIG_PCM_CONVERTER_FORMAT_FLOAT && CONFIG_PCM_CONVERTER_FORMAT_S24LE + { SOF_IPC_FRAME_S24_4LE, SOF_IPC_FRAME_FLOAT, pcm_convert_s24_to_f }, + { SOF_IPC_FRAME_FLOAT, SOF_IPC_FRAME_S24_4LE, pcm_convert_f_to_s24 }, +#endif +#if CONFIG_PCM_CONVERTER_FORMAT_FLOAT && CONFIG_PCM_CONVERTER_FORMAT_S32LE + { SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_FLOAT, pcm_convert_s32_to_f }, + { SOF_IPC_FRAME_FLOAT, SOF_IPC_FRAME_S32_LE, pcm_convert_f_to_s32 }, +#endif +}; + +const size_t pcm_func_count = ARRAY_SIZE(pcm_func_map); + +const struct pcm_func_vc_map pcm_func_vc_map[] = { +#if CONFIG_PCM_CONVERTER_FORMAT_S16_C16_AND_S16_C32 + { SOF_IPC_FRAME_S16_LE, SOF_IPC_FRAME_S16_LE, SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S16_LE, + pcm_convert_s16_c16_to_s16_c32 }, + { SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S16_LE, SOF_IPC_FRAME_S16_LE, SOF_IPC_FRAME_S16_LE, + pcm_convert_s16_c32_to_s16_c16 }, +#endif +#if CONFIG_PCM_CONVERTER_FORMAT_S16_C32_AND_S32_C32 + { SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S16_LE, SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S32_LE, + pcm_convert_s16_c32_to_s32_c32 }, + { SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S16_LE, + pcm_convert_s32_c32_to_s16_c32 }, +#endif +#if CONFIG_PCM_CONVERTER_FORMAT_S16_C32_AND_S24_C32 + { SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S16_LE, SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S24_4LE, + pcm_convert_s16_c32_to_s24_c32 }, + { SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S24_4LE, SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S16_LE, + pcm_convert_s24_c32_to_s16_c32 }, +#endif +#if CONFIG_PCM_CONVERTER_FORMAT_U8 && CONFIG_PCM_CONVERTER_FORMAT_S16LE + { SOF_IPC_FRAME_S16_LE, SOF_IPC_FRAME_U8, SOF_IPC_FRAME_S16_LE, SOF_IPC_FRAME_U8, + just_copy_2b }, +#endif /* CONFIG_PCM_CONVERTER_FORMAT_U8 && CONFIG_PCM_CONVERTER_FORMAT_S16LE */ +#if CONFIG_PCM_CONVERTER_FORMAT_S32LE && CONFIG_PCM_CONVERTER_FORMAT_S24LE + { SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S24_4LE, SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S24_4LE, + just_copy_4b }, + { SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S24_4LE, SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S32_LE, + pcm_convert_s24_to_s32 }, + { SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S24_4LE, + pcm_convert_s32_to_s24 }, +#endif +#if CONFIG_PCM_CONVERTER_FORMAT_S24LE && CONFIG_PCM_CONVERTER_FORMAT_S16LE + { SOF_IPC_FRAME_S16_LE, SOF_IPC_FRAME_S16_LE, SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S24_4LE, + pcm_convert_s16_to_s24 }, + { SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S24_4LE, SOF_IPC_FRAME_S16_LE, SOF_IPC_FRAME_S16_LE, + pcm_convert_s24_to_s16 }, +#endif +#if CONFIG_PCM_CONVERTER_FORMAT_S24_C24_AND_S24_C32 + { SOF_IPC_FRAME_S24_3LE, SOF_IPC_FRAME_S24_3LE, SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S24_4LE, + pcm_convert_s24_c24_to_s24_c32 }, + { SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S24_4LE, SOF_IPC_FRAME_S24_3LE, SOF_IPC_FRAME_S24_4LE, + pcm_convert_s24_c32_to_s24_c24 }, + { SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S24_4LE, SOF_IPC_FRAME_S24_3LE, SOF_IPC_FRAME_S24_3LE, + pcm_convert_s24_c32_to_s24_c24_link_gtw }, +#endif +#if CONFIG_PCM_CONVERTER_FORMAT_S16_C32_AND_S16_C32 + { SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S16_LE, SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S16_LE, + just_copy_4b }, +#endif +#if CONFIG_PCM_CONVERTER_FORMAT_S24_4LE_MSB && CONFIG_PCM_CONVERTER_FORMAT_S24LE + { SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S24_4LE, SOF_IPC_FRAME_S32_LE, + SOF_IPC_FRAME_S24_4LE_MSB, pcm_convert_s24_to_s32 }, + { SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S24_4LE_MSB, SOF_IPC_FRAME_S32_LE, + SOF_IPC_FRAME_S24_4LE, pcm_convert_s32_to_s24 }, + { SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S24_4LE_MSB, SOF_IPC_FRAME_S32_LE, + SOF_IPC_FRAME_S24_4LE_MSB, just_copy_4b }, +#endif +#if CONFIG_PCM_CONVERTER_FORMAT_S32LE && CONFIG_PCM_CONVERTER_FORMAT_S24_4LE_MSB + { SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S32_LE, + SOF_IPC_FRAME_S24_4LE_MSB, pcm_convert_s32_to_s24_be }, + { SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S24_4LE_MSB, SOF_IPC_FRAME_S32_LE, + SOF_IPC_FRAME_S32_LE, just_copy_4b }, +#endif +#if CONFIG_PCM_CONVERTER_FORMAT_S24_4LE_MSB && CONFIG_PCM_CONVERTER_FORMAT_S16LE + { SOF_IPC_FRAME_S16_LE, SOF_IPC_FRAME_S16_LE, SOF_IPC_FRAME_S32_LE, + SOF_IPC_FRAME_S24_4LE_MSB, pcm_convert_s16_to_s32 }, + { SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S24_4LE_MSB, SOF_IPC_FRAME_S16_LE, + SOF_IPC_FRAME_S16_LE, pcm_convert_s32_to_s16 }, +#endif +#if CONFIG_PCM_CONVERTER_FORMAT_FLOAT && CONFIG_PCM_CONVERTER_FORMAT_S24LE + { SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S24_4LE, SOF_IPC_FRAME_FLOAT, + SOF_IPC_FRAME_FLOAT, pcm_convert_s24_to_f }, + { SOF_IPC_FRAME_FLOAT, SOF_IPC_FRAME_FLOAT, SOF_IPC_FRAME_S32_LE, + SOF_IPC_FRAME_S24_4LE, pcm_convert_f_to_s24 }, +#endif +}; + +const size_t pcm_func_vc_count = ARRAY_SIZE(pcm_func_vc_map); + +#endif /* PCM_CONVERTER_ARM_SIMD */ diff --git a/src/include/sof/audio/pcm_converter.h b/src/include/sof/audio/pcm_converter.h index fd2565c7a6cc..c7bcfd5343b8 100644 --- a/src/include/sof/audio/pcm_converter.h +++ b/src/include/sof/audio/pcm_converter.h @@ -33,6 +33,8 @@ struct cir_buf_sink; #ifndef UNIT_TEST #if __XCC__ && XCHAL_HAVE_HIFI3 && CONFIG_FORMAT_CONVERT_HIFI3 #define PCM_CONVERTER_HIFI3 +#elif defined(CONFIG_FORMAT_CONVERT_ARM_SIMD) +#define PCM_CONVERTER_ARM_SIMD #else #define PCM_CONVERTER_GENERIC #endif