From 5e384d7b68d69138b35d6876bbd8664571ff860d Mon Sep 17 00:00:00 2001 From: Seppo Ingalsuo Date: Thu, 15 Jan 2026 22:31:14 +0200 Subject: [PATCH 1/8] Math: Rename square root function sqrt_int16() to sofm_sqrt_int16() The rename is done to avoid possible conflict with other math libraries. The change is done to prepare add of 32 bit square root function. Signed-off-by: Seppo Ingalsuo --- src/audio/tdfb/tdfb_direction.c | 2 +- src/include/sof/math/sqrt.h | 15 ++++++++++----- src/math/dct.c | 2 +- src/math/sqrt_int16.c | 4 ++-- test/cmocka/src/math/arithmetic/square_root.c | 2 +- 5 files changed, 15 insertions(+), 10 deletions(-) diff --git a/src/audio/tdfb/tdfb_direction.c b/src/audio/tdfb/tdfb_direction.c index f83337c9caa4..71b544003ee7 100644 --- a/src/audio/tdfb/tdfb_direction.c +++ b/src/audio/tdfb/tdfb_direction.c @@ -108,7 +108,7 @@ static inline int16_t tdfb_mic_distance_sqrt(int32_t x) xs = Q_SHIFT_RND(x, 24, 12); xs = MIN(xs, UINT16_MAX); - return sqrt_int16((uint16_t)xs); + return sofm_sqrt_int16((uint16_t)xs); } static int16_t max_mic_distance(struct tdfb_comp_data *cd) diff --git a/src/include/sof/math/sqrt.h b/src/include/sof/math/sqrt.h index 1cdb82afe9a0..b26f594349bf 100644 --- a/src/include/sof/math/sqrt.h +++ b/src/include/sof/math/sqrt.h @@ -1,15 +1,20 @@ /* SPDX-License-Identifier: BSD-3-Clause * - * Copyright(c) 2021 Intel Corporation. All rights reserved. + * Copyright(c) 2021-2026 Intel Corporation. * * Author: Shriram Shastry * */ -#ifndef __SOF_MATH__SQRTLOOKUP__H -#define __SOF_MATH__SQRTLOOKUP__H +#ifndef __SOF_MATH_SQRT_H__ +#define __SOF_MATH_SQRT_H__ #include -uint16_t sqrt_int16(uint16_t u); -#endif +/** + * sofm_sqrt_int16() - Calculate 16-bit fractional square root function. + * @param u Input value in Q4.12 format, from 0 to 16.0. + * @return Calculated square root of n in Q4.12 format, from 0 to 4.0. + */ +uint16_t sofm_sqrt_int16(uint16_t u); +#endif /* __SOF_MATH_SQRT_H__ */ diff --git a/src/math/dct.c b/src/math/dct.c index de27cab4471e..e4bbe5623dd8 100644 --- a/src/math/dct.c +++ b/src/math/dct.c @@ -58,7 +58,7 @@ int mod_dct_initialize_16(struct processing_module *mod, struct dct_plan_16 *dct c1 = PI_Q29 / dct->num_in; arg = Q_SHIFT_RND(TWO_Q29 / dct->num_in, 29, 12); - c2 = sqrt_int16(arg); /* Q4.12 */ + c2 = sofm_sqrt_int16(arg); /* Q4.12 */ for (n = 0; n < dct->num_in; n++) { for (k = 0; k < dct->num_out; k++) { /* Note: Current int16_t nk works up to DCT_MATRIX_SIZE_MAX = 91 */ diff --git a/src/math/sqrt_int16.c b/src/math/sqrt_int16.c index 9068717d638d..81772fc90c3c 100644 --- a/src/math/sqrt_int16.c +++ b/src/math/sqrt_int16.c @@ -29,7 +29,7 @@ * Arguments : uint16_t u * Return Type : int32_t */ -uint16_t sqrt_int16(uint16_t u) +uint16_t sofm_sqrt_int16(uint16_t u) { static const int32_t iv1[193] = { 46341, 46702, 47059, 47415, 47767, 48117, 48465, 48809, 49152, 49492, 49830, 50166, @@ -146,4 +146,4 @@ uint16_t sqrt_int16(uint16_t u) return y; } -EXPORT_SYMBOL(sqrt_int16); +EXPORT_SYMBOL(sofm_sqrt_int16); diff --git a/test/cmocka/src/math/arithmetic/square_root.c b/test/cmocka/src/math/arithmetic/square_root.c index f6c03c9c42cf..6a31588c1235 100644 --- a/test/cmocka/src/math/arithmetic/square_root.c +++ b/test/cmocka/src/math/arithmetic/square_root.c @@ -139,7 +139,7 @@ static void test_math_arithmetic_sqrt_fixed(void **state) memcpy_s((void *)&u[0], sizeof(u), (void *)&uv[0], 252U * sizeof(uint32_t)); for (i = 0; i < ARRAY_SIZE(sqrt_ref_table); i++) { - y = Q_CONVERT_QTOF(sqrt_int16(u[i]), 12); + y = Q_CONVERT_QTOF(sofm_sqrt_int16(u[i]), 12); diff = fabs(sqrt_ref_table[i] - y); if (diff > CMP_TOLERANCE) { From 0334de49bd5d5e56e3230e6e6b0db2640b307ac2 Mon Sep 17 00:00:00 2001 From: Seppo Ingalsuo Date: Mon, 26 Jan 2026 12:32:08 +0200 Subject: [PATCH 2/8] Math: Add 32-bit square root function sofm_sqrt_int32() This patch adds a higher precision 32-bit fractional integer square root function to SOF math library. The algorithm uses a lookup table for initial value and two iterations with Newton-Raphson method to improve the accuracy. Both input and output format is Q2.30. The format was chosen to match complex to polar conversions numbers range for Q1.31 complex values. Signed-off-by: Seppo Ingalsuo --- src/include/sof/math/sqrt.h | 12 +++++++ src/math/CMakeLists.txt | 2 +- src/math/sqrt_int32.c | 64 +++++++++++++++++++++++++++++++++++++ 3 files changed, 77 insertions(+), 1 deletion(-) create mode 100644 src/math/sqrt_int32.c diff --git a/src/include/sof/math/sqrt.h b/src/include/sof/math/sqrt.h index b26f594349bf..3d8fd0df72fe 100644 --- a/src/include/sof/math/sqrt.h +++ b/src/include/sof/math/sqrt.h @@ -17,4 +17,16 @@ * @return Calculated square root of n in Q4.12 format, from 0 to 4.0. */ uint16_t sofm_sqrt_int16(uint16_t u); + +/** + * sofm_sqrt_int32() - Calculate 32-bit fractional square root function. + * @param n Input value in Q2.30 format, from 0 to 2.0. + * @return Calculated square root of n in Q2.30 format. + * + * The input range of square root function is matched with Q1.31 + * complex numbers range where the magnitude squared can be to 2.0. + * The function returns zero for non-positive input values. + */ +int32_t sofm_sqrt_int32(int32_t n); + #endif /* __SOF_MATH_SQRT_H__ */ diff --git a/src/math/CMakeLists.txt b/src/math/CMakeLists.txt index 6afd2c342fe0..fb23b32b61a5 100644 --- a/src/math/CMakeLists.txt +++ b/src/math/CMakeLists.txt @@ -16,7 +16,7 @@ if(CONFIG_MATH_LUT_SINE_FIXED) endif() if(CONFIG_SQRT_FIXED) - list(APPEND base_files sqrt_int16.c) + list(APPEND base_files sqrt_int16.c sqrt_int32.c) endif() if(CONFIG_MATH_EXP) diff --git a/src/math/sqrt_int32.c b/src/math/sqrt_int32.c new file mode 100644 index 000000000000..6d412b33f33a --- /dev/null +++ b/src/math/sqrt_int32.c @@ -0,0 +1,64 @@ +// SPDX-License-Identifier: BSD-3-Clause +// +// Copyright(c) 2026 Intel Corporation. +// +// Author: Seppo Ingalsuo +// + +#include +#include +#include + +/* Lookup table for square root for initial value in iteration, + * created with Octave commands: + * + * arg=((1:64) * 2^25) / 2^30; lut = int32(sqrt(arg) * 2^30); + * fmt=['static const int32_t sqrt_int32_lut[] = {' repmat(' %d,',1, numel(lut)-1) ' %d};\n']; + * fprintf(fmt, lut) + */ +static const int32_t sqrt_int32_lut[] = { + 189812531, 268435456, 328764948, 379625062, 424433723, 464943848, 502196753, + 536870912, 569437594, 600239927, 629536947, 657529896, 684378814, 710213460, + 735140772, 759250125, 782617115, 805306368, 827373642, 848867446, 869830292, + 890299688, 910308921, 929887697, 949062656, 967857801, 986294844, 1004393507, + 1022171763, 1039646051, 1056831447, 1073741824, 1090389977, 1106787739, 1122946079, + 1138875187, 1154584553, 1170083026, 1185378878, 1200479854, 1215393219, 1230125796, + 1244684005, 1259073893, 1273301169, 1287371222, 1301289153, 1315059792, 1328687719, + 1342177280, 1355532607, 1368757628, 1381856086, 1394831545, 1407687407, 1420426919, + 1433053185, 1445569171, 1457977717, 1470281545, 1482483261, 1494585366, 1506590260, + 1518500250 +}; + +/* sofm_sqrt_int32() - Calculate 32-bit fractional square root function. */ +int32_t sofm_sqrt_int32(int32_t n) +{ + uint64_t n_shifted; + uint32_t x; + int shift; + + if (n < 1) + return 0; + + /* Scale input argument with 2^n, where n is even. + * Scale calculated sqrt() with 2^(-n/2). + */ + shift = (__builtin_clz(n) - 1) & ~1; /* Make even by clearing LSB */ + n = n << shift; + shift >>= 1; /* Divide by 2 for sqrt shift compensation */ + + /* For Q2.30 divide */ + n_shifted = (uint64_t)n << 30; + + /* Get initial guess from LUT, idx = 0 .. 63 */ + x = sqrt_int32_lut[n >> 25]; + + /* Iterate x(n+1) = 1/2 * (x(n) + N / x(n)) + * N is argument for square root + * x(n) is initial guess. Do two iterations. + */ + x = (uint32_t)(((n_shifted / x + x) + 1) >> 1); + x = (uint32_t)(((n_shifted / x + x) + 1) >> 1); + + return (int32_t)(x >> shift); +} +EXPORT_SYMBOL(sofm_sqrt_int32); From d7aeb5727a0578dc43e332526ae3244551814d81 Mon Sep 17 00:00:00 2001 From: Seppo Ingalsuo Date: Mon, 26 Jan 2026 14:19:57 +0200 Subject: [PATCH 3/8] Math: Separate icomplex16 and icomplex32 structs from FFT This patch helps with more generic use of complex numbers not directly related to the FFTs domain. It prepares to add polar complex numbers format that is commonly used in frequency domain signal processing. Signed-off-by: Seppo Ingalsuo --- src/audio/mfcc/mfcc_generic.c | 2 + src/audio/mfcc/mfcc_hifi4.c | 2 + src/audio/mfcc/mfcc_setup.c | 2 + src/include/sof/math/fft.h | 15 +--- src/include/sof/math/icomplex16.h | 84 +++++++++++++++++++ .../sof/math/icomplex32.h} | 22 ++++- src/math/auditory/mel_filterbank_16.c | 2 +- src/math/auditory/mel_filterbank_32.c | 2 +- src/math/fft/fft_16.c | 57 +------------ src/math/fft/fft_16_hifi3.c | 2 + src/math/fft/fft_32.c | 10 +-- src/math/fft/fft_multi.c | 2 +- test/cmocka/src/math/auditory/auditory.c | 2 + test/cmocka/src/math/fft/fft.c | 2 + test/cmocka/src/math/fft/fft_multi.c | 2 + 15 files changed, 129 insertions(+), 79 deletions(-) create mode 100644 src/include/sof/math/icomplex16.h rename src/{math/fft/fft_32.h => include/sof/math/icomplex32.h} (80%) diff --git a/src/audio/mfcc/mfcc_generic.c b/src/audio/mfcc/mfcc_generic.c index 9384b16e0cee..ecc95474326b 100644 --- a/src/audio/mfcc/mfcc_generic.c +++ b/src/audio/mfcc/mfcc_generic.c @@ -10,6 +10,8 @@ #include #include #include +#include +#include #include #include #include diff --git a/src/audio/mfcc/mfcc_hifi4.c b/src/audio/mfcc/mfcc_hifi4.c index d033dfe36124..60a4de62ec23 100644 --- a/src/audio/mfcc/mfcc_hifi4.c +++ b/src/audio/mfcc/mfcc_hifi4.c @@ -11,6 +11,8 @@ #include #include #include +#include +#include #include #include #include diff --git a/src/audio/mfcc/mfcc_setup.c b/src/audio/mfcc/mfcc_setup.c index 642b20df66f5..dded450673ad 100644 --- a/src/audio/mfcc/mfcc_setup.c +++ b/src/audio/mfcc/mfcc_setup.c @@ -8,6 +8,8 @@ #include #include #include +#include +#include #include #include #include diff --git a/src/include/sof/math/fft.h b/src/include/sof/math/fft.h index 033ea4527eb3..4570467478a6 100644 --- a/src/include/sof/math/fft.h +++ b/src/include/sof/math/fft.h @@ -11,6 +11,7 @@ #include #include +#include #include #include #include @@ -30,20 +31,6 @@ #define FFT_SIZE_MAX 1024 #define FFT_MULTI_COUNT_MAX 3 -struct icomplex32 { - int32_t real; - int32_t imag; -}; - -/* Note: the add of packed attribute to icmplex16 would significantly increase - * processing time of fft_execute_16() so it is not done. The optimized versions of - * FFT for HiFi will need a different packed data structure vs. generic C. - */ -struct icomplex16 { - int16_t real; - int16_t imag; -}; - struct fft_plan { uint32_t size; /* fft size */ uint32_t len; /* fft length in exponent of 2 */ diff --git a/src/include/sof/math/icomplex16.h b/src/include/sof/math/icomplex16.h new file mode 100644 index 000000000000..a2ade94660ba --- /dev/null +++ b/src/include/sof/math/icomplex16.h @@ -0,0 +1,84 @@ +// SPDX-License-Identifier: BSD-3-Clause +// +// Copyright(c) 2020-2026 Intel Corporation. +// +// Author: Amery Song +// Keyon Jie + +#include +#include +#include + +#ifndef __SOF_ICOMPLEX16_H__ +#define __SOF_ICOMPLEX16_H__ + +/* Note: the add of packed attribute to icmplex16 would significantly increase + * processing time of fft_execute_16() so it is not done. The optimized versions of + * FFT for HiFi will need a different packed data structure vs. generic C. + * + * TODO: Use with care with other than 16-bit FFT internals. Access with intrinsics + * will requires packed and aligned data. Currently there is no such usage in SOF. + */ + +/** + * struct icomplex16 - Storage for a normal complex number. + * @param real The real part in Q1.15 fractional format. + * @param imag The imaginary part in Q1.15 fractional format. + */ +struct icomplex16 { + int16_t real; + int16_t imag; +}; + +/* + * Helpers for 16 bit FFT calculation + */ +static inline void icomplex16_add(const struct icomplex16 *in1, const struct icomplex16 *in2, + struct icomplex16 *out) +{ + out->real = in1->real + in2->real; + out->imag = in1->imag + in2->imag; +} + +static inline void icomplex16_sub(const struct icomplex16 *in1, const struct icomplex16 *in2, + struct icomplex16 *out) +{ + out->real = in1->real - in2->real; + out->imag = in1->imag - in2->imag; +} + +static inline void icomplex16_mul(const struct icomplex16 *in1, const struct icomplex16 *in2, + struct icomplex16 *out) +{ + int32_t real = (int32_t)in1->real * in2->real - (int32_t)in1->imag * in2->imag; + int32_t imag = (int32_t)in1->real * in2->imag + (int32_t)in1->imag * in2->real; + + out->real = Q_SHIFT_RND(real, 30, 15); + out->imag = Q_SHIFT_RND(imag, 30, 15); +} + +/* complex conjugate */ +static inline void icomplex16_conj(struct icomplex16 *comp) +{ + comp->imag = sat_int16(-((int32_t)comp->imag)); +} + +/* shift a complex n bits, n > 0: left shift, n < 0: right shift */ +static inline void icomplex16_shift(const struct icomplex16 *input, int16_t n, + struct icomplex16 *output) +{ + int n1, n2; + + if (n >= 0) { + /* need saturation handling */ + output->real = sat_int16((int32_t)input->real << n); + output->imag = sat_int16((int32_t)input->imag << n); + } else { + n1 = -n; + n2 = 1 << (n1 - 1); + output->real = sat_int16(((int32_t)input->real + n2) >> n1); + output->imag = sat_int16(((int32_t)input->imag + n2) >> n1); + } +} + +#endif /* __SOF_ICOMPLEX16_H__ */ diff --git a/src/math/fft/fft_32.h b/src/include/sof/math/icomplex32.h similarity index 80% rename from src/math/fft/fft_32.h rename to src/include/sof/math/icomplex32.h index 92a3131b5189..84aa7b437b78 100644 --- a/src/math/fft/fft_32.h +++ b/src/include/sof/math/icomplex32.h @@ -1,13 +1,29 @@ // SPDX-License-Identifier: BSD-3-Clause // -// Copyright(c) 2020-2025 Intel Corporation. All rights reserved. +// Copyright(c) 2020-2026 Intel Corporation. // // Author: Amery Song // Keyon Jie #include -#include +#include +#include +#include #include +#include + +#ifndef __SOF_ICOMPLEX32_H__ +#define __SOF_ICOMPLEX32_H__ + +/** + * struct icomplex32 - Storage for a normal complex number. + * @param real The real part in Q1.31 fractional format. + * @param imag The imaginary part in Q1.31 fractional format. + */ +struct icomplex32 { + int32_t real; + int32_t imag; +}; /* * These helpers are optimized for FFT calculation only. @@ -62,3 +78,5 @@ static inline void icomplex32_shift(const struct icomplex32 *input, int32_t n, output->imag = input->imag >> -n; } } + +#endif /* __SOF_ICOMPLEX32_H__ */ diff --git a/src/math/auditory/mel_filterbank_16.c b/src/math/auditory/mel_filterbank_16.c index 2ed62c319d38..f5a18f052b77 100644 --- a/src/math/auditory/mel_filterbank_16.c +++ b/src/math/auditory/mel_filterbank_16.c @@ -6,7 +6,7 @@ #include #include -#include +#include #include #include #include diff --git a/src/math/auditory/mel_filterbank_32.c b/src/math/auditory/mel_filterbank_32.c index 69b781817870..a80d09ad624a 100644 --- a/src/math/auditory/mel_filterbank_32.c +++ b/src/math/auditory/mel_filterbank_32.c @@ -6,7 +6,7 @@ #include #include -#include +#include #include #include #include diff --git a/src/math/fft/fft_16.c b/src/math/fft/fft_16.c index 7af6c1eb2768..7a8ffad26054 100644 --- a/src/math/fft/fft_16.c +++ b/src/math/fft/fft_16.c @@ -1,68 +1,19 @@ // SPDX-License-Identifier: BSD-3-Clause // -// Copyright(c) 2020 Intel Corporation. All rights reserved. +// Copyright(c) 2020-2026 Intel Corporation. // // Author: Amery Song // Keyon Jie #include -#include #include +#include +#include +#include #ifdef FFT_GENERIC #include -/* - * Helpers for 16 bit FFT calculation - */ -static inline void icomplex16_add(const struct icomplex16 *in1, const struct icomplex16 *in2, - struct icomplex16 *out) -{ - out->real = in1->real + in2->real; - out->imag = in1->imag + in2->imag; -} - -static inline void icomplex16_sub(const struct icomplex16 *in1, const struct icomplex16 *in2, - struct icomplex16 *out) -{ - out->real = in1->real - in2->real; - out->imag = in1->imag - in2->imag; -} - -static inline void icomplex16_mul(const struct icomplex16 *in1, const struct icomplex16 *in2, - struct icomplex16 *out) -{ - int32_t real = (int32_t)in1->real * in2->real - (int32_t)in1->imag * in2->imag; - int32_t imag = (int32_t)in1->real * in2->imag + (int32_t)in1->imag * in2->real; - - out->real = Q_SHIFT_RND(real, 30, 15); - out->imag = Q_SHIFT_RND(imag, 30, 15); -} - -/* complex conjugate */ -static inline void icomplex16_conj(struct icomplex16 *comp) -{ - comp->imag = sat_int16(-((int32_t)comp->imag)); -} - -/* shift a complex n bits, n > 0: left shift, n < 0: right shift */ -static inline void icomplex16_shift(const struct icomplex16 *input, int16_t n, - struct icomplex16 *output) -{ - int n1, n2; - - if (n >= 0) { - /* need saturation handling */ - output->real = sat_int16((int32_t)input->real << n); - output->imag = sat_int16((int32_t)input->imag << n); - } else { - n1 = -n; - n2 = 1 << (n1 - 1); - output->real = sat_int16(((int32_t)input->real + n2) >> n1); - output->imag = sat_int16(((int32_t)input->imag + n2) >> n1); - } -} - /** * \brief Execute the 16-bits Fast Fourier Transform (FFT) or Inverse FFT (IFFT) * For the configured fft_pan. diff --git a/src/math/fft/fft_16_hifi3.c b/src/math/fft/fft_16_hifi3.c index 54e9cebc4a23..318fb0d1a216 100644 --- a/src/math/fft/fft_16_hifi3.c +++ b/src/math/fft/fft_16_hifi3.c @@ -7,6 +7,8 @@ #include #include #include +#include + #ifdef FFT_HIFI3 #include diff --git a/src/math/fft/fft_32.c b/src/math/fft/fft_32.c index 25dc71f57e2e..a6f8b2aa1ab3 100644 --- a/src/math/fft/fft_32.c +++ b/src/math/fft/fft_32.c @@ -6,18 +6,14 @@ // Keyon Jie #include -#include -#include #include - -#include +#include +#include +#include #ifdef FFT_GENERIC #include -#include "fft_32.h" - - /** * \brief Execute the 32-bits Fast Fourier Transform (FFT) or Inverse FFT (IFFT) * For the configured fft_pan. diff --git a/src/math/fft/fft_multi.c b/src/math/fft/fft_multi.c index baa404315380..2b01f1632478 100644 --- a/src/math/fft/fft_multi.c +++ b/src/math/fft/fft_multi.c @@ -7,13 +7,13 @@ #include #include #include +#include #include #include #include #include #include #include "fft_common.h" -#include "fft_32.h" LOG_MODULE_REGISTER(math_fft_multi, CONFIG_SOF_LOG_LEVEL); SOF_DEFINE_REG_UUID(math_fft_multi); diff --git a/test/cmocka/src/math/auditory/auditory.c b/test/cmocka/src/math/auditory/auditory.c index 2f8df53e8c6a..dc05c387cfae 100644 --- a/test/cmocka/src/math/auditory/auditory.c +++ b/test/cmocka/src/math/auditory/auditory.c @@ -16,6 +16,8 @@ #include #include #include +#include +#include #include #include "ref_hz_to_mel.h" #include "ref_mel_filterbank_16_test1.h" diff --git a/test/cmocka/src/math/fft/fft.c b/test/cmocka/src/math/fft/fft.c index abd8977d2d62..dc3dc8e70524 100644 --- a/test/cmocka/src/math/fft/fft.c +++ b/test/cmocka/src/math/fft/fft.c @@ -17,6 +17,8 @@ #include #include #include +#include +#include #include #include "input.h" diff --git a/test/cmocka/src/math/fft/fft_multi.c b/test/cmocka/src/math/fft/fft_multi.c index 6928af5807f3..bf5bd49efb08 100644 --- a/test/cmocka/src/math/fft/fft_multi.c +++ b/test/cmocka/src/math/fft/fft_multi.c @@ -5,6 +5,8 @@ // Author: Seppo Ingalsuo #include +#include +#include #include #include "ref_fft_multi_96_32.h" #include "ref_fft_multi_512_32.h" From ed7859f1399e5a25f032edfc94764e0a2880b422 Mon Sep 17 00:00:00 2001 From: Seppo Ingalsuo Date: Mon, 26 Jan 2026 14:42:37 +0200 Subject: [PATCH 4/8] Math: Add functions to/from polar format for complex numbers This patch adds functions sofm_icomplex32_to_polar() and sofm_ipolar32_to_complex(). In polar format the Q1.31 (real, imag) numbers pair is converted to (magnitude, angle). The magnitude is Q2.30 format and angle in -pi to +pi radians in Q3.29 format. The conversion to polar and back loses some quality so there currently is no support for icomplex16. Signed-off-by: Seppo Ingalsuo --- src/include/sof/math/icomplex32.h | 31 +++++++++++++++++ src/math/CMakeLists.txt | 4 +++ src/math/Kconfig | 8 +++++ src/math/complex.c | 57 +++++++++++++++++++++++++++++++ 4 files changed, 100 insertions(+) create mode 100644 src/math/complex.c diff --git a/src/include/sof/math/icomplex32.h b/src/include/sof/math/icomplex32.h index 84aa7b437b78..29d22daadab8 100644 --- a/src/include/sof/math/icomplex32.h +++ b/src/include/sof/math/icomplex32.h @@ -25,6 +25,16 @@ struct icomplex32 { int32_t imag; }; +/** + * struct ipolar32 - Storage for complex number in polar format. + * @param magnitude The length of vector in Q2.30 format. + * @param angle The phase angle of the vector -pi to +pi in Q3.29 format. + */ +struct ipolar32 { + int32_t magnitude; + int32_t angle; +}; + /* * These helpers are optimized for FFT calculation only. * e.g. _add/sub() assume the output won't be saturate so no check needed, @@ -79,4 +89,25 @@ static inline void icomplex32_shift(const struct icomplex32 *input, int32_t n, } } +/** + * sofm_icomplex32_to_polar() - Convert (re, im) complex number to polar. + * @param complex Pointer to input complex number in Q1.31 format. + * @param polar Pointer to output complex number in Q2.30 format for + * magnitude and Q3.29 for phase angle. + * + * The function can be used to convert data in-place with same address for + * input and output. It can be useful to save scratch memory. + */ +void sofm_icomplex32_to_polar(struct icomplex32 *complex, struct ipolar32 *polar); + +/** + * sofm_ipolar32_to_complex() - Convert complex number from polar to normal (re, im) format. + * @param polar Pointer to input complex number in polar format. + * @param complex Pointer to output complex number in normal format in Q1.31. + * + * This function can be used to convert data in-place with same address for input + * and output. It can be useful to save scratch memory. + */ +void sofm_ipolar32_to_complex(struct ipolar32 *polar, struct icomplex32 *complex); + #endif /* __SOF_ICOMPLEX32_H__ */ diff --git a/src/math/CMakeLists.txt b/src/math/CMakeLists.txt index fb23b32b61a5..c3dcfae0f070 100644 --- a/src/math/CMakeLists.txt +++ b/src/math/CMakeLists.txt @@ -91,6 +91,10 @@ if(CONFIG_MATH_MU_LAW_CODEC) list(APPEND base_files mu_law.c) endif() +if(CONFIG_MATH_COMPLEX) + list(APPEND base_files complex.c) +endif() + is_zephyr(zephyr) if(zephyr) ### Zephyr ### diff --git a/src/math/Kconfig b/src/math/Kconfig index a513f88799fc..8d5647a159be 100644 --- a/src/math/Kconfig +++ b/src/math/Kconfig @@ -95,9 +95,17 @@ config MATH_DECIBELS Select this to enable db2lin_fixed() and exp_fixed() functions. +config MATH_COMPLEX + bool "Operations for complex numbers" + default n + help + Select this to enable functions for complex numbers + arithmetic such as conversions to/from polar format. + config MATH_FFT bool "FFT library" default n + select MATH_COMPLEX help Enable Fast Fourier Transform library, this should not be selected directly, please select it from other audio components where need it. diff --git a/src/math/complex.c b/src/math/complex.c new file mode 100644 index 000000000000..cdc5f7fd1d58 --- /dev/null +++ b/src/math/complex.c @@ -0,0 +1,57 @@ +// SPDX-License-Identifier: BSD-3-Clause +// +// Copyright(c) 2026 Intel Corporation. +// +// Author: Seppo Ingalsuo +// + +#include +#include +#include +#include +#include + +/* sofm_icomplex32_to_polar() - Convert (re, im) complex number to polar. */ +void sofm_icomplex32_to_polar(struct icomplex32 *complex, struct ipolar32 *polar) +{ + struct icomplex32 c = *complex; + int64_t squares_sum; + int32_t sqrt_arg; + int32_t acos_arg; + int32_t acos_val; + + /* Calculate square of magnitudes Q1.31, result is Q2.62 */ + squares_sum = (int64_t)c.real * c.real + (int64_t)c.imag * c.imag; + + /* Square root */ + sqrt_arg = Q_SHIFT_RND(squares_sum, 62, 30); + polar->magnitude = sofm_sqrt_int32(sqrt_arg); /* Q2.30 */ + + /* Avoid divide by zero and ambiguous angle for a zero vector. */ + if (polar->magnitude == 0) { + polar->angle = 0; + return; + } + + /* Calculate phase angle with acos( complex->real / polar->magnitude) */ + acos_arg = sat_int32((((int64_t)c.real) << 29) / polar->magnitude); /* Q2.30 */ + acos_val = acos_fixed_32b(acos_arg); /* Q3.29 */ + polar->angle = (c.imag < 0) ? -acos_val : acos_val; +} +EXPORT_SYMBOL(sofm_icomplex32_to_polar); + +/* sofm_ipolar32_to_complex() - Convert complex number from polar to normal (re, im) format. */ +void sofm_ipolar32_to_complex(struct ipolar32 *polar, struct icomplex32 *complex) +{ + struct cordic_cmpx cexp; + int32_t phase; + int32_t magnitude; + + /* The conversion can happen in-place, so load copies of the values first */ + magnitude = polar->magnitude; + phase = Q_SHIFT_RND(polar->angle, 29, 28); /* Q3.29 to Q2.28 */ + cmpx_exp_32b(phase, &cexp); /* Q2.30 */ + complex->real = sat_int32(Q_MULTSR_32X32((int64_t)magnitude, cexp.re, 30, 30, 31)); + complex->imag = sat_int32(Q_MULTSR_32X32((int64_t)magnitude, cexp.im, 30, 30, 31)); +} +EXPORT_SYMBOL(sofm_ipolar32_to_complex); From 86887b96c78034a77697d732c9bb9cb2f88b7a9d Mon Sep 17 00:00:00 2001 From: Seppo Ingalsuo Date: Tue, 10 Feb 2026 20:14:26 +0200 Subject: [PATCH 5/8] Tools: Testbench: Increase copies timeout value The testbench quits after three file module copies without data written. The value is too low for components those accumulate more than one LL period of data before producing output or can't output at every copy. The value 10 should better ensure that testbench run is not ended too early. Currently testbench lacks the DP scheduler so, so the modules those are designed for DP can be run with this workaround. Signed-off-by: Seppo Ingalsuo --- tools/testbench/include/testbench/file.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tools/testbench/include/testbench/file.h b/tools/testbench/include/testbench/file.h index c9552c5a4425..dd1735026564 100644 --- a/tools/testbench/include/testbench/file.h +++ b/tools/testbench/include/testbench/file.h @@ -1,6 +1,6 @@ /* SPDX-License-Identifier: BSD-3-Clause * - * Copyright(c) 2018 Intel Corporation. All rights reserved. + * Copyright(c) 2018-2026 Intel Corporation. * * Author: Seppo Ingalsuo * Liam Girdwood @@ -13,7 +13,7 @@ #include -#define FILE_MAX_COPIES_TIMEOUT 3 +#define FILE_MAX_COPIES_TIMEOUT 16 /**< Convert with right shift a bytes count to samples count */ #define FILE_BYTES_TO_S16_SAMPLES(s) ((s) >> 1) From 38002d12760486d3b2756fe2fe3ea4bfe8bea90a Mon Sep 17 00:00:00 2001 From: Seppo Ingalsuo Date: Fri, 30 Jan 2026 16:45:31 +0200 Subject: [PATCH 6/8] Tools: Topology: Phase Vocoder: Bench topology to test the component Signed-off-by: Seppo Ingalsuo --- .../topology2/cavs-benchmark-hda.conf | 17 +++ .../topology2/cavs-benchmark-sdw.conf | 17 +++ .../development/tplg-targets-bench.cmake | 2 + .../one_input_output_format_s32_dp128.conf | 19 +++ .../one_input_output_format_s32_dp192.conf | 19 +++ .../one_input_output_format_s32_dp256.conf | 19 +++ .../one_input_output_format_s32_dp_5ms.conf | 22 ++++ .../bench/phase_vocoder_controls_capture.conf | 17 +++ .../phase_vocoder_controls_playback.conf | 17 +++ .../include/bench/phase_vocoder_route.conf | 19 +++ .../include/bench/phase_vocoder_s16.conf | 13 ++ .../include/bench/phase_vocoder_s24.conf | 13 ++ .../include/bench/phase_vocoder_s32.conf | 21 +++ .../include/components/phase_vocoder.conf | 122 ++++++++++++++++++ .../phase_vocoder/hann_1024_256.conf | 17 +++ .../phase_vocoder/hann_1024_256_mono.conf | 17 +++ .../phase_vocoder/hann_256_128.conf | 17 +++ .../phase_vocoder/hann_512_128.conf | 17 +++ .../phase_vocoder/hann_512_256.conf | 17 +++ 19 files changed, 422 insertions(+) create mode 100644 tools/topology/topology2/include/bench/one_input_output_format_s32_dp128.conf create mode 100644 tools/topology/topology2/include/bench/one_input_output_format_s32_dp192.conf create mode 100644 tools/topology/topology2/include/bench/one_input_output_format_s32_dp256.conf create mode 100644 tools/topology/topology2/include/bench/one_input_output_format_s32_dp_5ms.conf create mode 100644 tools/topology/topology2/include/bench/phase_vocoder_controls_capture.conf create mode 100644 tools/topology/topology2/include/bench/phase_vocoder_controls_playback.conf create mode 100644 tools/topology/topology2/include/bench/phase_vocoder_route.conf create mode 100644 tools/topology/topology2/include/bench/phase_vocoder_s16.conf create mode 100644 tools/topology/topology2/include/bench/phase_vocoder_s24.conf create mode 100644 tools/topology/topology2/include/bench/phase_vocoder_s32.conf create mode 100644 tools/topology/topology2/include/components/phase_vocoder.conf create mode 100644 tools/topology/topology2/include/components/phase_vocoder/hann_1024_256.conf create mode 100644 tools/topology/topology2/include/components/phase_vocoder/hann_1024_256_mono.conf create mode 100644 tools/topology/topology2/include/components/phase_vocoder/hann_256_128.conf create mode 100644 tools/topology/topology2/include/components/phase_vocoder/hann_512_128.conf create mode 100644 tools/topology/topology2/include/components/phase_vocoder/hann_512_256.conf diff --git a/tools/topology/topology2/cavs-benchmark-hda.conf b/tools/topology/topology2/cavs-benchmark-hda.conf index 6ee4b3e11882..b2bb6c2d6029 100644 --- a/tools/topology/topology2/cavs-benchmark-hda.conf +++ b/tools/topology/topology2/cavs-benchmark-hda.conf @@ -44,6 +44,7 @@ + @@ -837,6 +838,22 @@ IncludeByKey.BENCH_CONFIG { } + # + # Phase Vocoder component + # + + "phase_vocoder16" { + + } + + "phase_vocoder24" { + + } + + "phase_vocoder32" { + + } + # # RTNR component # diff --git a/tools/topology/topology2/cavs-benchmark-sdw.conf b/tools/topology/topology2/cavs-benchmark-sdw.conf index 98f08c7e13da..c2aaf9802e13 100644 --- a/tools/topology/topology2/cavs-benchmark-sdw.conf +++ b/tools/topology/topology2/cavs-benchmark-sdw.conf @@ -41,6 +41,7 @@ + @@ -460,6 +461,22 @@ IncludeByKey.BENCH_CONFIG { } + # + # Phase Vocoder component + # + + "phase_vocoder16" { + + } + + "phase_vocoder24" { + + } + + "phase_vocoder32" { + + } + # # RTNR component # diff --git a/tools/topology/topology2/development/tplg-targets-bench.cmake b/tools/topology/topology2/development/tplg-targets-bench.cmake index cb1b7300306e..3e76b55ee1ff 100644 --- a/tools/topology/topology2/development/tplg-targets-bench.cmake +++ b/tools/topology/topology2/development/tplg-targets-bench.cmake @@ -19,6 +19,7 @@ set(components "igo_nr" "level_multiplier" "micsel" + "phase_vocoder" "rtnr" "sound_dose" "src" @@ -44,6 +45,7 @@ set(component_parameters "BENCH_IGO_NR_PARAMS=default" "BENCH_LEVEL_MULTIPLIER_PARAMS=default" "BENCH_MICSEL_PARAMS=passthrough" + "BENCH_PHASE_VOCODER_PARAMS=default" "BENCH_RTNR_PARAMS=default" "BENCH_SOUND_DOSE_PARAMS=default" "BENCH_SRC_PARAMS=default" diff --git a/tools/topology/topology2/include/bench/one_input_output_format_s32_dp128.conf b/tools/topology/topology2/include/bench/one_input_output_format_s32_dp128.conf new file mode 100644 index 000000000000..379ee73f9eff --- /dev/null +++ b/tools/topology/topology2/include/bench/one_input_output_format_s32_dp128.conf @@ -0,0 +1,19 @@ + num_input_audio_formats 1 + num_output_audio_formats 1 + + # 32-bit 48KHz 2ch + Object.Base.input_audio_format [ + { + in_bit_depth 32 + in_valid_bit_depth 32 + ibs 1024 + } + ] + Object.Base.output_audio_format [ + { + out_bit_depth 32 + out_valid_bit_depth 32 + obs 1024 + } + ] + diff --git a/tools/topology/topology2/include/bench/one_input_output_format_s32_dp192.conf b/tools/topology/topology2/include/bench/one_input_output_format_s32_dp192.conf new file mode 100644 index 000000000000..b3989ce90f36 --- /dev/null +++ b/tools/topology/topology2/include/bench/one_input_output_format_s32_dp192.conf @@ -0,0 +1,19 @@ + num_input_audio_formats 1 + num_output_audio_formats 1 + + # 32-bit 48KHz 2ch + Object.Base.input_audio_format [ + { + in_bit_depth 32 + in_valid_bit_depth 32 + ibs 1536 + } + ] + Object.Base.output_audio_format [ + { + out_bit_depth 32 + out_valid_bit_depth 32 + obs 1536 + } + ] + diff --git a/tools/topology/topology2/include/bench/one_input_output_format_s32_dp256.conf b/tools/topology/topology2/include/bench/one_input_output_format_s32_dp256.conf new file mode 100644 index 000000000000..d753c1ea114f --- /dev/null +++ b/tools/topology/topology2/include/bench/one_input_output_format_s32_dp256.conf @@ -0,0 +1,19 @@ + num_input_audio_formats 1 + num_output_audio_formats 1 + + # 32-bit 48KHz 2ch + Object.Base.input_audio_format [ + { + in_bit_depth 32 + in_valid_bit_depth 32 + ibs 2048 + } + ] + Object.Base.output_audio_format [ + { + out_bit_depth 32 + out_valid_bit_depth 32 + obs 2048 + } + ] + diff --git a/tools/topology/topology2/include/bench/one_input_output_format_s32_dp_5ms.conf b/tools/topology/topology2/include/bench/one_input_output_format_s32_dp_5ms.conf new file mode 100644 index 000000000000..097f75fb121e --- /dev/null +++ b/tools/topology/topology2/include/bench/one_input_output_format_s32_dp_5ms.conf @@ -0,0 +1,22 @@ + num_input_audio_formats 1 + num_output_audio_formats 1 + + # 32-bit 48KHz 2ch, ibs/obs is set to match + # 5 ms or 240 stereo frames period. + Object.Base.input_audio_format [ + { + in_bit_depth 32 + in_valid_bit_depth 32 + in_channels 2 + ibs 1920 + } + ] + Object.Base.output_audio_format [ + { + out_bit_depth 32 + out_valid_bit_depth 32 + out_channels 2 + obs 1920 + } + ] + diff --git a/tools/topology/topology2/include/bench/phase_vocoder_controls_capture.conf b/tools/topology/topology2/include/bench/phase_vocoder_controls_capture.conf new file mode 100644 index 000000000000..c25aaebfcc58 --- /dev/null +++ b/tools/topology/topology2/include/bench/phase_vocoder_controls_capture.conf @@ -0,0 +1,17 @@ + # Created initially with script "./bench_comp_generate.sh phase_vocoder" + # may need edits to modify controls + Object.Control { + # Un-comment the supported controls in PHASE_VOCODER + bytes."1" { + name '$ANALOG_CAPTURE_PCM Phase Vocoder bytes' + IncludeByKey.BENCH_PHASE_VOCODER_PARAMS { + "default" "include/components/phase_vocoder/hann_1024_256.conf" + } + } + mixer."1" { + name '$ANALOG_CAPTURE_PCM Phase Vocoder enable' + } + enum."1" { + name '$ANALOG_CAPTURE_PCM Phase Vocoder speed' + } + } diff --git a/tools/topology/topology2/include/bench/phase_vocoder_controls_playback.conf b/tools/topology/topology2/include/bench/phase_vocoder_controls_playback.conf new file mode 100644 index 000000000000..fb6c3da0e369 --- /dev/null +++ b/tools/topology/topology2/include/bench/phase_vocoder_controls_playback.conf @@ -0,0 +1,17 @@ + # Created initially with script "./bench_comp_generate.sh phase_vocoder" + # may need edits to modify controls + Object.Control { + # Un-comment the supported controls in PHASE_VOCODER + bytes."1" { + name '$ANALOG_PLAYBACK_PCM Phase Vocoder bytes' + IncludeByKey.BENCH_PHASE_VOCODER_PARAMS { + "default" "include/components/phase_vocoder/hann_1024_256.conf" + } + } + mixer."1" { + name '$ANALOG_PLAYBACK_PCM Phase Vocoder enable' + } + enum."1" { + name '$ANALOG_PLAYBACK_PCM Phase Vocoder speed' + } + } diff --git a/tools/topology/topology2/include/bench/phase_vocoder_route.conf b/tools/topology/topology2/include/bench/phase_vocoder_route.conf new file mode 100644 index 000000000000..4213b01c87ed --- /dev/null +++ b/tools/topology/topology2/include/bench/phase_vocoder_route.conf @@ -0,0 +1,19 @@ + # Created with script "./bench_comp_generate.sh phase_vocoder" + Object.Base.route [ + { + sink '$BENCH_PLAYBACK_DAI_COPIER' + source 'phase_vocoder.$BENCH_PLAYBACK_HOST_PIPELINE.1' + } + { + sink 'phase_vocoder.$BENCH_PLAYBACK_HOST_PIPELINE.1' + source 'host-copier.0.playback' + } + { + source '$BENCH_CAPTURE_DAI_COPIER' + sink 'phase_vocoder.$BENCH_CAPTURE_HOST_PIPELINE.2' + } + { + source 'phase_vocoder.$BENCH_CAPTURE_HOST_PIPELINE.2' + sink 'host-copier.0.capture' + } + ] diff --git a/tools/topology/topology2/include/bench/phase_vocoder_s16.conf b/tools/topology/topology2/include/bench/phase_vocoder_s16.conf new file mode 100644 index 000000000000..c5fdb6aeef5c --- /dev/null +++ b/tools/topology/topology2/include/bench/phase_vocoder_s16.conf @@ -0,0 +1,13 @@ + # Created with script "./bench_comp_generate.sh phase_vocoder" + Object.Widget.phase_vocoder.1 { + index $BENCH_PLAYBACK_HOST_PIPELINE + + + } + Object.Widget.phase_vocoder.2 { + index $BENCH_CAPTURE_HOST_PIPELINE + + + } + + diff --git a/tools/topology/topology2/include/bench/phase_vocoder_s24.conf b/tools/topology/topology2/include/bench/phase_vocoder_s24.conf new file mode 100644 index 000000000000..9865e4e8f8b7 --- /dev/null +++ b/tools/topology/topology2/include/bench/phase_vocoder_s24.conf @@ -0,0 +1,13 @@ + # Created with script "./bench_comp_generate.sh phase_vocoder" + Object.Widget.phase_vocoder.1 { + index $BENCH_PLAYBACK_HOST_PIPELINE + + + } + Object.Widget.phase_vocoder.2 { + index $BENCH_CAPTURE_HOST_PIPELINE + + + } + + diff --git a/tools/topology/topology2/include/bench/phase_vocoder_s32.conf b/tools/topology/topology2/include/bench/phase_vocoder_s32.conf new file mode 100644 index 000000000000..46f9cad71989 --- /dev/null +++ b/tools/topology/topology2/include/bench/phase_vocoder_s32.conf @@ -0,0 +1,21 @@ + # Created with script "./bench_comp_generate.sh phase_vocoder" + Object.Widget.phase_vocoder.1 { + index $BENCH_PLAYBACK_HOST_PIPELINE + scheduler_domain DP + domain_id 123 + stack_bytes_requirement 4096 + heap_bytes_requirement 49152 + + + } + Object.Widget.phase_vocoder.2 { + index $BENCH_CAPTURE_HOST_PIPELINE + scheduler_domain DP + domain_id 123 + stack_bytes_requirement 4096 + heap_bytes_requirement 49152 + + + } + + diff --git a/tools/topology/topology2/include/components/phase_vocoder.conf b/tools/topology/topology2/include/components/phase_vocoder.conf new file mode 100644 index 000000000000..61e05d6375ab --- /dev/null +++ b/tools/topology/topology2/include/components/phase_vocoder.conf @@ -0,0 +1,122 @@ +# +# +# A PHASE_VOCODER component for SOF. All attributes defined herein are namespaced +# by alsatplg to "Object.Widget.phase_vocoder.attribute_name" +# +# Usage: this component can be used by declaring in the parent object. i.e. +# +# Object.Widget.phase_vocoder."N" { +# index 1 +# } +# } + +# +# Where M is pipeline ID and N is a unique integer in the parent object. + +Class.Widget."phase_vocoder" { + # + # Pipeline ID + # + DefineAttribute."index" { + type "integer" + } + + # + # Unique instance for PHASE_VOCODER widget + # + DefineAttribute."instance" { + type "integer" + } + + # Include common widget attributes definition + + + attributes { + !constructor [ + "index" + "instance" + ] + !mandatory [ + "num_input_pins" + "num_output_pins" + "num_input_audio_formats" + "num_output_audio_formats" + ] + + !immutable [ + "uuid" + "type" + ] + !deprecated [ + "preload_count" + ] + unique "instance" + } + + Object.Control { + # Switch controls + mixer."1" { + Object.Base.channel.1 { + name "fc" + shift 0 + } + Object.Base.ops.1 { + name "ctl" + info "volsw" + #259 binds the mixer control to switch get/put handlers + get 259 + put 259 + } + max 1 + } + + # Enum controls + enum."1" { + Object.Base { + channel.1 { + name "fc" + reg 3 + shift 0 + } + text.0 { + name "phase_vocoder_speed_enum" + !values [ + "0.5" + "0.6" + "0.7" + "0.8" + "0.9" + "1.0" + "1.1" + "1.2" + "1.3" + "1.4" + "1.5" + "1.6" + "1.7" + "1.8" + "1.9" + "2.0" + ] + } + ops.1 { + name "ctl" + info "enum" + #257 binds the mixer control to enum get/put handlers + get 257 + put 257 + } + } + } + } + + # + # Default attributes for phase_vocoder + # + + uuid "7a:cb:fb:09:c5:a9:57:4a:84:34:44:40:e5:98:ab:24" + type "effect" + no_pm "true" + num_input_pins 1 + num_output_pins 1 +} diff --git a/tools/topology/topology2/include/components/phase_vocoder/hann_1024_256.conf b/tools/topology/topology2/include/components/phase_vocoder/hann_1024_256.conf new file mode 100644 index 000000000000..01c7e35c9379 --- /dev/null +++ b/tools/topology/topology2/include/components/phase_vocoder/hann_1024_256.conf @@ -0,0 +1,17 @@ +# Exported PHASE_VOCODER configuration 02-Feb-2026 +# cd tools/tune/phase_vocoder; octave setup_phase_vocoder.m +Object.Base.data."phase_vocoder_config" { + bytes " + 0x53,0x4f,0x46,0x34,0x00,0x00,0x00,0x00, + 0x40,0x00,0x00,0x00,0x01,0xd0,0x01,0x03, + 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, + 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, + 0x40,0x00,0x00,0x00,0x00,0x00,0x00,0x00, + 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, + 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, + 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, + 0x00,0x00,0x00,0x00,0x80,0xbb,0x00,0x00, + 0x01,0xb0,0x6a,0x55,0x00,0x00,0x00,0x00, + 0x00,0x00,0x00,0x04,0x00,0x01,0x00,0x00, + 0x00,0x00,0x00,0x00,0x03,0x00,0x00,0x00" +} diff --git a/tools/topology/topology2/include/components/phase_vocoder/hann_1024_256_mono.conf b/tools/topology/topology2/include/components/phase_vocoder/hann_1024_256_mono.conf new file mode 100644 index 000000000000..3c4b1b70a096 --- /dev/null +++ b/tools/topology/topology2/include/components/phase_vocoder/hann_1024_256_mono.conf @@ -0,0 +1,17 @@ +# Exported PHASE_VOCODER configuration 26-Feb-2026 +# cd tools/tune/phase_vocoder; octave setup_phase_vocoder.m +Object.Base.data."phase_vocoder_config" { + bytes " + 0x53,0x4f,0x46,0x34,0x00,0x00,0x00,0x00, + 0x40,0x00,0x00,0x00,0x01,0xd0,0x01,0x03, + 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, + 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, + 0x40,0x00,0x00,0x00,0x00,0x00,0x00,0x00, + 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, + 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, + 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, + 0x00,0x00,0x00,0x00,0x80,0xbb,0x00,0x00, + 0x01,0xb0,0x6a,0x55,0x00,0x00,0x00,0x00, + 0x00,0x00,0x00,0x04,0x00,0x01,0x00,0x00, + 0x00,0x00,0x00,0x00,0x03,0x00,0x00,0x00" +} diff --git a/tools/topology/topology2/include/components/phase_vocoder/hann_256_128.conf b/tools/topology/topology2/include/components/phase_vocoder/hann_256_128.conf new file mode 100644 index 000000000000..bc31c0a0abb3 --- /dev/null +++ b/tools/topology/topology2/include/components/phase_vocoder/hann_256_128.conf @@ -0,0 +1,17 @@ +# Exported PHASE_VOCODER configuration 02-Feb-2026 +# cd tools/tune/phase_vocoder; octave setup_phase_vocoder.m +Object.Base.data."phase_vocoder_config" { + bytes " + 0x53,0x4f,0x46,0x34,0x00,0x00,0x00,0x00, + 0x40,0x00,0x00,0x00,0x01,0xd0,0x01,0x03, + 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, + 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, + 0x40,0x00,0x00,0x00,0x00,0x00,0x00,0x00, + 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, + 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, + 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, + 0x00,0x00,0x00,0x00,0x80,0xbb,0x00,0x00, + 0xab,0x00,0x56,0xab,0x00,0x00,0x00,0x00, + 0x00,0x00,0x00,0x01,0x80,0x00,0x00,0x00, + 0x00,0x00,0x00,0x00,0x03,0x00,0x00,0x00" +} diff --git a/tools/topology/topology2/include/components/phase_vocoder/hann_512_128.conf b/tools/topology/topology2/include/components/phase_vocoder/hann_512_128.conf new file mode 100644 index 000000000000..6196d9af217f --- /dev/null +++ b/tools/topology/topology2/include/components/phase_vocoder/hann_512_128.conf @@ -0,0 +1,17 @@ +# Exported PHASE_VOCODER configuration 02-Feb-2026 +# cd tools/tune/phase_vocoder; octave setup_phase_vocoder.m +Object.Base.data."phase_vocoder_config" { + bytes " + 0x53,0x4f,0x46,0x34,0x00,0x00,0x00,0x00, + 0x40,0x00,0x00,0x00,0x01,0xd0,0x01,0x03, + 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, + 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, + 0x40,0x00,0x00,0x00,0x00,0x00,0x00,0x00, + 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, + 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, + 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, + 0x00,0x00,0x00,0x00,0x80,0xbb,0x00,0x00, + 0x60,0x15,0x80,0x55,0x00,0x00,0x00,0x00, + 0x00,0x00,0x00,0x02,0x80,0x00,0x00,0x00, + 0x00,0x00,0x00,0x00,0x03,0x00,0x00,0x00" +} diff --git a/tools/topology/topology2/include/components/phase_vocoder/hann_512_256.conf b/tools/topology/topology2/include/components/phase_vocoder/hann_512_256.conf new file mode 100644 index 000000000000..3f3bec89b9e0 --- /dev/null +++ b/tools/topology/topology2/include/components/phase_vocoder/hann_512_256.conf @@ -0,0 +1,17 @@ +# Exported PHASE_VOCODER configuration 02-Feb-2026 +# cd tools/tune/phase_vocoder; octave setup_phase_vocoder.m +Object.Base.data."phase_vocoder_config" { + bytes " + 0x53,0x4f,0x46,0x34,0x00,0x00,0x00,0x00, + 0x40,0x00,0x00,0x00,0x01,0xd0,0x01,0x03, + 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, + 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, + 0x40,0x00,0x00,0x00,0x00,0x00,0x00,0x00, + 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, + 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, + 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, + 0x00,0x00,0x00,0x00,0x80,0xbb,0x00,0x00, + 0xc0,0x2a,0x00,0xab,0x00,0x00,0x00,0x00, + 0x00,0x00,0x00,0x02,0x00,0x01,0x00,0x00, + 0x00,0x00,0x00,0x00,0x03,0x00,0x00,0x00" +} From 9e79fd3744da1ef6d1ce9de20c82d308db97e8c7 Mon Sep 17 00:00:00 2001 From: Seppo Ingalsuo Date: Fri, 30 Jan 2026 15:21:05 +0200 Subject: [PATCH 7/8] Audio: Phase Vocoder: Add new component This patch adds the Phase Vocoder SOF module. It provides render speed control in range 0.5-2.0x. The pitch is preserved in audio waveform stretch or shorten. The module is using a frequency domain algorithm in STFT domain to interpolate magnitude and phase of output IFFT frames from input FFT frames. The render speed can be controlled via enable/disable switch and enum control with steps of 0.1, or with finer precision with bytes control. (WIP) The STFT parameters are configured with bytes control blob. The default is 1024 size FFT with hop of 256 and Hann window. Signed-off-by: Seppo Ingalsuo --- app/boards/intel_adsp_ace15_mtpm.conf | 1 + app/boards/intel_adsp_ace20_lnl.conf | 1 + app/boards/intel_adsp_ace30_ptl.conf | 1 + app/boards/intel_adsp_ace30_wcl.conf | 1 + src/arch/host/configs/library_defconfig | 1 + src/audio/CMakeLists.txt | 3 + src/audio/Kconfig | 1 + src/audio/phase_vocoder/CMakeLists.txt | 15 + src/audio/phase_vocoder/Kconfig | 14 + src/audio/phase_vocoder/llext/CMakeLists.txt | 11 + src/audio/phase_vocoder/llext/llext.toml.h | 6 + .../phase_vocoder/phase_vocoder-generic.c | 500 +++++++++++++++++ src/audio/phase_vocoder/phase_vocoder-ipc4.c | 89 +++ src/audio/phase_vocoder/phase_vocoder.c | 280 ++++++++++ src/audio/phase_vocoder/phase_vocoder.h | 333 ++++++++++++ src/audio/phase_vocoder/phase_vocoder.toml | 21 + .../phase_vocoder/phase_vocoder_common.c | 510 ++++++++++++++++++ src/audio/phase_vocoder/phase_vocoder_setup.c | 259 +++++++++ src/include/sof/audio/component.h | 1 + tools/rimage/config/lnl.toml.h | 4 + tools/rimage/config/mtl.toml.h | 4 + tools/rimage/config/ptl.toml.h | 4 + tools/rimage/config/wcl.toml.h | 4 + tools/testbench/utils_ipc4.c | 1 + uuid-registry.txt | 1 + 25 files changed, 2066 insertions(+) create mode 100644 src/audio/phase_vocoder/CMakeLists.txt create mode 100644 src/audio/phase_vocoder/Kconfig create mode 100644 src/audio/phase_vocoder/llext/CMakeLists.txt create mode 100644 src/audio/phase_vocoder/llext/llext.toml.h create mode 100644 src/audio/phase_vocoder/phase_vocoder-generic.c create mode 100644 src/audio/phase_vocoder/phase_vocoder-ipc4.c create mode 100644 src/audio/phase_vocoder/phase_vocoder.c create mode 100644 src/audio/phase_vocoder/phase_vocoder.h create mode 100644 src/audio/phase_vocoder/phase_vocoder.toml create mode 100644 src/audio/phase_vocoder/phase_vocoder_common.c create mode 100644 src/audio/phase_vocoder/phase_vocoder_setup.c diff --git a/app/boards/intel_adsp_ace15_mtpm.conf b/app/boards/intel_adsp_ace15_mtpm.conf index e95b18d97169..bbebd59569bb 100644 --- a/app/boards/intel_adsp_ace15_mtpm.conf +++ b/app/boards/intel_adsp_ace15_mtpm.conf @@ -15,6 +15,7 @@ CONFIG_COMP_MFCC=y CONFIG_COMP_MULTIBAND_DRC=y CONFIG_FORMAT_CONVERT_HIFI3=n CONFIG_SAMPLE_KEYPHRASE=y +CONFIG_COMP_PHASE_VOCODER=y CONFIG_COMP_STFT_PROCESS=y # SOF / audio modules / mocks diff --git a/app/boards/intel_adsp_ace20_lnl.conf b/app/boards/intel_adsp_ace20_lnl.conf index beb441d6e6c6..38accd2968bf 100644 --- a/app/boards/intel_adsp_ace20_lnl.conf +++ b/app/boards/intel_adsp_ace20_lnl.conf @@ -11,6 +11,7 @@ CONFIG_COMP_TESTER=m CONFIG_COMP_SRC_IPC4_FULL_MATRIX=y CONFIG_FORMAT_CONVERT_HIFI3=n CONFIG_SAMPLE_KEYPHRASE=y +CONFIG_COMP_PHASE_VOCODER=y CONFIG_COMP_STFT_PROCESS=y # SOF / infrastructure diff --git a/app/boards/intel_adsp_ace30_ptl.conf b/app/boards/intel_adsp_ace30_ptl.conf index 12aa78162c06..9664f29f1c94 100644 --- a/app/boards/intel_adsp_ace30_ptl.conf +++ b/app/boards/intel_adsp_ace30_ptl.conf @@ -14,6 +14,7 @@ CONFIG_FORMAT_CONVERT_HIFI3=n # tests it can't use extra CONFIGs. See #9410, #8722 and #9386 CONFIG_COMP_GOOGLE_RTC_AUDIO_PROCESSING=m CONFIG_GOOGLE_RTC_AUDIO_PROCESSING_MOCK=y +CONFIG_COMP_PHASE_VOCODER=y CONFIG_COMP_STFT_PROCESS=y # SOF / infrastructure diff --git a/app/boards/intel_adsp_ace30_wcl.conf b/app/boards/intel_adsp_ace30_wcl.conf index 621eb719de9f..3af7e69c144b 100644 --- a/app/boards/intel_adsp_ace30_wcl.conf +++ b/app/boards/intel_adsp_ace30_wcl.conf @@ -14,6 +14,7 @@ CONFIG_FORMAT_CONVERT_HIFI3=n # tests it can't use extra CONFIGs. See #9410, #8722 and #9386 CONFIG_COMP_GOOGLE_RTC_AUDIO_PROCESSING=m CONFIG_GOOGLE_RTC_AUDIO_PROCESSING_MOCK=y +CONFIG_COMP_PHASE_VOCODER=y CONFIG_COMP_STFT_PROCESS=y # SOF / infrastructure diff --git a/src/arch/host/configs/library_defconfig b/src/arch/host/configs/library_defconfig index 28c486bec58d..1279dd26c924 100644 --- a/src/arch/host/configs/library_defconfig +++ b/src/arch/host/configs/library_defconfig @@ -14,6 +14,7 @@ CONFIG_COMP_MFCC=y CONFIG_COMP_MODULE_ADAPTER=y CONFIG_COMP_MULTIBAND_DRC=y CONFIG_COMP_MUX=y +CONFIG_COMP_PHASE_VOCODER=y CONFIG_COMP_RTNR=y CONFIG_COMP_SEL=y CONFIG_COMP_SOUND_DOSE=y diff --git a/src/audio/CMakeLists.txt b/src/audio/CMakeLists.txt index 29e602871af7..559ce2253736 100644 --- a/src/audio/CMakeLists.txt +++ b/src/audio/CMakeLists.txt @@ -71,6 +71,9 @@ if(NOT CONFIG_COMP_MODULE_SHARED_LIBRARY_BUILD) if(CONFIG_COMP_MUX) add_subdirectory(mux) endif() + if(CONFIG_COMP_PHASE_VOCODER) + add_subdirectory(phase_vocoder) + endif() if(CONFIG_COMP_RTNR) add_subdirectory(rtnr) endif() diff --git a/src/audio/Kconfig b/src/audio/Kconfig index 1f7d362ffdc2..0250b9eb5ebb 100644 --- a/src/audio/Kconfig +++ b/src/audio/Kconfig @@ -139,6 +139,7 @@ rsource "module_adapter/Kconfig" rsource "multiband_drc/Kconfig" rsource "mux/Kconfig" rsource "nxp/Kconfig" +rsource "phase_vocoder/Kconfig" rsource "rtnr/Kconfig" rsource "selector/Kconfig" rsource "smart_amp/Kconfig" diff --git a/src/audio/phase_vocoder/CMakeLists.txt b/src/audio/phase_vocoder/CMakeLists.txt new file mode 100644 index 000000000000..b632c50af6cb --- /dev/null +++ b/src/audio/phase_vocoder/CMakeLists.txt @@ -0,0 +1,15 @@ +# SPDX-License-Identifier: BSD-3-Clause + +if(CONFIG_COMP_PHASE_VOCODER STREQUAL "m" AND DEFINED CONFIG_LLEXT) + add_subdirectory(llext ${PROJECT_BINARY_DIR}/phase_vocoder_llext) + add_dependencies(app phase_vocoder) +else() + add_local_sources(sof phase_vocoder.c) + add_local_sources(sof phase_vocoder_setup.c) + add_local_sources(sof phase_vocoder_common.c) + add_local_sources(sof phase_vocoder-generic.c) + + if(CONFIG_IPC_MAJOR_4) + add_local_sources(sof phase_vocoder-ipc4.c) + endif() +endif() diff --git a/src/audio/phase_vocoder/Kconfig b/src/audio/phase_vocoder/Kconfig new file mode 100644 index 000000000000..158537b731cb --- /dev/null +++ b/src/audio/phase_vocoder/Kconfig @@ -0,0 +1,14 @@ +# SPDX-License-Identifier: BSD-3-Clause + +config COMP_PHASE_VOCODER + tristate "Phase Vocoder component" + default n + select MATH_FFT + select MATH_32BIT_FFT + help + Select for phase_vocoder component. The component provides + render speed control in range 0.5-2.0x. The pitch is + preserved in audio waveform stretch or shorten. The module + is using a frequency domain algorithm in STFT domain to + interpolate magnitude and phase of output IFFT frames from + input FFT frames. diff --git a/src/audio/phase_vocoder/llext/CMakeLists.txt b/src/audio/phase_vocoder/llext/CMakeLists.txt new file mode 100644 index 000000000000..ffefa29c7080 --- /dev/null +++ b/src/audio/phase_vocoder/llext/CMakeLists.txt @@ -0,0 +1,11 @@ +# Copyright (c) 2026 Intel Corporation. +# SPDX-License-Identifier: Apache-2.0 + +sof_llext_build("phase_vocoder" + SOURCES ../phase_vocoder.c + ../phase_vocoder_setup.c + ../phase_vocoder_common.c + ../phase_vocoder-generic.c + ../phase_vocoder-ipc4.c + LIB openmodules +) diff --git a/src/audio/phase_vocoder/llext/llext.toml.h b/src/audio/phase_vocoder/llext/llext.toml.h new file mode 100644 index 000000000000..94cfd0d67d50 --- /dev/null +++ b/src/audio/phase_vocoder/llext/llext.toml.h @@ -0,0 +1,6 @@ +#include +#define LOAD_TYPE "2" +#include "../phase_vocoder.toml" + +[module] +count = __COUNTER__ diff --git a/src/audio/phase_vocoder/phase_vocoder-generic.c b/src/audio/phase_vocoder/phase_vocoder-generic.c new file mode 100644 index 000000000000..2a96302b1aad --- /dev/null +++ b/src/audio/phase_vocoder/phase_vocoder-generic.c @@ -0,0 +1,500 @@ +// SPDX-License-Identifier: BSD-3-Clause +// +// Copyright(c) 2025 Intel Corporation. + +#include +#include +#include +#include +#include +#include +#include +#include "phase_vocoder.h" + +#if CONFIG_FORMAT_S32LE +/** + * phase_vocoder_source_s32() - Process S16_LE format. + * @mod: Pointer to module data. + * @source: Source for PCM samples data. + * @sink: Sink for PCM samples data. + * @frames: Number of audio data frames to process. + * + * This is the processing function for 16-bit signed integer PCM formats. The + * audio samples in every frame are re-order to channels order defined in + * component data channel_map[]. + * + * Return: Value zero for success, otherwise an error code. + */ +int phase_vocoder_source_s32(struct phase_vocoder_comp_data *cd, struct sof_source *source, + int frames) +{ + struct phase_vocoder_state *state = &cd->state; + struct phase_vocoder_buffer *ibuf; + int32_t const *x, *x_start, *x_end; + const int32_t mix_gain = cd->mono_mix_coef; + int32_t mix; + int frames_left; + int x_size; + int bytes; + int ret; + int n1; + int n2; + int n; + int i; + int j; + int stream_channels = cd->stream_channels; + int process_channels = cd->process_channels; + bool process_mono = cd->config->mono; + + ibuf = &state->ibuf[0]; + frames = MIN(frames, ibuf->s_free); + bytes = frames * cd->frame_bytes; + + /* Get pointer to source data in circular buffer */ + ret = source_get_data_s32(source, bytes, &x, &x_start, &x_size); + if (ret) + return ret; + + /* Set helper pointers to buffer end for wrap check. Then loop until all + * samples are processed. + */ + x_end = x_start + x_size; + + frames_left = frames; + while (frames_left) { + /* Find out samples to process before first wrap or end of data. */ + ibuf = &state->ibuf[0]; + n1 = (x_end - x) / stream_channels; + n2 = phase_vocoder_buffer_samples_without_wrap(ibuf, ibuf->w_ptr); + n = MIN(n1, n2); + n = MIN(n, frames_left); + if (process_mono) { + for (i = 0; i < n; i++) { + mix = 0; + for (j = 0; j < stream_channels; j++) + mix += Q_MULTSR_32X32((int64_t)mix_gain, *x++, 31, 31, 31); + + *ibuf->w_ptr++ = mix; + } + } else { + for (i = 0; i < n; i++) { + for (j = 0; j < stream_channels; j++) { + ibuf = &state->ibuf[j]; + *ibuf->w_ptr++ = *x++; + } + } + } + + /* One of the buffers needs a wrap (or end of data), so check for wrap */ + for (j = 0; j < process_channels; j++) { + ibuf = &state->ibuf[j]; + ibuf->w_ptr = phase_vocoder_buffer_wrap(ibuf, ibuf->w_ptr); + } + + if (x >= x_end) + x -= x_size; + + /* Update processed samples count for next loop iteration. */ + frames_left -= n; + } + + /* Update the source for bytes consumed. Return success. */ + source_release_data(source, bytes); + for (j = 0; j < process_channels; j++) { + ibuf = &state->ibuf[j]; + ibuf->s_avail += frames; + ibuf->s_free -= frames; + } + + return 0; +} + +/** + * phase_vocoder_sink_s32() - Process S16_LE format. + * @mod: Pointer to module data. + * @source: Source for PCM samples data. + * @sink: Sink for PCM samples data. + * @frames: Number of audio data frames to process. + * + * This is the processing function for 16-bit signed integer PCM formats. The + * audio samples in every frame are re-order to channels order defined in + * component data channel_map[]. + * + * Return: Value zero for success, otherwise an error code. + */ +int phase_vocoder_sink_s32(struct phase_vocoder_comp_data *cd, struct sof_sink *sink, int frames) +{ + struct phase_vocoder_state *state = &cd->state; + struct phase_vocoder_buffer *obuf; + int32_t *y, *y_start, *y_end; + int frames_remain; + int bytes; + int y_size; + int ret; + int ch, n1, n, i; + int stream_channels = cd->stream_channels; + int process_channels = cd->process_channels; + bool process_mono = cd->config->mono; + + obuf = &state->obuf[0]; + frames = MIN(frames, obuf->s_avail); + if (!frames) + return 0; + + /* Get pointer to sink data in circular buffer */ + bytes = frames * cd->frame_bytes; + ret = sink_get_buffer_s32(sink, bytes, &y, &y_start, &y_size); + if (ret) + return ret; + + /* Set helper pointers to buffer end for wrap check. Then loop until all + * samples are processed. + */ + y_end = y_start + y_size; + + frames_remain = frames; + while (frames_remain) { + /* Find out samples to process before first wrap or end of data. */ + obuf = &state->obuf[0]; + n1 = (y_end - y) / stream_channels; + n = phase_vocoder_buffer_samples_without_wrap(obuf, obuf->r_ptr); + n = MIN(n1, n); + n = MIN(n, frames_remain); + + if (process_mono) { + for (i = 0; i < n; i++) { + for (ch = 0; ch < stream_channels; ch++) + *y++ = *obuf->r_ptr; + + *obuf->r_ptr++ = 0; /* clear overlap add mix */ + } + } else { + for (i = 0; i < n; i++) { + for (ch = 0; ch < stream_channels; ch++) { + obuf = &state->obuf[ch]; + *y++ = *obuf->r_ptr; + *obuf->r_ptr++ = 0; /* clear overlap add mix */ + } + } + } + + /* One of the buffers needs a wrap (or end of data), so check for wrap */ + for (ch = 0; ch < process_channels; ch++) { + obuf = &state->obuf[ch]; + obuf->r_ptr = phase_vocoder_buffer_wrap(obuf, obuf->r_ptr); + } + + if (y >= y_end) + y -= y_size; + + /* Update processed samples count for next loop iteration. */ + frames_remain -= n; + } + + /* Update the sink for bytes produced. Return success. */ + sink_commit_buffer(sink, bytes); + for (ch = 0; ch < process_channels; ch++) { + obuf = &state->obuf[ch]; + obuf->s_avail -= frames; + obuf->s_free += frames; + } + + return 0; +} +#endif /* CONFIG_FORMAT_S32LE */ + +#if CONFIG_FORMAT_S16LE +/** + * phase_vocoder_source_s16() - Process S16_LE format. + * @mod: Pointer to module data. + * @source: Source for PCM samples data. + * @sink: Sink for PCM samples data. + * @frames: Number of audio data frames to process. + * + * This is the processing function for 16-bit signed integer PCM formats. The + * audio samples in every frame are re-order to channels order defined in + * component data channel_map[]. + * + * Return: Value zero for success, otherwise an error code. + */ +int phase_vocoder_source_s16(struct phase_vocoder_comp_data *cd, struct sof_source *source, + int frames) +{ + struct phase_vocoder_state *state = &cd->state; + struct phase_vocoder_buffer *ibuf; + const int32_t mix_gain = cd->mono_mix_coef; + int32_t mix; + int16_t const *x, *x_start, *x_end; + int16_t in; + int frames_left; + int x_size; + int bytes; + int ret; + int n1; + int n2; + int n; + int i; + int j; + int stream_channels = cd->stream_channels; + int process_channels = cd->process_channels; + bool process_mono = cd->config->mono; + + ibuf = &state->ibuf[0]; + frames = MIN(frames, ibuf->s_free); + bytes = frames * cd->frame_bytes; + frames_left = frames; + + + /* Get pointer to source data in circular buffer, get buffer start and size to + * check for wrap. The size in bytes is converted to number of s16 samples to + * control the samples process loop. If the number of bytes requested is not + * possible, an error is returned. + */ + ret = source_get_data_s16(source, bytes, &x, &x_start, &x_size); + if (ret) + return ret; + + /* Set helper pointers to buffer end for wrap check. Then loop until all + * samples are processed. + */ + x_end = x_start + x_size; + + while (frames_left) { + /* Find out samples to process before first wrap or end of data. */ + ibuf = &state->ibuf[0]; + n1 = (x_end - x) / stream_channels; + n2 = phase_vocoder_buffer_samples_without_wrap(ibuf, ibuf->w_ptr); + n = MIN(n1, n2); + n = MIN(n, frames_left); + if (process_mono) { + for (i = 0; i < n; i++) { + mix = 0; + for (j = 0; j < stream_channels; j++) + mix += Q_MULTSR_32X32((int64_t)mix_gain, *x++, 31, 15, 31); + + *ibuf->w_ptr++ = mix; + } + } else { + for (i = 0; i < n; i++) { + for (j = 0; j < stream_channels; j++) { + ibuf = &state->ibuf[j]; + in = *x++; + *ibuf->w_ptr++ = (int32_t)in << 16; + } + } + } + + /* One of the buffers needs a wrap (or end of data), so check for wrap */ + for (j = 0; j < process_channels; j++) { + ibuf = &state->ibuf[j]; + ibuf->w_ptr = phase_vocoder_buffer_wrap(ibuf, ibuf->w_ptr); + } + + if (x >= x_end) + x -= x_size; + + /* Update processed samples count for next loop iteration. */ + frames_left -= n; + } + + /* Update the source for bytes consumed. Return success. */ + source_release_data(source, bytes); + for (j = 0; j < process_channels; j++) { + ibuf = &state->ibuf[j]; + ibuf->s_avail += frames; + ibuf->s_free -= frames; + } + return 0; +} + +/** + * phase_vocoder_sink_s16() - Process S16_LE format. + * @mod: Pointer to module data. + * @source: Source for PCM samples data. + * @sink: Sink for PCM samples data. + * @frames: Number of audio data frames to process. + * + * This is the processing function for 16-bit signed integer PCM formats. The + * audio samples in every frame are re-order to channels order defined in + * component data channel_map[]. + * + * Return: Value zero for success, otherwise an error code. + */ +int phase_vocoder_sink_s16(struct phase_vocoder_comp_data *cd, struct sof_sink *sink, int frames) +{ + struct phase_vocoder_state *state = &cd->state; + struct phase_vocoder_buffer *obuf; + int16_t sample; + int16_t *y, *y_start, *y_end; + int frames_remain; + int y_size; + int bytes; + int ret; + int ch, n1, n, i; + int stream_channels = cd->stream_channels; + int process_channels = cd->process_channels; + bool process_mono = cd->config->mono; + + obuf = &state->obuf[0]; + frames = MIN(frames, obuf->s_avail); + if (!frames) + return 0; + + /* Get pointer to sink data in circular buffer */ + bytes = frames * cd->frame_bytes; + frames_remain = frames; + ret = sink_get_buffer_s16(sink, bytes, &y, &y_start, &y_size); + if (ret) + return ret; + + /* Set helper pointers to buffer end for wrap check. Then loop until all + * samples are processed. + */ + y_end = y_start + y_size; + while (frames_remain) { + /* Find out samples to process before first wrap or end of data. */ + obuf = &state->obuf[0]; + n1 = (y_end - y) / stream_channels; + n = phase_vocoder_buffer_samples_without_wrap(obuf, obuf->r_ptr); + n = MIN(n1, n); + n = MIN(n, frames_remain); + + if (process_mono) { + for (i = 0; i < n; i++) { + sample = sat_int16(Q_SHIFT_RND(*obuf->r_ptr, 31, 15)); + for (ch = 0; ch < stream_channels; ch++) + *y++ = sample; + + *obuf->r_ptr++ = 0; /* clear overlap add mix */ + } + } else { + for (i = 0; i < n; i++) { + for (ch = 0; ch < stream_channels; ch++) { + obuf = &state->obuf[ch]; + *y++ = sat_int16(Q_SHIFT_RND(*obuf->r_ptr, 31, 15)); + *obuf->r_ptr++ = 0; /* clear overlap add mix */ + } + } + } + + /* One of the buffers needs a wrap (or end of data), so check for wrap */ + for (ch = 0; ch < process_channels; ch++) { + obuf = &state->obuf[ch]; + obuf->r_ptr = phase_vocoder_buffer_wrap(obuf, obuf->r_ptr); + } + + if (y >= y_end) + y -= y_size; + + /* Update processed samples count for next loop iteration. */ + frames_remain -= n; + } + + /* Update the sink for bytes produced. Return success. */ + sink_commit_buffer(sink, bytes); + for (ch = 0; ch < process_channels; ch++) { + obuf = &state->obuf[ch]; + obuf->s_avail -= frames; + obuf->s_free += frames; + } + + return 0; +} +#endif /* CONFIG_FORMAT_S16LE */ + +void phase_vocoder_fill_fft_buffer(struct phase_vocoder_state *state, int ch) +{ + struct phase_vocoder_buffer *ibuf = &state->ibuf[ch]; + struct phase_vocoder_fft *fft = &state->fft; + struct icomplex32 *fft_buf_ptr; + int32_t *prev_data = state->prev_data[ch]; + int32_t *r = ibuf->r_ptr; + const int prev_data_size = state->prev_data_size; + const int fft_hop_size = fft->fft_hop_size; + int samples_remain = fft_hop_size; + int j; + int n; + + /* Copy overlapped samples from state buffer. Imaginary part of input + * remains zero. + */ + fft_buf_ptr = fft->fft_buf; + for (j = 0; j < prev_data_size; j++) { + fft_buf_ptr->real = prev_data[j]; + fft_buf_ptr->imag = 0; + fft_buf_ptr++; + } + + /* Copy hop size of new data from circular buffer */ + fft_buf_ptr = &fft->fft_buf[prev_data_size]; + while (samples_remain) { + n = phase_vocoder_buffer_samples_without_wrap(ibuf, r); + n = MIN(n, samples_remain); + for (j = 0; j < n; j++) { + fft_buf_ptr->real = *r++; + fft_buf_ptr->imag = 0; + fft_buf_ptr++; + } + r = phase_vocoder_buffer_wrap(ibuf, r); + samples_remain -= n; + } + + ibuf->r_ptr = r; + ibuf->s_avail -= fft_hop_size; + ibuf->s_free += fft_hop_size; + + /* Copy for next time data back to input data overlap buffer */ + fft_buf_ptr = &fft->fft_buf[fft_hop_size]; + for (j = 0; j < prev_data_size; j++) + *prev_data++ = fft_buf_ptr++->real; +} + +int phase_vocoder_overlap_add_ifft_buffer(struct phase_vocoder_state *state, int ch) +{ + struct phase_vocoder_buffer *obuf = &state->obuf[ch]; + struct phase_vocoder_fft *fft = &state->fft; + int32_t *w = obuf->w_ptr; + int32_t sample; + int i; + int n; + int samples_remain = fft->fft_size; + int idx = 0; + + if (obuf->s_free < samples_remain) + return -EINVAL; + + while (samples_remain) { + n = phase_vocoder_buffer_samples_without_wrap(obuf, w); + n = MIN(samples_remain, n); + for (i = 0; i < n; i++) { + sample = Q_MULTSR_32X32((int64_t)state->gain_comp, fft->fft_buf[idx].real, + 31, 31, 31); + *w = sat_int32((int64_t)*w + sample); + w++; + idx++; + } + w = phase_vocoder_buffer_wrap(obuf, w); + samples_remain -= n; + } + + w = obuf->w_ptr + fft->fft_hop_size; + obuf->w_ptr = phase_vocoder_buffer_wrap(obuf, w); + obuf->s_avail += fft->fft_hop_size; + obuf->s_free -= fft->fft_hop_size; + return 0; +} + +void phase_vocoder_apply_window(struct phase_vocoder_state *state) +{ + struct phase_vocoder_fft *fft = &state->fft; + struct icomplex32 *fft_buf_ptr = fft->fft_buf; + const int32_t *window = state->window; + const int fft_size = fft->fft_size; + int i; + + for (i = 0; i < fft_size; i++) { + fft_buf_ptr->real = sat_int32(Q_MULTSR_32X32((int64_t)fft_buf_ptr->real, + window[i], 31, 31, 31)); + fft_buf_ptr++; + } +} diff --git a/src/audio/phase_vocoder/phase_vocoder-ipc4.c b/src/audio/phase_vocoder/phase_vocoder-ipc4.c new file mode 100644 index 000000000000..0928cb5fed49 --- /dev/null +++ b/src/audio/phase_vocoder/phase_vocoder-ipc4.c @@ -0,0 +1,89 @@ +// SPDX-License-Identifier: BSD-3-Clause +// +// Copyright(c) 2026 Intel Corporation. + +#include +#include +#include "phase_vocoder.h" + +LOG_MODULE_DECLARE(phase_vocoder, CONFIG_SOF_LOG_LEVEL); + +/* IPC4 controls handler */ +__cold int phase_vocoder_set_config(struct processing_module *mod, uint32_t param_id, + enum module_cfg_fragment_position pos, + uint32_t data_offset_size, const uint8_t *fragment, + size_t fragment_size, uint8_t *response, size_t response_size) +{ + struct sof_ipc4_control_msg_payload *ctl = (struct sof_ipc4_control_msg_payload *)fragment; + struct phase_vocoder_comp_data *cd = module_get_private_data(mod); + struct comp_dev *dev = mod->dev; + + assert_can_be_cold(); + + switch (param_id) { + case SOF_IPC4_SWITCH_CONTROL_PARAM_ID: + if (ctl->id != 0) { + comp_err(dev, "Illegal switch control id = %d.", ctl->id); + return -EINVAL; + } + + if (ctl->num_elems != 1) { + comp_err(dev, "Illegal switch control num_elems = %d.", ctl->num_elems); + return -EINVAL; + } + + cd->enable = ctl->chanv[0].value; + comp_info(dev, "enable = %d", cd->enable); + return 0; + + case SOF_IPC4_ENUM_CONTROL_PARAM_ID: + if (ctl->id != 0) { + comp_err(dev, "Illegal enum control id = %d.", ctl->id); + return -EINVAL; + } + + if (ctl->num_elems != 1) { + comp_err(dev, "Illegal enum control num_elems = %d.", ctl->num_elems); + return -EINVAL; + } + + if (ctl->chanv[0].value < 0 || ctl->chanv[0].value > 15) { + comp_err(dev, "Illegal enum control value = %d.", ctl->chanv[0].value); + return -EINVAL; + } + + cd->speed_enum = ctl->chanv[0].value; + cd->speed_ctrl = PHASE_VOCODER_MIN_SPEED_Q29 + + Q_MULTSR_32X32((int64_t)cd->speed_enum, PHASE_VOCODER_SPEED_STEP_Q31, + 0, 31, 29); + + comp_info(dev, "speed_enum = %d, speed = %d", cd->speed_enum, cd->speed_ctrl); + return 0; + } + + if (fragment_size != sizeof(struct sof_phase_vocoder_config)) { + comp_err(dev, "Illegal fragment size %d, expect %d.", fragment_size, + sizeof(struct sof_phase_vocoder_config)); + return -EINVAL; + } + + if (!cd->config) { + cd->config = mod_alloc(mod, sizeof(struct sof_phase_vocoder_config)); + if (!cd->config) { + comp_err(dev, "Failed to allocate configuration."); + return -ENOMEM; + } + } + + memcpy_s(cd->config, sizeof(struct sof_phase_vocoder_config), fragment, fragment_size); + return 0; +} + +/* Not used in IPC4 systems, if IPC4 only component, omit .get_configuration set */ +__cold int phase_vocoder_get_config(struct processing_module *mod, uint32_t config_id, + uint32_t *data_offset_size, uint8_t *fragment, + size_t fragment_size) +{ + assert_can_be_cold(); + return 0; +} diff --git a/src/audio/phase_vocoder/phase_vocoder.c b/src/audio/phase_vocoder/phase_vocoder.c new file mode 100644 index 000000000000..4bf20e35e7df --- /dev/null +++ b/src/audio/phase_vocoder/phase_vocoder.c @@ -0,0 +1,280 @@ +// SPDX-License-Identifier: BSD-3-Clause +// +// Copyright(c) 2025 Intel Corporation. + +#include +#include +#include +#include +#include +#include "phase_vocoder.h" + +/* UUID identifies the components. Use e.g. command uuidgen from package + * uuid-runtime, add it to uuid-registry.txt in SOF top level. + */ +SOF_DEFINE_REG_UUID(phase_vocoder); + +/* Creates logging data for the component */ +LOG_MODULE_REGISTER(phase_vocoder, CONFIG_SOF_LOG_LEVEL); + +/* Creates the component trace. Traces show in trace console the component + * info, warning, and error messages. + */ +DECLARE_TR_CTX(phase_vocoder_tr, SOF_UUID(phase_vocoder_uuid), LOG_LEVEL_INFO); + +#if STFT_DEBUG +FILE *stft_debug_fft_in_fh; +FILE *stft_debug_fft_out_fh; +FILE *stft_debug_ifft_out_fh; +#endif + +__cold static void phase_vocoder_reset_parameters(struct processing_module *mod) +{ + struct phase_vocoder_comp_data *cd = module_get_private_data(mod); + + memset(cd, 0, sizeof(*cd)); + + /* TODO: Set enable to false to make sure processing is not enabled if + * there is no control provided by tplg. + */ + cd->enable = true; + cd->speed_ctrl = PHASE_VOCODER_SPEED_NORMAL; +} + +/** + * phase_vocoder_init() - Initialize the phase_vocoder component. + * @mod: Pointer to module data. + * + * This function is called when the instance is created. The + * macro __cold informs that the code that is non-critical + * is loaded to slower but large DRAM. + * + * Return: Zero if success, otherwise error code. + */ +__cold static int phase_vocoder_init(struct processing_module *mod) +{ + struct module_data *md = &mod->priv; + struct comp_dev *dev = mod->dev; + struct phase_vocoder_comp_data *cd; + + assert_can_be_cold(); + + comp_info(dev, "phase_vocoder_init()"); + + cd = mod_alloc(mod, sizeof(*cd)); + if (!cd) + return -ENOMEM; + + md->private = cd; + phase_vocoder_reset_parameters(mod); + +#if STFT_DEBUG + stft_debug_fft_in_fh = fopen("stft_debug_fft_in.txt", "w"); + if (!stft_debug_fft_in_fh) { + fprintf(stderr, "Debug file open failed.\n"); + return -EINVAL; + } + + stft_debug_fft_out_fh = fopen("stft_debug_fft_out.txt", "w"); + if (!stft_debug_fft_out_fh) { + fprintf(stderr, "Debug file open failed.\n"); + return -EINVAL; + } + + stft_debug_ifft_out_fh = fopen("stft_debug_ifft_out.txt", "w"); + if (!stft_debug_ifft_out_fh) { + fprintf(stderr, "Debug file open failed.\n"); + fclose(stft_debug_fft_out_fh); + return -EINVAL; + } +#endif + + return 0; +} + +/** + * phase_vocoder_process() - The audio data processing function. + * @mod: Pointer to module data. + * @sources: Pointer to audio samples data sources array. + * @num_of_sources: Number of sources in the array. + * @sinks: Pointer to audio samples data sinks array. + * @num_of_sinks: Number of sinks in the array. + * + * This is the processing function that is called for scheduled + * pipelines. The processing is controlled by the enable switch. + * + * Return: Zero if success, otherwise error code. + */ +static int phase_vocoder_process(struct processing_module *mod, struct sof_source **sources, + int num_of_sources, struct sof_sink **sinks, int num_of_sinks) +{ + struct phase_vocoder_comp_data *cd = module_get_private_data(mod); + struct sof_source *source = sources[0]; /* One input in this example */ + struct sof_sink *sink = sinks[0]; /* One output in this example */ + int source_frames = source_get_data_frames_available(source); + int sink_frames = sink_get_free_frames(sink); + int frames; + int ret; + + if (cd->speed_ctrl != cd->state.speed) + phase_vocoder_reset_for_new_speed(cd); + + if (cd->enable) { + ret = cd->phase_vocoder_func(mod, source, sink, source_frames, sink_frames); + if (ret) + comp_err(mod->dev, "Failure, check the setup parameters."); + + return ret; + } + + /* Just copy from source to sink. */ + frames = MIN(source_frames, sink_frames); + source_to_sink_copy(source, sink, true, frames * cd->frame_bytes); + return 0; +} + +/** + * phase_vocoder_prepare() - Prepare the component for processing. + * @mod: Pointer to module data. + * @sources: Pointer to audio samples data sources array. + * @num_of_sources: Number of sources in the array. + * @sinks: Pointer to audio samples data sinks array. + * @num_of_sinks: Number of sinks in the array. + * + * Function prepare is called just before the pipeline is started. In + * this case the audio format parameters are for better code performance + * saved to component data to avoid to find out them in process. The + * processing function pointer is set to process the current audio format. + * + * Return: Value zero if success, otherwise error code. + */ +static int phase_vocoder_prepare(struct processing_module *mod, struct sof_source **sources, + int num_of_sources, struct sof_sink **sinks, int num_of_sinks) +{ + struct module_data *mod_priv = &mod->priv; + struct ipc4_base_module_cfg *base_cfg = &mod_priv->cfg.base_cfg; + struct phase_vocoder_comp_data *cd = module_get_private_data(mod); + struct comp_dev *dev = mod->dev; + enum sof_ipc_frame source_format; + int ret; + + comp_dbg(dev, "prepare"); + + /* The processing example in this component supports one input and one + * output. Generally there can be more. + */ + if (num_of_sources != 1 || num_of_sinks != 1) { + comp_err(dev, "Only one source and one sink is supported."); + return -EINVAL; + } + + /* Initialize STFT, max_frames is set to dev->frames + 4 */ + if (!cd->config) { + comp_err(dev, "Can't prepare without bytes control configuration."); + return -EINVAL; + } + + /* get source data format */ + cd->frame_bytes = source_get_frame_bytes(sources[0]); + cd->stream_channels = source_get_channels(sources[0]); + cd->sample_rate = source_get_rate(sources[0]); + + /* Note: dev->frames is zero, use ibs */ + cd->max_input_frames = base_cfg->ibs / cd->frame_bytes + PHASE_VOCODER_MAX_FRAMES_MARGIN; + cd->max_output_frames = base_cfg->obs / cd->frame_bytes + PHASE_VOCODER_MAX_FRAMES_MARGIN; + source_format = source_get_frm_fmt(sources[0]); + comp_info(dev, "source_format %d channels %d max_input_frames %d max_output_frames %d", + source_format, cd->stream_channels, cd->max_input_frames, cd->max_output_frames); + + ret = phase_vocoder_setup(mod); + if (ret < 0) { + comp_err(dev, "setup failed."); + return ret; + } + + cd->phase_vocoder_func = phase_vocoder_find_proc_func(source_format); + if (!cd->phase_vocoder_func) { + comp_err(dev, "No processing function found for format %d.", source_format); + return -EINVAL; + } + + return 0; +} + +/** + * phase_vocoder_reset() - Reset the component. + * @mod: Pointer to module data. + * + * The component reset is called when pipeline is stopped. The reset + * should return the component to same state as init. + * + * Return: Value zero, always success. + */ +static int phase_vocoder_reset(struct processing_module *mod) +{ + comp_dbg(mod->dev, "reset"); + + phase_vocoder_free_buffers(mod); + phase_vocoder_reset_parameters(mod); + return 0; +} + +/** + * phase_vocoder_free() - Free dynamic allocations. + * @mod: Pointer to module data. + * + * Component free is called when the pipelines are deleted. All + * dynamic allocations need to be freed here. The macro __cold + * instructs the build to locate this performance wise non-critical + * function to large and slower DRAM. + * + * Return: Value zero, always success. + */ +__cold static int phase_vocoder_free(struct processing_module *mod) +{ + struct phase_vocoder_comp_data *cd = module_get_private_data(mod); + + assert_can_be_cold(); + + comp_dbg(mod->dev, "free"); + mod_free(mod, cd); + +#if STFT_DEBUG + fclose(stft_debug_fft_in_fh); + fclose(stft_debug_fft_out_fh); + fclose(stft_debug_ifft_out_fh); +#endif + return 0; +} + +/* This defines the module operations */ +static const struct module_interface phase_vocoder_interface = { + .init = phase_vocoder_init, + .prepare = phase_vocoder_prepare, + .process = phase_vocoder_process, + .set_configuration = phase_vocoder_set_config, + .get_configuration = phase_vocoder_get_config, + .reset = phase_vocoder_reset, + .free = phase_vocoder_free}; + +/* This controls build of the module. If COMP_MODULE is selected in kconfig + * this is build as dynamically loadable module. + */ +#if CONFIG_COMP_PHASE_VOCODER_MODULE + +#include +#include +#include + +static const struct sof_man_module_manifest mod_manifest __section(".module") __used = + SOF_LLEXT_MODULE_MANIFEST("PHASE_VOCODER", &phase_vocoder_interface, 1, + SOF_REG_UUID(phase_vocoder), 40); + +SOF_LLEXT_BUILDINFO; + +#else + +DECLARE_MODULE_ADAPTER(phase_vocoder_interface, phase_vocoder_uuid, phase_vocoder_tr); +SOF_MODULE_INIT(phase_vocoder, sys_comp_module_phase_vocoder_interface_init); + +#endif diff --git a/src/audio/phase_vocoder/phase_vocoder.h b/src/audio/phase_vocoder/phase_vocoder.h new file mode 100644 index 000000000000..88c7c9c4279f --- /dev/null +++ b/src/audio/phase_vocoder/phase_vocoder.h @@ -0,0 +1,333 @@ +/* SPDX-License-Identifier: BSD-3-Clause + * + * Copyright(c) 2026 Intel Corporation. + * + */ +#ifndef __SOF_AUDIO_PHASE_VOCODER_H__ +#define __SOF_AUDIO_PHASE_VOCODER_H__ + +#include +#include +#include +#include + +#include +#include + +#if CONFIG_LIBRARY +#define STFT_DEBUG 0 /* Keep zero, produces large files with fprintf() */ +#else +#define STFT_DEBUG 0 +#endif + +#define PHASE_VOCODER_MAX_FRAMES_MARGIN 0 /* Adds to buffer size */ +#define PHASE_VOCODER_MIN_SPEED_Q29 Q_CONVERT_FLOAT(0.5, 29) /* Min. speed is 0.5 */ +#define PHASE_VOCODER_MAX_SPEED_Q29 Q_CONVERT_FLOAT(2.0, 29) /* Max. speed is 2.0 */ +#define PHASE_VOCODER_SPEED_STEP_Q31 Q_CONVERT_FLOAT((2 - 0.5) / 15, 31) /* Steps for enum ctrl */ +#define PHASE_VOCODER_SPEED_NORMAL Q_CONVERT_FLOAT(1.0, 29) /* Default to speed 1 */ +#define PHASE_VOCODER_ONE_Q29 Q_CONVERT_FLOAT(1.0, 29) /* One as Q29 */ +#define PHASE_VOCODER_HALF_Q29 Q_CONVERT_FLOAT(0.5, 29) /* 0.5 as Q29 */ + +#define PHASE_VOCODER_PI_Q28 843314857 /* int32(pi * 2^28), Q28 */ +#define PHASE_VOCODER_TWO_PI_Q28 1686629713 /* int32(2 * pi * 2^28), Q28 */ + +#define PHASE_VOCODER_PI_Q27 421657428 /* int32(pi * 2^27) */ +#define PHASE_VOCODER_TWO_PI_Q27 843314857 /* int32(2 * pi * 2^27) */ + +enum sof_phase_vocoder_fft_pad_type { + STFT_PAD_END = 0, + STFT_PAD_CENTER = 1, + STFT_PAD_START = 2, +}; + +enum sof_phase_vocoder_fft_window_type { + STFT_RECTANGULAR_WINDOW = 0, + STFT_BLACKMAN_WINDOW = 1, + STFT_HAMMING_WINDOW = 2, + STFT_HANN_WINDOW = 3, + STFT_POVEY_WINDOW = 4, +}; + +/** + * struct sof_phase_vocoder_config - IPC configuration blob for phase vocoder + * @size: Size of this struct in bytes + * @reserved: Reserved for future use + * @sample_frequency: Sample rate in Hz, e.g. 16000 + * @window_gain_comp: Q1.31 gain for IFFT + * @reserved_32: Reserved for future use + * @mono: Set to 1 for mono, zero for all channels + * @frame_length: Frame length in samples, e.g. 400 for 25 ms @ 16 kHz + * @frame_shift: Frame shift in samples, e.g. 160 for 10 ms @ 16 kHz + * @reserved_16: Reserved for future use + * @pad: Padding type, use PAD_END, PAD_CENTER, or PAD_START + * @window: Window type, use RECTANGULAR_WINDOW, etc. + */ +struct sof_phase_vocoder_config { + uint32_t size; + uint32_t reserved[8]; + int32_t sample_frequency; + int32_t window_gain_comp; + int32_t reserved_32; + int16_t mono; + int16_t frame_length; + int16_t frame_shift; + int16_t reserved_16; + enum sof_phase_vocoder_fft_pad_type pad; + enum sof_phase_vocoder_fft_window_type window; +} __attribute__((packed)); + +/** + * struct phase_vocoder_buffer - Circular buffer for phase vocoder audio data + * @addr: Start address of the buffer + * @end_addr: End address of the buffer + * @r_ptr: Read pointer + * @w_ptr: Write pointer + * @s_avail: Available samples count + * @s_free: Free samples count + * @s_length: Buffer length in samples for wrap + */ +struct phase_vocoder_buffer { + int32_t *addr; + int32_t *end_addr; + int32_t *r_ptr; + int32_t *w_ptr; + int s_avail; + int s_free; + int s_length; +}; + +/** + * struct phase_vocoder_fft - FFT processing state + * @fft_buf: FFT input buffer, size is fft_size + * @fft_out: FFT output buffer, size is fft_size + * @fft_plan: FFT plan instance + * @ifft_plan: Inverse FFT plan instance + * @fft_size: FFT length in samples + * @fft_hop_size: FFT hop size in samples + * @half_fft_size: Half of the FFT size + * @fft_buffer_size: FFT buffer size in bytes + */ +struct phase_vocoder_fft { + struct icomplex32 *fft_buf; + struct icomplex32 *fft_out; + struct fft_plan *fft_plan; + struct fft_plan *ifft_plan; + int fft_size; + int fft_hop_size; + int half_fft_size; + size_t fft_buffer_size; +}; + +/** + * struct phase_vocoder_polar - Polar domain processing data + * @polar: Current polar representation per channel + * @polar_prev: Previous polar representation per channel + * @polar_tmp: Temporary polar buffer + * @angle_delta_prev: Previous angle delta per channel + * @angle_delta: Current angle delta per channel + * @output_phase: Output phase per channel + */ +struct phase_vocoder_polar { + struct ipolar32 *polar[PLATFORM_MAX_CHANNELS]; + struct ipolar32 *polar_prev[PLATFORM_MAX_CHANNELS]; + struct ipolar32 *polar_tmp; + int32_t *angle_delta_prev[PLATFORM_MAX_CHANNELS]; + int32_t *angle_delta[PLATFORM_MAX_CHANNELS]; + int32_t *output_phase[PLATFORM_MAX_CHANNELS]; +}; + +/** + * struct phase_vocoder_state - Phase vocoder algorithm state + * @ibuf: Circular buffers for input data, per channel + * @obuf: Circular buffers for output data, per channel + * @fft: FFT instance, common for all channels + * @polar: Processing state in polar domain + * @prev_data: Previous frame data per channel, size is prev_data_size + * @buffers: Pointer to allocated memory for all buffers + * @window: Window function coefficients, size is fft_size + * @num_input_fft_to_use: Number of input FFTs to use for processing + * @num_input_fft: Total input FFTs count + * @num_output_ifft: Total output IFFTs count + * @gain_comp: Gain to compensate window gain + * @interpolate_fraction: Q3.29 interpolation coefficient + * @speed: Q3.29 actual render speed + * @prev_data_size: Size of previous data buffer + * @first_output_ifft_done: True after first output IFFT is completed + */ +struct phase_vocoder_state { + struct phase_vocoder_buffer ibuf[PLATFORM_MAX_CHANNELS]; + struct phase_vocoder_buffer obuf[PLATFORM_MAX_CHANNELS]; + struct phase_vocoder_fft fft; + struct phase_vocoder_polar polar; + size_t phase_vocoder_polar_bytes; + int32_t *prev_data[PLATFORM_MAX_CHANNELS]; + int32_t *buffers; + int32_t *window; + int32_t num_input_fft_to_use; + int32_t num_input_fft; + int32_t num_output_ifft; + int32_t gain_comp; + int32_t interpolate_fraction; + int32_t speed; + int prev_data_size; + bool first_output_ifft_done; +}; + +/** + * typedef phase_vocoder_func - Function pointer for process function + * @mod: Pointer to module data. + * @source: Source for PCM samples data. + * @sink: Sink for PCM samples data. + * @source_frames: Number of source audio data frames to process. + * @sink_frames: Number of sink audio data frames to produce. + */ +typedef int (*phase_vocoder_func)(const struct processing_module *mod, struct sof_source *source, + struct sof_sink *sink, uint32_t source_frames, + uint32_t sink_frames); + +/** + * struct phase_vocoder_comp_data - Phase vocoder component data + * @phase_vocoder_func: Pointer to used processing function. + * @state: Phase vocoder algorithm state. + * @config: Configuration blob for the module. + * @mono_mix_coef: Gain for channel for mixing, Q1.15 + * @sample_rate: Audio sample rate in Hz + * @speed_ctrl: Speed Q3.29, allowed range 0.5 to 2.0 + * @speed_enum: Speed control value as enum 0-15 + * @frame_bytes: Number of bytes in an audio frame. + * @max_input_frames: Maximum number of input frames per processing call + * @max_output_frames: Maximum number of output frames per processing call + * @stream_channels: Channels count to consume/produce + * @process_channels: Channels count to process. + * @enable: Control processing on/off, off is pass-through + */ +struct phase_vocoder_comp_data { + phase_vocoder_func phase_vocoder_func; + struct phase_vocoder_state state; + struct sof_phase_vocoder_config *config; + int32_t mono_mix_coef; + int32_t sample_rate; + int32_t speed_ctrl; + int32_t speed_enum; + size_t frame_bytes; + int max_input_frames; + int max_output_frames; + int stream_channels; + int process_channels; + bool enable; +}; + +static inline int phase_vocoder_buffer_samples_without_wrap(struct phase_vocoder_buffer *buffer, + int32_t *ptr) +{ + return buffer->end_addr - ptr; +} + +static inline int32_t *phase_vocoder_buffer_wrap(struct phase_vocoder_buffer *buffer, int32_t *ptr) +{ + if (ptr >= buffer->end_addr) + ptr -= buffer->s_length; + + return ptr; +} + +/** + * struct phase_vocoder_proc_fnmap - processing functions for frame formats + * @frame_fmt: Current frame format + * @phase_vocoder_proc_func: Function pointer for the suitable processing function + */ +struct phase_vocoder_proc_fnmap { + enum sof_ipc_frame frame_fmt; + phase_vocoder_func phase_vocoder_function; +}; + +/** + * phase_vocoder_find_proc_func() - Find suitable processing function. + * @src_fmt: Enum value for PCM format. + * + * This function finds the suitable processing function to use for + * the used PCM format. If not found, return NULL. + * + * Return: Pointer to processing function for the requested PCM format. + */ +phase_vocoder_func phase_vocoder_find_proc_func(enum sof_ipc_frame src_fmt); + +#if CONFIG_IPC_MAJOR_4 +/** + * phase_vocoder_set_config() - Handle controls set + * @mod: Pointer to module data. + * @param_id: Id to know control type, used to know ALSA control type. + * @pos: Position of the fragment in the large message. + * @data_offset_size: Size of the whole configuration if it is the first or only + * fragment. Otherwise it is offset of the fragment. + * @fragment: Message payload data. + * @fragment_size: Size of this fragment. + * @response_size: Size of response. + * + * This function handles the real-time controls. The ALSA controls have the + * param_id set to indicate the control type. The control ID, from topology, + * is used to separate the controls instances of same type. In control payload + * the num_elems defines to how many channels the control is applied to. + * + * Return: Zero if success, otherwise error code. + */ +int phase_vocoder_set_config(struct processing_module *mod, uint32_t param_id, + enum module_cfg_fragment_position pos, uint32_t data_offset_size, + const uint8_t *fragment, size_t fragment_size, uint8_t *response, + size_t response_size); +/** + * phase_vocoder_get_config() - Handle controls get + * @mod: Pointer to module data. + * @config_id: Configuration ID. + * @data_offset_size: Size of the whole configuration if it is the first or only + * fragment. Otherwise it is offset of the fragment. + * @fragment: Message payload data. + * @fragment_size: Size of this fragment. + * + * This function is used for controls get. + * + * Return: Zero if success, otherwise error code. + */ +int phase_vocoder_get_config(struct processing_module *mod, uint32_t config_id, + uint32_t *data_offset_size, uint8_t *fragment, size_t fragment_size); +#else +static inline int phase_vocoder_set_config(struct processing_module *mod, uint32_t param_id, + enum module_cfg_fragment_position pos, + uint32_t data_offset_size, const uint8_t *fragment, + size_t fragment_size, uint8_t *response, + size_t response_size) +{ + return 0; +} + +static inline int phase_vocoder_get_config(struct processing_module *mod, uint32_t config_id, + uint32_t *data_offset_size, uint8_t *fragment, + size_t fragment_size) +{ + return 0; +} +#endif + +void phase_vocoder_apply_window(struct phase_vocoder_state *state); + +void phase_vocoder_fill_fft_buffer(struct phase_vocoder_state *state, int ch); + +void phase_vocoder_free_buffers(struct processing_module *mod); + +int phase_vocoder_overlap_add_ifft_buffer(struct phase_vocoder_state *state, int ch); + +void phase_vocoder_reset_for_new_speed(struct phase_vocoder_comp_data *cd); + +int phase_vocoder_setup(struct processing_module *mod); + +int phase_vocoder_sink_s16(struct phase_vocoder_comp_data *cd, struct sof_sink *sink, int frames); + +int phase_vocoder_sink_s32(struct phase_vocoder_comp_data *cd, struct sof_sink *sink, int frames); + +int phase_vocoder_source_s16(struct phase_vocoder_comp_data *cd, struct sof_source *source, + int frames); + +int phase_vocoder_source_s32(struct phase_vocoder_comp_data *cd, struct sof_source *source, + int frames); + +#endif // __SOF_AUDIO_PHASE_VOCODER_H__ diff --git a/src/audio/phase_vocoder/phase_vocoder.toml b/src/audio/phase_vocoder/phase_vocoder.toml new file mode 100644 index 000000000000..9ec249fe01a5 --- /dev/null +++ b/src/audio/phase_vocoder/phase_vocoder.toml @@ -0,0 +1,21 @@ +#ifndef LOAD_TYPE +#define LOAD_TYPE "0" +#endif + + REM # Template component module config + [[module.entry]] + name = "PHASEVOC" + uuid = UUIDREG_STR_PHASE_VOCODER + affinity_mask = "0x1" + instance_count = "40" + domain_types = "0" + load_type = LOAD_TYPE + module_type = "9" + auto_start = "0" + sched_caps = [1, 0x00008000] + REM # pin = [dir, type, sample rate, size, container, channel-cfg] + pin = [0, 0, 0xfeef, 0xf, 0xf, 0x45ff, 1, 0, 0xfeef, 0xf, 0xf, 0x1ff] + REM # mod_cfg [PAR_0 PAR_1 PAR_2 PAR_3 IS_BYTES CPS IBS OBS MOD_FLAGS CPC OBLS] + mod_cfg = [0, 0, 0, 0, 4096, 1000000, 128, 128, 0, 0, 0] + + index = __COUNTER__ diff --git a/src/audio/phase_vocoder/phase_vocoder_common.c b/src/audio/phase_vocoder/phase_vocoder_common.c new file mode 100644 index 000000000000..c9dd201254fd --- /dev/null +++ b/src/audio/phase_vocoder/phase_vocoder_common.c @@ -0,0 +1,510 @@ +// SPDX-License-Identifier: BSD-3-Clause +// +// Copyright(c) 2026 Intel Corporation. + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "phase_vocoder.h" + +#include +#include +#include + +#if STFT_DEBUG +extern FILE *stft_debug_fft_in_fh; +extern FILE *stft_debug_fft_out_fh; +extern FILE *stft_debug_ifft_out_fh; + +static void debug_print_to_file_real(FILE *fh, struct icomplex32 *c, int n) +{ + for (int i = 0; i < n; i++) + fprintf(fh, "%d\n", c[i].real); +} + +static void debug_print_to_file_complex(FILE *fh, struct icomplex32 *c, int n) +{ + for (int i = 0; i < n; i++) + fprintf(fh, "%d %d\n", c[i].real, c[i].imag); +} +#endif + +LOG_MODULE_REGISTER(phase_vocoder_common, CONFIG_SOF_LOG_LEVEL); + +/* + * The main processing function for PHASE_VOCODER + */ + +static int stft_get_num_ffts_avail(struct phase_vocoder_state *state, int channel) +{ + struct phase_vocoder_buffer *ibuf = &state->ibuf[channel]; + struct phase_vocoder_fft *fft = &state->fft; + + /* Wait for FFT hop size of new data */ + return ibuf->s_avail / fft->fft_hop_size; +} + +static void stft_do_fft(struct phase_vocoder_state *state, int ch) +{ + struct phase_vocoder_fft *fft = &state->fft; + + /* Copy data to FFT input buffer from overlap buffer and from new samples buffer */ + phase_vocoder_fill_fft_buffer(state, ch); + + /* Window function */ + phase_vocoder_apply_window(state); + +#if STFT_DEBUG + debug_print_to_file_real(stft_debug_fft_in_fh, fft->fft_buf, fft->fft_size); +#endif + + /* Compute FFT. A full scale s16 sine input with 2^N samples period in low + * part of s32 real part and zero imaginary part gives to output about 0.5 + * full scale 32 bit output to real and imaginary. The scaling is same for + * all FFT sizes. + */ + fft_execute_32(fft->fft_plan, false); + +#if STFT_DEBUG + debug_print_to_file_complex(stft_debug_fft_out_fh, fft->fft_out, fft->fft_size); +#endif +} + +static int stft_do_ifft(struct phase_vocoder_state *state, int ch) +{ + struct phase_vocoder_fft *fft = &state->fft; + + /* Compute IFFT */ + fft_execute_32(fft->ifft_plan, true); + +#if STFT_DEBUG + debug_print_to_file_complex(stft_debug_ifft_out_fh, fft->fft_buf, fft->fft_size); +#endif + + /* Window function */ + phase_vocoder_apply_window(state); + + /* Copy to output buffer */ + return phase_vocoder_overlap_add_ifft_buffer(state, ch); +} + +/** + * stft_convert_to_polar() - Convert FFT output to polar format + * @fft: FFT state containing the FFT output buffer and parameters as Q1.31. + * @polar_data: Output buffer for polar data, size is half of FFT size. + * Magnitude is Q2.30 and phase is Q5.27. + */ +static void stft_convert_to_polar(struct phase_vocoder_fft *fft, struct ipolar32 *polar_data) +{ + int i; + + /* Get magnitude and phase, convert phase from Q3.29 to Q5.27. */ + for (i = 0; i < fft->half_fft_size; i++) { + sofm_icomplex32_to_polar(&fft->fft_out[i], &polar_data[i]); + polar_data[i].angle = Q_SHIFT_RND(polar_data[i].angle, 29, 27); + } +} + +/** + * stft_convert_to_complex() - Convert polar data back to complex format for IFFT + * @polar_data: Input polar data, size is half of FFT size. + * Magnitude is Q2.30 and phase is Q5.27. + * @fft: FFT state containing the output buffer where complex data will + * be stored and parameters as Q1.31. + */ +static void stft_convert_to_complex(struct ipolar32 *polar_data, struct phase_vocoder_fft *fft) +{ + int i; + + /* Convert phase from Q5.27 to Q3.29, and then to (re, im) complex format as Q1.31*/ + for (i = 0; i < fft->half_fft_size; i++) { + polar_data[i].angle = Q_SHIFT_LEFT(polar_data[i].angle, 27, 29); + sofm_ipolar32_to_complex(&polar_data[i], &fft->fft_out[i]); + } +} + +static void stft_apply_fft_symmetry(struct phase_vocoder_fft *fft) +{ + int i, j, k; + + j = 2 * fft->half_fft_size - 2; + for (i = fft->half_fft_size; i < fft->fft_size; i++) { + k = j - i; + fft->fft_out[i].real = fft->fft_out[k].real; + fft->fft_out[i].imag = -fft->fft_out[k].imag; + } +} + +static void phase_vocoder_interpolation_parameters(struct phase_vocoder_comp_data *cd) +{ + struct phase_vocoder_state *state = &cd->state; + int64_t input_frame_num_frac; + int32_t input_frame_num_floor; + + input_frame_num_frac = (int64_t)state->num_output_ifft * cd->state.speed; /* Q31.29 */ + input_frame_num_floor = (int32_t)(input_frame_num_frac >> 29); /* Use floor, not round */ + state->num_input_fft_to_use = input_frame_num_floor + 1; + state->interpolate_fraction = input_frame_num_frac - ((int64_t)input_frame_num_floor << 29); +} + +#if 0 +static int32_t unwrap_angle(int32_t angle) +{ + if (angle > PHASE_VOCODER_PI_Q28) + return angle - PHASE_VOCODER_TWO_PI_Q28; + else if (angle < -PHASE_VOCODER_PI_Q28) + return angle + PHASE_VOCODER_TWO_PI_Q28; + else + return angle; +} +#endif + +static int32_t unwrap_angle_q27(int32_t angle) +{ + while (angle > PHASE_VOCODER_PI_Q27) + angle -= PHASE_VOCODER_TWO_PI_Q27; + + while (angle < -PHASE_VOCODER_PI_Q27) + angle += PHASE_VOCODER_TWO_PI_Q27; + + return angle; +} + +void phase_vocoder_reset_for_new_speed(struct phase_vocoder_comp_data *cd) +{ + struct phase_vocoder_state *state = &cd->state; + + state->speed = cd->speed_ctrl; + + /* Keep num_input_fft at 1, not 0, to avoid re-entering the first + * analysis FFT cold-start path. That path sets angle_delta to the + * absolute phase angle (not a delta between frames). For non-unity + * speeds, multiple IFFTs are produced per input FFT, and the + * absolute angle gets repeatedly accumulated into output_phase + * causing random inter-bin phase incoherence and gain variation. + * + * By setting num_input_fft = 1 the existing polar, angle_delta, + * and output_phase state from the last processing step is preserved. + * These contain proper phase deltas, so interpolation continues + * correctly at the new speed. + */ + state->num_input_fft = 1; + state->num_output_ifft = 0; +} + +static void copy_polar_angles(int32_t *angle_delta_ch, struct ipolar32 *polar_data_ch, + int num_angles) +{ + int i; + + for (i = 0; i < num_angles; i++) + angle_delta_ch[i] = polar_data_ch[i].angle; +} + +// TODO: Civilized input and output counters reset to prevent int32_t wrap +static int stft_do_fft_ifft(const struct processing_module *mod) +{ + struct phase_vocoder_comp_data *cd = module_get_private_data(mod); + struct phase_vocoder_state *state = &cd->state; + struct phase_vocoder_polar *polar = &state->polar; + struct phase_vocoder_fft *fft = &state->fft; + struct ipolar32 *polar_data_prev_ch; + struct ipolar32 *polar_data_ch; + int32_t *angle_delta_prev_ch; + int32_t *angle_delta_ch; + int32_t *output_phase_ch; + int32_t one_minus_frac; + int32_t frac; + int32_t p1, p2; + int32_t a; + const size_t polar_fft_half_bytes = sizeof(struct ipolar32) * fft->half_fft_size; + const size_t int32_fft_half_bytes = sizeof(int32_t) * fft->half_fft_size; + int num_fft; + int ret = 0; + int ch; + int i; + + num_fft = stft_get_num_ffts_avail(state, 0); + if (!num_fft) + return 0; + + /* First analysis FFT */ + if (!state->num_input_fft) { + for (ch = 0; ch < cd->process_channels; ch++) { + stft_do_fft(state, ch); + + /* Convert half-FFT to polar. Magnitude is Q2.30 and phase + * angle is Q5.27. + */ + polar_data_ch = polar->polar[ch]; /* struct ipolar32 */ + stft_convert_to_polar(&state->fft, polar_data_ch); + + /* Copy the first phase angle to angle delta */ + angle_delta_ch = polar->angle_delta[ch]; /* int32_t */ + copy_polar_angles(angle_delta_ch, polar_data_ch, fft->half_fft_size); + + /* Initialize prev to same data so interpolation works + * correctly before second FFT is consumed. + */ + memcpy(polar->polar_prev[ch], polar_data_ch, polar_fft_half_bytes); + memcpy(polar->angle_delta_prev[ch], angle_delta_ch, int32_fft_half_bytes); + } + state->num_input_fft++; + num_fft--; + } + + phase_vocoder_interpolation_parameters(cd); + while (state->num_input_fft < state->num_input_fft_to_use && num_fft > 0) { + for (ch = 0; ch < cd->process_channels; ch++) { + stft_do_fft(state, ch); + + /* Update previous polar data. + * Note: Not using memcpy_s() since this is hot algorithm code. + */ + polar_data_prev_ch = polar->polar_prev[ch]; + polar_data_ch = polar->polar[ch]; + memcpy(polar_data_prev_ch, polar_data_ch, polar_fft_half_bytes); + + /* Convert half-FFT to polar. Magnitude is Q2.30 and phase + * angle is Q3.29. + */ + stft_convert_to_polar(&state->fft, polar_data_ch); + + /* Update previous delta phase data */ + angle_delta_ch = polar->angle_delta[ch]; + angle_delta_prev_ch = polar->angle_delta_prev[ch]; + memcpy(angle_delta_prev_ch, angle_delta_ch, int32_fft_half_bytes); + + /* Calculate new delta phase */ + for (i = 0; i < fft->half_fft_size; i++) { + a = polar_data_ch[i].angle - polar_data_prev_ch[i].angle; + angle_delta_ch[i] = unwrap_angle_q27(a); + } + } + state->num_input_fft++; + num_fft--; + } + + if (state->num_input_fft < state->num_input_fft_to_use) + return 0; + + /* Interpolate IFFT frame */ + frac = state->interpolate_fraction; + one_minus_frac = PHASE_VOCODER_ONE_Q29 - frac; + + for (ch = 0; ch < cd->process_channels; ch++) { + polar_data_prev_ch = polar->polar_prev[ch]; + polar_data_ch = polar->polar[ch]; + angle_delta_ch = polar->angle_delta[ch]; + angle_delta_prev_ch = polar->angle_delta_prev[ch]; + output_phase_ch = polar->output_phase[ch]; + + for (i = 0; i < fft->half_fft_size; i++) { + p1 = Q_MULTSR_32X32((int64_t)one_minus_frac, + polar_data_prev_ch[i].magnitude, 29, 30, 30); + p2 = Q_MULTSR_32X32((int64_t)frac, polar_data_ch[i].magnitude, 29, 30, 30); + polar->polar_tmp[i].magnitude = p1 + p2; + + a = output_phase_ch[i]; + p1 = Q_MULTSR_32X32((int64_t)one_minus_frac, angle_delta_prev_ch[i], 29, 27, + 27); + p2 = Q_MULTSR_32X32((int64_t)frac, angle_delta_ch[i], 29, 27, 27); + a = output_phase_ch[i] + p1 + p2; + output_phase_ch[i] = unwrap_angle_q27(a); + polar->polar_tmp[i].angle = output_phase_ch[i]; + } + + /* Convert back to (re, im) complex, and fix upper part */ + stft_convert_to_complex(polar->polar_tmp, &state->fft); + stft_apply_fft_symmetry(&state->fft); + ret = stft_do_ifft(state, ch); + if (ret) { + comp_err(mod->dev, "IFFT failure, check output overlap-add buffer size"); + return ret; + } + } + + comp_dbg(mod->dev, "no = %d, ni = %d, frac = %d", state->num_output_ifft, + state->num_input_fft, frac); + state->num_output_ifft++; + state->first_output_ifft_done = true; + return 0; +} + +static int phase_vocoder_check_fft_run_need(struct phase_vocoder_comp_data *cd) +{ + return cd->state.obuf[0].s_avail < cd->state.fft.fft_hop_size; +} + +#if CONFIG_FORMAT_S32LE +static int phase_vocoder_output_zeros_s32(struct phase_vocoder_comp_data *cd, struct sof_sink *sink, + int frames) +{ + int32_t *y, *y_start, *y_end; + int samples = frames * cd->stream_channels; + size_t bytes = samples * sizeof(int32_t); + int samples_without_wrap; + int y_size; + int ret; + + /* Get pointer to sink data in circular buffer, buffer start and size. */ + ret = sink_get_buffer_s32(sink, bytes, &y, &y_start, &y_size); + if (ret) + return ret; + + /* Set helper pointers to buffer end for wrap check. Then loop until all + * samples are processed. + */ + y_end = y_start + y_size; + while (samples) { + /* Find out samples to process before first wrap or end of data. */ + samples_without_wrap = y_end - y; + samples_without_wrap = MIN(samples_without_wrap, samples); + memset(y, 0, samples_without_wrap * sizeof(int32_t)); + y += samples_without_wrap; + + /* Check for wrap */ + if (y >= y_end) + y -= y_size; + + /* Update processed samples count for next loop iteration. */ + samples -= samples_without_wrap; + } + + /* Update the source and sink for bytes consumed and produced. Return success. */ + sink_commit_buffer(sink, bytes); + return 0; +} + +static int phase_vocoder_s32(const struct processing_module *mod, struct sof_source *source, + struct sof_sink *sink, uint32_t source_frames, uint32_t sink_frames) +{ + struct phase_vocoder_comp_data *cd = module_get_private_data(mod); + int ret; + + if (phase_vocoder_check_fft_run_need(cd)) { + /* Get samples from source buffer */ + phase_vocoder_source_s32(cd, source, source_frames); + + /* Do STFT, processing and inverse STFT */ + ret = stft_do_fft_ifft(mod); + if (ret) + return ret; + } + + /* Get samples from source buffer */ + if (cd->state.first_output_ifft_done) + ret = phase_vocoder_sink_s32(cd, sink, sink_frames); + else + ret = phase_vocoder_output_zeros_s32(cd, sink, sink_frames); + + return ret; +} +#endif /* CONFIG_FORMAT_S32LE */ + +#if CONFIG_FORMAT_S16LE +static int phase_vocoder_output_zeros_s16(struct phase_vocoder_comp_data *cd, struct sof_sink *sink, + int frames) +{ + int16_t *y, *y_start, *y_end; + int samples = frames * cd->stream_channels; + size_t bytes = samples * sizeof(int16_t); + int samples_without_wrap; + int y_size; + int ret; + + /* Get pointer to sink data in circular buffer, buffer start and size. */ + ret = sink_get_buffer_s16(sink, bytes, &y, &y_start, &y_size); + if (ret) + return ret; + + /* Set helper pointers to buffer end for wrap check. Then loop until all + * samples are processed. + */ + y_end = y_start + y_size; + while (samples) { + /* Find out samples to process before first wrap or end of data. */ + samples_without_wrap = y_end - y; + samples_without_wrap = MIN(samples_without_wrap, samples); + memset(y, 0, samples_without_wrap * sizeof(int16_t)); + y += samples_without_wrap; + + /* Check for wrap */ + if (y >= y_end) + y -= y_size; + + /* Update processed samples count for next loop iteration. */ + samples -= samples_without_wrap; + } + + /* Update the source and sink for bytes consumed and produced. Return success. */ + sink_commit_buffer(sink, bytes); + return 0; +} + +static int phase_vocoder_s16(const struct processing_module *mod, struct sof_source *source, + struct sof_sink *sink, uint32_t source_frames, uint32_t sink_frames) +{ + struct phase_vocoder_comp_data *cd = module_get_private_data(mod); + int ret; + + if (phase_vocoder_check_fft_run_need(cd)) { + /* Get samples from source buffer */ + phase_vocoder_source_s16(cd, source, source_frames); + + /* Do STFT, processing and inverse STFT */ + ret = stft_do_fft_ifft(mod); + if (ret) + return ret; + } + + /* Get samples from source buffer */ + if (cd->state.first_output_ifft_done) + ret = phase_vocoder_sink_s16(cd, sink, sink_frames); + else + ret = phase_vocoder_output_zeros_s16(cd, sink, sink_frames); + + return ret; +} +#endif /* CONFIG_FORMAT_S16LE */ + +#if CONFIG_FORMAT_S24LE +#endif /* CONFIG_FORMAT_S24LE */ + +/* This struct array defines the used processing functions for + * the PCM formats + */ +const struct phase_vocoder_proc_fnmap phase_vocoder_functions[] = { +#if CONFIG_FORMAT_S16LE + {SOF_IPC_FRAME_S16_LE, phase_vocoder_s16}, + {SOF_IPC_FRAME_S32_LE, phase_vocoder_s32}, +#endif +}; + +/** + * phase_vocoder_find_proc_func() - Find suitable processing function. + * @src_fmt: Enum value for PCM format. + * + * This function finds the suitable processing function to use for + * the used PCM format. If not found, return NULL. + * + * Return: Pointer to processing function for the requested PCM format. + */ +phase_vocoder_func phase_vocoder_find_proc_func(enum sof_ipc_frame src_fmt) +{ + int i; + + /* Find suitable processing function from map */ + for (i = 0; i < ARRAY_SIZE(phase_vocoder_functions); i++) + if (src_fmt == phase_vocoder_functions[i].frame_fmt) + return phase_vocoder_functions[i].phase_vocoder_function; + + return NULL; +} diff --git a/src/audio/phase_vocoder/phase_vocoder_setup.c b/src/audio/phase_vocoder/phase_vocoder_setup.c new file mode 100644 index 000000000000..80aa5bfa56aa --- /dev/null +++ b/src/audio/phase_vocoder/phase_vocoder_setup.c @@ -0,0 +1,259 @@ +// SPDX-License-Identifier: BSD-3-Clause +// +// Copyright(c) 2026 Intel Corporation. + +#include +#include +#include +#include +#include +#include +#include +#include "phase_vocoder.h" + +#include +#include +#include + +/* Definitions for cepstral lifter */ +#define PI_Q23 Q_CONVERT_FLOAT(3.1415926536, 23) +#define TWO_PI_Q23 Q_CONVERT_FLOAT(6.2831853072, 23) +#define ONE_Q9 Q_CONVERT_FLOAT(1, 9) + +#define STFT_MAX_ALLOC_SIZE 65536 + +LOG_MODULE_REGISTER(phase_vocoder_setup, CONFIG_SOF_LOG_LEVEL); + +static void phase_vocoder_init_buffer(struct phase_vocoder_buffer *buf, int32_t *base, int size) +{ + buf->addr = base; + buf->end_addr = base + size; + buf->r_ptr = base; + buf->w_ptr = base; + buf->s_free = size; + buf->s_avail = 0; + buf->s_length = size; +} + +static int phase_vocoder_get_window(struct phase_vocoder_state *state, + enum sof_phase_vocoder_fft_window_type name) +{ + struct phase_vocoder_fft *fft = &state->fft; + + switch (name) { + case STFT_RECTANGULAR_WINDOW: + win_rectangular_32b(state->window, fft->fft_size); + return 0; + case STFT_BLACKMAN_WINDOW: + win_blackman_32b(state->window, fft->fft_size, WIN_BLACKMAN_A0_Q31); + return 0; + case STFT_HAMMING_WINDOW: + win_hamming_32b(state->window, fft->fft_size); + return 0; + case STFT_HANN_WINDOW: + win_hann_32b(state->window, fft->fft_size); + return 0; + + default: + return -EINVAL; + } +} + +/* TODO phase_vocoder setup needs to use the config blob, not hard coded parameters. + * Also this is a too long function. Split to STFT, Mel filter, etc. parts. + */ +int phase_vocoder_setup(struct processing_module *mod) +{ + struct phase_vocoder_comp_data *cd = module_get_private_data(mod); + struct comp_dev *dev = mod->dev; + struct sof_phase_vocoder_config *config = cd->config; + struct phase_vocoder_state *state = &cd->state; + struct phase_vocoder_fft *fft = &state->fft; + struct phase_vocoder_polar *polar = &state->polar; + size_t sample_buffers_size; + size_t ibuf_size; + size_t obuf_size; + size_t prev_size; + int32_t *addr; + int channels; + int ret; + int i; + + comp_dbg(dev, "phase_vocoder_setup()"); + + /* Check size */ + if (config->size != sizeof(struct sof_phase_vocoder_config)) { + comp_err(dev, "Illegal configuration size %d.", config->size); + return -EINVAL; + } + + if (config->sample_frequency != cd->sample_rate) + comp_warn(dev, "Config sample_frequency does not match stream"); + + if (config->mono) { + cd->process_channels = 1; + cd->mono_mix_coef = (int32_t)((1LL << 31) / cd->stream_channels); /* Q1.31 */ + } else { + cd->process_channels = cd->stream_channels; + cd->mono_mix_coef = 0; + } + + comp_info(dev, "mono_mix_coef %d", cd->mono_mix_coef); + + fft->fft_size = config->frame_length; + fft->fft_hop_size = config->frame_shift; + fft->half_fft_size = (fft->fft_size >> 1) + 1; + + comp_info(dev, "fft_size = %d, fft_hop_size = %d, window = %d", fft->fft_size, + fft->fft_hop_size, config->window); + + /* Calculated parameters */ + prev_size = fft->fft_size - fft->fft_hop_size; + ibuf_size = fft->fft_hop_size + cd->max_input_frames; + obuf_size = fft->fft_size + fft->fft_hop_size; + state->prev_data_size = prev_size; + + /* Allocate buffer input samples, overlap buffer, window */ + channels = cd->process_channels; + sample_buffers_size = + sizeof(int32_t) * channels * (ibuf_size + obuf_size + prev_size + fft->fft_size); + + if (sample_buffers_size > STFT_MAX_ALLOC_SIZE || sample_buffers_size < 0) { + comp_err(dev, "Illegal allocation size"); + return -EINVAL; + } + + addr = mod_balloc(mod, sample_buffers_size); + if (!addr) { + comp_err(dev, "Failed buffer allocate"); + ret = -ENOMEM; + goto exit; + } + + memset(addr, 0, sample_buffers_size); + state->buffers = addr; + for (i = 0; i < channels; i++) { + phase_vocoder_init_buffer(&state->ibuf[i], addr, ibuf_size); + addr += ibuf_size; + phase_vocoder_init_buffer(&state->obuf[i], addr, obuf_size); + addr += obuf_size; + state->prev_data[i] = addr; + addr += prev_size; + } + state->window = addr; + + /* Allocate buffers for FFT input and output data */ + fft->fft_buffer_size = fft->fft_size * sizeof(struct icomplex32); + fft->fft_buf = mod_balloc(mod, fft->fft_buffer_size); + if (!fft->fft_buf) { + comp_err(dev, "Failed FFT buffer allocate"); + ret = -ENOMEM; + goto free_buffers; + } + + fft->fft_out = mod_balloc(mod, fft->fft_buffer_size); + if (!fft->fft_out) { + comp_err(dev, "Failed FFT output allocate"); + ret = -ENOMEM; + goto free_fft_buf; + } + + /* Setup FFT */ + fft->fft_plan = mod_fft_plan_new(mod, fft->fft_buf, fft->fft_out, fft->fft_size, 32); + if (!fft->fft_plan) { + comp_err(dev, "Failed FFT init"); + ret = -EINVAL; + goto free_fft_out; + } + + fft->ifft_plan = mod_fft_plan_new(mod, fft->fft_out, fft->fft_buf, fft->fft_size, 32); + if (!fft->ifft_plan) { + comp_err(dev, "Failed IFFT init"); + ret = -EINVAL; + goto free_ifft_out; + } + + /* Setup window */ + ret = phase_vocoder_get_window(state, config->window); + if (ret < 0) { + comp_err(dev, "Failed Window function"); + goto free_window_out; + } + + /* Need to compensate the window function gain */ + state->gain_comp = config->window_gain_comp; + + /* Allocate buffers for polar format data for magnitude and phase interpolation */ + state->phase_vocoder_polar_bytes = + channels * fft->half_fft_size * (2 * sizeof(struct ipolar32) + 3 * sizeof(int32_t)); + comp_info(dev, "polar buffers size %d", state->phase_vocoder_polar_bytes); + addr = mod_balloc(mod, state->phase_vocoder_polar_bytes); + if (!addr) { + comp_err(dev, "Failed polar data buffer allocate"); + ret = -ENOMEM; + goto free_window_out; + } + + memset(addr, 0, state->phase_vocoder_polar_bytes); + for (i = 0; i < channels; i++) { + polar->polar[i] = (struct ipolar32 *)addr; + addr = (int32_t *)((struct ipolar32 *)addr + fft->half_fft_size); + } + + for (i = 0; i < channels; i++) { + polar->polar_prev[i] = (struct ipolar32 *)addr; + addr = (int32_t *)((struct ipolar32 *)addr + fft->half_fft_size); + } + + for (i = 0; i < channels; i++) { + polar->angle_delta[i] = addr; + addr += fft->half_fft_size; + } + + for (i = 0; i < channels; i++) { + polar->angle_delta_prev[i] = addr; + addr += fft->half_fft_size; + } + + for (i = 0; i < channels; i++) { + polar->output_phase[i] = addr; + addr += fft->half_fft_size; + } + + /* Use FFT buffer as scratch */ + polar->polar_tmp = (struct ipolar32 *)fft->fft_out; + comp_dbg(dev, "phase_vocoder_setup(), done"); + return 0; + +free_window_out: + mod_free(mod, fft->ifft_plan); + +free_ifft_out: + mod_free(mod, fft->fft_plan); + +free_fft_out: + mod_free(mod, fft->fft_out); + +free_fft_buf: + mod_free(mod, fft->fft_buf); + +free_buffers: + mod_free(mod, state->buffers); + +exit: + return ret; +} + +void phase_vocoder_free_buffers(struct processing_module *mod) +{ + struct phase_vocoder_comp_data *cd = module_get_private_data(mod); + struct phase_vocoder_state *state = &cd->state; + struct phase_vocoder_fft *fft = &state->fft; + + mod_fft_plan_free(mod, fft->ifft_plan); + mod_fft_plan_free(mod, fft->fft_plan); + mod_free(mod, cd->state.fft.fft_buf); + mod_free(mod, cd->state.fft.fft_out); + mod_free(mod, cd->state.buffers); + mod_free(mod, cd->state.polar.polar[0]); +} diff --git a/src/include/sof/audio/component.h b/src/include/sof/audio/component.h index b6695b9cd312..db7ae4407c1a 100644 --- a/src/include/sof/audio/component.h +++ b/src/include/sof/audio/component.h @@ -963,6 +963,7 @@ void sys_comp_module_mixout_interface_init(void); void sys_comp_module_multiband_drc_interface_init(void); void sys_comp_module_mux_interface_init(void); void sys_comp_module_nxp_eap_interface_init(void); +void sys_comp_module_phase_vocoder_interface_init(void); void sys_comp_module_rtnr_interface_init(void); void sys_comp_module_selector_interface_init(void); void sys_comp_module_sound_dose_interface_init(void); diff --git a/tools/rimage/config/lnl.toml.h b/tools/rimage/config/lnl.toml.h index 7a574d4ae906..faefbb8acadb 100644 --- a/tools/rimage/config/lnl.toml.h +++ b/tools/rimage/config/lnl.toml.h @@ -162,5 +162,9 @@ #include