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
  41 /************************************************************/
  42 /* Internally used functions                                */
  43 /************************************************************/
  44 static void silk_P_Ana_calc_corr_st3(
  45     silk_float cross_corr_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ], /* O 3 DIM correlation array */
  46     const silk_float    frame[],            /* I vector to correlate                                            */
  47     opus_int            start_lag,          /* I start lag                                                      */
  48     opus_int            sf_length,          /* I sub frame length                                               */
  49     opus_int            nb_subfr,           /* I number of subframes                                            */
  50     opus_int            complexity          /* I Complexity setting                                             */
  51 );
  52
  53 static void silk_P_Ana_calc_energy_st3(
  54     silk_float energies_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ], /* O 3 DIM correlation array */
  55     const silk_float    frame[],            /* I vector to correlate                                            */
  56     opus_int            start_lag,          /* I start lag                                                      */
  57     opus_int            sf_length,          /* I sub frame length                                               */
  58     opus_int            nb_subfr,           /* I number of subframes                                            */
  59     opus_int            complexity          /* I Complexity setting                                             */
  60 );
  61
  62 /************************************************************/
  63 /* CORE PITCH ANALYSIS FUNCTION                             */
  64 /************************************************************/
  65 opus_int silk_pitch_analysis_core_FLP(      /* O    Voicing estimate: 0 voiced, 1 unvoiced                      */
  66     const silk_float    *frame,             /* I    Signal of length PE_FRAME_LENGTH_MS*Fs_kHz                  */
  67     opus_int            *pitch_out,         /* O    Pitch lag values [nb_subfr]                                 */
  68     opus_int16          *lagIndex,          /* O    Lag Index                                                   */
  69     opus_int8           *contourIndex,      /* O    Pitch contour Index                                         */
  70     silk_float          *LTPCorr,           /* I/O  Normalized correlation; input: value from previous frame    */
  71     opus_int            prevLag,            /* I    Last lag of previous frame; set to zero is unvoiced         */
  72     const silk_float    search_thres1,      /* I    First stage threshold for lag candidates 0 - 1              */
  73     const silk_float    search_thres2,      /* I    Final threshold for lag candidates 0 - 1                    */
  74     const opus_int      Fs_kHz,             /* I    sample frequency (kHz)                                      */
  75     const opus_int      complexity,         /* I    Complexity setting, 0-2, where 2 is highest                 */
  76     const opus_int      nb_subfr            /* I    Number of 5 ms subframes                                    */
  77 )
  78 {
  79     opus_int   i, k, d, j;
  80     silk_float frame_8kHz[  PE_MAX_FRAME_LENGTH_MS * 8 ];
  81     silk_float frame_4kHz[  PE_MAX_FRAME_LENGTH_MS * 4 ];
  82     opus_int16 frame_8_FIX[ PE_MAX_FRAME_LENGTH_MS * 8 ];
  83     opus_int16 frame_4_FIX[ PE_MAX_FRAME_LENGTH_MS * 4 ];
  84     opus_int32 filt_state[ 6 ];
  85     silk_float threshold, contour_bias;
  86     silk_float C[ PE_MAX_NB_SUBFR][ (PE_MAX_LAG >> 1) + 5 ];
  87     silk_float CC[ PE_NB_CBKS_STAGE2_EXT ];
  88     const silk_float *target_ptr, *basis_ptr;
  89     double    cross_corr, normalizer, energy, energy_tmp;
  90     opus_int   d_srch[ PE_D_SRCH_LENGTH ];
  91     opus_int16 d_comp[ (PE_MAX_LAG >> 1) + 5 ];
  92     opus_int   length_d_srch, length_d_comp;
  93     silk_float Cmax, CCmax, CCmax_b, CCmax_new_b, CCmax_new;
  94     opus_int   CBimax, CBimax_new, lag, start_lag, end_lag, lag_new;
  95     opus_int   cbk_size;
  96     silk_float lag_log2, prevLag_log2, delta_lag_log2_sqr;
  97     silk_float energies_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ];
  98     silk_float cross_corr_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ];
  99     opus_int   lag_counter;
 100     opus_int   frame_length, frame_length_8kHz, frame_length_4kHz;
 101     opus_int   sf_length, sf_length_8kHz, sf_length_4kHz;
 102     opus_int   min_lag, min_lag_8kHz, min_lag_4kHz;
 103     opus_int   max_lag, max_lag_8kHz, max_lag_4kHz;
 104     opus_int   nb_cbk_search;
 105     const opus_int8 *Lag_CB_ptr;
 106
 107     /* Check for valid sampling frequency */
 108     silk_assert( Fs_kHz == 8 || Fs_kHz == 12 || Fs_kHz == 16 );
 109
 110     /* Check for valid complexity setting */
 111     silk_assert( complexity >= SILK_PE_MIN_COMPLEX );
 112     silk_assert( complexity <= SILK_PE_MAX_COMPLEX );
 113
 114     silk_assert( search_thres1 >= 0.0f && search_thres1 <= 1.0f );
 115     silk_assert( search_thres2 >= 0.0f && search_thres2 <= 1.0f );
 116
 117     /* Set up frame lengths max / min lag for the sampling frequency */
 118     frame_length      = ( PE_LTP_MEM_LENGTH_MS + nb_subfr * PE_SUBFR_LENGTH_MS ) * Fs_kHz;
 119     frame_length_4kHz = ( PE_LTP_MEM_LENGTH_MS + nb_subfr * PE_SUBFR_LENGTH_MS ) * 4;
 120     frame_length_8kHz = ( PE_LTP_MEM_LENGTH_MS + nb_subfr * PE_SUBFR_LENGTH_MS ) * 8;
 121     sf_length         = PE_SUBFR_LENGTH_MS * Fs_kHz;
 122     sf_length_4kHz    = PE_SUBFR_LENGTH_MS * 4;
 123     sf_length_8kHz    = PE_SUBFR_LENGTH_MS * 8;
 124     min_lag           = PE_MIN_LAG_MS * Fs_kHz;
 125     min_lag_4kHz      = PE_MIN_LAG_MS * 4;
 126     min_lag_8kHz      = PE_MIN_LAG_MS * 8;
 127     max_lag           = PE_MAX_LAG_MS * Fs_kHz - 1;
 128     max_lag_4kHz      = PE_MAX_LAG_MS * 4;
 129     max_lag_8kHz      = PE_MAX_LAG_MS * 8 - 1;
 130
 131     /* Resample from input sampled at Fs_kHz to 8 kHz */
 132     if( Fs_kHz == 16 ) {
 133         /* Resample to 16 -> 8 khz */
 134         opus_int16 frame_16_FIX[ 16 * PE_MAX_FRAME_LENGTH_MS ];
 135         silk_float2short_array( frame_16_FIX, frame, frame_length );
 136         silk_memset( filt_state, 0, 2 * sizeof( opus_int32 ) );
 137         silk_resampler_down2( filt_state, frame_8_FIX, frame_16_FIX, frame_length );
 138         silk_short2float_array( frame_8kHz, frame_8_FIX, frame_length_8kHz );
 139     } else if( Fs_kHz == 12 ) {
 140         /* Resample to 12 -> 8 khz */
 141         opus_int16 frame_12_FIX[ 12 * PE_MAX_FRAME_LENGTH_MS ];
 142         silk_float2short_array( frame_12_FIX, frame, frame_length );
 143         silk_memset( filt_state, 0, 6 * sizeof( opus_int32 ) );
 144         silk_resampler_down2_3( filt_state, frame_8_FIX, frame_12_FIX, frame_length );
 145         silk_short2float_array( frame_8kHz, frame_8_FIX, frame_length_8kHz );
 146     } else {
 147         silk_assert( Fs_kHz == 8 );
 148         silk_float2short_array( frame_8_FIX, frame, frame_length_8kHz );
 149     }
 150
 151     /* Decimate again to 4 kHz */
 152     silk_memset( filt_state, 0, 2 * sizeof( opus_int32 ) );
 153     silk_resampler_down2( filt_state, frame_4_FIX, frame_8_FIX, frame_length_8kHz );
 154     silk_short2float_array( frame_4kHz, frame_4_FIX, frame_length_4kHz );
 155
 156     /* Low-pass filter */
 157     for( i = frame_length_4kHz - 1; i > 0; i-- ) {
 158         frame_4kHz[ i ] += frame_4kHz[ i - 1 ];
 159     }
 160
 161     /******************************************************************************
 162     * FIRST STAGE, operating in 4 khz
 163     ******************************************************************************/
 164     silk_memset(C, 0, sizeof(silk_float) * nb_subfr * ((PE_MAX_LAG >> 1) + 5));
 165     target_ptr = &frame_4kHz[ silk_LSHIFT( sf_length_4kHz, 2 ) ];
 166     for( k = 0; k < nb_subfr >> 1; k++ ) {
 167         /* Check that we are within range of the array */
 168         silk_assert( target_ptr >= frame_4kHz );
 169         silk_assert( target_ptr + sf_length_8kHz <= frame_4kHz + frame_length_4kHz );
 170
 171         basis_ptr = target_ptr - min_lag_4kHz;
 172
 173         /* Check that we are within range of the array */
 174         silk_assert( basis_ptr >= frame_4kHz );
 175         silk_assert( basis_ptr + sf_length_8kHz <= frame_4kHz + frame_length_4kHz );
 176
 177         /* Calculate first vector products before loop */
 178         cross_corr = silk_inner_product_FLP( target_ptr, basis_ptr, sf_length_8kHz );
 179         normalizer = silk_energy_FLP( target_ptr, sf_length_8kHz ) +
 180                      silk_energy_FLP( basis_ptr,  sf_length_8kHz ) +
 181                      sf_length_8kHz * 4000.0f;
 182
 183         C[ 0 ][ min_lag_4kHz ] += (silk_float)( 2 * cross_corr / 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)( 2 * cross_corr / 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     if( Cmax < 0.2f ) {
 218         silk_memset( pitch_out, 0, nb_subfr * sizeof( opus_int ) );
 219         *LTPCorr      = 0.0f;
 220         *lagIndex     = 0;
 221         *contourIndex = 0;
 222         return 1;
 223     }
 224
 225     threshold = search_thres1 * Cmax;
 226     for( i = 0; i < length_d_srch; i++ ) {
 227         /* Convert to 8 kHz indices for the sorted correlation that exceeds the threshold */
 228         if( C[ 0 ][ min_lag_4kHz + i ] > threshold ) {
 229             d_srch[ i ] = silk_LSHIFT( d_srch[ i ] + min_lag_4kHz, 1 );
 230         } else {
 231             length_d_srch = i;
 232             break;
 233         }
 234     }
 235     silk_assert( length_d_srch > 0 );
 236
 237     for( i = min_lag_8kHz - 5; i < max_lag_8kHz + 5; i++ ) {
 238         d_comp[ i ] = 0;
 239     }
 240     for( i = 0; i < length_d_srch; i++ ) {
 241         d_comp[ d_srch[ i ] ] = 1;
 242     }
 243
 244     /* Convolution */
 245     for( i = max_lag_8kHz + 3; i >= min_lag_8kHz; i-- ) {
 246         d_comp[ i ] += d_comp[ i - 1 ] + d_comp[ i - 2 ];
 247     }
 248
 249     length_d_srch = 0;
 250     for( i = min_lag_8kHz; i < max_lag_8kHz + 1; i++ ) {
 251         if( d_comp[ i + 1 ] > 0 ) {
 252             d_srch[ length_d_srch ] = i;
 253             length_d_srch++;
 254         }
 255     }
 256
 257     /* Convolution */
 258     for( i = max_lag_8kHz + 3; i >= min_lag_8kHz; i-- ) {
 259         d_comp[ i ] += d_comp[ i - 1 ] + d_comp[ i - 2 ] + d_comp[ i - 3 ];
 260     }
 261
 262     length_d_comp = 0;
 263     for( i = min_lag_8kHz; i < max_lag_8kHz + 4; i++ ) {
 264         if( d_comp[ i ] > 0 ) {
 265             d_comp[ length_d_comp ] = (opus_int16)( i - 2 );
 266             length_d_comp++;
 267         }
 268     }
 269
 270     /**********************************************************************************
 271     ** SECOND STAGE, operating at 8 kHz, on lag sections with high correlation
 272     *************************************************************************************/
 273     /*********************************************************************************
 274     * Find energy of each subframe projected onto its history, for a range of delays
 275     *********************************************************************************/
 276     silk_memset( C, 0, PE_MAX_NB_SUBFR*((PE_MAX_LAG >> 1) + 5) * sizeof(silk_float));
 277
 278     if( Fs_kHz == 8 ) {
 279         target_ptr = &frame[ PE_LTP_MEM_LENGTH_MS * 8 ];
 280     } else {
 281         target_ptr = &frame_8kHz[ PE_LTP_MEM_LENGTH_MS * 8 ];
 282     }
 283     for( k = 0; k < nb_subfr; k++ ) {
 284         energy_tmp = silk_energy_FLP( target_ptr, sf_length_8kHz ) + 1.0;
 285         for( j = 0; j < length_d_comp; j++ ) {
 286             d = d_comp[ j ];
 287             basis_ptr = target_ptr - d;
 288             cross_corr = silk_inner_product_FLP( basis_ptr, target_ptr, sf_length_8kHz );
 289             if( cross_corr > 0.0f ) {
 290                 energy = silk_energy_FLP( basis_ptr, sf_length_8kHz );
 291                 C[ k ][ d ] = (silk_float)( 2 * cross_corr / ( energy + energy_tmp ) );
 292             } else {
 293                 C[ k ][ d ] = 0.0f;
 294             }
 295         }
 296         target_ptr += sf_length_8kHz;
 297     }
 298
 299     /* search over lag range and lags codebook */
 300     /* scale factor for lag codebook, as a function of center lag */
 301
 302     CCmax   = 0.0f; /* This value doesn't matter */
 303     CCmax_b = -1000.0f;
 304
 305     CBimax = 0; /* To avoid returning undefined lag values */
 306     lag = -1;   /* To check if lag with strong enough correlation has been found */
 307
 308     if( prevLag > 0 ) {
 309         if( Fs_kHz == 12 ) {
 310             prevLag = silk_LSHIFT( prevLag, 1 ) / 3;
 311         } else if( Fs_kHz == 16 ) {
 312             prevLag = silk_RSHIFT( prevLag, 1 );
 313         }
 314         prevLag_log2 = silk_log2( (silk_float)prevLag );
 315     } else {
 316         prevLag_log2 = 0;
 317     }
 318
 319     /* Set up stage 2 codebook based on number of subframes */
 320     if( nb_subfr == PE_MAX_NB_SUBFR ) {
 321         cbk_size   = PE_NB_CBKS_STAGE2_EXT;
 322         Lag_CB_ptr = &silk_CB_lags_stage2[ 0 ][ 0 ];
 323         if( Fs_kHz == 8 && complexity > SILK_PE_MIN_COMPLEX ) {
 324             /* If input is 8 khz use a larger codebook here because it is last stage */
 325             nb_cbk_search = PE_NB_CBKS_STAGE2_EXT;
 326         } else {
 327             nb_cbk_search = PE_NB_CBKS_STAGE2;
 328         }
 329     } else {
 330         cbk_size       = PE_NB_CBKS_STAGE2_10MS;
 331         Lag_CB_ptr     = &silk_CB_lags_stage2_10_ms[ 0 ][ 0 ];
 332         nb_cbk_search  = PE_NB_CBKS_STAGE2_10MS;
 333     }
 334
 335     for( k = 0; k < length_d_srch; k++ ) {
 336         d = d_srch[ k ];
 337         for( j = 0; j < nb_cbk_search; j++ ) {
 338             CC[j] = 0.0f;
 339             for( i = 0; i < nb_subfr; i++ ) {
 340                 /* Try all codebooks */
 341                 CC[ j ] += C[ i ][ d + matrix_ptr( Lag_CB_ptr, i, j, cbk_size )];
 342             }
 343         }
 344         /* Find best codebook */
 345         CCmax_new  = -1000.0f;
 346         CBimax_new = 0;
 347         for( i = 0; i < nb_cbk_search; i++ ) {
 348             if( CC[ i ] > CCmax_new ) {
 349                 CCmax_new = CC[ i ];
 350                 CBimax_new = i;
 351             }
 352         }
 353
 354         /* Bias towards shorter lags */
 355         lag_log2 = silk_log2( (silk_float)d );
 356         CCmax_new_b = CCmax_new - PE_SHORTLAG_BIAS * nb_subfr * lag_log2;
 357
 358         /* Bias towards previous lag */
 359         if( prevLag > 0 ) {
 360             delta_lag_log2_sqr = lag_log2 - prevLag_log2;
 361             delta_lag_log2_sqr *= delta_lag_log2_sqr;
 362             CCmax_new_b -= PE_PREVLAG_BIAS * nb_subfr * (*LTPCorr) * delta_lag_log2_sqr / ( delta_lag_log2_sqr + 0.5f );
 363         }
 364
 365         if( CCmax_new_b > CCmax_b &&                /* Find maximum biased correlation                  */
 366             CCmax_new > nb_subfr * search_thres2    /* Correlation needs to be high enough to be voiced */
 367         ) {
 368             CCmax_b = CCmax_new_b;
 369             CCmax   = CCmax_new;
 370             lag     = d;
 371             CBimax  = CBimax_new;
 372         }
 373     }
 374
 375     if( lag == -1 ) {
 376         /* No suitable candidate found */
 377         silk_memset( pitch_out, 0, PE_MAX_NB_SUBFR * sizeof(opus_int) );
 378         *LTPCorr      = 0.0f;
 379         *lagIndex     = 0;
 380         *contourIndex = 0;
 381         return 1;
 382     }
 383
 384     /* Output normalized correlation */
 385     *LTPCorr = (silk_float)( CCmax / nb_subfr );
 386     silk_assert( *LTPCorr >= 0.0f );
 387
 388     if( Fs_kHz > 8 ) {
 389         /* Search in original signal */
 390
 391         /* Compensate for decimation */
 392         silk_assert( lag == silk_SAT16( lag ) );
 393         if( Fs_kHz == 12 ) {
 394             lag = silk_RSHIFT_ROUND( silk_SMULBB( lag, 3 ), 1 );
 395         } else { /* Fs_kHz == 16 */
 396             lag = silk_LSHIFT( lag, 1 );
 397         }
 398
 399         lag = silk_LIMIT_int( lag, min_lag, max_lag );
 400         start_lag = silk_max_int( lag - 2, min_lag );
 401         end_lag   = silk_min_int( lag + 2, max_lag );
 402         lag_new   = lag;                                    /* to avoid undefined lag */
 403         CBimax    = 0;                                      /* to avoid undefined lag */
 404
 405         CCmax = -1000.0f;
 406
 407         /* Calculate the correlations and energies needed in stage 3 */
 408         silk_P_Ana_calc_corr_st3( cross_corr_st3, frame, start_lag, sf_length, nb_subfr, complexity );
 409         silk_P_Ana_calc_energy_st3( energies_st3, frame, start_lag, sf_length, nb_subfr, complexity );
 410
 411         lag_counter = 0;
 412         silk_assert( lag == silk_SAT16( lag ) );
 413         contour_bias = PE_FLATCONTOUR_BIAS / lag;
 414
 415         /* Set up cbk parameters according to complexity setting and frame length */
 416         if( nb_subfr == PE_MAX_NB_SUBFR ) {
 417             nb_cbk_search = (opus_int)silk_nb_cbk_searchs_stage3[ complexity ];
 418             cbk_size      = PE_NB_CBKS_STAGE3_MAX;
 419             Lag_CB_ptr    = &silk_CB_lags_stage3[ 0 ][ 0 ];
 420         } else {
 421             nb_cbk_search = PE_NB_CBKS_STAGE3_10MS;
 422             cbk_size      = PE_NB_CBKS_STAGE3_10MS;
 423             Lag_CB_ptr    = &silk_CB_lags_stage3_10_ms[ 0 ][ 0 ];
 424         }
 425
 426         target_ptr = &frame[ PE_LTP_MEM_LENGTH_MS * Fs_kHz ];
 427         energy_tmp = silk_energy_FLP( target_ptr, nb_subfr * sf_length ) + 1.0;
 428         for( d = start_lag; d <= end_lag; d++ ) {
 429             for( j = 0; j < nb_cbk_search; j++ ) {
 430                 cross_corr = 0.0;
 431                 energy = energy_tmp;
 432                 for( k = 0; k < nb_subfr; k++ ) {
 433                     cross_corr += cross_corr_st3[ k ][ j ][ lag_counter ];
 434                     energy     +=   energies_st3[ k ][ j ][ lag_counter ];
 435                 }
 436                 if( cross_corr > 0.0 ) {
 437                     CCmax_new = (silk_float)( 2 * cross_corr / energy );
 438                     /* Reduce depending on flatness of contour */
 439                     CCmax_new *= 1.0f - contour_bias * j;
 440                 } else {
 441                     CCmax_new = 0.0f;
 442                 }
 443
 444                 if( CCmax_new > CCmax && ( d + (opus_int)silk_CB_lags_stage3[ 0 ][ j ] ) <= max_lag ) {
 445                     CCmax   = CCmax_new;
 446                     lag_new = d;
 447                     CBimax  = j;
 448                 }
 449             }
 450             lag_counter++;
 451         }
 452
 453         for( k = 0; k < nb_subfr; k++ ) {
 454             pitch_out[ k ] = lag_new + matrix_ptr( Lag_CB_ptr, k, CBimax, cbk_size );
 455             pitch_out[ k ] = silk_LIMIT( pitch_out[ k ], min_lag, PE_MAX_LAG_MS * Fs_kHz );
 456         }
 457         *lagIndex = (opus_int16)( lag_new - min_lag );
 458         *contourIndex = (opus_int8)CBimax;
 459     } else {        /* Fs_kHz == 8 */
 460         /* Save Lags */
 461         for( k = 0; k < nb_subfr; k++ ) {
 462             pitch_out[ k ] = lag + matrix_ptr( Lag_CB_ptr, k, CBimax, cbk_size );
 463             pitch_out[ k ] = silk_LIMIT( pitch_out[ k ], min_lag_8kHz, PE_MAX_LAG_MS * 8 );
 464         }
 465         *lagIndex = (opus_int16)( lag - min_lag_8kHz );
 466         *contourIndex = (opus_int8)CBimax;
 467     }
 468     silk_assert( *lagIndex >= 0 );
 469     /* return as voiced */
 470     return 0;
 471 }
 472
 473 /***********************************************************************
 474  * Calculates the correlations used in stage 3 search. In order to cover
 475  * the whole lag codebook for all the searched offset lags (lag +- 2),
 476  * the following correlations are needed in each sub frame:
 477  *
 478  * sf1: lag range [-8,...,7] total 16 correlations
 479  * sf2: lag range [-4,...,4] total 9 correlations
 480  * sf3: lag range [-3,....4] total 8 correltions
 481  * sf4: lag range [-6,....8] total 15 correlations
 482  *
 483  * In total 48 correlations. The direct implementation computed in worst
 484  * case 4*12*5 = 240 correlations, but more likely around 120.
 485  ***********************************************************************/
 486 static void silk_P_Ana_calc_corr_st3(
 487     silk_float cross_corr_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ], /* O 3 DIM correlation array */
 488     const silk_float    frame[],            /* I vector to correlate                                            */
 489     opus_int            start_lag,          /* I start lag                                                      */
 490     opus_int            sf_length,          /* I sub frame length                                               */
 491     opus_int            nb_subfr,           /* I number of subframes                                            */
 492     opus_int            complexity          /* I Complexity setting                                             */
 493 )
 494 {
 495     const silk_float *target_ptr, *basis_ptr;
 496     opus_int   i, j, k, lag_counter, lag_low, lag_high;
 497     opus_int   nb_cbk_search, delta, idx, cbk_size;
 498     silk_float scratch_mem[ SCRATCH_SIZE ];
 499     const opus_int8 *Lag_range_ptr, *Lag_CB_ptr;
 500
 501     silk_assert( complexity >= SILK_PE_MIN_COMPLEX );
 502     silk_assert( complexity <= SILK_PE_MAX_COMPLEX );
 503
 504     if( nb_subfr == PE_MAX_NB_SUBFR ) {
 505         Lag_range_ptr = &silk_Lag_range_stage3[ complexity ][ 0 ][ 0 ];
 506         Lag_CB_ptr    = &silk_CB_lags_stage3[ 0 ][ 0 ];
 507         nb_cbk_search = silk_nb_cbk_searchs_stage3[ complexity ];
 508         cbk_size      = PE_NB_CBKS_STAGE3_MAX;
 509     } else {
 510         silk_assert( nb_subfr == PE_MAX_NB_SUBFR >> 1);
 511         Lag_range_ptr = &silk_Lag_range_stage3_10_ms[ 0 ][ 0 ];
 512         Lag_CB_ptr    = &silk_CB_lags_stage3_10_ms[ 0 ][ 0 ];
 513         nb_cbk_search = PE_NB_CBKS_STAGE3_10MS;
 514         cbk_size      = PE_NB_CBKS_STAGE3_10MS;
 515     }
 516
 517     target_ptr = &frame[ silk_LSHIFT( sf_length, 2 ) ]; /* Pointer to middle of frame */
 518     for( k = 0; k < nb_subfr; k++ ) {
 519         lag_counter = 0;
 520
 521         /* Calculate the correlations for each subframe */
 522         lag_low  = matrix_ptr( Lag_range_ptr, k, 0, 2 );
 523         lag_high = matrix_ptr( Lag_range_ptr, k, 1, 2 );
 524         for( j = lag_low; j <= lag_high; j++ ) {
 525             basis_ptr = target_ptr - ( start_lag + j );
 526             silk_assert( lag_counter < SCRATCH_SIZE );
 527             scratch_mem[ lag_counter ] = (silk_float)silk_inner_product_FLP( target_ptr, basis_ptr, sf_length );
 528             lag_counter++;
 529         }
 530
 531         delta = matrix_ptr( Lag_range_ptr, k, 0, 2 );
 532         for( i = 0; i < nb_cbk_search; i++ ) {
 533             /* Fill out the 3 dim array that stores the correlations for */
 534             /* each code_book vector for each start lag */
 535             idx = matrix_ptr( Lag_CB_ptr, k, i, cbk_size ) - delta;
 536             for( j = 0; j < PE_NB_STAGE3_LAGS; j++ ) {
 537                 silk_assert( idx + j < SCRATCH_SIZE );
 538                 silk_assert( idx + j < lag_counter );
 539                 cross_corr_st3[ k ][ i ][ j ] = scratch_mem[ idx + j ];
 540             }
 541         }
 542         target_ptr += sf_length;
 543     }
 544 }
 545
 546 /********************************************************************/
 547 /* Calculate the energies for first two subframes. The energies are */
 548 /* calculated recursively.                                          */
 549 /********************************************************************/
 550 static void silk_P_Ana_calc_energy_st3(
 551     silk_float energies_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ], /* O 3 DIM correlation array */
 552     const silk_float    frame[],            /* I vector to correlate                                            */
 553     opus_int            start_lag,          /* I start lag                                                      */
 554     opus_int            sf_length,          /* I sub frame length                                               */
 555     opus_int            nb_subfr,           /* I number of subframes                                            */
 556     opus_int            complexity          /* I Complexity setting                                             */
 557 )
 558 {
 559     const silk_float *target_ptr, *basis_ptr;
 560     double    energy;
 561     opus_int   k, i, j, lag_counter;
 562     opus_int   nb_cbk_search, delta, idx, cbk_size, lag_diff;
 563     silk_float scratch_mem[ SCRATCH_SIZE ];
 564     const opus_int8 *Lag_range_ptr, *Lag_CB_ptr;
 565
 566     silk_assert( complexity >= SILK_PE_MIN_COMPLEX );
 567     silk_assert( complexity <= SILK_PE_MAX_COMPLEX );
 568
 569     if( nb_subfr == PE_MAX_NB_SUBFR ) {
 570         Lag_range_ptr = &silk_Lag_range_stage3[ complexity ][ 0 ][ 0 ];
 571         Lag_CB_ptr    = &silk_CB_lags_stage3[ 0 ][ 0 ];
 572         nb_cbk_search = silk_nb_cbk_searchs_stage3[ complexity ];
 573         cbk_size      = PE_NB_CBKS_STAGE3_MAX;
 574     } else {
 575         silk_assert( nb_subfr == PE_MAX_NB_SUBFR >> 1);
 576         Lag_range_ptr = &silk_Lag_range_stage3_10_ms[ 0 ][ 0 ];
 577         Lag_CB_ptr    = &silk_CB_lags_stage3_10_ms[ 0 ][ 0 ];
 578         nb_cbk_search = PE_NB_CBKS_STAGE3_10MS;
 579         cbk_size      = PE_NB_CBKS_STAGE3_10MS;
 580     }
 581
 582     target_ptr = &frame[ silk_LSHIFT( sf_length, 2 ) ];
 583     for( k = 0; k < nb_subfr; k++ ) {
 584         lag_counter = 0;
 585
 586         /* Calculate the energy for first lag */
 587         basis_ptr = target_ptr - ( start_lag + matrix_ptr( Lag_range_ptr, k, 0, 2 ) );
 588         energy = silk_energy_FLP( basis_ptr, sf_length ) + 1e-3;
 589         silk_assert( energy >= 0.0 );
 590         scratch_mem[lag_counter] = (silk_float)energy;
 591         lag_counter++;
 592
 593         lag_diff = ( matrix_ptr( Lag_range_ptr, k, 1, 2 ) -  matrix_ptr( Lag_range_ptr, k, 0, 2 ) + 1 );
 594         for( i = 1; i < lag_diff; i++ ) {
 595             /* remove part outside new window */
 596             energy -= basis_ptr[sf_length - i] * (double)basis_ptr[sf_length - i];
 597             silk_assert( energy >= 0.0 );
 598
 599             /* add part that comes into window */
 600             energy += basis_ptr[ -i ] * (double)basis_ptr[ -i ];
 601             silk_assert( energy >= 0.0 );
 602             silk_assert( lag_counter < SCRATCH_SIZE );
 603             scratch_mem[lag_counter] = (silk_float)energy;
 604             lag_counter++;
 605         }
 606
 607         delta = matrix_ptr( Lag_range_ptr, k, 0, 2 );
 608         for( i = 0; i < nb_cbk_search; i++ ) {
 609             /* Fill out the 3 dim array that stores the correlations for    */
 610             /* each code_book vector for each start lag                     */
 611             idx = matrix_ptr( Lag_CB_ptr, k, i, cbk_size ) - delta;
 612             for( j = 0; j < PE_NB_STAGE3_LAGS; j++ ) {
 613                 silk_assert( idx + j < SCRATCH_SIZE );
 614                 silk_assert( idx + j < lag_counter );
 615                 energies_st3[ k ][ i ][ j ] = scratch_mem[ idx + j ];
 616                 silk_assert( energies_st3[ k ][ i ][ j ] >= 0.0f );
 617             }
 618         }
 619         target_ptr += sf_length;
 620     }
 621 }