CRAS: cras_fmt_conv - add S24_3LE format support

Some devices only support S24_3LE format.  This change adds support
for converting between S24_3LE and S16_LE, so those devices will
work with ChromeOS.

BUG=chromium:437836
TEST=Plug in a device using S24_3LE format (ex: Dragonfly DAC) and
play music, verify the sound is audiable.

Change-Id: I86dafd62f4d082b651dfc6f0603e3e1b5b1301c0
Reviewed-on: https://chromium-review.googlesource.com/288253
Commit-Ready: Yao-Wen Mao <yaowen@google.com>
Tested-by: Yao-Wen Mao <yaowen@google.com>
Reviewed-by: Dylan Reid <dgreid@chromium.org>

6 files changed, 355 insertions, 0 deletions

diff --git a/cras/src/alsa_plugin/pcm_cras.c b/cras/src/alsa_plugin/pcm_cras.c
index f95179f9..93b61a31 100644
--- a/cras/src/alsa_plugin/pcm_cras.c
+++ b/cras/src/alsa_plugin/pcm_cras.c
@@ -388,6 +388,7 @@ static int set_hw_constraints(struct snd_pcm_cras *pcm_cras)
 		SND_PCM_FORMAT_S16_LE,
 		SND_PCM_FORMAT_S24_LE,
 		SND_PCM_FORMAT_S32_LE,
+		SND_PCM_FORMAT_S24_3LE,
 	};
 	int rc;
 
diff --git a/cras/src/server/cras_alsa_helpers.c b/cras/src/server/cras_alsa_helpers.c
index bb7a7886..d5e1762f 100644
--- a/cras/src/server/cras_alsa_helpers.c
+++ b/cras/src/server/cras_alsa_helpers.c
@@ -57,6 +57,7 @@ static const snd_pcm_format_t test_formats[] = {
 	SND_PCM_FORMAT_S16_LE,
 	SND_PCM_FORMAT_S24_LE,
 	SND_PCM_FORMAT_S32_LE,
+	SND_PCM_FORMAT_S24_3LE,
 	(snd_pcm_format_t)0
 };
 
diff --git a/cras/src/server/cras_empty_iodev.c b/cras/src/server/cras_empty_iodev.c
index d3cbae9d..ef592093 100644
--- a/cras/src/server/cras_empty_iodev.c
+++ b/cras/src/server/cras_empty_iodev.c
@@ -31,6 +31,7 @@ static snd_pcm_format_t empty_supported_formats[] = {
 	SND_PCM_FORMAT_S16_LE,
 	SND_PCM_FORMAT_S24_LE,
 	SND_PCM_FORMAT_S32_LE,
+	SND_PCM_FORMAT_S24_3LE,
 	0
 };
 
diff --git a/cras/src/server/cras_fmt_conv.c b/cras/src/server/cras_fmt_conv.c
index c9e7d713..d1be3156 100644
--- a/cras/src/server/cras_fmt_conv.c
+++ b/cras/src/server/cras_fmt_conv.c
@@ -96,6 +96,21 @@ static void convert_s32le_to_s16le(const uint8_t *in, size_t in_samples,
 		*_out = (int16_t)(*_in >> 16);
 }
 
+/* Converts from S24_3LE to S16. */
+static void convert_s243le_to_s16le(const uint8_t *in, size_t in_samples,
+				   uint8_t *out)
+{
+	/* find how to calculate in and out size, implement the conversion
+	 * between S24_3LE and S16 */
+
+	size_t i;
+	int8_t *_in = (int8_t *)in;
+	uint16_t *_out = (uint16_t *)out;
+
+	for (i = 0; i < in_samples; i++, _in += 3, _out++)
+		memcpy(_out, _in + 1, 2);
+}
+
 /* Converts from S16 to U8. */
 static void convert_s16le_to_u8(const uint8_t *in, size_t in_samples,
 				uint8_t *out)
@@ -131,6 +146,20 @@ static void convert_s16le_to_s32le(const uint8_t *in, size_t in_samples,
 		*_out = ((int32_t)*_in << 16);
 }
 
+/* Converts from S16 to S24_3LE. */
+static void convert_s16le_to_s243le(const uint8_t *in, size_t in_samples,
+				   uint8_t *out)
+{
+	size_t i;
+	int16_t *_in = (int16_t *)in;
+	uint8_t *_out = (uint8_t *)out;
+
+	for (i = 0; i < in_samples; i++, _in++, _out += 3) {
+		*_out = 0;
+		memcpy(_out + 1, _in, 2);
+	}
+}
+
 /*
  * Convert between different channel numbers.
  */
@@ -422,6 +451,9 @@ struct cras_fmt_conv *cras_fmt_conv_create(const struct cras_audio_format *in,
 		case SND_PCM_FORMAT_S32_LE:
 			conv->in_format_converter = convert_s32le_to_s16le;
 			break;
+		case SND_PCM_FORMAT_S24_3LE:
+			conv->in_format_converter = convert_s243le_to_s16le;
+			break;
 		default:
 			syslog(LOG_WARNING, "Invalid format %d", in->format);
 			cras_fmt_conv_destroy(conv);
@@ -442,6 +474,9 @@ struct cras_fmt_conv *cras_fmt_conv_create(const struct cras_audio_format *in,
 		case SND_PCM_FORMAT_S32_LE:
 			conv->out_format_converter = convert_s16le_to_s32le;
 			break;
+		case SND_PCM_FORMAT_S24_3LE:
+			conv->out_format_converter = convert_s16le_to_s243le;
+			break;
 		default:
 			syslog(LOG_WARNING, "Invalid format %d", out->format);
 			cras_fmt_conv_destroy(conv);
diff --git a/cras/src/server/cras_mix.c b/cras/src/server/cras_mix.c
index f74b64fc..d2d5e3cf 100644
--- a/cras/src/server/cras_mix.c
+++ b/cras/src/server/cras_mix.c
@@ -368,6 +368,161 @@ void cras_mix_add_stride_s32_le(uint8_t *dst, uint8_t *src,
 }
 
 /*
+ * Signed 24 bit little endian in three bytes functions.
+ */
+
+/* Convert 3bytes Signed 24bit integer to a Signed 32bit integer.
+ * Just a helper function. */
+static inline void convert_single_s243le_to_s32le(int32_t *dst,
+						  const uint8_t *src)
+{
+	*dst = 0;
+	memcpy((uint8_t *)dst + 1, src, 3);
+}
+
+static inline void convert_single_s32le_to_s243le(uint8_t *dst,
+						  const int32_t *src)
+{
+	memcpy(dst, (uint8_t *)src + 1, 3);
+}
+
+static void cras_mix_add_clip_s24_3le(uint8_t *dst,
+				      const uint8_t *src,
+				      size_t count)
+{
+	int64_t sum;
+	int32_t dst_frame;
+	int32_t src_frame;
+	size_t i;
+
+	for (i = 0; i < count; i++, dst += 3, src += 3) {
+		convert_single_s243le_to_s32le(&dst_frame, dst);
+		convert_single_s243le_to_s32le(&src_frame, src);
+		sum = (int64_t)dst_frame + (int64_t)src_frame;
+		if (sum > INT32_MAX)
+			sum = INT32_MAX;
+		else if (sum < INT32_MIN)
+			sum = INT32_MIN;
+		dst_frame = (int32_t)sum;
+		convert_single_s32le_to_s243le(dst, &dst_frame);
+	}
+}
+
+/* Adds src into dst, after scaling by vol.
+ * Just hard limits to the min and max S24 value, can be improved later. */
+static void scale_add_clip_s24_3le(uint8_t *dst,
+				   const uint8_t *src,
+				   size_t count,
+				   float vol)
+{
+	int64_t sum;
+	int32_t dst_frame;
+	int32_t src_frame;
+	size_t i;
+
+	if (vol > MAX_VOLUME_TO_SCALE)
+		return cras_mix_add_clip_s24_3le(dst, src, count);
+
+	for (i = 0; i < count; i++, dst += 3, src += 3) {
+		convert_single_s243le_to_s32le(&dst_frame, dst);
+		convert_single_s243le_to_s32le(&src_frame, src);
+		sum = (int64_t)dst_frame + (int64_t)(src_frame * vol);
+		if (sum > INT32_MAX)
+			sum = INT32_MAX;
+		else if (sum < INT32_MIN)
+			sum = INT32_MIN;
+		dst_frame = (int32_t)sum;
+		convert_single_s32le_to_s243le(dst, &dst_frame);
+	}
+}
+
+/* Adds the first stream to the mix.  Don't need to mix, just setup to the new
+ * values. If volume is 1.0, just memcpy. */
+static void copy_scaled_s24_3le(uint8_t *dst,
+			        const uint8_t *src,
+			        size_t count,
+			        float volume_scaler)
+{
+	int32_t frame;
+	size_t i;
+
+	if (volume_scaler > MAX_VOLUME_TO_SCALE) {
+		memcpy(dst, src, 3 * count * sizeof(*src));
+		return;
+	}
+
+	for (i = 0; i < count; i++, dst += 3, src += 3) {
+		convert_single_s243le_to_s32le(&frame, src);
+		frame *= volume_scaler;
+		convert_single_s32le_to_s243le(dst, &frame);
+	}
+}
+
+static void cras_scale_buffer_s24_3le(uint8_t *buffer, unsigned int count,
+				      float scaler)
+{
+	int32_t frame;
+	int i;
+
+	if (scaler > MAX_VOLUME_TO_SCALE)
+		return;
+
+	if (scaler < MIN_VOLUME_TO_SCALE) {
+		memset(buffer, 0, 3 * count * sizeof(*buffer));
+		return;
+	}
+
+	for (i = 0; i < count; i++, buffer += 3) {
+		convert_single_s243le_to_s32le(&frame, buffer);
+		frame *= scaler;
+		convert_single_s32le_to_s243le(buffer, &frame);
+	}
+}
+
+static void cras_mix_add_s24_3le(uint8_t *dst, uint8_t *src,
+				 unsigned int count, unsigned int index,
+				 int mute, float mix_vol)
+{
+	uint8_t *out = dst;
+	uint8_t *in = src;
+
+	if (mute || (mix_vol < MIN_VOLUME_TO_SCALE)) {
+		if (index == 0)
+			memset(out, 0, 3 * count * sizeof(*out));
+		return;
+	}
+
+	if (index == 0)
+		return copy_scaled_s24_3le(out, in, count, mix_vol);
+
+	scale_add_clip_s24_3le(out, in, count, mix_vol);
+}
+
+void cras_mix_add_stride_s24_3le(uint8_t *dst, uint8_t *src,
+				 unsigned int dst_stride,
+				 unsigned int src_stride,
+				 unsigned int count)
+{
+	unsigned int i;
+	int64_t sum;
+	int32_t dst_frame;
+	int32_t src_frame;
+
+	for (i = 0; i < count; i++) {
+		convert_single_s243le_to_s32le(&dst_frame, dst);
+		convert_single_s243le_to_s32le(&src_frame, src);
+		sum = (int64_t)dst_frame + (int64_t)src_frame;
+		if (sum > INT32_MAX)
+			sum = INT32_MAX;
+		else if (sum < INT32_MIN)
+			sum = INT32_MIN;
+		dst_frame = (int32_t)sum;
+		convert_single_s32le_to_s243le(dst, &dst_frame);
+		dst += dst_stride;
+		src += src_stride;
+	}
+}
+/*
  * Exported Interface
  */
 
@@ -381,6 +536,8 @@ void cras_scale_buffer(snd_pcm_format_t fmt, uint8_t *buff, unsigned int count,
 		return cras_scale_buffer_s24_le(buff, count, scaler);
 	case SND_PCM_FORMAT_S32_LE:
 		return cras_scale_buffer_s32_le(buff, count, scaler);
+	case SND_PCM_FORMAT_S24_3LE:
+		return cras_scale_buffer_s24_3le(buff, count, scaler);
 	default:
 		break;
 	}
@@ -400,6 +557,9 @@ void cras_mix_add(snd_pcm_format_t fmt, uint8_t *dst, uint8_t *src,
 	case SND_PCM_FORMAT_S32_LE:
 		return cras_mix_add_s32_le(dst, src, count, index, mute,
 					   mix_vol);
+	case SND_PCM_FORMAT_S24_3LE:
+		return cras_mix_add_s24_3le(dst, src, count, index, mute,
+					    mix_vol);
 	default:
 		break;
 	}
@@ -419,6 +579,9 @@ void cras_mix_add_stride(snd_pcm_format_t fmt, uint8_t *dst, uint8_t *src,
 	case SND_PCM_FORMAT_S32_LE:
 		return cras_mix_add_stride_s32_le(dst, src, dst_stride,
 						  src_stride, count);
+	case SND_PCM_FORMAT_S24_3LE:
+		return cras_mix_add_stride_s24_3le(dst, src, dst_stride,
+						   src_stride, count);
 	default:
 		break;
 	}
diff --git a/cras/src/tests/mix_unittest.cc b/cras/src/tests/mix_unittest.cc
index e399f7f2..401619a2 100644
--- a/cras/src/tests/mix_unittest.cc
+++ b/cras/src/tests/mix_unittest.cc
@@ -412,6 +412,160 @@ TEST_F(MixTestSuiteS32_LE, StrideCopy) {
   EXPECT_EQ(0, memcmp(compare_buffer_, mix_buffer_, kBufferFrames * 8));
 }
 
+class MixTestSuiteS24_3LE : public testing::Test{
+  protected:
+    virtual void SetUp() {
+      fmt_ = SND_PCM_FORMAT_S24_3LE;
+      fr_bytes_ = 3 * kNumChannels;
+      mix_buffer_ = (uint8_t *)malloc(kBufferFrames * fr_bytes_);
+      src_buffer_ = static_cast<uint8_t *>(
+          calloc(1, kBufferFrames * fr_bytes_ + sizeof(cras_audio_shm_area)));
+
+      for (size_t i = 0; i < kBufferFrames * kNumChannels; i++) {
+        memcpy(src_buffer_ + 3*i, &i, 3);
+        int32_t tmp = -i * 256;
+        memcpy(mix_buffer_ + 3*i, (uint8_t *)&tmp + 1, 3);
+      }
+
+      compare_buffer_ = (uint8_t *)malloc(kBufferFrames * fr_bytes_);
+    }
+
+    virtual void TearDown() {
+      free(mix_buffer_);
+      free(compare_buffer_);
+      free(src_buffer_);
+    }
+
+  uint8_t *mix_buffer_;
+  uint8_t *src_buffer_;
+  uint8_t *compare_buffer_;
+  snd_pcm_format_t fmt_;
+  unsigned int fr_bytes_;
+};
+
+TEST_F(MixTestSuiteS24_3LE, MixFirst) {
+  cras_mix_add(fmt_, (uint8_t *)mix_buffer_, (uint8_t *)src_buffer_,
+               kNumSamples, 0, 0, 1.0);
+  EXPECT_EQ(0, memcmp(mix_buffer_, src_buffer_, kBufferFrames * fr_bytes_));
+}
+
+TEST_F(MixTestSuiteS24_3LE, MixTwo) {
+  cras_mix_add(fmt_, (uint8_t *)mix_buffer_, (uint8_t *)src_buffer_,
+               kNumSamples, 0, 0, 1.0);
+  cras_mix_add(fmt_, (uint8_t *)mix_buffer_, (uint8_t *)src_buffer_,
+               kNumSamples, 1, 0, 1.0);
+
+  for (size_t i = 0; i < kBufferFrames * kNumChannels; i++) {
+    int32_t tmp = 0;
+    memcpy((uint8_t *)&tmp + 1, src_buffer_ + 3*i, 3);
+    tmp *= 2;
+    memcpy(compare_buffer_ + 3*i, (uint8_t *)&tmp + 1, 3);
+  }
+  EXPECT_EQ(0, memcmp(mix_buffer_, compare_buffer_, kBufferFrames * fr_bytes_));
+}
+
+TEST_F(MixTestSuiteS24_3LE, MixTwoClip) {
+  cras_mix_add(fmt_, (uint8_t *)mix_buffer_, (uint8_t *)src_buffer_,
+               kNumSamples, 0, 0, 1.0);
+  for (size_t i = 0; i < kBufferFrames * kNumChannels; i++) {
+    int32_t tmp = INT32_MAX;
+    memcpy(src_buffer_ + 3*i, (uint8_t *)&tmp + 1, 3);
+  }
+  cras_mix_add(fmt_, (uint8_t *)mix_buffer_, (uint8_t *)src_buffer_,
+               kNumSamples, 1, 0, 1.0);
+
+  for (size_t i = 0; i < kBufferFrames * kNumChannels; i++) {
+    int32_t tmp = INT32_MAX;
+    memcpy(compare_buffer_ + 3*i, (uint8_t *)&tmp + 1, 3);
+  }
+  EXPECT_EQ(0, memcmp(mix_buffer_, compare_buffer_, kBufferFrames * fr_bytes_));
+}
+
+TEST_F(MixTestSuiteS24_3LE, MixFirstMuted) {
+  cras_mix_add(fmt_, (uint8_t *)mix_buffer_, (uint8_t *)src_buffer_,
+               kNumSamples, 0, 1, 1.0);
+
+  for (size_t i = 0; i < kBufferFrames * kNumChannels; i++)
+    memset(compare_buffer_ + 3*i, 0, 3);
+  EXPECT_EQ(0, memcmp(mix_buffer_, compare_buffer_, kBufferFrames * fr_bytes_));
+}
+
+TEST_F(MixTestSuiteS24_3LE, MixFirstZeroVolume) {
+  cras_mix_add(fmt_, (uint8_t *)mix_buffer_, (uint8_t *)src_buffer_,
+                      kNumSamples, 0, 0, 0.0);
+
+  for (size_t i = 0; i < kBufferFrames * kNumChannels; i++)
+    memset(compare_buffer_ + 3*i, 0, 3);
+  EXPECT_EQ(0, memcmp(mix_buffer_, compare_buffer_, kBufferFrames * fr_bytes_));
+}
+
+TEST_F(MixTestSuiteS24_3LE, MixFirstHalfVolume) {
+  cras_mix_add(fmt_, (uint8_t *)mix_buffer_, (uint8_t *)src_buffer_,
+                      kNumSamples, 0, 0, 0.5);
+
+  for (size_t i = 0; i < kBufferFrames * kNumChannels; i++) {
+    int32_t tmp = 0;
+    memcpy((uint8_t *)&tmp + 1, src_buffer_ + 3*i, 3);
+    tmp *= 0.5;
+    memcpy(compare_buffer_ + 3*i, (uint8_t *)&tmp + 1, 3);
+  }
+  EXPECT_EQ(0, memcmp(mix_buffer_, compare_buffer_, kBufferFrames * fr_bytes_));
+}
+
+TEST_F(MixTestSuiteS24_3LE, MixTwoSecondHalfVolume) {
+  cras_mix_add(fmt_, (uint8_t *)mix_buffer_, (uint8_t *)src_buffer_,
+               kNumSamples, 0, 0, 1.0);
+  cras_mix_add(fmt_, (uint8_t *)mix_buffer_, (uint8_t *)src_buffer_,
+               kNumSamples, 1, 0, 0.5);
+
+  for (size_t i = 0; i < kBufferFrames * kNumChannels; i++) {
+    int32_t tmp1 = 0, tmp2 = 0;
+    memcpy((uint8_t *)&tmp1 + 1, src_buffer_ + 3*i, 3);
+    memcpy((uint8_t *)&tmp2 + 1, src_buffer_ + 3*i, 3);
+    tmp1 = tmp1 + (int32_t)(tmp2 * 0.5);
+    memcpy(compare_buffer_ + 3*i, (uint8_t *)&tmp1 + 1, 3);
+  }
+  EXPECT_EQ(0, memcmp(mix_buffer_, compare_buffer_, kBufferFrames * fr_bytes_));
+}
+
+TEST_F(MixTestSuiteS24_3LE, ScaleFullVolume) {
+  memcpy(compare_buffer_, src_buffer_, kBufferFrames  * fr_bytes_);
+  cras_scale_buffer(fmt_, (uint8_t *)mix_buffer_, kNumSamples, 0.999999999);
+
+  EXPECT_EQ(0, memcmp(compare_buffer_, src_buffer_, kBufferFrames * fr_bytes_));
+}
+
+TEST_F(MixTestSuiteS24_3LE, ScaleMinVolume) {
+  memset(compare_buffer_, 0, kBufferFrames * fr_bytes_);
+  cras_scale_buffer(fmt_, (uint8_t *)src_buffer_, kNumSamples, 0.0000000001);
+
+  EXPECT_EQ(0, memcmp(compare_buffer_, src_buffer_, kBufferFrames * fr_bytes_));
+}
+
+TEST_F(MixTestSuiteS24_3LE, ScaleHalfVolume) {
+  for (size_t i = 0; i < kBufferFrames * kNumChannels; i++) {
+    int32_t tmp = 0;
+    memcpy((uint8_t *)&tmp + 1, src_buffer_ + 3*i, 3);
+    tmp *= 0.5;
+    memcpy(compare_buffer_ + 3*i, (uint8_t *)&tmp + 1, 3);
+  }
+  cras_scale_buffer(fmt_, (uint8_t *)src_buffer_, kNumSamples, 0.5);
+
+  EXPECT_EQ(0, memcmp(compare_buffer_, src_buffer_, kBufferFrames * fr_bytes_));
+}
+
+TEST_F(MixTestSuiteS24_3LE, StrideCopy) {
+  for (size_t i = 0; i < kBufferFrames * kNumChannels; i += 2)
+    memcpy(compare_buffer_ + 3*i, src_buffer_ + 3*i/2, 3);
+  for (size_t i = 1; i < kBufferFrames * kNumChannels; i += 2)
+    memset(compare_buffer_ + 3*i, 0, 3);
+  memset(mix_buffer_, 0, kBufferFrames * 6);
+  cras_mix_add_stride(fmt_, (uint8_t *)mix_buffer_, (uint8_t *)src_buffer_,
+                      kBufferFrames, 6, 3);
+
+  EXPECT_EQ(0, memcmp(compare_buffer_, mix_buffer_, kBufferFrames * 6));
+}
+
 /* Stubs */
 extern "C" {