media/libopus/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(opus_val16 *x,
 106          const opus_val16 *num,
 107          int N)
 108 {
 109    int i;
 110    opus_val16 num0, num1, num2, num3, num4;
 111    opus_val32 mem0, mem1, mem2, mem3, mem4;
 112    num0=num[0];
 113    num1=num[1];
 114    num2=num[2];
 115    num3=num[3];
 116    num4=num[4];
 117    mem0=0;
 118    mem1=0;
 119    mem2=0;
 120    mem3=0;
 121    mem4=0;
 122    for (i=0;i<N;i++)
 123    {
 124       opus_val32 sum = SHL32(EXTEND32(x[i]), SIG_SHIFT);
 125       sum = MAC16_16(sum,num0,mem0);
 126       sum = MAC16_16(sum,num1,mem1);
 127       sum = MAC16_16(sum,num2,mem2);
 128       sum = MAC16_16(sum,num3,mem3);
 129       sum = MAC16_16(sum,num4,mem4);
 130       mem4 = mem3;
 131       mem3 = mem2;
 132       mem2 = mem1;
 133       mem1 = mem0;
 134       mem0 = x[i];
 135       x[i] = ROUND16(sum, SIG_SHIFT);
 136    }
 137 }
 138
 139
 140 void pitch_downsample(celt_sig * OPUS_RESTRICT x[], opus_val16 * OPUS_RESTRICT x_lp,
 141       int len, int C, int arch)
 142 {
 143    int i;
 144    opus_val32 ac[5];
 145    opus_val16 tmp=Q15ONE;
 146    opus_val16 lpc[4];
 147    opus_val16 lpc2[5];
 148    opus_val16 c1 = QCONST16(.8f,15);
 149 #ifdef FIXED_POINT
 150    int shift;
 151    opus_val32 maxabs = celt_maxabs32(x[0], len);
 152    if (C==2)
 153    {
 154       opus_val32 maxabs_1 = celt_maxabs32(x[1], len);
 155       maxabs = MAX32(maxabs, maxabs_1);
 156    }
 157    if (maxabs<1)
 158       maxabs=1;
 159    shift = celt_ilog2(maxabs)-10;
 160    if (shift<0)
 161       shift=0;
 162    if (C==2)
 163       shift++;
 164    for (i=1;i<len>>1;i++)
 165       x_lp[i] = SHR32(x[0][(2*i-1)], shift+2) + SHR32(x[0][(2*i+1)], shift+2) + SHR32(x[0][2*i], shift+1);
 166    x_lp[0] = SHR32(x[0][1], shift+2) + SHR32(x[0][0], shift+1);
 167    if (C==2)
 168    {
 169       for (i=1;i<len>>1;i++)
 170          x_lp[i] += SHR32(x[1][(2*i-1)], shift+2) + SHR32(x[1][(2*i+1)], shift+2) + SHR32(x[1][2*i], shift+1);
 171       x_lp[0] += SHR32(x[1][1], shift+2) + SHR32(x[1][0], shift+1);
 172    }
 173 #else
 174    for (i=1;i<len>>1;i++)
 175       x_lp[i] = .25f*x[0][(2*i-1)] + .25f*x[0][(2*i+1)] + .5f*x[0][2*i];
 176    x_lp[0] = .25f*x[0][1] + .5f*x[0][0];
 177    if (C==2)
 178    {
 179       for (i=1;i<len>>1;i++)
 180          x_lp[i] += .25f*x[1][(2*i-1)] + .25f*x[1][(2*i+1)] + .5f*x[1][2*i];
 181       x_lp[0] += .25f*x[1][1] + .5f*x[1][0];
 182    }
 183 #endif
 184    _celt_autocorr(x_lp, ac, NULL, 0,
 185                   4, len>>1, arch);
 186
 187    /* Noise floor -40 dB */
 188 #ifdef FIXED_POINT
 189    ac[0] += SHR32(ac[0],13);
 190 #else
 191    ac[0] *= 1.0001f;
 192 #endif
 193    /* Lag windowing */
 194    for (i=1;i<=4;i++)
 195    {
 196       /*ac[i] *= exp(-.5*(2*M_PI*.002*i)*(2*M_PI*.002*i));*/
 197 #ifdef FIXED_POINT
 198       ac[i] -= MULT16_32_Q15(2*i*i, ac[i]);
 199 #else
 200       ac[i] -= ac[i]*(.008f*i)*(.008f*i);
 201 #endif
 202    }
 203
 204    _celt_lpc(lpc, ac, 4);
 205    for (i=0;i<4;i++)
 206    {
 207       tmp = MULT16_16_Q15(QCONST16(.9f,15), tmp);
 208       lpc[i] = MULT16_16_Q15(lpc[i], tmp);
 209    }
 210    /* Add a zero */
 211    lpc2[0] = lpc[0] + QCONST16(.8f,SIG_SHIFT);
 212    lpc2[1] = lpc[1] + MULT16_16_Q15(c1,lpc[0]);
 213    lpc2[2] = lpc[2] + MULT16_16_Q15(c1,lpc[1]);
 214    lpc2[3] = lpc[3] + MULT16_16_Q15(c1,lpc[2]);
 215    lpc2[4] = MULT16_16_Q15(c1,lpc[3]);
 216    celt_fir5(x_lp, lpc2, len>>1);
 217 }
 218
 219 /* Pure C implementation. */
 220 #ifdef FIXED_POINT
 221 opus_val32
 222 #else
 223 void
 224 #endif
 225 celt_pitch_xcorr_c(const opus_val16 *_x, const opus_val16 *_y,
 226       opus_val32 *xcorr, int len, int max_pitch, int arch)
 227 {
 228
 229 #if 0 /* This is a simple version of the pitch correlation that should work
 230          well on DSPs like Blackfin and TI C5x/C6x */
 231    int i, j;
 232 #ifdef FIXED_POINT
 233    opus_val32 maxcorr=1;
 234 #endif
 235 #if !defined(OVERRIDE_PITCH_XCORR)
 236    (void)arch;
 237 #endif
 238    for (i=0;i<max_pitch;i++)
 239    {
 240       opus_val32 sum = 0;
 241       for (j=0;j<len;j++)
 242          sum = MAC16_16(sum, _x[j], _y[i+j]);
 243       xcorr[i] = sum;
 244 #ifdef FIXED_POINT
 245       maxcorr = MAX32(maxcorr, sum);
 246 #endif
 247    }
 248 #ifdef FIXED_POINT
 249    return maxcorr;
 250 #endif
 251
 252 #else /* Unrolled version of the pitch correlation -- runs faster on x86 and ARM */
 253    int i;
 254    /*The EDSP version requires that max_pitch is at least 1, and that _x is
 255       32-bit aligned.
 256      Since it's hard to put asserts in assembly, put them here.*/
 257 #ifdef FIXED_POINT
 258    opus_val32 maxcorr=1;
 259 #endif
 260    celt_assert(max_pitch>0);
 261    celt_sig_assert(((size_t)_x&3)==0);
 262    for (i=0;i<max_pitch-3;i+=4)
 263    {
 264       opus_val32 sum[4]={0,0,0,0};
 265       xcorr_kernel(_x, _y+i, sum, len, arch);
 266       xcorr[i]=sum[0];
 267       xcorr[i+1]=sum[1];
 268       xcorr[i+2]=sum[2];
 269       xcorr[i+3]=sum[3];
 270 #ifdef FIXED_POINT
 271       sum[0] = MAX32(sum[0], sum[1]);
 272       sum[2] = MAX32(sum[2], sum[3]);
 273       sum[0] = MAX32(sum[0], sum[2]);
 274       maxcorr = MAX32(maxcorr, sum[0]);
 275 #endif
 276    }
 277    /* In case max_pitch isn't a multiple of 4, do non-unrolled version. */
 278    for (;i<max_pitch;i++)
 279    {
 280       opus_val32 sum;
 281       sum = celt_inner_prod(_x, _y+i, len, arch);
 282       xcorr[i] = sum;
 283 #ifdef FIXED_POINT
 284       maxcorr = MAX32(maxcorr, sum);
 285 #endif
 286    }
 287 #ifdef FIXED_POINT
 288    return maxcorr;
 289 #endif
 290 #endif
 291 }
 292
 293 void pitch_search(const opus_val16 * OPUS_RESTRICT x_lp, opus_val16 * OPUS_RESTRICT y,
 294                   int len, int max_pitch, int *pitch, int arch)
 295 {
 296    int i, j;
 297    int lag;
 298    int best_pitch[2]={0,0};
 299    VARDECL(opus_val16, x_lp4);
 300    VARDECL(opus_val16, y_lp4);
 301    VARDECL(opus_val32, xcorr);
 302 #ifdef FIXED_POINT
 303    opus_val32 maxcorr;
 304    opus_val32 xmax, ymax;
 305    int shift=0;
 306 #endif
 307    int offset;
 308
 309    SAVE_STACK;
 310
 311    celt_assert(len>0);
 312    celt_assert(max_pitch>0);
 313    lag = len+max_pitch;
 314
 315    ALLOC(x_lp4, len>>2, opus_val16);
 316    ALLOC(y_lp4, lag>>2, opus_val16);
 317    ALLOC(xcorr, max_pitch>>1, opus_val32);
 318
 319    /* Downsample by 2 again */
 320    for (j=0;j<len>>2;j++)
 321       x_lp4[j] = x_lp[2*j];
 322    for (j=0;j<lag>>2;j++)
 323       y_lp4[j] = y[2*j];
 324
 325 #ifdef FIXED_POINT
 326    xmax = celt_maxabs16(x_lp4, len>>2);
 327    ymax = celt_maxabs16(y_lp4, lag>>2);
 328    shift = celt_ilog2(MAX32(1, MAX32(xmax, ymax)))-11;
 329    if (shift>0)
 330    {
 331       for (j=0;j<len>>2;j++)
 332          x_lp4[j] = SHR16(x_lp4[j], shift);
 333       for (j=0;j<lag>>2;j++)
 334          y_lp4[j] = SHR16(y_lp4[j], shift);
 335       /* Use double the shift for a MAC */
 336       shift *= 2;
 337    } else {
 338       shift = 0;
 339    }
 340 #endif
 341
 342    /* Coarse search with 4x decimation */
 343
 344 #ifdef FIXED_POINT
 345    maxcorr =
 346 #endif
 347    celt_pitch_xcorr(x_lp4, y_lp4, xcorr, len>>2, max_pitch>>2, arch);
 348
 349    find_best_pitch(xcorr, y_lp4, len>>2, max_pitch>>2, best_pitch
 350 #ifdef FIXED_POINT
 351                    , 0, maxcorr
 352 #endif
 353                    );
 354
 355    /* Finer search with 2x decimation */
 356 #ifdef FIXED_POINT
 357    maxcorr=1;
 358 #endif
 359    for (i=0;i<max_pitch>>1;i++)
 360    {
 361       opus_val32 sum;
 362       xcorr[i] = 0;
 363       if (abs(i-2*best_pitch[0])>2 && abs(i-2*best_pitch[1])>2)
 364          continue;
 365 #ifdef FIXED_POINT
 366       sum = 0;
 367       for (j=0;j<len>>1;j++)
 368          sum += SHR32(MULT16_16(x_lp[j],y[i+j]), shift);
 369 #else
 370       sum = celt_inner_prod(x_lp, y+i, len>>1, arch);
 371 #endif
 372       xcorr[i] = MAX32(-1, sum);
 373 #ifdef FIXED_POINT
 374       maxcorr = MAX32(maxcorr, sum);
 375 #endif
 376    }
 377    find_best_pitch(xcorr, y, len>>1, max_pitch>>1, best_pitch
 378 #ifdef FIXED_POINT
 379                    , shift+1, maxcorr
 380 #endif
 381                    );
 382
 383    /* Refine by pseudo-interpolation */
 384    if (best_pitch[0]>0 && best_pitch[0]<(max_pitch>>1)-1)
 385    {
 386       opus_val32 a, b, c;
 387       a = xcorr[best_pitch[0]-1];
 388       b = xcorr[best_pitch[0]];
 389       c = xcorr[best_pitch[0]+1];
 390       if ((c-a) > MULT16_32_Q15(QCONST16(.7f,15),b-a))
 391          offset = 1;
 392       else if ((a-c) > MULT16_32_Q15(QCONST16(.7f,15),b-c))
 393          offset = -1;
 394       else
 395          offset = 0;
 396    } else {
 397       offset = 0;
 398    }
 399    *pitch = 2*best_pitch[0]-offset;
 400
 401    RESTORE_STACK;
 402 }
 403
 404 #ifdef FIXED_POINT
 405 static opus_val16 compute_pitch_gain(opus_val32 xy, opus_val32 xx, opus_val32 yy)
 406 {
 407    opus_val32 x2y2;
 408    int sx, sy, shift;
 409    opus_val32 g;
 410    opus_val16 den;
 411    if (xy == 0 || xx == 0 || yy == 0)
 412       return 0;
 413    sx = celt_ilog2(xx)-14;
 414    sy = celt_ilog2(yy)-14;
 415    shift = sx + sy;
 416    x2y2 = SHR32(MULT16_16(VSHR32(xx, sx), VSHR32(yy, sy)), 14);
 417    if (shift & 1) {
 418       if (x2y2 < 32768)
 419       {
 420          x2y2 <<= 1;
 421          shift--;
 422       } else {
 423          x2y2 >>= 1;
 424          shift++;
 425       }
 426    }
 427    den = celt_rsqrt_norm(x2y2);
 428    g = MULT16_32_Q15(den, xy);
 429    g = VSHR32(g, (shift>>1)-1);
 430    return EXTRACT16(MIN32(g, Q15ONE));
 431 }
 432 #else
 433 static opus_val16 compute_pitch_gain(opus_val32 xy, opus_val32 xx, opus_val32 yy)
 434 {
 435    return xy/celt_sqrt(1+xx*yy);
 436 }
 437 #endif
 438
 439 static const int second_check[16] = {0, 0, 3, 2, 3, 2, 5, 2, 3, 2, 3, 2, 5, 2, 3, 2};
 440 opus_val16 remove_doubling(opus_val16 *x, int maxperiod, int minperiod,
 441       int N, int *T0_, int prev_period, opus_val16 prev_gain, int arch)
 442 {
 443    int k, i, T, T0;
 444    opus_val16 g, g0;
 445    opus_val16 pg;
 446    opus_val32 xy,xx,yy,xy2;
 447    opus_val32 xcorr[3];
 448    opus_val32 best_xy, best_yy;
 449    int offset;
 450    int minperiod0;
 451    VARDECL(opus_val32, yy_lookup);
 452    SAVE_STACK;
 453
 454    minperiod0 = minperiod;
 455    maxperiod /= 2;
 456    minperiod /= 2;
 457    *T0_ /= 2;
 458    prev_period /= 2;
 459    N /= 2;
 460    x += maxperiod;
 461    if (*T0_>=maxperiod)
 462       *T0_=maxperiod-1;
 463
 464    T = T0 = *T0_;
 465    ALLOC(yy_lookup, maxperiod+1, opus_val32);
 466    dual_inner_prod(x, x, x-T0, N, &xx, &xy, arch);
 467    yy_lookup[0] = xx;
 468    yy=xx;
 469    for (i=1;i<=maxperiod;i++)
 470    {
 471       yy = yy+MULT16_16(x[-i],x[-i])-MULT16_16(x[N-i],x[N-i]);
 472       yy_lookup[i] = MAX32(0, yy);
 473    }
 474    yy = yy_lookup[T0];
 475    best_xy = xy;
 476    best_yy = yy;
 477    g = g0 = compute_pitch_gain(xy, xx, yy);
 478    /* Look for any pitch at T/k */
 479    for (k=2;k<=15;k++)
 480    {
 481       int T1, T1b;
 482       opus_val16 g1;
 483       opus_val16 cont=0;
 484       opus_val16 thresh;
 485       T1 = celt_udiv(2*T0+k, 2*k);
 486       if (T1 < minperiod)
 487          break;
 488       /* Look for another strong correlation at T1b */
 489       if (k==2)
 490       {
 491          if (T1+T0>maxperiod)
 492             T1b = T0;
 493          else
 494             T1b = T0+T1;
 495       } else
 496       {
 497          T1b = celt_udiv(2*second_check[k]*T0+k, 2*k);
 498       }
 499       dual_inner_prod(x, &x[-T1], &x[-T1b], N, &xy, &xy2, arch);
 500       xy = HALF32(xy + xy2);
 501       yy = HALF32(yy_lookup[T1] + yy_lookup[T1b]);
 502       g1 = compute_pitch_gain(xy, xx, yy);
 503       if (abs(T1-prev_period)<=1)
 504          cont = prev_gain;
 505       else if (abs(T1-prev_period)<=2 && 5*k*k < T0)
 506          cont = HALF16(prev_gain);
 507       else
 508          cont = 0;
 509       thresh = MAX16(QCONST16(.3f,15), MULT16_16_Q15(QCONST16(.7f,15),g0)-cont);
 510       /* Bias against very high pitch (very short period) to avoid false-positives
 511          due to short-term correlation */
 512       if (T1<3*minperiod)
 513          thresh = MAX16(QCONST16(.4f,15), MULT16_16_Q15(QCONST16(.85f,15),g0)-cont);
 514       else if (T1<2*minperiod)
 515          thresh = MAX16(QCONST16(.5f,15), MULT16_16_Q15(QCONST16(.9f,15),g0)-cont);
 516       if (g1 > thresh)
 517       {
 518          best_xy = xy;
 519          best_yy = yy;
 520          T = T1;
 521          g = g1;
 522       }
 523    }
 524    best_xy = MAX32(0, best_xy);
 525    if (best_yy <= best_xy)
 526       pg = Q15ONE;
 527    else
 528       pg = SHR32(frac_div32(best_xy,best_yy+1),16);
 529
 530    for (k=0;k<3;k++)
 531       xcorr[k] = celt_inner_prod(x, x-(T+k-1), N, arch);
 532    if ((xcorr[2]-xcorr[0]) > MULT16_32_Q15(QCONST16(.7f,15),xcorr[1]-xcorr[0]))
 533       offset = 1;
 534    else if ((xcorr[0]-xcorr[2]) > MULT16_32_Q15(QCONST16(.7f,15),xcorr[1]-xcorr[2]))
 535       offset = -1;
 536    else
 537       offset = 0;
 538    if (pg > g)
 539       pg = g;
 540    *T0_ = 2*T+offset;
 541
 542    if (*T0_<minperiod0)
 543       *T0_=minperiod0;
 544    RESTORE_STACK;
 545    return pg;
 546 }