Audio: Up_down_mixer: Convert stubs with arithmetic to generic C

This patch adds more conversions those include simple arithmetic
operations.

Signed-off-by: Seppo Ingalsuo <seppo.ingalsuo@linux.intel.com>

Author: singalsu

src/audio/up_down_mixer/up_down_mixer_generic.c

@@ -5,6 +5,7 @@
 // Author: Bartosz Kokoszko <bartoszx.kokoszko@intel.com>
 //         Seppo Ingalsuo <seppo.ingalsuo@linux.intel.com>
 
+#include <sof/audio/format.h>
 #include <errno.h>
 #include <stddef.h>
 #include <stdint.h>
@@ -262,7 +263,86 @@ void downmix32bit_5_0_mono(struct up_down_mixer_data *cd, const uint8_t * const
 void downmix32bit_5_1(struct up_down_mixer_data *cd, const uint8_t * const in_data,
 		      const uint32_t in_size, uint8_t * const out_data)
 {
-	sof_panic(0);
+	const uint8_t left_slot =     get_channel_location(cd->in_channel_map, CHANNEL_LEFT);
+	const uint8_t center_slot =   get_channel_location(cd->in_channel_map, CHANNEL_CENTER);
+	const uint8_t right_slot =    get_channel_location(cd->in_channel_map, CHANNEL_RIGHT);
+	uint8_t left_surround_slot =  get_channel_location(cd->in_channel_map, CHANNEL_LEFT_SURROUND);
+	uint8_t right_surround_slot = get_channel_location(cd->in_channel_map, CHANNEL_RIGHT_SURROUND);
+	const uint8_t lfe_slot =      get_channel_location(cd->in_channel_map, CHANNEL_LFE);
+
+	/* Must support also 5.1 Surround */
+	const bool surround_5_1_channel_map = (left_surround_slot == CHANNEL_INVALID) &&
+					      (right_surround_slot == CHANNEL_INVALID);
+
+	if (surround_5_1_channel_map) {
+		left_surround_slot = get_channel_location(cd->in_channel_map, CHANNEL_LEFT_SIDE);
+		right_surround_slot = get_channel_location(cd->in_channel_map, CHANNEL_RIGHT_SIDE);
+	}
+
+	/* Load the downmix coefficients. */
+	int32_t P_coefficient_left = cd->downmix_coefficients[CHANNEL_LEFT];
+	int32_t P_coefficient_center = cd->downmix_coefficients[CHANNEL_CENTER];
+	int32_t P_coefficient_right = cd->downmix_coefficients[CHANNEL_RIGHT];
+	int32_t P_coefficient_left_surround = cd->downmix_coefficients[CHANNEL_LEFT_SURROUND];
+	int32_t P_coefficient_right_surround = cd->downmix_coefficients[CHANNEL_RIGHT_SURROUND];
+	int32_t P_coefficient_lfe = cd->downmix_coefficients[CHANNEL_LFE];
+
+	if (surround_5_1_channel_map) {
+		P_coefficient_left_surround  = cd->downmix_coefficients[CHANNEL_LEFT_SIDE];
+		P_coefficient_right_surround = cd->downmix_coefficients[CHANNEL_RIGHT_SIDE];
+	}
+
+	const int32_t *input_left =           (int32_t *)(in_data + (left_slot << 2));
+	const int32_t *input_center =         (int32_t *)(in_data + (center_slot << 2));
+	const int32_t *input_right =          (int32_t *)(in_data + (right_slot << 2));
+	const int32_t *input_left_surround =  (int32_t *)(in_data + (left_surround_slot << 2));
+	const int32_t *input_right_surround = (int32_t *)(in_data + (right_surround_slot << 2));
+	const int32_t *input_lfe =            (int32_t *)(in_data + (lfe_slot << 2));
+
+	/* Downmixer single store. */
+	int32_t *output_left = (int32_t *)(out_data);
+	int32_t *output_right = (int32_t *)(out_data + sizeof(int32_t));
+
+	/*
+	 * We don't need to initialize those registers in loop's body.
+	 * Using non accumulating multiplication for the first left,rignt channels
+	 * is more efficient.
+	 * For non 5.1 version we cannot use this optimization, we don't know
+	 * which channel is present and which multiplication should reset output
+	 * accumulators.
+	 */
+	int64_t Q_tmp_left;
+	int64_t Q_tmp_right;
+
+	const int32_t *const end_input_left = input_left + (in_size / (sizeof(int32_t)));
+
+	while (input_left < end_input_left) {
+		/* Accumulate as Q1.31 * Q1.31 -> Q2.62 */
+		Q_tmp_left = (int64_t)P_coefficient_center * *input_center;
+		input_center += 6;
+
+		Q_tmp_left += (int64_t)P_coefficient_lfe * *input_lfe;
+		input_lfe += 6;
+		Q_tmp_right = Q_tmp_left;
+
+		Q_tmp_left += (int64_t)P_coefficient_left * *input_left;
+		input_left += 6;
+
+		Q_tmp_right += (int64_t)P_coefficient_right * *input_right;
+		input_right += 6;
+
+		Q_tmp_left += (int64_t)P_coefficient_left_surround * *input_left_surround;
+		input_left_surround += 6;
+
+		Q_tmp_right += (int64_t)P_coefficient_right_surround * *input_right_surround;
+		input_right_surround += 6;
+
+		/* Shift with round to Q1.31 and saturate and store output */
+		*output_left = sat_int32(Q_SHIFT_RND(Q_tmp_left, 62, 31));
+		*output_right = sat_int32(Q_SHIFT_RND(Q_tmp_right, 62, 31));
+		output_left += 2;
+		output_right += 2;
+	}
 }
 
 void downmix32bit_7_1(struct up_down_mixer_data *cd, const uint8_t * const in_data,
@@ -274,7 +354,15 @@ void downmix32bit_7_1(struct up_down_mixer_data *cd, const uint8_t * const in_da
 void downmix16bit_stereo(struct up_down_mixer_data *cd, const uint8_t * const in_data,
 			 const uint32_t in_size, uint8_t * const out_data)
 {
-	sof_panic(0);
+	int32_t sum;
+	const int16_t *in_data16 = (int16_t *)in_data;
+	int16_t *out_data16 = (int16_t *)out_data;
+	int idx;
+
+	for (idx = 0; idx < (in_size >> 2); ++idx) {
+		sum = (int32_t)in_data16[2 * idx] + in_data16[2 * idx + 1];
+		out_data16[idx] = sat_int16((sum + 1) >> 1);
+	}
 }
 
 void shiftcopy16bit_mono(struct up_down_mixer_data *cd, const uint8_t * const in_data,
@@ -326,7 +414,15 @@ void downmix16bit_4ch_mono(struct up_down_mixer_data *cd, const uint8_t * const
 void downmix32bit_stereo(struct up_down_mixer_data *cd, const uint8_t * const in_data,
 			 const uint32_t in_size, uint8_t * const out_data)
 {
-	sof_panic(0);
+	int64_t sum;
+	int32_t *in_data32 = (int32_t *)in_data;
+	int32_t *out_data32 = (int32_t *)out_data;
+	int idx;
+
+	for (idx = 0; idx < (in_size >> 3); idx++) {
+		sum = (int64_t)in_data32[2 * idx] + in_data32[2 * idx + 1];
+		out_data32[idx] = sat_int32((sum + 1) >> 1);
+	}
 }
 
 void downmix32bit_3_1_mono(struct up_down_mixer_data *cd, const uint8_t * const in_data,