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;
}
