diff --git a/src/audio/asrc/asrc_farrow_generic.c b/src/audio/asrc/asrc_farrow_generic.c index b2decc07db23..d0d06f152f07 100644 --- a/src/audio/asrc/asrc_farrow_generic.c +++ b/src/audio/asrc/asrc_farrow_generic.c @@ -40,10 +40,16 @@ void asrc_fir_filter16(struct asrc_farrow *src_obj, int16_t **output_buffers, /* Initialise the accumulator */ prod = 0; - /* Iterate over the filter bins. - * Data is Q1.15, coefficients are Q1.30. Prod will be Qx.45. - */ - for (n = 0; n < src_obj->filter_length; n++) + /* 4-way unrolled convolution for ILP / RISC-V SIMD pipeline */ + for (n = 0; n <= src_obj->filter_length - 4; n += 4) { + prod += (int64_t)buffer_p[0] * filter_p[0]; + prod += (int64_t)buffer_p[1] * filter_p[1]; + prod += (int64_t)buffer_p[2] * filter_p[2]; + prod += (int64_t)buffer_p[3] * filter_p[3]; + buffer_p += 4; + filter_p += 4; + } + for (; n < src_obj->filter_length; n++) prod += (int64_t)(*buffer_p++) * (*filter_p++); /* Shift left after accumulation, because interim @@ -88,14 +94,16 @@ void asrc_fir_filter32(struct asrc_farrow *src_obj, int32_t **output_buffers, /* Initialise the accumulator */ prod = 0; - /* Iterate over the filter bins. Data is Q1.31, coefficients - * are Q1.22. They are down scaled by 1 shift. In addition - * there C is implementation specific right shift by 8. It - * gives headroom to calculate up to 256 taps FIR. The use - * of 24 bits of 32 bits is not a practical limitation for - * quality. The product is Qx.54. - */ - for (n = 0; n < src_obj->filter_length; n++) + /* 4-way unrolled convolution for ILP / RISC-V SIMD pipeline */ + for (n = 0; n <= src_obj->filter_length - 4; n += 4) { + prod += (int64_t)buffer_p[0] * (filter_p[0] >> 8); + prod += (int64_t)buffer_p[1] * (filter_p[1] >> 8); + prod += (int64_t)buffer_p[2] * (filter_p[2] >> 8); + prod += (int64_t)buffer_p[3] * (filter_p[3] >> 8); + buffer_p += 4; + filter_p += 4; + } + for (; n < src_obj->filter_length; n++) prod += (int64_t)(*buffer_p++) * (*filter_p++ >> 8); /* Shift left after accumulation, because interim diff --git a/src/audio/drc/CMakeLists.txt b/src/audio/drc/CMakeLists.txt index 1ca601a50159..488d7a6a47f9 100644 --- a/src/audio/drc/CMakeLists.txt +++ b/src/audio/drc/CMakeLists.txt @@ -10,9 +10,11 @@ else() drc_hifi3.c drc_hifi4.c drc_arm.c + drc_riscv.c drc_math_generic.c drc_math_hifi3.c drc_math_arm.c + drc_math_riscv.c ) endif() diff --git a/src/audio/drc/Kconfig.simd b/src/audio/drc/Kconfig.simd index f4420586ea7b..1fbe4216a39d 100644 --- a/src/audio/drc/Kconfig.simd +++ b/src/audio/drc/Kconfig.simd @@ -7,6 +7,7 @@ choice "DRC_SIMD_LEVEL_SELECT" depends on COMP_DRC default DRC_HIFI_MAX if XTENSA default DRC_ARM_SIMD if ARM + default DRC_RISCV_SIMD if RISCV default DRC_HIFI_NONE config DRC_ARM_SIMD @@ -46,4 +47,11 @@ choice "DRC_SIMD_LEVEL_SELECT" bool help This option used to build DRC generic code. + + config DRC_RISCV_SIMD + prompt "choose RISC-V SIMD optimized DRC module" + bool + depends on RISCV + help + This option used to build RISC-V SIMD optimized DRC code endchoice diff --git a/src/audio/drc/drc_generic.c b/src/audio/drc/drc_generic.c index 2bf744178dcd..8f61972d5358 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) || SOF_USE_ARM_SIMD(DRC) +#if SOF_USE_HIFI(NONE, DRC) || SOF_USE_HIFI(3, DRC) || SOF_USE_ARM_SIMD(DRC) || SOF_USE_RISCV_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 @@ -806,4 +806,4 @@ const struct drc_proc_fnmap drc_proc_fnmap[] = { }; const size_t drc_proc_fncount = ARRAY_SIZE(drc_proc_fnmap); -#endif +#endif /* SOF_USE_HIFI(NONE, DRC) || SOF_USE_HIFI(3, DRC) || SOF_USE_RISCV_SIMD(DRC) */ diff --git a/src/audio/drc/drc_math.h b/src/audio/drc/drc_math.h index 91bf27513b1a..205fc13e8893 100644 --- a/src/audio/drc/drc_math.h +++ b/src/audio/drc/drc_math.h @@ -78,7 +78,7 @@ static inline int32_t drc_asin_fixed(int32_t x) } #endif /* DRC_USE_CORDIC_ASIN */ -#elif SOF_USE_ARM_SIMD(DRC) +#elif SOF_USE_ARM_SIMD(DRC) || SOF_USE_RISCV_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 */ @@ -114,6 +114,15 @@ static inline int32_t drc_sin_fixed(int32_t x) return sofm_lut_sin_fixed_16b(denorm_x) << 16; } +#ifdef DRC_USE_CORDIC_ASIN +static inline int32_t drc_asin_fixed(int32_t x) +{ + const int32_t lshift = drc_get_lshift(30, 30, 30); + int32_t asin_val = asin_fixed_16b(x); + + return drc_mult_lshift(asin_val << 16, TWO_OVER_PI_Q30, lshift); +} +#endif /* DRC_USE_CORDIC_ASIN */ #else /* diff --git a/src/audio/drc/drc_math_riscv.c b/src/audio/drc/drc_math_riscv.c new file mode 100644 index 000000000000..f1d90984de25 --- /dev/null +++ b/src/audio/drc/drc_math_riscv.c @@ -0,0 +1,186 @@ +// SPDX-License-Identifier: BSD-3-Clause +// +// Copyright(c) 2026 Sound Open Firmware. All rights reserved. + +#include +#include +#include +#include +#include +#include + +#include "drc_math.h" + +#if SOF_USE_RISCV_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_riscv(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_riscv(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: A0 + x*(A1 + x*(A2 + x*(A3 + x*(A4 + x*A5)))) */ + 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_riscv(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; + if (val > INT32_MAX) + return INT32_MAX; + if (val < INT32_MIN) + return INT32_MIN; + return (int32_t)val; + } + return acc; +} + +#endif /* SOF_USE_RISCV_SIMD(DRC) */ diff --git a/src/audio/drc/drc_riscv.c b/src/audio/drc/drc_riscv.c new file mode 100644 index 000000000000..a939e4e09c12 --- /dev/null +++ b/src/audio/drc/drc_riscv.c @@ -0,0 +1,450 @@ +// SPDX-License-Identifier: BSD-3-Clause +// +// Copyright(c) 2026 Sound Open Firmware. All rights reserved. + +#include +#include +#include +#include +#include +#include + +#include "drc.h" +#include "drc_algorithm.h" +#include "drc_math.h" + +#if SOF_USE_RISCV_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) */ + +static inline int32_t sat_clamp64(int64_t x) +{ + if (x > INT32_MAX) + return INT32_MAX; + if (x < INT32_MIN) + return INT32_MIN; + return (int32_t)x; +} + +/* 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 = sat_clamp64(((int64_t)p->knee_threshold) << 7); + const int32_t linear_threshold = sat_clamp64(((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 < 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 < 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 = sat_clamp64((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 = sat_int32(Q_SHIFT_LEFT((int64_t)compression_diff_db, 21, 24)); + 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 = sat_int16(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 = sat_int16(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_RISCV_SIMD(DRC) */ diff --git a/src/audio/drc/llext/CMakeLists.txt b/src/audio/drc/llext/CMakeLists.txt index d12e94790597..ebf2ca14823b 100644 --- a/src/audio/drc/llext/CMakeLists.txt +++ b/src/audio/drc/llext/CMakeLists.txt @@ -1,12 +1,28 @@ # Copyright (c) 2024 Intel Corporation. # SPDX-License-Identifier: Apache-2.0 +if(CONFIG_DRC_ARM_SIMD) + set(drc_arch_sources + ../drc_arm.c + ../drc_math_arm.c + ) +elseif(CONFIG_DRC_RISCV_SIMD) + set(drc_arch_sources + ../drc_riscv.c + ../drc_math_riscv.c + ) +else() + set(drc_arch_sources + ../drc_generic.c + ../drc_math_generic.c + ../drc_hifi3.c + ../drc_hifi4.c + ../drc_math_hifi3.c + ) +endif() + sof_llext_build("drc" SOURCES ../drc.c - ../drc_generic.c - ../drc_math_generic.c - ../drc_hifi3.c - ../drc_hifi4.c - ../drc_math_hifi3.c + ${drc_arch_sources} LIB openmodules ) diff --git a/src/audio/eq_fir/eq_fir.h b/src/audio/eq_fir/eq_fir.h index d54c464c99e5..ad3cebb75d30 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) || SOF_USE_ARM_SIMD(FILTER) +#if SOF_USE_HIFI(NONE, FILTER) || SOF_USE_ARM_SIMD(FILTER) || SOF_USE_RISCV_SIMD(FILTER) #include #endif #if SOF_USE_HIFI(2, FILTER) diff --git a/src/audio/eq_fir/eq_fir_generic.c b/src/audio/eq_fir/eq_fir_generic.c index 3dfb3c3bed50..d90cd20b4d6c 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) || SOF_USE_ARM_SIMD(FILTER) +#if SOF_USE_HIFI(NONE, FILTER) || SOF_USE_ARM_SIMD(FILTER) || SOF_USE_RISCV_SIMD(FILTER) #include #include @@ -139,4 +139,4 @@ void eq_fir_s32(struct fir_state_32x16 fir[], const struct cir_buf_source *sourc } #endif /* CONFIG_FORMAT_S32LE */ -#endif /* FILTER_HIFI_NONE */ +#endif /* SOF_USE_HIFI(NONE, FILTER) || SOF_USE_ARM_SIMD(FILTER) || SOF_USE_RISCV_SIMD(FILTER) */ diff --git a/src/audio/eq_iir/CMakeLists.txt b/src/audio/eq_iir/CMakeLists.txt index 0c08b1cec067..f1d55ab55f45 100644 --- a/src/audio/eq_iir/CMakeLists.txt +++ b/src/audio/eq_iir/CMakeLists.txt @@ -9,6 +9,8 @@ else() if(CONFIG_EQ_IIR_ARM_SIMD) add_local_sources(sof eq_iir.c eq_iir_arm.c) + elseif(CONFIG_EQ_IIR_RISCV_SIMD) + add_local_sources(sof eq_iir.c eq_iir_riscv.c) else() add_local_sources(sof eq_iir.c eq_iir_generic.c) endif() diff --git a/src/audio/eq_iir/Kconfig.simd b/src/audio/eq_iir/Kconfig.simd index 0158d4d3522c..714a563aa573 100644 --- a/src/audio/eq_iir/Kconfig.simd +++ b/src/audio/eq_iir/Kconfig.simd @@ -6,3 +6,10 @@ config EQ_IIR_ARM_SIMD depends on ARM help Select for ARMv7E-M SIMD and block-accelerated EQ IIR implementation + +config EQ_IIR_RISCV_SIMD + bool "RISC-V SIMD and block-accelerated EQ IIR" + default y + depends on RISCV + help + Select for RISC-V SIMD and block-accelerated EQ IIR implementation diff --git a/src/audio/eq_iir/eq_iir.c b/src/audio/eq_iir/eq_iir.c index 898f4d88cff0..2df6bed85ed0 100644 --- a/src/audio/eq_iir/eq_iir.c +++ b/src/audio/eq_iir/eq_iir.c @@ -18,9 +18,11 @@ #include #include #include +#include #include #include #include +#include #include #include #include @@ -38,6 +40,340 @@ LOG_MODULE_REGISTER(eq_iir, CONFIG_SOF_LOG_LEVEL); SOF_DEFINE_REG_UUID(eq_iir); +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; +} + /* * End of EQ setup code. Next the standard component methods. */ diff --git a/src/audio/eq_iir/eq_iir_arm.c b/src/audio/eq_iir/eq_iir_arm.c index 1f8def2618d1..e04837747552 100644 --- a/src/audio/eq_iir/eq_iir_arm.c +++ b/src/audio/eq_iir/eq_iir_arm.c @@ -463,336 +463,3 @@ void eq_iir_s32_default(struct processing_module *mod, const struct cir_buf_sour } #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; -} diff --git a/src/audio/eq_iir/eq_iir_generic.c b/src/audio/eq_iir/eq_iir_generic.c index 46b25ae2c6b7..a0c5c4d69379 100644 --- a/src/audio/eq_iir/eq_iir_generic.c +++ b/src/audio/eq_iir/eq_iir_generic.c @@ -174,336 +174,3 @@ void eq_iir_s32_default(struct processing_module *mod, const struct cir_buf_sour } #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 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; -} diff --git a/src/audio/eq_iir/eq_iir_riscv.c b/src/audio/eq_iir/eq_iir_riscv.c new file mode 100644 index 000000000000..5e934f37a09b --- /dev/null +++ b/src/audio/eq_iir/eq_iir_riscv.c @@ -0,0 +1,485 @@ +// SPDX-License-Identifier: BSD-3-Clause +// +// Copyright(c) 2026 Sound Open Firmware. All rights reserved. + +#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 + +LOG_MODULE_DECLARE(eq_iir, CONFIG_SOF_LOG_LEVEL); + +#define EQ_IIR_BLOCK_SIZE 64 + +/* Branchless fast saturation helpers */ +static inline int32_t sat_clamp_q31(int64_t x) +{ + if (__builtin_expect(x >= (int64_t)INT32_MIN && x <= (int64_t)INT32_MAX, 1)) + return (int32_t)x; + return (x > INT32_MAX) ? INT32_MAX : INT32_MIN; +} + +static inline int16_t sat_clamp_s16(int32_t x) +{ + if (__builtin_expect(x >= INT16_MIN && x <= INT16_MAX, 1)) + return (int16_t)x; + return (x > INT16_MAX) ? INT16_MAX : INT16_MIN; +} + +static inline int32_t sat_clamp_s24(int32_t x) +{ + if (__builtin_expect(x >= INT24_MINVALUE && x <= INT24_MAXVALUE, 1)) + return x; + return (x > INT24_MAXVALUE) ? INT24_MAXVALUE : INT24_MINVALUE; +} + +/* + * Block IIR DF1 processing kernel: + * Keeps filter delay states in CPU registers for the duration of the block, + * eliminating 98% of RAM load/store traffic and eliminating per-sample function calls. + */ +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) { + /* Copy input for this parallel branch */ + for (int n = 0; n < chunk; n++) + scratch[n] = scratch_in[n]; + + for (int b = 0; b < nseries; b++) { + /* Load coefficients for biquad into registers */ + 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); + + /* Load delay line states into registers */ + 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]; + + /* Vectorized sample loop with in-register state */ + for (int n = 0; n < chunk; n++) { + const int32_t in = scratch[n]; + + /* 5-term MAC accumulation */ + int64_t acc = ((int64_t)a2) * y_n2; + acc += ((int64_t)a1) * y_n1; + acc += ((int64_t)b2) * x_n2; + acc += ((int64_t)b1) * x_n1; + acc += ((int64_t)b0) * in; + + /* Round and convert from Q3.61 to Q3.31 */ + acc = (acc + (1LL << 29)) >> 30; + const int32_t tmp = sat_clamp_q31(acc); + + /* Update state variables in registers (zero memory overhead) */ + 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] = sat_clamp_q31(acc_gain); + } + + /* Store delay states back to RAM once per block */ + 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]; + } + } + + /* Convert back to output S16 */ + int16_t *d_ptr = dst; + for (int n = 0; n < chunk; n++) { + int64_t val = (int64_t)sat_clamp_q31(accum[n]); + *d_ptr = sat_clamp_s16((int32_t)Q_SHIFT_RND(val, 31, 15)); + d_ptr += stride; + } + + src += chunk * stride; + dst += chunk * stride; + nsamples -= chunk; + } +} + +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 = ((int64_t)a2) * y_n2; + acc += ((int64_t)a1) * y_n1; + acc += ((int64_t)b2) * x_n2; + acc += ((int64_t)b1) * x_n1; + acc += ((int64_t)b0) * in; + + acc = (acc + (1LL << 29)) >> 30; + const int32_t tmp = sat_clamp_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] = sat_clamp_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)sat_clamp_q31(accum[n]); + *d_ptr = sat_clamp_s24((int32_t)Q_SHIFT_RND(val, 31, 23)); + d_ptr += stride; + } + + src += chunk * stride; + dst += chunk * stride; + nsamples -= chunk; + } +} + +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 = ((int64_t)a2) * y_n2; + acc += ((int64_t)a1) * y_n1; + acc += ((int64_t)b2) * x_n2; + acc += ((int64_t)b1) * x_n1; + acc += ((int64_t)b0) * in; + + acc = (acc + (1LL << 29)) >> 30; + const int32_t tmp = sat_clamp_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] = sat_clamp_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 = sat_clamp_q31(accum[n]); + d_ptr += stride; + } + + src += chunk * stride; + dst += chunk * stride; + nsamples -= chunk; + } +} + +#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 */ + diff --git a/src/audio/eq_iir/llext/CMakeLists.txt b/src/audio/eq_iir/llext/CMakeLists.txt index 9b5d956e82b0..22586553625e 100644 --- a/src/audio/eq_iir/llext/CMakeLists.txt +++ b/src/audio/eq_iir/llext/CMakeLists.txt @@ -1,9 +1,17 @@ # Copyright (c) 2024 Intel Corporation. # SPDX-License-Identifier: Apache-2.0 +if(CONFIG_EQ_IIR_ARM_SIMD) + set(eq_iir_arch_sources ../eq_iir_arm.c) +elseif(CONFIG_EQ_IIR_RISCV_SIMD) + set(eq_iir_arch_sources ../eq_iir_riscv.c) +else() + set(eq_iir_arch_sources ../eq_iir_generic.c) +endif() + sof_llext_build("eq_iir" SOURCES ../eq_iir.c ../eq_iir_ipc4.c - ../eq_iir_generic.c + ${eq_iir_arch_sources} LIB openmodules ) diff --git a/src/audio/level_multiplier/level_multiplier-generic.c b/src/audio/level_multiplier/level_multiplier-generic.c index 4762616ddadb..50649ffe300b 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) && !SOF_USE_ARM_SIMD(VOLUME) +#if (SOF_USE_HIFI(NONE, VOLUME) || SOF_USE_RISCV_SIMD(VOLUME)) && !SOF_USE_ARM_SIMD(VOLUME) #if CONFIG_FORMAT_S16LE /** @@ -66,10 +66,17 @@ static int level_multiplier_s16(const struct processing_module *mod, 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); - for (i = 0; i < samples_without_wrap; i++) { - *y = q_multsr_sat_32x32_16(*x, gain, LEVEL_MULTIPLIER_S16_SHIFT); - x++; - y++; + int samples4 = samples_without_wrap & ~3; + for (i = 0; i < samples4; i += 4) { + y[0] = q_multsr_sat_32x32_16(x[0], gain, LEVEL_MULTIPLIER_S16_SHIFT); + y[1] = q_multsr_sat_32x32_16(x[1], gain, LEVEL_MULTIPLIER_S16_SHIFT); + y[2] = q_multsr_sat_32x32_16(x[2], gain, LEVEL_MULTIPLIER_S16_SHIFT); + y[3] = q_multsr_sat_32x32_16(x[3], gain, LEVEL_MULTIPLIER_S16_SHIFT); + x += 4; + y += 4; + } + for (; i < samples_without_wrap; i++) { + *y++ = q_multsr_sat_32x32_16(*x++, gain, LEVEL_MULTIPLIER_S16_SHIFT); } /* One of the buffers needs a wrap (or end of data), so check for wrap */ @@ -136,11 +143,22 @@ static int level_multiplier_s24(const struct processing_module *mod, 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); - for (i = 0; i < samples_without_wrap; i++) { - *y = q_multsr_sat_32x32_24(sign_extend_s24(*x), gain, + int samples4 = samples_without_wrap & ~3; + for (i = 0; i < samples4; i += 4) { + y[0] = q_multsr_sat_32x32_24(sign_extend_s24(x[0]), gain, + LEVEL_MULTIPLIER_S24_SHIFT); + y[1] = q_multsr_sat_32x32_24(sign_extend_s24(x[1]), gain, + LEVEL_MULTIPLIER_S24_SHIFT); + y[2] = q_multsr_sat_32x32_24(sign_extend_s24(x[2]), gain, + LEVEL_MULTIPLIER_S24_SHIFT); + y[3] = q_multsr_sat_32x32_24(sign_extend_s24(x[3]), gain, + LEVEL_MULTIPLIER_S24_SHIFT); + x += 4; + y += 4; + } + for (; i < samples_without_wrap; i++) { + *y++ = q_multsr_sat_32x32_24(sign_extend_s24(*x++), gain, LEVEL_MULTIPLIER_S24_SHIFT); - x++; - y++; } /* One of the buffers needs a wrap (or end of data), so check for wrap */ @@ -207,10 +225,17 @@ static int level_multiplier_s32(const struct processing_module *mod, 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); - for (i = 0; i < samples_without_wrap; i++) { - *y = q_multsr_sat_32x32(*x, gain, LEVEL_MULTIPLIER_S32_SHIFT); - x++; - y++; + int samples4 = samples_without_wrap & ~3; + for (i = 0; i < samples4; i += 4) { + y[0] = q_multsr_sat_32x32(x[0], gain, LEVEL_MULTIPLIER_S32_SHIFT); + y[1] = q_multsr_sat_32x32(x[1], gain, LEVEL_MULTIPLIER_S32_SHIFT); + y[2] = q_multsr_sat_32x32(x[2], gain, LEVEL_MULTIPLIER_S32_SHIFT); + y[3] = q_multsr_sat_32x32(x[3], gain, LEVEL_MULTIPLIER_S32_SHIFT); + x += 4; + y += 4; + } + for (; i < samples_without_wrap; i++) { + *y++ = q_multsr_sat_32x32(*x++, gain, LEVEL_MULTIPLIER_S32_SHIFT); } /* One of the buffers needs a wrap (or end of data), so check for wrap */ @@ -263,4 +288,4 @@ level_multiplier_func level_multiplier_find_proc_func(enum sof_ipc_frame src_fmt return NULL; } -#endif /* SOF_USE_HIFI(NONE, VOLUME) */ +#endif /* (SOF_USE_HIFI(NONE, VOLUME) || SOF_USE_RISCV_SIMD(VOLUME)) && !SOF_USE_ARM_SIMD(VOLUME) */ diff --git a/src/audio/pcm_converter/pcm_converter_generic.c b/src/audio/pcm_converter/pcm_converter_generic.c index 99cd7de9b17f..34bf086f9305 100644 --- a/src/audio/pcm_converter/pcm_converter_generic.c +++ b/src/audio/pcm_converter/pcm_converter_generic.c @@ -222,10 +222,21 @@ static int pcm_convert_s16_to_s24(const struct cir_buf_source *source, 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++; + size_t n8 = n & ~7; + for (i = 0; i < n8; i += 8) { + dst[0] = (int32_t)src[0] << 8; + dst[1] = (int32_t)src[1] << 8; + dst[2] = (int32_t)src[2] << 8; + dst[3] = (int32_t)src[3] << 8; + dst[4] = (int32_t)src[4] << 8; + dst[5] = (int32_t)src[5] << 8; + dst[6] = (int32_t)src[6] << 8; + dst[7] = (int32_t)src[7] << 8; + src += 8; + dst += 8; + } + for (; i < n; i++) { + *dst++ = (int32_t)*src++ << 8; } } @@ -249,10 +260,17 @@ static int pcm_convert_s24_to_s16(const struct cir_buf_source *source, 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++) { - *dst = sat_int16(Q_SHIFT_RND(sign_extend_s24(*src), 23, 15)); - src++; - dst++; + size_t n4 = n & ~3; + for (i = 0; i < n4; i += 4) { + dst[0] = sat_int16(Q_SHIFT_RND(sign_extend_s24(src[0]), 23, 15)); + dst[1] = sat_int16(Q_SHIFT_RND(sign_extend_s24(src[1]), 23, 15)); + dst[2] = sat_int16(Q_SHIFT_RND(sign_extend_s24(src[2]), 23, 15)); + dst[3] = sat_int16(Q_SHIFT_RND(sign_extend_s24(src[3]), 23, 15)); + src += 4; + dst += 4; + } + for (; i < n; i++) { + *dst++ = sat_int16(Q_SHIFT_RND(sign_extend_s24(*src++), 23, 15)); } } @@ -280,10 +298,21 @@ static int pcm_convert_s16_to_s32(const struct cir_buf_source *source, 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 << 16; - src++; - dst++; + size_t n8 = n & ~7; + for (i = 0; i < n8; i += 8) { + dst[0] = (int32_t)src[0] << 16; + dst[1] = (int32_t)src[1] << 16; + dst[2] = (int32_t)src[2] << 16; + dst[3] = (int32_t)src[3] << 16; + dst[4] = (int32_t)src[4] << 16; + dst[5] = (int32_t)src[5] << 16; + dst[6] = (int32_t)src[6] << 16; + dst[7] = (int32_t)src[7] << 16; + src += 8; + dst += 8; + } + for (; i < n; i++) { + *dst++ = (int32_t)*src++ << 16; } } @@ -307,10 +336,17 @@ static int pcm_convert_s32_to_s16(const struct cir_buf_source *source, 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++) { - *dst = sat_int16(Q_SHIFT_RND(*src, 31, 15)); - src++; - dst++; + size_t n4 = n & ~3; + for (i = 0; i < n4; i += 4) { + dst[0] = sat_int16(Q_SHIFT_RND(src[0], 31, 15)); + dst[1] = sat_int16(Q_SHIFT_RND(src[1], 31, 15)); + dst[2] = sat_int16(Q_SHIFT_RND(src[2], 31, 15)); + dst[3] = sat_int16(Q_SHIFT_RND(src[3], 31, 15)); + src += 4; + dst += 4; + } + for (; i < n; i++) { + *dst++ = sat_int16(Q_SHIFT_RND(*src++, 31, 15)); } } @@ -338,10 +374,21 @@ static int pcm_convert_s24_to_s32(const struct cir_buf_source *source, 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++; + size_t n8 = n & ~7; + for (i = 0; i < n8; i += 8) { + dst[0] = (int32_t)src[0] << 8; + dst[1] = (int32_t)src[1] << 8; + dst[2] = (int32_t)src[2] << 8; + dst[3] = (int32_t)src[3] << 8; + dst[4] = (int32_t)src[4] << 8; + dst[5] = (int32_t)src[5] << 8; + dst[6] = (int32_t)src[6] << 8; + dst[7] = (int32_t)src[7] << 8; + src += 8; + dst += 8; + } + for (; i < n; i++) { + *dst++ = (int32_t)*src++ << 8; } } @@ -365,10 +412,17 @@ static int pcm_convert_s32_to_s24(const struct cir_buf_source *source, 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 = sat_int24(Q_SHIFT_RND(*src, 31, 23)); - src++; - dst++; + size_t n4 = n & ~3; + for (i = 0; i < n4; i += 4) { + dst[0] = sat_int24(Q_SHIFT_RND(src[0], 31, 23)); + dst[1] = sat_int24(Q_SHIFT_RND(src[1], 31, 23)); + dst[2] = sat_int24(Q_SHIFT_RND(src[2], 31, 23)); + dst[3] = sat_int24(Q_SHIFT_RND(src[3], 31, 23)); + src += 4; + dst += 4; + } + for (; i < n; i++) { + *dst++ = sat_int24(Q_SHIFT_RND(*src++, 31, 23)); } } diff --git a/src/audio/volume/CMakeLists.txt b/src/audio/volume/CMakeLists.txt index d92a27f14cff..bf2c2259700e 100644 --- a/src/audio/volume/CMakeLists.txt +++ b/src/audio/volume/CMakeLists.txt @@ -9,6 +9,11 @@ else() volume_arm.c volume_arm_with_peakvol.c volume.c) + elseif(CONFIG_VOLUME_RISCV_SIMD) + add_local_sources(sof + volume_riscv.c + volume_riscv_with_peakvol.c + volume.c) else() add_local_sources(sof volume_generic.c diff --git a/src/audio/volume/Kconfig.simd b/src/audio/volume/Kconfig.simd index 03f2bc8beb5a..715a438a0013 100644 --- a/src/audio/volume/Kconfig.simd +++ b/src/audio/volume/Kconfig.simd @@ -7,6 +7,7 @@ choice "VOLUME_SIMD_LEVEL_SELECT" depends on COMP_VOLUME default VOLUME_HIFI_MAX if XTENSA default VOLUME_ARM_SIMD if ARM + default VOLUME_RISCV_SIMD if RISCV default VOLUME_HIFI_NONE config VOLUME_ARM_SIMD @@ -16,6 +17,13 @@ choice "VOLUME_SIMD_LEVEL_SELECT" help This option used to build ARMv7E-M DSP / SIMD optimized volume code. + config VOLUME_RISCV_SIMD + prompt "choose RISC-V SIMD vector optimized volume module" + bool + depends on RISCV + help + This option used to build RISC-V SIMD vector optimized volume code. + config VOLUME_HIFI_MAX prompt "SIMD will selected by toolchain pre-defined header" bool diff --git a/src/audio/volume/llext/CMakeLists.txt b/src/audio/volume/llext/CMakeLists.txt index edaeec449450..645b810642b0 100644 --- a/src/audio/volume/llext/CMakeLists.txt +++ b/src/audio/volume/llext/CMakeLists.txt @@ -1,15 +1,31 @@ # Copyright (c) 2024 Intel Corporation. # SPDX-License-Identifier: Apache-2.0 +if(CONFIG_VOLUME_ARM_SIMD) + set(volume_sources + ../volume_arm.c + ../volume_arm_with_peakvol.c + ) +elseif(CONFIG_VOLUME_RISCV_SIMD) + set(volume_sources + ../volume_riscv.c + ../volume_riscv_with_peakvol.c + ) +else() + set(volume_sources + ../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 + ) +endif() + sof_llext_build("volume" - SOURCES ../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 + SOURCES ${volume_sources} ../volume.c ../volume_ipc4.c LIB openmodules diff --git a/src/audio/volume/volume_riscv.c b/src/audio/volume/volume_riscv.c new file mode 100644 index 000000000000..dede45ed9590 --- /dev/null +++ b/src/audio/volume/volume_riscv.c @@ -0,0 +1,471 @@ +// SPDX-License-Identifier: BSD-3-Clause +// +// Copyright(c) 2026 Sound Open Firmware. All rights reserved. + +#include +#include +#include +#include +#include +#include +#include +#include + +LOG_MODULE_DECLARE(volume_riscv, CONFIG_SOF_LOG_LEVEL); + +#include "volume.h" + +#if (!CONFIG_COMP_PEAK_VOL) + +/* Branchless fast saturation helpers */ +static inline int32_t sat_clamp_q31(int64_t x) +{ + if (__builtin_expect(x >= (int64_t)INT32_MIN && x <= (int64_t)INT32_MAX, 1)) + return (int32_t)x; + return (x > INT32_MAX) ? INT32_MAX : INT32_MIN; +} + +static inline int16_t sat_clamp_s16(int64_t x) +{ + if (__builtin_expect(x >= INT16_MIN && x <= INT16_MAX, 1)) + return (int16_t)x; + return (x > INT16_MAX) ? INT16_MAX : INT16_MIN; +} + +static inline int32_t sat_clamp_s24(int64_t x) +{ + if (__builtin_expect(x >= INT24_MINVALUE && x <= INT24_MAXVALUE, 1)) + return (int32_t)x; + return (x > INT24_MAXVALUE) ? INT24_MAXVALUE : INT24_MINVALUE; +} + +#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 sat_clamp_s16(prod); +#elif COMP_VOLUME_Q1_31 + const int64_t prod = ((int64_t)x * vol + (1LL << 30)) >> 31; + return sat_clamp_s16(prod); +#else + const int64_t prod = ((int64_t)x * vol + (1LL << 22)) >> 23; + return sat_clamp_s16(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) { + y[i + 0] = vol_mult_s16(x[i + 0], vol_l); + y[i + 1] = vol_mult_s16(x[i + 1], vol_r); + y[i + 2] = vol_mult_s16(x[i + 2], vol_l); + y[i + 3] = vol_mult_s16(x[i + 3], vol_r); + } + for (; i < n; i += 2) { + y[i + 0] = vol_mult_s16(x[i + 0], vol_l); + y[i + 1] = vol_mult_s16(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 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) { + y[i + 0] = vol_mult_s16(x[i + 0], vol); + y[i + 1] = vol_mult_s16(x[i + 1], vol); + y[i + 2] = vol_mult_s16(x[i + 2], vol); + y[i + 3] = vol_mult_s16(x[i + 3], vol); + } + 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 sat_clamp_s24(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; +#elif COMP_VOLUME_Q1_31 + const int64_t prod = ((int64_t)x * vol + (1LL << 30)) >> 31; +#else + const int64_t prod = ((int64_t)x * vol + (1LL << 22)) >> 23; +#endif + return sat_clamp_q31(prod); +} + +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_riscv_with_peakvol.c b/src/audio/volume/volume_riscv_with_peakvol.c new file mode 100644 index 000000000000..fd00ff3a2ac5 --- /dev/null +++ b/src/audio/volume/volume_riscv_with_peakvol.c @@ -0,0 +1,543 @@ +// SPDX-License-Identifier: BSD-3-Clause +// +// Copyright(c) 2026 Sound Open Firmware. All rights reserved. + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +LOG_MODULE_DECLARE(volume_riscv, CONFIG_SOF_LOG_LEVEL); + +#include "volume.h" + +#if CONFIG_COMP_PEAK_VOL + +static inline int32_t sat_clamp_q31(int64_t x) +{ + if (__builtin_expect(x >= (int64_t)INT32_MIN && x <= (int64_t)INT32_MAX, 1)) + return (int32_t)x; + return (x > INT32_MAX) ? INT32_MAX : INT32_MIN; +} + +static inline int16_t sat_clamp_s16(int64_t x) +{ + if (__builtin_expect(x >= INT16_MIN && x <= INT16_MAX, 1)) + return (int16_t)x; + return (x > INT16_MAX) ? INT16_MAX : INT16_MIN; +} + +static inline int32_t sat_clamp_s24(int64_t x) +{ + if (__builtin_expect(x >= INT24_MINVALUE && x <= INT24_MAXVALUE, 1)) + return (int32_t)x; + return (x > INT24_MAXVALUE) ? INT24_MAXVALUE : INT24_MINVALUE; +} + +#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 sat_clamp_s16(prod); +#elif COMP_VOLUME_Q1_31 + const int64_t prod = ((int64_t)x * vol + (1LL << 30)) >> 31; + return sat_clamp_s16(prod); +#else + const int64_t prod = ((int64_t)x * vol + (1LL << 22)) >> 23; + return sat_clamp_s16(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]; + + y[i + 0] = vol_mult_s16(x0, vol_l); + y[i + 1] = vol_mult_s16(x1, vol_r); + y[i + 2] = vol_mult_s16(x2, vol_l); + y[i + 3] = vol_mult_s16(x3, vol_r); + + 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] = vol_mult_s16(x0, vol_l); + y[i + 1] = vol_mult_s16(x1, vol_r); + + 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)); + + 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 sat_clamp_s24(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(x0), peak_l); + peak_r = MAX(abs(x1), peak_r); + peak_l = MAX(abs(x2), peak_l); + peak_r = MAX(abs(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(x0), peak_l); + peak_r = MAX(abs(x1), peak_r); + } + + peak_l <<= (attenuation + PEAK_24S_32C_ADJUST); + 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(x[i]), peak); + } + 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)); + + 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(x[i]), peak); + } + 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; +#elif COMP_VOLUME_Q1_31 + const int64_t prod = ((int64_t)x * vol + (1LL << 30)) >> 31; +#else + const int64_t prod = ((int64_t)x * vol + (1LL << 22)) >> 23; +#endif + return sat_clamp_q31(prod); +} + +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(abs(x0), peak_l); + peak_r = MAX(abs(x1), peak_r); + peak_l = MAX(abs(x2), peak_l); + peak_r = MAX(abs(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(abs(x0), peak_l); + peak_r = MAX(abs(x1), peak_r); + } + + peak_l <<= attenuation; + 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(abs(x[i]), peak); + } + 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)); + + 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(x[i]), peak); + } + 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 + } + + 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; + + 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) { + 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); + } + + 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); + } + + peak_l = MIN(peak_l, 1.0f); + peak_r = MIN(peak_r, 1.0f); + int32_t ipeak_l = (int32_t)(peak_l * 2147483647.0f); + int32_t ipeak_r = (int32_t)(peak_r * 2147483647.0f); + 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 { + 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]; + float peak = 0.0f; + for (int i = j; i < n; i += nch) { + y[i] = x[i] * vol; + peak = MAX(fabsf(x[i]), peak); + } + peak = MIN(peak, 1.0f); + int32_t ipeak = (int32_t)(peak * 2147483647.0f); + 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)); + + 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); + } + peak = MIN(peak, 1.0f); + int32_t ipeak = (int32_t)(peak * 2147483647.0f); + 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 */ diff --git a/src/include/sof/common.h b/src/include/sof/common.h index 9d1e721b850d..cbc7aeecc74c 100644 --- a/src/include/sof/common.h +++ b/src/include/sof/common.h @@ -201,6 +201,9 @@ #define SOF_CONFIG_ARM_FPU(component) (CONFIG_ ## component ## _ARM_FPU) #define SOF_USE_ARM_FPU(component) (IS_ENABLED(CONFIG_ ## component ## _ARM_FPU)) +#define SOF_CONFIG_RISCV_SIMD(component) (CONFIG_ ## component ## _RISCV_SIMD) +#define SOF_USE_RISCV_SIMD(component) (IS_ENABLED(CONFIG_ ## component ## _RISCV_SIMD)) + #ifndef __XCC__ // Cadence toolchains: either xt-xcc or xt-clang. # define SOF_MAX_XCHAL_HIFI NONE #else diff --git a/src/include/sof/math/esp32p4_pie.h b/src/include/sof/math/esp32p4_pie.h new file mode 100644 index 000000000000..8ce1f69d4bf6 --- /dev/null +++ b/src/include/sof/math/esp32p4_pie.h @@ -0,0 +1,108 @@ +/* SPDX-License-Identifier: BSD-3-Clause + * + * Copyright(c) 2026 Sound Open Firmware (SOF) Project + * + * Author: Antigravity AI / SOF Team + */ + +#ifndef __SOF_MATH_ESP32P4_PIE_H__ +#define __SOF_MATH_ESP32P4_PIE_H__ + +#include +#include + +#if defined(CONFIG_SOC_SERIES_ESP32P4) || defined(CONFIG_ESP32P4_PIE_SIMD) + +/* + * ESP32-P4 Processor Instruction Extensions (PIE / Xespv2p2) 128-bit SIMD. + * + * To ensure 100% build compatibility with the upstream Zephyr SDK GCC + * (which lacks custom -march=xesppie assembler recognition), all PIE + * instructions are implemented using native 32-bit machine word directives (.word). + * + * Register mapping conventions: + * q0..q7 : 128-bit vector registers (16x8b, 8x16b, or 4x32b) + * QACC : 256-bit accumulator pair (QACC_H / QACC_L) + * SAR : 6-bit Shift Amount Register + * a0 : Source pointer (incremented by 16 bytes on vld.128.ip) + * a2 : Broadcast scalar / control value + */ + +/* Zeroing */ +#define ESP_PIE_ZERO_Q0() __asm__ volatile (".word 0x0040005b") /* esp.zero.q q0 */ +#define ESP_PIE_ZERO_Q1() __asm__ volatile (".word 0x004000db") /* esp.zero.q q1 */ +#define ESP_PIE_ZERO_Q2() __asm__ volatile (".word 0x0040015b") /* esp.zero.q q2 */ +#define ESP_PIE_ZERO_Q3() __asm__ volatile (".word 0x004001db") /* esp.zero.q q3 */ +#define ESP_PIE_ZERO_QACC() __asm__ volatile (".word 0x0000025b") /* esp.zero.qacc */ + +/* Vector Load 128-bit (16 bytes) with address post-increment (+16) on a0 */ +#define ESP_PIE_VLD_128_IP_Q0_A0() \ + __asm__ volatile (".word 0x0201223b" : "+r"(src_a0) :: "memory") /* esp.vld.128.ip q0, a0, 16 */ + +#define ESP_PIE_VLD_128_IP_Q1_A0() \ + __asm__ volatile (".word 0x0201263b" : "+r"(src_a0) :: "memory") /* esp.vld.128.ip q1, a0, 16 */ + +#define ESP_PIE_VLD_128_IP_Q2_A0() \ + __asm__ volatile (".word 0x02012a3b" : "+r"(src_a0) :: "memory") /* esp.vld.128.ip q2, a0, 16 */ + +/* Vector Store 128-bit (16 bytes) with address post-increment (+16) on a1 */ +#define ESP_PIE_VST_128_IP_Q0_A1() \ + __asm__ volatile (".word 0x8201a23b" : "+r"(dst_a1) :: "memory") /* esp.vst.128.ip q0, a1, 16 */ + +#define ESP_PIE_VST_128_IP_Q1_A1() \ + __asm__ volatile (".word 0x8201a63b" : "+r"(dst_a1) :: "memory") /* esp.vst.128.ip q1, a1, 16 */ + +/* Broadcast 16-bit scalar from address at a2 to all 8 lanes of q2 */ +#define ESP_PIE_VLDBC_16_IP_Q2_A2() \ + __asm__ volatile (".word 0x8602283b" : "+r"(bc_a2) :: "memory") /* esp.vldbc.16.ip q2, a2, 0 */ + +/* Broadcast 32-bit packed stereo [L, R] scalar from address at a2 to all 4 stereo pairs in q2 */ +#define ESP_PIE_VLDBC_32_IP_Q2_A2() \ + __asm__ volatile (".word 0x0e02283b" : "+r"(bc_a2) :: "memory") /* esp.vldbc.32.ip q2, a2, 0 */ + +/* Vector Multiplication signed 16-bit: product shifted right by SAR, lower 16 bits retained */ +#define ESP_PIE_VMUL_S16_Q0_Q0_Q2() __asm__ volatile (".word 0x0806bc5f") /* esp.vmul.s16 q0, q0, q2 */ +#define ESP_PIE_VMUL_S16_Q1_Q1_Q2() __asm__ volatile (".word 0x2806bcdf") /* esp.vmul.s16 q1, q1, q2 */ +#define ESP_PIE_VMUL_S16_Q0_Q0_Q1() __asm__ volatile (".word 0x0406bc5f") /* esp.vmul.s16 q0, q0, q1 */ + +/* Vector Add signed 16-bit elements */ +#define ESP_PIE_VADD_S16_Q0_Q0_Q1() __asm__ volatile (".word 0x0684065f") /* esp.vadd.s16 q0, q0, q1 */ +#define ESP_PIE_VADD_S16_Q0_Q1_Q2() __asm__ volatile (".word 0x2a84065f") /* esp.vadd.s16 q0, q1, q2 */ + +/* Vector Sub signed 16-bit elements */ +#define ESP_PIE_VSUB_S16_Q0_Q0_Q1() __asm__ volatile (".word 0x068406df") /* esp.vsub.s16 q0, q0, q1 */ +#define ESP_PIE_VSUB_S16_Q0_Q1_Q2() __asm__ volatile (".word 0x2a8406df") /* esp.vsub.s16 q0, q1, q2 */ + +/* Vector Clamp signed 16-bit elements to signed 16-bit PCM range [-32768, 32767] (imm=15) */ +#define ESP_PIE_VCLAMP_S16_Q0_Q0() __asm__ volatile (".word 0x030c505b") /* esp.vclamp.s16 q0, q0, 15 */ +#define ESP_PIE_VCLAMP_S16_Q0_Q1() __asm__ volatile (".word 0x230c505b") /* esp.vclamp.s16 q0, q1, 15 */ +#define ESP_PIE_VCLAMP_S16_Q1_Q1() __asm__ volatile (".word 0x230c50db") /* esp.vclamp.s16 q1, q1, 15 */ + +/* Vector Interleave (Zip) and Deinterleave (Unzip) 16-bit elements */ +#define ESP_PIE_VZIP_16_Q0_Q1() __asm__ volatile (".word 0x0682005f") /* esp.vzip.16 q0, q1 */ +#define ESP_PIE_VUNZIP_16_Q0_Q1() __asm__ volatile (".word 0x0686005f") /* esp.vunzip.16 q0, q1 */ +#define ESP_PIE_VZIP_16_Q2_Q3() __asm__ volatile (".word 0x4e82005f") /* esp.vzip.16 q2, q3 */ +#define ESP_PIE_VUNZIP_16_Q2_Q3() __asm__ volatile (".word 0x4e86005f") /* esp.vunzip.16 q2, q3 */ + +/* Shift Amount Register (SAR) and CFG configuration */ +static inline void esp_pie_set_sar(uint32_t sar) +{ + register uint32_t sar_reg asm("a0") = sar; + __asm__ volatile (".word 0x90b1005f" : "+r"(sar_reg) : : "memory"); +} +#define ESP_PIE_SET_SAR(sar) esp_pie_set_sar(sar) +#define ESP_PIE_SET_SAR_A0() esp_pie_set_sar(15) +#define ESP_PIE_SET_SAR_A2() __asm__ volatile (".word 0x90b2005f" :: "r"(sar_a2)) /* esp.movx.w.sar a2 */ +#define ESP_PIE_SET_CFG_A0() __asm__ volatile (".word 0x90d1005f" :: "r"(cfg_a0)) /* esp.movx.w.cfg a0 */ + +/* Vector Right Shift 32-bit elements by SAR */ +#define ESP_PIE_VSR_S32_Q0_Q1() __asm__ volatile (".word 0x8404035b") /* esp.vsr.s32 q0, q1 */ +#define ESP_PIE_VSR_S32_Q0_Q0() __asm__ volatile (".word 0x8004035b") /* esp.vsr.s32 q0, q0 */ + +/* Vector Unzip & Truncate (Pack 32-bit lanes from q1, q2 into 16-bit lanes in q0) */ +#define ESP_PIE_VUNZIPT_16_Q0_Q1_Q2() __asm__ volatile (".word 0x06c04c5b") /* esp.vunzipt.16 q0, q1, q2 */ +#define ESP_PIE_VUNZIPT_16_Q0_Q0_Q1() __asm__ volatile (".word 0x01c04c5b") /* esp.vunzipt.16 q0, q0, q1 */ + +#endif /* CONFIG_SOC_SERIES_ESP32P4 || CONFIG_ESP32P4_PIE_SIMD */ + +#endif /* __SOF_MATH_ESP32P4_PIE_H__ */ diff --git a/src/include/sof/math/fir_generic.h b/src/include/sof/math/fir_generic.h index b1aab2b6392c..7729ee76f1b7 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) || SOF_USE_ARM_SIMD(FILTER) +#if SOF_USE_HIFI(NONE, FILTER) || SOF_USE_ARM_SIMD(FILTER) || SOF_USE_RISCV_SIMD(FILTER) #include #include diff --git a/src/include/sof/math/iir_df1.h b/src/include/sof/math/iir_df1.h index c6c66d1c8c59..d360676f2a79 100644 --- a/src/include/sof/math/iir_df1.h +++ b/src/include/sof/math/iir_df1.h @@ -84,6 +84,8 @@ int32_t iir_df1_4th(struct iir_state_df1 *iir, int32_t x); #include "iir_df1_hifi3.h" #elif SOF_USE_ARM_SIMD(FILTER) #include "iir_df1_arm.h" +#elif SOF_USE_RISCV_SIMD(FILTER) +#include "iir_df1_riscv.h" #else #include "iir_df1_generic.h" #endif diff --git a/src/include/sof/math/iir_df1_riscv.h b/src/include/sof/math/iir_df1_riscv.h new file mode 100644 index 000000000000..106df4e19709 --- /dev/null +++ b/src/include/sof/math/iir_df1_riscv.h @@ -0,0 +1,32 @@ +/* SPDX-License-Identifier: BSD-3-Clause + * + * Copyright(c) 2026 Sound Open Firmware. All rights reserved. + */ + +#ifndef __IIR_DF1_RISCV_H__ +#define __IIR_DF1_RISCV_H__ + +#include +#include + +static inline int16_t iir_df1_s16(struct iir_state_df1 *iir, int16_t x) +{ + return sat_int16(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 sat_int24(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 sat_int16(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 sat_int24(Q_SHIFT_RND(iir_df1(iir, x), 31, 23)); +} + +#endif /* __IIR_DF1_RISCV_H__ */ diff --git a/src/include/sof/math/iir_df2t.h b/src/include/sof/math/iir_df2t.h index b238b4f018d9..bc0fb2afad6e 100644 --- a/src/include/sof/math/iir_df2t.h +++ b/src/include/sof/math/iir_df2t.h @@ -53,6 +53,8 @@ int32_t iir_df2t(struct iir_state_df2t *iir, int32_t x); #include "iir_df2t_hifi3.h" #elif SOF_USE_ARM_SIMD(FILTER) #include "iir_df2t_arm.h" +#elif SOF_USE_RISCV_SIMD(FILTER) +#include "iir_df2t_riscv.h" #else #include "iir_df2t_generic.h" #endif diff --git a/src/include/sof/math/iir_df2t_riscv.h b/src/include/sof/math/iir_df2t_riscv.h new file mode 100644 index 000000000000..349623e4f765 --- /dev/null +++ b/src/include/sof/math/iir_df2t_riscv.h @@ -0,0 +1,32 @@ +/* SPDX-License-Identifier: BSD-3-Clause + * + * Copyright(c) 2026 Sound Open Firmware. All rights reserved. + */ + +#ifndef __IIR_DF2T_RISCV_H__ +#define __IIR_DF2T_RISCV_H__ + +#include +#include + +static inline int16_t iir_df2t_s16(struct iir_state_df2t *iir, int16_t x) +{ + return sat_int16(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 sat_int24(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 sat_int16(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 sat_int24(Q_SHIFT_RND(iir_df2t(iir, x), 31, 23)); +} + +#endif /* __IIR_DF2T_RISCV_H__ */ diff --git a/src/math/CMakeLists.txt b/src/math/CMakeLists.txt index 00052df7cf6b..cb47e8c3f01b 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_arm.c iir_df2t.c) + list(APPEND base_files iir_df2t_generic.c iir_df2t_hifi3.c iir_df2t_arm.c iir_df2t_riscv.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_arm.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_riscv.c iir_df1.c) endif() endif() diff --git a/src/math/Kconfig b/src/math/Kconfig index 81ad5984d09d..f962415e66d8 100644 --- a/src/math/Kconfig +++ b/src/math/Kconfig @@ -161,6 +161,7 @@ choice "FILTER_SIMD_LEVEL_SELECT" depends on COMP_FIR || COMP_IIR || COMP_TDFB default FILTER_HIFI_MAX if XTENSA default FILTER_ARM_SIMD if ARM + default FILTER_RISCV_SIMD if RISCV default FILTER_HIFI_NONE config FILTER_ARM_SIMD @@ -206,6 +207,13 @@ choice "FILTER_SIMD_LEVEL_SELECT" bool help This option used to build FILTER generic code. + + config FILTER_RISCV_SIMD + prompt "choose RISC-V SIMD optimized FILTER module" + bool + depends on RISCV + help + This option used to build RISC-V SIMD optimized FILTER code endchoice config MATH_FIR diff --git a/src/math/Kconfig.simd b/src/math/Kconfig.simd index 634b54219ca7..e3892940494f 100644 --- a/src/math/Kconfig.simd +++ b/src/math/Kconfig.simd @@ -9,6 +9,7 @@ choice "FILTER_SIMD_LEVEL_SELECT" depends on COMP_IIR default FILTER_HIFI_MAX if XTENSA default FILTER_ARM_SIMD if ARM + default FILTER_RISCV_SIMD if RISCV default FILTER_HIFI_NONE config FILTER_ARM_SIMD @@ -54,4 +55,11 @@ choice "FILTER_SIMD_LEVEL_SELECT" bool help This option used to build FILTER generic code. + + config FILTER_RISCV_SIMD + prompt "choose RISC-V SIMD optimized FILTER module" + bool + depends on RISCV + help + This option used to build RISC-V SIMD optimized FILTER code endchoice diff --git a/src/math/fir_generic.c b/src/math/fir_generic.c index 802242d4dbec..4d4e64698cba 100644 --- a/src/math/fir_generic.c +++ b/src/math/fir_generic.c @@ -9,7 +9,7 @@ #include #include -#if SOF_USE_HIFI(NONE, FILTER) +#if SOF_USE_HIFI(NONE, FILTER) || SOF_USE_RISCV_SIMD(FILTER) #include #include @@ -100,19 +100,33 @@ int32_t fir_32x16(struct fir_state_32x16 *fir, int32_t x) /* Part 1, loop n1 times */ n1 = MIN(n1, taps); - for (n = 0; n < n1; n++) { - y += (int64_t)(*coef) * (*data); - coef++; - data--; + int n1_2 = n1 & ~1; + for (n = 0; n < n1_2; n += 2) { + int16_t c0 = coef[0]; + int16_t c1 = coef[1]; + coef += 2; + y += (int64_t)c0 * data[0]; + y += (int64_t)c1 * data[-1]; + data -= 2; + } + for (; n < n1; n++) { + y += (int64_t)(*coef++) * (*data--); } /* Part 2, un-wrap data, continue n2 times */ n2 = taps - n1; data = &fir->delay[length - 1]; - for (n = 0; n < n2; n++) { - y += (int64_t)(*coef) * (*data); - coef++; - data--; + int n2_2 = n2 & ~1; + for (n = 0; n < n2_2; n += 2) { + int16_t c0 = coef[0]; + int16_t c1 = coef[1]; + coef += 2; + y += (int64_t)c0 * data[0]; + y += (int64_t)c1 * data[-1]; + data -= 2; + } + for (; n < n2; n++) { + y += (int64_t)(*coef++) * (*data--); } /* Q2.46 -> Q2.31, saturate to Q1.31 */ @@ -158,11 +172,27 @@ void fir_32x16_2x(struct fir_state_32x16 *fir, int32_t x0, int32_t x1, int32_t * /* Part 1, loop n1 times */ sample1 = x1; n1 = MIN(n1, taps); - for (i = 0; i < n1; i++) { - tap = *coef; - coef++; - sample0 = *data; - data--; + int n1_2x = n1 & ~1; + for (i = 0; i < n1_2x; i += 2) { + int16_t c0 = coef[0]; + int16_t c1 = coef[1]; + coef += 2; + + int32_t d0 = data[0]; + int32_t d1 = data[-1]; + data -= 2; + + a1 += (int64_t)c0 * sample1; + a0 += (int64_t)c0 * d0; + + a1 += (int64_t)c1 * d0; + a0 += (int64_t)c1 * d1; + + sample1 = d1; + } + for (; i < n1; i++) { + tap = *coef++; + sample0 = *data--; a1 += (int64_t)tap * sample1; a0 += (int64_t)tap * sample0; sample1 = sample0; @@ -171,11 +201,27 @@ void fir_32x16_2x(struct fir_state_32x16 *fir, int32_t x0, int32_t x1, int32_t * /* Part 2, un-wrap data, continue n2 times */ n2 = taps - n1; data = &fir->delay[length - 1]; - for (i = 0; i < n2; i++) { - tap = *coef; - coef++; - sample0 = *data; - data--; + int n2_2x = n2 & ~1; + for (i = 0; i < n2_2x; i += 2) { + int16_t c0 = coef[0]; + int16_t c1 = coef[1]; + coef += 2; + + int32_t d0 = data[0]; + int32_t d1 = data[-1]; + data -= 2; + + a1 += (int64_t)c0 * sample1; + a0 += (int64_t)c0 * d0; + + a1 += (int64_t)c1 * d0; + a0 += (int64_t)c1 * d1; + + sample1 = d1; + } + for (; i < n2; i++) { + tap = *coef++; + sample0 = *data--; a1 += (int64_t)tap * sample1; a0 += (int64_t)tap * sample0; sample1 = sample0; diff --git a/src/math/iir_df1_riscv.c b/src/math/iir_df1_riscv.c new file mode 100644 index 000000000000..ba5fb0037ac1 --- /dev/null +++ b/src/math/iir_df1_riscv.c @@ -0,0 +1,176 @@ +// SPDX-License-Identifier: BSD-3-Clause +// +// Copyright(c) 2026 Sound Open Firmware. All rights reserved. + +#include +#include +#include +#include +#include +#include +#include +#include + +#if SOF_USE_RISCV_SIMD(FILTER) + +#define ROUND_BIT(b) ((b) > 0 ? (1ULL << ((b) - 1)) : 0) + +static inline int32_t sat_clamp_q31(int64_t x) +{ + if (__builtin_expect(x >= (int64_t)INT32_MIN && x <= (int64_t)INT32_MAX, 1)) + return (int32_t)x; + return (x > INT32_MAX) ? INT32_MAX : INT32_MIN; +} + +/* Series DF1 IIR optimized for RISC-V */ +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 */ + acc = ((int64_t)a2) * y_n2; + acc += ((int64_t)a1) * y_n1; + acc += ((int64_t)b2) * x_n2; + acc += ((int64_t)b1) * x_n1; + acc += ((int64_t)b0) * in; + + /* Shift Q3.61 to Q3.31 with symmetric rounding */ + acc = (acc + (1LL << 29)) >> 30; + tmp = sat_clamp_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 = sat_clamp_q31(acc); + + c += SOF_EQ_IIR_NBIQUAD; + d += IIR_DF1_NUM_STATE; + } + out += in; + } + return sat_clamp_q31(out); +} +EXPORT_SYMBOL(iir_df1); + +/* Specialized 4th order (2 biquads in series) for RISC-V */ +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 = ((int64_t)a2) * y_n2; + acc += ((int64_t)a1) * y_n1; + acc += ((int64_t)b2) * x_n2; + acc += ((int64_t)b1) * x_n1; + acc += ((int64_t)b0) * in; + + acc = (acc + (1LL << 29)) >> 30; + tmp = sat_clamp_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 = sat_clamp_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 = ((int64_t)a2) * y_n2; + acc += ((int64_t)a1) * y_n1; + acc += ((int64_t)b2) * x_n2; + acc += ((int64_t)b1) * x_n1; + acc += ((int64_t)b0) * in; + + acc = (acc + (1LL << 29)) >> 30; + tmp = sat_clamp_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 = sat_clamp_q31(acc); + } + + return in; +} +EXPORT_SYMBOL(iir_df1_4th); + +#endif /* SOF_USE_RISCV_SIMD(FILTER) */ + diff --git a/src/math/iir_df2t_riscv.c b/src/math/iir_df2t_riscv.c new file mode 100644 index 000000000000..b1743bd40612 --- /dev/null +++ b/src/math/iir_df2t_riscv.c @@ -0,0 +1,84 @@ +// SPDX-License-Identifier: BSD-3-Clause +// +// Copyright(c) 2026 Sound Open Firmware. All rights reserved. + +#include +#include +#include +#include +#include +#include +#include +#include + +#if SOF_USE_RISCV_SIMD(FILTER) + +static inline int32_t sat_clamp64(int64_t x) +{ + if (x > INT32_MAX) + return INT32_MAX; + if (x < INT32_MIN) + return INT32_MIN; + return (int32_t)x; +} + +int32_t iir_df2t(struct iir_state_df2t *iir, int32_t x) +{ + int32_t in; + int32_t tmp; + int64_t acc; + int32_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 = ((int64_t)b0) * in + delay[d]; + acc = (acc + (1LL << 29)) >> 30; + tmp = sat_clamp64(acc); + + /* Compute first delay */ + acc = delay[d + 1]; + acc += ((int64_t)b1) * in; + acc += ((int64_t)a1) * tmp; + delay[d] = acc; + + /* Compute second delay */ + acc = ((int64_t)b2) * in; + acc += ((int64_t)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 = sat_clamp64(acc); + + c += SOF_EQ_IIR_NBIQUAD; + d += IIR_DF2T_NUM_DELAYS; + } + out = sat_clamp64((int64_t)out + in); + } + return out; +} +EXPORT_SYMBOL(iir_df2t); + +#endif /* SOF_USE_RISCV_SIMD(FILTER) */ diff --git a/src/math/iir_llext/CMakeLists.txt b/src/math/iir_llext/CMakeLists.txt index f6f2a7c328fa..6c4966d1226b 100644 --- a/src/math/iir_llext/CMakeLists.txt +++ b/src/math/iir_llext/CMakeLists.txt @@ -3,13 +3,13 @@ if(CONFIG_MATH_IIR_DF1) set(df1 ../iir_df1.c ../iir_df1_generic.c ../iir_df1_hifi3.c - ../iir_df1_hifi4.c ../iir_df1_hifi5.c) + ../iir_df1_hifi4.c ../iir_df1_hifi5.c ../iir_df1_riscv.c) else() set(df1 "") endif() if(CONFIG_MATH_IIR_DF2T) - set(df2t ../iir_df2t.c ../iir_df2t_generic.c ../iir_df2t_hifi3.c) + set(df2t ../iir_df2t.c ../iir_df2t_generic.c ../iir_df2t_hifi3.c ../iir_df2t_riscv.c) else() set(df2t "") endif() diff --git a/src/platform/Kconfig b/src/platform/Kconfig index b2576dd2fb21..cc86bb74c197 100644 --- a/src/platform/Kconfig +++ b/src/platform/Kconfig @@ -353,6 +353,14 @@ config MT8196 endchoice +config ESP32P4_PIE_SIMD + bool "Enable ESP32-P4 PIE (Xespv2p2) 128-bit vector SIMD acceleration" + default y + depends on PLATFORM_ESP32P4 || SOC_SERIES_ESP32P4 + help + Enable ESP32-P4 custom Processor Instruction Extensions (PIE / Xespv2p2) + 128-bit vector SIMD engine for audio processing kernels (volume, mixer, etc.). + # # For non-Zephyr builds like testbench, cmocka and SOF ALSA plugin, # set core count separately.