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