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