/* non-warped frequency scale. (So that it can be implemented with a minimum-phase monic filter.) */ SKP_INLINE SKP_int32 warped_gain( // gain in Q16 const SKP_int32 *coefs_Q24, SKP_int lambda_Q16, SKP_int order ) { SKP_int i; SKP_int32 gain_Q24; lambda_Q16 = -lambda_Q16; gain_Q24 = coefs_Q24[ order - 1 ]; for( i = order - 2; i >= 0; i-- ) { gain_Q24 = SKP_SMLAWB( coefs_Q24[ i ], gain_Q24, lambda_Q16 ); } gain_Q24 = SKP_SMLAWB( SKP_FIX_CONST( 1.0, 24 ), gain_Q24, -lambda_Q16 ); return SKP_INVERSE32_varQ( gain_Q24, 40 ); }
/* amplitude of monic warped coefficients by using bandwidth expansion on the true coefficients */ SKP_INLINE void limit_warped_coefs( SKP_int32 *coefs_syn_Q24, SKP_int32 *coefs_ana_Q24, SKP_int lambda_Q16, SKP_int32 limit_Q24, SKP_int order ) { SKP_int i, iter, ind = 0; SKP_int32 tmp, maxabs_Q24, chirp_Q16, gain_syn_Q16, gain_ana_Q16; SKP_int32 nom_Q16, den_Q24; /* Convert to monic coefficients */ lambda_Q16 = -lambda_Q16; for( i = order - 1; i > 0; i-- ) { coefs_syn_Q24[ i - 1 ] = SKP_SMLAWB( coefs_syn_Q24[ i - 1 ], coefs_syn_Q24[ i ], lambda_Q16 ); coefs_ana_Q24[ i - 1 ] = SKP_SMLAWB( coefs_ana_Q24[ i - 1 ], coefs_ana_Q24[ i ], lambda_Q16 ); } lambda_Q16 = -lambda_Q16; nom_Q16 = SKP_SMLAWB( SKP_FIX_CONST( 1.0, 16 ), -lambda_Q16, lambda_Q16 ); den_Q24 = SKP_SMLAWB( SKP_FIX_CONST( 1.0, 24 ), coefs_syn_Q24[ 0 ], lambda_Q16 ); gain_syn_Q16 = SKP_DIV32_varQ( nom_Q16, den_Q24, 24 ); den_Q24 = SKP_SMLAWB( SKP_FIX_CONST( 1.0, 24 ), coefs_ana_Q24[ 0 ], lambda_Q16 ); gain_ana_Q16 = SKP_DIV32_varQ( nom_Q16, den_Q24, 24 ); for( i = 0; i < order; i++ ) { coefs_syn_Q24[ i ] = SKP_SMULWW( gain_syn_Q16, coefs_syn_Q24[ i ] ); coefs_ana_Q24[ i ] = SKP_SMULWW( gain_ana_Q16, coefs_ana_Q24[ i ] ); } for( iter = 0; iter < 10; iter++ ) { /* Find maximum absolute value */ maxabs_Q24 = -1; for( i = 0; i < order; i++ ) { tmp = SKP_max( SKP_abs_int32( coefs_syn_Q24[ i ] ), SKP_abs_int32( coefs_ana_Q24[ i ] ) ); if( tmp > maxabs_Q24 ) { maxabs_Q24 = tmp; ind = i; } } if( maxabs_Q24 <= limit_Q24 ) { /* Coefficients are within range - done */ return; } /* Convert back to true warped coefficients */ for( i = 1; i < order; i++ ) { coefs_syn_Q24[ i - 1 ] = SKP_SMLAWB( coefs_syn_Q24[ i - 1 ], coefs_syn_Q24[ i ], lambda_Q16 ); coefs_ana_Q24[ i - 1 ] = SKP_SMLAWB( coefs_ana_Q24[ i - 1 ], coefs_ana_Q24[ i ], lambda_Q16 ); } gain_syn_Q16 = SKP_INVERSE32_varQ( gain_syn_Q16, 32 ); gain_ana_Q16 = SKP_INVERSE32_varQ( gain_ana_Q16, 32 ); for( i = 0; i < order; i++ ) { coefs_syn_Q24[ i ] = SKP_SMULWW( gain_syn_Q16, coefs_syn_Q24[ i ] ); coefs_ana_Q24[ i ] = SKP_SMULWW( gain_ana_Q16, coefs_ana_Q24[ i ] ); } /* Apply bandwidth expansion */ chirp_Q16 = SKP_FIX_CONST( 0.99, 16 ) - SKP_DIV32_varQ( SKP_SMULWB( maxabs_Q24 - limit_Q24, SKP_SMLABB( SKP_FIX_CONST( 0.8, 10 ), SKP_FIX_CONST( 0.1, 10 ), iter ) ), SKP_MUL( maxabs_Q24, ind + 1 ), 22 ); SKP_Silk_bwexpander_32( coefs_syn_Q24, order, chirp_Q16 ); SKP_Silk_bwexpander_32( coefs_ana_Q24, order, chirp_Q16 ); /* Convert to monic warped coefficients */ lambda_Q16 = -lambda_Q16; for( i = order - 1; i > 0; i-- ) { coefs_syn_Q24[ i - 1 ] = SKP_SMLAWB( coefs_syn_Q24[ i - 1 ], coefs_syn_Q24[ i ], lambda_Q16 ); coefs_ana_Q24[ i - 1 ] = SKP_SMLAWB( coefs_ana_Q24[ i - 1 ], coefs_ana_Q24[ i ], lambda_Q16 ); } lambda_Q16 = -lambda_Q16; nom_Q16 = SKP_SMLAWB( SKP_FIX_CONST( 1.0, 16 ), -lambda_Q16, lambda_Q16 ); den_Q24 = SKP_SMLAWB( SKP_FIX_CONST( 1.0, 24 ), coefs_syn_Q24[ 0 ], lambda_Q16 ); gain_syn_Q16 = SKP_DIV32_varQ( nom_Q16, den_Q24, 24 ); den_Q24 = SKP_SMLAWB( SKP_FIX_CONST( 1.0, 24 ), coefs_ana_Q24[ 0 ], lambda_Q16 ); gain_ana_Q16 = SKP_DIV32_varQ( nom_Q16, den_Q24, 24 ); for( i = 0; i < order; i++ ) { coefs_syn_Q24[ i ] = SKP_SMULWW( gain_syn_Q16, coefs_syn_Q24[ i ] ); coefs_ana_Q24[ i ] = SKP_SMULWW( gain_ana_Q16, coefs_ana_Q24[ i ] ); } } SKP_assert( 0 ); }
SKP_INLINE void SKP_Silk_LDL_factorize_FIX( SKP_int32 *A, /* I Pointer to Symetric Square Matrix */ SKP_int M, /* I Size of Matrix */ SKP_int32 *L_Q16, /* I/O Pointer to Square Upper triangular Matrix */ inv_D_t *inv_D /* I/O Pointer to vector holding inverted diagonal elements of D */ ) { SKP_int i, j, k, status, loop_count; const SKP_int32 *ptr1, *ptr2; SKP_int32 diag_min_value, tmp_32, err; SKP_int32 v_Q0[ MAX_MATRIX_SIZE ], D_Q0[ MAX_MATRIX_SIZE ]; SKP_int32 one_div_diag_Q36, one_div_diag_Q40, one_div_diag_Q48; SKP_assert( M <= MAX_MATRIX_SIZE ); status = 1; diag_min_value = SKP_max_32( SKP_SMMUL( SKP_ADD_SAT32( A[ 0 ], A[ SKP_SMULBB( M, M ) - 1 ] ), SKP_FIX_CONST( FIND_LTP_COND_FAC, 31 ) ), 1 << 9 ); for( loop_count = 0; loop_count < M && status == 1; loop_count++ ) { status = 0; for( j = 0; j < M; j++ ) { ptr1 = matrix_adr( L_Q16, j, 0, M ); tmp_32 = 0; for( i = 0; i < j; i++ ) { v_Q0[ i ] = SKP_SMULWW( D_Q0[ i ], ptr1[ i ] ); /* Q0 */ tmp_32 = SKP_SMLAWW( tmp_32, v_Q0[ i ], ptr1[ i ] ); /* Q0 */ } tmp_32 = SKP_SUB32( matrix_ptr( A, j, j, M ), tmp_32 ); if( tmp_32 < diag_min_value ) { tmp_32 = SKP_SUB32( SKP_SMULBB( loop_count + 1, diag_min_value ), tmp_32 ); /* Matrix not positive semi-definite, or ill conditioned */ for( i = 0; i < M; i++ ) { matrix_ptr( A, i, i, M ) = SKP_ADD32( matrix_ptr( A, i, i, M ), tmp_32 ); } status = 1; break; } D_Q0[ j ] = tmp_32; /* always < max(Correlation) */ /* two-step division */ one_div_diag_Q36 = SKP_INVERSE32_varQ( tmp_32, 36 ); /* Q36 */ one_div_diag_Q40 = SKP_LSHIFT( one_div_diag_Q36, 4 ); /* Q40 */ err = SKP_SUB32( 1 << 24, SKP_SMULWW( tmp_32, one_div_diag_Q40 ) ); /* Q24 */ one_div_diag_Q48 = SKP_SMULWW( err, one_div_diag_Q40 ); /* Q48 */ /* Save 1/Ds */ inv_D[ j ].Q36_part = one_div_diag_Q36; inv_D[ j ].Q48_part = one_div_diag_Q48; matrix_ptr( L_Q16, j, j, M ) = 65536; /* 1.0 in Q16 */ ptr1 = matrix_adr( A, j, 0, M ); ptr2 = matrix_adr( L_Q16, j + 1, 0, M ); for( i = j + 1; i < M; i++ ) { tmp_32 = 0; for( k = 0; k < j; k++ ) { tmp_32 = SKP_SMLAWW( tmp_32, v_Q0[ k ], ptr2[ k ] ); /* Q0 */ } tmp_32 = SKP_SUB32( ptr1[ i ], tmp_32 ); /* always < max(Correlation) */ /* tmp_32 / D_Q0[j] : Divide to Q16 */ matrix_ptr( L_Q16, i, j, M ) = SKP_ADD32( SKP_SMMUL( tmp_32, one_div_diag_Q48 ), SKP_RSHIFT( SKP_SMULWW( tmp_32, one_div_diag_Q36 ), 4 ) ); /* go to next column */ ptr2 += M; } } } SKP_assert( status == 0 ); }