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