[Sound-open-firmware] [PATCH] volume: Add support for 4 channel input
Ranjani Sridharan
ranjani.sridharan at linux.intel.com
Thu Mar 22 17:53:06 CET 2018
This patch adds support for 4 channel input in volume
Signed-off-by: Ranjani Sridharan <ranjani.sridharan at linux.intel.com>
---
src/audio/volume.c | 311 ++++++++++++++++++++++++++++++++++++++++++++++++-----
1 file changed, 284 insertions(+), 27 deletions(-)
diff --git a/src/audio/volume.c b/src/audio/volume.c
index 0620e18..df926c5 100644
--- a/src/audio/volume.c
+++ b/src/audio/volume.c
@@ -91,9 +91,11 @@ struct comp_func_map {
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(struct comp_dev *dev, struct comp_buffer *sink,
- struct comp_buffer *source, uint32_t frames)
+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;
@@ -109,8 +111,8 @@ static void vol_s16_to_s32(struct comp_dev *dev, struct comp_buffer *sink,
}
/* copy and scale volume from 32 bit source buffer to 16 bit dest buffer */
-static void vol_s32_to_s16(struct comp_dev *dev, struct comp_buffer *sink,
- struct comp_buffer *source, uint32_t frames)
+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;
@@ -128,8 +130,8 @@ static void vol_s32_to_s16(struct comp_dev *dev, struct comp_buffer *sink,
}
/* copy and scale volume from 32 bit source buffer to 32 bit dest buffer */
-static void vol_s32_to_s32(struct comp_dev *dev, struct comp_buffer *sink,
- struct comp_buffer *source, uint32_t frames)
+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;
@@ -147,8 +149,8 @@ static void vol_s32_to_s32(struct comp_dev *dev, struct comp_buffer *sink,
}
/* copy and scale volume from 16 bit source buffer to 16 bit dest buffer */
-static void vol_s16_to_s16(struct comp_dev *dev, struct comp_buffer *sink,
- struct comp_buffer *source, uint32_t frames)
+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;
@@ -166,8 +168,8 @@ static void vol_s16_to_s16(struct comp_dev *dev, struct comp_buffer *sink,
}
/* copy and scale volume from 16 bit source buffer to 24 bit on 32 bit boundary dest buffer */
-static void vol_s16_to_s24(struct comp_dev *dev, struct comp_buffer *sink,
- struct comp_buffer *source, uint32_t frames)
+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;
@@ -185,8 +187,8 @@ static void vol_s16_to_s24(struct comp_dev *dev, struct comp_buffer *sink,
}
/* copy and scale volume from 16 bit source buffer to 24 bit on 32 bit boundary dest buffer */
-static void vol_s24_to_s16(struct comp_dev *dev, struct comp_buffer *sink,
- struct comp_buffer *source, uint32_t frames)
+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;
@@ -206,8 +208,8 @@ static void vol_s24_to_s16(struct comp_dev *dev, struct comp_buffer *sink,
}
/* copy and scale volume from 32 bit source buffer to 24 bit on 32 bit boundary dest buffer */
-static void vol_s32_to_s24(struct comp_dev *dev, struct comp_buffer *sink,
- struct comp_buffer *source, uint32_t frames)
+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;
@@ -225,8 +227,8 @@ static void vol_s32_to_s24(struct comp_dev *dev, struct comp_buffer *sink,
}
/* copy and scale volume from 16 bit source buffer to 24 bit on 32 bit boundary dest buffer */
-static void vol_s24_to_s32(struct comp_dev *dev, struct comp_buffer *sink,
- struct comp_buffer *source, uint32_t frames)
+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;
@@ -248,8 +250,8 @@ static void vol_s24_to_s32(struct comp_dev *dev, struct comp_buffer *sink,
/* Copy and scale volume from 24 bit source buffer to 24 bit on 32 bit boundary
* dest buffer.
*/
-static void vol_s24_to_s24(struct comp_dev *dev, struct comp_buffer *sink,
- struct comp_buffer *source, uint32_t frames)
+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;
@@ -267,17 +269,272 @@ static void vol_s24_to_s24(struct comp_dev *dev, struct comp_buffer *sink,
}
}
+/* 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},
- {SOF_IPC_FRAME_S16_LE, SOF_IPC_FRAME_S32_LE, 2, vol_s16_to_s32},
- {SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S16_LE, 2, vol_s32_to_s16},
- {SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S32_LE, 2, vol_s32_to_s32},
- {SOF_IPC_FRAME_S16_LE, SOF_IPC_FRAME_S24_4LE, 2, vol_s16_to_s24},
- {SOF_IPC_FRAME_S24_4LE, SOF_IPC_FRAME_S16_LE, 2, vol_s24_to_s16},
- {SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S24_4LE, 2, vol_s32_to_s24},
- {SOF_IPC_FRAME_S24_4LE, SOF_IPC_FRAME_S32_LE, 2, vol_s24_to_s32},
- {SOF_IPC_FRAME_S24_4LE, SOF_IPC_FRAME_S24_4LE, 2, vol_s24_to_s24},
+ {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},
};
/* synchronise host mmap() volume with real value */
--
2.14.1
More information about the Sound-open-firmware
mailing list