...
[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 introduce 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    return SHR(SHL((spx_word32_t)spx_sqrt(1+DIV32(sum,len)),(sig_shift+3)),SIG_SHIFT);
184 }
185
186
187 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)
188 {
189    int i,j;
190    spx_sig_t xi,yi,nyi;
191
192    for (i=0;i<N;i++)
193    {
194       int xh,xl,yh,yl;
195       xi=SATURATE(x[i],805306368);
196       yi = SATURATE(ADD32(xi, SHL(mem[0],2)),805306368);
197       nyi = -yi;
198       xh = xi>>15; xl=xi&0x00007fff; yh = yi>>15; yl=yi&0x00007fff; 
199       for (j=0;j<ord-1;j++)
200       {
201          mem[j] = MAC16_32_Q15(MAC16_32_Q15(mem[j+1], num[j+1],xi), den[j+1],nyi);
202       }
203       mem[ord-1] = SUB32(MULT16_32_Q15(num[ord],xi), MULT16_32_Q15(den[ord],yi));
204       y[i] = yi;
205    }
206 }
207
208 void iir_mem2(spx_sig_t *x, spx_coef_t *den, spx_sig_t *y, int N, int ord, spx_mem_t *mem)
209 {
210    int i,j;
211    spx_word32_t xi,yi,nyi;
212
213    for (i=0;i<N;i++)
214    {
215       int yh,yl;
216       xi=SATURATE(x[i],805306368);
217       yi = SATURATE(xi + (mem[0]<<2),805306368);
218       nyi = -yi;
219       yh = yi>>15; yl=yi&0x00007fff; 
220       for (j=0;j<ord-1;j++)
221       {
222          mem[j] = MAC16_32_Q15(mem[j+1],den[j+1],nyi);
223       }
224       mem[ord-1] = - MULT16_32_Q15(den[ord],yi);
225       y[i] = yi;
226    }
227 }
228
229
230 void fir_mem2(spx_sig_t *x, spx_coef_t *num, spx_sig_t *y, int N, int ord, spx_mem_t *mem)
231 {
232    int i,j;
233    spx_word32_t xi,yi;
234
235    for (i=0;i<N;i++)
236    {
237       int xh,xl;
238       xi=SATURATE(x[i],805306368);
239       yi = xi + (mem[0]<<2);
240       xh = xi>>15; xl=xi&0x00007fff;
241       for (j=0;j<ord-1;j++)
242       {
243          mem[j] = MAC16_32_Q15(mem[j+1], num[j+1],xi);
244       }
245       mem[ord-1] = MULT16_32_Q15(num[ord],xi);
246       y[i] = SATURATE(yi,805306368);
247    }
248
249 }
250
251 #else
252
253
254
255 spx_word16_t compute_rms(spx_sig_t *x, int len)
256 {
257    int i;
258    float sum=0;
259    for (i=0;i<len;i++)
260    {
261       sum += x[i]*x[i];
262    }
263    return sqrt(.1+sum/len);
264 }
265
266 #ifdef _USE_SSE
267 #include "filters_sse.h"
268 #else
269
270
271 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)
272 {
273    int i,j;
274    float xi,yi;
275    for (i=0;i<N;i++)
276    {
277       xi=x[i];
278       y[i] = num[0]*xi + mem[0];
279       yi=y[i];
280       for (j=0;j<ord-1;j++)
281       {
282          mem[j] = mem[j+1] + num[j+1]*xi - den[j+1]*yi;
283       }
284       mem[ord-1] = num[ord]*xi - den[ord]*yi;
285    }
286 }
287
288
289 void iir_mem2(spx_sig_t *x, spx_coef_t *den, spx_sig_t *y, int N, int ord, spx_mem_t *mem)
290 {
291    int i,j;
292    for (i=0;i<N;i++)
293    {
294       y[i] = x[i] + mem[0];
295       for (j=0;j<ord-1;j++)
296       {
297          mem[j] = mem[j+1] - den[j+1]*y[i];
298       }
299       mem[ord-1] = - den[ord]*y[i];
300    }
301 }
302
303
304 #endif
305
306 void fir_mem2(spx_sig_t *x, spx_coef_t *num, spx_sig_t *y, int N, int ord, spx_mem_t *mem)
307 {
308    int i,j;
309    float xi;
310    for (i=0;i<N;i++)
311    {
312       xi=x[i];
313       y[i] = num[0]*xi + mem[0];
314       for (j=0;j<ord-1;j++)
315       {
316          mem[j] = mem[j+1] + num[j+1]*xi;
317       }
318       mem[ord-1] = num[ord]*xi;
319    }
320 }
321
322
323 #endif
324
325
326 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)
327 {
328    int i;
329    spx_mem_t *mem = PUSH(stack,ord, spx_mem_t);
330    for (i=0;i<ord;i++)
331      mem[i]=0;
332    iir_mem2(xx, ak, y, N, ord, mem);
333    for (i=0;i<ord;i++)
334       mem[i]=0;
335    filter_mem2(y, awk1, awk2, y, N, ord, mem);
336 }
337
338 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)
339 {
340    int i;
341    spx_mem_t *mem = PUSH(stack,ord, spx_mem_t);
342    for (i=0;i<ord;i++)
343       mem[i]=0;
344    filter_mem2(xx, ak, awk1, y, N, ord, mem);
345    for (i=0;i<ord;i++)
346      mem[i]=0;
347    fir_mem2(y, awk2, y, N, ord, mem);
348 }
349
350 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)
351 {
352    int i,j,k,M2;
353    spx_word16_t *a;
354    spx_word16_t *x;
355    spx_word16_t *x2;
356    
357    a = PUSH(stack, M, spx_word16_t);
358    x = PUSH(stack, N+M-1, spx_word16_t);
359    x2=x+M-1;
360    M2=M>>1;
361    for (i=0;i<M;i++)
362       a[M-i-1]= aa[i];
363
364    for (i=0;i<M-1;i++)
365       x[i]=mem[M-i-2];
366    for (i=0;i<N;i++)
367       x[i+M-1]=SATURATE(PSHR(xx[i],1),16383);
368    for (i=0,k=0;i<N;i+=2,k++)
369    {
370       y1[k]=0;
371       y2[k]=0;
372       for (j=0;j<M2;j++)
373       {
374          y1[k]+=SHR(MULT16_16(a[j],ADD16(x[i+j],x2[i-j])),1);
375          y2[k]-=SHR(MULT16_16(a[j],SUB16(x[i+j],x2[i-j])),1);
376          j++;
377          y1[k]+=SHR(MULT16_16(a[j],ADD16(x[i+j],x2[i-j])),1);
378          y2[k]+=SHR(MULT16_16(a[j],SUB16(x[i+j],x2[i-j])),1);
379       }
380    }
381    for (i=0;i<M-1;i++)
382      mem[i]=SATURATE(PSHR(xx[N-i-1],1),16383);
383 }
384
385
386 /* By segher */
387 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)
388    /* assumptions:
389       all odd x[i] are zero -- well, actually they are left out of the array now
390       N and M are multiples of 4 */
391 {
392    int i, j;
393    spx_word16_t *xx;
394    
395    xx= PUSH(stack, M+N-1, spx_word16_t);
396
397    for (i = 0; i < N/2; i++)
398       xx[2*i] = SHR(x[N/2-1-i],SIG_SHIFT+1);
399    for (i = 0; i < M - 1; i += 2)
400       xx[N+i] = mem[i+1];
401
402    for (i = 0; i < N; i += 4) {
403       spx_sig_t y0, y1, y2, y3;
404       spx_word16_t x0;
405
406       y0 = y1 = y2 = y3 = 0;
407       x0 = xx[N-4-i];
408
409       for (j = 0; j < M; j += 4) {
410          spx_word16_t x1;
411          spx_word16_t a0, a1;
412
413          a0 = a[j];
414          a1 = a[j+1];
415          x1 = xx[N-2+j-i];
416
417          y0 += SHR(MULT16_16(a0, x1),1);
418          y1 += SHR(MULT16_16(a1, x1),1);
419          y2 += SHR(MULT16_16(a0, x0),1);
420          y3 += SHR(MULT16_16(a1, x0),1);
421
422          a0 = a[j+2];
423          a1 = a[j+3];
424          x0 = xx[N+j-i];
425
426          y0 += SHR(MULT16_16(a0, x0),1);
427          y1 += SHR(MULT16_16(a1, x0),1);
428          y2 += SHR(MULT16_16(a0, x1),1);
429          y3 += SHR(MULT16_16(a1, x1),1);
430       }
431       y[i] = y0;
432       y[i+1] = y1;
433       y[i+2] = y2;
434       y[i+3] = y3;
435    }
436
437    for (i = 0; i < M - 1; i += 2)
438       mem[i+1] = xx[i];
439 }
440
441
442
443 void comb_filter_mem_init (CombFilterMem *mem)
444 {
445    mem->last_pitch=0;
446    mem->last_pitch_gain[0]=mem->last_pitch_gain[1]=mem->last_pitch_gain[2]=0;
447    mem->smooth_gain=1;
448 }
449
450 void comb_filter(
451 spx_sig_t *exc,          /*decoded excitation*/
452 spx_sig_t *new_exc,      /*enhanced excitation*/
453 spx_coef_t *ak,           /*LPC filter coefs*/
454 int p,               /*LPC order*/
455 int nsf,             /*sub-frame size*/
456 int pitch,           /*pitch period*/
457 spx_word16_t *spitch_gain,   /*pitch gain (3-tap)*/
458 float  comb_gain,    /*gain of comb filter*/
459 CombFilterMem *mem
460 )
461 {
462    int i;
463    float exc_energy=0, new_exc_energy=0;
464    float gain;
465    float step;
466    float fact;
467    float pitch_gain[3];
468
469    pitch_gain[0] = GAIN_SCALING_1*spitch_gain[0];
470    pitch_gain[1] = GAIN_SCALING_1*spitch_gain[1];
471    pitch_gain[2] = GAIN_SCALING_1*spitch_gain[2];
472
473    /*Compute excitation energy prior to enhancement*/
474    for (i=0;i<nsf;i++)
475       exc_energy+=((float)exc[i])*exc[i];
476
477    /*Some gain adjustment is pitch is too high or if unvoiced*/
478    {
479       float g=0;
480       g = .5*fabs(pitch_gain[0]+pitch_gain[1]+pitch_gain[2] +
481       mem->last_pitch_gain[0] + mem->last_pitch_gain[1] + mem->last_pitch_gain[2]);
482       if (g>1.3)
483          comb_gain*=1.3/g;
484       if (g<.5)
485          comb_gain*=2*g;
486    }
487    step = 1.0/nsf;
488    fact=0;
489    /*Apply pitch comb-filter (filter out noise between pitch harmonics)*/
490    for (i=0;i<nsf;i++)
491    {
492       fact += step;
493
494       new_exc[i] = exc[i] + comb_gain * fact * (
495                                          pitch_gain[0]*exc[i-pitch+1] +
496                                          pitch_gain[1]*exc[i-pitch] +
497                                          pitch_gain[2]*exc[i-pitch-1]
498                                          )
499       + comb_gain * (1-fact) * (
500                                          mem->last_pitch_gain[0]*exc[i-mem->last_pitch+1] +
501                                          mem->last_pitch_gain[1]*exc[i-mem->last_pitch] +
502                                          mem->last_pitch_gain[2]*exc[i-mem->last_pitch-1]
503                                          );
504    }
505
506    mem->last_pitch_gain[0] = pitch_gain[0];
507    mem->last_pitch_gain[1] = pitch_gain[1];
508    mem->last_pitch_gain[2] = pitch_gain[2];
509    mem->last_pitch = pitch;
510
511    /*Gain after enhancement*/
512    for (i=0;i<nsf;i++)
513       new_exc_energy+=((float)new_exc[i])*new_exc[i];
514
515    /*Compute scaling factor and normalize energy*/
516    gain = sqrt(exc_energy)/sqrt(.1+new_exc_energy);
517    if (gain < .5)
518       gain=.5;
519    if (gain>1)
520       gain=1;
521
522    for (i=0;i<nsf;i++)
523    {
524       mem->smooth_gain = .96*mem->smooth_gain + .04*gain;
525       new_exc[i] *= mem->smooth_gain;
526    }
527 }