050d4e4540953f8036d8f8a413af1d3675e7cfa9
[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             
207             ".filterloop: \n"
208             "\tldrsh %6, [%1], #2\n"
209             "\tsmull %8, %9, %4, %6\n"
210             ".filterloop: \n"
211             "\tldrsh %6, [%2], #2\n"
212             "\tldr %10, [%0, #4]\n"
213             "\tmov %8, %8, lsr #15\n"
214             "\tsmull %7, %11, %5, %6\n"
215             "\tadd %8, %8, %9, lsl #17\n"
216             "\tldrsh %6, [%1], #2\n"
217             "\tadd %10, %10, %8\n"
218             "\tsmull %8, %9, %4, %6\n"
219             "\tadd %10, %10, %7, lsr #15\n"
220             "\tsubs %3, %3, #1\n"
221             "\tadd %10, %10, %11, lsl #17\n"
222             "\tstr %10, [%0], #4 \n"
223             "\t bne .filterloop\n"
224
225             "\tmov %8, %8, lsr #15\n"
226             "\tadd %10, %8, %9, lsl #17\n"
227             "\tldrsh %6, [%2], #2\n"
228             "\tsmull %8, %9, %5, %6\n"
229             "\tadd %10, %10, %8, lsr #15\n"
230             "\tadd %10, %10, %9, lsl #17\n"
231             "\tstr %10, [%0], #4 \n"
232
233          : "=r" (deadm), "=r" (deadn), "=r" (deadd), "=r" (deadidx),
234            "=r" (xi), "=r" (nyi), "=r" (dead1), "=r" (dead2),
235            "=r" (dead3), "=r" (dead4), "=r" (dead5), "=r" (dead6)
236          : "0" (mem), "1" (num+1), "2" (den+1), "3" (ord-1), "4" (xi), "5" (nyi)
237          : "cc", "memory");
238    
239    }
240 }
241 #else
242 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)
243 {
244    int i,j;
245    spx_sig_t xi,yi,nyi;
246
247    for (i=0;i<N;i++)
248    {
249       int xh,xl,yh,yl;
250       xi=SATURATE(x[i],805306368);
251       yi = SATURATE(ADD32(xi, SHL(mem[0],2)),805306368);
252       nyi = -yi;
253       xh = xi>>15; xl=xi&0x00007fff; yh = yi>>15; yl=yi&0x00007fff; 
254       for (j=0;j<ord-1;j++)
255       {
256          mem[j] = MAC16_32_Q15(MAC16_32_Q15(mem[j+1], num[j+1],xi), den[j+1],nyi);
257       }
258       mem[ord-1] = SUB32(MULT16_32_Q15(num[ord],xi), MULT16_32_Q15(den[ord],yi));
259       y[i] = yi;
260    }
261 }
262 #endif
263
264 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)
265 {
266    int i,j;
267    spx_word32_t xi,yi,nyi;
268
269    for (i=0;i<N;i++)
270    {
271       int yh,yl;
272       xi=SATURATE(x[i],805306368);
273       yi = SATURATE(xi + (mem[0]<<2),805306368);
274       nyi = -yi;
275       yh = yi>>15; yl=yi&0x00007fff; 
276       for (j=0;j<ord-1;j++)
277       {
278          mem[j] = MAC16_32_Q15(mem[j+1],den[j+1],nyi);
279       }
280       mem[ord-1] = - MULT16_32_Q15(den[ord],yi);
281       y[i] = yi;
282    }
283 }
284
285
286 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)
287 {
288    int i,j;
289    spx_word32_t xi,yi;
290
291    for (i=0;i<N;i++)
292    {
293       int xh,xl;
294       xi=SATURATE(x[i],805306368);
295       yi = xi + (mem[0]<<2);
296       xh = xi>>15; xl=xi&0x00007fff;
297       for (j=0;j<ord-1;j++)
298       {
299          mem[j] = MAC16_32_Q15(mem[j+1], num[j+1],xi);
300       }
301       mem[ord-1] = MULT16_32_Q15(num[ord],xi);
302       y[i] = SATURATE(yi,805306368);
303    }
304
305 }
306
307 #else
308
309
310
311 spx_word16_t compute_rms(const spx_sig_t *x, int len)
312 {
313    int i;
314    float sum=0;
315    for (i=0;i<len;i++)
316    {
317       sum += x[i]*x[i];
318    }
319    return sqrt(.1+sum/len);
320 }
321
322 #ifdef _USE_SSE
323 #include "filters_sse.h"
324 #else
325
326
327 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)
328 {
329    int i,j;
330    float xi,yi;
331    for (i=0;i<N;i++)
332    {
333       xi=x[i];
334       y[i] = num[0]*xi + mem[0];
335       yi=y[i];
336       for (j=0;j<ord-1;j++)
337       {
338          mem[j] = mem[j+1] + num[j+1]*xi - den[j+1]*yi;
339       }
340       mem[ord-1] = num[ord]*xi - den[ord]*yi;
341    }
342 }
343
344
345 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)
346 {
347    int i,j;
348    for (i=0;i<N;i++)
349    {
350       y[i] = x[i] + mem[0];
351       for (j=0;j<ord-1;j++)
352       {
353          mem[j] = mem[j+1] - den[j+1]*y[i];
354       }
355       mem[ord-1] = - den[ord]*y[i];
356    }
357 }
358
359
360 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)
361 {
362    int i,j;
363    float xi;
364    for (i=0;i<N;i++)
365    {
366       xi=x[i];
367       y[i] = num[0]*xi + mem[0];
368       for (j=0;j<ord-1;j++)
369       {
370          mem[j] = mem[j+1] + num[j+1]*xi;
371       }
372       mem[ord-1] = num[ord]*xi;
373    }
374 }
375 #endif
376
377 #endif
378
379
380 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)
381 {
382    int i;
383    spx_mem_t *mem = PUSH(stack,ord, spx_mem_t);
384    for (i=0;i<ord;i++)
385      mem[i]=0;
386    iir_mem2(xx, ak, y, N, ord, mem);
387    for (i=0;i<ord;i++)
388       mem[i]=0;
389    filter_mem2(y, awk1, awk2, y, N, ord, mem);
390 }
391
392 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)
393 {
394    int i;
395    spx_mem_t *mem = PUSH(stack,ord, spx_mem_t);
396    for (i=0;i<ord;i++)
397       mem[i]=0;
398    filter_mem2(xx, ak, awk1, y, N, ord, mem);
399    for (i=0;i<ord;i++)
400      mem[i]=0;
401    fir_mem2(y, awk2, y, N, ord, mem);
402 }
403
404 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)
405 {
406    int i,j,k,M2;
407    spx_word16_t *a;
408    spx_word16_t *x;
409    spx_word16_t *x2;
410    
411    a = PUSH(stack, M, spx_word16_t);
412    x = PUSH(stack, N+M-1, spx_word16_t);
413    x2=x+M-1;
414    M2=M>>1;
415    for (i=0;i<M;i++)
416       a[M-i-1]= aa[i];
417
418    for (i=0;i<M-1;i++)
419       x[i]=mem[M-i-2];
420    for (i=0;i<N;i++)
421       x[i+M-1]=SATURATE(PSHR(xx[i],1),16383);
422    for (i=0,k=0;i<N;i+=2,k++)
423    {
424       y1[k]=0;
425       y2[k]=0;
426       for (j=0;j<M2;j++)
427       {
428          y1[k]+=SHR(MULT16_16(a[j],ADD16(x[i+j],x2[i-j])),1);
429          y2[k]-=SHR(MULT16_16(a[j],SUB16(x[i+j],x2[i-j])),1);
430          j++;
431          y1[k]+=SHR(MULT16_16(a[j],ADD16(x[i+j],x2[i-j])),1);
432          y2[k]+=SHR(MULT16_16(a[j],SUB16(x[i+j],x2[i-j])),1);
433       }
434    }
435    for (i=0;i<M-1;i++)
436      mem[i]=SATURATE(PSHR(xx[N-i-1],1),16383);
437 }
438
439
440 /* By segher */
441 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)
442    /* assumptions:
443       all odd x[i] are zero -- well, actually they are left out of the array now
444       N and M are multiples of 4 */
445 {
446    int i, j;
447    spx_word16_t *xx;
448    
449    xx= PUSH(stack, M+N-1, spx_word16_t);
450
451    for (i = 0; i < N/2; i++)
452       xx[2*i] = SHR(x[N/2-1-i],SIG_SHIFT+1);
453    for (i = 0; i < M - 1; i += 2)
454       xx[N+i] = mem[i+1];
455
456    for (i = 0; i < N; i += 4) {
457       spx_sig_t y0, y1, y2, y3;
458       spx_word16_t x0;
459
460       y0 = y1 = y2 = y3 = 0;
461       x0 = xx[N-4-i];
462
463       for (j = 0; j < M; j += 4) {
464          spx_word16_t x1;
465          spx_word16_t a0, a1;
466
467          a0 = a[j];
468          a1 = a[j+1];
469          x1 = xx[N-2+j-i];
470
471          y0 += SHR(MULT16_16(a0, x1),1);
472          y1 += SHR(MULT16_16(a1, x1),1);
473          y2 += SHR(MULT16_16(a0, x0),1);
474          y3 += SHR(MULT16_16(a1, x0),1);
475
476          a0 = a[j+2];
477          a1 = a[j+3];
478          x0 = xx[N+j-i];
479
480          y0 += SHR(MULT16_16(a0, x0),1);
481          y1 += SHR(MULT16_16(a1, x0),1);
482          y2 += SHR(MULT16_16(a0, x1),1);
483          y3 += SHR(MULT16_16(a1, x1),1);
484       }
485       y[i] = y0;
486       y[i+1] = y1;
487       y[i+2] = y2;
488       y[i+3] = y3;
489    }
490
491    for (i = 0; i < M - 1; i += 2)
492       mem[i+1] = xx[i];
493 }
494
495
496
497 void comb_filter_mem_init (CombFilterMem *mem)
498 {
499    mem->last_pitch=0;
500    mem->last_pitch_gain[0]=mem->last_pitch_gain[1]=mem->last_pitch_gain[2]=0;
501    mem->smooth_gain=1;
502 }
503
504 #ifdef FIXED_POINT
505 #define COMB_STEP 32767
506 #else
507 #define COMB_STEP 1.0
508 #endif
509
510 void comb_filter(
511 spx_sig_t *exc,          /*decoded excitation*/
512 spx_sig_t *new_exc,      /*enhanced excitation*/
513 spx_coef_t *ak,           /*LPC filter coefs*/
514 int p,               /*LPC order*/
515 int nsf,             /*sub-frame size*/
516 int pitch,           /*pitch period*/
517 spx_word16_t *pitch_gain,   /*pitch gain (3-tap)*/
518 spx_word16_t  comb_gain,    /*gain of comb filter*/
519 CombFilterMem *mem
520 )
521 {
522    int i;
523    spx_word16_t exc_energy=0, new_exc_energy=0;
524    float gain;
525    spx_word16_t step;
526    spx_word16_t fact;
527
528    /*Compute excitation amplitude prior to enhancement*/
529    exc_energy = compute_rms(exc, nsf);
530    /*for (i=0;i<nsf;i++)
531      exc_energy+=((float)exc[i])*exc[i];*/
532
533    /*Some gain adjustment if pitch is too high or if unvoiced*/
534    {
535       float g=0;
536       g = GAIN_SCALING_1*.5*(gain_3tap_to_1tap(pitch_gain)+gain_3tap_to_1tap(mem->last_pitch_gain));
537       if (g>1.3)
538          comb_gain*=1.3/g;
539       if (g<.5)
540          comb_gain*=2.*g;
541    }
542    step = DIV32(COMB_STEP, nsf);
543    fact=0;
544
545    /*Apply pitch comb-filter (filter out noise between pitch harmonics)*/
546    for (i=0;i<nsf;i++)
547    {
548       spx_word32_t exc1, exc2;
549
550       fact += step;
551       
552       exc1 = SHL(MULT16_32_Q15(SHL(pitch_gain[0],7),exc[i-pitch+1]) +
553                  MULT16_32_Q15(SHL(pitch_gain[1],7),exc[i-pitch]) +
554                  MULT16_32_Q15(SHL(pitch_gain[2],7),exc[i-pitch-1]) , 2);
555       exc2 = SHL(MULT16_32_Q15(SHL(mem->last_pitch_gain[0],7),exc[i-mem->last_pitch+1]) +
556                  MULT16_32_Q15(SHL(mem->last_pitch_gain[1],7),exc[i-mem->last_pitch]) +
557                  MULT16_32_Q15(SHL(mem->last_pitch_gain[2],7),exc[i-mem->last_pitch-1]),2);
558
559       new_exc[i] = exc[i] + MULT16_32_Q15(comb_gain,MULT16_32_Q15(fact,exc1)  + MULT16_32_Q15(SUB16(COMB_STEP,fact), exc2));
560    }
561
562    mem->last_pitch_gain[0] = pitch_gain[0];
563    mem->last_pitch_gain[1] = pitch_gain[1];
564    mem->last_pitch_gain[2] = pitch_gain[2];
565    mem->last_pitch = pitch;
566
567    /*Amplitude after enhancement*/
568    new_exc_energy = compute_rms(new_exc, nsf);
569
570    /*Compute scaling factor and normalize energy*/
571    gain = (exc_energy)/(.1+new_exc_energy);
572    if (gain < .5)
573       gain=.5;
574    if (gain>.9999)
575       gain=.9999;
576
577 #ifdef FIXED_POINT
578    {
579       spx_word16_t gg = gain*32768;
580       for (i=0;i<nsf;i++)
581    {
582       mem->smooth_gain = MULT16_16_Q15(31457,mem->smooth_gain) + MULT16_16_Q15(1311,gg);
583       new_exc[i] = MULT16_32_Q15(mem->smooth_gain, new_exc[i]);
584    }
585    }
586 #else
587    for (i=0;i<nsf;i++)
588    {
589       mem->smooth_gain = .96*mem->smooth_gain + .04*gain;
590       new_exc[i] *= mem->smooth_gain;
591    }
592 #endif
593 }