filters.c 24.2 KB
Newer Older
Tristan Matthews's avatar
Tristan Matthews committed
1
/* Copyright (C) 2002-2006 Jean-Marc Valin
2
3
4
   File: filters.c
   Various analysis/synthesis filters

jm's avatar
jm committed
5
6
7
   Redistribution and use in source and binary forms, with or without
   modification, are permitted provided that the following conditions
   are met:
Tristan Matthews's avatar
Tristan Matthews committed
8

jm's avatar
jm committed
9
10
   - Redistributions of source code must retain the above copyright
   notice, this list of conditions and the following disclaimer.
Tristan Matthews's avatar
Tristan Matthews committed
11

jm's avatar
jm committed
12
13
14
   - 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.
Tristan Matthews's avatar
Tristan Matthews committed
15

jm's avatar
jm committed
16
17
18
   - 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.
Tristan Matthews's avatar
Tristan Matthews committed
19

jm's avatar
jm committed
20
21
22
23
24
25
26
27
28
29
30
   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.
31
32
*/

33
34
35
36
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

37
#include "filters.h"
38
#include "stack_alloc.h"
jm's avatar
jm committed
39
#include "arch.h"
40
#include "math_approx.h"
jm's avatar
jm committed
41
#include "ltp.h"
42
#include <math.h>
43

jm's avatar
jm committed
44
45
46
47
#ifdef _USE_SSE
#include "filters_sse.h"
#elif defined (ARM4_ASM) || defined(ARM5E_ASM)
#include "filters_arm4.h"
48
49
#elif defined (BFIN_ASM)
#include "filters_bfin.h"
jm's avatar
jm committed
50
51
52
53
#endif



jm's avatar
jm committed
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
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
void sanitize_values32(spx_word32_t *vec, spx_word32_t min_val, spx_word32_t max_val, int len)
{
   int i;
   for (i=0;i<len;i++)
   {
      /* It's important we do the test that way so we can catch NaNs, which are neither greater nor smaller */
      if (!(vec[i]>=min_val && vec[i] <= max_val))
      {
         if (vec[i] < min_val)
            vec[i] = min_val;
         else if (vec[i] > max_val)
            vec[i] = max_val;
         else /* Has to be NaN */
            vec[i] = 0;
      }
   }
}

jm's avatar
jm committed
83
84
85
86
void highpass(const spx_word16_t *x, spx_word16_t *y, int len, int filtID, spx_mem_t *mem)
{
   int i;
#ifdef FIXED_POINT
jm's avatar
jm committed
87
88
   const spx_word16_t Pcoef[5][3] = {{16384, -31313, 14991}, {16384, -31569, 15249}, {16384, -31677, 15328}, {16384, -32313, 15947}, {16384, -22446, 6537}};
   const spx_word16_t Zcoef[5][3] = {{15672, -31344, 15672}, {15802, -31601, 15802}, {15847, -31694, 15847}, {16162, -32322, 16162}, {14418, -28836, 14418}};
jm's avatar
jm committed
89
#else
jm's avatar
jm committed
90
91
   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}};
   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}};
jm's avatar
jm committed
92
93
#endif
   const spx_word16_t *den, *num;
jm's avatar
jm committed
94
95
   if (filtID>4)
      filtID=4;
Tristan Matthews's avatar
Tristan Matthews committed
96

jm's avatar
jm committed
97
   den = Pcoef[filtID]; num = Zcoef[filtID];
98
   /*return;*/
jm's avatar
jm committed
99
100
   for (i=0;i<len;i++)
   {
jm's avatar
jm committed
101
      spx_word16_t yi;
102
103
      spx_word32_t vout = ADD32(MULT16_16(num[0], x[i]),mem[0]);
      yi = EXTRACT16(SATURATE(PSHR32(vout,14),32767));
104
105
      mem[0] = ADD32(MAC16_16(mem[1], num[1],x[i]), SHL32(MULT16_32_Q15(-den[1],vout),1));
      mem[1] = ADD32(MULT16_16(num[2],x[i]), SHL32(MULT16_32_Q15(-den[2],vout),1));
jm's avatar
jm committed
106
107
108
      y[i] = yi;
   }
}
109

110
111
#ifdef FIXED_POINT

jm's avatar
...    
jm committed
112
/* FIXME: These functions are ugly and probably introduce too much error */
jm's avatar
jm committed
113
void signal_mul(const spx_sig_t *x, spx_sig_t *y, spx_word32_t scale, int len)
114
115
116
{
   int i;
   for (i=0;i<len;i++)
117
   {
jm's avatar
jm committed
118
      y[i] = SHL32(MULT16_32_Q14(EXTRACT16(SHR32(x[i],7)),scale),7);
119
   }
120
121
}

122
#ifndef DISABLE_ENCODER
123
void signal_div(const spx_word16_t *x, spx_word16_t *y, spx_word32_t scale, int len)
124
125
{
   int i;
jm's avatar
jm committed
126
   if (scale > SHL32(EXTEND32(SIG_SCALING), 8))
127
   {
128
129
      spx_word16_t scale_1;
      scale = PSHR32(scale, SIG_SHIFT);
130
      scale_1 = EXTRACT16(PDIV32_16(SHL32(EXTEND32(SIG_SCALING),7),scale));
131
132
      for (i=0;i<len;i++)
      {
133
         y[i] = MULT16_16_P15(scale_1, x[i]);
134
      }
135
   } else if (scale > SHR32(EXTEND32(SIG_SCALING), 2)) {
136
137
      spx_word16_t scale_1;
      scale = PSHR32(scale, SIG_SHIFT-5);
jm's avatar
jm committed
138
      scale_1 = DIV32_16(SHL32(EXTEND32(SIG_SCALING),3),scale);
139
140
      for (i=0;i<len;i++)
      {
141
         y[i] = PSHR32(MULT16_16(scale_1, SHL16(x[i],2)),8);
142
      }
143
144
145
146
147
148
149
150
   } else {
      spx_word16_t scale_1;
      scale = PSHR32(scale, SIG_SHIFT-7);
      if (scale < 5)
         scale = 5;
      scale_1 = DIV32_16(SHL32(EXTEND32(SIG_SCALING),3),scale);
      for (i=0;i<len;i++)
      {
151
         y[i] = PSHR32(MULT16_16(scale_1, SHL16(x[i],2)),6);
152
      }
153
   }
154
}
155
#endif /* DISABLE_ENCODER */
156

157
#else /* FIXED_POINT */
158

jm's avatar
jm committed
159
void signal_mul(const spx_sig_t *x, spx_sig_t *y, spx_word32_t scale, int len)
160
161
162
163
164
165
{
   int i;
   for (i=0;i<len;i++)
      y[i] = scale*x[i];
}

166
#ifndef DISABLE_ENCODER
jm's avatar
jm committed
167
void signal_div(const spx_sig_t *x, spx_sig_t *y, spx_word32_t scale, int len)
168
169
{
   int i;
170
   float scale_1 = 1/scale;
171
172
173
   for (i=0;i<len;i++)
      y[i] = scale_1*x[i];
}
174
175
176
#endif /* DISABLE_ENCODER */

#endif /* !FIXED_POINT */
177

178

179

180
181
#ifdef FIXED_POINT

182
183


184
#if !defined (DISABLE_WIDEBAND) && !defined (DISABLE_ENCODER)
jm's avatar
jm committed
185
spx_word16_t compute_rms(const spx_sig_t *x, int len)
186
187
188
189
190
191
192
193
194
195
{
   int i;
   spx_word32_t sum=0;
   spx_sig_t max_val=1;
   int sig_shift;

   for (i=0;i<len;i++)
   {
      spx_sig_t tmp = x[i];
      if (tmp<0)
196
         tmp = -tmp;
197
198
199
200
201
      if (tmp > max_val)
         max_val = tmp;
   }

   sig_shift=0;
202
   while (max_val>16383)
203
204
205
206
207
208
209
210
211
   {
      sig_shift++;
      max_val >>= 1;
   }

   for (i=0;i<len;i+=4)
   {
      spx_word32_t sum2=0;
      spx_word16_t tmp;
jm's avatar
jm committed
212
      tmp = EXTRACT16(SHR32(x[i],sig_shift));
213
      sum2 = MAC16_16(sum2,tmp,tmp);
jm's avatar
jm committed
214
      tmp = EXTRACT16(SHR32(x[i+1],sig_shift));
215
      sum2 = MAC16_16(sum2,tmp,tmp);
jm's avatar
jm committed
216
      tmp = EXTRACT16(SHR32(x[i+2],sig_shift));
217
      sum2 = MAC16_16(sum2,tmp,tmp);
jm's avatar
jm committed
218
      tmp = EXTRACT16(SHR32(x[i+3],sig_shift));
219
      sum2 = MAC16_16(sum2,tmp,tmp);
jm's avatar
jm committed
220
      sum = ADD32(sum,SHR32(sum2,6));
221
   }
Tristan Matthews's avatar
Tristan Matthews committed
222

223
   return EXTRACT16(PSHR32(SHL32(EXTEND32(spx_sqrt(DIV32(sum,len))),(sig_shift+3)),SIG_SHIFT));
224
}
225
#endif /* !defined (DISABLE_WIDEBAND) && !defined (DISABLE_ENCODER) */
226

227
228
229
spx_word16_t compute_rms16(const spx_word16_t *x, int len)
{
   int i;
Tristan Matthews's avatar
Tristan Matthews committed
230
   spx_word16_t max_val=10;
231
232

   for (i=0;i<len;i++)
233
   {
234
235
236
237
238
239
240
241
242
243
244
245
      spx_sig_t tmp = x[i];
      if (tmp<0)
         tmp = -tmp;
      if (tmp > max_val)
         max_val = tmp;
   }
   if (max_val>16383)
   {
      spx_word32_t sum=0;
      for (i=0;i<len;i+=4)
      {
         spx_word32_t sum2=0;
246
247
248
249
         sum2 = MAC16_16(sum2,SHR16(x[i],1),SHR16(x[i],1));
         sum2 = MAC16_16(sum2,SHR16(x[i+1],1),SHR16(x[i+1],1));
         sum2 = MAC16_16(sum2,SHR16(x[i+2],1),SHR16(x[i+2],1));
         sum2 = MAC16_16(sum2,SHR16(x[i+3],1),SHR16(x[i+3],1));
250
251
         sum = ADD32(sum,SHR32(sum2,6));
      }
252
      return SHL16(spx_sqrt(DIV32(sum,len)),4);
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
   } else {
      spx_word32_t sum=0;
      int sig_shift=0;
      if (max_val < 8192)
         sig_shift=1;
      if (max_val < 4096)
         sig_shift=2;
      if (max_val < 2048)
         sig_shift=3;
      for (i=0;i<len;i+=4)
      {
         spx_word32_t sum2=0;
         sum2 = MAC16_16(sum2,SHL16(x[i],sig_shift),SHL16(x[i],sig_shift));
         sum2 = MAC16_16(sum2,SHL16(x[i+1],sig_shift),SHL16(x[i+1],sig_shift));
         sum2 = MAC16_16(sum2,SHL16(x[i+2],sig_shift),SHL16(x[i+2],sig_shift));
         sum2 = MAC16_16(sum2,SHL16(x[i+3],sig_shift),SHL16(x[i+3],sig_shift));
         sum = ADD32(sum,SHR32(sum2,6));
      }
Tristan Matthews's avatar
Tristan Matthews committed
271
      return SHL16(spx_sqrt(DIV32(sum,len)),3-sig_shift);
272
273
   }
}
274

jm's avatar
jm committed
275
#ifndef OVERRIDE_NORMALIZE16
276
int normalize16(const spx_sig_t *x, spx_word16_t *y, spx_sig_t max_scale, int len)
277
278
279
280
{
   int i;
   spx_sig_t max_val=1;
   int sig_shift;
Tristan Matthews's avatar
Tristan Matthews committed
281

282
283
284
285
   for (i=0;i<len;i++)
   {
      spx_sig_t tmp = x[i];
      if (tmp<0)
jm's avatar
jm committed
286
         tmp = NEG32(tmp);
287
288
      if (tmp >= max_val)
         max_val = tmp;
289
   }
jm's avatar
jm committed
290

291
292
   sig_shift=0;
   while (max_val>max_scale)
jm's avatar
jm committed
293
   {
294
295
296
      sig_shift++;
      max_val >>= 1;
   }
jm's avatar
jm committed
297

298
   for (i=0;i<len;i++)
jm's avatar
jm committed
299
      y[i] = EXTRACT16(SHR32(x[i], sig_shift));
Tristan Matthews's avatar
Tristan Matthews committed
300

301
   return sig_shift;
jm's avatar
jm committed
302
}
jm's avatar
jm committed
303
#endif
jm's avatar
jm committed
304

jm's avatar
jm committed
305
306
307
308
309
310
311
312
313
314
315
316
#else

spx_word16_t compute_rms(const spx_sig_t *x, int len)
{
   int i;
   float sum=0;
   for (i=0;i<len;i++)
   {
      sum += x[i]*x[i];
   }
   return sqrt(.1+sum/len);
}
317
318
319
320
spx_word16_t compute_rms16(const spx_word16_t *x, int len)
{
   return compute_rms(x, len);
}
jm's avatar
jm committed
321
322
#endif

323
324
325
#ifdef MERGE_FILTERS
const spx_word16_t zeros[10] = {0,0,0,0,0,0,0,0,0,0};
#endif  /* MERGE_FILTERS */
jm's avatar
jm committed
326

327
#if !defined(OVERRIDE_FILTER_MEM16) && !defined(DISABLE_ENCODER)
328
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)
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
{
   int i,j;
   spx_word16_t xi,yi,nyi;
   for (i=0;i<N;i++)
   {
      xi= x[i];
      yi = EXTRACT16(SATURATE(ADD32(EXTEND32(x[i]),PSHR32(mem[0],LPC_SHIFT)),32767));
      nyi = NEG16(yi);
      for (j=0;j<ord-1;j++)
      {
         mem[j] = MAC16_16(MAC16_16(mem[j+1], num[j],xi), den[j],nyi);
      }
      mem[ord-1] = ADD32(MULT16_16(num[ord-1],xi), MULT16_16(den[ord-1],nyi));
      y[i] = yi;
   }
}
345
#endif /* !defined(OVERRIDE_FILTER_MEM16) && !defined(DISABLE_ENCODER) */
346

jm's avatar
jm committed
347
#ifndef OVERRIDE_IIR_MEM16
348
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)
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
{
   int i,j;
   spx_word16_t yi,nyi;

   for (i=0;i<N;i++)
   {
      yi = EXTRACT16(SATURATE(ADD32(EXTEND32(x[i]),PSHR32(mem[0],LPC_SHIFT)),32767));
      nyi = NEG16(yi);
      for (j=0;j<ord-1;j++)
      {
         mem[j] = MAC16_16(mem[j+1],den[j],nyi);
      }
      mem[ord-1] = MULT16_16(den[ord-1],nyi);
      y[i] = yi;
   }
}
jm's avatar
jm committed
365
#endif
366

367
#if !defined(OVERRIDE_FIR_MEM16) && !defined(DISABLE_ENCODER)
368
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)
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
{
   int i,j;
   spx_word16_t xi,yi;

   for (i=0;i<N;i++)
   {
      xi=x[i];
      yi = EXTRACT16(SATURATE(ADD32(EXTEND32(x[i]),PSHR32(mem[0],LPC_SHIFT)),32767));
      for (j=0;j<ord-1;j++)
      {
         mem[j] = MAC16_16(mem[j+1], num[j],xi);
      }
      mem[ord-1] = MULT16_16(num[ord-1],xi);
      y[i] = yi;
   }
}
385
#endif /* !defined(OVERRIDE_FIR_MEM16) && !defined(DISABLE_ENCODER) */
386

387
#ifndef DISABLE_ENCODER
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
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)
{
   int i;
   VARDECL(spx_mem_t *mem);
   ALLOC(mem, ord, spx_mem_t);
   for (i=0;i<ord;i++)
      mem[i]=0;
   iir_mem16(xx, ak, y, N, ord, mem, stack);
   for (i=0;i<ord;i++)
      mem[i]=0;
   filter_mem16(y, awk1, awk2, y, N, ord, mem, stack);
}
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)
{
   int i;
   VARDECL(spx_mem_t *mem);
   ALLOC(mem, ord, spx_mem_t);
   for (i=0;i<ord;i++)
      mem[i]=0;
   filter_mem16(xx, ak, awk1, y, N, ord, mem, stack);
   for (i=0;i<ord;i++)
      mem[i]=0;
   fir_mem16(y, awk2, y, N, ord, mem, stack);
}
412
#endif /* DISABLE_ENCODER */
413

414
#if !defined(OVERRIDE_COMPUTE_IMPULSE_RESPONSE) && !defined(DISABLE_ENCODER)
415
416
417
418
419
420
421
422
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)
{
   int i,j;
   spx_word16_t y1, ny1i, ny2i;
   VARDECL(spx_mem_t *mem1);
   VARDECL(spx_mem_t *mem2);
   ALLOC(mem1, ord, spx_mem_t);
   ALLOC(mem2, ord, spx_mem_t);
Tristan Matthews's avatar
Tristan Matthews committed
423

424
425
426
427
   y[0] = LPC_SCALING;
   for (i=0;i<ord;i++)
      y[i+1] = awk1[i];
   i++;
428
429
430
431
432
433
   for (;i<N;i++)
      y[i] = VERY_SMALL;
   for (i=0;i<ord;i++)
      mem1[i] = mem2[i] = 0;
   for (i=0;i<N;i++)
   {
jm's avatar
jm committed
434
      y1 = ADD16(y[i], EXTRACT16(PSHR32(mem1[0],LPC_SHIFT)));
jm's avatar
jm committed
435
      ny1i = NEG16(y1);
436
      y[i] = PSHR32(ADD32(SHL32(EXTEND32(y1),LPC_SHIFT+1),mem2[0]),LPC_SHIFT);
jm's avatar
jm committed
437
      ny2i = NEG16(y[i]);
438
439
      for (j=0;j<ord-1;j++)
      {
440
441
         mem1[j] = MAC16_16(mem1[j+1], awk2[j],ny1i);
         mem2[j] = MAC16_16(mem2[j+1], ak[j],ny2i);
442
      }
443
444
      mem1[ord-1] = MULT16_16(awk2[ord-1],ny1i);
      mem2[ord-1] = MULT16_16(ak[ord-1],ny2i);
445
446
   }
}
447
#endif /* !defined(OVERRIDE_COMPUTE_IMPULSE_RESPONSE) && !defined(DISABLE_ENCODER) */
448

449
#ifndef DISABLE_WIDEBAND
450
/* Decomposes a signal into low-band and high-band using a QMF */
451
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)
452
{
453
   int i,j,k,M2;
454
455
   VARDECL(spx_word16_t *a);
   VARDECL(spx_word16_t *x);
jm's avatar
jm committed
456
   spx_word16_t *x2;
Tristan Matthews's avatar
Tristan Matthews committed
457

458
459
   ALLOC(a, M, spx_word16_t);
   ALLOC(x, N+M-1, spx_word16_t);
460
   x2=x+M-1;
461
   M2=M>>1;
462
   for (i=0;i<M;i++)
jm's avatar
jm committed
463
      a[M-i-1]= aa[i];
464
465
466
   for (i=0;i<M-1;i++)
      x[i]=mem[M-i-2];
   for (i=0;i<N;i++)
467
      x[i+M-1]=SHR16(xx[i],1);
468
469
   for (i=0;i<M-1;i++)
      mem[i]=SHR16(xx[N-i-1],1);
470
   for (i=0,k=0;i<N;i+=2,k++)
471
   {
472
      spx_word32_t y1k=0, y2k=0;
473
474
      for (j=0;j<M2;j++)
      {
475
476
         y1k=ADD32(y1k,MULT16_16(a[j],ADD16(x[i+j],x2[i-j])));
         y2k=SUB32(y2k,MULT16_16(a[j],SUB16(x[i+j],x2[i-j])));
477
         j++;
478
479
         y1k=ADD32(y1k,MULT16_16(a[j],ADD16(x[i+j],x2[i-j])));
         y2k=ADD32(y2k,MULT16_16(a[j],SUB16(x[i+j],x2[i-j])));
480
      }
481
482
      y1[k] = EXTRACT16(SATURATE(PSHR32(y1k,15),32767));
      y2[k] = EXTRACT16(SATURATE(PSHR32(y2k,15),32767));
483
484
   }
}
485

486
/* Re-synthesised a signal from the QMF low-band and high-band signals */
487
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)
488
489
490
491
492
   /* assumptions:
      all odd x[i] are zero -- well, actually they are left out of the array now
      N and M are multiples of 4 */
{
   int i, j;
493
   int M2, N2;
494
495
   VARDECL(spx_word16_t *xx1);
   VARDECL(spx_word16_t *xx2);
Tristan Matthews's avatar
Tristan Matthews committed
496

497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
   M2 = M>>1;
   N2 = N>>1;
   ALLOC(xx1, M2+N2, spx_word16_t);
   ALLOC(xx2, M2+N2, spx_word16_t);

   for (i = 0; i < N2; i++)
      xx1[i] = x1[N2-1-i];
   for (i = 0; i < M2; i++)
      xx1[N2+i] = mem1[2*i+1];
   for (i = 0; i < N2; i++)
      xx2[i] = x2[N2-1-i];
   for (i = 0; i < M2; i++)
      xx2[N2+i] = mem2[2*i+1];

   for (i = 0; i < N2; i += 2) {
512
513
514
515
      spx_sig_t y0, y1, y2, y3;
      spx_word16_t x10, x20;

      y0 = y1 = y2 = y3 = 0;
516
517
      x10 = xx1[N2-2-i];
      x20 = xx2[N2-2-i];
518

519
      for (j = 0; j < M2; j += 2) {
520
521
522
         spx_word16_t x11, x21;
         spx_word16_t a0, a1;

523
524
525
526
         a0 = a[2*j];
         a1 = a[2*j+1];
         x11 = xx1[N2-1+j-i];
         x21 = xx2[N2-1+j-i];
527
528
529
530
531
532
533
534
535
536
537
538
539

#ifdef FIXED_POINT
         /* We multiply twice by the same coef to avoid overflows */
         y0 = MAC16_16(MAC16_16(y0, a0, x11), NEG16(a0), x21);
         y1 = MAC16_16(MAC16_16(y1, a1, x11), a1, x21);
         y2 = MAC16_16(MAC16_16(y2, a0, x10), NEG16(a0), x20);
         y3 = MAC16_16(MAC16_16(y3, a1, x10), a1, x20);
#else
         y0 = ADD32(y0,MULT16_16(a0, x11-x21));
         y1 = ADD32(y1,MULT16_16(a1, x11+x21));
         y2 = ADD32(y2,MULT16_16(a0, x10-x20));
         y3 = ADD32(y3,MULT16_16(a1, x10+x20));
#endif
540
541
542
543
         a0 = a[2*j+2];
         a1 = a[2*j+3];
         x10 = xx1[N2+j-i];
         x20 = xx2[N2+j-i];
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558

#ifdef FIXED_POINT
         /* We multiply twice by the same coef to avoid overflows */
         y0 = MAC16_16(MAC16_16(y0, a0, x10), NEG16(a0), x20);
         y1 = MAC16_16(MAC16_16(y1, a1, x10), a1, x20);
         y2 = MAC16_16(MAC16_16(y2, a0, x11), NEG16(a0), x21);
         y3 = MAC16_16(MAC16_16(y3, a1, x11), a1, x21);
#else
         y0 = ADD32(y0,MULT16_16(a0, x10-x20));
         y1 = ADD32(y1,MULT16_16(a1, x10+x20));
         y2 = ADD32(y2,MULT16_16(a0, x11-x21));
         y3 = ADD32(y3,MULT16_16(a1, x11+x21));
#endif
      }
#ifdef FIXED_POINT
559
560
561
562
      y[2*i] = EXTRACT16(SATURATE32(PSHR32(y0,15),32767));
      y[2*i+1] = EXTRACT16(SATURATE32(PSHR32(y1,15),32767));
      y[2*i+2] = EXTRACT16(SATURATE32(PSHR32(y2,15),32767));
      y[2*i+3] = EXTRACT16(SATURATE32(PSHR32(y3,15),32767));
563
564
#else
      /* Normalize up explicitly if we're in float */
565
566
567
568
      y[2*i] = 2.f*y0;
      y[2*i+1] = 2.f*y1;
      y[2*i+2] = 2.f*y2;
      y[2*i+3] = 2.f*y3;
569
570
571
#endif
   }

572
573
574
575
   for (i = 0; i < M2; i++)
      mem1[2*i+1] = xx1[i];
   for (i = 0; i < M2; i++)
      mem2[2*i+1] = xx2[i];
576
}
577
578
579
580
#endif /* DISABLE_WIDEBAND */


#ifndef DISABLE_DECODER
581

582
583
#ifdef FIXED_POINT
#if 0
584
const spx_word16_t shift_filt[3][7] = {{-33,    1043,   -4551,   19959,   19959,   -4551,    1043},
585
586
587
                                 {-98,    1133,   -4425,   29179,    8895,   -2328,     444},
                                 {444,   -2328,    8895,   29179,   -4425,    1133,     -98}};
#else
588
const spx_word16_t shift_filt[3][7] = {{-390,    1540,   -4993,   20123,   20123,   -4993,    1540},
589
590
591
592
                                {-1064,    2817,   -6694,   31589,    6837,    -990,    -209},
                                 {-209,    -990,    6837,   31589,   -6694,    2817,   -1064}};
#endif
#else
jm's avatar
jm committed
593
#if 0
594
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},
jm's avatar
jm committed
595
596
                          {-0.0029937, 0.0345613, -0.1350474, 0.8904793, 0.2714479, -0.0710304, 0.0135403},
                          {0.0135403, -0.0710304, 0.2714479, 0.8904793, -0.1350474, 0.0345613,  -0.0029937}};
jm's avatar
jm committed
597
#else
598
const float shift_filt[3][7] = {{-0.011915f, 0.046995f, -0.152373f, 0.614108f, 0.614108f, -0.152373f, 0.046995f},
599
600
                          {-0.0324855f, 0.0859768f, -0.2042986f, 0.9640297f, 0.2086420f, -0.0302054f, -0.0063646f},
                          {-0.0063646f, -0.0302054f, 0.2086420f, 0.9640297f, -0.2042986f, 0.0859768f, -0.0324855f}};
jm's avatar
jm committed
601
#endif
602
#endif
jm's avatar
jm committed
603

Alfred E. Heggestad's avatar
Alfred E. Heggestad committed
604
static int interp_pitch(
605
606
spx_word16_t *exc,          /*decoded excitation*/
spx_word16_t *interp,          /*decoded excitation*/
jm's avatar
jm committed
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
int pitch,               /*pitch period*/
int len
)
{
   int i,j,k;
   spx_word32_t corr[4][7];
   spx_word32_t maxcorr;
   int maxi, maxj;
   for (i=0;i<7;i++)
   {
      corr[0][i] = inner_prod(exc, exc-pitch-3+i, len);
   }
   for (i=0;i<3;i++)
   {
      for (j=0;j<7;j++)
      {
623
         int i1, i2;
jm's avatar
jm committed
624
         spx_word32_t tmp=0;
625
626
627
628
629
630
631
632
         i1 = 3-j;
         if (i1<0)
            i1 = 0;
         i2 = 10-j;
         if (i2>7)
            i2 = 7;
         for (k=i1;k<i2;k++)
            tmp += MULT16_32_Q15(shift_filt[i][k],corr[0][j+k-3]);
633
         corr[i+1][j] = tmp;
jm's avatar
jm committed
634
635
      }
   }
636
637
   maxi=maxj=0;
   maxcorr = corr[0][0];
jm's avatar
jm committed
638
639
640
641
642
643
644
645
646
647
648
649
650
651
   for (i=0;i<4;i++)
   {
      for (j=0;j<7;j++)
      {
         if (corr[i][j] > maxcorr)
         {
            maxcorr = corr[i][j];
            maxi=i;
            maxj=j;
         }
      }
   }
   for (i=0;i<len;i++)
   {
652
      spx_word32_t tmp = 0;
jm's avatar
jm committed
653
654
655
656
      if (maxi>0)
      {
         for (k=0;k<7;k++)
         {
657
            tmp += MULT16_16(exc[i-(pitch-maxj+3)+k-3],shift_filt[maxi-1][k]);
jm's avatar
jm committed
658
659
         }
      } else {
660
         tmp = SHL32(exc[i-(pitch-maxj+3)],15);
jm's avatar
jm committed
661
      }
662
      interp[i] = PSHR32(tmp,15);
jm's avatar
jm committed
663
664
665
   }
   return pitch-maxj+3;
}
666

jm's avatar
jm committed
667
void multicomb(
668
spx_word16_t *exc,          /*decoded excitation*/
669
spx_word16_t *new_exc,      /*enhanced excitation*/
jm's avatar
jm committed
670
671
672
673
spx_coef_t *ak,           /*LPC filter coefs*/
int p,               /*LPC order*/
int nsf,             /*sub-frame size*/
int pitch,           /*pitch period*/
674
int max_pitch,
jm's avatar
jm committed
675
676
677
678
spx_word16_t  comb_gain,    /*gain of comb filter*/
char *stack
)
{
Tristan Matthews's avatar
Tristan Matthews committed
679
   int i;
680
   VARDECL(spx_word16_t *iexc);
681
   spx_word16_t old_ener, new_ener;
jm's avatar
jm committed
682
   int corr_pitch;
Tristan Matthews's avatar
Tristan Matthews committed
683

684
685
686
   spx_word16_t iexc0_mag, iexc1_mag, exc_mag;
   spx_word32_t corr0, corr1;
   spx_word16_t gain0, gain1;
687
688
689
690
   spx_word16_t pgain1, pgain2;
   spx_word16_t c1, c2;
   spx_word16_t g1, g2;
   spx_word16_t ngain;
jm's avatar
jm committed
691
   spx_word16_t gg1, gg2;
692
#ifdef FIXED_POINT
693
   int scaledown=0;
694
#endif
695
#if 0 /* Set to 1 to enable full pitch search */
696
697
698
   int nol_pitch[6];
   spx_word16_t nol_pitch_coef[6];
   spx_word16_t ol_pitch_coef;
Tristan Matthews's avatar
Tristan Matthews committed
699
   open_loop_nbest_pitch(exc, 20, 120, nsf,
700
701
702
703
704
                         nol_pitch, nol_pitch_coef, 6, stack);
   corr_pitch=nol_pitch[0];
   ol_pitch_coef = nol_pitch_coef[0];
   /*Try to remove pitch multiples*/
   for (i=1;i<6;i++)
jm's avatar
jm committed
705
   {
706
#ifdef FIXED_POINT
Tristan Matthews's avatar
Tristan Matthews committed
707
      if ((nol_pitch_coef[i]>MULT16_16_Q15(nol_pitch_coef[0],19661)) &&
jm's avatar
jm committed
708
#else
Tristan Matthews's avatar
Tristan Matthews committed
709
      if ((nol_pitch_coef[i]>.6*nol_pitch_coef[0]) &&
jm's avatar
jm committed
710
#endif
Tristan Matthews's avatar
Tristan Matthews committed
711
         (ABS(2*nol_pitch[i]-corr_pitch)<=2 || ABS(3*nol_pitch[i]-corr_pitch)<=3 ||
712
         ABS(4*nol_pitch[i]-corr_pitch)<=4 || ABS(5*nol_pitch[i]-corr_pitch)<=5))
jm's avatar
jm committed
713
      {
714
         corr_pitch = nol_pitch[i];
jm's avatar
jm committed
715
716
      }
   }
717
718
719
#else
   corr_pitch = pitch;
#endif
Tristan Matthews's avatar
Tristan Matthews committed
720

721
   ALLOC(iexc, 2*nsf, spx_word16_t);
Tristan Matthews's avatar
Tristan Matthews committed
722

jm's avatar
jm committed
723
   interp_pitch(exc, iexc, corr_pitch, 80);
724
   if (corr_pitch>max_pitch)
jm's avatar
jm committed
725
726
727
728
      interp_pitch(exc, iexc+nsf, 2*corr_pitch, 80);
   else
      interp_pitch(exc, iexc+nsf, -corr_pitch, 80);

729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
#ifdef FIXED_POINT
   for (i=0;i<nsf;i++)
   {
      if (ABS16(exc[i])>16383)
      {
         scaledown = 1;
         break;
      }
   }
   if (scaledown)
   {
      for (i=0;i<nsf;i++)
         exc[i] = SHR16(exc[i],1);
      for (i=0;i<2*nsf;i++)
         iexc[i] = SHR16(iexc[i],1);
   }
#endif
746
   /*interp_pitch(exc, iexc+2*nsf, 2*corr_pitch, 80);*/
Tristan Matthews's avatar
Tristan Matthews committed
747

jm's avatar
jm committed
748
   /*printf ("%d %d %f\n", pitch, corr_pitch, max_corr*ener_1);*/
749
750
751
752
   iexc0_mag = spx_sqrt(1000+inner_prod(iexc,iexc,nsf));
   iexc1_mag = spx_sqrt(1000+inner_prod(iexc+nsf,iexc+nsf,nsf));
   exc_mag = spx_sqrt(1+inner_prod(exc,exc,nsf));
   corr0  = inner_prod(iexc,exc,nsf);
jm's avatar
jm committed
753
754
   if (corr0<0)
      corr0=0;
755
   corr1 = inner_prod(iexc+nsf,exc,nsf);
jm's avatar
jm committed
756
757
758
759
   if (corr1<0)
      corr1=0;
#ifdef FIXED_POINT
   /* Doesn't cost much to limit the ratio and it makes the rest easier */
jm's avatar
jm committed
760
761
762
763
   if (SHL32(EXTEND32(iexc0_mag),6) < EXTEND32(exc_mag))
      iexc0_mag = ADD16(1,PSHR16(exc_mag,6));
   if (SHL32(EXTEND32(iexc1_mag),6) < EXTEND32(exc_mag))
      iexc1_mag = ADD16(1,PSHR16(exc_mag,6));
jm's avatar
jm committed
764
765
766
767
#endif
   if (corr0 > MULT16_16(iexc0_mag,exc_mag))
      pgain1 = QCONST16(1., 14);
   else
768
      pgain1 = PDIV32_16(SHL32(PDIV32(corr0, exc_mag),14),iexc0_mag);
jm's avatar
jm committed
769
770
771
   if (corr1 > MULT16_16(iexc1_mag,exc_mag))
      pgain2 = QCONST16(1., 14);
   else
772
773
774
      pgain2 = PDIV32_16(SHL32(PDIV32(corr1, exc_mag),14),iexc1_mag);
   gg1 = PDIV32_16(SHL32(EXTEND32(exc_mag),8), iexc0_mag);
   gg2 = PDIV32_16(SHL32(EXTEND32(exc_mag),8), iexc1_mag);
jm's avatar
jm committed
775
776
   if (comb_gain>0)
   {
777
#ifdef FIXED_POINT
jm's avatar
jm committed
778
      c1 = (MULT16_16_Q15(QCONST16(.4,15),comb_gain)+QCONST16(.07,15));
779
      c2 = QCONST16(.5,15)+MULT16_16_Q14(QCONST16(1.72,14),(c1-QCONST16(.07,15)));
780
#else
jm's avatar
jm committed
781
782
      c1 = .4*comb_gain+.07;
      c2 = .5+1.72*(c1-.07);
jm's avatar
jm committed
783
#endif
Tristan Matthews's avatar
Tristan Matthews committed
784
   } else
jm's avatar
jm committed
785
786
787
   {
      c1=c2=0;
   }
788
#ifdef FIXED_POINT
789
790
   g1 = 32767 - MULT16_16_Q13(MULT16_16_Q15(c2, pgain1),pgain1);
   g2 = 32767 - MULT16_16_Q13(MULT16_16_Q15(c2, pgain2),pgain2);
791
792
793
794
#else
   g1 = 1-c2*pgain1*pgain1;
   g2 = 1-c2*pgain2*pgain2;
#endif
jm's avatar
jm committed
795
796
797
798
   if (g1<c1)
      g1 = c1;
   if (g2<c1)
      g2 = c1;
799
800
801
   g1 = (spx_word16_t)PDIV32_16(SHL32(EXTEND32(c1),14),(spx_word16_t)g1);
   g2 = (spx_word16_t)PDIV32_16(SHL32(EXTEND32(c1),14),(spx_word16_t)g2);
   if (corr_pitch>max_pitch)
jm's avatar
jm committed
802
   {
jm's avatar
jm committed
803
804
      gain0 = MULT16_16_Q15(QCONST16(.7,15),MULT16_16_Q14(g1,gg1));
      gain1 = MULT16_16_Q15(QCONST16(.3,15),MULT16_16_Q14(g2,gg2));
jm's avatar
jm committed
805
   } else {
jm's avatar
jm committed
806
807
      gain0 = MULT16_16_Q15(QCONST16(.6,15),MULT16_16_Q14(g1,gg1));
      gain1 = MULT16_16_Q15(QCONST16(.6,15),MULT16_16_Q14(g2,gg2));
jm's avatar
jm committed
808
   }
809
810
   for (i=0;i<nsf;i++)
      new_exc[i] = ADD16(exc[i], EXTRACT16(PSHR32(ADD32(MULT16_16(gain0,iexc[i]), MULT16_16(gain1,iexc[i+nsf])),8)));
811
   /* FIXME: compute_rms16 is currently not quite accurate enough (but close) */
812
   new_ener = compute_rms16(new_exc, nsf);
813
   old_ener = compute_rms16(exc, nsf);
Tristan Matthews's avatar
Tristan Matthews committed
814

815
816
817
818
   if (old_ener < 1)
      old_ener = 1;
   if (new_ener < 1)
      new_ener = 1;
819
820
   if (old_ener > new_ener)
      old_ener = new_ener;
821
   ngain = PDIV32_16(SHL32(EXTEND32(old_ener),14),new_ener);
Tristan Matthews's avatar
Tristan Matthews committed
822

jm's avatar
jm committed
823
   for (i=0;i<nsf;i++)
824
      new_exc[i] = MULT16_16_Q14(ngain, new_exc[i]);
825
826
827
828
829
830
831
832
833
#ifdef FIXED_POINT
   if (scaledown)
   {
      for (i=0;i<nsf;i++)
         exc[i] = SHL16(exc[i],1);
      for (i=0;i<nsf;i++)
         new_exc[i] = SHL16(SATURATE16(new_exc[i],16383),1);
   }
#endif
jm's avatar
jm committed
834
}
jm's avatar
jm committed
835

836
#endif /* DISABLE_DECODER */