mirror of https://github.com/thesofproject/sof.git
volume: fix for DC offset and handling of sign extension for 24-bit samples
This patch fixes the DC offsets introduced in the volume component due to shifts and handles sign extension for 24-bit input samples Signed-off-by: Ranjani Sridharan <ranjani.sridharan@linux.intel.com>
This commit is contained in:
Ranjani Sridharan
Liam Girdwood
parent
7c16d2cd38
commit
034b06f43a
|
|
@ -149,7 +149,9 @@ static int32_t tonegen(struct tone_state *sg)
|
|||
|
||||
/* sg->w is angle in Q4.28 radians format, sin() returns Q1.31 */
|
||||
/* sg->a is amplitude as Q1.31 */
|
||||
sine = q_mults_32x32(sin_fixed(sg->w), sg->a, 31, 31, 31);
|
||||
sine =
|
||||
q_mults_32x32(sin_fixed(sg->w), sg->a,
|
||||
Q_SHIFT_BITS_64(31, 31, 31));
|
||||
|
||||
/* Next point */
|
||||
w = (int64_t) sg->w + sg->w_step;
|
||||
|
|
@ -208,15 +210,16 @@ static void tonegen_control(struct tone_state *sg)
|
|||
&& (sg->repeat_count + 1 < sg->repeats)) {
|
||||
sg->block_count = 0;
|
||||
if (sg->ampl_coef > 0) {
|
||||
sg->a_target = sat_int32(q_multsr_32x32(
|
||||
sg->a_target,
|
||||
sg->ampl_coef, 31, 30, 31));
|
||||
sg->a_target =
|
||||
sat_int32(q_multsr_32x32(sg->a_target,
|
||||
sg->ampl_coef, Q_SHIFT_BITS_64(31, 30, 31)));
|
||||
sg->a = (sg->ramp_step > sg->a_target)
|
||||
? sg->a_target : sg->ramp_step;
|
||||
}
|
||||
if (sg->freq_coef > 0) {
|
||||
/* f is Q16.16, freq_coef is Q2.30 */
|
||||
p = q_multsr_32x32(sg->f, sg->freq_coef, 16, 30, 16);
|
||||
p = q_multsr_32x32(sg->f, sg->freq_coef,
|
||||
Q_SHIFT_BITS_64(16, 30, 16));
|
||||
tonegen_update_f(sg, (int32_t) p); /* No saturation */
|
||||
}
|
||||
sg->repeat_count++;
|
||||
|
|
@ -297,7 +300,8 @@ static void tonegen_update_f(struct tone_state *sg, int32_t f)
|
|||
f_max = Q_SHIFT_LEFT((int64_t) sg->fs, 0, 16 - 1);
|
||||
f_max = (f_max > INT32_MAXVALUE) ? INT32_MAXVALUE : f_max;
|
||||
sg->f = (f > f_max) ? f_max : f;
|
||||
w_tmp = q_multsr_32x32(sg->f, sg->c, 16, 31, 28); /* Q16 x Q31 -> Q28 */
|
||||
/* Q16 x Q31 -> Q28 */
|
||||
w_tmp = q_multsr_32x32(sg->f, sg->c, Q_SHIFT_BITS_64(16, 31, 28));
|
||||
w_tmp = (w_tmp > PI_Q4_28) ? PI_Q4_28 : w_tmp; /* Limit to pi Q4.28 */
|
||||
sg->w_step = (int32_t) w_tmp;
|
||||
|
||||
|
|
@ -362,7 +366,8 @@ static int tonegen_init(struct tone_state *sg, int32_t fs, int32_t f, int32_t a)
|
|||
tonegen_update_f(sg, f);
|
||||
|
||||
/* 125us as Q1.31 is 268435, calculate fs * 125e-6 in Q31.0 */
|
||||
sg->samples_in_block = (int32_t) q_multsr_32x32(fs, 268435, 0, 31, 0);
|
||||
sg->samples_in_block =
|
||||
(int32_t) q_multsr_32x32(fs, 268435, Q_SHIFT_BITS_64(0, 31, 0));
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -99,8 +99,8 @@ static void vol_s16_to_s32(struct comp_dev *dev, struct comp_buffer *sink,
|
|||
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];
|
||||
|
|
@ -117,9 +117,12 @@ static void vol_s32_to_s16(struct comp_dev *dev, struct comp_buffer *sink,
|
|||
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] = (((int32_t)src[i] >> 16) * cd->volume[0]) >> 16;
|
||||
dest[i + 1] = (((int32_t)src[i + 1] >> 16) * cd->volume[1]) >> 16;
|
||||
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));
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -133,9 +136,12 @@ static void vol_s32_to_s32(struct comp_dev *dev, struct comp_buffer *sink,
|
|||
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] = ((int64_t)src[i] * cd->volume[0]) >> 16;
|
||||
dest[i + 1] = ((int64_t)src[i + 1] * cd->volume[1]) >> 16;
|
||||
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));
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -149,9 +155,12 @@ static void vol_s16_to_s16(struct comp_dev *dev, struct comp_buffer *sink,
|
|||
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] = ((int32_t)src[i] * cd->volume[0]) >> 16;
|
||||
dest[i + 1] = ((int32_t)src[i + 1] * cd->volume[1]) >> 16;
|
||||
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));
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -165,9 +174,12 @@ static void vol_s16_to_s24(struct comp_dev *dev, struct comp_buffer *sink,
|
|||
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] = ((int32_t)src[i] * cd->volume[0]) >> 8;
|
||||
dest[i + 1] = ((int32_t)src[i + 1] * cd->volume[1]) >> 8;
|
||||
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], cd->volume[0], Q_SHIFT_BITS_64(15, 16, 23));
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -181,11 +193,14 @@ static void vol_s24_to_s16(struct comp_dev *dev, struct comp_buffer *sink,
|
|||
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)((((int32_t)src[i] >> 8) *
|
||||
cd->volume[0]) >> 16);
|
||||
dest[i + 1] = (int16_t)((((int32_t)src[i + 1] >> 8) *
|
||||
cd->volume[1]) >> 16);
|
||||
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));
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -199,9 +214,12 @@ static void vol_s32_to_s24(struct comp_dev *dev, struct comp_buffer *sink,
|
|||
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] = ((int64_t)src[i] * cd->volume[0]) >> 24;
|
||||
dest[i + 1] = ((int64_t)src[i + 1] * cd->volume[1]) >> 24;
|
||||
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));
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -215,11 +233,14 @@ static void vol_s24_to_s32(struct comp_dev *dev, struct comp_buffer *sink,
|
|||
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] = (int32_t)(((int64_t)src[i] *
|
||||
cd->volume[0]) >> 8);
|
||||
dest[i + 1] = (int32_t)(((int64_t)src[i + 1] *
|
||||
cd->volume[1]) >> 8);
|
||||
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));
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -234,12 +255,14 @@ static void vol_s24_to_s24(struct comp_dev *dev, struct comp_buffer *sink,
|
|||
int32_t *dest = (int32_t*) sink->w_ptr;
|
||||
|
||||
/* buffer sizes are always divisible by period frames */
|
||||
/* Samples are Q1.23 and volume is Q1.16 */
|
||||
/* Samples are Q1.23 --> Q1.23 and volume is Q1.16 */
|
||||
for (i = 0; i < frames * 2; i += 2) {
|
||||
dest[i] = (int32_t)(Q_MULTS_32X32(
|
||||
(int64_t)src[i], cd->volume[0], 23, 16, 23));
|
||||
dest[i + 1] = (int32_t)(Q_MULTS_32X32(
|
||||
(int64_t)src[i+1], cd->volume[1], 23, 16, 23));
|
||||
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));
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -56,6 +56,18 @@
|
|||
#define MINUS_80DB_Q1_31 214748 /* 10^(-80/20) */
|
||||
#define MINUS_90DB_Q1_31 67909 /* 10^(-90/20) */
|
||||
|
||||
/* Compute the number of shifts
|
||||
* This will result in a compiler overflow error if shift bits are out of
|
||||
* range as INT64_MAX/MIN is greater than 32 bit Q shift parameter
|
||||
*/
|
||||
#define Q_SHIFT_BITS_64(qx, qy, qz) \
|
||||
((qx + qy - qz) <= 63 ? (((qx + qy - qz) >= 0) ? \
|
||||
(qx + qy - qz) : INT64_MIN) : INT64_MAX)
|
||||
|
||||
#define Q_SHIFT_BITS_32(qx, qy, qz) \
|
||||
((qx + qy - qz) <= 31 ? (((qx + qy - qz) >= 0) ? \
|
||||
(qx + qy - qz) : INT32_MIN) : INT32_MAX)
|
||||
|
||||
/* Convert a float number to fractional Qnx.ny format. Note that there is no
|
||||
* check for nx+ny number of bits to fit the word length of int.
|
||||
*/
|
||||
|
|
@ -82,28 +94,24 @@
|
|||
#define SATP_INT32(x) (((x) > INT32_MAXVALUE) ? INT32_MAXVALUE : (x))
|
||||
#define SATM_INT32(x) (((x) < INT32_MINVALUE) ? INT32_MINVALUE : (x))
|
||||
|
||||
static inline int64_t q_mults_32x32(int32_t x, int32_t y,
|
||||
const int qx, const int qy, const int qp)
|
||||
static inline int64_t q_mults_32x32(int32_t x, int32_t y, const int shift_bits)
|
||||
{
|
||||
return ((int64_t)x * y) >> (qx+qy-qp);
|
||||
return ((int64_t)x * y) >> shift_bits;
|
||||
}
|
||||
|
||||
static inline int64_t q_multsr_32x32(int32_t x, int32_t y,
|
||||
const int qx, const int qy, const int qp)
|
||||
static inline int64_t q_multsr_32x32(int32_t x, int32_t y, const int shift_bits)
|
||||
{
|
||||
return ((((int64_t)x * y) >> (qx+qy-qp-1)) + 1) >> 1;
|
||||
return ((((int64_t)x * y) >> (shift_bits - 1)) + 1) >> 1;
|
||||
}
|
||||
|
||||
static inline int32_t q_mults_16x16(int16_t x, int16_t y,
|
||||
const int qx, const int qy, const int qp)
|
||||
static inline int32_t q_mults_16x16(int16_t x, int32_t y, const int shift_bits)
|
||||
{
|
||||
return ((int32_t)x * y) >> (qx+qy-qp);
|
||||
return ((int32_t)x * y) >> shift_bits;
|
||||
}
|
||||
|
||||
static inline int16_t q_multsr_16x16(int16_t x, int16_t y,
|
||||
const int qx, const int qy, const int qp)
|
||||
static inline int16_t q_multsr_16x16(int16_t x, int32_t y, const int shift_bits)
|
||||
{
|
||||
return ((((int32_t)x * y) >> (qx+qy-qp-1)) + 1) >> 1;
|
||||
return ((((int32_t)x * y) >> (shift_bits - 1)) + 1) >> 1;
|
||||
}
|
||||
|
||||
/* Saturation inline functions */
|
||||
|
|
@ -147,4 +155,22 @@ static inline int16_t sat_int16(int32_t x)
|
|||
return (int16_t)x;
|
||||
}
|
||||
|
||||
/* Fractional multiplication with shift and saturation */
|
||||
static inline int32_t q_multsr_sat_32x32(int32_t x, int32_t y,
|
||||
const int shift_bits)
|
||||
{
|
||||
return sat_int32(((((int64_t)x * y) >> (shift_bits - 1)) + 1) >> 1);
|
||||
}
|
||||
|
||||
static inline int16_t q_multsr_sat_16x16(int16_t x, int32_t y,
|
||||
const int shift_bits)
|
||||
{
|
||||
return sat_int16(((((int32_t)x * y) >> (shift_bits - 1)) + 1) >> 1);
|
||||
}
|
||||
|
||||
static inline int32_t sign_extend_s24(int32_t x)
|
||||
{
|
||||
return (x << 8) >> 8;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -594,6 +594,7 @@ int32_t sin_fixed(int32_t w) {
|
|||
delta = s1 - s0; /* Q1.31 */
|
||||
//sine = (int64_t) frac*delta; /* Q1.31 x Q1.31 -> Q2.62 */
|
||||
//sine = (sine >> 31) + s0; /* Q2.31 */
|
||||
sine = s0 + q_mults_32x32(frac, delta, 31, 31, 31); /* All Q1.31 */
|
||||
/* All Q1.31 */
|
||||
sine = s0 + q_mults_32x32(frac, delta, Q_SHIFT_BITS_64(31, 31, 31));
|
||||
return (int32_t) sine;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue