Initial Skype commit taken from FreeSwitch, which got it from the IETF draft.
[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, 1000 );\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     /* Sort */\r
237     length_d_srch = 5 + complexity;\r
238     SKP_assert( length_d_srch <= PE_D_SRCH_LENGTH );\r
239       SKP_Silk_insertion_sort_decreasing_int16( &C[ 0 ][ min_lag_4kHz ], d_srch, max_lag_4kHz - min_lag_4kHz + 1, length_d_srch );\r
240 \r
241     /* Escape if correlation is very low already here */\r
242     target_ptr = &signal_4kHz[ SKP_SMULBB( sf_length_4kHz, nb_subfr ) ];\r
243     energy = SKP_Silk_inner_prod_aligned( target_ptr, target_ptr, SKP_LSHIFT( sf_length_4kHz, 2 ) );\r
244     energy = SKP_ADD_SAT32( energy, 1000 );                                  /* Q0 */\r
245     Cmax = (SKP_int)C[ 0 ][ min_lag_4kHz ];                                  /* Q-1 */\r
246     threshold = SKP_SMULBB( Cmax, Cmax );                                    /* Q-2 */\r
247     /* Compare in Q-2 domain */\r
248 \r
249     if( SKP_RSHIFT( energy, 4 + 2 ) > threshold ) {                            \r
250         SKP_memset( pitch_out, 0, nb_subfr * sizeof( SKP_int ) );\r
251         *LTPCorr_Q15  = 0;\r
252         *lagIndex     = 0;\r
253         *contourIndex = 0;\r
254         return 1;\r
255     }\r
256 \r
257     threshold = SKP_SMULWB( search_thres1_Q16, Cmax );\r
258     for( i = 0; i < length_d_srch; i++ ) {\r
259         /* Convert to 8 kHz indices for the sorted correlation that exceeds the threshold */\r
260         if( C[ 0 ][ min_lag_4kHz + i ] > threshold ) {\r
261             d_srch[ i ] = SKP_LSHIFT( d_srch[ i ] + min_lag_4kHz, 1 );\r
262         } else {\r
263             length_d_srch = i;\r
264             break;\r
265         }\r
266     }\r
267     SKP_assert( length_d_srch > 0 );\r
268 \r
269     for( i = min_lag_8kHz - 5; i < max_lag_8kHz + 5; i++ ) {\r
270         d_comp[ i ] = 0;\r
271     }\r
272     for( i = 0; i < length_d_srch; i++ ) {\r
273         d_comp[ d_srch[ i ] ] = 1;\r
274     }\r
275 \r
276     /* Convolution */\r
277     for( i = max_lag_8kHz + 3; i >= min_lag_8kHz; i-- ) {\r
278         d_comp[ i ] += d_comp[ i - 1 ] + d_comp[ i - 2 ];\r
279     }\r
280 \r
281     length_d_srch = 0;\r
282     for( i = min_lag_8kHz; i < max_lag_8kHz + 1; i++ ) {    \r
283         if( d_comp[ i + 1 ] > 0 ) {\r
284             d_srch[ length_d_srch ] = i;\r
285             length_d_srch++;\r
286         }\r
287     }\r
288 \r
289     /* Convolution */\r
290     for( i = max_lag_8kHz + 3; i >= min_lag_8kHz; i-- ) {\r
291         d_comp[ i ] += d_comp[ i - 1 ] + d_comp[ i - 2 ] + d_comp[ i - 3 ];\r
292     }\r
293 \r
294     length_d_comp = 0;\r
295     for( i = min_lag_8kHz; i < max_lag_8kHz + 4; i++ ) {    \r
296         if( d_comp[ i ] > 0 ) {\r
297             d_comp[ length_d_comp ] = i - 2;\r
298             length_d_comp++;\r
299         }\r
300     }\r
301 \r
302     /**********************************************************************************\r
303     ** SECOND STAGE, operating at 8 kHz, on lag sections with high correlation\r
304     *************************************************************************************/\r
305 \r
306     /******************************************************************************\r
307     ** Scale signal down to avoid correlations measures from overflowing\r
308     *******************************************************************************/\r
309     /* find scaling as max scaling for each subframe */\r
310     shift = SKP_FIX_P_Ana_find_scaling( signal_8kHz, frame_length_8kHz, sf_length_8kHz );\r
311     if( shift > 0 ) {\r
312         for( i = 0; i < frame_length_8kHz; i++ ) {\r
313             signal_8kHz[ i ] = SKP_RSHIFT( signal_8kHz[ i ], shift );\r
314         }\r
315     }\r
316 \r
317     /********************************************************************************* \r
318     * Find energy of each subframe projected onto its history, for a range of delays\r
319     *********************************************************************************/\r
320     SKP_memset( C, 0, PE_MAX_NB_SUBFR * ( ( PE_MAX_LAG >> 1 ) + 5 ) * sizeof( SKP_int16 ) );\r
321     \r
322     target_ptr = &signal_8kHz[ PE_LTP_MEM_LENGTH_MS * 8 ];\r
323     for( k = 0; k < nb_subfr; k++ ) {\r
324 \r
325         /* Check that we are within range of the array */\r
326         SKP_assert( target_ptr >= signal_8kHz );\r
327         SKP_assert( target_ptr + sf_length_8kHz <= signal_8kHz + frame_length_8kHz );\r
328 \r
329         energy_target = SKP_Silk_inner_prod_aligned( target_ptr, target_ptr, sf_length_8kHz );\r
330         // ToDo: Calculate 1 / energy_target here and save one division inside next for loop\r
331         for( j = 0; j < length_d_comp; j++ ) {\r
332             d = d_comp[ j ];\r
333             basis_ptr = target_ptr - d;\r
334 \r
335             /* Check that we are within range of the array */\r
336              SKP_assert( basis_ptr >= signal_8kHz );\r
337             SKP_assert( basis_ptr + sf_length_8kHz <= signal_8kHz + frame_length_8kHz );\r
338         \r
339             cross_corr   = SKP_Silk_inner_prod_aligned( target_ptr, basis_ptr, sf_length_8kHz );\r
340             energy_basis = SKP_Silk_inner_prod_aligned( basis_ptr,  basis_ptr, sf_length_8kHz );\r
341             if( cross_corr > 0 ) {\r
342                 energy = SKP_max( energy_target, energy_basis ); /* Find max to make sure first division < 1.0 */\r
343                 lz = SKP_Silk_CLZ32( cross_corr );\r
344                 lshift = SKP_LIMIT_32( lz - 1, 0, 15 );\r
345                 temp32 = SKP_DIV32( SKP_LSHIFT( cross_corr, lshift ), SKP_RSHIFT( energy, 15 - lshift ) + 1 ); /* Q15 */\r
346                 SKP_assert( temp32 == SKP_SAT16( temp32 ) );\r
347                 temp32 = SKP_SMULWB( cross_corr, temp32 ); /* Q(-1), cc * ( cc / max(b, t) ) */\r
348                 temp32 = SKP_ADD_SAT32( temp32, temp32 );  /* Q(0) */\r
349                 lz = SKP_Silk_CLZ32( temp32 );\r
350                 lshift = SKP_LIMIT_32( lz - 1, 0, 15 );\r
351                 energy = SKP_min( energy_target, energy_basis );\r
352                 C[ k ][ d ] = SKP_DIV32( SKP_LSHIFT( temp32, lshift ), SKP_RSHIFT( energy, 15 - lshift ) + 1 ); // Q15\r
353             } else {\r
354                 C[ k ][ d ] = 0;\r
355             }\r
356         }\r
357         target_ptr += sf_length_8kHz;\r
358     }\r
359 \r
360     /* search over lag range and lags codebook */\r
361     /* scale factor for lag codebook, as a function of center lag */\r
362 \r
363     CCmax   = SKP_int32_MIN;\r
364     CCmax_b = SKP_int32_MIN;\r
365 \r
366     CBimax = 0; /* To avoid returning undefined lag values */\r
367     lag = -1;   /* To check if lag with strong enough correlation has been found */\r
368 \r
369     if( prevLag > 0 ) {\r
370         if( Fs_kHz == 12 ) {\r
371             prevLag = SKP_DIV32_16( SKP_LSHIFT( prevLag, 1 ), 3 );\r
372         } else if( Fs_kHz == 16 ) {\r
373             prevLag = SKP_RSHIFT( prevLag, 1 );\r
374         } else if( Fs_kHz == 24 ) {\r
375             prevLag = SKP_DIV32_16( prevLag, 3 );\r
376         }\r
377         prevLag_log2_Q7 = SKP_Silk_lin2log( (SKP_int32)prevLag );\r
378     } else {\r
379         prevLag_log2_Q7 = 0;\r
380     }\r
381     SKP_assert( search_thres2_Q15 == SKP_SAT16( search_thres2_Q15 ) );\r
382     /* Setup stage 2 codebook based on number of subframes */\r
383     if( nb_subfr == PE_MAX_NB_SUBFR ) {\r
384         cbk_size   = PE_NB_CBKS_STAGE2_EXT;\r
385         Lag_CB_ptr = &SKP_Silk_CB_lags_stage2[ 0 ][ 0 ];\r
386         if( Fs_kHz == 8 && complexity > SKP_Silk_PE_MIN_COMPLEX ) {\r
387             /* If input is 8 khz use a larger codebook here because it is last stage */\r
388             nb_cbk_search = PE_NB_CBKS_STAGE2_EXT;\r
389         } else {\r
390             nb_cbk_search = PE_NB_CBKS_STAGE2;\r
391         }\r
392         corr_thres_Q15 = SKP_RSHIFT( SKP_SMULBB( search_thres2_Q15, search_thres2_Q15 ), 13 );\r
393     }else{\r
394         cbk_size       = PE_NB_CBKS_STAGE2_10MS;\r
395         Lag_CB_ptr     = &SKP_Silk_CB_lags_stage2_10_ms[ 0 ][ 0 ];\r
396         nb_cbk_search  = PE_NB_CBKS_STAGE2_10MS;\r
397         corr_thres_Q15 = SKP_RSHIFT( SKP_SMULBB( search_thres2_Q15, search_thres2_Q15 ), 14 );\r
398     }\r
399 \r
400     for( k = 0; k < length_d_srch; k++ ) {\r
401         d = d_srch[ k ];\r
402         for( j = 0; j < nb_cbk_search; j++ ) {\r
403             CC[ j ] = 0;\r
404             for( i = 0; i < nb_subfr; i++ ) {\r
405                 /* Try all codebooks */\r
406                 CC[ j ] = CC[ j ] + (SKP_int32)C[ i ][ d + matrix_ptr( Lag_CB_ptr, i, j, cbk_size )];\r
407             }\r
408         }\r
409         /* Find best codebook */\r
410         CCmax_new = SKP_int32_MIN;\r
411         CBimax_new = 0;\r
412         for( i = 0; i < nb_cbk_search; i++ ) {\r
413             if( CC[ i ] > CCmax_new ) {\r
414                 CCmax_new = CC[ i ];\r
415                 CBimax_new = i;\r
416             }\r
417         }\r
418 \r
419         /* Bias towards shorter lags */\r
420         lag_log2_Q7 = SKP_Silk_lin2log( (SKP_int32)d ); /* Q7 */\r
421         SKP_assert( lag_log2_Q7 == SKP_SAT16( lag_log2_Q7 ) );\r
422         SKP_assert( nb_subfr * PE_SHORTLAG_BIAS_Q15 == SKP_SAT16( nb_subfr * PE_SHORTLAG_BIAS_Q15 ) );\r
423         CCmax_new_b = CCmax_new - SKP_RSHIFT( SKP_SMULBB( nb_subfr * PE_SHORTLAG_BIAS_Q15, lag_log2_Q7 ), 7 ); /* Q15 */\r
424 \r
425         /* Bias towards previous lag */\r
426         SKP_assert( nb_subfr * PE_PREVLAG_BIAS_Q15 == SKP_SAT16( nb_subfr * PE_PREVLAG_BIAS_Q15 ) );\r
427         if( prevLag > 0 ) {\r
428             delta_lag_log2_sqr_Q7 = lag_log2_Q7 - prevLag_log2_Q7;\r
429             SKP_assert( delta_lag_log2_sqr_Q7 == SKP_SAT16( delta_lag_log2_sqr_Q7 ) );\r
430             delta_lag_log2_sqr_Q7 = SKP_RSHIFT( SKP_SMULBB( delta_lag_log2_sqr_Q7, delta_lag_log2_sqr_Q7 ), 7 );\r
431             prev_lag_bias_Q15 = SKP_RSHIFT( SKP_SMULBB( nb_subfr * PE_PREVLAG_BIAS_Q15, ( *LTPCorr_Q15 ) ), 15 ); /* Q15 */\r
432             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
433             CCmax_new_b -= prev_lag_bias_Q15; /* Q15 */\r
434         }\r
435 \r
436         if( CCmax_new_b > CCmax_b && CCmax_new > corr_thres_Q15 ) {\r
437             CCmax_b = CCmax_new_b;\r
438             CCmax   = CCmax_new;\r
439             lag     = d;\r
440             CBimax  = CBimax_new;\r
441         }\r
442     }\r
443 \r
444     if( lag == -1 ) {\r
445         /* No suitable candidate found */\r
446         SKP_memset( pitch_out, 0, nb_subfr * sizeof( SKP_int ) );\r
447         *LTPCorr_Q15  = 0;\r
448         *lagIndex     = 0;\r
449         *contourIndex = 0;\r
450         return 1;\r
451     }\r
452 \r
453     if( Fs_kHz > 8 ) {\r
454 \r
455         /******************************************************************************\r
456         ** Scale input signal down to avoid correlations measures from overflowing\r
457         *******************************************************************************/\r
458         /* find scaling as max scaling for each subframe */\r
459         shift = SKP_FIX_P_Ana_find_scaling( signal, frame_length, sf_length );\r
460         if( shift > 0 ) {\r
461             /* Move signal to scratch mem because the input signal should be unchanged */\r
462             /* Reuse the 32 bit scratch mem vector, use a 16 bit pointer from now */\r
463             input_signal_ptr = (SKP_int16*)scratch_mem;\r
464             for( i = 0; i < frame_length; i++ ) {\r
465                 input_signal_ptr[ i ] = SKP_RSHIFT( signal[ i ], shift );\r
466             }\r
467         } else {\r
468             input_signal_ptr = (SKP_int16*)signal;\r
469         }\r
470         /*********************************************************************************/\r
471 \r
472         /* Search in original signal */\r
473                     \r
474         CBimax_old = CBimax;\r
475         /* Compensate for decimation */\r
476         SKP_assert( lag == SKP_SAT16( lag ) );\r
477         if( Fs_kHz == 12 ) {\r
478             lag = SKP_RSHIFT( SKP_SMULBB( lag, 3 ), 1 );\r
479         } else if( Fs_kHz == 16 ) {\r
480             lag = SKP_LSHIFT( lag, 1 );\r
481         } else {\r
482             lag = SKP_SMULBB( lag, 3 );\r
483         }\r
484 \r
485         lag = SKP_LIMIT_int( lag, min_lag, max_lag );\r
486         start_lag = SKP_max_int( lag - 2, min_lag );\r
487         end_lag   = SKP_min_int( lag + 2, max_lag );\r
488         lag_new   = lag;                                    /* to avoid undefined lag */\r
489         CBimax    = 0;                                        /* to avoid undefined lag */\r
490         SKP_assert( SKP_LSHIFT( CCmax, 13 ) >= 0 ); \r
491         *LTPCorr_Q15 = (SKP_int)SKP_Silk_SQRT_APPROX( SKP_LSHIFT( CCmax, 13 ) ); /* Output normalized correlation */\r
492 \r
493         CCmax = SKP_int32_MIN;\r
494         /* pitch lags according to second stage */\r
495         for( k = 0; k < nb_subfr; k++ ) {\r
496             pitch_out[ k ] = lag + 2 * SKP_Silk_CB_lags_stage2[ k ][ CBimax_old ];\r
497         }\r
498         /* Calculate the correlations and energies needed in stage 3 */\r
499         SKP_FIX_P_Ana_calc_corr_st3(  crosscorr_st3, input_signal_ptr, start_lag, sf_length, nb_subfr, complexity );\r
500         SKP_FIX_P_Ana_calc_energy_st3( energies_st3, input_signal_ptr, start_lag, sf_length, nb_subfr, complexity );\r
501 \r
502         lag_counter = 0;\r
503         SKP_assert( lag == SKP_SAT16( lag ) );\r
504         contour_bias = SKP_DIV32_16( PE_FLATCONTOUR_BIAS_Q20, lag );\r
505 \r
506         /* Setup cbk parameters acording to complexity setting and frame length */\r
507         if( nb_subfr == PE_MAX_NB_SUBFR ) {\r
508             nb_cbk_search   = (SKP_int)SKP_Silk_nb_cbk_searchs_stage3[   complexity ];\r
509             cbk_size        = PE_NB_CBKS_STAGE3_MAX;\r
510             cbk_offset      = (SKP_int)SKP_Silk_cbk_offsets_stage3[ complexity ];\r
511             Lag_CB_ptr      = &SKP_Silk_CB_lags_stage3[ 0 ][ 0 ];\r
512         } else {\r
513             nb_cbk_search   = PE_NB_CBKS_STAGE3_10MS;\r
514             cbk_size        = PE_NB_CBKS_STAGE3_10MS;\r
515             cbk_offset      = 0;\r
516             Lag_CB_ptr      = &SKP_Silk_CB_lags_stage3_10_ms[ 0 ][ 0 ];\r
517         }\r
518         for( d = start_lag; d <= end_lag; d++ ) {\r
519             for( j = cbk_offset; j < ( cbk_offset + nb_cbk_search ); j++ ) {\r
520                 cross_corr = 0;\r
521                 energy     = 0;\r
522                 for( k = 0; k < nb_subfr; k++ ) {\r
523                     SKP_assert( PE_MAX_NB_SUBFR == 4 );\r
524                     energy     += SKP_RSHIFT( energies_st3[  k ][ j ][ lag_counter ], 2 ); /* use mean, to avoid overflow */\r
525                      SKP_assert( energy >= 0 );\r
526                     cross_corr += SKP_RSHIFT( crosscorr_st3[ k ][ j ][ lag_counter ], 2 ); /* use mean, to avoid overflow */\r
527                 }\r
528                 if( cross_corr > 0 ) {\r
529                     /* Divide cross_corr / energy and get result in Q15 */\r
530                     lz = SKP_Silk_CLZ32( cross_corr );\r
531                     /* Divide with result in Q13, cross_corr could be larger than energy */\r
532                     lshift = SKP_LIMIT_32( lz - 1, 0, 13 );\r
533                     CCmax_new = SKP_DIV32( SKP_LSHIFT( cross_corr, lshift ), SKP_RSHIFT( energy, 13 - lshift ) + 1 );\r
534                     CCmax_new = SKP_SAT16( CCmax_new );\r
535                     CCmax_new = SKP_SMULWB( cross_corr, CCmax_new );\r
536                     /* Saturate */\r
537                     if( CCmax_new > SKP_RSHIFT( SKP_int32_MAX, 3 ) ) {\r
538                         CCmax_new = SKP_int32_MAX;\r
539                     } else {\r
540                         CCmax_new = SKP_LSHIFT( CCmax_new, 3 );\r
541                     }\r
542                     /* Reduce depending on flatness of contour */\r
543                     diff = j - SKP_RSHIFT( cbk_size, 1 );\r
544                     diff = SKP_MUL( diff, diff );\r
545                     diff = SKP_int16_MAX - SKP_RSHIFT( SKP_MUL( contour_bias, diff ), 5 ); /* Q20 -> Q15 */\r
546                     SKP_assert( diff == SKP_SAT16( diff ) );\r
547                     CCmax_new = SKP_LSHIFT( SKP_SMULWB( CCmax_new, diff ), 1 );\r
548                 } else {\r
549                     CCmax_new = 0;\r
550                 }\r
551 \r
552                 if( CCmax_new > CCmax ) {\r
553                     CCmax   = CCmax_new;\r
554                     lag_new = d;\r
555                     CBimax  = j;\r
556                 }\r
557             }\r
558             lag_counter++;\r
559         }\r
560 \r
561         for( k = 0; k < nb_subfr; k++ ) {\r
562             pitch_out[ k ] = lag_new + matrix_ptr( Lag_CB_ptr, k, CBimax, cbk_size );\r
563         }\r
564         *lagIndex = lag_new - min_lag;\r
565         *contourIndex = CBimax;\r
566     } else {\r
567         /* Save Lags and correlation */\r
568         CCmax = SKP_max( CCmax, 0 );\r
569         *LTPCorr_Q15 = (SKP_int)SKP_Silk_SQRT_APPROX( SKP_LSHIFT( CCmax, 13 ) ); /* Output normalized correlation */\r
570         for( k = 0; k < nb_subfr; k++ ) {\r
571             pitch_out[ k ] = lag + SKP_Silk_CB_lags_stage2[ k ][ CBimax ];\r
572         }\r
573         *lagIndex = lag - min_lag_8kHz;\r
574         *contourIndex = CBimax;\r
575     }\r
576     SKP_assert( *lagIndex >= 0 );\r
577     /* return as voiced */\r
578     return 0;\r
579 }\r
580 \r
581 /*************************************************************************/\r
582 /* Calculates the correlations used in stage 3 search. In order to cover */\r
583 /* the whole lag codebook for all the searched offset lags (lag +- 2),   */\r
584 /*************************************************************************/\r
585 void SKP_FIX_P_Ana_calc_corr_st3(\r
586     SKP_int32        cross_corr_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ],/* (O) 3 DIM correlation array */\r
587     const SKP_int16  signal[],                        /* I vector to correlate         */\r
588     SKP_int          start_lag,                       /* I lag offset to search around */\r
589     SKP_int          sf_length,                       /* I length of a 5 ms subframe   */\r
590     SKP_int          nb_subfr,                        /* I number of subframes         */\r
591     SKP_int          complexity                       /* I Complexity setting          */\r
592 )\r
593 {\r
594     const SKP_int16 *target_ptr, *basis_ptr;\r
595     SKP_int32 cross_corr;\r
596     SKP_int   i, j, k, lag_counter, lag_low, lag_high;\r
597     SKP_int   cbk_offset, nb_cbk_search, delta, idx, cbk_size;\r
598     SKP_int32 scratch_mem[ SCRATCH_SIZE ];\r
599     const SKP_int8 *Lag_range_ptr, *Lag_CB_ptr;\r
600 \r
601     SKP_assert( complexity >= SKP_Silk_PE_MIN_COMPLEX );\r
602     SKP_assert( complexity <= SKP_Silk_PE_MAX_COMPLEX );\r
603 \r
604     if( nb_subfr == PE_MAX_NB_SUBFR ){\r
605         Lag_range_ptr = &SKP_Silk_Lag_range_stage3[ complexity ][ 0 ][ 0 ];\r
606         Lag_CB_ptr    = &SKP_Silk_CB_lags_stage3[ 0 ][ 0 ];\r
607         cbk_offset    = SKP_Silk_cbk_offsets_stage3[ complexity ];\r
608         nb_cbk_search = SKP_Silk_nb_cbk_searchs_stage3[   complexity ];\r
609         cbk_size      = PE_NB_CBKS_STAGE3_MAX;\r
610     } else {\r
611         SKP_assert( nb_subfr == PE_MAX_NB_SUBFR >> 1);\r
612         Lag_range_ptr = &SKP_Silk_Lag_range_stage3_10_ms[ 0 ][ 0 ];\r
613         Lag_CB_ptr    = &SKP_Silk_CB_lags_stage3_10_ms[ 0 ][ 0 ];\r
614         cbk_offset    = 0;\r
615         nb_cbk_search = PE_NB_CBKS_STAGE3_10MS;\r
616         cbk_size      = PE_NB_CBKS_STAGE3_10MS;\r
617     }\r
618 \r
619     target_ptr = &signal[ SKP_LSHIFT( sf_length, 2 ) ]; /* Pointer to middle of frame */\r
620     for( k = 0; k < nb_subfr; k++ ) {\r
621         lag_counter = 0;\r
622 \r
623         /* Calculate the correlations for each subframe */\r
624         lag_low  = matrix_ptr( Lag_range_ptr, k, 0, 2 );\r
625         lag_high = matrix_ptr( Lag_range_ptr, k, 1, 2 );\r
626         for( j = lag_low; j <= lag_high; j++ ) {\r
627             basis_ptr = target_ptr - ( start_lag + j );\r
628             cross_corr = SKP_Silk_inner_prod_aligned( (SKP_int16*)target_ptr, (SKP_int16*)basis_ptr, sf_length );\r
629             SKP_assert( lag_counter < SCRATCH_SIZE );\r
630             scratch_mem[ lag_counter ] = cross_corr;\r
631             lag_counter++;\r
632         }\r
633 \r
634         delta = matrix_ptr( Lag_range_ptr, k, 0, 2 );\r
635         for( i = cbk_offset; i < ( cbk_offset + nb_cbk_search ); i++ ) { \r
636             /* Fill out the 3 dim array that stores the correlations for */\r
637             /* each code_book vector for each start lag */\r
638             idx = matrix_ptr( Lag_CB_ptr, k, i, cbk_size ) - delta;\r
639             for( j = 0; j < PE_NB_STAGE3_LAGS; j++ ) {\r
640                 SKP_assert( idx + j < SCRATCH_SIZE );\r
641                 SKP_assert( idx + j < lag_counter );\r
642                 cross_corr_st3[ k ][ i ][ j ] = scratch_mem[ idx + j ];\r
643             }\r
644         }\r
645         target_ptr += sf_length;\r
646     }\r
647 }\r
648 \r
649 /********************************************************************/\r
650 /* Calculate the energies for first two subframes. The energies are */\r
651 /* calculated recursively.                                          */\r
652 /********************************************************************/\r
653 void SKP_FIX_P_Ana_calc_energy_st3(\r
654     SKP_int32        energies_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ],/* (O) 3 DIM energy array */\r
655     const SKP_int16  signal[],                        /* I vector to calc energy in    */\r
656     SKP_int          start_lag,                       /* I lag offset to search around */\r
657     SKP_int          sf_length,                       /* I length of one 5 ms subframe */\r
658     SKP_int          nb_subfr,                     /* I number of subframes         */\r
659     SKP_int          complexity                       /* I Complexity setting          */\r
660 )\r
661 {\r
662     const SKP_int16 *target_ptr, *basis_ptr;\r
663     SKP_int32 energy;\r
664     SKP_int   k, i, j, lag_counter;\r
665     SKP_int   cbk_offset, nb_cbk_search, delta, idx, cbk_size, lag_diff;\r
666     SKP_int32 scratch_mem[ SCRATCH_SIZE ];\r
667     const SKP_int8 *Lag_range_ptr, *Lag_CB_ptr;\r
668 \r
669     SKP_assert( complexity >= SKP_Silk_PE_MIN_COMPLEX );\r
670     SKP_assert( complexity <= SKP_Silk_PE_MAX_COMPLEX );\r
671 \r
672     if( nb_subfr == PE_MAX_NB_SUBFR ){\r
673         Lag_range_ptr = &SKP_Silk_Lag_range_stage3[ complexity ][ 0 ][ 0 ];\r
674         Lag_CB_ptr    = &SKP_Silk_CB_lags_stage3[ 0 ][ 0 ];\r
675         cbk_offset    = SKP_Silk_cbk_offsets_stage3[ complexity ];\r
676         nb_cbk_search = SKP_Silk_nb_cbk_searchs_stage3[   complexity ];\r
677         cbk_size      = PE_NB_CBKS_STAGE3_MAX;\r
678     } else {\r
679         SKP_assert( nb_subfr == PE_MAX_NB_SUBFR >> 1);\r
680         Lag_range_ptr = &SKP_Silk_Lag_range_stage3_10_ms[ 0 ][ 0 ];\r
681         Lag_CB_ptr    = &SKP_Silk_CB_lags_stage3_10_ms[ 0 ][ 0 ];\r
682         cbk_offset    = 0;\r
683         nb_cbk_search = PE_NB_CBKS_STAGE3_10MS;\r
684         cbk_size      = PE_NB_CBKS_STAGE3_10MS;\r
685     }\r
686     target_ptr = &signal[ SKP_LSHIFT( sf_length, 2 ) ];\r
687     for( k = 0; k < nb_subfr; k++ ) {\r
688         lag_counter = 0;\r
689 \r
690         /* Calculate the energy for first lag */\r
691         basis_ptr = target_ptr - ( start_lag + matrix_ptr( Lag_range_ptr, k, 0, 2 ) );\r
692         energy = SKP_Silk_inner_prod_aligned( basis_ptr, basis_ptr, sf_length );\r
693         SKP_assert( energy >= 0 );\r
694         scratch_mem[ lag_counter ] = energy;\r
695         lag_counter++;\r
696 \r
697         lag_diff = ( matrix_ptr( Lag_range_ptr, k, 1, 2 ) -  matrix_ptr( Lag_range_ptr, k, 0, 2 ) + 1 );\r
698         for( i = 1; i < lag_diff; i++ ) {\r
699             /* remove part outside new window */\r
700             energy -= SKP_SMULBB( basis_ptr[ sf_length - i ], basis_ptr[ sf_length - i ] );\r
701             SKP_assert( energy >= 0 );\r
702 \r
703             /* add part that comes into window */\r
704             energy = SKP_ADD_SAT32( energy, SKP_SMULBB( basis_ptr[ -i ], basis_ptr[ -i ] ) );\r
705             SKP_assert( energy >= 0 );\r
706             SKP_assert( lag_counter < SCRATCH_SIZE );\r
707             scratch_mem[ lag_counter ] = energy;\r
708             lag_counter++;\r
709         }\r
710 \r
711         delta = matrix_ptr( Lag_range_ptr, k, 0, 2 );\r
712         for( i = cbk_offset; i < ( cbk_offset + nb_cbk_search ); i++ ) { \r
713             /* Fill out the 3 dim array that stores the correlations for    */\r
714             /* each code_book vector for each start lag                     */\r
715             idx = matrix_ptr( Lag_CB_ptr, k, i, cbk_size ) - delta;\r
716             for( j = 0; j < PE_NB_STAGE3_LAGS; j++ ) {\r
717                 SKP_assert( idx + j < SCRATCH_SIZE );\r
718                 SKP_assert( idx + j < lag_counter );\r
719                 energies_st3[ k ][ i ][ j ] = scratch_mem[ idx + j ];\r
720                 SKP_assert( energies_st3[ k ][ i ][ j ] >= 0 );\r
721             }\r
722         }\r
723         target_ptr += sf_length;\r
724     }\r
725 }\r
726 \r
727 SKP_int32 SKP_FIX_P_Ana_find_scaling(\r
728     const SKP_int16  *signal,\r
729     const SKP_int    signal_length, \r
730     const SKP_int    sum_sqr_len\r
731 )\r
732 {\r
733     SKP_int32 nbits, x_max;\r
734     \r
735     x_max = SKP_Silk_int16_array_maxabs( signal, signal_length );\r
736 \r
737     if( x_max < SKP_int16_MAX ) {\r
738         /* Number of bits needed for the sum of the squares */\r
739         nbits = 32 - SKP_Silk_CLZ32( SKP_SMULBB( x_max, x_max ) ); \r
740     } else {\r
741         /* Here we don't know if x_max should have been SKP_int16_MAX + 1, so we expect the worst case */\r
742         nbits = 30;\r
743     }\r
744     nbits += 17 - SKP_Silk_CLZ16( sum_sqr_len );\r
745 \r
746     /* Without a guarantee of saturation, we need to keep the 31st bit free */\r
747     if( nbits < 31 ) {\r
748         return 0;\r
749     } else {\r
750         return( nbits - 30 );\r
751     }\r
752 }\r