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