From 489a99da1be2928b1656dcaf9a8fb7f498304dfc Mon Sep 17 00:00:00 2001 From: Piotr Hoppe Date: Mon, 21 Sep 2026 13:02:57 +0200 Subject: [PATCH 1/2] 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 --- src/audio/eq_fir/eq_fir.c | 127 ++++++--- src/audio/eq_fir/eq_fir.h | 40 +-- src/audio/eq_fir/eq_fir_generic.c | 96 +++---- src/audio/eq_fir/eq_fir_hifi2ep.c | 261 ++++++++++++------ src/audio/eq_fir/eq_fir_hifi3.c | 164 +++++++---- test/cmocka/src/audio/eq_fir/eq_fir_process.c | 239 +++++++++++++++- 6 files changed, 659 insertions(+), 268 deletions(-) diff --git a/src/audio/eq_fir/eq_fir.c b/src/audio/eq_fir/eq_fir.c index 6c4c3d630562..de9451496854 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,66 @@ 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; + if (buffer_size < source_bytes) { + source_release_data(source, 0); + return -ENOSPC; + } + 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; + } + if (buffer_size < sink_bytes) { + source_release_data(source, 0); + sink_commit_buffer(sink, 0); + return -ENOSPC; + } + 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,8 +477,9 @@ 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; + struct sof_source *source; + struct sof_sink *sink; int channels; enum sof_ipc_frame frame_fmt; int ret = 0; @@ -450,24 +487,34 @@ static int eq_fir_prepare(struct processing_module *mod, 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) { + if (num_of_sources != 1 || num_of_sinks != 1) { comp_err(dev, "no source or sink buffer"); 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 +561,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..2df8434c6dee 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); + struct cir_buf_source *source, + struct cir_buf_sink *sink, + int frames, 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, struct cir_buf_source *source, + struct cir_buf_sink *sink, int frames, 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, struct cir_buf_source *source, + struct cir_buf_sink *sink, int frames, 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, struct cir_buf_source *source, + struct cir_buf_sink *sink, int frames, 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, struct cir_buf_source *source, + struct cir_buf_sink *sink, int frames, 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, struct cir_buf_source *source, + struct cir_buf_sink *sink, int frames, 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, struct cir_buf_source *source, + struct cir_buf_sink *sink, int frames, 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..f898e612ca8a 100644 --- a/src/audio/eq_fir/eq_fir_generic.c +++ b/src/audio/eq_fir/eq_fir_generic.c @@ -22,113 +22,107 @@ 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[], struct cir_buf_source *source, + struct cir_buf_sink *sink, int frames, 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); + const int16_t *x0; + int16_t *y0; + const int16_t *x = source->ptr; + int16_t *y = sink->ptr; int nmax, n, i, j; - int nch = audio_stream_get_channels(source); - int remaining_samples = frames * nch; + int remaining_samples = frames * channels; 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[], struct cir_buf_source *source, + struct cir_buf_sink *sink, int frames, 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); + const int32_t *x0; + int32_t *y0; + const int32_t *x = source->ptr; + int32_t *y = sink->ptr; int nmax, n, i, j; - int nch = audio_stream_get_channels(source); - int remaining_samples = frames * nch; + int remaining_samples = frames * channels; 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[], struct cir_buf_source *source, + struct cir_buf_sink *sink, int frames, 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); + const int32_t *x0; + int32_t *y0; + const int32_t *x = source->ptr; + int32_t *y = sink->ptr; int nmax, n, i, j; - int nch = audio_stream_get_channels(source); - int remaining_samples = frames * nch; + int remaining_samples = frames * channels; 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..2a5eaaecdb62 100644 --- a/src/audio/eq_fir/eq_fir_hifi2ep.c +++ b/src/audio/eq_fir/eq_fir_hifi2ep.c @@ -27,60 +27,83 @@ 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[], struct cir_buf_source *source, + struct cir_buf_sink *sink, int frames, 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; 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; + int nch = channels; + int remaining_frames = frames; + + while (remaining_frames) { + int source_frames = cir_buf_samples_without_wrap_s32(src, source->buf_end) / nch; + int sink_frames = cir_buf_samples_without_wrap_s32(snk, sink->buf_end) / nch; + int 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[], struct cir_buf_source *source, + struct cir_buf_sink *sink, int frames, 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; @@ -88,46 +111,73 @@ void eq_fir_2x_s24(struct fir_state_32x16 fir[], struct input_stream_buffer *bso int i; 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; + int nch = channels; + int remaining_frames = frames; + + while (remaining_frames) { + int source_frames = cir_buf_samples_without_wrap_s32(src, source->buf_end) / nch; + int sink_frames = cir_buf_samples_without_wrap_s32(snk, sink->buf_end) / nch; + int 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[], struct cir_buf_source *source, + struct cir_buf_sink *sink, int frames, 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; @@ -135,30 +185,59 @@ void eq_fir_2x_s16(struct fir_state_32x16 fir[], struct input_stream_buffer *bso int i; 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; + int nch = channels; + int remaining_frames = frames; + + while (remaining_frames) { + int source_frames = cir_buf_samples_without_wrap_s16(src, source->buf_end) / nch; + int sink_frames = cir_buf_samples_without_wrap_s16(snk, sink->buf_end) / nch; + int 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..7af96bae384a 100644 --- a/src/audio/eq_fir/eq_fir_hifi3.c +++ b/src/audio/eq_fir/eq_fir_hifi3.c @@ -26,34 +26,48 @@ 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[], struct cir_buf_source *source, + struct cir_buf_sink *sink, int frames, 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 *src = (ae_int32 *)source->ptr; + ae_int32 *dst = (ae_int32 *)sink->ptr; ae_int32 *x; ae_int32 *y0; ae_int32 *y1; int ch; - int i, n, nmax; + int i; int rshift; int lshift; int shift; - int nch = audio_stream_get_channels(source); + int nch = channels; int inc_nch_s = nch * sizeof(int32_t); int inc_2nch_s = 2 * inc_nch_s; - int samples = nch * frames; + int remaining_frames = frames; + + while (remaining_frames) { + int source_frames = cir_buf_samples_without_wrap_s32(src, source->buf_end) / nch; + int sink_frames = cir_buf_samples_without_wrap_s32(dst, sink->buf_end) / nch; + int 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 = (ae_int32 *)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; + } - 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); for (ch = 0; ch < nch; ch++) { /* Get FIR instance and get shifts.*/ f = &fir[ch]; @@ -66,7 +80,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 +89,62 @@ 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 = (ae_int32 *)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[], struct cir_buf_source *source, + struct cir_buf_sink *sink, int frames, 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 *src = (ae_int32 *)source->ptr; + ae_int32 *dst = (ae_int32 *)sink->ptr; ae_int32 *x; ae_int32 *y; int ch; - int i, n, nmax; + int i; int rshift; int lshift; int shift; - int nch = audio_stream_get_channels(source); + int nch = channels; int inc_nch_s = nch * sizeof(int32_t); - int samples = nch * frames; + int remaining_frames = frames; + + while (remaining_frames) { + int source_frames = cir_buf_samples_without_wrap_s32(src, source->buf_end) / nch; + int sink_frames = cir_buf_samples_without_wrap_s32(dst, sink->buf_end) / nch; + int 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; + + f = &fir[ch]; + fir_get_lrshifts(f, &lshift, &rshift); + shift = lshift - rshift; + fir_core_setup_circular(f); + fir_32x16(f, input, &output, shift); + dst[ch] = sat_int24(Q_SHIFT_RND(output, 31, 23)); + } + src = (ae_int32 *)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; + } - 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); 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 = (ae_int32 *)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[], struct cir_buf_source *source, + struct cir_buf_sink *sink, int frames, 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,44 @@ 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 *src = (ae_int16 *)source->ptr; + ae_int16 *dst = (ae_int16 *)sink->ptr; ae_int16 *x; ae_int16 *y; int ch; - int i, n, nmax; + int i; int rshift; int lshift; int shift; - int nch = audio_stream_get_channels(source); + int nch = channels; int inc_nch_s = nch * sizeof(int16_t); - int samples = nch * frames; + int remaining_frames = frames; + + while (remaining_frames) { + int source_frames = cir_buf_samples_without_wrap_s16(src, source->buf_end) / nch; + int sink_frames = cir_buf_samples_without_wrap_s16(dst, sink->buf_end) / nch; + int chunk_frames = MIN(remaining_frames, MIN(source_frames, sink_frames)); + + chunk_frames &= ~0x1; + if (!chunk_frames) { + for (ch = 0; ch < nch; ch++) { + int32_t input = src[ch] << 16; + int32_t output; + + f = &fir[ch]; + fir_get_lrshifts(f, &lshift, &rshift); + shift = lshift - rshift; + fir_core_setup_circular(f); + fir_32x16(f, input, &output, shift); + dst[ch] = sat_int16(Q_SHIFT_RND(output, 31, 15)); + } + src = (ae_int16 *)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; + } - 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); for (ch = 0; ch < nch; ch++) { /* Get FIR instance and get shifts.*/ f = &fir[ch]; @@ -196,7 +250,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 +270,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 = (ae_int16 *)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..36f546d425b6 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" @@ -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; @@ -423,6 +450,160 @@ static void verify_sink_s32(struct test_data *td) } #endif /* CONFIG_FORMAT_S32LE */ +static void fill_source_for_test(struct test_data *td, int 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; + + 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); + + 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); + + 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); + + 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); + + 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); + + 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); + + 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); + + 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); + + 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); + + 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); + + 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 int frames_jitter(int frames) { int r = rand(); @@ -442,9 +623,15 @@ 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; 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); switch (audio_stream_get_frm_fmt(&source->stream)) { @@ -464,15 +651,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 +698,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 +712,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 527602c04afd4a3e3a4e38518c55addd75383130 Mon Sep 17 00:00:00 2001 From: Piotr Hoppe Date: Mon, 21 Sep 2026 14:09:38 +0200 Subject: [PATCH 2/2] 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 --- src/audio/eq_fir/eq_fir_generic.c | 140 ++++++++-------- src/audio/eq_fir/eq_fir_hifi2ep.c | 256 ++++++++++++++++-------------- src/audio/eq_fir/eq_fir_hifi3.c | 254 +++++++++++++++-------------- 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 f898e612ca8a..cd842c655e13 100644 --- a/src/audio/eq_fir/eq_fir_generic.c +++ b/src/audio/eq_fir/eq_fir_generic.c @@ -26,33 +26,37 @@ void eq_fir_s16(struct fir_state_32x16 fir[], struct cir_buf_source *source, struct cir_buf_sink *sink, int frames, 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; - int nmax, n, i, j; + int32_t filtered_sample; + const int16_t *src_channel; + int16_t *dst_channel; + const int16_t *src = source->ptr; + int16_t *dst = sink->ptr; + int max_samples; + int chunk_samples; + int sample_index; + int channel; int remaining_samples = frames * channels; 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 */ @@ -62,33 +66,37 @@ void eq_fir_s24(struct fir_state_32x16 fir[], struct cir_buf_source *source, struct cir_buf_sink *sink, int frames, 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; - int nmax, n, i, j; + int32_t filtered_sample; + const int32_t *src_channel; + int32_t *dst_channel; + const int32_t *src = source->ptr; + int32_t *dst = sink->ptr; + int max_samples; + int chunk_samples; + int sample_index; + int channel; int remaining_samples = frames * channels; 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 */ @@ -98,31 +106,35 @@ void eq_fir_s32(struct fir_state_32x16 fir[], struct cir_buf_source *source, struct cir_buf_sink *sink, int frames, 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; - int nmax, n, i, j; + const int32_t *src_channel; + int32_t *dst_channel; + const int32_t *src = source->ptr; + int32_t *dst = sink->ptr; + int max_samples; + int chunk_samples; + int sample_index; + int channel; int remaining_samples = frames * channels; 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 2a5eaaecdb62..4c8c3a94c56e 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[], struct cir_buf_source *source, struct cir_buf_sink *sink, int frames, 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; - int ch; - int i; + int32_t *dst = sink->ptr; + const int32_t *src_first; + int32_t *dst_first; + const int32_t *src_second; + int32_t *dst_second; + int channel; + int pair_index; int rshift; int lshift; - int nch = channels; + int channel_count = channels; int remaining_frames = frames; while (remaining_frames) { - int source_frames = cir_buf_samples_without_wrap_s32(src, source->buf_end) / nch; - int sink_frames = cir_buf_samples_without_wrap_s32(snk, sink->buf_end) / nch; + int source_frames = + cir_buf_samples_without_wrap_s32(src, source->buf_end) / channel_count; + int sink_frames = + cir_buf_samples_without_wrap_s32(dst, sink->buf_end) / channel_count; int 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[], struct cir_buf_source *source, void eq_fir_2x_s24(struct fir_state_32x16 fir[], struct cir_buf_source *source, struct cir_buf_sink *sink, int frames, 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; - int ch; - int 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; + int channel; + int pair_index; int rshift; int lshift; - int nch = channels; + int channel_count = channels; int remaining_frames = frames; while (remaining_frames) { - int source_frames = cir_buf_samples_without_wrap_s32(src, source->buf_end) / nch; - int sink_frames = cir_buf_samples_without_wrap_s32(snk, sink->buf_end) / nch; + int source_frames = + cir_buf_samples_without_wrap_s32(src, source->buf_end) / channel_count; + int sink_frames = + cir_buf_samples_without_wrap_s32(dst, sink->buf_end) / channel_count; int 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[], struct cir_buf_source *source, void eq_fir_2x_s16(struct fir_state_32x16 fir[], struct cir_buf_source *source, struct cir_buf_sink *sink, int frames, 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; - int ch; - int 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; + int channel; + int pair_index; int rshift; int lshift; - int nch = channels; + int channel_count = channels; int remaining_frames = frames; while (remaining_frames) { - int source_frames = cir_buf_samples_without_wrap_s16(src, source->buf_end) / nch; - int sink_frames = cir_buf_samples_without_wrap_s16(snk, sink->buf_end) / nch; + int source_frames = + cir_buf_samples_without_wrap_s16(src, source->buf_end) / channel_count; + int sink_frames = + cir_buf_samples_without_wrap_s16(dst, sink->buf_end) / channel_count; int 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 7af96bae384a..7059d2317637 100644 --- a/src/audio/eq_fir/eq_fir_hifi3.c +++ b/src/audio/eq_fir/eq_fir_hifi3.c @@ -29,69 +29,71 @@ LOG_MODULE_DECLARE(eq_fir, CONFIG_SOF_LOG_LEVEL); void eq_fir_2x_s32(struct fir_state_32x16 fir[], struct cir_buf_source *source, struct cir_buf_sink *sink, int frames, int channels) { - struct fir_state_32x16 *f; + struct fir_state_32x16 *filter; ae_int32x2 d0 = 0; ae_int32x2 d1 = 0; ae_int32 *src = (ae_int32 *)source->ptr; ae_int32 *dst = (ae_int32 *)sink->ptr; - ae_int32 *x; - ae_int32 *y0; - ae_int32 *y1; - int ch; - int i; + ae_int32 *src_channel; + ae_int32 *dst_first; + ae_int32 *dst_second; + int channel; + int pair_index; int rshift; int lshift; int shift; - int nch = channels; - int inc_nch_s = nch * sizeof(int32_t); - int inc_2nch_s = 2 * inc_nch_s; + int channel_count = channels; + int channel_stride_bytes = channel_count * sizeof(int32_t); + int pair_stride_bytes = 2 * channel_stride_bytes; int remaining_frames = frames; while (remaining_frames) { - int source_frames = cir_buf_samples_without_wrap_s32(src, source->buf_end) / nch; - int sink_frames = cir_buf_samples_without_wrap_s32(dst, sink->buf_end) / nch; + int source_frames = + cir_buf_samples_without_wrap_s32(src, source->buf_end) / channel_count; + int sink_frames = + cir_buf_samples_without_wrap_s32(dst, sink->buf_end) / channel_count; int 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 = (ae_int32 *)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 = (ae_int32 *)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 = (ae_int32 *)source_cir_buf_wrap(src + chunk_frames * nch, + dst = cir_buf_wrap(dst + chunk_frames * channel_count, sink->buf_start, + sink->buf_end); + src = (ae_int32 *)source_cir_buf_wrap(src + chunk_frames * channel_count, source->buf_start, source->buf_end); remaining_frames -= chunk_frames; } @@ -102,88 +104,91 @@ void eq_fir_2x_s32(struct fir_state_32x16 fir[], struct cir_buf_source *source, void eq_fir_2x_s24(struct fir_state_32x16 fir[], struct cir_buf_source *source, struct cir_buf_sink *sink, int frames, 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; ae_int32 *src = (ae_int32 *)source->ptr; ae_int32 *dst = (ae_int32 *)sink->ptr; - ae_int32 *x; - ae_int32 *y; - int ch; - int i; + ae_int32 *src_channel; + ae_int32 *dst_channel; + int channel; + int pair_index; int rshift; int lshift; int shift; - int nch = channels; - int inc_nch_s = nch * sizeof(int32_t); + int channel_count = channels; + int channel_stride_bytes = channel_count * sizeof(int32_t); int remaining_frames = frames; while (remaining_frames) { - int source_frames = cir_buf_samples_without_wrap_s32(src, source->buf_end) / nch; - int sink_frames = cir_buf_samples_without_wrap_s32(dst, sink->buf_end) / nch; + int source_frames = + cir_buf_samples_without_wrap_s32(src, source->buf_end) / channel_count; + int sink_frames = + cir_buf_samples_without_wrap_s32(dst, sink->buf_end) / channel_count; int 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; - 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); - dst[ch] = sat_int24(Q_SHIFT_RND(output, 31, 23)); + fir_core_setup_circular(filter); + fir_32x16(filter, input, &output, shift); + dst[channel] = sat_int24(Q_SHIFT_RND(output, 31, 23)); } - src = (ae_int32 *)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 = (ae_int32 *)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 = (ae_int32 *)source_cir_buf_wrap(src + chunk_frames * nch, + dst = cir_buf_wrap(dst + chunk_frames * channel_count, sink->buf_start, + sink->buf_end); + src = (ae_int32 *)source_cir_buf_wrap(src + chunk_frames * channel_count, source->buf_start, source->buf_end); remaining_frames -= chunk_frames; } @@ -194,85 +199,88 @@ void eq_fir_2x_s24(struct fir_state_32x16 fir[], struct cir_buf_source *source, void eq_fir_2x_s16(struct fir_state_32x16 fir[], struct cir_buf_source *source, struct cir_buf_sink *sink, int frames, 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; ae_int16 *src = (ae_int16 *)source->ptr; ae_int16 *dst = (ae_int16 *)sink->ptr; - ae_int16 *x; - ae_int16 *y; - int ch; - int i; + ae_int16 *src_channel; + ae_int16 *dst_channel; + int channel; + int pair_index; int rshift; int lshift; int shift; - int nch = channels; - int inc_nch_s = nch * sizeof(int16_t); + int channel_count = channels; + int channel_stride_bytes = channel_count * sizeof(int16_t); int remaining_frames = frames; while (remaining_frames) { - int source_frames = cir_buf_samples_without_wrap_s16(src, source->buf_end) / nch; - int sink_frames = cir_buf_samples_without_wrap_s16(dst, sink->buf_end) / nch; + int source_frames = + cir_buf_samples_without_wrap_s16(src, source->buf_end) / channel_count; + int sink_frames = + cir_buf_samples_without_wrap_s16(dst, sink->buf_end) / channel_count; int chunk_frames = MIN(remaining_frames, MIN(source_frames, sink_frames)); chunk_frames &= ~0x1; if (!chunk_frames) { - for (ch = 0; ch < nch; ch++) { - int32_t input = src[ch] << 16; + for (channel = 0; channel < channel_count; channel++) { + int32_t input = src[channel] << 16; int32_t output; - 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); - dst[ch] = sat_int16(Q_SHIFT_RND(output, 31, 15)); + fir_core_setup_circular(filter); + fir_32x16(filter, input, &output, shift); + dst[channel] = sat_int16(Q_SHIFT_RND(output, 31, 15)); } - src = (ae_int16 *)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 = (ae_int16 *)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 = (ae_int16 *)source_cir_buf_wrap(src + chunk_frames * nch, + dst = cir_buf_wrap(dst + chunk_frames * channel_count, sink->buf_start, + sink->buf_end); + src = (ae_int16 *)source_cir_buf_wrap(src + chunk_frames * channel_count, source->buf_start, source->buf_end); remaining_frames -= chunk_frames; }