Just a bunch of (scalar) float ops left in the new enhancer
[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 #ifdef FIXED_POINT
589 #if 0
590 spx_word16_t shift_filt[3][7] = {{-33,    1043,   -4551,   19959,   19959,   -4551,    1043},
591                                  {-98,    1133,   -4425,   29179,    8895,   -2328,     444},
592                                  {444,   -2328,    8895,   29179,   -4425,    1133,     -98}};
593 #else
594 spx_word16_t shift_filt[3][7] = {{-390,    1540,   -4993,   20123,   20123,   -4993,    1540},
595                                 {-1064,    2817,   -6694,   31589,    6837,    -990,    -209},
596                                  {-209,    -990,    6837,   31589,   -6694,    2817,   -1064}};
597 #endif
598 #else
599 #if 0
600 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},
601                           {-0.0029937, 0.0345613, -0.1350474, 0.8904793, 0.2714479, -0.0710304, 0.0135403},
602                           {0.0135403, -0.0710304, 0.2714479, 0.8904793, -0.1350474, 0.0345613,  -0.0029937}};
603 #else
604 float shift_filt[3][7] = {{-0.011915, 0.046995, -0.152373, 0.614108, 0.614108, -0.152373, 0.046995},
605                           {-0.0324855, 0.0859768, -0.2042986, 0.9640297, 0.2086420, -0.0302054, -0.0063646},
606                           {-0.0063646, -0.0302054, 0.2086420, 0.9640297, -0.2042986, 0.0859768, -0.0324855}};
607 #endif
608 #endif
609
610 int interp_pitch(
611 spx_word16_t *exc,          /*decoded excitation*/
612 spx_word16_t *interp,          /*decoded excitation*/
613 int pitch,               /*pitch period*/
614 int len
615 )
616 {
617    int i,j,k;
618    spx_word32_t corr[4][7];
619    spx_word32_t maxcorr;
620    int maxi, maxj;
621    for (i=0;i<7;i++)
622    {
623       corr[0][i] = inner_prod(exc, exc-pitch-3+i, len);
624    }
625    for (i=0;i<3;i++)
626    {
627       for (j=0;j<7;j++)
628       {
629          spx_word32_t tmp=0;
630          for (k=0;k<7;k++)
631          {
632             if (j+k-3 >= 0 && j+k-3 < 7)
633                tmp += MULT16_32_Q15(shift_filt[i][k],corr[0][j+k-3]);
634          }
635          corr[i+1][j] = tmp;
636       }
637    }
638    maxi=maxj=0;
639    maxcorr = corr[0][0];
640    for (i=0;i<4;i++)
641    {
642       for (j=0;j<7;j++)
643       {
644          if (corr[i][j] > maxcorr)
645          {
646             maxcorr = corr[i][j];
647             maxi=i;
648             maxj=j;
649          }
650       }
651    }
652    for (i=0;i<len;i++)
653    {
654       spx_word32_t tmp = 0;
655       if (maxi>0)
656       {
657          for (k=0;k<7;k++)
658          {
659             tmp += MULT16_16(exc[i-(pitch-maxj+3)+k-3],shift_filt[maxi-1][k]);
660          }
661       } else {
662          tmp = SHL32(exc[i-(pitch-maxj+3)],15);
663       }
664       interp[i] = PSHR32(tmp,15);
665    }
666    return pitch-maxj+3;
667 }
668       
669 #ifdef FIXED_POINT
670 #define GSCALE 256.
671 #else
672 #define GSCALE 1.
673 #endif
674
675 void multicomb(
676 spx_sig_t *_exc,          /*decoded excitation*/
677 spx_word16_t *new_exc,      /*enhanced excitation*/
678 spx_coef_t *ak,           /*LPC filter coefs*/
679 int p,               /*LPC order*/
680 int nsf,             /*sub-frame size*/
681 int pitch,           /*pitch period*/
682 spx_word16_t *pitch_gain,   /*pitch gain (3-tap)*/
683 spx_word16_t  comb_gain,    /*gain of comb filter*/
684 char *stack
685 )
686 {
687    int i; 
688    spx_word16_t iexc[3*nsf];
689    spx_word16_t old_ener, new_ener;
690    int corr_pitch;
691    
692    int nol_pitch[6];
693    spx_word16_t nol_pitch_coef[6];
694    spx_word16_t ol_pitch_coef;
695    spx_word16_t *exc;
696    spx_word16_t iexc0_mag, iexc1_mag, exc_mag;
697    spx_word32_t corr0, corr1;
698    spx_word16_t gain0, gain1;
699 #ifdef FIXED_POINT
700    VARDECL(spx_word16_t *exc2);
701    /* FIXME: Can it get uglier than that??? */
702    ALLOC(exc2, 510, spx_word16_t);
703    for (i=0;i<510;i++)
704       exc2[i] = 0;
705    exc = exc2+300;
706    for (i=-280;i<200;i++)
707       exc[i] = PSHR32(_exc[i], SIG_SHIFT);
708 #else
709    exc = _exc;
710 #endif
711    open_loop_nbest_pitch(exc, 20, 120, nsf, 
712                          nol_pitch, nol_pitch_coef, 6, stack);
713    corr_pitch=nol_pitch[0];
714    ol_pitch_coef = nol_pitch_coef[0];
715    /*Try to remove pitch multiples*/
716    for (i=1;i<6;i++)
717    {
718 #ifdef FIXED_POINT
719       if ((nol_pitch_coef[i]>MULT16_16_Q15(nol_pitch_coef[0],19661)) && 
720 #else
721       if ((nol_pitch_coef[i]>.6*nol_pitch_coef[0]) && 
722 #endif
723          (ABS(2*nol_pitch[i]-corr_pitch)<=2 || ABS(3*nol_pitch[i]-corr_pitch)<=3 || 
724          ABS(4*nol_pitch[i]-corr_pitch)<=4 || ABS(5*nol_pitch[i]-corr_pitch)<=5))
725       {
726          corr_pitch = nol_pitch[i];
727       }
728    }
729    interp_pitch(exc, iexc, corr_pitch, 80);
730    if (corr_pitch>40)
731       interp_pitch(exc, iexc+nsf, 2*corr_pitch, 80);
732    else
733       interp_pitch(exc, iexc+nsf, -corr_pitch, 80);
734
735    interp_pitch(exc, iexc+2*nsf, 2*corr_pitch, 80);
736    
737    /*printf ("%d %d %f\n", pitch, corr_pitch, max_corr*ener_1);*/
738    iexc0_mag = spx_sqrt(1000+inner_prod(iexc,iexc,nsf));
739    iexc1_mag = spx_sqrt(1000+inner_prod(iexc+nsf,iexc+nsf,nsf));
740    exc_mag = spx_sqrt(1+inner_prod(exc,exc,nsf));
741    corr0  = inner_prod(iexc,exc,nsf);
742    corr1 = inner_prod(iexc+nsf,exc,nsf);
743    float gg1 = 1.*exc_mag/iexc0_mag;
744    float gg2 = 1.*exc_mag/iexc1_mag;
745    float pgain1 = 1.*corr0/iexc0_mag/exc_mag;
746    float pgain2 = 1.*corr1/iexc1_mag/exc_mag;
747    if (pgain1<0)
748       pgain1=0;
749    if (pgain2<0)
750       pgain2=0;
751    if (pgain1>.99)
752       pgain1=.99;
753    if (pgain2>.99)
754       pgain2=.99;
755    float c1, c2;
756    float g1, g2;
757    float ngain;
758    if (comb_gain>0)
759    {
760 #ifdef FIXED_POINT
761       c1 = .4*comb_gain/32768.+.07;
762 #else
763       c1 = .4*comb_gain+.07;
764 #endif
765       c2 = .5+1.72*(c1-.07);
766    } else 
767    {
768       c1=c2=0;
769    }
770    g1 = c1/pow(1-pgain1*pgain1, c2);
771    g2 = c1/pow(1-pgain2*pgain2, c2);
772    if (g1>1)
773       g1 = 1;
774    if (g2 > 1)
775       g2 = 1;
776    /*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));*/
777    if (corr_pitch>40)
778    {
779       gain0 = GSCALE*.7*g1*gg1;
780       gain1 = GSCALE*.3*g2*gg2;
781    } else {
782       gain0 = GSCALE*.6*g1*gg1;
783       gain1 = GSCALE*.6*g2*gg2;
784    }
785    for (i=0;i<nsf;i++)
786       new_exc[i] = ADD16(exc[i], EXTRACT16(PSHR32(ADD32(MULT16_16(gain0,iexc[i]), MULT16_16(gain1,iexc[i+nsf])),8)));
787    new_ener = compute_rms16(new_exc, nsf);
788    old_ener = compute_rms16(exc, nsf);
789    
790    if (old_ener > new_ener)
791       old_ener = new_ener;
792
793    if (new_ener<1)
794       new_ener=1;
795    if (old_ener<1)
796       old_ener=1;
797    ngain = DIV32_16(SUB32(SHL32(old_ener,15),1),new_ener);
798    for (i=0;i<nsf;i++)
799       new_exc[i] = MULT16_16_Q15(ngain, new_exc[i]);
800 }
801 #endif
802
803 void comb_filter_mem_init (CombFilterMem *mem)
804 {
805    mem->last_pitch=0;
806    mem->last_pitch_gain[0]=mem->last_pitch_gain[1]=mem->last_pitch_gain[2]=0;
807    mem->smooth_gain=1;
808 }
809
810 #ifdef FIXED_POINT
811 #define COMB_STEP 32767
812 #else
813 #define COMB_STEP 1.0
814 #endif
815
816 void comb_filter(
817 spx_sig_t *exc,          /*decoded excitation*/
818 spx_word16_t *_new_exc,      /*enhanced excitation*/
819 spx_coef_t *ak,           /*LPC filter coefs*/
820 int p,               /*LPC order*/
821 int nsf,             /*sub-frame size*/
822 int pitch,           /*pitch period*/
823 spx_word16_t *pitch_gain,   /*pitch gain (3-tap)*/
824 spx_word16_t  comb_gain,    /*gain of comb filter*/
825 CombFilterMem *mem
826 )
827 {
828    int i;
829    spx_word16_t exc_energy=0, new_exc_energy=0;
830    spx_word16_t gain;
831    spx_word16_t step;
832    spx_word16_t fact;
833    /* FIXME: This is a temporary kludge */
834    spx_sig_t new_exc[40];
835    
836    /*Compute excitation amplitude prior to enhancement*/
837    exc_energy = compute_rms(exc, nsf);
838    /*for (i=0;i<nsf;i++)
839      exc_energy+=((float)exc[i])*exc[i];*/
840
841    /*Some gain adjustment if pitch is too high or if unvoiced*/
842 #ifdef FIXED_POINT
843    {
844       spx_word16_t g = gain_3tap_to_1tap(pitch_gain)+gain_3tap_to_1tap(mem->last_pitch_gain);
845       if (g > 166)
846          comb_gain = MULT16_16_Q15(DIV32_16(SHL32(EXTEND32(165),15),g), comb_gain);
847       if (g < 64)
848          comb_gain = MULT16_16_Q15(SHL16(g, 9), comb_gain);
849    }
850 #else
851    {
852       float g=0;
853       g = GAIN_SCALING_1*.5*(gain_3tap_to_1tap(pitch_gain)+gain_3tap_to_1tap(mem->last_pitch_gain));
854       if (g>1.3)
855          comb_gain*=1.3/g;
856       if (g<.5)
857          comb_gain*=2.*g;
858    }
859 #endif
860    step = DIV32(COMB_STEP, nsf);
861    fact=0;
862
863    /*Apply pitch comb-filter (filter out noise between pitch harmonics)*/
864    for (i=0;i<nsf;i++)
865    {
866       spx_word32_t exc1, exc2;
867
868       fact = ADD16(fact,step);
869       
870       exc1 = SHL32(MULT16_32_Q15(SHL16(pitch_gain[0],7),exc[i-pitch+1]) +
871                  MULT16_32_Q15(SHL16(pitch_gain[1],7),exc[i-pitch]) +
872                  MULT16_32_Q15(SHL16(pitch_gain[2],7),exc[i-pitch-1]) , 2);
873       exc2 = SHL32(MULT16_32_Q15(SHL16(mem->last_pitch_gain[0],7),exc[i-mem->last_pitch+1]) +
874                  MULT16_32_Q15(SHL16(mem->last_pitch_gain[1],7),exc[i-mem->last_pitch]) +
875                  MULT16_32_Q15(SHL16(mem->last_pitch_gain[2],7),exc[i-mem->last_pitch-1]),2);
876
877       new_exc[i] = exc[i] + MULT16_32_Q15(comb_gain, ADD32(MULT16_32_Q15(fact,exc1), MULT16_32_Q15(SUB16(COMB_STEP,fact), exc2)));
878    }
879
880    mem->last_pitch_gain[0] = pitch_gain[0];
881    mem->last_pitch_gain[1] = pitch_gain[1];
882    mem->last_pitch_gain[2] = pitch_gain[2];
883    mem->last_pitch = pitch;
884
885    /*Amplitude after enhancement*/
886    new_exc_energy = compute_rms(new_exc, nsf);
887
888    if (exc_energy > new_exc_energy)
889       exc_energy = new_exc_energy;
890    
891    if (new_exc_energy<1)
892       new_exc_energy=1;
893    if (exc_energy<1)
894       exc_energy=1;
895    gain = DIV32_16(SUB32(SHL32(exc_energy,15),1),new_exc_energy);
896
897 #ifdef FIXED_POINT
898    if (gain < 16384)
899       gain = 16384;
900 #else
901    if (gain < .5)
902       gain=.5;
903 #endif
904
905 #ifdef FIXED_POINT
906    for (i=0;i<nsf;i++)
907    {
908       mem->smooth_gain = ADD16(MULT16_16_Q15(31457,mem->smooth_gain), MULT16_16_Q15(1311,gain));
909       new_exc[i] = MULT16_32_Q15(mem->smooth_gain, new_exc[i]);
910    }
911 #else
912    for (i=0;i<nsf;i++)
913    {
914       mem->smooth_gain = .96*mem->smooth_gain + .04*gain;
915       new_exc[i] *= mem->smooth_gain;
916    }
917 #endif
918    for (i=0;i<nsf;i++)
919       _new_exc[i] = EXTRACT16(PSHR32(new_exc[i],SIG_SHIFT));
920 }