Merge branch 'exp_analysis7'
[opus.git] / silk / fixed / pitch_analysis_core_FIX.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_FIX.h"
36 #include "pitch_est_defines.h"
37 #include "debug.h"
38
39 #define SCRATCH_SIZE    22
40
41 /************************************************************/
42 /* Internally used functions                                */
43 /************************************************************/
44 void silk_P_Ana_calc_corr_st3(
45     opus_int32        cross_corr_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ],/* (O) 3 DIM correlation array */
46     const opus_int16  frame[],                         /* I vector to correlate         */
47     opus_int          start_lag,                       /* I lag offset to search around */
48     opus_int          sf_length,                       /* I length of a 5 ms subframe   */
49     opus_int          nb_subfr,                        /* I number of subframes         */
50     opus_int          complexity                       /* I Complexity setting          */
51 );
52
53 void silk_P_Ana_calc_energy_st3(
54     opus_int32        energies_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ],/* (O) 3 DIM energy array */
55     const opus_int16  frame[],                         /* I vector to calc energy in    */
56     opus_int          start_lag,                       /* I lag offset to search around */
57     opus_int          sf_length,                       /* I length of one 5 ms subframe */
58     opus_int          nb_subfr,                        /* I number of subframes         */
59     opus_int          complexity                       /* I Complexity setting          */
60 );
61
62 opus_int32 silk_P_Ana_find_scaling(
63     const opus_int16  *frame,
64     const opus_int    frame_length,
65     const opus_int    sum_sqr_len
66 );
67
68 /*************************************************************/
69 /*      FIXED POINT CORE PITCH ANALYSIS FUNCTION             */
70 /*************************************************************/
71 opus_int silk_pitch_analysis_core(                  /* O    Voicing estimate: 0 voiced, 1 unvoiced                      */
72     const opus_int16            *frame,             /* I    Signal of length PE_FRAME_LENGTH_MS*Fs_kHz                  */
73     opus_int                    *pitch_out,         /* O    4 pitch lag values                                          */
74     opus_int16                  *lagIndex,          /* O    Lag Index                                                   */
75     opus_int8                   *contourIndex,      /* O    Pitch contour Index                                         */
76     opus_int                    *LTPCorr_Q15,       /* I/O  Normalized correlation; input: value from previous frame    */
77     opus_int                    prevLag,            /* I    Last lag of previous frame; set to zero is unvoiced         */
78     const opus_int32            search_thres1_Q16,  /* I    First stage threshold for lag candidates 0 - 1              */
79     const opus_int              search_thres2_Q15,  /* I    Final threshold for lag candidates 0 - 1                    */
80     const opus_int              Fs_kHz,             /* I    Sample frequency (kHz)                                      */
81     const opus_int              complexity,         /* I    Complexity setting, 0-2, where 2 is highest                 */
82     const opus_int              nb_subfr            /* I    number of 5 ms subframes                                    */
83 )
84 {
85     opus_int16 frame_8kHz[ PE_MAX_FRAME_LENGTH_ST_2 ];
86     opus_int16 frame_4kHz[ PE_MAX_FRAME_LENGTH_ST_1 ];
87     opus_int32 filt_state[ 6 ];
88     opus_int32 scratch_mem[ 3 * PE_MAX_FRAME_LENGTH ];
89     opus_int16 *input_frame_ptr;
90     opus_int   i, k, d, j;
91     opus_int16 C[ PE_MAX_NB_SUBFR ][ ( PE_MAX_LAG >> 1 ) + 5 ];
92     const opus_int16 *target_ptr, *basis_ptr;
93     opus_int32 cross_corr, normalizer, energy, shift, energy_basis, energy_target;
94     opus_int   d_srch[ PE_D_SRCH_LENGTH ], Cmax, length_d_srch, length_d_comp;
95     opus_int16 d_comp[ ( PE_MAX_LAG >> 1 ) + 5 ];
96     opus_int32 sum, threshold, temp32, lag_counter;
97     opus_int   CBimax, CBimax_new, CBimax_old, lag, start_lag, end_lag, lag_new;
98     opus_int32 CC[ PE_NB_CBKS_STAGE2_EXT ], CCmax, CCmax_b, CCmax_new_b, CCmax_new;
99     opus_int32 energies_st3[  PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ];
100     opus_int32 crosscorr_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ];
101     opus_int   frame_length, frame_length_8kHz, frame_length_4kHz, max_sum_sq_length;
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_int32 contour_bias_Q20, diff, lz, lshift;
106     opus_int   nb_cbk_search, cbk_size;
107     opus_int32 delta_lag_log2_sqr_Q7, lag_log2_Q7, prevLag_log2_Q7, prev_lag_bias_Q15, corr_thres_Q15;
108     const opus_int8 *Lag_CB_ptr;
109     /* Check for valid sampling frequency */
110     silk_assert( Fs_kHz == 8 || Fs_kHz == 12 || Fs_kHz == 16 );
111
112     /* Check for valid complexity setting */
113     silk_assert( complexity >= SILK_PE_MIN_COMPLEX );
114     silk_assert( complexity <= SILK_PE_MAX_COMPLEX );
115
116     silk_assert( search_thres1_Q16 >= 0 && search_thres1_Q16 <= (1<<16) );
117     silk_assert( search_thres2_Q15 >= 0 && search_thres2_Q15 <= (1<<15) );
118
119     /* Set up frame lengths max / min lag for the sampling frequency */
120     frame_length      = ( PE_LTP_MEM_LENGTH_MS + nb_subfr * PE_SUBFR_LENGTH_MS ) * Fs_kHz;
121     frame_length_4kHz = ( PE_LTP_MEM_LENGTH_MS + nb_subfr * PE_SUBFR_LENGTH_MS ) * 4;
122     frame_length_8kHz = ( PE_LTP_MEM_LENGTH_MS + nb_subfr * PE_SUBFR_LENGTH_MS ) * 8;
123     sf_length         = PE_SUBFR_LENGTH_MS * Fs_kHz;
124     sf_length_4kHz    = PE_SUBFR_LENGTH_MS * 4;
125     sf_length_8kHz    = PE_SUBFR_LENGTH_MS * 8;
126     min_lag           = PE_MIN_LAG_MS * Fs_kHz;
127     min_lag_4kHz      = PE_MIN_LAG_MS * 4;
128     min_lag_8kHz      = PE_MIN_LAG_MS * 8;
129     max_lag           = PE_MAX_LAG_MS * Fs_kHz - 1;
130     max_lag_4kHz      = PE_MAX_LAG_MS * 4;
131     max_lag_8kHz      = PE_MAX_LAG_MS * 8 - 1;
132
133     silk_memset( C, 0, sizeof( opus_int16 ) * nb_subfr * ( ( PE_MAX_LAG >> 1 ) + 5) );
134
135     /* Resample from input sampled at Fs_kHz to 8 kHz */
136     if( Fs_kHz == 16 ) {
137         silk_memset( filt_state, 0, 2 * sizeof( opus_int32 ) );
138         silk_resampler_down2( filt_state, frame_8kHz, frame, frame_length );
139     } else if( Fs_kHz == 12 ) {
140         silk_memset( filt_state, 0, 6 * sizeof( opus_int32 ) );
141         silk_resampler_down2_3( filt_state, frame_8kHz, frame, frame_length );
142     } else {
143         silk_assert( Fs_kHz == 8 );
144         silk_memcpy( frame_8kHz, frame, frame_length_8kHz * sizeof(opus_int16) );
145     }
146
147     /* Decimate again to 4 kHz */
148     silk_memset( filt_state, 0, 2 * sizeof( opus_int32 ) );/* Set state to zero */
149     silk_resampler_down2( filt_state, frame_4kHz, frame_8kHz, frame_length_8kHz );
150
151     /* Low-pass filter */
152     for( i = frame_length_4kHz - 1; i > 0; i-- ) {
153         frame_4kHz[ i ] = silk_ADD_SAT16( frame_4kHz[ i ], frame_4kHz[ i - 1 ] );
154     }
155
156     /*******************************************************************************
157     ** Scale 4 kHz signal down to prevent correlations measures from overflowing
158     ** find scaling as max scaling for each 8kHz(?) subframe
159     *******************************************************************************/
160
161     /* Inner product is calculated with different lengths, so scale for the worst case */
162     max_sum_sq_length = silk_max_32( sf_length_8kHz, silk_LSHIFT( sf_length_4kHz, 2 ) );
163     shift = silk_P_Ana_find_scaling( frame_4kHz, frame_length_4kHz, max_sum_sq_length );
164     if( shift > 0 ) {
165         for( i = 0; i < frame_length_4kHz; i++ ) {
166             frame_4kHz[ i ] = silk_RSHIFT( frame_4kHz[ i ], shift );
167         }
168     }
169
170     /******************************************************************************
171     * FIRST STAGE, operating in 4 khz
172     ******************************************************************************/
173     target_ptr = &frame_4kHz[ silk_LSHIFT( sf_length_4kHz, 2 ) ];
174     for( k = 0; k < nb_subfr >> 1; k++ ) {
175         /* Check that we are within range of the array */
176         silk_assert( target_ptr >= frame_4kHz );
177         silk_assert( target_ptr + sf_length_8kHz <= frame_4kHz + frame_length_4kHz );
178
179         basis_ptr = target_ptr - min_lag_4kHz;
180
181         /* Check that we are within range of the array */
182         silk_assert( basis_ptr >= frame_4kHz );
183         silk_assert( basis_ptr + sf_length_8kHz <= frame_4kHz + frame_length_4kHz );
184
185         /* Calculate first vector products before loop */
186         cross_corr = silk_inner_prod_aligned( target_ptr, basis_ptr, sf_length_8kHz );
187         normalizer = silk_inner_prod_aligned( basis_ptr,  basis_ptr, sf_length_8kHz );
188         normalizer = silk_ADD_SAT32( normalizer, silk_SMULBB( sf_length_8kHz, 4000 ) );
189
190         temp32 = silk_DIV32( cross_corr, silk_SQRT_APPROX( normalizer ) + 1 );
191         C[ k ][ min_lag_4kHz ] = (opus_int16)silk_SAT16( temp32 );        /* Q0 */
192
193         /* From now on normalizer is computed recursively */
194         for( d = min_lag_4kHz + 1; d <= max_lag_4kHz; d++ ) {
195             basis_ptr--;
196
197             /* Check that we are within range of the array */
198             silk_assert( basis_ptr >= frame_4kHz );
199             silk_assert( basis_ptr + sf_length_8kHz <= frame_4kHz + frame_length_4kHz );
200
201             cross_corr = silk_inner_prod_aligned( target_ptr, basis_ptr, sf_length_8kHz );
202
203             /* Add contribution of new sample and remove contribution from oldest sample */
204             normalizer +=
205                 silk_SMULBB( basis_ptr[ 0 ], basis_ptr[ 0 ] ) -
206                 silk_SMULBB( basis_ptr[ sf_length_8kHz ], basis_ptr[ sf_length_8kHz ] );
207
208             temp32 = silk_DIV32( cross_corr, silk_SQRT_APPROX( normalizer ) + 1 );
209             C[ k ][ d ] = (opus_int16)silk_SAT16( temp32 );                        /* Q0 */
210         }
211         /* Update target pointer */
212         target_ptr += sf_length_8kHz;
213     }
214
215     /* Combine two subframes into single correlation measure and apply short-lag bias */
216     if( nb_subfr == PE_MAX_NB_SUBFR ) {
217         for( i = max_lag_4kHz; i >= min_lag_4kHz; i-- ) {
218             sum = (opus_int32)C[ 0 ][ i ] + (opus_int32)C[ 1 ][ i ];                /* Q0 */
219             silk_assert( silk_RSHIFT( sum, 1 ) == silk_SAT16( silk_RSHIFT( sum, 1 ) ) );
220             sum = silk_RSHIFT( sum, 1 );                                           /* Q-1 */
221             silk_assert( silk_LSHIFT( (opus_int32)-i, 4 ) == silk_SAT16( silk_LSHIFT( (opus_int32)-i, 4 ) ) );
222             sum = silk_SMLAWB( sum, sum, silk_LSHIFT( -i, 4 ) );                    /* Q-1 */
223             silk_assert( sum == silk_SAT16( sum ) );
224             C[ 0 ][ i ] = (opus_int16)sum;                                         /* Q-1 */
225         }
226     } else {
227         /* Only short-lag bias */
228         for( i = max_lag_4kHz; i >= min_lag_4kHz; i-- ) {
229             sum = (opus_int32)C[ 0 ][ i ];
230             sum = silk_SMLAWB( sum, sum, silk_LSHIFT( -i, 4 ) );                    /* Q-1 */
231             C[ 0 ][ i ] = (opus_int16)sum;                                         /* Q-1 */
232         }
233     }
234
235     /* Sort */
236     length_d_srch = silk_ADD_LSHIFT32( 4, complexity, 1 );
237     silk_assert( 3 * length_d_srch <= PE_D_SRCH_LENGTH );
238     silk_insertion_sort_decreasing_int16( &C[ 0 ][ min_lag_4kHz ], d_srch, max_lag_4kHz - min_lag_4kHz + 1, length_d_srch );
239
240     /* Escape if correlation is very low already here */
241     target_ptr = &frame_4kHz[ silk_SMULBB( sf_length_4kHz, nb_subfr ) ];
242     energy = silk_inner_prod_aligned( target_ptr, target_ptr, silk_LSHIFT( sf_length_4kHz, 2 ) );
243     energy = silk_ADD_SAT32( energy, 1000 );                                  /* Q0 */
244     Cmax = (opus_int)C[ 0 ][ min_lag_4kHz ];                                  /* Q-1 */
245     threshold = silk_SMULBB( Cmax, Cmax );                                    /* Q-2 */
246
247     /* Compare in Q-2 domain */
248     if( silk_RSHIFT( energy, 4 + 2 ) > threshold ) {
249         silk_memset( pitch_out, 0, nb_subfr * sizeof( opus_int ) );
250         *LTPCorr_Q15  = 0;
251         *lagIndex     = 0;
252         *contourIndex = 0;
253         return 1;
254     }
255
256     threshold = silk_SMULWB( search_thres1_Q16, Cmax );
257     for( i = 0; i < length_d_srch; i++ ) {
258         /* Convert to 8 kHz indices for the sorted correlation that exceeds the threshold */
259         if( C[ 0 ][ min_lag_4kHz + i ] > threshold ) {
260             d_srch[ i ] = silk_LSHIFT( d_srch[ i ] + min_lag_4kHz, 1 );
261         } else {
262             length_d_srch = i;
263             break;
264         }
265     }
266     silk_assert( length_d_srch > 0 );
267
268     for( i = min_lag_8kHz - 5; i < max_lag_8kHz + 5; i++ ) {
269         d_comp[ i ] = 0;
270     }
271     for( i = 0; i < length_d_srch; i++ ) {
272         d_comp[ d_srch[ i ] ] = 1;
273     }
274
275     /* Convolution */
276     for( i = max_lag_8kHz + 3; i >= min_lag_8kHz; i-- ) {
277         d_comp[ i ] += d_comp[ i - 1 ] + d_comp[ i - 2 ];
278     }
279
280     length_d_srch = 0;
281     for( i = min_lag_8kHz; i < max_lag_8kHz + 1; i++ ) {
282         if( d_comp[ i + 1 ] > 0 ) {
283             d_srch[ length_d_srch ] = i;
284             length_d_srch++;
285         }
286     }
287
288     /* Convolution */
289     for( i = max_lag_8kHz + 3; i >= min_lag_8kHz; i-- ) {
290         d_comp[ i ] += d_comp[ i - 1 ] + d_comp[ i - 2 ] + d_comp[ i - 3 ];
291     }
292
293     length_d_comp = 0;
294     for( i = min_lag_8kHz; i < max_lag_8kHz + 4; i++ ) {
295         if( d_comp[ i ] > 0 ) {
296             d_comp[ length_d_comp ] = i - 2;
297             length_d_comp++;
298         }
299     }
300
301     /**********************************************************************************
302     ** SECOND STAGE, operating at 8 kHz, on lag sections with high correlation
303     *************************************************************************************/
304
305     /******************************************************************************
306     ** Scale signal down to avoid correlations measures from overflowing
307     *******************************************************************************/
308     /* find scaling as max scaling for each subframe */
309     shift = silk_P_Ana_find_scaling( frame_8kHz, frame_length_8kHz, sf_length_8kHz );
310     if( shift > 0 ) {
311         for( i = 0; i < frame_length_8kHz; i++ ) {
312             frame_8kHz[ i ] = silk_RSHIFT( frame_8kHz[ i ], shift );
313         }
314     }
315
316     /*********************************************************************************
317     * Find energy of each subframe projected onto its history, for a range of delays
318     *********************************************************************************/
319     silk_memset( C, 0, PE_MAX_NB_SUBFR * ( ( PE_MAX_LAG >> 1 ) + 5 ) * sizeof( opus_int16 ) );
320
321     target_ptr = &frame_8kHz[ PE_LTP_MEM_LENGTH_MS * 8 ];
322     for( k = 0; k < nb_subfr; k++ ) {
323
324         /* Check that we are within range of the array */
325         silk_assert( target_ptr >= frame_8kHz );
326         silk_assert( target_ptr + sf_length_8kHz <= frame_8kHz + frame_length_8kHz );
327
328         energy_target = silk_inner_prod_aligned( target_ptr, target_ptr, sf_length_8kHz );
329         for( j = 0; j < length_d_comp; j++ ) {
330             d = d_comp[ j ];
331             basis_ptr = target_ptr - d;
332
333             /* Check that we are within range of the array */
334             silk_assert( basis_ptr >= frame_8kHz );
335             silk_assert( basis_ptr + sf_length_8kHz <= frame_8kHz + frame_length_8kHz );
336
337             cross_corr   = silk_inner_prod_aligned( target_ptr, basis_ptr, sf_length_8kHz );
338             energy_basis = silk_inner_prod_aligned( basis_ptr,  basis_ptr, sf_length_8kHz );
339             if( cross_corr > 0 ) {
340                 energy = silk_max( energy_target, energy_basis ); /* Find max to make sure first division < 1.0 */
341                 lz = silk_CLZ32( cross_corr );
342                 lshift = silk_LIMIT_32( lz - 1, 0, 15 );
343                 temp32 = silk_DIV32( silk_LSHIFT( cross_corr, lshift ), silk_RSHIFT( energy, 15 - lshift ) + 1 ); /* Q15 */
344                 silk_assert( temp32 == silk_SAT16( temp32 ) );
345                 temp32 = silk_SMULWB( cross_corr, temp32 ); /* Q(-1), cc * ( cc / max(b, t) ) */
346                 temp32 = silk_ADD_SAT32( temp32, temp32 );  /* Q(0) */
347                 lz = silk_CLZ32( temp32 );
348                 lshift = silk_LIMIT_32( lz - 1, 0, 15 );
349                 energy = silk_min( energy_target, energy_basis );
350                 C[ k ][ d ] = silk_DIV32( silk_LSHIFT( temp32, lshift ), silk_RSHIFT( energy, 15 - lshift ) + 1 ); /* Q15*/
351             } else {
352                 C[ k ][ d ] = 0;
353             }
354         }
355         target_ptr += sf_length_8kHz;
356     }
357
358     /* search over lag range and lags codebook */
359     /* scale factor for lag codebook, as a function of center lag */
360
361     CCmax   = silk_int32_MIN;
362     CCmax_b = silk_int32_MIN;
363
364     CBimax = 0; /* To avoid returning undefined lag values */
365     lag = -1;   /* To check if lag with strong enough correlation has been found */
366
367     if( prevLag > 0 ) {
368         if( Fs_kHz == 12 ) {
369             prevLag = silk_DIV32_16( silk_LSHIFT( prevLag, 1 ), 3 );
370         } else if( Fs_kHz == 16 ) {
371             prevLag = silk_RSHIFT( prevLag, 1 );
372         }
373         prevLag_log2_Q7 = silk_lin2log( (opus_int32)prevLag );
374     } else {
375         prevLag_log2_Q7 = 0;
376     }
377     silk_assert( search_thres2_Q15 == silk_SAT16( search_thres2_Q15 ) );
378     /* Set up stage 2 codebook based on number of subframes */
379     if( nb_subfr == PE_MAX_NB_SUBFR ) {
380         cbk_size   = PE_NB_CBKS_STAGE2_EXT;
381         Lag_CB_ptr = &silk_CB_lags_stage2[ 0 ][ 0 ];
382         if( Fs_kHz == 8 && complexity > SILK_PE_MIN_COMPLEX ) {
383             /* If input is 8 khz use a larger codebook here because it is last stage */
384             nb_cbk_search = PE_NB_CBKS_STAGE2_EXT;
385         } else {
386             nb_cbk_search = PE_NB_CBKS_STAGE2;
387         }
388         corr_thres_Q15 = silk_RSHIFT( silk_SMULBB( search_thres2_Q15, search_thres2_Q15 ), 13 );
389     } else {
390         cbk_size       = PE_NB_CBKS_STAGE2_10MS;
391         Lag_CB_ptr     = &silk_CB_lags_stage2_10_ms[ 0 ][ 0 ];
392         nb_cbk_search  = PE_NB_CBKS_STAGE2_10MS;
393         corr_thres_Q15 = silk_RSHIFT( silk_SMULBB( search_thres2_Q15, search_thres2_Q15 ), 14 );
394     }
395
396     for( k = 0; k < length_d_srch; k++ ) {
397         d = d_srch[ k ];
398         for( j = 0; j < nb_cbk_search; j++ ) {
399             CC[ j ] = 0;
400             for( i = 0; i < nb_subfr; i++ ) {
401                 /* Try all codebooks */
402                 CC[ j ] = CC[ j ] + (opus_int32)C[ i ][ d + matrix_ptr( Lag_CB_ptr, i, j, cbk_size )];
403             }
404         }
405         /* Find best codebook */
406         CCmax_new = silk_int32_MIN;
407         CBimax_new = 0;
408         for( i = 0; i < nb_cbk_search; i++ ) {
409             if( CC[ i ] > CCmax_new ) {
410                 CCmax_new = CC[ i ];
411                 CBimax_new = i;
412             }
413         }
414
415         /* Bias towards shorter lags */
416         lag_log2_Q7 = silk_lin2log( (opus_int32)d ); /* Q7 */
417         silk_assert( lag_log2_Q7 == silk_SAT16( lag_log2_Q7 ) );
418         silk_assert( nb_subfr * SILK_FIX_CONST( PE_SHORTLAG_BIAS, 15 ) == silk_SAT16( nb_subfr * SILK_FIX_CONST( PE_SHORTLAG_BIAS, 15 ) ) );
419         CCmax_new_b = CCmax_new - silk_RSHIFT( silk_SMULBB( nb_subfr * SILK_FIX_CONST( PE_SHORTLAG_BIAS, 15 ), lag_log2_Q7 ), 7 ); /* Q15 */
420
421         /* Bias towards previous lag */
422         silk_assert( nb_subfr * SILK_FIX_CONST( PE_PREVLAG_BIAS, 15 ) == silk_SAT16( nb_subfr * SILK_FIX_CONST( PE_PREVLAG_BIAS, 15 ) ) );
423         if( prevLag > 0 ) {
424             delta_lag_log2_sqr_Q7 = lag_log2_Q7 - prevLag_log2_Q7;
425             silk_assert( delta_lag_log2_sqr_Q7 == silk_SAT16( delta_lag_log2_sqr_Q7 ) );
426             delta_lag_log2_sqr_Q7 = silk_RSHIFT( silk_SMULBB( delta_lag_log2_sqr_Q7, delta_lag_log2_sqr_Q7 ), 7 );
427             prev_lag_bias_Q15 = silk_RSHIFT( silk_SMULBB( nb_subfr * SILK_FIX_CONST( PE_PREVLAG_BIAS, 15 ), *LTPCorr_Q15 ), 15 ); /* Q15 */
428             prev_lag_bias_Q15 = silk_DIV32( silk_MUL( prev_lag_bias_Q15, delta_lag_log2_sqr_Q7 ), delta_lag_log2_sqr_Q7 + ( 1 << 6 ) );
429             CCmax_new_b -= prev_lag_bias_Q15; /* Q15 */
430         }
431
432         if( CCmax_new_b > CCmax_b                                   &&  /* Find maximum biased correlation                  */
433             CCmax_new > corr_thres_Q15                              &&  /* Correlation needs to be high enough to be voiced */
434             silk_CB_lags_stage2[ 0 ][ CBimax_new ] <= min_lag_8kHz      /* Lag must be in range                             */
435          ) {
436             CCmax_b = CCmax_new_b;
437             CCmax   = CCmax_new;
438             lag     = d;
439             CBimax  = CBimax_new;
440         }
441     }
442
443     if( lag == -1 ) {
444         /* No suitable candidate found */
445         silk_memset( pitch_out, 0, nb_subfr * sizeof( opus_int ) );
446         *LTPCorr_Q15  = 0;
447         *lagIndex     = 0;
448         *contourIndex = 0;
449         return 1;
450     }
451
452     if( Fs_kHz > 8 ) {
453         /***************************************************************************/
454         /* Scale input signal down to avoid correlations measures from overflowing */
455         /***************************************************************************/
456         /* find scaling as max scaling for each subframe */
457         shift = silk_P_Ana_find_scaling( frame, frame_length, sf_length );
458         if( shift > 0 ) {
459             /* Move signal to scratch mem because the input signal should be unchanged */
460             /* Reuse the 32 bit scratch mem vector, use a 16 bit pointer from now */
461             input_frame_ptr = (opus_int16*)scratch_mem;
462             for( i = 0; i < frame_length; i++ ) {
463                 input_frame_ptr[ i ] = silk_RSHIFT( frame[ i ], shift );
464             }
465         } else {
466             input_frame_ptr = (opus_int16*)frame;
467         }
468
469         /* Search in original signal */
470
471         CBimax_old = CBimax;
472         /* Compensate for decimation */
473         silk_assert( lag == silk_SAT16( lag ) );
474         if( Fs_kHz == 12 ) {
475             lag = silk_RSHIFT( silk_SMULBB( lag, 3 ), 1 );
476         } else if( Fs_kHz == 16 ) {
477             lag = silk_LSHIFT( lag, 1 );
478         } else {
479             lag = silk_SMULBB( lag, 3 );
480         }
481
482         lag = silk_LIMIT_int( lag, min_lag, max_lag );
483         start_lag = silk_max_int( lag - 2, min_lag );
484         end_lag   = silk_min_int( lag + 2, max_lag );
485         lag_new   = lag;                                    /* to avoid undefined lag */
486         CBimax    = 0;                                        /* to avoid undefined lag */
487         silk_assert( silk_LSHIFT( CCmax, 13 ) >= 0 );
488         *LTPCorr_Q15 = (opus_int)silk_SQRT_APPROX( silk_LSHIFT( CCmax, 13 ) ); /* Output normalized correlation */
489
490         CCmax = silk_int32_MIN;
491         /* pitch lags according to second stage */
492         for( k = 0; k < nb_subfr; k++ ) {
493             pitch_out[ k ] = lag + 2 * silk_CB_lags_stage2[ k ][ CBimax_old ];
494         }
495         /* Calculate the correlations and energies needed in stage 3 */
496         silk_P_Ana_calc_corr_st3(  crosscorr_st3, input_frame_ptr, start_lag, sf_length, nb_subfr, complexity );
497         silk_P_Ana_calc_energy_st3( energies_st3, input_frame_ptr, start_lag, sf_length, nb_subfr, complexity );
498
499         lag_counter = 0;
500         silk_assert( lag == silk_SAT16( lag ) );
501         contour_bias_Q20 = silk_DIV32_16( SILK_FIX_CONST( PE_FLATCONTOUR_BIAS, 20 ), lag );
502
503         /* Set up codebook parameters according to complexity setting and frame length */
504         if( nb_subfr == PE_MAX_NB_SUBFR ) {
505             nb_cbk_search   = (opus_int)silk_nb_cbk_searchs_stage3[ complexity ];
506             cbk_size        = PE_NB_CBKS_STAGE3_MAX;
507             Lag_CB_ptr      = &silk_CB_lags_stage3[ 0 ][ 0 ];
508         } else {
509             nb_cbk_search   = PE_NB_CBKS_STAGE3_10MS;
510             cbk_size        = PE_NB_CBKS_STAGE3_10MS;
511             Lag_CB_ptr      = &silk_CB_lags_stage3_10_ms[ 0 ][ 0 ];
512         }
513         for( d = start_lag; d <= end_lag; d++ ) {
514             for( j = 0; j < nb_cbk_search; j++ ) {
515                 cross_corr = 0;
516                 energy     = 0;
517                 for( k = 0; k < nb_subfr; k++ ) {
518                     silk_assert( PE_MAX_NB_SUBFR == 4 );
519                     energy     += silk_RSHIFT( energies_st3[  k ][ j ][ lag_counter ], 2 ); /* use mean, to avoid overflow */
520                     silk_assert( energy >= 0 );
521                     cross_corr += silk_RSHIFT( crosscorr_st3[ k ][ j ][ lag_counter ], 2 ); /* use mean, to avoid overflow */
522                 }
523                 if( cross_corr > 0 ) {
524                     /* Divide cross_corr / energy and get result in Q15 */
525                     lz = silk_CLZ32( cross_corr );
526                     /* Divide with result in Q13, cross_corr could be larger than energy */
527                     lshift = silk_LIMIT_32( lz - 1, 0, 13 );
528                     CCmax_new = silk_DIV32( silk_LSHIFT( cross_corr, lshift ), silk_RSHIFT( energy, 13 - lshift ) + 1 );
529                     CCmax_new = silk_SAT16( CCmax_new );
530                     CCmax_new = silk_SMULWB( cross_corr, CCmax_new );
531                     /* Saturate */
532                     if( CCmax_new > silk_RSHIFT( silk_int32_MAX, 3 ) ) {
533                         CCmax_new = silk_int32_MAX;
534                     } else {
535                         CCmax_new = silk_LSHIFT( CCmax_new, 3 );
536                     }
537                     /* Reduce depending on flatness of contour */
538                     diff = silk_int16_MAX - silk_RSHIFT( silk_MUL( contour_bias_Q20, j ), 5 ); /* Q20 -> Q15 */
539                     silk_assert( diff == silk_SAT16( diff ) );
540                     CCmax_new = silk_LSHIFT( silk_SMULWB( CCmax_new, diff ), 1 );
541                 } else {
542                     CCmax_new = 0;
543                 }
544
545                 if( CCmax_new > CCmax                                               &&
546                    ( d + silk_CB_lags_stage3[ 0 ][ j ] ) <= max_lag
547                    ) {
548                     CCmax   = CCmax_new;
549                     lag_new = d;
550                     CBimax  = j;
551                 }
552             }
553             lag_counter++;
554         }
555
556         for( k = 0; k < nb_subfr; k++ ) {
557             pitch_out[ k ] = lag_new + matrix_ptr( Lag_CB_ptr, k, CBimax, cbk_size );
558             pitch_out[ k ] = silk_LIMIT( pitch_out[ k ], min_lag, PE_MAX_LAG_MS * Fs_kHz );
559         }
560         *lagIndex = (opus_int16)( lag_new - min_lag);
561         *contourIndex = (opus_int8)CBimax;
562     } else {        /* Fs_kHz == 8 */
563         /* Save Lags and correlation */
564         CCmax = silk_max( CCmax, 0 );
565         *LTPCorr_Q15 = (opus_int)silk_SQRT_APPROX( silk_LSHIFT( CCmax, 13 ) ); /* Output normalized correlation */
566         for( k = 0; k < nb_subfr; k++ ) {
567             pitch_out[ k ] = lag + matrix_ptr( Lag_CB_ptr, k, CBimax, cbk_size );
568             pitch_out[ k ] = silk_LIMIT( pitch_out[ k ], min_lag_8kHz, PE_MAX_LAG_MS * Fs_kHz );
569         }
570         *lagIndex = (opus_int16)( lag - min_lag_8kHz );
571         *contourIndex = (opus_int8)CBimax;
572     }
573     silk_assert( *lagIndex >= 0 );
574     /* return as voiced */
575     return 0;
576 }
577
578 /*************************************************************************/
579 /* Calculates the correlations used in stage 3 search. In order to cover */
580 /* the whole lag codebook for all the searched offset lags (lag +- 2),   */
581 /*************************************************************************/
582 void silk_P_Ana_calc_corr_st3(
583     opus_int32        cross_corr_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ],/* (O) 3 DIM correlation array */
584     const opus_int16  frame[],                         /* I vector to correlate         */
585     opus_int          start_lag,                       /* I lag offset to search around */
586     opus_int          sf_length,                       /* I length of a 5 ms subframe   */
587     opus_int          nb_subfr,                        /* I number of subframes         */
588     opus_int          complexity                       /* I Complexity setting          */
589 )
590 {
591     const opus_int16 *target_ptr, *basis_ptr;
592     opus_int32 cross_corr;
593     opus_int   i, j, k, lag_counter, lag_low, lag_high;
594     opus_int   nb_cbk_search, delta, idx, cbk_size;
595     opus_int32 scratch_mem[ SCRATCH_SIZE ];
596     const opus_int8 *Lag_range_ptr, *Lag_CB_ptr;
597
598     silk_assert( complexity >= SILK_PE_MIN_COMPLEX );
599     silk_assert( complexity <= SILK_PE_MAX_COMPLEX );
600
601     if( nb_subfr == PE_MAX_NB_SUBFR ) {
602         Lag_range_ptr = &silk_Lag_range_stage3[ complexity ][ 0 ][ 0 ];
603         Lag_CB_ptr    = &silk_CB_lags_stage3[ 0 ][ 0 ];
604         nb_cbk_search = silk_nb_cbk_searchs_stage3[ complexity ];
605         cbk_size      = PE_NB_CBKS_STAGE3_MAX;
606     } else {
607         silk_assert( nb_subfr == PE_MAX_NB_SUBFR >> 1);
608         Lag_range_ptr = &silk_Lag_range_stage3_10_ms[ 0 ][ 0 ];
609         Lag_CB_ptr    = &silk_CB_lags_stage3_10_ms[ 0 ][ 0 ];
610         nb_cbk_search = PE_NB_CBKS_STAGE3_10MS;
611         cbk_size      = PE_NB_CBKS_STAGE3_10MS;
612     }
613
614     target_ptr = &frame[ silk_LSHIFT( sf_length, 2 ) ]; /* Pointer to middle of frame */
615     for( k = 0; k < nb_subfr; k++ ) {
616         lag_counter = 0;
617
618         /* Calculate the correlations for each subframe */
619         lag_low  = matrix_ptr( Lag_range_ptr, k, 0, 2 );
620         lag_high = matrix_ptr( Lag_range_ptr, k, 1, 2 );
621         for( j = lag_low; j <= lag_high; j++ ) {
622             basis_ptr = target_ptr - ( start_lag + j );
623             cross_corr = silk_inner_prod_aligned( (opus_int16*)target_ptr, (opus_int16*)basis_ptr, sf_length );
624             silk_assert( lag_counter < SCRATCH_SIZE );
625             scratch_mem[ lag_counter ] = cross_corr;
626             lag_counter++;
627         }
628
629         delta = matrix_ptr( Lag_range_ptr, k, 0, 2 );
630         for( i = 0; i < nb_cbk_search; i++ ) {
631             /* Fill out the 3 dim array that stores the correlations for */
632             /* each code_book vector for each start lag */
633             idx = matrix_ptr( Lag_CB_ptr, k, i, cbk_size ) - delta;
634             for( j = 0; j < PE_NB_STAGE3_LAGS; j++ ) {
635                 silk_assert( idx + j < SCRATCH_SIZE );
636                 silk_assert( idx + j < lag_counter );
637                 cross_corr_st3[ k ][ i ][ j ] = scratch_mem[ idx + j ];
638             }
639         }
640         target_ptr += sf_length;
641     }
642 }
643
644 /********************************************************************/
645 /* Calculate the energies for first two subframes. The energies are */
646 /* calculated recursively.                                          */
647 /********************************************************************/
648 void silk_P_Ana_calc_energy_st3(
649     opus_int32        energies_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ],/* (O) 3 DIM energy array */
650     const opus_int16  frame[],                         /* I vector to calc energy in    */
651     opus_int          start_lag,                       /* I lag offset to search around */
652     opus_int          sf_length,                       /* I length of one 5 ms subframe */
653     opus_int          nb_subfr,                     /* I number of subframes         */
654     opus_int          complexity                       /* I Complexity setting          */
655 )
656 {
657     const opus_int16 *target_ptr, *basis_ptr;
658     opus_int32 energy;
659     opus_int   k, i, j, lag_counter;
660     opus_int   nb_cbk_search, delta, idx, cbk_size, lag_diff;
661     opus_int32 scratch_mem[ SCRATCH_SIZE ];
662     const opus_int8 *Lag_range_ptr, *Lag_CB_ptr;
663
664     silk_assert( complexity >= SILK_PE_MIN_COMPLEX );
665     silk_assert( complexity <= SILK_PE_MAX_COMPLEX );
666
667     if( nb_subfr == PE_MAX_NB_SUBFR ) {
668         Lag_range_ptr = &silk_Lag_range_stage3[ complexity ][ 0 ][ 0 ];
669         Lag_CB_ptr    = &silk_CB_lags_stage3[ 0 ][ 0 ];
670         nb_cbk_search = silk_nb_cbk_searchs_stage3[ complexity ];
671         cbk_size      = PE_NB_CBKS_STAGE3_MAX;
672     } else {
673         silk_assert( nb_subfr == PE_MAX_NB_SUBFR >> 1);
674         Lag_range_ptr = &silk_Lag_range_stage3_10_ms[ 0 ][ 0 ];
675         Lag_CB_ptr    = &silk_CB_lags_stage3_10_ms[ 0 ][ 0 ];
676         nb_cbk_search = PE_NB_CBKS_STAGE3_10MS;
677         cbk_size      = PE_NB_CBKS_STAGE3_10MS;
678     }
679     target_ptr = &frame[ silk_LSHIFT( sf_length, 2 ) ];
680     for( k = 0; k < nb_subfr; k++ ) {
681         lag_counter = 0;
682
683         /* Calculate the energy for first lag */
684         basis_ptr = target_ptr - ( start_lag + matrix_ptr( Lag_range_ptr, k, 0, 2 ) );
685         energy = silk_inner_prod_aligned( basis_ptr, basis_ptr, sf_length );
686         silk_assert( energy >= 0 );
687         scratch_mem[ lag_counter ] = energy;
688         lag_counter++;
689
690         lag_diff = ( matrix_ptr( Lag_range_ptr, k, 1, 2 ) -  matrix_ptr( Lag_range_ptr, k, 0, 2 ) + 1 );
691         for( i = 1; i < lag_diff; i++ ) {
692             /* remove part outside new window */
693             energy -= silk_SMULBB( basis_ptr[ sf_length - i ], basis_ptr[ sf_length - i ] );
694             silk_assert( energy >= 0 );
695
696             /* add part that comes into window */
697             energy = silk_ADD_SAT32( energy, silk_SMULBB( basis_ptr[ -i ], basis_ptr[ -i ] ) );
698             silk_assert( energy >= 0 );
699             silk_assert( lag_counter < SCRATCH_SIZE );
700             scratch_mem[ lag_counter ] = energy;
701             lag_counter++;
702         }
703
704         delta = matrix_ptr( Lag_range_ptr, k, 0, 2 );
705         for( i = 0; i < nb_cbk_search; i++ ) {
706             /* Fill out the 3 dim array that stores the correlations for    */
707             /* each code_book vector for each start lag                     */
708             idx = matrix_ptr( Lag_CB_ptr, k, i, cbk_size ) - delta;
709             for( j = 0; j < PE_NB_STAGE3_LAGS; j++ ) {
710                 silk_assert( idx + j < SCRATCH_SIZE );
711                 silk_assert( idx + j < lag_counter );
712                 energies_st3[ k ][ i ][ j ] = scratch_mem[ idx + j ];
713                 silk_assert( energies_st3[ k ][ i ][ j ] >= 0 );
714             }
715         }
716         target_ptr += sf_length;
717     }
718 }
719
720 opus_int32 silk_P_Ana_find_scaling(
721     const opus_int16  *frame,
722     const opus_int    frame_length,
723     const opus_int    sum_sqr_len
724 )
725 {
726     opus_int32 nbits, x_max;
727
728     x_max = silk_int16_array_maxabs( frame, frame_length );
729
730     if( x_max < silk_int16_MAX ) {
731         /* Number of bits needed for the sum of the squares */
732         nbits = 32 - silk_CLZ32( silk_SMULBB( x_max, x_max ) );
733     } else {
734         /* Here we don't know if x_max should have been silk_int16_MAX + 1, so we expect the worst case */
735         nbits = 30;
736     }
737     nbits += 17 - silk_CLZ16( sum_sqr_len );
738
739     /* Without a guarantee of saturation, we need to keep the 31st bit free */
740     if( nbits < 31 ) {
741         return 0;
742     } else {
743         return( nbits - 30 );
744     }
745 }