More ARM stuff
[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 <math.h>
40 #include "misc.h"
41 #include "math_approx.h"
42 #include "ltp.h"
43
44 #ifdef FIXED_POINT
45 void bw_lpc(spx_word16_t gamma, const spx_coef_t *lpc_in, spx_coef_t *lpc_out, int order)
46 {
47    int i;
48    spx_word16_t tmp=gamma;
49    lpc_out[0] = lpc_in[0];
50    for (i=1;i<order+1;i++)
51    {
52       lpc_out[i] = MULT16_16_P15(tmp,lpc_in[i]);
53       tmp = MULT16_16_P15(tmp, gamma);
54    }
55 }
56 #else
57 void bw_lpc(spx_word16_t gamma, const spx_coef_t *lpc_in, spx_coef_t *lpc_out, int order)
58 {
59    int i;
60    float tmp=1;
61    for (i=0;i<order+1;i++)
62    {
63       lpc_out[i] = tmp * lpc_in[i];
64       tmp *= gamma;
65    }
66 }
67
68 #endif
69
70
71 #ifdef FIXED_POINT
72
73 /* FIXME: These functions are ugly and probably introduce too much error */
74 void signal_mul(const spx_sig_t *x, spx_sig_t *y, spx_word32_t scale, int len)
75 {
76    int i;
77    for (i=0;i<len;i++)
78    {
79       y[i] = SHL(MULT16_32_Q14(SHR(x[i],7),scale),7);
80    }
81 }
82
83 void signal_div(const spx_sig_t *x, spx_sig_t *y, spx_word32_t scale, int len)
84 {
85    int i;
86    spx_word16_t scale_1;
87
88    scale = PSHR(scale, SIG_SHIFT);
89    if (scale<2)
90       scale_1 = 32767;
91    else
92       scale_1 = 32767/scale;
93    for (i=0;i<len;i++)
94    {
95       y[i] = MULT16_32_Q15(scale_1,x[i]);
96    }
97 }
98
99 #else
100
101 void signal_mul(const spx_sig_t *x, spx_sig_t *y, spx_word32_t scale, int len)
102 {
103    int i;
104    for (i=0;i<len;i++)
105       y[i] = scale*x[i];
106 }
107
108 void signal_div(const spx_sig_t *x, spx_sig_t *y, spx_word32_t scale, int len)
109 {
110    int i;
111    float scale_1 = 1/scale;
112    for (i=0;i<len;i++)
113       y[i] = scale_1*x[i];
114 }
115 #endif
116
117
118
119 #ifdef FIXED_POINT
120
121 int normalize16(const spx_sig_t *x, spx_word16_t *y, int max_scale, int len)
122 {
123    int i;
124    spx_sig_t max_val=1;
125    int sig_shift;
126    
127    for (i=0;i<len;i++)
128    {
129       spx_sig_t tmp = x[i];
130       if (tmp<0)
131          tmp = -tmp;
132       if (tmp >= max_val)
133          max_val = tmp;
134    }
135
136    sig_shift=0;
137    while (max_val>max_scale)
138    {
139       sig_shift++;
140       max_val >>= 1;
141    }
142
143    for (i=0;i<len;i++)
144       y[i] = SHR(x[i], sig_shift);
145    
146    return sig_shift;
147 }
148
149 spx_word16_t compute_rms(const spx_sig_t *x, int len)
150 {
151    int i;
152    spx_word32_t sum=0;
153    spx_sig_t max_val=1;
154    int sig_shift;
155
156    for (i=0;i<len;i++)
157    {
158       spx_sig_t tmp = x[i];
159       if (tmp<0)
160          tmp = -tmp;
161       if (tmp > max_val)
162          max_val = tmp;
163    }
164
165    sig_shift=0;
166    while (max_val>16383)
167    {
168       sig_shift++;
169       max_val >>= 1;
170    }
171
172    for (i=0;i<len;i+=4)
173    {
174       spx_word32_t sum2=0;
175       spx_word16_t tmp;
176       tmp = SHR(x[i],sig_shift);
177       sum2 += MULT16_16(tmp,tmp);
178       tmp = SHR(x[i+1],sig_shift);
179       sum2 += MULT16_16(tmp,tmp);
180       tmp = SHR(x[i+2],sig_shift);
181       sum2 += MULT16_16(tmp,tmp);
182       tmp = SHR(x[i+3],sig_shift);
183       sum2 += MULT16_16(tmp,tmp);
184       sum += SHR(sum2,6);
185    }
186    
187    return SHR(SHL((spx_word32_t)spx_sqrt(1+DIV32(sum,len)),(sig_shift+3)),SIG_SHIFT);
188 }
189
190 #ifdef ARM4_ASM
191 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)
192 {
193    int i,j;
194    spx_sig_t xi,yi,nyi;
195
196    for (i=0;i<N;i++)
197    {
198       int deadm, deadn, deadd, deadidx, x1, y1, dead1, dead2, dead3, dead4, dead5, dead6;
199       xi=SATURATE(x[i],805306368);
200       yi = SATURATE(ADD32(xi, SHL(mem[0],2)),805306368);
201       nyi = -yi;
202       y[i] = yi;
203       __asm__ __volatile__ (
204             "\tldrsh %6, [%1], #2\n"
205             "\tsmull %8, %9, %4, %6\n"
206             ".filterloop: \n"
207             "\tldrsh %6, [%2], #2\n"
208             "\tldr %10, [%0, #4]\n"
209             "\tmov %8, %8, lsr #15\n"
210             "\tsmull %7, %11, %5, %6\n"
211             "\tadd %8, %8, %9, lsl #17\n"
212             "\tldrsh %6, [%1], #2\n"
213             "\tadd %10, %10, %8\n"
214             "\tsmull %8, %9, %4, %6\n"
215             "\tadd %10, %10, %7, lsr #15\n"
216             "\tsubs %3, %3, #1\n"
217             "\tadd %10, %10, %11, lsl #17\n"
218             "\tstr %10, [%0], #4 \n"
219             "\t bne .filterloop\n"
220
221             "\tmov %8, %8, lsr #15\n"
222             "\tadd %10, %8, %9, lsl #17\n"
223             "\tldrsh %6, [%2], #2\n"
224             "\tsmull %8, %9, %5, %6\n"
225             "\tadd %10, %10, %8, lsr #15\n"
226             "\tadd %10, %10, %9, lsl #17\n"
227             "\tstr %10, [%0], #4 \n"
228
229          : "=r" (deadm), "=r" (deadn), "=r" (deadd), "=r" (deadidx),
230            "=r" (xi), "=r" (nyi), "=r" (dead1), "=r" (dead2),
231            "=r" (dead3), "=r" (dead4), "=r" (dead5), "=r" (dead6)
232          : "0" (mem), "1" (num+1), "2" (den+1), "3" (ord-1), "4" (xi), "5" (nyi)
233          : "cc", "memory");
234    
235    }
236 }
237
238 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)
239 {
240    int i,j;
241    spx_sig_t xi,yi,nyi;
242
243    for (i=0;i<N;i++)
244    {
245       int deadm, deadd, deadidx, dead1, dead2, dead3, dead4, dead5, dead6;
246       xi=SATURATE(x[i],805306368);
247       yi = SATURATE(ADD32(xi, SHL(mem[0],2)),805306368);
248       nyi = -yi;
249       y[i] = yi;
250       __asm__ __volatile__ (
251             ".iirloop: \n"
252
253             "\tldrsh %4, [%1], #2\n"
254             "\tsmull %5, %6, %3, %4\n"
255             "\tldr %7, [%0, #4]\n"
256
257             "\tmov %5, %5, lsr #15\n"
258             "\tadd %5, %5, %6, lsl #17\n"
259             "\tadd %7, %7, %5\n"
260             "\tstr %7, [%0], #4 \n"
261             "\tsubs %2, %2, #1\n"
262             "\t bne .iirloop\n"
263
264             "\tldrsh %4, [%1], #2\n"
265             "\tsmull %5, %6, %3, %4\n"
266
267             "\tmov %5, %5, lsr #15\n"
268             "\tadd %7, %5, %6, lsl #17\n"
269             "\tstr %7, [%0], #4 \n"
270          : "=r" (deadm), "=r" (deadd), "=r" (deadidx), "=r" (nyi),
271            "=r" (dead1), "=r" (dead2), "=r" (dead3), "=r" (dead4),
272            "=r" (dead5), "=r" (dead6)
273          : "0" (mem), "1" (den+1), "2" (ord-1), "3" (nyi)
274          : "cc", "memory");
275    
276    }
277 }
278
279 #else
280 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)
281 {
282    int i,j;
283    spx_sig_t xi,yi,nyi;
284
285    for (i=0;i<N;i++)
286    {
287       int xh,xl,yh,yl;
288       xi=SATURATE(x[i],805306368);
289       yi = SATURATE(ADD32(xi, SHL(mem[0],2)),805306368);
290       nyi = -yi;
291       xh = xi>>15; xl=xi&0x00007fff; yh = yi>>15; yl=yi&0x00007fff; 
292       for (j=0;j<ord-1;j++)
293       {
294          mem[j] = MAC16_32_Q15(MAC16_32_Q15(mem[j+1], num[j+1],xi), den[j+1],nyi);
295       }
296       mem[ord-1] = SUB32(MULT16_32_Q15(num[ord],xi), MULT16_32_Q15(den[ord],yi));
297       y[i] = yi;
298    }
299 }
300
301 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)
302 {
303    int i,j;
304    spx_word32_t xi,yi,nyi;
305
306    for (i=0;i<N;i++)
307    {
308       int yh,yl;
309       xi=SATURATE(x[i],805306368);
310       yi = SATURATE(xi + (mem[0]<<2),805306368);
311       nyi = -yi;
312       yh = yi>>15; yl=yi&0x00007fff; 
313       for (j=0;j<ord-1;j++)
314       {
315          mem[j] = MAC16_32_Q15(mem[j+1],den[j+1],nyi);
316       }
317       mem[ord-1] = - MULT16_32_Q15(den[ord],yi);
318       y[i] = yi;
319    }
320 }
321
322 #endif
323
324
325
326 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)
327 {
328    int i,j;
329    spx_word32_t xi,yi;
330
331    for (i=0;i<N;i++)
332    {
333       int xh,xl;
334       xi=SATURATE(x[i],805306368);
335       yi = xi + (mem[0]<<2);
336       xh = xi>>15; xl=xi&0x00007fff;
337       for (j=0;j<ord-1;j++)
338       {
339          mem[j] = MAC16_32_Q15(mem[j+1], num[j+1],xi);
340       }
341       mem[ord-1] = MULT16_32_Q15(num[ord],xi);
342       y[i] = SATURATE(yi,805306368);
343    }
344
345 }
346
347 #else
348
349
350
351 spx_word16_t compute_rms(const spx_sig_t *x, int len)
352 {
353    int i;
354    float sum=0;
355    for (i=0;i<len;i++)
356    {
357       sum += x[i]*x[i];
358    }
359    return sqrt(.1+sum/len);
360 }
361
362 #ifdef _USE_SSE
363 #include "filters_sse.h"
364 #else
365
366
367 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)
368 {
369    int i,j;
370    float xi,yi;
371    for (i=0;i<N;i++)
372    {
373       xi=x[i];
374       y[i] = num[0]*xi + mem[0];
375       yi=y[i];
376       for (j=0;j<ord-1;j++)
377       {
378          mem[j] = mem[j+1] + num[j+1]*xi - den[j+1]*yi;
379       }
380       mem[ord-1] = num[ord]*xi - den[ord]*yi;
381    }
382 }
383
384
385 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)
386 {
387    int i,j;
388    for (i=0;i<N;i++)
389    {
390       y[i] = x[i] + mem[0];
391       for (j=0;j<ord-1;j++)
392       {
393          mem[j] = mem[j+1] - den[j+1]*y[i];
394       }
395       mem[ord-1] = - den[ord]*y[i];
396    }
397 }
398
399
400 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)
401 {
402    int i,j;
403    float xi;
404    for (i=0;i<N;i++)
405    {
406       xi=x[i];
407       y[i] = num[0]*xi + mem[0];
408       for (j=0;j<ord-1;j++)
409       {
410          mem[j] = mem[j+1] + num[j+1]*xi;
411       }
412       mem[ord-1] = num[ord]*xi;
413    }
414 }
415 #endif
416
417 #endif
418
419
420 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)
421 {
422    int i;
423    spx_mem_t *mem = PUSH(stack,ord, spx_mem_t);
424    for (i=0;i<ord;i++)
425      mem[i]=0;
426    iir_mem2(xx, ak, y, N, ord, mem);
427    for (i=0;i<ord;i++)
428       mem[i]=0;
429    filter_mem2(y, awk1, awk2, y, N, ord, mem);
430 }
431
432 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)
433 {
434    int i;
435    spx_mem_t *mem = PUSH(stack,ord, spx_mem_t);
436    for (i=0;i<ord;i++)
437       mem[i]=0;
438    filter_mem2(xx, ak, awk1, y, N, ord, mem);
439    for (i=0;i<ord;i++)
440      mem[i]=0;
441    fir_mem2(y, awk2, y, N, ord, mem);
442 }
443
444 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)
445 {
446    int i,j,k,M2;
447    spx_word16_t *a;
448    spx_word16_t *x;
449    spx_word16_t *x2;
450    
451    a = PUSH(stack, M, spx_word16_t);
452    x = PUSH(stack, N+M-1, spx_word16_t);
453    x2=x+M-1;
454    M2=M>>1;
455    for (i=0;i<M;i++)
456       a[M-i-1]= aa[i];
457
458    for (i=0;i<M-1;i++)
459       x[i]=mem[M-i-2];
460    for (i=0;i<N;i++)
461       x[i+M-1]=SATURATE(PSHR(xx[i],1),16383);
462    for (i=0,k=0;i<N;i+=2,k++)
463    {
464       y1[k]=0;
465       y2[k]=0;
466       for (j=0;j<M2;j++)
467       {
468          y1[k]+=SHR(MULT16_16(a[j],ADD16(x[i+j],x2[i-j])),1);
469          y2[k]-=SHR(MULT16_16(a[j],SUB16(x[i+j],x2[i-j])),1);
470          j++;
471          y1[k]+=SHR(MULT16_16(a[j],ADD16(x[i+j],x2[i-j])),1);
472          y2[k]+=SHR(MULT16_16(a[j],SUB16(x[i+j],x2[i-j])),1);
473       }
474    }
475    for (i=0;i<M-1;i++)
476      mem[i]=SATURATE(PSHR(xx[N-i-1],1),16383);
477 }
478
479
480 /* By segher */
481 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)
482    /* assumptions:
483       all odd x[i] are zero -- well, actually they are left out of the array now
484       N and M are multiples of 4 */
485 {
486    int i, j;
487    spx_word16_t *xx;
488    
489    xx= PUSH(stack, M+N-1, spx_word16_t);
490
491    for (i = 0; i < N/2; i++)
492       xx[2*i] = SHR(x[N/2-1-i],SIG_SHIFT+1);
493    for (i = 0; i < M - 1; i += 2)
494       xx[N+i] = mem[i+1];
495
496    for (i = 0; i < N; i += 4) {
497       spx_sig_t y0, y1, y2, y3;
498       spx_word16_t x0;
499
500       y0 = y1 = y2 = y3 = 0;
501       x0 = xx[N-4-i];
502
503       for (j = 0; j < M; j += 4) {
504          spx_word16_t x1;
505          spx_word16_t a0, a1;
506
507          a0 = a[j];
508          a1 = a[j+1];
509          x1 = xx[N-2+j-i];
510
511          y0 += SHR(MULT16_16(a0, x1),1);
512          y1 += SHR(MULT16_16(a1, x1),1);
513          y2 += SHR(MULT16_16(a0, x0),1);
514          y3 += SHR(MULT16_16(a1, x0),1);
515
516          a0 = a[j+2];
517          a1 = a[j+3];
518          x0 = xx[N+j-i];
519
520          y0 += SHR(MULT16_16(a0, x0),1);
521          y1 += SHR(MULT16_16(a1, x0),1);
522          y2 += SHR(MULT16_16(a0, x1),1);
523          y3 += SHR(MULT16_16(a1, x1),1);
524       }
525       y[i] = y0;
526       y[i+1] = y1;
527       y[i+2] = y2;
528       y[i+3] = y3;
529    }
530
531    for (i = 0; i < M - 1; i += 2)
532       mem[i+1] = xx[i];
533 }
534
535
536
537 void comb_filter_mem_init (CombFilterMem *mem)
538 {
539    mem->last_pitch=0;
540    mem->last_pitch_gain[0]=mem->last_pitch_gain[1]=mem->last_pitch_gain[2]=0;
541    mem->smooth_gain=1;
542 }
543
544 #ifdef FIXED_POINT
545 #define COMB_STEP 32767
546 #else
547 #define COMB_STEP 1.0
548 #endif
549
550 void comb_filter(
551 spx_sig_t *exc,          /*decoded excitation*/
552 spx_sig_t *new_exc,      /*enhanced excitation*/
553 spx_coef_t *ak,           /*LPC filter coefs*/
554 int p,               /*LPC order*/
555 int nsf,             /*sub-frame size*/
556 int pitch,           /*pitch period*/
557 spx_word16_t *pitch_gain,   /*pitch gain (3-tap)*/
558 spx_word16_t  comb_gain,    /*gain of comb filter*/
559 CombFilterMem *mem
560 )
561 {
562    int i;
563    spx_word16_t exc_energy=0, new_exc_energy=0;
564    float gain;
565    spx_word16_t step;
566    spx_word16_t fact;
567
568    /*Compute excitation amplitude prior to enhancement*/
569    exc_energy = compute_rms(exc, nsf);
570    /*for (i=0;i<nsf;i++)
571      exc_energy+=((float)exc[i])*exc[i];*/
572
573    /*Some gain adjustment if pitch is too high or if unvoiced*/
574    {
575       float g=0;
576       g = GAIN_SCALING_1*.5*(gain_3tap_to_1tap(pitch_gain)+gain_3tap_to_1tap(mem->last_pitch_gain));
577       if (g>1.3)
578          comb_gain*=1.3/g;
579       if (g<.5)
580          comb_gain*=2.*g;
581    }
582    step = DIV32(COMB_STEP, nsf);
583    fact=0;
584
585    /*Apply pitch comb-filter (filter out noise between pitch harmonics)*/
586    for (i=0;i<nsf;i++)
587    {
588       spx_word32_t exc1, exc2;
589
590       fact += step;
591       
592       exc1 = SHL(MULT16_32_Q15(SHL(pitch_gain[0],7),exc[i-pitch+1]) +
593                  MULT16_32_Q15(SHL(pitch_gain[1],7),exc[i-pitch]) +
594                  MULT16_32_Q15(SHL(pitch_gain[2],7),exc[i-pitch-1]) , 2);
595       exc2 = SHL(MULT16_32_Q15(SHL(mem->last_pitch_gain[0],7),exc[i-mem->last_pitch+1]) +
596                  MULT16_32_Q15(SHL(mem->last_pitch_gain[1],7),exc[i-mem->last_pitch]) +
597                  MULT16_32_Q15(SHL(mem->last_pitch_gain[2],7),exc[i-mem->last_pitch-1]),2);
598
599       new_exc[i] = exc[i] + MULT16_32_Q15(comb_gain,MULT16_32_Q15(fact,exc1)  + MULT16_32_Q15(SUB16(COMB_STEP,fact), exc2));
600    }
601
602    mem->last_pitch_gain[0] = pitch_gain[0];
603    mem->last_pitch_gain[1] = pitch_gain[1];
604    mem->last_pitch_gain[2] = pitch_gain[2];
605    mem->last_pitch = pitch;
606
607    /*Amplitude after enhancement*/
608    new_exc_energy = compute_rms(new_exc, nsf);
609
610    /*Compute scaling factor and normalize energy*/
611    gain = (exc_energy)/(.1+new_exc_energy);
612    if (gain < .5)
613       gain=.5;
614    if (gain>.9999)
615       gain=.9999;
616
617 #ifdef FIXED_POINT
618    {
619       spx_word16_t gg = gain*32768;
620       for (i=0;i<nsf;i++)
621    {
622       mem->smooth_gain = MULT16_16_Q15(31457,mem->smooth_gain) + MULT16_16_Q15(1311,gg);
623       new_exc[i] = MULT16_32_Q15(mem->smooth_gain, new_exc[i]);
624    }
625    }
626 #else
627    for (i=0;i<nsf;i++)
628    {
629       mem->smooth_gain = .96*mem->smooth_gain + .04*gain;
630       new_exc[i] *= mem->smooth_gain;
631    }
632 #endif
633 }