/*==================================================================================== EVS Codec 3GPP TS26.442 Jun 30, 2015. Version CR 26.442-0010 ====================================================================================*/ #include #include #include #include #include "prot_fx.h" #include "basop_util.h" #include "stat_com.h" #include "stl.h" /* FOR wmc_tool */ void get_maxConv_and_pitch_x(Word16 *s_LP, Word16 s, Word16 e, Word16 N, Word32 *maxConv, Word16 *maxConv_bits, Word16 *pitch); Word16 get_voicing_x(Word16 *s_LP, Word16 pitch, Word32 covMax,Word16 maxConv_bits, Word16 Framesize); Word32 con_Log10(Word32 i_s32Val, Word16 i_s16Q); Word16 vadmin(Word16 a, Word16 b) { return s_min(a, b); } void set_state(Word16 *state, Word16 num, Word16 N) { Word16 i, tmp; tmp = sub(N, 1); FOR (i = 0; i < tmp; i++) { state[i] = state[i+1]; move16(); } state[tmp] = num; move16(); } void concealment_update_x(Word16 bfi, Word16 curr_mode, Word16 tonality, Word32 *invkoef, Word16 *invkoef_scale, void *_plcInfo) { T_PLCInfo *plcInfo = (T_PLCInfo*)_plcInfo; Word32 *data_reci2 = plcInfo->data_reci2_fx; Word16 *tcx_tonality = plcInfo->TCX_Tonality; Word16 FrameSize = plcInfo->FrameSize; Word16 subframe = plcInfo->subframe_fx; Word16 i; move16(); move16(); IF (sub(curr_mode ,1)==0) { set_state(plcInfo->Transient, curr_mode, MAX_POST_LEN); FOR (i = 0; i < FrameSize; i++) { data_reci2[i] = invkoef[i]; move32(); } plcInfo->data_reci2_scale = *invkoef_scale; move16(); IF (!bfi) { set_state(tcx_tonality, tonality, DEC_STATE_LEN); } } ELSE { IF (subframe == 0) { set_state(plcInfo->Transient, curr_mode, MAX_POST_LEN); IF (!bfi) { set_state(tcx_tonality, tonality, DEC_STATE_LEN); } } /* don't store the second subframe during frameloss; in pitch_search_fx(), low_freq_rate is derived on the last good TCX-10 spectrum */ test(); IF (!bfi || subframe == 0) { Word32 *ptr = data_reci2+subframe; Word16 FrameSize2 = shr(FrameSize,1); FOR (i = 0; i < FrameSize2; i++) { ptr[i] = invkoef[i]; move32(); } plcInfo->data_reci2_scale = *invkoef_scale; move16(); } } return; } static Word16 zero_pass_w32_x(Word16 *s, Word16 N) { Word16 i; Word32 temp, zp = L_deposit_l(0); FOR (i = 1; i < N; i++) { temp = L_mac0(-1L, s[i],s[i-1]); zp = L_sub(zp, L_shr(temp,31)); } return extract_l(zp); } Word16 Sqrt_x_fast(Word32 value) { Word16 norm; Word16 result, index; norm = sub(23, norm_l(value)); index = extract_l( L_shr_r(value, add(norm, s_and(norm, 1)))); result = shr(sqrt_table_pitch_search[index], sub(11, shr(add(norm,1),1))); return result; } Word32 dot_w32_accuracy_x(Word16 *s1, Word16 *s2, Word16 nbits, Word16 N) { Word16 i; Word32 eng = L_deposit_l(0), temp; FOR (i = 0; i < N; i++) { temp = L_mult0(s1[i], s2[i]); eng = L_add(eng, L_shr(temp,nbits)); } return eng; } Word16 int_div_s_x(Word16 a, Word16 b) { Word16 result = 0; Word16 norm, left=0, i; move16(); move16(); test(); IF (sub(a,b) < 0 || b == 0) { return 0; } ELSE { a = add(a, shr(b,1)); norm = sub(norm_s(b),norm_s(a)); FOR (i = norm; i>= 0; i--) { left = shr(a, i); result = shl(result,1); IF (sub(left,b) >= 0) { result = add(result,1); left= sub(left, b); a = add(shl(left,i), s_and(a, sub(shl(1,i),1))); } } } return result; } Word16 GetW32Norm_x(Word32 *s, Word16 N) { Word32 smax = L_deposit_l(0); Word16 smax_norm, i; FOR (i = 0; i < N; i++) { smax = L_or(smax, L_abs(s[i])); } smax_norm = norm_l(smax); return smax_norm; } Word16 harmo_x(Word32 *X, Word16 Framesize, Word16 pitch) { Word16 h, k, result = 0; Word32 ener = L_deposit_l(0), ener_harmo = L_deposit_l(0); Word16 norm1, d1, d2; Word16 ener_w, ener_harmo_w; Word16 nbits = sub(15, norm_s(Framesize)); move16(); norm1 = GetW32Norm_x(X, Framesize); FOR (k = 1; k < 9; k++) { h = sub(int_div_s_x(extract_l(L_mult(k, Framesize)), pitch),1); d1 = extract_h(L_shl(X[h], norm1)); d2 = extract_h(L_shl(X[h+1],norm1)); ener_harmo = L_add(ener_harmo, L_shr(L_mac0(L_mult0(d1, d1), d2, d2),nbits)); } FOR (k = 0; k < Framesize; k++) { d1 = extract_h(L_shl(X[k],norm1)); ener = L_add(ener, L_shr(L_mult0(d1, d1),nbits)); } norm1 = norm_l(ener); ener_w = extract_h(L_shl(ener,norm1)); ener_harmo_w = extract_h(L_shl(ener_harmo,norm1)); IF (L_sub(ener_harmo ,ener)>= 0) { return 32767; } test(); IF ((ener_harmo_w <= 0)||(ener_w <= 0)) { return 0; } result = div_s(ener_harmo_w, ener_w); return result; } static Word16 get_low_freq_eng_rate_x(Word32 *mdct_data, Word16 curr_mode, Word16 N) { Word16 N1, N2, i; Word32 low_eng = L_deposit_l(0), eng = L_deposit_l(0), smax = L_deposit_l(0); Word16 nbits, temp, norm = 0; move16(); N1 = 30; N2 = N; move16(); move16(); IF (sub(2 ,curr_mode)==0) { N1 = shr(30,1); N2 = shr(N,1); } nbits = sub(15, norm_s(N2)); FOR (i = 0; i < N2; i++) { smax = L_or(smax, L_abs(mdct_data[i])); } norm = norm_l(smax); FOR (i = 0; i < N1; i++) { temp = extract_h(L_shl(mdct_data[i], norm)); low_eng = L_add(low_eng, L_shr(L_mult0(temp, temp),nbits)); } FOR (i = N1; i < N2; i++) { temp = extract_h(L_shl(mdct_data[i], norm)); eng = L_add(eng, L_shr(L_mult0(temp, temp),nbits)); } eng = L_add(low_eng, eng); /* IF (low_eng<(eng+EPSILON)*0.02) return 1;ELSE return 0; */ /* smax=eng*0.02 */ smax = L_shr(Mpy_32_16_1(eng, 5243), 3); return (L_sub(low_eng,smax) <= 0); } void LpFilter2_x(Word16 *x, Word16 *y, Word16 N) { Word16 i; Word16 smax, norm; Word16 a1 = 5898; /* W16(0.18f); */ Word16 a2 = 20971; /* W16(0.64f); */ move16(); move16(); smax=0; move16(); FOR (i = 0; i < N; i++) { smax = s_or(smax, abs_s(x[i])); } norm = norm_s(smax); y[0] = mult(shl(x[0],norm), a1); move16(); y[1] = add(mult(y[0], a2),mult(shl(x[1],norm), a1)); move16(); FOR (i = 2; i < N; i++) { /* 5898*2+20971=32767 -->no overflow */ y[i] = add(mult(y[i-2],a1), add(mult(y[i-1],a2), mult(shl(x[i],norm),a1))); move16(); } } void sig_tilt_x(Word16 *s, Word16 FrameSize, Word32 *enr1, Word32 *enr2) { Word16 subFrameSize, shIFt; Word16 *p1, *p2; Word16 nbits; subFrameSize = shr(FrameSize, 2); p1 = s+subFrameSize; p2 = s+sub(subFrameSize, 2); shIFt = sub(FrameSize, subFrameSize); nbits = sub(15, norm_s(shIFt)); *enr1 = dot_w32_accuracy_x(p1, p2, nbits, shIFt); move32(); *enr2 = dot_w32_accuracy_x(p1, p1, nbits, shIFt); move32(); } void get_maxConv_and_pitch_x(Word16 *s_LP, Word16 s, Word16 e, Word16 N, Word32 *maxConv, Word16 *maxConv_bits, Word16 *pitch) { Word16 t, cov_size, size = N; Word32 tmp_sigma = L_deposit_l(0), cov_max_sigma = L_deposit_l(0); Word16 nbits,tmp_pitch=0; Word32 r1_high, r2_high; UWord16 r1_low, r2_low; Word32 tmp_sigma_last = L_deposit_l(0); /* not needed, just to avoid compiler warning */ Word16 cov_size_last = N; /* not needed, just to avoid compiler warning */ Word32 cov_max_sigma_tmp = L_deposit_l(0); Word16 size_tmp = N; move16(); move16(); nbits = sub(15, norm_s(sub(N, s))); FOR (t = s; t < e; t++) { cov_size = sub(N,t); tmp_sigma = dot_w32_accuracy_x(s_LP, s_LP+t, nbits, cov_size); IF (sub(t,s) > 0) /* don't use the first value */ { Mpy_32_16_ss(tmp_sigma , cov_size_last, &r1_high, &r1_low); Mpy_32_16_ss(tmp_sigma_last, cov_size , &r2_high, &r2_low); /* tmp_sigma > tmp_sigma_last */ test(); test(); move16(); move16(); /* moves are for the (Word32) casts */ IF((L_sub(r1_high, r2_high) > 0) || (L_sub(r1_high, r2_high) == 0 && L_sub((Word32)r1_low, (Word32)r2_low) > 0)) { /* store the current cov, if it is larger than the last one */ cov_max_sigma_tmp = tmp_sigma; move32(); size_tmp = cov_size; move16(); } ELSE { Mpy_32_16_ss(cov_max_sigma , size_tmp, &r1_high, &r1_low); Mpy_32_16_ss(cov_max_sigma_tmp, size , &r2_high, &r2_low); /* cov_max_sigma < cov_max_sigma_tmp */ test(); test(); move16(); move16(); /* moves are for the (Word32) casts */ IF((L_sub(r1_high, r2_high) < 0) || (L_sub(r1_high, r2_high) == 0 && L_sub((Word32)r1_low, (Word32)r2_low) < 0)) { /* otherwise */ /* use the last value of cov, being a max */ cov_max_sigma = cov_max_sigma_tmp; move32(); size = cov_size; move16(); /* and use the last index as pitch */ tmp_pitch = sub(t,1); } } } tmp_sigma_last = tmp_sigma; move32(); cov_size_last = cov_size; move16(); } *pitch = tmp_pitch; move16(); *maxConv = cov_max_sigma; move32(); *maxConv_bits = nbits; move16(); } Word16 get_voicing_x(Word16 *s_LP, Word16 pitch, Word32 covMax,Word16 maxConv_bits, Word16 Framesize) { Word32 eng1 = L_deposit_l(0), eng2 = L_deposit_l(0); Word16 voicing, norm; Word16 tmpLen, nbits; Word16 eng1_w, eng2_w; IF (covMax <= 0) { return 0; } ELSE { tmpLen = sub(Framesize, pitch); nbits = maxConv_bits; move16(); eng1 = dot_w32_accuracy_x(s_LP, s_LP, nbits, tmpLen); eng2 = dot_w32_accuracy_x(s_LP+pitch, s_LP+pitch, nbits, tmpLen); norm = sub(norm_l(L_or(eng1, L_or(eng2, covMax))),1); eng1 = L_shl(eng1, norm); eng2 = L_shl(eng2, norm); covMax = L_shl(covMax, norm); eng1_w = Sqrt_x_fast(eng1); eng2_w = Sqrt_x_fast(eng2); eng1 = L_mult0(eng1_w, eng2_w); norm = norm_l(eng1); eng1_w = extract_h(L_shl(eng1, norm)); eng2_w = extract_h(L_shl(covMax, norm)); IF (L_sub(covMax , eng1)>=0) { return 32767; } test(); IF ((eng2_w <= 0)||(eng1_w <= 0)) { return 0; } voicing = div_s(eng2_w, eng1_w); return voicing; } } void pitch_modify_x(Word16 *s_LP, Word16 *voicing, Word16 *pitch, Word16 FrameSize) { Word32 eng1, eng2, eng3; Word16 shIFt = shr(*pitch ,1); Word16 tmpLen, nbits, norm, voicing2; Word16 eng1_w, eng2_w; tmpLen = sub(FrameSize, shIFt); nbits = sub(15, norm_s(tmpLen)); eng1 = dot_w32_accuracy_x(s_LP+shIFt, s_LP+shIFt, nbits, tmpLen); eng2 = dot_w32_accuracy_x(s_LP, s_LP, nbits, tmpLen); eng3 = dot_w32_accuracy_x(s_LP+shIFt, s_LP, nbits, tmpLen); IF (eng3 <= 0) { return ; } norm = sub(norm_l(L_or(eng1, L_or(eng2, eng3))),1); eng1 = L_shl(eng1, norm); eng2 = L_shl(eng2, norm); eng3 = L_shl(eng3, norm); eng1_w = Sqrt_x_fast(eng1); eng2_w = Sqrt_x_fast(eng2); eng1 = L_mult0(eng1_w, eng2_w); norm = norm_l(eng1); eng1_w = extract_h(L_shl(eng1, norm)); eng2_w = extract_h(L_shl(eng3, norm)); IF (L_sub(eng3,eng1) >= 0) { voicing2 = 32767; move16(); } ELSE { test(); IF ((eng2_w <= 0)||(eng1_w <= 0)) { voicing2 = 0; move16(); } ELSE { voicing2 = div_s(eng2_w, eng1_w); } } IF (sub(voicing2, *voicing) > 0) { *pitch = shIFt; move16(); *voicing = voicing2; move16(); } } Word16 Is_Periodic_x(Word32 *mdct_data, Word16 cov_max, Word16 zp, Word32 ener, Word32 ener_mean, Word16 pitch, Word16 Framesize) { Word16 flag =0; Word16 harm; move16(); test(); test(); test(); IF (L_sub(ener, L_shl(50,8)) < 0 || (L_sub(ener, L_sub(ener_mean, L_shl(8,8))) < 0 && sub(cov_max, 29491) < 0 )) { flag = 0; move16(); } ELSE IF (sub(cov_max, 26214) > 0) { flag = 1; move16(); } ELSE IF (sub(zp, 100) > 0) { flag = 0; move16(); } ELSE IF (L_sub(ener, L_sub(ener_mean,L_shl(6,8))) < 0) { flag = 0; move16(); } ELSE IF (L_sub(ener, L_add(ener_mean, L_shl(1,8))) > 0 && sub(cov_max, 19661) > 0) { flag = 1; move16(); } ELSE { harm = harmo_x(mdct_data, Framesize, pitch); flag = 1; move16(); if (sub(harm, 22938) < 0) { flag = 0; move16(); } } return flag; } Word16 get_conv_relation_x(Word16 *s_LP, Word16 shIFt, Word16 N) { Word32 eng1,eng2, eng3; Word16 eng1_w, eng2_w; Word16 tmp, norm, nbits; nbits = sub(15, norm_s(N)); eng3 = dot_w32_accuracy_x(s_LP, s_LP+shIFt, nbits, N); IF (eng3 <= 0) { return 0; } eng1 = dot_w32_accuracy_x(s_LP+shIFt, s_LP+shIFt, nbits, N); eng2 = dot_w32_accuracy_x(s_LP, s_LP, nbits, N); norm = sub(norm_l(L_or(eng1, L_or(eng2, eng3))),1); eng1 = L_shl(eng1, norm); eng2 = L_shl(eng2, norm); eng3 = L_shl(eng3, norm); eng1_w = Sqrt_x_fast(eng1); eng2_w = Sqrt_x_fast(eng2); eng1 = L_mult0(eng1_w, eng2_w); norm = norm_l(eng1); eng1_w = extract_h(L_shl(eng1, norm)); eng2_w = extract_h(L_shl(eng3, norm)); IF (L_sub(eng3, eng1) >= 0) { return 32767; } test(); IF ((eng2_w <= 0)||(eng1_w <= 0)) { return 0; } tmp = div_s(eng2_w, eng1_w); return tmp; } static Word16 pitch_search_fx(Word16 *s, /* lastPcmOut */ Word16 *outx_new, Word16 Framesize, Word16 *voicing, Word16 zp, Word32 ener, Word32 ener_mean, Word32 *mdct_data, Word16 curr_mode ) { Word16 pitch = 0; Word32 cov_max = L_deposit_l(0), tilt_enr1, tilt_enr2; Word16 s_LP[L_FRAME_MAX]; Word16 start_pos, end_pos; Word16 low_freq_rate_result; Word16 flag = 0, zp_current; Word32 *mdctPtr; Word16 curr_frmsize; Word16 cov_max_bits=0; Word16 i; move16(); move16(); move16(); *voicing = 0; move16(); curr_frmsize = Framesize; move16(); if (sub(2, curr_mode) == 0) { curr_frmsize = shr(Framesize, 1); } zp_current = zero_pass_w32_x(outx_new, curr_frmsize); if (sub(2, curr_mode) == 0) { zp_current = shl(zp_current,1); } IF (sub(Framesize, 256) <= 0) { IF (sub(zp_current, 70) > 0) { return 0; } } ELSE { IF (sub(zp_current, 105) > 0) { return 0; } } mdctPtr = mdct_data; if (sub(2, curr_mode) == 0) { mdctPtr = mdct_data + shr(Framesize,1); } low_freq_rate_result = get_low_freq_eng_rate_x(mdctPtr, curr_mode, Framesize); IF(low_freq_rate_result) { return 0; } LpFilter2_x(s, s_LP, Framesize); sig_tilt_x(s_LP, Framesize, &tilt_enr1, &tilt_enr2); IF (sub(Framesize, 320) <= 0) { test(); IF ((0==tilt_enr2) || (L_sub(tilt_enr1, L_shr(tilt_enr2, 1)) < 0)) { return 0; } } ELSE { test(); IF ((0==tilt_enr2) || (L_sub(tilt_enr1, Mpy_32_16_1(tilt_enr2, 22938)) < 0)) { return 0; } } IF (sub(Framesize, 320) <= 0) { start_pos = extract_l(L_shr(L_mac0(0x80, 34, Framesize), 8)); end_pos = extract_l(L_shr(L_mac0(0x2 , 3, Framesize), 2)); get_maxConv_and_pitch_x(s_LP, start_pos, end_pos, Framesize, &cov_max, &cov_max_bits, &pitch); *voicing = get_voicing_x(s_LP, pitch, cov_max, cov_max_bits, Framesize); move16(); pitch_modify_x(s_LP, voicing, &pitch, Framesize); } ELSE { Word16 s_tmp[L_FRAME_MAX]; Word16 Framesize_tmp; Word16 pitch_tmp[3]; Word16 cov_size; Framesize_tmp = shr(Framesize, 1); FOR (i = 0; i < Framesize_tmp; i++) { s_tmp[i] = s_LP[2*i]; move16(); } start_pos = extract_l( L_shr(L_mac0(0x80, 34, Framesize_tmp), 8)); end_pos = extract_l( L_shr(L_mac0(0x2, 3, Framesize_tmp), 2)); cov_max = L_deposit_l(0); pitch = 0; move16(); get_maxConv_and_pitch_x(s_tmp, start_pos, end_pos, Framesize_tmp, &cov_max, &cov_max_bits, &pitch); IF (pitch > 0) { pitch_tmp[0] = 0; move16(); if (sub(shl(pitch, 1), 1) > 0) { pitch_tmp[0] = sub(shl(pitch, 1), 1); move16(); } pitch_tmp[1] = shl(pitch, 1); move16(); pitch_tmp[2] = add(shl(pitch, 1), 1); move16(); start_pos = 0; move16(); pitch = 0; move16(); FOR (i = 0; i < 3; i++) { cov_size = sub(Framesize, pitch_tmp[i]); end_pos = get_conv_relation_x(s_LP, pitch_tmp[i], cov_size); IF (sub(end_pos, start_pos) > 0) { start_pos = end_pos; move16(); pitch = pitch_tmp[i]; move16(); } } *voicing = start_pos; move16(); } } IF (pitch > 0) { flag = Is_Periodic_x(mdct_data, *voicing, zp, ener, ener_mean, pitch, Framesize); } if (flag == 0 ) { pitch = 0; move16(); } return pitch; } void concealment_init_x(Word16 N, void *_plcInfo) { T_PLCInfo *plcInfo = (T_PLCInfo*)_plcInfo; Word16 i; plcInfo->FrameSize = N; move16(); plcInfo->Pitch_fx = 0; move16(); plcInfo->T_bfi_fx = 0; move16(); plcInfo->outx_new_n1_fx = 0; move16(); plcInfo->nsapp_gain_fx = 0; move16(); plcInfo->nsapp_gain_n_fx = 0; move16(); plcInfo->ener_mean_fx = L_deposit_l(15213); /*Q8 59.4260f*256*/ plcInfo->ener_fx = L_deposit_l(0); plcInfo->zp_fx = N; move16(); plcInfo->recovery_gain = 0; move16(); plcInfo->step_concealgain_fx = 0; move16(); plcInfo->concealment_method = TCX_NONTONAL; move16(); plcInfo->subframe_fx = 0; move16(); plcInfo->nbLostCmpt = L_deposit_l(0); plcInfo->seed = 21845; move16(); FOR (i = 0; i < TCX_TONALITY_INIT_CNT; i++) { plcInfo->TCX_Tonality[i] = 1; move16(); } FOR (i = TCX_TONALITY_INIT_CNT; i < DEC_STATE_LEN; i++) { plcInfo->TCX_Tonality[i] = 0; move16(); } FOR (i = 0; i < MAX_POST_LEN; i++) { plcInfo->Transient[i] = 1; move16(); } FOR (i = 0; i < L_FRAME_MAX; i++) { plcInfo->data_reci2_fx[i] = L_deposit_l(0); } return; } static Word16 own_random_fix( /* o : output random value */ Word16 *seed /* i/o: random seed */ ) { *seed = extract_l(L_mac0(13849L, *seed , 31821)); return(*seed); } void concealment_decode_fix(Word16 curr_mode, Word32 *invkoef, Word16 *invkoef_scale,void *_plcInfo) { T_PLCInfo *plcInfo = (T_PLCInfo*)_plcInfo; Word16 i; Word16 N = plcInfo->FrameSize; Word16 *seed = &(plcInfo->seed); Word16 sign; move16(); IF (plcInfo->concealment_method == TCX_NONTONAL) /* #define TCX_NONTONAL 0 */ { IF (sub(curr_mode, 1) == 0) { /* copy the data of the last frame */ mvr2r_Word32(plcInfo->data_reci2_fx, invkoef, N); *invkoef_scale = plcInfo->data_reci2_scale; move16(); /* sign randomization */ FOR (i = 0; i < N; i++) { sign = add(shl(shr(own_random_fix(seed),15),1),1); if(sub(sign,-1)==0) { invkoef[i] = L_negate(invkoef[i]); move32(); } } } } return; } Word16 Spl_GetScalingSquare_x(Word16 *in_vector, Word16 in_vector_length, Word16 times) { Word16 nbits = sub(15, norm_s(times))/*Spl_GetSizeInBits_x(times)*/; Word16 i; Word16 smax = -1; Word16 sabs; Word16 *sptr = in_vector; Word16 t; move16(); FOR (i = in_vector_length; i > 0; i--) { sabs = abs_s(*sptr); sptr++; smax = s_max(sabs, smax); } t = norm_l(L_mult0(smax, smax)); IF (smax == 0) { return 0; /* Since norm(0) returns 0 */ } ELSE { nbits = sub(nbits, t); nbits = s_max(0,nbits); return nbits; } } Word32 Spl_Energy_x(Word16* vector, Word16 vector_length, Word16* scale_factor) { Word32 en = L_deposit_l(0); Word32 i; Word16 scaling = Spl_GetScalingSquare_x(vector, vector_length, vector_length); FOR (i = 0; i < vector_length; i++) { en = L_add(en,L_shr(L_mult0(vector[i], vector[i]), scaling)); } move32(); *scale_factor = scaling; return en; } void Log10OfEnergy_x(Word16 *s, Word32 *enerlogval, Word16 len) { Word32 energy = L_deposit_l(0), tmp2 = L_deposit_l(0); Word16 shfts = 0; Word32 Log10_energy = L_deposit_l(0), Log10_len = L_deposit_l(0); move16(); energy = Spl_Energy_x(s, len, &shfts);/* Q:-shfts */ IF (energy > 0) { Log10_energy = con_Log10(energy, negate(shfts)); /* Q25 */ Log10_len = con_Log10(L_deposit_l(len), 0); /* Q25 */ tmp2 = L_sub(Log10_energy,Log10_len); /* Q25 */ tmp2 = Mpy_32_16_1(tmp2,20480); /* Q11->10 Q=25+11-15=21 */ *enerlogval = L_shr(tmp2,13); /* Q8 */ move32(); } ELSE { *enerlogval = -25600; move32(); } } static Word32 fnLog2(Word32 L_Input) { Word16 swC0 = -0x2b2a, swC1 = 0x7fc5, swC2 = -0x54d0; Word16 siShIFtCnt, swInSqrd, swIn; Word32 LwIn; move16(); move16(); move16(); /*_________________________________________________________________________ | | | Executable Code | |_________________________________________________________________________| */ /* normalize input and store shIFts required */ /* ----------------------------------------- */ siShIFtCnt = norm_l(L_Input); LwIn = L_shl(L_Input, siShIFtCnt); siShIFtCnt = add(siShIFtCnt, 1); siShIFtCnt = negate(siShIFtCnt); /* calculate x*x*c0 */ /* ---------------- */ swIn = extract_h(LwIn); swInSqrd = mult_r(swIn, swIn); LwIn = L_mult(swInSqrd, swC0); /* add x*c1 */ /* --------- */ LwIn = L_mac(LwIn, swIn, swC1); /* add c2 */ /* ------ */ LwIn = L_add(LwIn, L_deposit_h(swC2)); /* apply *(4/32) */ /* ------------- */ LwIn = L_shr(LwIn, 3); LwIn = L_and(LwIn, 0x03ffffff); siShIFtCnt = shl(siShIFtCnt, 10); LwIn = L_add(LwIn, L_deposit_h(siShIFtCnt)); /* return log2 */ /* ----------- */ return (LwIn); } static Word32 fnLog10(Word32 L_Input) { Word16 Scale = 9864; /* 0.30103 = log10(2) */ Word32 LwIn; move16(); /*_________________________________________________________________________ | | | Executable Code | |_________________________________________________________________________| */ /* 0.30103*log2(x) */ /* ------------------- */ LwIn = fnLog2(L_Input); LwIn = Mpy_32_16_1(LwIn, Scale); return (LwIn); } Word32 con_Log10(Word32 i_s32Val, Word16 i_s16Q) { Word32 s32Out; Word32 s32Correct; /* corrected (31-q)*log10(2) */ const Word16 s16Log10_2 = 19728; /* log10(2)~Q16 */ move16(); IF(0 == i_s32Val) { return EVS_LW_MIN; } s32Out = fnLog10(i_s32Val); /* (2^26)*log10(a) */ s32Correct = L_mult(sub(31,i_s16Q), s16Log10_2); /* q = 17 */ s32Correct = L_shl(s32Correct, 8); /* q = 25 */ s32Out = L_shr(s32Out, 1); /* q = 25 */ s32Out = L_add(s32Out, s32Correct); return s32Out; } void concealment_update2_x(Word16 *outx_new, void *_plcInfo, Word16 FrameSize) { T_PLCInfo *plcInfo = (T_PLCInfo*)_plcInfo; plcInfo->zp_fx = zero_pass_w32_x(outx_new, FrameSize); move16(); Log10OfEnergy_x(outx_new, &plcInfo->ener_fx, FrameSize); /* Q8 */ test(); IF (sub(plcInfo->zp_fx, 100) < 0 && L_sub(plcInfo->ener_fx, L_shl(50,8)) > 0) { plcInfo->ener_mean_fx = L_add(Mpy_32_16_1(plcInfo->ener_mean_fx ,32112/* 0.98 Q15 */), Mpy_32_16_1(plcInfo->ener_fx , 655/* 0.02 Q15 */)); move32(); } return; } static Word16 array_max_indx_fx(Word16 *s, Word16 N) { Word16 i, indx = 0; move16(); FOR (i = 0; i < N; i++) { if (sub(s[i], s[indx]) > 0) { indx = i; move16(); } } return indx; } Word16 ffr_getSfWord16( /* o: measured headroom in range [0..15], 0 IF all x[i] == 0 */ Word16 *x, /* i: array containing 16-bit data */ Word16 len_x) /* i: length of the array to scan */ { Word16 i, i_min, i_max; Word16 x_min, x_max; x_max = 0; move16(); x_min = 0; move16(); FOR (i = 0; i < len_x; i++) { if (x[i] >= 0) x_max = s_max(x_max,x[i]); if (x[i] < 0) x_min = s_min(x_min,x[i]); } i_max = 0x10; move16(); i_min = 0x10; move16(); if (x_max != 0) i_max = norm_s(x_max); if (x_min != 0) i_min = norm_s(x_min); i = s_and(s_min(i_max, i_min),0xF); return i; } static Word16 OverlapAdd_fx(Word16 *pitch125_data, Word16 *sbuf, Word16 n, Word16 pitch, Word16 Framesize) { Word16 n1,n2,s,s16MaxCoefNorm,s16MaxCoefNorm2,tmp16; Word16 i; Word16 pitch125 =extract_l(L_shr(L_add(L_add(L_deposit_h(pitch), L_mult(pitch, 8192)), 32768) ,16)); Word16 Loverlap = sub(pitch125,pitch); Word16 tmp = sub(Framesize,n); n1 = tmp; move16(); n2 = tmp; move16(); if(sub(Loverlap,tmp)<0) { n1 = Loverlap; move16(); } if(sub(pitch125,tmp)<0) { n2 = pitch125; move16(); } s16MaxCoefNorm = sub(ffr_getSfWord16(sbuf+n, n1),1); s16MaxCoefNorm2 = ffr_getSfWord16(pitch125_data, n1); Loverlap = s_max(1, Loverlap); tmp16 =BASOP_Util_Divide1616_Scale(1, Loverlap,&s); FOR (i = 0; i < n1; i++) { Word16 tmp; Word16 dat; dat= shl(sbuf[n+i],s16MaxCoefNorm); tmp = extract_l(L_shl(L_mult0(i, tmp16), s)); /* q15 */ sbuf[n+i] = round_fx(L_add(L_shr(L_mult(dat, sub(32767,tmp)),s16MaxCoefNorm), L_shr(L_mult(shl(pitch125_data[i],s16MaxCoefNorm2),tmp),s16MaxCoefNorm2))); } FOR (i = n1; i < n2; i++) { sbuf[n+i] = pitch125_data[i]; move16(); } pitch = add(n, pitch); return pitch; } static void add_noise (Word16 * const sbuf, Word16 * const outx_new_n1, Word16 const* const noise_seg, Word16 const Len, Word16 * const gain, Word16 const* const gain_n, Word8 const firstFrame) { Word16 i; Word16 temp_OUT; IF( !firstFrame ) { temp_OUT = sub(noise_seg[0], mult((*outx_new_n1),22282/* 0.68 Q15 */)); sbuf[0] = add(sbuf[0], mult((temp_OUT), *gain)); move16(); *gain = mac_r(L_mult(32439/* 0.99 Q15 */,*gain),328/* 0.01 Q15 */,*gain_n); } FOR( i = 1; i < Len; i++ ) { temp_OUT = sub(noise_seg[i], mult((noise_seg[i-1]),22282/* 0.68 Q15 */)); sbuf[i] = add(sbuf[i], mult((temp_OUT), *gain)); move16(); *gain = mac_r(L_mult(32439/* 0.99 Q15 */,*gain),328/* 0.01 Q15 */,*gain_n); } *outx_new_n1 = noise_seg[i-1]; /*q0*/ return; } static Word16 waveform_adj_fix(Word16 *overlapbuf, Word16 *outdata2, Word16 *outx_new, Word16 *data_noise, Word16 *outx_new_n1, Word16 *nsapp_gain, Word16 *nsapp_gain_n, Word16 Framesize, Word8 T_bfi, Word16 voicing, Word16 curr_mode, Word16 pitch) { Word16 i, zp1, zp2,Framesizediv2,s16MaxCoefNorm; Word16 sbuf[L_FRAME_MAX]; Word16 tmp; Framesizediv2=shr(Framesize,1); zp1 = zero_pass_w32_x(outdata2, Framesizediv2); zp2 = zero_pass_w32_x(outdata2+Framesizediv2, Framesizediv2); /* judge if the pitch is usable */ tmp = 1; move16(); if (sub(zp1, 1) > 0) { tmp = zp1; move16(); } IF (sub(shl(tmp,2), zp2) < 0) { move16(); return pitch = 0; } /* adjust the pitch value */ test(); test(); test(); IF (T_bfi && (sub(pitch , Framesizediv2)<=0) && (sub(Framesize ,256)>0) && (sub(curr_mode , 1)==0)) { Word16 i1 = 0, i2 = 0; Word16 pos1, pos2, pos3; move16(); move16(); i1 = add(1 , array_max_indx_fx(outx_new, pitch)); i2 = add(1 , array_max_indx_fx(outx_new+pitch, pitch)); pos1 = add(i2,sub(pitch,i1)); pos3 = add(pos1, mult(pos1, 8192/* 0.25 Q15 */)); pos2 = add(pitch,mult(pitch, 8192/* 0.25 Q15 */)); test(); test(); IF ((sub(pos1,pos2)<0) && (sub(pos3,pitch)>0) && (sub(pos1,Framesizediv2)<0)) { pitch = add(i2,sub(pitch,i1)); } } { Word16 pitch125 = 0, Loverlap = 0, n = 0, i; Word16 pitch125_data[L_FRAME_MAX]; move16(); move16(); pitch125 = extract_l((L_shr(L_add(L_add(L_deposit_h(pitch), L_mult(pitch, 8192)), 32768) ,16))); Loverlap = sub(pitch125,pitch); FOR (i = 0; i < pitch; i++) { pitch125_data[i] = outdata2[Framesize-pitch+i]; move16(); } FOR (i = 0; i < Loverlap; i++) { pitch125_data[pitch+i] = outx_new[i]; move16(); } FOR (i = 0; i < Framesize; i++) { sbuf[i] = outx_new[i]; move16(); } { Word16 i,pitch125a1; Word16 tmp[2*L_FRAME_MAX], *p_tmp = tmp+1; FOR (i = 0; i < pitch125; i++) { p_tmp[i] = pitch125_data[i]; move16(); } p_tmp[-1] = outdata2[Framesize-pitch-1]; move16(); p_tmp[pitch125] = outx_new[Loverlap]; move16(); pitch125a1 = add(pitch125,1); s16MaxCoefNorm = sub(ffr_getSfWord16(p_tmp-1, pitch125a1),1); FOR (i = 0; i < pitch125a1; i++) { p_tmp[i-1] = shl(p_tmp[i-1],s16MaxCoefNorm); move16(); } FOR (i = 0; i < pitch125; i++) { pitch125_data[i] = round_fx(L_shr(L_add((L_mult(p_tmp[i], 20972)),L_mac(L_mult(p_tmp[i-1], 5898),p_tmp[i+1],5898)),s16MaxCoefNorm)); } } WHILE (sub(n, Framesize) < 0) /* periodical extension */ { n = OverlapAdd_fx(pitch125_data,sbuf,n,pitch,Framesize); } /* maximum pitch lag is 3/4 Framesize; pitch125_data is reused for temporary storage, since outdata2 (holding the pcm data of the last good frame) is still needed and overlapbuf overlaps outdata2 */ Copy(&sbuf[Framesize/4], pitch125_data, (3*Framesize)/4); *nsapp_gain = 0; move16(); *nsapp_gain_n = sub(32767 ,shr(voicing,1)); /* q15 */ tmp = Framesize; move16(); /* use last good signal for noise generation */ add_noise(sbuf, outx_new_n1, outdata2, tmp, nsapp_gain, nsapp_gain_n, 1); /* save current (noisy) output from IMDCT */ mvr2r_Word16(outx_new, data_noise, tmp); /* overlapbuf can now be filled with sbuf, needed for subsequently lost frames */ Copy(pitch125_data, &overlapbuf[Framesize/4], (3*Framesize)/4); } FOR (i = 0; i < Framesize; i++) { outx_new[i] = sbuf[i]; move16(); } return pitch; } void waveform_adj2_fix( Word16 *overlapbuf, Word16 *outx_new, Word16 *data_noise, Word16 *outx_new_n1, Word16 *nsapp_gain, Word16 *nsapp_gain_n, Word16 *recovery_gain, Word16 step_concealgain, Word16 pitch, Word16 Framesize, Word16 delay, Word16 bfi_cnt, Word16 bfi ) { Word16 i, n,tablescale,ratio, dat,Framesizesubn,Framesizesubp,tmp16,s,ptable,temp_OUT,s16MaxCoefNorm,s16MaxCoefNorm2; Word16 sbuf[L_FRAME_MAX]; n=0; move16(); Framesizesubn = sub(Framesize,n); Framesizesubp = sub(Framesize,pitch); IF (pitch > 0) { WHILE (Framesizesubn>0) { /* periodical extension */ Word16 tmp = vadmin(pitch, Framesizesubn); FOR (i = 0; i < tmp; i++) { sbuf[n+i] = overlapbuf[Framesizesubp+i]; move16(); } n = add(n, pitch); Framesizesubn = sub(Framesize,n); } FOR (i = 0; i < Framesize; i++) { overlapbuf[i] = sbuf[i]; move16(); } { Word16 size = Framesize; Word16* noise_ptr = data_noise; /* use last (noisy) output from IMDCT for noise generation */ add_noise(sbuf, outx_new_n1, noise_ptr, size, nsapp_gain, nsapp_gain_n, 0); /* save current (noisy) output from IMDCT */ IF( bfi ) { mvr2r_Word16(outx_new, noise_ptr, size); } } test(); IF (sub(bfi_cnt ,4)==0 || bfi == 0) { SWITCH ( Framesize) { case 160: { tablescale =8; move16(); ptable = 26214; /* (Word16)(32767*256/160.0+0.5); q7+15 */ move16(); BREAK; } case 320: { tablescale =9; move16(); ptable = 26214; /* (Word16)(32767*256/320.0+0.5); q8+15 */ move16(); BREAK; } case 512: { tablescale =10; move16(); ptable = 32767; /* q9+15 */ move16(); BREAK; } case 640: { tablescale =10; move16(); ptable = 26214; /* (Word16)(32767*512/640.0+0.5); q9+15 */ move16(); BREAK; } default: /* 960 */ { tablescale =10; move16(); ptable = 17456; /* (Word16)(32767*512/960.0); q9+15 */ move16(); BREAK; } } IF (bfi == 0) /* overlap-and-add */ { Word16 gain_zero_start = 10000; move16(); IF (step_concealgain > 0) { gain_zero_start = BASOP_Util_Divide1616_Scale(*recovery_gain, step_concealgain,&s); gain_zero_start= shl(gain_zero_start, sub(s,14)); /* q0 */ gain_zero_start= add(gain_zero_start,1); } IF (delay > 0) { Framesize = sub(Framesize,delay); } s16MaxCoefNorm = sub(ffr_getSfWord16(sbuf, Framesize),1); s16MaxCoefNorm2 = ffr_getSfWord16(outx_new, Framesize); tmp16 = vadmin(gain_zero_start, Framesize); FOR (i = 0; i < tmp16; i++) { ratio = extract_l(L_shr(L_mult(i, ptable), tablescale)); dat= shl(sbuf[i],s16MaxCoefNorm); temp_OUT= mult(*recovery_gain, sub(32767,ratio)); outx_new[i]= round_fx(L_add(L_shr(L_mult(temp_OUT,dat ),s16MaxCoefNorm-1), L_shr(L_mult(shl(outx_new[i],s16MaxCoefNorm2),ratio),s16MaxCoefNorm2))); move16(); *recovery_gain =sub(*recovery_gain,shr_r(step_concealgain,1)); /* q14 */ } FOR (i = gain_zero_start; i < Framesize; i++) { ratio = extract_l(L_shr(L_mult(i, ptable), tablescale)); outx_new[i] = round_fx(L_shr(L_mult(shl(outx_new[i],s16MaxCoefNorm2),ratio),s16MaxCoefNorm2)); } if (*recovery_gain < 0) { *recovery_gain = 0; move16(); } } ELSE { /* overlap-and-add */ Word16 tmp; Word16 dat; s16MaxCoefNorm = sub(ffr_getSfWord16(sbuf, Framesize),1); s16MaxCoefNorm2 = ffr_getSfWord16(outx_new, Framesize); FOR (i = 0; i < Framesize; i++) { dat= shl(sbuf[i],s16MaxCoefNorm); tmp = extract_l(L_shr(L_mult(i, ptable), tablescale)); outx_new[i] = round_fx(L_add(L_shr(L_mult(dat, sub(32767,tmp)),s16MaxCoefNorm), L_shr(L_mult(shl(outx_new[i],s16MaxCoefNorm2),tmp),s16MaxCoefNorm2))); } } } ELSE { FOR (i = 0; i < Framesize; i++) { outx_new[i] = sbuf[i]; move16(); } } } return; } void concealment_signal_tuning_fx(Word16 bfi, Word16 curr_mode, Word16 *outx_new_fx, void *_plcInfo, Word16 nbLostCmpt, Word16 pre_bfi, Word16 *OverlapBuf_fx, Word16 past_core_mode, Word16 *outdata2_fx, Decoder_State_fx *st) { T_PLCInfo *plcInfo = (T_PLCInfo*)_plcInfo; Word16 FrameSize = plcInfo->FrameSize; Word16 Pitch = plcInfo->Pitch_fx; Word16 voicing_fx = 0; move16(); move16(); move16(); move16(); IF (bfi) { test(); IF (st->enablePlcWaveadjust && plcInfo->concealment_method == TCX_NONTONAL) /* #define TCX_NONTONAL 0 */ { IF (sub(nbLostCmpt, 1) == 0) { plcInfo->Pitch_fx = pitch_search_fx(outdata2_fx, outx_new_fx, FrameSize, &voicing_fx, plcInfo->zp_fx, (plcInfo->ener_fx), (plcInfo->ener_mean_fx), plcInfo->data_reci2_fx, curr_mode ); move16(); IF (plcInfo->Pitch_fx) /* waveform adjustment for the first lost frame */ { plcInfo->Pitch_fx = waveform_adj_fix(OverlapBuf_fx, outdata2_fx, outx_new_fx, plcInfo->data_noise, &plcInfo->outx_new_n1_fx, &plcInfo->nsapp_gain_fx, &plcInfo->nsapp_gain_n_fx, FrameSize, plcInfo->T_bfi_fx, voicing_fx, curr_mode, plcInfo->Pitch_fx); move16(); } } ELSE IF (sub(nbLostCmpt, 5) < 0) /* waveform adjustment for the 2nd~4th lost frame */ { waveform_adj2_fix(OverlapBuf_fx, outx_new_fx, plcInfo->data_noise, &plcInfo->outx_new_n1_fx, &plcInfo->nsapp_gain_fx, &plcInfo->nsapp_gain_n_fx, &plcInfo->recovery_gain, plcInfo->step_concealgain_fx, Pitch, FrameSize, 0, nbLostCmpt, bfi); } } plcInfo->T_bfi_fx = 1; move16(); } ELSE { test(); test(); test(); IF (pre_bfi && past_core_mode != 0 && L_sub(st->last_total_brate_fx, 48000) >= 0 && sub(st->last_codec_mode, MODE2) == 0) { IF (plcInfo->concealment_method == TCX_NONTONAL) /* #define TCX_NONTONAL 0 */ { IF (L_sub(plcInfo->nbLostCmpt, 4) < 0) /* smoothing of the concealed signal with the good signal */ { waveform_adj2_fix(OverlapBuf_fx, outx_new_fx, plcInfo->data_noise, &plcInfo->outx_new_n1_fx, &plcInfo->nsapp_gain_fx, &plcInfo->nsapp_gain_n_fx, &plcInfo->recovery_gain, plcInfo->step_concealgain_fx, Pitch, FrameSize, 0, add(extract_l(plcInfo->nbLostCmpt), 1), bfi); } } } ELSE { plcInfo->T_bfi_fx = 0; move16(); } } return; }