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