From 483935f4e011e378303af5425863250289353cbd Mon Sep 17 00:00:00 2001 From: Piotr Hoppe Date: Mon, 28 Sep 2026 13:05:23 +0200 Subject: [PATCH 1/3] audio: sink_source_utils: add source release and commit helper Add release_source_and_commit_sink() to release processed source data and commit produced data to the sink. Always commit the sink, while preserving the source release error when both operations fail. Signed-off-by: Piotr Hoppe --- src/audio/sink_source_utils.c | 13 +++++++++++++ src/include/sof/audio/sink_source_utils.h | 17 +++++++++++++++++ 2 files changed, 30 insertions(+) diff --git a/src/audio/sink_source_utils.c b/src/audio/sink_source_utils.c index 1fb86df386c2..9b01753f8670 100644 --- a/src/audio/sink_source_utils.c +++ b/src/audio/sink_source_utils.c @@ -13,6 +13,19 @@ #include #include +int release_source_and_commit_sink(struct sof_source *source, size_t free_size, + struct sof_sink *sink, size_t commit_size) +{ + int src_ret; + int sink_ret; + + src_ret = source_release_data(source, free_size); + sink_ret = sink_commit_buffer(sink, (src_ret ? 0 : commit_size)); + + return src_ret ? src_ret : sink_ret; +} +EXPORT_SYMBOL(release_source_and_commit_sink); + int source_to_sink_copy(struct sof_source *source, struct sof_sink *sink, bool free, size_t size) { diff --git a/src/include/sof/audio/sink_source_utils.h b/src/include/sof/audio/sink_source_utils.h index 2d31666ac383..5e0e85ba73ce 100644 --- a/src/include/sof/audio/sink_source_utils.h +++ b/src/include/sof/audio/sink_source_utils.h @@ -11,6 +11,23 @@ #include #include +/** + * release processed data from source and commit produced data to sink + * + * @param source the data source to release data from + * @param free_size number of bytes to be released (freed) from the source + * @param sink the data target to commit data to + * @param commit_size number of bytes to be committed to the sink + * + * @return 0 on success; + * on error, the error code returned by source_release_data() takes + * precedence, otherwise the error code returned by sink_commit_buffer() + * is returned. The sink is always committed regardless of the source + * release result. + */ +int release_source_and_commit_sink(struct sof_source *source, size_t free_size, + struct sof_sink *sink, size_t commit_size); + /** * copy bytes from source to sink * From 4375db239bd2377932806219dfa9a1985e910dc2 Mon Sep 17 00:00:00 2001 From: Piotr Hoppe Date: Mon, 21 Sep 2026 13:02:57 +0200 Subject: [PATCH 2/3] audio: eq_fir: migrate processing to source and sink APIs Replace legacy input/output buffer processing with the source/sink API. Handle circular-buffer wrapping in FIR kernels and use direct source-to-sink copy for pass-through operation. Validate matching source and sink formats and add tests for invalid configurations and odd frame counts. Signed-off-by: Piotr Hoppe Co-authored-by: Copilot <223556219+Copilot@users.noreply.github.com> --- src/audio/eq_fir/eq_fir.c | 123 ++++--- src/audio/eq_fir/eq_fir.h | 40 +-- src/audio/eq_fir/eq_fir_generic.c | 105 +++--- src/audio/eq_fir/eq_fir_hifi2ep.c | 273 ++++++++++------ src/audio/eq_fir/eq_fir_hifi3.c | 191 +++++++---- test/cmocka/src/audio/eq_fir/eq_fir_process.c | 306 +++++++++++++++--- 6 files changed, 718 insertions(+), 320 deletions(-) diff --git a/src/audio/eq_fir/eq_fir.c b/src/audio/eq_fir/eq_fir.c index 3f85cf2ac623..066dea0f788d 100644 --- a/src/audio/eq_fir/eq_fir.c +++ b/src/audio/eq_fir/eq_fir.c @@ -11,6 +11,7 @@ #include #include #include +#include #include #include #include @@ -40,21 +41,6 @@ LOG_MODULE_REGISTER(eq_fir, CONFIG_SOF_LOG_LEVEL); SOF_DEFINE_REG_UUID(eq_fir); -/* Pass-through functions to replace FIR core while not configured for - * response. - */ - -static void eq_fir_passthrough(struct fir_state_32x16 fir[], - struct input_stream_buffer *bsource, - struct output_stream_buffer *bsink, - int frames) -{ - struct audio_stream *source = bsource->data; - struct audio_stream *sink = bsink->data; - - audio_stream_copy(source, 0, sink, 0, frames * audio_stream_get_channels(source)); -} - static void eq_fir_free_delaylines(struct processing_module *mod) { struct comp_data *cd = module_get_private_data(mod); @@ -378,14 +364,20 @@ static int eq_fir_set_config(struct processing_module *mod, uint32_t config_id, /* copy and process stream data from source to sink buffers */ static int eq_fir_process(struct processing_module *mod, - struct input_stream_buffer *input_buffers, - int num_input_buffers, - struct output_stream_buffer *output_buffers, - int num_output_buffers) + struct sof_source **sources, int num_of_sources, + struct sof_sink **sinks, int num_of_sinks) { struct comp_data *cd = module_get_private_data(mod); - struct audio_stream *source = input_buffers[0].data; - uint32_t frame_count = input_buffers[0].size; + struct sof_source *source = sources[0]; + struct sof_sink *sink = sinks[0]; + struct cir_buf_source source_buf; + struct cir_buf_sink sink_buf; + size_t source_frame_bytes; + size_t sink_frame_bytes; + size_t source_bytes; + size_t sink_bytes; + size_t buffer_size; + size_t frame_count; int ret; comp_dbg(mod->dev, "entry"); @@ -395,17 +387,17 @@ static int eq_fir_process(struct processing_module *mod, cd->config = comp_get_data_blob(cd->model_handler, &cd->config_size, NULL); if (!cd->config || eq_fir_check_blob_size(mod->dev, cd->config_size) < 0) return -EINVAL; - ret = eq_fir_setup(mod, audio_stream_get_channels(source)); + ret = eq_fir_setup(mod, source_get_channels(source)); if (ret < 0) { comp_err(mod->dev, "failed FIR setup"); return ret; } else if (cd->fir_delay_size) { comp_dbg(mod->dev, "active"); - ret = set_fir_func(mod, audio_stream_get_frm_fmt(source)); + ret = set_fir_func(mod, source_get_frm_fmt(source)); if (ret < 0) return ret; } else { - cd->eq_fir_func = eq_fir_passthrough; + cd->eq_fir_func = NULL; comp_dbg(mod->dev, "pass-through"); } } @@ -418,22 +410,57 @@ static int eq_fir_process(struct processing_module *mod, * break the delay line alignment if called with odd number of frames * so it can't be used here. */ - + frame_count = source_sink_avail_frames_aligned(source, sink); frame_count &= ~0x1; - if (frame_count) { - cd->eq_fir_func(cd->fir, &input_buffers[0], &output_buffers[0], frame_count); - module_update_buffer_position(&input_buffers[0], &output_buffers[0], frame_count); + if (!frame_count) + return 0; + + source_frame_bytes = source_get_frame_bytes(source); + sink_frame_bytes = sink_get_frame_bytes(sink); + source_bytes = frame_count * source_frame_bytes; + sink_bytes = frame_count * sink_frame_bytes; + + if (!cd->fir_delay_size) { + if (source_frame_bytes != sink_frame_bytes) + return -EINVAL; + + return source_to_sink_copy(source, sink, true, source_bytes); } - return 0; + if (!cd->eq_fir_func) + return -EINVAL; + + ret = source_get_data(source, source_bytes, &source_buf.ptr, + &source_buf.buf_start, &buffer_size); + if (ret < 0) + return ret; + source_buf.buf_end = (const char *)source_buf.buf_start + buffer_size; + + ret = sink_get_buffer(sink, sink_bytes, &sink_buf.ptr, &sink_buf.buf_start, + &buffer_size); + if (ret < 0) { + source_release_data(source, 0); + return ret; + } + sink_buf.buf_end = (char *)sink_buf.buf_start + buffer_size; + + cd->eq_fir_func(cd->fir, &source_buf, &sink_buf, frame_count, cd->nch); + + ret = source_release_data(source, source_bytes); + if (ret < 0) { + sink_commit_buffer(sink, 0); + return ret; + } + + return sink_commit_buffer(sink, sink_bytes); } -static void eq_fir_set_alignment(struct audio_stream *source) +static int eq_fir_set_alignment(struct sof_source *source) { const uint32_t byte_align = SOF_FRAME_BYTE_ALIGN; const uint32_t frame_align_req = 2; /* Process multiples of 2 frames */ - audio_stream_set_align(byte_align, frame_align_req, source); + return source_set_alignment_constants(source, byte_align, frame_align_req); } static int eq_fir_prepare(struct processing_module *mod, @@ -441,33 +468,45 @@ static int eq_fir_prepare(struct processing_module *mod, struct sof_sink **sinks, int num_of_sinks) { struct comp_data *cd = module_get_private_data(mod); - struct comp_buffer *sourceb, *sinkb; struct comp_dev *dev = mod->dev; - int channels; + struct sof_source *source; + struct sof_sink *sink; + unsigned int channels; enum sof_ipc_frame frame_fmt; int ret = 0; comp_dbg(dev, "entry"); /* EQ component will only ever have 1 source and 1 sink buffer. */ - sourceb = comp_dev_get_first_data_producer(dev); - sinkb = comp_dev_get_first_data_consumer(dev); - if (!sourceb || !sinkb) { - comp_err(dev, "no source or sink buffer"); + if (num_of_sources != 1 || num_of_sinks != 1) { + comp_err(dev, "expected exactly 1 source and 1 sink (got %d/%d)", num_of_sources, + num_of_sinks); return -ENOTCONN; } + source = sources[0]; + sink = sinks[0]; + ret = eq_fir_params(mod); if (ret < 0) { comp_set_state(dev, COMP_TRIGGER_RESET); return ret; } - eq_fir_set_alignment(&sourceb->stream); - channels = audio_stream_get_channels(&sinkb->stream); - frame_fmt = audio_stream_get_frm_fmt(&sourceb->stream); + ret = eq_fir_set_alignment(source); + if (ret < 0) + return ret; - cd->eq_fir_func = eq_fir_passthrough; + if (source_get_channels(source) != sink_get_channels(sink) || + source_get_frm_fmt(source) != sink_get_frm_fmt(sink)) { + comp_err(dev, "source and sink audio formats do not match"); + return -EINVAL; + } + + channels = sink_get_channels(sink); + frame_fmt = source_get_frm_fmt(source); + + cd->eq_fir_func = NULL; cd->config = comp_get_data_blob(cd->model_handler, &cd->config_size, NULL); if (cd->config) { if (eq_fir_check_blob_size(dev, cd->config_size) < 0) @@ -514,7 +553,7 @@ static const struct module_interface eq_fir_interface = { .free = eq_fir_free, .set_configuration = eq_fir_set_config, .get_configuration = eq_fir_get_config, - .process_audio_stream = eq_fir_process, + .process = eq_fir_process, .prepare = eq_fir_prepare, .reset = eq_fir_reset, }; diff --git a/src/audio/eq_fir/eq_fir.h b/src/audio/eq_fir/eq_fir.h index 5af74dc5674a..047f6c1387c5 100644 --- a/src/audio/eq_fir/eq_fir.h +++ b/src/audio/eq_fir/eq_fir.h @@ -26,11 +26,11 @@ #include #include -/** \brief Macros to convert without division bytes count to samples count */ -#define EQ_FIR_BYTES_TO_S16_SAMPLES(b) ((b) >> 1) -#define EQ_FIR_BYTES_TO_S32_SAMPLES(b) ((b) >> 2) - -/* fir component private data */ +/** + * \brief FIR component private data. + * + * FIR implementations receive already acquired circular source and sink views. + */ struct comp_data { struct fir_state_32x16 fir[PLATFORM_MAX_CHANNELS]; /**< filters state */ struct comp_data_blob_handler *model_handler; @@ -39,34 +39,34 @@ struct comp_data { size_t config_size; /**< configuration size */ size_t fir_delay_size; /**< allocated size */ void (*eq_fir_func)(struct fir_state_32x16 fir[], - struct input_stream_buffer *bsource, - struct output_stream_buffer *bsink, - int frames); + const struct cir_buf_source *source, + struct cir_buf_sink *sink, + size_t frames, unsigned int channels); int nch; }; #if CONFIG_FORMAT_S16LE -void eq_fir_s16(struct fir_state_32x16 *fir, struct input_stream_buffer *bsource, - struct output_stream_buffer *bsink, int frames); +void eq_fir_s16(struct fir_state_32x16 *fir, const struct cir_buf_source *source, + struct cir_buf_sink *sink, size_t frames, unsigned int channels); -void eq_fir_2x_s16(struct fir_state_32x16 *fir, struct input_stream_buffer *bsource, - struct output_stream_buffer *bsink, int frames); +void eq_fir_2x_s16(struct fir_state_32x16 *fir, const struct cir_buf_source *source, + struct cir_buf_sink *sink, size_t frames, unsigned int channels); #endif /* CONFIG_FORMAT_S16LE */ #if CONFIG_FORMAT_S24LE -void eq_fir_s24(struct fir_state_32x16 *fir, struct input_stream_buffer *bsource, - struct output_stream_buffer *bsink, int frames); +void eq_fir_s24(struct fir_state_32x16 *fir, const struct cir_buf_source *source, + struct cir_buf_sink *sink, size_t frames, unsigned int channels); -void eq_fir_2x_s24(struct fir_state_32x16 *fir, struct input_stream_buffer *bsource, - struct output_stream_buffer *bsink, int frames); +void eq_fir_2x_s24(struct fir_state_32x16 *fir, const struct cir_buf_source *source, + struct cir_buf_sink *sink, size_t frames, unsigned int channels); #endif /* CONFIG_FORMAT_S24LE */ #if CONFIG_FORMAT_S32LE -void eq_fir_s32(struct fir_state_32x16 *fir, struct input_stream_buffer *bsource, - struct output_stream_buffer *bsink, int frames); +void eq_fir_s32(struct fir_state_32x16 *fir, const struct cir_buf_source *source, + struct cir_buf_sink *sink, size_t frames, unsigned int channels); -void eq_fir_2x_s32(struct fir_state_32x16 *fir, struct input_stream_buffer *bsource, - struct output_stream_buffer *bsink, int frames); +void eq_fir_2x_s32(struct fir_state_32x16 *fir, const struct cir_buf_source *source, + struct cir_buf_sink *sink, size_t frames, unsigned int channels); #endif /* CONFIG_FORMAT_S32LE */ int set_fir_func(struct processing_module *mod, enum sof_ipc_frame fmt); diff --git a/src/audio/eq_fir/eq_fir_generic.c b/src/audio/eq_fir/eq_fir_generic.c index d13757e29716..dea5d682ff69 100644 --- a/src/audio/eq_fir/eq_fir_generic.c +++ b/src/audio/eq_fir/eq_fir_generic.c @@ -22,113 +22,110 @@ LOG_MODULE_DECLARE(eq_fir, CONFIG_SOF_LOG_LEVEL); #if CONFIG_FORMAT_S16LE -void eq_fir_s16(struct fir_state_32x16 fir[], struct input_stream_buffer *bsource, - struct output_stream_buffer *bsink, int frames) +void eq_fir_s16(struct fir_state_32x16 fir[], const struct cir_buf_source *source, + struct cir_buf_sink *sink, size_t frames, unsigned int channels) { - struct audio_stream *source = bsource->data; - struct audio_stream *sink = bsink->data; struct fir_state_32x16 *filter; int32_t z; - int16_t *x0, *y0; - int16_t *x = audio_stream_get_rptr(source); - int16_t *y = audio_stream_get_wptr(sink); - int nmax, n, i, j; - int nch = audio_stream_get_channels(source); - int remaining_samples = frames * nch; + const int16_t *x0; + int16_t *y0; + const int16_t *x = source->ptr; + int16_t *y = sink->ptr; + size_t nmax, n, i; + size_t remaining_samples = frames * channels; + unsigned int j; while (remaining_samples) { - nmax = EQ_FIR_BYTES_TO_S16_SAMPLES(audio_stream_bytes_without_wrap(source, x)); + nmax = cir_buf_samples_without_wrap_s16(x, source->buf_end); n = MIN(remaining_samples, nmax); - nmax = EQ_FIR_BYTES_TO_S16_SAMPLES(audio_stream_bytes_without_wrap(sink, y)); + nmax = cir_buf_samples_without_wrap_s16(y, sink->buf_end); n = MIN(n, nmax); - for (j = 0; j < nch; j++) { + for (j = 0; j < channels; j++) { x0 = x + j; y0 = y + j; filter = &fir[j]; - for (i = 0; i < n; i += nch) { + for (i = 0; i < n; i += channels) { z = fir_32x16(filter, *x0 << 16); *y0 = sat_int16(Q_SHIFT_RND(z, 31, 15)); - x0 += nch; - y0 += nch; + x0 += channels; + y0 += channels; } } remaining_samples -= n; - x = audio_stream_wrap(source, x + n); - y = audio_stream_wrap(sink, y + n); + x = source_cir_buf_wrap(x + n, source->buf_start, source->buf_end); + y = cir_buf_wrap(y + n, sink->buf_start, sink->buf_end); } } #endif /* CONFIG_FORMAT_S16LE */ #if CONFIG_FORMAT_S24LE -void eq_fir_s24(struct fir_state_32x16 fir[], struct input_stream_buffer *bsource, - struct output_stream_buffer *bsink, int frames) +void eq_fir_s24(struct fir_state_32x16 fir[], const struct cir_buf_source *source, + struct cir_buf_sink *sink, size_t frames, unsigned int channels) { - struct audio_stream *source = bsource->data; - struct audio_stream *sink = bsink->data; struct fir_state_32x16 *filter; int32_t z; - int32_t *x0, *y0; - int32_t *x = audio_stream_get_rptr(source); - int32_t *y = audio_stream_get_wptr(sink); - int nmax, n, i, j; - int nch = audio_stream_get_channels(source); - int remaining_samples = frames * nch; + const int32_t *x0; + int32_t *y0; + const int32_t *x = source->ptr; + int32_t *y = sink->ptr; + size_t nmax, n, i; + size_t remaining_samples = frames * channels; + unsigned int j; while (remaining_samples) { - nmax = EQ_FIR_BYTES_TO_S32_SAMPLES(audio_stream_bytes_without_wrap(source, x)); + nmax = cir_buf_samples_without_wrap_s32(x, source->buf_end); n = MIN(remaining_samples, nmax); - nmax = EQ_FIR_BYTES_TO_S32_SAMPLES(audio_stream_bytes_without_wrap(sink, y)); + nmax = cir_buf_samples_without_wrap_s32(y, sink->buf_end); n = MIN(n, nmax); - for (j = 0; j < nch; j++) { + for (j = 0; j < channels; j++) { x0 = x + j; y0 = y + j; filter = &fir[j]; - for (i = 0; i < n; i += nch) { + for (i = 0; i < n; i += channels) { z = fir_32x16(filter, *x0 << 8); *y0 = sat_int24(Q_SHIFT_RND(z, 31, 23)); - x0 += nch; - y0 += nch; + x0 += channels; + y0 += channels; } } remaining_samples -= n; - x = audio_stream_wrap(source, x + n); - y = audio_stream_wrap(sink, y + n); + x = source_cir_buf_wrap(x + n, source->buf_start, source->buf_end); + y = cir_buf_wrap(y + n, sink->buf_start, sink->buf_end); } } #endif /* CONFIG_FORMAT_S24LE */ #if CONFIG_FORMAT_S32LE -void eq_fir_s32(struct fir_state_32x16 fir[], struct input_stream_buffer *bsource, - struct output_stream_buffer *bsink, int frames) +void eq_fir_s32(struct fir_state_32x16 fir[], const struct cir_buf_source *source, + struct cir_buf_sink *sink, size_t frames, unsigned int channels) { - struct audio_stream *source = bsource->data; - struct audio_stream *sink = bsink->data; struct fir_state_32x16 *filter; - int32_t *x0, *y0; - int32_t *x = audio_stream_get_rptr(source); - int32_t *y = audio_stream_get_wptr(sink); - int nmax, n, i, j; - int nch = audio_stream_get_channels(source); - int remaining_samples = frames * nch; + const int32_t *x0; + int32_t *y0; + const int32_t *x = source->ptr; + int32_t *y = sink->ptr; + size_t nmax, n, i; + size_t remaining_samples = frames * channels; + unsigned int j; while (remaining_samples) { - nmax = EQ_FIR_BYTES_TO_S32_SAMPLES(audio_stream_bytes_without_wrap(source, x)); + nmax = cir_buf_samples_without_wrap_s32(x, source->buf_end); n = MIN(remaining_samples, nmax); - nmax = EQ_FIR_BYTES_TO_S32_SAMPLES(audio_stream_bytes_without_wrap(sink, y)); + nmax = cir_buf_samples_without_wrap_s32(y, sink->buf_end); n = MIN(n, nmax); - for (j = 0; j < nch; j++) { + for (j = 0; j < channels; j++) { x0 = x + j; y0 = y + j; filter = &fir[j]; - for (i = 0; i < n; i += nch) { + for (i = 0; i < n; i += channels) { *y0 = fir_32x16(filter, *x0); - x0 += nch; - y0 += nch; + x0 += channels; + y0 += channels; } } remaining_samples -= n; - x = audio_stream_wrap(source, x + n); - y = audio_stream_wrap(sink, y + n); + x = source_cir_buf_wrap(x + n, source->buf_start, source->buf_end); + y = cir_buf_wrap(y + n, sink->buf_start, sink->buf_end); } } #endif /* CONFIG_FORMAT_S32LE */ diff --git a/src/audio/eq_fir/eq_fir_hifi2ep.c b/src/audio/eq_fir/eq_fir_hifi2ep.c index 71b40edcee32..eb7790b7ed62 100644 --- a/src/audio/eq_fir/eq_fir_hifi2ep.c +++ b/src/audio/eq_fir/eq_fir_hifi2ep.c @@ -27,138 +27,217 @@ LOG_MODULE_DECLARE(eq_fir, CONFIG_SOF_LOG_LEVEL); /* For even frame lengths use FIR filter that processes two sequential * sample per call. */ -void eq_fir_2x_s32(struct fir_state_32x16 fir[], struct input_stream_buffer *bsource, - struct output_stream_buffer *bsink, int frames) +void eq_fir_2x_s32(struct fir_state_32x16 fir[], const struct cir_buf_source *source, + struct cir_buf_sink *sink, size_t frames, unsigned int channels) { - struct audio_stream *source = bsource->data; - struct audio_stream *sink = bsink->data; struct fir_state_32x16 *f; - int32_t *src = audio_stream_get_rptr(source); - int32_t *snk = audio_stream_get_wptr(sink); - int32_t *x0; + const int32_t *src = source->ptr; + int32_t *snk = sink->ptr; + const int32_t *x0; int32_t *y0; - int32_t *x1; + const int32_t *x1; int32_t *y1; - int ch; - int i; + size_t i; + size_t remaining_frames = frames; + unsigned int ch; + unsigned int nch = channels; int rshift; int lshift; - int nch = audio_stream_get_channels(source); - int inc = nch << 1; - - for (ch = 0; ch < nch; ch++) { - /* Get FIR instance and get shifts to e.g. apply mute - * without overhead. - */ - f = &fir[ch]; - fir_get_lrshifts(f, &lshift, &rshift); - - /* Setup circular buffer for FIR input data delay */ - fir_hifiep_setup_circular(f); - - x0 = src++; - y0 = snk++; - for (i = 0; i < (frames >> 1); i++) { - x1 = x0 + nch; - y1 = y0 + nch; - fir_32x16_2x(f, *x0, *x1, y0, y1, lshift, rshift); - x0 += inc; - y0 += inc; + + while (remaining_frames) { + size_t source_frames = cir_buf_samples_without_wrap_s32(src, source->buf_end) / nch; + size_t sink_frames = cir_buf_samples_without_wrap_s32(snk, sink->buf_end) / nch; + size_t chunk_frames = MIN(remaining_frames, MIN(source_frames, sink_frames)); + + chunk_frames &= ~0x1; + if (!chunk_frames) { + for (ch = 0; ch < nch; ch++) { + f = &fir[ch]; + fir_get_lrshifts(f, &lshift, &rshift); + fir_hifiep_setup_circular(f); + y0 = snk + ch; + fir_32x16(f, src[ch], y0, lshift, rshift); + } + src = source_cir_buf_wrap(src + nch, source->buf_start, source->buf_end); + snk = cir_buf_wrap(snk + nch, sink->buf_start, sink->buf_end); + remaining_frames--; + continue; + } + + for (ch = 0; ch < nch; ch++) { + /* Get FIR instance and get shifts to e.g. apply mute + * without overhead. + */ + f = &fir[ch]; + fir_get_lrshifts(f, &lshift, &rshift); + + /* Setup circular buffer for FIR input data delay */ + fir_hifiep_setup_circular(f); + + x0 = src + ch; + y0 = snk + ch; + for (i = 0; i < (chunk_frames >> 1); i++) { + x1 = x0 + nch; + y1 = y0 + nch; + fir_32x16_2x(f, *x0, *x1, y0, y1, lshift, rshift); + x0 += 2 * nch; + y0 += 2 * nch; + } } + + src = source_cir_buf_wrap(src + chunk_frames * nch, + source->buf_start, source->buf_end); + snk = cir_buf_wrap(snk + chunk_frames * nch, + sink->buf_start, sink->buf_end); + remaining_frames -= chunk_frames; } } #endif /* CONFIG_FORMAT_S32LE */ #if CONFIG_FORMAT_S24LE -void eq_fir_2x_s24(struct fir_state_32x16 fir[], struct input_stream_buffer *bsource, - struct output_stream_buffer *bsink, int frames) +void eq_fir_2x_s24(struct fir_state_32x16 fir[], const struct cir_buf_source *source, + struct cir_buf_sink *sink, size_t frames, unsigned int channels) { - struct audio_stream *source = bsource->data; - struct audio_stream *sink = bsink->data; struct fir_state_32x16 *f; - int32_t *src = audio_stream_get_rptr(source); - int32_t *snk = audio_stream_get_wptr(sink); - int32_t *x0; + const int32_t *src = source->ptr; + int32_t *snk = sink->ptr; + const int32_t *x0; int32_t *y0; - int32_t *x1; + const int32_t *x1; int32_t *y1; int32_t z0; int32_t z1; - int ch; - int i; + size_t i; + size_t remaining_frames = frames; + unsigned int ch; + unsigned int nch = channels; int rshift; int lshift; - int nch = audio_stream_get_channels(source); - int inc = nch << 1; - - for (ch = 0; ch < nch; ch++) { - /* Get FIR instance and get shifts to e.g. apply mute - * without overhead. - */ - f = &fir[ch]; - fir_get_lrshifts(f, &lshift, &rshift); - - /* Setup circular buffer for FIR input data delay */ - fir_hifiep_setup_circular(f); - - x0 = src++; - y0 = snk++; - for (i = 0; i < (frames >> 1); i++) { - x1 = x0 + nch; - y1 = y0 + nch; - fir_32x16_2x(f, *x0 << 8, *x1 << 8, &z0, &z1, lshift, rshift); - *y0 = sat_int24(Q_SHIFT_RND(z0, 31, 23)); - *y1 = sat_int24(Q_SHIFT_RND(z1, 31, 23)); - x0 += inc; - y0 += inc; + + while (remaining_frames) { + size_t source_frames = cir_buf_samples_without_wrap_s32(src, source->buf_end) / nch; + size_t sink_frames = cir_buf_samples_without_wrap_s32(snk, sink->buf_end) / nch; + size_t chunk_frames = MIN(remaining_frames, MIN(source_frames, sink_frames)); + + chunk_frames &= ~0x1; + if (!chunk_frames) { + for (ch = 0; ch < nch; ch++) { + int32_t z; + + f = &fir[ch]; + fir_get_lrshifts(f, &lshift, &rshift); + fir_hifiep_setup_circular(f); + fir_32x16(f, src[ch] << 8, &z, lshift, rshift); + snk[ch] = sat_int24(Q_SHIFT_RND(z, 31, 23)); + } + src = source_cir_buf_wrap(src + nch, source->buf_start, source->buf_end); + snk = cir_buf_wrap(snk + nch, sink->buf_start, sink->buf_end); + remaining_frames--; + continue; + } + + for (ch = 0; ch < nch; ch++) { + /* Get FIR instance and get shifts to e.g. apply mute + * without overhead. + */ + f = &fir[ch]; + fir_get_lrshifts(f, &lshift, &rshift); + + /* Setup circular buffer for FIR input data delay */ + fir_hifiep_setup_circular(f); + + x0 = src + ch; + y0 = snk + ch; + for (i = 0; i < (chunk_frames >> 1); i++) { + x1 = x0 + nch; + y1 = y0 + nch; + fir_32x16_2x(f, *x0 << 8, *x1 << 8, &z0, &z1, lshift, rshift); + *y0 = sat_int24(Q_SHIFT_RND(z0, 31, 23)); + *y1 = sat_int24(Q_SHIFT_RND(z1, 31, 23)); + x0 += 2 * nch; + y0 += 2 * nch; + } } + + src = source_cir_buf_wrap(src + chunk_frames * nch, + source->buf_start, source->buf_end); + snk = cir_buf_wrap(snk + chunk_frames * nch, + sink->buf_start, sink->buf_end); + remaining_frames -= chunk_frames; } } #endif /* CONFIG_FORMAT_S24LE */ #if CONFIG_FORMAT_S16LE -void eq_fir_2x_s16(struct fir_state_32x16 fir[], struct input_stream_buffer *bsource, - struct output_stream_buffer *bsink, int frames) +void eq_fir_2x_s16(struct fir_state_32x16 fir[], const struct cir_buf_source *source, + struct cir_buf_sink *sink, size_t frames, unsigned int channels) { - struct audio_stream *source = bsource->data; - struct audio_stream *sink = bsink->data; struct fir_state_32x16 *f; - int16_t *src = audio_stream_get_rptr(source); - int16_t *snk = audio_stream_get_wptr(sink); - int16_t *x0; + const int16_t *src = source->ptr; + int16_t *snk = sink->ptr; + const int16_t *x0; int16_t *y0; - int16_t *x1; + const int16_t *x1; int16_t *y1; int32_t z0; int32_t z1; - int ch; - int i; + size_t i; + size_t remaining_frames = frames; + unsigned int ch; + unsigned int nch = channels; int rshift; int lshift; - int nch = audio_stream_get_channels(source); - int inc = nch << 1; - - for (ch = 0; ch < nch; ch++) { - /* Get FIR instance and get shifts to e.g. apply mute - * without overhead. - */ - f = &fir[ch]; - fir_get_lrshifts(f, &lshift, &rshift); - - /* Setup circular buffer for FIR input data delay */ - fir_hifiep_setup_circular(f); - - x0 = src++; - y0 = snk++; - for (i = 0; i < (frames >> 1); i++) { - x1 = x0 + nch; - y1 = y0 + nch; - fir_32x16_2x(f, *x0 << 16, *x1 << 16, &z0, &z1, lshift, rshift); - *y0 = sat_int16(Q_SHIFT_RND(z0, 31, 15)); - *y1 = sat_int16(Q_SHIFT_RND(z1, 31, 15)); - x0 += inc; - y0 += inc; + + while (remaining_frames) { + size_t source_frames = cir_buf_samples_without_wrap_s16(src, source->buf_end) / nch; + size_t sink_frames = cir_buf_samples_without_wrap_s16(snk, sink->buf_end) / nch; + size_t chunk_frames = MIN(remaining_frames, MIN(source_frames, sink_frames)); + + chunk_frames &= ~0x1; + if (!chunk_frames) { + for (ch = 0; ch < nch; ch++) { + int32_t z; + + f = &fir[ch]; + fir_get_lrshifts(f, &lshift, &rshift); + fir_hifiep_setup_circular(f); + fir_32x16(f, src[ch] << 16, &z, lshift, rshift); + snk[ch] = sat_int16(Q_SHIFT_RND(z, 31, 15)); + } + src = source_cir_buf_wrap(src + nch, source->buf_start, source->buf_end); + snk = cir_buf_wrap(snk + nch, sink->buf_start, sink->buf_end); + remaining_frames--; + continue; } + + for (ch = 0; ch < nch; ch++) { + /* Get FIR instance and get shifts to e.g. apply mute + * without overhead. + */ + f = &fir[ch]; + fir_get_lrshifts(f, &lshift, &rshift); + + /* Setup circular buffer for FIR input data delay */ + fir_hifiep_setup_circular(f); + + x0 = src + ch; + y0 = snk + ch; + for (i = 0; i < (chunk_frames >> 1); i++) { + x1 = x0 + nch; + y1 = y0 + nch; + fir_32x16_2x(f, *x0 << 16, *x1 << 16, &z0, &z1, lshift, rshift); + *y0 = sat_int16(Q_SHIFT_RND(z0, 31, 15)); + *y1 = sat_int16(Q_SHIFT_RND(z1, 31, 15)); + x0 += 2 * nch; + y0 += 2 * nch; + } + } + + src = source_cir_buf_wrap(src + chunk_frames * nch, + source->buf_start, source->buf_end); + snk = cir_buf_wrap(snk + chunk_frames * nch, + sink->buf_start, sink->buf_end); + remaining_frames -= chunk_frames; } } #endif /* CONFIG_FORMAT_S16LE */ diff --git a/src/audio/eq_fir/eq_fir_hifi3.c b/src/audio/eq_fir/eq_fir_hifi3.c index 0d25851ea447..434741971250 100644 --- a/src/audio/eq_fir/eq_fir_hifi3.c +++ b/src/audio/eq_fir/eq_fir_hifi3.c @@ -26,34 +26,47 @@ LOG_MODULE_DECLARE(eq_fir, CONFIG_SOF_LOG_LEVEL); /* For even frame lengths use FIR filter that processes two sequential * sample per call. */ -void eq_fir_2x_s32(struct fir_state_32x16 fir[], struct input_stream_buffer *bsource, - struct output_stream_buffer *bsink, int frames) +void eq_fir_2x_s32(struct fir_state_32x16 fir[], const struct cir_buf_source *source, + struct cir_buf_sink *sink, size_t frames, unsigned int channels) { - struct audio_stream *source = bsource->data; - struct audio_stream *sink = bsink->data; struct fir_state_32x16 *f; ae_int32x2 d0 = 0; ae_int32x2 d1 = 0; - ae_int32 *src = audio_stream_get_rptr(source); - ae_int32 *dst = audio_stream_get_wptr(sink); - ae_int32 *x; + const ae_int32 *src = source->ptr; + ae_int32 *dst = (ae_int32 *)sink->ptr; + const ae_int32 *x; ae_int32 *y0; ae_int32 *y1; - int ch; - int i, n, nmax; + size_t i; + size_t inc_nch_s = channels * sizeof(int32_t); + size_t inc_2nch_s = 2 * inc_nch_s; + size_t remaining_frames = frames; + unsigned int ch; + unsigned int nch = channels; int rshift; int lshift; int shift; - int nch = audio_stream_get_channels(source); - int inc_nch_s = nch * sizeof(int32_t); - int inc_2nch_s = 2 * inc_nch_s; - int samples = nch * frames; - - while (samples) { - nmax = audio_stream_samples_without_wrap_s32(sink, dst); - n = MIN(nmax, samples); - nmax = audio_stream_samples_without_wrap_s32(source, src); - n = MIN(n, nmax); + + while (remaining_frames) { + size_t source_frames = cir_buf_samples_without_wrap_s32(src, source->buf_end) / nch; + size_t sink_frames = cir_buf_samples_without_wrap_s32(dst, sink->buf_end) / nch; + size_t chunk_frames = MIN(remaining_frames, MIN(source_frames, sink_frames)); + + chunk_frames &= ~0x1; + if (!chunk_frames) { + for (ch = 0; ch < nch; ch++) { + f = &fir[ch]; + fir_get_lrshifts(f, &lshift, &rshift); + shift = lshift - rshift; + fir_core_setup_circular(f); + fir_32x16(f, src[ch], dst + ch, shift); + } + src = source_cir_buf_wrap(src + nch, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst + nch, sink->buf_start, sink->buf_end); + remaining_frames--; + continue; + } + for (ch = 0; ch < nch; ch++) { /* Get FIR instance and get shifts.*/ f = &fir[ch]; @@ -66,7 +79,7 @@ void eq_fir_2x_s32(struct fir_state_32x16 fir[], struct input_stream_buffer *bso y0 = dst + ch; y1 = y0 + nch; - for (i = 0; i < (n >> 1); i += nch) { + for (i = 0; i < (chunk_frames >> 1); i++) { /* Load two input samples via input pointer x */ AE_L32_XP(d0, x, inc_nch_s); AE_L32_XP(d1, x, inc_nch_s); @@ -75,42 +88,63 @@ void eq_fir_2x_s32(struct fir_state_32x16 fir[], struct input_stream_buffer *bso AE_L32_XC(d1, y1, inc_2nch_s); } } - samples -= n; - dst = audio_stream_wrap(sink, dst + n); - src = audio_stream_wrap(source, src + n); + dst = cir_buf_wrap(dst + chunk_frames * nch, + sink->buf_start, sink->buf_end); + src = source_cir_buf_wrap(src + chunk_frames * nch, source->buf_start, + source->buf_end); + remaining_frames -= chunk_frames; } } #endif /* CONFIG_FORMAT_S32LE */ #if CONFIG_FORMAT_S24LE -void eq_fir_2x_s24(struct fir_state_32x16 fir[], struct input_stream_buffer *bsource, - struct output_stream_buffer *bsink, int frames) +void eq_fir_2x_s24(struct fir_state_32x16 fir[], const struct cir_buf_source *source, + struct cir_buf_sink *sink, size_t frames, unsigned int channels) { - struct audio_stream *source = bsource->data; - struct audio_stream *sink = bsink->data; struct fir_state_32x16 *f; ae_int32x2 d0 = 0; ae_int32x2 d1 = 0; ae_int32 z0; ae_int32 z1; - ae_int32 *src = audio_stream_get_rptr(source); - ae_int32 *dst = audio_stream_get_wptr(sink); - ae_int32 *x; + const ae_int32 *src = source->ptr; + ae_int32 *dst = (ae_int32 *)sink->ptr; + const ae_int32 *x; ae_int32 *y; - int ch; - int i, n, nmax; + size_t i; + size_t inc_nch_s = channels * sizeof(int32_t); + size_t remaining_frames = frames; + unsigned int ch; + unsigned int nch = channels; int rshift; int lshift; int shift; - int nch = audio_stream_get_channels(source); - int inc_nch_s = nch * sizeof(int32_t); - int samples = nch * frames; - - while (samples) { - nmax = audio_stream_samples_without_wrap_s24(sink, dst); - n = MIN(nmax, samples); - nmax = audio_stream_samples_without_wrap_s24(source, src); - n = MIN(n, nmax); + + while (remaining_frames) { + size_t source_frames = cir_buf_samples_without_wrap_s32(src, source->buf_end) / nch; + size_t sink_frames = cir_buf_samples_without_wrap_s32(dst, sink->buf_end) / nch; + size_t chunk_frames = MIN(remaining_frames, MIN(source_frames, sink_frames)); + + chunk_frames &= ~0x1; + if (!chunk_frames) { + for (ch = 0; ch < nch; ch++) { + ae_int32 input = src[ch] << 8; + ae_int32 output; + int32_t output_sample; + + f = &fir[ch]; + fir_get_lrshifts(f, &lshift, &rshift); + shift = lshift - rshift; + fir_core_setup_circular(f); + fir_32x16(f, input, &output, shift); + output_sample = AE_MOVAD32_L(output); + dst[ch] = sat_int24(Q_SHIFT_RND(output_sample, 31, 23)); + } + src = source_cir_buf_wrap(src + nch, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst + nch, sink->buf_start, sink->buf_end); + remaining_frames--; + continue; + } + for (ch = 0; ch < nch; ch++) { /* Get FIR instance and get shifts.*/ f = &fir[ch]; @@ -122,7 +156,7 @@ void eq_fir_2x_s24(struct fir_state_32x16 fir[], struct input_stream_buffer *bso x = src + ch; y = dst + ch; - for (i = 0; i < (n >> 1); i += nch) { + for (i = 0; i < (chunk_frames >> 1); i++) { /* Load two input samples via input pointer x */ AE_L32_XP(d0, x, inc_nch_s); AE_L32_XP(d1, x, inc_nch_s); @@ -147,19 +181,19 @@ void eq_fir_2x_s24(struct fir_state_32x16 fir[], struct input_stream_buffer *bso AE_S32_L_XC(d1, y, inc_nch_s); } } - samples -= n; - dst = audio_stream_wrap(sink, dst + n); - src = audio_stream_wrap(source, src + n); + dst = cir_buf_wrap(dst + chunk_frames * nch, + sink->buf_start, sink->buf_end); + src = source_cir_buf_wrap(src + chunk_frames * nch, source->buf_start, + source->buf_end); + remaining_frames -= chunk_frames; } } #endif /* CONFIG_FORMAT_S24LE */ #if CONFIG_FORMAT_S16LE -void eq_fir_2x_s16(struct fir_state_32x16 fir[], struct input_stream_buffer *bsource, - struct output_stream_buffer *bsink, int frames) +void eq_fir_2x_s16(struct fir_state_32x16 fir[], const struct cir_buf_source *source, + struct cir_buf_sink *sink, size_t frames, unsigned int channels) { - struct audio_stream *source = bsource->data; - struct audio_stream *sink = bsink->data; struct fir_state_32x16 *f; ae_int16x4 d0 = AE_ZERO16(); ae_int16x4 d1 = AE_ZERO16(); @@ -167,24 +201,45 @@ void eq_fir_2x_s16(struct fir_state_32x16 fir[], struct input_stream_buffer *bso ae_int32 z1; ae_int32 x0; ae_int32 x1; - ae_int16 *src = audio_stream_get_rptr(source); - ae_int16 *dst = audio_stream_get_wptr(sink); - ae_int16 *x; + const ae_int16 *src = source->ptr; + ae_int16 *dst = (ae_int16 *)sink->ptr; + const ae_int16 *x; ae_int16 *y; - int ch; - int i, n, nmax; + size_t i; + size_t inc_nch_s = channels * sizeof(int16_t); + size_t remaining_frames = frames; + unsigned int ch; + unsigned int nch = channels; int rshift; int lshift; int shift; - int nch = audio_stream_get_channels(source); - int inc_nch_s = nch * sizeof(int16_t); - int samples = nch * frames; - - while (samples) { - nmax = audio_stream_samples_without_wrap_s16(sink, dst); - n = MIN(nmax, samples); - nmax = audio_stream_samples_without_wrap_s16(source, src); - n = MIN(n, nmax); + + while (remaining_frames) { + size_t source_frames = cir_buf_samples_without_wrap_s16(src, source->buf_end) / nch; + size_t sink_frames = cir_buf_samples_without_wrap_s16(dst, sink->buf_end) / nch; + size_t chunk_frames = MIN(remaining_frames, MIN(source_frames, sink_frames)); + + chunk_frames &= ~0x1; + if (!chunk_frames) { + for (ch = 0; ch < nch; ch++) { + ae_int32 input = ((const int16_t *)src)[ch] << 16; + ae_int32 output; + int32_t output_sample; + + f = &fir[ch]; + fir_get_lrshifts(f, &lshift, &rshift); + shift = lshift - rshift; + fir_core_setup_circular(f); + fir_32x16(f, input, &output, shift); + output_sample = AE_MOVAD32_L(output); + dst[ch] = sat_int16(Q_SHIFT_RND(output_sample, 31, 15)); + } + src = source_cir_buf_wrap(src + nch, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst + nch, sink->buf_start, sink->buf_end); + remaining_frames--; + continue; + } + for (ch = 0; ch < nch; ch++) { /* Get FIR instance and get shifts.*/ f = &fir[ch]; @@ -196,7 +251,7 @@ void eq_fir_2x_s16(struct fir_state_32x16 fir[], struct input_stream_buffer *bso x = src + ch; y = dst + ch; - for (i = 0; i < (n >> 1); i += nch) { + for (i = 0; i < (chunk_frames >> 1); i++) { /* Load two input samples via input pointer x */ AE_L16_XP(d0, x, inc_nch_s); AE_L16_XP(d1, x, inc_nch_s); @@ -216,9 +271,11 @@ void eq_fir_2x_s16(struct fir_state_32x16 fir[], struct input_stream_buffer *bso AE_S16_0_XC(d1, y, inc_nch_s); } } - samples -= n; - dst = audio_stream_wrap(sink, dst + n); - src = audio_stream_wrap(source, src + n); + dst = cir_buf_wrap(dst + chunk_frames * nch, + sink->buf_start, sink->buf_end); + src = source_cir_buf_wrap(src + chunk_frames * nch, source->buf_start, + source->buf_end); + remaining_frames -= chunk_frames; } } #endif /* CONFIG_FORMAT_S16LE */ diff --git a/test/cmocka/src/audio/eq_fir/eq_fir_process.c b/test/cmocka/src/audio/eq_fir/eq_fir_process.c index 5e3b1dd5b156..0158b09dc29d 100644 --- a/test/cmocka/src/audio/eq_fir/eq_fir_process.c +++ b/test/cmocka/src/audio/eq_fir/eq_fir_process.c @@ -6,12 +6,18 @@ #include #include #include +#include #include #include #include #include +#include #include +#include +#include #include +#include +#include #include "../../util.h" #include "../../../include/cmocka_chirp_2ch.h" @@ -44,8 +50,8 @@ struct buffer_verify { } buffer_verify_data; struct test_parameters { - uint32_t channels; - uint32_t frames; + unsigned int channels; + size_t frames; uint32_t buffer_size_mult; uint32_t source_format; uint32_t sink_format; @@ -83,13 +89,15 @@ static struct sof_ipc_comp_process *create_eq_fir_comp_ipc(struct test_data *td) return ipc; } -static int eq_fir_send_config(struct processing_module *mod) +static int eq_fir_send_blob(struct processing_module *mod, + const struct sof_abi_hdr *blob, + size_t blob_storage_size) { const struct module_interface *const ops = mod->dev->drv->adapter_ops; - struct sof_abi_hdr *blob = (struct sof_abi_hdr *)fir_coef_2ch; - size_t cdata_size = sizeof(struct sof_ipc_ctrl_data) + - sizeof(struct sof_abi_hdr) + blob->size; + size_t cdata_size = sizeof(struct sof_ipc_ctrl_data) + sizeof(struct sof_abi_hdr) + + blob->size; struct sof_ipc_ctrl_data *cdata; + size_t copy_size; int ret; cdata = calloc(1, cdata_size); @@ -102,7 +110,10 @@ static int eq_fir_send_config(struct processing_module *mod) cdata->data[0].type = blob->type; cdata->data[0].size = blob->size; cdata->data[0].abi = blob->abi; - memcpy_s(cdata->data[0].data, blob->size, blob->data, blob->size); + copy_size = blob_storage_size > sizeof(*blob) ? blob_storage_size - sizeof(*blob) : 0; + copy_size = MIN(copy_size, (size_t)blob->size); + if (copy_size) + memcpy_s(cdata->data[0].data, blob->size, blob->data, copy_size); ret = ops->set_configuration(mod, 0, MODULE_CFG_FRAGMENT_SINGLE, blob->size, (const uint8_t *)cdata, @@ -112,6 +123,18 @@ static int eq_fir_send_config(struct processing_module *mod) return ret; } +static int eq_fir_send_config(struct processing_module *mod) +{ + return eq_fir_send_blob(mod, (const struct sof_abi_hdr *)fir_coef_2ch, + sizeof(fir_coef_2ch)); +} + +static void copy_eq_fir_blob(uint32_t *blob) +{ + assert_int_equal(memcpy_s(blob, sizeof(fir_coef_2ch), fir_coef_2ch, + sizeof(fir_coef_2ch)), 0); +} + static void prepare_sink(struct test_data *td, struct processing_module *mod) { struct test_parameters *parameters = td->params; @@ -158,6 +181,8 @@ static int setup(void **state) struct test_data *td; struct sof_ipc_comp_process *ipc; struct comp_dev *dev; + struct sof_source *sources[1]; + struct sof_sink *sinks[1]; int ret; td = test_malloc(sizeof(*td)); @@ -198,7 +223,9 @@ static int setup(void **state) mod->stream_params->channels = params->channels; mod->period_bytes = get_frame_bytes(params->source_format, params->channels) * 48000 / 1000; - ret = module_prepare(mod, NULL, 0, NULL, 0); + sources[0] = audio_buffer_get_source(&td->source->audio_buffer); + sinks[0] = audio_buffer_get_sink(&td->sink->audio_buffer); + ret = module_prepare(mod, sources, 1, sinks, 1); if (ret) return ret; @@ -226,18 +253,18 @@ static int teardown(void **state) } #if CONFIG_FORMAT_S16LE -static void fill_source_s16(struct test_data *td, int frames_max) +static void fill_source_s16(struct test_data *td, size_t frames_max) { struct processing_module *mod = comp_mod(td->dev); struct comp_dev *dev = td->dev; struct comp_buffer *sb; struct audio_stream *ss; int16_t *x; - int bytes_total; - int samples; - int frames; - int i; - int samples_processed = 0; + size_t bytes_total; + size_t samples; + size_t frames; + size_t i; + size_t samples_processed = 0; sb = comp_dev_get_first_data_producer(dev); ss = &sb->stream; @@ -271,8 +298,8 @@ static void verify_sink_s16(struct test_data *td) int32_t ref; int32_t out; int16_t *x; - int samples; - int i; + size_t samples; + size_t i; sb = comp_dev_get_first_data_consumer(dev); ss = &sb->stream; @@ -292,18 +319,18 @@ static void verify_sink_s16(struct test_data *td) #endif /* CONFIG_FORMAT_S16LE */ #if CONFIG_FORMAT_S24LE -static void fill_source_s24(struct test_data *td, int frames_max) +static void fill_source_s24(struct test_data *td, size_t frames_max) { struct processing_module *mod = comp_mod(td->dev); struct comp_dev *dev = td->dev; struct comp_buffer *sb; struct audio_stream *ss; int32_t *x; - int bytes_total; - int samples; - int frames; - int i; - int samples_processed = 0; + size_t bytes_total; + size_t samples; + size_t frames; + size_t i; + size_t samples_processed = 0; sb = comp_dev_get_first_data_producer(dev); ss = &sb->stream; @@ -337,8 +364,8 @@ static void verify_sink_s24(struct test_data *td) int32_t ref; int32_t out; int32_t *x; - int samples; - int i; + size_t samples; + size_t i; sb = comp_dev_get_first_data_consumer(dev); ss = &sb->stream; @@ -358,18 +385,18 @@ static void verify_sink_s24(struct test_data *td) #endif /* CONFIG_FORMAT_S24LE */ #if CONFIG_FORMAT_S32LE -static void fill_source_s32(struct test_data *td, int frames_max) +static void fill_source_s32(struct test_data *td, size_t frames_max) { struct processing_module *mod = comp_mod(td->dev); struct comp_dev *dev = td->dev; struct comp_buffer *sb; struct audio_stream *ss; int32_t *x; - int bytes_total; - int samples; - int frames; - int i; - int samples_processed = 0; + size_t bytes_total; + size_t samples; + size_t frames; + size_t i; + size_t samples_processed = 0; sb = comp_dev_get_first_data_producer(dev); ss = &sb->stream; @@ -403,8 +430,8 @@ static void verify_sink_s32(struct test_data *td) int32_t ref; int32_t out; int32_t *x; - int samples; - int i; + size_t samples; + size_t i; sb = comp_dev_get_first_data_consumer(dev); ss = &sb->stream; @@ -423,7 +450,172 @@ static void verify_sink_s32(struct test_data *td) } #endif /* CONFIG_FORMAT_S32LE */ -static int frames_jitter(int frames) +static void fill_source_for_test(struct test_data *td, size_t frames) +{ + switch (audio_stream_get_frm_fmt(&td->source->stream)) { +#if CONFIG_FORMAT_S16LE + case SOF_IPC_FRAME_S16_LE: + fill_source_s16(td, frames); + break; +#endif +#if CONFIG_FORMAT_S24LE + case SOF_IPC_FRAME_S24_4LE: + fill_source_s24(td, frames); + break; +#endif +#if CONFIG_FORMAT_S32LE + case SOF_IPC_FRAME_S32_LE: + fill_source_s32(td, frames); + break; +#endif + default: + assert_true(false); + break; + } +} + +static void test_eq_fir_rounds_down_odd_frame_count(void **state) +{ + struct test_data *td = *state; + struct processing_module *mod = comp_mod(td->dev); + struct sof_source *sources[1]; + struct sof_sink *sinks[1]; + const size_t frame_bytes = get_frame_bytes(td->params->source_format, + td->params->channels); + size_t source_avail_before; + size_t sink_avail_before; + int ret; + + sources[0] = audio_buffer_get_source(&td->source->audio_buffer); + sinks[0] = audio_buffer_get_sink(&td->sink->audio_buffer); + fill_source_for_test(td, 3); + source_avail_before = audio_stream_get_avail_bytes(&td->source->stream); + sink_avail_before = audio_stream_get_avail_bytes(&td->sink->stream); + mod->output_buffers[0].size = 0; + + ret = module_process_sink_src(mod, sources, 1, sinks, 1); + assert_int_equal(ret, 0); + assert_int_equal(source_avail_before - + audio_stream_get_avail_bytes(&td->source->stream), + 2 * frame_bytes); + assert_int_equal(audio_stream_get_avail_bytes(&td->sink->stream) - + sink_avail_before, 2 * frame_bytes); +} + +static void test_eq_fir_rejects_invalid_configurations(void **state) +{ + struct test_data *td = *state; + struct processing_module *mod = comp_mod(td->dev); + uint32_t blob_copy[ARRAY_SIZE(fir_coef_2ch)]; + struct sof_abi_hdr *abi; + struct sof_eq_fir_config *config; + struct sof_fir_coef_data *coef; + int ret; + + /* Reject a blob with no channel assignments or coefficient data. */ + copy_eq_fir_blob(blob_copy); + abi = (struct sof_abi_hdr *)blob_copy; + config = (struct sof_eq_fir_config *)abi->data; + abi->size = sizeof(*config); + config->size = abi->size; + ret = eq_fir_send_blob(mod, abi, sizeof(blob_copy)); + assert_int_equal(ret, -EINVAL); + + /* Reject a blob with a truncated coefficient header. */ + copy_eq_fir_blob(blob_copy); + abi = (struct sof_abi_hdr *)blob_copy; + config = (struct sof_eq_fir_config *)abi->data; + abi->size = sizeof(*config) + 2 * sizeof(int16_t) + + (SOF_FIR_COEF_NHEADER - 1) * sizeof(int16_t); + config->size = abi->size; + ret = eq_fir_send_blob(mod, abi, sizeof(blob_copy)); + assert_int_equal(ret, -EINVAL); + + /* Reject a blob smaller than the configuration header. */ + copy_eq_fir_blob(blob_copy); + abi = (struct sof_abi_hdr *)blob_copy; + abi->size = sizeof(struct sof_eq_fir_config) - 1; + ret = eq_fir_send_blob(mod, abi, sizeof(blob_copy)); + assert_int_equal(ret, -EINVAL); + + /* Reject a blob larger than the maximum configuration size. */ + copy_eq_fir_blob(blob_copy); + abi = (struct sof_abi_hdr *)blob_copy; + abi->size = SOF_EQ_FIR_MAX_SIZE + 1; + ret = eq_fir_send_blob(mod, abi, sizeof(blob_copy)); + assert_int_equal(ret, -EINVAL); + + /* Reject mismatched ABI and configuration payload sizes. */ + copy_eq_fir_blob(blob_copy); + abi = (struct sof_abi_hdr *)blob_copy; + config = (struct sof_eq_fir_config *)abi->data; + config->size = abi->size - sizeof(int16_t); + ret = eq_fir_send_blob(mod, abi, sizeof(blob_copy)); + assert_int_equal(ret, -EINVAL); + + /* Reject an odd channel count that misaligns coefficient data. */ + copy_eq_fir_blob(blob_copy); + abi = (struct sof_abi_hdr *)blob_copy; + config = (struct sof_eq_fir_config *)abi->data; + config->channels_in_config = 1; + ret = eq_fir_send_blob(mod, abi, sizeof(blob_copy)); + assert_int_equal(ret, -EINVAL); + + /* Reject more responses than the configuration format supports. */ + copy_eq_fir_blob(blob_copy); + abi = (struct sof_abi_hdr *)blob_copy; + config = (struct sof_eq_fir_config *)abi->data; + config->number_of_responses = SOF_EQ_FIR_MAX_RESPONSES + 1; + ret = eq_fir_send_blob(mod, abi, sizeof(blob_copy)); + assert_int_equal(ret, -EINVAL); + + /* Reject an empty FIR response. */ + copy_eq_fir_blob(blob_copy); + abi = (struct sof_abi_hdr *)blob_copy; + config = (struct sof_eq_fir_config *)abi->data; + coef = (struct sof_fir_coef_data *)&config->data[config->channels_in_config]; + coef->length = 0; + ret = eq_fir_send_blob(mod, abi, sizeof(blob_copy)); + assert_int_equal(ret, -EINVAL); + + /* Reject a filter length that is not a multiple of four. */ + copy_eq_fir_blob(blob_copy); + abi = (struct sof_abi_hdr *)blob_copy; + config = (struct sof_eq_fir_config *)abi->data; + coef = (struct sof_fir_coef_data *)&config->data[config->channels_in_config]; + coef->length = 3; + ret = eq_fir_send_blob(mod, abi, sizeof(blob_copy)); + assert_int_equal(ret, -EINVAL); + + /* Reject a response whose coefficients extend past the blob. */ + copy_eq_fir_blob(blob_copy); + abi = (struct sof_abi_hdr *)blob_copy; + config = (struct sof_eq_fir_config *)abi->data; + coef = (struct sof_fir_coef_data *)&config->data[config->channels_in_config]; + coef->length = SOF_FIR_MAX_LENGTH; + ret = eq_fir_send_blob(mod, abi, sizeof(blob_copy)); + assert_int_equal(ret, -EINVAL); + + /* Reject a channel assignment that references an undefined response. */ + copy_eq_fir_blob(blob_copy); + abi = (struct sof_abi_hdr *)blob_copy; + config = (struct sof_eq_fir_config *)abi->data; + config->data[0] = 1; + ret = eq_fir_send_blob(mod, abi, sizeof(blob_copy)); + assert_int_equal(ret, -EINVAL); +} + +#if CONFIG_IPC_MAJOR_3 +static void test_eq_fir_rejects_invalid_frame_format(void **state) +{ + struct test_data *td = *state; + struct processing_module *mod = comp_mod(td->dev); + + assert_int_equal(set_fir_func(mod, (enum sof_ipc_frame)0xffff), -EINVAL); +} +#endif + +static size_t frames_jitter(size_t frames) { int r = rand(); @@ -442,8 +634,14 @@ static void test_audio_eq_fir(void **state) struct comp_buffer *source = td->source; struct comp_buffer *sink = td->sink; + struct sof_source *sources[1]; + struct sof_sink *sinks[1]; + size_t avail_before; + size_t frames; int ret; - int frames; + + sources[0] = audio_buffer_get_source(&source->audio_buffer); + sinks[0] = audio_buffer_get_sink(&sink->audio_buffer); while (td->continue_loop) { frames = frames_jitter(td->params->frames); @@ -464,15 +662,15 @@ static void test_audio_eq_fir(void **state) break; } - mod->input_buffers[0].consumed = 0; mod->output_buffers[0].size = 0; - ret = module_process_legacy(mod, mod->input_buffers, 1, - mod->output_buffers, 1); + td->dev->frames = mod->input_buffers[0].size; + avail_before = audio_stream_get_avail_bytes(&sink->stream); + ret = module_process_sink_src(mod, sources, 1, sinks, 1); assert_int_equal(ret, 0); - comp_update_buffer_consume(source, mod->input_buffers[0].consumed); - comp_update_buffer_produce(sink, mod->output_buffers[0].size); + mod->output_buffers[0].size = audio_stream_get_avail_bytes(&sink->stream) - + avail_before; switch (audio_stream_get_frm_fmt(&sink->stream)) { case SOF_IPC_FRAME_S16_LE: @@ -511,7 +709,11 @@ int main(void) int ret; int i; - struct CMUnitTest tests[ARRAY_SIZE(parameters)]; +#if CONFIG_IPC_MAJOR_3 + struct CMUnitTest tests[ARRAY_SIZE(parameters) + 3]; +#else + struct CMUnitTest tests[ARRAY_SIZE(parameters) + 2]; +#endif for (i = 0; i < ARRAY_SIZE(parameters); i++) { tests[i].name = "test_audio_eq_fir"; @@ -521,6 +723,30 @@ int main(void) tests[i].initial_state = ¶meters[i]; } + i = ARRAY_SIZE(parameters); + tests[i].name = "test_eq_fir_rounds_down_odd_frame_count"; + tests[i].test_func = test_eq_fir_rounds_down_odd_frame_count; + tests[i].setup_func = setup; + tests[i].teardown_func = teardown; + tests[i].initial_state = ¶meters[0]; + i++; + + tests[i].name = "test_eq_fir_rejects_invalid_configurations"; + tests[i].test_func = test_eq_fir_rejects_invalid_configurations; + tests[i].setup_func = setup; + tests[i].teardown_func = teardown; + tests[i].initial_state = ¶meters[0]; + i++; + +#if CONFIG_IPC_MAJOR_3 + tests[i].name = "test_eq_fir_rejects_invalid_frame_format"; + tests[i].test_func = test_eq_fir_rejects_invalid_frame_format; + tests[i].setup_func = setup; + tests[i].teardown_func = teardown; + tests[i].initial_state = ¶meters[0]; + i++; +#endif + cmocka_set_message_output(CM_OUTPUT_TAP); #ifdef DEBUG_FILES From d8559af0046330ac77182c83c36000ab34523c1c Mon Sep 17 00:00:00 2001 From: Piotr Hoppe Date: Mon, 28 Sep 2026 10:47:09 +0200 Subject: [PATCH 3/3] audio: eq_fir: clarify FIR processing variable names Replace abbreviated local variable names with descriptive names across the generic, HiFi2EP, and HiFi3 EQ FIR implementations. Clarify channel, sample, pointer, filter, and stride handling without changing processing behavior. Signed-off-by: Piotr Hoppe Co-authored-by: Copilot <223556219+Copilot@users.noreply.github.com> --- src/audio/eq_fir/eq_fir_generic.c | 143 +++++++++-------- src/audio/eq_fir/eq_fir_hifi2ep.c | 256 ++++++++++++++++-------------- src/audio/eq_fir/eq_fir_hifi3.c | 251 +++++++++++++++-------------- 3 files changed, 343 insertions(+), 307 deletions(-) diff --git a/src/audio/eq_fir/eq_fir_generic.c b/src/audio/eq_fir/eq_fir_generic.c index dea5d682ff69..397e7feb6601 100644 --- a/src/audio/eq_fir/eq_fir_generic.c +++ b/src/audio/eq_fir/eq_fir_generic.c @@ -26,34 +26,37 @@ void eq_fir_s16(struct fir_state_32x16 fir[], const struct cir_buf_source *sourc struct cir_buf_sink *sink, size_t frames, unsigned int channels) { struct fir_state_32x16 *filter; - int32_t z; - const int16_t *x0; - int16_t *y0; - const int16_t *x = source->ptr; - int16_t *y = sink->ptr; - size_t nmax, n, i; + int32_t filtered_sample; + const int16_t *src_channel; + int16_t *dst_channel; + const int16_t *src = source->ptr; + int16_t *dst = sink->ptr; + size_t max_samples; + size_t chunk_samples; + size_t sample_index; size_t remaining_samples = frames * channels; - unsigned int j; + unsigned int channel; while (remaining_samples) { - nmax = cir_buf_samples_without_wrap_s16(x, source->buf_end); - n = MIN(remaining_samples, nmax); - nmax = cir_buf_samples_without_wrap_s16(y, sink->buf_end); - n = MIN(n, nmax); - for (j = 0; j < channels; j++) { - x0 = x + j; - y0 = y + j; - filter = &fir[j]; - for (i = 0; i < n; i += channels) { - z = fir_32x16(filter, *x0 << 16); - *y0 = sat_int16(Q_SHIFT_RND(z, 31, 15)); - x0 += channels; - y0 += channels; + max_samples = cir_buf_samples_without_wrap_s16(src, source->buf_end); + chunk_samples = MIN(remaining_samples, max_samples); + max_samples = cir_buf_samples_without_wrap_s16(dst, sink->buf_end); + chunk_samples = MIN(chunk_samples, max_samples); + for (channel = 0; channel < channels; channel++) { + src_channel = src + channel; + dst_channel = dst + channel; + filter = &fir[channel]; + for (sample_index = 0; sample_index < chunk_samples; + sample_index += channels) { + filtered_sample = fir_32x16(filter, *src_channel << 16); + *dst_channel = sat_int16(Q_SHIFT_RND(filtered_sample, 31, 15)); + src_channel += channels; + dst_channel += channels; } } - remaining_samples -= n; - x = source_cir_buf_wrap(x + n, source->buf_start, source->buf_end); - y = cir_buf_wrap(y + n, sink->buf_start, sink->buf_end); + remaining_samples -= chunk_samples; + src = source_cir_buf_wrap(src + chunk_samples, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst + chunk_samples, sink->buf_start, sink->buf_end); } } #endif /* CONFIG_FORMAT_S16LE */ @@ -63,34 +66,37 @@ void eq_fir_s24(struct fir_state_32x16 fir[], const struct cir_buf_source *sourc struct cir_buf_sink *sink, size_t frames, unsigned int channels) { struct fir_state_32x16 *filter; - int32_t z; - const int32_t *x0; - int32_t *y0; - const int32_t *x = source->ptr; - int32_t *y = sink->ptr; - size_t nmax, n, i; + int32_t filtered_sample; + const int32_t *src_channel; + int32_t *dst_channel; + const int32_t *src = source->ptr; + int32_t *dst = sink->ptr; + size_t max_samples; + size_t chunk_samples; + size_t sample_index; size_t remaining_samples = frames * channels; - unsigned int j; + unsigned int channel; while (remaining_samples) { - nmax = cir_buf_samples_without_wrap_s32(x, source->buf_end); - n = MIN(remaining_samples, nmax); - nmax = cir_buf_samples_without_wrap_s32(y, sink->buf_end); - n = MIN(n, nmax); - for (j = 0; j < channels; j++) { - x0 = x + j; - y0 = y + j; - filter = &fir[j]; - for (i = 0; i < n; i += channels) { - z = fir_32x16(filter, *x0 << 8); - *y0 = sat_int24(Q_SHIFT_RND(z, 31, 23)); - x0 += channels; - y0 += channels; + max_samples = cir_buf_samples_without_wrap_s32(src, source->buf_end); + chunk_samples = MIN(remaining_samples, max_samples); + max_samples = cir_buf_samples_without_wrap_s32(dst, sink->buf_end); + chunk_samples = MIN(chunk_samples, max_samples); + for (channel = 0; channel < channels; channel++) { + src_channel = src + channel; + dst_channel = dst + channel; + filter = &fir[channel]; + for (sample_index = 0; sample_index < chunk_samples; + sample_index += channels) { + filtered_sample = fir_32x16(filter, *src_channel << 8); + *dst_channel = sat_int24(Q_SHIFT_RND(filtered_sample, 31, 23)); + src_channel += channels; + dst_channel += channels; } } - remaining_samples -= n; - x = source_cir_buf_wrap(x + n, source->buf_start, source->buf_end); - y = cir_buf_wrap(y + n, sink->buf_start, sink->buf_end); + remaining_samples -= chunk_samples; + src = source_cir_buf_wrap(src + chunk_samples, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst + chunk_samples, sink->buf_start, sink->buf_end); } } #endif /* CONFIG_FORMAT_S24LE */ @@ -100,32 +106,35 @@ void eq_fir_s32(struct fir_state_32x16 fir[], const struct cir_buf_source *sourc struct cir_buf_sink *sink, size_t frames, unsigned int channels) { struct fir_state_32x16 *filter; - const int32_t *x0; - int32_t *y0; - const int32_t *x = source->ptr; - int32_t *y = sink->ptr; - size_t nmax, n, i; + const int32_t *src_channel; + int32_t *dst_channel; + const int32_t *src = source->ptr; + int32_t *dst = sink->ptr; + size_t max_samples; + size_t chunk_samples; + size_t sample_index; size_t remaining_samples = frames * channels; - unsigned int j; + unsigned int channel; while (remaining_samples) { - nmax = cir_buf_samples_without_wrap_s32(x, source->buf_end); - n = MIN(remaining_samples, nmax); - nmax = cir_buf_samples_without_wrap_s32(y, sink->buf_end); - n = MIN(n, nmax); - for (j = 0; j < channels; j++) { - x0 = x + j; - y0 = y + j; - filter = &fir[j]; - for (i = 0; i < n; i += channels) { - *y0 = fir_32x16(filter, *x0); - x0 += channels; - y0 += channels; + max_samples = cir_buf_samples_without_wrap_s32(src, source->buf_end); + chunk_samples = MIN(remaining_samples, max_samples); + max_samples = cir_buf_samples_without_wrap_s32(dst, sink->buf_end); + chunk_samples = MIN(chunk_samples, max_samples); + for (channel = 0; channel < channels; channel++) { + src_channel = src + channel; + dst_channel = dst + channel; + filter = &fir[channel]; + for (sample_index = 0; sample_index < chunk_samples; + sample_index += channels) { + *dst_channel = fir_32x16(filter, *src_channel); + src_channel += channels; + dst_channel += channels; } } - remaining_samples -= n; - x = source_cir_buf_wrap(x + n, source->buf_start, source->buf_end); - y = cir_buf_wrap(y + n, sink->buf_start, sink->buf_end); + remaining_samples -= chunk_samples; + src = source_cir_buf_wrap(src + chunk_samples, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst + chunk_samples, sink->buf_start, sink->buf_end); } } #endif /* CONFIG_FORMAT_S32LE */ diff --git a/src/audio/eq_fir/eq_fir_hifi2ep.c b/src/audio/eq_fir/eq_fir_hifi2ep.c index eb7790b7ed62..3b7904b462b7 100644 --- a/src/audio/eq_fir/eq_fir_hifi2ep.c +++ b/src/audio/eq_fir/eq_fir_hifi2ep.c @@ -30,65 +30,69 @@ LOG_MODULE_DECLARE(eq_fir, CONFIG_SOF_LOG_LEVEL); void eq_fir_2x_s32(struct fir_state_32x16 fir[], const struct cir_buf_source *source, struct cir_buf_sink *sink, size_t frames, unsigned int channels) { - struct fir_state_32x16 *f; + struct fir_state_32x16 *filter; const int32_t *src = source->ptr; - int32_t *snk = sink->ptr; - const int32_t *x0; - int32_t *y0; - const int32_t *x1; - int32_t *y1; - size_t i; + int32_t *dst = sink->ptr; + const int32_t *src_first; + int32_t *dst_first; + const int32_t *src_second; + int32_t *dst_second; + size_t pair_index; size_t remaining_frames = frames; - unsigned int ch; - unsigned int nch = channels; + unsigned int channel; + unsigned int channel_count = channels; int rshift; int lshift; while (remaining_frames) { - size_t source_frames = cir_buf_samples_without_wrap_s32(src, source->buf_end) / nch; - size_t sink_frames = cir_buf_samples_without_wrap_s32(snk, sink->buf_end) / nch; + size_t source_frames = + cir_buf_samples_without_wrap_s32(src, source->buf_end) / channel_count; + size_t sink_frames = + cir_buf_samples_without_wrap_s32(dst, sink->buf_end) / channel_count; size_t chunk_frames = MIN(remaining_frames, MIN(source_frames, sink_frames)); chunk_frames &= ~0x1; if (!chunk_frames) { - for (ch = 0; ch < nch; ch++) { - f = &fir[ch]; - fir_get_lrshifts(f, &lshift, &rshift); - fir_hifiep_setup_circular(f); - y0 = snk + ch; - fir_32x16(f, src[ch], y0, lshift, rshift); + for (channel = 0; channel < channel_count; channel++) { + filter = &fir[channel]; + fir_get_lrshifts(filter, &lshift, &rshift); + fir_hifiep_setup_circular(filter); + dst_first = dst + channel; + fir_32x16(filter, src[channel], dst_first, lshift, rshift); } - src = source_cir_buf_wrap(src + nch, source->buf_start, source->buf_end); - snk = cir_buf_wrap(snk + nch, sink->buf_start, sink->buf_end); + src = source_cir_buf_wrap(src + channel_count, source->buf_start, + source->buf_end); + dst = cir_buf_wrap(dst + channel_count, sink->buf_start, sink->buf_end); remaining_frames--; continue; } - for (ch = 0; ch < nch; ch++) { + for (channel = 0; channel < channel_count; channel++) { /* Get FIR instance and get shifts to e.g. apply mute * without overhead. */ - f = &fir[ch]; - fir_get_lrshifts(f, &lshift, &rshift); + filter = &fir[channel]; + fir_get_lrshifts(filter, &lshift, &rshift); /* Setup circular buffer for FIR input data delay */ - fir_hifiep_setup_circular(f); - - x0 = src + ch; - y0 = snk + ch; - for (i = 0; i < (chunk_frames >> 1); i++) { - x1 = x0 + nch; - y1 = y0 + nch; - fir_32x16_2x(f, *x0, *x1, y0, y1, lshift, rshift); - x0 += 2 * nch; - y0 += 2 * nch; + fir_hifiep_setup_circular(filter); + + src_first = src + channel; + dst_first = dst + channel; + for (pair_index = 0; pair_index < (chunk_frames >> 1); pair_index++) { + src_second = src_first + channel_count; + dst_second = dst_first + channel_count; + fir_32x16_2x(filter, *src_first, *src_second, dst_first, dst_second, + lshift, rshift); + src_first += 2 * channel_count; + dst_first += 2 * channel_count; } } - src = source_cir_buf_wrap(src + chunk_frames * nch, - source->buf_start, source->buf_end); - snk = cir_buf_wrap(snk + chunk_frames * nch, - sink->buf_start, sink->buf_end); + src = source_cir_buf_wrap(src + chunk_frames * channel_count, source->buf_start, + source->buf_end); + dst = cir_buf_wrap(dst + chunk_frames * channel_count, sink->buf_start, + sink->buf_end); remaining_frames -= chunk_frames; } } @@ -98,71 +102,77 @@ void eq_fir_2x_s32(struct fir_state_32x16 fir[], const struct cir_buf_source *so void eq_fir_2x_s24(struct fir_state_32x16 fir[], const struct cir_buf_source *source, struct cir_buf_sink *sink, size_t frames, unsigned int channels) { - struct fir_state_32x16 *f; + struct fir_state_32x16 *filter; const int32_t *src = source->ptr; - int32_t *snk = sink->ptr; - const int32_t *x0; - int32_t *y0; - const int32_t *x1; - int32_t *y1; - int32_t z0; - int32_t z1; - size_t i; + int32_t *dst = sink->ptr; + const int32_t *src_first; + int32_t *dst_first; + const int32_t *src_second; + int32_t *dst_second; + int32_t filtered_sample_0; + int32_t filtered_sample_1; + size_t pair_index; size_t remaining_frames = frames; - unsigned int ch; - unsigned int nch = channels; + unsigned int channel; + unsigned int channel_count = channels; int rshift; int lshift; while (remaining_frames) { - size_t source_frames = cir_buf_samples_without_wrap_s32(src, source->buf_end) / nch; - size_t sink_frames = cir_buf_samples_without_wrap_s32(snk, sink->buf_end) / nch; + size_t source_frames = + cir_buf_samples_without_wrap_s32(src, source->buf_end) / channel_count; + size_t sink_frames = + cir_buf_samples_without_wrap_s32(dst, sink->buf_end) / channel_count; size_t chunk_frames = MIN(remaining_frames, MIN(source_frames, sink_frames)); chunk_frames &= ~0x1; if (!chunk_frames) { - for (ch = 0; ch < nch; ch++) { - int32_t z; - - f = &fir[ch]; - fir_get_lrshifts(f, &lshift, &rshift); - fir_hifiep_setup_circular(f); - fir_32x16(f, src[ch] << 8, &z, lshift, rshift); - snk[ch] = sat_int24(Q_SHIFT_RND(z, 31, 23)); + for (channel = 0; channel < channel_count; channel++) { + int32_t filtered_sample; + + filter = &fir[channel]; + fir_get_lrshifts(filter, &lshift, &rshift); + fir_hifiep_setup_circular(filter); + fir_32x16(filter, src[channel] << 8, &filtered_sample, lshift, + rshift); + dst[channel] = sat_int24(Q_SHIFT_RND(filtered_sample, 31, 23)); } - src = source_cir_buf_wrap(src + nch, source->buf_start, source->buf_end); - snk = cir_buf_wrap(snk + nch, sink->buf_start, sink->buf_end); + src = source_cir_buf_wrap(src + channel_count, source->buf_start, + source->buf_end); + dst = cir_buf_wrap(dst + channel_count, sink->buf_start, sink->buf_end); remaining_frames--; continue; } - for (ch = 0; ch < nch; ch++) { + for (channel = 0; channel < channel_count; channel++) { /* Get FIR instance and get shifts to e.g. apply mute * without overhead. */ - f = &fir[ch]; - fir_get_lrshifts(f, &lshift, &rshift); + filter = &fir[channel]; + fir_get_lrshifts(filter, &lshift, &rshift); /* Setup circular buffer for FIR input data delay */ - fir_hifiep_setup_circular(f); - - x0 = src + ch; - y0 = snk + ch; - for (i = 0; i < (chunk_frames >> 1); i++) { - x1 = x0 + nch; - y1 = y0 + nch; - fir_32x16_2x(f, *x0 << 8, *x1 << 8, &z0, &z1, lshift, rshift); - *y0 = sat_int24(Q_SHIFT_RND(z0, 31, 23)); - *y1 = sat_int24(Q_SHIFT_RND(z1, 31, 23)); - x0 += 2 * nch; - y0 += 2 * nch; + fir_hifiep_setup_circular(filter); + + src_first = src + channel; + dst_first = dst + channel; + for (pair_index = 0; pair_index < (chunk_frames >> 1); pair_index++) { + src_second = src_first + channel_count; + dst_second = dst_first + channel_count; + fir_32x16_2x(filter, *src_first << 8, *src_second << 8, + &filtered_sample_0, &filtered_sample_1, lshift, + rshift); + *dst_first = sat_int24(Q_SHIFT_RND(filtered_sample_0, 31, 23)); + *dst_second = sat_int24(Q_SHIFT_RND(filtered_sample_1, 31, 23)); + src_first += 2 * channel_count; + dst_first += 2 * channel_count; } } - src = source_cir_buf_wrap(src + chunk_frames * nch, - source->buf_start, source->buf_end); - snk = cir_buf_wrap(snk + chunk_frames * nch, - sink->buf_start, sink->buf_end); + src = source_cir_buf_wrap(src + chunk_frames * channel_count, source->buf_start, + source->buf_end); + dst = cir_buf_wrap(dst + chunk_frames * channel_count, sink->buf_start, + sink->buf_end); remaining_frames -= chunk_frames; } } @@ -172,71 +182,77 @@ void eq_fir_2x_s24(struct fir_state_32x16 fir[], const struct cir_buf_source *so void eq_fir_2x_s16(struct fir_state_32x16 fir[], const struct cir_buf_source *source, struct cir_buf_sink *sink, size_t frames, unsigned int channels) { - struct fir_state_32x16 *f; + struct fir_state_32x16 *filter; const int16_t *src = source->ptr; - int16_t *snk = sink->ptr; - const int16_t *x0; - int16_t *y0; - const int16_t *x1; - int16_t *y1; - int32_t z0; - int32_t z1; - size_t i; + int16_t *dst = sink->ptr; + const int16_t *src_first; + int16_t *dst_first; + const int16_t *src_second; + int16_t *dst_second; + int32_t filtered_sample_0; + int32_t filtered_sample_1; + size_t pair_index; size_t remaining_frames = frames; - unsigned int ch; - unsigned int nch = channels; + unsigned int channel; + unsigned int channel_count = channels; int rshift; int lshift; while (remaining_frames) { - size_t source_frames = cir_buf_samples_without_wrap_s16(src, source->buf_end) / nch; - size_t sink_frames = cir_buf_samples_without_wrap_s16(snk, sink->buf_end) / nch; + size_t source_frames = + cir_buf_samples_without_wrap_s16(src, source->buf_end) / channel_count; + size_t sink_frames = + cir_buf_samples_without_wrap_s16(dst, sink->buf_end) / channel_count; size_t chunk_frames = MIN(remaining_frames, MIN(source_frames, sink_frames)); chunk_frames &= ~0x1; if (!chunk_frames) { - for (ch = 0; ch < nch; ch++) { - int32_t z; - - f = &fir[ch]; - fir_get_lrshifts(f, &lshift, &rshift); - fir_hifiep_setup_circular(f); - fir_32x16(f, src[ch] << 16, &z, lshift, rshift); - snk[ch] = sat_int16(Q_SHIFT_RND(z, 31, 15)); + for (channel = 0; channel < channel_count; channel++) { + int32_t filtered_sample; + + filter = &fir[channel]; + fir_get_lrshifts(filter, &lshift, &rshift); + fir_hifiep_setup_circular(filter); + fir_32x16(filter, src[channel] << 16, &filtered_sample, lshift, + rshift); + dst[channel] = sat_int16(Q_SHIFT_RND(filtered_sample, 31, 15)); } - src = source_cir_buf_wrap(src + nch, source->buf_start, source->buf_end); - snk = cir_buf_wrap(snk + nch, sink->buf_start, sink->buf_end); + src = source_cir_buf_wrap(src + channel_count, source->buf_start, + source->buf_end); + dst = cir_buf_wrap(dst + channel_count, sink->buf_start, sink->buf_end); remaining_frames--; continue; } - for (ch = 0; ch < nch; ch++) { + for (channel = 0; channel < channel_count; channel++) { /* Get FIR instance and get shifts to e.g. apply mute * without overhead. */ - f = &fir[ch]; - fir_get_lrshifts(f, &lshift, &rshift); + filter = &fir[channel]; + fir_get_lrshifts(filter, &lshift, &rshift); /* Setup circular buffer for FIR input data delay */ - fir_hifiep_setup_circular(f); - - x0 = src + ch; - y0 = snk + ch; - for (i = 0; i < (chunk_frames >> 1); i++) { - x1 = x0 + nch; - y1 = y0 + nch; - fir_32x16_2x(f, *x0 << 16, *x1 << 16, &z0, &z1, lshift, rshift); - *y0 = sat_int16(Q_SHIFT_RND(z0, 31, 15)); - *y1 = sat_int16(Q_SHIFT_RND(z1, 31, 15)); - x0 += 2 * nch; - y0 += 2 * nch; + fir_hifiep_setup_circular(filter); + + src_first = src + channel; + dst_first = dst + channel; + for (pair_index = 0; pair_index < (chunk_frames >> 1); pair_index++) { + src_second = src_first + channel_count; + dst_second = dst_first + channel_count; + fir_32x16_2x(filter, *src_first << 16, *src_second << 16, + &filtered_sample_0, &filtered_sample_1, lshift, + rshift); + *dst_first = sat_int16(Q_SHIFT_RND(filtered_sample_0, 31, 15)); + *dst_second = sat_int16(Q_SHIFT_RND(filtered_sample_1, 31, 15)); + src_first += 2 * channel_count; + dst_first += 2 * channel_count; } } - src = source_cir_buf_wrap(src + chunk_frames * nch, - source->buf_start, source->buf_end); - snk = cir_buf_wrap(snk + chunk_frames * nch, - sink->buf_start, sink->buf_end); + src = source_cir_buf_wrap(src + chunk_frames * channel_count, source->buf_start, + source->buf_end); + dst = cir_buf_wrap(dst + chunk_frames * channel_count, sink->buf_start, + sink->buf_end); remaining_frames -= chunk_frames; } } diff --git a/src/audio/eq_fir/eq_fir_hifi3.c b/src/audio/eq_fir/eq_fir_hifi3.c index 434741971250..51fb112ba3c1 100644 --- a/src/audio/eq_fir/eq_fir_hifi3.c +++ b/src/audio/eq_fir/eq_fir_hifi3.c @@ -29,68 +29,71 @@ LOG_MODULE_DECLARE(eq_fir, CONFIG_SOF_LOG_LEVEL); void eq_fir_2x_s32(struct fir_state_32x16 fir[], const struct cir_buf_source *source, struct cir_buf_sink *sink, size_t frames, unsigned int channels) { - struct fir_state_32x16 *f; + struct fir_state_32x16 *filter; ae_int32x2 d0 = 0; ae_int32x2 d1 = 0; const ae_int32 *src = source->ptr; ae_int32 *dst = (ae_int32 *)sink->ptr; - const ae_int32 *x; - ae_int32 *y0; - ae_int32 *y1; - size_t i; - size_t inc_nch_s = channels * sizeof(int32_t); - size_t inc_2nch_s = 2 * inc_nch_s; + const ae_int32 *src_channel; + ae_int32 *dst_first; + ae_int32 *dst_second; + size_t pair_index; + size_t channel_stride_bytes = channels * sizeof(int32_t); + size_t pair_stride_bytes = 2 * channel_stride_bytes; size_t remaining_frames = frames; - unsigned int ch; - unsigned int nch = channels; + unsigned int channel; + unsigned int channel_count = channels; int rshift; int lshift; int shift; while (remaining_frames) { - size_t source_frames = cir_buf_samples_without_wrap_s32(src, source->buf_end) / nch; - size_t sink_frames = cir_buf_samples_without_wrap_s32(dst, sink->buf_end) / nch; + size_t source_frames = + cir_buf_samples_without_wrap_s32(src, source->buf_end) / channel_count; + size_t sink_frames = + cir_buf_samples_without_wrap_s32(dst, sink->buf_end) / channel_count; size_t chunk_frames = MIN(remaining_frames, MIN(source_frames, sink_frames)); chunk_frames &= ~0x1; if (!chunk_frames) { - for (ch = 0; ch < nch; ch++) { - f = &fir[ch]; - fir_get_lrshifts(f, &lshift, &rshift); + for (channel = 0; channel < channel_count; channel++) { + filter = &fir[channel]; + fir_get_lrshifts(filter, &lshift, &rshift); shift = lshift - rshift; - fir_core_setup_circular(f); - fir_32x16(f, src[ch], dst + ch, shift); + fir_core_setup_circular(filter); + fir_32x16(filter, src[channel], dst + channel, shift); } - src = source_cir_buf_wrap(src + nch, source->buf_start, source->buf_end); - dst = cir_buf_wrap(dst + nch, sink->buf_start, sink->buf_end); + src = source_cir_buf_wrap(src + channel_count, source->buf_start, + source->buf_end); + dst = cir_buf_wrap(dst + channel_count, sink->buf_start, sink->buf_end); remaining_frames--; continue; } - for (ch = 0; ch < nch; ch++) { + for (channel = 0; channel < channel_count; channel++) { /* Get FIR instance and get shifts.*/ - f = &fir[ch]; - fir_get_lrshifts(f, &lshift, &rshift); + filter = &fir[channel]; + fir_get_lrshifts(filter, &lshift, &rshift); shift = lshift - rshift; - /* set f->delay as circular buffer */ - fir_core_setup_circular(f); - - x = src + ch; - y0 = dst + ch; - y1 = y0 + nch; - - for (i = 0; i < (chunk_frames >> 1); i++) { - /* Load two input samples via input pointer x */ - AE_L32_XP(d0, x, inc_nch_s); - AE_L32_XP(d1, x, inc_nch_s); - fir_32x16_2x(f, d0, d1, y0, y1, shift); - AE_L32_XC(d0, y0, inc_2nch_s); - AE_L32_XC(d1, y1, inc_2nch_s); + /* Set filter->delay as circular buffer. */ + fir_core_setup_circular(filter); + + src_channel = src + channel; + dst_first = dst + channel; + dst_second = dst_first + channel_count; + + for (pair_index = 0; pair_index < (chunk_frames >> 1); pair_index++) { + /* Load two input samples via the channel source pointer. */ + AE_L32_XP(d0, src_channel, channel_stride_bytes); + AE_L32_XP(d1, src_channel, channel_stride_bytes); + fir_32x16_2x(filter, d0, d1, dst_first, dst_second, shift); + AE_L32_XC(d0, dst_first, pair_stride_bytes); + AE_L32_XC(d1, dst_second, pair_stride_bytes); } } - dst = cir_buf_wrap(dst + chunk_frames * nch, - sink->buf_start, sink->buf_end); - src = source_cir_buf_wrap(src + chunk_frames * nch, source->buf_start, + dst = cir_buf_wrap(dst + chunk_frames * channel_count, sink->buf_start, + sink->buf_end); + src = source_cir_buf_wrap(src + chunk_frames * channel_count, source->buf_start, source->buf_end); remaining_frames -= chunk_frames; } @@ -101,89 +104,93 @@ void eq_fir_2x_s32(struct fir_state_32x16 fir[], const struct cir_buf_source *so void eq_fir_2x_s24(struct fir_state_32x16 fir[], const struct cir_buf_source *source, struct cir_buf_sink *sink, size_t frames, unsigned int channels) { - struct fir_state_32x16 *f; + struct fir_state_32x16 *filter; ae_int32x2 d0 = 0; ae_int32x2 d1 = 0; - ae_int32 z0; - ae_int32 z1; + ae_int32 filtered_sample_0; + ae_int32 filtered_sample_1; const ae_int32 *src = source->ptr; ae_int32 *dst = (ae_int32 *)sink->ptr; - const ae_int32 *x; - ae_int32 *y; - size_t i; - size_t inc_nch_s = channels * sizeof(int32_t); + const ae_int32 *src_channel; + ae_int32 *dst_channel; + size_t pair_index; + size_t channel_stride_bytes = channels * sizeof(int32_t); size_t remaining_frames = frames; - unsigned int ch; - unsigned int nch = channels; + unsigned int channel; + unsigned int channel_count = channels; int rshift; int lshift; int shift; while (remaining_frames) { - size_t source_frames = cir_buf_samples_without_wrap_s32(src, source->buf_end) / nch; - size_t sink_frames = cir_buf_samples_without_wrap_s32(dst, sink->buf_end) / nch; + size_t source_frames = + cir_buf_samples_without_wrap_s32(src, source->buf_end) / channel_count; + size_t sink_frames = + cir_buf_samples_without_wrap_s32(dst, sink->buf_end) / channel_count; size_t chunk_frames = MIN(remaining_frames, MIN(source_frames, sink_frames)); chunk_frames &= ~0x1; if (!chunk_frames) { - for (ch = 0; ch < nch; ch++) { - ae_int32 input = src[ch] << 8; + for (channel = 0; channel < channel_count; channel++) { + ae_int32 input = src[channel] << 8; ae_int32 output; int32_t output_sample; - f = &fir[ch]; - fir_get_lrshifts(f, &lshift, &rshift); + filter = &fir[channel]; + fir_get_lrshifts(filter, &lshift, &rshift); shift = lshift - rshift; - fir_core_setup_circular(f); - fir_32x16(f, input, &output, shift); + fir_core_setup_circular(filter); + fir_32x16(filter, input, &output, shift); output_sample = AE_MOVAD32_L(output); - dst[ch] = sat_int24(Q_SHIFT_RND(output_sample, 31, 23)); + dst[channel] = sat_int24(Q_SHIFT_RND(output_sample, 31, 23)); } - src = source_cir_buf_wrap(src + nch, source->buf_start, source->buf_end); - dst = cir_buf_wrap(dst + nch, sink->buf_start, sink->buf_end); + src = source_cir_buf_wrap(src + channel_count, source->buf_start, + source->buf_end); + dst = cir_buf_wrap(dst + channel_count, sink->buf_start, sink->buf_end); remaining_frames--; continue; } - for (ch = 0; ch < nch; ch++) { + for (channel = 0; channel < channel_count; channel++) { /* Get FIR instance and get shifts.*/ - f = &fir[ch]; - fir_get_lrshifts(f, &lshift, &rshift); + filter = &fir[channel]; + fir_get_lrshifts(filter, &lshift, &rshift); shift = lshift - rshift; - /* set f->delay as circular buffer */ - fir_core_setup_circular(f); + /* Set filter->delay as circular buffer. */ + fir_core_setup_circular(filter); - x = src + ch; - y = dst + ch; + src_channel = src + channel; + dst_channel = dst + channel; - for (i = 0; i < (chunk_frames >> 1); i++) { - /* Load two input samples via input pointer x */ - AE_L32_XP(d0, x, inc_nch_s); - AE_L32_XP(d1, x, inc_nch_s); + for (pair_index = 0; pair_index < (chunk_frames >> 1); pair_index++) { + /* Load two input samples via the channel source pointer. */ + AE_L32_XP(d0, src_channel, channel_stride_bytes); + AE_L32_XP(d1, src_channel, channel_stride_bytes); /* Convert Q1.23 to Q1.31 compatible format */ d0 = AE_SLAA32(d0, 8); d1 = AE_SLAA32(d1, 8); - fir_32x16_2x(f, d0, d1, &z0, &z1, shift); + fir_32x16_2x(filter, d0, d1, &filtered_sample_0, &filtered_sample_1, + shift); /* Shift and round to Q1.23 format */ - d0 = AE_SRAI32R(z0, 8); + d0 = AE_SRAI32R(filtered_sample_0, 8); d0 = AE_SLAI32S(d0, 8); d0 = AE_SRAI32(d0, 8); - d1 = AE_SRAI32R(z1, 8); + d1 = AE_SRAI32R(filtered_sample_1, 8); d1 = AE_SLAI32S(d1, 8); d1 = AE_SRAI32(d1, 8); /* Store output and update output pointers */ - AE_S32_L_XC(d0, y, inc_nch_s); - AE_S32_L_XC(d1, y, inc_nch_s); + AE_S32_L_XC(d0, dst_channel, channel_stride_bytes); + AE_S32_L_XC(d1, dst_channel, channel_stride_bytes); } } - dst = cir_buf_wrap(dst + chunk_frames * nch, - sink->buf_start, sink->buf_end); - src = source_cir_buf_wrap(src + chunk_frames * nch, source->buf_start, + dst = cir_buf_wrap(dst + chunk_frames * channel_count, sink->buf_start, + sink->buf_end); + src = source_cir_buf_wrap(src + chunk_frames * channel_count, source->buf_start, source->buf_end); remaining_frames -= chunk_frames; } @@ -194,86 +201,90 @@ void eq_fir_2x_s24(struct fir_state_32x16 fir[], const struct cir_buf_source *so void eq_fir_2x_s16(struct fir_state_32x16 fir[], const struct cir_buf_source *source, struct cir_buf_sink *sink, size_t frames, unsigned int channels) { - struct fir_state_32x16 *f; + struct fir_state_32x16 *filter; ae_int16x4 d0 = AE_ZERO16(); ae_int16x4 d1 = AE_ZERO16(); - ae_int32 z0; - ae_int32 z1; - ae_int32 x0; - ae_int32 x1; + ae_int32 filtered_sample_0; + ae_int32 filtered_sample_1; + ae_int32 input_sample_0; + ae_int32 input_sample_1; const ae_int16 *src = source->ptr; ae_int16 *dst = (ae_int16 *)sink->ptr; - const ae_int16 *x; - ae_int16 *y; - size_t i; - size_t inc_nch_s = channels * sizeof(int16_t); + const ae_int16 *src_channel; + ae_int16 *dst_channel; + size_t pair_index; + size_t channel_stride_bytes = channels * sizeof(int16_t); size_t remaining_frames = frames; - unsigned int ch; - unsigned int nch = channels; + unsigned int channel; + unsigned int channel_count = channels; int rshift; int lshift; int shift; while (remaining_frames) { - size_t source_frames = cir_buf_samples_without_wrap_s16(src, source->buf_end) / nch; - size_t sink_frames = cir_buf_samples_without_wrap_s16(dst, sink->buf_end) / nch; + size_t source_frames = + cir_buf_samples_without_wrap_s16(src, source->buf_end) / channel_count; + size_t sink_frames = + cir_buf_samples_without_wrap_s16(dst, sink->buf_end) / channel_count; size_t chunk_frames = MIN(remaining_frames, MIN(source_frames, sink_frames)); chunk_frames &= ~0x1; if (!chunk_frames) { - for (ch = 0; ch < nch; ch++) { - ae_int32 input = ((const int16_t *)src)[ch] << 16; + for (channel = 0; channel < channel_count; channel++) { + ae_int32 input = ((const int16_t *)src)[channel] << 16; ae_int32 output; int32_t output_sample; - f = &fir[ch]; - fir_get_lrshifts(f, &lshift, &rshift); + filter = &fir[channel]; + fir_get_lrshifts(filter, &lshift, &rshift); shift = lshift - rshift; - fir_core_setup_circular(f); - fir_32x16(f, input, &output, shift); + fir_core_setup_circular(filter); + fir_32x16(filter, input, &output, shift); output_sample = AE_MOVAD32_L(output); - dst[ch] = sat_int16(Q_SHIFT_RND(output_sample, 31, 15)); + dst[channel] = sat_int16(Q_SHIFT_RND(output_sample, 31, 15)); } - src = source_cir_buf_wrap(src + nch, source->buf_start, source->buf_end); - dst = cir_buf_wrap(dst + nch, sink->buf_start, sink->buf_end); + src = source_cir_buf_wrap(src + channel_count, source->buf_start, + source->buf_end); + dst = cir_buf_wrap(dst + channel_count, sink->buf_start, sink->buf_end); remaining_frames--; continue; } - for (ch = 0; ch < nch; ch++) { + for (channel = 0; channel < channel_count; channel++) { /* Get FIR instance and get shifts.*/ - f = &fir[ch]; - fir_get_lrshifts(f, &lshift, &rshift); + filter = &fir[channel]; + fir_get_lrshifts(filter, &lshift, &rshift); shift = lshift - rshift; - /* set f->delay as circular buffer */ - fir_core_setup_circular(f); + /* Set filter->delay as circular buffer. */ + fir_core_setup_circular(filter); - x = src + ch; - y = dst + ch; + src_channel = src + channel; + dst_channel = dst + channel; - for (i = 0; i < (chunk_frames >> 1); i++) { - /* Load two input samples via input pointer x */ - AE_L16_XP(d0, x, inc_nch_s); - AE_L16_XP(d1, x, inc_nch_s); + for (pair_index = 0; pair_index < (chunk_frames >> 1); pair_index++) { + /* Load two input samples via the channel source pointer. */ + AE_L16_XP(d0, src_channel, channel_stride_bytes); + AE_L16_XP(d1, src_channel, channel_stride_bytes); /* Convert Q1.15 to Q1.31 compatible format */ - x0 = AE_CVT32X2F16_32(d0); - x1 = AE_CVT32X2F16_32(d1); + input_sample_0 = AE_CVT32X2F16_32(d0); + input_sample_1 = AE_CVT32X2F16_32(d1); - fir_32x16_2x(f, x0, x1, &z0, &z1, shift); + fir_32x16_2x(filter, input_sample_0, input_sample_1, + &filtered_sample_0, &filtered_sample_1, shift); /* Round to Q1.15 format */ - d0 = AE_ROUND16X4F32SSYM(z0, z0); - d1 = AE_ROUND16X4F32SSYM(z1, z1); + d0 = AE_ROUND16X4F32SSYM(filtered_sample_0, filtered_sample_0); + d1 = AE_ROUND16X4F32SSYM(filtered_sample_1, filtered_sample_1); /* Store output and update output pointers */ - AE_S16_0_XC(d0, y, inc_nch_s); - AE_S16_0_XC(d1, y, inc_nch_s); + AE_S16_0_XC(d0, dst_channel, channel_stride_bytes); + AE_S16_0_XC(d1, dst_channel, channel_stride_bytes); } } - dst = cir_buf_wrap(dst + chunk_frames * nch, - sink->buf_start, sink->buf_end); - src = source_cir_buf_wrap(src + chunk_frames * nch, source->buf_start, + dst = cir_buf_wrap(dst + chunk_frames * channel_count, sink->buf_start, + sink->buf_end); + src = source_cir_buf_wrap(src + chunk_frames * channel_count, source->buf_start, source->buf_end); remaining_frames -= chunk_frames; }