int32_t Div_32(int32_t L_num, int16_t denom_hi, int16_t denom_lo)
{
int16_t approx, hi, lo, n_hi, n_lo;
int32_t L_32;
/* First approximation: 1 / L_denom = 1/denom_hi */
approx = bv_div_s((int16_t) 0x3fff, denom_hi); /* result in Q14 */
/* Note: 3fff = 0.5 in Q15 */
/* 1/L_denom = approx * (2.0 - L_denom * approx) */
L_32 = Mpy_32_16(denom_hi, denom_lo, approx); /* result in Q30 */
L_32 = L_bv_sub((int32_t) 0x7fffffffL, L_32); /* result in Q30 */
L_Extract(L_32, &hi, &lo);
L_32 = Mpy_32_16(hi, lo, approx); /* = 1/L_denom in Q29 */
/* L_num * (1/L_denom) */
L_Extract(L_32, &hi, &lo);
L_Extract(L_num, &n_hi, &n_lo);
L_32 = Mpy_32(n_hi, n_lo, hi, lo); /* result in Q29 */
L_32 = L_bv_shl(L_32, 2); /* From Q29 to Q31 */
return (L_32);
}
Word32 Div_32(Word32 L_num, Word16 denom_hi, Word16 denom_lo)
{
Word16 approx, hi, lo, n_hi, n_lo;
Word32 L_32;
/* First approximation: 1 / L_denom = 1/denom_hi */
approx = div_s( (Word16)0x3fff, denom_hi); /* result in Q14 */
/* Note: 3fff = 0.5 in Q15 */
/* 1/L_denom = approx * (2.0 - L_denom * approx) */
L_32 = Mpy_32_16(denom_hi, denom_lo, approx); /* result in Q30 */
L_32 = L_sub( (Word32)0x7fffffffL, L_32); /* result in Q30 */
L_Extract(L_32, &hi, &lo);
L_32 = Mpy_32_16(hi, lo, approx); /* = 1/L_denom in Q29 */
/* L_num * (1/L_denom) */
L_Extract(L_32, &hi, &lo);
L_Extract(L_num, &n_hi, &n_lo);
L_32 = Mpy_32(n_hi, n_lo, hi, lo); /* result in Q29 */
L_32 = L_shl(L_32, 2); /* From Q29 to Q31 */
return( L_32 );
}
static Word16 Lag_max( /* output: lag found */
Word16 signal[], /* input : signal used to compute the open loop pitch */
Word16 L_frame, /* input : length of frame to compute pitch */
Word16 lag_max, /* input : maximum lag */
Word16 lag_min, /* input : minimum lag */
Word16 *cor_max) /* output: normalized correlation of selected lag */
{
Word16 i, j;
Word16 *p, *p1;
Word32 max, t0, L_temp;
Word16 max_h, max_l, ener_h, ener_l;
Word16 p_max;
max = MIN_32;
/* initialization used only to suppress Microsoft Visual C++ warnings */
p_max = lag_max;
for (i = lag_max; i >= lag_min; i--)
{
p = signal;
p1 = &signal[-i];
t0 = 0;
for (j=0; j<L_frame; j++, p++, p1++)
t0 = L_mac(t0, *p, *p1);
L_temp = L_sub(t0,max);
if (L_temp >= 0L)
{
max = t0;
p_max = i;
}
}
/* compute energy */
t0 = 0;
p = &signal[-p_max];
for(i=0; i<L_frame; i++, p++)
t0 = L_mac(t0, *p, *p);
/* 1/sqrt(energy), result in Q30 */
t0 = Inv_sqrt(t0);
/* max = max/sqrt(energy) */
/* This result will always be on 16 bits !! */
L_Extract(max, &max_h, &max_l);
L_Extract(t0, &ener_h, &ener_l);
t0 = Mpy_32(max_h, max_l, ener_h, ener_l);
*cor_max = extract_l(t0);
return(p_max);
}
/*-----------------------------------------------------*
* Function Autocorr() *
* *
* Compute autocorrelations of signal with windowing *
* *
*-----------------------------------------------------*/
void Autocorr(
Word16 x[], /* (i) : Input signal */
Word16 m, /* (i) : LPC order */
Word16 r_h[], /* (o) : Autocorrelations (msb) */
Word16 r_l[] /* (o) : Autocorrelations (lsb) */
)
{
Word16 i, j, norm;
Word16 y[L_WINDOW];
Word32 sum;
extern Flag Overflow;
/* Windowing of signal */
for(i=0; i<L_WINDOW; i++)
{
y[i] = mult_r(x[i], hamwindow[i]);
}
/* Compute r[0] and test for overflow */
do {
Overflow = 0;
sum = 1; /* Avoid case of all zeros */
for(i=0; i<L_WINDOW; i++)
sum = L_mac(sum, y[i], y[i]);
/* If overflow divide y[] by 4 */
if(Overflow != 0)
{
for(i=0; i<L_WINDOW; i++)
{
y[i] = shr(y[i], 2);
}
}
}while (Overflow != 0);
/* Normalization of r[0] */
norm = norm_l(sum);
sum = L_shl(sum, norm);
L_Extract(sum, &r_h[0], &r_l[0]); /* Put in DPF format (see oper_32b) */
/* r[1] to r[m] */
for (i = 1; i <= m; i++)
{
sum = 0;
for(j=0; j<L_WINDOW-i; j++)
sum = L_mac(sum, y[j], y[j+i]);
sum = L_shl(sum, norm);
L_Extract(sum, &r_h[i], &r_l[i]);
}
return;
}
/*----------------------------------------------------------------------------
* select_ltp : selects best of (gain1, gain2)
* with gain1 = num1 * 2** sh_num1 / den1 * 2** sh_den1
* and gain2 = num2 * 2** sh_num2 / den2 * 2** sh_den2
*----------------------------------------------------------------------------
*/
static Word16 select_ltp( /* output : 1 = 1st gain, 2 = 2nd gain */
Word16 num1, /* input : numerator of gain1 */
Word16 den1, /* input : denominator of gain1 */
Word16 sh_num1, /* input : just. factor for num1 */
Word16 sh_den1, /* input : just. factor for den1 */
Word16 num2, /* input : numerator of gain2 */
Word16 den2, /* input : denominator of gain2 */
Word16 sh_num2, /* input : just. factor for num2 */
Word16 sh_den2) /* input : just. factor for den2 */
{
Word32 L_temp1, L_temp2;
Word16 temp1, temp2;
Word16 hi, lo;
Word32 L_temp;
if(den2 == 0) {
return(1);
}
/* compares criteria = num**2/den */
L_temp1 = L_mult(num1, num1);
L_Extract(L_temp1, &hi, &lo);
L_temp1 = Mpy_32_16(hi, lo, den2);
L_temp2 = L_mult(num2, num2);
L_Extract(L_temp2, &hi, &lo);
L_temp2 = Mpy_32_16(hi, lo, den1);
/* temp1 = sh_den2 + 2 * sh_num1 */
temp1 = shl(sh_num1, 1);
temp1 = add(temp1, sh_den2);
/* temp2 = sh_den1 + 2 * sh_num2; */
temp2 = shl(sh_num2, 1);
temp2 = add(temp2, sh_den1);
if(sub(temp2 ,temp1)>0) {
temp2 = sub(temp2, temp1);
L_temp1 = L_shr(L_temp1, temp2); /* temp2 > 0 */
}
else {
if(sub(temp1 ,temp2) >0) {
temp1 = sub(temp1, temp2);
L_temp2 = L_shr(L_temp2, temp1); /* temp1 > 0 */
}
}
L_temp = L_sub(L_temp2,L_temp1);
if(L_temp>0L) {
return(2);
}
else {
return(1);
}
}
void update_exc_err(
Word32 *L_exc_err,
Word16 gain_pit, /* (i) pitch gain */
Word16 T0 /* (i) integer part of pitch delay */
)
{
Word16 i, zone1, zone2, n;
Word32 L_worst, L_temp, L_acc;
Word16 hi, lo;
L_worst = -1L;
n = sub(T0, L_SUBFR);
if(n < 0) {
L_Extract(L_exc_err[0], &hi, &lo);
L_temp = Mpy_32_16(hi, lo, gain_pit);
L_temp = L_shl(L_temp, 1);
L_temp = L_add(0x00004000L, L_temp);
L_acc = L_sub(L_temp, L_worst);
if(L_acc > 0L) {
L_worst = L_temp;
}
L_Extract(L_temp, &hi, &lo);
L_temp = Mpy_32_16(hi, lo, gain_pit);
L_temp = L_shl(L_temp, 1);
L_temp = L_add(0x00004000L, L_temp);
L_acc = L_sub(L_temp, L_worst);
if(L_acc > 0L) {
L_worst = L_temp;
}
}
else {
zone1 = tab_zone[n];
i = sub(T0, 1);
zone2 = tab_zone[i];
for(i = zone1; i <= zone2; i++) {
L_Extract(L_exc_err[i], &hi, &lo);
L_temp = Mpy_32_16(hi, lo, gain_pit);
L_temp = L_shl(L_temp, 1);
L_temp = L_add(0x00004000L, L_temp);
L_acc = L_sub(L_temp, L_worst);
if(L_acc > 0L) L_worst = L_temp;
}
}
for(i=3; i>=1; i--) {
L_exc_err[i] = L_exc_err[i-1];
}
L_exc_err[0] = L_worst;
return;
}
/*-------------------------------------------------------------------*
* Function Qua_Sidgain *
* ~~~~~~~~~~~ *
*-------------------------------------------------------------------*/
void Qua_Sidgain(
Word16 *ener, /* (i) array of energies */
Word16 *sh_ener, /* (i) corresponding scaling factors */
Word16 nb_ener, /* (i) number of energies or */
Word16 *enerq, /* (o) decoded energies in dB */
Word16 *idx /* (o) SID gain quantization index */
)
{
Word16 i;
Word32 L_x;
Word16 sh1, temp;
Word16 hi, lo;
Word32 L_acc;
if(nb_ener == 0) {
/* Quantize energy saved for frame erasure case */
/* L_x = average_ener */
L_acc = L_deposit_l(*ener);
L_acc = L_shl(L_acc, *sh_ener); /* >> if *sh_ener < 0 */
L_Extract(L_acc, &hi, &lo);
L_x = Mpy_32_16(hi, lo, fact[0]);
sh1 = 0;
}
else {
/*
* Compute weighted average of energies
* ener[i] = enerR[i] x 2**sh_ener[i]
* L_x = k[nb_ener] x SUM(i=0->nb_ener-1) enerR[i]
* with k[nb_ener] = fact_ener / nb_ener x L_FRAME x nbAcf
*/
sh1 = sh_ener[0];
for(i=1; i<nb_ener; i++) {
if(sh_ener[i] < sh1) sh1 = sh_ener[i];
}
sh1 = add(sh1, (16-marg[nb_ener]));
L_x = 0L;
for(i=0; i<nb_ener; i++) {
temp = sub(sh1, sh_ener[i]);
L_acc = L_deposit_l(ener[i]);
L_acc = L_shl(L_acc, temp);
L_x = L_add(L_x, L_acc);
}
L_Extract(L_x, &hi, &lo);
L_x = Mpy_32_16(hi, lo, fact[i]);
}
*idx = Quant_Energy(L_x, sh1, enerq);
return;
}
void get_pq_polynomials(
Word32 *f, /* Q23 */
Word16 *lsp) /* Q15 */
{
Word16 i, n, hi, lo;
Word16 index, offset, coslsp, c;
Word32 a0;
f[0] = L_mult(2048, 2048); // 1.0 Q23
for(i = 1; i <= LPCO ; i++)
f[i]= 0;
for(n=1; n<=(LPCO>>1); n++) {
/* cosine mapping */
index = shr(lsp[2*n-2],9); // Q6
offset = lsp[2*n-2]&(Word16)0x01ff; // Q9
a0 = L_mult(sub(costable[index+1], costable[index]), offset); // Q10
coslsp = add(costable[index], intround(L_shl(a0, 6))); // Q15 cos((double)PI*lsp[2*n-2])
c = coslsp; // Q14 c = 2. * cos((double)PI*lsp[2*n-2])
for(i = 2*n; i >= 2; i--) {
L_Extract(f[i-1], &hi, &lo);
f[i] = L_add(f[i], f[i-2]); // Q23 f[i] += f[i-2]
a0 = Mpy_32_16(hi, lo, c); // Q22
f[i] = L_sub(f[i], L_shl(a0,1)); // Q23 f[i] += f[i-2] - c*f[i-1];
}
f[1] = L_msu(f[1], c, 256); // Q23 f[1] -= c;
}
return;
}
void Post_Process(
PopStatus *handle,
Word16 signal[], /* input/output signal */
Word16 lg) /* length of signal */
{
Word16 i, x2;
Word32 L_tmp;
for(i=0; i<lg; i++)
{
x2 = handle->x1;
handle->x1 = handle->x0;
handle->x0 = signal[i];
/* y[i] = b[0]*x[i] + b[1]*x[i-1] + b[2]*x[i-2] */
/* + a[1]*y[i-1] + a[2] * y[i-2]; */
L_tmp = Mpy_32_16(handle->y1_hi, handle->y1_lo, a100[1]);
L_tmp = L_add(L_tmp, Mpy_32_16(handle->y2_hi, handle->y2_lo, a100[2]));
L_tmp = L_mac(L_tmp, handle->x0, b100[0]);
L_tmp = L_mac(L_tmp, handle->x1, b100[1]);
L_tmp = L_mac(L_tmp, x2, b100[2]);
L_tmp = L_shl(L_tmp, 2); /* Q29 --> Q31 (Q13 --> Q15) */
/* Multiplication by two of output speech with saturation. */
signal[i] = round(L_shl(L_tmp, 1));
handle->y2_hi = handle->y1_hi;
handle->y2_lo = handle->y1_lo;
L_Extract(L_tmp, &(handle->y1_hi), &(handle->y1_lo));
}
return;
}
void
Pre_Process (CodState *coder,
Word16 signal[], /* input/output signal */
Word16 lg)
{ /* length of signal */
Word16 i, x2;
Word32 L_tmp;
for (i = 0; i < lg; i++) {
x2 = coder->x1;
coder->x1 = coder->x0;
coder->x0 = signal[i];
/* y[i] = b[0]*x[i]/2 + b[1]*x[i-1]/2 + b140[2]*x[i-2]/2 */
/* + a[1]*y[i-1] + a[2] * y[i-2]; */
L_tmp = Mpy_32_16 (coder->y1_hi, coder->y1_lo, a140[1]);
L_tmp = L_add (L_tmp, Mpy_32_16 (coder->y2_hi, coder->y2_lo, a140[2]));
L_tmp = L_mac (L_tmp, coder->x0, b140[0]);
L_tmp = L_mac (L_tmp, coder->x1, b140[1]);
L_tmp = L_mac (L_tmp, x2, b140[2]);
L_tmp = L_shl (L_tmp, 3); /* Q28 --> Q31 (Q12 --> Q15) */
signal[i] = wround (L_tmp);
coder->y2_hi = coder->y1_hi;
coder->y2_lo = coder->y1_lo;
L_Extract (L_tmp, &coder->y1_hi, &coder->y1_lo);
}
return;
}
void Pre_Process(
PrpStatus *handle,
Word16 signal[], /* input/output signal */
Word16 lg) /* length of signal */
{
Word16 i, x2;
Word32 L_tmp;
for(i=0; i<lg; i++)
{
x2 = handle->x1;
handle->x1 = handle->x0;
handle->x0 = signal[i];
/* y[i] = b[0]*x[i]/2 + b[1]*x[i-1]/2 + b140[2]*x[i-2]/2 */
/* + a[1]*y[i-1] + a[2] * y[i-2]; */
L_tmp = Mpy_32_16(handle->y1_hi, handle->y1_lo, a140[1]);
L_tmp = L_add(L_tmp, Mpy_32_16(handle->y2_hi, handle->y2_lo, a140[2]));
L_tmp = L_mac(L_tmp, handle->x0, b140[0]);
L_tmp = L_mac(L_tmp, handle->x1, b140[1]);
L_tmp = L_mac(L_tmp, x2, b140[2]);
L_tmp = L_shl(L_tmp, 3); /* Q28 --> Q31 (Q12 --> Q15) */
signal[i] = round(L_tmp);
handle->y2_hi = handle->y1_hi;
handle->y2_lo = handle->y1_lo;
L_Extract(L_tmp, &(handle->y1_hi), &(handle->y1_lo));
}
return;
}
void Post_Process(
int16_t signal[], /* input/output signal */
int16_t lg) /* length of signal */
{
int16_t i, x2;
int32_t L_tmp;
for(i=0; i<lg; i++)
{
x2 = x1;
x1 = x0;
x0 = signal[i];
/* y[i] = b[0]*x[i] + b[1]*x[i-1] + b[2]*x[i-2] */
/* + a[1]*y[i-1] + a[2] * y[i-2]; */
L_tmp = Mpy_32_16(y1_hi, y1_lo, a100[1]);
L_tmp = L_add(L_tmp, Mpy_32_16(y2_hi, y2_lo, a100[2]));
L_tmp = L_mac(L_tmp, x0, b100[0]);
L_tmp = L_mac(L_tmp, x1, b100[1]);
L_tmp = L_mac(L_tmp, x2, b100[2]);
L_tmp = L_shl(L_tmp, 2); /* Q29 --> Q31 (Q13 --> Q15) */
/* Multiplication by two of output speech with saturation. */
signal[i] = _round(L_shl(L_tmp, 1));
y2_hi = y1_hi;
y2_lo = y1_lo;
L_Extract(L_tmp, &y1_hi, &y1_lo);
}
return;
}
static void Get_lsp_pol(Word16 *lsp, Word32 *f)
{
Word16 i,j, hi, lo;
Word32 t0;
/* All computation in Q24 */
*f = L_mult(4096, 2048); /* f[0] = 1.0; in Q24 */
f++;
*f = L_msu((Word32)0, *lsp, 512); /* f[1] = -2.0 * lsp[0]; in Q24 */
f++;
lsp += 2; /* Advance lsp pointer */
for(i=2; i<=5; i++)
{
*f = f[-2];
for(j=1; j<i; j++, f--)
{
L_Extract(f[-1] ,&hi, &lo);
t0 = Mpy_32_16(hi, lo, *lsp); /* t0 = f[-1] * lsp */
t0 = L_shl(t0, 1);
*f = L_add(*f, f[-2]); /* *f += f[-2] */
*f = L_sub(*f, t0); /* *f -= t0 */
}
*f = L_msu(*f, *lsp, 512); /* *f -= lsp<<9 */
f += i; /* Advance f pointer */
lsp += 2; /* Advance lsp pointer */
}
return;
}
void LTP_flag_update (vadState2 * st, Word16 mode)
{
Word16 thresh;
Word16 hi1;
Word16 lo1;
Word32 Ltmp;
test(); test();
if ((sub(mode, MR475) == 0) || (sub(mode, MR515) == 0))
{
thresh = (Word16)(32768.0*0.55); move16();
}
else if (sub(mode, MR102) == 0)
{
thresh = (Word16)(32768.0*0.60); move16();
}
else
{
thresh = (Word16)(32768.0*0.65); move16();
}
L_Extract (st->L_R0, &hi1, &lo1);
Ltmp = Mpy_32_16(hi1, lo1, thresh); test();
if (L_sub(st->L_Rmax, Ltmp) > 0)
{
st->LTP_flag = TRUE; move16();
}
else
{
st->LTP_flag = FALSE; move16();
}
return;
}
void Pre_Process(
Word16 signal[], /* input/output signal */
Word16 lg) /* length of signal */
{
Word16 i, x2;
Word32 L_tmp;
for(i=0; i<lg; i++)
{
x2 = x1;
x1 = x0;
x0 = signal[i];
/* y[i] = b[0]*x[i]/2 + b[1]*x[i-1]/2 + b140[2]*x[i-2]/2 */
/* + a[1]*y[i-1] + a[2] * y[i-2]; */
L_tmp = Mpy_32_16(y1_hi, y1_lo, a140[1]);
L_tmp = L_add(L_tmp, Mpy_32_16(y2_hi, y2_lo, a140[2]));
L_tmp = L_mac(L_tmp, x0, b140[0]);
L_tmp = L_mac(L_tmp, x1, b140[1]);
L_tmp = L_mac(L_tmp, x2, b140[2]);
L_tmp = L_shl(L_tmp, 3); /* Q28 --> Q31 (Q12 --> Q15) */
signal[i] = round(L_tmp);
y2_hi = y1_hi;
y2_lo = y1_lo;
L_Extract(L_tmp, &y1_hi, &y1_lo);
}
return;
}
static Word16 Chebps_10(Word16 x, Word16 f[], Word16 n)
{
Word16 i, cheb;
Word16 b0_h, b0_l, b1_h, b1_l, b2_h, b2_l;
Word32 t0;
/* Note: All computation are done in Q23. */
b2_h = 128; /* b2 = 1.0 in Q23 DPF */
b2_l = 0;
t0 = L_mult(x, 256); /* 2*x in Q23 */
t0 = L_mac(t0, f[1], 4096); /* + f[1] in Q23 */
L_Extract(t0, &b1_h, &b1_l); /* b1 = 2*x + f[1] */
for (i = 2; i<n; i++)
{
t0 = Mpy_32_16(b1_h, b1_l, x); /* t0 = 2.0*x*b1 */
t0 = L_shl(t0, 1);
t0 = L_mac(t0,b2_h,(Word16)-32768L); /* t0 = 2.0*x*b1 - b2 */
t0 = L_msu(t0, b2_l, 1);
t0 = L_mac(t0, f[i], 4096); /* t0 = 2.0*x*b1 - b2 + f[i]; */
L_Extract(t0, &b0_h, &b0_l); /* b0 = 2.0*x*b1 - b2 + f[i]; */
b2_l = b1_l; /* b2 = b1; */
b2_h = b1_h;
b1_l = b0_l; /* b1 = b0; */
b1_h = b0_h;
}
t0 = Mpy_32_16(b1_h, b1_l, x); /* t0 = x*b1; */
t0 = L_mac(t0, b2_h,(Word16)-32768L); /* t0 = x*b1 - b2 */
t0 = L_msu(t0, b2_l, 1);
t0 = L_mac(t0, f[i], 2048); /* t0 = x*b1 - b2 + f[i]/2 */
t0 = L_shl(t0, 7); /* Q23 to Q30 with saturation */
cheb = extract_h(t0); /* Result in Q14 */
return(cheb);
}
/*
**************************************************************************
*
* Function : Chebps
* Purpose : Evaluates the Chebyshev polynomial series
* Description : - The polynomial order is n = m/2 = 5
* - The polynomial F(z) (F1(z) or F2(z)) is given by
* F(w) = 2 exp(-j5w) C(x)
* where
* C(x) = T_n(x) + f(1)T_n-1(x) + ... +f(n-1)T_1(x) + f(n)/2
* and T_m(x) = cos(mw) is the mth order Chebyshev
* polynomial ( x=cos(w) )
* Returns : C(x) for the input x.
*
**************************************************************************
*/
static Word16 Chebps (Word16 x,
Word16 f[], /* (n) */
Word16 n)
{
Word16 i, cheb;
Word16 b0_h, b0_l, b1_h, b1_l, b2_h, b2_l;
Word32 t0;
b2_h = 256; move16 (); /* b2 = 1.0 */
b2_l = 0; move16 ();
t0 = L_mult (x, 512); /* 2*x */
t0 = L_mac (t0, f[1], 8192); /* + f[1] */
L_Extract (t0, &b1_h, &b1_l); /* b1 = 2*x + f[1] */
for (i = 2; i < n; i++)
{
t0 = Mpy_32_16 (b1_h, b1_l, x); /* t0 = 2.0*x*b1 */
t0 = L_shl (t0, 1);
t0 = L_mac (t0, b2_h, (Word16) 0x8000); /* t0 = 2.0*x*b1 - b2 */
t0 = L_msu (t0, b2_l, 1);
t0 = L_mac (t0, f[i], 8192); /* t0 = 2.0*x*b1 - b2 + f[i] */
L_Extract (t0, &b0_h, &b0_l); /* b0 = 2.0*x*b1 - b2 + f[i]*/
b2_l = b1_l; move16 (); /* b2 = b1; */
b2_h = b1_h; move16 ();
b1_l = b0_l; move16 (); /* b1 = b0; */
b1_h = b0_h; move16 ();
}
t0 = Mpy_32_16 (b1_h, b1_l, x); /* t0 = x*b1; */
t0 = L_mac (t0, b2_h, (Word16) 0x8000); /* t0 = x*b1 - b2 */
t0 = L_msu (t0, b2_l, 1);
t0 = L_mac (t0, f[i], 4096); /* t0 = x*b1 - b2 + f[i]/2 */
t0 = L_shl (t0, 6);
cheb = extract_h (t0);
return (cheb);
}
void Post_Process(
Word16 signal[], /* input/output signal */
Word16 lg) /* length of signal */
{
Word16 i, x2;
Word32 L_tmp;
Word16 y2_hi, y2_lo, y1_hi, y1_lo, x0, x1;
y2_hi = pg729dec->ppost_pro->y2_hi;
y2_lo = pg729dec->ppost_pro->y2_lo;
y1_hi = pg729dec->ppost_pro->y1_hi;
y1_lo = pg729dec->ppost_pro->y1_lo;
x0 = pg729dec->ppost_pro->x0;
x1 = pg729dec->ppost_pro->x1;
for(i=0; i<lg; i++)
{
x2 = x1;
x1 = x0;
x0 = signal[i];
/* y[i] = b[0]*x[i] + b[1]*x[i-1] + b[2]*x[i-2] */
/* + a[1]*y[i-1] + a[2] * y[i-2]; */
L_tmp = Mpy_32_16(y1_hi, y1_lo, a100[1]);
L_tmp = L_add(L_tmp, Mpy_32_16(y2_hi, y2_lo, a100[2]));
L_tmp = L_mac(L_tmp, x0, b100[0]);
L_tmp = L_mac(L_tmp, x1, b100[1]);
L_tmp = L_mac(L_tmp, x2, b100[2]);
L_tmp = L_shl(L_tmp, 2); /* Q29 --> Q31 (Q13 --> Q15) */
/* Multiplication by two of output speech with saturation. */
signal[i] = round(L_shl(L_tmp, 1));
y2_hi = y1_hi;
y2_lo = y1_lo;
L_Extract(L_tmp, &y1_hi, &y1_lo);
}
pg729dec->ppost_pro->y2_hi = y2_hi;
pg729dec->ppost_pro->y2_lo = y2_lo;
pg729dec->ppost_pro->y1_hi = y1_hi;
pg729dec->ppost_pro->y1_lo = y1_lo;
pg729dec->ppost_pro->x0 = x0;
pg729dec->ppost_pro->x1 = x1;
return;
}
/*-------------------------------------------------------*
* Function Lag_window() *
* *
* Lag_window on autocorrelations. *
* *
* r[i] *= lag_wind[i] *
* *
* r[i] and lag_wind[i] are in special double precision.*
* See "oper_32b.c" for the format *
* *
*-------------------------------------------------------*/
void Lag_window(
Word16 m, /* (i) : LPC order */
Word16 r_h[], /* (i/o) : Autocorrelations (msb) */
Word16 r_l[] /* (i/o) : Autocorrelations (lsb) */
)
{
Word16 i;
Word32 x;
for(i=1; i<=m; i++)
{
x = Mpy_32(r_h[i], r_l[i], lag_h[i-1], lag_l[i-1]);
L_Extract(x, &r_h[i], &r_l[i]);
}
return;
}
/*************************************************************************
*
* FUNCTION: Post_Process()
*
* PURPOSE: Postprocessing of input speech.
*
* DESCRIPTION:
* - 2nd order high pass filtering with cut off frequency at 60 Hz.
* - Multiplication of output by two.
*
* Algorithm:
*
* y[i] = b[0]*x[i]*2 + b[1]*x[i-1]*2 + b[2]*x[i-2]*2
* + a[1]*y[i-1] + a[2]*y[i-2];
*
*
*************************************************************************/
int Post_Process (
Post_ProcessState *st, /* i/o : post process state */
Word16 signal[], /* i/o : signal */
Word16 lg /* i : length of signal */
)
{
Word16 i, x2;
Word32 L_tmp;
test (); test ();
for (i = 0; i < lg; i++)
{
x2 = st->x1; move16 ();
st->x1 = st->x0; move16 ();
st->x0 = signal[i]; move16 ();
/* y[i] = b[0]*x[i]*2 + b[1]*x[i-1]*2 + b140[2]*x[i-2]/2 */
/* + a[1]*y[i-1] + a[2] * y[i-2]; */
L_tmp = Mpy_32_16 (st->y1_hi, st->y1_lo, a[1]);
L_tmp = L_add (L_tmp, Mpy_32_16 (st->y2_hi, st->y2_lo, a[2]));
L_tmp = L_mac (L_tmp, st->x0, b[0]);
L_tmp = L_mac (L_tmp, st->x1, b[1]);
L_tmp = L_mac (L_tmp, x2, b[2]);
L_tmp = L_shl (L_tmp, 2);
/* Multiplication by two of output speech with saturation. */
signal[i] = round(L_shl(L_tmp, 1)); move16 ();
st->y2_hi = st->y1_hi; move16 ();
st->y2_lo = st->y1_lo; move16 ();
L_Extract (L_tmp, &st->y1_hi, &st->y1_lo);
}
return 0;
}
/*---------------------------------------------------------------------------*
* 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( sub(exp1, exp2)>0 ){
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( sub(exp1, exp2)>0 ){
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(sub(best_gain[0], GPCLIP2) > 0) best_gain[0] = GPCLIP2;
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( sub(exp1, exp2)>0 ){
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);
//exp = exp2--;
}
else{
L_tmp = L_sub( L_shr( L_tmp1,1 ), L_shr( L_tmp2, add(sub(exp2,exp1),1) ) );
exp = sub(exp1,1);
//exp = exp1--;
}
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( sub(exp_gcode0,4) >= 0 ){
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( sub(exp_min[i], e_min) < 0 ){
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 = (Word32)g_coeff[i] << 16;
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_temp < 0L ){
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);
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_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 *
*----------------------------------------------*/
Gain_update( past_qua_en, L_gbk12 );
return( add( map1[index1]*(Word16)NCODE2, map2[index2] ) );
}
static Word16 Lag_max( /* o : lag found */
vadState *vadSt, /* i/o : VAD state struct */
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 old_lag, /* i : old open-loop lag */
Word16 *cor_max, /* o : normalized correlation of selected lag */
Word16 wght_flg, /* i : is weighting function used */
Word16 *gain_flg, /* o : open-loop flag */
Flag dtx, /* i : dtx flag; use dtx=1, do not use dtx=0 */
Flag *pOverflow /* o : overflow flag */
)
{
Word16 i;
Word16 j;
Word16 *p;
Word16 *p1;
Word32 max;
Word32 t0;
Word16 t0_h;
Word16 t0_l;
Word16 p_max;
const Word16 *ww;
const Word16 *we;
Word32 t1;
Word16 temp;
ww = &corrweight[250];
we = &corrweight[123 + lag_max - old_lag];
max = MIN_32;
p_max = lag_max;
for (i = lag_max; i >= lag_min; i--)
{
t0 = corr[-i];
/* Weighting of the correlation function. */
L_Extract(corr[-i], &t0_h, &t0_l, pOverflow);
t0 = Mpy_32_16(t0_h, t0_l, *ww, pOverflow);
ww--;
if (wght_flg > 0)
{
/* Weight the neighbourhood of the old lag. */
L_Extract(t0, &t0_h, &t0_l, pOverflow);
t0 = Mpy_32_16(t0_h, t0_l, *we, pOverflow);
we--;
}
/* if (L_sub (t0, max) >= 0) */
if (t0 >= max)
{
max = t0;
p_max = i;
}
}
p = &scal_sig[0];
p1 = &scal_sig[-p_max];
t0 = 0;
t1 = 0;
for (j = 0; j < L_frame; j++, p++, p1++)
{
t0 = L_mac(t0, *p, *p1, pOverflow);
t1 = L_mac(t1, *p1, *p1, pOverflow);
}
if (dtx)
{ /* no test() call since this if is only in simulation env */
#ifdef VAD2
/* Save max correlation */
vadSt->L_Rmax = L_add(vadSt->L_Rmax, t0, pOverflow);
/* Save max energy */
vadSt->L_R0 = L_add(vadSt->L_R0, t1, pOverflow);
#else
/* update and detect tone */
vad_tone_detection_update(vadSt, 0, pOverflow);
vad_tone_detection(vadSt, t0, t1, pOverflow);
#endif
}
/* gain flag is set according to the open_loop gain */
/* is t2/t1 > 0.4 ? */
temp = pv_round(t1, pOverflow);
t1 = L_msu(t0, temp, 13107, pOverflow);
*gain_flg = pv_round(t1, pOverflow);
*cor_max = 0;
return (p_max);
}
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 BV16_Decode(
struct BV16_Bit_Stream *bs,
struct BV16_Decoder_State *ds,
Word16 *x)
{
Word32 lgq, lg_el;
Word16 gainq; /* Q3 */
Word16 pp;
Word32 a0;
Word16 gain_exp;
Word16 i;
Word16 a0hi, a0lo;
Word16 ltsym[LTMOFF+FRSZ];
Word16 xq[LXQ];
Word16 a[LPCO+1];
Word16 lspq[LPCO]; /* Q15 */
Word16 cbs[VDIM*CBSZ];
Word16 bq[3]; /* Q15 */
Word32 bss;
Word32 E;
/* set frame erasure flags */
if (ds->cfecount != 0) {
ds->ngfae = 1;
} else {
ds->ngfae++;
if (ds->ngfae>LGPORDER) ds->ngfae=LGPORDER+1;
}
/* reset frame erasure counter */
ds->cfecount = 0;
/* decode pitch period */
pp = (bs->ppidx + MINPP);
/* decode spectral information */
lspdec(lspq,bs->lspidx,ds->lsppm,ds->lsplast);
lsp2a(lspq,a);
W16copy(ds->lsplast, lspq, LPCO);
/* decode pitch taps */
pp3dec(bs->bqidx, bq);
/* decode gain */
a0 = gaindec(&lgq,bs->gidx,ds->lgpm,ds->prevlg,ds->level,
&ds->nggalgc,&lg_el);
/* gain normalization */
gain_exp = sub(norm_l(a0), 2);
/* scale down quantized gain by 1.5, 1/1.5=2/3 (21845 Q15) */
L_Extract(a0, &a0hi, &a0lo);
a0 = Mpy_32_16(a0hi, a0lo, 21845);
gainq = intround(L_shl(a0, gain_exp));
/* scale the scalar quantizer codebook to current signal level */
for (i=0;i<(VDIM*CBSZ);i++) cbs[i] = mult_r(gainq, cccb[i]);
/* copy state memory to buffer */
W16copy(xq, ds->xq, XQOFF);
W16copy(ltsym, ds->ltsym, LTMOFF);
/* decoding of the excitation signal with integrated long-term */
/* and short-term synthesis */
excdec_w_synth(xq+XQOFF,ltsym+LTMOFF,ds->stsym,bs->qvidx,bq,cbs,pp,
a,gain_exp,&E);
ds->E = E;
/* update the remaining state memory */
W16copy(ds->ltsym, ltsym+FRSZ, LTMOFF);
W16copy(ds->xq, xq+FRSZ, XQOFF);
ds->pp_last = pp;
W16copy(ds->bq_last, bq, 3);
/* level estimation */
estlevel(lg_el,&ds->level,&ds->lmax,&ds->lmin,&ds->lmean,&ds->x1,
ds->ngfae, ds->nggalgc,&ds->estl_alpha_min);
/* adaptive postfiltering */
postfilter(xq, pp, &(ds->ma_a), ds->b_prv, &(ds->pp_prv), x);
/* scale signal up by 1.5 */
for(i=0; i<FRSZ; i++)
x[i] = add(x[i], shr(x[i],1));
W16copy(ds->atplc, a, LPCO+1);
bss = L_add(L_add(bq[0], bq[1]), bq[2]);
if (bss > 32768)
bss = 32768;
else if (bss < 0)
bss = 0;
ds->per = add(shr(ds->per, 1), (Word16)L_shr(bss, 1));
}
static void Norm_Corr(Word16 exc[], Word16 xn[], Word16 h[], Word16 L_subfr,
Word16 t_min, Word16 t_max, Word16 corr_norm[])
{
Word16 i,j,k;
Word16 corr_h, corr_l, norm_h, norm_l;
Word32 s, L_temp;
Word16 excf[L_SUBFR];
Word16 scaling, h_fac, *s_excf, scaled_excf[L_SUBFR];
k = negate(t_min);
/* compute the filtered excitation for the first delay t_min */
Convolve(&exc[k], h, excf, L_subfr);
/* scaled "excf[]" to avoid overflow */
for(j=0; j<L_subfr; j++)
scaled_excf[j] = shr(excf[j], 2);
/* Compute energy of excf[] for danger of overflow */
s = 0;
for (j = 0; j < L_subfr; j++)
s = L_mac(s, excf[j], excf[j]);
L_temp = L_sub(s, 67108864L);
if (L_temp <= 0L) /* if (s <= 2^26) */
{
s_excf = excf;
h_fac = 15-12; /* h in Q12 */
scaling = 0;
}
else {
s_excf = scaled_excf; /* "excf[]" is divide by 2 */
h_fac = 15-12-2; /* h in Q12, divide by 2 */
scaling = 2;
}
/* loop for every possible period */
for (i = t_min; i <= t_max; i++)
{
/* Compute 1/sqrt(energy of excf[]) */
s = 0;
for (j = 0; j < L_subfr; j++)
s = L_mac(s, s_excf[j], s_excf[j]);
s = Inv_sqrt(s); /* Result in Q30 */
L_Extract(s, &norm_h, &norm_l);
/* Compute correlation between xn[] and excf[] */
s = 0;
for (j = 0; j < L_subfr; j++)
s = L_mac(s, xn[j], s_excf[j]);
L_Extract(s, &corr_h, &corr_l);
/* Normalize correlation = correlation * (1/sqrt(energy)) */
s = Mpy_32(corr_h, corr_l, norm_h, norm_l);
corr_norm[i] = extract_h(L_shl(s, 16)); /* Result is on 16 bits */
/* modify the filtered excitation excf[] for the next iteration */
if( sub(i, t_max) != 0)
{
k=sub(k,1);
for (j = L_subfr-(Word16)1; j > 0; j--)
{
s = L_mult(exc[k], h[j]);
s = L_shl(s, h_fac); /* h is in Q(12-scaling) */
s_excf[j] = add(extract_h(s), s_excf[j-1]);
}
s_excf[0] = shr(exc[k], scaling);
}
}
return;
}
Word16 Pitch_ol_fast( /* output: open loop pitch lag */
Word16 signal[], /* input : signal used to compute the open loop pitch */
/* signal[-pit_max] to signal[-1] should be known */
Word16 pit_max, /* input : maximum pitch lag */
Word16 L_frame /* input : length of frame to compute pitch */
)
{
Word16 i, j;
Word16 max1, max2, max3;
Word16 max_h, max_l, ener_h, ener_l;
Word16 T1, T2, T3;
Word16 *p, *p1;
Word32 max, sum, L_temp;
/* Scaled signal */
Word16 scaled_signal[L_FRAME+PIT_MAX];
Word16 *scal_sig;
scal_sig = &scaled_signal[pit_max];
/*--------------------------------------------------------*
* Verification for risk of overflow. *
*--------------------------------------------------------*/
Overflow = 0;
sum = 0;
for(i= -pit_max; i< L_frame; i+=2)
sum = L_mac(sum, signal[i], signal[i]);
/*--------------------------------------------------------*
* Scaling of input signal. *
* *
* if Overflow -> scal_sig[i] = signal[i]>>3 *
* else if sum < 1^20 -> scal_sig[i] = signal[i]<<3 *
* else -> scal_sig[i] = signal[i] *
*--------------------------------------------------------*/
if(Overflow == 1)
{
for(i=-pit_max; i<L_frame; i++)
{
scal_sig[i] = shr(signal[i], 3);
}
}
else {
L_temp = L_sub(sum, (Word32)1048576L);
if ( L_temp < (Word32)0 ) /* if (sum < 2^20) */
{
for(i=-pit_max; i<L_frame; i++)
{
scal_sig[i] = shl(signal[i], 3);
}
}
else
{
for(i=-pit_max; i<L_frame; i++)
{
scal_sig[i] = signal[i];
}
}
}
/*--------------------------------------------------------------------*
* The pitch lag search is divided in three sections. *
* Each section cannot have a pitch multiple. *
* We find a maximum for each section. *
* We compare the maxima of each section by favoring small lag. *
* *
* First section: lag delay = 20 to 39 *
* Second section: lag delay = 40 to 79 *
* Third section: lag delay = 80 to 143 *
*--------------------------------------------------------------------*/
/* First section */
max = MIN_32;
T1 = 20; /* Only to remove warning from some compilers */
for (i = 20; i < 40; i++) {
p = scal_sig;
p1 = &scal_sig[-i];
sum = 0;
for (j=0; j<L_frame; j+=2, p+=2, p1+=2)
sum = L_mac(sum, *p, *p1);
L_temp = L_sub(sum, max);
if (L_temp > 0) { max = sum; T1 = i; }
}
/* compute energy of maximum */
sum = 1; /* to avoid division by zero */
p = &scal_sig[-T1];
for(i=0; i<L_frame; i+=2, p+=2)
sum = L_mac(sum, *p, *p);
/* max1 = max/sqrt(energy) */
/* This result will always be on 16 bits !! */
sum = Inv_sqrt(sum); /* 1/sqrt(energy), result in Q30 */
L_Extract(max, &max_h, &max_l);
L_Extract(sum, &ener_h, &ener_l);
sum = Mpy_32(max_h, max_l, ener_h, ener_l);
max1 = extract_l(sum);
/* Second section */
max = MIN_32;
T2 = 40; /* Only to remove warning from some compilers */
for (i = 40; i < 80; i++) {
p = scal_sig;
p1 = &scal_sig[-i];
sum = 0;
for (j=0; j<L_frame; j+=2, p+=2, p1+=2)
sum = L_mac(sum, *p, *p1);
L_temp = L_sub(sum, max);
if (L_temp > 0) { max = sum; T2 = i; }
}
/* compute energy of maximum */
sum = 1; /* to avoid division by zero */
p = &scal_sig[-T2];
for(i=0; i<L_frame; i+=2, p+=2)
sum = L_mac(sum, *p, *p);
/* max2 = max/sqrt(energy) */
/* This result will always be on 16 bits !! */
sum = Inv_sqrt(sum); /* 1/sqrt(energy), result in Q30 */
L_Extract(max, &max_h, &max_l);
L_Extract(sum, &ener_h, &ener_l);
sum = Mpy_32(max_h, max_l, ener_h, ener_l);
max2 = extract_l(sum);
/* Third section */
max = MIN_32;
T3 = 80; /* Only to remove warning from some compilers */
for (i = 80; i < 143; i+=2) {
p = scal_sig;
p1 = &scal_sig[-i];
sum = 0;
for (j=0; j<L_frame; j+=2, p+=2, p1+=2)
sum = L_mac(sum, *p, *p1);
L_temp = L_sub(sum, max);
if (L_temp > 0) { max = sum; T3 = i; }
}
/* Test around max3 */
i = T3;
p = scal_sig;
p1 = &scal_sig[-(i+1)];
sum = 0;
for (j=0; j<L_frame; j+=2, p+=2, p1+=2)
sum = L_mac(sum, *p, *p1);
L_temp = L_sub(sum, max);
if (L_temp > 0) { max = sum; T3 = i+(Word16)1; }
p = scal_sig;
p1 = &scal_sig[-(i-1)];
sum = 0;
for (j=0; j<L_frame; j+=2, p+=2, p1+=2)
sum = L_mac(sum, *p, *p1);
L_temp = L_sub(sum, max);
if (L_temp > 0) { max = sum; T3 = i-(Word16)1; }
/* compute energy of maximum */
sum = 1; /* to avoid division by zero */
p = &scal_sig[-T3];
for(i=0; i<L_frame; i+=2, p+=2)
sum = L_mac(sum, *p, *p);
/* max1 = max/sqrt(energy) */
/* This result will always be on 16 bits !! */
sum = Inv_sqrt(sum); /* 1/sqrt(energy), result in Q30 */
L_Extract(max, &max_h, &max_l);
L_Extract(sum, &ener_h, &ener_l);
sum = Mpy_32(max_h, max_l, ener_h, ener_l);
max3 = extract_l(sum);
/*-----------------------*
* Test for multiple. *
*-----------------------*/
/* if( abs(T2*2 - T3) < 5) */
/* max2 += max3 * 0.25; */
i = sub(shl(T2,1), T3);
j = sub(abs_s(i), 5);
if(j < 0)
max2 = add(max2, shr(max3, 2));
/* if( abs(T2*3 - T3) < 7) */
/* max2 += max3 * 0.25; */
i = add(i, T2);
j = sub(abs_s(i), 7);
if(j < 0)
max2 = add(max2, shr(max3, 2));
/* if( abs(T1*2 - T2) < 5) */
/* max1 += max2 * 0.20; */
i = sub(shl(T1,1), T2);
j = sub(abs_s(i), 5);
if(j < 0)
max1 = add(max1, mult(max2, 6554));
/* if( abs(T1*3 - T2) < 7) */
/* max1 += max2 * 0.20; */
i = add(i, T1);
j = sub(abs_s(i), 7);
if(j < 0)
max1 = add(max1, mult(max2, 6554));
/*--------------------------------------------------------------------*
* Compare the 3 sections maxima. *
*--------------------------------------------------------------------*/
if( sub(max1, max2) < 0 ) {max1 = max2; T1 = T2; }
if( sub(max1, max3) <0 ) {T1 = T3; }
return T1;
}
/************************************************************************
*
* FUNCTION: Chebps
*
* PURPOSE: Evaluates the Chebyshev polynomial series
*
* DESCRIPTION:
* - The polynomial order is n = m/2 = 5
* - The polynomial F(z) (F1(z) or F2(z)) is given by
* F(w) = 2 exp(-j5w) C(x)
* where
* C(x) = T_n(x) + f(1)T_n-1(x) + ... +f(n-1)T_1(x) + f(n)/2
* and T_m(x) = cos(mw) is the mth order Chebyshev polynomial ( x=cos(w) )
* - The function returns the value of C(x) for the input x.
*
***********************************************************************/
static INT16 Chebps (INT16 x, INT16 f[], INT16 n)
{
INT16 cheb;
INT16 b0_h, b0_l, b1_h, b1_l, b2_h = 256, b2_l =0;
INT32 t0;
t0 = f[1] <<4;
t0 += x;
t0 = L_SHL_SAT(t0, 10);
L_Extract (t0, &b1_h, &b1_l); /* b1 = 2*x + f[1] */
t0 = Mpy_32_16 (b1_h, b1_l, x); /* t0 = 2.0*x*b1 */
t0 -= (b2_h<<15);
t0 -= (b2_l);
t0 += (f[2]<<13);
t0 = L_SHL_SAT(t0, 1);
L_Extract (t0, &b0_h, &b0_l); /* b0 = 2.0*x*b1 - b2 + f[i]*/
b2_l = b1_l; /* b2 = b1; */
b2_h = b1_h;
b1_l = b0_l; /* b1 = b0; */
b1_h = b0_h;
t0 = Mpy_32_16 (b1_h, b1_l, x); /* t0 = 2.0*x*b1 */
t0 -= (b2_h<<15);
t0 -= (b2_l);
t0 += (f[3]<<13);
t0 = L_SHL_SAT(t0, 1);
L_Extract (t0, &b0_h, &b0_l); /* b0 = 2.0*x*b1 - b2 + f[i]*/
b2_l = b1_l; /* b2 = b1; */
b2_h = b1_h;
b1_l = b0_l; /* b1 = b0; */
b1_h = b0_h;
t0 = Mpy_32_16 (b1_h, b1_l, x); /* t0 = 2.0*x*b1 */
t0 -= (b2_h<<15);
t0 -= (b2_l);
t0 += (f[4]<<13);
t0 = L_SHL_SAT(t0, 1);
L_Extract (t0, &b0_h, &b0_l); /* b0 = 2.0*x*b1 - b2 + f[i]*/
b2_l = b1_l; /* b2 = b1; */
b2_h = b1_h;
b1_l = b0_l; /* b1 = b0; */
b1_h = b0_h;
t0 = Mpy_32_16 (b1_h, b1_l, x); /* t0 = x*b1; */
t0 -= (b2_h<<16);
t0 -= (b2_l<<1);
t0 += (f[5]<<13);
t0 = L_SHL_SAT(t0, 6);
cheb = EXTRACT_H(t0);
return (cheb);
}
/*************************************************************************
*
* FUNCTION: Lag_max
*
* PURPOSE: Find the lag that has maximum correlation of scal_sig[] in a
* given delay range.
*
* DESCRIPTION:
* The correlation is given by
* cor[t] = <scal_sig[n],scal_sig[n-t]>, t=lag_min,...,lag_max
* The functions outputs the maximum correlation after normalization
* and the corresponding lag.
*
*************************************************************************/
Word16 Lag_max ( /* o : lag found */
vadState1 *vadSt, /* i/o : VAD state struct */
Word32 corr[], /* i : correlation vector. */
Word16 scal_sig[], /* i : scaled signal. */
Word16 scal_fac, /* i : scaled signal factor. */
Word16 scal_flag, /* i : if 1 use EFR compatible scaling */
Word16 L_frame, /* i : length of frame to compute pitch */
Word16 lag_max, /* i : maximum lag */
Word16 lag_min, /* i : minimum lag */
Word16 *cor_max, /* o : normalized correlation of selected lag */
Flag dtx /* i : dtx flag; use dtx=1, do not use dtx=0 */
)
{
Word16 i, j;
Word16 *p;
Word32 max, t0;
Word16 max_h, max_l, ener_h, ener_l;
Word16 p_max = 0; /* initialization only needed to keep gcc silent */
long lTemp;
max = MIN_32;
p_max = lag_max;
for (i = lag_max, j = (PIT_MAX-lag_max-1); i >= lag_min; i--, j--)
{
if (corr[-i] >= max)
{
max = corr[-i];
p_max = i;
}
}
/* compute energy */
t0 = 0;
p = &scal_sig[-p_max];
for (i = 0; i < L_frame; i++, p++)
{
//t0 = L_mac (t0, *p, *p);
lTemp = (*p) * (*p);
if (lTemp != 0x40000000)
{
lTemp = lTemp * 2;
}
else
{
lTemp = MAX_32;
}
lTemp = lTemp + t0;
if (lTemp > MAX_32)
{
t0 = MAX_32;
}
else if (lTemp < MIN_32)
{
t0 = MIN_32;
}
else
{
t0 = (Word32)lTemp;
}
}
/* 1/sqrt(energy) */
if (dtx)
{ /* no test() call since this if is only in simulation env */
/* check tone */
vad_tone_detection (vadSt, max, t0);
}
t0 = Inv_sqrt (t0); /* function result */
if (scal_flag)
{
t0 = L_shl (t0, 1);
}
/* max = max/sqrt(energy) */
L_Extract (max, &max_h, &max_l);
L_Extract (t0, &ener_h, &ener_l);
t0 = Mpy_32 (max_h, max_l, ener_h, ener_l);
if (scal_flag)
{
t0 = L_shr (t0, scal_fac);
*cor_max = extract_h (L_shl (t0, 15)); /* divide by 2 */
}
else
{
*cor_max = extract_l(t0);
}
return (p_max);
}
/*---------------------------------------------------------------------------*
* Function Gain_predict *
* ~~~~~~~~~~~~~~~~~~~~~~ *
* MA prediction is performed on the innovation energy (in dB with mean *
* removed). *
*---------------------------------------------------------------------------*/
void Gain_predict(
Word16 past_qua_en[], /* (i) Q10 :Past quantized energies */
Word16 code[], /* (i) Q13 :Innovative vector. */
Word16 L_subfr, /* (i) :Subframe length. */
Word16 *gcode0, /* (o) Qxx :Predicted codebook gain */
Word16 *exp_gcode0 /* (o) :Q-Format(gcode0) */
)
{
Word16 i, exp, frac;
Word32 L_tmp;
/*-------------------------------*
* Energy coming from code *
*-------------------------------*/
L_tmp = 0;
for(i=0; i<L_subfr; i++)
L_tmp = L_mac(L_tmp, code[i], code[i]);
/*-----------------------------------------------------------------*
* Compute: means_ener - 10log10(ener_code/ L_sufr) *
* Note: mean_ener change from 36 dB to 30 dB because input/2 *
* *
* = 30.0 - 10 log10( ener_code / lcode) + 10log10(2^27) *
* !!ener_code in Q27!! *
* = 30.0 - 3.0103 * log2(ener_code) + 10log10(40) + 10log10(2^27) *
* = 30.0 - 3.0103 * log2(ener_code) + 16.02 + 81.278 *
* = 127.298 - 3.0103 * log2(ener_code) *
*-----------------------------------------------------------------*/
Log2(L_tmp, &exp, &frac); /* Q27->Q0 ^Q0 ^Q15 */
L_tmp = Mpy_32_16(exp, frac, -24660); /* Q0 Q15 Q13 -> ^Q14 */
/* hi:Q0+Q13+1 */
/* lo:Q15+Q13-15+1 */
/* -24660[Q13]=-3.0103 */
L_tmp = L_mac(L_tmp, 32588, 32); /* 32588*32[Q14]=127.298 */
/*-----------------------------------------------------------------*
* Compute gcode0. *
* = Sum(i=0,3) pred[i]*past_qua_en[i] - ener_code + mean_ener *
*-----------------------------------------------------------------*/
L_tmp = L_shl(L_tmp, 10); /* From Q14 to Q24 */
for(i=0; i<4; i++)
L_tmp = L_mac(L_tmp, pred[i], past_qua_en[i]); /* Q13*Q10 ->Q24 */
*gcode0 = extract_h(L_tmp); /* From Q24 to Q8 */
/*-----------------------------------------------------------------*
* gcode0 = pow(10.0, gcode0/20) *
* = pow(2, 3.3219*gcode0/20) *
* = pow(2, 0.166*gcode0) *
*-----------------------------------------------------------------*/
L_tmp = L_mult(*gcode0, 5439); /* *0.166 in Q15, result in Q24*/
L_tmp = L_shr(L_tmp, 8); /* From Q24 to Q16 */
L_Extract(L_tmp, &exp, &frac); /* Extract exponent of gcode0 */
*gcode0 = extract_l(Pow2(14, frac)); /* Put 14 as exponent so that */
/* output of Pow2() will be: */
/* 16768 < Pow2() <= 32767 */
*exp_gcode0 = sub(14,exp);
}
/*-----------------------------------------------------------*
* 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;
}
