/**
*
* function name: FreqToBandWithRounding
* description: Retrieve index of nearest band border
* returnt: index
*
*/
static Word16 FreqToBandWithRounding(Word32 freq, /*!< frequency in Hertz */
Word32 fs, /*!< Sampling frequency in Hertz */
Word16 numOfBands, /*!< total number of bands */
const Word16 *bandStartOffset) /*!< table of band borders */
{
Word32 lineNumber, band;
Word32 temp, shift;
/* assert(freq >= 0); */
shift = norm_l(fs);
lineNumber = (extract_l(fixmul((bandStartOffset[numOfBands] << 2),Div_32(freq << shift,fs << shift))) + 1) >> 1;
/* freq > fs/2 */
temp = lineNumber - bandStartOffset[numOfBands] ;
if (temp >= 0)
return numOfBands;
/* find band the line number lies in */
for (band=0; band<numOfBands; band++) {
temp = bandStartOffset[band + 1] - lineNumber;
if (temp > 0) break;
}
temp = (lineNumber - bandStartOffset[band]);
temp = (temp - (bandStartOffset[band + 1] - lineNumber));
if ( temp > 0 )
{
band = band + 1;
}
return extract_l(band);
}
void Levinson(int32_t r32[], /* (i) : r32[] double precision vector of autocorrelation coefficients */
int16_t a[], /* (o) : a[] in Q12 - LPC coefficients */
int16_t old_a[], /* (i/o): old_a[] in Q12 - previous LPC coefficients */
int16_t m /* (i) : LPC order */
)
{
int16_t i, j, high, low, alpha_hi, alpha_lo, alpha_exp;
int16_t exp, r_hi[LPCO + 1], r_lo[LPCO + 1];
int16_t a_hi[LPCO + 1], a_lo[LPCO + 1], anew_hi[LPCO + 1],
anew_lo[LPCO + 1];
int16_t rc_hi, rc_lo;
int32_t a0, a1, alpha_man;
memzero(r_hi, (LPCO + 1) * sizeof(int16_t));
memzero(r_lo, (LPCO + 1) * sizeof(int16_t));
memzero(anew_hi, (LPCO + 1) * sizeof(int16_t));
memzero(anew_lo, (LPCO + 1) * sizeof(int16_t));
/* Normalization of autocorrelation coefficients */
exp = bv_norm_l(r32[0]);
for (i = 0; i <= m; i++) {
r32[i] = L_bv_shl(r32[i], exp);
L_Extract(r32[i], r_hi + i, r_lo + i);
}
/* a[1] = rc = -r[1]/r[0] */
a1 = bv_L_abs(r32[1]);
a0 = Div_32(a1, r_hi[0], r_lo[0]); // rc in Q31
if (r32[1] > 0)
a0 = L_bv_negate(a0);
L_Extract(L_bv_shr(a0, 4), a_hi + 1, a_lo + 1); // Q27
/* alpha = r[0]*(1-rc*rc) */
L_Extract(a0, &high, &low);
a0 = Mpy_32(high, low, high, low); // rc^2 in Q31
a0 = bv_L_abs(a0); // Lesson from G.729
a0 = L_bv_sub(0x40000000, L_bv_shr(a0, 1)); // 1-rc*rc in Q30
L_Extract(a0, &high, &low);
a0 = Mpy_32(r_hi[0], r_lo[0], high, low); // alpha in Q30
alpha_exp = bv_norm_l(a0);
alpha_man = L_bv_shl(a0, alpha_exp);
alpha_exp = bv_sub(alpha_exp, 1); // alpha: Q(31+alpha_exp)
/* Recursive solution of Yule-Walker equations */
for (i = 2; i <= m; i++) {
/* s = r[i] + sum{r[j]*a[i-j], j=1,2,...,i-1} */
a0 = 0;
for (j = 1; j < i; j++) {
a1 = Mpy_32(r_hi[j], r_lo[j], a_hi[i - j], a_lo[i - j]); // Q27
a0 = L_bv_add(a0, a1); // Q27
}
a0 = L_bv_shl(a0, 4); // Q31
a0 = L_bv_add(a0, r32[i]); // Q31
/* rc = -s/alpha */
exp = bv_norm_l(a0);
a0 = L_bv_shl(a0, exp);
a1 = bv_L_abs(a0);
if (L_bv_sub(a1, alpha_man) >= 0) {
a1 = L_bv_shr(a1, 1);
exp = bv_sub(exp, 1);
}
L_Extract(alpha_man, &alpha_hi, &alpha_lo);
a1 = Div_32(a1, alpha_hi, alpha_lo);
if (a0 > 0)
a1 = L_bv_negate(a1); // rc in Q(31+exp-alpha_exp)
a1 = L_bv_shr(a1, bv_sub(exp, alpha_exp)); // rc in Q31
L_Extract(a1, &rc_hi, &rc_lo); // rc in Q31
/* Check for absolute value of reflection coefficient - stability */
if (bv_sub(bv_abs_s(intround(a1)), 32750) > 0) {
a[0] = 4096;
for (j = 1; j <= m; j++)
a[j] = old_a[j];
return;
}
/* anew[j]=a[j]+rc*a[i-j], j=1,2,...i-1 */
/* anew[i]=rc */
for (j = 1; j < i; j++) {
a0 = Mpy_32(a_hi[i - j], a_lo[i - j], rc_hi, rc_lo); // Q27
a0 = L_bv_add(a0, L_Comp(a_hi[j], a_lo[j])); // Q27
L_Extract(a0, anew_hi + j, anew_lo + j); // Q27
}
L_Extract(L_bv_shr(a1, 4), anew_hi + i, anew_lo + i); // Q27
/* alpha = alpha*(1-rc*rc) */
a0 = Mpy_32(rc_hi, rc_lo, rc_hi, rc_lo); // rc*rc in Q31
a0 = bv_L_abs(a0); // Lesson from G.729
a0 = L_bv_shr(a0, 1); // rc*rc in Q30
a0 = L_bv_sub(0x40000000, a0); // 1-rc*rc in Q30
L_Extract(a0, &high, &low);
a0 = Mpy_32(alpha_hi, alpha_lo, high, low); // Q(30+alpha_exp)
exp = bv_norm_l(a0);
alpha_man = L_bv_shl(a0, exp); // Q(30+exp+alpha_exp)
alpha_exp = bv_sub(bv_add(alpha_exp, exp), 1); // alpha: Q(31+alpha_exp)
/* a[j] = anew[j] in Q(12+a1_exp) */
for (j = 1; j <= i; j++) {
a_hi[j] = anew_hi[j];
a_lo[j] = anew_lo[j];
}
}
/* convert lpc coefficients to Q12 and save new lpc as old lpc for next frame */
a[0] = 4096;
for (j = 1; j <= m; j++) {
a[j] = intround(L_bv_shl(L_Comp(a_hi[j], a_lo[j]), 1)); // Q12
old_a[j] = a[j];
}
return;
}
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: Levinson()
*
* PURPOSE: Levinson-Durbin algorithm in double precision. To compute the
* LP filter parameters from the speech autocorrelations.
*
* DESCRIPTION:
* R[i] autocorrelations.
* A[i] filter coefficients.
* K reflection coefficients.
* Alpha prediction gain.
*
* Initialisation:
* A[0] = 1
* K = -R[1]/R[0]
* A[1] = K
* Alpha = R[0] * (1-K**2]
*
* Do for i = 2 to M
*
* S = SUM ( R[j]*A[i-j] ,j=1,i-1 ) + R[i]
*
* K = -S / Alpha
*
* An[j] = A[j] + K*A[i-j] for j=1 to i-1
* where An[i] = new A[i]
* An[i]=K
*
* Alpha=Alpha * (1-K**2)
*
* END
*
*************************************************************************/
int Levinson (
LevinsonState *st,
Word16 Rh[], /* i : Rh[m+1] Vector of autocorrelations (msb) */
Word16 Rl[], /* i : Rl[m+1] Vector of autocorrelations (lsb) */
Word16 A[], /* o : A[m] LPC coefficients (m = 10) */
Word16 rc[] /* o : rc[4] First 4 reflection coefficients */
)
{
Word16 i, j;
Word16 hi, lo;
Word16 Kh, Kl; /* reflexion coefficient; hi and lo */
Word16 alp_h, alp_l, alp_exp; /* Prediction gain; hi lo and exponent */
Word16 Ah[M + 1], Al[M + 1]; /* LPC coef. in double prec. */
Word16 Anh[M + 1], Anl[M + 1];/* LPC coef.for next iteration in double
prec. */
Word32 t0, t1, t2; /* temporary variable */
/* K = A[1] = -R[1] / R[0] */
t1 = L_Comp (Rh[1], Rl[1]);
t2 = L_abs (t1); /* abs R[1] */
t0 = Div_32 (t2, Rh[0], Rl[0]); /* R[1]/R[0] */
if (t1 > 0)
t0 = L_negate (t0); /* -R[1]/R[0] */
L_Extract (t0, &Kh, &Kl); /* K in DPF */
rc[0] = round (t0);
t0 = L_shr (t0, 4); /* A[1] in */
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 */
t0 = L_abs (t0); /* Some case <0 !! */
t0 = L_sub ((Word32) 0x7fffffffL, t0); /* 1 - K*K */
L_Extract (t0, &hi, &lo); /* DPF format */
t0 = Mpy_32 (Rh[0], Rl[0], hi, lo); /* Alpha in */
/* 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);
t1 = L_Comp (Rh[i], Rl[i]);
t0 = L_add (t0, t1); /* add R[i] */
/* K = -t0 / Alpha */
t1 = L_abs (t0);
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 */
if (sub (i, 5) < 0)
{
rc[i - 1] = round (t2);
}
/* 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] = st->old_A[j];
}
for (j = 0; j < 4; j++)
{
rc[j] = 0;
}
return 0;
}
/*------------------------------------------*
* 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);
L_Extract (t2, &Anh[i], &Anl[i]);
/* Alpha = Alpha * (1-K**2) */
t0 = Mpy_32 (Kh, Kl, Kh, Kl); /* K*K */
t0 = L_abs (t0); /* Some case <0 !! */
t0 = L_sub ((Word32) 0x7fffffffL, t0); /* 1 - K*K */
L_Extract (t0, &hi, &lo); /* DPF format */
t0 = Mpy_32 (alp_h, alp_l, hi, lo);
/* 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];
}
}
A[0] = 4096;
for (i = 1; i <= M; i++)
{
t0 = L_Comp (Ah[i], Al[i]);
st->old_A[i] = A[i] = round (L_shl (t0, 1));
}
return 0;
}
