This patch adds support for 4 channel input in volume
Signed-off-by: Ranjani Sridharan ranjani.sridharan@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 */