|
5
|
1 /*
|
|
|
2 * SpanDSP - a series of DSP components for telephony
|
|
|
3 *
|
|
|
4 * modem_echo.c - An echo cancellor, suitable for electrical echos in GSTN modems
|
|
|
5 *
|
|
|
6 * Written by Steve Underwood <steveu@coppice.org>
|
|
|
7 *
|
|
|
8 * Copyright (C) 2001, 2003, 2004 Steve Underwood
|
|
|
9 *
|
|
|
10 * All rights reserved.
|
|
|
11 *
|
|
|
12 * This program is free software; you can redistribute it and/or modify
|
|
|
13 * it under the terms of the GNU General Public License version 2, as
|
|
|
14 * published by the Free Software Foundation.
|
|
|
15 *
|
|
|
16 * This program is distributed in the hope that it will be useful,
|
|
|
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
|
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
|
19 * GNU General Public License for more details.
|
|
|
20 *
|
|
|
21 * You should have received a copy of the GNU General Public License
|
|
|
22 * along with this program; if not, write to the Free Software
|
|
|
23 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
|
|
|
24 *
|
|
|
25 * $Id: modem_echo.c,v 1.17 2006/11/19 14:07:24 steveu Exp $
|
|
|
26 */
|
|
|
27
|
|
|
28 /*! \file */
|
|
|
29
|
|
|
30 /* The FIR taps must be adapted as 32 bit values, to get the necessary finesse
|
|
|
31 in the adaption process. However, they are applied as 16 bit values (bits 30-15
|
|
|
32 of the 32 bit values) in the FIR. For the working 16 bit values, we need 4 sets.
|
|
|
33 */
|
|
|
34
|
|
|
35 #ifdef HAVE_CONFIG_H
|
|
|
36 #include <config.h>
|
|
|
37 #endif
|
|
|
38
|
|
|
39 #include <stdlib.h>
|
|
|
40 #include <inttypes.h>
|
|
|
41 #include <string.h>
|
|
|
42 #include <stdio.h>
|
|
|
43 #if defined(HAVE_TGMATH_H)
|
|
|
44 #include <tgmath.h>
|
|
|
45 #endif
|
|
|
46 #if defined(HAVE_MATH_H)
|
|
|
47 #include <math.h>
|
|
|
48 #endif
|
|
|
49
|
|
|
50 #include "spandsp/telephony.h"
|
|
|
51 #include "spandsp/bit_operations.h"
|
|
|
52 #include "spandsp/dc_restore.h"
|
|
|
53 #include "spandsp/modem_echo.h"
|
|
|
54
|
|
|
55 modem_echo_can_state_t *modem_echo_can_create(int len)
|
|
|
56 {
|
|
|
57 modem_echo_can_state_t *ec;
|
|
|
58
|
|
|
59 ec = (modem_echo_can_state_t *) malloc(sizeof(*ec));
|
|
|
60 if (ec == NULL)
|
|
|
61 return NULL;
|
|
|
62 memset(ec, 0, sizeof(*ec));
|
|
|
63 ec->taps = len;
|
|
|
64 ec->curr_pos = ec->taps - 1;
|
|
|
65 if ((ec->fir_taps32 = (int32_t *) malloc(ec->taps*sizeof(int32_t))) == NULL)
|
|
|
66 {
|
|
|
67 free(ec);
|
|
|
68 return NULL;
|
|
|
69 }
|
|
|
70 memset(ec->fir_taps32, 0, ec->taps*sizeof(int32_t));
|
|
|
71 if ((ec->fir_taps16 = (int16_t *) malloc(ec->taps*sizeof(int16_t))) == NULL)
|
|
|
72 {
|
|
|
73 free(ec->fir_taps32);
|
|
|
74 free(ec);
|
|
|
75 return NULL;
|
|
|
76 }
|
|
|
77 memset(ec->fir_taps16, 0, ec->taps*sizeof(int16_t));
|
|
|
78 if (fir16_create(&ec->fir_state, ec->fir_taps16, ec->taps) == NULL)
|
|
|
79 {
|
|
|
80 free(ec->fir_taps16);
|
|
|
81 free(ec->fir_taps32);
|
|
|
82 free(ec);
|
|
|
83 return NULL;
|
|
|
84 }
|
|
|
85 return ec;
|
|
|
86 }
|
|
|
87 /*- End of function --------------------------------------------------------*/
|
|
|
88
|
|
|
89 void modem_echo_can_free(modem_echo_can_state_t *ec)
|
|
|
90 {
|
|
|
91 fir16_free(&ec->fir_state);
|
|
|
92 free(ec->fir_taps32);
|
|
|
93 free(ec->fir_taps16);
|
|
|
94 free(ec);
|
|
|
95 }
|
|
|
96 /*- End of function --------------------------------------------------------*/
|
|
|
97
|
|
|
98 void modem_echo_can_flush(modem_echo_can_state_t *ec)
|
|
|
99 {
|
|
|
100 ec->tx_power = 0;
|
|
|
101
|
|
|
102 fir16_flush(&ec->fir_state);
|
|
|
103 ec->fir_state.curr_pos = ec->taps - 1;
|
|
|
104 memset(ec->fir_taps32, 0, ec->taps*sizeof(int32_t));
|
|
|
105 memset(ec->fir_taps16, 0, ec->taps*sizeof(int16_t));
|
|
|
106 ec->curr_pos = ec->taps - 1;
|
|
|
107 }
|
|
|
108 /*- End of function --------------------------------------------------------*/
|
|
|
109
|
|
|
110 void modem_echo_can_adaption_mode(modem_echo_can_state_t *ec, int adapt)
|
|
|
111 {
|
|
|
112 ec->adapt = adapt;
|
|
|
113 }
|
|
|
114 /*- End of function --------------------------------------------------------*/
|
|
|
115
|
|
|
116 int16_t modem_echo_can_update(modem_echo_can_state_t *ec, int16_t tx, int16_t rx)
|
|
|
117 {
|
|
|
118 int32_t echo_value;
|
|
|
119 int clean_rx;
|
|
|
120 int shift;
|
|
|
121 int i;
|
|
|
122 int offset1;
|
|
|
123 int offset2;
|
|
|
124
|
|
|
125 /* Evaluate the echo - i.e. apply the FIR filter */
|
|
|
126 /* Assume the gain of the FIR does not exceed unity. Exceeding unity
|
|
|
127 would seem like a rather poor thing for an echo cancellor to do :)
|
|
|
128 This means we can compute the result with a total disregard for
|
|
|
129 overflows. 16bits x 16bits -> 31bits, so no overflow can occur in
|
|
|
130 any multiply. While accumulating we may overflow and underflow the
|
|
|
131 32 bit scale often. However, if the gain does not exceed unity,
|
|
|
132 everything should work itself out, and the final result will be
|
|
|
133 OK, without any saturation logic. */
|
|
|
134 /* Overflow is very much possible here, and we do nothing about it because
|
|
|
135 of the compute costs */
|
|
|
136 echo_value = fir16(&ec->fir_state, tx);
|
|
|
137
|
|
|
138 /* And the answer is..... */
|
|
|
139 clean_rx = rx - echo_value;
|
|
|
140 //printf("%8d %8d %8d %8d\n", tx, rx, echo_value, clean_rx);
|
|
|
141 if (ec->adapt)
|
|
|
142 {
|
|
|
143 /* Calculate short term power levels using very simple single pole IIRs */
|
|
|
144 /* TODO: Is the nasty modulus approach the fastest, or would a real
|
|
|
145 tx*tx power calculation actually be faster? Using the squares
|
|
|
146 makes the numbers grow a lot! */
|
|
|
147 ec->tx_power += ((tx*tx - ec->tx_power) >> 5);
|
|
|
148
|
|
|
149 shift = 1;
|
|
|
150 /* Update the FIR taps */
|
|
|
151 offset2 = ec->curr_pos;
|
|
|
152 offset1 = ec->taps - offset2;
|
|
|
153 for (i = ec->taps - 1; i >= offset1; i--)
|
|
|
154 {
|
|
|
155 /* Leak to avoid the coefficients drifting beyond the ability of the
|
|
|
156 adaption process to bring them back under control. */
|
|
|
157 ec->fir_taps32[i] -= (ec->fir_taps32[i] >> 23);
|
|
|
158 ec->fir_taps32[i] += (ec->fir_state.history[i - offset1]*clean_rx) >> shift;
|
|
|
159 ec->fir_taps16[i] = (int16_t) (ec->fir_taps32[i] >> 15);
|
|
|
160 }
|
|
|
161 for ( ; i >= 0; i--)
|
|
|
162 {
|
|
|
163 ec->fir_taps32[i] -= (ec->fir_taps32[i] >> 23);
|
|
|
164 ec->fir_taps32[i] += (ec->fir_state.history[i + offset2]*clean_rx) >> shift;
|
|
|
165 ec->fir_taps16[i] = (int16_t) (ec->fir_taps32[i] >> 15);
|
|
|
166 }
|
|
|
167 }
|
|
|
168
|
|
|
169 /* Roll around the rolling buffer */
|
|
|
170 if (ec->curr_pos <= 0)
|
|
|
171 ec->curr_pos = ec->taps;
|
|
|
172 ec->curr_pos--;
|
|
|
173 return (int16_t) clean_rx;
|
|
|
174 }
|
|
|
175 /*- End of function --------------------------------------------------------*/
|
|
|
176 /*- End of file ------------------------------------------------------------*/
|
