[Sound-open-firmware] [PATCH 2/2] volume: add HiFi3 implementation
Liam Girdwood
liam.r.girdwood at linux.intel.com
Thu Jun 14 18:46:18 CEST 2018
From: Tomasz Lauda <tomasz.lauda at 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 at 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 at linux.intel.com>
* Keyon Jie <yang.jie at linux.intel.com>
+ * Tomasz Lauda <tomasz.lauda at 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 at 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 at linux.intel.com>
+ * Keyon Jie <yang.jie at linux.intel.com>
+ * Tomasz Lauda <tomasz.lauda at 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 at 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
--
2.17.1
More information about the Sound-open-firmware
mailing list