ac5f68a091aff9a731064331d6967a90c28a2dfb
[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 #ifdef HAVE_CONFIG_H
34 #include "config.h"
35 #endif
36
37 #include "filters.h"
38 #include "stack_alloc.h"
39 #include <math.h>
40 #include "misc.h"
41 #include "math_approx.h"
42 #include "ltp.h"
43
44 #ifdef FIXED_POINT
45 void bw_lpc(spx_word16_t gamma, const spx_coef_t *lpc_in, spx_coef_t *lpc_out, int order)
46 {
47    int i;
48    spx_word16_t tmp=gamma;
49    lpc_out[0] = lpc_in[0];
50    for (i=1;i<order+1;i++)
51    {
52       lpc_out[i] = MULT16_16_P15(tmp,lpc_in[i]);
53       tmp = MULT16_16_P15(tmp, gamma);
54    }
55 }
56 #else
57 void bw_lpc(spx_word16_t gamma, const spx_coef_t *lpc_in, spx_coef_t *lpc_out, int order)
58 {
59    int i;
60    float tmp=1;
61    for (i=0;i<order+1;i++)
62    {
63       lpc_out[i] = tmp * lpc_in[i];
64       tmp *= gamma;
65    }
66 }
67
68 #endif
69
70
71 #ifdef FIXED_POINT
72
73 /* FIXME: These functions are ugly and probably introduce too much error */
74 void signal_mul(const spx_sig_t *x, spx_sig_t *y, spx_word32_t scale, int len)
75 {
76    int i;
77    for (i=0;i<len;i++)
78    {
79       y[i] = SHL(MULT16_32_Q14(SHR(x[i],7),scale),7);
80    }
81 }
82
83 void signal_div(const spx_sig_t *x, spx_sig_t *y, spx_word32_t scale, int len)
84 {
85    int i;
86    spx_word16_t scale_1;
87
88    scale = PSHR(scale, SIG_SHIFT);
89    if (scale<2)
90       scale_1 = 32767;
91    else
92       scale_1 = 32767/scale;
93    for (i=0;i<len;i++)
94    {
95       y[i] = MULT16_32_Q15(scale_1,x[i]);
96    }
97 }
98
99 #else
100
101 void signal_mul(const spx_sig_t *x, spx_sig_t *y, spx_word32_t scale, int len)
102 {
103    int i;
104    for (i=0;i<len;i++)
105       y[i] = scale*x[i];
106 }
107
108 void signal_div(const spx_sig_t *x, spx_sig_t *y, spx_word32_t scale, int len)
109 {
110    int i;
111    float scale_1 = 1/scale;
112    for (i=0;i<len;i++)
113       y[i] = scale_1*x[i];
114 }
115 #endif
116
117
118
119 #ifdef FIXED_POINT
120
121
122
123 spx_word16_t compute_rms(const spx_sig_t *x, int len)
124 {
125    int i;
126    spx_word32_t sum=0;
127    spx_sig_t max_val=1;
128    int sig_shift;
129
130    for (i=0;i<len;i++)
131    {
132       spx_sig_t tmp = x[i];
133       if (tmp<0)
134          tmp = -tmp;
135       if (tmp > max_val)
136          max_val = tmp;
137    }
138
139    sig_shift=0;
140    while (max_val>16383)
141    {
142       sig_shift++;
143       max_val >>= 1;
144    }
145
146    for (i=0;i<len;i+=4)
147    {
148       spx_word32_t sum2=0;
149       spx_word16_t tmp;
150       tmp = SHR(x[i],sig_shift);
151       sum2 += MULT16_16(tmp,tmp);
152       tmp = SHR(x[i+1],sig_shift);
153       sum2 += MULT16_16(tmp,tmp);
154       tmp = SHR(x[i+2],sig_shift);
155       sum2 += MULT16_16(tmp,tmp);
156       tmp = SHR(x[i+3],sig_shift);
157       sum2 += MULT16_16(tmp,tmp);
158       sum += SHR(sum2,6);
159    }
160    
161    return SHR(SHL((spx_word32_t)spx_sqrt(1+DIV32(sum,len)),(sig_shift+3)),SIG_SHIFT);
162 }
163
164 #if defined(ARM4_ASM) || defined(ARM5E_ASM)
165 #include "filters_arm4.h"
166 #else
167
168
169 int normalize16(const spx_sig_t *x, spx_word16_t *y, int max_scale, int len)
170 {
171    int i;
172    spx_sig_t max_val=1;
173    int sig_shift;
174    
175    for (i=0;i<len;i++)
176    {
177       spx_sig_t tmp = x[i];
178       if (tmp<0)
179          tmp = -tmp;
180       if (tmp >= max_val)
181          max_val = tmp;
182    }
183
184    sig_shift=0;
185    while (max_val>max_scale)
186    {
187       sig_shift++;
188       max_val >>= 1;
189    }
190
191    for (i=0;i<len;i++)
192       y[i] = SHR(x[i], sig_shift);
193    
194    return sig_shift;
195 }
196
197 void filter_mem2(const spx_sig_t *x, const spx_coef_t *num, const spx_coef_t *den, spx_sig_t *y, int N, int ord, spx_mem_t *mem)
198 {
199    int i,j;
200    spx_sig_t xi,yi,nyi;
201
202    for (i=0;i<N;i++)
203    {
204       int xh,xl,yh,yl;
205       xi=SATURATE(x[i],805306368);
206       yi = SATURATE(ADD32(xi, SHL(mem[0],2)),805306368);
207       nyi = -yi;
208       xh = xi>>15; xl=xi&0x00007fff; yh = yi>>15; yl=yi&0x00007fff; 
209       for (j=0;j<ord-1;j++)
210       {
211          mem[j] = MAC16_32_Q15(MAC16_32_Q15(mem[j+1], num[j+1],xi), den[j+1],nyi);
212       }
213       mem[ord-1] = SUB32(MULT16_32_Q15(num[ord],xi), MULT16_32_Q15(den[ord],yi));
214       y[i] = yi;
215    }
216 }
217
218 void iir_mem2(const spx_sig_t *x, const spx_coef_t *den, spx_sig_t *y, int N, int ord, spx_mem_t *mem)
219 {
220    int i,j;
221    spx_word32_t xi,yi,nyi;
222
223    for (i=0;i<N;i++)
224    {
225       int yh,yl;
226       xi=SATURATE(x[i],805306368);
227       yi = SATURATE(xi + (mem[0]<<2),805306368);
228       nyi = -yi;
229       yh = yi>>15; yl=yi&0x00007fff; 
230       for (j=0;j<ord-1;j++)
231       {
232          mem[j] = MAC16_32_Q15(mem[j+1],den[j+1],nyi);
233       }
234       mem[ord-1] = - MULT16_32_Q15(den[ord],yi);
235       y[i] = yi;
236    }
237 }
238
239 #endif
240
241
242
243 void fir_mem2(const spx_sig_t *x, const spx_coef_t *num, spx_sig_t *y, int N, int ord, spx_mem_t *mem)
244 {
245    int i,j;
246    spx_word32_t xi,yi;
247
248    for (i=0;i<N;i++)
249    {
250       int xh,xl;
251       xi=SATURATE(x[i],805306368);
252       yi = xi + (mem[0]<<2);
253       xh = xi>>15; xl=xi&0x00007fff;
254       for (j=0;j<ord-1;j++)
255       {
256          mem[j] = MAC16_32_Q15(mem[j+1], num[j+1],xi);
257       }
258       mem[ord-1] = MULT16_32_Q15(num[ord],xi);
259       y[i] = SATURATE(yi,805306368);
260    }
261
262 }
263
264 #else
265
266
267
268 spx_word16_t compute_rms(const spx_sig_t *x, int len)
269 {
270    int i;
271    float sum=0;
272    for (i=0;i<len;i++)
273    {
274       sum += x[i]*x[i];
275    }
276    return sqrt(.1+sum/len);
277 }
278
279 #ifdef _USE_SSE
280 #include "filters_sse.h"
281 #else
282
283
284 void filter_mem2(const spx_sig_t *x, const spx_coef_t *num, const spx_coef_t *den, spx_sig_t *y, int N, int ord,  spx_mem_t *mem)
285 {
286    int i,j;
287    float xi,yi;
288    for (i=0;i<N;i++)
289    {
290       xi=x[i];
291       y[i] = num[0]*xi + mem[0];
292       yi=y[i];
293       for (j=0;j<ord-1;j++)
294       {
295          mem[j] = mem[j+1] + num[j+1]*xi - den[j+1]*yi;
296       }
297       mem[ord-1] = num[ord]*xi - den[ord]*yi;
298    }
299 }
300
301
302 void iir_mem2(const spx_sig_t *x, const spx_coef_t *den, spx_sig_t *y, int N, int ord, spx_mem_t *mem)
303 {
304    int i,j;
305    for (i=0;i<N;i++)
306    {
307       y[i] = x[i] + mem[0];
308       for (j=0;j<ord-1;j++)
309       {
310          mem[j] = mem[j+1] - den[j+1]*y[i];
311       }
312       mem[ord-1] = - den[ord]*y[i];
313    }
314 }
315
316
317 void fir_mem2(const spx_sig_t *x, const spx_coef_t *num, spx_sig_t *y, int N, int ord, spx_mem_t *mem)
318 {
319    int i,j;
320    float xi;
321    for (i=0;i<N;i++)
322    {
323       xi=x[i];
324       y[i] = num[0]*xi + mem[0];
325       for (j=0;j<ord-1;j++)
326       {
327          mem[j] = mem[j+1] + num[j+1]*xi;
328       }
329       mem[ord-1] = num[ord]*xi;
330    }
331 }
332 #endif
333
334 #endif
335
336
337 void syn_percep_zero(const spx_sig_t *xx, const spx_coef_t *ak, const spx_coef_t *awk1, const spx_coef_t *awk2, spx_sig_t *y, int N, int ord, char *stack)
338 {
339    int i;
340    spx_mem_t *mem = PUSH(stack,ord, spx_mem_t);
341    for (i=0;i<ord;i++)
342      mem[i]=0;
343    iir_mem2(xx, ak, y, N, ord, mem);
344    for (i=0;i<ord;i++)
345       mem[i]=0;
346    filter_mem2(y, awk1, awk2, y, N, ord, mem);
347 }
348
349 void residue_percep_zero(const spx_sig_t *xx, const spx_coef_t *ak, const spx_coef_t *awk1, const spx_coef_t *awk2, spx_sig_t *y, int N, int ord, char *stack)
350 {
351    int i;
352    spx_mem_t *mem = PUSH(stack,ord, spx_mem_t);
353    for (i=0;i<ord;i++)
354       mem[i]=0;
355    filter_mem2(xx, ak, awk1, y, N, ord, mem);
356    for (i=0;i<ord;i++)
357      mem[i]=0;
358    fir_mem2(y, awk2, y, N, ord, mem);
359 }
360
361 void qmf_decomp(const spx_word16_t *xx, const spx_word16_t *aa, spx_sig_t *y1, spx_sig_t *y2, int N, int M, spx_word16_t *mem, char *stack)
362 {
363    int i,j,k,M2;
364    spx_word16_t *a;
365    spx_word16_t *x;
366    spx_word16_t *x2;
367    
368    a = PUSH(stack, M, spx_word16_t);
369    x = PUSH(stack, N+M-1, spx_word16_t);
370    x2=x+M-1;
371    M2=M>>1;
372    for (i=0;i<M;i++)
373       a[M-i-1]= aa[i];
374
375    for (i=0;i<M-1;i++)
376       x[i]=mem[M-i-2];
377    for (i=0;i<N;i++)
378       x[i+M-1]=SATURATE(PSHR(xx[i],1),16383);
379    for (i=0,k=0;i<N;i+=2,k++)
380    {
381       y1[k]=0;
382       y2[k]=0;
383       for (j=0;j<M2;j++)
384       {
385          y1[k]+=SHR(MULT16_16(a[j],ADD16(x[i+j],x2[i-j])),1);
386          y2[k]-=SHR(MULT16_16(a[j],SUB16(x[i+j],x2[i-j])),1);
387          j++;
388          y1[k]+=SHR(MULT16_16(a[j],ADD16(x[i+j],x2[i-j])),1);
389          y2[k]+=SHR(MULT16_16(a[j],SUB16(x[i+j],x2[i-j])),1);
390       }
391    }
392    for (i=0;i<M-1;i++)
393      mem[i]=SATURATE(PSHR(xx[N-i-1],1),16383);
394 }
395
396
397 /* By segher */
398 void fir_mem_up(const spx_sig_t *x, const spx_word16_t *a, spx_sig_t *y, int N, int M, spx_word32_t *mem, char *stack)
399    /* assumptions:
400       all odd x[i] are zero -- well, actually they are left out of the array now
401       N and M are multiples of 4 */
402 {
403    int i, j;
404    spx_word16_t *xx;
405    
406    xx= PUSH(stack, M+N-1, spx_word16_t);
407
408    for (i = 0; i < N/2; i++)
409       xx[2*i] = SHR(x[N/2-1-i],SIG_SHIFT+1);
410    for (i = 0; i < M - 1; i += 2)
411       xx[N+i] = mem[i+1];
412
413    for (i = 0; i < N; i += 4) {
414       spx_sig_t y0, y1, y2, y3;
415       spx_word16_t x0;
416
417       y0 = y1 = y2 = y3 = 0;
418       x0 = xx[N-4-i];
419
420       for (j = 0; j < M; j += 4) {
421          spx_word16_t x1;
422          spx_word16_t a0, a1;
423
424          a0 = a[j];
425          a1 = a[j+1];
426          x1 = xx[N-2+j-i];
427
428          y0 += SHR(MULT16_16(a0, x1),1);
429          y1 += SHR(MULT16_16(a1, x1),1);
430          y2 += SHR(MULT16_16(a0, x0),1);
431          y3 += SHR(MULT16_16(a1, x0),1);
432
433          a0 = a[j+2];
434          a1 = a[j+3];
435          x0 = xx[N+j-i];
436
437          y0 += SHR(MULT16_16(a0, x0),1);
438          y1 += SHR(MULT16_16(a1, x0),1);
439          y2 += SHR(MULT16_16(a0, x1),1);
440          y3 += SHR(MULT16_16(a1, x1),1);
441       }
442       y[i] = y0;
443       y[i+1] = y1;
444       y[i+2] = y2;
445       y[i+3] = y3;
446    }
447
448    for (i = 0; i < M - 1; i += 2)
449       mem[i+1] = xx[i];
450 }
451
452
453
454 void comb_filter_mem_init (CombFilterMem *mem)
455 {
456    mem->last_pitch=0;
457    mem->last_pitch_gain[0]=mem->last_pitch_gain[1]=mem->last_pitch_gain[2]=0;
458    mem->smooth_gain=1;
459 }
460
461 #ifdef FIXED_POINT
462 #define COMB_STEP 32767
463 #else
464 #define COMB_STEP 1.0
465 #endif
466
467 void comb_filter(
468 spx_sig_t *exc,          /*decoded excitation*/
469 spx_sig_t *new_exc,      /*enhanced excitation*/
470 spx_coef_t *ak,           /*LPC filter coefs*/
471 int p,               /*LPC order*/
472 int nsf,             /*sub-frame size*/
473 int pitch,           /*pitch period*/
474 spx_word16_t *pitch_gain,   /*pitch gain (3-tap)*/
475 spx_word16_t  comb_gain,    /*gain of comb filter*/
476 CombFilterMem *mem
477 )
478 {
479    int i;
480    spx_word16_t exc_energy=0, new_exc_energy=0;
481    float gain;
482    spx_word16_t step;
483    spx_word16_t fact;
484
485    /*Compute excitation amplitude prior to enhancement*/
486    exc_energy = compute_rms(exc, nsf);
487    /*for (i=0;i<nsf;i++)
488      exc_energy+=((float)exc[i])*exc[i];*/
489
490    /*Some gain adjustment if pitch is too high or if unvoiced*/
491    {
492       float g=0;
493       g = GAIN_SCALING_1*.5*(gain_3tap_to_1tap(pitch_gain)+gain_3tap_to_1tap(mem->last_pitch_gain));
494       if (g>1.3)
495          comb_gain*=1.3/g;
496       if (g<.5)
497          comb_gain*=2.*g;
498    }
499    step = DIV32(COMB_STEP, nsf);
500    fact=0;
501
502    /*Apply pitch comb-filter (filter out noise between pitch harmonics)*/
503    for (i=0;i<nsf;i++)
504    {
505       spx_word32_t exc1, exc2;
506
507       fact += step;
508       
509       exc1 = SHL(MULT16_32_Q15(SHL(pitch_gain[0],7),exc[i-pitch+1]) +
510                  MULT16_32_Q15(SHL(pitch_gain[1],7),exc[i-pitch]) +
511                  MULT16_32_Q15(SHL(pitch_gain[2],7),exc[i-pitch-1]) , 2);
512       exc2 = SHL(MULT16_32_Q15(SHL(mem->last_pitch_gain[0],7),exc[i-mem->last_pitch+1]) +
513                  MULT16_32_Q15(SHL(mem->last_pitch_gain[1],7),exc[i-mem->last_pitch]) +
514                  MULT16_32_Q15(SHL(mem->last_pitch_gain[2],7),exc[i-mem->last_pitch-1]),2);
515
516       new_exc[i] = exc[i] + MULT16_32_Q15(comb_gain,MULT16_32_Q15(fact,exc1)  + MULT16_32_Q15(SUB16(COMB_STEP,fact), exc2));
517    }
518
519    mem->last_pitch_gain[0] = pitch_gain[0];
520    mem->last_pitch_gain[1] = pitch_gain[1];
521    mem->last_pitch_gain[2] = pitch_gain[2];
522    mem->last_pitch = pitch;
523
524    /*Amplitude after enhancement*/
525    new_exc_energy = compute_rms(new_exc, nsf);
526
527    /*Compute scaling factor and normalize energy*/
528    gain = (exc_energy)/(.1+new_exc_energy);
529    if (gain < .5)
530       gain=.5;
531    if (gain>.9999)
532       gain=.9999;
533
534 #ifdef FIXED_POINT
535    {
536       spx_word16_t gg = gain*32768;
537       for (i=0;i<nsf;i++)
538    {
539       mem->smooth_gain = MULT16_16_Q15(31457,mem->smooth_gain) + MULT16_16_Q15(1311,gg);
540       new_exc[i] = MULT16_32_Q15(mem->smooth_gain, new_exc[i]);
541    }
542    }
543 #else
544    for (i=0;i<nsf;i++)
545    {
546       mem->smooth_gain = .96*mem->smooth_gain + .04*gain;
547       new_exc[i] *= mem->smooth_gain;
548    }
549 #endif
550 }