starting fixed-point version of resampler.
[speexdsp.git] / libspeex / resample.c
1 /* Copyright (C) 2007 Jean-Marc Valin
2       
3    File: resample.c
4    Resampling code
5       
6    The design goals of this code are:
7       - Very fast algorithm
8       - Low memory requirement
9       - Good *perceptual* quality (and not best SNR)
10
11    Redistribution and use in source and binary forms, with or without
12    modification, are permitted provided that the following conditions are
13    met:
14
15    1. Redistributions of source code must retain the above copyright notice,
16    this list of conditions and the following disclaimer.
17
18    2. Redistributions in binary form must reproduce the above copyright
19    notice, this list of conditions and the following disclaimer in the
20    documentation and/or other materials provided with the distribution.
21
22    3. The name of the author may not be used to endorse or promote products
23    derived from this software without specific prior written permission.
24
25    THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
26    IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
27    OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
28    DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT,
29    INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
30    (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
31    SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
32    HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
33    STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35    POSSIBILITY OF SUCH DAMAGE.
36 */
37
38 #ifdef HAVE_CONFIG_H
39 #include "config.h"
40 #endif
41
42 #ifdef OUTSIDE_SPEEX
43 #include <stdlib.h>
44 void *speex_alloc (int size) {return calloc(size,1);}
45 void *speex_realloc (void *ptr, int size) {return realloc(ptr, size);}
46 void speex_free (void *ptr) {free(ptr);}
47 #else
48 #include "misc.h"
49 #endif
50
51
52 #include <math.h>
53 #include "speex/speex_resampler.h"
54
55 /*#define float double*/
56 #define FILTER_SIZE 64
57 #define OVERSAMPLE 8
58
59 #define IMAX(a,b) ((a) > (b) ? (a) : (b))
60
61 typedef enum {SPEEX_RESAMPLER_DIRECT=0, SPEEX_RESAMPLER_INTERPOLATE=1} SpeexSincType;
62
63 struct SpeexResamplerState_ {
64    int    in_rate;
65    int    out_rate;
66    int    num_rate;
67    int    den_rate;
68    
69    int    nb_channels;
70    int    last_sample;
71    int    samp_frac_num;
72    int    filt_len;
73    int    int_advance;
74    int    frac_advance;
75    
76    float *mem;
77    float *sinc_table;
78    int    sinc_table_length;
79    int    in_stride;
80    int    out_stride;
81    SpeexSincType type;
82 } ;
83
84
85 /* The slow way of computing a sinc for the table. Should improve that some day */
86 static float sinc(float x, int N)
87 {
88    /*fprintf (stderr, "%f ", x);*/
89    if (fabs(x)<1e-6)
90       return 1;
91    else if (fabs(x) > .5f*N)
92       return 0;
93    /*FIXME: Can it really be any slower than this? */
94    return sin(M_PI*x)/(M_PI*x) * (.5+.5*cos(2*x*M_PI/N));
95 }
96
97 SpeexResamplerState *speex_resampler_init(int nb_channels, int in_rate, int out_rate, int in_rate_den, int out_rate_den)
98 {
99    int i;
100    SpeexResamplerState *st = (SpeexResamplerState *)speex_alloc(sizeof(SpeexResamplerState));
101    st->in_rate = 0;
102    st->out_rate = 0;
103    st->num_rate = 0;
104    st->den_rate = 0;
105    
106    st->nb_channels = nb_channels;
107    st->last_sample = 0;
108    st->filt_len = FILTER_SIZE;
109    st->mem = (float*)speex_alloc(nb_channels*(st->filt_len-1) * sizeof(float));
110    for (i=0;i<nb_channels*(st->filt_len-1);i++)
111       st->mem[i] = 0;
112    st->sinc_table_length = 0;
113    
114    speex_resample_set_rate(st, in_rate, out_rate, in_rate_den, out_rate_den);
115
116    st->in_stride = 1;
117    st->out_stride = 1;
118    return st;
119 }
120
121 void speex_resampler_destroy(SpeexResamplerState *st)
122 {
123    speex_free(st->mem);
124    speex_free(st->sinc_table);
125    speex_free(st);
126 }
127
128 static void speex_resampler_process_native(SpeexResamplerState *st, int channel_index, const float *in, int *in_len, float *out, int *out_len)
129 {
130    int j=0;
131    int N = st->filt_len;
132    int out_sample = 0;
133    float *mem;
134    mem = st->mem + channel_index * (N-1);
135    while (!(st->last_sample >= *in_len || out_sample >= *out_len))
136    {
137       int j;
138       float sum=0;
139       
140       if (st->type == SPEEX_RESAMPLER_DIRECT)
141       {
142          /* We already have all the filter coefficients pre-computed in the table */
143          const float *ptr;
144          /* Do the memory part */
145          for (j=0;st->last_sample-N+1+j < 0;j++)
146          {
147             sum += mem[st->last_sample+j]*st->sinc_table[st->samp_frac_num*st->filt_len+j];
148          }
149          
150          /* Do the new part */
151          ptr = in+st->last_sample-N+1+j;
152          for (;j<N;j++)
153          {
154             sum += *ptr*st->sinc_table[st->samp_frac_num*st->filt_len+j];
155             ptr += st->in_stride;
156          }
157       } else {
158          /* We need to interpolate the sinc filter */
159          float accum[4] = {0.f,0.f, 0.f, 0.f};
160          float interp[4];
161          const float *ptr;
162          float alpha = ((float)st->samp_frac_num)/st->den_rate;
163          int offset = st->samp_frac_num*OVERSAMPLE/st->den_rate;
164          float frac = alpha*OVERSAMPLE - offset;
165          /* This code is written like this to make it easy to optimise with SIMD.
166             For most DSPs, it would be best to split the loops in two because most DSPs 
167             have only two accumulators */
168          for (j=0;st->last_sample-N+1+j < 0;j++)
169          {
170             float curr_mem = mem[st->last_sample+j];
171             accum[0] += curr_mem*st->sinc_table[4+(j+1)*OVERSAMPLE-offset-2];
172             accum[1] += curr_mem*st->sinc_table[4+(j+1)*OVERSAMPLE-offset-1];
173             accum[2] += curr_mem*st->sinc_table[4+(j+1)*OVERSAMPLE-offset];
174             accum[3] += curr_mem*st->sinc_table[4+(j+1)*OVERSAMPLE-offset+1];
175          }
176          ptr = in+st->last_sample-N+1+j;
177          /* Do the new part */
178          for (;j<N;j++)
179          {
180             float curr_in = *ptr;
181             ptr += st->in_stride;
182             accum[0] += curr_in*st->sinc_table[4+(j+1)*OVERSAMPLE-offset-2];
183             accum[1] += curr_in*st->sinc_table[4+(j+1)*OVERSAMPLE-offset-1];
184             accum[2] += curr_in*st->sinc_table[4+(j+1)*OVERSAMPLE-offset];
185             accum[3] += curr_in*st->sinc_table[4+(j+1)*OVERSAMPLE-offset+1];
186          }
187          /* Compute interpolation coefficients. I'm not sure whether this corresponds to cubic interpolation
188             but I know it's MMSE-optimal on a sinc */
189          interp[0] =  -0.16667f*frac + 0.16667f*frac*frac*frac;
190          interp[1] = frac + 0.5f*frac*frac - 0.5f*frac*frac*frac;
191          /*interp[2] = 1.f - 0.5f*frac - frac*frac + 0.5f*frac*frac*frac;*/
192          interp[3] = -0.33333f*frac + 0.5f*frac*frac - 0.16667f*frac*frac*frac;
193          /* Just to make sure we don't have rounding problems */
194          interp[2] = 1.f-interp[0]-interp[1]-interp[3];
195          /*sum = frac*accum[1] + (1-frac)*accum[2];*/
196          sum = interp[0]*accum[0] + interp[1]*accum[1] + interp[2]*accum[2] + interp[3]*accum[3];
197       }
198       *out = sum;
199       out += st->out_stride;
200       out_sample++;
201       st->last_sample += st->int_advance;
202       st->samp_frac_num += st->frac_advance;
203       if (st->samp_frac_num >= st->den_rate)
204       {
205          st->samp_frac_num -= st->den_rate;
206          st->last_sample++;
207       }
208    }
209    if (st->last_sample < *in_len)
210       *in_len = st->last_sample;
211    *out_len = out_sample;
212    st->last_sample -= *in_len;
213    
214    for (j=0;j<N-1-*in_len;j++)
215       mem[j] = mem[j+*in_len];
216    for (;j<N-1;j++)
217       mem[j] = in[st->in_stride*(j+*in_len-N+1)];
218    
219 }
220
221 #ifdef FIXED_POINT
222 void speex_resampler_process_float(SpeexResamplerState *st, int channel_index, const float *in, int *in_len, float *out, int *out_len)
223 {
224    int i;
225    spx_word16_t x[*in_len];
226    spx_word16_t y[*out_len];
227    for (i=0;i<*in_len;i++)
228       x[i] = floor(.5+in[i]);
229    speex_resampler_process_native(st, channel_index, x, in_len, y, out_len);
230    for (i=0;i<*out_len;i++)
231       out[i] = y[i];
232
233 }
234 void speex_resampler_process_int(SpeexResamplerState *st, int channel_index, const spx_int16_t *in, int *in_len, spx_int16_t *out, int *out_len)
235 {
236    speex_resampler_process_native(st, channel_index, in, in_len, out, out_len);
237 }
238 #else
239 void speex_resampler_process_float(SpeexResamplerState *st, int channel_index, const float *in, int *in_len, float *out, int *out_len)
240 {
241    speex_resampler_process_native(st, channel_index, in, in_len, out, out_len);
242 }
243 void speex_resampler_process_int(SpeexResamplerState *st, int channel_index, const spx_int16_t *in, int *in_len, spx_int16_t *out, int *out_len)
244 {
245    int i;
246    spx_word16_t x[*in_len];
247    spx_word16_t y[*out_len];
248    for (i=0;i<*out_len;i++)
249       x[i] = in[i];
250    speex_resampler_process_native(st, channel_index, x, in_len, y, out_len);
251    for (i=0;i<*in_len;i++)
252       out[i] = floor(.5+y[i]);
253 }
254 #endif
255
256 void speex_resampler_process_interleaved_float(SpeexResamplerState *st, const float *in, int *in_len, float *out, int *out_len)
257 {
258    int i;
259    int istride_save, ostride_save;
260    istride_save = st->in_stride;
261    ostride_save = st->out_stride;
262    st->in_stride = st->out_stride = st->nb_channels;
263    for (i=0;i<st->nb_channels;i++)
264    {
265       speex_resampler_process_float(st, i, in+i, in_len, out+i, out_len);
266    }
267    st->in_stride = istride_save;
268    st->out_stride = ostride_save;
269 }
270
271 void speex_resample_set_rate(SpeexResamplerState *st, int in_rate, int out_rate, int in_rate_den, int out_rate_den)
272 {
273    float cutoff;
274    int i, fact;
275    if (st->in_rate == in_rate && st->out_rate == out_rate && st->num_rate == in_rate && st->den_rate == out_rate)
276       return;
277    
278    st->in_rate = in_rate;
279    st->out_rate = out_rate;
280    st->num_rate = in_rate;
281    st->den_rate = out_rate;
282    /* FIXME: This is terribly inefficient, but who cares (at least for now)? */
283    for (fact=2;fact<=sqrt(IMAX(in_rate, out_rate));fact++)
284    {
285       while ((st->num_rate % fact == 0) && (st->den_rate % fact == 0))
286       {
287          st->num_rate /= fact;
288          st->den_rate /= fact;
289       }
290    }
291    
292    /* FIXME: Is there a danger of overflow? */
293    if (in_rate*out_rate_den > out_rate*in_rate_den)
294    {
295       /* down-sampling */
296       cutoff = .92f * out_rate*in_rate_den / (in_rate*out_rate_den);
297    } else {
298       /* up-sampling */
299       cutoff = .97;
300    }
301    
302    /* Choose the resampling type that requires the least amount of memory */
303    if (st->den_rate <= OVERSAMPLE)
304    {
305       if (!st->sinc_table)
306          st->sinc_table = (float *)speex_alloc(st->filt_len*st->den_rate*sizeof(float));
307       else if (st->sinc_table_length < st->filt_len*st->den_rate)
308       {
309          st->sinc_table = (float *)speex_realloc(st->sinc_table,st->filt_len*st->den_rate*sizeof(float));
310          st->sinc_table_length = st->filt_len*st->den_rate;
311       }
312       for (i=0;i<st->den_rate;i++)
313       {
314          int j;
315          for (j=0;j<st->filt_len;j++)
316          {
317             st->sinc_table[i*st->filt_len+j] = sinc(cutoff*((j-st->filt_len/2+1)-((float)i)/st->den_rate), st->filt_len);
318          }
319       }
320       st->type = SPEEX_RESAMPLER_DIRECT;
321       /*fprintf (stderr, "resampler uses direct sinc table and normalised cutoff %f\n", cutoff);*/
322    } else {
323       if (!st->sinc_table)
324          st->sinc_table = (float *)speex_alloc((st->filt_len*OVERSAMPLE+8)*sizeof(float));
325       else if (st->sinc_table_length < st->filt_len*OVERSAMPLE+8)
326       {
327          st->sinc_table = (float *)speex_realloc(st->sinc_table,(st->filt_len*OVERSAMPLE+8)*sizeof(float));
328          st->sinc_table_length = st->filt_len*OVERSAMPLE+8;
329       }
330       for (i=-4;i<OVERSAMPLE*st->filt_len+4;i++)
331          st->sinc_table[i+4] = sinc(cutoff*(i/(float)OVERSAMPLE - st->filt_len/2), st->filt_len);
332       st->type = SPEEX_RESAMPLER_INTERPOLATE;
333       /*fprintf (stderr, "resampler uses interpolated sinc table and normalised cutoff %f\n", cutoff);*/
334    }
335    st->int_advance = st->num_rate/st->den_rate;
336    st->frac_advance = st->num_rate%st->den_rate;
337
338 }
339
340 void speex_resample_set_input_stride(SpeexResamplerState *st, int stride)
341 {
342    st->in_stride = stride;
343 }
344
345 void speex_resample_set_output_stride(SpeexResamplerState *st, int stride)
346 {
347    st->out_stride = stride;
348 }
349
350 void speex_resample_skip_zeros(SpeexResamplerState *st)
351 {
352    st->last_sample = st->filt_len/2;
353 }
354
355 void speex_resample_reset_mem(SpeexResamplerState *st)
356 {
357    int i;
358    for (i=0;i<st->nb_channels*(st->filt_len-1);i++)
359       st->mem[i] = 0;
360 }
361