/******************************************************** SV_common_include.c *********************************************************/ #include #include "SV_common_include.h" #include "SV_basic_op.h" #define SRCB_C_CODE_REARRANGED #ifndef SRCB_C_CODE_REARRANGED //SRCB C code re-arranged /*static*/ short tx_ch_tbl[NUM_CHAN][3] = { { 1, 2, 1 },//63 { 2, 3, 1 },//94 { 3, 4, 1 },//125 { 4, 6, 2 },//188 { 6, 8, 2 },//250 { 8, 11, 3 },//344 { 11, 14, 3 },//438 { 14, 17, 3 },//531 { 17, 21, 4 },//656 { 21, 25, 4 },//781 { 25, 29, 4 },//906 { 29, 33, 4 },//1031 { 33, 38, 5 },//1188 { 38, 43, 5 },//1344 { 43, 48, 5 },//1500 { 48, 53, 5 },//1656 { 53, 58, 5 },//1813 { 58, 64, 6 },//2000 { 64, 70, 6 },//2188 { 70, 76, 6 },//2375 { 76, 82, 6 },//2563 { 82, 88, 6 },//2750 { 88, 94, 6 },//2938 { 94, 100, 6 },//3125 { 100, 106, 6 },//3313 { 106, 113, 7 },//3531 { 113, 120, 7 },//3750 { 120, 127, 7 },//3969 { 127, 135, 8 },//4219 { 135, 143, 8 },//4469 { 143, 151, 8 },//4719 { 151, 161, 10 },//5031 { 161, 171, 10 },//5344 { 171, 183, 12 },//5719 { 183, 195, 12 },//6094 { 195, 207, 12 },//6469 { 207, 219, 12 },//6844 { 219, 231, 12 },//7219 { 231, 243, 12 },//7594 { 243, 256, 13 },//7969 }; #else /*static*/ short tx_ch_tbl[NUM_CHAN][3] = { { 1, 2, 32767 }, { 2, 3, 32767 }, { 3, 4, 32767 }, { 4, 6, 16384 }, { 6, 8, 16384 }, { 8, 11, 10922 }, { 11, 14, 10922 }, { 14, 17, 10922 }, { 17, 21, 8192 }, { 21, 25, 8192 }, { 25, 29, 8192 }, { 29, 33, 8192 }, { 33, 38, 6553 }, { 38, 43, 6553 }, { 43, 48, 6553 }, { 48, 53, 6553 }, { 53, 58, 6553 }, { 58, 64, 5461 }, { 64, 70, 5461 }, { 70, 76, 5461 }, { 76, 82, 5461 }, { 82, 88, 5461 }, { 88, 94, 5461 }, { 94, 100, 5461 }, { 100, 106, 5461 }, { 106, 113, 4681 }, { 113, 120, 4681 }, { 120, 127, 4681 }, { 127, 135, 4096 }, { 135, 143, 4096 }, { 143, 151, 4096 }, { 151, 161, 3276 }, { 161, 171, 3276 }, { 171, 183, 2730 }, { 183, 195, 2730 }, { 195, 207, 2730 }, { 207, 219, 2730 }, { 219, 231, 2730 }, { 231, 243, 2730 }, { 243, 256, 2520 }, }; #endif extern float Log10(float x) { return logf(x)*RECIP_LOGF10; } extern float GetdB(float in) { return 10.0f*Log10(in); } #ifdef USEOPT extern INT64 SV_GetEnergy(int* in, int n); INT64 SV_GetPower(int* in, int n) { INT64 Enrg = SV_GetEnergy(in, n); return Enrg/n; } #else INT64 SV_GetPower(int* in, int n) { int k = n; INT64 acc = 0; do { int d = *in++; acc += (INT64)d*d; } while (--k > 0); acc = (INT64)((float)acc / n); return acc; //return ((float)acc / n); } #endif extern float GetPowerdB(int* in, int n) { return GetdB((float)SV_GetPower(in, n)); } //extern INT64 fn_convolution(int *A, int *B, int n) //{ // int k = n; // INT64 acc = 0; // // do { // int a = *A++; // //int b = *(B+k-1); // int b = *B++; // acc += (INT64)a*b; // } while (--k > 0); // // return acc; //} //extern void fn_filterUpdate(int *W, int in, int *ref, int refPwr, int stepSize, int n) //{ // int k = n; // INT64 adf_var; // // //adf_var = (stepSize / refPwr) * in * ref; // //W = W + adf_var; // adf_var = ((INT64)stepSize * in) / refPwr;//Q.Q_W // // do { // *W++ += adf_var * *(ref++); //Q.Q_W // } while (--k > 0); // // return; //} extern void GetPSD_INT64(INT64* psd, ComplexInt* rbuf, int n) { do { *psd = (INT64)(rbuf->re) * rbuf->re + (INT64)(rbuf->im) * rbuf->im; rbuf++; psd++; } while (--n > 0); return; } extern int GetBlockNorm64(INT64* in, int headroom, int n) { int i, Q; INT64 max64 = 0; for (i = 0; i < n; i++) { if (in[i] > max64) max64 = in[i]; } Q = SV_LL_norm(max64) - headroom - 32; return Q; } extern void Conver64toInt(int *out, INT64* in, int Q, int minVal, int n) { int i; if (Q < 0) { for (i = 0; i < n; i++) { out[i] = (int)(MIN(MAX((in[i] >> (-1 * Q)), minVal), 2147483647)); } } else { for (i = 0; i < n; i++) { out[i] = (int)(MIN(MAX((in[i] << Q), minVal), 2147483647)); } } // return Q; } extern void ConverIntto64(INT64 *out, int* in, int Q, int n) { for (int i = 0; i < n; i++) { out[i] = (Q > 0) ? (INT64)in[i] >> Q : (INT64)in[i] << (-1 * Q); } return; } extern void GetCoh(ComplexInt64* coh, ComplexInt* rbuf1, ComplexInt* rbuf2, int n) { do { coh->re = (INT64)(rbuf1->re) * rbuf2->re + (INT64)(rbuf1->im) * rbuf2->im; coh->im = (INT64)(rbuf1->im) * rbuf2->re - (INT64)(rbuf1->re) * rbuf2->im; rbuf1++; rbuf2++; coh++; } while (--n > 0); return; } extern int HysteresisHangoverThresholding_MultiState(float val, float thr_table[][2], int table_length, int hangover_up, int hangover_down, int state, int *cnt_up, int *cnt_down) { float thr_up, thr_down; if (state < 1) thr_down = 0; else thr_down = thr_table[state - 1][0]; if (state > (table_length - 1)) thr_up = 100000000000000000; else thr_up = thr_table[state][1]; 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 = MIN(MAX(state, 0), table_length); return state; } extern float fn_RecursiveAverage(float newPwr, float prevPwr, float alpha_up, float alpha_down) { float alpha, oneminusalpha, outPwr; if (newPwr < prevPwr) // decreasing pwr { alpha = alpha_down; oneminusalpha = 1 - alpha; } else { alpha = alpha_up; oneminusalpha = 1 - alpha; } outPwr = prevPwr * alpha + newPwr * oneminusalpha; return outPwr; } #ifndef SRCB_C_CODE_REARRANGED //SRCB C code re-arranged extern void fn_bin2band64(INT64 *band, INT64 *bin, short (*ch_bound)[3], int power, int n) { int idx_start, idx_end, denom, k, idx = 0; INT64 enrgE; if (power == 1) { do { idx_start = ch_bound[idx][0]; idx_end = ch_bound[idx][1]; denom = ch_bound[idx][2]; enrgE = 0; for (k = idx_start; k < idx_end; k++) enrgE += bin[k]; *band = (INT64)((float)enrgE / denom); band++; idx++; } while (--n > 0); } else if (power == 2) { do { idx_start = ch_bound[idx][0]; idx_end = ch_bound[idx][1]; denom = ch_bound[idx][2]; enrgE = 0; for (k = idx_start; k < idx_end; k++) enrgE += (INT64)(bin[k] * bin[k]);//abs CO °I *band = (INT64)((float)enrgE / denom); band++; idx++; } while (--n > 0); } return; } extern void fn_bin2band(int *band, int *bin, short(*ch_bound)[3], int power, int n) { int idx_start, idx_end, denom, k, idx = 0; INT64 enrgE; if (power == 1) { do { idx_start = ch_bound[idx][0]; idx_end = ch_bound[idx][1]; denom = ch_bound[idx][2]; enrgE = 0; for (k = idx_start; k < idx_end; k++) enrgE += bin[k]; *band = fx_SV_64bit_Divide(enrgE, denom, 1, 31); band++; idx++; } while (--n > 0); } else if (power == 2) // ?필요함??? 필요하면 고칠 것 { do { idx_start = ch_bound[idx][0]; idx_end = ch_bound[idx][1]; denom = ch_bound[idx][2]; enrgE = 0; for (k = idx_start; k < idx_end; k++) enrgE += (INT64)(bin[k] * bin[k]);//abs CO °I *band = (INT64)((float)enrgE / denom); band++; idx++; } while (--n > 0); } return; } #else extern void fn_bin2band64(INT64 *band, INT64 *bin, short (*ch_bound)[3], int power, int n) { int idx_start, idx_end, band_gain, k, idx = 0; INT64 enrgE; if (power == 1) { do { idx_start = ch_bound[idx][0]; idx_end = ch_bound[idx][1]; band_gain = ch_bound[idx][2]; enrgE = 0; for (k = idx_start; k < idx_end; k++) enrgE += bin[k]; *band = (enrgE*band_gain) >> 15; band++; idx++; } while (--n > 0); } else if (power == 2) { do { idx_start = ch_bound[idx][0]; idx_end = ch_bound[idx][1]; band_gain = ch_bound[idx][2]; enrgE = 0; for (k = idx_start; k < idx_end; k++) enrgE += (INT64)(bin[k] * bin[k]); *band = (enrgE*band_gain) >> 15; band++; idx++; } while (--n > 0); } return; } extern void fn_bin2band(int *band, int *bin, short(*ch_bound)[3], int power, int n) { int idx_start, idx_end, band_gain, k, idx = 0; INT64 enrgE; if (power == 1) { do { idx_start = ch_bound[idx][0]; idx_end = ch_bound[idx][1]; band_gain = ch_bound[idx][2]; enrgE = 0; for (k = idx_start; k < idx_end; k++) enrgE += bin[k]; *band = (enrgE*band_gain) >> 15; band++; idx++; } while (--n > 0); } else if (power == 2) { do { idx_start = ch_bound[idx][0]; idx_end = ch_bound[idx][1]; band_gain = ch_bound[idx][2]; enrgE = 0; for (k = idx_start; k < idx_end; k++) enrgE += (INT64)(bin[k] * bin[k]); *band = (enrgE*band_gain) >> 15; band++; idx++; } while (--n > 0); } return; } #endif extern void fn_spectralavg_Soft3_double(double *out, double *in, int N) { int i; memcpy(out, in, sizeof(double)*N); for (i = 2; i < N - 3; i++) out[i] = in[i - 1] / 4. + in[i] / 2. + in[i + 1] / 4.; return; } extern void fn_spectralavg_Soft3(int *out, int *in, int N) { int i; memcpy(out, in, sizeof(int)*N); for (i = 2; i < N - 3; i++) out[i] = (in[i - 1] >> 2) + (in[i] >> 1) + (in[i + 1] >> 2); return; } extern void fn_SpectralSmoothing_double(double *out, double *in, int smFactor, int Nbin) { int i, j; double sum; double out_sm[300] = { 0, }; for (i = 0; i < smFactor; i++) { sum = 0; for (j = 0; j < 2 * i + 1; j++) sum += in[j]; out_sm[i] = (double)sum / (2 * i + 1); } for (; i < Nbin - smFactor; i++) { sum = 0; for (j = i - smFactor; j < i + smFactor + 1; j++) sum += in[j]; out_sm[i] = (double)sum / (2 * smFactor + 1); } for (; i < Nbin; i++) { sum = 0; for (j = 2 * i - Nbin + 1; j < Nbin; j++) sum += in[j]; out_sm[i] = (double)sum / (2 * Nbin - 2 * i - 1); } memcpy(out, out_sm, sizeof(double)*(Nbin)); return; } extern void fn_SpectralSmoothing64(INT64 *out, INT64 *in, int smFactor, int Nbin) { int i, j; INT64 sum; INT64 out_sm[300] = { 0, }; for (i = 0; i < smFactor; i++) { sum = 0; for (j = 0; j < 2 * i + 1; j++) sum += in[j]; out_sm[i] = (INT64)((float)sum / (2 * i + 1)); } for (; i < Nbin - smFactor; i++) { sum = 0; for (j = i - smFactor; j < i + smFactor + 1; j++) sum += in[j]; out_sm[i] = (INT64)((float)sum / (2 * smFactor + 1)); } for (; i < Nbin; i++) { sum = 0; for (j = 2 * i - Nbin + 1; j < Nbin; j++) sum += in[j]; out_sm[i] = (INT64)((float)sum / (2 * Nbin - 2 * i - 1)); } memcpy(out, out_sm, sizeof(INT64)*(Nbin)); return; } extern void fn_SpectralSmoothing(int *out, int *in, int smFactor, int Nbin) { int i, j; INT64 sum; int out_sm[300] = { 0, }; if (smFactor == 0) { memcpy(out, in, sizeof(int)*Nbin); return; } for (i = 0; i < smFactor; i++) { sum = 0; for (j = 0; j < 2 * i + 1; j++) sum += in[j]; out_sm[i] = (int)((float)sum / (2 * i + 1)); } for (; i < Nbin - smFactor; i++) { sum = 0; for (j = i - smFactor; j < i + smFactor + 1; j++) sum += in[j]; if(smFactor == 1) out_sm[i] = (int)((sum * 715827882) >> 31); else out_sm[i] = (int)((float)sum / (2 * smFactor + 1)); } for (; i < Nbin; i++) { sum = 0; for (j = 2 * i - Nbin + 1; j < Nbin; j++) sum += in[j]; out_sm[i] = (int)((float)sum / (2 * Nbin - 2 * i - 1)); } memcpy(out, out_sm, sizeof(int)*(Nbin)); return; } extern void fn_TimeSmoothing_double(double *new, double *old, double *current, float alpha_old, int n) { do { *new++ = alpha_old * (*old++) + (1 - alpha_old)*(*current++); } while (--n > 0); return; } extern int fn_TimeSmoothing(int *new, int *old, int Q_old, int *current, int Q_current, int alpha_old, int n) { int i, Q_ret, Q_shift, alpha_current; INT64 old64, current64; alpha_current = 2147483647 - alpha_old; if (Q_old < Q_current) { Q_shift = Q_current - Q_old; for (i = 0; i < n; i++) { old64 = (INT64)old[i]; current64 = (INT64)current[i] >> Q_shift; new[i] = (int)((alpha_old * old64 + alpha_current * current64) >> 31); } Q_ret = Q_old; } else { Q_shift = Q_old - Q_current; for (i = 0; i < n; i++) { old64 = (INT64)old[i] >> Q_shift; current64 = (INT64)current[i]; new[i] = (int)((alpha_old * old64 + alpha_current * current64) >> 31); } Q_ret = Q_current; } return Q_ret; } extern void fn_TimeSmoothing64(INT64 *new, INT64 *old, INT64 *current, float alpha_old, int n) { do { *new++ = alpha_old * (*old++) + (1 - alpha_old)*(*current++); } while (--n > 0); return; } extern void fn_ComplexFilter(ComplexInt *out, ComplexFloat *filter, ComplexInt *in, int n) { int i; for (i = 0; i < n; i++) { out[i].re = (int)(filter[i].re * in[i].re + filter[i].im * in[i].im); out[i].im = (int)(filter[i].re * in[i].im - filter[i].im * in[i].re); } return; } extern void fn_ComplexFilter_2ch(ComplexInt *out, float *filter1, float *filter2, ComplexInt *in1, ComplexInt *in2, int n) { int i; for (i = 0; i < n; i++) { out[i].re = filter1[2 * i] * in1[i].re + filter1[2 * i + 1] * in1[i].im + filter2[2 * i] * in2[i].re + filter2[2 * i + 1] * in2[i].im; out[i].im = filter1[2 * i] * in1[i].im - filter1[2 * i + 1] * in1[i].re + filter2[2 * i] * in2[i].im - filter2[2 * i + 1] * in2[i].re; } return; } extern void fn_Get_postSNR_double(double *postSNR, INT64 *EE, INT64 *Npsd, int NpsdBias, double minSNR, double maxSNR) { int i; for (i = 0; i < NUM_CHAN; i++) { postSNR[i] = (double)EE[i] / (Npsd[i] + NpsdBias); postSNR[i] = MIN(MAX(postSNR[i], minSNR), maxSNR); } return; } extern void fn_Get_postSNR64(int *postSNR, INT64 *EE, INT64 *Npsd, int NpsdBias, int minSNR, int maxSNR) { // output : postSNR (Q22, min : 1/512, max : 512) // input : EE (Q0), Npsd (Q0), NpsdBias(Q0) int i; for (i = 0; i < NUM_CHAN; i++) { postSNR[i] = fx_SV_64bit_Divide(EE[i], Npsd[i] + NpsdBias, 1, 9); postSNR[i] = MIN(MAX(postSNR[i], minSNR), maxSNR); } return; } extern void fn_Get_postSNR(int *postSNR, int *EE, int Q_EE, int *Npsd, int Q_Npsd, int NpsdBias, int minSNR, int maxSNR) { // output : postSNR (Q22, min : 1/512, max : 512) // input : EE (Q0), Npsd (Q0), NpsdBias(Q0) int i; int Q_shift; if (Q_EE > Q_Npsd) { Q_shift = Q_EE - Q_Npsd; for (i = 0; i < NUM_CHAN; i++) { postSNR[i] = fx_SV_32bit_Divide((EE[i] >> Q_shift), Npsd[i] + NpsdBias, 1, 9); postSNR[i] = MIN(MAX(postSNR[i], minSNR), maxSNR); } } else { Q_shift = Q_Npsd - Q_EE; for (i = 0; i < NUM_CHAN; i++) { postSNR[i] = fx_SV_32bit_Divide(EE[i], ((Npsd[i] + NpsdBias) >> Q_shift), 1, 9); postSNR[i] = MIN(MAX(postSNR[i], minSNR), maxSNR); } } return; } extern void fn_Get_posterioriSPP_double(double *spp, double *snr, double *xiopt, int rl, int n) { double snr_xiopt; do { if (*snr > 45) *snr = 45; snr_xiopt = -1.0 * (*snr)*(*xiopt / (1 + *xiopt))*rl; if (snr_xiopt < -22) snr_xiopt = -22; *spp++ = (double)1 / (1 + pow((double)(1 + *xiopt), (double)rl) * exp(snr_xiopt)); xiopt++; snr++; } while (--n > 0); return; } extern void fn_Get_posterioriSPP(int *spp/*Q31*/, int *snr/*Q22*/, int *xiopt/*Q25*/, int rl, int n) { INT64 val64;// , val64_2; int xip1, xi_1pxi, snr_xi, exp_val, pow_val, val; do { if (*snr > 188743680) *snr = 188743680; if (*xiopt == 33554432) { xi_1pxi = 1073741824; //Q31 (xiopt/(xiopt + 2^25)) << 31 if (rl == 1) pow_val = 32768;//Q14, ((1 + 10^(0.0)).^(1))*(2^14) else if (rl == 5) pow_val = 524288;//Q14, ((1 + 10^(0.0)).^(5))*(2^14) else pow_val = 1; } else if (*xiopt == 42242527) { xi_1pxi = 1196817618; //Q31 (xiopt/(xiopt + 2^25)) * (2^31) if (rl == 1) pow_val = 37010;//Q14, ((1 + 10^(0.1)).^(rl))*(2^14) else if (rl == 5) pow_val = 963672;//Q14, ((1 + 10^(0.1)).^(rl))*(2^14) else pow_val = 1; } else if (*xiopt == 53180190) { xi_1pxi = 1316701287; //Q31 (xiopt/(xiopt + 2^25)) * (2^31) if (rl == 1) pow_val = 42350;//Q14, ((1 + 10^(0.2)).^(1))*(2^14) else if (rl == 5) pow_val = 1890744;//Q14, ((1 + 10^(0.2)).^(5))*(2^14) else pow_val = 1; } else { xip1 = 33554432 + *xiopt; //Q25 (2 ~ 32.6228) xi_1pxi = fx_SV_32bit_Divide(*xiopt, xip1, 1, 0); //pow_val = pow((double)xip1 / (1 << 25), (double)rl); //Q14 if (rl == 1) pow_val = xip1 >> 11;//Q14 else if (rl == 5) { pow_val = (int)(((INT64)xip1 * xip1) >> (25 + 25 - 14)); pow_val = (int)(((INT64)pow_val * pow_val) >> 14); pow_val = (int)(((INT64)pow_val * xip1) >> 25); } else pow_val = 1; } val64 = (INT64)(*snr) * xi_1pxi * rl; //Q22 + 31 if (val64 > ((INT64)22 << (22 + 31))) val64 = (INT64)22 << (22 + 31); else val64 = val64; snr_xi = (int)((val64) >> (22 + 31 - 26)); //Q26 exp_val = SV_fnExp(-snr_xi); //Q31 val = (int)(16384/*Q14*/ + (((INT64)pow_val/*Q14*/ * exp_val/*Q31*/) >> 31)); //Q14 *spp++ = fx_SV_32bit_Divide(16384/*Q14*/, val, 1, 0); //Q31 xiopt++; snr++; } while (--n > 0); return; } extern void fn_Prevent_stagnation(int *spp, int *spp_smooth, int alpha, int n) { int i; for (i = 0; i < n; i++) //fn_TimeSmoothing64로 대체 spp_smooth[i] = (int)(((INT64)alpha * spp_smooth[i] + (INT64)(2147483647 - alpha)*spp[i])>>31); for (i = 0; i < n; i++) spp[i] = (spp_smooth[i] >= 2126008811) ? 2126008811 : spp[i]; return; } #ifndef SRCB_C_CODE_REARRANGED //SRCB C code re-arranged extern void fn_get_WienerGain(int *gain/*Q31*/, int *prioSNR/*Q22*/, double power, int n) { int i; int Gwiener; for (i = 0; i < n; i++) { //Gwiener = prioSNR[i] / (prioSNR[i] + 1); Gwiener = fx_SV_32bit_Divide(prioSNR[i], prioSNR[i] + 4194304, 1, 0); //Q31 gain[i] = (int)(pow(Gwiener / 2147483647., power) * 2147483647); } return; } #else static short sqrt4_s(short var1) { short i, guess, root, tmp; for (root = 0, i = 14; i >= 0; i--) { guess = root | (1 << i); tmp = (short)(((int)guess*guess) >> 15); tmp = (short)(((int)tmp*tmp) >> 15); if (var1 >= tmp) root = guess; } return root; } extern void fn_get_WienerGain(int *gain/*Q31*/, int *prioSNR/*Q22*/, double power, int n) { short norm, num, denom, Gwiener; int i; for (i = 0; i < n; i++) { norm = SV_L_norm(prioSNR[i] + 4194304)-16; num = (short)SV_L_shl(prioSNR[i], norm); denom = (short)SV_L_shl(prioSNR[i] + 4194304, norm); Gwiener = sqrt4_s(SV_div_s(num, denom)); gain[i] = (int)Gwiener<<16; } return; } #endif extern void fn_get_WienerGain_double(double *gain, double *prioSNR, double power, int n) { int i; double Gwiener; for (i = 0; i < n; i++) { Gwiener = prioSNR[i] / (prioSNR[i] + 1); gain[i] = pow(Gwiener, power); } return; }