--- src/audio/Makefile.am | 2 + src/audio/eq_iir.c | 496 +++++++++++++++++++++++++++++++++++++ src/audio/eq_iir.h | 83 +++++++ src/audio/iir.c | 161 ++++++++++++ src/audio/iir.h | 75 ++++++ src/include/reef/audio/component.h | 3 + src/include/reef/trace.h | 1 + src/include/uapi/ipc.h | 10 + src/tasks/audio.c | 1 + 9 files changed, 832 insertions(+) create mode 100644 src/audio/eq_iir.c create mode 100644 src/audio/eq_iir.h create mode 100644 src/audio/iir.c create mode 100644 src/audio/iir.h

diff --git a/src/audio/Makefile.am b/src/audio/Makefile.am index 1da6757..285422e 100644 --- a/src/audio/Makefile.am +++ b/src/audio/Makefile.am @@ -1,6 +1,8 @@ noinst_LIBRARIES = libaudio.a

libaudio_a_SOURCES = \ + eq_iir.c \ + iir.c \ eq_fir.c \ fir.c \ tone.c \ diff --git a/src/audio/eq_iir.c b/src/audio/eq_iir.c new file mode 100644 index 0000000..96570b2 --- /dev/null +++ b/src/audio/eq_iir.c @@ -0,0 +1,496 @@ +/* + * Copyright (c) 2017, Intel Corporation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Intel Corporation nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Seppo Ingalsuo seppo.ingalsuo@linux.intel.com + * Liam Girdwood liam.r.girdwood@linux.intel.com + * Keyon Jie yang.jie@linux.intel.com + */ + +#include <stdint.h> +#include <stddef.h> +#include <errno.h> +#include <reef/reef.h> +#include <reef/lock.h> +#include <reef/list.h> +#include <reef/stream.h> +#include <reef/alloc.h> +#include <reef/work.h> +#include <reef/clock.h> +#include <reef/audio/component.h> +#include <reef/audio/pipeline.h> +#include <reef/audio/format.h> +#include "eq_iir.h" +#include "iir.h" + +#ifdef MODULE_TEST +#include <stdio.h> +#endif + +#define trace_eq_iir(__e) trace_event(TRACE_CLASS_EQ_IIR, __e) +#define tracev_eq_iir(__e) tracev_event(TRACE_CLASS_EQ_IIR, __e) +#define trace_eq_iir_error(__e) trace_error(TRACE_CLASS_EQ_IIR, __e) + +/* src component private data */ +struct comp_data { + struct eq_iir_configuration *config; + struct iir_state_df2t iir[PLATFORM_MAX_CHANNELS]; + void (*eq_iir_func)(struct comp_dev *dev, + struct comp_buffer *source, + struct comp_buffer *sink, + uint32_t frames); +}; + +/* + * EQ IIR algorithm code + */ + +static void eq_iir_s32_default(struct comp_dev *dev, + struct comp_buffer *source, struct comp_buffer *sink, uint32_t frames) +{ + + int ch, n, n_wrap_src, n_wrap_snk, n_wrap_min; + int32_t *src = (int32_t *) source->r_ptr; + int32_t *snk = (int32_t *) sink->w_ptr; + int nch = source->params.pcm->channels; + int32_t *x = src + nch - 1; + int32_t *y = snk + nch - 1; + struct comp_data *cd = comp_get_drvdata(dev); + + + for (ch = 0; ch < nch; ch++) { + n = frames * nch; + x = src++; + y = snk++; + while (n > 0) { + n_wrap_src = (int32_t *) source->end_addr - x; + n_wrap_snk = (int32_t *) sink->end_addr - y; + n_wrap_min = (n_wrap_src < n_wrap_snk) ? + n_wrap_src : n_wrap_snk; + if (n < n_wrap_min) { + /* No circular wrap need */ + while (n > 0) { + *y = iir_df2t(&cd->iir[ch], *x); + x += nch; + y += nch; + n -= nch; + } + } else { + /* Wrap in n_wrap_min/nch samples */ + while (n_wrap_min > 0) { + *y = iir_df2t(&cd->iir[ch], *x); + x += nch; + y += nch; + n_wrap_min -= nch; + n -= nch; + } + /* Check both source and destination for wrap */ + if (x > (int32_t *) source->end_addr) + x = (int32_t *) + ((size_t) x - source->alloc_size); + if (snk > (int32_t *) sink->end_addr) + y = (int32_t *) + ((size_t) y - sink->alloc_size); + } + } + + } + source->r_ptr = x - nch + 1; /* After previous loop the x and y */ + sink->w_ptr = y - nch + 1; /* point to one frame -1. */ + + comp_wrap_source_r_ptr_circular(source); + comp_wrap_sink_w_ptr_circular(sink); + comp_update_source_free_avail(source, frames * nch); + comp_update_sink_free_avail(sink, frames * nch); +} + +static void eq_iir_free_parameters(struct eq_iir_configuration **config) +{ + if (*config != NULL) + rfree(*config); + + *config = NULL; +} + +static void eq_iir_free_delaylines(struct iir_state_df2t *iir) +{ + int i = 0; + int64_t *delay = NULL; /* TODO should not need to know the type */ + + /* 1st active EQ delay line data is at beginning of the single + * allocated buffer + */ + for (i = 0; i < PLATFORM_MAX_CHANNELS; i++) { + if ((iir[i].delay != NULL) && (delay == NULL)) + delay = iir[i].delay; + + /* Point all delays to NULL to avoid duplicated free later */ + iir[i].delay = NULL; + } + + if (delay != NULL) + rfree(delay); + +} + +static int eq_iir_setup(struct iir_state_df2t iir[], + struct eq_iir_configuration *config, int nch) +{ + int i, j, idx, resp; + size_t s; + size_t size_sum = 0; + int64_t *iir_delay; /* TODO should not need to know the type */ + int response_index[PLATFORM_MAX_CHANNELS]; + + if (nch > PLATFORM_MAX_CHANNELS) + return -EINVAL; + + /* Free existing IIR channels data if it was allocated */ + eq_iir_free_delaylines(iir); + + /* Collect index of respose start positions in all_coefficients[] */ + j = 0; + for (i = 0; i < PLATFORM_MAX_CHANNELS; i++) { + if (i < config->number_of_responses_defined) { + response_index[i] = j; + j += NHEADER_DF2T + + NBIQUAD_DF2T * config->all_coefficients[j]; + } else { + response_index[i] = 0; + } + } + + /* Initialize 1st phase */ + for (i = 0; i < nch; i++) { + resp = config->assign_response[i]; + if (resp < 0) { + /* Initialize EQ channel to bypass */ + iir_reset_df2t(&iir[i]); + } else { + /* Initialize EQ coefficients */ + idx = response_index[resp]; + s = iir_init_coef_df2t(&iir[i], + &config->all_coefficients[idx]); + if (s > 0) + size_sum += s; + else + return -EINVAL; + } + + } + + /* Allocate all IIR channels data in a big chunk and clear it */ + iir_delay = rmalloc(RZONE_RUNTIME, RFLAGS_NONE, size_sum); + if (iir_delay == NULL) + return -EINVAL; + + memset(iir_delay, 0, size_sum); + + /* Initialize 2nd phase to set EQ delay lines pointers */ + for (i = 0; i < nch; i++) { + resp = config->assign_response[i]; + if (resp >= 0) { + idx = response_index[resp]; + iir_init_delay_df2t(&iir[i], &iir_delay); + } + + } + + return 0; +} + +static int eq_iir_switch_response(struct iir_state_df2t iir[], + struct eq_iir_configuration *config, struct eq_iir_update *update, + int nch) +{ + int i; + + /* Copy assign response from update and re-initilize EQ */ + if (config == NULL) + return -EINVAL; + + for (i = 0; i < config->stream_max_channels; i++) { + if (i < update->stream_max_channels) + config->assign_response[i] = update->assign_response[i]; + } + + eq_iir_setup(iir, config, nch); + + return 0; +} + +/* + * End of EQ setup code. Next the standard component methods. + */ + +static struct comp_dev *eq_iir_new(struct sof_ipc_comp *comp) +{ + int i; + //struct comp_buffer *sink; + //struct comp_buffer *source; + struct comp_dev *dev; + struct comp_data *cd; + + trace_eq_iir("ENw"); + dev = rmalloc(RZONE_RUNTIME, RFLAGS_NONE, sizeof(*dev)); + if (dev == NULL) + return NULL; + + //memcpy(&dev->comp, comp, sizeof(struct sof_ipc_comp_eq_iir)); + + cd = rmalloc(RZONE_RUNTIME, RFLAGS_NONE, sizeof(*cd)); + if (cd == NULL) { + rfree(dev); + return NULL; + } + + comp_set_drvdata(dev, cd); + + cd->eq_iir_func = eq_iir_s32_default; + cd->config = NULL; + for (i = 0; i < PLATFORM_MAX_CHANNELS; i++) + iir_reset_df2t(&cd->iir[i]); + + return dev; +} + +static void eq_iir_free(struct comp_dev *dev) +{ + struct comp_data *cd = comp_get_drvdata(dev); + + trace_eq_iir("EFr"); + + eq_iir_free_delaylines(cd->iir); + eq_iir_free_parameters(&cd->config); + + rfree(cd); + rfree(dev); +} + +/* set component audio stream parameters */ +static int eq_iir_params(struct comp_dev *dev, struct stream_params *params) +{ + + trace_eq_iir("EPa"); + + /* EQ supports only S32_LE PCM format */ + if ((params->type != STREAM_TYPE_PCM) + || (params->pcm->frame_fmt != SOF_IPC_FRAME_S32_LE)) + return -EINVAL; + + /* don't do any data transformation */ + comp_set_sink_params(dev, params); + + return 0; +} + +/* used to pass standard and bespoke commands (with data) to component */ +static int eq_iir_cmd(struct comp_dev *dev, int cmd, void *data) +{ + trace_eq_iir("ECm"); + struct comp_data *cd = comp_get_drvdata(dev); + struct comp_buffer *source = list_first_item(&dev->bsource_list, + struct comp_buffer, sink_list); + struct sof_ipc_eq_iir_switch *assign; + struct sof_ipc_eq_iir_blob *blob; + int i; + size_t bs; + switch (cmd) { + case COMP_CMD_EQ_IIR_SWITCH: + trace_eq_iir("EFx"); + assign = (struct sof_ipc_eq_iir_switch *) data; + eq_iir_switch_response(cd->iir, cd->config, + (struct eq_iir_update *) assign->data, + source->params.pcm->channels); + break; + case COMP_CMD_EQ_IIR_CONFIG: + trace_eq_iir("EFc"); + /* Check and free old config */ + eq_iir_free_parameters(&cd->config); + + /* Copy new config, need to decode data to know the size */ + blob = (struct sof_ipc_eq_iir_blob *) data; + bs = blob->hdr.size - sizeof(struct sof_ipc_hdr); + if (bs > EQ_IIR_MAX_BLOB_SIZE) + return -EINVAL; + + /* Allocate and make a copy of the blob and setup IIR */ + cd->config = rmalloc(RZONE_RUNTIME, RFLAGS_NONE, bs); + if (cd->config != NULL) + memcpy(cd->config, blob->data, bs); + + eq_iir_setup(cd->iir, cd->config, source->params.pcm->channels); + break; + case COMP_CMD_MUTE: + trace_eq_iir("EFm"); + for (i = 0; i < PLATFORM_MAX_CHANNELS; i++) + iir_mute_df2t(&cd->iir[i]); + + break; + case COMP_CMD_UNMUTE: + trace_eq_iir("EFu"); + for (i = 0; i < PLATFORM_MAX_CHANNELS; i++) + iir_unmute_df2t(&cd->iir[i]); + + break; + case COMP_CMD_START: + trace_eq_iir("EFs"); + dev->state = COMP_STATE_RUNNING; + break; + case COMP_CMD_STOP: + trace_eq_iir("ESp"); + if (dev->state == COMP_STATE_RUNNING || + dev->state == COMP_STATE_DRAINING || + dev->state == COMP_STATE_PAUSED) { + comp_buffer_reset(dev); + dev->state = COMP_STATE_SETUP; + } + break; + case COMP_CMD_PAUSE: + trace_eq_iir("EPe"); + /* only support pausing for running */ + if (dev->state == COMP_STATE_RUNNING) + dev->state = COMP_STATE_PAUSED; + + break; + case COMP_CMD_RELEASE: + trace_eq_iir("ERl"); + dev->state = COMP_STATE_RUNNING; + break; + default: + trace_eq_iir("EDf"); + break; + } + + return 0; +} + +/* copy and process stream data from source to sink buffers */ +static int eq_iir_copy(struct comp_dev *dev) +{ + struct comp_data *cd = comp_get_drvdata(dev); + struct comp_buffer *source; + struct comp_buffer *sink; + int frames; + int need_source, need_sink; + + trace_comp("EqI"); + + /* src component needs 1 source and 1 sink buffer */ + source = list_first_item(&dev->bsource_list, struct comp_buffer, + sink_list); + sink = list_first_item(&dev->bsink_list, struct comp_buffer, + source_list); + + /* Check that source has enough frames available and that sink has + * enough frames free. + */ + //frames = source->params.period_frames; + //need_source = frames * source->params.frame_size; + //need_sink = frames * sink->params.frame_size; + frames = source->params.pcm->period_count; + need_source = frames * source->params.pcm->frame_size; + need_sink = frames * sink->params.pcm->frame_size; + + /* Run EQ if buffers have enough room */ + if ((source->avail >= need_source) && (sink->free >= need_sink)) + cd->eq_iir_func(dev, source, sink, frames); + + return 0; +} + +static int eq_iir_prepare(struct comp_dev *dev) +{ + struct comp_data *cd = comp_get_drvdata(dev); + struct comp_buffer *source; + + trace_eq_iir("EPp"); + + cd->eq_iir_func = eq_iir_s32_default; + + /* Initialize EQ */ + if (cd->config == NULL) + return -EINVAL; + + source = list_first_item(&dev->bsource_list, struct comp_buffer, + sink_list); + if (eq_iir_setup(cd->iir, cd->config, source->params.pcm->channels) < 0) + return -EINVAL; + + //dev->preload = PLAT_INT_PERIODS; + dev->state = COMP_STATE_PREPARE; + return 0; +} + +static int eq_iir_preload(struct comp_dev *dev) +{ + //int i; + + trace_eq_iir("EPl"); + + //for (i = 0; i < dev->preload; i++) + // eq_iir_copy(dev); + + return 0; +} + +static int eq_iir_reset(struct comp_dev *dev) +{ + int i; + struct comp_data *cd = comp_get_drvdata(dev); + + trace_eq_iir("ERe"); + + eq_iir_free_delaylines(cd->iir); + eq_iir_free_parameters(&cd->config); + + cd->eq_iir_func = eq_iir_s32_default; + for (i = 0; i < PLATFORM_MAX_CHANNELS; i++) + iir_reset_df2t(&cd->iir[i]); + + dev->state = COMP_STATE_INIT; + return 0; +} + +struct comp_driver comp_eq_iir = { + .type = SOF_COMP_EQ_IIR, + .ops = + { + .new = eq_iir_new, + .free = eq_iir_free, + .params = eq_iir_params, + .cmd = eq_iir_cmd, + .copy = eq_iir_copy, + .prepare = eq_iir_prepare, + .reset = eq_iir_reset, + .preload = eq_iir_preload, + }, +}; + +void sys_comp_eq_iir_init(void) +{ + comp_register(&comp_eq_iir); +} diff --git a/src/audio/eq_iir.h b/src/audio/eq_iir.h new file mode 100644 index 0000000..43c666d --- /dev/null +++ b/src/audio/eq_iir.h @@ -0,0 +1,83 @@ +/* + * Copyright (c) 2017, Intel Corporation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Intel Corporation nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Seppo Ingalsuo seppo.ingalsuo@linux.intel.com + * Liam Girdwood liam.r.girdwood@linux.intel.com + * Keyon Jie yang.jie@linux.intel.com + */ + +#ifndef EQ_IIR_H +#define EQ_IIR_H + + + +/* eq_iir_configuration + * uint32_t platform max channels + * uint32_t number_of_responses_defined + * 0=no responses, 1=one response defined, 2=two responses defined, etc. + * uint32_t assign_response[PLATFORM_MAX_CHANNELS] + * -1 = not defined, 0 = use first response, 1 = use 2nd response, etc. + * E.g. {0, 0, 0, 0, -1, -1, -1, -1} would apply to channels 0-3 the + * same first defined response and leave channels 4-7 unequalized. + * all_coefficients[] + * <1st EQ> + * uint32_t num_biquads + * uint32_t num_biquads_in_series + * <1st biquad> + * int32_t coef_a2 Q2.30 format + * int32_t coef_a1 Q2.30 format + * int32_t coef_b2 Q2.30 format + * int32_t coef_b1 Q2.30 format + * int32_t coef_b0 Q2.30 format + * int32_t output_shift number of shifts right, shift left is negative + * int32_t output_gain Q2.14 format + * <2nd biquad> + * ... + * <2nd EQ> + * + * Note: A flat response biquad can be made with a section set to + * b0 = 1.0, gain = 1.0, and other parameters set to 0 + * {0, 0, 0, 0, 1073741824, 0, 16484} + */ + +#define EQ_IIR_MAX_BLOB_SIZE 1024 + +#define NHEADER_EQ_IIR_BLOB 2 /* Blob is two words plus asssigns plus coef */ + +struct eq_iir_configuration { + int32_t stream_max_channels; + int32_t number_of_responses_defined; + int32_t assign_response[PLATFORM_MAX_CHANNELS]; + int32_t all_coefficients[]; +}; + +struct eq_iir_update { + int32_t stream_max_channels; + int32_t assign_response[PLATFORM_MAX_CHANNELS]; +}; + +#endif diff --git a/src/audio/iir.c b/src/audio/iir.c new file mode 100644 index 0000000..a713cdf --- /dev/null +++ b/src/audio/iir.c @@ -0,0 +1,161 @@ +/* + * Copyright (c) 2017, Intel Corporation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Intel Corporation nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Seppo Ingalsuo seppo.ingalsuo@linux.intel.com + * Liam Girdwood liam.r.girdwood@linux.intel.com + * Keyon Jie yang.jie@linux.intel.com + */ + +#include <stdint.h> +#include <stddef.h> +#include <errno.h> + +#ifdef MODULE_TEST +#include <stdio.h> +#endif + +#include <reef/audio/format.h> +#include "iir.h" + +/* + * Direct form II transposed second order filter block (biquad) + * + * +----+ +---+ +-------+ + * X(z) ---o--->| b0 |---> + -------------o--->| g |--->| shift |---> Y(z) + * | +----+ ^ | +---+ +-------+ + * | | | + * | +------+ | + * | | z^-1 | | + * | +------+ | + * | ^ | + * | +----+ | +----+ | + * o--->| b1 |---> + <---| a1 |---o + * | +----+ ^ +----+ | + * | | | + * | +------+ | + * | | z^-1 | | + * | +------+ | + * | ^ | + * | +----+ | +----+ | + * o--->| b2 |---> + <---| a2 |---+ + * +----+ +----+ + * + */ + +/* Series DF2T IIR */ + +/* 32 bit data, 32 bit coefficients and 64 bit state variables */ + +int32_t iir_df2t(struct iir_state_df2t *iir, int32_t x) +{ + int32_t in, tmp; + int64_t acc; + int32_t out = 0; + int i, j; + int d = 0; /* Index to delays */ + int c = 2; /* Index to coefficient a2 */ + + /* Coefficients order in coef[] is {a2, a1, b2, b1, b0, shift, gain} */ + in = x; + for (j = 0; j < iir->biquads; j += iir->biquads_in_series) { + for (i = 0; i < iir->biquads_in_series; i++) { + /* Compute output: Delay is Q3.61 + * Q2.30 x Q1.31 -> Q3.61 + * Shift Q3.61 to Q3.31 with rounding + */ + acc = ((int64_t) iir->coef[c + 4]) * in + iir->delay[d]; + tmp = (int32_t) Q_SHIFT_RND(acc, 61, 31); + + /* Compute 1st delay */ + acc = iir->delay[d + 1]; + acc += ((int64_t) iir->coef[c + 3]) * in; /* Coef b1 */ + acc += ((int64_t) iir->coef[c + 1]) * tmp; /* Coef a1 */ + iir->delay[d] = acc; + + /* Compute 2nd delay */ + acc = ((int64_t) iir->coef[c + 2]) * in; /* Coef b2 */ + acc += ((int64_t) iir->coef[c]) * tmp; /* Coef a2 */ + iir->delay[d + 1] = acc; + + /* Gain, output shift, prepare for next biquad + * Q2.14 x Q1.31 -> Q3.45, shift too Q3.31 and saturate + */ + acc = ((int64_t) iir->coef[c + 6]) * tmp; /* Gain */ + acc = Q_SHIFT_RND(acc, 45 + iir->coef[c + 5], 31); + in = sat_int32(acc); + c += 7; /* Next coefficients section */ + d += 2; /* Next biquad delays */ + } + /* Output of previous section is in variable in */ + out = sat_int32((int64_t) out + in); + } + return out; +} + +size_t iir_init_coef_df2t(struct iir_state_df2t *iir, int32_t config[]) +{ + iir->mute = 0; + iir->biquads = (int) config[0]; + iir->biquads_in_series = (int) config[1]; + iir->coef = &config[0]; /* TODO: Could change this to config[2] */ + iir->delay = NULL; + + if ((iir->biquads > IIR_DF2T_BIQUADS_MAX) || (iir->biquads < 1)) { + iir_reset_df2t(iir); + return -EINVAL; + } + + return 2 * iir->biquads * sizeof(int64_t); /* Needed delay line size */ +} + +void iir_init_delay_df2t(struct iir_state_df2t *iir, int64_t **delay) +{ + iir->delay = *delay; /* Delay line of this IIR */ + *delay += 2 * iir->biquads; /* Point to next IIR delay line start */ + +} + +void iir_mute_df2t(struct iir_state_df2t *iir) +{ + iir->mute = 1; +} + +void iir_unmute_df2t(struct iir_state_df2t *iir) +{ + iir->mute = 0; +} + +void iir_reset_df2t(struct iir_state_df2t *iir) +{ + iir->mute = 1; + iir->biquads = 0; + iir->biquads_in_series = 0; + iir->coef = NULL; + /* Note: May need to know the beginning of dynamic allocation after so + * omitting setting iir->delay to NULL. + */ +} diff --git a/src/audio/iir.h b/src/audio/iir.h new file mode 100644 index 0000000..0de275a --- /dev/null +++ b/src/audio/iir.h @@ -0,0 +1,75 @@ +/* + * Copyright (c) 2017, Intel Corporation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Intel Corporation nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Seppo Ingalsuo seppo.ingalsuo@linux.intel.com + * Liam Girdwood liam.r.girdwood@linux.intel.com + * Keyon Jie yang.jie@linux.intel.com + */ + +/* A full 22th order equalizer with 11 biquads cover octave bands 1-11 in + * in the 0 - 20 kHz bandwidth. + */ +#define IIR_DF2T_BIQUADS_MAX 11 + +struct iir_state_df2t { + int mute; /* Set to 1 to mute EQ output, 0 otherwise */ + int biquads; /* Number of IIR 2nd order sections total */ + int biquads_in_series; /* Number of IIR 2nd order sections in series*/ + int32_t *coef; /* Pointer to IIR coefficients */ + int64_t *delay; /* Pointer to IIR delay line */ +}; + +#define NHEADER_DF2T 2 + +struct iir_header_df2t { + int32_t num_sections; + int32_t num_sections_in_series; +}; + +#define NBIQUAD_DF2T 7 + +struct iir_biquad_df2t { + int32_t a2; /* Q2.30 */ + int32_t a1; /* Q2.30 */ + int32_t b2; /* Q2.30 */ + int32_t b1; /* Q2.30 */ + int32_t b0; /* Q2.30 */ + int32_t output_shift; /* Number of right shifts */ + int32_t output_gain; /* Q2.14 */ +}; + +int32_t iir_df2t(struct iir_state_df2t *iir, int32_t x); + +size_t iir_init_coef_df2t(struct iir_state_df2t *iir, int32_t config[]); + +void iir_init_delay_df2t(struct iir_state_df2t *iir, int64_t **delay); + +void iir_mute_df2t(struct iir_state_df2t *iir); + +void iir_unmute_df2t(struct iir_state_df2t *iir); + +void iir_reset_df2t(struct iir_state_df2t *iir); diff --git a/src/include/reef/audio/component.h b/src/include/reef/audio/component.h index 7de47c2..7e28dfc 100644 --- a/src/include/reef/audio/component.h +++ b/src/include/reef/audio/component.h @@ -78,6 +78,8 @@ #define COMP_CMD_TONE 106 /* Tone generator amplitude and frequency */ #define COMP_CMD_EQ_FIR_CONFIG 107 /* Configuration data for FIR EQ */ #define COMP_CMD_EQ_FIR_SWITCH 108 /* Update request for FIR EQ */ +#define COMP_CMD_EQ_IIR_CONFIG 109 /* Configuration data for IIR EQ */ +#define COMP_CMD_EQ_IIR_SWITCH 110 /* Response update request for IIR EQ */

/* MMAP IPC status */ #define COMP_CMD_IPC_MMAP_RPOS 200 /* host read position */ @@ -263,6 +265,7 @@ void sys_comp_switch_init(void); void sys_comp_volume_init(void); void sys_comp_src_init(void); void sys_comp_tone_init(void); +void sys_comp_eq_iir_init(void);

void sys_comp_eq_fir_init(void); diff --git a/src/include/reef/trace.h b/src/include/reef/trace.h index ff41caf..00dbb5b 100644 --- a/src/include/reef/trace.h +++ b/src/include/reef/trace.h @@ -85,6 +85,7 @@ #define TRACE_CLASS_SRC (17 << 24) #define TRACE_CLASS_TONE (18 << 24) #define TRACE_CLASS_EQ_FIR (19 << 24) +#define TRACE_CLASS_EQ_IIR (20 << 24)

/* move to config.h */ #define TRACE 1 diff --git a/src/include/uapi/ipc.h b/src/include/uapi/ipc.h index 3bb6f61..20048b6 100644 --- a/src/include/uapi/ipc.h +++ b/src/include/uapi/ipc.h @@ -680,10 +680,20 @@ struct sof_ipc_eq_fir_blob { int32_t data[]; } __attribute__((packed));

+struct sof_ipc_eq_iir_blob { + struct sof_ipc_hdr hdr; + struct sof_ipc_host_buffer buffer; + int32_t data[]; +} __attribute__((packed)); + struct sof_ipc_eq_fir_switch { struct sof_ipc_hdr hdr; int32_t data[]; } __attribute__((packed));

+struct sof_ipc_eq_iir_switch { + struct sof_ipc_hdr hdr; + int32_t data[]; +} __attribute__((packed));

#endif diff --git a/src/tasks/audio.c b/src/tasks/audio.c index c420fb1..123b964 100644 --- a/src/tasks/audio.c +++ b/src/tasks/audio.c @@ -65,6 +65,7 @@ int do_task(struct reef *reef) sys_comp_volume_init(); sys_comp_src_init(); sys_comp_tone_init(); + sys_comp_eq_iir_init(); sys_comp_eq_fir_init();

#if 0