/* * Mark's Second Echo Canceller * * Copyright (C) 2002, Digium, Inc. * * This program is free software and may be used and * distributed according to the terms of the GNU * General Public License, incorporated herein by * reference. * */ #ifndef _MARK2_ECHO_H #define _MARK2_ECHO_H #ifdef __KERNEL__ #include #include #define MALLOC(a) kmalloc((a), GFP_KERNEL) #define FREE(a) kfree(a) #else #include #include #include #include #include #define MALLOC(a) malloc(a) #define FREE(a) free(a) #endif /* Get optimized routines for math */ #include "arith.h" #ifndef NULL #define NULL 0 #endif #ifndef FALSE #define FALSE 0 #endif #ifndef TRUE #define TRUE (!FALSE) #endif #include "mec2_const.h" /* Circular buffer definition */ typedef struct { int idx_d; int size_d; short *buf_d; /* Twice as large as we need */ } echo_can_cb_s; // class definition // struct echo_can_state { /* Echo canceller definition */ /* absolute time */ int i_d; /* pre-computed constants */ int N_d; int beta2_i; // declare accumulators for power computations // int Ly_i; int Lu_i; // declare an accumulator for the near-end signal detector // int s_tilde_i; int HCNTR_d; // circular buffers and coefficients // int *a_i; short *a_s; echo_can_cb_s y_s; echo_can_cb_s s_s; echo_can_cb_s u_s; echo_can_cb_s y_tilde_s; int y_tilde_i; /* Max memory */ short max_y_tilde; int max_y_tilde_pos; }; static inline void init_cb_s(echo_can_cb_s *cb, int len, void *where) { cb->buf_d = (short *)where; cb->idx_d = 0; cb->size_d = len; } static inline void add_cc_s(echo_can_cb_s *cb, short newval) { /* Can't use modulus because N+M isn't a power of two (generally) */ cb->idx_d--; if (cb->idx_d < (int)0) {cb->idx_d += cb->size_d;} /* Load two copies into memory */ cb->buf_d[cb->idx_d] = newval; cb->buf_d[cb->idx_d + cb->size_d] = newval; } static inline short get_cc_s(echo_can_cb_s *cb, int pos) { /* Load two copies into memory */ return cb->buf_d[cb->idx_d + pos]; } static inline void init_cc(struct echo_can_state *ec, int N, int maxy, int maxu) { void *ptr = ec; unsigned long tmp; /* double-word align past end of state */ ptr += sizeof(struct echo_can_state); tmp = (unsigned long)ptr; tmp += 3; tmp &= ~3L; ptr = (void *)tmp; // reset parameters // ec->N_d = N; ec->beta2_i = DEFAULT_BETA1_I; // allocate coefficient memory // ec->a_i = ptr; ptr += (sizeof(int) * ec->N_d); ec->a_s = ptr; ptr += (sizeof(short) * ec->N_d); /* Reset Y circular buffer (short version) */ init_cb_s(&ec->y_s, maxy, ptr); ptr += (sizeof(short) * (maxy) * 2); /* Reset Sig circular buffer (short version for FIR filter) */ init_cb_s(&ec->s_s, (1 << DEFAULT_ALPHA_ST_I), ptr); ptr += (sizeof(short) * (1 << DEFAULT_ALPHA_ST_I) * 2); init_cb_s(&ec->u_s, maxu, ptr); ptr += (sizeof(short) * maxu * 2); // allocate a buffer for the reference signal power computation // init_cb_s(&ec->y_tilde_s, ec->N_d, ptr); // reset absolute time // ec->i_d = (int)0; // reset the power computations (for y and u) // ec->Ly_i = DEFAULT_CUTOFF_I; ec->Lu_i = DEFAULT_CUTOFF_I; // reset the near-end speech detector // ec->s_tilde_i = 0; ec->y_tilde_i = 0; ec->HCNTR_d = (int)0; // exit gracefully // } static inline void echo_can_free(struct echo_can_state *ec) { FREE(ec); } static inline short echo_can_update(struct echo_can_state *ec, short iref, short isig) { /* declare local variables that are used more than once */ int k; int rs; short u; int Py_i; int two_beta_i; /*************************************************************************** // // flow A on pg. 428 // ***************************************************************************/ /* eq. (16): high-pass filter the input to generate the next value; // push the current value into the circular buffer // // sdc_im1_d = sdc_d; // sdc_d = sig; // s_i_d = sdc_d; // s_d = s_i_d; // s_i_d = (float)(1.0 - gamma_d) * s_i_d + (float)(0.5 * (1.0 - gamma_d)) * (sdc_d - sdc_im1_d); */ /* Delete last sample from power estimate */ ec->y_tilde_i -= abs(get_cc_s(&ec->y_s, (1 << DEFAULT_ALPHA_YT_I) - 1 )) >> DEFAULT_ALPHA_YT_I; /* push the reference data onto the circular buffer */ add_cc_s(&ec->y_s, iref); /* eq. (2): compute r in fixed-point */ rs = CONVOLVE2(ec->a_s, ec->y_s.buf_d + ec->y_s.idx_d, ec->N_d); rs >>= 15; /* eq. (3): compute the output value (see figure 3) and the error // note: the error is the same as the output signal when near-end // speech is not present */ u = isig - rs; add_cc_s(&ec->u_s, u); /* Delete oldest part of received s_tilde */ ec->s_tilde_i -= abs(get_cc_s(&ec->s_s, (1 << DEFAULT_ALPHA_ST_I) - 1 )); /* push the signal on the circular buffer, too */ add_cc_s(&ec->s_s, isig); ec->s_tilde_i += abs(isig); ec->y_tilde_i += abs(iref) >> DEFAULT_ALPHA_YT_I; /* Add to our list of recent y_tilde's */ add_cc_s(&ec->y_tilde_s, ec->y_tilde_i); /**************************************************************************** // // flow B on pg. 428 // ****************************************************************************/ /* compute the new convergence factor */ if (!ec->HCNTR_d) { Py_i = (ec->Ly_i >> DEFAULT_SIGMA_LY_I) * (ec->Ly_i >> DEFAULT_SIGMA_LY_I); Py_i >>= 15; } else { Py_i = (1 << 15); } #if 0 printf("Py: %e, Py_i: %e\n", Py, Py_i * AMPL_SCALE_1); #endif /* Vary rate of adaptation depending on position in the file // Do not do this for the first (DEFAULT_UPDATE_TIME) secs after speech // has begun of the file to allow the echo cancellor to estimate the // channel accurately */ #if 0 if (ec->start_speech_d != 0 ){ if ( ec->i_d > (DEFAULT_T0 + ec->start_speech_d)*(SAMPLE_FREQ) ){ ec->beta2_d = max_cc_float(MIN_BETA, DEFAULT_BETA1 * exp((-1/DEFAULT_TAU)*((ec->i_d/(float)SAMPLE_FREQ) - DEFAULT_T0 - ec->start_speech_d))); } } else {ec->beta2_d = DEFAULT_BETA1;} #endif ec->beta2_i = DEFAULT_BETA1_I; /* Fixed point, inverted */ two_beta_i = (ec->beta2_i * Py_i) >> 15; /* Fixed point version, inverted */ if (!two_beta_i) two_beta_i++; /* Update Lu_i (Suppressed power estimate) */ ec->Lu_i -= abs(get_cc_s(&ec->u_s, (1 << DEFAULT_SIGMA_LU_I) - 1 )) ; ec->Lu_i += abs(u); /* eq. (10): update power estimate of the reference */ ec->Ly_i -= abs(get_cc_s(&ec->y_s, (1 << DEFAULT_SIGMA_LY_I) - 1)) ; ec->Ly_i += abs(iref); if (ec->Ly_i < DEFAULT_CUTOFF_I) ec->Ly_i = DEFAULT_CUTOFF_I; #if 0 printf("Float: %e, Int: %e\n", ec->Ly_d, (ec->Ly_i >> DEFAULT_SIGMA_LY_I) * AMPL_SCALE_1); #endif if (ec->y_tilde_i > ec->max_y_tilde) { /* New highest y_tilde with full life */ ec->max_y_tilde = ec->y_tilde_i; ec->max_y_tilde_pos = ec->N_d - 1; } else if (--ec->max_y_tilde_pos < 0) { /* Time to find new max y tilde... */ 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); } if ((ec->s_tilde_i >> (DEFAULT_ALPHA_ST_I - 1)) > ec->max_y_tilde) { ec->HCNTR_d = DEFAULT_HANGT; } else if (ec->HCNTR_d > (int)0) { ec->HCNTR_d--; } /* update coefficients if no near-end speech and we have enough signal * to bother trying to update. */ if (!ec->HCNTR_d && !(ec->i_d % DEFAULT_M) && (ec->Lu_i > MIN_UPDATE_THRESH_I)) { // loop over all filter coefficients // for (k=0; kN_d; k++) { // eq. (7): compute an expectation over M_d samples // int grad2; grad2 = CONVOLVE2(ec->u_s.buf_d + ec->u_s.idx_d, ec->y_s.buf_d + ec->y_s.idx_d + k, DEFAULT_M); // eq. (7): update the coefficient // ec->a_i[k] += grad2 / two_beta_i; ec->a_s[k] = ec->a_i[k] >> 16; } } /* paragraph below eq. (15): if no near-end speech, // check for residual error suppression */ #ifndef NO_ECHO_SUPPRESSOR #ifdef AGGRESSIVE_SUPPRESSOR #ifdef AGGRESSIVE_TIMELIMIT /* This allows the aggressive suppressor to turn off after set amount of time */ if (ec->i_d > AGGRESSIVE_TIMELIMIT ) { if ((ec->HCNTR_d == 0) && ((ec->Ly_i/(ec->Lu_i + 1)) > DEFAULT_SUPPR_I)) { u = u * (ec->Lu_i >> DEFAULT_SIGMA_LU_I) / ((ec->Ly_i >> (DEFAULT_SIGMA_LY_I + 2)) + 1); } } else { #endif if ((ec->HCNTR_d < AGGRESSIVE_HCNTR) && (ec->Ly_i > (ec->Lu_i << 1))) { u = u * (ec->Lu_i >> DEFAULT_SIGMA_LU_I) / ((ec->Ly_i >> (DEFAULT_SIGMA_LY_I)) + 1); u = u * (ec->Lu_i >> DEFAULT_SIGMA_LU_I) / ((ec->Ly_i >> (DEFAULT_SIGMA_LY_I)) + 1); } #ifdef AGGRESSIVE_TIMELIMIT } #endif #else if ((ec->HCNTR_d == 0) && ((ec->Ly_i/(ec->Lu_i + 1)) > DEFAULT_SUPPR_I)) { u = u * (ec->Lu_i >> DEFAULT_SIGMA_LU_I) / ((ec->Ly_i >> (DEFAULT_SIGMA_LY_I + 2)) + 1); } #endif #endif #if 0 if ((ec->HCNTR_d == 0) && ((ec->Lu_d/ec->Ly_d) < DEFAULT_SUPPR) && (ec->Lu_d/ec->Ly_d > EC_MIN_DB_VALUE)) { suppr_factor = (10/(float)(SUPPR_FLOOR-SUPPR_CEIL))*log(ec->Lu_d/ec->Ly_d) - SUPPR_CEIL/(float)(SUPPR_FLOOR - SUPPR_CEIL); u_suppr = pow(10.0,(suppr_factor)*RES_SUPR_FACTOR/10.0)*u_suppr; } #endif ec->i_d++; return u; } static inline struct echo_can_state *echo_can_create(int len, int adaption_mode) { struct echo_can_state *ec; int maxy; int maxu; maxy = len + DEFAULT_M; maxu = DEFAULT_M; if (maxy < (1 << DEFAULT_ALPHA_YT_I)) maxy = (1 << DEFAULT_ALPHA_YT_I); if (maxy < (1 << DEFAULT_SIGMA_LY_I)) maxy = (1 << DEFAULT_SIGMA_LY_I); if (maxu < (1 << DEFAULT_SIGMA_LU_I)) maxu = (1 << DEFAULT_SIGMA_LU_I); ec = (struct echo_can_state *)MALLOC(sizeof(echo_can_state_t) + 4 + /* align */ sizeof(int) * len + /* a_i */ sizeof(short) * len + /* a_s */ 2 * sizeof(short) * (maxy) + /* y_s */ 2 * sizeof(short) * (1 << DEFAULT_ALPHA_ST_I) + /* s_s */ 2 * sizeof(short) * (maxu) + /* u_s */ 2 * sizeof(short) * len); /* y_tilde_s */ if (ec) { memset(ec, 0, sizeof(struct echo_can_state) + 4 + /* align */ sizeof(int) * len + /* a_i */ sizeof(short) * len + /* a_s */ 2 * sizeof(short) * (maxy) + /* y_s */ 2 * sizeof(short) * (1 << DEFAULT_ALPHA_ST_I) + /* s_s */ 2 * sizeof(short) * (maxu) + /* u_s */ 2 * sizeof(short) * len); /* y_tilde_s */ init_cc(ec, len, maxy, maxu); } return ec; } static inline int echo_can_traintap(struct echo_can_state *ec, int pos, short val) { /* Reset hang counter to avoid adjustments after initial forced training */ ec->HCNTR_d = ec->N_d << 1; if (pos >= ec->N_d) return 1; ec->a_i[pos] = val << 17; ec->a_s[pos] = val << 1; if (++pos >= ec->N_d) return 1; return 0; } #endif