Word16 block_norm (Word16 * in, Word16 * out, Word16 length, Word16 headroom)
{
Word16 i, max, scnt, adata;
max = abs_s(in[0]);
for (i = 1; i < length; i++)
{
adata = abs_s(in[i]); test();
if (sub(adata, max) > 0)
{
max = adata; move16();
}
}
test();
if (max != 0)
{
scnt = sub(norm_s(max), headroom);
for (i = 0; i < length; i++)
{
out[i] = shl(in[i], scnt); move16();
}
}
else
{
scnt = sub(16, headroom);
for (i = 0; i < length; i++)
{
out[i] = 0; move16();
}
}
return (scnt);
}
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);
}
void Cor_h_X(
Word16 h[], /* (i) Q12 :Impulse response of filters */
Word16 X[], /* (i) :Target vector */
Word16 D[] /* (o) :Correlations between h[] and D[] */
/* Normalized to 13 bits */
)
{
Word16 i, j;
Word32 s, max, L_temp;
Word32 y32[L_SUBFR];
/* first keep the result on 32 bits and find absolute maximum */
max = 0;
for (i = 0; i < L_SUBFR; i++)
{
s = 0;
for (j = i; j < L_SUBFR; j++)
s = L_mac(s, X[j], h[j-i]);
y32[i] = s;
s = L_abs(s);
L_temp =L_sub(s,max);
if(L_temp>0L) {
max = s;
}
}
/* Find the number of right shifts to do on y32[] */
/* so that maximum is on 13 bits */
j = norm_l(max);
if( sub(j,16) > 0) {
j = 16;
}
j = sub(18, j);
if(j>=0)
{
for(i=0; i<L_SUBFR; i++) {
D[i] = extract_l( L_shr(y32[i], j) );
}
}
else
{
Word16 pj = abs_s(j);
for(i=0; i<L_SUBFR; i++) {
D[i] = extract_l( L_shr(y32[i], pj) );
}
}
return;
}
/*----------------------------------------------------------------------------
* calc_rc0_h - computes 1st parcor from composed filter impulse response
*----------------------------------------------------------------------------
*/
static void calc_rc0_he(
Word16 *h, /* input : impulse response of composed filter */
Word16 *rc0, /* output: 1st parcor */
Word16 long_h_st /*input : impulse response length */
)
{
Word16 acf0, acf1;
Word32 L_acc;
Word16 temp, sh_acf;
Word16 *ptrs;
Word16 i;
/* computation of the autocorrelation function acf */
L_acc = 0L;
for(i=0; i<long_h_st; i++) L_acc = L_mac(L_acc, h[i], h[i]);
sh_acf = norm_l(L_acc);
L_acc = L_shl(L_acc, sh_acf);
acf0 = extract_h(L_acc);
L_acc = 0L;
ptrs = h;
for(i=0; i<long_h_st-1; i++){
temp = *ptrs++;
L_acc = L_mac(L_acc, temp, *ptrs);
}
L_acc = L_shl(L_acc, sh_acf);
acf1 = extract_h(L_acc);
/* Compute 1st parcor */
/**********************/
if( sub(acf0, abs_s(acf1))<0) {
*rc0 = 0;
return;
}
*rc0 = div_s(abs_s(acf1), acf0);
if(acf1 > 0) {
*rc0 = negate(*rc0);
}
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;
}
/*----------------------------------------------------------------------------
* 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;
}
Word16 Pitch_ol_fast( /* output: open loop pitch lag */
Word16 signal[], /* input : signal used to compute the open loop pitch */
/* signal[-pit_max] to signal[-1] should be known */
Word16 pit_max, /* input : maximum pitch lag */
Word16 L_frame /* input : length of frame to compute pitch */
)
{
Word16 i, j;
Word16 max1, max2, max3;
Word16 max_h, max_l, ener_h, ener_l;
Word16 T1, T2, T3;
Word16 *p, *p1;
Word32 max, sum, L_temp;
/* Scaled signal */
Word16 scaled_signal[L_FRAME+PIT_MAX];
Word16 *scal_sig;
scal_sig = &scaled_signal[pit_max];
/*--------------------------------------------------------*
* Verification for risk of overflow. *
*--------------------------------------------------------*/
Overflow = 0;
sum = 0;
for(i= -pit_max; i< L_frame; i+=2)
sum = L_mac(sum, signal[i], signal[i]);
/*--------------------------------------------------------*
* Scaling of input signal. *
* *
* if Overflow -> scal_sig[i] = signal[i]>>3 *
* else if sum < 1^20 -> scal_sig[i] = signal[i]<<3 *
* else -> scal_sig[i] = signal[i] *
*--------------------------------------------------------*/
if(Overflow == 1)
{
for(i=-pit_max; i<L_frame; i++)
{
scal_sig[i] = shr(signal[i], 3);
}
}
else {
L_temp = L_sub(sum, (Word32)1048576L);
if ( L_temp < (Word32)0 ) /* if (sum < 2^20) */
{
for(i=-pit_max; i<L_frame; i++)
{
scal_sig[i] = shl(signal[i], 3);
}
}
else
{
for(i=-pit_max; i<L_frame; i++)
{
scal_sig[i] = signal[i];
}
}
}
/*--------------------------------------------------------------------*
* The pitch lag search is divided in three sections. *
* Each section cannot have a pitch multiple. *
* We find a maximum for each section. *
* We compare the maxima of each section by favoring small lag. *
* *
* First section: lag delay = 20 to 39 *
* Second section: lag delay = 40 to 79 *
* Third section: lag delay = 80 to 143 *
*--------------------------------------------------------------------*/
/* First section */
max = MIN_32;
T1 = 20; /* Only to remove warning from some compilers */
for (i = 20; i < 40; i++) {
p = scal_sig;
p1 = &scal_sig[-i];
sum = 0;
for (j=0; j<L_frame; j+=2, p+=2, p1+=2)
sum = L_mac(sum, *p, *p1);
L_temp = L_sub(sum, max);
if (L_temp > 0) { max = sum; T1 = i; }
}
/* compute energy of maximum */
sum = 1; /* to avoid division by zero */
p = &scal_sig[-T1];
for(i=0; i<L_frame; i+=2, p+=2)
sum = L_mac(sum, *p, *p);
/* max1 = max/sqrt(energy) */
/* This result will always be on 16 bits !! */
sum = Inv_sqrt(sum); /* 1/sqrt(energy), result in Q30 */
L_Extract(max, &max_h, &max_l);
L_Extract(sum, &ener_h, &ener_l);
sum = Mpy_32(max_h, max_l, ener_h, ener_l);
max1 = extract_l(sum);
/* Second section */
max = MIN_32;
T2 = 40; /* Only to remove warning from some compilers */
for (i = 40; i < 80; i++) {
p = scal_sig;
p1 = &scal_sig[-i];
sum = 0;
for (j=0; j<L_frame; j+=2, p+=2, p1+=2)
sum = L_mac(sum, *p, *p1);
L_temp = L_sub(sum, max);
if (L_temp > 0) { max = sum; T2 = i; }
}
/* compute energy of maximum */
sum = 1; /* to avoid division by zero */
p = &scal_sig[-T2];
for(i=0; i<L_frame; i+=2, p+=2)
sum = L_mac(sum, *p, *p);
/* max2 = max/sqrt(energy) */
/* This result will always be on 16 bits !! */
sum = Inv_sqrt(sum); /* 1/sqrt(energy), result in Q30 */
L_Extract(max, &max_h, &max_l);
L_Extract(sum, &ener_h, &ener_l);
sum = Mpy_32(max_h, max_l, ener_h, ener_l);
max2 = extract_l(sum);
/* Third section */
max = MIN_32;
T3 = 80; /* Only to remove warning from some compilers */
for (i = 80; i < 143; i+=2) {
p = scal_sig;
p1 = &scal_sig[-i];
sum = 0;
for (j=0; j<L_frame; j+=2, p+=2, p1+=2)
sum = L_mac(sum, *p, *p1);
L_temp = L_sub(sum, max);
if (L_temp > 0) { max = sum; T3 = i; }
}
/* Test around max3 */
i = T3;
p = scal_sig;
p1 = &scal_sig[-(i+1)];
sum = 0;
for (j=0; j<L_frame; j+=2, p+=2, p1+=2)
sum = L_mac(sum, *p, *p1);
L_temp = L_sub(sum, max);
if (L_temp > 0) { max = sum; T3 = i+(Word16)1; }
p = scal_sig;
p1 = &scal_sig[-(i-1)];
sum = 0;
for (j=0; j<L_frame; j+=2, p+=2, p1+=2)
sum = L_mac(sum, *p, *p1);
L_temp = L_sub(sum, max);
if (L_temp > 0) { max = sum; T3 = i-(Word16)1; }
/* compute energy of maximum */
sum = 1; /* to avoid division by zero */
p = &scal_sig[-T3];
for(i=0; i<L_frame; i+=2, p+=2)
sum = L_mac(sum, *p, *p);
/* max1 = max/sqrt(energy) */
/* This result will always be on 16 bits !! */
sum = Inv_sqrt(sum); /* 1/sqrt(energy), result in Q30 */
L_Extract(max, &max_h, &max_l);
L_Extract(sum, &ener_h, &ener_l);
sum = Mpy_32(max_h, max_l, ener_h, ener_l);
max3 = extract_l(sum);
/*-----------------------*
* Test for multiple. *
*-----------------------*/
/* if( abs(T2*2 - T3) < 5) */
/* max2 += max3 * 0.25; */
i = sub(shl(T2,1), T3);
j = sub(abs_s(i), 5);
if(j < 0)
max2 = add(max2, shr(max3, 2));
/* if( abs(T2*3 - T3) < 7) */
/* max2 += max3 * 0.25; */
i = add(i, T2);
j = sub(abs_s(i), 7);
if(j < 0)
max2 = add(max2, shr(max3, 2));
/* if( abs(T1*2 - T2) < 5) */
/* max1 += max2 * 0.20; */
i = sub(shl(T1,1), T2);
j = sub(abs_s(i), 5);
if(j < 0)
max1 = add(max1, mult(max2, 6554));
/* if( abs(T1*3 - T2) < 7) */
/* max1 += max2 * 0.20; */
i = add(i, T1);
j = sub(abs_s(i), 7);
if(j < 0)
max1 = add(max1, mult(max2, 6554));
/*--------------------------------------------------------------------*
* Compare the 3 sections maxima. *
*--------------------------------------------------------------------*/
if( sub(max1, max2) < 0 ) {max1 = max2; T1 = T2; }
if( sub(max1, max3) <0 ) {T1 = T3; }
return T1;
}
void modifyorig(short *residualm,
short *accshift,
short beta,
short *dpm,
short shiftrange,
short resolution,
short *TARGET,
short *residual,
short dp,
short sfend)
{
static short FirstTime = 1;
short best;
short tmp;
long ex0y[(RSHIFT * 2 + 2) * RRESOLUTION + 1]; /* Fraction sampled correlation function */
long ex0y2[RSHIFT * 2 + 2]; /* Integer sampled correlation function */
short Residual[2 * SubFrameSize + 2 * RSHIFT + 1];
static short a1[RRESOLUTION];
static short a2[RRESOLUTION];
static short a3[RRESOLUTION];
register short i, j, k, n;
short sfstart, length, shiftrangel, shiftranger;
long e01, e02;
short shft_fctr1, shft_fctr2, shft_fctr3;
long ltmp;
long laccshift;
long y;
/****** INITIALIZATION *****/
if (FirstTime)
{
FirstTime = 0;
/* Calculate interpolation coefficients */
a1[0] = 12288;
a1[1] = 8448;
a1[2] = 5120;
a1[3] = 2304;
a1[4] = 0;
a1[5] = -1792;
a1[6] = -3072;
a1[7] = -3840;
a2[0] = 24576;
a2[1] = 28160;
a2[2] = 30720;
a2[3] = 32256;
a2[4] = 32767;
a2[5] = 32256;
a2[6] = 30720;
a2[7] = 28160;
a3[0] = -4096;
a3[1] = -3840;
a3[2] = -3072;
a3[3] = -1792;
a3[4] = 0;
a3[5] = 2304;
a3[6] = 5120;
a3[7] = 8448;
}
/********************
* CORRELATION MATCH *
********************/
length = sub(sfend, dp);
sfstart = dp;
/*laccshift = L_shl(L_deposit_h(*accshift), 8); */
laccshift = L_deposit_h(*accshift); /* accshift scaled by 8 */
/* Perform before if
* * statement.
*/
if (shiftrange != 0)
{
/* Limit the search range to control accshift */
shiftrangel = shiftranger = shiftrange;
if (*accshift < 0)
shiftrangel = add(shiftrangel, 1);
if (*accshift > 0)
shiftranger = add(shiftranger, 1);
tmp = abs_s(*accshift);
/* For non-periodic signals */
if ((beta < 6554 && tmp > 15 * 256) || (beta < 9830 && tmp > 30 * 256))
{
if (*accshift < 0)
shiftranger = 1;
else
shiftrangel = 1;
}
if (add(shiftrangel, shr(*accshift, 8)) > 72)
{
shiftrangel = sub(72, shr(*accshift, 8));
fprintf(stderr, "mdfyorig:*** Buffer limit. shiftrangel is:%d\n", shiftrangel);
}
if (sub(shiftranger, shr(*accshift, 8)) > 72)
{
shiftranger = add(72, shr(*accshift, 8));
fprintf(stderr, "mdfyorig:*** Buffer limit. shiftranger is:%d\n", shiftranger);
}
/* Create a buffer of modify residual for match at low cut-off frequency */
tmp = add(length, shiftrangel);
tmp = add(tmp, shiftranger);
ltmp = L_deposit_h(add(*accshift, shl(shiftrangel, 8)));
for (i = 0; i <= tmp; i++)
{
/* POINTER ADDITION NOT CONVERTED -- NEED UNSIGNED ADDITION */
bl_intrp(Residual + i, residual + dp + i, ltmp, 16384, 3);
}
tmp = add(shiftrangel, shiftranger);
/* Search for all integer delays of residual */
for (n = 0; n <= tmp; n++)
{
ex0y2[n] = 0;
for (i = 0; i < length; i++)
{
ex0y2[n] = L_mac(ex0y2[n], Residual[n + i], TARGET[sfstart + i]);
}
ex0y2[n] = L_shr(ex0y2[n], 1);
}
/* Do quadratic interpolation of ex0y */
for (n = 1, k = 0; n < shiftrangel + shiftranger; n++)
{
for (j = 0; j < resolution; j++)
{
ex0y[k] = L_mpy_ls(ex0y2[n - 1], a1[j]);
ltmp = L_mpy_ls(ex0y2[n], a2[j]);
ex0y[k] = L_add(ex0y[k], ltmp);
ltmp = L_mpy_ls(ex0y2[n + 1], a3[j]);
ex0y[k] = L_add(ex0y[k], ltmp);
k++;
}
}
/* Find maximum with positive correlation */
y = 0;
best = sub(shl(shiftrangel, 3), 4);
for (n = 0; n < k; n++)
{
if (ex0y[n] > y)
{
y = ex0y[n];
best = n;
}
}
/* best value not very accurate since ex0y[] calculation not percise. To correct
* Residual[] should have more percision. This error does not seem to affect the
* final output using the test data.
*/
/* Calculate energy in selected shift index */
e01 = e02 = 0;
for (i = shiftrangel; i < length + shiftrangel; i++)
e01 = L_mac(e01, Residual[i], Residual[i]);
for (i = 0; i < length; i++)
e02 = L_mac(e02, TARGET[i + sfstart], TARGET[i + sfstart]);
if (e01 == 0 || e02 == 0)
y = 0;
else
{
shft_fctr1 = norm_l(y);
y = L_shl(y, shft_fctr1);
y = L_mpy_ll(y, y);
/* TO RECOVER Y VALUE:
* y = L_shl(y, 31-2*shft_fctr1) */
shft_fctr2 = norm_l(e01);
e01 = L_shl(e01, shft_fctr2);
shft_fctr3 = norm_l(e02);
e02 = L_shl(e02, shft_fctr3);
ltmp = L_mpy_ll(e01, e02);
ltmp = L_mpy_ls(ltmp, 16056);
ltmp = L_shl(ltmp, shl(shft_fctr1, 1) - add(add(2, shft_fctr2), shft_fctr3));
if (y > ltmp)
{
/*tmp = shl(shr(*accshift,8) + shiftrangel, 3) - (best + 4); */
tmp = *accshift + shl(shiftrangel, 8) - shl((best + 4), 5);
*accshift = tmp;
laccshift = L_deposit_h(tmp);
/**accshift = shift_r(tmp, -3);*/
/*
* if (laccshift == -75497472)
* laccshift = -88080384;
*/
}
}
}
for (k = 0; k < length; k++)
{
bl_intrp(residualm + dp + k, residual + dp + k, laccshift, BLFREQ, BLPRECISION);
}
*dpm = add(dp, length);
}
void track_pit( Word16 *T0, /* I/O Integer pitch delay */
Word16 *T0_frac, /* I/O Non integer correction */
Word16 *prev_pitch, /* I/O previous pitch delay */
Word16 *stat_pitch, /* I/O pitch stationnarity indicator */
Word16 *pitch_sta, /* I/O stationnary integer pitch delay */
Word16 *frac_sta /* I/O stationnary fractional pitch */
)
{
Word16 dist, dist_min, pitch_mult, temp;
Word16 j, flag_mult;
dist = sub(*T0, *prev_pitch);
if(dist < 0) {
flag_mult = 0;
dist = negate(dist);
}
else {
flag_mult = 1;
}
/* ------------------------ */
/* Test pitch stationnarity */
/* ------------------------ */
if (dist < 5) {
*stat_pitch += 1;
if (*stat_pitch > 7) *stat_pitch = 7 ;
*pitch_sta = *T0;
*frac_sta = *T0_frac;
}
else {
/* ------------------------------- */
/* Find multiples or sub-multiples */
/* ------------------------------- */
dist_min = dist;
if( flag_mult == 0) {
pitch_mult = add(*T0, *T0);
for (j=2; j<5; j++) {
dist = abs_s(sub(pitch_mult, *prev_pitch));
temp = sub(dist, dist_min);
if (temp <= 0) {
dist_min = dist;
}
pitch_mult = add(*T0, pitch_mult);
}
}
else {
pitch_mult = add(*prev_pitch, *prev_pitch);
for (j=2; j<5; j++) {
dist = abs_s(sub(pitch_mult, *T0));
temp = sub(dist, dist_min);
if (temp <= 0) {
dist_min = dist;
}
pitch_mult = add(*prev_pitch, pitch_mult);
}
}
if (dist_min < 5) { /* Multiple or sub-multiple detected */
if (*stat_pitch > 0) {
*T0 = *pitch_sta;
*T0_frac = *frac_sta;
}
*stat_pitch -= 1;
if (*stat_pitch < 0) *stat_pitch = 0 ;
}
else {
*stat_pitch = 0; /* No (sub-)multiple detected => Pitch transition */
*pitch_sta = *T0;
*frac_sta = *T0_frac;
}
}
*prev_pitch = *T0;
return;
}
/*-----------------------------------------------------------------*
* Functions vad *
* ~~~ *
* Input: *
* rc : reflection coefficient *
* lsf[] : unquantized lsf vector *
* r_h[] : upper 16-bits of the autocorrelation vector *
* r_l[] : lower 16-bits of the autocorrelation vector *
* exp_R0 : exponent of the autocorrelation vector *
* sigpp[] : preprocessed input signal *
* frm_count : frame counter *
* prev_marker : VAD decision of the last frame *
* pprev_marker : VAD decision of the frame before last frame *
* *
* Output: *
* *
* marker : VAD decision of the current frame *
* *
*-----------------------------------------------------------------*/
void vadg(
Word16 rc,
Word16 *lsf,
Word16 *r_h,
Word16 *r_l,
Word16 exp_R0,
Word16 *sigpp,
Word16 frm_count,
Word16 prev_marker,
Word16 pprev_marker,
Word16 *marker,
Word16 *ENERGY_db)
{
/* scalar */
Word32 acc0;
Word16 i, j, exp, frac;
Word16 ENERGY, ENERGY_low, SD, ZC, dSE, dSLE, dSZC;
Word16 COEF, C_COEF, COEFZC, C_COEFZC, COEFSD, C_COEFSD;
/* compute the frame energy */
acc0 = L_Comp(r_h[0], r_l[0]);
Log2(acc0, &exp, &frac);
acc0 = Mpy_32_16(exp, frac, 9864);
i = sub(exp_R0, 1);
i = sub(i, 1);
acc0 = L_mac(acc0, 9864, i);
acc0 = L_shl(acc0, 11);
ENERGY = extract_h(acc0);
ENERGY = sub(ENERGY, 4875);
*ENERGY_db = ENERGY;
/* compute the low band energy */
acc0 = 0;
for (i=1; i<=NP; i++)
acc0 = L_mac(acc0, r_h[i], lbf_corr[i]);
acc0 = L_shl(acc0, 1);
acc0 = L_mac(acc0, r_h[0], lbf_corr[0]);
Log2(acc0, &exp, &frac);
acc0 = Mpy_32_16(exp, frac, 9864);
i = sub(exp_R0, 1);
i = sub(i, 1);
acc0 = L_mac(acc0, 9864, i);
acc0 = L_shl(acc0, 11);
ENERGY_low = extract_h(acc0);
ENERGY_low = sub(ENERGY_low, 4875);
/* compute SD */
acc0 = 0;
for (i=0; i<M; i++){
j = sub(lsf[i], MeanLSF[i]);
acc0 = L_mac(acc0, j, j);
}
SD = extract_h(acc0); /* Q15 */
/* compute # zero crossing */
ZC = 0;
for (i=ZC_START+1; i<=ZC_END; i++)
if (mult(sigpp[i-1], sigpp[i]) < 0){
ZC = add(ZC, 410); /* Q15 */
}
/* Initialize and update Mins */
if(sub(frm_count, 129) < 0){
if (sub(ENERGY, Min) < 0){
Min = ENERGY;
Prev_Min = ENERGY;
}
if((frm_count & 0x0007) == 0){
i = sub(shr(frm_count,3),1);
Min_buffer[i] = Min;
Min = MAX_16;
}
}
if((frm_count & 0x0007) == 0){
Prev_Min = Min_buffer[0];
for (i=1; i<16; i++){
if (sub(Min_buffer[i], Prev_Min) < 0){
Prev_Min = Min_buffer[i];
}
}
}
if(sub(frm_count, 129) >= 0){
if(((frm_count & 0x0007) ^ (0x0001)) == 0){
Min = Prev_Min;
Next_Min = MAX_16;
}
if (sub(ENERGY, Min) < 0){
Min = ENERGY;
}
if (sub(ENERGY, Next_Min) < 0){
Next_Min = ENERGY;
}
if((frm_count & 0x0007) == 0){
for (i=0; i<15; i++)
Min_buffer[i] = Min_buffer[i+1];
Min_buffer[15] = Next_Min;
Prev_Min = Min_buffer[0];
for (i=1; i<16; i++)
if (sub(Min_buffer[i], Prev_Min) < 0){
Prev_Min = Min_buffer[i];
}
}
}
if (sub(frm_count, INIT_FRAME) <= 0){
if(sub(ENERGY, 3072) < 0){
*marker = NOISE;
less_count++;
}
else{
*marker = VOICE;
acc0 = L_deposit_h(MeanE);
acc0 = L_mac(acc0, ENERGY, 1024);
MeanE = extract_h(acc0);
acc0 = L_deposit_h(MeanSZC);
acc0 = L_mac(acc0, ZC, 1024);
MeanSZC = extract_h(acc0);
for (i=0; i<M; i++){
acc0 = L_deposit_h(MeanLSF[i]);
acc0 = L_mac(acc0, lsf[i], 1024);
MeanLSF[i] = extract_h(acc0);
}
}
}
if (sub(frm_count, INIT_FRAME) >= 0){
if (sub(frm_count, INIT_FRAME) == 0){
acc0 = L_mult(MeanE, factor_fx[less_count]);
acc0 = L_shl(acc0, shift_fx[less_count]);
MeanE = extract_h(acc0);
acc0 = L_mult(MeanSZC, factor_fx[less_count]);
acc0 = L_shl(acc0, shift_fx[less_count]);
MeanSZC = extract_h(acc0);
for (i=0; i<M; i++){
acc0 = L_mult(MeanLSF[i], factor_fx[less_count]);
acc0 = L_shl(acc0, shift_fx[less_count]);
MeanLSF[i] = extract_h(acc0);
}
MeanSE = sub(MeanE, 2048); /* Q11 */
MeanSLE = sub(MeanE, 2458); /* Q11 */
}
dSE = sub(MeanSE, ENERGY);
dSLE = sub(MeanSLE, ENERGY_low);
dSZC = sub(MeanSZC, ZC);
if(sub(ENERGY, 3072) < 0){
*marker = NOISE;
}
else {
*marker = MakeDec(dSLE, dSE, SD, dSZC);
}
v_flag = 0;
if((prev_marker==VOICE) && (*marker==NOISE) && (add(dSE,410)<0)
&& (sub(ENERGY, 3072)>0)){
*marker = VOICE;
v_flag = 1;
}
if(flag == 1){
if((pprev_marker == VOICE) &&
(prev_marker == VOICE) &&
(*marker == NOISE) &&
(sub(abs_s(sub(prev_energy,ENERGY)), 614) <= 0)){
count_ext++;
*marker = VOICE;
v_flag = 1;
if(sub(count_ext, 4) <= 0){
flag=1;
}
else{
count_ext=0;
flag=0;
}
}
}
else{
flag=1;
}
if(*marker == NOISE){
count_sil++;
}
if((*marker == VOICE) && (sub(count_sil, 10) > 0) &&
(sub(sub(ENERGY,prev_energy), 614) <= 0)){
*marker = NOISE;
count_sil=0;
}
if(*marker == VOICE){
count_sil=0;
}
if ((sub(sub(ENERGY, 614), MeanSE)<0) && (sub(frm_count, 128) > 0)
&& (!v_flag) && (sub(rc, 19661) < 0)){
*marker = NOISE;
}
if ((sub(sub(ENERGY,614),MeanSE) < 0) && (sub(rc, 24576) < 0)
&& (sub(SD, 83) < 0)){
count_update++;
if (sub(count_update, INIT_COUNT) < 0){
COEF = 24576;
C_COEF = 8192;
COEFZC = 26214;
C_COEFZC = 6554;
COEFSD = 19661;
C_COEFSD = 13017;
}
else
if (sub(count_update, INIT_COUNT+10) < 0){
COEF = 31130;
C_COEF = 1638;
COEFZC = 30147;
C_COEFZC = 2621;
COEFSD = 21299;
C_COEFSD = 11469;
}
else
if (sub(count_update, INIT_COUNT+20) < 0){
COEF = 31785;
C_COEF = 983;
COEFZC = 30802;
C_COEFZC = 1966;
COEFSD = 22938;
C_COEFSD = 9830;
}
else
if (sub(count_update, INIT_COUNT+30) < 0){
COEF = 32440;
C_COEF = 328;
COEFZC = 31457;
C_COEFZC = 1311;
COEFSD = 24576;
C_COEFSD = 8192;
}
else
if (sub(count_update, INIT_COUNT+40) < 0){
COEF = 32604;
C_COEF = 164;
COEFZC = 32440;
C_COEFZC = 328;
COEFSD = 24576;
C_COEFSD = 8192;
}
else{
COEF = 32604;
C_COEF = 164;
COEFZC = 32702;
C_COEFZC = 66;
COEFSD = 24576;
C_COEFSD = 8192;
}
/* compute MeanSE */
acc0 = L_mult(COEF, MeanSE);
acc0 = L_mac(acc0, C_COEF, ENERGY);
MeanSE = extract_h(acc0);
/* compute MeanSLE */
acc0 = L_mult(COEF, MeanSLE);
acc0 = L_mac(acc0, C_COEF, ENERGY_low);
MeanSLE = extract_h(acc0);
/* compute MeanSZC */
acc0 = L_mult(COEFZC, MeanSZC);
acc0 = L_mac(acc0, C_COEFZC, ZC);
MeanSZC = extract_h(acc0);
/* compute MeanLSF */
for (i=0; i<M; i++){
acc0 = L_mult(COEFSD, MeanLSF[i]);
acc0 = L_mac(acc0, C_COEFSD, lsf[i]);
MeanLSF[i] = extract_h(acc0);
}
}
if((sub(frm_count, 128) > 0) && (((sub(MeanSE,Min) < 0) && (sub(SD, 83) < 0)
) || (sub(MeanSE,Min) > 2048))){
MeanSE = Min;
count_update = 0;
}
}
prev_energy = ENERGY;
}
/*-----------------------------------------------------------*
* 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 pit_pst_filt(
Word16 *signal, /* (i) : input signal */
Word16 *scal_sig, /* (i) : input signal (scaled, divided by 4) */
Word16 t0_min, /* (i) : minimum value in the searched range */
Word16 t0_max, /* (i) : maximum value in the searched range */
Word16 L_subfr, /* (i) : size of filtering */
Word16 *signal_pst /* (o) : harmonically postfiltered signal */
)
{
Word16 i, j, t0;
Word16 g0, gain, cmax, en, en0;
Word16 *p, *p1, *deb_sig;
Word32 corr, cor_max, ener, ener0, temp;
Word32 L_temp;
/*---------------------------------------------------------------------------*
* Compute the correlations for all delays *
* and select the delay which maximizes the correlation *
*---------------------------------------------------------------------------*/
deb_sig = &scal_sig[-t0_min];
cor_max = MIN_32;
t0 = t0_min; /* Only to remove warning from some compilers */
for (i=t0_min; i<=t0_max; i++)
{
corr = 0;
p = scal_sig;
p1 = deb_sig;
for (j=0; j<L_subfr; j++)
corr = L_mac(corr, *p++, *p1++);
L_temp = L_sub(corr, cor_max);
if (L_temp > (Word32)0)
{
cor_max = corr;
t0 = i;
}
deb_sig--;
}
/* Compute the energy of the signal delayed by t0 */
ener = 1;
p = scal_sig - t0;
for ( i=0; i<L_subfr ;i++, p++)
ener = L_mac(ener, *p, *p);
/* Compute the signal energy in the present subframe */
ener0 = 1;
p = scal_sig;
for ( i=0; i<L_subfr; i++, p++)
ener0 = L_mac(ener0, *p, *p);
if (cor_max < 0)
{
cor_max = 0;
}
/* scale "cor_max", "ener" and "ener0" on 16 bits */
temp = cor_max;
if (ener > temp)
{
temp = ener;
}
if (ener0 > temp)
{
temp = ener0;
}
j = norm_l(temp);
if(j>0)
{
cmax = round(L_shl(cor_max, j));
en = round(L_shl(ener, j));
en0 = round(L_shl(ener0, j));
}
else
{
Word16 pj = abs_s(j);
cmax = round(L_shr(cor_max, pj));
en = round(L_shr(ener, pj));
en0 = round(L_shr(ener0, pj));
}
/* prediction gain (dB)= -10 log(1-cor_max*cor_max/(ener*ener0)) */
/* temp = (cor_max * cor_max) - (0.5 * ener * ener0) */
temp = L_mult(cmax, cmax);
temp = L_sub(temp, L_shr(L_mult(en, en0), 1));
if (temp < (Word32)0) /* if prediction gain < 3 dB */
{ /* switch off pitch postfilter */
for (i = 0; i < L_subfr; i++)
signal_pst[i] = signal[i];
return;
}
if (sub(cmax, en) > 0) /* if pitch gain > 1 */
{
g0 = INV_GAMMAP;
gain = GAMMAP_2;
}
else {
cmax = shr(mult(cmax, GAMMAP), 1); /* cmax(Q14) = cmax(Q15) * GAMMAP */
en = shr(en, 1); /* Q14 */
i = add(cmax, en);
if(i > 0)
{
gain = div_s(cmax, i); /* gain(Q15) = cor_max/(cor_max+ener) */
g0 = sub(32767, gain); /* g0(Q15) = 1 - gain */
}
else
{
g0 = 32767;
gain = 0;
}
}
for (i = 0; i < L_subfr; i++)
{
/* signal_pst[i] = g0*signal[i] + gain*signal[i-t0]; */
signal_pst[i] = add(mult(g0, signal[i]), mult(gain, signal[i-t0]));
}
return;
}
void Az_lsp(
Word16 a[], /* (i) Q12 : predictor coefficients */
Word16 lsp[], /* (o) Q15 : line spectral pairs */
Word16 old_lsp[] /* (i) : old lsp[] (in case not found 10 roots) */
)
{
Word16 i, j, nf, ip;
Word16 xlow, ylow, xhigh, yhigh, xmid, ymid, xint;
Word16 x, y, sign, exp;
Word16 *coef;
Word16 f1[M/2+1], f2[M/2+1];
Word32 t0, L_temp;
Flag ovf_coef;
Word16 (*pChebps)(Word16 x, Word16 f[], Word16 n);
/*-------------------------------------------------------------*
* find the sum and diff. pol. F1(z) and F2(z) *
* F1(z) <--- F1(z)/(1+z**-1) & F2(z) <--- F2(z)/(1-z**-1) *
* *
* f1[0] = 1.0; *
* f2[0] = 1.0; *
* *
* for (i = 0; i< NC; i++) *
* { *
* f1[i+1] = a[i+1] + a[M-i] - f1[i] ; *
* f2[i+1] = a[i+1] - a[M-i] + f2[i] ; *
* } *
*-------------------------------------------------------------*/
ovf_coef = 0;
pChebps = Chebps_11;
f1[0] = 2048; /* f1[0] = 1.0 is in Q11 */
f2[0] = 2048; /* f2[0] = 1.0 is in Q11 */
for (i = 0; i< NC; i++)
{
Overflow = 0;
t0 = L_mult(a[i+1], 16384); /* x = (a[i+1] + a[M-i]) >> 1 */
t0 = L_mac(t0, a[M-i], 16384); /* -> From Q12 to Q11 */
x = extract_h(t0);
if ( Overflow ) {
ovf_coef = 1; }
Overflow = 0;
f1[i+1] = sub(x, f1[i]); /* f1[i+1] = a[i+1] + a[M-i] - f1[i] */
if ( Overflow ) {
ovf_coef = 1; }
Overflow = 0;
t0 = L_mult(a[i+1], 16384); /* x = (a[i+1] - a[M-i]) >> 1 */
t0 = L_msu(t0, a[M-i], 16384); /* -> From Q12 to Q11 */
x = extract_h(t0);
if ( Overflow ) {
ovf_coef = 1; }
Overflow = 0;
f2[i+1] = add(x, f2[i]); /* f2[i+1] = a[i+1] - a[M-i] + f2[i] */
if ( Overflow ) {
ovf_coef = 1; }
}
if ( ovf_coef ) {
/*printf("===== OVF ovf_coef =====\n");*/
pChebps = Chebps_10;
f1[0] = 1024; /* f1[0] = 1.0 is in Q10 */
f2[0] = 1024; /* f2[0] = 1.0 is in Q10 */
for (i = 0; i< NC; i++)
{
t0 = L_mult(a[i+1], 8192); /* x = (a[i+1] + a[M-i]) >> 1 */
t0 = L_mac(t0, a[M-i], 8192); /* -> From Q11 to Q10 */
x = extract_h(t0);
f1[i+1] = sub(x, f1[i]); /* f1[i+1] = a[i+1] + a[M-i] - f1[i] */
t0 = L_mult(a[i+1], 8192); /* x = (a[i+1] - a[M-i]) >> 1 */
t0 = L_msu(t0, a[M-i], 8192); /* -> From Q11 to Q10 */
x = extract_h(t0);
f2[i+1] = add(x, f2[i]); /* f2[i+1] = a[i+1] - a[M-i] + f2[i] */
}
}
/*-------------------------------------------------------------*
* find the LSPs using the Chebichev pol. evaluation *
*-------------------------------------------------------------*/
nf=0; /* number of found frequencies */
ip=0; /* indicator for f1 or f2 */
coef = f1;
xlow = grid[0];
ylow = (*pChebps)(xlow, coef, NC);
j = 0;
while ( (nf < M) && (j < GRID_POINTS) )
{
j =add(j,1);
xhigh = xlow;
yhigh = ylow;
xlow = grid[j];
ylow = (*pChebps)(xlow,coef,NC);
L_temp = L_mult(ylow ,yhigh);
if ( L_temp <= (Word32)0)
{
/* divide 4 times the interval */
for (i = 0; i < 4; i++)
{
xmid = add( shr(xlow, 1) , shr(xhigh, 1)); /* xmid = (xlow + xhigh)/2 */
ymid = (*pChebps)(xmid,coef,NC);
L_temp = L_mult(ylow,ymid);
if ( L_temp <= (Word32)0)
{
yhigh = ymid;
xhigh = xmid;
}
else
{
ylow = ymid;
xlow = xmid;
}
}
/*-------------------------------------------------------------*
* Linear interpolation *
* xint = xlow - ylow*(xhigh-xlow)/(yhigh-ylow); *
*-------------------------------------------------------------*/
x = sub(xhigh, xlow);
y = sub(yhigh, ylow);
if(y == 0)
{
xint = xlow;
}
else
{
sign= y;
y = abs_s(y);
exp = norm_s(y);
y = shl(y, exp);
y = div_s( (Word16)16383, y);
t0 = L_mult(x, y);
t0 = L_shr(t0, sub(20, exp) );
y = extract_l(t0); /* y= (xhigh-xlow)/(yhigh-ylow) in Q11 */
if(sign < 0) y = negate(y);
t0 = L_mult(ylow, y); /* result in Q26 */
t0 = L_shr(t0, 11); /* result in Q15 */
xint = sub(xlow, extract_l(t0)); /* xint = xlow - ylow*y */
}
lsp[nf] = xint;
xlow = xint;
nf =add(nf,1);
if(ip == 0)
{
ip = 1;
coef = f2;
}
else
{
ip = 0;
coef = f1;
}
ylow = (*pChebps)(xlow,coef,NC);
}
}
/* Check if M roots found */
if( sub(nf, M) < 0)
{
for(i=0; i<M; i++)
{
lsp[i] = old_lsp[i];
}
/* printf("\n !!Not 10 roots found in Az_lsp()!!!\n"); */
}
return;
}
/*******************************************************************************
*
* functionname: scfCount
* returns : ---
* description : count bits used by scalefactors.
*
********************************************************************************/
static void scfCount(const Word16 *scalefacGain,
const UWord16 *maxValueInSfb,
SECTION_DATA * sectionData)
{
SECTION_INFO *psectionInfo;
SECTION_INFO *psectionInfom;
/* counter */
Word32 i = 0; /* section counter */
Word32 j = 0; /* sfb counter */
Word32 k = 0; /* current section auxiliary counter */
Word32 m = 0; /* other section auxiliary counter */
Word32 n = 0; /* other sfb auxiliary counter */
/* further variables */
Word32 lastValScf = 0;
Word32 deltaScf = 0;
Flag found = 0;
Word32 scfSkipCounter = 0;
sectionData->scalefacBits = 0;
if (scalefacGain == NULL) {
return;
}
lastValScf = 0;
sectionData->firstScf = 0;
psectionInfo = sectionData->sectionInfo;
for (i=0; i<sectionData->noOfSections; i++) {
if (psectionInfo->codeBook != CODE_BOOK_ZERO_NO) {
sectionData->firstScf = psectionInfo->sfbStart;
lastValScf = scalefacGain[sectionData->firstScf];
break;
}
psectionInfo += 1;
}
psectionInfo = sectionData->sectionInfo;
for (i=0; i<sectionData->noOfSections; i++, psectionInfo += 1) {
if (psectionInfo->codeBook != CODE_BOOK_ZERO_NO
&& psectionInfo->codeBook != CODE_BOOK_PNS_NO) {
for (j = psectionInfo->sfbStart;
j < (psectionInfo->sfbStart + psectionInfo->sfbCnt); j++) {
/* check if we can repeat the last value to save bits */
if (maxValueInSfb[j] == 0) {
found = 0;
if (scfSkipCounter == 0) {
/* end of section */
if (j - ((psectionInfo->sfbStart + psectionInfo->sfbCnt) - 1) == 0) {
found = 0;
}
else {
for (k = j + 1; k < psectionInfo->sfbStart + psectionInfo->sfbCnt; k++) {
if (maxValueInSfb[k] != 0) {
int tmp = L_abs(scalefacGain[k] - lastValScf);
found = 1;
if ( tmp < CODE_BOOK_SCF_LAV) {
/* save bits */
deltaScf = 0;
}
else {
/* do not save bits */
deltaScf = lastValScf - scalefacGain[j];
lastValScf = scalefacGain[j];
scfSkipCounter = 0;
}
break;
}
/* count scalefactor skip */
scfSkipCounter = scfSkipCounter + 1;
}
}
psectionInfom = psectionInfo + 1;
/* search for the next maxValueInSfb[] != 0 in all other sections */
for (m = i + 1; (m < sectionData->noOfSections) && (found == 0); m++) {
if ((psectionInfom->codeBook != CODE_BOOK_ZERO_NO) &&
(psectionInfom->codeBook != CODE_BOOK_PNS_NO)) {
for (n = psectionInfom->sfbStart;
n < (psectionInfom->sfbStart + psectionInfom->sfbCnt); n++) {
if (maxValueInSfb[n] != 0) {
found = 1;
if ( (abs_s(scalefacGain[n] - lastValScf) < CODE_BOOK_SCF_LAV)) {
deltaScf = 0;
}
else {
deltaScf = (lastValScf - scalefacGain[j]);
lastValScf = scalefacGain[j];
scfSkipCounter = 0;
}
break;
}
/* count scalefactor skip */
scfSkipCounter = scfSkipCounter + 1;
}
}
psectionInfom += 1;
}
if (found == 0) {
deltaScf = 0;
scfSkipCounter = 0;
}
}
else {
deltaScf = 0;
scfSkipCounter = scfSkipCounter - 1;
}
}
else {
deltaScf = lastValScf - scalefacGain[j];
lastValScf = scalefacGain[j];
}
sectionData->scalefacBits += bitCountScalefactorDelta(deltaScf);
}
}
}
}
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;
}
/*---------------------------------------------------------------------------*
* Function Dec_gain *
* ~~~~~~~~~~~~~~~~~~ *
* Decode the pitch and codebook gains *
* *
*---------------------------------------------------------------------------*
* input arguments: *
* *
* index :Quantization index *
* code[] :Innovative code vector *
* L_subfr :Subframe size *
* bfi :Bad frame indicator *
* *
* output arguments: *
* *
* gain_pit :Quantized pitch gain *
* gain_cod :Quantized codebook gain *
* *
*---------------------------------------------------------------------------*/
void Dec_gain(
Word16 index, /* (i) :Index of quantization. */
Word16 code[], /* (i) Q13 :Innovative vector. */
Word16 L_subfr, /* (i) :Subframe length. */
Word16 bfi, /* (i) :Bad frame indicator */
Word16 *gain_pit, /* (o) Q14 :Pitch gain. */
Word16 *gain_cod /* (o) Q1 :Code gain. */
)
{
Word16 index1, index2, tmp;
Word16 gcode0, exp_gcode0;
Word32 L_gbk12, L_acc, L_accb;
dec_gain_type* pdec_gain = pg729dec->pdec_gain;
void Gain_predict( Word16 past_qua_en[], Word16 code[], Word16 L_subfr,
Word16 *gcode0, Word16 *exp_gcode0 );
void Gain_update( Word16 past_qua_en[], Word32 L_gbk12 );
void Gain_update_erasure( Word16 past_qua_en[] );
/* Gain predictor, Past quantized energies = -14.0 in Q10 */
#if 0
static Word16 past_qua_en[4] = { -14336, -14336, -14336, -14336 };
#endif
/*-------------- Case of erasure. ---------------*/
if(bfi != 0){
*gain_pit = mult( *gain_pit, 29491 ); /* *0.9 in Q15 */
if (sub( *gain_pit, 29491) > 0) *gain_pit = 29491;
*gain_cod = mult( *gain_cod, 32111 ); /* *0.98 in Q15 */
/*----------------------------------------------*
* update table of past quantized energies *
* (frame erasure) *
*----------------------------------------------*/
Gain_update_erasure(pdec_gain->past_qua_en);
return;
}
/*-------------- Decode pitch gain ---------------*/
index1 = imap1[ shr(index,NCODE2_B) ] ;
index2 = imap2[ index & (NCODE2-1) ] ;
*gain_pit = add( gbk1[index1][0], gbk2[index2][0] );
/*-------------- Decode codebook gain ---------------*/
/*---------------------------------------------------*
*- energy due to innovation -*
*- predicted energy -*
*- predicted codebook gain => gcode0[exp_gcode0] -*
*---------------------------------------------------*/
Gain_predict( pdec_gain->past_qua_en, code, L_subfr, &gcode0, &exp_gcode0 );
/*-----------------------------------------------------------------*
* *gain_code = (gbk1[indice1][1]+gbk2[indice2][1]) * gcode0; *
*-----------------------------------------------------------------*/
L_acc = L_deposit_l( gbk1[index1][1] );
L_accb = L_deposit_l( gbk2[index2][1] );
L_gbk12 = L_add( L_acc, L_accb ); /* Q13 */
tmp = extract_l( L_shr( L_gbk12,1 ) ); /* Q12 */
L_acc = L_mult(tmp, gcode0); /* Q[exp_gcode0+12+1] */
{
Word16 shift = add( negate(exp_gcode0),(-12-1+1+16) );
if(shift>0)
L_acc = L_shl(L_acc, shift);
else
L_acc = L_shr(L_acc, abs_s(shift));
}
*gain_cod = extract_h( L_acc ); /* Q1 */
/*----------------------------------------------*
* update table of past quantized energies *
*----------------------------------------------*/
Gain_update( pdec_gain->past_qua_en, L_gbk12 );
return;
}
Word16 Cb_gain_average(
Cb_gain_averageState *st, /* i/o : State variables for CB gain averaging */
enum Mode mode, /* i : AMR mode */
Word16 gain_code, /* i : CB gain Q1 */
Word16 lsp[], /* i : The LSP for the current frame Q15 */
Word16 lspAver[], /* i : The average of LSP for 8 frames Q15 */
Word16 bfi, /* i : bad frame indication flag */
Word16 prev_bf, /* i : previous bad frame indication flag */
Word16 pdfi, /* i : potential degraded bad frame ind flag */
Word16 prev_pdf, /* i : prev pot. degraded bad frame ind flag */
Word16 inBackgroundNoise, /* i : background noise decision */
Word16 voicedHangover, /* i : # of frames after last voiced frame */
Flag *pOverflow
)
{
Word16 i;
Word16 cbGainMix;
Word16 diff;
Word16 tmp_diff;
Word16 bgMix;
Word16 cbGainMean;
Word32 L_sum;
Word16 tmp[M];
Word16 tmp1;
Word16 tmp2;
Word16 shift1;
Word16 shift2;
Word16 shift;
/*---------------------------------------------------------*
* Compute mixed cb gain, used to make cb gain more *
* smooth in background noise for modes 5.15, 5.9 and 6.7 *
* states that needs to be updated by all *
*---------------------------------------------------------*/
/* set correct cbGainMix for MR74, MR795, MR122 */
cbGainMix = gain_code;
/*-------------------------------------------------------*
* Store list of CB gain needed in the CB gain *
* averaging *
*-------------------------------------------------------*/
for (i = 0; i < (L_CBGAINHIST - 1); i++)
{
st->cbGainHistory[i] = st->cbGainHistory[i+1];
}
st->cbGainHistory[L_CBGAINHIST-1] = gain_code;
diff = 0;
/* compute lsp difference */
for (i = 0; i < M; i++)
{
tmp1 = abs_s(sub(*(lspAver + i), *(lsp + i), pOverflow));
/* Q15 */
shift1 = sub(norm_s(tmp1), 1, pOverflow); /* Qn */
tmp1 = shl(tmp1, shift1, pOverflow); /* Q15+Qn */
shift2 = norm_s(*(lspAver + i)); /* Qm */
tmp2 = shl(*(lspAver + i), shift2, pOverflow); /* Q15+Qm */
tmp[i] = div_s(tmp1, tmp2); /* Q15+(Q15+Qn)-(Q15+Qm) */
shift = 2 + shift1 - shift2;
if (shift >= 0)
{
*(tmp + i) = shr(*(tmp + i), shift, pOverflow);
/* Q15+Qn-Qm-Qx=Q13 */
}
else
{
*(tmp + i) = shl(*(tmp + i), negate(shift), pOverflow);
/* Q15+Qn-Qm-Qx=Q13 */
}
diff = add(diff, *(tmp + i), pOverflow); /* Q13 */
}
/* Compute hangover */
if (diff > 5325) /* 0.65 in Q11 */
{
st->hangVar += 1;
}
else
{
st->hangVar = 0;
}
if (st->hangVar > 10)
{
/* Speech period, reset hangover variable */
st->hangCount = 0;
}
/* Compute mix constant (bgMix) */
bgMix = 8192; /* 1 in Q13 */
if ((mode <= MR67) || (mode == MR102))
/* MR475, MR515, MR59, MR67, MR102 */
{
/* if errors and presumed noise make smoothing probability stronger */
if (((((pdfi != 0) && (prev_pdf != 0)) || (bfi != 0) ||
(prev_bf != 0))
&& (voicedHangover > 1)
&& (inBackgroundNoise != 0)
&& ((mode == MR475) || (mode == MR515) ||
(mode == MR59))))
{
/* bgMix = min(0.25, max(0.0, diff-0.55)) / 0.25; */
tmp_diff = sub(diff, 4506, pOverflow); /* 0.55 in Q13 */
}
else
{
/* bgMix = min(0.25, max(0.0, diff-0.40)) / 0.25; */
tmp_diff = sub(diff, 3277, pOverflow); /* 0.4 in Q13 */
}
/* max(0.0, diff-0.55) or */
/* max(0.0, diff-0.40) */
if (tmp_diff > 0)
{
tmp1 = tmp_diff;
}
else
{
tmp1 = 0;
}
/* min(0.25, tmp1) */
if (2048 < tmp1)
{
bgMix = 8192;
}
else
{
bgMix = shl(tmp1, 2, pOverflow);
}
if ((st->hangCount < 40) || (diff > 5325)) /* 0.65 in Q13 */
{
/* disable mix if too short time since */
bgMix = 8192;
}
/* Smoothen the cb gain trajectory */
/* smoothing depends on mix constant bgMix */
L_sum = L_mult(6554, st->cbGainHistory[2], pOverflow);
/* 0.2 in Q15; L_sum in Q17 */
for (i = 3; i < L_CBGAINHIST; i++)
{
L_sum = L_mac(L_sum, 6554, st->cbGainHistory[i], pOverflow);
}
cbGainMean = pv_round(L_sum, pOverflow); /* Q1 */
/* more smoothing in error and bg noise (NB no DFI used here) */
if (((bfi != 0) || (prev_bf != 0)) && (inBackgroundNoise != 0)
&& ((mode == MR475) || (mode == MR515)
|| (mode == MR59)))
{
/* 0.143 in Q15; L_sum in Q17 */
L_sum = L_mult(4681, st->cbGainHistory[0], pOverflow);
for (i = 1; i < L_CBGAINHIST; i++)
{
L_sum =
L_mac(L_sum, 4681, st->cbGainHistory[i], pOverflow);
}
cbGainMean = pv_round(L_sum, pOverflow); /* Q1 */
}
/* cbGainMix = bgMix*cbGainMix + (1-bgMix)*cbGainMean; */
/* L_sum in Q15 */
L_sum = L_mult(bgMix, cbGainMix, pOverflow);
L_sum = L_mac(L_sum, 8192, cbGainMean, pOverflow);
L_sum = L_msu(L_sum, bgMix, cbGainMean, pOverflow);
cbGainMix = pv_round(L_shl(L_sum, 2, pOverflow), pOverflow); /* Q1 */
}
st->hangCount += 1;
return (cbGainMix);
}
void perc_var (
Word16 *gamma1, /* Bandwidth expansion parameter */
Word16 *gamma2, /* Bandwidth expansion parameter */
Word16 *LsfInt, /* Interpolated LSP vector : 1st subframe */
Word16 *LsfNew, /* New LSP vector : 2nd subframe */
Word16 *r_c /* Reflection coefficients */
)
{
Word32 L_temp;
Word16 cur_rc; /* Q11 */
Word16 Lar[4]; /* Q11 */
Word16 *LarNew; /* Q11 */
Word16 *Lsf; /* Q15 */
Word16 CritLar0, CritLar1; /* Q11 */
Word16 temp;
Word16 d_min; /* Q10 */
Word16 i, k;
for (k=0; k<M; k++) {
LsfInt[k] = shl(LsfInt[k], 1);
LsfNew[k] = shl(LsfNew[k], 1);
}
LarNew = &Lar[2];
/* ---------------------------------------- */
/* Reflection coefficients ---> Lar */
/* Lar(i) = log10( (1+rc) / (1-rc) ) */
/* Approximated by */
/* x <= SEG1 y = x */
/* SEG1 < x <= SEG2 y = A1 x - B1_L */
/* SEG2 < x <= SEG3 y = A2 x - B2_L */
/* x > SEG3 y = A3 x - B3_L */
/* ---------------------------------------- */
for (i=0; i<2; i++) {
cur_rc = abs_s(r_c[i]);
cur_rc = shr(cur_rc, 4);
if (sub(cur_rc ,SEG1)<= 0) {
LarNew[i] = cur_rc;
}
else {
if (sub(cur_rc,SEG2)<= 0) {
cur_rc = shr(cur_rc, 1);
L_temp = L_mult(cur_rc, A1);
L_temp = L_sub(L_temp, L_B1);
L_temp = L_shr(L_temp, 11);
LarNew[i] = extract_l(L_temp);
}
else {
if (sub(cur_rc ,SEG3)<= 0) {
cur_rc = shr(cur_rc, 1);
L_temp = L_mult(cur_rc, A2);
L_temp = L_sub(L_temp, L_B2);
L_temp = L_shr(L_temp, 11);
LarNew[i] = extract_l(L_temp);
}
else {
cur_rc = shr(cur_rc, 1);
L_temp = L_mult(cur_rc, A3);
L_temp = L_sub(L_temp, L_B3);
L_temp = L_shr(L_temp, 11);
LarNew[i] = extract_l(L_temp);
}
}
}
if (r_c[i] < 0) {
LarNew[i] = sub(0, LarNew[i]);
}
}
/* Interpolation of Lar for the 1st subframe */
temp = add(LarNew[0], LarOld[0]);
Lar[0] = shr(temp, 1);
LarOld[0] = LarNew[0];
temp = add(LarNew[1], LarOld[1]);
Lar[1] = shr(temp, 1);
LarOld[1] = LarNew[1];
for (k=0; k<2; k++) { /* LOOP : gamma2 for 1st to 2nd subframes */
/* ---------------------------------------------------------- */
/* First criterion based on the first two Lars */
/* smooth == 1 ==> gamma2 can vary from 0.4 to 0.7 */
/* smooth == 0 ==> gamma2 is set to 0.6 */
/* */
/* Double threshold + hysteresis : */
/* if smooth = 1 */
/* if (CritLar0 < THRESH_L1) and (CritLar1 > THRESH_H1) */
/* smooth = 0 */
/* if smooth = 0 */
/* if (CritLar0 > THRESH_L2) or (CritLar1 < THRESH_H2) */
/* smooth = 1 */
/* ---------------------------------------------------------- */
CritLar0 = Lar[2*k];
CritLar1 = Lar[2*k+1];
if (smooth != 0) {
if ((sub(CritLar0,THRESH_L1)<0)&&( sub(CritLar1,THRESH_H1)>0)) {
smooth = 0;
}
}
else {
if ( (sub(CritLar0 ,THRESH_L2)>0) || (sub(CritLar1,THRESH_H2) <0) ) {
smooth = 1;
}
}
if (smooth == 0) {
/* ------------------------------------------------------ */
/* Second criterion based on the minimum distance between */
/* two successives LSPs */
/* */
/* gamma2[k] = -6.0 * pi * d_min + 1.0 */
/* */
/* with Lsfs normalized range 0.0 <= val <= 1.0 */
/* ------------------------------------------------------ */
gamma1[k] = GAMMA1_0;
if (k == 0) {
Lsf = LsfInt;
}
else {
Lsf = LsfNew;
}
d_min = sub(Lsf[1], Lsf[0]);
for (i=1; i<M-1; i++) {
temp = sub(Lsf[i+1],Lsf[i]);
if (sub(temp,d_min)<0) {
d_min = temp;
}
}
temp = mult(ALPHA, d_min);
temp = sub(BETA, temp);
temp = shl(temp, 5);
gamma2[k] = temp;
if (sub(gamma2[k] , GAMMA2_0_H)>0) {
gamma2[k] = GAMMA2_0_H;
}
if (sub(gamma2[k] ,GAMMA2_0_L)<0) {
gamma2[k] = GAMMA2_0_L;
}
}
else {
gamma1[k] = GAMMA1_1;
gamma2[k] = GAMMA2_1;
}
}
return;
}
void ABS_S(void)
{
if (check_cop1_unusable()) return;
abs_s(reg_cop1_simple[cffs], reg_cop1_simple[cffd]);
PC++;
}
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 () */
void fndppf(short *delay, short *beta, short *buf, short dmin, short dmax, short length)
{
static short b = -10224; /* rom storage */
static short a[3] =
{-18739, 16024, -4882}; /* a[] scaled down by 4 */
short dnew = 0;
short sum;
long Lsum;
register short m, i, n;
static short DECbuf[FrameSize / 4];
long Lcorrmax, Lcmax, Ltmp;
short tap1;
short M1, M2, dnewtmp = 0;
static short lastgoodpitch = 0;
static short lastbeta = 0;
static short memory[3];
static int FirstTime = 1;
short Lsum_scale;
short shift, Lcorr_scale, Lcmax_scale;
short n1, n2, nq, nq1;
long Ltempf;
/* init static variables (should be in init routine for implementation) */
if (FirstTime)
{
FirstTime = 0;
n1 = (shr(FrameSize, 2));
for (i = 0; i < n1; i++)
DECbuf[i] = 0;
memory[0] = memory[1] = memory[2] = 0;
}
/* Shift memory of DECbuf */
for (i = 0; i < shr(length, 3); i++)
{
DECbuf[i] = DECbuf[i + shr(length, 3)];
}
/* filter signal and decimate */
for (i = 0, n = shr(length, 3); i < shr(length, 1); i++)
{
Ltempf = L_shr(L_deposit_h(buf[i + shr(length, 1)]), 4);
Ltempf = L_msu(Ltempf, memory[0], a[0]);
Ltempf = L_msu(Ltempf, memory[1], a[1]);
Ltempf = L_msu(Ltempf, memory[2], a[2]);
Ltempf = L_shl(Ltempf, 2);
shift = 0;
if ((i + 1) % 4 == 0)
{
Lsum = L_add(Ltempf, L_deposit_h(memory[2]));
Lsum = L_mac(Lsum, memory[0], b);
Lsum = L_mac(Lsum, memory[1], b);
DECbuf[n++] = round(L_shl(Lsum, 1));
}
memory[2] = memory[1];
memory[1] = memory[0];
memory[0] = round(Ltempf);
}
/* perform first search for best delay value in decimated domain */
Lcorrmax = (LW_MIN);
Lcorr_scale = 1;
for (m = shr(dmin, 2); m <= shr(dmax, 2); m++)
{
n1 = 1;
for (i = 0, Lsum = 0; i < sub(shr(length, 2), m); i++)
{
Ltempf = L_mult(DECbuf[i], DECbuf[i + m]);
Ltempf = L_shr(Ltempf, n1);
Lsum = L_add(Lsum, Ltempf);
if (L_abs(Lsum) >= 0x40000000)
{
Lsum = L_shr(Lsum, 1);
n1++;
}
}
if (
((Lcorr_scale >= n1) && (L_shr(Lsum, sub(Lcorr_scale, n1)) > Lcorrmax))
|| ((Lcorr_scale < n1) && (Lsum > L_shr(Lcorrmax, sub(n1, Lcorr_scale))))
)
{
Lcorrmax = Lsum;
Lcorr_scale = n1;
dnew = m;
}
}
/* Compare against lastgoodpitch */
if (lastgoodpitch != 0 && (abs_s(sub(lastgoodpitch, shl(dnew, 2))) > 2))
{
M1 = sub(shr(lastgoodpitch, 2), 2);
if (M1 < shr(dmin, 2))
M1 = shr(dmin, 2);
M2 = add(M1, 4);
if (M2 > shr(dmax, 2))
M2 = shr(dmax, 2);
Lcmax = LW_MIN;
Lcmax_scale = 1;
for (m = M1; m <= M2; m++)
{
n1 = 1;
for (i = 0, Lsum = 0; i < sub(shr(length, 2), m); i++)
{
Ltempf = L_mult(DECbuf[i], DECbuf[i + m]);
Ltempf = L_shr(Ltempf, n1);
Lsum = L_add(Lsum, Ltempf);
if (L_abs(Lsum) >= 0x40000000)
{
Lsum = L_shr(Lsum, 1);
n1++;
}
}
if (
((Lcmax_scale >= n1) && (L_shr(Lsum, sub(Lcmax_scale, n1)) > Lcmax))
|| ((Lcmax_scale < n1) && (Lsum > L_shr(Lcmax, sub(n1, Lcmax_scale))))
)
{ /* Gives some bias to low delays */
Lcmax = Lsum;
Lcmax_scale = n1;
dnewtmp = m;
}
}
Lsum = L_mpy_ls(Lcorrmax, 27361);
if (
((Lcmax_scale >= Lcorr_scale) && (L_shr(Lsum, sub(Lcmax_scale, Lcorr_scale)) < Lcmax))
|| ((Lcmax_scale < Lcorr_scale) && (Lsum < L_shr(Lcmax, sub(Lcorr_scale, Lcmax_scale))))
)
{
dnew = dnewtmp;
}
}
/* perform first search for best delay value in non-decimated buffer */
M1 = Max(sub(shl(dnew, 2), 3), dmin);
if (M1 < dmin)
M1 = dmin;
M2 = Min(add(shl(dnew, 2), 3), dmax);
if (M2 > dmax)
M2 = dmax;
Lcorrmax = LW_MIN;
Lcorr_scale = 1;
for (m = M1; m <= M2; m++)
{
n1 = 1;
for (i = 0, Lsum = 0; i < sub(length, m); i++)
{
Ltempf = L_mult(buf[i], buf[i + m]);
Ltempf = L_shr(Ltempf, n1);
Lsum = L_add(Lsum, Ltempf);
if (L_abs(Lsum) >= 0x40000000)
{
Lsum = L_shr(Lsum, 1);
n1++;
}
}
if (
((Lcorr_scale >= n1) && (L_shr(Lsum, sub(Lcorr_scale, n1)) > Lcorrmax))
|| ((Lcorr_scale < n1) && (Lsum > L_shr(Lcorrmax, sub(n1, Lcorr_scale))))
)
{
Lcorrmax = Lsum;
Lcorr_scale = n1;
dnew = m;
}
}
Lsum_scale = 1;
for (i = 0, Lsum = 0; i < sub(length, dnew); i++)
{
Ltempf = L_mult(buf[i + dnew], buf[i + dnew]);
Ltempf = L_shr(Ltempf, Lsum_scale);
Lsum = L_add(Lsum, Ltempf);
if (L_abs(Lsum) >= 0x40000000)
{
Lsum = L_shr(Lsum, 1);
Lsum_scale++;
}
}
Lcmax_scale = 1;
for (i = 0, Lcmax = 0; i < length - dnew; i++)
{
Ltempf = L_mult(buf[i], buf[i]);
Ltempf = L_shr(Ltempf, Lcmax_scale);
Lcmax = L_add(Lcmax, Ltempf);
if (L_abs(Lcmax) >= 0x40000000)
{
Lcmax = L_shr(Lcmax, 1);
Lcmax_scale++;
}
}
nq = norm_l(Lsum);
Lsum = L_shl(Lsum, nq);
nq1 = norm_l(Lcmax);
Lcmax = L_shl(Lcmax, nq1);
Lsum = L_mpy_ll(Lsum, Lcmax);
n1 = norm_l(Lsum);
Lsum = L_shl(Lsum, n1);
sum = sqroot(Lsum);
n1 = add(add(n1, nq), nq1);
n1 = sub(sub(n1, Lcmax_scale), Lsum_scale);
n2 = shr(n1, 1);
if (n1 & 1)
Lsum = L_mult(sum, 23170);
else
Lsum = L_deposit_h(sum);
n2 = add(n2, Lcorr_scale);
Lcorrmax = L_shl(Lcorrmax, n2);
if ((Lsum == 0) || (Lcorrmax <= 0))
*beta = 0;
else if (Lcorrmax > Lsum)
*beta = 0x7fff;
else
*beta = round(L_divide(Lcorrmax, Lsum));
/* perform search for best delay value in around old pitch delay */
if (lastgoodpitch != 0)
{
M1 = lastgoodpitch - 6;
M2 = lastgoodpitch + 6;
if (M1 < dmin)
M1 = dmin;
if (M2 > dmax)
M2 = dmax;
if (dnew > M2 || dnew < M1)
{
Lcmax = LW_MIN;
Lcmax_scale = 1;
for (m = M1; m <= M2; m++)
{
n1 = 1;
for (i = 0, Lsum = 0; i < length - m; i++)
{
Ltempf = L_mult(buf[i], buf[i + m]);
Ltempf = L_shr(Ltempf, n1);
Lsum = L_add(Lsum, Ltempf);
if (L_abs(Lsum) >= 0x40000000)
{
Lsum = L_shr(Lsum, 1);
n1++;
}
}
if (
((Lcmax_scale >= n1) && (L_shr(Lsum, sub(Lcmax_scale, n1)) > Lcmax))
|| ((Lcmax_scale < n1) && (Lsum > L_shr(Lcmax, sub(n1, Lcmax_scale))))
)
{
Lcmax = Lsum;
dnewtmp = m;
Lcmax_scale = n1;
}
}
Lcorr_scale = 1;
for (i = 0, Ltmp = 0; i < length - dnewtmp; i++)
{
Ltempf = L_mult(buf[i + dnewtmp], buf[i + dnewtmp]);
Ltempf = L_shr(Ltempf, Lcorr_scale);
Ltmp = L_add(Ltmp, Ltempf);
if (L_abs(Ltmp) >= 0x40000000)
{
Ltmp = L_shr(Ltmp, 1);
Lcorr_scale++;
}
}
Lsum_scale = 1;
for (i = 0, Lsum = 0; i < length - dnewtmp; i++)
{
Ltempf = L_mult(buf[i], buf[i]);
Ltempf = L_shr(Ltempf, Lsum_scale);
Lsum = L_add(Lsum, Ltempf);
if (L_abs(Lsum) >= 0x40000000)
{
Lsum = L_shr(Lsum, 1);
Lsum_scale++;
}
}
nq = norm_l(Ltmp);
Ltmp = L_shl(Ltmp, nq);
nq1 = norm_l(Lsum);
Lsum = L_shl(Lsum, nq1);
Ltmp = L_mpy_ll(Ltmp, Lsum);
n1 = norm_l(Ltmp);
Ltmp = L_shl(Ltmp, n1);
sum = sqroot(Ltmp);
n1 = add(add(n1, nq), nq1);
n1 = sub(sub(n1, Lsum_scale), Lcorr_scale);
n2 = shr(n1, 1);
if (n1 & 1)
Ltmp = L_mult(sum, 23170);
else
Ltmp = L_deposit_h(sum);
n2 = add(n2, Lcmax_scale);
Lcmax = L_shl(Lcmax, n2);
if ((Ltmp == 0) || (Lcmax <= 0))
tap1 = 0;
else if (Lcmax >= Ltmp)
tap1 = 0x7fff;
else
tap1 = round(L_divide(Lcmax, Ltmp));
/* Replace dnew with dnewtmp if tap1 is large enough */
if ((dnew > M2 && (shr(tap1, 1) > mult_r(9830, *beta))) ||
(dnew < M1 && (shr(tap1, 1) > mult_r(19661, *beta))))
{
dnew = dnewtmp;
*beta = (tap1);
}
}
}
*delay = dnew;
if (*beta > 13107)
{
lastgoodpitch = dnew;
lastbeta = *beta;
}
else
{
lastbeta = mult_r(24576, lastbeta);
if (lastbeta < 9830)
lastgoodpitch = 0;
}
}
void Levinsone(
Word16 m, /* (i) : LPC order */
Word16 Rh[], /* (i) : Rh[M+1] Vector of autocorrelations (msb) */
Word16 Rl[], /* (i) : Rl[M+1] Vector of autocorrelations (lsb) */
Word16 A[], /* (o) Q12 : A[M] LPC coefficients (m = 10) */
Word16 rc[], /* (o) Q15 : rc[M] Reflection coefficients. */
Word16 old_A[], /* (i/o) Q12 : last stable filter LPC coefficients */
Word16 old_rc[] /* (i/o) Q15 : last stable filter Reflection coefficients. */
)
{
Word16 i, j;
Word16 hi, lo;
Word16 Kh, Kl; /* reflection coefficient; hi and lo */
Word16 alp_h, alp_l, alp_exp; /* Prediction gain; hi lo and exponent */
Word16 Ah[M_BWDP1], Al[M_BWDP1]; /* LPC coef. in double prec. */
Word16 Anh[M_BWDP1], Anl[M_BWDP1]; /* LPC coef.for next iteration in double prec. */
Word32 t0, t1, t2; /* temporary variable */
Word32 tmp;
/* K = A[1] = -R[1] / R[0] */
t1 = L_Comp(Rh[1], Rl[1]); /* R[1] in Q31 */
tmp = L_Comp(Rh[0], Rl[0]);
if (t1 > tmp) t1 = tmp;
t2 = L_abs(t1); /* abs R[1] */
t0 = Div_32(t2, Rh[0], Rl[0]); /* R[1]/R[0] in Q31 */
if(t1 > 0) t0= L_negate(t0); /* -R[1]/R[0] */
L_Extract(t0, &Kh, &Kl); /* K in DPF */
rc[0] = Kh;
t0 = L_shr(t0,4); /* A[1] in Q27 */
L_Extract(t0, &Ah[1], &Al[1]); /* A[1] in DPF */
/* Alpha = R[0] * (1-K**2) */
t0 = Mpy_32(Kh ,Kl, Kh, Kl); /* K*K in Q31 */
t0 = L_abs(t0); /* Some case <0 !! */
t0 = L_sub( (Word32)0x7fffffffL, t0 ); /* 1 - K*K in Q31 */
L_Extract(t0, &hi, &lo); /* DPF format */
t0 = Mpy_32(Rh[0] ,Rl[0], hi, lo); /* Alpha in Q31 */
/* Normalize Alpha */
alp_exp = norm_l(t0);
t0 = L_shl(t0, alp_exp);
L_Extract(t0, &alp_h, &alp_l); /* DPF format */
/*--------------------------------------*
* ITERATIONS I=2 to m *
*--------------------------------------*/
for(i= 2; i<=m; i++)
{
/* t0 = SUM ( R[j]*A[i-j] ,j=1,i-1 ) + R[i] */
t0 = 0;
for(j=1; j<i; j++)
t0 = L_add(t0, Mpy_32(Rh[j], Rl[j], Ah[i-j], Al[i-j]));
t0 = L_shl(t0,4); /* result in Q27 -> convert to Q31 */
/* No overflow possible */
t1 = L_Comp(Rh[i],Rl[i]);
t0 = L_add(t0, t1); /* add R[i] in Q31 */
/* K = -t0 / Alpha */
t1 = L_abs(t0);
tmp = L_Comp(alp_h, alp_l);
if (t1 > tmp) t1 = tmp;
t2 = Div_32(t1, alp_h, alp_l); /* abs(t0)/Alpha */
if(t0 > 0) t2= L_negate(t2); /* K =-t0/Alpha */
t2 = L_shl(t2, alp_exp); /* denormalize; compare to Alpha */
L_Extract(t2, &Kh, &Kl); /* K in DPF */
rc[i-1] = Kh;
/* Test for unstable filter. If unstable keep old A(z) */
if (sub(abs_s(Kh), 32750) > 0)
{
for(j=0; j<=m; j++)
{
A[j] = old_A[j];
}
rc[0] = old_rc[0]; /* only two rc coefficients are needed */
rc[1] = old_rc[1];
return;
}
/*------------------------------------------*
* Compute new LPC coeff. -> An[i] *
* An[j]= A[j] + K*A[i-j] , j=1 to i-1 *
* An[i]= K *
*------------------------------------------*/
for(j=1; j<i; j++)
{
t0 = Mpy_32(Kh, Kl, Ah[i-j], Al[i-j]);
t0 = L_add(t0, L_Comp(Ah[j], Al[j]));
L_Extract(t0, &Anh[j], &Anl[j]);
}
t2 = L_shr(t2, 4); /* t2 = K in Q31 ->convert to Q27 */
L_Extract(t2, &Anh[i], &Anl[i]); /* An[i] in Q27 */
/* Alpha = Alpha * (1-K**2) */
t0 = Mpy_32(Kh ,Kl, Kh, Kl); /* K*K in Q31 */
t0 = L_abs(t0); /* Some case <0 !! */
t0 = L_sub( (Word32)0x7fffffffL, t0 ); /* 1 - K*K in Q31 */
L_Extract(t0, &hi, &lo); /* DPF format */
t0 = Mpy_32(alp_h , alp_l, hi, lo); /* Alpha in Q31 */
/* Normalize Alpha */
j = norm_l(t0);
t0 = L_shl(t0, j);
L_Extract(t0, &alp_h, &alp_l); /* DPF format */
alp_exp = add(alp_exp, j); /* Add normalization to alp_exp */
/* A[j] = An[j] */
for(j=1; j<=i; j++)
{
Ah[j] =Anh[j];
Al[j] =Anl[j];
}
}
/* Truncate A[i] in Q27 to Q12 with rounding */
A[0] = 4096;
for(i=1; i<=m; i++)
{
t0 = L_Comp(Ah[i], Al[i]);
old_A[i] = A[i] = round(L_shl(t0, 1));
}
old_rc[0] = rc[0];
old_rc[1] = rc[1];
return;
}
/*
**
** Function: 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;
}
void QD0_OnCompare(void)
{
/* finding of source of interrupt (COMPARE1 or COMPARE2) */
Timer0Value=getReg(TMRA0_CNTR);
Timer0Difference1=abs_s((Timer0Value-QD0_GetCompare1Value()));
Timer0Difference2=abs_s((Timer0Value-QD0_GetCompare2Value()));
// can_printf("CMP1 %d", QD0_GetCompare1Value());
// can_printf("CMP2 %d", QD0_GetCompare2Value());
// can_printf("T %d",Timer0Value);
// can_printf("DF2 %d DF1 ,%d",Timer0Difference2, Timer0Difference1);
/* according to finding of interrupt source */
if (Timer0Difference1<Timer0Difference2)
{
/* set direction of spinning */
DirectionSpinning0 = 1;
/* increment commutation counter */
if (CommutationCounter0<6)
{
CommutationCounter0++;
}
else
{
CommutationCounter0=1;
}
/* set new value to COMPARE1/2 registers and compensate rounding of NORMAL_COMMUTATION_INTERVAL */
if (CommutationCounter0==4)
{
QD0_SetCompare1(QD0_GetCompare1Value() + SHORT_COMMUTATION_INTERVAL);
QD0_SetCompare2(QD0_GetCompare2Value() + SHORT_COMMUTATION_INTERVAL);
}
else
{
QD0_SetCompare1(QD0_GetCompare1Value() + NORMAL_COMMUTATION_INTERVAL);
QD0_SetCompare2(QD0_GetCompare2Value() + NORMAL_COMMUTATION_INTERVAL);
}
}
else
{
/* set direction of spinning */
DirectionSpinning0 = -1;
/* decrement commutation counter */
if (CommutationCounter0>1)
{
CommutationCounter0--;
}
else
{
CommutationCounter0=6;
}
/* set new value to COMPARE1/2 registers and compensate rounding of NORMAL_COMMUTATION_INTERVAL */
if (CommutationCounter0==3)
{
QD0_SetCompare2(QD0_GetCompare2Value() - SHORT_COMMUTATION_INTERVAL);
QD0_SetCompare1(QD0_GetCompare1Value() - SHORT_COMMUTATION_INTERVAL);
}
else
{
QD0_SetCompare2(QD0_GetCompare2Value() - NORMAL_COMMUTATION_INTERVAL);
QD0_SetCompare1(QD0_GetCompare1Value() - NORMAL_COMMUTATION_INTERVAL);
}
}
status0 = COMMUTATION_CONVERSION_TABLE[CommutationCounter0];
// write mask to PWM Channel Control Register
PWMState[0]= pTable0[status0];
tmp = getReg(PWMA_PMOUT) & 0x8000;
val=tmp | PWMState[0].MaskOut | (PWMState[0].Mask<<8);
setReg(PWMA_PMOUT,val);
old_status0 = status0;
}
void agc(
Word16 *sig_in, /* (i) : postfilter input signal */
Word16 *sig_out, /* (i/o) : postfilter output signal */
Word16 l_trm /* (i) : subframe size */
)
{
#if 0
static Word16 past_gain=4096; /* past_gain = 1.0 (Q12) */
#endif
Word16 i, exp;
Word16 gain_in, gain_out, g0, gain; /* Q12 */
Word32 s;
Word16 signal[L_SUBFR];
/* calculate gain_out with exponent */
for(i=0; i<l_trm; i++)
signal[i] = shr(sig_out[i], 2);
s = 0;
for(i=0; i<l_trm; i++)
s = L_mac(s, signal[i], signal[i]);
if (s == 0) {
pg729dec->ppost_filt->past_gain = 0;
return;
}
exp = sub(norm_l(s), 1);
if(exp>0)
gain_out = round(L_shl(s, exp));
else
gain_out = round(L_shr(s, abs_s(exp)));
/* calculate gain_in with exponent */
for(i=0; i<l_trm; i++)
signal[i] = shr(sig_in[i], 2);
s = 0;
for(i=0; i<l_trm; i++)
s = L_mac(s, signal[i], signal[i]);
if (s == 0) {
g0 = 0;
}
else {
i = norm_l(s);
if(i>=0)
gain_in = round(L_shl(s, i));
else
gain_in = round(L_shr(s, abs_s(i)));
exp = sub(exp, i);
/*---------------------------------------------------*
* g0(Q12) = (1-AGC_FAC) * sqrt(gain_in/gain_out); *
*---------------------------------------------------*/
s = L_deposit_l(div_s(gain_out,gain_in)); /* Q15 */
s = L_shl(s, 7); /* s(Q22) = gain_out / gain_in */
if(exp>0)
s = L_shr(s, exp); /* Q22, add exponent */
else
s = L_shl(s, abs_s(exp));
/* i(Q12) = s(Q19) = 1 / sqrt(s(Q22)) */
s = Inv_sqrt(s); /* Q19 */
i = round(L_shl(s,9)); /* Q12 */
/* g0(Q12) = i(Q12) * (1-AGC_FAC)(Q15) */
g0 = mult(i, AGC_FAC1); /* Q12 */
}
/* compute gain(n) = AGC_FAC gain(n-1) + (1-AGC_FAC)gain_in/gain_out */
/* sig_out(n) = gain(n) sig_out(n) */
gain = pg729dec->ppost_filt->past_gain;
for(i=0; i<l_trm; i++) {
gain = mult(gain, AGC_FAC);
gain = add(gain, g0);
sig_out[i] = extract_h(L_shl(L_mult(sig_out[i], gain), 3));
}
pg729dec->ppost_filt->past_gain = gain;
return;
}
void A_Refl(
Word16 a[], /* i : Directform coefficients */
Word16 refl[], /* o : Reflection coefficients */
Flag *pOverflow
)
{
/* local variables */
Word16 i;
Word16 j;
Word16 aState[M];
Word16 bState[M];
Word16 normShift;
Word16 normProd;
Word32 L_acc;
Word16 scale;
Word32 L_temp;
Word16 temp;
Word16 mult;
/* initialize states */
for (i = 0; i < M; i++)
{
aState[i] = a[i];
}
/* backward Levinson recursion */
for (i = M - 1; i >= 0; i--)
{
if (abs_s(aState[i]) >= 4096)
{
for (i = 0; i < M; i++)
{
refl[i] = 0;
}
break;
}
refl[i] = shl(aState[i], 3, pOverflow);
L_temp = L_mult(refl[i], refl[i], pOverflow);
L_acc = L_sub(MAX_32, L_temp, pOverflow);
normShift = norm_l(L_acc);
scale = sub(15, normShift, pOverflow);
L_acc = L_shl(L_acc, normShift, pOverflow);
normProd = pv_round(L_acc, pOverflow);
mult = div_s(16384, normProd);
for (j = 0; j < i; j++)
{
L_acc = L_deposit_h(aState[j]);
L_acc = L_msu(L_acc, refl[i], aState[i-j-1], pOverflow);
temp = pv_round(L_acc, pOverflow);
L_temp = L_mult(mult, temp, pOverflow);
L_temp = L_shr_r(L_temp, scale, pOverflow);
if (L_abs(L_temp) > 32767)
{
for (i = 0; i < M; i++)
{
refl[i] = 0;
}
break;
}
bState[j] = extract_l(L_temp);
}
for (j = 0; j < i; j++)
{
aState[j] = bState[j];
}
}
return;
}
/*-----------------------------------------------------------*
* 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;
}
/***************************************************************************
Function: vector_huffman
Syntax: Word16 vector_huffman(Word16 category,
Word16 power_index,
Word16 *raw_mlt_ptr,
UWord32 *word_ptr)
inputs: Word16 category
Word16 power_index
Word16 *raw_mlt_ptr
outputs: number_of_region_bits
*word_ptr
Description: Huffman encoding for each region based on category and power_index
WMOPS: 7kHz | 24kbit | 32kbit
-------|--------------|----------------
AVG | 0.03 | 0.03
-------|--------------|----------------
MAX | 0.04 | 0.04
-------|--------------|----------------
14kHz | 24kbit | 32kbit | 48kbit
-------|--------------|----------------|----------------
AVG | 0.03 | 0.03 | 0.03
-------|--------------|----------------|----------------
MAX | 0.04 | 0.04 | 0.04
-------|--------------|----------------|----------------
***************************************************************************/
Word16 vector_huffman(Word16 category,
Word16 power_index,
Word16 *raw_mlt_ptr,
UWord32 *word_ptr)
{
Word16 inv_of_step_size_times_std_dev;
Word16 j,n;
Word16 k;
Word16 number_of_region_bits;
Word16 number_of_non_zero;
Word16 vec_dim;
Word16 num_vecs;
Word16 kmax, kmax_plus_one;
Word16 index,signs_index;
Word16 *bitcount_table_ptr;
UWord16 *code_table_ptr;
Word32 code_bits;
Word16 number_of_code_bits;
UWord32 current_word;
Word16 current_word_bits_free;
Word32 acca;
Word32 accb;
Word16 temp;
Word16 mytemp; /* new variable in Release 1.2 */
Word16 myacca; /* new variable in Release 1.2 */
/* initialize variables */
vec_dim = vector_dimension[category];
move16();
num_vecs = number_of_vectors[category];
move16();
kmax = max_bin[category];
move16();
kmax_plus_one = add(kmax,1);
move16();
current_word = 0L;
move16();
current_word_bits_free = 32;
move16();
number_of_region_bits = 0;
move16();
/* set up table pointers */
bitcount_table_ptr = (Word16 *)table_of_bitcount_tables[category];
code_table_ptr = (UWord16 *) table_of_code_tables[category];
/* compute inverse of step size * standard deviation */
acca = L_mult(step_size_inverse_table[category],standard_deviation_inverse_table[power_index]);
acca = L_shr_nocheck(acca,1);
acca = L_add(acca,4096);
acca = L_shr_nocheck(acca,13);
/*
* The next two lines are new to Release 1.2
*/
mytemp = (Word16)(acca & 0x3);
acca = L_shr_nocheck(acca,2);
inv_of_step_size_times_std_dev = extract_l(acca);
for (n=0; n<num_vecs; n++)
{
index = 0;
move16();
signs_index = 0;
move16();
number_of_non_zero = 0;
move16();
for (j=0; j<vec_dim; j++)
{
k = abs_s(*raw_mlt_ptr);
acca = L_mult(k,inv_of_step_size_times_std_dev);
acca = L_shr_nocheck(acca,1);
/*
* The next four lines are new to Release 1.2
*/
myacca = (Word16)L_mult(k,mytemp);
myacca = (Word16)L_shr_nocheck(myacca,1);
myacca = (Word16)L_add(myacca,int_dead_zone_low_bits[category]);
myacca = (Word16)L_shr_nocheck(myacca,2);
acca = L_add(acca,int_dead_zone[category]);
/*
* The next two lines are new to Release 1.2
*/
acca = L_add(acca,myacca);
acca = L_shr_nocheck(acca,13);
k = extract_l(acca);
test();
if (k != 0)
{
number_of_non_zero = add(number_of_non_zero,1);
signs_index = shl_nocheck(signs_index,1);
test();
if (*raw_mlt_ptr > 0)
{
signs_index = add(signs_index,1);
}
temp = sub(k,kmax);
test();
if (temp > 0)
{
k = kmax;
move16();
}
}
acca = L_shr_nocheck(L_mult(index,(kmax_plus_one)),1);
index = extract_l(acca);
index = add(index,k);
raw_mlt_ptr++;
}
code_bits = *(code_table_ptr+index);
number_of_code_bits = add((*(bitcount_table_ptr+index)),number_of_non_zero);
number_of_region_bits = add(number_of_region_bits,number_of_code_bits);
acca = code_bits << number_of_non_zero;
accb = L_deposit_l(signs_index);
acca = L_add(acca,accb);
code_bits = acca;
move32();
/* msb of codebits is transmitted first. */
j = sub(current_word_bits_free,number_of_code_bits);
test();
if (j >= 0)
{
test();
acca = code_bits << j;
current_word = L_add(current_word,acca);
current_word_bits_free = j;
move16();
}
else
{
j = negate(j);
acca = L_shr_nocheck(code_bits,j);
current_word = L_add(current_word,acca);
*word_ptr++ = current_word;
move16();
current_word_bits_free = sub(32,j);
test();
current_word = code_bits << current_word_bits_free;
}
}
*word_ptr++ = current_word;
move16();
return (number_of_region_bits);
}
/*
**************************************************************************
*
* Function : Az_lsp
* Purpose : Compute the LSPs from the LP coefficients
*
**************************************************************************
*/
void Az_lsp (
Word16 a[], /* (i) : predictor coefficients (MP1) */
Word16 lsp[], /* (o) : line spectral pairs (M) */
Word16 old_lsp[] /* (i) : old lsp[] (in case not found 10 roots) (M) */
)
{
Word16 i, j, nf, ip;
Word16 xlow, ylow, xhigh, yhigh, xmid, ymid, xint;
Word16 x, y, sign, exp;
Word16 *coef;
Word16 f1[M / 2 + 1], f2[M / 2 + 1];
Word32 t0;
/*-------------------------------------------------------------*
* find the sum and diff. pol. F1(z) and F2(z) *
* F1(z) <--- F1(z)/(1+z**-1) & F2(z) <--- F2(z)/(1-z**-1) *
* *
* f1[0] = 1.0; *
* f2[0] = 1.0; *
* *
* for (i = 0; i< NC; i++) *
* { *
* f1[i+1] = a[i+1] + a[M-i] - f1[i] ; *
* f2[i+1] = a[i+1] - a[M-i] + f2[i] ; *
* } *
*-------------------------------------------------------------*/
f1[0] = 1024; move16 (); /* f1[0] = 1.0 */
f2[0] = 1024; move16 (); /* f2[0] = 1.0 */
for (i = 0; i < NC; i++)
{
t0 = L_mult (a[i + 1], 8192); /* x = (a[i+1] + a[M-i]) >> 2 */
t0 = L_mac (t0, a[M - i], 8192);
x = extract_h (t0);
/* f1[i+1] = a[i+1] + a[M-i] - f1[i] */
f1[i + 1] = sub (x, f1[i]);move16 ();
t0 = L_mult (a[i + 1], 8192); /* x = (a[i+1] - a[M-i]) >> 2 */
t0 = L_msu (t0, a[M - i], 8192);
x = extract_h (t0);
/* f2[i+1] = a[i+1] - a[M-i] + f2[i] */
f2[i + 1] = add (x, f2[i]);move16 ();
}
/*-------------------------------------------------------------*
* find the LSPs using the Chebychev pol. evaluation *
*-------------------------------------------------------------*/
nf = 0; move16 (); /* number of found frequencies */
ip = 0; move16 (); /* indicator for f1 or f2 */
coef = f1; move16 ();
xlow = grid[0]; move16 ();
ylow = Chebps (xlow, coef, NC);move16 ();
j = 0;
test (); test ();
/* while ( (nf < M) && (j < grid_points) ) */
while ((sub (nf, M) < 0) && (sub (j, grid_points) < 0))
{
j++;
xhigh = xlow; move16 ();
yhigh = ylow; move16 ();
xlow = grid[j]; move16 ();
ylow = Chebps (xlow, coef, NC);
move16 ();
test ();
if (L_mult (ylow, yhigh) <= (Word32) 0L)
{
/* divide 4 times the interval */
for (i = 0; i < 4; i++)
{
/* xmid = (xlow + xhigh)/2 */
xmid = add (shr (xlow, 1), shr (xhigh, 1));
ymid = Chebps (xmid, coef, NC);
move16 ();
test ();
if (L_mult (ylow, ymid) <= (Word32) 0L)
{
yhigh = ymid; move16 ();
xhigh = xmid; move16 ();
}
else
{
ylow = ymid; move16 ();
xlow = xmid; move16 ();
}
}
/*-------------------------------------------------------------*
* Linear interpolation *
* xint = xlow - ylow*(xhigh-xlow)/(yhigh-ylow); *
*-------------------------------------------------------------*/
x = sub (xhigh, xlow);
y = sub (yhigh, ylow);
test ();
if (y == 0)
{
xint = xlow; move16 ();
}
else
{
sign = y; move16 ();
y = abs_s (y);
exp = norm_s (y);
y = shl (y, exp);
y = div_s ((Word16) 16383, y);
t0 = L_mult (x, y);
t0 = L_shr (t0, sub (20, exp));
y = extract_l (t0); /* y= (xhigh-xlow)/(yhigh-ylow) */
test ();
if (sign < 0)
y = negate (y);
t0 = L_mult (ylow, y);
t0 = L_shr (t0, 11);
xint = sub (xlow, extract_l (t0)); /* xint = xlow - ylow*y */
}
lsp[nf] = xint; move16 ();
xlow = xint; move16 ();
nf++;
test ();
if (ip == 0)
{
ip = 1; move16 ();
coef = f2; move16 ();
}
else
{
ip = 0; move16 ();
coef = f1; move16 ();
}
ylow = Chebps (xlow, coef, NC);
move16 ();
}
test (); test ();
}
/* Check if M roots found */
test ();
if (sub (nf, M) < 0)
{
for (i = 0; i < M; i++)
{
lsp[i] = old_lsp[i]; move16 ();
}
}
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;
}