1/*********************************************************************** 2Copyright (c) 2006-2011, Skype Limited. All rights reserved. 3Redistribution and use in source and binary forms, with or without 4modification, are permitted provided that the following conditions 5are met: 6- Redistributions of source code must retain the above copyright notice, 7this list of conditions and the following disclaimer. 8- Redistributions in binary form must reproduce the above copyright 9notice, this list of conditions and the following disclaimer in the 10documentation and/or other materials provided with the distribution. 11- Neither the name of Internet Society, IETF or IETF Trust, nor the 12names of specific contributors, may be used to endorse or promote 13products derived from this software without specific prior written 14permission. 15THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS “AS IS” 16AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25POSSIBILITY OF SUCH DAMAGE. 26***********************************************************************/ 27 28#ifdef HAVE_CONFIG_H 29#include "config.h" 30#endif 31 32/***************************************************************************** 33* Pitch analyser function 34******************************************************************************/ 35#include "SigProc_FLP.h" 36#include "SigProc_FIX.h" 37#include "pitch_est_defines.h" 38 39#define SCRATCH_SIZE 22 40#define eps 1.192092896e-07f 41 42/************************************************************/ 43/* Internally used functions */ 44/************************************************************/ 45static void silk_P_Ana_calc_corr_st3( 46 silk_float cross_corr_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ], /* O 3 DIM correlation array */ 47 const silk_float frame[], /* I vector to correlate */ 48 opus_int start_lag, /* I start lag */ 49 opus_int sf_length, /* I sub frame length */ 50 opus_int nb_subfr, /* I number of subframes */ 51 opus_int complexity /* I Complexity setting */ 52); 53 54static void silk_P_Ana_calc_energy_st3( 55 silk_float energies_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ], /* O 3 DIM correlation array */ 56 const silk_float frame[], /* I vector to correlate */ 57 opus_int start_lag, /* I start lag */ 58 opus_int sf_length, /* I sub frame length */ 59 opus_int nb_subfr, /* I number of subframes */ 60 opus_int complexity /* I Complexity setting */ 61); 62 63/************************************************************/ 64/* CORE PITCH ANALYSIS FUNCTION */ 65/************************************************************/ 66opus_int silk_pitch_analysis_core_FLP( /* O Voicing estimate: 0 voiced, 1 unvoiced */ 67 const silk_float *frame, /* I Signal of length PE_FRAME_LENGTH_MS*Fs_kHz */ 68 opus_int *pitch_out, /* O Pitch lag values [nb_subfr] */ 69 opus_int16 *lagIndex, /* O Lag Index */ 70 opus_int8 *contourIndex, /* O Pitch contour Index */ 71 silk_float *LTPCorr, /* I/O Normalized correlation; input: value from previous frame */ 72 opus_int prevLag, /* I Last lag of previous frame; set to zero is unvoiced */ 73 const silk_float search_thres1, /* I First stage threshold for lag candidates 0 - 1 */ 74 const silk_float search_thres2, /* I Final threshold for lag candidates 0 - 1 */ 75 const opus_int Fs_kHz, /* I sample frequency (kHz) */ 76 const opus_int complexity, /* I Complexity setting, 0-2, where 2 is highest */ 77 const opus_int nb_subfr /* I Number of 5 ms subframes */ 78) 79{ 80 opus_int i, k, d, j; 81 silk_float frame_8kHz[ PE_MAX_FRAME_LENGTH_MS * 8 ]; 82 silk_float frame_4kHz[ PE_MAX_FRAME_LENGTH_MS * 4 ]; 83 opus_int16 frame_8_FIX[ PE_MAX_FRAME_LENGTH_MS * 8 ]; 84 opus_int16 frame_4_FIX[ PE_MAX_FRAME_LENGTH_MS * 4 ]; 85 opus_int32 filt_state[ 6 ]; 86 silk_float threshold, contour_bias; 87 silk_float C[ PE_MAX_NB_SUBFR][ (PE_MAX_LAG >> 1) + 5 ]; 88 silk_float CC[ PE_NB_CBKS_STAGE2_EXT ]; 89 const silk_float *target_ptr, *basis_ptr; 90 double cross_corr, normalizer, energy, energy_tmp; 91 opus_int d_srch[ PE_D_SRCH_LENGTH ]; 92 opus_int16 d_comp[ (PE_MAX_LAG >> 1) + 5 ]; 93 opus_int length_d_srch, length_d_comp; 94 silk_float Cmax, CCmax, CCmax_b, CCmax_new_b, CCmax_new; 95 opus_int CBimax, CBimax_new, lag, start_lag, end_lag, lag_new; 96 opus_int cbk_size; 97 silk_float lag_log2, prevLag_log2, delta_lag_log2_sqr; 98 silk_float energies_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ]; 99 silk_float cross_corr_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ]; 100 opus_int lag_counter; 101 opus_int frame_length, frame_length_8kHz, frame_length_4kHz; 102 opus_int sf_length, sf_length_8kHz, sf_length_4kHz; 103 opus_int min_lag, min_lag_8kHz, min_lag_4kHz; 104 opus_int max_lag, max_lag_8kHz, max_lag_4kHz; 105 opus_int nb_cbk_search; 106 const opus_int8 *Lag_CB_ptr; 107 108 /* Check for valid sampling frequency */ 109 silk_assert( Fs_kHz == 8 || Fs_kHz == 12 || Fs_kHz == 16 ); 110 111 /* Check for valid complexity setting */ 112 silk_assert( complexity >= SILK_PE_MIN_COMPLEX ); 113 silk_assert( complexity <= SILK_PE_MAX_COMPLEX ); 114 115 silk_assert( search_thres1 >= 0.0f && search_thres1 <= 1.0f ); 116 silk_assert( search_thres2 >= 0.0f && search_thres2 <= 1.0f ); 117 118 /* Set up frame lengths max / min lag for the sampling frequency */ 119 frame_length = ( PE_LTP_MEM_LENGTH_MS + nb_subfr * PE_SUBFR_LENGTH_MS ) * Fs_kHz; 120 frame_length_4kHz = ( PE_LTP_MEM_LENGTH_MS + nb_subfr * PE_SUBFR_LENGTH_MS ) * 4; 121 frame_length_8kHz = ( PE_LTP_MEM_LENGTH_MS + nb_subfr * PE_SUBFR_LENGTH_MS ) * 8; 122 sf_length = PE_SUBFR_LENGTH_MS * Fs_kHz; 123 sf_length_4kHz = PE_SUBFR_LENGTH_MS * 4; 124 sf_length_8kHz = PE_SUBFR_LENGTH_MS * 8; 125 min_lag = PE_MIN_LAG_MS * Fs_kHz; 126 min_lag_4kHz = PE_MIN_LAG_MS * 4; 127 min_lag_8kHz = PE_MIN_LAG_MS * 8; 128 max_lag = PE_MAX_LAG_MS * Fs_kHz - 1; 129 max_lag_4kHz = PE_MAX_LAG_MS * 4; 130 max_lag_8kHz = PE_MAX_LAG_MS * 8 - 1; 131 132 silk_memset(C, 0, sizeof(silk_float) * nb_subfr * ((PE_MAX_LAG >> 1) + 5)); 133 134 /* Resample from input sampled at Fs_kHz to 8 kHz */ 135 if( Fs_kHz == 16 ) { 136 /* Resample to 16 -> 8 khz */ 137 opus_int16 frame_16_FIX[ 16 * PE_MAX_FRAME_LENGTH_MS ]; 138 silk_float2short_array( frame_16_FIX, frame, frame_length ); 139 silk_memset( filt_state, 0, 2 * sizeof( opus_int32 ) ); 140 silk_resampler_down2( filt_state, frame_8_FIX, frame_16_FIX, frame_length ); 141 silk_short2float_array( frame_8kHz, frame_8_FIX, frame_length_8kHz ); 142 } else if( Fs_kHz == 12 ) { 143 /* Resample to 12 -> 8 khz */ 144 opus_int16 frame_12_FIX[ 12 * PE_MAX_FRAME_LENGTH_MS ]; 145 silk_float2short_array( frame_12_FIX, frame, frame_length ); 146 silk_memset( filt_state, 0, 6 * sizeof( opus_int32 ) ); 147 silk_resampler_down2_3( filt_state, frame_8_FIX, frame_12_FIX, frame_length ); 148 silk_short2float_array( frame_8kHz, frame_8_FIX, frame_length_8kHz ); 149 } else { 150 silk_assert( Fs_kHz == 8 ); 151 silk_float2short_array( frame_8_FIX, frame, frame_length_8kHz ); 152 } 153 154 /* Decimate again to 4 kHz */ 155 silk_memset( filt_state, 0, 2 * sizeof( opus_int32 ) ); 156 silk_resampler_down2( filt_state, frame_4_FIX, frame_8_FIX, frame_length_8kHz ); 157 silk_short2float_array( frame_4kHz, frame_4_FIX, frame_length_4kHz ); 158 159 /* Low-pass filter */ 160 for( i = frame_length_4kHz - 1; i > 0; i-- ) { 161 frame_4kHz[ i ] += frame_4kHz[ i - 1 ]; 162 } 163 164 /****************************************************************************** 165 * FIRST STAGE, operating in 4 khz 166 ******************************************************************************/ 167 target_ptr = &frame_4kHz[ silk_LSHIFT( sf_length_4kHz, 2 ) ]; 168 for( k = 0; k < nb_subfr >> 1; k++ ) { 169 /* Check that we are within range of the array */ 170 silk_assert( target_ptr >= frame_4kHz ); 171 silk_assert( target_ptr + sf_length_8kHz <= frame_4kHz + frame_length_4kHz ); 172 173 basis_ptr = target_ptr - min_lag_4kHz; 174 175 /* Check that we are within range of the array */ 176 silk_assert( basis_ptr >= frame_4kHz ); 177 silk_assert( basis_ptr + sf_length_8kHz <= frame_4kHz + frame_length_4kHz ); 178 179 /* Calculate first vector products before loop */ 180 cross_corr = silk_inner_product_FLP( target_ptr, basis_ptr, sf_length_8kHz ); 181 normalizer = silk_energy_FLP( basis_ptr, sf_length_8kHz ) + sf_length_8kHz * 4000.0f; 182 183 C[ 0 ][ min_lag_4kHz ] += (silk_float)(cross_corr / sqrt(normalizer)); 184 185 /* From now on normalizer is computed recursively */ 186 for(d = min_lag_4kHz + 1; d <= max_lag_4kHz; d++) { 187 basis_ptr--; 188 189 /* Check that we are within range of the array */ 190 silk_assert( basis_ptr >= frame_4kHz ); 191 silk_assert( basis_ptr + sf_length_8kHz <= frame_4kHz + frame_length_4kHz ); 192 193 cross_corr = silk_inner_product_FLP(target_ptr, basis_ptr, sf_length_8kHz); 194 195 /* Add contribution of new sample and remove contribution from oldest sample */ 196 normalizer += 197 basis_ptr[ 0 ] * (double)basis_ptr[ 0 ] - 198 basis_ptr[ sf_length_8kHz ] * (double)basis_ptr[ sf_length_8kHz ]; 199 C[ 0 ][ d ] += (silk_float)(cross_corr / sqrt( normalizer )); 200 } 201 /* Update target pointer */ 202 target_ptr += sf_length_8kHz; 203 } 204 205 /* Apply short-lag bias */ 206 for( i = max_lag_4kHz; i >= min_lag_4kHz; i-- ) { 207 C[ 0 ][ i ] -= C[ 0 ][ i ] * i / 4096.0f; 208 } 209 210 /* Sort */ 211 length_d_srch = 4 + 2 * complexity; 212 silk_assert( 3 * length_d_srch <= PE_D_SRCH_LENGTH ); 213 silk_insertion_sort_decreasing_FLP( &C[ 0 ][ min_lag_4kHz ], d_srch, max_lag_4kHz - min_lag_4kHz + 1, length_d_srch ); 214 215 /* Escape if correlation is very low already here */ 216 Cmax = C[ 0 ][ min_lag_4kHz ]; 217 target_ptr = &frame_4kHz[ silk_SMULBB( sf_length_4kHz, nb_subfr ) ]; 218 energy = 1000.0f; 219 for( i = 0; i < silk_LSHIFT( sf_length_4kHz, 2 ); i++ ) { 220 energy += target_ptr[i] * (double)target_ptr[i]; 221 } 222 threshold = Cmax * Cmax; 223 if( energy / 16.0f > threshold ) { 224 silk_memset( pitch_out, 0, nb_subfr * sizeof( opus_int ) ); 225 *LTPCorr = 0.0f; 226 *lagIndex = 0; 227 *contourIndex = 0; 228 return 1; 229 } 230 231 threshold = search_thres1 * Cmax; 232 for( i = 0; i < length_d_srch; i++ ) { 233 /* Convert to 8 kHz indices for the sorted correlation that exceeds the threshold */ 234 if( C[ 0 ][ min_lag_4kHz + i ] > threshold ) { 235 d_srch[ i ] = silk_LSHIFT( d_srch[ i ] + min_lag_4kHz, 1 ); 236 } else { 237 length_d_srch = i; 238 break; 239 } 240 } 241 silk_assert( length_d_srch > 0 ); 242 243 for( i = min_lag_8kHz - 5; i < max_lag_8kHz + 5; i++ ) { 244 d_comp[ i ] = 0; 245 } 246 for( i = 0; i < length_d_srch; i++ ) { 247 d_comp[ d_srch[ i ] ] = 1; 248 } 249 250 /* Convolution */ 251 for( i = max_lag_8kHz + 3; i >= min_lag_8kHz; i-- ) { 252 d_comp[ i ] += d_comp[ i - 1 ] + d_comp[ i - 2 ]; 253 } 254 255 length_d_srch = 0; 256 for( i = min_lag_8kHz; i < max_lag_8kHz + 1; i++ ) { 257 if( d_comp[ i + 1 ] > 0 ) { 258 d_srch[ length_d_srch ] = i; 259 length_d_srch++; 260 } 261 } 262 263 /* Convolution */ 264 for( i = max_lag_8kHz + 3; i >= min_lag_8kHz; i-- ) { 265 d_comp[ i ] += d_comp[ i - 1 ] + d_comp[ i - 2 ] + d_comp[ i - 3 ]; 266 } 267 268 length_d_comp = 0; 269 for( i = min_lag_8kHz; i < max_lag_8kHz + 4; i++ ) { 270 if( d_comp[ i ] > 0 ) { 271 d_comp[ length_d_comp ] = (opus_int16)( i - 2 ); 272 length_d_comp++; 273 } 274 } 275 276 /********************************************************************************** 277 ** SECOND STAGE, operating at 8 kHz, on lag sections with high correlation 278 *************************************************************************************/ 279 /********************************************************************************* 280 * Find energy of each subframe projected onto its history, for a range of delays 281 *********************************************************************************/ 282 silk_memset( C, 0, PE_MAX_NB_SUBFR*((PE_MAX_LAG >> 1) + 5) * sizeof(silk_float)); 283 284 if( Fs_kHz == 8 ) { 285 target_ptr = &frame[ PE_LTP_MEM_LENGTH_MS * 8 ]; 286 } else { 287 target_ptr = &frame_8kHz[ PE_LTP_MEM_LENGTH_MS * 8 ]; 288 } 289 for( k = 0; k < nb_subfr; k++ ) { 290 energy_tmp = silk_energy_FLP( target_ptr, sf_length_8kHz ); 291 for( j = 0; j < length_d_comp; j++ ) { 292 d = d_comp[ j ]; 293 basis_ptr = target_ptr - d; 294 cross_corr = silk_inner_product_FLP( basis_ptr, target_ptr, sf_length_8kHz ); 295 energy = silk_energy_FLP( basis_ptr, sf_length_8kHz ); 296 if( cross_corr > 0.0f ) { 297 C[ k ][ d ] = (silk_float)(cross_corr * cross_corr / (energy * energy_tmp + eps)); 298 } else { 299 C[ k ][ d ] = 0.0f; 300 } 301 } 302 target_ptr += sf_length_8kHz; 303 } 304 305 /* search over lag range and lags codebook */ 306 /* scale factor for lag codebook, as a function of center lag */ 307 308 CCmax = 0.0f; /* This value doesn't matter */ 309 CCmax_b = -1000.0f; 310 311 CBimax = 0; /* To avoid returning undefined lag values */ 312 lag = -1; /* To check if lag with strong enough correlation has been found */ 313 314 if( prevLag > 0 ) { 315 if( Fs_kHz == 12 ) { 316 prevLag = silk_LSHIFT( prevLag, 1 ) / 3; 317 } else if( Fs_kHz == 16 ) { 318 prevLag = silk_RSHIFT( prevLag, 1 ); 319 } 320 prevLag_log2 = silk_log2((silk_float)prevLag); 321 } else { 322 prevLag_log2 = 0; 323 } 324 325 /* Set up stage 2 codebook based on number of subframes */ 326 if( nb_subfr == PE_MAX_NB_SUBFR ) { 327 cbk_size = PE_NB_CBKS_STAGE2_EXT; 328 Lag_CB_ptr = &silk_CB_lags_stage2[ 0 ][ 0 ]; 329 if( Fs_kHz == 8 && complexity > SILK_PE_MIN_COMPLEX ) { 330 /* If input is 8 khz use a larger codebook here because it is last stage */ 331 nb_cbk_search = PE_NB_CBKS_STAGE2_EXT; 332 } else { 333 nb_cbk_search = PE_NB_CBKS_STAGE2; 334 } 335 } else { 336 cbk_size = PE_NB_CBKS_STAGE2_10MS; 337 Lag_CB_ptr = &silk_CB_lags_stage2_10_ms[ 0 ][ 0 ]; 338 nb_cbk_search = PE_NB_CBKS_STAGE2_10MS; 339 } 340 341 for( k = 0; k < length_d_srch; k++ ) { 342 d = d_srch[ k ]; 343 for( j = 0; j < nb_cbk_search; j++ ) { 344 CC[j] = 0.0f; 345 for( i = 0; i < nb_subfr; i++ ) { 346 /* Try all codebooks */ 347 CC[ j ] += C[ i ][ d + matrix_ptr( Lag_CB_ptr, i, j, cbk_size )]; 348 } 349 } 350 /* Find best codebook */ 351 CCmax_new = -1000.0f; 352 CBimax_new = 0; 353 for( i = 0; i < nb_cbk_search; i++ ) { 354 if( CC[ i ] > CCmax_new ) { 355 CCmax_new = CC[ i ]; 356 CBimax_new = i; 357 } 358 } 359 CCmax_new = silk_max_float(CCmax_new, 0.0f); /* To avoid taking square root of negative number later */ 360 CCmax_new_b = CCmax_new; 361 362 /* Bias towards shorter lags */ 363 lag_log2 = silk_log2((silk_float)d); 364 CCmax_new_b -= PE_SHORTLAG_BIAS * nb_subfr * lag_log2; 365 366 /* Bias towards previous lag */ 367 if( prevLag > 0 ) { 368 delta_lag_log2_sqr = lag_log2 - prevLag_log2; 369 delta_lag_log2_sqr *= delta_lag_log2_sqr; 370 CCmax_new_b -= PE_PREVLAG_BIAS * nb_subfr * (*LTPCorr) * delta_lag_log2_sqr / (delta_lag_log2_sqr + 0.5f); 371 } 372 373 if( CCmax_new_b > CCmax_b && /* Find maximum biased correlation */ 374 CCmax_new > nb_subfr * search_thres2 * search_thres2 && /* Correlation needs to be high enough to be voiced */ 375 silk_CB_lags_stage2[ 0 ][ CBimax_new ] <= min_lag_8kHz /* Lag must be in range */ 376 ) { 377 CCmax_b = CCmax_new_b; 378 CCmax = CCmax_new; 379 lag = d; 380 CBimax = CBimax_new; 381 } 382 } 383 384 if( lag == -1 ) { 385 /* No suitable candidate found */ 386 silk_memset( pitch_out, 0, PE_MAX_NB_SUBFR * sizeof(opus_int) ); 387 *LTPCorr = 0.0f; 388 *lagIndex = 0; 389 *contourIndex = 0; 390 return 1; 391 } 392 393 if( Fs_kHz > 8 ) { 394 /* Search in original signal */ 395 396 /* Compensate for decimation */ 397 silk_assert( lag == silk_SAT16( lag ) ); 398 if( Fs_kHz == 12 ) { 399 lag = silk_RSHIFT_ROUND( silk_SMULBB( lag, 3 ), 1 ); 400 } else { /* Fs_kHz == 16 */ 401 lag = silk_LSHIFT( lag, 1 ); 402 } 403 404 lag = silk_LIMIT_int( lag, min_lag, max_lag ); 405 start_lag = silk_max_int( lag - 2, min_lag ); 406 end_lag = silk_min_int( lag + 2, max_lag ); 407 lag_new = lag; /* to avoid undefined lag */ 408 CBimax = 0; /* to avoid undefined lag */ 409 silk_assert( CCmax >= 0.0f ); 410 *LTPCorr = (silk_float)sqrt( CCmax / nb_subfr ); /* Output normalized correlation */ 411 412 CCmax = -1000.0f; 413 414 /* Calculate the correlations and energies needed in stage 3 */ 415 silk_P_Ana_calc_corr_st3( cross_corr_st3, frame, start_lag, sf_length, nb_subfr, complexity ); 416 silk_P_Ana_calc_energy_st3( energies_st3, frame, start_lag, sf_length, nb_subfr, complexity ); 417 418 lag_counter = 0; 419 silk_assert( lag == silk_SAT16( lag ) ); 420 contour_bias = PE_FLATCONTOUR_BIAS / lag; 421 422 /* Set up cbk parameters according to complexity setting and frame length */ 423 if( nb_subfr == PE_MAX_NB_SUBFR ) { 424 nb_cbk_search = (opus_int)silk_nb_cbk_searchs_stage3[ complexity ]; 425 cbk_size = PE_NB_CBKS_STAGE3_MAX; 426 Lag_CB_ptr = &silk_CB_lags_stage3[ 0 ][ 0 ]; 427 } else { 428 nb_cbk_search = PE_NB_CBKS_STAGE3_10MS; 429 cbk_size = PE_NB_CBKS_STAGE3_10MS; 430 Lag_CB_ptr = &silk_CB_lags_stage3_10_ms[ 0 ][ 0 ]; 431 } 432 433 for( d = start_lag; d <= end_lag; d++ ) { 434 for( j = 0; j < nb_cbk_search; j++ ) { 435 cross_corr = 0.0; 436 energy = eps; 437 for( k = 0; k < nb_subfr; k++ ) { 438 energy += energies_st3[ k ][ j ][ lag_counter ]; 439 cross_corr += cross_corr_st3[ k ][ j ][ lag_counter ]; 440 } 441 if( cross_corr > 0.0 ) { 442 CCmax_new = (silk_float)(cross_corr * cross_corr / energy); 443 /* Reduce depending on flatness of contour */ 444 CCmax_new *= 1.0f - contour_bias * j; 445 } else { 446 CCmax_new = 0.0f; 447 } 448 449 if( CCmax_new > CCmax && 450 ( d + (opus_int)silk_CB_lags_stage3[ 0 ][ j ] ) <= max_lag 451 ) { 452 CCmax = CCmax_new; 453 lag_new = d; 454 CBimax = j; 455 } 456 } 457 lag_counter++; 458 } 459 460 for( k = 0; k < nb_subfr; k++ ) { 461 pitch_out[ k ] = lag_new + matrix_ptr( Lag_CB_ptr, k, CBimax, cbk_size ); 462 pitch_out[ k ] = silk_LIMIT( pitch_out[ k ], min_lag, PE_MAX_LAG_MS * Fs_kHz ); 463 } 464 *lagIndex = (opus_int16)( lag_new - min_lag ); 465 *contourIndex = (opus_int8)CBimax; 466 } else { /* Fs_kHz == 8 */ 467 /* Save Lags and correlation */ 468 silk_assert( CCmax >= 0.0f ); 469 *LTPCorr = (silk_float)sqrt( CCmax / nb_subfr ); /* Output normalized correlation */ 470 for( k = 0; k < nb_subfr; k++ ) { 471 pitch_out[ k ] = lag + matrix_ptr( Lag_CB_ptr, k, CBimax, cbk_size ); 472 pitch_out[ k ] = silk_LIMIT( pitch_out[ k ], min_lag_8kHz, PE_MAX_LAG_MS * Fs_kHz ); 473 } 474 *lagIndex = (opus_int16)( lag - min_lag_8kHz ); 475 *contourIndex = (opus_int8)CBimax; 476 } 477 silk_assert( *lagIndex >= 0 ); 478 /* return as voiced */ 479 return 0; 480} 481 482static void silk_P_Ana_calc_corr_st3( 483 silk_float cross_corr_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ], /* O 3 DIM correlation array */ 484 const silk_float frame[], /* I vector to correlate */ 485 opus_int start_lag, /* I start lag */ 486 opus_int sf_length, /* I sub frame length */ 487 opus_int nb_subfr, /* I number of subframes */ 488 opus_int complexity /* I Complexity setting */ 489) 490 /*********************************************************************** 491 Calculates the correlations used in stage 3 search. In order to cover 492 the whole lag codebook for all the searched offset lags (lag +- 2), 493 the following correlations are needed in each sub frame: 494 495 sf1: lag range [-8,...,7] total 16 correlations 496 sf2: lag range [-4,...,4] total 9 correlations 497 sf3: lag range [-3,....4] total 8 correltions 498 sf4: lag range [-6,....8] total 15 correlations 499 500 In total 48 correlations. The direct implementation computed in worst case 501 4*12*5 = 240 correlations, but more likely around 120. 502 **********************************************************************/ 503{ 504 const silk_float *target_ptr, *basis_ptr; 505 opus_int i, j, k, lag_counter, lag_low, lag_high; 506 opus_int nb_cbk_search, delta, idx, cbk_size; 507 silk_float scratch_mem[ SCRATCH_SIZE ]; 508 const opus_int8 *Lag_range_ptr, *Lag_CB_ptr; 509 510 silk_assert( complexity >= SILK_PE_MIN_COMPLEX ); 511 silk_assert( complexity <= SILK_PE_MAX_COMPLEX ); 512 513 if( nb_subfr == PE_MAX_NB_SUBFR ) { 514 Lag_range_ptr = &silk_Lag_range_stage3[ complexity ][ 0 ][ 0 ]; 515 Lag_CB_ptr = &silk_CB_lags_stage3[ 0 ][ 0 ]; 516 nb_cbk_search = silk_nb_cbk_searchs_stage3[ complexity ]; 517 cbk_size = PE_NB_CBKS_STAGE3_MAX; 518 } else { 519 silk_assert( nb_subfr == PE_MAX_NB_SUBFR >> 1); 520 Lag_range_ptr = &silk_Lag_range_stage3_10_ms[ 0 ][ 0 ]; 521 Lag_CB_ptr = &silk_CB_lags_stage3_10_ms[ 0 ][ 0 ]; 522 nb_cbk_search = PE_NB_CBKS_STAGE3_10MS; 523 cbk_size = PE_NB_CBKS_STAGE3_10MS; 524 } 525 526 target_ptr = &frame[ silk_LSHIFT( sf_length, 2 ) ]; /* Pointer to middle of frame */ 527 for( k = 0; k < nb_subfr; k++ ) { 528 lag_counter = 0; 529 530 /* Calculate the correlations for each subframe */ 531 lag_low = matrix_ptr( Lag_range_ptr, k, 0, 2 ); 532 lag_high = matrix_ptr( Lag_range_ptr, k, 1, 2 ); 533 for( j = lag_low; j <= lag_high; j++ ) { 534 basis_ptr = target_ptr - ( start_lag + j ); 535 silk_assert( lag_counter < SCRATCH_SIZE ); 536 scratch_mem[ lag_counter ] = (silk_float)silk_inner_product_FLP( target_ptr, basis_ptr, sf_length ); 537 lag_counter++; 538 } 539 540 delta = matrix_ptr( Lag_range_ptr, k, 0, 2 ); 541 for( i = 0; i < nb_cbk_search; i++ ) { 542 /* Fill out the 3 dim array that stores the correlations for */ 543 /* each code_book vector for each start lag */ 544 idx = matrix_ptr( Lag_CB_ptr, k, i, cbk_size ) - delta; 545 for( j = 0; j < PE_NB_STAGE3_LAGS; j++ ) { 546 silk_assert( idx + j < SCRATCH_SIZE ); 547 silk_assert( idx + j < lag_counter ); 548 cross_corr_st3[ k ][ i ][ j ] = scratch_mem[ idx + j ]; 549 } 550 } 551 target_ptr += sf_length; 552 } 553} 554 555static void silk_P_Ana_calc_energy_st3( 556 silk_float energies_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ], /* O 3 DIM correlation array */ 557 const silk_float frame[], /* I vector to correlate */ 558 opus_int start_lag, /* I start lag */ 559 opus_int sf_length, /* I sub frame length */ 560 opus_int nb_subfr, /* I number of subframes */ 561 opus_int complexity /* I Complexity setting */ 562) 563/**************************************************************** 564Calculate the energies for first two subframes. The energies are 565calculated recursively. 566****************************************************************/ 567{ 568 const silk_float *target_ptr, *basis_ptr; 569 double energy; 570 opus_int k, i, j, lag_counter; 571 opus_int nb_cbk_search, delta, idx, cbk_size, lag_diff; 572 silk_float scratch_mem[ SCRATCH_SIZE ]; 573 const opus_int8 *Lag_range_ptr, *Lag_CB_ptr; 574 575 silk_assert( complexity >= SILK_PE_MIN_COMPLEX ); 576 silk_assert( complexity <= SILK_PE_MAX_COMPLEX ); 577 578 if( nb_subfr == PE_MAX_NB_SUBFR ) { 579 Lag_range_ptr = &silk_Lag_range_stage3[ complexity ][ 0 ][ 0 ]; 580 Lag_CB_ptr = &silk_CB_lags_stage3[ 0 ][ 0 ]; 581 nb_cbk_search = silk_nb_cbk_searchs_stage3[ complexity ]; 582 cbk_size = PE_NB_CBKS_STAGE3_MAX; 583 } else { 584 silk_assert( nb_subfr == PE_MAX_NB_SUBFR >> 1); 585 Lag_range_ptr = &silk_Lag_range_stage3_10_ms[ 0 ][ 0 ]; 586 Lag_CB_ptr = &silk_CB_lags_stage3_10_ms[ 0 ][ 0 ]; 587 nb_cbk_search = PE_NB_CBKS_STAGE3_10MS; 588 cbk_size = PE_NB_CBKS_STAGE3_10MS; 589 } 590 591 target_ptr = &frame[ silk_LSHIFT( sf_length, 2 ) ]; 592 for( k = 0; k < nb_subfr; k++ ) { 593 lag_counter = 0; 594 595 /* Calculate the energy for first lag */ 596 basis_ptr = target_ptr - ( start_lag + matrix_ptr( Lag_range_ptr, k, 0, 2 ) ); 597 energy = silk_energy_FLP( basis_ptr, sf_length ) + 1e-3; 598 silk_assert( energy >= 0.0 ); 599 scratch_mem[lag_counter] = (silk_float)energy; 600 lag_counter++; 601 602 lag_diff = ( matrix_ptr( Lag_range_ptr, k, 1, 2 ) - matrix_ptr( Lag_range_ptr, k, 0, 2 ) + 1 ); 603 for( i = 1; i < lag_diff; i++ ) { 604 /* remove part outside new window */ 605 energy -= basis_ptr[sf_length - i] * (double)basis_ptr[sf_length - i]; 606 silk_assert( energy >= 0.0 ); 607 608 /* add part that comes into window */ 609 energy += basis_ptr[ -i ] * (double)basis_ptr[ -i ]; 610 silk_assert( energy >= 0.0 ); 611 silk_assert( lag_counter < SCRATCH_SIZE ); 612 scratch_mem[lag_counter] = (silk_float)energy; 613 lag_counter++; 614 } 615 616 delta = matrix_ptr( Lag_range_ptr, k, 0, 2 ); 617 for( i = 0; i < nb_cbk_search; i++ ) { 618 /* Fill out the 3 dim array that stores the correlations for */ 619 /* each code_book vector for each start lag */ 620 idx = matrix_ptr( Lag_CB_ptr, k, i, cbk_size ) - delta; 621 for( j = 0; j < PE_NB_STAGE3_LAGS; j++ ) { 622 silk_assert( idx + j < SCRATCH_SIZE ); 623 silk_assert( idx + j < lag_counter ); 624 energies_st3[ k ][ i ][ j ] = scratch_mem[ idx + j ]; 625 silk_assert( energies_st3[ k ][ i ][ j ] >= 0.0f ); 626 } 627 } 628 target_ptr += sf_length; 629 } 630} 631