4038ff27edceaf8a6e0e357956304b2c3910e1fe
[dahdi/linux.git] / drivers / dahdi / dahdi_echocan_mg2.c
1 /*
2  * ECHO_CAN_MG2
3  *
4  * by Michael Gernoth
5  *
6  * Based upon kb1ec.h and mec2.h
7  * 
8  * Copyright (C) 2002-2012, Digium, Inc.
9  *
10  * Additional background on the techniques used in this code can be found in:
11  *
12  *  Messerschmitt, David; Hedberg, David; Cole, Christopher; Haoui, Amine; 
13  *  Winship, Peter; "Digital Voice Echo Canceller with a TMS32020," 
14  *  in Digital Signal Processing Applications with the TMS320 Family, 
15  *  pp. 415-437, Texas Instruments, Inc., 1986. 
16  *
17  * A pdf of which is available by searching on the document title at http://www.ti.com/
18  *
19  */
20
21 /*
22  * See http://www.asterisk.org for more information about
23  * the Asterisk project. Please do not directly contact
24  * any of the maintainers of this project for assistance;
25  * the project provides a web site, mailing lists and IRC
26  * channels for your use.
27  *
28  * This program is free software, distributed under the terms of
29  * the GNU General Public License Version 2 as published by the
30  * Free Software Foundation. See the LICENSE file included with
31  * this program for more details.
32  */
33
34 #include <linux/kernel.h>
35 #include <linux/slab.h>
36 #include <linux/errno.h>
37 #include <linux/module.h>
38 #include <linux/init.h>
39 #include <linux/ctype.h>
40 #include <linux/moduleparam.h>
41
42 #include <dahdi/kernel.h>
43
44 static int debug;
45 static int aggressive;
46
47 #define ABS(a) abs(a!=-32768?a:-32767)
48
49 #define RESTORE_COEFFS {\
50                                 int x;\
51                                 memcpy(pvt->a_i, pvt->c_i, pvt->N_d*sizeof(int));\
52                                 for (x = 0; x < pvt->N_d; x++) {\
53                                         pvt->a_s[x] = pvt->a_i[x] >> 16;\
54                                 }\
55                                 pvt->backup = BACKUP;\
56                         }
57
58 /* Uncomment to provide summary statistics for overall echo can performance every 4000 samples */ 
59 /* #define MEC2_STATS 4000 */
60
61 /* Uncomment to generate per-sample statistics - this will severely degrade system performance and audio quality */
62 /* #define MEC2_STATS_DETAILED */
63
64 /* Uncomment to generate per-call DC bias offset messages */
65 /* #define MEC2_DCBIAS_MESSAGE */
66
67 /* Get optimized routines for math */
68 #include "arith.h"
69
70 /*
71    Important constants for tuning mg2 echo can
72  */
73
74 /* Convergence (aka. adaptation) speed -- higher means slower */
75 #define DEFAULT_BETA1_I 2048
76
77 /* Constants for various power computations */
78 #define DEFAULT_SIGMA_LY_I 7
79 #define DEFAULT_SIGMA_LU_I 7
80 #define DEFAULT_ALPHA_ST_I 5            /* near-end speech detection sensitivity factor */
81 #define DEFAULT_ALPHA_YT_I 5
82
83 #define DEFAULT_CUTOFF_I 128
84
85 /* Define the near-end speech hangover counter: if near-end speech 
86  *  is declared, hcntr is set equal to hangt (see pg. 432)
87  */
88 #define DEFAULT_HANGT 600               /* in samples, so 600 samples = 75ms */
89
90 /* define the residual error suppression threshold */
91 #define DEFAULT_SUPPR_I 16              /* 16 = -24db */
92
93 /* This is the minimum reference signal power estimate level 
94  *  that will result in filter adaptation.
95  * If this is too low then background noise will cause the filter 
96  *  coefficients to constantly be updated.
97  */
98 #define MIN_UPDATE_THRESH_I 2048
99
100 /* The number of samples used to update coefficients using the
101  *  the block update method (M). It should be related back to the 
102  *  length of the echo can.
103  * ie. it only updates coefficients when (sample number MOD default_m) = 0
104  *
105  *  Getting this wrong may cause an oops. Consider yourself warned!
106  */
107 #define DEFAULT_M 16                    /* every 16th sample */
108
109 /* If AGGRESSIVE supression is enabled, then we start cancelling residual 
110  * echos again even while there is potentially the very end of a near-side 
111  *  signal present.
112  * This defines how many samples of DEFAULT_HANGT can remain before we
113  *  kick back in
114  */
115 #define AGGRESSIVE_HCNTR 160            /* in samples, so 160 samples = 20ms */
116
117 /* Treat sample as error if it has a different sign as the
118  * input signal and is this number larger in ABS() as
119  * the input-signal */
120 #define MAX_SIGN_ERROR 3000
121
122 /* Number of coefficients really used for calculating the
123  * simulated echo. The value specifies how many of the
124  * biggest coefficients are used for calculating rs.
125  * This helps on long echo-tails by artificially limiting
126  * the number of coefficients for the calculation and
127  * preventing overflows.
128  * Comment this to deactivate the code */
129 #define USED_COEFFS 64
130
131 /* Backup coefficients every this number of samples */
132 #define BACKUP 256
133
134 /***************************************************************/
135 /* The following knobs are not implemented in the current code */
136
137 /* we need a dynamic level of suppression varying with the ratio of the 
138    power of the echo to the power of the reference signal this is 
139    done so that we have a  smoother background.                 
140    we have a higher suppression when the power ratio is closer to
141    suppr_ceil and reduces logarithmically as we approach suppr_floor.
142  */
143 #define SUPPR_FLOOR -64
144 #define SUPPR_CEIL -24
145
146 /* in a second departure, we calculate the residual error suppression
147  * as a percentage of the reference signal energy level. The threshold
148  * is defined in terms of dB below the reference signal.
149  */
150 #define RES_SUPR_FACTOR -20
151
152 #define DC_NORMALIZE
153
154 #ifndef NULL
155 #define NULL 0
156 #endif
157 #ifndef FALSE
158 #define FALSE 0
159 #endif
160 #ifndef TRUE
161 #define TRUE (!FALSE)
162 #endif
163
164 /* Generic circular buffer definition */
165 typedef struct {
166         /* Pointer to the relative 'start' of the buffer */
167         int idx_d;
168         /* The absolute size of the buffer */
169         int size_d;                      
170         /* The actual sample -  twice as large as we need, however we do store values at idx_d and idx_d+size_d */
171         short *buf_d;                   
172 } echo_can_cb_s;
173
174 static int echo_can_create(struct dahdi_chan *chan, struct dahdi_echocanparams *ecp,
175                            struct dahdi_echocanparam *p, struct dahdi_echocan_state **ec);
176 static void echo_can_free(struct dahdi_chan *chan, struct dahdi_echocan_state *ec);
177 static void echo_can_process(struct dahdi_echocan_state *ec, short *isig, const short *iref, u32 size);
178 static int echo_can_traintap(struct dahdi_echocan_state *ec, int pos, short val);
179 static void echocan_NLP_toggle(struct dahdi_echocan_state *ec, unsigned int enable);
180 static const char *name = "MG2";
181 static const char *ec_name(const struct dahdi_chan *chan) { return name; }
182
183 static const struct dahdi_echocan_factory my_factory = {
184         .get_name = ec_name,
185         .owner = THIS_MODULE,
186         .echocan_create = echo_can_create,
187 };
188
189 static const struct dahdi_echocan_features my_features = {
190         .NLP_toggle = 1,
191 };
192
193 static const struct dahdi_echocan_ops my_ops = {
194         .echocan_free = echo_can_free,
195         .echocan_process = echo_can_process,
196         .echocan_traintap = echo_can_traintap,
197         .echocan_NLP_toggle = echocan_NLP_toggle,
198 };
199
200 struct ec_pvt {
201         struct dahdi_echocan_state dahdi;
202         /* an arbitrary ID for this echo can - this really should be settable from the calling channel... */
203         int id;
204
205         /* absolute time - aka. sample number index - essentially the number of samples since this can was init'ed */
206         int i_d;
207   
208         /* Pre-computed constants */
209         /* ---------------------- */
210         /* Number of filter coefficents */
211         int N_d;
212         /* Rate of adaptation of filter */
213         int beta2_i;
214
215         /* Accumulators for power computations */
216         /* ----------------------------------- */
217         /* reference signal power estimate - aka. Average absolute value of y(k) */
218         int Ly_i;                       
219         /* ... */
220         int Lu_i;
221
222         /* Accumulators for signal detectors */
223         /* --------------------------------- */
224         /* Power estimate of the recent past of the near-end hybrid signal - aka. Short-time average of: 2 x |s(i)| */
225         int s_tilde_i;          
226         /* Power estimate of the recent past of the far-end receive signal - aka. Short-time average of:     |y(i)| */
227         int y_tilde_i;
228
229         /* Near end speech detection counter - stores Hangover counter time remaining, in samples */
230         int HCNTR_d;                    
231   
232         /* Circular buffers and coefficients */
233         /* --------------------------------- */
234         /* ... */
235         int *a_i;
236         /* ... */
237         short *a_s;
238         /* Backups */
239         int *b_i;
240         int *c_i;
241         /* Reference samples of far-end receive signal */
242         echo_can_cb_s y_s;
243         /* Reference samples of near-end signal */
244         echo_can_cb_s s_s;
245         /* Reference samples of near-end signal minus echo estimate */
246         echo_can_cb_s u_s;
247         /* Reference samples of far-end receive signal used to calculate short-time average */
248         echo_can_cb_s y_tilde_s;
249
250         /* Peak far-end receive signal */
251         /* --------------------------- */
252         /* Highest y_tilde value in the sample buffer */
253         short max_y_tilde;
254         /* Index of the sample containing the max_y_tilde value */
255         int max_y_tilde_pos;
256
257 #ifdef MEC2_STATS
258         /* Storage for performance statistics */
259         int cntr_nearend_speech_frames;
260         int cntr_residualcorrected_frames;
261         int cntr_residualcorrected_framesskipped;
262         int cntr_coeff_updates;
263         int cntr_coeff_missedupdates;
264  
265         int avg_Lu_i_toolow; 
266         int avg_Lu_i_ok;
267 #endif 
268         unsigned int aggressive:1;
269         short lastsig;
270         int lastcount;
271         int backup;
272 #ifdef DC_NORMALIZE
273         int dc_estimate;
274 #endif
275         int use_nlp;
276 };
277
278 #define dahdi_to_pvt(a) container_of(a, struct ec_pvt, dahdi)
279
280 static inline void init_cb_s(echo_can_cb_s *cb, int len, void *where)
281 {
282         cb->buf_d = (short *)where;
283         cb->idx_d = 0;
284         cb->size_d = len;
285 }
286
287 static inline void add_cc_s(echo_can_cb_s *cb, short newval)
288 {
289         /* Can't use modulus because N+M isn't a power of two (generally) */
290         cb->idx_d--;
291         if (cb->idx_d < (int)0) 
292                 /* Whoops - the pointer to the 'start' wrapped around so reset it to the top of the buffer */
293                 cb->idx_d += cb->size_d;
294         
295         /* Load two copies into memory */
296         cb->buf_d[cb->idx_d] = newval;
297         cb->buf_d[cb->idx_d + cb->size_d] = newval;
298 }
299
300 static inline short get_cc_s(echo_can_cb_s *cb, int pos)
301 {
302         /* Load two copies into memory */
303         return cb->buf_d[cb->idx_d + pos];
304 }
305
306 static inline void init_cc(struct ec_pvt *pvt, int N, int maxy, int maxu)
307 {
308         char *ptr = (char *) pvt;
309         unsigned long tmp;
310
311         /* Double-word align past end of state */
312         ptr += sizeof(*pvt);
313         tmp = (unsigned long)ptr;
314         tmp += 3;
315         tmp &= ~3L;
316         ptr = (void *)tmp;
317
318         /* Reset parameters */
319         pvt->N_d = N;
320         pvt->beta2_i = DEFAULT_BETA1_I;
321   
322         /* Allocate coefficient memory */
323         pvt->a_i = (int *) ptr;
324         ptr += (sizeof(int) * pvt->N_d);
325         pvt->a_s = (short *) ptr;
326         ptr += (sizeof(short) * pvt->N_d);
327
328         /* Allocate backup memory */
329         pvt->b_i = (int *) ptr;
330         ptr += (sizeof(int) * pvt->N_d);
331         pvt->c_i = (int *) ptr;
332         ptr += (sizeof(int) * pvt->N_d);
333
334         /* Reset Y circular buffer (short version) */
335         init_cb_s(&pvt->y_s, maxy, ptr);
336         ptr += (sizeof(short) * (maxy) * 2);
337   
338         /* Reset Sigma circular buffer (short version for FIR filter) */
339         init_cb_s(&pvt->s_s, (1 << DEFAULT_ALPHA_ST_I), ptr);
340         ptr += (sizeof(short) * (1 << DEFAULT_ALPHA_ST_I) * 2);
341
342         init_cb_s(&pvt->u_s, maxu, ptr);
343         ptr += (sizeof(short) * maxu * 2);
344
345         /* Allocate a buffer for the reference signal power computation */
346         init_cb_s(&pvt->y_tilde_s, pvt->N_d, ptr);
347
348         /* Reset the absolute time index */
349         pvt->i_d = (int)0;
350   
351         /* Reset the power computations (for y and u) */
352         pvt->Ly_i = DEFAULT_CUTOFF_I;
353         pvt->Lu_i = DEFAULT_CUTOFF_I;
354
355 #ifdef MEC2_STATS
356         /* set the identity */
357         pvt->id = (int)&ptr;
358   
359         /* Reset performance stats */
360         pvt->cntr_nearend_speech_frames = (int)0;
361         pvt->cntr_residualcorrected_frames = (int)0;
362         pvt->cntr_residualcorrected_framesskipped = (int)0;
363         pvt->cntr_coeff_updates = (int)0;
364         pvt->cntr_coeff_missedupdates = (int)0;
365
366         pvt->avg_Lu_i_toolow = (int)0;
367         pvt->avg_Lu_i_ok = (int)0;
368 #endif
369
370         /* Reset the near-end speech detector */
371         pvt->s_tilde_i = (int)0;
372         pvt->y_tilde_i = (int)0;
373         pvt->HCNTR_d = (int)0;
374
375 }
376
377 static void echo_can_free(struct dahdi_chan *chan, struct dahdi_echocan_state *ec)
378 {
379         struct ec_pvt *pvt = dahdi_to_pvt(ec);
380
381 #if defined(DC_NORMALIZE) && defined(MEC2_DCBIAS_MESSAGE)
382         printk(KERN_INFO "EC: DC bias calculated: %d V\n", pvt->dc_estimate >> 15);
383 #endif
384         kfree(pvt);
385 }
386
387 #ifdef DC_NORMALIZE
388 static inline short dc_removal(int *dc_estimate, short samp)
389 {
390         *dc_estimate += ((((int)samp << 15) - *dc_estimate) >> 9);
391         return samp - (*dc_estimate >> 15);
392 }
393 #endif
394
395 static inline short sample_update(struct ec_pvt *pvt, short iref, short isig)
396 {
397         /* Declare local variables that are used more than once */
398         /* ... */
399         int k;
400         /* ... */
401         int rs;
402         /* ... */
403         short u;
404         /* ... */
405         int Py_i;
406         /* ... */
407         int two_beta_i;
408
409 #ifdef DC_NORMALIZE
410         isig = dc_removal(&pvt->dc_estimate, isig);
411 #endif
412         
413         /* flow A on pg. 428 */
414         /* eq. (16): high-pass filter the input to generate the next value;
415          *           push the current value into the circular buffer
416          *
417          * sdc_im1_d = sdc_d;
418          *     sdc_d = sig;
419          *     s_i_d = sdc_d;
420          *       s_d = s_i_d;
421          *     s_i_d = (float)(1.0 - gamma_d) * s_i_d
422          *      + (float)(0.5 * (1.0 - gamma_d)) * (sdc_d - sdc_im1_d); 
423          */
424
425         /* Update the Far-end receive signal circular buffers and accumulators */
426         /* ------------------------------------------------------------------- */
427         /* Delete the oldest sample from the power estimate accumulator */
428         pvt->y_tilde_i -= abs(get_cc_s(&pvt->y_s, (1 << DEFAULT_ALPHA_YT_I) - 1)) >> DEFAULT_ALPHA_YT_I;
429         /* Add the new sample to the power estimate accumulator */
430         pvt->y_tilde_i += abs(iref) >> DEFAULT_ALPHA_ST_I;
431         /* Push a copy of the new sample into its circular buffer */
432         add_cc_s(&pvt->y_s, iref);
433  
434
435         /* eq. (2): compute r in fixed-point */
436         rs = CONVOLVE2(pvt->a_s,
437                        pvt->y_s.buf_d + pvt->y_s.idx_d,
438                        pvt->N_d);
439         rs >>= 15;
440
441         if (pvt->lastsig == isig) {
442                 pvt->lastcount++;
443         } else {
444                 pvt->lastcount = 0;
445                 pvt->lastsig = isig;
446         }
447
448         if (isig == 0) {
449                 u = 0;
450         } else if (pvt->lastcount > 255) {
451                 /* We have seen the same input-signal more than 255 times,
452                  * we should pass it through uncancelled, as we are likely on hold */
453                 u = isig;
454         } else {
455                 int sign_error;
456
457                 if (rs < -32768) {
458                         rs = -32768;
459                         pvt->HCNTR_d = DEFAULT_HANGT;
460                         RESTORE_COEFFS;
461                 } else if (rs > 32767) {
462                         rs = 32767;
463                         pvt->HCNTR_d = DEFAULT_HANGT;
464                         RESTORE_COEFFS;
465                 }
466
467                 sign_error = ABS(rs) - ABS(isig);
468
469                 if (ABS(sign_error) > MAX_SIGN_ERROR)
470                 {
471                         rs = 0;
472                         RESTORE_COEFFS;
473                 }
474
475                 /* eq. (3): compute the output value (see figure 3) and the error
476                  * note: the error is the same as the output signal when near-end
477                  * speech is not present
478                  */
479                 u = isig - rs;
480
481                 if (u / isig < 0)
482                         u = isig - (rs >> 1);
483         }
484
485         /* Push a copy of the output value sample into its circular buffer */
486         add_cc_s(&pvt->u_s, u);
487
488         if (!pvt->backup) {
489                 /* Backup coefficients periodically */
490                 pvt->backup = BACKUP;
491                 memcpy(pvt->c_i, pvt->b_i, pvt->N_d*sizeof(int));
492                 memcpy(pvt->b_i, pvt->a_i, pvt->N_d*sizeof(int));
493         } else
494                 pvt->backup--;
495
496
497         /* Update the Near-end hybrid signal circular buffers and accumulators */
498         /* ------------------------------------------------------------------- */
499         /* Delete the oldest sample from the power estimate accumulator */
500         pvt->s_tilde_i -= abs(get_cc_s(&pvt->s_s, (1 << DEFAULT_ALPHA_ST_I) - 1));
501         /* Add the new sample to the power estimate accumulator */
502         pvt->s_tilde_i += abs(isig);
503         /* Push a copy of the new sample into it's circular buffer */
504         add_cc_s(&pvt->s_s, isig);
505
506
507         /* Push a copy of the current short-time average of the far-end receive signal into it's circular buffer */
508         add_cc_s(&pvt->y_tilde_s, pvt->y_tilde_i);
509
510         /* flow B on pg. 428 */
511   
512         /* If the hangover timer isn't running then compute the new convergence factor, otherwise set Py_i to 32768 */
513         if (!pvt->HCNTR_d) {
514                 Py_i = (pvt->Ly_i >> DEFAULT_SIGMA_LY_I) * (pvt->Ly_i >> DEFAULT_SIGMA_LY_I);
515                 Py_i >>= 15;
516         } else {
517                 Py_i = (1 << 15);
518         }
519   
520 #if 0
521         /* Vary rate of adaptation depending on position in the file
522          *  Do not do this for the first (DEFAULT_UPDATE_TIME) secs after speech
523          *  has begun of the file to allow the echo cancellor to estimate the
524          *  channel accurately
525          * Still needs conversion!
526          */
527
528         if (pvt->start_speech_d != 0) {
529                 if (pvt->i_d > (DEFAULT_T0 + pvt->start_speech_d)*(SAMPLE_FREQ)) {
530                         pvt->beta2_d = max_cc_float(MIN_BETA, DEFAULT_BETA1 * exp((-1/DEFAULT_TAU)*((pvt->i_d/(float)SAMPLE_FREQ) - DEFAULT_T0 - pvt->start_speech_d)));
531                 }
532         } else {
533                 pvt->beta2_d = DEFAULT_BETA1;
534         }
535 #endif
536   
537         /* Fixed point, inverted */
538         pvt->beta2_i = DEFAULT_BETA1_I;
539   
540         /* Fixed point version, inverted */
541         two_beta_i = (pvt->beta2_i * Py_i) >> 15;
542         if (!two_beta_i)
543                 two_beta_i++;
544
545         /* Update the Suppressed signal power estimate accumulator */
546         /* ------------------------------------------------------- */
547         /* Delete the oldest sample from the power estimate accumulator */
548         pvt->Lu_i -= abs(get_cc_s(&pvt->u_s, (1 << DEFAULT_SIGMA_LU_I) - 1));
549         /* Add the new sample to the power estimate accumulator */
550         pvt->Lu_i += abs(u);
551
552         /* Update the Far-end reference signal power estimate accumulator */
553         /* -------------------------------------------------------------- */
554         /* eq. (10): update power estimate of the reference */
555         /* Delete the oldest sample from the power estimate accumulator */
556         pvt->Ly_i -= abs(get_cc_s(&pvt->y_s, (1 << DEFAULT_SIGMA_LY_I) - 1)) ;
557         /* Add the new sample to the power estimate accumulator */
558         pvt->Ly_i += abs(iref);
559
560         if (pvt->Ly_i < DEFAULT_CUTOFF_I)
561                 pvt->Ly_i = DEFAULT_CUTOFF_I;
562
563
564         /* Update the Peak far-end receive signal detected */
565         /* ----------------------------------------------- */
566         if (pvt->y_tilde_i > pvt->max_y_tilde) {
567                 /* New highest y_tilde with full life */
568                 pvt->max_y_tilde = pvt->y_tilde_i;
569                 pvt->max_y_tilde_pos = pvt->N_d - 1;
570         } else if (--pvt->max_y_tilde_pos < 0) {
571                 /* Time to find new max y tilde... */
572                 pvt->max_y_tilde = MAX16(pvt->y_tilde_s.buf_d + pvt->y_tilde_s.idx_d, pvt->N_d, &pvt->max_y_tilde_pos);
573         }
574
575         /* Determine if near end speech was detected in this sample */
576         /* -------------------------------------------------------- */
577         if (((pvt->s_tilde_i >> (DEFAULT_ALPHA_ST_I - 1)) > pvt->max_y_tilde)
578             && (pvt->max_y_tilde > 0))  {
579                 /* Then start the Hangover counter */
580                 pvt->HCNTR_d = DEFAULT_HANGT;
581                 RESTORE_COEFFS;
582 #ifdef MEC2_STATS_DETAILED
583                 printk(KERN_INFO "Reset near end speech timer with: s_tilde_i %d, stmnt %d, max_y_tilde %d\n", pvt->s_tilde_i, (pvt->s_tilde_i >> (DEFAULT_ALPHA_ST_I - 1)), pvt->max_y_tilde);
584 #endif
585 #ifdef MEC2_STATS
586                 ++pvt->cntr_nearend_speech_frames;
587 #endif
588         } else if (pvt->HCNTR_d > (int)0) {
589                 /* otherwise, if it's still non-zero, decrement the Hangover counter by one sample */
590 #ifdef MEC2_STATS
591                 ++pvt->cntr_nearend_speech_frames;
592 #endif
593                 pvt->HCNTR_d--;
594         } 
595
596         /* Update coefficients if no near-end speech in this sample (ie. HCNTR_d = 0)
597          * and we have enough signal to bother trying to update.
598          * --------------------------------------------------------------------------
599          */
600         if (!pvt->HCNTR_d &&                            /* no near-end speech present */
601             !(pvt->i_d % DEFAULT_M)) {          /* we only update on every DEFAULM_M'th sample from the stream */
602                 if (pvt->Lu_i > MIN_UPDATE_THRESH_I) {  /* there is sufficient energy above the noise floor to contain meaningful data */
603                                                         /* so loop over all the filter coefficients */
604 #ifdef USED_COEFFS
605                         int max_coeffs[USED_COEFFS];
606                         int *pos;
607
608                         if (pvt->N_d > USED_COEFFS)
609                                 memset(max_coeffs, 0, USED_COEFFS*sizeof(int));
610 #endif
611 #ifdef MEC2_STATS_DETAILED
612                         printk(KERN_INFO "updating coefficients with: pvt->Lu_i %9d\n", pvt->Lu_i);
613 #endif
614 #ifdef MEC2_STATS
615                         pvt->avg_Lu_i_ok = pvt->avg_Lu_i_ok + pvt->Lu_i;
616                         ++pvt->cntr_coeff_updates;
617 #endif
618                         for (k = 0; k < pvt->N_d; k++) {
619                                 /* eq. (7): compute an expectation over M_d samples */
620                                 int grad2;
621                                 grad2 = CONVOLVE2(pvt->u_s.buf_d + pvt->u_s.idx_d,
622                                                   pvt->y_s.buf_d + pvt->y_s.idx_d + k,
623                                                   DEFAULT_M);
624                                 /* eq. (7): update the coefficient */
625                                 pvt->a_i[k] += grad2 / two_beta_i;
626                                 pvt->a_s[k] = pvt->a_i[k] >> 16;
627
628 #ifdef USED_COEFFS
629                                 if (pvt->N_d > USED_COEFFS) {
630                                         if (abs(pvt->a_i[k]) > max_coeffs[USED_COEFFS-1]) {
631                                                 /* More or less insertion-sort... */
632                                                 pos = max_coeffs;
633                                                 while (*pos > abs(pvt->a_i[k]))
634                                                         pos++;
635
636                                                 if (*pos > max_coeffs[USED_COEFFS-1])
637                                                         memmove(pos+1, pos, (USED_COEFFS-(pos-max_coeffs)-1)*sizeof(int));
638
639                                                 *pos = abs(pvt->a_i[k]);
640                                         }
641                                 }
642 #endif
643                         }
644
645 #ifdef USED_COEFFS
646                         /* Filter out irrelevant coefficients */
647                         if (pvt->N_d > USED_COEFFS)
648                                 for (k = 0; k < pvt->N_d; k++)
649                                         if (abs(pvt->a_i[k]) < max_coeffs[USED_COEFFS-1])
650                                                 pvt->a_i[k] = pvt->a_s[k] = 0;
651 #endif
652                 } else {
653 #ifdef MEC2_STATS_DETAILED
654                         printk(KERN_INFO "insufficient signal to update coefficients pvt->Lu_i %5d < %5d\n", pvt->Lu_i, MIN_UPDATE_THRESH_I);
655 #endif
656 #ifdef MEC2_STATS
657                         pvt->avg_Lu_i_toolow = pvt->avg_Lu_i_toolow + pvt->Lu_i;
658                         ++pvt->cntr_coeff_missedupdates;
659 #endif
660                 }
661         }
662   
663         /* paragraph below eq. (15): if no near-end speech in the sample and 
664          * the reference signal power estimate > cutoff threshold
665          * then perform residual error suppression
666          */
667 #ifdef MEC2_STATS_DETAILED
668         if (pvt->HCNTR_d == 0)
669                 printk(KERN_INFO "possibly correcting frame with pvt->Ly_i %9d pvt->Lu_i %9d and expression %d\n", pvt->Ly_i, pvt->Lu_i, (pvt->Ly_i/(pvt->Lu_i + 1)));
670 #endif
671
672 #ifndef NO_ECHO_SUPPRESSOR
673         if (pvt->use_nlp) {
674                 if (pvt->aggressive) {
675                         if ((pvt->HCNTR_d < AGGRESSIVE_HCNTR) && (pvt->Ly_i > (pvt->Lu_i << 1))) {
676                                 for (k = 0; k < 2; k++) {
677                                         u = u * (pvt->Lu_i >> DEFAULT_SIGMA_LU_I) / ((pvt->Ly_i >> (DEFAULT_SIGMA_LY_I)) + 1);
678                                 }
679 #ifdef MEC2_STATS_DETAILED
680                                 printk(KERN_INFO "aggresively correcting frame with pvt->Ly_i %9d pvt->Lu_i %9d expression %d\n", pvt->Ly_i, pvt->Lu_i, (pvt->Ly_i/(pvt->Lu_i + 1)));
681 #endif
682 #ifdef MEC2_STATS
683                                 ++pvt->cntr_residualcorrected_frames;
684 #endif
685                         }
686                 } else {
687                         if (pvt->HCNTR_d == 0) {
688                                 if ((pvt->Ly_i/(pvt->Lu_i + 1)) > DEFAULT_SUPPR_I) {
689                                         for (k = 0; k < 1; k++) {
690                                                 u = u * (pvt->Lu_i >> DEFAULT_SIGMA_LU_I) / ((pvt->Ly_i >> (DEFAULT_SIGMA_LY_I + 2)) + 1);
691                                         }
692 #ifdef MEC2_STATS_DETAILED
693                                         printk(KERN_INFO "correcting frame with pvt->Ly_i %9d pvt->Lu_i %9d expression %d\n", pvt->Ly_i, pvt->Lu_i, (pvt->Ly_i/(pvt->Lu_i + 1)));
694 #endif
695 #ifdef MEC2_STATS
696                                         ++pvt->cntr_residualcorrected_frames;
697 #endif
698                                 }
699 #ifdef MEC2_STATS
700                                 else {
701                                         ++pvt->cntr_residualcorrected_framesskipped;
702                                 }
703 #endif
704                         }
705                 }
706         }
707 #endif  
708
709 #if 0
710         /* This will generate a non-linear supression factor, once converted */
711         if ((pvt->HCNTR_d == 0) &&
712                 ((pvt->Lu_d/pvt->Ly_d) < DEFAULT_SUPPR) &&
713                 (pvt->Lu_d/pvt->Ly_d > EC_MIN_DB_VALUE)) {
714                 suppr_factor = (10 / (float)(SUPPR_FLOOR - SUPPR_CEIL)) * log(pvt->Lu_d/pvt->Ly_d)
715                         - SUPPR_CEIL / (float)(SUPPR_FLOOR - SUPPR_CEIL);
716                 u_suppr = pow(10.0, (suppr_factor) * RES_SUPR_FACTOR / 10.0) * u_suppr;
717         }
718 #endif  
719
720 #ifdef MEC2_STATS
721         /* Periodically dump performance stats */
722         if ((pvt->i_d % MEC2_STATS) == 0) {
723                 /* make sure to avoid div0's! */
724                 if (pvt->cntr_coeff_missedupdates > 0)
725                         pvt->avg_Lu_i_toolow = (int)(pvt->avg_Lu_i_toolow / pvt->cntr_coeff_missedupdates);
726                 else
727                         pvt->avg_Lu_i_toolow = -1;
728
729                 if (pvt->cntr_coeff_updates > 0)
730                         pvt->avg_Lu_i_ok = (pvt->avg_Lu_i_ok / pvt->cntr_coeff_updates);
731                 else
732                         pvt->avg_Lu_i_ok = -1;
733
734                 printk(KERN_INFO "%d: Near end speech: %5d Residuals corrected/skipped: %5d/%5d Coefficients updated ok/low sig: %3d/%3d Lu_i avg ok/low sig %6d/%5d\n", 
735                        pvt->id,
736                        pvt->cntr_nearend_speech_frames,
737                        pvt->cntr_residualcorrected_frames, pvt->cntr_residualcorrected_framesskipped,
738                        pvt->cntr_coeff_updates, pvt->cntr_coeff_missedupdates,
739                        pvt->avg_Lu_i_ok, pvt->avg_Lu_i_toolow);
740
741                 pvt->cntr_nearend_speech_frames = 0;
742                 pvt->cntr_residualcorrected_frames = 0;
743                 pvt->cntr_residualcorrected_framesskipped = 0;
744                 pvt->cntr_coeff_updates = 0;
745                 pvt->cntr_coeff_missedupdates = 0;
746                 pvt->avg_Lu_i_ok = 0;
747                 pvt->avg_Lu_i_toolow = 0;
748         }
749 #endif
750
751         /* Increment the sample index and return the corrected sample */
752         pvt->i_d++;
753         return u;
754 }
755
756 static void echo_can_process(struct dahdi_echocan_state *ec, short *isig, const short *iref, u32 size)
757 {
758         struct ec_pvt *pvt = dahdi_to_pvt(ec);
759         u32 x;
760         short result;
761
762         for (x = 0; x < size; x++) {
763                 result = sample_update(pvt, *iref, *isig);
764                 *isig++ = result;
765                 ++iref;
766         }
767 }
768
769 static int echo_can_create(struct dahdi_chan *chan, struct dahdi_echocanparams *ecp,
770                            struct dahdi_echocanparam *p, struct dahdi_echocan_state **ec)
771 {
772         int maxy;
773         int maxu;
774         size_t size;
775         unsigned int x;
776         char *c;
777         struct ec_pvt *pvt;
778
779         maxy = ecp->tap_length + DEFAULT_M;
780         maxu = DEFAULT_M;
781         if (maxy < (1 << DEFAULT_ALPHA_YT_I))
782                 maxy = (1 << DEFAULT_ALPHA_YT_I);
783         if (maxy < (1 << DEFAULT_SIGMA_LY_I))
784                 maxy = (1 << DEFAULT_SIGMA_LY_I);
785         if (maxu < (1 << DEFAULT_SIGMA_LU_I))
786                 maxu = (1 << DEFAULT_SIGMA_LU_I);
787         size = sizeof(**ec) +
788                 4 +                                             /* align */
789                 sizeof(int) * ecp->tap_length +                 /* a_i */
790                 sizeof(short) * ecp->tap_length +               /* a_s */
791                 sizeof(int) * ecp->tap_length +                 /* b_i */
792                 sizeof(int) * ecp->tap_length +                 /* c_i */
793                 2 * sizeof(short) * (maxy) +                    /* y_s */
794                 2 * sizeof(short) * (1 << DEFAULT_ALPHA_ST_I) + /* s_s */
795                 2 * sizeof(short) * (maxu) +                    /* u_s */
796                 2 * sizeof(short) * ecp->tap_length;            /* y_tilde_s */
797
798         pvt = kzalloc(size, GFP_KERNEL);
799         if (!pvt)
800                 return -ENOMEM;
801
802         pvt->dahdi.ops = &my_ops;
803
804         pvt->aggressive = aggressive;
805         pvt->dahdi.features = my_features;
806
807         for (x = 0; x < ecp->param_count; x++) {
808                 for (c = p[x].name; *c; c++)
809                         *c = tolower(*c);
810                 if (!strcmp(p[x].name, "aggressive")) {
811                         pvt->aggressive = p[x].value ? 1 : 0;
812                 } else {
813                         printk(KERN_WARNING "Unknown parameter supplied to MG2 echo canceler: '%s'\n", p[x].name);
814                         kfree(pvt);
815
816                         return -EINVAL;
817                 }
818         }
819
820         init_cc(pvt, ecp->tap_length, maxy, maxu);
821         /* Non-linear processor - a fancy way to say "zap small signals, to avoid
822            accumulating noise". */
823         pvt->use_nlp = TRUE;
824
825         *ec = &pvt->dahdi;
826         return 0;
827 }
828
829 static int echo_can_traintap(struct dahdi_echocan_state *ec, int pos, short val)
830 {
831         struct ec_pvt *pvt = dahdi_to_pvt(ec);
832
833         /* Set the hangover counter to the length of the can to 
834          * avoid adjustments occuring immediately after initial forced training 
835          */
836         pvt->HCNTR_d = pvt->N_d << 1;
837
838         if (pos >= pvt->N_d) {
839                 memcpy(pvt->b_i, pvt->a_i, pvt->N_d*sizeof(int));
840                 memcpy(pvt->c_i, pvt->a_i, pvt->N_d*sizeof(int));
841                 return 1;
842         }
843
844         pvt->a_i[pos] = val << 17;
845         pvt->a_s[pos] = val << 1;
846
847         if (++pos >= pvt->N_d) {
848                 memcpy(pvt->b_i, pvt->a_i, pvt->N_d*sizeof(int));
849                 memcpy(pvt->c_i, pvt->a_i, pvt->N_d*sizeof(int));
850                 return 1;
851         }
852
853         return 0;
854 }
855
856 static void echocan_NLP_toggle(struct dahdi_echocan_state *ec, unsigned int enable)
857 {
858         struct ec_pvt *pvt = dahdi_to_pvt(ec);
859
860         pvt->use_nlp = enable ? 1 : 0;
861 }
862
863 static int __init mod_init(void)
864 {
865         if (dahdi_register_echocan_factory(&my_factory)) {
866                 module_printk(KERN_ERR, "could not register with DAHDI core\n");
867
868                 return -EPERM;
869         }
870
871         module_printk(KERN_NOTICE, "Registered echo canceler '%s'\n",
872                       my_factory.get_name(NULL));
873
874         return 0;
875 }
876
877 static void __exit mod_exit(void)
878 {
879         dahdi_unregister_echocan_factory(&my_factory);
880 }
881
882 module_param(debug, int, S_IRUGO | S_IWUSR);
883 module_param(aggressive, int, S_IRUGO | S_IWUSR);
884
885 MODULE_DESCRIPTION("DAHDI 'MG2' Echo Canceler");
886 MODULE_AUTHOR("Michael Gernoth");
887 MODULE_LICENSE("GPL v2");
888
889 module_init(mod_init);
890 module_exit(mod_exit);