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