Version 0.1.0 from FTP

git-svn-id: http://svn.digium.com/svn/zaptel/trunk@7 5390a7c7-147a-4af0-8ec9-7488f05a26cb

1 files changed, 265 insertions, 0 deletions

diff --git a/mec.h b/mec.h
new file mode 100755
index 0000000..779d949
--- /dev/null
+++ b/mec.h
@@ -0,0 +1,265 @@
+/*
+ * Mark's Echo Canceller
+ *
+ * Mark Spencer <markster@linux-support.net>
+ *
+ * Simple, LMS Echo Canceller with double talk detection.  
+ * Partly based on the TI App note:
+ *     "Digital Voice Echo Canceller with a TMS 32020"
+ *
+ * Special additional thanks to:
+ * 		Jim Dixon 	  (Lambda Telecommunications)
+ *		Iman Ghobrial (Adtran, Inc.)
+ *
+ * Copyright (C) 2001, Linux Support Services, 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 _MEC_H
+#define _MEC_H 
+
+#define NUM_TAPS 64 /* Must be power of two */
+#define TAP_MASK (NUM_TAPS-1)
+
+
+
+#define SIGMA_LU_POW 6
+#define SIGMA_LY_POW 6
+#define SIGMA_YT_POW 5
+#define SIGMA_ST_POW 5
+
+#define BETA_POW	  8
+
+#define CUTOFF_S 4
+
+/* The higher you make this, the better the quality, but the more CPU time required */
+#define MIN_QUALITY 100
+
+/* This optimization saves a lot of processor but may degrade quality */
+#define OPTIMIZEDIV
+
+#if 0
+/* This converges much more slowly but saves processor */
+#define MIN_UPDATE 256
+#define MIN_SKIP   8
+#endif
+
+#define HANG_T	600					/* 600 samples, or 75ms */
+
+typedef struct mark_ec {
+	/* Circular position */
+	int cpos;
+	short y[NUM_TAPS];		/* Last N samples (relative to cpos) transmitted */
+	short y_abs[NUM_TAPS];		/* Last N samples (relative to cpos) transmitted (abs value) */
+	short s[NUM_TAPS];		/* Last N samples (relative to cpos) received */
+	short s_abs[NUM_TAPS];		/* Last N samples (relative to cpos) received (abs value) */
+	short u[NUM_TAPS];		/* Last N samples (relative to cpos) with echo removed */
+	short u_abs[NUM_TAPS];		/* Last N samples (relative to cpos) with echo removed */
+	
+	int Ly;				/* tx power */
+	int Lu;				/* Power of echo-cancelled output */
+
+	int Ty[NUM_TAPS];		/* Short term power estimate of transmit */
+	int Ts;				/* Short term power estimate of received signal */
+
+	int a[NUM_TAPS];		/* Tap weight coefficients (not relative) */
+	
+	short sdc[NUM_TAPS];		/* Near end signal before High Pass Filter */
+
+	int samples;			/* Sample count */
+	int pass;				/* Number of passes we've made */
+
+	int hangt;
+
+	int lastmax;			/* Optimize maximum search */
+	int maxTy;			/* Maximum Ty */
+} mark_ec;
+
+#define INLINE inline
+
+static INLINE void init_cc(mark_ec *ec)
+{
+	memset(ec, 0, sizeof(*ec));
+}
+
+#define PASSPOS 32000
+#undef PASSPOS
+
+static INLINE short process_cc(mark_ec *ec, short tx, short txabs, short rx, short rxabs)
+{
+	/* Process a sample, where tx is the near end and rx is the far end + echo */
+
+	int suppr;
+	int nsuppr;
+	register int Lu;
+	register int x;
+	register int pos;
+	register int r_hat;		/* Estimated echo */
+	int oldrxabs;
+	int oldtxabs;
+	int oldsupprabs;
+	int supprabs;
+#ifdef MIN_UPDATE
+	int totalupd;
+#endif
+
+	ec->pass++;
+
+	r_hat = 0;
+
+	/* Load next value */
+	ec->y[ec->cpos] = tx;
+
+	/* Load next abs value */
+	oldtxabs = ec->y_abs[ec->cpos];
+	ec->y_abs[ec->cpos] = txabs;
+
+	/* Bring in receive value (near-end signal) */
+	ec->sdc[ec->cpos] = rx;
+	
+	/* Bring in receive value absolute value */
+	oldrxabs = ec->s_abs[ec->cpos];
+	ec->s_abs[ec->cpos] = rxabs;
+
+	Lu = ec->Lu | 1;
+
+#if 0
+	/* Apply first order high pass filter (3 dB @ 160 Hz) */
+	tx = ec->s[ec->cpos] = (1.0-DEFGAMMA) * ec->s[(ec->cpos - 1) & TAP_MASK] +
+							0.5 * (1.0-DEFGAMMA) * ( ec->sdc[(ec->cpos - 1) & TAP_MASK] - ec->sdc[(ec->cpos - 2) & TAP_MASK]);
+#endif
+
+	/* Estimate echo */
+	pos = ec->cpos;
+	for (x=0;x<NUM_TAPS;x++) {
+		r_hat += ec->a[x] * ec->y[pos];
+		/* Go backwards in time and loop around circular buffer */
+		pos = (pos - 1) & TAP_MASK;
+	}
+	
+	r_hat >>= 18;
+	
+	if (ec->hangt > 0)
+		ec->hangt--;
+
+	/* printf("rx: %F, rhat: %F\n", rx, r_hat); */
+	/* Calculate suppressed amount */
+	suppr = rx - r_hat;
+
+	if (ec->pass > NUM_TAPS) {
+		/* Have to have enough taps to start with */
+		if (ec->maxTy > ec->Ts) {
+			/* There is no near-end speech detected */
+			if (!ec->hangt) {
+				/* We're not in the hang-time from the end of near-end speech */
+				if ((ec->Ly > 1024) && ((ec->Ly / Lu) < MIN_QUALITY)) {
+#ifdef OPTIMIZEDIV
+					/* We both have enough signal on the transmit   */
+					nsuppr = (suppr << 18) / ec->Ly;
+				
+					if (nsuppr > 32767)
+						nsuppr = 32767;
+					if (nsuppr < -32768)
+						nsuppr =  -32768;
+			
+					nsuppr /= ec->Ly;
+#else					
+					/* We both have enough signal on the transmit   */
+					nsuppr = (suppr << 16) / ec->Ly;
+				
+					if (nsuppr > 32767)
+						nsuppr = 32767;
+					if (nsuppr < -32768)
+						nsuppr =  -32768;
+			
+#endif					
+			
+					/* Update coefficients */
+					pos = ec->cpos;
+#ifdef MIN_UPDATE
+					totalupd =0;
+#endif					
+					for (x=0;x<NUM_TAPS;x++) {
+						register int adj;
+						adj = ec->y[pos] * nsuppr;
+#ifndef OPTIMIZEDIV
+						adj /= ec->Ly;
+						adj >>= BETA_POW;
+#else						
+						adj >>= BETA_POW + 2;
+#endif						
+#ifdef PASSPOS
+						if (ec->pass > PASSPOS)
+							printf("tx: %d, old %d: %d, adj %d, nsuppr: %d, power: %d\n", tx, x, ec->a[x], adj, nsuppr, ec->Ly);
+#endif							
+						ec->a[x] += adj;
+#ifdef MIN_UPDATE
+						totalupd += abs(adj);
+#endif						
+						/* Go backwards in time and loop around circular buffer */
+						pos = (pos - 1) & TAP_MASK;
+					}
+#ifdef MIN_UPDATE
+					/* If we didn't update at least this much, delay for many more taps */
+					if (totalupd < MIN_UPDATE) {
+						ec->hangt += MIN_SKIP;
+					}
+#endif						
+				} 
+					
+			}
+		} else
+			/* Near end speech detected */
+			ec->hangt = HANG_T;
+	} 
+
+	/* Save supression and absolute values */
+	supprabs = abs(suppr);
+	oldsupprabs = ec->u_abs[ec->cpos];
+	ec->u[ec->cpos] = suppr;
+	ec->u_abs[ec->cpos] = supprabs;
+
+	/* Update tx power */
+	ec->Ly += (txabs >> SIGMA_LY_POW) - (oldtxabs >> SIGMA_LY_POW);
+
+	/* Update rx power */
+	ec->Lu += (supprabs  >> SIGMA_LU_POW) - (oldsupprabs >> SIGMA_LU_POW);
+
+	/* Short term power of tx */
+	ec->Ty[ec->cpos] = ec->Ty[(ec->cpos - 1) & TAP_MASK] + 
+		((txabs >> SIGMA_YT_POW ) - (oldtxabs >> SIGMA_YT_POW));
+	
+	/* Keep track of highest */
+	if (ec->lastmax == ec->cpos) {
+		register int maxTy = 0;
+		/* Have to loop through and find the new highest since our old highest expired */
+		/* Estimate echo */
+		pos = ec->cpos;
+		for (x=0;x<NUM_TAPS;x++) {
+			if (ec->Ty[pos] > maxTy)
+				maxTy = ec->Ty[pos];
+			/* Go backwards in time and loop around circular buffer */
+			pos = (pos - 1) & TAP_MASK;
+		}
+		ec->maxTy = maxTy;
+	} else {
+		/* Just keep the highest */
+		if (ec->Ty[ec->cpos] > ec->maxTy) {
+			ec->maxTy = ec->Ty[ec->cpos];
+			ec->lastmax = ec->cpos;
+		}
+	}
+	ec->Ts += (rxabs >> SIGMA_ST_POW) - (oldrxabs >> SIGMA_ST_POW) ;
+
+	/* Increment position memory */
+	ec->cpos = (ec->cpos + 1 ) & TAP_MASK;
+	
+	return suppr;
+}
+
+#endif