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