Including config.h from all the SILK files
[opus.git] / silk / silk_pitch_analysis_core.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, (subject to the limitations in the disclaimer below)
5 are permitted provided that the following conditions 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 Skype Limited, nor the names of specific
12 contributors, may be used to endorse or promote products derived from
13 this software without specific prior written permission.
14 NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED
15 BY THIS LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
16 CONTRIBUTORS ''AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING,
17 BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
18 FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
19 COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
20 INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
21 NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
22 USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
23 ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
25 OF THIS SOFTWARE, EVEN IF ADVISED OF THE 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 "silk_SigProc_FIX.h"
36 #include "silk_pitch_est_defines.h"
37 #include "silk_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  signal[],                        /* 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  signal[],                        /* 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  *signal,
64     const opus_int    signal_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  *signal,            /* 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 signal_8kHz[ PE_MAX_FRAME_LENGTH_ST_2 ];
86     opus_int16 signal_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_signal_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     SKP_assert( Fs_kHz == 8 || Fs_kHz == 12 || Fs_kHz == 16 );
111
112     /* Check for valid complexity setting */
113     SKP_assert( complexity >= SILK_PE_MIN_COMPLEX );
114     SKP_assert( complexity <= SILK_PE_MAX_COMPLEX );
115
116     SKP_assert( search_thres1_Q16 >= 0 && search_thres1_Q16 <= (1<<16) );
117     SKP_assert( search_thres2_Q15 >= 0 && search_thres2_Q15 <= (1<<15) );
118
119     /* Setup 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     SKP_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         SKP_memset( filt_state, 0, 2 * sizeof( opus_int32 ) );
138         silk_resampler_down2( filt_state, signal_8kHz, signal, frame_length );
139     } else if ( Fs_kHz == 12 ) {
140         SKP_memset( filt_state, 0, 6 * sizeof( opus_int32 ) );
141         silk_resampler_down2_3( filt_state, signal_8kHz, signal, frame_length );
142     } else {
143         SKP_assert( Fs_kHz == 8 );
144         SKP_memcpy( signal_8kHz, signal, frame_length_8kHz * sizeof(opus_int16) );
145     }
146
147     /* Decimate again to 4 kHz */
148     SKP_memset( filt_state, 0, 2 * sizeof( opus_int32 ) );/* Set state to zero */
149     silk_resampler_down2( filt_state, signal_4kHz, signal_8kHz, frame_length_8kHz );
150
151     /* Low-pass filter */
152     for( i = frame_length_4kHz - 1; i > 0; i-- ) {
153         signal_4kHz[ i ] = SKP_ADD_SAT16( signal_4kHz[ i ], signal_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 = SKP_max_32( sf_length_8kHz, SKP_LSHIFT( sf_length_4kHz, 2 ) );
163     shift = silk_P_Ana_find_scaling( signal_4kHz, frame_length_4kHz, max_sum_sq_length );
164     if( shift > 0 ) {
165         for( i = 0; i < frame_length_4kHz; i++ ) {
166             signal_4kHz[ i ] = SKP_RSHIFT( signal_4kHz[ i ], shift );
167         }
168     }
169
170     /******************************************************************************
171     * FIRST STAGE, operating in 4 khz
172     ******************************************************************************/
173     target_ptr = &signal_4kHz[ SKP_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         SKP_assert( target_ptr >= signal_4kHz );
177         SKP_assert( target_ptr + sf_length_8kHz <= signal_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         SKP_assert( basis_ptr >= signal_4kHz );
183         SKP_assert( basis_ptr + sf_length_8kHz <= signal_4kHz + frame_length_4kHz );
184
185         normalizer = 0;
186         cross_corr = 0;
187         /* Calculate first vector products before loop */
188         cross_corr = silk_inner_prod_aligned( target_ptr, basis_ptr, sf_length_8kHz );
189         normalizer = silk_inner_prod_aligned( basis_ptr,  basis_ptr, sf_length_8kHz );
190         normalizer = SKP_ADD_SAT32( normalizer, SKP_SMULBB( sf_length_8kHz, 4000 ) );
191
192         temp32 = SKP_DIV32( cross_corr, silk_SQRT_APPROX( normalizer ) + 1 );
193         C[ k ][ min_lag_4kHz ] = (opus_int16)SKP_SAT16( temp32 );        /* Q0 */
194
195         /* From now on normalizer is computed recursively */
196         for( d = min_lag_4kHz + 1; d <= max_lag_4kHz; d++ ) {
197             basis_ptr--;
198
199             /* Check that we are within range of the array */
200             SKP_assert( basis_ptr >= signal_4kHz );
201             SKP_assert( basis_ptr + sf_length_8kHz <= signal_4kHz + frame_length_4kHz );
202
203             cross_corr = silk_inner_prod_aligned( target_ptr, basis_ptr, sf_length_8kHz );
204
205             /* Add contribution of new sample and remove contribution from oldest sample */
206             normalizer +=
207                 SKP_SMULBB( basis_ptr[ 0 ], basis_ptr[ 0 ] ) -
208                 SKP_SMULBB( basis_ptr[ sf_length_8kHz ], basis_ptr[ sf_length_8kHz ] );
209
210             temp32 = SKP_DIV32( cross_corr, silk_SQRT_APPROX( normalizer ) + 1 );
211             C[ k ][ d ] = (opus_int16)SKP_SAT16( temp32 );                        /* Q0 */
212         }
213         /* Update target pointer */
214         target_ptr += sf_length_8kHz;
215     }
216
217     /* Combine two subframes into single correlation measure and apply short-lag bias */
218     if( nb_subfr == PE_MAX_NB_SUBFR ) {
219         for( i = max_lag_4kHz; i >= min_lag_4kHz; i-- ) {
220             sum = (opus_int32)C[ 0 ][ i ] + (opus_int32)C[ 1 ][ i ];                /* Q0 */
221             SKP_assert( SKP_RSHIFT( sum, 1 ) == SKP_SAT16( SKP_RSHIFT( sum, 1 ) ) );
222             sum = SKP_RSHIFT( sum, 1 );                                           /* Q-1 */
223             SKP_assert( SKP_LSHIFT( (opus_int32)-i, 4 ) == SKP_SAT16( SKP_LSHIFT( (opus_int32)-i, 4 ) ) );
224             sum = SKP_SMLAWB( sum, sum, SKP_LSHIFT( -i, 4 ) );                    /* Q-1 */
225             SKP_assert( sum == SKP_SAT16( sum ) );
226             C[ 0 ][ i ] = (opus_int16)sum;                                         /* Q-1 */
227         }
228     } else {
229         /* Only short-lag bias */
230         for( i = max_lag_4kHz; i >= min_lag_4kHz; i-- ) {
231             sum = (opus_int32)C[ 0 ][ i ];
232             sum = SKP_SMLAWB( sum, sum, SKP_LSHIFT( -i, 4 ) );                    /* Q-1 */
233             C[ 0 ][ i ] = (opus_int16)sum;                                         /* Q-1 */
234         }
235     }
236
237     /* Sort */
238     length_d_srch = SKP_ADD_LSHIFT32( 4, complexity, 1 );
239     SKP_assert( 3 * length_d_srch <= PE_D_SRCH_LENGTH );
240     silk_insertion_sort_decreasing_int16( &C[ 0 ][ min_lag_4kHz ], d_srch, max_lag_4kHz - min_lag_4kHz + 1, length_d_srch );
241
242     /* Escape if correlation is very low already here */
243     target_ptr = &signal_4kHz[ SKP_SMULBB( sf_length_4kHz, nb_subfr ) ];
244     energy = silk_inner_prod_aligned( target_ptr, target_ptr, SKP_LSHIFT( sf_length_4kHz, 2 ) );
245     energy = SKP_ADD_SAT32( energy, 1000 );                                  /* Q0 */
246     Cmax = (opus_int)C[ 0 ][ min_lag_4kHz ];                                  /* Q-1 */
247     threshold = SKP_SMULBB( Cmax, Cmax );                                    /* Q-2 */
248
249     /* Compare in Q-2 domain */
250     if( SKP_RSHIFT( energy, 4 + 2 ) > threshold ) {
251         SKP_memset( pitch_out, 0, nb_subfr * sizeof( opus_int ) );
252         *LTPCorr_Q15  = 0;
253         *lagIndex     = 0;
254         *contourIndex = 0;
255         return 1;
256     }
257
258     threshold = SKP_SMULWB( search_thres1_Q16, Cmax );
259     for( i = 0; i < length_d_srch; i++ ) {
260         /* Convert to 8 kHz indices for the sorted correlation that exceeds the threshold */
261         if( C[ 0 ][ min_lag_4kHz + i ] > threshold ) {
262             d_srch[ i ] = SKP_LSHIFT( d_srch[ i ] + min_lag_4kHz, 1 );
263         } else {
264             length_d_srch = i;
265             break;
266         }
267     }
268     SKP_assert( length_d_srch > 0 );
269
270     for( i = min_lag_8kHz - 5; i < max_lag_8kHz + 5; i++ ) {
271         d_comp[ i ] = 0;
272     }
273     for( i = 0; i < length_d_srch; i++ ) {
274         d_comp[ d_srch[ i ] ] = 1;
275     }
276
277     /* Convolution */
278     for( i = max_lag_8kHz + 3; i >= min_lag_8kHz; i-- ) {
279         d_comp[ i ] += d_comp[ i - 1 ] + d_comp[ i - 2 ];
280     }
281
282     length_d_srch = 0;
283     for( i = min_lag_8kHz; i < max_lag_8kHz + 1; i++ ) {
284         if( d_comp[ i + 1 ] > 0 ) {
285             d_srch[ length_d_srch ] = i;
286             length_d_srch++;
287         }
288     }
289
290     /* Convolution */
291     for( i = max_lag_8kHz + 3; i >= min_lag_8kHz; i-- ) {
292         d_comp[ i ] += d_comp[ i - 1 ] + d_comp[ i - 2 ] + d_comp[ i - 3 ];
293     }
294
295     length_d_comp = 0;
296     for( i = min_lag_8kHz; i < max_lag_8kHz + 4; i++ ) {
297         if( d_comp[ i ] > 0 ) {
298             d_comp[ length_d_comp ] = i - 2;
299             length_d_comp++;
300         }
301     }
302
303     /**********************************************************************************
304     ** SECOND STAGE, operating at 8 kHz, on lag sections with high correlation
305     *************************************************************************************/
306
307     /******************************************************************************
308     ** Scale signal down to avoid correlations measures from overflowing
309     *******************************************************************************/
310     /* find scaling as max scaling for each subframe */
311     shift = silk_P_Ana_find_scaling( signal_8kHz, frame_length_8kHz, sf_length_8kHz );
312     if( shift > 0 ) {
313         for( i = 0; i < frame_length_8kHz; i++ ) {
314             signal_8kHz[ i ] = SKP_RSHIFT( signal_8kHz[ i ], shift );
315         }
316     }
317
318     /*********************************************************************************
319     * Find energy of each subframe projected onto its history, for a range of delays
320     *********************************************************************************/
321     SKP_memset( C, 0, PE_MAX_NB_SUBFR * ( ( PE_MAX_LAG >> 1 ) + 5 ) * sizeof( opus_int16 ) );
322
323     target_ptr = &signal_8kHz[ PE_LTP_MEM_LENGTH_MS * 8 ];
324     for( k = 0; k < nb_subfr; k++ ) {
325
326         /* Check that we are within range of the array */
327         SKP_assert( target_ptr >= signal_8kHz );
328         SKP_assert( target_ptr + sf_length_8kHz <= signal_8kHz + frame_length_8kHz );
329
330         energy_target = silk_inner_prod_aligned( target_ptr, target_ptr, sf_length_8kHz );
331         // ToDo: Calculate 1 / energy_target here and save one division inside next for loop
332         for( j = 0; j < length_d_comp; j++ ) {
333             d = d_comp[ j ];
334             basis_ptr = target_ptr - d;
335
336             /* Check that we are within range of the array */
337             SKP_assert( basis_ptr >= signal_8kHz );
338             SKP_assert( basis_ptr + sf_length_8kHz <= signal_8kHz + frame_length_8kHz );
339
340             cross_corr   = silk_inner_prod_aligned( target_ptr, basis_ptr, sf_length_8kHz );
341             energy_basis = silk_inner_prod_aligned( basis_ptr,  basis_ptr, sf_length_8kHz );
342             if( cross_corr > 0 ) {
343                 energy = SKP_max( energy_target, energy_basis ); /* Find max to make sure first division < 1.0 */
344                 lz = silk_CLZ32( cross_corr );
345                 lshift = SKP_LIMIT_32( lz - 1, 0, 15 );
346                 temp32 = SKP_DIV32( SKP_LSHIFT( cross_corr, lshift ), SKP_RSHIFT( energy, 15 - lshift ) + 1 ); /* Q15 */
347                 SKP_assert( temp32 == SKP_SAT16( temp32 ) );
348                 temp32 = SKP_SMULWB( cross_corr, temp32 ); /* Q(-1), cc * ( cc / max(b, t) ) */
349                 temp32 = SKP_ADD_SAT32( temp32, temp32 );  /* Q(0) */
350                 lz = silk_CLZ32( temp32 );
351                 lshift = SKP_LIMIT_32( lz - 1, 0, 15 );
352                 energy = SKP_min( energy_target, energy_basis );
353                 C[ k ][ d ] = SKP_DIV32( SKP_LSHIFT( temp32, lshift ), SKP_RSHIFT( energy, 15 - lshift ) + 1 ); // Q15
354             } else {
355                 C[ k ][ d ] = 0;
356             }
357         }
358         target_ptr += sf_length_8kHz;
359     }
360
361     /* search over lag range and lags codebook */
362     /* scale factor for lag codebook, as a function of center lag */
363
364     CCmax   = SKP_int32_MIN;
365     CCmax_b = SKP_int32_MIN;
366
367     CBimax = 0; /* To avoid returning undefined lag values */
368     lag = -1;   /* To check if lag with strong enough correlation has been found */
369
370     if( prevLag > 0 ) {
371         if( Fs_kHz == 12 ) {
372             prevLag = SKP_DIV32_16( SKP_LSHIFT( prevLag, 1 ), 3 );
373         } else if( Fs_kHz == 16 ) {
374             prevLag = SKP_RSHIFT( prevLag, 1 );
375         }
376         prevLag_log2_Q7 = silk_lin2log( (opus_int32)prevLag );
377     } else {
378         prevLag_log2_Q7 = 0;
379     }
380     SKP_assert( search_thres2_Q15 == SKP_SAT16( search_thres2_Q15 ) );
381     /* Setup stage 2 codebook based on number of subframes */
382     if( nb_subfr == PE_MAX_NB_SUBFR ) {
383         cbk_size   = PE_NB_CBKS_STAGE2_EXT;
384         Lag_CB_ptr = &silk_CB_lags_stage2[ 0 ][ 0 ];
385         if( Fs_kHz == 8 && complexity > SILK_PE_MIN_COMPLEX ) {
386             /* If input is 8 khz use a larger codebook here because it is last stage */
387             nb_cbk_search = PE_NB_CBKS_STAGE2_EXT;
388         } else {
389             nb_cbk_search = PE_NB_CBKS_STAGE2;
390         }
391         corr_thres_Q15 = SKP_RSHIFT( SKP_SMULBB( search_thres2_Q15, search_thres2_Q15 ), 13 );
392     } else {
393         cbk_size       = PE_NB_CBKS_STAGE2_10MS;
394         Lag_CB_ptr     = &silk_CB_lags_stage2_10_ms[ 0 ][ 0 ];
395         nb_cbk_search  = PE_NB_CBKS_STAGE2_10MS;
396         corr_thres_Q15 = SKP_RSHIFT( SKP_SMULBB( search_thres2_Q15, search_thres2_Q15 ), 14 );
397     }
398
399     for( k = 0; k < length_d_srch; k++ ) {
400         d = d_srch[ k ];
401         for( j = 0; j < nb_cbk_search; j++ ) {
402             CC[ j ] = 0;
403             for( i = 0; i < nb_subfr; i++ ) {
404                 /* Try all codebooks */
405                 CC[ j ] = CC[ j ] + (opus_int32)C[ i ][ d + matrix_ptr( Lag_CB_ptr, i, j, cbk_size )];
406             }
407         }
408         /* Find best codebook */
409         CCmax_new = SKP_int32_MIN;
410         CBimax_new = 0;
411         for( i = 0; i < nb_cbk_search; i++ ) {
412             if( CC[ i ] > CCmax_new ) {
413                 CCmax_new = CC[ i ];
414                 CBimax_new = i;
415             }
416         }
417
418         /* Bias towards shorter lags */
419         lag_log2_Q7 = silk_lin2log( (opus_int32)d ); /* Q7 */
420         SKP_assert( lag_log2_Q7 == SKP_SAT16( lag_log2_Q7 ) );
421         SKP_assert( nb_subfr * SILK_FIX_CONST( PE_SHORTLAG_BIAS, 15 ) == SKP_SAT16( nb_subfr * SILK_FIX_CONST( PE_SHORTLAG_BIAS, 15 ) ) );
422         CCmax_new_b = CCmax_new - SKP_RSHIFT( SKP_SMULBB( nb_subfr * SILK_FIX_CONST( PE_SHORTLAG_BIAS, 15 ), lag_log2_Q7 ), 7 ); /* Q15 */
423
424         /* Bias towards previous lag */
425         SKP_assert( nb_subfr * SILK_FIX_CONST( PE_PREVLAG_BIAS, 15 ) == SKP_SAT16( nb_subfr * SILK_FIX_CONST( PE_PREVLAG_BIAS, 15 ) ) );
426         if( prevLag > 0 ) {
427             delta_lag_log2_sqr_Q7 = lag_log2_Q7 - prevLag_log2_Q7;
428             SKP_assert( delta_lag_log2_sqr_Q7 == SKP_SAT16( delta_lag_log2_sqr_Q7 ) );
429             delta_lag_log2_sqr_Q7 = SKP_RSHIFT( SKP_SMULBB( delta_lag_log2_sqr_Q7, delta_lag_log2_sqr_Q7 ), 7 );
430             prev_lag_bias_Q15 = SKP_RSHIFT( SKP_SMULBB( nb_subfr * SILK_FIX_CONST( PE_PREVLAG_BIAS, 15 ), *LTPCorr_Q15 ), 15 ); /* Q15 */
431             prev_lag_bias_Q15 = SKP_DIV32( SKP_MUL( prev_lag_bias_Q15, delta_lag_log2_sqr_Q7 ), delta_lag_log2_sqr_Q7 + ( 1 << 6 ) );
432             CCmax_new_b -= prev_lag_bias_Q15; /* Q15 */
433         }
434
435         if ( CCmax_new_b > CCmax_b                                          && /* Find maximum biased correlation                  */
436              CCmax_new > corr_thres_Q15                                     && /* Correlation needs to be high enough to be voiced */
437              silk_CB_lags_stage2[ 0 ][ CBimax_new ] <= min_lag_8kHz        /* Lag must be in range                             */
438             ) {
439             CCmax_b = CCmax_new_b;
440             CCmax   = CCmax_new;
441             lag     = d;
442             CBimax  = CBimax_new;
443         }
444     }
445
446     if( lag == -1 ) {
447         /* No suitable candidate found */
448         SKP_memset( pitch_out, 0, nb_subfr * sizeof( opus_int ) );
449         *LTPCorr_Q15  = 0;
450         *lagIndex     = 0;
451         *contourIndex = 0;
452         return 1;
453     }
454
455     if( Fs_kHz > 8 ) {
456
457         /******************************************************************************
458         ** Scale input signal down to avoid correlations measures from overflowing
459         *******************************************************************************/
460         /* find scaling as max scaling for each subframe */
461         shift = silk_P_Ana_find_scaling( signal, frame_length, sf_length );
462         if( shift > 0 ) {
463             /* Move signal to scratch mem because the input signal should be unchanged */
464             /* Reuse the 32 bit scratch mem vector, use a 16 bit pointer from now */
465             input_signal_ptr = (opus_int16*)scratch_mem;
466             for( i = 0; i < frame_length; i++ ) {
467                 input_signal_ptr[ i ] = SKP_RSHIFT( signal[ i ], shift );
468             }
469         } else {
470             input_signal_ptr = (opus_int16*)signal;
471         }
472         /*********************************************************************************/
473
474         /* Search in original signal */
475
476         CBimax_old = CBimax;
477         /* Compensate for decimation */
478         SKP_assert( lag == SKP_SAT16( lag ) );
479         if( Fs_kHz == 12 ) {
480             lag = SKP_RSHIFT( SKP_SMULBB( lag, 3 ), 1 );
481         } else if( Fs_kHz == 16 ) {
482             lag = SKP_LSHIFT( lag, 1 );
483         } else {
484             lag = SKP_SMULBB( lag, 3 );
485         }
486
487         lag = SKP_LIMIT_int( lag, min_lag, max_lag );
488         start_lag = SKP_max_int( lag - 2, min_lag );
489         end_lag   = SKP_min_int( lag + 2, max_lag );
490         lag_new   = lag;                                    /* to avoid undefined lag */
491         CBimax    = 0;                                        /* to avoid undefined lag */
492         SKP_assert( SKP_LSHIFT( CCmax, 13 ) >= 0 );
493         *LTPCorr_Q15 = (opus_int)silk_SQRT_APPROX( SKP_LSHIFT( CCmax, 13 ) ); /* Output normalized correlation */
494
495         CCmax = SKP_int32_MIN;
496         /* pitch lags according to second stage */
497         for( k = 0; k < nb_subfr; k++ ) {
498             pitch_out[ k ] = lag + 2 * silk_CB_lags_stage2[ k ][ CBimax_old ];
499         }
500         /* Calculate the correlations and energies needed in stage 3 */
501         silk_P_Ana_calc_corr_st3(  crosscorr_st3, input_signal_ptr, start_lag, sf_length, nb_subfr, complexity );
502         silk_P_Ana_calc_energy_st3( energies_st3, input_signal_ptr, start_lag, sf_length, nb_subfr, complexity );
503
504         lag_counter = 0;
505         SKP_assert( lag == SKP_SAT16( lag ) );
506         contour_bias_Q20 = SKP_DIV32_16( SILK_FIX_CONST( PE_FLATCONTOUR_BIAS, 20 ), lag );
507
508         /* Setup cbk parameters acording to complexity setting and frame length */
509         if( nb_subfr == PE_MAX_NB_SUBFR ) {
510             nb_cbk_search   = (opus_int)silk_nb_cbk_searchs_stage3[ complexity ];
511             cbk_size        = PE_NB_CBKS_STAGE3_MAX;
512             Lag_CB_ptr      = &silk_CB_lags_stage3[ 0 ][ 0 ];
513         } else {
514             nb_cbk_search   = PE_NB_CBKS_STAGE3_10MS;
515             cbk_size        = PE_NB_CBKS_STAGE3_10MS;
516             Lag_CB_ptr      = &silk_CB_lags_stage3_10_ms[ 0 ][ 0 ];
517         }
518         for( d = start_lag; d <= end_lag; d++ ) {
519             for( j = 0; j < nb_cbk_search; j++ ) {
520                 cross_corr = 0;
521                 energy     = 0;
522                 for( k = 0; k < nb_subfr; k++ ) {
523                     SKP_assert( PE_MAX_NB_SUBFR == 4 );
524                     energy     += SKP_RSHIFT( energies_st3[  k ][ j ][ lag_counter ], 2 ); /* use mean, to avoid overflow */
525                     SKP_assert( energy >= 0 );
526                     cross_corr += SKP_RSHIFT( crosscorr_st3[ k ][ j ][ lag_counter ], 2 ); /* use mean, to avoid overflow */
527                 }
528                 if( cross_corr > 0 ) {
529                     /* Divide cross_corr / energy and get result in Q15 */
530                     lz = silk_CLZ32( cross_corr );
531                     /* Divide with result in Q13, cross_corr could be larger than energy */
532                     lshift = SKP_LIMIT_32( lz - 1, 0, 13 );
533                     CCmax_new = SKP_DIV32( SKP_LSHIFT( cross_corr, lshift ), SKP_RSHIFT( energy, 13 - lshift ) + 1 );
534                     CCmax_new = SKP_SAT16( CCmax_new );
535                     CCmax_new = SKP_SMULWB( cross_corr, CCmax_new );
536                     /* Saturate */
537                     if( CCmax_new > SKP_RSHIFT( SKP_int32_MAX, 3 ) ) {
538                         CCmax_new = SKP_int32_MAX;
539                     } else {
540                         CCmax_new = SKP_LSHIFT( CCmax_new, 3 );
541                     }
542                     /* Reduce depending on flatness of contour */
543                     diff = SKP_int16_MAX - SKP_RSHIFT( SKP_MUL( contour_bias_Q20, j ), 5 ); /* Q20 -> Q15 */
544                     SKP_assert( diff == SKP_SAT16( diff ) );
545                     CCmax_new = SKP_LSHIFT( SKP_SMULWB( CCmax_new, diff ), 1 );
546                 } else {
547                     CCmax_new = 0;
548                 }
549
550                 if( CCmax_new > CCmax                                               &&
551                    ( d + silk_CB_lags_stage3[ 0 ][ j ] ) <= max_lag
552                    ) {
553                     CCmax   = CCmax_new;
554                     lag_new = d;
555                     CBimax  = j;
556                 }
557             }
558             lag_counter++;
559         }
560
561         for( k = 0; k < nb_subfr; k++ ) {
562             pitch_out[ k ] = lag_new + matrix_ptr( Lag_CB_ptr, k, CBimax, cbk_size );
563         }
564         *lagIndex = (opus_int16)( lag_new - min_lag);
565         *contourIndex = (opus_int8)CBimax;
566     } else {
567         /* Save Lags and correlation */
568         CCmax = SKP_max( CCmax, 0 );
569         *LTPCorr_Q15 = (opus_int)silk_SQRT_APPROX( SKP_LSHIFT( CCmax, 13 ) ); /* Output normalized correlation */
570         for( k = 0; k < nb_subfr; k++ ) {
571             pitch_out[ k ] = lag + matrix_ptr( Lag_CB_ptr, k, CBimax, cbk_size );
572         }
573         *lagIndex = (opus_int16)( lag - min_lag_8kHz );
574         *contourIndex = (opus_int8)CBimax;
575     }
576     SKP_assert( *lagIndex >= 0 );
577     /* return as voiced */
578     return 0;
579 }
580
581 /*************************************************************************/
582 /* Calculates the correlations used in stage 3 search. In order to cover */
583 /* the whole lag codebook for all the searched offset lags (lag +- 2),   */
584 /*************************************************************************/
585 void silk_P_Ana_calc_corr_st3(
586     opus_int32        cross_corr_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ],/* (O) 3 DIM correlation array */
587     const opus_int16  signal[],                        /* I vector to correlate         */
588     opus_int          start_lag,                       /* I lag offset to search around */
589     opus_int          sf_length,                       /* I length of a 5 ms subframe   */
590     opus_int          nb_subfr,                        /* I number of subframes         */
591     opus_int          complexity                       /* I Complexity setting          */
592 )
593 {
594     const opus_int16 *target_ptr, *basis_ptr;
595     opus_int32 cross_corr;
596     opus_int   i, j, k, lag_counter, lag_low, lag_high;
597     opus_int   nb_cbk_search, delta, idx, cbk_size;
598     opus_int32 scratch_mem[ SCRATCH_SIZE ];
599     const opus_int8 *Lag_range_ptr, *Lag_CB_ptr;
600
601     SKP_assert( complexity >= SILK_PE_MIN_COMPLEX );
602     SKP_assert( complexity <= SILK_PE_MAX_COMPLEX );
603
604     if( nb_subfr == PE_MAX_NB_SUBFR ){
605         Lag_range_ptr = &silk_Lag_range_stage3[ complexity ][ 0 ][ 0 ];
606         Lag_CB_ptr    = &silk_CB_lags_stage3[ 0 ][ 0 ];
607         nb_cbk_search = silk_nb_cbk_searchs_stage3[ complexity ];
608         cbk_size      = PE_NB_CBKS_STAGE3_MAX;
609     } else {
610         SKP_assert( nb_subfr == PE_MAX_NB_SUBFR >> 1);
611         Lag_range_ptr = &silk_Lag_range_stage3_10_ms[ 0 ][ 0 ];
612         Lag_CB_ptr    = &silk_CB_lags_stage3_10_ms[ 0 ][ 0 ];
613         nb_cbk_search = PE_NB_CBKS_STAGE3_10MS;
614         cbk_size      = PE_NB_CBKS_STAGE3_10MS;
615     }
616
617     target_ptr = &signal[ SKP_LSHIFT( sf_length, 2 ) ]; /* Pointer to middle of frame */
618     for( k = 0; k < nb_subfr; k++ ) {
619         lag_counter = 0;
620
621         /* Calculate the correlations for each subframe */
622         lag_low  = matrix_ptr( Lag_range_ptr, k, 0, 2 );
623         lag_high = matrix_ptr( Lag_range_ptr, k, 1, 2 );
624         for( j = lag_low; j <= lag_high; j++ ) {
625             basis_ptr = target_ptr - ( start_lag + j );
626             cross_corr = silk_inner_prod_aligned( (opus_int16*)target_ptr, (opus_int16*)basis_ptr, sf_length );
627             SKP_assert( lag_counter < SCRATCH_SIZE );
628             scratch_mem[ lag_counter ] = cross_corr;
629             lag_counter++;
630         }
631
632         delta = matrix_ptr( Lag_range_ptr, k, 0, 2 );
633         for( i = 0; i < nb_cbk_search; i++ ) {
634             /* Fill out the 3 dim array that stores the correlations for */
635             /* each code_book vector for each start lag */
636             idx = matrix_ptr( Lag_CB_ptr, k, i, cbk_size ) - delta;
637             for( j = 0; j < PE_NB_STAGE3_LAGS; j++ ) {
638                 SKP_assert( idx + j < SCRATCH_SIZE );
639                 SKP_assert( idx + j < lag_counter );
640                 cross_corr_st3[ k ][ i ][ j ] = scratch_mem[ idx + j ];
641             }
642         }
643         target_ptr += sf_length;
644     }
645 }
646
647 /********************************************************************/
648 /* Calculate the energies for first two subframes. The energies are */
649 /* calculated recursively.                                          */
650 /********************************************************************/
651 void silk_P_Ana_calc_energy_st3(
652     opus_int32        energies_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ],/* (O) 3 DIM energy array */
653     const opus_int16  signal[],                        /* I vector to calc energy in    */
654     opus_int          start_lag,                       /* I lag offset to search around */
655     opus_int          sf_length,                       /* I length of one 5 ms subframe */
656     opus_int          nb_subfr,                     /* I number of subframes         */
657     opus_int          complexity                       /* I Complexity setting          */
658 )
659 {
660     const opus_int16 *target_ptr, *basis_ptr;
661     opus_int32 energy;
662     opus_int   k, i, j, lag_counter;
663     opus_int   nb_cbk_search, delta, idx, cbk_size, lag_diff;
664     opus_int32 scratch_mem[ SCRATCH_SIZE ];
665     const opus_int8 *Lag_range_ptr, *Lag_CB_ptr;
666
667     SKP_assert( complexity >= SILK_PE_MIN_COMPLEX );
668     SKP_assert( complexity <= SILK_PE_MAX_COMPLEX );
669
670     if( nb_subfr == PE_MAX_NB_SUBFR ){
671         Lag_range_ptr = &silk_Lag_range_stage3[ complexity ][ 0 ][ 0 ];
672         Lag_CB_ptr    = &silk_CB_lags_stage3[ 0 ][ 0 ];
673         nb_cbk_search = silk_nb_cbk_searchs_stage3[ complexity ];
674         cbk_size      = PE_NB_CBKS_STAGE3_MAX;
675     } else {
676         SKP_assert( nb_subfr == PE_MAX_NB_SUBFR >> 1);
677         Lag_range_ptr = &silk_Lag_range_stage3_10_ms[ 0 ][ 0 ];
678         Lag_CB_ptr    = &silk_CB_lags_stage3_10_ms[ 0 ][ 0 ];
679         nb_cbk_search = PE_NB_CBKS_STAGE3_10MS;
680         cbk_size      = PE_NB_CBKS_STAGE3_10MS;
681     }
682     target_ptr = &signal[ SKP_LSHIFT( sf_length, 2 ) ];
683     for( k = 0; k < nb_subfr; k++ ) {
684         lag_counter = 0;
685
686         /* Calculate the energy for first lag */
687         basis_ptr = target_ptr - ( start_lag + matrix_ptr( Lag_range_ptr, k, 0, 2 ) );
688         energy = silk_inner_prod_aligned( basis_ptr, basis_ptr, sf_length );
689         SKP_assert( energy >= 0 );
690         scratch_mem[ lag_counter ] = energy;
691         lag_counter++;
692
693         lag_diff = ( matrix_ptr( Lag_range_ptr, k, 1, 2 ) -  matrix_ptr( Lag_range_ptr, k, 0, 2 ) + 1 );
694         for( i = 1; i < lag_diff; i++ ) {
695             /* remove part outside new window */
696             energy -= SKP_SMULBB( basis_ptr[ sf_length - i ], basis_ptr[ sf_length - i ] );
697             SKP_assert( energy >= 0 );
698
699             /* add part that comes into window */
700             energy = SKP_ADD_SAT32( energy, SKP_SMULBB( basis_ptr[ -i ], basis_ptr[ -i ] ) );
701             SKP_assert( energy >= 0 );
702             SKP_assert( lag_counter < SCRATCH_SIZE );
703             scratch_mem[ lag_counter ] = energy;
704             lag_counter++;
705         }
706
707         delta = matrix_ptr( Lag_range_ptr, k, 0, 2 );
708         for( i = 0; i < nb_cbk_search; i++ ) {
709             /* Fill out the 3 dim array that stores the correlations for    */
710             /* each code_book vector for each start lag                     */
711             idx = matrix_ptr( Lag_CB_ptr, k, i, cbk_size ) - delta;
712             for( j = 0; j < PE_NB_STAGE3_LAGS; j++ ) {
713                 SKP_assert( idx + j < SCRATCH_SIZE );
714                 SKP_assert( idx + j < lag_counter );
715                 energies_st3[ k ][ i ][ j ] = scratch_mem[ idx + j ];
716                 SKP_assert( energies_st3[ k ][ i ][ j ] >= 0 );
717             }
718         }
719         target_ptr += sf_length;
720     }
721 }
722
723 opus_int32 silk_P_Ana_find_scaling(
724     const opus_int16  *signal,
725     const opus_int    signal_length,
726     const opus_int    sum_sqr_len
727 )
728 {
729     opus_int32 nbits, x_max;
730
731     x_max = silk_int16_array_maxabs( signal, signal_length );
732
733     if( x_max < SKP_int16_MAX ) {
734         /* Number of bits needed for the sum of the squares */
735         nbits = 32 - silk_CLZ32( SKP_SMULBB( x_max, x_max ) );
736     } else {
737         /* Here we don't know if x_max should have been SKP_int16_MAX + 1, so we expect the worst case */
738         nbits = 30;
739     }
740     nbits += 17 - silk_CLZ16( sum_sqr_len );
741
742     /* Without a guarantee of saturation, we need to keep the 31st bit free */
743     if( nbits < 31 ) {
744         return 0;
745     } else {
746         return( nbits - 30 );
747     }
748 }