Add the noreturn attribute on the assert functions to aid static analysis, improve...
[opus.git] / 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 "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     /* 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     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         /* ToDo: Calculate 1 / energy_target here and save one division inside next for loop*/
330         for( j = 0; j < length_d_comp; j++ ) {
331             d = d_comp[ j ];
332             basis_ptr = target_ptr - d;
333
334             /* Check that we are within range of the array */
335             silk_assert( basis_ptr >= frame_8kHz );
336             silk_assert( basis_ptr + sf_length_8kHz <= frame_8kHz + frame_length_8kHz );
337
338             cross_corr   = silk_inner_prod_aligned( target_ptr, basis_ptr, sf_length_8kHz );
339             energy_basis = silk_inner_prod_aligned( basis_ptr,  basis_ptr, sf_length_8kHz );
340             if( cross_corr > 0 ) {
341                 energy = silk_max( energy_target, energy_basis ); /* Find max to make sure first division < 1.0 */
342                 lz = silk_CLZ32( cross_corr );
343                 lshift = silk_LIMIT_32( lz - 1, 0, 15 );
344                 temp32 = silk_DIV32( silk_LSHIFT( cross_corr, lshift ), silk_RSHIFT( energy, 15 - lshift ) + 1 ); /* Q15 */
345                 silk_assert( temp32 == silk_SAT16( temp32 ) );
346                 temp32 = silk_SMULWB( cross_corr, temp32 ); /* Q(-1), cc * ( cc / max(b, t) ) */
347                 temp32 = silk_ADD_SAT32( temp32, temp32 );  /* Q(0) */
348                 lz = silk_CLZ32( temp32 );
349                 lshift = silk_LIMIT_32( lz - 1, 0, 15 );
350                 energy = silk_min( energy_target, energy_basis );
351                 C[ k ][ d ] = silk_DIV32( silk_LSHIFT( temp32, lshift ), silk_RSHIFT( energy, 15 - lshift ) + 1 ); /* Q15*/
352             } else {
353                 C[ k ][ d ] = 0;
354             }
355         }
356         target_ptr += sf_length_8kHz;
357     }
358
359     /* search over lag range and lags codebook */
360     /* scale factor for lag codebook, as a function of center lag */
361
362     CCmax   = silk_int32_MIN;
363     CCmax_b = silk_int32_MIN;
364
365     CBimax = 0; /* To avoid returning undefined lag values */
366     lag = -1;   /* To check if lag with strong enough correlation has been found */
367
368     if( prevLag > 0 ) {
369         if( Fs_kHz == 12 ) {
370             prevLag = silk_DIV32_16( silk_LSHIFT( prevLag, 1 ), 3 );
371         } else if( Fs_kHz == 16 ) {
372             prevLag = silk_RSHIFT( prevLag, 1 );
373         }
374         prevLag_log2_Q7 = silk_lin2log( (opus_int32)prevLag );
375     } else {
376         prevLag_log2_Q7 = 0;
377     }
378     silk_assert( search_thres2_Q15 == silk_SAT16( search_thres2_Q15 ) );
379     /* Setup stage 2 codebook based on number of subframes */
380     if( nb_subfr == PE_MAX_NB_SUBFR ) {
381         cbk_size   = PE_NB_CBKS_STAGE2_EXT;
382         Lag_CB_ptr = &silk_CB_lags_stage2[ 0 ][ 0 ];
383         if( Fs_kHz == 8 && complexity > SILK_PE_MIN_COMPLEX ) {
384             /* If input is 8 khz use a larger codebook here because it is last stage */
385             nb_cbk_search = PE_NB_CBKS_STAGE2_EXT;
386         } else {
387             nb_cbk_search = PE_NB_CBKS_STAGE2;
388         }
389         corr_thres_Q15 = silk_RSHIFT( silk_SMULBB( search_thres2_Q15, search_thres2_Q15 ), 13 );
390     } else {
391         cbk_size       = PE_NB_CBKS_STAGE2_10MS;
392         Lag_CB_ptr     = &silk_CB_lags_stage2_10_ms[ 0 ][ 0 ];
393         nb_cbk_search  = PE_NB_CBKS_STAGE2_10MS;
394         corr_thres_Q15 = silk_RSHIFT( silk_SMULBB( search_thres2_Q15, search_thres2_Q15 ), 14 );
395     }
396
397     for( k = 0; k < length_d_srch; k++ ) {
398         d = d_srch[ k ];
399         for( j = 0; j < nb_cbk_search; j++ ) {
400             CC[ j ] = 0;
401             for( i = 0; i < nb_subfr; i++ ) {
402                 /* Try all codebooks */
403                 CC[ j ] = CC[ j ] + (opus_int32)C[ i ][ d + matrix_ptr( Lag_CB_ptr, i, j, cbk_size )];
404             }
405         }
406         /* Find best codebook */
407         CCmax_new = silk_int32_MIN;
408         CBimax_new = 0;
409         for( i = 0; i < nb_cbk_search; i++ ) {
410             if( CC[ i ] > CCmax_new ) {
411                 CCmax_new = CC[ i ];
412                 CBimax_new = i;
413             }
414         }
415
416         /* Bias towards shorter lags */
417         lag_log2_Q7 = silk_lin2log( (opus_int32)d ); /* Q7 */
418         silk_assert( lag_log2_Q7 == silk_SAT16( lag_log2_Q7 ) );
419         silk_assert( nb_subfr * SILK_FIX_CONST( PE_SHORTLAG_BIAS, 15 ) == silk_SAT16( nb_subfr * SILK_FIX_CONST( PE_SHORTLAG_BIAS, 15 ) ) );
420         CCmax_new_b = CCmax_new - silk_RSHIFT( silk_SMULBB( nb_subfr * SILK_FIX_CONST( PE_SHORTLAG_BIAS, 15 ), lag_log2_Q7 ), 7 ); /* Q15 */
421
422         /* Bias towards previous lag */
423         silk_assert( nb_subfr * SILK_FIX_CONST( PE_PREVLAG_BIAS, 15 ) == silk_SAT16( nb_subfr * SILK_FIX_CONST( PE_PREVLAG_BIAS, 15 ) ) );
424         if( prevLag > 0 ) {
425             delta_lag_log2_sqr_Q7 = lag_log2_Q7 - prevLag_log2_Q7;
426             silk_assert( delta_lag_log2_sqr_Q7 == silk_SAT16( delta_lag_log2_sqr_Q7 ) );
427             delta_lag_log2_sqr_Q7 = silk_RSHIFT( silk_SMULBB( delta_lag_log2_sqr_Q7, delta_lag_log2_sqr_Q7 ), 7 );
428             prev_lag_bias_Q15 = silk_RSHIFT( silk_SMULBB( nb_subfr * SILK_FIX_CONST( PE_PREVLAG_BIAS, 15 ), *LTPCorr_Q15 ), 15 ); /* Q15 */
429             prev_lag_bias_Q15 = silk_DIV32( silk_MUL( prev_lag_bias_Q15, delta_lag_log2_sqr_Q7 ), delta_lag_log2_sqr_Q7 + ( 1 << 6 ) );
430             CCmax_new_b -= prev_lag_bias_Q15; /* Q15 */
431         }
432
433         if ( CCmax_new_b > CCmax_b                                          && /* Find maximum biased correlation                  */
434              CCmax_new > corr_thres_Q15                                     && /* Correlation needs to be high enough to be voiced */
435              silk_CB_lags_stage2[ 0 ][ CBimax_new ] <= min_lag_8kHz        /* Lag must be in range                             */
436             ) {
437             CCmax_b = CCmax_new_b;
438             CCmax   = CCmax_new;
439             lag     = d;
440             CBimax  = CBimax_new;
441         }
442     }
443
444     if( lag == -1 ) {
445         /* No suitable candidate found */
446         silk_memset( pitch_out, 0, nb_subfr * sizeof( opus_int ) );
447         *LTPCorr_Q15  = 0;
448         *lagIndex     = 0;
449         *contourIndex = 0;
450         return 1;
451     }
452
453     if( Fs_kHz > 8 ) {
454
455         /******************************************************************************
456         ** Scale input signal down to avoid correlations measures from overflowing
457         *******************************************************************************/
458         /* find scaling as max scaling for each subframe */
459         shift = silk_P_Ana_find_scaling( frame, frame_length, sf_length );
460         if( shift > 0 ) {
461             /* Move signal to scratch mem because the input signal should be unchanged */
462             /* Reuse the 32 bit scratch mem vector, use a 16 bit pointer from now */
463             input_frame_ptr = (opus_int16*)scratch_mem;
464             for( i = 0; i < frame_length; i++ ) {
465                 input_frame_ptr[ i ] = silk_RSHIFT( frame[ i ], shift );
466             }
467         } else {
468             input_frame_ptr = (opus_int16*)frame;
469         }
470         /*********************************************************************************/
471
472         /* Search in original signal */
473
474         CBimax_old = CBimax;
475         /* Compensate for decimation */
476         silk_assert( lag == silk_SAT16( lag ) );
477         if( Fs_kHz == 12 ) {
478             lag = silk_RSHIFT( silk_SMULBB( lag, 3 ), 1 );
479         } else if( Fs_kHz == 16 ) {
480             lag = silk_LSHIFT( lag, 1 );
481         } else {
482             lag = silk_SMULBB( lag, 3 );
483         }
484
485         lag = silk_LIMIT_int( lag, min_lag, max_lag );
486         start_lag = silk_max_int( lag - 2, min_lag );
487         end_lag   = silk_min_int( lag + 2, max_lag );
488         lag_new   = lag;                                    /* to avoid undefined lag */
489         CBimax    = 0;                                        /* to avoid undefined lag */
490         silk_assert( silk_LSHIFT( CCmax, 13 ) >= 0 );
491         *LTPCorr_Q15 = (opus_int)silk_SQRT_APPROX( silk_LSHIFT( CCmax, 13 ) ); /* Output normalized correlation */
492
493         CCmax = silk_int32_MIN;
494         /* pitch lags according to second stage */
495         for( k = 0; k < nb_subfr; k++ ) {
496             pitch_out[ k ] = lag + 2 * silk_CB_lags_stage2[ k ][ CBimax_old ];
497         }
498         /* Calculate the correlations and energies needed in stage 3 */
499         silk_P_Ana_calc_corr_st3(  crosscorr_st3, input_frame_ptr, start_lag, sf_length, nb_subfr, complexity );
500         silk_P_Ana_calc_energy_st3( energies_st3, input_frame_ptr, start_lag, sf_length, nb_subfr, complexity );
501
502         lag_counter = 0;
503         silk_assert( lag == silk_SAT16( lag ) );
504         contour_bias_Q20 = silk_DIV32_16( SILK_FIX_CONST( PE_FLATCONTOUR_BIAS, 20 ), lag );
505
506         /* Setup cbk parameters acording to complexity setting and frame length */
507         if( nb_subfr == PE_MAX_NB_SUBFR ) {
508             nb_cbk_search   = (opus_int)silk_nb_cbk_searchs_stage3[ complexity ];
509             cbk_size        = PE_NB_CBKS_STAGE3_MAX;
510             Lag_CB_ptr      = &silk_CB_lags_stage3[ 0 ][ 0 ];
511         } else {
512             nb_cbk_search   = PE_NB_CBKS_STAGE3_10MS;
513             cbk_size        = PE_NB_CBKS_STAGE3_10MS;
514             Lag_CB_ptr      = &silk_CB_lags_stage3_10_ms[ 0 ][ 0 ];
515         }
516         for( d = start_lag; d <= end_lag; d++ ) {
517             for( j = 0; j < nb_cbk_search; j++ ) {
518                 cross_corr = 0;
519                 energy     = 0;
520                 for( k = 0; k < nb_subfr; k++ ) {
521                     silk_assert( PE_MAX_NB_SUBFR == 4 );
522                     energy     += silk_RSHIFT( energies_st3[  k ][ j ][ lag_counter ], 2 ); /* use mean, to avoid overflow */
523                     silk_assert( energy >= 0 );
524                     cross_corr += silk_RSHIFT( crosscorr_st3[ k ][ j ][ lag_counter ], 2 ); /* use mean, to avoid overflow */
525                 }
526                 if( cross_corr > 0 ) {
527                     /* Divide cross_corr / energy and get result in Q15 */
528                     lz = silk_CLZ32( cross_corr );
529                     /* Divide with result in Q13, cross_corr could be larger than energy */
530                     lshift = silk_LIMIT_32( lz - 1, 0, 13 );
531                     CCmax_new = silk_DIV32( silk_LSHIFT( cross_corr, lshift ), silk_RSHIFT( energy, 13 - lshift ) + 1 );
532                     CCmax_new = silk_SAT16( CCmax_new );
533                     CCmax_new = silk_SMULWB( cross_corr, CCmax_new );
534                     /* Saturate */
535                     if( CCmax_new > silk_RSHIFT( silk_int32_MAX, 3 ) ) {
536                         CCmax_new = silk_int32_MAX;
537                     } else {
538                         CCmax_new = silk_LSHIFT( CCmax_new, 3 );
539                     }
540                     /* Reduce depending on flatness of contour */
541                     diff = silk_int16_MAX - silk_RSHIFT( silk_MUL( contour_bias_Q20, j ), 5 ); /* Q20 -> Q15 */
542                     silk_assert( diff == silk_SAT16( diff ) );
543                     CCmax_new = silk_LSHIFT( silk_SMULWB( CCmax_new, diff ), 1 );
544                 } else {
545                     CCmax_new = 0;
546                 }
547
548                 if( CCmax_new > CCmax                                               &&
549                    ( d + silk_CB_lags_stage3[ 0 ][ j ] ) <= max_lag
550                    ) {
551                     CCmax   = CCmax_new;
552                     lag_new = d;
553                     CBimax  = j;
554                 }
555             }
556             lag_counter++;
557         }
558
559         for( k = 0; k < nb_subfr; k++ ) {
560             pitch_out[ k ] = lag_new + matrix_ptr( Lag_CB_ptr, k, CBimax, cbk_size );
561         }
562         *lagIndex = (opus_int16)( lag_new - min_lag);
563         *contourIndex = (opus_int8)CBimax;
564     } else {
565         /* Save Lags and correlation */
566         CCmax = silk_max( CCmax, 0 );
567         *LTPCorr_Q15 = (opus_int)silk_SQRT_APPROX( silk_LSHIFT( CCmax, 13 ) ); /* Output normalized correlation */
568         for( k = 0; k < nb_subfr; k++ ) {
569             pitch_out[ k ] = lag + matrix_ptr( Lag_CB_ptr, k, CBimax, cbk_size );
570         }
571         *lagIndex = (opus_int16)( lag - min_lag_8kHz );
572         *contourIndex = (opus_int8)CBimax;
573     }
574     silk_assert( *lagIndex >= 0 );
575     /* return as voiced */
576     return 0;
577 }
578
579 /*************************************************************************/
580 /* Calculates the correlations used in stage 3 search. In order to cover */
581 /* the whole lag codebook for all the searched offset lags (lag +- 2),   */
582 /*************************************************************************/
583 void silk_P_Ana_calc_corr_st3(
584     opus_int32        cross_corr_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ],/* (O) 3 DIM correlation array */
585     const opus_int16  frame[],                         /* I vector to correlate         */
586     opus_int          start_lag,                       /* I lag offset to search around */
587     opus_int          sf_length,                       /* I length of a 5 ms subframe   */
588     opus_int          nb_subfr,                        /* I number of subframes         */
589     opus_int          complexity                       /* I Complexity setting          */
590 )
591 {
592     const opus_int16 *target_ptr, *basis_ptr;
593     opus_int32 cross_corr;
594     opus_int   i, j, k, lag_counter, lag_low, lag_high;
595     opus_int   nb_cbk_search, delta, idx, cbk_size;
596     opus_int32 scratch_mem[ SCRATCH_SIZE ];
597     const opus_int8 *Lag_range_ptr, *Lag_CB_ptr;
598
599     silk_assert( complexity >= SILK_PE_MIN_COMPLEX );
600     silk_assert( complexity <= SILK_PE_MAX_COMPLEX );
601
602     if( nb_subfr == PE_MAX_NB_SUBFR ){
603         Lag_range_ptr = &silk_Lag_range_stage3[ complexity ][ 0 ][ 0 ];
604         Lag_CB_ptr    = &silk_CB_lags_stage3[ 0 ][ 0 ];
605         nb_cbk_search = silk_nb_cbk_searchs_stage3[ complexity ];
606         cbk_size      = PE_NB_CBKS_STAGE3_MAX;
607     } else {
608         silk_assert( nb_subfr == PE_MAX_NB_SUBFR >> 1);
609         Lag_range_ptr = &silk_Lag_range_stage3_10_ms[ 0 ][ 0 ];
610         Lag_CB_ptr    = &silk_CB_lags_stage3_10_ms[ 0 ][ 0 ];
611         nb_cbk_search = PE_NB_CBKS_STAGE3_10MS;
612         cbk_size      = PE_NB_CBKS_STAGE3_10MS;
613     }
614
615     target_ptr = &frame[ silk_LSHIFT( sf_length, 2 ) ]; /* Pointer to middle of frame */
616     for( k = 0; k < nb_subfr; k++ ) {
617         lag_counter = 0;
618
619         /* Calculate the correlations for each subframe */
620         lag_low  = matrix_ptr( Lag_range_ptr, k, 0, 2 );
621         lag_high = matrix_ptr( Lag_range_ptr, k, 1, 2 );
622         for( j = lag_low; j <= lag_high; j++ ) {
623             basis_ptr = target_ptr - ( start_lag + j );
624             cross_corr = silk_inner_prod_aligned( (opus_int16*)target_ptr, (opus_int16*)basis_ptr, sf_length );
625             silk_assert( lag_counter < SCRATCH_SIZE );
626             scratch_mem[ lag_counter ] = cross_corr;
627             lag_counter++;
628         }
629
630         delta = matrix_ptr( Lag_range_ptr, k, 0, 2 );
631         for( i = 0; i < nb_cbk_search; i++ ) {
632             /* Fill out the 3 dim array that stores the correlations for */
633             /* each code_book vector for each start lag */
634             idx = matrix_ptr( Lag_CB_ptr, k, i, cbk_size ) - delta;
635             for( j = 0; j < PE_NB_STAGE3_LAGS; j++ ) {
636                 silk_assert( idx + j < SCRATCH_SIZE );
637                 silk_assert( idx + j < lag_counter );
638                 cross_corr_st3[ k ][ i ][ j ] = scratch_mem[ idx + j ];
639             }
640         }
641         target_ptr += sf_length;
642     }
643 }
644
645 /********************************************************************/
646 /* Calculate the energies for first two subframes. The energies are */
647 /* calculated recursively.                                          */
648 /********************************************************************/
649 void silk_P_Ana_calc_energy_st3(
650     opus_int32        energies_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ],/* (O) 3 DIM energy array */
651     const opus_int16  frame[],                         /* I vector to calc energy in    */
652     opus_int          start_lag,                       /* I lag offset to search around */
653     opus_int          sf_length,                       /* I length of one 5 ms subframe */
654     opus_int          nb_subfr,                     /* I number of subframes         */
655     opus_int          complexity                       /* I Complexity setting          */
656 )
657 {
658     const opus_int16 *target_ptr, *basis_ptr;
659     opus_int32 energy;
660     opus_int   k, i, j, lag_counter;
661     opus_int   nb_cbk_search, delta, idx, cbk_size, lag_diff;
662     opus_int32 scratch_mem[ SCRATCH_SIZE ];
663     const opus_int8 *Lag_range_ptr, *Lag_CB_ptr;
664
665     silk_assert( complexity >= SILK_PE_MIN_COMPLEX );
666     silk_assert( complexity <= SILK_PE_MAX_COMPLEX );
667
668     if( nb_subfr == PE_MAX_NB_SUBFR ){
669         Lag_range_ptr = &silk_Lag_range_stage3[ complexity ][ 0 ][ 0 ];
670         Lag_CB_ptr    = &silk_CB_lags_stage3[ 0 ][ 0 ];
671         nb_cbk_search = silk_nb_cbk_searchs_stage3[ complexity ];
672         cbk_size      = PE_NB_CBKS_STAGE3_MAX;
673     } else {
674         silk_assert( nb_subfr == PE_MAX_NB_SUBFR >> 1);
675         Lag_range_ptr = &silk_Lag_range_stage3_10_ms[ 0 ][ 0 ];
676         Lag_CB_ptr    = &silk_CB_lags_stage3_10_ms[ 0 ][ 0 ];
677         nb_cbk_search = PE_NB_CBKS_STAGE3_10MS;
678         cbk_size      = PE_NB_CBKS_STAGE3_10MS;
679     }
680     target_ptr = &frame[ silk_LSHIFT( sf_length, 2 ) ];
681     for( k = 0; k < nb_subfr; k++ ) {
682         lag_counter = 0;
683
684         /* Calculate the energy for first lag */
685         basis_ptr = target_ptr - ( start_lag + matrix_ptr( Lag_range_ptr, k, 0, 2 ) );
686         energy = silk_inner_prod_aligned( basis_ptr, basis_ptr, sf_length );
687         silk_assert( energy >= 0 );
688         scratch_mem[ lag_counter ] = energy;
689         lag_counter++;
690
691         lag_diff = ( matrix_ptr( Lag_range_ptr, k, 1, 2 ) -  matrix_ptr( Lag_range_ptr, k, 0, 2 ) + 1 );
692         for( i = 1; i < lag_diff; i++ ) {
693             /* remove part outside new window */
694             energy -= silk_SMULBB( basis_ptr[ sf_length - i ], basis_ptr[ sf_length - i ] );
695             silk_assert( energy >= 0 );
696
697             /* add part that comes into window */
698             energy = silk_ADD_SAT32( energy, silk_SMULBB( basis_ptr[ -i ], basis_ptr[ -i ] ) );
699             silk_assert( energy >= 0 );
700             silk_assert( lag_counter < SCRATCH_SIZE );
701             scratch_mem[ lag_counter ] = energy;
702             lag_counter++;
703         }
704
705         delta = matrix_ptr( Lag_range_ptr, k, 0, 2 );
706         for( i = 0; i < nb_cbk_search; i++ ) {
707             /* Fill out the 3 dim array that stores the correlations for    */
708             /* each code_book vector for each start lag                     */
709             idx = matrix_ptr( Lag_CB_ptr, k, i, cbk_size ) - delta;
710             for( j = 0; j < PE_NB_STAGE3_LAGS; j++ ) {
711                 silk_assert( idx + j < SCRATCH_SIZE );
712                 silk_assert( idx + j < lag_counter );
713                 energies_st3[ k ][ i ][ j ] = scratch_mem[ idx + j ];
714                 silk_assert( energies_st3[ k ][ i ][ j ] >= 0 );
715             }
716         }
717         target_ptr += sf_length;
718     }
719 }
720
721 opus_int32 silk_P_Ana_find_scaling(
722     const opus_int16  *frame,
723     const opus_int    frame_length,
724     const opus_int    sum_sqr_len
725 )
726 {
727     opus_int32 nbits, x_max;
728
729     x_max = silk_int16_array_maxabs( frame, frame_length );
730
731     if( x_max < silk_int16_MAX ) {
732         /* Number of bits needed for the sum of the squares */
733         nbits = 32 - silk_CLZ32( silk_SMULBB( x_max, x_max ) );
734     } else {
735         /* Here we don't know if x_max should have been silk_int16_MAX + 1, so we expect the worst case */
736         nbits = 30;
737     }
738     nbits += 17 - silk_CLZ16( sum_sqr_len );
739
740     /* Without a guarantee of saturation, we need to keep the 31st bit free */
741     if( nbits < 31 ) {
742         return 0;
743     } else {
744         return( nbits - 30 );
745     }
746 }