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