1 /* Copyright (C) 2002 Jean-Marc Valin
3 Various analysis/synthesis filters
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions
9 - Redistributions of source code must retain the above copyright
10 notice, this list of conditions and the following disclaimer.
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.
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.
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.
38 #include "stack_alloc.h"
40 #include "math_approx.h"
45 #include "filters_sse.h"
46 #elif defined (ARM4_ASM) || defined(ARM5E_ASM)
47 #include "filters_arm4.h"
48 #elif defined (BFIN_ASM)
49 #include "filters_bfin.h"
54 void bw_lpc(spx_word16_t gamma
, const spx_coef_t
*lpc_in
, spx_coef_t
*lpc_out
, int order
)
57 spx_word16_t tmp
=gamma
;
60 lpc_out
[i
] = MULT16_16_P15(tmp
,lpc_in
[i
]);
61 tmp
= MULT16_16_P15(tmp
, gamma
);
68 /* FIXME: These functions are ugly and probably introduce too much error */
69 void signal_mul(const spx_sig_t
*x
, spx_sig_t
*y
, spx_word32_t scale
, int len
)
74 y
[i
] = SHL32(MULT16_32_Q14(EXTRACT16(SHR32(x
[i
],7)),scale
),7);
78 void signal_div(const spx_sig_t
*x
, spx_sig_t
*y
, spx_word32_t scale
, int len
)
81 if (scale
> SHL32(EXTEND32(SIG_SCALING
), 8))
84 scale
= PSHR32(scale
, SIG_SHIFT
);
85 scale_1
= EXTRACT16(DIV32_16(SHL32(EXTEND32(SIG_SCALING
),7),scale
));
88 y
[i
] = SHR32(MULT16_16(scale_1
, EXTRACT16(SHR32(x
[i
],SIG_SHIFT
))),7);
92 scale
= PSHR32(scale
, SIG_SHIFT
-5);
93 scale_1
= DIV32_16(SHL32(EXTEND32(SIG_SCALING
),3),scale
);
96 y
[i
] = MULT16_16(scale_1
, EXTRACT16(SHR32(x
[i
],SIG_SHIFT
-2)));
103 void signal_mul(const spx_sig_t
*x
, spx_sig_t
*y
, spx_word32_t scale
, int len
)
110 void signal_div(const spx_sig_t
*x
, spx_sig_t
*y
, spx_word32_t scale
, int len
)
113 float scale_1
= 1/scale
;
125 spx_word16_t
compute_rms(const spx_sig_t
*x
, int len
)
134 spx_sig_t tmp
= x
[i
];
142 while (max_val
>16383)
152 tmp
= EXTRACT16(SHR32(x
[i
],sig_shift
));
153 sum2
= MAC16_16(sum2
,tmp
,tmp
);
154 tmp
= EXTRACT16(SHR32(x
[i
+1],sig_shift
));
155 sum2
= MAC16_16(sum2
,tmp
,tmp
);
156 tmp
= EXTRACT16(SHR32(x
[i
+2],sig_shift
));
157 sum2
= MAC16_16(sum2
,tmp
,tmp
);
158 tmp
= EXTRACT16(SHR32(x
[i
+3],sig_shift
));
159 sum2
= MAC16_16(sum2
,tmp
,tmp
);
160 sum
= ADD32(sum
,SHR32(sum2
,6));
163 return EXTRACT16(SHR32(SHL32(EXTEND32(spx_sqrt(1+DIV32(sum
,len
))),(sig_shift
+3)),SIG_SHIFT
));
167 #ifndef OVERRIDE_NORMALIZE16
168 int normalize16(const spx_sig_t
*x
, spx_word16_t
*y
, spx_sig_t max_scale
, int len
)
176 spx_sig_t tmp
= x
[i
];
184 while (max_val
>max_scale
)
191 y
[i
] = EXTRACT16(SHR32(x
[i
], sig_shift
));
199 spx_word16_t
compute_rms(const spx_sig_t
*x
, int len
)
207 return sqrt(.1+sum
/len
);
213 #ifndef OVERRIDE_FILTER_MEM2
215 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
)
218 spx_word16_t xi
,yi
,nyi
;
222 xi
= EXTRACT16(PSHR32(SATURATE(x
[i
],536870911),SIG_SHIFT
));
223 yi
= EXTRACT16(PSHR32(SATURATE(ADD32(x
[i
], SHL32(mem
[0],1)),536870911),SIG_SHIFT
));
225 for (j
=0;j
<ord
-1;j
++)
227 mem
[j
] = MAC16_16(MAC16_16(mem
[j
+1], num
[j
],xi
), den
[j
],nyi
);
229 mem
[ord
-1] = ADD32(MULT16_16(num
[ord
-1],xi
), MULT16_16(den
[ord
-1],nyi
));
230 y
[i
] = SHL32(EXTEND32(yi
),SIG_SHIFT
);
234 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
)
241 xi
=SATURATE(x
[i
],805306368);
242 yi
= SATURATE(ADD32(xi
, SHL32(mem
[0],2)),805306368);
244 for (j
=0;j
<ord
-1;j
++)
246 mem
[j
] = MAC16_32_Q15(MAC16_32_Q15(mem
[j
+1], num
[j
],xi
), den
[j
],nyi
);
248 mem
[ord
-1] = SUB32(MULT16_32_Q15(num
[ord
-1],xi
), MULT16_32_Q15(den
[ord
-1],yi
));
255 #ifndef OVERRIDE_IIR_MEM2
257 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
)
264 yi
= EXTRACT16(PSHR32(SATURATE(x
[i
] + SHL32(mem
[0],1),536870911),SIG_SHIFT
));
266 for (j
=0;j
<ord
-1;j
++)
268 mem
[j
] = MAC16_16(mem
[j
+1],den
[j
],nyi
);
270 mem
[ord
-1] = MULT16_16(den
[ord
-1],nyi
);
271 y
[i
] = SHL32(EXTEND32(yi
),SIG_SHIFT
);
275 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
)
278 spx_word32_t xi
,yi
,nyi
;
282 xi
=SATURATE(x
[i
],805306368);
283 yi
= SATURATE(xi
+ SHL32(mem
[0],2),805306368);
285 for (j
=0;j
<ord
-1;j
++)
287 mem
[j
] = MAC16_32_Q15(mem
[j
+1],den
[j
],nyi
);
289 mem
[ord
-1] = MULT16_32_Q15(den
[ord
-1],nyi
);
296 #ifndef OVERRIDE_FIR_MEM2
298 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
)
305 xi
= EXTRACT16(PSHR32(SATURATE(x
[i
],536870911),SIG_SHIFT
));
306 yi
= EXTRACT16(PSHR32(SATURATE(x
[i
] + SHL32(mem
[0],1),536870911),SIG_SHIFT
));
307 for (j
=0;j
<ord
-1;j
++)
309 mem
[j
] = MAC16_16(mem
[j
+1], num
[j
],xi
);
311 mem
[ord
-1] = MULT16_16(num
[ord
-1],xi
);
312 y
[i
] = SHL32(EXTEND32(yi
),SIG_SHIFT
);
316 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
)
323 xi
=SATURATE(x
[i
],805306368);
324 yi
= xi
+ SHL32(mem
[0],2);
325 for (j
=0;j
<ord
-1;j
++)
327 mem
[j
] = MAC16_32_Q15(mem
[j
+1], num
[j
],xi
);
329 mem
[ord
-1] = MULT16_32_Q15(num
[ord
-1],xi
);
330 y
[i
] = SATURATE(yi
,805306368);
343 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
)
346 VARDECL(spx_mem_t
*mem
);
347 ALLOC(mem
, ord
, spx_mem_t
);
350 iir_mem2(xx
, ak
, y
, N
, ord
, mem
);
353 filter_mem2(y
, awk1
, awk2
, y
, N
, ord
, mem
);
356 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
)
359 VARDECL(spx_mem_t
*mem
);
360 ALLOC(mem
, ord
, spx_mem_t
);
363 filter_mem2(xx
, ak
, awk1
, y
, N
, ord
, mem
);
366 fir_mem2(y
, awk2
, y
, N
, ord
, mem
);
369 #ifndef OVERRIDE_COMPUTE_IMPULSE_RESPONSE
370 void compute_impulse_response(const spx_coef_t
*ak
, const spx_coef_t
*awk1
, const spx_coef_t
*awk2
, spx_word16_t
*y
, int N
, int ord
, char *stack
)
373 spx_word16_t y1
, ny1i
, ny2i
;
374 VARDECL(spx_mem_t
*mem1
);
375 VARDECL(spx_mem_t
*mem2
);
376 ALLOC(mem1
, ord
, spx_mem_t
);
377 ALLOC(mem2
, ord
, spx_mem_t
);
387 mem1
[i
] = mem2
[i
] = 0;
390 y1
= ADD16(y
[i
], EXTRACT16(PSHR32(mem1
[0],LPC_SHIFT
)));
392 y
[i
] = ADD16(SHL16(y1
,1), EXTRACT16(PSHR32(mem2
[0],LPC_SHIFT
)));
394 for (j
=0;j
<ord
-1;j
++)
396 mem1
[j
] = MAC16_16(mem1
[j
+1], awk2
[j
],ny1i
);
397 mem2
[j
] = MAC16_16(mem2
[j
+1], ak
[j
],ny2i
);
399 mem1
[ord
-1] = MULT16_16(awk2
[ord
-1],ny1i
);
400 mem2
[ord
-1] = MULT16_16(ak
[ord
-1],ny2i
);
405 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
)
408 VARDECL(spx_word16_t
*a
);
409 VARDECL(spx_word16_t
*x
);
412 ALLOC(a
, M
, spx_word16_t
);
413 ALLOC(x
, N
+M
-1, spx_word16_t
);
422 x
[i
+M
-1]=SATURATE(PSHR(xx
[i
],1),16383);
423 for (i
=0,k
=0;i
<N
;i
+=2,k
++)
429 y1
[k
]=ADD32(y1
[k
],SHR(MULT16_16(a
[j
],ADD16(x
[i
+j
],x2
[i
-j
])),1));
430 y2
[k
]=SUB32(y2
[k
],SHR(MULT16_16(a
[j
],SUB16(x
[i
+j
],x2
[i
-j
])),1));
432 y1
[k
]=ADD32(y1
[k
],SHR(MULT16_16(a
[j
],ADD16(x
[i
+j
],x2
[i
-j
])),1));
433 y2
[k
]=ADD32(y2
[k
],SHR(MULT16_16(a
[j
],SUB16(x
[i
+j
],x2
[i
-j
])),1));
437 mem
[i
]=SATURATE(PSHR(xx
[N
-i
-1],1),16383);
442 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
)
444 all odd x[i] are zero -- well, actually they are left out of the array now
445 N and M are multiples of 4 */
448 VARDECL(spx_word16_t
*xx
);
450 ALLOC(xx
, M
+N
-1, spx_word16_t
);
452 for (i
= 0; i
< N
/2; i
++)
453 xx
[2*i
] = SHR(x
[N
/2-1-i
],SIG_SHIFT
+1);
454 for (i
= 0; i
< M
- 1; i
+= 2)
457 for (i
= 0; i
< N
; i
+= 4) {
458 spx_sig_t y0
, y1
, y2
, y3
;
461 y0
= y1
= y2
= y3
= 0;
464 for (j
= 0; j
< M
; j
+= 4) {
472 y0
= ADD32(y0
,SHR(MULT16_16(a0
, x1
),1));
473 y1
= ADD32(y1
,SHR(MULT16_16(a1
, x1
),1));
474 y2
= ADD32(y2
,SHR(MULT16_16(a0
, x0
),1));
475 y3
= ADD32(y3
,SHR(MULT16_16(a1
, x0
),1));
481 y0
= ADD32(y0
,SHR(MULT16_16(a0
, x0
),1));
482 y1
= ADD32(y1
,SHR(MULT16_16(a1
, x0
),1));
483 y2
= ADD32(y2
,SHR(MULT16_16(a0
, x1
),1));
484 y3
= ADD32(y3
,SHR(MULT16_16(a1
, x1
),1));
492 for (i
= 0; i
< M
- 1; i
+= 2)
496 void filter_dc_notch(spx_sig_t
*in
, spx_word16_t radius
, spx_sig_t
*out
, int len
, spx_mem_t
*mem
)
499 spx_word16_t num
[3], den
[3];
504 den
[2] = radius
*radius
+ .7*(1-radius
)*(1-radius
);
507 out
[i
] = mem
[0] + num
[0]*in
[i
];
508 mem
[0] = mem
[1] + num
[1]*in
[i
] - den
[1]*out
[i
];
509 mem
[1] = num
[2]*in
[i
] - den
[2]*out
[i
];
513 void comb_filter_mem_init (CombFilterMem
*mem
)
516 mem
->last_pitch_gain
[0]=mem
->last_pitch_gain
[1]=mem
->last_pitch_gain
[2]=0;
521 #define COMB_STEP 32767
523 #define COMB_STEP 1.0
527 spx_sig_t
*exc
, /*decoded excitation*/
528 spx_sig_t
*new_exc
, /*enhanced excitation*/
529 spx_coef_t
*ak
, /*LPC filter coefs*/
531 int nsf
, /*sub-frame size*/
532 int pitch
, /*pitch period*/
533 spx_word16_t
*pitch_gain
, /*pitch gain (3-tap)*/
534 spx_word16_t comb_gain
, /*gain of comb filter*/
539 spx_word16_t exc_energy
=0, new_exc_energy
=0;
544 /*Compute excitation amplitude prior to enhancement*/
545 exc_energy
= compute_rms(exc
, nsf
);
546 /*for (i=0;i<nsf;i++)
547 exc_energy+=((float)exc[i])*exc[i];*/
549 /*Some gain adjustment if pitch is too high or if unvoiced*/
552 spx_word16_t g
= gain_3tap_to_1tap(pitch_gain
)+gain_3tap_to_1tap(mem
->last_pitch_gain
);
554 comb_gain
= MULT16_16_Q15(DIV32_16(SHL32(EXTEND32(165),15),g
), comb_gain
);
556 comb_gain
= MULT16_16_Q15(SHL16(g
, 9), comb_gain
);
561 g
= GAIN_SCALING_1
*.5*(gain_3tap_to_1tap(pitch_gain
)+gain_3tap_to_1tap(mem
->last_pitch_gain
));
568 step
= DIV32(COMB_STEP
, nsf
);
571 /*Apply pitch comb-filter (filter out noise between pitch harmonics)*/
574 spx_word32_t exc1
, exc2
;
576 fact
= ADD16(fact
,step
);
578 exc1
= SHL32(MULT16_32_Q15(SHL16(pitch_gain
[0],7),exc
[i
-pitch
+1]) +
579 MULT16_32_Q15(SHL16(pitch_gain
[1],7),exc
[i
-pitch
]) +
580 MULT16_32_Q15(SHL16(pitch_gain
[2],7),exc
[i
-pitch
-1]) , 2);
581 exc2
= SHL32(MULT16_32_Q15(SHL16(mem
->last_pitch_gain
[0],7),exc
[i
-mem
->last_pitch
+1]) +
582 MULT16_32_Q15(SHL16(mem
->last_pitch_gain
[1],7),exc
[i
-mem
->last_pitch
]) +
583 MULT16_32_Q15(SHL16(mem
->last_pitch_gain
[2],7),exc
[i
-mem
->last_pitch
-1]),2);
585 new_exc
[i
] = exc
[i
] + MULT16_32_Q15(comb_gain
, ADD32(MULT16_32_Q15(fact
,exc1
), MULT16_32_Q15(SUB16(COMB_STEP
,fact
), exc2
)));
588 mem
->last_pitch_gain
[0] = pitch_gain
[0];
589 mem
->last_pitch_gain
[1] = pitch_gain
[1];
590 mem
->last_pitch_gain
[2] = pitch_gain
[2];
591 mem
->last_pitch
= pitch
;
593 /*Amplitude after enhancement*/
594 new_exc_energy
= compute_rms(new_exc
, nsf
);
596 if (exc_energy
> new_exc_energy
)
597 exc_energy
= new_exc_energy
;
599 gain
= DIV32_16(SHL32(EXTEND32(exc_energy
),15),ADD16(1,new_exc_energy
));
612 mem
->smooth_gain
= ADD16(MULT16_16_Q15(31457,mem
->smooth_gain
), MULT16_16_Q15(1311,gain
));
613 new_exc
[i
] = MULT16_32_Q15(mem
->smooth_gain
, new_exc
[i
]);
618 mem
->smooth_gain
= .96*mem
->smooth_gain
+ .04*gain
;
619 new_exc
[i
] *= mem
->smooth_gain
;