From acbb8479bf7dbcb814a0a779b8c4e2b47a655033 Mon Sep 17 00:00:00 2001 From: Seppo Ingalsuo Date: Thu, 15 Jan 2026 22:31:14 +0200 Subject: [PATCH 1/7] 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 be9c9f0747d6312880f7446859dc9589e0f3cde1 Mon Sep 17 00:00:00 2001 From: Seppo Ingalsuo Date: Mon, 26 Jan 2026 12:32:08 +0200 Subject: [PATCH 2/7] 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 122b08b1a9418d1b126811817dd30ee5aca08706 Mon Sep 17 00:00:00 2001 From: Seppo Ingalsuo Date: Mon, 26 Jan 2026 14:19:57 +0200 Subject: [PATCH 3/7] 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 4e438f35db49810505c096e6fabccc2c86822497 Mon Sep 17 00:00:00 2001 From: Seppo Ingalsuo Date: Mon, 26 Jan 2026 14:42:37 +0200 Subject: [PATCH 4/7] 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 38ec2da00c3f4b245d2f4d201b73beb6888c162e Mon Sep 17 00:00:00 2001 From: Seppo Ingalsuo Date: Tue, 10 Feb 2026 20:14:26 +0200 Subject: [PATCH 5/7] 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..fc3e3d422871 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 10 /**< Convert with right shift a bytes count to samples count */ #define FILE_BYTES_TO_S16_SAMPLES(s) ((s) >> 1) From 21998427e78694b5d7cf07c81fde35efca567d4d Mon Sep 17 00:00:00 2001 From: Seppo Ingalsuo Date: Fri, 30 Jan 2026 16:45:31 +0200 Subject: [PATCH 6/7] 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_256_128.conf | 17 +++ .../phase_vocoder/hann_512_128.conf | 17 +++ .../phase_vocoder/hann_512_256.conf | 17 +++ 18 files changed, 405 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_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_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 e81696902668a3ebc43548bc33a8784f73d1a69f Mon Sep 17 00:00:00 2001 From: Seppo Ingalsuo Date: Fri, 30 Jan 2026 15:21:05 +0200 Subject: [PATCH 7/7] 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 | 446 +++++++++++++++++ src/audio/phase_vocoder/phase_vocoder-ipc4.c | 89 ++++ src/audio/phase_vocoder/phase_vocoder.c | 277 +++++++++++ src/audio/phase_vocoder/phase_vocoder.h | 262 ++++++++++ src/audio/phase_vocoder/phase_vocoder.toml | 21 + .../phase_vocoder/phase_vocoder_common.c | 458 ++++++++++++++++++ src/audio/phase_vocoder/phase_vocoder_setup.c | 267 ++++++++++ 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, 1894 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..7cf9d3a8388e --- /dev/null +++ b/src/audio/phase_vocoder/phase_vocoder-generic.c @@ -0,0 +1,446 @@ +// 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; + int frames_left; + int x_size; + int bytes; + int ret; + int n1; + int n2; + int n; + int i; + int j; + int channels = cd->channels; + + 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) / channels; + n2 = phase_vocoder_buffer_samples_without_wrap(ibuf, ibuf->w_ptr); + n = MIN(n1, n2); + n = MIN(n, frames_left); + for (i = 0; i < n; i++) { + for (j = 0; j < 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 < 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 < 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 channels = cd->channels; + int bytes; + int y_size; + int ret; + int ch, n1, n, i; + + 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) / cd->channels; + n = phase_vocoder_buffer_samples_without_wrap(obuf, obuf->r_ptr); + n = MIN(n1, n); + n = MIN(n, frames_remain); + + for (i = 0; i < n; i++) { + for (ch = 0; ch < 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 < 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 < 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; + int16_t const *x, *x_start, *x_end; + int16_t in; + int x_size; + int channels = cd->channels; + int bytes; + + ibuf = &state->ibuf[0]; + frames = MIN(frames, ibuf->s_free); + bytes = frames * cd->frame_bytes; + int frames_left = frames; + int ret; + int n1; + int n2; + int n; + int i; + int j; + + /* 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) / cd->channels; + n2 = phase_vocoder_buffer_samples_without_wrap(ibuf, ibuf->w_ptr); + n = MIN(n1, n2); + n = MIN(n, frames_left); + for (i = 0; i < n; i++) { + for (j = 0; j < 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 < 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 < 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 *y, *y_start, *y_end; + int frames_remain; + int channels = cd->channels; + int y_size; + int bytes; + int ret; + int ch, n1, n, i; + + 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) / cd->channels; + n = phase_vocoder_buffer_samples_without_wrap(obuf, obuf->r_ptr); + n = MIN(n1, n); + n = MIN(n, frames_remain); + + for (i = 0; i < n; i++) { + for (ch = 0; ch < 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 < 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 < 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..2d340c0f2eeb --- /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..3bfa9173b321 --- /dev/null +++ b/src/audio/phase_vocoder/phase_vocoder.c @@ -0,0 +1,277 @@ +// 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)); + 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->channels = source_get_channels(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->channels, cd->max_input_frames, cd->max_output_frames); + + ret = + phase_vocoder_setup(mod, source_get_rate(sources[0]), source_get_channels(sources[0])); + 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..07583e31f5a0 --- /dev/null +++ b/src/audio/phase_vocoder/phase_vocoder.h @@ -0,0 +1,262 @@ +/* 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 2 /* Samples margin for buffer */ +#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 { + uint32_t size; /**< Size of this struct in bytes */ + uint32_t reserved[8]; + int32_t sample_frequency; /**< Hz. e.g. 16000 */ + int32_t window_gain_comp; /**< Q1.31 gain for IFFT */ + int32_t reserved_32; + int16_t channel; /**< -1 expect mono, 0 left, 1 right, ... */ + int16_t frame_length; /**< samples, e.g. 400 for 25 ms @ 16 kHz*/ + int16_t frame_shift; /**< samples, e.g. 160 for 10 ms @ 16 kHz */ + int16_t reserved_16; + enum sof_phase_vocoder_fft_pad_type pad; /**< Use PAD_END, PAD_CENTER, PAD_START */ + enum sof_phase_vocoder_fft_window_type window; /**< Use RECTANGULAR_WINDOW, etc. */ +} __attribute__((packed)); + +struct phase_vocoder_buffer { + int32_t *addr; + int32_t *end_addr; + int32_t *r_ptr; + int32_t *w_ptr; + int s_avail; /**< samples count */ + int s_free; /**< samples count */ + int s_length; /**< length in samples for wrap */ +}; + +struct phase_vocoder_fft { + struct icomplex32 *fft_buf; /**< fft_size */ + struct icomplex32 *fft_out; /**< fft_size */ + 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; /**< bytes */ +}; + +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 { + struct phase_vocoder_buffer ibuf[PLATFORM_MAX_CHANNELS]; /**< Buffer for input data */ + struct phase_vocoder_buffer obuf[PLATFORM_MAX_CHANNELS]; /**< Buffer for output data */ + struct phase_vocoder_fft fft; /**< FFT instance, common */ + struct phase_vocoder_polar polar; /**< Processing in polar domain */ + int32_t *prev_data[PLATFORM_MAX_CHANNELS]; /**< prev_data_size */ + int32_t *buffers; + int32_t *window; /**< fft_size */ + int32_t num_input_fft_to_use; + int32_t num_input_fft; /**< Total input FFTs count */ + int32_t num_output_ifft; /**< Total output IFFTs count */ + int32_t gain_comp; /**< Gain to compensate window gain */ + int32_t interpolate_fraction; /**< Q3.29 coefficient */ + int32_t speed; /**< Q3.29 actual render speed */ + int source_channel; + int prev_data_size; + int sample_rate; + bool first_output_ifft_done; +}; + +/** + * struct phase_vocoder_func - Function call pointer for process function + * @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. + */ +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_func: Pointer to used processing function. + * @channels_order[]: Vector with desired sink channels order. + * @source_format: Source samples format. + * @frame_bytes: Number of bytes in an audio frame. + * @channels: Channels count. + * @enable: Control processing on/off, on - reorder channels + */ +struct phase_vocoder_comp_data { + phase_vocoder_func phase_vocoder_func; /**< processing function */ + struct phase_vocoder_state state; + struct sof_phase_vocoder_config *config; + int32_t speed_ctrl; /**< Speed Q3.29, allowed range 0.5 to 2.0 */ + int32_t speed_enum; /**< Speed control from enum 0-15 */ + size_t frame_bytes; + int source_channel; + int max_input_frames; + int max_output_frames; + int channels; + bool enable; /**< Processing enable flag */ +}; + +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_set_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 rate, int channels); + +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..8277c85ed316 --- /dev/null +++ b/src/audio/phase_vocoder/phase_vocoder_common.c @@ -0,0 +1,458 @@ +// 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); +} + +static void stft_convert_to_polar(struct phase_vocoder_fft *fft, struct ipolar32 *polar_data) +{ + int i; + + for (i = 0; i < fft->half_fft_size; i++) + sofm_icomplex32_to_polar(&fft->fft_out[i], &polar_data[i]); +} + +static void stft_convert_to_complex(struct ipolar32 *polar_data, struct phase_vocoder_fft *fft) +{ + int i; + + for (i = 0; i < fft->half_fft_size; i++) + 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_rnd; + + input_frame_num_frac = (int64_t)state->num_output_ifft * cd->state.speed; /* Q31.29 */ + input_frame_num_rnd = Q_SHIFT_RND(input_frame_num_frac, 29, 0); + state->num_input_fft_to_use = input_frame_num_rnd + 1; + state->interpolate_fraction = input_frame_num_frac - ((int64_t)input_frame_num_rnd << 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) +{ + cd->state.speed = cd->speed_ctrl; + cd->state.num_input_fft = 0; + cd->state.num_output_ifft = 0; +} + +// 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->channels; ch++) { + stft_do_fft(state, ch); + + /* Convert half-FFT to polar */ + polar_data_ch = polar->polar[ch]; + angle_delta_ch = polar->angle_delta[ch]; + stft_convert_to_polar(&state->fft, polar_data_ch); + for (i = 0; i < fft->half_fft_size; i++) + angle_delta_ch[i] = polar_data_ch[i].angle >> 2; + } + 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->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 */ + 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++) { + /* Calculate as Q4.28 */ + a = (polar_data_ch[i].angle >> 2) - + (polar_data_prev_ch[i].angle >> 2); + 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->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] << 2; /* Q29 */ + } + + /* 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->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->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..66ae962292fa --- /dev/null +++ b/src/audio/phase_vocoder/phase_vocoder_setup.c @@ -0,0 +1,267 @@ +// 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, int sample_rate, int channels) +{ + 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 polar_buffers_size; + size_t ibuf_size; + size_t obuf_size; + size_t prev_size; + int32_t *addr; + 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 != sample_rate) { + comp_err(dev, "Config sample_frequency does not match stream"); + return -EINVAL; + } + + state->sample_rate = sample_rate; + + comp_info(dev, "source_channel = %d, stream_channels = %d", config->channel, channels); + if (config->channel >= channels) { + comp_err(dev, "Illegal channel"); + return -EINVAL; + } + + if (config->channel < 0) + state->source_channel = 0; + else + state->source_channel = config->channel; + + 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 */ + sample_buffers_size = + sizeof(int32_t) * cd->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 < cd->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; + + /* Set initial state for STFT */ + // state->waiting_fill = true; + // state->prev_samples_valid = false; + + /* Allocate buffers for polar format data for magnitude and phase interpolation */ + polar_buffers_size = + channels * fft->half_fft_size * (2 * sizeof(struct ipolar32) + 3 * sizeof(int32_t)); + comp_info(dev, "polar buffers size %d", polar_buffers_size); + addr = mod_balloc(mod, polar_buffers_size); + if (!addr) { + comp_err(dev, "Failed polar data buffer allocate"); + ret = -ENOMEM; + goto free_window_out; + } + + memset(addr, 0, polar_buffers_size); + 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 720b5d7d6a4a..1e45a2474303 100644 --- a/src/include/sof/audio/component.h +++ b/src/include/sof/audio/component.h @@ -950,6 +950,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 38ea9d9fabff..86afd1e7278d 100644 --- a/tools/rimage/config/lnl.toml.h +++ b/tools/rimage/config/lnl.toml.h @@ -158,5 +158,9 @@ #include