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