void apf(short *in, short *coeff, short *out, long delayi, short alpha,
short beta, short u, short agc, short ltgain, short order, short length, short br)
{
static int FirstTime = 1;
static short FIRmem[ORDER]; /* FIR filter memory */
static short IIRmem[ORDER]; /* IIR filter memory */
static short last;
static short Residual[ACBMemSize + SubFrameSize]; /* local residual */
short wcoef1[ORDER];
short wcoef2[ORDER];
short scratch[SubFrameSize];
short temp[SubFrameSize];
short mem[ORDER];
long sum1, sum2;
long gamma, APFgain;
short i, j, n, best;
short Stemp, shift1, shift2;
long Ltemp;
/* initialization -- should be done in init routine for implementation */
if (FirstTime)
{
FirstTime = 0;
for (i = 0; i < ORDER; i++)
FIRmem[i] = 0;
for (i = 0; i < ORDER; i++)
IIRmem[i] = 0;
for (i = 0; i < ACBMemSize; i++)
Residual[i] = 0;
last = 0;
}
/* Compute weighted LPC coefficients */
weight(wcoef1, coeff, alpha, order);
weight(wcoef2, coeff, beta, order);
/* Tilt speech */
/*...no tilt in non-voiced regions...*/
for (i = 0, sum2 = 0; i < length - 1; i++)
sum2 = L_mac(sum2, in[i], in[i + 1]);
if (sum2 < 0)
u = 0; /*...no tilt...*/
for (i = 0; i < length; i++)
{
scratch[i] = msu_r(L_deposit_h(in[i]), u, last);
last = in[i];
}
/* Compute residual */
fir(scratch, scratch, wcoef1, FIRmem, order, length);
for (i = 0; i < SubFrameSize ; i++)
Residual[ACBMemSize+i] = scratch[i];
/* long term filtering */
/* Find best integer delay around delayi */
j = extract_h(L_add(delayi, 32768));
sum1 = 0;
shift1 = 1;
best = j;
for (i = Max(DMIN, j - 3); i <= Min(DMAX, j + 3); i++)
{
shift2 = 1;
for (n = ACBMemSize, sum2 = 0; n < ACBMemSize + length; n++)
{
Ltemp = L_mult(Residual[n], Residual[n - i]);
Ltemp = L_shr(Ltemp, shift2);
sum2 = L_add(sum2, Ltemp);
if (sum2 >= 0x40000000)
{
sum2 = L_shr(sum2, 1);
shift2++;
}
}
if( ((shift1 >= shift2) && (L_shr(sum2,sub(shift1,shift2)) > sum1))
|| ((shift1 < shift2) && (sum2 > L_shr(sum1,sub(shift2,shift1)))))
{
sum1 = sum2;
shift1 = shift2;
best = i;
}
}
/* Get beta for delayi */
shift1 = 1;
for (i = ACBMemSize, sum1 = 0; i < ACBMemSize + length; i++)
{
Ltemp = L_mult(Residual[i - best], Residual[i - best]);
Ltemp = L_shr(Ltemp, shift1);
sum1 = L_add(sum1, Ltemp);
if (sum1 >= 0x40000000)
{
sum1 = L_shr(sum1, 1);
shift1++;
}
}
shift2 = 1;
for (i = ACBMemSize, sum2 = 0; i < ACBMemSize + length; i++)
{
Ltemp = L_mult(Residual[i], Residual[i - best]);
Ltemp = L_shr(Ltemp, shift2);
sum2 = L_add(sum2, Ltemp);
if (sum2 >= 0x40000000)
{
sum2 = L_shr(sum2, 1);
shift2++;
}
}
if (shift1 > shift2)
{
shift1 = sub(shift1, shift2);
sum2 = L_shr(sum2, shift1);
}
else if (shift1 < shift2)
{
shift2 = sub(shift2, shift1);
sum1 = L_shr(sum1, shift2);
}
if ((sum2 == 0) || (sum1 == 0) || (br == 1))
for (i = 0; i < length; i++)
temp[i] = Residual[i + ACBMemSize];
else
{
if (sum2 >= sum1)
gamma = 0x7fffffff; /* Clip gamma at 1.0 */
else if (sum2 < 0)
gamma = 0;
else
{
shift1 = norm_l(sum1);
sum1 = L_shl(sum1, shift1);
sum2 = L_shl(sum2, shift1);
gamma = L_divide(sum2, sum1);
}
if (gamma < 0x40000000)
for (i = 0; i < length; i++)
temp[i] = Residual[i + ACBMemSize];
else
{
/* Do actual filtering */
for (i = 0; i < length; i++)
{
Ltemp = L_mpy_ls(gamma, ltgain);
Ltemp = L_mpy_ls(Ltemp, Residual[ACBMemSize + i - best]);
temp[i] = add(Residual[ACBMemSize + i], round(Ltemp));
}
}
}
/* iir short term filter - first run */
for (i = 0; i < length; i++)
scratch[i] = temp[i];
for (i = 0; i < order; i++)
mem[i] = IIRmem[i];
iir(scratch, scratch, wcoef2, mem, order, length);
/* Get filter gain */
shift1 = 1;
for (i = 0, sum1 = 0; i < length; i++)
{
Ltemp = L_mult(in[i], in[i]);
Ltemp = L_shr(Ltemp, shift1);
sum1 = L_add(sum1, Ltemp);
if (sum1 >= 0x40000000)
{
sum1 = L_shr(sum1, 1);
shift1++;
}
}
shift2 = 1;
for (i = 0, sum2 = 0; i < length; i++)
{
Ltemp = L_mult(scratch[i], scratch[i]);
Ltemp = L_shr(Ltemp, shift2);
sum2 = L_add(sum2, Ltemp);
if (sum2 >= 0x40000000)
{
sum2 = L_shr(sum2, 1);
shift2++;
}
}
if (shift1 > shift2)
{
shift1 = sub(shift1, shift2);
sum2 = L_shr(sum2, shift1);
}
else if (shift1 < shift2)
{
shift2 = sub(shift2, shift1);
sum1 = L_shr(sum1, shift2);
}
if (sum2 != 0)
{
shift1 = norm_l(sum2);
sum2 = L_shl(sum2, shift1);
shift1 = sub(shift1, 2); /* For (1. < APFgain < 2.) */
sum1 = L_shl(sum1, shift1);
Ltemp = L_divide(sum1, sum2);
shift1 = norm_l(Ltemp);
Ltemp = L_shl(Ltemp, shift1);
Stemp = sqroot(Ltemp);
if (shift1 & 1)
APFgain = L_mult(0x5a82, Stemp);
else
APFgain = L_deposit_h(Stemp);
shift1 = shr(shift1, 1);
APFgain = L_shr(APFgain, shift1);
/* Re-normalize the speech signal */
for (i = 0; i < length; i++)
{
Ltemp = L_mpy_ls(APFgain, temp[i]);
Ltemp = L_shl(Ltemp, 1); /* For (1. < APFgain < 2.) */
temp[i] = round(Ltemp);
}
}
else
APFgain = 0x40000000;
/* iir short term filter - second run */
iir(out, temp, wcoef2, IIRmem, order, length);
/* Update residual buffer */
for (i = 0; i < ACBMemSize; i++)
Residual[i] = Residual[i + length];
}
Word16 refinepitch(
Word16 *x, /* (i) Q1 */
Word16 cpp,
Word16 *ppt) /* (o) Q9 */
{
Word32 a0, a1;
Word32 cor, energy, cormax, enermax32; /* Q3 */
Word16 energymax, energymax_exp, ener, ener_exp;
Word16 cor2, cor2_exp, cor2max, cor2max_exp;
Word16 *sp0, *sp1, *sp2, *sp3;
Word16 *xt;
Word16 s, t;
Word16 lb, ub;
int pp, i, j;
if (cpp >= MAXPP) cpp = MAXPP-1;
if (cpp < MINPP) cpp = MINPP;
lb = sub((Word16)cpp,DEV);
if (lb < MINPP) lb = MINPP; /* lower bound of pitch period search range */
ub = add((Word16)cpp,DEV);
/* to avoid selecting HMAXPP as the refined pitch period */
if (ub >= MAXPP) ub = MAXPP-1;/* lower bound of pitch period search range */
i = lb; /* start the search from lower bound */
xt = x+XOFF;
sp0 = xt;
sp1 = xt-i;
cor = energy = 0;
for (j=0;j<FRSZ; j++)
{
s = *sp1++;
t = *sp0++;
energy = L_mac0(energy, s, s);
cor = L_mac0(cor, s, t);
}
pp = i;
cormax = cor;
enermax32 = energy;
energymax_exp = norm_l(enermax32);
energymax = extract_h(L_shl(enermax32, energymax_exp));
a0 = cor;
cor2max_exp = norm_l(a0);
s = extract_h(L_shl(a0, cor2max_exp));
cor2max_exp = shl(cor2max_exp, 1);
cor2max = extract_h(L_mult0(s, s));
sp0 = xt+FRSZ-lb-1;
sp1 = xt-lb-1;
for (i=lb+1;i<=ub;i++)
{
sp2 = xt;
sp3 = xt-i;
cor = 0;
for (j=0;j<FRSZ;j++)
cor = L_mac0(cor, *sp2++, *sp3++);
a0 = cor;
cor2_exp = norm_l(a0);
s = extract_h(L_shl(a0, cor2_exp));
cor2_exp = shl(cor2_exp, 1);
cor2 = extract_h(L_mult0(s, s));
s = *sp0--;
t = *sp1--;
energy = L_msu0(energy, s, s);
energy = L_mac0(energy, t, t);
a0 = energy;
ener_exp = norm_l(a0);
ener = extract_h(L_shl(a0, ener_exp));
if (ener>0)
{
a0 = L_mult0(cor2, energymax);
a1 = L_mult0(cor2max, ener);
s = add(cor2_exp, energymax_exp);
t = add(cor2max_exp, ener_exp);
if (s>=t) a0 = L_shr(a0, sub(s,t));
else a1 = L_shr(a1, sub(t,s));
if (a0 > a1)
{
pp = i;
cormax = cor;
enermax32 = energy;
cor2max = cor2;
cor2max_exp = cor2_exp;
energymax = ener;
energymax_exp = ener_exp;
}
}
}
if ((enermax32 == 0) || (cormax<=0))
*ppt = 0;
else
{
ub = sub(norm_l(cormax),1);
lb = norm_l(enermax32);
s = extract_h(L_shl(cormax,ub));
t = extract_h(L_shl(enermax32,lb));
s = div_s(s, t);
lb = sub(sub(lb,ub),6);
*ppt = shl(s, lb);
}
return pp;
}
/* Standard Long-Term Postfilter */
void postfilter(
Word16 *s, /* input : quantized speech signal */
Word16 pp, /* input : pitch period */
Word16 *ma_a,
Word16 *b_prv,
Word16 *pp_prv,
Word16 *e) /* output: enhanced speech signal */
{
int n;
Word16 len, t0, t1, t2, t3, shift, aa, R0norm, R0_exp;
Word32 a0, a1, R0, R1, R01, R01max, Rx;
Word16 *fp1;
Word16 ppt, pptmin, pptmax, ppnew;
Word16 bb[2];
Word16 R1max_exp, R1max, R01Sqmax_exp, R01Sqmax, R01Sq_exp, R01Sq, R1_exp, R1n;
Word16 gainn, Rx_exp;
Word16 buf[MAXPP+FRSZ];
Word16 *ps, ww1, ww2;
Word32 step, delta;
Word16 bi0, bi1c, bi1p;
ps = s+XQOFF;
/********************************************************************/
/* pitch search around decoded pitch */
/********************************************************************/
pptmin = sub(pp, DPPQNS);
pptmax = add(pp, DPPQNS);
if (pptmin<MINPP)
{
pptmin = MINPP;
pptmax = add(pptmin, 2*DPPQNS);
}
else if (pptmax>MAXPP)
{
pptmax = MAXPP;
pptmin = sub(pptmax, 2*DPPQNS);
}
fp1 = &s[XQOFF-pptmax];
len = add(FRSZ, pptmax);
a0 = 0;
for (n=0;n<len;n++)
{
t1 = shr(*fp1++, 3);
a0 = L_mac0(a0,t1,t1);
}
shift = norm_l(a0);
if (a0==0) shift=31;
shift = sub(6, shift);
if (shift > 0)
{
ps = buf+pptmax;
fp1 = &s[XQOFF-pptmax];
shift = shr(add(shift, 1), 1);
for (n=0;n<len;n++)
{
buf[n] = shr(fp1[n], shift);
}
}
else shift=0;
R0 = 0;
R1 = 0;
R01 = 0;
for(n=0; n<FRSZ; n++)
{
R0 = L_mac0(R0, ps[n], ps[n]);
R1 = L_mac0(R1, ps[n-pptmin], ps[n-pptmin]);
R01 = L_mac0(R01,ps[n], ps[n-pptmin]);
}
R0_exp = norm_l(R0);
R0norm = extract_h(L_shl(R0, R0_exp));
R0_exp = R0_exp-16;
ppnew = pptmin;
R1max_exp = norm_l(R1);
R1max = extract_h(L_shl(R1, R1max_exp));
R01Sqmax_exp = norm_l(R01);
t1 = extract_h(L_shl(R01, R01Sqmax_exp));
R01Sqmax_exp = shl(R01Sqmax_exp, 1);
R01Sqmax = extract_h(L_mult(t1, t1));
R01max = R01;
for(ppt=pptmin+1; ppt<=pptmax; ppt++)
{
R1 = L_msu0(R1,ps[FRSZ-ppt], ps[FRSZ-ppt]);
R1 = L_mac0(R1,ps[-ppt], ps[-ppt]);
R01= 0;
for(n=0; n<FRSZ; n++)
{
R01 = L_mac0(R01, ps[n], ps[n-ppt]);
}
R01Sq_exp = norm_l(R01);
t1 = extract_h(L_shl(R01, R01Sq_exp));
R01Sq_exp = shl(R01Sq_exp, 1);
R01Sq = extract_h(L_mult(t1, t1));
R1_exp = norm_l(R1);
R1n = extract_h(L_shl(R1, R1_exp));
a0 = L_mult(R01Sq, R1max);
a1 = L_mult(R01Sqmax, R1n);
t1 = add(R01Sq_exp, R1max_exp);
t2 = add(R01Sqmax_exp, R1_exp);
t2 = sub(t1, t2);
if (t2>=0) a0 = L_shr(a0, t2);
if (t2<0) a1 = L_shl(a1, t2);
if (L_sub(a0, a1)>0)
{
R01Sqmax = R01Sq;
R01Sqmax_exp = R01Sq_exp;
R1max = R1n; R1max_exp = R1_exp;
ppnew = ppt;
R01max = R01;
}
}
/******************************************************************/
/* calculate all-zero pitch postfilter */
/******************************************************************/
if (R1max==0 || R0==0 || R01max <= 0)
{
aa = 0;
}
else
{
a0 = R1max_exp-16;
t1 = mult(R1max, R0norm);
a0 = a0+R0_exp-15;
sqrt_i(t1, (Word16)a0, &t1, &t2);
t0 = norm_l(R01max);
t3 = extract_h(L_shl(R01max, t0));
t0 = t0-16;
aa = mult(t3, t1);
t0 = t0+t2-15;
t0 = t0-15;
if (t0<0) aa = shl(aa, sub(0,t0));
else aa = shr(aa, t0);
}
a0 = L_mult(8192, aa);
a0 = L_mac(a0, 24576, *ma_a);
*ma_a = intround(a0);
if((*ma_a < ATHLD1) && (aa < (ATHLD2)))
aa = 0;
bb[1] = mult(ScLTPF, aa);
/******************************************************************/
/* calculate normalization energies */
/******************************************************************/
Rx = 0;
R0 = 0;
for(n=0; n<FRSZ; n++)
{
a0 = L_shl(s[XQOFF+n], 15);
a0 = L_add(a0, L_mult0(bb[1], s[XQOFF+n-ppnew]));
e[n] = intround(a0);
t1 = shr(e[n], shift);
t2 = shr(s[XQOFF+n], shift);
Rx = L_mac0(Rx, t1, t1);
R0 = L_mac0(R0, t2, t2);
}
R0 = L_shr(R0, 2);
if(R0 == 0 || Rx == 0)
gainn = 32767;
else
{
Rx_exp = norm_l(Rx);
t1 = extract_h(L_shl(Rx, Rx_exp));
t2 = extract_h(L_shl(R0, Rx_exp));
if (t2>= t1)
gainn = 32767;
else
{
t1 = div_s(t2, t1);
gainn = sqrts(t1);
}
}
/******************************************************************/
/* interpolate from the previous postfilter to the current */
/******************************************************************/
bb[0] = gainn;
bb[1] = mult(gainn, bb[1]);
step = (Word32)((1.0/(NINT+1))*(2147483648.0));
delta = 0;
for(n=0; n<NINT; n++)
{
delta = L_add(delta, step);
ww1 = intround(delta);
ww2 = add(sub(32767, ww1), 1);
/* interpolate between two filters */
bi0 = intround(L_mac(L_mult(ww1, bb[0]), ww2, b_prv[0]));
bi1c= mult(ww1, bb[1]);
bi1p= mult(ww2, b_prv[1]);
e[n] = intround(L_mac(L_mac(L_mult(bi1c, s[XQOFF+n-ppnew]), bi1p, s[XQOFF+n-(*pp_prv)]), bi0, s[XQOFF+n]));
}
for(n=NINT; n<FRSZ; n++)
{
e[n] = intround(L_shl(L_mult(gainn, e[n]),1));
}
/******************************************************************/
/* save state memory */
/******************************************************************/
*pp_prv = ppnew;
b_prv[0] = bb[0];
b_prv[1] = bb[1];
return;
}
Word16 G_pitch( /* (o) Q14 : Gain of pitch lag saturated to 1.2 */
Word16 xn[], /* (i) : Pitch target. */
Word16 y1[], /* (i) : Filtered adaptive codebook. */
Word16 g_coeff[], /* (i) : Correlations need for gain quantization. */
Word16 L_subfr /* (i) : Length of subframe. */
)
{
Word16 i;
Word16 xy, yy, exp_xy, exp_yy, gain;
Word32 s;
Word16 scaled_y1[L_SUBFR];
/* divide "y1[]" by 4 to avoid overflow */
for(i=0; i<L_subfr; i++)
scaled_y1[i] = shr(y1[i], 2);
/* Compute scalar product <y1[],y1[]> */
Overflow = 0;
s = 1; /* Avoid case of all zeros */
for(i=0; i<L_subfr; i++)
s = L_mac(s, y1[i], y1[i]);
if (Overflow == 0) {
exp_yy = norm_l(s);
yy = round( L_shl(s, exp_yy) );
}
else {
s = 1; /* Avoid case of all zeros */
for(i=0; i<L_subfr; i++)
s = L_mac(s, scaled_y1[i], scaled_y1[i]);
exp_yy = norm_l(s);
yy = round( L_shl(s, exp_yy) );
exp_yy = sub(exp_yy, 4);
}
/* Compute scalar product <xn[],y1[]> */
Overflow = 0;
s = 0;
for(i=0; i<L_subfr; i++)
s = L_mac(s, xn[i], y1[i]);
if (Overflow == 0) {
exp_xy = norm_l(s);
xy = round( L_shl(s, exp_xy) );
}
else {
s = 0;
for(i=0; i<L_subfr; i++)
s = L_mac(s, xn[i], scaled_y1[i]);
exp_xy = norm_l(s);
xy = round( L_shl(s, exp_xy) );
exp_xy = sub(exp_xy, 2);
}
g_coeff[0] = yy;
g_coeff[1] = sub(15, exp_yy);
g_coeff[2] = xy;
g_coeff[3] = sub(15, exp_xy);
/* If (xy <= 0) gain = 0 */
if (xy <= 0)
{
g_coeff[3] = -15; /* Force exp_xy to -15 = (15-30) */
return( (Word16) 0);
}
/* compute gain = xy/yy */
xy = shr(xy, 1); /* Be sure xy < yy */
gain = div_s( xy, yy);
i = sub(exp_xy, exp_yy);
gain = shr(gain, i); /* saturation if > 1.99 in Q14 */
/* if(gain >1.2) gain = 1.2 in Q14 */
if( sub(gain, 19661) > 0)
{
gain = 19661;
}
return(gain);
}
void pit_pst_filt(
int16_t *signal, /* (i) : input signal */
int16_t *scal_sig, /* (i) : input signal (scaled, divided by 4) */
int16_t t0_min, /* (i) : minimum value in the searched range */
int16_t t0_max, /* (i) : maximum value in the searched range */
int16_t L_subfr, /* (i) : size of filtering */
int16_t *signal_pst /* (o) : harmonically postfiltered signal */
)
{
int16_t i, j, t0;
int16_t g0, gain, cmax, en, en0;
int16_t *p, *p1, *deb_sig;
int32_t corr, cor_max, ener, ener0, temp;
int32_t L_temp;
/*---------------------------------------------------------------------------*
* Compute the correlations for all delays *
* and select the delay which maximizes the correlation *
*---------------------------------------------------------------------------*/
deb_sig = &scal_sig[-t0_min];
cor_max = MIN_32;
t0 = t0_min; /* Only to remove warning from some compilers */
for (i=t0_min; i<=t0_max; i++)
{
corr = 0;
p = scal_sig;
p1 = deb_sig;
for (j=0; j<L_subfr; j++)
corr = L_mac(corr, *p++, *p1++);
L_temp = L_sub(corr, cor_max);
if (L_temp > (int32_t)0)
{
cor_max = corr;
t0 = i;
}
deb_sig--;
}
/* Compute the energy of the signal delayed by t0 */
ener = 1;
p = scal_sig - t0;
for ( i=0; i<L_subfr ;i++, p++)
ener = L_mac(ener, *p, *p);
/* Compute the signal energy in the present subframe */
ener0 = 1;
p = scal_sig;
for ( i=0; i<L_subfr; i++, p++)
ener0 = L_mac(ener0, *p, *p);
if (cor_max < 0)
{
cor_max = 0;
}
/* scale "cor_max", "ener" and "ener0" on 16 bits */
temp = cor_max;
if (ener > temp)
{
temp = ener;
}
if (ener0 > temp)
{
temp = ener0;
}
j = norm_l(temp);
cmax = _round(L_shl(cor_max, j));
en = _round(L_shl(ener, j));
en0 = _round(L_shl(ener0, j));
/* prediction gain (dB)= -10 log(1-cor_max*cor_max/(ener*ener0)) */
/* temp = (cor_max * cor_max) - (0.5 * ener * ener0) */
temp = L_mult(cmax, cmax);
temp = L_sub(temp, L_shr(L_mult(en, en0), 1));
if (temp < (int32_t)0) /* if prediction gain < 3 dB */
{ /* switch off pitch postfilter */
for (i = 0; i < L_subfr; i++)
signal_pst[i] = signal[i];
return;
}
if (sub(cmax, en) > 0) /* if pitch gain > 1 */
{
g0 = INV_GAMMAP;
gain = GAMMAP_2;
}
else {
cmax = shr(mult(cmax, GAMMAP), 1); /* cmax(Q14) = cmax(Q15) * GAMMAP */
en = shr(en, 1); /* Q14 */
i = add(cmax, en);
if(i > 0)
{
gain = div_s(cmax, i); /* gain(Q15) = cor_max/(cor_max+ener) */
g0 = sub(32767, gain); /* g0(Q15) = 1 - gain */
}
else
{
g0 = 32767;
gain = 0;
}
}
for (i = 0; i < L_subfr; i++)
{
/* signal_pst[i] = g0*signal[i] + gain*signal[i-t0]; */
signal_pst[i] = add(mult(g0, signal[i]), mult(gain, signal[i-t0]));
}
return;
}
/*---------------------------------------------------------------------------*
* routine corr_xy2() *
* ~~~~~~~~~~~~~~~~~~~~ *
* Find the correlations between the target xn[], the filtered adaptive *
* codebook excitation y1[], and the filtered 1st codebook innovation y2[]. *
* g_coeff[2]:exp_g_coeff[2] = <y2,y2> *
* g_coeff[3]:exp_g_coeff[3] = -2<xn,y2> *
* g_coeff[4]:exp_g_coeff[4] = 2<y1,y2> *
*---------------------------------------------------------------------------*/
void Corr_xy2(
Word16 xn[], /* (i) Q0 :Target vector. */
Word16 y1[], /* (i) Q0 :Adaptive codebook. */
Word16 y2[], /* (i) Q12 :Filtered innovative vector. */
Word16 g_coeff[], /* (o) Q[exp]:Correlations between xn,y1,y2 */
Word16 exp_g_coeff[] /* (o) :Q-format of g_coeff[] */
)
{
Word16 i,exp;
Word16 exp_y2y2,exp_xny2,exp_y1y2;
Word16 y2y2, xny2, y1y2;
Word32 L_acc;
Word16 scaled_y2[L_SUBFR]; /* Q9 */
/*------------------------------------------------------------------*
* Scale down y2[] from Q12 to Q9 to avoid overflow *
*------------------------------------------------------------------*/
for(i=0; i<L_SUBFR; i++)
scaled_y2[i] = shr(y2[i], 3);
/* Compute scalar product <y2[],y2[]> */
L_acc = 1; /* Avoid case of all zeros */
for(i=0; i<L_SUBFR; i++)
L_acc = L_mac(L_acc, scaled_y2[i], scaled_y2[i]); /* L_acc:Q19 */
exp = norm_l(L_acc);
y2y2 = round( L_shl(L_acc, exp) );
exp_y2y2 = add(exp, 19-16); /* Q[19+exp-16] */
g_coeff[2] = y2y2;
exp_g_coeff[2] = exp_y2y2;
/* Compute scalar product <xn[],y2[]> */
L_acc = 1; /* Avoid case of all zeros */
for(i=0; i<L_SUBFR; i++)
L_acc = L_mac(L_acc, xn[i], scaled_y2[i]); /* L_acc:Q10 */
exp = norm_l(L_acc);
xny2 = round( L_shl(L_acc, exp) );
exp_xny2 = add(exp, 10-16); /* Q[10+exp-16] */
g_coeff[3] = negate(xny2);
exp_g_coeff[3] = sub(exp_xny2,1); /* -2<xn,y2> */
/* Compute scalar product <y1[],y2[]> */
L_acc = 1; /* Avoid case of all zeros */
for(i=0; i<L_SUBFR; i++)
L_acc = L_mac(L_acc, y1[i], scaled_y2[i]); /* L_acc:Q10 */
exp = norm_l(L_acc);
y1y2 = round( L_shl(L_acc, exp) );
exp_y1y2 = add(exp, 10-16); /* Q[10+exp-16] */
g_coeff[4] = y1y2;
exp_g_coeff[4] = sub(exp_y1y2,1); ; /* 2<y1,y2> */
return;
}
for(i=0; i<L_WINDOW; i++)
{
y[i] >>= 2;
sum += ((Word32)y[i] * (Word32)y[i]);
}
sum <<= 1;
sum += 1; /* Avoid case of all zeros */
if (sum > 0)
break;
}
}
else
sum += 1; /* Avoid case of all zeros */
/* Normalization of r[0] */
norm = norm_l (sum);
sum <<= norm;
/* Put in DPF format (see oper_32b) */
r_h[0] = (Word16)(sum >> 16);
r_l[0] = (Word16)((sum >> 1) - ((Word32)r_h[0] << 15));
/* r[1] to r[m] */
for (i = 1; i <= m; i++)
{
sum = 0;
for(j=0; j<L_WINDOW-i; j++)
sum += (Word32)y[j] * (Word32)y[j+i];
sum <<= norm + 1;
r_h[i] = (Word16)(sum >> 16);
r_l[i] = (Word16)((sum >> 1) - ((Word32)r_h[i] << 15));
static void Cor_h(
Word16 *H, /* (i) Q12 :Impulse response of filters */
Word16 *rr /* (o) :Correlations of H[] */
)
{
Word16 *rri0i0, *rri1i1, *rri2i2, *rri3i3, *rri4i4;
Word16 *rri0i1, *rri0i2, *rri0i3, *rri0i4;
Word16 *rri1i2, *rri1i3, *rri1i4;
Word16 *rri2i3, *rri2i4;
Word16 *p0, *p1, *p2, *p3, *p4;
Word16 *ptr_hd, *ptr_hf, *ptr_h1, *ptr_h2;
Word32 cor;
Word16 i, k, ldec, l_fin_sup, l_fin_inf;
Word16 h[L_SUBFR];
/* Scaling h[] for maximum precision */
cor = 0;
for(i=0; i<L_SUBFR; i++)
cor = L_mac(cor, H[i], H[i]);
if(sub(extract_h(cor),32000) > 0)
{
for(i=0; i<L_SUBFR; i++) {
h[i] = shr(H[i], 1);
}
}
else
{
k = norm_l(cor);
k = shr(k, 1);
for(i=0; i<L_SUBFR; i++) {
h[i] = shl(H[i], k);
}
}
/*------------------------------------------------------------*
* Compute rri0i0[], rri1i1[], rri2i2[], rri3i3 and rri4i4[] *
*------------------------------------------------------------*/
/* Init pointers */
rri0i0 = rr;
rri1i1 = rri0i0 + NB_POS;
rri2i2 = rri1i1 + NB_POS;
rri3i3 = rri2i2 + NB_POS;
rri4i4 = rri3i3 + NB_POS;
rri0i1 = rri4i4 + NB_POS;
rri0i2 = rri0i1 + MSIZE;
rri0i3 = rri0i2 + MSIZE;
rri0i4 = rri0i3 + MSIZE;
rri1i2 = rri0i4 + MSIZE;
rri1i3 = rri1i2 + MSIZE;
rri1i4 = rri1i3 + MSIZE;
rri2i3 = rri1i4 + MSIZE;
rri2i4 = rri2i3 + MSIZE;
p0 = rri0i0 + NB_POS-1; /* Init pointers to last position of rrixix[] */
p1 = rri1i1 + NB_POS-1;
p2 = rri2i2 + NB_POS-1;
p3 = rri3i3 + NB_POS-1;
p4 = rri4i4 + NB_POS-1;
ptr_h1 = h;
cor = 0;
for(i=0; i<NB_POS; i++)
{
cor = L_mac(cor, *ptr_h1, *ptr_h1); ptr_h1++;
*p4-- = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h1); ptr_h1++;
*p3-- = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h1); ptr_h1++;
*p2-- = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h1); ptr_h1++;
*p1-- = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h1); ptr_h1++;
*p0-- = extract_h(cor);
}
/*-----------------------------------------------------------------*
* Compute elements of: rri2i3[], rri1i2[], rri0i1[] and rri0i4[] *
*-----------------------------------------------------------------*/
l_fin_sup = MSIZE-1;
l_fin_inf = l_fin_sup-(Word16)1;
ldec = NB_POS+1;
ptr_hd = h;
ptr_hf = ptr_hd + 1;
for(k=0; k<NB_POS; k++) {
p3 = rri2i3 + l_fin_sup;
p2 = rri1i2 + l_fin_sup;
p1 = rri0i1 + l_fin_sup;
p0 = rri0i4 + l_fin_inf;
cor = 0;
ptr_h1 = ptr_hd;
ptr_h2 = ptr_hf;
for(i=k+(Word16)1; i<NB_POS; i++ ) {
cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
*p3 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
*p2 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
*p1 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
*p0 = extract_h(cor);
p3 -= ldec;
p2 -= ldec;
p1 -= ldec;
p0 -= ldec;
}
cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
*p3 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
*p2 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
*p1 = extract_h(cor);
l_fin_sup -= NB_POS;
l_fin_inf--;
ptr_hf += STEP;
}
/*---------------------------------------------------------------------*
* Compute elements of: rri2i4[], rri1i3[], rri0i2[], rri1i4[], rri0i3 *
*---------------------------------------------------------------------*/
ptr_hd = h;
ptr_hf = ptr_hd + 2;
l_fin_sup = MSIZE-1;
l_fin_inf = l_fin_sup-(Word16)1;
for(k=0; k<NB_POS; k++) {
p4 = rri2i4 + l_fin_sup;
p3 = rri1i3 + l_fin_sup;
p2 = rri0i2 + l_fin_sup;
p1 = rri1i4 + l_fin_inf;
p0 = rri0i3 + l_fin_inf;
cor = 0;
ptr_h1 = ptr_hd;
ptr_h2 = ptr_hf;
for(i=k+(Word16)1; i<NB_POS; i++ ) {
cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
*p4 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
*p3 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
*p2 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
*p1 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
*p0 = extract_h(cor);
p4 -= ldec;
p3 -= ldec;
p2 -= ldec;
p1 -= ldec;
p0 -= ldec;
}
cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
*p4 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
*p3 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
*p2 = extract_h(cor);
l_fin_sup -= NB_POS;
l_fin_inf--;
ptr_hf += STEP;
}
/*----------------------------------------------------------------------*
* Compute elements of: rri1i4[], rri0i3[], rri2i4[], rri1i3[], rri0i2 *
*----------------------------------------------------------------------*/
ptr_hd = h;
ptr_hf = ptr_hd + 3;
l_fin_sup = MSIZE-1;
l_fin_inf = l_fin_sup-(Word16)1;
for(k=0; k<NB_POS; k++) {
p4 = rri1i4 + l_fin_sup;
p3 = rri0i3 + l_fin_sup;
p2 = rri2i4 + l_fin_inf;
p1 = rri1i3 + l_fin_inf;
p0 = rri0i2 + l_fin_inf;
ptr_h1 = ptr_hd;
ptr_h2 = ptr_hf;
cor = 0;
for(i=k+(Word16)1; i<NB_POS; i++ ) {
cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
*p4 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
*p3 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
*p2 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
*p1 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
*p0 = extract_h(cor);
p4 -= ldec;
p3 -= ldec;
p2 -= ldec;
p1 -= ldec;
p0 -= ldec;
}
cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
*p4 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
*p3 = extract_h(cor);
l_fin_sup -= NB_POS;
l_fin_inf--;
ptr_hf += STEP;
}
/*----------------------------------------------------------------------*
* Compute elements of: rri0i4[], rri2i3[], rri1i2[], rri0i1[] *
*----------------------------------------------------------------------*/
ptr_hd = h;
ptr_hf = ptr_hd + 4;
l_fin_sup = MSIZE-1;
l_fin_inf = l_fin_sup-(Word16)1;
for(k=0; k<NB_POS; k++) {
p3 = rri0i4 + l_fin_sup;
p2 = rri2i3 + l_fin_inf;
p1 = rri1i2 + l_fin_inf;
p0 = rri0i1 + l_fin_inf;
ptr_h1 = ptr_hd;
ptr_h2 = ptr_hf;
cor = 0;
for(i=k+(Word16)1; i<NB_POS; i++ ) {
cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
*p3 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
*p2 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
*p1 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
*p0 = extract_h(cor);
p3 -= ldec;
p2 -= ldec;
p1 -= ldec;
p0 -= ldec;
}
cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
*p3 = extract_h(cor);
l_fin_sup -= NB_POS;
l_fin_inf--;
ptr_hf += STEP;
}
return;
}
void GetExc800bps(
/* 1 */ short *output,
/* 2 */ short *best,
/* 3 */ short scale,
/* 4 */ short *input,
/* 5 */ short length,
/* 6 */ short flag,
/* 7 */ short n)
{
short k, j;
short D;
long tmp;
long ltmp;
short stmp;
short *ptr;
long sum, lscale;
short ssum, sscale;
static short Seed = 1234;
static short Sum[NoOfSubFrames];
short shft_scale;
short shft_sum;
short stemp1;
if (!flag)
Seed = 1234;
/*
* sum is an integer value, not a fractional value.
*/
/* Get energy of next sub frame */
for (k = 0, sum = 0; k < length; k++)
{
sum = L_add(sum, L_deposit_l(abs_s(input[k])));
}
sum = L_shl(sum, 1); /* correct for scaling */
if (sum < SubFrameSize)
{
sum = fnLog10(L_deposit_h(scale));
sum = L_negate(L_add(L_shl(sum, 3), 484942745)); /* add (log8)/4 = 484842745 (VALUE ADJUSTED TO BETTER MATCH FLOAT MODEL) */
}
else
{
lscale = L_mult(SubFrameSize, scale);
shft_scale = norm_l(lscale);
sscale = round(L_shl(lscale, shft_scale));
shft_sum = norm_l(sum) - 1;
ssum = round(L_shl(sum, shft_sum));
/*
* The following divide/L_shl produced sum scaled down by 64.
* sum can have a max value of 40.xx in the sample data.
*/
/* sum = L_shl(L_divide(sum, lscale), (shft_scale + 7 - shft_sum)); */
sum = L_deposit_h(shl(divide_s(ssum, sscale), (shft_scale - shft_sum)));
/* sum = fnLog10(sum); */
sum = L_add(fnLog10(sum), 141412467); /* add log (2^7) scaled down by 32 */
sum = L_add(L_shl(sum, 3), 969485490); /* add (log8)/2 = 969685490 (VALUE ADJUSTED TO BETTER MATCH FLOAT MODEL) */
}
/*
* Sum scaled down by 4.
*/
Sum[n] = round(sum);
/* Quantize if last frame */
if (n == NoOfSubFrames - 1)
{
/* Quantize to 8 bits */
for (k = 0, sum = 2147483647, ptr = Logqtbl; k < 256; k++)
{
for (j = 0, tmp = 0; j < 3; j++)
{
/*
* Sum and Logqtbl both scaled down by 4.
* Change Logqtbl to short if Lw not required.
*/
D = sub(Sum[j], (*ptr++));
tmp = L_mac(tmp, D, D);
}
if (tmp < sum)
{
ltmp = sum;
sum = tmp;
*best = k;
}
}
for (j = 0; j < 3; j++)
{
Sum[j] = Powqtbl[*best * 3 + j];
}
/* Get excitation */
j = FrameSize - ACBMemSize;
for (k = 0; k < FrameSize - 1; k++)
{
if (k >= j)
{
stmp = e_ran_g(&Seed);
stemp1 = k / (length - 1);
output[k - j] = round(L_shr(L_mult(stmp, Sum[stemp1]), 5));
}
}
stmp = e_ran_g(&Seed);
output[k - j] = round(L_shr(L_mult(stmp, Sum[2]), 5)); /* last excitation */
}
}
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;
}
/*----------------------------------------------------------------------------
* scale_st - control of the subframe gain
* gain[n] = AGC_FAC * gain[n-1] + (1 - AGC_FAC) g_in/g_out
*----------------------------------------------------------------------------
*/
static void scale_st(
Word16 *sig_in, /* input : postfilter input signal */
Word16 *sig_out, /* in/out: postfilter output signal */
Word16 *gain_prec /* in/out: last value of gain for subframe */
)
{
int i;
Word16 scal_in, scal_out;
Word32 L_acc, L_temp;
Word16 s_g_in, s_g_out, temp, sh_g0, g0;
Word16 gain;
/* compute input gain */
L_acc = 0L;
for(i=0; i<L_SUBFR; i++) {
L_temp = L_abs(L_deposit_l(sig_in[i]));
L_acc = L_add(L_acc, L_temp);
}
if(L_acc == 0L) {
g0 = 0;
}
else {
scal_in = norm_l(L_acc);
L_acc = L_shl(L_acc, scal_in);
s_g_in = extract_h(L_acc); /* normalized */
/* Compute output gain */
L_acc = 0L;
for(i=0; i<L_SUBFR; i++) {
L_temp = L_abs(L_deposit_l(sig_out[i]));
L_acc = L_add(L_acc, L_temp);
}
if(L_acc == 0L) {
*gain_prec = 0;
return;
}
scal_out = norm_l(L_acc);
L_acc = L_shl(L_acc, scal_out);
s_g_out = extract_h(L_acc); /* normalized */
sh_g0 = add(scal_in, 1);
sh_g0 = sub(sh_g0, scal_out); /* scal_in - scal_out + 1 */
if(sub(s_g_in ,s_g_out)<0) {
g0 = div_s(s_g_in, s_g_out); /* s_g_in/s_g_out in Q15 */
}
else {
temp = sub(s_g_in, s_g_out); /* sufficient since normalized */
g0 = shr(div_s(temp, s_g_out), 1);
g0 = add(g0, (Word16)0x4000);/* s_g_in/s_g_out in Q14 */
sh_g0 = sub(sh_g0, 1);
}
/* L_gain_in/L_gain_out in Q14 */
/* overflows if L_gain_in > 2 * L_gain_out */
g0 = shr(g0, sh_g0); /* sh_g0 may be >0, <0, or =0 */
g0 = mult_r(g0, AGC_FAC1); /* L_gain_in/L_gain_out * AGC_FAC1 */
}
/* gain(n) = AGC_FAC gain(n-1) + AGC_FAC1 gain_in/gain_out */
/* sig_out(n) = gain(n) sig_out(n) */
gain = *gain_prec;
for(i=0; i<L_SUBFR; i++) {
temp = mult_r(AGC_FAC, gain);
gain = add(temp, g0); /* in Q14 */
L_temp = L_mult(gain, sig_out[i]);
L_temp = L_shl(L_temp, 1);
sig_out[i] = round(L_temp);
}
*gain_prec = gain;
return;
}
/*----------------------------------------------------------------------------
* compute_ltp_l : compute delayed signal,
num & den of gain for fractional delay
* with long interpolation filter
*----------------------------------------------------------------------------
*/
static void compute_ltp_l(
Word16 *s_in, /* input signal with past*/
Word16 ltpdel, /* delay factor */
Word16 phase, /* phase factor */
Word16 *y_up, /* delayed signal */
Word16 *num, /* numerator of LTP gain */
Word16 *den, /* denominator of LTP gain */
Word16 *sh_num, /* justification factor of num */
Word16 *sh_den /* justification factor of den */
)
{
/**** Table of constants declared externally */
extern Word16 tab_hup_l[SIZ_TAB_HUP_L];
/* Pointer on table of constants */
Word16 *ptr_h;
/* Local variables */
int n, i;
Word16 *ptr2;
Word16 temp;
Word32 L_acc;
temp = sub(phase, 1);
temp = shl(temp, L2_LH2_L);
ptr_h = tab_hup_l + temp; /* tab_hup_l + LH2_L * (phase-1) */
temp = sub(LH_UP_L, ltpdel);
ptr2 = s_in + temp; ;
/* Compute y_up */
for(n = 0; n<L_SUBFR; n++) {
L_acc = 0L;
for(i=0; i<LH2_L; i++) {
L_acc = L_mac(L_acc, ptr_h[i], (*ptr2--));
}
y_up[n] = round(L_acc);
ptr2 += LH2_L_P1;
}
/* Compute num */
L_acc = 0L;
for(n=0; n<L_SUBFR; n++) {
L_acc = L_mac(L_acc, y_up[n], s_in[n]);
}
if(L_acc < 0L) {
*num = 0;
*sh_num = 0;
}
else {
temp = sub(16, norm_l(L_acc));
if(temp < 0) {
temp = 0;
}
L_acc = L_shr(L_acc, temp); /* with temp >= 0 */
*num = extract_l(L_acc);
*sh_num = temp;
}
/* Compute den */
L_acc = 0L;
for(n=0; n<L_SUBFR; n++) {
L_acc = L_mac(L_acc, y_up[n], y_up[n]);
}
temp = sub(16, norm_l(L_acc));
if(temp < 0) {
temp = 0;
}
L_acc = L_shr(L_acc, temp); /* with temp >= 0 */
*den = extract_l(L_acc);
*sh_den = temp;
return;
}
/*----------------------------------------------------------------------------
* search_del: computes best (shortest) integer LTP delay + fine search
*----------------------------------------------------------------------------
*/
static void search_del(
Word16 t0, /* input : pitch delay given by coder */
Word16 *ptr_sig_in, /* input : input signal (with delay line) */
Word16 *ltpdel, /* output: delay = *ltpdel - *phase / f_up */
Word16 *phase, /* output: phase */
Word16 *num_gltp, /* output: 16 bits numerator of LTP gain */
Word16 *den_gltp, /* output: 16 bits denominator of LTP gain */
Word16 *sh_num_gltp, /* output: justification for num_gltp */
Word16 *sh_den_gltp, /* output: justification for den_gltp */
Word16 *y_up, /* output: LT delayed signal if fract. delay */
Word16 *off_yup /* output: offset in y_up */
)
{
/* Tables of constants */
extern Word16 tab_hup_s[SIZ_TAB_HUP_S];
Word32 L_den0[F_UP_PST-1];
Word32 L_den1[F_UP_PST-1];
Word32 *ptr_L_den0, *ptr_L_den1;
int i, n;
Word16 *ptr_h;
Word16 *ptr_sig_past, *ptr_sig_past0;
Word16 *ptr1, *ptr_y_up;
Word16 num, den0, den1;
Word16 den_max, num_max;
Word32 L_num_int, L_den_int, L_den_max;
Word16 hi_numsq, hi_numsq_max;
Word16 lo_numsq, lo_numsq_max;
Word16 ener;
Word16 sh_num, sh_den, sh_ener;
Word16 i_max, lambda, phi, phi_max, ioff;
Word16 temp;
Word32 L_temp0, L_temp1;
Word32 L_acc;
Word32 L_temp;
/* Computes energy of current signal */
/*************************************/
L_acc = 0L;
for(i=0; i<L_SUBFR; i++) {
L_acc = L_mac( L_acc, ptr_sig_in[i] , ptr_sig_in[i]);
}
if(L_acc == 0) {
*num_gltp = 0;
*den_gltp = 1;
*ltpdel = 0;
*phase = 0;
return;
}
sh_ener = sub(16, norm_l(L_acc));
/* save energy for final decision */
if(sh_ener > 0) {
ener = extract_l(L_shr(L_acc, sh_ener));
}
else {
sh_ener = 0;
ener = extract_l(L_acc);
}
/*************************************/
/* Selects best of 3 integer delays */
/* Maximum of 3 numerators around t0 */
/*************************************/
lambda = sub(t0,1);
ptr_sig_past = ptr_sig_in - lambda;
L_num_int = -1L;
/* initialization used only to suppress Microsoft Visual C++ warnings */
i_max = (Word16)0;
for(i=0; i<3; i++) {
L_acc = 0L;
for(n=0; n<L_SUBFR; n++) {
L_acc = L_mac( L_acc, ptr_sig_in[n] , ptr_sig_past[n]);
}
if(L_acc < 0) {
L_acc = 0L;
}
L_temp =L_sub(L_acc ,L_num_int);
if(L_temp > 0L) {
L_num_int = L_acc;
i_max = (Word16)i;
}
ptr_sig_past--;
}
if(L_num_int == 0) {
*num_gltp = 0;
*den_gltp = 1;
*ltpdel = 0;
*phase = 0;
return;
}
/* Compute den for i_max */
lambda = add(lambda, (Word16)i_max);
ptr_sig_past = ptr_sig_in - lambda;
L_acc = 0L;
for(i=0; i<L_SUBFR; i++) {
temp = *ptr_sig_past++;
L_acc = L_mac( L_acc, temp, temp);
}
if(L_acc == 0L) {
*num_gltp = 0;
*den_gltp = 1;
*ltpdel = 0;
*phase = 0;
return;
}
L_den_int = L_acc;
/***********************************/
/* Select best phase around lambda */
/***********************************/
/* Compute y_up & denominators */
/*******************************/
ptr_y_up = y_up;
L_den_max = L_den_int;
ptr_L_den0 = L_den0;
ptr_L_den1 = L_den1;
ptr_h = tab_hup_s;
temp = sub(lambda, LH_UP_SM1);
ptr_sig_past0 = ptr_sig_in - temp;
/* Loop on phase */
for(phi=1; phi<F_UP_PST; phi++) {
/* Compute y_up for lambda+1 - phi/F_UP_PST */
/* and lambda - phi/F_UP_PST */
ptr_sig_past = ptr_sig_past0;
for(n = 0; n<=L_SUBFR; n++) {
ptr1 = ptr_sig_past++;
L_acc = 0L;
for(i=0; i<LH2_S; i++) {
L_acc = L_mac(L_acc, ptr_h[i], ptr1[-i]);
}
ptr_y_up[n] = round(L_acc);
}
/* compute den0 (lambda+1) and den1 (lambda) */
/* part common to den0 and den1 */
L_acc = 0L;
for(n=1; n<L_SUBFR; n++) {
L_acc = L_mac(L_acc, ptr_y_up[n] ,ptr_y_up[n]);
}
L_temp0 = L_acc; /* saved for den1 */
/* den0 */
L_acc = L_mac(L_acc, ptr_y_up[0] ,ptr_y_up[0]);
*ptr_L_den0 = L_acc;
/* den1 */
L_acc = L_mac(L_temp0, ptr_y_up[L_SUBFR] ,ptr_y_up[L_SUBFR]);
*ptr_L_den1 = L_acc;
if(sub(abs_s(ptr_y_up[0]),abs_s(ptr_y_up[L_SUBFR])) >0) {
L_temp =L_sub(*ptr_L_den0 ,L_den_max );
if(L_temp> 0L) {
L_den_max = *ptr_L_den0;
}
}
else {
L_temp =L_sub(*ptr_L_den1 ,L_den_max );
if(L_temp> 0L) {
L_den_max = *ptr_L_den1;
}
}
ptr_L_den0++;
ptr_L_den1++;
ptr_y_up += L_SUBFRP1;
ptr_h += LH2_S;
}
if(L_den_max == 0) {
*num_gltp = 0;
*den_gltp = 1;
*ltpdel = 0;
*phase = 0;
return;
}
sh_den = sub(16, norm_l(L_den_max));
/* if sh_den <= 0 : dynamic between current frame */
/* and delay line too high */
if(sh_den <= 0) {
*num_gltp = 0;
*den_gltp = 1;
*ltpdel = 0;
*phase = 0;
return;
}
/* search sh_num to justify correlations */
/* sh_num = Max(sh_den, sh_ener) */
sh_num = (sub( sh_den , sh_ener)>=0) ? sh_den : sh_ener;
/* Computation of the numerators */
/* and selection of best num*num/den */
/* for non null phases */
/* Initialize with null phase */
L_acc = L_shr(L_den_int, sh_den); /* sh_den > 0 */
den_max = extract_l(L_acc);
L_acc = L_shr(L_num_int, sh_num); /* sh_num > 0 */
num_max = extract_l(L_acc);
L_acc = L_mult(num_max, num_max);
L_Extract(L_acc, &hi_numsq_max, &lo_numsq_max);
phi_max = 0;
ioff = 1;
ptr_L_den0 = L_den0;
ptr_L_den1 = L_den1;
ptr_y_up = y_up;
/* if den_max = 0 : will be selected and declared unvoiced */
/* if num!=0 & den=0 : will be selected and declared unvoiced */
/* degenerated seldom cases, switch off LT is OK */
/* Loop on phase */
for(phi=1; phi<F_UP_PST; phi++) {
/* compute num for lambda+1 - phi/F_UP_PST */
L_acc = 0L;
for(n = 0; n<L_SUBFR; n++) {
L_acc = L_mac(L_acc, ptr_sig_in[n] ,ptr_y_up[n]);
}
L_acc = L_shr(L_acc, sh_num); /* sh_num > 0 */
if(L_acc < 0L) {
num = 0;
}
else {
num = extract_l(L_acc);
}
/* selection if num**2/den0 max */
L_acc = L_mult(num, num);
L_Extract(L_acc, &hi_numsq, &lo_numsq);
L_temp0 = Mpy_32_16(hi_numsq, lo_numsq, den_max);
L_acc = *ptr_L_den0++;
L_acc = L_shr(L_acc, sh_den); /* sh_den > 0 */
den0 = extract_l(L_acc);
L_temp1 = Mpy_32_16(hi_numsq_max, lo_numsq_max, den0);
L_temp = L_sub(L_temp0, L_temp1);
if(L_temp>0L) {
num_max = num;
hi_numsq_max = hi_numsq;
lo_numsq_max = lo_numsq;
den_max = den0;
ioff = 0;
phi_max = phi;
}
/* compute num for lambda - phi/F_UP_PST */
ptr_y_up++;
L_acc = 0L;
for(n = 0; n<L_SUBFR; n++) {
L_acc = L_mac(L_acc, ptr_sig_in[n] ,ptr_y_up[n]);
}
L_acc = L_shr(L_acc, sh_num); /* sh_num > 0 */
if(L_acc < 0L) {
num = 0;
}
else {
num = extract_l(L_acc);
}
/* selection if num**2/den1 max */
L_acc = L_mult(num, num);
L_Extract(L_acc, &hi_numsq, &lo_numsq);
L_temp0 = Mpy_32_16(hi_numsq, lo_numsq, den_max);
L_acc = *ptr_L_den1++;
L_acc = L_shr(L_acc, sh_den); /* sh_den > 0 */
den1 = extract_l(L_acc);
L_temp1 = Mpy_32_16(hi_numsq_max, lo_numsq_max, den1);
L_temp = L_sub(L_temp0,L_temp1);
if(L_temp> 0L) {
num_max = num;
hi_numsq_max = hi_numsq;
lo_numsq_max = lo_numsq;
den_max = den1;
ioff = 1;
phi_max = phi;
}
ptr_y_up += L_SUBFR;
}
/***************************************************/
/*** test if normalized crit0[iopt] > THRESHCRIT ***/
/***************************************************/
if((num_max == 0) || (sub(den_max,1) <= 0)) {
*num_gltp = 0;
*den_gltp = 1;
*ltpdel = 0;
*phase = 0;
return;
}
/* compare num**2 */
/* to ener * den * 0.5 */
/* (THRESHCRIT = 0.5) */
L_temp1 = L_mult(den_max, ener);
L_temp0 = L_Comp(hi_numsq_max, lo_numsq_max);
/* temp = 2 * sh_num - sh_den - sh_ener + 1 */
/* 16 bits with no overflows */
temp = shl(sh_num,1);
temp = sub(temp, sh_den);
temp = sub(temp, sh_ener);
temp = add(temp, 1);
if(temp < 0) {
temp = negate(temp); /* no overflow */
L_temp0 = L_shr(L_temp0, temp);
}
else {
if(temp > 0) L_temp1 = L_shr(L_temp1, temp);
}
L_temp = L_sub(L_temp0 ,L_temp1);
if(L_temp >= 0L) {
temp = add(lambda, 1);
*ltpdel = sub(temp, ioff);
*off_yup = ioff;
*phase = phi_max;
*num_gltp = num_max;
*den_gltp = den_max;
*sh_den_gltp = sh_den;
*sh_num_gltp = sh_num;
}
else {
*num_gltp = 0;
*den_gltp = 1;
*ltpdel = 0;
*phase = 0;
}
return;
}
static void Cor_h_X(
Word16 h[], /* (i) Q12 :Impulse response of filters */
Word16 X[], /* (i) :Target vector */
Word16 D[] /* (o) :Correlations between h[] and D[] */
/* Normalized to 13 bits */
)
{
Word16 i, j;
Word32 s, max, L_temp;
Word32 y32[L_SUBFR];
/* first keep the result on 32 bits and find absolute maximum */
max = 0;
for (i = 0; i < L_SUBFR; i++)
{
s = 0;
for (j = i; j < L_SUBFR; j++)
s = L_mac(s, X[j], h[j-i]);
y32[i] = s;
s = L_abs(s);
L_temp =L_sub(s,max);
if(L_temp>0L) {
max = s;
}
}
/* Find the number of right shifts to do on y32[] */
/* so that maximum is on 13 bits */
j = norm_l(max);
if( sub(j,16) > 0) {
j = 16;
}
j = sub(18, j);
for(i=0; i<L_SUBFR; i++) {
D[i] = extract_l( L_shr(y32[i], j) );
}
return;
}
static void Cor_h_D(
Word16 *H, /* (i) Q12 :Impulse response of filters */
Word16 *rr /* (o) :Correlations of H[] */
)
{
Word16 *rri0i0, *rri1i1, *rri2i2, *rri3i3, *rri4i4;
Word16 *rri0i1, *rri0i2, *rri0i3, *rri0i4;
Word16 *rri1i2, *rri1i3, *rri1i4;
Word16 *rri2i3, *rri2i4;
Word16 *p0, *p1, *p2, *p3, *p4;
Word16 *ptr_hd, *ptr_hf, *ptr_h1, *ptr_h2;
Word32 cor;
Word16 i, k, ldec, l_fin_sup, l_fin_inf;
Word16 h[L_SUBFR];
Word32 L_tmp;
Word16 lsym;
/* Scaling h[] for maximum precision */
cor = 0;
for(i=0; i<L_SUBFR; i++)
cor = L_mac(cor, H[i], H[i]);
L_tmp = L_sub(extract_h(cor),32000);
if(L_tmp>0L )
{
for(i=0; i<L_SUBFR; i++) {
h[i] = shr(H[i], 1);
}
}
else
{
k = norm_l(cor);
k = shr(k, 1);
for(i=0; i<L_SUBFR; i++) {
h[i] = shl(H[i], k);
}
}
/*-----------------------------------------------------------------*
* In case of G729 mode, nine cross correlations has to be *
* calculated, namely the following: *
* *
* rri0i1[], *
* rri0i2[], rri1i2[], *
* rri0i3[], rri1i3[], rri2i3[], *
* rri0i4[], rri1i4[], rri2i4[], *
* *
* In case of G729 on 6.4 kbps mode, three of the above nine cross *
* correlations are not needed for the codebook search, namely *
* rri0i2[], rri0i4[] and rri2i4[]. Two of these three 64-element *
* positions are instead used by two cross correlations needed *
* only by the 6.4 kbps mode (see D2i40_11() for details). *
*-----------------------------------------------------------------*/
/* Init pointers */
rri0i0 = rr;
rri1i1 = rri0i0 + NB_POS;
rri2i2 = rri1i1 + NB_POS;
rri3i3 = rri2i2 + NB_POS;
rri4i4 = rri3i3 + NB_POS;
rri0i1 = rri4i4 + NB_POS;
rri0i2 = rri0i1 + MSIZE; /* Holds RRi1i1[] in 6.4 kbps mode */
rri0i3 = rri0i2 + MSIZE;
rri0i4 = rri0i3 + MSIZE; /* Holds RRi3i4[] in 6.4 kbps mode */
rri1i2 = rri0i4 + MSIZE;
rri1i3 = rri1i2 + MSIZE;
rri1i4 = rri1i3 + MSIZE;
rri2i3 = rri1i4 + MSIZE;
rri2i4 = rri2i3 + MSIZE; /* Not used in 6.4 kbps mode */
/*------------------------------------------------------------*
* Compute rri0i0[], rri1i1[], rri2i2[], rri3i3 and rri4i4[] *
*------------------------------------------------------------*/
p0 = rri0i0 + NB_POS-1; /* Init pointers to last position of rrixix[] */
p1 = rri1i1 + NB_POS-1;
p2 = rri2i2 + NB_POS-1;
p3 = rri3i3 + NB_POS-1;
p4 = rri4i4 + NB_POS-1;
ptr_h1 = h;
cor = 0;
for(i=0; i<NB_POS; i++)
{
cor = L_mac(cor, *ptr_h1, *ptr_h1);
ptr_h1++;
*p4-- = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h1);
ptr_h1++;
*p3-- = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h1);
ptr_h1++;
*p2-- = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h1);
ptr_h1++;
*p1-- = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h1);
ptr_h1++;
*p0-- = extract_h(cor);
}
/*-----------------------------------------------------------------*
* Compute elements of: rri2i3[], rri1i2[], rri0i1[] and rri0i4[] *
*-----------------------------------------------------------------*/
l_fin_sup = MSIZE-1;
l_fin_inf = l_fin_sup-(Word16)1;
ldec = NB_POS+1;
ptr_hd = h;
ptr_hf = ptr_hd + 1;
for(k=0; k<NB_POS; k++) {
p4 = rri0i4 + l_fin_sup;
p3 = rri2i3 + l_fin_sup;
p2 = rri1i2 + l_fin_sup;
p1 = rri0i1 + l_fin_sup;
p0 = rri0i4 + l_fin_inf;
cor = 0;
ptr_h1 = ptr_hd;
ptr_h2 = ptr_hf;
for(i=k+(Word16)1; i<NB_POS; i++ ) {
cor = L_mac(cor, *ptr_h1, *ptr_h2);
ptr_h1++;
ptr_h2++;
if (sub(CODEC_MODE, 1) == 0) *p4 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2);
ptr_h1++;
ptr_h2++;
*p3 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2);
ptr_h1++;
ptr_h2++;
*p2 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2);
ptr_h1++;
ptr_h2++;
*p1 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2);
ptr_h1++;
ptr_h2++;
if (sub(CODEC_MODE, 2) == 0) *p0 = extract_h(cor);
p4 -= ldec;
p3 -= ldec;
p2 -= ldec;
p1 -= ldec;
p0 -= ldec;
}
cor = L_mac(cor, *ptr_h1, *ptr_h2);
ptr_h1++;
ptr_h2++;
if (sub(CODEC_MODE, 1) == 0) *p4 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2);
ptr_h1++;
ptr_h2++;
*p3 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2);
ptr_h1++;
ptr_h2++;
*p2 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2);
ptr_h1++;
ptr_h2++;
*p1 = extract_h(cor);
l_fin_sup -= NB_POS;
l_fin_inf--;
ptr_hf += STEP;
}
/*---------------------------------------------------------------------*
* Compute elements of: rri2i4[], rri1i3[], rri0i2[], rri1i4[], rri0i3 *
*---------------------------------------------------------------------*/
ptr_hd = h;
ptr_hf = ptr_hd + 2;
l_fin_sup = MSIZE-1;
l_fin_inf = l_fin_sup-(Word16)1;
for(k=0; k<NB_POS; k++) {
p4 = rri2i4 + l_fin_sup;
p3 = rri1i3 + l_fin_sup;
p2 = rri0i2 + l_fin_sup;
p1 = rri1i4 + l_fin_inf;
p0 = rri0i3 + l_fin_inf;
cor = 0;
ptr_h1 = ptr_hd;
ptr_h2 = ptr_hf;
for(i=k+(Word16)1; i<NB_POS; i++ ) {
cor = L_mac(cor, *ptr_h1, *ptr_h2);
ptr_h1++;
ptr_h2++;
*p4 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2);
ptr_h1++;
ptr_h2++;
*p3 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2);
ptr_h1++;
ptr_h2++;
*p2 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2);
ptr_h1++;
ptr_h2++;
*p1 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2);
ptr_h1++;
ptr_h2++;
*p0 = extract_h(cor);
p4 -= ldec;
p3 -= ldec;
p2 -= ldec;
p1 -= ldec;
p0 -= ldec;
}
cor = L_mac(cor, *ptr_h1, *ptr_h2);
ptr_h1++;
ptr_h2++;
*p4 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2);
ptr_h1++;
ptr_h2++;
*p3 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2);
ptr_h1++;
ptr_h2++;
*p2 = extract_h(cor);
l_fin_sup -= NB_POS;
l_fin_inf--;
ptr_hf += STEP;
}
/*----------------------------------------------------------------------*
* Compute elements of: rri1i4[], rri0i3[], rri2i4[], rri1i3[], rri0i2 *
*----------------------------------------------------------------------*/
ptr_hd = h;
ptr_hf = ptr_hd + 3;
l_fin_sup = MSIZE-1;
l_fin_inf = l_fin_sup-(Word16)1;
for(k=0; k<NB_POS; k++) {
p4 = rri1i4 + l_fin_sup;
p3 = rri0i3 + l_fin_sup;
p2 = rri2i4 + l_fin_inf;
p1 = rri1i3 + l_fin_inf;
p0 = rri0i2 + l_fin_inf;
ptr_h1 = ptr_hd;
ptr_h2 = ptr_hf;
cor = 0;
for(i=k+(Word16)1; i<NB_POS; i++ ) {
cor = L_mac(cor, *ptr_h1, *ptr_h2);
ptr_h1++;
ptr_h2++;
*p4 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2);
ptr_h1++;
ptr_h2++;
*p3 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2);
ptr_h1++;
ptr_h2++;
*p2 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2);
ptr_h1++;
ptr_h2++;
*p1 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2);
ptr_h1++;
ptr_h2++;
*p0 = extract_h(cor);
p4 -= ldec;
p3 -= ldec;
p2 -= ldec;
p1 -= ldec;
p0 -= ldec;
}
cor = L_mac(cor, *ptr_h1, *ptr_h2);
ptr_h1++;
ptr_h2++;
*p4 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2);
ptr_h1++;
ptr_h2++;
*p3 = extract_h(cor);
l_fin_sup -= NB_POS;
l_fin_inf--;
ptr_hf += STEP;
}
/*----------------------------------------------------------------------*
* Compute elements of: rri0i4[], rri2i3[], rri1i2[], rri0i1[] *
*----------------------------------------------------------------------*/
ptr_hd = h;
ptr_hf = ptr_hd + 4;
l_fin_sup = MSIZE-1;
l_fin_inf = l_fin_sup-(Word16)1;
for(k=0; k<NB_POS; k++) {
if (sub(CODEC_MODE, 2) == 0)
p3 = rri0i4 + l_fin_sup;
if (sub(CODEC_MODE, 1) == 0)
p3 = rri0i4 + l_fin_inf;
p2 = rri2i3 + l_fin_inf;
p1 = rri1i2 + l_fin_inf;
p0 = rri0i1 + l_fin_inf;
ptr_h1 = ptr_hd;
ptr_h2 = ptr_hf;
cor = 0;
for(i=k+(Word16)1; i<NB_POS; i++ ) {
cor = L_mac(cor, *ptr_h1, *ptr_h2);
ptr_h1++;
ptr_h2++;
if (sub(CODEC_MODE, 2) == 0) *p3 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2);
ptr_h1++;
ptr_h2++;
if (sub(CODEC_MODE, 1) == 0) *p3 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2);
ptr_h1++;
ptr_h2++;
*p2 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2);
ptr_h1++;
ptr_h2++;
*p1 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2);
ptr_h1++;
ptr_h2++;
*p0 = extract_h(cor);
p3 -= ldec;
p2 -= ldec;
p1 -= ldec;
p0 -= ldec;
}
cor = L_mac(cor, *ptr_h1, *ptr_h2);
ptr_h1++;
ptr_h2++;
if (sub(CODEC_MODE, 2) == 0) *p3 = extract_h(cor);
l_fin_sup -= NB_POS;
l_fin_inf--;
ptr_hf += STEP;
}
if (sub(CODEC_MODE, 1) == 0) {
/*-----------------------------------------------------------------*
* Compute elements of RRi1i1[] *
*-----------------------------------------------------------------*/
p0 = rri0i2;
for (k=0; k<NB_POS; k++) {
*p0 = *rri1i1;
rri1i1++;
p0 += ldec;
}
ptr_hd = h;
ptr_hf = ptr_hd + 5;
l_fin_sup = MSIZE-1;
l_fin_inf = l_fin_sup-NB_POS;
lsym = NB_POS - (Word16)1;
for(k=(Word16)1; k<NB_POS; k++) {
p0 = rri0i2 + l_fin_inf;
ptr_h1 = ptr_hd;
ptr_h2 = ptr_hf;
cor = 0;
cor = L_mac(cor, *ptr_h1, *ptr_h2);
ptr_h1++;
ptr_h2++;
cor = L_mac(cor, *ptr_h1, *ptr_h2);
ptr_h1++;
ptr_h2++;
cor = L_mac(cor, *ptr_h1, *ptr_h2);
ptr_h1++;
ptr_h2++;
cor = L_mac(cor, *ptr_h1, *ptr_h2);
ptr_h1++;
ptr_h2++;
*p0 = extract_h(cor);
*(p0+lsym) = extract_h(cor);
p0 -= ldec;
for(i=k+(Word16)1; i<NB_POS; i++ ) {
cor = L_mac(cor, *ptr_h1, *ptr_h2);
ptr_h1++;
ptr_h2++;
cor = L_mac(cor, *ptr_h1, *ptr_h2);
ptr_h1++;
ptr_h2++;
cor = L_mac(cor, *ptr_h1, *ptr_h2);
ptr_h1++;
ptr_h2++;
cor = L_mac(cor, *ptr_h1, *ptr_h2);
ptr_h1++;
ptr_h2++;
cor = L_mac(cor, *ptr_h1, *ptr_h2);
ptr_h1++;
ptr_h2++;
*p0 = extract_h(cor);
*(p0+lsym) = extract_h(cor);
p0 -= ldec;
}
l_fin_inf -= NB_POS;
ptr_hf += STEP;
lsym += NB_POS - (Word16)1;
}
}
return;
}
/*************************************************************************
*
* FUNCTION: gc_pred()
*
* PURPOSE: MA prediction of the innovation energy
* (in dB/(20*log10(2))) with mean removed).
*
*************************************************************************/
void
gc_pred(
gc_predState *st, /* i/o: State struct */
enum Mode mode, /* i : AMR mode */
Word16 *code, /* i : innovative codebook vector (L_SUBFR) */
/* MR122: Q12, other modes: Q13 */
Word16 *exp_gcode0, /* o : exponent of predicted gain factor, Q0 */
Word16 *frac_gcode0,/* o : fraction of predicted gain factor Q15 */
Word16 *exp_en, /* o : exponent of innovation energy, Q0 */
/* (only calculated for MR795) */
Word16 *frac_en /* o : fraction of innovation energy, Q15 */
/* (only calculated for MR795) */
)
{
Word16 i;
Word32 ener_code;
Word16 exp, frac;
/*-------------------------------------------------------------------*
* energy of code: *
* ~~~~~~~~~~~~~~~ *
* ener_code = sum(code[i]^2) *
*-------------------------------------------------------------------*/
ener_code = L_mac((Word32) 0, code[0], code[0]);
/* MR122: Q12*Q12 -> Q25 */
/* others: Q13*Q13 -> Q27 */
for (i = 1; i < L_SUBFR; i++)
ener_code = L_mac(ener_code, code[i], code[i]);
test ();
if (sub (mode, MR122) == 0)
{
Word32 ener;
/* ener_code = ener_code / lcode; lcode = 40; 1/40 = 26214 Q20 */
ener_code = L_mult (round (ener_code), 26214); /* Q9 * Q20 -> Q30 */
/*-------------------------------------------------------------------*
* energy of code: *
* ~~~~~~~~~~~~~~~ *
* ener_code(Q17) = 10 * Log10(energy) / constant *
* = 1/2 * Log2(energy) *
* constant = 20*Log10(2) *
*-------------------------------------------------------------------*/
/* ener_code = 1/2 * Log2(ener_code); Note: Log2=log2+30 */
Log2(ener_code, &exp, &frac);
ener_code = L_Comp (sub (exp, 30), frac); /* Q16 for log() */
/* ->Q17 for 1/2 log()*/
/*-------------------------------------------------------------------*
* predicted energy: *
* ~~~~~~~~~~~~~~~~~ *
* ener(Q24) = (Emean + sum{pred[i]*past_en[i]})/constant *
* = MEAN_ENER + sum(pred[i]*past_qua_en[i]) *
* constant = 20*Log10(2) *
*-------------------------------------------------------------------*/
ener = MEAN_ENER_MR122;
move32 (); /* Q24 (Q17) */
for (i = 0; i < NPRED; i++)
{
ener = L_mac (ener, st->past_qua_en_MR122[i], pred_MR122[i]);
/* Q10 * Q13 -> Q24 */
/* Q10 * Q6 -> Q17 */
}
/*-------------------------------------------------------------------*
* predicted codebook gain *
* ~~~~~~~~~~~~~~~~~~~~~~~ *
* gc0 = Pow10( (ener*constant - ener_code*constant) / 20 ) *
* = Pow2(ener-ener_code) *
* = Pow2(int(d)+frac(d)) *
* *
* (store exp and frac for pow2()) *
*-------------------------------------------------------------------*/
ener = L_shr (L_sub (ener, ener_code), 1); /* Q16 */
L_Extract(ener, exp_gcode0, frac_gcode0);
}
else /* all modes except 12.2 */
{
Word32 L_tmp;
Word16 exp_code, gcode0;
/*-----------------------------------------------------------------*
* Compute: means_ener - 10log10(ener_code/ L_sufr) *
*-----------------------------------------------------------------*/
exp_code = norm_l (ener_code);
ener_code = L_shl (ener_code, exp_code);
/* Log2 = log2 + 27 */
Log2_norm (ener_code, exp_code, &exp, &frac);
/* fact = 10/log2(10) = 3.01 = 24660 Q13 */
L_tmp = Mpy_32_16(exp, frac, -24660); /* Q0.Q15 * Q13 -> Q14 */
/* L_tmp = means_ener - 10log10(ener_code/L_SUBFR)
* = means_ener - 10log10(ener_code) + 10log10(L_SUBFR)
* = K - fact * Log2(ener_code)
* = K - fact * log2(ener_code) - fact*27
*
* ==> K = means_ener + fact*27 + 10log10(L_SUBFR)
*
* means_ener = 33 = 540672 Q14 (MR475, MR515, MR59)
* means_ener = 28.75 = 471040 Q14 (MR67)
* means_ener = 30 = 491520 Q14 (MR74)
* means_ener = 36 = 589824 Q14 (MR795)
* means_ener = 33 = 540672 Q14 (MR102)
* 10log10(L_SUBFR) = 16.02 = 262481.51 Q14
* fact * 27 = 1331640 Q14
* -----------------------------------------
* (MR475, MR515, MR59) K = 2134793.51 Q14 ~= 16678 * 64 * 2
* (MR67) K = 2065161.51 Q14 ~= 32268 * 32 * 2
* (MR74) K = 2085641.51 Q14 ~= 32588 * 32 * 2
* (MR795) K = 2183945.51 Q14 ~= 17062 * 64 * 2
* (MR102) K = 2134793.51 Q14 ~= 16678 * 64 * 2
*/
if (test (), sub (mode, MR102) == 0)
{
/* mean = 33 dB */
L_tmp = L_mac(L_tmp, 16678, 64); /* Q14 */
}
else if (test (), sub (mode, MR795) == 0)
{
/* ener_code = <xn xn> * 2^27*2^exp_code
frac_en = ener_code / 2^16
= <xn xn> * 2^11*2^exp_code
<xn xn> = <xn xn>*2^11*2^exp * 2^exp_en
:= frac_en * 2^exp_en
==> exp_en = -11-exp_code;
*/
*frac_en = extract_h (ener_code);
move16 ();
*exp_en = sub (-11, exp_code);
move16 ();
/* mean = 36 dB */
L_tmp = L_mac(L_tmp, 17062, 64); /* Q14 */
}
else if (test (), sub (mode, MR74) == 0)
{
/* mean = 30 dB */
L_tmp = L_mac(L_tmp, 32588, 32); /* Q14 */
}
else if (test (), sub (mode, MR67) == 0)
{
/* mean = 28.75 dB */
L_tmp = L_mac(L_tmp, 32268, 32); /* Q14 */
}
else /* MR59, MR515, MR475 */
{
/* mean = 33 dB */
L_tmp = L_mac(L_tmp, 16678, 64); /* Q14 */
}
/*-----------------------------------------------------------------*
* Compute gcode0. *
* = Sum(i=0,3) pred[i]*past_qua_en[i] - ener_code + mean_ener *
*-----------------------------------------------------------------*/
L_tmp = L_shl(L_tmp, 10); /* Q24 */
for (i = 0; i < 4; i++)
L_tmp = L_mac(L_tmp, pred[i], st->past_qua_en[i]);
/* Q13 * Q10 -> Q24 */
gcode0 = extract_h(L_tmp); /* Q8 */
/*-----------------------------------------------------------------*
* gcode0 = pow(10.0, gcode0/20) *
* = pow(2, 3.3219*gcode0/20) *
* = pow(2, 0.166*gcode0) *
*-----------------------------------------------------------------*/
/* 5439 Q15 = 0.165985 */
/* (correct: 1/(20*log10(2)) 0.166096 = 5443 Q15) */
test ();
if (sub (mode, MR74) == 0) /* For IS641 bitexactness */
L_tmp = L_mult(gcode0, 5439); /* Q8 * Q15 -> Q24 */
else
L_tmp = L_mult(gcode0, 5443); /* Q8 * Q15 -> Q24 */
L_tmp = L_shr(L_tmp, 8); /* -> Q16 */
L_Extract(L_tmp, exp_gcode0, frac_gcode0); /* -> Q0.Q15 */
}
}
Flag Comp_Vad( Word16 *Dpnt)
{
int i,j ;
Word32 Acc0,Acc1 ;
Word16 Tm0, Tm1, Tm2 ;
Word16 Minp ;
Flag VadState = 1 ;
static Word16 ScfTab[11] = {
9170 ,
9170 ,
9170 ,
9170 ,
10289 ,
11544 ,
12953 ,
14533 ,
16306 ,
18296 ,
20529 ,
} ;
if ( !UseVx )
return VadState ;
/* Find Minimum pitch period */
Minp = PitchMax ;
for ( i = 0 ; i < 4 ; i ++ ) {
if ( Minp > VadStat->Polp[i] )
Minp = VadStat->Polp[i] ;
}
/* Check that all are multiplies of the minimum */
Tm2 = 0 ;
for ( i = 0 ; i < 4 ; i ++ ) {
Tm1 = Minp ;
for ( j = 0 ; j < 8 ; j ++ ) {
Tm0 = sub( Tm1, VadStat->Polp[i] ) ;
Tm0 = abs_s( Tm0 ) ;
if ( Tm0 <= 3 )
Tm2 ++ ;
Tm1 = add( Tm1, Minp ) ;
}
}
/* Update adaptation enable counter if not periodic and not sine */
if ( (Tm2 == 4) || (CodStat->SinDet < 0) )
VadStat->Aen += 2 ;
else
VadStat->Aen -- ;
/* Clip it */
if ( VadStat->Aen > 6 )
VadStat->Aen = 6 ;
if ( VadStat->Aen < 0 )
VadStat->Aen = 0 ;
/* Inverse filter the data */
Acc1 = 0L ;
for ( i = SubFrLen ; i < Frame ; i ++ ) {
Acc0 = L_mult( Dpnt[i], 0x2000 ) ;
for ( j = 0 ; j < LpcOrder ; j ++ )
Acc0 = L_msu( Acc0, Dpnt[i-j-1], VadStat->NLpc[j] ) ;
Tm0 = g723_round( Acc0 ) ;
Acc1 = L_mac( Acc1, Tm0, Tm0 ) ;
}
/* Scale the rezidual energy */
Acc1 = L_mls( Acc1, (Word16) 2913 ) ;
/* Clip noise level in any case */
if ( VadStat->Nlev > VadStat->Penr ) {
Acc0 = L_sub( VadStat->Penr, L_shr( VadStat->Penr, 2 ) ) ;
VadStat->Nlev = L_add( Acc0, L_shr( VadStat->Nlev, 2 ) ) ;
}
/* Update the noise level, if adaptation is enabled */
if ( !VadStat->Aen ) {
VadStat->Nlev = L_add( VadStat->Nlev, L_shr( VadStat->Nlev, 5 ) ) ;
}
/* Decay Nlev by small amount */
else {
VadStat->Nlev = L_sub( VadStat->Nlev, L_shr( VadStat->Nlev,11 ) ) ;
}
/* Update previous energy */
VadStat->Penr = Acc1 ;
/* CLip Noise Level */
if ( VadStat->Nlev < 0x00000080L )
VadStat->Nlev = 0x00000080L ;
if ( VadStat->Nlev > 0x0001ffffL )
VadStat->Nlev = 0x0001ffffL ;
/* Compute the treshold */
Acc0 = L_shl( VadStat->Nlev, 13 ) ;
Tm0 = norm_l( Acc0 ) ;
Acc0 = L_shl( Acc0, Tm0 ) ;
Acc0 &= 0x3f000000L ;
Acc0 <<= 1 ;
Tm1 = extract_h( Acc0 ) ;
Acc0 = L_deposit_h( ScfTab[Tm0] ) ;
Acc0 = L_mac( Acc0, Tm1, ScfTab[Tm0-1] ) ;
Acc0 = L_msu( Acc0, Tm1, ScfTab[Tm0] ) ;
Tm1 = extract_h( Acc0 ) ;
Tm0 = extract_l( L_shr( VadStat->Nlev, 2 ) ) ;
Acc0 = L_mult( Tm0, Tm1 ) ;
Acc0 >>= 11 ;
/* Compare with the treshold */
if ( Acc0 > Acc1 )
VadState = 0 ;
/* Do the various counters */
if ( VadState ) {
VadStat->Vcnt ++ ;
VadStat->Hcnt ++ ;
}
else {
VadStat->Vcnt -- ;
if ( VadStat->Vcnt < 0 )
VadStat->Vcnt = 0 ;
}
if ( VadStat->Vcnt >= 2 ) {
VadStat->Hcnt = 6 ;
if ( VadStat->Vcnt >= 3 )
VadStat->Vcnt = 3 ;
}
if ( VadStat->Hcnt ) {
VadState = 1 ;
if ( VadStat->Vcnt == 0 )
VadStat->Hcnt -- ;
}
/* Update Periodicy detector */
VadStat->Polp[0] = VadStat->Polp[2] ;
VadStat->Polp[1] = VadStat->Polp[3] ;
return VadState ;
}
/*
********************************************************************************
* PUBLIC PROGRAM CODE
********************************************************************************
*/
Word16 hp_max (
Word32 corr[], /* i : correlation vector. */
Word16 scal_sig[], /* i : scaled signal. */
Word16 L_frame, /* i : length of frame to compute pitch */
Word16 lag_max, /* i : maximum lag */
Word16 lag_min, /* i : minimum lag */
Word16 *cor_hp_max) /* o : max high-pass filtered norm. correlation */
{
Word16 i;
Word16 *p, *p1;
Word32 max, t0, t1;
Word16 max16, t016, cor_max;
Word16 shift, shift1, shift2;
max = MIN_32; move32 ();
t0 = 0L; move32 ();
for (i = lag_max-1; i > lag_min; i--)
{
/* high-pass filtering */
t0 = L_sub (L_sub(L_shl(corr[-i], 1), corr[-i-1]), corr[-i+1]);
t0 = L_abs (t0);
test ();
if (L_sub (t0, max) >= 0)
{
max = t0; move32 ();
}
}
/* compute energy */
p = scal_sig; move16 ();
p1 = &scal_sig[0]; move16 ();
t0 = 0L; move32 ();
for (i = 0; i < L_frame; i++, p++, p1++)
{
t0 = L_mac (t0, *p, *p1);
}
p = scal_sig; move16 ();
p1 = &scal_sig[-1]; move16 ();
t1 = 0L; move32 ();
for (i = 0; i < L_frame; i++, p++, p1++)
{
t1 = L_mac (t1, *p, *p1);
}
/* high-pass filtering */
t0 = L_sub(L_shl(t0, 1), L_shl(t1, 1));
t0 = L_abs (t0);
/* max/t0 */
shift1 = sub(norm_l(max), 1);
max16 = extract_h(L_shl(max, shift1));
shift2 = norm_l(t0);
t016 = extract_h(L_shl(t0, shift2));
test ();
if (t016 != 0)
{
cor_max = div_s(max16, t016);
}
else
{
cor_max = 0; move16 ();
}
shift = sub(shift1, shift2);
test ();
if (shift >= 0)
{
*cor_hp_max = shr(cor_max, shift); move16 (); /* Q15 */
}
else
{
*cor_hp_max = shl(cor_max, negate(shift)); move16 (); /* Q15 */
}
return 0;
}
/*
**
** Function: AtoLsp()
**
** Description: Transforms 10 LPC coefficients to the 10
** corresponding LSP frequencies for a subframe.
** This transformation is done once per frame,
** for subframe 3 only. The transform algorithm
** generates sum and difference polynomials from
** the LPC coefficients. It then evaluates the
** sum and difference polynomials at uniform
** intervals of pi/256 along the unit circle.
** Intervals where a sign change occurs are
** interpolated to find the zeros of the
** polynomials, which are the LSP frequencies.
**
** Links to text: Section 2.5
**
** Arguments:
**
** Word16 *LspVect Empty Buffer
** Word16 Lpc[] Unquantized LPC coefficients (10 words)
** Word16 PrevLsp[] LSP frequencies from the previous frame (10 words)
**
** Outputs:
**
** Word16 LspVect[] LSP frequencies for the current frame (10 words)
**
** Return value: None
**
**/
void AtoLsp( Word16 *LspVect, Word16 *Lpc, Word16 *PrevLsp )
{
int i,j,k ;
Word32 Lpq[LpcOrder+2] ;
Word16 Spq[LpcOrder+2] ;
Word16 Exp ;
Word16 LspCnt ;
Word32 PrevVal,CurrVal ;
Word32 Acc0,Acc1 ;
/*
* Perform a bandwidth expansion on the LPC coefficients. This
* scales the poles of the LPC synthesis filter by a factor of
* 0.994.
*/
for ( i = 0 ; i < LpcOrder ; i ++ )
LspVect[i] = mult_r( Lpc[i], BandExpTable[i] ) ;
/*
* Compute the sum and difference polynomials with the roots at z =
* -1 (sum) or z = +1 (difference) removed. Let these polynomials
* be P(z) and Q(z) respectively, and let their coefficients be
* {p_i} amd {q_i}. The coefficients are stored in the array Lpq[]
* as follows: p_0, q_0, p_1, q_1, ..., p_5, q_5. There is no need
* to store the other coefficients because of symmetry.
*/
/*
* Set p_0 = q_0 = 1. The LPC coefficients are already scaled by
* 1/4. P(z) and Q(z) are scaled by an additional scaling factor of
* 1/16, for an overall factor of 1/64 = 0x02000000L.
*/
Lpq[0] = Lpq[1] = (Word32) 0x02000000L ;
/*
* This loop computes the coefficients of P(z) and Q(z). The long
* division (to remove the real zeros) is done recursively.
*/
for ( i = 0 ; i < LpcOrder/2 ; i ++ ) {
/* P(z) */
Acc0 = L_negate( Lpq[2*i+0] ) ;
Acc1 = L_deposit_h( LspVect[i] ) ;
Acc1 = L_shr( Acc1, (Word16) 4 ) ;
Acc0 = L_sub( Acc0, Acc1 ) ;
Acc1 = L_deposit_h( LspVect[LpcOrder-1-i] ) ;
Acc1 = L_shr( Acc1, (Word16) 4 ) ;
Acc0 = L_sub( Acc0, Acc1 ) ;
Lpq[2*i+2] = Acc0 ;
/* Q(z) */
Acc0 = Lpq[2*i+1] ;
Acc1 = L_deposit_h( LspVect[i] ) ;
Acc1 = L_shr( Acc1, (Word16) 4 ) ;
Acc0 = L_sub( Acc0, Acc1 ) ;
Acc1 = L_deposit_h( LspVect[LpcOrder-1-i] ) ;
Acc1 = L_shr( Acc1, (Word16) 4 ) ;
// Acc0 = L_add( Acc0, Acc1 ) ;
L_ADD(Acc0, Acc1, Acc0);
Lpq[2*i+3] = Acc0 ;
}
/*
* Divide p_5 and q_5 by 2 for proper weighting during polynomial
* evaluation.
*/
Lpq[LpcOrder+0] = L_shr( Lpq[LpcOrder+0], (Word16) 1 ) ;
Lpq[LpcOrder+1] = L_shr( Lpq[LpcOrder+1], (Word16) 1 ) ;
/*
* Normalize the polynomial coefficients and convert to shorts
*/
/* Find the maximum */
Acc1 = L_abs( Lpq[0] ) ;
for ( i = 1 ; i < LpcOrder+2 ; i ++ ) {
Acc0 = L_abs( Lpq[i] ) ;
if ( Acc0 > Acc1 )
Acc1 = Acc0 ;
}
/* Compute the normalization factor */
Exp = norm_l( Acc1 ) ;
/* Normalize and convert to shorts */
for ( i = 0 ; i < LpcOrder+2 ; i ++ ) {
Acc0 = L_shl( Lpq[i], Exp ) ;
Spq[i] = round( Acc0 ) ;
}
/*
* Initialize the search loop
*/
/*
* The variable k is a flag that indicates which polynomial (sum or
* difference) the algorithm is currently evaluating. Start with
* the sum.
*/
k = 0 ;
/* Evaluate the sum polynomial at frequency zero */
PrevVal = (Word32) 0 ;
for ( j = 0 ; j <= LpcOrder/2 ; j ++ )
PrevVal = L_mac( PrevVal, Spq[2*j], CosineTable[0] ) ;
/*
* Search loop. Evaluate P(z) and Q(z) at uniform intervals of
* pi/256 along the unit circle. Check for zero crossings. The
* zeros of P(w) and Q(w) alternate, so only one of them need by
* evaluated at any given step.
*/
LspCnt = (Word16) 0 ;
for ( i = 1 ; i < CosineTableSize/2 ; i ++ ) {
/* Evaluate the selected polynomial */
CurrVal = (Word32) 0 ;
for ( j = 0 ; j <= LpcOrder/2 ; j ++ )
CurrVal = L_mac( CurrVal, Spq[LpcOrder-2*j+k],
CosineTable[i*j%CosineTableSize] ) ;
/* Check for a sign change, indicating a zero crossing */
if ( (CurrVal ^ PrevVal) < (Word32) 0 ) {
/*
* Interpolate to find the bottom 7 bits of the
* zero-crossing frequency
*/
Acc0 = L_abs( CurrVal ) ;
Acc1 = L_abs( PrevVal ) ;
// Acc0 = L_add( Acc0, Acc1 ) ;
L_ADD(Acc0, Acc1, Acc0);
/* Normalize the sum */
Exp = norm_l( Acc0 ) ;
Acc0 = L_shl( Acc0, Exp ) ;
Acc1 = L_shl( Acc1, Exp ) ;
Acc1 = L_shr( Acc1, (Word16) 8 ) ;
LspVect[LspCnt] = div_l( Acc1, extract_h( Acc0 ) ) ;
/*
* Add the upper part of the zero-crossing frequency,
* i.e. bits 7-15
*/
Exp = shl( (Word16) (i-1), (Word16) 7 ) ;
LspVect[LspCnt] = add( LspVect[LspCnt], Exp ) ;
LspCnt ++ ;
/* Check if all zeros have been found */
if ( LspCnt == (Word16) LpcOrder )
break ;
/*
* Switch the pointer between sum and difference polynomials
*/
k ^= 1 ;
/*
* Evaluate the new polynomial at the current frequency
*/
CurrVal = (Word32) 0 ;
for ( j = 0 ; j <= LpcOrder/2 ; j ++ )
CurrVal = L_mac( CurrVal, Spq[LpcOrder-2*j+k],
CosineTable[i*j%CosineTableSize] ) ;
}
/* Update the previous value */
PrevVal = CurrVal ;
}
/*
* Check if all 10 zeros were found. If not, ignore the results of
* the search and use the previous frame's LSP frequencies instead.
*/
if ( LspCnt != (Word16) LpcOrder ) {
for ( j = 0 ; j < LpcOrder ; j ++ )
LspVect[j] = PrevLsp[j] ;
}
return ;
}
/*
**************************************************************************
*
* Function : agc
* Purpose : Scales the postfilter output on a subframe basis
*
**************************************************************************
*/
int agc (
agcState *st, /* i/o : agc state */
Word16 *sig_in, /* i : postfilter input signal (l_trm) */
Word16 *sig_out, /* i/o : postfilter output signal (l_trm) */
Word16 agc_fac, /* i : AGC factor */
Word16 l_trm /* i : subframe size */
)
{
Word16 i, exp;
Word16 gain_in, gain_out, g0, gain;
Word32 s;
/* calculate gain_out with exponent */
s = energy_new(sig_out, l_trm); /* function result */
if (s == 0)
{
st->past_gain = 0;
return 0;
}
exp = sub (norm_l (s), 1);
gain_out = round (L_shl (s, exp));
/* calculate gain_in with exponent */
s = energy_new(sig_in, l_trm); /* function result */
if (s == 0)
{
g0 = 0;
}
else
{
i = norm_l (s);
gain_in = round (L_shl (s, i));
exp = sub (exp, i);
/*---------------------------------------------------*
* g0 = (1-agc_fac) * sqrt(gain_in/gain_out); *
*---------------------------------------------------*/
s = L_deposit_l (div_s (gain_out, gain_in));
s = L_shl (s, 7); /* s = gain_out / gain_in */
s = L_shr (s, exp); /* add exponent */
s = Inv_sqrt (s); /* function result */
i = round (L_shl (s, 9));
/* g0 = i * (1-agc_fac) */
g0 = mult (i, sub (32767, agc_fac));
}
/* compute gain[n] = agc_fac * gain[n-1]
+ (1-agc_fac) * sqrt(gain_in/gain_out) */
/* sig_out[n] = gain[n] * sig_out[n] */
gain = st->past_gain;
for (i = 0; i < l_trm; i++)
{
gain = mult (gain, agc_fac);
gain = add (gain, g0);
sig_out[i] = extract_h (L_shl (L_mult (sig_out[i], gain), 3));
}
st->past_gain = gain;
return 0;
}
void Levinsone(
Word16 m, /* (i) : LPC order */
Word16 Rh[], /* (i) : Rh[M+1] Vector of autocorrelations (msb) */
Word16 Rl[], /* (i) : Rl[M+1] Vector of autocorrelations (lsb) */
Word16 A[], /* (o) Q12 : A[M] LPC coefficients (m = 10) */
Word16 rc[], /* (o) Q15 : rc[M] Reflection coefficients. */
Word16 old_A[], /* (i/o) Q12 : last stable filter LPC coefficients */
Word16 old_rc[] /* (i/o) Q15 : last stable filter Reflection coefficients. */
)
{
Word16 i, j;
Word16 hi, lo;
Word16 Kh, Kl; /* reflection coefficient; hi and lo */
Word16 alp_h, alp_l, alp_exp; /* Prediction gain; hi lo and exponent */
Word16 Ah[M_BWDP1], Al[M_BWDP1]; /* LPC coef. in double prec. */
Word16 Anh[M_BWDP1], Anl[M_BWDP1]; /* LPC coef.for next iteration in double prec. */
Word32 t0, t1, t2; /* temporary variable */
Word32 tmp;
/* K = A[1] = -R[1] / R[0] */
t1 = L_Comp(Rh[1], Rl[1]); /* R[1] in Q31 */
tmp = L_Comp(Rh[0], Rl[0]);
if (t1 > tmp) t1 = tmp;
t2 = L_abs(t1); /* abs R[1] */
t0 = Div_32(t2, Rh[0], Rl[0]); /* R[1]/R[0] in Q31 */
if(t1 > 0) t0= L_negate(t0); /* -R[1]/R[0] */
L_Extract(t0, &Kh, &Kl); /* K in DPF */
rc[0] = Kh;
t0 = L_shr(t0,4); /* A[1] in Q27 */
L_Extract(t0, &Ah[1], &Al[1]); /* A[1] in DPF */
/* Alpha = R[0] * (1-K**2) */
t0 = Mpy_32(Kh ,Kl, Kh, Kl); /* K*K in Q31 */
t0 = L_abs(t0); /* Some case <0 !! */
t0 = L_sub( (Word32)0x7fffffffL, t0 ); /* 1 - K*K in Q31 */
L_Extract(t0, &hi, &lo); /* DPF format */
t0 = Mpy_32(Rh[0] ,Rl[0], hi, lo); /* Alpha in Q31 */
/* Normalize Alpha */
alp_exp = norm_l(t0);
t0 = L_shl(t0, alp_exp);
L_Extract(t0, &alp_h, &alp_l); /* DPF format */
/*--------------------------------------*
* ITERATIONS I=2 to m *
*--------------------------------------*/
for(i= 2; i<=m; i++)
{
/* t0 = SUM ( R[j]*A[i-j] ,j=1,i-1 ) + R[i] */
t0 = 0;
for(j=1; j<i; j++)
t0 = L_add(t0, Mpy_32(Rh[j], Rl[j], Ah[i-j], Al[i-j]));
t0 = L_shl(t0,4); /* result in Q27 -> convert to Q31 */
/* No overflow possible */
t1 = L_Comp(Rh[i],Rl[i]);
t0 = L_add(t0, t1); /* add R[i] in Q31 */
/* K = -t0 / Alpha */
t1 = L_abs(t0);
tmp = L_Comp(alp_h, alp_l);
if (t1 > tmp) t1 = tmp;
t2 = Div_32(t1, alp_h, alp_l); /* abs(t0)/Alpha */
if(t0 > 0) t2= L_negate(t2); /* K =-t0/Alpha */
t2 = L_shl(t2, alp_exp); /* denormalize; compare to Alpha */
L_Extract(t2, &Kh, &Kl); /* K in DPF */
rc[i-1] = Kh;
/* Test for unstable filter. If unstable keep old A(z) */
if (sub(abs_s(Kh), 32750) > 0)
{
for(j=0; j<=m; j++)
{
A[j] = old_A[j];
}
rc[0] = old_rc[0]; /* only two rc coefficients are needed */
rc[1] = old_rc[1];
return;
}
/*------------------------------------------*
* Compute new LPC coeff. -> An[i] *
* An[j]= A[j] + K*A[i-j] , j=1 to i-1 *
* An[i]= K *
*------------------------------------------*/
for(j=1; j<i; j++)
{
t0 = Mpy_32(Kh, Kl, Ah[i-j], Al[i-j]);
t0 = L_add(t0, L_Comp(Ah[j], Al[j]));
L_Extract(t0, &Anh[j], &Anl[j]);
}
t2 = L_shr(t2, 4); /* t2 = K in Q31 ->convert to Q27 */
L_Extract(t2, &Anh[i], &Anl[i]); /* An[i] in Q27 */
/* Alpha = Alpha * (1-K**2) */
t0 = Mpy_32(Kh ,Kl, Kh, Kl); /* K*K in Q31 */
t0 = L_abs(t0); /* Some case <0 !! */
t0 = L_sub( (Word32)0x7fffffffL, t0 ); /* 1 - K*K in Q31 */
L_Extract(t0, &hi, &lo); /* DPF format */
t0 = Mpy_32(alp_h , alp_l, hi, lo); /* Alpha in Q31 */
/* Normalize Alpha */
j = norm_l(t0);
t0 = L_shl(t0, j);
L_Extract(t0, &alp_h, &alp_l); /* DPF format */
alp_exp = add(alp_exp, j); /* Add normalization to alp_exp */
/* A[j] = An[j] */
for(j=1; j<=i; j++)
{
Ah[j] =Anh[j];
Al[j] =Anl[j];
}
}
/* Truncate A[i] in Q27 to Q12 with rounding */
A[0] = 4096;
for(i=1; i<=m; i++)
{
t0 = L_Comp(Ah[i], Al[i]);
old_A[i] = A[i] = round(L_shl(t0, 1));
}
old_rc[0] = rc[0];
old_rc[1] = rc[1];
return;
}
/*----------------------------------------------------------------------------
; FUNCTION CODE
----------------------------------------------------------------------------*/
Word16 hp_max(
Word32 corr[], /* i : correlation vector. */
Word16 scal_sig[], /* i : scaled signal. */
Word16 L_frame, /* i : length of frame to compute pitch */
Word16 lag_max, /* i : maximum lag */
Word16 lag_min, /* i : minimum lag */
Word16 *cor_hp_max, /* o : max high-pass filtered norm. correlation */
Flag *pOverflow /* i/o : overflow Flag */
)
{
Word16 i;
Word16 *p, *p1;
Word32 max, t0, t1;
Word16 max16, t016, cor_max;
Word16 shift, shift1, shift2;
Word32 L_temp;
max = MIN_32;
t0 = 0L;
for (i = lag_max - 1; i > lag_min; i--)
{
/* high-pass filtering */
t0 = L_shl(corr[-i], 1, pOverflow);
L_temp = L_sub(t0, corr[-i-1], pOverflow);
t0 = L_sub(L_temp, corr[-i+1], pOverflow);
t0 = L_abs(t0);
if (t0 >= max)
{
max = t0;
}
}
/* compute energy */
p = scal_sig;
p1 = &scal_sig[0];
t0 = 0L;
for (i = 0; i < L_frame; i++, p++, p1++)
{
t0 = L_mac(t0, *p, *p1, pOverflow);
}
p = scal_sig;
p1 = &scal_sig[-1];
t1 = 0L;
for (i = 0; i < L_frame; i++, p++, p1++)
{
t1 = L_mac(t1, *p, *p1, pOverflow);
}
/* high-pass filtering */
L_temp = L_shl(t0, 1, pOverflow);
t1 = L_shl(t1, 1, pOverflow);
t0 = L_sub(L_temp, t1, pOverflow);
t0 = L_abs(t0);
/* max/t0 */
/* shift1 = sub(norm_l(max), 1);
max16 = extract_h(L_shl(max, shift1));
shift2 = norm_l(t0);
t016 = extract_h(L_shl(t0, shift2)); */
t016 = norm_l(max);
shift1 = sub(t016, 1, pOverflow);
L_temp = L_shl(max, shift1, pOverflow);
max16 = (Word16)(L_temp >> 16);
shift2 = norm_l(t0);
L_temp = L_shl(t0, shift2, pOverflow);
t016 = (Word16)(L_temp >> 16);
if (t016 != 0)
{
cor_max = div_s(max16, t016);
}
else
{
cor_max = 0;
}
shift = sub(shift1, shift2, pOverflow);
if (shift >= 0)
{
*cor_hp_max = shr(cor_max, shift, pOverflow); /* Q15 */
}
else
{
*cor_hp_max = shl(cor_max, negate(shift), pOverflow); /* Q15 */
}
return 0;
}
void A_Refl(
Word16 a[], /* i : Directform coefficients */
Word16 refl[], /* o : Reflection coefficients */
Flag *pOverflow
)
{
/* local variables */
Word16 i;
Word16 j;
Word16 aState[M];
Word16 bState[M];
Word16 normShift;
Word16 normProd;
Word32 L_acc;
Word16 scale;
Word32 L_temp;
Word16 temp;
Word16 mult;
/* initialize states */
for (i = 0; i < M; i++)
{
aState[i] = a[i];
}
/* backward Levinson recursion */
for (i = M - 1; i >= 0; i--)
{
if (abs_s(aState[i]) >= 4096)
{
for (i = 0; i < M; i++)
{
refl[i] = 0;
}
break;
}
refl[i] = shl(aState[i], 3, pOverflow);
L_temp = L_mult(refl[i], refl[i], pOverflow);
L_acc = L_sub(MAX_32, L_temp, pOverflow);
normShift = norm_l(L_acc);
scale = sub(15, normShift, pOverflow);
L_acc = L_shl(L_acc, normShift, pOverflow);
normProd = pv_round(L_acc, pOverflow);
mult = div_s(16384, normProd);
for (j = 0; j < i; j++)
{
L_acc = L_deposit_h(aState[j]);
L_acc = L_msu(L_acc, refl[i], aState[i-j-1], pOverflow);
temp = pv_round(L_acc, pOverflow);
L_temp = L_mult(mult, temp, pOverflow);
L_temp = L_shr_r(L_temp, scale, pOverflow);
if (L_abs(L_temp) > 32767)
{
for (i = 0; i < M; i++)
{
refl[i] = 0;
}
break;
}
bState[j] = extract_l(L_temp);
}
for (j = 0; j < i; j++)
{
aState[j] = bState[j];
}
}
return;
}
void agc(
int16_t *sig_in, /* (i) : postfilter input signal */
int16_t *sig_out, /* (i/o) : postfilter output signal */
int16_t l_trm /* (i) : subframe size */
)
{
static int16_t past_gain=4096; /* past_gain = 1.0 (Q12) */
int16_t i, exp;
int16_t gain_in, gain_out, g0, gain; /* Q12 */
int32_t s;
int16_t signal[L_SUBFR];
/* calculate gain_out with exponent */
for(i=0; i<l_trm; i++)
signal[i] = shr(sig_out[i], 2);
s = 0;
for(i=0; i<l_trm; i++)
s = L_mac(s, signal[i], signal[i]);
if (s == 0) {
past_gain = 0;
return;
}
exp = sub(norm_l(s), 1);
gain_out = _round(L_shl(s, exp));
/* calculate gain_in with exponent */
for(i=0; i<l_trm; i++)
signal[i] = shr(sig_in[i], 2);
s = 0;
for(i=0; i<l_trm; i++)
s = L_mac(s, signal[i], signal[i]);
if (s == 0) {
g0 = 0;
}
else {
i = norm_l(s);
gain_in = _round(L_shl(s, i));
exp = sub(exp, i);
/*---------------------------------------------------*
* g0(Q12) = (1-AGC_FAC) * sqrt(gain_in/gain_out); *
*---------------------------------------------------*/
s = L_deposit_l(div_s(gain_out,gain_in)); /* Q15 */
s = L_shl(s, 7); /* s(Q22) = gain_out / gain_in */
s = L_shr(s, exp); /* Q22, add exponent */
/* i(Q12) = s(Q19) = 1 / sqrt(s(Q22)) */
s = Inv_sqrt(s); /* Q19 */
i = _round(L_shl(s,9)); /* Q12 */
/* g0(Q12) = i(Q12) * (1-AGC_FAC)(Q15) */
g0 = mult(i, AGC_FAC1); /* Q12 */
}
/* compute gain(n) = AGC_FAC gain(n-1) + (1-AGC_FAC)gain_in/gain_out */
/* sig_out(n) = gain(n) sig_out(n) */
gain = past_gain;
for(i=0; i<l_trm; i++) {
gain = mult(gain, AGC_FAC);
gain = add(gain, g0);
sig_out[i] = extract_h(L_shl(L_mult(sig_out[i], gain), 3));
}
past_gain = gain;
return;
}
/*************************************************************************
*
* FUNCTION: calc_filt_energies
*
* PURPOSE: calculation of several energy coefficients for filtered
* excitation signals
*
* Compute coefficients need for the quantization and the optimum
* codebook gain gcu (for MR475 only).
*
* coeff[0] = y1 y1
* coeff[1] = -2 xn y1
* coeff[2] = y2 y2
* coeff[3] = -2 xn y2
* coeff[4] = 2 y1 y2
*
*
* gcu = <xn2, y2> / <y2, y2> (0 if <xn2, y2> <= 0)
*
* Product <y1 y1> and <xn y1> have been computed in G_pitch() and
* are in vector g_coeff[].
*
*************************************************************************/
void
calc_filt_energies(
enum Mode mode, /* i : coder mode */
Word16 xn[], /* i : LTP target vector, Q0 */
Word16 xn2[], /* i : CB target vector, Q0 */
Word16 y1[], /* i : Adaptive codebook, Q0 */
Word16 Y2[], /* i : Filtered innovative vector, Q12 */
Word16 g_coeff[], /* i : Correlations <xn y1> <y1 y1> */
/* computed in G_pitch() */
Word16 frac_coeff[],/* o : energy coefficients (5), fraction part, Q15 */
Word16 exp_coeff[], /* o : energy coefficients (5), exponent part, Q0 */
Word16 *cod_gain_frac,/* o: optimum codebook gain (fraction part), Q15 */
Word16 *cod_gain_exp /* o: optimum codebook gain (exponent part), Q0 */
)
{
Word32 s, ener_init;
Word16 i, exp, frac;
Word16 y2[L_SUBFR];
if (test(), sub(mode, MR795) == 0 || sub(mode, MR475) == 0)
{
ener_init = 0L; move32 ();
}
else
{
ener_init = 1L; move32 ();
}
for (i = 0; i < L_SUBFR; i++) {
y2[i] = shr(Y2[i], 3); move16 ();
}
frac_coeff[0] = g_coeff[0]; move16 ();
exp_coeff[0] = g_coeff[1]; move16 ();
frac_coeff[1] = negate(g_coeff[2]); move16 (); /* coeff[1] = -2 xn y1 */
exp_coeff[1] = add(g_coeff[3], 1); move16 ();
/* Compute scalar product <y2[],y2[]> */
s = L_mac(ener_init, y2[0], y2[0]);
for (i = 1; i < L_SUBFR; i++)
s = L_mac(s, y2[i], y2[i]);
exp = norm_l(s);
frac_coeff[2] = extract_h(L_shl(s, exp)); move16 ();
exp_coeff[2] = sub(15 - 18, exp); move16();
/* Compute scalar product -2*<xn[],y2[]> */
s = L_mac(ener_init, xn[0], y2[0]);
for (i = 1; i < L_SUBFR; i++)
s = L_mac(s, xn[i], y2[i]);
exp = norm_l(s);
frac_coeff[3] = negate(extract_h(L_shl(s, exp))); move16 ();
exp_coeff[3] = sub(15 - 9 + 1, exp); move16 ();
/* Compute scalar product 2*<y1[],y2[]> */
s = L_mac(ener_init, y1[0], y2[0]);
for (i = 1; i < L_SUBFR; i++)
s = L_mac(s, y1[i], y2[i]);
exp = norm_l(s);
frac_coeff[4] = extract_h(L_shl(s, exp)); move16 ();
exp_coeff[4] = sub(15 - 9 + 1, exp); move16();
if (test(), test (), sub(mode, MR475) == 0 || sub(mode, MR795) == 0)
{
/* Compute scalar product <xn2[],y2[]> */
s = L_mac(ener_init, xn2[0], y2[0]);
for (i = 1; i < L_SUBFR; i++)
s = L_mac(s, xn2[i], y2[i]);
exp = norm_l(s);
frac = extract_h(L_shl(s, exp));
exp = sub(15 - 9, exp);
if (test (), frac <= 0)
{
*cod_gain_frac = 0; move16 ();
*cod_gain_exp = 0; move16 ();
}
else
{
/*
gcu = <xn2, y2> / c[2]
= (frac>>1)/frac[2] * 2^(exp+1-exp[2])
= div_s(frac>>1, frac[2])*2^-15 * 2^(exp+1-exp[2])
= div_s * 2^(exp-exp[2]-14)
*/
*cod_gain_frac = div_s (shr (frac,1), frac_coeff[2]); move16 ();
*cod_gain_exp = sub (sub (exp, exp_coeff[2]), 14); move16 ();
}
}
}
/*---------------------------------------------------------------------------*
* Function Qua_gain *
* ~~~~~~~~~~~~~~~~~~ *
* Inputs: *
* code[] :Innovative codebook. *
* g_coeff[] :Correlations compute for pitch. *
* L_subfr :Subframe length. *
* *
* Outputs: *
* gain_pit :Quantized pitch gain. *
* gain_cod :Quantized code gain. *
* *
* Return: *
* Index of quantization. *
* *
*--------------------------------------------------------------------------*/
Word16 Qua_gain(
Word16 code[], /* (i) Q13 :Innovative vector. */
Word16 g_coeff[], /* (i) :Correlations <xn y1> -2<y1 y1> */
/* <y2,y2>, -2<xn,y2>, 2<y1,y2> */
Word16 exp_coeff[], /* (i) :Q-Format g_coeff[] */
Word16 L_subfr, /* (i) :Subframe length. */
Word16 *gain_pit, /* (o) Q14 :Pitch gain. */
Word16 *gain_cod, /* (o) Q1 :Code gain. */
Word16 tameflag /* (i) : set to 1 if taming is needed */
)
{
Word16 i, j, index1, index2;
Word16 cand1, cand2;
Word16 exp, gcode0, exp_gcode0, gcode0_org, e_min ;
Word16 nume, denom, inv_denom;
Word16 exp1,exp2,exp_nume,exp_denom,exp_inv_denom,sft,tmp;
Word16 g_pitch, g2_pitch, g_code, g2_code, g_pit_cod;
Word16 coeff[5], coeff_lsf[5];
Word16 exp_min[5];
Word32 L_gbk12;
Word32 L_tmp, L_dist_min, L_tmp1, L_tmp2, L_acc, L_accb;
Word16 best_gain[2];
/* Gain predictor, Past quantized energies = -14.0 in Q10 */
static Word16 past_qua_en[4] = { -14336, -14336, -14336, -14336 };
/*---------------------------------------------------*
*- energy due to innovation -*
*- predicted energy -*
*- predicted codebook gain => gcode0[exp_gcode0] -*
*---------------------------------------------------*/
Gain_predict( past_qua_en, code, L_subfr, &gcode0, &exp_gcode0 );
/*-----------------------------------------------------------------*
* pre-selection *
*-----------------------------------------------------------------*/
/*-----------------------------------------------------------------*
* calculate best gain *
* *
* tmp = -1./(4.*coeff[0]*coeff[2]-coeff[4]*coeff[4]) ; *
* best_gain[0] = (2.*coeff[2]*coeff[1]-coeff[3]*coeff[4])*tmp ; *
* best_gain[1] = (2.*coeff[0]*coeff[3]-coeff[1]*coeff[4])*tmp ; *
* gbk_presel(best_gain,&cand1,&cand2,gcode0) ; *
* *
*-----------------------------------------------------------------*/
/*-----------------------------------------------------------------*
* tmp = -1./(4.*coeff[0]*coeff[2]-coeff[4]*coeff[4]) ; *
*-----------------------------------------------------------------*/
L_tmp1 = L_mult( g_coeff[0], g_coeff[2] );
exp1 = add( add( exp_coeff[0], exp_coeff[2] ), 1-2 );
L_tmp2 = L_mult( g_coeff[4], g_coeff[4] );
exp2 = add( add( exp_coeff[4], exp_coeff[4] ), 1 );
if( exp1 > exp2 ){
L_tmp = L_sub( L_shr( L_tmp1, sub(exp1,exp2) ), L_tmp2 );
exp = exp2;
}
else{
L_tmp = L_sub( L_tmp1, L_shr( L_tmp2, sub(exp2,exp1) ) );
exp = exp1;
}
sft = norm_l( L_tmp );
denom = extract_h( L_shl(L_tmp, sft) );
exp_denom = sub( add( exp, sft ), 16 );
inv_denom = div_s(16384,denom);
inv_denom = negate( inv_denom );
exp_inv_denom = sub( 14+15, exp_denom );
/*-----------------------------------------------------------------*
* best_gain[0] = (2.*coeff[2]*coeff[1]-coeff[3]*coeff[4])*tmp ; *
*-----------------------------------------------------------------*/
L_tmp1 = L_mult( g_coeff[2], g_coeff[1] );
exp1 = add( exp_coeff[2], exp_coeff[1] );
L_tmp2 = L_mult( g_coeff[3], g_coeff[4] );
exp2 = add( add( exp_coeff[3], exp_coeff[4] ), 1 );
if( exp1 > exp2 ){
L_tmp = L_sub( L_shr( L_tmp1, add(sub(exp1,exp2),1 )), L_shr( L_tmp2,1 ) );
exp = sub(exp2,1);
}
else{
L_tmp = L_sub( L_shr( L_tmp1,1 ), L_shr( L_tmp2, add(sub(exp2,exp1),1 )) );
exp = sub(exp1,1);
}
sft = norm_l( L_tmp );
nume = extract_h( L_shl(L_tmp, sft) );
exp_nume = sub( add( exp, sft ), 16 );
sft = sub( add( exp_nume, exp_inv_denom ), (9+16-1) );
L_acc = L_shr( L_mult( nume,inv_denom ), sft );
best_gain[0] = extract_h( L_acc ); /*-- best_gain[0]:Q9 --*/
if (tameflag == 1){
if(best_gain[0] > GPCLIP2) best_gain[0] = GPCLIP2;
}
/*-----------------------------------------------------------------*
* best_gain[1] = (2.*coeff[0]*coeff[3]-coeff[1]*coeff[4])*tmp ; *
*-----------------------------------------------------------------*/
L_tmp1 = L_mult( g_coeff[0], g_coeff[3] );
exp1 = add( exp_coeff[0], exp_coeff[3] ) ;
L_tmp2 = L_mult( g_coeff[1], g_coeff[4] );
exp2 = add( add( exp_coeff[1], exp_coeff[4] ), 1 );
if( exp1 > exp2 ){
L_tmp = L_sub( L_shr( L_tmp1, add(sub(exp1,exp2),1) ), L_shr( L_tmp2,1 ) );
exp = sub(exp2,1);
}
else{
L_tmp = L_sub( L_shr( L_tmp1,1 ), L_shr( L_tmp2, add(sub(exp2,exp1),1) ) );
exp = sub(exp1,1);
}
sft = norm_l( L_tmp );
nume = extract_h( L_shl(L_tmp, sft) );
exp_nume = sub( add( exp, sft ), 16 );
sft = sub( add( exp_nume, exp_inv_denom ), (2+16-1) );
L_acc = L_shr( L_mult( nume,inv_denom ), sft );
best_gain[1] = extract_h( L_acc ); /*-- best_gain[1]:Q2 --*/
/*--- Change Q-format of gcode0 ( Q[exp_gcode0] -> Q4 ) ---*/
if( exp_gcode0 >= 4){
gcode0_org = shr( gcode0, sub(exp_gcode0,4) );
}
else{
L_acc = L_deposit_l( gcode0 );
L_acc = L_shl( L_acc, sub( (4+16), exp_gcode0 ) );
gcode0_org = extract_h( L_acc ); /*-- gcode0_org:Q4 --*/
}
/*----------------------------------------------*
* - presearch for gain codebook - *
*----------------------------------------------*/
Gbk_presel(best_gain, &cand1, &cand2, gcode0_org );
/*---------------------------------------------------------------------------*
* *
* Find the best quantizer. *
* *
* dist_min = MAX_32; *
* for ( i=0 ; i<NCAN1 ; i++ ){ *
* for ( j=0 ; j<NCAN2 ; j++ ){ *
* g_pitch = gbk1[cand1+i][0] + gbk2[cand2+j][0]; *
* g_code = gcode0 * (gbk1[cand1+i][1] + gbk2[cand2+j][1]); *
* dist = g_pitch*g_pitch * coeff[0] *
* + g_pitch * coeff[1] *
* + g_code*g_code * coeff[2] *
* + g_code * coeff[3] *
* + g_pitch*g_code * coeff[4] ; *
* *
* if (dist < dist_min){ *
* dist_min = dist; *
* indice1 = cand1 + i ; *
* indice2 = cand2 + j ; *
* } *
* } *
* } *
* *
* g_pitch = Q13 *
* g_pitch*g_pitch = Q11:(13+13+1-16) *
* g_code = Q[exp_gcode0-3]:(exp_gcode0+(13-1)+1-16) *
* g_code*g_code = Q[2*exp_gcode0-21]:(exp_gcode0-3+exp_gcode0-3+1-16) *
* g_pitch*g_code = Q[exp_gcode0-5]:(13+exp_gcode0-3+1-16) *
* *
* term 0: g_pitch*g_pitch*coeff[0] ;exp_min0 = 13 +exp_coeff[0] *
* term 1: g_pitch *coeff[1] ;exp_min1 = 14 +exp_coeff[1] *
* term 2: g_code*g_code *coeff[2] ;exp_min2 = 2*exp_gcode0-21+exp_coeff[2] *
* term 3: g_code *coeff[3] ;exp_min3 = exp_gcode0 - 3+exp_coeff[3] *
* term 4: g_pitch*g_code *coeff[4] ;exp_min4 = exp_gcode0 - 4+exp_coeff[4] *
* *
*---------------------------------------------------------------------------*/
exp_min[0] = add( exp_coeff[0], 13 );
exp_min[1] = add( exp_coeff[1], 14 );
exp_min[2] = add( exp_coeff[2], sub( shl( exp_gcode0, 1 ), 21 ) );
exp_min[3] = add( exp_coeff[3], sub( exp_gcode0, 3 ) );
exp_min[4] = add( exp_coeff[4], sub( exp_gcode0, 4 ) );
e_min = exp_min[0];
for(i=1; i<5; i++){
if( exp_min[i] < e_min ){
e_min = exp_min[i];
}
}
/* align coeff[] and save in special 32 bit double precision */
for(i=0; i<5; i++){
j = sub( exp_min[i], e_min );
L_tmp = L_deposit_h( g_coeff[i] );
L_tmp = L_shr( L_tmp, j ); /* L_tmp:Q[exp_g_coeff[i]+16-j] */
L_Extract( L_tmp, &coeff[i], &coeff_lsf[i] ); /* DPF */
}
/* Codebook search */
L_dist_min = MAX_32;
/* initialization used only to suppress Microsoft Visual C++ warnings */
index1 = cand1;
index2 = cand2;
if(tameflag == 1){
for(i=0; i<NCAN1; i++){
for(j=0; j<NCAN2; j++){
g_pitch = add( gbk1[cand1+i][0], gbk2[cand2+j][0] ); /* Q14 */
if(g_pitch < GP0999) {
L_acc = L_deposit_l( gbk1[cand1+i][1] );
L_accb = L_deposit_l( gbk2[cand2+j][1] ); /* Q13 */
L_tmp = L_add( L_acc,L_accb );
tmp = extract_l( L_shr( L_tmp,1 ) ); /* Q12 */
g_code = mult( gcode0, tmp ); /* Q[exp_gcode0+12-15] */
g2_pitch = mult(g_pitch, g_pitch); /* Q13 */
g2_code = mult(g_code, g_code); /* Q[2*exp_gcode0-6-15] */
g_pit_cod= mult(g_code, g_pitch); /* Q[exp_gcode0-3+14-15] */
L_tmp = Mpy_32_16(coeff[0], coeff_lsf[0], g2_pitch);
//L_tmp = L_add(L_tmp, Mpy_32_16(coeff[1], coeff_lsf[1], g_pitch) );
//L_tmp = L_add(L_tmp, Mpy_32_16(coeff[2], coeff_lsf[2], g2_code) );
//L_tmp = L_add(L_tmp, Mpy_32_16(coeff[3], coeff_lsf[3], g_code) );
//L_tmp = L_add(L_tmp, Mpy_32_16(coeff[4], coeff_lsf[4], g_pit_cod) );
L_tmp += Mpy_32_16(coeff[1], coeff_lsf[1], g_pitch);
L_tmp += Mpy_32_16(coeff[2], coeff_lsf[2], g2_code);
L_tmp += Mpy_32_16(coeff[3], coeff_lsf[3], g_code);
L_tmp += Mpy_32_16(coeff[4], coeff_lsf[4], g_pit_cod);
//L_temp = L_sub(L_tmp, L_dist_min);
if( L_tmp < L_dist_min ){
L_dist_min = L_tmp;
index1 = add(cand1,i);
index2 = add(cand2,j);
}
}
}
}
}
else{
for(i=0; i<NCAN1; i++){
for(j=0; j<NCAN2; j++){
g_pitch = add( gbk1[cand1+i][0], gbk2[cand2+j][0] ); /* Q14 */
L_acc = L_deposit_l( gbk1[cand1+i][1] );
L_accb = L_deposit_l( gbk2[cand2+j][1] ); /* Q13 */
L_tmp = L_add( L_acc,L_accb );
tmp = extract_l( L_shr( L_tmp,1 ) ); /* Q12 */
g_code = mult( gcode0, tmp ); /* Q[exp_gcode0+12-15] */
g2_pitch = mult(g_pitch, g_pitch); /* Q13 */
g2_code = mult(g_code, g_code); /* Q[2*exp_gcode0-6-15] */
g_pit_cod= mult(g_code, g_pitch); /* Q[exp_gcode0-3+14-15] */
L_tmp = Mpy_32_16(coeff[0], coeff_lsf[0], g2_pitch);
//L_tmp = L_add(L_tmp, Mpy_32_16(coeff[1], coeff_lsf[1], g_pitch) );
//L_tmp = L_add(L_tmp, Mpy_32_16(coeff[2], coeff_lsf[2], g2_code) );
//L_tmp = L_add(L_tmp, Mpy_32_16(coeff[3], coeff_lsf[3], g_code) );
//L_tmp = L_add(L_tmp, Mpy_32_16(coeff[4], coeff_lsf[4], g_pit_cod) );
L_tmp += Mpy_32_16(coeff[1], coeff_lsf[1], g_pitch);
L_tmp += Mpy_32_16(coeff[2], coeff_lsf[2], g2_code);
L_tmp += Mpy_32_16(coeff[3], coeff_lsf[3], g_code);
L_tmp += Mpy_32_16(coeff[4], coeff_lsf[4], g_pit_cod);
//L_temp = L_sub(L_tmp, L_dist_min);
if( L_tmp < L_dist_min ){
L_dist_min = L_tmp;
index1 = add(cand1,i);
index2 = add(cand2,j);
}
}
}
}
/* Read the quantized gain */
/*-----------------------------------------------------------------*
* *gain_pit = gbk1[indice1][0] + gbk2[indice2][0]; *
*-----------------------------------------------------------------*/
*gain_pit = add( gbk1[index1][0], gbk2[index2][0] ); /* Q14 */
/*-----------------------------------------------------------------*
* *gain_code = (gbk1[indice1][1]+gbk2[indice2][1]) * gcode0; *
*-----------------------------------------------------------------*/
//L_acc = L_deposit_l( gbk1[index1][1] );
//L_accb = L_deposit_l( gbk2[index2][1] );
//L_gbk12 = L_add( L_acc, L_accb ); /* Q13 */
L_gbk12 = (Word32)gbk1[index1][1] + (Word32)gbk2[index2][1]; /* Q13 */
tmp = extract_l( L_shr( L_gbk12,1 ) ); /* Q12 */
L_acc = L_mult(tmp, gcode0); /* Q[exp_gcode0+12+1] */
L_acc = L_shl(L_acc, add( negate(exp_gcode0),(-12-1+1+16) ));
*gain_cod = extract_h( L_acc ); /* Q1 */
/*----------------------------------------------*
* update table of past quantized energies *
/*************************************************************************
*
* FUNCTION: calc_unfilt_energies
*
* PURPOSE: calculation of several energy coefficients for unfiltered
* excitation signals and the LTP coding gain
*
* frac_en[0]*2^exp_en[0] = <res res> // LP residual energy
* frac_en[1]*2^exp_en[1] = <exc exc> // LTP residual energy
* frac_en[2]*2^exp_en[2] = <exc code> // LTP/CB innovation dot product
* frac_en[3]*2^exp_en[3] = <lres lres> // LTP residual energy
* // (lres = res - gain_pit*exc)
* ltpg = log2(LP_res_en / LTP_res_en)
*
*************************************************************************/
void
calc_unfilt_energies(
Word16 res[], /* i : LP residual, Q0 */
Word16 exc[], /* i : LTP excitation (unfiltered), Q0 */
Word16 code[], /* i : CB innovation (unfiltered), Q13 */
Word16 gain_pit, /* i : pitch gain, Q14 */
Word16 L_subfr, /* i : Subframe length */
Word16 frac_en[], /* o : energy coefficients (4), fraction part, Q15 */
Word16 exp_en[], /* o : energy coefficients (4), exponent part, Q0 */
Word16 *ltpg /* o : LTP coding gain (log2()), Q13 */
)
{
Word32 s, L_temp;
Word16 i, exp, tmp;
Word16 ltp_res_en, pred_gain;
Word16 ltpg_exp, ltpg_frac;
/* Compute residual energy */
s = L_mac((Word32) 0, res[0], res[0]);
for (i = 1; i < L_subfr; i++)
s = L_mac(s, res[i], res[i]);
/* ResEn := 0 if ResEn < 200.0 (= 400 Q1) */
test();
if (L_sub (s, 400L) < 0)
{
frac_en[0] = 0; move16 ();
exp_en[0] = -15; move16 ();
}
else
{
exp = norm_l(s);
frac_en[0] = extract_h(L_shl(s, exp)); move16 ();
exp_en[0] = sub(15, exp); move16 ();
}
/* Compute ltp excitation energy */
s = L_mac((Word32) 0, exc[0], exc[0]);
for (i = 1; i < L_subfr; i++)
s = L_mac(s, exc[i], exc[i]);
exp = norm_l(s);
frac_en[1] = extract_h(L_shl(s, exp)); move16 ();
exp_en[1] = sub(15, exp); move16 ();
/* Compute scalar product <exc[],code[]> */
s = L_mac((Word32) 0, exc[0], code[0]);
for (i = 1; i < L_subfr; i++)
s = L_mac(s, exc[i], code[i]);
exp = norm_l(s);
frac_en[2] = extract_h(L_shl(s, exp)); move16 ();
exp_en[2] = sub(16-14, exp); move16 ();
/* Compute energy of LTP residual */
s = 0L; move32 ();
for (i = 0; i < L_subfr; i++)
{
L_temp = L_mult(exc[i], gain_pit);
L_temp = L_shl(L_temp, 1);
tmp = sub(res[i], round(L_temp)); /* LTP residual, Q0 */
s = L_mac (s, tmp, tmp);
}
exp = norm_l(s);
ltp_res_en = extract_h (L_shl (s, exp));
exp = sub (15, exp);
frac_en[3] = ltp_res_en; move16 ();
exp_en[3] = exp; move16 ();
/* calculate LTP coding gain, i.e. energy reduction LP res -> LTP res */
test (); test ();
if (ltp_res_en > 0 && frac_en[0] != 0)
{
/* gain = ResEn / LTPResEn */
pred_gain = div_s (shr (frac_en[0], 1), ltp_res_en);
exp = sub (exp, exp_en[0]);
/* L_temp = ltpGain * 2^(30 + exp) */
L_temp = L_deposit_h (pred_gain);
/* L_temp = ltpGain * 2^27 */
L_temp = L_shr (L_temp, add (exp, 3));
/* Log2 = log2() + 27 */
Log2(L_temp, <pg_exp, <pg_frac);
/* ltpg = log2(LtpGain) * 2^13 --> range: +- 4 = +- 12 dB */
L_temp = L_Comp (sub (ltpg_exp, 27), ltpg_frac);
*ltpg = round (L_shl (L_temp, 13)); /* Q13 */
}
else
{
*ltpg = 0; move16 ();
}
}
/*-----------------------------------------------------------*
* 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;
}
void Decod_ld8a(
Word16 parm[], /* (i) : vector of synthesis parameters
parm[0] = bad frame indicator (bfi) */
Word16 synth[], /* (o) : synthesis speech */
Word16 A_t[], /* (o) : decoded LP filter in 2 subframes */
Word16 *T2, /* (o) : decoded pitch lag in 2 subframes */
Word16 *Vad /* (o) : frame type */
)
{
Word16 *Az; /* Pointer on A_t */
Word16 lsp_new[M]; /* LSPs */
Word16 code[L_SUBFR]; /* ACELP codevector */
/* Scalars */
Word16 i, j, i_subfr;
Word16 T0, T0_frac, index;
Word16 bfi;
Word32 L_temp;
Word16 bad_pitch; /* bad pitch indicator */
extern Word16 bad_lsf; /* bad LSF indicator */
/* for G.729B */
Word16 ftyp;
Word16 lsfq_mem[MA_NP][M];
/* Test bad frame indicator (bfi) */
bfi = *parm++;
/* for G.729B */
ftyp = *parm;
if(bfi == 1) {
if(past_ftyp == 1) {
ftyp = 1;
parm[4] = 1; /* G.729 maintenance */
}
else ftyp = 0;
*parm = ftyp; /* modification introduced in version V1.3 */
}
*Vad = ftyp;
/* Processing non active frames (SID & not transmitted) */
if(ftyp != 1) {
Get_decfreq_prev(lsfq_mem);
Dec_cng(past_ftyp, sid_sav, sh_sid_sav, parm, exc, lsp_old,
A_t, &seed, lsfq_mem);
Update_decfreq_prev(lsfq_mem);
Az = A_t;
for (i_subfr = 0; i_subfr < L_FRAME; i_subfr += L_SUBFR) {
Overflow = 0;
Syn_filt(Az, &exc[i_subfr], &synth[i_subfr], L_SUBFR, mem_syn, 0);
if(Overflow != 0) {
/* In case of overflow in the synthesis */
/* -> Scale down vector exc[] and redo synthesis */
for(i=0; i<PIT_MAX+L_INTERPOL+L_FRAME; i++)
old_exc[i] = shr(old_exc[i], 2);
Syn_filt(Az, &exc[i_subfr], &synth[i_subfr], L_SUBFR, mem_syn, 1);
}
else
Copy(&synth[i_subfr+L_SUBFR-M], mem_syn, M);
Az += MP1;
*T2++ = old_T0;
}
sharp = SHARPMIN;
}
/* Processing active frame */
else {
seed = INIT_SEED;
parm++;
/* Decode the LSPs */
D_lsp(parm, lsp_new, add(bfi, bad_lsf));
parm += 2;
/*
Note: "bad_lsf" is introduce in case the standard is used with
channel protection.
*/
/* Interpolation of LPC for the 2 subframes */
Int_qlpc(lsp_old, lsp_new, A_t);
/* update the LSFs for the next frame */
Copy(lsp_new, lsp_old, M);
/*------------------------------------------------------------------------*
* Loop for every subframe in the analysis frame *
*------------------------------------------------------------------------*
* The subframe size is L_SUBFR and the loop is repeated L_FRAME/L_SUBFR *
* times *
* - decode the pitch delay *
* - decode algebraic code *
* - decode pitch and codebook gains *
* - find the excitation and compute synthesis speech *
*------------------------------------------------------------------------*/
Az = A_t; /* pointer to interpolated LPC parameters */
for (i_subfr = 0; i_subfr < L_FRAME; i_subfr += L_SUBFR)
{
index = *parm++; /* pitch index */
if(i_subfr == 0)
{
i = *parm++; /* get parity check result */
bad_pitch = add(bfi, i);
if( bad_pitch == 0)
{
Dec_lag3(index, PIT_MIN, PIT_MAX, i_subfr, &T0, &T0_frac);
old_T0 = T0;
}
else /* Bad frame, or parity error */
{
T0 = old_T0;
T0_frac = 0;
old_T0 = add( old_T0, 1);
if( sub(old_T0, PIT_MAX) > 0) {
old_T0 = PIT_MAX;
}
}
}
else /* second subframe */
{
if( bfi == 0)
{
Dec_lag3(index, PIT_MIN, PIT_MAX, i_subfr, &T0, &T0_frac);
old_T0 = T0;
}
else
{
T0 = old_T0;
T0_frac = 0;
old_T0 = add( old_T0, 1);
if( sub(old_T0, PIT_MAX) > 0) {
old_T0 = PIT_MAX;
}
}
}
*T2++ = T0;
/*-------------------------------------------------*
* - Find the adaptive codebook vector. *
*-------------------------------------------------*/
Pred_lt_3(&exc[i_subfr], T0, T0_frac, L_SUBFR);
/*-------------------------------------------------------*
* - Decode innovative codebook. *
* - Add the fixed-gain pitch contribution to code[]. *
*-------------------------------------------------------*/
if(bfi != 0) /* Bad frame */
{
parm[0] = Random(&seed_fer) & (Word16)0x1fff; /* 13 bits random */
parm[1] = Random(&seed_fer) & (Word16)0x000f; /* 4 bits random */
}
Decod_ACELP(parm[1], parm[0], code);
parm +=2;
j = shl(sharp, 1); /* From Q14 to Q15 */
if(sub(T0, L_SUBFR) <0 ) {
for (i = T0; i < L_SUBFR; i++) {
code[i] = add(code[i], mult(code[i-T0], j));
}
}
/*-------------------------------------------------*
* - Decode pitch and codebook gains. *
*-------------------------------------------------*/
index = *parm++; /* index of energy VQ */
Dec_gain(index, code, L_SUBFR, bfi, &gain_pitch, &gain_code);
/*-------------------------------------------------------------*
* - Update pitch sharpening "sharp" with quantized gain_pitch *
*-------------------------------------------------------------*/
sharp = gain_pitch;
if (sub(sharp, SHARPMAX) > 0) { sharp = SHARPMAX; }
if (sub(sharp, SHARPMIN) < 0) { sharp = SHARPMIN; }
/*-------------------------------------------------------*
* - Find the total excitation. *
* - Find synthesis speech corresponding to exc[]. *
*-------------------------------------------------------*/
for (i = 0; i < L_SUBFR; i++)
{
/* exc[i] = gain_pitch*exc[i] + gain_code*code[i]; */
/* exc[i] in Q0 gain_pitch in Q14 */
/* code[i] in Q13 gain_codeode in Q1 */
L_temp = L_mult(exc[i+i_subfr], gain_pitch);
L_temp = L_mac(L_temp, code[i], gain_code);
L_temp = L_shl(L_temp, 1);
exc[i+i_subfr] = round(L_temp);
}
Overflow = 0;
Syn_filt(Az, &exc[i_subfr], &synth[i_subfr], L_SUBFR, mem_syn, 0);
if(Overflow != 0)
{
/* In case of overflow in the synthesis */
/* -> Scale down vector exc[] and redo synthesis */
for(i=0; i<PIT_MAX+L_INTERPOL+L_FRAME; i++)
old_exc[i] = shr(old_exc[i], 2);
Syn_filt(Az, &exc[i_subfr], &synth[i_subfr], L_SUBFR, mem_syn, 1);
}
else
Copy(&synth[i_subfr+L_SUBFR-M], mem_syn, M);
Az += MP1; /* interpolated LPC parameters for next subframe */
}
}
/*------------*
* For G729b
*-----------*/
if(bfi == 0) {
L_temp = 0L;
for(i=0; i<L_FRAME; i++) {
L_temp = L_mac(L_temp, exc[i], exc[i]);
} /* may overflow => last level of SID quantizer */
sh_sid_sav = norm_l(L_temp);
sid_sav = round(L_shl(L_temp, sh_sid_sav));
sh_sid_sav = sub(16, sh_sid_sav);
}
/*--------------------------------------------------*
* Update signal for next frame. *
* -> shift to the left by L_FRAME exc[] *
*--------------------------------------------------*/
Copy(&old_exc[L_FRAME], &old_exc[0], PIT_MAX+L_INTERPOL);
/* for G729b */
past_ftyp = ftyp;
return;
}
