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