1 /* 2 * Copyright (c) 2011 The WebRTC project authors. All Rights Reserved. 3 * 4 * Use of this source code is governed by a BSD-style license 5 * that can be found in the LICENSE file in the root of the source 6 * tree. An additional intellectual property rights grant can be found 7 * in the file PATENTS. All contributing project authors may 8 * be found in the AUTHORS file in the root of the source tree. 9 */ 10 11 #include "webrtc/modules/audio_coding/codecs/isac/fix/source/pitch_estimator.h" 12 13 #ifdef WEBRTC_ARCH_ARM_NEON 14 #include <arm_neon.h> 15 #endif 16 17 #include "webrtc/common_audio/signal_processing/include/signal_processing_library.h" 18 #include "webrtc/system_wrappers/interface/compile_assert_c.h" 19 20 /* log2[0.2, 0.5, 0.98] in Q8 */ 21 static const int16_t kLogLagWinQ8[3] = { 22 -594, -256, -7 23 }; 24 25 /* [1 -0.75 0.25] in Q12 */ 26 static const int16_t kACoefQ12[3] = { 27 4096, -3072, 1024 28 }; 29 30 int32_t WebRtcIsacfix_Log2Q8(uint32_t x) { 31 int32_t zeros, lg2; 32 int16_t frac; 33 34 zeros=WebRtcSpl_NormU32(x); 35 frac=(int16_t)WEBRTC_SPL_RSHIFT_W32(((uint32_t)(WEBRTC_SPL_LSHIFT_W32(x, zeros))&0x7FFFFFFF), 23); 36 /* log2(magn(i)) */ 37 38 lg2= (WEBRTC_SPL_LSHIFT_W32((31-zeros), 8)+frac); 39 return lg2; 40 41 } 42 43 static __inline int16_t Exp2Q10(int16_t x) { // Both in and out in Q10 44 45 int16_t tmp16_1, tmp16_2; 46 47 tmp16_2=(int16_t)(0x0400|(x&0x03FF)); 48 tmp16_1=-(int16_t)WEBRTC_SPL_RSHIFT_W16(x,10); 49 if(tmp16_1>0) 50 return (int16_t) WEBRTC_SPL_RSHIFT_W16(tmp16_2, tmp16_1); 51 else 52 return (int16_t) WEBRTC_SPL_LSHIFT_W16(tmp16_2, -tmp16_1); 53 54 } 55 56 57 58 /* 1D parabolic interpolation . All input and output values are in Q8 */ 59 static __inline void Intrp1DQ8(int32_t *x, int32_t *fx, int32_t *y, int32_t *fy) { 60 61 int16_t sign1=1, sign2=1; 62 int32_t r32, q32, t32, nom32, den32; 63 int16_t t16, tmp16, tmp16_1; 64 65 if ((fx[0]>0) && (fx[2]>0)) { 66 r32=fx[1]-fx[2]; 67 q32=fx[0]-fx[1]; 68 nom32=q32+r32; 69 den32 = (q32 - r32) * 2; 70 if (nom32<0) 71 sign1=-1; 72 if (den32<0) 73 sign2=-1; 74 75 /* t = (q32+r32)/(2*(q32-r32)) = (fx[0]-fx[1] + fx[1]-fx[2])/(2 * fx[0]-fx[1] - (fx[1]-fx[2]))*/ 76 /* (Signs are removed because WebRtcSpl_DivResultInQ31 can't handle negative numbers) */ 77 /* t in Q31, without signs */ 78 t32 = WebRtcSpl_DivResultInQ31(nom32 * sign1, den32 * sign2); 79 80 t16=(int16_t)WEBRTC_SPL_RSHIFT_W32(t32, 23); /* Q8 */ 81 t16=t16*sign1*sign2; /* t in Q8 with signs */ 82 83 *y = x[0]+t16; /* Q8 */ 84 // *y = x[1]+t16; /* Q8 */ 85 86 /* The following code calculates fy in three steps */ 87 /* fy = 0.5 * t * (t-1) * fx[0] + (1-t*t) * fx[1] + 0.5 * t * (t+1) * fx[2]; */ 88 89 /* Part I: 0.5 * t * (t-1) * fx[0] */ 90 tmp16_1=(int16_t)WEBRTC_SPL_MUL_16_16(t16,t16); /* Q8*Q8=Q16 */ 91 tmp16_1 = WEBRTC_SPL_RSHIFT_W16(tmp16_1,2); /* Q16>>2 = Q14 */ 92 t16 = (int16_t)WEBRTC_SPL_MUL_16_16(t16, 64); /* Q8<<6 = Q14 */ 93 tmp16 = tmp16_1-t16; 94 *fy = WEBRTC_SPL_MUL_16_32_RSFT15(tmp16, fx[0]); /* (Q14 * Q8 >>15)/2 = Q8 */ 95 96 /* Part II: (1-t*t) * fx[1] */ 97 tmp16 = 16384-tmp16_1; /* 1 in Q14 - Q14 */ 98 *fy += WEBRTC_SPL_MUL_16_32_RSFT14(tmp16, fx[1]);/* Q14 * Q8 >> 14 = Q8 */ 99 100 /* Part III: 0.5 * t * (t+1) * fx[2] */ 101 tmp16 = tmp16_1+t16; 102 *fy += WEBRTC_SPL_MUL_16_32_RSFT15(tmp16, fx[2]);/* (Q14 * Q8 >>15)/2 = Q8 */ 103 } else { 104 *y = x[0]; 105 *fy= fx[1]; 106 } 107 } 108 109 110 static void FindFour32(int32_t *in, int16_t length, int16_t *bestind) 111 { 112 int32_t best[4]= {-100, -100, -100, -100}; 113 int16_t k; 114 115 for (k=0; k<length; k++) { 116 if (in[k] > best[3]) { 117 if (in[k] > best[2]) { 118 if (in[k] > best[1]) { 119 if (in[k] > best[0]) { // The Best 120 best[3] = best[2]; 121 bestind[3] = bestind[2]; 122 best[2] = best[1]; 123 bestind[2] = bestind[1]; 124 best[1] = best[0]; 125 bestind[1] = bestind[0]; 126 best[0] = in[k]; 127 bestind[0] = k; 128 } else { // 2nd best 129 best[3] = best[2]; 130 bestind[3] = bestind[2]; 131 best[2] = best[1]; 132 bestind[2] = bestind[1]; 133 best[1] = in[k]; 134 bestind[1] = k; 135 } 136 } else { // 3rd best 137 best[3] = best[2]; 138 bestind[3] = bestind[2]; 139 best[2] = in[k]; 140 bestind[2] = k; 141 } 142 } else { // 4th best 143 best[3] = in[k]; 144 bestind[3] = k; 145 } 146 } 147 } 148 } 149 150 151 152 153 154 extern void WebRtcIsacfix_PCorr2Q32(const int16_t *in, int32_t *logcorQ8); 155 156 157 158 void WebRtcIsacfix_InitialPitch(const int16_t *in, /* Q0 */ 159 PitchAnalysisStruct *State, 160 int16_t *lagsQ7 /* Q7 */ 161 ) 162 { 163 int16_t buf_dec16[PITCH_CORR_LEN2+PITCH_CORR_STEP2+PITCH_MAX_LAG/2+2]; 164 int32_t *crrvecQ8_1,*crrvecQ8_2; 165 int32_t cv1q[PITCH_LAG_SPAN2+2],cv2q[PITCH_LAG_SPAN2+2], peakvq[PITCH_LAG_SPAN2+2]; 166 int k; 167 int16_t peaks_indq; 168 int16_t peakiq[PITCH_LAG_SPAN2]; 169 int32_t corr; 170 int32_t corr32, corr_max32, corr_max_o32; 171 int16_t npkq; 172 int16_t best4q[4]={0,0,0,0}; 173 int32_t xq[3],yq[1],fyq[1]; 174 int32_t *fxq; 175 int32_t best_lag1q, best_lag2q; 176 int32_t tmp32a,tmp32b,lag32,ratq; 177 int16_t start; 178 int16_t oldgQ12, tmp16a, tmp16b, gain_bias16,tmp16c, tmp16d, bias16; 179 int32_t tmp32c,tmp32d, tmp32e; 180 int16_t old_lagQ; 181 int32_t old_lagQ8; 182 int32_t lagsQ8[4]; 183 184 old_lagQ = State->PFstr_wght.oldlagQ7; // Q7 185 old_lagQ8= WEBRTC_SPL_LSHIFT_W32((int32_t)old_lagQ,1); //Q8 186 187 oldgQ12= State->PFstr_wght.oldgainQ12; 188 189 crrvecQ8_1=&cv1q[1]; 190 crrvecQ8_2=&cv2q[1]; 191 192 193 /* copy old values from state buffer */ 194 memcpy(buf_dec16, State->dec_buffer16, WEBRTC_SPL_MUL_16_16(sizeof(int16_t), (PITCH_CORR_LEN2+PITCH_CORR_STEP2+PITCH_MAX_LAG/2-PITCH_FRAME_LEN/2+2))); 195 196 /* decimation; put result after the old values */ 197 WebRtcIsacfix_DecimateAllpass32(in, State->decimator_state32, PITCH_FRAME_LEN, 198 &buf_dec16[PITCH_CORR_LEN2+PITCH_CORR_STEP2+PITCH_MAX_LAG/2-PITCH_FRAME_LEN/2+2]); 199 200 /* low-pass filtering */ 201 start= PITCH_CORR_LEN2+PITCH_CORR_STEP2+PITCH_MAX_LAG/2-PITCH_FRAME_LEN/2+2; 202 WebRtcSpl_FilterARFastQ12(&buf_dec16[start],&buf_dec16[start],(int16_t*)kACoefQ12,3, PITCH_FRAME_LEN/2); 203 204 /* copy end part back into state buffer */ 205 for (k = 0; k < (PITCH_CORR_LEN2+PITCH_CORR_STEP2+PITCH_MAX_LAG/2-PITCH_FRAME_LEN/2+2); k++) 206 State->dec_buffer16[k] = buf_dec16[k+PITCH_FRAME_LEN/2]; 207 208 209 /* compute correlation for first and second half of the frame */ 210 WebRtcIsacfix_PCorr2Q32(buf_dec16, crrvecQ8_1); 211 WebRtcIsacfix_PCorr2Q32(buf_dec16 + PITCH_CORR_STEP2, crrvecQ8_2); 212 213 214 /* bias towards pitch lag of previous frame */ 215 tmp32a = WebRtcIsacfix_Log2Q8((uint32_t) old_lagQ8) - 2304; 216 // log2(0.5*oldlag) in Q8 217 tmp32b = WEBRTC_SPL_MUL_16_16_RSFT(oldgQ12,oldgQ12, 10); //Q12 & * 4.0; 218 gain_bias16 = (int16_t) tmp32b; //Q12 219 if (gain_bias16 > 3276) gain_bias16 = 3276; // 0.8 in Q12 220 221 222 for (k = 0; k < PITCH_LAG_SPAN2; k++) 223 { 224 if (crrvecQ8_1[k]>0) { 225 tmp32b = WebRtcIsacfix_Log2Q8((uint32_t) (k + (PITCH_MIN_LAG/2-2))); 226 tmp16a = (int16_t) (tmp32b - tmp32a); // Q8 & fabs(ratio)<4 227 tmp32c = WEBRTC_SPL_MUL_16_16_RSFT(tmp16a,tmp16a, 6); //Q10 228 tmp16b = (int16_t) tmp32c; // Q10 & <8 229 tmp32d = WEBRTC_SPL_MUL_16_16_RSFT(tmp16b, 177 , 8); // mult with ln2 in Q8 230 tmp16c = (int16_t) tmp32d; // Q10 & <4 231 tmp16d = Exp2Q10((int16_t) -tmp16c); //Q10 232 tmp32c = WEBRTC_SPL_MUL_16_16_RSFT(gain_bias16,tmp16d,13); // Q10 & * 0.5 233 bias16 = (int16_t) (1024 + tmp32c); // Q10 234 tmp32b = WebRtcIsacfix_Log2Q8((uint32_t)bias16) - 2560; 235 // Q10 in -> Q8 out with 10*2^8 offset 236 crrvecQ8_1[k] += tmp32b ; // -10*2^8 offset 237 } 238 } 239 240 /* taper correlation functions */ 241 for (k = 0; k < 3; k++) { 242 crrvecQ8_1[k] += kLogLagWinQ8[k]; 243 crrvecQ8_2[k] += kLogLagWinQ8[k]; 244 245 crrvecQ8_1[PITCH_LAG_SPAN2-1-k] += kLogLagWinQ8[k]; 246 crrvecQ8_2[PITCH_LAG_SPAN2-1-k] += kLogLagWinQ8[k]; 247 } 248 249 250 /* Make zeropadded corr vectors */ 251 cv1q[0]=0; 252 cv2q[0]=0; 253 cv1q[PITCH_LAG_SPAN2+1]=0; 254 cv2q[PITCH_LAG_SPAN2+1]=0; 255 corr_max32 = 0; 256 257 for (k = 1; k <= PITCH_LAG_SPAN2; k++) 258 { 259 260 261 corr32=crrvecQ8_1[k-1]; 262 if (corr32 > corr_max32) 263 corr_max32 = corr32; 264 265 corr32=crrvecQ8_2[k-1]; 266 corr32 += -4; // Compensate for later (log2(0.99)) 267 268 if (corr32 > corr_max32) 269 corr_max32 = corr32; 270 271 } 272 273 /* threshold value to qualify as a peak */ 274 // corr_max32 += -726; // log(0.14)/log(2.0) in Q8 275 corr_max32 += -1000; // log(0.14)/log(2.0) in Q8 276 corr_max_o32 = corr_max32; 277 278 279 /* find peaks in corr1 */ 280 peaks_indq = 0; 281 for (k = 1; k <= PITCH_LAG_SPAN2; k++) 282 { 283 corr32=cv1q[k]; 284 if (corr32>corr_max32) { // Disregard small peaks 285 if ((corr32>=cv1q[k-1]) && (corr32>cv1q[k+1])) { // Peak? 286 peakvq[peaks_indq] = corr32; 287 peakiq[peaks_indq++] = k; 288 } 289 } 290 } 291 292 293 /* find highest interpolated peak */ 294 corr_max32=0; 295 best_lag1q =0; 296 if (peaks_indq > 0) { 297 FindFour32(peakvq, (int16_t) peaks_indq, best4q); 298 npkq = WEBRTC_SPL_MIN(peaks_indq, 4); 299 300 for (k=0;k<npkq;k++) { 301 302 lag32 = peakiq[best4q[k]]; 303 fxq = &cv1q[peakiq[best4q[k]]-1]; 304 xq[0]= lag32; 305 xq[0] = WEBRTC_SPL_LSHIFT_W32(xq[0], 8); 306 Intrp1DQ8(xq, fxq, yq, fyq); 307 308 tmp32a= WebRtcIsacfix_Log2Q8((uint32_t) *yq) - 2048; // offset 8*2^8 309 /* Bias towards short lags */ 310 /* log(pow(0.8, log(2.0 * *y )))/log(2.0) */ 311 tmp32b= WEBRTC_SPL_MUL_16_16_RSFT((int16_t) tmp32a, -42, 8); 312 tmp32c= tmp32b + 256; 313 *fyq += tmp32c; 314 if (*fyq > corr_max32) { 315 corr_max32 = *fyq; 316 best_lag1q = *yq; 317 } 318 } 319 tmp32a = best_lag1q - OFFSET_Q8; 320 tmp32b = WEBRTC_SPL_LSHIFT_W32(tmp32a, 1); 321 lagsQ8[0] = tmp32b + PITCH_MIN_LAG_Q8; 322 lagsQ8[1] = lagsQ8[0]; 323 } else { 324 lagsQ8[0] = old_lagQ8; 325 lagsQ8[1] = lagsQ8[0]; 326 } 327 328 /* Bias towards constant pitch */ 329 tmp32a = lagsQ8[0] - PITCH_MIN_LAG_Q8; 330 ratq = WEBRTC_SPL_RSHIFT_W32(tmp32a, 1) + OFFSET_Q8; 331 332 for (k = 1; k <= PITCH_LAG_SPAN2; k++) 333 { 334 tmp32a = WEBRTC_SPL_LSHIFT_W32(k, 7); // 0.5*k Q8 335 tmp32b = (int32_t) (WEBRTC_SPL_LSHIFT_W32(tmp32a, 1)) - ratq; // Q8 336 tmp32c = WEBRTC_SPL_MUL_16_16_RSFT((int16_t) tmp32b, (int16_t) tmp32b, 8); // Q8 337 338 tmp32b = (int32_t)tmp32c + (int32_t)WEBRTC_SPL_RSHIFT_W32(ratq, 1); 339 // (k-r)^2 + 0.5 * r Q8 340 tmp32c = WebRtcIsacfix_Log2Q8((uint32_t)tmp32a) - 2048; 341 // offset 8*2^8 , log2(0.5*k) Q8 342 tmp32d = WebRtcIsacfix_Log2Q8((uint32_t)tmp32b) - 2048; 343 // offset 8*2^8 , log2(0.5*k) Q8 344 tmp32e = tmp32c - tmp32d; 345 346 cv2q[k] += WEBRTC_SPL_RSHIFT_W32(tmp32e, 1); 347 348 } 349 350 /* find peaks in corr2 */ 351 corr_max32 = corr_max_o32; 352 peaks_indq = 0; 353 354 for (k = 1; k <= PITCH_LAG_SPAN2; k++) 355 { 356 corr=cv2q[k]; 357 if (corr>corr_max32) { // Disregard small peaks 358 if ((corr>=cv2q[k-1]) && (corr>cv2q[k+1])) { // Peak? 359 peakvq[peaks_indq] = corr; 360 peakiq[peaks_indq++] = k; 361 } 362 } 363 } 364 365 366 367 /* find highest interpolated peak */ 368 corr_max32 = 0; 369 best_lag2q =0; 370 if (peaks_indq > 0) { 371 372 FindFour32(peakvq, (int16_t) peaks_indq, best4q); 373 npkq = WEBRTC_SPL_MIN(peaks_indq, 4); 374 for (k=0;k<npkq;k++) { 375 376 lag32 = peakiq[best4q[k]]; 377 fxq = &cv2q[peakiq[best4q[k]]-1]; 378 379 xq[0]= lag32; 380 xq[0] = WEBRTC_SPL_LSHIFT_W32(xq[0], 8); 381 Intrp1DQ8(xq, fxq, yq, fyq); 382 383 /* Bias towards short lags */ 384 /* log(pow(0.8, log(2.0f * *y )))/log(2.0f) */ 385 tmp32a= WebRtcIsacfix_Log2Q8((uint32_t) *yq) - 2048; // offset 8*2^8 386 tmp32b= WEBRTC_SPL_MUL_16_16_RSFT((int16_t) tmp32a, -82, 8); 387 tmp32c= tmp32b + 256; 388 *fyq += tmp32c; 389 if (*fyq > corr_max32) { 390 corr_max32 = *fyq; 391 best_lag2q = *yq; 392 } 393 } 394 395 tmp32a = best_lag2q - OFFSET_Q8; 396 tmp32b = WEBRTC_SPL_LSHIFT_W32(tmp32a, 1); 397 lagsQ8[2] = tmp32b + PITCH_MIN_LAG_Q8; 398 lagsQ8[3] = lagsQ8[2]; 399 } else { 400 lagsQ8[2] = lagsQ8[0]; 401 lagsQ8[3] = lagsQ8[0]; 402 } 403 404 lagsQ7[0]=(int16_t) WEBRTC_SPL_RSHIFT_W32(lagsQ8[0], 1); 405 lagsQ7[1]=(int16_t) WEBRTC_SPL_RSHIFT_W32(lagsQ8[1], 1); 406 lagsQ7[2]=(int16_t) WEBRTC_SPL_RSHIFT_W32(lagsQ8[2], 1); 407 lagsQ7[3]=(int16_t) WEBRTC_SPL_RSHIFT_W32(lagsQ8[3], 1); 408 409 410 } 411 412 413 414 void WebRtcIsacfix_PitchAnalysis(const int16_t *inn, /* PITCH_FRAME_LEN samples */ 415 int16_t *outQ0, /* PITCH_FRAME_LEN+QLOOKAHEAD samples */ 416 PitchAnalysisStruct *State, 417 int16_t *PitchLags_Q7, 418 int16_t *PitchGains_Q12) 419 { 420 int16_t inbufQ0[PITCH_FRAME_LEN + QLOOKAHEAD]; 421 int16_t k; 422 423 /* inital pitch estimate */ 424 WebRtcIsacfix_InitialPitch(inn, State, PitchLags_Q7); 425 426 427 /* Calculate gain */ 428 WebRtcIsacfix_PitchFilterGains(inn, &(State->PFstr_wght), PitchLags_Q7, PitchGains_Q12); 429 430 /* concatenate previous input's end and current input */ 431 for (k = 0; k < QLOOKAHEAD; k++) { 432 inbufQ0[k] = State->inbuf[k]; 433 } 434 for (k = 0; k < PITCH_FRAME_LEN; k++) { 435 inbufQ0[k+QLOOKAHEAD] = (int16_t) inn[k]; 436 } 437 438 /* lookahead pitch filtering for masking analysis */ 439 WebRtcIsacfix_PitchFilter(inbufQ0, outQ0, &(State->PFstr), PitchLags_Q7,PitchGains_Q12, 2); 440 441 442 /* store last part of input */ 443 for (k = 0; k < QLOOKAHEAD; k++) { 444 State->inbuf[k] = inbufQ0[k + PITCH_FRAME_LEN]; 445 } 446 } 447
