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