Version 0.2.0 from FTP

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

1 files changed, 415 insertions, 0 deletions

diff --git a/sec-2.h b/sec-2.h
new file mode 100755
index 0000000..d139e86
--- /dev/null
+++ b/sec-2.h
@@ -0,0 +1,415 @@
+/*
+ * SpanDSP - a series of DSP components for telephony
+ *
+ * echo.c - An echo cancellor, suitable for electrical and acoustic
+ *	    cancellation. This code does not currently comply with
+ *	    any relevant standards (e.g. G.164/5/7/8). One day....
+ *
+ * Written by Steve Underwood <steveu@coppice.org>
+ *
+ * Copyright (C) 2001 Steve Underwood
+ *
+ * Based on a bit from here, a bit from there, eye of toad,
+ * ear of bat, etc - plus, of course, my own 2 cents.
+ *
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ */
+
+/* TODO:
+   Finish the echo suppressor option, however nasty suppression may be
+   Add an option to reintroduce side tone at -24dB under appropriate conditions.
+   Improve double talk detector (iterative!)
+*/
+
+#ifndef _ZAPTEL_SEC_H
+#define _ZAPTEL_SEC_H
+
+#ifdef __KERNEL__
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#define MALLOC(a) kmalloc((a), GFP_KERNEL)
+#define FREE(a) kfree(a)
+#else
+#include <stdlib.h>
+#include <unistd.h>
+#include <stdint.h>
+#include <string.h>
+#define MALLOC(a) malloc(a)
+#define FREE(a) free(a)
+#endif
+
+#include "fir.h"
+
+#ifndef NULL
+#define NULL 0
+#endif
+#ifndef FALSE
+#define FALSE 0
+#endif
+#ifndef TRUE
+#define TRUE (!FALSE)
+#endif
+
+#define NONUPDATE_DWELL_TIME	600 	/* 600 samples, or 75ms */
+
+typedef struct
+{
+    int tx_power;
+    int rx_power;
+    int clean_rx_power;
+
+    int rx_power_threshold;
+    int nonupdate_dwell;
+
+    fir16_state_t fir_state;
+    int16_t *fir_taps16;	/* 16-bit version of FIR taps */
+    int32_t *fir_taps32;	 /* 32-bit version of FIR taps */
+
+    int curr_pos;
+	
+    int taps;
+    int tap_mask;
+    int use_nlp;
+    int use_suppressor;
+    
+    int32_t supp_test1;
+    int32_t supp_test2;
+    int32_t supp1;
+    int32_t supp2;
+
+    int32_t latest_correction;  /* Indication of the magnitude of the latest
+    				   adaption, or a code to indicate why adaption
+				   was skipped, for test purposes */
+} echo_can_state_t;
+
+static echo_can_state_t *echo_can_create(int len, int adaption_mode);
+static void echo_can_free(echo_can_state_t *ec);
+static int16_t echo_can_update(echo_can_state_t *ec, int16_t tx, int16_t rx);
+
+/*
+ * According to Jim...
+ */
+#define MIN_TX_POWER_FOR_ADAPTION   512
+#define MIN_RX_POWER_FOR_ADAPTION   64
+
+/*
+ * According to Steve...
+ */
+/* #define MIN_TX_POWER_FOR_ADAPTION 4096
+#define MIN_RX_POWER_FOR_ADAPTION 64 */
+
+static inline echo_can_state_t *echo_can_create(int len, int adaption_mode)
+{
+    echo_can_state_t *ec;
+    void *ptr;
+    
+    ptr = ec = (echo_can_state_t *) MALLOC(sizeof(*ec) + len * sizeof(int32_t) +
+						   len * sizeof(int16_t));
+    if (ec == NULL)
+    	return  NULL;
+    memset(ec, 0, sizeof(*ec) + len * sizeof(int32_t) + len * sizeof(int16_t));
+    ec->taps = len;
+    ec->curr_pos = len - 1;
+    ec->tap_mask = len - 1;
+    ec->fir_taps32 = (int32_t *) (ptr + sizeof(*ec));
+    ec->fir_taps16 = (int16_t *) (ptr + sizeof(*ec) + len * sizeof(int32_t));
+    /* Create FIR filter */
+    fir16_create(&ec->fir_state, ec->fir_taps16, ec->taps);
+    ec->rx_power_threshold = 10000000;
+    ec->use_suppressor = FALSE;
+    /* Non-linear processor - a fancy way to say "zap small signals, to avoid
+       accumulating noise". */
+    ec->use_nlp = FALSE;
+    return  ec;
+}
+/*- End of function --------------------------------------------------------*/
+
+static inline void echo_can_free(echo_can_state_t *ec)
+{
+    fir16_free(&ec->fir_state);
+    FREE(ec);
+}
+/*- End of function --------------------------------------------------------*/
+
+static inline int16_t echo_can_update(echo_can_state_t *ec, int16_t tx, int16_t rx)
+{
+    int offset1;
+    int offset2;
+    int32_t echo_value;
+    int clean_rx;
+    int nsuppr;
+    int i;
+    int correction;
+
+    /* Evaluate the echo - i.e. apply the FIR filter */
+    /* Assume the gain of the FIR does not exceed unity. Exceeding unity
+       would seem like a rather poor thing for an echo cancellor to do :)
+       This means we can compute the result with a total disregard for
+       overflows. 16bits x 16bits -> 31bits, so no overflow can occur in
+       any multiply. While accumulating we may overflow and underflow the
+       32 bit scale often. However, if the gain does not exceed unity,
+       everything should work itself out, and the final result will be
+       OK, without any saturation logic. */
+    /* Overflow is very much possible here, and we do nothing about it because
+       of the compute costs */
+    /* 16 bit coeffs for the LMS give lousy results (maths good, actual sound
+       bad!), but 32 bit coeffs require some shifting. On balance 32 bit seems
+       best */
+    echo_value = fir16 (&ec->fir_state, tx);
+
+    /* And the answer is..... */
+    clean_rx = rx - echo_value;
+
+    /* That was the easy part. Now we need to adapt! */
+    if (ec->nonupdate_dwell > 0)
+    	ec->nonupdate_dwell--;
+
+    /* If there is very little being transmitted, any attempt to train is
+       futile. We would either be training on the far end's noise or signal,
+       the channel's own noise, or our noise. Either way, this is hardly good
+       training, so don't do it (avoid trouble). */
+    /* If the received power is very low, either we are sending very little or
+       we are already well adapted. There is little point in trying to improve
+       the adaption under these circumstanceson, so don't do it (reduce the
+       compute load). */
+    if (ec->tx_power > MIN_TX_POWER_FOR_ADAPTION
+    	&&
+	ec->rx_power > MIN_RX_POWER_FOR_ADAPTION)
+    {
+    	/* This is a really crude piece of decision logic, but it does OK
+	   for now. */
+    	if (ec->tx_power > 2*ec->rx_power)
+	{
+            /* There is no far-end speech detected */
+            if (ec->nonupdate_dwell == 0)
+	    {
+	    	/* ... and we are not in the dwell time from previous speech. */
+		//nsuppr = saturate((clean_rx << 16)/ec->tx_power);
+		nsuppr = clean_rx >> 3;
+
+		/* Update the FIR taps */
+                offset2 = ec->curr_pos + 1;
+                offset1 = ec->taps - offset2;
+		ec->latest_correction = 0;
+                for (i = ec->taps - 1;  i >= offset1;  i--)
+		{
+		    correction = ec->fir_state.history[i - offset1]*nsuppr;
+                    /* Leak to avoid false training on signals with multiple
+                       strong correlations. */
+                    ec->fir_taps32[i] -= (ec->fir_taps32[i] >> 12);
+		    ec->fir_taps32[i] += correction;
+		    ec->fir_state.coeffs[i] = ec->fir_taps32[i] >> 15;
+		    ec->latest_correction += abs(correction);
+        	}
+                for (  ;  i >= 0;  i--)
+		{
+		    correction = ec->fir_state.history[i + offset2]*nsuppr;
+                    /* Leak to avoid false training on signals with multiple
+                       strong correlations. */
+                    ec->fir_taps32[i] -= (ec->fir_taps32[i] >> 12);
+		    ec->fir_taps32[i] += correction;
+                    ec->fir_state.coeffs[i] = ec->fir_taps32[i] >> 15;
+		    ec->latest_correction += abs(correction);
+    	    	}
+    	    }
+	    else
+	    {
+        	ec->latest_correction = -1;
+    	    }
+	}
+	else
+	{
+            ec->nonupdate_dwell = NONUPDATE_DWELL_TIME;
+    	    ec->latest_correction = -2;
+	}
+    }
+    else
+    {
+        ec->nonupdate_dwell = 0;
+        ec->latest_correction = -3;
+    }
+    /* Calculate short term power levels using very simple single pole IIRs */
+    /* TODO: Is the nasty modulus approach the fastest, or would a real
+       tx*tx power calculation actually be faster? */
+    ec->tx_power += ((abs(tx) - ec->tx_power) >> 5);
+    ec->rx_power += ((abs(rx) - ec->rx_power) >> 5);
+    ec->clean_rx_power += ((abs(clean_rx) - ec->clean_rx_power) >> 5);
+
+#if defined(XYZZY)
+    if (ec->use_suppressor)
+    {
+    	ec->supp_test1 += (ec->fir_state.history[ec->curr_pos] - ec->fir_state.history[(ec->curr_pos - 7) & ec->tap_mask]);
+    	ec->supp_test2 += (ec->fir_state.history[(ec->curr_pos - 24) & ec->tap_mask] - ec->fir_state.history[(ec->curr_pos - 31) & ec->tap_mask]);
+    	if (ec->supp_test1 > 42  &&  ec->supp_test2 > 42)
+    	    supp_change = 25;
+    	else
+    	    supp_change = 50;
+    	supp = supp_change + k1*ec->supp1 + k2*ec->supp2;
+	ec->supp2 = ec->supp1;
+	ec->supp1 = supp;
+	clean_rx *= (1 - supp);
+    }
+#endif
+
+    if (ec->use_nlp  &&  ec->rx_power < 32)
+    	clean_rx = 0;
+
+    /* Roll around the rolling buffer */
+    if (ec->curr_pos <= 0)
+    	ec->curr_pos = ec->taps;
+    ec->curr_pos--;
+
+    return  clean_rx;
+}
+
+#if 0
+static inline int16_t echo_can_update(echo_can_state_t *ec, int16_t tx, int16_t rx)
+{
+    int offset;
+    int limit;
+    int32_t echo_value;
+    int clean_rx;
+    int nsuppr;
+    int i;
+    int correction;
+
+    ec->tx_history[ec->curr_pos] = tx;
+
+    /* Evaluate the echo - i.e. apply the FIR filter */
+    /* Assume the gain of the FIR does not exceed unity. Exceeding unity
+       would seem like a rather poor thing for an echo cancellor to do :)
+       This means we can compute the result with a total disregard for
+       overflows. 16bits x 16bits -> 31bits, so no overflow can occur in
+       any multiply. While accumulating we may overflow and underflow the
+       32 bit scale often. However, if the gain does not exceed unity,
+       everything should work itself out, and the final result will be
+       OK, without any saturation logic. */
+    /* Overflow is very much possible here, and we do nothing about it because
+       of the compute costs */
+    /* 16 bit coeffs for the LMS give lousy results (maths good, actual sound
+       bad!), but 32 bit coeffs require some shifting. On balance 32 bit seems
+       best */
+    offset = ec->curr_pos;
+    limit = ec->taps - offset;
+    echo_value = 0;
+    for (i = 0;  i < limit;  i++)
+        echo_value += (ec->fir_taps[i] >> 16)*ec->tx_history[i + offset];
+    offset = ec->taps - ec->curr_pos;
+    for (  ;  i < ec->taps;  i++)
+        echo_value += (ec->fir_taps[i] >> 16)*ec->tx_history[i - offset];
+    echo_value >>= 16;
+
+    /* And the answer is..... */
+    clean_rx = rx - echo_value;
+
+    /* That was the easy part. Now we need to adapt! */
+    if (ec->nonupdate_dwell > 0)
+    	ec->nonupdate_dwell--;
+
+    /* If there is very little being transmitted, any attempt to train is
+       futile. We would either be training on the far end's noise or signal,
+       the channel's own noise, or our noise. Either way, this is hardly good
+       training, so don't do it (avoid trouble). */
+    /* If the received power is very low, either we are sending very little or
+       we are already well adapted. There is little point in trying to improve
+       the adaption under these circumstanceson, so don't do it (reduce the
+       compute load). */
+    if (ec->tx_power > MIN_TX_POWER_FOR_ADAPTION
+    	&&
+	ec->rx_power > MIN_RX_POWER_FOR_ADAPTION)
+    {
+    	/* This is a really crude piece of decision logic, but it does OK
+	   for now. */
+    	if (ec->tx_power > 2*ec->rx_power)
+	{
+            /* There is no far-end speech detected */
+            if (ec->nonupdate_dwell == 0)
+	    {
+	    	/* ... and we are not in the dwell time from previous speech. */
+		//nsuppr = saturate((clean_rx << 16)/ec->tx_power);
+		nsuppr = clean_rx >> 3;
+
+		/* Update the FIR taps */
+    	        offset = ec->curr_pos;
+    	    	limit = ec->taps - offset;
+		ec->latest_correction = 0;
+    	    	for (i = 0;  i < limit;  i++)
+		{
+		    correction = ec->tx_history[i + offset]*nsuppr;
+		    ec->fir_taps[i] += correction;
+		    //ec->latest_correction += abs(correction);
+        	}
+		offset = ec->taps - ec->curr_pos;
+    		for (  ;  i < ec->taps;  i++)
+		{
+		    correction = ec->tx_history[i - offset]*nsuppr;
+		    ec->fir_taps[i] += correction;
+		    //ec->latest_correction += abs(correction);
+    	    	}
+    	    }
+	    else
+	    {
+        	ec->latest_correction = -3;
+    	    }
+	}
+	else
+	{
+            ec->nonupdate_dwell = NONUPDATE_DWELL_TIME;
+    	    ec->latest_correction = -2;
+	}
+    }
+    else
+    {
+        ec->nonupdate_dwell = 0;
+        ec->latest_correction = -1;
+    }
+    /* Calculate short term power levels using very simple single pole IIRs */
+    /* TODO: Is the nasty modulus approach the fastest, or would a real
+       tx*tx power calculation actually be faster? */
+    ec->tx_power += ((abs(tx) - ec->tx_power) >> 5);
+    ec->rx_power += ((abs(rx) - ec->rx_power) >> 5);
+    ec->clean_rx_power += ((abs(clean_rx) - ec->clean_rx_power) >> 5);
+
+#if defined(XYZZY)
+    if (ec->use_suppressor)
+    {
+    	ec->supp_test1 += (ec->tx_history[ec->curr_pos] - ec->tx_history[(ec->curr_pos - 7) & ec->tap_mask]);
+    	ec->supp_test2 += (ec->tx_history[(ec->curr_pos - 24) & ec->tap_mask] - ec->tx_history[(ec->curr_pos - 31) & ec->tap_mask]);
+    	if (ec->supp_test1 > 42  &&  ec->supp_test2 > 42)
+    	    supp_change = 25;
+    	else
+    	    supp_change = 50;
+    	supp = supp_change + k1*ec->supp1 + k2*ec->supp2;
+	ec->supp2 = ec->supp1;
+	ec->supp1 = supp;
+	clean_rx *= (1 - supp);
+    }
+#endif
+
+    if (ec->use_nlp  &&  ec->rx_power < 32)
+    	clean_rx = 0;
+
+    /* Roll around the rolling buffer */
+    ec->curr_pos = (ec->curr_pos + 1) & ec->tap_mask;
+
+    return clean_rx;
+}
+/*- End of function --------------------------------------------------------*/
+/*- End of file ------------------------------------------------------------*/
+#endif
+#endif