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