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