silk/float/pitch_analysis_core_FLP.c

   1 /***********************************************************************
   2 Copyright (c) 2006-2011, Skype Limited. All rights reserved.
   3 Redistribution and use in source and binary forms, with or without
   4 modification, are permitted provided that the following conditions
   5 are met:
   6 - Redistributions of source code must retain the above copyright notice,
   7 this list of conditions and the following disclaimer.
   8 - Redistributions in binary form must reproduce the above copyright
   9 notice, this list of conditions and the following disclaimer in the
  10 documentation and/or other materials provided with the distribution.
  11 - Neither the name of Internet Society, IETF or IETF Trust, nor the
  12 names of specific contributors, may be used to endorse or promote
  13 products derived from this software without specific prior written
  14 permission.
  15 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS “AS IS”
  16 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  17 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  18 ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
  19 LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  20 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
  21 SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  22 INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  23 CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  24 ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  25 POSSIBILITY 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 /************************************************************/
  45 static 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
  54 static 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 /************************************************************/
  66 opus_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
 482 static 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
 555 static 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 /****************************************************************
 564 Calculate the energies for first two subframes. The energies are
 565 calculated 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 }