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