diff --git a/src/audio/copier/copier_gain.c b/src/audio/copier/copier_gain.c index d8cb61fe2f1b..93b389b1404f 100644 --- a/src/audio/copier/copier_gain.c +++ b/src/audio/copier/copier_gain.c @@ -33,3 +33,42 @@ int copier_gain_set_params(struct comp_dev *dev, struct dai_data *dd) 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; +} diff --git a/src/audio/copier/copier_gain.h b/src/audio/copier/copier_gain.h index 6abdc7236b61..bf2b7b6eae14 100644 --- a/src/audio/copier/copier_gain.h +++ b/src/audio/copier/copier_gain.h @@ -37,6 +37,7 @@ /* 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 */ @@ -142,4 +143,57 @@ 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); + #endif /* __SOF_COPIER_GAIN_H__ */ diff --git a/src/audio/copier/copier_generic.c b/src/audio/copier/copier_generic.c index be7b3e0e49ba..957c0b6edbd6 100644 --- a/src/audio/copier/copier_generic.c +++ b/src/audio/copier/copier_generic.c @@ -107,6 +107,188 @@ int copier_gain_set_fade_params(struct comp_dev *dev, struct dai_data *dd, 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; +} + #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 92199f06cb53..84f9c4598230 100644 --- a/src/audio/copier/copier_hifi.c +++ b/src/audio/copier/copier_hifi.c @@ -136,4 +136,283 @@ int copier_gain_set_fade_params(struct comp_dev *dev, struct dai_data *dd, 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; +} + #endif