drivers/misc/echo/echo.c

   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
 106 static 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
 135 static 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
 143 struct 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
 195 error_snap:
 196         fir16_free(&ec->fir_state_bg);
 197 error_state_bg:
 198         fir16_free(&ec->fir_state);
 199 error_state:
 200         kfree(ec->fir_taps16[1]);
 201 error_oom_1:
 202         kfree(ec->fir_taps16[0]);
 203 error_oom_0:
 204         kfree(ec);
 205         return NULL;
 206 }
 207 EXPORT_SYMBOL_GPL(oslec_create);
 208
 209 void 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 }
 220 EXPORT_SYMBOL_GPL(oslec_free);
 221
 222 void oslec_adaption_mode(struct oslec_state *ec, int adaption_mode)
 223 {
 224         ec->adaption_mode = adaption_mode;
 225 }
 226 EXPORT_SYMBOL_GPL(oslec_adaption_mode);
 227
 228 void 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 }
 252 EXPORT_SYMBOL_GPL(oslec_flush);
 253
 254 void oslec_snapshot(struct oslec_state *ec)
 255 {
 256         memcpy(ec->snapshot, ec->fir_taps16[0], ec->taps * sizeof(int16_t));
 257 }
 258 EXPORT_SYMBOL_GPL(oslec_snapshot);
 259
 260 /* Dual Path Echo Canceller */
 261
 262 int16_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 }
 530 EXPORT_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
 554 int16_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 }
 584 EXPORT_SYMBOL_GPL(oslec_hpf_tx);
 585
 586 MODULE_LICENSE("GPL");
 587 MODULE_AUTHOR("David Rowe");
 588 MODULE_DESCRIPTION("Open Source Line Echo Canceller");
 589 MODULE_VERSION("0.3.0");