diff --git a/src/audio/module_adapter/CMakeLists.txt b/src/audio/module_adapter/CMakeLists.txt index 844fa0721913..a7b5b219a2f6 100644 --- a/src/audio/module_adapter/CMakeLists.txt +++ b/src/audio/module_adapter/CMakeLists.txt @@ -10,8 +10,10 @@ if((NOT CONFIG_LIBRARY) OR CONFIG_LIBRARY_STATIC) add_local_sources(sof module/volume/volume_generic.c module/volume/volume_hifi3.c + module/volume/volume_hifi4.c module/volume/volume_generic_with_peakvol.c module/volume/volume_hifi3_with_peakvol.c + module/volume/volume_hifi4_with_peakvol.c module/volume/volume.c) endif() diff --git a/src/audio/module_adapter/module/volume/volume.c b/src/audio/module_adapter/module/volume/volume.c index f36b03da1590..03e47272f478 100644 --- a/src/audio/module_adapter/module/volume/volume.c +++ b/src/audio/module_adapter/module/volume/volume.c @@ -267,8 +267,10 @@ static inline int32_t volume_windows_fade_ramp(struct vol_data *cd, int32_t ramp */ /* Note: Using inline saves 0.4 MCPS */ -static inline void volume_ramp(struct vol_data *cd) +static inline void volume_ramp(struct processing_module *mod) { + struct vol_data *cd = module_get_private_data(mod); + struct comp_dev *dev = mod->dev; int32_t new_vol; int32_t tvolume; int32_t volume; @@ -326,6 +328,29 @@ static inline void volume_ramp(struct vol_data *cd) } cd->volume[i] = new_vol; } + + cd->is_passthrough = cd->ramp_finished; + for (i = 0; i < cd->channels; i++) { + if (cd->volume[i] != VOL_ZERO_DB) { + cd->is_passthrough = false; + break; + } + } + +#if CONFIG_IPC_MAJOR_4 + cd->scale_vol = vol_get_processing_function(dev, cd); +#else + struct comp_buffer *sourceb; + struct comp_buffer __sparse_cache *source_c; + + sourceb = list_first_item(&dev->bsource_list, + struct comp_buffer, sink_list); + source_c = buffer_acquire(sourceb); + + cd->scale_vol = vol_get_processing_function(dev, source_c, cd); + + buffer_release(source_c); +#endif } /** @@ -368,6 +393,7 @@ static void reset_state(struct vol_data *cd) cd->vol_ramp_elapsed_frames = 0; cd->sample_rate_inv = 0; cd->copy_gain = true; + cd->is_passthrough = false; } #if CONFIG_IPC_MAJOR_3 @@ -397,6 +423,7 @@ static int volume_init(struct processing_module *mod) } md->private = cd; + cd->is_passthrough = false; /* Set the default volumes. If IPC sets min_value or max_value to * not-zero, use them. Otherwise set to internal limits and notify @@ -605,6 +632,7 @@ static int volume_init(struct processing_module *mod) cd->mailbox_offset += instance_id * sizeof(struct ipc4_peak_volume_regs); cd->attenuation = 0; + cd->is_passthrough = false; reset_state(cd); @@ -971,6 +999,17 @@ static int volume_set_volume(struct processing_module *mod, const uint8_t *data, cd->ramp_finished = false; } + cd->is_passthrough = cd->ramp_finished; + + for (i = 0; i < channels_count; i++) { + if (cd->volume[i] != VOL_ZERO_DB) { + cd->is_passthrough = false; + break; + } + } + + cd->scale_vol = vol_get_processing_function(dev, cd); + prepare_ramp(dev, cd); return 0; @@ -1161,7 +1200,7 @@ static int volume_process(struct processing_module *mod, } if (!cd->ramp_finished) { - volume_ramp(cd); + volume_ramp(mod); cd->vol_ramp_elapsed_frames += frames; } @@ -1291,8 +1330,12 @@ static int volume_prepare(struct processing_module *mod, ret = -ENOMEM; goto err; } +#if CONFIG_IPC_MAJOR_4 + cd->scale_vol = vol_get_processing_function(dev, cd); +#else + cd->scale_vol = vol_get_processing_function(dev, sink_c, cd); +#endif - cd->scale_vol = vol_get_processing_function(dev, sink_c); if (!cd->scale_vol) { comp_err(dev, "volume_prepare(): invalid cd->scale_vol"); diff --git a/src/audio/module_adapter/module/volume/volume_generic.c b/src/audio/module_adapter/module/volume/volume_generic.c index 893b27302aa7..edfd6abcc356 100644 --- a/src/audio/module_adapter/module/volume/volume_generic.c +++ b/src/audio/module_adapter/module/volume/volume_generic.c @@ -27,7 +27,7 @@ LOG_MODULE_DECLARE(volume_generic, CONFIG_SOF_LOG_LEVEL); #include -#ifdef CONFIG_GENERIC +#ifdef VOLUME_GENERIC #if (!CONFIG_COMP_PEAK_VOL) @@ -93,6 +93,47 @@ static void vol_s24_to_s24(struct processing_module *mod, struct input_stream_bu y = audio_stream_wrap(sink, y + n); } } + +/** + * \brief Volume passthrough from 24/32 bit to 24/32 bit. + * \param[in,out] dev Volume base component device. + * \param[in,out] sink Destination buffer. + * \param[in,out] source Input buffer. + * \param[in] frames Number of frames to process. + * \param[in] attenuation factor for peakmeter adjustment (unused) + * + * Copy and scale volume from 24/32 bit source buffer + * to 24/32 bit destination buffer. + */ +static void vol_passthrough_s24_to_s24(struct processing_module *mod, + struct input_stream_buffer *bsource, + struct output_stream_buffer *bsink, uint32_t frames, + uint32_t attenuation) +{ + struct audio_stream __sparse_cache *source = bsource->data; + struct audio_stream __sparse_cache *sink = bsink->data; + int32_t *x; + int32_t *y; + int nmax, n; + const int nch = audio_stream_get_channels(source); + int remaining_samples = frames * nch; + + x = audio_stream_wrap(source, (char *)audio_stream_get_rptr(source) + bsource->consumed); + y = audio_stream_wrap(sink, (char *)audio_stream_get_wptr(sink) + bsink->size); + + bsource->consumed += VOL_S32_SAMPLES_TO_BYTES(remaining_samples); + bsink->size += VOL_S32_SAMPLES_TO_BYTES(remaining_samples); + while (remaining_samples) { + nmax = audio_stream_samples_without_wrap_s24(source, x); + n = MIN(remaining_samples, nmax); + nmax = audio_stream_samples_without_wrap_s24(sink, y); + n = MIN(n, nmax); + memcpy_s(y, n * sizeof(int32_t), x, n * sizeof(int32_t)); + remaining_samples -= n; + x = audio_stream_wrap(source, x + n); + y = audio_stream_wrap(sink, y + n); + } +} #endif /* CONFIG_FORMAT_S24LE */ #if CONFIG_FORMAT_S32LE @@ -147,6 +188,46 @@ static void vol_s32_to_s32(struct processing_module *mod, struct input_stream_bu y = audio_stream_wrap(sink, y + n); } } + +/** + * \brief Volume passthrough from 32 bit to 32 bit. + * \param[in,out] dev Volume base component device. + * \param[in,out] sink Destination buffer. + * \param[in,out] source Input buffer. + * \param[in] frames Number of frames to process. + * \param[in] attenuation factor for peakmeter adjustment (unused) + * + * Copy and scale volume from 32 bit source buffer + * to 32 bit destination buffer. + */ +static void vol_passthrough_s32_to_s32(struct processing_module *mod, + struct input_stream_buffer *bsource, + struct output_stream_buffer *bsink, uint32_t frames, + uint32_t attenuation) +{ + struct audio_stream __sparse_cache *source = bsource->data; + struct audio_stream __sparse_cache *sink = bsink->data; + int32_t *x; + int32_t *y; + int nmax, n; + const int nch = audio_stream_get_channels(source); + int remaining_samples = frames * nch; + + x = audio_stream_wrap(source, (char *)audio_stream_get_rptr(source) + bsource->consumed); + y = audio_stream_wrap(sink, (char *)audio_stream_get_wptr(sink) + bsink->size); + bsource->consumed += VOL_S32_SAMPLES_TO_BYTES(remaining_samples); + bsink->size += VOL_S32_SAMPLES_TO_BYTES(remaining_samples); + while (remaining_samples) { + nmax = audio_stream_samples_without_wrap_s32(source, x); + n = MIN(remaining_samples, nmax); + nmax = audio_stream_samples_without_wrap_s32(sink, y); + n = MIN(n, nmax); + memcpy_s(y, n * sizeof(int32_t), x, n * sizeof(int32_t)); + remaining_samples -= n; + x = audio_stream_wrap(source, x + n); + y = audio_stream_wrap(sink, y + n); + } +} #endif /* CONFIG_FORMAT_S32LE */ #if CONFIG_FORMAT_S16LE @@ -198,18 +279,58 @@ static void vol_s16_to_s16(struct processing_module *mod, struct input_stream_bu y = audio_stream_wrap(sink, y + n); } } + +/** + * \brief Volume passthrough from 16 bit to 16 bit. + * \param[in,out] dev Volume base component device. + * \param[in,out] sink Destination buffer. + * \param[in,out] source Input buffer. + * \param[in] frames Number of frames to process. + * \param[in] attenuation factor for peakmeter adjustment (unused) + * + * Copy and scale volume from 16 bit source buffer + * to 16 bit destination buffer. + */ +static void vol_passthrough_s16_to_s16(struct processing_module *mod, + struct input_stream_buffer *bsource, + struct output_stream_buffer *bsink, uint32_t frames, + uint32_t attenuation) +{ + struct audio_stream __sparse_cache *source = bsource->data; + struct audio_stream __sparse_cache *sink = bsink->data; + int16_t *x; + int16_t *y; + int nmax, n; + const int nch = audio_stream_get_channels(source); + int remaining_samples = frames * nch; + + x = audio_stream_wrap(source, (char *)audio_stream_get_rptr(source) + bsource->consumed); + y = audio_stream_wrap(sink, (char *)audio_stream_get_wptr(sink) + bsink->size); + bsource->consumed += VOL_S16_SAMPLES_TO_BYTES(remaining_samples); + bsink->size += VOL_S16_SAMPLES_TO_BYTES(remaining_samples); + while (remaining_samples) { + nmax = audio_stream_samples_without_wrap_s16(source, x); + n = MIN(remaining_samples, nmax); + nmax = audio_stream_samples_without_wrap_s16(sink, y); + n = MIN(n, nmax); + memcpy_s(y, n * sizeof(int16_t), x, n * sizeof(int16_t)); + remaining_samples -= n; + x = audio_stream_wrap(source, x + n); + y = audio_stream_wrap(sink, y + n); + } +} #endif /* CONFIG_FORMAT_S16LE */ const struct comp_func_map volume_func_map[] = { #if CONFIG_FORMAT_S16LE - { SOF_IPC_FRAME_S16_LE, vol_s16_to_s16 }, -#endif /* CONFIG_FORMAT_S16LE */ + { SOF_IPC_FRAME_S16_LE, vol_s16_to_s16, vol_passthrough_s16_to_s16}, +#endif #if CONFIG_FORMAT_S24LE - { SOF_IPC_FRAME_S24_4LE, vol_s24_to_s24 }, -#endif /* CONFIG_FORMAT_S24LE */ + { SOF_IPC_FRAME_S24_4LE, vol_s24_to_s24, vol_passthrough_s24_to_s24}, +#endif #if CONFIG_FORMAT_S32LE - { SOF_IPC_FRAME_S32_LE, vol_s32_to_s32 }, -#endif /* CONFIG_FORMAT_S32LE */ + { SOF_IPC_FRAME_S32_LE, vol_s32_to_s32, vol_passthrough_s32_to_s32}, +#endif }; const size_t volume_func_count = ARRAY_SIZE(volume_func_map); diff --git a/src/audio/module_adapter/module/volume/volume_generic_with_peakvol.c b/src/audio/module_adapter/module/volume/volume_generic_with_peakvol.c index 0fd966ac221e..35861a31bbe0 100644 --- a/src/audio/module_adapter/module/volume/volume_generic_with_peakvol.c +++ b/src/audio/module_adapter/module/volume/volume_generic_with_peakvol.c @@ -23,7 +23,7 @@ LOG_MODULE_DECLARE(volume_generic, CONFIG_SOF_LOG_LEVEL); #include -#ifdef CONFIG_GENERIC +#ifdef VOLUME_GENERIC #if CONFIG_COMP_PEAK_VOL @@ -94,6 +94,59 @@ static void vol_s24_to_s24(struct processing_module *mod, struct input_stream_bu y = audio_stream_wrap(sink, y + n); } } + +/** + * \brief Volume passthrough from 24/32 bit to 24/32 bit. + * \param[in,out] dev Volume base component device. + * \param[in,out] sink Destination buffer. + * \param[in,out] source Input buffer. + * \param[in] frames Number of frames to process. + * \param[in] attenuation factor for peakmeter adjustment + * + * Copy and scale volume from 24/32 bit source buffer + * to 24/32 bit destination buffer. + */ +static void vol_passthrough_s24_to_s24(struct processing_module *mod, + struct input_stream_buffer *bsource, + struct output_stream_buffer *bsink, uint32_t frames, + uint32_t attenuation) +{ + struct vol_data *cd = module_get_private_data(mod); + struct audio_stream __sparse_cache *source = bsource->data; + struct audio_stream __sparse_cache *sink = bsink->data; + int32_t *x, *x0; + int32_t *y, *y0; + int nmax, n, i, j; + const int nch = audio_stream_get_channels(source); + int remaining_samples = frames * nch; + int32_t tmp; + + x = audio_stream_wrap(source, (char *)audio_stream_get_rptr(source) + bsource->consumed); + y = audio_stream_wrap(sink, (char *)audio_stream_get_wptr(sink) + bsink->size); + + bsource->consumed += VOL_S32_SAMPLES_TO_BYTES(remaining_samples); + bsink->size += VOL_S32_SAMPLES_TO_BYTES(remaining_samples); + while (remaining_samples) { + nmax = audio_stream_samples_without_wrap_s24(source, x); + n = MIN(remaining_samples, nmax); + nmax = audio_stream_samples_without_wrap_s24(sink, y); + n = MIN(n, nmax); + for (j = 0; j < nch; j++) { + x0 = x + j; + y0 = y + j; + tmp = 0; + for (i = 0; i < n; i += nch) { + y0[i] = x0[i]; + tmp = MAX(abs(x0[i]), tmp); + } + tmp = tmp << (attenuation + PEAK_24S_32C_ADJUST); + cd->peak_regs.peak_meter[j] = MAX(tmp, cd->peak_regs.peak_meter[j]); + } + remaining_samples -= n; + x = audio_stream_wrap(source, x + n); + y = audio_stream_wrap(sink, y + n); + } +} #endif /* CONFIG_FORMAT_S24LE */ #if CONFIG_FORMAT_S32LE @@ -153,6 +206,61 @@ static void vol_s32_to_s32(struct processing_module *mod, struct input_stream_bu y = audio_stream_wrap(sink, y + n); } } + +/** + * \brief Volume passthrough from 32 bit to 32 bit. + * \param[in,out] dev Volume base component device. + * \param[in,out] sink Destination buffer. + * \param[in,out] source Input buffer. + * \param[in] frames Number of frames to process. + * \param[in] attenuation factor for peakmeter adjustment + * + * Copy and scale volume from 32 bit source buffer + * to 32 bit destination buffer. + */ +static void vol_passthrough_s32_to_s32(struct processing_module *mod, + struct input_stream_buffer *bsource, + struct output_stream_buffer *bsink, uint32_t frames, + uint32_t attenuation) +{ + struct vol_data *cd = module_get_private_data(mod); + struct audio_stream __sparse_cache *source = bsource->data; + struct audio_stream __sparse_cache *sink = bsink->data; + int32_t *x, *x0; + int32_t *y, *y0; + int nmax, n, i, j; + const int nch = audio_stream_get_channels(source); + int remaining_samples = frames * nch; + int32_t tmp; + + x = audio_stream_wrap(source, (char *)audio_stream_get_rptr(source) + bsource->consumed); + y = audio_stream_wrap(sink, (char *)audio_stream_get_wptr(sink) + bsink->size); + bsource->consumed += VOL_S32_SAMPLES_TO_BYTES(remaining_samples); + bsink->size += VOL_S32_SAMPLES_TO_BYTES(remaining_samples); + while (remaining_samples) { + nmax = audio_stream_samples_without_wrap_s32(source, x); + n = MIN(remaining_samples, nmax); + nmax = audio_stream_samples_without_wrap_s32(sink, y); + n = MIN(n, nmax); + /* Note: on Xtensa processing one channel volume at time performed slightly + * better than simpler interleaved code version (average 19 us vs. 20 us). + */ + for (j = 0; j < nch; j++) { + x0 = x + j; + y0 = y + j; + tmp = 0; + for (i = 0; i < n; i += nch) { + y0[i] = x0[i]; + tmp = MAX(abs(x0[i]), tmp); + } + tmp = tmp << attenuation; + cd->peak_regs.peak_meter[j] = MAX(tmp, cd->peak_regs.peak_meter[j]); + } + remaining_samples -= n; + x = audio_stream_wrap(source, x + n); + y = audio_stream_wrap(sink, y + n); + } +} #endif /* CONFIG_FORMAT_S32LE */ #if CONFIG_FORMAT_S16LE @@ -209,18 +317,70 @@ static void vol_s16_to_s16(struct processing_module *mod, struct input_stream_bu y = audio_stream_wrap(sink, y + n); } } + +/** + * \brief Volume passthrough from 16 bit to 16 bit. + * \param[in,out] dev Volume base component device. + * \param[in,out] sink Destination buffer. + * \param[in,out] source Input buffer. + * \param[in] frames Number of frames to process. + * \param[in] attenuation factor for peakmeter adjustment (unused for 16bit) + * + * Copy and scale volume from 16 bit source buffer + * to 16 bit destination buffer. + */ +static void vol_passthrough_s16_to_s16(struct processing_module *mod, + struct input_stream_buffer *bsource, + struct output_stream_buffer *bsink, uint32_t frames, + uint32_t attenuation) +{ + struct vol_data *cd = module_get_private_data(mod); + struct audio_stream __sparse_cache *source = bsource->data; + struct audio_stream __sparse_cache *sink = bsink->data; + int32_t *x, *x0; + int32_t *y, *y0; + int nmax, n, i, j; + const int nch = audio_stream_get_channels(source); + int remaining_samples = frames * nch; + int32_t tmp; + + x = audio_stream_wrap(source, (char *)audio_stream_get_rptr(source) + bsource->consumed); + y = audio_stream_wrap(sink, (char *)audio_stream_get_wptr(sink) + bsink->size); + + bsource->consumed += VOL_S16_SAMPLES_TO_BYTES(remaining_samples); + bsink->size += VOL_S16_SAMPLES_TO_BYTES(remaining_samples); + while (remaining_samples) { + nmax = audio_stream_samples_without_wrap_s16(source, x); + n = MIN(remaining_samples, nmax); + nmax = audio_stream_samples_without_wrap_s16(sink, y); + n = MIN(n, nmax); + for (j = 0; j < nch; j++) { + x0 = x + j; + y0 = y + j; + tmp = 0; + for (i = 0; i < n; i += nch) { + y0[i] = x0[i]; + tmp = MAX(abs(x0[i]), tmp); + } + cd->peak_regs.peak_meter[j] = MAX(tmp, cd->peak_regs.peak_meter[j]); + } + remaining_samples -= n; + x = audio_stream_wrap(source, x + n); + y = audio_stream_wrap(sink, y + n); + } +} #endif const struct comp_func_map volume_func_map[] = { #if CONFIG_FORMAT_S16LE - { SOF_IPC_FRAME_S16_LE, vol_s16_to_s16 }, -#endif /* CONFIG_FORMAT_S16LE */ + { SOF_IPC_FRAME_S16_LE, vol_s16_to_s16, vol_passthrough_s16_to_s16}, +#endif #if CONFIG_FORMAT_S24LE - { SOF_IPC_FRAME_S24_4LE, vol_s24_to_s24 }, -#endif /* CONFIG_FORMAT_S24LE */ + { SOF_IPC_FRAME_S24_4LE, vol_s24_to_s24, vol_passthrough_s24_to_s24}, +#endif #if CONFIG_FORMAT_S32LE - { SOF_IPC_FRAME_S32_LE, vol_s32_to_s32 }, -#endif /* CONFIG_FORMAT_S32LE */ + { SOF_IPC_FRAME_S32_LE, vol_s32_to_s32, vol_passthrough_s32_to_s32}, +#endif }; const size_t volume_func_count = ARRAY_SIZE(volume_func_map); diff --git a/src/audio/module_adapter/module/volume/volume_hifi3.c b/src/audio/module_adapter/module/volume/volume_hifi3.c index c905ad5601dd..df46eeb9969d 100644 --- a/src/audio/module_adapter/module/volume/volume_hifi3.c +++ b/src/audio/module_adapter/module/volume/volume_hifi3.c @@ -21,7 +21,9 @@ LOG_MODULE_DECLARE(volume_hifi3, CONFIG_SOF_LOG_LEVEL); #include -#if defined(__XCC__) && XCHAL_HAVE_HIFI3 && (!CONFIG_COMP_PEAK_VOL) +#ifdef VOLUME_HIFI3 + +#if (!CONFIG_COMP_PEAK_VOL) #include @@ -131,6 +133,55 @@ static void vol_s24_to_s24_s32(struct processing_module *mod, struct input_strea out = audio_stream_wrap(sink, out); } } + +/** + * \brief HiFi3 enabled volume passthrough from 24/32 bit to 24/32 or 32 bit. + * \param[in,out] dev Volume base component device. + * \param[in,out] sink Destination buffer. + * \param[in,out] source Input buffer. + * \param[in] frames Number of frames to process. + * \param[in] attenuation factor for peakmeter adjustment (unused) + */ +static void vol_passthrough_s24_to_s24_s32(struct processing_module *mod, + struct input_stream_buffer *bsource, + struct output_stream_buffer *bsink, uint32_t frames, + uint32_t attenuation) +{ + struct audio_stream __sparse_cache *source = bsource->data; + struct audio_stream __sparse_cache *sink = bsink->data; + ae_f32x2 in_sample = AE_ZERO32(); + int i, n, m; + ae_valign inu = AE_ZALIGN64(); + ae_valign outu = AE_ZALIGN64(); + ae_f32x2 *in = (ae_f32x2 *)audio_stream_wrap(source, (char *)audio_stream_get_rptr(source) + + bsource->consumed); + ae_f32x2 *out = (ae_f32x2 *)audio_stream_wrap(sink, (char *)audio_stream_get_wptr(sink) + + bsink->size); + const int channels_count = audio_stream_get_channels(sink); + int samples = channels_count * frames; + + bsource->consumed += VOL_S32_SAMPLES_TO_BYTES(samples); + bsink->size += VOL_S32_SAMPLES_TO_BYTES(samples); + + while (samples) { + m = audio_stream_samples_without_wrap_s24(source, in); + n = MIN(m, samples); + m = audio_stream_samples_without_wrap_s24(sink, out); + n = MIN(m, n); + inu = AE_LA64_PP(in); + /* process two continuous sample data once */ + for (i = 0; i < n; i += 2) { + /* Load the input sample */ + AE_LA32X2_IP(in_sample, inu, in); + /* Store the output sample */ + AE_SA32X2_IP(in_sample, outu, out); + } + AE_SA64POS_FP(outu, out); + samples -= n; + in = audio_stream_wrap(source, in); + out = audio_stream_wrap(sink, out); + } +} #endif /* CONFIG_FORMAT_S24LE */ #if CONFIG_FORMAT_S32LE @@ -224,6 +275,54 @@ static void vol_s32_to_s24_s32(struct processing_module *mod, struct input_strea out = audio_stream_wrap(sink, out); } } + +/** + * \brief HiFi3 enabled volume passthrough from 32 bit to 24/32 or 32 bit. + * \param[in,out] mod Pointer to struct processing_module + * \param[in,out] sink Destination buffer. + * \param[in,out] source Input buffer. + * \param[in] frames Number of frames to process. + * \param[in] attenuation factor for peakmeter adjustment (unused) + */ +static void vol_passthrough_s32_to_s24_s32(struct processing_module *mod, + struct input_stream_buffer *bsource, + struct output_stream_buffer *bsink, uint32_t frames, + uint32_t attenuation) +{ + struct audio_stream __sparse_cache *source = bsource->data; + struct audio_stream __sparse_cache *sink = bsink->data; + ae_f32x2 in_sample = AE_ZERO32(); + int i, n, m; + ae_valign inu = AE_ZALIGN64(); + ae_valign outu = AE_ZALIGN64(); + const int channels_count = audio_stream_get_channels(sink); + int samples = channels_count * frames; + ae_f32x2 *in = (ae_f32x2 *)audio_stream_wrap(source, (char *)audio_stream_get_rptr(source) + + bsource->consumed); + ae_f32x2 *out = (ae_f32x2 *)audio_stream_wrap(sink, (char *)audio_stream_get_wptr(sink) + + bsink->size); + + bsource->consumed += VOL_S32_SAMPLES_TO_BYTES(samples); + bsink->size += VOL_S32_SAMPLES_TO_BYTES(samples); + + while (samples) { + m = audio_stream_samples_without_wrap_s32(source, in); + n = MIN(m, samples); + m = audio_stream_samples_without_wrap_s32(sink, out); + n = MIN(m, n); + inu = AE_LA64_PP(in); + /* process two continuous sample data once */ + for (i = 0; i < n; i += 2) { + /* Load the input sample */ + AE_LA32X2_IP(in_sample, inu, in); + AE_SA32X2_IP(in_sample, outu, out); + } + AE_SA64POS_FP(outu, out); + samples -= n; + in = audio_stream_wrap(source, in); + out = audio_stream_wrap(sink, out); + } +} #endif /* CONFIG_FORMAT_S32LE */ #if CONFIG_FORMAT_S16LE @@ -323,19 +422,65 @@ static void vol_s16_to_s16(struct processing_module *mod, struct input_stream_bu out = audio_stream_wrap(sink, out); } } + +/** + * \brief HiFi3 enabled volume passthrough from 16 bit to 16 bit. + * \param[in,out] dev Volume base component device. + * \param[in,out] sink Destination buffer. + * \param[in,out] source Input buffer. + * \param[in] frames Number of frames to process. + * \param[in] attenuation factor for peakmeter adjustment (unused) + */ +static void vol_passthrough_s16_to_s16(struct processing_module *mod, + struct input_stream_buffer *bsource, + struct output_stream_buffer *bsink, uint32_t frames, + uint32_t attenuation) +{ + struct audio_stream __sparse_cache *source = bsource->data; + struct audio_stream __sparse_cache *sink = bsink->data; + ae_f16x4 in_sample = AE_ZERO16(); + int i, n, m; + ae_valign inu = AE_ZALIGN64(); + ae_valign outu = AE_ZALIGN64(); + ae_f16x4 *in = (ae_f16x4 *)audio_stream_wrap(source, (char *)audio_stream_get_rptr(source) + + bsource->consumed); + ae_f16x4 *out = (ae_f16x4 *)audio_stream_wrap(sink, (char *)audio_stream_get_wptr(sink) + + bsink->size); + const int channels_count = audio_stream_get_channels(sink); + int samples = channels_count * frames; + + while (samples) { + m = audio_stream_samples_without_wrap_s16(source, in); + n = MIN(m, samples); + m = audio_stream_samples_without_wrap_s16(sink, out); + n = MIN(m, n); + inu = AE_LA64_PP(in); + for (i = 0; i < n; i += 4) { + /* Load the input sample */ + AE_LA16X4_IP(in_sample, inu, in); + AE_SA16X4_IP(in_sample, outu, out); + } + AE_SA64POS_FP(outu, out); + samples -= n; + bsource->consumed += VOL_S16_SAMPLES_TO_BYTES(n); + bsink->size += VOL_S16_SAMPLES_TO_BYTES(n); + in = audio_stream_wrap(source, in); + out = audio_stream_wrap(sink, out); + } +} #endif /* CONFIG_FORMAT_S16LE */ const struct comp_func_map volume_func_map[] = { #if CONFIG_FORMAT_S16LE - { SOF_IPC_FRAME_S16_LE, vol_s16_to_s16 }, + { SOF_IPC_FRAME_S16_LE, vol_s16_to_s16, vol_passthrough_s16_to_s16}, #endif #if CONFIG_FORMAT_S24LE - { SOF_IPC_FRAME_S24_4LE, vol_s24_to_s24_s32 }, + { SOF_IPC_FRAME_S24_4LE, vol_s24_to_s24_s32, vol_passthrough_s24_to_s24_s32 }, #endif #if CONFIG_FORMAT_S32LE - { SOF_IPC_FRAME_S32_LE, vol_s32_to_s24_s32 }, + { SOF_IPC_FRAME_S32_LE, vol_s32_to_s24_s32, vol_passthrough_s32_to_s24_s32 }, #endif }; const size_t volume_func_count = ARRAY_SIZE(volume_func_map); - +#endif #endif diff --git a/src/audio/module_adapter/module/volume/volume_hifi3_with_peakvol.c b/src/audio/module_adapter/module/volume/volume_hifi3_with_peakvol.c index c0db21caa508..29ab315d74b7 100644 --- a/src/audio/module_adapter/module/volume/volume_hifi3_with_peakvol.c +++ b/src/audio/module_adapter/module/volume/volume_hifi3_with_peakvol.c @@ -6,7 +6,7 @@ /** * \file - * \brief Volume HiFi3 & 4 processing implementation with peak volume detection + * \brief Volume HiFi3 processing implementation with peak volume detection * \authors Andrula Song */ @@ -21,24 +21,11 @@ LOG_MODULE_DECLARE(volume_hifi3, CONFIG_SOF_LOG_LEVEL); #include -#if defined(__XCC__) && XCHAL_HAVE_HIFI3 && CONFIG_COMP_PEAK_VOL +#ifdef VOLUME_HIFI3 -#if XCHAL_HAVE_HIFI4 /* HIFI4 VERSION */ -#include +#if CONFIG_COMP_PEAK_VOL -static inline void vol_store_gain(struct vol_data *cd, const int channels_count) -{ - int32_t i; - - /* using for loop instead of memcpy_s(), because for loop costs less cycles */ - for (i = 0; i < channels_count; i++) { - cd->vol[i] = cd->volume[i]; - cd->vol[i + channels_count * 1] = cd->volume[i]; - cd->vol[i + channels_count * 2] = cd->volume[i]; - cd->vol[i + channels_count * 3] = cd->volume[i]; - } - cd->copy_gain = false; -} +#include #if CONFIG_FORMAT_S24LE /** @@ -59,323 +46,132 @@ static void vol_s24_to_s24_s32(struct processing_module *mod, struct input_strea ae_f32x2 in_sample = AE_ZERO32(); ae_f32x2 out_sample = AE_ZERO32(); ae_f32x2 volume = AE_ZERO32(); - int i, n, m; - ae_f32x2 *vol; - ae_valign inu = AE_ZALIGN64(); - ae_valign outu = AE_ZALIGN64(); - ae_f32x2 *in = (ae_f32x2 *)audio_stream_wrap(source, (char *)audio_stream_get_rptr(source) - + bsource->consumed); - ae_f32x2 *out = (ae_f32x2 *)audio_stream_wrap(sink, (char *)audio_stream_get_wptr(sink) - + bsink->size); + int channel, n, i, m; + ae_f32 *in0 = (ae_f32 *)audio_stream_wrap(source, (char *)audio_stream_get_rptr(source) + + bsource->consumed); + ae_f32 *out0 = (ae_f32 *)audio_stream_wrap(sink, (char *)audio_stream_get_wptr(sink) + + bsink->size); + ae_f32 *in, *out; const int channels_count = audio_stream_get_channels(sink); - const int inc = sizeof(ae_f32x2); + const int inc = sizeof(ae_f32) * channels_count; int samples = channels_count * frames; - ae_f32x2 temp; - ae_f32x2 *peakvol = (ae_f32x2 *)cd->peak_vol; - - /* Set peakvol(which stores the peak volume data twice) as circular buffer */ - memset(peakvol, 0, sizeof(ae_f32) * channels_count * 2); - AE_SETCBEGIN1(cd->peak_vol); - AE_SETCEND1(cd->peak_vol + channels_count * 2); - - /** to ensure the adsress is 8-byte aligned and avoid risk of - * error loading of volume gain while the cd->vol would be set - * as circular buffer - */ - if (cd->copy_gain) - vol_store_gain(cd, channels_count); - - vol = (ae_f32x2 *)cd->vol; - /* Set buf who stores the volume gain data as circular buffer */ - AE_SETCBEGIN0(vol); - AE_SETCEND0(cd->vol + channels_count * 2); + ae_f32x2 peak_vol; + uint32_t *peak_meter = cd->peak_regs.peak_meter; bsource->consumed += VOL_S32_SAMPLES_TO_BYTES(samples); bsink->size += VOL_S32_SAMPLES_TO_BYTES(samples); - while (samples) { - m = audio_stream_samples_without_wrap_s32(source, in); + m = audio_stream_samples_without_wrap_s32(source, in0); n = MIN(m, samples); - m = audio_stream_samples_without_wrap_s16(sink, out); + m = audio_stream_samples_without_wrap_s32(sink, out0); n = MIN(m, n); - inu = AE_LA64_PP(in); - /* process two continuous sample data once */ - for (i = 0; i < n; i += 2) { - /* Load the volume value */ - AE_L32X2_XC(volume, vol, inc); - - /* Load the input sample */ - AE_LA32X2_IP(in_sample, inu, in); - /* calculate the peak volume*/ - AE_L32X2_XC1(temp, peakvol, 0); - temp = AE_MAXABS32S(in_sample, temp); - AE_S32X2_XC1(temp, peakvol, inc); - /* Multiply the input sample */ + for (channel = 0; channel < channels_count; channel++) { + peak_vol = AE_ZERO32(); + /* set start address of sample load */ + in = in0 + channel; + /* set start address of sample store */ + out = out0 + channel; + /* Load volume */ + volume = (ae_f32x2)cd->volume[channel]; + for (i = 0; i < n; i += channels_count) { + /* Load the input sample */ + AE_L32_XP(in_sample, in, inc); + /* calc peak vol */ + peak_vol = AE_MAXABS32S(in_sample, peak_vol); #if COMP_VOLUME_Q8_16 - out_sample = AE_MULFP32X2RS(AE_SLAI32S(volume, 7), AE_SLAI32(in_sample, 8)); + /* Multiply the input sample */ + out_sample = AE_MULFP32X2RS(AE_SLAI32S(volume, 7), + AE_SLAI32(in_sample, 8)); #elif COMP_VOLUME_Q1_23 - out_sample = AE_MULFP32X2RS(volume, AE_SLAI32(in_sample, 8)); + out_sample = AE_MULFP32X2RS(volume, AE_SLAI32S(in_sample, 8)); #else #error "Need CONFIG_COMP_VOLUME_Qx_y" #endif - - /* Shift for S24_LE */ - out_sample = AE_SLAI32S(out_sample, 8); - out_sample = AE_SRAI32(out_sample, 8); - - /* Store the output sample */ - AE_SA32X2_IP(out_sample, outu, out); + /* Shift for S24_LE */ + out_sample = AE_SLAI32S(out_sample, 8); + out_sample = AE_SRAI32(out_sample, 8); + /* Store the output sample */ + AE_S32_L_XP(out_sample, out, inc); + } + peak_vol = AE_SLAA32S(peak_vol, attenuation + PEAK_24S_32C_ADJUST); + peak_meter[channel] = AE_MAX32(peak_vol, peak_meter[channel]); } - AE_SA64POS_FP(outu, out); samples -= n; - in = audio_stream_wrap(source, in); - out = audio_stream_wrap(sink, out); + out0 = audio_stream_wrap(sink, out0 + n); + in0 = audio_stream_wrap(source, in0 + n); } - for (i = 0; i < channels_count; i++) - cd->peak_regs.peak_meter[i] = MAX(cd->peak_vol[i], - cd->peak_vol[i + channels_count]) - << (attenuation + PEAK_24S_32C_ADJUST); } -#endif /* CONFIG_FORMAT_S24LE */ -#if CONFIG_FORMAT_S32LE /** - * \brief HiFi3 enabled volume processing from 32 bit to 24/32 or 32 bit. - * \param[in,out] mod Pointer to struct processing_module + * \brief HiFi3 enabled volume passthrough from 24/32 bit to 24/32 or 32 bit. + * \param[in,out] dev Volume base component device. * \param[in,out] sink Destination buffer. * \param[in,out] source Input buffer. * \param[in] frames Number of frames to process. * \param[in] attenuation factor for peakmeter adjustment */ -static void vol_s32_to_s24_s32(struct processing_module *mod, struct input_stream_buffer *bsource, - struct output_stream_buffer *bsink, uint32_t frames, - uint32_t attenuation) +static void vol_passthrough_s24_to_s24_s32(struct processing_module *mod, + struct input_stream_buffer *bsource, + struct output_stream_buffer *bsink, uint32_t frames, + uint32_t attenuation) { struct vol_data *cd = module_get_private_data(mod); struct audio_stream __sparse_cache *source = bsource->data; struct audio_stream __sparse_cache *sink = bsink->data; ae_f32x2 in_sample = AE_ZERO32(); - ae_f32x2 out_sample = AE_ZERO32(); - ae_f32x2 volume = AE_ZERO32(); - int i, n, m; - ae_f64 mult0; - ae_f64 mult1; - ae_f32x2 *buf; - ae_f32x2 *buf_end; - ae_f32x2 *vol; - ae_valign inu = AE_ZALIGN64(); - ae_valign outu = AE_ZALIGN64(); + int channel, n, i, m; + ae_f32 *in0 = (ae_f32 *)audio_stream_wrap(source, (char *)audio_stream_get_rptr(source) + + bsource->consumed); + ae_f32 *out0 = (ae_f32 *)audio_stream_wrap(sink, (char *)audio_stream_get_wptr(sink) + + bsink->size); + ae_f32 *in, *out; const int channels_count = audio_stream_get_channels(sink); - const int inc = sizeof(ae_f32x2); + const int inc = sizeof(ae_f32) * channels_count; int samples = channels_count * frames; - ae_f32x2 *in = (ae_f32x2 *)audio_stream_wrap(source, (char *)audio_stream_get_rptr(source) - + bsource->consumed); - ae_f32x2 *out = (ae_f32x2 *)audio_stream_wrap(sink, (char *)audio_stream_get_wptr(sink) - + bsink->size); - ae_f32x2 temp; - ae_f32x2 *peakvol = (ae_f32x2 *)cd->peak_vol; - - /* Set peakvol(which stores the peak volume data twice) as circular buffer */ - memset(peakvol, 0, sizeof(ae_f32) * channels_count * 2); - AE_SETCBEGIN1(cd->peak_vol); - AE_SETCEND1(cd->peak_vol + channels_count * 2); - - /** to ensure the address is 8-byte aligned and avoid risk of - * error loading of volume gain while the cd->vol would be set - * as circular buffer - */ - if (cd->copy_gain) - vol_store_gain(cd, channels_count); - - buf = (ae_f32x2 *)cd->vol; - buf_end = (ae_f32x2 *)(cd->vol + channels_count * 2); - vol = buf; - /* Set buf who stores the volume gain data as circular buffer */ - AE_SETCBEGIN0(buf); - AE_SETCEND0(buf_end); + ae_f32x2 peak_vol; + uint32_t *peak_meter = cd->peak_regs.peak_meter; bsource->consumed += VOL_S32_SAMPLES_TO_BYTES(samples); bsink->size += VOL_S32_SAMPLES_TO_BYTES(samples); - - while (samples) { - m = audio_stream_samples_without_wrap_s32(source, in); - n = MIN(m, samples); - m = audio_stream_samples_without_wrap_s32(sink, out); - n = MIN(m, n); - inu = AE_LA64_PP(in); - /* process two continuous sample data once */ - for (i = 0; i < n; i += 2) { - /* Load the volume value */ - AE_L32X2_XC(volume, vol, inc); - - /* Load the input sample */ - AE_LA32X2_IP(in_sample, inu, in); - /* calculate the peak volume*/ - AE_L32X2_XC1(temp, peakvol, 0); - temp = AE_MAXABS32S(in_sample, temp); - AE_S32X2_XC1(temp, peakvol, inc); -#if COMP_VOLUME_Q8_16 - /* Q8.16 x Q1.31 << 1 -> Q9.48 */ - mult0 = AE_MULF32S_HH(volume, in_sample); - mult0 = AE_SRAI64(mult0, 1); /* Q9.47 */ - mult1 = AE_MULF32S_LL(volume, in_sample); - mult1 = AE_SRAI64(mult1, 1); - out_sample = AE_ROUND32X2F48SSYM(mult0, mult1); /* Q9.47 -> Q1.31 */ -#elif COMP_VOLUME_Q1_23 - /* Q1.23 x Q1.31 << 1 -> Q2.55 */ - mult0 = AE_MULF32S_HH(volume, in_sample); - mult0 = AE_SRAI64(mult0, 8); /* Q2.47 */ - mult1 = AE_MULF32S_LL(volume, in_sample); - mult1 = AE_SRAI64(mult1, 8); - out_sample = AE_ROUND32X2F48SSYM(mult0, mult1); /* Q2.47 -> Q1.31 */ -#else -#error "Need CONFIG_COMP_VOLUME_Qx_y" -#endif - AE_SA32X2_IP(out_sample, outu, out); - } - AE_SA64POS_FP(outu, out); - samples -= n; - in = audio_stream_wrap(source, in); - out = audio_stream_wrap(sink, out); - } - for (i = 0; i < channels_count; i++) - cd->peak_regs.peak_meter[i] = MAX(cd->peak_vol[i], - cd->peak_vol[i + channels_count]) << attenuation; -} -#endif /* CONFIG_FORMAT_S32LE */ - -#if CONFIG_FORMAT_S16LE -/** - * \brief HiFi3 enabled volume processing from 16 bit to 16 bit. - * \param[in,out] dev Volume base component device. - * \param[in,out] sink Destination buffer. - * \param[in,out] source Input buffer. - * \param[in] frames Number of frames to process. - * \param[in] attenuation factor for peakmeter adjustment (unused for 16bit) - */ -static void vol_s16_to_s16(struct processing_module *mod, struct input_stream_buffer *bsource, - struct output_stream_buffer *bsink, uint32_t frames, - uint32_t attenuation) -{ - struct vol_data *cd = module_get_private_data(mod); - struct audio_stream __sparse_cache *source = bsource->data; - struct audio_stream __sparse_cache *sink = bsink->data; - ae_f32x2 volume0 = AE_ZERO32(); - ae_f32x2 volume1 = AE_ZERO32(); - ae_f32x2 out_sample0 = AE_ZERO32(); - ae_f32x2 out_sample1 = AE_ZERO32(); - ae_f16x4 in_sample = AE_ZERO16(); - ae_f16x4 out_sample = AE_ZERO16(); - int i, n, m; - ae_f32x2 *buf; - ae_f32x2 *buf_end; - ae_f32x2 *vol; - ae_valign inu = AE_ZALIGN64(); - ae_valign outu = AE_ZALIGN64(); - ae_f16x4 *in = (ae_f16x4 *)audio_stream_wrap(source, (char *)audio_stream_get_rptr(source) - + bsource->consumed); - ae_f16x4 *out = (ae_f16x4 *)audio_stream_wrap(sink, (char *)audio_stream_get_wptr(sink) - + bsink->size); - const int channels_count = audio_stream_get_channels(sink); - const int inc = sizeof(ae_f32x2); - int samples = channels_count * frames; - ae_f32x2 temp; - ae_f32x2 *peakvol = (ae_f32x2 *)cd->peak_vol; - - /* Set peakvol(which stores the peak volume data 4 times) as circular buffer */ - memset(peakvol, 0, sizeof(ae_f32) * channels_count * 4); - AE_SETCBEGIN1(cd->peak_vol); - AE_SETCEND1(cd->peak_vol + channels_count * 4); - - /** to ensure the adsress is 8-byte aligned and avoid risk of - * error loading of volume gain while the cd->vol would be set - * as circular buffer - */ - if (cd->copy_gain) - vol_store_gain(cd, channels_count); - - buf = (ae_f32x2 *)cd->vol; - buf_end = (ae_f32x2 *)(cd->vol + channels_count * 4); - vol = buf; - - /* Set buf as circular buffer */ - AE_SETCBEGIN0(buf); - AE_SETCEND0(buf_end); - while (samples) { - m = audio_stream_samples_without_wrap_s16(source, in); + m = audio_stream_samples_without_wrap_s32(source, in0); n = MIN(m, samples); - m = audio_stream_samples_without_wrap_s16(sink, out); + m = audio_stream_samples_without_wrap_s32(sink, out0); n = MIN(m, n); - inu = AE_LA64_PP(in); - for (i = 0; i < n; i += 4) { - /* load first two volume gain */ - AE_L32X2_XC(volume0, vol, inc); - - /* load second two volume gain */ - AE_L32X2_XC(volume1, vol, inc); - -#if COMP_VOLUME_Q8_16 - /* Q8.16 to Q9.23 */ - volume0 = AE_SLAI32S(volume0, 7); - volume1 = AE_SLAI32S(volume1, 7); -#elif COMP_VOLUME_Q1_23 - /* No need to shift, Q1.23 is OK as such */ -#else -#error "Need CONFIG_COMP_VOLUME_Qx_y" -#endif - /* Load the input sample */ - AE_LA16X4_IP(in_sample, inu, in); - /* calculate the peak volume*/ - AE_L32X2_XC1(temp, peakvol, 0); - temp = AE_MAXABS32S(AE_SEXT32X2D16_32(in_sample), temp); - AE_S32X2_XC1(temp, peakvol, inc); - /* calculate the peak volume*/ - AE_L32X2_XC1(temp, peakvol, 0); - temp = AE_MAXABS32S(AE_SEXT32X2D16_10(in_sample), temp); - AE_S32X2_XC1(temp, peakvol, inc); - /* Multiply the input sample */ - out_sample0 = AE_MULFP32X16X2RS_H(volume0, in_sample); - out_sample1 = AE_MULFP32X16X2RS_L(volume1, in_sample); - - /* Q9.23 to Q1.31 */ - out_sample0 = AE_SLAI32S(out_sample0, 8); - out_sample1 = AE_SLAI32S(out_sample1, 8); - - /* store the output */ - out_sample = AE_ROUND16X4F32SSYM(out_sample0, out_sample1); - AE_SA16X4_IP(out_sample, outu, out); + for (channel = 0; channel < channels_count; channel++) { + peak_vol = AE_ZERO32(); + /* set start address of sample load */ + in = in0 + channel; + /* set start address of sample store */ + out = out0 + channel; + for (i = 0; i < n; i += channels_count) { + /* Load the input sample */ + AE_L32_XP(in_sample, in, inc); + /* calc peak vol */ + peak_vol = AE_MAXABS32S(in_sample, peak_vol); + /* Store the output sample */ + AE_S32_L_XP(in_sample, out, inc); + } + peak_vol = AE_SLAA32S(peak_vol, attenuation + PEAK_24S_32C_ADJUST); + peak_meter[channel] = AE_MAX32(peak_vol, peak_meter[channel]); } - AE_SA64POS_FP(outu, out); samples -= n; - in = audio_stream_wrap(source, in); - out = audio_stream_wrap(sink, out); - bsource->consumed += VOL_S16_SAMPLES_TO_BYTES(n); - bsink->size += VOL_S16_SAMPLES_TO_BYTES(n); - } - for (i = 0; i < channels_count; i++) { - m = MAX(cd->peak_vol[i], cd->peak_vol[i + channels_count]); - m = MAX(m, cd->peak_vol[i + channels_count * 2]); - m = MAX(m, cd->peak_vol[i + channels_count * 3]); - cd->peak_regs.peak_meter[i] = m; + out0 = audio_stream_wrap(sink, out0 + n); + in0 = audio_stream_wrap(source, in0 + n); } } -#endif /* CONFIG_FORMAT_S16LE */ - -#else /* HIFI3 VERSION */ - -#include +#endif /* CONFIG_FORMAT_S24LE */ -#if CONFIG_FORMAT_S24LE +#if CONFIG_FORMAT_S32LE /** - * \brief HiFi3 enabled volume processing from 24/32 bit to 24/32 or 32 bit. - * \param[in,out] dev Volume base component device. + * \brief HiFi3 enabled volume processing from 32 bit to 24/32 or 32 bit. + * \param[in,out] mod Pointer to struct processing_module * \param[in,out] sink Destination buffer. * \param[in,out] source Input buffer. * \param[in] frames Number of frames to process. * \param[in] attenuation factor for peakmeter adjustment */ -static void vol_s24_to_s24_s32(struct processing_module *mod, struct input_stream_buffer *bsource, +static void vol_s32_to_s24_s32(struct processing_module *mod, struct input_stream_buffer *bsource, struct output_stream_buffer *bsink, uint32_t frames, uint32_t attenuation) { @@ -385,15 +181,16 @@ static void vol_s24_to_s24_s32(struct processing_module *mod, struct input_strea ae_f32x2 in_sample = AE_ZERO32(); ae_f32x2 out_sample = AE_ZERO32(); ae_f32x2 volume = AE_ZERO32(); - int channel, n, i, m; + int i, n, channel, m; + ae_f64 mult0; + const int channels_count = audio_stream_get_channels(sink); + const int inc = sizeof(ae_f32) * channels_count; + int samples = channels_count * frames; ae_f32 *in0 = (ae_f32 *)audio_stream_wrap(source, (char *)audio_stream_get_rptr(source) + bsource->consumed); ae_f32 *out0 = (ae_f32 *)audio_stream_wrap(sink, (char *)audio_stream_get_wptr(sink) + bsink->size); ae_f32 *in, *out; - const int channels_count = audio_stream_get_channels(sink); - const int inc = sizeof(ae_f32) * channels_count; - int samples = channels_count * frames; ae_f32x2 peak_vol; uint32_t *peak_meter = cd->peak_regs.peak_meter; @@ -418,21 +215,21 @@ static void vol_s24_to_s24_s32(struct processing_module *mod, struct input_strea /* calc peak vol */ peak_vol = AE_MAXABS32S(in_sample, peak_vol); #if COMP_VOLUME_Q8_16 - /* Multiply the input sample */ - out_sample = AE_MULFP32X2RS(AE_SLAI32S(volume, 7), - AE_SLAI32(in_sample, 8)); + /* Q8.16 x Q1.31 << 1 -> Q9.48 */ + mult0 = AE_MULF32S_HH(volume, in_sample); + mult0 = AE_SRAI64(mult0, 1); /* Q9.47 */ + out_sample = AE_ROUND32F48SASYM(mult0); /* Q9.47 -> Q1.31 */ #elif COMP_VOLUME_Q1_23 - out_sample = AE_MULFP32X2RS(volume, AE_SLAI32S(in_sample, 8)); + /* Q1.23 x Q1.31 << 1 -> Q2.55 */ + mult0 = AE_MULF32S_HH(volume, in_sample); + mult0 = AE_SRAI64(mult0, 8); /* Q2.47 */ + out_sample = AE_ROUND32F48SSYM(mult0); /* Q2.47 -> Q1.31 */ #else #error "Need CONFIG_COMP_VOLUME_Qx_y" #endif - /* Shift for S24_LE */ - out_sample = AE_SLAI32S(out_sample, 8); - out_sample = AE_SRAI32(out_sample, 8); - /* Store the output sample */ AE_S32_L_XP(out_sample, out, inc); } - peak_vol = AE_SLAA32S(peak_vol, attenuation + PEAK_24S_32C_ADJUST); + peak_vol = AE_SLAA32S(peak_vol, attenuation); peak_meter[channel] = AE_MAX32(peak_vol, peak_meter[channel]); } samples -= n; @@ -440,29 +237,25 @@ static void vol_s24_to_s24_s32(struct processing_module *mod, struct input_strea in0 = audio_stream_wrap(source, in0 + n); } } -#endif /* CONFIG_FORMAT_S24LE */ -#if CONFIG_FORMAT_S32LE /** - * \brief HiFi3 enabled volume processing from 32 bit to 24/32 or 32 bit. + * \brief HiFi3 enabled volume passthrough from 32 bit to 24/32 or 32 bit. * \param[in,out] mod Pointer to struct processing_module * \param[in,out] sink Destination buffer. * \param[in,out] source Input buffer. * \param[in] frames Number of frames to process. * \param[in] attenuation factor for peakmeter adjustment */ -static void vol_s32_to_s24_s32(struct processing_module *mod, struct input_stream_buffer *bsource, - struct output_stream_buffer *bsink, uint32_t frames, - uint32_t attenuation) +static void vol_passthrough_s32_to_s24_s32(struct processing_module *mod, + struct input_stream_buffer *bsource, + struct output_stream_buffer *bsink, uint32_t frames, + uint32_t attenuation) { struct vol_data *cd = module_get_private_data(mod); struct audio_stream __sparse_cache *source = bsource->data; struct audio_stream __sparse_cache *sink = bsink->data; ae_f32x2 in_sample = AE_ZERO32(); - ae_f32x2 out_sample = AE_ZERO32(); - ae_f32x2 volume = AE_ZERO32(); int i, n, channel, m; - ae_f64 mult0; const int channels_count = audio_stream_get_channels(sink); const int inc = sizeof(ae_f32) * channels_count; int samples = channels_count * frames; @@ -487,27 +280,13 @@ static void vol_s32_to_s24_s32(struct processing_module *mod, struct input_strea in = in0 + channel; /* set start address of sample store */ out = out0 + channel; - /* Load volume */ - volume = (ae_f32x2)cd->volume[channel]; for (i = 0; i < n; i += channels_count) { /* Load the input sample */ AE_L32_XP(in_sample, in, inc); /* calc peak vol */ peak_vol = AE_MAXABS32S(in_sample, peak_vol); -#if COMP_VOLUME_Q8_16 - /* Q8.16 x Q1.31 << 1 -> Q9.48 */ - mult0 = AE_MULF32S_HH(volume, in_sample); - mult0 = AE_SRAI64(mult0, 1); /* Q9.47 */ - out_sample = AE_ROUND32F48SASYM(mult0); /* Q9.47 -> Q1.31 */ -#elif COMP_VOLUME_Q1_23 - /* Q1.23 x Q1.31 << 1 -> Q2.55 */ - mult0 = AE_MULF32S_HH(volume, in_sample); - mult0 = AE_SRAI64(mult0, 8); /* Q2.47 */ - out_sample = AE_ROUND32F48SSYM(mult0); /* Q2.47 -> Q1.31 */ -#else -#error "Need CONFIG_COMP_VOLUME_Qx_y" -#endif - AE_S32_L_XP(out_sample, out, inc); + + AE_S32_L_XP(in_sample, out, inc); } peak_vol = AE_SLAA32S(peak_vol, attenuation); peak_meter[channel] = AE_MAX32(peak_vol, peak_meter[channel]); @@ -599,21 +378,81 @@ static void vol_s16_to_s16(struct processing_module *mod, struct input_stream_bu bsink->size += VOL_S16_SAMPLES_TO_BYTES(n); } } + +/** + * \brief HiFi3 enabled volume passthrough from 16 bit to 16 bit. + * \param[in,out] dev Volume base component device. + * \param[in,out] sink Destination buffer. + * \param[in,out] source Input buffer. + * \param[in] frames Number of frames to process. + * \param[in] attenuation factor for peakmeter adjustment (unused for 16bit) + */ +static void vol_passthrough_s16_to_s16(struct processing_module *mod, + struct input_stream_buffer *bsource, + struct output_stream_buffer *bsink, uint32_t frames, + uint32_t attenuation) +{ + struct vol_data *cd = module_get_private_data(mod); + struct audio_stream __sparse_cache *source = bsource->data; + struct audio_stream __sparse_cache *sink = bsink->data; + ae_f16x4 in_sample = AE_ZERO16(); + int i, n, channel, m; + ae_f16 *in; + ae_f16 *out; + ae_f16 *in0 = (ae_f16 *)audio_stream_wrap(source, (char *)audio_stream_get_rptr(source) + + bsource->consumed); + ae_f16 *out0 = (ae_f16 *)audio_stream_wrap(sink, (char *)audio_stream_get_wptr(sink) + + bsink->size); + const int channels_count = audio_stream_get_channels(sink); + const int inc = sizeof(ae_f16) * channels_count; + int samples = channels_count * frames; + ae_f32x2 peak_vol; + uint32_t *peak_meter = cd->peak_regs.peak_meter; + + while (samples) { + m = audio_stream_samples_without_wrap_s16(source, in0); + n = MIN(m, samples); + m = audio_stream_samples_without_wrap_s16(sink, out0); + n = MIN(m, n); + for (channel = 0; channel < channels_count; channel++) { + peak_vol = AE_ZERO32(); + /* set start address of sample load */ + in = in0 + channel; + /* set start address of sample store */ + out = out0 + channel; + for (i = 0; i < n; i += channels_count) { + /* Load the input sample */ + AE_L16_XP(in_sample, in, inc); + + /* calc peak vol */ + peak_vol = AE_MAXABS32S(AE_SEXT32X2D16_32(in_sample), peak_vol); + + /* store the output */ + AE_S16_0_XP(in_sample, out, inc); + } + peak_meter[channel] = AE_MAX32(peak_vol, peak_meter[channel]); + } + out0 = audio_stream_wrap(sink, out0 + n); + in0 = audio_stream_wrap(source, in0 + n); + samples -= n; + bsource->consumed += VOL_S16_SAMPLES_TO_BYTES(n); + bsink->size += VOL_S16_SAMPLES_TO_BYTES(n); + } +} #endif /* CONFIG_FORMAT_S16LE */ -#endif /* HIFI3 VERSION */ const struct comp_func_map volume_func_map[] = { #if CONFIG_FORMAT_S16LE - { SOF_IPC_FRAME_S16_LE, vol_s16_to_s16 }, + { SOF_IPC_FRAME_S16_LE, vol_s16_to_s16, vol_passthrough_s16_to_s16}, #endif #if CONFIG_FORMAT_S24LE - { SOF_IPC_FRAME_S24_4LE, vol_s24_to_s24_s32 }, + { SOF_IPC_FRAME_S24_4LE, vol_s24_to_s24_s32, vol_passthrough_s24_to_s24_s32}, #endif #if CONFIG_FORMAT_S32LE - { SOF_IPC_FRAME_S32_LE, vol_s32_to_s24_s32 }, + { SOF_IPC_FRAME_S32_LE, vol_s32_to_s24_s32, vol_passthrough_s32_to_s24_s32}, #endif }; const size_t volume_func_count = ARRAY_SIZE(volume_func_map); - +#endif #endif diff --git a/src/audio/module_adapter/module/volume/volume_hifi4.c b/src/audio/module_adapter/module/volume/volume_hifi4.c new file mode 100644 index 000000000000..6092054bdb52 --- /dev/null +++ b/src/audio/module_adapter/module/volume/volume_hifi4.c @@ -0,0 +1,484 @@ +// SPDX-License-Identifier: BSD-3-Clause +// +// Copyright(c) 2023 Intel Corporation. All rights reserved. +// +// Author: Andrula Song + +/** + * \file + * \brief Volume HiFi4 processing implementation without peak volume detection + * \authors Andrula Song + */ + +#include +#include +#include +#include +#include +#include + +LOG_MODULE_DECLARE(volume_hifi4, CONFIG_SOF_LOG_LEVEL); + +#include + +#ifdef VOLUME_HIFI4 + +#if (!CONFIG_COMP_PEAK_VOL) + +#include + +/** + * \brief store volume gain 4 times for xtensa multi-way intrinsic operations. + * Simultaneous processing 2 data. + * \param[in,out] cd Volume component private data. + * \param[in] channels_count Number of channels to process. + */ +static void vol_store_gain(struct vol_data *cd, const int channels_count) +{ + int32_t i; + + /* using for loop instead of memcpy_s(), because for loop costs less cycles */ + for (i = 0; i < channels_count; i++) { + cd->vol[i] = cd->volume[i]; + cd->vol[i + channels_count * 1] = cd->volume[i]; + cd->vol[i + channels_count * 2] = cd->volume[i]; + cd->vol[i + channels_count * 3] = cd->volume[i]; + } + cd->copy_gain = false; +} + +#if CONFIG_FORMAT_S24LE +/** + * \brief HiFi4 enabled volume processing from 24/32 bit to 24/32 or 32 bit. + * \param[in,out] dev Volume base component device. + * \param[in,out] sink Destination buffer. + * \param[in,out] source Input buffer. + * \param[in] frames Number of frames to process. + * \param[in] attenuation factor for peakmeter adjustment (unused) + */ +static void vol_s24_to_s24_s32(struct processing_module *mod, struct input_stream_buffer *bsource, + struct output_stream_buffer *bsink, uint32_t frames, + uint32_t attenuation) +{ + struct vol_data *cd = module_get_private_data(mod); + struct audio_stream __sparse_cache *source = bsource->data; + struct audio_stream __sparse_cache *sink = bsink->data; + ae_f32x2 in_sample = AE_ZERO32(); + ae_f32x2 out_sample = AE_ZERO32(); + ae_f32x2 volume = AE_ZERO32(); + ae_f32x2 *buf; + ae_f32x2 *buf_end; + int i, n, m; + ae_f32x2 *vol; + ae_valign inu = AE_ZALIGN64(); + ae_valign outu = AE_ZALIGN64(); + ae_f32x2 *in = (ae_f32x2 *)audio_stream_wrap(source, (char *)audio_stream_get_rptr(source) + + bsource->consumed); + ae_f32x2 *out = (ae_f32x2 *)audio_stream_wrap(sink, (char *)audio_stream_get_wptr(sink) + + bsink->size); + const int channels_count = audio_stream_get_channels(sink); + const int inc = sizeof(ae_f32x2); + int samples = channels_count * frames; + + /** to ensure the adsress is 8-byte aligned and avoid risk of + * error loading of volume gain while the cd->vol would be set + * as circular buffer + */ + if (cd->copy_gain) + vol_store_gain(cd, channels_count); + + buf = (ae_f32x2 *)cd->vol; + buf_end = (ae_f32x2 *)(cd->vol + channels_count * 2); + vol = buf; + /* Set buf who stores the volume gain data as circular buffer */ + AE_SETCBEGIN0(buf); + AE_SETCEND0(buf_end); + + bsource->consumed += VOL_S32_SAMPLES_TO_BYTES(samples); + bsink->size += VOL_S32_SAMPLES_TO_BYTES(samples); + + while (samples) { + m = audio_stream_samples_without_wrap_s24(source, in); + n = MIN(m, samples); + m = audio_stream_samples_without_wrap_s24(sink, out); + n = MIN(m, n); + inu = AE_LA64_PP(in); + /* process two continuous sample data once */ + for (i = 0; i < n; i += 2) { + /* Load the volume value */ + AE_L32X2_XC(volume, vol, inc); + + /* Load the input sample */ + AE_LA32X2_IP(in_sample, inu, in); + + /* Multiply the input sample */ +#if COMP_VOLUME_Q8_16 + out_sample = AE_MULFP32X2RS(AE_SLAI32S(volume, 7), AE_SLAI32(in_sample, 8)); +#elif COMP_VOLUME_Q1_23 + out_sample = AE_MULFP32X2RS(volume, AE_SLAI32(in_sample, 8)); +#else +#error "Need CONFIG_COMP_VOLUME_Qx_y" +#endif + + /* Shift for S24_LE */ + out_sample = AE_SLAI32S(out_sample, 8); + out_sample = AE_SRAI32(out_sample, 8); + + /* Store the output sample */ + AE_SA32X2_IP(out_sample, outu, out); + } + AE_SA64POS_FP(outu, out); + samples -= n; + in = audio_stream_wrap(source, in); + out = audio_stream_wrap(sink, out); + } +} + +/** + * \brief HiFi4 enabled volume passthrough from 24/32 bit to 24/32 or 32 bit. + * \param[in,out] dev Volume base component device. + * \param[in,out] sink Destination buffer. + * \param[in,out] source Input buffer. + * \param[in] frames Number of frames to process. + * \param[in] attenuation factor for peakmeter adjustment (unused) + */ +static void vol_passthrough_s24_to_s24_s32(struct processing_module *mod, + struct input_stream_buffer *bsource, + struct output_stream_buffer *bsink, uint32_t frames, + uint32_t attenuation) +{ + struct audio_stream __sparse_cache *source = bsource->data; + struct audio_stream __sparse_cache *sink = bsink->data; + ae_f32x2 in_sample = AE_ZERO32(); + int i, n, m; + ae_valign inu = AE_ZALIGN64(); + ae_valign outu = AE_ZALIGN64(); + ae_f32x2 *in = (ae_f32x2 *)audio_stream_wrap(source, (char *)audio_stream_get_rptr(source) + + bsource->consumed); + ae_f32x2 *out = (ae_f32x2 *)audio_stream_wrap(sink, (char *)audio_stream_get_wptr(sink) + + bsink->size); + int samples = audio_stream_get_channels(sink) * frames; + + bsource->consumed += VOL_S32_SAMPLES_TO_BYTES(samples); + bsink->size += VOL_S32_SAMPLES_TO_BYTES(samples); + + while (samples) { + m = audio_stream_samples_without_wrap_s24(source, in); + n = MIN(m, samples); + m = audio_stream_samples_without_wrap_s24(sink, out); + n = MIN(m, n); + inu = AE_LA64_PP(in); + /* process two continuous sample data once */ + for (i = 0; i < n; i += 2) { + /* Load the input sample */ + AE_LA32X2_IP(in_sample, inu, in); + /* Store the output sample */ + AE_SA32X2_IP(in_sample, outu, out); + } + AE_SA64POS_FP(outu, out); + samples -= n; + in = audio_stream_wrap(source, in); + out = audio_stream_wrap(sink, out); + } +} + +#endif /* CONFIG_FORMAT_S24LE */ + +#if CONFIG_FORMAT_S32LE +/** + * \brief HiFi4 enabled volume processing from 32 bit to 24/32 or 32 bit. + * \param[in,out] mod Pointer to struct processing_module + * \param[in,out] sink Destination buffer. + * \param[in,out] source Input buffer. + * \param[in] frames Number of frames to process. + * \param[in] attenuation factor for peakmeter adjustment (unused) + */ +static void vol_s32_to_s24_s32(struct processing_module *mod, struct input_stream_buffer *bsource, + struct output_stream_buffer *bsink, uint32_t frames, + uint32_t attenuation) +{ + struct vol_data *cd = module_get_private_data(mod); + struct audio_stream __sparse_cache *source = bsource->data; + struct audio_stream __sparse_cache *sink = bsink->data; + ae_f32x2 in_sample = AE_ZERO32(); + ae_f32x2 out_sample = AE_ZERO32(); + ae_f32x2 volume = AE_ZERO32(); + int i, n, m; + ae_f64 mult0; + ae_f64 mult1; + ae_f32x2 *buf; + ae_f32x2 *buf_end; + ae_f32x2 *vol; + ae_valign inu = AE_ZALIGN64(); + ae_valign outu = AE_ZALIGN64(); + const int channels_count = audio_stream_get_channels(sink); + const int inc = sizeof(ae_f32x2); + int samples = channels_count * frames; + ae_f32x2 *in = (ae_f32x2 *)audio_stream_wrap(source, (char *)audio_stream_get_rptr(source) + + bsource->consumed); + ae_f32x2 *out = (ae_f32x2 *)audio_stream_wrap(sink, (char *)audio_stream_get_wptr(sink) + + bsink->size); + + /** to ensure the address is 8-byte aligned and avoid risk of + * error loading of volume gain while the cd->vol would be set + * as circular buffer + */ + if (cd->copy_gain) + vol_store_gain(cd, channels_count); + + buf = (ae_f32x2 *)cd->vol; + buf_end = (ae_f32x2 *)(cd->vol + channels_count * 2); + vol = buf; + /* Set buf who stores the volume gain data as circular buffer */ + AE_SETCBEGIN0(buf); + AE_SETCEND0(buf_end); + + bsource->consumed += VOL_S32_SAMPLES_TO_BYTES(samples); + bsink->size += VOL_S32_SAMPLES_TO_BYTES(samples); + + while (samples) { + m = audio_stream_samples_without_wrap_s32(source, in); + n = MIN(m, samples); + m = audio_stream_samples_without_wrap_s32(sink, out); + n = MIN(m, n); + inu = AE_LA64_PP(in); + /* process two continuous sample data once */ + for (i = 0; i < n; i += 2) { + /* Load the volume value */ + AE_L32X2_XC(volume, vol, inc); + + /* Load the input sample */ + AE_LA32X2_IP(in_sample, inu, in); + +#if COMP_VOLUME_Q8_16 + /* Q8.16 x Q1.31 << 1 -> Q9.48 */ + mult0 = AE_MULF32S_HH(volume, in_sample); + mult0 = AE_SRAI64(mult0, 1); /* Q9.47 */ + mult1 = AE_MULF32S_LL(volume, in_sample); + mult1 = AE_SRAI64(mult1, 1); + out_sample = AE_ROUND32X2F48SSYM(mult0, mult1); /* Q9.47 -> Q1.31 */ +#elif COMP_VOLUME_Q1_23 + /* Q1.23 x Q1.31 << 1 -> Q2.55 */ + mult0 = AE_MULF32S_HH(volume, in_sample); + mult0 = AE_SRAI64(mult0, 8); /* Q2.47 */ + mult1 = AE_MULF32S_LL(volume, in_sample); + mult1 = AE_SRAI64(mult1, 8); + out_sample = AE_ROUND32X2F48SSYM(mult0, mult1); /* Q2.47 -> Q1.31 */ +#else +#error "Need CONFIG_COMP_VOLUME_Qx_y" +#endif + AE_SA32X2_IP(out_sample, outu, out); + } + AE_SA64POS_FP(outu, out); + samples -= n; + in = audio_stream_wrap(source, in); + out = audio_stream_wrap(sink, out); + } +} + +/** + * \brief HiFi4 enabled volume passthrough from 32 bit to 24/32 or 32 bit. + * \param[in,out] mod Pointer to struct processing_module + * \param[in,out] sink Destination buffer. + * \param[in,out] source Input buffer. + * \param[in] frames Number of frames to process. + * \param[in] attenuation factor for peakmeter adjustment (unused) + */ +static void vol_passthrough_s32_to_s24_s32(struct processing_module *mod, + struct input_stream_buffer *bsource, + struct output_stream_buffer *bsink, uint32_t frames, + uint32_t attenuation) +{ + struct audio_stream __sparse_cache *source = bsource->data; + struct audio_stream __sparse_cache *sink = bsink->data; + ae_f32x2 in_sample = AE_ZERO32(); + int i, n, m; + ae_valign inu = AE_ZALIGN64(); + ae_valign outu = AE_ZALIGN64(); + const int channels_count = audio_stream_get_channels(sink); + int samples = channels_count * frames; + ae_f32x2 *in = (ae_f32x2 *)audio_stream_wrap(source, (char *)audio_stream_get_rptr(source) + + bsource->consumed); + ae_f32x2 *out = (ae_f32x2 *)audio_stream_wrap(sink, (char *)audio_stream_get_wptr(sink) + + bsink->size); + + bsource->consumed += VOL_S32_SAMPLES_TO_BYTES(samples); + bsink->size += VOL_S32_SAMPLES_TO_BYTES(samples); + while (samples) { + m = audio_stream_samples_without_wrap_s32(source, in); + n = MIN(m, samples); + m = audio_stream_samples_without_wrap_s32(sink, out); + n = MIN(m, n); + inu = AE_LA64_PP(in); + /* process two continuous sample data once */ + for (i = 0; i < n; i += 2) { + /* Load the input sample */ + AE_LA32X2_IP(in_sample, inu, in); + AE_SA32X2_IP(in_sample, outu, out); + } + AE_SA64POS_FP(outu, out); + samples -= n; + in = audio_stream_wrap(source, in); + out = audio_stream_wrap(sink, out); + } +} +#endif /* CONFIG_FORMAT_S32LE */ + +#if CONFIG_FORMAT_S16LE +/** + * \brief HiFi4 enabled volume processing from 16 bit to 16 bit. + * \param[in,out] dev Volume base component device. + * \param[in,out] sink Destination buffer. + * \param[in,out] source Input buffer. + * \param[in] frames Number of frames to process. + * \param[in] attenuation factor for peakmeter adjustment (unused) + */ +static void vol_s16_to_s16(struct processing_module *mod, struct input_stream_buffer *bsource, + struct output_stream_buffer *bsink, uint32_t frames, + uint32_t attenuation) +{ + struct vol_data *cd = module_get_private_data(mod); + struct audio_stream __sparse_cache *source = bsource->data; + struct audio_stream __sparse_cache *sink = bsink->data; + ae_f32x2 volume0 = AE_ZERO32(); + ae_f32x2 volume1 = AE_ZERO32(); + ae_f32x2 out_sample0 = AE_ZERO32(); + ae_f32x2 out_sample1 = AE_ZERO32(); + ae_f16x4 in_sample = AE_ZERO16(); + ae_f16x4 out_sample = AE_ZERO16(); + int i, n, m; + ae_f32x2 *buf; + ae_f32x2 *buf_end; + ae_f32x2 *vol; + ae_valign inu = AE_ZALIGN64(); + ae_valign outu = AE_ZALIGN64(); + ae_f16x4 *in = (ae_f16x4 *)audio_stream_wrap(source, (char *)audio_stream_get_rptr(source) + + bsource->consumed); + ae_f16x4 *out = (ae_f16x4 *)audio_stream_wrap(sink, (char *)audio_stream_get_wptr(sink) + + bsink->size); + const int channels_count = audio_stream_get_channels(sink); + const int inc = sizeof(ae_f32x2); + int samples = channels_count * frames; + + /** to ensure the adsress is 8-byte aligned and avoid risk of + * error loading of volume gain while the cd->vol would be set + * as circular buffer + */ + if (cd->copy_gain) + vol_store_gain(cd, channels_count); + + buf = (ae_f32x2 *)cd->vol; + buf_end = (ae_f32x2 *)(cd->vol + channels_count * 4); + vol = buf; + + /* Set buf as circular buffer */ + AE_SETCBEGIN0(buf); + AE_SETCEND0(buf_end); + + while (samples) { + m = audio_stream_samples_without_wrap_s16(source, in); + n = MIN(m, samples); + m = audio_stream_samples_without_wrap_s16(sink, out); + n = MIN(m, n); + inu = AE_LA64_PP(in); + for (i = 0; i < n; i += 4) { + /* load first two volume gain */ + AE_L32X2_XC(volume0, vol, inc); + + /* load second two volume gain */ + AE_L32X2_XC(volume1, vol, inc); + +#if COMP_VOLUME_Q8_16 + /* Q8.16 to Q9.23 */ + volume0 = AE_SLAI32S(volume0, 7); + volume1 = AE_SLAI32S(volume1, 7); +#elif COMP_VOLUME_Q1_23 + /* No need to shift, Q1.23 is OK as such */ +#else +#error "Need CONFIG_COMP_VOLUME_Qx_y" +#endif + /* Load the input sample */ + AE_LA16X4_IP(in_sample, inu, in); + + /* Multiply the input sample */ + out_sample0 = AE_MULFP32X16X2RS_H(volume0, in_sample); + out_sample1 = AE_MULFP32X16X2RS_L(volume1, in_sample); + + /* Q9.23 to Q1.31 */ + out_sample0 = AE_SLAI32S(out_sample0, 8); + out_sample1 = AE_SLAI32S(out_sample1, 8); + + /* store the output */ + out_sample = AE_ROUND16X4F32SSYM(out_sample0, out_sample1); + // AE_SA16X4_IC(out_sample, outu, out); + AE_SA16X4_IP(out_sample, outu, out); + } + AE_SA64POS_FP(outu, out); + samples -= n; + bsource->consumed += VOL_S16_SAMPLES_TO_BYTES(n); + bsink->size += VOL_S16_SAMPLES_TO_BYTES(n); + in = audio_stream_wrap(source, in); + out = audio_stream_wrap(sink, out); + } +} + +/** + * \brief HiFi4 enabled volume passthrough from 16 bit to 16 bit. + * \param[in,out] dev Volume base component device. + * \param[in,out] sink Destination buffer. + * \param[in,out] source Input buffer. + * \param[in] frames Number of frames to process. + * \param[in] attenuation factor for peakmeter adjustment (unused) + */ +static void vol_passthrough_s16_to_s16(struct processing_module *mod, + struct input_stream_buffer *bsource, + struct output_stream_buffer *bsink, uint32_t frames, + uint32_t attenuation) +{ + struct audio_stream __sparse_cache *source = bsource->data; + struct audio_stream __sparse_cache *sink = bsink->data; + ae_f16x4 in_sample = AE_ZERO16(); + int i, n, m; + ae_valign inu = AE_ZALIGN64(); + ae_valign outu = AE_ZALIGN64(); + ae_f16x4 *in = (ae_f16x4 *)audio_stream_wrap(source, (char *)audio_stream_get_rptr(source) + + bsource->consumed); + ae_f16x4 *out = (ae_f16x4 *)audio_stream_wrap(sink, (char *)audio_stream_get_wptr(sink) + + bsink->size); + const int channels_count = audio_stream_get_channels(sink); + int samples = channels_count * frames; + + bsource->consumed += VOL_S16_SAMPLES_TO_BYTES(samples); + bsink->size += VOL_S16_SAMPLES_TO_BYTES(samples); + while (samples) { + m = audio_stream_samples_without_wrap_s16(source, in); + n = MIN(m, samples); + m = audio_stream_samples_without_wrap_s16(sink, out); + n = MIN(m, n); + inu = AE_LA64_PP(in); + for (i = 0; i < n; i += 4) { + /* Load the input sample */ + AE_LA16X4_IP(in_sample, inu, in); + AE_SA16X4_IP(in_sample, outu, out); + } + AE_SA64POS_FP(outu, out); + samples -= n; + in = audio_stream_wrap(source, in); + out = audio_stream_wrap(sink, out); + } +} +#endif /* CONFIG_FORMAT_S16LE */ +const struct comp_func_map volume_func_map[] = { +#if CONFIG_FORMAT_S16LE + { SOF_IPC_FRAME_S16_LE, vol_s16_to_s16, vol_passthrough_s16_to_s16}, +#endif +#if CONFIG_FORMAT_S24LE + { SOF_IPC_FRAME_S24_4LE, vol_s24_to_s24_s32, vol_passthrough_s24_to_s24_s32}, +#endif +#if CONFIG_FORMAT_S32LE + { SOF_IPC_FRAME_S32_LE, vol_s32_to_s24_s32, vol_passthrough_s32_to_s24_s32}, +#endif +}; +const size_t volume_func_count = ARRAY_SIZE(volume_func_map); +#endif +#endif diff --git a/src/audio/module_adapter/module/volume/volume_hifi4_with_peakvol.c b/src/audio/module_adapter/module/volume/volume_hifi4_with_peakvol.c new file mode 100644 index 000000000000..ba234b931313 --- /dev/null +++ b/src/audio/module_adapter/module/volume/volume_hifi4_with_peakvol.c @@ -0,0 +1,581 @@ +// SPDX-License-Identifier: BSD-3-Clause +// +// Copyright(c) 2023 Intel Corporation. All rights reserved. +// +// Author: Andrula Song + +/** + * \file + * \brief Volume HIFI4 processing implementation with peak volume detection + * \authors Andrula Song + */ + +#include +#include +#include +#include +#include +#include + +LOG_MODULE_DECLARE(volume_hifi4, CONFIG_SOF_LOG_LEVEL); + +#include + +#ifdef VOLUME_HIFI4 + +#if CONFIG_COMP_PEAK_VOL +#include + +static inline void vol_store_gain(struct vol_data *cd, const int channels_count) +{ + int32_t i; + + /* using for loop instead of memcpy_s(), because for loop costs less cycles */ + for (i = 0; i < channels_count; i++) { + cd->vol[i] = cd->volume[i]; + cd->vol[i + channels_count * 1] = cd->volume[i]; + cd->vol[i + channels_count * 2] = cd->volume[i]; + cd->vol[i + channels_count * 3] = cd->volume[i]; + } + cd->copy_gain = false; +} + +#if CONFIG_FORMAT_S24LE +/** + * \brief HiFi4 enabled volume processing from 24/32 bit to 24/32 or 32 bit. + * \param[in,out] dev Volume base component device. + * \param[in,out] sink Destination buffer. + * \param[in,out] source Input buffer. + * \param[in] frames Number of frames to process. + * \param[in] attenuation factor for peakmeter adjustment + */ +static void vol_s24_to_s24_s32(struct processing_module *mod, struct input_stream_buffer *bsource, + struct output_stream_buffer *bsink, uint32_t frames, + uint32_t attenuation) +{ + struct vol_data *cd = module_get_private_data(mod); + struct audio_stream __sparse_cache *source = bsource->data; + struct audio_stream __sparse_cache *sink = bsink->data; + ae_f32x2 in_sample = AE_ZERO32(); + ae_f32x2 out_sample = AE_ZERO32(); + ae_f32x2 volume = AE_ZERO32(); + int i, n, m; + ae_f32x2 *vol; + ae_valign inu = AE_ZALIGN64(); + ae_valign outu = AE_ZALIGN64(); + ae_f32x2 *in = (ae_f32x2 *)audio_stream_wrap(source, (char *)audio_stream_get_rptr(source) + + bsource->consumed); + ae_f32x2 *out = (ae_f32x2 *)audio_stream_wrap(sink, (char *)audio_stream_get_wptr(sink) + + bsink->size); + const int channels_count = audio_stream_get_channels(sink); + const int inc = sizeof(ae_f32x2); + int samples = channels_count * frames; + ae_f32x2 temp; + ae_f32x2 *peakvol = (ae_f32x2 *)cd->peak_vol; + + /* Set peakvol(which stores the peak volume data twice) as circular buffer */ + memset(peakvol, 0, sizeof(ae_f32) * channels_count * 2); + AE_SETCBEGIN1(cd->peak_vol); + AE_SETCEND1(cd->peak_vol + channels_count * 2); + + /** to ensure the adsress is 8-byte aligned and avoid risk of + * error loading of volume gain while the cd->vol would be set + * as circular buffer + */ + if (cd->copy_gain) + vol_store_gain(cd, channels_count); + + vol = (ae_f32x2 *)cd->vol; + /* Set buf who stores the volume gain data as circular buffer */ + AE_SETCBEGIN0(vol); + AE_SETCEND0(cd->vol + channels_count * 2); + + bsource->consumed += VOL_S32_SAMPLES_TO_BYTES(samples); + bsink->size += VOL_S32_SAMPLES_TO_BYTES(samples); + + while (samples) { + m = audio_stream_samples_without_wrap_s32(source, in); + n = MIN(m, samples); + m = audio_stream_samples_without_wrap_s16(sink, out); + n = MIN(m, n); + inu = AE_LA64_PP(in); + /* process two continuous sample data once */ + for (i = 0; i < n; i += 2) { + /* Load the volume value */ + AE_L32X2_XC(volume, vol, inc); + + /* Load the input sample */ + AE_LA32X2_IP(in_sample, inu, in); + /* calculate the peak volume*/ + AE_L32X2_XC1(temp, peakvol, 0); + temp = AE_MAXABS32S(in_sample, temp); + AE_S32X2_XC1(temp, peakvol, inc); + /* Multiply the input sample */ +#if COMP_VOLUME_Q8_16 + out_sample = AE_MULFP32X2RS(AE_SLAI32S(volume, 7), AE_SLAI32(in_sample, 8)); +#elif COMP_VOLUME_Q1_23 + out_sample = AE_MULFP32X2RS(volume, AE_SLAI32(in_sample, 8)); +#else +#error "Need CONFIG_COMP_VOLUME_Qx_y" +#endif + + /* Shift for S24_LE */ + out_sample = AE_SLAI32S(out_sample, 8); + out_sample = AE_SRAI32(out_sample, 8); + + /* Store the output sample */ + AE_SA32X2_IP(out_sample, outu, out); + } + AE_SA64POS_FP(outu, out); + samples -= n; + in = audio_stream_wrap(source, in); + out = audio_stream_wrap(sink, out); + } + for (i = 0; i < channels_count; i++) + cd->peak_regs.peak_meter[i] = MAX(cd->peak_vol[i], + cd->peak_vol[i + channels_count]) + << (attenuation + PEAK_24S_32C_ADJUST); +} + +/** + * \brief HiFi4 enabled volume passthrough from 24/32 bit to 24/32 or 32 bit. + * \param[in,out] dev Volume base component device. + * \param[in,out] sink Destination buffer. + * \param[in,out] source Input buffer. + * \param[in] frames Number of frames to process. + * \param[in] attenuation factor for peakmeter adjustment + */ +static void vol_passthrough_s24_to_s24_s32(struct processing_module *mod, + struct input_stream_buffer *bsource, + struct output_stream_buffer *bsink, uint32_t frames, + uint32_t attenuation) +{ + struct vol_data *cd = module_get_private_data(mod); + struct audio_stream __sparse_cache *source = bsource->data; + struct audio_stream __sparse_cache *sink = bsink->data; + ae_f32x2 in_sample = AE_ZERO32(); + + int i, n, m; + ae_f32x2 *vol; + ae_valign inu = AE_ZALIGN64(); + ae_valign outu = AE_ZALIGN64(); + ae_f32x2 *in = (ae_f32x2 *)audio_stream_wrap(source, (char *)audio_stream_get_rptr(source) + + bsource->consumed); + ae_f32x2 *out = (ae_f32x2 *)audio_stream_wrap(sink, (char *)audio_stream_get_wptr(sink) + + bsink->size); + const int channels_count = audio_stream_get_channels(sink); + const int inc = sizeof(ae_f32x2); + int samples = channels_count * frames; + ae_f32x2 temp; + ae_f32x2 *peakvol = (ae_f32x2 *)cd->peak_vol; + + /* Set peakvol(which stores the peak volume data twice) as circular buffer */ + memset(peakvol, 0, sizeof(ae_f32) * channels_count * 2); + AE_SETCBEGIN1(cd->peak_vol); + AE_SETCEND1(cd->peak_vol + channels_count * 2); + + bsource->consumed += VOL_S32_SAMPLES_TO_BYTES(samples); + bsink->size += VOL_S32_SAMPLES_TO_BYTES(samples); + + while (samples) { + m = audio_stream_samples_without_wrap_s32(source, in); + n = MIN(m, samples); + m = audio_stream_samples_without_wrap_s16(sink, out); + n = MIN(m, n); + inu = AE_LA64_PP(in); + /* process two continuous sample data once */ + for (i = 0; i < n; i += 2) { + /* Load the input sample */ + AE_LA32X2_IP(in_sample, inu, in); + /* calculate the peak volume*/ + AE_L32X2_XC1(temp, peakvol, 0); + temp = AE_MAXABS32S(in_sample, temp); + AE_S32X2_XC1(temp, peakvol, inc); + /* Store the output sample */ + AE_SA32X2_IP(in_sample, outu, out); + } + AE_SA64POS_FP(outu, out); + samples -= n; + in = audio_stream_wrap(source, in); + out = audio_stream_wrap(sink, out); + } + for (i = 0; i < channels_count; i++) + cd->peak_regs.peak_meter[i] = MAX(cd->peak_vol[i], + cd->peak_vol[i + channels_count]) + << (attenuation + PEAK_24S_32C_ADJUST); +} +#endif /* CONFIG_FORMAT_S24LE */ + +#if CONFIG_FORMAT_S32LE +/** + * \brief HiFi4 enabled volume processing from 32 bit to 24/32 or 32 bit. + * \param[in,out] mod Pointer to struct processing_module + * \param[in,out] sink Destination buffer. + * \param[in,out] source Input buffer. + * \param[in] frames Number of frames to process. + * \param[in] attenuation factor for peakmeter adjustment + */ +static void vol_s32_to_s24_s32(struct processing_module *mod, struct input_stream_buffer *bsource, + struct output_stream_buffer *bsink, uint32_t frames, + uint32_t attenuation) +{ + struct vol_data *cd = module_get_private_data(mod); + struct audio_stream __sparse_cache *source = bsource->data; + struct audio_stream __sparse_cache *sink = bsink->data; + ae_f32x2 in_sample = AE_ZERO32(); + ae_f32x2 out_sample = AE_ZERO32(); + ae_f32x2 volume = AE_ZERO32(); + int i, n, m; + ae_f64 mult0; + ae_f64 mult1; + ae_f32x2 *buf; + ae_f32x2 *buf_end; + ae_f32x2 *vol; + ae_valign inu = AE_ZALIGN64(); + ae_valign outu = AE_ZALIGN64(); + const int channels_count = audio_stream_get_channels(sink); + const int inc = sizeof(ae_f32x2); + int samples = channels_count * frames; + ae_f32x2 *in = (ae_f32x2 *)audio_stream_wrap(source, (char *)audio_stream_get_rptr(source) + + bsource->consumed); + ae_f32x2 *out = (ae_f32x2 *)audio_stream_wrap(sink, (char *)audio_stream_get_wptr(sink) + + bsink->size); + ae_f32x2 temp; + ae_f32x2 *peakvol = (ae_f32x2 *)cd->peak_vol; + + /* Set peakvol(which stores the peak volume data twice) as circular buffer */ + memset(peakvol, 0, sizeof(ae_f32) * channels_count * 2); + AE_SETCBEGIN1(cd->peak_vol); + AE_SETCEND1(cd->peak_vol + channels_count * 2); + + /** to ensure the address is 8-byte aligned and avoid risk of + * error loading of volume gain while the cd->vol would be set + * as circular buffer + */ + if (cd->copy_gain) + vol_store_gain(cd, channels_count); + + buf = (ae_f32x2 *)cd->vol; + buf_end = (ae_f32x2 *)(cd->vol + channels_count * 2); + vol = buf; + /* Set buf who stores the volume gain data as circular buffer */ + AE_SETCBEGIN0(buf); + AE_SETCEND0(buf_end); + + bsource->consumed += VOL_S32_SAMPLES_TO_BYTES(samples); + bsink->size += VOL_S32_SAMPLES_TO_BYTES(samples); + + while (samples) { + m = audio_stream_samples_without_wrap_s32(source, in); + n = MIN(m, samples); + m = audio_stream_samples_without_wrap_s32(sink, out); + n = MIN(m, n); + inu = AE_LA64_PP(in); + /* process two continuous sample data once */ + for (i = 0; i < n; i += 2) { + /* Load the volume value */ + AE_L32X2_XC(volume, vol, inc); + + /* Load the input sample */ + AE_LA32X2_IP(in_sample, inu, in); + /* calculate the peak volume*/ + AE_L32X2_XC1(temp, peakvol, 0); + temp = AE_MAXABS32S(in_sample, temp); + AE_S32X2_XC1(temp, peakvol, inc); +#if COMP_VOLUME_Q8_16 + /* Q8.16 x Q1.31 << 1 -> Q9.48 */ + mult0 = AE_MULF32S_HH(volume, in_sample); + mult0 = AE_SRAI64(mult0, 1); /* Q9.47 */ + mult1 = AE_MULF32S_LL(volume, in_sample); + mult1 = AE_SRAI64(mult1, 1); + out_sample = AE_ROUND32X2F48SSYM(mult0, mult1); /* Q9.47 -> Q1.31 */ +#elif COMP_VOLUME_Q1_23 + /* Q1.23 x Q1.31 << 1 -> Q2.55 */ + mult0 = AE_MULF32S_HH(volume, in_sample); + mult0 = AE_SRAI64(mult0, 8); /* Q2.47 */ + mult1 = AE_MULF32S_LL(volume, in_sample); + mult1 = AE_SRAI64(mult1, 8); + out_sample = AE_ROUND32X2F48SSYM(mult0, mult1); /* Q2.47 -> Q1.31 */ +#else +#error "Need CONFIG_COMP_VOLUME_Qx_y" +#endif + AE_SA32X2_IP(out_sample, outu, out); + } + AE_SA64POS_FP(outu, out); + samples -= n; + in = audio_stream_wrap(source, in); + out = audio_stream_wrap(sink, out); + } + for (i = 0; i < channels_count; i++) + cd->peak_regs.peak_meter[i] = MAX(cd->peak_vol[i], + cd->peak_vol[i + channels_count]) << attenuation; +} + +/** + * \brief HiFi4 enabled volume passthrough from 32 bit to 24/32 or 32 bit. + * \param[in,out] mod Pointer to struct processing_module + * \param[in,out] sink Destination buffer. + * \param[in,out] source Input buffer. + * \param[in] frames Number of frames to process. + * \param[in] attenuation factor for peakmeter adjustment + */ +static void vol_passthrough_s32_to_s24_s32(struct processing_module *mod, + struct input_stream_buffer *bsource, + struct output_stream_buffer *bsink, uint32_t frames, + uint32_t attenuation) +{ + struct vol_data *cd = module_get_private_data(mod); + struct audio_stream __sparse_cache *source = bsource->data; + struct audio_stream __sparse_cache *sink = bsink->data; + ae_f32x2 in_sample = AE_ZERO32(); + int i, n, m; + ae_valign inu = AE_ZALIGN64(); + ae_valign outu = AE_ZALIGN64(); + const int channels_count = audio_stream_get_channels(sink); + const int inc = sizeof(ae_f32x2); + int samples = channels_count * frames; + ae_f32x2 *in = (ae_f32x2 *)audio_stream_wrap(source, (char *)audio_stream_get_rptr(source) + + bsource->consumed); + ae_f32x2 *out = (ae_f32x2 *)audio_stream_wrap(sink, (char *)audio_stream_get_wptr(sink) + + bsink->size); + ae_f32x2 temp; + ae_f32x2 *peakvol = (ae_f32x2 *)cd->peak_vol; + + /* Set peakvol(which stores the peak volume data twice) as circular buffer */ + memset(peakvol, 0, sizeof(ae_f32) * channels_count * 2); + AE_SETCBEGIN1(cd->peak_vol); + AE_SETCEND1(cd->peak_vol + channels_count * 2); + bsource->consumed += VOL_S32_SAMPLES_TO_BYTES(samples); + bsink->size += VOL_S32_SAMPLES_TO_BYTES(samples); + + while (samples) { + m = audio_stream_samples_without_wrap_s32(source, in); + n = MIN(m, samples); + m = audio_stream_samples_without_wrap_s32(sink, out); + n = MIN(m, n); + inu = AE_LA64_PP(in); + /* process two continuous sample data once */ + for (i = 0; i < n; i += 2) { + /* Load the input sample */ + AE_LA32X2_IP(in_sample, inu, in); + /* calculate the peak volume*/ + AE_L32X2_XC1(temp, peakvol, 0); + temp = AE_MAXABS32S(in_sample, temp); + AE_S32X2_XC1(temp, peakvol, inc); + + AE_SA32X2_IP(in_sample, outu, out); + } + AE_SA64POS_FP(outu, out); + samples -= n; + in = audio_stream_wrap(source, in); + out = audio_stream_wrap(sink, out); + } + for (i = 0; i < channels_count; i++) + cd->peak_regs.peak_meter[i] = MAX(cd->peak_vol[i], + cd->peak_vol[i + channels_count]) << attenuation; +} +#endif /* CONFIG_FORMAT_S32LE */ + +#if CONFIG_FORMAT_S16LE +/** + * \brief HiFi4 enabled volume processing from 16 bit to 16 bit. + * \param[in,out] dev Volume base component device. + * \param[in,out] sink Destination buffer. + * \param[in,out] source Input buffer. + * \param[in] frames Number of frames to process. + * \param[in] attenuation factor for peakmeter adjustment (unused for 16bit) + */ +static void vol_s16_to_s16(struct processing_module *mod, struct input_stream_buffer *bsource, + struct output_stream_buffer *bsink, uint32_t frames, + uint32_t attenuation) +{ + struct vol_data *cd = module_get_private_data(mod); + struct audio_stream __sparse_cache *source = bsource->data; + struct audio_stream __sparse_cache *sink = bsink->data; + ae_f32x2 volume0 = AE_ZERO32(); + ae_f32x2 volume1 = AE_ZERO32(); + ae_f32x2 out_sample0 = AE_ZERO32(); + ae_f32x2 out_sample1 = AE_ZERO32(); + ae_f16x4 in_sample = AE_ZERO16(); + ae_f16x4 out_sample = AE_ZERO16(); + int i, n, m; + ae_f32x2 *buf; + ae_f32x2 *buf_end; + ae_f32x2 *vol; + ae_valign inu = AE_ZALIGN64(); + ae_valign outu = AE_ZALIGN64(); + ae_f16x4 *in = (ae_f16x4 *)audio_stream_wrap(source, (char *)audio_stream_get_rptr(source) + + bsource->consumed); + ae_f16x4 *out = (ae_f16x4 *)audio_stream_wrap(sink, (char *)audio_stream_get_wptr(sink) + + bsink->size); + const int channels_count = audio_stream_get_channels(sink); + const int inc = sizeof(ae_f32x2); + int samples = channels_count * frames; + ae_f32x2 temp; + ae_f32x2 *peakvol = (ae_f32x2 *)cd->peak_vol; + + /* Set peakvol(which stores the peak volume data 4 times) as circular buffer */ + memset(peakvol, 0, sizeof(ae_f32) * channels_count * 4); + AE_SETCBEGIN1(cd->peak_vol); + AE_SETCEND1(cd->peak_vol + channels_count * 4); + + /** to ensure the adsress is 8-byte aligned and avoid risk of + * error loading of volume gain while the cd->vol would be set + * as circular buffer + */ + if (cd->copy_gain) + vol_store_gain(cd, channels_count); + + buf = (ae_f32x2 *)cd->vol; + buf_end = (ae_f32x2 *)(cd->vol + channels_count * 4); + vol = buf; + + /* Set buf as circular buffer */ + AE_SETCBEGIN0(buf); + AE_SETCEND0(buf_end); + + while (samples) { + m = audio_stream_samples_without_wrap_s16(source, in); + n = MIN(m, samples); + m = audio_stream_samples_without_wrap_s16(sink, out); + n = MIN(m, n); + inu = AE_LA64_PP(in); + for (i = 0; i < n; i += 4) { + /* load first two volume gain */ + AE_L32X2_XC(volume0, vol, inc); + + /* load second two volume gain */ + AE_L32X2_XC(volume1, vol, inc); + +#if COMP_VOLUME_Q8_16 + /* Q8.16 to Q9.23 */ + volume0 = AE_SLAI32S(volume0, 7); + volume1 = AE_SLAI32S(volume1, 7); +#elif COMP_VOLUME_Q1_23 + /* No need to shift, Q1.23 is OK as such */ +#else +#error "Need CONFIG_COMP_VOLUME_Qx_y" +#endif + /* Load the input sample */ + AE_LA16X4_IP(in_sample, inu, in); + /* calculate the peak volume*/ + AE_L32X2_XC1(temp, peakvol, 0); + temp = AE_MAXABS32S(AE_SEXT32X2D16_32(in_sample), temp); + AE_S32X2_XC1(temp, peakvol, inc); + /* calculate the peak volume*/ + AE_L32X2_XC1(temp, peakvol, 0); + temp = AE_MAXABS32S(AE_SEXT32X2D16_10(in_sample), temp); + AE_S32X2_XC1(temp, peakvol, inc); + /* Multiply the input sample */ + out_sample0 = AE_MULFP32X16X2RS_H(volume0, in_sample); + out_sample1 = AE_MULFP32X16X2RS_L(volume1, in_sample); + + /* Q9.23 to Q1.31 */ + out_sample0 = AE_SLAI32S(out_sample0, 8); + out_sample1 = AE_SLAI32S(out_sample1, 8); + + /* store the output */ + out_sample = AE_ROUND16X4F32SSYM(out_sample0, out_sample1); + AE_SA16X4_IP(out_sample, outu, out); + } + AE_SA64POS_FP(outu, out); + samples -= n; + in = audio_stream_wrap(source, in); + out = audio_stream_wrap(sink, out); + bsource->consumed += VOL_S16_SAMPLES_TO_BYTES(n); + bsink->size += VOL_S16_SAMPLES_TO_BYTES(n); + } + for (i = 0; i < channels_count; i++) { + m = MAX(cd->peak_vol[i], cd->peak_vol[i + channels_count]); + m = MAX(m, cd->peak_vol[i + channels_count * 2]); + m = MAX(m, cd->peak_vol[i + channels_count * 3]); + cd->peak_regs.peak_meter[i] = m; + } +} + +/** + * \brief HiFi4 enabled volume passthrough from 16 bit to 16 bit. + * \param[in,out] dev Volume base component device. + * \param[in,out] sink Destination buffer. + * \param[in,out] source Input buffer. + * \param[in] frames Number of frames to process. + * \param[in] attenuation factor for peakmeter adjustment (unused for 16bit) + */ +static void vol_passthrough_s16_to_s16(struct processing_module *mod, + struct input_stream_buffer *bsource, + struct output_stream_buffer *bsink, uint32_t frames, + uint32_t attenuation) +{ + struct vol_data *cd = module_get_private_data(mod); + struct audio_stream __sparse_cache *source = bsource->data; + struct audio_stream __sparse_cache *sink = bsink->data; + ae_f16x4 in_sample = AE_ZERO16(); + int i, n, m; + ae_valign inu = AE_ZALIGN64(); + ae_valign outu = AE_ZALIGN64(); + ae_f16x4 *in = (ae_f16x4 *)audio_stream_wrap(source, (char *)audio_stream_get_rptr(source) + + bsource->consumed); + ae_f16x4 *out = (ae_f16x4 *)audio_stream_wrap(sink, (char *)audio_stream_get_wptr(sink) + + bsink->size); + const int channels_count = audio_stream_get_channels(sink); + const int inc = sizeof(ae_f32x2); + int samples = channels_count * frames; + ae_f32x2 temp; + ae_f32x2 *peakvol = (ae_f32x2 *)cd->peak_vol; + + /* Set peakvol(which stores the peak volume data 4 times) as circular buffer */ + memset(peakvol, 0, sizeof(ae_f32) * channels_count * 4); + AE_SETCBEGIN1(cd->peak_vol); + AE_SETCEND1(cd->peak_vol + channels_count * 4); + + while (samples) { + m = audio_stream_samples_without_wrap_s16(source, in); + n = MIN(m, samples); + m = audio_stream_samples_without_wrap_s16(sink, out); + n = MIN(m, n); + inu = AE_LA64_PP(in); + for (i = 0; i < n; i += 4) { + /* Load the input sample */ + AE_LA16X4_IP(in_sample, inu, in); + /* calculate the peak volume*/ + AE_L32X2_XC1(temp, peakvol, 0); + temp = AE_MAXABS32S(AE_SEXT32X2D16_32(in_sample), temp); + AE_S32X2_XC1(temp, peakvol, inc); + /* calculate the peak volume*/ + AE_L32X2_XC1(temp, peakvol, 0); + temp = AE_MAXABS32S(AE_SEXT32X2D16_10(in_sample), temp); + AE_S32X2_XC1(temp, peakvol, inc); + /* store the output */ + AE_SA16X4_IP(in_sample, outu, out); + } + AE_SA64POS_FP(outu, out); + samples -= n; + in = audio_stream_wrap(source, in); + out = audio_stream_wrap(sink, out); + bsource->consumed += VOL_S16_SAMPLES_TO_BYTES(n); + bsink->size += VOL_S16_SAMPLES_TO_BYTES(n); + } + for (i = 0; i < channels_count; i++) { + m = MAX(cd->peak_vol[i], cd->peak_vol[i + channels_count]); + m = MAX(m, cd->peak_vol[i + channels_count * 2]); + m = MAX(m, cd->peak_vol[i + channels_count * 3]); + cd->peak_regs.peak_meter[i] = m; + } +} +#endif /* CONFIG_FORMAT_S16LE */ + +const struct comp_func_map volume_func_map[] = { +#if CONFIG_FORMAT_S16LE + { SOF_IPC_FRAME_S16_LE, vol_s16_to_s16, vol_passthrough_s16_to_s16}, +#endif +#if CONFIG_FORMAT_S24LE + { SOF_IPC_FRAME_S24_4LE, vol_s24_to_s24_s32, vol_passthrough_s24_to_s24_s32}, +#endif +#if CONFIG_FORMAT_S32LE + { SOF_IPC_FRAME_S32_LE, vol_s32_to_s24_s32, vol_passthrough_s32_to_s24_s32}, +#endif +}; + +const size_t volume_func_count = ARRAY_SIZE(volume_func_map); +#endif +#endif diff --git a/src/include/sof/audio/volume.h b/src/include/sof/audio/volume.h index ca009856e262..0480145286d8 100644 --- a/src/include/sof/audio/volume.h +++ b/src/include/sof/audio/volume.h @@ -34,15 +34,17 @@ struct comp_buffer; struct sof_ipc_ctrl_value_chan; -#define CONFIG_GENERIC - #if defined(__XCC__) -#include - -#if XCHAL_HAVE_HIFI3 -#undef CONFIG_GENERIC -#endif - +# include +# if XCHAL_HAVE_HIFI4 +# define VOLUME_HIFI4 +# elif XCHAL_HAVE_HIFI3 +# define VOLUME_HIFI3 +# else +# define VOLUME_GENERIC +# endif +#else +# define VOLUME_GENERIC #endif /** @@ -167,12 +169,14 @@ struct vol_data { vol_zc_func zc_get; /**< function getting nearest zero crossing frame */ bool copy_gain; /**< control copy gain or not */ uint32_t attenuation; /**< peakmeter adjustment in range [0 - 31] */ + bool is_passthrough; /**< is passthrough or do gain multiplication */ }; /** \brief Volume processing functions map. */ struct comp_func_map { uint16_t frame_fmt; /**< frame format */ vol_scale_func func; /**< volume processing function */ + vol_scale_func passthrough_func; /**< volume passthrough function */ }; /** \brief Map of formats with dedicated processing functions. */ @@ -192,9 +196,11 @@ struct comp_zc_func_map { * \brief Retrievies volume processing function. * \param[in,out] dev Volume base component device. * \param[in] sinkb Sink buffer to match against + * \param[in] cd Volume data structure. */ static inline vol_scale_func vol_get_processing_function(struct comp_dev *dev, - struct comp_buffer __sparse_cache *sinkb) + struct comp_buffer __sparse_cache *sinkb, + struct vol_data *cd) { int i; @@ -203,33 +209,52 @@ static inline vol_scale_func vol_get_processing_function(struct comp_dev *dev, if (audio_stream_get_frm_fmt(&sinkb->stream) != volume_func_map[i].frame_fmt) continue; - return volume_func_map[i].func; + if (cd->is_passthrough) + return volume_func_map[i].passthrough_func; + else + return volume_func_map[i].func; } return NULL; } + #else /** * \brief Retrievies volume processing function. * \param[in,out] dev Volume base component device. - * \param[in] sinkb Sink buffer to match against + * \param[in] cd Volume data structure */ static inline vol_scale_func vol_get_processing_function(struct comp_dev *dev, - struct comp_buffer __sparse_cache *sinkb) + struct vol_data *cd) { struct processing_module *mod = comp_get_drvdata(dev); - switch (mod->priv.cfg.base_cfg.audio_fmt.valid_bit_depth) { - case IPC4_DEPTH_16BIT: - return volume_func_map[0].func; - case IPC4_DEPTH_24BIT: - return volume_func_map[1].func; - case IPC4_DEPTH_32BIT: - return volume_func_map[2].func; - default: - comp_err(dev, "vol_get_processing_function(): unsupported depth %d", - mod->priv.cfg.base_cfg.audio_fmt.depth); - return NULL; + if (cd->is_passthrough) { + switch (mod->priv.cfg.base_cfg.audio_fmt.valid_bit_depth) { + case IPC4_DEPTH_16BIT: + return volume_func_map[0].passthrough_func; + case IPC4_DEPTH_24BIT: + return volume_func_map[1].passthrough_func; + case IPC4_DEPTH_32BIT: + return volume_func_map[2].passthrough_func; + default: + comp_err(dev, "vol_get_processing_function(): unsupported depth %d", + mod->priv.cfg.base_cfg.audio_fmt.depth); + return NULL; + } + } else { + switch (mod->priv.cfg.base_cfg.audio_fmt.valid_bit_depth) { + case IPC4_DEPTH_16BIT: + return volume_func_map[0].func; + case IPC4_DEPTH_24BIT: + return volume_func_map[1].func; + case IPC4_DEPTH_32BIT: + return volume_func_map[2].func; + default: + comp_err(dev, "vol_get_processing_function(): unsupported depth %d", + mod->priv.cfg.base_cfg.audio_fmt.depth); + return NULL; + } } } #endif diff --git a/test/cmocka/src/audio/volume/CMakeLists.txt b/test/cmocka/src/audio/volume/CMakeLists.txt index 49e75a2e948a..0a793f8df522 100644 --- a/test/cmocka/src/audio/volume/CMakeLists.txt +++ b/test/cmocka/src/audio/volume/CMakeLists.txt @@ -15,8 +15,10 @@ add_library(audio_for_volume STATIC ${PROJECT_SOURCE_DIR}/src/audio/module_adapter/module/volume/volume.c ${PROJECT_SOURCE_DIR}/src/audio/module_adapter/module/volume/volume_generic.c ${PROJECT_SOURCE_DIR}/src/audio/module_adapter/module/volume/volume_hifi3.c + ${PROJECT_SOURCE_DIR}/src/audio/module_adapter/module/volume/volume_hifi4.c ${PROJECT_SOURCE_DIR}/src/audio/module_adapter/module/volume/volume_generic_with_peakvol.c ${PROJECT_SOURCE_DIR}/src/audio/module_adapter/module/volume/volume_hifi3_with_peakvol.c + ${PROJECT_SOURCE_DIR}/src/audio/module_adapter/module/volume/volume_hifi4_with_peakvol.c ${PROJECT_SOURCE_DIR}/src/audio/module_adapter/module_adapter.c ${PROJECT_SOURCE_DIR}/src/audio/module_adapter/module/generic.c ${PROJECT_SOURCE_DIR}/src/audio/buffer.c diff --git a/test/cmocka/src/audio/volume/volume_process.c b/test/cmocka/src/audio/volume/volume_process.c index 94d72a6fb8a1..7b50fde2f310 100644 --- a/test/cmocka/src/audio/volume/volume_process.c +++ b/test/cmocka/src/audio/volume/volume_process.c @@ -62,6 +62,7 @@ static int setup(void **state) cd = test_malloc(sizeof(*cd)); md = &vol_state->mod->priv; md->private = cd; + cd->is_passthrough = false; /* malloc memory to store current volume 4 times to ensure the address * is 8-byte aligned for multi-way xtensa intrinsic operations. @@ -71,7 +72,11 @@ static int setup(void **state) cd->vol = test_malloc(vol_size); /* set processing function and volume */ - cd->scale_vol = vol_get_processing_function(vol_state->mod->dev, vol_state->sinks[0]); +#if CONFIG_IPC_MAJOR_4 + cd->scale_vol = vol_get_processing_function(vol_state->mod->dev, cd); +#else + cd->scale_vol = vol_get_processing_function(vol_state->mod->dev, vol_state->sinks[0], cd); +#endif set_volume(cd->volume, vol_parameters->volume, vol_state->parameters.channels); /* assign test state */ diff --git a/zephyr/CMakeLists.txt b/zephyr/CMakeLists.txt index eab95c51c67f..ce81697cb89f 100644 --- a/zephyr/CMakeLists.txt +++ b/zephyr/CMakeLists.txt @@ -565,8 +565,10 @@ zephyr_library_sources_ifdef(CONFIG_SAMPLE_KEYPHRASE ) zephyr_library_sources_ifdef(CONFIG_COMP_VOLUME + ${SOF_AUDIO_MODULES_PATH}/volume/volume_hifi4.c ${SOF_AUDIO_MODULES_PATH}/volume/volume_hifi3.c ${SOF_AUDIO_MODULES_PATH}/volume/volume_generic.c + ${SOF_AUDIO_MODULES_PATH}/volume/volume_hifi4_with_peakvol.c ${SOF_AUDIO_MODULES_PATH}/volume/volume_hifi3_with_peakvol.c ${SOF_AUDIO_MODULES_PATH}/volume/volume_generic_with_peakvol.c ${SOF_AUDIO_MODULES_PATH}/volume/volume.c