Word16 sqrts(Word16 x) { Word16 xb, y, exp, idx, sub_frac, sub_tab; Word32 a0; if(x <= 0){ y = 0; } else{ exp = norm_s(x); /* use 65-entry table */ xb = shl(x, exp); // normalization of x idx = shr(xb, 9); // for 65 entry table a0 = L_deposit_h(tabsqrt[idx]); // Q31 table look-up value sub_frac = shl((Word16)(xb & 0x01FF), 6); // Q15 sub-fraction sub_tab = sub(tabsqrt[idx+1], tabsqrt[idx]); // Q15 table interval for interpolation a0 = L_mac(a0, sub_frac, sub_tab); // Q31 linear interpolation between table entries if(exp & 0x0001){ exp = shr(add(exp, 1), 1); // normalization of sqrt() a0 = L_shr(a0, exp); y = intround(a0); // Q15 a0 = L_mac(a0, 13573, y); // Q31 incorporate the missing "/sqrt(2)" } else{ exp = shr(exp, 1); // normalization of sqrt() a0 = L_shr(a0, exp); // Q31 } y = intround(a0); // Q15 } return y; }
Word16 block_norm (Word16 * in, Word16 * out, Word16 length, Word16 headroom) { Word16 i, max, scnt, adata; max = abs_s(in[0]); for (i = 1; i < length; i++) { adata = abs_s(in[i]); test(); if (sub(adata, max) > 0) { max = adata; move16(); } } test(); if (max != 0) { scnt = sub(norm_s(max), headroom); for (i = 0; i < length; i++) { out[i] = shl(in[i], scnt); move16(); } } else { scnt = sub(16, headroom); for (i = 0; i < length; i++) { out[i] = 0; move16(); } } return (scnt); }
void Get_wegt( Word16 flsp[], /* (i) Q13 : M LSP parameters */ Word16 wegt[] /* (o) Q11->norm : M weighting coefficients */ ) { Word16 i; Word16 tmp; Word32 L_acc; Word16 sft; Word16 buf[M]; /* in Q13 */ buf[0] = sub( flsp[1], (PI04+8192) ); /* 8192:1.0(Q13) */ for ( i = 1 ; i < M-1 ; i++ ) { tmp = sub( flsp[i+1], flsp[i-1] ); buf[i] = sub( tmp, 8192 ); } buf[M-1] = sub( (PI92-8192), flsp[M-2] ); /* */ for ( i = 0 ; i < M ; i++ ) { if ( buf[i] > 0 ){ wegt[i] = 2048; /* 2048:1.0(Q11) */ } else { L_acc = L_mult( buf[i], buf[i] ); /* L_acc in Q27 */ tmp = extract_h( L_shl( L_acc, 2 ) ); /* tmp in Q13 */ L_acc = L_mult( tmp, CONST10 ); /* L_acc in Q25 */ tmp = extract_h( L_shl( L_acc, 2 ) ); /* tmp in Q11 */ wegt[i] = add( tmp, 2048 ); /* wegt in Q11 */ } } /* */ L_acc = L_mult( wegt[4], CONST12 ); /* L_acc in Q26 */ wegt[4] = extract_h( L_shl( L_acc, 1 ) ); /* wegt in Q11 */ L_acc = L_mult( wegt[5], CONST12 ); /* L_acc in Q26 */ wegt[5] = extract_h( L_shl( L_acc, 1 ) ); /* wegt in Q11 */ /* wegt: Q11 -> normalized */ tmp = 0; for ( i = 0; i < M; i++ ) { if ( sub(wegt[i], tmp) > 0 ) { tmp = wegt[i]; } } sft = norm_s(tmp); for ( i = 0; i < M; i++ ) { wegt[i] = shl(wegt[i], sft); /* wegt in Q(11+sft) */ } return; }
void sqrt_i(Word16 x_man, Word16 x_exp, Word16 *y_man, Word16 *y_exp) { Word16 x_manb, x_expb, y, exp, idx, sub_frac, sub_tab; Word32 a0; if(x_man <= 0){ *y_man = 0; *y_exp = 0; } else{ exp = norm_s(x_man); x_manb = shl(x_man, exp); // normalize to Q15 in [0; 1] for table look-up x_expb = add(x_exp, exp); // update exponent for x (left-shifting by additionl exp) x_expb = sub(x_expb, 15); // we need to take sqrt of 0-32767 number but table is for 0-1 idx = shr(x_manb, 9); // for 65 entry table a0 = L_deposit_h(tabsqrt[idx]); // Q31 table look-up value sub_frac = shl((Word16)(x_manb & 0x01FF), 6);// Q15 sub-fraction sub_tab = sub(tabsqrt[idx+1], tabsqrt[idx]); // Q15 table interval for interpolation a0 = L_mac(a0, sub_frac, sub_tab); // Q31 linear interpolation between table entries exp = norm_l(a0); // exponent of a0 y = intround(L_shl(a0,exp)); // normalize sqrt-root and drop 16 LBSs exp = add(15, exp); // exponent of a0 taking Q15 of y into account if(x_expb & 0x0001){ if(y < 0x5A82){ // 1*sqrt(2) in Q14) - with div_s(y1, y2), y2 must be >= y1 exp = add(exp, shr(add(x_expb,1), 1)); // normalization for sqrt() *y_man = div_s(0x2D41, y); // 0x2D41 is 1/sqrt(2) in Q14 } else{ exp = add(exp, shr(sub(x_expb,1), 1)); // normalization for sqrt() *y_man = div_s(0x5A82, y); // 0x5A82 is 1*sqrt(2) in Q14 } *y_exp = sub(29, exp); // ...and div_s returns fraction divide in Q15 } else{ exp = add(exp, shr(x_expb, 1)); // normalization for sqrt() *y_man = div_s(0x4000, y); *y_exp = sub(29, exp); // 0x4000 is 1 in Q14 and div_s returns fraction divide in Q15 } } return; }
Word16 voice_factor( /* (o) Q15 : factor (-1=unvoiced to 1=voiced) */ Word16 exc[], /* (i) Q_exc : pitch excitation */ Word16 Q_exc, /* (i) : exc format */ Word16 gain_pit, /* (i) Q14 : gain of pitch */ Word16 code[], /* (i) Q9 : Fixed codebook excitation */ Word16 gain_code, /* (i) Q0 : gain of code */ Word16 L_subfr /* (i) : subframe length */ ) { Word16 tmp, exp, ener1, exp1, ener2, exp2; Word32 i, L_tmp; #ifdef ASM_OPT /* asm optimization branch */ ener1 = extract_h(Dot_product12_asm(exc, exc, L_subfr, &exp1)); #else ener1 = extract_h(Dot_product12(exc, exc, L_subfr, &exp1)); #endif exp1 = exp1 - (Q_exc + Q_exc); L_tmp = vo_L_mult(gain_pit, gain_pit); exp = norm_l(L_tmp); tmp = extract_h(L_tmp << exp); ener1 = vo_mult(ener1, tmp); exp1 = exp1 - exp - 10; /* 10 -> gain_pit Q14 to Q9 */ #ifdef ASM_OPT /* asm optimization branch */ ener2 = extract_h(Dot_product12_asm(code, code, L_subfr, &exp2)); #else ener2 = extract_h(Dot_product12(code, code, L_subfr, &exp2)); #endif exp = norm_s(gain_code); tmp = gain_code << exp; tmp = vo_mult(tmp, tmp); ener2 = vo_mult(ener2, tmp); exp2 = exp2 - (exp + exp); i = exp1 - exp2; if (i >= 0) { ener1 = ener1 >> 1; ener2 = ener2 >> (i + 1); } else
//============================================================================ //函数功能:找到语音因子 //函数参数:"exc[]"表示因子,作为输入参数;"Q_exc"表示激励,表示输入参数;"gain_pit" // 表示增益音高,作为输入参数;"code[]"表示固定码本激励,作为输入参数; // "gain_code"表示编码增益,作为输入参数;"L_subfr"表示子帧长度 //============================================================================ int16 voice_factor( /* (o) Q15 : factor (-1=unvoiced to 1=voiced) */ int16 exc[], /* (i) Q_exc : pitch excitation */ int16 Q_exc, /* (i) : exc format */ int16 gain_pit, /* (i) Q14 : gain of pitch */ int16 code[], /* (i) Q9 : Fixed codebook excitation */ int16 gain_code, /* (i) Q0 : gain of code */ int16 L_subfr /* (i) : subframe length */ ) { int16 i, tmp, exp, ener1, exp1, ener2, exp2; int32 L_tmp; ener1 = extract_h(Dot_product12(exc, exc, L_subfr, &exp1)); exp1 = sub_int16(exp1, Q_exc << 1); L_tmp = mul_16by16_to_int32(gain_pit, gain_pit); exp = normalize_amr_wb(L_tmp); tmp = (int16)((L_tmp << exp) >> 16); ener1 = mult_int16(ener1, tmp); exp1 -= (exp + 10); /* 10 -> gain_pit Q14 to Q9 */ ener2 = extract_h(Dot_product12(code, code, L_subfr, &exp2)); exp = norm_s(gain_code); tmp = shl_int16(gain_code, exp); tmp = mult_int16(tmp, tmp); ener2 = mult_int16(ener2, tmp); exp2 -= (exp << 1); i = exp1 - exp2; if (i >= 0) { ener1 >>= 1; ener2 >>= (i + 1); }
/* ************************************************************************** * * Function : Az_lsp * Purpose : Compute the LSPs from the LP coefficients * ************************************************************************** */ void Az_lsp ( Word16 a[], /* (i) : predictor coefficients (MP1) */ Word16 lsp[], /* (o) : line spectral pairs (M) */ Word16 old_lsp[] /* (i) : old lsp[] (in case not found 10 roots) (M) */ ) { Word16 i, j, nf, ip; Word16 xlow, ylow, xhigh, yhigh, xmid, ymid, xint; Word16 x, y, sign, exp; Word16 *coef; Word16 f1[M / 2 + 1], f2[M / 2 + 1]; Word32 t0; /*-------------------------------------------------------------* * find the sum and diff. pol. F1(z) and F2(z) * * F1(z) <--- F1(z)/(1+z**-1) & F2(z) <--- F2(z)/(1-z**-1) * * * * f1[0] = 1.0; * * f2[0] = 1.0; * * * * for (i = 0; i< NC; i++) * * { * * f1[i+1] = a[i+1] + a[M-i] - f1[i] ; * * f2[i+1] = a[i+1] - a[M-i] + f2[i] ; * * } * *-------------------------------------------------------------*/ f1[0] = 1024; move16 (); /* f1[0] = 1.0 */ f2[0] = 1024; move16 (); /* f2[0] = 1.0 */ for (i = 0; i < NC; i++) { t0 = L_mult (a[i + 1], 8192); /* x = (a[i+1] + a[M-i]) >> 2 */ t0 = L_mac (t0, a[M - i], 8192); x = extract_h (t0); /* f1[i+1] = a[i+1] + a[M-i] - f1[i] */ f1[i + 1] = sub (x, f1[i]);move16 (); t0 = L_mult (a[i + 1], 8192); /* x = (a[i+1] - a[M-i]) >> 2 */ t0 = L_msu (t0, a[M - i], 8192); x = extract_h (t0); /* f2[i+1] = a[i+1] - a[M-i] + f2[i] */ f2[i + 1] = add (x, f2[i]);move16 (); } /*-------------------------------------------------------------* * find the LSPs using the Chebychev pol. evaluation * *-------------------------------------------------------------*/ nf = 0; move16 (); /* number of found frequencies */ ip = 0; move16 (); /* indicator for f1 or f2 */ coef = f1; move16 (); xlow = grid[0]; move16 (); ylow = Chebps (xlow, coef, NC);move16 (); j = 0; test (); test (); /* while ( (nf < M) && (j < grid_points) ) */ while ((sub (nf, M) < 0) && (sub (j, grid_points) < 0)) { j++; xhigh = xlow; move16 (); yhigh = ylow; move16 (); xlow = grid[j]; move16 (); ylow = Chebps (xlow, coef, NC); move16 (); test (); if (L_mult (ylow, yhigh) <= (Word32) 0L) { /* divide 4 times the interval */ for (i = 0; i < 4; i++) { /* xmid = (xlow + xhigh)/2 */ xmid = add (shr (xlow, 1), shr (xhigh, 1)); ymid = Chebps (xmid, coef, NC); move16 (); test (); if (L_mult (ylow, ymid) <= (Word32) 0L) { yhigh = ymid; move16 (); xhigh = xmid; move16 (); } else { ylow = ymid; move16 (); xlow = xmid; move16 (); } } /*-------------------------------------------------------------* * Linear interpolation * * xint = xlow - ylow*(xhigh-xlow)/(yhigh-ylow); * *-------------------------------------------------------------*/ x = sub (xhigh, xlow); y = sub (yhigh, ylow); test (); if (y == 0) { xint = xlow; move16 (); } else { sign = y; move16 (); y = abs_s (y); exp = norm_s (y); y = shl (y, exp); y = div_s ((Word16) 16383, y); t0 = L_mult (x, y); t0 = L_shr (t0, sub (20, exp)); y = extract_l (t0); /* y= (xhigh-xlow)/(yhigh-ylow) */ test (); if (sign < 0) y = negate (y); t0 = L_mult (ylow, y); t0 = L_shr (t0, 11); xint = sub (xlow, extract_l (t0)); /* xint = xlow - ylow*y */ } lsp[nf] = xint; move16 (); xlow = xint; move16 (); nf++; test (); if (ip == 0) { ip = 1; move16 (); coef = f2; move16 (); } else { ip = 0; move16 (); coef = f1; move16 (); } ylow = Chebps (xlow, coef, NC); move16 (); } test (); test (); } /* Check if M roots found */ test (); if (sub (nf, M) < 0) { for (i = 0; i < M; i++) { lsp[i] = old_lsp[i]; move16 (); } } return; }
/*---------------------------------------------------------------------------- * pst_ltp - harmonic postfilter *---------------------------------------------------------------------------- */ static void pst_ltp( Word16 t0, /* input : pitch delay given by coder */ Word16 *ptr_sig_in, /* input : postfilter input filter (residu2) */ Word16 *ptr_sig_pst0, /* output: harmonic postfilter output */ Word16 *vo /* output: voicing decision 0 = uv, > 0 delay */ ) { /**** Declare variables */ int i; Word16 temp; Word16 ltpdel, phase; Word16 num_gltp, den_gltp; Word16 num2_gltp, den2_gltp; Word16 sh_num, sh_den; Word16 sh_num2, sh_den2; Word16 gain_plt; Word16 y_up[SIZ_Y_UP]; Word16 *ptr_y_up; Word16 off_yup; Word16 *ptr_sig; Word16 sig_cadr[SIZ_RES2], *ptr_sig_cadr; Word16 nb_sh_sig; Word32 L_temp; /* input signal justified on 13 bits */ ptr_sig = ptr_sig_in - MEM_RES2; temp = 0; for(i=0; i<SIZ_RES2; i++) { temp |= abs_s(ptr_sig[i]); } nb_sh_sig = sub(3, norm_s(temp)); for(i=0; i<SIZ_RES2; i++) { /* nb_sh_sig may be >0, <0 or =0 */ sig_cadr[i] = shr( ptr_sig[i], nb_sh_sig); } ptr_sig_cadr = sig_cadr + MEM_RES2; /* Sub optimal delay search */ search_del(t0, ptr_sig_cadr, <pdel, &phase, &num_gltp, &den_gltp, &sh_num, &sh_den, y_up, &off_yup); *vo = ltpdel; if(num_gltp == 0) { Copy(ptr_sig_in, ptr_sig_pst0, L_SUBFR); } else { if(phase == 0) { ptr_y_up = ptr_sig_in - ltpdel; } else { /* Filtering with long filter */ compute_ltp_l(ptr_sig_cadr, ltpdel, phase, ptr_sig_pst0, &num2_gltp, &den2_gltp, &sh_num2, &sh_den2); if(select_ltp(num_gltp, den_gltp, sh_num, sh_den, num2_gltp, den2_gltp, sh_num2, sh_den2) == 1) { /* select short filter */ temp = sub(phase, 1); L_temp = L_mult(temp, L_SUBFRP1); L_temp = L_shr(L_temp, 1); temp = extract_l(L_temp); temp = add(temp, off_yup); /* ptr_y_up = y_up + (phase-1) * L_SUBFRP1 + off_yup */ ptr_y_up = y_up + temp; } else { /* select long filter */ num_gltp = num2_gltp; den_gltp = den2_gltp; sh_num = sh_num2; sh_den = sh_den2; ptr_y_up = ptr_sig_pst0; } /* rescale y_up */ for(i=0; i<L_SUBFR; i++) { /* nb_sh_sig may be >0, <0 or =0 */ ptr_y_up[i] = shl(ptr_y_up[i], nb_sh_sig); } } temp = sub(sh_num,sh_den); if(temp >= 0) den_gltp = shr(den_gltp, temp); else { num_gltp = shl(num_gltp, temp); /* >> (-temp) */ } if(sub(num_gltp ,den_gltp)>=0) { /* beta bounded to 1 */ gain_plt = MIN_GPLT; } else { /* GAMMA_G = 0.5 */ /* gain_plt = den_gltp x 2**15 / (den_gltp + 0.5 num_gltp) */ /* shift 1 bit to avoid overflows in add */ num_gltp = shr(num_gltp, 2); den_gltp = shr(den_gltp, 1); temp = add(den_gltp, num_gltp); gain_plt = div_s(den_gltp, temp); /* Q15 */ } /** filtering by H0(z) = harmonic filter **/ filt_plt(ptr_sig_in, ptr_y_up, ptr_sig_pst0, gain_plt); } return; }
/* ** ** Function: Calc_Exc_Rand() ** ** Description: Computation of random excitation for inactive frames: ** Adaptive codebook entry selected randomly ** Higher rate innovation pattern selected randomly ** Computes innovation gain to match curGain ** ** Links to text: ** ** Arguments: ** ** Word16 curGain current average gain to match ** Word16 *PrevExc previous/current excitation (updated) ** Word16 *DataEXc current frame excitation ** Word16 *nRandom random generator status (input/output) ** ** Outputs: ** ** Word16 *PrevExc ** Word16 *DataExc ** Word16 *nRandom ** ** Return value: None ** */ void Calc_Exc_Rand(Word16 curGain, Word16 *PrevExc, Word16 *DataExc, Word16 *nRandom, LINEDEF *Line) { int i, i_subfr, iblk; Word16 temp, temp2; Word16 j; Word16 TabPos[2*NbPulsBlk], TabSign[2*NbPulsBlk]; Word16 *ptr_TabPos, *ptr_TabSign; Word16 *ptr1, *curExc; Word16 sh1, x1, x2, inter_exc, delta, b0; Word32 L_acc, L_c, L_temp; Word16 tmp[SubFrLen/Sgrid]; Word16 offset[SubFrames]; Word16 tempExc[SubFrLenD]; /* * generate LTP codes */ Line->Olp[0] = random_number(21, nRandom) + (Word16)123; Line->Olp[1] = random_number(21, nRandom) + (Word16)123; for(i_subfr=0; i_subfr<SubFrames; i_subfr++) { /* in [1, NbFilt] */ Line->Sfs[i_subfr].AcGn = random_number(NbFilt, nRandom) + (Word16)1; } Line->Sfs[0].AcLg = 1; Line->Sfs[1].AcLg = 0; Line->Sfs[2].AcLg = 1; Line->Sfs[3].AcLg = 3; /* * Random innovation : * Selection of the grids, signs and pulse positions */ /* Signs and Grids */ ptr_TabSign = TabSign; ptr1 = offset; for(iblk=0; iblk<SubFrames/2; iblk++) { temp = random_number((1 << (NbPulsBlk+2)), nRandom); *ptr1++ = temp & (Word16)0x0001; temp = shr(temp, 1); *ptr1++ = add( (Word16) SubFrLen, (Word16) (temp & 0x0001) ); for(i=0; i<NbPulsBlk; i++) { *ptr_TabSign++= shl(sub((temp & (Word16)0x0002), 1), 14); temp = shr(temp, 1); } } /* Positions */ ptr_TabPos = TabPos; for(i_subfr=0; i_subfr<SubFrames; i_subfr++) { for(i=0; i<(SubFrLen/Sgrid); i++) tmp[i] = (Word16)i; temp = (SubFrLen/Sgrid); for(i=0; i<Nb_puls[i_subfr]; i++) { j = random_number(temp, nRandom); *ptr_TabPos++ = add(shl(tmp[(int)j],1), offset[i_subfr]); temp = sub(temp, 1); tmp[(int)j] = tmp[(int)temp]; } } /* * Compute fixed codebook gains */ ptr_TabPos = TabPos; ptr_TabSign = TabSign; curExc = DataExc; i_subfr = 0; for(iblk=0; iblk<SubFrames/2; iblk++) { /* decode LTP only */ Decod_Acbk(curExc, &PrevExc[0], Line->Olp[iblk], Line->Sfs[i_subfr].AcLg, Line->Sfs[i_subfr].AcGn); Decod_Acbk(&curExc[SubFrLen], &PrevExc[SubFrLen], Line->Olp[iblk], Line->Sfs[i_subfr+1].AcLg, Line->Sfs[i_subfr+1].AcGn); temp2 = 0; for(i=0; i<SubFrLenD; i++) { temp = abs_s(curExc[i]); if(temp > temp2) temp2 = temp; } if(temp2 == 0) sh1 = 0; else { sh1 = sub(4,norm_s(temp2)); /* 4 bits of margin */ if(sh1 < -2) sh1 = -2; } L_temp = 0L; for(i=0; i<SubFrLenD; i++) { temp = shr(curExc[i], sh1); /* left if sh1 < 0 */ L_temp = L_mac(L_temp, temp, temp); tempExc[i] = temp; } /* ener_ltp x 2**(-2sh1+1) */ L_acc = 0L; for(i=0; i<NbPulsBlk; i++) { L_acc = L_mac(L_acc, tempExc[(int)ptr_TabPos[i]], ptr_TabSign[i]); } inter_exc = extract_h(L_shl(L_acc, 1)); /* inter_exc x 2-sh1 */ /* compute SubFrLenD x curGain**2 x 2**(-2sh1+1) */ /* curGain input = 2**5 curGain */ // L_acc = L_mult(curGain, SubFrLen); L_MULT(curGain, SubFrLen, L_acc); L_acc = L_shr(L_acc, 6); temp = extract_l(L_acc); /* SubFrLen x curGain : avoids overflows */ // L_acc = L_mult(temp, curGain); L_MULT(temp, curGain, L_acc); temp = shl(sh1, 1); temp = add(temp, 4); L_acc = L_shr(L_acc, temp); /* SubFrLenD x curGain**2 x 2**(-2sh1+1) */ /* c = (ener_ltp - SubFrLenD x curGain**2)/nb_pulses_blk */ /* compute L_c = c >> 2sh1-1 */ L_acc = L_sub(L_temp, L_acc); /* x 1/nb_pulses_blk */ L_c = L_mls(L_acc, InvNbPulsBlk); /* * Solve EQ(X) = X**2 + 2 b0 X + c */ /* delta = b0 x b0 - c */ b0 = mult_r(inter_exc, InvNbPulsBlk); /* b0 >> sh1 */ L_acc = L_msu(L_c, b0, b0); /* (c - b0**2) >> 2sh1-1 */ L_acc = L_negate(L_acc); /* delta x 2**(-2sh1+1) */ /* Case delta <= 0 */ if(L_acc <= 0) { /* delta <= 0 */ x1 = negate(b0); /* sh1 */ } /* Case delta > 0 */ else { delta = Sqrt_lbc(L_acc); /* >> sh1 */ x1 = sub(delta, b0); /* x1 >> sh1 */ x2 = add(b0, delta); /* (-x2) >> sh1 */ if(abs_s(x2) < abs_s(x1)) { x1 = negate(x2); } } /* Update DataExc */ sh1 = add(sh1, 1); temp = shl(x1, sh1); if(temp > (2*Gexc_Max)) temp = (2*Gexc_Max); if(temp < -(2*Gexc_Max)) temp = -(2*Gexc_Max); for(i=0; i<NbPulsBlk; i++) { j = *ptr_TabPos++; curExc[(int)j] = add(curExc[(int)j], mult(temp, (*ptr_TabSign++)) ); } /* update PrevExc */ ptr1 = PrevExc; for(i=SubFrLenD; i<PitchMax; i++) *ptr1++ = PrevExc[i]; for(i=0; i<SubFrLenD; i++) *ptr1++ = curExc[i]; curExc += SubFrLenD; i_subfr += 2; } /* end of loop on LTP blocks */ return; }
Word16 coarsepitch( Word16 *xw, /* (i) (normalized) weighted signal */ struct BV16_Encoder_State *cstate) /* (i/o) Coder State */ { Word16 s; /* Q2 */ Word16 a, b; Word16 im; Word16 maxdev, flag, mpflag; Word32 eni, deltae; Word32 cc; Word16 ah,al, bh, bl; Word32 a0, a1, a2, a3; Word32 *lp0; Word16 exp, new_exp; Word16 *fp0, *fp1, *fp2, *fp3, *sp; Word16 *fp1_h, *fp1_l, *fp2_h, *fp2_l; Word16 cor2max, cor2max_exp; Word16 cor2m, cor2m_exp; Word16 s0, t0, t1, exp0, exp1, e2, e3; Word16 threshold; Word16 mplth; /* Q2 */ Word16 i, j, k, n, npeaks, imax, idx[MAXPPD-MINPPD+1]; Word16 cpp; Word16 plag[HMAXPPD], cor2[MAXPPD1], cor2_exp[MAXPPD1]; Word16 cor2i[HMAXPPD], cor2i_exp[HMAXPPD], xwd[LXD]; Word16 tmp_h[DFO+FRSZ], tmp_l[DFO+FRSZ]; /* DPF Q7 */ Word32 cor[MAXPPD1], energy[MAXPPD1], lxwd[FRSZD]; Word16 energy_man[MAXPPD1], energy_exp[MAXPPD1]; Word16 energyi_man[HMAXPPD], energyi_exp[HMAXPPD]; Word16 energym_man, energym_exp; Word16 energymax_man, energymax_exp; /* Lowpass filter xw() to 800 hz; shift & output into xwd() */ /* AP and AZ filtering and decimation */ fp1_h = tmp_h + DFO; fp1_l = tmp_l + DFO; sp = xw; a1 = 1; for (i=0;i<DFO;i++) tmp_h[i] = cstate->dfm_h[2*i+1]; for (i=0;i<DFO;i++) tmp_l[i] = cstate->dfm_h[2*i]; lp0 = lxwd; for (i=0;i<FRSZD;i++) { for (k=0;k<DECF;k++) { a0 = L_shr(L_deposit_h(*sp++),10); fp2_h = fp1_h-1; fp2_l = fp1_l-1; for (j=0;j<DFO;j++) a0=L_sub(a0,Mpy_32(*fp2_h--,*fp2_l--,adf_h[j+1],adf_l[j+1])); a0 = L_shl(a0, 2); /* adf Q13 */ L_Extract(a0, fp1_h++, fp1_l++); } fp2_h = fp1_h-1; fp2_l = fp1_l-1; a0 = Mpy_32_16(*fp2_h--, *fp2_l--, bdf[0]); for (j=0;j<DFO;j++) a0=L_add(a0,Mpy_32_16(*fp2_h--,*fp2_l--,bdf[j+1])); *lp0++ = a0; a0 = L_abs(a0); if (a1 < a0) a1 = a0; } /* copy temp buffer to memory */ fp1_h -= DFO; fp1_l -= DFO; for (i=0;i<DFO;i++) { cstate->dfm_h[2*i+1] = fp1_h[i]; cstate->dfm_h[2*i] = fp1_l[i]; } lp0 = lxwd; new_exp = sub(norm_l(a1), 3); /* headroom to avoid overflow */ exp = sub(cstate->xwd_exp,new_exp); /* increase in bit-resolution */ if (exp < 0) { /* Descending signal level */ new_exp = cstate->xwd_exp; exp = 0; } for (i=0;i<XDOFF;i++) xwd[i] = shr(cstate->xwd[i], exp); /* fill-in new exponent */ fp0 = xwd + XDOFF; for (i=0;i<FRSZD;i++) fp0[i] = intround(L_shl(lp0[i],new_exp)); /* update signal memory for next frame */ exp0 = 1; for (i=0;i<XDOFF;i++) { exp1 = abs_s(xwd[FRSZD+i]); if (exp1 > exp0) exp0 = exp1; } exp0 = sub(norm_s(exp0),3); /* extra exponent for next frame */ exp = sub(exp0, exp); if (exp >=0) { for (i=0;i<XDOFF-FRSZD;i++) cstate->xwd[i] = shl(cstate->xwd[i+FRSZD], exp); } else { exp = -exp; if (exp >=15) exp = 15; for (i=0;i<XDOFF-FRSZD;i++) cstate->xwd[i] = shr(cstate->xwd[i+FRSZD], exp); } for (;i<XDOFF;i++) cstate->xwd[i] = shl(xwd[FRSZD+i],exp0); cstate->xwd_exp = add(new_exp, exp0); /* Compute correlation & energy of prediction basis vector */ /* reset local buffers */ for (i=0;i<MAXPPD1;i++) cor[i] = energy[i] = 0; fp0 = xwd+MAXPPD1; fp1 = xwd+MAXPPD1-M1; a0 = a1 = 0; for (i=0;i<(LXD-MAXPPD1);i++) { a0 = L_mac0(a0, *fp1, *fp1); a1 = L_mac0(a1, *fp0++, *fp1++); } cor[M1-1] = a1; energy[M1-1] = a0; energy_exp[M1-1] = norm_l(energy[M1-1]); energy_man[M1-1] = extract_h(L_shl(energy[M1-1], energy_exp[M1-1])); s0 = cor2_exp[M1-1] = norm_l(a1); t0 = extract_h(L_shl(a1, s0)); cor2[M1-1] = extract_h(L_mult(t0, t0)); if (a1 < 0) cor2[M1-1] = negate(cor2[M1-1]); fp2 = xwd+LXD-M1-1; fp3 = xwd+MAXPPD1-M1-1; for (i=M1;i<M2;i++) { fp0 = xwd+MAXPPD1; fp1 = xwd+MAXPPD1-i-1; a1 = 0; for (j=0;j<(LXD-MAXPPD1);j++) a1 = L_mac0(a1,*fp0++,*fp1++); cor[i] = a1; a0 = L_msu0(a0, *fp2, *fp2); a0 = L_mac0(a0, *fp3, *fp3); fp2--; fp3--; energy[i] = a0; energy_exp[i] = norm_l(energy[i]); energy_man[i] = extract_h(L_shl(energy[i], energy_exp[i])); s0 = cor2_exp[i] = norm_l(a1); t0 = extract_h(L_shl(a1, s0)); cor2[i] = extract_h(L_mult(t0, t0)); if (a1 < 0) cor2[i] = negate(cor2[i]); } /* Find positive correlation peaks */ /* Find maximum of cor*cor/energy among positive correlation peaks */ npeaks = 0; n = MINPPD-1; while ((npeaks < MAX_NPEAKS) && (n<MAXPPD)) { if (cor[n]>0) { a0 = L_mult(energy_man[n-1],cor2[n]); a1 = L_mult(energy_man[n], cor2[n-1]); exp0 = shl(sub(cor2_exp[n], cor2_exp[n-1]),1); exp0 = add(exp0, energy_exp[n-1]); exp0 = sub(exp0, energy_exp[n]); if (exp0>=0) a0 = L_shr(a0, exp0); else a1 = L_shl(a1, exp0); if (a0 > a1) { a0 = L_mult(energy_man[n+1],cor2[n]); a1 = L_mult(energy_man[n], cor2[n+1]); exp0 = shl(sub(cor2_exp[n], cor2_exp[n+1]),1); exp0 = add(exp0, energy_exp[n+1]); exp0 = sub(exp0, energy_exp[n]); if (exp0>=0) a0 = L_shr(a0, exp0); else a1 = L_shl(a1, exp0); if (a0 > a1) { idx[npeaks] = n; npeaks++; } } } n++; } /* Return early if there is no peak or only one peak */ if (npeaks == 0){ /* if there are no positive peak, */ return MINPPD*DECF; /* return minimum pitch period in decimated domain */ } if (npeaks == 1){ /* if there is exactly one peak, */ return (idx[0]+1)*DECF; /* return the time lag for this single peak */ } /* If program proceeds to here, there are 2 or more peaks */ cor2max=(Word16) 0x8000; cor2max_exp= (Word16) 0; energymax_man=1; energymax_exp=0; imax=0; for (i=0; i < npeaks; i++) { /* Use quadratic interpolation to find the interpolated cor[] and energy[] corresponding to interpolated peak of cor2[]/energy[] */ /* first calculate coefficients of y(x)=ax^2+bx+c; */ n=idx[i]; a0=L_sub(L_shr(L_add(cor[n+1],cor[n-1]),1),cor[n]); L_Extract(a0, &ah, &al); a0=L_shr(L_sub(cor[n+1],cor[n-1]),1); L_Extract(a0, &bh, &bl); cc=cor[n]; /* Initialize variables before searching for interpolated peak */ im=0; cor2m_exp = cor2_exp[n]; cor2m = cor2[n]; energym_exp = energy_exp[n]; energym_man = energy_man[n]; eni=energy[n]; /* Determine which side the interpolated peak falls in, then do the search in the appropriate range */ a0 = L_mult(energy_man[n-1],cor2[n+1]); a1 = L_mult(energy_man[n+1], cor2[n-1]); exp0 = shl(sub(cor2_exp[n+1], cor2_exp[n-1]),1); exp0 = add(exp0, energy_exp[n-1]); exp0 = sub(exp0, energy_exp[n+1]); if (exp0>=0) a0 = L_shr(a0, exp0); else a1 = L_shl(a1, exp0); if (a0 > a1) { /* if right side */ deltae = L_shr(L_sub(energy[n+1], eni), 2); for (k = 0; k < HDECF; k++) { a0=L_add(L_add(Mpy_32_16(ah,al,x2[k]),Mpy_32_16(bh,bl,x[k])),cc); eni = L_add(eni, deltae); a1 = eni; exp0 = norm_l(a0); s0 = extract_h(L_shl(a0, exp0)); s0 = extract_h(L_mult(s0, s0)); e2 = energym_exp; t0 = energym_man; a2 = L_mult(t0, s0); e3 = norm_l(a1); t1 = extract_h(L_shl(a1, e3)); a3 = L_mult(t1, cor2m); exp1 = shl(sub(exp0, cor2m_exp),1); exp1 = add(exp1, e2); exp1 = sub(exp1, e3); if (exp1>=0) a2 = L_shr(a2, exp1); else a3 = L_shl(a3, exp1); if (a2 > a3) { im = k+1; cor2m = s0; cor2m_exp = exp0; energym_exp = e3; energym_man = t1; } } } else { /* if interpolated peak is on the left side */ deltae = L_shr(L_sub(energy[n-1], eni), 2); for (k = 0; k < HDECF; k++) { a0=L_add(L_sub(Mpy_32_16(ah,al,x2[k]),Mpy_32_16(bh,bl,x[k])),cc); eni = L_add(eni, deltae); a1=eni; exp0 = norm_l(a0); s0 = extract_h(L_shl(a0, exp0)); s0 = extract_h(L_mult(s0, s0)); e2 = energym_exp; t0 = energym_man; a2 = L_mult(t0, s0); e3 = norm_l(a1); t1 = extract_h(L_shl(a1, e3)); a3 = L_mult(t1, cor2m); exp1 = shl(sub(exp0, cor2m_exp),1); exp1 = add(exp1, e2); exp1 = sub(exp1, e3); if (exp1>=0) a2 = L_shr(a2, exp1); else a3 = L_shl(a3, exp1); if (a2 > a3) { im = -k-1; cor2m = s0; cor2m_exp = exp0; energym_exp = e3; energym_man = t1; } } } /* Search done; assign cor2[] and energy[] corresponding to interpolated peak */ plag[i]=add(shl(add(idx[i],1),2),im); /* lag of interp. peak */ cor2i[i]=cor2m; cor2i_exp[i]=cor2m_exp; /* interpolated energy[] of i-th interpolated peak */ energyi_exp[i] = energym_exp; energyi_man[i] = energym_man; /* Search for global maximum of interpolated cor2[]/energy[] peak */ a0 = L_mult(cor2m,energymax_man); a1 = L_mult(cor2max, energyi_man[i]); exp0 = shl(sub(cor2m_exp, cor2max_exp),1); exp0 = add(exp0, energymax_exp); exp0 = sub(exp0, energyi_exp[i]); if (exp0 >=0) a0 = L_shr(a0, exp0); else a1 = L_shl(a1, exp0); if (a0 > a1) { imax=i; cor2max=cor2m; cor2max_exp=cor2m_exp; energymax_exp = energyi_exp[i]; energymax_man = energyi_man[i]; } } cpp=plag[imax]; /* first candidate for coarse pitch period */ mplth=plag[npeaks-1]; /* set mplth to the lag of last peak */ /* Find the largest peak (if there is any) around the last pitch */ maxdev= shr(cstate->cpplast,2); /* maximum deviation from last pitch */ im = -1; cor2m=(Word16) 0x8000; cor2m_exp= (Word16) 0; energym_man = 1; energym_exp = 0; for (i=0;i<npeaks;i++) { /* loop thru the peaks before the largest peak */ if (abs_s(sub(plag[i],cstate->cpplast)) <= maxdev) { a0 = L_mult(cor2i[i],energym_man); a1 = L_mult(cor2m, energyi_man[i]); exp0 = shl(sub(cor2i_exp[i], cor2m_exp),1); exp0 = add(exp0, energym_exp); exp0 = sub(exp0, energyi_exp[i]); if (exp0 >=0) a0 = L_shr(a0, exp0); else a1 = L_shl(a1, exp0); if (a0 > a1) { im=i; cor2m=cor2i[i]; cor2m_exp=cor2i_exp[i]; energym_man = energyi_man[i]; energym_exp = energyi_exp[i]; } } } /* if there is no peaks around last pitch, then im is still -1 */ /* Now see if we should pick any alternatice peak */ /* first, search first half of pitch range, see if any qualified peak has large enough peaks at every multiple of its lag */ i=0; while (2*plag[i] < mplth) { /* Determine the appropriate threshold for this peak */ if (i != im) { /* if not around last pitch, */ threshold = TH1; /* use a higher threshold */ } else { /* if around last pitch */ threshold = TH2; /* use a lower threshold */ } /* If threshold exceeded, test peaks at multiples of this lag */ a0 = L_mult(cor2i[i],energymax_man); t1 = extract_h(L_mult(energyi_man[i], threshold)); a1 = L_mult(cor2max, t1); exp0 = shl(sub(cor2i_exp[i], cor2max_exp),1); exp0 = add(exp0, energymax_exp); exp0 = sub(exp0, energyi_exp[i]); if (exp0 >=0) a0 = L_shr(a0, exp0); else a1 = L_shl(a1, exp0); if (a0 > a1) { flag=1; j=i+1; k=0; s=shl(plag[i],1); /* initialize t to twice the current lag */ while (s<=mplth) { /* loop thru all multiple lag <= mplth */ mpflag=0; /* initialize multiple pitch flag to 0 */ t0 = mult_r(s,MPDTH); a=sub(s, t0); /* multiple pitch range lower bound */ b=add(s, t0); /* multiple pitch range upper bound */ while (j < npeaks) { /* loop thru peaks with larger lags */ if (plag[j] > b) { /* if range exceeded, */ break; /* break the innermost while loop */ } /* if didn't break, then plag[j] <= b */ if (plag[j] > a) { /* if current peak lag within range, */ /* then check if peak value large enough */ a0 = L_mult(cor2i[j],energymax_man); if (k<4) t1 = MPTH[k]; else t1 = MPTH4; t1 = extract_h(L_mult(t1, energyi_man[j])); a1 = L_mult(cor2max, t1); exp0 = shl(sub(cor2i_exp[j], cor2max_exp),1); exp0 = add(exp0, energymax_exp); exp0 = sub(exp0, energyi_exp[j]); if (exp0 >=0) a0 = L_shr(a0, exp0); else a1 = L_shl(a1, exp0); if (a0 > a1) { mpflag=1; /* if peak large enough, set mpflag, */ break; /* and break the innermost while loop */ } } j++; } /* if no qualified peak found at this multiple lag */ if (mpflag == 0) { flag=0; /* disqualify the lag plag[i] */ break; /* and break the while (s<=mplth) loop */ } k++; s = add(s, plag[i]); /* update s to the next multiple pitch lag */ } /* if there is a qualified peak at every multiple of plag[i], */ if (flag == 1) { cpp = plag[i]; /* then accept this as final pitch */ return cpp; /* and return to calling function */ } } i++; if (i == npeaks) break; /* to avoid out of array bound error */ } /* If program proceeds to here, none of the peaks with lags < 0.5*mplth qualifies as the final pitch. in this case, check if there is any peak large enough around last pitch. if so, use its lag as the final pitch. */ if (im != -1) { /* if there is at least one peak around last pitch */ if (im == imax) { /* if this peak is also the global maximum, */ return cpp; /* return first pitch candidate at global max */ } if (im < imax) { /* if lag of this peak < lag of global max, */ a0 = L_mult(cor2m,energymax_man); t1 = extract_h(L_mult(energym_man, LPTH2)); a1 = L_mult(cor2max, t1); exp0 = shl(sub(cor2m_exp, cor2max_exp),1); exp0 = add(exp0, energymax_exp); exp0 = sub(exp0, energym_exp); if (exp0 >=0) a0 = L_shr(a0, exp0); else a1 = L_shl(a1, exp0); if (a0 > a1) { if (plag[im] > HMAXPPD*DECF) { cpp=plag[im]; return cpp; } for (k=2; k<=5;k++) { /* check if current candidate pitch */ s=mult(plag[imax],invk[k-2]); /* is a sub-multiple of */ t0 = mult_r(s,SMDTH); a=sub(s, t0); /* the time lag of */ b=add(s, t0); /* the global maximum peak */ if (plag[im]>a && plag[im]<b) { /* if so, */ cpp=plag[im]; /* accept this peak, */ return cpp; /* and return as pitch */ } } } } else { /* if lag of this peak > lag of global max, */ a0 = L_mult(cor2m,energymax_man); t1 = extract_h(L_mult(energym_man, LPTH1)); a1 = L_mult(cor2max, t1); exp0 = shl(sub(cor2m_exp, cor2max_exp),1); exp0 = add(exp0, energymax_exp); exp0 = sub(exp0, energym_exp); if (exp0 >=0) a0 = L_shr(a0, exp0); else a1 = L_shl(a1, exp0); if (a0 > a1) { cpp = plag[im]; /* if this peak is large enough, */ return cpp; /* accept its lag */ } } } /* If program proceeds to here, we have no choice but to accept the lag of the global maximum */ return cpp; }
void Az_lsp ( INT16 a[], /* (i) : predictor coefficients */ INT16 lsp[], /* (o) : line spectral pairs */ INT16 old_lsp[] /* (i) : old lsp[] (in case not found 10 roots) */ ) { INT16 i, j, nf, ip; INT16 xlow, ylow, xhigh, yhigh, xmid, ymid, xint; INT16 x, y, sign, exp; INT16 *coef; INT16 f1[M / 2 + 1], f2[M / 2 + 1]; INT32 t0=0; VPP_EFR_PROFILE_FUNCTION_ENTER(Az_lsp); /*-------------------------------------------------------------* * find the sum and diff. pol. F1(z) and F2(z) * * F1(z) <--- F1(z)/(1+z**-1) & F2(z) <--- F2(z)/(1-z**-1) * * * * f1[0] = 1.0; * * f2[0] = 1.0; * * * * for (i = 0; i< NC; i++) * * { * * f1[i+1] = a[i+1] + a[M-i] - f1[i] ; * * f2[i+1] = a[i+1] - a[M-i] + f2[i] ; * * } * *-------------------------------------------------------------*/ f1[0] = 1024; /* f1[0] = 1.0 */ f2[0] = 1024; /* f2[0] = 1.0 */ for (i = 0; i < NC; i++) { //VPP_MLX16 (t0_hi,t0_lo,a[i + 1], 8192); //VPP_MLA16 ( t0_hi, t0_lo, a[M - i], 8192); //t0 = VPP_SCALE64_TO_16( t0_hi, t0_lo); //x = EXTRACT_H(t0); t0 = (INT32) a[i + 1] + (INT32)a[M - i]; x = (INT16)(L_SHR_D(t0,2)); /* f1[i+1] = a[i+1] + a[M-i] - f1[i] */ f1[i + 1] = SUB (x, f1[i]); //VPP_MLX16(t0_hi, t0_lo, a[i + 1], 8192); //VPP_MLA16(t0_hi, t0_lo, a[M - i], -8192); //x = EXTRACT_H(VPP_SCALE64_TO_16(t0_hi, t0_lo)); t0 = (INT32) a[i + 1] - (INT32)a[M - i]; x = (INT16)(L_SHR_D(t0,2)); //f2[i + 1] = add (x, f2[i]); f2[i + 1] = ADD(x, f2[i]); } /*-------------------------------------------------------------* * find the LSPs using the Chebychev pol. evaluation * *-------------------------------------------------------------*/ nf = 0; /* number of found frequencies */ ip = 0; /* indicator for f1 or f2 */ coef = f1; xlow = grid[0]; ylow = Chebps (xlow, coef, NC); j = 0; /* while ( (nf < M) && (j < grid_points) ) */ //while ((sub (nf, M) < 0) && (sub (j, grid_points) < 0)) while ((SUB (nf, M) < 0) && (SUB (j, grid_points) < 0)) { j++; xhigh = xlow; yhigh = ylow; xlow = grid[j]; ylow = Chebps (xlow, coef, NC); //if (L_mult (ylow, yhigh) <= (INT32) 0L) if (L_MULT(ylow, yhigh) <= (INT32) 0L) { /* divide 4 times the interval */ for (i = 0; i < 4; i++) { /* xmid = (xlow + xhigh)/2 */ // xmid = add (shr (xlow, 1), shr (xhigh, 1)); xmid = ADD ((SHR_D(xlow, 1)),(SHR_D(xhigh, 1))); ymid = Chebps (xmid, coef, NC); //if (L_mult (ylow, ymid) <= (INT32) 0L) if (L_MULT(ylow, ymid) <= (INT32) 0L) { yhigh = ymid; xhigh = xmid; } else { ylow = ymid; xlow = xmid; } } /*-------------------------------------------------------------* * Linear interpolation * * xint = xlow - ylow*(xhigh-xlow)/(yhigh-ylow); * *-------------------------------------------------------------*/ //x = sub (xhigh, xlow); x = SUB (xhigh, xlow); //y = sub (yhigh, ylow); y = SUB (yhigh, ylow); if (y == 0) { xint = xlow; } else { sign = y; //y = abs_s (y); y = ABS_S(y); exp = norm_s (y); //y = shl (y, exp); y = SHL(y, exp); y = div_s ((INT16) 16383, y); //t0 = L_mult (x, y); t0 = L_MULT(x, y); //t0 = L_shr (t0, sub (20, exp)); t0 = L_SHR_V(t0, SUB (20, exp)); //y = extract_l (t0); /* y= (xhigh-xlow)/(yhigh-ylow) */ y = EXTRACT_L(t0); if (sign < 0) { //y = negate (y); y = NEGATE(y); } //t0 = L_mult (ylow, y); t0 = L_MULT(ylow, y); //t0 = L_shr (t0, 11); t0 = L_SHR_D(t0, 11); //xint = sub (xlow, extract_l (t0)); /* xint = xlow - ylow*y */ xint = SUB (xlow, EXTRACT_L(t0)); } lsp[nf] = xint; xlow = xint; nf++; if (ip == 0) { ip = 1; coef = f2; } else { ip = 0; coef = f1; } ylow = Chebps (xlow, coef, NC); } } /* Check if M roots found */ //if (sub (nf, M) < 0) if (SUB (nf, M) < 0) { for (i = 0; i < M; i++) { lsp[i] = old_lsp[i]; } } VPP_EFR_PROFILE_FUNCTION_EXIT(Az_lsp); return; }
int16_t samples_to_rmlt_coefs(const int16_t new_samples[], int16_t old_samples[], int16_t coefs[], int dct_length) { int i; int half_dct_length; int last; int16_t mag_shift; int16_t n; int16_t windowed_data[MAX_DCT_LENGTH]; const int16_t *win; int32_t acca; int32_t accb; int16_t temp; int16_t temp1; int16_t temp2; half_dct_length = dct_length >> 1; win = (dct_length == DCT_LENGTH) ? samples_to_rmlt_window : max_samples_to_rmlt_window; /* Get the first half of the windowed samples */ last = half_dct_length - 1; for (i = 0; i < half_dct_length; i++) { acca = L_mult(win[last - i], old_samples[last - i]); acca = L_mac(acca, win[half_dct_length + i], old_samples[half_dct_length + i]); windowed_data[i] = xround(acca); } /* Get the second half of the windowed samples */ last = dct_length - 1; for (i = 0; i < half_dct_length; i++) { acca = L_mult(win[last - i], new_samples[i]); acca = L_mac(acca, negate(win[i]), new_samples[last - i]); windowed_data[half_dct_length + i] = xround(acca); } /* Save the new samples for next time, when they will be the old samples. */ vec_copyi16(old_samples, new_samples, dct_length); /* Calculate how many bits to shift up the input to the DCT. */ temp1 = 0; for (i = 0; i < dct_length; i++) { temp2 = abs_s(windowed_data[i]); temp = sub(temp2, temp1); if (temp > 0) temp1 = temp2; } mag_shift = 0; temp = sub(temp1, 14000); if (temp < 0) { temp = sub(temp1, 438); temp = (temp < 0) ? add(temp1, 1) : temp1; accb = L_mult(temp, 9587); acca = L_shr(accb, 20); temp = norm_s((int16_t) acca); mag_shift = (temp == 0) ? 9 : sub(temp, 6); } acca = 0; for (i = 0; i < dct_length; i++) { temp = abs_s(windowed_data[i]); acca = L_add(acca, temp); } acca = L_shr(acca, 7); if (temp1 < acca) mag_shift = sub(mag_shift, 1); if (mag_shift > 0) { for (i = 0; i < dct_length; i++) windowed_data[i] = shl(windowed_data[i], mag_shift); } else if (mag_shift < 0) { n = negate(mag_shift); for (i = 0; i < dct_length; i++) windowed_data[i] = shr(windowed_data[i], n); } /* Perform a Type IV DCT on the windowed data to get the coefficients */ dct_type_iv_a(windowed_data, coefs, dct_length); return mag_shift; }
Word16 samples_to_rmlt_coefs(const Word16 *new_samples,Word16 *old_samples,Word16 *coefs,Word16 dct_length) { Word16 index, vals_left,mag_shift,n; Word16 windowed_data[MAX_DCT_LENGTH]; Word16 *old_ptr; const Word16 *new_ptr, *sam_low, *sam_high; Word16 *win_low, *win_high; Word16 *dst_ptr; Word16 neg_win_low; Word16 samp_high; Word16 half_dct_size; Word32 acca; Word32 accb; Word16 temp; Word16 temp1; Word16 temp2; Word16 temp5; half_dct_size = shr_nocheck(dct_length,1); /*++++++++++++++++++++++++++++++++++++++++++++*/ /* Get the first half of the windowed samples */ /*++++++++++++++++++++++++++++++++++++++++++++*/ dst_ptr = windowed_data; move16(); /* address arithmetic */ test(); if (dct_length==DCT_LENGTH) { win_high = samples_to_rmlt_window + half_dct_size; } else { win_high = max_samples_to_rmlt_window + half_dct_size; } win_low = win_high; move16(); /* address arithmetic */ sam_high = old_samples + half_dct_size; sam_low = sam_high; move16(); for (vals_left = half_dct_size;vals_left > 0;vals_left--) { acca = 0L; move32(); acca = L_mac(acca,*--win_low, *--sam_low); acca = L_mac(acca,*win_high++, *sam_high++); temp = itu_round(acca); *dst_ptr++ = temp; move16(); } /*+++++++++++++++++++++++++++++++++++++++++++++*/ /* Get the second half of the windowed samples */ /*+++++++++++++++++++++++++++++++++++++++++++++*/ sam_low = new_samples; move16(); /* address arithmetic */ sam_high = new_samples + dct_length; for (vals_left = half_dct_size; vals_left > 0; vals_left--) { acca = 0L; move32(); acca = L_mac(acca,*--win_high, *sam_low++); neg_win_low = negate(*win_low++); samp_high = *--sam_high; acca = L_mac(acca, neg_win_low, samp_high); temp = itu_round(acca); *dst_ptr++=temp; move16(); } /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ /* Save the new samples for next time, when they will be the old samples */ /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ new_ptr = new_samples; move16(); old_ptr = old_samples; move16(); for (vals_left = dct_length;vals_left > 0;vals_left--) { *old_ptr++ = *new_ptr++; move16(); } /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ /* Calculate how many bits to shift up the input to the DCT. */ /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ temp1=0; move16(); for(index=0;index<dct_length;index++) { temp2 = abs_s(windowed_data[index]); temp = sub(temp2,temp1); test(); if(temp > 0) { move16(); temp1 = temp2; } } mag_shift=0; move16(); temp = sub(temp1,14000); test(); if (temp >= 0) { mag_shift = 0; move16(); } else { temp = sub(temp1,438); test(); if(temp < 0) temp = add(temp1,1); else { temp = temp1; move16(); } accb = L_mult(temp,9587); acca = L_shr_nocheck(accb,20); temp5 = extract_l(acca); temp = norm_s(temp5); test(); if (temp == 0) { mag_shift = 9; move16(); } else mag_shift = sub(temp,6); } acca = 0L; move32(); for(index=0; index<dct_length; index++) { temp = abs_s( windowed_data[index]); acca = L_add(acca,temp); } acca = L_shr_nocheck(acca,7); test(); if (temp1 < acca) { mag_shift = sub(mag_shift,1); } test(); if (mag_shift > 0) { for(index=0;index<dct_length;index++) { windowed_data[index] = shl_nocheck(windowed_data[index],mag_shift); } } else { test(); if (mag_shift < 0) { n = negate(mag_shift); for(index=0;index<dct_length;index++) { windowed_data[index] = shr_nocheck(windowed_data[index],n); move16(); } } } /*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ /* Perform a Type IV DCT on the windowed data to get the coefficients */ /*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ dct_type_iv_a(windowed_data, coefs, dct_length); return(mag_shift); }
/*-----------------------------------------------------------* * procedure Calc_exc_rand * * ~~~~~~~~~~~~~ * * Computes comfort noise excitation * * for SID and not-transmitted frames * *-----------------------------------------------------------*/ void Calc_exc_rand( Word32 L_exc_err[4], Word16 cur_gain, /* (i) : target sample gain */ Word16 *exc, /* (i/o) : excitation array */ Word16 *seed, /* (i) : current Vad decision */ Flag flag_cod /* (i) : encoder/decoder flag */ ) { Word16 i, j, i_subfr; Word16 temp1, temp2; Word16 pos[4]; Word16 sign[4]; Word16 t0, frac; Word16 *cur_exc; Word16 g, Gp, Gp2; Word16 excg[L_SUBFR], excs[L_SUBFR]; Word32 L_acc, L_ener, L_k; Word16 max, hi, lo, inter_exc; Word16 sh; Word16 x1, x2; if(cur_gain == 0) { for(i=0; i<L_FRAME; i++) { exc[i] = 0; } Gp = 0; t0 = add(L_SUBFR,1); for (i_subfr = 0; i_subfr < L_FRAME; i_subfr += L_SUBFR) { if(flag_cod != FLAG_DEC) update_exc_err(L_exc_err, Gp, t0); } return; } /* Loop on subframes */ cur_exc = exc; for (i_subfr = 0; i_subfr < L_FRAME; i_subfr += L_SUBFR) { /* generate random adaptive codebook & fixed codebook parameters */ /*****************************************************************/ temp1 = Random(seed); frac = sub((temp1 & (Word16)0x0003), 1); if(sub(frac, 2) == 0) frac = 0; temp1 = shr(temp1, 2); t0 = add((temp1 & (Word16)0x003F), 40); temp1 = shr(temp1, 6); temp2 = temp1 & (Word16)0x0007; pos[0] = add(shl(temp2, 2), temp2); /* 5 * temp2 */ temp1 = shr(temp1, 3); sign[0] = temp1 & (Word16)0x0001; temp1 = shr(temp1, 1); temp2 = temp1 & (Word16)0x0007; temp2 = add(shl(temp2, 2), temp2); pos[1] = add(temp2, 1); /* 5 * x + 1 */ temp1 = shr(temp1, 3); sign[1] = temp1 & (Word16)0x0001; temp1 = Random(seed); temp2 = temp1 & (Word16)0x0007; temp2 = add(shl(temp2, 2), temp2); pos[2] = add(temp2, 2); /* 5 * x + 2 */ temp1 = shr(temp1, 3); sign[2] = temp1 & (Word16)0x0001; temp1 = shr(temp1, 1); temp2 = temp1 & (Word16)0x000F; pos[3] = add((temp2 & (Word16)1), 3); /* j+3*/ temp2 = (shr(temp2, 1)) & (Word16)7; temp2 = add(shl(temp2, 2), temp2); /* 5i */ pos[3] = add(pos[3], temp2); temp1 = shr(temp1, 4); sign[3] = temp1 & (Word16)0x0001; Gp = Random(seed) & (Word16)0x1FFF; /* < 0.5 Q14 */ Gp2 = shl(Gp, 1); /* Q15 */ /* Generate gaussian excitation */ /********************************/ L_acc = 0L; for(i=0; i<L_SUBFR; i++) { temp1 = Gauss(seed); L_acc = L_mac(L_acc, temp1, temp1); excg[i] = temp1; } /* Compute fact = alpha x cur_gain * sqrt(L_SUBFR / Eg) with Eg = SUM(i=0->39) excg[i]^2 and alpha = 0.5 alpha x sqrt(L_SUBFR)/2 = 1 + FRAC1 */ L_acc = Inv_sqrt(L_shr(L_acc,1)); /* Q30 */ L_Extract(L_acc, &hi, &lo); /* cur_gain = cur_gainR << 3 */ temp1 = mult_r(cur_gain, FRAC1); temp1 = add(cur_gain, temp1); /* <=> alpha x cur_gainR x 2^2 x sqrt(L_SUBFR) */ L_acc = Mpy_32_16(hi, lo, temp1); /* fact << 17 */ sh = norm_l(L_acc); temp1 = extract_h(L_shl(L_acc, sh)); /* fact << (sh+1) */ sh = sub(sh, 14); for(i=0; i<L_SUBFR; i++) { temp2 = mult_r(excg[i], temp1); temp2 = shr_r(temp2, sh); /* shl if sh < 0 */ excg[i] = temp2; } /* generate random adaptive excitation */ /****************************************/ Pred_lt_3(cur_exc, t0, frac, L_SUBFR); /* compute adaptive + gaussian exc -> cur_exc */ /**********************************************/ max = 0; for(i=0; i<L_SUBFR; i++) { temp1 = mult_r(cur_exc[i], Gp2); temp1 = add(temp1, excg[i]); /* may overflow */ cur_exc[i] = temp1; temp1 = abs_s(temp1); if(sub(temp1,max) > 0) max = temp1; } /* rescale cur_exc -> excs */ if(max == 0) sh = 0; else { sh = sub(3, norm_s(max)); if(sh <= 0) sh = 0; } for(i=0; i<L_SUBFR; i++) { excs[i] = shr(cur_exc[i], sh); } /* Compute fixed code gain */ /***************************/ /**********************************************************/ /*** Solve EQ(X) = 4 X**2 + 2 b X + c */ /**********************************************************/ L_ener = 0L; for(i=0; i<L_SUBFR; i++) { L_ener = L_mac(L_ener, excs[i], excs[i]); } /* ener x 2^(-2sh + 1) */ /* inter_exc = b >> sh */ inter_exc = 0; for(i=0; i<4; i++) { j = pos[i]; if(sign[i] == 0) { inter_exc = sub(inter_exc, excs[j]); } else { inter_exc = add(inter_exc, excs[j]); } } /* Compute k = cur_gainR x cur_gainR x L_SUBFR */ L_acc = L_mult(cur_gain, L_SUBFR); L_acc = L_shr(L_acc, 6); temp1 = extract_l(L_acc); /* cur_gainR x L_SUBFR x 2^(-2) */ L_k = L_mult(cur_gain, temp1); /* k << 2 */ temp1 = add(1, shl(sh,1)); L_acc = L_shr(L_k, temp1); /* k x 2^(-2sh+1) */ /* Compute delta = b^2 - 4 c */ L_acc = L_sub(L_acc, L_ener); /* - 4 c x 2^(-2sh-1) */ inter_exc = shr(inter_exc, 1); L_acc = L_mac(L_acc, inter_exc, inter_exc); /* 2^(-2sh-1) */ sh = add(sh, 1); /* inter_exc = b x 2^(-sh) */ /* L_acc = delta x 2^(-2sh+1) */ if(L_acc < 0) { /* adaptive excitation = 0 */ Copy(excg, cur_exc, L_SUBFR); temp1 = abs_s(excg[(int)pos[0]]) | abs_s(excg[(int)pos[1]]); temp2 = abs_s(excg[(int)pos[2]]) | abs_s(excg[(int)pos[3]]); temp1 = temp1 | temp2; sh = ((temp1 & (Word16)0x4000) == 0) ? (Word16)1 : (Word16)2; inter_exc = 0; for(i=0; i<4; i++) { temp1 = shr(excg[(int)pos[i]], sh); if(sign[i] == 0) { inter_exc = sub(inter_exc, temp1); } else { inter_exc = add(inter_exc, temp1); } } /* inter_exc = b >> sh */ L_Extract(L_k, &hi, &lo); L_acc = Mpy_32_16(hi, lo, K0); /* k x (1- alpha^2) << 2 */ temp1 = sub(shl(sh, 1), 1); /* temp1 > 0 */ L_acc = L_shr(L_acc, temp1); /* 4k x (1 - alpha^2) << (-2sh+1) */ L_acc = L_mac(L_acc, inter_exc, inter_exc); /* delta << (-2sh+1) */ Gp = 0; } temp2 = Sqrt(L_acc); /* >> sh */ x1 = sub(temp2, inter_exc); x2 = negate(add(inter_exc, temp2)); /* x 2^(-sh+2) */ if(sub(abs_s(x2),abs_s(x1)) < 0) x1 = x2; temp1 = sub(2, sh); g = shr_r(x1, temp1); /* shl if temp1 < 0 */ if(g >= 0) { if(sub(g, G_MAX) > 0) g = G_MAX; } else { if(add(g, G_MAX) < 0) g = negate(G_MAX); } /* Update cur_exc with ACELP excitation */ for(i=0; i<4; i++) { j = pos[i]; if(sign[i] != 0) { cur_exc[j] = add(cur_exc[j], g); } else { cur_exc[j] = sub(cur_exc[j], g); } } if(flag_cod != FLAG_DEC) update_exc_err(L_exc_err, Gp, t0); cur_exc += L_SUBFR; } /* end of loop on subframes */ return; }
short e_ran_g(short *seed0) { static int iset = 0; static long gset; long rsq, ltemp1, ltemp2; short sv1, sv2, rsq_s; short shft_fctr, stemp1; short shft_fctr1; /* ======================================================================== */ long ltmp1, ltmp2; short ans = 0; short stmp2; /* ======================================================================== */ if (iset == 0) { sv1 = shl(sub(ran0(seed0), 16384), 1); sv2 = shl(sub(ran0(seed0), 16384), 1); /* rsq = sv1 * sv1 + sv2 * sv2; */ ltemp1 = L_mult(sv1, sv1); ltemp2 = L_mult(sv2, sv2); rsq = L_add(L_shr(ltemp1, 1), L_shr(ltemp2, 1)); if (rsq >= 1073741824 || rsq == 0){ /* If condition not met, don't iterate; use */ /* rough approximation. */ ans = shr(sv1,3); ans = add(ans, shr(sv2,3)); ans = add(ans, shr(sub(ran0(seed0), 16384),2)); return (ans); } /* * error in rsq doesn't seem to contribute to the final error in e_ran_g */ /* * rsq scale down by two: input to fnLog must be scaled up by 2. */ rsq = L_shl(rsq, 1); /* stemp1 = round(L_negate(fnLog(rsq))); */ ltmp1 = L_negate(fnLog(rsq)); /* * rsq must be greater than the log of lsq for the fractional * divide to work. therfore normalize rsq. */ shft_fctr = norm_l(rsq); rsq_s = round(L_shl(rsq, shft_fctr)); stmp2 = (divide_s(round(ltmp1), rsq_s)); /* * stemp2 must be normalized before taking its square root. * (increases precision). */ shft_fctr1 = norm_s(stmp2); ltmp2 = L_deposit_h(shl(stmp2, shft_fctr1)); stemp1 = sqroot(ltmp2); /* * shifting involved before taking the square root: * LEFT << shft_fctr. (LEFT because rsq is in the denominator * of ltemp2 quotion). * LEFT << 6. (multiply by 2 in original code and multiply by 32 * because output of fnLog scaled down by 32). * RIGHT >> shft_fctr1. (normalization taken before sqroot). */ shft_fctr = shft_fctr + 6 - shft_fctr1; /* * PROPERTY: sqrt(2^n) = 2^(n/2) * if shft_fctr is odd; multiply stemp1 by sqrt(2)/2 and * increment number of shifts by 1. Can now use shft_fctr / 2. */ if (shft_fctr & 0x0001) { stemp1 = mult(stemp1, 23170); shft_fctr++; } shft_fctr = shr(shft_fctr, 1); /* * normalize stemp1 for the following multiplication. * adjust shft_fctr accordingly. */ shft_fctr1 = norm_s(stemp1); stemp1 = shl(stemp1, shft_fctr1); shft_fctr = shft_fctr - shft_fctr1; gset = L_mult(sv1, stemp1); /* * final output is scaled down by 4, therefore shift up by * shft_fctr - 2. */ gset = L_shl(gset, shft_fctr - 2); iset = 1; return round(L_shl(L_mult(sv2, stemp1), shft_fctr - 2)); } else { iset = 0; return round(gset); } }
void a2lsp( Word16 pc[], /* (i) Q12: predictor coefficients */ Word16 lsp[], /* (o) Q15: line spectral pairs */ Word16 old_lsp[]) /* (i) Q15: old lsp */ { Word16 i, j, exp; Word16 fa_man[NAB], fa_exp[NAB], fb_man[NAB], fb_exp[NAB]; Word16 ta_man[NAB], ta_exp[NAB], tb_man[NAB], tb_exp[NAB]; Word16 *t_man, *t_exp; Word32 a0; Word16 nd2, nf, ngrd; Word16 xroot, xlow, ylow, ind, xhigh, yhigh, xmid, ymid, dx, dy, dxdy, x, sign; /* Find normalization for fa and fb */ /* fb[0] = fa[0] = 1.0; */ /* for (i = 1, j = LPCO; i <= (LPCO/2); i++, j--) { */ /* fa[i] = pc[i] + pc[j] - fa[i-1]; */ /* fb[i] = pc[i] - pc[j] + fb[i-1]; */ /* } */ fa_man[0] = 16384; fa_exp[0] = 6; // fa_man[0] in high 16-bits >> fa_exp[0] = 1.0 in Q24 fb_man[0] = 16384; fb_exp[0] = 6; // fb_man[0] in high 16-bits >> fb_exp[0] = 1.0 in Q24 for (i = 1, j = LPCO; i <= (LPCO/2); i++, j--) { a0 = L_mult0(pc[i], 4096); // Q24 a0 = L_mac0(a0, pc[j], 4096); // Q24 a0 = L_sub(a0, L_shr(L_deposit_h(fa_man[i-1]),fa_exp[i-1])); // Q24 fa_exp[i] = norm_l(a0); fa_man[i] = intround(L_shl(a0, fa_exp[i])); // Q(8+fb_exp[i]) a0 = L_mult0(pc[i], 4096); // Q24 a0 = L_msu0(a0, pc[j], 4096); // Q24 a0 = L_add(a0, L_shr(L_deposit_h(fb_man[i-1]),fb_exp[i-1])); // Q24 fb_exp[i] = norm_l(a0); fb_man[i] = intround(L_shl(a0, fb_exp[i])); // Q(8+fb_exp[i]) } nd2 = (LPCO)/2; /* ta[] and tb[] in Q(7+exp) */ /* ta[0] = fa[nab-1]; ta[i] = 2.0 * fa[j]; */ /* tb[0] = fb[nab-1]; tb[i] = 2.0 * fb[j]; */ ta_man[0] = fa_man[NAB-1]; ta_exp[0] = add(fa_exp[NAB-1], 1); tb_man[0] = fb_man[NAB-1]; tb_exp[0] = add(fb_exp[NAB-1], 1); for (i = 1, j = NAB - 2; i < NAB; ++i, --j) { ta_man[i] = fa_man[j]; ta_exp[i] = fa_exp[j]; tb_man[i] = fb_man[j]; tb_exp[i] = fb_exp[j]; } nf = 0; t_man = ta_man; t_exp = ta_exp; xroot = 0x7fff; ngrd = 0; xlow = grid[0]; // Q15 ylow = FNevChebP(xlow, t_man, t_exp, nd2); ind = 0; /* Root search loop */ while (ngrd<(Ngrd-1) && nf < LPCO) { ngrd++; xhigh = xlow; yhigh = ylow; xlow = grid[ngrd]; ylow = FNevChebP(xlow, t_man, t_exp, nd2); if ( L_mult(ylow ,yhigh) <= 0) { /* Bisections of the interval containing a sign change */ dx = xhigh - xlow; for (i = 1; i <= NBIS; ++i) { dx = shr(dx, 1); xmid = add(xlow, dx); ymid = FNevChebP(xmid, t_man, t_exp, nd2); if (L_mult(ylow,ymid) <= 0) { yhigh = ymid; xhigh = xmid; } else { ylow = ymid; xlow = xmid; } } /* * Linear interpolation in the subinterval with a sign change * (take care if yhigh=ylow=0) */ dx = sub(xhigh, xlow); dy = sub(ylow, yhigh); if (dy != 0) { sign = dy; dy = abs_s(dy); exp = norm_s(dy); dy = shl(dy, exp); /* The maximum grid distance is 1629 => */ /* Maximum dx=1629/2^4=101.8125, i.e. 16384/101.8125=160.92~128 (7 bits) */ /* However, due to the starting point for the search of a new root, */ /* xlow = xroot, 1 more bit of headroom for the division is required. */ dxdy = div_s(shl(dx,6), dy); a0 = L_mult(dxdy, ylow); a0 = L_shr(a0, sub(6, exp)); x = intround(a0); if(sign < 0) x = negate(x); xmid = add(xlow, x); } else { xmid = add(xlow, shr(dx,1)); } /* acos mapping for New lsp component */ while (( costable[ind] >= xmid ) && (ind < 63)) ind++; ind--; a0 = L_mult( sub(xmid, costable[ind]) , acosslope[ind] ); x = intround(L_shl(a0, 4)); lsp[nf] = add(x, shl(ind, 9)); ++nf; /* Start the search for the roots of next polynomial at the estimated * location of the root just found. We have to catch the case that the * two polynomials have roots at the same place to avoid getting stuck at * that root. */ if (xmid >= xroot) xmid = xlow - dx; xroot = xmid; if (t_man == ta_man){ t_man = tb_man; t_exp = tb_exp; } else{ t_man = ta_man; t_exp = ta_exp; } xlow = xmid; ylow = FNevChebP(xlow, t_man, t_exp, nd2); } } /* Check if all LSPs are found */ if( sub(nf, LPCO) < 0) { W16copy(lsp, old_lsp, LPCO); } return; }
Word16 Cb_gain_average( Cb_gain_averageState *st, /* i/o : State variables for CB gain averaging */ enum Mode mode, /* i : AMR mode */ Word16 gain_code, /* i : CB gain Q1 */ Word16 lsp[], /* i : The LSP for the current frame Q15 */ Word16 lspAver[], /* i : The average of LSP for 8 frames Q15 */ Word16 bfi, /* i : bad frame indication flag */ Word16 prev_bf, /* i : previous bad frame indication flag */ Word16 pdfi, /* i : potential degraded bad frame ind flag */ Word16 prev_pdf, /* i : prev pot. degraded bad frame ind flag */ Word16 inBackgroundNoise, /* i : background noise decision */ Word16 voicedHangover, /* i : # of frames after last voiced frame */ Flag *pOverflow ) { Word16 i; Word16 cbGainMix; Word16 diff; Word16 tmp_diff; Word16 bgMix; Word16 cbGainMean; Word32 L_sum; Word16 tmp[M]; Word16 tmp1; Word16 tmp2; Word16 shift1; Word16 shift2; Word16 shift; /*---------------------------------------------------------* * Compute mixed cb gain, used to make cb gain more * * smooth in background noise for modes 5.15, 5.9 and 6.7 * * states that needs to be updated by all * *---------------------------------------------------------*/ /* set correct cbGainMix for MR74, MR795, MR122 */ cbGainMix = gain_code; /*-------------------------------------------------------* * Store list of CB gain needed in the CB gain * * averaging * *-------------------------------------------------------*/ for (i = 0; i < (L_CBGAINHIST - 1); i++) { st->cbGainHistory[i] = st->cbGainHistory[i+1]; } st->cbGainHistory[L_CBGAINHIST-1] = gain_code; diff = 0; /* compute lsp difference */ for (i = 0; i < M; i++) { tmp1 = abs_s(sub(*(lspAver + i), *(lsp + i), pOverflow)); /* Q15 */ shift1 = sub(norm_s(tmp1), 1, pOverflow); /* Qn */ tmp1 = shl(tmp1, shift1, pOverflow); /* Q15+Qn */ shift2 = norm_s(*(lspAver + i)); /* Qm */ tmp2 = shl(*(lspAver + i), shift2, pOverflow); /* Q15+Qm */ tmp[i] = div_s(tmp1, tmp2); /* Q15+(Q15+Qn)-(Q15+Qm) */ shift = 2 + shift1 - shift2; if (shift >= 0) { *(tmp + i) = shr(*(tmp + i), shift, pOverflow); /* Q15+Qn-Qm-Qx=Q13 */ } else { *(tmp + i) = shl(*(tmp + i), negate(shift), pOverflow); /* Q15+Qn-Qm-Qx=Q13 */ } diff = add(diff, *(tmp + i), pOverflow); /* Q13 */ } /* Compute hangover */ if (diff > 5325) /* 0.65 in Q11 */ { st->hangVar += 1; } else { st->hangVar = 0; } if (st->hangVar > 10) { /* Speech period, reset hangover variable */ st->hangCount = 0; } /* Compute mix constant (bgMix) */ bgMix = 8192; /* 1 in Q13 */ if ((mode <= MR67) || (mode == MR102)) /* MR475, MR515, MR59, MR67, MR102 */ { /* if errors and presumed noise make smoothing probability stronger */ if (((((pdfi != 0) && (prev_pdf != 0)) || (bfi != 0) || (prev_bf != 0)) && (voicedHangover > 1) && (inBackgroundNoise != 0) && ((mode == MR475) || (mode == MR515) || (mode == MR59)))) { /* bgMix = min(0.25, max(0.0, diff-0.55)) / 0.25; */ tmp_diff = sub(diff, 4506, pOverflow); /* 0.55 in Q13 */ } else { /* bgMix = min(0.25, max(0.0, diff-0.40)) / 0.25; */ tmp_diff = sub(diff, 3277, pOverflow); /* 0.4 in Q13 */ } /* max(0.0, diff-0.55) or */ /* max(0.0, diff-0.40) */ if (tmp_diff > 0) { tmp1 = tmp_diff; } else { tmp1 = 0; } /* min(0.25, tmp1) */ if (2048 < tmp1) { bgMix = 8192; } else { bgMix = shl(tmp1, 2, pOverflow); } if ((st->hangCount < 40) || (diff > 5325)) /* 0.65 in Q13 */ { /* disable mix if too short time since */ bgMix = 8192; } /* Smoothen the cb gain trajectory */ /* smoothing depends on mix constant bgMix */ L_sum = L_mult(6554, st->cbGainHistory[2], pOverflow); /* 0.2 in Q15; L_sum in Q17 */ for (i = 3; i < L_CBGAINHIST; i++) { L_sum = L_mac(L_sum, 6554, st->cbGainHistory[i], pOverflow); } cbGainMean = pv_round(L_sum, pOverflow); /* Q1 */ /* more smoothing in error and bg noise (NB no DFI used here) */ if (((bfi != 0) || (prev_bf != 0)) && (inBackgroundNoise != 0) && ((mode == MR475) || (mode == MR515) || (mode == MR59))) { /* 0.143 in Q15; L_sum in Q17 */ L_sum = L_mult(4681, st->cbGainHistory[0], pOverflow); for (i = 1; i < L_CBGAINHIST; i++) { L_sum = L_mac(L_sum, 4681, st->cbGainHistory[i], pOverflow); } cbGainMean = pv_round(L_sum, pOverflow); /* Q1 */ } /* cbGainMix = bgMix*cbGainMix + (1-bgMix)*cbGainMean; */ /* L_sum in Q15 */ L_sum = L_mult(bgMix, cbGainMix, pOverflow); L_sum = L_mac(L_sum, 8192, cbGainMean, pOverflow); L_sum = L_msu(L_sum, bgMix, cbGainMean, pOverflow); cbGainMix = pv_round(L_shl(L_sum, 2, pOverflow), pOverflow); /* Q1 */ } st->hangCount += 1; return (cbGainMix); }
/* ** ** Function: Lsp_Qnt() ** ** Description: Vector quantizes the LSP frequencies. The LSP ** vector is divided into 3 sub-vectors, or ** bands, of dimension 3, 3, and 4. Each band is ** quantized separately using a different VQ ** table. Each table has 256 entries, so the ** quantization generates three indices of 8 bits ** each. (Only the LSP vector for subframe 3 is ** quantized per frame.) ** ** Links to text: Section 2.5 ** ** Arguments: ** ** Word16 CurrLsp[] Unquantized LSP frequencies for the current frame (10 words) ** Word16 PrevLsp[] LSP frequencies from the previous frame (10 words) ** ** Outputs: Quantized LSP frequencies for the current frame (10 words) ** ** Return value: ** ** Word32 Long word packed with the 3 VQ indices. Band 0 ** corresponds to bits [23:16], band 1 corresponds ** to bits [15:8], and band 2 corresponds to bits [7:0]. ** (Bit 0 is the least significant.) ** */ Word32 Lsp_Qnt( Word16 *CurrLsp, Word16 *PrevLsp ) { int i ; Word16 Wvect[LpcOrder] ; Word16 Tmp0,Tmp1 ; Word16 Exp ; /* * Compute the VQ weighting vector. The weights assign greater * precision to those frequencies that are closer together. */ /* Compute the end differences */ Wvect[0] = sub( CurrLsp[1], CurrLsp[0] ) ; Wvect[LpcOrder-1] = sub( CurrLsp[LpcOrder-1], CurrLsp[LpcOrder-2] ) ; /* Compute the rest of the differences */ for ( i = 1 ; i < LpcOrder-1 ; i ++ ) { Tmp0 = sub( CurrLsp[i+1], CurrLsp[i] ) ; Tmp1 = sub( CurrLsp[i], CurrLsp[i-1] ) ; if ( Tmp0 > Tmp1 ) Wvect[i] = Tmp1 ; else Wvect[i] = Tmp0 ; } /* Invert the differences */ Tmp0 = (Word16) 0x0020 ; for ( i = 0 ; i < LpcOrder ; i ++ ) { if ( Wvect[i] > Tmp0 ) Wvect[i] = div_s( Tmp0, Wvect[i] ) ; else Wvect[i] = MAX_16 ; } /* Normalize the weight vector */ Tmp0 = (Word16) 0 ; for ( i = 0 ; i < LpcOrder ; i ++ ) if ( Wvect[i] > Tmp0 ) Tmp0 = Wvect[i] ; Exp = norm_s( Tmp0 ) ; for ( i = 0 ; i < LpcOrder ; i ++ ) Wvect[i] = shl( Wvect[i], Exp ) ; /* * Compute the VQ target vector. This is the residual that remains * after subtracting both the DC and predicted * components. */ /* * Subtract the DC component from both the current and previous LSP * vectors. */ for ( i = 0 ; i < LpcOrder ; i ++ ) { CurrLsp[i] = sub( CurrLsp[i], LspDcTable[i] ) ; PrevLsp[i] = sub( PrevLsp[i], LspDcTable[i] ) ; } /* * Generate the prediction vector and subtract it. Use a constant * first-order predictor based on the previous (DC-free) LSP * vector. */ for ( i = 0 ; i < LpcOrder ; i ++ ) { Tmp0 = mult_r( PrevLsp[i], (Word16) LspPrd0 ) ; CurrLsp[i] = sub( CurrLsp[i], Tmp0 ) ; } /* * Add the DC component back to the previous LSP vector. This * vector is needed in later routines. */ for ( i = 0 ; i < LpcOrder ; i ++ ) PrevLsp[i] = add( PrevLsp[i], LspDcTable[i] ) ; /* * Do the vector quantization for all three bands */ return Lsp_Svq( CurrLsp, Wvect ) ; }
void Az_lsp( Word16 a[], /* (i) Q12 : predictor coefficients */ Word16 lsp[], /* (o) Q15 : line spectral pairs */ Word16 old_lsp[] /* (i) : old lsp[] (in case not found 10 roots) */ ) { Word16 i, j, nf, ip; Word16 xlow, ylow, xhigh, yhigh, xmid, ymid, xint; Word16 x, y, sign, exp; Word16 *coef; Word16 f1[M/2+1], f2[M/2+1]; Word32 t0, L_temp; Flag ovf_coef; Word16 (*pChebps)(Word16 x, Word16 f[], Word16 n); /*-------------------------------------------------------------* * find the sum and diff. pol. F1(z) and F2(z) * * F1(z) <--- F1(z)/(1+z**-1) & F2(z) <--- F2(z)/(1-z**-1) * * * * f1[0] = 1.0; * * f2[0] = 1.0; * * * * for (i = 0; i< NC; i++) * * { * * f1[i+1] = a[i+1] + a[M-i] - f1[i] ; * * f2[i+1] = a[i+1] - a[M-i] + f2[i] ; * * } * *-------------------------------------------------------------*/ ovf_coef = 0; pChebps = Chebps_11; f1[0] = 2048; /* f1[0] = 1.0 is in Q11 */ f2[0] = 2048; /* f2[0] = 1.0 is in Q11 */ for (i = 0; i< NC; i++) { Overflow = 0; t0 = L_mult(a[i+1], 16384); /* x = (a[i+1] + a[M-i]) >> 1 */ t0 = L_mac(t0, a[M-i], 16384); /* -> From Q12 to Q11 */ x = extract_h(t0); if ( Overflow ) { ovf_coef = 1; } Overflow = 0; f1[i+1] = sub(x, f1[i]); /* f1[i+1] = a[i+1] + a[M-i] - f1[i] */ if ( Overflow ) { ovf_coef = 1; } Overflow = 0; t0 = L_mult(a[i+1], 16384); /* x = (a[i+1] - a[M-i]) >> 1 */ t0 = L_msu(t0, a[M-i], 16384); /* -> From Q12 to Q11 */ x = extract_h(t0); if ( Overflow ) { ovf_coef = 1; } Overflow = 0; f2[i+1] = add(x, f2[i]); /* f2[i+1] = a[i+1] - a[M-i] + f2[i] */ if ( Overflow ) { ovf_coef = 1; } } if ( ovf_coef ) { /*printf("===== OVF ovf_coef =====\n");*/ pChebps = Chebps_10; f1[0] = 1024; /* f1[0] = 1.0 is in Q10 */ f2[0] = 1024; /* f2[0] = 1.0 is in Q10 */ for (i = 0; i< NC; i++) { t0 = L_mult(a[i+1], 8192); /* x = (a[i+1] + a[M-i]) >> 1 */ t0 = L_mac(t0, a[M-i], 8192); /* -> From Q11 to Q10 */ x = extract_h(t0); f1[i+1] = sub(x, f1[i]); /* f1[i+1] = a[i+1] + a[M-i] - f1[i] */ t0 = L_mult(a[i+1], 8192); /* x = (a[i+1] - a[M-i]) >> 1 */ t0 = L_msu(t0, a[M-i], 8192); /* -> From Q11 to Q10 */ x = extract_h(t0); f2[i+1] = add(x, f2[i]); /* f2[i+1] = a[i+1] - a[M-i] + f2[i] */ } } /*-------------------------------------------------------------* * find the LSPs using the Chebichev pol. evaluation * *-------------------------------------------------------------*/ nf=0; /* number of found frequencies */ ip=0; /* indicator for f1 or f2 */ coef = f1; xlow = grid[0]; ylow = (*pChebps)(xlow, coef, NC); j = 0; while ( (nf < M) && (j < GRID_POINTS) ) { j =add(j,1); xhigh = xlow; yhigh = ylow; xlow = grid[j]; ylow = (*pChebps)(xlow,coef,NC); L_temp = L_mult(ylow ,yhigh); if ( L_temp <= (Word32)0) { /* divide 4 times the interval */ for (i = 0; i < 4; i++) { xmid = add( shr(xlow, 1) , shr(xhigh, 1)); /* xmid = (xlow + xhigh)/2 */ ymid = (*pChebps)(xmid,coef,NC); L_temp = L_mult(ylow,ymid); if ( L_temp <= (Word32)0) { yhigh = ymid; xhigh = xmid; } else { ylow = ymid; xlow = xmid; } } /*-------------------------------------------------------------* * Linear interpolation * * xint = xlow - ylow*(xhigh-xlow)/(yhigh-ylow); * *-------------------------------------------------------------*/ x = sub(xhigh, xlow); y = sub(yhigh, ylow); if(y == 0) { xint = xlow; } else { sign= y; y = abs_s(y); exp = norm_s(y); y = shl(y, exp); y = div_s( (Word16)16383, y); t0 = L_mult(x, y); t0 = L_shr(t0, sub(20, exp) ); y = extract_l(t0); /* y= (xhigh-xlow)/(yhigh-ylow) in Q11 */ if(sign < 0) y = negate(y); t0 = L_mult(ylow, y); /* result in Q26 */ t0 = L_shr(t0, 11); /* result in Q15 */ xint = sub(xlow, extract_l(t0)); /* xint = xlow - ylow*y */ } lsp[nf] = xint; xlow = xint; nf =add(nf,1); if(ip == 0) { ip = 1; coef = f2; } else { ip = 0; coef = f1; } ylow = (*pChebps)(xlow,coef,NC); } } /* Check if M roots found */ if( sub(nf, M) < 0) { for(i=0; i<M; i++) { lsp[i] = old_lsp[i]; } /* printf("\n !!Not 10 roots found in Az_lsp()!!!\n"); */ } return; }