summaryrefslogtreecommitdiffstats
path: root/gst-libs/gst/resample/resample.c
diff options
context:
space:
mode:
Diffstat (limited to 'gst-libs/gst/resample/resample.c')
-rw-r--r--gst-libs/gst/resample/resample.c530
1 files changed, 530 insertions, 0 deletions
diff --git a/gst-libs/gst/resample/resample.c b/gst-libs/gst/resample/resample.c
new file mode 100644
index 00000000..cedb874e
--- /dev/null
+++ b/gst-libs/gst/resample/resample.c
@@ -0,0 +1,530 @@
+/* Resampling library
+ * Copyright (C) <2001> David A. Schleef <ds@schleef.org>
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Library General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Library General Public License for more details.
+ *
+ * You should have received a copy of the GNU Library General Public
+ * License along with this library; if not, write to the
+ * Free Software Foundation, Inc., 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+
+#include <string.h>
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <resample.h>
+
+inline double sinc(double x)
+{
+ if(x==0)return 1;
+ return sin(x) / x;
+}
+
+inline double window_func(double x)
+{
+ x = 1 - x*x;
+ return x*x;
+}
+
+signed short double_to_s16(double x)
+{
+ if(x<-32768){
+ printf("clipped\n");
+ return -32768;
+ }
+ if(x>32767){
+ printf("clipped\n");
+ return -32767;
+ }
+ return rint(x);
+}
+
+signed short double_to_s16_ppcasm(double x)
+{
+ if(x<-32768){
+ return -32768;
+ }
+ if(x>32767){
+ return -32767;
+ }
+ return rint(x);
+}
+
+static void resample_sinc_ft(resample_t * r);
+
+void resample_init(resample_t * r)
+{
+ r->i_start = 0;
+ if(r->filter_length&1){
+ r->o_start = 0;
+ }else{
+ r->o_start = r->o_inc * 0.5;
+ }
+
+ memset(r->acc, 0, sizeof(r->acc));
+
+ resample_reinit(r);
+}
+
+void resample_reinit(resample_t * r)
+{
+ /* i_inc is the number of samples that the output increments for
+ * each input sample. o_inc is the opposite. */
+ r->i_inc = (double) r->o_rate / r->i_rate;
+ r->o_inc = (double) r->i_rate / r->o_rate;
+
+ 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;
+ }
+}
+
+/*
+ * Prepare to be confused.
+ *
+ * We keep a "timebase" that is based on output samples. The zero
+ * of the timebase cooresponds to the next output sample that will
+ * be written.
+ *
+ * i_start is the "time" that corresponds to the first input sample
+ * in an incoming buffer. Since the output depends on input samples
+ * ahead in time, i_start will tend to be around halftaps.
+ *
+ * i_start_buf is the time of the first sample in the temporary
+ * buffer.
+ */
+void resample_scale(resample_t * r, void *i_buf, unsigned int i_size)
+{
+ int o_size;
+
+ r->i_buf = i_buf;
+
+ r->i_samples = i_size / 2 / r->channels;
+
+ r->i_start_buf = r->i_start - r->filter_length * r->i_inc;
+
+ /* i_start is the offset (in a given output sample) that is the
+ * beginning of the current input buffer */
+ r->i_end = r->i_start + r->i_inc * r->i_samples;
+
+ r->o_samples = floor(r->i_end - r->halftaps * r->i_inc);
+
+ o_size = r->o_samples * r->channels * 2;
+ r->o_buf = r->get_buffer(r->priv, o_size);
+
+ if(r->verbose){
+ printf("resample_scale: i_buf=%p i_size=%d\n",
+ i_buf,i_size);
+ printf("resample_scale: i_samples=%d o_samples=%d i_inc=%g o_buf=%p\n",
+ r->i_samples, r->o_samples, r->i_inc, r->o_buf);
+ printf("resample_scale: i_start=%g i_end=%g o_start=%g\n",
+ r->i_start, r->i_end, r->o_start);
+ }
+
+ if ((r->filter_length + r->i_samples)*2*2 > r->buffer_len) {
+ int size = (r->filter_length + r->i_samples) * sizeof(double) * 2;
+
+ if(r->verbose){
+ printf("resample temp buffer size=%d\n",size);
+ }
+ if(r->buffer)free(r->buffer);
+ r->buffer_len = size;
+ r->buffer = malloc(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);
+ }
+
+ r->scale(r);
+
+ memcpy(r->buffer,
+ r->buffer + r->i_samples * sizeof(double) * 2,
+ r->filter_length * sizeof(double) * 2);
+
+ /* updating times */
+ r->i_start += r->i_samples * r->i_inc;
+ r->o_start += r->o_samples * r->o_inc - r->i_samples;
+
+ /* adjusting timebase zero */
+ r->i_start -= r->o_samples;
+}
+
+void resample_nearest(resample_t * r)
+{
+ signed short *i_ptr, *o_ptr;
+ int i_count = 0;
+ double a;
+ int i;
+
+ i_ptr = (signed short *) r->i_buf;
+ o_ptr = (signed short *) 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(resample_t * r)
+{
+ signed short *i_ptr, *o_ptr;
+ int o_count = 0;
+ double b;
+ int i;
+ double acc0, acc1;
+
+ i_ptr = (signed short *) r->i_buf;
+ o_ptr = (signed short *) 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] = rint(acc0);
+ //printf("out %d\n",o_ptr[0]);
+ o_ptr[1] = rint(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(resample_t * r)
+{
+ signed short *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 * 2 * r->channels;
+
+ printf("resample temp buffer\n");
+ r->buffer = malloc(size);
+ memset(r->buffer, 0, size);
+ }
+
+ i_ptr = (signed short *) r->i_buf;
+ o_ptr = (signed short *) r->o_buf;
+
+ a = r->i_start;
+#define GETBUF(index,chan) (((index)<0) \
+ ? ((short *)(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] = rint(c0);
+ o_ptr[1] = rint(c1);
+ o_ptr += 2;
+ a += r->o_inc;
+ }
+ }
+
+ 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)
+{
+ double *ptr;
+ signed short *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 = (signed short *) 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] = double_to_s16(c0);
+ o_ptr[1] = double_to_s16(c1);
+ o_ptr += 2;
+ }
+}
+
+
+
+
+/*
+ * Resampling audio is best done using a sinc() filter.
+ *
+ *
+ * out[t] = Sum( in[t'] * sinc((t-t')/delta_t), all t')
+ *
+ * The immediate problem with this algorithm is that it involves a
+ * sum over an infinite number of input samples, both in the past
+ * and future. Note that even though sinc(x) is bounded by 1/x,
+ * and thus decays to 0 for large x, since sum(x,{x=0,1..,n}) diverges
+ * as log(n), we need to be careful about convergence. This is
+ * typically done by using a windowing function, which also makes
+ * the sum over a finite number of input samples.
+ *
+ * The next problem is computational: sinc(), and especially
+ * sinc() multiplied by a non-trivial windowing function is expensive
+ * to calculate, and also difficult to find SIMD optimizations. Since
+ * the time increment on input and output is different, it is not
+ * possible to use a FIR filter, because the taps would have to be
+ * recalculated for every t.
+ *
+ * To get around the expense of calculating sinc() for every point,
+ * we pre-calculate sinc() at a number of points, and then interpolate
+ * for the values we want in calculations. The interpolation method
+ * chosen is bi-cubic, which requires both the evalated function and
+ * its derivative at every pre-sampled point. Also, if the sampled
+ * points are spaced commensurate with the input delta_t, we notice
+ * that the interpolating weights are the same for every input point.
+ * This decreases the number of operations to 4 multiplies and 4 adds
+ * for each tap, regardless of the complexity of the filtering function.
+ *
+ * At this point, it is possible to rearrange the problem as the sum
+ * of 4 properly weghted FIR filters. Typical SIMD computation units
+ * are highly optimized for FIR filters, making long filter lengths
+ * reasonable.
+ */
+
+static functable_t *ft;
+
+double out_tmp[10000];
+
+static void resample_sinc_ft(resample_t * r)
+{
+ double *ptr;
+ signed short *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 = (signed short *) 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_short_double(r->o_buf,out_tmp,2 * r->o_samples);
+ }else{
+ conv_short_double_sstr(r->o_buf,out_tmp,r->o_samples,2 * sizeof(double));
+ }
+}
+
generated by cgit v1.2.1 (git 2.18.0) at 2024-10-31 07:58:23 +0000