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/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/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/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/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/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 53% rename from src/math/fft/fft_32.h rename to src/include/sof/math/icomplex32.h index 92a3131b5189..29d22daadab8 100644 --- a/src/math/fft/fft_32.h +++ b/src/include/sof/math/icomplex32.h @@ -1,13 +1,39 @@ // 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; +}; + +/** + * 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. @@ -62,3 +88,26 @@ static inline void icomplex32_shift(const struct icomplex32 *input, int32_t n, output->imag = input->imag >> -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/include/sof/math/sqrt.h b/src/include/sof/math/sqrt.h index 1cdb82afe9a0..3d8fd0df72fe 100644 --- a/src/include/sof/math/sqrt.h +++ b/src/include/sof/math/sqrt.h @@ -1,15 +1,32 @@ /* 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); + +/** + * 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..c3dcfae0f070 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) @@ -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/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/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); 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/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/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/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); 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) { 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" 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