[Sound-open-firmware] [PATCH 1/2] volume: fix errors in volume scaling multiplication

[Ranjani Sridharan]

This patch fixes the errors in fixed-point multiplication function
for volume scaling that caused sign inversions. It also breaks up the
operation into 4 separate functions depending on the source and sink
formats.

Signed-off-by: Ranjani Sridharan <ranjani.sridharan at linux.intel.com>
---
 src/include/reef/audio/format.h | 114 ++++++++++++++++++++++++++++++++++++----
 1 file changed, 105 insertions(+), 9 deletions(-)

diff --git a/src/include/reef/audio/format.h b/src/include/reef/audio/format.h
index c9c23ec..669a314 100644
--- a/src/include/reef/audio/format.h
+++ b/src/include/reef/audio/format.h
@@ -153,22 +153,118 @@ 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)
+static inline int32_t sign_extend_s24(int32_t x)
 {
-	return sat_int32(((((int64_t)x * y) >> (shift_bits - 1)) + 1) >> 1);
+	return (x << 8) >> 8;
 }
 
-static inline int16_t q_multsr_sat_16x16(int16_t x, int32_t y,
-	const int shift_bits)
+/* Truncate a 64-bit int by shift_bits
+ * This function rounds the result before truncating
+ */
+static inline int64_t shift_64(int64_t x, int shift_bits)
 {
-	return sat_int16(((((int32_t)x * y) >> (shift_bits - 1)) + 1) >> 1);
+	return (int64_t)(((x >> (shift_bits - 1)) + 1) >> 1);
 }
 
-static inline int32_t sign_extend_s24(int32_t x)
+/* Truncate a 32-bit int by shift_bits
+ * This function rounds the result before truncating
+ */
+static inline int32_t shift_32(int32_t x, int shift_bits)
 {
-	return (x << 8) >> 8;
+	return (int32_t)(((x >> (shift_bits - 1)) + 1) >> 1);
+}
+
+/* Fractional multiplication with shift and saturation
+ * This function multiplies a 16-bit number by a 32-bit unsigned number and
+ * copies the result to a 32-bit int
+ */
+static inline void q_multsr_sat_16x32_32(int16_t *src, uint32_t vol,
+					 const int shift_bits, int32_t *dest,
+					 int offset, int sign_extend)
+{
+	int64_t temp;
+
+	if (shift_bits == 0) {
+		/* Samples are Q1.15 and volume unsigned is Q1.16 */
+		dest[offset] = (int32_t)(src[offset] * vol);
+		return;
+	}
+
+	/* store the intermediate product in an int64 */
+	temp = (int64_t)((int64_t)src[offset] * vol);
+
+	/* round, truncate and saturate if needed */
+	dest[offset] = sat_int32(shift_64(temp, shift_bits));
+}
+
+/* Fractional multiplication with shift and saturation
+ * This function multiplies 2 32-bit numbers and stores
+ * copies the result to a 32-bit int
+ */
+static inline void q_multsr_sat_32x32_32(int32_t *src, uint32_t vol,
+					 const int shift_bits, int32_t *dest,
+					 int offset, int sign_extend)
+{
+	int64_t temp;
+
+	/* store the intermediate product in an int64 */
+	if (sign_extend) {
+		/* Sample are Q1.23 and volume is unsigned Q1.16
+		 * sign extend the input samples
+		 */
+		temp = (int64_t)((int64_t)sign_extend_s24(src[offset]) *
+				 vol);
+	} else {
+		/* Sample are Q1.31 and volume unsigned is Q1.16 */
+		temp = (int64_t)((int64_t)src[offset] * vol);
+	}
+
+	/* round, truncate and saturate if needed */
+	dest[offset] = sat_int32(shift_64(temp, shift_bits));
+}
+
+/* Fractional multiplication with shift and saturation
+ * This function multiplies a 16-bit number by a 32-bit unsigned number and
+ * copies the result to a 16-bit int
+ */
+static inline void q_multsr_sat_16x16_16(int16_t *src, uint32_t vol,
+					 const int shift_bits, int16_t *dest,
+					 int offset, int sign_extend)
+{
+	int32_t temp;
+
+	/* Sample are Q1.15 and volume is unsigned Q1.16
+	 * store the intermediate product in an int32
+	 */
+	temp = (int32_t)((int32_t)src[offset] * vol);
+
+	/* round, truncate and saturate if needed */
+	dest[offset] = sat_int16(shift_32(temp, shift_bits));
+}
+
+/* Fractional multiplication with shift and saturation
+ * This function multiplies 2 32-bit numbers and
+ * copies the result to a 16-bit int
+ */
+static inline void q_multsr_sat_32x32_16(int32_t *src, uint32_t vol,
+					 const int shift_bits, int16_t *dest,
+					 int offset, int sign_extend)
+{
+	int64_t temp;
+
+	/* store the intermediate product in an int64 */
+	if (sign_extend) {
+		/* Sample are Q1.23 and volume is unsigned Q1.16
+		 * sign extend the input samples
+		 */
+		temp = (int64_t)((int64_t)sign_extend_s24(src[offset]) * vol);
+	} else {
+		/* Sample are Q1.31 and volume is unsigned Q1.16 */
+		temp = (int64_t)((int64_t)src[offset] * vol);
+	}
+
+	/* round, truncate and saturate if needed */
+	dest[offset] = (int16_t)sat_int32(shift_64(temp, shift_bits));
 }
 
 #endif
-- 
2.14.1