Word16 coarsepitch(
Word16 *xw, /* (i) (normalized) weighted signal */
struct BV16_Encoder_State *cstate) /* (i/o) Coder State */
{
Word16 s; /* Q2 */
Word16 a, b;
Word16 im;
Word16 maxdev, flag, mpflag;
Word32 eni, deltae;
Word32 cc;
Word16 ah,al, bh, bl;
Word32 a0, a1, a2, a3;
Word32 *lp0;
Word16 exp, new_exp;
Word16 *fp0, *fp1, *fp2, *fp3, *sp;
Word16 *fp1_h, *fp1_l, *fp2_h, *fp2_l;
Word16 cor2max, cor2max_exp;
Word16 cor2m, cor2m_exp;
Word16 s0, t0, t1, exp0, exp1, e2, e3;
Word16 threshold;
Word16 mplth; /* Q2 */
Word16 i, j, k, n, npeaks, imax, idx[MAXPPD-MINPPD+1];
Word16 cpp;
Word16 plag[HMAXPPD], cor2[MAXPPD1], cor2_exp[MAXPPD1];
Word16 cor2i[HMAXPPD], cor2i_exp[HMAXPPD], xwd[LXD];
Word16 tmp_h[DFO+FRSZ], tmp_l[DFO+FRSZ]; /* DPF Q7 */
Word32 cor[MAXPPD1], energy[MAXPPD1], lxwd[FRSZD];
Word16 energy_man[MAXPPD1], energy_exp[MAXPPD1];
Word16 energyi_man[HMAXPPD], energyi_exp[HMAXPPD];
Word16 energym_man, energym_exp;
Word16 energymax_man, energymax_exp;
/* Lowpass filter xw() to 800 hz; shift & output into xwd() */
/* AP and AZ filtering and decimation */
fp1_h = tmp_h + DFO;
fp1_l = tmp_l + DFO;
sp = xw;
a1 = 1;
for (i=0;i<DFO;i++) tmp_h[i] = cstate->dfm_h[2*i+1];
for (i=0;i<DFO;i++) tmp_l[i] = cstate->dfm_h[2*i];
lp0 = lxwd;
for (i=0;i<FRSZD;i++) {
for (k=0;k<DECF;k++) {
a0 = L_shr(L_deposit_h(*sp++),10);
fp2_h = fp1_h-1;
fp2_l = fp1_l-1;
for (j=0;j<DFO;j++)
a0=L_sub(a0,Mpy_32(*fp2_h--,*fp2_l--,adf_h[j+1],adf_l[j+1]));
a0 = L_shl(a0, 2); /* adf Q13 */
L_Extract(a0, fp1_h++, fp1_l++);
}
fp2_h = fp1_h-1;
fp2_l = fp1_l-1;
a0 = Mpy_32_16(*fp2_h--, *fp2_l--, bdf[0]);
for (j=0;j<DFO;j++)
a0=L_add(a0,Mpy_32_16(*fp2_h--,*fp2_l--,bdf[j+1]));
*lp0++ = a0;
a0 = L_abs(a0);
if (a1 < a0)
a1 = a0;
}
/* copy temp buffer to memory */
fp1_h -= DFO;
fp1_l -= DFO;
for (i=0;i<DFO;i++) {
cstate->dfm_h[2*i+1] = fp1_h[i];
cstate->dfm_h[2*i] = fp1_l[i];
}
lp0 = lxwd;
new_exp = sub(norm_l(a1), 3); /* headroom to avoid overflow */
exp = sub(cstate->xwd_exp,new_exp); /* increase in bit-resolution */
if (exp < 0) { /* Descending signal level */
new_exp = cstate->xwd_exp;
exp = 0;
}
for (i=0;i<XDOFF;i++)
xwd[i] = shr(cstate->xwd[i], exp);
/* fill-in new exponent */
fp0 = xwd + XDOFF;
for (i=0;i<FRSZD;i++)
fp0[i] = intround(L_shl(lp0[i],new_exp));
/* update signal memory for next frame */
exp0 = 1;
for (i=0;i<XDOFF;i++) {
exp1 = abs_s(xwd[FRSZD+i]);
if (exp1 > exp0)
exp0 = exp1;
}
exp0 = sub(norm_s(exp0),3); /* extra exponent for next frame */
exp = sub(exp0, exp);
if (exp >=0)
{
for (i=0;i<XDOFF-FRSZD;i++)
cstate->xwd[i] = shl(cstate->xwd[i+FRSZD], exp);
}
else
{
exp = -exp;
if (exp >=15)
exp = 15;
for (i=0;i<XDOFF-FRSZD;i++)
cstate->xwd[i] = shr(cstate->xwd[i+FRSZD], exp);
}
for (;i<XDOFF;i++)
cstate->xwd[i] = shl(xwd[FRSZD+i],exp0);
cstate->xwd_exp = add(new_exp, exp0);
/* Compute correlation & energy of prediction basis vector */
/* reset local buffers */
for (i=0;i<MAXPPD1;i++)
cor[i] = energy[i] = 0;
fp0 = xwd+MAXPPD1;
fp1 = xwd+MAXPPD1-M1;
a0 = a1 = 0;
for (i=0;i<(LXD-MAXPPD1);i++) {
a0 = L_mac0(a0, *fp1, *fp1);
a1 = L_mac0(a1, *fp0++, *fp1++);
}
cor[M1-1] = a1;
energy[M1-1] = a0;
energy_exp[M1-1] = norm_l(energy[M1-1]);
energy_man[M1-1] = extract_h(L_shl(energy[M1-1], energy_exp[M1-1]));
s0 = cor2_exp[M1-1] = norm_l(a1);
t0 = extract_h(L_shl(a1, s0));
cor2[M1-1] = extract_h(L_mult(t0, t0));
if (a1 < 0)
cor2[M1-1] = negate(cor2[M1-1]);
fp2 = xwd+LXD-M1-1;
fp3 = xwd+MAXPPD1-M1-1;
for (i=M1;i<M2;i++) {
fp0 = xwd+MAXPPD1;
fp1 = xwd+MAXPPD1-i-1;
a1 = 0;
for (j=0;j<(LXD-MAXPPD1);j++)
a1 = L_mac0(a1,*fp0++,*fp1++);
cor[i] = a1;
a0 = L_msu0(a0, *fp2, *fp2);
a0 = L_mac0(a0, *fp3, *fp3);
fp2--; fp3--;
energy[i] = a0;
energy_exp[i] = norm_l(energy[i]);
energy_man[i] = extract_h(L_shl(energy[i], energy_exp[i]));
s0 = cor2_exp[i] = norm_l(a1);
t0 = extract_h(L_shl(a1, s0));
cor2[i] = extract_h(L_mult(t0, t0));
if (a1 < 0)
cor2[i] = negate(cor2[i]);
}
/* Find positive correlation peaks */
/* Find maximum of cor*cor/energy among positive correlation peaks */
npeaks = 0;
n = MINPPD-1;
while ((npeaks < MAX_NPEAKS) && (n<MAXPPD)) {
if (cor[n]>0) {
a0 = L_mult(energy_man[n-1],cor2[n]);
a1 = L_mult(energy_man[n], cor2[n-1]);
exp0 = shl(sub(cor2_exp[n], cor2_exp[n-1]),1);
exp0 = add(exp0, energy_exp[n-1]);
exp0 = sub(exp0, energy_exp[n]);
if (exp0>=0)
a0 = L_shr(a0, exp0);
else
a1 = L_shl(a1, exp0);
if (a0 > a1) {
a0 = L_mult(energy_man[n+1],cor2[n]);
a1 = L_mult(energy_man[n], cor2[n+1]);
exp0 = shl(sub(cor2_exp[n], cor2_exp[n+1]),1);
exp0 = add(exp0, energy_exp[n+1]);
exp0 = sub(exp0, energy_exp[n]);
if (exp0>=0)
a0 = L_shr(a0, exp0);
else
a1 = L_shl(a1, exp0);
if (a0 > a1) {
idx[npeaks] = n;
npeaks++;
}
}
}
n++;
}
/* Return early if there is no peak or only one peak */
if (npeaks == 0){ /* if there are no positive peak, */
return MINPPD*DECF; /* return minimum pitch period in decimated domain */
}
if (npeaks == 1){ /* if there is exactly one peak, */
return (idx[0]+1)*DECF; /* return the time lag for this single peak */
}
/* If program proceeds to here, there are 2 or more peaks */
cor2max=(Word16) 0x8000;
cor2max_exp= (Word16) 0;
energymax_man=1;
energymax_exp=0;
imax=0;
for (i=0; i < npeaks; i++) {
/* Use quadratic interpolation to find the interpolated cor[] and
energy[] corresponding to interpolated peak of cor2[]/energy[] */
/* first calculate coefficients of y(x)=ax^2+bx+c; */
n=idx[i];
a0=L_sub(L_shr(L_add(cor[n+1],cor[n-1]),1),cor[n]);
L_Extract(a0, &ah, &al);
a0=L_shr(L_sub(cor[n+1],cor[n-1]),1);
L_Extract(a0, &bh, &bl);
cc=cor[n];
/* Initialize variables before searching for interpolated peak */
im=0;
cor2m_exp = cor2_exp[n];
cor2m = cor2[n];
energym_exp = energy_exp[n];
energym_man = energy_man[n];
eni=energy[n];
/* Determine which side the interpolated peak falls in, then
do the search in the appropriate range */
a0 = L_mult(energy_man[n-1],cor2[n+1]);
a1 = L_mult(energy_man[n+1], cor2[n-1]);
exp0 = shl(sub(cor2_exp[n+1], cor2_exp[n-1]),1);
exp0 = add(exp0, energy_exp[n-1]);
exp0 = sub(exp0, energy_exp[n+1]);
if (exp0>=0)
a0 = L_shr(a0, exp0);
else
a1 = L_shl(a1, exp0);
if (a0 > a1) { /* if right side */
deltae = L_shr(L_sub(energy[n+1], eni), 2);
for (k = 0; k < HDECF; k++) {
a0=L_add(L_add(Mpy_32_16(ah,al,x2[k]),Mpy_32_16(bh,bl,x[k])),cc);
eni = L_add(eni, deltae);
a1 = eni;
exp0 = norm_l(a0);
s0 = extract_h(L_shl(a0, exp0));
s0 = extract_h(L_mult(s0, s0));
e2 = energym_exp;
t0 = energym_man;
a2 = L_mult(t0, s0);
e3 = norm_l(a1);
t1 = extract_h(L_shl(a1, e3));
a3 = L_mult(t1, cor2m);
exp1 = shl(sub(exp0, cor2m_exp),1);
exp1 = add(exp1, e2);
exp1 = sub(exp1, e3);
if (exp1>=0)
a2 = L_shr(a2, exp1);
else
a3 = L_shl(a3, exp1);
if (a2 > a3) {
im = k+1;
cor2m = s0;
cor2m_exp = exp0;
energym_exp = e3;
energym_man = t1;
}
}
} else { /* if interpolated peak is on the left side */
deltae = L_shr(L_sub(energy[n-1], eni), 2);
for (k = 0; k < HDECF; k++) {
a0=L_add(L_sub(Mpy_32_16(ah,al,x2[k]),Mpy_32_16(bh,bl,x[k])),cc);
eni = L_add(eni, deltae);
a1=eni;
exp0 = norm_l(a0);
s0 = extract_h(L_shl(a0, exp0));
s0 = extract_h(L_mult(s0, s0));
e2 = energym_exp;
t0 = energym_man;
a2 = L_mult(t0, s0);
e3 = norm_l(a1);
t1 = extract_h(L_shl(a1, e3));
a3 = L_mult(t1, cor2m);
exp1 = shl(sub(exp0, cor2m_exp),1);
exp1 = add(exp1, e2);
exp1 = sub(exp1, e3);
if (exp1>=0)
a2 = L_shr(a2, exp1);
else
a3 = L_shl(a3, exp1);
if (a2 > a3) {
im = -k-1;
cor2m = s0;
cor2m_exp = exp0;
energym_exp = e3;
energym_man = t1;
}
}
}
/* Search done; assign cor2[] and energy[] corresponding to
interpolated peak */
plag[i]=add(shl(add(idx[i],1),2),im); /* lag of interp. peak */
cor2i[i]=cor2m;
cor2i_exp[i]=cor2m_exp;
/* interpolated energy[] of i-th interpolated peak */
energyi_exp[i] = energym_exp;
energyi_man[i] = energym_man;
/* Search for global maximum of interpolated cor2[]/energy[] peak */
a0 = L_mult(cor2m,energymax_man);
a1 = L_mult(cor2max, energyi_man[i]);
exp0 = shl(sub(cor2m_exp, cor2max_exp),1);
exp0 = add(exp0, energymax_exp);
exp0 = sub(exp0, energyi_exp[i]);
if (exp0 >=0)
a0 = L_shr(a0, exp0);
else
a1 = L_shl(a1, exp0);
if (a0 > a1) {
imax=i;
cor2max=cor2m;
cor2max_exp=cor2m_exp;
energymax_exp = energyi_exp[i];
energymax_man = energyi_man[i];
}
}
cpp=plag[imax]; /* first candidate for coarse pitch period */
mplth=plag[npeaks-1]; /* set mplth to the lag of last peak */
/* Find the largest peak (if there is any) around the last pitch */
maxdev= shr(cstate->cpplast,2); /* maximum deviation from last pitch */
im = -1;
cor2m=(Word16) 0x8000;
cor2m_exp= (Word16) 0;
energym_man = 1;
energym_exp = 0;
for (i=0;i<npeaks;i++) { /* loop thru the peaks before the largest peak */
if (abs_s(sub(plag[i],cstate->cpplast)) <= maxdev) {
a0 = L_mult(cor2i[i],energym_man);
a1 = L_mult(cor2m, energyi_man[i]);
exp0 = shl(sub(cor2i_exp[i], cor2m_exp),1);
exp0 = add(exp0, energym_exp);
exp0 = sub(exp0, energyi_exp[i]);
if (exp0 >=0)
a0 = L_shr(a0, exp0);
else
a1 = L_shl(a1, exp0);
if (a0 > a1) {
im=i;
cor2m=cor2i[i];
cor2m_exp=cor2i_exp[i];
energym_man = energyi_man[i];
energym_exp = energyi_exp[i];
}
}
} /* if there is no peaks around last pitch, then im is still -1 */
/* Now see if we should pick any alternatice peak */
/* first, search first half of pitch range, see if any qualified peak
has large enough peaks at every multiple of its lag */
i=0;
while (2*plag[i] < mplth) {
/* Determine the appropriate threshold for this peak */
if (i != im) { /* if not around last pitch, */
threshold = TH1; /* use a higher threshold */
} else { /* if around last pitch */
threshold = TH2; /* use a lower threshold */
}
/* If threshold exceeded, test peaks at multiples of this lag */
a0 = L_mult(cor2i[i],energymax_man);
t1 = extract_h(L_mult(energyi_man[i], threshold));
a1 = L_mult(cor2max, t1);
exp0 = shl(sub(cor2i_exp[i], cor2max_exp),1);
exp0 = add(exp0, energymax_exp);
exp0 = sub(exp0, energyi_exp[i]);
if (exp0 >=0)
a0 = L_shr(a0, exp0);
else
a1 = L_shl(a1, exp0);
if (a0 > a1) {
flag=1;
j=i+1;
k=0;
s=shl(plag[i],1); /* initialize t to twice the current lag */
while (s<=mplth) { /* loop thru all multiple lag <= mplth */
mpflag=0; /* initialize multiple pitch flag to 0 */
t0 = mult_r(s,MPDTH);
a=sub(s, t0); /* multiple pitch range lower bound */
b=add(s, t0); /* multiple pitch range upper bound */
while (j < npeaks) { /* loop thru peaks with larger lags */
if (plag[j] > b) { /* if range exceeded, */
break; /* break the innermost while loop */
} /* if didn't break, then plag[j] <= b */
if (plag[j] > a) { /* if current peak lag within range, */
/* then check if peak value large enough */
a0 = L_mult(cor2i[j],energymax_man);
if (k<4)
t1 = MPTH[k];
else
t1 = MPTH4;
t1 = extract_h(L_mult(t1, energyi_man[j]));
a1 = L_mult(cor2max, t1);
exp0 = shl(sub(cor2i_exp[j], cor2max_exp),1);
exp0 = add(exp0, energymax_exp);
exp0 = sub(exp0, energyi_exp[j]);
if (exp0 >=0)
a0 = L_shr(a0, exp0);
else
a1 = L_shl(a1, exp0);
if (a0 > a1) {
mpflag=1; /* if peak large enough, set mpflag, */
break; /* and break the innermost while loop */
}
}
j++;
}
/* if no qualified peak found at this multiple lag */
if (mpflag == 0) {
flag=0; /* disqualify the lag plag[i] */
break; /* and break the while (s<=mplth) loop */
}
k++;
s = add(s, plag[i]); /* update s to the next multiple pitch lag */
}
/* if there is a qualified peak at every multiple of plag[i], */
if (flag == 1) {
cpp = plag[i]; /* then accept this as final pitch */
return cpp; /* and return to calling function */
}
}
i++;
if (i == npeaks)
break; /* to avoid out of array bound error */
}
/* If program proceeds to here, none of the peaks with lags < 0.5*mplth
qualifies as the final pitch. in this case, check if
there is any peak large enough around last pitch. if so, use its
lag as the final pitch. */
if (im != -1) { /* if there is at least one peak around last pitch */
if (im == imax) { /* if this peak is also the global maximum, */
return cpp; /* return first pitch candidate at global max */
}
if (im < imax) { /* if lag of this peak < lag of global max, */
a0 = L_mult(cor2m,energymax_man);
t1 = extract_h(L_mult(energym_man, LPTH2));
a1 = L_mult(cor2max, t1);
exp0 = shl(sub(cor2m_exp, cor2max_exp),1);
exp0 = add(exp0, energymax_exp);
exp0 = sub(exp0, energym_exp);
if (exp0 >=0)
a0 = L_shr(a0, exp0);
else
a1 = L_shl(a1, exp0);
if (a0 > a1) {
if (plag[im] > HMAXPPD*DECF) {
cpp=plag[im];
return cpp;
}
for (k=2; k<=5;k++) { /* check if current candidate pitch */
s=mult(plag[imax],invk[k-2]); /* is a sub-multiple of */
t0 = mult_r(s,SMDTH);
a=sub(s, t0); /* the time lag of */
b=add(s, t0); /* the global maximum peak */
if (plag[im]>a && plag[im]<b) { /* if so, */
cpp=plag[im]; /* accept this peak, */
return cpp; /* and return as pitch */
}
}
}
} else { /* if lag of this peak > lag of global max, */
a0 = L_mult(cor2m,energymax_man);
t1 = extract_h(L_mult(energym_man, LPTH1));
a1 = L_mult(cor2max, t1);
exp0 = shl(sub(cor2m_exp, cor2max_exp),1);
exp0 = add(exp0, energymax_exp);
exp0 = sub(exp0, energym_exp);
if (exp0 >=0)
a0 = L_shr(a0, exp0);
else
a1 = L_shl(a1, exp0);
if (a0 > a1) {
cpp = plag[im]; /* if this peak is large enough, */
return cpp; /* accept its lag */
}
}
}
/* If program proceeds to here, we have no choice but to accept the
lag of the global maximum */
return cpp;
}
void 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;
}
/* 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 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;
}