From c935000338bcb7d886b2206c6b61a913af8d0966 Mon Sep 17 00:00:00 2001 From: ArturX Kloniecki Date: Tue, 16 Apr 2019 22:15:49 +0200 Subject: [PATCH] MUX: Generic Mux component implementation. Mux component implementation. Support for mux and demux operation modes, channel routing and summation with saturation. Mux component can operate in 2 modes: demux - with single input stream and multiple output streams (up to MUX_MAX_STREAMS) mux - with multiple input streams and single output stream It can also work with 1:1 I/O in both modes to act as channel router / summator. It is configured via binary blob specifying channel routing using bitmasks. In case of demux mode every output stream has an array of bitmasks, one mask per channel, specifying which of the input channels should be used to compose it. Similarly for mux, each input stream has bitmask for each of it's channels to specify to which output channel it contributes. Signed-off-by: ArturX Kloniecki --- CODEOWNERS | 1 + src/arch/xtensa/configs/baytrail_defconfig | 1 + src/arch/xtensa/configs/cherrytrail_defconfig | 1 + src/audio/CMakeLists.txt | 1 + src/audio/mux.c | 401 ++++++++++++++++- src/audio/mux.h | 110 +++++ src/audio/mux_generic.c | 406 ++++++++++++++++++ src/include/uapi/ipc/topology.h | 1 + 8 files changed, 915 insertions(+), 7 deletions(-) create mode 100644 src/audio/mux.h create mode 100644 src/audio/mux_generic.c diff --git a/CODEOWNERS b/CODEOWNERS index 15954e9c4f42..d94d5af30541 100644 --- a/CODEOWNERS +++ b/CODEOWNERS @@ -22,6 +22,7 @@ src/audio/fir* @singalsu src/audio/iir* @singalsu src/audio/tone.c @singalsu src/audio/kpb.c @mrajwa +src/audio/mux* @akloniex # platforms src/arch/xtensa/* @tlauda diff --git a/src/arch/xtensa/configs/baytrail_defconfig b/src/arch/xtensa/configs/baytrail_defconfig index 433ac3e103ea..d1e10981ebc4 100644 --- a/src/arch/xtensa/configs/baytrail_defconfig +++ b/src/arch/xtensa/configs/baytrail_defconfig @@ -2,3 +2,4 @@ CONFIG_BAYTRAIL=y CONFIG_COMP_TEST_KEYPHRASE=n CONFIG_COMP_KPB=n CONFIG_COMP_SEL=n +CONFIG_COMP_MUX=n diff --git a/src/arch/xtensa/configs/cherrytrail_defconfig b/src/arch/xtensa/configs/cherrytrail_defconfig index 38106adfbb60..9aa624a7a9fc 100644 --- a/src/arch/xtensa/configs/cherrytrail_defconfig +++ b/src/arch/xtensa/configs/cherrytrail_defconfig @@ -2,3 +2,4 @@ CONFIG_CHERRYTRAIL=y CONFIG_COMP_TEST_KEYPHRASE=n CONFIG_COMP_KPB=n CONFIG_COMP_SEL=n +CONFIG_COMP_MUX=n diff --git a/src/audio/CMakeLists.txt b/src/audio/CMakeLists.txt index cd03493ac4c3..d9d5ccdaf4e5 100644 --- a/src/audio/CMakeLists.txt +++ b/src/audio/CMakeLists.txt @@ -31,6 +31,7 @@ if(NOT BUILD_LIBRARY) if(CONFIG_COMP_MUX) add_local_sources(sof mux.c + mux_generic.c ) endif() if(CONFIG_COMP_SWITCH) diff --git a/src/audio/mux.c b/src/audio/mux.c index 1326b51fec46..1ed06789723c 100644 --- a/src/audio/mux.c +++ b/src/audio/mux.c @@ -26,50 +26,360 @@ * POSSIBILITY OF SUCH DAMAGE. * * Author: Liam Girdwood + Artur Kloniecki */ +#include + +#if CONFIG_COMP_MUX + #include #include #include #include #include #include +#include +#include "mux.h" + +static int mux_set_values(struct comp_data *cd, struct sof_mux_config *cfg) +{ + uint8_t i; + uint8_t j; + + /* check if number of streams configured doesn't exceed maximum */ + if (cfg->num_streams > MUX_MAX_STREAMS) { + trace_mux_error("mux_set_values() error: configured number of " + "streams (%u) exceeds maximum = " + META_QUOTE(MUX_MAX_STREAMS), + cfg->num_streams); + return -EINVAL; + } + + /* check if all streams configured have distinct IDs */ + for (i = 0; i < cfg->num_streams; i++) { + for (j = i + 1; j < cfg->num_streams; j++) { + if (cfg->streams[i].pipeline_id == + cfg->streams[j].pipeline_id) { + trace_mux_error("mux_set_values() error: " + "multiple configured streams " + "have same pipeline ID = %u", + cfg->streams[i].pipeline_id); + return -EINVAL; + } + } + } -/* tracing */ -#define trace_mux(__e) trace_event(TRACE_CLASS_MUX, __e) -#define trace_mux_error(__e) trace_error(TRACE_CLASS_MUX, __e) -#define tracev_mux(__e) tracev_event(TRACE_CLASS_MUX, __e) + /* check if number of channels per stream doesn't exceed maximum */ + for (i = 0; i < cfg->num_streams; i++) { + if (cfg->streams[i].num_channels > PLATFORM_MAX_CHANNELS) { + trace_mux_error("mux_set_values() error: configured " + "number of channels for stream %u " + "exceeds platform maximum = " + META_QUOTE(PLATFORM_MAX_CHANNELS), + i); + return -EINVAL; + } + } + + cd->config.num_channels = cfg->num_channels; + cd->config.frame_format = cfg->frame_format; + + for (i = 0; i < cfg->num_streams; i++) { + cd->config.streams[i].num_channels = cfg->streams[i].num_channels; + cd->config.streams[i].pipeline_id = cfg->streams[i].pipeline_id; + for (j = 0; j < cfg->streams[i].num_channels; j++) + cd->config.streams[i].mask[j] = cfg->streams[i].mask[j]; + } + + return 0; +} static struct comp_dev *mux_new(struct sof_ipc_comp *comp) { + struct sof_ipc_comp_process *ipc_process = + (struct sof_ipc_comp_process *)comp; + size_t bs = ipc_process->size; + struct comp_dev *dev; + struct comp_data *cd; + int ret; + trace_mux("mux_new()"); - return NULL; + if (IPC_IS_SIZE_INVALID(ipc_process->config)) { + IPC_SIZE_ERROR_TRACE(TRACE_CLASS_MUX, ipc_process->config); + return NULL; + } + + dev = rzalloc(RZONE_RUNTIME, SOF_MEM_CAPS_RAM, + COMP_SIZE(struct sof_ipc_comp_process)); + if (!dev) + return NULL; + + memcpy(&dev->comp, comp, sizeof(struct sof_ipc_comp_process)); + + cd = rzalloc(RZONE_RUNTIME, SOF_MEM_CAPS_RAM, + sizeof(*cd) + MUX_MAX_STREAMS * sizeof(struct mux_stream_data)); + if (!cd) { + rfree(dev); + return NULL; + } + + comp_set_drvdata(dev, cd); + + memcpy_s(&cd->config, sizeof(struct sof_mux_config) + + MUX_MAX_STREAMS * sizeof(struct mux_stream_data), + ipc_process->data, bs); + + /* verification of initial parameters */ + ret = mux_set_values(cd, &cd->config); + + if (ret < 0) { + rfree(cd); + rfree(dev); + return NULL; + } + + dev->state = COMP_STATE_READY; + return dev; } static void mux_free(struct comp_dev *dev) { + struct comp_data *cd = comp_get_drvdata(dev); + + trace_mux("mux_free()"); + rfree(cd); + rfree(dev); } /* set component audio stream parameters */ static int mux_params(struct comp_dev *dev) { + struct comp_data *cd = comp_get_drvdata(dev); + + trace_mux("mux_params()"); + + cd->config.num_channels = dev->params.channels; + cd->config.frame_format = dev->params.frame_fmt; return 0; } +static int mux_ctrl_set_cmd(struct comp_dev *dev, + struct sof_ipc_ctrl_data *cdata) +{ + struct comp_data *cd = comp_get_drvdata(dev); + struct sof_mux_config *cfg; + int ret = 0; + + trace_mux("mux_ctrl_set_cmd(), cdata->cmd = 0x%08x", cdata->cmd); + + switch (cdata->cmd) { + case SOF_CTRL_CMD_BINARY: + cfg = (struct sof_mux_config *)cdata->data->data; + + ret = mux_set_values(cd, cfg); + break; + default: + trace_mux_error("mux_ctrl_set_cmd() error: invalid cdata->cmd =" + " 0x%08x", cdata->cmd); + ret = -EINVAL; + break; + } + + return ret; +} + /* used to pass standard and bespoke commands (with data) to component */ static int mux_cmd(struct comp_dev *dev, int cmd, void *data, int max_data_size) { - /* mux will use buffer "connected" status */ + struct sof_ipc_ctrl_data *cdata = data; + + trace_mux("mux_cmd() cmd = 0x%08x", cmd); + + switch (cmd) { + case COMP_CMD_SET_DATA: + return mux_ctrl_set_cmd(dev, cdata); + default: + return -EINVAL; + } +} + +static uint8_t get_stream_index(struct comp_data *cd, uint32_t pipe_id) +{ + int i; + + for (i = 0; i < MUX_MAX_STREAMS; i++) { + if (cd->config.streams[i].pipeline_id == pipe_id) + return i; + } + trace_mux_error("get_stream_index() error: couldn't find configuration " + "for connected pipeline %u", pipe_id); return 0; } -/* copy and process stream data from source to sink buffers */ +/* process and copy stream data from source to sink buffers */ +static int demux_copy(struct comp_dev *dev) +{ + struct comp_data *cd = comp_get_drvdata(dev); + struct comp_buffer *source; + struct comp_buffer *sink; + struct comp_buffer *sinks[MUX_MAX_STREAMS] = { NULL }; + struct list_item *clist; + uint32_t num_sinks = 0; + uint32_t i = 0; + uint32_t frames = -1; + uint32_t source_bytes; + uint32_t sinks_bytes[MUX_MAX_STREAMS] = { 0 }; + + tracev_mux("demux_copy()"); + + // align sink streams with their respective configurations + list_for_item(clist, &dev->bsink_list) { + sink = container_of(clist, struct comp_buffer, source_list); + if (sink->sink->state == dev->state) { + num_sinks++; + i = get_stream_index(cd, sink->ipc_buffer.comp.pipeline_id); + sinks[i] = sink; + } + } + + /* if there are no sinks active */ + if (num_sinks == 0) + return 0; + + source = list_first_item(&dev->bsource_list, struct comp_buffer, + sink_list); + + /* check if source is active */ + if (source->source->state != dev->state) + return 0; + + /* check for underrun */ + if (source->avail == 0) { + trace_mux_error("demux_copy() error: source component buffer " + "has not enough data avaialble."); + comp_underrun(dev, source, 0, 0); + return -EIO; + } + + for (i = 0; i < MUX_MAX_STREAMS; i++) { + if (!sinks[i]) + continue; + frames = MIN(frames, comp_avail_frames(source, sinks[i])); + } + + source_bytes = frames * comp_frame_bytes(source->source); + for (i = 0; i < MUX_MAX_STREAMS; i++) { + if (!sinks[i]) + continue; + sinks_bytes[i] = frames * comp_frame_bytes(sinks[i]->sink); + } + + /* produce output, one sink at a time */ + for (i = 0; i < MUX_MAX_STREAMS; i++) { + if (!sinks[i]) + continue; + + cd->demux(dev, sinks[i], source, frames, &cd->config.streams[i]); + } + + /* update components */ + for (i = 0; i < MUX_MAX_STREAMS; i++) { + if (!sinks[i]) + continue; + comp_update_buffer_produce(sinks[i], sinks_bytes[i]); + } + comp_update_buffer_consume(source, source_bytes); + + return 0; +} + +/* process and copy stream data from source to sink buffers */ static int mux_copy(struct comp_dev *dev) { + struct comp_data *cd = comp_get_drvdata(dev); + struct comp_buffer *sink; + struct comp_buffer *source; + struct comp_buffer *sources[MUX_MAX_STREAMS] = { NULL }; + struct list_item *clist; + uint32_t num_sources = 0; + uint32_t i = 0; + uint32_t frames = -1; + uint32_t sources_bytes[MUX_MAX_STREAMS] = { 0 }; + uint32_t sink_bytes; + + tracev_mux("mux_copy()"); + + /* align source streams with their respective configurations */ + list_for_item(clist, &dev->bsource_list) { + source = container_of(clist, struct comp_buffer, sink_list); + if (source->source->state == dev->state) { + num_sources++; + i = get_stream_index(cd, source->ipc_buffer.comp.pipeline_id); + sources[i] = source; + } + } + + /* check if there are any sources active */ + if (num_sources == 0) + return 0; + + sink = list_first_item(&dev->bsink_list, struct comp_buffer, + source_list); + + /* check if sink is active */ + if (sink->sink->state != dev->state) + return 0; + + /* check for underrun */ + for (i = 0; i < MUX_MAX_STREAMS; i++) { + if (!sources[i]) + continue; + if (sources[i]->avail == 0) { + trace_mux_error("mux_copy() error: source " + "component buffer has not enough data " + "avaialble."); + comp_underrun(dev, sources[i], 0, 0); + return -EIO; + } + } + + /* check for overrun */ + if (sink->free == 0) { + trace_mux_error("mux_copy() error: sink component " + "buffer has not enough free bytes."); + comp_overrun(dev, sink, 0, 0); + return -EIO; + } + + for (i = 0; i < MUX_MAX_STREAMS; i++) { + if (!sources[i]) + continue; + frames = MIN(frames, comp_avail_frames(sources[i], sink)); + } + + for (i = 0; i < MUX_MAX_STREAMS; i++) { + if (!sources[i]) + continue; + sources_bytes[i] = frames * + comp_frame_bytes(sources[i]->source); + } + sink_bytes = frames * comp_frame_bytes(sink->sink); + + /* produce output */ + cd->mux(dev, sink, &sources[0], frames, &cd->config.streams[0]); + + /* update components */ + comp_update_buffer_produce(sink, sink_bytes); + for (i = 0; i < MUX_MAX_STREAMS; i++) { + if (!sources[i]) + continue; + comp_update_buffer_consume(sources[i], sources_bytes[i]); + } return 0; } @@ -81,7 +391,66 @@ static int mux_reset(struct comp_dev *dev) static int mux_prepare(struct comp_dev *dev) { + struct comp_data *cd = comp_get_drvdata(dev); + int ret; + + trace_mux("mux_prepare()"); + + ret = comp_set_state(dev, COMP_TRIGGER_PREPARE); + if (ret) { + trace_mux("mux_prepare() comp_set_state() returned non-zero."); + return ret; + } + + cd->mux = mux_get_processing_function(dev); + if (!cd->mux) { + trace_mux_error("mux_prepare() error: couldn't find appropriate" + " mux processing function for component."); + ret = -EINVAL; + goto err; + } + return 0; + +err: + comp_set_state(dev, COMP_TRIGGER_RESET); + return ret; +} + +static int demux_prepare(struct comp_dev *dev) +{ + struct comp_data *cd = comp_get_drvdata(dev); + int ret; + + trace_mux("demux_prepare()"); + + ret = comp_set_state(dev, COMP_TRIGGER_PREPARE); + if (ret) { + trace_mux("demux_prepare() comp_set_state() returned non-zero"); + return ret; + } + + cd->demux = demux_get_processing_function(dev); + if (!cd->demux) { + trace_mux_error("demux_prepare() error: couldn't find " + "appropriate demux processing function for " + "component."); + ret = -EINVAL; + goto err; + } + + return 0; + +err: + comp_set_state(dev, COMP_TRIGGER_RESET); + return ret; +} + +static int mux_trigger(struct comp_dev *dev, int cmd) +{ + trace_mux("mux_trigger(), command = %u", cmd); + + return comp_set_state(dev, cmd); } struct comp_driver comp_mux = { @@ -94,12 +463,30 @@ struct comp_driver comp_mux = { .copy = mux_copy, .prepare = mux_prepare, .reset = mux_reset, + .trigger = mux_trigger, + }, +}; + +struct comp_driver comp_demux = { + .type = SOF_COMP_DEMUX, + .ops = { + .new = mux_new, + .free = mux_free, + .params = mux_params, + .cmd = mux_cmd, + .copy = demux_copy, + .prepare = demux_prepare, + .reset = mux_reset, + .trigger = mux_trigger, }, }; static void sys_comp_mux_init(void) { comp_register(&comp_mux); + comp_register(&comp_demux); } DECLARE_MODULE(sys_comp_mux_init); + +#endif /* CONFIG_COMP_MUX */ diff --git a/src/audio/mux.h b/src/audio/mux.h new file mode 100644 index 000000000000..70dcb80aa50b --- /dev/null +++ b/src/audio/mux.h @@ -0,0 +1,110 @@ +/* + * Copyright (c) 2019, 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: Artur Kloniecki + */ + +/** + * \file audio/mux.h + * \brief Multiplexer component header file + * \authors Artur Kloniecki + */ + +#ifndef MUX_H +#define MUX_H + +#include + +#if CONFIG_COMP_MUX + +#include +#include +#include +#include + + /* tracing */ +#define trace_mux(__e, ...) trace_event(TRACE_CLASS_MUX, __e, ##__VA_ARGS__) +#define trace_mux_error(__e, ...) \ + trace_error(TRACE_CLASS_MUX, __e, ##__VA_ARGS__) +#define tracev_mux(__e, ...) \ + tracev_event(TRACE_CLASS_MUX, __e, ##__VA_ARGS__) + +/** \brief Supported streams count. */ +#define MUX_MAX_STREAMS 4 + +/** guard against invalid amount of streams defined */ +STATIC_ASSERT(MUX_MAX_STREAMS < PLATFORM_MAX_STREAMS, + unsupported_amount_of_streams_for_mux); + +struct mux_stream_data { + uint32_t pipeline_id; + uint8_t num_channels; + uint8_t mask[PLATFORM_MAX_CHANNELS]; + + uint8_t reserved[(20 - PLATFORM_MAX_CHANNELS - 1) % 4]; // padding to ensure proper alignment of following instances +}; + +typedef void(*demux_func)(struct comp_dev *dev, struct comp_buffer *sink, + struct comp_buffer *source, uint32_t frames, + struct mux_stream_data *data); +typedef void(*mux_func)(struct comp_dev *dev, struct comp_buffer *sink, + struct comp_buffer **sources, uint32_t frames, + struct mux_stream_data *data); + +struct sof_mux_config { + uint16_t frame_format; + uint16_t num_channels; + uint16_t num_streams; + + uint16_t reserved; // padding to ensure proper alignment + + struct mux_stream_data streams[]; +}; + +struct comp_data { + union { + mux_func mux; + demux_func demux; + }; + + struct sof_mux_config config; +}; + +struct comp_func_map { + uint16_t frame_format; + mux_func mux_proc_func; + demux_func demux_proc_func; +}; + +extern const struct comp_func_map mux_func_map[]; + +mux_func mux_get_processing_function(struct comp_dev *dev); +demux_func demux_get_processing_function(struct comp_dev *dev); + +#endif /* CONFIG_COMP_MUX */ + +#endif /* MUX_H */ diff --git a/src/audio/mux_generic.c b/src/audio/mux_generic.c new file mode 100644 index 000000000000..18a5ebf3be07 --- /dev/null +++ b/src/audio/mux_generic.c @@ -0,0 +1,406 @@ +/* + * Copyright (c) 2019, 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: Artur Kloniecki + */ + +#include + +#if CONFIG_COMP_MUX + +#include "mux.h" + +/* + * \brief Fetch 16b samples from source buffer and perform routing operations + * based on mask provided. + * \param[in,out] source Source buffer. + * \param[in] num_ch Number of channels in source buffer. + * \param[in] offset Offset in source buffer. + * \param[in] mask Routing bitmask for calculating output sample. + */ +static inline int32_t calc_sample_s16le(struct comp_buffer *source, + uint8_t num_ch, uint32_t offset, + uint8_t mask) +{ + int32_t sample = 0; + int16_t *src; + int8_t in_ch; + + if (mask == 0) + return 0; + + for (in_ch = 0; in_ch < num_ch; in_ch++) { + if (mask & BIT(in_ch)) { + src = buffer_read_frag_s16(source, offset + in_ch); + sample += *src; + } + } + + return sample; +} + +/* + * \brief Fetch 24b samples from source buffer and perform routing operations + * based on mask provided. + * \param[in,out] source Source buffer. + * \param[in] num_ch Number of channels in source buffer. + * \param[in] offset Offset in source buffer. + * \param[in] mask Routing bitmask for calculating output sample. + */ +static inline int32_t calc_sample_s24le(struct comp_buffer *source, + uint8_t num_ch, uint32_t offset, + uint8_t mask) +{ + int32_t sample = 0; + int32_t *src; + int8_t in_ch; + + if (mask == 0) + return 0; + + for (in_ch = 0; in_ch < num_ch; in_ch++) { + if (mask & BIT(in_ch)) { + src = buffer_read_frag_s32(source, offset + in_ch); + sample += sign_extend_s24(*src); + } + } + + return sample; +} + +/* + * \brief Fetch 32b samples from source buffer and perform routing operations + * based on mask provided. + * \param[in,out] source Source buffer. + * \param[in] num_ch Number of channels in source buffer. + * \param[in] offset Offset in source buffer. + * \param[in] mask Routing bitmask for calculating output sample. + */ +static inline int64_t calc_sample_s32le(struct comp_buffer *source, + uint8_t num_ch, uint32_t offset, + uint8_t mask) +{ + int64_t sample = 0; + int32_t *src; + int8_t in_ch; + + if (mask == 0) + return 0; + + for (in_ch = 0; in_ch < num_ch; in_ch++) { + if (mask & BIT(in_ch)) { + src = buffer_read_frag_s32(source, offset + in_ch); + sample += *src; + } + } + + return sample; +} + +/* \brief Demuxing 16 bit streams. + * + * Source stream is routed to sink with regard to routing bitmasks from + * mux_stream_data structure. Each bitmask describes composition of single + * output channel. + * + * \param[in,out] dev Demux base component device. + * \param[in,out] sink Destination buffer. + * \param[in,out] source Source buffer. + * \param[in] frames Number of frames to process. + * \param[in] data Parameters describing channel count and routing. + */ +static void demux_s16le(struct comp_dev *dev, struct comp_buffer *sink, + struct comp_buffer *source, uint32_t frames, + struct mux_stream_data *data) +{ + struct comp_data *cd = comp_get_drvdata(dev); + int32_t sample; + int16_t *dst; + uint8_t i; + uint8_t out_ch; + + for (i = 0; i < frames; i++) { + for (out_ch = 0; out_ch < data->num_channels; out_ch++) { + sample = calc_sample_s16le(source, + cd->config.num_channels, + i * cd->config.num_channels, + data->mask[out_ch]); + + /* saturate to 16 bits */ + dst = buffer_write_frag_s16(sink, + i * data->num_channels + out_ch); + *dst = sat_int16(sample); + } + } +} + +/* \brief Demuxing 24 bit streams. + * + * Source stream is routed to sink with regard to routing bitmasks from + * mux_stream_data structure. Each bitmask describes composition of single + * output channel. + * + * \param[in,out] dev Demux base component device. + * \param[in,out] sink Destination buffer. + * \param[in,out] source Source buffer. + * \param[in] frames Number of frames to process. + * \param[in] data Parameters describing channel count and routing. + */ +static void demux_s24le(struct comp_dev *dev, struct comp_buffer *sink, + struct comp_buffer *source, uint32_t frames, + struct mux_stream_data *data) +{ + struct comp_data *cd = comp_get_drvdata(dev); + int32_t sample; + int32_t *dst; + uint8_t i; + uint8_t out_ch; + + for (i = 0; i < frames; i++) { + for (out_ch = 0; out_ch < data->num_channels; out_ch++) { + sample = calc_sample_s24le(source, + cd->config.num_channels, + i * cd->config.num_channels, + data->mask[out_ch]); + + /* saturate to 24 bits */ + dst = buffer_write_frag_s32(sink, + i * data->num_channels + out_ch); + *dst = sat_int24(sample); + } + } +} + +/* \brief Demuxing 32 bit streams. + * + * Source stream is routed to sink with regard to routing bitmasks from + * mux_stream_data structure. Each bitmask describes composition of single + * output channel. + * + * \param[in,out] dev Demux base component device. + * \param[in,out] sink Destination buffer. + * \param[in,out] source Source buffer. + * \param[in] frames Number of frames to process. + * \param[in] data Parameters describing channel count and routing. + */ +static void demux_s32le(struct comp_dev *dev, struct comp_buffer *sink, + struct comp_buffer *source, uint32_t frames, + struct mux_stream_data *data) +{ + struct comp_data *cd = comp_get_drvdata(dev); + int64_t sample; + int32_t *dst; + uint8_t i; + uint8_t out_ch; + + for (i = 0; i < frames; i++) { + for (out_ch = 0; out_ch < data->num_channels; out_ch++) { + sample = calc_sample_s32le(source, + cd->config.num_channels, + i * cd->config.num_channels, + data->mask[out_ch]); + + /* saturate to 32 bits */ + dst = buffer_write_frag_s32(sink, + i * data->num_channels + out_ch); + *dst = sat_int32(sample); + } + } +} + +/* \brief Muxing 16 bit streams. + * + * Source streams are routed to sink with regard to routing bitmasks from + * mux_stream_data structures array. Each source stream has bitmask for each + * of it's channels describing to which channels of output stream it + * contributes. + * + * \param[in,out] dev Demux base component device. + * \param[in,out] sink Destination buffer. + * \param[in,out] sources Array of source buffers. + * \param[in] frames Number of frames to process. + * \param[in] data Array of parameters describing channel count and routing for + * each stream. + */ +static void mux_s16le(struct comp_dev *dev, struct comp_buffer *sink, + struct comp_buffer **sources, uint32_t frames, + struct mux_stream_data *data) +{ + struct comp_data *cd = comp_get_drvdata(dev); + struct comp_buffer *source; + uint8_t i; + uint8_t j; + uint8_t out_ch; + int16_t *dst; + int32_t sample; + + for (i = 0; i < frames; i++) { + for (out_ch = 0; out_ch < cd->config.num_channels; out_ch++) { + sample = 0; + + for (j = 0; j < MUX_MAX_STREAMS; j++) { + source = sources[j]; + if (!source) + continue; + + sample += calc_sample_s16le(source, + data[j].num_channels, + i * data[j].num_channels, + data[j].mask[out_ch]); + } + dst = buffer_write_frag_s16(sink, + i * data->num_channels + out_ch); + *dst = sat_int16(sample); + } + } +} + +/* \brief Muxing 24 bit streams. + * + * Source streams are routed to sink with regard to routing bitmasks from + * mux_stream_data structures array. Each source stream has bitmask for each + * of it's channels describing to which channels of output stream it + * contributes. + * + * \param[in,out] dev Demux base component device. + * \param[in,out] sink Destination buffer. + * \param[in,out] sources Array of source buffers. + * \param[in] frames Number of frames to process. + * \param[in] data Array of parameters describing channel count and routing for + * each stream. + */ +static void mux_s24le(struct comp_dev *dev, struct comp_buffer *sink, + struct comp_buffer **sources, uint32_t frames, + struct mux_stream_data *data) +{ + struct comp_data *cd = comp_get_drvdata(dev); + struct comp_buffer *source; + uint8_t i; + uint8_t j; + uint8_t out_ch; + int32_t *dst; + int32_t sample; + + for (i = 0; i < frames; i++) { + for (out_ch = 0; out_ch < cd->config.num_channels; out_ch++) { + sample = 0; + for (j = 0; j < MUX_MAX_STREAMS; j++) { + source = sources[j]; + if (!source) + continue; + + sample += calc_sample_s24le(source, + data[j].num_channels, + i * data[j].num_channels, + data[j].mask[out_ch]); + } + dst = buffer_write_frag_s32(sink, + i * data->num_channels + out_ch); + *dst = sat_int24(sample); + } + } +} + +/* \brief Muxing 32 bit streams. + * + * Source streams are routed to sink with regard to routing bitmasks from + * mux_stream_data structures array. Each source stream has bitmask for each + * of it's channels describing to which channels of output stream it + * contributes. + * + * \param[in,out] dev Demux base component device. + * \param[in,out] sink Destination buffer. + * \param[in,out] sources Array of source buffers. + * \param[in] frames Number of frames to process. + * \param[in] data Array of parameters describing channel count and routing for + * each stream. + */ +static void mux_s32le(struct comp_dev *dev, struct comp_buffer *sink, + struct comp_buffer **sources, uint32_t frames, + struct mux_stream_data *data) +{ + struct comp_data *cd = comp_get_drvdata(dev); + struct comp_buffer *source; + uint8_t i; + uint8_t j; + uint8_t out_ch; + int32_t *dst; + int64_t sample; + + for (i = 0; i < frames; i++) { + for (out_ch = 0; out_ch < cd->config.num_channels; out_ch++) { + sample = 0; + for (j = 0; j < MUX_MAX_STREAMS; j++) { + source = sources[j]; + if (!source) + continue; + + sample += calc_sample_s32le(source, + data[j].num_channels, + i * data[j].num_channels, + data[j].mask[out_ch]); + } + dst = buffer_write_frag_s32(sink, + i * data->num_channels + out_ch); + *dst = sat_int32(sample); + } + } +} + +const struct comp_func_map mux_func_map[] = { + { SOF_IPC_FRAME_S16_LE, &mux_s16le, &demux_s16le }, + { SOF_IPC_FRAME_S24_4LE, &mux_s24le, &demux_s24le }, + { SOF_IPC_FRAME_S32_LE, &mux_s32le, &demux_s32le }, +}; + +mux_func mux_get_processing_function(struct comp_dev *dev) +{ + struct comp_data *cd = comp_get_drvdata(dev); + uint8_t i; + + for (i = 0; i < ARRAY_SIZE(mux_func_map); i++) { + if (cd->config.frame_format == mux_func_map[i].frame_format) + return mux_func_map[i].mux_proc_func; + } + + return NULL; +} + +demux_func demux_get_processing_function(struct comp_dev *dev) +{ + struct comp_data *cd = comp_get_drvdata(dev); + uint8_t i; + + for (i = 0; i < ARRAY_SIZE(mux_func_map); i++) { + if (cd->config.frame_format == mux_func_map[i].frame_format) + return mux_func_map[i].demux_proc_func; + } + + return NULL; +} + +#endif /* CONFIG_COMP_MUX */ diff --git a/src/include/uapi/ipc/topology.h b/src/include/uapi/ipc/topology.h index 6322f16018fa..987a32ac6a44 100644 --- a/src/include/uapi/ipc/topology.h +++ b/src/include/uapi/ipc/topology.h @@ -65,6 +65,7 @@ enum sof_comp_type { SOF_COMP_KEYWORD_DETECT, SOF_COMP_KPB, /* A key phrase buffer component */ SOF_COMP_SELECTOR, /**< channel selector component */ + SOF_COMP_DEMUX, /* 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 */