diff --git a/src/audio/copier/CMakeLists.txt b/src/audio/copier/CMakeLists.txt index 163f4287685c..7cead10dabe0 100644 --- a/src/audio/copier/CMakeLists.txt +++ b/src/audio/copier/CMakeLists.txt @@ -4,3 +4,7 @@ if(CONFIG_IPC4_GATEWAY) copier_ipcgtw.c ) endif() + +if(CONFIG_COPIER_GAIN) + add_local_sources(sof copier_gain.c) +endif() diff --git a/src/audio/copier/Kconfig b/src/audio/copier/Kconfig index 7c4c38a1776b..ec3e55016c10 100644 --- a/src/audio/copier/Kconfig +++ b/src/audio/copier/Kconfig @@ -28,3 +28,16 @@ config COMP_DAI_GROUP help Select for grouping physical DAIs into a logical DAI that can be triggered atomically to synchronise stream start and stop operations. + +if COMP_COPIER + +config COPIER_GAIN + bool "COPIER gain feature" + default y + help + Select for using copier gain feature. There are three modes available: + - Static gain: gain is set at initialization and remains constant. + - Mute: gain is set to 0, signal is muted. + - Transition gain: gain is set to a target value over a specified time. + Common use cases are fade-in and fade-out effects. +endif diff --git a/src/audio/copier/copier_dai.c b/src/audio/copier/copier_dai.c index 741bbad9db3b..c1f748d5eeb4 100644 --- a/src/audio/copier/copier_dai.c +++ b/src/audio/copier/copier_dai.c @@ -10,6 +10,7 @@ #include #include "copier.h" #include "dai_copier.h" +#include "copier_gain.h" LOG_MODULE_DECLARE(copier, CONFIG_SOF_LOG_LEVEL); @@ -217,9 +218,29 @@ static int copier_dai_init(struct comp_dev *dev, if (ret < 0) goto e_zephyr_free; + /* Allocate gain data if selected for this dai type and set basic params */ + if (dai->apply_gain) { + struct copier_gain_params *gain_data = rzalloc(SOF_MEM_ZONE_RUNTIME_SHARED, + 0, SOF_MEM_CAPS_RAM, + sizeof(*gain_data)); + if (!gain_data) { + ret = -ENOMEM; + goto e_zephyr_free; + } + cd->dd[index]->gain_data = gain_data; + + ret = copier_gain_set_params(dev, cd->dd[index]); + if (ret < 0) { + comp_err(dev, "Failed to set gain params!"); + goto gain_free; + } + } + cd->endpoint_num++; return 0; +gain_free: + rfree(dd->gain_data); e_zephyr_free: dai_common_free(dd); free_dd: @@ -348,6 +369,7 @@ void copier_dai_free(struct copier_data *cd) { for (int i = 0; i < cd->endpoint_num; i++) { dai_common_free(cd->dd[i]); + rfree(cd->dd[i]->gain_data); rfree(cd->dd[i]); } /* only dai have multi endpoint case */ diff --git a/src/audio/copier/copier_gain.c b/src/audio/copier/copier_gain.c new file mode 100644 index 000000000000..06fe92629b01 --- /dev/null +++ b/src/audio/copier/copier_gain.c @@ -0,0 +1,150 @@ +// SPDX-License-Identifier: BSD-3-Clause +// +// Copyright(c) 2024 Intel Corporation. +// +// Author: Ievgen Ganakov + +#include +#include +#include +#include +#include "copier.h" +#include "copier_gain.h" + +LOG_MODULE_DECLARE(copier, CONFIG_SOF_LOG_LEVEL); + +int copier_gain_set_params(struct comp_dev *dev, struct dai_data *dd) +{ + struct processing_module *mod = comp_mod(dev); + struct copier_data *cd = module_get_private_data(mod); + struct ipc4_base_module_cfg *ipc4_cfg = &cd->config.base; + uint32_t sampling_freq = ipc4_cfg->audio_fmt.sampling_frequency; + uint32_t frames = sampling_freq / dev->pipeline->period; + uint32_t fade_period = GAIN_DEFAULT_FADE_PERIOD; + int ret; + + /* Set basic gain parameters */ + copier_gain_set_basic_params(dev, dd, ipc4_cfg); + + /* Set fade parameters */ + ret = copier_gain_set_fade_params(dev, dd, ipc4_cfg, fade_period, frames); + if (ret) + comp_err(dev, "Failed to set fade params"); + + return ret; +} + +int copier_gain_input(struct comp_dev *dev, struct comp_buffer *buff, + struct copier_gain_params *gain_params, + enum copier_gain_envelope_dir dir, uint32_t stream_bytes) +{ + enum sof_ipc_frame frame_fmt = audio_stream_get_frm_fmt(&buff->stream); + uint32_t frames = stream_bytes / audio_stream_frame_bytes(&buff->stream); + enum copier_gain_state state; + + if (!gain_params) + return -EINVAL; + + state = copier_gain_eval_state(gain_params); + + comp_dbg(dev, "copier selected gain state %d", state); + + switch (frame_fmt) { + case SOF_IPC_FRAME_S16_LE: + return copier_gain_input16(buff, state, dir, gain_params, frames); + case SOF_IPC_FRAME_S32_LE: + return copier_gain_input32(buff, state, dir, gain_params, frames); + default: + comp_err(dev, "unsupported frame format %d for copier gain", frame_fmt); + return -EINVAL; + } +} + +enum copier_gain_state copier_gain_eval_state(struct copier_gain_params *gain_params) +{ + enum copier_gain_state state = STATIC_GAIN; + + if (gain_params->silence_sg_count < gain_params->silence_sg_length) + state = MUTE; + else if ((gain_params->fade_in_sg_count < gain_params->fade_sg_length) && + (gain_params->fade_sg_length != 0)) + state = TRANS_GAIN; + + return state; +} + +int copier_gain_dma_control(uint32_t node_id, const uint32_t *config_data, + size_t config_size, enum sof_ipc_dai_type dai_type) +{ + struct ipc *ipc = ipc_get(); + struct ipc_comp_dev *icd; + struct comp_dev *dev; + struct list_item *clist; + + int ret; + + list_for_item(clist, &ipc->comp_list) { + struct gain_dma_control_data *gain_data = NULL; + + icd = container_of(clist, struct ipc_comp_dev, list); + + if (!icd || icd->type != COMP_TYPE_COMPONENT) + continue; + + dev = icd->cd; + + if (!dev || dev->ipc_config.type != SOF_COMP_DAI) + continue; + + struct processing_module *mod = comp_mod(dev); + struct copier_data *cd = module_get_private_data(mod); + + ret = copier_set_gain(dev, cd->dd[0], gain_data); + if (ret) + comp_err(dev, "Gain DMA control: failed to set gain"); + return ret; + } + + return -ENODEV; +} + +int copier_set_gain(struct comp_dev *dev, struct dai_data *dd, + struct gain_dma_control_data *gain_data) +{ + struct copier_gain_params *gain_params = dd->gain_data; + struct ipc4_copier_module_cfg *copier_cfg = dd->dai_spec_config; + const int channels = copier_cfg->base.audio_fmt.channels_count; + uint16_t static_gain[MAX_GAIN_COEFFS_CNT]; + int ret; + + if (!gain_data) { + comp_err(dev, "Gain data is NULL"); + return -EINVAL; + } + + /* Set gain coefficients */ + comp_info(dev, "Update gain coefficients from DMA_CONTROL ipc"); + + size_t gain_coef_size = channels * sizeof(uint16_t); + + ret = memcpy_s(static_gain, gain_coef_size, gain_data->gain_coeffs, + gain_coef_size); + if (ret) { + comp_err(dev, "memcpy_s failed with error %d", ret); + return ret; + } + + for (int i = channels; i < MAX_GAIN_COEFFS_CNT; i++) + static_gain[i] = static_gain[i % channels]; + + ret = memcpy_s(gain_params->gain_coeffs, sizeof(static_gain), + static_gain, sizeof(static_gain)); + if (ret) { + comp_err(dev, "memcpy_s failed with error %d", ret); + return ret; + } + + gain_params->unity_gain = copier_is_unity_gain(gain_params); + + return 0; +} diff --git a/src/audio/copier/copier_gain.h b/src/audio/copier/copier_gain.h new file mode 100644 index 000000000000..0ec6e3a502a7 --- /dev/null +++ b/src/audio/copier/copier_gain.h @@ -0,0 +1,242 @@ +/* SPDX-License-Identifier: BSD-3-Clause + * + * Copyright(c) 2024 Intel Corporation. + * + * Author: Ievgen Ganakov + */ + +#ifndef __SOF_COPIER_GAIN_H__ +#define __SOF_COPIER_GAIN_H__ + +#include +#include +#include +#if SOF_USE_HIFI(3, COPIER) || SOF_USE_HIFI(4, COPIER) || SOF_USE_HIFI(5, COPIER) +#include +#endif +/** + * @file copier_gain.h + * @brief Header file containing definitions and functions related to audio gain + * processing for a copier module. + * + * This file provides functions, constants and structure definitions for applying gain to + * input audio buffers, both in 16-bit and 32-bit container formats. The gain can be + * applied in different directions (addition or subtraction) and has three modes: + * - static gain + * - transition gain (fade-in/fade-out) + * - mute + */ + +/* Maximum number of gain coefficients */ +#define MAX_GAIN_COEFFS_CNT 4 + +/* Common const values for applying gain feature */ +#define Q10_TO_Q31_SHIFT 6 +#define Q10_TO_Q15_SHIFT 5 +#define GAIN_Q10_INT_SHIFT 10 + +/* 16x2 store operation requires shift to middle part of 32 bit register */ +#define I64_TO_I16_SHIFT 48 +#define I64_TO_I32_SHIFT 32 +#define MIDDLE_PART_SHIFT 8 + +/* Unit gain in q10 format applied by default */ +#define UNITY_GAIN_4X_Q10 0x0400040004000400 +#define UNITY_GAIN_GENERIC 0x0400 + +/* Default fade transition in ms in high quality mode (Freq > 16000Hz) */ +#define GAIN_DEFAULT_HQ_TRANS_MS 500 +/* Default fade transition in ms in low quality mode */ +#define GAIN_DEFAULT_LQ_TRANS_MS 100 + +#define GAIN_ZERO_TRANS_MS 0xFFFF +#define GAIN_DEFAULT_FADE_PERIOD 0 + +struct dai_data; + +/** + * @brief Enumeration representing the state of the copier gain processing. + */ +enum copier_gain_state { + MUTE = 0, /**< Mute state, zero gain value applied */ + TRANS_GAIN, /**< Transition gain state, used for fade-in/fade-out */ + STATIC_GAIN, /**< Static gain state, gain value is not changing over time */ +}; + +/** + * @brief Enumeration representing the change direction of the gain envelope in + * fade context. + */ +enum copier_gain_envelope_dir { + GAIN_ADD = 0, /**< gain envelope add direction */ + GAIN_SUBTRACT, /**< gain envelope subtract direction */ +}; + +/** + * @brief Structure representing the parameters for copier gain processing. + */ +struct copier_gain_params { +#if SOF_USE_HIFI(3, COPIER) || SOF_USE_HIFI(4, COPIER) || SOF_USE_HIFI(5, COPIER) + /**< Input gain coefficients in Q10 format */ + ae_int16x4 gain_coeffs[ROUND_UP(MAX_GAIN_COEFFS_CNT, 4) >> 2]; + /**< Step for fade-in lower precision */ + ae_f16x4 step_f16; + /**< Initial gain depending on the number of channels */ + ae_f16x4 init_gain; +#else /* Generic version of gain processing */ + /**< Input gain coefficients */ + int16_t gain_coeffs[MAX_GAIN_COEFFS_CNT]; + /**< Step for fade-in */ + int16_t step_f16; + /**< Initial gain */ + int16_t init_gain[MAX_GAIN_COEFFS_CNT]; +#endif + bool unity_gain; /**< Indicates unity gain coefficients, no processing is required */ + uint32_t silence_sg_count; /**< Accumulates sample group spent on silence */ + uint32_t fade_in_sg_count; /**< Accumulates sample group spent on fade-in */ + uint32_t silence_sg_length; /**< Total count of sample group spent on silence */ + uint32_t fade_sg_length; /**< Total count of sample group spent on fade-in */ + uint64_t gain_env; /**< Gain envelope for fade-in calculated in high precision */ + uint64_t step_i64; /**< Step for fade-in envelope in high precision */ + uint16_t channels_count; /**< Number of channels */ +}; + +/** Gain Coefficients IO Control + * + * This parameter is sent by the driver to add/modify a static gain. + * Coefficients are encoded in Q10 format. + */ +struct gain_dma_control_data { + uint16_t gain_coeffs[MAX_GAIN_COEFFS_CNT]; +} __packed __aligned(4); + +/** + * @brief Sets gain parameters. + * + * This function sets the gain parameters for the copier component specified by + * the given device and DAI data. + * + * @param dev The pointer to the component device structure. + * @param dd The pointer to the DAI data structure. + * @return 0 on success, negative error code on failure. + */ +int copier_gain_set_params(struct comp_dev *dev, struct dai_data *dd); + +/** + * @brief Sets the basic gain parameters. + * + * This function sets the basic gain parameters for the copier component specified + * by the given device and DAI data. + * + * @param dev The pointer to the component device structure. + * @param dd The pointer to the DAI data structure. + * @param ipc4_cfg The pointer to the IPC4 base module config. + */ +void copier_gain_set_basic_params(struct comp_dev *dev, struct dai_data *dd, + struct ipc4_base_module_cfg *ipc4_cfg); + +/** + * @brief Sets the gain fade parameters. + * + * This function sets the fade gain parameters for the copier component specified + * by the given device and DAI data. + * + * @param dev The pointer to the component device structure. + * @param dd The pointer to the DAI data structure. + * @param ipc4_cfg The pointer to the IPC4 base module config. + * @param fade_period The fade period in milliseconds. + * @param frames The number of frames to fade. + * @return 0 on success, negative error code on failure. + */ +int copier_gain_set_fade_params(struct comp_dev *dev, struct dai_data *dd, + struct ipc4_base_module_cfg *ipc4_cfg, + uint32_t fade_period, uint32_t frames); + +/** + * @brief Applies gain to a 16-bit container size. + * + * This function applies gain to the input audio buffer. There are three gain modes + * supported: static gain, mute, and gain transition (fade-in or fade-out). + * + * @param buff Pointer to the input audio buffer. + * @param state The state of the gain processing. + * @param dir direction of the gain envelope change. + * @param frames The number of frames to be processed. + */ +int copier_gain_input16(struct comp_buffer *buff, enum copier_gain_state state, + enum copier_gain_envelope_dir dir, + struct copier_gain_params *gain_params, uint32_t frames); + +/** + * @brief Applies gain to a 32-bit container size. + * + * This function applies gain to the input audio buffer. There are three gain modes + * supported: static gain, mute, and gain transition (fade-in or fade-out). + * + * @param buff Pointer to the input audio buffer. + * @param state The state of the gain processing. + * @param dir Direction of the gain envelope change. + * @param gain_params The pointer to the copier_gain_params structure. + * @param frames The number of frames to be processed. + */ +int copier_gain_input32(struct comp_buffer *buff, enum copier_gain_state state, + enum copier_gain_envelope_dir dir, + struct copier_gain_params *gain_params, uint32_t frames); + +/** + * @brief Applies gain to the input audio buffer, selects the appropriate gain method. + * + * @param dev The pointer to the comp_dev structure representing the audio component device. + * @param buff The pointer to the comp_buffer structure representing the input buffer. + * @param gain_params The pointer to the copier_gain_params structure. + * @param dir Direction of the gain envelope change. + * @param stream_bytes The number of bytes in the input buffer. + * @return 0 on success, negative error code on failure. + */ +int copier_gain_input(struct comp_dev *dev, struct comp_buffer *buff, + struct copier_gain_params *gain_params, + enum copier_gain_envelope_dir dir, uint32_t stream_bytes); + +/** + * Evaluates appropriate gain mode based on the current gain parameters + * + * @param gain_params The pointer to the copier_gain_params structure. + * @return The state of the copier gain (enum copier_gain_state). + */ +enum copier_gain_state copier_gain_eval_state(struct copier_gain_params *gain_params); + +/** + * Sets/modify gain for a copier module in runtime. + * + * @param dev The copier device structure. + * @param dd The DAI data structure. + * @param gain_data The gain control data structure. + * @return 0 on success, otherwise a negative error code. + */ +int copier_set_gain(struct comp_dev *dev, struct dai_data *dd, + struct gain_dma_control_data *gain_data); + +/** + * Checks for unity gain mode. + * + * @param gain_params The copier gain parameters structure. + * @return true if the gain is set to unity gain, false otherwise. + */ +bool copier_is_unity_gain(struct copier_gain_params *gain_params); + +/** + * Controls the gain for a copier device using DMA Control IPC message. + * + * This function retrieves gain data from the DMA Control IPC message and updates + * corresponding dai device gain params structure. + * + * @param node_id Gateway node id. + * @param config_data The gain configuration data. + * @param config_size The size of the gain configuration data. + * @param dai_type The type of the DAI device. + * @return 0 on success, otherwise a negative error code. + */ +int copier_gain_dma_control(uint32_t node_id, const uint32_t *config_data, + size_t config_size, enum sof_ipc_dai_type dai_type); + +#endif /* __SOF_COPIER_GAIN_H__ */ diff --git a/src/audio/copier/copier_generic.c b/src/audio/copier/copier_generic.c index a0502d6376df..b166bc77ed2f 100644 --- a/src/audio/copier/copier_generic.c +++ b/src/audio/copier/copier_generic.c @@ -6,8 +6,11 @@ #include #include -#include "copier.h" +#include #include +#include +#include "copier.h" +#include LOG_MODULE_DECLARE(copier, CONFIG_SOF_LOG_LEVEL); @@ -20,6 +23,7 @@ LOG_MODULE_DECLARE(copier, CONFIG_SOF_LOG_LEVEL); #include #include #include +#include "copier_gain.h" int apply_attenuation(struct comp_dev *dev, struct copier_data *cd, struct comp_buffer *sink, int frame) @@ -56,6 +60,245 @@ int apply_attenuation(struct comp_dev *dev, struct copier_data *cd, return -EINVAL; } } + +void copier_gain_set_basic_params(struct comp_dev *dev, struct dai_data *dd, + struct ipc4_base_module_cfg *ipc4_cfg) +{ + struct copier_gain_params *gain_params = dd->gain_data; + + gain_params->channels_count = ipc4_cfg->audio_fmt.channels_count; + + for (int i = 0; i < MAX_GAIN_COEFFS_CNT; i++) + gain_params->gain_coeffs[i] = UNITY_GAIN_GENERIC; +} + +int copier_gain_set_fade_params(struct comp_dev *dev, struct dai_data *dd, + struct ipc4_base_module_cfg *ipc4_cfg, + uint32_t fade_period, uint32_t frames) +{ + struct copier_gain_params *gain_params = dd->gain_data; + uint16_t step_i64_to_i16; + + if (fade_period == GAIN_DEFAULT_FADE_PERIOD) { + /* Set fade transition delay to default value*/ + if (ipc4_cfg->audio_fmt.sampling_frequency > IPC4_FS_16000HZ) + gain_params->fade_sg_length = frames * GAIN_DEFAULT_HQ_TRANS_MS; + else + gain_params->fade_sg_length = frames * GAIN_DEFAULT_LQ_TRANS_MS; + } else if (fade_period == GAIN_ZERO_TRANS_MS) { + /* Special case for GAIN_ZERO_TRANS_MS to support zero fade-in transition time */ + gain_params->fade_sg_length = 0; + return 0; + } + + /* High precision step for fade-in calculation, keeps accurate precision */ + gain_params->step_i64 = INT64_MAX / gain_params->fade_sg_length; + step_i64_to_i16 = gain_params->step_i64 >> I64_TO_I16_SHIFT; + + /* lower precision step for HIFI SIMD fade-in calculation, converted to Q16 format */ + gain_params->step_f16 = (MAX_GAIN_COEFFS_CNT / gain_params->channels_count) * + step_i64_to_i16; + + /* Initialization gain for HIFI SIMD addition, depends on channel configuration */ + for (int i = 0; i < MAX_GAIN_COEFFS_CNT; i++) { + gain_params->init_gain[i] = (i / gain_params->channels_count) * + step_i64_to_i16; + } + return 0; +} + +int copier_gain_input16(struct comp_buffer *buff, enum copier_gain_state state, + enum copier_gain_envelope_dir dir, + struct copier_gain_params *gain_params, uint32_t frames) +{ + int16_t *dst = audio_stream_get_rptr(&buff->stream); + const int nch = audio_stream_get_channels(&buff->stream); + int samples = frames * nch; + int16_t gain_env[MAX_GAIN_COEFFS_CNT] = {0}; + int16_t gain_env_sq; + int16_t gain_env_i16; + int16_t *dst_tmp; + int16_t gain; + int nmax, i, j; + + switch (state) { + case STATIC_GAIN: + /* static gain */ + if (gain_params->unity_gain) + return 0; + + while (samples) { + nmax = audio_stream_samples_without_wrap_s16(&buff->stream, dst); + nmax = MIN(samples, nmax); + + for (j = 0; j < nch; j++) { + dst_tmp = dst + j; + gain = gain_params->gain_coeffs[j]; + for (i = 0; i < nmax; i += nch) + dst_tmp[i] = q_multsr_sat_16x16(dst_tmp[i], gain, + GAIN_Q10_INT_SHIFT); + } + samples -= nmax; + dst = audio_stream_wrap(&buff->stream, dst + nmax); + } + break; + case MUTE: + while (samples) { + nmax = audio_stream_samples_without_wrap_s16(&buff->stream, dst); + nmax = MIN(samples, nmax); + size_t zeroed_bytes = nmax * sizeof(int16_t); + /* Apply mute */ + memset_s(dst, zeroed_bytes, 0, zeroed_bytes); + samples -= nmax; + dst = audio_stream_wrap(&buff->stream, dst + nmax); + } + break; + case TRANS_GAIN: + while (samples) { + nmax = audio_stream_samples_without_wrap_s16(&buff->stream, dst); + nmax = MIN(samples, nmax); + + /* Precalculate gain envelope */ + gain_env_i16 = gain_params->gain_env >> I64_TO_I16_SHIFT; + for (i = 0; i < MAX_GAIN_COEFFS_CNT; i++) + gain_env[i] = gain_env_i16 + gain_params->init_gain[i]; + + /* Apply fade */ + for (j = 0; j < nch; j++) { + dst += j; + /* Quadratic fade part in Q15 format*/ + gain_env_sq = q_multsr_16x16(gain_env[j], gain_env[j], 15); + + /* Calculate gain value. Gain coeffs in Q10 format but + * gain_env_sq in Q15. So shifting result by 15 bits. + */ + gain = q_multsr_16x16(gain_params->gain_coeffs[j], + gain_env_sq, 15); + + for (i = 0; i < nmax; i += nch) + dst[i] = q_multsr_sat_16x16(dst[i], gain, + GAIN_Q10_INT_SHIFT); + } + samples -= nmax; + dst = audio_stream_wrap(&buff->stream, dst + nmax); + } + break; + } + + if (state == MUTE) { + gain_params->silence_sg_count += frames; + } else if (state == TRANS_GAIN) { + gain_params->fade_in_sg_count += frames; + if (dir == GAIN_ADD) + gain_params->gain_env += gain_params->step_i64 * frames; + else + gain_params->gain_env -= gain_params->step_i64 * frames; + } + + return 0; +} + +int copier_gain_input32(struct comp_buffer *buff, enum copier_gain_state state, + enum copier_gain_envelope_dir dir, + struct copier_gain_params *gain_params, uint32_t frames) +{ + int32_t *dst = audio_stream_get_rptr(&buff->stream); + const int nch = audio_stream_get_channels(&buff->stream); + int samples = frames * nch; + int16_t gain_env[MAX_GAIN_COEFFS_CNT] = {0}; + int32_t *dst_tmp; + int16_t gain, gain_env_i16, gain_env_sq; + int nmax, i, j; + + switch (state) { + case STATIC_GAIN: + /* static gain */ + if (gain_params->unity_gain) + return 0; + + while (samples) { + nmax = audio_stream_samples_without_wrap_s32(&buff->stream, dst); + nmax = MIN(samples, nmax); + + for (j = 0; j < nch; j++) { + dst_tmp = dst + j; + /* Gain is in Q21.10 format */ + gain = gain_params->gain_coeffs[j]; + for (i = 0; i < nmax; i += nch) + dst_tmp[i] = q_multsr_sat_32x32(dst_tmp[i], gain, + GAIN_Q10_INT_SHIFT); + } + samples -= nmax; + dst = audio_stream_wrap(&buff->stream, dst + nmax); + } + break; + case MUTE: + while (samples) { + nmax = audio_stream_samples_without_wrap_s32(&buff->stream, dst); + nmax = MIN(samples, nmax); + size_t zeroed_bytes = nmax * sizeof(int32_t); + + /* Apply mute*/ + memset_s(dst, zeroed_bytes, 0, zeroed_bytes); + samples -= nmax; + dst = audio_stream_wrap(&buff->stream, dst + nmax); + } + break; + case TRANS_GAIN: + while (samples) { + nmax = audio_stream_samples_without_wrap_s32(&buff->stream, dst); + nmax = MIN(samples, nmax); + + /* Precalculate gain envelope */ + gain_env_i16 = gain_params->gain_env >> I64_TO_I16_SHIFT; + for (i = 0; i < MAX_GAIN_COEFFS_CNT; i++) + gain_env[i] = gain_env_i16 + gain_params->init_gain[i]; + + /* Apply fade */ + for (j = 0; j < nch; j++) { + dst += j; + /* Quadratic fade part in Q15 format*/ + gain_env_sq = q_multsr_16x16(gain_env[j], gain_env[j], 15); + + /* Calculate gain value. Gain coeffs in Q10 format but + * gain_env_sq in Q15. So shifting result by 15 bits. + */ + gain = q_multsr_16x16(gain_params->gain_coeffs[j], + gain_env_sq, 15); + + for (i = 0; i < nmax; i += nch) + dst[i] = q_multsr_sat_32x32(dst[i], gain, + GAIN_Q10_INT_SHIFT); + } + samples -= nmax; + dst = audio_stream_wrap(&buff->stream, dst + nmax); + } + break; + } + + if (state == MUTE) { + gain_params->silence_sg_count += frames; + } else if (state == TRANS_GAIN) { + gain_params->fade_in_sg_count += frames; + if (dir == GAIN_ADD) + gain_params->gain_env += gain_params->step_i64 * frames; + else + gain_params->gain_env -= gain_params->step_i64 * frames; + } + + return 0; +} + +bool copier_is_unity_gain(struct copier_gain_params *gain_params) +{ + /* Set unity gain flag */ + for (int i = 0; i < MAX_GAIN_COEFFS_CNT; i++) { + if (gain_params->gain_coeffs[i] != UNITY_GAIN_GENERIC) + return false; + } + return true; +} + #endif void copier_update_params(struct copier_data *cd, struct comp_dev *dev, diff --git a/src/audio/copier/copier_hifi.c b/src/audio/copier/copier_hifi.c index f428d1f9008e..4651b8cdd339 100644 --- a/src/audio/copier/copier_hifi.c +++ b/src/audio/copier/copier_hifi.c @@ -13,10 +13,12 @@ #include #include #include +#include #include #include #include #include +#include "copier_gain.h" LOG_MODULE_REGISTER(copier_hifi, CONFIG_SOF_LOG_LEVEL); @@ -72,4 +74,353 @@ int apply_attenuation(struct comp_dev *dev, struct copier_data *cd, return -EINVAL; } } + +void copier_gain_set_basic_params(struct comp_dev *dev, struct dai_data *dd, + struct ipc4_base_module_cfg *ipc4_cfg) +{ + struct copier_gain_params *gain_params = dd->gain_data; + + /* Set default gain coefficients */ + for (int i = 0; i < ARRAY_SIZE(gain_params->gain_coeffs); ++i) + gain_params->gain_coeffs[i] = AE_MOVF16X4_FROMINT64(UNITY_GAIN_4X_Q10); + + gain_params->step_f16 = AE_ZERO16(); + gain_params->init_gain = AE_ZERO16(); + + /* Set channels count */ + gain_params->channels_count = ipc4_cfg->audio_fmt.channels_count; +} + +int copier_gain_set_fade_params(struct comp_dev *dev, struct dai_data *dd, + struct ipc4_base_module_cfg *ipc4_cfg, + uint32_t fade_period, uint32_t frames) +{ + struct copier_gain_params *gain_params = dd->gain_data; + uint16_t init_gain[MAX_GAIN_COEFFS_CNT]; + uint16_t step_i64_to_i16; + ae_f16 step_f16; + + /* For backward compatibility add a case with default fade transition. + * Backward compatibility is referring to clock_on_delay in DMIC blob. + */ + if (fade_period == GAIN_DEFAULT_FADE_PERIOD) { + /* Set fade transition delay to default value*/ + if (ipc4_cfg->audio_fmt.sampling_frequency > IPC4_FS_16000HZ) + gain_params->fade_sg_length = frames * GAIN_DEFAULT_HQ_TRANS_MS; + else + gain_params->fade_sg_length = frames * GAIN_DEFAULT_LQ_TRANS_MS; + } else if (fade_period == GAIN_ZERO_TRANS_MS) { + /* Special case for GAIN_ZERO_TRANS_MS to support zero fade in transition time */ + gain_params->fade_sg_length = 0; + return 0; + } + + /* High precision step for fade-in calculation, keeps accurate precision */ + gain_params->step_i64 = INT64_MAX / gain_params->fade_sg_length; + step_i64_to_i16 = gain_params->step_i64 >> I64_TO_I16_SHIFT; + + step_f16 = step_i64_to_i16 * (MAX_GAIN_COEFFS_CNT / gain_params->channels_count); + + /* Lower precision step for HIFI SIMD fade-in calculation */ + gain_params->step_f16 = step_f16; + + /* Initialization gain for HIFI SIMD addition, depends on channel configuration */ + for (int i = 0; i < MAX_GAIN_COEFFS_CNT; i++) + init_gain[i] = (i / gain_params->channels_count) * step_i64_to_i16; + + int ret = memcpy_s(&gain_params->init_gain, sizeof(gain_params->init_gain), init_gain, + sizeof(init_gain)); + if (ret) + comp_err(dev, "memcpy_s failed with error code %d", ret); + + return ret; +} + +inline ae_int16x4 copier_load_slots_and_gain16(ae_int16x4 **addr, + ae_valign *align_in, + const ae_int16x4 gains) +{ + ae_int16x4 d16_1 = AE_ZERO16(); + ae_int32x2 d32_1 = AE_ZERO32(); + ae_int32x2 d32_2 = AE_ZERO32(); + + AE_LA16X4_IC(d16_1, align_in[0], addr[0]); + AE_MUL16X4(d32_1, d32_2, d16_1, gains); + + /* Saturate if exists by moving to Q31 */ + d32_1 = AE_SLAA32S(d32_1, Q10_TO_Q31_SHIFT); + d32_2 = AE_SLAA32S(d32_2, Q10_TO_Q31_SHIFT); + + /* Returns desired samples selection */ + return AE_TRUNC16X4F32(d32_1, d32_2); +} + +inline void copier_load_slots_and_gain32(ae_int32x2 **addr, ae_valign *align_in, + const ae_int16x4 gains, ae_int32x2 *out_d32_h, + ae_int32x2 *out_d32_l) +{ + ae_int32x2 d32tmp_h = AE_ZERO32(); + ae_int32x2 d32tmp_l = AE_ZERO32(); + + AE_LA32X2_IC(d32tmp_h, align_in[0], addr[0]); + AE_LA32X2_IC(d32tmp_l, align_in[0], addr[0]); + + /* Apply gains */ + d32tmp_h = AE_MULFP32X16X2RAS_H(d32tmp_h, gains); + d32tmp_l = AE_MULFP32X16X2RAS_L(d32tmp_l, gains); + + /* Gain is Q10 but treated in AE_MULFP32X16 as Q15, + * so we need to compensate by shifting with saturation + */ + *out_d32_h = AE_SLAA32S(d32tmp_h, Q10_TO_Q15_SHIFT); + *out_d32_l = AE_SLAA32S(d32tmp_l, Q10_TO_Q15_SHIFT); +} + +int copier_gain_input16(struct comp_buffer *buff, enum copier_gain_state state, + enum copier_gain_envelope_dir dir, + struct copier_gain_params *gain_params, uint32_t frames) +{ + uint16_t *dst = audio_stream_get_rptr(&buff->stream); + const int nch = audio_stream_get_channels(&buff->stream); + int samples = frames * nch; + const ae_int16x4 gain_i16 = gain_params->gain_coeffs[0]; + ae_valign align_in = AE_ZALIGN64(); + ae_valign align_out = AE_ZALIGN64(); + ae_f16x4 gain_env = AE_ZERO16(); + ae_int16x4 *out_ptr; + ae_int16x4 *in_ptr; + ae_int16x4 d_r; + ae_int16x4 d16_1; + int rest, n, nmax; + + while (samples) { + nmax = audio_stream_samples_without_wrap_s16(&buff->stream, dst); + out_ptr = (ae_int16x4 *)(dst); + in_ptr = (ae_int16x4 *)(dst); + nmax = MIN(samples, nmax); + rest = nmax & 0x3; + + AE_LA16X4POS_PC(align_in, in_ptr); + + switch (state) { + case STATIC_GAIN: + for (n = 0; n < (nmax >> 2); n++) { + d16_1 = copier_load_slots_and_gain16(&in_ptr, &align_in, gain_i16); + AE_SA16X4_IC(d16_1, align_out, out_ptr); + } + break; + case MUTE: + d16_1 = AE_ZERO16(); + for (size_t n = 0; n < (nmax >> 2); n++) + AE_SA16X4_IC(d16_1, align_out, out_ptr); + break; + case TRANS_GAIN: + gain_env = (int16_t)(gain_params->gain_env >> I64_TO_I16_SHIFT); + gain_env = AE_ADD16S(gain_env, gain_params->init_gain); + for (n = 0; n < (nmax >> 2); n++) { + /* static gain part */ + if (!gain_params->unity_gain) + d16_1 = copier_load_slots_and_gain16(&in_ptr, &align_in, + gain_i16); + else + AE_LA16X4_IC(d16_1, align_in, in_ptr); + + /* quadratic fade-in part */ + d16_1 = AE_MULFP16X4S(d16_1, gain_env); + d16_1 = AE_MULFP16X4S(d16_1, gain_env); + + AE_SA16X4_IC(d16_1, align_out, out_ptr); + if (dir == GAIN_ADD) + gain_env = AE_ADD16S(gain_env, gain_params->step_f16); + else + gain_env = AE_SUB16S(gain_env, gain_params->step_f16); + } + break; + } + + /* Process rest samples */ + AE_SA64POS_FP(align_out, out_ptr); + if (rest) { + switch (state) { + case STATIC_GAIN: + d_r = copier_load_slots_and_gain16(&in_ptr, &align_in, gain_i16); + break; + case MUTE: + d_r = AE_ZERO16(); + break; + case TRANS_GAIN: + if (!gain_params->unity_gain) + d_r = copier_load_slots_and_gain16(&in_ptr, &align_in, + gain_i16); + else + AE_LA16X4_IC(d_r, align_in, in_ptr); + + d_r = AE_MULFP16X4S(d_r, gain_env); + d_r = AE_MULFP16X4S(d_r, gain_env); + break; + } + + AE_S16_0_IP(AE_MOVAD16_3(d_r), (ae_int16 *)(out_ptr), sizeof(uint16_t)); + if (rest > 1) { + AE_S16_0_IP(AE_MOVAD16_2(d_r), (ae_int16 *)(out_ptr), + sizeof(uint16_t)); + if (rest > 2) + AE_S16_0_IP(AE_MOVAD16_1(d_r), (ae_int16 *)(out_ptr), 0); + } + } + samples -= nmax; + dst = audio_stream_wrap(&buff->stream, dst + nmax); + } + + if (state == MUTE) { + gain_params->silence_sg_count += frames; + } else if (state == TRANS_GAIN) { + gain_params->fade_in_sg_count += frames; + if (dir == GAIN_ADD) + gain_params->gain_env += gain_params->step_i64 * frames; + else + gain_params->gain_env -= gain_params->step_i64 * frames; + } + return 0; +} + +int copier_gain_input32(struct comp_buffer *buff, enum copier_gain_state state, + enum copier_gain_envelope_dir dir, + struct copier_gain_params *gain_params, uint32_t frames) +{ + uint32_t *dst = audio_stream_get_rptr(&buff->stream); + const int nch = audio_stream_get_channels(&buff->stream); + int samples = frames * nch; + ae_int16x4 gain_i16 = gain_params->gain_coeffs[0]; + ae_valign align_in = AE_ZALIGN64(); + ae_valign align_out = AE_ZALIGN64(); + ae_int32x2 d32_h = AE_ZERO32(); + ae_int32x2 d32_l = AE_ZERO32(); + ae_int32x2 r_d32_h = AE_ZERO32(); + ae_int32x2 r_d32_l = AE_ZERO32(); + ae_f16x4 gain_env = AE_ZERO16(); + ae_int32x2 *out_ptr; + ae_int32x2 *in_ptr; + int rest, n, nmax; + + while (samples) { + nmax = audio_stream_samples_without_wrap_s32(&buff->stream, dst); + out_ptr = (ae_int32x2 *)(dst); + in_ptr = (ae_int32x2 *)(dst); + nmax = MIN(samples, nmax); + rest = nmax & 0x3; + + /* Align input pointer access */ + AE_LA32X2POS_PC(align_in, in_ptr); + + switch (state) { + case STATIC_GAIN: + for (n = 0; n < (nmax >> 2); n++) { + copier_load_slots_and_gain32(&in_ptr, &align_in, gain_i16, + &d32_h, &d32_l); + AE_SA32X2_IC(d32_h, align_out, out_ptr); + AE_SA32X2_IC(d32_l, align_out, out_ptr); + } + break; + case MUTE: + d32_l = AE_ZERO32(); + for (size_t n = 0; n < (nmax >> 2); n++) { + AE_SA32X2_IC(d32_l, align_out, out_ptr); + AE_SA32X2_IC(d32_l, align_out, out_ptr); + } + break; + case TRANS_GAIN: + gain_env = (int16_t)(gain_params->gain_env >> I64_TO_I16_SHIFT); + gain_env = AE_ADD16S(gain_env, gain_params->init_gain); + for (n = 0; n < (nmax >> 2); n++) { + /* static gain part */ + if (!gain_params->unity_gain) { + copier_load_slots_and_gain32(&in_ptr, &align_in, gain_i16, + &d32_h, &d32_l); + } else { + AE_LA32X2_IC(d32_h, align_in, in_ptr); + AE_LA32X2_IC(d32_l, align_in, in_ptr); + } + /* quadratic fade-in part */ + d32_h = AE_MULFP32X16X2RAS_H(d32_h, gain_env); + d32_h = AE_MULFP32X16X2RAS_H(d32_h, gain_env); + d32_l = AE_MULFP32X16X2RAS_L(d32_l, gain_env); + d32_l = AE_MULFP32X16X2RAS_L(d32_l, gain_env); + AE_SA32X2_IC(d32_h, align_out, out_ptr); + AE_SA32X2_IC(d32_l, align_out, out_ptr); + + if (dir == GAIN_ADD) + gain_env = AE_ADD16S(gain_env, gain_params->step_f16); + else + gain_env = AE_SUB16S(gain_env, gain_params->step_f16); + } + break; + default: + return -EINVAL; + } + + AE_SA64POS_FP(align_out, out_ptr); + if (rest) { + switch (state) { + case STATIC_GAIN: + copier_load_slots_and_gain32(&in_ptr, &align_in, gain_i16, + &r_d32_h, &r_d32_l); + break; + case MUTE: + break; + case TRANS_GAIN: + if (!gain_params->unity_gain) { + copier_load_slots_and_gain32(&in_ptr, &align_in, gain_i16, + &r_d32_h, &r_d32_l); + } else { + AE_LA32X2_IC(r_d32_h, align_in, in_ptr); + AE_LA32X2_IC(r_d32_l, align_in, in_ptr); + } + r_d32_h = AE_MULFP32X16X2RAS_H(r_d32_h, gain_env); + r_d32_h = AE_MULFP32X16X2RAS_H(r_d32_h, gain_env); + r_d32_l = AE_MULFP32X16X2RAS_L(r_d32_l, gain_env); + r_d32_l = AE_MULFP32X16X2RAS_L(r_d32_l, gain_env); + break; + } + + if (rest > 1) { + AE_SA32X2_IC(r_d32_h, align_out, out_ptr); + AE_SA64POS_FP(align_out, out_ptr); + + if (rest > 2) { + ae_int32 tmp = AE_MOVAD32_H(r_d32_l); + + AE_S32_L_XC(tmp, (ae_int32 *)out_ptr, 0); + } + } else { + ae_int32 tmp = AE_MOVAD32_H(r_d32_h); + + AE_S32_L_XC(tmp, (ae_int32 *)out_ptr, 0); + } + } + samples -= nmax; + dst = audio_stream_wrap(&buff->stream, dst + nmax); + } + + if (state == MUTE) { + gain_params->silence_sg_count += frames; + } else if (state == TRANS_GAIN) { + gain_params->fade_in_sg_count += frames; + if (dir == GAIN_ADD) + gain_params->gain_env += gain_params->step_i64 * frames; + else + gain_params->gain_env -= gain_params->step_i64 * frames; + } + + return 0; +} + +bool copier_is_unity_gain(struct copier_gain_params *gain_params) +{ + ae_int16x4 gain_coeffs = AE_MOVF16X4_FROMINT64(UNITY_GAIN_4X_Q10); + xtbool4 unity_gain_check = AE_EQ16(gain_params->gain_coeffs[0], gain_coeffs); + + return XT_ALL4(unity_gain_check) ? true : false; +} + #endif diff --git a/src/audio/dai-zephyr.c b/src/audio/dai-zephyr.c index 5e6708437992..f027fd021e80 100644 --- a/src/audio/dai-zephyr.c +++ b/src/audio/dai-zephyr.c @@ -9,6 +9,7 @@ #include #include #include +#include #include #include #include @@ -37,6 +38,7 @@ #include "copier/copier.h" #include "copier/dai_copier.h" +#include "copier/copier_gain.h" #include #include @@ -319,6 +321,15 @@ dai_dma_cb(struct dai_data *dd, struct comp_dev *dev, uint32_t bytes, ret = stream_copy_from_no_consume(dd->dma_buffer, dd->local_buffer, dd->process, bytes, dd->chmap); #if CONFIG_IPC_MAJOR_4 + /* Apply gain to the local buffer */ + if (dd->ipc_config.apply_gain) { + ret = copier_gain_input(dev, dd->local_buffer, dd->gain_data, + GAIN_ADD, bytes); + if (ret) + comp_err(dev, "copier_gain_input() failed err=%d", ret); + buffer_stream_writeback(dd->local_buffer, bytes); + } + /* Skip in case of endpoint DAI devices created by the copier */ if (converter) { /* diff --git a/src/include/sof/audio/ipc-config.h b/src/include/sof/audio/ipc-config.h index f0c74f2da2a2..49dbb9eac217 100644 --- a/src/include/sof/audio/ipc-config.h +++ b/src/include/sof/audio/ipc-config.h @@ -80,6 +80,8 @@ struct ipc_config_dai { const struct ipc4_audio_format *out_fmt;/**< audio format for output pin 0 - required * for ACE 2.0 and newer */ + /* Gain feature flag */ + bool apply_gain; }; /* generic volume component */ diff --git a/src/include/sof/lib/dai-zephyr.h b/src/include/sof/lib/dai-zephyr.h index 5af6f5aeec24..d7ef31b4fb21 100644 --- a/src/include/sof/lib/dai-zephyr.h +++ b/src/include/sof/lib/dai-zephyr.h @@ -163,6 +163,8 @@ struct dai_data { /* io performance measurement */ struct io_perf_data_item *io_perf_bytes_count; #endif + /* Copier gain params */ + struct copier_gain_params *gain_data; }; /* these 3 are here to satisfy clk.c and ssp.h interconnection, will be removed leter */ diff --git a/zephyr/CMakeLists.txt b/zephyr/CMakeLists.txt index 366aa561059d..36c6126f4dd0 100644 --- a/zephyr/CMakeLists.txt +++ b/zephyr/CMakeLists.txt @@ -687,6 +687,10 @@ zephyr_library_sources_ifdef(CONFIG_IPC4_GATEWAY ${SOF_AUDIO_PATH}/copier/copier_ipcgtw.c ) +zephyr_library_sources_ifdef(CONFIG_COPIER_GAIN + ${SOF_AUDIO_PATH}/copier/copier_gain.c +) + zephyr_library_sources_ifdef(CONFIG_SAMPLE_KEYPHRASE ${SOF_SAMPLES_PATH}/audio/detect_test.c )