Move to fixed point mec3

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

1 files changed, 76 insertions, 77 deletions

diff --git a/mec3.h b/mec3.h
index 6f42824..9f0256b 100755
--- a/mec3.h
+++ b/mec3.h
@@ -22,7 +22,6 @@
 #include <linux/slab.h>
 #define MALLOC(a) kmalloc((a), GFP_KERNEL)
 #define FREE(a) kfree(a)
-#include <math.h>
 #else
 #include <stdlib.h>
 #include <unistd.h>
@@ -33,23 +32,25 @@
 #define FREE(a) free(a)
 #endif
 
+/* Features */
+
 /*
- * Define COEFF_BACKUP for experimental coefficient backup code
+ * DO_BACKUP -- Backup coefficients, and revert in the presense of double talk to try to prevent
+ * them from diverging during the ramp-up before the DTD kicks in
  */
+/* #define DO_BACKUP */
 
+#define STEP_SHIFT			2		/* Convergence rate higher = slower / better (as a shift) */
 
-#define STEP_SIZE			0.4		/* Convergence rate */
-
-#define SIGMA_P				0.01		/* Minimum adjustment */
-#define SIGMA_REF_PWR		0.01		/* Keep denominator from being 0 */
+#define SIGMA_REF_PWR		655		/* Keep denominator from being 0 */
 
-#define MIN_TX_ENERGY		256.0/32767.0		/* Must have at least this much reference */
-#define MIN_RX_ENERGY		 32.0/32767.0		/* Must have at least this much receive energy */
+#define MIN_TX_ENERGY		256		/* Must have at least this much reference */
+#define MIN_RX_ENERGY		 32		/* Must have at least this much receive energy */
 
-#define MAX_ATTENUATION		64.0				/* Maximum amount of loss we care about */
-#define MAX_BETA			0.1
+#define MAX_ATTENUATION_SHIFT	6		/* Maximum amount of loss we care about */
+#define MAX_BETA			1024
 
-#define SUPPR_ATTENUATION	16.0				/* Amount of loss at which we suppress audio */
+#define SUPPR_SHIFT			  4				/* Amount of loss at which we suppress audio */
 
 #define HANG_TIME			600		/* Hangover time */
 
@@ -57,26 +58,33 @@
 
 #define BACKUP				256			/* Backup every this number of samples */
 
+#define POWER_OFFSET		5			/* Shift power by this amount to be sure we don't overflow the
+										   reference power.  Higher = less likely to overflow, lower = more accurage */
+
+#include "arith.h"
+
 typedef struct {
-	float buf[NTAPS * 2];
-	float max;
+	short buf[NTAPS * 2];
+	short max;
 	int maxexp;
-} cbuf_f;
+} cbuf_s;
 
 typedef struct {
-	float  	a[NTAPS];				/* Coefficients */
-#ifdef COEFF_BACKUP
-	float  	b[NTAPS];				/* Coefficients */
-	float  	c[NTAPS];				/* Coefficients */
-	int backup;						/* Backup timer */
+	short	a_s[NTAPS];				/* Coefficients in shorts */
+	int  	a_i[NTAPS];				/* Coefficients in ints*/
+#ifdef DO_BACKUP
+	int  	b_i[NTAPS];				/* Coefficients (backup1) */
+	int  	c_i[NTAPS];				/* Coefficients (backup2) */
 #endif	
-	cbuf_f 	ref;					/* Reference excitation */
-	cbuf_f 	sig;					/* Signal (echo + near end + noise) */
-	cbuf_f	e;						/* Error */
-	float	refpwr;					/* Reference power */
+	cbuf_s 	ref;					/* Reference excitation */
+	cbuf_s 	sig;					/* Signal (echo + near end + noise) */
+	cbuf_s	e;						/* Error */
+	int		refpwr;					/* Reference power */
 	int		taps;					/* Number of taps */
+	int		tappwr;					/* Power of taps */
 	int		hcntr;					/* Hangtime counter */
 	int pos;						/* Position in curcular buffers */
+	int backup;						/* Backup timer */
 } echo_can_state_t;
 
 static inline void echo_can_free(echo_can_state_t *ec)
@@ -84,7 +92,7 @@ static inline void echo_can_free(echo_can_state_t *ec)
 	FREE(ec);
 }
 
-static inline void buf_add(cbuf_f *b, float sample, int pos, int taps)
+static inline void buf_add(cbuf_s *b, short sample, int pos, int taps)
 {
 	/* Store and keep track of maxima */
 	int x;
@@ -98,61 +106,45 @@ static inline void buf_add(cbuf_f *b, float sample, int pos, int taps)
 		if (!b->maxexp) {
 			b->max = 0;
 			for (x=0;x<taps;x++)
-				if (b->max < fabs(b->buf[pos + x])) {
-					b->max = fabs(b->buf[pos + x]);
+				if (b->max < abs(b->buf[pos + x])) {
+					b->max = abs(b->buf[pos + x]);
 					b->maxexp = x + 1;
 				}
 		}
 	}
 }
 
-static inline short echo_can_update(echo_can_state_t *ec, short iref, short isig)
+static inline short echo_can_update(echo_can_state_t *ec, short ref, short sig)
 {
 	int x;
-	float ref;
-	float sig;
-	float u;
-	float refpwr;
-	float beta;			/* Factor */
-	float se;			/* Simulated echo */
-	/* Convert to floats about 1.0 */
-	ref = (((float)iref)/32767.0);
-	sig = (((float)isig)/32767.0);
-
-#if 0
-	printf("start: %d, finish: %d\n", ec->start, ec->finish);
-#endif	
-
-#ifdef COEFF_BACKUP
+	short u;
+	int refpwr;
+	int beta;			/* Factor */
+	int se;			/* Simulated echo */
+#ifdef DO_BACKUP
 	if (!ec->backup) {
 		/* Backup coefficients periodically */
 		ec->backup = BACKUP;
-		memcpy(ec->c,ec->b,sizeof(ec->c));
-		memcpy(ec->b,ec->a,sizeof(ec->b));
+		memcpy(ec->c_i,ec->b_i,sizeof(ec->c_i));
+		memcpy(ec->b_i,ec->a_i,sizeof(ec->b_i));
 	} else
 		ec->backup--;
 #endif		
 	/* Remove old samples from reference power calculation */
-	ec->refpwr -= (ec->ref.buf[ec->pos] * ec->ref.buf[ec->pos]);
+	ec->refpwr -= ((ec->ref.buf[ec->pos] * ec->ref.buf[ec->pos]) >> POWER_OFFSET);
 
 	/* Store signal and reference */
 	buf_add(&ec->ref, ref, ec->pos, ec->taps);
 	buf_add(&ec->sig, sig, ec->pos, ec->taps);
 
 	/* Add new reference power */	
-	ec->refpwr += (ec->ref.buf[ec->pos] * ec->ref.buf[ec->pos]);
+	ec->refpwr += ((ec->ref.buf[ec->pos] * ec->ref.buf[ec->pos]) >> POWER_OFFSET);
 
 
 	/* Calculate simulated echo */
-	se = 0.0;
-	for (x=0;x<ec->taps;x++) 
-		se += ec->a[x] * ec->ref.buf[ec->pos + x];
-
-#if 0
-	if (!(ec->pos2++ % 1024)) {
-		printk("sig: %d, se: %d\n", (int)(32768.0 * sig), (int)(32768.0 * se));
-	}
-#endif
+	se = CONVOLVE2(ec->a_s, ec->ref.buf + ec->pos, ec->taps);		
+	se >>= 15;
+	
 	u = sig - se;
 	if (ec->hcntr)
 		ec->hcntr--;
@@ -161,67 +153,74 @@ static inline short echo_can_update(echo_can_state_t *ec, short iref, short isig
 	buf_add(&ec->e, sig, ec->pos, ec->taps);
 	if ((ec->ref.max > MIN_TX_ENERGY) && 
 	    (ec->sig.max > MIN_RX_ENERGY) &&
-		(ec->e.max * MAX_ATTENUATION > ec->ref.max)) {
+		(ec->e.max > (ec->ref.max >> MAX_ATTENUATION_SHIFT))) {
 		/* We have sufficient energy */
-		if (ec->sig.max * 2.0 < ec->ref.max)  {
+		if (ec->sig.max < (ec->ref.max >> 1))  {
 			/* No double talk */
 			if (!ec->hcntr) {
-				if (ec->refpwr < SIGMA_REF_PWR)
+				refpwr = ec->refpwr >> (16 - POWER_OFFSET);
+				if (refpwr < SIGMA_REF_PWR)
 					refpwr = SIGMA_REF_PWR;
-				else
-					refpwr = ec->refpwr;
-				beta = STEP_SIZE * u / refpwr;
+				beta = (u << 16) / refpwr;
+				beta >>= STEP_SHIFT;
 				if (beta > MAX_BETA)	
 					beta = MAX_BETA;
 				if (beta < -MAX_BETA)
 					beta = -MAX_BETA;
 				/* Update coefficients */
 				for (x=0;x<ec->taps;x++) {
-					ec->a[x] += beta * ec->ref.buf[ec->pos + x];
+					ec->a_i[x] += beta * ec->ref.buf[ec->pos + x];
+					ec->a_s[x] = ec->a_i[x] >> 16;
 				}
 			}
 		} else {
-#ifdef COEFF_BACKUP
+#ifdef DO_BACKUP
 			if (!ec->hcntr) {
 				/* Our double talk detector is turning on for the first time.  Revert
 				   our coefficients, since we're probably well into the double talk by now */
-				memcpy(ec->a, ec->c, sizeof(ec->a));
+				memcpy(ec->a_i, ec->c_i, sizeof(ec->a_i));
+				for (x=0;x<ec->taps;x++) {
+					ec->a_s[x] = ec->a_i[x] >> 16;
+				}
 			}
-			ec->backup = BACKUP;
-#endif
+#endif			
 			/* Reset hang-time counter, and prevent backups */
 			ec->hcntr = HANG_TIME;
+#ifdef DO_BACKUP
+			ec->backup = BACKUP;
+#endif			
 		}
 	}
-#ifndef NO_ECHO_SUPPRESSOR	
-	if (ec->e.max * SUPPR_ATTENUATION < ec->ref.max) {
+#ifndef NO_ECHO__SUPPRESSOR	
+	if (ec->e.max < (ec->ref.max >> SUPPR_SHIFT)) {
 		/* Suppress residual echo */
 		u *= u;
+		u >>= 16;
 	} 
 #endif	
 	ec->pos--;
 	if (ec->pos < 0)
 		ec->pos = ec->taps-1;
-	u *= 32767.0;
-	if (u < -32768.0)
-		u = -32768.0;
-	if (u > 32767.0)
-		u = 32767.0;
-	return (short)(u);
+	return u;
 }
 
 static inline echo_can_state_t *echo_can_create(int taps, int adaption_mode)
 {
 	echo_can_state_t *ec;
+	int x;
+
 	taps = NTAPS;
 	ec = MALLOC(sizeof(echo_can_state_t));
 	if (ec) {
-		printk("Allocating MEC3 canceller (%d)\n", taps);
 		memset(ec, 0, sizeof(echo_can_state_t));
 		ec->taps = taps;
-		if (ec->taps > NTAPS)
-			ec->taps = NTAPS;
 		ec->pos = ec->taps-1;
+		for (x=0;x<31;x++) {
+			if ((1 << x) >= ec->taps) {
+				ec->tappwr = x;
+				break;
+			}
+		}
 	}
 	return ec;
 }