From 3c1667fa7c7d81451f3e26c77418038b80071638 Mon Sep 17 00:00:00 2001 From: Sebastiano Carlucci Date: Tue, 7 Apr 2020 07:57:31 -0700 Subject: [PATCH 1/6] sof: eq_iir: modularize iir_df2t() function This commit extracts the biquad processing from iir_df2t(). It allows to reuse the biquad processing code independently of the equalizer implementation. Components such as crossover use biquads for processing input. But it could not do so because iir_df2t() was specific to single output audio processing. The motivation behind this change was to reuse the coefficients of the LR4 biquads for the crossover more freely. An LR4 filter is made of two biquads in series with same coefficients. Therefore to save memory, we can store one copy of each set of coefficient, and pass those to the biquad processing function. Signed-off-by: Sebastiano Carlucci --- src/audio/eq_iir/iir_generic.c | 35 ++--------------------- src/include/sof/audio/eq_iir/iir.h | 45 ++++++++++++++++++++++++++++++ 2 files changed, 47 insertions(+), 33 deletions(-) diff --git a/src/audio/eq_iir/iir_generic.c b/src/audio/eq_iir/iir_generic.c index 9dd47508533f..fde2d79cc63e 100644 --- a/src/audio/eq_iir/iir_generic.c +++ b/src/audio/eq_iir/iir_generic.c @@ -42,13 +42,9 @@ /* 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; - int32_t tmp; - int64_t acc; int32_t out = 0; int i; int j; @@ -63,35 +59,8 @@ int32_t iir_df2t(struct iir_state_df2t *iir, int32_t x) 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 first 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 second 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; - - /* Apply gain Q2.14 x Q1.31 -> Q3.45 */ - acc = ((int64_t)iir->coef[c + 6]) * tmp; /* Gain */ - - /* Apply biquad output shift right parameter - * simultaneously with Q3.45 to Q3.31 conversion. Then - * saturate to 32 bits Q1.31 and prepare for next - * biquad. - */ - acc = Q_SHIFT_RND(acc, 45 + iir->coef[c + 5], 31); - in = sat_int32(acc); - + in = iir_process_biquad(in, &iir->coef[c], + &iir->delay[d]); /* Proceed to next biquad coefficients and delay * lines. */ diff --git a/src/include/sof/audio/eq_iir/iir.h b/src/include/sof/audio/eq_iir/iir.h index f15f1eddac7b..9c6e33d78aff 100644 --- a/src/include/sof/audio/eq_iir/iir.h +++ b/src/include/sof/audio/eq_iir/iir.h @@ -12,6 +12,7 @@ #include #include +#include struct sof_eq_iir_header_df2t; @@ -61,6 +62,50 @@ struct iir_state_df2t { int64_t *delay; /* Pointer to IIR delay line */ }; +/* + * \brief Returns the output of a biquad. + * + * 32 bit data, 32 bit coefficients and 64 bit state variables + * \param in input to the biquad Q1.31 + * \param coef coefficients of the biquad Q2.30 + * \param delay delay of the biquads Q3.61 + */ +static inline int32_t iir_process_biquad(int32_t in, int32_t *coef, + int64_t *delay) +{ + int32_t tmp; + int64_t acc; + + /* Compute output: Delay is Q3.61 + * Q2.30 x Q1.31 -> Q3.61 + * Shift Q3.61 to Q3.31 with rounding + */ + acc = ((int64_t)coef[4]) * in + delay[0]; + tmp = (int32_t)Q_SHIFT_RND(acc, 61, 31); + + /* Compute first delay */ + acc = delay[1]; + acc += ((int64_t)coef[3]) * in; /* Coef b1 */ + acc += ((int64_t)coef[1]) * tmp; /* Coef a1 */ + delay[0] = acc; + + /* Compute second delay */ + acc = ((int64_t)coef[2]) * in; /* Coef b2 */ + acc += ((int64_t)coef[0]) * tmp; /* Coef a2 */ + delay[1] = acc; + + /* Apply gain Q2.14 x Q1.31 -> Q3.45 */ + acc = ((int64_t)coef[6]) * tmp; /* Gain */ + + /* Apply biquad output shift right parameter + * simultaneously with Q3.45 to Q3.31 conversion. Then + * saturate to 32 bits Q1.31 and prepare for next + * biquad. + */ + acc = Q_SHIFT_RND(acc, 45 + coef[5], 31); + return sat_int32(acc); +} + int32_t iir_df2t(struct iir_state_df2t *iir, int32_t x); int iir_init_coef_df2t(struct iir_state_df2t *iir, From 82a9237a90cd779c1f6c849f1cce5a670c01d39a Mon Sep 17 00:00:00 2001 From: Sebastiano Carlucci Date: Thu, 16 Apr 2020 14:30:28 -0700 Subject: [PATCH 2/6] sof: crossover: Add Crossover component This commit adds Crossover to the list of SOF components. A crossover filter can be used to split an input to different frequency bands. The number of outputs should be set statically in the topology. The user then uses the control bytes to route the frequency bands to different outputs. (similar to the demux component). This commit adds support for the following formats: - S16_LE - S24_LE - S32_LE Signed-off-by: Sebastiano Carlucci --- CODEOWNERS | 1 + src/audio/CMakeLists.txt | 3 + src/audio/Kconfig | 8 + src/audio/crossover/CMakeLists.txt | 2 + src/audio/crossover/crossover.c | 858 ++++++++++++++++++++ src/audio/crossover/crossover_generic.c | 291 +++++++ src/audio/eq_iir/iir_generic.c | 3 +- src/include/ipc/topology.h | 1 + src/include/sof/audio/crossover/crossover.h | 160 ++++ src/include/sof/audio/eq_iir/iir.h | 6 +- src/include/user/crossover.h | 104 +++ 11 files changed, 1432 insertions(+), 5 deletions(-) create mode 100644 src/audio/crossover/CMakeLists.txt create mode 100644 src/audio/crossover/crossover.c create mode 100644 src/audio/crossover/crossover_generic.c create mode 100644 src/include/sof/audio/crossover/crossover.h create mode 100644 src/include/user/crossover.h diff --git a/CODEOWNERS b/CODEOWNERS index ecafca17df12..c18fcbeee78d 100644 --- a/CODEOWNERS +++ b/CODEOWNERS @@ -26,6 +26,7 @@ src/audio/tone.c @singalsu src/audio/kpb.c @mrajwa src/audio/mux/* @akloniex src/audio/dcblock* @cujomalainey @dgreid +src/audio/crossover* @cujomalainey @dgreid # platforms src/arch/xtensa/* @tlauda diff --git a/src/audio/CMakeLists.txt b/src/audio/CMakeLists.txt index c423d39261db..0f8aea451ef3 100644 --- a/src/audio/CMakeLists.txt +++ b/src/audio/CMakeLists.txt @@ -24,6 +24,9 @@ if(NOT CONFIG_LIBRARY) if(CONFIG_COMP_DCBLOCK) add_subdirectory(dcblock) endif() + if(CONFIG_COMP_CROSSOVER) + add_subdirectory(crossover) + endif() if(CONFIG_COMP_TONE) add_local_sources(sof tone.c diff --git a/src/audio/Kconfig b/src/audio/Kconfig index 0fa7adb83de0..30d78117db5c 100644 --- a/src/audio/Kconfig +++ b/src/audio/Kconfig @@ -115,6 +115,14 @@ config COMP_SEL help Select for SEL component +config COMP_CROSSOVER + bool "Crossover Filter component" + default y + help + Select for Crossover Filter component. A crossover can be used to + split a signal into two or more frequency ranges, so that the outputs + can be sent to drivers that are designed for those ranges. + config COMP_DCBLOCK bool "DC Blocking Filter component" default y diff --git a/src/audio/crossover/CMakeLists.txt b/src/audio/crossover/CMakeLists.txt new file mode 100644 index 000000000000..15a68035621e --- /dev/null +++ b/src/audio/crossover/CMakeLists.txt @@ -0,0 +1,2 @@ +add_local_sources(sof crossover.c) +add_local_sources(sof crossover_generic.c) diff --git a/src/audio/crossover/crossover.c b/src/audio/crossover/crossover.c new file mode 100644 index 000000000000..97d644d2cf99 --- /dev/null +++ b/src/audio/crossover/crossover.c @@ -0,0 +1,858 @@ +// SPDX-License-Identifier: BSD-3-Clause +// +// Copyright(c) 2020 Google LLC. All rights reserved. +// +// Author: Sebastiano Carlucci + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +static const struct comp_driver comp_crossover; + +/* 948c9ad1-806a-4131-ad6c-b2bda9e35a9f */ +DECLARE_SOF_UUID("crossover", crossover_uuid, 0x948c9ad1, 0x806a, 0x4131, + 0xad, 0x6c, 0xb2, 0xbd, 0xa9, 0xe3, 0x5a, 0x9f); + +DECLARE_TR_CTX(crossover_tr, SOF_UUID(crossover_uuid), LOG_LEVEL_INFO); + +static void crossover_free_config(struct sof_crossover_config **config) +{ + rfree(*config); + *config = NULL; +} + +/** + * \brief Reset the state of an LR4 filter. + */ +static void crossover_reset_state_lr4(struct iir_state_df2t *lr4) +{ + rfree(lr4->delay); + + lr4->coef = NULL; + lr4->delay = NULL; +} + +/** + * \brief Reset the state (coefficients and delay) of the crossover filter + * of a single channel. + */ +static void crossover_reset_state_ch(struct crossover_state *ch_state) +{ + int i; + + for (i = 0; i < CROSSOVER_MAX_LR4; i++) { + crossover_reset_state_lr4(&ch_state->lowpass[i]); + crossover_reset_state_lr4(&ch_state->highpass[i]); + } +} + +/** + * \brief Reset the state (coefficients and delay) of the crossover filter + * across all channels + */ +static void crossover_reset_state(struct comp_data *cd) +{ + int i; + + for (i = 0; i < PLATFORM_MAX_CHANNELS; i++) + crossover_reset_state_ch(&cd->state[i]); +} + +/** + * \brief Returns the index i such that assign_sink[i] = pipe_id. + * + * The assign_sink array in the configuration maps to the pipeline ids. + * + * \return the position at which pipe_id is found in config->assign_sink. + * -1 if not found. + */ +static uint8_t crossover_get_stream_index(struct sof_crossover_config *config, + uint32_t pipe_id) +{ + int i; + uint32_t *assign_sink = config->assign_sink; + + for (i = 0; i < config->num_sinks; i++) + if (assign_sink[i] == pipe_id) + return i; + + comp_cl_err(&comp_crossover, "crossover_get_stream_index() error: couldn't find any assignment for connected pipeline %u", + pipe_id); + + return -1; +} + +/* + * \brief Aligns the sinks with their respective assignments + * in the configuration. + * + * Refer to sof/src/include/sof/crossover.h for more information on assigning + * sinks to an output. + * + * \param[out] sinks array where the sinks are assigned + * \return number of sinks assigned. This number should be equal to + * config->num_sinks if no errors were found. + */ +static int crossover_assign_sinks(struct comp_dev *dev, + struct sof_crossover_config *config, + struct comp_buffer **sinks) +{ + struct comp_buffer *sink; + struct list_item *sink_list; + int num_sinks = 0; + int i; + + // align sink streams with their respective configurations + list_for_item(sink_list, &dev->bsink_list) { + sink = container_of(sink_list, struct comp_buffer, source_list); + if (sink->sink->state == dev->state) { + // if no config is set, then assign the sinks in order + if (!config) { + sinks[num_sinks++] = sink; + continue; + } + + i = crossover_get_stream_index(config, + sink->pipeline_id); + + /* If this sink buffer is not assigned + * in the configuration. + */ + if (i < 0) { + comp_err(dev, "crossover_assign_sinks(), could not find sink %d in config", + sink->pipeline_id); + break; + } + + if (sinks[i]) { + comp_err(dev, "crossover_assign_sinks(), multiple sinks from pipeline %d are assigned", + sink->pipeline_id); + break; + } + + sinks[i] = sink; + num_sinks++; + } + } + + return num_sinks; +} + +/** + * \brief Sets the state of a single LR4 filter. + * + * An LR4 filter is built by cascading two biquads in series. + * + * \param coef struct containing the coefficients of a butterworth + * high/low pass filter. + * \param[out] lr4 initialized struct + */ +static int crossover_init_coef_lr4(struct sof_eq_iir_biquad_df2t *coef, + struct iir_state_df2t *lr4) +{ + /* Reuse the same coefficients for biquads so we only + * store one copy of it. The processing functions will + * feed the same coef array to both biquads. + */ + lr4->coef = (int32_t *)ASSUME_ALIGNED(coef, 4); + + /* LR4 filters are two 2nd order filters, so only need 4 delay slots + * delay[0..1] -> state for first biquad + * delay[2..3] -> state for second biquad + */ + lr4->delay = rzalloc(SOF_MEM_ZONE_RUNTIME, 0, SOF_MEM_CAPS_RAM, + sizeof(uint64_t) * CROSSOVER_NUM_DELAYS_LR4); + if (!lr4->delay) + return -ENOMEM; + + lr4->biquads = 2; + lr4->biquads_in_series = 2; + + return 0; +} + +/** + * \brief Initializes the crossover coefficients for one channel + */ +static int crossover_init_coef_ch(struct sof_eq_iir_biquad_df2t *coef, + struct crossover_state *ch_state, + int32_t num_sinks) +{ + int32_t i; + int32_t j = 0; + int32_t num_lr4s = num_sinks == CROSSOVER_2WAY_NUM_SINKS ? 1 : 3; + int err = 0; + + for (i = 0; i < num_lr4s; i++) { + /* Get the low pass coefficients */ + err = crossover_init_coef_lr4(&coef[j], + &ch_state->lowpass[i]); + if (err < 0) + return -EINVAL; + /* Get the high pass coefficients */ + err = crossover_init_coef_lr4(&coef[j + 1], + &ch_state->highpass[i]); + if (err < 0) + return -EINVAL; + j += 2; + } + + return 0; +} + +/** + * \brief Initializes the coefficients of the crossover filter + * and assign them to the first nch channels. + * + * \param nch number of channels in the audio stream. + */ +static int crossover_init_coef(struct comp_data *cd, int nch) +{ + struct sof_eq_iir_biquad_df2t *crossover; + struct sof_crossover_config *config = cd->config; + int ch, err; + + if (!config) { + comp_cl_err(&comp_crossover, "crossover_init_coef(), no config is set"); + return -EINVAL; + } + + /* Sanity checks */ + if (nch > PLATFORM_MAX_CHANNELS) { + comp_cl_err(&comp_crossover, "crossover_init_coef(), invalid channels count (%i)", + nch); + return -EINVAL; + } + + comp_cl_info(&comp_crossover, "crossover_init_coef(), initiliazing %i-way crossover", + config->num_sinks); + + /* Collect the coef array and assign it to every channel */ + crossover = config->coef; + for (ch = 0; ch < nch; ch++) { + err = crossover_init_coef_ch(crossover, &cd->state[ch], + config->num_sinks); + /* Free all previously allocated blocks in case of an error */ + if (err < 0) { + comp_cl_err(&comp_crossover, "crossover_init_coef(), could not assign coefficients to ch %d", + ch); + crossover_reset_state(cd); + return err; + } + } + + return 0; +} + +/** + * \brief Setup the state, coefficients and processing functions for crossover. + * + * \param dev component device + */ +static int crossover_setup(struct comp_data *cd, int nch) +{ + int ret = 0; + + /* Reset any previous state. */ + crossover_reset_state(cd); + + /* Assign LR4 coefficients from config */ + ret = crossover_init_coef(cd, nch); + + return ret; +} + +/** + * \brief Creates a Crossover Filter component. + * \return Pointer to Crossover Filter component device. + */ +static struct comp_dev *crossover_new(const struct comp_driver *drv, + struct sof_ipc_comp *comp) +{ + struct comp_dev *dev; + struct comp_data *cd; + struct sof_ipc_comp_process *crossover; + struct sof_ipc_comp_process *ipc_crossover = + (struct sof_ipc_comp_process *)comp; + size_t bs = ipc_crossover->size; + int ret; + + comp_cl_info(&comp_crossover, "crossover_new()"); + + /* Check that the coefficients blob size is sane. */ + if (bs > SOF_CROSSOVER_MAX_SIZE) { + comp_cl_err(&comp_crossover, "crossover_new(), blob size (%d) exceeds maximum allowed size (%i)", + bs, SOF_CROSSOVER_MAX_SIZE); + return NULL; + } + + dev = comp_alloc(drv, COMP_SIZE(struct sof_ipc_comp_process)); + if (!dev) + return NULL; + + crossover = COMP_GET_IPC(dev, sof_ipc_comp_process); + ret = memcpy_s(crossover, sizeof(*crossover), ipc_crossover, + sizeof(struct sof_ipc_comp_process)); + assert(!ret); + + cd = rzalloc(SOF_MEM_ZONE_RUNTIME, 0, + SOF_MEM_CAPS_RAM, sizeof(*cd)); + if (!cd) { + rfree(dev); + return NULL; + } + + comp_set_drvdata(dev, cd); + + cd->crossover_process = NULL; + cd->crossover_split = NULL; + cd->config = NULL; + cd->config_new = NULL; + + if (bs) { + cd->config = rzalloc(SOF_MEM_ZONE_RUNTIME, 0, + SOF_MEM_CAPS_RAM, bs); + if (!cd->config) { + rfree(dev); + rfree(cd); + return NULL; + } + + ret = memcpy_s(cd->config, bs, ipc_crossover->data, bs); + assert(!ret); + } + + dev->state = COMP_STATE_READY; + return dev; +} + +/** + * \brief Frees Crossover Filter component. + */ +static void crossover_free(struct comp_dev *dev) +{ + struct comp_data *cd = comp_get_drvdata(dev); + + comp_info(dev, "crossover_free()"); + + crossover_free_config(&cd->config); + crossover_free_config(&cd->config_new); + + crossover_reset_state(cd); + + rfree(cd); + rfree(dev); +} + +/** + * \brief Verifies that the config is formatted correctly. + * + * The function can only be called after the buffers have been initialized. + */ +static int crossover_validate_config(struct comp_dev *dev, + struct sof_crossover_config *config) +{ + struct comp_buffer *sink; + struct list_item *sink_list; + uint32_t size = config->size; + int32_t num_assigned_sinks = 0; + uint8_t assigned_sinks[SOF_CROSSOVER_MAX_STREAMS] = {0}; + int i; + + if (size > SOF_CROSSOVER_MAX_SIZE || !size) { + comp_err(dev, "crossover_validate_config(), size %d is invalid", + size); + return -EINVAL; + } + + if (config->num_sinks > SOF_CROSSOVER_MAX_STREAMS || + config->num_sinks < 2) { + comp_err(dev, "crossover_validate_config(), invalid num_sinks %i, expected number between 2 and %i", + config->num_sinks, SOF_CROSSOVER_MAX_STREAMS); + return -EINVAL; + } + + // Align the crossover's sinks, to their respective configuation in + // the config. + list_for_item(sink_list, &dev->bsink_list) { + sink = container_of(sink_list, struct comp_buffer, source_list); + i = crossover_get_stream_index(config, sink->pipeline_id); + if (i < 0) { + comp_warn(dev, "crossover_validate_config(), could not assign sink %d", + sink->pipeline_id); + break; + } + + if (assigned_sinks[i]) { + comp_warn(dev, "crossover_validate_config(), multiple sinks from pipeline %d are assigned", + sink->pipeline_id); + break; + } + + assigned_sinks[i] = true; + num_assigned_sinks++; + } + + // config is invalid if the number of assigned sinks + // is different than what is configured + if (num_assigned_sinks != config->num_sinks) { + comp_err(dev, "crossover_validate_config(), number of assigned sinks %d, expected from config %d", + num_assigned_sinks, config->num_sinks); + return -EINVAL; + } + + return 0; +} + +static int crossover_verify_params(struct comp_dev *dev, + struct sof_ipc_stream_params *params) +{ + int ret; + + comp_dbg(dev, "crossover_verify_params()"); + + ret = comp_verify_params(dev, 0, params); + if (ret < 0) { + comp_err(dev, "crossover_verify_params() error: comp_verify_params() failed."); + return ret; + } + + return 0; +} + +/** + * \brief Sets Crossover Filter component audio stream parameters. + * \param[in,out] dev Crossover Filter base component device. + * \return Error code. + */ +static int crossover_params(struct comp_dev *dev, + struct sof_ipc_stream_params *params) +{ + int err = 0; + + comp_dbg(dev, "crossover_params()"); + + err = crossover_verify_params(dev, params); + if (err < 0) + comp_err(dev, "crossover_params(): pcm params verification failed"); + + return err; +} + +static int crossover_cmd_set_data(struct comp_dev *dev, + struct sof_ipc_ctrl_data *cdata) +{ + struct comp_data *cd = comp_get_drvdata(dev); + struct sof_crossover_config *request; + uint32_t bs; + int ret = 0; + + switch (cdata->cmd) { + case SOF_CTRL_CMD_BINARY: + comp_info(dev, "crossover_cmd_set_data(), SOF_CTRL_CMD_BINARY"); + + /* Find size from header */ + request = (struct sof_crossover_config *)ASSUME_ALIGNED(cdata->data->data, 4); + bs = request->size; + + /* Check that there is no work-in-progress previous request */ + if (cd->config_new) { + comp_err(dev, "crossover_cmd_set_data(), busy with previous"); + return -EBUSY; + } + + /* Allocate and make a copy of the blob */ + cd->config_new = rzalloc(SOF_MEM_ZONE_RUNTIME, 0, + SOF_MEM_CAPS_RAM, bs); + if (!cd->config_new) { + comp_err(dev, "crossover_cmd_set_data(), alloc fail"); + return -EINVAL; + } + + /* Copy the configuration. If the component state is ready + * the Crossover will initialize in prepare(). + */ + ret = memcpy_s(cd->config_new, bs, request, bs); + assert(!ret); + + /* If component state is READY we can omit old configuration + * immediately. When in playback/capture the new configuration + * presence is checked in copy(). + */ + if (dev->state == COMP_STATE_READY) + crossover_free_config(&cd->config); + + /* If there is no existing configuration the received can + * be set to current immediately. It will be applied in + * prepare() when streaming starts. + */ + if (!cd->config) { + cd->config = cd->config_new; + cd->config_new = NULL; + } + + break; + default: + comp_err(dev, "crossover_cmd_set_data(), invalid command"); + ret = -EINVAL; + break; + } + + return ret; +} + +static int crossover_cmd_get_data(struct comp_dev *dev, + struct sof_ipc_ctrl_data *cdata, int max_size) +{ + struct comp_data *cd = comp_get_drvdata(dev); + uint32_t bs; + int ret = 0; + + switch (cdata->cmd) { + case SOF_CTRL_CMD_BINARY: + comp_info(dev, "crossover_cmd_get_data(), SOF_CTRL_CMD_BINARY"); + + /* Copy back to user space */ + if (cd->config) { + bs = cd->config->size; + comp_info(dev, "crossover_cmd_get_data(), size %u", + bs); + if (bs > SOF_CROSSOVER_MAX_SIZE || bs == 0 || + bs > max_size) + return -EINVAL; + ret = memcpy_s(cdata->data->data, + cdata->data->size, + cd->config, bs); + assert(!ret); + + cdata->data->abi = SOF_ABI_VERSION; + cdata->data->size = bs; + } else { + comp_err(dev, "crossover_cmd_get_data(), no config"); + ret = -EINVAL; + } + break; + default: + comp_err(dev, "crossover_cmd_get_data(), invalid command"); + ret = -EINVAL; + break; + } + return ret; +} + +/** + * \brief Handles incoming IPC commands for Crossover component. + */ +static int crossover_cmd(struct comp_dev *dev, int cmd, void *data, + int max_data_size) +{ + struct sof_ipc_ctrl_data *cdata = data; + int ret = 0; + + comp_info(dev, "crossover_cmd()"); + + switch (cmd) { + case COMP_CMD_SET_DATA: + ret = crossover_cmd_set_data(dev, cdata); + break; + case COMP_CMD_GET_DATA: + ret = crossover_cmd_get_data(dev, cdata, max_data_size); + break; + default: + comp_err(dev, "crossover_cmd(), invalid command"); + ret = -EINVAL; + } + + return ret; +} + +/** + * \brief Sets Crossover Filter component state. + * \param[in,out] dev Crossover Filter base component device. + * \param[in] cmd Command type. + * \return Error code. + */ +static int crossover_trigger(struct comp_dev *dev, int cmd) +{ + comp_info(dev, "crossover_trigger()"); + + return comp_set_state(dev, cmd); +} + +/** + * \brief Copies and processes stream data. + * \param[in,out] dev Crossover Filter base component device. + * \return Error code. + */ +static int crossover_copy(struct comp_dev *dev) +{ + struct comp_data *cd = comp_get_drvdata(dev); + struct comp_buffer *source; + struct comp_buffer *sinks[SOF_CROSSOVER_MAX_STREAMS] = { NULL }; + int i, ret; + uint32_t num_sinks; + uint32_t num_assigned_sinks = 0; + uint32_t frames = UINT_MAX; + uint32_t source_bytes, avail; + uint32_t flags = 0; + uint32_t sinks_bytes[SOF_CROSSOVER_MAX_STREAMS] = { 0 }; + + comp_dbg(dev, "crossover_copy()"); + + source = list_first_item(&dev->bsource_list, struct comp_buffer, + sink_list); + + /* Check for changed configuration */ + if (cd->config_new) { + crossover_free_config(&cd->config); + cd->config = cd->config_new; + cd->config_new = NULL; + ret = crossover_setup(cd, source->stream.channels); + if (ret < 0) { + comp_err(dev, "crossover_copy(), setup failed"); + return -EINVAL; + } + } + + /* Use the assign_sink array from the config to route + * the output to the corresponding sinks. + * It is possible for an assigned sink to be in a different + * state than the component. Therefore not all sinks are guaranteed + * to be assigned: sink[i] can be NULL, 0 <= i <= config->num_sinks + */ + num_assigned_sinks = crossover_assign_sinks(dev, cd->config, sinks); + if (cd->config && num_assigned_sinks != cd->config->num_sinks) + comp_dbg(dev, "crossover_copy(), number of assigned sinks (%i) does not match number of sinks in config (%i).", + num_assigned_sinks, cd->config->num_sinks); + + /* If no config is set then assign the number of sinks to the number + * of sinks that were assigned + */ + if (cd->config) + num_sinks = cd->config->num_sinks; + else + num_sinks = num_assigned_sinks; + + buffer_lock(source, &flags); + + /* check if source is active */ + if (source->source->state != dev->state) { + buffer_unlock(source, flags); + return -EINVAL; + } + + /* Find the number of frames to copy over */ + for (i = 0; i < num_sinks; i++) { + if (!sinks[i]) + continue; + buffer_lock(sinks[i], &flags); + avail = audio_stream_avail_frames(&source->stream, + &sinks[i]->stream); + frames = MIN(frames, avail); + buffer_unlock(sinks[i], flags); + } + + buffer_unlock(source, flags); + + source_bytes = frames * audio_stream_frame_bytes(&source->stream); + + for (i = 0; i < num_sinks; i++) { + if (!sinks[i]) + continue; + sinks_bytes[i] = frames * + audio_stream_frame_bytes(&sinks[i]->stream); + } + + /* Process crossover */ + buffer_invalidate(source, source_bytes); + cd->crossover_process(dev, source, sinks, num_sinks, frames); + + for (i = 0; i < num_sinks; i++) { + if (!sinks[i]) + continue; + buffer_writeback(sinks[i], sinks_bytes[i]); + comp_update_buffer_produce(sinks[i], sinks_bytes[i]); + } + comp_update_buffer_consume(source, source_bytes); + + return 0; +} + +/** + * \brief Prepares Crossover Filter component for processing. + * \param[in,out] dev Crossover Filter base component device. + * \return Error code. + */ +static int crossover_prepare(struct comp_dev *dev) +{ + struct comp_data *cd = comp_get_drvdata(dev); + struct sof_ipc_comp_config *config = dev_comp_config(dev); + struct comp_buffer *source, *sink; + struct list_item *sink_list; + int32_t sink_period_bytes; + int ret; + + comp_info(dev, "crossover_prepare()"); + + ret = comp_set_state(dev, COMP_TRIGGER_PREPARE); + if (ret < 0) + return ret; + + if (ret == COMP_STATUS_STATE_ALREADY_SET) + return PPL_STATUS_PATH_STOP; + + /* Crossover has a variable number of sinks.*/ + source = list_first_item(&dev->bsource_list, + struct comp_buffer, sink_list); + + /* get source data format */ + cd->source_format = source->stream.frame_fmt; + + /* Validate frame format and buffer size of sinks */ + list_for_item(sink_list, &dev->bsink_list) { + sink = container_of(sink_list, struct comp_buffer, source_list); + if (cd->source_format != sink->stream.frame_fmt) { + comp_err(dev, "crossover_prepare(): Source fmt %d and sink fmt %d are different for sink %d.", + cd->source_format, sink->stream.frame_fmt, + sink->pipeline_id); + ret = -EINVAL; + goto err; + } + + sink_period_bytes = audio_stream_period_bytes(&sink->stream, + dev->frames); + if (sink->stream.size < + config->periods_sink * sink_period_bytes) { + comp_err(dev, "crossover_prepare(), sink %d buffer size %d is insufficient", + sink->pipeline_id, sink->stream.size); + ret = -ENOMEM; + goto err; + } + } + + comp_info(dev, "crossover_prepare(), source_format=%d, sink_formats=%d, nch=%d", + cd->source_format, cd->source_format, + source->stream.channels); + + /* Initialize Crossover */ + if (cd->config && crossover_validate_config(dev, cd->config) < 0) { + /* If config is invalid then delete it.*/ + comp_err(dev, "crossover_prepare(), invalid binary config format"); + crossover_free_config(&cd->config); + } + + if (cd->config) { + ret = crossover_setup(cd, source->stream.channels); + if (ret < 0) { + comp_err(dev, "crossover_prepare(), setup failed"); + goto err; + } + + cd->crossover_process = + crossover_find_proc_func(cd->source_format); + if (!cd->crossover_process) { + comp_err(dev, "crossover_prepare(), No processing function matching frame_fmt %i", + cd->source_format); + ret = -EINVAL; + goto err; + } + + cd->crossover_split = + crossover_find_split_func(cd->config->num_sinks); + if (!cd->crossover_split) { + comp_err(dev, "crossover_prepare(), No split function matching num_sinks %i", + cd->config->num_sinks); + ret = -EINVAL; + goto err; + } + } else { + comp_info(dev, "crossover_prepare(), setting crossover to passthrough mode"); + + cd->crossover_process = + crossover_find_proc_func_pass(cd->source_format); + + if (!cd->crossover_process) { + comp_err(dev, "crossover_prepare(), No passthrough function matching frame_fmt %i", + cd->source_format); + ret = -EINVAL; + goto err; + } + } + + return 0; + +err: + comp_set_state(dev, COMP_TRIGGER_RESET); + return ret; +} + +/** + * \brief Resets Crossover Filter component. + * \param[in,out] dev Crossover Filter base component device. + * \return Error code. + */ +static int crossover_reset(struct comp_dev *dev) +{ + struct comp_data *cd = comp_get_drvdata(dev); + + comp_info(dev, "crossover_reset()"); + + crossover_reset_state(cd); + + comp_set_state(dev, COMP_TRIGGER_RESET); + + return 0; +} + +/** \brief Crossover Filter component definition. */ +static const struct comp_driver comp_crossover = { + .type = SOF_COMP_CROSSOVER, + .uid = SOF_UUID(crossover_uuid), + .tctx = &crossover_tr, + .ops = { + .create = crossover_new, + .free = crossover_free, + .params = crossover_params, + .cmd = crossover_cmd, + .trigger = crossover_trigger, + .copy = crossover_copy, + .prepare = crossover_prepare, + .reset = crossover_reset, + }, +}; + +static SHARED_DATA struct comp_driver_info comp_crossover_info = { + .drv = &comp_crossover, +}; + +static void sys_comp_crossover_init(void) +{ + comp_register(platform_shared_get(&comp_crossover_info, + sizeof(comp_crossover_info))); +} + +DECLARE_MODULE(sys_comp_crossover_init); diff --git a/src/audio/crossover/crossover_generic.c b/src/audio/crossover/crossover_generic.c new file mode 100644 index 000000000000..f25280762057 --- /dev/null +++ b/src/audio/crossover/crossover_generic.c @@ -0,0 +1,291 @@ +// SPDX-License-Identifier: BSD-3-Clause +// +// Copyright(c) 2020 Google LLC. All rights reserved. +// +// Author: Sebastiano Carlucci + +#include +#include +#include +#include +#include +#include + +/* + * \brief Splits x into two based on the coefficients set in the lp + * and hp filters. The output of the lp is in y1, the output of + * the hp is in y2. + * + * As a side effect, this function mutates the delay values of both + * filters. + */ +static void crossover_generic_lr4_split(struct iir_state_df2t *lp, + struct iir_state_df2t *hp, + int32_t x, int32_t *y1, int32_t *y2) +{ + *y1 = crossover_generic_process_lr4(x, lp); + *y2 = crossover_generic_process_lr4(x, hp); +} + +/* + * \brief Splits input signal into two and merges it back to it's + * original form. + * + * With 3-way crossovers, one output goes through only one LR4 filter, + * whereas the other two go through two LR4 filters. This causes the signals + * to be out of phase. We need to pass the signal through another set of LR4 + * filters to align back the phase. + */ +static void crossover_generic_lr4_merge(struct iir_state_df2t *lp, + struct iir_state_df2t *hp, + int32_t x, int32_t *y) +{ + int32_t z1, z2; + + z1 = crossover_generic_process_lr4(x, lp); + z2 = crossover_generic_process_lr4(x, hp); + *y = sat_int32(((int64_t)z1) + z2); +} + +static void crossover_generic_split_2way(int32_t in, + int32_t out[], + struct crossover_state *state) +{ + crossover_generic_lr4_split(&state->lowpass[0], &state->highpass[0], + in, &out[0], &out[1]); +} + +static void crossover_generic_split_3way(int32_t in, + int32_t out[], + struct crossover_state *state) +{ + int32_t z1, z2; + + crossover_generic_lr4_split(&state->lowpass[0], &state->highpass[0], + in, &z1, &z2); + /* Realign the phase of z1 */ + crossover_generic_lr4_merge(&state->lowpass[1], &state->highpass[1], + z1, &out[0]); + crossover_generic_lr4_split(&state->lowpass[2], &state->highpass[2], + z2, &out[1], &out[2]); +} + +static void crossover_generic_split_4way(int32_t in, + int32_t out[], + struct crossover_state *state) +{ + int32_t z1, z2; + + crossover_generic_lr4_split(&state->lowpass[1], &state->highpass[1], + in, &z1, &z2); + crossover_generic_lr4_split(&state->lowpass[0], &state->highpass[0], + z1, &out[0], &out[1]); + crossover_generic_lr4_split(&state->lowpass[2], &state->highpass[2], + z2, &out[2], &out[3]); +} + +#if CONFIG_FORMAT_S16LE +static void crossover_s16_default_pass(const struct comp_dev *dev, + const struct comp_buffer *source, + struct comp_buffer *sinks[], + int32_t num_sinks, + uint32_t frames) +{ + const struct audio_stream *source_stream = &source->stream; + int16_t *x; + int32_t *y; + int i, j; + int n = source_stream->channels * frames; + + for (i = 0; i < n; i++) { + x = audio_stream_read_frag_s16(source_stream, i); + for (j = 0; j < num_sinks; j++) { + if (!sinks[j]) + continue; + y = audio_stream_read_frag_s16((&sinks[j]->stream), i); + *y = *x; + } + } +} +#endif // CONFIG_FORMAT_S16LE + +#if CONFIG_FORMAT_S24LE || CONFIG_FORMAT_S32LE +static void crossover_s32_default_pass(const struct comp_dev *dev, + const struct comp_buffer *source, + struct comp_buffer *sinks[], + int32_t num_sinks, + uint32_t frames) +{ + const struct audio_stream *source_stream = &source->stream; + int32_t *x, *y; + int i, j; + int n = source_stream->channels * frames; + + for (i = 0; i < n; i++) { + x = audio_stream_read_frag_s32(source_stream, i); + for (j = 0; j < num_sinks; j++) { + if (!sinks[j]) + continue; + y = audio_stream_read_frag_s32((&sinks[j]->stream), i); + *y = *x; + } + } +} +#endif // CONFIG_FORMAT_S24LE || CONFIG_FORMAT_S32LE + +#if CONFIG_FORMAT_S16LE +static void crossover_s16_default(const struct comp_dev *dev, + const struct comp_buffer *source, + struct comp_buffer *sinks[], + int32_t num_sinks, + uint32_t frames) +{ + struct comp_data *cd = comp_get_drvdata(dev); + struct crossover_state *state; + const struct audio_stream *source_stream = &source->stream; + struct audio_stream *sink_stream; + int16_t *x, *y; + int ch, i, j; + int idx = 0; + int nch = source_stream->channels; + int32_t out[num_sinks]; + + for (ch = 0; ch < nch; ch++) { + idx = ch; + state = &cd->state[ch]; + for (i = 0; i < frames; i++) { + x = audio_stream_read_frag_s16(source_stream, idx); + cd->crossover_split(*x << 16, out, state); + + for (j = 0; j < num_sinks; j++) { + if (!sinks[j]) + continue; + sink_stream = &sinks[j]->stream; + y = audio_stream_read_frag_s16(sink_stream, + idx); + *y = sat_int16(Q_SHIFT_RND(out[j], 31, 15)); + } + + idx += nch; + } + } +} +#endif // CONFIG_FORMAT_S16LE + +#if CONFIG_FORMAT_S24LE +static void crossover_s24_default(const struct comp_dev *dev, + const struct comp_buffer *source, + struct comp_buffer *sinks[], + int32_t num_sinks, + uint32_t frames) +{ + struct comp_data *cd = comp_get_drvdata(dev); + struct crossover_state *state; + const struct audio_stream *source_stream = &source->stream; + struct audio_stream *sink_stream; + int32_t *x, *y; + int ch, i, j; + int idx = 0; + int nch = source_stream->channels; + int32_t out[num_sinks]; + + for (ch = 0; ch < nch; ch++) { + idx = ch; + state = &cd->state[ch]; + for (i = 0; i < frames; i++) { + x = audio_stream_read_frag_s32(source_stream, idx); + cd->crossover_split(*x << 8, out, state); + + for (j = 0; j < num_sinks; j++) { + if (!sinks[j]) + continue; + sink_stream = &sinks[j]->stream; + y = audio_stream_read_frag_s32(sink_stream, + idx); + *y = sat_int24(Q_SHIFT_RND(out[j], 31, 23)); + } + + idx += nch; + } + } +} +#endif // CONFIG_FORMAT_S24LE + +#if CONFIG_FORMAT_S32LE +static void crossover_s32_default(const struct comp_dev *dev, + const struct comp_buffer *source, + struct comp_buffer *sinks[], + int32_t num_sinks, + uint32_t frames) +{ + struct comp_data *cd = comp_get_drvdata(dev); + struct crossover_state *state; + const struct audio_stream *source_stream = &source->stream; + struct audio_stream *sink_stream; + int32_t *x, *y; + int ch, i, j; + int idx = 0; + int nch = source_stream->channels; + int32_t out[num_sinks]; + + for (ch = 0; ch < nch; ch++) { + idx = ch; + state = &cd->state[0]; + for (i = 0; i < frames; i++) { + x = audio_stream_read_frag_s32(source_stream, idx); + cd->crossover_split(*x, out, state); + + for (j = 0; j < num_sinks; j++) { + if (!sinks[j]) + continue; + sink_stream = &sinks[j]->stream; + y = audio_stream_read_frag_s32(sink_stream, + idx); + *y = out[j]; + } + + idx += nch; + } + } +} +#endif // CONFIG_FORMAT_S32LE + +const struct crossover_proc_fnmap crossover_proc_fnmap[] = { +/* { SOURCE_FORMAT , PROCESSING FUNCTION } */ +#if CONFIG_FORMAT_S16LE + { SOF_IPC_FRAME_S16_LE, crossover_s16_default }, +#endif /* CONFIG_FORMAT_S16LE */ + +#if CONFIG_FORMAT_S24LE + { SOF_IPC_FRAME_S24_4LE, crossover_s24_default }, +#endif /* CONFIG_FORMAT_S24LE */ + +#if CONFIG_FORMAT_S32LE + { SOF_IPC_FRAME_S32_LE, crossover_s32_default }, +#endif /* CONFIG_FORMAT_S32LE */ +}; + +const struct crossover_proc_fnmap crossover_proc_fnmap_pass[] = { +/* { SOURCE_FORMAT , PROCESSING FUNCTION } */ +#if CONFIG_FORMAT_S16LE + { SOF_IPC_FRAME_S16_LE, crossover_s16_default_pass }, +#endif /* CONFIG_FORMAT_S16LE */ + +#if CONFIG_FORMAT_S24LE + { SOF_IPC_FRAME_S24_4LE, crossover_s32_default_pass }, +#endif /* CONFIG_FORMAT_S24LE */ + +#if CONFIG_FORMAT_S32LE + { SOF_IPC_FRAME_S32_LE, crossover_s32_default_pass }, +#endif /* CONFIG_FORMAT_S32LE */ +}; + +const size_t crossover_proc_fncount = ARRAY_SIZE(crossover_proc_fnmap); + +const crossover_split crossover_split_fnmap[] = { + crossover_generic_split_2way, + crossover_generic_split_3way, + crossover_generic_split_4way, +}; + +const size_t crossover_split_fncount = ARRAY_SIZE(crossover_split_fnmap); diff --git a/src/audio/eq_iir/iir_generic.c b/src/audio/eq_iir/iir_generic.c index fde2d79cc63e..dadce43faf8f 100644 --- a/src/audio/eq_iir/iir_generic.c +++ b/src/audio/eq_iir/iir_generic.c @@ -59,8 +59,7 @@ int32_t iir_df2t(struct iir_state_df2t *iir, int32_t x) in = x; for (j = 0; j < iir->biquads; j += iir->biquads_in_series) { for (i = 0; i < iir->biquads_in_series; i++) { - in = iir_process_biquad(in, &iir->coef[c], - &iir->delay[d]); + in = iir_df2t_biquad(in, &iir->coef[c], &iir->delay[d]); /* Proceed to next biquad coefficients and delay * lines. */ diff --git a/src/include/ipc/topology.h b/src/include/ipc/topology.h index 58c9074ab0ab..9f69e09ffa76 100644 --- a/src/include/ipc/topology.h +++ b/src/include/ipc/topology.h @@ -47,6 +47,7 @@ enum sof_comp_type { SOF_COMP_ASRC, /**< Asynchronous sample rate converter */ SOF_COMP_DCBLOCK, SOF_COMP_SMART_AMP, /**< smart amplifier component */ + SOF_COMP_CROSSOVER, /* keep FILEREAD/FILEWRITE as the last ones */ SOF_COMP_FILEREAD = 10000, /**< host test based file IO */ SOF_COMP_FILEWRITE = 10001, /**< host test based file IO */ diff --git a/src/include/sof/audio/crossover/crossover.h b/src/include/sof/audio/crossover/crossover.h new file mode 100644 index 000000000000..8ebf70d3bf1d --- /dev/null +++ b/src/include/sof/audio/crossover/crossover.h @@ -0,0 +1,160 @@ +/* SPDX-License-Identifier: BSD-3-Clause + * + * Copyright(c) 2020 Google LLC. All rights reserved. + * + * Author: Sebastiano Carlucci + */ +#ifndef __SOF_AUDIO_CROSSOVER_CROSSOVER_H__ +#define __SOF_AUDIO_CROSSOVER_CROSSOVER_H__ + +#include +#include +#include +#include + +struct comp_buffer; +struct comp_dev; + +/* Maximum number of LR4 highpass OR lowpass filters */ +#define CROSSOVER_MAX_LR4 3 +/* Number of delay slots allocated for LR4 Filters */ +#define CROSSOVER_NUM_DELAYS_LR4 4 + +/* Number of sinks for a 2 way crossover filter */ +#define CROSSOVER_2WAY_NUM_SINKS 2 +/* Number of sinks for a 3 way crossover filter */ +#define CROSSOVER_3WAY_NUM_SINKS 3 +/* Number of sinks for a 4 way crossover filter */ +#define CROSSOVER_4WAY_NUM_SINKS 4 + +/** + * The Crossover filter will have from 2 to 4 outputs. + * Diagram of a 4-way Crossover filter (6 LR4 Filters). + * + * o---- LR4 LO-PASS --> y1(n) + * | + * o--- LR4 LO-PASS --o + * | | + * | o--- LR4 HI-PASS --> y2(n) + * x(n) --- o + * | o--- LR4 LO-PASS --> y3(n) + * | | + * o--- LR4 HI-PASS --o + * | + * o--- LR4 HI-PASS --> y4(n) + * + * Refer to include/user/crossover.h for diagrams of 2-way and 3-way crossovers + * The low and high pass LR4 filters have opposite phase responses, causing + * the intermediary outputs to be out of phase by 180 degrees. + * For 2-way and 3-way, the phases of the signals need to be synchronized. + * + * Each LR4 is made of two butterworth filters in series with the same params. + * + * x(n) --> BIQUAD --> z(n) --> BIQUAD --> y(n) + * + * In total, we keep track of the state of at most 6 IIRs each made of two + * biquads in series. + * + */ + +/** + * Stores the state of one channel of the Crossover filter + */ +struct crossover_state { + /* Store the state for each LR4 filter. */ + struct iir_state_df2t lowpass[CROSSOVER_MAX_LR4]; + struct iir_state_df2t highpass[CROSSOVER_MAX_LR4]; +}; + +typedef void (*crossover_process)(const struct comp_dev *dev, + const struct comp_buffer *source, + struct comp_buffer *sinks[], + int32_t num_sinks, + uint32_t frames); + +typedef void (*crossover_split)(int32_t in, int32_t out[], + struct crossover_state *state); + +/* Crossover component private data */ +struct comp_data { + /**< filter state */ + struct crossover_state state[PLATFORM_MAX_CHANNELS]; + struct sof_crossover_config *config; /**< pointer to setup blob */ + struct sof_crossover_config *config_new; /**< pointer to new setup */ + enum sof_ipc_frame source_format; /**< source frame format */ + crossover_process crossover_process; /**< processing function */ + crossover_split crossover_split; /**< split function */ +}; + +struct crossover_proc_fnmap { + enum sof_ipc_frame frame_fmt; + crossover_process crossover_proc_func; +}; + +extern const struct crossover_proc_fnmap crossover_proc_fnmap[]; +extern const struct crossover_proc_fnmap crossover_proc_fnmap_pass[]; +extern const size_t crossover_proc_fncount; + +/** + * \brief Returns Crossover processing function. + */ +static inline crossover_process + crossover_find_proc_func(enum sof_ipc_frame src_fmt) +{ + int i; + + /* Find suitable processing function from map */ + for (i = 0; i < crossover_proc_fncount; i++) + if (src_fmt == crossover_proc_fnmap[i].frame_fmt) + return crossover_proc_fnmap[i].crossover_proc_func; + + return NULL; +} + +/** + * \brief Returns Crossover passthrough functions. + */ +static inline crossover_process + crossover_find_proc_func_pass(enum sof_ipc_frame src_fmt) +{ + int i; + + /* Find suitable processing function from map */ + for (i = 0; i < crossover_proc_fncount; i++) + if (src_fmt == crossover_proc_fnmap_pass[i].frame_fmt) + return crossover_proc_fnmap_pass[i].crossover_proc_func; + + return NULL; +} + +extern const crossover_split crossover_split_fnmap[]; +extern const size_t crossover_split_fncount; + +/** + * \brief Returns Crossover split function. + */ +static inline crossover_split crossover_find_split_func(int32_t num_sinks) +{ + if (num_sinks < CROSSOVER_2WAY_NUM_SINKS && + num_sinks > CROSSOVER_4WAY_NUM_SINKS) + return NULL; + // The functions in the map are offset by 2 indices. + return crossover_split_fnmap[num_sinks - CROSSOVER_2WAY_NUM_SINKS]; +} + +/* + * \brief Runs input in through the LR4 filter and returns it's output. + */ +static inline int32_t crossover_generic_process_lr4(int32_t in, + struct iir_state_df2t *lr4) +{ + int i; + + /* Cascade two biquads with same coefficients in series. */ + for (i = 0; i < lr4->biquads_in_series; i++) + in = iir_df2t_biquad(in, lr4->coef, &lr4->delay[2 * i]); + + return in; +} + +#endif // __SOF_AUDIO_CROSSOVER_CROSSOVER_H__ diff --git a/src/include/sof/audio/eq_iir/iir.h b/src/include/sof/audio/eq_iir/iir.h index 9c6e33d78aff..6ce53ecc41f2 100644 --- a/src/include/sof/audio/eq_iir/iir.h +++ b/src/include/sof/audio/eq_iir/iir.h @@ -63,15 +63,15 @@ struct iir_state_df2t { }; /* - * \brief Returns the output of a biquad. + * \brief Returns the output of a biquad using its df2t form. * * 32 bit data, 32 bit coefficients and 64 bit state variables * \param in input to the biquad Q1.31 * \param coef coefficients of the biquad Q2.30 * \param delay delay of the biquads Q3.61 */ -static inline int32_t iir_process_biquad(int32_t in, int32_t *coef, - int64_t *delay) +static inline int32_t iir_df2t_biquad(int32_t in, int32_t *coef, + int64_t *delay) { int32_t tmp; int64_t acc; diff --git a/src/include/user/crossover.h b/src/include/user/crossover.h new file mode 100644 index 000000000000..e0ae8ee9e174 --- /dev/null +++ b/src/include/user/crossover.h @@ -0,0 +1,104 @@ +/* SPDX-License-Identifier: BSD-3-Clause + * + * Copyright(c) 2020 Google LLC. All rights reserved. + * + * Author: Sebastiano Carlucci + */ + +#ifndef __USER_CROSSOVER_H__ +#define __USER_CROSSOVER_H__ + +#include +#include + +/* Maximum Number of sinks allowed in config */ +#define SOF_CROSSOVER_MAX_STREAMS 4 + +/* Maximum number allowed in configuration blob */ +#define SOF_CROSSOVER_MAX_SIZE 1024 + + /* crossover_configuration + * uint32_t number_of_sinks <= 4 + * 1=passthrough, n=n-way cossover. + * uint32_t assign_sinks[SOF_CROSSOVER_MAX_STREAMS] + * Mapping between sink positions and the sink's pipeline id. To assign + * output j to sink i, set: assign_sinks[j] = i. Refer to the diagrams + * below for more information on the sink outputs. + * + * 4-way: + * o---- LR4 LP0 --> LOW assign_sink[0] + * | + * o--- LR4 LP1 --o + * | | + * | o---- LR4 HP0 --> MID_LOW assign_sink[1] + * x(n) --- o + * | o---- LR4 LP2 --> MID_HIGH assign_sink[2] + * | | + * o--- LR4 HP1 --o + * | + * o---- LR4 HP2 --> HIGH assign_sink[3] + * + * Merging is necessary for 3way to adjsut the phase of the + * outputs. + * 3-way: + * o---- LR4 LP1 --o + * | | + * o--- LR4 LP0 --o +-> LOW assign_sink[0] + * | | | + * | o---- LR4 HP1 --o + * x(n) --- o + * | o---- LR4 LP2 -----> MID assign_sink[1] + * | | + * o--- LR4 HP0 --o + * | + * o---- LR4 HP2 -----> HIGH assign_sink[2] + * + * 2-way: + * o--- LR4 LP0 ---> LOW assign_sink[0] + * | + * x(n) --- o + * | + * o--- LR4 HP0 ---> HIGH assign_sink[1] + * + * struct sof_eq_iir_biquad_df2t coef[(num_sinks - 1)*2] + * The coefficients data for the LR4s. Depending on many + * sinks are set, the number entries of this field can vary. + * Each entry of the array defines the coefficients for one biquad + * of the LR4 filter. The order the coefficients are assigned is: + * [LR4 LP0, LR4 HP0, LR4 LP1, LR4 HP1, LR4 LP2, LR4 HP2]. + * coef[0] = coefficients of LR4 LP0... + * coef[1] = coefficients of LR4 HP0... + * ... + * + * Since an LR4 is two biquads in series with same coefficients, + * the config takes the coefficients for one biquad and + * assigns it to both biquads of the LR4. + * + * <1st Low Pass LR4> + * 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 right shift (nve for left) + * int32_t output_gain Q2.14 format + * <1nd High Pass LR4> + * ... + * <2nd Low Pass LR4> + * <2nd High Pass LR4> + * ... + * ... At most 3 Low Pass LR4s and 3 High Pass LR4s ... + * + */ +struct sof_crossover_config { + uint32_t size; + uint32_t num_sinks; + + /* reserved */ + uint32_t reserved[4]; + + uint32_t assign_sink[SOF_CROSSOVER_MAX_STREAMS]; + struct sof_eq_iir_biquad_df2t coef[]; +}; + +#endif // __USER_CROSSOVER_H__ From 4cabd427c70ac1e8d3eb5d3d8deb6868c628c861 Mon Sep 17 00:00:00 2001 From: Sebastiano Carlucci Date: Thu, 16 Apr 2020 14:37:25 -0700 Subject: [PATCH 3/6] tools: topology: Add crossover topology files This commit adds the topology files for the crossover component. The control bytes are generated by the tools in tune/crossover. Signed-off-by: Sebastiano Carlucci --- tools/topology/m4/crossover.m4 | 49 +++++++++++ tools/topology/m4/crossover_coef_default.m4 | 19 +++++ tools/topology/sof/pipe-crossover-playback.m4 | 83 +++++++++++++++++++ 3 files changed, 151 insertions(+) create mode 100644 tools/topology/m4/crossover.m4 create mode 100644 tools/topology/m4/crossover_coef_default.m4 create mode 100644 tools/topology/sof/pipe-crossover-playback.m4 diff --git a/tools/topology/m4/crossover.m4 b/tools/topology/m4/crossover.m4 new file mode 100644 index 000000000000..df9f25dadd65 --- /dev/null +++ b/tools/topology/m4/crossover.m4 @@ -0,0 +1,49 @@ +divert(-1) + +dnl Define macro for crossover widget + +dnl Crossover name) +define(`N_CROSSOVER', `CROSSOVER'PIPELINE_ID`.'$1) + +dnl W_CROSSOVER(name, format, periods_sink, periods_source, kcontrol_list) +define(`W_CROSSOVER', +`SectionVendorTuples."'N_CROSSOVER($1)`_tuples_w" {' +` tokens "sof_comp_tokens"' +` tuples."word" {' +` SOF_TKN_COMP_PERIOD_SINK_COUNT' STR($3) +` SOF_TKN_COMP_PERIOD_SOURCE_COUNT' STR($4) +` }' +`}' +`SectionData."'N_CROSSOVER($1)`_data_w" {' +` tuples "'N_CROSSOVER($1)`_tuples_w"' +`}' +`SectionVendorTuples."'N_CROSSOVER($1)`_tuples_str" {' +` tokens "sof_comp_tokens"' +` tuples."string" {' +` SOF_TKN_COMP_FORMAT' STR($2) +` }' +`}' +`SectionVendorTuples."'N_CROSSOVER($1)`_crossover_process_tuples_str" {' +` tokens "sof_process_tokens"' +` tuples."string" {' +` SOF_TKN_PROCESS_TYPE' "CROSSOVER" +` }' +`}' +`SectionData."'N_CROSSOVER($1)`_data_str" {' +` tuples "'N_CROSSOVER($1)`_tuples_str"' +` tuples "'N_CROSSOVER($1)`_crossover_process_tuples_str"' +`}' +`SectionWidget."'N_CROSSOVER($1)`" {' +` index "'PIPELINE_ID`"' +` type "effect"' +` no_pm "true"' +` data [' +` "'N_CROSSOVER($1)`_data_w"' +` "'N_CROSSOVER($1)`_data_str"' +` ]' +` bytes [' + $5 +` ]' +`}') + +divert(0)dnl diff --git a/tools/topology/m4/crossover_coef_default.m4 b/tools/topology/m4/crossover_coef_default.m4 new file mode 100644 index 000000000000..25d86f4a5c5b --- /dev/null +++ b/tools/topology/m4/crossover_coef_default.m4 @@ -0,0 +1,19 @@ +# Exported Control Bytes 17-Apr-2020 +CONTROLBYTES_PRIV(CROSSOVER_priv, +` bytes "0x53,0x4f,0x46,0x00,0x00,0x00,0x00,0x00,' +` 0x60,0x00,0x00,0x00,0x00,0xf0,0x00,0x03,' +` 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,' +` 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,' +` 0x60,0x00,0x00,0x00,0x02,0x00,0x00,0x00,' +` 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,' +` 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,' +` 0x01,0x00,0x00,0x00,0x0a,0x00,0x00,0x00,' +` 0x03,0x00,0x00,0x00,0x04,0x00,0x00,0x00,' +` 0x8e,0x6f,0xa8,0xc5,0xb3,0x81,0x14,0x7a,' +` 0xb0,0xc3,0x10,0x00,0x5f,0x87,0x21,0x00,' +` 0xb0,0xc3,0x10,0x00,0x00,0x00,0x00,0x00,' +` 0x00,0x40,0x00,0x00,0x8e,0x6f,0xa8,0xc5,' +` 0xb3,0x81,0x14,0x7a,0x89,0x04,0x1b,0x3d,' +` 0xee,0xf6,0xc9,0x85,0x89,0x04,0x1b,0x3d,' +` 0x00,0x00,0x00,0x00,0x00,0x40,0x00,0x00"' +) diff --git a/tools/topology/sof/pipe-crossover-playback.m4 b/tools/topology/sof/pipe-crossover-playback.m4 new file mode 100644 index 000000000000..d342c809d092 --- /dev/null +++ b/tools/topology/sof/pipe-crossover-playback.m4 @@ -0,0 +1,83 @@ +# Crossover Volume Pipeline +# +# Low Latency Playback with demux and volume. +# +# Pipeline Endpoints for connection are : +# +# B2 (DAI buffer) +# Playback Crossover +# +# host PCM_P -- B0 --> Crossover 0 --> B1 ---> DAI0 +# | +# pipeline n+1 --> DAI + +# Include topology builder +include(`utils.m4') +include(`buffer.m4') +include(`pcm.m4') +include(`crossover.m4') +include(`bytecontrol.m4') + +# +# Controls for Crossover +# + +define(CROSSOVER_priv, concat(`crossover_bytes_', PIPELINE_ID)) +define(MY_CROSSOVER_CTRL, concat(`crossover_control_', PIPELINE_ID)) +include(`crossover_coef_default.m4') +C_CONTROLBYTES(MY_CROSSOVER_CTRL, PIPELINE_ID, + CONTROLBYTES_OPS(bytes, 258 binds the control to bytes get/put handlers, 258, 258), + CONTROLBYTES_EXTOPS(258 binds the control to bytes get/put handlers, 258, 258), + , , , + CONTROLBYTES_MAX(, 1024), + , + CROSSOVER_priv) + +# +# Components and Buffers +# + +# Host "Low latency Playback" PCM +# with 2 sink and 0 source periods +W_PCM_PLAYBACK(PCM_ID, Low Latency Playback, 2, 0) + +# Crossover filter has 2 sink and source periods. TODO for controls +W_CROSSOVER(0, PIPELINE_FORMAT, 2, 2, LIST(` ', "MY_CROSSOVER_CTRL")) + +# Low Latency Buffers +W_BUFFER(0, COMP_BUFFER_SIZE(2, + COMP_SAMPLE_SIZE(PIPELINE_FORMAT), PIPELINE_CHANNELS, COMP_PERIOD_FRAMES(PCM_MAX_RATE, SCHEDULE_PERIOD)), + PLATFORM_HOST_MEM_CAP) +W_BUFFER(1, COMP_BUFFER_SIZE(DAI_PERIODS, + COMP_SAMPLE_SIZE(PIPELINE_FORMAT), PIPELINE_CHANNELS, COMP_PERIOD_FRAMES(PCM_MAX_RATE, SCHEDULE_PERIOD)), + PLATFORM_COMP_MEM_CAP) + + +# +# Pipeline Graph +# host PCM_P -- B0 --> Crossover 0 --> B1 ---> DAI +# | +# pipeline n+1 --> DAI + +P_GRAPH(pipe-ll-playback-PIPELINE_ID, PIPELINE_ID, + LIST(` ', + `dapm(N_BUFFER(0), N_PCMP(PCM_ID))', + `dapm(N_CROSSOVER(0), N_BUFFER(0))', + `dapm(N_BUFFER(1), N_CROSSOVER(0))')) +# +# Pipeline Source and Sinks +# +indir(`define', concat(`PIPELINE_SOURCE_', PIPELINE_ID), N_BUFFER(1)) +indir(`define', concat(`PIPELINE_CROSSOVER_', PIPELINE_ID), N_CROSSOVER(0)) +indir(`define', concat(`PIPELINE_PCM_', PIPELINE_ID), Low Latency Playback PCM_ID) + +# +# PCM Configuration +# + + +# PCM capabilities supported by FW +PCM_CAPABILITIES(Low Latency Playback PCM_ID, `S32_LE,S24_LE,S16_LE', 48000, 48000, 2, PIPELINE_CHANNELS, 2, 16, 192, 16384, 65536, 65536) + +undefine(`MY_CROSSOVER_CTRL') +undefine(`CROSSOVER_priv') From 53c49fdb82546b1a0368c5e8e7a8fe4a54ff2929 Mon Sep 17 00:00:00 2001 From: Sebastiano Carlucci Date: Thu, 16 Apr 2020 17:09:32 -0700 Subject: [PATCH 4/6] tools: tune: Add tools to generate ctrl bytes for Crossover This commit adds the tools to generate the control bytes for the crossover component. To generate the control bytes, run the example_crossover.m script. The parameters of the crossover components are: - number of outputs - sink assignments (routing crossover output to different pipelines) - frequency cutoffs To tweak the parameters modify the values in example_crossover.m and run it. Refer to sof/src/include/user/crossover.h for more information on how the crossover config is structured and how sink assignments are done. Signed-off-by: Sebastiano Carlucci --- tools/tune/crossover/crossover_build_blob.m | 79 ++++++++++++ tools/tune/crossover/crossover_coef_quant.m | 31 +++++ tools/tune/crossover/crossover_gen_coefs.m | 114 ++++++++++++++++++ .../crossover/crossover_generate_config.m | 20 +++ tools/tune/crossover/crossover_plot_freq.m | 111 +++++++++++++++++ tools/tune/crossover/example_crossover.m | 54 +++++++++ 6 files changed, 409 insertions(+) create mode 100644 tools/tune/crossover/crossover_build_blob.m create mode 100644 tools/tune/crossover/crossover_coef_quant.m create mode 100644 tools/tune/crossover/crossover_gen_coefs.m create mode 100644 tools/tune/crossover/crossover_generate_config.m create mode 100644 tools/tune/crossover/crossover_plot_freq.m create mode 100644 tools/tune/crossover/example_crossover.m diff --git a/tools/tune/crossover/crossover_build_blob.m b/tools/tune/crossover/crossover_build_blob.m new file mode 100644 index 000000000000..eb6e2879a825 --- /dev/null +++ b/tools/tune/crossover/crossover_build_blob.m @@ -0,0 +1,79 @@ +function blob8 = crossover_build_blob(blob_struct, endian) + +%% Settings +bits_R = 32; %Q2.30 +qy_R = 30; + +if nargin < 2 + endian = 'little' +endif + +%% Shift values for little/big endian +switch lower(endian) + case 'little' + sh = [0 -8 -16 -24]; + case 'big' + sh = [-24 -16 -8 0]; + otherwise + error('Unknown endiannes'); +end + +%% Build Blob +% refer to sof/src/include/user/crossover.h for the config struct. +data_size = 4 * (2 + 4 + 4 + numel(blob_struct.all_coef)); +[abi_bytes, abi_size] = crossover_get_abi(data_size); + +blob_size = data_size + abi_size; +blob8 = uint8(zeros(1, blob_size)); + +% Pack Blob data +% Insert ABI Header +blob8(1:abi_size) = abi_bytes; +j = abi_size + 1; + +% Insert Data +blob8(j:j+3) = word2byte(data_size, sh); j=j+4; +blob8(j:j+3) = word2byte(blob_struct.num_sinks, sh); j=j+4; +blob8(j:j+3) = word2byte(0, sh); j=j+4; % Reserved +blob8(j:j+3) = word2byte(0, sh); j=j+4; % Reserved +blob8(j:j+3) = word2byte(0, sh); j=j+4; % Reserved +blob8(j:j+3) = word2byte(0, sh); j=j+4; % Reserved + +for i=1:4 + blob8(j:j+3) = word2byte(blob_struct.assign_sinks(i), sh); + j=j+4; +end + +for i=1:length(blob_struct.all_coef) + blob8(j:j+3) = word2byte(blob_struct.all_coef(i), sh); + j=j+4; +end + +endfunction + +function bytes = word2byte(word, sh) +bytes = uint8(zeros(1,4)); +bytes(1) = bitand(bitshift(word, sh(1)), 255); +bytes(2) = bitand(bitshift(word, sh(2)), 255); +bytes(3) = bitand(bitshift(word, sh(3)), 255); +bytes(4) = bitand(bitshift(word, sh(4)), 255); +end + +function [bytes, nbytes] = crossover_get_abi(setsize) + +%% Return current SOF ABI header +%% Use sof-ctl to write ABI header into a file +abifn = 'crossover_get_abi.bin'; +cmd = sprintf('sof-ctl -g %d -b -o %s', setsize, abifn); +system(cmd); + +%% Read file and delete it +fh = fopen(abifn, 'r'); +if fh < 0 + error("Failed to get ABI header. Is sof-ctl installed?"); +end +[bytes, nbytes] = fread(fh, inf, 'uint8'); +fclose(fh); +delete(abifn); + +end diff --git a/tools/tune/crossover/crossover_coef_quant.m b/tools/tune/crossover/crossover_coef_quant.m new file mode 100644 index 000000000000..1546845268db --- /dev/null +++ b/tools/tune/crossover/crossover_coef_quant.m @@ -0,0 +1,31 @@ +function crossover_quant = crossover_coef_quant(lowpass, highpass); + +bits_iir = 32; % Q2.30 +qf_iir = 30; + +addpath ./../eq + +if length(lowpass) != length(highpass) + error("length of lowpass and highpass array do not match"); +end + +n = length(lowpass); +crossover_quant.lp_coef = cell(1,n); +crossover_quant.hp_coef = cell(1,n); +for i = 1:n + lp = lowpass(i); + hp = highpass(i); + lp_a = eq_coef_quant(-lp.a(3:-1:2), bits_iir, qf_iir); + lp_b = eq_coef_quant(lp.b(3:-1:1), bits_iir, qf_iir); + hp_a = eq_coef_quant(-hp.a(3:-1:2), bits_iir, qf_iir); + hp_b = eq_coef_quant(hp.b(3:-1:1), bits_iir, qf_iir); + + crossover_quant.lp_coef(i) = [lp_a lp_b 0 16384]; + crossover_quant.hp_coef(i) = [hp_a hp_b 0 16384]; +end + +crossover_quant.lp_coef = cell2mat(crossover_quant.lp_coef); +crossover_quant.hp_coef = cell2mat(crossover_quant.hp_coef); + +rmpath ./../eq +end diff --git a/tools/tune/crossover/crossover_gen_coefs.m b/tools/tune/crossover/crossover_gen_coefs.m new file mode 100644 index 000000000000..10e00bf04d63 --- /dev/null +++ b/tools/tune/crossover/crossover_gen_coefs.m @@ -0,0 +1,114 @@ +function crossover = crossover_gen_coefs(fs, fc_low, fc_mid, fc_high); + +addpath ./../eq/ +switch nargin + case 2, crossover = crossover_generate_2way(fs, fc_low); + case 3, crossover = crossover_generate_3way(fs, fc_low, fc_mid); + case 4, crossover = crossover_generate_4way(fs, fc_low, fc_mid, fc_high); + otherwise, error("Invalid number of arguments"); +end +rmpath ./../eq +end + +function crossover_2way = crossover_generate_2way(fs, fc); + crossover_2way.lp = [lp_iir(fs, fc, 0)]; + crossover_2way.hp = [hp_iir(fs, fc, 0)]; +end + +function crossover_3way = crossover_generate_3way(fs, fc_low, fc_high); + % Duplicate one set of coefficients. The duplicate set will be used to merge back the + % output that is out of phase. + crossover_3way.lp = [lp_iir(fs, fc_low, 0) lp_iir(fs, fc_high, 0) lp_iir(fs, fc_high, 0)]; + crossover_3way.hp = [hp_iir(fs, fc_low, 0) hp_iir(fs, fc_high, 0) hp_iir(fs, fc_high, 0)]; +end + +function crossover_4way = crossover_generate_4way(fs, fc_low, fc_mid, fc_high); + crossover_4way.lp = [lp_iir(fs, fc_low, 0) lp_iir(fs, fc_mid, 0) lp_iir(fs, fc_high, 0)]; + crossover_4way.hp = [hp_iir(fs, fc_low, 0) hp_iir(fs, fc_mid, 0) hp_iir(fs, fc_high, 0)]; +end + +% Generate the a,b coefficients for a second order +% low pass butterworth filter +function lp = lp_iir(fs, fc, gain_db) +[lp.b, lp.a] = low_pass_2nd_resonance(fc, 0, fs); +end + +% Generate the a,b coefficients for a second order +% low pass butterworth filter +function hp = hp_iir(fs, fc, gain_db) +[hp.b, hp.a] = high_pass_2nd_resonance(fc, 0, fs); +end + +function [b, a] = high_pass_2nd_resonance(f, resonance, fs) + cutoff = f/(fs/2); + % Limit cutoff to 0 to 1. + cutoff = max(0.0, min(cutoff, 1.0)); + + if cutoff == 1 || cutoff == 0 + % When cutoff is one, the z-transform is 0. + % When cutoff is zero, we need to be careful because the above + % gives a quadratic divided by the same quadratic, with poles + % and zeros on the unit circle in the same place. When cutoff + % is zero, the z-transform is 1. + + b = [1 - cutoff, 0, 0]; + a = [1, 0, 0]; + return; + endif + + % Compute biquad coefficients for highpass filter + resonance = max(0.0, resonance); % can't go negative + g = 10.0^(0.05 * resonance); + d = sqrt((4 - sqrt(16 - 16 / (g * g))) / 2); + + theta = pi * cutoff; + sn = 0.5 * d * sin(theta); + beta = 0.5 * (1 - sn) / (1 + sn); + gamma = (0.5 + beta) * cos(theta); + alpha = 0.25 * (0.5 + beta + gamma); + + b0 = 2 * alpha; + b1 = 2 * -2 * alpha; + b2 = 2 * alpha; + a1 = 2 * -gamma; + a2 = 2 * beta; + + b = [b0, b1, b2]; + a = [1.0, a1, a2]; +end + +function [b, a] = low_pass_2nd_resonance(f, resonance, fs) + cutoff = f/(fs/2); + % Limit cutoff to 0 to 1. + cutoff = max(0.0, min(cutoff, 1.0)); + + if cutoff == 1 || cutoff == 0 + % When cutoff is 1, the z-transform is 1. + % When cutoff is zero, nothing gets through the filter, so set + % coefficients up correctly. + + b = [cutoff, 0, 0]; + a = [1, 0, 0]; + return; + endif + + % Compute biquad coefficients for lowpass filter + resonance = max(0.0, resonance); % can't go negative + g = 10.0^(0.05 * resonance); + d = sqrt((4 - sqrt(16 - 16 / (g * g))) / 2); + + theta = pi * cutoff; + sn = 0.5 * d * sin(theta); + beta = 0.5 * (1 - sn) / (1 + sn); + gamma = (0.5 + beta) * cos(theta); + alpha = 0.25 * (0.5 + beta - gamma); + + b0 = 2 * alpha; + b1 = 2 * 2 * alpha; + b2 = 2 * alpha; + a1 = 2 * -gamma; + a2 = 2 * beta; + + b = [b0, b1, b2]; + a = [1.0, a1, a2]; +end diff --git a/tools/tune/crossover/crossover_generate_config.m b/tools/tune/crossover/crossover_generate_config.m new file mode 100644 index 000000000000..b6fed758fcf8 --- /dev/null +++ b/tools/tune/crossover/crossover_generate_config.m @@ -0,0 +1,20 @@ +function config = crossover_generate_config(crossover_bqs, num_sinks, assign_sinks); + +config.num_sinks = num_sinks; +config.assign_sinks = assign_sinks; +% Interleave the coefficients for the low and high pass filters +% For 2 way crossover we have 1 pair of LR4s. +% For 3,4 way crossover we have 3 pair of LR4s. +if num_sinks == 2 + n = 1; +else + n = 3; +end +j = 1; +k = 1; +for i = 1:n + config.all_coef(k:k+6) = crossover_bqs.lp_coef(j:j+6); k = k+7; + config.all_coef(k:k+6) = crossover_bqs.hp_coef(j:j+6); k = k+7; + j = j+7; +end +end diff --git a/tools/tune/crossover/crossover_plot_freq.m b/tools/tune/crossover/crossover_plot_freq.m new file mode 100644 index 000000000000..c75e18536e8b --- /dev/null +++ b/tools/tune/crossover/crossover_plot_freq.m @@ -0,0 +1,111 @@ +function crossover_plot_freq(lp, hp, fs, num_sinks); +% Plot the transfer function of each sink. We need to reconstruct a filter +% that represents the path the samples go through for each sinks. +% Example 4-way crossover: +% +% o---- LR4 LO-PASS --> y1(n) +% | +% o--- LR4 LO-PASS --o +% | | +% | o--- LR4 HI-PASS --> y2(n) +% x(n) --- o +% | o--- LR4 LO-PASS --> y3(n) +% | | +% o--- LR4 HI-PASS --o +% | +% o--- LR4 HI-PASS --> y4(n) +% +% Then to plot the transferfn for y1 we would create a filter such as: +% x(n) ---> LR4 LO-PASS --> LR4 LO-PASS --> y1(n) + +f = linspace(1, fs/2, 500); + +if num_sinks == 2 + h1 = cascade_bqs_fr(f, fs, lp(1), lp(1)); + h2 = cascade_bqs_fr(f, fs, hp(1), hp(1)); + + subplot(2 ,1, 2) + freqz_plot(f, h1) + subplot(2, 1, 1) + plot_mag_resp(f, h1, h2) +end + +if num_sinks == 3 + % First LR4 Low Pass Filters + h1 = cascade_bqs_fr(f, fs, lp(1), lp(1)); + % Second LR4 Filters + tmp = cascade_bqs_fr(f, fs, lp(2), lp(2)); + tmp2 = cascade_bqs_fr(f, fs, hp(2), hp(2)); + % Merge the second LR4 Filters + tmp = tmp + tmp2; + % Cascade the First LR4 and the result of the previous merge + h1 = h1.*tmp; + + h2 = cascade_bqs_fr(f, fs, hp(1), hp(1), lp(3), lp(3)); + h3 = cascade_bqs_fr(f, fs, hp(1), hp(1), hp(3), hp(3)); + + subplot(2, 1, 2) + plot_phase_resp(f, h1, h2, h3) + subplot(2, 1, 1) + plot_mag_resp(f, h1, h2, h3) +end + +if num_sinks == 4 + h1 = cascade_bqs_fr(f, fs, lp(2), lp(2), lp(1), lp(1)); + h2 = cascade_bqs_fr(f, fs, lp(2), lp(2), hp(1), hp(1)); + h3 = cascade_bqs_fr(f, fs, hp(2), hp(2), lp(3), lp(3)); + h4 = cascade_bqs_fr(f, fs, hp(2), hp(2), hp(3), hp(3)); + + subplot(2, 1, 2) + plot_phase_resp(f, h1, h2, h3, h4) + subplot(2, 1, 1) + plot_mag_resp(f, h1, h2, h3, h4) +end +end + +function [h12, w] = cascade_bqs_fr(f, fs, varargin); +bq1 = varargin{1}; +bq2 = varargin{2}; +[h1, w] = freqz(bq1.b, bq1.a, f, fs); +[h2, w] = freqz(bq2.b, bq2.a, f, fs); +h12 = h1.*h2; +for i=3:length(varargin) + bq = varargin{i}; + [h1, w] = freqz(bq.b, bq.a, f, fs); + h12 = h12.*h1; +end +end + +function plot_phase_resp(f,varargin) +n = length(varargin); +labels = cellstr(n); +hold on +grid on +for i=1:n + h = varargin{i}; + semilogx(f, unwrap(arg(h)) * 180 / pi) + labels(i) = sprintf("out%d", i); +end +legend(labels, 'Location', 'NorthEast') +xlabel('Frequency (Hz)'); +ylabel('Phase Shift (degrees)'); +tstr = "Crossover Filter Phase Response"; +title(tstr); +end + +function plot_mag_resp(f,varargin) +n = length(varargin); +labels = cellstr(n); +hold on +grid on +for i=1:n + h = varargin{i}; + semilogx(f,20*log10(h)) + labels(i) = sprintf("out%d", i); +end +legend(labels, 'Location', 'NorthEast') +xlabel('Frequency (Hz)'); +ylabel('Magnitude (dB)'); +tstr = "Crossover Filter Magnitude Response"; +title(tstr); +end diff --git a/tools/tune/crossover/example_crossover.m b/tools/tune/crossover/example_crossover.m new file mode 100644 index 000000000000..3166f4a4f022 --- /dev/null +++ b/tools/tune/crossover/example_crossover.m @@ -0,0 +1,54 @@ +function example_crossover(); + +% Set the parameters here +tplg_fn = "../../topology/m4/crossover_coef_default.m4" % Control Bytes File +% Use those files with sof-ctl to update the component's configuration +blob_fn = "../../ctl/crossover_coef.blob" % Blob binary file +alsa_fn = "../../ctl/crossover_coef.txt" % ALSA CSV format file + +endian = "little"; + +% Sampling Frequency and Frequency cut-offs for crossover +fs = 48e3; +fc_low = 200; +fc_med = 1000; +fc_high = 3000; + +% 4 way crossover +num_sinks = 4; +% This array is an example on how to assign a buffer from pipeline 1 to output 0, +% buffer from pipeline 2 to output 1, etc... +% Refer to sof/src/inlude/user/crossover.h for more information on assigning +% buffers to outputs. +assign_sinks = zeros(1, 4); +assign_sinks(1) = 1; % sink[0] +assign_sinks(2) = 2; % sink[1] +assign_sinks(3) = 3; % sink[2] +assign_sinks(4) = 4; % sink[3] + +% Generate zeros, poles and gain for crossover with the given frequencies +%crossover = crossover_gen_coefs(fs, fc_low); % 2 way crossover +% crossover = crossover_gen_coefs(fs, fc_low, fc_med); % 3 way crossover +crossover = crossover_gen_coefs(fs, fc_low, fc_med, fc_high); % 4 way crossover + +% Convert the [a,b] coefficients to values usable with SOF +crossover_bqs = crossover_coef_quant(crossover.lp, crossover.hp); + +% Convert coefficients to sof_crossover_config struct +config = crossover_generate_config(crossover_bqs, num_sinks, assign_sinks); + +% Convert struct to binary blob +blob8 = crossover_build_blob(config, endian); + +% Generate output files +addpath ./../common + +tplg_write(tplg_fn, blob8, "CROSSOVER"); +blob_write(blob_fn, blob8); +alsactl_write(alsa_fn, blob8); + +% Plot Magnitude and Phase Response of each sink +crossover_plot_freq(crossover.lp, crossover.hp, fs, num_sinks); +rmpath ./../common + +endfunction From ab38ccbaf0eac34c1ed39cceb2afbed00006d318 Mon Sep 17 00:00:00 2001 From: Pin-chih Lin Date: Fri, 31 Jul 2020 13:48:06 +0800 Subject: [PATCH 5/6] tools: tplg_parser: Add Crossover component Add Crossover component for tplg_parser. Signed-off-by: Pin-chih Lin --- src/audio/CMakeLists.txt | 3 ++- tools/tplg_parser/include/tplg_parser/topology.h | 1 + tools/tplg_parser/tplg_parser.c | 3 ++- 3 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/audio/CMakeLists.txt b/src/audio/CMakeLists.txt index 0f8aea451ef3..288e2d04de63 100644 --- a/src/audio/CMakeLists.txt +++ b/src/audio/CMakeLists.txt @@ -115,7 +115,7 @@ check_optimization(fma -mfma -DOPS_FMA) check_optimization(hifi2ep -mhifi2ep -DOPS_HIFI2EP) check_optimization(hifi3 -mhifi3 -DOPS_HIFI3) -set(sof_audio_modules volume src asrc eq-fir eq-iir dcblock) +set(sof_audio_modules volume src asrc eq-fir eq-iir dcblock crossover) # sources for each module set(volume_sources volume/volume.c volume/volume_generic.c) @@ -124,6 +124,7 @@ set(asrc_sources asrc/asrc.c asrc/asrc_farrow.c asrc/asrc_farrow_generic.c) set(eq-fir_sources eq_fir/eq_fir.c eq_fir/fir.c) set(eq-iir_sources eq_iir/eq_iir.c eq_iir/iir.c eq_iir/iir_generic.c) set(dcblock_sources dcblock/dcblock.c dcblock/dcblock_generic.c) +set(crossover_sources crossover/crossover.c crossover/crossover_generic.c) foreach(audio_module ${sof_audio_modules}) # first compile with no optimizations diff --git a/tools/tplg_parser/include/tplg_parser/topology.h b/tools/tplg_parser/include/tplg_parser/topology.h index d98acdb71496..6ca7b024ddc0 100644 --- a/tools/tplg_parser/include/tplg_parser/topology.h +++ b/tools/tplg_parser/include/tplg_parser/topology.h @@ -53,6 +53,7 @@ enum sof_ipc_process_type { SOF_PROCESS_MUX, SOF_PROCESS_DEMUX, SOF_PROCESS_DCBLOCK, + SOF_PROCESS_CROSSOVER, }; struct sof_topology_token { diff --git a/tools/tplg_parser/tplg_parser.c b/tools/tplg_parser/tplg_parser.c index b0a85199bd80..54818fd7eb72 100644 --- a/tools/tplg_parser/tplg_parser.c +++ b/tools/tplg_parser/tplg_parser.c @@ -33,7 +33,8 @@ static const struct sof_process_types sof_process[] = { {"CHAN_SELECTOR", SOF_PROCESS_CHAN_SELECTOR, SOF_COMP_SELECTOR}, {"MUX", SOF_PROCESS_MUX, SOF_COMP_MUX}, {"DEMUX", SOF_PROCESS_DEMUX, SOF_COMP_DEMUX}, - {"DCBLOCK", SOF_PROCESS_DCBLOCK, SOF_COMP_DCBLOCK} + {"DCBLOCK", SOF_PROCESS_DCBLOCK, SOF_COMP_DCBLOCK}, + {"CROSSOVER", SOF_PROCESS_CROSSOVER, SOF_COMP_CROSSOVER} }; static enum sof_ipc_process_type find_process(const char *name) From 471ffa7aebd34570166c4ed5a1c9a7fe2d539d9c Mon Sep 17 00:00:00 2001 From: Pin-chih Lin Date: Tue, 11 Aug 2020 14:08:51 +0800 Subject: [PATCH 6/6] src: math: move iir_df2t function to src/math Moved iir_df2t() from src/audio/eq_iir to src/math as a common library for IIR and Crossover usage. Signed-off-by: Pin-chih Lin --- src/audio/CMakeLists.txt | 2 +- src/audio/eq_iir/CMakeLists.txt | 2 +- src/audio/eq_iir/eq_iir.c | 1 + src/include/sof/audio/crossover/crossover.h | 9 +- src/include/sof/audio/eq_iir/iir.h | 93 +------------------ src/include/sof/math/iir_df2t.h | 63 +++++++++++++ src/math/CMakeLists.txt | 4 +- .../iir_generic.c => math/iir_df2t_generic.c} | 37 +++++++- .../iir_hifi3.c => math/iir_df2t_hifi3.c} | 2 +- 9 files changed, 106 insertions(+), 107 deletions(-) create mode 100644 src/include/sof/math/iir_df2t.h rename src/{audio/eq_iir/iir_generic.c => math/iir_df2t_generic.c} (66%) rename src/{audio/eq_iir/iir_hifi3.c => math/iir_df2t_hifi3.c} (99%) diff --git a/src/audio/CMakeLists.txt b/src/audio/CMakeLists.txt index 288e2d04de63..ec48d376614f 100644 --- a/src/audio/CMakeLists.txt +++ b/src/audio/CMakeLists.txt @@ -122,7 +122,7 @@ set(volume_sources volume/volume.c volume/volume_generic.c) set(src_sources src/src.c src/src_generic.c) set(asrc_sources asrc/asrc.c asrc/asrc_farrow.c asrc/asrc_farrow_generic.c) set(eq-fir_sources eq_fir/eq_fir.c eq_fir/fir.c) -set(eq-iir_sources eq_iir/eq_iir.c eq_iir/iir.c eq_iir/iir_generic.c) +set(eq-iir_sources eq_iir/eq_iir.c eq_iir/iir.c) set(dcblock_sources dcblock/dcblock.c dcblock/dcblock_generic.c) set(crossover_sources crossover/crossover.c crossover/crossover_generic.c) diff --git a/src/audio/eq_iir/CMakeLists.txt b/src/audio/eq_iir/CMakeLists.txt index 04f5c4262ef2..397b6081d4e6 100644 --- a/src/audio/eq_iir/CMakeLists.txt +++ b/src/audio/eq_iir/CMakeLists.txt @@ -1,3 +1,3 @@ # SPDX-License-Identifier: BSD-3-Clause -add_local_sources(sof eq_iir.c iir.c iir_generic.c iir_hifi3.c) +add_local_sources(sof eq_iir.c iir.c) diff --git a/src/audio/eq_iir/eq_iir.c b/src/audio/eq_iir/eq_iir.c index 4e630379cd95..14ce2d108a9f 100644 --- a/src/audio/eq_iir/eq_iir.c +++ b/src/audio/eq_iir/eq_iir.c @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include diff --git a/src/include/sof/audio/crossover/crossover.h b/src/include/sof/audio/crossover/crossover.h index 8ebf70d3bf1d..26ba60d9d341 100644 --- a/src/include/sof/audio/crossover/crossover.h +++ b/src/include/sof/audio/crossover/crossover.h @@ -9,7 +9,7 @@ #include #include -#include +#include #include struct comp_buffer; @@ -148,13 +148,8 @@ static inline crossover_split crossover_find_split_func(int32_t num_sinks) static inline int32_t crossover_generic_process_lr4(int32_t in, struct iir_state_df2t *lr4) { - int i; - /* Cascade two biquads with same coefficients in series. */ - for (i = 0; i < lr4->biquads_in_series; i++) - in = iir_df2t_biquad(in, lr4->coef, &lr4->delay[2 * i]); - - return in; + return iir_df2t(lr4, in); } #endif // __SOF_AUDIO_CROSSOVER_CROSSOVER_H__ diff --git a/src/include/sof/audio/eq_iir/iir.h b/src/include/sof/audio/eq_iir/iir.h index 6ce53ecc41f2..0471a143a0fc 100644 --- a/src/include/sof/audio/eq_iir/iir.h +++ b/src/include/sof/audio/eq_iir/iir.h @@ -13,101 +13,10 @@ #include #include #include +#include struct sof_eq_iir_header_df2t; -/* Get platforms configuration */ - - -/* If next defines are set to 1 the EQ is configured automatically. Setting - * to zero temporarily is useful is for testing needs. - * Setting EQ_FIR_AUTOARCH to 0 allows to manually set the code variant. - */ -#define IIR_AUTOARCH 1 - -/* Force manually some code variant when IIR_AUTOARCH is set to zero. These - * are useful in code debugging. - */ -#if IIR_AUTOARCH == 0 -#define IIR_GENERIC 1 -#define IIR_HIFI3 0 -#endif - -/* Select optimized code variant when xt-xcc compiler is used */ -#if IIR_AUTOARCH == 1 -#if defined __XCC__ -#include -#if XCHAL_HAVE_HIFI3 == 1 -#define IIR_GENERIC 0 -#define IIR_HIFI3 1 -#else -#define IIR_GENERIC 1 -#define IIR_HIFI3 0 -#endif /* XCHAL_HAVE_HIFI3 */ -#else -/* GCC */ -#define IIR_GENERIC 1 -#define IIR_HIFI3 0 -#endif /* __XCC__ */ -#endif /* IIR_AUTOARCH */ - -#define IIR_DF2T_NUM_DELAYS 2 - -struct iir_state_df2t { - unsigned int biquads; /* Number of IIR 2nd order sections total */ - unsigned 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 */ -}; - -/* - * \brief Returns the output of a biquad using its df2t form. - * - * 32 bit data, 32 bit coefficients and 64 bit state variables - * \param in input to the biquad Q1.31 - * \param coef coefficients of the biquad Q2.30 - * \param delay delay of the biquads Q3.61 - */ -static inline int32_t iir_df2t_biquad(int32_t in, int32_t *coef, - int64_t *delay) -{ - int32_t tmp; - int64_t acc; - - /* Compute output: Delay is Q3.61 - * Q2.30 x Q1.31 -> Q3.61 - * Shift Q3.61 to Q3.31 with rounding - */ - acc = ((int64_t)coef[4]) * in + delay[0]; - tmp = (int32_t)Q_SHIFT_RND(acc, 61, 31); - - /* Compute first delay */ - acc = delay[1]; - acc += ((int64_t)coef[3]) * in; /* Coef b1 */ - acc += ((int64_t)coef[1]) * tmp; /* Coef a1 */ - delay[0] = acc; - - /* Compute second delay */ - acc = ((int64_t)coef[2]) * in; /* Coef b2 */ - acc += ((int64_t)coef[0]) * tmp; /* Coef a2 */ - delay[1] = acc; - - /* Apply gain Q2.14 x Q1.31 -> Q3.45 */ - acc = ((int64_t)coef[6]) * tmp; /* Gain */ - - /* Apply biquad output shift right parameter - * simultaneously with Q3.45 to Q3.31 conversion. Then - * saturate to 32 bits Q1.31 and prepare for next - * biquad. - */ - acc = Q_SHIFT_RND(acc, 45 + coef[5], 31); - return sat_int32(acc); -} - -int32_t iir_df2t(struct iir_state_df2t *iir, int32_t x); - int iir_init_coef_df2t(struct iir_state_df2t *iir, struct sof_eq_iir_header_df2t *config); diff --git a/src/include/sof/math/iir_df2t.h b/src/include/sof/math/iir_df2t.h new file mode 100644 index 000000000000..40f8ec86796e --- /dev/null +++ b/src/include/sof/math/iir_df2t.h @@ -0,0 +1,63 @@ +/* SPDX-License-Identifier: BSD-3-Clause + * + * Copyright(c) 2017 Intel Corporation. All rights reserved. + * + * Author: Seppo Ingalsuo + * Liam Girdwood + * Keyon Jie + */ + +#ifndef __SOF_MATH_IIR_DF2T_H__ +#define __SOF_MATH_IIR_DF2T_H__ + +#include +#include + +/* Get platforms configuration */ + +/* If next defines are set to 1 the EQ is configured automatically. Setting + * to zero temporarily is useful is for testing needs. + * Setting EQ_FIR_AUTOARCH to 0 allows to manually set the code variant. + */ +#define IIR_AUTOARCH 1 + +/* Force manually some code variant when IIR_AUTOARCH is set to zero. These + * are useful in code debugging. + */ +#if IIR_AUTOARCH == 0 +#define IIR_GENERIC 1 +#define IIR_HIFI3 0 +#endif + +/* Select optimized code variant when xt-xcc compiler is used */ +#if IIR_AUTOARCH == 1 +#if defined __XCC__ +#include +#if XCHAL_HAVE_HIFI3 == 1 +#define IIR_GENERIC 0 +#define IIR_HIFI3 1 +#else +#define IIR_GENERIC 1 +#define IIR_HIFI3 0 +#endif /* XCHAL_HAVE_HIFI3 */ +#else +/* GCC */ +#define IIR_GENERIC 1 +#define IIR_HIFI3 0 +#endif /* __XCC__ */ +#endif /* IIR_AUTOARCH */ + +#define IIR_DF2T_NUM_DELAYS 2 + +struct iir_state_df2t { + unsigned int biquads; /* Number of IIR 2nd order sections total */ + unsigned 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 */ +}; + +int32_t iir_df2t(struct iir_state_df2t *iir, int32_t x); + +#endif /* __SOF_MATH_IIR_DF2T_H__ */ diff --git a/src/math/CMakeLists.txt b/src/math/CMakeLists.txt index 1a5158f531cc..de36aaccc95a 100644 --- a/src/math/CMakeLists.txt +++ b/src/math/CMakeLists.txt @@ -1,9 +1,9 @@ # SPDX-License-Identifier: BSD-3-Clause -add_local_sources(sof numbers.c trig.c decibels.c) +add_local_sources(sof numbers.c trig.c decibels.c iir_df2t_generic.c iir_df2t_hifi3.c) if(BUILD_LIBRARY) return() endif() -add_local_sources(sof trig.c decibels.c) \ No newline at end of file +add_local_sources(sof trig.c decibels.c iir_df2t_generic.c iir_df2t_hifi3.c) diff --git a/src/audio/eq_iir/iir_generic.c b/src/math/iir_df2t_generic.c similarity index 66% rename from src/audio/eq_iir/iir_generic.c rename to src/math/iir_df2t_generic.c index dadce43faf8f..7779307c5705 100644 --- a/src/audio/eq_iir/iir_generic.c +++ b/src/math/iir_df2t_generic.c @@ -6,8 +6,8 @@ // Liam Girdwood // Keyon Jie -#include #include +#include #include #include #include @@ -42,9 +42,13 @@ /* 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; + int32_t tmp; + int64_t acc; int32_t out = 0; int i; int j; @@ -59,7 +63,35 @@ int32_t iir_df2t(struct iir_state_df2t *iir, int32_t x) in = x; for (j = 0; j < iir->biquads; j += iir->biquads_in_series) { for (i = 0; i < iir->biquads_in_series; i++) { - in = iir_df2t_biquad(in, &iir->coef[c], &iir->delay[d]); + /* 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 first 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 second 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; + + /* Apply gain Q2.14 x Q1.31 -> Q3.45 */ + acc = ((int64_t)iir->coef[c + 6]) * tmp; /* Gain */ + + /* Apply biquad output shift right parameter + * simultaneously with Q3.45 to Q3.31 conversion. Then + * saturate to 32 bits Q1.31 and prepare for next + * biquad. + */ + acc = Q_SHIFT_RND(acc, 45 + iir->coef[c + 5], 31); + in = sat_int32(acc); + /* Proceed to next biquad coefficients and delay * lines. */ @@ -73,4 +105,3 @@ int32_t iir_df2t(struct iir_state_df2t *iir, int32_t x) } #endif - diff --git a/src/audio/eq_iir/iir_hifi3.c b/src/math/iir_df2t_hifi3.c similarity index 99% rename from src/audio/eq_iir/iir_hifi3.c rename to src/math/iir_df2t_hifi3.c index 597fa006bccb..b3483c4b7ebd 100644 --- a/src/audio/eq_iir/iir_hifi3.c +++ b/src/math/iir_df2t_hifi3.c @@ -10,7 +10,7 @@ #include #include #include -#include +#include #include #if IIR_HIFI3