diff --git a/.gitignore b/.gitignore index 71a498d..bb490dd 100644 --- a/.gitignore +++ b/.gitignore @@ -7,7 +7,6 @@ Makefile.in *.la *.pc aclocal.m4 -m4/*.m4 autom4te.cache config.h* config.sub @@ -41,4 +40,6 @@ tests/testsuite.log # vi/vim files *.sw? +src/libecho/libecho.a src/osmo-v5-le +tests/answer_detect diff --git a/Makefile.am b/Makefile.am index 5901112..900c8f7 100644 --- a/Makefile.am +++ b/Makefile.am @@ -1,7 +1,7 @@ ACLOCAL_AMFLAGS = -I m4 AM_CPPFLAGS = $(all_includes) -SUBDIRS = src +SUBDIRS = src tests dist-hook: echo $(VERSION) > $(distdir)/.tarball-version diff --git a/configure.ac b/configure.ac index e28939e..87ad0e5 100644 --- a/configure.ac +++ b/configure.ac @@ -102,9 +102,13 @@ then CPPFLAGS="$CPPFLAGS $WERROR_FLAGS" fi +AX_CHECK_X86_FEATURES() + AC_MSG_RESULT([CFLAGS="$CFLAGS"]) AC_MSG_RESULT([CPPFLAGS="$CPPFLAGS"]) AC_OUTPUT( + src/libecho/Makefile src/Makefile + tests/Makefile Makefile) diff --git a/m4/ax_check_x86_features.m4 b/m4/ax_check_x86_features.m4 new file mode 100644 index 0000000..8d132fd --- /dev/null +++ b/m4/ax_check_x86_features.m4 @@ -0,0 +1,13 @@ +AC_DEFUN([AX_CHECK_X86_FEATURES], + [m4_foreach_w( + [ax_x86_feature], + [mmx popcnt sse sse2 sse3 sse4.1 sse4.2 sse4a avx avx2 avx512f fma fma4 bmi bmi2], + [AX_GCC_X86_CPU_SUPPORTS(ax_x86_feature, + [X86_FEATURE_CFLAGS="$X86_FEATURE_CFLAGS -m[]ax_x86_feature"], + []) + ]) + AC_SUBST([X86_FEATURE_CFLAGS]) + m4_ifval([$1],[$1], + [CFLAGS="$CFLAGS $X86_FEATURE_CFLAGS"]) + $2 +]) diff --git a/m4/ax_gcc_x86_cpu_supports.m4 b/m4/ax_gcc_x86_cpu_supports.m4 new file mode 100644 index 0000000..c020ce3 --- /dev/null +++ b/m4/ax_gcc_x86_cpu_supports.m4 @@ -0,0 +1,44 @@ +AC_DEFUN_ONCE([_AX_GCC_X86_CPU_INIT], + [AC_LANG_PUSH([C]) + AC_CACHE_CHECK([for gcc __builtin_cpu_init function], + [ax_cv_gcc_check_x86_cpu_init], + [AC_RUN_IFELSE( + [AC_LANG_PROGRAM([#include ], + [__builtin_cpu_init ();]) + ], + [ax_cv_gcc_check_x86_cpu_init=yes], + [ax_cv_gcc_check_x86_cpu_init=no])]) + AC_LANG_POP([C]) + AS_IF([test "X$ax_cv_gcc_check_x86_cpu_init" = "Xno"], + [AC_MSG_ERROR([Need GCC to support X86 CPU features tests])]) +]) + +AC_DEFUN([AX_GCC_X86_CPU_SUPPORTS], + [AC_REQUIRE([AC_PROG_CC]) + AC_REQUIRE([_AX_GCC_X86_CPU_INIT]) + AC_LANG_PUSH([C]) + AS_VAR_PUSHDEF([gcc_x86_feature], [AS_TR_SH([ax_cv_gcc_x86_cpu_supports_$1])]) + AC_CACHE_CHECK([for x86 $1 instruction support], + [gcc_x86_feature], + [AC_RUN_IFELSE( + [AC_LANG_PROGRAM( [#include ], + [ __builtin_cpu_init (); + if (__builtin_cpu_supports("$1")) + return 0; + return 1; + ])], + [gcc_x86_feature=yes], + [gcc_x86_feature=no] + )] + ) + AC_LANG_POP([C]) + AS_VAR_IF([gcc_x86_feature],[yes], + [AC_DEFINE( + AS_TR_CPP([HAVE_$1_INSTRUCTIONS]), + [1], + [Define if $1 instructions are supported]) + $2], + [$3] + ) + AS_VAR_POPDEF([gcc_x86_feature]) +]) diff --git a/src/Makefile.am b/src/Makefile.am index 959878f..e87b94a 100644 --- a/src/Makefile.am +++ b/src/Makefile.am @@ -3,6 +3,8 @@ AM_CFLAGS= -Wall -Wextra -g $(LIBOSMOCORE_CFLAGS) $(LIBOSMOGSM_CFLAGS) $(LIBOSMO AM_LDFLAGS = $(COVERAGE_LDFLAGS) COMMONLIBS = $(LIBOSMOCORE_LIBS) $(LIBOSMOGSM_LIBS) $(LIBOSMOVTY_LIBS) $(LIBOSMOABIS_LIBS) $(LIBOSMOCTRL_LIBS) +SUBDIRS = libecho + bin_PROGRAMS = osmo-v5-le osmo_v5_le_SOURCES = logging.c \ diff --git a/src/libecho/Makefile.am b/src/libecho/Makefile.am new file mode 100644 index 0000000..c31fcd8 --- /dev/null +++ b/src/libecho/Makefile.am @@ -0,0 +1,7 @@ +AM_CPPFLAGS = $(all_includes) +AM_CFLAGS= -Wall -Wextra -g + +noinst_LIBRARIES = libecho.a + +libecho_a_SOURCES = echo.c \ + answertone.c diff --git a/src/libecho/answertone.c b/src/libecho/answertone.c new file mode 100644 index 0000000..95100ba --- /dev/null +++ b/src/libecho/answertone.c @@ -0,0 +1,267 @@ +/* An answer tone detection algrotihm using Goertzel to detect level and phase + * + * The answer tone is described in ITU V.8 and is also used to disable echo + * suppression and cancelation in the network. The detection uses phase to + * detect frequencies that are too far off, as well a phase reversal, to + * distinguish the answer tone from other continuous tones. + * + * (C) 2023 by Andreas Eversberg + * All Rights Reserved + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU Affero General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Affero General Public License for more details. + * + * You should have received a copy of the GNU Affero General Public License + * along with this program. If not, see . + * + */ + +#include +#include +#include +#include +#include +#include +#include "answertone.h" + +#define ANS_FREQUENCY 2100.0 +#define WINDOW_DURATION 0.010 /* 10 MS */ +#define DETECT_DURATION 0.400 /* < 425 MS */ +#define MIN_LEVEL 0.05 /* -26 dBm0 */ +#define MAX_NOISE 0.5 /* -6 dB */ +#define MAX_FREQUENCY 22 /* frequency error */ +#define MIN_REVERSAL 2.967 /* about 10 degrees off 180 (PI) */ + +// #define DEBUG +// #define HEAVY_DEBUG + +void goertzel_init(struct answer_tone *at, float frequency, float samplerate) +{ + float omega; + + omega = 2.0 * M_PI * frequency / samplerate; + at->sine = sin(omega); + at->cosine = cos(omega); + at->coeff = 2.0 * at->cosine; +} + +/* with rectangular window, the frequency response is: + * if it matches the filter frequency: 0 dB + * if it is 0.5/duration off the filter frequency: about -4 dB + * if it is 1/duration off the filter frequency; -INF dB + * if it is 1.5/duration off the filter frequency: about -13.5 dB + */ +void goertzel_calculate(struct answer_tone *at, float *data, int length, float *level_p, float *magnitude_p, float *phase_p) +{ + float q0 = 0, q1 = 0, q2 = 0; + float real, imag; + float scaling = (float)length / 2.0; + float upper = 0, lower = 0; + int i; + + upper = lower = *data; + + for (i = 0; i < length; i++) { + if (*data > upper) + upper = *data; + else if (*data < lower) + lower = *data; + q0 = at->coeff * q1 - q2 + *data++; + q2 = q1; + q1 = q0; + } + + real = (q1 * at->cosine - q2) / scaling; + imag = (q1 * at->sine) / scaling; + + *level_p = (upper - lower) / 2.0; + *magnitude_p = sqrtf(real*real + imag*imag); + *phase_p = atan2(imag, real); +} + +int answertone_init(struct answer_tone *at, int samplerate) +{ + memset(at, 0, sizeof(*at)); + + /* use 10 ms as window size */ + at->buffer_size = samplerate * WINDOW_DURATION; + at->buffer = calloc(at->buffer_size, sizeof(*at->buffer)); + if (!at->buffer) + return -ENOMEM; + + goertzel_init(at, ANS_FREQUENCY, samplerate); + + return 0; +} + +void answertone_reset(struct answer_tone *at) +{ + at->state = AT_STATE_NONE; +} + +void answertone_exit(struct answer_tone *at) +{ + free(at->buffer); +} + +enum at_fail answertone_check(struct answer_tone *at, float *phase_p, float *frequency_p) +{ + float level, magnitude, last_phase, shift; + + goertzel_calculate(at, at->buffer, at->buffer_size, &level, &magnitude, phase_p); + last_phase = at->last_phase; + at->last_phase = *phase_p; + shift = (*phase_p - last_phase); + if (shift > M_PI) + shift -= M_PI * 2.0; + else if (shift < -M_PI) + shift += M_PI * 2.0; + *frequency_p = shift / M_PI / 2.0 / WINDOW_DURATION; + +#if HEAVY_DEBUG + printf("level=%6.3f magnitude=%6.3f snr=%6.3f phase=%9.3f frequency=%8.3f\n", + level, magnitude, magnitude/level, *phase_p / M_PI * 180, *frequency_p); +#endif + + /* fails due to low level */ + if (magnitude < MIN_LEVEL) + return AT_FAIL_MIN_LEVEL; + + /* fails due to bad SNR */ + if (1.0 - magnitude/level > MAX_NOISE) + return AT_FAIL_MAX_NOISE; + + /* fails due to wrong frequency */ + if (fabsf(*frequency_p) > MAX_FREQUENCY) + return AT_FAIL_MAX_FREQUENCY; + + return AT_FAIL_NONE; +} + +#if DEBUG +static const char *answertone_cause[] = { + "ok", + "level too low", + "too noisy", + "wrong frequency", +}; +#endif + +static enum at_state answertone_chunk(struct answer_tone *at) +{ + float phase, frequency, shift; + enum at_fail fail; + int rc = 0; + + fail = answertone_check(at, &phase, &frequency); + + switch (at->state) { + case AT_STATE_NONE: + if (fail == AT_FAIL_NONE) { +#if DEBUG + printf("DETECTED TONE\n"); +#endif + at->state = AT_STATE_DETECT; + at->tone_duration = WINDOW_DURATION; + /* count frequency values */ + at->frequency_sum = 0.0; + at->count = 0; + } + break; + case AT_STATE_DETECT: + if (fail != AT_FAIL_NONE) { +#if DEBUG + printf("LOST TONE AFTER %.3f SECONDS (because %s)\n", at->tone_duration, cause[fail]); +#endif + at->state = AT_STATE_NONE; + break; + } + at->frequency_sum += frequency; + at->count++; + at->tone_duration += WINDOW_DURATION; + at->last2_valid_phase = at->last1_valid_phase; + at->last1_valid_phase = phase; + if (at->tone_duration >= DETECT_DURATION) { +#if DEBUG + printf("LONG TONE VALID WITH FREQUENCY OFFSET %.3f\n", at->frequency_sum / at->count); +#endif + at->state = AT_STATE_TONE; + } + break; + case AT_STATE_TONE: + if (fail != AT_FAIL_NONE) { +#if DEBUG + printf("LONG TONE CHANGED, CHECK PHASE REVERSAL\n"); +#endif + at->last_valid_frequency = at->frequency_sum / at->count; + at->state = AT_STATE_PAUSE; + /* count pause chunks */ + at->count = 1; + break; + } + at->frequency_sum += frequency; + at->count++; + at->tone_duration += WINDOW_DURATION; + at->last2_valid_phase = at->last1_valid_phase; + at->last1_valid_phase = phase; + break; + case AT_STATE_PAUSE: + /* three pause chunks: two may be invalid due to phase reversal, one extra to get valid frequency */ + if (at->count++ < 3) + break; + /* phase change over 5 chunks */ + shift = (phase - at->last2_valid_phase); + if (shift > M_PI) + shift -= M_PI * 2.0; + else if (shift < -M_PI) + shift += M_PI * 2.0; + /* substract phase change to be expected over 5 chunk */ + shift -= at->last_valid_frequency * WINDOW_DURATION * 5.0 * M_PI * 2.0; + if (shift > M_PI) + shift -= M_PI * 2.0; + else if (shift < -M_PI) + shift += M_PI * 2.0; + /* if there is change of less than about 10 degrees off 180 */ + if (fabsf(shift) > MIN_REVERSAL) { +#if DEBUG + printf("PHASE REVERSAL VALID (phase shift %.0f deg)\n", shift / M_PI * 180.0); +#endif + at->frequency_sum = 0.0; + at->count = 0; + at->state = AT_STATE_DETECT; + rc = 1; + } else { +#if DEBUG + printf("LOST TONE, NO VALID PHASE REVERSAL\n"); +#endif + at->state = AT_STATE_NONE; + } + break; + } + + return rc; +} + +/* convert stream of int16_t (ISDN) stream into chunks of float (dBm0); return 1, if there was a valid answer tone */ +int answertone_process(struct answer_tone *at, int16_t *data, int length) +{ + int rc = 0; + + while (length--) { + /* convert ISDN data to float with dBm level (1 dBm = -3 dB below full int16_t scale) */ + at->buffer[at->buffer_pos++] = (float)(*data++) / 23170.0; + if (at->buffer_pos == at->buffer_size) { + rc |= answertone_chunk(at); + at->buffer_pos = 0; + } + } + + return rc; +} diff --git a/src/libecho/answertone.h b/src/libecho/answertone.h new file mode 100644 index 0000000..d20fc8d --- /dev/null +++ b/src/libecho/answertone.h @@ -0,0 +1,37 @@ + +enum at_fail { + AT_FAIL_NONE = 0, + AT_FAIL_MIN_LEVEL, + AT_FAIL_MAX_NOISE, + AT_FAIL_MAX_FREQUENCY, +}; + +enum at_state { + AT_STATE_NONE = 0, + AT_STATE_DETECT, + AT_STATE_TONE, + AT_STATE_PAUSE, +}; + +struct answer_tone { + float sine; + float cosine; + float coeff; + float *buffer; + int buffer_size; + int buffer_pos; + float last_phase; + enum at_state state; + float tone_duration; + float frequency_sum; + int count; + float last1_valid_phase; + float last2_valid_phase; + float last_valid_frequency; +}; + +int answertone_init(struct answer_tone *at, int samplerate); +void answertone_reset(struct answer_tone *at); +void answertone_exit(struct answer_tone *at); +enum at_fail answertone_check(struct answer_tone *at, float *phase_p, float *frequency_p); +int answertone_process(struct answer_tone *at, int16_t *data, int length); diff --git a/src/libecho/bit_operations.h b/src/libecho/bit_operations.h new file mode 100644 index 0000000..b522635 --- /dev/null +++ b/src/libecho/bit_operations.h @@ -0,0 +1,253 @@ +/* + * SpanDSP - a series of DSP components for telephony + * + * bit_operations.h - Various bit level operations, such as bit reversal + * + * Written by Steve Underwood + * + * Copyright (C) 2006 Steve Underwood + * + * All rights reserved. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2, as + * published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + * + * $Id: bit_operations.h,v 1.11 2006/11/28 15:37:03 steveu Exp $ + */ + +/*! \file */ + +#if !defined(_BIT_OPERATIONS_H_) +#define _BIT_OPERATIONS_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#if defined(__i386__) || defined(__x86_64__) +/*! \brief Find the bit position of the highest set bit in a word + \param bits The word to be searched + \return The bit number of the highest set bit, or -1 if the word is zero. */ +static __inline__ int top_bit(unsigned int bits) +{ + int res; + + __asm__ (" xorl %[res],%[res];\n" + " decl %[res];\n" + " bsrl %[bits],%[res]\n" + : [res] "=&r" (res) + : [bits] "rm" (bits)); + return res; +} +/*- End of function --------------------------------------------------------*/ + +/*! \brief Find the bit position of the lowest set bit in a word + \param bits The word to be searched + \return The bit number of the lowest set bit, or -1 if the word is zero. */ +static __inline__ int bottom_bit(unsigned int bits) +{ + int res; + + __asm__ (" xorl %[res],%[res];\n" + " decl %[res];\n" + " bsfl %[bits],%[res]\n" + : [res] "=&r" (res) + : [bits] "rm" (bits)); + return res; +} +/*- End of function --------------------------------------------------------*/ +#else +static __inline__ int top_bit(unsigned int bits) +{ + int i; + + if (bits == 0) + return -1; + i = 0; + if (bits & 0xFFFF0000) + { + bits &= 0xFFFF0000; + i += 16; + } + if (bits & 0xFF00FF00) + { + bits &= 0xFF00FF00; + i += 8; + } + if (bits & 0xF0F0F0F0) + { + bits &= 0xF0F0F0F0; + i += 4; + } + if (bits & 0xCCCCCCCC) + { + bits &= 0xCCCCCCCC; + i += 2; + } + if (bits & 0xAAAAAAAA) + { + bits &= 0xAAAAAAAA; + i += 1; + } + return i; +} +/*- End of function --------------------------------------------------------*/ + +static __inline__ int bottom_bit(unsigned int bits) +{ + int i; + + if (bits == 0) + return -1; + i = 32; + if (bits & 0x0000FFFF) + { + bits &= 0x0000FFFF; + i -= 16; + } + if (bits & 0x00FF00FF) + { + bits &= 0x00FF00FF; + i -= 8; + } + if (bits & 0x0F0F0F0F) + { + bits &= 0x0F0F0F0F; + i -= 4; + } + if (bits & 0x33333333) + { + bits &= 0x33333333; + i -= 2; + } + if (bits & 0x55555555) + { + bits &= 0x55555555; + i -= 1; + } + return i; +} +/*- End of function --------------------------------------------------------*/ +#endif + +/*! \brief Bit reverse a byte. + \param data The byte to be reversed. + \return The bit reversed version of data. */ +static __inline__ uint8_t bit_reverse8(uint8_t x) +{ +#if defined(__i386__) || defined(__x86_64__) + /* If multiply is fast */ + return ((x*0x0802U & 0x22110U) | (x*0x8020U & 0x88440U))*0x10101U >> 16; +#else + /* If multiply is slow, but we have a barrel shifter */ + x = (x >> 4) | (x << 4); + x = ((x & 0xCC) >> 2) | ((x & 0x33) << 2); + return ((x & 0xAA) >> 1) | ((x & 0x55) << 1); +#endif +} +/*- End of function --------------------------------------------------------*/ + +/*! \brief Bit reverse a 16 bit word. + \param data The word to be reversed. + \return The bit reversed version of data. */ +uint16_t bit_reverse16(uint16_t data); + +/*! \brief Bit reverse a 32 bit word. + \param data The word to be reversed. + \return The bit reversed version of data. */ +uint32_t bit_reverse32(uint32_t data); + +/*! \brief Bit reverse each of the four bytes in a 32 bit word. + \param data The word to be reversed. + \return The bit reversed version of data. */ +uint32_t bit_reverse_4bytes(uint32_t data); + +/*! \brief Find the number of set bits in a 32 bit word. + \param x The word to be searched. + \return The number of set bits. */ +int one_bits32(uint32_t x); + +/*! \brief Create a mask as wide as the number in a 32 bit word. + \param x The word to be searched. + \return The mask. */ +uint32_t make_mask32(uint32_t x); + +/*! \brief Create a mask as wide as the number in a 16 bit word. + \param x The word to be searched. + \return The mask. */ +uint16_t make_mask16(uint16_t x); + +/*! \brief Find the least significant one in a word, and return a word + with just that bit set. + \param x The word to be searched. + \return The word with the single set bit. */ +static __inline__ uint32_t least_significant_one32(uint32_t x) +{ + return (x & (-(int32_t) x)); +} +/*- End of function --------------------------------------------------------*/ + +/*! \brief Find the most significant one in a word, and return a word + with just that bit set. + \param x The word to be searched. + \return The word with the single set bit. */ +static __inline__ uint32_t most_significant_one32(uint32_t x) +{ +#if defined(__i386__) || defined(__x86_64__) + return 1 << top_bit(x); +#else + x = make_mask32(x); + return (x ^ (x >> 1)); +#endif +} +/*- End of function --------------------------------------------------------*/ + +/*! \brief Find the parity of a byte. + \param x The byte to be checked. + \return 1 for odd, or 0 for even. */ +static __inline__ int parity8(uint8_t x) +{ + x = (x ^ (x >> 4)) & 0x0F; + return (0x6996 >> x) & 1; +} +/*- End of function --------------------------------------------------------*/ + +/*! \brief Find the parity of a 16 bit word. + \param x The word to be checked. + \return 1 for odd, or 0 for even. */ +static __inline__ int parity16(uint16_t x) +{ + x ^= (x >> 8); + x = (x ^ (x >> 4)) & 0x0F; + return (0x6996 >> x) & 1; +} +/*- End of function --------------------------------------------------------*/ + +/*! \brief Find the parity of a 32 bit word. + \param x The word to be checked. + \return 1 for odd, or 0 for even. */ +static __inline__ int parity32(uint32_t x) +{ + x ^= (x >> 16); + x ^= (x >> 8); + x = (x ^ (x >> 4)) & 0x0F; + return (0x6996 >> x) & 1; +} +/*- End of function --------------------------------------------------------*/ + +#ifdef __cplusplus +} +#endif + +#endif +/*- End of file ------------------------------------------------------------*/ diff --git a/src/libecho/echo.c b/src/libecho/echo.c new file mode 100644 index 0000000..830b34c --- /dev/null +++ b/src/libecho/echo.c @@ -0,0 +1,658 @@ +/* + * SpanDSP - a series of DSP components for telephony + * + * echo.c - A line echo canceller. This code is being developed + * against and partially complies with G168. + * + * Written by Steve Underwood + * and David Rowe + * + * Copyright (C) 2001, 2003 Steve Underwood, 2007 David Rowe + * + * Based on a bit from here, a bit from there, eye of toad, ear of + * bat, 15 years of failed attempts by David and a few fried brain + * cells. + * + * All rights reserved. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2, as + * published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + * + * $Id: echo.c,v 1.20 2006/12/01 18:00:48 steveu Exp $ + */ + +/*! \file */ + +/* Implementation Notes + David Rowe + April 2007 + + This code started life as Steve's NLMS algorithm with a tap + rotation algorithm to handle divergence during double talk. I + added a Geigel Double Talk Detector (DTD) [2] and performed some + G168 tests. However I had trouble meeting the G168 requirements, + especially for double talk - there were always cases where my DTD + failed, for example where near end speech was under the 6dB + threshold required for declaring double talk. + + So I tried a two path algorithm [1], which has so far given better + results. The original tap rotation/Geigel algorithm is available + in SVN http://svn.rowetel.com/software/oslec/tags/before_16bit. + It's probably possible to make it work if some one wants to put some + serious work into it. + + At present no special treatment is provided for tones, which + generally cause NLMS algorithms to diverge. Initial runs of a + subset of the G168 tests for tones (e.g ./echo_test 6) show the + current algorithm is passing OK, which is kind of surprising. The + full set of tests needs to be performed to confirm this result. + + One other interesting change is that I have managed to get the NLMS + code to work with 16 bit coefficients, rather than the original 32 + bit coefficents. This reduces the MIPs and storage required. + I evaulated the 16 bit port using g168_tests.sh and listening tests + on 4 real-world samples. + + I also attempted the implementation of a block based NLMS update + [2] but although this passes g168_tests.sh it didn't converge well + on the real-world samples. I have no idea why, perhaps a scaling + problem. The block based code is also available in SVN + http://svn.rowetel.com/software/oslec/tags/before_16bit. If this + code can be debugged, it will lead to further reduction in MIPS, as + the block update code maps nicely onto DSP instruction sets (it's a + dot product) compared to the current sample-by-sample update. + + Steve also has some nice notes on echo cancellers in echo.h + + + References: + + [1] Ochiai, Areseki, and Ogihara, "Echo Canceller with Two Echo + Path Models", IEEE Transactions on communications, COM-25, + No. 6, June + 1977. + http://www.rowetel.com/images/echo/dual_path_paper.pdf + + [2] The classic, very useful paper that tells you how to + actually build a real world echo canceller: + Messerschmitt, Hedberg, Cole, Haoui, Winship, "Digital Voice + Echo Canceller with a TMS320020, + http://www.rowetel.com/images/echo/spra129.pdf + + [3] I have written a series of blog posts on this work, here is + Part 1: http://www.rowetel.com/blog/?p=18 + + [4] The source code http://svn.rowetel.com/software/oslec/ + + [5] A nice reference on LMS filters: + http://en.wikipedia.org/wiki/Least_mean_squares_filter + + Credits: + + Thanks to Steve Underwood, Jean-Marc Valin, and Ramakrishnan + Muthukrishnan for their suggestions and email discussions. Thanks + also to those people who collected echo samples for me such as + Mark, Pawel, and Pavel. +*/ + +#ifdef HAVE_CONFIG_H +#include +#endif +#ifdef __KERNEL__ +#include /* We're doing kernel work */ +#include +#include +#include +#define malloc(a) kmalloc((a), GFP_KERNEL) +#define free(a) kfree(a) +#else +#include +#include +#include +#include + +#endif + +#include "bit_operations.h" +#include "echo.h" + +#if !defined(NULL) +#define NULL (void *) 0 +#endif +#if !defined(FALSE) +#define FALSE 0 +#endif +#if !defined(TRUE) +#define TRUE (!FALSE) +#endif + +#define MIN_TX_POWER_FOR_ADAPTION 64 +#define MIN_RX_POWER_FOR_ADAPTION 64 +#define DTD_HANGOVER 600 /* 600 samples, or 75ms */ +#define DC_LOG2BETA 3 /* log2() of DC filter Beta */ + +/*-----------------------------------------------------------------------*\ + + FUNCTIONS + +\*-----------------------------------------------------------------------*/ + +/* adapting coeffs using the traditional stochastic descent (N)LMS algorithm */ + + +#ifdef __BLACKFIN_ASM__ +static void __inline__ lms_adapt_bg(echo_can_state_t *ec, int clean, int shift) +{ + int i, j; + int offset1; + int offset2; + int factor; + int exp; + int16_t *phist; + int n; + + if (shift > 0) + factor = clean << shift; + else + factor = clean >> -shift; + + /* Update the FIR taps */ + + offset2 = ec->curr_pos; + offset1 = ec->taps - offset2; + phist = &ec->fir_state_bg.history[offset2]; + + /* st: and en: help us locate the assembler in echo.s */ + + //asm("st:"); + n = ec->taps; + for (i = 0, j = offset2; i < n; i++, j++) + { + exp = *phist++ * factor; + ec->fir_taps16[1][i] += (int16_t) ((exp+(1<<14)) >> 15); + } + //asm("en:"); + + /* Note the asm for the inner loop above generated by Blackfin gcc + 4.1.1 is pretty good (note even parallel instructions used): + + R0 = W [P0++] (X); + R0 *= R2; + R0 = R0 + R3 (NS) || + R1 = W [P1] (X) || + nop; + R0 >>>= 15; + R0 = R0 + R1; + W [P1++] = R0; + + A block based update algorithm would be much faster but the + above can't be improved on much. Every instruction saved in + the loop above is 2 MIPs/ch! The for loop above is where the + Blackfin spends most of it's time - about 17 MIPs/ch measured + with speedtest.c with 256 taps (32ms). Write-back and + Write-through cache gave about the same performance. + */ +} + +/* + IDEAS for further optimisation of lms_adapt_bg(): + + 1/ The rounding is quite costly. Could we keep as 32 bit coeffs + then make filter pluck the MS 16-bits of the coeffs when filtering? + However this would lower potential optimisation of filter, as I + think the dual-MAC architecture requires packed 16 bit coeffs. + + 2/ Block based update would be more efficient, as per comments above, + could use dual MAC architecture. + + 3/ Look for same sample Blackfin LMS code, see if we can get dual-MAC + packing. + + 4/ Execute the whole e/c in a block of say 20ms rather than sample + by sample. Processing a few samples every ms is inefficient. +*/ + +#else +static __inline__ void lms_adapt_bg(echo_can_state_t *ec, int clean, int shift) +{ + int i; + + int offset1; + int offset2; + int factor; + int exp; + + if (shift > 0) + factor = clean << shift; + else + factor = clean >> -shift; + + /* Update the FIR taps */ + + offset2 = ec->curr_pos; + offset1 = ec->taps - offset2; + + for (i = ec->taps - 1; i >= offset1; i--) + { + exp = (ec->fir_state_bg.history[i - offset1]*factor); + ec->fir_taps16[1][i] += (int16_t) ((exp+(1<<14)) >> 15); + } + for ( ; i >= 0; i--) + { + exp = (ec->fir_state_bg.history[i + offset2]*factor); + ec->fir_taps16[1][i] += (int16_t) ((exp+(1<<14)) >> 15); + } +} +#endif + +/*- End of function --------------------------------------------------------*/ + +echo_can_state_t *echo_can_create(int len, int adaption_mode) +{ + echo_can_state_t *ec; + int i; + int j; + + ec = (echo_can_state_t *) malloc(sizeof(*ec)); + if (ec == NULL) + return NULL; + memset(ec, 0, sizeof(*ec)); + + ec->taps = len; + ec->log2taps = top_bit(len); + ec->curr_pos = ec->taps - 1; + + for (i = 0; i < 2; i++) + { + if ((ec->fir_taps16[i] = (int16_t *) malloc((ec->taps)*sizeof(int16_t))) == NULL) + { + for (j = 0; j < i; j++) + free(ec->fir_taps16[j]); + free(ec); + return NULL; + } + memset(ec->fir_taps16[i], 0, (ec->taps)*sizeof(int16_t)); + } + + fir16_create(&ec->fir_state, + ec->fir_taps16[0], + ec->taps); + fir16_create(&ec->fir_state_bg, + ec->fir_taps16[1], + ec->taps); + + for(i=0; i<5; i++) { + ec->xvtx[i] = ec->yvtx[i] = ec->xvrx[i] = ec->yvrx[i] = 0; + } + + ec->cng_level = 1000; + echo_can_adaption_mode(ec, adaption_mode); + + ec->snapshot = (int16_t*)malloc(ec->taps*sizeof(int16_t)); + memset(ec->snapshot, 0, sizeof(int16_t)*ec->taps); + + ec->cond_met = 0; + ec->Pstates = 0; + ec->Ltxacc = ec->Lrxacc = ec->Lcleanacc = ec->Lclean_bgacc = 0; + ec->Ltx = ec->Lrx = ec->Lclean = ec->Lclean_bg = 0; + ec->tx_1 = ec->tx_2 = ec->rx_1 = ec->rx_2 = 0; + ec->Lbgn = ec->Lbgn_acc = 0; + ec->Lbgn_upper = 200; + ec->Lbgn_upper_acc = ec->Lbgn_upper << 13; + + return ec; +} +/*- End of function --------------------------------------------------------*/ + +void echo_can_free(echo_can_state_t *ec) +{ + int i; + + fir16_free(&ec->fir_state); + fir16_free(&ec->fir_state_bg); + for (i = 0; i < 2; i++) + free(ec->fir_taps16[i]); + free(ec->snapshot); + free(ec); +} +/*- End of function --------------------------------------------------------*/ + +void echo_can_adaption_mode(echo_can_state_t *ec, int adaption_mode) +{ + ec->adaption_mode = adaption_mode; +} +/*- End of function --------------------------------------------------------*/ + +void echo_can_flush(echo_can_state_t *ec) +{ + int i; + + ec->Ltxacc = ec->Lrxacc = ec->Lcleanacc = ec->Lclean_bgacc = 0; + ec->Ltx = ec->Lrx = ec->Lclean = ec->Lclean_bg = 0; + ec->tx_1 = ec->tx_2 = ec->rx_1 = ec->rx_2 = 0; + + ec->Lbgn = ec->Lbgn_acc = 0; + ec->Lbgn_upper = 200; + ec->Lbgn_upper_acc = ec->Lbgn_upper << 13; + + ec->nonupdate_dwell = 0; + + fir16_flush(&ec->fir_state); + fir16_flush(&ec->fir_state_bg); + ec->fir_state.curr_pos = ec->taps - 1; + ec->fir_state_bg.curr_pos = ec->taps - 1; + for (i = 0; i < 2; i++) + memset(ec->fir_taps16[i], 0, ec->taps*sizeof(int16_t)); + + ec->curr_pos = ec->taps - 1; + ec->Pstates = 0; +} +/*- End of function --------------------------------------------------------*/ + +void echo_can_snapshot(echo_can_state_t *ec) { + memcpy(ec->snapshot, ec->fir_taps16[0], ec->taps*sizeof(int16_t)); +} +/*- End of function --------------------------------------------------------*/ + +/* Dual Path Echo Canceller ------------------------------------------------*/ + +int16_t echo_can_update(echo_can_state_t *ec, int16_t tx, int16_t rx) +{ + int32_t echo_value; + int clean_bg; + int tmp, tmp1; + + /* Input scaling was found be required to prevent problems when tx + starts clipping. Another possible way to handle this would be the + filter coefficent scaling. */ + + ec->tx = tx; ec->rx = rx; + tx >>=1; + rx >>=1; + + /* + Filter DC, 3dB point is 160Hz (I think), note 32 bit precision required + otherwise values do not track down to 0. Zero at DC, Pole at (1-Beta) + only real axis. Some chip sets (like Si labs) don't need + this, but something like a $10 X100P card does. Any DC really slows + down convergence. + + Note: removes some low frequency from the signal, this reduces + the speech quality when listening to samples through headphones + but may not be obvious through a telephone handset. + + Note that the 3dB frequency in radians is approx Beta, e.g. for + Beta = 2^(-3) = 0.125, 3dB freq is 0.125 rads = 159Hz. + */ + + if (ec->adaption_mode & ECHO_CAN_USE_RX_HPF) { + tmp = rx << 15; +#if 1 + /* Make sure the gain of the HPF is 1.0. This can still saturate a little under + impulse conditions, and it might roll to 32768 and need clipping on sustained peak + level signals. However, the scale of such clipping is small, and the error due to + any saturation should not markedly affect the downstream processing. */ + tmp -= (tmp >> 4); +#endif + ec->rx_1 += -(ec->rx_1>>DC_LOG2BETA) + tmp - ec->rx_2; + + /* hard limit filter to prevent clipping. Note that at this stage + rx should be limited to +/- 16383 due to right shift above */ + tmp1 = ec->rx_1 >> 15; + if (tmp1 > 16383) tmp1 = 16383; + if (tmp1 < -16383) tmp1 = -16383; + rx = tmp1; + ec->rx_2 = tmp; + } + + /* Block average of power in the filter states. Used for + adaption power calculation. */ + + { + int new, old; + + /* efficient "out with the old and in with the new" algorithm so + we don't have to recalculate over the whole block of + samples. */ + new = (int)tx * (int)tx; + old = (int)ec->fir_state.history[ec->fir_state.curr_pos] * + (int)ec->fir_state.history[ec->fir_state.curr_pos]; + ec->Pstates += ((new - old) + (1<<(ec->log2taps-1))) >> ec->log2taps; + if (ec->Pstates < 0) ec->Pstates = 0; + } + + /* Calculate short term average levels using simple single pole IIRs */ + + ec->Ltxacc += abs(tx) - ec->Ltx; + ec->Ltx = (ec->Ltxacc + (1<<4)) >> 5; + ec->Lrxacc += abs(rx) - ec->Lrx; + ec->Lrx = (ec->Lrxacc + (1<<4)) >> 5; + + /* Foreground filter ---------------------------------------------------*/ + + ec->fir_state.coeffs = ec->fir_taps16[0]; + echo_value = fir16(&ec->fir_state, tx); + ec->clean = rx - echo_value; + ec->Lcleanacc += abs(ec->clean) - ec->Lclean; + ec->Lclean = (ec->Lcleanacc + (1<<4)) >> 5; + + /* Background filter ---------------------------------------------------*/ + + echo_value = fir16(&ec->fir_state_bg, tx); + clean_bg = rx - echo_value; + ec->Lclean_bgacc += abs(clean_bg) - ec->Lclean_bg; + ec->Lclean_bg = (ec->Lclean_bgacc + (1<<4)) >> 5; + + /* Background Filter adaption -----------------------------------------*/ + + /* Almost always adap bg filter, just simple DT and energy + detection to minimise adaption in cases of strong double talk. + However this is not critical for the dual path algorithm. + */ + ec->factor = 0; + ec->shift = 0; + if ((ec->nonupdate_dwell == 0)) { + int P, logP, shift; + + /* Determine: + + f = Beta * clean_bg_rx/P ------ (1) + + where P is the total power in the filter states. + + The Boffins have shown that if we obey (1) we converge + quickly and avoid instability. + + The correct factor f must be in Q30, as this is the fixed + point format required by the lms_adapt_bg() function, + therefore the scaled version of (1) is: + + (2^30) * f = (2^30) * Beta * clean_bg_rx/P + factor = (2^30) * Beta * clean_bg_rx/P ----- (2) + + We have chosen Beta = 0.25 by experiment, so: + + factor = (2^30) * (2^-2) * clean_bg_rx/P + + (30 - 2 - log2(P)) + factor = clean_bg_rx 2 ----- (3) + + To avoid a divide we approximate log2(P) as top_bit(P), + which returns the position of the highest non-zero bit in + P. This approximation introduces an error as large as a + factor of 2, but the algorithm seems to handle it OK. + + Come to think of it a divide may not be a big deal on a + modern DSP, so its probably worth checking out the cycles + for a divide versus a top_bit() implementation. + */ + + P = MIN_TX_POWER_FOR_ADAPTION + ec->Pstates; + logP = top_bit(P) + ec->log2taps; + shift = 30 - 2 - logP; + ec->shift = shift; + + lms_adapt_bg(ec, clean_bg, shift); + } + + /* very simple DTD to make sure we dont try and adapt with strong + near end speech */ + + ec->adapt = 0; + if ((ec->Lrx > MIN_RX_POWER_FOR_ADAPTION) && (ec->Lrx > ec->Ltx)) + ec->nonupdate_dwell = DTD_HANGOVER; + if (ec->nonupdate_dwell) + ec->nonupdate_dwell--; + + /* Transfer logic ------------------------------------------------------*/ + + /* These conditions are from the dual path paper [1], I messed with + them a bit to improve performance. */ + + if ((ec->adaption_mode & ECHO_CAN_USE_ADAPTION) && + (ec->nonupdate_dwell == 0) && + (8*ec->Lclean_bg < 7*ec->Lclean) /* (ec->Lclean_bg < 0.875*ec->Lclean) */ && + (8*ec->Lclean_bg < ec->Ltx) /* (ec->Lclean_bg < 0.125*ec->Ltx) */ ) + { + if (ec->cond_met == 6) { + /* BG filter has had better results for 6 consecutive samples */ + ec->adapt = 1; + memcpy(ec->fir_taps16[0], ec->fir_taps16[1], ec->taps*sizeof(int16_t)); + } + else + ec->cond_met++; + } + else + ec->cond_met = 0; + + /* Non-Linear Processing ---------------------------------------------------*/ + + ec->clean_nlp = ec->clean; + if (ec->adaption_mode & ECHO_CAN_USE_NLP) + { + /* Non-linear processor - a fancy way to say "zap small signals, to avoid + residual echo due to (uLaw/ALaw) non-linearity in the channel.". */ + + if ((16*ec->Lclean < ec->Ltx)) + { + /* Our e/c has improved echo by at least 24 dB (each factor of 2 is 6dB, + so 2*2*2*2=16 is the same as 6+6+6+6=24dB) */ + if (ec->adaption_mode & ECHO_CAN_USE_CNG) + { + ec->cng_level = ec->Lbgn; + + /* Very elementary comfort noise generation. Just random + numbers rolled off very vaguely Hoth-like. DR: This + noise doesn't sound quite right to me - I suspect there + are some overlfow issues in the filtering as it's too + "crackly". TODO: debug this, maybe just play noise at + high level or look at spectrum. + */ + + ec->cng_rndnum = 1664525U*ec->cng_rndnum + 1013904223U; + ec->cng_filter = ((ec->cng_rndnum & 0xFFFF) - 32768 + 5*ec->cng_filter) >> 3; + ec->clean_nlp = (ec->cng_filter*ec->cng_level*8) >> 14; + + } + else if (ec->adaption_mode & ECHO_CAN_USE_CLIP) + { + /* This sounds much better than CNG */ + if (ec->clean_nlp > ec->Lbgn) + ec->clean_nlp = ec->Lbgn; + if (ec->clean_nlp < -ec->Lbgn) + ec->clean_nlp = -ec->Lbgn; + } + else + { + /* just mute the residual, doesn't sound very good, used mainly + in G168 tests */ + ec->clean_nlp = 0; + } + } + else { + /* Background noise estimator. I tried a few algorithms + here without much luck. This very simple one seems to + work best, we just average the level using a slow (1 sec + time const) filter if the current level is less than a + (experimentally derived) constant. This means we dont + include high level signals like near end speech. When + combined with CNG or especially CLIP seems to work OK. + */ + if (ec->Lclean < 40) { + ec->Lbgn_acc += abs(ec->clean) - ec->Lbgn; + ec->Lbgn = (ec->Lbgn_acc + (1<<11)) >> 12; + } + } + } + + /* Roll around the taps buffer */ + if (ec->curr_pos <= 0) + ec->curr_pos = ec->taps; + ec->curr_pos--; + + if (ec->adaption_mode & ECHO_CAN_DISABLE) + ec->clean_nlp = rx; + + /* Output scaled back up again to match input scaling */ + + return (int16_t) ec->clean_nlp << 1; +} + +/*- End of function --------------------------------------------------------*/ + +/* This function is seperated from the echo canceller is it is usually called + as part of the tx process. See rx HP (DC blocking) filter above, it's + the same design. + + Some soft phones send speech signals with a lot of low frequency + energy, e.g. down to 20Hz. This can make the hybrid non-linear + which causes the echo canceller to fall over. This filter can help + by removing any low frequency before it gets to the tx port of the + hybrid. + + It can also help by removing and DC in the tx signal. DC is bad + for LMS algorithms. + + This is one of the classic DC removal filters, adjusted to provide sufficient + bass rolloff to meet the above requirement to protect hybrids from things that + upset them. The difference between successive samples produces a lousy HPF, and + then a suitably placed pole flattens things out. The final result is a nicely + rolled off bass end. The filtering is implemented with extended fractional + precision, which noise shapes things, giving very clean DC removal. +*/ + +int16_t echo_can_hpf_tx(echo_can_state_t *ec, int16_t tx) { + int tmp, tmp1; + + if (ec->adaption_mode & ECHO_CAN_USE_TX_HPF) { + tmp = tx << 15; +#if 1 + /* Make sure the gain of the HPF is 1.0. The first can still saturate a little under + impulse conditions, and it might roll to 32768 and need clipping on sustained peak + level signals. However, the scale of such clipping is small, and the error due to + any saturation should not markedly affect the downstream processing. */ + tmp -= (tmp >> 4); +#endif + ec->tx_1 += -(ec->tx_1>>DC_LOG2BETA) + tmp - ec->tx_2; + tmp1 = ec->tx_1 >> 15; + if (tmp1 > 32767) tmp1 = 32767; + if (tmp1 < -32767) tmp1 = -32767; + tx = tmp1; + ec->tx_2 = tmp; + } + + return tx; +} + +/*- End of function --------------------------------------------------------*/ +/*- End of file ------------------------------------------------------------*/ diff --git a/src/libecho/echo.h b/src/libecho/echo.h new file mode 100644 index 0000000..abbeb77 --- /dev/null +++ b/src/libecho/echo.h @@ -0,0 +1,230 @@ +/* + * SpanDSP - a series of DSP components for telephony + * + * echo.c - A line echo canceller. This code is being developed + * against and partially complies with G168. + * + * Written by Steve Underwood + * and David Rowe + * + * Copyright (C) 2001 Steve Underwood and 2007 David Rowe + * + * All rights reserved. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2, as + * published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + * + * $Id: echo.h,v 1.9 2006/10/24 13:45:28 steveu Exp $ + */ + +/*! \file */ + +#if !defined(_ECHO_H_) +#define _ECHO_H_ + +/*! \page echo_can_page Line echo cancellation for voice + +\section echo_can_page_sec_1 What does it do? +This module aims to provide G.168-2002 compliant echo cancellation, to remove +electrical echoes (e.g. from 2-4 wire hybrids) from voice calls. + +\section echo_can_page_sec_2 How does it work? +The heart of the echo cancellor is FIR filter. This is adapted to match the echo +impulse response of the telephone line. It must be long enough to adequately cover +the duration of that impulse response. The signal transmitted to the telephone line +is passed through the FIR filter. Once the FIR is properly adapted, the resulting +output is an estimate of the echo signal received from the line. This is subtracted +from the received signal. The result is an estimate of the signal which originated +at the far end of the line, free from echos of our own transmitted signal. + +The least mean squares (LMS) algorithm is attributed to Widrow and Hoff, and was +introduced in 1960. It is the commonest form of filter adaption used in things +like modem line equalisers and line echo cancellers. There it works very well. +However, it only works well for signals of constant amplitude. It works very poorly +for things like speech echo cancellation, where the signal level varies widely. +This is quite easy to fix. If the signal level is normalised - similar to applying +AGC - LMS can work as well for a signal of varying amplitude as it does for a modem +signal. This normalised least mean squares (NLMS) algorithm is the commonest one used +for speech echo cancellation. Many other algorithms exist - e.g. RLS (essentially +the same as Kalman filtering), FAP, etc. Some perform significantly better than NLMS. +However, factors such as computational complexity and patents favour the use of NLMS. + +A simple refinement to NLMS can improve its performance with speech. NLMS tends +to adapt best to the strongest parts of a signal. If the signal is white noise, +the NLMS algorithm works very well. However, speech has more low frequency than +high frequency content. Pre-whitening (i.e. filtering the signal to flatten +its spectrum) the echo signal improves the adapt rate for speech, and ensures the +final residual signal is not heavily biased towards high frequencies. A very low +complexity filter is adequate for this, so pre-whitening adds little to the +compute requirements of the echo canceller. + +An FIR filter adapted using pre-whitened NLMS performs well, provided certain +conditions are met: + + - The transmitted signal has poor self-correlation. + - There is no signal being generated within the environment being cancelled. + +The difficulty is that neither of these can be guaranteed. + +If the adaption is performed while transmitting noise (or something fairly noise +like, such as voice) the adaption works very well. If the adaption is performed +while transmitting something highly correlative (typically narrow band energy +such as signalling tones or DTMF), the adaption can go seriously wrong. The reason +is there is only one solution for the adaption on a near random signal - the impulse +response of the line. For a repetitive signal, there are any number of solutions +which converge the adaption, and nothing guides the adaption to choose the generalised +one. Allowing an untrained canceller to converge on this kind of narrowband +energy probably a good thing, since at least it cancels the tones. Allowing a well +converged canceller to continue converging on such energy is just a way to ruin +its generalised adaption. A narrowband detector is needed, so adapation can be +suspended at appropriate times. + +The adaption process is based on trying to eliminate the received signal. When +there is any signal from within the environment being cancelled it may upset the +adaption process. Similarly, if the signal we are transmitting is small, noise +may dominate and disturb the adaption process. If we can ensure that the +adaption is only performed when we are transmitting a significant signal level, +and the environment is not, things will be OK. Clearly, it is easy to tell when +we are sending a significant signal. Telling, if the environment is generating a +significant signal, and doing it with sufficient speed that the adaption will +not have diverged too much more we stop it, is a little harder. + +The key problem in detecting when the environment is sourcing significant energy +is that we must do this very quickly. Given a reasonably long sample of the +received signal, there are a number of strategies which may be used to assess +whether that signal contains a strong far end component. However, by the time +that assessment is complete the far end signal will have already caused major +mis-convergence in the adaption process. An assessment algorithm is needed which +produces a fairly accurate result from a very short burst of far end energy. + +\section echo_can_page_sec_3 How do I use it? +The echo cancellor processes both the transmit and receive streams sample by +sample. The processing function is not declared inline. Unfortunately, +cancellation requires many operations per sample, so the call overhead is only a +minor burden. +*/ + +#include "../../config.h" + +#ifdef HAVE_MMX_INSTRUCTIONS +#define USE_MMX +#endif + +#ifdef HAVE_SSE2_INSTRUCTIONS +#define USE_SSE2 +#endif + +#include "fir.h" + +/* Mask bits for the adaption mode */ + +#define ECHO_CAN_USE_ADAPTION 0x01 +#define ECHO_CAN_USE_NLP 0x02 +#define ECHO_CAN_USE_CNG 0x04 +#define ECHO_CAN_USE_CLIP 0x08 +#define ECHO_CAN_USE_TX_HPF 0x10 +#define ECHO_CAN_USE_RX_HPF 0x20 +#define ECHO_CAN_DISABLE 0x40 + +/*! + G.168 echo canceller descriptor. This defines the working state for a line + echo canceller. +*/ +typedef struct +{ + int16_t tx,rx; + int16_t clean; + int16_t clean_nlp; + + int nonupdate_dwell; + int curr_pos; + int taps; + int log2taps; + int adaption_mode; + + int cond_met; + int32_t Pstates; + int16_t adapt; + int32_t factor; + int16_t shift; + + /* Average levels and averaging filter states */ + int Ltxacc, Lrxacc, Lcleanacc, Lclean_bgacc; + int Ltx, Lrx; + int Lclean; + int Lclean_bg; + int Lbgn, Lbgn_acc, Lbgn_upper, Lbgn_upper_acc; + + /* foreground and background filter states */ + fir16_state_t fir_state; + fir16_state_t fir_state_bg; + int16_t *fir_taps16[2]; + + /* DC blocking filter states */ + int tx_1, tx_2, rx_1, rx_2; + + /* optional High Pass Filter states */ + int32_t xvtx[5], yvtx[5]; + int32_t xvrx[5], yvrx[5]; + + /* Parameters for the optional Hoth noise generator */ + int cng_level; + int cng_rndnum; + int cng_filter; + + /* snapshot sample of coeffs used for development */ + int16_t *snapshot; + +} echo_can_state_t; + +/*! Create a voice echo canceller context. + \param len The length of the canceller, in samples. + \return The new canceller context, or NULL if the canceller could not be created. +*/ +echo_can_state_t *echo_can_create(int len, int adaption_mode); + +/*! Free a voice echo canceller context. + \param ec The echo canceller context. +*/ +void echo_can_free(echo_can_state_t *ec); + +/*! Flush (reinitialise) a voice echo canceller context. + \param ec The echo canceller context. +*/ +void echo_can_flush(echo_can_state_t *ec); + +/*! Set the adaption mode of a voice echo canceller context. + \param ec The echo canceller context. + \param adapt The mode. +*/ +void echo_can_adaption_mode(echo_can_state_t *ec, int adaption_mode); + +void echo_can_snapshot(echo_can_state_t *ec); + +/*! Process a sample through a voice echo canceller. + \param ec The echo canceller context. + \param tx The transmitted audio sample. + \param rx The received audio sample. + \return The clean (echo cancelled) received sample. +*/ +int16_t echo_can_update(echo_can_state_t *ec, int16_t tx, int16_t rx); + +/*! Process to high pass filter the tx signal. + \param ec The echo canceller context. + \param tx The transmitted auio sample. + \return The HP filtered transmit sample, send this to your D/A. +*/ +int16_t echo_can_hpf_tx(echo_can_state_t *ec, int16_t tx); + +#endif +/*- End of file ------------------------------------------------------------*/ diff --git a/src/libecho/fir.h b/src/libecho/fir.h new file mode 100644 index 0000000..9ca6bea --- /dev/null +++ b/src/libecho/fir.h @@ -0,0 +1,369 @@ +/* + * SpanDSP - a series of DSP components for telephony + * + * fir.h - General telephony FIR routines + * + * Written by Steve Underwood + * + * Copyright (C) 2002 Steve Underwood + * + * All rights reserved. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2, as + * published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + * + * $Id: fir.h,v 1.8 2006/10/24 13:45:28 steveu Exp $ + */ + +/*! \page fir_page FIR filtering +\section fir_page_sec_1 What does it do? +???. + +\section fir_page_sec_2 How does it work? +???. +*/ + +#if !defined(_FIR_H_) +#define _FIR_H_ + +/* + Blackfin NOTES & IDEAS: + + A simple dot product function is used to implement the filter. This performs + just one MAC/cycle which is inefficient but was easy to implement as a first + pass. The current Blackfin code also uses an unrolled form of the filter + history to avoid 0 length hardware loop issues. This is wasteful of + memory. + + Ideas for improvement: + + 1/ Rewrite filter for dual MAC inner loop. The issue here is handling + history sample offsets that are 16 bit aligned - the dual MAC needs + 32 bit aligmnent. There are some good examples in libbfdsp. + + 2/ Use the hardware circular buffer facility tohalve memory usage. + + 3/ Consider using internal memory. + + Using less memory might also improve speed as cache misses will be + reduced. A drop in MIPs and memory approaching 50% should be + possible. + + The foreground and background filters currenlty use a total of + about 10 MIPs/ch as measured with speedtest.c on a 256 TAP echo + can. +*/ + +#if defined(USE_MMX) || defined(USE_SSE2) +#include "mmx.h" +#endif + +/*! + 16 bit integer FIR descriptor. This defines the working state for a single + instance of an FIR filter using 16 bit integer coefficients. +*/ +typedef struct +{ + int taps; + int curr_pos; + const int16_t *coeffs; + int16_t *history; +} fir16_state_t; + +/*! + 32 bit integer FIR descriptor. This defines the working state for a single + instance of an FIR filter using 32 bit integer coefficients, and filtering + 16 bit integer data. +*/ +typedef struct +{ + int taps; + int curr_pos; + const int32_t *coeffs; + int16_t *history; +} fir32_state_t; + +/*! + Floating point FIR descriptor. This defines the working state for a single + instance of an FIR filter using floating point coefficients and data. +*/ +typedef struct +{ + int taps; + int curr_pos; + const float *coeffs; + float *history; +} fir_float_state_t; + +#ifdef __cplusplus +extern "C" { +#endif + +static __inline__ const int16_t *fir16_create(fir16_state_t *fir, + const int16_t *coeffs, + int taps) +{ + fir->taps = taps; + fir->curr_pos = taps - 1; + fir->coeffs = coeffs; +#if defined(USE_MMX) || defined(USE_SSE2) || defined(__BLACKFIN_ASM__) + if ((fir->history = malloc(2*taps*sizeof(int16_t)))) + memset(fir->history, 0, 2*taps*sizeof(int16_t)); +#else + if ((fir->history = (int16_t *) malloc(taps*sizeof(int16_t)))) + memset(fir->history, 0, taps*sizeof(int16_t)); +#endif + return fir->history; +} +/*- End of function --------------------------------------------------------*/ + +static __inline__ void fir16_flush(fir16_state_t *fir) +{ +#if defined(USE_MMX) || defined(USE_SSE2) || defined(__BLACKFIN_ASM__) + memset(fir->history, 0, 2*fir->taps*sizeof(int16_t)); +#else + memset(fir->history, 0, fir->taps*sizeof(int16_t)); +#endif +} +/*- End of function --------------------------------------------------------*/ + +static __inline__ void fir16_free(fir16_state_t *fir) +{ + free(fir->history); +} +/*- End of function --------------------------------------------------------*/ + +#ifdef __BLACKFIN_ASM__ +static inline int32_t dot_asm(short *x, short *y, int len) +{ + int dot; + + len--; + + __asm__ + ( + "I0 = %1;\n\t" + "I1 = %2;\n\t" + "A0 = 0;\n\t" + "R0.L = W[I0++] || R1.L = W[I1++];\n\t" + "LOOP dot%= LC0 = %3;\n\t" + "LOOP_BEGIN dot%=;\n\t" + "A0 += R0.L * R1.L (IS) || R0.L = W[I0++] || R1.L = W[I1++];\n\t" + "LOOP_END dot%=;\n\t" + "A0 += R0.L*R1.L (IS);\n\t" + "R0 = A0;\n\t" + "%0 = R0;\n\t" + : "=&d" (dot) + : "a" (x), "a" (y), "a" (len) + : "I0", "I1", "A1", "A0", "R0", "R1" + ); + + return dot; +} +#endif +/*- End of function --------------------------------------------------------*/ + +static __inline__ int16_t fir16(fir16_state_t *fir, int16_t sample) +{ + int32_t y; +#if defined(USE_MMX) + int i; + mmx_t *mmx_coeffs; + mmx_t *mmx_hist; + + fir->history[fir->curr_pos] = sample; + fir->history[fir->curr_pos + fir->taps] = sample; + + mmx_coeffs = (mmx_t *) fir->coeffs; + mmx_hist = (mmx_t *) &fir->history[fir->curr_pos]; + i = fir->taps; + pxor_r2r(mm4, mm4); + /* 8 samples per iteration, so the filter must be a multiple of 8 long. */ + while (i > 0) + { + movq_m2r(mmx_coeffs[0], mm0); + movq_m2r(mmx_coeffs[1], mm2); + movq_m2r(mmx_hist[0], mm1); + movq_m2r(mmx_hist[1], mm3); + mmx_coeffs += 2; + mmx_hist += 2; + pmaddwd_r2r(mm1, mm0); + pmaddwd_r2r(mm3, mm2); + paddd_r2r(mm0, mm4); + paddd_r2r(mm2, mm4); + i -= 8; + } + movq_r2r(mm4, mm0); + psrlq_i2r(32, mm0); + paddd_r2r(mm0, mm4); + movd_r2m(mm4, y); + emms(); +#elif defined(USE_SSE2) + int i; + xmm_t *xmm_coeffs; + xmm_t *xmm_hist; + + fir->history[fir->curr_pos] = sample; + fir->history[fir->curr_pos + fir->taps] = sample; + + xmm_coeffs = (xmm_t *) fir->coeffs; + xmm_hist = (xmm_t *) &fir->history[fir->curr_pos]; + i = fir->taps; + pxor_r2r(xmm4, xmm4); + /* 16 samples per iteration, so the filter must be a multiple of 16 long. */ + while (i > 0) + { + movdqu_m2r(xmm_coeffs[0], xmm0); + movdqu_m2r(xmm_coeffs[1], xmm2); + movdqu_m2r(xmm_hist[0], xmm1); + movdqu_m2r(xmm_hist[1], xmm3); + xmm_coeffs += 2; + xmm_hist += 2; + pmaddwd_r2r(xmm1, xmm0); + pmaddwd_r2r(xmm3, xmm2); + paddd_r2r(xmm0, xmm4); + paddd_r2r(xmm2, xmm4); + i -= 16; + } + movdqa_r2r(xmm4, xmm0); + psrldq_i2r(8, xmm0); + paddd_r2r(xmm0, xmm4); + movdqa_r2r(xmm4, xmm0); + psrldq_i2r(4, xmm0); + paddd_r2r(xmm0, xmm4); + movd_r2m(xmm4, y); +#elif defined(__BLACKFIN_ASM__) + fir->history[fir->curr_pos] = sample; + fir->history[fir->curr_pos + fir->taps] = sample; + y = dot_asm((int16_t*)fir->coeffs, &fir->history[fir->curr_pos], fir->taps); +#else + int i; + int offset1; + int offset2; + + fir->history[fir->curr_pos] = sample; + + offset2 = fir->curr_pos; + offset1 = fir->taps - offset2; + y = 0; + for (i = fir->taps - 1; i >= offset1; i--) + y += fir->coeffs[i]*fir->history[i - offset1]; + for ( ; i >= 0; i--) + y += fir->coeffs[i]*fir->history[i + offset2]; +#endif + if (fir->curr_pos <= 0) + fir->curr_pos = fir->taps; + fir->curr_pos--; + return (int16_t) (y >> 15); +} +/*- End of function --------------------------------------------------------*/ + +static __inline__ const int16_t *fir32_create(fir32_state_t *fir, + const int32_t *coeffs, + int taps) +{ + fir->taps = taps; + fir->curr_pos = taps - 1; + fir->coeffs = coeffs; + fir->history = (int16_t *) malloc(taps*sizeof(int16_t)); + if (fir->history) + memset(fir->history, '\0', taps*sizeof(int16_t)); + return fir->history; +} +/*- End of function --------------------------------------------------------*/ + +static __inline__ void fir32_flush(fir32_state_t *fir) +{ + memset(fir->history, 0, fir->taps*sizeof(int16_t)); +} +/*- End of function --------------------------------------------------------*/ + +static __inline__ void fir32_free(fir32_state_t *fir) +{ + free(fir->history); +} +/*- End of function --------------------------------------------------------*/ + +static __inline__ int16_t fir32(fir32_state_t *fir, int16_t sample) +{ + int i; + int32_t y; + int offset1; + int offset2; + + fir->history[fir->curr_pos] = sample; + offset2 = fir->curr_pos; + offset1 = fir->taps - offset2; + y = 0; + for (i = fir->taps - 1; i >= offset1; i--) + y += fir->coeffs[i]*fir->history[i - offset1]; + for ( ; i >= 0; i--) + y += fir->coeffs[i]*fir->history[i + offset2]; + if (fir->curr_pos <= 0) + fir->curr_pos = fir->taps; + fir->curr_pos--; + return (int16_t) (y >> 15); +} +/*- End of function --------------------------------------------------------*/ + +#ifndef __KERNEL__ +static __inline__ const float *fir_float_create(fir_float_state_t *fir, + const float *coeffs, + int taps) +{ + fir->taps = taps; + fir->curr_pos = taps - 1; + fir->coeffs = coeffs; + fir->history = (float *) malloc(taps*sizeof(float)); + if (fir->history) + memset(fir->history, '\0', taps*sizeof(float)); + return fir->history; +} +/*- End of function --------------------------------------------------------*/ + +static __inline__ void fir_float_free(fir_float_state_t *fir) +{ + free(fir->history); +} +/*- End of function --------------------------------------------------------*/ + +static __inline__ int16_t fir_float(fir_float_state_t *fir, int16_t sample) +{ + int i; + float y; + int offset1; + int offset2; + + fir->history[fir->curr_pos] = sample; + + offset2 = fir->curr_pos; + offset1 = fir->taps - offset2; + y = 0; + for (i = fir->taps - 1; i >= offset1; i--) + y += fir->coeffs[i]*fir->history[i - offset1]; + for ( ; i >= 0; i--) + y += fir->coeffs[i]*fir->history[i + offset2]; + if (fir->curr_pos <= 0) + fir->curr_pos = fir->taps; + fir->curr_pos--; + return (int16_t) y; +} +/*- End of function --------------------------------------------------------*/ +#endif + +#ifdef __cplusplus +} +#endif + +#endif +/*- End of file ------------------------------------------------------------*/ diff --git a/src/libecho/mmx.h b/src/libecho/mmx.h new file mode 100644 index 0000000..b5a3964 --- /dev/null +++ b/src/libecho/mmx.h @@ -0,0 +1,288 @@ +/* + * mmx.h + * Copyright (C) 1997-2001 H. Dietz and R. Fisher + * + * This file is part of FFmpeg. + * + * FFmpeg is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * FFmpeg is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with FFmpeg; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + */ +#ifndef AVCODEC_I386MMX_H +#define AVCODEC_I386MMX_H + +/* + * The type of an value that fits in an MMX register (note that long + * long constant values MUST be suffixed by LL and unsigned long long + * values by ULL, lest they be truncated by the compiler) + */ + +typedef union { + long long q; /* Quadword (64-bit) value */ + unsigned long long uq; /* Unsigned Quadword */ + int d[2]; /* 2 Doubleword (32-bit) values */ + unsigned int ud[2]; /* 2 Unsigned Doubleword */ + short w[4]; /* 4 Word (16-bit) values */ + unsigned short uw[4]; /* 4 Unsigned Word */ + char b[8]; /* 8 Byte (8-bit) values */ + unsigned char ub[8]; /* 8 Unsigned Byte */ + float s[2]; /* Single-precision (32-bit) value */ +} mmx_t; /* On an 8-byte (64-bit) boundary */ + +/* SSE registers */ +typedef union { + char b[16]; +} xmm_t; + + +#define mmx_i2r(op,imm,reg) \ + __asm__ __volatile__ (#op " %0, %%" #reg \ + : /* nothing */ \ + : "i" (imm) ) + +#define mmx_m2r(op,mem,reg) \ + __asm__ __volatile__ (#op " %0, %%" #reg \ + : /* nothing */ \ + : "m" (mem)) + +#define mmx_r2m(op,reg,mem) \ + __asm__ __volatile__ (#op " %%" #reg ", %0" \ + : "=m" (mem) \ + : /* nothing */ ) + +#define mmx_r2r(op,regs,regd) \ + __asm__ __volatile__ (#op " %" #regs ", %" #regd) + + +#define emms() __asm__ __volatile__ ("emms") + +#define movd_m2r(var,reg) mmx_m2r (movd, var, reg) +#define movd_r2m(reg,var) mmx_r2m (movd, reg, var) +#define movd_r2r(regs,regd) mmx_r2r (movd, regs, regd) + +#define movq_m2r(var,reg) mmx_m2r (movq, var, reg) +#define movq_r2m(reg,var) mmx_r2m (movq, reg, var) +#define movq_r2r(regs,regd) mmx_r2r (movq, regs, regd) + +#define packssdw_m2r(var,reg) mmx_m2r (packssdw, var, reg) +#define packssdw_r2r(regs,regd) mmx_r2r (packssdw, regs, regd) +#define packsswb_m2r(var,reg) mmx_m2r (packsswb, var, reg) +#define packsswb_r2r(regs,regd) mmx_r2r (packsswb, regs, regd) + +#define packuswb_m2r(var,reg) mmx_m2r (packuswb, var, reg) +#define packuswb_r2r(regs,regd) mmx_r2r (packuswb, regs, regd) + +#define paddb_m2r(var,reg) mmx_m2r (paddb, var, reg) +#define paddb_r2r(regs,regd) mmx_r2r (paddb, regs, regd) +#define paddd_m2r(var,reg) mmx_m2r (paddd, var, reg) +#define paddd_r2r(regs,regd) mmx_r2r (paddd, regs, regd) +#define paddw_m2r(var,reg) mmx_m2r (paddw, var, reg) +#define paddw_r2r(regs,regd) mmx_r2r (paddw, regs, regd) + +#define paddsb_m2r(var,reg) mmx_m2r (paddsb, var, reg) +#define paddsb_r2r(regs,regd) mmx_r2r (paddsb, regs, regd) +#define paddsw_m2r(var,reg) mmx_m2r (paddsw, var, reg) +#define paddsw_r2r(regs,regd) mmx_r2r (paddsw, regs, regd) + +#define paddusb_m2r(var,reg) mmx_m2r (paddusb, var, reg) +#define paddusb_r2r(regs,regd) mmx_r2r (paddusb, regs, regd) +#define paddusw_m2r(var,reg) mmx_m2r (paddusw, var, reg) +#define paddusw_r2r(regs,regd) mmx_r2r (paddusw, regs, regd) + +#define pand_m2r(var,reg) mmx_m2r (pand, var, reg) +#define pand_r2r(regs,regd) mmx_r2r (pand, regs, regd) + +#define pandn_m2r(var,reg) mmx_m2r (pandn, var, reg) +#define pandn_r2r(regs,regd) mmx_r2r (pandn, regs, regd) + +#define pcmpeqb_m2r(var,reg) mmx_m2r (pcmpeqb, var, reg) +#define pcmpeqb_r2r(regs,regd) mmx_r2r (pcmpeqb, regs, regd) +#define pcmpeqd_m2r(var,reg) mmx_m2r (pcmpeqd, var, reg) +#define pcmpeqd_r2r(regs,regd) mmx_r2r (pcmpeqd, regs, regd) +#define pcmpeqw_m2r(var,reg) mmx_m2r (pcmpeqw, var, reg) +#define pcmpeqw_r2r(regs,regd) mmx_r2r (pcmpeqw, regs, regd) + +#define pcmpgtb_m2r(var,reg) mmx_m2r (pcmpgtb, var, reg) +#define pcmpgtb_r2r(regs,regd) mmx_r2r (pcmpgtb, regs, regd) +#define pcmpgtd_m2r(var,reg) mmx_m2r (pcmpgtd, var, reg) +#define pcmpgtd_r2r(regs,regd) mmx_r2r (pcmpgtd, regs, regd) +#define pcmpgtw_m2r(var,reg) mmx_m2r (pcmpgtw, var, reg) +#define pcmpgtw_r2r(regs,regd) mmx_r2r (pcmpgtw, regs, regd) + +#define pmaddwd_m2r(var,reg) mmx_m2r (pmaddwd, var, reg) +#define pmaddwd_r2r(regs,regd) mmx_r2r (pmaddwd, regs, regd) + +#define pmulhw_m2r(var,reg) mmx_m2r (pmulhw, var, reg) +#define pmulhw_r2r(regs,regd) mmx_r2r (pmulhw, regs, regd) + +#define pmullw_m2r(var,reg) mmx_m2r (pmullw, var, reg) +#define pmullw_r2r(regs,regd) mmx_r2r (pmullw, regs, regd) + +#define por_m2r(var,reg) mmx_m2r (por, var, reg) +#define por_r2r(regs,regd) mmx_r2r (por, regs, regd) + +#define pslld_i2r(imm,reg) mmx_i2r (pslld, imm, reg) +#define pslld_m2r(var,reg) mmx_m2r (pslld, var, reg) +#define pslld_r2r(regs,regd) mmx_r2r (pslld, regs, regd) +#define psllq_i2r(imm,reg) mmx_i2r (psllq, imm, reg) +#define psllq_m2r(var,reg) mmx_m2r (psllq, var, reg) +#define psllq_r2r(regs,regd) mmx_r2r (psllq, regs, regd) +#define psllw_i2r(imm,reg) mmx_i2r (psllw, imm, reg) +#define psllw_m2r(var,reg) mmx_m2r (psllw, var, reg) +#define psllw_r2r(regs,regd) mmx_r2r (psllw, regs, regd) + +#define psrad_i2r(imm,reg) mmx_i2r (psrad, imm, reg) +#define psrad_m2r(var,reg) mmx_m2r (psrad, var, reg) +#define psrad_r2r(regs,regd) mmx_r2r (psrad, regs, regd) +#define psraw_i2r(imm,reg) mmx_i2r (psraw, imm, reg) +#define psraw_m2r(var,reg) mmx_m2r (psraw, var, reg) +#define psraw_r2r(regs,regd) mmx_r2r (psraw, regs, regd) + +#define psrld_i2r(imm,reg) mmx_i2r (psrld, imm, reg) +#define psrld_m2r(var,reg) mmx_m2r (psrld, var, reg) +#define psrld_r2r(regs,regd) mmx_r2r (psrld, regs, regd) +#define psrlq_i2r(imm,reg) mmx_i2r (psrlq, imm, reg) +#define psrlq_m2r(var,reg) mmx_m2r (psrlq, var, reg) +#define psrlq_r2r(regs,regd) mmx_r2r (psrlq, regs, regd) +#define psrlw_i2r(imm,reg) mmx_i2r (psrlw, imm, reg) +#define psrlw_m2r(var,reg) mmx_m2r (psrlw, var, reg) +#define psrlw_r2r(regs,regd) mmx_r2r (psrlw, regs, regd) + +#define psubb_m2r(var,reg) mmx_m2r (psubb, var, reg) +#define psubb_r2r(regs,regd) mmx_r2r (psubb, regs, regd) +#define psubd_m2r(var,reg) mmx_m2r (psubd, var, reg) +#define psubd_r2r(regs,regd) mmx_r2r (psubd, regs, regd) +#define psubw_m2r(var,reg) mmx_m2r (psubw, var, reg) +#define psubw_r2r(regs,regd) mmx_r2r (psubw, regs, regd) + +#define psubsb_m2r(var,reg) mmx_m2r (psubsb, var, reg) +#define psubsb_r2r(regs,regd) mmx_r2r (psubsb, regs, regd) +#define psubsw_m2r(var,reg) mmx_m2r (psubsw, var, reg) +#define psubsw_r2r(regs,regd) mmx_r2r (psubsw, regs, regd) + +#define psubusb_m2r(var,reg) mmx_m2r (psubusb, var, reg) +#define psubusb_r2r(regs,regd) mmx_r2r (psubusb, regs, regd) +#define psubusw_m2r(var,reg) mmx_m2r (psubusw, var, reg) +#define psubusw_r2r(regs,regd) mmx_r2r (psubusw, regs, regd) + +#define punpckhbw_m2r(var,reg) mmx_m2r (punpckhbw, var, reg) +#define punpckhbw_r2r(regs,regd) mmx_r2r (punpckhbw, regs, regd) +#define punpckhdq_m2r(var,reg) mmx_m2r (punpckhdq, var, reg) +#define punpckhdq_r2r(regs,regd) mmx_r2r (punpckhdq, regs, regd) +#define punpckhwd_m2r(var,reg) mmx_m2r (punpckhwd, var, reg) +#define punpckhwd_r2r(regs,regd) mmx_r2r (punpckhwd, regs, regd) + +#define punpcklbw_m2r(var,reg) mmx_m2r (punpcklbw, var, reg) +#define punpcklbw_r2r(regs,regd) mmx_r2r (punpcklbw, regs, regd) +#define punpckldq_m2r(var,reg) mmx_m2r (punpckldq, var, reg) +#define punpckldq_r2r(regs,regd) mmx_r2r (punpckldq, regs, regd) +#define punpcklwd_m2r(var,reg) mmx_m2r (punpcklwd, var, reg) +#define punpcklwd_r2r(regs,regd) mmx_r2r (punpcklwd, regs, regd) + +#define pxor_m2r(var,reg) mmx_m2r (pxor, var, reg) +#define pxor_r2r(regs,regd) mmx_r2r (pxor, regs, regd) + + +/* 3DNOW extensions */ + +#define pavgusb_m2r(var,reg) mmx_m2r (pavgusb, var, reg) +#define pavgusb_r2r(regs,regd) mmx_r2r (pavgusb, regs, regd) + + +/* AMD MMX extensions - also available in intel SSE */ + + +#define mmx_m2ri(op,mem,reg,imm) \ + __asm__ __volatile__ (#op " %1, %0, %%" #reg \ + : /* nothing */ \ + : "m" (mem), "i" (imm)) +#define mmx_r2ri(op,regs,regd,imm) \ + __asm__ __volatile__ (#op " %0, %%" #regs ", %%" #regd \ + : /* nothing */ \ + : "i" (imm) ) + +#define mmx_fetch(mem,hint) \ + __asm__ __volatile__ ("prefetch" #hint " %0" \ + : /* nothing */ \ + : "m" (mem)) + + +#define maskmovq(regs,maskreg) mmx_r2ri (maskmovq, regs, maskreg) + +#define movntq_r2m(mmreg,var) mmx_r2m (movntq, mmreg, var) + +#define pavgb_m2r(var,reg) mmx_m2r (pavgb, var, reg) +#define pavgb_r2r(regs,regd) mmx_r2r (pavgb, regs, regd) +#define pavgw_m2r(var,reg) mmx_m2r (pavgw, var, reg) +#define pavgw_r2r(regs,regd) mmx_r2r (pavgw, regs, regd) + +#define pextrw_r2r(mmreg,reg,imm) mmx_r2ri (pextrw, mmreg, reg, imm) + +#define pinsrw_r2r(reg,mmreg,imm) mmx_r2ri (pinsrw, reg, mmreg, imm) + +#define pmaxsw_m2r(var,reg) mmx_m2r (pmaxsw, var, reg) +#define pmaxsw_r2r(regs,regd) mmx_r2r (pmaxsw, regs, regd) + +#define pmaxub_m2r(var,reg) mmx_m2r (pmaxub, var, reg) +#define pmaxub_r2r(regs,regd) mmx_r2r (pmaxub, regs, regd) + +#define pminsw_m2r(var,reg) mmx_m2r (pminsw, var, reg) +#define pminsw_r2r(regs,regd) mmx_r2r (pminsw, regs, regd) + +#define pminub_m2r(var,reg) mmx_m2r (pminub, var, reg) +#define pminub_r2r(regs,regd) mmx_r2r (pminub, regs, regd) + +#define pmovmskb(mmreg,reg) \ + __asm__ __volatile__ ("movmskps %" #mmreg ", %" #reg) + +#define pmulhuw_m2r(var,reg) mmx_m2r (pmulhuw, var, reg) +#define pmulhuw_r2r(regs,regd) mmx_r2r (pmulhuw, regs, regd) + +#define prefetcht0(mem) mmx_fetch (mem, t0) +#define prefetcht1(mem) mmx_fetch (mem, t1) +#define prefetcht2(mem) mmx_fetch (mem, t2) +#define prefetchnta(mem) mmx_fetch (mem, nta) + +#define psadbw_m2r(var,reg) mmx_m2r (psadbw, var, reg) +#define psadbw_r2r(regs,regd) mmx_r2r (psadbw, regs, regd) + +#define pshufw_m2r(var,reg,imm) mmx_m2ri(pshufw, var, reg, imm) +#define pshufw_r2r(regs,regd,imm) mmx_r2ri(pshufw, regs, regd, imm) + +#define sfence() __asm__ __volatile__ ("sfence\n\t") + +/* SSE2 */ +#define pshufhw_m2r(var,reg,imm) mmx_m2ri(pshufhw, var, reg, imm) +#define pshufhw_r2r(regs,regd,imm) mmx_r2ri(pshufhw, regs, regd, imm) +#define pshuflw_m2r(var,reg,imm) mmx_m2ri(pshuflw, var, reg, imm) +#define pshuflw_r2r(regs,regd,imm) mmx_r2ri(pshuflw, regs, regd, imm) + +#define pshufd_r2r(regs,regd,imm) mmx_r2ri(pshufd, regs, regd, imm) + +#define movdqa_m2r(var,reg) mmx_m2r (movdqa, var, reg) +#define movdqa_r2m(reg,var) mmx_r2m (movdqa, reg, var) +#define movdqa_r2r(regs,regd) mmx_r2r (movdqa, regs, regd) +#define movdqu_m2r(var,reg) mmx_m2r (movdqu, var, reg) +#define movdqu_r2m(reg,var) mmx_r2m (movdqu, reg, var) +#define movdqu_r2r(regs,regd) mmx_r2r (movdqu, regs, regd) + +#define pmullw_r2m(reg,var) mmx_r2m (pmullw, reg, var) + +#define pslldq_i2r(imm,reg) mmx_i2r (pslldq, imm, reg) +#define psrldq_i2r(imm,reg) mmx_i2r (psrldq, imm, reg) + +#define punpcklqdq_r2r(regs,regd) mmx_r2r (punpcklqdq, regs, regd) +#define punpckhqdq_r2r(regs,regd) mmx_r2r (punpckhqdq, regs, regd) + + +#endif /* AVCODEC_I386MMX_H */ diff --git a/tests/Makefile.am b/tests/Makefile.am new file mode 100644 index 0000000..1ddb4d0 --- /dev/null +++ b/tests/Makefile.am @@ -0,0 +1,13 @@ +AM_CPPFLAGS = $(all_includes) +AM_CFLAGS= -Wall -Wextra -g $(LIBOSMOCORE_CFLAGS) $(LIBOSMOGSM_CFLAGS) $(LIBOSMOVTY_CFLAGS) $(LIBOSMOABIS_CFLAGS) $(LIBOSMOCTRL_CFLAGS) +AM_LDFLAGS = $(COVERAGE_LDFLAGS) +COMMONLIBS = $(LIBOSMOCORE_LIBS) $(LIBOSMOGSM_LIBS) $(LIBOSMOVTY_LIBS) $(LIBOSMOABIS_LIBS) $(LIBOSMOCTRL_LIBS) + +noinst_PROGRAMS = answer_detect + +answer_detect_SOURCES = answer_detect.c + +answer_detect_LDADD = $(COMMON_LA) \ + $(COMMONLIBS) \ + ../src/libecho/libecho.a \ + -lm diff --git a/tests/answer_detect.c b/tests/answer_detect.c new file mode 100644 index 0000000..fb474d7 --- /dev/null +++ b/tests/answer_detect.c @@ -0,0 +1,179 @@ +/* Testing the answer tone algorithm of libecho + * + * (C) 2023 by Andreas Eversberg + * All Rights Reserved + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU Affero General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Affero General Public License for more details. + * + * You should have received a copy of the GNU Affero General Public License + * along with this program. If not, see . + * + */ + +#include +#include +#include +#include +#include +#include + +#include "../src/libecho/answertone.h" + +#define db2level(db) pow(10, (double)(db) / 20.0) + +#define ANS_dB -13.0 +#define ANS_DURATION 4 +#define ANS_PHASE_REV 0.450 +#define SAMPLERATE 8000 +#define BANDWIDTH 100.0 + +float tone[SAMPLERATE * ANS_DURATION]; +int16_t tone_int[SAMPLERATE * ANS_DURATION]; + +static void gen_ans(float frequency, float level) +{ + double phase = 0.0, rev = 0.0; + int i; + + for (i = 0; i < SAMPLERATE * ANS_DURATION; i++) { + /* given tone */ + tone[i] = sin(2.0 * M_PI * frequency * (double)i / (double)SAMPLERATE + phase); + /* 15 Hz amplitude modulation */ + tone[i] *= 1.0 + sin(2.0 * M_PI * 15.0 * (double)i / (double)SAMPLERATE) * 0.2; + /* given level */ + tone[i] *= level; + tone_int[i] = tone[i] * 32767.0; + /* apply phase reversal */ + rev += 1.0 / (double)SAMPLERATE; + if (rev >= ANS_PHASE_REV) { + rev -= ANS_PHASE_REV; + phase += M_PI; + } + } +} + +static enum at_fail perform_test(struct answer_tone *at, float frequency, float level, float *phase1_p, float *phase2_p) +{ + enum at_fail fail; + int offset = 0; + float _frequency; + + gen_ans(frequency, level); + + answertone_init(at, SAMPLERATE); + + memcpy(at->buffer, tone + offset, sizeof(*at->buffer) * at->buffer_size); + offset += at->buffer_size; + answertone_check(at, phase1_p, &_frequency); + memcpy(at->buffer, tone + offset, sizeof(*at->buffer) * at->buffer_size); + offset += at->buffer_size; + fail = answertone_check(at, phase2_p, &_frequency); + + switch (fail) { + case AT_FAIL_MIN_LEVEL: + printf("failed, level too low\n"); + break; + case AT_FAIL_MAX_NOISE: + printf("failed, too noisy\n"); + break; + case AT_FAIL_MAX_FREQUENCY: + printf("failed, frequency too much off\n"); + break; + case AT_FAIL_NONE: + printf("ok\n"); + } + + return fail; +} + +static void _assert(bool what) +{ + if (what) { + printf("The result of this test was as expected.\n\n"); + return; + } + printf("*\nThe result of this test was not as expected, please FIX!.\n*\n"); + exit(-1); +} + +int main(void) +{ + struct answer_tone answer_tone, *at = &answer_tone; + int offset; + enum at_fail fail; + float phase1, phase2, _frequency; + int rc; + + printf("Test chunk with perfect tone: "); + fail = perform_test(at, 2100, db2level(ANS_dB), &phase1, &phase2); + _assert(!fail); + printf("Check phase: %.3f == %.3f\n", phase1, phase2); + _assert(fabsf(phase1 - phase2) < 0.001); + printf("Test chunk with 2101 Hz tone: "); + fail = perform_test(at, 2101, db2level(ANS_dB), &phase1, &phase2); + _assert(!fail); + printf("Check phase: %.3f != %.3f\n", phase1, phase2); + _assert(fabsf(phase1 - phase2) > 0.001); + printf("Test chunk with 2121 Hz tone: "); + fail = perform_test(at, 2121, db2level(ANS_dB), &phase1, &phase2); + _assert(!fail); + printf("Test chunk with -26 dBm0 tone: "); + fail = perform_test(at, 2100, db2level(-26), &phase1, &phase2); + _assert(!fail); + printf("Test chunk with 2123 Hz tone: "); + fail = perform_test(at, 2123, db2level(ANS_dB), &phase1, &phase2); + _assert(fail); + printf("Test chunk with -28 dBm0 tone: "); + fail = perform_test(at, 2100, db2level(-28), &phase1, &phase2); + _assert(fail); + + printf("Test chunk with long sequence of the tone: "); + gen_ans(2100+10, db2level(ANS_dB-8)); + answertone_init(at, SAMPLERATE); + for (offset = 0; offset <= (double)SAMPLERATE * ANS_PHASE_REV - at->buffer_size; offset += at->buffer_size) { + memcpy(at->buffer, tone + offset, sizeof(*at->buffer) * at->buffer_size); + fail = answertone_check(at, &phase1, &_frequency); + switch (fail) { + case AT_FAIL_MIN_LEVEL: + printf("level too low\n"); + _assert(false); + break; + case AT_FAIL_MAX_NOISE: + printf("too noisy\n"); + _assert(false); + break; + case AT_FAIL_MAX_FREQUENCY: + if (offset == 0) + break; + printf("frequency too much off\n"); + _assert(false); + break; + case AT_FAIL_NONE: + ; + } + } + printf("ok\n"); + _assert(true); + + printf("Test complete process with long sequence of the tone: "); + gen_ans(2100+10, db2level(ANS_dB-8)); + answertone_init(at, SAMPLERATE); + rc = answertone_process(at, tone_int, SAMPLERATE * ANS_DURATION); + if (rc > 0) + printf("tone detected\n"); + else + printf("no tone detected\n"); + answertone_exit(at); + _assert(rc > 0); + + return 0; +} +