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