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