--- src/audio/Makefile.am | 2 + src/audio/eq_fir.c | 487 +++++++++++++++++++++++++++++++++++++ src/audio/eq_fir.h | 70 ++++++ src/audio/fir.c | 92 +++++++ src/audio/fir.h | 130 ++++++++++ src/include/reef/audio/component.h | 3 + src/include/reef/trace.h | 1 + src/include/uapi/ipc.h | 12 + src/tasks/audio.c | 1 + 9 files changed, 798 insertions(+) create mode 100644 src/audio/eq_fir.c create mode 100644 src/audio/eq_fir.h create mode 100644 src/audio/fir.c create mode 100644 src/audio/fir.h
diff --git a/src/audio/Makefile.am b/src/audio/Makefile.am index 6f0381a..1da6757 100644 --- a/src/audio/Makefile.am +++ b/src/audio/Makefile.am @@ -1,6 +1,8 @@ noinst_LIBRARIES = libaudio.a
libaudio_a_SOURCES = \ + eq_fir.c \ + fir.c \ tone.c \ src.c \ src_core.c \ diff --git a/src/audio/eq_fir.c b/src/audio/eq_fir.c new file mode 100644 index 0000000..6c8f1fb --- /dev/null +++ b/src/audio/eq_fir.c @@ -0,0 +1,487 @@ +/* + * 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 "fir.h" +#include "eq_fir.h" + +#ifdef MODULE_TEST +#include <stdio.h> +#endif + +#define trace_src(__e) trace_event(TRACE_CLASS_SRC, __e) +#define tracev_src(__e) tracev_event(TRACE_CLASS_SRC, __e) +#define trace_src_error(__e) trace_error(TRACE_CLASS_SRC, __e) + +/* src component private data */ +struct comp_data { + struct eq_fir_configuration *config; + struct fir_state_32x16 fir[PLATFORM_MAX_CHANNELS]; + void (*eq_fir_func)(struct comp_dev *dev, + struct comp_buffer *source, + struct comp_buffer *sink, + uint32_t frames); +}; + +/* + * EQ FIR algorithm code + */ + +static void eq_fir_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 = fir_32x16(&cd->fir[ch], *x); + x += nch; + y += nch; + n -= nch; + } + } else { + /* Wrap in n_wrap_min/nch samples */ + while (n_wrap_min > 0) { + *y = fir_32x16(&cd->fir[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_fir_free_parameters(struct eq_fir_configuration **config) +{ + if (*config != NULL) + rfree(*config); + + *config = NULL; +} + +static void eq_fir_free_delaylines(struct fir_state_32x16 fir[]) +{ + int i = 0; + int32_t *data = NULL; + + /* 1st active EQ data is at beginning of the single allocated buffer */ + for (i = 0; i < PLATFORM_MAX_CHANNELS; i++) { + if ((fir[i].delay != NULL) && (data == NULL)) + data = fir[i].delay; + + /* Set all to NULL to avoid duplicated free later */ + fir[i].delay = NULL; + } + + if (data != NULL) + rbfree(data); + +} + +static int eq_fir_setup(struct fir_state_32x16 fir[], + struct eq_fir_configuration *config, int nch) +{ + int i, j, idx, length, resp; + int32_t *fir_data; + int response_index[PLATFORM_MAX_CHANNELS]; + int length_sum = 0; + + if (nch > PLATFORM_MAX_CHANNELS) + return -EINVAL; + + /* 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 += 3 + 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 */ + fir_reset(&fir[i]); + } else { + /* Initialize EQ coefficients */ + idx = response_index[resp]; + length = fir_init_coef(&fir[i], + &config->all_coefficients[idx]); + if (length > 0) + length_sum += length; + } + + } + /* Free existing FIR channels data if it was allocated */ + eq_fir_free_delaylines(fir); + + /* Allocate all FIR channels data in a big chunk and clear it */ + fir_data = rballoc(RZONE_SYS, RFLAGS_NONE, + length_sum * sizeof(int32_t)); + if (fir_data == NULL) + return -EINVAL; + + memset(fir_data, 0, length_sum * sizeof(int32_t)); + + /* 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]; + fir_init_delay(&fir[i], &config->all_coefficients[idx], + &fir_data); + } + + } + + return 0; +} + +static int eq_fir_switch_response(struct fir_state_32x16 fir[], + struct eq_fir_configuration *config, struct eq_fir_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_fir_setup(fir, config, nch); + + return 0; +} + +/* + * End of algorithm code. Next the standard component methods. + */ + +static struct comp_dev *eq_fir_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_src("ENw"); + dev = rmalloc(RZONE_RUNTIME, RFLAGS_NONE, sizeof(*dev)); + if (dev == NULL) + return NULL; + + //memcpy(&dev->comp, comp, sizeof(struct sof_ipc_comp_eq_fir)); + + cd = rmalloc(RZONE_RUNTIME, RFLAGS_NONE, sizeof(*cd)); + if (cd == NULL) { + rfree(dev); + return NULL; + } + + comp_set_drvdata(dev, cd); + + cd->eq_fir_func = eq_fir_s32_default; + cd->config = NULL; + for (i = 0; i < PLATFORM_MAX_CHANNELS; i++) + fir_reset(&cd->fir[i]); + + return dev; +} + +static void eq_fir_free(struct comp_dev *dev) +{ + struct comp_data *cd = comp_get_drvdata(dev); + + trace_src("EFr"); + + eq_fir_free_delaylines(cd->fir); + eq_fir_free_parameters(&cd->config); + + rfree(cd); + rfree(dev); +} + +/* set component audio stream parameters */ +static int eq_fir_params(struct comp_dev *dev, struct stream_params *params) +{ + + trace_src("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_fir_cmd(struct comp_dev *dev, int cmd, void *data) +{ + trace_src("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_fir_blob *blob; + struct sof_ipc_eq_fir_switch *assign; + + int i; + size_t bs; + + switch (cmd) { + case COMP_CMD_EQ_FIR_SWITCH: + trace_src("EFx"); + assign = (struct sof_ipc_eq_fir_switch *) data; + eq_fir_switch_response(cd->fir, cd->config, + (struct eq_fir_update *) assign->data, + source->params.pcm->channels); + break; + case COMP_CMD_EQ_FIR_CONFIG: + trace_src("EFc"); + /* Check and free old config */ + eq_fir_free_parameters(&cd->config); + + /* Copy new config, need to decode data to know the size */ + blob = (struct sof_ipc_eq_fir_blob *) data; + bs = blob->hdr.size - sizeof(struct sof_ipc_hdr); + if (bs > EQ_FIR_MAX_BLOB_SIZE) + return -EINVAL; + + cd->config = rmalloc(RZONE_RUNTIME, RFLAGS_NONE, bs); + if (cd->config != NULL) + memcpy(cd->config, blob->data, bs); + + eq_fir_setup(cd->fir, cd->config, source->params.pcm->channels); + break; + case COMP_CMD_MUTE: + trace_src("EFm"); + for (i = 0; i < PLATFORM_MAX_CHANNELS; i++) + fir_mute(&cd->fir[i]); + + break; + case COMP_CMD_UNMUTE: + trace_src("EFu"); + for (i = 0; i < PLATFORM_MAX_CHANNELS; i++) + fir_unmute(&cd->fir[i]); + + break; + case COMP_CMD_START: + trace_src("EFs"); + dev->state = COMP_STATE_RUNNING; + break; + case COMP_CMD_STOP: + trace_src("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_src("EPe"); + /* only support pausing for running */ + if (dev->state == COMP_STATE_RUNNING) + dev->state = COMP_STATE_PAUSED; + + break; + case COMP_CMD_RELEASE: + trace_src("ERl"); + dev->state = COMP_STATE_RUNNING; + break; + default: + trace_src("EDf"); + break; + } + + return 0; +} + +/* copy and process stream data from source to sink buffers */ +static int eq_fir_copy(struct comp_dev *dev) +{ + struct comp_data *sd = comp_get_drvdata(dev); + struct comp_buffer *source; + struct comp_buffer *sink; + int frames; + int need_source, need_sink; + + trace_comp("EqF"); + + /* 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 sink enough + * frames free. + */ + 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)) + sd->eq_fir_func(dev, source, sink, frames); + + return 0; +} + +static int eq_fir_prepare(struct comp_dev *dev) +{ + struct comp_data *cd = comp_get_drvdata(dev); + struct comp_buffer *source; + + trace_src("EPp"); + + cd->eq_fir_func = eq_fir_s32_default; + + /* Initialize EQ */ + if (cd->config == NULL) + return -EINVAL; + + source = list_first_item(&dev->bsource_list, struct comp_buffer, + sink_list); + if (eq_fir_setup(cd->fir, cd->config, source->params.pcm->channels) < 0) + return -EINVAL; + + //dev->preload = PLAT_INT_PERIODS; + dev->state = COMP_STATE_PREPARE; + return 0; +} + +static int eq_fir_preload(struct comp_dev *dev) +{ + //int i; + + trace_src("EPl"); + //for (i = 0; i < dev->preload; i++) + // eq_fir_copy(dev); + + return 0; +} + +static int eq_fir_reset(struct comp_dev *dev) +{ + int i; + struct comp_data *cd = comp_get_drvdata(dev); + + trace_src("ERe"); + + eq_fir_free_delaylines(cd->fir); + eq_fir_free_parameters(&cd->config); + + cd->eq_fir_func = eq_fir_s32_default; + for (i = 0; i < PLATFORM_MAX_CHANNELS; i++) + fir_reset(&cd->fir[i]); + + dev->state = COMP_STATE_INIT; + return 0; +} + +struct comp_driver comp_eq_fir = { + .type = SOF_COMP_EQ_FIR, + .ops = + { + .new = eq_fir_new, + .free = eq_fir_free, + .params = eq_fir_params, + .cmd = eq_fir_cmd, + .copy = eq_fir_copy, + .prepare = eq_fir_prepare, + .reset = eq_fir_reset, + .preload = eq_fir_preload, + }, +}; + +void sys_comp_eq_fir_init(void) +{ + comp_register(&comp_eq_fir); +} diff --git a/src/audio/eq_fir.h b/src/audio/eq_fir.h new file mode 100644 index 0000000..22af4af --- /dev/null +++ b/src/audio/eq_fir.h @@ -0,0 +1,70 @@ +/* + * 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_FIR_H +#define EQ_FIR_H + + +/* + * eq_fir_configuration data structure contains this information + * stream max channels + * number_of_responses_defined + * 0=no respones, 1=one response defined, 2=two responses defined, etc. + * assign_response[STREAM_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[] + * Repeated data { filter_length, input_shift, output_shift, h[] } + * where vector h has filter_length number of coefficients. + * Coefficients in h[] are in Q1.15 format. 16384 = 0.5. The shifts + * are number of right shifts. + */ + +#define NHEADER_EQ_FIR_BLOB 2 /* Header is two words plus assigns plus coef */ + +#define EQ_FIR_MAX_BLOB_SIZE 4096 /* Max size allowed for blob */ + +struct eq_fir_configuration { + uint16_t stream_max_channels; + uint16_t number_of_responses_defined; + uint16_t assign_response[PLATFORM_MAX_CHANNELS]; + int16_t all_coefficients[]; +}; + +struct eq_fir_update { + uint16_t stream_max_channels; + uint16_t assign_response[PLATFORM_MAX_CHANNELS]; + +}; + +#endif diff --git a/src/audio/fir.c b/src/audio/fir.c new file mode 100644 index 0000000..beb7e72 --- /dev/null +++ b/src/audio/fir.c @@ -0,0 +1,92 @@ +/* + * 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 "fir.h" + +#ifdef MODULE_TEST +#include <stdio.h> +#endif + +/* + * EQ FIR algorithm code + */ + +void fir_reset(struct fir_state_32x16 *fir) +{ + fir->mute = 1; + fir->rwi = 0; + fir->length = 0; + fir->delay_size = 0; + fir->in_shift = 0; + fir->out_shift = 0; + fir->coef = NULL; + /* There may need to know the beginning of dynamic allocation after + * reset so omitting setting also fir->delay to NULL. + */ +} + +int fir_init_coef(struct fir_state_32x16 *fir, int16_t config[]) +{ + struct fir_coef_32x16 *setup; + + setup = (struct fir_coef_32x16 *) config; + fir->mute = 0; + fir->rwi = 0; + fir->length = (int) setup->length; + fir->in_shift = (int) setup->in_shift; + fir->out_shift = (int) setup->out_shift; + fir->coef = &setup->coef; + fir->delay = NULL; + fir->delay_size = 0; + + if ((fir->length > MAX_FIR_LENGTH) || (fir->length < 1)) + return -EINVAL; + + return fir->length; +} + +void fir_init_delay(struct fir_state_32x16 *fir, int16_t config[], + int32_t **data) +{ + fir->delay = *data; + fir->delay_size = fir->length; + *data += fir->delay_size; /* Point to next delay line start */ +} diff --git a/src/audio/fir.h b/src/audio/fir.h new file mode 100644 index 0000000..7044a22 --- /dev/null +++ b/src/audio/fir.h @@ -0,0 +1,130 @@ +/* + * 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 <reef/audio/format.h> + +#define MAX_FIR_LENGTH 192 + +#define NHEADER_FIR_COEF_32x16 3 + +struct fir_coef_32x16 { + int16_t length; /* Number of FIR taps */ + int16_t in_shift; /* Amount of right shifts at input */ + int16_t out_shift; /* Amount of right shifts at output */ + int16_t coef; /* FIR coefficients */ +}; + +struct fir_state_32x16 { + int mute; /* Set to 1 to mute EQ output, 0 otherwise */ + int rwi; /* Circular read and write index */ + int length; /* Number of FIR taps */ + int delay_size; /* Actual delay lentgh, must be >= length */ + int in_shift; /* Amount of right shifts at input */ + int out_shift; /* Amount of right shifts at output */ + int16_t *coef; /* Pointer to FIR coefficients */ + int32_t *delay; /* Pointer to FIR delay line */ +}; + +void fir_reset(struct fir_state_32x16 *fir); + +int fir_init_coef(struct fir_state_32x16 *fir, int16_t config[]); + +void fir_init_delay(struct fir_state_32x16 *fir, int16_t config[], + int32_t **data); + +/* The next trivial functions are inlined */ + +static inline void fir_mute(struct fir_state_32x16 *fir) +{ + fir->mute = 1; +} + +static inline void fir_unmute(struct fir_state_32x16 *fir) +{ + fir->mute = 0; +} + +/* The next functions are inlined to optmize execution speed */ + +static inline void fir_part_32x16(int64_t *y, int taps, const int16_t c[], + int *ic, int32_t d[], int *id) +{ + int n; + + /* Data is Q8.24, coef is Q1.15, product is Q9.39 */ + for (n = 0; n < taps; n++) { + *y += (int64_t) c[*ic] * d[*id]; + (*ic)++; + (*id)--; + } +} + +static inline int32_t fir_32x16(struct fir_state_32x16 *fir, int32_t x) +{ + int64_t y = 0; + int n1; + int n2; + int i = 0; /* Start from 1st tap */ + int tmp_ri; + + /* Write sample to delay */ + fir->delay[fir->rwi] = x >> fir->in_shift; + + /* Start FIR calculation. Calculate first number of taps possible to + * calculate before circular wrap need. + */ + n1 = fir->rwi + 1; + tmp_ri = (fir->rwi)++; /* Point to newest sample and advance read index */ + if (fir->rwi == fir->delay_size) + fir->rwi = 0; + + if (n1 > fir->length) { + /* No need to un-wrap fir read index, make sure ri + * is >= 0 after FIR computation */ + fir_part_32x16(&y, fir->length, fir->coef, &i, fir->delay, &tmp_ri); + } else { + n2 = fir->length - n1; + /* Part 1, loop n1 times, fir_ri becomes -1 */ + fir_part_32x16(&y, n1, fir->coef, &i, fir->delay, &tmp_ri); + + /* Part 2, unwrap fir_ri, continue rest of filter */ + tmp_ri = fir->delay_size - 1; + fir_part_32x16(&y, n2, fir->coef, &i, fir->delay, &tmp_ri); + } + /* Q9.39 -> Q9.24, saturate to Q8.24 */ + y = sat_int32(y >> (15 + fir->out_shift)); + + if (fir->mute) + return 0; + else + return(int32_t) y; +} diff --git a/src/include/reef/audio/component.h b/src/include/reef/audio/component.h index e4f4f8f..7de47c2 100644 --- a/src/include/reef/audio/component.h +++ b/src/include/reef/audio/component.h @@ -76,6 +76,8 @@ #define COMP_CMD_LOOPBACK 105
#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 */
/* MMAP IPC status */ #define COMP_CMD_IPC_MMAP_RPOS 200 /* host read position */ @@ -263,6 +265,7 @@ void sys_comp_src_init(void); void sys_comp_tone_init(void);
+void sys_comp_eq_fir_init(void);
static inline void comp_set_endpoint(struct comp_dev *dev) { diff --git a/src/include/reef/trace.h b/src/include/reef/trace.h index 04b8d5d..ff41caf 100644 --- a/src/include/reef/trace.h +++ b/src/include/reef/trace.h @@ -84,6 +84,7 @@ #define TRACE_CLASS_MUX (16 << 24) #define TRACE_CLASS_SRC (17 << 24) #define TRACE_CLASS_TONE (18 << 24) +#define TRACE_CLASS_EQ_FIR (19 << 24)
/* move to config.h */ #define TRACE 1 diff --git a/src/include/uapi/ipc.h b/src/include/uapi/ipc.h index 7b0fd96..3bb6f61 100644 --- a/src/include/uapi/ipc.h +++ b/src/include/uapi/ipc.h @@ -673,5 +673,17 @@ struct sof_ipc_window { uint32_t num_windows; struct sof_ipc_window_elem window[0]; } __attribute__((packed)); +/* IPC to pass configuration blobs to equalizers and re-assign responses */ +struct sof_ipc_eq_fir_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)); +
#endif diff --git a/src/tasks/audio.c b/src/tasks/audio.c index 8dc42ce..c420fb1 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_fir_init();
#if 0 /* init static pipeline */