SILK update
[opus.git] / src_SigProc_FIX / SKP_Silk_pitch_analysis_core.c
1 /***********************************************************************\r
2 Copyright (c) 2006-2010, Skype Limited. All rights reserved. \r
3 Redistribution and use in source and binary forms, with or without \r
4 modification, (subject to the limitations in the disclaimer below) \r
5 are permitted provided that the following conditions are met:\r
6 - Redistributions of source code must retain the above copyright notice,\r
7 this list of conditions and the following disclaimer.\r
8 - Redistributions in binary form must reproduce the above copyright \r
9 notice, this list of conditions and the following disclaimer in the \r
10 documentation and/or other materials provided with the distribution.\r
11 - Neither the name of Skype Limited, nor the names of specific \r
12 contributors, may be used to endorse or promote products derived from \r
13 this software without specific prior written permission.\r
14 NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED \r
15 BY THIS LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND \r
16 CONTRIBUTORS ''AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING,\r
17 BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND \r
18 FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE \r
19 COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, \r
20 INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT\r
21 NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF \r
22 USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON \r
23 ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT \r
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE \r
25 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\r
26 ***********************************************************************/\r
27 \r
28 /***********************************************************\r
29 * Pitch analyser function\r
30 ********************************************************** */\r
31 #include "SKP_Silk_SigProc_FIX.h"\r
32 #include "SKP_Silk_pitch_est_defines.h"\r
33 #include "SKP_debug.h"\r
34 \r
35 #define SCRATCH_SIZE    22\r
36 \r
37 /************************************************************/\r
38 /* Internally used functions                                */\r
39 /************************************************************/\r
40 void SKP_FIX_P_Ana_calc_corr_st3(\r
41     SKP_int32        cross_corr_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ],/* (O) 3 DIM correlation array */\r
42     const SKP_int16  signal[],                        /* I vector to correlate         */\r
43     SKP_int          start_lag,                       /* I lag offset to search around */\r
44     SKP_int          sf_length,                       /* I length of a 5 ms subframe   */\r
45     SKP_int          nb_subfr,                        /* I number of subframes         */\r
46     SKP_int          complexity                       /* I Complexity setting          */\r
47 );\r
48 \r
49 void SKP_FIX_P_Ana_calc_energy_st3(\r
50     SKP_int32        energies_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ],/* (O) 3 DIM energy array */\r
51     const SKP_int16  signal[],                        /* I vector to calc energy in    */\r
52     SKP_int          start_lag,                       /* I lag offset to search around */\r
53     SKP_int          sf_length,                       /* I length of one 5 ms subframe */\r
54     SKP_int          nb_subfr,                        /* I number of subframes         */\r
55     SKP_int          complexity                       /* I Complexity setting          */\r
56 );\r
57 \r
58 SKP_int32 SKP_FIX_P_Ana_find_scaling(\r
59     const SKP_int16  *signal,\r
60     const SKP_int    signal_length, \r
61     const SKP_int    sum_sqr_len\r
62 );\r
63 \r
64 /*************************************************************/\r
65 /*      FIXED POINT CORE PITCH ANALYSIS FUNCTION             */\r
66 /*************************************************************/\r
67 SKP_int SKP_Silk_pitch_analysis_core(    /* O    Voicing estimate: 0 voiced, 1 unvoiced                        */\r
68     const SKP_int16  *signal,            /* I    Signal of length PE_FRAME_LENGTH_MS*Fs_kHz           */\r
69     SKP_int          *pitch_out,         /* O    4 pitch lag values                                          */\r
70     SKP_int          *lagIndex,          /* O    Lag Index                                                   */\r
71     SKP_int          *contourIndex,      /* O    Pitch contour Index                                         */\r
72     SKP_int          *LTPCorr_Q15,       /* I/O  Normalized correlation; input: value from previous frame    */\r
73     SKP_int          prevLag,            /* I    Last lag of previous frame; set to zero is unvoiced         */\r
74     const SKP_int32  search_thres1_Q16,  /* I    First stage threshold for lag candidates 0 - 1              */\r
75     const SKP_int    search_thres2_Q15,  /* I    Final threshold for lag candidates 0 - 1                    */\r
76     const SKP_int    Fs_kHz,             /* I    Sample frequency (kHz)                                      */\r
77     const SKP_int    complexity,         /* I    Complexity setting, 0-2, where 2 is highest                 */\r
78     const SKP_int    nb_subfr            /* I    number of 5 ms subframes                                    */\r
79 )\r
80 {\r
81     SKP_int16 signal_8kHz[ PE_MAX_FRAME_LENGTH_ST_2 ];\r
82     SKP_int16 signal_4kHz[ PE_MAX_FRAME_LENGTH_ST_1 ];\r
83     SKP_int32 scratch_mem[ 3 * PE_MAX_FRAME_LENGTH ];\r
84     SKP_int16 *input_signal_ptr;\r
85     SKP_int32 filt_state[ PE_MAX_DECIMATE_STATE_LENGTH ];\r
86     SKP_int   i, k, d, j;\r
87     SKP_int16 C[ PE_MAX_NB_SUBFR ][ ( PE_MAX_LAG >> 1 ) + 5 ];\r
88     const SKP_int16 *target_ptr, *basis_ptr;\r
89     SKP_int32 cross_corr, normalizer, energy, shift, energy_basis, energy_target;\r
90     SKP_int   d_srch[ PE_D_SRCH_LENGTH ], Cmax, length_d_srch, length_d_comp;\r
91     SKP_int16 d_comp[ ( PE_MAX_LAG >> 1 ) + 5 ];\r
92     SKP_int32 sum, threshold, temp32, lag_counter;\r
93     SKP_int   CBimax, CBimax_new, CBimax_old, lag, start_lag, end_lag, lag_new;\r
94     SKP_int32 CC[ PE_NB_CBKS_STAGE2_EXT ], CCmax, CCmax_b, CCmax_new_b, CCmax_new;\r
95     SKP_int32 energies_st3[  PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ];\r
96     SKP_int32 crosscorr_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ];\r
97     SKP_int   frame_length, frame_length_8kHz, frame_length_4kHz, max_sum_sq_length;\r
98     SKP_int   sf_length, sf_length_8kHz, sf_length_4kHz;\r
99     SKP_int   min_lag, min_lag_8kHz, min_lag_4kHz;\r
100     SKP_int   max_lag, max_lag_8kHz, max_lag_4kHz;\r
101     SKP_int32 contour_bias, diff, lz, lshift;\r
102     SKP_int   cbk_offset, nb_cbk_search, cbk_size;\r
103     SKP_int32 delta_lag_log2_sqr_Q7, lag_log2_Q7, prevLag_log2_Q7, prev_lag_bias_Q15, corr_thres_Q15;\r
104     const SKP_int8 *Lag_CB_ptr;\r
105     /* Check for valid sampling frequency */\r
106     SKP_assert( Fs_kHz == 8 || Fs_kHz == 12 || Fs_kHz == 16 || Fs_kHz == 24 );\r
107 \r
108     /* Check for valid complexity setting */\r
109     SKP_assert( complexity >= SKP_Silk_PE_MIN_COMPLEX );\r
110     SKP_assert( complexity <= SKP_Silk_PE_MAX_COMPLEX );\r
111 \r
112     SKP_assert( search_thres1_Q16 >= 0 && search_thres1_Q16 <= (1<<16) );\r
113     SKP_assert( search_thres2_Q15 >= 0 && search_thres2_Q15 <= (1<<15) );\r
114 \r
115     /* Setup frame lengths max / min lag for the sampling frequency */\r
116     frame_length      = ( PE_LTP_MEM_LENGTH_MS + nb_subfr * PE_SUBFR_LENGTH_MS ) * Fs_kHz;\r
117     frame_length_4kHz = ( PE_LTP_MEM_LENGTH_MS + nb_subfr * PE_SUBFR_LENGTH_MS ) * 4;\r
118     frame_length_8kHz = ( PE_LTP_MEM_LENGTH_MS + nb_subfr * PE_SUBFR_LENGTH_MS ) * 8;\r
119     sf_length         = PE_SUBFR_LENGTH_MS * Fs_kHz;\r
120     sf_length_4kHz    = PE_SUBFR_LENGTH_MS * 4;\r
121     sf_length_8kHz    = PE_SUBFR_LENGTH_MS * 8;\r
122     min_lag           = PE_MIN_LAG_MS * Fs_kHz;\r
123     min_lag_4kHz      = PE_MIN_LAG_MS * 4;\r
124     min_lag_8kHz      = PE_MIN_LAG_MS * 8;\r
125     max_lag           = PE_MAX_LAG_MS * Fs_kHz;\r
126     max_lag_4kHz      = PE_MAX_LAG_MS * 4;\r
127     max_lag_8kHz      = PE_MAX_LAG_MS * 8;\r
128 \r
129     SKP_memset( C, 0, sizeof( SKP_int16 ) * nb_subfr * ( ( PE_MAX_LAG >> 1 ) + 5) );\r
130     \r
131     /* Resample from input sampled at Fs_kHz to 8 kHz */\r
132     if( Fs_kHz == 16 ) {\r
133         SKP_memset( filt_state, 0, 2 * sizeof( SKP_int32 ) );\r
134         SKP_Silk_resampler_down2( filt_state, signal_8kHz, signal, frame_length );\r
135     } else if ( Fs_kHz == 12 ) {\r
136         SKP_int32 R23[ 6 ];\r
137         SKP_memset( R23, 0, 6 * sizeof( SKP_int32 ) );\r
138         SKP_Silk_resampler_down2_3( R23, signal_8kHz, signal, frame_length );\r
139     } else if( Fs_kHz == 24 ) {\r
140         SKP_int32 filt_state_fix[ 8 ];\r
141         SKP_memset( filt_state_fix, 0, 8 * sizeof(SKP_int32) );\r
142         SKP_Silk_resampler_down3( filt_state_fix, signal_8kHz, signal, frame_length );\r
143     } else {\r
144         SKP_assert( Fs_kHz == 8 );\r
145         SKP_memcpy( signal_8kHz, signal, frame_length_8kHz * sizeof(SKP_int16) );\r
146     }\r
147     /* Decimate again to 4 kHz */\r
148     SKP_memset( filt_state, 0, 2 * sizeof( SKP_int32 ) );/* Set state to zero */\r
149     SKP_Silk_resampler_down2( filt_state, signal_4kHz, signal_8kHz, frame_length_8kHz );\r
150 \r
151     /* Low-pass filter */\r
152     for( i = frame_length_4kHz - 1; i > 0; i-- ) {\r
153         signal_4kHz[ i ] = SKP_ADD_SAT16( signal_4kHz[ i ], signal_4kHz[ i - 1 ] );\r
154     }\r
155 \r
156     /*******************************************************************************\r
157     ** Scale 4 kHz signal down to prevent correlations measures from overflowing\r
158     ** find scaling as max scaling for each 8kHz(?) subframe\r
159     *******************************************************************************/\r
160     \r
161     /* Inner product is calculated with different lengths, so scale for the worst case */\r
162     max_sum_sq_length = SKP_max_32( sf_length_8kHz, SKP_LSHIFT( sf_length_4kHz, 2 ) );\r
163     shift = SKP_FIX_P_Ana_find_scaling( signal_4kHz, frame_length_4kHz, max_sum_sq_length );\r
164     if( shift > 0 ) {\r
165         for( i = 0; i < frame_length_4kHz; i++ ) {\r
166             signal_4kHz[ i ] = SKP_RSHIFT( signal_4kHz[ i ], shift );\r
167         }\r
168     }\r
169 \r
170     /******************************************************************************\r
171     * FIRST STAGE, operating in 4 khz\r
172     ******************************************************************************/\r
173     target_ptr = &signal_4kHz[ SKP_LSHIFT( sf_length_4kHz, 2 ) ];\r
174     for( k = 0; k < nb_subfr >> 1; k++ ) {\r
175         /* Check that we are within range of the array */\r
176         SKP_assert( target_ptr >= signal_4kHz );\r
177         SKP_assert( target_ptr + sf_length_8kHz <= signal_4kHz + frame_length_4kHz );\r
178 \r
179         basis_ptr = target_ptr - min_lag_4kHz;\r
180 \r
181         /* Check that we are within range of the array */\r
182         SKP_assert( basis_ptr >= signal_4kHz );\r
183         SKP_assert( basis_ptr + sf_length_8kHz <= signal_4kHz + frame_length_4kHz );\r
184 \r
185         normalizer = 0;\r
186         cross_corr = 0;\r
187         /* Calculate first vector products before loop */\r
188         cross_corr = SKP_Silk_inner_prod_aligned( target_ptr, basis_ptr, sf_length_8kHz );\r
189         normalizer = SKP_Silk_inner_prod_aligned( basis_ptr,  basis_ptr, sf_length_8kHz );\r
190         normalizer = SKP_ADD_SAT32( normalizer, SKP_SMULBB( sf_length_8kHz, 4000 ) );\r
191 \r
192         temp32 = SKP_DIV32( cross_corr, SKP_Silk_SQRT_APPROX( normalizer ) + 1 );\r
193         C[ k ][ min_lag_4kHz ] = (SKP_int16)SKP_SAT16( temp32 );        /* Q0 */\r
194 \r
195         /* From now on normalizer is computed recursively */\r
196         for( d = min_lag_4kHz + 1; d <= max_lag_4kHz; d++ ) {\r
197             basis_ptr--;\r
198 \r
199             /* Check that we are within range of the array */\r
200             SKP_assert( basis_ptr >= signal_4kHz );\r
201             SKP_assert( basis_ptr + sf_length_8kHz <= signal_4kHz + frame_length_4kHz );\r
202 \r
203             cross_corr = SKP_Silk_inner_prod_aligned( target_ptr, basis_ptr, sf_length_8kHz );\r
204 \r
205             /* Add contribution of new sample and remove contribution from oldest sample */\r
206             normalizer +=\r
207                 SKP_SMULBB( basis_ptr[ 0 ], basis_ptr[ 0 ] ) - \r
208                 SKP_SMULBB( basis_ptr[ sf_length_8kHz ], basis_ptr[ sf_length_8kHz ] ); \r
209     \r
210             temp32 = SKP_DIV32( cross_corr, SKP_Silk_SQRT_APPROX( normalizer ) + 1 );\r
211             C[ k ][ d ] = (SKP_int16)SKP_SAT16( temp32 );                        /* Q0 */\r
212         }\r
213         /* Update target pointer */\r
214         target_ptr += sf_length_8kHz;\r
215     }\r
216 \r
217     /* Combine two subframes into single correlation measure and apply short-lag bias */\r
218     if( nb_subfr == PE_MAX_NB_SUBFR ) {\r
219         for( i = max_lag_4kHz; i >= min_lag_4kHz; i-- ) {\r
220             sum = (SKP_int32)C[ 0 ][ i ] + (SKP_int32)C[ 1 ][ i ];                /* Q0 */\r
221             SKP_assert( SKP_RSHIFT( sum, 1 ) == SKP_SAT16( SKP_RSHIFT( sum, 1 ) ) );\r
222             sum = SKP_RSHIFT( sum, 1 );                                           /* Q-1 */\r
223             SKP_assert( SKP_LSHIFT( (SKP_int32)-i, 4 ) == SKP_SAT16( SKP_LSHIFT( (SKP_int32)-i, 4 ) ) );\r
224             sum = SKP_SMLAWB( sum, sum, SKP_LSHIFT( -i, 4 ) );                    /* Q-1 */\r
225             SKP_assert( sum == SKP_SAT16( sum ) );\r
226             C[ 0 ][ i ] = (SKP_int16)sum;                                         /* Q-1 */\r
227         }\r
228     } else {\r
229         /* Only short-lag bias */\r
230         for( i = max_lag_4kHz; i >= min_lag_4kHz; i-- ) {\r
231             sum = (SKP_int32)C[ 0 ][ i ];\r
232             sum = SKP_SMLAWB( sum, sum, SKP_LSHIFT( -i, 4 ) );                    /* Q-1 */\r
233             C[ 0 ][ i ] = (SKP_int16)sum;                                         /* Q-1 */\r
234         }\r
235     }\r
236 \r
237     /* Sort */\r
238     length_d_srch = SKP_ADD_LSHIFT32( 4, complexity, 1 );\r
239     SKP_assert( 3 * length_d_srch <= PE_D_SRCH_LENGTH );\r
240     SKP_Silk_insertion_sort_decreasing_int16( &C[ 0 ][ min_lag_4kHz ], d_srch, max_lag_4kHz - min_lag_4kHz + 1, length_d_srch );\r
241 \r
242     /* Escape if correlation is very low already here */\r
243     target_ptr = &signal_4kHz[ SKP_SMULBB( sf_length_4kHz, nb_subfr ) ];\r
244     energy = SKP_Silk_inner_prod_aligned( target_ptr, target_ptr, SKP_LSHIFT( sf_length_4kHz, 2 ) );\r
245     energy = SKP_ADD_SAT32( energy, 1000 );                                  /* Q0 */\r
246     Cmax = (SKP_int)C[ 0 ][ min_lag_4kHz ];                                  /* Q-1 */\r
247     threshold = SKP_SMULBB( Cmax, Cmax );                                    /* Q-2 */\r
248 \r
249     /* Compare in Q-2 domain */\r
250     if( SKP_RSHIFT( energy, 4 + 2 ) > threshold ) {                            \r
251         SKP_memset( pitch_out, 0, nb_subfr * sizeof( SKP_int ) );\r
252         *LTPCorr_Q15  = 0;\r
253         *lagIndex     = 0;\r
254         *contourIndex = 0;\r
255         return 1;\r
256     }\r
257 \r
258     threshold = SKP_SMULWB( search_thres1_Q16, Cmax );\r
259     for( i = 0; i < length_d_srch; i++ ) {\r
260         /* Convert to 8 kHz indices for the sorted correlation that exceeds the threshold */\r
261         if( C[ 0 ][ min_lag_4kHz + i ] > threshold ) {\r
262             d_srch[ i ] = SKP_LSHIFT( d_srch[ i ] + min_lag_4kHz, 1 );\r
263         } else {\r
264             length_d_srch = i;\r
265             break;\r
266         }\r
267     }\r
268     SKP_assert( length_d_srch > 0 );\r
269 \r
270     for( i = min_lag_8kHz - 5; i < max_lag_8kHz + 5; i++ ) {\r
271         d_comp[ i ] = 0;\r
272     }\r
273     for( i = 0; i < length_d_srch; i++ ) {\r
274         d_comp[ d_srch[ i ] ] = 1;\r
275     }\r
276 \r
277     /* Convolution */\r
278     for( i = max_lag_8kHz + 3; i >= min_lag_8kHz; i-- ) {\r
279         d_comp[ i ] += d_comp[ i - 1 ] + d_comp[ i - 2 ];\r
280     }\r
281 \r
282     length_d_srch = 0;\r
283     for( i = min_lag_8kHz; i < max_lag_8kHz + 1; i++ ) {    \r
284         if( d_comp[ i + 1 ] > 0 ) {\r
285             d_srch[ length_d_srch ] = i;\r
286             length_d_srch++;\r
287         }\r
288     }\r
289 \r
290     /* Convolution */\r
291     for( i = max_lag_8kHz + 3; i >= min_lag_8kHz; i-- ) {\r
292         d_comp[ i ] += d_comp[ i - 1 ] + d_comp[ i - 2 ] + d_comp[ i - 3 ];\r
293     }\r
294 \r
295     length_d_comp = 0;\r
296     for( i = min_lag_8kHz; i < max_lag_8kHz + 4; i++ ) {    \r
297         if( d_comp[ i ] > 0 ) {\r
298             d_comp[ length_d_comp ] = i - 2;\r
299             length_d_comp++;\r
300         }\r
301     }\r
302 \r
303     /**********************************************************************************\r
304     ** SECOND STAGE, operating at 8 kHz, on lag sections with high correlation\r
305     *************************************************************************************/\r
306 \r
307     /******************************************************************************\r
308     ** Scale signal down to avoid correlations measures from overflowing\r
309     *******************************************************************************/\r
310     /* find scaling as max scaling for each subframe */\r
311     shift = SKP_FIX_P_Ana_find_scaling( signal_8kHz, frame_length_8kHz, sf_length_8kHz );\r
312     if( shift > 0 ) {\r
313         for( i = 0; i < frame_length_8kHz; i++ ) {\r
314             signal_8kHz[ i ] = SKP_RSHIFT( signal_8kHz[ i ], shift );\r
315         }\r
316     }\r
317 \r
318     /********************************************************************************* \r
319     * Find energy of each subframe projected onto its history, for a range of delays\r
320     *********************************************************************************/\r
321     SKP_memset( C, 0, PE_MAX_NB_SUBFR * ( ( PE_MAX_LAG >> 1 ) + 5 ) * sizeof( SKP_int16 ) );\r
322     \r
323     target_ptr = &signal_8kHz[ PE_LTP_MEM_LENGTH_MS * 8 ];\r
324     for( k = 0; k < nb_subfr; k++ ) {\r
325 \r
326         /* Check that we are within range of the array */\r
327         SKP_assert( target_ptr >= signal_8kHz );\r
328         SKP_assert( target_ptr + sf_length_8kHz <= signal_8kHz + frame_length_8kHz );\r
329 \r
330         energy_target = SKP_Silk_inner_prod_aligned( target_ptr, target_ptr, sf_length_8kHz );\r
331         // ToDo: Calculate 1 / energy_target here and save one division inside next for loop\r
332         for( j = 0; j < length_d_comp; j++ ) {\r
333             d = d_comp[ j ];\r
334             basis_ptr = target_ptr - d;\r
335 \r
336             /* Check that we are within range of the array */\r
337             SKP_assert( basis_ptr >= signal_8kHz );\r
338             SKP_assert( basis_ptr + sf_length_8kHz <= signal_8kHz + frame_length_8kHz );\r
339         \r
340             cross_corr   = SKP_Silk_inner_prod_aligned( target_ptr, basis_ptr, sf_length_8kHz );\r
341             energy_basis = SKP_Silk_inner_prod_aligned( basis_ptr,  basis_ptr, sf_length_8kHz );\r
342             if( cross_corr > 0 ) {\r
343                 energy = SKP_max( energy_target, energy_basis ); /* Find max to make sure first division < 1.0 */\r
344                 lz = SKP_Silk_CLZ32( cross_corr );\r
345                 lshift = SKP_LIMIT_32( lz - 1, 0, 15 );\r
346                 temp32 = SKP_DIV32( SKP_LSHIFT( cross_corr, lshift ), SKP_RSHIFT( energy, 15 - lshift ) + 1 ); /* Q15 */\r
347                 SKP_assert( temp32 == SKP_SAT16( temp32 ) );\r
348                 temp32 = SKP_SMULWB( cross_corr, temp32 ); /* Q(-1), cc * ( cc / max(b, t) ) */\r
349                 temp32 = SKP_ADD_SAT32( temp32, temp32 );  /* Q(0) */\r
350                 lz = SKP_Silk_CLZ32( temp32 );\r
351                 lshift = SKP_LIMIT_32( lz - 1, 0, 15 );\r
352                 energy = SKP_min( energy_target, energy_basis );\r
353                 C[ k ][ d ] = SKP_DIV32( SKP_LSHIFT( temp32, lshift ), SKP_RSHIFT( energy, 15 - lshift ) + 1 ); // Q15\r
354             } else {\r
355                 C[ k ][ d ] = 0;\r
356             }\r
357         }\r
358         target_ptr += sf_length_8kHz;\r
359     }\r
360 \r
361     /* search over lag range and lags codebook */\r
362     /* scale factor for lag codebook, as a function of center lag */\r
363 \r
364     CCmax   = SKP_int32_MIN;\r
365     CCmax_b = SKP_int32_MIN;\r
366 \r
367     CBimax = 0; /* To avoid returning undefined lag values */\r
368     lag = -1;   /* To check if lag with strong enough correlation has been found */\r
369 \r
370     if( prevLag > 0 ) {\r
371         if( Fs_kHz == 12 ) {\r
372             prevLag = SKP_DIV32_16( SKP_LSHIFT( prevLag, 1 ), 3 );\r
373         } else if( Fs_kHz == 16 ) {\r
374             prevLag = SKP_RSHIFT( prevLag, 1 );\r
375         } else if( Fs_kHz == 24 ) {\r
376             prevLag = SKP_DIV32_16( prevLag, 3 );\r
377         }\r
378         prevLag_log2_Q7 = SKP_Silk_lin2log( (SKP_int32)prevLag );\r
379     } else {\r
380         prevLag_log2_Q7 = 0;\r
381     }\r
382     SKP_assert( search_thres2_Q15 == SKP_SAT16( search_thres2_Q15 ) );\r
383     /* Setup stage 2 codebook based on number of subframes */\r
384     if( nb_subfr == PE_MAX_NB_SUBFR ) {\r
385         cbk_size   = PE_NB_CBKS_STAGE2_EXT;\r
386         Lag_CB_ptr = &SKP_Silk_CB_lags_stage2[ 0 ][ 0 ];\r
387         if( Fs_kHz == 8 && complexity > SKP_Silk_PE_MIN_COMPLEX ) {\r
388             /* If input is 8 khz use a larger codebook here because it is last stage */\r
389             nb_cbk_search = PE_NB_CBKS_STAGE2_EXT;\r
390         } else {\r
391             nb_cbk_search = PE_NB_CBKS_STAGE2;\r
392         }\r
393         corr_thres_Q15 = SKP_RSHIFT( SKP_SMULBB( search_thres2_Q15, search_thres2_Q15 ), 13 );\r
394     }else{\r
395         cbk_size       = PE_NB_CBKS_STAGE2_10MS;\r
396         Lag_CB_ptr     = &SKP_Silk_CB_lags_stage2_10_ms[ 0 ][ 0 ];\r
397         nb_cbk_search  = PE_NB_CBKS_STAGE2_10MS;\r
398         corr_thres_Q15 = SKP_RSHIFT( SKP_SMULBB( search_thres2_Q15, search_thres2_Q15 ), 14 );\r
399     }\r
400 \r
401     for( k = 0; k < length_d_srch; k++ ) {\r
402         d = d_srch[ k ];\r
403         for( j = 0; j < nb_cbk_search; j++ ) {\r
404             CC[ j ] = 0;\r
405             for( i = 0; i < nb_subfr; i++ ) {\r
406                 /* Try all codebooks */\r
407                 CC[ j ] = CC[ j ] + (SKP_int32)C[ i ][ d + matrix_ptr( Lag_CB_ptr, i, j, cbk_size )];\r
408             }\r
409         }\r
410         /* Find best codebook */\r
411         CCmax_new = SKP_int32_MIN;\r
412         CBimax_new = 0;\r
413         for( i = 0; i < nb_cbk_search; i++ ) {\r
414             if( CC[ i ] > CCmax_new ) {\r
415                 CCmax_new = CC[ i ];\r
416                 CBimax_new = i;\r
417             }\r
418         }\r
419 \r
420         /* Bias towards shorter lags */\r
421         lag_log2_Q7 = SKP_Silk_lin2log( (SKP_int32)d ); /* Q7 */\r
422         SKP_assert( lag_log2_Q7 == SKP_SAT16( lag_log2_Q7 ) );\r
423         SKP_assert( nb_subfr * PE_SHORTLAG_BIAS_Q15 == SKP_SAT16( nb_subfr * PE_SHORTLAG_BIAS_Q15 ) );\r
424         CCmax_new_b = CCmax_new - SKP_RSHIFT( SKP_SMULBB( nb_subfr * PE_SHORTLAG_BIAS_Q15, lag_log2_Q7 ), 7 ); /* Q15 */\r
425 \r
426         /* Bias towards previous lag */\r
427         SKP_assert( nb_subfr * PE_PREVLAG_BIAS_Q15 == SKP_SAT16( nb_subfr * PE_PREVLAG_BIAS_Q15 ) );\r
428         if( prevLag > 0 ) {\r
429             delta_lag_log2_sqr_Q7 = lag_log2_Q7 - prevLag_log2_Q7;\r
430             SKP_assert( delta_lag_log2_sqr_Q7 == SKP_SAT16( delta_lag_log2_sqr_Q7 ) );\r
431             delta_lag_log2_sqr_Q7 = SKP_RSHIFT( SKP_SMULBB( delta_lag_log2_sqr_Q7, delta_lag_log2_sqr_Q7 ), 7 );\r
432             prev_lag_bias_Q15 = SKP_RSHIFT( SKP_SMULBB( nb_subfr * PE_PREVLAG_BIAS_Q15, ( *LTPCorr_Q15 ) ), 15 ); /* Q15 */\r
433             prev_lag_bias_Q15 = SKP_DIV32( SKP_MUL( prev_lag_bias_Q15, delta_lag_log2_sqr_Q7 ), delta_lag_log2_sqr_Q7 + ( 1 << 6 ) );\r
434             CCmax_new_b -= prev_lag_bias_Q15; /* Q15 */\r
435         }\r
436 \r
437         if ( CCmax_new_b > CCmax_b                                          && /* Find maximum biased correlation                  */\r
438              CCmax_new > corr_thres_Q15                                     && /* Correlation needs to be high enough to be voiced */\r
439              SKP_Silk_CB_lags_stage2[ 0 ][ CBimax_new ] <= min_lag_8kHz        /* Lag must be in range                             */\r
440             ) {\r
441             CCmax_b = CCmax_new_b;\r
442             CCmax   = CCmax_new;\r
443             lag     = d;\r
444             CBimax  = CBimax_new;\r
445         }\r
446     }\r
447 \r
448     if( lag == -1 ) {\r
449         /* No suitable candidate found */\r
450         SKP_memset( pitch_out, 0, nb_subfr * sizeof( SKP_int ) );\r
451         *LTPCorr_Q15  = 0;\r
452         *lagIndex     = 0;\r
453         *contourIndex = 0;\r
454         return 1;\r
455     }\r
456 \r
457     if( Fs_kHz > 8 ) {\r
458 \r
459         /******************************************************************************\r
460         ** Scale input signal down to avoid correlations measures from overflowing\r
461         *******************************************************************************/\r
462         /* find scaling as max scaling for each subframe */\r
463         shift = SKP_FIX_P_Ana_find_scaling( signal, frame_length, sf_length );\r
464         if( shift > 0 ) {\r
465             /* Move signal to scratch mem because the input signal should be unchanged */\r
466             /* Reuse the 32 bit scratch mem vector, use a 16 bit pointer from now */\r
467             input_signal_ptr = (SKP_int16*)scratch_mem;\r
468             for( i = 0; i < frame_length; i++ ) {\r
469                 input_signal_ptr[ i ] = SKP_RSHIFT( signal[ i ], shift );\r
470             }\r
471         } else {\r
472             input_signal_ptr = (SKP_int16*)signal;\r
473         }\r
474         /*********************************************************************************/\r
475 \r
476         /* Search in original signal */\r
477                     \r
478         CBimax_old = CBimax;\r
479         /* Compensate for decimation */\r
480         SKP_assert( lag == SKP_SAT16( lag ) );\r
481         if( Fs_kHz == 12 ) {\r
482             lag = SKP_RSHIFT( SKP_SMULBB( lag, 3 ), 1 );\r
483         } else if( Fs_kHz == 16 ) {\r
484             lag = SKP_LSHIFT( lag, 1 );\r
485         } else {\r
486             lag = SKP_SMULBB( lag, 3 );\r
487         }\r
488 \r
489         lag = SKP_LIMIT_int( lag, min_lag, max_lag );\r
490         start_lag = SKP_max_int( lag - 2, min_lag );\r
491         end_lag   = SKP_min_int( lag + 2, max_lag );\r
492         lag_new   = lag;                                    /* to avoid undefined lag */\r
493         CBimax    = 0;                                        /* to avoid undefined lag */\r
494         SKP_assert( SKP_LSHIFT( CCmax, 13 ) >= 0 ); \r
495         *LTPCorr_Q15 = (SKP_int)SKP_Silk_SQRT_APPROX( SKP_LSHIFT( CCmax, 13 ) ); /* Output normalized correlation */\r
496 \r
497         CCmax = SKP_int32_MIN;\r
498         /* pitch lags according to second stage */\r
499         for( k = 0; k < nb_subfr; k++ ) {\r
500             pitch_out[ k ] = lag + 2 * SKP_Silk_CB_lags_stage2[ k ][ CBimax_old ];\r
501         }\r
502         /* Calculate the correlations and energies needed in stage 3 */\r
503         SKP_FIX_P_Ana_calc_corr_st3(  crosscorr_st3, input_signal_ptr, start_lag, sf_length, nb_subfr, complexity );\r
504         SKP_FIX_P_Ana_calc_energy_st3( energies_st3, input_signal_ptr, start_lag, sf_length, nb_subfr, complexity );\r
505 \r
506         lag_counter = 0;\r
507         SKP_assert( lag == SKP_SAT16( lag ) );\r
508         contour_bias = SKP_DIV32_16( PE_FLATCONTOUR_BIAS_Q20, lag );\r
509 \r
510         /* Setup cbk parameters acording to complexity setting and frame length */\r
511         if( nb_subfr == PE_MAX_NB_SUBFR ) {\r
512             nb_cbk_search   = (SKP_int)SKP_Silk_nb_cbk_searchs_stage3[   complexity ];\r
513             cbk_size        = PE_NB_CBKS_STAGE3_MAX;\r
514             cbk_offset      = (SKP_int)SKP_Silk_cbk_offsets_stage3[ complexity ];\r
515             Lag_CB_ptr      = &SKP_Silk_CB_lags_stage3[ 0 ][ 0 ];\r
516         } else {\r
517             nb_cbk_search   = PE_NB_CBKS_STAGE3_10MS;\r
518             cbk_size        = PE_NB_CBKS_STAGE3_10MS;\r
519             cbk_offset      = 0;\r
520             Lag_CB_ptr      = &SKP_Silk_CB_lags_stage3_10_ms[ 0 ][ 0 ];\r
521         }\r
522         for( d = start_lag; d <= end_lag; d++ ) {\r
523             for( j = cbk_offset; j < ( cbk_offset + nb_cbk_search ); j++ ) {\r
524                 cross_corr = 0;\r
525                 energy     = 0;\r
526                 for( k = 0; k < nb_subfr; k++ ) {\r
527                     SKP_assert( PE_MAX_NB_SUBFR == 4 );\r
528                     energy     += SKP_RSHIFT( energies_st3[  k ][ j ][ lag_counter ], 2 ); /* use mean, to avoid overflow */\r
529                     SKP_assert( energy >= 0 );\r
530                     cross_corr += SKP_RSHIFT( crosscorr_st3[ k ][ j ][ lag_counter ], 2 ); /* use mean, to avoid overflow */\r
531                 }\r
532                 if( cross_corr > 0 ) {\r
533                     /* Divide cross_corr / energy and get result in Q15 */\r
534                     lz = SKP_Silk_CLZ32( cross_corr );\r
535                     /* Divide with result in Q13, cross_corr could be larger than energy */\r
536                     lshift = SKP_LIMIT_32( lz - 1, 0, 13 );\r
537                     CCmax_new = SKP_DIV32( SKP_LSHIFT( cross_corr, lshift ), SKP_RSHIFT( energy, 13 - lshift ) + 1 );\r
538                     CCmax_new = SKP_SAT16( CCmax_new );\r
539                     CCmax_new = SKP_SMULWB( cross_corr, CCmax_new );\r
540                     /* Saturate */\r
541                     if( CCmax_new > SKP_RSHIFT( SKP_int32_MAX, 3 ) ) {\r
542                         CCmax_new = SKP_int32_MAX;\r
543                     } else {\r
544                         CCmax_new = SKP_LSHIFT( CCmax_new, 3 );\r
545                     }\r
546                     /* Reduce depending on flatness of contour */\r
547                     diff = j - SKP_RSHIFT( cbk_size, 1 );\r
548                     diff = SKP_MUL( diff, diff );\r
549                     diff = SKP_int16_MAX - SKP_RSHIFT( SKP_MUL( contour_bias, diff ), 5 ); /* Q20 -> Q15 */\r
550                     SKP_assert( diff == SKP_SAT16( diff ) );\r
551                     CCmax_new = SKP_LSHIFT( SKP_SMULWB( CCmax_new, diff ), 1 );\r
552                 } else {\r
553                     CCmax_new = 0;\r
554                 }\r
555 \r
556                 if( CCmax_new > CCmax                                               && \r
557                    ( d + (SKP_int)SKP_Silk_CB_lags_stage3[ 0 ][ j ] ) <= max_lag  \r
558                    ) {\r
559                     CCmax   = CCmax_new;\r
560                     lag_new = d;\r
561                     CBimax  = j;\r
562                 }\r
563             }\r
564             lag_counter++;\r
565         }\r
566 \r
567         for( k = 0; k < nb_subfr; k++ ) {\r
568             pitch_out[ k ] = lag_new + matrix_ptr( Lag_CB_ptr, k, CBimax, cbk_size );\r
569         }\r
570         *lagIndex = lag_new - min_lag;\r
571         *contourIndex = CBimax;\r
572     } else {\r
573         /* Save Lags and correlation */\r
574         CCmax = SKP_max( CCmax, 0 );\r
575         *LTPCorr_Q15 = (SKP_int)SKP_Silk_SQRT_APPROX( SKP_LSHIFT( CCmax, 13 ) ); /* Output normalized correlation */\r
576         for( k = 0; k < nb_subfr; k++ ) {\r
577             pitch_out[ k ] = lag + SKP_Silk_CB_lags_stage2[ k ][ CBimax ];\r
578         }\r
579         *lagIndex = lag - min_lag_8kHz;\r
580         *contourIndex = CBimax;\r
581     }\r
582     SKP_assert( *lagIndex >= 0 );\r
583     /* return as voiced */\r
584     return 0;\r
585 }\r
586 \r
587 /*************************************************************************/\r
588 /* Calculates the correlations used in stage 3 search. In order to cover */\r
589 /* the whole lag codebook for all the searched offset lags (lag +- 2),   */\r
590 /*************************************************************************/\r
591 void SKP_FIX_P_Ana_calc_corr_st3(\r
592     SKP_int32        cross_corr_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ],/* (O) 3 DIM correlation array */\r
593     const SKP_int16  signal[],                        /* I vector to correlate         */\r
594     SKP_int          start_lag,                       /* I lag offset to search around */\r
595     SKP_int          sf_length,                       /* I length of a 5 ms subframe   */\r
596     SKP_int          nb_subfr,                        /* I number of subframes         */\r
597     SKP_int          complexity                       /* I Complexity setting          */\r
598 )\r
599 {\r
600     const SKP_int16 *target_ptr, *basis_ptr;\r
601     SKP_int32 cross_corr;\r
602     SKP_int   i, j, k, lag_counter, lag_low, lag_high;\r
603     SKP_int   cbk_offset, nb_cbk_search, delta, idx, cbk_size;\r
604     SKP_int32 scratch_mem[ SCRATCH_SIZE ];\r
605     const SKP_int8 *Lag_range_ptr, *Lag_CB_ptr;\r
606 \r
607     SKP_assert( complexity >= SKP_Silk_PE_MIN_COMPLEX );\r
608     SKP_assert( complexity <= SKP_Silk_PE_MAX_COMPLEX );\r
609 \r
610     if( nb_subfr == PE_MAX_NB_SUBFR ){\r
611         Lag_range_ptr = &SKP_Silk_Lag_range_stage3[ complexity ][ 0 ][ 0 ];\r
612         Lag_CB_ptr    = &SKP_Silk_CB_lags_stage3[ 0 ][ 0 ];\r
613         cbk_offset    = SKP_Silk_cbk_offsets_stage3[ complexity ];\r
614         nb_cbk_search = SKP_Silk_nb_cbk_searchs_stage3[   complexity ];\r
615         cbk_size      = PE_NB_CBKS_STAGE3_MAX;\r
616     } else {\r
617         SKP_assert( nb_subfr == PE_MAX_NB_SUBFR >> 1);\r
618         Lag_range_ptr = &SKP_Silk_Lag_range_stage3_10_ms[ 0 ][ 0 ];\r
619         Lag_CB_ptr    = &SKP_Silk_CB_lags_stage3_10_ms[ 0 ][ 0 ];\r
620         cbk_offset    = 0;\r
621         nb_cbk_search = PE_NB_CBKS_STAGE3_10MS;\r
622         cbk_size      = PE_NB_CBKS_STAGE3_10MS;\r
623     }\r
624 \r
625     target_ptr = &signal[ SKP_LSHIFT( sf_length, 2 ) ]; /* Pointer to middle of frame */\r
626     for( k = 0; k < nb_subfr; k++ ) {\r
627         lag_counter = 0;\r
628 \r
629         /* Calculate the correlations for each subframe */\r
630         lag_low  = matrix_ptr( Lag_range_ptr, k, 0, 2 );\r
631         lag_high = matrix_ptr( Lag_range_ptr, k, 1, 2 );\r
632         for( j = lag_low; j <= lag_high; j++ ) {\r
633             basis_ptr = target_ptr - ( start_lag + j );\r
634             cross_corr = SKP_Silk_inner_prod_aligned( (SKP_int16*)target_ptr, (SKP_int16*)basis_ptr, sf_length );\r
635             SKP_assert( lag_counter < SCRATCH_SIZE );\r
636             scratch_mem[ lag_counter ] = cross_corr;\r
637             lag_counter++;\r
638         }\r
639 \r
640         delta = matrix_ptr( Lag_range_ptr, k, 0, 2 );\r
641         for( i = cbk_offset; i < ( cbk_offset + nb_cbk_search ); i++ ) { \r
642             /* Fill out the 3 dim array that stores the correlations for */\r
643             /* each code_book vector for each start lag */\r
644             idx = matrix_ptr( Lag_CB_ptr, k, i, cbk_size ) - delta;\r
645             for( j = 0; j < PE_NB_STAGE3_LAGS; j++ ) {\r
646                 SKP_assert( idx + j < SCRATCH_SIZE );\r
647                 SKP_assert( idx + j < lag_counter );\r
648                 cross_corr_st3[ k ][ i ][ j ] = scratch_mem[ idx + j ];\r
649             }\r
650         }\r
651         target_ptr += sf_length;\r
652     }\r
653 }\r
654 \r
655 /********************************************************************/\r
656 /* Calculate the energies for first two subframes. The energies are */\r
657 /* calculated recursively.                                          */\r
658 /********************************************************************/\r
659 void SKP_FIX_P_Ana_calc_energy_st3(\r
660     SKP_int32        energies_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ],/* (O) 3 DIM energy array */\r
661     const SKP_int16  signal[],                        /* I vector to calc energy in    */\r
662     SKP_int          start_lag,                       /* I lag offset to search around */\r
663     SKP_int          sf_length,                       /* I length of one 5 ms subframe */\r
664     SKP_int          nb_subfr,                     /* I number of subframes         */\r
665     SKP_int          complexity                       /* I Complexity setting          */\r
666 )\r
667 {\r
668     const SKP_int16 *target_ptr, *basis_ptr;\r
669     SKP_int32 energy;\r
670     SKP_int   k, i, j, lag_counter;\r
671     SKP_int   cbk_offset, nb_cbk_search, delta, idx, cbk_size, lag_diff;\r
672     SKP_int32 scratch_mem[ SCRATCH_SIZE ];\r
673     const SKP_int8 *Lag_range_ptr, *Lag_CB_ptr;\r
674 \r
675     SKP_assert( complexity >= SKP_Silk_PE_MIN_COMPLEX );\r
676     SKP_assert( complexity <= SKP_Silk_PE_MAX_COMPLEX );\r
677 \r
678     if( nb_subfr == PE_MAX_NB_SUBFR ){\r
679         Lag_range_ptr = &SKP_Silk_Lag_range_stage3[ complexity ][ 0 ][ 0 ];\r
680         Lag_CB_ptr    = &SKP_Silk_CB_lags_stage3[ 0 ][ 0 ];\r
681         cbk_offset    = SKP_Silk_cbk_offsets_stage3[ complexity ];\r
682         nb_cbk_search = SKP_Silk_nb_cbk_searchs_stage3[   complexity ];\r
683         cbk_size      = PE_NB_CBKS_STAGE3_MAX;\r
684     } else {\r
685         SKP_assert( nb_subfr == PE_MAX_NB_SUBFR >> 1);\r
686         Lag_range_ptr = &SKP_Silk_Lag_range_stage3_10_ms[ 0 ][ 0 ];\r
687         Lag_CB_ptr    = &SKP_Silk_CB_lags_stage3_10_ms[ 0 ][ 0 ];\r
688         cbk_offset    = 0;\r
689         nb_cbk_search = PE_NB_CBKS_STAGE3_10MS;\r
690         cbk_size      = PE_NB_CBKS_STAGE3_10MS;\r
691     }\r
692     target_ptr = &signal[ SKP_LSHIFT( sf_length, 2 ) ];\r
693     for( k = 0; k < nb_subfr; k++ ) {\r
694         lag_counter = 0;\r
695 \r
696         /* Calculate the energy for first lag */\r
697         basis_ptr = target_ptr - ( start_lag + matrix_ptr( Lag_range_ptr, k, 0, 2 ) );\r
698         energy = SKP_Silk_inner_prod_aligned( basis_ptr, basis_ptr, sf_length );\r
699         SKP_assert( energy >= 0 );\r
700         scratch_mem[ lag_counter ] = energy;\r
701         lag_counter++;\r
702 \r
703         lag_diff = ( matrix_ptr( Lag_range_ptr, k, 1, 2 ) -  matrix_ptr( Lag_range_ptr, k, 0, 2 ) + 1 );\r
704         for( i = 1; i < lag_diff; i++ ) {\r
705             /* remove part outside new window */\r
706             energy -= SKP_SMULBB( basis_ptr[ sf_length - i ], basis_ptr[ sf_length - i ] );\r
707             SKP_assert( energy >= 0 );\r
708 \r
709             /* add part that comes into window */\r
710             energy = SKP_ADD_SAT32( energy, SKP_SMULBB( basis_ptr[ -i ], basis_ptr[ -i ] ) );\r
711             SKP_assert( energy >= 0 );\r
712             SKP_assert( lag_counter < SCRATCH_SIZE );\r
713             scratch_mem[ lag_counter ] = energy;\r
714             lag_counter++;\r
715         }\r
716 \r
717         delta = matrix_ptr( Lag_range_ptr, k, 0, 2 );\r
718         for( i = cbk_offset; i < ( cbk_offset + nb_cbk_search ); i++ ) { \r
719             /* Fill out the 3 dim array that stores the correlations for    */\r
720             /* each code_book vector for each start lag                     */\r
721             idx = matrix_ptr( Lag_CB_ptr, k, i, cbk_size ) - delta;\r
722             for( j = 0; j < PE_NB_STAGE3_LAGS; j++ ) {\r
723                 SKP_assert( idx + j < SCRATCH_SIZE );\r
724                 SKP_assert( idx + j < lag_counter );\r
725                 energies_st3[ k ][ i ][ j ] = scratch_mem[ idx + j ];\r
726                 SKP_assert( energies_st3[ k ][ i ][ j ] >= 0 );\r
727             }\r
728         }\r
729         target_ptr += sf_length;\r
730     }\r
731 }\r
732 \r
733 SKP_int32 SKP_FIX_P_Ana_find_scaling(\r
734     const SKP_int16  *signal,\r
735     const SKP_int    signal_length, \r
736     const SKP_int    sum_sqr_len\r
737 )\r
738 {\r
739     SKP_int32 nbits, x_max;\r
740     \r
741     x_max = SKP_Silk_int16_array_maxabs( signal, signal_length );\r
742 \r
743     if( x_max < SKP_int16_MAX ) {\r
744         /* Number of bits needed for the sum of the squares */\r
745         nbits = 32 - SKP_Silk_CLZ32( SKP_SMULBB( x_max, x_max ) ); \r
746     } else {\r
747         /* Here we don't know if x_max should have been SKP_int16_MAX + 1, so we expect the worst case */\r
748         nbits = 30;\r
749     }\r
750     nbits += 17 - SKP_Silk_CLZ16( sum_sqr_len );\r
751 \r
752     /* Without a guarantee of saturation, we need to keep the 31st bit free */\r
753     if( nbits < 31 ) {\r
754         return 0;\r
755     } else {\r
756         return( nbits - 30 );\r
757     }\r
758 }\r