28b9f1c51a75fedf1514bafbe9a7ca9a453625a9
[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 #include "filters.h"
34 #include "stack_alloc.h"
35 #include <math.h>
36 #include "misc.h"
37
38 void bw_lpc(float gamma, spx_coef_t *lpc_in, spx_coef_t *lpc_out, int order)
39 {
40    int i;
41    float tmp=1;
42    for (i=0;i<order+1;i++)
43    {
44       lpc_out[i] = tmp * lpc_in[i];
45       tmp *= gamma;
46    }
47 }
48
49
50 #ifdef FIXED_POINT
51
52 spx_word16_t compute_rms(spx_sig_t *x, int len)
53 {
54    int i;
55    spx_word32_t sum=0;
56    spx_sig_t max_val=1;
57    int sig_shift;
58
59    for (i=0;i<len;i++)
60    {
61       spx_sig_t tmp = x[i];
62       if (tmp<0)
63          tmp = -1;
64       if (tmp > max_val)
65          max_val = tmp;
66    }
67
68    sig_shift=0;
69    while (max_val>2048)
70    {
71       sig_shift++;
72       max_val >>= 1;
73    }
74
75    for (i=0;i<len;i+=4)
76    {
77       spx_word32_t sum2=0;
78       spx_word16_t tmp;
79       tmp = SHR(x[i],sig_shift);
80       sum2 += MULT16_16(tmp,tmp);
81       tmp = SHR(x[i+1],sig_shift);
82       sum2 += MULT16_16(tmp,tmp);
83       tmp = SHR(x[i+2],sig_shift);
84       sum2 += MULT16_16(tmp,tmp);
85       tmp = SHR(x[i+3],sig_shift);
86       sum2 += MULT16_16(tmp,tmp);
87       sum += sum2;
88    }
89    
90    /*FIXME: remove division*/
91    return (1<<sig_shift)*sqrt(1+sum/len)/SIG_SCALING;
92 }
93
94 #define MUL_16_32_R15(a,bh,bl) ((a)*(bh) + ((a)*(bl)>>15))
95
96
97 void filter_mem2(spx_sig_t *x, spx_coef_t *num, spx_coef_t *den, spx_sig_t *y, int N, int ord, spx_mem_t *mem)
98 {
99    int i,j;
100    int xi,yi;
101
102    for (i=0;i<N;i++)
103    {
104       int xh,xl,yh,yl;
105       xi=floor(.5+x[i]);
106       yi = xi + (mem[0]<<2);
107       xh = xi>>15; xl=xi&0x00007fff; yh = yi>>15; yl=yi&0x00007fff; 
108       for (j=0;j<ord-1;j++)
109       {
110          mem[j] = mem[j+1] +  MUL_16_32_R15(num[j+1],xh,xl) - MUL_16_32_R15(den[j+1],yh,yl);
111       }
112       mem[ord-1] = MUL_16_32_R15(num[ord],xh,xl) - MUL_16_32_R15(den[ord],yh,yl);
113       y[i] = yi;
114    }
115 }
116
117 void iir_mem2(spx_sig_t *x, spx_coef_t *den, spx_sig_t *y, int N, int ord, spx_mem_t *mem)
118 {
119    int i,j;
120    int xi,yi;
121
122    for (i=0;i<N;i++)
123    {
124       int yh,yl;
125       xi=floor(.5+x[i]);
126       yi = xi + (mem[0]<<2);
127       yh = yi>>15; yl=yi&0x00007fff; 
128       for (j=0;j<ord-1;j++)
129       {
130          mem[j] = mem[j+1] - MUL_16_32_R15(den[j+1],yh,yl);
131       }
132       mem[ord-1] = - MUL_16_32_R15(den[ord],yh,yl);
133       y[i] = yi;
134    }
135 }
136
137
138 void fir_mem2(spx_sig_t *x, spx_coef_t *num, spx_sig_t *y, int N, int ord, spx_mem_t *mem)
139 {
140    int i,j;
141    int xi,yi;
142
143    for (i=0;i<N;i++)
144    {
145       int xh,xl;
146       xi=floor(.5+x[i]);
147       yi = xi + (mem[0]<<2);
148       xh = xi>>15; xl=xi&0x00007fff;
149       for (j=0;j<ord-1;j++)
150       {
151          mem[j] = mem[j+1] +  MUL_16_32_R15(num[j+1],xh,xl);
152       }
153       mem[ord-1] = MUL_16_32_R15(num[ord],xh,xl);
154       y[i] = yi;
155    }
156
157 }
158
159 #else
160
161
162
163 #ifdef _USE_SSE
164 #include "filters_sse.h"
165 #else
166
167 spx_word16_t compute_rms(spx_sig_t *x, int len)
168 {
169    int i;
170    float sum=0;
171    for (i=0;i<len;i++)
172    {
173       sum += x[i]*x[i];
174    }
175    return sqrt(.1+sum/len);
176 }
177
178 void filter_mem2(spx_sig_t *x, spx_coef_t *num, spx_coef_t *den, spx_sig_t *y, int N, int ord,  spx_mem_t *mem)
179 {
180    int i,j;
181    float xi,yi;
182    for (i=0;i<N;i++)
183    {
184       xi=x[i];
185       y[i] = num[0]*xi + mem[0];
186       yi=y[i];
187       for (j=0;j<ord-1;j++)
188       {
189          mem[j] = mem[j+1] + num[j+1]*xi - den[j+1]*yi;
190       }
191       mem[ord-1] = num[ord]*xi - den[ord]*yi;
192    }
193 }
194
195
196 void iir_mem2(spx_sig_t *x, spx_coef_t *den, spx_sig_t *y, int N, int ord, spx_mem_t *mem)
197 {
198    int i,j;
199    for (i=0;i<N;i++)
200    {
201       y[i] = x[i] + mem[0];
202       for (j=0;j<ord-1;j++)
203       {
204          mem[j] = mem[j+1] - den[j+1]*y[i];
205       }
206       mem[ord-1] = - den[ord]*y[i];
207    }
208 }
209
210
211 #endif
212
213 void fir_mem2(spx_sig_t *x, spx_coef_t *num, spx_sig_t *y, int N, int ord, spx_mem_t *mem)
214 {
215    int i,j;
216    float xi;
217    for (i=0;i<N;i++)
218    {
219       xi=x[i];
220       y[i] = num[0]*xi + mem[0];
221       for (j=0;j<ord-1;j++)
222       {
223          mem[j] = mem[j+1] + num[j+1]*xi;
224       }
225       mem[ord-1] = num[ord]*xi;
226    }
227 }
228
229
230 #endif
231
232
233 void syn_percep_zero(spx_sig_t *xx, spx_coef_t *ak, spx_coef_t *awk1, spx_coef_t *awk2, spx_sig_t *y, int N, int ord, char *stack)
234 {
235    int i;
236    spx_mem_t *mem = PUSH(stack,ord, spx_mem_t);
237    for (i=0;i<ord;i++)
238      mem[i]=0;
239    iir_mem2(xx, ak, y, N, ord, mem);
240    for (i=0;i<ord;i++)
241       mem[i]=0;
242    filter_mem2(y, awk1, awk2, y, N, ord, mem);
243 }
244
245 void residue_percep_zero(spx_sig_t *xx, spx_coef_t *ak, spx_coef_t *awk1, spx_coef_t *awk2, spx_sig_t *y, int N, int ord, char *stack)
246 {
247    int i;
248    spx_mem_t *mem = PUSH(stack,ord, spx_mem_t);
249    for (i=0;i<ord;i++)
250       mem[i]=0;
251    filter_mem2(xx, ak, awk1, y, N, ord, mem);
252    for (i=0;i<ord;i++)
253      mem[i]=0;
254    fir_mem2(y, awk2, y, N, ord, mem);
255 }
256
257
258 void qmf_decomp(float *xx, float *aa, spx_sig_t *y1, spx_sig_t *y2, int N, int M, float *mem, char *stack)
259 {
260    int i,j,k,M2;
261    float *a;
262    float *x;
263    float *x2;
264    
265    a = PUSH(stack, M, float);
266    x = PUSH(stack, N+M-1, float);
267    x2=x+M-1;
268    M2=M>>1;
269    for (i=0;i<M;i++)
270       a[M-i-1]=aa[i];
271    for (i=0;i<M-1;i++)
272       x[i]=mem[M-i-2];
273    for (i=0;i<N;i++)
274       x[i+M-1]=xx[i];
275    for (i=0,k=0;i<N;i+=2,k++)
276    {
277       y1[k]=0;
278       y2[k]=0;
279       for (j=0;j<M2;j++)
280       {
281          y1[k]+=a[j]*(x[i+j]+x2[i-j]);
282          y2[k]-=a[j]*(x[i+j]-x2[i-j]);
283          j++;
284          y1[k]+=a[j]*(x[i+j]+x2[i-j]);
285          y2[k]+=a[j]*(x[i+j]-x2[i-j]);
286       }
287    }
288    for (i=0;i<M-1;i++)
289      mem[i]=xx[N-i-1];
290 }
291
292 /* By segher */
293 void fir_mem_up(spx_sig_t *x, float *a, spx_sig_t *y, int N, int M, float *mem, char *stack)
294    /* assumptions:
295       all odd x[i] are zero -- well, actually they are left out of the array now
296       N and M are multiples of 4 */
297 {
298    int i, j;
299    float *xx=PUSH(stack, M+N-1, float);
300
301    for (i = 0; i < N/2; i++)
302       xx[2*i] = x[N/2-1-i];
303    for (i = 0; i < M - 1; i += 2)
304       xx[N+i] = mem[i+1];
305
306    for (i = 0; i < N; i += 4) {
307       float y0, y1, y2, y3;
308       float x0;
309
310       y0 = y1 = y2 = y3 = 0.f;
311       x0 = xx[N-4-i];
312
313       for (j = 0; j < M; j += 4) {
314          float x1;
315          float a0, a1;
316
317          a0 = a[j];
318          a1 = a[j+1];
319          x1 = xx[N-2+j-i];
320
321          y0 += a0 * x1;
322          y1 += a1 * x1;
323          y2 += a0 * x0;
324          y3 += a1 * x0;
325
326          a0 = a[j+2];
327          a1 = a[j+3];
328          x0 = xx[N+j-i];
329
330          y0 += a0 * x0;
331          y1 += a1 * x0;
332          y2 += a0 * x1;
333          y3 += a1 * x1;
334       }
335       y[i] = y0;
336       y[i+1] = y1;
337       y[i+2] = y2;
338       y[i+3] = y3;
339    }
340
341    for (i = 0; i < M - 1; i += 2)
342       mem[i+1] = xx[i];
343 }
344
345
346 void comp_filter_mem_init (CombFilterMem *mem)
347 {
348    mem->last_pitch=0;
349    mem->last_pitch_gain[0]=mem->last_pitch_gain[1]=mem->last_pitch_gain[2]=0;
350    mem->smooth_gain=1;
351 }
352
353 void comb_filter(
354 spx_sig_t *exc,          /*decoded excitation*/
355 spx_sig_t *new_exc,      /*enhanced excitation*/
356 spx_coef_t *ak,           /*LPC filter coefs*/
357 int p,               /*LPC order*/
358 int nsf,             /*sub-frame size*/
359 int pitch,           /*pitch period*/
360 float *pitch_gain,   /*pitch gain (3-tap)*/
361 float  comb_gain,    /*gain of comb filter*/
362 CombFilterMem *mem
363 )
364 {
365    int i;
366    float exc_energy=0, new_exc_energy=0;
367    float gain;
368    float step;
369    float fact;
370    /*Compute excitation energy prior to enhancement*/
371    for (i=0;i<nsf;i++)
372       exc_energy+=exc[i]*exc[i];
373
374    /*Some gain adjustment is pitch is too high or if unvoiced*/
375    {
376       float g=0;
377       g = .5*fabs(pitch_gain[0]+pitch_gain[1]+pitch_gain[2] +
378       mem->last_pitch_gain[0] + mem->last_pitch_gain[1] + mem->last_pitch_gain[2]);
379       if (g>1.3)
380          comb_gain*=1.3/g;
381       if (g<.5)
382          comb_gain*=2*g;
383    }
384    step = 1.0/nsf;
385    fact=0;
386    /*Apply pitch comb-filter (filter out noise between pitch harmonics)*/
387    for (i=0;i<nsf;i++)
388    {
389       fact += step;
390
391       new_exc[i] = exc[i] + comb_gain * fact * (
392                                          pitch_gain[0]*exc[i-pitch+1] +
393                                          pitch_gain[1]*exc[i-pitch] +
394                                          pitch_gain[2]*exc[i-pitch-1]
395                                          )
396       + comb_gain * (1-fact) * (
397                                          mem->last_pitch_gain[0]*exc[i-mem->last_pitch+1] +
398                                          mem->last_pitch_gain[1]*exc[i-mem->last_pitch] +
399                                          mem->last_pitch_gain[2]*exc[i-mem->last_pitch-1]
400                                          );
401    }
402
403    mem->last_pitch_gain[0] = pitch_gain[0];
404    mem->last_pitch_gain[1] = pitch_gain[1];
405    mem->last_pitch_gain[2] = pitch_gain[2];
406    mem->last_pitch = pitch;
407
408    /*Gain after enhancement*/
409    for (i=0;i<nsf;i++)
410       new_exc_energy+=new_exc[i]*new_exc[i];
411
412    /*Compute scaling factor and normalize energy*/
413    gain = sqrt(exc_energy)/sqrt(.1+new_exc_energy);
414    if (gain < .5)
415       gain=.5;
416    if (gain>1)
417       gain=1;
418
419    for (i=0;i<nsf;i++)
420    {
421       mem->smooth_gain = .96*mem->smooth_gain + .04*gain;
422       new_exc[i] *= mem->smooth_gain;
423    }
424 }