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