diff options
| author | Benny Prijono <bennylp@teluu.com> | 2006-06-18 01:58:38 +0000 |
|---|---|---|
| committer | Benny Prijono <bennylp@teluu.com> | 2006-06-18 01:58:38 +0000 |
| commit | 44fae93c28e19e06d5384bc346630b8c6f6bf1a1 (patch) | |
| tree | cebf84e10d777ce533d6e4f122200fda7941d2e8 | |
| parent | 3034f9480369cb8083cd625b358354b4618cd985 (diff) | |
Added speex jitter buffer and AEC files to the library
git-svn-id: http://svn.pjsip.org/repos/pjproject/trunk@516 74dad513-b988-da41-8d7b-12977e46ad98
| -rw-r--r-- | pjmedia/src/pjmedia-codec/speex/AUTHORS | 18 | ||||
| -rw-r--r-- | pjmedia/src/pjmedia-codec/speex/COPYING | 33 | ||||
| -rw-r--r-- | pjmedia/src/pjmedia-codec/speex/ChangeLog | 21 | ||||
| -rw-r--r-- | pjmedia/src/pjmedia-codec/speex/_kiss_fft_guts.h | 147 | ||||
| -rw-r--r-- | pjmedia/src/pjmedia-codec/speex/fftwrap.c | 301 | ||||
| -rw-r--r-- | pjmedia/src/pjmedia-codec/speex/fftwrap.h | 58 | ||||
| -rw-r--r-- | pjmedia/src/pjmedia-codec/speex/jitter.c | 316 | ||||
| -rw-r--r-- | pjmedia/src/pjmedia-codec/speex/kiss_fft.c | 430 | ||||
| -rw-r--r-- | pjmedia/src/pjmedia-codec/speex/kiss_fft.h | 110 | ||||
| -rw-r--r-- | pjmedia/src/pjmedia-codec/speex/kiss_fftr.c | 163 | ||||
| -rw-r--r-- | pjmedia/src/pjmedia-codec/speex/kiss_fftr.h | 46 | ||||
| -rw-r--r-- | pjmedia/src/pjmedia-codec/speex/lpc.c | 201 | ||||
| -rw-r--r-- | pjmedia/src/pjmedia-codec/speex/ltp.c | 2 | ||||
| -rw-r--r-- | pjmedia/src/pjmedia-codec/speex/mdf.c | 741 |
14 files changed, 2586 insertions, 1 deletions
diff --git a/pjmedia/src/pjmedia-codec/speex/AUTHORS b/pjmedia/src/pjmedia-codec/speex/AUTHORS new file mode 100644 index 00000000..9bd47bc4 --- /dev/null +++ b/pjmedia/src/pjmedia-codec/speex/AUTHORS @@ -0,0 +1,18 @@ +Jean-Marc Valin <jean-marc.valin@usherbrooke.ca> + All the code except the following + +David Rowe <david@voicetronix.com.au> via VoiceTronix + lsp.c lsp.h + Also ideas and feedback + +John Francis Edwards: + wave_out.[ch], some #ifdefs for windows port and MSVC project files + +Segher Boessenkool + Misc. optimizations (for QMF in particular) + +Atsuhiko Yamanaka <ymnk@jcraft.com>: + Patch to speexenc.c to add Vorbis comment format + +Radim Kolar <hsn@cybermail.net>: + Patch to speexenc.c for supporting more input formats diff --git a/pjmedia/src/pjmedia-codec/speex/COPYING b/pjmedia/src/pjmedia-codec/speex/COPYING new file mode 100644 index 00000000..5c710ac9 --- /dev/null +++ b/pjmedia/src/pjmedia-codec/speex/COPYING @@ -0,0 +1,33 @@ +Copyright 2002-2005 + Xiph.org Foundation + Jean-Marc Valin + David Rowe + EpicGames + Analog Devices + +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/pjmedia/src/pjmedia-codec/speex/ChangeLog b/pjmedia/src/pjmedia-codec/speex/ChangeLog new file mode 100644 index 00000000..9a6cd8af --- /dev/null +++ b/pjmedia/src/pjmedia-codec/speex/ChangeLog @@ -0,0 +1,21 @@ +2005-09-07 Thomas Vander Stichele <thomas at apestaart dot org> + + * libspeex/cb_search.c: (split_cb_search_shape_sign_N1): + add declaration for n, seems like an obvious build fix, slap + me down if it's not + +2004-02-18 Jean-Marc Valin <jean-marc.valin@usherbrooke.ca> + Patch for compiling with mingw32 sent by j@thing.net + +2004-02-18 Jean-Marc Valin <jean-marc.valin@usherbrooke.ca> + Integrated IRIX patch (getopt stuff) from Michael Pruett <michael@68k.org> + +2004-02-18 Jean-Marc Valin <jean-marc.valin@usherbrooke.ca> + Changed the Makefile.am so that KDevelop can parse SUBDIRS correctly + +2002/03/27 Jean-Marc Valin: +Working encoder and decoder for both narrowband and wideband. + +2002/02/27 Jean-Marc Valin: +Got the basic encoder working as a demo with quantization only on some +parameters. diff --git a/pjmedia/src/pjmedia-codec/speex/_kiss_fft_guts.h b/pjmedia/src/pjmedia-codec/speex/_kiss_fft_guts.h new file mode 100644 index 00000000..abbd8b1f --- /dev/null +++ b/pjmedia/src/pjmedia-codec/speex/_kiss_fft_guts.h @@ -0,0 +1,147 @@ +/* +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 <limits.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 +# define FRACBITS 15 +# define SAMPPROD 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 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_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) + + +/* a debugging function */ +#define pcpx(c)\ + fprintf(stderr,"%g + %gi\n",(double)((c)->r),(double)((c)->i) ) diff --git a/pjmedia/src/pjmedia-codec/speex/fftwrap.c b/pjmedia/src/pjmedia-codec/speex/fftwrap.c new file mode 100644 index 00000000..de093569 --- /dev/null +++ b/pjmedia/src/pjmedia-codec/speex/fftwrap.c @@ -0,0 +1,301 @@ +/* Copyright (C) 2005 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 + +/*#define USE_SMALLFT*/ +#define USE_KISS_FFT + + +#include "misc.h" + + +#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;i<len;i++) + { + if (in[i]>max_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<len;i++) + { + out[i] = in[i] << shift; + } + return shift; +} + +static void renorm_range(spx_word16_t *in, spx_word16_t *out, int shift, int len) +{ + int i; + for (i=0;i<len;i++) + { + out[i] = (in[i] + (1<<(shift-1))) >> shift; + } +} +#endif + +#ifdef USE_SMALLFT + +#include "smallft.h" +#include <math.h> + +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; + speex_warning("FFT should not be done in-place"); + float scale = 1./((struct drft_lookup *)table)->n; + 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) + { + int i; + 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_KISS_FFT) + +#include "kiss_fftr.h" +#include "kiss_fft.h" + +struct kiss_config { + kiss_fftr_cfg forward; + kiss_fftr_cfg backward; + kiss_fft_cpx *freq_data; + int N; +}; + +void *spx_fft_init(int size) +{ + struct kiss_config *table; + table = speex_alloc(sizeof(struct kiss_config)); + table->freq_data = speex_alloc(sizeof(kiss_fft_cpx)*((size>>1)+1)); + table->forward = 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(t->freq_data); + speex_free(table); +} + +#ifdef FIXED_POINT + +void spx_fft(void *table, spx_word16_t *in, spx_word16_t *out) +{ + int i; + int shift; + struct kiss_config *t = (struct kiss_config *)table; + shift = maximize_range(in, in, 32000, t->N); + kiss_fftr(t->forward, in, t->freq_data); + out[0] = t->freq_data[0].r; + for (i=1;i<t->N>>1;i++) + { + out[(i<<1)-1] = t->freq_data[i].r; + out[(i<<1)] = t->freq_data[i].i; + } + out[(i<<1)-1] = t->freq_data[i].r; + 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_fftr(t->forward, in, t->freq_data); + out[0] = scale*t->freq_data[0].r; + for (i=1;i<t->N>>1;i++) + { + out[(i<<1)-1] = scale*t->freq_data[i].r; + out[(i<<1)] = scale*t->freq_data[i].i; + } + out[(i<<1)-1] = scale*t->freq_data[i].r; +} +#endif + +void spx_ifft(void *table, spx_word16_t *in, spx_word16_t *out) +{ + int i; + struct kiss_config *t = (struct kiss_config *)table; + t->freq_data[0].r = in[0]; + t->freq_data[0].i = 0; + for (i=1;i<t->N>>1;i++) + { + t->freq_data[i].r = in[(i<<1)-1]; + t->freq_data[i].i = in[(i<<1)]; + } + t->freq_data[i].r = in[(i<<1)-1]; + t->freq_data[i].i = 0; + + kiss_fftri(t->backward, t->freq_data, out); +} + + +#else + +#error No other FFT implemented + +#endif + + +int fixed_point = 1; +#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 + spx_word16_t _in[N]; + spx_word16_t _out[N]; + for (i=0;i<N;i++) + _in[i] = (int)floor(.5+in[i]); + spx_fft(table, _in, _out); + for (i=0;i<N;i++) + out[i] = _out[i]; + if (!fixed_point) + { + float scale; + struct drft_lookup t; + spx_drft_init(&t, ((struct kiss_config *)table)->N); + 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); + } +} + +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 + spx_word16_t _in[N]; + spx_word16_t _out[N]; + for (i=0;i<N;i++) + _in[i] = (int)floor(.5+in[i]); + spx_ifft(table, _in, _out); + for (i=0;i<N;i++) + out[i] = _out[i]; + if (!fixed_point) + { + int i; + struct drft_lookup t; + spx_drft_init(&t, ((struct kiss_config *)table)->N); + for (i=0;i<((struct kiss_config *)table)->N;i++) + out[i] = in[i]; + spx_drft_backward(&t, out); + spx_drft_clear(&t); + } +} + +#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/pjmedia/src/pjmedia-codec/speex/fftwrap.h b/pjmedia/src/pjmedia-codec/speex/fftwrap.h new file mode 100644 index 00000000..826b38e9 --- /dev/null +++ b/pjmedia/src/pjmedia-codec/speex/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 "misc.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/pjmedia/src/pjmedia-codec/speex/jitter.c b/pjmedia/src/pjmedia-codec/speex/jitter.c new file mode 100644 index 00000000..55a0833f --- /dev/null +++ b/pjmedia/src/pjmedia-codec/speex/jitter.c @@ -0,0 +1,316 @@ +/* Copyright (C) 2002 Jean-Marc Valin + File: speex_jitter.h + + Adaptive jitter buffer 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. + +*/ + +#ifdef HAVE_CONFIG_H +#include "config.h" +#endif + +#ifndef NULL +#define NULL 0 +#endif + +#include "misc.h" +#include <speex/speex.h> +#include <speex/speex_bits.h> +#include <speex/speex_jitter.h> +#include <stdio.h> + +#define LATE_BINS 4 + +void speex_jitter_init(SpeexJitter *jitter, void *decoder, int sampling_rate) +{ + int i; + for (i=0;i<SPEEX_JITTER_MAX_BUFFER_SIZE;i++) + { + jitter->len[i]=-1; + jitter->timestamp[i]=-1; + } + + jitter->dec = decoder; + speex_decoder_ctl(decoder, SPEEX_GET_FRAME_SIZE, &jitter->frame_size); + jitter->frame_time = jitter->frame_size; + + speex_bits_init(&jitter->current_packet); + jitter->valid_bits = 0; + + jitter->buffer_size = 4; + + jitter->pointer_timestamp = -jitter->frame_time * jitter->buffer_size; + jitter->reset_state = 1; + jitter->lost_count = 0; + jitter->loss_rate = 0; +} + +void speex_jitter_destroy(SpeexJitter *jitter) +{ + speex_bits_destroy(&jitter->current_packet); +} + + +void speex_jitter_put(SpeexJitter *jitter, char *packet, int len, int timestamp) +{ + int i,j; + int arrival_margin; + + if (jitter->reset_state) + { + jitter->reset_state=0; + jitter->pointer_timestamp = timestamp-jitter->frame_time * jitter->buffer_size; + for (i=0;i<MAX_MARGIN;i++) + { + jitter->shortterm_margin[i] = 0; + jitter->longterm_margin[i] = 0; + } + for (i=0;i<SPEEX_JITTER_MAX_BUFFER_SIZE;i++) + { + jitter->len[i]=-1; + jitter->timestamp[i]=-1; + } + fprintf(stderr, "reset to %d\n", timestamp); + } + + /* Cleanup buffer (remove old packets that weren't played) */ + for (i=0;i<SPEEX_JITTER_MAX_BUFFER_SIZE;i++) + { + if (jitter->timestamp[i]<jitter->pointer_timestamp) + { + jitter->len[i]=-1; + /*if (jitter->timestamp[i] != -1) + fprintf (stderr, "discarding %d %d\n", jitter->timestamp[i], jitter->pointer_timestamp);*/ + } + } + + /*Find an empty slot in the buffer*/ + for (i=0;i<SPEEX_JITTER_MAX_BUFFER_SIZE;i++) + { + if (jitter->len[i]==-1) + break; + } + + /*fprintf(stderr, "%d %d %f\n", timestamp, jitter->pointer_timestamp, jitter->drift_average);*/ + if (i==SPEEX_JITTER_MAX_BUFFER_SIZE) + { + int earliest=jitter->timestamp[0]; + i=0; + for (j=1;j<SPEEX_JITTER_MAX_BUFFER_SIZE;j++) + { + if (jitter->timestamp[j]<earliest) + { + earliest = jitter->timestamp[j]; + i=j; + } + } + /*fprintf (stderr, "Buffer is full, discarding earliest frame %d (currently at %d)\n", timestamp, jitter->pointer_timestamp);*/ + /*No place left in the buffer*/ + + /*skip some frame(s) */ + /*return;*/ + } + + /* Copy packet in buffer */ + if (len>SPEEX_JITTER_MAX_PACKET_SIZE) + len=SPEEX_JITTER_MAX_PACKET_SIZE; + for (j=0;j<len/BYTES_PER_CHAR;j++) + jitter->buf[i][j]=packet[j]; + jitter->timestamp[i]=timestamp; + jitter->len[i]=len; + + /* Don't count late packets when adjusting the synchro (we're taking care of them elsewhere) */ + /*if (timestamp <= jitter->pointer_timestamp) + { + fprintf (stderr, "frame for timestamp %d arrived too late (at time %d)\n", timestamp, jitter->pointer_timestamp); + }*/ + + /* Adjust the buffer size depending on network conditions */ + arrival_margin = (timestamp - jitter->pointer_timestamp - jitter->frame_time); + + if (arrival_margin >= -LATE_BINS*jitter->frame_time) + { + int int_margin; + for (i=0;i<MAX_MARGIN;i++) + { + jitter->shortterm_margin[i] *= .98; + jitter->longterm_margin[i] *= .995; + } + int_margin = (arrival_margin + LATE_BINS*jitter->frame_time)/jitter->frame_time; + if (int_margin>MAX_MARGIN-1) + int_margin = MAX_MARGIN-1; + if (int_margin>=0) + { + jitter->shortterm_margin[int_margin] += .02; + jitter->longterm_margin[int_margin] += .005; + } + } + + /*fprintf (stderr, "margin : %d %d %f %f %f %f\n", arrival_margin, jitter->buffer_size, 100*jitter->loss_rate, 100*jitter->late_ratio, 100*jitter->ontime_ratio, 100*jitter->early_ratio);*/ +} + +void speex_jitter_get(SpeexJitter *jitter, short *out, int *current_timestamp) +{ + int i; + int ret; + float late_ratio_short; + float late_ratio_long; + float ontime_ratio_short; + float ontime_ratio_long; + float early_ratio_short; + float early_ratio_long; + + late_ratio_short = 0; + late_ratio_long = 0; + for (i=0;i<LATE_BINS;i++) + { + late_ratio_short += jitter->shortterm_margin[i]; + late_ratio_long += jitter->longterm_margin[i]; + } + ontime_ratio_short = jitter->shortterm_margin[LATE_BINS]; + ontime_ratio_long = jitter->longterm_margin[LATE_BINS]; + early_ratio_short = early_ratio_long = 0; + for (i=LATE_BINS+1;i<MAX_MARGIN;i++) + { + early_ratio_short += jitter->shortterm_margin[i]; + early_ratio_long += jitter->longterm_margin[i]; + } + if (0&&jitter->pointer_timestamp%1000==0) + { + fprintf (stderr, "%f %f %f %f %f %f\n", early_ratio_short, early_ratio_long, ontime_ratio_short, ontime_ratio_long, late_ratio_short, late_ratio_long); + /*fprintf (stderr, "%f %f\n", early_ratio_short + ontime_ratio_short + late_ratio_short, early_ratio_long + ontime_ratio_long + late_ratio_long);*/ + } + + if (late_ratio_short > .1 || late_ratio_long > .03) + { + jitter->shortterm_margin[MAX_MARGIN-1] += jitter->shortterm_margin[MAX_MARGIN-2]; + jitter->longterm_margin[MAX_MARGIN-1] += jitter->longterm_margin[MAX_MARGIN-2]; + for (i=MAX_MARGIN-3;i>=0;i--) + { + jitter->shortterm_margin[i+1] = jitter->shortterm_margin[i]; + jitter->longterm_margin[i+1] = jitter->longterm_margin[i]; + } + jitter->shortterm_margin[0] = 0; + jitter->longterm_margin[0] = 0; + /*fprintf (stderr, "interpolate frame\n");*/ + speex_decode_int(jitter->dec, NULL, out); + if (current_timestamp) + *current_timestamp = jitter->pointer_timestamp; + return; + } + + /* Increment timestamp */ + jitter->pointer_timestamp += jitter->frame_time; + + if (late_ratio_short + ontime_ratio_short < .005 && late_ratio_long + ontime_ratio_long < .01 && early_ratio_short > .8) + { + jitter->shortterm_margin[0] += jitter->shortterm_margin[1]; + jitter->longterm_margin[0] += jitter->longterm_margin[1]; + for (i=1;i<MAX_MARGIN-1;i++) + { + jitter->shortterm_margin[i] = jitter->shortterm_margin[i+1]; + jitter->longterm_margin[i] = jitter->longterm_margin[i+1]; + } + jitter->shortterm_margin[MAX_MARGIN-1] = 0; + jitter->longterm_margin[MAX_MARGIN-1] = 0; + /*fprintf (stderr, "drop frame\n");*/ + jitter->pointer_timestamp += jitter->frame_time; + } + + if (current_timestamp) + *current_timestamp = jitter->pointer_timestamp; + + /* Send zeros while we fill in the buffer */ + if (jitter->pointer_timestamp<0) + { + for (i=0;i<jitter->frame_size;i++) + out[i]=0; + return; + } + + /* Search the buffer for a packet with the right timestamp */ + for (i=0;i<SPEEX_JITTER_MAX_BUFFER_SIZE;i++) + { + if (jitter->len[i]!=-1 && jitter->timestamp[i]==jitter->pointer_timestamp) + break; + } + + if (i==SPEEX_JITTER_MAX_BUFFER_SIZE) + { + /* No packet found */ + if (jitter->valid_bits) + { + /* Try decoding last received packet */ + ret = speex_decode_int(jitter->dec, &jitter->current_packet, out); + if (ret == 0) + { + jitter->lost_count = 0; + return; + } else { + jitter->valid_bits = 0; + } + } + + /*fprintf (stderr, "lost/late frame %d\n", jitter->pointer_timestamp);*/ + /*Packet is late or lost*/ + speex_decode_int(jitter->dec, NULL, out); + jitter->lost_count++; + if (jitter->lost_count>=25) + { + jitter->lost_count = 0; + jitter->reset_state = 1; + speex_decoder_ctl(jitter->dec, SPEEX_RESET_STATE, NULL); + } + jitter->loss_rate = .999*jitter->loss_rate + .001; + } else { + jitter->lost_count = 0; + /* Found the right packet */ + speex_bits_read_from(&jitter->current_packet, jitter->buf[i], jitter->len[i]); + jitter->len[i]=-1; + /* Decode packet */ + ret = speex_decode_int(jitter->dec, &jitter->current_packet, out); + if (ret == 0) + { + jitter->valid_bits = 1; + } else { + /* Error while decoding */ + for (i=0;i<jitter->frame_size;i++) + out[i]=0; + } + jitter->loss_rate = .999*jitter->loss_rate; + } + + +} + +int speex_jitter_get_pointer_timestamp(SpeexJitter *jitter) +{ + return jitter->pointer_timestamp; +} diff --git a/pjmedia/src/pjmedia-codec/speex/kiss_fft.c b/pjmedia/src/pjmedia-codec/speex/kiss_fft.c new file mode 100644 index 00000000..d6763dbc --- /dev/null +++ b/pjmedia/src/pjmedia-codec/speex/kiss_fft.c @@ -0,0 +1,430 @@ +/* +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 "_kiss_fft_guts.h" +#include "misc.h" + +/* The guts header contains all the multiplication and addition macros that are defined for + fixed or floating point complex numbers. It also delares the kf_ internal functions. + */ + +static kiss_fft_cpx *scratchbuf=NULL; +static size_t nscratchbuf=0; +static kiss_fft_cpx *tmpbuf=NULL; +static size_t ntmpbuf=0; + +#define CHECKBUF(buf,nbuf,n) \ + do { \ + if ( nbuf < (size_t)(n) ) {\ + free(buf); \ + buf = (kiss_fft_cpx*)KISS_FFT_MALLOC(sizeof(kiss_fft_cpx)*(n)); \ + nbuf = (size_t)(n); \ + } \ + }while(0) + +static void kf_bfly2( + kiss_fft_cpx * Fout, + const size_t fstride, + const kiss_fft_cfg st, + int m + ) +{ + kiss_fft_cpx * Fout2; + kiss_fft_cpx * tw1 = st->twiddles; + kiss_fft_cpx t; + Fout2 = Fout + m; + if (!st->inverse) { + int i; + kiss_fft_cpx *x=Fout; + for (i=0;i<2*m;i++) + { + x[i].r = SHR(x[i].r,1); + x[i].i = SHR(x[i].i,1); + } + } + + do{ + C_MUL (t, *Fout2 , *tw1); + tw1 += fstride; + C_SUB( *Fout2 , *Fout , t ); + C_ADDTO( *Fout , t ); + ++Fout2; + ++Fout; + }while (--m); +} + +static void kf_bfly4( + kiss_fft_cpx * Fout, + const size_t fstride, + const kiss_fft_cfg st, + const size_t m + ) +{ + kiss_fft_cpx *tw1,*tw2,*tw3; + kiss_fft_cpx scratch[6]; + size_t k=m; + const size_t m2=2*m; + const size_t m3=3*m; + + tw3 = tw2 = tw1 = st->twiddles; + + if (!st->inverse) { + int i; + kiss_fft_cpx *x=Fout; + for (i=0;i<4*m;i++) + { + x[i].r = PSHR16(x[i].r,2); + x[i].i = PSHR16(x[i].i,2); + } + } + if (st->inverse) + { + do { + C_MUL(scratch[0],Fout[m] , *tw1 ); + C_MUL(scratch[1],Fout[m2] , *tw2 ); + C_MUL(scratch[2],Fout[m3] , *tw3 ); + + 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] ); + 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; + } while(--k); + } else + { + do { + C_MUL(scratch[0],Fout[m] , *tw1 ); + C_MUL(scratch[1],Fout[m2] , *tw2 ); + C_MUL(scratch[2],Fout[m3] , *tw3 ); + + 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] ); + 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; + }while(--k); + } +} + +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; u<m; ++u ) { + if (!st->inverse) { + 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; + int Norig = st->nfft; + + CHECKBUF(scratchbuf,nscratchbuf,p); + + for ( u=0; u<m; ++u ) { + k=u; + for ( q1=0 ; q1<p ; ++q1 ) { + scratchbuf[q1] = Fout[ k ]; + if (!st->inverse) { + C_FIXDIV(scratchbuf[q1],p); + } + k += m; + } + + k=u; + for ( q1=0 ; q1<p ; ++q1 ) { + int twidx=0; + Fout[ k ] = scratchbuf[0]; + for (q=1;q<p;++q ) { + twidx += fstride * k; + if (twidx>=Norig) twidx-=Norig; + C_MUL(t,scratchbuf[q] , twiddles[twidx] ); + C_ADDTO( Fout[ k ] ,t); + } + k += m; + } + } +} + +static +void kf_work( + kiss_fft_cpx * Fout, + const kiss_fft_cpx * f, + const size_t fstride, + int in_stride, + int * factors, + const kiss_fft_cfg st + ) +{ + kiss_fft_cpx * Fout_beg=Fout; + const int p=*factors++; /* the radix */ + const int m=*factors++; /* stage's fft length/p */ + const kiss_fft_cpx * Fout_end = Fout + p*m; + + if (m==1) { + do{ + *Fout = *f; + f += fstride*in_stride; + }while(++Fout != Fout_end ); + }else{ + do{ + kf_work( Fout , f, fstride*p, in_stride, factors,st); + f += fstride*in_stride; + }while( (Fout += m) != Fout_end ); + } + + Fout=Fout_beg; + + switch (p) { + case 2: kf_bfly2(Fout,fstride,st,m); break; + case 3: kf_bfly3(Fout,fstride,st,m); break; + case 4: kf_bfly4(Fout,fstride,st,m); break; + case 5: kf_bfly5(Fout,fstride,st,m); break; + default: kf_bfly_generic(Fout,fstride,st,m,p); break; + } +} + +/* facbuf is populated by p1,m1,p2,m2, ... + where + p[i] * m[i] = m[i-1] + m0 = n */ +static +void kf_factor(int n,int * facbuf) +{ + int p=4; + double floor_sqrt; + floor_sqrt = floor( sqrt((double)n) ); + + /*factor out powers of 4, powers of 2, then any remaining primes */ + do { + while (n % p) { + switch (p) { + case 4: p = 2; break; + case 2: p = 3; break; + default: p += 2; break; + } + if (p > floor_sqrt) + 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; + + for (i=0;i<nfft;++i) { + const double pi=3.14159265358979323846264338327; + double phase = ( -2*pi /nfft ) * i; + if (st->inverse) + phase *= -1; + kf_cexp(st->twiddles+i, phase ); + } + + 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) { + CHECKBUF(tmpbuf,ntmpbuf,st->nfft); + kf_work(tmpbuf,fin,1,in_stride, st->factors,st); + memcpy(fout,tmpbuf,sizeof(kiss_fft_cpx)*st->nfft); + }else{ + kf_work( fout, fin, 1,in_stride, st->factors,st ); + } +} + +void kiss_fft(kiss_fft_cfg cfg,const kiss_fft_cpx *fin,kiss_fft_cpx *fout) +{ + kiss_fft_stride(cfg,fin,fout,1); +} + + +/* not really necessary to call, but if someone is doing in-place ffts, they may want to free the + buffers from CHECKBUF + */ +void kiss_fft_cleanup(void) +{ + free(scratchbuf); + scratchbuf = NULL; + nscratchbuf=0; + free(tmpbuf); + tmpbuf=NULL; + ntmpbuf=0; +} diff --git a/pjmedia/src/pjmedia-codec/speex/kiss_fft.h b/pjmedia/src/pjmedia-codec/speex/kiss_fft.h new file mode 100644 index 00000000..4d73f31a --- /dev/null +++ b/pjmedia/src/pjmedia-codec/speex/kiss_fft.h @@ -0,0 +1,110 @@ +#ifndef KISS_FFT_H +#define KISS_FFT_H + +#include <stdlib.h> +#include <stdio.h> +#include <math.h> +#include <memory.h> +#include <malloc.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 <xmmintrin.h> +# define kiss_fft_scalar __m128 +#define KISS_FFT_MALLOC(nbytes) memalign(16,nbytes) +#else +#define KISS_FFT_MALLOC malloc +#endif + + +#ifdef FIXED_POINT +#include <sys/types.h> +# define kiss_fft_scalar 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 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/pjmedia/src/pjmedia-codec/speex/kiss_fftr.c b/pjmedia/src/pjmedia-codec/speex/kiss_fftr.c new file mode 100644 index 00000000..3ac4db98 --- /dev/null +++ b/pjmedia/src/pjmedia-codec/speex/kiss_fftr.c @@ -0,0 +1,163 @@ +/* +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 "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) { + fprintf(stderr,"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); + + for (i = 0; i < nfft; ++i) { + double phase = + -3.14159265358979323846264338327 * ((double) i / nfft + .5); + if (inverse_fft) + phase *= -1; + kf_cexp (st->super_twiddles+i,phase); + } + 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) { + fprintf(stderr,"kiss fft usage error: improper alloc\n"); + exit(1); + } + + 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) { + fprintf (stderr, "kiss fft usage error: improper alloc\n"); + exit (1); + } + + 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); +} diff --git a/pjmedia/src/pjmedia-codec/speex/kiss_fftr.h b/pjmedia/src/pjmedia-codec/speex/kiss_fftr.h new file mode 100644 index 00000000..72e5a577 --- /dev/null +++ b/pjmedia/src/pjmedia-codec/speex/kiss_fftr.h @@ -0,0 +1,46 @@ +#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_fftri(kiss_fftr_cfg cfg,const kiss_fft_cpx *freqdata,kiss_fft_scalar *timedata); +/* + input freqdata has nfft/2+1 complex points + output timedata has nfft scalar points +*/ + +#define kiss_fftr_free free + +#ifdef __cplusplus +} +#endif +#endif diff --git a/pjmedia/src/pjmedia-codec/speex/lpc.c b/pjmedia/src/pjmedia-codec/speex/lpc.c new file mode 100644 index 00000000..c465faea --- /dev/null +++ b/pjmedia/src/pjmedia-codec/speex/lpc.c @@ -0,0 +1,201 @@ +/* + Copyright 1992, 1993, 1994 by Jutta Degener and Carsten Bormann, + Technische Universitaet Berlin + + Any use of this software is permitted provided that this notice is not + removed and that neither the authors nor the Technische Universitaet Berlin + are deemed to have made any representations as to the suitability of this + software for any purpose nor are held responsible for any defects of + this software. THERE IS ABSOLUTELY NO WARRANTY FOR THIS SOFTWARE. + + As a matter of courtesy, the authors request to be informed about uses + this software has found, about bugs in this software, and about any + improvements that may be of general interest. + + Berlin, 28.11.1994 + Jutta Degener + Carsten Bormann + + + Code modified by Jean-Marc Valin + + Speex License: + + 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 "lpc.h" + +#ifdef BFIN_ASM +#include "lpc_bfin.h" +#endif + +/* LPC analysis + * + * The next two functions calculate linear prediction coefficients + * and/or the related reflection coefficients from the first P_MAX+1 + * values of the autocorrelation function. + */ + +/* Invented by N. Levinson in 1947, modified by J. Durbin in 1959. + */ + +/* returns minimum mean square error */ +spx_word32_t _spx_lpc( +spx_coef_t *lpc, /* out: [0...p-1] LPC coefficients */ +const spx_word16_t *ac, /* in: [0...p] autocorrelation values */ +int p +) +{ + int i, j; + spx_word16_t r; + spx_word16_t error = ac[0]; + + if (ac[0] == 0) + { + for (i = 0; i < p; i++) + lpc[i] = 0; + return 0; + } + + for (i = 0; i < p; i++) { + + /* Sum up this iteration's reflection coefficient */ + spx_word32_t rr = NEG32(SHL32(EXTEND32(ac[i + 1]),13)); + for (j = 0; j < i; j++) + rr = SUB32(rr,MULT16_16(lpc[j],ac[i - j])); +#ifdef FIXED_POINT + r = DIV32_16(rr,ADD16(error,16)); +#else + r = rr/(error+.003*ac[0]); +#endif + /* Update LPC coefficients and total error */ + lpc[i] = r; + for (j = 0; j < i>>1; j++) + { + spx_word16_t tmp = lpc[j]; + lpc[j] = MAC16_16_Q13(lpc[j],r,lpc[i-1-j]); + lpc[i-1-j] = MAC16_16_Q13(lpc[i-1-j],r,tmp); + } + if (i & 1) + lpc[j] = MAC16_16_Q13(lpc[j],lpc[j],r); + + error = SUB16(error,MULT16_16_Q13(r,MULT16_16_Q13(error,r))); + } + return error; +} + + +#ifdef FIXED_POINT + +/* Compute the autocorrelation + * ,--, + * ac(i) = > x(n) * x(n-i) for all n + * `--' + * for lags between 0 and lag-1, and x == 0 outside 0...n-1 + */ + +#ifndef OVERRIDE_SPEEX_AUTOCORR +void _spx_autocorr( +const spx_word16_t *x, /* in: [0...n-1] samples x */ +spx_word16_t *ac, /* out: [0...lag-1] ac values */ +int lag, +int n +) +{ + spx_word32_t d; + int i, j; + spx_word32_t ac0=1; + int shift, ac_shift; + + for (j=0;j<n;j++) + ac0 = ADD32(ac0,SHR32(MULT16_16(x[j],x[j]),8)); + ac0 = ADD32(ac0,n); + shift = 8; + while (shift && ac0<0x40000000) + { + shift--; + ac0 <<= 1; + } + ac_shift = 18; + while (ac_shift && ac0<0x40000000) + { + ac_shift--; + ac0 <<= 1; + } + + + for (i=0;i<lag;i++) + { + d=0; + for (j=i;j<n;j++) + { + d = ADD32(d,SHR32(MULT16_16(x[j],x[j-i]), shift)); + } + + ac[i] = SHR32(d, ac_shift); + } +} +#endif + + +#else + + + +/* Compute the autocorrelation + * ,--, + * ac(i) = > x(n) * x(n-i) for all n + * `--' + * for lags between 0 and lag-1, and x == 0 outside 0...n-1 + */ +void _spx_autocorr( +const spx_word16_t *x, /* in: [0...n-1] samples x */ +float *ac, /* out: [0...lag-1] ac values */ +int lag, +int n +) +{ + float d; + int i; + while (lag--) + { + for (i = lag, d = 0; i < n; i++) + d += x[i] * x[i-lag]; + ac[lag] = d; + } + ac[0] += 10; +} + +#endif + + diff --git a/pjmedia/src/pjmedia-codec/speex/ltp.c b/pjmedia/src/pjmedia-codec/speex/ltp.c index 8620dd47..94189c34 100644 --- a/pjmedia/src/pjmedia-codec/speex/ltp.c +++ b/pjmedia/src/pjmedia-codec/speex/ltp.c @@ -176,7 +176,7 @@ void open_loop_nbest_pitch(spx_sig_t *sw, int start, int end, int len, int *pitc VARDECL(spx_word32_t *corr); VARDECL(spx_word32_t *energy); VARDECL(spx_word32_t *score); - /*VARDECL(spx_word16_t *swn2);*/ + VARDECL(spx_word16_t *swn2); spx_word16_t *swn; ALLOC(best_score, N, spx_word32_t); diff --git a/pjmedia/src/pjmedia-codec/speex/mdf.c b/pjmedia/src/pjmedia-codec/speex/mdf.c new file mode 100644 index 00000000..7895927b --- /dev/null +++ b/pjmedia/src/pjmedia-codec/speex/mdf.c @@ -0,0 +1,741 @@ +/* Copyright (C) 2003-2006 Jean-Marc Valin + + File: mdf.c + Echo canceller based on the MDF algorithm (see below) + + 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. +*/ + +/* + The echo canceller is based on the MDF algorithm described in: + + J. S. Soo, K. K. Pang Multidelay block frequency adaptive filter, + IEEE Trans. Acoust. Speech Signal Process., Vol. ASSP-38, No. 2, + February 1990. + + We use the Alternatively Updated MDF (AUMDF) variant. Robustness to + double-talk is achieved using a variable learning rate as described in: + + Valin, J.-M., On Adjusting the Learning Rate in Frequency Domain Echo + Cancellation With Double-Talk. Submitted to IEEE Transactions on Speech + and Audio Processing, 2006. + + There is no explicit double-talk detection, but a continuous variation + in the learning rate based on residual echo, double-talk and background + noise. + + About the fixed-point version: + All the signals are represented with 16-bit words. The filter weights + are represented with 32-bit words, but only the top 16 bits are used + in most cases. The lower 16 bits are completely unreliable (due to the + fact that the update is done only on the top bits), but help in the + adaptation -- probably by removing a "threshold effect" due to + quantization (rounding going to zero) when the gradient is small. + + Another kludge that seems to work good: when performing the weight + update, we only move half the way toward the "goal" this seems to + reduce the effect of quantization noise in the update phase. This + can be seen as applying a gradient descent on a "soft constraint" + instead of having a hard constraint. + +*/ + +#ifdef HAVE_CONFIG_H +#include "config.h" +#endif + +#include "misc.h" +#include "speex/speex_echo.h" +#include "fftwrap.h" +#include "pseudofloat.h" +#include "math_approx.h" + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +#define min(a,b) ((a)<(b) ? (a) : (b)) +#define max(a,b) ((a)>(b) ? (a) : (b)) + +#ifdef FIXED_POINT +#define WEIGHT_SHIFT 11 +#define NORMALIZE_SCALEDOWN 5 +#define NORMALIZE_SCALEUP 3 +#else +#define WEIGHT_SHIFT 0 +#endif + +#ifdef FIXED_POINT +static const spx_float_t MIN_LEAK = ((spx_float_t){16777, -24}); +#define TOP16(x) ((x)>>16) +#else +static const spx_float_t MIN_LEAK = .001f; +#define TOP16(x) (x) +#endif + + +/** 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; + 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 *e; + spx_word16_t *x; + spx_word16_t *X; + spx_word16_t *d; + spx_word16_t *y; + spx_word16_t *last_y; + spx_word32_t *Yps; + spx_word16_t *Y; + spx_word16_t *E; + spx_word32_t *PHI; + spx_word32_t *W; + spx_word32_t *power; + spx_float_t *power_1; + spx_word16_t *wtmp; +#ifdef FIXED_POINT + spx_word16_t *wtmp2; +#endif + spx_word32_t *Rf; + spx_word32_t *Yf; + spx_word32_t *Xf; + spx_word32_t *Eh; + spx_word32_t *Yh; + spx_float_t Pey; + spx_float_t Pyy; + spx_word16_t *window; + void *fft_table; + spx_word16_t memX, memD, memE; + spx_word16_t preemph; + spx_word16_t notch_radius; + spx_mem_t notch_mem[2]; +}; + +static inline void filter_dc_notch16(spx_int16_t *in, spx_word16_t radius, spx_word16_t *out, int len, spx_mem_t *mem) +{ + 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<len;i++) + { + spx_word16_t vin = in[i]; + spx_word32_t vout = mem[0] + SHL32(EXTEND32(vin),15); +#ifdef FIXED_POINT + mem[0] = mem[1] + SHL32(SHL32(-EXTEND32(vin),15) + MULT16_32_Q15(radius,vout),1); +#else + mem[0] = mem[1] + 2*(-vin + radius*vout); +#endif + mem[1] = SHL32(EXTEND32(vin),15) - MULT16_32_Q15(den2,vout); + out[i] = SATURATE32(PSHR32(MULT16_32_Q15(radius,vout),15),32767); + } +} + +static inline spx_word32_t inner_prod(const spx_word16_t *x, const spx_word16_t *y, int len) +{ + spx_word32_t sum=0; + len >>= 2; + while(len--) + { + spx_word32_t part=0; + part = MAC16_16(part,*x++,*y++); + part = MAC16_16(part,*x++,*y++); + 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(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<N-1;i+=2,j++) + { + ps[j] = MULT16_16(X[i],X[i]) + MULT16_16(X[i+1],X[i+1]); + } + ps[j]=MULT16_16(X[i],X[i]); +} + +/** Compute cross-power spectrum of a half-complex (packed) vectors and add to acc */ +#ifdef FIXED_POINT +static inline void spectral_mul_accum(spx_word16_t *X, spx_word32_t *Y, spx_word16_t *acc, int N, int M) +{ + int i,j; + spx_word32_t tmp1=0,tmp2=0; + for (j=0;j<M;j++) + { + tmp1 = MAC16_16(tmp1, X[j*N],TOP16(Y[j*N])); + } + acc[0] = PSHR32(tmp1,WEIGHT_SHIFT); + for (i=1;i<N-1;i+=2) + { + tmp1 = tmp2 = 0; + for (j=0;j<M;j++) + { + tmp1 = SUB32(MAC16_16(tmp1, X[j*N+i],TOP16(Y[j*N+i])), MULT16_16(X[j*N+i+1],TOP16(Y[j*N+i+1]))); + tmp2 = MAC16_16(MAC16_16(tmp2, X[j*N+i+1],TOP16(Y[j*N+i])), X[j*N+i], TOP16(Y[j*N+i+1])); + } + acc[i] = PSHR32(tmp1,WEIGHT_SHIFT); + acc[i+1] = PSHR32(tmp2,WEIGHT_SHIFT); + } + tmp1 = tmp2 = 0; + for (j=0;j<M;j++) + { + tmp1 = MAC16_16(tmp1, X[(j+1)*N-1],TOP16(Y[(j+1)*N-1])); + } + acc[N-1] = PSHR32(tmp1,WEIGHT_SHIFT); +} +#else +static inline void spectral_mul_accum(spx_word16_t *X, spx_word32_t *Y, spx_word16_t *acc, int N, int M) +{ + int i,j; + for (i=0;i<N;i++) + acc[i] = 0; + for (j=0;j<M;j++) + { + acc[0] += X[0]*Y[0]; + for (i=1;i<N-1;i+=2) + { + acc[i] += (X[i]*Y[i] - X[i+1]*Y[i+1]); + acc[i+1] += (X[i+1]*Y[i] + X[i]*Y[i+1]); + } + acc[i] += X[i]*Y[i]; + X += N; + Y += N; + } +} +#endif + +/** Compute weighted cross-power spectrum of a half-complex (packed) vector with conjugate */ +static inline void weighted_spectral_mul_conj(spx_float_t *w, spx_word16_t *X, spx_word16_t *Y, spx_word32_t *prod, int N) +{ + int i, j; + prod[0] = FLOAT_MUL32(w[0],MULT16_16(X[0],Y[0])); + for (i=1,j=1;i<N-1;i+=2,j++) + { + prod[i] = FLOAT_MUL32(w[j],MAC16_16(MULT16_16(X[i],Y[i]), X[i+1],Y[i+1])); + prod[i+1] = FLOAT_MUL32(w[j],MAC16_16(MULT16_16(-X[i+1],Y[i]), X[i],Y[i+1])); + } + prod[i] = FLOAT_MUL32(w[j],MULT16_16(X[i],Y[i])); +} + + +/** Creates a new echo canceller state */ +SpeexEchoState *speex_echo_state_init(int frame_size, int filter_length) +{ + int i,N,M; + SpeexEchoState *st = (SpeexEchoState *)speex_alloc(sizeof(SpeexEchoState)); + + 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; + /* FIXME: Make that an init option (new API call?) */ + st->sampling_rate = 8000; + st->spec_average = DIV32_16(SHL32(st->frame_size, 15), st->sampling_rate); +#ifdef FIXED_POINT + st->beta0 = DIV32_16(SHL32(st->frame_size, 16), st->sampling_rate); + st->beta_max = DIV32_16(SHL32(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->fft_table = spx_fft_init(N); + + st->e = (spx_word16_t*)speex_alloc(N*sizeof(spx_word16_t)); + st->x = (spx_word16_t*)speex_alloc(N*sizeof(spx_word16_t)); + st->d = (spx_word16_t*)speex_alloc(N*sizeof(spx_word16_t)); + st->y = (spx_word16_t*)speex_alloc(N*sizeof(spx_word16_t)); + st->Yps = (spx_word32_t*)speex_alloc(N*sizeof(spx_word32_t)); + st->last_y = (spx_word16_t*)speex_alloc(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(M*N*sizeof(spx_word16_t)); + st->Y = (spx_word16_t*)speex_alloc(N*sizeof(spx_word16_t)); + st->E = (spx_word16_t*)speex_alloc(N*sizeof(spx_word16_t)); + st->W = (spx_word32_t*)speex_alloc(M*N*sizeof(spx_word32_t)); + st->PHI = (spx_word32_t*)speex_alloc(M*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->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<N>>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;i<N;i++) + st->window[i] = .5-.5*cos(2*M_PI*i/N); +#endif + for (i=0;i<N*M;i++) + { + st->W[i] = st->PHI[i] = 0; + } + st->memX=st->memD=st->memE=0; + 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[0] = st->notch_mem[1] = 0; + st->adapted = 0; + st->Pey = st->Pyy = FLOAT_ONE; + return st; +} + +/** Resets echo canceller state */ +void speex_echo_state_reset(SpeexEchoState *st) +{ + int i, M, N; + st->cancel_count=0; + N = st->window_size; + M = st->M; + for (i=0;i<N*M;i++) + { + st->W[i] = 0; + st->X[i] = 0; + } + for (i=0;i<=st->frame_size;i++) + st->power[i] = 0; + + st->adapted = 0; + st->sum_adapt = 0; + st->Pey = st->Pyy = FLOAT_ONE; + +} + +/** Destroys an echo canceller state */ +void speex_echo_state_destroy(SpeexEchoState *st) +{ + spx_fft_destroy(st->fft_table); + + speex_free(st->e); + speex_free(st->x); + speex_free(st->d); + speex_free(st->y); + speex_free(st->last_y); + speex_free(st->Yps); + 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); + speex_free(st->PHI); + speex_free(st->power); + speex_free(st->power_1); + speex_free(st->window); + speex_free(st->wtmp); +#ifdef FIXED_POINT + speex_free(st->wtmp2); +#endif + speex_free(st); +} + +extern int fixed_point; +/** Performs echo cancellation on a frame */ +void speex_echo_cancel(SpeexEchoState *st, short *ref, short *echo, short *out, spx_int32_t *Yout) +{ + int i,j; + int N,M; + spx_word32_t Syy,See; + spx_word16_t leak_estimate; + 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; + spx_word16_t M_1; + + N = st->window_size; + M = st->M; + st->cancel_count++; +#ifdef FIXED_POINT + ss=DIV32_16(11469,M); + ss_1 = SUB16(32767,ss); + M_1 = DIV32_16(32767,M); +#else + ss=.35/M; + ss_1 = 1-ss; + M_1 = 1.f/M; +#endif + + filter_dc_notch16(ref, st->notch_radius, st->d, st->frame_size, st->notch_mem); + /* Copy input data to buffer */ + for (i=0;i<st->frame_size;i++) + { + spx_word16_t tmp; + st->x[i] = st->x[i+st->frame_size]; + st->x[i+st->frame_size] = SUB16(echo[i], MULT16_16_P15(st->preemph, st->memX)); + st->memX = echo[i]; + + tmp = st->d[i]; + st->d[i] = st->d[i+st->frame_size]; + st->d[i+st->frame_size] = SUB16(tmp, MULT16_16_P15(st->preemph, st->memD)); + st->memD = tmp; + } + + /* Shift memory: this could be optimized eventually*/ + for (i=0;i<N*(M-1);i++) + st->X[i]=st->X[i+N]; + + /* Convert x (echo input) to frequency domain */ + spx_fft(st->fft_table, st->x, &st->X[(M-1)*N]); + + /* Compute filter response Y */ + spectral_mul_accum(st->X, st->W, st->Y, N, M); + + spx_ifft(st->fft_table, st->Y, st->y); + +#if 1 + spectral_mul_accum(st->X, st->PHI, st->Y, N, M); + spx_ifft(st->fft_table, st->Y, st->e); +#endif + + /* Compute error signal (for the output with de-emphasis) */ + for (i=0;i<st->frame_size;i++) + { + spx_word32_t tmp_out; +#if 1 + spx_word16_t y = MULT16_16_Q15(st->window[i+st->frame_size],st->e[i+st->frame_size]) + MULT16_16_Q15(st->window[i],st->y[i+st->frame_size]); + tmp_out = SUB32(EXTEND32(st->d[i+st->frame_size]), EXTEND32(y)); +#else + tmp_out = SUB32(EXTEND32(st->d[i+st->frame_size]), EXTEND32(st->y[i+st->frame_size])); +#endif + + /* Saturation */ + if (tmp_out>32767) + tmp_out = 32767; + else if (tmp_out<-32768) + tmp_out = -32768; + tmp_out = ADD32(tmp_out, EXTEND32(MULT16_16_P15(st->preemph, st->memE))); + out[i] = tmp_out; + st->memE = tmp_out; + } + + /* Compute error signal (filter update version) */ + for (i=0;i<st->frame_size;i++) + { + st->e[i] = 0; + st->e[i+st->frame_size] = st->d[i+st->frame_size] - st->y[i+st->frame_size]; + } + + /* Compute a bunch of correlations */ + See = inner_prod(st->e+st->frame_size, st->e+st->frame_size, st->frame_size); + See = ADD32(See, SHR32(10000,6)); + Syy = inner_prod(st->y+st->frame_size, st->y+st->frame_size, st->frame_size); + + /* Convert error to frequency domain */ + spx_fft(st->fft_table, st->e, st->E); + for (i=0;i<st->frame_size;i++) + st->y[i] = 0; + spx_fft(st->fft_table, st->y, st->Y); + + /* Compute power spectrum of echo (X), error (E) and filter response (Y) */ + power_spectrum(st->E, st->Rf, N); + power_spectrum(st->Y, st->Yf, N); + power_spectrum(&st->X[(M-1)*N], st->Xf, N); + + /* Smooth echo 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]); + + /* Enable this to compute the power based only on the tail (would need to compute more + efficiently to make this really useful */ + if (0) + { + float scale2 = .5f/M; + for (j=0;j<=st->frame_size;j++) + st->power[j] = 0; + for (i=0;i<M;i++) + { + power_spectrum(&st->X[i*N], st->Xf, N); + for (j=0;j<=st->frame_size;j++) + st->power[j] += scale2*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 + } + + /* 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 limear regression result */ + leak_estimate = FLOAT_EXTRACT16(FLOAT_SHL(FLOAT_DIVU(st->Pey, st->Pyy),14)); + if (leak_estimate > 16383) + leak_estimate = 32767; + else + leak_estimate = SHL16(leak_estimate,1); + /*printf ("%f\n", leak_estimate);*/ + + /* Compute Residual to Error Ratio */ +#ifdef FIXED_POINT + tmp32 = MULT16_32_Q15(leak_estimate,Syy); + tmp32 = ADD32(tmp32, SHL32(tmp32,1)); + if (tmp32 > SHR32(See,1)) + tmp32 = SHR32(See,1); + RER = FLOAT_EXTRACT16(FLOAT_SHL(FLOAT_DIV32(tmp32,See),15)); +#else + RER = 3.*MULT16_32_Q15(leak_estimate,Syy) / See; + 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 > QCONST32(1,15)) + { + st->adapted = 1; + } + + if (st->adapted) + { + for (i=0;i<=st->frame_size;i++) + { + spx_word32_t r, e; + /* Compute frequency-domain adaptation mask */ + r = MULT16_32_Q15(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(.8,15),r) + MULT16_32_Q15(QCONST16(.2,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(MULT16_32_Q15(M_1,r),FLOAT_MUL32U(e,st->power[i]+10)),WEIGHT_SHIFT+16); + } + } else { + spx_word32_t Sxx; + spx_word16_t adapt_rate=0; + + Sxx = inner_prod(st->x+st->frame_size, st->x+st->frame_size, st->frame_size); + /* Temporary adaption rate if filter is not adapted correctly */ + + tmp32 = MULT16_32_Q15(QCONST16(.15f, 15), Sxx); +#ifdef FIXED_POINT + if (Sxx > SHR32(See,2)) + Sxx = SHR32(See,2); +#else + if (Sxx > .25*See) + Sxx = .25*See; +#endif + adapt_rate = FLOAT_EXTRACT16(FLOAT_SHL(FLOAT_DIV32(MULT16_32_Q15(M_1,Sxx), 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); + } + /* Compute weight gradient */ + for (j=0;j<M;j++) + { + weighted_spectral_mul_conj(st->power_1, &st->X[j*N], st->E, st->PHI+N*j, N); + } + + /* Gradient descent */ + for (i=0;i<M*N;i++) + { + st->W[i] += st->PHI[i]; + /* Old value of W in PHI */ + st->PHI[i] = st->W[i] - st->PHI[i]; + } + + /* Update weight to prevent circular convolution (MDF / AUMDF) */ + for (j=0;j<M;j++) + { + /* This is a variant of the Alternatively Updated MDF (AUMDF) */ + /* Remove the "if" to make this an MDF filter */ + if (j==M-1 || st->cancel_count%(M-1) == j) + { +#ifdef FIXED_POINT + for (i=0;i<N;i++) + st->wtmp2[i] = PSHR32(st->W[j*N+i],NORMALIZE_SCALEDOWN+16); + spx_ifft(st->fft_table, st->wtmp2, st->wtmp); + for (i=0;i<st->frame_size;i++) + { + st->wtmp[i]=0; + } + for (i=st->frame_size;i<N;i++) + { + st->wtmp[i]=SHL(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;i<N;i++) + st->W[j*N+i] -= SHL32(st->wtmp2[i],16+NORMALIZE_SCALEDOWN-NORMALIZE_SCALEUP-1); +#else + spx_ifft(st->fft_table, &st->W[j*N], st->wtmp); + for (i=st->frame_size;i<N;i++) + { + st->wtmp[i]=0; + } + spx_fft(st->fft_table, st->wtmp, &st->W[j*N]); +#endif + } + } + + /* Compute spectrum of estimated echo for use in an echo post-filter (if necessary)*/ + if (Yout) + { + spx_word16_t leak2; + if (st->adapted) + { + /* If the filter is adapted, take the filtered echo */ + for (i=0;i<st->frame_size;i++) + st->last_y[i] = st->last_y[st->frame_size+i]; + for (i=0;i<st->frame_size;i++) + st->last_y[st->frame_size+i] = ref[i]-out[i]; + } else { + /* If filter isn't adapted yet, all we can do is take the echo signal directly */ + for (i=0;i<N;i++) + st->last_y[i] = st->x[i]; + } + + /* Apply hanning window (should pre-compute it)*/ + for (i=0;i<N;i++) + st->y[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, st->Yps, N); + +#ifdef FIXED_POINT + if (leak_estimate > 16383) + leak2 = 32767; + else + leak2 = SHL16(leak_estimate, 1); +#else + if (leak_estimate>.5) + leak2 = 1; + else + leak2 = 2*leak_estimate; +#endif + /* Estimate residual echo */ + for (i=0;i<=st->frame_size;i++) + Yout[i] = MULT16_32_Q15(leak2,st->Yps[i]); + } +} + + +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(st->frame_size, 15), st->sampling_rate); +#ifdef FIXED_POINT + st->beta0 = DIV32_16(SHL32(st->frame_size, 16), st->sampling_rate); + st->beta_max = DIV32_16(SHL32(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; + default: + speex_warning_int("Unknown speex_echo_ctl request: ", request); + return -1; + } + return 0; +} |