/* Find least-squares prediction gain for one signal based on another and quantize it */
int32_t silk_stereo_find_predictor( /* O Returns predictor in Q13 */
int32_t * ratio_Q14, /* O Ratio of residual and mid energies */
const int16_t x[], /* I Basis signal */
const int16_t y[], /* I Target signal */
int32_t mid_res_amp_Q0[], /* I/O Smoothed mid, residual norms */
int length, /* I Number of samples */
int smooth_coef_Q16 /* I Smoothing coefficient */
)
{
int scale, scale1, scale2;
int32_t nrgx, nrgy, corr, pred_Q13, pred2_Q10;
/* Find predictor */
silk_sum_sqr_shift(&nrgx, &scale1, x, length);
silk_sum_sqr_shift(&nrgy, &scale2, y, length);
scale = silk_max_int(scale1, scale2);
scale = scale + (scale & 1); /* make even */
nrgy = silk_RSHIFT32(nrgy, scale - scale2);
nrgx = silk_RSHIFT32(nrgx, scale - scale1);
nrgx = silk_max_int(nrgx, 1);
corr = silk_inner_prod_aligned_scale(x, y, scale, length);
pred_Q13 = silk_DIV32_varQ(corr, nrgx, 13);
pred_Q13 = silk_LIMIT(pred_Q13, -(1 << 14), 1 << 14);
pred2_Q10 = silk_SMULWB(pred_Q13, pred_Q13);
/* Faster update for signals with large prediction parameters */
smooth_coef_Q16 =
(int) silk_max_int(smooth_coef_Q16, silk_abs(pred2_Q10));
/* Smoothed mid and residual norms */
assert(smooth_coef_Q16 < 32768);
scale = silk_RSHIFT(scale, 1);
mid_res_amp_Q0[0] =
silk_SMLAWB(mid_res_amp_Q0[0],
silk_LSHIFT(silk_SQRT_APPROX(nrgx),
scale) - mid_res_amp_Q0[0],
smooth_coef_Q16);
/* Residual energy = nrgy - 2 * pred * corr + pred^2 * nrgx */
nrgy = silk_SUB_LSHIFT32(nrgy, silk_SMULWB(corr, pred_Q13), 3 + 1);
nrgy = silk_ADD_LSHIFT32(nrgy, silk_SMULWB(nrgx, pred2_Q10), 6);
mid_res_amp_Q0[1] =
silk_SMLAWB(mid_res_amp_Q0[1],
silk_LSHIFT(silk_SQRT_APPROX(nrgy),
scale) - mid_res_amp_Q0[1],
smooth_coef_Q16);
/* Ratio of smoothed residual and mid norms */
*ratio_Q14 =
silk_DIV32_varQ(mid_res_amp_Q0[1], silk_max(mid_res_amp_Q0[0], 1),
14);
*ratio_Q14 = silk_LIMIT(*ratio_Q14, 0, 32767);
return pred_Q13;
}
/* Convert int32 coefficients to int16 coefs and make sure there's no wrap-around */
void silk_LPC_fit(
opus_int16 *a_QOUT, /* O Output signal */
opus_int32 *a_QIN, /* I/O Input signal */
const opus_int QOUT, /* I Input Q domain */
const opus_int QIN, /* I Input Q domain */
const opus_int d /* I Filter order */
)
{
opus_int i, k, idx = 0;
opus_int32 maxabs, absval, chirp_Q16;
/* Limit the maximum absolute value of the prediction coefficients, so that they'll fit in int16 */
for( i = 0; i < 10; i++ ) {
/* Find maximum absolute value and its index */
maxabs = 0;
for( k = 0; k < d; k++ ) {
absval = silk_abs( a_QIN[k] );
if( absval > maxabs ) {
maxabs = absval;
idx = k;
}
}
maxabs = silk_RSHIFT_ROUND( maxabs, QIN - QOUT );
if( maxabs > silk_int16_MAX ) {
/* Reduce magnitude of prediction coefficients */
maxabs = silk_min( maxabs, 163838 ); /* ( silk_int32_MAX >> 14 ) + silk_int16_MAX = 163838 */
chirp_Q16 = SILK_FIX_CONST( 0.999, 16 ) - silk_DIV32( silk_LSHIFT( maxabs - silk_int16_MAX, 14 ),
silk_RSHIFT32( silk_MUL( maxabs, idx + 1), 2 ) );
silk_bwexpander_32( a_QIN, d, chirp_Q16 );
} else {
break;
}
}
if( i == 10 ) {
/* Reached the last iteration, clip the coefficients */
for( k = 0; k < d; k++ ) {
a_QOUT[ k ] = (opus_int16)silk_SAT16( silk_RSHIFT_ROUND( a_QIN[ k ], QIN - QOUT ) );
a_QIN[ k ] = silk_LSHIFT( (opus_int32)a_QOUT[ k ], QIN - QOUT );
}
} else {
for( k = 0; k < d; k++ ) {
a_QOUT[ k ] = (opus_int16)silk_RSHIFT_ROUND( a_QIN[ k ], QIN - QOUT );
}
}
}
/* Quantize mid/side predictors */
void silk_stereo_quant_pred(
opus_int32 pred_Q13[], /* I/O Predictors (out: quantized) */
opus_int8 ix[ 2 ][ 3 ] /* O Quantization indices */
)
{
opus_int i, j, n;
opus_int32 low_Q13, step_Q13, lvl_Q13, err_min_Q13, err_Q13, quant_pred_Q13 = 0;
/* Quantize */
for( n = 0; n < 2; n++ ) {
/* Brute-force search over quantization levels */
err_min_Q13 = silk_int32_MAX;
for( i = 0; i < STEREO_QUANT_TAB_SIZE - 1; i++ ) {
low_Q13 = silk_stereo_pred_quant_Q13[ i ];
step_Q13 = silk_SMULWB( silk_stereo_pred_quant_Q13[ i + 1 ] - low_Q13,
SILK_FIX_CONST( 0.5 / STEREO_QUANT_SUB_STEPS, 16 ) );
for( j = 0; j < STEREO_QUANT_SUB_STEPS; j++ ) {
lvl_Q13 = silk_SMLABB( low_Q13, step_Q13, 2 * j + 1 );
err_Q13 = silk_abs( pred_Q13[ n ] - lvl_Q13 );
if( err_Q13 < err_min_Q13 ) {
err_min_Q13 = err_Q13;
quant_pred_Q13 = lvl_Q13;
ix[ n ][ 0 ] = i;
ix[ n ][ 1 ] = j;
} else {
/* Error increasing, so we're past the optimum */
goto done;
}
}
}
done:
ix[ n ][ 2 ] = silk_DIV32_16( ix[ n ][ 0 ], 3 );
ix[ n ][ 0 ] -= ix[ n ][ 2 ] * 3;
pred_Q13[ n ] = quant_pred_Q13;
}
/* Subtract second from first predictor (helps when actually applying these) */
pred_Q13[ 0 ] -= pred_Q13[ 1 ];
}
void silk_find_LTP_FIX(
opus_int16 b_Q14[ MAX_NB_SUBFR * LTP_ORDER ], /* O LTP coefs */
opus_int32 WLTP[ MAX_NB_SUBFR * LTP_ORDER * LTP_ORDER ], /* O Weight for LTP quantization */
opus_int *LTPredCodGain_Q7, /* O LTP coding gain */
const opus_int16 r_lpc[], /* I residual signal after LPC signal + state for first 10 ms */
const opus_int lag[ MAX_NB_SUBFR ], /* I LTP lags */
const opus_int32 Wght_Q15[ MAX_NB_SUBFR ], /* I weights */
const opus_int subfr_length, /* I subframe length */
const opus_int nb_subfr, /* I number of subframes */
const opus_int mem_offset, /* I number of samples in LTP memory */
opus_int corr_rshifts[ MAX_NB_SUBFR ] /* O right shifts applied to correlations */
)
{
opus_int i, k, lshift;
const opus_int16 *r_ptr, *lag_ptr;
opus_int16 *b_Q14_ptr;
opus_int32 regu;
opus_int32 *WLTP_ptr;
opus_int32 b_Q16[ LTP_ORDER ], delta_b_Q14[ LTP_ORDER ], d_Q14[ MAX_NB_SUBFR ], nrg[ MAX_NB_SUBFR ], g_Q26;
opus_int32 w[ MAX_NB_SUBFR ], WLTP_max, max_abs_d_Q14, max_w_bits;
opus_int32 temp32, denom32;
opus_int extra_shifts;
opus_int rr_shifts, maxRshifts, maxRshifts_wxtra, LZs;
opus_int32 LPC_res_nrg, LPC_LTP_res_nrg, div_Q16;
opus_int32 Rr[ LTP_ORDER ], rr[ MAX_NB_SUBFR ];
opus_int32 wd, m_Q12;
b_Q14_ptr = b_Q14;
WLTP_ptr = WLTP;
r_ptr = &r_lpc[ mem_offset ];
for( k = 0; k < nb_subfr; k++ ) {
lag_ptr = r_ptr - ( lag[ k ] + LTP_ORDER / 2 );
silk_sum_sqr_shift( &rr[ k ], &rr_shifts, r_ptr, subfr_length ); /* rr[ k ] in Q( -rr_shifts ) */
/* Assure headroom */
LZs = silk_CLZ32( rr[k] );
if( LZs < LTP_CORRS_HEAD_ROOM ) {
rr[ k ] = silk_RSHIFT_ROUND( rr[ k ], LTP_CORRS_HEAD_ROOM - LZs );
rr_shifts += ( LTP_CORRS_HEAD_ROOM - LZs );
}
corr_rshifts[ k ] = rr_shifts;
silk_corrMatrix_FIX( lag_ptr, subfr_length, LTP_ORDER, LTP_CORRS_HEAD_ROOM, WLTP_ptr, &corr_rshifts[ k ] ); /* WLTP_fix_ptr in Q( -corr_rshifts[ k ] ) */
/* The correlation vector always has lower max abs value than rr and/or RR so head room is assured */
silk_corrVector_FIX( lag_ptr, r_ptr, subfr_length, LTP_ORDER, Rr, corr_rshifts[ k ] ); /* Rr_fix_ptr in Q( -corr_rshifts[ k ] ) */
if( corr_rshifts[ k ] > rr_shifts ) {
rr[ k ] = silk_RSHIFT( rr[ k ], corr_rshifts[ k ] - rr_shifts ); /* rr[ k ] in Q( -corr_rshifts[ k ] ) */
}
silk_assert( rr[ k ] >= 0 );
regu = 1;
regu = silk_SMLAWB( regu, rr[ k ], SILK_FIX_CONST( LTP_DAMPING/3, 16 ) );
regu = silk_SMLAWB( regu, matrix_ptr( WLTP_ptr, 0, 0, LTP_ORDER ), SILK_FIX_CONST( LTP_DAMPING/3, 16 ) );
regu = silk_SMLAWB( regu, matrix_ptr( WLTP_ptr, LTP_ORDER-1, LTP_ORDER-1, LTP_ORDER ), SILK_FIX_CONST( LTP_DAMPING/3, 16 ) );
silk_regularize_correlations_FIX( WLTP_ptr, &rr[k], regu, LTP_ORDER );
silk_solve_LDL_FIX( WLTP_ptr, LTP_ORDER, Rr, b_Q16 ); /* WLTP_fix_ptr and Rr_fix_ptr both in Q(-corr_rshifts[k]) */
/* Limit and store in Q14 */
silk_fit_LTP( b_Q16, b_Q14_ptr );
/* Calculate residual energy */
nrg[ k ] = silk_residual_energy16_covar_FIX( b_Q14_ptr, WLTP_ptr, Rr, rr[ k ], LTP_ORDER, 14 ); /* nrg_fix in Q( -corr_rshifts[ k ] ) */
/* temp = Wght[ k ] / ( nrg[ k ] * Wght[ k ] + 0.01f * subfr_length ); */
extra_shifts = silk_min_int( corr_rshifts[ k ], LTP_CORRS_HEAD_ROOM );
denom32 = silk_LSHIFT_SAT32( silk_SMULWB( nrg[ k ], Wght_Q15[ k ] ), 1 + extra_shifts ) + /* Q( -corr_rshifts[ k ] + extra_shifts ) */
silk_RSHIFT( silk_SMULWB( subfr_length, 655 ), corr_rshifts[ k ] - extra_shifts ); /* Q( -corr_rshifts[ k ] + extra_shifts ) */
denom32 = silk_max( denom32, 1 );
silk_assert( ((opus_int64)Wght_Q15[ k ] << 16 ) < silk_int32_MAX ); /* Wght always < 0.5 in Q0 */
temp32 = silk_DIV32( silk_LSHIFT( (opus_int32)Wght_Q15[ k ], 16 ), denom32 ); /* Q( 15 + 16 + corr_rshifts[k] - extra_shifts ) */
temp32 = silk_RSHIFT( temp32, 31 + corr_rshifts[ k ] - extra_shifts - 26 ); /* Q26 */
/* Limit temp such that the below scaling never wraps around */
WLTP_max = 0;
for( i = 0; i < LTP_ORDER * LTP_ORDER; i++ ) {
WLTP_max = silk_max( WLTP_ptr[ i ], WLTP_max );
}
lshift = silk_CLZ32( WLTP_max ) - 1 - 3; /* keep 3 bits free for vq_nearest_neighbor_fix */
silk_assert( 26 - 18 + lshift >= 0 );
if( 26 - 18 + lshift < 31 ) {
temp32 = silk_min_32( temp32, silk_LSHIFT( (opus_int32)1, 26 - 18 + lshift ) );
}
silk_scale_vector32_Q26_lshift_18( WLTP_ptr, temp32, LTP_ORDER * LTP_ORDER ); /* WLTP_ptr in Q( 18 - corr_rshifts[ k ] ) */
w[ k ] = matrix_ptr( WLTP_ptr, LTP_ORDER/2, LTP_ORDER/2, LTP_ORDER ); /* w in Q( 18 - corr_rshifts[ k ] ) */
silk_assert( w[k] >= 0 );
r_ptr += subfr_length;
b_Q14_ptr += LTP_ORDER;
WLTP_ptr += LTP_ORDER * LTP_ORDER;
}
maxRshifts = 0;
for( k = 0; k < nb_subfr; k++ ) {
maxRshifts = silk_max_int( corr_rshifts[ k ], maxRshifts );
}
/* Compute LTP coding gain */
if( LTPredCodGain_Q7 != NULL ) {
LPC_LTP_res_nrg = 0;
LPC_res_nrg = 0;
silk_assert( LTP_CORRS_HEAD_ROOM >= 2 ); /* Check that no overflow will happen when adding */
for( k = 0; k < nb_subfr; k++ ) {
LPC_res_nrg = silk_ADD32( LPC_res_nrg, silk_RSHIFT( silk_ADD32( silk_SMULWB( rr[ k ], Wght_Q15[ k ] ), 1 ), 1 + ( maxRshifts - corr_rshifts[ k ] ) ) ); /* Q( -maxRshifts ) */
LPC_LTP_res_nrg = silk_ADD32( LPC_LTP_res_nrg, silk_RSHIFT( silk_ADD32( silk_SMULWB( nrg[ k ], Wght_Q15[ k ] ), 1 ), 1 + ( maxRshifts - corr_rshifts[ k ] ) ) ); /* Q( -maxRshifts ) */
}
LPC_LTP_res_nrg = silk_max( LPC_LTP_res_nrg, 1 ); /* avoid division by zero */
div_Q16 = silk_DIV32_varQ( LPC_res_nrg, LPC_LTP_res_nrg, 16 );
*LTPredCodGain_Q7 = ( opus_int )silk_SMULBB( 3, silk_lin2log( div_Q16 ) - ( 16 << 7 ) );
silk_assert( *LTPredCodGain_Q7 == ( opus_int )silk_SAT16( silk_MUL( 3, silk_lin2log( div_Q16 ) - ( 16 << 7 ) ) ) );
}
/* smoothing */
/* d = sum( B, 1 ); */
b_Q14_ptr = b_Q14;
for( k = 0; k < nb_subfr; k++ ) {
d_Q14[ k ] = 0;
for( i = 0; i < LTP_ORDER; i++ ) {
d_Q14[ k ] += b_Q14_ptr[ i ];
}
b_Q14_ptr += LTP_ORDER;
}
/* m = ( w * d' ) / ( sum( w ) + 1e-3 ); */
/* Find maximum absolute value of d_Q14 and the bits used by w in Q0 */
max_abs_d_Q14 = 0;
max_w_bits = 0;
for( k = 0; k < nb_subfr; k++ ) {
max_abs_d_Q14 = silk_max_32( max_abs_d_Q14, silk_abs( d_Q14[ k ] ) );
/* w[ k ] is in Q( 18 - corr_rshifts[ k ] ) */
/* Find bits needed in Q( 18 - maxRshifts ) */
max_w_bits = silk_max_32( max_w_bits, 32 - silk_CLZ32( w[ k ] ) + corr_rshifts[ k ] - maxRshifts );
}
/* max_abs_d_Q14 = (5 << 15); worst case, i.e. LTP_ORDER * -silk_int16_MIN */
silk_assert( max_abs_d_Q14 <= ( 5 << 15 ) );
/* How many bits is needed for w*d' in Q( 18 - maxRshifts ) in the worst case, of all d_Q14's being equal to max_abs_d_Q14 */
extra_shifts = max_w_bits + 32 - silk_CLZ32( max_abs_d_Q14 ) - 14;
/* Subtract what we got available; bits in output var plus maxRshifts */
extra_shifts -= ( 32 - 1 - 2 + maxRshifts ); /* Keep sign bit free as well as 2 bits for accumulation */
extra_shifts = silk_max_int( extra_shifts, 0 );
maxRshifts_wxtra = maxRshifts + extra_shifts;
temp32 = silk_RSHIFT( 262, maxRshifts + extra_shifts ) + 1; /* 1e-3f in Q( 18 - (maxRshifts + extra_shifts) ) */
wd = 0;
for( k = 0; k < nb_subfr; k++ ) {
/* w has at least 2 bits of headroom so no overflow should happen */
temp32 = silk_ADD32( temp32, silk_RSHIFT( w[ k ], maxRshifts_wxtra - corr_rshifts[ k ] ) ); /* Q( 18 - maxRshifts_wxtra ) */
wd = silk_ADD32( wd, silk_LSHIFT( silk_SMULWW( silk_RSHIFT( w[ k ], maxRshifts_wxtra - corr_rshifts[ k ] ), d_Q14[ k ] ), 2 ) ); /* Q( 18 - maxRshifts_wxtra ) */
}
m_Q12 = silk_DIV32_varQ( wd, temp32, 12 );
b_Q14_ptr = b_Q14;
for( k = 0; k < nb_subfr; k++ ) {
/* w_fix[ k ] from Q( 18 - corr_rshifts[ k ] ) to Q( 16 ) */
if( 2 - corr_rshifts[k] > 0 ) {
temp32 = silk_RSHIFT( w[ k ], 2 - corr_rshifts[ k ] );
} else {
temp32 = silk_LSHIFT_SAT32( w[ k ], corr_rshifts[ k ] - 2 );
}
g_Q26 = silk_MUL(
silk_DIV32(
SILK_FIX_CONST( LTP_SMOOTHING, 26 ),
silk_RSHIFT( SILK_FIX_CONST( LTP_SMOOTHING, 26 ), 10 ) + temp32 ), /* Q10 */
silk_LSHIFT_SAT32( silk_SUB_SAT32( (opus_int32)m_Q12, silk_RSHIFT( d_Q14[ k ], 2 ) ), 4 ) ); /* Q16 */
temp32 = 0;
for( i = 0; i < LTP_ORDER; i++ ) {
delta_b_Q14[ i ] = silk_max_16( b_Q14_ptr[ i ], 1638 ); /* 1638_Q14 = 0.1_Q0 */
temp32 += delta_b_Q14[ i ]; /* Q14 */
}
temp32 = silk_DIV32( g_Q26, temp32 ); /* Q14 -> Q12 */
for( i = 0; i < LTP_ORDER; i++ ) {
b_Q14_ptr[ i ] = silk_LIMIT_32( (opus_int32)b_Q14_ptr[ i ] + silk_SMULWB( silk_LSHIFT_SAT32( temp32, 4 ), delta_b_Q14[ i ] ), -16000, 28000 );
}
b_Q14_ptr += LTP_ORDER;
}
}
/* Encode quantization indices of excitation */
void silk_encode_pulses(
ec_enc *psRangeEnc, /* I/O compressor data structure */
const opus_int signalType, /* I Signal type */
const opus_int quantOffsetType, /* I quantOffsetType */
opus_int8 pulses[], /* I quantization indices */
const opus_int frame_length /* I Frame length */
)
{
opus_int i, k, j, iter, bit, nLS, scale_down, RateLevelIndex = 0;
opus_int32 abs_q, minSumBits_Q5, sumBits_Q5;
opus_int abs_pulses[ MAX_FRAME_LENGTH ];
opus_int sum_pulses[ MAX_NB_SHELL_BLOCKS ];
opus_int nRshifts[ MAX_NB_SHELL_BLOCKS ];
opus_int pulses_comb[ 8 ];
opus_int *abs_pulses_ptr;
const opus_int8 *pulses_ptr;
const opus_uint8 *cdf_ptr;
const opus_uint8 *nBits_ptr;
silk_memset( pulses_comb, 0, 8 * sizeof( opus_int ) ); /* Fixing Valgrind reported problem*/
/****************************/
/* Prepare for shell coding */
/****************************/
/* Calculate number of shell blocks */
silk_assert( 1 << LOG2_SHELL_CODEC_FRAME_LENGTH == SHELL_CODEC_FRAME_LENGTH );
iter = silk_RSHIFT( frame_length, LOG2_SHELL_CODEC_FRAME_LENGTH );
if( iter * SHELL_CODEC_FRAME_LENGTH < frame_length ) {
silk_assert( frame_length == 12 * 10 ); /* Make sure only happens for 10 ms @ 12 kHz */
iter++;
silk_memset( &pulses[ frame_length ], 0, SHELL_CODEC_FRAME_LENGTH * sizeof(opus_int8));
}
/* Take the absolute value of the pulses */
for( i = 0; i < iter * SHELL_CODEC_FRAME_LENGTH; i+=4 ) {
abs_pulses[i+0] = ( opus_int )silk_abs( pulses[ i + 0 ] );
abs_pulses[i+1] = ( opus_int )silk_abs( pulses[ i + 1 ] );
abs_pulses[i+2] = ( opus_int )silk_abs( pulses[ i + 2 ] );
abs_pulses[i+3] = ( opus_int )silk_abs( pulses[ i + 3 ] );
}
/* Calc sum pulses per shell code frame */
abs_pulses_ptr = abs_pulses;
for( i = 0; i < iter; i++ ) {
nRshifts[ i ] = 0;
while( 1 ) {
/* 1+1 -> 2 */
scale_down = combine_and_check( pulses_comb, abs_pulses_ptr, silk_max_pulses_table[ 0 ], 8 );
/* 2+2 -> 4 */
scale_down += combine_and_check( pulses_comb, pulses_comb, silk_max_pulses_table[ 1 ], 4 );
/* 4+4 -> 8 */
scale_down += combine_and_check( pulses_comb, pulses_comb, silk_max_pulses_table[ 2 ], 2 );
/* 8+8 -> 16 */
scale_down += combine_and_check( &sum_pulses[ i ], pulses_comb, silk_max_pulses_table[ 3 ], 1 );
if( scale_down ) {
/* We need to downscale the quantization signal */
nRshifts[ i ]++;
for( k = 0; k < SHELL_CODEC_FRAME_LENGTH; k++ ) {
abs_pulses_ptr[ k ] = silk_RSHIFT( abs_pulses_ptr[ k ], 1 );
}
} else {
/* Jump out of while(1) loop and go to next shell coding frame */
break;
}
}
abs_pulses_ptr += SHELL_CODEC_FRAME_LENGTH;
}
/**************/
/* Rate level */
/**************/
/* find rate level that leads to fewest bits for coding of pulses per block info */
minSumBits_Q5 = silk_int32_MAX;
for( k = 0; k < N_RATE_LEVELS - 1; k++ ) {
nBits_ptr = silk_pulses_per_block_BITS_Q5[ k ];
sumBits_Q5 = silk_rate_levels_BITS_Q5[ signalType >> 1 ][ k ];
for( i = 0; i < iter; i++ ) {
if( nRshifts[ i ] > 0 ) {
sumBits_Q5 += nBits_ptr[ MAX_PULSES + 1 ];
} else {
sumBits_Q5 += nBits_ptr[ sum_pulses[ i ] ];
}
}
if( sumBits_Q5 < minSumBits_Q5 ) {
minSumBits_Q5 = sumBits_Q5;
RateLevelIndex = k;
}
}
ec_enc_icdf( psRangeEnc, RateLevelIndex, silk_rate_levels_iCDF[ signalType >> 1 ], 8 );
/***************************************************/
/* Sum-Weighted-Pulses Encoding */
/***************************************************/
cdf_ptr = silk_pulses_per_block_iCDF[ RateLevelIndex ];
for( i = 0; i < iter; i++ ) {
if( nRshifts[ i ] == 0 ) {
ec_enc_icdf( psRangeEnc, sum_pulses[ i ], cdf_ptr, 8 );
} else {
ec_enc_icdf( psRangeEnc, MAX_PULSES + 1, cdf_ptr, 8 );
for( k = 0; k < nRshifts[ i ] - 1; k++ ) {
ec_enc_icdf( psRangeEnc, MAX_PULSES + 1, silk_pulses_per_block_iCDF[ N_RATE_LEVELS - 1 ], 8 );
}
ec_enc_icdf( psRangeEnc, sum_pulses[ i ], silk_pulses_per_block_iCDF[ N_RATE_LEVELS - 1 ], 8 );
}
}
/******************/
/* Shell Encoding */
/******************/
for( i = 0; i < iter; i++ ) {
if( sum_pulses[ i ] > 0 ) {
silk_shell_encoder( psRangeEnc, &abs_pulses[ i * SHELL_CODEC_FRAME_LENGTH ] );
}
}
/****************/
/* LSB Encoding */
/****************/
for( i = 0; i < iter; i++ ) {
if( nRshifts[ i ] > 0 ) {
pulses_ptr = &pulses[ i * SHELL_CODEC_FRAME_LENGTH ];
nLS = nRshifts[ i ] - 1;
for( k = 0; k < SHELL_CODEC_FRAME_LENGTH; k++ ) {
abs_q = (opus_int8)silk_abs( pulses_ptr[ k ] );
for( j = nLS; j > 0; j-- ) {
bit = silk_RSHIFT( abs_q, j ) & 1;
ec_enc_icdf( psRangeEnc, bit, silk_lsb_iCDF, 8 );
}
bit = abs_q & 1;
ec_enc_icdf( psRangeEnc, bit, silk_lsb_iCDF, 8 );
}
}
}
/****************/
/* Encode signs */
/****************/
silk_encode_signs( psRangeEnc, pulses, frame_length, signalType, quantOffsetType, sum_pulses );
}
/* test if LPC coefficients are stable (all poles within unit circle) */
static opus_int32 LPC_inverse_pred_gain_QA_c( /* O Returns inverse prediction gain in energy domain, Q30 */
opus_int32 A_QA[ SILK_MAX_ORDER_LPC ], /* I Prediction coefficients */
const opus_int order /* I Prediction order */
)
{
opus_int k, n, mult2Q;
opus_int32 invGain_Q30, rc_Q31, rc_mult1_Q30, rc_mult2, tmp1, tmp2;
invGain_Q30 = SILK_FIX_CONST( 1, 30 );
for( k = order - 1; k > 0; k-- ) {
/* Check for stability */
if( ( A_QA[ k ] > A_LIMIT ) || ( A_QA[ k ] < -A_LIMIT ) ) {
return 0;
}
/* Set RC equal to negated AR coef */
rc_Q31 = -silk_LSHIFT( A_QA[ k ], 31 - QA );
/* rc_mult1_Q30 range: [ 1 : 2^30 ] */
rc_mult1_Q30 = silk_SUB32( SILK_FIX_CONST( 1, 30 ), silk_SMMUL( rc_Q31, rc_Q31 ) );
silk_assert( rc_mult1_Q30 > ( 1 << 15 ) ); /* reduce A_LIMIT if fails */
silk_assert( rc_mult1_Q30 <= ( 1 << 30 ) );
/* Update inverse gain */
/* invGain_Q30 range: [ 0 : 2^30 ] */
invGain_Q30 = silk_LSHIFT( silk_SMMUL( invGain_Q30, rc_mult1_Q30 ), 2 );
silk_assert( invGain_Q30 >= 0 );
silk_assert( invGain_Q30 <= ( 1 << 30 ) );
if( invGain_Q30 < SILK_FIX_CONST( 1.0f / MAX_PREDICTION_POWER_GAIN, 30 ) ) {
return 0;
}
/* rc_mult2 range: [ 2^30 : silk_int32_MAX ] */
mult2Q = 32 - silk_CLZ32( silk_abs( rc_mult1_Q30 ) );
rc_mult2 = silk_INVERSE32_varQ( rc_mult1_Q30, mult2Q + 30 );
/* Update AR coefficient */
for( n = 0; n < (k + 1) >> 1; n++ ) {
opus_int64 tmp64;
tmp1 = A_QA[ n ];
tmp2 = A_QA[ k - n - 1 ];
tmp64 = silk_RSHIFT_ROUND64( silk_SMULL( silk_SUB_SAT32(tmp1,
MUL32_FRAC_Q( tmp2, rc_Q31, 31 ) ), rc_mult2 ), mult2Q);
if( tmp64 > silk_int32_MAX || tmp64 < silk_int32_MIN ) {
return 0;
}
A_QA[ n ] = ( opus_int32 )tmp64;
tmp64 = silk_RSHIFT_ROUND64( silk_SMULL( silk_SUB_SAT32(tmp2,
MUL32_FRAC_Q( tmp1, rc_Q31, 31 ) ), rc_mult2), mult2Q);
if( tmp64 > silk_int32_MAX || tmp64 < silk_int32_MIN ) {
return 0;
}
A_QA[ k - n - 1 ] = ( opus_int32 )tmp64;
}
}
/* Check for stability */
if( ( A_QA[ k ] > A_LIMIT ) || ( A_QA[ k ] < -A_LIMIT ) ) {
return 0;
}
/* Set RC equal to negated AR coef */
rc_Q31 = -silk_LSHIFT( A_QA[ 0 ], 31 - QA );
/* Range: [ 1 : 2^30 ] */
rc_mult1_Q30 = silk_SUB32( SILK_FIX_CONST( 1, 30 ), silk_SMMUL( rc_Q31, rc_Q31 ) );
/* Update inverse gain */
/* Range: [ 0 : 2^30 ] */
invGain_Q30 = silk_LSHIFT( silk_SMMUL( invGain_Q30, rc_mult1_Q30 ), 2 );
silk_assert( invGain_Q30 >= 0 );
silk_assert( invGain_Q30 <= ( 1 << 30 ) );
if( invGain_Q30 < SILK_FIX_CONST( 1.0f / MAX_PREDICTION_POWER_GAIN, 30 ) ) {
return 0;
}
return invGain_Q30;
}
/* Residual energy: nrg = wxx - 2 * wXx * c + c' * wXX * c */
opus_int32 silk_residual_energy16_covar_FIX(
const opus_int16 *c, /* I Prediction vector */
const opus_int32 *wXX, /* I Correlation matrix */
const opus_int32 *wXx, /* I Correlation vector */
opus_int32 wxx, /* I Signal energy */
opus_int D, /* I Dimension */
opus_int cQ /* I Q value for c vector 0 - 15 */
)
{
opus_int i, j, lshifts, Qxtra;
opus_int32 c_max, w_max, tmp, tmp2, nrg;
opus_int cn[ MAX_MATRIX_SIZE ];
const opus_int32 *pRow;
/* Safety checks */
silk_assert( D >= 0 );
silk_assert( D <= 16 );
silk_assert( cQ > 0 );
silk_assert( cQ < 16 );
lshifts = 16 - cQ;
Qxtra = lshifts;
c_max = 0;
for( i = 0; i < D; i++ ) {
c_max = silk_max_32( c_max, silk_abs( (opus_int32)c[ i ] ) );
}
Qxtra = silk_min_int( Qxtra, silk_CLZ32( c_max ) - 17 );
w_max = silk_max_32( wXX[ 0 ], wXX[ D * D - 1 ] );
Qxtra = silk_min_int( Qxtra, silk_CLZ32( silk_MUL( D, silk_RSHIFT( silk_SMULWB( w_max, c_max ), 4 ) ) ) - 5 );
Qxtra = silk_max_int( Qxtra, 0 );
for( i = 0; i < D; i++ ) {
cn[ i ] = silk_LSHIFT( ( opus_int )c[ i ], Qxtra );
silk_assert( silk_abs(cn[i]) <= ( silk_int16_MAX + 1 ) ); /* Check that silk_SMLAWB can be used */
}
lshifts -= Qxtra;
/* Compute wxx - 2 * wXx * c */
tmp = 0;
for( i = 0; i < D; i++ ) {
tmp = silk_SMLAWB( tmp, wXx[ i ], cn[ i ] );
}
nrg = silk_RSHIFT( wxx, 1 + lshifts ) - tmp; /* Q: -lshifts - 1 */
/* Add c' * wXX * c, assuming wXX is symmetric */
tmp2 = 0;
for( i = 0; i < D; i++ ) {
tmp = 0;
pRow = &wXX[ i * D ];
for( j = i + 1; j < D; j++ ) {
tmp = silk_SMLAWB( tmp, pRow[ j ], cn[ j ] );
}
tmp = silk_SMLAWB( tmp, silk_RSHIFT( pRow[ i ], 1 ), cn[ i ] );
tmp2 = silk_SMLAWB( tmp2, tmp, cn[ i ] );
}
nrg = silk_ADD_LSHIFT32( nrg, tmp2, lshifts ); /* Q: -lshifts - 1 */
/* Keep one bit free always, because we add them for LSF interpolation */
if( nrg < 1 ) {
nrg = 1;
} else if( nrg > silk_RSHIFT( silk_int32_MAX, lshifts + 2 ) ) {
nrg = silk_int32_MAX >> 1;
} else {
/* If not all roots are found, the a_Q16 coefficients are bandwidth expanded until convergence. */
void silk_A2NLSF(
opus_int16 *NLSF, /* O Normalized Line Spectral Frequencies in Q15 (0..2^15-1) [d] */
opus_int32 *a_Q16, /* I/O Monic whitening filter coefficients in Q16 [d] */
const opus_int d /* I Filter order (must be even) */
)
{
opus_int i, k, m, dd, root_ix, ffrac;
opus_int32 xlo, xhi, xmid;
opus_int32 ylo, yhi, ymid, thr;
opus_int32 nom, den;
opus_int32 P[ SILK_MAX_ORDER_LPC / 2 + 1 ];
opus_int32 Q[ SILK_MAX_ORDER_LPC / 2 + 1 ];
opus_int32 *PQ[ 2 ];
opus_int32 *p;
/* Store pointers to array */
PQ[ 0 ] = P;
PQ[ 1 ] = Q;
dd = silk_RSHIFT( d, 1 );
silk_A2NLSF_init( a_Q16, P, Q, dd );
/* Find roots, alternating between P and Q */
p = P; /* Pointer to polynomial */
xlo = silk_LSFCosTab_FIX_Q12[ 0 ]; /* Q12*/
ylo = silk_A2NLSF_eval_poly( p, xlo, dd );
if( ylo < 0 ) {
/* Set the first NLSF to zero and move on to the next */
NLSF[ 0 ] = 0;
p = Q; /* Pointer to polynomial */
ylo = silk_A2NLSF_eval_poly( p, xlo, dd );
root_ix = 1; /* Index of current root */
} else {
root_ix = 0; /* Index of current root */
}
k = 1; /* Loop counter */
i = 0; /* Counter for bandwidth expansions applied */
thr = 0;
while( 1 ) {
/* Evaluate polynomial */
xhi = silk_LSFCosTab_FIX_Q12[ k ]; /* Q12 */
yhi = silk_A2NLSF_eval_poly( p, xhi, dd );
/* Detect zero crossing */
if( ( ylo <= 0 && yhi >= thr ) || ( ylo >= 0 && yhi <= -thr ) ) {
if( yhi == 0 ) {
/* If the root lies exactly at the end of the current */
/* interval, look for the next root in the next interval */
thr = 1;
} else {
thr = 0;
}
/* Binary division */
ffrac = -256;
for( m = 0; m < BIN_DIV_STEPS_A2NLSF_FIX; m++ ) {
/* Evaluate polynomial */
xmid = silk_RSHIFT_ROUND( xlo + xhi, 1 );
ymid = silk_A2NLSF_eval_poly( p, xmid, dd );
/* Detect zero crossing */
if( ( ylo <= 0 && ymid >= 0 ) || ( ylo >= 0 && ymid <= 0 ) ) {
/* Reduce frequency */
xhi = xmid;
yhi = ymid;
} else {
/* Increase frequency */
xlo = xmid;
ylo = ymid;
ffrac = silk_ADD_RSHIFT( ffrac, 128, m );
}
}
/* Interpolate */
if( silk_abs( ylo ) < 65536 ) {
/* Avoid dividing by zero */
den = ylo - yhi;
nom = silk_LSHIFT( ylo, 8 - BIN_DIV_STEPS_A2NLSF_FIX ) + silk_RSHIFT( den, 1 );
if( den != 0 ) {
ffrac += silk_DIV32( nom, den );
}
} else {
/* No risk of dividing by zero because abs(ylo - yhi) >= abs(ylo) >= 65536 */
ffrac += silk_DIV32( ylo, silk_RSHIFT( ylo - yhi, 8 - BIN_DIV_STEPS_A2NLSF_FIX ) );
}
NLSF[ root_ix ] = (opus_int16)silk_min_32( silk_LSHIFT( (opus_int32)k, 8 ) + ffrac, silk_int16_MAX );
silk_assert( NLSF[ root_ix ] >= 0 );
root_ix++; /* Next root */
if( root_ix >= d ) {
/* Found all roots */
break;
}
/* Alternate pointer to polynomial */
p = PQ[ root_ix & 1 ];
/* Evaluate polynomial */
xlo = silk_LSFCosTab_FIX_Q12[ k - 1 ]; /* Q12*/
ylo = silk_LSHIFT( 1 - ( root_ix & 2 ), 12 );
} else {
/* Increment loop counter */
k++;
xlo = xhi;
ylo = yhi;
thr = 0;
if( k > LSF_COS_TAB_SZ_FIX ) {
i++;
if( i > MAX_ITERATIONS_A2NLSF_FIX ) {
/* Set NLSFs to white spectrum and exit */
NLSF[ 0 ] = (opus_int16)silk_DIV32_16( 1 << 15, d + 1 );
for( k = 1; k < d; k++ ) {
NLSF[ k ] = (opus_int16)silk_SMULBB( k + 1, NLSF[ 0 ] );
}
return;
}
/* Error: Apply progressively more bandwidth expansion and run again */
silk_bwexpander_32( a_Q16, d, 65536 - silk_SMULBB( 10 + i, i ) ); /* 10_Q16 = 0.00015*/
silk_A2NLSF_init( a_Q16, P, Q, dd );
p = P; /* Pointer to polynomial */
xlo = silk_LSFCosTab_FIX_Q12[ 0 ]; /* Q12*/
ylo = silk_A2NLSF_eval_poly( p, xlo, dd );
if( ylo < 0 ) {
/* Set the first NLSF to zero and move on to the next */
NLSF[ 0 ] = 0;
p = Q; /* Pointer to polynomial */
ylo = silk_A2NLSF_eval_poly( p, xlo, dd );
root_ix = 1; /* Index of current root */
} else {
root_ix = 0; /* Index of current root */
}
k = 1; /* Reset loop counter */
}
}
}
}
/* test if LPC coefficients are stable (all poles within unit circle) */
static opus_int32 LPC_inverse_pred_gain_QA( /* O Returns inverse prediction gain in energy domain, Q30 */
opus_int32 A_QA[ 2 ][ SILK_MAX_ORDER_LPC ], /* I Prediction coefficients */
const opus_int order /* I Prediction order */
)
{
opus_int k, n, mult2Q;
opus_int32 invGain_Q30, rc_Q31, rc_mult1_Q30, rc_mult2, tmp_QA;
opus_int32 *Aold_QA, *Anew_QA;
Anew_QA = A_QA[ order & 1 ];
invGain_Q30 = (opus_int32)1 << 30;
for( k = order - 1; k > 0; k-- ) {
/* Check for stability */
if( ( Anew_QA[ k ] > A_LIMIT ) || ( Anew_QA[ k ] < -A_LIMIT ) ) {
return 0;
}
/* Set RC equal to negated AR coef */
rc_Q31 = -silk_LSHIFT( Anew_QA[ k ], 31 - QA );
/* rc_mult1_Q30 range: [ 1 : 2^30 ] */
rc_mult1_Q30 = ( (opus_int32)1 << 30 ) - silk_SMMUL( rc_Q31, rc_Q31 );
silk_assert( rc_mult1_Q30 > ( 1 << 15 ) ); /* reduce A_LIMIT if fails */
silk_assert( rc_mult1_Q30 <= ( 1 << 30 ) );
/* rc_mult2 range: [ 2^30 : silk_int32_MAX ] */
mult2Q = 32 - silk_CLZ32( silk_abs( rc_mult1_Q30 ) );
rc_mult2 = silk_INVERSE32_varQ( rc_mult1_Q30, mult2Q + 30 );
/* Update inverse gain */
/* invGain_Q30 range: [ 0 : 2^30 ] */
invGain_Q30 = silk_LSHIFT( silk_SMMUL( invGain_Q30, rc_mult1_Q30 ), 2 );
silk_assert( invGain_Q30 >= 0 );
silk_assert( invGain_Q30 <= ( 1 << 30 ) );
/* Swap pointers */
Aold_QA = Anew_QA;
Anew_QA = A_QA[ k & 1 ];
/* Update AR coefficient */
for( n = 0; n < k; n++ ) {
tmp_QA = Aold_QA[ n ] - MUL32_FRAC_Q( Aold_QA[ k - n - 1 ], rc_Q31, 31 );
Anew_QA[ n ] = MUL32_FRAC_Q( tmp_QA, rc_mult2 , mult2Q );
}
}
/* Check for stability */
if( ( Anew_QA[ 0 ] > A_LIMIT ) || ( Anew_QA[ 0 ] < -A_LIMIT ) ) {
return 0;
}
/* Set RC equal to negated AR coef */
rc_Q31 = -silk_LSHIFT( Anew_QA[ 0 ], 31 - QA );
/* Range: [ 1 : 2^30 ] */
rc_mult1_Q30 = ( (opus_int32)1 << 30 ) - silk_SMMUL( rc_Q31, rc_Q31 );
/* Update inverse gain */
/* Range: [ 0 : 2^30 ] */
invGain_Q30 = silk_LSHIFT( silk_SMMUL( invGain_Q30, rc_mult1_Q30 ), 2 );
silk_assert( invGain_Q30 >= 0 );
silk_assert( invGain_Q30 <= 1<<30 );
return invGain_Q30;
}
/* Compute reflection coefficients from input signal */
void silk_burg_modified(
opus_int32 *res_nrg, /* O Residual energy */
opus_int *res_nrg_Q, /* O Residual energy Q value */
opus_int32 A_Q16[], /* O Prediction coefficients (length order) */
const opus_int16 x[], /* I Input signal, length: nb_subfr * ( D + subfr_length ) */
const opus_int subfr_length, /* I Input signal subframe length (incl. D preceeding samples) */
const opus_int nb_subfr, /* I Number of subframes stacked in x */
const opus_int32 WhiteNoiseFrac_Q32, /* I Fraction added to zero-lag autocorrelation */
const opus_int D /* I Order */
)
{
opus_int k, n, s, lz, rshifts, rshifts_extra;
opus_int32 C0, num, nrg, rc_Q31, Atmp_QA, Atmp1, tmp1, tmp2, x1, x2;
const opus_int16 *x_ptr;
opus_int32 C_first_row[ SILK_MAX_ORDER_LPC ];
opus_int32 C_last_row[ SILK_MAX_ORDER_LPC ];
opus_int32 Af_QA[ SILK_MAX_ORDER_LPC ];
opus_int32 CAf[ SILK_MAX_ORDER_LPC + 1 ];
opus_int32 CAb[ SILK_MAX_ORDER_LPC + 1 ];
silk_assert( subfr_length * nb_subfr <= MAX_FRAME_SIZE );
silk_assert( nb_subfr <= MAX_NB_SUBFR );
/* Compute autocorrelations, added over subframes */
silk_sum_sqr_shift( &C0, &rshifts, x, nb_subfr * subfr_length );
if( rshifts > MAX_RSHIFTS ) {
C0 = silk_LSHIFT32( C0, rshifts - MAX_RSHIFTS );
silk_assert( C0 > 0 );
rshifts = MAX_RSHIFTS;
} else {
lz = silk_CLZ32( C0 ) - 1;
rshifts_extra = N_BITS_HEAD_ROOM - lz;
if( rshifts_extra > 0 ) {
rshifts_extra = silk_min( rshifts_extra, MAX_RSHIFTS - rshifts );
C0 = silk_RSHIFT32( C0, rshifts_extra );
} else {
rshifts_extra = silk_max( rshifts_extra, MIN_RSHIFTS - rshifts );
C0 = silk_LSHIFT32( C0, -rshifts_extra );
}
rshifts += rshifts_extra;
}
silk_memset( C_first_row, 0, SILK_MAX_ORDER_LPC * sizeof( opus_int32 ) );
if( rshifts > 0 ) {
for( s = 0; s < nb_subfr; s++ ) {
x_ptr = x + s * subfr_length;
for( n = 1; n < D + 1; n++ ) {
C_first_row[ n - 1 ] += (opus_int32)silk_RSHIFT64(
silk_inner_prod16_aligned_64( x_ptr, x_ptr + n, subfr_length - n ), rshifts );
}
}
} else {
for( s = 0; s < nb_subfr; s++ ) {
x_ptr = x + s * subfr_length;
for( n = 1; n < D + 1; n++ ) {
C_first_row[ n - 1 ] += silk_LSHIFT32(
silk_inner_prod_aligned( x_ptr, x_ptr + n, subfr_length - n ), -rshifts );
}
}
}
silk_memcpy( C_last_row, C_first_row, SILK_MAX_ORDER_LPC * sizeof( opus_int32 ) );
/* Initialize */
CAb[ 0 ] = CAf[ 0 ] = C0 + silk_SMMUL( WhiteNoiseFrac_Q32, C0 ) + 1; /* Q(-rshifts)*/
for( n = 0; n < D; n++ ) {
/* Update first row of correlation matrix (without first element) */
/* Update last row of correlation matrix (without last element, stored in reversed order) */
/* Update C * Af */
/* Update C * flipud(Af) (stored in reversed order) */
if( rshifts > -2 ) {
for( s = 0; s < nb_subfr; s++ ) {
x_ptr = x + s * subfr_length;
x1 = -silk_LSHIFT32( (opus_int32)x_ptr[ n ], 16 - rshifts ); /* Q(16-rshifts)*/
x2 = -silk_LSHIFT32( (opus_int32)x_ptr[ subfr_length - n - 1 ], 16 - rshifts ); /* Q(16-rshifts)*/
tmp1 = silk_LSHIFT32( (opus_int32)x_ptr[ n ], QA - 16 ); /* Q(QA-16)*/
tmp2 = silk_LSHIFT32( (opus_int32)x_ptr[ subfr_length - n - 1 ], QA - 16 ); /* Q(QA-16)*/
for( k = 0; k < n; k++ ) {
C_first_row[ k ] = silk_SMLAWB( C_first_row[ k ], x1, x_ptr[ n - k - 1 ] ); /* Q( -rshifts )*/
C_last_row[ k ] = silk_SMLAWB( C_last_row[ k ], x2, x_ptr[ subfr_length - n + k ] ); /* Q( -rshifts )*/
Atmp_QA = Af_QA[ k ];
tmp1 = silk_SMLAWB( tmp1, Atmp_QA, x_ptr[ n - k - 1 ] ); /* Q(QA-16)*/
tmp2 = silk_SMLAWB( tmp2, Atmp_QA, x_ptr[ subfr_length - n + k ] ); /* Q(QA-16)*/
}
tmp1 = silk_LSHIFT32( -tmp1, 32 - QA - rshifts ); /* Q(16-rshifts)*/
tmp2 = silk_LSHIFT32( -tmp2, 32 - QA - rshifts ); /* Q(16-rshifts)*/
for( k = 0; k <= n; k++ ) {
CAf[ k ] = silk_SMLAWB( CAf[ k ], tmp1, x_ptr[ n - k ] ); /* Q( -rshift )*/
CAb[ k ] = silk_SMLAWB( CAb[ k ], tmp2, x_ptr[ subfr_length - n + k - 1 ] ); /* Q( -rshift )*/
}
}
} else {
for( s = 0; s < nb_subfr; s++ ) {
x_ptr = x + s * subfr_length;
x1 = -silk_LSHIFT32( (opus_int32)x_ptr[ n ], -rshifts ); /* Q( -rshifts )*/
x2 = -silk_LSHIFT32( (opus_int32)x_ptr[ subfr_length - n - 1 ], -rshifts ); /* Q( -rshifts )*/
tmp1 = silk_LSHIFT32( (opus_int32)x_ptr[ n ], 17 ); /* Q17*/
tmp2 = silk_LSHIFT32( (opus_int32)x_ptr[ subfr_length - n - 1 ], 17 ); /* Q17*/
for( k = 0; k < n; k++ ) {
C_first_row[ k ] = silk_MLA( C_first_row[ k ], x1, x_ptr[ n - k - 1 ] ); /* Q( -rshifts )*/
C_last_row[ k ] = silk_MLA( C_last_row[ k ], x2, x_ptr[ subfr_length - n + k ] ); /* Q( -rshifts )*/
Atmp1 = silk_RSHIFT_ROUND( Af_QA[ k ], QA - 17 ); /* Q17*/
tmp1 = silk_MLA( tmp1, x_ptr[ n - k - 1 ], Atmp1 ); /* Q17*/
tmp2 = silk_MLA( tmp2, x_ptr[ subfr_length - n + k ], Atmp1 ); /* Q17*/
}
tmp1 = -tmp1; /* Q17*/
tmp2 = -tmp2; /* Q17*/
for( k = 0; k <= n; k++ ) {
CAf[ k ] = silk_SMLAWW( CAf[ k ], tmp1,
silk_LSHIFT32( (opus_int32)x_ptr[ n - k ], -rshifts - 1 ) ); /* Q( -rshift )*/
CAb[ k ] = silk_SMLAWW( CAb[ k ], tmp2,
silk_LSHIFT32( (opus_int32)x_ptr[ subfr_length - n + k - 1 ], -rshifts - 1 ) ); /* Q( -rshift )*/
}
}
}
/* Calculate nominator and denominator for the next order reflection (parcor) coefficient */
tmp1 = C_first_row[ n ]; /* Q( -rshifts )*/
tmp2 = C_last_row[ n ]; /* Q( -rshifts )*/
num = 0; /* Q( -rshifts )*/
nrg = silk_ADD32( CAb[ 0 ], CAf[ 0 ] ); /* Q( 1-rshifts )*/
for( k = 0; k < n; k++ ) {
Atmp_QA = Af_QA[ k ];
lz = silk_CLZ32( silk_abs( Atmp_QA ) ) - 1;
lz = silk_min( 32 - QA, lz );
Atmp1 = silk_LSHIFT32( Atmp_QA, lz ); /* Q( QA + lz )*/
tmp1 = silk_ADD_LSHIFT32( tmp1, silk_SMMUL( C_last_row[ n - k - 1 ], Atmp1 ), 32 - QA - lz ); /* Q( -rshifts )*/
tmp2 = silk_ADD_LSHIFT32( tmp2, silk_SMMUL( C_first_row[ n - k - 1 ], Atmp1 ), 32 - QA - lz ); /* Q( -rshifts )*/
num = silk_ADD_LSHIFT32( num, silk_SMMUL( CAb[ n - k ], Atmp1 ), 32 - QA - lz ); /* Q( -rshifts )*/
nrg = silk_ADD_LSHIFT32( nrg, silk_SMMUL( silk_ADD32( CAb[ k + 1 ], CAf[ k + 1 ] ),
Atmp1 ), 32 - QA - lz ); /* Q( 1-rshifts )*/
}
CAf[ n + 1 ] = tmp1; /* Q( -rshifts )*/
CAb[ n + 1 ] = tmp2; /* Q( -rshifts )*/
num = silk_ADD32( num, tmp2 ); /* Q( -rshifts )*/
num = silk_LSHIFT32( -num, 1 ); /* Q( 1-rshifts )*/
/* Calculate the next order reflection (parcor) coefficient */
if( silk_abs( num ) < nrg ) {
rc_Q31 = silk_DIV32_varQ( num, nrg, 31 );
} else {
/* Negative energy or ratio too high; set remaining coefficients to zero and exit loop */
silk_memset( &Af_QA[ n ], 0, ( D - n ) * sizeof( opus_int32 ) );
silk_assert( 0 );
break;
}
/* Update the AR coefficients */
for( k = 0; k < (n + 1) >> 1; k++ ) {
tmp1 = Af_QA[ k ]; /* QA*/
tmp2 = Af_QA[ n - k - 1 ]; /* QA*/
Af_QA[ k ] = silk_ADD_LSHIFT32( tmp1, silk_SMMUL( tmp2, rc_Q31 ), 1 ); /* QA*/
Af_QA[ n - k - 1 ] = silk_ADD_LSHIFT32( tmp2, silk_SMMUL( tmp1, rc_Q31 ), 1 ); /* QA*/
}
Af_QA[ n ] = silk_RSHIFT32( rc_Q31, 31 - QA ); /* QA*/
/* Update C * Af and C * Ab */
for( k = 0; k <= n + 1; k++ ) {
tmp1 = CAf[ k ]; /* Q( -rshifts )*/
tmp2 = CAb[ n - k + 1 ]; /* Q( -rshifts )*/
CAf[ k ] = silk_ADD_LSHIFT32( tmp1, silk_SMMUL( tmp2, rc_Q31 ), 1 ); /* Q( -rshifts )*/
CAb[ n - k + 1 ] = silk_ADD_LSHIFT32( tmp2, silk_SMMUL( tmp1, rc_Q31 ), 1 ); /* Q( -rshifts )*/
}
}
/* Return residual energy */
nrg = CAf[ 0 ]; /* Q( -rshifts )*/
tmp1 = 1 << 16; /* Q16*/
for( k = 0; k < D; k++ ) {
Atmp1 = silk_RSHIFT_ROUND( Af_QA[ k ], QA - 16 ); /* Q16*/
nrg = silk_SMLAWW( nrg, CAf[ k + 1 ], Atmp1 ); /* Q( -rshifts )*/
tmp1 = silk_SMLAWW( tmp1, Atmp1, Atmp1 ); /* Q16*/
A_Q16[ k ] = -Atmp1;
}
*res_nrg = silk_SMLAWW( nrg, silk_SMMUL( WhiteNoiseFrac_Q32, C0 ), -tmp1 ); /* Q( -rshifts )*/
*res_nrg_Q = -rshifts;
}
/* compute whitening filter coefficients from normalized line spectral frequencies */
void silk_NLSF2A(
opus_int16 *a_Q12, /* O monic whitening filter coefficients in Q12, [ d ] */
const opus_int16 *NLSF, /* I normalized line spectral frequencies in Q15, [ d ] */
const opus_int d /* I filter order (should be even) */
)
{
/* This ordering was found to maximize quality. It improves numerical accuracy of
silk_NLSF2A_find_poly() compared to "standard" ordering. */
static const unsigned char ordering16[16] = {
0, 15, 8, 7, 4, 11, 12, 3, 2, 13, 10, 5, 6, 9, 14, 1
};
static const unsigned char ordering10[10] = {
0, 9, 6, 3, 4, 5, 8, 1, 2, 7
};
const unsigned char *ordering;
opus_int k, i, dd;
opus_int32 cos_LSF_QA[ SILK_MAX_ORDER_LPC ];
opus_int32 P[ SILK_MAX_ORDER_LPC / 2 + 1 ], Q[ SILK_MAX_ORDER_LPC / 2 + 1 ];
opus_int32 Ptmp, Qtmp, f_int, f_frac, cos_val, delta;
opus_int32 a32_QA1[ SILK_MAX_ORDER_LPC ];
opus_int32 maxabs, absval, idx=0, sc_Q16;
silk_assert( LSF_COS_TAB_SZ_FIX == 128 );
silk_assert( d==10||d==16 );
/* convert LSFs to 2*cos(LSF), using piecewise linear curve from table */
ordering = d == 16 ? ordering16 : ordering10;
for( k = 0; k < d; k++ ) {
silk_assert(NLSF[k] >= 0 );
/* f_int on a scale 0-127 (rounded down) */
f_int = silk_RSHIFT( NLSF[k], 15 - 7 );
/* f_frac, range: 0..255 */
f_frac = NLSF[k] - silk_LSHIFT( f_int, 15 - 7 );
silk_assert(f_int >= 0);
silk_assert(f_int < LSF_COS_TAB_SZ_FIX );
/* Read start and end value from table */
cos_val = silk_LSFCosTab_FIX_Q12[ f_int ]; /* Q12 */
delta = silk_LSFCosTab_FIX_Q12[ f_int + 1 ] - cos_val; /* Q12, with a range of 0..200 */
/* Linear interpolation */
cos_LSF_QA[ordering[k]] = silk_RSHIFT_ROUND( silk_LSHIFT( cos_val, 8 ) + silk_MUL( delta, f_frac ), 20 - QA ); /* QA */
}
dd = silk_RSHIFT( d, 1 );
/* generate even and odd polynomials using convolution */
silk_NLSF2A_find_poly( P, &cos_LSF_QA[ 0 ], dd );
silk_NLSF2A_find_poly( Q, &cos_LSF_QA[ 1 ], dd );
/* convert even and odd polynomials to opus_int32 Q12 filter coefs */
for( k = 0; k < dd; k++ ) {
Ptmp = P[ k+1 ] + P[ k ];
Qtmp = Q[ k+1 ] - Q[ k ];
/* the Ptmp and Qtmp values at this stage need to fit in int32 */
a32_QA1[ k ] = -Qtmp - Ptmp; /* QA+1 */
a32_QA1[ d-k-1 ] = Qtmp - Ptmp; /* QA+1 */
}
/* Limit the maximum absolute value of the prediction coefficients, so that they'll fit in int16 */
for( i = 0; i < 10; i++ ) {
/* Find maximum absolute value and its index */
maxabs = 0;
for( k = 0; k < d; k++ ) {
absval = silk_abs( a32_QA1[k] );
if( absval > maxabs ) {
maxabs = absval;
idx = k;
}
}
maxabs = silk_RSHIFT_ROUND( maxabs, QA + 1 - 12 ); /* QA+1 -> Q12 */
if( maxabs > silk_int16_MAX ) {
/* Reduce magnitude of prediction coefficients */
maxabs = silk_min( maxabs, 163838 ); /* ( silk_int32_MAX >> 14 ) + silk_int16_MAX = 163838 */
sc_Q16 = SILK_FIX_CONST( 0.999, 16 ) - silk_DIV32( silk_LSHIFT( maxabs - silk_int16_MAX, 14 ),
silk_RSHIFT32( silk_MUL( maxabs, idx + 1), 2 ) );
silk_bwexpander_32( a32_QA1, d, sc_Q16 );
} else {
break;
}
}
if( i == 10 ) {
/* Reached the last iteration, clip the coefficients */
for( k = 0; k < d; k++ ) {
a_Q12[ k ] = (opus_int16)silk_SAT16( silk_RSHIFT_ROUND( a32_QA1[ k ], QA + 1 - 12 ) ); /* QA+1 -> Q12 */
a32_QA1[ k ] = silk_LSHIFT( (opus_int32)a_Q12[ k ], QA + 1 - 12 );
}
} else {
for( k = 0; k < d; k++ ) {
a_Q12[ k ] = (opus_int16)silk_RSHIFT_ROUND( a32_QA1[ k ], QA + 1 - 12 ); /* QA+1 -> Q12 */
}
}
for( i = 0; i < MAX_LPC_STABILIZE_ITERATIONS; i++ ) {
if( silk_LPC_inverse_pred_gain( a_Q12, d ) < SILK_FIX_CONST( 1.0 / MAX_PREDICTION_POWER_GAIN, 30 ) ) {
/* Prediction coefficients are (too close to) unstable; apply bandwidth expansion */
/* on the unscaled coefficients, convert to Q12 and measure again */
silk_bwexpander_32( a32_QA1, d, 65536 - silk_LSHIFT( 2, i ) );
for( k = 0; k < d; k++ ) {
a_Q12[ k ] = (opus_int16)silk_RSHIFT_ROUND( a32_QA1[ k ], QA + 1 - 12 ); /* QA+1 -> Q12 */
}
} else {
break;
}
}
}
/* Compute reflection coefficients from input signal */
void silk_burg_modified_sse4_1(
opus_int32 *res_nrg, /* O Residual energy */
opus_int *res_nrg_Q, /* O Residual energy Q value */
opus_int32 A_Q16[], /* O Prediction coefficients (length order) */
const opus_int16 x[], /* I Input signal, length: nb_subfr * (D + subfr_length) */
const opus_int32 minInvGain_Q30, /* I Inverse of max prediction gain */
const opus_int subfr_length, /* I Input signal subframe length (incl. D preceding samples) */
const opus_int nb_subfr, /* I Number of subframes stacked in x */
const opus_int D, /* I Order */
int arch /* I Run-time architecture */
)
{
opus_int k, n, s, lz, rshifts, rshifts_extra, reached_max_gain;
opus_int32 C0, num, nrg, rc_Q31, invGain_Q30, Atmp_QA, Atmp1, tmp1, tmp2, x1, x2;
const opus_int16 *x_ptr;
opus_int32 C_first_row[ SILK_MAX_ORDER_LPC ];
opus_int32 C_last_row[ SILK_MAX_ORDER_LPC ];
opus_int32 Af_QA[ SILK_MAX_ORDER_LPC ];
opus_int32 CAf[ SILK_MAX_ORDER_LPC + 1 ];
opus_int32 CAb[ SILK_MAX_ORDER_LPC + 1 ];
opus_int32 xcorr[ SILK_MAX_ORDER_LPC ];
__m128i FIRST_3210, LAST_3210, ATMP_3210, TMP1_3210, TMP2_3210, T1_3210, T2_3210, PTR_3210, SUBFR_3210, X1_3210, X2_3210;
__m128i CONST1 = _mm_set1_epi32(1);
silk_assert(subfr_length * nb_subfr <= MAX_FRAME_SIZE);
/* Compute autocorrelations, added over subframes */
silk_sum_sqr_shift(&C0, &rshifts, x, nb_subfr * subfr_length);
if(rshifts > MAX_RSHIFTS) {
C0 = silk_LSHIFT32(C0, rshifts - MAX_RSHIFTS);
silk_assert(C0 > 0);
rshifts = MAX_RSHIFTS;
} else {
lz = silk_CLZ32(C0) - 1;
rshifts_extra = N_BITS_HEAD_ROOM - lz;
if(rshifts_extra > 0) {
rshifts_extra = silk_min(rshifts_extra, MAX_RSHIFTS - rshifts);
C0 = silk_RSHIFT32(C0, rshifts_extra);
} else {
rshifts_extra = silk_max(rshifts_extra, MIN_RSHIFTS - rshifts);
C0 = silk_LSHIFT32(C0, -rshifts_extra);
}
rshifts += rshifts_extra;
}
CAb[ 0 ] = CAf[ 0 ] = C0 + silk_SMMUL(SILK_FIX_CONST(FIND_LPC_COND_FAC, 32), C0) + 1; /* Q(-rshifts) */
silk_memset(C_first_row, 0, SILK_MAX_ORDER_LPC * sizeof(opus_int32));
if(rshifts > 0) {
for(s = 0; s < nb_subfr; s++) {
x_ptr = x + s * subfr_length;
for(n = 1; n < D + 1; n++) {
C_first_row[ n - 1 ] += (opus_int32)silk_RSHIFT64(
silk_inner_prod16_aligned_64(x_ptr, x_ptr + n, subfr_length - n, arch), rshifts);
}
}
} else {
for(s = 0; s < nb_subfr; s++) {
int i;
opus_int32 d;
x_ptr = x + s * subfr_length;
celt_pitch_xcorr(x_ptr, x_ptr + 1, xcorr, subfr_length - D, D, arch);
for(n = 1; n < D + 1; n++) {
for (i = n + subfr_length - D, d = 0; i < subfr_length; i++)
d = MAC16_16(d, x_ptr[ i ], x_ptr[ i - n ]);
xcorr[ n - 1 ] += d;
}
for(n = 1; n < D + 1; n++) {
C_first_row[ n - 1 ] += silk_LSHIFT32(xcorr[ n - 1 ], -rshifts);
}
}
}
silk_memcpy(C_last_row, C_first_row, SILK_MAX_ORDER_LPC * sizeof(opus_int32));
/* Initialize */
CAb[ 0 ] = CAf[ 0 ] = C0 + silk_SMMUL(SILK_FIX_CONST(FIND_LPC_COND_FAC, 32), C0) + 1; /* Q(-rshifts) */
invGain_Q30 = (opus_int32)1 << 30;
reached_max_gain = 0;
for(n = 0; n < D; n++) {
/* Update first row of correlation matrix (without first element) */
/* Update last row of correlation matrix (without last element, stored in reversed order) */
/* Update C * Af */
/* Update C * flipud(Af) (stored in reversed order) */
if(rshifts > -2) {
for(s = 0; s < nb_subfr; s++) {
x_ptr = x + s * subfr_length;
x1 = -silk_LSHIFT32((opus_int32)x_ptr[ n ], 16 - rshifts); /* Q(16-rshifts) */
x2 = -silk_LSHIFT32((opus_int32)x_ptr[ subfr_length - n - 1 ], 16 - rshifts); /* Q(16-rshifts) */
tmp1 = silk_LSHIFT32((opus_int32)x_ptr[ n ], QA - 16); /* Q(QA-16) */
tmp2 = silk_LSHIFT32((opus_int32)x_ptr[ subfr_length - n - 1 ], QA - 16); /* Q(QA-16) */
for(k = 0; k < n; k++) {
C_first_row[ k ] = silk_SMLAWB(C_first_row[ k ], x1, x_ptr[ n - k - 1 ] ); /* Q(-rshifts) */
C_last_row[ k ] = silk_SMLAWB(C_last_row[ k ], x2, x_ptr[ subfr_length - n + k ]); /* Q(-rshifts) */
Atmp_QA = Af_QA[ k ];
tmp1 = silk_SMLAWB(tmp1, Atmp_QA, x_ptr[ n - k - 1 ] ); /* Q(QA-16) */
tmp2 = silk_SMLAWB(tmp2, Atmp_QA, x_ptr[ subfr_length - n + k ]); /* Q(QA-16) */
}
tmp1 = silk_LSHIFT32(-tmp1, 32 - QA - rshifts); /* Q(16-rshifts) */
tmp2 = silk_LSHIFT32(-tmp2, 32 - QA - rshifts); /* Q(16-rshifts) */
for(k = 0; k <= n; k++) {
CAf[ k ] = silk_SMLAWB(CAf[ k ], tmp1, x_ptr[ n - k ] ); /* Q(-rshift) */
CAb[ k ] = silk_SMLAWB(CAb[ k ], tmp2, x_ptr[ subfr_length - n + k - 1 ]); /* Q(-rshift) */
}
}
} else {
for(s = 0; s < nb_subfr; s++) {
x_ptr = x + s * subfr_length;
x1 = -silk_LSHIFT32((opus_int32)x_ptr[ n ], -rshifts); /* Q(-rshifts) */
x2 = -silk_LSHIFT32((opus_int32)x_ptr[ subfr_length - n - 1 ], -rshifts); /* Q(-rshifts) */
tmp1 = silk_LSHIFT32((opus_int32)x_ptr[ n ], 17); /* Q17 */
tmp2 = silk_LSHIFT32((opus_int32)x_ptr[ subfr_length - n - 1 ], 17); /* Q17 */
X1_3210 = _mm_set1_epi32(x1);
X2_3210 = _mm_set1_epi32(x2);
TMP1_3210 = _mm_setzero_si128();
TMP2_3210 = _mm_setzero_si128();
for(k = 0; k < n - 3; k += 4) {
PTR_3210 = OP_CVTEPI16_EPI32_M64(&x_ptr[ n - k - 1 - 3 ]);
SUBFR_3210 = OP_CVTEPI16_EPI32_M64(&x_ptr[ subfr_length - n + k ]);
FIRST_3210 = _mm_loadu_si128((__m128i *)&C_first_row[ k ]);
PTR_3210 = _mm_shuffle_epi32(PTR_3210, _MM_SHUFFLE(0, 1, 2, 3));
LAST_3210 = _mm_loadu_si128((__m128i *)&C_last_row[ k ]);
ATMP_3210 = _mm_loadu_si128((__m128i *)&Af_QA[ k ]);
T1_3210 = _mm_mullo_epi32(PTR_3210, X1_3210);
T2_3210 = _mm_mullo_epi32(SUBFR_3210, X2_3210);
ATMP_3210 = _mm_srai_epi32(ATMP_3210, 7);
ATMP_3210 = _mm_add_epi32(ATMP_3210, CONST1);
ATMP_3210 = _mm_srai_epi32(ATMP_3210, 1);
FIRST_3210 = _mm_add_epi32(FIRST_3210, T1_3210);
LAST_3210 = _mm_add_epi32(LAST_3210, T2_3210);
PTR_3210 = _mm_mullo_epi32(ATMP_3210, PTR_3210);
SUBFR_3210 = _mm_mullo_epi32(ATMP_3210, SUBFR_3210);
_mm_storeu_si128((__m128i *)&C_first_row[ k ], FIRST_3210);
_mm_storeu_si128((__m128i *)&C_last_row[ k ], LAST_3210);
TMP1_3210 = _mm_add_epi32(TMP1_3210, PTR_3210);
TMP2_3210 = _mm_add_epi32(TMP2_3210, SUBFR_3210);
}
TMP1_3210 = _mm_add_epi32(TMP1_3210, _mm_unpackhi_epi64(TMP1_3210, TMP1_3210));
TMP2_3210 = _mm_add_epi32(TMP2_3210, _mm_unpackhi_epi64(TMP2_3210, TMP2_3210));
TMP1_3210 = _mm_add_epi32(TMP1_3210, _mm_shufflelo_epi16(TMP1_3210, 0x0E));
TMP2_3210 = _mm_add_epi32(TMP2_3210, _mm_shufflelo_epi16(TMP2_3210, 0x0E));
tmp1 += _mm_cvtsi128_si32(TMP1_3210);
tmp2 += _mm_cvtsi128_si32(TMP2_3210);
for(; k < n; k++) {
C_first_row[ k ] = silk_MLA(C_first_row[ k ], x1, x_ptr[ n - k - 1 ] ); /* Q(-rshifts) */
C_last_row[ k ] = silk_MLA(C_last_row[ k ], x2, x_ptr[ subfr_length - n + k ]); /* Q(-rshifts) */
Atmp1 = silk_RSHIFT_ROUND(Af_QA[ k ], QA - 17); /* Q17 */
tmp1 = silk_MLA(tmp1, x_ptr[ n - k - 1 ], Atmp1); /* Q17 */
tmp2 = silk_MLA(tmp2, x_ptr[ subfr_length - n + k ], Atmp1); /* Q17 */
}
tmp1 = -tmp1; /* Q17 */
tmp2 = -tmp2; /* Q17 */
{
__m128i xmm_tmp1, xmm_tmp2;
__m128i xmm_x_ptr_n_k_x2x0, xmm_x_ptr_n_k_x3x1;
__m128i xmm_x_ptr_sub_x2x0, xmm_x_ptr_sub_x3x1;
xmm_tmp1 = _mm_set1_epi32(tmp1);
xmm_tmp2 = _mm_set1_epi32(tmp2);
for(k = 0; k <= n - 3; k += 4) {
xmm_x_ptr_n_k_x2x0 = OP_CVTEPI16_EPI32_M64(&x_ptr[ n - k - 3 ]);
xmm_x_ptr_sub_x2x0 = OP_CVTEPI16_EPI32_M64(&x_ptr[ subfr_length - n + k - 1 ]);
xmm_x_ptr_n_k_x2x0 = _mm_shuffle_epi32(xmm_x_ptr_n_k_x2x0, _MM_SHUFFLE(0, 1, 2, 3));
xmm_x_ptr_n_k_x2x0 = _mm_slli_epi32(xmm_x_ptr_n_k_x2x0, -rshifts - 1);
xmm_x_ptr_sub_x2x0 = _mm_slli_epi32(xmm_x_ptr_sub_x2x0, -rshifts - 1);
/* equal shift right 4 bytes, xmm_x_ptr_n_k_x3x1 = _mm_srli_si128(xmm_x_ptr_n_k_x2x0, 4)*/
xmm_x_ptr_n_k_x3x1 = _mm_shuffle_epi32(xmm_x_ptr_n_k_x2x0, _MM_SHUFFLE(0, 3, 2, 1));
xmm_x_ptr_sub_x3x1 = _mm_shuffle_epi32(xmm_x_ptr_sub_x2x0, _MM_SHUFFLE(0, 3, 2, 1));
xmm_x_ptr_n_k_x2x0 = _mm_mul_epi32(xmm_x_ptr_n_k_x2x0, xmm_tmp1);
xmm_x_ptr_n_k_x3x1 = _mm_mul_epi32(xmm_x_ptr_n_k_x3x1, xmm_tmp1);
xmm_x_ptr_sub_x2x0 = _mm_mul_epi32(xmm_x_ptr_sub_x2x0, xmm_tmp2);
xmm_x_ptr_sub_x3x1 = _mm_mul_epi32(xmm_x_ptr_sub_x3x1, xmm_tmp2);
xmm_x_ptr_n_k_x2x0 = _mm_srli_epi64(xmm_x_ptr_n_k_x2x0, 16);
xmm_x_ptr_n_k_x3x1 = _mm_slli_epi64(xmm_x_ptr_n_k_x3x1, 16);
xmm_x_ptr_sub_x2x0 = _mm_srli_epi64(xmm_x_ptr_sub_x2x0, 16);
xmm_x_ptr_sub_x3x1 = _mm_slli_epi64(xmm_x_ptr_sub_x3x1, 16);
xmm_x_ptr_n_k_x2x0 = _mm_blend_epi16(xmm_x_ptr_n_k_x2x0, xmm_x_ptr_n_k_x3x1, 0xCC);
xmm_x_ptr_sub_x2x0 = _mm_blend_epi16(xmm_x_ptr_sub_x2x0, xmm_x_ptr_sub_x3x1, 0xCC);
X1_3210 = _mm_loadu_si128((__m128i *)&CAf[ k ]);
PTR_3210 = _mm_loadu_si128((__m128i *)&CAb[ k ]);
X1_3210 = _mm_add_epi32(X1_3210, xmm_x_ptr_n_k_x2x0);
PTR_3210 = _mm_add_epi32(PTR_3210, xmm_x_ptr_sub_x2x0);
_mm_storeu_si128((__m128i *)&CAf[ k ], X1_3210);
_mm_storeu_si128((__m128i *)&CAb[ k ], PTR_3210);
}
for(; k <= n; k++) {
CAf[ k ] = silk_SMLAWW(CAf[ k ], tmp1,
silk_LSHIFT32((opus_int32)x_ptr[ n - k ], -rshifts - 1)); /* Q(-rshift) */
CAb[ k ] = silk_SMLAWW(CAb[ k ], tmp2,
silk_LSHIFT32((opus_int32)x_ptr[ subfr_length - n + k - 1 ], -rshifts - 1)); /* Q(-rshift) */
}
}
}
}
/* Calculate nominator and denominator for the next order reflection (parcor) coefficient */
tmp1 = C_first_row[ n ]; /* Q(-rshifts) */
tmp2 = C_last_row[ n ]; /* Q(-rshifts) */
num = 0; /* Q(-rshifts) */
nrg = silk_ADD32(CAb[ 0 ], CAf[ 0 ]); /* Q(1-rshifts) */
for(k = 0; k < n; k++) {
Atmp_QA = Af_QA[ k ];
lz = silk_CLZ32(silk_abs(Atmp_QA)) - 1;
lz = silk_min(32 - QA, lz);
Atmp1 = silk_LSHIFT32(Atmp_QA, lz); /* Q(QA + lz) */
tmp1 = silk_ADD_LSHIFT32(tmp1, silk_SMMUL(C_last_row[ n - k - 1 ], Atmp1), 32 - QA - lz); /* Q(-rshifts) */
tmp2 = silk_ADD_LSHIFT32(tmp2, silk_SMMUL(C_first_row[ n - k - 1 ], Atmp1), 32 - QA - lz); /* Q(-rshifts) */
num = silk_ADD_LSHIFT32(num, silk_SMMUL(CAb[ n - k ], Atmp1), 32 - QA - lz); /* Q(-rshifts) */
nrg = silk_ADD_LSHIFT32(nrg, silk_SMMUL(silk_ADD32(CAb[ k + 1 ], CAf[ k + 1 ]),
Atmp1), 32 - QA - lz); /* Q(1-rshifts) */
}
CAf[ n + 1 ] = tmp1; /* Q(-rshifts) */
CAb[ n + 1 ] = tmp2; /* Q(-rshifts) */
num = silk_ADD32(num, tmp2); /* Q(-rshifts) */
num = silk_LSHIFT32(-num, 1); /* Q(1-rshifts) */
/* Calculate the next order reflection (parcor) coefficient */
if(silk_abs(num) < nrg) {
rc_Q31 = silk_DIV32_varQ(num, nrg, 31);
} else {
rc_Q31 = (num > 0) ? silk_int32_MAX : silk_int32_MIN;
}
/* Update inverse prediction gain */
tmp1 = ((opus_int32)1 << 30) - silk_SMMUL(rc_Q31, rc_Q31);
tmp1 = silk_LSHIFT(silk_SMMUL(invGain_Q30, tmp1), 2);
if(tmp1 <= minInvGain_Q30) {
/* Max prediction gain exceeded; set reflection coefficient such that max prediction gain is exactly hit */
tmp2 = ((opus_int32)1 << 30) - silk_DIV32_varQ(minInvGain_Q30, invGain_Q30, 30); /* Q30 */
rc_Q31 = silk_SQRT_APPROX(tmp2); /* Q15 */
/* Newton-Raphson iteration */
rc_Q31 = silk_RSHIFT32(rc_Q31 + silk_DIV32(tmp2, rc_Q31), 1); /* Q15 */
rc_Q31 = silk_LSHIFT32(rc_Q31, 16); /* Q31 */
if(num < 0) {
/* Ensure adjusted reflection coefficients has the original sign */
rc_Q31 = -rc_Q31;
}
invGain_Q30 = minInvGain_Q30;
reached_max_gain = 1;
} else {
invGain_Q30 = tmp1;
}
/* Update the AR coefficients */
for(k = 0; k < (n + 1) >> 1; k++) {
tmp1 = Af_QA[ k ]; /* QA */
tmp2 = Af_QA[ n - k - 1 ]; /* QA */
Af_QA[ k ] = silk_ADD_LSHIFT32(tmp1, silk_SMMUL(tmp2, rc_Q31), 1); /* QA */
Af_QA[ n - k - 1 ] = silk_ADD_LSHIFT32(tmp2, silk_SMMUL(tmp1, rc_Q31), 1); /* QA */
}
Af_QA[ n ] = silk_RSHIFT32(rc_Q31, 31 - QA); /* QA */
if(reached_max_gain) {
/* Reached max prediction gain; set remaining coefficients to zero and exit loop */
for(k = n + 1; k < D; k++) {
Af_QA[ k ] = 0;
}
break;
}
/* Update C * Af and C * Ab */
for(k = 0; k <= n + 1; k++) {
tmp1 = CAf[ k ]; /* Q(-rshifts) */
tmp2 = CAb[ n - k + 1 ]; /* Q(-rshifts) */
CAf[ k ] = silk_ADD_LSHIFT32(tmp1, silk_SMMUL(tmp2, rc_Q31), 1); /* Q(-rshifts) */
CAb[ n - k + 1 ] = silk_ADD_LSHIFT32(tmp2, silk_SMMUL(tmp1, rc_Q31), 1); /* Q(-rshifts) */
}
}
if(reached_max_gain) {
for(k = 0; k < D; k++) {
/* Scale coefficients */
A_Q16[ k ] = -silk_RSHIFT_ROUND(Af_QA[ k ], QA - 16);
}
/* Subtract energy of preceding samples from C0 */
if(rshifts > 0) {
for(s = 0; s < nb_subfr; s++) {
x_ptr = x + s * subfr_length;
C0 -= (opus_int32)silk_RSHIFT64(silk_inner_prod16_aligned_64(x_ptr, x_ptr, D, arch), rshifts);
}
} else {
for(s = 0; s < nb_subfr; s++) {
x_ptr = x + s * subfr_length;
C0 -= silk_LSHIFT32(silk_inner_prod_aligned(x_ptr, x_ptr, D, arch), -rshifts);
}
}
/* Approximate residual energy */
*res_nrg = silk_LSHIFT(silk_SMMUL(invGain_Q30, C0), 2);
*res_nrg_Q = -rshifts;
} else {
/* Return residual energy */
nrg = CAf[ 0 ]; /* Q(-rshifts) */
tmp1 = (opus_int32)1 << 16; /* Q16 */
for(k = 0; k < D; k++) {
Atmp1 = silk_RSHIFT_ROUND(Af_QA[ k ], QA - 16); /* Q16 */
nrg = silk_SMLAWW(nrg, CAf[ k + 1 ], Atmp1); /* Q(-rshifts) */
tmp1 = silk_SMLAWW(tmp1, Atmp1, Atmp1); /* Q16 */
A_Q16[ k ] = -Atmp1;
}
*res_nrg = silk_SMLAWW(nrg, silk_SMMUL(SILK_FIX_CONST(FIND_LPC_COND_FAC, 32), C0), -tmp1);/* Q(-rshifts) */
*res_nrg_Q = -rshifts;
}
}
/* Compute reflection coefficients from input signal */
void silk_burg_modified(
opus_int32 *res_nrg, /* O Residual energy */
opus_int *res_nrg_Q, /* O Residual energy Q value */
opus_int32 A_Q16[], /* O Prediction coefficients (length order) */
const opus_int16 x[], /* I Input signal, length: nb_subfr * ( D + subfr_length ) */
const opus_int32 minInvGain_Q30, /* I Inverse of max prediction gain */
const opus_int subfr_length, /* I Input signal subframe length (incl. D preceding samples) */
const opus_int nb_subfr, /* I Number of subframes stacked in x */
const opus_int D /* I Order */
)
{
opus_int k, n, s, lz, rshifts, rshifts_extra, reached_max_gain;
opus_int32 C0, num, nrg, rc_Q31, invGain_Q30, Atmp_QA, Atmp1, tmp1, tmp2, x1, x2;
const opus_int16 *x_ptr;
opus_int32 C_first_row[ SILK_MAX_ORDER_LPC ];
opus_int32 C_last_row[ SILK_MAX_ORDER_LPC ];
opus_int32 Af_QA[ SILK_MAX_ORDER_LPC ];
opus_int32 CAf[ SILK_MAX_ORDER_LPC + 1 ];
opus_int32 CAb[ SILK_MAX_ORDER_LPC + 1 ];
silk_assert( subfr_length * nb_subfr <= MAX_FRAME_SIZE );
/* Compute autocorrelations, added over subframes */
silk_sum_sqr_shift( &C0, &rshifts, x, nb_subfr * subfr_length );
if( rshifts > MAX_RSHIFTS ) {
C0 = silk_LSHIFT32( C0, rshifts - MAX_RSHIFTS );
silk_assert( C0 > 0 );
rshifts = MAX_RSHIFTS;
} else {
lz = silk_CLZ32( C0 ) - 1;
rshifts_extra = N_BITS_HEAD_ROOM - lz;
if( rshifts_extra > 0 ) {
rshifts_extra = silk_min( rshifts_extra, MAX_RSHIFTS - rshifts );
C0 = silk_RSHIFT32( C0, rshifts_extra );
} else {
rshifts_extra = silk_max( rshifts_extra, MIN_RSHIFTS - rshifts );
C0 = silk_LSHIFT32( C0, -rshifts_extra );
}
rshifts += rshifts_extra;
}
CAb[ 0 ] = CAf[ 0 ] = C0 + silk_SMMUL( SILK_FIX_CONST( FIND_LPC_COND_FAC, 32 ), C0 ) + 1; /* Q(-rshifts) */
silk_memset( C_first_row, 0, SILK_MAX_ORDER_LPC * sizeof( opus_int32 ) );
if( rshifts > 0 ) {
for( s = 0; s < nb_subfr; s++ ) {
x_ptr = x + s * subfr_length;
for( n = 1; n < D + 1; n++ ) {
C_first_row[ n - 1 ] += (opus_int32)silk_RSHIFT64(
silk_inner_prod16_aligned_64( x_ptr, x_ptr + n, subfr_length - n ), rshifts );
}
}
} else {
for( s = 0; s < nb_subfr; s++ ) {
x_ptr = x + s * subfr_length;
for( n = 1; n < D + 1; n++ ) {
C_first_row[ n - 1 ] += silk_LSHIFT32(
silk_inner_prod_aligned( x_ptr, x_ptr + n, subfr_length - n ), -rshifts );
}
}
}
silk_memcpy( C_last_row, C_first_row, SILK_MAX_ORDER_LPC * sizeof( opus_int32 ) );
/* Initialize */
CAb[ 0 ] = CAf[ 0 ] = C0 + silk_SMMUL( SILK_FIX_CONST( FIND_LPC_COND_FAC, 32 ), C0 ) + 1; /* Q(-rshifts) */
invGain_Q30 = (opus_int32)1 << 30;
reached_max_gain = 0;
for( n = 0; n < D; n++ ) {
/* Update first row of correlation matrix (without first element) */
/* Update last row of correlation matrix (without last element, stored in reversed order) */
/* Update C * Af */
/* Update C * flipud(Af) (stored in reversed order) */
if( rshifts > -2 ) {
for( s = 0; s < nb_subfr; s++ ) {
x_ptr = x + s * subfr_length;
x1 = -silk_LSHIFT32( (opus_int32)x_ptr[ n ], 16 - rshifts ); /* Q(16-rshifts) */
x2 = -silk_LSHIFT32( (opus_int32)x_ptr[ subfr_length - n - 1 ], 16 - rshifts ); /* Q(16-rshifts) */
tmp1 = silk_LSHIFT32( (opus_int32)x_ptr[ n ], QA - 16 ); /* Q(QA-16) */
tmp2 = silk_LSHIFT32( (opus_int32)x_ptr[ subfr_length - n - 1 ], QA - 16 ); /* Q(QA-16) */
for( k = 0; k < n; k++ ) {
C_first_row[ k ] = silk_SMLAWB( C_first_row[ k ], x1, x_ptr[ n - k - 1 ] ); /* Q( -rshifts ) */
C_last_row[ k ] = silk_SMLAWB( C_last_row[ k ], x2, x_ptr[ subfr_length - n + k ] ); /* Q( -rshifts ) */
Atmp_QA = Af_QA[ k ];
tmp1 = silk_SMLAWB( tmp1, Atmp_QA, x_ptr[ n - k - 1 ] ); /* Q(QA-16) */
tmp2 = silk_SMLAWB( tmp2, Atmp_QA, x_ptr[ subfr_length - n + k ] ); /* Q(QA-16) */
}
tmp1 = silk_LSHIFT32( -tmp1, 32 - QA - rshifts ); /* Q(16-rshifts) */
tmp2 = silk_LSHIFT32( -tmp2, 32 - QA - rshifts ); /* Q(16-rshifts) */
for( k = 0; k <= n; k++ ) {
CAf[ k ] = silk_SMLAWB( CAf[ k ], tmp1, x_ptr[ n - k ] ); /* Q( -rshift ) */
CAb[ k ] = silk_SMLAWB( CAb[ k ], tmp2, x_ptr[ subfr_length - n + k - 1 ] ); /* Q( -rshift ) */
}
}
} else {
for( s = 0; s < nb_subfr; s++ ) {
x_ptr = x + s * subfr_length;
x1 = -silk_LSHIFT32( (opus_int32)x_ptr[ n ], -rshifts ); /* Q( -rshifts ) */
x2 = -silk_LSHIFT32( (opus_int32)x_ptr[ subfr_length - n - 1 ], -rshifts ); /* Q( -rshifts ) */
tmp1 = silk_LSHIFT32( (opus_int32)x_ptr[ n ], 17 ); /* Q17 */
tmp2 = silk_LSHIFT32( (opus_int32)x_ptr[ subfr_length - n - 1 ], 17 ); /* Q17 */
for( k = 0; k < n; k++ ) {
C_first_row[ k ] = silk_MLA( C_first_row[ k ], x1, x_ptr[ n - k - 1 ] ); /* Q( -rshifts ) */
C_last_row[ k ] = silk_MLA( C_last_row[ k ], x2, x_ptr[ subfr_length - n + k ] ); /* Q( -rshifts ) */
Atmp1 = silk_RSHIFT_ROUND( Af_QA[ k ], QA - 17 ); /* Q17 */
tmp1 = silk_MLA( tmp1, x_ptr[ n - k - 1 ], Atmp1 ); /* Q17 */
tmp2 = silk_MLA( tmp2, x_ptr[ subfr_length - n + k ], Atmp1 ); /* Q17 */
}
tmp1 = -tmp1; /* Q17 */
tmp2 = -tmp2; /* Q17 */
for( k = 0; k <= n; k++ ) {
CAf[ k ] = silk_SMLAWW( CAf[ k ], tmp1,
silk_LSHIFT32( (opus_int32)x_ptr[ n - k ], -rshifts - 1 ) ); /* Q( -rshift ) */
CAb[ k ] = silk_SMLAWW( CAb[ k ], tmp2,
silk_LSHIFT32( (opus_int32)x_ptr[ subfr_length - n + k - 1 ], -rshifts - 1 ) ); /* Q( -rshift ) */
}
}
}
/* Calculate nominator and denominator for the next order reflection (parcor) coefficient */
tmp1 = C_first_row[ n ]; /* Q( -rshifts ) */
tmp2 = C_last_row[ n ]; /* Q( -rshifts ) */
num = 0; /* Q( -rshifts ) */
nrg = silk_ADD32( CAb[ 0 ], CAf[ 0 ] ); /* Q( 1-rshifts ) */
for( k = 0; k < n; k++ ) {
Atmp_QA = Af_QA[ k ];
lz = silk_CLZ32( silk_abs( Atmp_QA ) ) - 1;
lz = silk_min( 32 - QA, lz );
Atmp1 = silk_LSHIFT32( Atmp_QA, lz ); /* Q( QA + lz ) */
tmp1 = silk_ADD_LSHIFT32( tmp1, silk_SMMUL( C_last_row[ n - k - 1 ], Atmp1 ), 32 - QA - lz ); /* Q( -rshifts ) */
tmp2 = silk_ADD_LSHIFT32( tmp2, silk_SMMUL( C_first_row[ n - k - 1 ], Atmp1 ), 32 - QA - lz ); /* Q( -rshifts ) */
num = silk_ADD_LSHIFT32( num, silk_SMMUL( CAb[ n - k ], Atmp1 ), 32 - QA - lz ); /* Q( -rshifts ) */
nrg = silk_ADD_LSHIFT32( nrg, silk_SMMUL( silk_ADD32( CAb[ k + 1 ], CAf[ k + 1 ] ),
Atmp1 ), 32 - QA - lz ); /* Q( 1-rshifts ) */
}
CAf[ n + 1 ] = tmp1; /* Q( -rshifts ) */
CAb[ n + 1 ] = tmp2; /* Q( -rshifts ) */
num = silk_ADD32( num, tmp2 ); /* Q( -rshifts ) */
num = silk_LSHIFT32( -num, 1 ); /* Q( 1-rshifts ) */
/* Calculate the next order reflection (parcor) coefficient */
if( silk_abs( num ) < nrg ) {
rc_Q31 = silk_DIV32_varQ( num, nrg, 31 );
} else {
rc_Q31 = ( num > 0 ) ? silk_int32_MAX : silk_int32_MIN;
}
/* Update inverse prediction gain */
tmp1 = ( (opus_int32)1 << 30 ) - silk_SMMUL( rc_Q31, rc_Q31 );
tmp1 = silk_LSHIFT( silk_SMMUL( invGain_Q30, tmp1 ), 2 );
if( tmp1 <= minInvGain_Q30 ) {
/* Max prediction gain exceeded; set reflection coefficient such that max prediction gain is exactly hit */
tmp2 = ( (opus_int32)1 << 30 ) - silk_DIV32_varQ( minInvGain_Q30, invGain_Q30, 30 ); /* Q30 */
rc_Q31 = silk_SQRT_APPROX( tmp2 ); /* Q15 */
/* Newton-Raphson iteration */
rc_Q31 = silk_RSHIFT32( rc_Q31 + silk_DIV32( tmp2, rc_Q31 ), 1 ); /* Q15 */
rc_Q31 = silk_LSHIFT32( rc_Q31, 16 ); /* Q31 */
if( num < 0 ) {
/* Ensure adjusted reflection coefficients has the original sign */
rc_Q31 = -rc_Q31;
}
invGain_Q30 = minInvGain_Q30;
reached_max_gain = 1;
} else {
invGain_Q30 = tmp1;
}
/* Update the AR coefficients */
for( k = 0; k < (n + 1) >> 1; k++ ) {
tmp1 = Af_QA[ k ]; /* QA */
tmp2 = Af_QA[ n - k - 1 ]; /* QA */
Af_QA[ k ] = silk_ADD_LSHIFT32( tmp1, silk_SMMUL( tmp2, rc_Q31 ), 1 ); /* QA */
Af_QA[ n - k - 1 ] = silk_ADD_LSHIFT32( tmp2, silk_SMMUL( tmp1, rc_Q31 ), 1 ); /* QA */
}
Af_QA[ n ] = silk_RSHIFT32( rc_Q31, 31 - QA ); /* QA */
if( reached_max_gain ) {
/* Reached max prediction gain; set remaining coefficients to zero and exit loop */
for( k = n + 1; k < D; k++ ) {
Af_QA[ k ] = 0;
}
break;
}
/* Update C * Af and C * Ab */
for( k = 0; k <= n + 1; k++ ) {
tmp1 = CAf[ k ]; /* Q( -rshifts ) */
tmp2 = CAb[ n - k + 1 ]; /* Q( -rshifts ) */
CAf[ k ] = silk_ADD_LSHIFT32( tmp1, silk_SMMUL( tmp2, rc_Q31 ), 1 ); /* Q( -rshifts ) */
CAb[ n - k + 1 ] = silk_ADD_LSHIFT32( tmp2, silk_SMMUL( tmp1, rc_Q31 ), 1 ); /* Q( -rshifts ) */
}
}
if( reached_max_gain ) {
for( k = 0; k < D; k++ ) {
/* Scale coefficients */
A_Q16[ k ] = -silk_RSHIFT_ROUND( Af_QA[ k ], QA - 16 );
}
/* Subtract energy of preceding samples from C0 */
if( rshifts > 0 ) {
for( s = 0; s < nb_subfr; s++ ) {
x_ptr = x + s * subfr_length;
C0 -= (opus_int32)silk_RSHIFT64( silk_inner_prod16_aligned_64( x_ptr, x_ptr, D ), rshifts );
}
} else {
for( s = 0; s < nb_subfr; s++ ) {
x_ptr = x + s * subfr_length;
C0 -= silk_LSHIFT32( silk_inner_prod_aligned( x_ptr, x_ptr, D ), -rshifts );
}
}
/* Approximate residual energy */
*res_nrg = silk_LSHIFT( silk_SMMUL( invGain_Q30, C0 ), 2 );
*res_nrg_Q = -rshifts;
} else {
/* Return residual energy */
nrg = CAf[ 0 ]; /* Q( -rshifts ) */
tmp1 = (opus_int32)1 << 16; /* Q16 */
for( k = 0; k < D; k++ ) {
Atmp1 = silk_RSHIFT_ROUND( Af_QA[ k ], QA - 16 ); /* Q16 */
nrg = silk_SMLAWW( nrg, CAf[ k + 1 ], Atmp1 ); /* Q( -rshifts ) */
tmp1 = silk_SMLAWW( tmp1, Atmp1, Atmp1 ); /* Q16 */
A_Q16[ k ] = -Atmp1;
}
*res_nrg = silk_SMLAWW( nrg, silk_SMMUL( FIND_LPC_COND_FAC, C0 ), -tmp1 ); /* Q( -rshifts ) */
*res_nrg_Q = -rshifts;
}
}
