Version 0.4.0 from FTP

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

1 files changed, 212 insertions, 0 deletions

diff --git a/mec3.h b/mec3.h
new file mode 100755
index 0000000..88ad3cb
--- /dev/null
+++ b/mec3.h
@@ -0,0 +1,212 @@
+/*
+ * Mark's Third Echo Canceller
+ *
+ * Copyright (C) 2003, Digium, Inc.
+ *
+ * This program is free software and may be used
+ * and distributed under the terms of the GNU General Public 
+ * License, incorporated herein by reference.  
+ *
+ * Dedicated to the crew of the Columbia, STS-107 for their
+ * bravery and courageous sacrifice for science.
+ *
+ */
+
+#ifndef _MARK3_ECHO_H
+#define _MARK3_ECHO_H
+
+
+
+#ifdef __KERNEL__
+#include <linux/kernel.h>
+#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>
+#include <stdint.h>
+#include <string.h>
+#include <math.h>
+#define MALLOC(a) malloc(a)
+#define FREE(a) free(a)
+#endif
+
+
+
+#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 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 MAX_ATTENUATION		64.0				/* Maximum amount of loss we care about */
+#define MAX_BETA			0.1
+
+#define SUPPR_ATTENUATION	16.0				/* Amount of loss at which we suppress audio */
+
+#define HANG_TIME			600		/* Hangover time */
+
+#define NTAPS				128			/* Number of echo can taps */
+
+#define BACKUP				256			/* Backup every this number of samples */
+
+typedef struct {
+	float buf[NTAPS * 2];
+	float max;
+	int maxexp;
+} cbuf_f;
+
+typedef struct {
+	float  	a[NTAPS];				/* Coefficients */
+	float  	b[NTAPS];				/* Coefficients */
+	float  	c[NTAPS];				/* Coefficients */
+	cbuf_f 	ref;					/* Reference excitation */
+	cbuf_f 	sig;					/* Signal (echo + near end + noise) */
+	cbuf_f	e;						/* Error */
+	float	refpwr;					/* Reference power */
+	int		taps;					/* Number 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)
+{
+	FREE(ec);
+}
+
+static inline void buf_add(cbuf_f *b, float sample, int pos, int taps)
+{
+	/* Store and keep track of maxima */
+	int x;
+	b->buf[pos] = sample;
+	b->buf[pos + taps] = sample;
+	if (sample > b->max) {
+		b->max = sample;
+		b->maxexp = taps;
+	} else {
+		b->maxexp--;
+		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]);
+					b->maxexp = x + 1;
+				}
+		}
+	}
+}
+
+static inline short echo_can_update(echo_can_state_t *ec, short iref, short isig)
+{
+	int x;
+	float ref;
+	float sig;
+	float u;
+	float refpwr;
+	float beta;			/* Factor */
+	float se;			/* Simulated echo */
+	/* Convert to floats about 1.0 */
+	ref = (iref/32767.0);
+	sig = (isig/32767.0);
+
+#if 0
+	printf("start: %d, finish: %d\n", ec->start, ec->finish);
+#endif	
+
+	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));
+	} else
+		ec->backup--;
+	/* Remove old samples from reference power calculation */
+	ec->refpwr -= (ec->ref.buf[ec->pos] * ec->ref.buf[ec->pos]);
+
+	/* 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]);
+
+
+	/* Calculate simulated echo */
+	se = 0.0;
+	for (x=0;x<ec->taps;x++) 
+		se += ec->a[x] * ec->ref.buf[ec->pos + x];
+
+	u = sig - se;
+	if (ec->hcntr)
+		ec->hcntr--;
+
+	/* Store error */
+	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)) {
+		/* We have sufficient energy */
+		if (ec->sig.max * 2.0 < ec->ref.max)  {
+			/* No double talk */
+			if (!ec->hcntr) {
+				if (ec->refpwr < SIGMA_REF_PWR)
+					refpwr = SIGMA_REF_PWR;
+				else
+					refpwr = ec->refpwr;
+				beta = STEP_SIZE * u / refpwr;
+				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];
+				}
+			}
+		} else {
+			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));
+			}
+			/* Reset hang-time counter, and prevent backups */
+			ec->hcntr = HANG_TIME;
+			ec->backup = BACKUP;
+		}
+	}
+#ifndef NO_ECHO_SUPPRESSOR	
+	if (ec->e.max * SUPPR_ATTENUATION < ec->ref.max) {
+		/* Suppress residual echo */
+		u *= u;
+	} 
+#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);
+}
+
+static inline echo_can_state_t *echo_can_create(int taps, int adaption_mode)
+{
+	echo_can_state_t *ec;
+	taps = NTAPS;
+	ec = MALLOC(sizeof(echo_can_state_t));
+	if (ec) {
+		memset(ec, 0, sizeof(ec));
+		ec->taps = taps;
+		ec->pos = ec->taps-1;
+	}
+	return ec;
+}
+
+#endif