Fixes a crash on fixed-point stereo voice and some valgrind uninitialized errors
[opus.git] / silk / float / silk_pitch_analysis_core_FLP.c
1 /***********************************************************************\r
2 Copyright (c) 2006-2011, 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 *\r
30 * Pitch analyser function\r
31 *\r
32 ******************************************************************************/\r
33 #include "silk_SigProc_FLP.h"\r
34 #include "silk_SigProc_FIX.h"\r
35 #include "silk_pitch_est_defines.h"\r
36 \r
37 #define SCRATCH_SIZE    22\r
38 \r
39 /************************************************************/\r
40 /* Definitions                                              */\r
41 /************************************************************/\r
42 #define eps                     1.192092896e-07f\r
43 \r
44 /************************************************************/\r
45 /* Internally used functions                                */\r
46 /************************************************************/\r
47 static void silk_P_Ana_calc_corr_st3(\r
48     SKP_float cross_corr_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ], /* O 3 DIM correlation array */\r
49     const SKP_float signal[],           /* I vector to correlate                                            */\r
50     SKP_int         start_lag,          /* I start lag                                                      */\r
51     SKP_int         sf_length,          /* I sub frame length                                               */\r
52     SKP_int         nb_subfr,           /* I number of subframes                                            */\r
53     SKP_int         complexity          /* I Complexity setting                                             */\r
54 );\r
55 \r
56 static void silk_P_Ana_calc_energy_st3(\r
57     SKP_float energies_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ], /* O 3 DIM correlation array */\r
58     const SKP_float signal[],           /* I vector to correlate                                            */\r
59     SKP_int         start_lag,          /* I start lag                                                      */\r
60     SKP_int         sf_length,          /* I sub frame length                                               */\r
61     SKP_int         nb_subfr,           /* I number of subframes                                            */\r
62     SKP_int         complexity          /* I Complexity setting                                             */\r
63 );\r
64 \r
65 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
66 //%             CORE PITCH ANALYSIS FUNCTION                %\r
67 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
68 SKP_int silk_pitch_analysis_core_FLP( /* O voicing estimate: 0 voiced, 1 unvoiced                       */\r
69     const SKP_float *signal,            /* I signal of length PE_FRAME_LENGTH_MS*Fs_kHz                     */\r
70     SKP_int         *pitch_out,         /* O 4 pitch lag values                                             */\r
71     SKP_int16        *lagIndex,         /* O lag Index                                                      */\r
72     SKP_int8        *contourIndex,      /* O pitch contour Index                                            */\r
73     SKP_float       *LTPCorr,           /* I/O normalized correlation; input: value from previous frame     */\r
74     SKP_int         prevLag,            /* I last lag of previous frame; set to zero is unvoiced            */\r
75     const SKP_float search_thres1,      /* I first stage threshold for lag candidates 0 - 1                 */\r
76     const SKP_float search_thres2,      /* I final threshold for lag candidates 0 - 1                       */\r
77     const SKP_int   Fs_kHz,             /* I sample frequency (kHz)                                         */\r
78     const SKP_int   complexity,         /* I Complexity setting, 0-2, where 2 is highest                    */\r
79     const SKP_int   nb_subfr            /* I    number of 5 ms subframes                                    */\r
80 )\r
81 {\r
82     SKP_int   i, k, d, j;\r
83     SKP_float signal_8kHz[  PE_MAX_FRAME_LENGTH_MS * 8 ];\r
84     SKP_float signal_4kHz[  PE_MAX_FRAME_LENGTH_MS * 4 ];\r
85     SKP_int16 signal_8_FIX[ PE_MAX_FRAME_LENGTH_MS * 8 ];\r
86     SKP_int16 signal_4_FIX[ PE_MAX_FRAME_LENGTH_MS * 4 ];\r
87     SKP_int32 filt_state[ 6 ];\r
88     SKP_float threshold, contour_bias;\r
89     SKP_float C[ PE_MAX_NB_SUBFR][ (PE_MAX_LAG >> 1) + 5 ];\r
90     SKP_float CC[ PE_NB_CBKS_STAGE2_EXT ];\r
91     const SKP_float *target_ptr, *basis_ptr;\r
92     double    cross_corr, normalizer, energy, energy_tmp;\r
93     SKP_int   d_srch[ PE_D_SRCH_LENGTH ];\r
94     SKP_int16 d_comp[ (PE_MAX_LAG >> 1) + 5 ];\r
95     SKP_int   length_d_srch, length_d_comp;\r
96     SKP_float Cmax, CCmax, CCmax_b, CCmax_new_b, CCmax_new;\r
97     SKP_int   CBimax, CBimax_new, lag, start_lag, end_lag, lag_new;\r
98     SKP_int   cbk_size;\r
99     SKP_float lag_log2, prevLag_log2, delta_lag_log2_sqr;\r
100     SKP_float energies_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ];\r
101     SKP_float cross_corr_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ];\r
102     SKP_int   lag_counter;\r
103     SKP_int   frame_length, frame_length_8kHz, frame_length_4kHz;\r
104     SKP_int   sf_length, sf_length_8kHz, sf_length_4kHz;\r
105     SKP_int   min_lag, min_lag_8kHz, min_lag_4kHz;\r
106     SKP_int   max_lag, max_lag_8kHz, max_lag_4kHz;\r
107     SKP_int   nb_cbk_search;\r
108     const SKP_int8 *Lag_CB_ptr;\r
109 \r
110     /* Check for valid sampling frequency */\r
111     SKP_assert( Fs_kHz == 8 || Fs_kHz == 12 || Fs_kHz == 16 );\r
112 \r
113     /* Check for valid complexity setting */\r
114     SKP_assert( complexity >= SigProc_PE_MIN_COMPLEX );\r
115     SKP_assert( complexity <= SigProc_PE_MAX_COMPLEX );\r
116 \r
117     SKP_assert( search_thres1 >= 0.0f && search_thres1 <= 1.0f );\r
118     SKP_assert( search_thres2 >= 0.0f && search_thres2 <= 1.0f );\r
119 \r
120     /* Setup frame lengths max / min lag for the sampling frequency */\r
121     frame_length      = ( PE_LTP_MEM_LENGTH_MS + nb_subfr * PE_SUBFR_LENGTH_MS ) * Fs_kHz;\r
122     frame_length_4kHz = ( PE_LTP_MEM_LENGTH_MS + nb_subfr * PE_SUBFR_LENGTH_MS ) * 4;\r
123     frame_length_8kHz = ( PE_LTP_MEM_LENGTH_MS + nb_subfr * PE_SUBFR_LENGTH_MS ) * 8;\r
124     sf_length         = PE_SUBFR_LENGTH_MS * Fs_kHz;\r
125     sf_length_4kHz    = PE_SUBFR_LENGTH_MS * 4;\r
126     sf_length_8kHz    = PE_SUBFR_LENGTH_MS * 8;\r
127     min_lag           = PE_MIN_LAG_MS * Fs_kHz;\r
128     min_lag_4kHz      = PE_MIN_LAG_MS * 4;\r
129     min_lag_8kHz      = PE_MIN_LAG_MS * 8;\r
130     max_lag           = PE_MAX_LAG_MS * Fs_kHz - 1;\r
131     max_lag_4kHz      = PE_MAX_LAG_MS * 4;\r
132     max_lag_8kHz      = PE_MAX_LAG_MS * 8 - 1;\r
133 \r
134     SKP_memset(C, 0, sizeof(SKP_float) * nb_subfr * ((PE_MAX_LAG >> 1) + 5));\r
135     \r
136     /* Resample from input sampled at Fs_kHz to 8 kHz */\r
137     if( Fs_kHz == 16 ) {\r
138         /* Resample to 16 -> 8 khz */\r
139         SKP_int16 signal_16_FIX[ 16 * PE_MAX_FRAME_LENGTH_MS ];\r
140         SKP_float2short_array( signal_16_FIX, signal, frame_length );\r
141         SKP_memset( filt_state, 0, 2 * sizeof( SKP_int32 ) );\r
142         silk_resampler_down2( filt_state, signal_8_FIX, signal_16_FIX, frame_length );\r
143         SKP_short2float_array( signal_8kHz, signal_8_FIX, frame_length_8kHz );\r
144     } else if( Fs_kHz == 12 ) {\r
145         /* Resample to 12 -> 8 khz */\r
146         SKP_int16 signal_12_FIX[ 12 * PE_MAX_FRAME_LENGTH_MS ];\r
147         SKP_float2short_array( signal_12_FIX, signal, frame_length );\r
148         SKP_memset( filt_state, 0, 6 * sizeof( SKP_int32 ) );\r
149         silk_resampler_down2_3( filt_state, signal_8_FIX, signal_12_FIX, frame_length );\r
150         SKP_short2float_array( signal_8kHz, signal_8_FIX, frame_length_8kHz );\r
151     } else {\r
152         SKP_assert( Fs_kHz == 8 );\r
153         SKP_float2short_array( signal_8_FIX, signal, frame_length_8kHz );\r
154     }\r
155 \r
156     /* Decimate again to 4 kHz */\r
157     SKP_memset( filt_state, 0, 2 * sizeof( SKP_int32 ) );\r
158     silk_resampler_down2( filt_state, signal_4_FIX, signal_8_FIX, frame_length_8kHz );\r
159     SKP_short2float_array( signal_4kHz, signal_4_FIX, frame_length_4kHz );\r
160 \r
161     /* Low-pass filter */\r
162     for( i = frame_length_4kHz - 1; i > 0; i-- ) {\r
163         signal_4kHz[ i ] += signal_4kHz[ i - 1 ];\r
164     }\r
165 \r
166     /******************************************************************************\r
167     * FIRST STAGE, operating in 4 khz\r
168     ******************************************************************************/\r
169     target_ptr = &signal_4kHz[ SKP_LSHIFT( sf_length_4kHz, 2 ) ];\r
170     for( k = 0; k < nb_subfr >> 1; k++ ) {\r
171         /* Check that we are within range of the array */\r
172         SKP_assert( target_ptr >= signal_4kHz );\r
173         SKP_assert( target_ptr + sf_length_8kHz <= signal_4kHz + frame_length_4kHz );\r
174 \r
175         basis_ptr = target_ptr - min_lag_4kHz;\r
176 \r
177         /* Check that we are within range of the array */\r
178         SKP_assert( basis_ptr >= signal_4kHz );\r
179         SKP_assert( basis_ptr + sf_length_8kHz <= signal_4kHz + frame_length_4kHz );\r
180 \r
181         /* Calculate first vector products before loop */\r
182         cross_corr = silk_inner_product_FLP( target_ptr, basis_ptr, sf_length_8kHz );\r
183         normalizer = silk_energy_FLP( basis_ptr, sf_length_8kHz ) + sf_length_8kHz * 4000.0f;\r
184 \r
185         C[ 0 ][ min_lag_4kHz ] += (SKP_float)(cross_corr / sqrt(normalizer));\r
186 \r
187         /* From now on normalizer is computed recursively */\r
188         for(d = min_lag_4kHz + 1; d <= max_lag_4kHz; d++) {\r
189             basis_ptr--;\r
190 \r
191             /* Check that we are within range of the array */\r
192             SKP_assert( basis_ptr >= signal_4kHz );\r
193             SKP_assert( basis_ptr + sf_length_8kHz <= signal_4kHz + frame_length_4kHz );\r
194 \r
195             cross_corr = silk_inner_product_FLP(target_ptr, basis_ptr, sf_length_8kHz);\r
196 \r
197             /* Add contribution of new sample and remove contribution from oldest sample */\r
198             normalizer +=\r
199                 basis_ptr[ 0 ] * basis_ptr[ 0 ] - \r
200                 basis_ptr[ sf_length_8kHz ] * basis_ptr[ sf_length_8kHz ];\r
201             C[ 0 ][ d ] += (SKP_float)(cross_corr / sqrt( normalizer ));\r
202         }\r
203         /* Update target pointer */\r
204         target_ptr += sf_length_8kHz;\r
205     }\r
206 \r
207     /* Apply short-lag bias */\r
208     for( i = max_lag_4kHz; i >= min_lag_4kHz; i-- ) {\r
209         C[ 0 ][ i ] -= C[ 0 ][ i ] * i / 4096.0f;\r
210     }\r
211 \r
212     /* Sort */\r
213     length_d_srch = 4 + 2 * complexity;\r
214     SKP_assert( 3 * length_d_srch <= PE_D_SRCH_LENGTH );\r
215     silk_insertion_sort_decreasing_FLP( &C[ 0 ][ min_lag_4kHz ], d_srch, max_lag_4kHz - min_lag_4kHz + 1, length_d_srch );\r
216 \r
217     /* Escape if correlation is very low already here */\r
218     Cmax = C[ 0 ][ min_lag_4kHz ];\r
219     target_ptr = &signal_4kHz[ SKP_SMULBB( sf_length_4kHz, nb_subfr ) ];\r
220     energy = 1000.0f;\r
221     for( i = 0; i < SKP_LSHIFT( sf_length_4kHz, 2 ); i++ ) {\r
222         energy += target_ptr[i] * target_ptr[i];\r
223     }\r
224     threshold = Cmax * Cmax; \r
225     if( energy / 16.0f > threshold ) {\r
226         SKP_memset( pitch_out, 0, nb_subfr * sizeof( SKP_int ) );\r
227         *LTPCorr      = 0.0f;\r
228         *lagIndex     = 0;\r
229         *contourIndex = 0;\r
230         return 1;\r
231     }\r
232 \r
233     threshold = search_thres1 * Cmax;\r
234     for( i = 0; i < length_d_srch; i++ ) {\r
235         /* Convert to 8 kHz indices for the sorted correlation that exceeds the threshold */\r
236         if( C[ 0 ][ min_lag_4kHz + i ] > threshold ) {\r
237             d_srch[ i ] = SKP_LSHIFT( d_srch[ i ] + min_lag_4kHz, 1 );\r
238         } else {\r
239             length_d_srch = i;\r
240             break;\r
241         }\r
242     }\r
243     SKP_assert( length_d_srch > 0 );\r
244 \r
245     for( i = min_lag_8kHz - 5; i < max_lag_8kHz + 5; i++ ) {\r
246         d_comp[ i ] = 0;\r
247     }\r
248     for( i = 0; i < length_d_srch; i++ ) {\r
249         d_comp[ d_srch[ i ] ] = 1;\r
250     }\r
251 \r
252     /* Convolution */\r
253     for( i = max_lag_8kHz + 3; i >= min_lag_8kHz; i-- ) {\r
254         d_comp[ i ] += d_comp[ i - 1 ] + d_comp[ i - 2 ];\r
255     }\r
256 \r
257     length_d_srch = 0;\r
258     for( i = min_lag_8kHz; i < max_lag_8kHz + 1; i++ ) {    \r
259         if( d_comp[ i + 1 ] > 0 ) {\r
260             d_srch[ length_d_srch ] = i;\r
261             length_d_srch++;\r
262         }\r
263     }\r
264 \r
265     /* Convolution */\r
266     for( i = max_lag_8kHz + 3; i >= min_lag_8kHz; i-- ) {\r
267         d_comp[ i ] += d_comp[ i - 1 ] + d_comp[ i - 2 ] + d_comp[ i - 3 ];\r
268     }\r
269 \r
270     length_d_comp = 0;\r
271     for( i = min_lag_8kHz; i < max_lag_8kHz + 4; i++ ) {    \r
272         if( d_comp[ i ] > 0 ) {\r
273             d_comp[ length_d_comp ] = (SKP_int16)( i - 2 );\r
274             length_d_comp++;\r
275         }\r
276     }\r
277 \r
278     /**********************************************************************************\r
279     ** SECOND STAGE, operating at 8 kHz, on lag sections with high correlation\r
280     *************************************************************************************/\r
281     /********************************************************************************* \r
282     * Find energy of each subframe projected onto its history, for a range of delays\r
283     *********************************************************************************/\r
284     SKP_memset( C, 0, PE_MAX_NB_SUBFR*((PE_MAX_LAG >> 1) + 5) * sizeof(SKP_float)); // Is this needed?\r
285     \r
286     if( Fs_kHz == 8 ) {\r
287         target_ptr = &signal[ PE_LTP_MEM_LENGTH_MS * 8 ];\r
288     } else {\r
289         target_ptr = &signal_8kHz[ PE_LTP_MEM_LENGTH_MS * 8 ];\r
290     }\r
291     for( k = 0; k < nb_subfr; k++ ) {\r
292         energy_tmp = silk_energy_FLP( target_ptr, sf_length_8kHz );\r
293         for( j = 0; j < length_d_comp; j++ ) {\r
294             d = d_comp[ j ];\r
295             basis_ptr = target_ptr - d;\r
296             cross_corr = silk_inner_product_FLP( basis_ptr, target_ptr, sf_length_8kHz );\r
297             energy     = silk_energy_FLP( basis_ptr, sf_length_8kHz );\r
298             if (cross_corr > 0.0f) {\r
299                 C[ k ][ d ] = (SKP_float)(cross_corr * cross_corr / (energy * energy_tmp + eps));\r
300             } else {\r
301                 C[ k ][ d ] = 0.0f;\r
302             }\r
303         }\r
304         target_ptr += sf_length_8kHz;\r
305     }\r
306 \r
307     /* search over lag range and lags codebook */\r
308     /* scale factor for lag codebook, as a function of center lag */\r
309 \r
310     CCmax   = 0.0f; /* This value doesn't matter */\r
311     CCmax_b = -1000.0f;\r
312 \r
313     CBimax = 0; /* To avoid returning undefined lag values */\r
314     lag = -1;   /* To check if lag with strong enough correlation has been found */\r
315 \r
316     if( prevLag > 0 ) {\r
317         if( Fs_kHz == 12 ) {\r
318             prevLag = SKP_LSHIFT( prevLag, 1 ) / 3;\r
319         } else if( Fs_kHz == 16 ) {\r
320             prevLag = SKP_RSHIFT( prevLag, 1 );\r
321         }\r
322         prevLag_log2 = silk_log2((SKP_float)prevLag);\r
323     } else {\r
324         prevLag_log2 = 0;\r
325     }\r
326 \r
327     /* Setup stage 2 codebook based on number of subframes */\r
328     if( nb_subfr == PE_MAX_NB_SUBFR ) {\r
329         cbk_size   = PE_NB_CBKS_STAGE2_EXT;\r
330         Lag_CB_ptr = &silk_CB_lags_stage2[ 0 ][ 0 ];\r
331         if( Fs_kHz == 8 && complexity > SigProc_PE_MIN_COMPLEX ) {\r
332             /* If input is 8 khz use a larger codebook here because it is last stage */\r
333             nb_cbk_search = PE_NB_CBKS_STAGE2_EXT;\r
334         } else {\r
335             nb_cbk_search = PE_NB_CBKS_STAGE2;\r
336         }\r
337     } else {\r
338         cbk_size       = PE_NB_CBKS_STAGE2_10MS;\r
339         Lag_CB_ptr     = &silk_CB_lags_stage2_10_ms[ 0 ][ 0 ];\r
340         nb_cbk_search  = PE_NB_CBKS_STAGE2_10MS;\r
341     }\r
342 \r
343     for( k = 0; k < length_d_srch; k++ ) {\r
344         d = d_srch[ k ];\r
345         for( j = 0; j < nb_cbk_search; j++ ) {\r
346             CC[j] = 0.0f;\r
347             for( i = 0; i < nb_subfr; i++ ) {\r
348                 /* Try all codebooks */\r
349                 CC[ j ] += C[ i ][ d + matrix_ptr( Lag_CB_ptr, i, j, cbk_size )];\r
350             }\r
351         }\r
352         /* Find best codebook */\r
353         CCmax_new  = -1000.0f;\r
354         CBimax_new = 0;\r
355         for( i = 0; i < nb_cbk_search; i++ ) {\r
356             if( CC[ i ] > CCmax_new ) {\r
357                 CCmax_new = CC[ i ];\r
358                 CBimax_new = i;\r
359             }\r
360         }\r
361         CCmax_new = SKP_max_float(CCmax_new, 0.0f); /* To avoid taking square root of negative number later */\r
362         CCmax_new_b = CCmax_new;\r
363 \r
364         /* Bias towards shorter lags */\r
365         lag_log2 = silk_log2((SKP_float)d);\r
366         CCmax_new_b -= PE_SHORTLAG_BIAS * nb_subfr * lag_log2;\r
367 \r
368         /* Bias towards previous lag */\r
369         if ( prevLag > 0 ) {\r
370             delta_lag_log2_sqr = lag_log2 - prevLag_log2;\r
371             delta_lag_log2_sqr *= delta_lag_log2_sqr;\r
372             CCmax_new_b -= PE_PREVLAG_BIAS * nb_subfr * (*LTPCorr) * delta_lag_log2_sqr / (delta_lag_log2_sqr + 0.5f);\r
373         }\r
374 \r
375         if ( CCmax_new_b > CCmax_b                                      && /* Find maximum biased correlation                  */\r
376              CCmax_new > nb_subfr * search_thres2 * search_thres2       && /* Correlation needs to be high enough to be voiced */\r
377              silk_CB_lags_stage2[ 0 ][ CBimax_new ] <= min_lag_8kHz    /* Lag must be in range                             */\r
378             ) {\r
379             CCmax_b = CCmax_new_b;\r
380             CCmax   = CCmax_new;\r
381             lag     = d;\r
382             CBimax  = CBimax_new;\r
383         }\r
384     }\r
385 \r
386     if( lag == -1 ) {\r
387         /* No suitable candidate found */\r
388         SKP_memset( pitch_out, 0, PE_MAX_NB_SUBFR * sizeof(SKP_int) );\r
389         *LTPCorr      = 0.0f;\r
390         *lagIndex     = 0;\r
391         *contourIndex = 0;\r
392         return 1;\r
393     }\r
394 \r
395     if( Fs_kHz > 8 ) {\r
396         /* Search in original signal */\r
397 \r
398         /* Compensate for decimation */\r
399         SKP_assert( lag == SKP_SAT16( lag ) );\r
400         if( Fs_kHz == 12 ) {\r
401             lag = SKP_RSHIFT_ROUND( SKP_SMULBB( lag, 3 ), 1 );\r
402         } else if( Fs_kHz == 16 ) {\r
403             lag = SKP_LSHIFT( lag, 1 );\r
404         } else {\r
405             lag = SKP_SMULBB( lag, 3 );\r
406         }\r
407 \r
408         lag = SKP_LIMIT_int( lag, min_lag, max_lag );\r
409         start_lag = SKP_max_int( lag - 2, min_lag );\r
410         end_lag   = SKP_min_int( lag + 2, max_lag );\r
411         lag_new   = lag;                                    /* to avoid undefined lag */\r
412         CBimax    = 0;                                      /* to avoid undefined lag */\r
413         SKP_assert( CCmax >= 0.0f ); \r
414         *LTPCorr = (SKP_float)sqrt( CCmax / nb_subfr );     /* Output normalized correlation */\r
415 \r
416         CCmax = -1000.0f;\r
417 \r
418         /* Calculate the correlations and energies needed in stage 3 */\r
419         silk_P_Ana_calc_corr_st3( cross_corr_st3, signal, start_lag, sf_length, nb_subfr, complexity );\r
420         silk_P_Ana_calc_energy_st3( energies_st3, signal, start_lag, sf_length, nb_subfr, complexity );\r
421 \r
422         lag_counter = 0;\r
423         SKP_assert( lag == SKP_SAT16( lag ) );\r
424         contour_bias = PE_FLATCONTOUR_BIAS / lag;\r
425 \r
426         /* Setup cbk parameters acording to complexity setting and frame length */\r
427         if( nb_subfr == PE_MAX_NB_SUBFR ) {\r
428             nb_cbk_search = (SKP_int)silk_nb_cbk_searchs_stage3[ complexity ];\r
429             cbk_size      = PE_NB_CBKS_STAGE3_MAX;\r
430             Lag_CB_ptr    = &silk_CB_lags_stage3[ 0 ][ 0 ];\r
431         } else {\r
432             nb_cbk_search = PE_NB_CBKS_STAGE3_10MS;\r
433             cbk_size      = PE_NB_CBKS_STAGE3_10MS;\r
434             Lag_CB_ptr    = &silk_CB_lags_stage3_10_ms[ 0 ][ 0 ];\r
435         }\r
436 \r
437         for( d = start_lag; d <= end_lag; d++ ) {\r
438             for( j = 0; j < nb_cbk_search; j++ ) {\r
439                 cross_corr = 0.0;\r
440                 energy = eps;\r
441                 for( k = 0; k < nb_subfr; k++ ) {\r
442                     energy     +=   energies_st3[ k ][ j ][ lag_counter ];\r
443                     cross_corr += cross_corr_st3[ k ][ j ][ lag_counter ];\r
444                 }\r
445                 if( cross_corr > 0.0 ) {\r
446                     CCmax_new = (SKP_float)(cross_corr * cross_corr / energy);\r
447                     /* Reduce depending on flatness of contour */\r
448                     CCmax_new *= 1.0f - contour_bias * j;\r
449                 } else {\r
450                     CCmax_new = 0.0f;               \r
451                 }\r
452 \r
453                 if( CCmax_new > CCmax &&\r
454                    ( d + (SKP_int)silk_CB_lags_stage3[ 0 ][ j ] ) <= max_lag  \r
455                    ) {\r
456                     CCmax   = CCmax_new;\r
457                     lag_new = d;\r
458                     CBimax  = j;\r
459                 }\r
460             }\r
461             lag_counter++;\r
462         }\r
463 \r
464         for( k = 0; k < nb_subfr; k++ ) {\r
465             pitch_out[ k ] = lag_new + matrix_ptr( Lag_CB_ptr, k, CBimax, cbk_size );\r
466         }\r
467         *lagIndex = (SKP_int16)( lag_new - min_lag );\r
468         *contourIndex = (SKP_int8)CBimax;\r
469     } else {\r
470         /* Save Lags and correlation */\r
471         SKP_assert( CCmax >= 0.0f );\r
472         *LTPCorr = (SKP_float)sqrt( CCmax / nb_subfr ); /* Output normalized correlation */\r
473         for( k = 0; k < nb_subfr; k++ ) {\r
474             pitch_out[ k ] = lag + matrix_ptr( Lag_CB_ptr, k, CBimax, cbk_size );\r
475         }\r
476         *lagIndex = (SKP_int16)( lag - min_lag );\r
477         *contourIndex = (SKP_int8)CBimax;\r
478     }\r
479     SKP_assert( *lagIndex >= 0 );\r
480     /* return as voiced */\r
481     return 0;\r
482 }\r
483 \r
484 static void silk_P_Ana_calc_corr_st3(\r
485     SKP_float cross_corr_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ], /* O 3 DIM correlation array */\r
486     const SKP_float signal[],           /* I vector to correlate                                            */\r
487     SKP_int         start_lag,          /* I start lag                                                      */\r
488     SKP_int         sf_length,          /* I sub frame length                                               */\r
489     SKP_int         nb_subfr,           /* I number of subframes                                            */\r
490     SKP_int         complexity          /* I Complexity setting                                             */\r
491 )\r
492     /***********************************************************************\r
493      Calculates the correlations used in stage 3 search. In order to cover \r
494      the whole lag codebook for all the searched offset lags (lag +- 2), \r
495      the following correlations are needed in each sub frame:\r
496 \r
497      sf1: lag range [-8,...,7] total 16 correlations\r
498      sf2: lag range [-4,...,4] total 9 correlations\r
499      sf3: lag range [-3,....4] total 8 correltions\r
500      sf4: lag range [-6,....8] total 15 correlations\r
501 \r
502      In total 48 correlations. The direct implementation computed in worst case \r
503      4*12*5 = 240 correlations, but more likely around 120. \r
504      **********************************************************************/\r
505 {\r
506     const SKP_float *target_ptr, *basis_ptr;\r
507     SKP_int   i, j, k, lag_counter, lag_low, lag_high;\r
508     SKP_int   nb_cbk_search, delta, idx, cbk_size;\r
509     SKP_float scratch_mem[ SCRATCH_SIZE ];\r
510     const SKP_int8 *Lag_range_ptr, *Lag_CB_ptr;\r
511 \r
512     SKP_assert( complexity >= SigProc_PE_MIN_COMPLEX );\r
513     SKP_assert( complexity <= SigProc_PE_MAX_COMPLEX );\r
514 \r
515     if( nb_subfr == PE_MAX_NB_SUBFR ){\r
516         Lag_range_ptr = &silk_Lag_range_stage3[ complexity ][ 0 ][ 0 ];\r
517         Lag_CB_ptr    = &silk_CB_lags_stage3[ 0 ][ 0 ];\r
518         nb_cbk_search = silk_nb_cbk_searchs_stage3[ complexity ];\r
519         cbk_size      = PE_NB_CBKS_STAGE3_MAX;\r
520     } else {\r
521         SKP_assert( nb_subfr == PE_MAX_NB_SUBFR >> 1);\r
522         Lag_range_ptr = &silk_Lag_range_stage3_10_ms[ 0 ][ 0 ];\r
523         Lag_CB_ptr    = &silk_CB_lags_stage3_10_ms[ 0 ][ 0 ];\r
524         nb_cbk_search = PE_NB_CBKS_STAGE3_10MS;\r
525         cbk_size      = PE_NB_CBKS_STAGE3_10MS;\r
526     }\r
527 \r
528     target_ptr = &signal[ SKP_LSHIFT( sf_length, 2 ) ]; /* Pointer to middle of frame */\r
529     for( k = 0; k < nb_subfr; k++ ) {\r
530         lag_counter = 0;\r
531 \r
532         /* Calculate the correlations for each subframe */\r
533         lag_low  = matrix_ptr( Lag_range_ptr, k, 0, 2 );\r
534         lag_high = matrix_ptr( Lag_range_ptr, k, 1, 2 );\r
535         for( j = lag_low; j <= lag_high; j++ ) {\r
536             basis_ptr = target_ptr - ( start_lag + j );\r
537             SKP_assert( lag_counter < SCRATCH_SIZE );\r
538             scratch_mem[ lag_counter ] = (SKP_float)silk_inner_product_FLP( target_ptr, basis_ptr, sf_length );\r
539             lag_counter++;\r
540         }\r
541 \r
542         delta = matrix_ptr( Lag_range_ptr, k, 0, 2 );\r
543         for( i = 0; i < nb_cbk_search; i++ ) { \r
544             /* Fill out the 3 dim array that stores the correlations for */\r
545             /* each code_book vector for each start lag */\r
546             idx = matrix_ptr( Lag_CB_ptr, k, i, cbk_size ) - delta;\r
547             for( j = 0; j < PE_NB_STAGE3_LAGS; j++ ) {\r
548                 SKP_assert( idx + j < SCRATCH_SIZE );\r
549                 SKP_assert( idx + j < lag_counter );\r
550                 cross_corr_st3[ k ][ i ][ j ] = scratch_mem[ idx + j ];\r
551             }\r
552         }\r
553         target_ptr += sf_length;\r
554     }\r
555 }\r
556 \r
557 static void silk_P_Ana_calc_energy_st3(\r
558     SKP_float energies_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ], /* O 3 DIM correlation array */\r
559     const SKP_float signal[],           /* I vector to correlate                                            */\r
560     SKP_int         start_lag,          /* I start lag                                                      */\r
561     SKP_int         sf_length,          /* I sub frame length                                               */\r
562     SKP_int         nb_subfr,           /* I number of subframes                                            */\r
563     SKP_int         complexity          /* I Complexity setting                                             */\r
564 )\r
565 /****************************************************************\r
566 Calculate the energies for first two subframes. The energies are\r
567 calculated recursively. \r
568 ****************************************************************/\r
569 {\r
570     const SKP_float *target_ptr, *basis_ptr;\r
571     double    energy;\r
572     SKP_int   k, i, j, lag_counter;\r
573     SKP_int   nb_cbk_search, delta, idx, cbk_size, lag_diff;\r
574     SKP_float scratch_mem[ SCRATCH_SIZE ];\r
575     const SKP_int8 *Lag_range_ptr, *Lag_CB_ptr;\r
576 \r
577     SKP_assert( complexity >= SigProc_PE_MIN_COMPLEX );\r
578     SKP_assert( complexity <= SigProc_PE_MAX_COMPLEX );\r
579 \r
580     if( nb_subfr == PE_MAX_NB_SUBFR ){\r
581         Lag_range_ptr = &silk_Lag_range_stage3[ complexity ][ 0 ][ 0 ];\r
582         Lag_CB_ptr    = &silk_CB_lags_stage3[ 0 ][ 0 ];\r
583         nb_cbk_search = silk_nb_cbk_searchs_stage3[ complexity ];\r
584         cbk_size      = PE_NB_CBKS_STAGE3_MAX;\r
585     } else {\r
586         SKP_assert( nb_subfr == PE_MAX_NB_SUBFR >> 1);\r
587         Lag_range_ptr = &silk_Lag_range_stage3_10_ms[ 0 ][ 0 ];\r
588         Lag_CB_ptr    = &silk_CB_lags_stage3_10_ms[ 0 ][ 0 ];\r
589         nb_cbk_search = PE_NB_CBKS_STAGE3_10MS;\r
590         cbk_size      = PE_NB_CBKS_STAGE3_10MS;\r
591     }\r
592 \r
593     target_ptr = &signal[ SKP_LSHIFT( sf_length, 2 ) ];\r
594     for( k = 0; k < nb_subfr; k++ ) {\r
595         lag_counter = 0;\r
596 \r
597         /* Calculate the energy for first lag */\r
598         basis_ptr = target_ptr - ( start_lag + matrix_ptr( Lag_range_ptr, k, 0, 2 ) );\r
599         energy = silk_energy_FLP( basis_ptr, sf_length ) + 1e-3;\r
600         SKP_assert( energy >= 0.0 );\r
601         scratch_mem[lag_counter] = (SKP_float)energy;\r
602         lag_counter++;\r
603 \r
604         lag_diff = ( matrix_ptr( Lag_range_ptr, k, 1, 2 ) -  matrix_ptr( Lag_range_ptr, k, 0, 2 ) + 1 );\r
605         for( i = 1; i < lag_diff; i++ ) {\r
606             /* remove part outside new window */\r
607             energy -= basis_ptr[sf_length - i] * basis_ptr[sf_length - i];\r
608             SKP_assert( energy >= 0.0 );\r
609 \r
610             /* add part that comes into window */\r
611             energy += basis_ptr[ -i ] * basis_ptr[ -i ];\r
612             SKP_assert( energy >= 0.0 );\r
613             SKP_assert( lag_counter < SCRATCH_SIZE );\r
614             scratch_mem[lag_counter] = (SKP_float)energy;\r
615             lag_counter++;\r
616         }\r
617 \r
618         delta = matrix_ptr( Lag_range_ptr, k, 0, 2 );\r
619         for( i = 0; i < nb_cbk_search; i++ ) { \r
620             /* Fill out the 3 dim array that stores the correlations for    */\r
621             /* each code_book vector for each start lag                     */\r
622             idx = matrix_ptr( Lag_CB_ptr, k, i, cbk_size ) - delta;\r
623             for( j = 0; j < PE_NB_STAGE3_LAGS; j++ ) {\r
624                 SKP_assert( idx + j < SCRATCH_SIZE );\r
625                 SKP_assert( idx + j < lag_counter );\r
626                 energies_st3[ k ][ i ][ j ] = scratch_mem[ idx + j ];\r
627                 SKP_assert( energies_st3[ k ][ i ][ j ] >= 0.0f );\r
628             }\r
629         }\r
630         target_ptr += sf_length;\r
631     }\r
632 }\r