CRAS: fmt_conv: Support quad to 5.1 channel conversion

Add the logic to convert quad output to 5.1 surround by
copying the {front,rear} {left,right} of input to the {front,rear}
{left,right} of output respectively and fill others with zero.

BUG=b:183070810
TEST=1. `sox -n -c6 -b 16 -r 48000 /tmp/zero.raw synth 300 sine 0`
     2. `sox -n -c4 -b 16 -r 48000 /tmp/sine.raw synth 30 sine 300\
         sin 400 sin 500 sin 600`
     3. `cras_test_client -c6 -P /tmp/zero.raw`
     4. `cras_test_client -c4 -P /tmp/sine.raw` at the same time.
     5. Verified that each of the 4 chanel are are output with the
         correct tone with a 6 channel audio device.
TEST=`FEATURES=test USE=asan emerge-${BOARD} adhd`

Change-Id: I12784c3ff38da1d30994d0711ce0630e009775a9
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/third_party/adhd/+/2771463
Commit-Queue: Judy Hsiao <judyhsiao@chromium.org>
Commit-Queue: Yu-Hsuan Hsu <yuhsuan@chromium.org>
Tested-by: Judy Hsiao <judyhsiao@chromium.org>
Auto-Submit: Judy Hsiao <judyhsiao@chromium.org>
Reviewed-by: Yu-Hsuan Hsu <yuhsuan@chromium.org>

4 files changed, 93 insertions, 0 deletions

diff --git a/cras/src/server/cras_fmt_conv.c b/cras/src/server/cras_fmt_conv.c
index 509db1eb..842529b9 100644
--- a/cras/src/server/cras_fmt_conv.c
+++ b/cras/src/server/cras_fmt_conv.c
@@ -216,6 +216,19 @@ static size_t stereo_to_51(struct cras_fmt_conv *conv, const uint8_t *in,
 	return s16_stereo_to_51(left, right, center, in, in_frames, out);
 }
 
+static size_t quad_to_51(struct cras_fmt_conv *conv, const uint8_t *in,
+			 size_t in_frames, uint8_t *out)
+{
+	size_t fl, fr, rl, rr;
+
+	fl = conv->out_fmt.channel_layout[CRAS_CH_FL];
+	fr = conv->out_fmt.channel_layout[CRAS_CH_FR];
+	rl = conv->out_fmt.channel_layout[CRAS_CH_RL];
+	rr = conv->out_fmt.channel_layout[CRAS_CH_RR];
+
+	return s16_quad_to_51(fl, fr, rl, rr, in, in_frames, out);
+}
+
 static size_t _51_to_stereo(struct cras_fmt_conv *conv, const uint8_t *in,
 			    size_t in_frames, uint8_t *out)
 {
@@ -398,6 +411,8 @@ struct cras_fmt_conv *cras_fmt_conv_create(const struct cras_audio_format *in,
 			conv->channel_converter = quad_to_stereo;
 		} else if (in->num_channels == 2 && out->num_channels == 6) {
 			conv->channel_converter = stereo_to_51;
+		} else if (in->num_channels == 4 && out->num_channels == 6) {
+			conv->channel_converter = quad_to_51;
 		} else if (in->num_channels == 6 &&
 			   (out->num_channels == 2 || out->num_channels == 4)) {
 			int in_channel_layout_set = 0;
diff --git a/cras/src/server/cras_fmt_conv_ops.c b/cras/src/server/cras_fmt_conv_ops.c
index a306d216..adc55215 100644
--- a/cras/src/server/cras_fmt_conv_ops.c
+++ b/cras/src/server/cras_fmt_conv_ops.c
@@ -223,6 +223,44 @@ size_t s16_stereo_to_51(size_t left, size_t right, size_t center,
 }
 
 /*
+ * Channel converter: quad to 5.1 surround.
+ *
+ * Fit the front left/right of input to the front left/right of output
+ * and rear left/right of input to the rear left/right of output
+ * respectively and fill others with zero.
+ */
+size_t s16_quad_to_51(size_t font_left, size_t front_right, size_t rear_left,
+		      size_t rear_right, const uint8_t *_in, size_t in_frames,
+		      uint8_t *_out)
+{
+	size_t i;
+	const int16_t *in = (const int16_t *)_in;
+	int16_t *out = (int16_t *)_out;
+
+	memset(out, 0, sizeof(*out) * 6 * in_frames);
+
+	if (font_left != -1 && front_right != -1 && rear_left != -1 &&
+	    rear_right != -1)
+		for (i = 0; i < in_frames; i++) {
+			out[6 * i + font_left] = in[4 * i];
+			out[6 * i + front_right] = in[4 * i + 1];
+			out[6 * i + rear_left] = in[4 * i + 2];
+			out[6 * i + rear_right] = in[4 * i + 3];
+		}
+	else
+		/* Use default 5.1 channel mapping for the conversion.
+		 */
+		for (i = 0; i < in_frames; i++) {
+			out[6 * i] = in[4 * i];
+			out[6 * i + 1] = in[4 * i + 1];
+			out[6 * i + 4] = in[4 * i + 2];
+			out[6 * i + 5] = in[4 * i + 3];
+		}
+
+	return in_frames;
+}
+
+/*
  * Channel converter: 5.1 surround to stereo.
  *
  * The out buffer can have room for just stereo samples. This convert function
diff --git a/cras/src/server/cras_fmt_conv_ops.h b/cras/src/server/cras_fmt_conv_ops.h
index a1a57487..0af7564b 100644
--- a/cras/src/server/cras_fmt_conv_ops.h
+++ b/cras/src/server/cras_fmt_conv_ops.h
@@ -46,6 +46,13 @@ size_t s16_stereo_to_51(size_t left, size_t right, size_t center,
 			const uint8_t *in, size_t in_frames, uint8_t *out);
 
 /*
+ * Channel converter: quad to 5.1 surround.
+ */
+size_t s16_quad_to_51(size_t font_left, size_t front_right, size_t rear_left,
+		      size_t rear_right, const uint8_t *in, size_t in_frames,
+		      uint8_t *out);
+
+/*
  * Channel converter: 5.1 surround to stereo.
  */
 size_t s16_51_to_stereo(const uint8_t *in, size_t in_frames, uint8_t *out);
diff --git a/cras/src/tests/fmt_conv_ops_unittest.cc b/cras/src/tests/fmt_conv_ops_unittest.cc
index ebe8b65d..0baf37b1 100644
--- a/cras/src/tests/fmt_conv_ops_unittest.cc
+++ b/cras/src/tests/fmt_conv_ops_unittest.cc
@@ -418,6 +418,39 @@ TEST(FormatConverterOpsTest, StereoTo51S16LECenter) {
   }
 }
 
+// Test Quad to 5.1 conversion. S16_LE.
+TEST(FormatConverterOpsTest, QuadTo51S16LE) {
+  const size_t frames = 4096;
+  const size_t in_ch = 4;
+  const size_t out_ch = 6;
+  const unsigned int fl_quad = 0;
+  const unsigned int fr_quad = 1;
+  const unsigned int rl_quad = 2;
+  const unsigned int rr_quad = 3;
+
+  const unsigned int fl_51 = 0;
+  const unsigned int fr_51 = 1;
+  const unsigned int center_51 = 2;
+  const unsigned int lfe_51 = 3;
+  const unsigned int rl_51 = 4;
+  const unsigned int rr_51 = 5;
+
+  S16LEPtr src = CreateS16LE(frames * in_ch);
+  S16LEPtr dst = CreateS16LE(frames * out_ch);
+
+  size_t ret = s16_quad_to_51(fl_51, fr_51, rl_51, rr_51, (uint8_t*)src.get(),
+                              frames, (uint8_t*)dst.get());
+  EXPECT_EQ(ret, frames);
+  for (size_t i = 0; i < frames; ++i) {
+    EXPECT_EQ(0, dst[i * 6 + center_51]);
+    EXPECT_EQ(0, dst[i * 6 + lfe_51]);
+    EXPECT_EQ(src[i * 4 + fl_quad], dst[i * 6 + fl_51]);
+    EXPECT_EQ(src[i * 4 + fr_quad], dst[i * 6 + fr_51]);
+    EXPECT_EQ(src[i * 4 + rl_quad], dst[i * 6 + rl_51]);
+    EXPECT_EQ(src[i * 4 + rr_quad], dst[i * 6 + rr_51]);
+  }
+}
+
 // Test Stereo to 5.1 conversion.  S16_LE, LeftRight.
 TEST(FormatConverterOpsTest, StereoTo51S16LELeftRight) {
   const size_t frames = 4096;