Tweaking to the AGC to handle really low volume input properly.
[speexdsp.git] / tmv / mdf_tm.h
1 /* Copyright (C) 2007 Hong Zhiqian */\r
2 /**\r
3    @file mdf_tm.h\r
4    @author Hong Zhiqian\r
5    @brief Various compatibility routines for Speex (TriMedia version)\r
6 */\r
7 /*\r
8    Redistribution and use in source and binary forms, with or without\r
9    modification, are permitted provided that the following conditions\r
10    are met:\r
11    \r
12    - Redistributions of source code must retain the above copyright\r
13    notice, this list of conditions and the following disclaimer.\r
14    \r
15    - Redistributions in binary form must reproduce the above copyright\r
16    notice, this list of conditions and the following disclaimer in the\r
17    documentation and/or other materials provided with the distribution.\r
18    \r
19    - Neither the name of the Xiph.org Foundation nor the names of its\r
20    contributors may be used to endorse or promote products derived from\r
21    this software without specific prior written permission.\r
22    \r
23    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\r
24    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\r
25    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR\r
26    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR\r
27    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,\r
28    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,\r
29    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\r
30    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\r
31    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\r
32    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\r
33    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\r
34 */\r
35 #include <ops/custom_defs.h>\r
36 #include "profile_tm.h"\r
37 \r
38 // shifted power spectrum to fftwrap.c so that optimisation can be shared between mdf.c and preprocess.c\r
39 #define OVERRIDE_POWER_SPECTRUM                                 \r
40 \r
41 #ifdef FIXED_POINT\r
42 \r
43 #else\r
44 \r
45 #define OVERRIDE_FILTER_DC_NOTCH16\r
46 void filter_dc_notch16(\r
47         const spx_int16_t * restrict in, \r
48         float radius, \r
49         float * restrict out, \r
50         int len, \r
51         float * restrict mem\r
52 )\r
53 {\r
54         register int i;\r
55         register float den2, r1;\r
56         register float mem0, mem1;\r
57 \r
58         FILTERDCNOTCH16_START();\r
59 \r
60         r1 = 1 - radius;\r
61         den2 = (radius * radius) + (0.7 * r1 * r1);\r
62         mem0 = mem[0];\r
63         mem1 = mem[1];\r
64 \r
65 #if (TM_UNROLL && TM_UNROLL_FILTERDCNOTCH16)\r
66 #pragma TCS_unroll=4\r
67 #pragma TCS_unrollexact=1\r
68 #endif\r
69         for ( i=0 ; i<len ; ++i )\r
70         {\r
71                 register float vin  = in[i];\r
72                 register float vout = mem0 + vin;\r
73                 register float rvout = radius * vout;\r
74 \r
75                 mem0 = mem1 + 2 * (-vin + rvout);\r
76                 mem1 = vin - (den2 * vout);\r
77                 \r
78                 out[i] = rvout;\r
79         }\r
80 #if (TM_UNROLL && TM_UNROLL_FILTERDCNOTCH16)\r
81 #pragma TCS_unrollexact=0\r
82 #pragma TCS_unroll=0\r
83 #endif\r
84 \r
85         mem[0] = mem0;\r
86         mem[1] = mem1;\r
87 \r
88         FILTERDCNOTCH16_STOP();\r
89 }\r
90 \r
91 #define OVERRIDE_MDF_INNER_PROD\r
92 float mdf_inner_prod(\r
93         const float * restrict x, \r
94         const float * restrict y, \r
95         int len\r
96 )\r
97 {\r
98         register float sum = 0;\r
99    \r
100         MDFINNERPROD_START();\r
101 \r
102         len >>= 1;\r
103   \r
104 #if (TM_UNROLL && TM_UNROLL_MDFINNERPRODUCT)\r
105 #pragma TCS_unroll=4\r
106 #pragma TCS_unrollexact=1\r
107 #endif\r
108         while(len--)\r
109         {\r
110                 register float acc0, acc1;\r
111 \r
112                 acc0 = (*x++) * (*y++);\r
113                 acc1 = (*x++) * (*y++);\r
114 \r
115                 sum      += acc0 + acc1;\r
116         }\r
117 #if (TM_UNROLL && TM_UNROLL_MDFINNERPRODUCT)\r
118 #pragma TCS_unrollexact=0\r
119 #pragma TCS_unroll=0\r
120 #endif\r
121 \r
122         MDFINNERPROD_STOP();\r
123 \r
124         return sum;\r
125 }\r
126 \r
127 #define OVERRIDE_SPECTRAL_MUL_ACCUM\r
128 void spectral_mul_accum(\r
129         const float * restrict X, \r
130         const float * restrict Y, \r
131         float * restrict acc, \r
132         int N, int M\r
133 )\r
134 {\r
135         register int i, j;\r
136         register float Xi, Yi, Xii, Yii;\r
137         register int _N;\r
138 \r
139         SPECTRALMULACCUM_START();\r
140 \r
141         acc[0] = X[0] * Y[0];\r
142         _N = N-1;\r
143 \r
144         for ( i=1 ; i<_N ; i+=2 )\r
145         {\r
146                 Xi = X[i];\r
147                 Yi = Y[i];\r
148                 Xii = X[i+1];\r
149                 Yii = Y[i+1];\r
150 \r
151                 acc[i]  = (Xi  * Yi - Xii * Yii);\r
152                 acc[i+1]= (Xii * Yi + Xi  * Yii);\r
153         }\r
154       \r
155         acc[_N] = X[_N] * Y[_N];\r
156 \r
157         for ( j=1,X+=N,Y+=N ; j<M ; j++ )\r
158         {       \r
159                 acc[0] += X[0] * Y[0];\r
160 \r
161                 for ( i=1 ; i<N-1 ; i+=2 )\r
162                 {\r
163                         Xi = X[i];\r
164                         Yi = Y[i];\r
165                         Xii = X[i+1];\r
166                         Yii = Y[i+1];\r
167 \r
168                         acc[i]  += (Xi  * Yi - Xii * Yii);\r
169                         acc[i+1]+= (Xii * Yi + Xi  * Yii);\r
170                 }\r
171       \r
172                 acc[_N] += X[_N] * Y[_N];\r
173                 X += N;\r
174                 Y += N;\r
175         }\r
176 \r
177         SPECTRALMULACCUM_STOP();\r
178 }\r
179 \r
180 #define OVERRIDE_WEIGHTED_SPECTRAL_MUL_CONJ\r
181 void weighted_spectral_mul_conj(\r
182         const float * restrict w, \r
183         const float p, \r
184         const float * restrict X, \r
185         const float * restrict Y, \r
186         float * restrict prod, \r
187         int N\r
188 )\r
189 {\r
190         register int i, j;\r
191         register int _N;\r
192 \r
193         WEIGHTEDSPECTRALMULCONJ_START();\r
194 \r
195         prod[0] = p * w[0] * X[0] * Y[0]; \r
196         _N = N-1;\r
197 \r
198         for (i=1,j=1;i<_N;i+=2,j++)\r
199         {\r
200                 register float W;\r
201                 register float Xi, Yi, Xii, Yii;\r
202 \r
203                 Xi = X[i];\r
204                 Yi = Y[i];\r
205                 Xii = X[i+1];\r
206                 Yii = Y[i+1];\r
207                 W = p * w[j];\r
208 \r
209 \r
210                 prod[i]  = W * (Xi  * Yi + Xii * Yii);\r
211                 prod[i+1]= W * (Xi  * Yii - Xii * Yi);\r
212         }\r
213    \r
214         prod[_N] = p * w[j] * X[_N] * Y[_N];\r
215 \r
216         WEIGHTEDSPECTRALMULCONJ_STOP();\r
217 }\r
218 \r
219 #define OVERRIDE_MDF_ADJUST_PROP\r
220 void mdf_adjust_prop(\r
221         const float * restrict W, \r
222         int N, \r
223         int M, \r
224         float * restrict prop\r
225 )\r
226 {\r
227         register int i, j;\r
228         register float max_sum = 1;\r
229         register float prop_sum = 1;\r
230 \r
231         MDFADJUSTPROP_START();\r
232 \r
233         for ( i=0 ; i<M ; ++i )\r
234         {\r
235                 register float tmp = 1;\r
236                 register int k = i * N;\r
237                 register int l = k + N;\r
238                 register float propi;\r
239 \r
240 #if (TM_UNROLL && TM_UNROLL_MDFADJUSTPROP)\r
241 #pragma TCS_unroll=4\r
242 #pragma TCS_unrollexact=1\r
243 #endif\r
244                 for ( j=k ; j<l ; ++j )\r
245                 {       \r
246                         register float wi = W[j];\r
247                         \r
248                         tmp += wi * wi;\r
249                 }\r
250 #if (TM_UNROLL && TM_UNROLL_MDFADJUSTPROP)\r
251 #pragma TCS_unrollexact=0\r
252 #pragma TCS_unroll=0\r
253 #endif\r
254 \r
255                 propi  = spx_sqrt(tmp);\r
256                 prop[i]= propi;\r
257                 max_sum= fmux(propi > max_sum, propi, max_sum);\r
258         }\r
259 \r
260         for ( i=0 ; i<M ; ++i )\r
261         {\r
262                 register float propi = prop[i];\r
263 \r
264                 propi += .1f * max_sum;\r
265                 prop_sum += propi;\r
266                 prop[i] = propi;\r
267         }\r
268 \r
269         prop_sum = 0.99f / prop_sum;\r
270         for ( i=0 ; i<M ; ++i )\r
271         {       prop[i] = prop_sum * prop[i];\r
272         }\r
273 \r
274         MDFADJUSTPROP_STOP();\r
275 }\r
276 \r
277 \r
278 #define OVERRIDE_SPEEX_ECHO_GET_RESIDUAL\r
279 void speex_echo_get_residual(\r
280         SpeexEchoState * restrict st, \r
281         float * restrict residual_echo, \r
282         int len\r
283 )\r
284 {\r
285         register int i;\r
286         register float leak2, leake;\r
287         register int N;\r
288         register float * restrict window;\r
289         register float * restrict last_y;\r
290         register float * restrict y;\r
291 \r
292         SPEEXECHOGETRESIDUAL_START();\r
293 \r
294         window = st->window;\r
295         last_y = st->last_y;\r
296         y = st->y;\r
297         N = st->window_size;\r
298 \r
299 \r
300 #if (TM_UNROLL && TM_UNROLL_SPEEXECHOGETRESIDUAL)\r
301 #pragma TCS_unroll=4\r
302 #pragma TCS_unrollexact=1\r
303 #endif\r
304         for (i=0;i<N;i++)\r
305         {       y[i] = window[i] * last_y[i];   \r
306         }\r
307 #if (TM_UNROLL && TM_UNROLL_SPEEXECHOGETRESIDUAL)\r
308 #pragma TCS_unrollexact=0\r
309 #pragma TCS_unroll=0\r
310 #endif\r
311       \r
312         spx_fft(st->fft_table, st->y, st->Y);\r
313         power_spectrum(st->Y, residual_echo, N);\r
314       \r
315         leake = st->leak_estimate;\r
316         leak2 = fmux(leake > .5, 1, 2 * leake);\r
317         N = st->frame_size;\r
318 \r
319 #if (TM_UNROLL && TM_UNROLL_SPEEXECHOGETRESIDUAL)\r
320 #pragma TCS_unroll=4\r
321 #pragma TCS_unrollexact=1\r
322 #endif\r
323         for ( i=0 ; i<N ; ++i )\r
324         {       residual_echo[i] *= leak2;\r
325         }\r
326 #if (TM_UNROLL && TM_UNROLL_SPEEXECHOGETRESIDUAL)\r
327 #pragma TCS_unrollexact=0\r
328 #pragma TCS_unroll=0\r
329 #endif\r
330 \r
331         residual_echo[N] *= leak2;\r
332 \r
333 #ifndef NO_REMARK\r
334    (void)len;\r
335 #endif\r
336 \r
337    SPEEXECHOGETRESIDUAL_STOP();\r
338 }\r
339 #endif\r
340 \r
341 \r
342 void mdf_preemph(\r
343         SpeexEchoState * restrict st, \r
344         spx_word16_t * restrict x,\r
345         const spx_int16_t * restrict far_end,\r
346         int framesize\r
347 )\r
348 {\r
349         register spx_word16_t preemph = st->preemph;\r
350         register spx_word16_t memX = st->memX;\r
351         register spx_word16_t memD = st->memD;\r
352         register spx_word16_t * restrict input = st->input;\r
353         register int i;\r
354 #ifdef FIXED_POINT\r
355         register int saturated = st->saturated;\r
356 #endif\r
357 \r
358 #if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION)\r
359 #pragma TCS_unroll=4\r
360 #pragma TCS_unrollexact=1\r
361 #endif\r
362         for ( i=0 ; i<framesize ; ++i )\r
363         {\r
364                 register spx_int16_t far_endi = far_end[i];\r
365                 register spx_word32_t tmp32;\r
366                 register spx_word16_t inputi = input[i];\r
367 \r
368                 tmp32 = SUB32(EXTEND32(far_endi), EXTEND32(MULT16_16_P15(preemph,memX)));\r
369 \r
370 #ifdef FIXED_POINT\r
371                 saturated = mux(iabs(tmp32) > 32767, M+1, saturated);\r
372                 tmp32 = iclipi(tmp32,32767);\r
373 #endif\r
374 \r
375                 x[i] = EXTRACT16(tmp32);\r
376                 memX = far_endi;\r
377                 tmp32 = SUB32(EXTEND32(inputi), EXTEND32(MULT16_16_P15(preemph, memD)));\r
378 \r
379 #ifdef FIXED_POINT\r
380                 saturated = mux( ((tmp32 > 32767) && (saturated == 0)), 1,\r
381                                         mux( ((tmp32 <-32767) && (saturated == 0)), 1, saturated ));\r
382                 tmp32 = iclipi(tmp32,32767);\r
383 #endif\r
384                 memD = inputi;\r
385                 input[i] = tmp32;\r
386         }\r
387 #if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION)\r
388 #pragma TCS_unrollexact=0\r
389 #pragma TCS_unroll=0\r
390 #endif\r
391 \r
392         st->memD = memD;\r
393         st->memX = memX;\r
394 \r
395 #ifdef FIXED_POINT\r
396         st->saturated = saturated;\r
397 #endif\r
398 }\r
399 \r
400 void mdf_sub(\r
401         spx_word16_t * restrict dest,\r
402         const spx_word16_t * restrict src1,\r
403         const spx_word16_t * restrict src2,\r
404         int framesize\r
405 )\r
406 {\r
407         register int i;\r
408 \r
409 #if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION)\r
410 #pragma TCS_unroll=4\r
411 #pragma TCS_unrollexact=1\r
412 #endif\r
413 \r
414 #ifdef FIXED_POINT\r
415         for ( i=0,framesize<<=1 ; i<framesize ; i+=4 )\r
416         {       register int src1i, src2i, desti;\r
417 \r
418                 src1i = ld32d(src1,i);\r
419                 src2i = ld32d(src2,i);\r
420                 desti = dspidualsub(src1i,src2i);\r
421                 st32d(i, dest, desti);\r
422         }\r
423 #else\r
424         for ( i=0 ; i<framesize ; ++i )\r
425         {       dest[i] = src1[i] - src2[i];\r
426         }\r
427 #endif\r
428 \r
429 #if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION)\r
430 #pragma TCS_unrollexact=0\r
431 #pragma TCS_unroll=0\r
432 #endif   \r
433 }\r
434 \r
435 void mdf_sub_int(\r
436         spx_word16_t * restrict dest,\r
437         const spx_int16_t * restrict src1,\r
438         const spx_int16_t * restrict src2,\r
439         int framesize\r
440 )\r
441 {\r
442         register int i, j;\r
443 \r
444 #if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION)\r
445 #pragma TCS_unroll=4\r
446 #pragma TCS_unrollexact=1\r
447 #endif\r
448 \r
449 #ifdef FIXED_POINT\r
450         for ( i=0,framesize<<=1 ; i<framesize ; i+=4 )\r
451         {       register int src1i, src2i, desti;\r
452 \r
453                 src1i = ld32d(src1,i);\r
454                 src2i = ld32d(src2,i);\r
455                 desti = dspidualsub(src1i,src2i);\r
456                 st32d(i, dest, desti);\r
457         }\r
458 #else\r
459         for ( i=0,j=0 ; i<framesize ; i+=2,++j )\r
460         {       register int src1i, src2i, desti;\r
461                 \r
462                 \r
463                 src1i = ld32d(src1,j);\r
464                 src2i = ld32d(src2,j);\r
465                 desti = dspidualsub(src1i,src2i);\r
466 \r
467                 dest[i] = sex16(desti);\r
468                 dest[i+1] = asri(16,desti);\r
469         }\r
470 #endif\r
471 \r
472 #if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION)\r
473 #pragma TCS_unrollexact=0\r
474 #pragma TCS_unroll=0\r
475 #endif   \r
476 }\r
477 \r
478 void mdf_compute_weight_gradient(\r
479         SpeexEchoState * restrict st,\r
480         spx_word16_t * restrict X,\r
481         int N,\r
482         int M\r
483 )\r
484 {\r
485         register int i, j;\r
486         register spx_word32_t * restrict PHI = st->PHI;\r
487 \r
488         for (j=M-1;j>=0;j--)\r
489         {\r
490                 register spx_word32_t * restrict W = &(st->W[j*N]);\r
491 \r
492                 weighted_spectral_mul_conj(\r
493                         st->power_1, \r
494                         FLOAT_SHL(PSEUDOFLOAT(st->prop[j]),-15), \r
495                         &X[(j+1)*N], \r
496                         st->E, \r
497                         st->PHI, \r
498                         N);\r
499 #if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION)\r
500 #pragma TCS_unroll=4\r
501 #pragma TCS_unrollexact=1\r
502 #endif\r
503                 for (i=0;i<N;i++)\r
504                 {       W[i] = ADD32(W[i],PHI[i]);\r
505                 }\r
506 #if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION)\r
507 #pragma TCS_unrollexact=0\r
508 #pragma TCS_unroll=0\r
509 #endif         \r
510         }\r
511 }\r
512 \r
513 void mdf_update_weight(\r
514         SpeexEchoState * restrict st,\r
515         int N,\r
516         int M,\r
517         int framesize\r
518 )\r
519 {\r
520         register int j;\r
521         register int cancel_count = st->cancel_count;\r
522         register spx_word16_t * restrict wtmp = st->wtmp;\r
523 #ifdef FIXED_POINT\r
524         register spx_word16_t * restrict wtmp2 = st->wtmp2;\r
525         register int i;\r
526 #endif\r
527 \r
528         for ( j=0 ; j<M ; j++ )\r
529         {\r
530                 register spx_word32_t * restrict W = &(st->W[j*N]);\r
531     \r
532                 if (j==0 || cancel_count%(M-1) == j-1)\r
533                 {\r
534 #ifdef FIXED_POINT\r
535 \r
536 #if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION)\r
537 #pragma TCS_unroll=4\r
538 #pragma TCS_unrollexact=1\r
539 #endif\r
540                         for ( i=0 ; i<N ; i++ )\r
541                                 wtmp2[i] = EXTRACT16(PSHR32(W[i],NORMALIZE_SCALEDOWN+16));\r
542 #if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION)\r
543 #pragma TCS_unrollexact=0\r
544 #pragma TCS_unroll=0\r
545 #endif         \r
546                         spx_ifft(st->fft_table, wtmp2, wtmp);\r
547                         memset(wtmp, 0, framesize * sizeof(spx_word16_t));\r
548 \r
549 #if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION)\r
550 #pragma TCS_unroll=4\r
551 #pragma TCS_unrollexact=1\r
552 #endif\r
553                         for (j=framesize; j<N ; ++j)\r
554                         {       wtmp[j]=SHL16(wtmp[j],NORMALIZE_SCALEUP);\r
555                         }\r
556 #if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION)\r
557 #pragma TCS_unrollexact=0\r
558 #pragma TCS_unroll=0\r
559 #endif    \r
560 \r
561                         spx_fft(st->fft_table, wtmp, wtmp2);\r
562 \r
563 #if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION)\r
564 #pragma TCS_unroll=4\r
565 #pragma TCS_unrollexact=1\r
566 #endif\r
567                         for (i=0;i<N;i++)\r
568                         {       W[i] -= SHL32(EXTEND32(wtmp2[i]),16+NORMALIZE_SCALEDOWN-NORMALIZE_SCALEUP-1);\r
569                         }\r
570 #if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION)\r
571 #pragma TCS_unrollexact=0\r
572 #pragma TCS_unroll=0\r
573 #endif    \r
574 \r
575 #else\r
576                         spx_ifft(st->fft_table, W, wtmp);\r
577                         memset(&wtmp[framesize], 0, (N-framesize) * sizeof(spx_word16_t));\r
578                         spx_fft(st->fft_table, wtmp, W);\r
579 #endif  \r
580                 }\r
581         }\r
582 }\r
583 \r
584 #ifdef TWO_PATH\r
585 // first four parameters is passed by registers\r
586 // generate faster performance with 4 parameters functions\r
587 spx_word32_t mdf_update_foreground(\r
588         SpeexEchoState * restrict st,\r
589         spx_word32_t Dbf,\r
590         spx_word32_t Sff,\r
591         spx_word32_t See\r
592 )                                                                               \r
593 {                                                               \r
594         register spx_word32_t Davg1 = st->Davg1;                                                                                                                                        \r
595         register spx_word32_t Davg2 = st->Davg2;                                                                                                                                        \r
596         register spx_word32_t Dvar1 = st->Dvar1;                                                                                                                                        \r
597         register spx_word32_t Dvar2 = st->Dvar2;\r
598         register spx_word16_t * restrict input = st->input;\r
599         register int framesize = st->frame_size;\r
600         register spx_word16_t * restrict xx = st->x + framesize;\r
601         register spx_word16_t * restrict y = st->y + framesize;\r
602         register spx_word16_t * restrict ee = st->e + framesize;\r
603         register int update_foreground;\r
604         register int i;\r
605     register int N = st->window_size;\r
606         register int M = st->M;\r
607                                                                                                                                                         \r
608 #ifdef FIXED_POINT                                                                                                                                                                                              \r
609         register spx_word32_t sc0 = SUB32(Sff,See);                                                                                                                                     \r
610         register spx_float_t sc1 = FLOAT_MUL32U(Sff,Dbf);                                                                                                                       \r
611                                                                                                                                                                                                                                 \r
612         Davg1 = ADD32(MULT16_32_Q15(QCONST16(.6f,15),Davg1), MULT16_32_Q15(QCONST16(.4f,15),sc0));                                      \r
613         Davg2 = ADD32(MULT16_32_Q15(QCONST16(.85f,15),Davg2), MULT16_32_Q15(QCONST16(.15f,15),sc0));                            \r
614         Dvar1 = FLOAT_ADD(                                                                                                                                                                                      \r
615                                 FLOAT_MULT(VAR1_SMOOTH,Dvar1),                                                                                                                                  \r
616                                 FLOAT_MUL32U(MULT16_32_Q15(QCONST16(.4f,15),Sff),                                                                                               \r
617                                 MULT16_32_Q15(QCONST16(.4f,15),Dbf)));                                                                                                                  \r
618         Dvar2 = FLOAT_ADD(                                                                                                                                                                                      \r
619                                 FLOAT_MULT(VAR2_SMOOTH,Dvar2),                                                                                                                                  \r
620                                 FLOAT_MUL32U(MULT16_32_Q15(QCONST16(.15f,15),Sff),                                                                                              \r
621                                 MULT16_32_Q15(QCONST16(.15f,15),Dbf)));                                                                                                                 \r
622 #else\r
623         register spx_word32_t sc0 = Sff - See;                                                                                                                                          \r
624         register spx_word32_t sc1 = Sff * Dbf;                                                                                                                                          \r
625                                                                                                                                                                                                                                 \r
626         Davg1 = .6*Davg1 + .4*sc0;                                                                                                                                                                      \r
627         Davg2 = .85*Davg2 + .15*sc0;                                                                                                                                                            \r
628         Dvar1 = VAR1_SMOOTH*Dvar1 + .16*sc1;                                                                                                                                            \r
629         Dvar2 = VAR2_SMOOTH*Dvar2 + .0225*sc1;                                                                                                                                          \r
630 #endif\r
631                                                                                                                                                                                                                                 \r
632         update_foreground =                                                                                                                                                                                     \r
633            mux( FLOAT_GT(FLOAT_MUL32U(sc0, VABS(sc0)), sc1), 1,                                                                                                         \r
634            mux( FLOAT_GT(FLOAT_MUL32U(Davg1, VABS(Davg1)), FLOAT_MULT(VAR1_UPDATE,(Dvar1))), 1,                                 \r
635            mux( FLOAT_GT(FLOAT_MUL32U(Davg2, VABS(Davg2)), FLOAT_MULT(VAR2_UPDATE,(Dvar2))), 1, 0)));                           \r
636 \r
637         if ( update_foreground )                                                                                                                                                                        \r
638         {                                                                                                                                                                                                                       \r
639                 register spx_word16_t * restrict windowf = st->window + framesize;                                                                              \r
640                 register spx_word16_t * restrict window = st->window;                                                                                                   \r
641                                                                                                                                                                                                                                 \r
642                 st->Davg1 = st->Davg2 = 0;                                                                                                                                                              \r
643                 st->Dvar1 = st->Dvar2 = FLOAT_ZERO;                                                                                                                                             \r
644                                                                                                                                                                                                                                 \r
645                 memcpy(st->foreground, st->W, N*M*sizeof(spx_word32_t));                                                                                                \r
646                                                                                                                                                                                                                                 \r
647 #if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION)                                                                                                                              \r
648 #pragma TCS_unroll=4                                                                                                                                                                                    \r
649 #pragma TCS_unrollexact=1                                                                                                                                                                               \r
650 #endif                                                                                                                                                                                                                  \r
651                 for ( i=0 ; i<framesize ; ++i)                                                                                                                                                  \r
652                 {       register spx_word16_t wi = window[i];                                                                                                                           \r
653                         register spx_word16_t wfi = windowf[i];                                                                                                                         \r
654                         register spx_word16_t ei = ee[i];                                                                                                                                       \r
655                         register spx_word16_t yi = y[i];                                                                                                                                        \r
656                                                                                                                                                                                                                                 \r
657                         ee[i] = MULT16_16_Q15(wfi,ei) + MULT16_16_Q15(wi,yi);                                                                                           \r
658                 }                                                                                                                                                                                                               \r
659 #if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION)                                                                                                                              \r
660 #pragma TCS_unrollexact=0                                                                                                                                                                               \r
661 #pragma TCS_unroll=0                                                                                                                                                                                    \r
662 #endif                                                                                                                                                                                                                  \r
663                                                                                                                                                                                                                                 \r
664         } else                                                                                                                                                                                                          \r
665         {                                                                                                                                                                                                                       \r
666                 register int reset_background;                                                                                                                                                  \r
667                                                                                                                                                                                                                                 \r
668                 reset_background =                                                                                                                                                                              \r
669                         mux( FLOAT_GT(FLOAT_MUL32U(-(sc0),VABS(sc0)), FLOAT_MULT(VAR_BACKTRACK,sc1)), 1,                                \r
670                         mux( FLOAT_GT(FLOAT_MUL32U(-(Davg1), VABS(Davg1)), FLOAT_MULT(VAR_BACKTRACK,Dvar1)), 1,                 \r
671                         mux( FLOAT_GT(FLOAT_MUL32U(-(Davg2), VABS(Davg2)), FLOAT_MULT(VAR_BACKTRACK,Dvar2)), 1, 0)));   \r
672                                                                                                                                                                                                                                 \r
673                 if ( reset_background )                                                                                                                                                                 \r
674                 {                                                                                                                                                                                                               \r
675                         memcpy(st->W, st->foreground, N*M*sizeof(spx_word32_t));                                                                                        \r
676                         memcpy(y, ee, framesize * sizeof(spx_word16_t));                                                                                                        \r
677                         mdf_sub(xx,input,y,framesize);                                                                                                                                          \r
678                         See = Sff;                                                                                                                                                                                      \r
679                         st->Davg1 = st->Davg2 = 0;                                                                                                                                                      \r
680                         st->Dvar1 = st->Dvar2 = FLOAT_ZERO;                                                                                                                                     \r
681                 } else\r
682                 {\r
683                         st->Davg1 = Davg1;\r
684                         st->Davg2 = Davg2;\r
685                         st->Dvar1 = Dvar1;\r
686                         st->Dvar2 = Dvar2;\r
687                 }\r
688         }\r
689 \r
690         return See;\r
691 }       \r
692 #endif\r
693                                                                                                                                                                    \r
694 void mdf_compute_error_signal(\r
695         SpeexEchoState * restrict st,\r
696         const spx_int16_t * restrict in,\r
697         spx_int16_t * restrict out,\r
698         int framesize\r
699 )\r
700 {\r
701         register spx_word16_t preemph = st->preemph;\r
702         register spx_word16_t memE = st->memE;\r
703         register int saturated = st->saturated;\r
704         register spx_word16_t * restrict e = st->e;\r
705         register spx_word16_t * restrict ee = st->e + framesize;\r
706         register spx_word16_t * restrict input = st->input;\r
707         register spx_word16_t * restrict xx = st->x + framesize;\r
708         register int i;\r
709 \r
710 #if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION)\r
711 #pragma TCS_unroll=4\r
712 #pragma TCS_unrollexact=1\r
713 #endif\r
714         for ( i=0 ; i<framesize ; ++i )\r
715         {\r
716                 register spx_word32_t tmp_out;\r
717                 register spx_int16_t ini = in[i];\r
718                 register int flg;\r
719 \r
720 #ifdef FIXED_POINT\r
721 \r
722 #ifdef TWO_PATH\r
723                 tmp_out = SUB32(EXTEND32(input[i]), EXTEND32(ee[i]));\r
724                 tmp_out = iclipi(tmp_out,32767);\r
725 #else\r
726                 tmp_out = SUB32(EXTEND32(input[i]), EXTEND32(y[i]));\r
727                 tmp_out = iclipi(tmp_out,32767);\r
728 #endif\r
729 \r
730 #else\r
731 #ifdef TWO_PATH\r
732                 tmp_out = SUB32(EXTEND32(input[i]), EXTEND32(ee[i]));\r
733 #else\r
734                 tmp_out = SUB32(EXTEND32(input[i]), EXTEND32(y[i]));\r
735 #endif\r
736                 tmp_out = \r
737                         fmux( tmp_out > 32767, 32767,\r
738                         fmux( tmp_out < -32768, -32768, tmp_out));\r
739 #endif\r
740 \r
741                 tmp_out = ADD32(tmp_out, EXTEND32(MULT16_16_P15(preemph,memE)));\r
742                 flg = iabs(ini) >= 32000;\r
743                 tmp_out = VMUX( flg, 0, tmp_out);\r
744                 saturated = mux( flg && (saturated == 0), 1, saturated);\r
745 \r
746                 out[i] = (spx_int16_t)tmp_out;\r
747                 memE = tmp_out;\r
748         }\r
749 #if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION)\r
750 #pragma TCS_unrollexact=0\r
751 #pragma TCS_unroll=0\r
752 #endif \r
753 \r
754         st->memE = memE;\r
755         st->saturated = saturated;\r
756         memset(e, 0, framesize * sizeof(spx_word16_t));\r
757         memcpy(ee, xx, framesize * sizeof(spx_word16_t));\r
758 }\r
759 \r
760 inline int mdf_check(\r
761         SpeexEchoState * restrict st,\r
762         spx_int16_t * out,\r
763         spx_word32_t Syy, \r
764         spx_word32_t Sxx, \r
765         spx_word32_t See,\r
766         spx_word32_t Sff,\r
767         spx_word32_t Sdd\r
768 )\r
769 {\r
770     register int N = st->window_size;\r
771         register spx_word32_t N1e9 = N * 1e9;\r
772         register int screwed_up = st->screwed_up;\r
773         register int framesize = st->frame_size;\r
774 \r
775         if (!(Syy>=0 && Sxx>=0 && See >= 0)\r
776 #ifndef FIXED_POINT\r
777                 || !(Sff < N1e9 && Syy < N1e9 && Sxx < N1e9 )\r
778 #endif\r
779       )\r
780         {    \r
781                 screwed_up += 50;\r
782                 memset(out, 0, framesize * sizeof(spx_int16_t));\r
783 \r
784         } else\r
785         {       screwed_up = mux( SHR32(Sff, 2) > ADD32(Sdd, SHR32(MULT16_16(N, 10000),6)), screwed_up+1, 0);\r
786         }\r
787 \r
788         st->screwed_up = screwed_up;\r
789 \r
790         return screwed_up;\r
791 }\r
792 \r
793 void mdf_smooth(\r
794         spx_word32_t * restrict power,\r
795         spx_word32_t * restrict Xf,\r
796         int framesize,\r
797         int M\r
798 )\r
799 {\r
800     register spx_word16_t ss, ss_1, pf, xff;\r
801         register int j;\r
802 \r
803 #ifdef FIXED_POINT\r
804         ss=DIV32_16(11469,M);\r
805         ss_1 = SUB16(32767,ss);\r
806 #else\r
807         ss=.35/M;\r
808         ss_1 = 1-ss;\r
809 #endif\r
810 \r
811 #if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION)\r
812 #pragma TCS_unroll=4\r
813 #pragma TCS_unrollexact=1\r
814 #endif  \r
815         for ( j=0 ; j<framesize ; ++j )\r
816         {       register spx_word32_t pi  = power[j];\r
817                 register spx_word32_t xfi = Xf[j];\r
818 \r
819                 power[j] = MULT16_32_Q15(ss_1,pi) + 1 + MULT16_32_Q15(ss,xfi);\r
820         }\r
821 #if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION)\r
822 #pragma TCS_unrollexact=0\r
823 #pragma TCS_unroll=0\r
824 #endif \r
825 \r
826         pf = power[framesize];\r
827         xff = Xf[framesize];\r
828         power[framesize] = MULT16_32_Q15(ss_1,pf) + 1 + MULT16_32_Q15(ss,xff);\r
829 }\r
830 \r
831 void mdf_compute_filtered_spectra_crosscorrelations(\r
832         SpeexEchoState * restrict st,\r
833         spx_word32_t Syy, \r
834         spx_word32_t See,\r
835         int framesize\r
836 )\r
837 {   \r
838         register spx_float_t Pey = FLOAT_ONE;\r
839         register spx_float_t Pyy = FLOAT_ONE;\r
840         register spx_word16_t spec_average = st->spec_average;\r
841         register spx_word32_t * restrict pRf = st->Rf;\r
842         register spx_word32_t * restrict pYf = st->Yf;\r
843         register spx_word32_t * restrict pEh = st->Eh;\r
844         register spx_word32_t * restrict pYh = st->Yh;\r
845         register spx_word16_t beta0 = st->beta0;\r
846         register spx_word16_t beta_max = st->beta_max;\r
847         register spx_float_t alpha, alpha_1;\r
848         register spx_word32_t tmp32, tmpx;\r
849         register spx_float_t sPey = st->Pey;\r
850         register spx_float_t sPyy = st->Pyy;\r
851         register spx_float_t tmp;\r
852         register spx_word16_t leak_estimate;\r
853         register int j;\r
854         register spx_float_t  Eh, Yh;\r
855         register spx_word32_t _Ehj, _Rfj, _Yfj, _Yhj;\r
856 \r
857 #ifdef FIXED_POINT\r
858         register spx_word16_t spec_average1 = SUB16(32767,spec_average);\r
859 #else\r
860         register spx_word16_t spec_average1 = 1 - spec_average;\r
861 #endif\r
862 \r
863 #if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION)\r
864 #pragma TCS_unroll=4\r
865 #pragma TCS_unrollexact=1\r
866 #endif  \r
867         for (j=framesize; j>0 ; --j)\r
868         {\r
869                 _Ehj = pEh[j];\r
870                 _Rfj = pRf[j];\r
871                 _Yfj = pYf[j];\r
872                 _Yhj = pYh[j];\r
873 \r
874                 Eh = PSEUDOFLOAT(_Rfj - _Ehj);\r
875                 Yh = PSEUDOFLOAT(_Yfj - _Yhj);\r
876 \r
877                 Pey = FLOAT_ADD(Pey,FLOAT_MULT(Eh,Yh));\r
878                 Pyy = FLOAT_ADD(Pyy,FLOAT_MULT(Yh,Yh));\r
879 \r
880                 pEh[j] = MAC16_32_Q15(MULT16_32_Q15(spec_average1, _Ehj), spec_average, _Rfj);\r
881                 pYh[j] = MAC16_32_Q15(MULT16_32_Q15(spec_average1, _Yhj), spec_average, _Yfj);\r
882    }\r
883 #if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION)\r
884 #pragma TCS_unrollexact=0\r
885 #pragma TCS_unroll=0\r
886 #endif \r
887         _Ehj = pEh[0];\r
888         _Rfj = pRf[0];\r
889         _Yfj = pYf[0];\r
890         _Yhj = pYh[0];\r
891 \r
892         Eh = PSEUDOFLOAT(_Rfj - _Ehj);\r
893         Yh = PSEUDOFLOAT(_Yfj - _Yhj);\r
894 \r
895         Pey = FLOAT_ADD(Pey,FLOAT_MULT(Eh,Yh));\r
896         Pyy = FLOAT_ADD(Pyy,FLOAT_MULT(Yh,Yh));\r
897 \r
898         pEh[0] = MAC16_32_Q15(MULT16_32_Q15(spec_average1, _Ehj), spec_average, _Rfj);\r
899         pYh[0] = MAC16_32_Q15(MULT16_32_Q15(spec_average1, _Yhj), spec_average, _Yfj);\r
900 \r
901         Pyy = FLOAT_SQRT(Pyy);\r
902         Pey = FLOAT_DIVU(Pey,Pyy);\r
903 \r
904         tmp32 = MULT16_32_Q15(beta0,Syy);\r
905         tmpx = MULT16_32_Q15(beta_max,See);\r
906         tmp32 = VMUX(tmp32 > tmpx, tmpx, tmp32);\r
907         alpha = FLOAT_DIV32(tmp32, See);\r
908         alpha_1 = FLOAT_SUB(FLOAT_ONE, alpha);\r
909    \r
910         sPey = FLOAT_ADD(FLOAT_MULT(alpha_1,sPey) , FLOAT_MULT(alpha,Pey));\r
911         sPyy = FLOAT_ADD(FLOAT_MULT(alpha_1,sPyy) , FLOAT_MULT(alpha,Pyy));   \r
912         tmp = FLOAT_MULT(MIN_LEAK,sPyy);\r
913 \r
914 #ifndef FIXED_POINT\r
915         sPyy = VMUX(FLOAT_LT(sPyy, FLOAT_ONE), FLOAT_ONE, sPyy);\r
916         sPey = VMUX(FLOAT_LT(sPey, tmp), tmp, sPey);\r
917         sPey = VMUX(FLOAT_LT(sPey, sPyy), sPey, sPyy); \r
918 #else\r
919         sPyy = FLOAT_LT(sPyy, FLOAT_ONE) ? FLOAT_ONE : sPyy;\r
920         sPey = FLOAT_LT(sPey, tmp) ? tmp : sPey;\r
921         sPey = FLOAT_LT(sPey, sPyy) ? sPey : sPyy; \r
922 #endif\r
923         \r
924         leak_estimate = FLOAT_EXTRACT16(FLOAT_SHL(FLOAT_DIVU(sPey, sPyy),14));\r
925 \r
926         leak_estimate = VMUX( leak_estimate > 16383, 32767, SHL16(leak_estimate,1));\r
927         st->Pey = sPey;\r
928         st->Pyy = sPyy;\r
929         st->leak_estimate = leak_estimate;\r
930 }\r
931 \r
932 inline spx_word16_t mdf_compute_RER(\r
933         spx_word32_t See,\r
934         spx_word32_t Syy,\r
935         spx_word32_t Sey,\r
936         spx_word32_t Sxx,\r
937         spx_word16_t leake\r
938 )\r
939 {\r
940         register spx_word16_t RER;\r
941 \r
942 #ifdef FIXED_POINT\r
943         register spx_word32_t tmp32;\r
944         register spx_word32_t tmp;\r
945         spx_float_t bound = PSEUDOFLOAT(Sey);\r
946               \r
947         tmp32 = MULT16_32_Q15(leake,Syy);\r
948         tmp32 = ADD32(SHR32(Sxx,13), ADD32(tmp32, SHL32(tmp32,1)));\r
949    \r
950         bound = FLOAT_DIVU(FLOAT_MULT(bound, bound), PSEUDOFLOAT(ADD32(1,Syy)));\r
951         tmp   = FLOAT_EXTRACT32(bound);\r
952         tmp32 = imux( FLOAT_GT(bound, PSEUDOFLOAT(See)), See,\r
953                         imux( tmp32 < tmp,  tmp, tmp32));\r
954 \r
955         tmp   = SHR32(See,1);\r
956         tmp32 = imux(tmp32 > tmp, tmp, tmp32);\r
957         RER   = FLOAT_EXTRACT16(FLOAT_SHL(FLOAT_DIV32(tmp32,See),15));\r
958 #else\r
959         register spx_word32_t r0;\r
960 \r
961         r0 = (Sey * Sey)/(1 + See * Syy);\r
962 \r
963         RER = (.0001*Sxx + 3.* MULT16_32_Q15(leake,Syy)) / See;\r
964         RER = fmux( RER < r0, r0, RER);\r
965         RER = fmux( RER > .5, .5, RER);\r
966 #endif\r
967 \r
968         return RER;\r
969 }\r
970 \r
971 void mdf_adapt(\r
972         SpeexEchoState * restrict st,\r
973         spx_word16_t RER,\r
974         spx_word32_t Syy,\r
975         spx_word32_t See, \r
976         spx_word32_t Sxx\r
977 )\r
978 {\r
979         register spx_float_t  * restrict power_1 = st->power_1;\r
980         register spx_word32_t  * restrict power = st->power;\r
981         register int adapted = st->adapted;\r
982         register spx_word32_t sum_adapt = st->sum_adapt;\r
983         register spx_word16_t leake = st->leak_estimate;\r
984         register int framesize = st->frame_size;\r
985         register int i;\r
986         register int M = st->M;\r
987 \r
988         adapted = mux( !adapted && sum_adapt > QCONST32(M,15) && \r
989                                         MULT16_32_Q15(leake,Syy) > MULT16_32_Q15(QCONST16(.03f,15),Syy), 1, adapted);\r
990 \r
991         if ( adapted )\r
992         {       register spx_word32_t * restrict Yf = st->Yf; \r
993                 register spx_word32_t * restrict Rf = st->Rf;\r
994                 register spx_word32_t r, e, e2;\r
995 \r
996 #if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION)\r
997 #pragma TCS_unroll=4\r
998 #pragma TCS_unrollexact=1\r
999 #endif  \r
1000                 for ( i=0 ; i<framesize ; ++i )\r
1001                 {       \r
1002                         r = SHL32(Yf[i],3);\r
1003                         r = MULT16_32_Q15(leake,r);\r
1004                         e = SHL32(Rf[i],3)+1;\r
1005 \r
1006 #ifdef FIXED_POINT\r
1007                         e2 = SHR32(e,1);\r
1008                         r = mux( r > e2, e2, r);\r
1009 #else\r
1010                         e2 = e * .5;\r
1011                         r = fmux( r > e2, e2, r);\r
1012 #endif\r
1013 \r
1014                         r = MULT16_32_Q15(QCONST16(.7,15),r) + \r
1015                                 MULT16_32_Q15(QCONST16(.3,15),(spx_word32_t)(MULT16_32_Q15(RER,e)));\r
1016 \r
1017                         power_1[i] = FLOAT_SHL(FLOAT_DIV32_FLOAT(r,FLOAT_MUL32U(e,power[i]+10)),WEIGHT_SHIFT+16);\r
1018                 }\r
1019 #if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION)\r
1020 #pragma TCS_unrollexact=0\r
1021 #pragma TCS_unroll=0\r
1022 #endif \r
1023                 \r
1024                 r = SHL32(Yf[framesize],3);\r
1025                 r = MULT16_32_Q15(leake,r);\r
1026                 e = SHL32(Rf[framesize],3)+1;\r
1027 \r
1028 #ifdef FIXED_POINT\r
1029                 e2 = SHR32(e,1);\r
1030                 r = mux( r > e2, e2, r);\r
1031 #else\r
1032                 e2 = e * .5;\r
1033                 r = fmux( r > e2, e2, r);\r
1034 #endif\r
1035 \r
1036                 r = MULT16_32_Q15(QCONST16(.7,15),r) + \r
1037                         MULT16_32_Q15(QCONST16(.3,15),(spx_word32_t)(MULT16_32_Q15(RER,e)));\r
1038 \r
1039                 power_1[framesize] = FLOAT_SHL(FLOAT_DIV32_FLOAT(r,FLOAT_MUL32U(e,power[framesize]+10)),WEIGHT_SHIFT+16);\r
1040 \r
1041         } else \r
1042         {\r
1043                 register spx_word16_t adapt_rate=0;\r
1044                 register int N = st->window_size;\r
1045 \r
1046                 if ( Sxx > SHR32(MULT16_16(N, 1000),6) ) \r
1047                 {       register spx_word32_t tmp32, tmp32q;\r
1048          \r
1049                         tmp32 = MULT16_32_Q15(QCONST16(.25f, 15), Sxx);\r
1050 #ifdef FIXED_POINT\r
1051                         tmp32q = SHR32(See,2);\r
1052                         tmp32  = mux(tmp32 > tmp32q, tmp32q, tmp32);\r
1053 #else\r
1054                         tmp32q = 0.25 * See;\r
1055                         tmp32 = fmux(tmp32 > tmp32q, tmp32q, tmp32);\r
1056 #endif\r
1057                         adapt_rate = FLOAT_EXTRACT16(FLOAT_SHL(FLOAT_DIV32(tmp32, See),15));\r
1058                 }\r
1059       \r
1060 #if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION)\r
1061 #pragma TCS_unroll=4\r
1062 #pragma TCS_unrollexact=1\r
1063 #endif                  \r
1064                 for (i=0;i<framesize;i++)\r
1065                         power_1[i] = FLOAT_SHL(FLOAT_DIV32(EXTEND32(adapt_rate),ADD32(power[i],10)),WEIGHT_SHIFT+1);\r
1066 #if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION)\r
1067 #pragma TCS_unrollexact=0\r
1068 #pragma TCS_unroll=0\r
1069 #endif \r
1070                 power_1[framesize] = FLOAT_SHL(FLOAT_DIV32(EXTEND32(adapt_rate),ADD32(power[framesize],10)),WEIGHT_SHIFT+1);\r
1071                 sum_adapt = ADD32(sum_adapt,adapt_rate);\r
1072         }\r
1073         \r
1074         st->sum_adapt = sum_adapt;\r
1075         st->adapted = adapted;\r
1076 }\r
1077 \r
1078 #define OVERRIDE_ECHO_CANCELLATION\r
1079 void speex_echo_cancellation(\r
1080         SpeexEchoState * restrict st, \r
1081         const spx_int16_t * restrict in, \r
1082         const spx_int16_t * restrict far_end, \r
1083         spx_int16_t * restrict out\r
1084 )\r
1085\r
1086         register int framesize = st->frame_size;\r
1087         register spx_word16_t * restrict x = st->x;\r
1088         register spx_word16_t * restrict xx = st->x + framesize;\r
1089         register spx_word16_t * restrict yy = st->y + framesize;\r
1090         register spx_word16_t * restrict ee = st->e + framesize;\r
1091         register spx_word32_t Syy, See, Sxx, Sdd, Sff;\r
1092         register spx_word16_t RER;\r
1093         register spx_word32_t Sey;\r
1094         register int j;\r
1095         register int N,M;\r
1096 #ifdef TWO_PATH\r
1097         register spx_word32_t Dbf;\r
1098 #endif\r
1099 \r
1100         N = st->window_size;\r
1101         M = st->M;\r
1102         st->cancel_count++;\r
1103 \r
1104         filter_dc_notch16(in, st->notch_radius, st->input, framesize, st->notch_mem);\r
1105         mdf_preemph(st, xx, far_end, framesize);\r
1106    \r
1107         {\r
1108 \r
1109         register spx_word16_t * restrict X = st->X;\r
1110 \r
1111         for ( j=M-1 ; j>=0 ; j-- )\r
1112         {       register spx_word16_t * restrict Xdes = &(X[(j+1)*N]);\r
1113                 register spx_word16_t * restrict Xsrc = &(X[j*N]);\r
1114 \r
1115                 memcpy(Xdes, Xsrc, N * sizeof(spx_word16_t));\r
1116         }\r
1117 \r
1118         spx_fft(st->fft_table, x, X);\r
1119         memcpy(st->last_y, st->x, N * sizeof(spx_word16_t));\r
1120         Sxx = mdf_inner_prod(xx, xx, framesize);\r
1121         memcpy(x, xx, framesize * sizeof(spx_word16_t));\r
1122         \r
1123 #ifdef TWO_PATH\r
1124         spectral_mul_accum(st->X, st->foreground, st->Y, N, M);   \r
1125         spx_ifft(st->fft_table, st->Y, st->e);\r
1126         mdf_sub(xx, st->input, ee, framesize);\r
1127         Sff = mdf_inner_prod(xx, xx, framesize);\r
1128 #endif\r
1129    \r
1130         mdf_adjust_prop (st->W, N, M, st->prop);\r
1131   \r
1132         if (st->saturated == 0)\r
1133         {       mdf_compute_weight_gradient(st, X, N, M);\r
1134         } else \r
1135         {       st->saturated--;\r
1136         }\r
1137         }\r
1138 \r
1139         mdf_update_weight(st, N, M, framesize);\r
1140         spectral_mul_accum(st->X, st->W, st->Y, N, M);\r
1141         spx_ifft(st->fft_table, st->Y, st->y);\r
1142 \r
1143 #ifdef TWO_PATH\r
1144         mdf_sub(xx, ee, yy, framesize);   \r
1145         Dbf = 10+mdf_inner_prod(xx, xx, framesize);\r
1146 #endif\r
1147 \r
1148         mdf_sub(xx, st->input, yy, framesize);\r
1149         See = mdf_inner_prod(xx, xx, framesize);\r
1150 \r
1151 #ifndef TWO_PATH\r
1152         Sff = See;\r
1153 #else\r
1154         See = mdf_update_foreground(st,Dbf,Sff,See);\r
1155 #endif\r
1156 \r
1157         \r
1158         mdf_compute_error_signal(st, in, out, framesize);\r
1159         Sey = mdf_inner_prod(ee, yy, framesize);\r
1160         Syy = mdf_inner_prod(yy, yy, framesize);\r
1161         Sdd = mdf_inner_prod(st->input, st->input, framesize);\r
1162    \r
1163         if ( mdf_check(st,out,Syy,Sxx,See,Sff,Sdd) >= 50 )\r
1164         {       speex_warning("The echo canceller started acting funny and got slapped (reset). It swears it will behave now.");\r
1165                 speex_echo_state_reset(st);\r
1166                 return;\r
1167         }\r
1168 \r
1169         See = MAX32(See, SHR32(MULT16_16(N, 100),6));\r
1170         spx_fft(st->fft_table, st->e, st->E);\r
1171         memset(st->y, 0, framesize * sizeof(spx_word16_t));\r
1172         spx_fft(st->fft_table, st->y, st->Y);\r
1173         power_spectrum(st->E, st->Rf, N);\r
1174         power_spectrum(st->Y, st->Yf, N);\r
1175         power_spectrum(st->X, st->Xf, N);\r
1176 \r
1177         mdf_smooth(st->power,st->Xf,framesize, M);\r
1178         mdf_compute_filtered_spectra_crosscorrelations(st,Syy,See,framesize);\r
1179         RER = mdf_compute_RER(See,Syy,Sey,Sxx,st->leak_estimate);\r
1180         mdf_adapt(st, RER, Syy, See, Sxx);\r
1181         \r
1182         if ( st->adapted )\r
1183         {       register spx_word16_t * restrict last_yy = st->last_y + framesize;\r
1184                 \r
1185                 memcpy(st->last_y, last_yy, framesize * sizeof(spx_word16_t));\r
1186                 mdf_sub_int(last_yy, in, out, framesize);\r
1187 \r
1188         }\r
1189 }\r
1190 \r
1191 \r
1192 \r