[Sound-open-firmware] [PATCH 1/2] ipc: add parameters validation
From: Tomasz Lauda tomasz.lauda@linux.intel.com
This patch adds parameters validation for ipc. It uses generic macros.
Signed-off-by: Tomasz Lauda tomasz.lauda@linux.intel.com --- src/ipc/handler.c | 53 ++++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 52 insertions(+), 1 deletion(-)
diff --git a/src/ipc/handler.c b/src/ipc/handler.c index 581ce9e..a83ef69 100644 --- a/src/ipc/handler.c +++ b/src/ipc/handler.c @@ -62,6 +62,9 @@ #define iGS(x) ((x >> SOF_GLB_TYPE_SHIFT) & 0xf) #define iCS(x) ((x >> SOF_CMD_TYPE_SHIFT) & 0xfff)
+#define IPC_INVALID_SIZE(ipc) \ + (sizeof(*(ipc)) != ipc->hdr.size) + /* IPC context - shared with platform IPC driver */ struct ipc *_ipc;
@@ -106,12 +109,18 @@ static int ipc_stream_pcm_params(uint32_t stream)
trace_ipc("SAl");
+ /* sanity check size */ + if (IPC_INVALID_SIZE(pcm_params)) { + trace_ipc_error("eAS"); + return -EINVAL; + } + /* get the pcm_dev */ pcm_dev = ipc_get_comp(_ipc, pcm_params->comp_id); if (pcm_dev == NULL) { trace_ipc_error("eAC"); trace_error_value(pcm_params->comp_id); - return -EINVAL; + return -ENODEV; }
/* sanity check comp */ @@ -214,6 +223,12 @@ static int ipc_stream_pcm_free(uint32_t header)
trace_ipc("SFr");
+ /* sanity check size */ + if (IPC_INVALID_SIZE(free_req)) { + trace_ipc_error("eFs"); + return -EINVAL; + } + /* get the pcm_dev */ pcm_dev = ipc_get_comp(_ipc, free_req->comp_id); if (pcm_dev == NULL) { @@ -243,6 +258,12 @@ static int ipc_stream_position(uint32_t header)
memset(&posn, 0, sizeof(posn));
+ /* sanity check size */ + if (IPC_INVALID_SIZE(stream)) { + trace_ipc_error("ePs"); + return -EINVAL; + } + /* get the pcm_dev */ pcm_dev = ipc_get_comp(_ipc, stream->comp_id); if (pcm_dev == NULL) { @@ -314,6 +335,12 @@ static int ipc_stream_trigger(uint32_t header)
trace_ipc("tri");
+ /* sanity check size */ + if (IPC_INVALID_SIZE(stream)) { + trace_ipc_error("eRs"); + return -EINVAL; + } + /* get the pcm_dev */ pcm_dev = ipc_get_comp(_ipc, stream->comp_id); if (pcm_dev == NULL) { @@ -525,6 +552,12 @@ static int ipc_dma_trace_config(uint32_t header)
trace_ipc("DA1");
+ /* sanity check size */ + if (IPC_INVALID_SIZE(params)) { + trace_ipc_error("DAs"); + return -EINVAL; + } + #ifdef CONFIG_HOST_PTABLE
list_init(&elem_list); @@ -734,6 +767,12 @@ static int ipc_glb_tplg_pipe_new(uint32_t header)
trace_ipc("Ipn");
+ /* sanity check size */ + if (IPC_INVALID_SIZE(ipc_pipeline)) { + trace_ipc_error("Ips"); + return -EINVAL; + } + ret = ipc_pipeline_new(_ipc, ipc_pipeline); if (ret < 0) { trace_ipc_error("pn1"); @@ -764,6 +803,12 @@ static int ipc_glb_tplg_comp_connect(uint32_t header)
trace_ipc("Icn");
+ /* sanity check size */ + if (IPC_INVALID_SIZE(connect)) { + trace_ipc_error("Ics"); + return -EINVAL; + } + return ipc_comp_connect(_ipc, connect); }
@@ -774,6 +819,12 @@ static int ipc_glb_tplg_free(uint32_t header,
trace_ipc("Tcf");
+ /* sanity check size */ + if (IPC_INVALID_SIZE(ipc_free)) { + trace_ipc_error("Tcs"); + return -EINVAL; + } + /* free the object */ free_func(_ipc, ipc_free->id);
From: Tomasz Lauda tomasz.lauda@linux.intel.com
This patch adds HiFi3 implementation of volume component. Implementation was splitted into generic and HiFi 3 specific.
Signed-off-by: Tomasz Lauda tomasz.lauda@linux.intel.com --- src/audio/Makefile.am | 26 +- src/audio/volume.c | 568 +++---------------------------------- src/audio/volume.h | 95 +++++++ src/audio/volume_generic.c | 510 +++++++++++++++++++++++++++++++++ src/audio/volume_hifi3.c | 307 ++++++++++++++++++++ 5 files changed, 964 insertions(+), 542 deletions(-) create mode 100644 src/audio/volume.h create mode 100644 src/audio/volume_generic.c create mode 100644 src/audio/volume_hifi3.c
diff --git a/src/audio/Makefile.am b/src/audio/Makefile.am index 5f69b65..482c982 100644 --- a/src/audio/Makefile.am +++ b/src/audio/Makefile.am @@ -6,7 +6,8 @@ include_HEADERS = \ fir.h \ src_config.h \ src.h \ - eq_fir.h + eq_fir.h \ + volume.h
COMP_SRC = \ eq_iir.c \ @@ -19,6 +20,7 @@ COMP_SRC = \ mixer.c \ mux.c \ volume.c \ + volume_generic.c \ switch.c \ dai.c \ host.c \ @@ -45,6 +47,10 @@ EQ_IIR_SRC = \ eq_iir.c \ iir.c
+VOLUME_SRC = \ + volume.c \ + volume_generic.c + if BUILD_LIB
# only host builds shared libraries, the rest are static @@ -109,7 +115,7 @@ libsof_eq_iir_la_LDFLAGS = \ # libsof_volume lib_LTLIBRARIES += libsof_volume.la
-libsof_volume_la_SOURCES = volume.c +libsof_volume_la_SOURCES = $(VOLUME_SRC)
libsof_volume_la_CFLAGS = \ $(ARCH_CFLAGS) \ @@ -240,7 +246,7 @@ libsof_eq_iir_sse42_la_LDFLAGS = \ # libsof_volume lib_LTLIBRARIES += libsof_volume_sse42.la
-libsof_volume_sse42_la_SOURCES = volume.c +libsof_volume_sse42_la_SOURCES = $(VOLUME_SRC)
libsof_volume_sse42_la_CFLAGS = \ $(ARCH_CFLAGS) \ @@ -377,7 +383,7 @@ libsof_eq_iir_avx_la_LDFLAGS = \ # libsof_volume lib_LTLIBRARIES += libsof_volume_avx.la
-libsof_volume_avx_la_SOURCES = volume.c +libsof_volume_avx_la_SOURCES = $(VOLUME_SRC)
libsof_volume_avx_la_CFLAGS = \ $(ARCH_CFLAGS) \ @@ -514,7 +520,7 @@ libsof_eq_iir_avx2_la_LDFLAGS = \ # libsof_volume lib_LTLIBRARIES += libsof_volume_avx2.la
-libsof_volume_avx2_la_SOURCES = volume.c +libsof_volume_avx2_la_SOURCES = $(VOLUME_SRC)
libsof_volume_avx2_la_CFLAGS = \ $(ARCH_CFLAGS) \ @@ -652,7 +658,7 @@ libsof_eq_iir_fma_la_LDFLAGS = \ # libsof_volume lib_LTLIBRARIES += libsof_volume_fma.la
-libsof_volume_fma_la_SOURCES = volume.c +libsof_volume_fma_la_SOURCES = $(VOLUME_SRC)
libsof_volume_fma_la_CFLAGS = \ $(ARCH_CFLAGS) \ @@ -768,7 +774,7 @@ libsof_eq_iir_a_CFLAGS = \ # libsof_volume lib_LIBRARIES += libsof_volume.a
-libsof_volume_a_SOURCES = volume.c +libsof_volume_a_SOURCES = $(VOLUME_SRC)
libsof_volume_a_CFLAGS = \ $(ARCH_CFLAGS) \ @@ -855,7 +861,7 @@ libsof_eq_iir_hifi2ep_a_CFLAGS = \ # libsof_volume lib_LIBRARIES += libsof_volume_hifi2ep.a
-libsof_volume_hifi2ep_a_SOURCES = volume.c +libsof_volume_hifi2ep_a_SOURCES = $(VOLUME_SRC)
libsof_volume_hifi2ep_a_CFLAGS = \ $(ARCH_CFLAGS) \ @@ -947,7 +953,7 @@ libsof_eq_iir_hifi3_a_CFLAGS = \ # libsof_volume lib_LIBRARIES += libsof_volume_hifi3.a
-libsof_volume_hifi3_a_SOURCES = volume.c +libsof_volume_hifi3_a_SOURCES = $(VOLUME_SRC)
libsof_volume_hifi3_a_CFLAGS = \ $(ARCH_CFLAGS) \ @@ -1016,6 +1022,8 @@ libaudio_a_SOURCES = \ mixer.c \ mux.c \ volume.c \ + volume_generic.c \ + volume_hifi3.c \ switch.c \ dai.c \ host.c \ diff --git a/src/audio/volume.c b/src/audio/volume.c index d0b690b..9e2968d 100644 --- a/src/audio/volume.c +++ b/src/audio/volume.c @@ -27,9 +27,9 @@ * * Author: Liam Girdwood liam.r.girdwood@linux.intel.com * Keyon Jie yang.jie@linux.intel.com + * Tomasz Lauda tomasz.lauda@linux.intel.com */
-#include <stdint.h> #include <stddef.h> #include <errno.h> #include <sof/sof.h> @@ -39,503 +39,7 @@ #include <sof/alloc.h> #include <sof/work.h> #include <sof/clock.h> -#include <sof/audio/component.h> -#include <sof/audio/pipeline.h> -#include <sof/audio/format.h> - -#define trace_volume(__e) trace_event(TRACE_CLASS_VOLUME, __e) -#define tracev_volume(__e) tracev_event(TRACE_CLASS_VOLUME, __e) -#define trace_volume_error(__e) trace_error(TRACE_CLASS_VOLUME, __e) - -/* this should ramp from 0dB to mute in 64ms. - * i.e 2^16 -> 0 in 32 * 2048 steps each lasting 2ms - */ -#define VOL_RAMP_US 2000 -#define VOL_RAMP_STEP (1 << 11) -#define VOL_MAX (1 << 16) -#define VOL_MIN 0 - -/* - * Simple volume control - * - * Gain amplitude value is between 0 (mute) ... 2^16 (0dB) ... 2^24 (~+48dB) - * - * Currently we use 16 bit data for copies to/from DAIs and HOST PCM buffers, - * 32 bit data is used in all other cases for overhead. - * TODO: Add 24 bit (4 byte aligned) support using HiFi2 EP SIMD. - */ - -/* volume component private data */ -struct comp_data { - uint32_t source_period_bytes; - uint32_t sink_period_bytes; - enum sof_ipc_frame source_format; - enum sof_ipc_frame sink_format; - uint32_t chan[SOF_IPC_MAX_CHANNELS]; - uint32_t volume[SOF_IPC_MAX_CHANNELS]; /* current volume */ - uint32_t tvolume[SOF_IPC_MAX_CHANNELS]; /* target volume */ - uint32_t mvolume[SOF_IPC_MAX_CHANNELS]; /* mute volume */ - void (*scale_vol)(struct comp_dev *dev, struct comp_buffer *sink, - struct comp_buffer *source, uint32_t frames); - struct work volwork; - - /* host volume readback */ - struct sof_ipc_ctrl_value_chan *hvol; -}; - -struct comp_func_map { - uint16_t source; /* source format */ - uint16_t sink; /* sink format */ - uint16_t channels; /* channel number for the stream */ - void (*func)(struct comp_dev *dev, struct comp_buffer *sink, - struct comp_buffer *source, uint32_t frames); -}; - -/* volume scaling functions for stereo input */ - -/* copy and scale volume from 16 bit source buffer to 32 bit dest buffer */ -static void vol_s16_to_s32_2ch(struct comp_dev *dev, struct comp_buffer *sink, - struct comp_buffer *source, uint32_t frames) -{ - struct comp_data *cd = comp_get_drvdata(dev); - int16_t *src = (int16_t *) source->r_ptr; - int32_t *dest = (int32_t *) sink->w_ptr; - int32_t i; - - /* buffer sizes are always divisible by period frames */ - /* Samples are Q1.15 --> Q1.31 and volume is Q1.16 */ - for (i = 0; i < frames * 2; i += 2) { - dest[i] = (int32_t)src[i] * cd->volume[0]; - dest[i + 1] = (int32_t)src[i + 1] * cd->volume[1]; - } -} - -/* copy and scale volume from 32 bit source buffer to 16 bit dest buffer */ -static void vol_s32_to_s16_2ch(struct comp_dev *dev, struct comp_buffer *sink, - struct comp_buffer *source, uint32_t frames) -{ - struct comp_data *cd = comp_get_drvdata(dev); - int32_t *src = (int32_t *) source->r_ptr; - int16_t *dest = (int16_t *) sink->w_ptr; - int32_t i; - - /* buffer sizes are always divisible by period frames */ - /* Samples are Q1.31 --> Q1.15 and volume is Q1.16 */ - for (i = 0; i < frames * 2; i += 2) { - dest[i] = (int16_t)q_multsr_sat_32x32( - src[i], cd->volume[0], Q_SHIFT_BITS_64(31, 16, 15)); - dest[i + 1] = (int16_t)q_multsr_sat_32x32( - src[i + 1], cd->volume[1], Q_SHIFT_BITS_64(31, 16, 15)); - } -} - -/* copy and scale volume from 32 bit source buffer to 32 bit dest buffer */ -static void vol_s32_to_s32_2ch(struct comp_dev *dev, struct comp_buffer *sink, - struct comp_buffer *source, uint32_t frames) -{ - struct comp_data *cd = comp_get_drvdata(dev); - int32_t *src = (int32_t *) source->r_ptr; - int32_t *dest = (int32_t *) sink->w_ptr; - int32_t i; - - /* buffer sizes are always divisible by period frames */ - /* Samples are Q1.31 --> Q1.31 and volume is Q1.16 */ - for (i = 0; i < frames * 2; i += 2) { - dest[i] = q_multsr_sat_32x32( - src[i], cd->volume[0], Q_SHIFT_BITS_64(31, 16, 31)); - dest[i + 1] = q_multsr_sat_32x32( - src[i + 1], cd->volume[1], Q_SHIFT_BITS_64(31, 16, 31)); - } -} - -/* copy and scale volume from 16 bit source buffer to 16 bit dest buffer */ -static void vol_s16_to_s16_2ch(struct comp_dev *dev, struct comp_buffer *sink, - struct comp_buffer *source, uint32_t frames) -{ - struct comp_data *cd = comp_get_drvdata(dev); - int16_t *src = (int16_t *) source->r_ptr; - int16_t *dest = (int16_t *) sink->w_ptr; - int32_t i; - - /* buffer sizes are always divisible by period frames */ - /* Samples are Q1.15 --> Q1.15 and volume is Q1.16 */ - for (i = 0; i < frames * 2; i += 2) { - dest[i] = q_multsr_sat_16x16( - src[i], cd->volume[0], Q_SHIFT_BITS_32(15, 16, 15)); - dest[i + 1] = q_multsr_sat_16x16( - src[i + 1], cd->volume[1], Q_SHIFT_BITS_32(15, 16, 15)); - } -} - -/* copy and scale volume from 16 bit source buffer to 24 bit on 32 bit boundary dest buffer */ -static void vol_s16_to_s24_2ch(struct comp_dev *dev, struct comp_buffer *sink, - struct comp_buffer *source, uint32_t frames) -{ - struct comp_data *cd = comp_get_drvdata(dev); - int16_t *src = (int16_t *) source->r_ptr; - int32_t *dest = (int32_t *) sink->w_ptr; - int32_t i; - - /* buffer sizes are always divisible by period frames */ - /* Samples are Q1.15 and volume is Q1.16 */ - for (i = 0; i < frames * 2; i += 2) { - dest[i] = q_multsr_sat_32x32( - src[i], cd->volume[0], Q_SHIFT_BITS_64(15, 16, 23)); - dest[i + 1] = q_multsr_sat_32x32( - src[i + 1], cd->volume[1], Q_SHIFT_BITS_64(15, 16, 23)); - } -} - -/* copy and scale volume from 16 bit source buffer to 24 bit on 32 bit boundary dest buffer */ -static void vol_s24_to_s16_2ch(struct comp_dev *dev, struct comp_buffer *sink, - struct comp_buffer *source, uint32_t frames) -{ - struct comp_data *cd = comp_get_drvdata(dev); - int32_t *src = (int32_t *) source->r_ptr; - int16_t *dest = (int16_t *) sink->w_ptr; - int32_t i; - - /* buffer sizes are always divisible by period frames */ - /* Samples are Q1.23 --> Q1.15 and volume is Q1.16 */ - for (i = 0; i < frames * 2; i += 2) { - dest[i] = (int16_t)q_multsr_sat_32x32( - sign_extend_s24(src[i]), cd->volume[0], - Q_SHIFT_BITS_64(23, 16, 15)); - dest[i + 1] = (int16_t)q_multsr_sat_32x32( - sign_extend_s24(src[i + 1]), cd->volume[1], - Q_SHIFT_BITS_64(23, 16, 15)); - } -} - -/* copy and scale volume from 32 bit source buffer to 24 bit on 32 bit boundary dest buffer */ -static void vol_s32_to_s24_2ch(struct comp_dev *dev, struct comp_buffer *sink, - struct comp_buffer *source, uint32_t frames) -{ - struct comp_data *cd = comp_get_drvdata(dev); - int32_t *src = (int32_t *) source->r_ptr; - int32_t *dest = (int32_t *) sink->w_ptr; - int32_t i; - - /* buffer sizes are always divisible by period frames */ - /* Samples are Q1.31 --> Q1.23 and volume is Q1.16 */ - for (i = 0; i < frames * 2; i += 2) { - dest[i] = q_multsr_sat_32x32( - src[i], cd->volume[0], Q_SHIFT_BITS_64(31, 16, 23)); - dest[i + 1] = q_multsr_sat_32x32( - src[i + 1], cd->volume[1], Q_SHIFT_BITS_64(31, 16, 23)); - } -} - -/* copy and scale volume from 16 bit source buffer to 24 bit on 32 bit boundary dest buffer */ -static void vol_s24_to_s32_2ch(struct comp_dev *dev, struct comp_buffer *sink, - struct comp_buffer *source, uint32_t frames) -{ - struct comp_data *cd = comp_get_drvdata(dev); - int32_t *src = (int32_t *) source->r_ptr; - int32_t *dest = (int32_t *) sink->w_ptr; - int32_t i; - - /* buffer sizes are always divisible by period frames */ - /* Samples are Q1.23 --> Q1.31 and volume is Q1.16 */ - for (i = 0; i < frames * 2; i += 2) { - dest[i] = q_multsr_sat_32x32( - sign_extend_s24(src[i]), cd->volume[0], - Q_SHIFT_BITS_64(23, 16, 31)); - dest[i + 1] = q_multsr_sat_32x32( - sign_extend_s24(src[i + 1]), cd->volume[1], - Q_SHIFT_BITS_64(23, 16, 31)); - } -} - -/* Copy and scale volume from 24 bit source buffer to 24 bit on 32 bit boundary - * dest buffer. - */ -static void vol_s24_to_s24_2ch(struct comp_dev *dev, struct comp_buffer *sink, - struct comp_buffer *source, uint32_t frames) -{ - struct comp_data *cd = comp_get_drvdata(dev); - int32_t i, *src = (int32_t*) source->r_ptr; - int32_t *dest = (int32_t*) sink->w_ptr; - - /* buffer sizes are always divisible by period frames */ - /* Samples are Q1.23 --> Q1.23 and volume is Q1.16 */ - for (i = 0; i < frames * 2; i += 2) { - dest[i] = q_multsr_sat_32x32( - sign_extend_s24(src[i]), cd->volume[0], - Q_SHIFT_BITS_64(23, 16, 23)); - dest[i + 1] = q_multsr_sat_32x32( - sign_extend_s24(src[i + 1]), cd->volume[1], - Q_SHIFT_BITS_64(23, 16, 23)); - } -} - -/* volume scaling functions for 4-channel input */ - -/* copy and scale volume from 16 bit source buffer to 32 bit dest buffer */ -static void vol_s16_to_s32_4ch(struct comp_dev *dev, struct comp_buffer *sink, - struct comp_buffer *source, uint32_t frames) -{ - struct comp_data *cd = comp_get_drvdata(dev); - int16_t *src = (int16_t *)source->r_ptr; - int32_t *dest = (int32_t *)sink->w_ptr; - int32_t i; - - /* buffer sizes are always divisible by period frames */ - /* Samples are Q1.15 --> Q1.31 and volume is Q1.16 */ - for (i = 0; i < frames * 4; i += 4) { - dest[i] = (int32_t)src[i] * cd->volume[0]; - dest[i + 1] = (int32_t)src[i + 1] * cd->volume[1]; - dest[i + 2] = (int32_t)src[i + 2] * cd->volume[2]; - dest[i + 3] = (int32_t)src[i + 3] * cd->volume[3]; - } -} - -/* copy and scale volume from 32 bit source buffer to 16 bit dest buffer */ -static void vol_s32_to_s16_4ch(struct comp_dev *dev, struct comp_buffer *sink, - struct comp_buffer *source, uint32_t frames) -{ - struct comp_data *cd = comp_get_drvdata(dev); - int32_t *src = (int32_t *)source->r_ptr; - int16_t *dest = (int16_t *)sink->w_ptr; - int32_t i; - - /* buffer sizes are always divisible by period frames */ - /* Samples are Q1.31 --> Q1.15 and volume is Q1.16 */ - for (i = 0; i < frames * 4; i += 4) { - dest[i] = (int16_t)q_multsr_sat_32x32(src[i], cd->volume[0], - Q_SHIFT_BITS_64(31, 16, - 15)); - dest[i + 1] = (int16_t)q_multsr_sat_32x32(src[i + 1], - cd->volume[1], - Q_SHIFT_BITS_64(31, - 16, - 15)); - dest[i + 2] = (int16_t)q_multsr_sat_32x32(src[i + 2], - cd->volume[2], - Q_SHIFT_BITS_64(31, - 16, - 15)); - dest[i + 3] = (int16_t)q_multsr_sat_32x32(src[i + 3], - cd->volume[3], - Q_SHIFT_BITS_64(31, - 16, - 15)); - } -} - -/* copy and scale volume from 32 bit source buffer to 32 bit dest buffer */ -static void vol_s32_to_s32_4ch(struct comp_dev *dev, struct comp_buffer *sink, - struct comp_buffer *source, uint32_t frames) -{ - struct comp_data *cd = comp_get_drvdata(dev); - int32_t *src = (int32_t *)source->r_ptr; - int32_t *dest = (int32_t *)sink->w_ptr; - int32_t i; - - /* buffer sizes are always divisible by period frames */ - /* Samples are Q1.31 --> Q1.31 and volume is Q1.16 */ - for (i = 0; i < frames * 4; i += 4) { - dest[i] = q_multsr_sat_32x32(src[i], cd->volume[0], - Q_SHIFT_BITS_64(31, 16, 31)); - dest[i + 1] = q_multsr_sat_32x32(src[i + 1], cd->volume[1], - Q_SHIFT_BITS_64(31, 16, 31)); - dest[i + 2] = q_multsr_sat_32x32(src[i + 2], cd->volume[2], - Q_SHIFT_BITS_64(31, 16, 31)); - dest[i + 3] = q_multsr_sat_32x32(src[i + 3], cd->volume[3], - Q_SHIFT_BITS_64(31, 16, 31)); - } -} - -/* copy and scale volume from 16 bit source buffer to 16 bit dest buffer */ -static void vol_s16_to_s16_4ch(struct comp_dev *dev, struct comp_buffer *sink, - struct comp_buffer *source, uint32_t frames) -{ - struct comp_data *cd = comp_get_drvdata(dev); - int16_t *src = (int16_t *)source->r_ptr; - int16_t *dest = (int16_t *)sink->w_ptr; - int32_t i; - - /* buffer sizes are always divisible by period frames */ - /* Samples are Q1.15 --> Q1.15 and volume is Q1.16 */ - for (i = 0; i < frames * 4; i += 4) { - dest[i] = q_multsr_sat_16x16(src[i], cd->volume[0], - Q_SHIFT_BITS_32(15, 16, 15)); - dest[i + 1] = q_multsr_sat_16x16(src[i + 1], cd->volume[1], - Q_SHIFT_BITS_32(15, 16, 15)); - dest[i + 2] = q_multsr_sat_16x16(src[i + 2], cd->volume[2], - Q_SHIFT_BITS_32(15, 16, 15)); - dest[i + 3] = q_multsr_sat_16x16(src[i + 3], cd->volume[3], - Q_SHIFT_BITS_32(15, 16, 15)); - } -} - -/* copy and scale volume from 16 bit source buffer to 24 bit - * on 32 bit boundary buffer - */ -static void vol_s16_to_s24_4ch(struct comp_dev *dev, struct comp_buffer *sink, - struct comp_buffer *source, uint32_t frames) -{ - struct comp_data *cd = comp_get_drvdata(dev); - int16_t *src = (int16_t *)source->r_ptr; - int32_t *dest = (int32_t *)sink->w_ptr; - int32_t i; - - /* buffer sizes are always divisible by period frames */ - /* Samples are Q1.15 and volume is Q1.16 */ - for (i = 0; i < frames * 4; i += 4) { - dest[i] = q_multsr_sat_32x32(src[i], cd->volume[0], - Q_SHIFT_BITS_64(15, 16, 23)); - dest[i + 1] = q_multsr_sat_32x32(src[i + 1], cd->volume[1], - Q_SHIFT_BITS_64(15, 16, 23)); - dest[i + 2] = q_multsr_sat_32x32(src[i + 2], cd->volume[2], - Q_SHIFT_BITS_64(15, 16, 23)); - dest[i + 3] = q_multsr_sat_32x32(src[i + 3], cd->volume[3], - Q_SHIFT_BITS_64(15, 16, 23)); - } -} - -/* copy and scale volume from 16 bit source buffer to 24 bit - * on 32 bit boundary dest buffer - */ -static void vol_s24_to_s16_4ch(struct comp_dev *dev, struct comp_buffer *sink, - struct comp_buffer *source, uint32_t frames) -{ - struct comp_data *cd = comp_get_drvdata(dev); - int32_t *src = (int32_t *)source->r_ptr; - int16_t *dest = (int16_t *)sink->w_ptr; - int32_t i, sample; - - /* buffer sizes are always divisible by period frames */ - /* Samples are Q1.23 --> Q1.15 and volume is Q1.16 */ - for (i = 0; i < frames * 4; i += 4) { - sample = sign_extend_s24(src[i]); - dest[i] = (int16_t)q_multsr_sat_32x32(sample, cd->volume[0], - Q_SHIFT_BITS_64(23, 16, - 15)); - sample = sign_extend_s24(src[i + 1]); - dest[i + 1] = (int16_t)q_multsr_sat_32x32(sample, - cd->volume[1], - Q_SHIFT_BITS_64(23, - 16, - 15)); - sample = sign_extend_s24(src[i + 2]); - dest[i + 2] = (int16_t)q_multsr_sat_32x32(sample, - cd->volume[2], - Q_SHIFT_BITS_64(23, - 16, - 15)); - sample = sign_extend_s24(src[i + 3]); - dest[i + 3] = (int16_t)q_multsr_sat_32x32(sample, - cd->volume[3], - Q_SHIFT_BITS_64(23, - 16, - 15)); - } -} - -/* copy and scale volume from 32 bit source buffer to 24 bit - * on 32 bit boundary dest buffer - */ -static void vol_s32_to_s24_4ch(struct comp_dev *dev, struct comp_buffer *sink, - struct comp_buffer *source, uint32_t frames) -{ - struct comp_data *cd = comp_get_drvdata(dev); - int32_t *src = (int32_t *)source->r_ptr; - int32_t *dest = (int32_t *)sink->w_ptr; - int32_t i; - - /* buffer sizes are always divisible by period frames */ - /* Samples are Q1.31 --> Q1.23 and volume is Q1.16 */ - for (i = 0; i < frames * 4; i += 4) { - dest[i] = q_multsr_sat_32x32(src[i], cd->volume[0], - Q_SHIFT_BITS_64(31, 16, 23)); - dest[i + 1] = q_multsr_sat_32x32(src[i + 1], cd->volume[1], - Q_SHIFT_BITS_64(31, 16, 23)); - dest[i + 2] = q_multsr_sat_32x32(src[i + 2], cd->volume[2], - Q_SHIFT_BITS_64(31, 16, 23)); - dest[i + 3] = q_multsr_sat_32x32(src[i + 3], cd->volume[3], - Q_SHIFT_BITS_64(31, 16, 23)); - } -} - -/* copy and scale volume from 16 bit source buffer to 24 bit - * on 32 bit boundary dest buffer - */ -static void vol_s24_to_s32_4ch(struct comp_dev *dev, struct comp_buffer *sink, - struct comp_buffer *source, uint32_t frames) -{ - struct comp_data *cd = comp_get_drvdata(dev); - int32_t *src = (int32_t *)source->r_ptr; - int32_t *dest = (int32_t *)sink->w_ptr; - int32_t i; - - /* buffer sizes are always divisible by period frames */ - /* Samples are Q1.23 --> Q1.31 and volume is Q1.16 */ - for (i = 0; i < frames * 4; i += 4) { - dest[i] = q_multsr_sat_32x32(sign_extend_s24(src[i]), - cd->volume[0], - Q_SHIFT_BITS_64(23, 16, 31)); - dest[i + 1] = q_multsr_sat_32x32(sign_extend_s24(src[i + 1]), - cd->volume[1], - Q_SHIFT_BITS_64(23, 16, 31)); - dest[i + 2] = q_multsr_sat_32x32(sign_extend_s24(src[i + 2]), - cd->volume[2], - Q_SHIFT_BITS_64(23, 16, 31)); - dest[i + 3] = q_multsr_sat_32x32(sign_extend_s24(src[i + 3]), - cd->volume[3], - Q_SHIFT_BITS_64(23, 16, 31)); - } -} - -/* Copy and scale volume from 24 bit source buffer to 24 bit on 32 bit boundary - * dest buffer. - */ -static void vol_s24_to_s24_4ch(struct comp_dev *dev, struct comp_buffer *sink, - struct comp_buffer *source, uint32_t frames) -{ - struct comp_data *cd = comp_get_drvdata(dev); - int32_t i, *src = (int32_t *)source->r_ptr; - int32_t *dest = (int32_t *)sink->w_ptr; - - /* buffer sizes are always divisible by period frames */ - /* Samples are Q1.23 --> Q1.23 and volume is Q1.16 */ - for (i = 0; i < frames * 4; i += 4) { - dest[i] = q_multsr_sat_32x32(sign_extend_s24(src[i]), - cd->volume[0], - Q_SHIFT_BITS_64(23, 16, 23)); - dest[i + 1] = q_multsr_sat_32x32(sign_extend_s24(src[i + 1]), - cd->volume[1], - Q_SHIFT_BITS_64(23, 16, 23)); - dest[i + 2] = q_multsr_sat_32x32(sign_extend_s24(src[i + 2]), - cd->volume[2], - Q_SHIFT_BITS_64(23, 16, 23)); - dest[i + 3] = q_multsr_sat_32x32(sign_extend_s24(src[i + 3]), - cd->volume[3], - Q_SHIFT_BITS_64(23, 16, 23)); - } -} - -/* map of source and sink buffer formats to volume function */ -static const struct comp_func_map func_map[] = { - {SOF_IPC_FRAME_S16_LE, SOF_IPC_FRAME_S16_LE, 2, vol_s16_to_s16_2ch}, - {SOF_IPC_FRAME_S16_LE, SOF_IPC_FRAME_S32_LE, 2, vol_s16_to_s32_2ch}, - {SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S16_LE, 2, vol_s32_to_s16_2ch}, - {SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S32_LE, 2, vol_s32_to_s32_2ch}, - {SOF_IPC_FRAME_S16_LE, SOF_IPC_FRAME_S24_4LE, 2, vol_s16_to_s24_2ch}, - {SOF_IPC_FRAME_S24_4LE, SOF_IPC_FRAME_S16_LE, 2, vol_s24_to_s16_2ch}, - {SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S24_4LE, 2, vol_s32_to_s24_2ch}, - {SOF_IPC_FRAME_S24_4LE, SOF_IPC_FRAME_S32_LE, 2, vol_s24_to_s32_2ch}, - {SOF_IPC_FRAME_S24_4LE, SOF_IPC_FRAME_S24_4LE, 2, vol_s24_to_s24_2ch}, - {SOF_IPC_FRAME_S16_LE, SOF_IPC_FRAME_S16_LE, 4, vol_s16_to_s16_4ch}, - {SOF_IPC_FRAME_S16_LE, SOF_IPC_FRAME_S32_LE, 4, vol_s16_to_s32_4ch}, - {SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S16_LE, 4, vol_s32_to_s16_4ch}, - {SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S32_LE, 4, vol_s32_to_s32_4ch}, - {SOF_IPC_FRAME_S16_LE, SOF_IPC_FRAME_S24_4LE, 4, vol_s16_to_s24_4ch}, - {SOF_IPC_FRAME_S24_4LE, SOF_IPC_FRAME_S16_LE, 4, vol_s24_to_s16_4ch}, - {SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S24_4LE, 4, vol_s32_to_s24_4ch}, - {SOF_IPC_FRAME_S24_4LE, SOF_IPC_FRAME_S32_LE, 4, vol_s24_to_s32_4ch}, - {SOF_IPC_FRAME_S24_4LE, SOF_IPC_FRAME_S24_4LE, 4, vol_s24_to_s24_4ch}, -}; +#include "volume.h"
/* synchronise host mmap() volume with real value */ static void vol_sync_host(struct comp_data *cd, uint32_t chan) @@ -570,9 +74,8 @@ static uint64_t vol_work(void *data, uint64_t delay) for (i = 0; i < PLATFORM_MAX_CHANNELS; i++) {
/* skip if target reached */ - if (cd->volume[i] == cd->tvolume[i]) { + if (cd->volume[i] == cd->tvolume[i]) continue; - }
vol = cd->volume[i];
@@ -620,7 +123,8 @@ static struct comp_dev *volume_new(struct sof_ipc_comp *comp) { struct comp_dev *dev; struct sof_ipc_comp_volume *vol; - struct sof_ipc_comp_volume *ipc_vol = (struct sof_ipc_comp_volume *)comp; + struct sof_ipc_comp_volume *ipc_vol = + (struct sof_ipc_comp_volume *)comp; struct comp_data *cd; int i;
@@ -664,8 +168,8 @@ static void volume_free(struct comp_dev *dev) }
/* - * Set volume component audio stream paramters - All done in prepare() since - * wee need to know source and sink component params. + * Set volume component audio stream parameters - All done in prepare() since + * we need to know source and sink component params. */ static int volume_params(struct comp_dev *dev) { @@ -717,7 +221,8 @@ static inline void volume_set_chan_unmute(struct comp_dev *dev, int chan) cd->tvolume[chan] = cd->mvolume[chan]; }
-static int volume_ctrl_set_cmd(struct comp_dev *dev, struct sof_ipc_ctrl_data *cdata) +static int volume_ctrl_set_cmd(struct comp_dev *dev, + struct sof_ipc_ctrl_data *cdata) { struct comp_data *cd = comp_get_drvdata(dev); int i; @@ -775,7 +280,8 @@ static int volume_ctrl_set_cmd(struct comp_dev *dev, struct sof_ipc_ctrl_data *c return 0; }
-static int volume_ctrl_get_cmd(struct comp_dev *dev, struct sof_ipc_ctrl_data *cdata) +static int volume_ctrl_get_cmd(struct comp_dev *dev, + struct sof_ipc_ctrl_data *cdata) { struct comp_data *cd = comp_get_drvdata(dev); int j; @@ -839,12 +345,15 @@ static int volume_copy(struct comp_dev *dev) tracev_volume("cpy");
/* volume components will only ever have 1 source and 1 sink buffer */ - source = list_first_item(&dev->bsource_list, struct comp_buffer, sink_list); - sink = list_first_item(&dev->bsink_list, struct comp_buffer, source_list); + source = list_first_item(&dev->bsource_list, + struct comp_buffer, sink_list); + sink = list_first_item(&dev->bsink_list, + struct comp_buffer, source_list);
/* make sure source component buffer has enough data available and that * the sink component buffer has enough free bytes for copy. Also - * check for XRUNs */ + * check for XRUNs + */ if (source->avail < cd->source_period_bytes) { trace_volume_error("xru"); comp_underrun(dev, source, cd->source_period_bytes, 0); @@ -857,7 +366,7 @@ static int volume_copy(struct comp_dev *dev) }
/* copy and scale volume */ - cd->scale_vol(dev, sink, source, dev->frames); + cd->scale_vol(dev, sink, source);
/* calc new free and available */ comp_update_buffer_produce(sink, cd->sink_period_bytes); @@ -868,7 +377,7 @@ static int volume_copy(struct comp_dev *dev)
/* * Volume component is usually first and last in pipelines so it makes sense - * to also do some type conversion here too. + * to also do some type conversion too. */ static int volume_prepare(struct comp_dev *dev) { @@ -887,8 +396,10 @@ static int volume_prepare(struct comp_dev *dev) return ret;
/* volume components will only ever have 1 source and 1 sink buffer */ - sourceb = list_first_item(&dev->bsource_list, struct comp_buffer, sink_list); - sinkb = list_first_item(&dev->bsink_list, struct comp_buffer, source_list); + sourceb = list_first_item(&dev->bsource_list, + struct comp_buffer, sink_list); + sinkb = list_first_item(&dev->bsink_list, + struct comp_buffer, source_list);
/* get source data format */ switch (sourceb->source->comp.type) { @@ -958,33 +469,24 @@ static int volume_prepare(struct comp_dev *dev) goto err; }
- /* map the volume function for source and sink buffers */ - for (i = 0; i < ARRAY_SIZE(func_map); i++) { - - if (cd->source_format != func_map[i].source) - continue; - if (cd->sink_format != func_map[i].sink) - continue; - if (dev->params.channels != func_map[i].channels) - continue; - - cd->scale_vol = func_map[i].func; - goto found; + cd->scale_vol = vol_get_processing_function(dev); + if (!cd->scale_vol) { + trace_volume_error("vp3"); + trace_error_value(cd->source_format); + trace_error_value(cd->sink_format); + trace_error_value(dev->params.channels); + ret = -EINVAL; + goto err; } - trace_volume_error("vp3"); - trace_error_value(cd->source_format); - trace_error_value(cd->sink_format); - trace_error_value(dev->params.channels);
-err: - comp_set_state(dev, COMP_TRIGGER_RESET); - return ret; - -found: for (i = 0; i < PLATFORM_MAX_CHANNELS; i++) vol_sync_host(cd, i);
return 0; + +err: + comp_set_state(dev, COMP_TRIGGER_RESET); + return ret; }
static int volume_reset(struct comp_dev *dev) diff --git a/src/audio/volume.h b/src/audio/volume.h new file mode 100644 index 0000000..f7c9b43 --- /dev/null +++ b/src/audio/volume.h @@ -0,0 +1,95 @@ +/* + * Copyright (c) 2018, 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: Tomasz Lauda tomasz.lauda@linux.intel.com + */ + +#ifndef VOLUME_H +#define VOLUME_H + +#include <stdint.h> +#include <sof/audio/component.h> +#include <sof/audio/pipeline.h> +#include <sof/audio/format.h> + +#define CONFIG_GENERIC + +#if defined(__XCC__) +#include <xtensa/config/core-isa.h> + +#if XCHAL_HAVE_HIFI3 +#undef CONFIG_GENERIC +#endif + +#endif + +#define trace_volume(__e) trace_event(TRACE_CLASS_VOLUME, __e) +#define tracev_volume(__e) tracev_event(TRACE_CLASS_VOLUME, __e) +#define trace_volume_error(__e) trace_error(TRACE_CLASS_VOLUME, __e) + +/* this should ramp from 0dB to mute in 64ms. + * i.e 2^16 -> 0 in 32 * 2048 steps each lasting 2ms + */ +#define VOL_RAMP_US 2000 +#define VOL_RAMP_STEP (1 << 11) +#define VOL_MAX (1 << 16) +#define VOL_MIN 0 + +/* volume component private data */ +struct comp_data { + uint32_t source_period_bytes; + uint32_t sink_period_bytes; + enum sof_ipc_frame source_format; + enum sof_ipc_frame sink_format; + uint32_t volume[SOF_IPC_MAX_CHANNELS]; /* current volume */ + uint32_t tvolume[SOF_IPC_MAX_CHANNELS]; /* target volume */ + uint32_t mvolume[SOF_IPC_MAX_CHANNELS]; /* mute volume */ + void (*scale_vol)(struct comp_dev *dev, struct comp_buffer *sink, + struct comp_buffer *source); + struct work volwork; + + /* host volume readback */ + struct sof_ipc_ctrl_value_chan *hvol; +}; + +struct comp_func_map { + uint16_t source; /* source format */ + uint16_t sink; /* sink format */ + uint16_t channels; /* channel number for the stream */ + void (*func)(struct comp_dev *dev, struct comp_buffer *sink, + struct comp_buffer *source); +}; + +/* map of source and sink buffer formats to volume function */ +extern const struct comp_func_map func_map[]; + +typedef void (*scale_vol)(struct comp_dev *, struct comp_buffer *, + struct comp_buffer *); + +scale_vol vol_get_processing_function(struct comp_dev *dev); + +#endif /* VOLUME_H */ diff --git a/src/audio/volume_generic.c b/src/audio/volume_generic.c new file mode 100644 index 0000000..200cf12 --- /dev/null +++ b/src/audio/volume_generic.c @@ -0,0 +1,510 @@ +/* + * Copyright (c) 2018, 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: Liam Girdwood liam.r.girdwood@linux.intel.com + * Keyon Jie yang.jie@linux.intel.com + * Tomasz Lauda tomasz.lauda@linux.intel.com + */ + +#include "volume.h" + +#ifdef CONFIG_GENERIC + +/* volume scaling functions for stereo input */ + +/* copy and scale volume from 16 bit source buffer to 32 bit dest buffer */ +static void vol_s16_to_s32_2ch(struct comp_dev *dev, struct comp_buffer *sink, + struct comp_buffer *source) +{ + struct comp_data *cd = comp_get_drvdata(dev); + int16_t *src = (int16_t *)source->r_ptr; + int32_t *dest = (int32_t *)sink->w_ptr; + int32_t i; + + /* buffer sizes are always divisible by period frames */ + /* Samples are Q1.15 --> Q1.31 and volume is Q1.16 */ + for (i = 0; i < dev->frames * 2; i += 2) { + dest[i] = (int32_t)src[i] * cd->volume[0]; + dest[i + 1] = (int32_t)src[i + 1] * cd->volume[1]; + } +} + +/* copy and scale volume from 32 bit source buffer to 16 bit dest buffer */ +static void vol_s32_to_s16_2ch(struct comp_dev *dev, struct comp_buffer *sink, + struct comp_buffer *source) +{ + struct comp_data *cd = comp_get_drvdata(dev); + int32_t *src = (int32_t *)source->r_ptr; + int16_t *dest = (int16_t *)sink->w_ptr; + int32_t i; + + /* buffer sizes are always divisible by period frames */ + /* Samples are Q1.31 --> Q1.15 and volume is Q1.16 */ + for (i = 0; i < dev->frames * 2; i += 2) { + dest[i] = (int16_t)q_multsr_sat_32x32( + src[i], cd->volume[0], Q_SHIFT_BITS_64(31, 16, 15)); + dest[i + 1] = (int16_t)q_multsr_sat_32x32( + src[i + 1], cd->volume[1], Q_SHIFT_BITS_64(31, 16, 15)); + } +} + +/* copy and scale volume from 32 bit source buffer to 32 bit dest buffer */ +static void vol_s32_to_s32_2ch(struct comp_dev *dev, struct comp_buffer *sink, + struct comp_buffer *source) +{ + struct comp_data *cd = comp_get_drvdata(dev); + int32_t *src = (int32_t *)source->r_ptr; + int32_t *dest = (int32_t *)sink->w_ptr; + int32_t i; + + /* buffer sizes are always divisible by period frames */ + /* Samples are Q1.31 --> Q1.31 and volume is Q1.16 */ + for (i = 0; i < dev->frames * 2; i += 2) { + dest[i] = q_multsr_sat_32x32( + src[i], cd->volume[0], Q_SHIFT_BITS_64(31, 16, 31)); + dest[i + 1] = q_multsr_sat_32x32( + src[i + 1], cd->volume[1], Q_SHIFT_BITS_64(31, 16, 31)); + } +} + +/* copy and scale volume from 16 bit source buffer to 16 bit dest buffer */ +static void vol_s16_to_s16_2ch(struct comp_dev *dev, struct comp_buffer *sink, + struct comp_buffer *source) +{ + struct comp_data *cd = comp_get_drvdata(dev); + int16_t *src = (int16_t *)source->r_ptr; + int16_t *dest = (int16_t *)sink->w_ptr; + int32_t i; + + /* buffer sizes are always divisible by period frames */ + /* Samples are Q1.15 --> Q1.15 and volume is Q1.16 */ + for (i = 0; i < dev->frames * 2; i += 2) { + dest[i] = q_multsr_sat_16x16( + src[i], cd->volume[0], Q_SHIFT_BITS_32(15, 16, 15)); + dest[i + 1] = q_multsr_sat_16x16( + src[i + 1], cd->volume[1], Q_SHIFT_BITS_32(15, 16, 15)); + } +} + +/* copy and scale volume from 16 bit source buffer to 24 bit + * on 32 bit boundary dest buffer + */ +static void vol_s16_to_s24_2ch(struct comp_dev *dev, struct comp_buffer *sink, + struct comp_buffer *source) +{ + struct comp_data *cd = comp_get_drvdata(dev); + int16_t *src = (int16_t *)source->r_ptr; + int32_t *dest = (int32_t *)sink->w_ptr; + int32_t i; + + /* buffer sizes are always divisible by period frames */ + /* Samples are Q1.15 and volume is Q1.16 */ + for (i = 0; i < dev->frames * 2; i += 2) { + dest[i] = q_multsr_sat_32x32( + src[i], cd->volume[0], Q_SHIFT_BITS_64(15, 16, 23)); + dest[i + 1] = q_multsr_sat_32x32( + src[i + 1], cd->volume[1], Q_SHIFT_BITS_64(15, 16, 23)); + } +} + +/* copy and scale volume from 16 bit source buffer to 24 bit + * on 32 bit boundary dest buffer + */ +static void vol_s24_to_s16_2ch(struct comp_dev *dev, struct comp_buffer *sink, + struct comp_buffer *source) +{ + struct comp_data *cd = comp_get_drvdata(dev); + int32_t *src = (int32_t *)source->r_ptr; + int16_t *dest = (int16_t *)sink->w_ptr; + int32_t i; + + /* buffer sizes are always divisible by period frames */ + /* Samples are Q1.23 --> Q1.15 and volume is Q1.16 */ + for (i = 0; i < dev->frames * 2; i += 2) { + dest[i] = (int16_t)q_multsr_sat_32x32( + sign_extend_s24(src[i]), cd->volume[0], + Q_SHIFT_BITS_64(23, 16, 15)); + dest[i + 1] = (int16_t)q_multsr_sat_32x32( + sign_extend_s24(src[i + 1]), cd->volume[1], + Q_SHIFT_BITS_64(23, 16, 15)); + } +} + +/* copy and scale volume from 32 bit source buffer to 24 bit + * on 32 bit boundary dest buffer + */ +static void vol_s32_to_s24_2ch(struct comp_dev *dev, struct comp_buffer *sink, + struct comp_buffer *source) +{ + struct comp_data *cd = comp_get_drvdata(dev); + int32_t *src = (int32_t *)source->r_ptr; + int32_t *dest = (int32_t *)sink->w_ptr; + int32_t i; + + /* buffer sizes are always divisible by period frames */ + /* Samples are Q1.31 --> Q1.23 and volume is Q1.16 */ + for (i = 0; i < dev->frames * 2; i += 2) { + dest[i] = q_multsr_sat_32x32( + src[i], cd->volume[0], Q_SHIFT_BITS_64(31, 16, 23)); + dest[i + 1] = q_multsr_sat_32x32( + src[i + 1], cd->volume[1], Q_SHIFT_BITS_64(31, 16, 23)); + } +} + +/* copy and scale volume from 16 bit source buffer to 24 bit + * on 32 bit boundary dest buffer + */ +static void vol_s24_to_s32_2ch(struct comp_dev *dev, struct comp_buffer *sink, + struct comp_buffer *source) +{ + struct comp_data *cd = comp_get_drvdata(dev); + int32_t *src = (int32_t *)source->r_ptr; + int32_t *dest = (int32_t *)sink->w_ptr; + int32_t i; + + /* buffer sizes are always divisible by period frames */ + /* Samples are Q1.23 --> Q1.31 and volume is Q1.16 */ + for (i = 0; i < dev->frames * 2; i += 2) { + dest[i] = q_multsr_sat_32x32( + sign_extend_s24(src[i]), cd->volume[0], + Q_SHIFT_BITS_64(23, 16, 31)); + dest[i + 1] = q_multsr_sat_32x32( + sign_extend_s24(src[i + 1]), cd->volume[1], + Q_SHIFT_BITS_64(23, 16, 31)); + } +} + +/* copy and scale volume from 24 bit source buffer to 24 bit on 32 bit boundary + * dest buffer. + */ +static void vol_s24_to_s24_2ch(struct comp_dev *dev, struct comp_buffer *sink, + struct comp_buffer *source) +{ + struct comp_data *cd = comp_get_drvdata(dev); + int32_t i, *src = (int32_t *)source->r_ptr; + int32_t *dest = (int32_t *)sink->w_ptr; + + /* buffer sizes are always divisible by period frames */ + /* Samples are Q1.23 --> Q1.23 and volume is Q1.16 */ + for (i = 0; i < dev->frames * 2; i += 2) { + dest[i] = q_multsr_sat_32x32( + sign_extend_s24(src[i]), cd->volume[0], + Q_SHIFT_BITS_64(23, 16, 23)); + dest[i + 1] = q_multsr_sat_32x32( + sign_extend_s24(src[i + 1]), cd->volume[1], + Q_SHIFT_BITS_64(23, 16, 23)); + } +} + +/* volume scaling functions for 4-channel input */ + +/* copy and scale volume from 16 bit source buffer to 32 bit dest buffer */ +static void vol_s16_to_s32_4ch(struct comp_dev *dev, struct comp_buffer *sink, + struct comp_buffer *source) +{ + struct comp_data *cd = comp_get_drvdata(dev); + int16_t *src = (int16_t *)source->r_ptr; + int32_t *dest = (int32_t *)sink->w_ptr; + int32_t i; + + /* buffer sizes are always divisible by period frames */ + /* Samples are Q1.15 --> Q1.31 and volume is Q1.16 */ + for (i = 0; i < dev->frames * 4; i += 4) { + dest[i] = (int32_t)src[i] * cd->volume[0]; + dest[i + 1] = (int32_t)src[i + 1] * cd->volume[1]; + dest[i + 2] = (int32_t)src[i + 2] * cd->volume[2]; + dest[i + 3] = (int32_t)src[i + 3] * cd->volume[3]; + } +} + +/* copy and scale volume from 32 bit source buffer to 16 bit dest buffer */ +static void vol_s32_to_s16_4ch(struct comp_dev *dev, struct comp_buffer *sink, + struct comp_buffer *source) +{ + struct comp_data *cd = comp_get_drvdata(dev); + int32_t *src = (int32_t *)source->r_ptr; + int16_t *dest = (int16_t *)sink->w_ptr; + int32_t i; + + /* buffer sizes are always divisible by period frames */ + /* Samples are Q1.31 --> Q1.15 and volume is Q1.16 */ + for (i = 0; i < dev->frames * 4; i += 4) { + dest[i] = (int16_t)q_multsr_sat_32x32(src[i], cd->volume[0], + Q_SHIFT_BITS_64(31, 16, + 15)); + dest[i + 1] = (int16_t)q_multsr_sat_32x32(src[i + 1], + cd->volume[1], + Q_SHIFT_BITS_64(31, + 16, + 15)); + dest[i + 2] = (int16_t)q_multsr_sat_32x32(src[i + 2], + cd->volume[2], + Q_SHIFT_BITS_64(31, + 16, + 15)); + dest[i + 3] = (int16_t)q_multsr_sat_32x32(src[i + 3], + cd->volume[3], + Q_SHIFT_BITS_64(31, + 16, + 15)); + } +} + +/* copy and scale volume from 32 bit source buffer to 32 bit dest buffer */ +static void vol_s32_to_s32_4ch(struct comp_dev *dev, struct comp_buffer *sink, + struct comp_buffer *source) +{ + struct comp_data *cd = comp_get_drvdata(dev); + int32_t *src = (int32_t *)source->r_ptr; + int32_t *dest = (int32_t *)sink->w_ptr; + int32_t i; + + /* buffer sizes are always divisible by period frames */ + /* Samples are Q1.31 --> Q1.31 and volume is Q1.16 */ + for (i = 0; i < dev->frames * 4; i += 4) { + dest[i] = q_multsr_sat_32x32(src[i], cd->volume[0], + Q_SHIFT_BITS_64(31, 16, 31)); + dest[i + 1] = q_multsr_sat_32x32(src[i + 1], cd->volume[1], + Q_SHIFT_BITS_64(31, 16, 31)); + dest[i + 2] = q_multsr_sat_32x32(src[i + 2], cd->volume[2], + Q_SHIFT_BITS_64(31, 16, 31)); + dest[i + 3] = q_multsr_sat_32x32(src[i + 3], cd->volume[3], + Q_SHIFT_BITS_64(31, 16, 31)); + } +} + +/* copy and scale volume from 16 bit source buffer to 16 bit dest buffer */ +static void vol_s16_to_s16_4ch(struct comp_dev *dev, struct comp_buffer *sink, + struct comp_buffer *source) +{ + struct comp_data *cd = comp_get_drvdata(dev); + int16_t *src = (int16_t *)source->r_ptr; + int16_t *dest = (int16_t *)sink->w_ptr; + int32_t i; + + /* buffer sizes are always divisible by period frames */ + /* Samples are Q1.15 --> Q1.15 and volume is Q1.16 */ + for (i = 0; i < dev->frames * 4; i += 4) { + dest[i] = q_multsr_sat_16x16(src[i], cd->volume[0], + Q_SHIFT_BITS_32(15, 16, 15)); + dest[i + 1] = q_multsr_sat_16x16(src[i + 1], cd->volume[1], + Q_SHIFT_BITS_32(15, 16, 15)); + dest[i + 2] = q_multsr_sat_16x16(src[i + 2], cd->volume[2], + Q_SHIFT_BITS_32(15, 16, 15)); + dest[i + 3] = q_multsr_sat_16x16(src[i + 3], cd->volume[3], + Q_SHIFT_BITS_32(15, 16, 15)); + } +} + +/* copy and scale volume from 16 bit source buffer to 24 bit + * on 32 bit boundary buffer + */ +static void vol_s16_to_s24_4ch(struct comp_dev *dev, struct comp_buffer *sink, + struct comp_buffer *source) +{ + struct comp_data *cd = comp_get_drvdata(dev); + int16_t *src = (int16_t *)source->r_ptr; + int32_t *dest = (int32_t *)sink->w_ptr; + int32_t i; + + /* buffer sizes are always divisible by period frames */ + /* Samples are Q1.15 and volume is Q1.16 */ + for (i = 0; i < dev->frames * 4; i += 4) { + dest[i] = q_multsr_sat_32x32(src[i], cd->volume[0], + Q_SHIFT_BITS_64(15, 16, 23)); + dest[i + 1] = q_multsr_sat_32x32(src[i + 1], cd->volume[1], + Q_SHIFT_BITS_64(15, 16, 23)); + dest[i + 2] = q_multsr_sat_32x32(src[i + 2], cd->volume[2], + Q_SHIFT_BITS_64(15, 16, 23)); + dest[i + 3] = q_multsr_sat_32x32(src[i + 3], cd->volume[3], + Q_SHIFT_BITS_64(15, 16, 23)); + } +} + +/* copy and scale volume from 16 bit source buffer to 24 bit + * on 32 bit boundary dest buffer + */ +static void vol_s24_to_s16_4ch(struct comp_dev *dev, struct comp_buffer *sink, + struct comp_buffer *source) +{ + struct comp_data *cd = comp_get_drvdata(dev); + int32_t *src = (int32_t *)source->r_ptr; + int16_t *dest = (int16_t *)sink->w_ptr; + int32_t i, sample; + + /* buffer sizes are always divisible by period frames */ + /* Samples are Q1.23 --> Q1.15 and volume is Q1.16 */ + for (i = 0; i < dev->frames * 4; i += 4) { + sample = sign_extend_s24(src[i]); + dest[i] = (int16_t)q_multsr_sat_32x32(sample, cd->volume[0], + Q_SHIFT_BITS_64(23, 16, + 15)); + sample = sign_extend_s24(src[i + 1]); + dest[i + 1] = (int16_t)q_multsr_sat_32x32(sample, + cd->volume[1], + Q_SHIFT_BITS_64(23, + 16, + 15)); + sample = sign_extend_s24(src[i + 2]); + dest[i + 2] = (int16_t)q_multsr_sat_32x32(sample, + cd->volume[2], + Q_SHIFT_BITS_64(23, + 16, + 15)); + sample = sign_extend_s24(src[i + 3]); + dest[i + 3] = (int16_t)q_multsr_sat_32x32(sample, + cd->volume[3], + Q_SHIFT_BITS_64(23, + 16, + 15)); + } +} + +/* copy and scale volume from 32 bit source buffer to 24 bit + * on 32 bit boundary dest buffer + */ +static void vol_s32_to_s24_4ch(struct comp_dev *dev, struct comp_buffer *sink, + struct comp_buffer *source) +{ + struct comp_data *cd = comp_get_drvdata(dev); + int32_t *src = (int32_t *)source->r_ptr; + int32_t *dest = (int32_t *)sink->w_ptr; + int32_t i; + + /* buffer sizes are always divisible by period frames */ + /* Samples are Q1.31 --> Q1.23 and volume is Q1.16 */ + for (i = 0; i < dev->frames * 4; i += 4) { + dest[i] = q_multsr_sat_32x32(src[i], cd->volume[0], + Q_SHIFT_BITS_64(31, 16, 23)); + dest[i + 1] = q_multsr_sat_32x32(src[i + 1], cd->volume[1], + Q_SHIFT_BITS_64(31, 16, 23)); + dest[i + 2] = q_multsr_sat_32x32(src[i + 2], cd->volume[2], + Q_SHIFT_BITS_64(31, 16, 23)); + dest[i + 3] = q_multsr_sat_32x32(src[i + 3], cd->volume[3], + Q_SHIFT_BITS_64(31, 16, 23)); + } +} + +/* copy and scale volume from 16 bit source buffer to 24 bit + * on 32 bit boundary dest buffer + */ +static void vol_s24_to_s32_4ch(struct comp_dev *dev, struct comp_buffer *sink, + struct comp_buffer *source) +{ + struct comp_data *cd = comp_get_drvdata(dev); + int32_t *src = (int32_t *)source->r_ptr; + int32_t *dest = (int32_t *)sink->w_ptr; + int32_t i; + + /* buffer sizes are always divisible by period frames */ + /* Samples are Q1.23 --> Q1.31 and volume is Q1.16 */ + for (i = 0; i < dev->frames * 4; i += 4) { + dest[i] = q_multsr_sat_32x32(sign_extend_s24(src[i]), + cd->volume[0], + Q_SHIFT_BITS_64(23, 16, 31)); + dest[i + 1] = q_multsr_sat_32x32(sign_extend_s24(src[i + 1]), + cd->volume[1], + Q_SHIFT_BITS_64(23, 16, 31)); + dest[i + 2] = q_multsr_sat_32x32(sign_extend_s24(src[i + 2]), + cd->volume[2], + Q_SHIFT_BITS_64(23, 16, 31)); + dest[i + 3] = q_multsr_sat_32x32(sign_extend_s24(src[i + 3]), + cd->volume[3], + Q_SHIFT_BITS_64(23, 16, 31)); + } +} + +/* Copy and scale volume from 24 bit source buffer to 24 bit on 32 bit boundary + * dest buffer. + */ +static void vol_s24_to_s24_4ch(struct comp_dev *dev, struct comp_buffer *sink, + struct comp_buffer *source) +{ + struct comp_data *cd = comp_get_drvdata(dev); + int32_t i, *src = (int32_t *)source->r_ptr; + int32_t *dest = (int32_t *)sink->w_ptr; + + /* buffer sizes are always divisible by period frames */ + /* Samples are Q1.23 --> Q1.23 and volume is Q1.16 */ + for (i = 0; i < dev->frames * 4; i += 4) { + dest[i] = q_multsr_sat_32x32(sign_extend_s24(src[i]), + cd->volume[0], + Q_SHIFT_BITS_64(23, 16, 23)); + dest[i + 1] = q_multsr_sat_32x32(sign_extend_s24(src[i + 1]), + cd->volume[1], + Q_SHIFT_BITS_64(23, 16, 23)); + dest[i + 2] = q_multsr_sat_32x32(sign_extend_s24(src[i + 2]), + cd->volume[2], + Q_SHIFT_BITS_64(23, 16, 23)); + dest[i + 3] = q_multsr_sat_32x32(sign_extend_s24(src[i + 3]), + cd->volume[3], + Q_SHIFT_BITS_64(23, 16, 23)); + } +} + +const struct comp_func_map func_map[] = { + {SOF_IPC_FRAME_S16_LE, SOF_IPC_FRAME_S16_LE, 2, vol_s16_to_s16_2ch}, + {SOF_IPC_FRAME_S16_LE, SOF_IPC_FRAME_S32_LE, 2, vol_s16_to_s32_2ch}, + {SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S16_LE, 2, vol_s32_to_s16_2ch}, + {SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S32_LE, 2, vol_s32_to_s32_2ch}, + {SOF_IPC_FRAME_S16_LE, SOF_IPC_FRAME_S24_4LE, 2, vol_s16_to_s24_2ch}, + {SOF_IPC_FRAME_S24_4LE, SOF_IPC_FRAME_S16_LE, 2, vol_s24_to_s16_2ch}, + {SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S24_4LE, 2, vol_s32_to_s24_2ch}, + {SOF_IPC_FRAME_S24_4LE, SOF_IPC_FRAME_S32_LE, 2, vol_s24_to_s32_2ch}, + {SOF_IPC_FRAME_S24_4LE, SOF_IPC_FRAME_S24_4LE, 2, vol_s24_to_s24_2ch}, + {SOF_IPC_FRAME_S16_LE, SOF_IPC_FRAME_S16_LE, 4, vol_s16_to_s16_4ch}, + {SOF_IPC_FRAME_S16_LE, SOF_IPC_FRAME_S32_LE, 4, vol_s16_to_s32_4ch}, + {SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S16_LE, 4, vol_s32_to_s16_4ch}, + {SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S32_LE, 4, vol_s32_to_s32_4ch}, + {SOF_IPC_FRAME_S16_LE, SOF_IPC_FRAME_S24_4LE, 4, vol_s16_to_s24_4ch}, + {SOF_IPC_FRAME_S24_4LE, SOF_IPC_FRAME_S16_LE, 4, vol_s24_to_s16_4ch}, + {SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S24_4LE, 4, vol_s32_to_s24_4ch}, + {SOF_IPC_FRAME_S24_4LE, SOF_IPC_FRAME_S32_LE, 4, vol_s24_to_s32_4ch}, + {SOF_IPC_FRAME_S24_4LE, SOF_IPC_FRAME_S24_4LE, 4, vol_s24_to_s24_4ch}, +}; + +scale_vol vol_get_processing_function(struct comp_dev *dev) +{ + struct comp_data *cd = comp_get_drvdata(dev); + int i; + + /* map the volume function for source and sink buffers */ + for (i = 0; i < ARRAY_SIZE(func_map); i++) { + if (cd->source_format != func_map[i].source) + continue; + if (cd->sink_format != func_map[i].sink) + continue; + if (dev->params.channels != func_map[i].channels) + continue; + + return func_map[i].func; + } + + return NULL; +} + +#endif diff --git a/src/audio/volume_hifi3.c b/src/audio/volume_hifi3.c new file mode 100644 index 0000000..e410de3 --- /dev/null +++ b/src/audio/volume_hifi3.c @@ -0,0 +1,307 @@ +/* + * Copyright (c) 2018, 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: Tomasz Lauda tomasz.lauda@linux.intel.com + */ + +#include "volume.h" + +#if defined(__XCC__) && XCHAL_HAVE_HIFI3 + +#include <xtensa/tie/xt_hifi3.h> + +#define VOL_SCALE (uint32_t)((double)INT32_MAX / VOL_MAX) + +static void vol_s16_to_s16(struct comp_dev *dev, struct comp_buffer *sink, + struct comp_buffer *source) +{ + struct comp_data *cd = comp_get_drvdata(dev); + uint32_t vol_scaled[SOF_IPC_MAX_CHANNELS]; + ae_f32x2 volume; + ae_f32x2 mult; + ae_f32x2 out_sample; + ae_f16x4 in_sample; + size_t channel; + int i; + int limit = source->size / (dev->params.channels << 1); + ae_int16 *in = (ae_int16 *)source->r_ptr; + ae_int16 *out = (ae_int16 *)sink->w_ptr; + + /* Scale to VOL_MAX */ + for (channel = 0; channel < dev->params.channels; channel++) + vol_scaled[channel] = cd->volume[channel] * VOL_SCALE; + + /* Main processing loop */ + for (i = 0; i < limit; i++) { + /* Processing per channel */ + for (channel = 0; channel < dev->params.channels; channel++) { + /* Load the input sample */ + AE_L16_XP(in_sample, in, sizeof(ae_int16)); + + /* Get gain coefficients */ + volume = *((ae_f32 *)&vol_scaled[channel]); + + /* Multiply the input sample */ + mult = AE_MULFP32X16X2RS_L(volume, in_sample); + + /* Shift right and round to get 16 in 32 bits */ + out_sample = AE_SRAA32RS(mult, 16); + + /* Store the output sample */ + AE_S16_0_XP(AE_MOVF16X4_FROMF32X2(out_sample), + out, sizeof(ae_int16)); + } + } +} + +static void vol_s16_to_sX(struct comp_dev *dev, struct comp_buffer *sink, + struct comp_buffer *source) +{ + struct comp_data *cd = comp_get_drvdata(dev); + uint32_t vol_scaled[SOF_IPC_MAX_CHANNELS]; + ae_f32x2 volume; + ae_f32x2 mult; + ae_f32x2 out_sample; + ae_f16x4 in_sample; + size_t channel; + uint8_t shift_left = 0; + int i; + int limit = source->size / (dev->params.channels << 1); + ae_int16 *in = (ae_int16 *)source->r_ptr; + ae_int32 *out = (ae_int32 *)sink->w_ptr; + + /* Get value of shift left */ + if (cd->sink_format == SOF_IPC_FRAME_S24_4LE) + shift_left = 8; + else if (cd->sink_format == SOF_IPC_FRAME_S32_LE) + shift_left = 16; + + /* Scale to VOL_MAX */ + for (channel = 0; channel < dev->params.channels; channel++) + vol_scaled[channel] = cd->volume[channel] * VOL_SCALE; + + /* Main processing loop */ + for (i = 0; i < limit; i++) { + /* Processing per channel */ + for (channel = 0; channel < dev->params.channels; channel++) { + /* Load the input sample */ + AE_L16_XP(in_sample, in, sizeof(ae_int16)); + + /* Get gain coefficients */ + volume = *((ae_f32 *)&vol_scaled[channel]); + + /* Multiply the input sample */ + mult = AE_MULFP32X16X2RS_L(volume, in_sample); + + /* Shift right and round to get 16 in 32 bits */ + out_sample = AE_SRAA32RS(mult, 16); + + /* Shift left to get the right alignment */ + out_sample = AE_SLAA32(out_sample, shift_left); + + /* Store the output sample */ + AE_S32_L_XP(out_sample, out, sizeof(ae_int32)); + } + } +} + +static void vol_sX_to_s16(struct comp_dev *dev, struct comp_buffer *sink, + struct comp_buffer *source) +{ + struct comp_data *cd = comp_get_drvdata(dev); + uint32_t vol_scaled[SOF_IPC_MAX_CHANNELS]; + ae_f32x2 volume; + ae_f32x2 mult; + ae_f32x2 in_sample; + ae_f16x4 out_sample; + size_t channel; + uint8_t shift_left = 0; + int i; + int limit = source->size / (dev->params.channels << 2); + ae_int32 *in = (ae_int32 *)source->r_ptr; + ae_int16 *out = (ae_int16 *)sink->w_ptr; + + /* Get value of shift left */ + if (cd->source_format == SOF_IPC_FRAME_S24_4LE) + shift_left = 8; + + /* Scale to VOL_MAX */ + for (channel = 0; channel < dev->params.channels; channel++) + vol_scaled[channel] = cd->volume[channel] * VOL_SCALE; + + /* Main processing loop */ + for (i = 0; i < limit; i++) { + /* Processing per channel */ + for (channel = 0; channel < dev->params.channels; channel++) { + /* Load the input sample */ + AE_L32_XP(in_sample, in, sizeof(ae_int32)); + + /* Shift left to get the right alignment */ + in_sample = AE_SLAA32(in_sample, shift_left); + + /* Get gain coefficients */ + volume = *((ae_f32 *)&vol_scaled[channel]); + + /* Multiply the input sample */ + mult = AE_MULFP32X2RS(volume, in_sample); + + /* Shift right to get 16 in 32 bits */ + out_sample = AE_MOVF16X4_FROMF32X2 + (AE_SRLA32(mult, 16)); + + /* Store the output sample */ + AE_S16_0_XP(out_sample, out, sizeof(ae_int16)); + } + } +} + +static void vol_s24_to_s24_s32(struct comp_dev *dev, struct comp_buffer *sink, + struct comp_buffer *source) +{ + struct comp_data *cd = comp_get_drvdata(dev); + uint32_t vol_scaled[SOF_IPC_MAX_CHANNELS]; + ae_f32x2 volume; + ae_f32x2 in_sample; + ae_f32x2 out_sample; + ae_f32x2 mult; + size_t channel; + uint8_t shift_left = 0; + int i; + int limit = source->size / (dev->params.channels << 2); + ae_int32 *in = (ae_int32 *)source->r_ptr; + ae_int32 *out = (ae_int32 *)sink->w_ptr; + + /* Get value of shift left */ + if (cd->sink_format == SOF_IPC_FRAME_S32_LE) + shift_left = 8; + + /* Scale to VOL_MAX */ + for (channel = 0; channel < dev->params.channels; channel++) + vol_scaled[channel] = cd->volume[channel] * VOL_SCALE; + + /* Main processing loop */ + for (i = 0; i < limit; i++) { + /* Processing per channel */ + for (channel = 0; channel < dev->params.channels; channel++) { + /* Load the input sample */ + AE_L32_XP(in_sample, in, sizeof(ae_int32)); + + /* Get gain coefficients */ + volume = *((ae_f32 *)&vol_scaled[channel]); + + /* Multiply the input sample */ + mult = AE_MULFP32X2RS(volume, AE_SLAA32(in_sample, 8)); + + /* Shift right to get 24 in 32 bits (LSB) */ + out_sample = AE_SRLA32(mult, 8); + + /* Shift left to get the right alignment */ + out_sample = AE_SLAA32(out_sample, shift_left); + + /* Store the output sample */ + AE_S32_L_XP(out_sample, out, sizeof(ae_int32)); + } + } +} + +static void vol_s32_to_s24_s32(struct comp_dev *dev, struct comp_buffer *sink, + struct comp_buffer *source) +{ + struct comp_data *cd = comp_get_drvdata(dev); + uint32_t vol_scaled[SOF_IPC_MAX_CHANNELS]; + ae_f32x2 volume; + ae_f32x2 in_sample; + ae_f32x2 out_sample; + ae_f32x2 mult; + size_t channel; + uint8_t shift_right = 0; + int i; + int limit = source->size / (dev->params.channels << 2); + ae_int32 *in = (ae_int32 *)source->r_ptr; + ae_int32 *out = (ae_int32 *)sink->w_ptr; + + /* Get value of shift right */ + if (cd->sink_format == SOF_IPC_FRAME_S24_4LE) + shift_right = 8; + + /* Scale to VOL_MAX */ + for (channel = 0; channel < dev->params.channels; channel++) + vol_scaled[channel] = cd->volume[channel] * VOL_SCALE; + + /* Main processing loop */ + for (i = 0; i < limit; i++) { + /* Processing per channel */ + for (channel = 0; channel < dev->params.channels; channel++) { + /* Load the input sample */ + AE_L32_XP(in_sample, in, sizeof(ae_int32)); + + /* Get gain coefficients */ + volume = *((ae_f32 *)&vol_scaled[channel]); + + /* Multiply the input sample */ + mult = AE_MULFP32X2RS(volume, in_sample); + + /* Shift right to get the right alignment */ + out_sample = AE_SRLA32(mult, shift_right); + + /* Store the output sample */ + AE_S32_L_XP(out_sample, out, sizeof(ae_int32)); + } + } +} + +const struct comp_func_map func_map[] = { + {SOF_IPC_FRAME_S16_LE, SOF_IPC_FRAME_S16_LE, 0, vol_s16_to_s16}, + {SOF_IPC_FRAME_S16_LE, SOF_IPC_FRAME_S24_4LE, 0, vol_s16_to_sX}, + {SOF_IPC_FRAME_S16_LE, SOF_IPC_FRAME_S32_LE, 0, vol_s16_to_sX}, + {SOF_IPC_FRAME_S24_4LE, SOF_IPC_FRAME_S16_LE, 0, vol_sX_to_s16}, + {SOF_IPC_FRAME_S24_4LE, SOF_IPC_FRAME_S24_4LE, 0, vol_s24_to_s24_s32}, + {SOF_IPC_FRAME_S24_4LE, SOF_IPC_FRAME_S32_LE, 0, vol_s24_to_s24_s32}, + {SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S16_LE, 0, vol_sX_to_s16}, + {SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S24_4LE, 0, vol_s32_to_s24_s32}, + {SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S32_LE, 0, vol_s32_to_s24_s32}, +}; + +scale_vol vol_get_processing_function(struct comp_dev *dev) +{ + struct comp_data *cd = comp_get_drvdata(dev); + int i; + + /* map the volume function for source and sink buffers */ + for (i = 0; i < ARRAY_SIZE(func_map); i++) { + if (cd->source_format != func_map[i].source) + continue; + if (cd->sink_format != func_map[i].sink) + continue; + + return func_map[i].func; + } + + return NULL; +} + +#endif
On Thu, 2018-06-14 at 17:46 +0100, Liam Girdwood wrote:
From: Tomasz Lauda tomasz.lauda@linux.intel.com
This patch adds parameters validation for ipc. It uses generic macros.
All applied.
Thanks
Liam
participants (1)
-
Liam Girdwood