1 files changed, 295 insertions, 22 deletions

diff --git a/audio_in.c b/audio_in.c
index 560aa25..3933bbf 100644
--- a/audio_in.c
+++ b/audio_in.c
@@ -1,6 +1,6 @@
 /*
  * Copyright (C) 2012 Paul Kocialkowski <contact@paulk.fr>
- * 
+ *
  * This is based on Galaxy Nexus audio.primary.tuna implementation:
  * Copyright 2011, The Android Open-Source Project
  *
@@ -31,6 +31,7 @@
 
 #define EFFECT_UUID_NULL EFFECT_UUID_NULL_IN
 #define EFFECT_UUID_NULL_STR EFFECT_UUID_NULL_STR_IN
+#include <audio_utils/resampler.h>
 #include "audio_hw.h"
 #include "mixer.h"
 
@@ -73,6 +74,9 @@ int audio_in_pcm_open(struct tinyalsa_audio_stream_in *stream_in)
 
 	stream_in->pcm = pcm;
 
+	if(stream_in->resampler != NULL)
+		stream_in->resampler->reset(stream_in->resampler);
+
 	return 0;
 }
 
@@ -108,12 +112,239 @@ int audio_in_set_route(struct tinyalsa_audio_stream_in *stream_in,
 	return 0;
 }
 
+int audio_in_resampler_open(struct tinyalsa_audio_stream_in *stream_in)
+{
+	int rc;
+
+	if(stream_in == NULL)
+		return -1;
+
+	rc = create_resampler(stream_in->mixer_props->rate,
+		stream_in->rate,
+		popcount(stream_in->mixer_props->channels),
+		RESAMPLER_QUALITY_DEFAULT,
+		&stream_in->buffer_provider,
+		&stream_in->resampler);
+	if(rc < 0 || stream_in->resampler == NULL) {
+		LOGE("Failed to create resampler");
+		return -1;
+	}
+
+	stream_in->buffer = malloc(stream_in->mixer_props->period_size *
+		popcount(stream_in->mixer_props->channels) *
+		audio_bytes_per_sample(stream_in->mixer_props->format));
+
+	return 0;
+}
+
+void audio_in_resampler_close(struct tinyalsa_audio_stream_in *stream_in)
+{
+	if(stream_in == NULL)
+		return;
+
+	if(stream_in->resampler != NULL) {
+		release_resampler(stream_in->resampler);
+		stream_in->resampler = NULL;
+	}
+
+	if(stream_in->buffer != NULL) {
+		free(stream_in->buffer);
+		stream_in->buffer = NULL;
+	}
+}
+
+int audio_in_get_next_buffer(struct resampler_buffer_provider *buffer_provider,
+	struct resampler_buffer *buffer)
+{
+	struct tinyalsa_audio_stream_in *stream_in;
+	int rc;
+
+	if(buffer_provider == NULL || buffer == NULL)
+		return -1;
+
+	stream_in = (struct tinyalsa_audio_stream_in *) ((void *) buffer_provider -
+		offsetof(struct tinyalsa_audio_stream_in, buffer_provider));
+
+	if(stream_in->frames_left == 0) {
+		if(stream_in->pcm == NULL || !pcm_is_ready(stream_in->pcm)) {
+			LOGE("pcm device is not ready");
+			goto error_pcm;
+		}
+
+		rc = pcm_read(stream_in->pcm, stream_in->buffer,
+			stream_in->mixer_props->period_size *
+			popcount(stream_in->mixer_props->channels) *
+			audio_bytes_per_sample(stream_in->mixer_props->format));
+		if(rc != 0) {
+			LOGE("pcm read failed!");
+			goto error_pcm;
+		}
+
+		stream_in->frames_left = stream_in->mixer_props->period_size;
+	}
+
+	buffer->frame_count = (buffer->frame_count > stream_in->frames_left) ?
+		stream_in->frames_left : buffer->frame_count;
+
+	buffer->raw = stream_in->buffer +
+		(stream_in->mixer_props->period_size - stream_in->frames_left) *
+		popcount(stream_in->mixer_props->channels) *
+		audio_bytes_per_sample(stream_in->mixer_props->format);
+
+	return 0;
+
+error_pcm:
+	buffer->raw = NULL;
+	buffer->frame_count = 0;
+	return -1;
+}
+
+void audio_in_release_buffer(struct resampler_buffer_provider *buffer_provider,
+	struct resampler_buffer *buffer)
+{
+	struct tinyalsa_audio_stream_in *stream_in;
+
+	if(buffer_provider == NULL || buffer == NULL)
+		return;
+
+	stream_in = (struct tinyalsa_audio_stream_in *) ((void *) buffer_provider -
+		offsetof(struct tinyalsa_audio_stream_in, buffer_provider));
+
+	stream_in->frames_left -= buffer->frame_count;
+}
+
+int audio_in_read_process(struct tinyalsa_audio_stream_in *stream_in, 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_read;
+	int size_out_read;
+	void *buffer_out_read = NULL;
+
+	int frames_out_channels;
+	int size_out_channels;
+	void *buffer_out_channels = NULL;
+
+	int i, j;
+	int rc;
+
+	if(stream_in == NULL || buffer == NULL || size <= 0)
+		return -1;
+
+	frames_in = size / audio_stream_frame_size((struct audio_stream *) stream_in);
+
+	if(stream_in->resampler != NULL) {
+		frames_out_resampler = frames_in;
+		size_out_resampler = frames_out_resampler * popcount(stream_in->mixer_props->channels) * audio_bytes_per_sample(stream_in->mixer_props->format);
+		buffer_out_resampler = calloc(1, size_out_resampler);
+
+		frames_out = 0;
+		while(frames_out < frames_out_resampler) {
+			frames_in = frames_out_resampler - frames_out;
+			stream_in->resampler->resample_from_provider(stream_in->resampler,
+				buffer_out_resampler + (size_out_resampler / frames_out_resampler) * frames_out,
+				&frames_in);
+
+			frames_out += frames_in;
+		}
+
+		frames_in = frames_out_resampler;
+		size_in = size_out_resampler;
+		buffer_in = buffer_out_resampler;
+	} else {
+		frames_out_read = frames_in;
+		size_out_read = frames_out_read * popcount(stream_in->mixer_props->channels) * audio_bytes_per_sample(stream_in->mixer_props->format);
+		buffer_out_read = calloc(1, size_out_read);
+
+		if(stream_in->pcm == NULL || !pcm_is_ready(stream_in->pcm)) {
+			LOGE("pcm device is not ready");
+			goto error;
+		}
+
+		rc = pcm_read(stream_in->pcm, buffer_out_read, size_out_read);
+		if(rc != 0) {
+			LOGE("pcm read failed!");
+			goto error;
+		}
+
+		frames_in = frames_out_read;
+		size_in = size_out_read;
+		buffer_in = buffer_out_read;
+	}
+
+	if(buffer_in == NULL)
+		goto error;
+
+	//FIXME: This is only for PCM 16
+	if(popcount(stream_in->channels) < popcount(stream_in->mixer_props->channels)) {
+		frames_out_channels = frames_in;
+		size_out_channels = frames_out_channels * audio_stream_frame_size((struct audio_stream *) stream_in);
+		buffer_out_channels = calloc(1, size_out_channels);
+
+		int channels_count_in = popcount(stream_in->mixer_props->channels);
+		int channels_count_out = popcount(stream_in->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_in->channels) != popcount(stream_in->mixer_props->channels)) {
+		LOGE("Asked for more channels than hardware can provide!");
+		goto error;
+	}
+
+	if(buffer_in != NULL)
+		memcpy(buffer, buffer_in, size);
+
+	if(buffer_out_resampler != NULL)
+		free(buffer_out_resampler);
+	if(buffer_out_read != NULL)
+		free(buffer_out_read);
+	if(buffer_out_channels != NULL)
+		free(buffer_out_channels);
+
+	return 0;
+
+error:
+	if(buffer_out_resampler != NULL)
+		free(buffer_out_resampler);
+	if(buffer_out_read != NULL)
+		free(buffer_out_read);
+	if(buffer_out_channels != NULL)
+		free(buffer_out_channels);
+
+	return -1;
+}
+
 static uint32_t audio_in_get_sample_rate(const struct audio_stream *stream)
 {
 	struct tinyalsa_audio_stream_in *stream_in;
 
 	if(stream == NULL)
-		return -1;
+		return 0;
 
 	stream_in = (struct tinyalsa_audio_stream_in *) stream;
 
@@ -131,8 +362,16 @@ static int audio_in_set_sample_rate(struct audio_stream *stream, uint32_t rate)
 
 	stream_in = (struct tinyalsa_audio_stream_in *) stream;
 
-	// FIXME: If rate is different, change resampler
-	stream_in->rate = rate;
+	if(stream_in->rate != (int) rate) {
+		stream_in->rate = rate;
+
+		if(stream_in->rate != stream_in->mixer_props->rate) {
+			audio_in_resampler_close(stream_in);
+			audio_in_resampler_open(stream_in);
+
+			stream_in->standby = 1;
+		}
+	}
 
 	return 0;
 }
@@ -148,7 +387,7 @@ static size_t audio_in_get_buffer_size(const struct audio_stream *stream)
 	stream_in = (struct tinyalsa_audio_stream_in *) stream;
 
 	size = (stream_in->mixer_props->period_size * stream_in->rate) /
-		stream_in->rate;
+		stream_in->mixer_props->rate;
 	size = ((size + 15) / 16) * 16;
 	size = size * audio_stream_frame_size((struct audio_stream *) stream);
 
@@ -190,8 +429,12 @@ static int audio_in_set_format(struct audio_stream *stream, int format)
 
 	stream_in = (struct tinyalsa_audio_stream_in *) stream;
 
-	// FIXME: If format is different, change resampler
-	stream_in->format = format;
+	if(stream_in->format != (audio_format_t) format) {
+		stream_in->format = format;
+
+		if(stream_in->format != stream_in->mixer_props->format)
+			stream_in->standby = 1;
+	}
 
 	return 0;
 }
@@ -329,14 +572,9 @@ static ssize_t audio_in_read(struct audio_stream_in *stream,
 		stream_in->standby = 0;
 	}
 
-	if(stream_in->pcm == NULL || !pcm_is_ready(stream_in->pcm)) {
-		LOGE("pcm device is not ready");
-		return -1;
-	}
-
-	rc = pcm_read(stream_in->pcm, (void *) buffer, (int) bytes);
-	if(rc != 0) {
-		LOGE("pcm read failed!");
+	rc = audio_in_read_process(stream_in, buffer, (int) bytes);
+	if(rc < 0) {
+		LOGE("Read and process failed!");
 		return -1;
 	}
 
@@ -379,6 +617,9 @@ void audio_hw_close_input_stream(struct audio_hw_device *dev,
 
 	stream_in = (struct tinyalsa_audio_stream_in *) stream;
 
+	if(stream_in != NULL && stream_in->resampler != NULL)
+		audio_in_resampler_close(stream_in);
+
 #ifdef YAMAHA_MC1N2_AUDIO
 	if(stream_in != NULL && !stream_in->standby)
 		yamaha_mc1n2_audio_input_stop(stream_in->device->mc1n2_pdata);
@@ -409,7 +650,7 @@ int audio_hw_open_input_stream(struct audio_hw_device *dev,
 	LOGD("%s(%p, %d, %p, %p, %p, %d, %p)",
 		__func__, dev, devices, format, channels, sample_rate, acoustics, stream_in);
 
-	if(dev == NULL || stream_in == NULL)
+	if(dev == NULL || format == NULL || channels == NULL || sample_rate == NULL || stream_in == NULL)
 		return -EINVAL;
 
 	tinyalsa_audio_device = (struct tinyalsa_audio_device *) dev;
@@ -447,12 +688,42 @@ int audio_hw_open_input_stream(struct audio_hw_device *dev,
 	if(tinyalsa_audio_stream_in->mixer_props == NULL)
 		goto error_stream;
 
-	tinyalsa_audio_stream_in->rate =
-		tinyalsa_audio_stream_in->mixer_props->rate;
-	tinyalsa_audio_stream_in->channels =
-		tinyalsa_audio_stream_in->mixer_props->channels;
-	tinyalsa_audio_stream_in->format =
-		tinyalsa_audio_stream_in->mixer_props->format;
+	// Default values
+	if(tinyalsa_audio_stream_in->mixer_props->rate == 0)
+		tinyalsa_audio_stream_in->mixer_props->rate = 44100;
+	if(tinyalsa_audio_stream_in->mixer_props->channels == 0)
+		tinyalsa_audio_stream_in->mixer_props->channels = AUDIO_CHANNEL_IN_STEREO;
+	if(tinyalsa_audio_stream_in->mixer_props->format == 0)
+		tinyalsa_audio_stream_in->mixer_props->format = AUDIO_FORMAT_PCM_16_BIT;
+
+	if(*sample_rate == 0)
+		tinyalsa_audio_stream_in->rate =
+			tinyalsa_audio_stream_in->mixer_props->rate;
+	else
+		tinyalsa_audio_stream_in->rate = *sample_rate;
+	if(*channels == 0)
+		tinyalsa_audio_stream_in->channels =
+			tinyalsa_audio_stream_in->mixer_props->channels;
+	else
+		tinyalsa_audio_stream_in->channels = *channels;
+	if(*format == 0)
+		tinyalsa_audio_stream_in->format =
+			tinyalsa_audio_stream_in->mixer_props->format;
+	else
+		tinyalsa_audio_stream_in->format = *format;
+
+        tinyalsa_audio_stream_in->buffer_provider.get_next_buffer =
+		audio_in_get_next_buffer;
+        tinyalsa_audio_stream_in->buffer_provider.release_buffer =
+		audio_in_release_buffer;
+
+	if(tinyalsa_audio_stream_in->rate != tinyalsa_audio_stream_in->mixer_props->rate) {
+		rc = audio_in_resampler_open(tinyalsa_audio_stream_in);
+		if(rc < 0) {
+			LOGE("Unable to open resampler!");
+			goto error_stream;
+		}
+	}
 
 	*sample_rate = (uint32_t) tinyalsa_audio_stream_in->rate;
 	*channels = (uint32_t) tinyalsa_audio_stream_in->channels;
@@ -482,6 +753,8 @@ int audio_hw_open_input_stream(struct audio_hw_device *dev,
 
 error_stream:
 	*stream_in = NULL;
+	if(tinyalsa_audio_stream_in->resampler != NULL)
+		audio_in_resampler_close(tinyalsa_audio_stream_in);
 	free(tinyalsa_audio_stream_in);
 	tinyalsa_audio_device->stream_in = NULL;