dbcd9433a688ab6d28c4cd939ec046764a37267a
[speexdsp.git] / libspeex / filters.c
1 /* Copyright (C) 2002 Jean-Marc Valin 
2    File: filters.c
3    Various analysis/synthesis filters
4
5    Redistribution and use in source and binary forms, with or without
6    modification, are permitted provided that the following conditions
7    are met:
8    
9    - Redistributions of source code must retain the above copyright
10    notice, this list of conditions and the following disclaimer.
11    
12    - Redistributions in binary form must reproduce the above copyright
13    notice, this list of conditions and the following disclaimer in the
14    documentation and/or other materials provided with the distribution.
15    
16    - Neither the name of the Xiph.org Foundation nor the names of its
17    contributors may be used to endorse or promote products derived from
18    this software without specific prior written permission.
19    
20    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
23    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
24    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
25    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
26    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
27    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
28    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
29    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
30    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31 */
32
33 #include "filters.h"
34 #include "stack_alloc.h"
35 #include <math.h>
36 #include "misc.h"
37 #include "math_approx.h"
38
39
40 #ifdef FIXED_POINT
41 void bw_lpc(spx_word16_t gamma, spx_coef_t *lpc_in, spx_coef_t *lpc_out, int order)
42 {
43    int i;
44    spx_word16_t tmp=gamma;
45    lpc_out[0] = lpc_in[0];
46    for (i=1;i<order+1;i++)
47    {
48       lpc_out[i] = MULT16_16_P15(tmp,lpc_in[i]);
49       tmp = MULT16_16_P15(tmp, gamma);
50    }
51 }
52 #else
53 void bw_lpc(spx_word16_t gamma, spx_coef_t *lpc_in, spx_coef_t *lpc_out, int order)
54 {
55    int i;
56    float tmp=1;
57    for (i=0;i<order+1;i++)
58    {
59       lpc_out[i] = tmp * lpc_in[i];
60       tmp *= gamma;
61    }
62 }
63
64 #endif
65
66
67 #ifdef FIXED_POINT
68
69 /* FIXME: These functions are ugly and probably might too much error */
70 void signal_mul(spx_sig_t *x, spx_sig_t *y, spx_word32_t scale, int len)
71 {
72    int i;
73    for (i=0;i<len;i++)
74    {
75       y[i] = MULT16_32_Q14((x[i]>>4),scale)<<4;
76    }
77 }
78
79 void signal_div(spx_sig_t *x, spx_sig_t *y, spx_word32_t scale, int len)
80 {
81    int i;
82    spx_word16_t scale_1;
83
84    scale = PSHR(scale, SIG_SHIFT);
85    if (scale<2)
86       scale_1 = 32767;
87    else
88       scale_1 = 32767/scale;
89    for (i=0;i<len;i++)
90    {
91       y[i] = MULT16_32_Q15(scale_1,x[i]);
92    }
93 }
94
95 #else
96
97 void signal_mul(spx_sig_t *x, spx_sig_t *y, spx_word32_t scale, int len)
98 {
99    int i;
100    for (i=0;i<len;i++)
101       y[i] = scale*x[i];
102 }
103
104 void signal_div(spx_sig_t *x, spx_sig_t *y, spx_word32_t scale, int len)
105 {
106    int i;
107    float scale_1 = 1/scale;
108    for (i=0;i<len;i++)
109       y[i] = scale_1*x[i];
110 }
111 #endif
112
113
114
115 #ifdef FIXED_POINT
116
117 int normalize16(spx_sig_t *x, spx_word16_t *y, int max_scale, int len)
118 {
119    int i;
120    spx_sig_t max_val=1;
121    int sig_shift;
122    
123    for (i=0;i<len;i++)
124    {
125       spx_sig_t tmp = x[i];
126       if (tmp<0)
127          tmp = -tmp;
128       if (tmp >= max_val)
129          max_val = tmp;
130    }
131
132    sig_shift=0;
133    while (max_val>max_scale)
134    {
135       sig_shift++;
136       max_val >>= 1;
137    }
138
139    for (i=0;i<len;i++)
140       y[i] = SHR(x[i], sig_shift);
141    
142    return sig_shift;
143 }
144
145 spx_word16_t compute_rms(spx_sig_t *x, int len)
146 {
147    int i;
148    spx_word32_t sum=0;
149    spx_sig_t max_val=1;
150    int sig_shift;
151
152    for (i=0;i<len;i++)
153    {
154       spx_sig_t tmp = x[i];
155       if (tmp<0)
156          tmp = -tmp;
157       if (tmp > max_val)
158          max_val = tmp;
159    }
160
161    sig_shift=0;
162    while (max_val>16383)
163    {
164       sig_shift++;
165       max_val >>= 1;
166    }
167
168    for (i=0;i<len;i+=4)
169    {
170       spx_word32_t sum2=0;
171       spx_word16_t tmp;
172       tmp = SHR(x[i],sig_shift);
173       sum2 += MULT16_16(tmp,tmp);
174       tmp = SHR(x[i+1],sig_shift);
175       sum2 += MULT16_16(tmp,tmp);
176       tmp = SHR(x[i+2],sig_shift);
177       sum2 += MULT16_16(tmp,tmp);
178       tmp = SHR(x[i+3],sig_shift);
179       sum2 += MULT16_16(tmp,tmp);
180       sum += SHR(sum2,6);
181    }
182    
183    /*FIXME: remove division*/
184    return SHR(SHL((spx_word32_t)spx_sqrt(1+sum/len),(sig_shift+3)),SIG_SHIFT);
185 }
186
187
188 void filter_mem2(spx_sig_t *x, spx_coef_t *num, spx_coef_t *den, spx_sig_t *y, int N, int ord, spx_mem_t *mem)
189 {
190    int i,j;
191    spx_sig_t xi,yi,nyi;
192
193    for (i=0;i<N;i++)
194    {
195       int xh,xl,yh,yl;
196       xi=SATURATE(x[i],805306368);
197       yi = SATURATE(ADD32(xi, SHL(mem[0],2)),805306368);
198       nyi = -yi;
199       xh = xi>>15; xl=xi&0x00007fff; yh = yi>>15; yl=yi&0x00007fff; 
200       for (j=0;j<ord-1;j++)
201       {
202          mem[j] = MAC16_32_Q15(MAC16_32_Q15(mem[j+1], num[j+1],xi), den[j+1],nyi);
203       }
204       mem[ord-1] = SUB32(MULT16_32_Q15(num[ord],xi), MULT16_32_Q15(den[ord],yi));
205       y[i] = yi;
206    }
207 }
208
209 void iir_mem2(spx_sig_t *x, spx_coef_t *den, spx_sig_t *y, int N, int ord, spx_mem_t *mem)
210 {
211    int i,j;
212    spx_word32_t xi,yi,nyi;
213
214    for (i=0;i<N;i++)
215    {
216       int yh,yl;
217       xi=SATURATE(x[i],805306368);
218       yi = SATURATE(xi + (mem[0]<<2),805306368);
219       nyi = -yi;
220       yh = yi>>15; yl=yi&0x00007fff; 
221       for (j=0;j<ord-1;j++)
222       {
223          mem[j] = MAC16_32_Q15(mem[j+1],den[j+1],nyi);
224       }
225       mem[ord-1] = - MULT16_32_Q15(den[ord],yi);
226       y[i] = yi;
227    }
228 }
229
230
231 void fir_mem2(spx_sig_t *x, spx_coef_t *num, spx_sig_t *y, int N, int ord, spx_mem_t *mem)
232 {
233    int i,j;
234    spx_word32_t xi,yi;
235
236    for (i=0;i<N;i++)
237    {
238       int xh,xl;
239       xi=SATURATE(x[i],805306368);
240       yi = xi + (mem[0]<<2);
241       xh = xi>>15; xl=xi&0x00007fff;
242       for (j=0;j<ord-1;j++)
243       {
244          mem[j] = MAC16_32_Q15(mem[j+1], num[j+1],xi);
245       }
246       mem[ord-1] = MULT16_32_Q15(num[ord],xi);
247       y[i] = SATURATE(yi,805306368);
248    }
249
250 }
251
252 #else
253
254
255
256 spx_word16_t compute_rms(spx_sig_t *x, int len)
257 {
258    int i;
259    float sum=0;
260    for (i=0;i<len;i++)
261    {
262       sum += x[i]*x[i];
263    }
264    return sqrt(.1+sum/len);
265 }
266
267 #ifdef _USE_SSE
268 #include "filters_sse.h"
269 #else
270
271
272 void filter_mem2(spx_sig_t *x, spx_coef_t *num, spx_coef_t *den, spx_sig_t *y, int N, int ord,  spx_mem_t *mem)
273 {
274    int i,j;
275    float xi,yi;
276    for (i=0;i<N;i++)
277    {
278       xi=x[i];
279       y[i] = num[0]*xi + mem[0];
280       yi=y[i];
281       for (j=0;j<ord-1;j++)
282       {
283          mem[j] = mem[j+1] + num[j+1]*xi - den[j+1]*yi;
284       }
285       mem[ord-1] = num[ord]*xi - den[ord]*yi;
286    }
287 }
288
289
290 void iir_mem2(spx_sig_t *x, spx_coef_t *den, spx_sig_t *y, int N, int ord, spx_mem_t *mem)
291 {
292    int i,j;
293    for (i=0;i<N;i++)
294    {
295       y[i] = x[i] + mem[0];
296       for (j=0;j<ord-1;j++)
297       {
298          mem[j] = mem[j+1] - den[j+1]*y[i];
299       }
300       mem[ord-1] = - den[ord]*y[i];
301    }
302 }
303
304
305 #endif
306
307 void fir_mem2(spx_sig_t *x, spx_coef_t *num, spx_sig_t *y, int N, int ord, spx_mem_t *mem)
308 {
309    int i,j;
310    float xi;
311    for (i=0;i<N;i++)
312    {
313       xi=x[i];
314       y[i] = num[0]*xi + mem[0];
315       for (j=0;j<ord-1;j++)
316       {
317          mem[j] = mem[j+1] + num[j+1]*xi;
318       }
319       mem[ord-1] = num[ord]*xi;
320    }
321 }
322
323
324 #endif
325
326
327 void syn_percep_zero(spx_sig_t *xx, spx_coef_t *ak, spx_coef_t *awk1, spx_coef_t *awk2, spx_sig_t *y, int N, int ord, char *stack)
328 {
329    int i;
330    spx_mem_t *mem = PUSH(stack,ord, spx_mem_t);
331    for (i=0;i<ord;i++)
332      mem[i]=0;
333    iir_mem2(xx, ak, y, N, ord, mem);
334    for (i=0;i<ord;i++)
335       mem[i]=0;
336    filter_mem2(y, awk1, awk2, y, N, ord, mem);
337 }
338
339 void residue_percep_zero(spx_sig_t *xx, spx_coef_t *ak, spx_coef_t *awk1, spx_coef_t *awk2, spx_sig_t *y, int N, int ord, char *stack)
340 {
341    int i;
342    spx_mem_t *mem = PUSH(stack,ord, spx_mem_t);
343    for (i=0;i<ord;i++)
344       mem[i]=0;
345    filter_mem2(xx, ak, awk1, y, N, ord, mem);
346    for (i=0;i<ord;i++)
347      mem[i]=0;
348    fir_mem2(y, awk2, y, N, ord, mem);
349 }
350
351 void qmf_decomp(short *xx, spx_word16_t *aa, spx_sig_t *y1, spx_sig_t *y2, int N, int M, spx_word16_t *mem, char *stack)
352 {
353    int i,j,k,M2;
354    spx_word16_t *a;
355    spx_word16_t *x;
356    spx_word16_t *x2;
357    
358    a = PUSH(stack, M, spx_word16_t);
359    x = PUSH(stack, N+M-1, spx_word16_t);
360    x2=x+M-1;
361    M2=M>>1;
362    for (i=0;i<M;i++)
363       a[M-i-1]= aa[i];
364
365    for (i=0;i<M-1;i++)
366       x[i]=mem[M-i-2];
367    for (i=0;i<N;i++)
368       x[i+M-1]=SATURATE(PSHR(xx[i],1),16383);
369    for (i=0,k=0;i<N;i+=2,k++)
370    {
371       y1[k]=0;
372       y2[k]=0;
373       for (j=0;j<M2;j++)
374       {
375          y1[k]+=SHR(MULT16_16(a[j],ADD16(x[i+j],x2[i-j])),1);
376          y2[k]-=SHR(MULT16_16(a[j],SUB16(x[i+j],x2[i-j])),1);
377          j++;
378          y1[k]+=SHR(MULT16_16(a[j],ADD16(x[i+j],x2[i-j])),1);
379          y2[k]+=SHR(MULT16_16(a[j],SUB16(x[i+j],x2[i-j])),1);
380       }
381    }
382    for (i=0;i<M-1;i++)
383      mem[i]=SATURATE(PSHR(xx[N-i-1],1),16383);
384 }
385
386
387 /* By segher */
388 void fir_mem_up(spx_sig_t *x, spx_word16_t *a, spx_sig_t *y, int N, int M, spx_word32_t *mem, char *stack)
389    /* assumptions:
390       all odd x[i] are zero -- well, actually they are left out of the array now
391       N and M are multiples of 4 */
392 {
393    int i, j;
394    spx_word16_t *xx;
395    
396    xx= PUSH(stack, M+N-1, spx_word16_t);
397
398    for (i = 0; i < N/2; i++)
399       xx[2*i] = SHR(x[N/2-1-i],SIG_SHIFT+1);
400    for (i = 0; i < M - 1; i += 2)
401       xx[N+i] = mem[i+1];
402
403    for (i = 0; i < N; i += 4) {
404       spx_sig_t y0, y1, y2, y3;
405       spx_word16_t x0;
406
407       y0 = y1 = y2 = y3 = 0;
408       x0 = xx[N-4-i];
409
410       for (j = 0; j < M; j += 4) {
411          spx_word16_t x1;
412          spx_word16_t a0, a1;
413
414          a0 = a[j];
415          a1 = a[j+1];
416          x1 = xx[N-2+j-i];
417
418          y0 += SHR(MULT16_16(a0, x1),1);
419          y1 += SHR(MULT16_16(a1, x1),1);
420          y2 += SHR(MULT16_16(a0, x0),1);
421          y3 += SHR(MULT16_16(a1, x0),1);
422
423          a0 = a[j+2];
424          a1 = a[j+3];
425          x0 = xx[N+j-i];
426
427          y0 += SHR(MULT16_16(a0, x0),1);
428          y1 += SHR(MULT16_16(a1, x0),1);
429          y2 += SHR(MULT16_16(a0, x1),1);
430          y3 += SHR(MULT16_16(a1, x1),1);
431       }
432       y[i] = y0;
433       y[i+1] = y1;
434       y[i+2] = y2;
435       y[i+3] = y3;
436    }
437
438    for (i = 0; i < M - 1; i += 2)
439       mem[i+1] = xx[i];
440 }
441
442
443
444 void comb_filter_mem_init (CombFilterMem *mem)
445 {
446    mem->last_pitch=0;
447    mem->last_pitch_gain[0]=mem->last_pitch_gain[1]=mem->last_pitch_gain[2]=0;
448    mem->smooth_gain=1;
449 }
450
451 void comb_filter(
452 spx_sig_t *exc,          /*decoded excitation*/
453 spx_sig_t *new_exc,      /*enhanced excitation*/
454 spx_coef_t *ak,           /*LPC filter coefs*/
455 int p,               /*LPC order*/
456 int nsf,             /*sub-frame size*/
457 int pitch,           /*pitch period*/
458 spx_word16_t *spitch_gain,   /*pitch gain (3-tap)*/
459 float  comb_gain,    /*gain of comb filter*/
460 CombFilterMem *mem
461 )
462 {
463    int i;
464    float exc_energy=0, new_exc_energy=0;
465    float gain;
466    float step;
467    float fact;
468    float pitch_gain[3];
469
470    pitch_gain[0] = GAIN_SCALING_1*spitch_gain[0];
471    pitch_gain[1] = GAIN_SCALING_1*spitch_gain[1];
472    pitch_gain[2] = GAIN_SCALING_1*spitch_gain[2];
473
474    /*Compute excitation energy prior to enhancement*/
475    for (i=0;i<nsf;i++)
476       exc_energy+=((float)exc[i])*exc[i];
477
478    /*Some gain adjustment is pitch is too high or if unvoiced*/
479    {
480       float g=0;
481       g = .5*fabs(pitch_gain[0]+pitch_gain[1]+pitch_gain[2] +
482       mem->last_pitch_gain[0] + mem->last_pitch_gain[1] + mem->last_pitch_gain[2]);
483       if (g>1.3)
484          comb_gain*=1.3/g;
485       if (g<.5)
486          comb_gain*=2*g;
487    }
488    step = 1.0/nsf;
489    fact=0;
490    /*Apply pitch comb-filter (filter out noise between pitch harmonics)*/
491    for (i=0;i<nsf;i++)
492    {
493       fact += step;
494
495       new_exc[i] = exc[i] + comb_gain * fact * (
496                                          pitch_gain[0]*exc[i-pitch+1] +
497                                          pitch_gain[1]*exc[i-pitch] +
498                                          pitch_gain[2]*exc[i-pitch-1]
499                                          )
500       + comb_gain * (1-fact) * (
501                                          mem->last_pitch_gain[0]*exc[i-mem->last_pitch+1] +
502                                          mem->last_pitch_gain[1]*exc[i-mem->last_pitch] +
503                                          mem->last_pitch_gain[2]*exc[i-mem->last_pitch-1]
504                                          );
505    }
506
507    mem->last_pitch_gain[0] = pitch_gain[0];
508    mem->last_pitch_gain[1] = pitch_gain[1];
509    mem->last_pitch_gain[2] = pitch_gain[2];
510    mem->last_pitch = pitch;
511
512    /*Gain after enhancement*/
513    for (i=0;i<nsf;i++)
514       new_exc_energy+=((float)new_exc[i])*new_exc[i];
515
516    /*Compute scaling factor and normalize energy*/
517    gain = sqrt(exc_energy)/sqrt(.1+new_exc_energy);
518    if (gain < .5)
519       gain=.5;
520    if (gain>1)
521       gain=1;
522
523    for (i=0;i<nsf;i++)
524    {
525       mem->smooth_gain = .96*mem->smooth_gain + .04*gain;
526       new_exc[i] *= mem->smooth_gain;
527    }
528 }