#include "SamsungSolomonVoiceW_basic_op.h" #if(FLAG_SELECT_C_DSP == 1) #include #include #endif #define DVTX_MIN(X, Y) (((X) < (Y)) ? (X) : (Y)) #define DVTX_MAX(X, Y) (((X) > (Y)) ? (X) : (Y)) int DVTXOP_Overflow = 0; int DVTXOP_Carry = 0; #define LOG10QFIX /* fnLog10(2) */ -605866608 #define LOG10FIX /* fnLog10(1) */ -626068080 #ifndef FLAG_DSP_ASM_ON short DVTXOP_saturate(int L_var1) { short var_out; if (L_var1 > 0X00007fffL) { DVTXOP_Overflow = 1; var_out = DVTX_MAX_16; } else if (L_var1 < (int)0xffff8000L) { DVTXOP_Overflow = 1; var_out = DVTX_MIN_16; } else { DVTXOP_Overflow = 0; var_out = DVTXOP_extract_l(L_var1); } return (var_out); } short DVTXOP_add(short var1, short var2) { short var_out; int L_sum; L_sum = (int)var1 + var2; var_out = DVTXOP_saturate(L_sum); return (var_out); } short DVTXOP_sub(short var1, short var2) { short var_out; int L_diff; L_diff = (int)var1 - var2; var_out = DVTXOP_saturate(L_diff); return (var_out); } short DVTXOP_abs_s(short var1) { short var_out; if (var1 == (short)0X8000) { var_out = DVTX_MAX_16; } else { if (var1 < 0) { var_out = -var1; } else { var_out = var1; } } return (var_out); } short DVTXOP_shl(short var1, short var2) { short var_out; int result; if (var2 < 0) { var_out = DVTXOP_shr(var1, (short)(-var2)); } else { result = (int)var1 *((int)1 << var2); if ((var2 > 15 && var1 != 0) || (result != (int)((short)result))) { DVTXOP_Overflow = 1; var_out = (var1 > 0) ? DVTX_MAX_16 : DVTX_MIN_16; } else { var_out = DVTXOP_extract_l(result); } } return (var_out); } short DVTXOP_shr(short var1, short var2) { short var_out; if (var2 < 0) { var_out = DVTXOP_shl(var1, (short)(-var2)); } else { if (var2 >= 15) { var_out = (var1 < 0) ? -1 : 0; } else { if (var1 < 0) { var_out = ~((~var1) >> var2); } else { var_out = var1 >> var2; } } } return (var_out); } short DVTXOP_mult(short var1, short var2) { short var_out; int L_product; L_product = (int)var1 *(int)var2; L_product = (L_product & (int)0xffff8000L) >> 15; if (L_product & (int)0x00010000L) L_product = L_product | (int)0xffff0000L; var_out = DVTXOP_saturate(L_product); return (var_out); } int DVTXOP_L_mult(short var1, short var2) { int L_var_out; L_var_out = (int)var1 *(int)var2; if (L_var_out != (int)0x40000000L) { L_var_out *= 2; } else { DVTXOP_Overflow = 1; L_var_out = DVTX_MAX_32; } return (L_var_out); } short DVTXOP_negate(short var1) { short var_out; var_out = (var1 == DVTX_MIN_16) ? DVTX_MAX_16 : -var1; return (var_out); } short DVTXOP_extract_h(int L_var1) { short var_out; var_out = (short)(L_var1 >> 16); return (var_out); } short DVTXOP_extract_l(int L_var1) { short var_out; var_out = (short)L_var1; return (var_out); } short DVTXOP_round(int L_var1) { short var_out; int L_rounded; L_rounded = DVTXOP_L_add(L_var1, (int)0x00008000L); var_out = DVTXOP_extract_h(L_rounded); return (var_out); } int DVTXOP_L_mac(int L_var3, short var1, short var2) { int L_var_out; int L_product; L_product = DVTXOP_L_mult(var1, var2); L_var_out = DVTXOP_L_add(L_var3, L_product); return (L_var_out); } int DVTXOP_L_msu(int L_var3, short var1, short var2) { int L_var_out; int L_product; L_product = DVTXOP_L_mult(var1, var2); L_var_out = DVTXOP_L_sub(L_var3, L_product); return (L_var_out); } int DVTXOP_L_add(int L_var1, int L_var2) { int L_var_out; L_var_out = L_var1 + L_var2; if (((L_var1 ^ L_var2) & DVTX_MIN_32) == 0) { if ((L_var_out ^ L_var1) & DVTX_MIN_32) { L_var_out = (L_var1 < 0) ? DVTX_MIN_32 : DVTX_MAX_32; DVTXOP_Overflow = 1; } } return (L_var_out); } int DVTXOP_L_sub(int L_var1, int L_var2) { int L_var_out; L_var_out = L_var1 - L_var2; if (((L_var1 ^ L_var2) & DVTX_MIN_32) != 0) { if ((L_var_out ^ L_var1) & DVTX_MIN_32) { L_var_out = (L_var1 < 0L) ? DVTX_MIN_32 : DVTX_MAX_32; DVTXOP_Overflow = 1; } } return (L_var_out); } int DVTXOP_L_negate(int L_var1) { int L_var_out; L_var_out = (L_var1 == DVTX_MIN_32) ? DVTX_MAX_32 : -L_var1; return (L_var_out); } short DVTXOP_mult_r(short var1, short var2) { short var_out; int L_product_arr; L_product_arr = (int)var1 *(int)var2; /* product */ L_product_arr += (int)0x00004000L; /* DVTXOP_round */ L_product_arr &= (int)0xffff8000L; L_product_arr >>= 15; /* shift */ if (L_product_arr & (int)0x00010000L) /* sign extend when necessary */ { L_product_arr |= (int)0xffff0000L; } var_out = DVTXOP_saturate(L_product_arr); return (var_out); } int DVTXOP_L_shl(int L_var1, short var2) { int L_var_out; if (var2 <= 0) { L_var_out = DVTXOP_L_shr(L_var1, (short)(-var2)); } else { for (; var2 > 0; var2--) { if (L_var1 > (int)0X3fffffffL) { DVTXOP_Overflow = 1; L_var_out = DVTX_MAX_32; break; } else { if (L_var1 < (int)0xc0000000L) { DVTXOP_Overflow = 1; L_var_out = DVTX_MIN_32; break; } } L_var1 *= 2; L_var_out = L_var1; } } return (L_var_out); } int DVTXOP_L_shr(int L_var1, short var2) { int L_var_out; if (var2 < 0) { L_var_out = DVTXOP_L_shl(L_var1, (short)(-var2)); } else { if (var2 >= 31) { L_var_out = (L_var1 < 0L) ? -1 : 0; } else { if (L_var1 < 0) { L_var_out = ~((~L_var1) >> var2); } else { L_var_out = L_var1 >> var2; } } } return (L_var_out); } short DVTXOP_shr_r(short var1, short var2) { short var_out; if (var2 > 15) { var_out = 0; } else { var_out = DVTXOP_shr(var1, var2); if (var2 > 0) { if ((var1 & ((short)1 << (var2 - 1))) != 0) { var_out++; } } } return (var_out); } short DVTXOP_mac_r(int L_var3, short var1, short var2) { short var_out; L_var3 = DVTXOP_L_mac(L_var3, var1, var2); L_var3 = DVTXOP_L_add(L_var3, (int)0x00008000L); var_out = DVTXOP_extract_h(L_var3); return (var_out); } short DVTXOP_msu_r(int L_var3, short var1, short var2) { short var_out; L_var3 = DVTXOP_L_msu(L_var3, var1, var2); L_var3 = DVTXOP_L_add(L_var3, (int)0x00008000L); var_out = DVTXOP_extract_h(L_var3); return (var_out); } int DVTXOP_L_deposit_h(short var1) { int L_var_out; L_var_out = (int)var1 << 16; return (L_var_out); } int DVTXOP_L_deposit_l(short var1) { int L_var_out; L_var_out = (int)var1; return (L_var_out); } int DVTXOP_L_shr_r(int L_var1, short var2) { int L_var_out; if (var2 > 31) { L_var_out = 0; } else { L_var_out = DVTXOP_L_shr(L_var1, var2); if (var2 > 0) { if ((L_var1 & ((int)1 << (var2 - 1))) != 0) { L_var_out++; } } } return (L_var_out); } int DVTXOP_L_abs(int L_var1) { int L_var_out; if (L_var1 == DVTX_MIN_32) { L_var_out = DVTX_MAX_32; } else { if (L_var1 < 0) { L_var_out = -L_var1; } else { L_var_out = L_var1; } } return (L_var_out); } short DVTXOP_norm_s(short var1) { short var_out; if (var1 == 0) { var_out = 0; } else { if (var1 == (short)0xffff) { var_out = 15; } else { if (var1 < 0) { var1 = ~var1; } for (var_out = 0; var1 < 0x4000; var_out++) { var1 <<= 1; } } } return (var_out); } short DVTXOP_div_s(short var1, short var2) { short var_out = 0; short iteration; int L_num; int L_denom; if (var2 == 0) var2 = 1; if (var1 == 0) { var_out = 0; } else { if (var1 == var2) { var_out = DVTX_MAX_16; } else { L_num = DVTXOP_L_deposit_l(var1); L_denom = DVTXOP_L_deposit_l(var2); for (iteration = 0; iteration < 15; iteration++) { var_out <<= 1; L_num <<= 1; if (L_num >= L_denom) { L_num = DVTXOP_L_sub(L_num, L_denom); var_out = DVTXOP_add(var_out, 1); } } } } return (var_out); } short DVTXOP_norm_l(int L_var1) { short var_out; if (L_var1 == 0) { var_out = 0; } else { if (L_var1 == (int)0xffffffffL) { var_out = 31; } else { if (L_var1 < 0) { L_var1 = ~L_var1; } for (var_out = 0; L_var1 < (int)0x40000000L; var_out++) { L_var1 <<= 1; } } } return (var_out); } short DVTXOP_norm_ll(long long L_var1) { short var_out; if (L_var1 == 0) { var_out = 0; } else { if (L_var1 == (long long)0xffffffffffffffffL) { var_out = 63; } else { if (L_var1 < 0) { L_var1 = ~L_var1; } for (var_out = 0; L_var1 < (long long)0x4000000000000000L; var_out++) { L_var1 <<= 1; } } } return (var_out); } int DVTXOP_Ls_mult_r(int L_var1, short var2) { long long LL_product_arr; if (L_var1 == 0x80000000L && var2 == 0x8000) return 0x7fffffffL; LL_product_arr = (long long)L_var1 * var2; /* product */ LL_product_arr += (long long)0x0000000000004000LL; /* DVTXOP_round */ LL_product_arr &= (long long)0xffffffffffff8000LL; LL_product_arr >>= 15; /* shift */ return ((int)LL_product_arr); } int DVTXOP_L_mult_r(int L_var1, int L_var2) { long long LL_product_arr; if(L_var1 == 0x80000000L && L_var2 == 0x80000000L) return 0x7fffffffL; LL_product_arr = (long long)L_var1 * L_var2; /* product */ LL_product_arr += (long long)0x0000000040000000LL; /* DVTXOP_round */ LL_product_arr &= (long long)0xffffffff80000000LL; LL_product_arr >>= 31; /* shift */ return ((int)LL_product_arr); } #else short DVTXOP_norm_ll(long long L_var1) { short var_out; if (L_var1 == 0) { var_out = 0; } else { if (L_var1 == (long long)0xffffffffffffffffL) { var_out = 63; } else { if (L_var1 < 0) { L_var1 = ~L_var1; } for (var_out = 0; L_var1 < (long long)0x4000000000000000L; var_out++) { L_var1 <<= 1; } } } return (var_out); } #endif #ifndef INLINE_BASIC_OPS int DVTXOP_L_mpy_ls(int L_var2, short var1) { #if(FLAG_SELECT_C_DSP == 1) int L_varOut; short swtemp; //#define MULT16_32_Q15(a,b) ADD32(SHL(MULT16_16((a),SHR((b),16)),1), SHR(MULT16_16SU((a),((b)&0x0000ffff)),15)) swtemp = DVTXOP_shr(DVTXOP_extract_l(L_var2), 1); swtemp = (short)32767 & (short)swtemp; L_varOut = DVTXOP_L_mult(var1, swtemp); L_varOut = DVTXOP_L_shr(L_varOut, 15); L_varOut = DVTXOP_L_mac(L_varOut, var1, DVTXOP_extract_h(L_var2)); return (L_varOut); #else #ifndef OPT_DVTXOP_L_mpy_ls int L_varOut; short hi, lo; hi = AE_TRUNC16X4F32_scalar(L_var2); lo = (ae_int16)(ae_int32)AE_SRLI_32(AE_SLLI_32(L_var2, 16), 17); L_varOut = AE_MULF16X4SS_scalar(var1, lo);//DVTXOP_L_mult(var1, lo); L_varOut = AE_SRAA32S_scalar(L_varOut, 15);//DVTXOP_L_shr(L_varOut, 15); L_varOut = AE_MULAF16X4SS_scalar(L_varOut, var1, hi);//DVTXOP_L_mac(L_varOut, var1, hi); return L_varOut; #else ae_int64 tmp; ae_int32x2 tmp32; int out; ////////////////////////////////////////////////// L_var2 = L_var2&0xfffffffe; ////////////////////////////////////////////////// tmp = AE_MUL32_LL(L_var2, var1); tmp = AE_SRAI64(tmp,15); tmp32 = AE_MOVF32X2_FROMF64(tmp); out = AE_MOVAD32_L(tmp32); return out; #endif #endif } /**************************************************************************** * * FUNCTION NAME: DVTXOP_L_mpy_ll * * PURPOSE: Multiply a 32 bit number (L_var1) and a 32 bit number * (L_var2), and return a 32 bit result. * * INPUTS: * * L_var1 A int input variable * * L_var2 A int input variable * * OUTPUTS: none * * IMPLEMENTATION: * * Performs a 31x31 bit multiply, Complexity=24 Ops. * * Let x1x0, or y1y0, be the two constituent halves * of a 32 bit number. This function performs the * following: * * low = ((x0 >> 1)*(y0 >> 1)) >> 16 (low * low) * mid1 = [(x1 * (y0 >> 1)) >> 1 ] (high * low) * mid2 = [(y1 * (x0 >> 1)) >> 1] (high * low) * mid = (mid1 + low + mid2) >> 14 (sum so far) * output = (y1*x1) + mid (high * high) * * * RETURN VALUE: A int value * * KEYWORDS: DVTXOP_mult,mpy,multiplication * ***************************************************************************/ int DVTXOP_L_mpy_ll(int L_var1, int L_var2) { #if(FLAG_SELECT_C_DSP == 1) short swLow1, swLow2, swHigh1, swHigh2; int L_varOut, L_low, L_mid1, L_mid2, L_mid; swLow1 = DVTXOP_shr(DVTXOP_extract_l(L_var1), 1); swLow1 = DVTX_MAX_16 & swLow1; swLow2 = DVTXOP_shr(DVTXOP_extract_l(L_var2), 1); swLow2 = DVTX_MAX_16 & swLow2; swHigh1 = DVTXOP_extract_h(L_var1); swHigh2 = DVTXOP_extract_h(L_var2); L_low = DVTXOP_L_mult(swLow1, swLow2); L_low = DVTXOP_L_shr(L_low, 16); L_mid1 = DVTXOP_L_mult(swHigh1, swLow2); L_mid1 = DVTXOP_L_shr(L_mid1, 1); L_mid = DVTXOP_L_add(L_mid1, L_low); L_mid2 = DVTXOP_L_mult(swHigh2, swLow1); L_mid2 = DVTXOP_L_shr(L_mid2, 1); L_mid = DVTXOP_L_add(L_mid, L_mid2); L_mid = DVTXOP_L_shr(L_mid, 14); L_varOut = DVTXOP_L_mac(L_mid, swHigh1, swHigh2); return (L_varOut); #else #ifndef OPT_DVTXOP_L_mpy_ll short swLow1, swLow2, swHigh1, swHigh2; int L_varOut, L_low, L_mid1, L_mid2, L_mid; swHigh1 = AE_TRUNC16X4F32_scalar(L_var1); swLow1 = (ae_int16)(ae_int32)AE_SRLI_32(AE_SLLI_32(L_var1, 16), 17); swHigh2 = AE_TRUNC16X4F32_scalar(L_var2); swLow2 = (ae_int16)(ae_int32)AE_SRLI_32(AE_SLLI_32(L_var2, 16), 17); L_low = AE_MULF16X4SS_scalar(swLow1, swLow2); L_low = AE_SRAA32S_scalar(L_low, 16); L_mid1 = AE_MULF16X4SS_scalar(swHigh1, swLow2); L_mid1 = AE_SRAA32S_scalar(L_mid1, 1); L_mid2 = AE_MULF16X4SS_scalar(swHigh2, swLow1); L_mid2 = AE_SRAA32S_scalar(L_mid2, 1); L_mid = AE_ADD32S_scalar(L_mid1, L_low); L_mid = AE_ADD32S_scalar(L_mid, L_mid2); L_mid = AE_SRAA32S_scalar(L_mid, 14); L_varOut = AE_MULAF16X4SS_scalar(L_mid, swHigh1, swHigh2); return (L_varOut); #else ae_int64 tmp; ae_int32x2 tmp32; int out; ///////////////////////////////////////// L_var2 = L_var2&0xfffffffe; L_var1 = L_var1&0xfffffffe; ///////////////////////////////////////// tmp = AE_MUL32_LL(L_var1, L_var2); tmp = AE_SRAI64(tmp,31); tmp32 = AE_MOVF32X2_FROMF64(tmp); out = AE_MOVAD32_L(tmp32); return out; #endif #endif } #endif #ifndef DVTXOP_block_norm_OPT_DSP short DVTXOP_block_norm(short * data, short size, short headroom) { short i, max, scnt, adata; max = abs(data[0]); for (i = 1; i < size; i++) { adata = abs(data[i]); if (adata > max) max = adata; } if (max == 0) scnt = 16 - headroom; else scnt = DVTXOP_norm_s(max) - headroom; for (i = 0; i < size; i++) { data[i] = DVTXOP_shr_r(data[i], DVTXOP_negate(scnt)); } return (scnt); } #else short DVTXOP_block_norm(short * data, short size, short headroom) { short i, max, scnt, adata, shift; long long tmpll; int max0, max1; ae_valign align, align1; ae_int16x4 A1, mask0, mask1, A0, tzero; ae_int32x2 B0, L_tmp0; ae_int64 All; ae_int16x4 *pA, *pout; pA=(ae_int16x4 *)data; align = AE_LA64_PP(pA); tmpll = 0xFFFF0000FFFF0000LL; All = AE_MOV64(tmpll); mask0 = AE_MOVF16X4_FROMINT64(All); tmpll = 0x0000FFFF0000ffffLL; All = AE_MOV64(tmpll); mask1 = AE_MOVF16X4_FROMINT64(All); tzero = AE_ZERO16(); L_tmp0 = AE_ZERO32(); for (i = 0; i < size; i+= 4) { AE_LA16X4_IP(A0, align, pA); A0 = AE_ABS16S(A0); A1 = AE_AND16(mask1, A0); B0 = AE_MOVF32X2_FROMINT16X4(A1); L_tmp0 = AE_MAX32(L_tmp0, B0); A1 = AE_AND16(mask0, A0); A1 = AE_SEL16_4321(tzero,A1); B0 = AE_MOVF32X2_FROMINT16X4(A1); L_tmp0 = AE_MAX32(L_tmp0, B0); } max0 = AE_MOVAD32_L(L_tmp0); max1 = AE_MOVAD32_H(L_tmp0); max = DVTX_MAX(max0,max1); if (max == 0) scnt = 16 - headroom; else scnt = DVTXOP_norm_s(max) - headroom; pA=(ae_int16x4 *)data; pout=(ae_int16x4 *)data; align = AE_LA64_PP(pA); align1 = AE_ZALIGN64(); shift = -scnt; for (i = 0; i < size; i+= 4) { AE_LA16X4_IP(A0, align, pA); A0 = AE_SRAA16RS(A0, shift); AE_SA16X4_IP(A0, align1, pout); } AE_SA64POS_FP(align1, pout); return (scnt); } #endif #ifndef Right_Shift_vecter_OPT_DSP void DVTXOP_block_denorm(short * data, short size, short scnt) { short i; for (i = 0; i < size; i++) { data[i] = DVTXOP_shr_r(data[i], scnt); } return; } #endif #ifndef INLINE_BASIC_OPS int DVTXOP_L_divide(int L_num, int L_denom) { short approx; int L_div; if (L_num < 0 || L_denom < 0 || L_num > L_denom) { return (0); } /* First approximation: 1 / L_denom = 1/DVTXOP_extract_h(L_denom) */ approx = DVTXOP_div_s((short)0x3fff, DVTXOP_extract_h(L_denom)); /* 1/L_denom = approx * (2.0 - L_denom * approx) */ L_div = DVTXOP_L_mpy_ls(L_denom, approx); L_div = DVTXOP_L_sub((int)0x7fffffffL, L_div); L_div = DVTXOP_L_mpy_ls(L_div, approx); /* L_num * (1/L_denom) */ L_div = DVTXOP_L_mpy_ll(L_num, L_div); L_div = DVTXOP_L_shl(L_div, 2); return (L_div); } #endif int DVTXOP_fnLog10(int L_Input) { static short Scale = 9864; /* 0.30103 = log10(2) */ //Q15 int LwIn; if (L_Input == 0) return (-2147483647); /* 0.30103*log2(x) */ /* ------------------- */ LwIn = DVTXOP_fnLog2(L_Input); LwIn = DVTXOP_L_mpy_ls(LwIn, Scale); return (LwIn); } /*************************************************************************** * * FUNCTION NAME: DVTXOP_fnExp10 * * PURPOSE: * The purpose of this function is to implement a base ten exponential * 10**(32*x) by polynomial approximation * * * INPUTS: * * L_Input * unnormalized input exponent (input range constrained * to be < 0) * * OUTPUTS: * * none * * RETURN VALUE: * * LwIn * exponential output * * DESCRIPTION: * * polynomial approximation is used for the generation of the exponential * * 10**(32*X) = 2**((32*X) / log10(2)) * = 2**((32*X) / 0.30103) * * 2**(32*X) = 0.1713425*X*X + 0.6674432*X + 0.9979554 * c2 c1 c0 * *************************************************************************/ int DVTXOP_fnExp10(int L_Input) { static short InvScale = 27213; /* (1/log10(2))/4 */ int LwIn; LwIn = DVTXOP_L_mpy_ls(L_Input, InvScale); LwIn = DVTXOP_L_shl(LwIn, 2); LwIn = DVTXOP_fnExp2(LwIn); return (LwIn); } /*************************************************************************** * * FUNCTION NAME: DVTXOP_fnExp2 * * PURPOSE: * The purpose of this function is to implement a base two exponential * 2**(32*x) by polynomial approximation * * * INPUTS: * * L_Input * unnormalized input exponent (input range constrained * to be < 0) * * OUTPUTS: * * none * * RETURN VALUE: * * LwIn * exponential output * * DESCRIPTION: * * polynomial approximation is used for the generation of the exponential * * 2**(32*X) = 0.1713425*X*X + 0.6674432*X + 0.9979554 * c2 c1 c0 * *************************************************************************/ int DVTXOP_fnExp2(int L_Input) { static short pswPCoefE[3] = { /* c2, c1, c0 */ 0x15ef, 0x556f, 0x7fbd }; short swTemp1, swTemp2, swTemp3, swTemp4; int LwIn; swTemp3 = 0x0020; /* determine normlization shift count */ /* ---------------------------------- */ swTemp1 = DVTXOP_extract_h(L_Input); LwIn = DVTXOP_L_mult(swTemp1, swTemp3); swTemp2 = DVTXOP_extract_h(LwIn); /* determine un-normalized shift count */ /* ----------------------------------- */ swTemp3 = -0x0001; swTemp4 = DVTXOP_sub(swTemp3, swTemp2); /* normalize input */ /* --------------- */ LwIn = LwIn & 0xffff; LwIn = DVTXOP_L_add(LwIn, DVTXOP_L_deposit_h(swTemp3)); LwIn = DVTXOP_L_shr(LwIn, 1); swTemp1 = DVTXOP_extract_l(LwIn); /* calculate x*x*c2 */ /* ---------------- */ swTemp2 = DVTXOP_mult_r(swTemp1, swTemp1); LwIn = DVTXOP_L_mult(swTemp2, pswPCoefE[0]); /* calculate x*x*c2 + x*c1 */ /* ----------------------- */ LwIn = DVTXOP_L_mac(LwIn, swTemp1, pswPCoefE[1]); /* calculate x*x*c2 + x*c1 + c0 */ /* --------------------------- */ LwIn = DVTXOP_L_add(LwIn, DVTXOP_L_deposit_h(pswPCoefE[2])); /* un-normalize exponent if its requires it */ /* ---------------------------------------- */ if (swTemp4 > 0) { LwIn = DVTXOP_L_shr(LwIn, swTemp4); } /* return result */ /* ------------- */ return (LwIn); } int DVTXOP_fnLog2(int L_Input) { static short swC0 = -0x2b2a, swC1 = 0x7fc5, swC2 = -0x54d0; short siShiftCnt, swInSqrd, swIn; int LwIn; /* normalize input and store shifts required */ /* ----------------------------------------- */ siShiftCnt = DVTXOP_norm_l(L_Input); LwIn = DVTXOP_L_shl(L_Input, siShiftCnt); siShiftCnt = DVTXOP_add(siShiftCnt, 1); siShiftCnt = DVTXOP_negate(siShiftCnt); /* calculate x*x*c0 */ /* ---------------- */ swIn = DVTXOP_extract_h(LwIn); swInSqrd = DVTXOP_mult_r(swIn, swIn); LwIn = DVTXOP_L_mult(swInSqrd, swC0); /* DVTXOP_add x*c1 */ /* --------- */ LwIn = DVTXOP_L_mac(LwIn, swIn, swC1); /* DVTXOP_add c2 */ /* ------ */ LwIn = DVTXOP_L_add(LwIn, DVTXOP_L_deposit_h(swC2)); /* apply *(4/32) */ /* ------------- */ LwIn = DVTXOP_L_shr(LwIn, 3); LwIn = LwIn & 0x03ffffff; siShiftCnt = DVTXOP_shl(siShiftCnt, 10); LwIn = DVTXOP_L_add(LwIn, DVTXOP_L_deposit_h(siShiftCnt)); /* return log2 */ /* ----------- */ return (LwIn); } #ifndef BASIC_LL_OPS /*___________________________________________________________________________ | | | Function Name : DVTXOP_LL_deposit_h | | | | Purpose : | | | | Deposit the 32 bit var1 into the 32 MS bits of the 64 bit output. The | | 32 LS bits of the output are zeroed. | | | |___________________________________________________________________________| */ long long DVTXOP_LL_deposit_h(int var1) { long long L_var_out; L_var_out = (long long)var1 << 32; return (L_var_out); } /*___________________________________________________________________________ | | | Function Name : DVTXOP_LL_add | | | | Purpose : | | | | 32 bits addition of the two 32 bits variables (L_var1+L_var2) with | | overflow control and saturation; | | | |___________________________________________________________________________| */ long long DVTXOP_LL_add(long long L_var1, long long L_var2) { long long L_var_out; L_var_out = L_var1 + L_var2; if (((L_var1 ^ L_var2) & DVTX_MIN_64) == 0) { if ((L_var_out ^ L_var1) & DVTX_MIN_64) { L_var_out = (L_var1 < 0) ? DVTX_MIN_64 : DVTX_MAX_64; DVTXOP_Overflow = 1; } } return (L_var_out); } /*___________________________________________________________________________ | | | Function Name : DVTXOP_LL_sub | | | | Purpose : | | | | 64 bits subtraction of the two 64 bits variables (L_var1-L_var2) with | | overflow control and saturation | |___________________________________________________________________________| */ long long DVTXOP_LL_sub(long long L_var1, long long L_var2) { long long L_var_out; L_var_out = L_var1 - L_var2; if (((L_var1 ^ L_var2) & DVTX_MIN_64) != 0) { if ((L_var_out ^ L_var1) & DVTX_MIN_64) { L_var_out = (L_var1 < 0L) ? DVTX_MIN_64 : DVTX_MAX_64; DVTXOP_Overflow = 1; } } return (L_var_out); } long long DVTXOP_LL_mult(int var1, int var2) { long long L_var_out; L_var_out = (long long)var1 *(long long)var2; if (L_var_out != (long long)0x4000000000000000LL) { L_var_out *= 2; } else { DVTXOP_Overflow = 1; L_var_out = DVTX_MAX_64; } return (L_var_out); } int DVTXOP_L_extract_h(long long L_var1) { int var_out; var_out = (int)(L_var1 >> 32); return (var_out); } int DVTXOP_L_round(long long L_var1) { int var_out; long long L_rounded; L_rounded = DVTXOP_LL_add(L_var1, (long long)0x0000000080000000LL); var_out = DVTXOP_L_extract_h(L_rounded); return (var_out); } long long DVTXOP_LL_negate(long long L_var1) { long long L_var_out; L_var_out = (L_var1 == DVTX_MIN_64) ? DVTX_MAX_64 : -L_var1; return (L_var_out); } long long DVTXOP_LL_shr(long long L_var1, short var2) { long long L_var_out; if (var2 < 0) { L_var_out = DVTXOP_L_shl(L_var1, (short)(-var2)); } else { if (var2 >= 63) { L_var_out = (L_var1 < 0LL) ? -1 : 0; } else { if (L_var1 < 0) { L_var_out = ~((~L_var1) >> var2); } else { L_var_out = L_var1 >> var2; } } } return (L_var_out); } #endif /*Solomonvoice function Q format for num and denom should be same output Q format : Q,31-Q */ int fx_SolomonVoice_32bit_Divide(int numerator, int denominator, short MIN_denominator, short Qbit) { short norm_shift_num, norm_shift_denom; int L_num, L_denom, Lout; // numerator norm_shift_num = DVTXOP_norm_l(numerator); L_num = DVTXOP_L_shl(numerator, (short)(norm_shift_num - 1)); //denominator norm_shift_denom = DVTXOP_norm_l(denominator); L_denom = DVTXOP_L_shl(denominator, norm_shift_denom); L_denom = DVTX_MAX(L_denom, MIN_denominator); // normalization // devide Lout = DVTXOP_L_divide(L_num, L_denom); //2 Lout = DVTXOP_L_shr_r(Lout, (short)(Qbit - 1 + norm_shift_num - norm_shift_denom)); return Lout; } short int_sqrt_0_50(int num) //in: Q31, out:Q15 { int i; short guess, root; root = 0; for (i = 14; i >= 0; i--) { guess = root | (1 << i); if (num >= DVTXOP_L_mult(guess, guess)) root = guess; } return root; } #ifdef OPT_int_sqrt_0_50_four void int_sqrt_0_50_four(int * in, short* out, int times) //in: Q31, out:Q15 { int i, j; int g0, g1, g2, g3; ae_int32x2 numv0,numv1, tmpLL0, tmpLL1, tmpLL2, tmpLL3,tmpLL4, tmpLL5 ; ae_int16x4 rootv, tmpv, guessv, onev; xtbool2 bool2; ae_int32x2* ptin; ae_int16x4* ptout; ae_valign align1, align2; ptin = (ae_int32x2*)in; ptout = (ae_int16x4*)out; align1 = AE_LA64_PP(ptin); align2 = AE_ZALIGN64(); onev = AE_MOVDA16(1); for(j = 0; j< times; j++) { AE_LA32X2_IP(numv0,align1, ptin); AE_LA32X2_IP(numv1,align1, ptin); rootv = AE_MOVDA16(0); for (i = 14; i >= 0; i--) { tmpv = AE_SLAA16S(onev, i); guessv = AE_OR16(rootv, tmpv); AE_MULF16X4SS(tmpLL0, tmpLL1, guessv, guessv); tmpLL2 = AE_SEXT32X2D16_32(guessv); bool2 = AE_LE32(tmpLL0, numv0); tmpLL3 = AE_SEXT32X2D16_32(rootv); AE_MOVT32X2(tmpLL3, tmpLL2, bool2); tmpLL4 = AE_SEXT32X2D16_10(guessv); bool2 = AE_LE32(tmpLL1, numv1); tmpLL5 = AE_SEXT32X2D16_10(rootv); AE_MOVT32X2(tmpLL5, tmpLL4, bool2); rootv = AE_CVT16X4(tmpLL3, tmpLL5); } AE_SA16X4_IP(rootv,align2, ptout); } AE_SA64POS_FP(align2, ptout); } #endif short int_sqrt_0_25(int num) //in: Q31, out:Q15 { int i; short guess, tmp, root; root = 0; for (i = 14; i >= 0; i--) { guess = root | (1 << i); tmp = DVTXOP_mult(guess, guess); if (num >= DVTXOP_L_mult(tmp, tmp)) root = guess; } return root; } #ifdef OPT_int_sqrt_0_25_four void int_sqrt_0_25_four(int * in, short* out) //in: Q31, out:Q15 { int i; int g0, g1, g2, g3; ae_int32x2 numv0,numv1, tmpLL0, tmpLL1, tmpLL2, tmpLL3,tmpLL4, tmpLL5 ; ae_int16x4 rootv, tmpv, guessv, onev; xtbool2 bool2; ae_int32x2* ptin; ae_int16x4* ptout; ae_valign align1, align2; ptin = (ae_int32x2*)in; ptout = (ae_int16x4*)out; align1 = AE_LA64_PP(ptin); align2 = AE_ZALIGN64(); AE_LA32X2_IP(numv0,align1, ptin); AE_LA32X2_IP(numv1,align1, ptin); onev = AE_MOVDA16(1); rootv = AE_MOVDA16(0); for (i = 14; i >= 0; i--) { tmpv = AE_SLAA16S(onev, i); guessv = AE_OR16(rootv, tmpv); tmpv = AE_MULFP16X4S(guessv, guessv); AE_MULF16X4SS(tmpLL0, tmpLL1, tmpv, tmpv); tmpLL2 = AE_SEXT32X2D16_32(guessv); bool2 = AE_LE32(tmpLL0, numv0); tmpLL3 = AE_SEXT32X2D16_32(rootv); AE_MOVT32X2(tmpLL3, tmpLL2, bool2); tmpLL4 = AE_SEXT32X2D16_10(guessv); bool2 = AE_LE32(tmpLL1, numv1); tmpLL5 = AE_SEXT32X2D16_10(rootv); AE_MOVT32X2(tmpLL5, tmpLL4, bool2); rootv = AE_CVT16X4(tmpLL3, tmpLL5); } AE_SA16X4_IP(rootv,align2, ptout); AE_SA64POS_FP(align2, ptout); } #endif short int_sqrt_0_75(int num) //in: Q31, out:Q15 { short root; root = int_sqrt_0_50(num); num = DVTXOP_L_mpy_ls(num, root); root = int_sqrt_0_50(num); return root; } int DVTXOP_Mpy_32_16(short hi, short lo, short n) { int L_32; L_32 = DVTXOP_L_mult(hi, n); L_32 = DVTXOP_L_mac(L_32, DVTXOP_mult(lo, n), 1); return (L_32); } void DVTXOP_L_Extract(int L_32, short *hi, short *lo) { *hi = DVTXOP_extract_h(L_32); *lo = DVTXOP_extract_l(DVTXOP_L_msu(DVTXOP_L_shr(L_32, 1), *hi, 16384)); return; } int divide(int var1_denorm, int q1, int var2_denorm, int q2, int dest_qForm) { int signbit = 0; int var1 = var1_denorm; int var2 = var2_denorm; int ret = 0; short n1; short n2; if (var1_denorm < 0) { var1 = DVTXOP_L_negate(var1_denorm); signbit = !signbit; } if (var2_denorm < 0) { var2 = DVTXOP_L_negate(var2_denorm); signbit = !signbit; } if (var1_denorm == 0) { return 0; } if (var2_denorm == 0) { //divide by 0, undefined, just return var1_denorm return var1_denorm; } n1 = DVTXOP_norm_l(var1); n2 = DVTXOP_norm_l(var2); //adjust var1 to 0x20000000 to 0x3fffffff, adjust var2 to 0x40000000 to 0x7fffffff if (n1 == 0) { var1 = var1 >> 1; } else { var1 = var1 << (n1 - 1); } var2 = var2 << n2; ret = DVTXOP_L_divide(var1, var2); //6 //ret = Fixed2Fixed_f(div_ret, 31 + q1 + n1 - 1 - q2 - n2, dest_qForm); ret = DVTXOP_L_shl(ret, (short)(dest_qForm - (31 + q1 + n1 - 1 - q2 - n2))); if (signbit) { ret = DVTXOP_L_negate(ret); } return ret; } int ilog2(unsigned int _v) { #ifndef FLAG_DSP_ASM_ON int ret, m; ret = !!_v; m = !!(_v & 0xFFFF0000) << 4; _v >>= m; ret |= m; m = !!(_v & 0xFF00) << 3; _v >>= m; ret |= m; m = !!(_v & 0xF0) << 2; _v >>= m; ret |= m; m = !!(_v & 0xC) << 1; _v >>= m; ret |= m; ret += !!(_v & 0x2); return ret; #else return (31 - AE_NSAZ32_L(_v)); #endif } int sqrt32(int x) //input: Qe, output Qe/2 { int k, rt; short n, tmp; static const short C[5] = { 23175, 11561, -3011, 1699, -664 }; if (x == 0) return 0; else if (x >= 1073741824) return 32767; #ifndef FLAG_DSP_ASM_ON k = ((ilog2(x) - 1) >> 1) - 7; x = (k >= 0) ? (x >> (2 * k)) : (x << (-2 * k)); #else k = ((30 - AE_NSAZ32_L(x)) >> 1) - 7; x = (int)(AE_INT32_SLA(x, (-2 * k))); #endif n = x - 32768; #ifndef FLAG_DSP_ASM_ON tmp = (short)(((int)2 * n*C[4]) >> 16); tmp += C[3]; tmp = (short)(((int)2 * n*tmp) >> 16); tmp += C[2]; tmp = (short)(((int)2 * n*tmp) >> 16); tmp += C[1]; tmp = (short)(((int)2 * n*tmp) >> 16); #else tmp = (short)(((int)MULT16_16(n, C[4])) >> 15); tmp += C[3]; tmp = (short)(((int)MULT16_16(n, tmp)) >> 15); tmp += C[2]; tmp = (short)(((int)MULT16_16(n, tmp)) >> 15); tmp += C[1]; tmp = (short)(((int)MULT16_16(n, tmp)) >> 15); #endif tmp += C[0]; k = 7 - k; #ifndef FLAG_DSP_ASM_ON rt = (k >= 0) ? (tmp >> k) : (tmp << (-k)); #else rt = (int)(AE_INT32_SLA(tmp, (-k))); #endif return rt; } /** Calculate 10log10(pwr) @param pwr Q(pwr_q) @param pwr_q Q for pwr variable (~31) @return 10log10(pwr), Q23 */ int DVTXOP_10log10_l(int pwr, short pwr_q) { int pwrdB; /* fnLog10(pwr) - fnLog10(1) - pwr_q*(fnLog10(2) - fnLog10(1)) */ // pwrdB: Q26(31-5) /* pwr = L_sub(L_sub(fnLog10(L_add(pwr, L_shl(1, pwr_q))), LOG10FIX), L_shl(L_mpy_ls(L_sub(LOG10QFIX, LOG10FIX), shl(pwr_q, 10)), 5)); // 8*log10(x): Q23 pwrdB = L_add(pwr, L_shr(pwr, 2)); // 10*log10(x): Q23 */ int Ltmp1, Ltmp2, Ltmp3; Ltmp1 = DVTXOP_L_sub(DVTXOP_fnLog10(pwr), LOG10FIX); // 8*log10(pwr), Q23 Ltmp2 = DVTXOP_L_sub(LOG10QFIX, LOG10FIX); // 8*log10(2), Q23 Ltmp3 = DVTXOP_L_shl(DVTXOP_L_mpy_ls(Ltmp2, DVTXOP_shl(pwr_q, 10)), 5); // 8 * pwr_q * log10(2), Q23 Ltmp2 = DVTXOP_L_sub(Ltmp1, Ltmp3); // 8*log10(x) = 8*(log10(pwr) - pwr_q*log10(2)), Q23 pwrdB = DVTXOP_L_add(Ltmp2, DVTXOP_L_shr_r(Ltmp2, 2)); // 10*log10(x): Q23 return pwrdB; } /************************************************************************* * * FUNCTION: Log2() * * PURPOSE: Computes log2(L_x), where L_x is positive. * If L_x is negative or zero, the result is 0. * * DESCRIPTION: * The function Log2(L_x) is approximated by a table and linear * interpolation. The following steps are used to compute Log2(L_x) * * 1- Normalization of L_x. * 2- exponent = 30-exponent * 3- i = bit25-b31 of L_x; 32<=i<=63 (because of normalization). * 4- a = bit10-b24 * 5- i -=32 * 6- fraction = table[i]<<16 - (table[i] - table[i+1]) * a * 2 * *************************************************************************/ short DVTXOP_Log2( long L_x, /* (i) : input value */ short *exponent, /* (o) : Integer part of Log2. (range: 0<=val<=30) */ short *fraction /* (o) : Fractional part of Log2. (range: 0<=val<1) */ ) { short exp, i, a, tmp; long L_y; if (L_x <= (long)0) { *exponent = 0; *fraction = 0; return 0; } exp = DVTXOP_norm_l(L_x); L_x = DVTXOP_L_shl(L_x, exp); /* L_x is normalized */ *exponent = DVTXOP_sub(30, exp); L_x = DVTXOP_L_shr(L_x, 9); i = DVTXOP_extract_h(L_x); /* Extract b25-b31 */ L_x = DVTXOP_L_shr(L_x, 1); a = DVTXOP_extract_l(L_x); /* Extract b10-b24 of fraction */ a = a & (short)0x7fff; i = DVTXOP_sub(i, 32); L_y = DVTXOP_L_deposit_h(table[i]); /* table[i] << 16 */ tmp = DVTXOP_sub(table[i], table[i + 1]); /* table[i] - table[i+1] */ L_y = DVTXOP_L_msu(L_y, tmp, a); /* L_y -= tmp*a*2 */ *fraction = DVTXOP_extract_h(L_y); return 0; }