diff --git a/doc/tasks.md b/doc/tasks.md index 13004f74..bf1cfb5d 100644 --- a/doc/tasks.md +++ b/doc/tasks.md @@ -3,6 +3,22 @@ Сюда пишется список задач с короткой аннотацией. Если задача большая и имеет ТЗ - то ТЗ оформляется отдельным файлом, а сюда помещается аннотация и ссылка на ТЗ. ## Текущая задача +[+] **SpeexDSP AEC (эхоподавление) в lib/** — акустическое эхоподавление звонка. + Вендорен SpeexDSP (mdf/fftwrap/kiss_fft, 3-clause BSD) в `lib/speexdsp/`, + обёртка `lib/speex_aec.c/h` (C-API, FLOATING_POINT + USE_KISS_FFT, без внешних + зависимостей). Собственная линия задержки рендера на `delay_frames` кадров + + синхронный `speex_echo_cancellation` (встроенный буфер SpeexDSP фиксирован в + 2 кадра и для Android не годится); дрейф потоков — через переполнение/недозаполнение + линии (drop/dup + passthrough). Категория лога `aec`. + Ядро: `call_audio_set_aec_enabled/delay_frames`, AEC в feed_pcm (захват→encode) + и pull_pcm (рендер-референс), lifecycle в start/release. Desktop — настройка GUI + `aec_enabled` (`[chatserver]`, читается в `call_audio_start`). Android — включается + только на спикерфоне (`route==SPEAKER`, live через JNI `callAudioSetAec`), + задержка меряется `AudioTrack/AudioRecord.getTimestamp`; `CallAudioEngine.playLoop` + переведён на absolute-deadline (устранён систематический дрейф). Тест + `test_speex_aec` (подавление эха 25.5 дБ, сохранность речи 0.956, delay-line, NULL-гарды). + Сборки: autotools + chatgui CMake + Android CMake/APK. + [+] **Silero VAD в lib/** — добавлен детектор голосовой активности (Silero VAD v5, стриминговая ONNX-модель + обёртка `lib/silero_vad.c/h` над ONNX Runtime C API). API: `silero_vad_create/create_default/destroy/reset/process/process_pcm16` diff --git a/lib/Makefile.am b/lib/Makefile.am index b092b0b6..06110593 100644 --- a/lib/Makefile.am +++ b/lib/Makefile.am @@ -40,7 +40,13 @@ libuasync_a_SOURCES = \ dns.c \ dns.h \ async_dns.c \ - async_dns.h + async_dns.h \ + speex_aec.c \ + speex_aec.h \ + speexdsp/mdf.c \ + speexdsp/fftwrap.c \ + speexdsp/kiss_fft.c \ + speexdsp/kiss_fftr.c if HAVE_SILERO_VAD libuasync_a_SOURCES += \ @@ -53,10 +59,15 @@ libuasync_a_CFLAGS = \ -DDEBUG_OUTPUT_STDERR \ -DSQLITE_THREADSAFE=1 \ -DDNS_RANDOM=RAND_bytes \ + -DFLOATING_POINT \ + -DUSE_KISS_FFT \ + -DEXPORT= \ -g \ -I$(top_srcdir)/src \ -I$(top_srcdir)/lib \ - -I$(top_srcdir)/lib/libopus/include + -I$(top_srcdir)/lib/libopus/include \ + -I$(top_srcdir)/lib/speexdsp \ + -I$(top_srcdir)/lib/speexdsp/include if HAVE_SILERO_VAD libuasync_a_CFLAGS += -DHAVE_SILERO_VAD diff --git a/lib/debug_config.c b/lib/debug_config.c index 98ffe204..e4e6908d 100644 --- a/lib/debug_config.c +++ b/lib/debug_config.c @@ -135,6 +135,7 @@ static const struct { {"call", DEBUG_CATEGORY_CALL}, {"radio", DEBUG_CATEGORY_RADIO}, {"vad", DEBUG_CATEGORY_VAD}, + {"aec", DEBUG_CATEGORY_AEC}, {"all", DEBUG_CATEGORY_ALL}, {NULL, DEBUG_CATEGORY_NONE} }; diff --git a/lib/debug_config.h b/lib/debug_config.h index 7f43154b..3b58a6b6 100644 --- a/lib/debug_config.h +++ b/lib/debug_config.h @@ -73,7 +73,8 @@ typedef int debug_category_t; #define DEBUG_CATEGORY_CALL 32 // Call — P2P audio call (signaling + media) #define DEBUG_CATEGORY_RADIO 33 // Radio — PTT walkie-talkie multicast (voice) #define DEBUG_CATEGORY_VAD 34 // VAD — Silero voice activity detector (ONNX Runtime) -#define DEBUG_CATEGORY_COUNT 35 // Total number of categories +#define DEBUG_CATEGORY_AEC 35 // AEC — acoustic echo cancellation (SpeexDSP) +#define DEBUG_CATEGORY_COUNT 36 // Total number of categories #define DEBUG_CATEGORY_ALL (-1) // special value for all categories /* Debug configuration structure */ diff --git a/lib/speex_aec.c b/lib/speex_aec.c new file mode 100644 index 00000000..d5d7ee6b --- /dev/null +++ b/lib/speex_aec.c @@ -0,0 +1,169 @@ +// speex_aec.c — C-обёртка над SpeexDSP mdf.c (акустическое эхоподавление). +// +// API см. в speex_aec.h. Ключевые детали реализации: +// - собственная линия задержки рендера на depth кадров (SpeexDSP даёт только +// фиксированные 2 кадра — PLAYBACK_DELAY в mdf.c); +// - синхронный speex_echo_cancellation(rec, play_delayed, out) с выравниванием +// рендер↔захват по линии задержки; +// - дрейф независимых потоков: переполнение линии → отбрасываем старый кадр, +// недозаполнение (старт/underflow) → passthrough без канселлера. +// +// Захват и рендер приходят чанками разного размера (480/960), поэтому обе +// стороны накапливаются до полного кадра (frame_samples) внутри обёртки. + +#include "speex_aec.h" +#include "debug_config.h" +#include "mem.h" + +#include + +#include + +#define AEC_ID "speex_aec" + +struct speex_aec { + SpeexEchoState* st; + int frame_samples; /* сэмплов в кадре (960 @48кГц) */ + int depth; /* глубина линии задержки в кадрах (>=1) */ + int16_t* line; /* кольцо depth*frame_samples */ + int head; /* индекс самого старого кадра в line */ + int count; /* кадров в линии (0..depth) */ + int16_t* play_acc; /* аккумулятор рендера до кадра */ + int play_len; + int16_t* cap_acc; /* аккумулятор захвата до кадра */ + int cap_len; + uint32_t overruns; /* отброшено рендер-кадров (дрейф: рендер быстрее) */ + uint32_t underruns; /* passthrough-кадров (дрейф: рендер медленнее/старт) */ +}; + +/* Положить полный рендер-кадр в линию задержки (вытесняя старейший при переполнении). */ +static void aec_enqueue(speex_aec_t* a, const int16_t* frame) { + if (a->count == a->depth) { + a->head = (a->head + 1) % a->depth; + a->count--; + a->overruns++; + if (a->overruns == 1 || (a->overruns & 0xff) == 0) { + DEBUG_WARN(DEBUG_CATEGORY_AEC, + "%s: delay line overflow (render faster than capture) overruns=%u", AEC_ID, a->overruns); + } + } + memcpy(a->line + (size_t)((a->head + a->count) % a->depth) * a->frame_samples, + frame, (size_t)a->frame_samples * sizeof(int16_t)); + a->count++; +} + +speex_aec_t* speex_aec_create(int sample_rate, int frame_samples, int filter_samples, int delay_frames) { + speex_aec_t* a; + int rate; + + if (sample_rate <= 0 || frame_samples <= 0 || filter_samples < frame_samples) { + DEBUG_ERROR(DEBUG_CATEGORY_AEC, "%s: bad args rate=%d frame=%d filter=%d delay=%d", + AEC_ID, sample_rate, frame_samples, filter_samples, delay_frames); + return NULL; + } + + a = (speex_aec_t*)u_calloc(1, sizeof(*a)); + if (!a) { + DEBUG_ERROR(DEBUG_CATEGORY_AEC, "%s: OOM for state", AEC_ID); + return NULL; + } + + a->st = speex_echo_state_init(frame_samples, filter_samples); + rate = sample_rate; + if (speex_echo_ctl(a->st, SPEEX_ECHO_SET_SAMPLING_RATE, &rate) != 0) { + DEBUG_ERROR(DEBUG_CATEGORY_AEC, "%s: SET_SAMPLING_RATE(%d) failed", AEC_ID, sample_rate); + speex_echo_state_destroy(a->st); + u_free(a); + return NULL; + } + + a->frame_samples = frame_samples; + a->depth = delay_frames > 0 ? delay_frames : 1; + a->line = (int16_t*)u_calloc((uint32_t)(a->depth * frame_samples), sizeof(int16_t)); + a->play_acc = (int16_t*)u_calloc((uint32_t)frame_samples, sizeof(int16_t)); + a->cap_acc = (int16_t*)u_calloc((uint32_t)frame_samples, sizeof(int16_t)); + if (!a->line || !a->play_acc || !a->cap_acc) { + DEBUG_ERROR(DEBUG_CATEGORY_AEC, "%s: OOM for buffers", AEC_ID); + if (a->line) u_free(a->line); + if (a->play_acc) u_free(a->play_acc); + if (a->cap_acc) u_free(a->cap_acc); + speex_echo_state_destroy(a->st); + u_free(a); + return NULL; + } + + DEBUG_INFO(DEBUG_CATEGORY_AEC, "%s: created rate=%d frame=%d filter=%d delay=%d frames", + AEC_ID, sample_rate, frame_samples, filter_samples, a->depth); + return a; +} + +void speex_aec_destroy(speex_aec_t* a) { + if (!a) return; + speex_echo_state_destroy(a->st); + u_free(a->line); + u_free(a->play_acc); + u_free(a->cap_acc); + u_free(a); +} + +void speex_aec_reset(speex_aec_t* a) { + if (!a) return; + speex_echo_state_reset(a->st); + a->head = 0; + a->count = 0; + a->play_len = 0; + a->cap_len = 0; + DEBUG_INFO(DEBUG_CATEGORY_AEC, "%s: reset (filter + delay line)", AEC_ID); +} + +void speex_aec_feed_playback(speex_aec_t* a, const int16_t* pcm, int count) { + int fs, take; + + if (!a || !pcm || count <= 0) return; + fs = a->frame_samples; + + take = count; + if (a->play_len + count > fs) take = fs - a->play_len; + memcpy(a->play_acc + a->play_len, pcm, (size_t)take * sizeof(int16_t)); + a->play_len += take; + if (a->play_len < fs) return; + + aec_enqueue(a, a->play_acc); + a->play_len = 0; +} + +int speex_aec_process_capture(speex_aec_t* a, const int16_t* pcm, int count, int16_t* out) { + int fs, take; + + if (!a || !pcm || !out || count <= 0) return 0; + fs = a->frame_samples; + + take = count; + if (a->cap_len + count > fs) take = fs - a->cap_len; + memcpy(a->cap_acc + a->cap_len, pcm, (size_t)take * sizeof(int16_t)); + a->cap_len += take; + if (a->cap_len < fs) return 0; + + if (a->count == a->depth) { + const int16_t* ref = a->line + (size_t)a->head * fs; + speex_echo_cancellation(a->st, a->cap_acc, ref, out); + a->head = (a->head + 1) % a->depth; + a->count--; + } else { + /* линия не наполнена (старт или рендер отстаёт): passthrough без канселлера */ + if (a->underruns == 0 || (a->underruns & 0xff) == 0) { + DEBUG_WARN(DEBUG_CATEGORY_AEC, + "%s: delay line underflow (render slower than capture) underruns=%u fill=%d/%d", + AEC_ID, a->underruns, a->count, a->depth); + } + memcpy(out, a->cap_acc, (size_t)fs * sizeof(int16_t)); + a->underruns++; + } + + a->cap_len = 0; + return fs; +} + +int speex_aec_delay_fill(const speex_aec_t* a) { + return a ? a->count : 0; +} diff --git a/lib/speex_aec.h b/lib/speex_aec.h new file mode 100644 index 00000000..26462ed6 --- /dev/null +++ b/lib/speex_aec.h @@ -0,0 +1,74 @@ +/* + * speex_aec.h — акустическое эхоподавление (AEC), C-обёртка над SpeexDSP mdf.c. + * + * Подавляет эхо дальнего конца (то, что играем в динамик) в сигнале микрофона + * перед кодированием. Работает на int16 PCM, один канал, частота 48000 (можно + * 8000/16000/32000/48000), кадр 20 мс (960 сэмплов @48 кГц). + * + * Модель использования (duplex-контур звонка): + * - рендер (far-end, то что пошло в динамик) → speex_aec_feed_playback(); + * - захват (near-end, микрофон) → speex_aec_process_capture(). + * + * Обёртка держит собственную линию задержки рендера на `delay_frames` кадров и + * зовёт синхронный speex_echo_cancellation(rec, play_delayed, out). Это делает + * выравнивание рендер↔захват предсказуемым и настраиваемым (встроенный буфер + * SpeexDSP фиксирован в 2 кадра и для Android-задержки не годится). + * + * Дрейф двух независимых потоков (capture/play на Android) компенсируется + * ограниченной глубиной линии: переполнение → отбрасываем старый рендер-кадр, + * недозаполнение → passthrough без канселлера (с подробным логом категории "aec"). + * + * Зависимости: SpeexDSP (mdf.c/fftwrap.c/kiss_fft*) из lib/speexdsp, флаги + * FLOATING_POINT + USE_KISS_FFT. Собирается всегда (без внешних зависимостей). + * + * Лицензия SpeexDSP: 3-clause BSD (Xiph) — см. lib/speexdsp/COPYING. + */ +#ifndef SPEEX_AEC_H +#define SPEEX_AEC_H + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct speex_aec speex_aec_t; + +/** + * Создать эхоканселлер. + * sample_rate — 48000 (также 8000/16000/32000); + * frame_samples — сэмплов в кадре (960 = 20 мс @48 кГц); + * filter_samples— длина эхо-хвоста в сэмплах (14400 = 300 мс, кратно кадру); + * delay_frames — задержка рендер→захват в кадрах (>=0; 0 = без линии задержки). + * Возвращает NULL при ошибке (лог категории "aec"). + */ +speex_aec_t* speex_aec_create(int sample_rate, int frame_samples, int filter_samples, int delay_frames); + +/* Освободить канселлер. NULL безопасен. */ +void speex_aec_destroy(speex_aec_t* aec); + +/* Сбросить адаптивный фильтр и линию задержки (смена устройства/роута). */ +void speex_aec_reset(speex_aec_t* aec); + +/** + * Подать рендер (дальний конец). Накопление до кадра внутри; полный кадр + * кладётся в линию задержки. count может быть любым (обычно 480 или 960). + */ +void speex_aec_feed_playback(speex_aec_t* aec, const int16_t* pcm, int count); + +/** + * Обработать захват (микрофон). Накопление до кадра внутри. Когда кадр собран — + * подавляет эхо (с учётом линии задержки) и пишет результат в out. + * Возвращает число сэмплов, записанных в out (frame_samples, если кадр готов; + * 0 — кадр ещё накапливается). out не должен алиаситься с pcm. + */ +int speex_aec_process_capture(speex_aec_t* aec, const int16_t* pcm, int count, int16_t* out); + +/* Актуальная глубина линии задержки (кадров) — для диагностики дрейфа. */ +int speex_aec_delay_fill(const speex_aec_t* aec); + +#ifdef __cplusplus +} +#endif + +#endif /* SPEEX_AEC_H */ diff --git a/lib/speexdsp/COPYING b/lib/speexdsp/COPYING new file mode 100644 index 00000000..7d6dc4aa --- /dev/null +++ b/lib/speexdsp/COPYING @@ -0,0 +1,35 @@ +Copyright 2002-2008 Xiph.org Foundation +Copyright 2002-2008 Jean-Marc Valin +Copyright 2005-2007 Analog Devices Inc. +Copyright 2005-2008 Commonwealth Scientific and Industrial Research + Organisation (CSIRO) +Copyright 1993, 2002, 2006 David Rowe +Copyright 2003 EpicGames +Copyright 1992-1994 Jutta Degener, Carsten Bormann + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions +are met: + +- Redistributions of source code must retain the above copyright +notice, this list of conditions and the following disclaimer. + +- Redistributions in binary form must reproduce the above copyright +notice, this list of conditions and the following disclaimer in the +documentation and/or other materials provided with the distribution. + +- Neither the name of the Xiph.org Foundation nor the names of its +contributors may be used to endorse or promote products derived from +this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE FOUNDATION OR +CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, +EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, +PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/lib/speexdsp/_kiss_fft_guts.h b/lib/speexdsp/_kiss_fft_guts.h new file mode 100644 index 00000000..f5f2a5fd --- /dev/null +++ b/lib/speexdsp/_kiss_fft_guts.h @@ -0,0 +1,160 @@ +/* +Copyright (c) 2003-2004, Mark Borgerding + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. + * Neither the author nor the names of any contributors may be used to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#define MIN(a,b) ((a)<(b) ? (a):(b)) +#define MAX(a,b) ((a)>(b) ? (a):(b)) + +/* kiss_fft.h + defines kiss_fft_scalar as either short or a float type + and defines + typedef struct { kiss_fft_scalar r; kiss_fft_scalar i; }kiss_fft_cpx; */ +#include "kiss_fft.h" +#include "math_approx.h" + +#define MAXFACTORS 32 +/* e.g. an fft of length 128 has 4 factors + as far as kissfft is concerned + 4*4*4*2 + */ + +struct kiss_fft_state{ + int nfft; + int inverse; + int factors[2*MAXFACTORS]; + kiss_fft_cpx twiddles[1]; +}; + +/* + Explanation of macros dealing with complex math: + + C_MUL(m,a,b) : m = a*b + C_FIXDIV( c , div ) : if a fixed point impl., c /= div. noop otherwise + C_SUB( res, a,b) : res = a - b + C_SUBFROM( res , a) : res -= a + C_ADDTO( res , a) : res += a + * */ +#ifdef FIXED_POINT +#include "arch.h" +# define FRACBITS 15 +# define SAMPPROD spx_int32_t +#define SAMP_MAX 32767 + +#define SAMP_MIN -SAMP_MAX + +#if defined(CHECK_OVERFLOW) +# define CHECK_OVERFLOW_OP(a,op,b) \ + if ( (SAMPPROD)(a) op (SAMPPROD)(b) > SAMP_MAX || (SAMPPROD)(a) op (SAMPPROD)(b) < SAMP_MIN ) { \ + fprintf(stderr,"WARNING:overflow @ " __FILE__ "(%d): (%d " #op" %d) = %ld\n",__LINE__,(a),(b),(SAMPPROD)(a) op (SAMPPROD)(b) ); } +#endif + + +# define smul(a,b) ( (SAMPPROD)(a)*(b) ) +# define sround( x ) (kiss_fft_scalar)( ( (x) + (1<<(FRACBITS-1)) ) >> FRACBITS ) + +# define S_MUL(a,b) sround( smul(a,b) ) + +# define C_MUL(m,a,b) \ + do{ (m).r = sround( smul((a).r,(b).r) - smul((a).i,(b).i) ); \ + (m).i = sround( smul((a).r,(b).i) + smul((a).i,(b).r) ); }while(0) + +# define C_MUL4(m,a,b) \ + do{ (m).r = PSHR32( smul((a).r,(b).r) - smul((a).i,(b).i),17 ); \ + (m).i = PSHR32( smul((a).r,(b).i) + smul((a).i,(b).r),17 ); }while(0) + +# define DIVSCALAR(x,k) \ + (x) = sround( smul( x, SAMP_MAX/k ) ) + +# define C_FIXDIV(c,div) \ + do { DIVSCALAR( (c).r , div); \ + DIVSCALAR( (c).i , div); }while (0) + +# define C_MULBYSCALAR( c, s ) \ + do{ (c).r = sround( smul( (c).r , s ) ) ;\ + (c).i = sround( smul( (c).i , s ) ) ; }while(0) + +#else /* not FIXED_POINT*/ + +# define S_MUL(a,b) ( (a)*(b) ) +#define C_MUL(m,a,b) \ + do{ (m).r = (a).r*(b).r - (a).i*(b).i;\ + (m).i = (a).r*(b).i + (a).i*(b).r; }while(0) + +#define C_MUL4(m,a,b) C_MUL(m,a,b) + +# define C_FIXDIV(c,div) /* NOOP */ +# define C_MULBYSCALAR( c, s ) \ + do{ (c).r *= (s);\ + (c).i *= (s); }while(0) +#endif + +#ifndef CHECK_OVERFLOW_OP +# define CHECK_OVERFLOW_OP(a,op,b) /* noop */ +#endif + +#define C_ADD( res, a,b)\ + do { \ + CHECK_OVERFLOW_OP((a).r,+,(b).r)\ + CHECK_OVERFLOW_OP((a).i,+,(b).i)\ + (res).r=(a).r+(b).r; (res).i=(a).i+(b).i; \ + }while(0) +#define C_SUB( res, a,b)\ + do { \ + CHECK_OVERFLOW_OP((a).r,-,(b).r)\ + CHECK_OVERFLOW_OP((a).i,-,(b).i)\ + (res).r=(a).r-(b).r; (res).i=(a).i-(b).i; \ + }while(0) +#define C_ADDTO( res , a)\ + do { \ + CHECK_OVERFLOW_OP((res).r,+,(a).r)\ + CHECK_OVERFLOW_OP((res).i,+,(a).i)\ + (res).r += (a).r; (res).i += (a).i;\ + }while(0) + +#define C_SUBFROM( res , a)\ + do {\ + CHECK_OVERFLOW_OP((res).r,-,(a).r)\ + CHECK_OVERFLOW_OP((res).i,-,(a).i)\ + (res).r -= (a).r; (res).i -= (a).i; \ + }while(0) + + +#ifdef FIXED_POINT +# define KISS_FFT_COS(phase) floor(MIN(32767,MAX(-32767,.5+32768 * cos (phase)))) +# define KISS_FFT_SIN(phase) floor(MIN(32767,MAX(-32767,.5+32768 * sin (phase)))) +# define HALF_OF(x) ((x)>>1) +#elif defined(USE_SIMD) +# define KISS_FFT_COS(phase) _mm_set1_ps( cos(phase) ) +# define KISS_FFT_SIN(phase) _mm_set1_ps( sin(phase) ) +# define HALF_OF(x) ((x)*_mm_set1_ps(.5)) +#else +# define KISS_FFT_COS(phase) (kiss_fft_scalar) cos(phase) +# define KISS_FFT_SIN(phase) (kiss_fft_scalar) sin(phase) +# define HALF_OF(x) ((x)*.5) +#endif + +#define kf_cexp(x,phase) \ + do{ \ + (x)->r = KISS_FFT_COS(phase);\ + (x)->i = KISS_FFT_SIN(phase);\ + }while(0) +#define kf_cexp2(x,phase) \ + do{ \ + (x)->r = spx_cos_norm((phase));\ + (x)->i = spx_cos_norm((phase)-32768);\ +}while(0) + + +/* a debugging function */ +#define pcpx(c)\ + fprintf(stderr,"%g + %gi\n",(double)((c)->r),(double)((c)->i) ) diff --git a/lib/speexdsp/arch.h b/lib/speexdsp/arch.h new file mode 100644 index 00000000..1cac3d9c --- /dev/null +++ b/lib/speexdsp/arch.h @@ -0,0 +1,232 @@ +/* Copyright (C) 2003 Jean-Marc Valin */ +/** + @file arch.h + @brief Various architecture definitions Speex +*/ +/* + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions + are met: + + - Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + + - Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + + - Neither the name of the Xiph.org Foundation nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE FOUNDATION OR + CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#ifndef ARCH_H +#define ARCH_H + +/* A couple test to catch stupid option combinations */ +#ifdef FIXED_POINT + +#ifdef FLOATING_POINT +#error You cannot compile as floating point and fixed point at the same time +#endif +#ifdef USE_SSE +#error SSE is only for floating-point +#endif +#if defined(ARM4_ASM) + defined(ARM5E_ASM) + defined(BFIN_ASM) > 1 +#error Make up your mind. What CPU do you have? +#endif +#ifdef VORBIS_PSYCHO +#error Vorbis-psy model currently not implemented in fixed-point +#endif + +#else + +#ifndef FLOATING_POINT +#error You now need to define either FIXED_POINT or FLOATING_POINT +#endif +#if defined(ARM4_ASM) || defined(ARM5E_ASM) || defined(BFIN_ASM) +#error I suppose you can have a [ARM4/ARM5E/Blackfin] that has float instructions? +#endif +#ifdef FIXED_DEBUG +#error "Don't you think enabling fixed-point is a good thing to do if you want to debug that?" +#endif + + +#endif + +#ifndef OUTSIDE_SPEEX +#include "speex/speexdsp_types.h" +#endif + +#define ABS(x) ((x) < 0 ? (-(x)) : (x)) /**< Absolute integer value. */ +#define ABS16(x) ((x) < 0 ? (-(x)) : (x)) /**< Absolute 16-bit value. */ +#define MIN16(a,b) ((a) < (b) ? (a) : (b)) /**< Maximum 16-bit value. */ +#define MAX16(a,b) ((a) > (b) ? (a) : (b)) /**< Maximum 16-bit value. */ +#define ABS32(x) ((x) < 0 ? (-(x)) : (x)) /**< Absolute 32-bit value. */ +#define MIN32(a,b) ((a) < (b) ? (a) : (b)) /**< Maximum 32-bit value. */ +#define MAX32(a,b) ((a) > (b) ? (a) : (b)) /**< Maximum 32-bit value. */ + +#ifdef FIXED_POINT + +typedef spx_int16_t spx_word16_t; +typedef spx_int32_t spx_word32_t; +typedef spx_word32_t spx_mem_t; +typedef spx_word16_t spx_coef_t; +typedef spx_word16_t spx_lsp_t; +typedef spx_word32_t spx_sig_t; + +#define Q15ONE 32767 + +#define LPC_SCALING 8192 +#define SIG_SCALING 16384 +#define LSP_SCALING 8192. +#define GAMMA_SCALING 32768. +#define GAIN_SCALING 64 +#define GAIN_SCALING_1 0.015625 + +#define LPC_SHIFT 13 +#define LSP_SHIFT 13 +#define SIG_SHIFT 14 +#define GAIN_SHIFT 6 + +#define WORD2INT(x) ((x) < -32767 ? -32768 : ((x) > 32766 ? 32767 : (x))) + +#define VERY_SMALL 0 +#define VERY_LARGE32 ((spx_word32_t)2147483647) +#define VERY_LARGE16 ((spx_word16_t)32767) +#define Q15_ONE ((spx_word16_t)32767) + + +#ifdef FIXED_DEBUG +#include "fixed_debug.h" +#else + +#include "fixed_generic.h" + +#ifdef ARM5E_ASM +#include "fixed_arm5e.h" +#elif defined(ARM4_ASM) +#include "fixed_arm4.h" +#elif defined(BFIN_ASM) +#include "fixed_bfin.h" +#endif + +#endif + + +#else + +typedef float spx_mem_t; +typedef float spx_coef_t; +typedef float spx_lsp_t; +typedef float spx_sig_t; +typedef float spx_word16_t; +typedef float spx_word32_t; + +#define Q15ONE 1.0f +#define LPC_SCALING 1.f +#define SIG_SCALING 1.f +#define LSP_SCALING 1.f +#define GAMMA_SCALING 1.f +#define GAIN_SCALING 1.f +#define GAIN_SCALING_1 1.f + + +#define VERY_SMALL 1e-15f +#define VERY_LARGE32 1e15f +#define VERY_LARGE16 1e15f +#define Q15_ONE ((spx_word16_t)1.f) + +#define QCONST16(x,bits) (x) +#define QCONST32(x,bits) (x) + +#define NEG16(x) (-(x)) +#define NEG32(x) (-(x)) +#define EXTRACT16(x) (x) +#define EXTEND32(x) (x) +#define SHR16(a,shift) (a) +#define SHL16(a,shift) (a) +#define SHR32(a,shift) (a) +#define SHL32(a,shift) (a) +#define PSHR16(a,shift) (a) +#define PSHR32(a,shift) (a) +#define VSHR32(a,shift) (a) +#define SATURATE16(x,a) (x) +#define SATURATE32(x,a) (x) +#define SATURATE32PSHR(x,shift,a) (x) + +#define PSHR(a,shift) (a) +#define SHR(a,shift) (a) +#define SHL(a,shift) (a) +#define SATURATE(x,a) (x) + +#define ADD16(a,b) ((a)+(b)) +#define SUB16(a,b) ((a)-(b)) +#define ADD32(a,b) ((a)+(b)) +#define SUB32(a,b) ((a)-(b)) +#define MULT16_16_16(a,b) ((a)*(b)) +#define MULT16_32_32(a,b) ((a)*(b)) +#define MULT16_16(a,b) ((spx_word32_t)(a)*(spx_word32_t)(b)) +#define MAC16_16(c,a,b) ((c)+(spx_word32_t)(a)*(spx_word32_t)(b)) + +#define MULT16_32_Q15(a,b) ((a)*(b)) +#define MULT16_32_P15(a,b) ((a)*(b)) + +#define MAC16_32_Q15(c,a,b) ((c)+(a)*(b)) + +#define MAC16_16_Q11(c,a,b) ((c)+(a)*(b)) +#define MAC16_16_Q13(c,a,b) ((c)+(a)*(b)) +#define MAC16_16_P13(c,a,b) ((c)+(a)*(b)) +#define MULT16_16_Q11_32(a,b) ((a)*(b)) +#define MULT16_16_Q13(a,b) ((a)*(b)) +#define MULT16_16_Q14(a,b) ((a)*(b)) +#define MULT16_16_Q15(a,b) ((a)*(b)) +#define MULT16_16_P15(a,b) ((a)*(b)) +#define MULT16_16_P13(a,b) ((a)*(b)) +#define MULT16_16_P14(a,b) ((a)*(b)) + +#define DIV32_16(a,b) (((spx_word32_t)(a))/(spx_word16_t)(b)) +#define PDIV32_16(a,b) (((spx_word32_t)(a))/(spx_word16_t)(b)) +#define DIV32(a,b) (((spx_word32_t)(a))/(spx_word32_t)(b)) +#define PDIV32(a,b) (((spx_word32_t)(a))/(spx_word32_t)(b)) + +#define WORD2INT(x) ((x) < -32767.5f ? -32768 : \ + ((x) > 32766.5f ? 32767 : (spx_int16_t)floor(.5 + (x)))) +#endif + + +#if defined(CONFIG_TI_C54X) || defined(CONFIG_TI_C55X) + +/* 2 on TI C5x DSP */ +#define BYTES_PER_CHAR 2 +#define BITS_PER_CHAR 16 +#define LOG2_BITS_PER_CHAR 4 + +#else + +#define BYTES_PER_CHAR 1 +#define BITS_PER_CHAR 8 +#define LOG2_BITS_PER_CHAR 3 + +#endif + + + +#ifdef FIXED_DEBUG +extern long long spx_mips; +#endif + + +#endif diff --git a/lib/speexdsp/fftwrap.c b/lib/speexdsp/fftwrap.c new file mode 100644 index 00000000..4573479d --- /dev/null +++ b/lib/speexdsp/fftwrap.c @@ -0,0 +1,448 @@ +/* Copyright (C) 2005-2006 Jean-Marc Valin + File: fftwrap.c + + Wrapper for various FFTs + + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions + are met: + + - Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + + - Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + + - Neither the name of the Xiph.org Foundation nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE FOUNDATION OR + CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +*/ + +#ifdef HAVE_CONFIG_H +#include "config.h" +#endif + +#include "arch.h" +#include "os_support.h" + +#define MAX_FFT_SIZE 2048 + +#ifdef FIXED_POINT +static int maximize_range(spx_word16_t *in, spx_word16_t *out, spx_word16_t bound, int len) +{ + int i, shift; + spx_word16_t max_val = 0; + for (i=0;imax_val) + max_val = in[i]; + if (-in[i]>max_val) + max_val = -in[i]; + } + shift=0; + while (max_val <= (bound>>1) && max_val != 0) + { + max_val <<= 1; + shift++; + } + for (i=0;i + +void *spx_fft_init(int size) +{ + struct drft_lookup *table; + table = speex_alloc(sizeof(struct drft_lookup)); + spx_drft_init((struct drft_lookup *)table, size); + return (void*)table; +} + +void spx_fft_destroy(void *table) +{ + spx_drft_clear(table); + speex_free(table); +} + +void spx_fft(void *table, float *in, float *out) +{ + if (in==out) + { + int i; + float scale = 1./((struct drft_lookup *)table)->n; + speex_warning("FFT should not be done in-place"); + for (i=0;i<((struct drft_lookup *)table)->n;i++) + out[i] = scale*in[i]; + } else { + int i; + float scale = 1./((struct drft_lookup *)table)->n; + for (i=0;i<((struct drft_lookup *)table)->n;i++) + out[i] = scale*in[i]; + } + spx_drft_forward((struct drft_lookup *)table, out); +} + +void spx_ifft(void *table, float *in, float *out) +{ + if (in==out) + { + speex_warning("FFT should not be done in-place"); + } else { + int i; + for (i=0;i<((struct drft_lookup *)table)->n;i++) + out[i] = in[i]; + } + spx_drft_backward((struct drft_lookup *)table, out); +} + +#elif defined(USE_INTEL_MKL) +#include + +struct mkl_config { + DFTI_DESCRIPTOR_HANDLE desc; + int N; +}; + +void *spx_fft_init(int size) +{ + struct mkl_config *table = (struct mkl_config *) speex_alloc(sizeof(struct mkl_config)); + table->N = size; + DftiCreateDescriptor(&table->desc, DFTI_SINGLE, DFTI_REAL, 1, size); + DftiSetValue(table->desc, DFTI_PACKED_FORMAT, DFTI_PACK_FORMAT); + DftiSetValue(table->desc, DFTI_PLACEMENT, DFTI_NOT_INPLACE); + DftiSetValue(table->desc, DFTI_FORWARD_SCALE, 1.0f / size); + DftiCommitDescriptor(table->desc); + return table; +} + +void spx_fft_destroy(void *table) +{ + struct mkl_config *t = (struct mkl_config *) table; + DftiFreeDescriptor(t->desc); + speex_free(table); +} + +void spx_fft(void *table, spx_word16_t *in, spx_word16_t *out) +{ + struct mkl_config *t = (struct mkl_config *) table; + DftiComputeForward(t->desc, in, out); +} + +void spx_ifft(void *table, spx_word16_t *in, spx_word16_t *out) +{ + struct mkl_config *t = (struct mkl_config *) table; + DftiComputeBackward(t->desc, in, out); +} + +#elif defined(USE_INTEL_IPP) + +#include + +struct ipp_fft_config +{ + IppsDFTSpec_R_32f *dftSpec; + Ipp8u *buffer; +}; + +void *spx_fft_init(int size) +{ + int bufferSize = 0; + int hint; + struct ipp_fft_config *table; + + table = (struct ipp_fft_config *)speex_alloc(sizeof(struct ipp_fft_config)); + + /* there appears to be no performance difference between ippAlgHintFast and + ippAlgHintAccurate when using the with the floating point version + of the fft. */ + hint = ippAlgHintAccurate; + + ippsDFTInitAlloc_R_32f(&table->dftSpec, size, IPP_FFT_DIV_FWD_BY_N, hint); + + ippsDFTGetBufSize_R_32f(table->dftSpec, &bufferSize); + table->buffer = ippsMalloc_8u(bufferSize); + + return table; +} + +void spx_fft_destroy(void *table) +{ + struct ipp_fft_config *t = (struct ipp_fft_config *)table; + ippsFree(t->buffer); + ippsDFTFree_R_32f(t->dftSpec); + speex_free(t); +} + +void spx_fft(void *table, spx_word16_t *in, spx_word16_t *out) +{ + struct ipp_fft_config *t = (struct ipp_fft_config *)table; + ippsDFTFwd_RToPack_32f(in, out, t->dftSpec, t->buffer); +} + +void spx_ifft(void *table, spx_word16_t *in, spx_word16_t *out) +{ + struct ipp_fft_config *t = (struct ipp_fft_config *)table; + ippsDFTInv_PackToR_32f(in, out, t->dftSpec, t->buffer); +} + +#elif defined(USE_GPL_FFTW3) + +#include + +struct fftw_config { + float *in; + float *out; + fftwf_plan fft; + fftwf_plan ifft; + int N; +}; + +void *spx_fft_init(int size) +{ + struct fftw_config *table = (struct fftw_config *) speex_alloc(sizeof(struct fftw_config)); + table->in = fftwf_malloc(sizeof(float) * (size+2)); + table->out = fftwf_malloc(sizeof(float) * (size+2)); + + table->fft = fftwf_plan_dft_r2c_1d(size, table->in, (fftwf_complex *) table->out, FFTW_PATIENT); + table->ifft = fftwf_plan_dft_c2r_1d(size, (fftwf_complex *) table->in, table->out, FFTW_PATIENT); + + table->N = size; + return table; +} + +void spx_fft_destroy(void *table) +{ + struct fftw_config *t = (struct fftw_config *) table; + fftwf_destroy_plan(t->fft); + fftwf_destroy_plan(t->ifft); + fftwf_free(t->in); + fftwf_free(t->out); + speex_free(table); +} + + +void spx_fft(void *table, spx_word16_t *in, spx_word16_t *out) +{ + int i; + struct fftw_config *t = (struct fftw_config *) table; + const int N = t->N; + float *iptr = t->in; + float *optr = t->out; + const float m = 1.0 / N; + for(i=0;ifft); + + out[0] = optr[0]; + for(i=1;iN; + float *iptr = t->in; + float *optr = t->out; + + iptr[0] = in[0]; + iptr[1] = 0.0f; + for(i=1;iifft); + + for(i=0;iforward = kiss_fftr_alloc(size,0,NULL,NULL); + table->backward = kiss_fftr_alloc(size,1,NULL,NULL); + table->N = size; + return table; +} + +void spx_fft_destroy(void *table) +{ + struct kiss_config *t = (struct kiss_config *)table; + kiss_fftr_free(t->forward); + kiss_fftr_free(t->backward); + speex_free(table); +} + +#ifdef FIXED_POINT + +void spx_fft(void *table, spx_word16_t *in, spx_word16_t *out) +{ + int shift; + struct kiss_config *t = (struct kiss_config *)table; + shift = maximize_range(in, in, 32000, t->N); + kiss_fftr2(t->forward, in, out); + renorm_range(in, in, shift, t->N); + renorm_range(out, out, shift, t->N); +} + +#else + +void spx_fft(void *table, spx_word16_t *in, spx_word16_t *out) +{ + int i; + float scale; + struct kiss_config *t = (struct kiss_config *)table; + scale = 1./t->N; + kiss_fftr2(t->forward, in, out); + for (i=0;iN;i++) + out[i] *= scale; +} +#endif + +void spx_ifft(void *table, spx_word16_t *in, spx_word16_t *out) +{ + struct kiss_config *t = (struct kiss_config *)table; + kiss_fftri2(t->backward, in, out); +} + + +#else + +#error No other FFT implemented + +#endif + + +#ifdef FIXED_POINT +/*#include "smallft.h"*/ + + +void spx_fft_float(void *table, float *in, float *out) +{ + int i; +#ifdef USE_SMALLFT + int N = ((struct drft_lookup *)table)->n; +#elif defined(USE_KISS_FFT) + int N = ((struct kiss_config *)table)->N; +#else +#endif +#ifdef VAR_ARRAYS + spx_word16_t _in[N]; + spx_word16_t _out[N]; +#else + spx_word16_t _in[MAX_FFT_SIZE]; + spx_word16_t _out[MAX_FFT_SIZE]; +#endif + for (i=0;iN); + scale = 1./((struct kiss_config *)table)->N; + for (i=0;i<((struct kiss_config *)table)->N;i++) + out[i] = scale*in[i]; + spx_drft_forward(&t, out); + spx_drft_clear(&t); + } +#endif +} + +void spx_ifft_float(void *table, float *in, float *out) +{ + int i; +#ifdef USE_SMALLFT + int N = ((struct drft_lookup *)table)->n; +#elif defined(USE_KISS_FFT) + int N = ((struct kiss_config *)table)->N; +#else +#endif +#ifdef VAR_ARRAYS + spx_word16_t _in[N]; + spx_word16_t _out[N]; +#else + spx_word16_t _in[MAX_FFT_SIZE]; + spx_word16_t _out[MAX_FFT_SIZE]; +#endif + for (i=0;iN); + for (i=0;i<((struct kiss_config *)table)->N;i++) + out[i] = in[i]; + spx_drft_backward(&t, out); + spx_drft_clear(&t); + } +#endif +} + +#else + +void spx_fft_float(void *table, float *in, float *out) +{ + spx_fft(table, in, out); +} +void spx_ifft_float(void *table, float *in, float *out) +{ + spx_ifft(table, in, out); +} + +#endif diff --git a/lib/speexdsp/fftwrap.h b/lib/speexdsp/fftwrap.h new file mode 100644 index 00000000..2c074255 --- /dev/null +++ b/lib/speexdsp/fftwrap.h @@ -0,0 +1,58 @@ +/* Copyright (C) 2005 Jean-Marc Valin + File: fftwrap.h + + Wrapper for various FFTs + + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions + are met: + + - Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + + - Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + + - Neither the name of the Xiph.org Foundation nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE FOUNDATION OR + CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +*/ + +#ifndef FFTWRAP_H +#define FFTWRAP_H + +#include "arch.h" + +/** Compute tables for an FFT */ +void *spx_fft_init(int size); + +/** Destroy tables for an FFT */ +void spx_fft_destroy(void *table); + +/** Forward (real to half-complex) transform */ +void spx_fft(void *table, spx_word16_t *in, spx_word16_t *out); + +/** Backward (half-complex to real) transform */ +void spx_ifft(void *table, spx_word16_t *in, spx_word16_t *out); + +/** Forward (real to half-complex) transform of float data */ +void spx_fft_float(void *table, float *in, float *out); + +/** Backward (half-complex to real) transform of float data */ +void spx_ifft_float(void *table, float *in, float *out); + +#endif diff --git a/lib/speexdsp/include/speex/speex_echo.h b/lib/speexdsp/include/speex/speex_echo.h new file mode 100644 index 00000000..4c1aa5a5 --- /dev/null +++ b/lib/speexdsp/include/speex/speex_echo.h @@ -0,0 +1,170 @@ +/* Copyright (C) Jean-Marc Valin */ +/** + @file speex_echo.h + @brief Echo cancellation +*/ +/* + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions are + met: + + 1. Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. + + 2. Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + + 3. The name of the author may not be used to endorse or promote products + derived from this software without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR + IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES + OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, + INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + POSSIBILITY OF SUCH DAMAGE. +*/ + +#ifndef SPEEX_ECHO_H +#define SPEEX_ECHO_H +/** @defgroup SpeexEchoState SpeexEchoState: Acoustic echo canceller + * This is the acoustic echo canceller module. + * @{ + */ +#include "speexdsp_types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/** Obtain frame size used by the AEC */ +#define SPEEX_ECHO_GET_FRAME_SIZE 3 + +/** Set sampling rate */ +#define SPEEX_ECHO_SET_SAMPLING_RATE 24 +/** Get sampling rate */ +#define SPEEX_ECHO_GET_SAMPLING_RATE 25 + +/* Can't set window sizes */ +/** Get size of impulse response (int32) */ +#define SPEEX_ECHO_GET_IMPULSE_RESPONSE_SIZE 27 + +/* Can't set window content */ +/** Get impulse response (int32[]) */ +#define SPEEX_ECHO_GET_IMPULSE_RESPONSE 29 + +/** Internal echo canceller state. Should never be accessed directly. */ +struct SpeexEchoState_; + +/** @class SpeexEchoState + * This holds the state of the echo canceller. You need one per channel. +*/ + +/** Internal echo canceller state. Should never be accessed directly. */ +typedef struct SpeexEchoState_ SpeexEchoState; + +/** Creates a new echo canceller state + * @param frame_size Number of samples to process at one time (should correspond to 10-20 ms) + * @param filter_length Number of samples of echo to cancel (should generally correspond to 100-500 ms) + * @return Newly-created echo canceller state + */ +SpeexEchoState *speex_echo_state_init(int frame_size, int filter_length); + +/** Creates a new multi-channel echo canceller state + * @param frame_size Number of samples to process at one time (should correspond to 10-20 ms) + * @param filter_length Number of samples of echo to cancel (should generally correspond to 100-500 ms) + * @param nb_mic Number of microphone channels + * @param nb_speakers Number of speaker channels + * @return Newly-created echo canceller state + */ +SpeexEchoState *speex_echo_state_init_mc(int frame_size, int filter_length, int nb_mic, int nb_speakers); + +/** Destroys an echo canceller state + * @param st Echo canceller state +*/ +void speex_echo_state_destroy(SpeexEchoState *st); + +/** Performs echo cancellation a frame, based on the audio sent to the speaker (no delay is added + * to playback in this form) + * + * @param st Echo canceller state + * @param rec Signal from the microphone (near end + far end echo) + * @param play Signal played to the speaker (received from far end) + * @param out Returns near-end signal with echo removed + */ +void speex_echo_cancellation(SpeexEchoState *st, const spx_int16_t *rec, const spx_int16_t *play, spx_int16_t *out); + +/** Performs echo cancellation a frame (deprecated) */ +void speex_echo_cancel(SpeexEchoState *st, const spx_int16_t *rec, const spx_int16_t *play, spx_int16_t *out, spx_int32_t *Yout); + +/** Perform echo cancellation using internal playback buffer, which is delayed by two frames + * to account for the delay introduced by most soundcards (but it could be off!) + * @param st Echo canceller state + * @param rec Signal from the microphone (near end + far end echo) + * @param out Returns near-end signal with echo removed +*/ +void speex_echo_capture(SpeexEchoState *st, const spx_int16_t *rec, spx_int16_t *out); + +/** Let the echo canceller know that a frame was just queued to the soundcard + * @param st Echo canceller state + * @param play Signal played to the speaker (received from far end) +*/ +void speex_echo_playback(SpeexEchoState *st, const spx_int16_t *play); + +/** Reset the echo canceller to its original state + * @param st Echo canceller state + */ +void speex_echo_state_reset(SpeexEchoState *st); + +/** Used like the ioctl function to control the echo canceller parameters + * + * @param st Echo canceller state + * @param request ioctl-type request (one of the SPEEX_ECHO_* macros) + * @param ptr Data exchanged to-from function + * @return 0 if no error, -1 if request in unknown + */ +int speex_echo_ctl(SpeexEchoState *st, int request, void *ptr); + + + +struct SpeexDecorrState_; + +typedef struct SpeexDecorrState_ SpeexDecorrState; + + +/** Create a state for the channel decorrelation algorithm + This is useful for multi-channel echo cancellation only + * @param rate Sampling rate + * @param channels Number of channels (it's a bit pointless if you don't have at least 2) + * @param frame_size Size of the frame to process at ones (counting samples *per* channel) +*/ +SpeexDecorrState *speex_decorrelate_new(int rate, int channels, int frame_size); + +/** Remove correlation between the channels by modifying the phase and possibly + adding noise in a way that is not (or little) perceptible. + * @param st Decorrelator state + * @param in Input audio in interleaved format + * @param out Result of the decorrelation (out *may* alias in) + * @param strength How much alteration of the audio to apply from 0 to 100. +*/ +void speex_decorrelate(SpeexDecorrState *st, const spx_int16_t *in, spx_int16_t *out, int strength); + +/** Destroy a Decorrelation state + * @param st State to destroy +*/ +void speex_decorrelate_destroy(SpeexDecorrState *st); + + +#ifdef __cplusplus +} +#endif + + +/** @}*/ +#endif diff --git a/lib/speexdsp/include/speex/speexdsp_config_types.h b/lib/speexdsp/include/speex/speexdsp_config_types.h new file mode 100644 index 00000000..f9e8aad5 --- /dev/null +++ b/lib/speexdsp/include/speex/speexdsp_config_types.h @@ -0,0 +1,14 @@ +/* speexdsp_config_types.h — статически сгенерировано для POSIX (Linux/FreeBSD/Android). + * В оригинале генерируется configure из speexdsp_config_types.h.in. На Windows ветка + * _WIN32 в speexdsp_types.h использует нативные типы и этот файл не инклудится. */ +#ifndef __SPEEX_TYPES_H__ +#define __SPEEX_TYPES_H__ + +#include + +typedef int16_t spx_int16_t; +typedef uint16_t spx_uint16_t; +typedef int32_t spx_int32_t; +typedef uint32_t spx_uint32_t; + +#endif diff --git a/lib/speexdsp/include/speex/speexdsp_types.h b/lib/speexdsp/include/speex/speexdsp_types.h new file mode 100644 index 00000000..4b4a76a4 --- /dev/null +++ b/lib/speexdsp/include/speex/speexdsp_types.h @@ -0,0 +1,126 @@ +/* speexdsp_types.h taken from libogg */ +/******************************************************************** + * * + * THIS FILE IS PART OF THE OggVorbis SOFTWARE CODEC SOURCE CODE. * + * USE, DISTRIBUTION AND REPRODUCTION OF THIS LIBRARY SOURCE IS * + * GOVERNED BY A BSD-STYLE SOURCE LICENSE INCLUDED WITH THIS SOURCE * + * IN 'COPYING'. PLEASE READ THESE TERMS BEFORE DISTRIBUTING. * + * * + * THE OggVorbis SOURCE CODE IS (C) COPYRIGHT 1994-2002 * + * by the Xiph.Org Foundation http://www.xiph.org/ * + * * + ******************************************************************** + + function: #ifdef jail to whip a few platforms into the UNIX ideal. + last mod: $Id: os_types.h 7524 2004-08-11 04:20:36Z conrad $ + + ********************************************************************/ +/** + @file speexdsp_types.h + @brief Speex types +*/ +#ifndef _SPEEX_TYPES_H +#define _SPEEX_TYPES_H + +#if defined(_WIN32) + +# if defined(__CYGWIN__) +# include <_G_config.h> + typedef _G_int32_t spx_int32_t; + typedef _G_uint32_t spx_uint32_t; + typedef _G_int16_t spx_int16_t; + typedef _G_uint16_t spx_uint16_t; +# elif defined(__MINGW32__) + typedef short spx_int16_t; + typedef unsigned short spx_uint16_t; + typedef int spx_int32_t; + typedef unsigned int spx_uint32_t; +# elif defined(__MWERKS__) + typedef int spx_int32_t; + typedef unsigned int spx_uint32_t; + typedef short spx_int16_t; + typedef unsigned short spx_uint16_t; +# else + /* MSVC/Borland */ + typedef __int32 spx_int32_t; + typedef unsigned __int32 spx_uint32_t; + typedef __int16 spx_int16_t; + typedef unsigned __int16 spx_uint16_t; +# endif + +#elif defined(__MACOS__) + +# include + typedef SInt16 spx_int16_t; + typedef UInt16 spx_uint16_t; + typedef SInt32 spx_int32_t; + typedef UInt32 spx_uint32_t; + +#elif (defined(__APPLE__) && defined(__MACH__)) /* MacOS X Framework build */ + +# include + typedef int16_t spx_int16_t; + typedef u_int16_t spx_uint16_t; + typedef int32_t spx_int32_t; + typedef u_int32_t spx_uint32_t; + +#elif defined(__BEOS__) + + /* Be */ +# include + typedef int16_t spx_int16_t; + typedef u_int16_t spx_uint16_t; + typedef int32_t spx_int32_t; + typedef u_int32_t spx_uint32_t; + +#elif defined (__EMX__) + + /* OS/2 GCC */ + typedef short spx_int16_t; + typedef unsigned short spx_uint16_t; + typedef int spx_int32_t; + typedef unsigned int spx_uint32_t; + +#elif defined (DJGPP) + + /* DJGPP */ + typedef short spx_int16_t; + typedef int spx_int32_t; + typedef unsigned int spx_uint32_t; + +#elif defined(R5900) + + /* PS2 EE */ + typedef int spx_int32_t; + typedef unsigned spx_uint32_t; + typedef short spx_int16_t; + +#elif defined(__SYMBIAN32__) + + /* Symbian GCC */ + typedef signed short spx_int16_t; + typedef unsigned short spx_uint16_t; + typedef signed int spx_int32_t; + typedef unsigned int spx_uint32_t; + +#elif defined(CONFIG_TI_C54X) || defined (CONFIG_TI_C55X) + + typedef short spx_int16_t; + typedef unsigned short spx_uint16_t; + typedef long spx_int32_t; + typedef unsigned long spx_uint32_t; + +#elif defined(CONFIG_TI_C6X) + + typedef short spx_int16_t; + typedef unsigned short spx_uint16_t; + typedef int spx_int32_t; + typedef unsigned int spx_uint32_t; + +#else + +#include "speexdsp_config_types.h" + +#endif + +#endif /* _SPEEX_TYPES_H */ diff --git a/lib/speexdsp/kiss_fft.c b/lib/speexdsp/kiss_fft.c new file mode 100644 index 00000000..8dc93454 --- /dev/null +++ b/lib/speexdsp/kiss_fft.c @@ -0,0 +1,523 @@ +/* +Copyright (c) 2003-2004, Mark Borgerding +Copyright (c) 2005-2007, Jean-Marc Valin + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. + * Neither the author nor the names of any contributors may be used to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + + +#ifdef HAVE_CONFIG_H +#include "config.h" +#endif + +#include "_kiss_fft_guts.h" +#include "arch.h" +#include "os_support.h" + +/* The guts header contains all the multiplication and addition macros that are defined for + fixed or floating point complex numbers. It also declares the kf_ internal functions. + */ + +static void kf_bfly2( + kiss_fft_cpx * Fout, + const size_t fstride, + const kiss_fft_cfg st, + int m, + int N, + int mm + ) +{ + kiss_fft_cpx * Fout2; + kiss_fft_cpx * tw1; + kiss_fft_cpx t; + if (!st->inverse) { + int i,j; + kiss_fft_cpx * Fout_beg = Fout; + for (i=0;itwiddles; + for(j=0;jr , tw1->r),MULT16_16(Fout2->i , tw1->i)), 1); + ti = SHR32(ADD32(MULT16_16(Fout2->i , tw1->r),MULT16_16(Fout2->r , tw1->i)), 1); + tw1 += fstride; + Fout2->r = PSHR32(SUB32(SHL32(EXTEND32(Fout->r), 14), tr), 15); + Fout2->i = PSHR32(SUB32(SHL32(EXTEND32(Fout->i), 14), ti), 15); + Fout->r = PSHR32(ADD32(SHL32(EXTEND32(Fout->r), 14), tr), 15); + Fout->i = PSHR32(ADD32(SHL32(EXTEND32(Fout->i), 14), ti), 15); + ++Fout2; + ++Fout; + } + } + } else { + int i,j; + kiss_fft_cpx * Fout_beg = Fout; + for (i=0;itwiddles; + for(j=0;jinverse) + { + kiss_fft_cpx * Fout_beg = Fout; + for (i=0;itwiddles; + for (j=0;jtwiddles; + for (j=0;jr = PSHR16(Fout->r, 2); + Fout->i = PSHR16(Fout->i, 2); + C_SUB( scratch[5] , *Fout, scratch[1] ); + C_ADDTO(*Fout, scratch[1]); + C_ADD( scratch[3] , scratch[0] , scratch[2] ); + C_SUB( scratch[4] , scratch[0] , scratch[2] ); + Fout[m2].r = PSHR16(Fout[m2].r, 2); + Fout[m2].i = PSHR16(Fout[m2].i, 2); + C_SUB( Fout[m2], *Fout, scratch[3] ); + tw1 += fstride; + tw2 += fstride*2; + tw3 += fstride*3; + C_ADDTO( *Fout , scratch[3] ); + + Fout[m].r = scratch[5].r + scratch[4].i; + Fout[m].i = scratch[5].i - scratch[4].r; + Fout[m3].r = scratch[5].r - scratch[4].i; + Fout[m3].i = scratch[5].i + scratch[4].r; + ++Fout; + } + } + } +} + +static void kf_bfly3( + kiss_fft_cpx * Fout, + const size_t fstride, + const kiss_fft_cfg st, + size_t m + ) +{ + size_t k=m; + const size_t m2 = 2*m; + kiss_fft_cpx *tw1,*tw2; + kiss_fft_cpx scratch[5]; + kiss_fft_cpx epi3; + epi3 = st->twiddles[fstride*m]; + + tw1=tw2=st->twiddles; + + do{ + if (!st->inverse) { + C_FIXDIV(*Fout,3); C_FIXDIV(Fout[m],3); C_FIXDIV(Fout[m2],3); + } + + C_MUL(scratch[1],Fout[m] , *tw1); + C_MUL(scratch[2],Fout[m2] , *tw2); + + C_ADD(scratch[3],scratch[1],scratch[2]); + C_SUB(scratch[0],scratch[1],scratch[2]); + tw1 += fstride; + tw2 += fstride*2; + + Fout[m].r = Fout->r - HALF_OF(scratch[3].r); + Fout[m].i = Fout->i - HALF_OF(scratch[3].i); + + C_MULBYSCALAR( scratch[0] , epi3.i ); + + C_ADDTO(*Fout,scratch[3]); + + Fout[m2].r = Fout[m].r + scratch[0].i; + Fout[m2].i = Fout[m].i - scratch[0].r; + + Fout[m].r -= scratch[0].i; + Fout[m].i += scratch[0].r; + + ++Fout; + }while(--k); +} + +static void kf_bfly5( + kiss_fft_cpx * Fout, + const size_t fstride, + const kiss_fft_cfg st, + int m + ) +{ + kiss_fft_cpx *Fout0,*Fout1,*Fout2,*Fout3,*Fout4; + int u; + kiss_fft_cpx scratch[13]; + kiss_fft_cpx * twiddles = st->twiddles; + kiss_fft_cpx *tw; + kiss_fft_cpx ya,yb; + ya = twiddles[fstride*m]; + yb = twiddles[fstride*2*m]; + + Fout0=Fout; + Fout1=Fout0+m; + Fout2=Fout0+2*m; + Fout3=Fout0+3*m; + Fout4=Fout0+4*m; + + tw=st->twiddles; + for ( u=0; uinverse) { + C_FIXDIV( *Fout0,5); C_FIXDIV( *Fout1,5); C_FIXDIV( *Fout2,5); C_FIXDIV( *Fout3,5); C_FIXDIV( *Fout4,5); + } + scratch[0] = *Fout0; + + C_MUL(scratch[1] ,*Fout1, tw[u*fstride]); + C_MUL(scratch[2] ,*Fout2, tw[2*u*fstride]); + C_MUL(scratch[3] ,*Fout3, tw[3*u*fstride]); + C_MUL(scratch[4] ,*Fout4, tw[4*u*fstride]); + + C_ADD( scratch[7],scratch[1],scratch[4]); + C_SUB( scratch[10],scratch[1],scratch[4]); + C_ADD( scratch[8],scratch[2],scratch[3]); + C_SUB( scratch[9],scratch[2],scratch[3]); + + Fout0->r += scratch[7].r + scratch[8].r; + Fout0->i += scratch[7].i + scratch[8].i; + + scratch[5].r = scratch[0].r + S_MUL(scratch[7].r,ya.r) + S_MUL(scratch[8].r,yb.r); + scratch[5].i = scratch[0].i + S_MUL(scratch[7].i,ya.r) + S_MUL(scratch[8].i,yb.r); + + scratch[6].r = S_MUL(scratch[10].i,ya.i) + S_MUL(scratch[9].i,yb.i); + scratch[6].i = -S_MUL(scratch[10].r,ya.i) - S_MUL(scratch[9].r,yb.i); + + C_SUB(*Fout1,scratch[5],scratch[6]); + C_ADD(*Fout4,scratch[5],scratch[6]); + + scratch[11].r = scratch[0].r + S_MUL(scratch[7].r,yb.r) + S_MUL(scratch[8].r,ya.r); + scratch[11].i = scratch[0].i + S_MUL(scratch[7].i,yb.r) + S_MUL(scratch[8].i,ya.r); + scratch[12].r = - S_MUL(scratch[10].i,yb.i) + S_MUL(scratch[9].i,ya.i); + scratch[12].i = S_MUL(scratch[10].r,yb.i) - S_MUL(scratch[9].r,ya.i); + + C_ADD(*Fout2,scratch[11],scratch[12]); + C_SUB(*Fout3,scratch[11],scratch[12]); + + ++Fout0;++Fout1;++Fout2;++Fout3;++Fout4; + } +} + +/* perform the butterfly for one stage of a mixed radix FFT */ +static void kf_bfly_generic( + kiss_fft_cpx * Fout, + const size_t fstride, + const kiss_fft_cfg st, + int m, + int p + ) +{ + int u,k,q1,q; + kiss_fft_cpx * twiddles = st->twiddles; + kiss_fft_cpx t; + kiss_fft_cpx scratchbuf[17]; + int Norig = st->nfft; + + /*CHECKBUF(scratchbuf,nscratchbuf,p);*/ + if (p>17) + speex_fatal("KissFFT: max radix supported is 17"); + + for ( u=0; uinverse) { + C_FIXDIV(scratchbuf[q1],p); + } + k += m; + } + + k=u; + for ( q1=0 ; q1

=Norig) twidx-=Norig; + C_MUL(t,scratchbuf[q] , twiddles[twidx] ); + C_ADDTO( Fout[ k ] ,t); + } + k += m; + } + } +} + +static +void kf_shuffle( + kiss_fft_cpx * Fout, + const kiss_fft_cpx * f, + const size_t fstride, + int in_stride, + int * factors, + const kiss_fft_cfg st + ) +{ + const int p=*factors++; /* the radix */ + const int m=*factors++; /* stage's fft length/p */ + + /*printf ("fft %d %d %d %d %d %d\n", p*m, m, p, s2, fstride*in_stride, N);*/ + if (m==1) + { + int j; + for (j=0;j32000 || (spx_int32_t)p*(spx_int32_t)p > n) + p = n; /* no more factors, skip to end */ + } + n /= p; + *facbuf++ = p; + *facbuf++ = n; + } while (n > 1); +} +/* + * + * User-callable function to allocate all necessary storage space for the fft. + * + * The return value is a contiguous block of memory, allocated with malloc. As such, + * It can be freed with free(), rather than a kiss_fft-specific function. + * */ +kiss_fft_cfg kiss_fft_alloc(int nfft,int inverse_fft,void * mem,size_t * lenmem ) +{ + kiss_fft_cfg st=NULL; + size_t memneeded = sizeof(struct kiss_fft_state) + + sizeof(kiss_fft_cpx)*(nfft-1); /* twiddle factors*/ + + if ( lenmem==NULL ) { + st = ( kiss_fft_cfg)KISS_FFT_MALLOC( memneeded ); + }else{ + if (mem != NULL && *lenmem >= memneeded) + st = (kiss_fft_cfg)mem; + *lenmem = memneeded; + } + if (st) { + int i; + st->nfft=nfft; + st->inverse = inverse_fft; +#ifdef FIXED_POINT + for (i=0;iinverse) + phase = -phase; + kf_cexp2(st->twiddles+i, DIV32(SHL32(phase,17),nfft)); + } +#else + for (i=0;iinverse) + phase *= -1; + kf_cexp(st->twiddles+i, phase ); + } +#endif + kf_factor(nfft,st->factors); + } + return st; +} + + + + +void kiss_fft_stride(kiss_fft_cfg st,const kiss_fft_cpx *fin,kiss_fft_cpx *fout,int in_stride) +{ + if (fin == fout) + { + speex_fatal("In-place FFT not supported"); + /*CHECKBUF(tmpbuf,ntmpbuf,st->nfft); + kf_work(tmpbuf,fin,1,in_stride, st->factors,st); + SPEEX_MOVE(fout,tmpbuf,st->nfft);*/ + } else { + kf_shuffle( fout, fin, 1,in_stride, st->factors,st); + kf_work( fout, fin, 1,in_stride, st->factors,st, 1, in_stride, 1); + } +} + +void kiss_fft(kiss_fft_cfg cfg,const kiss_fft_cpx *fin,kiss_fft_cpx *fout) +{ + kiss_fft_stride(cfg,fin,fout,1); +} + diff --git a/lib/speexdsp/kiss_fft.h b/lib/speexdsp/kiss_fft.h new file mode 100644 index 00000000..bb19eadb --- /dev/null +++ b/lib/speexdsp/kiss_fft.h @@ -0,0 +1,108 @@ +#ifndef KISS_FFT_H +#define KISS_FFT_H + +#include +#include +#include "arch.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/* + ATTENTION! + If you would like a : + -- a utility that will handle the caching of fft objects + -- real-only (no imaginary time component ) FFT + -- a multi-dimensional FFT + -- a command-line utility to perform ffts + -- a command-line utility to perform fast-convolution filtering + + Then see kfc.h kiss_fftr.h kiss_fftnd.h fftutil.c kiss_fastfir.c + in the tools/ directory. +*/ + +#ifdef USE_SIMD +# include +# define kiss_fft_scalar __m128 +#define KISS_FFT_MALLOC(nbytes) memalign(16,nbytes) +#else +#define KISS_FFT_MALLOC speex_alloc +#endif + + +#ifdef FIXED_POINT +#include "arch.h" +# define kiss_fft_scalar spx_int16_t +#else +# ifndef kiss_fft_scalar +/* default is float */ +# define kiss_fft_scalar float +# endif +#endif + +typedef struct { + kiss_fft_scalar r; + kiss_fft_scalar i; +}kiss_fft_cpx; + +typedef struct kiss_fft_state* kiss_fft_cfg; + +/* + * kiss_fft_alloc + * + * Initialize a FFT (or IFFT) algorithm's cfg/state buffer. + * + * typical usage: kiss_fft_cfg mycfg=kiss_fft_alloc(1024,0,NULL,NULL); + * + * The return value from fft_alloc is a cfg buffer used internally + * by the fft routine or NULL. + * + * If lenmem is NULL, then kiss_fft_alloc will allocate a cfg buffer using malloc. + * The returned value should be free()d when done to avoid memory leaks. + * + * The state can be placed in a user supplied buffer 'mem': + * If lenmem is not NULL and mem is not NULL and *lenmem is large enough, + * then the function places the cfg in mem and the size used in *lenmem + * and returns mem. + * + * If lenmem is not NULL and ( mem is NULL or *lenmem is not large enough), + * then the function returns NULL and places the minimum cfg + * buffer size in *lenmem. + * */ + +kiss_fft_cfg kiss_fft_alloc(int nfft,int inverse_fft,void * mem,size_t * lenmem); + +/* + * kiss_fft(cfg,in_out_buf) + * + * Perform an FFT on a complex input buffer. + * for a forward FFT, + * fin should be f[0] , f[1] , ... ,f[nfft-1] + * fout will be F[0] , F[1] , ... ,F[nfft-1] + * Note that each element is complex and can be accessed like + f[k].r and f[k].i + * */ +void kiss_fft(kiss_fft_cfg cfg,const kiss_fft_cpx *fin,kiss_fft_cpx *fout); + +/* + A more generic version of the above function. It reads its input from every Nth sample. + * */ +void kiss_fft_stride(kiss_fft_cfg cfg,const kiss_fft_cpx *fin,kiss_fft_cpx *fout,int fin_stride); + +/* If kiss_fft_alloc allocated a buffer, it is one contiguous + buffer and can be simply free()d when no longer needed*/ +#define kiss_fft_free speex_free + +/* + Cleans up some memory that gets managed internally. Not necessary to call, but it might clean up + your compiler output to call this before you exit. +*/ +void kiss_fft_cleanup(void); + + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/lib/speexdsp/kiss_fftr.c b/lib/speexdsp/kiss_fftr.c new file mode 100644 index 00000000..827e0b14 --- /dev/null +++ b/lib/speexdsp/kiss_fftr.c @@ -0,0 +1,297 @@ +/* +Copyright (c) 2003-2004, Mark Borgerding + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. + * Neither the author nor the names of any contributors may be used to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#ifdef HAVE_CONFIG_H +#include "config.h" +#endif + +#include "os_support.h" +#include "kiss_fftr.h" +#include "_kiss_fft_guts.h" + +struct kiss_fftr_state{ + kiss_fft_cfg substate; + kiss_fft_cpx * tmpbuf; + kiss_fft_cpx * super_twiddles; +#ifdef USE_SIMD + long pad; +#endif +}; + +kiss_fftr_cfg kiss_fftr_alloc(int nfft,int inverse_fft,void * mem,size_t * lenmem) +{ + int i; + kiss_fftr_cfg st = NULL; + size_t subsize, memneeded; + + if (nfft & 1) { + speex_warning("Real FFT optimization must be even.\n"); + return NULL; + } + nfft >>= 1; + + kiss_fft_alloc (nfft, inverse_fft, NULL, &subsize); + memneeded = sizeof(struct kiss_fftr_state) + subsize + sizeof(kiss_fft_cpx) * ( nfft * 2); + + if (lenmem == NULL) { + st = (kiss_fftr_cfg) KISS_FFT_MALLOC (memneeded); + } else { + if (*lenmem >= memneeded) + st = (kiss_fftr_cfg) mem; + *lenmem = memneeded; + } + if (!st) + return NULL; + + st->substate = (kiss_fft_cfg) (st + 1); /*just beyond kiss_fftr_state struct */ + st->tmpbuf = (kiss_fft_cpx *) (((char *) st->substate) + subsize); + st->super_twiddles = st->tmpbuf + nfft; + kiss_fft_alloc(nfft, inverse_fft, st->substate, &subsize); + +#ifdef FIXED_POINT + for (i=0;i>1); + if (!inverse_fft) + phase = -phase; + kf_cexp2(st->super_twiddles+i, DIV32(SHL32(phase,16),nfft)); + } +#else + for (i=0;isuper_twiddles+i, phase ); + } +#endif + return st; +} + +void kiss_fftr(kiss_fftr_cfg st,const kiss_fft_scalar *timedata,kiss_fft_cpx *freqdata) +{ + /* input buffer timedata is stored row-wise */ + int k,ncfft; + kiss_fft_cpx fpnk,fpk,f1k,f2k,tw,tdc; + + if ( st->substate->inverse) { + speex_fatal("kiss fft usage error: improper alloc\n"); + } + + ncfft = st->substate->nfft; + + /*perform the parallel fft of two real signals packed in real,imag*/ + kiss_fft( st->substate , (const kiss_fft_cpx*)timedata, st->tmpbuf ); + /* The real part of the DC element of the frequency spectrum in st->tmpbuf + * contains the sum of the even-numbered elements of the input time sequence + * The imag part is the sum of the odd-numbered elements + * + * The sum of tdc.r and tdc.i is the sum of the input time sequence. + * yielding DC of input time sequence + * The difference of tdc.r - tdc.i is the sum of the input (dot product) [1,-1,1,-1... + * yielding Nyquist bin of input time sequence + */ + + tdc.r = st->tmpbuf[0].r; + tdc.i = st->tmpbuf[0].i; + C_FIXDIV(tdc,2); + CHECK_OVERFLOW_OP(tdc.r ,+, tdc.i); + CHECK_OVERFLOW_OP(tdc.r ,-, tdc.i); + freqdata[0].r = tdc.r + tdc.i; + freqdata[ncfft].r = tdc.r - tdc.i; +#ifdef USE_SIMD + freqdata[ncfft].i = freqdata[0].i = _mm_set1_ps(0); +#else + freqdata[ncfft].i = freqdata[0].i = 0; +#endif + + for ( k=1;k <= ncfft/2 ; ++k ) { + fpk = st->tmpbuf[k]; + fpnk.r = st->tmpbuf[ncfft-k].r; + fpnk.i = - st->tmpbuf[ncfft-k].i; + C_FIXDIV(fpk,2); + C_FIXDIV(fpnk,2); + + C_ADD( f1k, fpk , fpnk ); + C_SUB( f2k, fpk , fpnk ); + C_MUL( tw , f2k , st->super_twiddles[k]); + + freqdata[k].r = HALF_OF(f1k.r + tw.r); + freqdata[k].i = HALF_OF(f1k.i + tw.i); + freqdata[ncfft-k].r = HALF_OF(f1k.r - tw.r); + freqdata[ncfft-k].i = HALF_OF(tw.i - f1k.i); + } +} + +void kiss_fftri(kiss_fftr_cfg st,const kiss_fft_cpx *freqdata, kiss_fft_scalar *timedata) +{ + /* input buffer timedata is stored row-wise */ + int k, ncfft; + + if (st->substate->inverse == 0) { + speex_fatal("kiss fft usage error: improper alloc\n"); + } + + ncfft = st->substate->nfft; + + st->tmpbuf[0].r = freqdata[0].r + freqdata[ncfft].r; + st->tmpbuf[0].i = freqdata[0].r - freqdata[ncfft].r; + /*C_FIXDIV(st->tmpbuf[0],2);*/ + + for (k = 1; k <= ncfft / 2; ++k) { + kiss_fft_cpx fk, fnkc, fek, fok, tmp; + fk = freqdata[k]; + fnkc.r = freqdata[ncfft - k].r; + fnkc.i = -freqdata[ncfft - k].i; + /*C_FIXDIV( fk , 2 ); + C_FIXDIV( fnkc , 2 );*/ + + C_ADD (fek, fk, fnkc); + C_SUB (tmp, fk, fnkc); + C_MUL (fok, tmp, st->super_twiddles[k]); + C_ADD (st->tmpbuf[k], fek, fok); + C_SUB (st->tmpbuf[ncfft - k], fek, fok); +#ifdef USE_SIMD + st->tmpbuf[ncfft - k].i *= _mm_set1_ps(-1.0); +#else + st->tmpbuf[ncfft - k].i *= -1; +#endif + } + kiss_fft (st->substate, st->tmpbuf, (kiss_fft_cpx *) timedata); +} + +void kiss_fftr2(kiss_fftr_cfg st,const kiss_fft_scalar *timedata,kiss_fft_scalar *freqdata) +{ + /* input buffer timedata is stored row-wise */ + int k,ncfft; + kiss_fft_cpx f2k,tdc; + spx_word32_t f1kr, f1ki, twr, twi; + + if ( st->substate->inverse) { + speex_fatal("kiss fft usage error: improper alloc\n"); + } + + ncfft = st->substate->nfft; + + /*perform the parallel fft of two real signals packed in real,imag*/ + kiss_fft( st->substate , (const kiss_fft_cpx*)timedata, st->tmpbuf ); + /* The real part of the DC element of the frequency spectrum in st->tmpbuf + * contains the sum of the even-numbered elements of the input time sequence + * The imag part is the sum of the odd-numbered elements + * + * The sum of tdc.r and tdc.i is the sum of the input time sequence. + * yielding DC of input time sequence + * The difference of tdc.r - tdc.i is the sum of the input (dot product) [1,-1,1,-1... + * yielding Nyquist bin of input time sequence + */ + + tdc.r = st->tmpbuf[0].r; + tdc.i = st->tmpbuf[0].i; + C_FIXDIV(tdc,2); + CHECK_OVERFLOW_OP(tdc.r ,+, tdc.i); + CHECK_OVERFLOW_OP(tdc.r ,-, tdc.i); + freqdata[0] = tdc.r + tdc.i; + freqdata[2*ncfft-1] = tdc.r - tdc.i; + + for ( k=1;k <= ncfft/2 ; ++k ) + { + /*fpk = st->tmpbuf[k]; + fpnk.r = st->tmpbuf[ncfft-k].r; + fpnk.i = - st->tmpbuf[ncfft-k].i; + C_FIXDIV(fpk,2); + C_FIXDIV(fpnk,2); + + C_ADD( f1k, fpk , fpnk ); + C_SUB( f2k, fpk , fpnk ); + + C_MUL( tw , f2k , st->super_twiddles[k]); + + freqdata[2*k-1] = HALF_OF(f1k.r + tw.r); + freqdata[2*k] = HALF_OF(f1k.i + tw.i); + freqdata[2*(ncfft-k)-1] = HALF_OF(f1k.r - tw.r); + freqdata[2*(ncfft-k)] = HALF_OF(tw.i - f1k.i); + */ + + /*f1k.r = PSHR32(ADD32(EXTEND32(st->tmpbuf[k].r), EXTEND32(st->tmpbuf[ncfft-k].r)),1); + f1k.i = PSHR32(SUB32(EXTEND32(st->tmpbuf[k].i), EXTEND32(st->tmpbuf[ncfft-k].i)),1); + f2k.r = PSHR32(SUB32(EXTEND32(st->tmpbuf[k].r), EXTEND32(st->tmpbuf[ncfft-k].r)),1); + f2k.i = SHR32(ADD32(EXTEND32(st->tmpbuf[k].i), EXTEND32(st->tmpbuf[ncfft-k].i)),1); + + C_MUL( tw , f2k , st->super_twiddles[k]); + + freqdata[2*k-1] = HALF_OF(f1k.r + tw.r); + freqdata[2*k] = HALF_OF(f1k.i + tw.i); + freqdata[2*(ncfft-k)-1] = HALF_OF(f1k.r - tw.r); + freqdata[2*(ncfft-k)] = HALF_OF(tw.i - f1k.i); + */ + f2k.r = SHR32(SUB32(EXTEND32(st->tmpbuf[k].r), EXTEND32(st->tmpbuf[ncfft-k].r)),1); + f2k.i = PSHR32(ADD32(EXTEND32(st->tmpbuf[k].i), EXTEND32(st->tmpbuf[ncfft-k].i)),1); + + f1kr = SHL32(ADD32(EXTEND32(st->tmpbuf[k].r), EXTEND32(st->tmpbuf[ncfft-k].r)),13); + f1ki = SHL32(SUB32(EXTEND32(st->tmpbuf[k].i), EXTEND32(st->tmpbuf[ncfft-k].i)),13); + + twr = SHR32(SUB32(MULT16_16(f2k.r,st->super_twiddles[k].r),MULT16_16(f2k.i,st->super_twiddles[k].i)), 1); + twi = SHR32(ADD32(MULT16_16(f2k.i,st->super_twiddles[k].r),MULT16_16(f2k.r,st->super_twiddles[k].i)), 1); + +#ifdef FIXED_POINT + freqdata[2*k-1] = PSHR32(f1kr + twr, 15); + freqdata[2*k] = PSHR32(f1ki + twi, 15); + freqdata[2*(ncfft-k)-1] = PSHR32(f1kr - twr, 15); + freqdata[2*(ncfft-k)] = PSHR32(twi - f1ki, 15); +#else + freqdata[2*k-1] = .5f*(f1kr + twr); + freqdata[2*k] = .5f*(f1ki + twi); + freqdata[2*(ncfft-k)-1] = .5f*(f1kr - twr); + freqdata[2*(ncfft-k)] = .5f*(twi - f1ki); + +#endif + } +} + +void kiss_fftri2(kiss_fftr_cfg st,const kiss_fft_scalar *freqdata,kiss_fft_scalar *timedata) +{ + /* input buffer timedata is stored row-wise */ + int k, ncfft; + + if (st->substate->inverse == 0) { + speex_fatal ("kiss fft usage error: improper alloc\n"); + } + + ncfft = st->substate->nfft; + + st->tmpbuf[0].r = freqdata[0] + freqdata[2*ncfft-1]; + st->tmpbuf[0].i = freqdata[0] - freqdata[2*ncfft-1]; + /*C_FIXDIV(st->tmpbuf[0],2);*/ + + for (k = 1; k <= ncfft / 2; ++k) { + kiss_fft_cpx fk, fnkc, fek, fok, tmp; + fk.r = freqdata[2*k-1]; + fk.i = freqdata[2*k]; + fnkc.r = freqdata[2*(ncfft - k)-1]; + fnkc.i = -freqdata[2*(ncfft - k)]; + /*C_FIXDIV( fk , 2 ); + C_FIXDIV( fnkc , 2 );*/ + + C_ADD (fek, fk, fnkc); + C_SUB (tmp, fk, fnkc); + C_MUL (fok, tmp, st->super_twiddles[k]); + C_ADD (st->tmpbuf[k], fek, fok); + C_SUB (st->tmpbuf[ncfft - k], fek, fok); +#ifdef USE_SIMD + st->tmpbuf[ncfft - k].i *= _mm_set1_ps(-1.0); +#else + st->tmpbuf[ncfft - k].i *= -1; +#endif + } + kiss_fft (st->substate, st->tmpbuf, (kiss_fft_cpx *) timedata); +} diff --git a/lib/speexdsp/kiss_fftr.h b/lib/speexdsp/kiss_fftr.h new file mode 100644 index 00000000..8cfeda13 --- /dev/null +++ b/lib/speexdsp/kiss_fftr.h @@ -0,0 +1,51 @@ +#ifndef KISS_FTR_H +#define KISS_FTR_H + +#include "kiss_fft.h" +#ifdef __cplusplus +extern "C" { +#endif + + +/* + + Real optimized version can save about 45% cpu time vs. complex fft of a real seq. + + + + */ + +typedef struct kiss_fftr_state *kiss_fftr_cfg; + + +kiss_fftr_cfg kiss_fftr_alloc(int nfft,int inverse_fft,void * mem, size_t * lenmem); +/* + nfft must be even + + If you don't care to allocate space, use mem = lenmem = NULL +*/ + + +void kiss_fftr(kiss_fftr_cfg cfg,const kiss_fft_scalar *timedata,kiss_fft_cpx *freqdata); +/* + input timedata has nfft scalar points + output freqdata has nfft/2+1 complex points +*/ + +void kiss_fftr2(kiss_fftr_cfg st,const kiss_fft_scalar *timedata,kiss_fft_scalar *freqdata); + +void kiss_fftri(kiss_fftr_cfg cfg,const kiss_fft_cpx *freqdata,kiss_fft_scalar *timedata); + +void kiss_fftri2(kiss_fftr_cfg st,const kiss_fft_scalar *freqdata, kiss_fft_scalar *timedata); + +/* + input freqdata has nfft/2+1 complex points + output timedata has nfft scalar points +*/ + +#define kiss_fftr_free speex_free + +#ifdef __cplusplus +} +#endif +#endif diff --git a/lib/speexdsp/math_approx.h b/lib/speexdsp/math_approx.h new file mode 100644 index 00000000..596dfdc1 --- /dev/null +++ b/lib/speexdsp/math_approx.h @@ -0,0 +1,332 @@ +/* Copyright (C) 2002 Jean-Marc Valin */ +/** + @file math_approx.h + @brief Various math approximation functions for Speex +*/ +/* + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions + are met: + + - Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + + - Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + + - Neither the name of the Xiph.org Foundation nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE FOUNDATION OR + CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#ifndef MATH_APPROX_H +#define MATH_APPROX_H + +#include "arch.h" + +#ifndef FIXED_POINT + +#define spx_sqrt sqrt +#define spx_acos acos +#define spx_exp exp +#define spx_cos_norm(x) (cos((.5f*M_PI)*(x))) +#define spx_atan atan + +/** Generate a pseudo-random number */ +static inline spx_word16_t speex_rand(spx_word16_t std, spx_int32_t *seed) +{ + const unsigned int jflone = 0x3f800000; + const unsigned int jflmsk = 0x007fffff; + union {int i; float f;} ran; + *seed = 1664525 * *seed + 1013904223; + ran.i = jflone | (jflmsk & *seed); + ran.f -= 1.5; + return 3.4642*std*ran.f; +} + + +#endif + + +static inline spx_int16_t spx_ilog2(spx_uint32_t x) +{ + int r=0; + if (x>=(spx_int32_t)65536) + { + x >>= 16; + r += 16; + } + if (x>=256) + { + x >>= 8; + r += 8; + } + if (x>=16) + { + x >>= 4; + r += 4; + } + if (x>=4) + { + x >>= 2; + r += 2; + } + if (x>=2) + { + r += 1; + } + return r; +} + +static inline spx_int16_t spx_ilog4(spx_uint32_t x) +{ + int r=0; + if (x>=(spx_int32_t)65536) + { + x >>= 16; + r += 8; + } + if (x>=256) + { + x >>= 8; + r += 4; + } + if (x>=16) + { + x >>= 4; + r += 2; + } + if (x>=4) + { + r += 1; + } + return r; +} + +#ifdef FIXED_POINT + +/** Generate a pseudo-random number */ +static inline spx_word16_t speex_rand(spx_word16_t std, spx_int32_t *seed) +{ + spx_word32_t res; + *seed = 1664525 * *seed + 1013904223; + res = MULT16_16(EXTRACT16(SHR32(*seed,16)),std); + return EXTRACT16(PSHR32(SUB32(res, SHR32(res, 3)),14)); +} + +/* sqrt(x) ~= 0.22178 + 1.29227*x - 0.77070*x^2 + 0.25723*x^3 (for .25 < x < 1) */ +/*#define C0 3634 +#define C1 21173 +#define C2 -12627 +#define C3 4215*/ + +/* sqrt(x) ~= 0.22178 + 1.29227*x - 0.77070*x^2 + 0.25659*x^3 (for .25 < x < 1) */ +#define C0 3634 +#define C1 21173 +#define C2 -12627 +#define C3 4204 + +static inline spx_word16_t spx_sqrt(spx_word32_t x) +{ + int k; + spx_word32_t rt; + k = spx_ilog4(x)-6; + x = VSHR32(x, (k<<1)); + rt = ADD16(C0, MULT16_16_Q14(x, ADD16(C1, MULT16_16_Q14(x, ADD16(C2, MULT16_16_Q14(x, (C3))))))); + rt = VSHR32(rt,7-k); + return rt; +} + +/* log(x) ~= -2.18151 + 4.20592*x - 2.88938*x^2 + 0.86535*x^3 (for .5 < x < 1) */ + + +#define A1 16469 +#define A2 2242 +#define A3 1486 + +static inline spx_word16_t spx_acos(spx_word16_t x) +{ + int s=0; + spx_word16_t ret; + spx_word16_t sq; + if (x<0) + { + s=1; + x = NEG16(x); + } + x = SUB16(16384,x); + + x = x >> 1; + sq = MULT16_16_Q13(x, ADD16(A1, MULT16_16_Q13(x, ADD16(A2, MULT16_16_Q13(x, (A3)))))); + ret = spx_sqrt(SHL32(EXTEND32(sq),13)); + + /*ret = spx_sqrt(67108864*(-1.6129e-04 + 2.0104e+00*f + 2.7373e-01*f*f + 1.8136e-01*f*f*f));*/ + if (s) + ret = SUB16(25736,ret); + return ret; +} + + +#define K1 8192 +#define K2 -4096 +#define K3 340 +#define K4 -10 + +static inline spx_word16_t spx_cos(spx_word16_t x) +{ + spx_word16_t x2; + + if (x<12868) + { + x2 = MULT16_16_P13(x,x); + return ADD32(K1, MULT16_16_P13(x2, ADD32(K2, MULT16_16_P13(x2, ADD32(K3, MULT16_16_P13(K4, x2)))))); + } else { + x = SUB16(25736,x); + x2 = MULT16_16_P13(x,x); + return SUB32(-K1, MULT16_16_P13(x2, ADD32(K2, MULT16_16_P13(x2, ADD32(K3, MULT16_16_P13(K4, x2)))))); + } +} + +#define L1 32767 +#define L2 -7651 +#define L3 8277 +#define L4 -626 + +static inline spx_word16_t _spx_cos_pi_2(spx_word16_t x) +{ + spx_word16_t x2; + + x2 = MULT16_16_P15(x,x); + return ADD16(1,MIN16(32766,ADD32(SUB16(L1,x2), MULT16_16_P15(x2, ADD32(L2, MULT16_16_P15(x2, ADD32(L3, MULT16_16_P15(L4, x2)))))))); +} + +static inline spx_word16_t spx_cos_norm(spx_word32_t x) +{ + x = x&0x0001ffff; + if (x>SHL32(EXTEND32(1), 16)) + x = SUB32(SHL32(EXTEND32(1), 17),x); + if (x&0x00007fff) + { + if (x14) + return 0x7fffffff; + else if (integer < -15) + return 0; + frac = SHL16(x-SHL16(integer,11),3); + frac = ADD16(D0, MULT16_16_Q14(frac, ADD16(D1, MULT16_16_Q14(frac, ADD16(D2 , MULT16_16_Q14(D3,frac)))))); + return VSHR32(EXTEND32(frac), -integer-2); +} + +/* Input in Q11 format, output in Q16 */ +static inline spx_word32_t spx_exp(spx_word16_t x) +{ + if (x>21290) + return 0x7fffffff; + else if (x<-21290) + return 0; + else + return spx_exp2(MULT16_16_P14(23637,x)); +} +#define M1 32767 +#define M2 -21 +#define M3 -11943 +#define M4 4936 + +static inline spx_word16_t spx_atan01(spx_word16_t x) +{ + return MULT16_16_P15(x, ADD32(M1, MULT16_16_P15(x, ADD32(M2, MULT16_16_P15(x, ADD32(M3, MULT16_16_P15(M4, x))))))); +} + +#undef M1 +#undef M2 +#undef M3 +#undef M4 + +/* Input in Q15, output in Q14 */ +static inline spx_word16_t spx_atan(spx_word32_t x) +{ + if (x <= 32767) + { + return SHR16(spx_atan01(x),1); + } else { + int e = spx_ilog2(x); + if (e>=29) + return 25736; + x = DIV32_16(SHL32(EXTEND32(32767),29-e), EXTRACT16(SHR32(x, e-14))); + return SUB16(25736, SHR16(spx_atan01(x),1)); + } +} +#else + +#ifndef M_PI +#define M_PI 3.14159265358979323846 /* pi */ +#endif + +#define C1 0.9999932946f +#define C2 -0.4999124376f +#define C3 0.0414877472f +#define C4 -0.0012712095f + + +#define SPX_PI_2 1.5707963268 +static inline spx_word16_t spx_cos(spx_word16_t x) +{ + if (x>16) + +#else + +static const spx_float_t MIN_LEAK = .005f; + +/* Constants for the two-path filter */ +static const spx_float_t VAR1_SMOOTH = .36f; +static const spx_float_t VAR2_SMOOTH = .7225f; +static const spx_float_t VAR1_UPDATE = .5f; +static const spx_float_t VAR2_UPDATE = .25f; +static const spx_float_t VAR_BACKTRACK = 4.f; +#define TOP16(x) (x) +#endif + + +#define PLAYBACK_DELAY 2 + +void speex_echo_get_residual(SpeexEchoState *st, spx_word32_t *Yout, int len); + + +/** Speex echo cancellation state. */ +struct SpeexEchoState_ { + int frame_size; /**< Number of samples processed each time */ + int window_size; + int M; + int cancel_count; + int adapted; + int saturated; + int screwed_up; + int C; /** Number of input channels (microphones) */ + int K; /** Number of output channels (loudspeakers) */ + spx_int32_t sampling_rate; + spx_word16_t spec_average; + spx_word16_t beta0; + spx_word16_t beta_max; + spx_word32_t sum_adapt; + spx_word16_t leak_estimate; + + spx_word16_t *e; /* scratch */ + spx_word16_t *x; /* Far-end input buffer (2N) */ + spx_word16_t *X; /* Far-end buffer (M+1 frames) in frequency domain */ + spx_word16_t *input; /* scratch */ + spx_word16_t *y; /* scratch */ + spx_word16_t *last_y; + spx_word16_t *Y; /* scratch */ + spx_word16_t *E; + spx_word32_t *PHI; /* scratch */ + spx_word32_t *W; /* (Background) filter weights */ +#ifdef TWO_PATH + spx_word16_t *foreground; /* Foreground filter weights */ + spx_word32_t Davg1; /* 1st recursive average of the residual power difference */ + spx_word32_t Davg2; /* 2nd recursive average of the residual power difference */ + spx_float_t Dvar1; /* Estimated variance of 1st estimator */ + spx_float_t Dvar2; /* Estimated variance of 2nd estimator */ +#endif + spx_word32_t *power; /* Power of the far-end signal */ + spx_float_t *power_1;/* Inverse power of far-end */ + spx_word16_t *wtmp; /* scratch */ +#ifdef FIXED_POINT + spx_word16_t *wtmp2; /* scratch */ +#endif + spx_word32_t *Rf; /* scratch */ + spx_word32_t *Yf; /* scratch */ + spx_word32_t *Xf; /* scratch */ + spx_word32_t *Eh; + spx_word32_t *Yh; + spx_float_t Pey; + spx_float_t Pyy; + spx_word16_t *window; + spx_word16_t *prop; + void *fft_table; + spx_word16_t *memX, *memD, *memE; + spx_word16_t preemph; + spx_word16_t notch_radius; + spx_mem_t *notch_mem; + + /* NOTE: If you only use speex_echo_cancel() and want to save some memory, remove this */ + spx_int16_t *play_buf; + int play_buf_pos; + int play_buf_started; +}; + +static inline void filter_dc_notch16(const spx_int16_t *in, spx_word16_t radius, spx_word16_t *out, int len, spx_mem_t *mem, int stride) +{ + int i; + spx_word16_t den2; +#ifdef FIXED_POINT + den2 = MULT16_16_Q15(radius,radius) + MULT16_16_Q15(QCONST16(.7,15),MULT16_16_Q15(32767-radius,32767-radius)); +#else + den2 = radius*radius + .7*(1-radius)*(1-radius); +#endif + /*printf ("%d %d %d %d %d %d\n", num[0], num[1], num[2], den[0], den[1], den[2]);*/ + for (i=0;i>= 1; + while(len--) + { + spx_word32_t part=0; + part = MAC16_16(part,*x++,*y++); + part = MAC16_16(part,*x++,*y++); + /* HINT: If you had a 40-bit accumulator, you could shift only at the end */ + sum = ADD32(sum,SHR32(part,6)); + } + return sum; +} + +/** Compute power spectrum of a half-complex (packed) vector */ +static inline void power_spectrum(const spx_word16_t *X, spx_word32_t *ps, int N) +{ + int i, j; + ps[0]=MULT16_16(X[0],X[0]); + for (i=1,j=1;i max_sum) + max_sum = prop[i]; + } + for (i=0;i +static FILE *rFile=NULL, *pFile=NULL, *oFile=NULL; + +static void dump_audio(const spx_int16_t *rec, const spx_int16_t *play, const spx_int16_t *out, int len) +{ + if (!(rFile && pFile && oFile)) + { + speex_fatal("Dump files not open"); + } + fwrite(rec, sizeof(spx_int16_t), len, rFile); + fwrite(play, sizeof(spx_int16_t), len, pFile); + fwrite(out, sizeof(spx_int16_t), len, oFile); +} +#endif + +/** Creates a new echo canceller state */ +EXPORT SpeexEchoState *speex_echo_state_init(int frame_size, int filter_length) +{ + return speex_echo_state_init_mc(frame_size, filter_length, 1, 1); +} + +EXPORT SpeexEchoState *speex_echo_state_init_mc(int frame_size, int filter_length, int nb_mic, int nb_speakers) +{ + int i,N,M, C, K; + SpeexEchoState *st = (SpeexEchoState *)speex_alloc(sizeof(SpeexEchoState)); + + st->K = nb_speakers; + st->C = nb_mic; + C=st->C; + K=st->K; +#ifdef DUMP_ECHO_CANCEL_DATA + if (rFile || pFile || oFile) + speex_fatal("Opening dump files twice"); + rFile = fopen("aec_rec.sw", "wb"); + pFile = fopen("aec_play.sw", "wb"); + oFile = fopen("aec_out.sw", "wb"); +#endif + + st->frame_size = frame_size; + st->window_size = 2*frame_size; + N = st->window_size; + M = st->M = (filter_length+st->frame_size-1)/frame_size; + st->cancel_count=0; + st->sum_adapt = 0; + st->saturated = 0; + st->screwed_up = 0; + /* This is the default sampling rate */ + st->sampling_rate = 8000; + st->spec_average = DIV32_16(SHL32(EXTEND32(st->frame_size), 15), st->sampling_rate); +#ifdef FIXED_POINT + st->beta0 = DIV32_16(SHL32(EXTEND32(st->frame_size), 16), st->sampling_rate); + st->beta_max = DIV32_16(SHL32(EXTEND32(st->frame_size), 14), st->sampling_rate); +#else + st->beta0 = (2.0f*st->frame_size)/st->sampling_rate; + st->beta_max = (.5f*st->frame_size)/st->sampling_rate; +#endif + st->leak_estimate = 0; + + st->fft_table = spx_fft_init(N); + + st->e = (spx_word16_t*)speex_alloc(C*N*sizeof(spx_word16_t)); + st->x = (spx_word16_t*)speex_alloc(K*N*sizeof(spx_word16_t)); + st->input = (spx_word16_t*)speex_alloc(C*st->frame_size*sizeof(spx_word16_t)); + st->y = (spx_word16_t*)speex_alloc(C*N*sizeof(spx_word16_t)); + st->last_y = (spx_word16_t*)speex_alloc(C*N*sizeof(spx_word16_t)); + st->Yf = (spx_word32_t*)speex_alloc((st->frame_size+1)*sizeof(spx_word32_t)); + st->Rf = (spx_word32_t*)speex_alloc((st->frame_size+1)*sizeof(spx_word32_t)); + st->Xf = (spx_word32_t*)speex_alloc((st->frame_size+1)*sizeof(spx_word32_t)); + st->Yh = (spx_word32_t*)speex_alloc((st->frame_size+1)*sizeof(spx_word32_t)); + st->Eh = (spx_word32_t*)speex_alloc((st->frame_size+1)*sizeof(spx_word32_t)); + + st->X = (spx_word16_t*)speex_alloc(K*(M+1)*N*sizeof(spx_word16_t)); + st->Y = (spx_word16_t*)speex_alloc(C*N*sizeof(spx_word16_t)); + st->E = (spx_word16_t*)speex_alloc(C*N*sizeof(spx_word16_t)); + st->W = (spx_word32_t*)speex_alloc(C*K*M*N*sizeof(spx_word32_t)); +#ifdef TWO_PATH + st->foreground = (spx_word16_t*)speex_alloc(M*N*C*K*sizeof(spx_word16_t)); +#endif + st->PHI = (spx_word32_t*)speex_alloc(N*sizeof(spx_word32_t)); + st->power = (spx_word32_t*)speex_alloc((frame_size+1)*sizeof(spx_word32_t)); + st->power_1 = (spx_float_t*)speex_alloc((frame_size+1)*sizeof(spx_float_t)); + st->window = (spx_word16_t*)speex_alloc(N*sizeof(spx_word16_t)); + st->prop = (spx_word16_t*)speex_alloc(M*sizeof(spx_word16_t)); + st->wtmp = (spx_word16_t*)speex_alloc(N*sizeof(spx_word16_t)); +#ifdef FIXED_POINT + st->wtmp2 = (spx_word16_t*)speex_alloc(N*sizeof(spx_word16_t)); + for (i=0;i>1;i++) + { + st->window[i] = (16383-SHL16(spx_cos(DIV32_16(MULT16_16(25736,i<<1),N)),1)); + st->window[N-i-1] = st->window[i]; + } +#else + for (i=0;iwindow[i] = .5-.5*cos(2*M_PI*i/N); +#endif + for (i=0;i<=st->frame_size;i++) + st->power_1[i] = FLOAT_ONE; + for (i=0;iW[i] = 0; + { + spx_word32_t sum = 0; + /* Ratio of ~10 between adaptation rate of first and last block */ + spx_word16_t decay = SHR32(spx_exp(NEG16(DIV32_16(QCONST16(2.4,11),M))),1); + st->prop[0] = QCONST16(.7, 15); + sum = EXTEND32(st->prop[0]); + for (i=1;iprop[i] = MULT16_16_Q15(st->prop[i-1], decay); + sum = ADD32(sum, EXTEND32(st->prop[i])); + } + for (i=M-1;i>=0;i--) + { + st->prop[i] = DIV32(MULT16_16(QCONST16(.8f,15), st->prop[i]),sum); + } + } + + st->memX = (spx_word16_t*)speex_alloc(K*sizeof(spx_word16_t)); + st->memD = (spx_word16_t*)speex_alloc(C*sizeof(spx_word16_t)); + st->memE = (spx_word16_t*)speex_alloc(C*sizeof(spx_word16_t)); + st->preemph = QCONST16(.9,15); + if (st->sampling_rate<12000) + st->notch_radius = QCONST16(.9, 15); + else if (st->sampling_rate<24000) + st->notch_radius = QCONST16(.982, 15); + else + st->notch_radius = QCONST16(.992, 15); + + st->notch_mem = (spx_mem_t*)speex_alloc(2*C*sizeof(spx_mem_t)); + st->adapted = 0; + st->Pey = st->Pyy = FLOAT_ONE; + +#ifdef TWO_PATH + st->Davg1 = st->Davg2 = 0; + st->Dvar1 = st->Dvar2 = FLOAT_ZERO; +#endif + + st->play_buf = (spx_int16_t*)speex_alloc(K*(PLAYBACK_DELAY+1)*st->frame_size*sizeof(spx_int16_t)); + st->play_buf_pos = PLAYBACK_DELAY*st->frame_size; + st->play_buf_started = 0; + + return st; +} + +/** Resets echo canceller state */ +EXPORT void speex_echo_state_reset(SpeexEchoState *st) +{ + int i, M, N, C, K; + st->cancel_count=0; + st->screwed_up = 0; + N = st->window_size; + M = st->M; + C=st->C; + K=st->K; + for (i=0;iW[i] = 0; +#ifdef TWO_PATH + for (i=0;iforeground[i] = 0; +#endif + for (i=0;iX[i] = 0; + for (i=0;i<=st->frame_size;i++) + { + st->power[i] = 0; + st->power_1[i] = FLOAT_ONE; + st->Eh[i] = 0; + st->Yh[i] = 0; + } + for (i=0;iframe_size;i++) + { + st->last_y[i] = 0; + } + for (i=0;iE[i] = 0; + } + for (i=0;ix[i] = 0; + } + for (i=0;i<2*C;i++) + st->notch_mem[i] = 0; + for (i=0;imemD[i]=st->memE[i]=0; + for (i=0;imemX[i]=0; + + st->saturated = 0; + st->adapted = 0; + st->sum_adapt = 0; + st->Pey = st->Pyy = FLOAT_ONE; +#ifdef TWO_PATH + st->Davg1 = st->Davg2 = 0; + st->Dvar1 = st->Dvar2 = FLOAT_ZERO; +#endif + for (i=0;i<3*st->frame_size;i++) + st->play_buf[i] = 0; + st->play_buf_pos = PLAYBACK_DELAY*st->frame_size; + st->play_buf_started = 0; + +} + +/** Destroys an echo canceller state */ +EXPORT void speex_echo_state_destroy(SpeexEchoState *st) +{ + spx_fft_destroy(st->fft_table); + + speex_free(st->e); + speex_free(st->x); + speex_free(st->input); + speex_free(st->y); + speex_free(st->last_y); + speex_free(st->Yf); + speex_free(st->Rf); + speex_free(st->Xf); + speex_free(st->Yh); + speex_free(st->Eh); + + speex_free(st->X); + speex_free(st->Y); + speex_free(st->E); + speex_free(st->W); +#ifdef TWO_PATH + speex_free(st->foreground); +#endif + speex_free(st->PHI); + speex_free(st->power); + speex_free(st->power_1); + speex_free(st->window); + speex_free(st->prop); + speex_free(st->wtmp); +#ifdef FIXED_POINT + speex_free(st->wtmp2); +#endif + speex_free(st->memX); + speex_free(st->memD); + speex_free(st->memE); + speex_free(st->notch_mem); + + speex_free(st->play_buf); + speex_free(st); + +#ifdef DUMP_ECHO_CANCEL_DATA + fclose(rFile); + fclose(pFile); + fclose(oFile); + rFile = pFile = oFile = NULL; +#endif +} + +EXPORT void speex_echo_capture(SpeexEchoState *st, const spx_int16_t *rec, spx_int16_t *out) +{ + int i; + /*speex_warning_int("capture with fill level ", st->play_buf_pos/st->frame_size);*/ + st->play_buf_started = 1; + if (st->play_buf_pos>=st->frame_size) + { + speex_echo_cancellation(st, rec, st->play_buf, out); + st->play_buf_pos -= st->frame_size; + for (i=0;iplay_buf_pos;i++) + st->play_buf[i] = st->play_buf[i+st->frame_size]; + } else { + speex_warning("No playback frame available (your application is buggy and/or got xruns)"); + if (st->play_buf_pos!=0) + { + speex_warning("internal playback buffer corruption?"); + st->play_buf_pos = 0; + } + for (i=0;iframe_size;i++) + out[i] = rec[i]; + } +} + +EXPORT void speex_echo_playback(SpeexEchoState *st, const spx_int16_t *play) +{ + /*speex_warning_int("playback with fill level ", st->play_buf_pos/st->frame_size);*/ + if (!st->play_buf_started) + { + speex_warning("discarded first playback frame"); + return; + } + if (st->play_buf_pos<=PLAYBACK_DELAY*st->frame_size) + { + int i; + for (i=0;iframe_size;i++) + st->play_buf[st->play_buf_pos+i] = play[i]; + st->play_buf_pos += st->frame_size; + if (st->play_buf_pos <= (PLAYBACK_DELAY-1)*st->frame_size) + { + speex_warning("Auto-filling the buffer (your application is buggy and/or got xruns)"); + for (i=0;iframe_size;i++) + st->play_buf[st->play_buf_pos+i] = play[i]; + st->play_buf_pos += st->frame_size; + } + } else { + speex_warning("Had to discard a playback frame (your application is buggy and/or got xruns)"); + } +} + +/** Performs echo cancellation on a frame (deprecated, last arg now ignored) */ +EXPORT void speex_echo_cancel(SpeexEchoState *st, const spx_int16_t *in, const spx_int16_t *far_end, spx_int16_t *out, spx_int32_t *Yout) +{ + speex_echo_cancellation(st, in, far_end, out); +} + +/** Performs echo cancellation on a frame */ +EXPORT void speex_echo_cancellation(SpeexEchoState *st, const spx_int16_t *in, const spx_int16_t *far_end, spx_int16_t *out) +{ + int i,j, chan, speak; + int N,M, C, K; + spx_word32_t Syy,See,Sxx,Sdd, Sff; +#ifdef TWO_PATH + spx_word32_t Dbf; + int update_foreground; +#endif + spx_word32_t Sey; + spx_word16_t ss, ss_1; + spx_float_t Pey = FLOAT_ONE, Pyy=FLOAT_ONE; + spx_float_t alpha, alpha_1; + spx_word16_t RER; + spx_word32_t tmp32; + + N = st->window_size; + M = st->M; + C = st->C; + K = st->K; + + st->cancel_count++; +#ifdef FIXED_POINT + ss=DIV32_16(11469,M); + ss_1 = SUB16(32767,ss); +#else + ss=.35/M; + ss_1 = 1-ss; +#endif + + for (chan = 0; chan < C; chan++) + { + /* Apply a notch filter to make sure DC doesn't end up causing problems */ + filter_dc_notch16(in+chan, st->notch_radius, st->input+chan*st->frame_size, st->frame_size, st->notch_mem+2*chan, C); + /* Copy input data to buffer and apply pre-emphasis */ + /* Copy input data to buffer */ + for (i=0;iframe_size;i++) + { + spx_word32_t tmp32; + /* FIXME: This core has changed a bit, need to merge properly */ + tmp32 = SUB32(EXTEND32(st->input[chan*st->frame_size+i]), EXTEND32(MULT16_16_P15(st->preemph, st->memD[chan]))); +#ifdef FIXED_POINT + if (tmp32 > 32767) + { + tmp32 = 32767; + if (st->saturated == 0) + st->saturated = 1; + } + if (tmp32 < -32767) + { + tmp32 = -32767; + if (st->saturated == 0) + st->saturated = 1; + } +#endif + st->memD[chan] = st->input[chan*st->frame_size+i]; + st->input[chan*st->frame_size+i] = EXTRACT16(tmp32); + } + } + + for (speak = 0; speak < K; speak++) + { + for (i=0;iframe_size;i++) + { + spx_word32_t tmp32; + st->x[speak*N+i] = st->x[speak*N+i+st->frame_size]; + tmp32 = SUB32(EXTEND32(far_end[i*K+speak]), EXTEND32(MULT16_16_P15(st->preemph, st->memX[speak]))); +#ifdef FIXED_POINT + /*FIXME: If saturation occurs here, we need to freeze adaptation for M frames (not just one) */ + if (tmp32 > 32767) + { + tmp32 = 32767; + st->saturated = M+1; + } + if (tmp32 < -32767) + { + tmp32 = -32767; + st->saturated = M+1; + } +#endif + st->x[speak*N+i+st->frame_size] = EXTRACT16(tmp32); + st->memX[speak] = far_end[i*K+speak]; + } + } + + for (speak = 0; speak < K; speak++) + { + /* Shift memory: this could be optimized eventually*/ + for (j=M-1;j>=0;j--) + { + for (i=0;iX[(j+1)*N*K+speak*N+i] = st->X[j*N*K+speak*N+i]; + } + /* Convert x (echo input) to frequency domain */ + spx_fft(st->fft_table, st->x+speak*N, &st->X[speak*N]); + } + + Sxx = 0; + for (speak = 0; speak < K; speak++) + { + Sxx += mdf_inner_prod(st->x+speak*N+st->frame_size, st->x+speak*N+st->frame_size, st->frame_size); + power_spectrum_accum(st->X+speak*N, st->Xf, N); + } + + Sff = 0; + for (chan = 0; chan < C; chan++) + { +#ifdef TWO_PATH + /* Compute foreground filter */ + spectral_mul_accum16(st->X, st->foreground+chan*N*K*M, st->Y+chan*N, N, M*K); + spx_ifft(st->fft_table, st->Y+chan*N, st->e+chan*N); + for (i=0;iframe_size;i++) + st->e[chan*N+i] = SUB16(st->input[chan*st->frame_size+i], st->e[chan*N+i+st->frame_size]); + Sff += mdf_inner_prod(st->e+chan*N, st->e+chan*N, st->frame_size); +#endif + } + + /* Adjust proportional adaption rate */ + /* FIXME: Adjust that for C, K*/ + if (st->adapted) + mdf_adjust_prop (st->W, N, M, C*K, st->prop); + /* Compute weight gradient */ + if (st->saturated == 0) + { + for (chan = 0; chan < C; chan++) + { + for (speak = 0; speak < K; speak++) + { + for (j=M-1;j>=0;j--) + { + weighted_spectral_mul_conj(st->power_1, FLOAT_SHL(PSEUDOFLOAT(st->prop[j]),-15), &st->X[(j+1)*N*K+speak*N], st->E+chan*N, st->PHI, N); + for (i=0;iW[chan*N*K*M + j*N*K + speak*N + i] += st->PHI[i]; + } + } + } + } else { + st->saturated--; + } + + /* FIXME: MC conversion required */ + /* Update weight to prevent circular convolution (MDF / AUMDF) */ + for (chan = 0; chan < C; chan++) + { + for (speak = 0; speak < K; speak++) + { + for (j=0;jcancel_count%(M-1) == j-1) + { +#ifdef FIXED_POINT + for (i=0;iwtmp2[i] = EXTRACT16(PSHR32(st->W[chan*N*K*M + j*N*K + speak*N + i],NORMALIZE_SCALEDOWN+16)); + spx_ifft(st->fft_table, st->wtmp2, st->wtmp); + for (i=0;iframe_size;i++) + { + st->wtmp[i]=0; + } + for (i=st->frame_size;iwtmp[i]=SHL16(st->wtmp[i],NORMALIZE_SCALEUP); + } + spx_fft(st->fft_table, st->wtmp, st->wtmp2); + /* The "-1" in the shift is a sort of kludge that trades less efficient update speed for decrease noise */ + for (i=0;iW[chan*N*K*M + j*N*K + speak*N + i] -= SHL32(EXTEND32(st->wtmp2[i]),16+NORMALIZE_SCALEDOWN-NORMALIZE_SCALEUP-1); +#else + spx_ifft(st->fft_table, &st->W[chan*N*K*M + j*N*K + speak*N], st->wtmp); + for (i=st->frame_size;iwtmp[i]=0; + } + spx_fft(st->fft_table, st->wtmp, &st->W[chan*N*K*M + j*N*K + speak*N]); +#endif + } + } + } + } + + /* So we can use power_spectrum_accum */ + for (i=0;i<=st->frame_size;i++) + st->Rf[i] = st->Yf[i] = st->Xf[i] = 0; + + Dbf = 0; + See = 0; +#ifdef TWO_PATH + /* Difference in response, this is used to estimate the variance of our residual power estimate */ + for (chan = 0; chan < C; chan++) + { + spectral_mul_accum(st->X, st->W+chan*N*K*M, st->Y+chan*N, N, M*K); + spx_ifft(st->fft_table, st->Y+chan*N, st->y+chan*N); + for (i=0;iframe_size;i++) + st->e[chan*N+i] = SUB16(st->e[chan*N+i+st->frame_size], st->y[chan*N+i+st->frame_size]); + Dbf += 10+mdf_inner_prod(st->e+chan*N, st->e+chan*N, st->frame_size); + for (i=0;iframe_size;i++) + st->e[chan*N+i] = SUB16(st->input[chan*st->frame_size+i], st->y[chan*N+i+st->frame_size]); + See += mdf_inner_prod(st->e+chan*N, st->e+chan*N, st->frame_size); + } +#endif + +#ifndef TWO_PATH + Sff = See; +#endif + +#ifdef TWO_PATH + /* Logic for updating the foreground filter */ + + /* For two time windows, compute the mean of the energy difference, as well as the variance */ + st->Davg1 = ADD32(MULT16_32_Q15(QCONST16(.6f,15),st->Davg1), MULT16_32_Q15(QCONST16(.4f,15),SUB32(Sff,See))); + st->Davg2 = ADD32(MULT16_32_Q15(QCONST16(.85f,15),st->Davg2), MULT16_32_Q15(QCONST16(.15f,15),SUB32(Sff,See))); + st->Dvar1 = FLOAT_ADD(FLOAT_MULT(VAR1_SMOOTH, st->Dvar1), FLOAT_MUL32U(MULT16_32_Q15(QCONST16(.4f,15),Sff), MULT16_32_Q15(QCONST16(.4f,15),Dbf))); + st->Dvar2 = FLOAT_ADD(FLOAT_MULT(VAR2_SMOOTH, st->Dvar2), FLOAT_MUL32U(MULT16_32_Q15(QCONST16(.15f,15),Sff), MULT16_32_Q15(QCONST16(.15f,15),Dbf))); + + /* Equivalent float code: + st->Davg1 = .6*st->Davg1 + .4*(Sff-See); + st->Davg2 = .85*st->Davg2 + .15*(Sff-See); + st->Dvar1 = .36*st->Dvar1 + .16*Sff*Dbf; + st->Dvar2 = .7225*st->Dvar2 + .0225*Sff*Dbf; + */ + + update_foreground = 0; + /* Check if we have a statistically significant reduction in the residual echo */ + /* Note that this is *not* Gaussian, so we need to be careful about the longer tail */ + if (FLOAT_GT(FLOAT_MUL32U(SUB32(Sff,See),ABS32(SUB32(Sff,See))), FLOAT_MUL32U(Sff,Dbf))) + update_foreground = 1; + else if (FLOAT_GT(FLOAT_MUL32U(st->Davg1, ABS32(st->Davg1)), FLOAT_MULT(VAR1_UPDATE,(st->Dvar1)))) + update_foreground = 1; + else if (FLOAT_GT(FLOAT_MUL32U(st->Davg2, ABS32(st->Davg2)), FLOAT_MULT(VAR2_UPDATE,(st->Dvar2)))) + update_foreground = 1; + + /* Do we update? */ + if (update_foreground) + { + st->Davg1 = st->Davg2 = 0; + st->Dvar1 = st->Dvar2 = FLOAT_ZERO; + /* Copy background filter to foreground filter */ + for (i=0;iforeground[i] = EXTRACT16(PSHR32(st->W[i],16)); + /* Apply a smooth transition so as to not introduce blocking artifacts */ + for (chan = 0; chan < C; chan++) + for (i=0;iframe_size;i++) + st->e[chan*N+i+st->frame_size] = MULT16_16_Q15(st->window[i+st->frame_size],st->e[chan*N+i+st->frame_size]) + MULT16_16_Q15(st->window[i],st->y[chan*N+i+st->frame_size]); + } else { + int reset_background=0; + /* Otherwise, check if the background filter is significantly worse */ + if (FLOAT_GT(FLOAT_MUL32U(NEG32(SUB32(Sff,See)),ABS32(SUB32(Sff,See))), FLOAT_MULT(VAR_BACKTRACK,FLOAT_MUL32U(Sff,Dbf)))) + reset_background = 1; + if (FLOAT_GT(FLOAT_MUL32U(NEG32(st->Davg1), ABS32(st->Davg1)), FLOAT_MULT(VAR_BACKTRACK,st->Dvar1))) + reset_background = 1; + if (FLOAT_GT(FLOAT_MUL32U(NEG32(st->Davg2), ABS32(st->Davg2)), FLOAT_MULT(VAR_BACKTRACK,st->Dvar2))) + reset_background = 1; + if (reset_background) + { + /* Copy foreground filter to background filter */ + for (i=0;iW[i] = SHL32(EXTEND32(st->foreground[i]),16); + /* We also need to copy the output so as to get correct adaptation */ + for (chan = 0; chan < C; chan++) + { + for (i=0;iframe_size;i++) + st->y[chan*N+i+st->frame_size] = st->e[chan*N+i+st->frame_size]; + for (i=0;iframe_size;i++) + st->e[chan*N+i] = SUB16(st->input[chan*st->frame_size+i], st->y[chan*N+i+st->frame_size]); + } + See = Sff; + st->Davg1 = st->Davg2 = 0; + st->Dvar1 = st->Dvar2 = FLOAT_ZERO; + } + } +#endif + + Sey = Syy = Sdd = 0; + for (chan = 0; chan < C; chan++) + { + /* Compute error signal (for the output with de-emphasis) */ + for (i=0;iframe_size;i++) + { + spx_word32_t tmp_out; +#ifdef TWO_PATH + tmp_out = SUB32(EXTEND32(st->input[chan*st->frame_size+i]), EXTEND32(st->e[chan*N+i+st->frame_size])); +#else + tmp_out = SUB32(EXTEND32(st->input[chan*st->frame_size+i]), EXTEND32(st->y[chan*N+i+st->frame_size])); +#endif + tmp_out = ADD32(tmp_out, EXTEND32(MULT16_16_P15(st->preemph, st->memE[chan]))); + /* This is an arbitrary test for saturation in the microphone signal */ + if (in[i*C+chan] <= -32000 || in[i*C+chan] >= 32000) + { + if (st->saturated == 0) + st->saturated = 1; + } + out[i*C+chan] = WORD2INT(tmp_out); + st->memE[chan] = tmp_out; + } + +#ifdef DUMP_ECHO_CANCEL_DATA + dump_audio(in, far_end, out, st->frame_size); +#endif + + /* Compute error signal (filter update version) */ + for (i=0;iframe_size;i++) + { + st->e[chan*N+i+st->frame_size] = st->e[chan*N+i]; + st->e[chan*N+i] = 0; + } + + /* Compute a bunch of correlations */ + /* FIXME: bad merge */ + Sey += mdf_inner_prod(st->e+chan*N+st->frame_size, st->y+chan*N+st->frame_size, st->frame_size); + Syy += mdf_inner_prod(st->y+chan*N+st->frame_size, st->y+chan*N+st->frame_size, st->frame_size); + Sdd += mdf_inner_prod(st->input+chan*st->frame_size, st->input+chan*st->frame_size, st->frame_size); + + /* Convert error to frequency domain */ + spx_fft(st->fft_table, st->e+chan*N, st->E+chan*N); + for (i=0;iframe_size;i++) + st->y[i+chan*N] = 0; + spx_fft(st->fft_table, st->y+chan*N, st->Y+chan*N); + + /* Compute power spectrum of echo (X), error (E) and filter response (Y) */ + power_spectrum_accum(st->E+chan*N, st->Rf, N); + power_spectrum_accum(st->Y+chan*N, st->Yf, N); + + } + + /*printf ("%f %f %f %f\n", Sff, See, Syy, Sdd, st->update_cond);*/ + + /* Do some sanity check */ + if (!(Syy>=0 && Sxx>=0 && See >= 0) +#ifndef FIXED_POINT + || !(Sff < N*1e9 && Syy < N*1e9 && Sxx < N*1e9) +#endif + ) + { + /* Things have gone really bad */ + st->screwed_up += 50; + for (i=0;iframe_size*C;i++) + out[i] = 0; + } else if (SHR32(Sff, 2) > ADD32(Sdd, SHR32(MULT16_16(N, 10000),6))) + { + /* AEC seems to add lots of echo instead of removing it, let's see if it will improve */ + st->screwed_up++; + } else { + /* Everything's fine */ + st->screwed_up=0; + } + if (st->screwed_up>=50) + { + speex_warning("The echo canceller started acting funny and got slapped (reset). It swears it will behave now."); + speex_echo_state_reset(st); + return; + } + + /* Add a small noise floor to make sure not to have problems when dividing */ + See = MAX32(See, SHR32(MULT16_16(N, 100),6)); + + for (speak = 0; speak < K; speak++) + { + Sxx += mdf_inner_prod(st->x+speak*N+st->frame_size, st->x+speak*N+st->frame_size, st->frame_size); + power_spectrum_accum(st->X+speak*N, st->Xf, N); + } + + + /* Smooth far end energy estimate over time */ + for (j=0;j<=st->frame_size;j++) + st->power[j] = MULT16_32_Q15(ss_1,st->power[j]) + 1 + MULT16_32_Q15(ss,st->Xf[j]); + + /* Compute filtered spectra and (cross-)correlations */ + for (j=st->frame_size;j>=0;j--) + { + spx_float_t Eh, Yh; + Eh = PSEUDOFLOAT(st->Rf[j] - st->Eh[j]); + Yh = PSEUDOFLOAT(st->Yf[j] - st->Yh[j]); + Pey = FLOAT_ADD(Pey,FLOAT_MULT(Eh,Yh)); + Pyy = FLOAT_ADD(Pyy,FLOAT_MULT(Yh,Yh)); +#ifdef FIXED_POINT + st->Eh[j] = MAC16_32_Q15(MULT16_32_Q15(SUB16(32767,st->spec_average),st->Eh[j]), st->spec_average, st->Rf[j]); + st->Yh[j] = MAC16_32_Q15(MULT16_32_Q15(SUB16(32767,st->spec_average),st->Yh[j]), st->spec_average, st->Yf[j]); +#else + st->Eh[j] = (1-st->spec_average)*st->Eh[j] + st->spec_average*st->Rf[j]; + st->Yh[j] = (1-st->spec_average)*st->Yh[j] + st->spec_average*st->Yf[j]; +#endif + } + + Pyy = FLOAT_SQRT(Pyy); + Pey = FLOAT_DIVU(Pey,Pyy); + + /* Compute correlation updatete rate */ + tmp32 = MULT16_32_Q15(st->beta0,Syy); + if (tmp32 > MULT16_32_Q15(st->beta_max,See)) + tmp32 = MULT16_32_Q15(st->beta_max,See); + alpha = FLOAT_DIV32(tmp32, See); + alpha_1 = FLOAT_SUB(FLOAT_ONE, alpha); + /* Update correlations (recursive average) */ + st->Pey = FLOAT_ADD(FLOAT_MULT(alpha_1,st->Pey) , FLOAT_MULT(alpha,Pey)); + st->Pyy = FLOAT_ADD(FLOAT_MULT(alpha_1,st->Pyy) , FLOAT_MULT(alpha,Pyy)); + if (FLOAT_LT(st->Pyy, FLOAT_ONE)) + st->Pyy = FLOAT_ONE; + /* We don't really hope to get better than 33 dB (MIN_LEAK-3dB) attenuation anyway */ + if (FLOAT_LT(st->Pey, FLOAT_MULT(MIN_LEAK,st->Pyy))) + st->Pey = FLOAT_MULT(MIN_LEAK,st->Pyy); + if (FLOAT_GT(st->Pey, st->Pyy)) + st->Pey = st->Pyy; + /* leak_estimate is the linear regression result */ + st->leak_estimate = FLOAT_EXTRACT16(FLOAT_SHL(FLOAT_DIVU(st->Pey, st->Pyy),14)); + /* This looks like a stupid bug, but it's right (because we convert from Q14 to Q15) */ + if (st->leak_estimate > 16383) + st->leak_estimate = 32767; + else + st->leak_estimate = SHL16(st->leak_estimate,1); + /*printf ("%f\n", st->leak_estimate);*/ + + /* Compute Residual to Error Ratio */ +#ifdef FIXED_POINT + tmp32 = MULT16_32_Q15(st->leak_estimate,Syy); + tmp32 = ADD32(SHR32(Sxx,13), ADD32(tmp32, SHL32(tmp32,1))); + /* Check for y in e (lower bound on RER) */ + { + spx_float_t bound = PSEUDOFLOAT(Sey); + bound = FLOAT_DIVU(FLOAT_MULT(bound, bound), PSEUDOFLOAT(ADD32(1,Syy))); + if (FLOAT_GT(bound, PSEUDOFLOAT(See))) + tmp32 = See; + else if (tmp32 < FLOAT_EXTRACT32(bound)) + tmp32 = FLOAT_EXTRACT32(bound); + } + if (tmp32 > SHR32(See,1)) + tmp32 = SHR32(See,1); + RER = FLOAT_EXTRACT16(FLOAT_SHL(FLOAT_DIV32(tmp32,See),15)); +#else + RER = (.0001*Sxx + 3.*MULT16_32_Q15(st->leak_estimate,Syy)) / See; + /* Check for y in e (lower bound on RER) */ + if (RER < Sey*Sey/(1+See*Syy)) + RER = Sey*Sey/(1+See*Syy); + if (RER > .5) + RER = .5; +#endif + + /* We consider that the filter has had minimal adaptation if the following is true*/ + if (!st->adapted && st->sum_adapt > SHL32(EXTEND32(M),15) && MULT16_32_Q15(st->leak_estimate,Syy) > MULT16_32_Q15(QCONST16(.03f,15),Syy)) + { + st->adapted = 1; + } + + if (st->adapted) + { + /* Normal learning rate calculation once we're past the minimal adaptation phase */ + for (i=0;i<=st->frame_size;i++) + { + spx_word32_t r, e; + /* Compute frequency-domain adaptation mask */ + r = MULT16_32_Q15(st->leak_estimate,SHL32(st->Yf[i],3)); + e = SHL32(st->Rf[i],3)+1; +#ifdef FIXED_POINT + if (r>SHR32(e,1)) + r = SHR32(e,1); +#else + if (r>.5*e) + r = .5*e; +#endif + r = MULT16_32_Q15(QCONST16(.7,15),r) + MULT16_32_Q15(QCONST16(.3,15),(spx_word32_t)(MULT16_32_Q15(RER,e))); + /*st->power_1[i] = adapt_rate*r/(e*(1+st->power[i]));*/ + st->power_1[i] = FLOAT_SHL(FLOAT_DIV32_FLOAT(r,FLOAT_MUL32U(e,st->power[i]+10)),WEIGHT_SHIFT+16); + } + } else { + /* Temporary adaption rate if filter is not yet adapted enough */ + spx_word16_t adapt_rate=0; + + if (Sxx > SHR32(MULT16_16(N, 1000),6)) + { + tmp32 = MULT16_32_Q15(QCONST16(.25f, 15), Sxx); +#ifdef FIXED_POINT + if (tmp32 > SHR32(See,2)) + tmp32 = SHR32(See,2); +#else + if (tmp32 > .25*See) + tmp32 = .25*See; +#endif + adapt_rate = FLOAT_EXTRACT16(FLOAT_SHL(FLOAT_DIV32(tmp32, See),15)); + } + for (i=0;i<=st->frame_size;i++) + st->power_1[i] = FLOAT_SHL(FLOAT_DIV32(EXTEND32(adapt_rate),ADD32(st->power[i],10)),WEIGHT_SHIFT+1); + + + /* How much have we adapted so far? */ + st->sum_adapt = ADD32(st->sum_adapt,adapt_rate); + } + + /* FIXME: MC conversion required */ + for (i=0;iframe_size;i++) + st->last_y[i] = st->last_y[st->frame_size+i]; + if (st->adapted) + { + /* If the filter is adapted, take the filtered echo */ + for (i=0;iframe_size;i++) + st->last_y[st->frame_size+i] = in[i]-out[i]; + } else { + /* If filter isn't adapted yet, all we can do is take the far end signal directly */ + /* moved earlier: for (i=0;ilast_y[i] = st->x[i];*/ + } + +} + +/* Compute spectrum of estimated echo for use in an echo post-filter */ +void speex_echo_get_residual(SpeexEchoState *st, spx_word32_t *residual_echo, int len) +{ + int i; + spx_word16_t leak2; + int N; + + N = st->window_size; + + /* Apply hanning window (should pre-compute it)*/ + for (i=0;iy[i] = MULT16_16_Q15(st->window[i],st->last_y[i]); + + /* Compute power spectrum of the echo */ + spx_fft(st->fft_table, st->y, st->Y); + power_spectrum(st->Y, residual_echo, N); + +#ifdef FIXED_POINT + if (st->leak_estimate > 16383) + leak2 = 32767; + else + leak2 = SHL16(st->leak_estimate, 1); +#else + if (st->leak_estimate>.5) + leak2 = 1; + else + leak2 = 2*st->leak_estimate; +#endif + /* Estimate residual echo */ + for (i=0;i<=st->frame_size;i++) + residual_echo[i] = (spx_int32_t)MULT16_32_Q15(leak2,residual_echo[i]); + +} + +EXPORT int speex_echo_ctl(SpeexEchoState *st, int request, void *ptr) +{ + switch(request) + { + + case SPEEX_ECHO_GET_FRAME_SIZE: + (*(int*)ptr) = st->frame_size; + break; + case SPEEX_ECHO_SET_SAMPLING_RATE: + st->sampling_rate = (*(int*)ptr); + st->spec_average = DIV32_16(SHL32(EXTEND32(st->frame_size), 15), st->sampling_rate); +#ifdef FIXED_POINT + st->beta0 = DIV32_16(SHL32(EXTEND32(st->frame_size), 16), st->sampling_rate); + st->beta_max = DIV32_16(SHL32(EXTEND32(st->frame_size), 14), st->sampling_rate); +#else + st->beta0 = (2.0f*st->frame_size)/st->sampling_rate; + st->beta_max = (.5f*st->frame_size)/st->sampling_rate; +#endif + if (st->sampling_rate<12000) + st->notch_radius = QCONST16(.9, 15); + else if (st->sampling_rate<24000) + st->notch_radius = QCONST16(.982, 15); + else + st->notch_radius = QCONST16(.992, 15); + break; + case SPEEX_ECHO_GET_SAMPLING_RATE: + (*(int*)ptr) = st->sampling_rate; + break; + case SPEEX_ECHO_GET_IMPULSE_RESPONSE_SIZE: + /*FIXME: Implement this for multiple channels */ + *((spx_int32_t *)ptr) = st->M * st->frame_size; + break; + case SPEEX_ECHO_GET_IMPULSE_RESPONSE: + { + int M = st->M, N = st->window_size, n = st->frame_size, i, j; + spx_int32_t *filt = (spx_int32_t *) ptr; + for(j=0;jwtmp2[i] = EXTRACT16(PSHR32(st->W[j*N+i],16+NORMALIZE_SCALEDOWN)); + spx_ifft(st->fft_table, st->wtmp2, st->wtmp); +#else + spx_ifft(st->fft_table, &st->W[j*N], st->wtmp); +#endif + for(i=0;iwtmp[i]), WEIGHT_SHIFT-NORMALIZE_SCALEDOWN); + } + } + break; + default: + speex_warning_int("Unknown speex_echo_ctl request: ", request); + return -1; + } + return 0; +} diff --git a/lib/speexdsp/os_support.h b/lib/speexdsp/os_support.h new file mode 100644 index 00000000..0db31a61 --- /dev/null +++ b/lib/speexdsp/os_support.h @@ -0,0 +1,169 @@ +/* Copyright (C) 2007 Jean-Marc Valin + + File: os_support.h + This is the (tiny) OS abstraction layer. Aside from math.h, this is the + only place where system headers are allowed. + + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions are + met: + + 1. Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. + + 2. Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + + 3. The name of the author may not be used to endorse or promote products + derived from this software without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR + IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES + OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, + INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + POSSIBILITY OF SUCH DAMAGE. +*/ + +#ifndef OS_SUPPORT_H +#define OS_SUPPORT_H + +#include +#include +#include + +#ifdef HAVE_CONFIG_H +#include "config.h" +#endif +#ifdef OS_SUPPORT_CUSTOM +#include "os_support_custom.h" +#endif + +/** Speex wrapper for calloc. To do your own dynamic allocation, all you need to do is replace this function, speex_realloc and speex_free + NOTE: speex_alloc needs to CLEAR THE MEMORY */ +#ifndef OVERRIDE_SPEEX_ALLOC +static inline void *speex_alloc (int size) +{ + /* WARNING: this is not equivalent to malloc(). If you want to use malloc() + or your own allocator, YOU NEED TO CLEAR THE MEMORY ALLOCATED. Otherwise + you will experience strange bugs */ + return calloc(size,1); +} +#endif + +/** Same as speex_alloc, except that the area is only needed inside a Speex call (might cause problem with wideband though) */ +#ifndef OVERRIDE_SPEEX_ALLOC_SCRATCH +static inline void *speex_alloc_scratch (int size) +{ + /* Scratch space doesn't need to be cleared */ + return calloc(size,1); +} +#endif + +/** Speex wrapper for realloc. To do your own dynamic allocation, all you need to do is replace this function, speex_alloc and speex_free */ +#ifndef OVERRIDE_SPEEX_REALLOC +static inline void *speex_realloc (void *ptr, int size) +{ + return realloc(ptr, size); +} +#endif + +/** Speex wrapper for calloc. To do your own dynamic allocation, all you need to do is replace this function, speex_realloc and speex_alloc */ +#ifndef OVERRIDE_SPEEX_FREE +static inline void speex_free (void *ptr) +{ + free(ptr); +} +#endif + +/** Same as speex_free, except that the area is only needed inside a Speex call (might cause problem with wideband though) */ +#ifndef OVERRIDE_SPEEX_FREE_SCRATCH +static inline void speex_free_scratch (void *ptr) +{ + free(ptr); +} +#endif + +/** Copy n elements from src to dst. The 0* term provides compile-time type checking */ +#ifndef OVERRIDE_SPEEX_COPY +#define SPEEX_COPY(dst, src, n) (memcpy((dst), (src), (n)*sizeof(*(dst)) + 0*((dst)-(src)) )) +#endif + +/** Copy n elements from src to dst, allowing overlapping regions. The 0* term + provides compile-time type checking */ +#ifndef OVERRIDE_SPEEX_MOVE +#define SPEEX_MOVE(dst, src, n) (memmove((dst), (src), (n)*sizeof(*(dst)) + 0*((dst)-(src)) )) +#endif + +/** For n elements worth of memory, set every byte to the value of c, starting at address dst */ +#ifndef OVERRIDE_SPEEX_MEMSET +#define SPEEX_MEMSET(dst, c, n) (memset((dst), (c), (n)*sizeof(*(dst)))) +#endif + + +#ifndef OVERRIDE_SPEEX_FATAL +static inline void _speex_fatal(const char *str, const char *file, int line) +{ + fprintf (stderr, "Fatal (internal) error in %s, line %d: %s\n", file, line, str); + exit(1); +} +#endif + +#ifndef OVERRIDE_SPEEX_WARNING +static inline void speex_warning(const char *str) +{ +#ifndef DISABLE_WARNINGS + fprintf (stderr, "warning: %s\n", str); +#endif +} +#endif + +#ifndef OVERRIDE_SPEEX_WARNING_INT +static inline void speex_warning_int(const char *str, int val) +{ +#ifndef DISABLE_WARNINGS + fprintf (stderr, "warning: %s %d\n", str, val); +#endif +} +#endif + +#ifndef OVERRIDE_SPEEX_NOTIFY +static inline void speex_notify(const char *str) +{ +#ifndef DISABLE_NOTIFICATIONS + fprintf (stderr, "notification: %s\n", str); +#endif +} +#endif + +#ifndef OVERRIDE_SPEEX_PUTC +/** Speex wrapper for putc */ +static inline void _speex_putc(int ch, void *file) +{ + FILE *f = (FILE *)file; + fprintf(f, "%c", ch); +} +#endif + +#define speex_fatal(str) _speex_fatal(str, __FILE__, __LINE__); +#define speex_assert(cond) {if (!(cond)) {speex_fatal("assertion failed: " #cond);}} + +#ifndef RELEASE +static inline void print_vec(float *vec, int len, char *name) +{ + int i; + printf ("%s ", name); + for (i=0;i + +#ifdef FIXED_POINT + +typedef struct { + spx_int16_t m; + spx_int16_t e; +} spx_float_t; + +static const spx_float_t FLOAT_ZERO = {0,0}; +static const spx_float_t FLOAT_ONE = {16384,-14}; +static const spx_float_t FLOAT_HALF = {16384,-15}; + +#define MIN(a,b) ((a)<(b)?(a):(b)) +static inline spx_float_t PSEUDOFLOAT(spx_int32_t x) +{ + int e=0; + int sign=0; + if (x<0) + { + sign = 1; + x = -x; + } + if (x==0) + { + spx_float_t r = {0,0}; + return r; + } + e = spx_ilog2(ABS32(x))-14; + x = VSHR32(x, e); + if (sign) + { + spx_float_t r; + r.m = -x; + r.e = e; + return r; + } + else + { + spx_float_t r; + r.m = x; + r.e = e; + return r; + } +} + + +static inline spx_float_t FLOAT_ADD(spx_float_t a, spx_float_t b) +{ + spx_float_t r; + if (a.m==0) + return b; + else if (b.m==0) + return a; + if ((a).e > (b).e) + { + r.m = ((a).m>>1) + ((b).m>>MIN(15,(a).e-(b).e+1)); + r.e = (a).e+1; + } + else + { + r.m = ((b).m>>1) + ((a).m>>MIN(15,(b).e-(a).e+1)); + r.e = (b).e+1; + } + if (r.m>0) + { + if (r.m<16384) + { + r.m<<=1; + r.e-=1; + } + } else { + if (r.m>-16384) + { + r.m<<=1; + r.e-=1; + } + } + /*printf ("%f + %f = %f\n", REALFLOAT(a), REALFLOAT(b), REALFLOAT(r));*/ + return r; +} + +static inline spx_float_t FLOAT_SUB(spx_float_t a, spx_float_t b) +{ + spx_float_t r; + if (a.m==0) + return b; + else if (b.m==0) + return a; + if ((a).e > (b).e) + { + r.m = ((a).m>>1) - ((b).m>>MIN(15,(a).e-(b).e+1)); + r.e = (a).e+1; + } + else + { + r.m = ((a).m>>MIN(15,(b).e-(a).e+1)) - ((b).m>>1); + r.e = (b).e+1; + } + if (r.m>0) + { + if (r.m<16384) + { + r.m<<=1; + r.e-=1; + } + } else { + if (r.m>-16384) + { + r.m<<=1; + r.e-=1; + } + } + /*printf ("%f + %f = %f\n", REALFLOAT(a), REALFLOAT(b), REALFLOAT(r));*/ + return r; +} + +static inline int FLOAT_LT(spx_float_t a, spx_float_t b) +{ + if (a.m==0) + return b.m>0; + else if (b.m==0) + return a.m<0; + if ((a).e > (b).e) + return ((a).m>>1) < ((b).m>>MIN(15,(a).e-(b).e+1)); + else + return ((b).m>>1) > ((a).m>>MIN(15,(b).e-(a).e+1)); + +} + +static inline int FLOAT_GT(spx_float_t a, spx_float_t b) +{ + return FLOAT_LT(b,a); +} + +static inline spx_float_t FLOAT_MULT(spx_float_t a, spx_float_t b) +{ + spx_float_t r; + r.m = (spx_int16_t)((spx_int32_t)(a).m*(b).m>>15); + r.e = (a).e+(b).e+15; + if (r.m>0) + { + if (r.m<16384) + { + r.m<<=1; + r.e-=1; + } + } else { + if (r.m>-16384) + { + r.m<<=1; + r.e-=1; + } + } + /*printf ("%f * %f = %f\n", REALFLOAT(a), REALFLOAT(b), REALFLOAT(r));*/ + return r; +} + +static inline spx_float_t FLOAT_AMULT(spx_float_t a, spx_float_t b) +{ + spx_float_t r; + r.m = (spx_int16_t)((spx_int32_t)(a).m*(b).m>>15); + r.e = (a).e+(b).e+15; + return r; +} + + +static inline spx_float_t FLOAT_SHL(spx_float_t a, int b) +{ + spx_float_t r; + r.m = a.m; + r.e = a.e+b; + return r; +} + +static inline spx_int16_t FLOAT_EXTRACT16(spx_float_t a) +{ + if (a.e<0) + return EXTRACT16((EXTEND32(a.m)+(EXTEND32(1)<<(-a.e-1)))>>-a.e); + else + return a.m<>-a.e; + else + return EXTEND32(a.m)<=SHL32(EXTEND32(b.m-1),15)) + { + a >>= 1; + e++; + } + r.m = DIV32_16(a,b.m); + r.e = e-b.e; + return r; +} + + +/* Do NOT attempt to divide by a negative number */ +static inline spx_float_t FLOAT_DIV32(spx_word32_t a, spx_word32_t b) +{ + int e0=0,e=0; + spx_float_t r; + if (a==0) + { + return FLOAT_ZERO; + } + if (b>32767) + { + e0 = spx_ilog2(b)-14; + b = VSHR32(b, e0); + e0 = -e0; + } + e = spx_ilog2(ABS32(a))-spx_ilog2(b-1)-15; + a = VSHR32(a, e); + if (ABS32(a)>=SHL32(EXTEND32(b-1),15)) + { + a >>= 1; + e++; + } + e += e0; + r.m = DIV32_16(a,b); + r.e = e; + return r; +} + +/* Do NOT attempt to divide by a negative number */ +static inline spx_float_t FLOAT_DIVU(spx_float_t a, spx_float_t b) +{ + int e=0; + spx_int32_t num; + spx_float_t r; + if (b.m<=0) + { + speex_warning_int("Attempted to divide by", b.m); + return FLOAT_ONE; + } + num = a.m; + a.m = ABS16(a.m); + while (a.m >= b.m) + { + e++; + a.m >>= 1; + } + num = num << (15-e); + r.m = DIV32_16(num,b.m); + r.e = a.e-b.e-15+e; + return r; +} + +static inline spx_float_t FLOAT_SQRT(spx_float_t a) +{ + spx_float_t r; + spx_int32_t m; + m = SHL32(EXTEND32(a.m), 14); + r.e = a.e - 14; + if (r.e & 1) + { + r.e -= 1; + m <<= 1; + } + r.e >>= 1; + r.m = spx_sqrt(m); + return r; +} + +#else + +#define spx_float_t float +#define FLOAT_ZERO 0.f +#define FLOAT_ONE 1.f +#define FLOAT_HALF 0.5f +#define PSEUDOFLOAT(x) (x) +#define FLOAT_MULT(a,b) ((a)*(b)) +#define FLOAT_AMULT(a,b) ((a)*(b)) +#define FLOAT_MUL32(a,b) ((a)*(b)) +#define FLOAT_DIV32(a,b) ((a)/(b)) +#define FLOAT_EXTRACT16(a) (a) +#define FLOAT_EXTRACT32(a) (a) +#define FLOAT_ADD(a,b) ((a)+(b)) +#define FLOAT_SUB(a,b) ((a)-(b)) +#define REALFLOAT(x) (x) +#define FLOAT_DIV32_FLOAT(a,b) ((a)/(b)) +#define FLOAT_MUL32U(a,b) ((a)*(b)) +#define FLOAT_SHL(a,b) (a) +#define FLOAT_LT(a,b) ((a)<(b)) +#define FLOAT_GT(a,b) ((a)>(b)) +#define FLOAT_DIVU(a,b) ((a)/(b)) +#define FLOAT_SQRT(a) (spx_sqrt(a)) + +#endif + +#endif diff --git a/src/call/call_audio.c b/src/call/call_audio.c index 64a26b47..9f55e119 100644 --- a/src/call/call_audio.c +++ b/src/call/call_audio.c @@ -11,11 +11,13 @@ #include "call.h" #include "voice_jitter.h" #include "call_tones.h" +#include "../chat/chat_setting.h" #include "../utun_instance.h" #include "../../lib/opus_codec.h" #include "../../lib/debug_config.h" #include "../../lib/mem.h" #include "../../lib/u_async.h" +#include "../../lib/speex_aec.h" #include #include @@ -24,6 +26,8 @@ #define CALL_AUDIO_BITRATE 32000 #define CALL_AJ_MAX_FRAMES 50 /* ~1000мс @20мс */ #define CALL_STALL_TB 5000 /* 500мс без медиа → глитч-тон */ +#define CALL_AEC_FILTER_SAMPLES 14400 /* хвост эха 300мс @48кГц */ +#define CALL_AEC_DELAY_FRAMES 2 /* desktop: минимальная задержка duplex */ static pthread_mutex_t g_mtx = PTHREAD_MUTEX_INITIALIZER; static struct UTUN_INSTANCE* g_inst = NULL; @@ -36,12 +40,18 @@ static opus_codec_encoder_t* g_encoder = NULL; static opus_codec_decoder_t* g_decoder = NULL; static struct vj* g_vj = NULL; static struct call_tones* g_tones = NULL; +static speex_aec_t* g_aec = NULL; +static int g_aec_delay_frames = CALL_AEC_DELAY_FRAMES; +static int g_aec_enabled = 1; /* по умолчанию: настройка aec_enabled */ static const struct call_audio_ops g_ops = { .on_media = call_audio_on_media, .get_stats = call_audio_get_stats, }; +/* Создать AEC с текущей задержкой (объявление; определение ниже). */ +static speex_aec_t* call_audio_aec_create(void); + /* vj decode-коллбэк: дёргается только из аудио-потока (vj_pull). */ static int call_audio_decode_cb(void* arg, const uint8_t* enc, int len, int16_t* pcm, int max_samples) { opus_codec_decoder_t* dec = (opus_codec_decoder_t*)arg; @@ -133,10 +143,21 @@ int call_audio_start(uint64_t call_id) { return -1; } + g_aec_enabled = g_inst ? chat_setting_get_int(g_inst, "aec_enabled", 1) : 1; + speex_aec_t* aec = NULL; + if (g_aec_enabled) { + aec = call_audio_aec_create(); + if (!aec) { + g_aec_enabled = 0; + DEBUG_WARN(DEBUG_CATEGORY_CALL, "%s: AEC create failed — call without echo cancellation", CALL_AUDIO_ID); + } + } + g_encoder = enc; g_decoder = dec; g_vj = j; g_tones = tones; + g_aec = aec; g_call_id = call_id; g_active = 1; g_ending = 0; @@ -144,7 +165,8 @@ int call_audio_start(uint64_t call_id) { g_last_media_tb = 0; pthread_mutex_unlock(&g_mtx); - DEBUG_INFO(DEBUG_CATEGORY_CALL, "%s: started call=0x%016llx", CALL_AUDIO_ID, (unsigned long long)call_id); + DEBUG_INFO(DEBUG_CATEGORY_CALL, "%s: started call=0x%016llx aec=%d delay=%d", CALL_AUDIO_ID, + (unsigned long long)call_id, g_aec ? 1 : 0, g_aec_delay_frames); return 0; } @@ -162,6 +184,7 @@ void call_audio_release(uint64_t call_id) { opus_codec_decoder_t* dec = NULL; struct vj* j = NULL; struct call_tones* tones = NULL; + speex_aec_t* aec = NULL; int was_active = 0; pthread_mutex_lock(&g_mtx); @@ -176,6 +199,7 @@ void call_audio_release(uint64_t call_id) { dec = g_decoder; g_decoder = NULL; j = g_vj; g_vj = NULL; tones = g_tones; g_tones = NULL; + aec = g_aec; g_aec = NULL; } pthread_mutex_unlock(&g_mtx); @@ -186,6 +210,7 @@ void call_audio_release(uint64_t call_id) { if (dec) opus_codec_decoder_destroy(dec); if (j) vj_destroy(j); if (tones) call_tones_destroy(tones); + if (aec) speex_aec_destroy(aec); DEBUG_INFO(DEBUG_CATEGORY_CALL, "%s: released call=0x%016llx", CALL_AUDIO_ID, (unsigned long long)call_id); } @@ -197,12 +222,51 @@ void call_audio_stop(void) { if (cid) call_audio_release(cid); } +/* ── AEC (SpeexDSP): рантайм-управление ── */ + +/* Создать AEC с текущей задержкой. Вызывается под g_mtx. */ +static speex_aec_t* call_audio_aec_create(void) { + return speex_aec_create(CALL_AUDIO_SAMPLE_RATE, CALL_AUDIO_FRAME_SAMPLES, + CALL_AEC_FILTER_SAMPLES, g_aec_delay_frames); +} + +void call_audio_set_aec_enabled(int enabled) { + pthread_mutex_lock(&g_mtx); + g_aec_enabled = enabled ? 1 : 0; + if (g_active) { + if (g_aec_enabled && !g_aec) { + g_aec = call_audio_aec_create(); + if (!g_aec) DEBUG_ERROR(DEBUG_CATEGORY_CALL, "%s: AEC create failed (enabled)", CALL_AUDIO_ID); + else DEBUG_INFO(DEBUG_CATEGORY_CALL, "%s: AEC enabled (delay=%d frames)", CALL_AUDIO_ID, g_aec_delay_frames); + } else if (!g_aec_enabled && g_aec) { + speex_aec_destroy(g_aec); + g_aec = NULL; + DEBUG_INFO(DEBUG_CATEGORY_CALL, "%s: AEC disabled", CALL_AUDIO_ID); + } + } + pthread_mutex_unlock(&g_mtx); +} + +void call_audio_set_aec_delay_frames(int frames) { + if (frames < 1) frames = 1; + pthread_mutex_lock(&g_mtx); + g_aec_delay_frames = frames; + if (g_active && g_aec_enabled && g_aec) { + speex_aec_destroy(g_aec); + g_aec = call_audio_aec_create(); + if (!g_aec) DEBUG_ERROR(DEBUG_CATEGORY_CALL, "%s: AEC recreate failed (delay=%d)", CALL_AUDIO_ID, frames); + else DEBUG_INFO(DEBUG_CATEGORY_CALL, "%s: AEC delay -> %d frames", CALL_AUDIO_ID, frames); + } + pthread_mutex_unlock(&g_mtx); +} + /* ── аудио-поток: TX / RX ── */ int call_audio_feed_pcm(uint64_t call_id, const int16_t* pcm, int count) { if (!pcm || count != CALL_AUDIO_FRAME_SAMPLES) return -1; uint8_t opus[256]; + int16_t cleaned[CALL_AUDIO_FRAME_SAMPLES]; struct UASYNC* ua = NULL; struct UTUN_INSTANCE* inst = NULL; int len = -1; @@ -212,7 +276,12 @@ int call_audio_feed_pcm(uint64_t call_id, const int16_t* pcm, int count) { pthread_mutex_unlock(&g_mtx); return -1; } - len = opus_codec_encode(g_encoder, pcm, count, opus, (int)sizeof(opus)); + const int16_t* tx = pcm; + if (g_aec) { + if (speex_aec_process_capture(g_aec, pcm, count, cleaned) == count) + tx = cleaned; + } + len = opus_codec_encode(g_encoder, tx, count, opus, (int)sizeof(opus)); ua = g_inst ? g_inst->ua : NULL; inst = g_inst; pthread_mutex_unlock(&g_mtx); @@ -253,6 +322,8 @@ int call_audio_pull_pcm(uint64_t call_id, int16_t* out, int max_samples) { if (!active || !j || !tones) return 0; + int n = 0; + /* завершение: доигрываем тон (450мс), затем тишина. Тон стартует лениво * здесь (аудио-поток) — begin_end лишь выставляет флаг, чтобы не гонять * генератор тонов с GUI-потоком. */ @@ -262,25 +333,31 @@ int call_audio_pull_pcm(uint64_t call_id, int16_t* out, int max_samples) { g_ended_started = 1; } if (call_tones_ended_pending(tones)) - return call_tones_render(tones, out, max_samples); - return 0; + n = call_tones_render(tones, out, max_samples); + } else { + n = vj_pull(j, out, max_samples); + if (n < max_samples) { + /* недобор: либо сталь (глитч-тон), либо тишина */ + uint64_t now = get_time_tb(); + int stalled = (last_media != 0) && (now - last_media > CALL_STALL_TB); + if (stalled) { + call_tones_glitch_start(tones); + n += call_tones_render(tones, out + n, max_samples - n); + } else { + call_tones_glitch_stop(tones); + } + for (int i = n; i < max_samples; i++) out[i] = 0; + n = max_samples; + } } - int n = vj_pull(j, out, max_samples); - if (n >= max_samples) return n; - - /* недобор: либо сталь (глитч-тон), либо тишина */ - uint64_t now = get_time_tb(); - int stalled = (last_media != 0) && (now - last_media > CALL_STALL_TB); - if (stalled) { - call_tones_glitch_start(tones); - int m = call_tones_render(tones, out + n, max_samples - n); - n += m; - } else { - call_tones_glitch_stop(tones); + /* рендер-референс для AEC: то, что реально пошло в динамик */ + if (n > 0) { + pthread_mutex_lock(&g_mtx); + if (g_aec) speex_aec_feed_playback(g_aec, out, n); + pthread_mutex_unlock(&g_mtx); } - for (int i = n; i < max_samples; i++) out[i] = 0; - return max_samples; + return n; } /* ── uasync-поток: метрики ── */ diff --git a/src/call/call_audio.h b/src/call/call_audio.h index 50a81e3c..7b42dc01 100644 --- a/src/call/call_audio.h +++ b/src/call/call_audio.h @@ -62,6 +62,15 @@ void call_audio_on_media(struct UTUN_INSTANCE* inst, uint64_t call_id, int call_audio_get_stats(uint64_t call_id, int* buffer_ms, int* tempo_x100, uint32_t* dropped, uint32_t* underruns); +/* Включить/выключить AEC (SpeexDSP) в рантайме. desktop — по настройке GUI, + * Android — по спикерфону (route == SPEAKER). Работает и в активном звонке. */ +void call_audio_set_aec_enabled(int enabled); + +/* Задать задержку рендер→захват AEC в кадрах (20 мс). Android измеряет через + * AudioTrack/AudioRecord getTimestamp, desktop использует дефолт. Если AEC уже + * активен — пересоздаётся с новой задержкой. */ +void call_audio_set_aec_delay_frames(int frames); + #ifdef __cplusplus } #endif diff --git a/src/chat/chat_setting.c b/src/chat/chat_setting.c index 5c5a0017..53cfb8c0 100644 --- a/src/chat/chat_setting.c +++ b/src/chat/chat_setting.c @@ -56,6 +56,7 @@ static const struct chat_setting_def g_setting_defs[] = { {"whisper_enabled", CHAT_SETTING_BOOL, 0, 0, 1}, {"whisper_model_path", CHAT_SETTING_STRING, 0, 0, 0}, {"group_autoconnect", CHAT_SETTING_BOOL, 1, 0, 1}, + {"aec_enabled", CHAT_SETTING_BOOL, 1, 0, 1}, /* AEC (SpeexDSP) в звонке, desktop GUI */ }; #define CHAT_SETTING_COUNT (sizeof(g_setting_defs) / sizeof(g_setting_defs[0])) diff --git a/tests/Makefile.am b/tests/Makefile.am index 9c70b9ea..bde3a6f5 100644 --- a/tests/Makefile.am +++ b/tests/Makefile.am @@ -122,6 +122,9 @@ if HAVE_SILERO_VAD check_PROGRAMS += test_silero_vad endif +# AEC (SpeexDSP): всегда (без внешних зависимостей) +check_PROGRAMS += test_speex_aec + # Долгие тесты: запускаются только вручную noinst_PROGRAMS = @@ -506,6 +509,10 @@ test_silero_vad_SOURCES = test_silero_vad.c test_silero_vad_CFLAGS = -I$(top_srcdir)/lib test_silero_vad_LDADD = $(COMMON_LIBS) @SILERO_VAD_LIBS@ +test_speex_aec_SOURCES = test_speex_aec.c +test_speex_aec_CFLAGS = -I$(top_srcdir)/lib +test_speex_aec_LDADD = $(COMMON_LIBS) + test_media_async_SOURCES = test_media_async.c test_media_async_CFLAGS = -I$(top_srcdir)/src -I$(top_srcdir)/src/media_async -I$(top_srcdir)/lib test_media_async_LDADD = $(LIBUTUN) $(CRYPTO_LIBS) $(COMMON_LIBS) diff --git a/tests/test_speex_aec.c b/tests/test_speex_aec.c new file mode 100644 index 00000000..cb7a6dc6 --- /dev/null +++ b/tests/test_speex_aec.c @@ -0,0 +1,219 @@ +/* Тест обёртки эхоподавления lib/speex_aec (SpeexDSP mdf). + * + * Проверяет: + * - жизненный цикл и NULL/невалидные аргументы (create/destroy/reset/process); + * - линию задержки: passthrough на старте, наполнение до delay_frames, учёт дрейфа; + * - накопление захвата/рендера чанками произвольного размера (480/960); + * - собственно подавление эха: синтетический far-end (шум) + near-end (тон), + * после адаптации остаток эха должен быть заметно меньше исходного эха. + */ +#include +#include +#include + +#include "../lib/speex_aec.h" +#include "../lib/debug_config.h" +#include "../lib/mem.h" + +#define RATE 48000 +#define FS 960 /* 20 мс */ +#define FILTER 14400 /* хвост 300 мс */ +#define DELAY_FRAMES 2 +#define ECHO_GAIN 0.5 +#define N_FRAMES 160 +#define ADAPT_FRAMES 100 /* фаза адаптации: только far-end */ +#define DT_START (ADAPT_FRAMES) +#define DT_FRAMES 20 /* double-talk: речь + эхо */ +#define MEAS_START (DT_START + DT_FRAMES) + +static int g_failures = 0; + +#define CHECK(cond, ...) do { \ + if (!(cond)) { \ + printf(" FAIL: " __VA_ARGS__); printf("\n"); \ + g_failures++; \ + } \ +} while (0) + +/* Детерминированный «шум» far-end (LCG). */ +static uint32_t g_rng = 0x12345678; +static int16_t aec_noise(void) { + g_rng = g_rng * 1664525u + 1013904223u; + return (int16_t)(((g_rng >> 16) & 0x3fff) - 0x1fff); +} + +static int test_lifecycle_and_null(void) { + speex_aec_t* a; + int16_t pcm[FS], out[FS]; + + printf("[speex_aec] lifecycle + NULL guards\n"); + memset(pcm, 0, sizeof(pcm)); + + a = speex_aec_create(RATE, FS, FILTER, DELAY_FRAMES); + CHECK(a != NULL, "create failed"); + if (!a) return 1; + + CHECK(speex_aec_create(0, FS, FILTER, DELAY_FRAMES) == NULL, "create(rate=0) should fail"); + CHECK(speex_aec_create(RATE, 0, FILTER, DELAY_FRAMES) == NULL, "create(frame=0) should fail"); + CHECK(speex_aec_create(RATE, FS, FS - 1, DELAY_FRAMES) == NULL, "create(filter= DT_START && k < MEAS_START); + for (i = 0; i < FS; i++) { + int echo_idx = k - DELAY_FRAMES; + int16_t echo = echo_idx >= 0 ? (int16_t)(ECHO_GAIN * render_hist[echo_idx * FS + i]) : 0; + cap[i] = double_talk ? (int16_t)(speech[i] + echo) : echo; + } + + n = speex_aec_process_capture(a, cap, FS, out); + CHECK(n == FS, "process_capture returned %d (expect %d)", n, FS); + speex_aec_feed_playback(a, render_hist + k * FS, FS); + + if (double_talk) { + for (i = 0; i < FS; i++) { + dt_speech_pow += (double)speech[i] * speech[i]; + dt_out_pow += (double)out[i] * out[i]; + dt_cross += (double)speech[i] * out[i]; + } + dt_measured += FS; + } else if (k >= MEAS_START) { + for (i = 0; i < FS; i++) { + int echo_idx = k - DELAY_FRAMES; + int16_t echo = echo_idx >= 0 ? (int16_t)(ECHO_GAIN * render_hist[echo_idx * FS + i]) : 0; + echo_pow += (double)echo * echo; + resid_pow += (double)out[i] * out[i]; + } + measured += FS; + } + } + + { + double echo_rms = sqrt(echo_pow / measured); + double resid_rms = sqrt(resid_pow / measured); + double db = echo_rms > 0 ? 20.0 * log10(echo_rms / (resid_rms > 1.0 ? resid_rms : 1.0)) : 0.0; + printf(" echo-only suppression: echo_rms=%.1f resid_rms=%.1f => %.1f dB\n", echo_rms, resid_rms, db); + CHECK(echo_rms > 100.0, "echo too weak to measure (echo_rms=%.1f)", echo_rms); + CHECK(resid_pow < 0.05 * echo_pow, "echo not suppressed: resid=%.0f echo=%.0f", resid_pow, echo_pow); + } + { + double corr = sqrt(dt_speech_pow * dt_out_pow); + double sim = corr > 0.0 ? dt_cross / corr : 0.0; + printf(" double-talk speech preservation: correlation=%.3f\n", sim); + CHECK(sim > 0.7, "near-end speech distorted: correlation=%.3f", sim); + } + + speex_aec_destroy(a); + u_free(render_hist); + return 0; +} + +int main(void) { + debug_config_init(); + debug_set_level(DEBUG_LEVEL_INFO); + debug_set_category_level(DEBUG_CATEGORY_AEC, DEBUG_LEVEL_INFO); + + printf("SpeexDSP AEC test\n"); + + test_lifecycle_and_null(); + test_delay_line(); + test_echo_suppression(); + + if (g_failures == 0) { + printf("TEST PASSED\n"); + return 0; + } + printf("TEST FAILED (%d failures)\n", g_failures); + return 1; +} diff --git a/tools/chatgui-android/app/src/main/cpp/CMakeLists.txt b/tools/chatgui-android/app/src/main/cpp/CMakeLists.txt index e881df27..afbd6aee 100644 --- a/tools/chatgui-android/app/src/main/cpp/CMakeLists.txt +++ b/tools/chatgui-android/app/src/main/cpp/CMakeLists.txt @@ -28,6 +28,12 @@ list(APPEND UTUN_INCLUDE_DIRS "${LIB_DIR}/libopus" "${LIB_DIR}/libopus/include" # Opus requires these to be defined (or HAVE_CONFIG_H + config.h with VAR_ARRAYS) list(APPEND UTUN_DEFINES OPUS_BUILD VAR_ARRAYS USE_ALLOCA) +# SpeexDSP (акустическое эхоподавление AEC) — обёртка speex_aec.c уже в lib/*.c GLOB. +file(GLOB _SPEEXDSP_SRC "${LIB_DIR}/speexdsp/*.c") +list(APPEND UTUN_SOURCES ${_SPEEXDSP_SRC}) +list(APPEND UTUN_INCLUDE_DIRS "${LIB_DIR}/speexdsp" "${LIB_DIR}/speexdsp/include") +list(APPEND UTUN_DEFINES FLOATING_POINT USE_KISS_FFT "EXPORT=") + # Голосовой стек звонка (SoundTouch + voice_jitter + call_audio + call_tones), # C++ внутри, C-интерфейс наружу (call_audio.h). set(CALL_DIR ${SRC_DIR}/call) diff --git a/tools/chatgui-android/app/src/main/java/com/utun/chat/data/CallAudioEngine.kt b/tools/chatgui-android/app/src/main/java/com/utun/chat/data/CallAudioEngine.kt index 372d4c06..95d599ed 100644 --- a/tools/chatgui-android/app/src/main/java/com/utun/chat/data/CallAudioEngine.kt +++ b/tools/chatgui-android/app/src/main/java/com/utun/chat/data/CallAudioEngine.kt @@ -6,8 +6,11 @@ import android.media.AudioDeviceInfo import android.media.AudioFormat import android.media.AudioManager import android.media.AudioRecord +import android.media.AudioTimestamp import android.media.AudioTrack import android.media.MediaRecorder +import android.os.Handler +import android.os.Looper import com.utun.chat.ChatApplication /** @@ -27,6 +30,11 @@ class CallAudioEngine { const val SAMPLE_RATE = 48000 const val FRAME_MS = 20 const val FRAME_SAMPLES = SAMPLE_RATE * FRAME_MS / 1000 // 960 + + /** Задержка рендер→захват AEC по умолчанию (кадров), если замер недоступен. */ + const val AEC_DEFAULT_DELAY_FRAMES = 4 + /** Акустический ход эха ~3мс (сэмплы @48кГц), добавляется к замеру. */ + const val AEC_ACOUSTIC_SAMPLES = 150L } private var audioRecord: AudioRecord? = null @@ -44,6 +52,12 @@ class CallAudioEngine { @Volatile private var muted = false + /* Для замера задержки AEC (getTimestamp) — счётчики кадров play/capture. */ + @Volatile private var playFramesWritten = 0L + @Volatile private var capFramesRead = 0L + private val mainHandler = Handler(Looper.getMainLooper()) + private val aecDelayRunnable = Runnable { measureAecDelay() } + fun isActive(): Boolean = active /** Mute: пир слышит тишину (отправляем нулевые кадры вместо захваченного PCM). */ @@ -80,6 +94,12 @@ class CallAudioEngine { router.start() + /* AEC: включён только на громкой связи (спикерфон); на гарнитуре/разговорном + * динамике эхо незначимо. Задержка рендер→захват замеряется чуть позже. */ + playFramesWritten = 0L + capFramesRead = 0L + syncAecForRoute(router.route) + // ── AudioRecord (захват) ── val recMin = AudioRecord.getMinBufferSize( SAMPLE_RATE, AudioFormat.CHANNEL_IN_MONO, AudioFormat.ENCODING_PCM_16BIT) @@ -134,6 +154,9 @@ class CallAudioEngine { priority = Thread.MAX_PRIORITY; start() } + /* Через ~300мс буферы устройства заполнены — замеряем реальную задержку AEC. */ + mainHandler.postDelayed(aecDelayRunnable, 300) + LogManager.addLog("INFO", "CallAudio", "started id=$id recBuf=$recMin trackBuf=$trackBuf") return true } @@ -141,6 +164,7 @@ class CallAudioEngine { fun stop() { if (!active) return active = false + mainHandler.removeCallbacks(aecDelayRunnable) captureThread?.join(1000); captureThread = null playThread?.join(1000); playThread = null @@ -172,6 +196,7 @@ class CallAudioEngine { read += n } val out = if (muted) silence else buf + capFramesRead += FRAME_SAMPLES if (!NativeLib.callAudioFeed(callId, out)) { LogManager.addLog("WARN", "CallAudio", "callAudioFeed returned false") } @@ -182,21 +207,64 @@ class CallAudioEngine { private fun playLoop() { val buf = ShortArray(FRAME_SAMPLES) val silence = ShortArray(FRAME_SAMPLES) + val frameNs = FRAME_MS * 1_000_000L + var nextNs = System.nanoTime() while (active) { - val t0 = System.nanoTime() val track = audioTrack ?: return val n = NativeLib.callAudioPull(callId, buf) if (n > 0) track.write(buf, 0, n) if (n < FRAME_SAMPLES) track.write(silence, 0, FRAME_SAMPLES - n) - val elapsedMs = (System.nanoTime() - t0) / 1_000_000 - val sleepMs = FRAME_MS - elapsedMs - if (sleepMs > 0) { - try { Thread.sleep(sleepMs) } catch (_: InterruptedException) { return } + playFramesWritten += FRAME_SAMPLES + + /* абсолютный deadline: не даём Thread.sleep накопить систематический дрейф + * (важно для стабильного выравнивания рендер↔захват в AEC). */ + nextNs += frameNs + val waitNs = nextNs - System.nanoTime() + if (waitNs > 0) { + try { Thread.sleep(waitNs / 1_000_000L, (waitNs % 1_000_000L).toInt()) } + catch (_: InterruptedException) { return } + } else { + nextNs = System.nanoTime() /* отстали — сбрасываем, не копим долг */ } } LogManager.addLog("DEBUG", "CallAudio", "play loop done") } + /** AEC включается только на громкой связи (спикерфон). */ + fun syncAecForRoute(route: AudioRoute) { + val on = route == AudioRoute.SPEAKER + LogManager.addLog("INFO", "CallAudio", "AEC ${if (on) "on" else "off"} (route=$route)") + NativeLib.callAudioSetAec(on) + } + + /** Замер реальной задержки рендер→захват и применение к AEC (один раз после старта). */ + private fun measureAecDelay() { + if (!active) return + val track = audioTrack ?: return + val rec = audioRecord ?: return + try { + val tTs = AudioTimestamp() + val rTs = AudioTimestamp() + val trackOk = track.getTimestamp(tTs) + val recOk = rec.getTimestamp(rTs, AudioTimestamp.TIMEBASE_MONOTONIC) == AudioRecord.SUCCESS + if (!trackOk || !recOk) { + LogManager.addLog("WARN", "CallAudio", "getTimestamp unavailable; AEC delay = $AEC_DEFAULT_DELAY_FRAMES frames") + NativeLib.callAudioSetAecDelay(AEC_DEFAULT_DELAY_FRAMES) + return + } + val trackLatency = (playFramesWritten - tTs.framePosition).coerceAtLeast(0L) + val recLatency = (rTs.framePosition - capFramesRead).coerceAtLeast(0L) + val total = trackLatency + recLatency + AEC_ACOUSTIC_SAMPLES + val delayFrames = ((total + FRAME_SAMPLES - 1) / FRAME_SAMPLES).toInt().coerceIn(1, 20) + LogManager.addLog("INFO", "CallAudio", + "AEC delay measured: trackLat=$trackLatency recLat=$recLatency total=$total -> $delayFrames frames") + NativeLib.callAudioSetAecDelay(delayFrames) + } catch (e: Exception) { + LogManager.addLog("WARN", "CallAudio", "AEC delay measure failed: ${e.message}") + NativeLib.callAudioSetAecDelay(AEC_DEFAULT_DELAY_FRAMES) + } + } + private fun restoreAudioMode() { try { val am = ChatApplication.instance.getSystemService(Context.AUDIO_SERVICE) as AudioManager diff --git a/tools/chatgui-android/app/src/main/java/com/utun/chat/data/NativeLib.kt b/tools/chatgui-android/app/src/main/java/com/utun/chat/data/NativeLib.kt index 78ef3fae..5407756b 100644 --- a/tools/chatgui-android/app/src/main/java/com/utun/chat/data/NativeLib.kt +++ b/tools/chatgui-android/app/src/main/java/com/utun/chat/data/NativeLib.kt @@ -156,6 +156,10 @@ object NativeLib { fun callAudioPull(callId: Long, out: ShortArray): Int = nativeCallAudioPull(callId, out) fun callAudioStop() { nativeCallAudioStop() } + /* AEC (эхоподавление): включается по спикерфону; delay — измеренная задержка в кадрах. */ + fun callAudioSetAec(enabled: Boolean) { nativeCallAudioSetAec(enabled) } + fun callAudioSetAecDelay(frames: Int) { nativeCallAudioSetAecDelay(frames) } + /* ── Рация (PTT walkie-talkie) ── */ fun radioSetActive(groupId: Long, on: Boolean) { nativeRadioSetActive(groupId, on) } fun radioSubscribers(groupId: Long): Int = nativeRadioSubscribers(groupId) @@ -263,6 +267,8 @@ object NativeLib { private external fun nativeCallAudioFeed(callId: Long, samples: ShortArray): Boolean private external fun nativeCallAudioPull(callId: Long, out: ShortArray): Int private external fun nativeCallAudioStop() + private external fun nativeCallAudioSetAec(enabled: Boolean) + private external fun nativeCallAudioSetAecDelay(frames: Int) /* ── Рация JNI ── */ private external fun nativeRadioSetActive(groupId: Long, on: Boolean) diff --git a/tools/chatgui-android/app/src/main/java/com/utun/chat/viewmodel/ChatViewModel.kt b/tools/chatgui-android/app/src/main/java/com/utun/chat/viewmodel/ChatViewModel.kt index 614a4b4b..3899b4b2 100644 --- a/tools/chatgui-android/app/src/main/java/com/utun/chat/viewmodel/ChatViewModel.kt +++ b/tools/chatgui-android/app/src/main/java/com/utun/chat/viewmodel/ChatViewModel.kt @@ -611,6 +611,7 @@ class ChatViewModel : ViewModel() { callAudio.router.onChanged = { r, headset -> _audioRoute.value = r _headsetAvailable.value = headset + callAudio.syncAecForRoute(r) } _audioRoute.value = callAudio.router.route _headsetAvailable.value = callAudio.router.headsetAvailable diff --git a/tools/chatgui-android/jni_bridge/android_jni_bridge.c b/tools/chatgui-android/jni_bridge/android_jni_bridge.c index e6303dca..cfb63024 100644 --- a/tools/chatgui-android/jni_bridge/android_jni_bridge.c +++ b/tools/chatgui-android/jni_bridge/android_jni_bridge.c @@ -1491,6 +1491,14 @@ int utun_bridge_call_audio_pull(uint64_t call_id, int16_t* out, int max_samples) return call_audio_pull_pcm(call_id, out, max_samples); } +void utun_bridge_call_audio_set_aec(int enabled) { + call_audio_set_aec_enabled(enabled); +} + +void utun_bridge_call_audio_set_aec_delay(int frames) { + call_audio_set_aec_delay_frames(frames); +} + /* ────────────────────────────────────────────────────────────────── * Рация (PTT walkie-talkie): подписка + аудио-движок * ────────────────────────────────────────────────────────────────── */ @@ -2404,6 +2412,18 @@ JNIEXPORT jint JNICALL Java_com_utun_chat_data_NativeLib_nativeCallAudioPull( return (jint)n; } +JNIEXPORT void JNICALL Java_com_utun_chat_data_NativeLib_nativeCallAudioSetAec( + JNIEnv* env, jobject thiz, jboolean enabled) { + (void)env; (void)thiz; + utun_bridge_call_audio_set_aec(enabled ? 1 : 0); +} + +JNIEXPORT void JNICALL Java_com_utun_chat_data_NativeLib_nativeCallAudioSetAecDelay( + JNIEnv* env, jobject thiz, jint frames) { + (void)env; (void)thiz; + utun_bridge_call_audio_set_aec_delay((int)frames); +} + /* ── Рация (PTT walkie-talkie) ── */ JNIEXPORT void JNICALL Java_com_utun_chat_data_NativeLib_nativeRadioSetActive( diff --git a/tools/chatgui/libutun/CMakeLists.txt b/tools/chatgui/libutun/CMakeLists.txt index 431f236b..345081a1 100644 --- a/tools/chatgui/libutun/CMakeLists.txt +++ b/tools/chatgui/libutun/CMakeLists.txt @@ -13,7 +13,13 @@ set(UASYNC_LMDB_SOURCES "${LIB_DIR}/liblmdb/mdb.c" "${LIB_DIR}/liblmdb/midl.c" ) -list(APPEND UASYNC_SOURCES ${UASYNC_LMDB_SOURCES}) +set(UASYNC_SPEEXDSP_SOURCES + "${LIB_DIR}/speexdsp/mdf.c" + "${LIB_DIR}/speexdsp/fftwrap.c" + "${LIB_DIR}/speexdsp/kiss_fft.c" + "${LIB_DIR}/speexdsp/kiss_fftr.c" +) +list(APPEND UASYNC_SOURCES ${UASYNC_LMDB_SOURCES} ${UASYNC_SPEEXDSP_SOURCES}) add_library(uasync STATIC ${UASYNC_SOURCES}) @@ -22,6 +28,8 @@ target_include_directories(uasync PUBLIC ${SRC_DIR} ${LIB_DIR}/liblmdb ${LIB_DIR}/libopus/include + ${LIB_DIR}/speexdsp + ${LIB_DIR}/speexdsp/include ) file(GLOB OPUS_SOURCES CONFIGURE_DEPENDS @@ -61,6 +69,9 @@ target_link_libraries(uasync PUBLIC opus_internal) target_compile_definitions(uasync PRIVATE _ISOC99_SOURCE DEBUG_OUTPUT_STDERR + FLOATING_POINT + USE_KISS_FFT + EXPORT= ) set_target_properties(uasync PROPERTIES diff --git a/tools/chatgui/src/audiodevicesettingspage.cpp b/tools/chatgui/src/audiodevicesettingspage.cpp index 72842562..a7223e1f 100644 --- a/tools/chatgui/src/audiodevicesettingspage.cpp +++ b/tools/chatgui/src/audiodevicesettingspage.cpp @@ -197,6 +197,7 @@ void AudioDeviceSettingsPage::loadFromDb() { m_savedCompressorEnabled = m_db->getUiStateInt("compressor_enabled", 0); m_savedCompressorMaxGainDb = m_db->getUiStateInt("compressor_max_gain_db", 25); m_savedCompressorRiseRate = m_db->getUiStateInt("compressor_rise_rate", 10); + m_savedAecEnabled = m_db->getUiStateInt("aec_enabled", 1); } /* Select saved indices */ @@ -225,6 +226,7 @@ void AudioDeviceSettingsPage::loadFromDb() { m_compressorEnabledCb->setChecked(m_savedCompressorEnabled != 0); m_compressorMaxGainSlider->setValue(m_savedCompressorMaxGainDb); m_compressorRiseRateSlider->setValue(m_savedCompressorRiseRate); + m_aecEnabledCb->setChecked(m_savedAecEnabled != 0); updateCompressorLabels(); } @@ -267,6 +269,9 @@ void AudioDeviceSettingsPage::applyAndSave() { saveKey("compressor_max_gain_db", compMaxGain); saveKey("compressor_rise_rate", compRiseRate); + int aecEnabled = m_aecEnabledCb->isChecked() ? 1 : 0; + saveKey("aec_enabled", aecEnabled); + /* also update [chat] config section + live apply for headless-compat settings */ if (!m_configPath.isEmpty()) { auto postChat = [](const QString& path, const char* name, int val) { @@ -288,6 +293,7 @@ void AudioDeviceSettingsPage::applyAndSave() { postChat(m_configPath, "compressor_enabled", compEnabled); postChat(m_configPath, "compressor_max_gain_db", compMaxGain); postChat(m_configPath, "compressor_rise_rate", compRiseRate); + postChat(m_configPath, "aec_enabled", aecEnabled); } m_savedOutputIdx = outIdx; @@ -299,9 +305,10 @@ void AudioDeviceSettingsPage::applyAndSave() { m_savedCompressorEnabled = compEnabled; m_savedCompressorMaxGainDb = compMaxGain; m_savedCompressorRiseRate = compRiseRate; - DEBUG_DEBUG(DEBUG_CATEGORY_DEBUG, "saved output=%d input=%d codec=%d radioOpus=%d/%dkbps stereo=%d compressor=%d gain=%ddB rise=%.1fx", + m_savedAecEnabled = aecEnabled; + DEBUG_DEBUG(DEBUG_CATEGORY_DEBUG, "saved output=%d input=%d codec=%d radioOpus=%d/%dkbps stereo=%d compressor=%d gain=%ddB rise=%.1fx aec=%d", outIdx, inIdx, codecPreset, radioOpusMode, radioOpusBitrate, radioCaptureStereo, - compEnabled, compMaxGain, (float)compRiseRate); + compEnabled, compMaxGain, (float)compRiseRate, aecEnabled); if (m_recorder) { m_recorder->setCompressorEnabled(compEnabled != 0); @@ -385,6 +392,10 @@ void AudioDeviceSettingsPage::buildCompressorUI(QVBoxLayout* layout) { m_compressorEnabledCb->setStyleSheet("font-size: 12px;"); layout->addWidget(m_compressorEnabledCb); + m_aecEnabledCb = new QCheckBox("Enable echo cancellation (AEC) in calls", this); + m_aecEnabledCb->setStyleSheet("font-size: 12px;"); + layout->addWidget(m_aecEnabledCb); + auto makeSliderRow = [&](const QString& name, QSlider*& slider, QLabel*& lbl, int min, int max, int step) { auto* row = new QHBoxLayout(); diff --git a/tools/chatgui/src/audiodevicesettingspage.h b/tools/chatgui/src/audiodevicesettingspage.h index ca749ccb..d7e4dc7f 100644 --- a/tools/chatgui/src/audiodevicesettingspage.h +++ b/tools/chatgui/src/audiodevicesettingspage.h @@ -54,6 +54,9 @@ private: int m_savedCompressorMaxGainDb = 25; int m_savedCompressorRiseRate = 10; + QCheckBox* m_aecEnabledCb; + int m_savedAecEnabled = 1; + QComboBox* m_radioOpusModeCombo; QComboBox* m_radioOpusBitrateCombo; QCheckBox* m_radioCaptureStereoCb;