#include "SamsungSolomonVoiceW_Int.h" #include "SamsungSolomonVoiceW_Table.h" #include "SamsungSolomonVoiceW_basic_op.h" #include "SamsungSolomonVoiceW_Default_functions.h" #if(FLAG_SELECT_C_DSP == 1) #include #include #include #endif /* Filter */ #ifndef FN_2ndIIR_FILTERING_OPT #ifndef FN_2ndIIR_FILTERING_OPT_DSP extern int FN_2ndIIR_FILTERING(short(*inout), short n, int* coeffs, int *state) { // load state int lst1 = state[0]; int lst2 = state[1]; int lst3 = state[2]; int lst4 = state[3]; unsigned int lstqer = state[4]; // load coeffs int b0 = coeffs[0]; int b1 = coeffs[1]; int b2 = coeffs[2]; int a1 = coeffs[3]; int a2 = coeffs[4]; do { int outdata; //int data=(*inout)[0]<<(DVTX_NOISEBITS+(32-DVTX_QCOEFFLARGE)); int data = (*inout) << (DVTX_NOISEBITS + (32 - DVTX_QCOEFFLARGE)); INT64 acc = lstqer; acc = DVTX_SMLAL(acc, (data), b0); acc = DVTX_SMLAL(acc, lst1, b1); acc = DVTX_SMLAL(acc, lst2, b2); acc = DVTX_SMLAL(acc, lst3, a1); acc = DVTX_SMLAL(acc, lst4, a2); lstqer = acc & DVTX_QERMASK; outdata = ((int)(acc >> 32)); lst4 = lst3; lst3 = outdata << (32 - DVTX_QCOEFFLARGE); lst2 = lst1; lst1 = data; //outdata>>=DVTX_NOISEBITS; outdata = (outdata + (1 << (DVTX_NOISEBITS - 1))) >> DVTX_NOISEBITS; // saturation if (outdata > 32767) outdata = 32767; if (outdata < -32768) outdata = -32768; //(*inout++)[0] = (short)outdata; (*inout++) = (short)outdata; } while (--n > 0); // save state state[0] = lst1; state[1] = lst2; state[2] = lst3; state[3] = lst4; state[4] = lstqer; return 0; } #else extern FN_2ndIIR_FILTERING(short(*inout), short n, int* coeffs, int *state) { ae_int32x2 lsta, lstb, b12, a12; ae_valign alignstate; ae_int32x2 *pstate; ae_int64 acc1, acc2, QMask; ae_int32x2 acctmp1, acctmp2; ae_int32x2 OUTMAX, OUTMIN; int outdata, tmpdata, data; pstate = (ae_int32x2 *)&state[0]; alignstate = AE_LA64_PP(pstate); AE_LA32X2_IP(lsta, alignstate, pstate); AE_LA32X2_IP(lstb, alignstate, pstate); pstate = (ae_int32x2 *)&coeffs[1]; alignstate = AE_LA64_PP(pstate); AE_LA32X2_IP(b12, alignstate, pstate); AE_LA32X2_IP(a12, alignstate, pstate); alignstate = AE_ZALIGN64(); pstate = (ae_int32x2 *)&state[0]; int temp = state[4]; int b0 = coeffs[0]; QMask = AE_MOV64(0x00000000ffffffffLL); acctmp1 = AE_MOVDA32X2(0, temp); acc2 = AE_CVT64F32_L(acctmp1); acc2 = AE_SRLI64(acc2, 32); do { data = (*inout) << (DVTX_NOISEBITS + (32 - DVTX_QCOEFFLARGE)); acctmp1 = AE_MOVDA32X2(0, b0); acctmp2 = AE_MOVDA32X2(0, data); AE_MULA32_LL(acc2, lsta, b12); AE_MULA32_HH(acc2, lsta, b12); AE_MULA32_LL(acc2, lstb, a12); AE_MULA32_HH(acc2, lstb, a12); AE_MULA32_LL(acc2, acctmp1, acctmp2); acctmp1 = AE_TRUNCA32F64S(acc2, 0); outdata = AE_MOVAD32_H(acctmp1); tmpdata = outdata << (32 - DVTX_QCOEFFLARGE); acc2 = AE_AND64(acc2, QMask); acctmp1 = AE_MOVDA32X2(data, tmpdata); lsta = AE_SEL32_HH(acctmp1, lsta); lstb = AE_SEL32_LH(acctmp1, lstb); outdata = (outdata + (1 << (DVTX_NOISEBITS - 1))) >> DVTX_NOISEBITS; if (outdata > 32767) outdata = 32767; else if (outdata < -32768) outdata = -32768; (*inout++) = (short)outdata; }while(--n > 0); acc2 = AE_SLAI64(acc2, 32); acctmp1 = AE_TRUNCA32F64S(acc2, 0); outdata = AE_MOVAD32_L(acctmp1); AE_SA32X2_IP(lsta, alignstate, pstate); AE_SA32X2_IP(lstb, alignstate, pstate); AE_SA64POS_FP(alignstate, pstate); state[4] = outdata; return 0; } #endif #else extern int FN_2ndIIR_FILTERING(short(*inout), short n, int* coeffs, int *state) { // load state int lst1 = state[0]; int lst2 = state[1]; int lst3 = state[2]; int lst4 = state[3]; unsigned int lstqer = state[4]; // load coeffs int b0 = coeffs[0]; int b1 = coeffs[1]; int b2 = coeffs[2]; int a1 = coeffs[3]; int a2 = coeffs[4]; do { int outdata; //int data=(*inout)[0]<<(DVTX_NOISEBITS+(32-DVTX_QCOEFFLARGE)); int data = (*inout) << (DVTX_NOISEBITS + (32 - DVTX_QCOEFFLARGE)); INT64 acc = lstqer; acc = DVTX_SMLAL(acc, (data), b0); acc = DVTX_SMLAL(acc, lst1, b1); acc = DVTX_SMLAL(acc, lst2, b2); acc = DVTX_SMLAL(acc, lst3, a1); acc = DVTX_SMLAL(acc, lst4, a2); lstqer = acc & DVTX_QERMASK; outdata = ((int)(acc >> 32)); lst4 = lst3; lst3 = outdata << (32 - DVTX_QCOEFFLARGE); lst2 = lst1; lst1 = data; //outdata>>=DVTX_NOISEBITS; outdata = (outdata + (1 << (DVTX_NOISEBITS - 1))) >> DVTX_NOISEBITS; // saturation if (outdata > 32767) outdata = 32767; if (outdata < -32768) outdata = -32768; //(*inout++)[0] = (short)outdata; (*inout++) = (short)outdata; } while (--n > 0); // save state state[0] = lst1; state[1] = lst2; state[2] = lst3; state[3] = lst4; state[4] = lstqer; return 0; } #endif /** fixed point port of Fn_vector_copy for short arrays @param[out] Out array, size N, Q same as in A @param[in] A array, size N, any Q @param N arrays' size */ #ifndef Fx_vector_copy_s_OPT_DSP void Fx_vector_copy_s(short *Out, short *A, short N) { short k; for (k = 0; k < N; k++) Out[k] = A[k]; } #else #ifndef KHW_OPTI_20191216_HIFI void Fx_vector_copy_s(short *Out, short *A, short N) { ae_valign align1, align2; ae_int16x4 *pt1, *pt2; ae_int16x4 data; int i; pt1=(ae_int16x4 *)A; pt2=(ae_int16x4 *)Out; align1 = AE_LA64_PP(pt1); align2 = AE_ZALIGN64(); for (i = 0; i < N; i+=4) { AE_LA16X4_IP(data, align1, pt1); AE_SA16X4_IP(data, align2, pt2); } AE_SA64POS_FP(align2, pt2); } #endif #ifndef KHW_OPTI_20191216_HIFI ////////////// not used /////////////////////// void Fx_vector_copy_s_32(int *Out, int *A, int N) { ae_valign align1, align2; ae_int32x2 *pt1, *pt2; ae_int32x2 data; int i; pt1=(ae_int32x2 *)A; pt2=(ae_int32x2 *)Out; align1 = AE_LA64_PP(pt1); align2 = AE_ZALIGN64(); for (i = 0; i < N; i+=2) { AE_LA32X2_IP(data, align1, pt1); AE_SA32X2_IP(data, align2, pt2); } AE_SA64POS_FP(align2, pt2); } /////////////////////////////////////////////////// #endif #endif //void Fx_vector_copy_s(short *Out, short *A, short N) #ifndef KHW_OPTI_20191216_HIFI void Fx_vector_copy_s_for(short *Out, short *A, short N) { short k; for (k = 0; k < N; k++) Out[k] = A[k]; } #endif #ifndef Fx_vector_gain_OPT_DSP void Fx_vector_gain(short* A, short gain, short gain_q, short N) { short k; short gain_q_1 = DVTXOP_add(gain_q, 1); int Ltmp; for (k = 0; k < N; k++) { // Ltmp = DVTXOP_L_shr(DVTXOP_L_mult(A[k], gain), gain_q_1); Ltmp = DVTXOP_L_shr_r(DVTXOP_L_mult(A[k], gain), gain_q_1); if (Ltmp > DVTX_MAX_16) Ltmp = DVTX_MAX_16; if (Ltmp < DVTX_MIN_16) Ltmp = DVTX_MIN_16; A[k] = DVTXOP_extract_l(Ltmp); } } #else void Fx_vector_gain(short* A, short gain, short gain_q, short N) { short k; short gain_q_1 = DVTXOP_add(gain_q, 1); ae_f32x2 L_tmp1, L_tmp2; ae_f16x4 A1, A2; ae_valign align1, align2; ae_f16x4 *pA=(ae_f16x4 *)A; ae_f16x4 *pOut=(ae_f16x4 *)A; ae_f16x4 vgain = AE_MOVDA16(gain); align1 = AE_LA64_PP(pA); align2 = AE_ZALIGN64(); for (k = 0; k < N; k+=4) { AE_LA16X4_IP(A1, align1, pA); AE_MULF16X4SS(L_tmp1, L_tmp2, A1, vgain); // L_tmp1 = AE_SRAA32S(L_tmp1, gain_q_1); // L_tmp2 = AE_SRAA32S(L_tmp2, gain_q_1); // DVTXOP_L_shr L_tmp1 = AE_SRAA32RS(L_tmp1, gain_q_1); // DVTXOP_L_shr_r L_tmp2 = AE_SRAA32RS(L_tmp2, gain_q_1); A2 = AE_SAT16X4(L_tmp1, L_tmp2); AE_SA16X4_IP(A2, align2, pOut); } AE_SA64POS_FP(align2, pOut); } #endif /** Calculate sum of squares for given signal window @param[in] A array, size N, Q0 @param N array size @param[out] sum calculated sum, Q(sum_q) @param[out] sum_q Q of sum variable */ #ifndef Fx_vector_sum_squares_OPT_DSP int Fx_vector_sum_squares(short *A, short N, short *sum_q) { int tmp, sum; short k; sum = 0; *sum_q = 0; for (k = 0; k < N; k++) { tmp = DVTXOP_L_mult(A[k], A[k]); // tmp: Q1 always positive tmp = DVTXOP_L_shr(tmp, *sum_q); sum = DVTXOP_L_add(sum, tmp); // sum: Q(1-sum_q) if (sum > 0x4000000) { sum = DVTXOP_L_shr(sum, 1); *sum_q = DVTXOP_add(*sum_q, 1); } } return sum; } #else int Fx_vector_sum_squares(short *A, short N, short *sum_q) { int tmp, sum; short k; ae_f32x2 L_tmp1, L_tmp2; ae_f16x4 A1; ae_valign align1; ae_f16x4 *pA=(ae_f16x4 *)A; sum = 0; *sum_q = 0; align1 = AE_LA64_PP(pA); for (k = 0; k < N; k+= 4 ) { AE_LA16X4_IP(A1, align1, pA); AE_MULF16X4SS(L_tmp1, L_tmp2, A1, A1); tmp = AE_MOVAD32_H(L_tmp1); tmp = DVTXOP_L_shr(tmp, *sum_q); sum = DVTXOP_L_add(sum, tmp); if (sum > 0x4000000) { sum = DVTXOP_L_shr(sum, 1); *sum_q = DVTXOP_add(*sum_q, 1); } tmp = AE_MOVAD32_L(L_tmp1); tmp = DVTXOP_L_shr(tmp, *sum_q); sum = DVTXOP_L_add(sum, tmp); if (sum > 0x4000000) { sum = DVTXOP_L_shr(sum, 1); *sum_q = DVTXOP_add(*sum_q, 1); } tmp = AE_MOVAD32_H(L_tmp2); tmp = DVTXOP_L_shr(tmp, *sum_q); sum = DVTXOP_L_add(sum, tmp); if (sum > 0x4000000) { sum = DVTXOP_L_shr(sum, 1); *sum_q = DVTXOP_add(*sum_q, 1); } tmp = AE_MOVAD32_L(L_tmp2); tmp = DVTXOP_L_shr(tmp, *sum_q); sum = DVTXOP_L_add(sum, tmp); if (sum > 0x4000000) { sum = DVTXOP_L_shr(sum, 1); *sum_q = DVTXOP_add(*sum_q, 1); } } return sum; } #endif /** Calculate 10log10(pwr) @param pwr Q(pwr_q) @param pwr_q Q for pwr variable @return 10log10(pwr), Q8 */ short Fx_10log10(int pwr, short pwr_q) { short pwrdB; /* DVTXOP_fnLog10(pwr) - DVTXOP_fnLog10(1) - pwr_q*(DVTXOP_fnLog10(2) - DVTXOP_fnLog10(1)) */ // pwr: Q26(31-5) pwr = DVTXOP_L_sub(DVTXOP_L_sub(DVTXOP_fnLog10(DVTXOP_L_add(pwr, DVTXOP_shl(1, pwr_q))), DVTX_LOG10FIX), DVTXOP_L_shl(DVTXOP_L_mpy_ls(DVTXOP_L_sub(DVTX_LOG10QFIX, DVTX_LOG10FIX), DVTXOP_shl(pwr_q, 11)), 4)); pwr = DVTXOP_L_shr(pwr, 4); // Q26 -> Q22 pwr = DVTXOP_L_shl(DVTXOP_L_mpy_ls(pwr, 0x2800), 5); // 10 << 10 and then again DVTXOP_shl to obtain Q15 pwrdB = DVTXOP_extract_l(DVTXOP_L_shr(pwr, 14)); // Q22 -> Q8 return pwrdB; } /** Calculate RMS power for given signal window @param[in] A array, size N, Q0 @param N array size @return power in decibels, Q8 */ short Fx_time_vector_power_dB(short *A, short N) { int pwr; short pwrdB, k, q, trunc_shift; pwr = Fx_vector_sum_squares(A, N, &trunc_shift); //int pwr1 = pwr; if (pwr == 0) return 0; //k = DVTXOP_norm_l(N); // for N=160 k is 23 //q = DVTXOP_sub(32, trunc_shift); // q (Q31+1-trunc_shift-k) //q = DVTXOP_sub(q, k); //pwr = DVTXOP_L_divide(pwr, DVTXOP_L_shl(N, k)); // N: Q0 -> Q.k, pwr: Q(31+1-trunc_shift-k) //pwrdB = Fx_10log10(pwr, q); // jaemo yang : substitute DVTXOP_L_divide to DVTXOP_L_mpy_ls... k = 24; // for N=160 k is 23 q = DVTXOP_sub(32, trunc_shift); // q (Q31+1-trunc_shift-k) q = DVTXOP_sub(q, k); if (N == 160) //NB pwr = DVTXOP_L_mpy_ls(pwr, 26214); //26214 = 1/160 Q22 N: Q0 -> Q.k, pwr: Q(31+1-trunc_shift-k) else if (N == 320) //WB pwr = DVTXOP_L_mpy_ls(pwr, 13107); // N: Q0 -> Q.k, pwr: Q(31+1-trunc_shift-k) else //SWB pwr = DVTXOP_L_mpy_ls(pwr, 6553); // N: Q0 -> Q.k, pwr: Q(31+1-trunc_shift-k) pwrdB = Fx_10log10(pwr, q); return pwrdB; } extern int Fx_recursive_avg(int A, int B, short beta) { short beta_inv = DVTXOP_add(DVTXOP_sub(DVTX_MAX_16, beta), 1); return DVTXOP_L_add(DVTXOP_L_mpy_ls(A, beta), DVTXOP_L_mpy_ls(B, beta_inv)); } extern void fx_SolomonVoice_FFT_to_FFTpwr_ave(int *FFTpwrVecAve, int *FFTpwrVec, short *FFTbinVec, short shift, short alpha, short oneminusalpha, short Nffthalf) { short i, j; int enrgE; for (i = 0; i < Nffthalf; i++) { j = 2 * i; enrgE = DVTXOP_L_mult(FFTbinVec[j], FFTbinVec[j]); //Q15,0*Q15,0 = Q30,1 enrgE = DVTXOP_L_mac(enrgE, FFTbinVec[j + 1], FFTbinVec[j + 1]); //q30,1 FFTpwrVec[i] = enrgE; enrgE = DVTXOP_L_shr(enrgE, shift); FFTpwrVecAve[i] = ((long long)FFTpwrVecAve[i] * alpha + (long long)enrgE * oneminusalpha) >> 15; } } #ifdef fx_SolomonVoice_FFT_to_FFTpwr_ave_OPT_DSP void fx_SolomonVoice_FFT_to_FFTpwr_ave_vector(int *FFTpwrVecAve, int *FFTpwrVec, short *FFTbinVec, short shift, short alpha, short oneminusalpha, short Nffthalf) { ae_f16x4 *pt1; ae_f32x2 *pt2, *pt3, *pt4; ae_f16x4 A1, A2; ae_f32x2 L_tmp1, L_tmp2, L_tmp3, ave; ae_f64 tmpLL1, tmpLL2; ae_valign align1, align2, align3, align4; int i; pt1 = (ae_f16x4 *)FFTbinVec; pt2 = (ae_f32x2 *)FFTpwrVec; pt3 = (ae_f32x2 *)FFTpwrVecAve; pt4 = (ae_f32x2 *)FFTpwrVecAve; align1 = AE_ZALIGN64(); align2 = AE_ZALIGN64(); align3 = AE_LA64_PP(pt3); align4 = AE_LA64_PP(pt1); for (i = 0; i < Nffthalf; i += 2) { // AE_L16X4_IP(A1,pt1,8); AE_LA16X4_IP(A1, align4, pt1); AE_MULF16X4SS(L_tmp1, L_tmp2, A1, A1); L_tmp3 = AE_SEL32_LH(L_tmp2, L_tmp1); L_tmp1 = AE_ADD32S(L_tmp1, L_tmp3); L_tmp3 = AE_SEL32_LH(L_tmp2, L_tmp2); L_tmp2 = AE_ADD32S(L_tmp2, L_tmp3); L_tmp1 = AE_SEL32_LL(L_tmp1, L_tmp2); AE_SA32X2_IP(L_tmp1, align1, pt2); L_tmp1 = AE_SRAA32(L_tmp1, shift); AE_LA32X2_IP(ave, align3, pt3); tmpLL1 = AE_MULF32S_HL(ave, alpha); AE_MULAF32S_HL(tmpLL1, L_tmp1, oneminusalpha); tmpLL2 = AE_MULF32S_LL(ave, alpha); AE_MULAF32S_LL(tmpLL2, L_tmp1, oneminusalpha); tmpLL1 = AE_SLAI64S(tmpLL1, 16); tmpLL2 = AE_SLAI64S(tmpLL2, 16); // L_tmp1 = AE_ROUND32X2F64SSYM(tmpLL1, tmpLL2); L_tmp1 = AE_TRUNCI32X2F64S(tmpLL1, tmpLL2, 0); AE_SA32X2_IP(L_tmp1, align2, pt4); } AE_SA64POS_FP(align1, pt2); AE_SA64POS_FP(align2, pt4); } #endif extern void fx_SolomonVoice_FFT_to_Phi_ave(int *phi_cross_r, int *phi_cross_i, short *FFTbinVec_main, short *FFTbinVec_sub, short shift_main, short shift_sub, short alpha, short oneminusalpha, short Nffthalf) { short i, j; int tmpr, tmpi; short fft_main[16] = { 0, }; short fft_sub[16] = { 0, }; short shift, shift_diff; /* normalize */ if (shift_main > shift_sub) { shift = 2 * (shift_sub - 1); shift_diff = shift_main - shift_sub; for (i = 0; i < 16; i++) { fft_main[i] = DVTXOP_shr(FFTbinVec_main[i], shift_diff); fft_sub[i] = FFTbinVec_sub[i]; } } else if (shift_main < shift_sub) { shift = 2 * (shift_main - 1); shift_diff = shift_sub - shift_main; for (i = 0; i < 16; i++) { fft_main[i] = FFTbinVec_main[i]; fft_sub[i] = DVTXOP_shr(FFTbinVec_sub[i], shift_diff); } } else { shift = 2 * (shift_main - 1); for (i = 0; i < 16; i++) { fft_main[i] = FFTbinVec_main[i]; fft_sub[i] = FFTbinVec_sub[i]; } } for (i = 0; i < Nffthalf; i++) { j = 2 * i; tmpr = DVTXOP_L_mult(fft_main[j], fft_sub[j]); //Q15,0*Q15,0 = Q30,1 tmpr = DVTXOP_L_mac(tmpr, fft_main[j + 1], fft_sub[j + 1]); //q30,1 tmpr = DVTXOP_L_shr(tmpr, shift); phi_cross_r[i] = ((long long)phi_cross_r[i] * alpha + (long long)tmpr * oneminusalpha) >> 15; tmpi = DVTXOP_L_mult(fft_main[j + 1], fft_sub[j]); //Q15,0*Q15,0 = Q30,1 tmpi = DVTXOP_L_msu(tmpi, fft_main[j], fft_sub[j + 1]); //q30,1 tmpi = DVTXOP_L_shr(tmpi, shift); phi_cross_i[i] = ((long long)phi_cross_i[i] * alpha + (long long)tmpi * oneminusalpha) >> 15; } return; } extern void fx_SolomonVoice_FFT_to_FFTpwr(int *FFTpwrVec, short *FFTbinVec, short Nffthalf) { #ifndef FLAG_DSP_ASM_ON short i, j; int enrgE; for (i = 0; i < Nffthalf; i++) { j = 2 * i; enrgE = DVTXOP_L_mult(FFTbinVec[j], FFTbinVec[j]); //Q15,0*Q15,0 = Q30,1 FFTpwrVec[i] = DVTXOP_L_mac(enrgE, FFTbinVec[j + 1], FFTbinVec[j + 1]); //q30,1 } #else ae_f16x4 *pt1; ae_f32x2 *pt2; ae_f16x4 A1, A2; ae_f32x2 L_tmp1, L_tmp2, L_tmp3; ae_valign alignout, alignin; int i; pt1=(ae_f16x4 *)FFTbinVec; pt2=(ae_f32x2 *)FFTpwrVec; alignout = AE_ZALIGN64(); alignin = AE_LA64_PP(pt1); for (i = 0; i < Nffthalf; i+=2) { AE_LA16X4_IP(A1, alignin, pt1); AE_MULF16X4SS(L_tmp1, L_tmp2, A1, A1); L_tmp3 = AE_SEL32_LH(L_tmp2, L_tmp1); L_tmp1 = AE_ADD32S(L_tmp1, L_tmp3); L_tmp3 = AE_SEL32_LH(L_tmp2, L_tmp2); L_tmp2 = AE_ADD32S(L_tmp2, L_tmp3); L_tmp1 = AE_SEL32_LL(L_tmp1, L_tmp2); AE_SA32X2_IP(L_tmp1, alignout, pt2); } AE_SA64POS_FP(alignout, pt2); #endif } /*Solomonvoice function FFTbin to Band power input : Q30,1, Nfft/2+1 output : Q30,1, Nband */ extern void fx_SolomonVoice_FFTpwr_to_FFTband(int *FFTband, int *FFTbin, short CHindex[][2], short CHbin[][2], short Nband) { short i, j, j1, j2, norm_shift, tmp; int enrgE; for (i = 0; i <= Nband; i++) { enrgE = 0; j1 = CHindex[i][0]; j2 = CHindex[i][1]; for (j = j1; j <= j2; j++) { enrgE = DVTXOP_L_add(enrgE, FFTbin[j]); //Q15,0*Q15,0 = Q30,1 } if (CHbin[i][0] == DVTX_TRUE) { enrgE = DVTXOP_L_shr(enrgE, CHbin[i][1]); } else { norm_shift = DVTXOP_norm_l(enrgE); tmp = DVTXOP_extract_h(DVTXOP_L_shl(enrgE, norm_shift)); enrgE = DVTXOP_L_mult(tmp, CHbin[i][1]); enrgE = DVTXOP_L_shr(enrgE, norm_shift); } FFTband[i] = enrgE; //q30,1 } } #ifdef Merge_FFTband_calc void fx_SolomonVoice_FFTpwr_to_FFTband_merge(int *FFTband, int *FFTbin, int *FFTband1, int *FFTbin1, short CHindex[][2], short CHbin[][2], short Nband) { short i, j, j1, j2, norm_shift, tmp; int enrgE, enrgE1; for (i = 0; i <= Nband; i++) { enrgE = 0; enrgE1 = 0; j1 = CHindex[i][0]; j2 = CHindex[i][1]; for (j = j1; j <= j2; j++) { enrgE = DVTXOP_L_add(enrgE, FFTbin[j]); //Q15,0*Q15,0 = Q30,1 enrgE1 = DVTXOP_L_add(enrgE1, FFTbin1[j]); } if (CHbin[i][0] == DVTX_TRUE) { enrgE = DVTXOP_L_shr(enrgE, CHbin[i][1]); enrgE1 = DVTXOP_L_shr(enrgE1, CHbin[i][1]); } else { norm_shift = DVTXOP_norm_l(enrgE); tmp = DVTXOP_extract_h(DVTXOP_L_shl(enrgE, norm_shift)); enrgE = DVTXOP_L_mult(tmp, CHbin[i][1]); enrgE = DVTXOP_L_shr(enrgE, norm_shift); norm_shift = DVTXOP_norm_l(enrgE1); tmp = DVTXOP_extract_h(DVTXOP_L_shl(enrgE1, norm_shift)); enrgE1 = DVTXOP_L_mult(tmp, CHbin[i][1]); enrgE1 = DVTXOP_L_shr(enrgE1, norm_shift); } FFTband[i] = enrgE; //q30,1 FFTband1[i] = enrgE1; //q30,1 } } #endif #ifndef OPT_fx_SolomonVoice_PwrBin_FixQ_BlkDeNorm extern void fx_SolomonVoice_PwrBin_FixQ_BlkDeNorm(int *out, int *in, short blk_norm, short Q, short minval, short NhalfFFT) { short i; for (i = 0; i < NhalfFFT; i++) { out[i] = DVTX_MAX(DVTXOP_L_shl(in[i], (short)(Q - (2 * blk_norm))), minval);/*30,1 -> 23,8*/ } } #else void fx_SolomonVoice_PwrBin_FixQ_BlkDeNorm(int *out, int *in, short blk_norm, short Q, short minval, short NhalfFFT) { short i; int shift; ae_int32x2 *pin, *pout, A0, minv; ae_valign align1, align2; xtbool2 bool2; pin=(ae_int32x2 *)in; pout= (ae_int32x2 *)out; minv = AE_MOVDA32(minval); shift = (short)(Q - (2 * blk_norm)); align1 = AE_LA64_PP(pin); align2 = AE_ZALIGN64(); for (i = 0; i < NhalfFFT; i+= 2) { AE_LA32X2_IP(A0, align1, pin); A0 = AE_SLAA32S(A0, shift); bool2 = AE_LT32(A0, minv); AE_MOVT32X2(A0, minv, bool2); AE_SA32X2_IP(A0, align2, pout); } AE_SA64POS_FP(align2, pout); } #endif extern int HysteresisHangoverThresholding_MultiState(int val, int thr_table[][2], int table_length, int hangover_up, int hangover_down, int state, short *p_cnt_up, short *p_cnt_down) { short cnt_up = *p_cnt_up; short cnt_down = *p_cnt_down; int thr_down = (state < 1) ? 0 : thr_table[state - 1][0]; int thr_up = (state >(table_length - 1)) ? 2147483647 : thr_table[state][1]; // up threshold //bool q = (val>thr); //if (!state) && q) if (val > thr_up) { if (cnt_up == 0) { cnt_up = hangover_up; } cnt_up--; if (cnt_up <= 0) { state++; } } else if (val < thr_down) { if (cnt_down == 0) { cnt_down = hangover_down; } cnt_down--; if (cnt_down <= 0) { state--; } } else { cnt_up = 0; cnt_down = 0; } state = (state < 0) ? 0 : state; state = (state > table_length) ? table_length : state; *p_cnt_up = cnt_up; *p_cnt_down = cnt_down; return state; } #ifndef OPT_Fn_vector_smoothing_over_3_rectangularwin_length extern void Fn_vector_smoothing_over_3_rectangularwin_length(int *Out, int* A, short N, short LEN) { short i; int Ltmp, Ltmp1, Ltmp2; for (i = 1; i < N - 1; i++) { Ltmp1 = DVTXOP_L_mpy_ls(A[i - 1], 10922); // 1/4 Ltmp2 = DVTXOP_L_mpy_ls(A[i + 1], 10922); Ltmp = DVTXOP_L_add(Ltmp1, Ltmp2); Out[i] = DVTXOP_L_add(Ltmp, DVTXOP_L_mpy_ls(A[i], 10923)); if (Out[i] < 1) Out[i] = 1; } Out[0] = DVTXOP_L_add(DVTXOP_L_shr(A[0], 1), DVTXOP_L_shr(A[1], 1)); Out[N - 1] = DVTXOP_L_add(DVTXOP_L_shr(A[N - 1], 1), DVTXOP_L_shr(A[N - 2], 1)); } #else void Fn_vector_smoothing_over_3_rectangularwin_length(int *Out, int* A, short N, short LEN) { short i; ae_f32x2 *ptOut, *ptA; ae_f32x2 A1, A2, L_tmp1; ae_f32x2 gainv1, gainv2, gainv3, gainv4, mask; ae_valign align1, align2; ae_f64 tmpLL, tmpLL1, tmpLL2; int s; ptOut = (ae_f32x2 *)Out; ptA = (ae_f32x2 *)A; align1 = AE_LA64_PP(ptA); align2 = AE_ZALIGN64(); gainv1 = AE_MOVDA32X2(10922, 10923); mask = AE_MOVDA32X2(0xfffffffe, 0xfffffffe); AE_LA32X2_IP(A1, align1, ptA); A1 = AE_AND32(A1, mask); for (i = 1; i < N - 1; i+=2) { AE_LA32X2_IP(A2, align1, ptA); A2 = AE_AND32(A2, mask); tmpLL = AE_MULF32S_HH(A1,gainv1); tmpLL = AE_SRAI64(tmpLL,16); tmpLL1 = AE_MULF32S_LL(A1,gainv1); tmpLL1 = AE_SRAI64(tmpLL1,16); tmpLL2 = AE_MULF32S_HH(A2,gainv1); tmpLL2 = AE_SRAI64(tmpLL2,16); tmpLL = AE_ADD64(tmpLL, tmpLL1); tmpLL = AE_ADD64(tmpLL, tmpLL2); L_tmp1 = AE_MOVF32X2_FROMF64(tmpLL); Out[i]= AE_MOVAD32_L(L_tmp1); if (Out[i] < 1) Out[i] = 1; tmpLL = AE_MULF32S_LH(A1,gainv1); tmpLL = AE_SRAI64(tmpLL,16); tmpLL1 = AE_MULF32S_HL(A2,gainv1); tmpLL1 = AE_SRAI64(tmpLL1,16); tmpLL2 = AE_MULF32S_LH(A2,gainv1); tmpLL2 = AE_SRAI64(tmpLL2,16); tmpLL = AE_ADD64(tmpLL, tmpLL1); tmpLL = AE_ADD64(tmpLL, tmpLL2); L_tmp1 = AE_MOVF32X2_FROMF64(tmpLL); Out[i+1]= AE_MOVAD32_L(L_tmp1); if (Out[i + 1] < 1) Out[i + 1] = 1; A1 = A2; } Out[0] = DVTXOP_L_add(DVTXOP_L_shr(A[0], 1), DVTXOP_L_shr(A[1], 1)); Out[N - 1] = DVTXOP_L_add(DVTXOP_L_shr(A[N - 1], 1), DVTXOP_L_shr(A[N - 2], 1)); } #endif #ifdef SRCB_1016_OPT void Pre_emp(short * in, short * out,int len, short emp_fac) { int i; ae_f16x4 *pt1, *pt2, *pt3; ae_int16x4 A1, A2, fac; ae_valign align1, align2, align3; pt1=(ae_f16x4 *)in; pt2=(ae_f16x4 *)(in -1); pt3=(ae_f16x4 *)out; align1 = AE_LA64_PP(pt1); align2 = AE_LA64_PP(pt2); align3 = AE_ZALIGN64(); fac = AE_MOVDA16(emp_fac); for(i=0; i< len -3; i+= 4 ) { AE_LA16X4_IP(A1, align1, pt1); AE_LA16X4_IP(A2, align2, pt2); A2 = AE_MULFP16X4S(A2, fac); A1 = AE_ADD16S(A1, A2); AE_SA16X4_IP(A1, align3, pt3); } AE_SA64POS_FP(align3, pt3); for (; i < len; i++) { out[i] = DVTXOP_add(*(in + i), DVTXOP_mult(emp_fac, *(in + i - 1) )); } } #ifdef MinGain_ApplyGain_DSP_OPT void MinGain_ApplyGain(short * out, short * in, int len) { int i; short gain; // for (i = 0; i < 4; i++) // { // gain = DVTX_MAX(in[i], 1000); // in[i] = gain; // out[2 * i] = DVTXOP_mult(out[2 * i], gain); // out[2 * i + 1] = DVTXOP_mult(out[2 * i + 1], gain); // } ae_valign align1,align2, align3,align4; ae_int16x4* p_in =(ae_int16x4*)(in);// source pointer ae_int16x4* p_in_s =(ae_int16x4*)(in);// destination pointer ae_int16x4* p_out =(ae_int16x4*)(out);// destination pointer ae_int16x4* p_out_s =(ae_int16x4*)(out);// destination pointer ae_int16x4 tmp_con=AE_MOVDA16(1000); ae_int16x4 tmp1,tmp2,tmp3,tmp4,tmp5; xtbool4 bool2; align1 = AE_LA64_PP(p_in); align2= AE_ZALIGN64(); align3 = AE_LA64_PP(p_out); align4 = AE_ZALIGN64(); // for (; i < len; i+=4) // { // gain = DVTX_MAX(in[i], 1000); // in[i] = gain; AE_LA16X4_IP(tmp1,align1, p_in); bool2 = AE_LT16(tmp1, tmp_con); AE_MOVT16X4(tmp1, tmp_con, bool2);//根据bool2 AE_SA16X4_IP(tmp1,align2, p_in_s); tmp2=AE_SEL16_7362(tmp1, tmp1);//gain 3322 tmp3=AE_SEL16_5410(tmp1, tmp1);//gain tmp3=AE_SEL16_7362(tmp3, tmp3);//gain 1100 AE_LA16X4_IP(tmp4,align3, p_out); AE_LA16X4_IP(tmp5,align3, p_out); tmp4=AE_MULFP16X4RAS_vector(tmp4, tmp2); tmp5=AE_MULFP16X4RAS_vector(tmp5, tmp3); AE_SA16X4_IP(tmp4,align4, p_out_s); AE_SA16X4_IP(tmp5,align4, p_out_s); // } // AE_SA64POS_FP(align2, p_in_s); // AE_SA64POS_FP(align4, p_out_s); // for (; i < 16; i++) // { // gain = DVTX_MAX(in[i], 500); // in[i] = gain; // out[2 * i] = DVTXOP_mult(out[2 * i], gain); // out[2 * i + 1] = DVTXOP_mult(out[2 * i + 1], gain); // } tmp_con=AE_MOVDA16(120); for (i=4; i < 16; i+=4) { AE_LA16X4_IP(tmp1,align1, p_in); bool2 = AE_LT16(tmp1, tmp_con); AE_MOVT16X4(tmp1, tmp_con, bool2);//根据bool2 AE_SA16X4_IP(tmp1,align2, p_in_s); tmp2=AE_SEL16_7362(tmp1, tmp1);//gain 3322 tmp3=AE_SEL16_5410(tmp1, tmp1);//gain tmp3=AE_SEL16_7362(tmp3, tmp3);//gain 1100 AE_LA16X4_IP(tmp4,align3, p_out); AE_LA16X4_IP(tmp5,align3, p_out); tmp4=AE_MULFP16X4RAS_vector(tmp4, tmp2); tmp5=AE_MULFP16X4RAS_vector(tmp5, tmp3); AE_SA16X4_IP(tmp4,align4, p_out_s); AE_SA16X4_IP(tmp5,align4, p_out_s); } // for (; i < 32; i++) // { // gain = DVTX_MAX(in[i], 100); // in[i] = gain; // out[2 * i] = DVTXOP_mult(out[2 * i], gain); // out[2 * i + 1] = DVTXOP_mult(out[2 * i + 1], gain); // } tmp_con=AE_MOVDA16(120); for (i=16; i < 32; i+=4) { AE_LA16X4_IP(tmp1,align1, p_in); bool2 = AE_LT16(tmp1, tmp_con); AE_MOVT16X4(tmp1, tmp_con, bool2);//根据bool2 AE_SA16X4_IP(tmp1,align2, p_in_s); tmp2=AE_SEL16_7362(tmp1, tmp1);//gain 3322 tmp3=AE_SEL16_5410(tmp1, tmp1);//gain tmp3=AE_SEL16_7362(tmp3, tmp3);//gain 1100 AE_LA16X4_IP(tmp4,align3, p_out); AE_LA16X4_IP(tmp5,align3, p_out); tmp4=AE_MULFP16X4RAS_vector(tmp4, tmp2); tmp5=AE_MULFP16X4RAS_vector(tmp5, tmp3); AE_SA16X4_IP(tmp4,align4, p_out_s); AE_SA16X4_IP(tmp5,align4, p_out_s); } // for (; i < len; i++) // { // gain = DVTX_MAX(in[i], 50); // in[i] = gain; // out[2 * i] = DVTXOP_mult(out[2 * i], gain); // out[2 * i + 1] = DVTXOP_mult(out[2 * i + 1], gain); // } tmp_con=AE_MOVDA16(10); for (i=32; i < len; i+=4) { AE_LA16X4_IP(tmp1,align1, p_in); bool2 = AE_LT16(tmp1, tmp_con); AE_MOVT16X4(tmp1, tmp_con, bool2);//根据bool2 AE_SA16X4_IP(tmp1,align2, p_in_s); tmp2=AE_SEL16_7362(tmp1, tmp1);//gain 3322 tmp3=AE_SEL16_5410(tmp1, tmp1);//gain tmp3=AE_SEL16_7362(tmp3, tmp3);//gain 1100 AE_LA16X4_IP(tmp4,align3, p_out); AE_LA16X4_IP(tmp5,align3, p_out); tmp4=AE_MULFP16X4RAS_vector(tmp4, tmp2); tmp5=AE_MULFP16X4RAS_vector(tmp5, tmp3); AE_SA16X4_IP(tmp4,align4, p_out_s); AE_SA16X4_IP(tmp5,align4, p_out_s); } AE_SA64POS_FP(align2, p_in_s); AE_SA64POS_FP(align4, p_out_s); } #else void MinGain_ApplyGain(short * out, short * in, int len) { int i; short gain; for (i = 0; i < 4; i++) { gain = DVTX_MAX(in[i], 1000); in[i] = gain; out[2 * i] = DVTXOP_mult(out[2 * i], gain); out[2 * i + 1] = DVTXOP_mult(out[2 * i + 1], gain); } for (; i < 16; i++) { gain = DVTX_MAX(in[i], 500); in[i] = gain; out[2 * i] = DVTXOP_mult(out[2 * i], gain); out[2 * i + 1] = DVTXOP_mult(out[2 * i + 1], gain); } for (; i < 32; i++) { gain = DVTX_MAX(in[i], 100); in[i] = gain; out[2 * i] = DVTXOP_mult(out[2 * i], gain); out[2 * i + 1] = DVTXOP_mult(out[2 * i + 1], gain); } for (; i < len; i++) { gain = DVTX_MAX(in[i], 10); in[i] = gain; out[2 * i] = DVTXOP_mult(out[2 * i], gain); out[2 * i + 1] = DVTXOP_mult(out[2 * i + 1], gain); } } #endif #endif extern int fx_SolomonVoice_peak_peaking_based_Harmonic_Contour_estimation(short *f0Contour, short SPPminlocal, short VAD, int *EEii, int *EXooS, short wl, short fh, short NfftHalf) { short k, f0k, Nf0, f0k_lowband_lastbin; short prev_f0bin, crt_f0bin, f0binavg; // initialize f0Contour for (k = 0; k < fh; k++) f0Contour[k] = 10000; for (k = fh; k < NfftHalf; k++) f0Contour[k] = DVTX_MAX_16; //f0kMax = DVTX_MIN(fh * 6, NfftHalf);//5000/31.25 //for (k = 0; k < f0kMax; k++) // f0Contour[k] = 10000; //for (k = f0kMax; k < NfftHalf; k++) // f0Contour[k] = DVTX_MAX_16; // if (VAD == 1) { Nf0 = 0; prev_f0bin = 1; f0binavg = 0; // low band f0 : peak selection by inner mic for (k = wl; k < fh; k++) { f0k = k; f0k = DVTX_MAX(f0k, 2); if (EEii[f0k - 2] < EEii[f0k] && EEii[f0k - 1] < EEii[f0k] && EEii[f0k + 2] < EEii[f0k] && EEii[f0k + 1] < EEii[f0k]) { crt_f0bin = f0k; f0binavg = f0binavg + (crt_f0bin - prev_f0bin); prev_f0bin = crt_f0bin; Nf0++; f0Contour[f0k] = DVTX_MAX_16; f0Contour[f0k - 1] = SPPminlocal; f0Contour[f0k + 1] = SPPminlocal; f0k_lowband_lastbin = f0k; } } } return 0; } extern void fx_SolomonVoice_VectorAdd_ll(int *Out, int *A, int *B, short N) { short i; for (i = 0; i <= N; i++) { Out[i] = DVTXOP_L_add(A[i], B[i]); } } /* 64point cepstrum analysis input : SNRvecotr Q16.15(no block norm since input Q is fixed) output : cepstrum 16bit */ extern void fx_SolomonVoice_64point_cepstrum_transform(int *inSNR, short *outCeps, short Nband) { short i; int Ltmp; for (i = 0; i <= Nband; i++) { Ltmp = DVTXOP_L_sub(DVTXOP_fnLog10(inSNR[i]), DVTX_LOG10FIX);//Q26 outCeps[i] = DVTXOP_extract_h(Ltmp); } for (i = Nband + 1; i < 64; i++) outCeps[i] = 0; Tx_r_fft(outCeps, +1, 64); } // cepstrum to smoothed SNR // Q16.15 #ifdef fx_SolomonVoice_64point_INVERSE_cepstrum_transform_DSP_OPT extern void fx_SolomonVoice_64point_INVERSE_cepstrum_transform(int *outSNR, short *inCeps, short Nband) { short i; int Ltmp; Tx_r_fft(inCeps, -1, 64); // for (i = 0; i <= Nband; i++) // { // Ltmp = DVTXOP_L_add(DVTXOP_L_shl((int)inCeps[i], 16), DVTX_LOG10FIX); // outSNR[i] = DVTXOP_fnExp10(Ltmp); // } //{ // ae_int32x2 tmp_con1=AE_MOVDA32(DVTX_LOG10FIX); // ae_int32x2* p_inCeps = (ae_int32x2*)inCeps; // ae_int32x2 tmp1, tmp2; // // for (i = 0; i <= Nband; i+=4) //Nband=39 // { // //Ltmp = DVTXOP_L_add(DVTXOP_L_shl((int)inCeps[i], 16), DVTX_LOG10FIX); // tmp1=AE_MOVDA32X2(inCeps[i],inCeps[i+1]); // tmp1=AE_SLAI32S(tmp1, 16); // tmp1=AE_ADD32S(tmp1, tmp_con1); // // tmp2=AE_MOVDA32X2(inCeps[i+2],inCeps[i+3]); // tmp2=AE_SLAI32S(tmp2, 16); // tmp2=AE_ADD32S(tmp2, tmp_con1); // //outSNR[i] = DVTXOP_fnExp10(Ltmp); // DVTXOP_fnExp10_four(tmp1,tmp2,outSNR); // } //} { ae_int32x2 tmp_xp, tmp1, tmp2, tmp, tmp3, tmp4; ae_int16x4 tmp_con = AE_MOVDA16X2(3276, 3276); ae_int32x2 tmp_con1 = AE_MOVDA32(DVTX_LOG10FIX);//DVTX_LOG10FIX这个是可以优化的这里先不优化， fang ae_int16x4 tmp_con2 = AE_MOVDA16X2(27213, 27213); static short pswPCoefE[3] = { // c2, c1, c0 0x15ef, 0x556f, 0x7fbd }; ae_f16x4 pswPCoefE0 = AE_MOVDA16X2(pswPCoefE[0], pswPCoefE[0]); ae_f16x4 pswPCoefE1 = AE_MOVDA16X2(pswPCoefE[1], pswPCoefE[1]); ae_f32x2 pswPCoefE2 = AE_MOVDA32(DVTXOP_L_deposit_h(pswPCoefE[2])); //ae_int32x2 tmp_xp,tmp1,tmp2,tmp; ae_f16x4 swTemp1, swTemp2, swTemp3, swTemp4; ae_int16x4 tmp_con3 = AE_MOVDA16X2(0x0020, 0x0020); ae_int16x4 tmp_con4 = AE_MOVDA16X2(-0x0001, -0x0001); ae_int32x2 tmp_con5 = AE_MOVDA32(0xffff); ae_int32x2 tmp_con6 = AE_MOVDA32(DVTXOP_L_deposit_h(tmp_con4)); int bool1; for (i = 0; i <= Nband; i += 4) { // tmp_xp = AE_MOVDA32X2(XX_GSCcpes[2 * i], XX_GSCcpes[2 * (i + 1)]); // tmp1 = AE_MULFP32X16X2RAS_L(tmp_xp, tmp_con); // tmp1 = AE_ADD32S(tmp_xp, tmp1); // tmp1 = AE_ADD32S(tmp1, tmp_con1); tmp_xp=AE_MOVDA32X2(inCeps[i],inCeps[i+1]); tmp1=AE_SLAI32S(tmp_xp, 16); tmp1=AE_ADD32S(tmp1, tmp_con1); //////////////////////////////////DVTXOP_fnExp10 //XX_GSCcpes[i] = DVTXOP_fnExp10(XX_GSCcpes[i]); tmp1 = AE_MULFP32X16X2RAS_L(tmp1, tmp_con2); // tmp1 = AE_MULFP32X16X2RAS_L(tmp_xp1, tmp_con2); tmp2 = AE_SLAI32S(tmp1, 2); // tmp_xp = AE_MOVDA32X2(XX_GSCcpes[2 * (i + 2)], XX_GSCcpes[2 * (i + 3)]); // tmp1 = AE_MULFP32X16X2RAS_L(tmp_xp, tmp_con); //// tmp1 = AE_MULFP32X16X2RAS_L(tmp_xp2, tmp_con); // tmp1 = AE_ADD32S(tmp_xp2, tmp1); // tmp1 = AE_ADD32S(tmp1, tmp_con1); tmp1=AE_MOVDA32X2(inCeps[i+2],inCeps[i+3]); tmp1=AE_SLAI32S(tmp1, 16); tmp1=AE_ADD32S(tmp1, tmp_con1); //////////////////////////////////DVTXOP_fnExp10 //XX_GSCcpes[i] = DVTXOP_fnExp10(XX_GSCcpes[i]); tmp1 = AE_MULFP32X16X2RAS_L(tmp1, tmp_con2); tmp1 = AE_SLAI32S(tmp1, 2); swTemp1 = AE_TRUNC16X4F32(tmp2, tmp1); AE_MULF16X4SS(tmp1, tmp2, swTemp1, tmp_con3);// LwIn int swTemp2 = AE_TRUNC16X4F32(tmp1, tmp2); swTemp4 = AE_SUB16S(tmp_con4, swTemp2); tmp1 = AE_AND32(tmp1, tmp_con5); tmp2 = AE_AND32(tmp2, tmp_con5); tmp1 = AE_ADD32S(tmp1, tmp_con6); tmp2 = AE_ADD32S(tmp2, tmp_con6); tmp1 = AE_SRAI32(tmp1, 1);//好像缺少了一个饱和 tmp2 = AE_SRAI32(tmp2, 1);//好像缺少了一个饱和 swTemp1 = AE_CVT16X4(tmp1, tmp2); swTemp2 = AE_MULFP16X4RAS(swTemp1, swTemp1); AE_MULF16X4SS(tmp1, tmp2, swTemp2, pswPCoefE0); AE_MULAF16X4SS(tmp1, tmp2, swTemp1, pswPCoefE1); tmp1 = AE_ADD32S(tmp1, pswPCoefE2); tmp2 = AE_ADD32S(tmp2, pswPCoefE2); bool1 = AE_MOVAD16_3(swTemp4); outSNR[i]=AE_MOVAD32_H(tmp1); if (bool1 > 0) outSNR[i] = DVTXOP_L_shr(AE_MOVAD32_H(tmp1), bool1); bool1 = AE_MOVAD16_2(swTemp4); outSNR[i + 1]=AE_MOVAD32_L(tmp1); if (bool1 > 0) outSNR[i + 1] = DVTXOP_L_shr(AE_MOVAD32_L(tmp1), bool1); bool1 = AE_MOVAD16_1(swTemp4); outSNR[i + 2]=AE_MOVAD32_H(tmp2); if (bool1 > 0) outSNR[i + 2] = DVTXOP_L_shr(AE_MOVAD32_H(tmp2), bool1); bool1 = AE_MOVAD16_0(swTemp4); outSNR[i + 3]=AE_MOVAD32_L(tmp2); if (bool1 > 0) outSNR[i + 3] = DVTXOP_L_shr(AE_MOVAD32_L(tmp2), bool1); } } } #else extern void fx_SolomonVoice_64point_INVERSE_cepstrum_transform(int *outSNR, short *inCeps, short Nband) { short i; int Ltmp; Tx_r_fft(inCeps, -1, 64); for (i = 0; i <= Nband; i++) { Ltmp = DVTXOP_L_add(DVTXOP_L_shl((int)inCeps[i], 16), DVTX_LOG10FIX); outSNR[i] = DVTXOP_fnExp10(Ltmp); } } #endif /* SolomonVoice_SubBlocks #22: SNR cepstrum smoothing and return to SNR L_SNR_by_Nmmse_ini_cepsmooth : used at 2nd Npsd estimation Q16.15 */ // cepstrum smoothing extern void fx_SolomonVoice_cepstral_smoothing_softForgetting(short *outSNRcepsAvg, short *outSNRcepsAvgTMP, short *inSNRceps, short *forgetVec) { short i, forgetNew, forgetOld; for (i = 0; i < 64; i++) { forgetNew = DVTXOP_sub(DVTX_MAX_16, forgetVec[i]); forgetOld = forgetVec[i]; outSNRcepsAvg[i] = DVTXOP_add(DVTXOP_mult(outSNRcepsAvg[i], forgetOld), DVTXOP_mult(inSNRceps[i], forgetNew)); outSNRcepsAvgTMP[i] = outSNRcepsAvg[i]; } } extern void fx_SolomonVoice_cepstral_smoothing_FixForgetting(short Flag_loud, short *outSNRcepsAvg, short *outSNRcepsAvgTMP, short *inSNRceps, short BetaBand1_slow, short BetaBand2_slow, short BetaBand3_slow, short BetaBand1_fast, short BetaBand2_fast, short BetaBand3_fast) { short i, forgetNew, forgetOld, forgetOld1, forgetOld2; if (Flag_loud == 0) //slow avg. for noise frame or in loud noisy { forgetOld = BetaBand1_slow; forgetOld1 = BetaBand2_slow; forgetOld2 = BetaBand3_slow; } else // fast avg ...for speech frame or in mild noise { forgetOld = BetaBand1_fast; forgetOld1 = BetaBand2_fast; forgetOld2 = BetaBand3_fast; } forgetNew = DVTXOP_sub(DVTX_MAX_16, forgetOld); for (i = 0; i < 3; i++) { outSNRcepsAvg[2 * i] = DVTXOP_add(DVTXOP_mult(outSNRcepsAvg[2 * i], forgetOld), DVTXOP_mult(inSNRceps[2 * i], forgetNew)); outSNRcepsAvg[2 * i + 1] = DVTXOP_add(DVTXOP_mult(outSNRcepsAvg[2 * i + 1], forgetOld), DVTXOP_mult(inSNRceps[2 * i + 1], forgetNew)); outSNRcepsAvgTMP[2 * i] = outSNRcepsAvg[2 * i]; outSNRcepsAvgTMP[2 * i + 1] = outSNRcepsAvg[2 * i + 1]; } forgetNew = DVTXOP_sub(DVTX_MAX_16, forgetOld1); for (i = 3; i < 16; i++) { outSNRcepsAvg[2 * i] = DVTXOP_add(DVTXOP_mult(outSNRcepsAvg[2 * i], forgetOld1), DVTXOP_mult(inSNRceps[2 * i], forgetNew)); outSNRcepsAvg[2 * i + 1] = DVTXOP_add(DVTXOP_mult(outSNRcepsAvg[2 * i + 1], forgetOld1), DVTXOP_mult(inSNRceps[2 * i + 1], forgetNew)); outSNRcepsAvgTMP[2 * i] = outSNRcepsAvg[2 * i]; outSNRcepsAvgTMP[2 * i + 1] = outSNRcepsAvg[2 * i + 1]; } forgetNew = DVTXOP_sub(DVTX_MAX_16, forgetOld2); for (i = 16; i < 32; i++) { outSNRcepsAvg[2 * i] = DVTXOP_add(DVTXOP_mult(outSNRcepsAvg[2 * i], forgetOld2), DVTXOP_mult(inSNRceps[2 * i], forgetNew)); outSNRcepsAvg[2 * i + 1] = DVTXOP_add(DVTXOP_mult(outSNRcepsAvg[2 * i + 1], forgetOld2), DVTXOP_mult(inSNRceps[2 * i + 1], forgetNew)); outSNRcepsAvgTMP[2 * i] = outSNRcepsAvg[2 * i]; outSNRcepsAvgTMP[2 * i + 1] = outSNRcepsAvg[2 * i + 1]; } } extern int HysteresisHangoverThresholding(int a, int hangover_up, int hangover_down, int s, int* p_cnt) { int cnt = *p_cnt; int q = (a == 1); if ((!s) && q) { if (cnt == 0) { cnt = hangover_up; } cnt--; if (cnt <= 0) { s = 1; } } else if (s && (!q)) { if (cnt == 0) { cnt = hangover_down; } cnt--; if (cnt <= 0) { s = 0; } } else { cnt = 0; } *p_cnt = cnt; return s; } short int_rcp_exp(short num) { short r, num2, num3, num4; num2 = DVTXOP_mult(num, num); num3 = DVTXOP_mult(num2, num); num4 = DVTXOP_mult(num3, num); num2 = DVTXOP_shr(num2, 1); num3 = DVTXOP_mult(num3, 5461); num4 = DVTXOP_mult(num4, 1365); r = 32767 - num + num2 - num3 + num4; return r; } // result = 1/(1 + ((1+Xi)*exp(-(Xi/(1+Xi))*zeta))^rg); extern short bayes_prob(int rg, short xi_db, int zeta, short increase_flag) //zeta:[0~8) Q12 { int i; short res, xi_a, xi_c, intg, frac, tmp, nm, qv, rv; xi_c = xi_c_table[xi_db]; //Q15 tmp = DVTXOP_mult(zeta, xi_c); //Q12 tmp = DVTXOP_mult(tmp, 23637); //Q11, 23637=(1.4427 in Q14) intg = tmp >> 11; frac = (tmp & 0x07ff) << 4; //Q15 tmp = DVTXOP_mult(frac, 22713); //Q15, 22713=(0.6931 in Q15) tmp = int_rcp_exp(tmp); //Q(15+intg) xi_a = xi_a_table[xi_db]; //Q9 tmp = DVTXOP_mult(tmp, xi_a); //Q(9+intg) nm = DVTXOP_norm_s(tmp); intg = nm + intg + 9; tmp = tmp << nm; //Q(intg) qv = intg; rv = tmp; for (i = 1; i> 1; //Q(qv) if (qv <= 14) res = DVTXOP_div_s((1 << qv), (1 << qv) + rv); //Q15 else if (qv >= 28) res = 32767; //Q15 else res = DVTXOP_div_s((1 << 14), (1 << 14) + (rv >> (qv - 14))); //Q15 if (increase_flag == 1) res = 24575 + (res >> 2); //Q15, 24575=(0.75 in Q15) return res; } /*Solomonvoice function when Q=7, 30,1 -> 23.8 */ extern void fx_SolomonVoice_PwrBand_FixQ_BlkDeNorm(int *out, int *in, short blk_norm, short Q, short Nband) { short i; for (i = 0; i <= Nband; i++) { out[i] = DVTXOP_L_shl(in[i], (short)(Q - (2 * blk_norm)));/*30,1 -> 23,8*/ } } #ifndef Fx_convolve_OPT_DSP /* int Fx_convolve( const short *signal, const short *filter, const short signal_length, const short filter_length) { int f; short s; int sum; short tmp_signal[2 * DVTX_Nadf_WB]; sum = 0; for (s = 0; s < filter_length; s++) { tmp_signal[s] = signal[s]; tmp_signal[s + filter_length] = signal[s]; } for (f = 0, s = signal_length + filter_length; f < filter_length; f++, s--) { sum = DVTXOP_L_mac(sum, tmp_signal[s], filter[f]); } return sum; }*/ int Fx_convolve( const short *signal, const short *filter, const short signal_length, const short filter_length) { int f; short s; int sum; int sum1,sum2,sum3,sum4; short tmp_signal[2 * DVTX_Nadf_WB]; sum = 0; for (s = 0; s < filter_length; s++) { tmp_signal[s] = signal[s]; tmp_signal[s + filter_length] = signal[s]; } sum1 = sum2 = sum3 = sum4 = 0; for (f = 0, s = signal_length + filter_length; f < filter_length; f += 4, s -= 4) { sum1 = DVTXOP_L_mac(sum1, *(tmp_signal + s),filter[f]); sum2 = DVTXOP_L_mac(sum2, *(tmp_signal + s- 1),filter[f + 1]); sum3 = DVTXOP_L_mac(sum3, *(tmp_signal + s- 2),filter[f + 2]); sum4 = DVTXOP_L_mac(sum4, *(tmp_signal + s- 3),filter[f + 3]); } sum1 = DVTXOP_L_add(sum1, sum3); sum2 = DVTXOP_L_add(sum2, sum4); sum = DVTXOP_L_add(sum1, sum2); return sum; } #else int Fx_convolve_optimized(const short *signal, const short *filter, const short filter_length) //filter_length = 4*(integer) { int i; ae_f16x4 *data1, *data2; ae_f16x4 tmp1, tmp2; ae_f32x2 L_tmp1, L_tmp2; ae_valign align1, align2; data1 = (ae_f16x4 *)signal; data2 = (ae_f16x4 *)filter; L_tmp1 = AE_ZERO32(); L_tmp2 = AE_ZERO32(); align1 = AE_LA64_PP(data1); align2 = AE_LA64_PP(data2); for(i=0; iL_Pwr_tonality_ringband = 0; AEC_buf->L_Pwr_tonality_otherband = 0; #if 1//ndef Power_calculation_shift_OPT_DSP L_DDrawBin = 0; for (i = 0; i < FFT_HALFLEN; i++) { j = 2 * i; enrgD = L_DDBin2[i]; if (i>11 && i<16) { L_DDrawBin = DVTXOP_L_shl(enrgD, (short)(4 - (2 * BLK_NORM))); AEC_buf->L_Pwr_tonality_ringband = DVTXOP_L_add(AEC_buf->L_Pwr_tonality_ringband, L_DDrawBin); } else if (i>21 && i<26) { L_DDrawBin = DVTXOP_L_shl(enrgD, (short)(4 - (2 * BLK_NORM))); AEC_buf->L_Pwr_tonality_otherband = DVTXOP_L_add(AEC_buf->L_Pwr_tonality_otherband, L_DDrawBin); } } #else int L_DDrawBin[FFT_HALFLEN]; Power_calculation_shift(data_buffer_rx,L_DDrawBin, FFT_HALFLEN,(short)(4 - (2 * BLK_NORM))); for (i = 12; i < 16; i++) // 400 to 500 AEC_buf->L_Pwr_tonality_ringband = DVTXOP_L_add(AEC_buf->L_Pwr_tonality_ringband, L_DDrawBin[i]); // other sig pwr for (i = 22; i < 26; i++) // 687 to 812 AEC_buf->L_Pwr_tonality_otherband = DVTXOP_L_add(AEC_buf->L_Pwr_tonality_otherband, L_DDrawBin[i]); #endif return; } extern void Fn_vector_smoothing_over_3_win_length(int *Out, int* A, short N, short LEN) { short i; int Ltmp, Ltmp1, Ltmp2; for (i = 1; i < N - 1; i++) { Ltmp1 = DVTXOP_L_shr(A[i - 1], 2); // 1/4 Ltmp2 = DVTXOP_L_shr(A[i + 1], 2); // 1/4 Ltmp = DVTXOP_L_add(Ltmp1, Ltmp2); Out[i] = DVTXOP_L_add(Ltmp, DVTXOP_L_shr(A[i], 1)); // 1/2 //Ltmp1 = DVTXOP_L_mpy_ls(A[i - 1], 3000); // 1/4 //Ltmp2 = DVTXOP_L_mpy_ls(A[i + 1], 3000); //Ltmp = DVTXOP_L_add(Ltmp1, Ltmp2); //Out[i] = DVTXOP_L_add(Ltmp, DVTXOP_L_mpy_ls(A[i], 26767)); if (Out[i] < 1) Out[i] = 1; } Out[0] = DVTXOP_L_add(DVTXOP_L_shr(A[0], 1), DVTXOP_L_shr(A[1], 1)); Out[N - 1] = DVTXOP_L_add(DVTXOP_L_shr(A[N - 1], 1), DVTXOP_L_shr(A[N - 2], 1)); } #ifdef FN_SOLOMONVOICEW_TX_AGC_2ch_DSP_OPT2 void FN_SOLOMONVOICEW_TX_AGC_2ch_OPT2(short* TXAGCSm_WB,short* In1,short G,short G_old,short maxboost2, int mix_size) /* for (i = 0; i < mix_size; i++) { L_tmp = DVTXOP_L_mult(TXAGCSm_WB[i], G); // Q16.15 * Q16. S_tmp = DVTXOP_sub(0x7fff, TXAGCSm_WB[i]); // Q16.15 L_tmp = DVTXOP_L_mac(L_tmp, S_tmp, G_old); S_tmp = DVTXOP_round(L_tmp); L_tmp = DVTXOP_L_mult(In1[i], S_tmp); S_tmp = DVTXOP_round(L_tmp); // Q16.15 L_tmp = DVTXOP_L_mult(S_tmp, maxboost2); // Q32.27 = Q16.15*Q16.11 L_tmp = DVTXOP_L_shl(L_tmp, 4); // Q32.31 In1[i] = DVTXOP_round(L_tmp); } */ { int i; ae_int16x4 *pt1, *pt2, *pt3; ae_int16x4 Gv, G_oldv, A1, A2, onev, S_tmpv, maxboost2v; ae_int32x2 L_tmp1, L_tmp2; ae_valign align1, align2, align3; pt1=(ae_int16x4 *)TXAGCSm_WB; pt2=(ae_int16x4 *)In1; pt3=(ae_int16x4 *)In1; align1 = AE_LA64_PP(pt1); align2 = AE_LA64_PP(pt2); align3 = AE_ZALIGN64(); Gv = AE_MOVDA16(G); G_oldv = AE_MOVDA16(G_old); onev = AE_MOVDA16(0x7fff); maxboost2v = AE_MOVDA16(maxboost2); for (i = 0; i < mix_size; i+=4) { AE_LA16X4_IP(A1, align1, pt1); //TXAGCSm_WB[i] AE_LA16X4_IP(A2, align2, pt2); //In1[i] AE_MULF16X4SS(L_tmp1, L_tmp2, A1, Gv); S_tmpv = AE_SUB16S(onev, A1); AE_MULAF16X4SS(L_tmp1, L_tmp2, S_tmpv, G_oldv); S_tmpv = AE_ROUND16X4F32SASYM(L_tmp1, L_tmp2); AE_MULF16X4SS(L_tmp1, L_tmp2, A2, S_tmpv); S_tmpv = AE_ROUND16X4F32SASYM(L_tmp1, L_tmp2); AE_MULF16X4SS(L_tmp1, L_tmp2, S_tmpv, maxboost2v); L_tmp1 = AE_SLAA32S(L_tmp1, 4); L_tmp2 = AE_SLAA32S(L_tmp2, 4); S_tmpv = AE_ROUND16X4F32SASYM(L_tmp1, L_tmp2); AE_SA16X4_IP(S_tmpv, align3, pt3); } AE_SA64POS_FP(align3, pt3); } #endif #ifdef FN_SOLOMONVOICEW_TX_AGC_2ch_DSP_OPT3 void FN_SOLOMONVOICEW_TX_AGC_2ch_OPT3(short* In1,short G,short maxboost2, int mix_size) /* for (i = mix_size; i < size; i++) { L_tmp = DVTXOP_L_mult(In1[i], G); S_tmp = DVTXOP_round(L_tmp); L_tmp = DVTXOP_L_mult(S_tmp, maxboost2); // Q32.27 = Q16.15*Q16.11 L_tmp = DVTXOP_L_shl(L_tmp, 4); // Q32.31 In1[i] = DVTXOP_round(L_tmp); // Q16.15 } */ { int i; ae_int16x4 *pt1, *pt2; ae_int16x4 Gv, G_oldv, A1, S_tmpv, maxboost2v; ae_int32x2 L_tmp1, L_tmp2; ae_valign align1, align2; pt1=(ae_int16x4 *)In1; pt2=(ae_int16x4 *)In1; align1 = AE_LA64_PP(pt1); align2 = AE_ZALIGN64(); Gv = AE_MOVDA16(G); maxboost2v = AE_MOVDA16(maxboost2); for (i = 0; i < mix_size; i+=4) { AE_LA16X4_IP(A1, align1, pt1); //In1[i] AE_MULF16X4SS(L_tmp1, L_tmp2, A1, Gv); S_tmpv = AE_ROUND16X4F32SASYM(L_tmp1, L_tmp2); AE_MULF16X4SS(L_tmp1, L_tmp2, S_tmpv, maxboost2v); L_tmp1 = AE_SLAA32S(L_tmp1, 4); L_tmp2 = AE_SLAA32S(L_tmp2, 4); S_tmpv = AE_ROUND16X4F32SASYM(L_tmp1, L_tmp2); AE_SA16X4_IP(S_tmpv, align2, pt2); } AE_SA64POS_FP(align2, pt2); } #endif #ifdef FN_SOLOMONVOICEW_TX_MBDRC_2ch_DSP_OPT1 void FN_SOLOMONVOICEW_TX_MBDRC_2ch_OPT1(short* Xo1,short* MBDRCout_Xo1, short* Xo2, short* MBDRCout_Xo2, short* ch_gain_bin, int fftbin) { /* for (i = 0; i < fftbin; i++) { MBDRCout_Xo1[2 * i] = (short)(DVTXOP_L_shl(DVTXOP_L_mult(Xo1[2 * i], ch_gain_bin[i]), 4) >> 16); MBDRCout_Xo1[2 * i + 1] = (short)(DVTXOP_L_shl(DVTXOP_L_mult(Xo1[2 * i + 1], ch_gain_bin[i]), 4) >> 16); MBDRCout_Xo2[2 * i] = (short)(DVTXOP_L_shl(DVTXOP_L_mult(Xo2[2 * i], ch_gain_bin[i]), 4) >> 16); MBDRCout_Xo2[2 * i + 1] = (short)(DVTXOP_L_shl(DVTXOP_L_mult(Xo2[2 * i + 1], ch_gain_bin[i]), 4) >> 16); } */ ae_valign align1,align2, align3,align4,align5 ; ae_int16x4 *pin1, *pin2, *pout1, *pout2, *pgain; ae_int16x4 A1, A2, A3, A4, gain, g1, g2; ae_int32x2 Ltmp1, Ltmp2; int i; pin1 =(ae_int16x4*)(Xo1); pin2 =(ae_int16x4*)(Xo2); pout1 =(ae_int16x4*)(MBDRCout_Xo1); pout2 =(ae_int16x4*)(MBDRCout_Xo2); pgain = (ae_int16x4*)(ch_gain_bin); align1 = AE_LA64_PP(pin1); align2 = AE_LA64_PP(pin2); align3= AE_ZALIGN64(); align4= AE_ZALIGN64(); align5 = AE_LA64_PP(pgain); for (i = 0; i < fftbin; i+=4) { AE_LA16X4_IP(A1,align1, pin1); // i0,j0, i1, j1 AE_LA16X4_IP(A2,align2, pin2); AE_LA16X4_IP(A3,align1, pin1); // i2,j2, i3, j3 AE_LA16X4_IP(A4,align2, pin2); AE_LA16X4_IP(gain,align5, pgain); g1=AE_SEL16_7362(gain, gain); //gain 0011 g2=AE_SEL16_5410(gain, gain); //gain g2=AE_SEL16_7362(g2, g2); //gain 2233 AE_MULF16X4SS(Ltmp1, Ltmp2, A1, g1); //A i0,j0, i1, j1 ; gain 0011 Ltmp1 = AE_SLAA32S(Ltmp1, 4); Ltmp2 = AE_SLAA32S(Ltmp2, 4); A1 = AE_TRUNC16X4F32(Ltmp1, Ltmp2); AE_SA16X4_IP(A1,align3, pout1); AE_MULF16X4SS(Ltmp1, Ltmp2, A2, g1); //A i0,j0, i1, j1 ; gain 0011 Ltmp1 = AE_SLAA32S(Ltmp1, 4); Ltmp2 = AE_SLAA32S(Ltmp2, 4); A2 = AE_TRUNC16X4F32(Ltmp1, Ltmp2); AE_SA16X4_IP(A2,align4, pout2); AE_MULF16X4SS(Ltmp1, Ltmp2, A3, g2); Ltmp1 = AE_SLAA32S(Ltmp1, 4); Ltmp2 = AE_SLAA32S(Ltmp2, 4); A3 = AE_TRUNC16X4F32(Ltmp1, Ltmp2); AE_SA16X4_IP(A3,align3, pout1); AE_MULF16X4SS(Ltmp1, Ltmp2, A4, g2); Ltmp1 = AE_SLAA32S(Ltmp1, 4); Ltmp2 = AE_SLAA32S(Ltmp2, 4); A4 = AE_TRUNC16X4F32(Ltmp1, Ltmp2); AE_SA16X4_IP(A4,align4, pout2); } } #endif