David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 1 | /* |
| 2 | * SpanDSP - a series of DSP components for telephony |
| 3 | * |
| 4 | * echo.c - A line echo canceller. This code is being developed |
| 5 | * against and partially complies with G168. |
| 6 | * |
| 7 | * Written by Steve Underwood <steveu@coppice.org> |
| 8 | * and David Rowe <david_at_rowetel_dot_com> |
| 9 | * |
| 10 | * Copyright (C) 2001, 2003 Steve Underwood, 2007 David Rowe |
| 11 | * |
| 12 | * Based on a bit from here, a bit from there, eye of toad, ear of |
| 13 | * bat, 15 years of failed attempts by David and a few fried brain |
| 14 | * cells. |
| 15 | * |
| 16 | * All rights reserved. |
| 17 | * |
| 18 | * This program is free software; you can redistribute it and/or modify |
| 19 | * it under the terms of the GNU General Public License version 2, as |
| 20 | * published by the Free Software Foundation. |
| 21 | * |
| 22 | * This program is distributed in the hope that it will be useful, |
| 23 | * but WITHOUT ANY WARRANTY; without even the implied warranty of |
| 24 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| 25 | * GNU General Public License for more details. |
| 26 | * |
| 27 | * You should have received a copy of the GNU General Public License |
| 28 | * along with this program; if not, write to the Free Software |
| 29 | * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. |
| 30 | * |
| 31 | * $Id: echo.c,v 1.20 2006/12/01 18:00:48 steveu Exp $ |
| 32 | */ |
| 33 | |
| 34 | /*! \file */ |
| 35 | |
| 36 | /* Implementation Notes |
| 37 | David Rowe |
| 38 | April 2007 |
| 39 | |
| 40 | This code started life as Steve's NLMS algorithm with a tap |
| 41 | rotation algorithm to handle divergence during double talk. I |
| 42 | added a Geigel Double Talk Detector (DTD) [2] and performed some |
| 43 | G168 tests. However I had trouble meeting the G168 requirements, |
| 44 | especially for double talk - there were always cases where my DTD |
| 45 | failed, for example where near end speech was under the 6dB |
| 46 | threshold required for declaring double talk. |
| 47 | |
| 48 | So I tried a two path algorithm [1], which has so far given better |
| 49 | results. The original tap rotation/Geigel algorithm is available |
| 50 | in SVN http://svn.rowetel.com/software/oslec/tags/before_16bit. |
| 51 | It's probably possible to make it work if some one wants to put some |
| 52 | serious work into it. |
| 53 | |
| 54 | At present no special treatment is provided for tones, which |
| 55 | generally cause NLMS algorithms to diverge. Initial runs of a |
| 56 | subset of the G168 tests for tones (e.g ./echo_test 6) show the |
| 57 | current algorithm is passing OK, which is kind of surprising. The |
| 58 | full set of tests needs to be performed to confirm this result. |
| 59 | |
| 60 | One other interesting change is that I have managed to get the NLMS |
| 61 | code to work with 16 bit coefficients, rather than the original 32 |
| 62 | bit coefficents. This reduces the MIPs and storage required. |
| 63 | I evaulated the 16 bit port using g168_tests.sh and listening tests |
| 64 | on 4 real-world samples. |
| 65 | |
| 66 | I also attempted the implementation of a block based NLMS update |
| 67 | [2] but although this passes g168_tests.sh it didn't converge well |
| 68 | on the real-world samples. I have no idea why, perhaps a scaling |
| 69 | problem. The block based code is also available in SVN |
| 70 | http://svn.rowetel.com/software/oslec/tags/before_16bit. If this |
| 71 | code can be debugged, it will lead to further reduction in MIPS, as |
| 72 | the block update code maps nicely onto DSP instruction sets (it's a |
| 73 | dot product) compared to the current sample-by-sample update. |
| 74 | |
| 75 | Steve also has some nice notes on echo cancellers in echo.h |
| 76 | |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 77 | References: |
| 78 | |
| 79 | [1] Ochiai, Areseki, and Ogihara, "Echo Canceller with Two Echo |
| 80 | Path Models", IEEE Transactions on communications, COM-25, |
| 81 | No. 6, June |
| 82 | 1977. |
| 83 | http://www.rowetel.com/images/echo/dual_path_paper.pdf |
| 84 | |
| 85 | [2] The classic, very useful paper that tells you how to |
| 86 | actually build a real world echo canceller: |
| 87 | Messerschmitt, Hedberg, Cole, Haoui, Winship, "Digital Voice |
| 88 | Echo Canceller with a TMS320020, |
| 89 | http://www.rowetel.com/images/echo/spra129.pdf |
| 90 | |
| 91 | [3] I have written a series of blog posts on this work, here is |
| 92 | Part 1: http://www.rowetel.com/blog/?p=18 |
| 93 | |
| 94 | [4] The source code http://svn.rowetel.com/software/oslec/ |
| 95 | |
| 96 | [5] A nice reference on LMS filters: |
| 97 | http://en.wikipedia.org/wiki/Least_mean_squares_filter |
| 98 | |
| 99 | Credits: |
| 100 | |
| 101 | Thanks to Steve Underwood, Jean-Marc Valin, and Ramakrishnan |
| 102 | Muthukrishnan for their suggestions and email discussions. Thanks |
| 103 | also to those people who collected echo samples for me such as |
| 104 | Mark, Pawel, and Pavel. |
| 105 | */ |
| 106 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 107 | #include <linux/kernel.h> /* We're doing kernel work */ |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 108 | #include <linux/module.h> |
| 109 | #include <linux/kernel.h> |
| 110 | #include <linux/slab.h> |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 111 | |
| 112 | #include "bit_operations.h" |
| 113 | #include "echo.h" |
| 114 | |
| 115 | #define MIN_TX_POWER_FOR_ADAPTION 64 |
| 116 | #define MIN_RX_POWER_FOR_ADAPTION 64 |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 117 | #define DTD_HANGOVER 600 /* 600 samples, or 75ms */ |
| 118 | #define DC_LOG2BETA 3 /* log2() of DC filter Beta */ |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 119 | |
| 120 | /*-----------------------------------------------------------------------*\ |
| 121 | FUNCTIONS |
| 122 | \*-----------------------------------------------------------------------*/ |
| 123 | |
| 124 | /* adapting coeffs using the traditional stochastic descent (N)LMS algorithm */ |
| 125 | |
Tzafrir Cohen | f55ccbf | 2008-10-12 08:13:21 +0200 | [diff] [blame] | 126 | #ifdef __bfin__ |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 127 | static void __inline__ lms_adapt_bg(struct oslec_state *ec, int clean, |
| 128 | int shift) |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 129 | { |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 130 | int i, j; |
| 131 | int offset1; |
| 132 | int offset2; |
| 133 | int factor; |
| 134 | int exp; |
| 135 | int16_t *phist; |
| 136 | int n; |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 137 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 138 | if (shift > 0) |
| 139 | factor = clean << shift; |
| 140 | else |
| 141 | factor = clean >> -shift; |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 142 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 143 | /* Update the FIR taps */ |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 144 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 145 | offset2 = ec->curr_pos; |
| 146 | offset1 = ec->taps - offset2; |
| 147 | phist = &ec->fir_state_bg.history[offset2]; |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 148 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 149 | /* st: and en: help us locate the assembler in echo.s */ |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 150 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 151 | //asm("st:"); |
| 152 | n = ec->taps; |
| 153 | for (i = 0, j = offset2; i < n; i++, j++) { |
| 154 | exp = *phist++ * factor; |
| 155 | ec->fir_taps16[1][i] += (int16_t) ((exp + (1 << 14)) >> 15); |
| 156 | } |
| 157 | //asm("en:"); |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 158 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 159 | /* Note the asm for the inner loop above generated by Blackfin gcc |
| 160 | 4.1.1 is pretty good (note even parallel instructions used): |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 161 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 162 | R0 = W [P0++] (X); |
| 163 | R0 *= R2; |
| 164 | R0 = R0 + R3 (NS) || |
| 165 | R1 = W [P1] (X) || |
| 166 | nop; |
| 167 | R0 >>>= 15; |
| 168 | R0 = R0 + R1; |
| 169 | W [P1++] = R0; |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 170 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 171 | A block based update algorithm would be much faster but the |
| 172 | above can't be improved on much. Every instruction saved in |
| 173 | the loop above is 2 MIPs/ch! The for loop above is where the |
| 174 | Blackfin spends most of it's time - about 17 MIPs/ch measured |
| 175 | with speedtest.c with 256 taps (32ms). Write-back and |
| 176 | Write-through cache gave about the same performance. |
| 177 | */ |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 178 | } |
| 179 | |
| 180 | /* |
| 181 | IDEAS for further optimisation of lms_adapt_bg(): |
| 182 | |
| 183 | 1/ The rounding is quite costly. Could we keep as 32 bit coeffs |
| 184 | then make filter pluck the MS 16-bits of the coeffs when filtering? |
| 185 | However this would lower potential optimisation of filter, as I |
| 186 | think the dual-MAC architecture requires packed 16 bit coeffs. |
| 187 | |
| 188 | 2/ Block based update would be more efficient, as per comments above, |
| 189 | could use dual MAC architecture. |
| 190 | |
| 191 | 3/ Look for same sample Blackfin LMS code, see if we can get dual-MAC |
| 192 | packing. |
| 193 | |
| 194 | 4/ Execute the whole e/c in a block of say 20ms rather than sample |
| 195 | by sample. Processing a few samples every ms is inefficient. |
| 196 | */ |
| 197 | |
| 198 | #else |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 199 | static __inline__ void lms_adapt_bg(struct oslec_state *ec, int clean, |
| 200 | int shift) |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 201 | { |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 202 | int i; |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 203 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 204 | int offset1; |
| 205 | int offset2; |
| 206 | int factor; |
| 207 | int exp; |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 208 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 209 | if (shift > 0) |
| 210 | factor = clean << shift; |
| 211 | else |
| 212 | factor = clean >> -shift; |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 213 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 214 | /* Update the FIR taps */ |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 215 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 216 | offset2 = ec->curr_pos; |
| 217 | offset1 = ec->taps - offset2; |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 218 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 219 | for (i = ec->taps - 1; i >= offset1; i--) { |
| 220 | exp = (ec->fir_state_bg.history[i - offset1] * factor); |
| 221 | ec->fir_taps16[1][i] += (int16_t) ((exp + (1 << 14)) >> 15); |
| 222 | } |
| 223 | for (; i >= 0; i--) { |
| 224 | exp = (ec->fir_state_bg.history[i + offset2] * factor); |
| 225 | ec->fir_taps16[1][i] += (int16_t) ((exp + (1 << 14)) >> 15); |
| 226 | } |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 227 | } |
| 228 | #endif |
| 229 | |
Tzafrir Cohen | 9d8f2d5 | 2008-10-12 07:17:26 +0200 | [diff] [blame] | 230 | struct oslec_state *oslec_create(int len, int adaption_mode) |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 231 | { |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 232 | struct oslec_state *ec; |
| 233 | int i; |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 234 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 235 | ec = kzalloc(sizeof(*ec), GFP_KERNEL); |
| 236 | if (!ec) |
| 237 | return NULL; |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 238 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 239 | ec->taps = len; |
| 240 | ec->log2taps = top_bit(len); |
| 241 | ec->curr_pos = ec->taps - 1; |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 242 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 243 | for (i = 0; i < 2; i++) { |
| 244 | ec->fir_taps16[i] = |
| 245 | kcalloc(ec->taps, sizeof(int16_t), GFP_KERNEL); |
| 246 | if (!ec->fir_taps16[i]) |
| 247 | goto error_oom; |
| 248 | } |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 249 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 250 | fir16_create(&ec->fir_state, ec->fir_taps16[0], ec->taps); |
| 251 | fir16_create(&ec->fir_state_bg, ec->fir_taps16[1], ec->taps); |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 252 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 253 | for (i = 0; i < 5; i++) { |
| 254 | ec->xvtx[i] = ec->yvtx[i] = ec->xvrx[i] = ec->yvrx[i] = 0; |
| 255 | } |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 256 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 257 | ec->cng_level = 1000; |
| 258 | oslec_adaption_mode(ec, adaption_mode); |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 259 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 260 | ec->snapshot = kcalloc(ec->taps, sizeof(int16_t), GFP_KERNEL); |
| 261 | if (!ec->snapshot) |
| 262 | goto error_oom; |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 263 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 264 | ec->cond_met = 0; |
| 265 | ec->Pstates = 0; |
| 266 | ec->Ltxacc = ec->Lrxacc = ec->Lcleanacc = ec->Lclean_bgacc = 0; |
| 267 | ec->Ltx = ec->Lrx = ec->Lclean = ec->Lclean_bg = 0; |
| 268 | ec->tx_1 = ec->tx_2 = ec->rx_1 = ec->rx_2 = 0; |
| 269 | ec->Lbgn = ec->Lbgn_acc = 0; |
| 270 | ec->Lbgn_upper = 200; |
| 271 | ec->Lbgn_upper_acc = ec->Lbgn_upper << 13; |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 272 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 273 | return ec; |
Pekka Enberg | db2af14 | 2008-10-17 20:55:03 +0300 | [diff] [blame] | 274 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 275 | error_oom: |
| 276 | for (i = 0; i < 2; i++) |
| 277 | kfree(ec->fir_taps16[i]); |
Pekka Enberg | db2af14 | 2008-10-17 20:55:03 +0300 | [diff] [blame] | 278 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 279 | kfree(ec); |
| 280 | return NULL; |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 281 | } |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 282 | |
Tzafrir Cohen | 9d8f2d5 | 2008-10-12 07:17:26 +0200 | [diff] [blame] | 283 | EXPORT_SYMBOL_GPL(oslec_create); |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 284 | |
Tzafrir Cohen | 9d8f2d5 | 2008-10-12 07:17:26 +0200 | [diff] [blame] | 285 | void oslec_free(struct oslec_state *ec) |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 286 | { |
| 287 | int i; |
| 288 | |
| 289 | fir16_free(&ec->fir_state); |
| 290 | fir16_free(&ec->fir_state_bg); |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 291 | for (i = 0; i < 2; i++) |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 292 | kfree(ec->fir_taps16[i]); |
| 293 | kfree(ec->snapshot); |
| 294 | kfree(ec); |
| 295 | } |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 296 | |
Tzafrir Cohen | 9d8f2d5 | 2008-10-12 07:17:26 +0200 | [diff] [blame] | 297 | EXPORT_SYMBOL_GPL(oslec_free); |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 298 | |
Tzafrir Cohen | 9d8f2d5 | 2008-10-12 07:17:26 +0200 | [diff] [blame] | 299 | void oslec_adaption_mode(struct oslec_state *ec, int adaption_mode) |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 300 | { |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 301 | ec->adaption_mode = adaption_mode; |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 302 | } |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 303 | |
Tzafrir Cohen | 9d8f2d5 | 2008-10-12 07:17:26 +0200 | [diff] [blame] | 304 | EXPORT_SYMBOL_GPL(oslec_adaption_mode); |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 305 | |
Tzafrir Cohen | 9d8f2d5 | 2008-10-12 07:17:26 +0200 | [diff] [blame] | 306 | void oslec_flush(struct oslec_state *ec) |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 307 | { |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 308 | int i; |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 309 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 310 | ec->Ltxacc = ec->Lrxacc = ec->Lcleanacc = ec->Lclean_bgacc = 0; |
| 311 | ec->Ltx = ec->Lrx = ec->Lclean = ec->Lclean_bg = 0; |
| 312 | ec->tx_1 = ec->tx_2 = ec->rx_1 = ec->rx_2 = 0; |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 313 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 314 | ec->Lbgn = ec->Lbgn_acc = 0; |
| 315 | ec->Lbgn_upper = 200; |
| 316 | ec->Lbgn_upper_acc = ec->Lbgn_upper << 13; |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 317 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 318 | ec->nonupdate_dwell = 0; |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 319 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 320 | fir16_flush(&ec->fir_state); |
| 321 | fir16_flush(&ec->fir_state_bg); |
| 322 | ec->fir_state.curr_pos = ec->taps - 1; |
| 323 | ec->fir_state_bg.curr_pos = ec->taps - 1; |
| 324 | for (i = 0; i < 2; i++) |
| 325 | memset(ec->fir_taps16[i], 0, ec->taps * sizeof(int16_t)); |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 326 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 327 | ec->curr_pos = ec->taps - 1; |
| 328 | ec->Pstates = 0; |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 329 | } |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 330 | |
Tzafrir Cohen | 9d8f2d5 | 2008-10-12 07:17:26 +0200 | [diff] [blame] | 331 | EXPORT_SYMBOL_GPL(oslec_flush); |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 332 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 333 | void oslec_snapshot(struct oslec_state *ec) |
| 334 | { |
| 335 | memcpy(ec->snapshot, ec->fir_taps16[0], ec->taps * sizeof(int16_t)); |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 336 | } |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 337 | |
Tzafrir Cohen | 9d8f2d5 | 2008-10-12 07:17:26 +0200 | [diff] [blame] | 338 | EXPORT_SYMBOL_GPL(oslec_snapshot); |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 339 | |
| 340 | /* Dual Path Echo Canceller ------------------------------------------------*/ |
| 341 | |
Tzafrir Cohen | 9d8f2d5 | 2008-10-12 07:17:26 +0200 | [diff] [blame] | 342 | int16_t oslec_update(struct oslec_state *ec, int16_t tx, int16_t rx) |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 343 | { |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 344 | int32_t echo_value; |
| 345 | int clean_bg; |
| 346 | int tmp, tmp1; |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 347 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 348 | /* Input scaling was found be required to prevent problems when tx |
| 349 | starts clipping. Another possible way to handle this would be the |
| 350 | filter coefficent scaling. */ |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 351 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 352 | ec->tx = tx; |
| 353 | ec->rx = rx; |
| 354 | tx >>= 1; |
| 355 | rx >>= 1; |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 356 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 357 | /* |
| 358 | Filter DC, 3dB point is 160Hz (I think), note 32 bit precision required |
| 359 | otherwise values do not track down to 0. Zero at DC, Pole at (1-Beta) |
| 360 | only real axis. Some chip sets (like Si labs) don't need |
| 361 | this, but something like a $10 X100P card does. Any DC really slows |
| 362 | down convergence. |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 363 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 364 | Note: removes some low frequency from the signal, this reduces |
| 365 | the speech quality when listening to samples through headphones |
| 366 | but may not be obvious through a telephone handset. |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 367 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 368 | Note that the 3dB frequency in radians is approx Beta, e.g. for |
| 369 | Beta = 2^(-3) = 0.125, 3dB freq is 0.125 rads = 159Hz. |
| 370 | */ |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 371 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 372 | if (ec->adaption_mode & ECHO_CAN_USE_RX_HPF) { |
| 373 | tmp = rx << 15; |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 374 | #if 1 |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 375 | /* Make sure the gain of the HPF is 1.0. This can still saturate a little under |
| 376 | impulse conditions, and it might roll to 32768 and need clipping on sustained peak |
| 377 | level signals. However, the scale of such clipping is small, and the error due to |
| 378 | any saturation should not markedly affect the downstream processing. */ |
| 379 | tmp -= (tmp >> 4); |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 380 | #endif |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 381 | ec->rx_1 += -(ec->rx_1 >> DC_LOG2BETA) + tmp - ec->rx_2; |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 382 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 383 | /* hard limit filter to prevent clipping. Note that at this stage |
| 384 | rx should be limited to +/- 16383 due to right shift above */ |
| 385 | tmp1 = ec->rx_1 >> 15; |
| 386 | if (tmp1 > 16383) |
| 387 | tmp1 = 16383; |
| 388 | if (tmp1 < -16383) |
| 389 | tmp1 = -16383; |
| 390 | rx = tmp1; |
| 391 | ec->rx_2 = tmp; |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 392 | } |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 393 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 394 | /* Block average of power in the filter states. Used for |
| 395 | adaption power calculation. */ |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 396 | |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 397 | { |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 398 | int new, old; |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 399 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 400 | /* efficient "out with the old and in with the new" algorithm so |
| 401 | we don't have to recalculate over the whole block of |
| 402 | samples. */ |
| 403 | new = (int)tx *(int)tx; |
| 404 | old = (int)ec->fir_state.history[ec->fir_state.curr_pos] * |
| 405 | (int)ec->fir_state.history[ec->fir_state.curr_pos]; |
| 406 | ec->Pstates += |
| 407 | ((new - old) + (1 << ec->log2taps)) >> ec->log2taps; |
| 408 | if (ec->Pstates < 0) |
| 409 | ec->Pstates = 0; |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 410 | } |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 411 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 412 | /* Calculate short term average levels using simple single pole IIRs */ |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 413 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 414 | ec->Ltxacc += abs(tx) - ec->Ltx; |
| 415 | ec->Ltx = (ec->Ltxacc + (1 << 4)) >> 5; |
| 416 | ec->Lrxacc += abs(rx) - ec->Lrx; |
| 417 | ec->Lrx = (ec->Lrxacc + (1 << 4)) >> 5; |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 418 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 419 | /* Foreground filter --------------------------------------------------- */ |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 420 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 421 | ec->fir_state.coeffs = ec->fir_taps16[0]; |
| 422 | echo_value = fir16(&ec->fir_state, tx); |
| 423 | ec->clean = rx - echo_value; |
| 424 | ec->Lcleanacc += abs(ec->clean) - ec->Lclean; |
| 425 | ec->Lclean = (ec->Lcleanacc + (1 << 4)) >> 5; |
| 426 | |
| 427 | /* Background filter --------------------------------------------------- */ |
| 428 | |
| 429 | echo_value = fir16(&ec->fir_state_bg, tx); |
| 430 | clean_bg = rx - echo_value; |
| 431 | ec->Lclean_bgacc += abs(clean_bg) - ec->Lclean_bg; |
| 432 | ec->Lclean_bg = (ec->Lclean_bgacc + (1 << 4)) >> 5; |
| 433 | |
| 434 | /* Background Filter adaption ----------------------------------------- */ |
| 435 | |
| 436 | /* Almost always adap bg filter, just simple DT and energy |
| 437 | detection to minimise adaption in cases of strong double talk. |
| 438 | However this is not critical for the dual path algorithm. |
| 439 | */ |
| 440 | ec->factor = 0; |
| 441 | ec->shift = 0; |
| 442 | if ((ec->nonupdate_dwell == 0)) { |
| 443 | int P, logP, shift; |
| 444 | |
| 445 | /* Determine: |
| 446 | |
| 447 | f = Beta * clean_bg_rx/P ------ (1) |
| 448 | |
| 449 | where P is the total power in the filter states. |
| 450 | |
| 451 | The Boffins have shown that if we obey (1) we converge |
| 452 | quickly and avoid instability. |
| 453 | |
| 454 | The correct factor f must be in Q30, as this is the fixed |
| 455 | point format required by the lms_adapt_bg() function, |
| 456 | therefore the scaled version of (1) is: |
| 457 | |
| 458 | (2^30) * f = (2^30) * Beta * clean_bg_rx/P |
| 459 | factor = (2^30) * Beta * clean_bg_rx/P ----- (2) |
| 460 | |
| 461 | We have chosen Beta = 0.25 by experiment, so: |
| 462 | |
| 463 | factor = (2^30) * (2^-2) * clean_bg_rx/P |
| 464 | |
| 465 | (30 - 2 - log2(P)) |
| 466 | factor = clean_bg_rx 2 ----- (3) |
| 467 | |
| 468 | To avoid a divide we approximate log2(P) as top_bit(P), |
| 469 | which returns the position of the highest non-zero bit in |
| 470 | P. This approximation introduces an error as large as a |
| 471 | factor of 2, but the algorithm seems to handle it OK. |
| 472 | |
| 473 | Come to think of it a divide may not be a big deal on a |
| 474 | modern DSP, so its probably worth checking out the cycles |
| 475 | for a divide versus a top_bit() implementation. |
| 476 | */ |
| 477 | |
| 478 | P = MIN_TX_POWER_FOR_ADAPTION + ec->Pstates; |
| 479 | logP = top_bit(P) + ec->log2taps; |
| 480 | shift = 30 - 2 - logP; |
| 481 | ec->shift = shift; |
| 482 | |
| 483 | lms_adapt_bg(ec, clean_bg, shift); |
| 484 | } |
| 485 | |
| 486 | /* very simple DTD to make sure we dont try and adapt with strong |
| 487 | near end speech */ |
| 488 | |
| 489 | ec->adapt = 0; |
| 490 | if ((ec->Lrx > MIN_RX_POWER_FOR_ADAPTION) && (ec->Lrx > ec->Ltx)) |
| 491 | ec->nonupdate_dwell = DTD_HANGOVER; |
| 492 | if (ec->nonupdate_dwell) |
| 493 | ec->nonupdate_dwell--; |
| 494 | |
| 495 | /* Transfer logic ------------------------------------------------------ */ |
| 496 | |
| 497 | /* These conditions are from the dual path paper [1], I messed with |
| 498 | them a bit to improve performance. */ |
| 499 | |
| 500 | if ((ec->adaption_mode & ECHO_CAN_USE_ADAPTION) && |
| 501 | (ec->nonupdate_dwell == 0) && |
| 502 | (8 * ec->Lclean_bg < |
| 503 | 7 * ec->Lclean) /* (ec->Lclean_bg < 0.875*ec->Lclean) */ && |
| 504 | (8 * ec->Lclean_bg < |
| 505 | ec->Ltx) /* (ec->Lclean_bg < 0.125*ec->Ltx) */ ) { |
| 506 | if (ec->cond_met == 6) { |
| 507 | /* BG filter has had better results for 6 consecutive samples */ |
| 508 | ec->adapt = 1; |
| 509 | memcpy(ec->fir_taps16[0], ec->fir_taps16[1], |
| 510 | ec->taps * sizeof(int16_t)); |
| 511 | } else |
| 512 | ec->cond_met++; |
| 513 | } else |
| 514 | ec->cond_met = 0; |
| 515 | |
| 516 | /* Non-Linear Processing --------------------------------------------------- */ |
| 517 | |
| 518 | ec->clean_nlp = ec->clean; |
| 519 | if (ec->adaption_mode & ECHO_CAN_USE_NLP) { |
| 520 | /* Non-linear processor - a fancy way to say "zap small signals, to avoid |
| 521 | residual echo due to (uLaw/ALaw) non-linearity in the channel.". */ |
| 522 | |
| 523 | if ((16 * ec->Lclean < ec->Ltx)) { |
| 524 | /* Our e/c has improved echo by at least 24 dB (each factor of 2 is 6dB, |
| 525 | so 2*2*2*2=16 is the same as 6+6+6+6=24dB) */ |
| 526 | if (ec->adaption_mode & ECHO_CAN_USE_CNG) { |
| 527 | ec->cng_level = ec->Lbgn; |
| 528 | |
| 529 | /* Very elementary comfort noise generation. Just random |
| 530 | numbers rolled off very vaguely Hoth-like. DR: This |
| 531 | noise doesn't sound quite right to me - I suspect there |
| 532 | are some overlfow issues in the filtering as it's too |
| 533 | "crackly". TODO: debug this, maybe just play noise at |
| 534 | high level or look at spectrum. |
| 535 | */ |
| 536 | |
| 537 | ec->cng_rndnum = |
| 538 | 1664525U * ec->cng_rndnum + 1013904223U; |
| 539 | ec->cng_filter = |
| 540 | ((ec->cng_rndnum & 0xFFFF) - 32768 + |
| 541 | 5 * ec->cng_filter) >> 3; |
| 542 | ec->clean_nlp = |
| 543 | (ec->cng_filter * ec->cng_level * 8) >> 14; |
| 544 | |
| 545 | } else if (ec->adaption_mode & ECHO_CAN_USE_CLIP) { |
| 546 | /* This sounds much better than CNG */ |
| 547 | if (ec->clean_nlp > ec->Lbgn) |
| 548 | ec->clean_nlp = ec->Lbgn; |
| 549 | if (ec->clean_nlp < -ec->Lbgn) |
| 550 | ec->clean_nlp = -ec->Lbgn; |
| 551 | } else { |
| 552 | /* just mute the residual, doesn't sound very good, used mainly |
| 553 | in G168 tests */ |
| 554 | ec->clean_nlp = 0; |
| 555 | } |
| 556 | } else { |
| 557 | /* Background noise estimator. I tried a few algorithms |
| 558 | here without much luck. This very simple one seems to |
| 559 | work best, we just average the level using a slow (1 sec |
| 560 | time const) filter if the current level is less than a |
| 561 | (experimentally derived) constant. This means we dont |
| 562 | include high level signals like near end speech. When |
| 563 | combined with CNG or especially CLIP seems to work OK. |
| 564 | */ |
| 565 | if (ec->Lclean < 40) { |
| 566 | ec->Lbgn_acc += abs(ec->clean) - ec->Lbgn; |
| 567 | ec->Lbgn = (ec->Lbgn_acc + (1 << 11)) >> 12; |
| 568 | } |
| 569 | } |
| 570 | } |
| 571 | |
| 572 | /* Roll around the taps buffer */ |
| 573 | if (ec->curr_pos <= 0) |
| 574 | ec->curr_pos = ec->taps; |
| 575 | ec->curr_pos--; |
| 576 | |
| 577 | if (ec->adaption_mode & ECHO_CAN_DISABLE) |
| 578 | ec->clean_nlp = rx; |
| 579 | |
| 580 | /* Output scaled back up again to match input scaling */ |
| 581 | |
| 582 | return (int16_t) ec->clean_nlp << 1; |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 583 | } |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 584 | |
Tzafrir Cohen | 9d8f2d5 | 2008-10-12 07:17:26 +0200 | [diff] [blame] | 585 | EXPORT_SYMBOL_GPL(oslec_update); |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 586 | |
| 587 | /* This function is seperated from the echo canceller is it is usually called |
| 588 | as part of the tx process. See rx HP (DC blocking) filter above, it's |
| 589 | the same design. |
| 590 | |
| 591 | Some soft phones send speech signals with a lot of low frequency |
| 592 | energy, e.g. down to 20Hz. This can make the hybrid non-linear |
| 593 | which causes the echo canceller to fall over. This filter can help |
| 594 | by removing any low frequency before it gets to the tx port of the |
| 595 | hybrid. |
| 596 | |
| 597 | It can also help by removing and DC in the tx signal. DC is bad |
| 598 | for LMS algorithms. |
| 599 | |
| 600 | This is one of the classic DC removal filters, adjusted to provide sufficient |
| 601 | bass rolloff to meet the above requirement to protect hybrids from things that |
| 602 | upset them. The difference between successive samples produces a lousy HPF, and |
| 603 | then a suitably placed pole flattens things out. The final result is a nicely |
| 604 | rolled off bass end. The filtering is implemented with extended fractional |
| 605 | precision, which noise shapes things, giving very clean DC removal. |
| 606 | */ |
| 607 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 608 | int16_t oslec_hpf_tx(struct oslec_state * ec, int16_t tx) |
| 609 | { |
| 610 | int tmp, tmp1; |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 611 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 612 | if (ec->adaption_mode & ECHO_CAN_USE_TX_HPF) { |
| 613 | tmp = tx << 15; |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 614 | #if 1 |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 615 | /* Make sure the gain of the HPF is 1.0. The first can still saturate a little under |
| 616 | impulse conditions, and it might roll to 32768 and need clipping on sustained peak |
| 617 | level signals. However, the scale of such clipping is small, and the error due to |
| 618 | any saturation should not markedly affect the downstream processing. */ |
| 619 | tmp -= (tmp >> 4); |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 620 | #endif |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 621 | ec->tx_1 += -(ec->tx_1 >> DC_LOG2BETA) + tmp - ec->tx_2; |
| 622 | tmp1 = ec->tx_1 >> 15; |
| 623 | if (tmp1 > 32767) |
| 624 | tmp1 = 32767; |
| 625 | if (tmp1 < -32767) |
| 626 | tmp1 = -32767; |
| 627 | tx = tmp1; |
| 628 | ec->tx_2 = tmp; |
| 629 | } |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 630 | |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 631 | return tx; |
David Rowe | 10602db | 2008-10-06 21:41:46 -0700 | [diff] [blame] | 632 | } |
J.R. Mauro | 4460a86 | 2008-10-20 19:01:31 -0400 | [diff] [blame] | 633 | |
Tzafrir Cohen | 9d8f2d5 | 2008-10-12 07:17:26 +0200 | [diff] [blame] | 634 | EXPORT_SYMBOL_GPL(oslec_hpf_tx); |
Tzafrir Cohen | 68b8d9f | 2008-10-12 06:55:40 +0200 | [diff] [blame] | 635 | |
| 636 | MODULE_LICENSE("GPL"); |
| 637 | MODULE_AUTHOR("David Rowe"); |
| 638 | MODULE_DESCRIPTION("Open Source Line Echo Canceller"); |
| 639 | MODULE_VERSION("0.3.0"); |