Create echo "Pretraining" ioctl

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

3 files changed, 113 insertions, 12 deletions

diff --git a/mec2.h b/mec2.h
index 92cc011..a24b01a 100755
--- a/mec2.h
+++ b/mec2.h
@@ -391,6 +391,18 @@ static inline echo_can_state_t *echo_can_create(int len, int adaption_mode)
 	return ec;
 }
 
-
+static inline int echo_can_traintap(echo_can_state_t *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
diff --git a/zaptel.c b/zaptel.c
index b5589d1..2e8589d 100755
--- a/zaptel.c
+++ b/zaptel.c
@@ -52,6 +52,14 @@
 #include <linux/if_ppp.h>
 #endif
 
+#define __ECHO_STATE_MUTE			(1 << 8)
+#define ECHO_STATE_IDLE				(0)
+#define ECHO_STATE_PRETRAINING		(1 | (__ECHO_STATE_MUTE))
+#define ECHO_STATE_STARTTRAINING	(2 | (__ECHO_STATE_MUTE))
+#define ECHO_STATE_AWAITINGECHO		(3 | (__ECHO_STATE_MUTE))
+#define ECHO_STATE_TRAINING			(4 | (__ECHO_STATE_MUTE))
+#define ECHO_STATE_ACTIVE			(5)
+
 /* #define BUF_MUNGE */
 
 /* Grab fasthdlc with tables */
@@ -1791,6 +1799,9 @@ static int initialize_channel(struct zt_chan *chan)
 	ec = chan->ec;
 	chan->ec = NULL;
 	chan->echocancel = 0;
+	chan->echostate = ECHO_STATE_IDLE;
+	chan->echolastupdate = 0;
+	chan->echotimer = 0;
 
 	chan->txdisable = 0;
 	chan->rxdisable = 0;
@@ -2613,6 +2624,8 @@ static int zt_common_ioctl(struct inode *node, struct file *file, unsigned int c
 			mychan.confna, mychan._confn, mychan.confmode, mychan.confmute);
 		printk("ec: %08x, echocancel: %d, deflaw: %d, xlaw: %08x\n",
 			(int) mychan.ec, mychan.echocancel, mychan.deflaw, (int) mychan.xlaw);
+		printk("echostate: %02x, echotimer: %d, echolastupdate: %d\n",
+			(int) mychan.echostate, mychan.echotimer, mychan.echolastupdate);
 		printk("itimer: %d, otimer: %d, ringdebtimer: %d\n\n",
 			mychan.itimer,mychan.otimer,mychan.ringdebtimer);
 #if 0
@@ -3551,6 +3564,9 @@ static int zt_chan_ioctl(struct inode *inode, struct file *file, unsigned int cm
 					tec = chan->ec;
 					chan->ec = NULL;
 					chan->echocancel = 0;
+					chan->echostate = ECHO_STATE_IDLE;
+					chan->echolastupdate = 0;
+					chan->echotimer = 0;
 					/* Make sure there's no gain */
 					if (chan->gainalloc)
 						kfree(chan->rxgain);
@@ -3621,6 +3637,9 @@ static int zt_chan_ioctl(struct inode *inode, struct file *file, unsigned int cm
 			tec = chan->ec;
 			chan->echocancel = j;
 			chan->ec = ec;
+			chan->echostate = ECHO_STATE_IDLE;
+			chan->echolastupdate = 0;
+			chan->echotimer = 0;
 			echo_can_disable_detector_init(&chan->txecdis);
 			echo_can_disable_detector_init(&chan->rxecdis);
 			spin_unlock_irqrestore(&chan->lock, flags);
@@ -3631,11 +3650,26 @@ static int zt_chan_ioctl(struct inode *inode, struct file *file, unsigned int cm
 			tec = chan->ec;
 			chan->echocancel = 0;
 			chan->ec = NULL;
+			chan->echostate = ECHO_STATE_IDLE;
+			chan->echolastupdate = 0;
+			chan->echotimer = 0;
 			spin_unlock_irqrestore(&chan->lock, flags);
 			if (tec)
 				echo_can_free(tec);
 		}
 		break;
+	case ZT_ECHOTRAIN:
+		get_user(j, (int *)data); /* get pre-training time from user */
+		if ((j < 0) || (j >= ZT_MAX_PRETRAINING))
+			return -EINVAL;
+		j <<= 3;
+		if (chan->ec) {
+			/* Start pretraining stage */
+			chan->echostate = ECHO_STATE_PRETRAINING;
+			chan->echotimer = j;
+		} else
+			return -EINVAL;
+		break;
 	case ZT_SETTXBITS:
 		if (chan->sig != ZT_SIG_CAS)
 			return -EINVAL;
@@ -4112,6 +4146,9 @@ static inline void __zt_process_getaudio_chunk(struct zt_chan *ss, unsigned char
 			if (echo_can_disable_detector_update(&ms->txecdis, getlin[x])) {
 				printk("zaptel Disabled echo canceller because of tone (tx) on channel %d\n", ss->channo);
 				ms->echocancel = 0;
+				ms->echostate = ECHO_STATE_IDLE;
+				ms->echolastupdate = 0;
+				ms->echotimer = 0;
 				kfree(ms->ec);
 				ms->ec = NULL;
 				break;
@@ -4244,9 +4281,14 @@ static inline void __zt_process_getaudio_chunk(struct zt_chan *ss, unsigned char
 			break;
 		}
 	}
-	if (ms->confmute) {
+	if (ms->confmute || (ms->echostate & __ECHO_STATE_MUTE)) {
 		txb[0] = ZT_LIN2X(0, ms);
 		memset(txb + 1, txb[0], ZT_CHUNKSIZE - 1);
+		if (ms->echostate == ECHO_STATE_STARTTRAINING) {
+			/* Transmit impulse now */
+			txb[0] = ZT_LIN2X(16384, ms);
+			ms->echostate = ECHO_STATE_AWAITINGECHO;
+		}
 	}
 	/* save value from last chunk */
 	memcpy(ms->getlin_lastchunk, ms->getlin, ZT_CHUNKSIZE * sizeof(short));
@@ -4405,7 +4447,7 @@ out in the later versions, and is put back now. */
 			memset(txb, ZT_LIN2X(0, ms), bytes);	/* Lastly we use silence on telephony channels */
 			bytes = 0;
 		}
-	}
+	}	
 }
 
 static inline void rbs_itimer_expire(struct zt_chan *chan)
@@ -4705,7 +4747,7 @@ void zt_rbsbits(struct zt_chan *chan, int cursig)
 
 void zt_ec_chunk(struct zt_chan *ss, unsigned char *rxchunk, const unsigned char *txchunk)
 {
-	short rxlin;
+	short rxlin, txlin;
 	int x;
 	long flags;
 	spin_lock_irqsave(&ss->lock, flags);
@@ -4714,10 +4756,38 @@ void zt_ec_chunk(struct zt_chan *ss, unsigned char *rxchunk, const unsigned char
 #if defined(CONFIG_ZAPTEL_MMX) || defined(ECHO_CAN_FP)
 		zt_kernel_fpu_begin();
 #endif		
-		for (x=0;x<ZT_CHUNKSIZE;x++) {
-			rxlin = ZT_XLAW(rxchunk[x], ss);
-			rxlin = echo_can_update(ss->ec, ZT_XLAW(txchunk[x], ss), rxlin);
-			rxchunk[x] = ZT_LIN2X((int)rxlin, ss);
+		if (ss->echostate & __ECHO_STATE_MUTE) {
+			/* Special stuff for training the echo can */
+			for (x=0;x<ZT_CHUNKSIZE;x++) {
+				rxlin = ZT_XLAW(rxchunk[x], ss);
+				txlin = ZT_XLAW(txchunk[x], ss);
+				if (ss->echostate == ECHO_STATE_PRETRAINING) {
+					if (--ss->echotimer <= 0) {
+						ss->echotimer = 0;
+						ss->echostate = ECHO_STATE_STARTTRAINING;
+					}
+				}
+				if ((ss->echostate == ECHO_STATE_AWAITINGECHO) && (txlin > 8000)) {
+					ss->echolastupdate = 0;
+					ss->echostate = ECHO_STATE_TRAINING;
+				}
+				if (ss->echostate == ECHO_STATE_TRAINING) {
+					if (echo_can_traintap(ss->ec, ss->echolastupdate++, rxlin)) {
+#if 0
+						printk("Finished training (%d taps trained)!\n", ss->echolastupdate);
+#endif						
+						ss->echostate = ECHO_STATE_ACTIVE;
+					}
+				}
+				rxlin = 0;
+				rxchunk[x] = ZT_LIN2X((int)rxlin, ss);
+			}
+		} else {
+			for (x=0;x<ZT_CHUNKSIZE;x++) {
+				rxlin = ZT_XLAW(rxchunk[x], ss);
+				rxlin = echo_can_update(ss->ec, ZT_XLAW(txchunk[x], ss), rxlin);
+				rxchunk[x] = ZT_LIN2X((int)rxlin, ss);
+			}
 		}
 #if defined(CONFIG_ZAPTEL_MMX) || defined(ECHO_CAN_FP)
 		kernel_fpu_end();
@@ -4801,6 +4871,9 @@ static inline void __zt_process_putaudio_chunk(struct zt_chan *ss, unsigned char
 			if (echo_can_disable_detector_update(&ms->rxecdis, putlin[x])) {
 				printk("zaptel Disabled echo canceller because of tone (rx) on channel %d\n", ss->channo);
 				ms->echocancel = 0;
+				ms->echostate = ECHO_STATE_IDLE;
+				ms->echolastupdate = 0;
+				ms->echotimer = 0;
 				kfree(ms->ec);
 				ms->ec = NULL;
 				break;
diff --git a/zaptel.h b/zaptel.h
index ebe4bea..936d213 100755
--- a/zaptel.h
+++ b/zaptel.h
@@ -539,11 +539,21 @@ char dialstr[ZT_MAX_DTMF_BUF];
 #define ZT_TIMERACK _IOW (ZT_CODE, 48, int)
 
 /*
- * Set Conference to mute mode
+ * Get Conference to mute mode
  */
 #define	ZT_GETCONFMUTE		_IOR (ZT_CODE, 49, int)
 
 /*
+ * Request echo training in some number of ms (with muting in the mean time)
+ */
+#define	ZT_ECHOTRAIN		_IOW (ZT_CODE, 50, int)
+
+/*
+ * Set on hook transfer for n number of ms -- implemnted by low level driver
+ */
+#define	ZT_ONHOOKTRANSFER		_IOW (ZT_CODE, 51, int)
+
+/*
  *  60-80 are reserved for private drivers
  *  80-85 are reserved for dynamic span stuff
  */
@@ -805,6 +815,8 @@ struct zt_tone_def {		/* Structure for zone programming */
 #define	ZT_KEWLTIME 500		/* 500ms for kewl pulse */
 #define	ZT_AFTERKEWLTIME 300    /* 300ms after kewl pulse */
 
+#define ZT_MAX_PRETRAINING   1000	/* 1000ms max pretraining time */
+
 #define ZT_MAX_SPANS		128		/* Max, 128 spans */
 #define ZT_MAX_CHANNELS		1024	/* Max, 1024 channels */
 #define ZT_MAX_CONF			1024	/* Max, 1024 conferences */
@@ -982,12 +994,12 @@ struct zt_chan {
 	struct confq confin;
 	struct confq confout;
 
-	short	getlin[ZT_MAX_CHUNKSIZE];		/* Last transmitted samples */
+	short	getlin[ZT_MAX_CHUNKSIZE];			/* Last transmitted samples */
 	unsigned char getraw[ZT_MAX_CHUNKSIZE];		/* Last received raw data */
 	short	getlin_lastchunk[ZT_MAX_CHUNKSIZE];	/* Last transmitted samples from last chunk */
-	short	putlin[ZT_MAX_CHUNKSIZE];		/* Last received samples */
+	short	putlin[ZT_MAX_CHUNKSIZE];			/* Last received samples */
 	unsigned char putraw[ZT_MAX_CHUNKSIZE];		/* Last received raw data */
-	short	conflast[ZT_MAX_CHUNKSIZE];		/* Last conference sample -- base part of channel */
+	short	conflast[ZT_MAX_CHUNKSIZE];			/* Last conference sample -- base part of channel */
 	short	conflast1[ZT_MAX_CHUNKSIZE];		/* Last conference sample  -- pseudo part of channel */
 	short	conflast2[ZT_MAX_CHUNKSIZE];		/* Previous last conference sample -- pseudo part of channel */
 	
@@ -997,6 +1009,10 @@ struct zt_chan {
 	echo_can_state_t	*ec;
 	echo_can_disable_detector_state_t txecdis;
 	echo_can_disable_detector_state_t rxecdis;
+	
+	int 	echostate;		/* State of echo canceller */
+	int		echolastupdate;	/* Last echo can update pos */
+	int		echotimer;		/* Timer for echo update */
 
 	/* RBS timings  */
 	int		prewinktime;  /* pre-wink time (ms) */