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