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..cd842c655e13 100644 --- a/src/audio/eq_fir/eq_fir_generic.c +++ b/src/audio/eq_fir/eq_fir_generic.c @@ -22,113 +22,119 @@ 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); - int nmax, n, i, j; - int nch = audio_stream_get_channels(source); - int remaining_samples = frames * nch; + 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 = EQ_FIR_BYTES_TO_S16_SAMPLES(audio_stream_bytes_without_wrap(source, x)); - n = MIN(remaining_samples, nmax); - nmax = EQ_FIR_BYTES_TO_S16_SAMPLES(audio_stream_bytes_without_wrap(sink, y)); - n = MIN(n, nmax); - for (j = 0; j < nch; j++) { - x0 = x + j; - y0 = y + j; - filter = &fir[j]; - for (i = 0; i < n; i += nch) { - z = fir_32x16(filter, *x0 << 16); - *y0 = sat_int16(Q_SHIFT_RND(z, 31, 15)); - x0 += nch; - y0 += nch; + 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 = audio_stream_wrap(source, x + n); - y = audio_stream_wrap(sink, y + n); + 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 */ #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); - int nmax, n, i, j; - int nch = audio_stream_get_channels(source); - int remaining_samples = frames * nch; + 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 = EQ_FIR_BYTES_TO_S32_SAMPLES(audio_stream_bytes_without_wrap(source, x)); - n = MIN(remaining_samples, nmax); - nmax = EQ_FIR_BYTES_TO_S32_SAMPLES(audio_stream_bytes_without_wrap(sink, y)); - n = MIN(n, nmax); - for (j = 0; j < nch; j++) { - x0 = x + j; - y0 = y + j; - filter = &fir[j]; - for (i = 0; i < n; i += nch) { - z = fir_32x16(filter, *x0 << 8); - *y0 = sat_int24(Q_SHIFT_RND(z, 31, 23)); - x0 += nch; - y0 += nch; + 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 = audio_stream_wrap(source, x + n); - y = audio_stream_wrap(sink, y + n); + 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 */ #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); - int nmax, n, i, j; - int nch = audio_stream_get_channels(source); - int remaining_samples = frames * nch; + 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 = EQ_FIR_BYTES_TO_S32_SAMPLES(audio_stream_bytes_without_wrap(source, x)); - n = MIN(remaining_samples, nmax); - nmax = EQ_FIR_BYTES_TO_S32_SAMPLES(audio_stream_bytes_without_wrap(sink, y)); - n = MIN(n, nmax); - for (j = 0; j < nch; j++) { - x0 = x + j; - y0 = y + j; - filter = &fir[j]; - for (i = 0; i < n; i += nch) { - *y0 = fir_32x16(filter, *x0); - x0 += nch; - y0 += nch; + 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 = audio_stream_wrap(source, x + n); - y = audio_stream_wrap(sink, y + n); + 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 71b40edcee32..4c8c3a94c56e 100644 --- a/src/audio/eq_fir/eq_fir_hifi2ep.c +++ b/src/audio/eq_fir/eq_fir_hifi2ep.c @@ -27,138 +27,233 @@ 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; - int32_t *y0; - int32_t *x1; - int32_t *y1; - int ch; - int i; + struct fir_state_32x16 *filter; + const int32_t *src = source->ptr; + 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 = 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 channel_count = channels; + int remaining_frames = frames; + + while (remaining_frames) { + 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 (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 + 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 (channel = 0; channel < channel_count; channel++) { + /* Get FIR instance and get shifts to e.g. apply mute + * without overhead. + */ + filter = &fir[channel]; + fir_get_lrshifts(filter, &lshift, &rshift); + + /* Setup circular buffer for FIR input data delay */ + 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 * 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; } } #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; - int32_t *y0; - int32_t *x1; - int32_t *y1; - int32_t z0; - int32_t z1; - int ch; - int i; + struct fir_state_32x16 *filter; + const int32_t *src = source->ptr; + 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 = 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 channel_count = channels; + int remaining_frames = frames; + + while (remaining_frames) { + 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 (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 + 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 (channel = 0; channel < channel_count; channel++) { + /* Get FIR instance and get shifts to e.g. apply mute + * without overhead. + */ + filter = &fir[channel]; + fir_get_lrshifts(filter, &lshift, &rshift); + + /* Setup circular buffer for FIR input data delay */ + 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 * 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; } } #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; - int16_t *y0; - int16_t *x1; - int16_t *y1; - int32_t z0; - int32_t z1; - int ch; - int i; + struct fir_state_32x16 *filter; + const int16_t *src = source->ptr; + 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 = 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 channel_count = channels; + int remaining_frames = frames; + + while (remaining_frames) { + 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 (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 + 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 (channel = 0; channel < channel_count; channel++) { + /* Get FIR instance and get shifts to e.g. apply mute + * without overhead. + */ + filter = &fir[channel]; + fir_get_lrshifts(filter, &lshift, &rshift); + + /* Setup circular buffer for FIR input data delay */ + 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 * 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; } } #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..7059d2317637 100644 --- a/src/audio/eq_fir/eq_fir_hifi3.c +++ b/src/audio/eq_fir/eq_fir_hifi3.c @@ -26,199 +26,263 @@ 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; + struct fir_state_32x16 *filter; 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; - ae_int32 *y0; - ae_int32 *y1; - int ch; - int i, n, nmax; + ae_int32 *src = (ae_int32 *)source->ptr; + ae_int32 *dst = (ae_int32 *)sink->ptr; + 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 = 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); - for (ch = 0; ch < nch; ch++) { + 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) / 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 (channel = 0; channel < channel_count; channel++) { + filter = &fir[channel]; + fir_get_lrshifts(filter, &lshift, &rshift); + shift = lshift - rshift; + fir_core_setup_circular(filter); + fir_32x16(filter, src[channel], dst + channel, shift); + } + 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 (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 < (n >> 1); i += nch) { - /* 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); } } - samples -= n; - dst = audio_stream_wrap(sink, dst + n); - src = audio_stream_wrap(source, src + n); + 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; } } #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; + struct fir_state_32x16 *filter; 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; - ae_int32 *y; - int ch; - int i, n, nmax; + 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 *src_channel; + ae_int32 *dst_channel; + int channel; + int pair_index; 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); - for (ch = 0; ch < nch; ch++) { + 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) / 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 (channel = 0; channel < channel_count; channel++) { + ae_int32 input = src[channel] << 8; + ae_int32 output; + + filter = &fir[channel]; + fir_get_lrshifts(filter, &lshift, &rshift); + shift = lshift - rshift; + 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 + 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 (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 < (n >> 1); i += nch) { - /* 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); } } - samples -= n; - dst = audio_stream_wrap(sink, dst + n); - src = audio_stream_wrap(source, src + n); + 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; } } #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; + 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_int16 *src = audio_stream_get_rptr(source); - ae_int16 *dst = audio_stream_get_wptr(sink); - ae_int16 *x; - ae_int16 *y; - int ch; - int i, n, nmax; + 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 *src_channel; + ae_int16 *dst_channel; + int channel; + int pair_index; 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); - for (ch = 0; ch < nch; ch++) { + 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) / 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 (channel = 0; channel < channel_count; channel++) { + int32_t input = src[channel] << 16; + int32_t output; + + filter = &fir[channel]; + fir_get_lrshifts(filter, &lshift, &rshift); + shift = lshift - rshift; + 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 + 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 (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 < (n >> 1); i += nch) { - /* 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); } } - samples -= n; - dst = audio_stream_wrap(sink, dst + n); - src = audio_stream_wrap(source, src + n); + 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; } } #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