Fix some warnings.
[opus.git] / silk / fixed / pitch_analysis_core_FIX.c
1 /***********************************************************************
2 Copyright (c) 2006-2011, Skype Limited. All rights reserved.
3 Redistribution and use in source and binary forms, with or without
4 modification, are permitted provided that the following conditions
5 are met:
6 - Redistributions of source code must retain the above copyright notice,
7 this list of conditions and the following disclaimer.
8 - Redistributions in binary form must reproduce the above copyright
9 notice, this list of conditions and the following disclaimer in the
10 documentation and/or other materials provided with the distribution.
11 - Neither the name of Internet Society, IETF or IETF Trust, nor the 
12 names of specific contributors, may be used to endorse or promote
13 products derived from this software without specific prior written
14 permission.
15 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS “AS IS”
16 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18 ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
19 LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
20 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
21 SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
22 INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
23 CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
24 ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
25 POSSIBILITY OF SUCH DAMAGE.
26 ***********************************************************************/
27
28 #ifdef HAVE_CONFIG_H
29 #include "config.h"
30 #endif
31
32 /***********************************************************
33 * Pitch analyser function
34 ********************************************************** */
35 #include "SigProc_FIX.h"
36 #include "pitch_est_defines.h"
37 #include "debug.h"
38
39 #define SCRATCH_SIZE    22
40
41 /************************************************************/
42 /* Internally used functions                                */
43 /************************************************************/
44 static void silk_P_Ana_calc_corr_st3(
45     opus_int32        cross_corr_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ],/* O 3 DIM correlation array */
46     const opus_int16  frame[],                         /* I vector to correlate         */
47     opus_int          start_lag,                       /* I lag offset to search around */
48     opus_int          sf_length,                       /* I length of a 5 ms subframe   */
49     opus_int          nb_subfr,                        /* I number of subframes         */
50     opus_int          complexity                       /* I Complexity setting          */
51 );
52
53 static void silk_P_Ana_calc_energy_st3(
54     opus_int32        energies_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ],/* O 3 DIM energy array */
55     const opus_int16  frame[],                         /* I vector to calc energy in    */
56     opus_int          start_lag,                       /* I lag offset to search around */
57     opus_int          sf_length,                       /* I length of one 5 ms subframe */
58     opus_int          nb_subfr,                        /* I number of subframes         */
59     opus_int          complexity                       /* I Complexity setting          */
60 );
61
62 /*************************************************************/
63 /*      FIXED POINT CORE PITCH ANALYSIS FUNCTION             */
64 /*************************************************************/
65 opus_int silk_pitch_analysis_core(                  /* O    Voicing estimate: 0 voiced, 1 unvoiced                      */
66     const opus_int16            *frame,             /* I    Signal of length PE_FRAME_LENGTH_MS*Fs_kHz                  */
67     opus_int                    *pitch_out,         /* O    4 pitch lag values                                          */
68     opus_int16                  *lagIndex,          /* O    Lag Index                                                   */
69     opus_int8                   *contourIndex,      /* O    Pitch contour Index                                         */
70     opus_int                    *LTPCorr_Q15,       /* I/O  Normalized correlation; input: value from previous frame    */
71     opus_int                    prevLag,            /* I    Last lag of previous frame; set to zero is unvoiced         */
72     const opus_int32            search_thres1_Q16,  /* I    First stage threshold for lag candidates 0 - 1              */
73     const opus_int              search_thres2_Q13,  /* I    Final threshold for lag candidates 0 - 1                    */
74     const opus_int              Fs_kHz,             /* I    Sample frequency (kHz)                                      */
75     const opus_int              complexity,         /* I    Complexity setting, 0-2, where 2 is highest                 */
76     const opus_int              nb_subfr            /* I    number of 5 ms subframes                                    */
77 )
78 {
79     opus_int16 frame_8kHz[ PE_MAX_FRAME_LENGTH_ST_2 ];
80     opus_int16 frame_4kHz[ PE_MAX_FRAME_LENGTH_ST_1 ];
81     opus_int32 filt_state[ 6 ];
82     opus_int32 scratch_mem[ 3 * PE_MAX_FRAME_LENGTH ];
83     opus_int16 *input_frame_ptr;
84     opus_int   i, k, d, j;
85     opus_int16 C[ PE_MAX_NB_SUBFR ][ ( PE_MAX_LAG >> 1 ) + 5 ];
86     const opus_int16 *target_ptr, *basis_ptr;
87     opus_int32 cross_corr, normalizer, energy, shift, energy_basis, energy_target;
88     opus_int   d_srch[ PE_D_SRCH_LENGTH ], Cmax, length_d_srch, length_d_comp;
89     opus_int16 d_comp[ ( PE_MAX_LAG >> 1 ) + 5 ];
90     opus_int32 sum, threshold, lag_counter;
91     opus_int   CBimax, CBimax_new, CBimax_old, lag, start_lag, end_lag, lag_new;
92     opus_int32 CC[ PE_NB_CBKS_STAGE2_EXT ], CCmax, CCmax_b, CCmax_new_b, CCmax_new;
93     opus_int32 energies_st3[  PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ];
94     opus_int32 crosscorr_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ];
95     opus_int   frame_length, frame_length_8kHz, frame_length_4kHz;
96     opus_int   sf_length, sf_length_8kHz, sf_length_4kHz;
97     opus_int   min_lag, min_lag_8kHz, min_lag_4kHz;
98     opus_int   max_lag, max_lag_8kHz, max_lag_4kHz;
99     opus_int32 contour_bias_Q15, diff;
100     opus_int   nb_cbk_search, cbk_size;
101     opus_int32 delta_lag_log2_sqr_Q7, lag_log2_Q7, prevLag_log2_Q7, prev_lag_bias_Q13;
102     const opus_int8 *Lag_CB_ptr;
103     /* Check for valid sampling frequency */
104     silk_assert( Fs_kHz == 8 || Fs_kHz == 12 || Fs_kHz == 16 );
105
106     /* Check for valid complexity setting */
107     silk_assert( complexity >= SILK_PE_MIN_COMPLEX );
108     silk_assert( complexity <= SILK_PE_MAX_COMPLEX );
109
110     silk_assert( search_thres1_Q16 >= 0 && search_thres1_Q16 <= (1<<16) );
111     silk_assert( search_thres2_Q13 >= 0 && search_thres2_Q13 <= (1<<13) );
112
113     /* Set up frame lengths max / min lag for the sampling frequency */
114     frame_length      = ( PE_LTP_MEM_LENGTH_MS + nb_subfr * PE_SUBFR_LENGTH_MS ) * Fs_kHz;
115     frame_length_4kHz = ( PE_LTP_MEM_LENGTH_MS + nb_subfr * PE_SUBFR_LENGTH_MS ) * 4;
116     frame_length_8kHz = ( PE_LTP_MEM_LENGTH_MS + nb_subfr * PE_SUBFR_LENGTH_MS ) * 8;
117     sf_length         = PE_SUBFR_LENGTH_MS * Fs_kHz;
118     sf_length_4kHz    = PE_SUBFR_LENGTH_MS * 4;
119     sf_length_8kHz    = PE_SUBFR_LENGTH_MS * 8;
120     min_lag           = PE_MIN_LAG_MS * Fs_kHz;
121     min_lag_4kHz      = PE_MIN_LAG_MS * 4;
122     min_lag_8kHz      = PE_MIN_LAG_MS * 8;
123     max_lag           = PE_MAX_LAG_MS * Fs_kHz - 1;
124     max_lag_4kHz      = PE_MAX_LAG_MS * 4;
125     max_lag_8kHz      = PE_MAX_LAG_MS * 8 - 1;
126
127     /* Resample from input sampled at Fs_kHz to 8 kHz */
128     if( Fs_kHz == 16 ) {
129         silk_memset( filt_state, 0, 2 * sizeof( opus_int32 ) );
130         silk_resampler_down2( filt_state, frame_8kHz, frame, frame_length );
131     } else if( Fs_kHz == 12 ) {
132         silk_memset( filt_state, 0, 6 * sizeof( opus_int32 ) );
133         silk_resampler_down2_3( filt_state, frame_8kHz, frame, frame_length );
134     } else {
135         silk_assert( Fs_kHz == 8 );
136         silk_memcpy( frame_8kHz, frame, frame_length_8kHz * sizeof(opus_int16) );
137     }
138
139     /* Decimate again to 4 kHz */
140     silk_memset( filt_state, 0, 2 * sizeof( opus_int32 ) );/* Set state to zero */
141     silk_resampler_down2( filt_state, frame_4kHz, frame_8kHz, frame_length_8kHz );
142
143     /* Low-pass filter */
144     for( i = frame_length_4kHz - 1; i > 0; i-- ) {
145         frame_4kHz[ i ] = silk_ADD_SAT16( frame_4kHz[ i ], frame_4kHz[ i - 1 ] );
146     }
147
148     /*******************************************************************************
149     ** Scale 4 kHz signal down to prevent correlations measures from overflowing
150     ** find scaling as max scaling for each 8kHz(?) subframe
151     *******************************************************************************/
152
153     /* Inner product is calculated with different lengths, so scale for the worst case */
154     silk_sum_sqr_shift( &energy, &shift, frame_4kHz, frame_length_4kHz );
155     if( shift > 0 ) {
156         shift = silk_RSHIFT( shift, 1 );
157         for( i = 0; i < frame_length_4kHz; i++ ) {
158             frame_4kHz[ i ] = silk_RSHIFT( frame_4kHz[ i ], shift );
159         }
160     }
161
162     /******************************************************************************
163     * FIRST STAGE, operating in 4 khz
164     ******************************************************************************/
165     silk_memset( C, 0, sizeof( opus_int16 ) * nb_subfr * ( ( PE_MAX_LAG >> 1 ) + 5) );
166     target_ptr = &frame_4kHz[ silk_LSHIFT( sf_length_4kHz, 2 ) ];
167     for( k = 0; k < nb_subfr >> 1; k++ ) {
168         /* Check that we are within range of the array */
169         silk_assert( target_ptr >= frame_4kHz );
170         silk_assert( target_ptr + sf_length_8kHz <= frame_4kHz + frame_length_4kHz );
171
172         basis_ptr = target_ptr - min_lag_4kHz;
173
174         /* Check that we are within range of the array */
175         silk_assert( basis_ptr >= frame_4kHz );
176         silk_assert( basis_ptr + sf_length_8kHz <= frame_4kHz + frame_length_4kHz );
177
178         /* Calculate first vector products before loop */
179         cross_corr = silk_inner_prod_aligned( target_ptr, basis_ptr,  sf_length_8kHz );
180         normalizer = silk_inner_prod_aligned( target_ptr, target_ptr, sf_length_8kHz );
181         normalizer = silk_ADD32( normalizer, silk_inner_prod_aligned( basis_ptr,  basis_ptr, sf_length_8kHz ) );
182         normalizer = silk_ADD32( normalizer, silk_SMULBB( sf_length_8kHz, 4000 ) );
183
184         C[ k ][ min_lag_4kHz ] = (opus_int16)silk_DIV32_varQ( cross_corr, normalizer, 13 + 1 ); /* Q13 */
185
186         /* From now on normalizer is computed recursively */
187         for( d = min_lag_4kHz + 1; d <= max_lag_4kHz; d++ ) {
188             basis_ptr--;
189
190             /* Check that we are within range of the array */
191             silk_assert( basis_ptr >= frame_4kHz );
192             silk_assert( basis_ptr + sf_length_8kHz <= frame_4kHz + frame_length_4kHz );
193
194             cross_corr = silk_inner_prod_aligned( target_ptr, basis_ptr, sf_length_8kHz );
195
196             /* Add contribution of new sample and remove contribution from oldest sample */
197             normalizer = silk_ADD32( normalizer,
198                 silk_SMULBB( basis_ptr[ 0 ], basis_ptr[ 0 ] ) -
199                 silk_SMULBB( basis_ptr[ sf_length_8kHz ], basis_ptr[ sf_length_8kHz ] ) );
200
201             C[ k ][ d ] = (opus_int16)silk_DIV32_varQ( cross_corr, normalizer, 13 + 1 );        /* Q13 */
202         }
203         /* Update target pointer */
204         target_ptr += sf_length_8kHz;
205     }
206
207     /* Combine two subframes into single correlation measure and apply short-lag bias */
208     if( nb_subfr == PE_MAX_NB_SUBFR ) {
209         for( i = max_lag_4kHz; i >= min_lag_4kHz; i-- ) {
210             sum = (opus_int32)C[ 0 ][ i ] + (opus_int32) C[ 1 ][ i ];                           /* Q14 */
211             sum = silk_SMLAWB( sum, sum, silk_LSHIFT( -i, 4 ) );                                /* Q14 */
212             C[ 0 ][ i ] = (opus_int16)sum;                                                      /* Q14 */
213         }
214     } else {
215         /* Only short-lag bias */
216         for( i = max_lag_4kHz; i >= min_lag_4kHz; i-- ) {
217             sum = silk_LSHIFT( (opus_int32)C[ 0 ][ i ], 1 );                                    /* Q14 */
218             sum = silk_SMLAWB( sum, sum, silk_LSHIFT( -i, 4 ) );                                /* Q14 */
219             C[ 0 ][ i ] = (opus_int16)sum;                                                      /* Q14 */
220         }
221     }
222
223     /* Sort */
224     length_d_srch = silk_ADD_LSHIFT32( 4, complexity, 1 );
225     silk_assert( 3 * length_d_srch <= PE_D_SRCH_LENGTH );
226     silk_insertion_sort_decreasing_int16( &C[ 0 ][ min_lag_4kHz ], d_srch, max_lag_4kHz - min_lag_4kHz + 1, length_d_srch );
227
228     /* Escape if correlation is very low already here */
229     Cmax = (opus_int)C[ 0 ][ min_lag_4kHz ];                                                    /* Q14 */
230     if( Cmax < SILK_FIX_CONST( 0.2, 14 ) ) {
231         silk_memset( pitch_out, 0, nb_subfr * sizeof( opus_int ) );
232         *LTPCorr_Q15  = 0;
233         *lagIndex     = 0;
234         *contourIndex = 0;
235         return 1;
236     }
237
238     threshold = silk_SMULWB( search_thres1_Q16, Cmax );
239     for( i = 0; i < length_d_srch; i++ ) {
240         /* Convert to 8 kHz indices for the sorted correlation that exceeds the threshold */
241         if( C[ 0 ][ min_lag_4kHz + i ] > threshold ) {
242             d_srch[ i ] = silk_LSHIFT( d_srch[ i ] + min_lag_4kHz, 1 );
243         } else {
244             length_d_srch = i;
245             break;
246         }
247     }
248     silk_assert( length_d_srch > 0 );
249
250     for( i = min_lag_8kHz - 5; i < max_lag_8kHz + 5; i++ ) {
251         d_comp[ i ] = 0;
252     }
253     for( i = 0; i < length_d_srch; i++ ) {
254         d_comp[ d_srch[ i ] ] = 1;
255     }
256
257     /* Convolution */
258     for( i = max_lag_8kHz + 3; i >= min_lag_8kHz; i-- ) {
259         d_comp[ i ] += d_comp[ i - 1 ] + d_comp[ i - 2 ];
260     }
261
262     length_d_srch = 0;
263     for( i = min_lag_8kHz; i < max_lag_8kHz + 1; i++ ) {
264         if( d_comp[ i + 1 ] > 0 ) {
265             d_srch[ length_d_srch ] = i;
266             length_d_srch++;
267         }
268     }
269
270     /* Convolution */
271     for( i = max_lag_8kHz + 3; i >= min_lag_8kHz; i-- ) {
272         d_comp[ i ] += d_comp[ i - 1 ] + d_comp[ i - 2 ] + d_comp[ i - 3 ];
273     }
274
275     length_d_comp = 0;
276     for( i = min_lag_8kHz; i < max_lag_8kHz + 4; i++ ) {
277         if( d_comp[ i ] > 0 ) {
278             d_comp[ length_d_comp ] = i - 2;
279             length_d_comp++;
280         }
281     }
282
283     /**********************************************************************************
284     ** SECOND STAGE, operating at 8 kHz, on lag sections with high correlation
285     *************************************************************************************/
286
287     /******************************************************************************
288     ** Scale signal down to avoid correlations measures from overflowing
289     *******************************************************************************/
290     /* find scaling as max scaling for each subframe */
291     silk_sum_sqr_shift( &energy, &shift, frame_8kHz, frame_length_8kHz );
292     if( shift > 0 ) {
293         shift = silk_RSHIFT( shift, 1 );
294         for( i = 0; i < frame_length_8kHz; i++ ) {
295             frame_8kHz[ i ] = silk_RSHIFT( frame_8kHz[ i ], shift );
296         }
297     }
298
299     /*********************************************************************************
300     * Find energy of each subframe projected onto its history, for a range of delays
301     *********************************************************************************/
302     silk_memset( C, 0, PE_MAX_NB_SUBFR * ( ( PE_MAX_LAG >> 1 ) + 5 ) * sizeof( opus_int16 ) );
303
304     target_ptr = &frame_8kHz[ PE_LTP_MEM_LENGTH_MS * 8 ];
305     for( k = 0; k < nb_subfr; k++ ) {
306
307         /* Check that we are within range of the array */
308         silk_assert( target_ptr >= frame_8kHz );
309         silk_assert( target_ptr + sf_length_8kHz <= frame_8kHz + frame_length_8kHz );
310
311         energy_target = silk_ADD32( silk_inner_prod_aligned( target_ptr, target_ptr, sf_length_8kHz ), 1 );
312         for( j = 0; j < length_d_comp; j++ ) {
313             d = d_comp[ j ];
314             basis_ptr = target_ptr - d;
315
316             /* Check that we are within range of the array */
317             silk_assert( basis_ptr >= frame_8kHz );
318             silk_assert( basis_ptr + sf_length_8kHz <= frame_8kHz + frame_length_8kHz );
319
320             cross_corr = silk_inner_prod_aligned( target_ptr, basis_ptr, sf_length_8kHz );
321             if( cross_corr > 0 ) {
322                 energy_basis = silk_inner_prod_aligned( basis_ptr, basis_ptr, sf_length_8kHz );
323                 C[ k ][ d ] = (opus_int16)silk_DIV32_varQ( cross_corr, silk_ADD32( energy_target, energy_basis ), 13 + 1 );        /* Q13 */
324             } else {
325                 C[ k ][ d ] = 0;
326             }
327         }
328         target_ptr += sf_length_8kHz;
329     }
330
331     /* search over lag range and lags codebook */
332     /* scale factor for lag codebook, as a function of center lag */
333
334     CCmax   = silk_int32_MIN;
335     CCmax_b = silk_int32_MIN;
336
337     CBimax = 0; /* To avoid returning undefined lag values */
338     lag = -1;   /* To check if lag with strong enough correlation has been found */
339
340     if( prevLag > 0 ) {
341         if( Fs_kHz == 12 ) {
342             prevLag = silk_DIV32_16( silk_LSHIFT( prevLag, 1 ), 3 );
343         } else if( Fs_kHz == 16 ) {
344             prevLag = silk_RSHIFT( prevLag, 1 );
345         }
346         prevLag_log2_Q7 = silk_lin2log( (opus_int32)prevLag );
347     } else {
348         prevLag_log2_Q7 = 0;
349     }
350     silk_assert( search_thres2_Q13 == silk_SAT16( search_thres2_Q13 ) );
351     /* Set up stage 2 codebook based on number of subframes */
352     if( nb_subfr == PE_MAX_NB_SUBFR ) {
353         cbk_size   = PE_NB_CBKS_STAGE2_EXT;
354         Lag_CB_ptr = &silk_CB_lags_stage2[ 0 ][ 0 ];
355         if( Fs_kHz == 8 && complexity > SILK_PE_MIN_COMPLEX ) {
356             /* If input is 8 khz use a larger codebook here because it is last stage */
357             nb_cbk_search = PE_NB_CBKS_STAGE2_EXT;
358         } else {
359             nb_cbk_search = PE_NB_CBKS_STAGE2;
360         }
361     } else {
362         cbk_size       = PE_NB_CBKS_STAGE2_10MS;
363         Lag_CB_ptr     = &silk_CB_lags_stage2_10_ms[ 0 ][ 0 ];
364         nb_cbk_search  = PE_NB_CBKS_STAGE2_10MS;
365     }
366
367     for( k = 0; k < length_d_srch; k++ ) {
368         d = d_srch[ k ];
369         for( j = 0; j < nb_cbk_search; j++ ) {
370             CC[ j ] = 0;
371             for( i = 0; i < nb_subfr; i++ ) {
372                 /* Try all codebooks */
373                 CC[ j ] = CC[ j ] + (opus_int32)C[ i ][ d + matrix_ptr( Lag_CB_ptr, i, j, cbk_size ) ];
374             }
375         }
376         /* Find best codebook */
377         CCmax_new = silk_int32_MIN;
378         CBimax_new = 0;
379         for( i = 0; i < nb_cbk_search; i++ ) {
380             if( CC[ i ] > CCmax_new ) {
381                 CCmax_new = CC[ i ];
382                 CBimax_new = i;
383             }
384         }
385
386         /* Bias towards shorter lags */
387         lag_log2_Q7 = silk_lin2log( d ); /* Q7 */
388         silk_assert( lag_log2_Q7 == silk_SAT16( lag_log2_Q7 ) );
389         silk_assert( nb_subfr * SILK_FIX_CONST( PE_SHORTLAG_BIAS, 13 ) == silk_SAT16( nb_subfr * SILK_FIX_CONST( PE_SHORTLAG_BIAS, 13 ) ) );
390         CCmax_new_b = CCmax_new - silk_RSHIFT( silk_SMULBB( nb_subfr * SILK_FIX_CONST( PE_SHORTLAG_BIAS, 13 ), lag_log2_Q7 ), 7 ); /* Q13 */
391
392         /* Bias towards previous lag */
393         silk_assert( nb_subfr * SILK_FIX_CONST( PE_PREVLAG_BIAS, 13 ) == silk_SAT16( nb_subfr * SILK_FIX_CONST( PE_PREVLAG_BIAS, 13 ) ) );
394         if( prevLag > 0 ) {
395             delta_lag_log2_sqr_Q7 = lag_log2_Q7 - prevLag_log2_Q7;
396             silk_assert( delta_lag_log2_sqr_Q7 == silk_SAT16( delta_lag_log2_sqr_Q7 ) );
397             delta_lag_log2_sqr_Q7 = silk_RSHIFT( silk_SMULBB( delta_lag_log2_sqr_Q7, delta_lag_log2_sqr_Q7 ), 7 );
398             prev_lag_bias_Q13 = silk_RSHIFT( silk_SMULBB( nb_subfr * SILK_FIX_CONST( PE_PREVLAG_BIAS, 13 ), *LTPCorr_Q15 ), 15 ); /* Q13 */
399             prev_lag_bias_Q13 = silk_DIV32( silk_MUL( prev_lag_bias_Q13, delta_lag_log2_sqr_Q7 ), delta_lag_log2_sqr_Q7 + SILK_FIX_CONST( 0.5, 7 ) );
400             CCmax_new_b -= prev_lag_bias_Q13; /* Q13 */
401         }
402
403         if( CCmax_new_b > CCmax_b                                   &&  /* Find maximum biased correlation                  */
404             CCmax_new > silk_SMULBB( nb_subfr, search_thres2_Q13 )  &&  /* Correlation needs to be high enough to be voiced */
405             silk_CB_lags_stage2[ 0 ][ CBimax_new ] <= min_lag_8kHz      /* Lag must be in range                             */
406          ) {
407             CCmax_b = CCmax_new_b;
408             CCmax   = CCmax_new;
409             lag     = d;
410             CBimax  = CBimax_new;
411         }
412     }
413
414     if( lag == -1 ) {
415         /* No suitable candidate found */
416         silk_memset( pitch_out, 0, nb_subfr * sizeof( opus_int ) );
417         *LTPCorr_Q15  = 0;
418         *lagIndex     = 0;
419         *contourIndex = 0;
420         return 1;
421     }
422
423     /* Output normalized correlation */
424     *LTPCorr_Q15 = (opus_int)silk_LSHIFT( silk_DIV32_16( CCmax, nb_subfr ), 2 ); 
425     silk_assert( *LTPCorr_Q15 >= 0 );
426
427     if( Fs_kHz > 8 ) {
428         /***************************************************************************/
429         /* Scale input signal down to avoid correlations measures from overflowing */
430         /***************************************************************************/
431         /* find scaling as max scaling for each subframe */
432         silk_sum_sqr_shift( &energy, &shift, frame, frame_length );
433         if( shift > 0 ) {
434             /* Move signal to scratch mem because the input signal should be unchanged */
435             /* Reuse the 32 bit scratch mem vector, use a 16 bit pointer from now */
436             shift = silk_RSHIFT( shift, 1 );
437             input_frame_ptr = (opus_int16*)scratch_mem;
438             for( i = 0; i < frame_length; i++ ) {
439                 input_frame_ptr[ i ] = silk_RSHIFT( frame[ i ], shift );
440             }
441         } else {
442             input_frame_ptr = (opus_int16*)frame;
443         }
444
445         /* Search in original signal */
446
447         CBimax_old = CBimax;
448         /* Compensate for decimation */
449         silk_assert( lag == silk_SAT16( lag ) );
450         if( Fs_kHz == 12 ) {
451             lag = silk_RSHIFT( silk_SMULBB( lag, 3 ), 1 );
452         } else if( Fs_kHz == 16 ) {
453             lag = silk_LSHIFT( lag, 1 );
454         } else {
455             lag = silk_SMULBB( lag, 3 );
456         }
457
458         lag = silk_LIMIT_int( lag, min_lag, max_lag );
459         start_lag = silk_max_int( lag - 2, min_lag );
460         end_lag   = silk_min_int( lag + 2, max_lag );
461         lag_new   = lag;                                    /* to avoid undefined lag */
462         CBimax    = 0;                                      /* to avoid undefined lag */
463
464         CCmax = silk_int32_MIN;
465         /* pitch lags according to second stage */
466         for( k = 0; k < nb_subfr; k++ ) {
467             pitch_out[ k ] = lag + 2 * silk_CB_lags_stage2[ k ][ CBimax_old ];
468         }
469         /* Calculate the correlations and energies needed in stage 3 */
470         silk_P_Ana_calc_corr_st3(  crosscorr_st3, input_frame_ptr, start_lag, sf_length, nb_subfr, complexity );
471         silk_P_Ana_calc_energy_st3( energies_st3, input_frame_ptr, start_lag, sf_length, nb_subfr, complexity );
472
473         lag_counter = 0;
474         silk_assert( lag == silk_SAT16( lag ) );
475         contour_bias_Q15 = silk_DIV32_16( SILK_FIX_CONST( PE_FLATCONTOUR_BIAS, 15 ), lag );
476
477         /* Set up codebook parameters according to complexity setting and frame length */
478         if( nb_subfr == PE_MAX_NB_SUBFR ) {
479             nb_cbk_search   = (opus_int)silk_nb_cbk_searchs_stage3[ complexity ];
480             cbk_size        = PE_NB_CBKS_STAGE3_MAX;
481             Lag_CB_ptr      = &silk_CB_lags_stage3[ 0 ][ 0 ];
482         } else {
483             nb_cbk_search   = PE_NB_CBKS_STAGE3_10MS;
484             cbk_size        = PE_NB_CBKS_STAGE3_10MS;
485             Lag_CB_ptr      = &silk_CB_lags_stage3_10_ms[ 0 ][ 0 ];
486         }
487
488         target_ptr = &input_frame_ptr[ PE_LTP_MEM_LENGTH_MS * Fs_kHz ];
489         energy_target = silk_ADD32( silk_inner_prod_aligned( target_ptr, target_ptr, nb_subfr * sf_length ), 1 );
490         for( d = start_lag; d <= end_lag; d++ ) {
491             for( j = 0; j < nb_cbk_search; j++ ) {
492                 cross_corr = 0;
493                 energy     = energy_target;
494                 for( k = 0; k < nb_subfr; k++ ) {
495                     cross_corr = silk_ADD32( cross_corr, crosscorr_st3[ k ][ j ][ lag_counter ] );
496                     energy     = silk_ADD32( energy, energies_st3[  k ][ j ][ lag_counter ] );
497                     silk_assert( energy >= 0 );
498                 }
499                 if( cross_corr > 0 ) {
500                     CCmax_new = silk_DIV32_varQ( cross_corr, energy, 13 + 1 );          /* Q13 */
501                     /* Reduce depending on flatness of contour */
502                     diff = silk_int16_MAX - silk_MUL( contour_bias_Q15, j );            /* Q15 */
503                     silk_assert( diff == silk_SAT16( diff ) );
504                     CCmax_new = silk_SMULWB( CCmax_new, diff );                         /* Q14 */
505                 } else {
506                     CCmax_new = 0;
507                 }
508
509                 if( CCmax_new > CCmax && ( d + silk_CB_lags_stage3[ 0 ][ j ] ) <= max_lag ) {
510                     CCmax   = CCmax_new;
511                     lag_new = d;
512                     CBimax  = j;
513                 }
514             }
515             lag_counter++;
516         }
517
518         for( k = 0; k < nb_subfr; k++ ) {
519             pitch_out[ k ] = lag_new + matrix_ptr( Lag_CB_ptr, k, CBimax, cbk_size );
520             pitch_out[ k ] = silk_LIMIT( pitch_out[ k ], min_lag, PE_MAX_LAG_MS * Fs_kHz );
521         }
522         *lagIndex = (opus_int16)( lag_new - min_lag);
523         *contourIndex = (opus_int8)CBimax;
524     } else {        /* Fs_kHz == 8 */
525         /* Save Lags */
526         for( k = 0; k < nb_subfr; k++ ) {
527             pitch_out[ k ] = lag + matrix_ptr( Lag_CB_ptr, k, CBimax, cbk_size );
528             pitch_out[ k ] = silk_LIMIT( pitch_out[ k ], min_lag_8kHz, PE_MAX_LAG_MS * 8 );
529         }
530         *lagIndex = (opus_int16)( lag - min_lag_8kHz );
531         *contourIndex = (opus_int8)CBimax;
532     }
533     silk_assert( *lagIndex >= 0 );
534     /* return as voiced */
535     return 0;
536 }
537
538 /***********************************************************************
539  * Calculates the correlations used in stage 3 search. In order to cover
540  * the whole lag codebook for all the searched offset lags (lag +- 2),
541  * the following correlations are needed in each sub frame:
542  *
543  * sf1: lag range [-8,...,7] total 16 correlations
544  * sf2: lag range [-4,...,4] total 9 correlations
545  * sf3: lag range [-3,....4] total 8 correltions
546  * sf4: lag range [-6,....8] total 15 correlations
547  *
548  * In total 48 correlations. The direct implementation computed in worst
549  * case 4*12*5 = 240 correlations, but more likely around 120.
550  ***********************************************************************/
551 static void silk_P_Ana_calc_corr_st3(
552     opus_int32        cross_corr_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ],/* O 3 DIM correlation array */
553     const opus_int16  frame[],                         /* I vector to correlate         */
554     opus_int          start_lag,                       /* I lag offset to search around */
555     opus_int          sf_length,                       /* I length of a 5 ms subframe   */
556     opus_int          nb_subfr,                        /* I number of subframes         */
557     opus_int          complexity                       /* I Complexity setting          */
558 )
559 {
560     const opus_int16 *target_ptr, *basis_ptr;
561     opus_int32 cross_corr;
562     opus_int   i, j, k, lag_counter, lag_low, lag_high;
563     opus_int   nb_cbk_search, delta, idx, cbk_size;
564     opus_int32 scratch_mem[ SCRATCH_SIZE ];
565     const opus_int8 *Lag_range_ptr, *Lag_CB_ptr;
566
567     silk_assert( complexity >= SILK_PE_MIN_COMPLEX );
568     silk_assert( complexity <= SILK_PE_MAX_COMPLEX );
569
570     if( nb_subfr == PE_MAX_NB_SUBFR ) {
571         Lag_range_ptr = &silk_Lag_range_stage3[ complexity ][ 0 ][ 0 ];
572         Lag_CB_ptr    = &silk_CB_lags_stage3[ 0 ][ 0 ];
573         nb_cbk_search = silk_nb_cbk_searchs_stage3[ complexity ];
574         cbk_size      = PE_NB_CBKS_STAGE3_MAX;
575     } else {
576         silk_assert( nb_subfr == PE_MAX_NB_SUBFR >> 1);
577         Lag_range_ptr = &silk_Lag_range_stage3_10_ms[ 0 ][ 0 ];
578         Lag_CB_ptr    = &silk_CB_lags_stage3_10_ms[ 0 ][ 0 ];
579         nb_cbk_search = PE_NB_CBKS_STAGE3_10MS;
580         cbk_size      = PE_NB_CBKS_STAGE3_10MS;
581     }
582
583     target_ptr = &frame[ silk_LSHIFT( sf_length, 2 ) ]; /* Pointer to middle of frame */
584     for( k = 0; k < nb_subfr; k++ ) {
585         lag_counter = 0;
586
587         /* Calculate the correlations for each subframe */
588         lag_low  = matrix_ptr( Lag_range_ptr, k, 0, 2 );
589         lag_high = matrix_ptr( Lag_range_ptr, k, 1, 2 );
590         for( j = lag_low; j <= lag_high; j++ ) {
591             basis_ptr = target_ptr - ( start_lag + j );
592             cross_corr = silk_inner_prod_aligned( target_ptr, basis_ptr, sf_length );
593             silk_assert( lag_counter < SCRATCH_SIZE );
594             scratch_mem[ lag_counter ] = cross_corr;
595             lag_counter++;
596         }
597
598         delta = matrix_ptr( Lag_range_ptr, k, 0, 2 );
599         for( i = 0; i < nb_cbk_search; i++ ) {
600             /* Fill out the 3 dim array that stores the correlations for */
601             /* each code_book vector for each start lag */
602             idx = matrix_ptr( Lag_CB_ptr, k, i, cbk_size ) - delta;
603             for( j = 0; j < PE_NB_STAGE3_LAGS; j++ ) {
604                 silk_assert( idx + j < SCRATCH_SIZE );
605                 silk_assert( idx + j < lag_counter );
606                 cross_corr_st3[ k ][ i ][ j ] = scratch_mem[ idx + j ];
607             }
608         }
609         target_ptr += sf_length;
610     }
611 }
612
613 /********************************************************************/
614 /* Calculate the energies for first two subframes. The energies are */
615 /* calculated recursively.                                          */
616 /********************************************************************/
617 static void silk_P_Ana_calc_energy_st3(
618     opus_int32        energies_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ],    /* O 3 DIM energy array */
619     const opus_int16  frame[],                          /* I vector to calc energy in    */
620     opus_int          start_lag,                        /* I lag offset to search around */
621     opus_int          sf_length,                        /* I length of one 5 ms subframe */
622     opus_int          nb_subfr,                         /* I number of subframes         */
623     opus_int          complexity                        /* I Complexity setting          */
624 )
625 {
626     const opus_int16 *target_ptr, *basis_ptr;
627     opus_int32 energy;
628     opus_int   k, i, j, lag_counter;
629     opus_int   nb_cbk_search, delta, idx, cbk_size, lag_diff;
630     opus_int32 scratch_mem[ SCRATCH_SIZE ];
631     const opus_int8 *Lag_range_ptr, *Lag_CB_ptr;
632
633     silk_assert( complexity >= SILK_PE_MIN_COMPLEX );
634     silk_assert( complexity <= SILK_PE_MAX_COMPLEX );
635
636     if( nb_subfr == PE_MAX_NB_SUBFR ) {
637         Lag_range_ptr = &silk_Lag_range_stage3[ complexity ][ 0 ][ 0 ];
638         Lag_CB_ptr    = &silk_CB_lags_stage3[ 0 ][ 0 ];
639         nb_cbk_search = silk_nb_cbk_searchs_stage3[ complexity ];
640         cbk_size      = PE_NB_CBKS_STAGE3_MAX;
641     } else {
642         silk_assert( nb_subfr == PE_MAX_NB_SUBFR >> 1);
643         Lag_range_ptr = &silk_Lag_range_stage3_10_ms[ 0 ][ 0 ];
644         Lag_CB_ptr    = &silk_CB_lags_stage3_10_ms[ 0 ][ 0 ];
645         nb_cbk_search = PE_NB_CBKS_STAGE3_10MS;
646         cbk_size      = PE_NB_CBKS_STAGE3_10MS;
647     }
648     target_ptr = &frame[ silk_LSHIFT( sf_length, 2 ) ];
649     for( k = 0; k < nb_subfr; k++ ) {
650         lag_counter = 0;
651
652         /* Calculate the energy for first lag */
653         basis_ptr = target_ptr - ( start_lag + matrix_ptr( Lag_range_ptr, k, 0, 2 ) );
654         energy = silk_inner_prod_aligned( basis_ptr, basis_ptr, sf_length );
655         silk_assert( energy >= 0 );
656         scratch_mem[ lag_counter ] = energy;
657         lag_counter++;
658
659         lag_diff = ( matrix_ptr( Lag_range_ptr, k, 1, 2 ) -  matrix_ptr( Lag_range_ptr, k, 0, 2 ) + 1 );
660         for( i = 1; i < lag_diff; i++ ) {
661             /* remove part outside new window */
662             energy -= silk_SMULBB( basis_ptr[ sf_length - i ], basis_ptr[ sf_length - i ] );
663             silk_assert( energy >= 0 );
664
665             /* add part that comes into window */
666             energy = silk_ADD_SAT32( energy, silk_SMULBB( basis_ptr[ -i ], basis_ptr[ -i ] ) );
667             silk_assert( energy >= 0 );
668             silk_assert( lag_counter < SCRATCH_SIZE );
669             scratch_mem[ lag_counter ] = energy;
670             lag_counter++;
671         }
672
673         delta = matrix_ptr( Lag_range_ptr, k, 0, 2 );
674         for( i = 0; i < nb_cbk_search; i++ ) {
675             /* Fill out the 3 dim array that stores the correlations for    */
676             /* each code_book vector for each start lag                     */
677             idx = matrix_ptr( Lag_CB_ptr, k, i, cbk_size ) - delta;
678             for( j = 0; j < PE_NB_STAGE3_LAGS; j++ ) {
679                 silk_assert( idx + j < SCRATCH_SIZE );
680                 silk_assert( idx + j < lag_counter );
681                 energies_st3[ k ][ i ][ j ] = scratch_mem[ idx + j ];
682                 silk_assert( energies_st3[ k ][ i ][ j ] >= 0 );
683             }
684         }
685         target_ptr += sf_length;
686     }
687 }