845f3b0af5559646144921c76278b2d06d450871
[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 "misc.h"
40 #include "math_approx.h"
41 #include "ltp.h"
42 #include <math.h>
43
44 #ifdef _USE_SSE
45 #include "filters_sse.h"
46 #elif defined (ARM4_ASM) || defined(ARM5E_ASM)
47 #include "filters_arm4.h"
48 #elif defined (BFIN_ASM)
49 #include "filters_bfin.h"
50 #endif
51
52
53
54 void bw_lpc(spx_word16_t gamma, const spx_coef_t *lpc_in, spx_coef_t *lpc_out, int order)
55 {
56    int i;
57    spx_word16_t tmp=gamma;
58    for (i=0;i<order;i++)
59    {
60       lpc_out[i] = MULT16_16_P15(tmp,lpc_in[i]);
61       tmp = MULT16_16_P15(tmp, gamma);
62    }
63 }
64
65
66 #ifdef FIXED_POINT
67
68 /* FIXME: These functions are ugly and probably introduce too much error */
69 void signal_mul(const spx_sig_t *x, spx_sig_t *y, spx_word32_t scale, int len)
70 {
71    int i;
72    for (i=0;i<len;i++)
73    {
74       y[i] = SHL32(MULT16_32_Q14(EXTRACT16(SHR32(x[i],7)),scale),7);
75    }
76 }
77
78 void signal_div(const spx_sig_t *x, spx_sig_t *y, spx_word32_t scale, int len)
79 {
80    int i;
81    if (scale > SHL32(EXTEND32(SIG_SCALING), 8))
82    {
83       spx_word16_t scale_1;
84       scale = PSHR32(scale, SIG_SHIFT);
85       scale_1 = EXTRACT16(DIV32_16(SHL32(EXTEND32(SIG_SCALING),7),scale));
86       for (i=0;i<len;i++)
87       {
88          y[i] = SHR32(MULT16_16(scale_1, EXTRACT16(SHR32(x[i],SIG_SHIFT))),7);
89       }
90    } else {
91       spx_word16_t scale_1;
92       scale = PSHR32(scale, SIG_SHIFT-5);
93       scale_1 = DIV32_16(SHL32(EXTEND32(SIG_SCALING),3),scale);
94       for (i=0;i<len;i++)
95       {
96          y[i] = MULT16_16(scale_1, EXTRACT16(SHR32(x[i],SIG_SHIFT-2)));
97       }
98    }
99 }
100
101 #else
102
103 void signal_mul(const spx_sig_t *x, spx_sig_t *y, spx_word32_t scale, int len)
104 {
105    int i;
106    for (i=0;i<len;i++)
107       y[i] = scale*x[i];
108 }
109
110 void signal_div(const spx_sig_t *x, spx_sig_t *y, spx_word32_t scale, int len)
111 {
112    int i;
113    float scale_1 = 1/scale;
114    for (i=0;i<len;i++)
115       y[i] = scale_1*x[i];
116 }
117 #endif
118
119
120
121 #ifdef FIXED_POINT
122
123
124
125 spx_word16_t compute_rms(const spx_sig_t *x, int len)
126 {
127    int i;
128    spx_word32_t sum=0;
129    spx_sig_t max_val=1;
130    int sig_shift;
131
132    for (i=0;i<len;i++)
133    {
134       spx_sig_t tmp = x[i];
135       if (tmp<0)
136          tmp = -tmp;
137       if (tmp > max_val)
138          max_val = tmp;
139    }
140
141    sig_shift=0;
142    while (max_val>16383)
143    {
144       sig_shift++;
145       max_val >>= 1;
146    }
147
148    for (i=0;i<len;i+=4)
149    {
150       spx_word32_t sum2=0;
151       spx_word16_t tmp;
152       tmp = EXTRACT16(SHR32(x[i],sig_shift));
153       sum2 = MAC16_16(sum2,tmp,tmp);
154       tmp = EXTRACT16(SHR32(x[i+1],sig_shift));
155       sum2 = MAC16_16(sum2,tmp,tmp);
156       tmp = EXTRACT16(SHR32(x[i+2],sig_shift));
157       sum2 = MAC16_16(sum2,tmp,tmp);
158       tmp = EXTRACT16(SHR32(x[i+3],sig_shift));
159       sum2 = MAC16_16(sum2,tmp,tmp);
160       sum = ADD32(sum,SHR32(sum2,6));
161    }
162    
163    return EXTRACT16(PSHR32(SHL32(EXTEND32(spx_sqrt(1+DIV32(sum,len))),(sig_shift+3)),SIG_SHIFT));
164 }
165
166 spx_word16_t compute_rms16(const spx_word16_t *x, int len)
167 {
168    int i;
169    spx_word32_t sum=0;
170    for (i=0;i<len;i+=4)
171    {
172       spx_word32_t sum2=0;
173       sum2 = MAC16_16(sum2,x[i],x[i]);
174       sum2 = MAC16_16(sum2,x[i+1],x[i+1]);
175       sum2 = MAC16_16(sum2,x[i+2],x[i+2]);
176       sum2 = MAC16_16(sum2,x[i+3],x[i+3]);
177       sum = ADD32(sum,SHR32(sum2,6));
178    }
179    
180    return SHL16(spx_sqrt(1+DIV32(sum,len)),3);
181 }
182
183 #ifndef OVERRIDE_NORMALIZE16
184 int normalize16(const spx_sig_t *x, spx_word16_t *y, spx_sig_t max_scale, int len)
185 {
186    int i;
187    spx_sig_t max_val=1;
188    int sig_shift;
189    
190    for (i=0;i<len;i++)
191    {
192       spx_sig_t tmp = x[i];
193       if (tmp<0)
194          tmp = NEG32(tmp);
195       if (tmp >= max_val)
196          max_val = tmp;
197    }
198
199    sig_shift=0;
200    while (max_val>max_scale)
201    {
202       sig_shift++;
203       max_val >>= 1;
204    }
205
206    for (i=0;i<len;i++)
207       y[i] = EXTRACT16(SHR32(x[i], sig_shift));
208    
209    return sig_shift;
210 }
211 #endif
212
213 #else
214
215 spx_word16_t compute_rms(const spx_sig_t *x, int len)
216 {
217    int i;
218    float sum=0;
219    for (i=0;i<len;i++)
220    {
221       sum += x[i]*x[i];
222    }
223    return sqrt(.1+sum/len);
224 }
225 spx_word16_t compute_rms16(const spx_word16_t *x, int len)
226 {
227    return compute_rms(x, len);
228 }
229 #endif
230
231
232
233 #ifndef OVERRIDE_FILTER_MEM2
234 #ifdef PRECISION16
235 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)
236 {
237    int i,j;
238    spx_word16_t xi,yi,nyi;
239
240    for (i=0;i<N;i++)
241    {
242       xi= EXTRACT16(PSHR32(SATURATE(x[i],536870911),SIG_SHIFT));
243       yi = EXTRACT16(PSHR32(SATURATE(ADD32(x[i], SHL32(mem[0],1)),536870911),SIG_SHIFT));
244       nyi = NEG16(yi);
245       for (j=0;j<ord-1;j++)
246       {
247          mem[j] = MAC16_16(MAC16_16(mem[j+1], num[j],xi), den[j],nyi);
248       }
249       mem[ord-1] = ADD32(MULT16_16(num[ord-1],xi), MULT16_16(den[ord-1],nyi));
250       y[i] = SHL32(EXTEND32(yi),SIG_SHIFT);
251    }
252 }
253 #else
254 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)
255 {
256    int i,j;
257    spx_sig_t xi,yi,nyi;
258
259    for (i=0;i<ord;i++)
260       mem[i] = SHR32(mem[i],1);   
261    for (i=0;i<N;i++)
262    {
263       xi=SATURATE(x[i],805306368);
264       yi = SATURATE(ADD32(xi, SHL32(mem[0],2)),805306368);
265       nyi = NEG32(yi);
266       for (j=0;j<ord-1;j++)
267       {
268          mem[j] = MAC16_32_Q15(MAC16_32_Q15(mem[j+1], num[j],xi), den[j],nyi);
269       }
270       mem[ord-1] = SUB32(MULT16_32_Q15(num[ord-1],xi), MULT16_32_Q15(den[ord-1],yi));
271       y[i] = yi;
272    }
273    for (i=0;i<ord;i++)
274       mem[i] = SHL32(mem[i],1);   
275 }
276 #endif
277 #endif
278
279 void filter_mem16(const spx_word16_t *x, const spx_coef_t *num, const spx_coef_t *den, spx_word16_t *y, int N, int ord, spx_mem_t *mem)
280 {
281    int i,j;
282    spx_word16_t xi,yi,nyi;
283    for (i=0;i<N;i++)
284    {
285       xi= x[i];
286       yi = EXTRACT16(SATURATE(ADD32(EXTEND32(x[i]),PSHR32(mem[0],LPC_SHIFT)),32767));
287       nyi = NEG16(yi);
288       for (j=0;j<ord-1;j++)
289       {
290          mem[j] = MAC16_16(MAC16_16(mem[j+1], num[j],xi), den[j],nyi);
291       }
292       mem[ord-1] = ADD32(MULT16_16(num[ord-1],xi), MULT16_16(den[ord-1],nyi));
293       y[i] = yi;
294    }
295 }
296
297
298 #ifndef OVERRIDE_IIR_MEM2
299 #ifdef PRECISION16
300 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)
301 {
302    int i,j;
303    spx_word16_t yi,nyi;
304
305    for (i=0;i<N;i++)
306    {
307       yi = EXTRACT16(PSHR32(SATURATE(x[i] + SHL32(mem[0],1),536870911),SIG_SHIFT));
308       nyi = NEG16(yi);
309       for (j=0;j<ord-1;j++)
310       {
311          mem[j] = MAC16_16(mem[j+1],den[j],nyi);
312       }
313       mem[ord-1] = MULT16_16(den[ord-1],nyi);
314       y[i] = SHL32(EXTEND32(yi),SIG_SHIFT);
315    }
316 }
317 #else
318 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)
319 {
320    int i,j;
321    spx_word32_t xi,yi,nyi;
322
323    for (i=0;i<ord;i++)
324       mem[i] = SHR32(mem[i],1);   
325    for (i=0;i<N;i++)
326    {
327       xi=SATURATE(x[i],805306368);
328       yi = SATURATE(xi + SHL32(mem[0],2),805306368);
329       nyi = NEG32(yi);
330       for (j=0;j<ord-1;j++)
331       {
332          mem[j] = MAC16_32_Q15(mem[j+1],den[j],nyi);
333       }
334       mem[ord-1] = MULT16_32_Q15(den[ord-1],nyi);
335       y[i] = yi;
336    }
337    for (i=0;i<ord;i++)
338       mem[i] = SHL32(mem[i],1);   
339 }
340 #endif
341 #endif
342
343
344 void iir_mem16(const spx_word16_t *x, const spx_coef_t *den, spx_word16_t *y, int N, int ord, spx_mem_t *mem)
345 {
346    int i,j;
347    spx_word16_t yi,nyi;
348
349    for (i=0;i<N;i++)
350    {
351       yi = EXTRACT16(SATURATE(ADD32(EXTEND32(x[i]),PSHR32(mem[0],LPC_SHIFT)),32767));
352       nyi = NEG16(yi);
353       for (j=0;j<ord-1;j++)
354       {
355          mem[j] = MAC16_16(mem[j+1],den[j],nyi);
356       }
357       mem[ord-1] = MULT16_16(den[ord-1],nyi);
358       y[i] = yi;
359    }
360 }
361
362
363 #ifndef OVERRIDE_FIR_MEM2
364 #ifdef PRECISION16
365 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)
366 {
367    int i,j;
368    spx_word16_t xi,yi;
369
370    for (i=0;i<N;i++)
371    {
372       xi= EXTRACT16(PSHR32(SATURATE(x[i],536870911),SIG_SHIFT));
373       yi = EXTRACT16(PSHR32(SATURATE(x[i] + SHL32(mem[0],1),536870911),SIG_SHIFT));
374       for (j=0;j<ord-1;j++)
375       {
376          mem[j] = MAC16_16(mem[j+1], num[j],xi);
377       }
378       mem[ord-1] = MULT16_16(num[ord-1],xi);
379       y[i] = SHL32(EXTEND32(yi),SIG_SHIFT);
380    }
381 }
382 #else
383 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)
384 {
385    int i,j;
386    spx_word32_t xi,yi;
387
388    for (i=0;i<ord;i++)
389       mem[i] = SHR32(mem[i],1);   
390    for (i=0;i<N;i++)
391    {
392       xi=SATURATE(x[i],805306368);
393       yi = xi + SHL32(mem[0],2);
394       for (j=0;j<ord-1;j++)
395       {
396          mem[j] = MAC16_32_Q15(mem[j+1], num[j],xi);
397       }
398       mem[ord-1] = MULT16_32_Q15(num[ord-1],xi);
399       y[i] = SATURATE(yi,805306368);
400    }
401    for (i=0;i<ord;i++)
402       mem[i] = SHL32(mem[i],1);   
403 }
404 #endif
405 #endif
406
407 void fir_mem16(const spx_word16_t *x, const spx_coef_t *num, spx_word16_t *y, int N, int ord, spx_mem_t *mem)
408 {
409    int i,j;
410    spx_word16_t xi,yi;
411
412    for (i=0;i<N;i++)
413    {
414       xi=x[i];
415       yi = EXTRACT16(SATURATE(ADD32(EXTEND32(x[i]),PSHR32(mem[0],LPC_SHIFT)),32767));
416       for (j=0;j<ord-1;j++)
417       {
418          mem[j] = MAC16_16(mem[j+1], num[j],xi);
419       }
420       mem[ord-1] = MULT16_16(num[ord-1],xi);
421       y[i] = yi;
422    }
423 }
424
425
426
427
428
429
430
431 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)
432 {
433    int i;
434    VARDECL(spx_mem_t *mem);
435    ALLOC(mem, ord, spx_mem_t);
436    for (i=0;i<ord;i++)
437      mem[i]=0;
438    iir_mem2(xx, ak, y, N, ord, mem);
439    for (i=0;i<ord;i++)
440       mem[i]=0;
441    filter_mem2(y, awk1, awk2, y, N, ord, mem);
442 }
443
444 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)
445 {
446    int i;
447    VARDECL(spx_mem_t *mem);
448    ALLOC(mem, ord, spx_mem_t);
449    for (i=0;i<ord;i++)
450       mem[i]=0;
451    filter_mem2(xx, ak, awk1, y, N, ord, mem);
452    for (i=0;i<ord;i++)
453      mem[i]=0;
454    fir_mem2(y, awk2, y, N, ord, mem);
455 }
456
457 #ifndef OVERRIDE_COMPUTE_IMPULSE_RESPONSE
458 void compute_impulse_response(const spx_coef_t *ak, const spx_coef_t *awk1, const spx_coef_t *awk2, spx_word16_t *y, int N, int ord, char *stack)
459 {
460    int i,j;
461    spx_word16_t y1, ny1i, ny2i;
462    VARDECL(spx_mem_t *mem1);
463    VARDECL(spx_mem_t *mem2);
464    ALLOC(mem1, ord, spx_mem_t);
465    ALLOC(mem2, ord, spx_mem_t);
466    
467    y[0] = LPC_SCALING;
468    for (i=0;i<ord;i++)
469       y[i+1] = awk1[i];
470    i++;
471    for (;i<N;i++)
472       y[i] = VERY_SMALL;
473    
474    for (i=0;i<ord;i++)
475       mem1[i] = mem2[i] = 0;
476    for (i=0;i<N;i++)
477    {
478       y1 = ADD16(y[i], EXTRACT16(PSHR32(mem1[0],LPC_SHIFT)));
479       ny1i = NEG16(y1);
480       y[i] = ADD16(SHL16(y1,1), EXTRACT16(PSHR32(mem2[0],LPC_SHIFT)));
481       ny2i = NEG16(y[i]);
482       for (j=0;j<ord-1;j++)
483       {
484          mem1[j] = MAC16_16(mem1[j+1], awk2[j],ny1i);
485          mem2[j] = MAC16_16(mem2[j+1], ak[j],ny2i);
486       }
487       mem1[ord-1] = MULT16_16(awk2[ord-1],ny1i);
488       mem2[ord-1] = MULT16_16(ak[ord-1],ny2i);
489    }
490 }
491 #endif
492
493 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)
494 {
495    int i,j,k,M2;
496    VARDECL(spx_word16_t *a);
497    VARDECL(spx_word16_t *x);
498    spx_word16_t *x2;
499    
500    ALLOC(a, M, spx_word16_t);
501    ALLOC(x, N+M-1, spx_word16_t);
502    x2=x+M-1;
503    M2=M>>1;
504    for (i=0;i<M;i++)
505       a[M-i-1]= aa[i];
506
507    for (i=0;i<M-1;i++)
508       x[i]=mem[M-i-2];
509    for (i=0;i<N;i++)
510       x[i+M-1]=SATURATE(PSHR(xx[i],1),16383);
511    for (i=0,k=0;i<N;i+=2,k++)
512    {
513       y1[k]=0;
514       y2[k]=0;
515       for (j=0;j<M2;j++)
516       {
517          y1[k]=ADD32(y1[k],MULT16_16(a[j],ADD16(x[i+j],x2[i-j])));
518          y2[k]=SUB32(y2[k],MULT16_16(a[j],SUB16(x[i+j],x2[i-j])));
519          j++;
520          y1[k]=ADD32(y1[k],MULT16_16(a[j],ADD16(x[i+j],x2[i-j])));
521          y2[k]=ADD32(y2[k],MULT16_16(a[j],SUB16(x[i+j],x2[i-j])));
522       }
523       y1[k] = SHR32(y1[k],1);
524       y2[k] = SHR32(y2[k],1);
525    }
526    for (i=0;i<M-1;i++)
527      mem[i]=SATURATE(PSHR(xx[N-i-1],1),16383);
528 }
529
530
531 /* By segher */
532 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)
533    /* assumptions:
534       all odd x[i] are zero -- well, actually they are left out of the array now
535       N and M are multiples of 4 */
536 {
537    int i, j;
538    VARDECL(spx_word16_t *xx);
539    
540    ALLOC(xx, M+N-1, spx_word16_t);
541
542    for (i = 0; i < N/2; i++)
543       xx[2*i] = PSHR(x[N/2-1-i],SIG_SHIFT+1);
544    for (i = 0; i < M - 1; i += 2)
545       xx[N+i] = mem[i+1];
546
547    for (i = 0; i < N; i += 4) {
548       spx_sig_t y0, y1, y2, y3;
549       spx_word16_t x0;
550
551       y0 = y1 = y2 = y3 = 0;
552       x0 = xx[N-4-i];
553
554       for (j = 0; j < M; j += 4) {
555          spx_word16_t x1;
556          spx_word16_t a0, a1;
557
558          a0 = a[j];
559          a1 = a[j+1];
560          x1 = xx[N-2+j-i];
561
562          y0 = ADD32(y0,SHR(MULT16_16(a0, x1),1));
563          y1 = ADD32(y1,SHR(MULT16_16(a1, x1),1));
564          y2 = ADD32(y2,SHR(MULT16_16(a0, x0),1));
565          y3 = ADD32(y3,SHR(MULT16_16(a1, x0),1));
566
567          a0 = a[j+2];
568          a1 = a[j+3];
569          x0 = xx[N+j-i];
570
571          y0 = ADD32(y0,SHR(MULT16_16(a0, x0),1));
572          y1 = ADD32(y1,SHR(MULT16_16(a1, x0),1));
573          y2 = ADD32(y2,SHR(MULT16_16(a0, x1),1));
574          y3 = ADD32(y3,SHR(MULT16_16(a1, x1),1));
575       }
576       y[i] = y0;
577       y[i+1] = y1;
578       y[i+2] = y2;
579       y[i+3] = y3;
580    }
581
582    for (i = 0; i < M - 1; i += 2)
583       mem[i+1] = xx[i];
584 }
585
586 #ifdef NEW_ENHANCER
587
588 #if 0
589 float shift_filt[3][7] = {{-9.9369e-04, 3.1831e-02, -1.3889e-01, 6.0910e-01, 6.0910e-01, -1.3889e-01, 3.1831e-02},
590                           {-0.0029937, 0.0345613, -0.1350474, 0.8904793, 0.2714479, -0.0710304, 0.0135403},
591                           {0.0135403, -0.0710304, 0.2714479, 0.8904793, -0.1350474, 0.0345613,  -0.0029937}};
592 #else
593 float shift_filt[3][7] = {{-0.011915, 0.046995, -0.152373, 0.614108, 0.614108, -0.152373, 0.046995},
594                           {-0.0324855, 0.0859768, -0.2042986, 0.9640297, 0.2086420, -0.0302054, -0.0063646},
595                           {-0.0063646, -0.0302054, 0.2086420, 0.9640297, -0.2042986, 0.0859768, -0.0324855}};
596 #endif
597
598 int interp_pitch(
599 spx_word16_t *exc,          /*decoded excitation*/
600 spx_word16_t *interp,          /*decoded excitation*/
601 int pitch,               /*pitch period*/
602 int len
603 )
604 {
605    int i,j,k;
606    spx_word32_t corr[4][7];
607    spx_word32_t maxcorr;
608    int maxi, maxj;
609    for (i=0;i<7;i++)
610    {
611       corr[0][i] = inner_prod(exc, exc-pitch-3+i, len);
612    }
613    for (i=0;i<3;i++)
614    {
615       for (j=0;j<7;j++)
616       {
617          float tmp=0;
618          for (k=0;k<7;k++)
619          {
620             if (j+k-3 >= 0 && j+k-3 < 7)
621                tmp += corr[0][j+k-3]*shift_filt[i][k];
622          }
623          corr[i+1][j] = tmp;
624       }
625    }
626    maxi=maxj=-1;
627    maxcorr = -1e9;
628    for (i=0;i<4;i++)
629    {
630       for (j=0;j<7;j++)
631       {
632          if (corr[i][j] > maxcorr)
633          {
634             maxcorr = corr[i][j];
635             maxi=i;
636             maxj=j;
637          }
638       }
639    }
640    for (i=0;i<len;i++)
641    {
642       float tmp = 0;
643       if (maxi>0)
644       {
645          for (k=0;k<7;k++)
646          {
647             tmp += exc[i-(pitch-maxj+3)+k-3]*shift_filt[maxi-1][k];
648          }
649       } else {
650          tmp = exc[i-(pitch-maxj+3)];
651       }
652       interp[i] = tmp;
653    }
654    return pitch-maxj+3;
655 }
656       
657 void multicomb(
658 spx_sig_t *_exc,          /*decoded excitation*/
659 spx_word16_t *new_exc,      /*enhanced excitation*/
660 spx_coef_t *ak,           /*LPC filter coefs*/
661 int p,               /*LPC order*/
662 int nsf,             /*sub-frame size*/
663 int pitch,           /*pitch period*/
664 spx_word16_t *pitch_gain,   /*pitch gain (3-tap)*/
665 spx_word16_t  comb_gain,    /*gain of comb filter*/
666 char *stack
667 )
668 {
669    int i; 
670    spx_word16_t iexc[3*nsf];
671    float old_ener, new_ener;
672    int corr_pitch;
673    
674    int nol_pitch[6];
675    spx_word16_t nol_pitch_coef[6];
676    spx_word16_t ol_pitch_coef;
677    spx_word16_t *exc;
678    
679 #ifdef FIXED_POINT
680    VARDECL(spx_word16_t *exc2);
681    /* FIXME: Can it get uglier than that??? */
682    ALLOC(exc2, 510, spx_word16_t);
683    for (i=0;i<510;i++)
684       exc2[i] = 0;
685    exc = exc2+300;
686    for (i=-280;i<200;i++)
687       exc[i] = PSHR32(_exc[i], SIG_SHIFT);
688 #else
689    exc = _exc;
690 #endif
691    open_loop_nbest_pitch(exc, 20, 120, nsf, 
692                          nol_pitch, nol_pitch_coef, 6, stack);
693    corr_pitch=nol_pitch[0];
694    ol_pitch_coef = nol_pitch_coef[0];
695    /*Try to remove pitch multiples*/
696    for (i=1;i<6;i++)
697    {
698 #ifdef FIXED_POINT
699       if ((nol_pitch_coef[i]>MULT16_16_Q15(nol_pitch_coef[0],19661)) && 
700 #else
701       if ((nol_pitch_coef[i]>.6*nol_pitch_coef[0]) && 
702 #endif
703          (ABS(2*nol_pitch[i]-corr_pitch)<=2 || ABS(3*nol_pitch[i]-corr_pitch)<=3 || 
704          ABS(4*nol_pitch[i]-corr_pitch)<=4 || ABS(5*nol_pitch[i]-corr_pitch)<=5))
705       {
706          /*ol_pitch_coef=nol_pitch_coef[i];*/
707          corr_pitch = nol_pitch[i];
708       }
709    }
710    interp_pitch(exc, iexc, corr_pitch, 80);
711    if (corr_pitch>40)
712       interp_pitch(exc, iexc+nsf, 2*corr_pitch, 80);
713    else
714       interp_pitch(exc, iexc+nsf, -corr_pitch, 80);
715
716    interp_pitch(exc, iexc+2*nsf, 2*corr_pitch, 80);
717    
718    /*printf ("%d %d %f\n", pitch, corr_pitch, max_corr*ener_1);*/
719    
720    float gg1 = sqrt((1.+inner_prod(exc,exc,nsf))/(1000.+inner_prod(iexc,iexc,nsf)));
721    float gg2 = sqrt((1.+inner_prod(exc,exc,nsf))/(1000.+inner_prod(iexc+nsf,iexc+nsf,nsf)));
722    float pgain1 = inner_prod(iexc,exc,nsf)/sqrt(1000.+inner_prod(iexc,iexc,nsf)*1.*inner_prod(exc,exc,nsf));
723    float pgain2 = inner_prod(iexc+nsf,exc,nsf)/sqrt(1000.+inner_prod(iexc+nsf,iexc+nsf,nsf)*1.*inner_prod(exc,exc,nsf));
724    if (pgain1<0)
725       pgain1=0;
726    if (pgain2<0)
727       pgain2=0;
728    if (pgain1>.99)
729       pgain1=.99;
730    if (pgain2>.99)
731       pgain2=.99;
732    float c1, c2;
733    float g1, g2;
734    float ngain;
735    if (comb_gain>0)
736    {
737 #ifdef FIXED_POINT
738       c1 = .4*comb_gain/32768.+.07;
739 #else
740       c1 = .4*comb_gain+.07;
741 #endif
742       c2 = .5+1.72*(c1-.07);
743    } else 
744    {
745       c1=c2=0;
746    }
747    g1 = c1/pow(1-pgain1*pgain1, c2);
748    g2 = c1/pow(1-pgain2*pgain2, c2);
749    if (g1>1)
750       g1 = 1;
751    if (g2 > 1)
752       g2 = 1;
753    /*printf ("%d %f %f %f %f %d %d %d\n", corr_pitch, gg1, gg2, g1, g2, inner_prod(iexc+nsf,exc,nsf),inner_prod(iexc+nsf,iexc+nsf,nsf),inner_prod(exc,exc,nsf));*/
754    if (corr_pitch>40)
755    {
756       for (i=0;i<nsf;i++)
757          new_exc[i] = floor(.5+exc[i] + (.7*g1*gg1*iexc[i] + .3*g2*gg2*iexc[i+nsf]));
758    } else {
759       for (i=0;i<nsf;i++)
760          new_exc[i] = floor(.5+exc[i] + (.6*g1*gg1*iexc[i] + .6*g2*gg2*iexc[i+nsf]));
761    }
762    new_ener = compute_rms16(new_exc, nsf);
763    old_ener = compute_rms16(exc, nsf);
764    ngain = old_ener/(1.+new_ener);
765    for (i=0;i<nsf;i++)
766       new_exc[i] = floor(.5+new_exc[i]*ngain);
767 }
768 #endif
769
770 void comb_filter_mem_init (CombFilterMem *mem)
771 {
772    mem->last_pitch=0;
773    mem->last_pitch_gain[0]=mem->last_pitch_gain[1]=mem->last_pitch_gain[2]=0;
774    mem->smooth_gain=1;
775 }
776
777 #ifdef FIXED_POINT
778 #define COMB_STEP 32767
779 #else
780 #define COMB_STEP 1.0
781 #endif
782
783 void comb_filter(
784 spx_sig_t *exc,          /*decoded excitation*/
785 spx_word16_t *_new_exc,      /*enhanced excitation*/
786 spx_coef_t *ak,           /*LPC filter coefs*/
787 int p,               /*LPC order*/
788 int nsf,             /*sub-frame size*/
789 int pitch,           /*pitch period*/
790 spx_word16_t *pitch_gain,   /*pitch gain (3-tap)*/
791 spx_word16_t  comb_gain,    /*gain of comb filter*/
792 CombFilterMem *mem
793 )
794 {
795    int i;
796    spx_word16_t exc_energy=0, new_exc_energy=0;
797    spx_word16_t gain;
798    spx_word16_t step;
799    spx_word16_t fact;
800    /* FIXME: This is a temporary kludge */
801    spx_sig_t new_exc[40];
802    
803    /*Compute excitation amplitude prior to enhancement*/
804    exc_energy = compute_rms(exc, nsf);
805    /*for (i=0;i<nsf;i++)
806      exc_energy+=((float)exc[i])*exc[i];*/
807
808    /*Some gain adjustment if pitch is too high or if unvoiced*/
809 #ifdef FIXED_POINT
810    {
811       spx_word16_t g = gain_3tap_to_1tap(pitch_gain)+gain_3tap_to_1tap(mem->last_pitch_gain);
812       if (g > 166)
813          comb_gain = MULT16_16_Q15(DIV32_16(SHL32(EXTEND32(165),15),g), comb_gain);
814       if (g < 64)
815          comb_gain = MULT16_16_Q15(SHL16(g, 9), comb_gain);
816    }
817 #else
818    {
819       float g=0;
820       g = GAIN_SCALING_1*.5*(gain_3tap_to_1tap(pitch_gain)+gain_3tap_to_1tap(mem->last_pitch_gain));
821       if (g>1.3)
822          comb_gain*=1.3/g;
823       if (g<.5)
824          comb_gain*=2.*g;
825    }
826 #endif
827    step = DIV32(COMB_STEP, nsf);
828    fact=0;
829
830    /*Apply pitch comb-filter (filter out noise between pitch harmonics)*/
831    for (i=0;i<nsf;i++)
832    {
833       spx_word32_t exc1, exc2;
834
835       fact = ADD16(fact,step);
836       
837       exc1 = SHL32(MULT16_32_Q15(SHL16(pitch_gain[0],7),exc[i-pitch+1]) +
838                  MULT16_32_Q15(SHL16(pitch_gain[1],7),exc[i-pitch]) +
839                  MULT16_32_Q15(SHL16(pitch_gain[2],7),exc[i-pitch-1]) , 2);
840       exc2 = SHL32(MULT16_32_Q15(SHL16(mem->last_pitch_gain[0],7),exc[i-mem->last_pitch+1]) +
841                  MULT16_32_Q15(SHL16(mem->last_pitch_gain[1],7),exc[i-mem->last_pitch]) +
842                  MULT16_32_Q15(SHL16(mem->last_pitch_gain[2],7),exc[i-mem->last_pitch-1]),2);
843
844       new_exc[i] = exc[i] + MULT16_32_Q15(comb_gain, ADD32(MULT16_32_Q15(fact,exc1), MULT16_32_Q15(SUB16(COMB_STEP,fact), exc2)));
845    }
846
847    mem->last_pitch_gain[0] = pitch_gain[0];
848    mem->last_pitch_gain[1] = pitch_gain[1];
849    mem->last_pitch_gain[2] = pitch_gain[2];
850    mem->last_pitch = pitch;
851
852    /*Amplitude after enhancement*/
853    new_exc_energy = compute_rms(new_exc, nsf);
854
855    if (exc_energy > new_exc_energy)
856       exc_energy = new_exc_energy;
857    
858    if (new_exc_energy<1)
859       new_exc_energy=1;
860    if (exc_energy<1)
861       exc_energy=1;
862    gain = DIV32_16(SUB32(SHL32(exc_energy,15),1),new_exc_energy);
863
864 #ifdef FIXED_POINT
865    if (gain < 16384)
866       gain = 16384;
867 #else
868    if (gain < .5)
869       gain=.5;
870 #endif
871
872 #ifdef FIXED_POINT
873    for (i=0;i<nsf;i++)
874    {
875       mem->smooth_gain = ADD16(MULT16_16_Q15(31457,mem->smooth_gain), MULT16_16_Q15(1311,gain));
876       new_exc[i] = MULT16_32_Q15(mem->smooth_gain, new_exc[i]);
877    }
878 #else
879    for (i=0;i<nsf;i++)
880    {
881       mem->smooth_gain = .96*mem->smooth_gain + .04*gain;
882       new_exc[i] *= mem->smooth_gain;
883    }
884 #endif
885    for (i=0;i<nsf;i++)
886       _new_exc[i] = EXTRACT16(PSHR32(new_exc[i],SIG_SHIFT));
887 }