celt/pitch.c

   1 /* Copyright (c) 2007-2008 CSIRO
   2    Copyright (c) 2007-2009 Xiph.Org Foundation
   3    Written by Jean-Marc Valin */
   4 /**
   5    @file pitch.c
   6    @brief Pitch analysis
   7  */
   8
   9 /*
  10    Redistribution and use in source and binary forms, with or without
  11    modification, are permitted provided that the following conditions
  12    are met:
  13
  14    - Redistributions of source code must retain the above copyright
  15    notice, this list of conditions and the following disclaimer.
  16
  17    - Redistributions in binary form must reproduce the above copyright
  18    notice, this list of conditions and the following disclaimer in the
  19    documentation and/or other materials provided with the distribution.
  20
  21    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  22    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  23    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
  24    A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER
  25    OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
  26    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
  27    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
  28    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
  29    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
  30    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  31    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  32 */
  33
  34 #ifdef HAVE_CONFIG_H
  35 #include "config.h"
  36 #endif
  37
  38 #include "pitch.h"
  39 #include "os_support.h"
  40 #include "modes.h"
  41 #include "stack_alloc.h"
  42 #include "mathops.h"
  43 #include "celt_lpc.h"
  44
  45 static void find_best_pitch(opus_val32 *xcorr, opus_val16 *y, int len,
  46                             int max_pitch, int *best_pitch
  47 #ifdef FIXED_POINT
  48                             , int yshift, opus_val32 maxcorr
  49 #endif
  50                             )
  51 {
  52    int i, j;
  53    opus_val32 Syy=1;
  54    opus_val16 best_num[2];
  55    opus_val32 best_den[2];
  56 #ifdef FIXED_POINT
  57    int xshift;
  58
  59    xshift = celt_ilog2(maxcorr)-14;
  60 #endif
  61
  62    best_num[0] = -1;
  63    best_num[1] = -1;
  64    best_den[0] = 0;
  65    best_den[1] = 0;
  66    best_pitch[0] = 0;
  67    best_pitch[1] = 1;
  68    for (j=0;j<len;j++)
  69       Syy = ADD32(Syy, SHR32(MULT16_16(y[j],y[j]), yshift));
  70    for (i=0;i<max_pitch;i++)
  71    {
  72       if (xcorr[i]>0)
  73       {
  74          opus_val16 num;
  75          opus_val32 xcorr16;
  76          xcorr16 = EXTRACT16(VSHR32(xcorr[i], xshift));
  77 #ifndef FIXED_POINT
  78          /* Considering the range of xcorr16, this should avoid both underflows
  79             and overflows (inf) when squaring xcorr16 */
  80          xcorr16 *= 1e-12f;
  81 #endif
  82          num = MULT16_16_Q15(xcorr16,xcorr16);
  83          if (MULT16_32_Q15(num,best_den[1]) > MULT16_32_Q15(best_num[1],Syy))
  84          {
  85             if (MULT16_32_Q15(num,best_den[0]) > MULT16_32_Q15(best_num[0],Syy))
  86             {
  87                best_num[1] = best_num[0];
  88                best_den[1] = best_den[0];
  89                best_pitch[1] = best_pitch[0];
  90                best_num[0] = num;
  91                best_den[0] = Syy;
  92                best_pitch[0] = i;
  93             } else {
  94                best_num[1] = num;
  95                best_den[1] = Syy;
  96                best_pitch[1] = i;
  97             }
  98          }
  99       }
 100       Syy += SHR32(MULT16_16(y[i+len],y[i+len]),yshift) - SHR32(MULT16_16(y[i],y[i]),yshift);
 101       Syy = MAX32(1, Syy);
 102    }
 103 }
 104
 105 static void celt_fir5(const opus_val16 *x,
 106          const opus_val16 *num,
 107          opus_val16 *y,
 108          int N,
 109          opus_val16 *mem)
 110 {
 111    int i;
 112    opus_val16 num0, num1, num2, num3, num4;
 113    opus_val32 mem0, mem1, mem2, mem3, mem4;
 114    num0=num[0];
 115    num1=num[1];
 116    num2=num[2];
 117    num3=num[3];
 118    num4=num[4];
 119    mem0=mem[0];
 120    mem1=mem[1];
 121    mem2=mem[2];
 122    mem3=mem[3];
 123    mem4=mem[4];
 124    for (i=0;i<N;i++)
 125    {
 126       opus_val32 sum = SHL32(EXTEND32(x[i]), SIG_SHIFT);
 127       sum = MAC16_16(sum,num0,mem0);
 128       sum = MAC16_16(sum,num1,mem1);
 129       sum = MAC16_16(sum,num2,mem2);
 130       sum = MAC16_16(sum,num3,mem3);
 131       sum = MAC16_16(sum,num4,mem4);
 132       mem4 = mem3;
 133       mem3 = mem2;
 134       mem2 = mem1;
 135       mem1 = mem0;
 136       mem0 = x[i];
 137       y[i] = ROUND16(sum, SIG_SHIFT);
 138    }
 139    mem[0]=mem0;
 140    mem[1]=mem1;
 141    mem[2]=mem2;
 142    mem[3]=mem3;
 143    mem[4]=mem4;
 144 }
 145
 146
 147 void pitch_downsample(celt_sig * OPUS_RESTRICT x[], opus_val16 * OPUS_RESTRICT x_lp,
 148       int len, int C)
 149 {
 150    int i;
 151    opus_val32 ac[5];
 152    opus_val16 tmp=Q15ONE;
 153    opus_val16 lpc[4], mem[5]={0,0,0,0,0};
 154    opus_val16 lpc2[5];
 155    opus_val16 c1 = QCONST16(.8f,15);
 156 #ifdef FIXED_POINT
 157    int shift;
 158    opus_val32 maxabs = celt_maxabs32(x[0], len);
 159    if (C==2)
 160    {
 161       opus_val32 maxabs_1 = celt_maxabs32(x[1], len);
 162       maxabs = MAX32(maxabs, maxabs_1);
 163    }
 164    if (maxabs<1)
 165       maxabs=1;
 166    shift = celt_ilog2(maxabs)-10;
 167    if (shift<0)
 168       shift=0;
 169    if (C==2)
 170       shift++;
 171 #endif
 172    for (i=1;i<len>>1;i++)
 173       x_lp[i] = SHR32(HALF32(HALF32(x[0][(2*i-1)]+x[0][(2*i+1)])+x[0][2*i]), shift);
 174    x_lp[0] = SHR32(HALF32(HALF32(x[0][1])+x[0][0]), shift);
 175    if (C==2)
 176    {
 177       for (i=1;i<len>>1;i++)
 178          x_lp[i] += SHR32(HALF32(HALF32(x[1][(2*i-1)]+x[1][(2*i+1)])+x[1][2*i]), shift);
 179       x_lp[0] += SHR32(HALF32(HALF32(x[1][1])+x[1][0]), shift);
 180    }
 181
 182    _celt_autocorr(x_lp, ac, NULL, 0,
 183                   4, len>>1);
 184
 185    /* Noise floor -40 dB */
 186 #ifdef FIXED_POINT
 187    ac[0] += SHR32(ac[0],13);
 188 #else
 189    ac[0] *= 1.0001f;
 190 #endif
 191    /* Lag windowing */
 192    for (i=1;i<=4;i++)
 193    {
 194       /*ac[i] *= exp(-.5*(2*M_PI*.002*i)*(2*M_PI*.002*i));*/
 195 #ifdef FIXED_POINT
 196       ac[i] -= MULT16_32_Q15(2*i*i, ac[i]);
 197 #else
 198       ac[i] -= ac[i]*(.008f*i)*(.008f*i);
 199 #endif
 200    }
 201
 202    _celt_lpc(lpc, ac, 4);
 203    for (i=0;i<4;i++)
 204    {
 205       tmp = MULT16_16_Q15(QCONST16(.9f,15), tmp);
 206       lpc[i] = MULT16_16_Q15(lpc[i], tmp);
 207    }
 208    /* Add a zero */
 209    lpc2[0] = lpc[0] + QCONST16(.8f,SIG_SHIFT);
 210    lpc2[1] = lpc[1] + MULT16_16_Q15(c1,lpc[0]);
 211    lpc2[2] = lpc[2] + MULT16_16_Q15(c1,lpc[1]);
 212    lpc2[3] = lpc[3] + MULT16_16_Q15(c1,lpc[2]);
 213    lpc2[4] = MULT16_16_Q15(c1,lpc[3]);
 214    celt_fir5(x_lp, lpc2, x_lp, len>>1, mem);
 215 }
 216
 217 #if 0 /* This is a simple version of the pitch correlation that should work
 218          well on DSPs like Blackfin and TI C5x/C6x */
 219
 220 #ifdef FIXED_POINT
 221 opus_val32
 222 #else
 223 void
 224 #endif
 225 pitch_xcorr(opus_val16 *x, opus_val16 *y, opus_val32 *xcorr, int len, int max_pitch)
 226 {
 227    int i, j;
 228 #ifdef FIXED_POINT
 229    opus_val32 maxcorr=1;
 230 #endif
 231    for (i=0;i<max_pitch;i++)
 232    {
 233       opus_val32 sum = 0;
 234       for (j=0;j<len;j++)
 235          sum = MAC16_16(sum, x[j],y[i+j]);
 236       xcorr[i] = sum;
 237 #ifdef FIXED_POINT
 238       maxcorr = MAX32(maxcorr, sum);
 239 #endif
 240    }
 241 #ifdef FIXED_POINT
 242    return maxcorr;
 243 #endif
 244 }
 245
 246 #else /* Unrolled version of the pitch correlation -- runs faster on x86 and ARM */
 247
 248 #ifdef FIXED_POINT
 249 opus_val32
 250 #else
 251 void
 252 #endif
 253 pitch_xcorr(opus_val16 *_x, opus_val16 *_y, opus_val32 *xcorr, int len, int max_pitch)
 254 {
 255    int i,j;
 256 #ifdef FIXED_POINT
 257    opus_val32 maxcorr=1;
 258 #endif
 259    for (i=0;i<max_pitch-3;i+=4)
 260    {
 261       /* Compute correlation*/
 262       /*corr[nb_pitch-1-i]=inner_prod(x, _y+i, len);*/
 263       opus_val32 sum1=0;
 264       opus_val32 sum2=0;
 265       opus_val32 sum3=0;
 266       opus_val32 sum4=0;
 267       const opus_val16 *y = _y+i;
 268       const opus_val16 *x = _x;
 269       opus_val16 y_0, y_1, y_2, y_3;
 270       y_3=0; /* gcc doesn't realize that y_3 can't be used uninitialized */
 271       y_0=*y++;
 272       y_1=*y++;
 273       y_2=*y++;
 274       for (j=0;j<len-3;j+=4)
 275       {
 276          opus_val16 tmp;
 277          tmp = *x++;
 278          y_3=*y++;
 279          sum1 = MAC16_16(sum1,tmp,y_0);
 280          sum2 = MAC16_16(sum2,tmp,y_1);
 281          sum3 = MAC16_16(sum3,tmp,y_2);
 282          sum4 = MAC16_16(sum4,tmp,y_3);
 283          tmp=*x++;
 284          y_0=*y++;
 285          sum1 = MAC16_16(sum1,tmp,y_1);
 286          sum2 = MAC16_16(sum2,tmp,y_2);
 287          sum3 = MAC16_16(sum3,tmp,y_3);
 288          sum4 = MAC16_16(sum4,tmp,y_0);
 289          tmp=*x++;
 290          y_1=*y++;
 291          sum1 = MAC16_16(sum1,tmp,y_2);
 292          sum2 = MAC16_16(sum2,tmp,y_3);
 293          sum3 = MAC16_16(sum3,tmp,y_0);
 294          sum4 = MAC16_16(sum4,tmp,y_1);
 295          tmp=*x++;
 296          y_2=*y++;
 297          sum1 = MAC16_16(sum1,tmp,y_3);
 298          sum2 = MAC16_16(sum2,tmp,y_0);
 299          sum3 = MAC16_16(sum3,tmp,y_1);
 300          sum4 = MAC16_16(sum4,tmp,y_2);
 301       }
 302       if (j++<len)
 303       {
 304          opus_val16 tmp = *x++;
 305          y_3=*y++;
 306          sum1 = MAC16_16(sum1,tmp,y_0);
 307          sum2 = MAC16_16(sum2,tmp,y_1);
 308          sum3 = MAC16_16(sum3,tmp,y_2);
 309          sum4 = MAC16_16(sum4,tmp,y_3);
 310       }
 311       if (j++<len)
 312       {
 313          opus_val16 tmp=*x++;
 314          y_0=*y++;
 315          sum1 = MAC16_16(sum1,tmp,y_1);
 316          sum2 = MAC16_16(sum2,tmp,y_2);
 317          sum3 = MAC16_16(sum3,tmp,y_3);
 318          sum4 = MAC16_16(sum4,tmp,y_0);
 319       }
 320       if (j<len)
 321       {
 322          opus_val16 tmp=*x++;
 323          y_1=*y++;
 324          sum1 = MAC16_16(sum1,tmp,y_2);
 325          sum2 = MAC16_16(sum2,tmp,y_3);
 326          sum3 = MAC16_16(sum3,tmp,y_0);
 327          sum4 = MAC16_16(sum4,tmp,y_1);
 328       }
 329       xcorr[i]=sum1;
 330       xcorr[i+1]=sum2;
 331       xcorr[i+2]=sum3;
 332       xcorr[i+3]=sum4;
 333 #ifdef FIXED_POINT
 334       sum1 = MAX32(sum1, sum2);
 335       sum3 = MAX32(sum3, sum4);
 336       sum1 = MAX32(sum1, sum3);
 337       maxcorr = MAX32(maxcorr, sum1);
 338 #endif
 339    }
 340    /* In case max_pitch isn't a multiple of 4, do non-unrolled version. */
 341    for (;i<max_pitch;i++)
 342    {
 343       opus_val32 sum = 0;
 344       for (j=0;j<len;j++)
 345          sum = MAC16_16(sum, _x[j],_y[i+j]);
 346       xcorr[i] = sum;
 347 #ifdef FIXED_POINT
 348       maxcorr = MAX32(maxcorr, sum);
 349 #endif
 350    }
 351 #ifdef FIXED_POINT
 352    return maxcorr;
 353 #endif
 354 }
 355
 356 #endif
 357 void pitch_search(const opus_val16 * OPUS_RESTRICT x_lp, opus_val16 * OPUS_RESTRICT y,
 358                   int len, int max_pitch, int *pitch)
 359 {
 360    int i, j;
 361    int lag;
 362    int best_pitch[2]={0,0};
 363    VARDECL(opus_val16, x_lp4);
 364    VARDECL(opus_val16, y_lp4);
 365    VARDECL(opus_val32, xcorr);
 366 #ifdef FIXED_POINT
 367    opus_val32 maxcorr;
 368    opus_val32 xmax, ymax;
 369    int shift=0;
 370 #endif
 371    int offset;
 372
 373    SAVE_STACK;
 374
 375    celt_assert(len>0);
 376    celt_assert(max_pitch>0);
 377    lag = len+max_pitch;
 378
 379    ALLOC(x_lp4, len>>2, opus_val16);
 380    ALLOC(y_lp4, lag>>2, opus_val16);
 381    ALLOC(xcorr, max_pitch>>1, opus_val32);
 382
 383    /* Downsample by 2 again */
 384    for (j=0;j<len>>2;j++)
 385       x_lp4[j] = x_lp[2*j];
 386    for (j=0;j<lag>>2;j++)
 387       y_lp4[j] = y[2*j];
 388
 389 #ifdef FIXED_POINT
 390    xmax = celt_maxabs16(x_lp4, len>>2);
 391    ymax = celt_maxabs16(y_lp4, lag>>2);
 392    shift = celt_ilog2(MAX32(1, MAX32(xmax, ymax)))-11;
 393    if (shift>0)
 394    {
 395       for (j=0;j<len>>2;j++)
 396          x_lp4[j] = SHR16(x_lp4[j], shift);
 397       for (j=0;j<lag>>2;j++)
 398          y_lp4[j] = SHR16(y_lp4[j], shift);
 399       /* Use double the shift for a MAC */
 400       shift *= 2;
 401    } else {
 402       shift = 0;
 403    }
 404 #endif
 405
 406    /* Coarse search with 4x decimation */
 407
 408 #ifdef FIXED_POINT
 409    maxcorr =
 410 #endif
 411    pitch_xcorr(x_lp4, y_lp4, xcorr, len>>2, max_pitch>>2);
 412
 413    find_best_pitch(xcorr, y_lp4, len>>2, max_pitch>>2, best_pitch
 414 #ifdef FIXED_POINT
 415                    , 0, maxcorr
 416 #endif
 417                    );
 418
 419    /* Finer search with 2x decimation */
 420 #ifdef FIXED_POINT
 421    maxcorr=1;
 422 #endif
 423    for (i=0;i<max_pitch>>1;i++)
 424    {
 425       opus_val32 sum=0;
 426       xcorr[i] = 0;
 427       if (abs(i-2*best_pitch[0])>2 && abs(i-2*best_pitch[1])>2)
 428          continue;
 429       for (j=0;j<len>>1;j++)
 430          sum += SHR32(MULT16_16(x_lp[j],y[i+j]), shift);
 431       xcorr[i] = MAX32(-1, sum);
 432 #ifdef FIXED_POINT
 433       maxcorr = MAX32(maxcorr, sum);
 434 #endif
 435    }
 436    find_best_pitch(xcorr, y, len>>1, max_pitch>>1, best_pitch
 437 #ifdef FIXED_POINT
 438                    , shift+1, maxcorr
 439 #endif
 440                    );
 441
 442    /* Refine by pseudo-interpolation */
 443    if (best_pitch[0]>0 && best_pitch[0]<(max_pitch>>1)-1)
 444    {
 445       opus_val32 a, b, c;
 446       a = xcorr[best_pitch[0]-1];
 447       b = xcorr[best_pitch[0]];
 448       c = xcorr[best_pitch[0]+1];
 449       if ((c-a) > MULT16_32_Q15(QCONST16(.7f,15),b-a))
 450          offset = 1;
 451       else if ((a-c) > MULT16_32_Q15(QCONST16(.7f,15),b-c))
 452          offset = -1;
 453       else
 454          offset = 0;
 455    } else {
 456       offset = 0;
 457    }
 458    *pitch = 2*best_pitch[0]-offset;
 459
 460    RESTORE_STACK;
 461 }
 462
 463 static const int second_check[16] = {0, 0, 3, 2, 3, 2, 5, 2, 3, 2, 3, 2, 5, 2, 3, 2};
 464 opus_val16 remove_doubling(opus_val16 *x, int maxperiod, int minperiod,
 465       int N, int *T0_, int prev_period, opus_val16 prev_gain)
 466 {
 467    int k, i, T, T0;
 468    opus_val16 g, g0;
 469    opus_val16 pg;
 470    opus_val32 xy,xx,yy;
 471    opus_val32 xcorr[3];
 472    opus_val32 best_xy, best_yy;
 473    int offset;
 474    int minperiod0;
 475    int max
 476    VARDECL(opus_val32, yy_lookup);
 477    SAVE_STACK;
 478
 479    minperiod0 = minperiod;
 480    maxperiod /= 2;
 481    minperiod /= 2;
 482    *T0_ /= 2;
 483    prev_period /= 2;
 484    N /= 2;
 485    x += maxperiod;
 486    if (*T0_>=maxperiod)
 487       *T0_=maxperiod-1;
 488
 489    T = T0 = *T0_;
 490    ALLOC(yy_lookup, maxperiod+1, opus_val32);
 491    xy=xx=0;
 492    for (i=0;i<N;i++)
 493    {
 494       xx = MAC16_16(xx, x[i], x[i]);
 495       xy = MAC16_16(xy, x[i], x[i-T0]);
 496    }
 497    yy_lookup[0] = xx;
 498    yy=xx;
 499    for (i=1;i<=maxperiod;i++)
 500    {
 501       yy = yy+MULT16_16(x[-i],x[-i])-MULT16_16(x[N-i],x[N-i]);
 502       yy_lookup[i] = MAX32(0, yy);
 503    }
 504    yy = yy_lookup[T0];
 505    best_xy = xy;
 506    best_yy = yy;
 507 #ifdef FIXED_POINT
 508       {
 509          opus_val32 x2y2;
 510          int sh, t;
 511          x2y2 = 1+HALF32(MULT32_32_Q31(xx,yy));
 512          sh = celt_ilog2(x2y2)>>1;
 513          t = VSHR32(x2y2, 2*(sh-7));
 514          g = g0 = VSHR32(MULT16_32_Q15(celt_rsqrt_norm(t), xy),sh+1);
 515       }
 516 #else
 517       g = g0 = xy/celt_sqrt(1+xx*yy);
 518 #endif
 519    /* Look for any pitch at T/k */
 520    for (k=2;k<=15;k++)
 521    {
 522       int T1, T1b;
 523       opus_val16 g1;
 524       opus_val16 cont=0;
 525       opus_val16 thresh;
 526       T1 = (2*T0+k)/(2*k);
 527       if (T1 < minperiod)
 528          break;
 529       /* Look for another strong correlation at T1b */
 530       if (k==2)
 531       {
 532          if (T1+T0>maxperiod)
 533             T1b = T0;
 534          else
 535             T1b = T0+T1;
 536       } else
 537       {
 538          T1b = (2*second_check[k]*T0+k)/(2*k);
 539       }
 540       xy=0;
 541       for (i=0;i<N;i++)
 542       {
 543          xy = MAC16_16(xy, x[i], x[i-T1]);
 544          xy = MAC16_16(xy, x[i], x[i-T1b]);
 545       }
 546       yy = yy_lookup[T1] + yy_lookup[T1b];
 547 #ifdef FIXED_POINT
 548       {
 549          opus_val32 x2y2;
 550          int sh, t;
 551          x2y2 = 1+MULT32_32_Q31(xx,yy);
 552          sh = celt_ilog2(x2y2)>>1;
 553          t = VSHR32(x2y2, 2*(sh-7));
 554          g1 = VSHR32(MULT16_32_Q15(celt_rsqrt_norm(t), xy),sh+1);
 555       }
 556 #else
 557       g1 = xy/celt_sqrt(1+2.f*xx*1.f*yy);
 558 #endif
 559       if (abs(T1-prev_period)<=1)
 560          cont = prev_gain;
 561       else if (abs(T1-prev_period)<=2 && 5*k*k < T0)
 562          cont = HALF32(prev_gain);
 563       else
 564          cont = 0;
 565       thresh = MAX16(QCONST16(.3f,15), MULT16_16_Q15(QCONST16(.7f,15),g0)-cont);
 566       /* Bias against very high pitch (very short period) to avoid false-positives
 567          due to short-term correlation */
 568       if (T1<3*minperiod)
 569          thresh = MAX16(QCONST16(.4f,15), MULT16_16_Q15(QCONST16(.85f,15),g0)-cont);
 570       else if (T1<2*minperiod)
 571          thresh = MAX16(QCONST16(.5f,15), MULT16_16_Q15(QCONST16(.9f,15),g0)-cont);
 572       if (g1 > thresh)
 573       {
 574          best_xy = xy;
 575          best_yy = yy;
 576          T = T1;
 577          g = g1;
 578       }
 579    }
 580    best_xy = MAX32(0, best_xy);
 581    if (best_yy <= best_xy)
 582       pg = Q15ONE;
 583    else
 584       pg = SHR32(frac_div32(best_xy,best_yy+1),16);
 585
 586    for (k=0;k<3;k++)
 587    {
 588       int T1 = T+k-1;
 589       xy = 0;
 590       for (i=0;i<N;i++)
 591          xy = MAC16_16(xy, x[i], x[i-T1]);
 592       xcorr[k] = xy;
 593    }
 594    if ((xcorr[2]-xcorr[0]) > MULT16_32_Q15(QCONST16(.7f,15),xcorr[1]-xcorr[0]))
 595       offset = 1;
 596    else if ((xcorr[0]-xcorr[2]) > MULT16_32_Q15(QCONST16(.7f,15),xcorr[1]-xcorr[2]))
 597       offset = -1;
 598    else
 599       offset = 0;
 600    if (pg > g)
 601       pg = g;
 602    *T0_ = 2*T+offset;
 603
 604    if (*T0_<minperiod0)
 605       *T0_=minperiod0;
 606    RESTORE_STACK;
 607    return pg;
 608 }