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