fixed stupid fixed-point bug. Also defined WORD2INT()
[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 #ifdef FIXED_POINT
56 #define WORD2INT(x) ((x) < -32767 ? -32768 : ((x) > 32766 ? 32767 : (x)))  
57 #else
58 #define WORD2INT(x) ((x) < -32767.5f ? -32768 : ((x) > 32766.5f ? 32767 : floor(.5+(x))))  
59 #endif
60                
61 /*#define float double*/
62 #define FILTER_SIZE 64
63 #define OVERSAMPLE 8
64
65 #define IMAX(a,b) ((a) > (b) ? (a) : (b))
66
67 typedef enum {SPEEX_RESAMPLER_DIRECT=0, SPEEX_RESAMPLER_INTERPOLATE=1} SpeexSincType;
68
69 struct SpeexResamplerState_ {
70    int    in_rate;
71    int    out_rate;
72    int    num_rate;
73    int    den_rate;
74    
75    int    nb_channels;
76    int    last_sample;
77    int    samp_frac_num;
78    int    filt_len;
79    int    int_advance;
80    int    frac_advance;
81    
82    spx_word16_t *mem;
83    spx_word16_t *sinc_table;
84    int    sinc_table_length;
85    int    in_stride;
86    int    out_stride;
87    SpeexSincType type;
88 } ;
89
90 #ifdef FIXED_POINT
91 /* The slow way of computing a sinc for the table. Should improve that some day */
92 static spx_word16_t sinc(float cutoff, float x, int N)
93 {
94    /*fprintf (stderr, "%f ", x);*/
95    x *= cutoff;
96    if (fabs(x)<1e-6f)
97       return WORD2INT(32768.f*cutoff);
98    else if (fabs(x) > .5f*N)
99       return 0;
100    /*FIXME: Can it really be any slower than this? */
101    return WORD2INT(32768.f*cutoff*sin(M_PI*x)/(M_PI*x) * (.5f+.5f*cos(2*x*M_PI/N)));
102 }
103 #else
104 /* The slow way of computing a sinc for the table. Should improve that some day */
105 static spx_word16_t sinc(float cutoff, float x, int N)
106 {
107    /*fprintf (stderr, "%f ", x);*/
108    x *= cutoff;
109    if (fabs(x)<1e-6)
110       return cutoff;
111    else if (fabs(x) > .5f*N)
112       return 0;
113    /*FIXME: Can it really be any slower than this? */
114    return cutoff*sin(M_PI*x)/(M_PI*x) * (.5+.5*cos(2*x*M_PI/N));
115 }
116 #endif
117
118 SpeexResamplerState *speex_resampler_init(int nb_channels, int in_rate, int out_rate, int in_rate_den, int out_rate_den)
119 {
120    int i;
121    SpeexResamplerState *st = (SpeexResamplerState *)speex_alloc(sizeof(SpeexResamplerState));
122    st->in_rate = 0;
123    st->out_rate = 0;
124    st->num_rate = 0;
125    st->den_rate = 0;
126    
127    st->nb_channels = nb_channels;
128    st->last_sample = 0;
129    st->filt_len = FILTER_SIZE;
130    st->mem = (spx_word16_t*)speex_alloc(nb_channels*(st->filt_len-1) * sizeof(spx_word16_t));
131    for (i=0;i<nb_channels*(st->filt_len-1);i++)
132       st->mem[i] = 0;
133    st->sinc_table_length = 0;
134    
135    speex_resample_set_rate(st, in_rate, out_rate, in_rate_den, out_rate_den);
136
137    st->in_stride = 1;
138    st->out_stride = 1;
139    return st;
140 }
141
142 void speex_resampler_destroy(SpeexResamplerState *st)
143 {
144    speex_free(st->mem);
145    speex_free(st->sinc_table);
146    speex_free(st);
147 }
148
149 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)
150 {
151    int j=0;
152    int N = st->filt_len;
153    int out_sample = 0;
154    spx_word16_t *mem;
155    mem = st->mem + channel_index * (N-1);
156    while (!(st->last_sample >= *in_len || out_sample >= *out_len))
157    {
158       int j;
159       spx_word32_t sum=0;
160       
161       if (st->type == SPEEX_RESAMPLER_DIRECT)
162       {
163          /* We already have all the filter coefficients pre-computed in the table */
164          const spx_word16_t *ptr;
165          /* Do the memory part */
166          for (j=0;st->last_sample-N+1+j < 0;j++)
167          {
168             sum += MULT16_16(mem[st->last_sample+j],st->sinc_table[st->samp_frac_num*st->filt_len+j]);
169          }
170          
171          /* Do the new part */
172          ptr = in+st->last_sample-N+1+j;
173          for (;j<N;j++)
174          {
175             sum += MULT16_16(*ptr,st->sinc_table[st->samp_frac_num*st->filt_len+j]);
176             ptr += st->in_stride;
177          }
178       } else {
179          /* We need to interpolate the sinc filter */
180          spx_word32_t accum[4] = {0.f,0.f, 0.f, 0.f};
181          float interp[4];
182          const spx_word16_t *ptr;
183          float alpha = ((float)st->samp_frac_num)/st->den_rate;
184          int offset = st->samp_frac_num*OVERSAMPLE/st->den_rate;
185          float frac = alpha*OVERSAMPLE - offset;
186          /* This code is written like this to make it easy to optimise with SIMD.
187             For most DSPs, it would be best to split the loops in two because most DSPs 
188             have only two accumulators */
189          for (j=0;st->last_sample-N+1+j < 0;j++)
190          {
191             spx_word16_t curr_mem = mem[st->last_sample+j];
192             accum[0] += MULT16_16(curr_mem,st->sinc_table[4+(j+1)*OVERSAMPLE-offset-2]);
193             accum[1] += MULT16_16(curr_mem,st->sinc_table[4+(j+1)*OVERSAMPLE-offset-1]);
194             accum[2] += MULT16_16(curr_mem,st->sinc_table[4+(j+1)*OVERSAMPLE-offset]);
195             accum[3] += MULT16_16(curr_mem,st->sinc_table[4+(j+1)*OVERSAMPLE-offset+1]);
196          }
197          ptr = in+st->last_sample-N+1+j;
198          /* Do the new part */
199          for (;j<N;j++)
200          {
201             spx_word16_t curr_in = *ptr;
202             ptr += st->in_stride;
203             accum[0] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*OVERSAMPLE-offset-2]);
204             accum[1] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*OVERSAMPLE-offset-1]);
205             accum[2] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*OVERSAMPLE-offset]);
206             accum[3] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*OVERSAMPLE-offset+1]);
207          }
208          /* Compute interpolation coefficients. I'm not sure whether this corresponds to cubic interpolation
209             but I know it's MMSE-optimal on a sinc */
210          interp[0] =  -0.16667f*frac + 0.16667f*frac*frac*frac;
211          interp[1] = frac + 0.5f*frac*frac - 0.5f*frac*frac*frac;
212          /*interp[2] = 1.f - 0.5f*frac - frac*frac + 0.5f*frac*frac*frac;*/
213          interp[3] = -0.33333f*frac + 0.5f*frac*frac - 0.16667f*frac*frac*frac;
214          /* Just to make sure we don't have rounding problems */
215          interp[2] = 1.f-interp[0]-interp[1]-interp[3];
216          /*sum = frac*accum[1] + (1-frac)*accum[2];*/
217          sum = interp[0]*accum[0] + interp[1]*accum[1] + interp[2]*accum[2] + interp[3]*accum[3];
218       }
219       *out = PSHR32(sum,15);
220       out += st->out_stride;
221       out_sample++;
222       st->last_sample += st->int_advance;
223       st->samp_frac_num += st->frac_advance;
224       if (st->samp_frac_num >= st->den_rate)
225       {
226          st->samp_frac_num -= st->den_rate;
227          st->last_sample++;
228       }
229    }
230    if (st->last_sample < *in_len)
231       *in_len = st->last_sample;
232    *out_len = out_sample;
233    st->last_sample -= *in_len;
234    
235    for (j=0;j<N-1-*in_len;j++)
236       mem[j] = mem[j+*in_len];
237    for (;j<N-1;j++)
238       mem[j] = in[st->in_stride*(j+*in_len-N+1)];
239    
240 }
241
242 #ifdef FIXED_POINT
243 void speex_resampler_process_float(SpeexResamplerState *st, int channel_index, const float *in, int *in_len, float *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<*in_len;i++)
249       x[i] = WORD2INT(in[i]);
250    speex_resampler_process_native(st, channel_index, x, in_len, y, out_len);
251    for (i=0;i<*out_len;i++)
252       out[i] = y[i];
253
254 }
255 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)
256 {
257    speex_resampler_process_native(st, channel_index, in, in_len, out, out_len);
258 }
259 #else
260 void speex_resampler_process_float(SpeexResamplerState *st, int channel_index, const float *in, int *in_len, float *out, int *out_len)
261 {
262    speex_resampler_process_native(st, channel_index, in, in_len, out, out_len);
263 }
264 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)
265 {
266    int i;
267    spx_word16_t x[*in_len];
268    spx_word16_t y[*out_len];
269    for (i=0;i<*out_len;i++)
270       x[i] = in[i];
271    speex_resampler_process_native(st, channel_index, x, in_len, y, out_len);
272    for (i=0;i<*in_len;i++)
273       out[i] = WORD2INT(y[i]);
274 }
275 #endif
276
277 void speex_resampler_process_interleaved_float(SpeexResamplerState *st, const float *in, int *in_len, float *out, int *out_len)
278 {
279    int i;
280    int istride_save, ostride_save;
281    istride_save = st->in_stride;
282    ostride_save = st->out_stride;
283    st->in_stride = st->out_stride = st->nb_channels;
284    for (i=0;i<st->nb_channels;i++)
285    {
286       speex_resampler_process_float(st, i, in+i, in_len, out+i, out_len);
287    }
288    st->in_stride = istride_save;
289    st->out_stride = ostride_save;
290 }
291
292 void speex_resample_set_rate(SpeexResamplerState *st, int in_rate, int out_rate, int in_rate_den, int out_rate_den)
293 {
294    float cutoff;
295    int i, fact;
296    if (st->in_rate == in_rate && st->out_rate == out_rate && st->num_rate == in_rate && st->den_rate == out_rate)
297       return;
298    
299    st->in_rate = in_rate;
300    st->out_rate = out_rate;
301    st->num_rate = in_rate;
302    st->den_rate = out_rate;
303    /* FIXME: This is terribly inefficient, but who cares (at least for now)? */
304    for (fact=2;fact<=sqrt(IMAX(in_rate, out_rate));fact++)
305    {
306       while ((st->num_rate % fact == 0) && (st->den_rate % fact == 0))
307       {
308          st->num_rate /= fact;
309          st->den_rate /= fact;
310       }
311    }
312    
313    /* FIXME: Is there a danger of overflow? */
314    if (in_rate*out_rate_den > out_rate*in_rate_den)
315    {
316       /* down-sampling */
317       cutoff = .92f * out_rate*in_rate_den / (in_rate*out_rate_den);
318    } else {
319       /* up-sampling */
320       cutoff = .97f;
321    }
322    
323    /* Choose the resampling type that requires the least amount of memory */
324    if (st->den_rate <= OVERSAMPLE)
325    {
326       if (!st->sinc_table)
327          st->sinc_table = (spx_word16_t *)speex_alloc(st->filt_len*st->den_rate*sizeof(spx_word16_t));
328       else if (st->sinc_table_length < st->filt_len*st->den_rate)
329       {
330          st->sinc_table = (spx_word16_t *)speex_realloc(st->sinc_table,st->filt_len*st->den_rate*sizeof(spx_word16_t));
331          st->sinc_table_length = st->filt_len*st->den_rate;
332       }
333       for (i=0;i<st->den_rate;i++)
334       {
335          int j;
336          for (j=0;j<st->filt_len;j++)
337          {
338             st->sinc_table[i*st->filt_len+j] = sinc(cutoff,((j-st->filt_len/2+1)-((float)i)/st->den_rate), st->filt_len);
339          }
340       }
341       st->type = SPEEX_RESAMPLER_DIRECT;
342       /*fprintf (stderr, "resampler uses direct sinc table and normalised cutoff %f\n", cutoff);*/
343    } else {
344       if (!st->sinc_table)
345          st->sinc_table = (spx_word16_t *)speex_alloc((st->filt_len*OVERSAMPLE+8)*sizeof(spx_word16_t));
346       else if (st->sinc_table_length < st->filt_len*OVERSAMPLE+8)
347       {
348          st->sinc_table = (spx_word16_t *)speex_realloc(st->sinc_table,(st->filt_len*OVERSAMPLE+8)*sizeof(spx_word16_t));
349          st->sinc_table_length = st->filt_len*OVERSAMPLE+8;
350       }
351       for (i=-4;i<OVERSAMPLE*st->filt_len+4;i++)
352          st->sinc_table[i+4] = sinc(cutoff,(i/(float)OVERSAMPLE - st->filt_len/2), st->filt_len);
353       st->type = SPEEX_RESAMPLER_INTERPOLATE;
354       /*fprintf (stderr, "resampler uses interpolated sinc table and normalised cutoff %f\n", cutoff);*/
355    }
356    st->int_advance = st->num_rate/st->den_rate;
357    st->frac_advance = st->num_rate%st->den_rate;
358
359 }
360
361 void speex_resample_set_input_stride(SpeexResamplerState *st, int stride)
362 {
363    st->in_stride = stride;
364 }
365
366 void speex_resample_set_output_stride(SpeexResamplerState *st, int stride)
367 {
368    st->out_stride = stride;
369 }
370
371 void speex_resample_skip_zeros(SpeexResamplerState *st)
372 {
373    st->last_sample = st->filt_len/2;
374 }
375
376 void speex_resample_reset_mem(SpeexResamplerState *st)
377 {
378    int i;
379    for (i=0;i<st->nb_channels*(st->filt_len-1);i++)
380       st->mem[i] = 0;
381 }
382