Word32 L_divide(Word32 L_num, Word32 L_denom)
{
Word16 approx;
Word32 L_div;
if (L_num < 0 || L_denom < 0 || L_num > L_denom)
{
printf("ERROR: Invalid input into L_divide!\n");
return (0);
}
/* First approximation: 1 / L_denom = 1/extract_h(L_denom) */
approx = divide_s((Word16) 0x3fff, extract_h(L_denom));
/* 1/L_denom = approx * (2.0 - L_denom * approx) */
L_div = L_mpy_ls(L_denom, approx);
L_div = L_sub((Word32) 0x7fffffffL, L_div);
L_div = L_mpy_ls(L_div, approx);
/* L_num * (1/L_denom) */
L_div = L_mpy_ll(L_num, L_div);
L_div = L_shl(L_div, 2);
return (L_div);
}
void fndppf(short *delay, short *beta, short *buf, short dmin, short dmax, short length)
{
static short b = -10224; /* rom storage */
static short a[3] =
{-18739, 16024, -4882}; /* a[] scaled down by 4 */
short dnew = 0;
short sum;
long Lsum;
register short m, i, n;
static short DECbuf[FrameSize / 4];
long Lcorrmax, Lcmax, Ltmp;
short tap1;
short M1, M2, dnewtmp = 0;
static short lastgoodpitch = 0;
static short lastbeta = 0;
static short memory[3];
static int FirstTime = 1;
short Lsum_scale;
short shift, Lcorr_scale, Lcmax_scale;
short n1, n2, nq, nq1;
long Ltempf;
/* init static variables (should be in init routine for implementation) */
if (FirstTime)
{
FirstTime = 0;
n1 = (shr(FrameSize, 2));
for (i = 0; i < n1; i++)
DECbuf[i] = 0;
memory[0] = memory[1] = memory[2] = 0;
}
/* Shift memory of DECbuf */
for (i = 0; i < shr(length, 3); i++)
{
DECbuf[i] = DECbuf[i + shr(length, 3)];
}
/* filter signal and decimate */
for (i = 0, n = shr(length, 3); i < shr(length, 1); i++)
{
Ltempf = L_shr(L_deposit_h(buf[i + shr(length, 1)]), 4);
Ltempf = L_msu(Ltempf, memory[0], a[0]);
Ltempf = L_msu(Ltempf, memory[1], a[1]);
Ltempf = L_msu(Ltempf, memory[2], a[2]);
Ltempf = L_shl(Ltempf, 2);
shift = 0;
if ((i + 1) % 4 == 0)
{
Lsum = L_add(Ltempf, L_deposit_h(memory[2]));
Lsum = L_mac(Lsum, memory[0], b);
Lsum = L_mac(Lsum, memory[1], b);
DECbuf[n++] = round(L_shl(Lsum, 1));
}
memory[2] = memory[1];
memory[1] = memory[0];
memory[0] = round(Ltempf);
}
/* perform first search for best delay value in decimated domain */
Lcorrmax = (LW_MIN);
Lcorr_scale = 1;
for (m = shr(dmin, 2); m <= shr(dmax, 2); m++)
{
n1 = 1;
for (i = 0, Lsum = 0; i < sub(shr(length, 2), m); i++)
{
Ltempf = L_mult(DECbuf[i], DECbuf[i + m]);
Ltempf = L_shr(Ltempf, n1);
Lsum = L_add(Lsum, Ltempf);
if (L_abs(Lsum) >= 0x40000000)
{
Lsum = L_shr(Lsum, 1);
n1++;
}
}
if (
((Lcorr_scale >= n1) && (L_shr(Lsum, sub(Lcorr_scale, n1)) > Lcorrmax))
|| ((Lcorr_scale < n1) && (Lsum > L_shr(Lcorrmax, sub(n1, Lcorr_scale))))
)
{
Lcorrmax = Lsum;
Lcorr_scale = n1;
dnew = m;
}
}
/* Compare against lastgoodpitch */
if (lastgoodpitch != 0 && (abs_s(sub(lastgoodpitch, shl(dnew, 2))) > 2))
{
M1 = sub(shr(lastgoodpitch, 2), 2);
if (M1 < shr(dmin, 2))
M1 = shr(dmin, 2);
M2 = add(M1, 4);
if (M2 > shr(dmax, 2))
M2 = shr(dmax, 2);
Lcmax = LW_MIN;
Lcmax_scale = 1;
for (m = M1; m <= M2; m++)
{
n1 = 1;
for (i = 0, Lsum = 0; i < sub(shr(length, 2), m); i++)
{
Ltempf = L_mult(DECbuf[i], DECbuf[i + m]);
Ltempf = L_shr(Ltempf, n1);
Lsum = L_add(Lsum, Ltempf);
if (L_abs(Lsum) >= 0x40000000)
{
Lsum = L_shr(Lsum, 1);
n1++;
}
}
if (
((Lcmax_scale >= n1) && (L_shr(Lsum, sub(Lcmax_scale, n1)) > Lcmax))
|| ((Lcmax_scale < n1) && (Lsum > L_shr(Lcmax, sub(n1, Lcmax_scale))))
)
{ /* Gives some bias to low delays */
Lcmax = Lsum;
Lcmax_scale = n1;
dnewtmp = m;
}
}
Lsum = L_mpy_ls(Lcorrmax, 27361);
if (
((Lcmax_scale >= Lcorr_scale) && (L_shr(Lsum, sub(Lcmax_scale, Lcorr_scale)) < Lcmax))
|| ((Lcmax_scale < Lcorr_scale) && (Lsum < L_shr(Lcmax, sub(Lcorr_scale, Lcmax_scale))))
)
{
dnew = dnewtmp;
}
}
/* perform first search for best delay value in non-decimated buffer */
M1 = Max(sub(shl(dnew, 2), 3), dmin);
if (M1 < dmin)
M1 = dmin;
M2 = Min(add(shl(dnew, 2), 3), dmax);
if (M2 > dmax)
M2 = dmax;
Lcorrmax = LW_MIN;
Lcorr_scale = 1;
for (m = M1; m <= M2; m++)
{
n1 = 1;
for (i = 0, Lsum = 0; i < sub(length, m); i++)
{
Ltempf = L_mult(buf[i], buf[i + m]);
Ltempf = L_shr(Ltempf, n1);
Lsum = L_add(Lsum, Ltempf);
if (L_abs(Lsum) >= 0x40000000)
{
Lsum = L_shr(Lsum, 1);
n1++;
}
}
if (
((Lcorr_scale >= n1) && (L_shr(Lsum, sub(Lcorr_scale, n1)) > Lcorrmax))
|| ((Lcorr_scale < n1) && (Lsum > L_shr(Lcorrmax, sub(n1, Lcorr_scale))))
)
{
Lcorrmax = Lsum;
Lcorr_scale = n1;
dnew = m;
}
}
Lsum_scale = 1;
for (i = 0, Lsum = 0; i < sub(length, dnew); i++)
{
Ltempf = L_mult(buf[i + dnew], buf[i + dnew]);
Ltempf = L_shr(Ltempf, Lsum_scale);
Lsum = L_add(Lsum, Ltempf);
if (L_abs(Lsum) >= 0x40000000)
{
Lsum = L_shr(Lsum, 1);
Lsum_scale++;
}
}
Lcmax_scale = 1;
for (i = 0, Lcmax = 0; i < length - dnew; i++)
{
Ltempf = L_mult(buf[i], buf[i]);
Ltempf = L_shr(Ltempf, Lcmax_scale);
Lcmax = L_add(Lcmax, Ltempf);
if (L_abs(Lcmax) >= 0x40000000)
{
Lcmax = L_shr(Lcmax, 1);
Lcmax_scale++;
}
}
nq = norm_l(Lsum);
Lsum = L_shl(Lsum, nq);
nq1 = norm_l(Lcmax);
Lcmax = L_shl(Lcmax, nq1);
Lsum = L_mpy_ll(Lsum, Lcmax);
n1 = norm_l(Lsum);
Lsum = L_shl(Lsum, n1);
sum = sqroot(Lsum);
n1 = add(add(n1, nq), nq1);
n1 = sub(sub(n1, Lcmax_scale), Lsum_scale);
n2 = shr(n1, 1);
if (n1 & 1)
Lsum = L_mult(sum, 23170);
else
Lsum = L_deposit_h(sum);
n2 = add(n2, Lcorr_scale);
Lcorrmax = L_shl(Lcorrmax, n2);
if ((Lsum == 0) || (Lcorrmax <= 0))
*beta = 0;
else if (Lcorrmax > Lsum)
*beta = 0x7fff;
else
*beta = round(L_divide(Lcorrmax, Lsum));
/* perform search for best delay value in around old pitch delay */
if (lastgoodpitch != 0)
{
M1 = lastgoodpitch - 6;
M2 = lastgoodpitch + 6;
if (M1 < dmin)
M1 = dmin;
if (M2 > dmax)
M2 = dmax;
if (dnew > M2 || dnew < M1)
{
Lcmax = LW_MIN;
Lcmax_scale = 1;
for (m = M1; m <= M2; m++)
{
n1 = 1;
for (i = 0, Lsum = 0; i < length - m; i++)
{
Ltempf = L_mult(buf[i], buf[i + m]);
Ltempf = L_shr(Ltempf, n1);
Lsum = L_add(Lsum, Ltempf);
if (L_abs(Lsum) >= 0x40000000)
{
Lsum = L_shr(Lsum, 1);
n1++;
}
}
if (
((Lcmax_scale >= n1) && (L_shr(Lsum, sub(Lcmax_scale, n1)) > Lcmax))
|| ((Lcmax_scale < n1) && (Lsum > L_shr(Lcmax, sub(n1, Lcmax_scale))))
)
{
Lcmax = Lsum;
dnewtmp = m;
Lcmax_scale = n1;
}
}
Lcorr_scale = 1;
for (i = 0, Ltmp = 0; i < length - dnewtmp; i++)
{
Ltempf = L_mult(buf[i + dnewtmp], buf[i + dnewtmp]);
Ltempf = L_shr(Ltempf, Lcorr_scale);
Ltmp = L_add(Ltmp, Ltempf);
if (L_abs(Ltmp) >= 0x40000000)
{
Ltmp = L_shr(Ltmp, 1);
Lcorr_scale++;
}
}
Lsum_scale = 1;
for (i = 0, Lsum = 0; i < length - dnewtmp; i++)
{
Ltempf = L_mult(buf[i], buf[i]);
Ltempf = L_shr(Ltempf, Lsum_scale);
Lsum = L_add(Lsum, Ltempf);
if (L_abs(Lsum) >= 0x40000000)
{
Lsum = L_shr(Lsum, 1);
Lsum_scale++;
}
}
nq = norm_l(Ltmp);
Ltmp = L_shl(Ltmp, nq);
nq1 = norm_l(Lsum);
Lsum = L_shl(Lsum, nq1);
Ltmp = L_mpy_ll(Ltmp, Lsum);
n1 = norm_l(Ltmp);
Ltmp = L_shl(Ltmp, n1);
sum = sqroot(Ltmp);
n1 = add(add(n1, nq), nq1);
n1 = sub(sub(n1, Lsum_scale), Lcorr_scale);
n2 = shr(n1, 1);
if (n1 & 1)
Ltmp = L_mult(sum, 23170);
else
Ltmp = L_deposit_h(sum);
n2 = add(n2, Lcmax_scale);
Lcmax = L_shl(Lcmax, n2);
if ((Ltmp == 0) || (Lcmax <= 0))
tap1 = 0;
else if (Lcmax >= Ltmp)
tap1 = 0x7fff;
else
tap1 = round(L_divide(Lcmax, Ltmp));
/* Replace dnew with dnewtmp if tap1 is large enough */
if ((dnew > M2 && (shr(tap1, 1) > mult_r(9830, *beta))) ||
(dnew < M1 && (shr(tap1, 1) > mult_r(19661, *beta))))
{
dnew = dnewtmp;
*beta = (tap1);
}
}
}
*delay = dnew;
if (*beta > 13107)
{
lastgoodpitch = dnew;
lastbeta = *beta;
}
else
{
lastbeta = mult_r(24576, lastbeta);
if (lastbeta < 9830)
lastgoodpitch = 0;
}
}
void modifyorig(short *residualm,
short *accshift,
short beta,
short *dpm,
short shiftrange,
short resolution,
short *TARGET,
short *residual,
short dp,
short sfend)
{
static short FirstTime = 1;
short best;
short tmp;
long ex0y[(RSHIFT * 2 + 2) * RRESOLUTION + 1]; /* Fraction sampled correlation function */
long ex0y2[RSHIFT * 2 + 2]; /* Integer sampled correlation function */
short Residual[2 * SubFrameSize + 2 * RSHIFT + 1];
static short a1[RRESOLUTION];
static short a2[RRESOLUTION];
static short a3[RRESOLUTION];
register short i, j, k, n;
short sfstart, length, shiftrangel, shiftranger;
long e01, e02;
short shft_fctr1, shft_fctr2, shft_fctr3;
long ltmp;
long laccshift;
long y;
/****** INITIALIZATION *****/
if (FirstTime)
{
FirstTime = 0;
/* Calculate interpolation coefficients */
a1[0] = 12288;
a1[1] = 8448;
a1[2] = 5120;
a1[3] = 2304;
a1[4] = 0;
a1[5] = -1792;
a1[6] = -3072;
a1[7] = -3840;
a2[0] = 24576;
a2[1] = 28160;
a2[2] = 30720;
a2[3] = 32256;
a2[4] = 32767;
a2[5] = 32256;
a2[6] = 30720;
a2[7] = 28160;
a3[0] = -4096;
a3[1] = -3840;
a3[2] = -3072;
a3[3] = -1792;
a3[4] = 0;
a3[5] = 2304;
a3[6] = 5120;
a3[7] = 8448;
}
/********************
* CORRELATION MATCH *
********************/
length = sub(sfend, dp);
sfstart = dp;
/*laccshift = L_shl(L_deposit_h(*accshift), 8); */
laccshift = L_deposit_h(*accshift); /* accshift scaled by 8 */
/* Perform before if
* * statement.
*/
if (shiftrange != 0)
{
/* Limit the search range to control accshift */
shiftrangel = shiftranger = shiftrange;
if (*accshift < 0)
shiftrangel = add(shiftrangel, 1);
if (*accshift > 0)
shiftranger = add(shiftranger, 1);
tmp = abs_s(*accshift);
/* For non-periodic signals */
if ((beta < 6554 && tmp > 15 * 256) || (beta < 9830 && tmp > 30 * 256))
{
if (*accshift < 0)
shiftranger = 1;
else
shiftrangel = 1;
}
if (add(shiftrangel, shr(*accshift, 8)) > 72)
{
shiftrangel = sub(72, shr(*accshift, 8));
fprintf(stderr, "mdfyorig:*** Buffer limit. shiftrangel is:%d\n", shiftrangel);
}
if (sub(shiftranger, shr(*accshift, 8)) > 72)
{
shiftranger = add(72, shr(*accshift, 8));
fprintf(stderr, "mdfyorig:*** Buffer limit. shiftranger is:%d\n", shiftranger);
}
/* Create a buffer of modify residual for match at low cut-off frequency */
tmp = add(length, shiftrangel);
tmp = add(tmp, shiftranger);
ltmp = L_deposit_h(add(*accshift, shl(shiftrangel, 8)));
for (i = 0; i <= tmp; i++)
{
/* POINTER ADDITION NOT CONVERTED -- NEED UNSIGNED ADDITION */
bl_intrp(Residual + i, residual + dp + i, ltmp, 16384, 3);
}
tmp = add(shiftrangel, shiftranger);
/* Search for all integer delays of residual */
for (n = 0; n <= tmp; n++)
{
ex0y2[n] = 0;
for (i = 0; i < length; i++)
{
ex0y2[n] = L_mac(ex0y2[n], Residual[n + i], TARGET[sfstart + i]);
}
ex0y2[n] = L_shr(ex0y2[n], 1);
}
/* Do quadratic interpolation of ex0y */
for (n = 1, k = 0; n < shiftrangel + shiftranger; n++)
{
for (j = 0; j < resolution; j++)
{
ex0y[k] = L_mpy_ls(ex0y2[n - 1], a1[j]);
ltmp = L_mpy_ls(ex0y2[n], a2[j]);
ex0y[k] = L_add(ex0y[k], ltmp);
ltmp = L_mpy_ls(ex0y2[n + 1], a3[j]);
ex0y[k] = L_add(ex0y[k], ltmp);
k++;
}
}
/* Find maximum with positive correlation */
y = 0;
best = sub(shl(shiftrangel, 3), 4);
for (n = 0; n < k; n++)
{
if (ex0y[n] > y)
{
y = ex0y[n];
best = n;
}
}
/* best value not very accurate since ex0y[] calculation not percise. To correct
* Residual[] should have more percision. This error does not seem to affect the
* final output using the test data.
*/
/* Calculate energy in selected shift index */
e01 = e02 = 0;
for (i = shiftrangel; i < length + shiftrangel; i++)
e01 = L_mac(e01, Residual[i], Residual[i]);
for (i = 0; i < length; i++)
e02 = L_mac(e02, TARGET[i + sfstart], TARGET[i + sfstart]);
if (e01 == 0 || e02 == 0)
y = 0;
else
{
shft_fctr1 = norm_l(y);
y = L_shl(y, shft_fctr1);
y = L_mpy_ll(y, y);
/* TO RECOVER Y VALUE:
* y = L_shl(y, 31-2*shft_fctr1) */
shft_fctr2 = norm_l(e01);
e01 = L_shl(e01, shft_fctr2);
shft_fctr3 = norm_l(e02);
e02 = L_shl(e02, shft_fctr3);
ltmp = L_mpy_ll(e01, e02);
ltmp = L_mpy_ls(ltmp, 16056);
ltmp = L_shl(ltmp, shl(shft_fctr1, 1) - add(add(2, shft_fctr2), shft_fctr3));
if (y > ltmp)
{
/*tmp = shl(shr(*accshift,8) + shiftrangel, 3) - (best + 4); */
tmp = *accshift + shl(shiftrangel, 8) - shl((best + 4), 5);
*accshift = tmp;
laccshift = L_deposit_h(tmp);
/**accshift = shift_r(tmp, -3);*/
/*
* if (laccshift == -75497472)
* laccshift = -88080384;
*/
}
}
}
for (k = 0; k < length; k++)
{
bl_intrp(residualm + dp + k, residual + dp + k, laccshift, BLFREQ, BLPRECISION);
}
*dpm = add(dp, length);
}
