interleaved access, fixed short input buf.
[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 #include "misc.h"
39 #include <math.h>
40 #include <stdio.h>
41             
42 /*#define float double*/
43 #define FILTER_SIZE 64
44 #define OVERSAMPLE 8
45
46 typedef enum {SPEEX_RESAMPLER_DIRECT=0, SPEEX_RESAMPLER_INTERPOLATE=1} SpeexSincType;
47
48 typedef struct {
49    int    in_rate;
50    int    out_rate;
51    int    num_rate;
52    int    den_rate;
53    
54    int    nb_channels;
55    int    last_sample;
56    int    samp_frac_num;
57    int    filt_len;
58    
59    float *mem;
60    float *sinc_table;
61    int    sinc_table_length;
62    int    in_stride;
63    int    out_stride;
64    SpeexSincType type;
65 } SpeexResamplerState;
66
67
68 /* The slow way of computing a sinc for the table. Should improve that some day */
69 static float sinc(float x, int N)
70 {
71    /*fprintf (stderr, "%f ", x);*/
72    if (fabs(x)<1e-6)
73       return 1;
74    else if (fabs(x) > .5f*N)
75       return 0;
76    /*FIXME: Can it really be any slower than this? */
77    return sin(M_PI*x)/(M_PI*x) * (.5+.5*cos(2*x*M_PI/N));
78 }
79
80 void speex_resample_set_rate(SpeexResamplerState *st, int in_rate, int out_rate, int in_rate_den, int out_rate_den);
81
82 SpeexResamplerState *speex_resampler_init(int nb_channels, int in_rate, int out_rate, int in_rate_den, int out_rate_den)
83 {
84    int i;
85    SpeexResamplerState *st = (SpeexResamplerState *)speex_alloc(sizeof(SpeexResamplerState));
86    st->in_rate = 0;
87    st->out_rate = 0;
88    st->num_rate = 0;
89    st->den_rate = 0;
90    
91    st->nb_channels = nb_channels;
92    st->last_sample = 0;
93    st->filt_len = FILTER_SIZE;
94    st->mem = (float*)speex_alloc(nb_channels*(st->filt_len-1) * sizeof(float));
95    for (i=0;i<nb_channels*(st->filt_len-1);i++)
96       st->mem[i] = 0;
97    st->sinc_table_length = 0;
98    
99    speex_resample_set_rate(st, in_rate, out_rate, in_rate_den, out_rate_den);
100
101    st->in_stride = 1;
102    st->out_stride = 1;
103    return st;
104 }
105
106 void speex_resampler_destroy(SpeexResamplerState *st)
107 {
108    speex_free(st->mem);
109    speex_free(st->sinc_table);
110    speex_free(st);
111 }
112
113 void speex_resampler_process_float(SpeexResamplerState *st, int channel_index, const float *in, int *in_len, float *out, int *out_len)
114 {
115    int j=0;
116    int N = st->filt_len;
117    int out_sample = 0;
118    float *mem;
119    mem = st->mem + channel_index * (N-1);
120    while (!(st->last_sample >= *in_len || out_sample >= *out_len))
121    {
122       int j;
123       float sum=0;
124       
125       if (st->type == SPEEX_RESAMPLER_DIRECT)
126       {
127          /* We already have all the filter coefficients pre-computed in the table */
128          const float *ptr;
129          /* Do the memory part */
130          for (j=0;st->last_sample-N+1+j < 0;j++)
131          {
132             sum += mem[st->last_sample+j]*st->sinc_table[st->samp_frac_num*st->filt_len+j];
133          }
134          
135          /* Do the new part */
136          ptr = in+st->last_sample-N+1+j;
137          for (;j<N;j++)
138          {
139             sum += *ptr*st->sinc_table[st->samp_frac_num*st->filt_len+j];
140             ptr += st->in_stride;
141          }
142       } else {
143          /* We need to interpolate the sinc filter */
144          float accum[4] = {0.f,0.f, 0.f, 0.f};
145          float interp[4];
146          const float *ptr;
147          float alpha = ((float)st->samp_frac_num)/st->den_rate;
148          int offset = st->samp_frac_num*OVERSAMPLE/st->den_rate;
149          float frac = alpha*OVERSAMPLE - offset;
150          /* This code is written like this to make it easy to optimise with SIMD.
151             For most DSPs, it would be best to split the loops in two because most DSPs 
152             have only two accumulators */
153          for (j=0;st->last_sample-N+1+j < 0;j++)
154          {
155             float curr_mem = mem[st->last_sample+j];
156             accum[0] += curr_mem*st->sinc_table[4+(j+1)*OVERSAMPLE-offset-2];
157             accum[1] += curr_mem*st->sinc_table[4+(j+1)*OVERSAMPLE-offset-1];
158             accum[2] += curr_mem*st->sinc_table[4+(j+1)*OVERSAMPLE-offset];
159             accum[3] += curr_mem*st->sinc_table[4+(j+1)*OVERSAMPLE-offset+1];
160          }
161          ptr = in+st->last_sample-N+1+j;
162          /* Do the new part */
163          for (;j<N;j++)
164          {
165             float curr_in = *ptr;
166             ptr += st->in_stride;
167             accum[0] += curr_in*st->sinc_table[4+(j+1)*OVERSAMPLE-offset-2];
168             accum[1] += curr_in*st->sinc_table[4+(j+1)*OVERSAMPLE-offset-1];
169             accum[2] += curr_in*st->sinc_table[4+(j+1)*OVERSAMPLE-offset];
170             accum[3] += curr_in*st->sinc_table[4+(j+1)*OVERSAMPLE-offset+1];
171          }
172          /* Compute interpolation coefficients. I'm not sure whether this corresponds to cubic interpolation
173             but I know it's MMSE-optimal on a sinc */
174          interp[0] =  -0.16667f*frac + 0.16667f*frac*frac*frac;
175          interp[1] = frac + 0.5f*frac*frac - 0.5f*frac*frac*frac;
176          interp[2] = 1.f - 0.5f*frac - frac*frac + 0.5f*frac*frac*frac;
177          interp[3] = -0.33333f*frac + 0.5f*frac*frac - 0.16667f*frac*frac*frac;
178          /*sum = frac*accum[1] + (1-frac)*accum[2];*/
179          sum = interp[0]*accum[0] + interp[1]*accum[1] + interp[2]*accum[2] + interp[3]*accum[3];
180       }
181       *out = sum;
182       out += st->out_stride;
183       out_sample++;
184       st->last_sample += st->num_rate/st->den_rate;
185       st->samp_frac_num += st->num_rate%st->den_rate;
186       if (st->samp_frac_num >= st->den_rate)
187       {
188          st->samp_frac_num -= st->den_rate;
189          st->last_sample++;
190       }
191    }
192    if (st->last_sample < *in_len)
193       *in_len = st->last_sample;
194    *out_len = out_sample;
195    st->last_sample -= *in_len;
196    
197    for (j=0;j<N-1-*in_len;j++)
198       mem[j] = mem[j+*in_len];
199    for (;j<N-1;j++)
200       mem[j] = in[st->in_stride*(j+*in_len-N+1)];
201    
202 }
203
204 void speex_resampler_process_interleaved_float(SpeexResamplerState *st, const float *in, int *in_len, float *out, int *out_len)
205 {
206    int i;
207    int istride_save, ostride_save;
208    istride_save = st->in_stride;
209    ostride_save = st->out_stride;
210    st->in_stride = st->out_stride = st->nb_channels;
211    for (i=0;i<st->nb_channels;i++)
212    {
213       speex_resampler_process_float(st, i, in+i, in_len, out+i, out_len);
214    }
215    st->in_stride = istride_save;
216    st->out_stride = ostride_save;
217 }
218
219 void speex_resample_set_rate(SpeexResamplerState *st, int in_rate, int out_rate, int in_rate_den, int out_rate_den)
220 {
221    float cutoff;
222    int i, fact;
223    if (st->in_rate == in_rate && st->out_rate == out_rate && st->num_rate == in_rate && st->den_rate == out_rate)
224       return;
225    
226    st->in_rate = in_rate;
227    st->out_rate = out_rate;
228    st->num_rate = in_rate;
229    st->den_rate = out_rate;
230    /* FIXME: This is terribly inefficient, but who cares (at least for now)? */
231    for (fact=2;fact<=sqrt(MAX32(in_rate, out_rate));fact++)
232    {
233       while ((st->num_rate % fact == 0) && (st->den_rate % fact == 0))
234       {
235          st->num_rate /= fact;
236          st->den_rate /= fact;
237       }
238    }
239    
240    /* FIXME: Is there a danger of overflow? */
241    if (in_rate*out_rate_den > out_rate*in_rate_den)
242    {
243       /* down-sampling */
244       cutoff = .92f * out_rate*in_rate_den / (in_rate*out_rate_den);
245    } else {
246       /* up-sampling */
247       cutoff = .97;
248    }
249    
250    /* Choose the resampling type that requires the least amount of memory */
251    if (st->den_rate <= OVERSAMPLE)
252    {
253       if (!st->sinc_table)
254          st->sinc_table = (float *)speex_alloc(st->filt_len*st->den_rate*sizeof(float));
255       else if (st->sinc_table_length < st->filt_len*st->den_rate)
256       {
257          st->sinc_table = (float *)speex_realloc(st->sinc_table,st->filt_len*st->den_rate*sizeof(float));
258          st->sinc_table_length = st->filt_len*st->den_rate;
259       }
260       for (i=0;i<st->den_rate;i++)
261       {
262          int j;
263          for (j=0;j<st->filt_len;j++)
264          {
265             st->sinc_table[i*st->filt_len+j] = sinc(cutoff*((j-st->filt_len/2+1)-((float)i)/st->den_rate), st->filt_len);
266          }
267       }
268       st->type = SPEEX_RESAMPLER_DIRECT;
269       fprintf (stderr, "resampler uses direct sinc table and normalised cutoff %f\n", cutoff);
270    } else {
271       if (!st->sinc_table)
272          st->sinc_table = (float *)speex_alloc((st->filt_len*OVERSAMPLE+8)*sizeof(float));
273       else if (st->sinc_table_length < st->filt_len*OVERSAMPLE+8)
274       {
275          st->sinc_table = (float *)speex_realloc(st->sinc_table,(st->filt_len*OVERSAMPLE+8)*sizeof(float));
276          st->sinc_table_length = st->filt_len*OVERSAMPLE+8;
277       }
278       for (i=-4;i<OVERSAMPLE*st->filt_len+4;i++)
279          st->sinc_table[i+4] = sinc(cutoff*(i/(float)OVERSAMPLE - st->filt_len/2), st->filt_len);
280       st->type = SPEEX_RESAMPLER_INTERPOLATE;
281       fprintf (stderr, "resampler uses interpolated sinc table and normalised cutoff %f\n", cutoff);
282    }
283
284 }
285
286 void speex_resample_set_input_stride(SpeexResamplerState *st, int stride)
287 {
288    st->in_stride = stride;
289 }
290
291 void speex_resample_set_output_stride(SpeexResamplerState *st, int stride)
292 {
293    st->out_stride = stride;
294 }
295
296 void speex_resample_skip_zeros(SpeexResamplerState *st)
297 {
298    st->last_sample = st->filt_len/2;
299 }
300
301 void speex_resample_reset_mem(SpeexResamplerState *st)
302 {
303    int i;
304    for (i=0;i<st->nb_channels*(st->filt_len-1);i++)
305       st->mem[i] = 0;
306 }
307
308 #define NN 256
309
310 int main(int argc, char **argv)
311 {
312    int i;
313    short *in;
314    short *out;
315    float *fin, *fout;
316    SpeexResamplerState *st = speex_resampler_init(1, 8000, 12000, 1, 1);
317    speex_resample_set_rate(st, 8000, 16000, 1, 1);
318    speex_resample_skip_zeros(st);
319    
320    in = speex_alloc(NN*sizeof(short));
321    out = speex_alloc(2*NN*sizeof(short));
322    fin = speex_alloc(NN*sizeof(float));
323    fout = speex_alloc(2*NN*sizeof(float));
324    while (1)
325    {
326       int in_len;
327       int out_len;
328       fread(in, sizeof(short), NN, stdin);
329       if (feof(stdin))
330          break;
331       for (i=0;i<NN;i++)
332          fin[i]=in[i];
333       in_len = NN;
334       out_len = 2*NN;
335       speex_resampler_process_float(st, 0, fin, &in_len, fout, &out_len);
336       for (i=0;i<out_len;i++)
337          out[i]=floor(.5+fout[i]);
338       fwrite(out, sizeof(short), out_len, stdout);
339    }
340    speex_resampler_destroy(st);
341    speex_free(in);
342    speex_free(out);
343    speex_free(fin);
344    speex_free(fout);
345    return 0;
346 }
347