1 files changed, 207 insertions, 22 deletions

diff --git a/audio_out.c b/audio_out.c
index 9b814a6..3438f53 100644
--- a/audio_out.c
+++ b/audio_out.c
@@ -35,6 +35,7 @@
 
 #define EFFECT_UUID_NULL EFFECT_UUID_NULL_OUT
 #define EFFECT_UUID_NULL_STR EFFECT_UUID_NULL_STR_OUT
+#include <audio_utils/resampler.h>
 #include "audio_hw.h"
 #include "mixer.h"
 
@@ -77,6 +78,9 @@ int audio_out_pcm_open(struct tinyalsa_audio_stream_out *stream_out)
 
 	stream_out->pcm = pcm;
 
+	if(stream_out->resampler != NULL)
+		stream_out->resampler->reset(stream_out->resampler);
+
 	return 0;
 }
 
@@ -111,12 +115,151 @@ int audio_out_set_route(struct tinyalsa_audio_stream_out *stream_out,
 	return 0;
 }
 
+int audio_out_resampler_open(struct tinyalsa_audio_stream_out *stream_out)
+{
+	int rc;
+
+	if(stream_out == NULL)
+		return -1;
+
+	rc = create_resampler(stream_out->rate,
+		stream_out->mixer_props->rate,
+		popcount(stream_out->channels),
+		RESAMPLER_QUALITY_DEFAULT,
+		NULL,
+		&stream_out->resampler);
+	if(rc < 0 || stream_out->resampler == NULL) {
+		LOGE("Failed to create resampler");
+		return -1;
+	}
+
+	return 0;
+}
+
+void audio_out_resampler_close(struct tinyalsa_audio_stream_out *stream_out)
+{
+	if(stream_out == NULL)
+		return;
+
+	if(stream_out->resampler != NULL) {
+		release_resampler(stream_out->resampler);
+		stream_out->resampler = NULL;
+	}
+}
+
+int audio_out_write_process(struct tinyalsa_audio_stream_out *stream_out, void *buffer, int size)
+{
+	size_t frames_out;
+
+	size_t frames_in;
+	int size_in;
+	void *buffer_in = NULL;
+
+	int frames_out_resampler;
+	int size_out_resampler;
+	void *buffer_out_resampler = NULL;
+
+	int frames_out_channels;
+	int size_out_channels;
+	void *buffer_out_channels = NULL;
+
+	int i, j;
+	int rc;
+
+	if(stream_out == NULL || buffer == NULL || size <= 0)
+		return -1;
+
+	frames_in = size / audio_stream_frame_size((struct audio_stream *) stream_out);
+	size_in = size;
+	buffer_in = buffer;
+
+	if(stream_out->resampler != NULL) {
+		frames_out_resampler = (frames_in * stream_out->mixer_props->rate) /
+			stream_out->rate;
+		frames_out_resampler = ((frames_out_resampler + 15) / 16) * 16;
+		size_out_resampler = frames_out_resampler * audio_stream_frame_size((struct audio_stream *) stream_out);
+		buffer_out_resampler = calloc(1, size_out_resampler);
+
+		frames_out = frames_out_resampler;
+		stream_out->resampler->resample_from_input(stream_out->resampler,
+			buffer_in, &frames_in, buffer_out_resampler, &frames_out);
+
+		frames_in = frames_out;
+		size_in = frames_out * audio_stream_frame_size((struct audio_stream *) stream_out);
+		buffer_in = buffer_out_resampler;
+	}
+
+	if(buffer_in == NULL)
+		goto error;
+
+	//FIXME: This is only for PCM 16
+	if(popcount(stream_out->mixer_props->channels) < popcount(stream_out->channels)) {
+		frames_out_channels = frames_in;
+		size_out_channels = frames_out_channels * popcount(stream_out->mixer_props->channels) * audio_bytes_per_sample(stream_out->mixer_props->format);
+		buffer_out_channels = calloc(1, size_out_channels);
+
+		int channels_count_in = popcount(stream_out->channels);
+		int channels_count_out = popcount(stream_out->mixer_props->channels);
+		int ratio = channels_count_in / channels_count_out;
+
+		int16_t *byte_in = (int16_t *) buffer_in;
+		int16_t *byte_out = (int16_t *) buffer_out_channels;
+		int16_t byte;
+
+		for(i=0 ; i < frames_out_channels * channels_count_out; i++) {
+			byte = 0;
+			for(j=0 ; j < ratio ; j++) {
+				byte += *byte_in / ratio;
+				byte_in++;
+			}
+
+			*byte_out = byte;
+			byte_out++;
+		}
+
+		frames_in = frames_out_channels;
+		size_in = size_out_channels;
+		buffer_in = buffer_out_channels;
+	} else if(popcount(stream_out->channels) != popcount(stream_out->mixer_props->channels)) {
+		LOGE("Asked for more channels than software can provide!");
+		goto error;
+	}
+
+	if(buffer_in != NULL) {
+		if(stream_out->pcm == NULL || !pcm_is_ready(stream_out->pcm)) {
+			LOGE("pcm device is not ready");
+			goto error;
+		}
+
+		rc = pcm_write(stream_out->pcm, buffer_in, size_in);
+		if(rc != 0) {
+			LOGE("pcm write failed!");
+			goto error;
+		}
+	}
+
+	if(buffer_out_resampler != NULL)
+		free(buffer_out_resampler);
+	if(buffer_out_channels != NULL)
+		free(buffer_out_channels);
+
+	return 0;
+
+error:
+	if(buffer_out_resampler != NULL)
+		free(buffer_out_resampler);
+	if(buffer_out_channels != NULL)
+		free(buffer_out_channels);
+
+	return -1;
+}
+
 static uint32_t audio_out_get_sample_rate(const struct audio_stream *stream)
 {
 	struct tinyalsa_audio_stream_out *stream_out;
 
 	if(stream == NULL)
-		return -1;
+		return 0;
 
 	stream_out = (struct tinyalsa_audio_stream_out *) stream;
 
@@ -134,8 +277,16 @@ static int audio_out_set_sample_rate(struct audio_stream *stream, uint32_t rate)
 
 	stream_out = (struct tinyalsa_audio_stream_out *) stream;
 
-	// FIXME: If rate is different, change resampler
-	stream_out->rate = rate;
+	if(stream_out->rate != (int) rate) {
+		stream_out->rate = rate;
+
+		if(stream_out->rate != stream_out->mixer_props->rate) {
+			audio_out_resampler_close(stream_out);
+			audio_out_resampler_open(stream_out);
+
+			stream_out->standby = 1;
+		}
+	}
 
 	return 0;
 }
@@ -151,7 +302,7 @@ static size_t audio_out_get_buffer_size(const struct audio_stream *stream)
 	stream_out = (struct tinyalsa_audio_stream_out *) stream;
 
 	size = (stream_out->mixer_props->period_size * stream_out->rate) /
-		stream_out->rate;
+		stream_out->mixer_props->rate;
 	size = ((size + 15) / 16) * 16;
 	size = size * audio_stream_frame_size((struct audio_stream *) stream);
 
@@ -193,8 +344,12 @@ static int audio_out_set_format(struct audio_stream *stream, int format)
 
 	stream_out = (struct tinyalsa_audio_stream_out *) stream;
 
-	// FIXME: If format is different, change resampler
-	stream_out->format = format;
+	if(stream_out->format != (audio_format_t) format) {
+		stream_out->format = format;
+
+		if(stream_out->format != stream_out->mixer_props->format)
+			stream_out->standby = 1;
+	}
 
 	return 0;
 }
@@ -287,9 +442,21 @@ static char *audio_out_get_parameters(const struct audio_stream *stream, const c
 
 static uint32_t audio_out_get_latency(const struct audio_stream_out *stream)
 {
+	struct tinyalsa_audio_stream_out *stream_out;
+	uint32_t latency;
+
 	LOGD("%s(%p)", __func__, stream);
 
-	return 0;
+	if(stream == NULL)
+		return -1;
+
+	stream_out = (struct tinyalsa_audio_stream_out *) stream;
+
+	latency = (stream_out->mixer_props->period_size *
+		stream_out->mixer_props->period_count * 1000) /
+		stream_out->mixer_props->rate;
+
+	return latency;
 }
 
 static int audio_out_set_volume(struct audio_stream_out *stream, float left,
@@ -344,14 +511,9 @@ static ssize_t audio_out_write(struct audio_stream_out *stream,
 		stream_out->standby = 0;
 	}
 
-	if(stream_out->pcm == NULL || !pcm_is_ready(stream_out->pcm)) {
-		LOGE("pcm device is not ready");
-		return -1;
-	}
-
-	rc = pcm_write(stream_out->pcm, (void *) buffer, (int) bytes);
-	if(rc != 0) {
-		LOGE("pcm write failed!");
+	rc = audio_out_write_process(stream_out, (void *) buffer, (int) bytes);
+	if(rc < 0) {
+		LOGE("Process and write failed!");
 		return -1;
 	}
 
@@ -424,7 +586,7 @@ int audio_hw_open_output_stream(struct audio_hw_device *dev,
 	LOGD("%s(%p, %d, %p, %p, %p, %p)",
 		__func__, dev, devices, format, channels, sample_rate, stream_out);
 
-	if(dev == NULL || stream_out == NULL)
+	if(dev == NULL || format == NULL || channels == NULL || sample_rate == NULL || stream_out == NULL)
 		return -EINVAL;
 
 	tinyalsa_audio_device = (struct tinyalsa_audio_device *) dev;
@@ -463,12 +625,35 @@ int audio_hw_open_output_stream(struct audio_hw_device *dev,
 	if(tinyalsa_audio_stream_out->mixer_props == NULL)
 		goto error_stream;
 
-	tinyalsa_audio_stream_out->rate =
-		tinyalsa_audio_stream_out->mixer_props->rate;
-	tinyalsa_audio_stream_out->channels =
-		tinyalsa_audio_stream_out->mixer_props->channels;
-	tinyalsa_audio_stream_out->format =
-		tinyalsa_audio_stream_out->mixer_props->format;
+	// Default values
+	if(tinyalsa_audio_stream_out->mixer_props->rate == 0)
+		tinyalsa_audio_stream_out->mixer_props->rate = 44100;
+	if(tinyalsa_audio_stream_out->mixer_props->channels == 0)
+		tinyalsa_audio_stream_out->mixer_props->channels = AUDIO_CHANNEL_OUT_STEREO;
+	if(tinyalsa_audio_stream_out->mixer_props->format == 0)
+		tinyalsa_audio_stream_out->mixer_props->format = AUDIO_FORMAT_PCM_16_BIT;
+
+	//Default incoming data will always be 44100Hz, stereo, PCM 16
+	if(*sample_rate == 0)
+		tinyalsa_audio_stream_out->rate = 44100;
+	else
+		tinyalsa_audio_stream_out->rate = *sample_rate;
+	if(*channels == 0)
+		tinyalsa_audio_stream_out->channels = AUDIO_CHANNEL_OUT_STEREO;
+	else
+		tinyalsa_audio_stream_out->channels = *channels;
+	if(*format == 0)
+		tinyalsa_audio_stream_out->format = AUDIO_FORMAT_PCM_16_BIT;
+	else
+		tinyalsa_audio_stream_out->format = *format;
+
+	if(tinyalsa_audio_stream_out->rate != tinyalsa_audio_stream_out->mixer_props->rate) {
+		rc = audio_out_resampler_open(tinyalsa_audio_stream_out);
+		if(rc < 0) {
+			LOGE("Unable to open resampler!");
+			goto error_stream;
+		}
+	}
 
 	*sample_rate = (uint32_t) tinyalsa_audio_stream_out->rate;
 	*channels = (uint32_t) tinyalsa_audio_stream_out->channels;