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