ltp.c 24.1 KB
Newer Older
Tristan Matthews's avatar
Tristan Matthews committed
1
/* Copyright (C) 2002-2006 Jean-Marc Valin
2
   File: ltp.c
3
   Long-Term Prediction functions
jmvalin's avatar
jmvalin committed
4

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

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

jm's avatar
cleanup  
jm committed
44 45 46 47
#ifndef NULL
#define NULL 0
#endif

jm's avatar
jm committed
48

49 50
#ifdef _USE_SSE
#include "ltp_sse.h"
51 52
#elif defined (ARM4_ASM) || defined(ARM5E_ASM)
#include "ltp_arm4.h"
53 54
#elif defined (BFIN_ASM)
#include "ltp_bfin.h"
jm's avatar
jm committed
55
#endif
56

jm's avatar
jm committed
57
#ifndef OVERRIDE_INNER_PROD
jm's avatar
jm committed
58
spx_word32_t inner_prod(const spx_word16_t *x, const spx_word16_t *y, int len)
59 60
{
   spx_word32_t sum=0;
jm's avatar
jm committed
61 62
   len >>= 2;
   while(len--)
63 64
   {
      spx_word32_t part=0;
jm's avatar
jm committed
65 66 67 68 69
      part = MAC16_16(part,*x++,*y++);
      part = MAC16_16(part,*x++,*y++);
      part = MAC16_16(part,*x++,*y++);
      part = MAC16_16(part,*x++,*y++);
      /* HINT: If you had a 40-bit accumulator, you could shift only at the end */
jm's avatar
jm committed
70
      sum = ADD32(sum,SHR32(part,6));
71 72 73
   }
   return sum;
}
jm's avatar
jm committed
74
#endif
75

76 77
#ifndef DISABLE_ENCODER

jm's avatar
jm committed
78
#ifndef OVERRIDE_PITCH_XCORR
jm's avatar
jm committed
79
#if 0 /* HINT: Enable this for machines with enough registers (i.e. not x86) */
80
static void pitch_xcorr(const spx_word16_t *_x, const spx_word16_t *_y, spx_word32_t *corr, int len, int nb_pitch, char *stack)
81 82 83 84 85
{
   int i,j;
   for (i=0;i<nb_pitch;i+=4)
   {
      /* Compute correlation*/
jm's avatar
jm committed
86
      /*corr[nb_pitch-1-i]=inner_prod(x, _y+i, len);*/
87 88 89 90 91 92 93
      spx_word32_t sum1=0;
      spx_word32_t sum2=0;
      spx_word32_t sum3=0;
      spx_word32_t sum4=0;
      const spx_word16_t *y = _y+i;
      const spx_word16_t *x = _x;
      spx_word16_t y0, y1, y2, y3;
jm's avatar
jm committed
94
      /*y0=y[0];y1=y[1];y2=y[2];y3=y[3];*/
95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128
      y0=*y++;
      y1=*y++;
      y2=*y++;
      y3=*y++;
      for (j=0;j<len;j+=4)
      {
         spx_word32_t part1;
         spx_word32_t part2;
         spx_word32_t part3;
         spx_word32_t part4;
         part1 = MULT16_16(*x,y0);
         part2 = MULT16_16(*x,y1);
         part3 = MULT16_16(*x,y2);
         part4 = MULT16_16(*x,y3);
         x++;
         y0=*y++;
         part1 = MAC16_16(part1,*x,y1);
         part2 = MAC16_16(part2,*x,y2);
         part3 = MAC16_16(part3,*x,y3);
         part4 = MAC16_16(part4,*x,y0);
         x++;
         y1=*y++;
         part1 = MAC16_16(part1,*x,y2);
         part2 = MAC16_16(part2,*x,y3);
         part3 = MAC16_16(part3,*x,y0);
         part4 = MAC16_16(part4,*x,y1);
         x++;
         y2=*y++;
         part1 = MAC16_16(part1,*x,y3);
         part2 = MAC16_16(part2,*x,y0);
         part3 = MAC16_16(part3,*x,y1);
         part4 = MAC16_16(part4,*x,y2);
         x++;
         y3=*y++;
Tristan Matthews's avatar
Tristan Matthews committed
129

jm's avatar
jm committed
130 131 132 133
         sum1 = ADD32(sum1,SHR32(part1,6));
         sum2 = ADD32(sum2,SHR32(part2,6));
         sum3 = ADD32(sum3,SHR32(part3,6));
         sum4 = ADD32(sum4,SHR32(part4,6));
134 135 136 137 138 139 140 141 142
      }
      corr[nb_pitch-1-i]=sum1;
      corr[nb_pitch-2-i]=sum2;
      corr[nb_pitch-3-i]=sum3;
      corr[nb_pitch-4-i]=sum4;
   }

}
#else
143
static void pitch_xcorr(const spx_word16_t *_x, const spx_word16_t *_y, spx_word32_t *corr, int len, int nb_pitch, char *stack)
144 145 146 147 148 149 150 151 152
{
   int i;
   for (i=0;i<nb_pitch;i++)
   {
      /* Compute correlation*/
      corr[nb_pitch-1-i]=inner_prod(_x, _y+i, len);
   }

}
153
#endif
154
#endif
155

jm's avatar
jm committed
156
#ifndef OVERRIDE_COMPUTE_PITCH_ERROR
157
static inline spx_word32_t compute_pitch_error(spx_word16_t *C, spx_word16_t *g, spx_word16_t pitch_control)
158 159
{
   spx_word32_t sum = 0;
160 161 162 163 164 165 166 167 168
   sum = ADD32(sum,MULT16_16(MULT16_16_16(g[0],pitch_control),C[0]));
   sum = ADD32(sum,MULT16_16(MULT16_16_16(g[1],pitch_control),C[1]));
   sum = ADD32(sum,MULT16_16(MULT16_16_16(g[2],pitch_control),C[2]));
   sum = SUB32(sum,MULT16_16(MULT16_16_16(g[0],g[1]),C[3]));
   sum = SUB32(sum,MULT16_16(MULT16_16_16(g[2],g[1]),C[4]));
   sum = SUB32(sum,MULT16_16(MULT16_16_16(g[2],g[0]),C[5]));
   sum = SUB32(sum,MULT16_16(MULT16_16_16(g[0],g[0]),C[6]));
   sum = SUB32(sum,MULT16_16(MULT16_16_16(g[1],g[1]),C[7]));
   sum = SUB32(sum,MULT16_16(MULT16_16_16(g[2],g[2]),C[8]));
169 170
   return sum;
}
jm's avatar
jm committed
171 172
#endif

173
#ifndef OVERRIDE_OPEN_LOOP_NBEST_PITCH
174
void open_loop_nbest_pitch(spx_word16_t *sw, int start, int end, int len, int *pitch, spx_word16_t *gain, int N, char *stack)
175 176
{
   int i,j,k;
177
   VARDECL(spx_word32_t *best_score);
178
   VARDECL(spx_word32_t *best_ener);
179
   spx_word32_t e0;
180
   VARDECL(spx_word32_t *corr);
181
#ifdef FIXED_POINT
Tristan Matthews's avatar
Tristan Matthews committed
182
   /* In fixed-point, we need only one (temporary) array of 32-bit values and two (corr16, ener16)
183 184 185 186 187
      arrays for (normalized) 16-bit values */
   VARDECL(spx_word16_t *corr16);
   VARDECL(spx_word16_t *ener16);
   spx_word32_t *energy;
   int cshift=0, eshift=0;
188
   int scaledown = 0;
189 190 191 192 193 194 195 196 197 198 199 200 201
   ALLOC(corr16, end-start+1, spx_word16_t);
   ALLOC(ener16, end-start+1, spx_word16_t);
   ALLOC(corr, end-start+1, spx_word32_t);
   energy = corr;
#else
   /* In floating-point, we need to float arrays and no normalized copies */
   VARDECL(spx_word32_t *energy);
   spx_word16_t *corr16;
   spx_word16_t *ener16;
   ALLOC(energy, end-start+2, spx_word32_t);
   ALLOC(corr, end-start+1, spx_word32_t);
   corr16 = corr;
   ener16 = energy;
202
#endif
Tristan Matthews's avatar
Tristan Matthews committed
203

204
   ALLOC(best_score, N, spx_word32_t);
205
   ALLOC(best_ener, N, spx_word32_t);
206 207 208
   for (i=0;i<N;i++)
   {
        best_score[i]=-1;
209
        best_ener[i]=0;
210
        pitch[i]=start;
211
   }
Tristan Matthews's avatar
Tristan Matthews committed
212

213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228
#ifdef FIXED_POINT
   for (i=-end;i<len;i++)
   {
      if (ABS16(sw[i])>16383)
      {
         scaledown=1;
         break;
      }
   }
   /* If the weighted input is close to saturation, then we scale it down */
   if (scaledown)
   {
      for (i=-end;i<len;i++)
      {
         sw[i]=SHR16(sw[i],1);
      }
Tristan Matthews's avatar
Tristan Matthews committed
229
   }
230
#endif
231 232
   energy[0]=inner_prod(sw-start, sw-start, len);
   e0=inner_prod(sw, sw, len);
233
   for (i=start;i<end;i++)
234 235
   {
      /* Update energy for next pitch*/
236
      energy[i-start+1] = SUB32(ADD32(energy[i-start],SHR32(MULT16_16(sw[-i-1],sw[-i-1]),6)), SHR32(MULT16_16(sw[-i+len-1],sw[-i+len-1]),6));
237 238
      if (energy[i-start+1] < 0)
         energy[i-start+1] = 0;
239
   }
Tristan Matthews's avatar
Tristan Matthews committed
240

241 242 243
#ifdef FIXED_POINT
   eshift = normalize16(energy, ener16, 32766, end-start+1);
#endif
Tristan Matthews's avatar
Tristan Matthews committed
244

245
   /* In fixed-point, this actually overrites the energy array (aliased to corr) */
246
   pitch_xcorr(sw, sw-end, corr, len, end-start+1, stack);
Tristan Matthews's avatar
Tristan Matthews committed
247

248
#ifdef FIXED_POINT
249 250
   /* Normalize to 180 so we can square it and it still fits in 16 bits */
   cshift = normalize16(corr, corr16, 180, end-start+1);
251 252 253 254 255 256 257
   /* If we scaled weighted input down, we need to scale it up again (OK, so we've just lost the LSB, who cares?) */
   if (scaledown)
   {
      for (i=-end;i<len;i++)
      {
         sw[i]=SHL16(sw[i],1);
      }
Tristan Matthews's avatar
Tristan Matthews committed
258
   }
259
#endif
260

261 262
   /* Search for the best pitch prediction gain */
   for (i=start;i<=end;i++)
263
   {
264 265 266
      spx_word16_t tmp = MULT16_16_16(corr16[i-start],corr16[i-start]);
      /* Instead of dividing the tmp by the energy, we multiply on the other side */
      if (MULT16_16(tmp,best_ener[N-1])>MULT16_16(best_score[N-1],ADD16(1,ener16[i-start])))
267
      {
268 269 270 271 272 273
         /* We can safely put it last and then check */
         best_score[N-1]=tmp;
         best_ener[N-1]=ener16[i-start]+1;
         pitch[N-1]=i;
         /* Check if it comes in front of others */
         for (j=0;j<N-1;j++)
jm's avatar
jm committed
274
         {
275
            if (MULT16_16(tmp,best_ener[j])>MULT16_16(best_score[j],ADD16(1,ener16[i-start])))
276
            {
277
               for (k=N-1;k>j;k--)
278
               {
279 280 281
                  best_score[k]=best_score[k-1];
                  best_ener[k]=best_ener[k-1];
                  pitch[k]=pitch[k-1];
282
               }
283 284 285 286
               best_score[j]=tmp;
               best_ener[j]=ener16[i-start]+1;
               pitch[j]=i;
               break;
287
            }
jm's avatar
jm committed
288
         }
289 290
      }
   }
Tristan Matthews's avatar
Tristan Matthews committed
291

292
   /* Compute open-loop gain if necessary */
jm's avatar
jm committed
293
   if (gain)
294
   {
295 296 297 298 299 300 301 302 303 304
      for (j=0;j<N;j++)
      {
         spx_word16_t g;
         i=pitch[j];
         g = DIV32(SHL32(EXTEND32(corr16[i-start]),cshift), 10+SHR32(MULT16_16(spx_sqrt(e0),spx_sqrt(SHL32(EXTEND32(ener16[i-start]),eshift))),6));
         /* FIXME: g = max(g,corr/energy) */
         if (g<0)
            g = 0;
         gain[j]=g;
      }
305
   }
306 307


308
}
309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328
#endif

#ifndef OVERRIDE_PITCH_GAIN_SEARCH_3TAP_VQ
static int pitch_gain_search_3tap_vq(
  const signed char *gain_cdbk,
  int                gain_cdbk_size,
  spx_word16_t      *C16,
  spx_word16_t       max_gain
)
{
  const signed char *ptr=gain_cdbk;
  int                best_cdbk=0;
  spx_word32_t       best_sum=-VERY_LARGE32;
  spx_word32_t       sum=0;
  spx_word16_t       g[3];
  spx_word16_t       pitch_control=64;
  spx_word16_t       gain_sum;
  int                i;

  for (i=0;i<gain_cdbk_size;i++) {
Tristan Matthews's avatar
Tristan Matthews committed
329

330 331 332 333 334
    ptr = gain_cdbk+4*i;
    g[0]=ADD16((spx_word16_t)ptr[0],32);
    g[1]=ADD16((spx_word16_t)ptr[1],32);
    g[2]=ADD16((spx_word16_t)ptr[2],32);
    gain_sum = (spx_word16_t)ptr[3];
Tristan Matthews's avatar
Tristan Matthews committed
335

336
    sum = compute_pitch_error(C16, g, pitch_control);
Tristan Matthews's avatar
Tristan Matthews committed
337

338 339 340 341 342
    if (sum>best_sum && gain_sum<=max_gain) {
      best_sum=sum;
      best_cdbk=i;
    }
  }
343

344 345 346
  return best_cdbk;
}
#endif
347

348
/** Finds the best quantized 3-tap pitch predictor by analysis by synthesis */
349
static spx_word32_t pitch_gain_search_3tap(
350
const spx_word16_t target[],       /* Target vector */
351 352 353 354
const spx_coef_t ak[],          /* LPCs for this subframe */
const spx_coef_t awk1[],        /* Weighted LPCs #1 for this subframe */
const spx_coef_t awk2[],        /* Weighted LPCs #2 for this subframe */
spx_sig_t exc[],                /* Excitation */
355 356
const signed char *gain_cdbk,
int gain_cdbk_size,
357
int   pitch,                    /* Pitch value */
358
int   p,                        /* Number of LPC coeffs */
359
int   nsf,                      /* Number of samples in subframe */
360
SpeexBits *bits,
361
char *stack,
362
const spx_word16_t *exc2,
363
const spx_word16_t *r,
364
spx_word16_t *new_target,
365
int  *cdbk_index,
366
int plc_tuning,
367 368
spx_word32_t cumul_gain,
int scaledown
369
)
jmvalin's avatar
jmvalin committed
370
{
371
   int i,j;
372 373 374
   VARDECL(spx_word16_t *tmp1);
   VARDECL(spx_word16_t *e);
   spx_word16_t *x[3];
375 376
   spx_word32_t corr[3];
   spx_word32_t A[3][3];
jm's avatar
cleanup  
jm committed
377
   spx_word16_t gain[3];
378
   spx_word32_t err;
379
   spx_word16_t max_gain=128;
380
   int          best_cdbk=0;
381

382 383
   ALLOC(tmp1, 3*nsf, spx_word16_t);
   ALLOC(e, nsf, spx_word16_t);
jmvalin's avatar
jmvalin committed
384

385 386
   if (cumul_gain > 262144)
      max_gain = 31;
Tristan Matthews's avatar
Tristan Matthews committed
387

388 389 390
   x[0]=tmp1;
   x[1]=tmp1+nsf;
   x[2]=tmp1+2*nsf;
Tristan Matthews's avatar
Tristan Matthews committed
391

392 393 394
   for (j=0;j<nsf;j++)
      new_target[j] = target[j];

jmvalin's avatar
jmvalin committed
395
   {
396
      int bound;
jm's avatar
C89 fix  
jm committed
397
      VARDECL(spx_mem_t *mm);
398
      int pp=pitch-1;
jm's avatar
jm committed
399
      ALLOC(mm, p, spx_mem_t);
400 401 402 403 404 405 406 407 408 409 410 411
      bound = nsf;
      if (nsf-pp>0)
         bound = pp;
      for (j=0;j<bound;j++)
         e[j]=exc2[j-pp];
      bound = nsf;
      if (nsf-pp-pitch>0)
         bound = pp+pitch;
      for (;j<bound;j++)
         e[j]=exc2[j-pp-pitch];
      for (;j<nsf;j++)
         e[j]=0;
412 413 414 415 416 417 418 419 420 421
#ifdef FIXED_POINT
      /* Scale target and excitation down if needed (avoiding overflow) */
      if (scaledown)
      {
         for (j=0;j<nsf;j++)
            e[j] = SHR16(e[j],1);
         for (j=0;j<nsf;j++)
            new_target[j] = SHR16(new_target[j],1);
      }
#endif
422 423
      for (j=0;j<p;j++)
         mm[j] = 0;
424
      iir_mem16(e, ak, e, nsf, p, mm, stack);
425 426
      for (j=0;j<p;j++)
         mm[j] = 0;
427
      filter10(e, awk1, awk2, e, nsf, mm, stack);
428 429
      for (j=0;j<nsf;j++)
         x[2][j] = e[j];
430
   }
431
   for (i=1;i>=0;i--)
432
   {
433
      spx_word16_t e0=exc2[-pitch-1+i];
434 435 436 437 438
#ifdef FIXED_POINT
      /* Scale excitation down if needed (avoiding overflow) */
      if (scaledown)
         e0 = SHR16(e0,1);
#endif
439 440 441
      x[i][0]=MULT16_16_Q14(r[0], e0);
      for (j=0;j<nsf-1;j++)
         x[i][j+1]=ADD32(x[i+1][j],MULT16_16_P14(r[j+1], e0));
442
   }
443 444

   for (i=0;i<3;i++)
445
      corr[i]=inner_prod(x[i],new_target,nsf);
446 447 448
   for (i=0;i<3;i++)
      for (j=0;j<=i;j++)
         A[i][j]=A[j][i]=inner_prod(x[i],x[j],nsf);
449

450
   {
451
      spx_word32_t C[9];
452 453 454 455
#ifdef FIXED_POINT
      spx_word16_t C16[9];
#else
      spx_word16_t *C16=C;
Tristan Matthews's avatar
Tristan Matthews committed
456
#endif
457 458 459 460 461
      C[0]=corr[2];
      C[1]=corr[1];
      C[2]=corr[0];
      C[3]=A[1][2];
      C[4]=A[0][1];
Tristan Matthews's avatar
Tristan Matthews committed
462
      C[5]=A[0][2];
463 464 465
      C[6]=A[2][2];
      C[7]=A[1][1];
      C[8]=A[0][0];
Tristan Matthews's avatar
Tristan Matthews committed
466

467 468 469
      /*plc_tuning *= 2;*/
      if (plc_tuning<2)
         plc_tuning=2;
470 471
      if (plc_tuning>30)
         plc_tuning=30;
472
#ifdef FIXED_POINT
473 474 475 476 477 478
      C[0] = SHL32(C[0],1);
      C[1] = SHL32(C[1],1);
      C[2] = SHL32(C[2],1);
      C[3] = SHL32(C[3],1);
      C[4] = SHL32(C[4],1);
      C[5] = SHL32(C[5],1);
479 480 481
      C[6] = MAC16_32_Q15(C[6],MULT16_16_16(plc_tuning,655),C[6]);
      C[7] = MAC16_32_Q15(C[7],MULT16_16_16(plc_tuning,655),C[7]);
      C[8] = MAC16_32_Q15(C[8],MULT16_16_16(plc_tuning,655),C[8]);
482
      normalize16(C, C16, 32767, 9);
483
#else
484 485 486
      C[6]*=.5*(1+.02*plc_tuning);
      C[7]*=.5*(1+.02*plc_tuning);
      C[8]*=.5*(1+.02*plc_tuning);
487
#endif
488 489 490

      best_cdbk = pitch_gain_search_3tap_vq(gain_cdbk, gain_cdbk_size, C16, max_gain);

jm's avatar
cleanup  
jm committed
491
#ifdef FIXED_POINT
492 493 494
      gain[0] = ADD16(32,(spx_word16_t)gain_cdbk[best_cdbk*4]);
      gain[1] = ADD16(32,(spx_word16_t)gain_cdbk[best_cdbk*4+1]);
      gain[2] = ADD16(32,(spx_word16_t)gain_cdbk[best_cdbk*4+2]);
495
      /*printf ("%d %d %d %d\n",gain[0],gain[1],gain[2], best_cdbk);*/
jm's avatar
cleanup  
jm committed
496
#else
497 498 499
      gain[0] = 0.015625*gain_cdbk[best_cdbk*4]  + .5;
      gain[1] = 0.015625*gain_cdbk[best_cdbk*4+1]+ .5;
      gain[2] = 0.015625*gain_cdbk[best_cdbk*4+2]+ .5;
jm's avatar
cleanup  
jm committed
500
#endif
501
      *cdbk_index=best_cdbk;
502
   }
503

504
   SPEEX_MEMSET(exc, 0, nsf);
505 506 507 508 509 510 511 512 513 514 515 516 517 518 519 520
   for (i=0;i<3;i++)
   {
      int j;
      int tmp1, tmp3;
      int pp=pitch+1-i;
      tmp1=nsf;
      if (tmp1>pp)
         tmp1=pp;
      for (j=0;j<tmp1;j++)
         exc[j]=MAC16_16(exc[j],SHL16(gain[2-i],7),exc2[j-pp]);
      tmp3=nsf;
      if (tmp3>pp+pitch)
         tmp3=pp+pitch;
      for (j=tmp1;j<tmp3;j++)
         exc[j]=MAC16_16(exc[j],SHL16(gain[2-i],7),exc2[j-pp-pitch]);
   }
521
   for (i=0;i<nsf;i++)
522
   {
523 524
      spx_word32_t tmp = ADD32(ADD32(MULT16_16(gain[0],x[2][i]),MULT16_16(gain[1],x[1][i])),
                            MULT16_16(gain[2],x[0][i]));
525
      new_target[i] = SUB16(new_target[i], EXTRACT16(PSHR32(tmp,6)));
526
   }
527
   err = inner_prod(new_target, new_target, nsf);
528

529
   return err;
530 531 532 533
}

/** Finds the best quantized 3-tap pitch predictor by analysis by synthesis */
int pitch_search_3tap(
534
spx_word16_t target[],                 /* Target vector */
535
spx_word16_t *sw,
536 537 538
spx_coef_t ak[],                     /* LPCs for this subframe */
spx_coef_t awk1[],                   /* Weighted LPCs #1 for this subframe */
spx_coef_t awk2[],                   /* Weighted LPCs #2 for this subframe */
539
spx_sig_t exc[],                    /* Excitation */
jm's avatar
jm committed
540
const void *par,
541 542
int   start,                    /* Smallest pitch value allowed */
int   end,                      /* Largest pitch value allowed */
jm's avatar
jm committed
543
spx_word16_t pitch_coef,               /* Voicing (pitch) coefficient */
544 545
int   p,                        /* Number of LPC coeffs */
int   nsf,                      /* Number of samples in subframe */
546
SpeexBits *bits,
547
char *stack,
548
spx_word16_t *exc2,
549
spx_word16_t *r,
550
int complexity,
551
int cdbk_offset,
552 553
int plc_tuning,
spx_word32_t *cumul_gain
554 555
)
{
jm's avatar
jm committed
556
   int i;
jmvalin's avatar
jmvalin committed
557
   int cdbk_index, pitch=0, best_gain_index=0;
558
   VARDECL(spx_sig_t *best_exc);
559 560
   VARDECL(spx_word16_t *new_target);
   VARDECL(spx_word16_t *best_target);
561
   int best_pitch=0;
562
   spx_word32_t err, best_err=-1;
563
   int N;
564
   const ltp_params *params;
565 566
   const signed char *gain_cdbk;
   int   gain_cdbk_size;
567
   int scaledown=0;
Tristan Matthews's avatar
Tristan Matthews committed
568

569
   VARDECL(int *nbest);
Tristan Matthews's avatar
Tristan Matthews committed
570

571 572 573
   params = (const ltp_params*) par;
   gain_cdbk_size = 1<<params->gain_bits;
   gain_cdbk = params->gain_cdbk + 4*gain_cdbk_size*cdbk_offset;
Tristan Matthews's avatar
Tristan Matthews committed
574

jmvalin's avatar
jmvalin committed
575 576 577
   N=complexity;
   if (N>10)
      N=10;
578 579
   if (N<1)
      N=1;
580

581
   ALLOC(nbest, N, int);
582
   params = (const ltp_params*) par;
583

584
   if (end<start)
585 586 587
   {
      speex_bits_pack(bits, 0, params->pitch_bits);
      speex_bits_pack(bits, 0, params->gain_bits);
588
      SPEEX_MEMSET(exc, 0, nsf);
589 590
      return start;
   }
Tristan Matthews's avatar
Tristan Matthews committed
591

592 593 594 595 596 597 598 599 600 601
#ifdef FIXED_POINT
   /* Check if we need to scale everything down in the pitch search to avoid overflows */
   for (i=0;i<nsf;i++)
   {
      if (ABS16(target[i])>16383)
      {
         scaledown=1;
         break;
      }
   }
602
   for (i=-end;i<0;i++)
603 604 605 606 607 608 609
   {
      if (ABS16(exc2[i])>16383)
      {
         scaledown=1;
         break;
      }
   }
610
#endif
611 612
   if (N>end-start+1)
      N=end-start+1;
613 614 615 616
   if (end != start)
      open_loop_nbest_pitch(sw, start, end, nsf, nbest, NULL, N, stack);
   else
      nbest[0] = start;
Tristan Matthews's avatar
Tristan Matthews committed
617

618
   ALLOC(best_exc, nsf, spx_sig_t);
619 620
   ALLOC(new_target, nsf, spx_word16_t);
   ALLOC(best_target, nsf, spx_word16_t);
Tristan Matthews's avatar
Tristan Matthews committed
621

622
   for (i=0;i<N;i++)
623
   {
624
      pitch=nbest[i];
625
      SPEEX_MEMSET(exc, 0, nsf);
626
      err=pitch_gain_search_3tap(target, ak, awk1, awk2, exc, gain_cdbk, gain_cdbk_size, pitch, p, nsf,
627
                                 bits, stack, exc2, r, new_target, &cdbk_index, plc_tuning, *cumul_gain, scaledown);
628 629
      if (err<best_err || best_err<0)
      {
630 631
         SPEEX_COPY(best_exc, exc, nsf);
         SPEEX_COPY(best_target, new_target, nsf);
632 633
         best_err=err;
         best_pitch=pitch;
634
         best_gain_index=cdbk_index;
635 636
      }
   }
637
   /*printf ("pitch: %d %d\n", best_pitch, best_gain_index);*/
638 639
   speex_bits_pack(bits, best_pitch-start, params->pitch_bits);
   speex_bits_pack(bits, best_gain_index, params->gain_bits);
640 641 642 643 644 645
#ifdef FIXED_POINT
   *cumul_gain = MULT16_32_Q13(SHL16(params->gain_cdbk[4*best_gain_index+3],8), MAX32(1024,*cumul_gain));
#else
   *cumul_gain = 0.03125*MAX32(1024,*cumul_gain)*params->gain_cdbk[4*best_gain_index+3];
#endif
   /*printf ("%f\n", cumul_gain);*/
jmvalin's avatar
jmvalin committed
646
   /*printf ("encode pitch: %d %d\n", best_pitch, best_gain_index);*/
647 648
   SPEEX_COPY(exc, best_exc, nsf);
   SPEEX_COPY(target, best_target, nsf);
649 650 651 652 653 654 655 656
#ifdef FIXED_POINT
   /* Scale target back up if needed */
   if (scaledown)
   {
      for (i=0;i<nsf;i++)
         target[i]=SHL16(target[i],1);
   }
#endif
657
   return pitch;
658
}
659
#endif /* DISABLE_ENCODER */
jmvalin's avatar
jmvalin committed
660

661
#ifndef DISABLE_DECODER
jmvalin's avatar
jmvalin committed
662
void pitch_unquant_3tap(
663 664
spx_word16_t exc[],             /* Input excitation */
spx_word32_t exc_out[],         /* Output excitation */
jmvalin's avatar
jmvalin committed
665 666
int   start,                    /* Smallest pitch value allowed */
int   end,                      /* Largest pitch value allowed */
667
spx_word16_t pitch_coef,        /* Voicing (pitch) coefficient */
jm's avatar
jm committed
668
const void *par,
jmvalin's avatar
jmvalin committed
669
int   nsf,                      /* Number of samples in subframe */
670
int *pitch_val,
671
spx_word16_t *gain_val,
672
SpeexBits *bits,
673
char *stack,
674 675
int count_lost,
int subframe_offset,
jm's avatar
jm committed
676
spx_word16_t last_pitch_gain,
677 678
int cdbk_offset
)
jmvalin's avatar
jmvalin committed
679 680 681 682
{
   int i;
   int pitch;
   int gain_index;
683
   spx_word16_t gain[3];
jm's avatar
jm committed
684
   const signed char *gain_cdbk;
685
   int gain_cdbk_size;
686
   const ltp_params *params;
687

688
   params = (const ltp_params*) par;
689
   gain_cdbk_size = 1<<params->gain_bits;
690
   gain_cdbk = params->gain_cdbk + 4*gain_cdbk_size*cdbk_offset;
691

692
   pitch = speex_bits_unpack_unsigned(bits, params->pitch_bits);
jmvalin's avatar
jmvalin committed
693
   pitch += start;
694
   gain_index = speex_bits_unpack_unsigned(bits, params->gain_bits);
jmvalin's avatar
jmvalin committed
695
   /*printf ("decode pitch: %d %d\n", pitch, gain_index);*/
jm's avatar
...  
jm committed
696
#ifdef FIXED_POINT
697 698 699
   gain[0] = ADD16(32,(spx_word16_t)gain_cdbk[gain_index*4]);
   gain[1] = ADD16(32,(spx_word16_t)gain_cdbk[gain_index*4+1]);
   gain[2] = ADD16(32,(spx_word16_t)gain_cdbk[gain_index*4+2]);
jm's avatar
...  
jm committed
700
#else
701 702 703
   gain[0] = 0.015625*gain_cdbk[gain_index*4]+.5;
   gain[1] = 0.015625*gain_cdbk[gain_index*4+1]+.5;
   gain[2] = 0.015625*gain_cdbk[gain_index*4+2]+.5;
jm's avatar
...  
jm committed
704
#endif
705 706

   if (count_lost && pitch > subframe_offset)
707
   {
708
      spx_word16_t gain_sum;
709
      if (1) {
710 711 712 713 714 715
#ifdef FIXED_POINT
         spx_word16_t tmp = count_lost < 4 ? last_pitch_gain : SHR16(last_pitch_gain,1);
         if (tmp>62)
            tmp=62;
#else
         spx_word16_t tmp = count_lost < 4 ? last_pitch_gain : 0.5 * last_pitch_gain;
716 717
         if (tmp>.95)
            tmp=.95;
718 719
#endif
         gain_sum = gain_3tap_to_1tap(gain);
720

721 722 723 724 725 726
         if (gain_sum > tmp)
         {
            spx_word16_t fact = DIV32_16(SHL32(EXTEND32(tmp),14),gain_sum);
            for (i=0;i<3;i++)
               gain[i]=MULT16_16_Q14(fact,gain[i]);
         }
727 728 729 730

      }

   }
731 732

   *pitch_val = pitch;
733 734 735
   gain_val[0]=gain[0];
   gain_val[1]=gain[1];
   gain_val[2]=gain[2];
736 737 738
   gain[0] = SHL16(gain[0],7);
   gain[1] = SHL16(gain[1],7);
   gain[2] = SHL16(gain[2],7);
739
   SPEEX_MEMSET(exc_out, 0, nsf);
740
   for (i=0;i<3;i++)
741
   {
742 743 744 745 746 747 748
      int j;
      int tmp1, tmp3;
      int pp=pitch+1-i;
      tmp1=nsf;
      if (tmp1>pp)
         tmp1=pp;
      for (j=0;j<tmp1;j++)
749
         exc_out[j]=MAC16_16(exc_out[j],gain[2-i],exc[j-pp]);
750 751 752 753
      tmp3=nsf;
      if (tmp3>pp+pitch)
         tmp3=pp+pitch;
      for (j=tmp1;j<tmp3;j++)
754
         exc_out[j]=MAC16_16(exc_out[j],gain[2-i],exc[j-pp-pitch]);
755
   }
756 757
   /*for (i=0;i<nsf;i++)
   exc[i]=PSHR32(exc32[i],13);*/
jmvalin's avatar
jmvalin committed
758
}
759
#endif /* DISABLE_DECODER */
jmvalin's avatar
jmvalin committed
760

761
#ifndef DISABLE_ENCODER
jmvalin's avatar
jmvalin committed
762 763
/** Forced pitch delay and gain */
int forced_pitch_quant(
764
spx_word16_t target[],                 /* Target vector */
765
spx_word16_t *sw,
766 767 768
spx_coef_t ak[],                     /* LPCs for this subframe */
spx_coef_t awk1[],                   /* Weighted LPCs #1 for this subframe */
spx_coef_t awk2[],                   /* Weighted LPCs #2 for this subframe */
769
spx_sig_t exc[],                    /* Excitation */
jm's avatar
jm committed
770
const void *par,
jmvalin's avatar
jmvalin committed
771 772
int   start,                    /* Smallest pitch value allowed */
int   end,                      /* Largest pitch value allowed */
jm's avatar
jm committed
773
spx_word16_t pitch_coef,               /* Voicing (pitch) coefficient */
jmvalin's avatar
jmvalin committed
774 775 776
int   p,                        /* Number of LPC coeffs */
int   nsf,                      /* Number of samples in subframe */
SpeexBits *bits,
777
char *stack,
778
spx_word16_t *exc2,
779
spx_word16_t *r,
780
int complexity,
781
int cdbk_offset,
782 783
int plc_tuning,
spx_word32_t *cumul_gain
jmvalin's avatar
jmvalin committed
784 785 786
)
{
   int i;
787 788
   VARDECL(spx_word16_t *res);
   ALLOC(res, nsf, spx_word16_t);
789 790 791 792 793 794 795
#ifdef FIXED_POINT
   if (pitch_coef>63)
      pitch_coef=63;
#else
   if (pitch_coef>.99)
      pitch_coef=.99;
#endif
796 797 798 799 800
   for (i=0;i<nsf&&i<start;i++)
   {
      exc[i]=MULT16_16(SHL16(pitch_coef, 7),exc2[i-start]);
   }
   for (;i<nsf;i++)
jmvalin's avatar
jmvalin committed
801
   {
802
      exc[i]=MULT16_32_Q15(SHL16(pitch_coef, 9),exc[i-start]);
jmvalin's avatar
jmvalin committed
803
   }
804
   for (i=0;i<nsf;i++)
805 806 807 808
      res[i] = EXTRACT16(PSHR32(exc[i], SIG_SHIFT-1));
   syn_percep_zero16(res, ak, awk1, awk2, res, nsf, p, stack);
   for (i=0;i<<