diff --git a/src/libecho/Makefile.am b/src/libecho/Makefile.am index c31fcd8..98ef35a 100644 --- a/src/libecho/Makefile.am +++ b/src/libecho/Makefile.am @@ -3,5 +3,6 @@ AM_CFLAGS= -Wall -Wextra -g noinst_LIBRARIES = libecho.a -libecho_a_SOURCES = echo.c \ +libecho_a_SOURCES = echo_cancel.c \ + echo_suppress.c \ answertone.c diff --git a/src/libecho/answertone.c b/src/libecho/answertone.c index 95100ba..40172b2 100644 --- a/src/libecho/answertone.c +++ b/src/libecho/answertone.c @@ -25,6 +25,7 @@ #include #include +#include #include #include #include @@ -154,7 +155,7 @@ static const char *answertone_cause[] = { }; #endif -static enum at_state answertone_chunk(struct answer_tone *at) +static enum at_state answertone_chunk(struct answer_tone *at, bool phase_reversal) { float phase, frequency, shift; enum at_fail fail; @@ -197,6 +198,14 @@ static enum at_state answertone_chunk(struct answer_tone *at) break; case AT_STATE_TONE: if (fail != AT_FAIL_NONE) { + if (!phase_reversal) { +#if DEBUG + printf("LONG TONE LOST\n"); +#endif + at->state = AT_STATE_TONE; + rc = 1; + break; + } #if DEBUG printf("LONG TONE CHANGED, CHECK PHASE REVERSAL\n"); #endif @@ -250,7 +259,7 @@ static enum at_state answertone_chunk(struct answer_tone *at) } /* 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 answertone_process(struct answer_tone *at, int16_t *data, int length, bool phase_reversal) { int rc = 0; @@ -258,7 +267,7 @@ int answertone_process(struct answer_tone *at, int16_t *data, int 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); + rc |= answertone_chunk(at, phase_reversal); at->buffer_pos = 0; } } diff --git a/src/libecho/answertone.h b/src/libecho/answertone.h index d20fc8d..b686471 100644 --- a/src/libecho/answertone.h +++ b/src/libecho/answertone.h @@ -34,4 +34,4 @@ 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); +int answertone_process(struct answer_tone *at, int16_t *data, int length, bool phase_reversal); diff --git a/src/libecho/echo.c b/src/libecho/echo_cancel.c similarity index 99% rename from src/libecho/echo.c rename to src/libecho/echo_cancel.c index 830b34c..de5c797 100644 --- a/src/libecho/echo.c +++ b/src/libecho/echo_cancel.c @@ -124,7 +124,7 @@ #endif #include "bit_operations.h" -#include "echo.h" +#include "echo_cancel.h" #if !defined(NULL) #define NULL (void *) 0 diff --git a/src/libecho/echo.h b/src/libecho/echo_cancel.h similarity index 100% rename from src/libecho/echo.h rename to src/libecho/echo_cancel.h diff --git a/src/libecho/echo_suppress.c b/src/libecho/echo_suppress.c new file mode 100644 index 0000000..d067afc --- /dev/null +++ b/src/libecho/echo_suppress.c @@ -0,0 +1,316 @@ +/* + * (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 . + * + */ + +/* This Echo Suppressor is based on ITU-T G.164, but it does not conform with it! + * + * The 'receive' signal (received by the interface) will cause an echo what is + * included in the 'send' signal (sent by the interface). To remove echo, the + * 'send' signal is muted when speech is received by the interface. + * + * Figure 6 / G,164 shows a diagram with the states in this code: + * + * The 'silence' state is depicted as X. It is entered when both sides do not + * talk. + * + * The 'suppression' state is depticted as Z. It is entered when speech is + * received by the interface. + * + * The 'break-in' state is depicted as W. It is entered when speech is sent to + * the interface. + * + * The depiced area V is implemented as a 6 dB loss, so that a transition from + * 'break-in' state to 'suppession' state requires 6 dB more level in the + * receive path. + * + * The input is based on an int16_t that has a range of +3 dBm. + * + * Timers are used to enter state (operate) or leave state (hangover). + * + * Six filters are used, to detect the speech level, three in each direction: + * + * A low-pass filter and a high-pass filter are used to limit the band within + * the 500-3400 Hz range. + * + * The the signal is rectified, amplified and filtered with a low-pass filter. + * The resulting level represents the average volume of the input. (The peak + * level of a sine wave at the input will result in same level on the output.) + * + * In the 'suppression' state, the send signal is muted. + * + * In the 'break-in' state, the receive signal is NOT changed. The loss is only + * inserted in the processing, not is the actual signal. + */ + +#include + +#include +#include + +#include "echo_suppress.h" + +#define db2level(db) pow(10, (double)db / 20.0) +#define level2db(level) (20 * log10(level)) + +#define SUPPRESS_THRESHOLD -31.0 +#define RELEASE_SUPPRESS -31.0 +#define INSERTION_LOSS 6.0 + +#define SUPPRESS_OPERATE 0.000 +#define SUPPRESS_HANGOVER 0.050 +#define BREAK_IN_OPERATE 0.030 +#define BREAK_IN_HANGOVER 0.050 + +#define LP_FREQUENCY 3400.0 +#define HP_FREQUENCY 500.0 +#define ENV_FREQUENCY 500.0 + +//#define DEBUG_ES + +/* + * IIR filter + */ + +static inline void iir_lowpass_init(iir_filter_t *filter, float frequency, int samplerate) +{ + float Fc, Q, K, norm; + + Q = pow(sqrt(0.5), 1.0); /* 0.7071 */ + Fc = frequency / (float)samplerate; + K = tan(M_PI * Fc); + norm = 1 / (1 + K / Q + K * K); + filter->a0 = K * K * norm; + filter->a1 = 2 * filter->a0; + filter->a2 = filter->a0; + filter->b1 = 2 * (K * K - 1) * norm; + filter->b2 = (1 - K / Q + K * K) * norm; +} + +static inline void iir_highpass_init(iir_filter_t *filter, float frequency, int samplerate) +{ + float Fc, Q, K, norm; + + Q = pow(sqrt(0.5), 1.0); /* 0.7071 */ + Fc = frequency / (float)samplerate; + K = tan(M_PI * Fc); + norm = 1 / (1 + K / Q + K * K); + filter->a0 = 1 * norm; + filter->a1 = -2 * filter->a0; + filter->a2 = filter->a0; + filter->b1 = 2 * (K * K - 1) * norm; + filter->b2 = (1 - K / Q + K * K) * norm; +} + +static inline void iir_process(iir_filter_t *filter, float *samples, int length) +{ + float a0, a1, a2, b1, b2; + float z1, z2; + float in, out; + int i; + + /* get states */ + a0 = filter->a0; + a1 = filter->a1; + a2 = filter->a2; + b1 = filter->b1; + b2 = filter->b2; + + z1 = filter->z1; + z2 = filter->z2; + + /* process filter */ + for (i = 0; i < length; i++) { + /* add a small value, otherwise this loop will perform really bad on 0-samples */ + in = *samples + 0.000000001; + out = in * a0 + z1; + z1 = in * a1 + z2 - b1 * out; + z2 = in * a2 - b2 * out; + in = out; + *samples++ = in; + } + + filter->z1 = z1; + filter->z2 = z2; +} + +static inline void iir_reset(iir_filter_t *filter) +{ + filter->z1 = 0.0; + filter->z2 = 0.0; +} + +/* + * echo suppressor + */ + +echo_sup_state_t *echo_sup_create(void *ctx, int samplerate) +{ + echo_sup_state_t *es; + + es = talloc_zero(ctx, struct echo_sup_state); + if (!es) + return NULL; + + es->suppress_threshold = db2level(SUPPRESS_THRESHOLD); + es->release_suppress = db2level(RELEASE_SUPPRESS); + es->insertion_loss = db2level(INSERTION_LOSS); + + es->suppress_operate = floor(SUPPRESS_OPERATE * (double)samplerate); + es->suppress_hangover = floor(SUPPRESS_HANGOVER * (double)samplerate); + es->break_in_operate = floor(BREAK_IN_OPERATE * (double)samplerate); + es->break_in_hangover = floor(BREAK_IN_HANGOVER * (double)samplerate); + + iir_lowpass_init(&es->lp_send, LP_FREQUENCY, samplerate); + iir_lowpass_init(&es->lp_receive, LP_FREQUENCY, samplerate); + iir_highpass_init(&es->hp_send, HP_FREQUENCY, samplerate); + iir_highpass_init(&es->hp_receive, HP_FREQUENCY, samplerate); + iir_lowpass_init(&es->env_send, ENV_FREQUENCY, samplerate); + iir_lowpass_init(&es->env_receive, ENV_FREQUENCY, samplerate); + + return es; +} + +void echo_sup_free(echo_sup_state_t *es) +{ + talloc_free(es); +} + +/* rx is what we received from user interface, aka 'send' */ +/* tx is what we sent to user interface, aka 'receive' */ +int16_t echo_sup_update(echo_sup_state_t *es, int16_t tx, int16_t rx) +{ + float send, receive; + + /* convert from integer samples to float (@ 0 dBm scale) */ + receive = (float)tx / 23196.0; /* towards user interface */ + send = (float)rx / 23196.0; /* from user interface */ + + /* filter band */ + iir_process(&es->lp_send, &send, 1); + iir_process(&es->lp_receive, &receive, 1); + iir_process(&es->hp_send, &send, 1); + iir_process(&es->hp_receive, &receive, 1); + + /* get absolute value (rectifying with 3 dB gain) */ + send = fabsf(send) * 1.4142136; + receive = fabsf(receive) * 1.4142136; + + /* filter envelope */ + iir_process(&es->env_send, &send, 1); + iir_process(&es->env_receive, &receive, 1); + +#ifdef DEBUG_ES + static int debug_interval = 0; + float send_max = 0, receive_max = 0; + if (send > send_max) + send_max = send; + if (receive > receive_max) + receive_max = receive; + if (++debug_interval == 1000) { + printf("Level (Ls=%.0f Lr=%.0f)\n", level2db(send_max), level2db(receive_max)); + debug_interval = 0; + send_max = 0; + receive_max = 0; + } +#endif + + switch (es->state) { + case SUP_STATE_SILENCE: + if (receive <= es->suppress_threshold && send <= es->suppress_threshold) { + es->timer = 0; + break; + } + if (receive < send) { + if (++es->timer < es->break_in_operate) + break; +#ifdef DEBUG_ES + printf("Change from silence to break-in\n"); +#endif + es->state = SUP_STATE_BREAK_IN; + } else { + if (++es->timer < es->suppress_operate) + break; +#ifdef DEBUG_ES + printf("Change from silence to suppression\n"); +#endif + es->state = SUP_STATE_SUPPRESSION; + } + es->timer = 0; + break; + case SUP_STATE_SUPPRESSION: + if (receive <= es->release_suppress && send <= es->release_suppress) { + if (++es->timer < es->suppress_hangover) + break; +#ifdef DEBUG_ES + printf("Change from suppression to silence\n"); +#endif + es->state = SUP_STATE_SILENCE; + es->timer = 0; + break; + } + if (receive < send) { + if (++es->timer < es->break_in_operate) + break; +#ifdef DEBUG_ES + printf("Change from suppression to break-in\n"); +#endif + es->state = SUP_STATE_BREAK_IN; + } + es->timer = 0; + break; + case SUP_STATE_BREAK_IN: + if (receive <= es->suppress_threshold && send <= es->suppress_threshold) { + if (++es->timer < es->break_in_hangover) + break; +#ifdef DEBUG_ES + printf("Change from break-in to silence\n"); +#endif + es->state = SUP_STATE_SILENCE; + es->timer = 0; + break; + } + /* insert loss, so that receive level must be 6 dB higher than send level */ + if (receive / es->insertion_loss > send) { + if (++es->timer < es->break_in_hangover) + break; +#ifdef DEBUG_ES + printf("Change from break-in to suppression\n"); +#endif + es->state = SUP_STATE_SUPPRESSION; + } + es->timer = 0; + break; + } + + if (es->state == SUP_STATE_SUPPRESSION) + return 0; + return rx; +} + +void echo_sup_flush(echo_sup_state_t *es) +{ + es->state = SUP_STATE_SILENCE; + es->timer = 0; + + iir_reset(&es->lp_send); + iir_reset(&es->lp_receive); + iir_reset(&es->hp_send); + iir_reset(&es->hp_receive); + iir_reset(&es->env_send); + iir_reset(&es->env_receive); +} diff --git a/src/libecho/echo_suppress.h b/src/libecho/echo_suppress.h new file mode 100644 index 0000000..cbf0f82 --- /dev/null +++ b/src/libecho/echo_suppress.h @@ -0,0 +1,31 @@ + +typedef struct iir_filter { + float a0, a1, a2, b1, b2; + float z1, z2; +} iir_filter_t; + +enum sup_state { + SUP_STATE_SILENCE, + SUP_STATE_SUPPRESSION, + SUP_STATE_BREAK_IN, +}; + +typedef struct echo_sup_state { + float suppress_threshold; /* linear value of suppression th */ + float release_suppress; /* linear value of release suppression th */ + float insertion_loss; /* linear value of insertion loss */ + int suppress_operate; /* number of sample until transition */ + int suppress_hangover; + int break_in_operate; + int break_in_hangover; + enum sup_state state; /* current state */ + int timer; /* timer to delay transition */ + iir_filter_t lp_send, lp_receive; /* filter to cut off high frequencies */ + iir_filter_t hp_send, hp_receive; /* filter to cut off low frequencies */ + iir_filter_t env_send, env_receive; /* filter to get the envelope after rectifying */ +} echo_sup_state_t; + +echo_sup_state_t *echo_sup_create(void *ctx, int samplerate); +void echo_sup_free(echo_sup_state_t *es); +int16_t echo_sup_update(echo_sup_state_t *es, int16_t tx, int16_t rx); +void echo_sup_flush(echo_sup_state_t *es); diff --git a/tests/answer_detect.c b/tests/answer_detect.c index fb474d7..744485c 100644 --- a/tests/answer_detect.c +++ b/tests/answer_detect.c @@ -166,7 +166,7 @@ int main(void) 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); + rc = answertone_process(at, tone_int, SAMPLERATE * ANS_DURATION, true); if (rc > 0) printf("tone detected\n"); else