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 celt_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 celt_pitch_xcorr(const opus_val16 *_x, const 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 opus_val32 sum[4]={0,0,0,0}; 262 xcorr_kernel(_x, _y+i, sum, len); 263 xcorr[i]=sum[0]; 264 xcorr[i+1]=sum[1]; 265 xcorr[i+2]=sum[2]; 266 xcorr[i+3]=sum[3]; 267 #ifdef FIXED_POINT 268 sum[0] = MAX32(sum[0], sum[1]); 269 sum[2] = MAX32(sum[2], sum[3]); 270 sum[0] = MAX32(sum[0], sum[2]); 271 maxcorr = MAX32(maxcorr, sum[0]); 272 #endif 273 } 274 /* In case max_pitch isn't a multiple of 4, do non-unrolled version. */ 275 for (;i<max_pitch;i++) 276 { 277 opus_val32 sum = 0; 278 for (j=0;j<len;j++) 279 sum = MAC16_16(sum, _x[j],_y[i+j]); 280 xcorr[i] = sum; 281 #ifdef FIXED_POINT 282 maxcorr = MAX32(maxcorr, sum); 283 #endif 284 } 285 #ifdef FIXED_POINT 286 return maxcorr; 287 #endif 288 } 289 290 #endif 291 void pitch_search(const opus_val16 * OPUS_RESTRICT x_lp, opus_val16 * OPUS_RESTRICT y, 292 int len, int max_pitch, int *pitch) 293 { 294 int i, j; 295 int lag; 296 int best_pitch[2]={0,0}; 297 VARDECL(opus_val16, x_lp4); 298 VARDECL(opus_val16, y_lp4); 299 VARDECL(opus_val32, xcorr); 300 #ifdef FIXED_POINT 301 opus_val32 maxcorr; 302 opus_val32 xmax, ymax; 303 int shift=0; 304 #endif 305 int offset; 306 307 SAVE_STACK; 308 309 celt_assert(len>0); 310 celt_assert(max_pitch>0); 311 lag = len+max_pitch; 312 313 ALLOC(x_lp4, len>>2, opus_val16); 314 ALLOC(y_lp4, lag>>2, opus_val16); 315 ALLOC(xcorr, max_pitch>>1, opus_val32); 316 317 /* Downsample by 2 again */ 318 for (j=0;j<len>>2;j++) 319 x_lp4[j] = x_lp[2*j]; 320 for (j=0;j<lag>>2;j++) 321 y_lp4[j] = y[2*j]; 322 323 #ifdef FIXED_POINT 324 xmax = celt_maxabs16(x_lp4, len>>2); 325 ymax = celt_maxabs16(y_lp4, lag>>2); 326 shift = celt_ilog2(MAX32(1, MAX32(xmax, ymax)))-11; 327 if (shift>0) 328 { 329 for (j=0;j<len>>2;j++) 330 x_lp4[j] = SHR16(x_lp4[j], shift); 331 for (j=0;j<lag>>2;j++) 332 y_lp4[j] = SHR16(y_lp4[j], shift); 333 /* Use double the shift for a MAC */ 334 shift *= 2; 335 } else { 336 shift = 0; 337 } 338 #endif 339 340 /* Coarse search with 4x decimation */ 341 342 #ifdef FIXED_POINT 343 maxcorr = 344 #endif 345 celt_pitch_xcorr(x_lp4, y_lp4, xcorr, len>>2, max_pitch>>2); 346 347 find_best_pitch(xcorr, y_lp4, len>>2, max_pitch>>2, best_pitch 348 #ifdef FIXED_POINT 349 , 0, maxcorr 350 #endif 351 ); 352 353 /* Finer search with 2x decimation */ 354 #ifdef FIXED_POINT 355 maxcorr=1; 356 #endif 357 for (i=0;i<max_pitch>>1;i++) 358 { 359 opus_val32 sum=0; 360 xcorr[i] = 0; 361 if (abs(i-2*best_pitch[0])>2 && abs(i-2*best_pitch[1])>2) 362 continue; 363 for (j=0;j<len>>1;j++) 364 sum += SHR32(MULT16_16(x_lp[j],y[i+j]), shift); 365 xcorr[i] = MAX32(-1, sum); 366 #ifdef FIXED_POINT 367 maxcorr = MAX32(maxcorr, sum); 368 #endif 369 } 370 find_best_pitch(xcorr, y, len>>1, max_pitch>>1, best_pitch 371 #ifdef FIXED_POINT 372 , shift+1, maxcorr 373 #endif 374 ); 375 376 /* Refine by pseudo-interpolation */ 377 if (best_pitch[0]>0 && best_pitch[0]<(max_pitch>>1)-1) 378 { 379 opus_val32 a, b, c; 380 a = xcorr[best_pitch[0]-1]; 381 b = xcorr[best_pitch[0]]; 382 c = xcorr[best_pitch[0]+1]; 383 if ((c-a) > MULT16_32_Q15(QCONST16(.7f,15),b-a)) 384 offset = 1; 385 else if ((a-c) > MULT16_32_Q15(QCONST16(.7f,15),b-c)) 386 offset = -1; 387 else 388 offset = 0; 389 } else { 390 offset = 0; 391 } 392 *pitch = 2*best_pitch[0]-offset; 393 394 RESTORE_STACK; 395 } 396 397 static const int second_check[16] = {0, 0, 3, 2, 3, 2, 5, 2, 3, 2, 3, 2, 5, 2, 3, 2}; 398 opus_val16 remove_doubling(opus_val16 *x, int maxperiod, int minperiod, 399 int N, int *T0_, int prev_period, opus_val16 prev_gain) 400 { 401 int k, i, T, T0; 402 opus_val16 g, g0; 403 opus_val16 pg; 404 opus_val32 xy,xx,yy,xy2; 405 opus_val32 xcorr[3]; 406 opus_val32 best_xy, best_yy; 407 int offset; 408 int minperiod0; 409 VARDECL(opus_val32, yy_lookup); 410 SAVE_STACK; 411 412 minperiod0 = minperiod; 413 maxperiod /= 2; 414 minperiod /= 2; 415 *T0_ /= 2; 416 prev_period /= 2; 417 N /= 2; 418 x += maxperiod; 419 if (*T0_>=maxperiod) 420 *T0_=maxperiod-1; 421 422 T = T0 = *T0_; 423 ALLOC(yy_lookup, maxperiod+1, opus_val32); 424 dual_inner_prod(x, x, x-T0, N, &xx, &xy); 425 yy_lookup[0] = xx; 426 yy=xx; 427 for (i=1;i<=maxperiod;i++) 428 { 429 yy = yy+MULT16_16(x[-i],x[-i])-MULT16_16(x[N-i],x[N-i]); 430 yy_lookup[i] = MAX32(0, yy); 431 } 432 yy = yy_lookup[T0]; 433 best_xy = xy; 434 best_yy = yy; 435 #ifdef FIXED_POINT 436 { 437 opus_val32 x2y2; 438 int sh, t; 439 x2y2 = 1+HALF32(MULT32_32_Q31(xx,yy)); 440 sh = celt_ilog2(x2y2)>>1; 441 t = VSHR32(x2y2, 2*(sh-7)); 442 g = g0 = VSHR32(MULT16_32_Q15(celt_rsqrt_norm(t), xy),sh+1); 443 } 444 #else 445 g = g0 = xy/celt_sqrt(1+xx*yy); 446 #endif 447 /* Look for any pitch at T/k */ 448 for (k=2;k<=15;k++) 449 { 450 int T1, T1b; 451 opus_val16 g1; 452 opus_val16 cont=0; 453 opus_val16 thresh; 454 T1 = (2*T0+k)/(2*k); 455 if (T1 < minperiod) 456 break; 457 /* Look for another strong correlation at T1b */ 458 if (k==2) 459 { 460 if (T1+T0>maxperiod) 461 T1b = T0; 462 else 463 T1b = T0+T1; 464 } else 465 { 466 T1b = (2*second_check[k]*T0+k)/(2*k); 467 } 468 dual_inner_prod(x, &x[-T1], &x[-T1b], N, &xy, &xy2); 469 xy += xy2; 470 yy = yy_lookup[T1] + yy_lookup[T1b]; 471 #ifdef FIXED_POINT 472 { 473 opus_val32 x2y2; 474 int sh, t; 475 x2y2 = 1+MULT32_32_Q31(xx,yy); 476 sh = celt_ilog2(x2y2)>>1; 477 t = VSHR32(x2y2, 2*(sh-7)); 478 g1 = VSHR32(MULT16_32_Q15(celt_rsqrt_norm(t), xy),sh+1); 479 } 480 #else 481 g1 = xy/celt_sqrt(1+2.f*xx*1.f*yy); 482 #endif 483 if (abs(T1-prev_period)<=1) 484 cont = prev_gain; 485 else if (abs(T1-prev_period)<=2 && 5*k*k < T0) 486 cont = HALF32(prev_gain); 487 else 488 cont = 0; 489 thresh = MAX16(QCONST16(.3f,15), MULT16_16_Q15(QCONST16(.7f,15),g0)-cont); 490 /* Bias against very high pitch (very short period) to avoid false-positives 491 due to short-term correlation */ 492 if (T1<3*minperiod) 493 thresh = MAX16(QCONST16(.4f,15), MULT16_16_Q15(QCONST16(.85f,15),g0)-cont); 494 else if (T1<2*minperiod) 495 thresh = MAX16(QCONST16(.5f,15), MULT16_16_Q15(QCONST16(.9f,15),g0)-cont); 496 if (g1 > thresh) 497 { 498 best_xy = xy; 499 best_yy = yy; 500 T = T1; 501 g = g1; 502 } 503 } 504 best_xy = MAX32(0, best_xy); 505 if (best_yy <= best_xy) 506 pg = Q15ONE; 507 else 508 pg = SHR32(frac_div32(best_xy,best_yy+1),16); 509 510 for (k=0;k<3;k++) 511 { 512 int T1 = T+k-1; 513 xy = 0; 514 for (i=0;i<N;i++) 515 xy = MAC16_16(xy, x[i], x[i-T1]); 516 xcorr[k] = xy; 517 } 518 if ((xcorr[2]-xcorr[0]) > MULT16_32_Q15(QCONST16(.7f,15),xcorr[1]-xcorr[0])) 519 offset = 1; 520 else if ((xcorr[0]-xcorr[2]) > MULT16_32_Q15(QCONST16(.7f,15),xcorr[1]-xcorr[2])) 521 offset = -1; 522 else 523 offset = 0; 524 if (pg > g) 525 pg = g; 526 *T0_ = 2*T+offset; 527 528 if (*T0_<minperiod0) 529 *T0_=minperiod0; 530 RESTORE_STACK; 531 return pg; 532 } 533
