← 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/filters.c

  • Committer: Package Import Robot
  • Author(s): Francois Marier, Francois Marier, Mark Purcell
  • Date: 2014-10-18 15:08:50 UTC
  • mfrom: (1.1.12)
  • mto: This revision was merged to the branch mainline in revision 29.
  • Revision ID: package-import@ubuntu.com-20141018150850-2exfk34ckb15pcwi
Tags: 1.4.1-0.1
http://bugs.debian.org/759576
http://bugs.debian.org/741130
[ Francois Marier ]
* Non-maintainer upload
* New upstream release (closes: #759576, #741130)
  - debian/rules +PJPROJECT_VERSION := 2.2.1
  - add upstream patch to fix broken TLS support
  - add patch to fix pjproject regression

[ Mark Purcell ]
* Build-Depends:
  - sflphone-daemon + libavformat-dev, libavcodec-dev, libswscale-dev,
  libavdevice-dev, libavutil-dev
  - sflphone-gnome + libclutter-gtk-1.0-dev

Show diffs side-by-side

added added

removed removed

Lines of Context:
daemon/libs/pjproject-2.1.0/third_party/speex/libspeex/filters.c
1
 
/* Copyright (C) 2002-2006 Jean-Marc Valin 
2
 
   File: filters.c
3
 
   Various analysis/synthesis filters
4
 
 
5
 
   Redistribution and use in source and binary forms, with or without
6
 
   modification, are permitted provided that the following conditions
7
 
   are met:
8
 
   
9
 
   - Redistributions of source code must retain the above copyright
10
 
   notice, this list of conditions and the following disclaimer.
11
 
   
12
 
   - Redistributions in binary form must reproduce the above copyright
13
 
   notice, this list of conditions and the following disclaimer in the
14
 
   documentation and/or other materials provided with the distribution.
15
 
   
16
 
   - Neither the name of the Xiph.org Foundation nor the names of its
17
 
   contributors may be used to endorse or promote products derived from
18
 
   this software without specific prior written permission.
19
 
   
20
 
   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21
 
   ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22
 
   LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
23
 
   A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
24
 
   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
25
 
   EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
26
 
   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
27
 
   PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
28
 
   LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
29
 
   NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
30
 
   SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31
 
*/
32
 
 
33
 
#ifdef HAVE_CONFIG_H
34
 
#include "config.h"
35
 
#endif
36
 
 
37
 
#include "filters.h"
38
 
#include "stack_alloc.h"
39
 
#include "arch.h"
40
 
#include "math_approx.h"
41
 
#include "ltp.h"
42
 
#include <math.h>
43
 
 
44
 
#ifdef _USE_SSE
45
 
#include "filters_sse.h"
46
 
#elif defined (ARM4_ASM) || defined(ARM5E_ASM)
47
 
#include "filters_arm4.h"
48
 
#elif defined (BFIN_ASM)
49
 
#include "filters_bfin.h"
50
 
#endif
51
 
 
52
 
 
53
 
 
54
 
void bw_lpc(spx_word16_t gamma, const spx_coef_t *lpc_in, spx_coef_t *lpc_out, int order)
55
 
{
56
 
   int i;
57
 
   spx_word16_t tmp=gamma;
58
 
   for (i=0;i<order;i++)
59
 
   {
60
 
      lpc_out[i] = MULT16_16_P15(tmp,lpc_in[i]);
61
 
      tmp = MULT16_16_P15(tmp, gamma);
62
 
   }
63
 
}
64
 
 
65
 
void sanitize_values32(spx_word32_t *vec, spx_word32_t min_val, spx_word32_t max_val, int len)
66
 
{
67
 
   int i;
68
 
   for (i=0;i<len;i++)
69
 
   {
70
 
      /* It's important we do the test that way so we can catch NaNs, which are neither greater nor smaller */
71
 
      if (!(vec[i]>=min_val && vec[i] <= max_val))
72
 
      {
73
 
         if (vec[i] < min_val)
74
 
            vec[i] = min_val;
75
 
         else if (vec[i] > max_val)
76
 
            vec[i] = max_val;
77
 
         else /* Has to be NaN */
78
 
            vec[i] = 0;
79
 
      }
80
 
   }
81
 
}
82
 
 
83
 
void highpass(const spx_word16_t *x, spx_word16_t *y, int len, int filtID, spx_mem_t *mem)
84
 
{
85
 
   int i;
86
 
#ifdef FIXED_POINT
87
 
   const spx_word16_t Pcoef[5][3] = {{16384, -31313, 14991}, {16384, -31569, 15249}, {16384, -31677, 15328}, {16384, -32313, 15947}, {16384, -22446, 6537}};
88
 
   const spx_word16_t Zcoef[5][3] = {{15672, -31344, 15672}, {15802, -31601, 15802}, {15847, -31694, 15847}, {16162, -32322, 16162}, {14418, -28836, 14418}};
89
 
#else
90
 
   const spx_word16_t Pcoef[5][3] = {{1.00000f, -1.91120f, 0.91498f}, {1.00000f, -1.92683f, 0.93071f}, {1.00000f, -1.93338f, 0.93553f}, {1.00000f, -1.97226f, 0.97332f}, {1.00000f, -1.37000f, 0.39900f}};
91
 
   const spx_word16_t Zcoef[5][3] = {{0.95654f, -1.91309f, 0.95654f}, {0.96446f, -1.92879f, 0.96446f}, {0.96723f, -1.93445f, 0.96723f}, {0.98645f, -1.97277f, 0.98645f}, {0.88000f, -1.76000f, 0.88000f}};
92
 
#endif
93
 
   const spx_word16_t *den, *num;
94
 
   if (filtID>4)
95
 
      filtID=4;
96
 
   
97
 
   den = Pcoef[filtID]; num = Zcoef[filtID];
98
 
   /*return;*/
99
 
   for (i=0;i<len;i++)
100
 
   {
101
 
      spx_word16_t yi;
102
 
      spx_word32_t vout = ADD32(MULT16_16(num[0], x[i]),mem[0]);
103
 
      yi = EXTRACT16(SATURATE(PSHR32(vout,14),32767));
104
 
      mem[0] = ADD32(MAC16_16(mem[1], num[1],x[i]), SHL32(MULT16_32_Q15(-den[1],vout),1));
105
 
      mem[1] = ADD32(MULT16_16(num[2],x[i]), SHL32(MULT16_32_Q15(-den[2],vout),1));
106
 
      y[i] = yi;
107
 
   }
108
 
}
109
 
 
110
 
#ifdef FIXED_POINT
111
 
 
112
 
/* FIXME: These functions are ugly and probably introduce too much error */
113
 
void signal_mul(const spx_sig_t *x, spx_sig_t *y, spx_word32_t scale, int len)
114
 
{
115
 
   int i;
116
 
   for (i=0;i<len;i++)
117
 
   {
118
 
      y[i] = SHL32(MULT16_32_Q14(EXTRACT16(SHR32(x[i],7)),scale),7);
119
 
   }
120
 
}
121
 
 
122
 
void signal_div(const spx_word16_t *x, spx_word16_t *y, spx_word32_t scale, int len)
123
 
{
124
 
   int i;
125
 
   if (scale > SHL32(EXTEND32(SIG_SCALING), 8))
126
 
   {
127
 
      spx_word16_t scale_1;
128
 
      scale = PSHR32(scale, SIG_SHIFT);
129
 
      scale_1 = EXTRACT16(PDIV32_16(SHL32(EXTEND32(SIG_SCALING),7),scale));
130
 
      for (i=0;i<len;i++)
131
 
      {
132
 
         y[i] = MULT16_16_P15(scale_1, x[i]);
133
 
      }
134
 
   } else if (scale > SHR32(EXTEND32(SIG_SCALING), 2)) {
135
 
      spx_word16_t scale_1;
136
 
      scale = PSHR32(scale, SIG_SHIFT-5);
137
 
      scale_1 = DIV32_16(SHL32(EXTEND32(SIG_SCALING),3),scale);
138
 
      for (i=0;i<len;i++)
139
 
      {
140
 
         y[i] = PSHR32(MULT16_16(scale_1, SHL16(x[i],2)),8);
141
 
      }
142
 
   } else {
143
 
      spx_word16_t scale_1;
144
 
      scale = PSHR32(scale, SIG_SHIFT-7);
145
 
      if (scale < 5)
146
 
         scale = 5;
147
 
      scale_1 = DIV32_16(SHL32(EXTEND32(SIG_SCALING),3),scale);
148
 
      for (i=0;i<len;i++)
149
 
      {
150
 
         y[i] = PSHR32(MULT16_16(scale_1, SHL16(x[i],2)),6);
151
 
      }
152
 
   }
153
 
}
154
 
 
155
 
#else
156
 
 
157
 
void signal_mul(const spx_sig_t *x, spx_sig_t *y, spx_word32_t scale, int len)
158
 
{
159
 
   int i;
160
 
   for (i=0;i<len;i++)
161
 
      y[i] = scale*x[i];
162
 
}
163
 
 
164
 
void signal_div(const spx_sig_t *x, spx_sig_t *y, spx_word32_t scale, int len)
165
 
{
166
 
   int i;
167
 
   float scale_1 = 1/scale;
168
 
   for (i=0;i<len;i++)
169
 
      y[i] = scale_1*x[i];
170
 
}
171
 
#endif
172
 
 
173
 
 
174
 
 
175
 
#ifdef FIXED_POINT
176
 
 
177
 
 
178
 
 
179
 
spx_word16_t compute_rms(const spx_sig_t *x, int len)
180
 
{
181
 
   int i;
182
 
   spx_word32_t sum=0;
183
 
   spx_sig_t max_val=1;
184
 
   int sig_shift;
185
 
 
186
 
   for (i=0;i<len;i++)
187
 
   {
188
 
      spx_sig_t tmp = x[i];
189
 
      if (tmp<0)
190
 
         tmp = -tmp;
191
 
      if (tmp > max_val)
192
 
         max_val = tmp;
193
 
   }
194
 
 
195
 
   sig_shift=0;
196
 
   while (max_val>16383)
197
 
   {
198
 
      sig_shift++;
199
 
      max_val >>= 1;
200
 
   }
201
 
 
202
 
   for (i=0;i<len;i+=4)
203
 
   {
204
 
      spx_word32_t sum2=0;
205
 
      spx_word16_t tmp;
206
 
      tmp = EXTRACT16(SHR32(x[i],sig_shift));
207
 
      sum2 = MAC16_16(sum2,tmp,tmp);
208
 
      tmp = EXTRACT16(SHR32(x[i+1],sig_shift));
209
 
      sum2 = MAC16_16(sum2,tmp,tmp);
210
 
      tmp = EXTRACT16(SHR32(x[i+2],sig_shift));
211
 
      sum2 = MAC16_16(sum2,tmp,tmp);
212
 
      tmp = EXTRACT16(SHR32(x[i+3],sig_shift));
213
 
      sum2 = MAC16_16(sum2,tmp,tmp);
214
 
      sum = ADD32(sum,SHR32(sum2,6));
215
 
   }
216
 
   
217
 
   return EXTRACT16(PSHR32(SHL32(EXTEND32(spx_sqrt(DIV32(sum,len))),(sig_shift+3)),SIG_SHIFT));
218
 
}
219
 
 
220
 
spx_word16_t compute_rms16(const spx_word16_t *x, int len)
221
 
{
222
 
   int i;
223
 
   spx_word16_t max_val=10; 
224
 
 
225
 
   for (i=0;i<len;i++)
226
 
   {
227
 
      spx_sig_t tmp = x[i];
228
 
      if (tmp<0)
229
 
         tmp = -tmp;
230
 
      if (tmp > max_val)
231
 
         max_val = tmp;
232
 
   }
233
 
   if (max_val>16383)
234
 
   {
235
 
      spx_word32_t sum=0;
236
 
      for (i=0;i<len;i+=4)
237
 
      {
238
 
         spx_word32_t sum2=0;
239
 
         sum2 = MAC16_16(sum2,SHR16(x[i],1),SHR16(x[i],1));
240
 
         sum2 = MAC16_16(sum2,SHR16(x[i+1],1),SHR16(x[i+1],1));
241
 
         sum2 = MAC16_16(sum2,SHR16(x[i+2],1),SHR16(x[i+2],1));
242
 
         sum2 = MAC16_16(sum2,SHR16(x[i+3],1),SHR16(x[i+3],1));
243
 
         sum = ADD32(sum,SHR32(sum2,6));
244
 
      }
245
 
      return SHL16(spx_sqrt(DIV32(sum,len)),4);
246
 
   } else {
247
 
      spx_word32_t sum=0;
248
 
      int sig_shift=0;
249
 
      if (max_val < 8192)
250
 
         sig_shift=1;
251
 
      if (max_val < 4096)
252
 
         sig_shift=2;
253
 
      if (max_val < 2048)
254
 
         sig_shift=3;
255
 
      for (i=0;i<len;i+=4)
256
 
      {
257
 
         spx_word32_t sum2=0;
258
 
         sum2 = MAC16_16(sum2,SHL16(x[i],sig_shift),SHL16(x[i],sig_shift));
259
 
         sum2 = MAC16_16(sum2,SHL16(x[i+1],sig_shift),SHL16(x[i+1],sig_shift));
260
 
         sum2 = MAC16_16(sum2,SHL16(x[i+2],sig_shift),SHL16(x[i+2],sig_shift));
261
 
         sum2 = MAC16_16(sum2,SHL16(x[i+3],sig_shift),SHL16(x[i+3],sig_shift));
262
 
         sum = ADD32(sum,SHR32(sum2,6));
263
 
      }
264
 
      return SHL16(spx_sqrt(DIV32(sum,len)),3-sig_shift);   
265
 
   }
266
 
}
267
 
 
268
 
#ifndef OVERRIDE_NORMALIZE16
269
 
int normalize16(const spx_sig_t *x, spx_word16_t *y, spx_sig_t max_scale, int len)
270
 
{
271
 
   int i;
272
 
   spx_sig_t max_val=1;
273
 
   int sig_shift;
274
 
   
275
 
   for (i=0;i<len;i++)
276
 
   {
277
 
      spx_sig_t tmp = x[i];
278
 
      if (tmp<0)
279
 
         tmp = NEG32(tmp);
280
 
      if (tmp >= max_val)
281
 
         max_val = tmp;
282
 
   }
283
 
 
284
 
   sig_shift=0;
285
 
   while (max_val>max_scale)
286
 
   {
287
 
      sig_shift++;
288
 
      max_val >>= 1;
289
 
   }
290
 
 
291
 
   for (i=0;i<len;i++)
292
 
      y[i] = EXTRACT16(SHR32(x[i], sig_shift));
293
 
   
294
 
   return sig_shift;
295
 
}
296
 
#endif
297
 
 
298
 
#else
299
 
 
300
 
spx_word16_t compute_rms(const spx_sig_t *x, int len)
301
 
{
302
 
   int i;
303
 
   float sum=0;
304
 
   for (i=0;i<len;i++)
305
 
   {
306
 
      sum += x[i]*x[i];
307
 
   }
308
 
   return sqrt(.1+sum/len);
309
 
}
310
 
spx_word16_t compute_rms16(const spx_word16_t *x, int len)
311
 
{
312
 
   return compute_rms(x, len);
313
 
}
314
 
#endif
315
 
 
316
 
 
317
 
 
318
 
#ifndef OVERRIDE_FILTER_MEM16
319
 
void filter_mem16(const spx_word16_t *x, const spx_coef_t *num, const spx_coef_t *den, spx_word16_t *y, int N, int ord, spx_mem_t *mem, char *stack)
320
 
{
321
 
   int i,j;
322
 
   spx_word16_t xi,yi,nyi;
323
 
   for (i=0;i<N;i++)
324
 
   {
325
 
      xi= x[i];
326
 
      yi = EXTRACT16(SATURATE(ADD32(EXTEND32(x[i]),PSHR32(mem[0],LPC_SHIFT)),32767));
327
 
      nyi = NEG16(yi);
328
 
      for (j=0;j<ord-1;j++)
329
 
      {
330
 
         mem[j] = MAC16_16(MAC16_16(mem[j+1], num[j],xi), den[j],nyi);
331
 
      }
332
 
      mem[ord-1] = ADD32(MULT16_16(num[ord-1],xi), MULT16_16(den[ord-1],nyi));
333
 
      y[i] = yi;
334
 
   }
335
 
}
336
 
#endif
337
 
 
338
 
#ifndef OVERRIDE_IIR_MEM16
339
 
void iir_mem16(const spx_word16_t *x, const spx_coef_t *den, spx_word16_t *y, int N, int ord, spx_mem_t *mem, char *stack)
340
 
{
341
 
   int i,j;
342
 
   spx_word16_t yi,nyi;
343
 
 
344
 
   for (i=0;i<N;i++)
345
 
   {
346
 
      yi = EXTRACT16(SATURATE(ADD32(EXTEND32(x[i]),PSHR32(mem[0],LPC_SHIFT)),32767));
347
 
      nyi = NEG16(yi);
348
 
      for (j=0;j<ord-1;j++)
349
 
      {
350
 
         mem[j] = MAC16_16(mem[j+1],den[j],nyi);
351
 
      }
352
 
      mem[ord-1] = MULT16_16(den[ord-1],nyi);
353
 
      y[i] = yi;
354
 
   }
355
 
}
356
 
#endif
357
 
 
358
 
#ifndef OVERRIDE_FIR_MEM16
359
 
void fir_mem16(const spx_word16_t *x, const spx_coef_t *num, spx_word16_t *y, int N, int ord, spx_mem_t *mem, char *stack)
360
 
{
361
 
   int i,j;
362
 
   spx_word16_t xi,yi;
363
 
 
364
 
   for (i=0;i<N;i++)
365
 
   {
366
 
      xi=x[i];
367
 
      yi = EXTRACT16(SATURATE(ADD32(EXTEND32(x[i]),PSHR32(mem[0],LPC_SHIFT)),32767));
368
 
      for (j=0;j<ord-1;j++)
369
 
      {
370
 
         mem[j] = MAC16_16(mem[j+1], num[j],xi);
371
 
      }
372
 
      mem[ord-1] = MULT16_16(num[ord-1],xi);
373
 
      y[i] = yi;
374
 
   }
375
 
}
376
 
#endif
377
 
 
378
 
 
379
 
void syn_percep_zero16(const spx_word16_t *xx, const spx_coef_t *ak, const spx_coef_t *awk1, const spx_coef_t *awk2, spx_word16_t *y, int N, int ord, char *stack)
380
 
{
381
 
   int i;
382
 
   VARDECL(spx_mem_t *mem);
383
 
   ALLOC(mem, ord, spx_mem_t);
384
 
   for (i=0;i<ord;i++)
385
 
      mem[i]=0;
386
 
   iir_mem16(xx, ak, y, N, ord, mem, stack);
387
 
   for (i=0;i<ord;i++)
388
 
      mem[i]=0;
389
 
   filter_mem16(y, awk1, awk2, y, N, ord, mem, stack);
390
 
}
391
 
void residue_percep_zero16(const spx_word16_t *xx, const spx_coef_t *ak, const spx_coef_t *awk1, const spx_coef_t *awk2, spx_word16_t *y, int N, int ord, char *stack)
392
 
{
393
 
   int i;
394
 
   VARDECL(spx_mem_t *mem);
395
 
   ALLOC(mem, ord, spx_mem_t);
396
 
   for (i=0;i<ord;i++)
397
 
      mem[i]=0;
398
 
   filter_mem16(xx, ak, awk1, y, N, ord, mem, stack);
399
 
   for (i=0;i<ord;i++)
400
 
      mem[i]=0;
401
 
   fir_mem16(y, awk2, y, N, ord, mem, stack);
402
 
}
403
 
 
404
 
 
405
 
#ifndef OVERRIDE_COMPUTE_IMPULSE_RESPONSE
406
 
void compute_impulse_response(const spx_coef_t *ak, const spx_coef_t *awk1, const spx_coef_t *awk2, spx_word16_t *y, int N, int ord, char *stack)
407
 
{
408
 
   int i,j;
409
 
   spx_word16_t y1, ny1i, ny2i;
410
 
   VARDECL(spx_mem_t *mem1);
411
 
   VARDECL(spx_mem_t *mem2);
412
 
   ALLOC(mem1, ord, spx_mem_t);
413
 
   ALLOC(mem2, ord, spx_mem_t);
414
 
   
415
 
   y[0] = LPC_SCALING;
416
 
   for (i=0;i<ord;i++)
417
 
      y[i+1] = awk1[i];
418
 
   i++;
419
 
   for (;i<N;i++)
420
 
      y[i] = VERY_SMALL;
421
 
   for (i=0;i<ord;i++)
422
 
      mem1[i] = mem2[i] = 0;
423
 
   for (i=0;i<N;i++)
424
 
   {
425
 
      y1 = ADD16(y[i], EXTRACT16(PSHR32(mem1[0],LPC_SHIFT)));
426
 
      ny1i = NEG16(y1);
427
 
      y[i] = PSHR32(ADD32(SHL32(EXTEND32(y1),LPC_SHIFT+1),mem2[0]),LPC_SHIFT);
428
 
      ny2i = NEG16(y[i]);
429
 
      for (j=0;j<ord-1;j++)
430
 
      {
431
 
         mem1[j] = MAC16_16(mem1[j+1], awk2[j],ny1i);
432
 
         mem2[j] = MAC16_16(mem2[j+1], ak[j],ny2i);
433
 
      }
434
 
      mem1[ord-1] = MULT16_16(awk2[ord-1],ny1i);
435
 
      mem2[ord-1] = MULT16_16(ak[ord-1],ny2i);
436
 
   }
437
 
}
438
 
#endif
439
 
 
440
 
/* Decomposes a signal into low-band and high-band using a QMF */
441
 
void qmf_decomp(const spx_word16_t *xx, const spx_word16_t *aa, spx_word16_t *y1, spx_word16_t *y2, int N, int M, spx_word16_t *mem, char *stack)
442
 
{
443
 
   int i,j,k,M2;
444
 
   VARDECL(spx_word16_t *a);
445
 
   VARDECL(spx_word16_t *x);
446
 
   spx_word16_t *x2;
447
 
   
448
 
   ALLOC(a, M, spx_word16_t);
449
 
   ALLOC(x, N+M-1, spx_word16_t);
450
 
   x2=x+M-1;
451
 
   M2=M>>1;
452
 
   for (i=0;i<M;i++)
453
 
      a[M-i-1]= aa[i];
454
 
   for (i=0;i<M-1;i++)
455
 
      x[i]=mem[M-i-2];
456
 
   for (i=0;i<N;i++)
457
 
      x[i+M-1]=SHR16(xx[i],1);
458
 
   for (i=0;i<M-1;i++)
459
 
      mem[i]=SHR16(xx[N-i-1],1);
460
 
   for (i=0,k=0;i<N;i+=2,k++)
461
 
   {
462
 
      spx_word32_t y1k=0, y2k=0;
463
 
      for (j=0;j<M2;j++)
464
 
      {
465
 
         y1k=ADD32(y1k,MULT16_16(a[j],ADD16(x[i+j],x2[i-j])));
466
 
         y2k=SUB32(y2k,MULT16_16(a[j],SUB16(x[i+j],x2[i-j])));
467
 
         j++;
468
 
         y1k=ADD32(y1k,MULT16_16(a[j],ADD16(x[i+j],x2[i-j])));
469
 
         y2k=ADD32(y2k,MULT16_16(a[j],SUB16(x[i+j],x2[i-j])));
470
 
      }
471
 
      y1[k] = EXTRACT16(SATURATE(PSHR32(y1k,15),32767));
472
 
      y2[k] = EXTRACT16(SATURATE(PSHR32(y2k,15),32767));
473
 
   }
474
 
}
475
 
 
476
 
/* Re-synthesised a signal from the QMF low-band and high-band signals */
477
 
void qmf_synth(const spx_word16_t *x1, const spx_word16_t *x2, const spx_word16_t *a, spx_word16_t *y, int N, int M, spx_word16_t *mem1, spx_word16_t *mem2, char *stack)
478
 
   /* assumptions:
479
 
      all odd x[i] are zero -- well, actually they are left out of the array now
480
 
      N and M are multiples of 4 */
481
 
{
482
 
   int i, j;
483
 
   int M2, N2;
484
 
   VARDECL(spx_word16_t *xx1);
485
 
   VARDECL(spx_word16_t *xx2);
486
 
   
487
 
   M2 = M>>1;
488
 
   N2 = N>>1;
489
 
   ALLOC(xx1, M2+N2, spx_word16_t);
490
 
   ALLOC(xx2, M2+N2, spx_word16_t);
491
 
 
492
 
   for (i = 0; i < N2; i++)
493
 
      xx1[i] = x1[N2-1-i];
494
 
   for (i = 0; i < M2; i++)
495
 
      xx1[N2+i] = mem1[2*i+1];
496
 
   for (i = 0; i < N2; i++)
497
 
      xx2[i] = x2[N2-1-i];
498
 
   for (i = 0; i < M2; i++)
499
 
      xx2[N2+i] = mem2[2*i+1];
500
 
 
501
 
   for (i = 0; i < N2; i += 2) {
502
 
      spx_sig_t y0, y1, y2, y3;
503
 
      spx_word16_t x10, x20;
504
 
 
505
 
      y0 = y1 = y2 = y3 = 0;
506
 
      x10 = xx1[N2-2-i];
507
 
      x20 = xx2[N2-2-i];
508
 
 
509
 
      for (j = 0; j < M2; j += 2) {
510
 
         spx_word16_t x11, x21;
511
 
         spx_word16_t a0, a1;
512
 
 
513
 
         a0 = a[2*j];
514
 
         a1 = a[2*j+1];
515
 
         x11 = xx1[N2-1+j-i];
516
 
         x21 = xx2[N2-1+j-i];
517
 
 
518
 
#ifdef FIXED_POINT
519
 
         /* We multiply twice by the same coef to avoid overflows */
520
 
         y0 = MAC16_16(MAC16_16(y0, a0, x11), NEG16(a0), x21);
521
 
         y1 = MAC16_16(MAC16_16(y1, a1, x11), a1, x21);
522
 
         y2 = MAC16_16(MAC16_16(y2, a0, x10), NEG16(a0), x20);
523
 
         y3 = MAC16_16(MAC16_16(y3, a1, x10), a1, x20);
524
 
#else
525
 
         y0 = ADD32(y0,MULT16_16(a0, x11-x21));
526
 
         y1 = ADD32(y1,MULT16_16(a1, x11+x21));
527
 
         y2 = ADD32(y2,MULT16_16(a0, x10-x20));
528
 
         y3 = ADD32(y3,MULT16_16(a1, x10+x20));
529
 
#endif
530
 
         a0 = a[2*j+2];
531
 
         a1 = a[2*j+3];
532
 
         x10 = xx1[N2+j-i];
533
 
         x20 = xx2[N2+j-i];
534
 
 
535
 
#ifdef FIXED_POINT
536
 
         /* We multiply twice by the same coef to avoid overflows */
537
 
         y0 = MAC16_16(MAC16_16(y0, a0, x10), NEG16(a0), x20);
538
 
         y1 = MAC16_16(MAC16_16(y1, a1, x10), a1, x20);
539
 
         y2 = MAC16_16(MAC16_16(y2, a0, x11), NEG16(a0), x21);
540
 
         y3 = MAC16_16(MAC16_16(y3, a1, x11), a1, x21);
541
 
#else
542
 
         y0 = ADD32(y0,MULT16_16(a0, x10-x20));
543
 
         y1 = ADD32(y1,MULT16_16(a1, x10+x20));
544
 
         y2 = ADD32(y2,MULT16_16(a0, x11-x21));
545
 
         y3 = ADD32(y3,MULT16_16(a1, x11+x21));
546
 
#endif
547
 
      }
548
 
#ifdef FIXED_POINT
549
 
      y[2*i] = EXTRACT16(SATURATE32(PSHR32(y0,15),32767));
550
 
      y[2*i+1] = EXTRACT16(SATURATE32(PSHR32(y1,15),32767));
551
 
      y[2*i+2] = EXTRACT16(SATURATE32(PSHR32(y2,15),32767));
552
 
      y[2*i+3] = EXTRACT16(SATURATE32(PSHR32(y3,15),32767));
553
 
#else
554
 
      /* Normalize up explicitly if we're in float */
555
 
      y[2*i] = 2.f*y0;
556
 
      y[2*i+1] = 2.f*y1;
557
 
      y[2*i+2] = 2.f*y2;
558
 
      y[2*i+3] = 2.f*y3;
559
 
#endif
560
 
   }
561
 
 
562
 
   for (i = 0; i < M2; i++)
563
 
      mem1[2*i+1] = xx1[i];
564
 
   for (i = 0; i < M2; i++)
565
 
      mem2[2*i+1] = xx2[i];
566
 
}
567
 
 
568
 
#ifdef FIXED_POINT
569
 
#if 0
570
 
const spx_word16_t shift_filt[3][7] = {{-33,    1043,   -4551,   19959,   19959,   -4551,    1043},
571
 
                                 {-98,    1133,   -4425,   29179,    8895,   -2328,     444},
572
 
                                 {444,   -2328,    8895,   29179,   -4425,    1133,     -98}};
573
 
#else
574
 
const spx_word16_t shift_filt[3][7] = {{-390,    1540,   -4993,   20123,   20123,   -4993,    1540},
575
 
                                {-1064,    2817,   -6694,   31589,    6837,    -990,    -209},
576
 
                                 {-209,    -990,    6837,   31589,   -6694,    2817,   -1064}};
577
 
#endif
578
 
#else
579
 
#if 0
580
 
const float shift_filt[3][7] = {{-9.9369e-04, 3.1831e-02, -1.3889e-01, 6.0910e-01, 6.0910e-01, -1.3889e-01, 3.1831e-02},
581
 
                          {-0.0029937, 0.0345613, -0.1350474, 0.8904793, 0.2714479, -0.0710304, 0.0135403},
582
 
                          {0.0135403, -0.0710304, 0.2714479, 0.8904793, -0.1350474, 0.0345613,  -0.0029937}};
583
 
#else
584
 
const float shift_filt[3][7] = {{-0.011915f, 0.046995f, -0.152373f, 0.614108f, 0.614108f, -0.152373f, 0.046995f},
585
 
                          {-0.0324855f, 0.0859768f, -0.2042986f, 0.9640297f, 0.2086420f, -0.0302054f, -0.0063646f},
586
 
                          {-0.0063646f, -0.0302054f, 0.2086420f, 0.9640297f, -0.2042986f, 0.0859768f, -0.0324855f}};
587
 
#endif
588
 
#endif
589
 
 
590
 
int interp_pitch(
591
 
spx_word16_t *exc,          /*decoded excitation*/
592
 
spx_word16_t *interp,          /*decoded excitation*/
593
 
int pitch,               /*pitch period*/
594
 
int len
595
 
)
596
 
{
597
 
   int i,j,k;
598
 
   spx_word32_t corr[4][7];
599
 
   spx_word32_t maxcorr;
600
 
   int maxi, maxj;
601
 
   for (i=0;i<7;i++)
602
 
   {
603
 
      corr[0][i] = inner_prod(exc, exc-pitch-3+i, len);
604
 
   }
605
 
   for (i=0;i<3;i++)
606
 
   {
607
 
      for (j=0;j<7;j++)
608
 
      {
609
 
         int i1, i2;
610
 
         spx_word32_t tmp=0;
611
 
         i1 = 3-j;
612
 
         if (i1<0)
613
 
            i1 = 0;
614
 
         i2 = 10-j;
615
 
         if (i2>7)
616
 
            i2 = 7;
617
 
         for (k=i1;k<i2;k++)
618
 
            tmp += MULT16_32_Q15(shift_filt[i][k],corr[0][j+k-3]);
619
 
         corr[i+1][j] = tmp;
620
 
      }
621
 
   }
622
 
   maxi=maxj=0;
623
 
   maxcorr = corr[0][0];
624
 
   for (i=0;i<4;i++)
625
 
   {
626
 
      for (j=0;j<7;j++)
627
 
      {
628
 
         if (corr[i][j] > maxcorr)
629
 
         {
630
 
            maxcorr = corr[i][j];
631
 
            maxi=i;
632
 
            maxj=j;
633
 
         }
634
 
      }
635
 
   }
636
 
   for (i=0;i<len;i++)
637
 
   {
638
 
      spx_word32_t tmp = 0;
639
 
      if (maxi>0)
640
 
      {
641
 
         for (k=0;k<7;k++)
642
 
         {
643
 
            tmp += MULT16_16(exc[i-(pitch-maxj+3)+k-3],shift_filt[maxi-1][k]);
644
 
         }
645
 
      } else {
646
 
         tmp = SHL32(exc[i-(pitch-maxj+3)],15);
647
 
      }
648
 
      interp[i] = PSHR32(tmp,15);
649
 
   }
650
 
   return pitch-maxj+3;
651
 
}
652
 
 
653
 
void multicomb(
654
 
spx_word16_t *exc,          /*decoded excitation*/
655
 
spx_word16_t *new_exc,      /*enhanced excitation*/
656
 
spx_coef_t *ak,           /*LPC filter coefs*/
657
 
int p,               /*LPC order*/
658
 
int nsf,             /*sub-frame size*/
659
 
int pitch,           /*pitch period*/
660
 
int max_pitch,
661
 
spx_word16_t  comb_gain,    /*gain of comb filter*/
662
 
char *stack
663
 
)
664
 
{
665
 
   int i; 
666
 
   VARDECL(spx_word16_t *iexc);
667
 
   spx_word16_t old_ener, new_ener;
668
 
   int corr_pitch;
669
 
   
670
 
   spx_word16_t iexc0_mag, iexc1_mag, exc_mag;
671
 
   spx_word32_t corr0, corr1;
672
 
   spx_word16_t gain0, gain1;
673
 
   spx_word16_t pgain1, pgain2;
674
 
   spx_word16_t c1, c2;
675
 
   spx_word16_t g1, g2;
676
 
   spx_word16_t ngain;
677
 
   spx_word16_t gg1, gg2;
678
 
#ifdef FIXED_POINT
679
 
   int scaledown=0;
680
 
#endif
681
 
#if 0 /* Set to 1 to enable full pitch search */
682
 
   int nol_pitch[6];
683
 
   spx_word16_t nol_pitch_coef[6];
684
 
   spx_word16_t ol_pitch_coef;
685
 
   open_loop_nbest_pitch(exc, 20, 120, nsf, 
686
 
                         nol_pitch, nol_pitch_coef, 6, stack);
687
 
   corr_pitch=nol_pitch[0];
688
 
   ol_pitch_coef = nol_pitch_coef[0];
689
 
   /*Try to remove pitch multiples*/
690
 
   for (i=1;i<6;i++)
691
 
   {
692
 
#ifdef FIXED_POINT
693
 
      if ((nol_pitch_coef[i]>MULT16_16_Q15(nol_pitch_coef[0],19661)) && 
694
 
#else
695
 
      if ((nol_pitch_coef[i]>.6*nol_pitch_coef[0]) && 
696
 
#endif
697
 
         (ABS(2*nol_pitch[i]-corr_pitch)<=2 || ABS(3*nol_pitch[i]-corr_pitch)<=3 || 
698
 
         ABS(4*nol_pitch[i]-corr_pitch)<=4 || ABS(5*nol_pitch[i]-corr_pitch)<=5))
699
 
      {
700
 
         corr_pitch = nol_pitch[i];
701
 
      }
702
 
   }
703
 
#else
704
 
   corr_pitch = pitch;
705
 
#endif
706
 
   
707
 
   ALLOC(iexc, 2*nsf, spx_word16_t);
708
 
   
709
 
   interp_pitch(exc, iexc, corr_pitch, 80);
710
 
   if (corr_pitch>max_pitch)
711
 
      interp_pitch(exc, iexc+nsf, 2*corr_pitch, 80);
712
 
   else
713
 
      interp_pitch(exc, iexc+nsf, -corr_pitch, 80);
714
 
 
715
 
#ifdef FIXED_POINT
716
 
   for (i=0;i<nsf;i++)
717
 
   {
718
 
      if (ABS16(exc[i])>16383)
719
 
      {
720
 
         scaledown = 1;
721
 
         break;
722
 
      }
723
 
   }
724
 
   if (scaledown)
725
 
   {
726
 
      for (i=0;i<nsf;i++)
727
 
         exc[i] = SHR16(exc[i],1);
728
 
      for (i=0;i<2*nsf;i++)
729
 
         iexc[i] = SHR16(iexc[i],1);
730
 
   }
731
 
#endif
732
 
   /*interp_pitch(exc, iexc+2*nsf, 2*corr_pitch, 80);*/
733
 
   
734
 
   /*printf ("%d %d %f\n", pitch, corr_pitch, max_corr*ener_1);*/
735
 
   iexc0_mag = spx_sqrt(1000+inner_prod(iexc,iexc,nsf));
736
 
   iexc1_mag = spx_sqrt(1000+inner_prod(iexc+nsf,iexc+nsf,nsf));
737
 
   exc_mag = spx_sqrt(1+inner_prod(exc,exc,nsf));
738
 
   corr0  = inner_prod(iexc,exc,nsf);
739
 
   if (corr0<0)
740
 
      corr0=0;
741
 
   corr1 = inner_prod(iexc+nsf,exc,nsf);
742
 
   if (corr1<0)
743
 
      corr1=0;
744
 
#ifdef FIXED_POINT
745
 
   /* Doesn't cost much to limit the ratio and it makes the rest easier */
746
 
   if (SHL32(EXTEND32(iexc0_mag),6) < EXTEND32(exc_mag))
747
 
      iexc0_mag = ADD16(1,PSHR16(exc_mag,6));
748
 
   if (SHL32(EXTEND32(iexc1_mag),6) < EXTEND32(exc_mag))
749
 
      iexc1_mag = ADD16(1,PSHR16(exc_mag,6));
750
 
#endif
751
 
   if (corr0 > MULT16_16(iexc0_mag,exc_mag))
752
 
      pgain1 = QCONST16(1., 14);
753
 
   else
754
 
      pgain1 = PDIV32_16(SHL32(PDIV32(corr0, exc_mag),14),iexc0_mag);
755
 
   if (corr1 > MULT16_16(iexc1_mag,exc_mag))
756
 
      pgain2 = QCONST16(1., 14);
757
 
   else
758
 
      pgain2 = PDIV32_16(SHL32(PDIV32(corr1, exc_mag),14),iexc1_mag);
759
 
   gg1 = PDIV32_16(SHL32(EXTEND32(exc_mag),8), iexc0_mag);
760
 
   gg2 = PDIV32_16(SHL32(EXTEND32(exc_mag),8), iexc1_mag);
761
 
   if (comb_gain>0)
762
 
   {
763
 
#ifdef FIXED_POINT
764
 
      c1 = (MULT16_16_Q15(QCONST16(.4,15),comb_gain)+QCONST16(.07,15));
765
 
      c2 = QCONST16(.5,15)+MULT16_16_Q14(QCONST16(1.72,14),(c1-QCONST16(.07,15)));
766
 
#else
767
 
      c1 = .4*comb_gain+.07;
768
 
      c2 = .5+1.72*(c1-.07);
769
 
#endif
770
 
   } else 
771
 
   {
772
 
      c1=c2=0;
773
 
   }
774
 
#ifdef FIXED_POINT
775
 
   g1 = 32767 - MULT16_16_Q13(MULT16_16_Q15(c2, pgain1),pgain1);
776
 
   g2 = 32767 - MULT16_16_Q13(MULT16_16_Q15(c2, pgain2),pgain2);
777
 
#else
778
 
   g1 = 1-c2*pgain1*pgain1;
779
 
   g2 = 1-c2*pgain2*pgain2;
780
 
#endif
781
 
   if (g1<c1)
782
 
      g1 = c1;
783
 
   if (g2<c1)
784
 
      g2 = c1;
785
 
   g1 = (spx_word16_t)PDIV32_16(SHL32(EXTEND32(c1),14),(spx_word16_t)g1);
786
 
   g2 = (spx_word16_t)PDIV32_16(SHL32(EXTEND32(c1),14),(spx_word16_t)g2);
787
 
   if (corr_pitch>max_pitch)
788
 
   {
789
 
      gain0 = MULT16_16_Q15(QCONST16(.7,15),MULT16_16_Q14(g1,gg1));
790
 
      gain1 = MULT16_16_Q15(QCONST16(.3,15),MULT16_16_Q14(g2,gg2));
791
 
   } else {
792
 
      gain0 = MULT16_16_Q15(QCONST16(.6,15),MULT16_16_Q14(g1,gg1));
793
 
      gain1 = MULT16_16_Q15(QCONST16(.6,15),MULT16_16_Q14(g2,gg2));
794
 
   }
795
 
   for (i=0;i<nsf;i++)
796
 
      new_exc[i] = ADD16(exc[i], EXTRACT16(PSHR32(ADD32(MULT16_16(gain0,iexc[i]), MULT16_16(gain1,iexc[i+nsf])),8)));
797
 
   /* FIXME: compute_rms16 is currently not quite accurate enough (but close) */
798
 
   new_ener = compute_rms16(new_exc, nsf);
799
 
   old_ener = compute_rms16(exc, nsf);
800
 
   
801
 
   if (old_ener < 1)
802
 
      old_ener = 1;
803
 
   if (new_ener < 1)
804
 
      new_ener = 1;
805
 
   if (old_ener > new_ener)
806
 
      old_ener = new_ener;
807
 
   ngain = PDIV32_16(SHL32(EXTEND32(old_ener),14),new_ener);
808
 
   
809
 
   for (i=0;i<nsf;i++)
810
 
      new_exc[i] = MULT16_16_Q14(ngain, new_exc[i]);
811
 
#ifdef FIXED_POINT
812
 
   if (scaledown)
813
 
   {
814
 
      for (i=0;i<nsf;i++)
815
 
         exc[i] = SHL16(exc[i],1);
816
 
      for (i=0;i<nsf;i++)
817
 
         new_exc[i] = SHL16(SATURATE16(new_exc[i],16383),1);
818
 
   }
819
 
#endif
820
 
}
821