1 files changed, 372 insertions, 33 deletions

diff --git a/gst-libs/gst/resample/resample.c b/gst-libs/gst/resample/resample.c
index b78ceb49..3d944843 100644
--- a/gst-libs/gst/resample/resample.c
+++ b/gst-libs/gst/resample/resample.c
@@ -23,7 +23,7 @@
 #include <stdio.h>
 #include <stdlib.h>
 
-#include <resample.h>
+#include "private.h"
 #include <gst/gstplugin.h>
 #include <gst/gstversion.h>
 
@@ -63,8 +63,6 @@ signed short double_to_s16_ppcasm(double x)
 	return rint(x);
 }
 
-static void resample_sinc_ft(resample_t * r);
-
 void resample_init(resample_t * r)
 {
 	r->i_start = 0;
@@ -88,20 +86,40 @@ void resample_reinit(resample_t * r)
 
 	r->halftaps = (r->filter_length - 1.0) * 0.5;
 
-	switch (r->method) {
-	default:
-	case RESAMPLE_NEAREST:
-		r->scale = resample_nearest;
-		break;
-	case RESAMPLE_BILINEAR:
-		r->scale = resample_bilinear;
-		break;
-	case RESAMPLE_SINC_SLOW:
-		r->scale = resample_sinc;
-		break;
-	case RESAMPLE_SINC:
-		r->scale = resample_sinc_ft;
-		break;
+	if (r->format == RESAMPLE_S16) {
+		switch (r->method) {
+		default:
+		case RESAMPLE_NEAREST:
+			r->scale = resample_nearest_s16;
+			break;
+		case RESAMPLE_BILINEAR:
+			r->scale = resample_bilinear_s16;
+			break;
+		case RESAMPLE_SINC_SLOW:
+			r->scale = resample_sinc_s16;
+			break;
+		case RESAMPLE_SINC:
+			r->scale = resample_sinc_ft_s16;
+			break;
+		}
+	} else if (r->format == RESAMPLE_FLOAT) {
+		switch (r->method) {
+		default:
+		case RESAMPLE_NEAREST:
+			r->scale = resample_nearest_float;
+			break;
+		case RESAMPLE_BILINEAR:
+			r->scale = resample_bilinear_float;
+			break;
+		case RESAMPLE_SINC_SLOW:
+			r->scale = resample_sinc_float;
+			break;
+		case RESAMPLE_SINC:
+			r->scale = resample_sinc_ft_float;
+			break;
+		}
+	} else {
+		fprintf (stderr, "resample: Unexpected format \"%d\"\n", r->format);
 	}
 }
 
@@ -159,14 +177,26 @@ void resample_scale(resample_t * r, void *i_buf, unsigned int i_size)
 		memset(r->buffer, 0, size);
 	}
 
-	if(r->channels==2){
-		conv_double_short(
-			r->buffer + r->filter_length * sizeof(double) * 2,
-			r->i_buf, r->i_samples * 2);
-	}else{
-		conv_double_short_dstr(
-			r->buffer + r->filter_length * sizeof(double) * 2,
-			r->i_buf, r->i_samples, sizeof(double) * 2);
+        if (r->format==RESAMPLE_S16) {
+		if(r->channels==2){
+			conv_double_short(
+					r->buffer + r->filter_length * sizeof(double) * 2,
+					r->i_buf, r->i_samples * 2);
+		} else {
+			conv_double_short_dstr(
+					r->buffer + r->filter_length * sizeof(double) * 2,
+					r->i_buf, r->i_samples, sizeof(double) * 2);
+		}
+	} else if (r->format==RESAMPLE_FLOAT) {
+		if(r->channels==2){
+			conv_double_float(
+					r->buffer + r->filter_length * sizeof(double) * 2,
+					r->i_buf, r->i_samples * 2);
+		} else {
+			conv_double_float_dstr(
+					r->buffer + r->filter_length * sizeof(double) * 2,
+					r->i_buf, r->i_samples, sizeof(double) * 2);
+		}
 	}
 
 	r->scale(r);
@@ -183,7 +213,7 @@ void resample_scale(resample_t * r, void *i_buf, unsigned int i_size)
 	r->i_start -= r->o_samples;
 }
 
-void resample_nearest(resample_t * r)
+void resample_nearest_s16(resample_t * r)
 {
 	signed short *i_ptr, *o_ptr;
 	int i_count = 0;
@@ -229,7 +259,7 @@ void resample_nearest(resample_t * r)
 	}
 }
 
-void resample_bilinear(resample_t * r)
+void resample_bilinear_s16(resample_t * r)
 {
 	signed short *i_ptr, *o_ptr;
 	int o_count = 0;
@@ -278,7 +308,7 @@ void resample_bilinear(resample_t * r)
 	}
 }
 
-void resample_sinc_slow(resample_t * r)
+void resample_sinc_slow_s16(resample_t * r)
 {
 	signed short *i_ptr, *o_ptr;
 	int i, j;
@@ -336,13 +366,15 @@ void resample_sinc_slow(resample_t * r)
 			a += r->o_inc;
 		}
 	}
+#undef GETBUF
 
 	memcpy(r->buffer,
 	       i_ptr + (r->i_samples - r->filter_length) * r->channels,
 	       r->filter_length * 2 * r->channels);
 }
 
-void resample_sinc(resample_t * r)
+/* only works for channels == 2 ???? */
+void resample_sinc_s16(resample_t * r)
 {
 	double *ptr;
 	signed short *o_ptr;
@@ -392,9 +424,6 @@ void resample_sinc(resample_t * r)
 	}
 }
 
-
-
-
 /*
  * Resampling audio is best done using a sinc() filter.
  *
@@ -436,7 +465,7 @@ static functable_t *ft;
 
 double out_tmp[10000];
 
-static void resample_sinc_ft(resample_t * r)
+void resample_sinc_ft_s16(resample_t * r)
 {
 	double *ptr;
 	signed short *o_ptr;
@@ -530,6 +559,316 @@ static void resample_sinc_ft(resample_t * r)
 	}
 }
 
+/********
+ ** float code below
+ ********/
+
+
+void resample_nearest_float(resample_t * r)
+{
+	float *i_ptr, *o_ptr;
+	int i_count = 0;
+	double a;
+	int i;
+
+	i_ptr = (float *) r->i_buf;
+	o_ptr = (float *) r->o_buf;
+
+	a = r->o_start;
+	i_count = 0;
+#define SCALE_LOOP(COPY,INC) \
+	for (i = 0; i < r->o_samples; i++) {	\
+		COPY;							\
+		a += r->o_inc;						\
+		while (a >= 1) {				\
+			a -= 1;						\
+			i_ptr+=INC;					\
+			i_count++;					\
+		}								\
+		o_ptr+=INC;						\
+   	}
+
+	switch (r->channels) {
+	case 1:
+		SCALE_LOOP(o_ptr[0] = i_ptr[0], 1);
+		break;
+	case 2:
+		SCALE_LOOP(o_ptr[0] = i_ptr[0];
+			   o_ptr[1] = i_ptr[1], 2);
+		break;
+	default:
+	{
+		int n, n_chan = r->channels;
+
+		SCALE_LOOP(for (n = 0; n < n_chan; n++) o_ptr[n] =
+			   i_ptr[n], n_chan);
+	}
+	}
+	if (i_count != r->i_samples) {
+		printf("handled %d in samples (expected %d)\n", i_count,
+		       r->i_samples);
+	}
+}
+
+void resample_bilinear_float(resample_t * r)
+{
+	float *i_ptr, *o_ptr;
+	int o_count = 0;
+	double b;
+	int i;
+	double acc0, acc1;
+
+	i_ptr = (float *) r->i_buf;
+	o_ptr = (float *) r->o_buf;
+
+	acc0 = r->acc[0];
+	acc1 = r->acc[1];
+	b = r->i_start;
+	for (i = 0; i < r->i_samples; i++) {
+		b += r->i_inc;
+		/*printf("in %d\n",i_ptr[0]); */
+		if(b>=2){
+			printf("not expecting b>=2\n");
+		}
+		if (b >= 1) {
+			acc0 += (1.0 - (b-r->i_inc)) * i_ptr[0];
+			acc1 += (1.0 - (b-r->i_inc)) * i_ptr[1];
+
+			o_ptr[0] = acc0;
+			/*printf("out %d\n",o_ptr[0]); */
+			o_ptr[1] = acc1;
+			o_ptr += 2;
+			o_count++;
+
+			b -= 1.0;
+
+			acc0 = b * i_ptr[0];
+			acc1 = b * i_ptr[1];
+		} else {
+			acc0 += i_ptr[0] * r->i_inc;
+			acc1 += i_ptr[1] * r->i_inc;
+		}
+		i_ptr += 2;
+	}
+	r->acc[0] = acc0;
+	r->acc[1] = acc1;
+
+	if (o_count != r->o_samples) {
+		printf("handled %d out samples (expected %d)\n", o_count,
+		       r->o_samples);
+	}
+}
+
+void resample_sinc_slow_float(resample_t * r)
+{
+	float *i_ptr, *o_ptr;
+	int i, j;
+	double c0, c1;
+	double a;
+	int start;
+	double center;
+	double weight;
+
+	if (!r->buffer) {
+		int size = r->filter_length * sizeof(float) * r->channels;
+
+		printf("resample temp buffer\n");
+		r->buffer = malloc(size);
+		memset(r->buffer, 0, size);
+	}
+
+	i_ptr = (float *) r->i_buf;
+	o_ptr = (float *) r->o_buf;
+
+	a = r->i_start;
+#define GETBUF(index,chan) (((index)<0) \
+			? ((float *)(r->buffer))[((index)+r->filter_length)*2+(chan)] \
+			: i_ptr[(index)*2+(chan)])
+	{
+		double sinx, cosx, sind, cosd;
+		double x, d;
+		double t;
+
+		for (i = 0; i < r->o_samples; i++) {
+			start = floor(a) - r->filter_length;
+			center = a - r->halftaps;
+			x = M_PI * (start - center) * r->o_inc;
+			sinx = sin(M_PI * (start - center) * r->o_inc);
+			cosx = cos(M_PI * (start - center) * r->o_inc);
+			d = M_PI * r->o_inc;
+			sind = sin(M_PI * r->o_inc);
+			cosd = cos(M_PI * r->o_inc);
+			c0 = 0;
+			c1 = 0;
+			for (j = 0; j < r->filter_length; j++) {
+				weight = (x==0)?1:(sinx/x);
+/*printf("j %d sin %g cos %g\n",j,sinx,cosx); */
+/*printf("j %d sin %g x %g sinc %g\n",j,sinx,x,weight); */
+				c0 += weight * GETBUF((start + j), 0);
+				c1 += weight * GETBUF((start + j), 1);
+				t = cosx * cosd - sinx * sind;
+				sinx = cosx * sind + sinx * cosd;
+				cosx = t;
+				x += d;
+			}
+			o_ptr[0] = c0;
+			o_ptr[1] = c1;
+			o_ptr += 2;
+			a += r->o_inc;
+		}
+	}
+#undef GETBUF
+
+	memcpy(r->buffer,
+	       i_ptr + (r->i_samples - r->filter_length) * r->channels,
+	       r->filter_length * sizeof(float) * r->channels);
+}
+
+/* only works for channels == 2 ???? */
+void resample_sinc_float(resample_t * r)
+{
+	double *ptr;
+	float *o_ptr;
+	int i, j;
+	double c0, c1;
+	double a;
+	int start;
+	double center;
+	double weight;
+	double x0, x, d;
+	double scale;
+
+	ptr = (double *) r->buffer;
+	o_ptr = (float *) r->o_buf;
+
+	/* scale provides a cutoff frequency for the low
+	 * pass filter aspects of sinc().  scale=M_PI
+	 * will cut off at the input frequency, which is
+	 * good for up-sampling, but will cause aliasing
+	 * for downsampling.  Downsampling needs to be
+	 * cut off at o_rate, thus scale=M_PI*r->i_inc. */
+	/* actually, it needs to be M_PI*r->i_inc*r->i_inc.
+	 * Need to research why. */
+	scale = M_PI*r->i_inc;
+	for (i = 0; i < r->o_samples; i++) {
+		a = r->o_start + i * r->o_inc;
+		start = floor(a - r->halftaps);
+/*printf("%d: a=%g start=%d end=%d\n",i,a,start,start+r->filter_length-1); */
+		center = a;
+		/*x = M_PI * (start - center) * r->o_inc; */
+		/*d = M_PI * r->o_inc; */
+		/*x = (start - center) * r->o_inc; */
+		x0 = (start - center) * r->o_inc;
+		d = r->o_inc;
+		c0 = 0;
+		c1 = 0;
+		for (j = 0; j < r->filter_length; j++) {
+			x = x0 + d * j;
+			weight = sinc(x*scale*r->i_inc)*scale/M_PI;
+			weight *= window_func(x/r->halftaps*r->i_inc);
+			c0 += weight * ptr[(start + j + r->filter_length)*2 + 0];
+			c1 += weight * ptr[(start + j + r->filter_length)*2 + 1];
+		}
+		o_ptr[0] = c0;
+		o_ptr[1] = c1;
+		o_ptr += 2;
+	}
+}
+
+void resample_sinc_ft_float(resample_t * r)
+{
+	double *ptr;
+	float *o_ptr;
+	int i;
+	/*int j; */
+	double c0, c1;
+	/*double a; */
+	double start_f, start_x;
+	int start;
+	double center;
+	/*double weight; */
+	double x, d;
+	double scale;
+	int n = 4;
+
+	scale = r->i_inc;	/* cutoff at 22050 */
+	/*scale = 1.0;		// cutoff at 24000 */
+	/*scale = r->i_inc * 0.5;	// cutoff at 11025 */
+
+	if(!ft){
+		ft = malloc(sizeof(*ft));
+		memset(ft,0,sizeof(*ft));
+
+		ft->len = (r->filter_length + 2) * n;
+		ft->offset = 1.0 / n;
+		ft->start = - ft->len * 0.5 * ft->offset;
+
+		ft->func_x = functable_sinc;
+		ft->func_dx = functable_dsinc;
+		ft->scale = M_PI * scale;
+
+		ft->func2_x = functable_window_std;
+		ft->func2_dx = functable_window_dstd;
+		ft->scale2 = 1.0 / r->halftaps;
+	
+		functable_init(ft);
+
+		/*printf("len=%d offset=%g start=%g\n",ft->len,ft->offset,ft->start); */
+	}
+
+	ptr = r->buffer;
+	o_ptr = (float *) r->o_buf;
+
+	center = r->o_start;
+	start_x = center - r->halftaps;
+	start_f = floor(start_x);
+	start_x -= start_f;
+	start = start_f;
+	for (i = 0; i < r->o_samples; i++) {
+		/*start_f = floor(center - r->halftaps); */
+/*printf("%d: a=%g start=%d end=%d\n",i,a,start,start+r->filter_length-1); */
+		x = start_f - center;
+		d = 1;
+		c0 = 0;
+		c1 = 0;
+/*#define slow */
+#ifdef slow
+		for (j = 0; j < r->filter_length; j++) {
+			weight = functable_eval(ft,x)*scale;
+			/*weight = sinc(M_PI * scale * x)*scale*r->i_inc; */
+			/*weight *= window_func(x / r->halftaps); */
+			c0 += weight * ptr[(start + j + r->filter_length)*2 + 0];
+			c1 += weight * ptr[(start + j + r->filter_length)*2 + 1];
+			x += d;
+		}
+#else
+		functable_fir2(ft,
+			&c0,&c1,
+			x, n,
+			ptr+(start + r->filter_length)*2,
+			r->filter_length);
+		c0 *= scale;
+		c1 *= scale;
+#endif
+
+		out_tmp[2 * i + 0] = c0;
+		out_tmp[2 * i + 1] = c1;
+		center += r->o_inc;
+		start_x += r->o_inc;
+		while(start_x>=1.0){
+			start_f++;
+			start_x -= 1.0;
+			start++;
+		}
+	}
+
+	if(r->channels==2){
+		conv_float_double(r->o_buf,out_tmp,2 * r->o_samples);
+	}else{
+		conv_float_double_sstr(r->o_buf,out_tmp,r->o_samples,2 * sizeof(double));
+	}
+}
+
 static gboolean
 plugin_init (GModule *module, GstPlugin *plugin)
 {