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_FLP.h"
36 #include "SigProc_FIX.h"
37 #include "pitch_est_defines.h"
38 #include "pitch.h"
39
40 #define SCRATCH_SIZE 22
41
42 /************************************************************/
43 /* Internally used functions */
44 /************************************************************/
45 static void silk_P_Ana_calc_corr_st3(
46 silk_float cross_corr_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ], /* O 3 DIM correlation array */
47 const silk_float frame[], /* I vector to correlate */
48 opus_int start_lag, /* I start lag */
49 opus_int sf_length, /* I sub frame length */
50 opus_int nb_subfr, /* I number of subframes */
51 opus_int complexity, /* I Complexity setting */
52 int arch /* I Run-time architecture */
53 );
54
55 static void silk_P_Ana_calc_energy_st3(
56 silk_float energies_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ], /* O 3 DIM correlation array */
57 const silk_float frame[], /* I vector to correlate */
58 opus_int start_lag, /* I start lag */
59 opus_int sf_length, /* I sub frame length */
60 opus_int nb_subfr, /* I number of subframes */
61 opus_int complexity /* I Complexity setting */
62 );
63
64 /************************************************************/
65 /* CORE PITCH ANALYSIS FUNCTION */
66 /************************************************************/
silk_pitch_analysis_core_FLP(const silk_float * frame,opus_int * pitch_out,opus_int16 * lagIndex,opus_int8 * contourIndex,silk_float * LTPCorr,opus_int prevLag,const silk_float search_thres1,const silk_float search_thres2,const opus_int Fs_kHz,const opus_int complexity,const opus_int nb_subfr,int arch)67 opus_int silk_pitch_analysis_core_FLP( /* O Voicing estimate: 0 voiced, 1 unvoiced */
68 const silk_float *frame, /* I Signal of length PE_FRAME_LENGTH_MS*Fs_kHz */
69 opus_int *pitch_out, /* O Pitch lag values [nb_subfr] */
70 opus_int16 *lagIndex, /* O Lag Index */
71 opus_int8 *contourIndex, /* O Pitch contour Index */
72 silk_float *LTPCorr, /* I/O Normalized correlation; input: value from previous frame */
73 opus_int prevLag, /* I Last lag of previous frame; set to zero is unvoiced */
74 const silk_float search_thres1, /* I First stage threshold for lag candidates 0 - 1 */
75 const silk_float search_thres2, /* I Final threshold for lag candidates 0 - 1 */
76 const opus_int Fs_kHz, /* I sample frequency (kHz) */
77 const opus_int complexity, /* I Complexity setting, 0-2, where 2 is highest */
78 const opus_int nb_subfr, /* I Number of 5 ms subframes */
79 int arch /* I Run-time architecture */
80 )
81 {
82 opus_int i, k, d, j;
83 silk_float frame_8kHz[ PE_MAX_FRAME_LENGTH_MS * 8 ];
84 silk_float frame_4kHz[ PE_MAX_FRAME_LENGTH_MS * 4 ];
85 opus_int16 frame_8_FIX[ PE_MAX_FRAME_LENGTH_MS * 8 ];
86 opus_int16 frame_4_FIX[ PE_MAX_FRAME_LENGTH_MS * 4 ];
87 opus_int32 filt_state[ 6 ];
88 silk_float threshold, contour_bias;
89 silk_float C[ PE_MAX_NB_SUBFR][ (PE_MAX_LAG >> 1) + 5 ];
90 opus_val32 xcorr[ PE_MAX_LAG_MS * 4 - PE_MIN_LAG_MS * 4 + 1 ];
91 silk_float CC[ PE_NB_CBKS_STAGE2_EXT ];
92 const silk_float *target_ptr, *basis_ptr;
93 double cross_corr, normalizer, energy, energy_tmp;
94 opus_int d_srch[ PE_D_SRCH_LENGTH ];
95 opus_int16 d_comp[ (PE_MAX_LAG >> 1) + 5 ];
96 opus_int length_d_srch, length_d_comp;
97 silk_float Cmax, CCmax, CCmax_b, CCmax_new_b, CCmax_new;
98 opus_int CBimax, CBimax_new, lag, start_lag, end_lag, lag_new;
99 opus_int cbk_size;
100 silk_float lag_log2, prevLag_log2, delta_lag_log2_sqr;
101 silk_float energies_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ];
102 silk_float cross_corr_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ];
103 opus_int lag_counter;
104 opus_int frame_length, frame_length_8kHz, frame_length_4kHz;
105 opus_int sf_length, sf_length_8kHz, sf_length_4kHz;
106 opus_int min_lag, min_lag_8kHz, min_lag_4kHz;
107 opus_int max_lag, max_lag_8kHz, max_lag_4kHz;
108 opus_int nb_cbk_search;
109 const opus_int8 *Lag_CB_ptr;
110
111 /* Check for valid sampling frequency */
112 celt_assert( Fs_kHz == 8 || Fs_kHz == 12 || Fs_kHz == 16 );
113
114 /* Check for valid complexity setting */
115 celt_assert( complexity >= SILK_PE_MIN_COMPLEX );
116 celt_assert( complexity <= SILK_PE_MAX_COMPLEX );
117
118 silk_assert( search_thres1 >= 0.0f && search_thres1 <= 1.0f );
119 silk_assert( search_thres2 >= 0.0f && search_thres2 <= 1.0f );
120
121 /* Set up frame lengths max / min lag for the sampling frequency */
122 frame_length = ( PE_LTP_MEM_LENGTH_MS + nb_subfr * PE_SUBFR_LENGTH_MS ) * Fs_kHz;
123 frame_length_4kHz = ( PE_LTP_MEM_LENGTH_MS + nb_subfr * PE_SUBFR_LENGTH_MS ) * 4;
124 frame_length_8kHz = ( PE_LTP_MEM_LENGTH_MS + nb_subfr * PE_SUBFR_LENGTH_MS ) * 8;
125 sf_length = PE_SUBFR_LENGTH_MS * Fs_kHz;
126 sf_length_4kHz = PE_SUBFR_LENGTH_MS * 4;
127 sf_length_8kHz = PE_SUBFR_LENGTH_MS * 8;
128 min_lag = PE_MIN_LAG_MS * Fs_kHz;
129 min_lag_4kHz = PE_MIN_LAG_MS * 4;
130 min_lag_8kHz = PE_MIN_LAG_MS * 8;
131 max_lag = PE_MAX_LAG_MS * Fs_kHz - 1;
132 max_lag_4kHz = PE_MAX_LAG_MS * 4;
133 max_lag_8kHz = PE_MAX_LAG_MS * 8 - 1;
134
135 /* Resample from input sampled at Fs_kHz to 8 kHz */
136 if( Fs_kHz == 16 ) {
137 /* Resample to 16 -> 8 khz */
138 opus_int16 frame_16_FIX[ 16 * PE_MAX_FRAME_LENGTH_MS ];
139 silk_float2short_array( frame_16_FIX, frame, frame_length );
140 silk_memset( filt_state, 0, 2 * sizeof( opus_int32 ) );
141 silk_resampler_down2( filt_state, frame_8_FIX, frame_16_FIX, frame_length );
142 silk_short2float_array( frame_8kHz, frame_8_FIX, frame_length_8kHz );
143 } else if( Fs_kHz == 12 ) {
144 /* Resample to 12 -> 8 khz */
145 opus_int16 frame_12_FIX[ 12 * PE_MAX_FRAME_LENGTH_MS ];
146 silk_float2short_array( frame_12_FIX, frame, frame_length );
147 silk_memset( filt_state, 0, 6 * sizeof( opus_int32 ) );
148 silk_resampler_down2_3( filt_state, frame_8_FIX, frame_12_FIX, frame_length );
149 silk_short2float_array( frame_8kHz, frame_8_FIX, frame_length_8kHz );
150 } else {
151 celt_assert( Fs_kHz == 8 );
152 silk_float2short_array( frame_8_FIX, frame, frame_length_8kHz );
153 }
154
155 /* Decimate again to 4 kHz */
156 silk_memset( filt_state, 0, 2 * sizeof( opus_int32 ) );
157 silk_resampler_down2( filt_state, frame_4_FIX, frame_8_FIX, frame_length_8kHz );
158 silk_short2float_array( frame_4kHz, frame_4_FIX, frame_length_4kHz );
159
160 /* Low-pass filter */
161 for( i = frame_length_4kHz - 1; i > 0; i-- ) {
162 frame_4kHz[ i ] = silk_ADD_SAT16( frame_4kHz[ i ], frame_4kHz[ i - 1 ] );
163 }
164
165 /******************************************************************************
166 * FIRST STAGE, operating in 4 khz
167 ******************************************************************************/
168 silk_memset(C, 0, sizeof(silk_float) * nb_subfr * ((PE_MAX_LAG >> 1) + 5));
169 target_ptr = &frame_4kHz[ silk_LSHIFT( sf_length_4kHz, 2 ) ];
170 for( k = 0; k < nb_subfr >> 1; k++ ) {
171 /* Check that we are within range of the array */
172 celt_assert( target_ptr >= frame_4kHz );
173 celt_assert( target_ptr + sf_length_8kHz <= frame_4kHz + frame_length_4kHz );
174
175 basis_ptr = target_ptr - min_lag_4kHz;
176
177 /* Check that we are within range of the array */
178 celt_assert( basis_ptr >= frame_4kHz );
179 celt_assert( basis_ptr + sf_length_8kHz <= frame_4kHz + frame_length_4kHz );
180
181 celt_pitch_xcorr( target_ptr, target_ptr-max_lag_4kHz, xcorr, sf_length_8kHz, max_lag_4kHz - min_lag_4kHz + 1, arch );
182
183 /* Calculate first vector products before loop */
184 cross_corr = xcorr[ max_lag_4kHz - min_lag_4kHz ];
185 normalizer = silk_energy_FLP( target_ptr, sf_length_8kHz ) +
186 silk_energy_FLP( basis_ptr, sf_length_8kHz ) +
187 sf_length_8kHz * 4000.0f;
188
189 C[ 0 ][ min_lag_4kHz ] += (silk_float)( 2 * cross_corr / normalizer );
190
191 /* From now on normalizer is computed recursively */
192 for( d = min_lag_4kHz + 1; d <= max_lag_4kHz; d++ ) {
193 basis_ptr--;
194
195 /* Check that we are within range of the array */
196 silk_assert( basis_ptr >= frame_4kHz );
197 silk_assert( basis_ptr + sf_length_8kHz <= frame_4kHz + frame_length_4kHz );
198
199 cross_corr = xcorr[ max_lag_4kHz - d ];
200
201 /* Add contribution of new sample and remove contribution from oldest sample */
202 normalizer +=
203 basis_ptr[ 0 ] * (double)basis_ptr[ 0 ] -
204 basis_ptr[ sf_length_8kHz ] * (double)basis_ptr[ sf_length_8kHz ];
205 C[ 0 ][ d ] += (silk_float)( 2 * cross_corr / normalizer );
206 }
207 /* Update target pointer */
208 target_ptr += sf_length_8kHz;
209 }
210
211 /* Apply short-lag bias */
212 for( i = max_lag_4kHz; i >= min_lag_4kHz; i-- ) {
213 C[ 0 ][ i ] -= C[ 0 ][ i ] * i / 4096.0f;
214 }
215
216 /* Sort */
217 length_d_srch = 4 + 2 * complexity;
218 celt_assert( 3 * length_d_srch <= PE_D_SRCH_LENGTH );
219 silk_insertion_sort_decreasing_FLP( &C[ 0 ][ min_lag_4kHz ], d_srch, max_lag_4kHz - min_lag_4kHz + 1, length_d_srch );
220
221 /* Escape if correlation is very low already here */
222 Cmax = C[ 0 ][ min_lag_4kHz ];
223 if( Cmax < 0.2f ) {
224 silk_memset( pitch_out, 0, nb_subfr * sizeof( opus_int ) );
225 *LTPCorr = 0.0f;
226 *lagIndex = 0;
227 *contourIndex = 0;
228 return 1;
229 }
230
231 threshold = search_thres1 * Cmax;
232 for( i = 0; i < length_d_srch; i++ ) {
233 /* Convert to 8 kHz indices for the sorted correlation that exceeds the threshold */
234 if( C[ 0 ][ min_lag_4kHz + i ] > threshold ) {
235 d_srch[ i ] = silk_LSHIFT( d_srch[ i ] + min_lag_4kHz, 1 );
236 } else {
237 length_d_srch = i;
238 break;
239 }
240 }
241 celt_assert( length_d_srch > 0 );
242
243 for( i = min_lag_8kHz - 5; i < max_lag_8kHz + 5; i++ ) {
244 d_comp[ i ] = 0;
245 }
246 for( i = 0; i < length_d_srch; i++ ) {
247 d_comp[ d_srch[ i ] ] = 1;
248 }
249
250 /* Convolution */
251 for( i = max_lag_8kHz + 3; i >= min_lag_8kHz; i-- ) {
252 d_comp[ i ] += d_comp[ i - 1 ] + d_comp[ i - 2 ];
253 }
254
255 length_d_srch = 0;
256 for( i = min_lag_8kHz; i < max_lag_8kHz + 1; i++ ) {
257 if( d_comp[ i + 1 ] > 0 ) {
258 d_srch[ length_d_srch ] = i;
259 length_d_srch++;
260 }
261 }
262
263 /* Convolution */
264 for( i = max_lag_8kHz + 3; i >= min_lag_8kHz; i-- ) {
265 d_comp[ i ] += d_comp[ i - 1 ] + d_comp[ i - 2 ] + d_comp[ i - 3 ];
266 }
267
268 length_d_comp = 0;
269 for( i = min_lag_8kHz; i < max_lag_8kHz + 4; i++ ) {
270 if( d_comp[ i ] > 0 ) {
271 d_comp[ length_d_comp ] = (opus_int16)( i - 2 );
272 length_d_comp++;
273 }
274 }
275
276 /**********************************************************************************
277 ** SECOND STAGE, operating at 8 kHz, on lag sections with high correlation
278 *************************************************************************************/
279 /*********************************************************************************
280 * Find energy of each subframe projected onto its history, for a range of delays
281 *********************************************************************************/
282 silk_memset( C, 0, PE_MAX_NB_SUBFR*((PE_MAX_LAG >> 1) + 5) * sizeof(silk_float));
283
284 if( Fs_kHz == 8 ) {
285 target_ptr = &frame[ PE_LTP_MEM_LENGTH_MS * 8 ];
286 } else {
287 target_ptr = &frame_8kHz[ PE_LTP_MEM_LENGTH_MS * 8 ];
288 }
289 for( k = 0; k < nb_subfr; k++ ) {
290 energy_tmp = silk_energy_FLP( target_ptr, sf_length_8kHz ) + 1.0;
291 for( j = 0; j < length_d_comp; j++ ) {
292 d = d_comp[ j ];
293 basis_ptr = target_ptr - d;
294 cross_corr = silk_inner_product_FLP( basis_ptr, target_ptr, sf_length_8kHz );
295 if( cross_corr > 0.0f ) {
296 energy = silk_energy_FLP( basis_ptr, sf_length_8kHz );
297 C[ k ][ d ] = (silk_float)( 2 * cross_corr / ( energy + energy_tmp ) );
298 } else {
299 C[ k ][ d ] = 0.0f;
300 }
301 }
302 target_ptr += sf_length_8kHz;
303 }
304
305 /* search over lag range and lags codebook */
306 /* scale factor for lag codebook, as a function of center lag */
307
308 CCmax = 0.0f; /* This value doesn't matter */
309 CCmax_b = -1000.0f;
310
311 CBimax = 0; /* To avoid returning undefined lag values */
312 lag = -1; /* To check if lag with strong enough correlation has been found */
313
314 if( prevLag > 0 ) {
315 if( Fs_kHz == 12 ) {
316 prevLag = silk_LSHIFT( prevLag, 1 ) / 3;
317 } else if( Fs_kHz == 16 ) {
318 prevLag = silk_RSHIFT( prevLag, 1 );
319 }
320 prevLag_log2 = silk_log2( (silk_float)prevLag );
321 } else {
322 prevLag_log2 = 0;
323 }
324
325 /* Set up stage 2 codebook based on number of subframes */
326 if( nb_subfr == PE_MAX_NB_SUBFR ) {
327 cbk_size = PE_NB_CBKS_STAGE2_EXT;
328 Lag_CB_ptr = &silk_CB_lags_stage2[ 0 ][ 0 ];
329 if( Fs_kHz == 8 && complexity > SILK_PE_MIN_COMPLEX ) {
330 /* If input is 8 khz use a larger codebook here because it is last stage */
331 nb_cbk_search = PE_NB_CBKS_STAGE2_EXT;
332 } else {
333 nb_cbk_search = PE_NB_CBKS_STAGE2;
334 }
335 } else {
336 cbk_size = PE_NB_CBKS_STAGE2_10MS;
337 Lag_CB_ptr = &silk_CB_lags_stage2_10_ms[ 0 ][ 0 ];
338 nb_cbk_search = PE_NB_CBKS_STAGE2_10MS;
339 }
340
341 for( k = 0; k < length_d_srch; k++ ) {
342 d = d_srch[ k ];
343 for( j = 0; j < nb_cbk_search; j++ ) {
344 CC[j] = 0.0f;
345 for( i = 0; i < nb_subfr; i++ ) {
346 /* Try all codebooks */
347 CC[ j ] += C[ i ][ d + matrix_ptr( Lag_CB_ptr, i, j, cbk_size )];
348 }
349 }
350 /* Find best codebook */
351 CCmax_new = -1000.0f;
352 CBimax_new = 0;
353 for( i = 0; i < nb_cbk_search; i++ ) {
354 if( CC[ i ] > CCmax_new ) {
355 CCmax_new = CC[ i ];
356 CBimax_new = i;
357 }
358 }
359
360 /* Bias towards shorter lags */
361 lag_log2 = silk_log2( (silk_float)d );
362 CCmax_new_b = CCmax_new - PE_SHORTLAG_BIAS * nb_subfr * lag_log2;
363
364 /* Bias towards previous lag */
365 if( prevLag > 0 ) {
366 delta_lag_log2_sqr = lag_log2 - prevLag_log2;
367 delta_lag_log2_sqr *= delta_lag_log2_sqr;
368 CCmax_new_b -= PE_PREVLAG_BIAS * nb_subfr * (*LTPCorr) * delta_lag_log2_sqr / ( delta_lag_log2_sqr + 0.5f );
369 }
370
371 if( CCmax_new_b > CCmax_b && /* Find maximum biased correlation */
372 CCmax_new > nb_subfr * search_thres2 /* Correlation needs to be high enough to be voiced */
373 ) {
374 CCmax_b = CCmax_new_b;
375 CCmax = CCmax_new;
376 lag = d;
377 CBimax = CBimax_new;
378 }
379 }
380
381 if( lag == -1 ) {
382 /* No suitable candidate found */
383 silk_memset( pitch_out, 0, PE_MAX_NB_SUBFR * sizeof(opus_int) );
384 *LTPCorr = 0.0f;
385 *lagIndex = 0;
386 *contourIndex = 0;
387 return 1;
388 }
389
390 /* Output normalized correlation */
391 *LTPCorr = (silk_float)( CCmax / nb_subfr );
392 silk_assert( *LTPCorr >= 0.0f );
393
394 if( Fs_kHz > 8 ) {
395 /* Search in original signal */
396
397 /* Compensate for decimation */
398 silk_assert( lag == silk_SAT16( lag ) );
399 if( Fs_kHz == 12 ) {
400 lag = silk_RSHIFT_ROUND( silk_SMULBB( lag, 3 ), 1 );
401 } else { /* Fs_kHz == 16 */
402 lag = silk_LSHIFT( lag, 1 );
403 }
404
405 lag = silk_LIMIT_int( lag, min_lag, max_lag );
406 start_lag = silk_max_int( lag - 2, min_lag );
407 end_lag = silk_min_int( lag + 2, max_lag );
408 lag_new = lag; /* to avoid undefined lag */
409 CBimax = 0; /* to avoid undefined lag */
410
411 CCmax = -1000.0f;
412
413 /* Calculate the correlations and energies needed in stage 3 */
414 silk_P_Ana_calc_corr_st3( cross_corr_st3, frame, start_lag, sf_length, nb_subfr, complexity, arch );
415 silk_P_Ana_calc_energy_st3( energies_st3, frame, start_lag, sf_length, nb_subfr, complexity );
416
417 lag_counter = 0;
418 silk_assert( lag == silk_SAT16( lag ) );
419 contour_bias = PE_FLATCONTOUR_BIAS / lag;
420
421 /* Set up cbk parameters according to complexity setting and frame length */
422 if( nb_subfr == PE_MAX_NB_SUBFR ) {
423 nb_cbk_search = (opus_int)silk_nb_cbk_searchs_stage3[ complexity ];
424 cbk_size = PE_NB_CBKS_STAGE3_MAX;
425 Lag_CB_ptr = &silk_CB_lags_stage3[ 0 ][ 0 ];
426 } else {
427 nb_cbk_search = PE_NB_CBKS_STAGE3_10MS;
428 cbk_size = PE_NB_CBKS_STAGE3_10MS;
429 Lag_CB_ptr = &silk_CB_lags_stage3_10_ms[ 0 ][ 0 ];
430 }
431
432 target_ptr = &frame[ PE_LTP_MEM_LENGTH_MS * Fs_kHz ];
433 energy_tmp = silk_energy_FLP( target_ptr, nb_subfr * sf_length ) + 1.0;
434 for( d = start_lag; d <= end_lag; d++ ) {
435 for( j = 0; j < nb_cbk_search; j++ ) {
436 cross_corr = 0.0;
437 energy = energy_tmp;
438 for( k = 0; k < nb_subfr; k++ ) {
439 cross_corr += cross_corr_st3[ k ][ j ][ lag_counter ];
440 energy += energies_st3[ k ][ j ][ lag_counter ];
441 }
442 if( cross_corr > 0.0 ) {
443 CCmax_new = (silk_float)( 2 * cross_corr / energy );
444 /* Reduce depending on flatness of contour */
445 CCmax_new *= 1.0f - contour_bias * j;
446 } else {
447 CCmax_new = 0.0f;
448 }
449
450 if( CCmax_new > CCmax && ( d + (opus_int)silk_CB_lags_stage3[ 0 ][ j ] ) <= max_lag ) {
451 CCmax = CCmax_new;
452 lag_new = d;
453 CBimax = j;
454 }
455 }
456 lag_counter++;
457 }
458
459 for( k = 0; k < nb_subfr; k++ ) {
460 pitch_out[ k ] = lag_new + matrix_ptr( Lag_CB_ptr, k, CBimax, cbk_size );
461 pitch_out[ k ] = silk_LIMIT( pitch_out[ k ], min_lag, PE_MAX_LAG_MS * Fs_kHz );
462 }
463 *lagIndex = (opus_int16)( lag_new - min_lag );
464 *contourIndex = (opus_int8)CBimax;
465 } else { /* Fs_kHz == 8 */
466 /* Save Lags */
467 for( k = 0; k < nb_subfr; k++ ) {
468 pitch_out[ k ] = lag + matrix_ptr( Lag_CB_ptr, k, CBimax, cbk_size );
469 pitch_out[ k ] = silk_LIMIT( pitch_out[ k ], min_lag_8kHz, PE_MAX_LAG_MS * 8 );
470 }
471 *lagIndex = (opus_int16)( lag - min_lag_8kHz );
472 *contourIndex = (opus_int8)CBimax;
473 }
474 celt_assert( *lagIndex >= 0 );
475 /* return as voiced */
476 return 0;
477 }
478
479 /***********************************************************************
480 * Calculates the correlations used in stage 3 search. In order to cover
481 * the whole lag codebook for all the searched offset lags (lag +- 2),
482 * the following correlations are needed in each sub frame:
483 *
484 * sf1: lag range [-8,...,7] total 16 correlations
485 * sf2: lag range [-4,...,4] total 9 correlations
486 * sf3: lag range [-3,....4] total 8 correltions
487 * sf4: lag range [-6,....8] total 15 correlations
488 *
489 * In total 48 correlations. The direct implementation computed in worst
490 * case 4*12*5 = 240 correlations, but more likely around 120.
491 ***********************************************************************/
silk_P_Ana_calc_corr_st3(silk_float cross_corr_st3[PE_MAX_NB_SUBFR][PE_NB_CBKS_STAGE3_MAX][PE_NB_STAGE3_LAGS],const silk_float frame[],opus_int start_lag,opus_int sf_length,opus_int nb_subfr,opus_int complexity,int arch)492 static void silk_P_Ana_calc_corr_st3(
493 silk_float cross_corr_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ], /* O 3 DIM correlation array */
494 const silk_float frame[], /* I vector to correlate */
495 opus_int start_lag, /* I start lag */
496 opus_int sf_length, /* I sub frame length */
497 opus_int nb_subfr, /* I number of subframes */
498 opus_int complexity, /* I Complexity setting */
499 int arch /* I Run-time architecture */
500 )
501 {
502 const silk_float *target_ptr;
503 opus_int i, j, k, lag_counter, lag_low, lag_high;
504 opus_int nb_cbk_search, delta, idx, cbk_size;
505 silk_float scratch_mem[ SCRATCH_SIZE ];
506 opus_val32 xcorr[ SCRATCH_SIZE ];
507 const opus_int8 *Lag_range_ptr, *Lag_CB_ptr;
508
509 celt_assert( complexity >= SILK_PE_MIN_COMPLEX );
510 celt_assert( complexity <= SILK_PE_MAX_COMPLEX );
511
512 if( nb_subfr == PE_MAX_NB_SUBFR ) {
513 Lag_range_ptr = &silk_Lag_range_stage3[ complexity ][ 0 ][ 0 ];
514 Lag_CB_ptr = &silk_CB_lags_stage3[ 0 ][ 0 ];
515 nb_cbk_search = silk_nb_cbk_searchs_stage3[ complexity ];
516 cbk_size = PE_NB_CBKS_STAGE3_MAX;
517 } else {
518 celt_assert( nb_subfr == PE_MAX_NB_SUBFR >> 1);
519 Lag_range_ptr = &silk_Lag_range_stage3_10_ms[ 0 ][ 0 ];
520 Lag_CB_ptr = &silk_CB_lags_stage3_10_ms[ 0 ][ 0 ];
521 nb_cbk_search = PE_NB_CBKS_STAGE3_10MS;
522 cbk_size = PE_NB_CBKS_STAGE3_10MS;
523 }
524
525 target_ptr = &frame[ silk_LSHIFT( sf_length, 2 ) ]; /* Pointer to middle of frame */
526 for( k = 0; k < nb_subfr; k++ ) {
527 lag_counter = 0;
528
529 /* Calculate the correlations for each subframe */
530 lag_low = matrix_ptr( Lag_range_ptr, k, 0, 2 );
531 lag_high = matrix_ptr( Lag_range_ptr, k, 1, 2 );
532 silk_assert(lag_high-lag_low+1 <= SCRATCH_SIZE);
533 celt_pitch_xcorr( target_ptr, target_ptr - start_lag - lag_high, xcorr, sf_length, lag_high - lag_low + 1, arch );
534 for( j = lag_low; j <= lag_high; j++ ) {
535 silk_assert( lag_counter < SCRATCH_SIZE );
536 scratch_mem[ lag_counter ] = xcorr[ lag_high - j ];
537 lag_counter++;
538 }
539
540 delta = matrix_ptr( Lag_range_ptr, k, 0, 2 );
541 for( i = 0; i < nb_cbk_search; i++ ) {
542 /* Fill out the 3 dim array that stores the correlations for */
543 /* each code_book vector for each start lag */
544 idx = matrix_ptr( Lag_CB_ptr, k, i, cbk_size ) - delta;
545 for( j = 0; j < PE_NB_STAGE3_LAGS; j++ ) {
546 silk_assert( idx + j < SCRATCH_SIZE );
547 silk_assert( idx + j < lag_counter );
548 cross_corr_st3[ k ][ i ][ j ] = scratch_mem[ idx + j ];
549 }
550 }
551 target_ptr += sf_length;
552 }
553 }
554
555 /********************************************************************/
556 /* Calculate the energies for first two subframes. The energies are */
557 /* calculated recursively. */
558 /********************************************************************/
silk_P_Ana_calc_energy_st3(silk_float energies_st3[PE_MAX_NB_SUBFR][PE_NB_CBKS_STAGE3_MAX][PE_NB_STAGE3_LAGS],const silk_float frame[],opus_int start_lag,opus_int sf_length,opus_int nb_subfr,opus_int complexity)559 static void silk_P_Ana_calc_energy_st3(
560 silk_float energies_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ], /* O 3 DIM correlation array */
561 const silk_float frame[], /* I vector to correlate */
562 opus_int start_lag, /* I start lag */
563 opus_int sf_length, /* I sub frame length */
564 opus_int nb_subfr, /* I number of subframes */
565 opus_int complexity /* I Complexity setting */
566 )
567 {
568 const silk_float *target_ptr, *basis_ptr;
569 double energy;
570 opus_int k, i, j, lag_counter;
571 opus_int nb_cbk_search, delta, idx, cbk_size, lag_diff;
572 silk_float scratch_mem[ SCRATCH_SIZE ];
573 const opus_int8 *Lag_range_ptr, *Lag_CB_ptr;
574
575 celt_assert( complexity >= SILK_PE_MIN_COMPLEX );
576 celt_assert( complexity <= SILK_PE_MAX_COMPLEX );
577
578 if( nb_subfr == PE_MAX_NB_SUBFR ) {
579 Lag_range_ptr = &silk_Lag_range_stage3[ complexity ][ 0 ][ 0 ];
580 Lag_CB_ptr = &silk_CB_lags_stage3[ 0 ][ 0 ];
581 nb_cbk_search = silk_nb_cbk_searchs_stage3[ complexity ];
582 cbk_size = PE_NB_CBKS_STAGE3_MAX;
583 } else {
584 celt_assert( nb_subfr == PE_MAX_NB_SUBFR >> 1);
585 Lag_range_ptr = &silk_Lag_range_stage3_10_ms[ 0 ][ 0 ];
586 Lag_CB_ptr = &silk_CB_lags_stage3_10_ms[ 0 ][ 0 ];
587 nb_cbk_search = PE_NB_CBKS_STAGE3_10MS;
588 cbk_size = PE_NB_CBKS_STAGE3_10MS;
589 }
590
591 target_ptr = &frame[ silk_LSHIFT( sf_length, 2 ) ];
592 for( k = 0; k < nb_subfr; k++ ) {
593 lag_counter = 0;
594
595 /* Calculate the energy for first lag */
596 basis_ptr = target_ptr - ( start_lag + matrix_ptr( Lag_range_ptr, k, 0, 2 ) );
597 energy = silk_energy_FLP( basis_ptr, sf_length ) + 1e-3;
598 silk_assert( energy >= 0.0 );
599 scratch_mem[lag_counter] = (silk_float)energy;
600 lag_counter++;
601
602 lag_diff = ( matrix_ptr( Lag_range_ptr, k, 1, 2 ) - matrix_ptr( Lag_range_ptr, k, 0, 2 ) + 1 );
603 for( i = 1; i < lag_diff; i++ ) {
604 /* remove part outside new window */
605 energy -= basis_ptr[sf_length - i] * (double)basis_ptr[sf_length - i];
606 silk_assert( energy >= 0.0 );
607
608 /* add part that comes into window */
609 energy += basis_ptr[ -i ] * (double)basis_ptr[ -i ];
610 silk_assert( energy >= 0.0 );
611 silk_assert( lag_counter < SCRATCH_SIZE );
612 scratch_mem[lag_counter] = (silk_float)energy;
613 lag_counter++;
614 }
615
616 delta = matrix_ptr( Lag_range_ptr, k, 0, 2 );
617 for( i = 0; i < nb_cbk_search; i++ ) {
618 /* Fill out the 3 dim array that stores the correlations for */
619 /* each code_book vector for each start lag */
620 idx = matrix_ptr( Lag_CB_ptr, k, i, cbk_size ) - delta;
621 for( j = 0; j < PE_NB_STAGE3_LAGS; j++ ) {
622 silk_assert( idx + j < SCRATCH_SIZE );
623 silk_assert( idx + j < lag_counter );
624 energies_st3[ k ][ i ][ j ] = scratch_mem[ idx + j ];
625 silk_assert( energies_st3[ k ][ i ][ j ] >= 0.0f );
626 }
627 }
628 target_ptr += sf_length;
629 }
630 }
631