← Back to branch summary

~ubuntu-branches/ubuntu/wily/sflphone/wily

« back to all changes in this revision

Viewing changes to daemon/libs/pjproject-2.1.0/third_party/speex/libspeex/preprocess.c

  • Committer: Package Import Robot
  • Author(s): Jonathan Riddell
  • Date: 2015-01-07 14:51:16 UTC
  • mfrom: (4.3.5 sid)
  • Revision ID: package-import@ubuntu.com-20150107145116-yxnafinf4lrdvrmx
Tags: 1.4.1-0.1ubuntu1
* Merge with Debian, remaining changes:
 - Drop soprano, nepomuk build-dep
* Drop ubuntu patches, now upstream

Show diffs side-by-side

added added

removed removed

Lines of Context:
daemon/libs/pjproject-2.1.0/third_party/speex/libspeex/preprocess.c
1
 
/* Copyright (C) 2003 Epic Games (written by Jean-Marc Valin)
2
 
   Copyright (C) 2004-2006 Epic Games 
3
 
   
4
 
   File: preprocess.c
5
 
   Preprocessor with denoising based on the algorithm by Ephraim and Malah
6
 
 
7
 
   Redistribution and use in source and binary forms, with or without
8
 
   modification, are permitted provided that the following conditions are
9
 
   met:
10
 
 
11
 
   1. Redistributions of source code must retain the above copyright notice,
12
 
   this list of conditions and the following disclaimer.
13
 
 
14
 
   2. Redistributions in binary form must reproduce the above copyright
15
 
   notice, this list of conditions and the following disclaimer in the
16
 
   documentation and/or other materials provided with the distribution.
17
 
 
18
 
   3. The name of the author may not be used to endorse or promote products
19
 
   derived from this software without specific prior written permission.
20
 
 
21
 
   THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
22
 
   IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
23
 
   OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
24
 
   DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT,
25
 
   INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
26
 
   (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
27
 
   SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
28
 
   HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
29
 
   STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30
 
   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31
 
   POSSIBILITY OF SUCH DAMAGE.
32
 
*/
33
 
 
34
 
 
35
 
/*
36
 
   Recommended papers:
37
 
   
38
 
   Y. Ephraim and D. Malah, "Speech enhancement using minimum mean-square error
39
 
   short-time spectral amplitude estimator". IEEE Transactions on Acoustics, 
40
 
   Speech and Signal Processing, vol. ASSP-32, no. 6, pp. 1109-1121, 1984.
41
 
   
42
 
   Y. Ephraim and D. Malah, "Speech enhancement using minimum mean-square error
43
 
   log-spectral amplitude estimator". IEEE Transactions on Acoustics, Speech and 
44
 
   Signal Processing, vol. ASSP-33, no. 2, pp. 443-445, 1985.
45
 
   
46
 
   I. Cohen and B. Berdugo, "Speech enhancement for non-stationary noise environments".
47
 
   Signal Processing, vol. 81, no. 2, pp. 2403-2418, 2001.
48
 
 
49
 
   Stefan Gustafsson, Rainer Martin, Peter Jax, and Peter Vary. "A psychoacoustic 
50
 
   approach to combined acoustic echo cancellation and noise reduction". IEEE 
51
 
   Transactions on Speech and Audio Processing, 2002.
52
 
   
53
 
   J.-M. Valin, J. Rouat, and F. Michaud, "Microphone array post-filter for separation
54
 
   of simultaneous non-stationary sources". In Proceedings IEEE International 
55
 
   Conference on Acoustics, Speech, and Signal Processing, 2004.
56
 
*/
57
 
 
58
 
#ifdef HAVE_CONFIG_H
59
 
#include "config.h"
60
 
#endif
61
 
 
62
 
#include <math.h>
63
 
#include "speex/speex_preprocess.h"
64
 
#include "speex/speex_echo.h"
65
 
#include "arch.h"
66
 
#include "fftwrap.h"
67
 
#include "filterbank.h"
68
 
#include "math_approx.h"
69
 
#include "os_support.h"
70
 
 
71
 
#ifndef M_PI
72
 
#define M_PI 3.14159263
73
 
#endif
74
 
 
75
 
#define LOUDNESS_EXP 5.f
76
 
#define AMP_SCALE .001f
77
 
#define AMP_SCALE_1 1000.f
78
 
      
79
 
#define NB_BANDS 24
80
 
 
81
 
#define SPEECH_PROB_START_DEFAULT       QCONST16(0.35f,15)
82
 
#define SPEECH_PROB_CONTINUE_DEFAULT    QCONST16(0.20f,15)
83
 
#define NOISE_SUPPRESS_DEFAULT       -15
84
 
#define ECHO_SUPPRESS_DEFAULT        -40
85
 
#define ECHO_SUPPRESS_ACTIVE_DEFAULT -15
86
 
 
87
 
#ifndef NULL
88
 
#define NULL 0
89
 
#endif
90
 
 
91
 
#define SQR(x) ((x)*(x))
92
 
#define SQR16(x) (MULT16_16((x),(x)))
93
 
#define SQR16_Q15(x) (MULT16_16_Q15((x),(x)))
94
 
 
95
 
#ifdef FIXED_POINT
96
 
static inline spx_word16_t DIV32_16_Q8(spx_word32_t a, spx_word32_t b)
97
 
{
98
 
   if (SHR32(a,7) >= b)
99
 
   {
100
 
      return 32767;
101
 
   } else {
102
 
      if (b>=QCONST32(1,23))
103
 
      {
104
 
         a = SHR32(a,8);
105
 
         b = SHR32(b,8);
106
 
      }
107
 
      if (b>=QCONST32(1,19))
108
 
      {
109
 
         a = SHR32(a,4);
110
 
         b = SHR32(b,4);
111
 
      }
112
 
      if (b>=QCONST32(1,15))
113
 
      {
114
 
         a = SHR32(a,4);
115
 
         b = SHR32(b,4);
116
 
      }
117
 
      a = SHL32(a,8);
118
 
      return PDIV32_16(a,b);
119
 
   }
120
 
   
121
 
}
122
 
static inline spx_word16_t DIV32_16_Q15(spx_word32_t a, spx_word32_t b)
123
 
{
124
 
   if (SHR32(a,15) >= b)
125
 
   {
126
 
      return 32767;
127
 
   } else {
128
 
      if (b>=QCONST32(1,23))
129
 
      {
130
 
         a = SHR32(a,8);
131
 
         b = SHR32(b,8);
132
 
      }
133
 
      if (b>=QCONST32(1,19))
134
 
      {
135
 
         a = SHR32(a,4);
136
 
         b = SHR32(b,4);
137
 
      }
138
 
      if (b>=QCONST32(1,15))
139
 
      {
140
 
         a = SHR32(a,4);
141
 
         b = SHR32(b,4);
142
 
      }
143
 
      a = SHL32(a,15)-a;
144
 
      return DIV32_16(a,b);
145
 
   }
146
 
}
147
 
#define SNR_SCALING 256.f
148
 
#define SNR_SCALING_1 0.0039062f
149
 
#define SNR_SHIFT 8
150
 
 
151
 
#define FRAC_SCALING 32767.f
152
 
#define FRAC_SCALING_1 3.0518e-05
153
 
#define FRAC_SHIFT 1
154
 
 
155
 
#define EXPIN_SCALING 2048.f
156
 
#define EXPIN_SCALING_1 0.00048828f
157
 
#define EXPIN_SHIFT 11
158
 
#define EXPOUT_SCALING_1 1.5259e-05
159
 
 
160
 
#define NOISE_SHIFT 7
161
 
 
162
 
#else
163
 
 
164
 
#define DIV32_16_Q8(a,b) ((a)/(b))
165
 
#define DIV32_16_Q15(a,b) ((a)/(b))
166
 
#define SNR_SCALING 1.f
167
 
#define SNR_SCALING_1 1.f
168
 
#define SNR_SHIFT 0
169
 
#define FRAC_SCALING 1.f
170
 
#define FRAC_SCALING_1 1.f
171
 
#define FRAC_SHIFT 0
172
 
#define NOISE_SHIFT 0
173
 
 
174
 
#define EXPIN_SCALING 1.f
175
 
#define EXPIN_SCALING_1 1.f
176
 
#define EXPOUT_SCALING_1 1.f
177
 
 
178
 
#endif
179
 
 
180
 
/** Speex pre-processor state. */
181
 
struct SpeexPreprocessState_ {
182
 
   /* Basic info */
183
 
   int    frame_size;        /**< Number of samples processed each time */
184
 
   int    ps_size;           /**< Number of points in the power spectrum */
185
 
   int    sampling_rate;     /**< Sampling rate of the input/output */
186
 
   int    nbands;
187
 
   FilterBank *bank;
188
 
   
189
 
   /* Parameters */
190
 
   int    denoise_enabled;
191
 
   int    vad_enabled;
192
 
   int    dereverb_enabled;
193
 
   spx_word16_t  reverb_decay;
194
 
   spx_word16_t  reverb_level;
195
 
   spx_word16_t speech_prob_start;
196
 
   spx_word16_t speech_prob_continue;
197
 
   int    noise_suppress;
198
 
   int    echo_suppress;
199
 
   int    echo_suppress_active;
200
 
   SpeexEchoState *echo_state;
201
 
   
202
 
   spx_word16_t speech_prob;  /**< Probability last frame was speech */
203
 
 
204
 
   /* DSP-related arrays */
205
 
   spx_word16_t *frame;      /**< Processing frame (2*ps_size) */
206
 
   spx_word16_t *ft;         /**< Processing frame in freq domain (2*ps_size) */
207
 
   spx_word32_t *ps;         /**< Current power spectrum */
208
 
   spx_word16_t *gain2;      /**< Adjusted gains */
209
 
   spx_word16_t *gain_floor; /**< Minimum gain allowed */
210
 
   spx_word16_t *window;     /**< Analysis/Synthesis window */
211
 
   spx_word32_t *noise;      /**< Noise estimate */
212
 
   spx_word32_t *reverb_estimate; /**< Estimate of reverb energy */
213
 
   spx_word32_t *old_ps;     /**< Power spectrum for last frame */
214
 
   spx_word16_t *gain;       /**< Ephraim Malah gain */
215
 
   spx_word16_t *prior;      /**< A-priori SNR */
216
 
   spx_word16_t *post;       /**< A-posteriori SNR */
217
 
 
218
 
   spx_word32_t *S;          /**< Smoothed power spectrum */
219
 
   spx_word32_t *Smin;       /**< See Cohen paper */
220
 
   spx_word32_t *Stmp;       /**< See Cohen paper */
221
 
   int *update_prob;         /**< Probability of speech presence for noise update */
222
 
 
223
 
   spx_word16_t *zeta;       /**< Smoothed a priori SNR */
224
 
   spx_word32_t *echo_noise;
225
 
   spx_word32_t *residual_echo;
226
 
 
227
 
   /* Misc */
228
 
   spx_word16_t *inbuf;      /**< Input buffer (overlapped analysis) */
229
 
   spx_word16_t *outbuf;     /**< Output buffer (for overlap and add) */
230
 
 
231
 
   /* AGC stuff, only for floating point for now */
232
 
#ifndef FIXED_POINT
233
 
   int    agc_enabled;
234
 
   float  agc_level;
235
 
   float  loudness_accum;
236
 
   float *loudness_weight;   /**< Perceptual loudness curve */
237
 
   float  loudness;          /**< Loudness estimate */
238
 
   float  agc_gain;          /**< Current AGC gain */
239
 
   float  max_gain;          /**< Maximum gain allowed */
240
 
   float  max_increase_step; /**< Maximum increase in gain from one frame to another */
241
 
   float  max_decrease_step; /**< Maximum decrease in gain from one frame to another */
242
 
   float  prev_loudness;     /**< Loudness of previous frame */
243
 
   float  init_max;          /**< Current gain limit during initialisation */
244
 
#endif
245
 
   int    nb_adapt;          /**< Number of frames used for adaptation so far */
246
 
   int    was_speech;
247
 
   int    min_count;         /**< Number of frames processed so far */
248
 
   void  *fft_lookup;        /**< Lookup table for the FFT */
249
 
#ifdef FIXED_POINT
250
 
   int    frame_shift;
251
 
#endif
252
 
};
253
 
 
254
 
 
255
 
static void conj_window(spx_word16_t *w, int len)
256
 
{
257
 
   int i;
258
 
   for (i=0;i<len;i++)
259
 
   {
260
 
      spx_word16_t tmp;
261
 
#ifdef FIXED_POINT
262
 
      spx_word16_t x = DIV32_16(MULT16_16(32767,i),len);
263
 
#else      
264
 
      spx_word16_t x = DIV32_16(MULT16_16(QCONST16(4.f,13),i),len);
265
 
#endif
266
 
      int inv=0;
267
 
      if (x<QCONST16(1.f,13))
268
 
      {
269
 
      } else if (x<QCONST16(2.f,13))
270
 
      {
271
 
         x=QCONST16(2.f,13)-x;
272
 
         inv=1;
273
 
      } else if (x<QCONST16(3.f,13))
274
 
      {
275
 
         x=x-QCONST16(2.f,13);
276
 
         inv=1;
277
 
      } else {
278
 
         x=QCONST16(2.f,13)-x+QCONST16(2.f,13); /* 4 - x */
279
 
      }
280
 
      x = MULT16_16_Q14(QCONST16(1.271903f,14), x);
281
 
      tmp = SQR16_Q15(QCONST16(.5f,15)-MULT16_16_P15(QCONST16(.5f,15),spx_cos_norm(SHL32(EXTEND32(x),2))));
282
 
      if (inv)
283
 
         tmp=SUB16(Q15_ONE,tmp);
284
 
      w[i]=spx_sqrt(SHL32(EXTEND32(tmp),15));
285
 
   }
286
 
}
287
 
 
288
 
      
289
 
#ifdef FIXED_POINT
290
 
/* This function approximates the gain function 
291
 
   y = gamma(1.25)^2 * M(-.25;1;-x) / sqrt(x)  
292
 
   which multiplied by xi/(1+xi) is the optimal gain
293
 
   in the loudness domain ( sqrt[amplitude] )
294
 
   Input in Q11 format, output in Q15
295
 
*/
296
 
static inline spx_word32_t hypergeom_gain(spx_word32_t xx)
297
 
{
298
 
   int ind;
299
 
   spx_word16_t frac;
300
 
   /* Q13 table */
301
 
   static const spx_word16_t table[21] = {
302
 
       6730,  8357,  9868, 11267, 12563, 13770, 14898,
303
 
      15959, 16961, 17911, 18816, 19682, 20512, 21311,
304
 
      22082, 22827, 23549, 24250, 24931, 25594, 26241};
305
 
      ind = SHR32(xx,10);
306
 
      if (ind<0)
307
 
         return Q15_ONE;
308
 
      if (ind>19)
309
 
         return ADD32(EXTEND32(Q15_ONE),EXTEND32(DIV32_16(QCONST32(.1296,23), SHR32(xx,EXPIN_SHIFT-SNR_SHIFT))));
310
 
      frac = SHL32(xx-SHL32(ind,10),5);
311
 
      return SHL32(DIV32_16(PSHR32(MULT16_16(Q15_ONE-frac,table[ind]) + MULT16_16(frac,table[ind+1]),7),(spx_sqrt(SHL32(xx,15)+6711))),7);
312
 
}
313
 
 
314
 
static inline spx_word16_t qcurve(spx_word16_t x)
315
 
{
316
 
   x = MAX16(x, 1);
317
 
   return DIV32_16(SHL32(EXTEND32(32767),9),ADD16(512,MULT16_16_Q15(QCONST16(.60f,15),DIV32_16(32767,x))));
318
 
}
319
 
 
320
 
/* Compute the gain floor based on different floors for the background noise and residual echo */
321
 
static void compute_gain_floor(int noise_suppress, int effective_echo_suppress, spx_word32_t *noise, spx_word32_t *echo, spx_word16_t *gain_floor, int len)
322
 
{
323
 
   int i;
324
 
   
325
 
   if (noise_suppress > effective_echo_suppress)
326
 
   {
327
 
      spx_word16_t noise_gain, gain_ratio;
328
 
      noise_gain = EXTRACT16(MIN32(Q15_ONE,SHR32(spx_exp(MULT16_16(QCONST16(0.11513,11),noise_suppress)),1)));
329
 
      gain_ratio = EXTRACT16(MIN32(Q15_ONE,SHR32(spx_exp(MULT16_16(QCONST16(.2302585f,11),effective_echo_suppress-noise_suppress)),1)));
330
 
 
331
 
      /* gain_floor = sqrt [ (noise*noise_floor + echo*echo_floor) / (noise+echo) ] */
332
 
      for (i=0;i<len;i++)
333
 
         gain_floor[i] = MULT16_16_Q15(noise_gain,
334
 
                                       spx_sqrt(SHL32(EXTEND32(DIV32_16_Q15(PSHR32(noise[i],NOISE_SHIFT) + MULT16_32_Q15(gain_ratio,echo[i]),
335
 
                                             (1+PSHR32(noise[i],NOISE_SHIFT) + echo[i]) )),15)));
336
 
   } else {
337
 
      spx_word16_t echo_gain, gain_ratio;
338
 
      echo_gain = EXTRACT16(MIN32(Q15_ONE,SHR32(spx_exp(MULT16_16(QCONST16(0.11513,11),effective_echo_suppress)),1)));
339
 
      gain_ratio = EXTRACT16(MIN32(Q15_ONE,SHR32(spx_exp(MULT16_16(QCONST16(.2302585f,11),noise_suppress-effective_echo_suppress)),1)));
340
 
 
341
 
      /* gain_floor = sqrt [ (noise*noise_floor + echo*echo_floor) / (noise+echo) ] */
342
 
      for (i=0;i<len;i++)
343
 
         gain_floor[i] = MULT16_16_Q15(echo_gain,
344
 
                                       spx_sqrt(SHL32(EXTEND32(DIV32_16_Q15(MULT16_32_Q15(gain_ratio,PSHR32(noise[i],NOISE_SHIFT)) + echo[i],
345
 
                                             (1+PSHR32(noise[i],NOISE_SHIFT) + echo[i]) )),15)));
346
 
   }
347
 
}
348
 
 
349
 
#else
350
 
/* This function approximates the gain function 
351
 
   y = gamma(1.25)^2 * M(-.25;1;-x) / sqrt(x)  
352
 
   which multiplied by xi/(1+xi) is the optimal gain
353
 
   in the loudness domain ( sqrt[amplitude] )
354
 
*/
355
 
static inline spx_word32_t hypergeom_gain(spx_word32_t xx)
356
 
{
357
 
   int ind;
358
 
   float integer, frac;
359
 
   float x;
360
 
   static const float table[21] = {
361
 
      0.82157f, 1.02017f, 1.20461f, 1.37534f, 1.53363f, 1.68092f, 1.81865f,
362
 
      1.94811f, 2.07038f, 2.18638f, 2.29688f, 2.40255f, 2.50391f, 2.60144f,
363
 
      2.69551f, 2.78647f, 2.87458f, 2.96015f, 3.04333f, 3.12431f, 3.20326f};
364
 
      x = EXPIN_SCALING_1*xx;
365
 
      integer = floor(2*x);
366
 
      ind = (int)integer;
367
 
      if (ind<0)
368
 
         return FRAC_SCALING;
369
 
      if (ind>19)
370
 
         return FRAC_SCALING*(1+.1296/x);
371
 
      frac = 2*x-integer;
372
 
      return FRAC_SCALING*((1-frac)*table[ind] + frac*table[ind+1])/sqrt(x+.0001f);
373
 
}
374
 
 
375
 
static inline spx_word16_t qcurve(spx_word16_t x)
376
 
{
377
 
   return 1.f/(1.f+.15f/(SNR_SCALING_1*x));
378
 
}
379
 
 
380
 
static void compute_gain_floor(int noise_suppress, int effective_echo_suppress, spx_word32_t *noise, spx_word32_t *echo, spx_word16_t *gain_floor, int len)
381
 
{
382
 
   int i;
383
 
   float echo_floor;
384
 
   float noise_floor;
385
 
 
386
 
   noise_floor = exp(.2302585f*noise_suppress);
387
 
   echo_floor = exp(.2302585f*effective_echo_suppress);
388
 
 
389
 
   /* Compute the gain floor based on different floors for the background noise and residual echo */
390
 
   for (i=0;i<len;i++)
391
 
      gain_floor[i] = FRAC_SCALING*sqrt(noise_floor*PSHR32(noise[i],NOISE_SHIFT) + echo_floor*echo[i])/sqrt(1+PSHR32(noise[i],NOISE_SHIFT) + echo[i]);
392
 
}
393
 
 
394
 
#endif
395
 
EXPORT SpeexPreprocessState *speex_preprocess_state_init(int frame_size, int sampling_rate)
396
 
{
397
 
   int i;
398
 
   int N, N3, N4, M;
399
 
 
400
 
   SpeexPreprocessState *st = (SpeexPreprocessState *)speex_alloc(sizeof(SpeexPreprocessState));
401
 
   st->frame_size = frame_size;
402
 
 
403
 
   /* Round ps_size down to the nearest power of two */
404
 
#if 0
405
 
   i=1;
406
 
   st->ps_size = st->frame_size;
407
 
   while(1)
408
 
   {
409
 
      if (st->ps_size & ~i)
410
 
      {
411
 
         st->ps_size &= ~i;
412
 
         i<<=1;
413
 
      } else {
414
 
         break;
415
 
      }
416
 
   }
417
 
   
418
 
   
419
 
   if (st->ps_size < 3*st->frame_size/4)
420
 
      st->ps_size = st->ps_size * 3 / 2;
421
 
#else
422
 
   st->ps_size = st->frame_size;
423
 
#endif
424
 
 
425
 
   N = st->ps_size;
426
 
   N3 = 2*N - st->frame_size;
427
 
   N4 = st->frame_size - N3;
428
 
   
429
 
   st->sampling_rate = sampling_rate;
430
 
   st->denoise_enabled = 1;
431
 
   st->vad_enabled = 0;
432
 
   st->dereverb_enabled = 0;
433
 
   st->reverb_decay = 0;
434
 
   st->reverb_level = 0;
435
 
   st->noise_suppress = NOISE_SUPPRESS_DEFAULT;
436
 
   st->echo_suppress = ECHO_SUPPRESS_DEFAULT;
437
 
   st->echo_suppress_active = ECHO_SUPPRESS_ACTIVE_DEFAULT;
438
 
 
439
 
   st->speech_prob_start = SPEECH_PROB_START_DEFAULT;
440
 
   st->speech_prob_continue = SPEECH_PROB_CONTINUE_DEFAULT;
441
 
 
442
 
   st->echo_state = NULL;
443
 
   
444
 
   st->nbands = NB_BANDS;
445
 
   M = st->nbands;
446
 
   st->bank = filterbank_new(M, sampling_rate, N, 1);
447
 
   
448
 
   st->frame = (spx_word16_t*)speex_alloc(2*N*sizeof(spx_word16_t));
449
 
   st->window = (spx_word16_t*)speex_alloc(2*N*sizeof(spx_word16_t));
450
 
   st->ft = (spx_word16_t*)speex_alloc(2*N*sizeof(spx_word16_t));
451
 
   
452
 
   st->ps = (spx_word32_t*)speex_alloc((N+M)*sizeof(spx_word32_t));
453
 
   st->noise = (spx_word32_t*)speex_alloc((N+M)*sizeof(spx_word32_t));
454
 
   st->echo_noise = (spx_word32_t*)speex_alloc((N+M)*sizeof(spx_word32_t));
455
 
   st->residual_echo = (spx_word32_t*)speex_alloc((N+M)*sizeof(spx_word32_t));
456
 
   st->reverb_estimate = (spx_word32_t*)speex_alloc((N+M)*sizeof(spx_word32_t));
457
 
   st->old_ps = (spx_word32_t*)speex_alloc((N+M)*sizeof(spx_word32_t));
458
 
   st->prior = (spx_word16_t*)speex_alloc((N+M)*sizeof(spx_word16_t));
459
 
   st->post = (spx_word16_t*)speex_alloc((N+M)*sizeof(spx_word16_t));
460
 
   st->gain = (spx_word16_t*)speex_alloc((N+M)*sizeof(spx_word16_t));
461
 
   st->gain2 = (spx_word16_t*)speex_alloc((N+M)*sizeof(spx_word16_t));
462
 
   st->gain_floor = (spx_word16_t*)speex_alloc((N+M)*sizeof(spx_word16_t));
463
 
   st->zeta = (spx_word16_t*)speex_alloc((N+M)*sizeof(spx_word16_t));
464
 
   
465
 
   st->S = (spx_word32_t*)speex_alloc(N*sizeof(spx_word32_t));
466
 
   st->Smin = (spx_word32_t*)speex_alloc(N*sizeof(spx_word32_t));
467
 
   st->Stmp = (spx_word32_t*)speex_alloc(N*sizeof(spx_word32_t));
468
 
   st->update_prob = (int*)speex_alloc(N*sizeof(int));
469
 
   
470
 
   st->inbuf = (spx_word16_t*)speex_alloc(N3*sizeof(spx_word16_t));
471
 
   st->outbuf = (spx_word16_t*)speex_alloc(N3*sizeof(spx_word16_t));
472
 
 
473
 
   conj_window(st->window, 2*N3);
474
 
   for (i=2*N3;i<2*st->ps_size;i++)
475
 
      st->window[i]=Q15_ONE;
476
 
   
477
 
   if (N4>0)
478
 
   {
479
 
      for (i=N3-1;i>=0;i--)
480
 
      {
481
 
         st->window[i+N3+N4]=st->window[i+N3];
482
 
         st->window[i+N3]=1;
483
 
      }
484
 
   }
485
 
   for (i=0;i<N+M;i++)
486
 
   {
487
 
      st->noise[i]=QCONST32(1.f,NOISE_SHIFT);
488
 
      st->reverb_estimate[i]=0;
489
 
      st->old_ps[i]=1;
490
 
      st->gain[i]=Q15_ONE;
491
 
      st->post[i]=SHL16(1, SNR_SHIFT);
492
 
      st->prior[i]=SHL16(1, SNR_SHIFT);
493
 
   }
494
 
 
495
 
   for (i=0;i<N;i++)
496
 
      st->update_prob[i] = 1;
497
 
   for (i=0;i<N3;i++)
498
 
   {
499
 
      st->inbuf[i]=0;
500
 
      st->outbuf[i]=0;
501
 
   }
502
 
#ifndef FIXED_POINT
503
 
   st->agc_enabled = 0;
504
 
   st->agc_level = 8000;
505
 
   st->loudness_weight = (float*)speex_alloc(N*sizeof(float));
506
 
   for (i=0;i<N;i++)
507
 
   {
508
 
      float ff=((float)i)*.5*sampling_rate/((float)N);
509
 
      /*st->loudness_weight[i] = .5f*(1.f/(1.f+ff/8000.f))+1.f*exp(-.5f*(ff-3800.f)*(ff-3800.f)/9e5f);*/
510
 
      st->loudness_weight[i] = .35f-.35f*ff/16000.f+.73f*exp(-.5f*(ff-3800)*(ff-3800)/9e5f);
511
 
      if (st->loudness_weight[i]<.01f)
512
 
         st->loudness_weight[i]=.01f;
513
 
      st->loudness_weight[i] *= st->loudness_weight[i];
514
 
   }
515
 
   /*st->loudness = pow(AMP_SCALE*st->agc_level,LOUDNESS_EXP);*/
516
 
   st->loudness = 1e-15;
517
 
   st->agc_gain = 1;
518
 
   st->max_gain = 30;
519
 
   st->max_increase_step = exp(0.11513f * 12.*st->frame_size / st->sampling_rate);
520
 
   st->max_decrease_step = exp(-0.11513f * 40.*st->frame_size / st->sampling_rate);
521
 
   st->prev_loudness = 1;
522
 
   st->init_max = 1;
523
 
#endif
524
 
   st->was_speech = 0;
525
 
 
526
 
   st->fft_lookup = spx_fft_init(2*N);
527
 
 
528
 
   st->nb_adapt=0;
529
 
   st->min_count=0;
530
 
   return st;
531
 
}
532
 
 
533
 
EXPORT void speex_preprocess_state_destroy(SpeexPreprocessState *st)
534
 
{
535
 
   speex_free(st->frame);
536
 
   speex_free(st->ft);
537
 
   speex_free(st->ps);
538
 
   speex_free(st->gain2);
539
 
   speex_free(st->gain_floor);
540
 
   speex_free(st->window);
541
 
   speex_free(st->noise);
542
 
   speex_free(st->reverb_estimate);
543
 
   speex_free(st->old_ps);
544
 
   speex_free(st->gain);
545
 
   speex_free(st->prior);
546
 
   speex_free(st->post);
547
 
#ifndef FIXED_POINT
548
 
   speex_free(st->loudness_weight);
549
 
#endif
550
 
   speex_free(st->echo_noise);
551
 
   speex_free(st->residual_echo);
552
 
 
553
 
   speex_free(st->S);
554
 
   speex_free(st->Smin);
555
 
   speex_free(st->Stmp);
556
 
   speex_free(st->update_prob);
557
 
   speex_free(st->zeta);
558
 
 
559
 
   speex_free(st->inbuf);
560
 
   speex_free(st->outbuf);
561
 
 
562
 
   spx_fft_destroy(st->fft_lookup);
563
 
   filterbank_destroy(st->bank);
564
 
   speex_free(st);
565
 
}
566
 
 
567
 
/* FIXME: The AGC doesn't work yet with fixed-point*/
568
 
#ifndef FIXED_POINT
569
 
static void speex_compute_agc(SpeexPreprocessState *st, spx_word16_t Pframe, spx_word16_t *ft)
570
 
{
571
 
   int i;
572
 
   int N = st->ps_size;
573
 
   float target_gain;
574
 
   float loudness=1.f;
575
 
   float rate;
576
 
   
577
 
   for (i=2;i<N;i++)
578
 
   {
579
 
      loudness += 2.f*N*st->ps[i]* st->loudness_weight[i];
580
 
   }
581
 
   loudness=sqrt(loudness);
582
 
      /*if (loudness < 2*pow(st->loudness, 1.0/LOUDNESS_EXP) &&
583
 
   loudness*2 > pow(st->loudness, 1.0/LOUDNESS_EXP))*/
584
 
   if (Pframe>.3f)
585
 
   {
586
 
      /*rate=2.0f*Pframe*Pframe/(1+st->nb_loudness_adapt);*/
587
 
      rate = .03*Pframe*Pframe;
588
 
      st->loudness = (1-rate)*st->loudness + (rate)*pow(AMP_SCALE*loudness, LOUDNESS_EXP);
589
 
      st->loudness_accum = (1-rate)*st->loudness_accum + rate;
590
 
      if (st->init_max < st->max_gain && st->nb_adapt > 20)
591
 
         st->init_max *= 1.f + .1f*Pframe*Pframe;
592
 
   }
593
 
   /*printf ("%f %f %f %f\n", Pframe, loudness, pow(st->loudness, 1.0f/LOUDNESS_EXP), st->loudness2);*/
594
 
   
595
 
   target_gain = AMP_SCALE*st->agc_level*pow(st->loudness/(1e-4+st->loudness_accum), -1.0f/LOUDNESS_EXP);
596
 
 
597
 
   if ((Pframe>.5  && st->nb_adapt > 20) || target_gain < st->agc_gain)
598
 
   {
599
 
      if (target_gain > st->max_increase_step*st->agc_gain)
600
 
         target_gain = st->max_increase_step*st->agc_gain;
601
 
      if (target_gain < st->max_decrease_step*st->agc_gain && loudness < 10*st->prev_loudness)
602
 
         target_gain = st->max_decrease_step*st->agc_gain;
603
 
      if (target_gain > st->max_gain)
604
 
         target_gain = st->max_gain;
605
 
      if (target_gain > st->init_max)
606
 
         target_gain = st->init_max;
607
 
   
608
 
      st->agc_gain = target_gain;
609
 
   }
610
 
   /*fprintf (stderr, "%f %f %f\n", loudness, (float)AMP_SCALE_1*pow(st->loudness, 1.0f/LOUDNESS_EXP), st->agc_gain);*/
611
 
      
612
 
   for (i=0;i<2*N;i++)
613
 
      ft[i] *= st->agc_gain;
614
 
   st->prev_loudness = loudness;
615
 
}
616
 
#endif
617
 
 
618
 
static void preprocess_analysis(SpeexPreprocessState *st, spx_int16_t *x)
619
 
{
620
 
   int i;
621
 
   int N = st->ps_size;
622
 
   int N3 = 2*N - st->frame_size;
623
 
   int N4 = st->frame_size - N3;
624
 
   spx_word32_t *ps=st->ps;
625
 
 
626
 
   /* 'Build' input frame */
627
 
   for (i=0;i<N3;i++)
628
 
      st->frame[i]=st->inbuf[i];
629
 
   for (i=0;i<st->frame_size;i++)
630
 
      st->frame[N3+i]=x[i];
631
 
   
632
 
   /* Update inbuf */
633
 
   for (i=0;i<N3;i++)
634
 
      st->inbuf[i]=x[N4+i];
635
 
 
636
 
   /* Windowing */
637
 
   for (i=0;i<2*N;i++)
638
 
      st->frame[i] = MULT16_16_Q15(st->frame[i], st->window[i]);
639
 
 
640
 
#ifdef FIXED_POINT
641
 
   {
642
 
      spx_word16_t max_val=0;
643
 
      for (i=0;i<2*N;i++)
644
 
         max_val = MAX16(max_val, ABS16(st->frame[i]));
645
 
      st->frame_shift = 14-spx_ilog2(EXTEND32(max_val));
646
 
      for (i=0;i<2*N;i++)
647
 
         st->frame[i] = SHL16(st->frame[i], st->frame_shift);
648
 
   }
649
 
#endif
650
 
   
651
 
   /* Perform FFT */
652
 
   spx_fft(st->fft_lookup, st->frame, st->ft);
653
 
         
654
 
   /* Power spectrum */
655
 
   ps[0]=MULT16_16(st->ft[0],st->ft[0]);
656
 
   for (i=1;i<N;i++)
657
 
      ps[i]=MULT16_16(st->ft[2*i-1],st->ft[2*i-1]) + MULT16_16(st->ft[2*i],st->ft[2*i]);
658
 
   for (i=0;i<N;i++)
659
 
      st->ps[i] = PSHR32(st->ps[i], 2*st->frame_shift);
660
 
 
661
 
   filterbank_compute_bank32(st->bank, ps, ps+N);
662
 
}
663
 
 
664
 
static void update_noise_prob(SpeexPreprocessState *st)
665
 
{
666
 
   int i;
667
 
   int min_range;
668
 
   int N = st->ps_size;
669
 
 
670
 
   for (i=1;i<N-1;i++)
671
 
      st->S[i] =  MULT16_32_Q15(QCONST16(.8f,15),st->S[i]) + MULT16_32_Q15(QCONST16(.05f,15),st->ps[i-1]) 
672
 
                      + MULT16_32_Q15(QCONST16(.1f,15),st->ps[i]) + MULT16_32_Q15(QCONST16(.05f,15),st->ps[i+1]);
673
 
   st->S[0] =  MULT16_32_Q15(QCONST16(.8f,15),st->S[0]) + MULT16_32_Q15(QCONST16(.2f,15),st->ps[0]);
674
 
   st->S[N-1] =  MULT16_32_Q15(QCONST16(.8f,15),st->S[N-1]) + MULT16_32_Q15(QCONST16(.2f,15),st->ps[N-1]);
675
 
   
676
 
   if (st->nb_adapt==1)
677
 
   {
678
 
      for (i=0;i<N;i++)
679
 
         st->Smin[i] = st->Stmp[i] = 0;
680
 
   }
681
 
 
682
 
   if (st->nb_adapt < 100)
683
 
      min_range = 15;
684
 
   else if (st->nb_adapt < 1000)
685
 
      min_range = 50;
686
 
   else if (st->nb_adapt < 10000)
687
 
      min_range = 150;
688
 
   else
689
 
      min_range = 300;
690
 
   if (st->min_count > min_range)
691
 
   {
692
 
      st->min_count = 0;
693
 
      for (i=0;i<N;i++)
694
 
      {
695
 
         st->Smin[i] = MIN32(st->Stmp[i], st->S[i]);
696
 
         st->Stmp[i] = st->S[i];
697
 
      }
698
 
   } else {
699
 
      for (i=0;i<N;i++)
700
 
      {
701
 
         st->Smin[i] = MIN32(st->Smin[i], st->S[i]);
702
 
         st->Stmp[i] = MIN32(st->Stmp[i], st->S[i]);      
703
 
      }
704
 
   }
705
 
   for (i=0;i<N;i++)
706
 
   {
707
 
      if (MULT16_32_Q15(QCONST16(.4f,15),st->S[i]) > st->Smin[i])
708
 
         st->update_prob[i] = 1;
709
 
      else
710
 
         st->update_prob[i] = 0;
711
 
      /*fprintf (stderr, "%f ", st->S[i]/st->Smin[i]);*/
712
 
      /*fprintf (stderr, "%f ", st->update_prob[i]);*/
713
 
   }
714
 
 
715
 
}
716
 
 
717
 
#define NOISE_OVERCOMPENS 1.
718
 
 
719
 
void speex_echo_get_residual(SpeexEchoState *st, spx_word32_t *Yout, int len);
720
 
 
721
 
EXPORT int speex_preprocess(SpeexPreprocessState *st, spx_int16_t *x, spx_int32_t *echo)
722
 
{
723
 
   return speex_preprocess_run(st, x);
724
 
}
725
 
 
726
 
EXPORT int speex_preprocess_run(SpeexPreprocessState *st, spx_int16_t *x)
727
 
{
728
 
   int i;
729
 
   int M;
730
 
   int N = st->ps_size;
731
 
   int N3 = 2*N - st->frame_size;
732
 
   int N4 = st->frame_size - N3;
733
 
   spx_word32_t *ps=st->ps;
734
 
   spx_word32_t Zframe;
735
 
   spx_word16_t Pframe;
736
 
   spx_word16_t beta, beta_1;
737
 
   spx_word16_t effective_echo_suppress;
738
 
   
739
 
   st->nb_adapt++;
740
 
   if (st->nb_adapt>20000)
741
 
      st->nb_adapt = 20000;
742
 
   st->min_count++;
743
 
   
744
 
   beta = MAX16(QCONST16(.03,15),DIV32_16(Q15_ONE,st->nb_adapt));
745
 
   beta_1 = Q15_ONE-beta;
746
 
   M = st->nbands;
747
 
   /* Deal with residual echo if provided */
748
 
   if (st->echo_state)
749
 
   {
750
 
      speex_echo_get_residual(st->echo_state, st->residual_echo, N);
751
 
#ifndef FIXED_POINT
752
 
      /* If there are NaNs or ridiculous values, it'll show up in the DC and we just reset everything to zero */
753
 
      if (!(st->residual_echo[0] >=0 && st->residual_echo[0]<N*1e9f))
754
 
      {
755
 
         for (i=0;i<N;i++)
756
 
            st->residual_echo[i] = 0;
757
 
      }
758
 
#endif
759
 
      for (i=0;i<N;i++)
760
 
         st->echo_noise[i] = MAX32(MULT16_32_Q15(QCONST16(.6f,15),st->echo_noise[i]), st->residual_echo[i]);
761
 
      filterbank_compute_bank32(st->bank, st->echo_noise, st->echo_noise+N);
762
 
   } else {
763
 
      for (i=0;i<N+M;i++)
764
 
         st->echo_noise[i] = 0;
765
 
   }
766
 
   preprocess_analysis(st, x);
767
 
 
768
 
   update_noise_prob(st);
769
 
 
770
 
   /* Noise estimation always updated for the 10 first frames */
771
 
   /*if (st->nb_adapt<10)
772
 
   {
773
 
      for (i=1;i<N-1;i++)
774
 
         st->update_prob[i] = 0;
775
 
   }
776
 
   */
777
 
   
778
 
   /* Update the noise estimate for the frequencies where it can be */
779
 
   for (i=0;i<N;i++)
780
 
   {
781
 
      if (!st->update_prob[i] || st->ps[i] < PSHR32(st->noise[i], NOISE_SHIFT))
782
 
         st->noise[i] = MAX32(EXTEND32(0),MULT16_32_Q15(beta_1,st->noise[i]) + MULT16_32_Q15(beta,SHL32(st->ps[i],NOISE_SHIFT)));
783
 
   }
784
 
   filterbank_compute_bank32(st->bank, st->noise, st->noise+N);
785
 
 
786
 
   /* Special case for first frame */
787
 
   if (st->nb_adapt==1)
788
 
      for (i=0;i<N+M;i++)
789
 
         st->old_ps[i] = ps[i];
790
 
 
791
 
   /* Compute a posteriori SNR */
792
 
   for (i=0;i<N+M;i++)
793
 
   {
794
 
      spx_word16_t gamma;
795
 
      
796
 
      /* Total noise estimate including residual echo and reverberation */
797
 
      spx_word32_t tot_noise = ADD32(ADD32(ADD32(EXTEND32(1), PSHR32(st->noise[i],NOISE_SHIFT)) , st->echo_noise[i]) , st->reverb_estimate[i]);
798
 
      
799
 
      /* A posteriori SNR = ps/noise - 1*/
800
 
      st->post[i] = SUB16(DIV32_16_Q8(ps[i],tot_noise), QCONST16(1.f,SNR_SHIFT));
801
 
      st->post[i]=MIN16(st->post[i], QCONST16(100.f,SNR_SHIFT));
802
 
      
803
 
      /* Computing update gamma = .1 + .9*(old/(old+noise))^2 */
804
 
      gamma = QCONST16(.1f,15)+MULT16_16_Q15(QCONST16(.89f,15),SQR16_Q15(DIV32_16_Q15(st->old_ps[i],ADD32(st->old_ps[i],tot_noise))));
805
 
      
806
 
      /* A priori SNR update = gamma*max(0,post) + (1-gamma)*old/noise */
807
 
      st->prior[i] = EXTRACT16(PSHR32(ADD32(MULT16_16(gamma,MAX16(0,st->post[i])), MULT16_16(Q15_ONE-gamma,DIV32_16_Q8(st->old_ps[i],tot_noise))), 15));
808
 
      st->prior[i]=MIN16(st->prior[i], QCONST16(100.f,SNR_SHIFT));
809
 
   }
810
 
 
811
 
   /*print_vec(st->post, N+M, "");*/
812
 
 
813
 
   /* Recursive average of the a priori SNR. A bit smoothed for the psd components */
814
 
   st->zeta[0] = PSHR32(ADD32(MULT16_16(QCONST16(.7f,15),st->zeta[0]), MULT16_16(QCONST16(.3f,15),st->prior[0])),15);
815
 
   for (i=1;i<N-1;i++)
816
 
      st->zeta[i] = PSHR32(ADD32(ADD32(ADD32(MULT16_16(QCONST16(.7f,15),st->zeta[i]), MULT16_16(QCONST16(.15f,15),st->prior[i])),
817
 
                           MULT16_16(QCONST16(.075f,15),st->prior[i-1])), MULT16_16(QCONST16(.075f,15),st->prior[i+1])),15);
818
 
   for (i=N-1;i<N+M;i++)
819
 
      st->zeta[i] = PSHR32(ADD32(MULT16_16(QCONST16(.7f,15),st->zeta[i]), MULT16_16(QCONST16(.3f,15),st->prior[i])),15);
820
 
 
821
 
   /* Speech probability of presence for the entire frame is based on the average filterbank a priori SNR */
822
 
   Zframe = 0;
823
 
   for (i=N;i<N+M;i++)
824
 
      Zframe = ADD32(Zframe, EXTEND32(st->zeta[i]));
825
 
   Pframe = QCONST16(.1f,15)+MULT16_16_Q15(QCONST16(.899f,15),qcurve(DIV32_16(Zframe,st->nbands)));
826
 
   
827
 
   effective_echo_suppress = EXTRACT16(PSHR32(ADD32(MULT16_16(SUB16(Q15_ONE,Pframe), st->echo_suppress), MULT16_16(Pframe, st->echo_suppress_active)),15));
828
 
   
829
 
   compute_gain_floor(st->noise_suppress, effective_echo_suppress, st->noise+N, st->echo_noise+N, st->gain_floor+N, M);
830
 
         
831
 
   /* Compute Ephraim & Malah gain speech probability of presence for each critical band (Bark scale) 
832
 
      Technically this is actually wrong because the EM gaim assumes a slightly different probability 
833
 
      distribution */
834
 
   for (i=N;i<N+M;i++)
835
 
   {
836
 
      /* See EM and Cohen papers*/
837
 
      spx_word32_t theta;
838
 
      /* Gain from hypergeometric function */
839
 
      spx_word32_t MM;
840
 
      /* Weiner filter gain */
841
 
      spx_word16_t prior_ratio;
842
 
      /* a priority probability of speech presence based on Bark sub-band alone */
843
 
      spx_word16_t P1;
844
 
      /* Speech absence a priori probability (considering sub-band and frame) */
845
 
      spx_word16_t q;
846
 
#ifdef FIXED_POINT
847
 
      spx_word16_t tmp;
848
 
#endif
849
 
      
850
 
      prior_ratio = PDIV32_16(SHL32(EXTEND32(st->prior[i]), 15), ADD16(st->prior[i], SHL32(1,SNR_SHIFT)));
851
 
      theta = MULT16_32_P15(prior_ratio, QCONST32(1.f,EXPIN_SHIFT)+SHL32(EXTEND32(st->post[i]),EXPIN_SHIFT-SNR_SHIFT));
852
 
 
853
 
      MM = hypergeom_gain(theta);
854
 
      /* Gain with bound */
855
 
      st->gain[i] = EXTRACT16(MIN32(Q15_ONE, MULT16_32_Q15(prior_ratio, MM)));
856
 
      /* Save old Bark power spectrum */
857
 
      st->old_ps[i] = MULT16_32_P15(QCONST16(.2f,15),st->old_ps[i]) + MULT16_32_P15(MULT16_16_P15(QCONST16(.8f,15),SQR16_Q15(st->gain[i])),ps[i]);
858
 
 
859
 
      P1 = QCONST16(.199f,15)+MULT16_16_Q15(QCONST16(.8f,15),qcurve (st->zeta[i]));
860
 
      q = Q15_ONE-MULT16_16_Q15(Pframe,P1);
861
 
#ifdef FIXED_POINT
862
 
      theta = MIN32(theta, EXTEND32(32767));
863
 
/*Q8*/tmp = MULT16_16_Q15((SHL32(1,SNR_SHIFT)+st->prior[i]),EXTRACT16(MIN32(Q15ONE,SHR32(spx_exp(-EXTRACT16(theta)),1))));
864
 
      tmp = MIN16(QCONST16(3.,SNR_SHIFT), tmp); /* Prevent overflows in the next line*/
865
 
/*Q8*/tmp = EXTRACT16(PSHR32(MULT16_16(PDIV32_16(SHL32(EXTEND32(q),8),(Q15_ONE-q)),tmp),8));
866
 
      st->gain2[i]=DIV32_16(SHL32(EXTEND32(32767),SNR_SHIFT), ADD16(256,tmp));
867
 
#else
868
 
      st->gain2[i]=1/(1.f + (q/(1.f-q))*(1+st->prior[i])*exp(-theta));
869
 
#endif
870
 
   }
871
 
   /* Convert the EM gains and speech prob to linear frequency */
872
 
   filterbank_compute_psd16(st->bank,st->gain2+N, st->gain2);
873
 
   filterbank_compute_psd16(st->bank,st->gain+N, st->gain);
874
 
   
875
 
   /* Use 1 for linear gain resolution (best) or 0 for Bark gain resolution (faster) */
876
 
   if (1)
877
 
   {
878
 
      filterbank_compute_psd16(st->bank,st->gain_floor+N, st->gain_floor);
879
 
   
880
 
      /* Compute gain according to the Ephraim-Malah algorithm -- linear frequency */
881
 
      for (i=0;i<N;i++)
882
 
      {
883
 
         spx_word32_t MM;
884
 
         spx_word32_t theta;
885
 
         spx_word16_t prior_ratio;
886
 
         spx_word16_t tmp;
887
 
         spx_word16_t p;
888
 
         spx_word16_t g;
889
 
         
890
 
         /* Wiener filter gain */
891
 
         prior_ratio = PDIV32_16(SHL32(EXTEND32(st->prior[i]), 15), ADD16(st->prior[i], SHL32(1,SNR_SHIFT)));
892
 
         theta = MULT16_32_P15(prior_ratio, QCONST32(1.f,EXPIN_SHIFT)+SHL32(EXTEND32(st->post[i]),EXPIN_SHIFT-SNR_SHIFT));
893
 
 
894
 
         /* Optimal estimator for loudness domain */
895
 
         MM = hypergeom_gain(theta);
896
 
         /* EM gain with bound */
897
 
         g = EXTRACT16(MIN32(Q15_ONE, MULT16_32_Q15(prior_ratio, MM)));
898
 
         /* Interpolated speech probability of presence */
899
 
         p = st->gain2[i];
900
 
                  
901
 
         /* Constrain the gain to be close to the Bark scale gain */
902
 
         if (MULT16_16_Q15(QCONST16(.333f,15),g) > st->gain[i])
903
 
            g = MULT16_16(3,st->gain[i]);
904
 
         st->gain[i] = g;
905
 
         
906
 
         /* Save old power spectrum */
907
 
         st->old_ps[i] = MULT16_32_P15(QCONST16(.2f,15),st->old_ps[i]) + MULT16_32_P15(MULT16_16_P15(QCONST16(.8f,15),SQR16_Q15(st->gain[i])),ps[i]);
908
 
         
909
 
         /* Apply gain floor */
910
 
         if (st->gain[i] < st->gain_floor[i])
911
 
            st->gain[i] = st->gain_floor[i];
912
 
 
913
 
         /* Exponential decay model for reverberation (unused) */
914
 
         /*st->reverb_estimate[i] = st->reverb_decay*st->reverb_estimate[i] + st->reverb_decay*st->reverb_level*st->gain[i]*st->gain[i]*st->ps[i];*/
915
 
         
916
 
         /* Take into account speech probability of presence (loudness domain MMSE estimator) */
917
 
         /* gain2 = [p*sqrt(gain)+(1-p)*sqrt(gain _floor) ]^2 */
918
 
         tmp = MULT16_16_P15(p,spx_sqrt(SHL32(EXTEND32(st->gain[i]),15))) + MULT16_16_P15(SUB16(Q15_ONE,p),spx_sqrt(SHL32(EXTEND32(st->gain_floor[i]),15)));
919
 
         st->gain2[i]=SQR16_Q15(tmp);
920
 
 
921
 
         /* Use this if you want a log-domain MMSE estimator instead */
922
 
         /*st->gain2[i] = pow(st->gain[i], p) * pow(st->gain_floor[i],1.f-p);*/
923
 
      }
924
 
   } else {
925
 
      for (i=N;i<N+M;i++)
926
 
      {
927
 
         spx_word16_t tmp;
928
 
         spx_word16_t p = st->gain2[i];
929
 
         st->gain[i] = MAX16(st->gain[i], st->gain_floor[i]);         
930
 
         tmp = MULT16_16_P15(p,spx_sqrt(SHL32(EXTEND32(st->gain[i]),15))) + MULT16_16_P15(SUB16(Q15_ONE,p),spx_sqrt(SHL32(EXTEND32(st->gain_floor[i]),15)));
931
 
         st->gain2[i]=SQR16_Q15(tmp);
932
 
      }
933
 
      filterbank_compute_psd16(st->bank,st->gain2+N, st->gain2);
934
 
   }
935
 
   
936
 
   /* If noise suppression is off, don't apply the gain (but then why call this in the first place!) */
937
 
   if (!st->denoise_enabled)
938
 
   {
939
 
      for (i=0;i<N+M;i++)
940
 
         st->gain2[i]=Q15_ONE;
941
 
   }
942
 
      
943
 
   /* Apply computed gain */
944
 
   for (i=1;i<N;i++)
945
 
   {
946
 
      st->ft[2*i-1] = MULT16_16_P15(st->gain2[i],st->ft[2*i-1]);
947
 
      st->ft[2*i] = MULT16_16_P15(st->gain2[i],st->ft[2*i]);
948
 
   }
949
 
   st->ft[0] = MULT16_16_P15(st->gain2[0],st->ft[0]);
950
 
   st->ft[2*N-1] = MULT16_16_P15(st->gain2[N-1],st->ft[2*N-1]);
951
 
   
952
 
   /*FIXME: This *will* not work for fixed-point */
953
 
#ifndef FIXED_POINT
954
 
   if (st->agc_enabled)
955
 
      speex_compute_agc(st, Pframe, st->ft);
956
 
#endif
957
 
 
958
 
   /* Inverse FFT with 1/N scaling */
959
 
   spx_ifft(st->fft_lookup, st->ft, st->frame);
960
 
   /* Scale back to original (lower) amplitude */
961
 
   for (i=0;i<2*N;i++)
962
 
      st->frame[i] = PSHR16(st->frame[i], st->frame_shift);
963
 
 
964
 
   /*FIXME: This *will* not work for fixed-point */
965
 
#ifndef FIXED_POINT
966
 
   if (st->agc_enabled)
967
 
   {
968
 
      float max_sample=0;
969
 
      for (i=0;i<2*N;i++)
970
 
         if (fabs(st->frame[i])>max_sample)
971
 
            max_sample = fabs(st->frame[i]);
972
 
      if (max_sample>28000.f)
973
 
      {
974
 
         float damp = 28000.f/max_sample;
975
 
         for (i=0;i<2*N;i++)
976
 
            st->frame[i] *= damp;
977
 
      }
978
 
   }
979
 
#endif
980
 
   
981
 
   /* Synthesis window (for WOLA) */
982
 
   for (i=0;i<2*N;i++)
983
 
      st->frame[i] = MULT16_16_Q15(st->frame[i], st->window[i]);
984
 
 
985
 
   /* Perform overlap and add */
986
 
   for (i=0;i<N3;i++)
987
 
      x[i] = st->outbuf[i] + st->frame[i];
988
 
   for (i=0;i<N4;i++)
989
 
      x[N3+i] = st->frame[N3+i];
990
 
   
991
 
   /* Update outbuf */
992
 
   for (i=0;i<N3;i++)
993
 
      st->outbuf[i] = st->frame[st->frame_size+i];
994
 
 
995
 
   /* FIXME: This VAD is a kludge */
996
 
   st->speech_prob = Pframe;
997
 
   if (st->vad_enabled)
998
 
   {
999
 
      if (st->speech_prob > st->speech_prob_start || (st->was_speech && st->speech_prob > st->speech_prob_continue))
1000
 
      {
1001
 
         st->was_speech=1;
1002
 
         return 1;
1003
 
      } else
1004
 
      {
1005
 
         st->was_speech=0;
1006
 
         return 0;
1007
 
      }
1008
 
   } else {
1009
 
      return 1;
1010
 
   }
1011
 
}
1012
 
 
1013
 
EXPORT void speex_preprocess_estimate_update(SpeexPreprocessState *st, spx_int16_t *x)
1014
 
{
1015
 
   int i;
1016
 
   int N = st->ps_size;
1017
 
   int N3 = 2*N - st->frame_size;
1018
 
   int M;
1019
 
   spx_word32_t *ps=st->ps;
1020
 
 
1021
 
   M = st->nbands;
1022
 
   st->min_count++;
1023
 
   
1024
 
   preprocess_analysis(st, x);
1025
 
 
1026
 
   update_noise_prob(st);
1027
 
   
1028
 
   for (i=1;i<N-1;i++)
1029
 
   {
1030
 
      if (!st->update_prob[i] || st->ps[i] < PSHR32(st->noise[i],NOISE_SHIFT))
1031
 
      {
1032
 
         st->noise[i] = MULT16_32_Q15(QCONST16(.95f,15),st->noise[i]) + MULT16_32_Q15(QCONST16(.05f,15),SHL32(st->ps[i],NOISE_SHIFT));
1033
 
      }
1034
 
   }
1035
 
 
1036
 
   for (i=0;i<N3;i++)
1037
 
      st->outbuf[i] = MULT16_16_Q15(x[st->frame_size-N3+i],st->window[st->frame_size+i]);
1038
 
 
1039
 
   /* Save old power spectrum */
1040
 
   for (i=0;i<N+M;i++)
1041
 
      st->old_ps[i] = ps[i];
1042
 
 
1043
 
   for (i=0;i<N;i++)
1044
 
      st->reverb_estimate[i] = MULT16_32_Q15(st->reverb_decay, st->reverb_estimate[i]);
1045
 
}
1046
 
 
1047
 
 
1048
 
EXPORT int speex_preprocess_ctl(SpeexPreprocessState *state, int request, void *ptr)
1049
 
{
1050
 
   int i;
1051
 
   SpeexPreprocessState *st;
1052
 
   st=(SpeexPreprocessState*)state;
1053
 
   switch(request)
1054
 
   {
1055
 
   case SPEEX_PREPROCESS_SET_DENOISE:
1056
 
      st->denoise_enabled = (*(spx_int32_t*)ptr);
1057
 
      break;
1058
 
   case SPEEX_PREPROCESS_GET_DENOISE:
1059
 
      (*(spx_int32_t*)ptr) = st->denoise_enabled;
1060
 
      break;
1061
 
#ifndef FIXED_POINT
1062
 
   case SPEEX_PREPROCESS_SET_AGC:
1063
 
      st->agc_enabled = (*(spx_int32_t*)ptr);
1064
 
      break;
1065
 
   case SPEEX_PREPROCESS_GET_AGC:
1066
 
      (*(spx_int32_t*)ptr) = st->agc_enabled;
1067
 
      break;
1068
 
#ifndef DISABLE_FLOAT_API
1069
 
   case SPEEX_PREPROCESS_SET_AGC_LEVEL:
1070
 
      st->agc_level = (*(float*)ptr);
1071
 
      if (st->agc_level<1)
1072
 
         st->agc_level=1;
1073
 
      if (st->agc_level>32768)
1074
 
         st->agc_level=32768;
1075
 
      break;
1076
 
   case SPEEX_PREPROCESS_GET_AGC_LEVEL:
1077
 
      (*(float*)ptr) = st->agc_level;
1078
 
      break;
1079
 
#endif /* #ifndef DISABLE_FLOAT_API */
1080
 
   case SPEEX_PREPROCESS_SET_AGC_INCREMENT:
1081
 
      st->max_increase_step = exp(0.11513f * (*(spx_int32_t*)ptr)*st->frame_size / st->sampling_rate);
1082
 
      break;
1083
 
   case SPEEX_PREPROCESS_GET_AGC_INCREMENT:
1084
 
      (*(spx_int32_t*)ptr) = floor(.5+8.6858*log(st->max_increase_step)*st->sampling_rate/st->frame_size);
1085
 
      break;
1086
 
   case SPEEX_PREPROCESS_SET_AGC_DECREMENT:
1087
 
      st->max_decrease_step = exp(0.11513f * (*(spx_int32_t*)ptr)*st->frame_size / st->sampling_rate);
1088
 
      break;
1089
 
   case SPEEX_PREPROCESS_GET_AGC_DECREMENT:
1090
 
      (*(spx_int32_t*)ptr) = floor(.5+8.6858*log(st->max_decrease_step)*st->sampling_rate/st->frame_size);
1091
 
      break;
1092
 
   case SPEEX_PREPROCESS_SET_AGC_MAX_GAIN:
1093
 
      st->max_gain = exp(0.11513f * (*(spx_int32_t*)ptr));
1094
 
      break;
1095
 
   case SPEEX_PREPROCESS_GET_AGC_MAX_GAIN:
1096
 
      (*(spx_int32_t*)ptr) = floor(.5+8.6858*log(st->max_gain));
1097
 
      break;
1098
 
#endif
1099
 
   case SPEEX_PREPROCESS_SET_VAD:
1100
 
      speex_warning("The VAD has been replaced by a hack pending a complete rewrite");
1101
 
      st->vad_enabled = (*(spx_int32_t*)ptr);
1102
 
      break;
1103
 
   case SPEEX_PREPROCESS_GET_VAD:
1104
 
      (*(spx_int32_t*)ptr) = st->vad_enabled;
1105
 
      break;
1106
 
   
1107
 
   case SPEEX_PREPROCESS_SET_DEREVERB:
1108
 
      st->dereverb_enabled = (*(spx_int32_t*)ptr);
1109
 
      for (i=0;i<st->ps_size;i++)
1110
 
         st->reverb_estimate[i]=0;
1111
 
      break;
1112
 
   case SPEEX_PREPROCESS_GET_DEREVERB:
1113
 
      (*(spx_int32_t*)ptr) = st->dereverb_enabled;
1114
 
      break;
1115
 
 
1116
 
   case SPEEX_PREPROCESS_SET_DEREVERB_LEVEL:
1117
 
      /* FIXME: Re-enable when de-reverberation is actually enabled again */
1118
 
      /*st->reverb_level = (*(float*)ptr);*/
1119
 
      break;
1120
 
   case SPEEX_PREPROCESS_GET_DEREVERB_LEVEL:
1121
 
      /* FIXME: Re-enable when de-reverberation is actually enabled again */
1122
 
      /*(*(float*)ptr) = st->reverb_level;*/
1123
 
      break;
1124
 
   
1125
 
   case SPEEX_PREPROCESS_SET_DEREVERB_DECAY:
1126
 
      /* FIXME: Re-enable when de-reverberation is actually enabled again */
1127
 
      /*st->reverb_decay = (*(float*)ptr);*/
1128
 
      break;
1129
 
   case SPEEX_PREPROCESS_GET_DEREVERB_DECAY:
1130
 
      /* FIXME: Re-enable when de-reverberation is actually enabled again */
1131
 
      /*(*(float*)ptr) = st->reverb_decay;*/
1132
 
      break;
1133
 
 
1134
 
   case SPEEX_PREPROCESS_SET_PROB_START:
1135
 
      *(spx_int32_t*)ptr = MIN32(100,MAX32(0, *(spx_int32_t*)ptr));
1136
 
      st->speech_prob_start = DIV32_16(MULT16_16(Q15ONE,*(spx_int32_t*)ptr), 100);
1137
 
      break;
1138
 
   case SPEEX_PREPROCESS_GET_PROB_START:
1139
 
      (*(spx_int32_t*)ptr) = MULT16_16_Q15(st->speech_prob_start, 100);
1140
 
      break;
1141
 
 
1142
 
   case SPEEX_PREPROCESS_SET_PROB_CONTINUE:
1143
 
      *(spx_int32_t*)ptr = MIN32(100,MAX32(0, *(spx_int32_t*)ptr));
1144
 
      st->speech_prob_continue = DIV32_16(MULT16_16(Q15ONE,*(spx_int32_t*)ptr), 100);
1145
 
      break;
1146
 
   case SPEEX_PREPROCESS_GET_PROB_CONTINUE:
1147
 
      (*(spx_int32_t*)ptr) = MULT16_16_Q15(st->speech_prob_continue, 100);
1148
 
      break;
1149
 
 
1150
 
   case SPEEX_PREPROCESS_SET_NOISE_SUPPRESS:
1151
 
      st->noise_suppress = -ABS(*(spx_int32_t*)ptr);
1152
 
      break;
1153
 
   case SPEEX_PREPROCESS_GET_NOISE_SUPPRESS:
1154
 
      (*(spx_int32_t*)ptr) = st->noise_suppress;
1155
 
      break;
1156
 
   case SPEEX_PREPROCESS_SET_ECHO_SUPPRESS:
1157
 
      st->echo_suppress = -ABS(*(spx_int32_t*)ptr);
1158
 
      break;
1159
 
   case SPEEX_PREPROCESS_GET_ECHO_SUPPRESS:
1160
 
      (*(spx_int32_t*)ptr) = st->echo_suppress;
1161
 
      break;
1162
 
   case SPEEX_PREPROCESS_SET_ECHO_SUPPRESS_ACTIVE:
1163
 
      st->echo_suppress_active = -ABS(*(spx_int32_t*)ptr);
1164
 
      break;
1165
 
   case SPEEX_PREPROCESS_GET_ECHO_SUPPRESS_ACTIVE:
1166
 
      (*(spx_int32_t*)ptr) = st->echo_suppress_active;
1167
 
      break;
1168
 
   case SPEEX_PREPROCESS_SET_ECHO_STATE:
1169
 
      st->echo_state = (SpeexEchoState*)ptr;
1170
 
      break;
1171
 
   case SPEEX_PREPROCESS_GET_ECHO_STATE:
1172
 
      (*(SpeexEchoState**)ptr) = (SpeexEchoState*)st->echo_state;
1173
 
      break;
1174
 
#ifndef FIXED_POINT
1175
 
   case SPEEX_PREPROCESS_GET_AGC_LOUDNESS:
1176
 
      (*(spx_int32_t*)ptr) = pow(st->loudness, 1.0/LOUDNESS_EXP);
1177
 
      break;
1178
 
   case SPEEX_PREPROCESS_GET_AGC_GAIN:
1179
 
      (*(spx_int32_t*)ptr) = floor(.5+8.6858*log(st->agc_gain));
1180
 
      break;
1181
 
#endif
1182
 
   case SPEEX_PREPROCESS_GET_PSD_SIZE:
1183
 
   case SPEEX_PREPROCESS_GET_NOISE_PSD_SIZE:
1184
 
      (*(spx_int32_t*)ptr) = st->ps_size;
1185
 
      break;
1186
 
   case SPEEX_PREPROCESS_GET_PSD:
1187
 
      for(i=0;i<st->ps_size;i++)
1188
 
        ((spx_int32_t *)ptr)[i] = (spx_int32_t) st->ps[i];
1189
 
      break;
1190
 
   case SPEEX_PREPROCESS_GET_NOISE_PSD:
1191
 
      for(i=0;i<st->ps_size;i++)
1192
 
        ((spx_int32_t *)ptr)[i] = (spx_int32_t) PSHR32(st->noise[i], NOISE_SHIFT);
1193
 
      break;
1194
 
   case SPEEX_PREPROCESS_GET_PROB:
1195
 
      (*(spx_int32_t*)ptr) = MULT16_16_Q15(st->speech_prob, 100);
1196
 
      break;
1197
 
#ifndef FIXED_POINT
1198
 
   case SPEEX_PREPROCESS_SET_AGC_TARGET:
1199
 
      st->agc_level = (*(spx_int32_t*)ptr);
1200
 
      if (st->agc_level<1)
1201
 
         st->agc_level=1;
1202
 
      if (st->agc_level>32768)
1203
 
         st->agc_level=32768;
1204
 
      break;
1205
 
   case SPEEX_PREPROCESS_GET_AGC_TARGET:
1206
 
      (*(spx_int32_t*)ptr) = st->agc_level;
1207
 
      break;
1208
 
#endif
1209
 
   default:
1210
 
      speex_warning_int("Unknown speex_preprocess_ctl request: ", request);
1211
 
      return -1;
1212
 
   }
1213
 
   return 0;
1214
 
}
1215
 
 
1216
 
#ifdef FIXED_DEBUG
1217
 
long long spx_mips=0;
1218
 
#endif
1219