3 files changed, 60 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);