/*-----------------------------------------------------------*
* procedure Dec_cng: *
* ~~~~~~~~ *
* Receives frame type *
* 0 : for untransmitted frames *
* 2 : for SID frames *
* Decodes SID frames *
* Computes current frame excitation *
* Computes current frame LSPs
*-----------------------------------------------------------*/
void Dec_cng(
Word16 past_ftyp, /* (i) : past frame type */
Word16 sid_sav, /* (i) : energy to recover SID gain */
Word16 sh_sid_sav, /* (i) : corresponding scaling factor */
Word16 *parm, /* (i) : coded SID parameters */
Word16 *exc, /* (i/o) : excitation array */
Word16 *lsp_old, /* (i/o) : previous lsp */
Word16 *A_t, /* (o) : set of interpolated LPC coefficients */
Word16 *seed, /* (i/o) : random generator seed */
Word16 freq_prev[MA_NP][M]
/* (i/o) : previous LPS for quantization */
)
{
Word16 temp, ind;
Word16 dif;
dif = sub(past_ftyp, 1);
/* SID Frame */
/*************/
if(parm[0] != 0) {
sid_gain = tab_Sidgain[(int)parm[4]];
/* Inverse quantization of the LSP */
sid_lsfq_decode(&parm[1], lspSid, freq_prev);
}
/* non SID Frame */
/*****************/
else {
/* Case of 1st SID frame erased : quantize-decode */
/* energy estimate stored in sid_gain */
if(dif > 0) {
Qua_Sidgain(&sid_sav, &sh_sid_sav, 0, &temp, &ind);
sid_gain = tab_Sidgain[(int)ind];
}
}
if(dif > 0) {
cur_gain = sid_gain;
}
else {
cur_gain = mult_r(cur_gain, A_GAIN0);
cur_gain = add(cur_gain, mult_r(sid_gain, A_GAIN1));
}
Calc_exc_rand(cur_gain, exc, seed, FLAG_DEC);
/* Interpolate the Lsp vectors */
Int_qlpc(lsp_old, lspSid, A_t);
Copy(lspSid, lsp_old, M);
return;
}
/*----------------------------------------------------------------------------
* filt_mu - tilt filtering with : (1 + mu z-1) * (1/1-|mu|)
* computes y[n] = (1/1-|mu|) (x[n]+mu*x[n-1])
*----------------------------------------------------------------------------
*/
static void filt_mu(
Word16 *sig_in, /* input : input signal (beginning at sample -1) */
Word16 *sig_out, /* output: output signal */
Word16 parcor0 /* input : parcor0 (mu = parcor0 * gamma3) */
)
{
int n;
Word16 mu, mu2, ga, temp;
Word32 L_acc, L_temp, L_fact;
Word16 fact, sh_fact1;
Word16 *ptrs;
if(parcor0 > 0) {
mu = mult_r(parcor0, GAMMA3_PLUS);
/* GAMMA3_PLUS < 0.5 */
sh_fact1 = 15; /* sh_fact + 1 */
fact = (Word16)0x4000; /* 2**sh_fact */
L_fact = (Word32)0x00004000L;
}
else {
mu = mult_r(parcor0, GAMMA3_MINUS);
/* GAMMA3_MINUS < 0.9375 */
sh_fact1 = 12; /* sh_fact + 1 */
fact = (Word16)0x0800; /* 2**sh_fact */
L_fact = (Word32)0x00000800L;
}
temp = sub(1, abs_s(mu));
mu2 = add(32767, temp); /* 2**15 (1 - |mu|) */
ga = div_s(fact, mu2); /* 2**sh_fact / (1 - |mu|) */
ptrs = sig_in; /* points on sig_in(-1) */
mu = shr(mu, 1); /* to avoid overflows */
for(n=0; n<L_SUBFR; n++) {
temp = *ptrs++;
L_temp = L_deposit_l(*ptrs);
L_acc = L_shl(L_temp, 15); /* sig_in(n) * 2**15 */
L_temp = L_mac(L_acc, mu, temp);
L_temp = L_add(L_temp, 0x00004000L);
temp = extract_l(L_shr(L_temp,15));
/* ga x temp x 2 with rounding */
L_temp = L_add(L_mult(temp, ga),L_fact);
L_temp = L_shr(L_temp, sh_fact1); /* mult. temp x ga */
sig_out[n] = sature(L_temp);
}
return;
}
long interpolation_cos129( short freq )
{
short sin_data,cos_data,count,temp ;
long Ltemp,Lresult;
/* cos(x)=cos(a)+(x-a)sin(a)-pow((a-x),2)*cos(a) */
count=shr(abs_s(freq ),7 );
temp=sub( extract_l(L_mult( count,64)) , freq );
/* (a-x)sin a */
/* Scale factor for (a-x): 3217=pi2/64 */
sin_data=sin129_table [ count];
cos_data=cos129_table [count];
Ltemp=L_mpy_ls(L_mult(3217,temp),sin_data);
/* (a-x) sin(a) - [(a-x)*(a-x)*cos(a)] /2 */
/* Scale factor for (a-x)*(a-x): 20213=pi2*pi2/64 */
Ltemp=L_sub(Ltemp,
L_mpy_ls(L_mult(mult_r(10106,temp),temp),cos_data));
/* Scaled up by 64/2 times */
Ltemp=L_shl( Ltemp ,6 );
Lresult= L_add(L_deposit_h(cos_data), (Ltemp)) ;
return(Lresult);
}
/*-----------------------------------------------------*
* 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;
}
Word32 fnLog2(Word32 L_Input)
{
static Word16
swC0 = -0x2b2a, swC1 = 0x7fc5, swC2 = -0x54d0;
Word16 siShiftCnt, swInSqrd, swIn;
Word32 LwIn;
/*_________________________________________________________________________
| |
| Executable Code |
|_________________________________________________________________________|
*/
/* normalize input and store shifts required */
/* ----------------------------------------- */
siShiftCnt = norm_l(L_Input);
LwIn = L_shl(L_Input, siShiftCnt);
siShiftCnt = add(siShiftCnt, 1);
siShiftCnt = negate(siShiftCnt);
/* calculate x*x*c0 */
/* ---------------- */
swIn = extract_h(LwIn);
swInSqrd = mult_r(swIn, swIn);
LwIn = L_mult(swInSqrd, swC0);
/* add x*c1 */
/* --------- */
LwIn = L_mac(LwIn, swIn, swC1);
/* add c2 */
/* ------ */
LwIn = L_add(LwIn, L_deposit_h(swC2));
/* apply *(4/32) */
/* ------------- */
LwIn = L_shr(LwIn, 3);
LwIn = LwIn & 0x03ffffff;
siShiftCnt = shl(siShiftCnt, 10);
LwIn = L_add(LwIn, L_deposit_h(siShiftCnt));
/* return log2 */
/* ----------- */
return (LwIn);
}
static Word16 Cmp_filt(Word16 *RCoeff, Word16 sh_RCoeff, Word16 *acf,
Word16 alpha, Word16 FracThresh)
{
Word32 L_temp0, L_temp1;
Word16 temp1, temp2, sh[2], ind;
Word16 i;
Word16 diff, flag;
extern Flag Overflow;
sh[0] = 0;
sh[1] = 0;
ind = 1;
flag = 0;
do {
Overflow = 0;
temp1 = shr(RCoeff[0], sh[0]);
temp2 = shr(acf[0], sh[1]);
L_temp0 = L_shr(L_mult(temp1, temp2),1);
for(i=1; i <= M; i++) {
temp1 = shr(RCoeff[i], sh[0]);
temp2 = shr(acf[i], sh[1]);
L_temp0 = L_mac(L_temp0, temp1, temp2);
}
if(Overflow != 0) {
sh[(int)ind] = add(sh[(int)ind], 1);
ind = sub(1, ind);
}
else flag = 1;
} while (flag == 0);
temp1 = mult_r(alpha, FracThresh);
L_temp1 = L_add(L_deposit_l(temp1), L_deposit_l(alpha));
temp1 = add(sh_RCoeff, 9); /* 9 = Lpc_justif. * 2 - 16 + 1 */
temp2 = add(sh[0], sh[1]);
temp1 = sub(temp1, temp2);
L_temp1 = L_shl(L_temp1, temp1);
L_temp0 = L_sub(L_temp0, L_temp1);
if(L_temp0 > 0L) diff = 1;
else diff = 0;
return(diff);
}
static Word16 Quant_Energy(
Word32 L_x, /* (i) : Energy */
Word16 sh, /* (i) : Exponent of the energy */
Word16 *enerq /* (o) : quantized energy in dB */
)
{
Word16 exp, frac;
Word16 e_tmp, temp, index;
Log2(L_x, &exp, &frac);
temp = sub(exp, sh);
e_tmp = shl(temp, 10);
e_tmp = add(e_tmp, mult_r(frac, 1024)); /* 2^10 x log2(L_x . 2^-sh) */
/* log2(ener) = 10log10(ener) / K */
/* K = 10 Log2 / Log10 */
temp = sub(e_tmp, -2721); /* -2721 -> -8dB */
if(temp <= 0) {
*enerq = -12;
return(0);
}
temp = sub(e_tmp, 22111); /* 22111 -> 65 dB */
if(temp > 0) {
*enerq = 66;
return(31);
}
temp = sub(e_tmp, 4762); /* 4762 -> 14 dB */
if(temp <= 0){
e_tmp = add(e_tmp, 3401);
index = mult(e_tmp, 24);
if (index < 1) index = 1;
*enerq = sub(shl(index, 2), 8);
return(index);
}
e_tmp = sub(e_tmp, 340);
index = sub(shr(mult(e_tmp, 193), 2), 1);
if (index < 6) index = 6;
*enerq = add(shl(index, 1), 4);
return(index);
}
/*----------------------------------------------------------------------------
* calc_st_filt - computes impulse response of A(gamma2) / A(gamma1)
* controls gain : computation of energy impulse response as
* SUMn (abs (h[n])) and computes parcor0
*----------------------------------------------------------------------------
*/
static void calc_st_filte(
Word16 *apond2, /* input : coefficients of numerator */
Word16 *apond1, /* input : coefficients of denominator */
Word16 *parcor0, /* output: 1st parcor calcul. on composed filter */
Word16 *sig_ltp_ptr, /* in/out: input of 1/A(gamma1) : scaled by 1/g0 */
Word16 long_h_st, /* input : impulse response length */
Word16 m_pst /* input : LPC order */
)
{
Word16 h[LONG_H_ST_E];
Word32 L_temp, L_g0;
Word16 g0, temp;
Word16 i;
/* compute i.r. of composed filter apond2 / apond1 */
Syn_filte(m_pst, apond1, apond2, h, long_h_st, mem_zero, 0);
/* compute 1st parcor */
calc_rc0_he(h, parcor0, long_h_st);
/* compute g0 */
L_g0 = 0L;
for(i=0; i<long_h_st; i++) {
L_temp = L_deposit_l(abs_s(h[i]));
L_g0 = L_add(L_g0, L_temp);
}
g0 = extract_h(L_shl(L_g0, 14));
/* Scale signal input of 1/A(gamma1) */
if(sub(g0, 1024)>0) {
temp = div_s(1024, g0); /* temp = 2**15 / gain0 */
for(i=0; i<L_SUBFR; i++) {
sig_ltp_ptr[i] = mult_r(sig_ltp_ptr[i], temp);
}
}
return;
}
/*
**
** Function: Calc_Exc_Rand()
**
** Description: Computation of random excitation for inactive frames:
** Adaptive codebook entry selected randomly
** Higher rate innovation pattern selected randomly
** Computes innovation gain to match curGain
**
** Links to text:
**
** Arguments:
**
** Word16 curGain current average gain to match
** Word16 *PrevExc previous/current excitation (updated)
** Word16 *DataEXc current frame excitation
** Word16 *nRandom random generator status (input/output)
**
** Outputs:
**
** Word16 *PrevExc
** Word16 *DataExc
** Word16 *nRandom
**
** Return value: None
**
*/
void Calc_Exc_Rand(Word16 curGain, Word16 *PrevExc, Word16 *DataExc,
Word16 *nRandom, LINEDEF *Line)
{
int i, i_subfr, iblk;
Word16 temp, temp2;
Word16 j;
Word16 TabPos[2*NbPulsBlk], TabSign[2*NbPulsBlk];
Word16 *ptr_TabPos, *ptr_TabSign;
Word16 *ptr1, *curExc;
Word16 sh1, x1, x2, inter_exc, delta, b0;
Word32 L_acc, L_c, L_temp;
Word16 tmp[SubFrLen/Sgrid];
Word16 offset[SubFrames];
Word16 tempExc[SubFrLenD];
/*
* generate LTP codes
*/
Line->Olp[0] = random_number(21, nRandom) + (Word16)123;
Line->Olp[1] = random_number(21, nRandom) + (Word16)123;
for(i_subfr=0; i_subfr<SubFrames; i_subfr++) { /* in [1, NbFilt] */
Line->Sfs[i_subfr].AcGn = random_number(NbFilt, nRandom) + (Word16)1;
}
Line->Sfs[0].AcLg = 1;
Line->Sfs[1].AcLg = 0;
Line->Sfs[2].AcLg = 1;
Line->Sfs[3].AcLg = 3;
/*
* Random innovation :
* Selection of the grids, signs and pulse positions
*/
/* Signs and Grids */
ptr_TabSign = TabSign;
ptr1 = offset;
for(iblk=0; iblk<SubFrames/2; iblk++) {
temp = random_number((1 << (NbPulsBlk+2)), nRandom);
*ptr1++ = temp & (Word16)0x0001;
temp = shr(temp, 1);
*ptr1++ = add( (Word16) SubFrLen, (Word16) (temp & 0x0001) );
for(i=0; i<NbPulsBlk; i++) {
*ptr_TabSign++= shl(sub((temp & (Word16)0x0002), 1), 14);
temp = shr(temp, 1);
}
}
/* Positions */
ptr_TabPos = TabPos;
for(i_subfr=0; i_subfr<SubFrames; i_subfr++) {
for(i=0; i<(SubFrLen/Sgrid); i++) tmp[i] = (Word16)i;
temp = (SubFrLen/Sgrid);
for(i=0; i<Nb_puls[i_subfr]; i++) {
j = random_number(temp, nRandom);
*ptr_TabPos++ = add(shl(tmp[(int)j],1), offset[i_subfr]);
temp = sub(temp, 1);
tmp[(int)j] = tmp[(int)temp];
}
}
/*
* Compute fixed codebook gains
*/
ptr_TabPos = TabPos;
ptr_TabSign = TabSign;
curExc = DataExc;
i_subfr = 0;
for(iblk=0; iblk<SubFrames/2; iblk++) {
/* decode LTP only */
Decod_Acbk(curExc, &PrevExc[0], Line->Olp[iblk],
Line->Sfs[i_subfr].AcLg, Line->Sfs[i_subfr].AcGn);
Decod_Acbk(&curExc[SubFrLen], &PrevExc[SubFrLen], Line->Olp[iblk],
Line->Sfs[i_subfr+1].AcLg, Line->Sfs[i_subfr+1].AcGn);
temp2 = 0;
for(i=0; i<SubFrLenD; i++) {
temp = abs_s(curExc[i]);
if(temp > temp2) temp2 = temp;
}
if(temp2 == 0) sh1 = 0;
else {
sh1 = sub(4,norm_s(temp2)); /* 4 bits of margin */
if(sh1 < -2) sh1 = -2;
}
L_temp = 0L;
for(i=0; i<SubFrLenD; i++) {
temp = shr(curExc[i], sh1); /* left if sh1 < 0 */
L_temp = L_mac(L_temp, temp, temp);
tempExc[i] = temp;
} /* ener_ltp x 2**(-2sh1+1) */
L_acc = 0L;
for(i=0; i<NbPulsBlk; i++) {
L_acc = L_mac(L_acc, tempExc[(int)ptr_TabPos[i]], ptr_TabSign[i]);
}
inter_exc = extract_h(L_shl(L_acc, 1)); /* inter_exc x 2-sh1 */
/* compute SubFrLenD x curGain**2 x 2**(-2sh1+1) */
/* curGain input = 2**5 curGain */
// L_acc = L_mult(curGain, SubFrLen);
L_MULT(curGain, SubFrLen, L_acc);
L_acc = L_shr(L_acc, 6);
temp = extract_l(L_acc); /* SubFrLen x curGain : avoids overflows */
// L_acc = L_mult(temp, curGain);
L_MULT(temp, curGain, L_acc);
temp = shl(sh1, 1);
temp = add(temp, 4);
L_acc = L_shr(L_acc, temp); /* SubFrLenD x curGain**2 x 2**(-2sh1+1) */
/* c = (ener_ltp - SubFrLenD x curGain**2)/nb_pulses_blk */
/* compute L_c = c >> 2sh1-1 */
L_acc = L_sub(L_temp, L_acc);
/* x 1/nb_pulses_blk */
L_c = L_mls(L_acc, InvNbPulsBlk);
/*
* Solve EQ(X) = X**2 + 2 b0 X + c
*/
/* delta = b0 x b0 - c */
b0 = mult_r(inter_exc, InvNbPulsBlk); /* b0 >> sh1 */
L_acc = L_msu(L_c, b0, b0); /* (c - b0**2) >> 2sh1-1 */
L_acc = L_negate(L_acc); /* delta x 2**(-2sh1+1) */
/* Case delta <= 0 */
if(L_acc <= 0) { /* delta <= 0 */
x1 = negate(b0); /* sh1 */
}
/* Case delta > 0 */
else {
delta = Sqrt_lbc(L_acc); /* >> sh1 */
x1 = sub(delta, b0); /* x1 >> sh1 */
x2 = add(b0, delta); /* (-x2) >> sh1 */
if(abs_s(x2) < abs_s(x1)) {
x1 = negate(x2);
}
}
/* Update DataExc */
sh1 = add(sh1, 1);
temp = shl(x1, sh1);
if(temp > (2*Gexc_Max)) temp = (2*Gexc_Max);
if(temp < -(2*Gexc_Max)) temp = -(2*Gexc_Max);
for(i=0; i<NbPulsBlk; i++) {
j = *ptr_TabPos++;
curExc[(int)j] = add(curExc[(int)j], mult(temp,
(*ptr_TabSign++)) );
}
/* update PrevExc */
ptr1 = PrevExc;
for(i=SubFrLenD; i<PitchMax; i++) *ptr1++ = PrevExc[i];
for(i=0; i<SubFrLenD; i++) *ptr1++ = curExc[i];
curExc += SubFrLenD;
i_subfr += 2;
} /* end of loop on LTP blocks */
return;
}
/*
**
** Function: Qua_SidGain()
**
** Description: Quantization of Sid gain
** Pseudo-log quantizer in 3 segments
** 1st segment : length = 16, resolution = 2
** 2nd segment : length = 16, resolution = 4
** 3rd segment : length = 32, resolution = 8
** quantizes a sum of energies
**
** Links to text:
**
** Arguments:
**
** Word16 *Ener table of the energies
** Word16 *shEner corresponding scaling factors
** Word16 nq if nq >= 1 : quantization of nq energies
** for SID gain calculation in function Cod_Cng()
** if nq = 0 : in function Comp_Info(),
** quantization of saved estimated excitation energy
**
** Outputs: None
**
**
** Return value: index of quantized energy
**
*/
Word16 Qua_SidGain(Word16 *Ener, Word16 *shEner, Word16 nq)
{
Word16 temp, iseg, iseg_p1;
Word16 j, j2, k, exp;
Word32 L_x, L_y;
Word16 sh1;
Word32 L_acc;
int i;
if(nq == 0) {
/* Quantize energy saved for frame erasure case */
/* L_x = 2 x average_ener */
temp = shl(*shEner, 1);
temp = sub(16, temp);
L_acc = L_deposit_l(*Ener);
L_acc = L_shl(L_acc, temp); /* may overflow, and >> if temp < 0 */
L_x = L_mls(L_acc, fact[0]);
}
else {
/*
* Compute weighted average of energies
* Ener[i] = enerR[i] x 2**(shEner[i]-14)
* L_x = k[nq] x SUM(i=0->nq-1) enerR[i]
* with k[nq] = 2 x fact_mul x fact_mul / nq x Frame
*/
sh1 = shEner[0];
for(i=1; i<nq; i++) {
if(shEner[i] < sh1) sh1 = shEner[i];
}
for(i=0, L_x=0L; i<nq; i++) {
temp = sub(shEner[i], sh1);
temp = shr(Ener[i], temp);
temp = mult_r(fact[nq], temp);
L_x = L_add(L_x, L_deposit_l(temp));
}
temp = sub(15, sh1);
L_x = L_shl(L_x, temp);
}
/* Quantize L_x */
if(L_x >= L_bseg[2]) return(63);
/* Compute segment number iseg */
if(L_x >= L_bseg[1]) {
iseg = 2;
exp = 4;
}
else {
exp = 3;
if(L_x >= L_bseg[0]) iseg = 1;
else iseg = 0;
}
iseg_p1 = add(iseg,1);
j = shl(1, exp);
k = shr(j,1);
/* Binary search in segment iseg */
for(i=0; i<exp; i++) {
temp = add(base[iseg], shl(j, iseg_p1));
// L_y = L_mult(temp, temp);
L_MULT(temp, temp, L_y);
if(L_x >= L_y) j = add(j, k);
else j = sub(j, k);
k = shr(k, 1);
}
temp = add(base[iseg], shl(j, iseg_p1));
L_y = L_mult(temp, temp);
L_y = L_sub(L_y, L_x);
if(L_y <= 0L) {
j2 = add(j, 1);
temp = add(base[iseg], shl(j2, iseg_p1));
L_acc = L_mult(temp, temp);
L_acc = L_sub(L_x, L_acc);
if(L_y > L_acc) temp = add(shl(iseg,4), j);
else temp = add(shl(iseg,4), j2);
}
else {
j2 = sub(j, 1);
temp = add(base[iseg], shl(j2, iseg_p1));
L_acc = L_mult(temp, temp);
L_acc = L_sub(L_x, L_acc);
if(L_y < L_acc) temp = add(shl(iseg,4), j);
else temp = add(shl(iseg,4), j2);
}
return(temp);
}
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;
}
}
/*-----------------------------------------------------------*
* procedure Calc_exc_rand *
* ~~~~~~~~~~~~~ *
* Computes comfort noise excitation *
* for SID and not-transmitted frames *
*-----------------------------------------------------------*/
void Calc_exc_rand(
Word32 L_exc_err[4],
Word16 cur_gain, /* (i) : target sample gain */
Word16 *exc, /* (i/o) : excitation array */
Word16 *seed, /* (i) : current Vad decision */
Flag flag_cod /* (i) : encoder/decoder flag */
)
{
Word16 i, j, i_subfr;
Word16 temp1, temp2;
Word16 pos[4];
Word16 sign[4];
Word16 t0, frac;
Word16 *cur_exc;
Word16 g, Gp, Gp2;
Word16 excg[L_SUBFR], excs[L_SUBFR];
Word32 L_acc, L_ener, L_k;
Word16 max, hi, lo, inter_exc;
Word16 sh;
Word16 x1, x2;
if(cur_gain == 0) {
for(i=0; i<L_FRAME; i++) {
exc[i] = 0;
}
Gp = 0;
t0 = add(L_SUBFR,1);
for (i_subfr = 0; i_subfr < L_FRAME; i_subfr += L_SUBFR) {
if(flag_cod != FLAG_DEC) update_exc_err(L_exc_err, Gp, t0);
}
return;
}
/* Loop on subframes */
cur_exc = exc;
for (i_subfr = 0; i_subfr < L_FRAME; i_subfr += L_SUBFR) {
/* generate random adaptive codebook & fixed codebook parameters */
/*****************************************************************/
temp1 = Random(seed);
frac = sub((temp1 & (Word16)0x0003), 1);
if(sub(frac, 2) == 0) frac = 0;
temp1 = shr(temp1, 2);
t0 = add((temp1 & (Word16)0x003F), 40);
temp1 = shr(temp1, 6);
temp2 = temp1 & (Word16)0x0007;
pos[0] = add(shl(temp2, 2), temp2); /* 5 * temp2 */
temp1 = shr(temp1, 3);
sign[0] = temp1 & (Word16)0x0001;
temp1 = shr(temp1, 1);
temp2 = temp1 & (Word16)0x0007;
temp2 = add(shl(temp2, 2), temp2);
pos[1] = add(temp2, 1); /* 5 * x + 1 */
temp1 = shr(temp1, 3);
sign[1] = temp1 & (Word16)0x0001;
temp1 = Random(seed);
temp2 = temp1 & (Word16)0x0007;
temp2 = add(shl(temp2, 2), temp2);
pos[2] = add(temp2, 2); /* 5 * x + 2 */
temp1 = shr(temp1, 3);
sign[2] = temp1 & (Word16)0x0001;
temp1 = shr(temp1, 1);
temp2 = temp1 & (Word16)0x000F;
pos[3] = add((temp2 & (Word16)1), 3); /* j+3*/
temp2 = (shr(temp2, 1)) & (Word16)7;
temp2 = add(shl(temp2, 2), temp2); /* 5i */
pos[3] = add(pos[3], temp2);
temp1 = shr(temp1, 4);
sign[3] = temp1 & (Word16)0x0001;
Gp = Random(seed) & (Word16)0x1FFF; /* < 0.5 Q14 */
Gp2 = shl(Gp, 1); /* Q15 */
/* Generate gaussian excitation */
/********************************/
L_acc = 0L;
for(i=0; i<L_SUBFR; i++) {
temp1 = Gauss(seed);
L_acc = L_mac(L_acc, temp1, temp1);
excg[i] = temp1;
}
/*
Compute fact = alpha x cur_gain * sqrt(L_SUBFR / Eg)
with Eg = SUM(i=0->39) excg[i]^2
and alpha = 0.5
alpha x sqrt(L_SUBFR)/2 = 1 + FRAC1
*/
L_acc = Inv_sqrt(L_shr(L_acc,1)); /* Q30 */
L_Extract(L_acc, &hi, &lo);
/* cur_gain = cur_gainR << 3 */
temp1 = mult_r(cur_gain, FRAC1);
temp1 = add(cur_gain, temp1);
/* <=> alpha x cur_gainR x 2^2 x sqrt(L_SUBFR) */
L_acc = Mpy_32_16(hi, lo, temp1); /* fact << 17 */
sh = norm_l(L_acc);
temp1 = extract_h(L_shl(L_acc, sh)); /* fact << (sh+1) */
sh = sub(sh, 14);
for(i=0; i<L_SUBFR; i++) {
temp2 = mult_r(excg[i], temp1);
temp2 = shr_r(temp2, sh); /* shl if sh < 0 */
excg[i] = temp2;
}
/* generate random adaptive excitation */
/****************************************/
Pred_lt_3(cur_exc, t0, frac, L_SUBFR);
/* compute adaptive + gaussian exc -> cur_exc */
/**********************************************/
max = 0;
for(i=0; i<L_SUBFR; i++) {
temp1 = mult_r(cur_exc[i], Gp2);
temp1 = add(temp1, excg[i]); /* may overflow */
cur_exc[i] = temp1;
temp1 = abs_s(temp1);
if(sub(temp1,max) > 0) max = temp1;
}
/* rescale cur_exc -> excs */
if(max == 0) sh = 0;
else {
sh = sub(3, norm_s(max));
if(sh <= 0) sh = 0;
}
for(i=0; i<L_SUBFR; i++) {
excs[i] = shr(cur_exc[i], sh);
}
/* Compute fixed code gain */
/***************************/
/**********************************************************/
/*** Solve EQ(X) = 4 X**2 + 2 b X + c */
/**********************************************************/
L_ener = 0L;
for(i=0; i<L_SUBFR; i++) {
L_ener = L_mac(L_ener, excs[i], excs[i]);
} /* ener x 2^(-2sh + 1) */
/* inter_exc = b >> sh */
inter_exc = 0;
for(i=0; i<4; i++) {
j = pos[i];
if(sign[i] == 0) {
inter_exc = sub(inter_exc, excs[j]);
}
else {
inter_exc = add(inter_exc, excs[j]);
}
}
/* Compute k = cur_gainR x cur_gainR x L_SUBFR */
L_acc = L_mult(cur_gain, L_SUBFR);
L_acc = L_shr(L_acc, 6);
temp1 = extract_l(L_acc); /* cur_gainR x L_SUBFR x 2^(-2) */
L_k = L_mult(cur_gain, temp1); /* k << 2 */
temp1 = add(1, shl(sh,1));
L_acc = L_shr(L_k, temp1); /* k x 2^(-2sh+1) */
/* Compute delta = b^2 - 4 c */
L_acc = L_sub(L_acc, L_ener); /* - 4 c x 2^(-2sh-1) */
inter_exc = shr(inter_exc, 1);
L_acc = L_mac(L_acc, inter_exc, inter_exc); /* 2^(-2sh-1) */
sh = add(sh, 1);
/* inter_exc = b x 2^(-sh) */
/* L_acc = delta x 2^(-2sh+1) */
if(L_acc < 0) {
/* adaptive excitation = 0 */
Copy(excg, cur_exc, L_SUBFR);
temp1 = abs_s(excg[(int)pos[0]]) | abs_s(excg[(int)pos[1]]);
temp2 = abs_s(excg[(int)pos[2]]) | abs_s(excg[(int)pos[3]]);
temp1 = temp1 | temp2;
sh = ((temp1 & (Word16)0x4000) == 0) ? (Word16)1 : (Word16)2;
inter_exc = 0;
for(i=0; i<4; i++) {
temp1 = shr(excg[(int)pos[i]], sh);
if(sign[i] == 0) {
inter_exc = sub(inter_exc, temp1);
}
else {
inter_exc = add(inter_exc, temp1);
}
} /* inter_exc = b >> sh */
L_Extract(L_k, &hi, &lo);
L_acc = Mpy_32_16(hi, lo, K0); /* k x (1- alpha^2) << 2 */
temp1 = sub(shl(sh, 1), 1); /* temp1 > 0 */
L_acc = L_shr(L_acc, temp1); /* 4k x (1 - alpha^2) << (-2sh+1) */
L_acc = L_mac(L_acc, inter_exc, inter_exc); /* delta << (-2sh+1) */
Gp = 0;
}
temp2 = Sqrt(L_acc); /* >> sh */
x1 = sub(temp2, inter_exc);
x2 = negate(add(inter_exc, temp2)); /* x 2^(-sh+2) */
if(sub(abs_s(x2),abs_s(x1)) < 0) x1 = x2;
temp1 = sub(2, sh);
g = shr_r(x1, temp1); /* shl if temp1 < 0 */
if(g >= 0) {
if(sub(g, G_MAX) > 0) g = G_MAX;
}
else {
if(add(g, G_MAX) < 0) g = negate(G_MAX);
}
/* Update cur_exc with ACELP excitation */
for(i=0; i<4; i++) {
j = pos[i];
if(sign[i] != 0) {
cur_exc[j] = add(cur_exc[j], g);
}
else {
cur_exc[j] = sub(cur_exc[j], g);
}
}
if(flag_cod != FLAG_DEC) update_exc_err(L_exc_err, Gp, t0);
cur_exc += L_SUBFR;
} /* end of loop on subframes */
return;
}
void Coder_ld8g(
Word16 ana[], /* (o) : analysis parameters */
Word16 frame, /* input : frame counter */
Word16 dtx_enable, /* input : DTX enable flag */
Word16 rate /* input : rate selector/frame =0 6.4kbps , =1 8kbps,= 2 11.8 kbps*/
)
{
/* LPC analysis */
Word16 r_l_fwd[NP+1], r_h_fwd[NP+1]; /* Autocorrelations low and hi (forward) */
Word32 r_bwd[M_BWDP1]; /* Autocorrelations (backward) */
Word16 r_l_bwd[M_BWDP1]; /* Autocorrelations low (backward) */
Word16 r_h_bwd[M_BWDP1]; /* Autocorrelations high (backward) */
Word16 rc_fwd[M]; /* Reflection coefficients : forward analysis */
Word16 rc_bwd[M_BWD]; /* Reflection coefficients : backward analysis */
Word16 A_t_fwd[MP1*2]; /* A(z) forward unquantized for the 2 subframes */
Word16 A_t_fwd_q[MP1*2]; /* A(z) forward quantized for the 2 subframes */
Word16 A_t_bwd[2*M_BWDP1]; /* A(z) backward for the 2 subframes */
Word16 *Aq; /* A(z) "quantized" for the 2 subframes */
Word16 *Ap; /* A(z) "unquantized" for the 2 subframes */
Word16 *pAp, *pAq;
Word16 Ap1[M_BWDP1]; /* A(z) with spectral expansion */
Word16 Ap2[M_BWDP1]; /* A(z) with spectral expansion */
Word16 lsp_new[M], lsp_new_q[M]; /* LSPs at 2th subframe */
Word16 lsf_int[M]; /* Interpolated LSF 1st subframe. */
Word16 lsf_new[M];
Word16 lp_mode; /* Backward / Forward Indication mode */
Word16 m_ap, m_aq, i_gamma;
Word16 code_lsp[2];
/* Other vectors */
Word16 h1[L_SUBFR]; /* Impulse response h1[] */
Word16 xn[L_SUBFR]; /* Target vector for pitch search */
Word16 xn2[L_SUBFR]; /* Target vector for codebook search */
Word16 code[L_SUBFR]; /* Fixed codebook excitation */
Word16 y1[L_SUBFR]; /* Filtered adaptive excitation */
Word16 y2[L_SUBFR]; /* Filtered fixed codebook excitation */
Word16 g_coeff[4]; /* Correlations between xn & y1 */
Word16 res2[L_SUBFR]; /* residual after long term prediction*/
Word16 g_coeff_cs[5];
Word16 exp_g_coeff_cs[5]; /* Correlations between xn, y1, & y2
<y1,y1>, -2<xn,y1>,
<y2,y2>, -2<xn,y2>, 2<y1,y2> */
/* Scalars */
Word16 i, j, k, i_subfr;
Word16 T_op, T0, T0_min, T0_max, T0_frac;
Word16 gain_pit, gain_code, index;
Word16 taming, pit_sharp;
Word16 sat_filter;
Word32 L_temp;
Word16 freq_cur[M];
/* For G.729B */
Word16 rh_nbe[MP1];
Word16 lsfq_mem[MA_NP][M];
Word16 exp_R0, Vad;
Word16 tmp1, tmp2,avg_lag;
Word16 temp, Energy_db;
/*------------------------------------------------------------------------*
* - Perform LPC analysis: *
* * autocorrelation + lag windowing *
* * Levinson-durbin algorithm to find a[] *
* * convert a[] to lsp[] *
* * quantize and code the LSPs *
* * find the interpolated LSPs and convert to a[] for the 2 *
* subframes (both quantized and unquantized) *
*------------------------------------------------------------------------*/
/* ------------------- */
/* LP Forward analysis */
/* ------------------- */
Autocorrg(p_window, NP, r_h_fwd, r_l_fwd, &exp_R0); /* Autocorrelations */
Copy(r_h_fwd, rh_nbe, MP1);
Lag_window(NP, r_h_fwd, r_l_fwd); /* Lag windowing */
Levinsong(M, r_h_fwd, r_l_fwd, &A_t_fwd[MP1], rc_fwd, old_A_fwd, old_rc_fwd,&temp ); /* Levinson Durbin */
Az_lsp(&A_t_fwd[MP1], lsp_new, lsp_old); /* From A(z) to lsp */
/* For G.729B */
/* ------ VAD ------- */
if (dtx_enable == 1) {
Lsp_lsf(lsp_new, lsf_new, M);
vadg(rc_fwd[1], lsf_new, r_h_fwd, r_l_fwd, exp_R0, p_window, frame,
pastVad, ppastVad, &Vad, &Energy_db);
musdetect( rate, r_h_fwd[0], r_l_fwd[0], exp_R0,rc_fwd ,lag_buf , pgain_buf,
prev_lp_mode, frame,pastVad, &Vad, Energy_db);
Update_cng(rh_nbe, exp_R0, Vad);
}
else Vad = 1;
/* -------------------- */
/* LP Backward analysis */
/* -------------------- */
/* -------------------- */
/* LP Backward analysis */
/* -------------------- */
if ( (rate-(1-Vad))== G729E) {
/* LPC recursive Window as in G728 */
autocorr_hyb_window(synth, r_bwd, rexp); /* Autocorrelations */
Lag_window_bwd(r_bwd, r_h_bwd, r_l_bwd); /* Lag windowing */
/* Fixed Point Levinson (as in G729) */
Levinsong(M_BWD, r_h_bwd, r_l_bwd, &A_t_bwd[M_BWDP1], rc_bwd,
old_A_bwd, old_rc_bwd, &temp );
/* Tests saturation of A_t_bwd */
sat_filter = 0;
for (i=M_BWDP1; i<2*M_BWDP1; i++) if (A_t_bwd[i] >= 32767) sat_filter = 1;
if (sat_filter == 1) Copy(A_t_bwd_mem, &A_t_bwd[M_BWDP1], M_BWDP1);
else Copy(&A_t_bwd[M_BWDP1], A_t_bwd_mem, M_BWDP1);
/* Additional bandwidth expansion on backward filter */
Weight_Az(&A_t_bwd[M_BWDP1], GAMMA_BWD, M_BWD, &A_t_bwd[M_BWDP1]);
}
/*--------------------------------------------------*
* Update synthesis signal for next frame. *
*--------------------------------------------------*/
Copy(&synth[L_FRAME], &synth[0], MEM_SYN_BWD);
/*--------------------------------------------------------------------*
* Find interpolated LPC parameters in all subframes (unquantized). *
* The interpolated parameters are in array A_t[] of size (M+1)*4 *
*--------------------------------------------------------------------*/
if( prev_lp_mode == 0) {
Int_lpc(lsp_old, lsp_new, lsf_int, lsf_new, A_t_fwd);
}
else {
/* no interpolation */
/* unquantized */
Lsp_Az(lsp_new, A_t_fwd); /* Subframe 1 */
Lsp_lsf(lsp_new, lsf_new, M); /* transformation from LSP to LSF (freq.domain) */
Copy(lsf_new, lsf_int, M); /* Subframe 1 */
}
if(Vad == 1) {
/* ---------------- */
/* LSP quantization */
/* ---------------- */
Qua_lspe(lsp_new, lsp_new_q, code_lsp, freq_prev, freq_cur);
/*--------------------------------------------------------------------*
* Find interpolated LPC parameters in all subframes (quantized) *
* the quantized interpolated parameters are in array Aq_t[] *
*--------------------------------------------------------------------*/
if( prev_lp_mode == 0) {
Int_qlpc(lsp_old_q, lsp_new_q, A_t_fwd_q);
}
else {
/* no interpolation */
Lsp_Az(lsp_new_q, &A_t_fwd_q[MP1]); /* Subframe 2 */
Copy(&A_t_fwd_q[MP1], A_t_fwd_q, MP1); /* Subframe 1 */
}
/*---------------------------------------------------------------------*
* - Decision for the switch Forward / Backward *
*---------------------------------------------------------------------*/
if(rate == G729E) {
set_lpc_modeg(speech, A_t_fwd_q, A_t_bwd, &lp_mode,
lsp_new, lsp_old, &bwd_dominant, prev_lp_mode, prev_filter,
&C_int, &glob_stat, &stat_bwd, &val_stat_bwd);
}
else {
update_bwd( &lp_mode, &bwd_dominant, &C_int, &glob_stat);
}
}
else update_bwd( &lp_mode, &bwd_dominant, &C_int, &glob_stat);
/* ---------------------------------- */
/* update the LSPs for the next frame */
/* ---------------------------------- */
Copy(lsp_new, lsp_old, M);
/*----------------------------------------------------------------------*
* - Find the weighted input speech w_sp[] for the whole speech frame *
*----------------------------------------------------------------------*/
if(lp_mode == 0) {
m_ap = M;
if (bwd_dominant == 0) Ap = A_t_fwd;
else Ap = A_t_fwd_q;
perc_var(gamma1, gamma2, lsf_int, lsf_new, rc_fwd);
}
else {
if (bwd_dominant == 0) {
m_ap = M;
Ap = A_t_fwd;
}
else {
m_ap = M_BWD;
Ap = A_t_bwd;
}
perc_vare(gamma1, gamma2, bwd_dominant);
}
pAp = Ap;
for (i=0; i<2; i++) {
Weight_Az(pAp, gamma1[i], m_ap, Ap1);
Weight_Az(pAp, gamma2[i], m_ap, Ap2);
Residue(m_ap, Ap1, &speech[i*L_SUBFR], &wsp[i*L_SUBFR], L_SUBFR);
Syn_filte(m_ap, Ap2, &wsp[i*L_SUBFR], &wsp[i*L_SUBFR], L_SUBFR,
&mem_w[M_BWD-m_ap], 0);
for(j=0; j<M_BWD; j++) mem_w[j] = wsp[i*L_SUBFR+L_SUBFR-M_BWD+j];
pAp += m_ap+1;
}
/* ---------------------- */
/* Case of Inactive frame */
/* ---------------------- */
if (Vad == 0){
for (i=0; i<MA_NP; i++) Copy(&freq_prev[i][0], &lsfq_mem[i][0], M);
Cod_cngg(exc, pastVad, lsp_old_q, old_A_fwd, old_rc_fwd, A_t_fwd_q, ana, lsfq_mem, &seed);
for (i=0; i<MA_NP; i++) Copy(&lsfq_mem[i][0], &freq_prev[i][0], M);
ppastVad = pastVad;
pastVad = Vad;
/* UPDATE wsp, mem_w, mem_syn, mem_err, and mem_w0 */
pAp = A_t_fwd; /* pointer to interpolated LPC parameters */
pAq = A_t_fwd_q; /* pointer to interpolated quantized LPC parameters */
i_gamma = 0;
for(i_subfr=0; i_subfr < L_FRAME; i_subfr += L_SUBFR) {
Weight_Az(pAp, gamma1[i_gamma], M, Ap1);
Weight_Az(pAp, gamma2[i_gamma], M, Ap2);
i_gamma = add(i_gamma,1);
/* update mem_syn */
Syn_filte(M, pAq, &exc[i_subfr], &synth_ptr[i_subfr], L_SUBFR, &mem_syn[M_BWD-M], 0);
for(j=0; j<M_BWD; j++) mem_syn[j] = synth_ptr[i_subfr+L_SUBFR-M_BWD+j];
/* update mem_w0 */
for (i=0; i<L_SUBFR; i++)
error[i] = speech[i_subfr+i] - synth_ptr[i_subfr+i];
Residue(M, Ap1, error, xn, L_SUBFR);
Syn_filte(M, Ap2, xn, xn, L_SUBFR, &mem_w0[M_BWD-M], 0);
for(j=0; j<M_BWD; j++) mem_w0[j] = xn[L_SUBFR-M_BWD+j];
/* update mem_err */
for (i = L_SUBFR-M_BWD, j = 0; i < L_SUBFR; i++, j++)
mem_err[j] = error[i];
for (i= 0; i< 4; i++)
pgain_buf[i] = pgain_buf[i+1];
pgain_buf[4] = 8192;
pAp += MP1;
pAq += MP1;
}
/* update previous filter for next frame */
Copy(&A_t_fwd_q[MP1], prev_filter, MP1);
for(i=MP1; i <M_BWDP1; i++) prev_filter[i] = 0;
prev_lp_mode = lp_mode;
sharp = SHARPMIN;
/* Update memories for next frames */
Copy(&old_speech[L_FRAME], &old_speech[0], L_TOTAL-L_FRAME);
Copy(&old_wsp[L_FRAME], &old_wsp[0], PIT_MAX);
Copy(&old_exc[L_FRAME], &old_exc[0], PIT_MAX+L_INTERPOL);
return;
} /* End of inactive frame case */
/* -------------------- */
/* Case of Active frame */
/* -------------------- */
*ana++ = rate+ (Word16)2; /* bit rate mode */
if(lp_mode == 0) {
m_aq = M;
Aq = A_t_fwd_q;
/* update previous filter for next frame */
Copy(&Aq[MP1], prev_filter, MP1);
for(i=MP1; i <M_BWDP1; i++) prev_filter[i] = 0;
for(j=MP1; j<M_BWDP1; j++) ai_zero[j] = 0;
}
else {
m_aq = M_BWD;
Aq = A_t_bwd;
if (bwd_dominant == 0) {
for(j=MP1; j<M_BWDP1; j++) ai_zero[j] = 0;
}
/* update previous filter for next frame */
Copy(&Aq[M_BWDP1], prev_filter, M_BWDP1);
}
if(dtx_enable == 1) {
seed = INIT_SEED;
ppastVad = pastVad;
pastVad = Vad;
}
if (rate == G729E) *ana++ = lp_mode;
/*----------------------------------------------------------------------*
* - Find the weighted input speech w_sp[] for the whole speech frame *
* - Find the open-loop pitch delay *
*----------------------------------------------------------------------*/
if( lp_mode == 0) {
Copy(lsp_new_q, lsp_old_q, M);
Lsp_prev_update(freq_cur, freq_prev);
*ana++ = code_lsp[0];
*ana++ = code_lsp[1];
}
/* Find open loop pitch lag */
T_op = Pitch_ol(wsp, PIT_MIN, PIT_MAX, L_FRAME);
for (i= 0; i< 4; i++)
lag_buf[i] = lag_buf[i+1];
avg_lag = add(lag_buf[0],lag_buf[1]);
avg_lag = add(avg_lag,lag_buf[2]);
avg_lag = add(avg_lag,lag_buf[3]);
avg_lag = mult_r(avg_lag,8192);
tmp1 = sub(T_op,shl(avg_lag,1));
tmp2 = sub(T_op,add(shl(avg_lag,1),avg_lag));
if( sub(abs_s(tmp1), 4)<0){
lag_buf[4] = shr(T_op,1);
}
else if( sub(abs_s(tmp2),6)<0){
lag_buf[4] = mult(T_op,10923);
}
else{
lag_buf[4] = T_op;
}
/* Range for closed loop pitch search in 1st subframe */
T0_min = sub(T_op, 3);
if (sub(T0_min,PIT_MIN)<0) {
T0_min = PIT_MIN;
}
T0_max = add(T0_min, 6);
if (sub(T0_max ,PIT_MAX)>0)
{
T0_max = PIT_MAX;
T0_min = sub(T0_max, 6);
}
/*------------------------------------------------------------------------*
* Loop for every subframe in the analysis frame *
*------------------------------------------------------------------------*
* To find the pitch and innovation parameters. The subframe size is *
* L_SUBFR and the loop is repeated 2 times. *
* - find the weighted LPC coefficients *
* - find the LPC residual signal res[] *
* - compute the target signal for pitch search *
* - compute impulse response of weighted synthesis filter (h1[]) *
* - find the closed-loop pitch parameters *
* - encode the pitch delay *
* - update the impulse response h1[] by including fixed-gain pitch *
* - find target vector for codebook search *
* - codebook search *
* - encode codebook address *
* - VQ of pitch and codebook gains *
* - find synthesis speech *
* - update states of weighting filter *
*------------------------------------------------------------------------*/
pAp = Ap; /* pointer to interpolated "unquantized"LPC parameters */
pAq = Aq; /* pointer to interpolated "quantized" LPC parameters */
i_gamma = 0;
for (i_subfr = 0; i_subfr < L_FRAME; i_subfr += L_SUBFR) {
/*---------------------------------------------------------------*
* Find the weighted LPC coefficients for the weighting filter. *
*---------------------------------------------------------------*/
Weight_Az(pAp, gamma1[i_gamma], m_ap, Ap1);
Weight_Az(pAp, gamma2[i_gamma], m_ap, Ap2);
/*---------------------------------------------------------------*
* Compute impulse response, h1[], of weighted synthesis filter *
*---------------------------------------------------------------*/
for (i = 0; i <=m_ap; i++) ai_zero[i] = Ap1[i];
Syn_filte(m_aq, pAq, ai_zero, h1, L_SUBFR, zero, 0);
Syn_filte(m_ap, Ap2, h1, h1, L_SUBFR, zero, 0);
/*------------------------------------------------------------------------*
* *
* Find the target vector for pitch search: *
* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ *
* *
* |------| res[n] *
* speech[n]---| A(z) |-------- *
* |------| | |--------| error[n] |------| *
* zero -- (-)--| 1/A(z) |-----------| W(z) |-- target *
* exc |--------| |------| *
* *
* Instead of subtracting the zero-input response of filters from *
* the weighted input speech, the above configuration is used to *
* compute the target vector. This configuration gives better performance *
* with fixed-point implementation. The memory of 1/A(z) is updated by *
* filtering (res[n]-exc[n]) through 1/A(z), or simply by subtracting *
* the synthesis speech from the input speech: *
* error[n] = speech[n] - syn[n]. *
* The memory of W(z) is updated by filtering error[n] through W(z), *
* or more simply by subtracting the filtered adaptive and fixed *
* codebook excitations from the target: *
* target[n] - gain_pit*y1[n] - gain_code*y2[n] *
* as these signals are already available. *
* *
*------------------------------------------------------------------------*/
Residue(m_aq, pAq, &speech[i_subfr], &exc[i_subfr], L_SUBFR); /* LPC residual */
for (i=0; i<L_SUBFR; i++) res2[i] = exc[i_subfr+i];
Syn_filte(m_aq, pAq, &exc[i_subfr], error, L_SUBFR,
&mem_err[M_BWD-m_aq], 0);
Residue(m_ap, Ap1, error, xn, L_SUBFR);
Syn_filte(m_ap, Ap2, xn, xn, L_SUBFR, &mem_w0[M_BWD-m_ap], 0); /* target signal xn[]*/
/*----------------------------------------------------------------------*
* Closed-loop fractional pitch search *
*----------------------------------------------------------------------*/
T0 = Pitch_fr3(&exc[i_subfr], xn, h1, L_SUBFR, T0_min, T0_max,
i_subfr, &T0_frac);
index = Enc_lag3(T0, T0_frac, &T0_min, &T0_max,PIT_MIN,PIT_MAX,
i_subfr);
*ana++ = index;
if ( (i_subfr == 0) ) {
*ana = Parity_Pitch(index);
if( rate == G729E) {
*ana ^= (shr(index, 1) & 0x0001);
}
ana++;
}
/*-----------------------------------------------------------------*
* - find unity gain pitch excitation (adaptive codebook entry) *
* with fractional interpolation. *
* - find filtered pitch exc. y1[]=exc[] convolve with h1[]) *
* - compute pitch gain and limit between 0 and 1.2 *
* - update target vector for codebook search *
* - find LTP residual. *
*-----------------------------------------------------------------*/
Pred_lt_3(&exc[i_subfr], T0, T0_frac, L_SUBFR);
Convolve(&exc[i_subfr], h1, y1, L_SUBFR);
gain_pit = G_pitch(xn, y1, g_coeff, L_SUBFR);
/* clip pitch gain if taming is necessary */
taming = test_err(T0, T0_frac);
if( taming == 1){
if (sub(gain_pit, GPCLIP) > 0) {
gain_pit = GPCLIP;
}
}
/* xn2[i] = xn[i] - y1[i] * gain_pit */
for (i = 0; i < L_SUBFR; i++) {
L_temp = L_mult(y1[i], gain_pit);
L_temp = L_shl(L_temp, 1); /* gain_pit in Q14 */
xn2[i] = sub(xn[i], extract_h(L_temp));
}
/*-----------------------------------------------------*
* - Innovative codebook search. *
*-----------------------------------------------------*/
switch (rate) {
case G729: /* 8 kbit/s */
{
/* case 8 kbit/s */
index = ACELP_Codebook(xn2, h1, T0, sharp, i_subfr, code, y2, &i);
*ana++ = index; /* Positions index */
*ana++ = i; /* Signs index */
break;
}
case G729E: /* 11.8 kbit/s */
{
/*-----------------------------------------------------------------*
* Include fixed-gain pitch contribution into impulse resp. h[] *
*-----------------------------------------------------------------*/
pit_sharp = shl(sharp, 1); /* From Q14 to Q15 */
if(T0 < L_SUBFR) {
for (i = T0; i < L_SUBFR; i++){ /* h[i] += pitch_sharp*h[i-T0] */
h1[i] = add(h1[i], mult(h1[i-T0], pit_sharp));
}
}
/* calculate residual after long term prediction */
/* res2[i] -= exc[i+i_subfr] * gain_pit */
for (i = 0; i < L_SUBFR; i++) {
L_temp = L_mult(exc[i+i_subfr], gain_pit);
L_temp = L_shl(L_temp, 1); /* gain_pit in Q14 */
res2[i] = sub(res2[i], extract_h(L_temp));
}
if (lp_mode == 0) ACELP_10i40_35bits(xn2, res2, h1, code, y2, ana); /* Forward */
else ACELP_12i40_44bits(xn2, res2, h1, code, y2, ana); /* Backward */
ana += 5;
/*-----------------------------------------------------------------*
* Include fixed-gain pitch contribution into code[]. *
*-----------------------------------------------------------------*/
if(T0 < L_SUBFR) {
for (i = T0; i < L_SUBFR; i++) { /* code[i] += pitch_sharp*code[i-T0] */
code[i] = add(code[i], mult(code[i-T0], pit_sharp));
}
}
break;
}
default : {
printf("Unrecognized bit rate\n");
exit(-1);
}
} /* end of switch */
/*-----------------------------------------------------*
* - Quantization of gains. *
*-----------------------------------------------------*/
g_coeff_cs[0] = g_coeff[0]; /* <y1,y1> */
exp_g_coeff_cs[0] = negate(g_coeff[1]); /* Q-Format:XXX -> JPN */
g_coeff_cs[1] = negate(g_coeff[2]); /* (xn,y1) -> -2<xn,y1> */
exp_g_coeff_cs[1] = negate(add(g_coeff[3], 1)); /* Q-Format:XXX -> JPN */
Corr_xy2( xn, y1, y2, g_coeff_cs, exp_g_coeff_cs ); /* Q0 Q0 Q12 ^Qx ^Q0 */
/* g_coeff_cs[3]:exp_g_coeff_cs[3] = <y2,y2> */
/* g_coeff_cs[4]:exp_g_coeff_cs[4] = -2<xn,y2> */
/* g_coeff_cs[5]:exp_g_coeff_cs[5] = 2<y1,y2> */
index = Qua_gain(code, g_coeff_cs, exp_g_coeff_cs, L_SUBFR,
&gain_pit, &gain_code, taming);
*ana++ = index;
/*------------------------------------------------------------*
* - Update pitch sharpening "sharp" with quantized gain_pit *
*------------------------------------------------------------*/
for (i= 0; i< 4; i++)
pgain_buf[i] = pgain_buf[i+1];
pgain_buf[4] = gain_pit;
sharp = gain_pit;
if (sub(sharp, SHARPMAX) > 0) sharp = SHARPMAX;
else {
if (sub(sharp, SHARPMIN) < 0) sharp = SHARPMIN;
}
/*------------------------------------------------------*
* - Find the total excitation *
* - find synthesis speech corresponding to exc[] *
* - update filters memories for finding the target *
* vector in the next subframe *
* (update error[-m..-1] and mem_w_err[]) *
* update error function for taming process *
*------------------------------------------------------*/
for (i = 0; i < L_SUBFR; i++) {
/* exc[i] = gain_pit*exc[i] + gain_code*code[i]; */
/* exc[i] in Q0 gain_pit in Q14 */
/* code[i] in Q13 gain_cod in Q1 */
L_temp = L_mult(exc[i+i_subfr], gain_pit);
L_temp = L_mac(L_temp, code[i], gain_code);
L_temp = L_shl(L_temp, 1);
exc[i+i_subfr] = round(L_temp);
}
update_exc_err(gain_pit, T0);
Syn_filte(m_aq, pAq, &exc[i_subfr], &synth_ptr[i_subfr], L_SUBFR,
&mem_syn[M_BWD-m_aq], 0);
for(j=0; j<M_BWD; j++) mem_syn[j] = synth_ptr[i_subfr+L_SUBFR-M_BWD+j];
for (i = L_SUBFR-M_BWD, j = 0; i < L_SUBFR; i++, j++) {
mem_err[j] = sub(speech[i_subfr+i], synth_ptr[i_subfr+i]);
temp = extract_h(L_shl( L_mult(y1[i], gain_pit), 1) );
k = extract_h(L_shl( L_mult(y2[i], gain_code), 2) );
mem_w0[j] = sub(xn[i], add(temp, k));
}
pAp += m_ap+1;
pAq += m_aq+1;
i_gamma = add(i_gamma,1);
}
/*--------------------------------------------------*
* Update signal for next frame. *
* -> shift to the left by L_FRAME: *
* speech[], wsp[] and exc[] *
*--------------------------------------------------*/
Copy(&old_speech[L_FRAME], &old_speech[0], L_TOTAL-L_FRAME);
Copy(&old_wsp[L_FRAME], &old_wsp[0], PIT_MAX);
Copy(&old_exc[L_FRAME], &old_exc[0], PIT_MAX+L_INTERPOL);
prev_lp_mode = lp_mode;
return;
}
/*
**
** Function: Lsp_Svq()
**
** Description: Performs the search of the VQ tables to find
** the optimum LSP indices for all three bands.
** For each band, the search finds the index which
** minimizes the weighted squared error between
** the table entry and the target.
**
** Links to text: Section 2.5
**
** Arguments:
**
** Word16 Tv[] VQ target vector (10 words)
** Word16 Wvect[] VQ weight vector (10 words)
**
** Outputs: None
**
** Return value:
**
** Word32 Long word packed with the 3 VQ indices. Band 0
** corresponds to bits [23:16], band 1 corresponds
** to bits [15:8], and band 2 corresponds to bits [7:0].
**
*/
Word32 Lsp_Svq( Word16 *Tv, Word16 *Wvect )
{
int i,j,k ;
Word32 Rez,Indx ;
Word32 Acc0,Acc1 ;
Word16 Tmp[LpcOrder] ;
Word16 *LspQntPnt ;
/*
* Initialize the return value
*/
Rez = (Word32) 0 ;
/*
* Quantize each band separately
*/
for ( k = 0 ; k < LspQntBands ; k ++ ) {
/*
* Search over the entire VQ table to find the index that
* minimizes the error.
*/
/* Initialize the search */
Acc1 = (Word32) -1 ;
Indx = (Word32) 0 ;
LspQntPnt = BandQntTable[k] ;
for ( i = 0 ; i < LspCbSize ; i ++ ) {
/*
* Generate the metric, which is the negative error with the
* constant component removed.
*/
for ( j = 0 ; j < BandInfoTable[k][1] ; j ++ )
Tmp[j] = mult_r( Wvect[BandInfoTable[k][0]+j],
LspQntPnt[j] ) ;
Acc0 = (Word32) 0 ;
for ( j = 0 ; j < BandInfoTable[k][1] ; j ++ )
Acc0 = L_mac( Acc0, Tv[BandInfoTable[k][0]+j], Tmp[j] ) ;
Acc0 = L_shl( Acc0, (Word16) 1 ) ;
for ( j = 0 ; j < BandInfoTable[k][1] ; j ++ )
Acc0 = L_msu( Acc0, LspQntPnt[j], Tmp[j] ) ;
LspQntPnt += BandInfoTable[k][1] ;
/*
* Compare the metric to the previous maximum and select the
* new index
*/
if ( Acc0 > Acc1 ) {
Acc1 = Acc0 ;
Indx = (Word32) i ;
}
}
/*
* Pack the result with the optimum index for this band
*/
Rez = L_shl( Rez, (Word16) LspCbBits ) ;
// Rez = L_add( Rez, Indx ) ;
L_ADD(Rez, Indx, Rez);
}
return Rez ;
}
/*----------------------------------------------------------------------------
* pst_ltp - harmonic postfilter
*----------------------------------------------------------------------------
*/
static void pst_ltpe(
Word16 t0, /* input : pitch delay given by coder */
Word16 *ptr_sig_in, /* input : postfilter input filter (residu2) */
Word16 *ptr_sig_pst0, /* output: harmonic postfilter output */
Word16 *vo, /* output: voicing decision 0 = uv, > 0 delay */
Word16 gamma_harm /* input: harmonic postfilter coefficient */
)
{
/**** Declare variables */
Word16 i;
Word16 temp;
Word16 ltpdel, phase;
Word16 num_gltp, den_gltp;
Word16 num2_gltp, den2_gltp;
Word16 sh_num, sh_den;
Word16 sh_num2, sh_den2;
Word16 gain_plt;
Word16 y_up[SIZ_Y_UP];
Word16 *ptr_y_up;
Word16 off_yup;
Word16 *ptr_sig;
Word16 sig_cadr[SIZ_RES2], *ptr_sig_cadr;
Word16 nb_sh_sig;
Word32 L_temp;
/* input signal justified on 13 bits */
ptr_sig = ptr_sig_in - MEM_RES2;
temp = 0;
for(i=0; i<SIZ_RES2; i++) {
temp |= abs_s(ptr_sig[i]);
}
nb_sh_sig = sub(3, norm_s(temp));
for(i=0; i<SIZ_RES2; i++) { /* nb_sh_sig may be >0, <0 or =0 */
sig_cadr[i] = shr( ptr_sig[i], nb_sh_sig);
}
ptr_sig_cadr = sig_cadr + MEM_RES2;
/* Sub optimal delay search */
search_del(t0, ptr_sig_cadr, <pdel, &phase, &num_gltp, &den_gltp,
&sh_num, &sh_den, y_up, &off_yup);
*vo = ltpdel;
if(num_gltp == 0) {
Copy(ptr_sig_in, ptr_sig_pst0, L_SUBFR);
}
else {
if(phase == 0) {
ptr_y_up = ptr_sig_in - ltpdel;
}
else {
/* Filtering with long filter */
compute_ltp_l(ptr_sig_cadr, ltpdel, phase, ptr_sig_pst0,
&num2_gltp, &den2_gltp, &sh_num2, &sh_den2);
if(select_ltp(num_gltp, den_gltp, sh_num, sh_den,
num2_gltp, den2_gltp, sh_num2, sh_den2) == 1) {
/* select short filter */
temp = sub(phase, 1);
L_temp = L_mult(temp, L_SUBFRP1);
L_temp = L_shr(L_temp, 1);
temp = extract_l(L_temp);
temp = add(temp, off_yup);
/* ptr_y_up = y_up + (phase-1) * L_SUBFRP1 + off_yup */
ptr_y_up = y_up + temp;
}
else {
/* select long filter */
num_gltp = num2_gltp;
den_gltp = den2_gltp;
sh_num = sh_num2;
sh_den = sh_den2;
ptr_y_up = ptr_sig_pst0;
}
/* rescale y_up */
for(i=0; i<L_SUBFR; i++) { /* nb_sh_sig may be >0, <0 or =0 */
ptr_y_up[i] = shl(ptr_y_up[i], nb_sh_sig);
}
}
temp = sub(sh_num,sh_den);
if(temp >= 0) den_gltp = shr(den_gltp, temp);
else {
num_gltp = shl(num_gltp, temp); /* >> (-temp) */
}
if(sub(num_gltp ,den_gltp)>=0) {
/* beta bounded to 1 */
if (gamma_harm == GAMMA_HARM) gain_plt = MIN_GPLT;
else {
num_gltp = mult_r(den_gltp, gamma_harm);
num_gltp = shr(num_gltp, 1);
den_gltp = shr(den_gltp, 1);
temp = add(den_gltp, num_gltp);
gain_plt = div_s(den_gltp, temp);
}
}
else {
/*gain_plt = den_gltp x 2**15 / (den_gltp + 0.5 num_gltp) */
/* shift 1 bit to avoid overflows in add */
if (gamma_harm == GAMMA_HARM) {
num_gltp = shr(num_gltp, 2);
}
else {
num_gltp = mult_r(num_gltp, gamma_harm);
num_gltp = shr(num_gltp, 1);
}
den_gltp = shr(den_gltp, 1);
temp = add(den_gltp, num_gltp);
gain_plt = div_s(den_gltp, temp);
}
/** filtering by H0(z) = harmonic filter **/
filt_plt(ptr_sig_in, ptr_y_up, ptr_sig_pst0, gain_plt);
}
return;
}
Word16 vad2 (Word16 * farray_ptr, vadState2 * st)
{
/*
* The channel table is defined below. In this table, the
* lower and higher frequency coefficients for each of the 16
* channels are specified. The table excludes the coefficients
* with numbers 0 (DC), 1, and 64 (Foldover frequency).
*/
const static Word16 ch_tbl[NUM_CHAN][2] =
{
{2, 3},
{4, 5},
{6, 7},
{8, 9},
{10, 11},
{12, 13},
{14, 16},
{17, 19},
{20, 22},
{23, 26},
{27, 30},
{31, 35},
{36, 41},
{42, 48},
{49, 55},
{56, 63}
};
/* channel energy scaling table - allows efficient division by number
* of DFT bins in the channel: 1/2, 1/3, 1/4, etc.
*/
const static Word16 ch_tbl_sh[NUM_CHAN] =
{
16384, 16384, 16384, 16384, 16384, 16384, 10923, 10923,
10923, 8192, 8192, 6554, 5461, 4681, 4681, 4096
};
/*
* The voice metric table is defined below. It is a non-
* linear table with a deadband near zero. It maps the SNR
* index (quantized SNR value) to a number that is a measure
* of voice quality.
*/
const static Word16 vm_tbl[90] =
{
2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2,
3, 3, 3, 3, 3, 4, 4, 4, 5, 5, 5, 6, 6, 7, 7, 7,
8, 8, 9, 9, 10, 10, 11, 12, 12, 13, 13, 14, 15,
15, 16, 17, 17, 18, 19, 20, 20, 21, 22, 23, 24,
24, 25, 26, 27, 28, 28, 29, 30, 31, 32, 33, 34,
35, 36, 37, 37, 38, 39, 40, 41, 42, 43, 44, 45,
46, 47, 48, 49, 50, 50, 50, 50, 50, 50, 50, 50,
50, 50
};
/* hangover as a function of peak SNR (3 dB steps) */
const static Word16 hangover_table[20] =
{
30, 30, 30, 30, 30, 30, 28, 26, 24, 22, 20, 18, 16, 14, 12, 10, 8, 8, 8, 8
};
/* burst sensitivity as a function of peak SNR (3 dB steps) */
const static Word16 burstcount_table[20] =
{
8, 8, 8, 8, 8, 8, 8, 8, 7, 6, 5, 4, 4, 4, 4, 4, 4, 4, 4, 4
};
/* voice metric sensitivity as a function of peak SNR (3 dB steps) */
const static Word16 vm_threshold_table[20] =
{
34, 34, 34, 34, 34, 34, 34, 34, 34, 34, 34, 40, 51, 71, 100, 139, 191, 257, 337, 432
};
/* State tables that use 22,9 or 27,4 scaling for ch_enrg[] */
const static Word16 noise_floor_chan[2] = {NOISE_FLOOR_CHAN_0, NOISE_FLOOR_CHAN_1};
const static Word16 min_chan_enrg[2] = {MIN_CHAN_ENRG_0, MIN_CHAN_ENRG_1};
const static Word16 ine_noise[2] = {INE_NOISE_0, INE_NOISE_1};
const static Word16 fbits[2] = {FRACTIONAL_BITS_0, FRACTIONAL_BITS_1};
const static Word16 state_change_shift_r[2] = {STATE_1_TO_0_SHIFT_R, STATE_0_TO_1_SHIFT_R};
/* Energy scale table given 30,1 input scaling (also account for -6 dB shift on input) */
const static Word16 enrg_norm_shift[2] = {(FRACTIONAL_BITS_0-1+2), (FRACTIONAL_BITS_1-1+2)};
/* Automatic variables */
Word32 Lenrg; /* scaled as 30,1 */
Word32 Ltne; /* scaled as 22,9 */
Word32 Ltce; /* scaled as 22,9 or 27,4 */
Word16 tne_db; /* scaled as 7,8 */
Word16 tce_db; /* scaled as 7,8 */
Word16 input_buffer[FRM_LEN]; /* used for block normalising input data */
Word16 data_buffer[FFT_LEN]; /* used for in-place FFT */
Word16 ch_snr[NUM_CHAN]; /* scaled as 7,8 */
Word16 ch_snrq; /* scaled as 15,0 (in 0.375 dB steps) */
Word16 vm_sum; /* scaled as 15,0 */
Word16 ch_enrg_dev; /* scaled as 7,8 */
Word32 Lpeak; /* maximum channel energy */
Word16 p2a_flag; /* flag to indicate spectral peak-to-average ratio > 10 dB */
Word16 ch_enrg_db[NUM_CHAN]; /* scaled as 7,8 */
Word16 ch_noise_db; /* scaled as 7,8 */
Word16 alpha; /* scaled as 0,15 */
Word16 one_m_alpha; /* scaled as 0,15 */
Word16 update_flag; /* set to indicate a background noise estimate update */
Word16 i, j, j1, j2; /* Scratch variables */
Word16 hi1, lo1;
Word32 Ltmp, Ltmp1, Ltmp2;
Word16 tmp;
Word16 normb_shift; /* block norm shift count */
Word16 ivad; /* intermediate VAD decision (return value) */
Word16 tsnrq; /* total signal-to-noise ratio (quantized 3 dB steps) scaled as 15,0 */
Word16 xt; /* instantaneous frame SNR in dB, scaled as 7,8 */
Word16 state_change;
/* Increment frame counter */
st->Lframe_cnt = L_add(st->Lframe_cnt, 1);
/* Block normalize the input */
normb_shift = block_norm(farray_ptr, input_buffer, FRM_LEN, FFT_HEADROOM);
/* Pre-emphasize the input data and store in the data buffer with the appropriate offset */
for (i = 0; i < DELAY; i++)
{
data_buffer[i] = 0; move16();
}
st->pre_emp_mem = shr_r(st->pre_emp_mem, sub(st->last_normb_shift, normb_shift));
st->last_normb_shift = normb_shift; move16();
data_buffer[DELAY] = add(input_buffer[0], mult(PRE_EMP_FAC, st->pre_emp_mem)); move16();
for (i = DELAY + 1, j = 1; i < DELAY + FRM_LEN; i++, j++)
{
data_buffer[i] = add(input_buffer[j], mult(PRE_EMP_FAC, input_buffer[j-1])); move16();
}
st->pre_emp_mem = input_buffer[FRM_LEN-1]; move16();
for (i = DELAY + FRM_LEN; i < FFT_LEN; i++)
{
data_buffer[i] = 0; move16();
}
/* Perform FFT on the data buffer */
r_fft(data_buffer);
/* Use normb_shift factor to determine the scaling of the energy estimates */
state_change = 0; move16();
test();
if (st->shift_state == 0)
{ test();
if (sub(normb_shift, -FFT_HEADROOM+2) <= 0)
{
state_change = 1; move16();
st->shift_state = 1; move16();
}
}
else
{ test();
if (sub(normb_shift, -FFT_HEADROOM+5) >= 0)
{
state_change = 1; move16();
st->shift_state = 0; move16();
}
}
/* Scale channel energy estimate */ test();
if (state_change)
{
for (i = LO_CHAN; i <= HI_CHAN; i++)
{
st->Lch_enrg[i] = L_shr(st->Lch_enrg[i], state_change_shift_r[st->shift_state]); move32();
}
}
/* Estimate the energy in each channel */
test();
if (L_sub(st->Lframe_cnt, 1) == 0)
{
alpha = 32767; move16();
one_m_alpha = 0; move16();
}
else
{
alpha = CEE_SM_FAC; move16();
one_m_alpha = ONE_MINUS_CEE_SM_FAC; move16();
}
for (i = LO_CHAN; i <= HI_CHAN; i++)
{
Lenrg = 0; move16();
j1 = ch_tbl[i][0]; move16();
j2 = ch_tbl[i][1]; move16();
for (j = j1; j <= j2; j++)
{
Lenrg = L_mac(Lenrg, data_buffer[2 * j], data_buffer[2 * j]);
Lenrg = L_mac(Lenrg, data_buffer[2 * j + 1], data_buffer[2 * j + 1]);
}
/* Denorm energy & scale 30,1 according to the state */
Lenrg = L_shr_r(Lenrg, sub(shl(normb_shift, 1), enrg_norm_shift[st->shift_state]));
/* integrate over time: e[i] = (1-alpha)*e[i] + alpha*enrg/num_bins_in_chan */
tmp = mult(alpha, ch_tbl_sh[i]);
L_Extract (Lenrg, &hi1, &lo1);
Ltmp = Mpy_32_16(hi1, lo1, tmp);
L_Extract (st->Lch_enrg[i], &hi1, &lo1);
st->Lch_enrg[i] = L_add(Ltmp, Mpy_32_16(hi1, lo1, one_m_alpha)); move32();
test();
if (L_sub(st->Lch_enrg[i], min_chan_enrg[st->shift_state]) < 0)
{
st->Lch_enrg[i] = min_chan_enrg[st->shift_state]; move32();
}
}
/* Compute the total channel energy estimate (Ltce) */
Ltce = 0; move16();
for (i = LO_CHAN; i <= HI_CHAN; i++)
{
Ltce = L_add(Ltce, st->Lch_enrg[i]);
}
/* Calculate spectral peak-to-average ratio, set flag if p2a > 10 dB */
Lpeak = 0; move32();
for (i = LO_CHAN+2; i <= HI_CHAN; i++) /* Sine waves not valid for low frequencies */
{ test();
if (L_sub(st->Lch_enrg [i], Lpeak) > 0)
{
Lpeak = st->Lch_enrg [i]; move32();
}
}
/* Set p2a_flag if peak (dB) > average channel energy (dB) + 10 dB */
/* Lpeak > Ltce/num_channels * 10^(10/10) */
/* Lpeak > (10/16)*Ltce */
L_Extract (Ltce, &hi1, &lo1);
Ltmp = Mpy_32_16(hi1, lo1, 20480);
test();
if (L_sub(Lpeak, Ltmp) > 0)
{
p2a_flag = TRUE; move16();
}
else
{
p2a_flag = FALSE; move16();
}
/* Initialize channel noise estimate to either the channel energy or fixed level */
/* Scale the energy appropriately to yield state 0 (22,9) scaling for noise */
test();
if (L_sub(st->Lframe_cnt, 4) <= 0)
{ test();
if (p2a_flag == TRUE)
{
for (i = LO_CHAN; i <= HI_CHAN; i++)
{
st->Lch_noise[i] = INE_NOISE_0; move32();
}
}
else
{
for (i = LO_CHAN; i <= HI_CHAN; i++)
{ test();
if (L_sub(st->Lch_enrg[i], ine_noise[st->shift_state]) < 0)
{
st->Lch_noise[i] = INE_NOISE_0; move32();
}
else
{ test();
if (st->shift_state == 1)
{
st->Lch_noise[i] = L_shr(st->Lch_enrg[i], state_change_shift_r[0]);
move32();
}
else
{
st->Lch_noise[i] = st->Lch_enrg[i]; move32();
}
}
}
}
}
/* Compute the channel energy (in dB), the channel SNRs, and the sum of voice metrics */
vm_sum = 0; move16();
for (i = LO_CHAN; i <= HI_CHAN; i++)
{
ch_enrg_db[i] = fn10Log10(st->Lch_enrg[i], fbits[st->shift_state]); move16();
ch_noise_db = fn10Log10(st->Lch_noise[i], FRACTIONAL_BITS_0);
ch_snr[i] = sub(ch_enrg_db[i], ch_noise_db); move16();
/* quantize channel SNR in 3/8 dB steps (scaled 7,8 => 15,0) */
/* ch_snr = round((snr/(3/8))>>8) */
/* = round(((0.6667*snr)<<2)>>8) */
/* = round((0.6667*snr)>>6) */
ch_snrq = shr_r(mult(21845, ch_snr[i]), 6);
/* Accumulate the sum of voice metrics */ test();
if (sub(ch_snrq, 89) < 0)
{ test();
if (ch_snrq > 0)
{
j = ch_snrq; move16();
}
else
{
j = 0; move16();
}
}
else
{
j = 89; move16();
}
vm_sum = add(vm_sum, vm_tbl[j]);
}
/* Initialize NOMINAL peak voice energy and average noise energy, calculate instantaneous SNR */
test(),test(),logic16();
if (L_sub(st->Lframe_cnt, 4) <= 0 || st->fupdate_flag == TRUE)
{
/* tce_db = (96 - 22 - 10*log10(64) (due to FFT)) scaled as 7,8 */
tce_db = 14320; move16();
st->negSNRvar = 0; move16();
st->negSNRbias = 0; move16();
/* Compute the total noise estimate (Ltne) */
Ltne = 0; move32();
for (i = LO_CHAN; i <= HI_CHAN; i++)
{
Ltne = L_add(Ltne, st->Lch_noise[i]);
}
/* Get total noise in dB */
tne_db = fn10Log10(Ltne, FRACTIONAL_BITS_0);
/* Initialise instantaneous and long-term peak signal-to-noise ratios */
xt = sub(tce_db, tne_db);
st->tsnr = xt; move16();
}
else
{
/* Calculate instantaneous frame signal-to-noise ratio */
/* xt = 10*log10( sum(2.^(ch_snr*0.1*log2(10)))/length(ch_snr) ) */
Ltmp1 = 0; move32();
for (i=LO_CHAN; i<=HI_CHAN; i++) {
/* Ltmp2 = ch_snr[i] * 0.1 * log2(10); (ch_snr scaled as 7,8) */
Ltmp2 = L_shr(L_mult(ch_snr[i], 10885), 8);
L_Extract(Ltmp2, &hi1, &lo1);
hi1 = add(hi1, 3); /* 2^3 to compensate for negative SNR */
Ltmp1 = L_add(Ltmp1, Pow2(hi1, lo1));
}
xt = fn10Log10(Ltmp1, 4+3); /* average by 16, inverse compensation 2^3 */
/* Estimate long-term "peak" SNR */ test(),test();
if (sub(xt, st->tsnr) > 0)
{
/* tsnr = 0.9*tsnr + 0.1*xt; */
st->tsnr = round(L_add(L_mult(29491, st->tsnr), L_mult(3277, xt)));
}
/* else if (xt > 0.625*tsnr) */
else if (sub(xt, mult(20480, st->tsnr)) > 0)
{
/* tsnr = 0.998*tsnr + 0.002*xt; */
st->tsnr = round(L_add(L_mult(32702, st->tsnr), L_mult(66, xt)));
}
}
/* Quantize the long-term SNR in 3 dB steps, limit to 0 <= tsnrq <= 19 */
tsnrq = shr(mult(st->tsnr, 10923), 8);
/* tsnrq = min(19, max(0, tsnrq)); */ test(),test();
if (sub(tsnrq, 19) > 0)
{
tsnrq = 19; move16();
}
else if (tsnrq < 0)
{
tsnrq = 0; move16();
}
/* Calculate the negative SNR sensitivity bias */
test();
if (xt < 0)
{
/* negSNRvar = 0.99*negSNRvar + 0.01*xt*xt; */
/* xt scaled as 7,8 => xt*xt scaled as 14,17, shift to 7,8 and round */
tmp = round(L_shl(L_mult(xt, xt), 7));
st->negSNRvar = round(L_add(L_mult(32440, st->negSNRvar), L_mult(328, tmp)));
/* if (negSNRvar > 4.0) negSNRvar = 4.0; */ test();
if (sub(st->negSNRvar, 1024) > 0)
{
st->negSNRvar = 1024; move16();
}
/* negSNRbias = max(12.0*(negSNRvar - 0.65), 0.0); */
tmp = mult_r(shl(sub(st->negSNRvar, 166), 4), 24576); test();
if (tmp < 0)
{
st->negSNRbias = 0; move16();
}
else
{
st->negSNRbias = shr(tmp, 8);
}
}
/* Determine VAD as a function of the voice metric sum and quantized SNR */
tmp = add(vm_threshold_table[tsnrq], st->negSNRbias); test();
if (sub(vm_sum, tmp) > 0)
{
ivad = 1; move16();
st->burstcount = add(st->burstcount, 1); test();
if (sub(st->burstcount, burstcount_table[tsnrq]) > 0)
{
st->hangover = hangover_table[tsnrq]; move16();
}
}
else
{
st->burstcount = 0; move16();
st->hangover = sub(st->hangover, 1); test();
if (st->hangover <= 0)
{
ivad = 0; move16();
st->hangover = 0; move16();
}
else
{
ivad = 1; move16();
}
}
/* Calculate log spectral deviation */
ch_enrg_dev = 0; move16();
test();
if (L_sub(st->Lframe_cnt, 1) == 0)
{
for (i = LO_CHAN; i <= HI_CHAN; i++)
{
st->ch_enrg_long_db[i] = ch_enrg_db[i]; move16();
}
}
else
{
for (i = LO_CHAN; i <= HI_CHAN; i++)
{
tmp = abs_s(sub(st->ch_enrg_long_db[i], ch_enrg_db[i]));
ch_enrg_dev = add(ch_enrg_dev, tmp);
}
}
/*
* Calculate long term integration constant as a function of instantaneous SNR
* (i.e., high SNR (tsnr dB) -> slower integration (alpha = HIGH_ALPHA),
* low SNR (0 dB) -> faster integration (alpha = LOW_ALPHA)
*/
/* alpha = HIGH_ALPHA - ALPHA_RANGE * (tsnr - xt) / tsnr, low <= alpha <= high */
tmp = sub(st->tsnr, xt); test(),logic16(),test(),test();
if (tmp <= 0 || st->tsnr <= 0)
{
alpha = HIGH_ALPHA; move16();
one_m_alpha = 32768L-HIGH_ALPHA; move16();
}
else if (sub(tmp, st->tsnr) > 0)
{
alpha = LOW_ALPHA; move16();
one_m_alpha = 32768L-LOW_ALPHA; move16();
}
else
{
tmp = div_s(tmp, st->tsnr);
alpha = sub(HIGH_ALPHA, mult(ALPHA_RANGE, tmp));
one_m_alpha = sub(32767, alpha);
}
/* Calc long term log spectral energy */
for (i = LO_CHAN; i <= HI_CHAN; i++)
{
Ltmp1 = L_mult(one_m_alpha, ch_enrg_db[i]);
Ltmp2 = L_mult(alpha, st->ch_enrg_long_db[i]);
st->ch_enrg_long_db[i] = round(L_add(Ltmp1, Ltmp2));
}
/* Set or clear the noise update flags */
update_flag = FALSE; move16();
st->fupdate_flag = FALSE; move16();
test(),test();
if (sub(vm_sum, UPDATE_THLD) <= 0)
{ test();
if (st->burstcount == 0)
{
update_flag = TRUE; move16();
st->update_cnt = 0; move16();
}
}
else if (L_sub(Ltce, noise_floor_chan[st->shift_state]) > 0)
{ test();
if (sub(ch_enrg_dev, DEV_THLD) < 0)
{ test();
if (p2a_flag == FALSE)
{ test();
if (st->LTP_flag == FALSE)
{
st->update_cnt = add(st->update_cnt, 1); test();
if (sub(st->update_cnt, UPDATE_CNT_THLD) >= 0)
{
update_flag = TRUE; move16();
st->fupdate_flag = TRUE; move16();
}
}
}
}
}
test();
if (sub(st->update_cnt, st->last_update_cnt) == 0)
{
st->hyster_cnt = add(st->hyster_cnt, 1);
}
else
{
st->hyster_cnt = 0; move16();
}
st->last_update_cnt = st->update_cnt; move16();
test();
if (sub(st->hyster_cnt, HYSTER_CNT_THLD) > 0)
{
st->update_cnt = 0; move16();
}
/* Conditionally update the channel noise estimates */
test();
if (update_flag == TRUE)
{
/* Check shift state */ test();
if (st->shift_state == 1)
{
/* get factor to shift ch_enrg[] from state 1 to 0 (noise always state 0) */
tmp = state_change_shift_r[0]; move16();
}
else
{
/* No shift if already state 0 */
tmp = 0; move16();
}
/* Update noise energy estimate */
for (i = LO_CHAN; i <= HI_CHAN; i++)
{ test();
/* integrate over time: en[i] = (1-alpha)*en[i] + alpha*e[n] */
/* (extract with shift compensation for state 1) */
L_Extract (L_shr(st->Lch_enrg[i], tmp), &hi1, &lo1);
Ltmp = Mpy_32_16(hi1, lo1, CNE_SM_FAC);
L_Extract (st->Lch_noise[i], &hi1, &lo1);
st->Lch_noise[i] = L_add(Ltmp, Mpy_32_16(hi1, lo1, ONE_MINUS_CNE_SM_FAC)); move32();
/* Limit low level noise */ test();
if (L_sub(st->Lch_noise[i], MIN_NOISE_ENRG_0) < 0)
{
st->Lch_noise[i] = MIN_NOISE_ENRG_0; move32();
}
}
}
return(ivad);
} /* end of vad2 () */
/*-----------------------------------------------------------*
* procedure Cod_cng: *
* ~~~~~~~~ *
* computes DTX decision *
* encodes SID frames *
* computes CNG excitation for encoder update *
*-----------------------------------------------------------*/
void Cod_cng(
DtxStatus *handle,
Word16 *exc, /* (i/o) : excitation array */
Word16 pastVad, /* (i) : previous VAD decision */
Word16 *lsp_old_q, /* (i/o) : previous quantized lsp */
Word16 *Aq, /* (o) : set of interpolated LPC coefficients */
Word16 *ana, /* (o) : coded SID parameters */
Word16 freq_prev[MA_NP][M],
/* (i/o) : previous LPS for quantization */
Word16 *seed /* (i/o) : random generator seed */
)
{
Word16 i;
Word16 curAcf[MP1];
Word16 bid[M], zero[MP1];
Word16 curCoeff[MP1];
Word16 lsp_new[M];
Word16 *lpcCoeff;
Word16 cur_igain;
Word16 energyq, temp;
/* Update Ener and sh_ener */
for(i = NB_GAIN-1; i>=1; i--) {
handle->ener[i] = handle->ener[i-1];
handle->sh_ener[i] = handle->sh_ener[i-1];
}
/* Compute current Acfs */
Calc_sum_acf(handle->Acf, handle->sh_Acf, curAcf, &(handle->sh_ener[0]), NB_CURACF);
/* Compute LPC coefficients and residual energy */
if(curAcf[0] == 0) {
handle->ener[0] = 0; /* should not happen */
}
else {
Set_zero(zero, MP1);
Levinson(handle, curAcf, zero, curCoeff, bid, &(handle->ener[0]));
}
/* if first frame of silence => SID frame */
if(pastVad != 0) {
ana[0] = 2;
handle->count_fr0 = 0;
handle->nb_ener = 1;
Qua_Sidgain(handle->ener, handle->sh_ener, handle->nb_ener, &energyq, &cur_igain);
}
else {
handle->nb_ener = add(handle->nb_ener, 1);
if(sub(handle->nb_ener, NB_GAIN) > 0) handle->nb_ener = NB_GAIN;
Qua_Sidgain(handle->ener, handle->sh_ener, handle->nb_ener, &energyq, &cur_igain);
/* Compute stationarity of current filter */
/* versus reference filter */
if(Cmp_filt(handle->RCoeff, handle->sh_RCoeff, curAcf, handle->ener[0], FRAC_THRESH1) != 0) {
handle->flag_chang = 1;
}
/* compare energy difference between current frame and last frame */
temp = abs_s(sub(handle->prev_energy, energyq));
temp = sub(temp, 2);
if (temp > 0) handle->flag_chang = 1;
handle->count_fr0 = add(handle->count_fr0, 1);
if(sub(handle->count_fr0, FR_SID_MIN) < 0) {
ana[0] = 0; /* no transmission */
}
else {
if(handle->flag_chang != 0) {
ana[0] = 2; /* transmit SID frame */
}
else{
ana[0] = 0;
}
handle->count_fr0 = FR_SID_MIN; /* to avoid overflow */
}
}
if(sub(ana[0], 2) == 0) {
/* Reset frame count and change flag */
handle->count_fr0 = 0;
handle->flag_chang = 0;
/* Compute past average filter */
Calc_pastfilt(handle, handle->pastCoeff);
Calc_RCoeff(handle->pastCoeff, handle->RCoeff, &(handle->sh_RCoeff));
/* Compute stationarity of current filter */
/* versus past average filter */
/* if stationary */
/* transmit average filter => new ref. filter */
if(Cmp_filt(handle->RCoeff, handle->sh_RCoeff, curAcf, handle->ener[0], FRAC_THRESH2) == 0) {
lpcCoeff = handle->pastCoeff;
}
/* else */
/* transmit current filter => new ref. filter */
else {
lpcCoeff = curCoeff;
Calc_RCoeff(curCoeff, handle->RCoeff, &(handle->sh_RCoeff));
}
/* Compute SID frame codes */
Az_lsp(lpcCoeff, lsp_new, lsp_old_q); /* From A(z) to lsp */
/* LSP quantization */
lsfq_noise(lsp_new, handle->lspSid_q, freq_prev, &ana[1]);
handle->prev_energy = energyq;
ana[4] = cur_igain;
handle->sid_gain = tab_Sidgain[cur_igain];
} /* end of SID frame case */
/* Compute new excitation */
if(pastVad != 0) {
handle->cur_gain = handle->sid_gain;
}
else {
handle->cur_gain = mult_r(handle->cur_gain, A_GAIN0);
handle->cur_gain = add(handle->cur_gain, mult_r(handle->sid_gain, A_GAIN1));
}
Calc_exc_rand(handle->L_exc_err, handle->cur_gain, exc, seed, FLAG_COD);
Int_qlpc(lsp_old_q, handle->lspSid_q, Aq);
for(i=0; i<M; i++) {
lsp_old_q[i] = handle->lspSid_q[i];
}
/* Update sumAcf if fr_cur = 0 */
if(handle->fr_cur == 0) {
Update_sumAcf(handle);
}
return;
}
/*----------------------------------------------------------------------------
* scale_st - control of the subframe gain
* gain[n] = AGC_FAC * gain[n-1] + (1 - AGC_FAC) g_in/g_out
*----------------------------------------------------------------------------
*/
static void scale_st(
Word16 *sig_in, /* input : postfilter input signal */
Word16 *sig_out, /* in/out: postfilter output signal */
Word16 *gain_prec /* in/out: last value of gain for subframe */
)
{
int i;
Word16 scal_in, scal_out;
Word32 L_acc, L_temp;
Word16 s_g_in, s_g_out, temp, sh_g0, g0;
Word16 gain;
/* compute input gain */
L_acc = 0L;
for(i=0; i<L_SUBFR; i++) {
L_temp = L_abs(L_deposit_l(sig_in[i]));
L_acc = L_add(L_acc, L_temp);
}
if(L_acc == 0L) {
g0 = 0;
}
else {
scal_in = norm_l(L_acc);
L_acc = L_shl(L_acc, scal_in);
s_g_in = extract_h(L_acc); /* normalized */
/* Compute output gain */
L_acc = 0L;
for(i=0; i<L_SUBFR; i++) {
L_temp = L_abs(L_deposit_l(sig_out[i]));
L_acc = L_add(L_acc, L_temp);
}
if(L_acc == 0L) {
*gain_prec = 0;
return;
}
scal_out = norm_l(L_acc);
L_acc = L_shl(L_acc, scal_out);
s_g_out = extract_h(L_acc); /* normalized */
sh_g0 = add(scal_in, 1);
sh_g0 = sub(sh_g0, scal_out); /* scal_in - scal_out + 1 */
if(sub(s_g_in ,s_g_out)<0) {
g0 = div_s(s_g_in, s_g_out); /* s_g_in/s_g_out in Q15 */
}
else {
temp = sub(s_g_in, s_g_out); /* sufficient since normalized */
g0 = shr(div_s(temp, s_g_out), 1);
g0 = add(g0, (Word16)0x4000);/* s_g_in/s_g_out in Q14 */
sh_g0 = sub(sh_g0, 1);
}
/* L_gain_in/L_gain_out in Q14 */
/* overflows if L_gain_in > 2 * L_gain_out */
g0 = shr(g0, sh_g0); /* sh_g0 may be >0, <0, or =0 */
g0 = mult_r(g0, AGC_FAC1); /* L_gain_in/L_gain_out * AGC_FAC1 */
}
/* gain(n) = AGC_FAC gain(n-1) + AGC_FAC1 gain_in/gain_out */
/* sig_out(n) = gain(n) sig_out(n) */
gain = *gain_prec;
for(i=0; i<L_SUBFR; i++) {
temp = mult_r(AGC_FAC, gain);
gain = add(temp, g0); /* in Q14 */
L_temp = L_mult(gain, sig_out[i]);
L_temp = L_shl(L_temp, 1);
sig_out[i] = round(L_temp);
}
*gain_prec = gain;
return;
}
/*
**
** Function: AtoLsp()
**
** Description: Transforms 10 LPC coefficients to the 10
** corresponding LSP frequencies for a subframe.
** This transformation is done once per frame,
** for subframe 3 only. The transform algorithm
** generates sum and difference polynomials from
** the LPC coefficients. It then evaluates the
** sum and difference polynomials at uniform
** intervals of pi/256 along the unit circle.
** Intervals where a sign change occurs are
** interpolated to find the zeros of the
** polynomials, which are the LSP frequencies.
**
** Links to text: Section 2.5
**
** Arguments:
**
** Word16 *LspVect Empty Buffer
** Word16 Lpc[] Unquantized LPC coefficients (10 words)
** Word16 PrevLsp[] LSP frequencies from the previous frame (10 words)
**
** Outputs:
**
** Word16 LspVect[] LSP frequencies for the current frame (10 words)
**
** Return value: None
**
**/
void AtoLsp( Word16 *LspVect, Word16 *Lpc, Word16 *PrevLsp )
{
int i,j,k ;
Word32 Lpq[LpcOrder+2] ;
Word16 Spq[LpcOrder+2] ;
Word16 Exp ;
Word16 LspCnt ;
Word32 PrevVal,CurrVal ;
Word32 Acc0,Acc1 ;
/*
* Perform a bandwidth expansion on the LPC coefficients. This
* scales the poles of the LPC synthesis filter by a factor of
* 0.994.
*/
for ( i = 0 ; i < LpcOrder ; i ++ )
LspVect[i] = mult_r( Lpc[i], BandExpTable[i] ) ;
/*
* Compute the sum and difference polynomials with the roots at z =
* -1 (sum) or z = +1 (difference) removed. Let these polynomials
* be P(z) and Q(z) respectively, and let their coefficients be
* {p_i} amd {q_i}. The coefficients are stored in the array Lpq[]
* as follows: p_0, q_0, p_1, q_1, ..., p_5, q_5. There is no need
* to store the other coefficients because of symmetry.
*/
/*
* Set p_0 = q_0 = 1. The LPC coefficients are already scaled by
* 1/4. P(z) and Q(z) are scaled by an additional scaling factor of
* 1/16, for an overall factor of 1/64 = 0x02000000L.
*/
Lpq[0] = Lpq[1] = (Word32) 0x02000000L ;
/*
* This loop computes the coefficients of P(z) and Q(z). The long
* division (to remove the real zeros) is done recursively.
*/
for ( i = 0 ; i < LpcOrder/2 ; i ++ ) {
/* P(z) */
Acc0 = L_negate( Lpq[2*i+0] ) ;
Acc1 = L_deposit_h( LspVect[i] ) ;
Acc1 = L_shr( Acc1, (Word16) 4 ) ;
Acc0 = L_sub( Acc0, Acc1 ) ;
Acc1 = L_deposit_h( LspVect[LpcOrder-1-i] ) ;
Acc1 = L_shr( Acc1, (Word16) 4 ) ;
Acc0 = L_sub( Acc0, Acc1 ) ;
Lpq[2*i+2] = Acc0 ;
/* Q(z) */
Acc0 = Lpq[2*i+1] ;
Acc1 = L_deposit_h( LspVect[i] ) ;
Acc1 = L_shr( Acc1, (Word16) 4 ) ;
Acc0 = L_sub( Acc0, Acc1 ) ;
Acc1 = L_deposit_h( LspVect[LpcOrder-1-i] ) ;
Acc1 = L_shr( Acc1, (Word16) 4 ) ;
// Acc0 = L_add( Acc0, Acc1 ) ;
L_ADD(Acc0, Acc1, Acc0);
Lpq[2*i+3] = Acc0 ;
}
/*
* Divide p_5 and q_5 by 2 for proper weighting during polynomial
* evaluation.
*/
Lpq[LpcOrder+0] = L_shr( Lpq[LpcOrder+0], (Word16) 1 ) ;
Lpq[LpcOrder+1] = L_shr( Lpq[LpcOrder+1], (Word16) 1 ) ;
/*
* Normalize the polynomial coefficients and convert to shorts
*/
/* Find the maximum */
Acc1 = L_abs( Lpq[0] ) ;
for ( i = 1 ; i < LpcOrder+2 ; i ++ ) {
Acc0 = L_abs( Lpq[i] ) ;
if ( Acc0 > Acc1 )
Acc1 = Acc0 ;
}
/* Compute the normalization factor */
Exp = norm_l( Acc1 ) ;
/* Normalize and convert to shorts */
for ( i = 0 ; i < LpcOrder+2 ; i ++ ) {
Acc0 = L_shl( Lpq[i], Exp ) ;
Spq[i] = round( Acc0 ) ;
}
/*
* Initialize the search loop
*/
/*
* The variable k is a flag that indicates which polynomial (sum or
* difference) the algorithm is currently evaluating. Start with
* the sum.
*/
k = 0 ;
/* Evaluate the sum polynomial at frequency zero */
PrevVal = (Word32) 0 ;
for ( j = 0 ; j <= LpcOrder/2 ; j ++ )
PrevVal = L_mac( PrevVal, Spq[2*j], CosineTable[0] ) ;
/*
* Search loop. Evaluate P(z) and Q(z) at uniform intervals of
* pi/256 along the unit circle. Check for zero crossings. The
* zeros of P(w) and Q(w) alternate, so only one of them need by
* evaluated at any given step.
*/
LspCnt = (Word16) 0 ;
for ( i = 1 ; i < CosineTableSize/2 ; i ++ ) {
/* Evaluate the selected polynomial */
CurrVal = (Word32) 0 ;
for ( j = 0 ; j <= LpcOrder/2 ; j ++ )
CurrVal = L_mac( CurrVal, Spq[LpcOrder-2*j+k],
CosineTable[i*j%CosineTableSize] ) ;
/* Check for a sign change, indicating a zero crossing */
if ( (CurrVal ^ PrevVal) < (Word32) 0 ) {
/*
* Interpolate to find the bottom 7 bits of the
* zero-crossing frequency
*/
Acc0 = L_abs( CurrVal ) ;
Acc1 = L_abs( PrevVal ) ;
// Acc0 = L_add( Acc0, Acc1 ) ;
L_ADD(Acc0, Acc1, Acc0);
/* Normalize the sum */
Exp = norm_l( Acc0 ) ;
Acc0 = L_shl( Acc0, Exp ) ;
Acc1 = L_shl( Acc1, Exp ) ;
Acc1 = L_shr( Acc1, (Word16) 8 ) ;
LspVect[LspCnt] = div_l( Acc1, extract_h( Acc0 ) ) ;
/*
* Add the upper part of the zero-crossing frequency,
* i.e. bits 7-15
*/
Exp = shl( (Word16) (i-1), (Word16) 7 ) ;
LspVect[LspCnt] = add( LspVect[LspCnt], Exp ) ;
LspCnt ++ ;
/* Check if all zeros have been found */
if ( LspCnt == (Word16) LpcOrder )
break ;
/*
* Switch the pointer between sum and difference polynomials
*/
k ^= 1 ;
/*
* Evaluate the new polynomial at the current frequency
*/
CurrVal = (Word32) 0 ;
for ( j = 0 ; j <= LpcOrder/2 ; j ++ )
CurrVal = L_mac( CurrVal, Spq[LpcOrder-2*j+k],
CosineTable[i*j%CosineTableSize] ) ;
}
/* Update the previous value */
PrevVal = CurrVal ;
}
/*
* Check if all 10 zeros were found. If not, ignore the results of
* the search and use the previous frame's LSP frequencies instead.
*/
if ( LspCnt != (Word16) LpcOrder ) {
for ( j = 0 ; j < LpcOrder ; j ++ )
LspVect[j] = PrevLsp[j] ;
}
return ;
}
/*
**
** Function: Lsp_Inq()
**
** Description: Performs inverse vector quantization of the
** LSP frequencies. The LSP vector is divided
** into 3 sub-vectors, or bands, of dimension 3,
** 3, and 4. Each band is inverse quantized
** separately using a different VQ table. Each
** table has 256 entries, so each VQ index is 8
** bits. (Only the LSP vector for subframe 3 is
** quantized per frame.)
**
** Links to text: Sections 2.6, 3.2
**
** Arguments:
**
** Word16 *Lsp Empty buffer
** Word16 PrevLsp[] Quantized LSP frequencies from the previous frame
** (10 words)
** Word32 LspId Long word packed with the 3 VQ indices. Band 0
** corresponds to bits [23:16], band 1 corresponds
** to bits [15:8], and band 2 corresponds to bits
** [7:0].
** Word16 Crc Frame erasure indicator
**
** Outputs:
**
** Word16 Lsp[] Quantized LSP frequencies for current frame (10
** words)
**
** Return value: None
**
*/
void Lsp_Inq( Word16 *Lsp, Word16 *PrevLsp, Word32 LspId, Word16 Crc )
{
int i,j ;
Word16 *LspQntPnt ;
Word16 Scon ;
Word16 Lprd ;
Word16 Tmp ;
Flag Test ;
/*
* Check for frame erasure. If a frame erasure has occurred, the
* resulting VQ table entries are zero. In addition, a different
* fixed predictor and minimum frequency separation are used.
*/
if ( Crc == (Word16) 0 ) {
Scon = (Word16) 0x0100 ;
Lprd = LspPrd0 ;
}
else {
LspId = (Word32) 0 ;
Scon = (Word16) 0x0200 ;
Lprd = LspPrd1 ;
}
/*
* Inverse quantize the 10th-order LSP vector. Each band is done
* separately.
*/
for ( i = LspQntBands-1; i >= 0 ; i -- ) {
/*
* Get the VQ table entry corresponding to the transmitted index
*/
Tmp = (Word16) ( LspId & (Word32) 0x000000ff ) ;
LspId >>= 8 ;
LspQntPnt = BandQntTable[i] ;
for ( j = 0 ; j < BandInfoTable[i][1] ; j ++ )
Lsp[BandInfoTable[i][0] + j] =
LspQntPnt[Tmp*BandInfoTable[i][1] + j] ;
}
/*
* Subtract the DC component from the previous frame's quantized
* vector
*/
for ( j = 0 ; j < LpcOrder ; j ++ )
PrevLsp[j] = sub(PrevLsp[j], LspDcTable[j] ) ;
/*
* Generate the prediction vector using a fixed first-order
* predictor based on the previous frame's (DC-free) quantized
* vector
*/
for ( j = 0 ; j < LpcOrder ; j ++ ) {
Tmp = mult_r( PrevLsp[j], Lprd ) ;
Lsp[j] = add( Lsp[j], Tmp ) ;
}
/*
* Add the DC component back to the previous quantized vector,
* which is needed in later routines
*/
for ( j = 0 ; j < LpcOrder ; j ++ ) {
PrevLsp[j] = add( PrevLsp[j], LspDcTable[j] ) ;
Lsp[j] = add( Lsp[j], LspDcTable[j] ) ;
}
/*
* Perform a stability test on the quantized LSP frequencies. This
* test checks that the frequencies are ordered, with a minimum
* separation between each. If the test fails, the frequencies are
* iteratively modified until the test passes. If after 10
* iterations the test has not passed, the previous frame's
* quantized LSP vector is used.
*/
for ( i = 0 ; i < LpcOrder ; i ++ ) {
/* Check the first frequency */
if ( Lsp[0] < (Word16) 0x180 )
Lsp[0] = (Word16) 0x180 ;
/* Check the last frequency */
if ( Lsp[LpcOrder-1] > (Word16) 0x7e00 )
Lsp[LpcOrder-1] = (Word16) 0x7e00 ;
/* Perform the modification */
for ( j = 1 ; j < LpcOrder ; j ++ ) {
Tmp = add( Scon, Lsp[j-1] ) ;
Tmp = sub( Tmp, Lsp[j] ) ;
if ( Tmp > (Word16) 0 ) {
Tmp = shr( Tmp, (Word16) 1 ) ;
Lsp[j-1] = sub( Lsp[j-1], Tmp ) ;
Lsp[j] = add( Lsp[j], Tmp ) ;
}
}
Test = False ;
/*
* Test the modified frequencies for stability. Break out of
* the loop if the frequencies are stable.
*/
for ( j = 1 ; j < LpcOrder ; j ++ ) {
Tmp = add( Lsp[j-1], Scon ) ;
Tmp = sub( Tmp, (Word16) 4 ) ;
Tmp = sub( Tmp, Lsp[j] ) ;
if ( Tmp > (Word16) 0 )
Test = True ;
}
if ( Test == False )
break ;
}
/*
* Return the result of the stability check. True = not stable,
* False = stable.
*/
if ( Test == True) {
for ( j = 0 ; j < LpcOrder ; j ++ )
Lsp[j] = PrevLsp[j] ;
}
return;
}
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));
}
/*-----------------------------------------------------------*
* procedure Calc_exc_rand *
* ~~~~~~~~~~~~~ *
* Computes comfort noise excitation *
* for SID and not-transmitted frames *
*-----------------------------------------------------------*/
void WebRtcG729fix_Calc_exc_rand(
int32_t L_exc_err[],
int16_t cur_gain, /* (i) : target sample gain */
int16_t *exc, /* (i/o) : excitation array */
int16_t *seed, /* (i) : current Vad decision */
int flag_cod /* (i) : encoder/decoder flag */
)
{
int16_t i, j, i_subfr;
int16_t temp1, temp2;
int16_t pos[4];
int16_t sign[4];
int16_t t0, frac;
int16_t *cur_exc;
int16_t g, Gp, Gp2;
int16_t excg[L_SUBFR], excs[L_SUBFR];
int32_t L_acc, L_ener, L_k;
int16_t max, hi, lo, inter_exc;
int16_t sh;
int16_t x1, x2;
if (cur_gain == 0) {
WebRtcSpl_ZerosArrayW16(exc, L_FRAME);
Gp = 0;
t0 = WebRtcSpl_AddSatW16(L_SUBFR,1);
for (i_subfr = 0; i_subfr < L_FRAME; i_subfr += L_SUBFR) {
if (flag_cod != FLAG_DEC)
WebRtcG729fix_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 = WebRtcG729fix_Random(seed);
frac = WebRtcSpl_SubSatW16((temp1 & (int16_t)0x0003), 1);
if(frac == 2) frac = 0;
temp1 = shr(temp1, 2);
t0 = WebRtcSpl_AddSatW16((temp1 & (int16_t)0x003F), 40);
temp1 = shr(temp1, 6);
temp2 = temp1 & (int16_t)0x0007;
pos[0] = WebRtcSpl_AddSatW16(shl(temp2, 2), temp2); /* 5 * temp2 */
temp1 = shr(temp1, 3);
sign[0] = temp1 & (int16_t)0x0001;
temp1 = shr(temp1, 1);
temp2 = temp1 & (int16_t)0x0007;
temp2 = WebRtcSpl_AddSatW16(shl(temp2, 2), temp2);
pos[1] = WebRtcSpl_AddSatW16(temp2, 1); /* 5 * x + 1 */
temp1 = shr(temp1, 3);
sign[1] = temp1 & (int16_t)0x0001;
temp1 = WebRtcG729fix_Random(seed);
temp2 = temp1 & (int16_t)0x0007;
temp2 = WebRtcSpl_AddSatW16(shl(temp2, 2), temp2);
pos[2] = WebRtcSpl_AddSatW16(temp2, 2); /* 5 * x + 2 */
temp1 = shr(temp1, 3);
sign[2] = temp1 & (int16_t)0x0001;
temp1 = shr(temp1, 1);
temp2 = temp1 & (int16_t)0x000F;
pos[3] = WebRtcSpl_AddSatW16((temp2 & (int16_t)1), 3); /* j+3*/
temp2 = (shr(temp2, 1)) & (int16_t)7;
temp2 = WebRtcSpl_AddSatW16(shl(temp2, 2), temp2); /* 5i */
pos[3] = WebRtcSpl_AddSatW16(pos[3], temp2);
temp1 = shr(temp1, 4);
sign[3] = temp1 & (int16_t)0x0001;
Gp = WebRtcG729fix_Random(seed) & (int16_t)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 = WebRtcG729fix_Inv_sqrt(L_shr(L_acc,1)); /* Q30 */
WebRtcG729fix_L_Extract(L_acc, &hi, &lo);
/* cur_gain = cur_gainR << 3 */
temp1 = mult_r(cur_gain, FRAC1);
temp1 = WebRtcSpl_AddSatW16(cur_gain, temp1);
/* <=> alpha x cur_gainR x 2^2 x sqrt(L_SUBFR) */
L_acc = WebRtcG729fix_Mpy_32_16(hi, lo, temp1); /* fact << 17 */
sh = WebRtcSpl_NormW32(L_acc);
temp1 = extract_h(L_shl(L_acc, sh)); /* fact << (sh+1) */
sh = WebRtcSpl_SubSatW16(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 */
/****************************************/
WebRtcG729fix_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 = WebRtcSpl_AddSatW16(temp1, excg[i]); /* may overflow */
cur_exc[i] = temp1;
temp1 = abs_s(temp1);
if (temp1 > max)
max = temp1;
}
/* rescale cur_exc -> excs */
if (max == 0)
sh = 0;
else {
sh = WebRtcSpl_SubSatW16(3, WebRtcSpl_NormW16(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 = WebRtcSpl_SubSatW16(inter_exc, excs[j]);
}
else {
inter_exc = WebRtcSpl_AddSatW16(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 = WebRtcSpl_AddSatW16(1, shl(sh,1));
L_acc = L_shr(L_k, temp1); /* k x 2^(-2sh+1) */
/* Compute delta = b^2 - 4 c */
L_acc = WebRtcSpl_SubSatW32(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 = WebRtcSpl_AddSatW16(sh, 1);
/* inter_exc = b x 2^(-sh) */
/* L_acc = delta x 2^(-2sh+1) */
if (L_acc < 0) {
/* adaptive excitation = 0 */
WEBRTC_SPL_MEMCPY_W16(cur_exc, excg, 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 & (int16_t)0x4000) == 0) ? (int16_t)1 : (int16_t)2;
inter_exc = 0;
for(i=0; i<4; i++) {
temp1 = shr(excg[(int)pos[i]], sh);
if(sign[i] == 0) {
inter_exc = WebRtcSpl_SubSatW16(inter_exc, temp1);
}
else {
inter_exc = WebRtcSpl_AddSatW16(inter_exc, temp1);
}
} /* inter_exc = b >> sh */
WebRtcG729fix_L_Extract(L_k, &hi, &lo);
L_acc = WebRtcG729fix_Mpy_32_16(hi, lo, K0); /* k x (1- alpha^2) << 2 */
temp1 = WebRtcSpl_SubSatW16(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 = WebRtcSpl_SubSatW16(temp2, inter_exc);
x2 = negate(WebRtcSpl_AddSatW16(inter_exc, temp2)); /* x 2^(-sh+2) */
if(abs_s(x2) < abs_s(x1)) x1 = x2;
temp1 = WebRtcSpl_SubSatW16(2, sh);
g = shr_r(x1, temp1); /* shl if temp1 < 0 */
if (g >= 0) {
if (g > G_MAX)
g = G_MAX;
}
else {
if (WebRtcSpl_AddSatW16(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] = WebRtcSpl_AddSatW16(cur_exc[j], g);
}
else {
cur_exc[j] = WebRtcSpl_SubSatW16(cur_exc[j], g);
}
}
if (flag_cod != FLAG_DEC)
WebRtcG729fix_update_exc_err(L_exc_err, Gp, t0);
cur_exc += L_SUBFR;
} /* end of loop on subframes */
return;
}
Word32 fnExp2(Word32 L_Input)
{
/*_________________________________________________________________________
| |
| Local Static Variables |
|_________________________________________________________________________|
*/
static Word16 pswPCoefE[3] =
{ /* c2, c1, c0 */
0x15ef, 0x556f, 0x7fbd
};
/*_________________________________________________________________________
| |
| Automatic Variables |
|_________________________________________________________________________|
*/
Word16 swTemp1, swTemp2, swTemp3, swTemp4;
Word32 LwIn;
/*_________________________________________________________________________
| |
| Executable Code |
|_________________________________________________________________________|
*/
/* initialize */
/* ---------- */
swTemp3 = 0x0020;
/* determine normlization shift count */
/* ---------------------------------- */
swTemp1 = extract_h(L_Input);
LwIn = L_mult(swTemp1, swTemp3);
swTemp2 = extract_h(LwIn);
/* determine un-normalized shift count */
/* ----------------------------------- */
swTemp3 = -0x0001;
swTemp4 = sub(swTemp3, swTemp2);
/* normalize input */
/* --------------- */
LwIn = LwIn & 0xffff;
LwIn = L_add(LwIn, L_deposit_h(swTemp3));
LwIn = L_shr(LwIn, 1);
swTemp1 = extract_l(LwIn);
/* calculate x*x*c2 */
/* ---------------- */
swTemp2 = mult_r(swTemp1, swTemp1);
LwIn = L_mult(swTemp2, pswPCoefE[0]);
/* calculate x*x*c2 + x*c1 */
/* ----------------------- */
LwIn = L_mac(LwIn, swTemp1, pswPCoefE[1]);
/* calculate x*x*c2 + x*c1 + c0 */
/* --------------------------- */
LwIn = L_add(LwIn, L_deposit_h(pswPCoefE[2]));
/* un-normalize exponent if its requires it */
/* ---------------------------------------- */
if (swTemp4 > 0)
{
LwIn = L_shr(LwIn, swTemp4);
}
/* return result */
/* ------------- */
return (LwIn);
}
/*
**
** Function: Lsp_Qnt()
**
** Description: Vector quantizes the LSP frequencies. The LSP
** vector is divided into 3 sub-vectors, or
** bands, of dimension 3, 3, and 4. Each band is
** quantized separately using a different VQ
** table. Each table has 256 entries, so the
** quantization generates three indices of 8 bits
** each. (Only the LSP vector for subframe 3 is
** quantized per frame.)
**
** Links to text: Section 2.5
**
** Arguments:
**
** Word16 CurrLsp[] Unquantized LSP frequencies for the current frame (10 words)
** Word16 PrevLsp[] LSP frequencies from the previous frame (10 words)
**
** Outputs: Quantized LSP frequencies for the current frame (10 words)
**
** Return value:
**
** Word32 Long word packed with the 3 VQ indices. Band 0
** corresponds to bits [23:16], band 1 corresponds
** to bits [15:8], and band 2 corresponds to bits [7:0].
** (Bit 0 is the least significant.)
**
*/
Word32 Lsp_Qnt( Word16 *CurrLsp, Word16 *PrevLsp )
{
int i ;
Word16 Wvect[LpcOrder] ;
Word16 Tmp0,Tmp1 ;
Word16 Exp ;
/*
* Compute the VQ weighting vector. The weights assign greater
* precision to those frequencies that are closer together.
*/
/* Compute the end differences */
Wvect[0] = sub( CurrLsp[1], CurrLsp[0] ) ;
Wvect[LpcOrder-1] = sub( CurrLsp[LpcOrder-1], CurrLsp[LpcOrder-2] ) ;
/* Compute the rest of the differences */
for ( i = 1 ; i < LpcOrder-1 ; i ++ ) {
Tmp0 = sub( CurrLsp[i+1], CurrLsp[i] ) ;
Tmp1 = sub( CurrLsp[i], CurrLsp[i-1] ) ;
if ( Tmp0 > Tmp1 )
Wvect[i] = Tmp1 ;
else
Wvect[i] = Tmp0 ;
}
/* Invert the differences */
Tmp0 = (Word16) 0x0020 ;
for ( i = 0 ; i < LpcOrder ; i ++ ) {
if ( Wvect[i] > Tmp0 )
Wvect[i] = div_s( Tmp0, Wvect[i] ) ;
else
Wvect[i] = MAX_16 ;
}
/* Normalize the weight vector */
Tmp0 = (Word16) 0 ;
for ( i = 0 ; i < LpcOrder ; i ++ )
if ( Wvect[i] > Tmp0 )
Tmp0 = Wvect[i] ;
Exp = norm_s( Tmp0 ) ;
for ( i = 0 ; i < LpcOrder ; i ++ )
Wvect[i] = shl( Wvect[i], Exp ) ;
/*
* Compute the VQ target vector. This is the residual that remains
* after subtracting both the DC and predicted
* components.
*/
/*
* Subtract the DC component from both the current and previous LSP
* vectors.
*/
for ( i = 0 ; i < LpcOrder ; i ++ ) {
CurrLsp[i] = sub( CurrLsp[i], LspDcTable[i] ) ;
PrevLsp[i] = sub( PrevLsp[i], LspDcTable[i] ) ;
}
/*
* Generate the prediction vector and subtract it. Use a constant
* first-order predictor based on the previous (DC-free) LSP
* vector.
*/
for ( i = 0 ; i < LpcOrder ; i ++ ) {
Tmp0 = mult_r( PrevLsp[i], (Word16) LspPrd0 ) ;
CurrLsp[i] = sub( CurrLsp[i], Tmp0 ) ;
}
/*
* Add the DC component back to the previous LSP vector. This
* vector is needed in later routines.
*/
for ( i = 0 ; i < LpcOrder ; i ++ )
PrevLsp[i] = add( PrevLsp[i], LspDcTable[i] ) ;
/*
* Do the vector quantization for all three bands
*/
return Lsp_Svq( CurrLsp, Wvect ) ;
}
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 schur_recursion (
Word32 L_av1[],
Word16 vpar[]
)
{
Word16 acf[9], pp[9], kk[9], temp;
Word16 i, k, m, n;
/*** Schur recursion with 16-bit arithmetic ***/
if (L_av1[0] == 0)
{
for (i = 0; i < 8; i++)
{
vpar[i] = 0;
}
return;
}
temp = norm_l (L_av1[0]);
for (k = 0; k <= 8; k++)
{
if(temp<=0)
acf[k] = extract_h (L_shr (L_av1[k], -temp));
else
acf[k] = extract_h (L_shl (L_av1[k], temp));
}
/*** Initialize arrays pp[..] and kk[..] for the recursion: ***/
for (i = 1; i <= 7; i++)
{
kk[9 - i] = acf[i];
}
for (i = 0; i <= 8; i++)
{
pp[i] = acf[i];
}
/*** Compute Parcor coefficients: ***/
for (n = 0; n < 8; n++)
{
if ((pp[0] == 0) ||
(sub (pp[0], abs_s (pp[1])) < 0))
{
for (i = n; i < 8; i++)
{
vpar[i] = 0;
}
return;
}
vpar[n] = div_s (abs_s (pp[1]), pp[0]);
if (pp[1] > 0)
{
vpar[n] = negate (vpar[n]);
}
if (sub (n, 7) == 0)
{
return;
}
/*** Schur recursion: ***/
pp[0] = add (pp[0], mult_r (pp[1], vpar[n]));
for (m = 1; m <= 7 - n; m++)
{
pp[m] = add (pp[1 + m], mult_r (kk[9 - m], vpar[n]));
kk[9 - m] = add (kk[9 - m], mult_r (pp[1 + m], vpar[n]));
}
}
return;
}
