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