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