2
* Mark's Second Echo Canceller
4
* Copyright (C) 2002, Digium, Inc.
6
* This program is free software and may be used and
7
* distributed according to the terms of the GNU
8
* General Public License, incorporated herein by
16
#include <linux/kernel.h>
17
#include <linux/slab.h>
18
#define MALLOC(a) kmalloc((a), GFP_KERNEL)
19
#define FREE(a) kfree(a)
26
#define MALLOC(a) malloc(a)
27
#define FREE(a) free(a)
30
/* Get optimized routines for math */
43
#include "mec2_const.h"
45
/* Circular buffer definition */
49
short *buf_d; /* Twice as large as we need */
55
/* Echo canceller definition */
60
/* pre-computed constants */
65
// declare accumulators for power computations
70
// declare an accumulator for the near-end signal detector
75
// circular buffers and coefficients
82
echo_can_cb_s y_tilde_s;
91
static inline void init_cb_s(echo_can_cb_s *cb, int len, void *where)
93
cb->buf_d = (short *)where;
98
static inline void add_cc_s(echo_can_cb_s *cb, short newval)
100
/* Can't use modulus because N+M isn't a power of two (generally) */
102
if (cb->idx_d < (int)0)
103
{cb->idx_d += cb->size_d;}
104
/* Load two copies into memory */
105
cb->buf_d[cb->idx_d] = newval;
106
cb->buf_d[cb->idx_d + cb->size_d] = newval;
109
static inline short get_cc_s(echo_can_cb_s *cb, int pos)
111
/* Load two copies into memory */
112
return cb->buf_d[cb->idx_d + pos];
115
static inline void init_cc(echo_can_state_t *ec, int N, int maxy, int maxu) {
119
/* double-word align past end of state */
120
ptr += sizeof(echo_can_state_t);
121
tmp = (unsigned long)ptr;
129
ec->beta2_i = DEFAULT_BETA1_I;
131
// allocate coefficient memory
134
ptr += (sizeof(int) * ec->N_d);
136
ptr += (sizeof(short) * ec->N_d);
138
/* Reset Y circular buffer (short version) */
139
init_cb_s(&ec->y_s, maxy, ptr);
140
ptr += (sizeof(short) * (maxy) * 2);
142
/* Reset Sig circular buffer (short version for FIR filter) */
143
init_cb_s(&ec->s_s, (1 << DEFAULT_ALPHA_ST_I), ptr);
144
ptr += (sizeof(short) * (1 << DEFAULT_ALPHA_ST_I) * 2);
146
init_cb_s(&ec->u_s, maxu, ptr);
147
ptr += (sizeof(short) * maxu * 2);
149
// allocate a buffer for the reference signal power computation
151
init_cb_s(&ec->y_tilde_s, ec->N_d, ptr);
154
// reset absolute time
158
// reset the power computations (for y and u)
160
ec->Ly_i = DEFAULT_CUTOFF_I;
161
ec->Lu_i = DEFAULT_CUTOFF_I;
163
// reset the near-end speech detector
167
ec->HCNTR_d = (int)0;
173
static inline void echo_can_free(echo_can_state_t *ec)
178
static inline short echo_can_update(echo_can_state_t *ec, short iref, short isig) {
180
/* declare local variables that are used more than once
188
/***************************************************************************
192
***************************************************************************/
194
/* eq. (16): high-pass filter the input to generate the next value;
195
// push the current value into the circular buffer
197
// sdc_im1_d = sdc_d;
201
// s_i_d = (float)(1.0 - gamma_d) * s_i_d
202
+ (float)(0.5 * (1.0 - gamma_d)) * (sdc_d - sdc_im1_d); */
205
/* Delete last sample from power estimate */
206
ec->y_tilde_i -= abs(get_cc_s(&ec->y_s, (1 << DEFAULT_ALPHA_YT_I) - 1 )) >> DEFAULT_ALPHA_YT_I;
207
/* push the reference data onto the circular buffer */
208
add_cc_s(&ec->y_s, iref);
210
/* eq. (2): compute r in fixed-point */
211
rs = CONVOLVE2(ec->a_s, ec->y_s.buf_d + ec->y_s.idx_d, ec->N_d);
214
/* eq. (3): compute the output value (see figure 3) and the error
215
// note: the error is the same as the output signal when near-end
216
// speech is not present
220
add_cc_s(&ec->u_s, u);
224
/* Delete oldest part of received s_tilde */
225
ec->s_tilde_i -= abs(get_cc_s(&ec->s_s, (1 << DEFAULT_ALPHA_ST_I) - 1 ));
227
/* push the signal on the circular buffer, too */
228
add_cc_s(&ec->s_s, isig);
229
ec->s_tilde_i += abs(isig);
230
ec->y_tilde_i += abs(iref) >> DEFAULT_ALPHA_YT_I;
232
/* Add to our list of recent y_tilde's */
233
add_cc_s(&ec->y_tilde_s, ec->y_tilde_i);
235
/****************************************************************************
239
****************************************************************************/
241
/* compute the new convergence factor
244
Py_i = (ec->Ly_i >> DEFAULT_SIGMA_LY_I) * (ec->Ly_i >> DEFAULT_SIGMA_LY_I);
251
printf("Py: %e, Py_i: %e\n", Py, Py_i * AMPL_SCALE_1);
254
/* Vary rate of adaptation depending on position in the file
255
// Do not do this for the first (DEFAULT_UPDATE_TIME) secs after speech
256
// has begun of the file to allow the echo cancellor to estimate the
257
// channel accurately
260
if (ec->start_speech_d != 0 ){
261
if ( ec->i_d > (DEFAULT_T0 + ec->start_speech_d)*(SAMPLE_FREQ) ){
262
ec->beta2_d = max_cc_float(MIN_BETA,
263
DEFAULT_BETA1 * exp((-1/DEFAULT_TAU)*((ec->i_d/(float)SAMPLE_FREQ) -
265
ec->start_speech_d)));
268
else {ec->beta2_d = DEFAULT_BETA1;}
271
ec->beta2_i = DEFAULT_BETA1_I; /* Fixed point, inverted */
273
two_beta_i = (ec->beta2_i * Py_i) >> 15; /* Fixed point version, inverted */
277
/* Update Lu_i (Suppressed power estimate) */
278
ec->Lu_i -= abs(get_cc_s(&ec->u_s, (1 << DEFAULT_SIGMA_LU_I) - 1 )) ;
281
/* eq. (10): update power estimate of the reference
283
ec->Ly_i -= abs(get_cc_s(&ec->y_s, (1 << DEFAULT_SIGMA_LY_I) - 1)) ;
284
ec->Ly_i += abs(iref);
286
if (ec->Ly_i < DEFAULT_CUTOFF_I)
287
ec->Ly_i = DEFAULT_CUTOFF_I;
290
printf("Float: %e, Int: %e\n", ec->Ly_d, (ec->Ly_i >> DEFAULT_SIGMA_LY_I) * AMPL_SCALE_1);
293
if (ec->y_tilde_i > ec->max_y_tilde) {
294
/* New highest y_tilde with full life */
295
ec->max_y_tilde = ec->y_tilde_i;
296
ec->max_y_tilde_pos = ec->N_d - 1;
297
} else if (--ec->max_y_tilde_pos < 0) {
298
/* Time to find new max y tilde... */
299
ec->max_y_tilde = MAX16(ec->y_tilde_s.buf_d + ec->y_tilde_s.idx_d, ec->N_d, &ec->max_y_tilde_pos);
302
if ((ec->s_tilde_i >> (DEFAULT_ALPHA_ST_I - 1)) > ec->max_y_tilde)
304
ec->HCNTR_d = DEFAULT_HANGT;
306
else if (ec->HCNTR_d > (int)0)
311
/* update coefficients if no near-end speech and we have enough signal
312
* to bother trying to update.
314
if (!ec->HCNTR_d && !(ec->i_d % DEFAULT_M) &&
315
(ec->Lu_i > MIN_UPDATE_THRESH_I)) {
316
// loop over all filter coefficients
318
for (k=0; k<ec->N_d; k++) {
320
// eq. (7): compute an expectation over M_d samples
323
grad2 = CONVOLVE2(ec->u_s.buf_d + ec->u_s.idx_d,
324
ec->y_s.buf_d + ec->y_s.idx_d + k, DEFAULT_M);
325
// eq. (7): update the coefficient
327
ec->a_i[k] += grad2 / two_beta_i;
328
ec->a_s[k] = ec->a_i[k] >> 16;
332
/* paragraph below eq. (15): if no near-end speech,
333
// check for residual error suppression
335
#ifndef NO_ECHO_SUPPRESSOR
336
#ifdef AGGRESSIVE_SUPPRESSOR
337
#ifdef AGGRESSIVE_TIMELIMIT /* This allows the aggressive suppressor to turn off after set amount of time */
338
if (ec->i_d > AGGRESSIVE_TIMELIMIT ) {
339
if ((ec->HCNTR_d == 0) && ((ec->Ly_i/(ec->Lu_i + 1)) > DEFAULT_SUPPR_I)) {
340
u = u * (ec->Lu_i >> DEFAULT_SIGMA_LU_I) / ((ec->Ly_i >> (DEFAULT_SIGMA_LY_I + 2)) + 1);
345
if ((ec->HCNTR_d < AGGRESSIVE_HCNTR) && (ec->Ly_i > (ec->Lu_i << 1))) {
346
u = u * (ec->Lu_i >> DEFAULT_SIGMA_LU_I) / ((ec->Ly_i >> (DEFAULT_SIGMA_LY_I)) + 1);
347
u = u * (ec->Lu_i >> DEFAULT_SIGMA_LU_I) / ((ec->Ly_i >> (DEFAULT_SIGMA_LY_I)) + 1);
349
#ifdef AGGRESSIVE_TIMELIMIT
353
if ((ec->HCNTR_d == 0) && ((ec->Ly_i/(ec->Lu_i + 1)) > DEFAULT_SUPPR_I)) {
354
u = u * (ec->Lu_i >> DEFAULT_SIGMA_LU_I) / ((ec->Ly_i >> (DEFAULT_SIGMA_LY_I + 2)) + 1);
360
if ((ec->HCNTR_d == 0) && ((ec->Lu_d/ec->Ly_d) < DEFAULT_SUPPR) &&
361
(ec->Lu_d/ec->Ly_d > EC_MIN_DB_VALUE)) {
362
suppr_factor = (10/(float)(SUPPR_FLOOR-SUPPR_CEIL))*log(ec->Lu_d/ec->Ly_d)
363
- SUPPR_CEIL/(float)(SUPPR_FLOOR - SUPPR_CEIL);
365
u_suppr = pow(10.0,(suppr_factor)*RES_SUPR_FACTOR/10.0)*u_suppr;
373
static inline echo_can_state_t *echo_can_create(int len, int adaption_mode)
375
echo_can_state_t *ec;
378
maxy = len + DEFAULT_M;
380
if (maxy < (1 << DEFAULT_ALPHA_YT_I))
381
maxy = (1 << DEFAULT_ALPHA_YT_I);
382
if (maxy < (1 << DEFAULT_SIGMA_LY_I))
383
maxy = (1 << DEFAULT_SIGMA_LY_I);
384
if (maxu < (1 << DEFAULT_SIGMA_LU_I))
385
maxu = (1 << DEFAULT_SIGMA_LU_I);
386
ec = (echo_can_state_t *)MALLOC(sizeof(echo_can_state_t) +
388
sizeof(int) * len + /* a_i */
389
sizeof(short) * len + /* a_s */
390
2 * sizeof(short) * (maxy) + /* y_s */
391
2 * sizeof(short) * (1 << DEFAULT_ALPHA_ST_I) + /* s_s */
392
2 * sizeof(short) * (maxu) + /* u_s */
393
2 * sizeof(short) * len); /* y_tilde_s */
395
memset(ec, 0, sizeof(echo_can_state_t) +
397
sizeof(int) * len + /* a_i */
398
sizeof(short) * len + /* a_s */
399
2 * sizeof(short) * (maxy) + /* y_s */
400
2 * sizeof(short) * (1 << DEFAULT_ALPHA_ST_I) + /* s_s */
401
2 * sizeof(short) * (maxu) + /* u_s */
402
2 * sizeof(short) * len); /* y_tilde_s */
403
init_cc(ec, len, maxy, maxu);
408
static inline int echo_can_traintap(echo_can_state_t *ec, int pos, short val)
410
/* Reset hang counter to avoid adjustments after
411
initial forced training */
412
ec->HCNTR_d = ec->N_d << 1;
415
ec->a_i[pos] = val << 17;
416
ec->a_s[pos] = val << 1;
417
if (++pos >= ec->N_d)
added
removed
mec2.h
