/*-------------------------------------------------------------------*
* Function set_sign() *
* ~~~~~~~~~~~~~~~~~~~~ *
* Set the sign of each pulse position. *
*-------------------------------------------------------------------*/
static void set_sign(
Word16 fac_cn, /* (i) Q15: residual weight for sign determination */
Word16 cn[], /* (i) Q0 : residual after long term prediction */
Word16 dn[], /* (i) Q0 : correlation between target and h[] */
Word16 sign[], /* (o) Q15: sign vector (sign of each position) */
Word16 inv_sign[], /* (o) Q15: inverse of sign[] */
Word16 pos_max[], /* (o) : pos of max of correlation */
Word32 corr[] /* (o) : correlation of each track */
)
{
Word16 i, k, pos, k_cn, k_dn, val;
Word32 s, max;
/* calculate energy for normalization of cn[] and dn[] */
s = 0;
for (i=0; i<L_SUBFR; i++) s = L_mac(s, cn[i], cn[i]);
if (s < 512) s = 512;
s = Inv_sqrt(s);
k_cn = extract_h(L_shl(s, 5)); /* k_cn = 11..23170 */
k_cn = mult(k_cn, fac_cn);
s = 0;
for (i=0; i<L_SUBFR; i++) s = L_mac(s, dn[i], dn[i]);
if (s < 512) s = 512;
s = Inv_sqrt(s);
k_dn = extract_h(L_shl(s, 5)); /* k_dn = 11..23170 */
/* set sign according to en[] = k_cn*cn[] + k_dn*dn[] */
/* find position of maximum of correlation in each track */
for (k=0; k<NB_TRACK; k++) {
max = -1;
for (i=k; i<L_SUBFR; i+=STEP) {
val = dn[i];
s = L_mac(L_mult(k_cn, cn[i]), k_dn, val);
if (s >= 0) {
sign[i] = 32767L; /* sign = +1 (Q15) */
inv_sign[i] = -32768L;
}
else {
sign[i] = -32768L; /* sign = -1 (Q15) */
inv_sign[i] = 32767L;
val = negate(val);
}
dn[i] = val; /* modify dn[] according to the fixed sign */
s = L_abs(s);
if (s > max) {
max = s;
pos = i;
}
}
pos_max[k] = pos;
corr[k] = max;
}
return;
}
static Word16 Lag_max( /* output: lag found */
Word16 signal[], /* input : signal used to compute the open loop pitch */
Word16 L_frame, /* input : length of frame to compute pitch */
Word16 lag_max, /* input : maximum lag */
Word16 lag_min, /* input : minimum lag */
Word16 *cor_max) /* output: normalized correlation of selected lag */
{
Word16 i, j;
Word16 *p, *p1;
Word32 max, t0, L_temp;
Word16 max_h, max_l, ener_h, ener_l;
Word16 p_max;
max = MIN_32;
/* initialization used only to suppress Microsoft Visual C++ warnings */
p_max = lag_max;
for (i = lag_max; i >= lag_min; i--)
{
p = signal;
p1 = &signal[-i];
t0 = 0;
for (j=0; j<L_frame; j++, p++, p1++)
t0 = L_mac(t0, *p, *p1);
L_temp = L_sub(t0,max);
if (L_temp >= 0L)
{
max = t0;
p_max = i;
}
}
/* compute energy */
t0 = 0;
p = &signal[-p_max];
for(i=0; i<L_frame; i++, p++)
t0 = L_mac(t0, *p, *p);
/* 1/sqrt(energy), result in Q30 */
t0 = Inv_sqrt(t0);
/* max = max/sqrt(energy) */
/* This result will always be on 16 bits !! */
L_Extract(max, &max_h, &max_l);
L_Extract(t0, &ener_h, &ener_l);
t0 = Mpy_32(max_h, max_l, ener_h, ener_l);
*cor_max = extract_l(t0);
return(p_max);
}
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;
}
static void Norm_Corr(Word16 exc[], Word16 xn[], Word16 h[], Word16 L_subfr,
Word16 t_min, Word16 t_max, Word16 corr_norm[])
{
Word16 i,j,k;
Word16 corr_h, corr_l, norm_h, norm_l;
Word32 s, L_temp;
Word16 excf[L_SUBFR];
Word16 scaling, h_fac, *s_excf, scaled_excf[L_SUBFR];
k = negate(t_min);
/* compute the filtered excitation for the first delay t_min */
Convolve(&exc[k], h, excf, L_subfr);
/* scaled "excf[]" to avoid overflow */
for(j=0; j<L_subfr; j++)
scaled_excf[j] = shr(excf[j], 2);
/* Compute energy of excf[] for danger of overflow */
s = 0;
for (j = 0; j < L_subfr; j++)
s = L_mac(s, excf[j], excf[j]);
L_temp = L_sub(s, 67108864L);
if (L_temp <= 0L) /* if (s <= 2^26) */
{
s_excf = excf;
h_fac = 15-12; /* h in Q12 */
scaling = 0;
}
else {
s_excf = scaled_excf; /* "excf[]" is divide by 2 */
h_fac = 15-12-2; /* h in Q12, divide by 2 */
scaling = 2;
}
/* loop for every possible period */
for (i = t_min; i <= t_max; i++)
{
/* Compute 1/sqrt(energy of excf[]) */
s = 0;
for (j = 0; j < L_subfr; j++)
s = L_mac(s, s_excf[j], s_excf[j]);
s = Inv_sqrt(s); /* Result in Q30 */
L_Extract(s, &norm_h, &norm_l);
/* Compute correlation between xn[] and excf[] */
s = 0;
for (j = 0; j < L_subfr; j++)
s = L_mac(s, xn[j], s_excf[j]);
L_Extract(s, &corr_h, &corr_l);
/* Normalize correlation = correlation * (1/sqrt(energy)) */
s = Mpy_32(corr_h, corr_l, norm_h, norm_l);
corr_norm[i] = extract_h(L_shl(s, 16)); /* Result is on 16 bits */
/* modify the filtered excitation excf[] for the next iteration */
if( sub(i, t_max) != 0)
{
k=sub(k,1);
for (j = L_subfr-(Word16)1; j > 0; j--)
{
s = L_mult(exc[k], h[j]);
s = L_shl(s, h_fac); /* h is in Q(12-scaling) */
s_excf[j] = add(extract_h(s), s_excf[j-1]);
}
s_excf[0] = shr(exc[k], scaling);
}
}
return;
}
/*************************************************************************
*
* FUNCTION: Lag_max
*
* PURPOSE: Find the lag that has maximum correlation of scal_sig[] in a
* given delay range.
*
* DESCRIPTION:
* The correlation is given by
* cor[t] = <scal_sig[n],scal_sig[n-t]>, t=lag_min,...,lag_max
* The functions outputs the maximum correlation after normalization
* and the corresponding lag.
*
*************************************************************************/
Word16 Lag_max ( /* o : lag found */
vadState1 *vadSt, /* i/o : VAD state struct */
Word32 corr[], /* i : correlation vector. */
Word16 scal_sig[], /* i : scaled signal. */
Word16 scal_fac, /* i : scaled signal factor. */
Word16 scal_flag, /* i : if 1 use EFR compatible scaling */
Word16 L_frame, /* i : length of frame to compute pitch */
Word16 lag_max, /* i : maximum lag */
Word16 lag_min, /* i : minimum lag */
Word16 *cor_max, /* o : normalized correlation of selected lag */
Flag dtx /* i : dtx flag; use dtx=1, do not use dtx=0 */
)
{
Word16 i, j;
Word16 *p;
Word32 max, t0;
Word16 max_h, max_l, ener_h, ener_l;
Word16 p_max = 0; /* initialization only needed to keep gcc silent */
long lTemp;
max = MIN_32;
p_max = lag_max;
for (i = lag_max, j = (PIT_MAX-lag_max-1); i >= lag_min; i--, j--)
{
if (corr[-i] >= max)
{
max = corr[-i];
p_max = i;
}
}
/* compute energy */
t0 = 0;
p = &scal_sig[-p_max];
for (i = 0; i < L_frame; i++, p++)
{
//t0 = L_mac (t0, *p, *p);
lTemp = (*p) * (*p);
if (lTemp != 0x40000000)
{
lTemp = lTemp * 2;
}
else
{
lTemp = MAX_32;
}
lTemp = lTemp + t0;
if (lTemp > MAX_32)
{
t0 = MAX_32;
}
else if (lTemp < MIN_32)
{
t0 = MIN_32;
}
else
{
t0 = (Word32)lTemp;
}
}
/* 1/sqrt(energy) */
if (dtx)
{ /* no test() call since this if is only in simulation env */
/* check tone */
vad_tone_detection (vadSt, max, t0);
}
t0 = Inv_sqrt (t0); /* function result */
if (scal_flag)
{
t0 = L_shl (t0, 1);
}
/* max = max/sqrt(energy) */
L_Extract (max, &max_h, &max_l);
L_Extract (t0, &ener_h, &ener_l);
t0 = Mpy_32 (max_h, max_l, ener_h, ener_l);
if (scal_flag)
{
t0 = L_shr (t0, scal_fac);
*cor_max = extract_h (L_shl (t0, 15)); /* divide by 2 */
}
else
{
*cor_max = extract_l(t0);
}
return (p_max);
}
/*************************************************************************
*
* FUNCTION set_sign12k2()
*
* PURPOSE: Builds sign[] vector according to "dn[]" and "cn[]", and modifies
* dn[] to include the sign information (dn[i]=sign[i]*dn[i]).
* Also finds the position of maximum of correlation in each track
* and the starting position for each pulse.
*
*************************************************************************/
void set_sign12k2 (
Word16 dn[], /* i/o : correlation between target and h[] */
Word16 cn[], /* i : residual after long term prediction */
Word16 sign[], /* o : sign of d[n] */
Word16 pos_max[], /* o : position of maximum correlation */
Word16 nb_track, /* i : number of tracks tracks */
Word16 ipos[], /* o : starting position for each pulse */
Word16 step /* i : the step size in the tracks */
)
{
Word16 i, j;
Word16 val, cor, k_cn, k_dn, max, max_of_all;
Word16 pos = 0; /* initialization only needed to keep gcc silent */
Word16 en[L_CODE]; /* correlation vector */
Word32 s;
/* calculate energy for normalization of cn[] and dn[] */
s = 256; move32 ();
for (i = 0; i < L_CODE; i++)
{
s = L_mac (s, cn[i], cn[i]);
}
s = Inv_sqrt (s); move32 ();
k_cn = extract_h (L_shl (s, 5));
s = 256; move32 ();
for (i = 0; i < L_CODE; i++)
{
s = L_mac (s, dn[i], dn[i]);
}
s = Inv_sqrt (s); move32 ();
k_dn = extract_h (L_shl (s, 5));
for (i = 0; i < L_CODE; i++)
{
val = dn[i]; move16 ();
cor = round (L_shl (L_mac (L_mult (k_cn, cn[i]), k_dn, val), 10));
test ();
if (cor >= 0)
{
sign[i] = 32767; move16 (); /* sign = +1 */
}
else
{
sign[i] = -32767; move16 (); /* sign = -1 */
cor = negate (cor);
val = negate (val);
}
/* modify dn[] according to the fixed sign */
dn[i] = val; move16 ();
en[i] = cor; move16 ();
}
max_of_all = -1; move16 ();
for (i = 0; i < nb_track; i++)
{
max = -1; move16 ();
for (j = i; j < L_CODE; j += step)
{
cor = en[j]; move16 ();
val = sub (cor, max);
test ();
if (val > 0)
{
max = cor; move16 ();
pos = j; move16 ();
}
}
/* store maximum correlation position */
pos_max[i] = pos; move16 ();
val = sub (max, max_of_all);
test ();
if (val > 0)
{
max_of_all = max; move16 ();
/* starting position for i0 */
ipos[0] = i; move16 ();
}
}
/*----------------------------------------------------------------*
* Set starting position of each pulse. *
*----------------------------------------------------------------*/
pos = ipos[0]; move16 ();
ipos[nb_track] = pos; move16 ();
for (i = 1; i < nb_track; i++)
{
pos = add (pos, 1);
test ();
if (sub (pos, nb_track) >= 0)
{
pos = 0; move16 ();
}
ipos[i] = pos; move16 ();
ipos[add(i, nb_track)] = pos; move16 ();
}
}
Word16 Pitch_ol_fast( /* output: open loop pitch lag */
Word16 signal[], /* input : signal used to compute the open loop pitch */
/* signal[-pit_max] to signal[-1] should be known */
Word16 pit_max, /* input : maximum pitch lag */
Word16 L_frame /* input : length of frame to compute pitch */
)
{
Word16 i, j;
Word16 max1, max2, max3;
Word16 max_h, max_l, ener_h, ener_l;
Word16 T1, T2, T3;
Word16 *p, *p1;
Word32 max, sum, L_temp;
/* Scaled signal */
Word16 scaled_signal[L_FRAME+PIT_MAX];
Word16 *scal_sig;
scal_sig = &scaled_signal[pit_max];
/*--------------------------------------------------------*
* Verification for risk of overflow. *
*--------------------------------------------------------*/
Overflow = 0;
sum = 0;
for(i= -pit_max; i< L_frame; i+=2)
sum = L_mac(sum, signal[i], signal[i]);
/*--------------------------------------------------------*
* Scaling of input signal. *
* *
* if Overflow -> scal_sig[i] = signal[i]>>3 *
* else if sum < 1^20 -> scal_sig[i] = signal[i]<<3 *
* else -> scal_sig[i] = signal[i] *
*--------------------------------------------------------*/
if(Overflow == 1)
{
for(i=-pit_max; i<L_frame; i++)
{
scal_sig[i] = shr(signal[i], 3);
}
}
else {
L_temp = L_sub(sum, (Word32)1048576L);
if ( L_temp < (Word32)0 ) /* if (sum < 2^20) */
{
for(i=-pit_max; i<L_frame; i++)
{
scal_sig[i] = shl(signal[i], 3);
}
}
else
{
for(i=-pit_max; i<L_frame; i++)
{
scal_sig[i] = signal[i];
}
}
}
/*--------------------------------------------------------------------*
* The pitch lag search is divided in three sections. *
* Each section cannot have a pitch multiple. *
* We find a maximum for each section. *
* We compare the maxima of each section by favoring small lag. *
* *
* First section: lag delay = 20 to 39 *
* Second section: lag delay = 40 to 79 *
* Third section: lag delay = 80 to 143 *
*--------------------------------------------------------------------*/
/* First section */
max = MIN_32;
T1 = 20; /* Only to remove warning from some compilers */
for (i = 20; i < 40; i++) {
p = scal_sig;
p1 = &scal_sig[-i];
sum = 0;
for (j=0; j<L_frame; j+=2, p+=2, p1+=2)
sum = L_mac(sum, *p, *p1);
L_temp = L_sub(sum, max);
if (L_temp > 0) { max = sum; T1 = i; }
}
/* compute energy of maximum */
sum = 1; /* to avoid division by zero */
p = &scal_sig[-T1];
for(i=0; i<L_frame; i+=2, p+=2)
sum = L_mac(sum, *p, *p);
/* max1 = max/sqrt(energy) */
/* This result will always be on 16 bits !! */
sum = Inv_sqrt(sum); /* 1/sqrt(energy), result in Q30 */
L_Extract(max, &max_h, &max_l);
L_Extract(sum, &ener_h, &ener_l);
sum = Mpy_32(max_h, max_l, ener_h, ener_l);
max1 = extract_l(sum);
/* Second section */
max = MIN_32;
T2 = 40; /* Only to remove warning from some compilers */
for (i = 40; i < 80; i++) {
p = scal_sig;
p1 = &scal_sig[-i];
sum = 0;
for (j=0; j<L_frame; j+=2, p+=2, p1+=2)
sum = L_mac(sum, *p, *p1);
L_temp = L_sub(sum, max);
if (L_temp > 0) { max = sum; T2 = i; }
}
/* compute energy of maximum */
sum = 1; /* to avoid division by zero */
p = &scal_sig[-T2];
for(i=0; i<L_frame; i+=2, p+=2)
sum = L_mac(sum, *p, *p);
/* max2 = max/sqrt(energy) */
/* This result will always be on 16 bits !! */
sum = Inv_sqrt(sum); /* 1/sqrt(energy), result in Q30 */
L_Extract(max, &max_h, &max_l);
L_Extract(sum, &ener_h, &ener_l);
sum = Mpy_32(max_h, max_l, ener_h, ener_l);
max2 = extract_l(sum);
/* Third section */
max = MIN_32;
T3 = 80; /* Only to remove warning from some compilers */
for (i = 80; i < 143; i+=2) {
p = scal_sig;
p1 = &scal_sig[-i];
sum = 0;
for (j=0; j<L_frame; j+=2, p+=2, p1+=2)
sum = L_mac(sum, *p, *p1);
L_temp = L_sub(sum, max);
if (L_temp > 0) { max = sum; T3 = i; }
}
/* Test around max3 */
i = T3;
p = scal_sig;
p1 = &scal_sig[-(i+1)];
sum = 0;
for (j=0; j<L_frame; j+=2, p+=2, p1+=2)
sum = L_mac(sum, *p, *p1);
L_temp = L_sub(sum, max);
if (L_temp > 0) { max = sum; T3 = i+(Word16)1; }
p = scal_sig;
p1 = &scal_sig[-(i-1)];
sum = 0;
for (j=0; j<L_frame; j+=2, p+=2, p1+=2)
sum = L_mac(sum, *p, *p1);
L_temp = L_sub(sum, max);
if (L_temp > 0) { max = sum; T3 = i-(Word16)1; }
/* compute energy of maximum */
sum = 1; /* to avoid division by zero */
p = &scal_sig[-T3];
for(i=0; i<L_frame; i+=2, p+=2)
sum = L_mac(sum, *p, *p);
/* max1 = max/sqrt(energy) */
/* This result will always be on 16 bits !! */
sum = Inv_sqrt(sum); /* 1/sqrt(energy), result in Q30 */
L_Extract(max, &max_h, &max_l);
L_Extract(sum, &ener_h, &ener_l);
sum = Mpy_32(max_h, max_l, ener_h, ener_l);
max3 = extract_l(sum);
/*-----------------------*
* Test for multiple. *
*-----------------------*/
/* if( abs(T2*2 - T3) < 5) */
/* max2 += max3 * 0.25; */
i = sub(shl(T2,1), T3);
j = sub(abs_s(i), 5);
if(j < 0)
max2 = add(max2, shr(max3, 2));
/* if( abs(T2*3 - T3) < 7) */
/* max2 += max3 * 0.25; */
i = add(i, T2);
j = sub(abs_s(i), 7);
if(j < 0)
max2 = add(max2, shr(max3, 2));
/* if( abs(T1*2 - T2) < 5) */
/* max1 += max2 * 0.20; */
i = sub(shl(T1,1), T2);
j = sub(abs_s(i), 5);
if(j < 0)
max1 = add(max1, mult(max2, 6554));
/* if( abs(T1*3 - T2) < 7) */
/* max1 += max2 * 0.20; */
i = add(i, T1);
j = sub(abs_s(i), 7);
if(j < 0)
max1 = add(max1, mult(max2, 6554));
/*--------------------------------------------------------------------*
* Compare the 3 sections maxima. *
*--------------------------------------------------------------------*/
if( sub(max1, max2) < 0 ) {max1 = max2; T1 = T2; }
if( sub(max1, max3) <0 ) {T1 = T3; }
return T1;
}
/*-------------------------------------------------------------------*
* Function cor_h_e() *
* ~~~~~~~~~~~~~~~~~ *
* Compute correlations of h[] needed for the codebook search. *
*-------------------------------------------------------------------*/
static void cor_h_e(
Word16 H[], /* (i) Q12 :Impulse response of filters */
Word16 sign[], /* (i) Q15: sign vector */
Word16 inv_sign[], /* (i) Q15: inverse of sign[] */
Word16 h[], /* (o) : scaled h[] */
Word16 h_inv[], /* (o) : inverse of scaled h[] */
Word16 rrixix[][NB_POS], /* (o) energy of h[]. */
Word16 rrixiy[][MSIZE] /* (o) correlation between 2 pulses. */
)
{
Word16 i, j, k, pos;
Word16 *ptr_h1, *ptr_h2, *ptr_hf, *psign;
Word16 *p0, *p1, *p2, *p3, *p4;
Word32 cor;
/*------------------------------------------------------------*
* normalize h[] for maximum precision on correlation. *
*------------------------------------------------------------*/
cor = 0;
for(i=0; i<L_SUBFR; i++) cor = L_mac(cor, H[i], H[i]);
/* scale h[] with shift operation */
k = norm_l(cor);
k = shr(k, 1);
for(i=0; i<L_SUBFR; i++) h[i] = shl(H[i], k);
cor = L_shl(cor, add(k, k));
/*------------------------------------------------------------*
* Scaling h[] with a factor (0.5 < fac < 0.25) *
* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ *
* extract_h(cor) = 8192 .. 32768 --> scale to 4096 (1/8 Q15) *
* *
* 4096 (1/8) = fac^2 * extract_h(cor) *
* fac = sqrt(4096/extract_h(cor)) *
* *
* fac = 1/sqrt(cor/4096) * 256 = 0.125 to 0.5 *
*------------------------------------------------------------*/
cor = L_shr(cor, 12);
k = extract_h(L_shl(Inv_sqrt(cor), 8));
for(i=0; i<L_SUBFR; i++) {
h[i] = mult(h[i], k);
h_inv[i] = negate(h[i]);
}
/*------------------------------------------------------------*
* Compute rrixix[][] needed for the codebook search. *
* This algorithm compute impulse response energy of all *
* positions (8) in each track (5). Total = 5x8 = 40. *
*------------------------------------------------------------*/
/* storage order --> i4i4, i3i3, i2i2, i1i1, i0i0 */
/* Init pointers to last position of rrixix[] */
p0 = &rrixix[0][NB_POS-1];
p1 = &rrixix[1][NB_POS-1];
p2 = &rrixix[2][NB_POS-1];
p3 = &rrixix[3][NB_POS-1];
p4 = &rrixix[4][NB_POS-1];
ptr_h1 = h;
cor = 0x00010000L; /* 1.0 (for rounding) */
for(i=0; i<NB_POS; i++) {
cor = L_mac(cor, *ptr_h1, *ptr_h1); ptr_h1++;
*p4-- = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h1); ptr_h1++;
*p3-- = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h1); ptr_h1++;
*p2-- = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h1); ptr_h1++;
*p1-- = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h1); ptr_h1++;
*p0-- = extract_h(cor);
}
/* Divide all elements of rrixix[][] by 2. */
p0 = &rrixix[0][0];
for(i=0; i<L_SUBFR; i++)
{
*p0 = shr(*p0, 1);
p0++;
}
/*------------------------------------------------------------*
* Compute rrixiy[][] needed for the codebook search. *
* This algorithm compute correlation between 2 pulses *
* (2 impulses responses) in 5 possible adjacents tracks. *
* (track 0-1, 1-2, 2-3, 3-4 and 4-0). Total = 5x8x8 = 320. *
*------------------------------------------------------------*/
/* storage order --> i3i4, i2i3, i1i2, i0i1, i4i0 */
pos = MSIZE-1;
ptr_hf = h + 1;
for(k=0; k<NB_POS; k++) {
p4 = &rrixiy[3][pos];
p3 = &rrixiy[2][pos];
p2 = &rrixiy[1][pos];
p1 = &rrixiy[0][pos];
p0 = &rrixiy[4][pos-NB_POS];
cor = 0x00008000L; /* 0.5 (for rounding) */
ptr_h1 = h;
ptr_h2 = ptr_hf;
for(i=k+(Word16)1; i<NB_POS; i++ ) {
cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
*p4 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
*p3 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
*p2 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
*p1 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
*p0 = extract_h(cor);
p4 -= (NB_POS+1);
p3 -= (NB_POS+1);
p2 -= (NB_POS+1);
p1 -= (NB_POS+1);
p0 -= (NB_POS+1);
}
cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
*p4 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
*p3 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
*p2 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
*p1 = extract_h(cor);
pos -= NB_POS;
ptr_hf += STEP;
}
/* storage order --> i4i0, i3i4, i2i3, i1i2, i0i1 */
pos = MSIZE-1;
ptr_hf = h + 4;
for(k=0; k<NB_POS; k++) {
p4 = &rrixiy[4][pos];
p3 = &rrixiy[3][pos-1];
p2 = &rrixiy[2][pos-1];
p1 = &rrixiy[1][pos-1];
p0 = &rrixiy[0][pos-1];
cor = 0x00008000L; /* 0.5 (for rounding) */
ptr_h1 = h;
ptr_h2 = ptr_hf;
for(i=k+(Word16)1; i<NB_POS; i++ ) {
cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
*p4 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
*p3 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
*p2 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
*p1 = extract_h(cor);
cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
*p0 = extract_h(cor);
p4 -= (NB_POS+1);
p3 -= (NB_POS+1);
p2 -= (NB_POS+1);
p1 -= (NB_POS+1);
p0 -= (NB_POS+1);
}
cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
*p4 = extract_h(cor);
pos--;
ptr_hf += STEP;
}
/*------------------------------------------------------------*
* Modification of rrixiy[][] to take signs into account. *
*------------------------------------------------------------*/
p0 = &rrixiy[0][0];
for (k=0; k<NB_TRACK; k++) {
for(i=k; i<L_SUBFR; i+=STEP) {
psign = sign;
if (psign[i] < 0) psign = inv_sign;
for(j=(Word16)((k+(Word16)1)%NB_TRACK); j<(Word16)L_SUBFR; j+=(Word16)STEP) {
*p0 = mult(*p0, psign[j]); p0++;
}
}
}
return;
}
/*-----------------------------------------------------------*
* 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 agc(
int16_t *sig_in, /* (i) : postfilter input signal */
int16_t *sig_out, /* (i/o) : postfilter output signal */
int16_t l_trm /* (i) : subframe size */
)
{
static int16_t past_gain=4096; /* past_gain = 1.0 (Q12) */
int16_t i, exp;
int16_t gain_in, gain_out, g0, gain; /* Q12 */
int32_t s;
int16_t 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) {
past_gain = 0;
return;
}
exp = sub(norm_l(s), 1);
gain_out = _round(L_shl(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);
gain_in = _round(L_shl(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 */
s = L_shr(s, exp); /* Q22, add exponent */
/* 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 = 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));
}
past_gain = gain;
return;
}
static Word16 Lag_max( /* o : lag found */
vadState *vadSt, /* i/o : VAD state struct */
Word32 corr[], /* i : correlation vector. */
Word16 scal_sig[], /* i : scaled signal. */
Word16 scal_fac, /* i : scaled signal factor. */
Word16 scal_flag, /* i : if 1 use EFR compatible scaling */
Word16 L_frame, /* i : length of frame to compute pitch */
Word16 lag_max, /* i : maximum lag */
Word16 lag_min, /* i : minimum lag */
Word16 *cor_max, /* o : normalized correlation of selected lag */
Flag dtx, /* i : dtx flag; use dtx=1, do not use dtx=0 */
Flag *pOverflow /* i/o : overflow Flag */
)
#endif
{
register Word16 i;
Word16 *p;
Word32 max;
Word32 t0;
Word16 max_h;
Word16 max_l;
Word16 ener_h;
Word16 ener_l;
Word16 p_max = 0; /* initialization only needed to keep gcc silent */
Word32 L_temp;
Word32 L_temp_2;
Word32 L_temp_3;
Word32 *p_corr = &corr[-lag_max];
max = MIN_32;
p_max = lag_max;
for (i = lag_max; i >= lag_min; i--)
{
/* The negative array index is equivalent to a negative */
/* address offset, i.e., corr[-i] == *(corr - i) */
if (*(p_corr++) >= max)
{
p_corr--;
max = *(p_corr++);
p_max = i;
}
}
/* compute energy */
t0 = 0;
/* The negative array index is equivalent to a negative */
/* address offset, i.e., scal_sig[-p_max] == *(scal_sig - p_max) */
p = &scal_sig[-p_max];
for (i = (L_frame >> 2); i != 0; i--)
{
t0 = amrnb_fxp_mac_16_by_16bb((Word32) * (p), (Word32) * (p), t0);
p++;
t0 = amrnb_fxp_mac_16_by_16bb((Word32) * (p), (Word32) * (p), t0);
p++;
t0 = amrnb_fxp_mac_16_by_16bb((Word32) * (p), (Word32) * (p), t0);
p++;
t0 = amrnb_fxp_mac_16_by_16bb((Word32) * (p), (Word32) * (p), t0);
p++;
}
t0 <<= 1;
/* 1/sqrt(energy) */
if (dtx)
{ /* no test() call since this if is only in simulation env */
/* check tone */
#ifdef VAD2
*rmax = max;
*r0 = t0;
#else
/* check tone */
vad_tone_detection(vadSt, max, t0, pOverflow);
#endif
}
t0 = Inv_sqrt(t0, pOverflow);
if (scal_flag)
{
if (t0 > (Word32) 0x3fffffffL)
{
t0 = MAX_32;
}
else
{
t0 = t0 << 1;
}
}
/* max = max/sqrt(energy) */
/* The following code is an inlined version of */
/* L_Extract (max, &max_h, &max_l), i.e. */
/* */
/* *max_h = extract_h (max); */
max_h = (Word16)(max >> 16);
/* L_temp_2 = L_shr(max,1), which is used in */
/* the calculation of *max_l (see next operation) */
L_temp_2 = max >> 1;
/* *max_l = extract_l (L_msu (L_shr (max, 1), *max_h, 16384)); */
L_temp_3 = (Word32)(max_h << 15);
L_temp = L_temp_2 - L_temp_3;
max_l = (Word16)L_temp;
/* The following code is an inlined version of */
/* L_Extract (t0, &ener_h, &ener_l), i.e. */
/* */
/* *ener_h = extract_h (t0); */
ener_h = (Word16)(t0 >> 16);
/* L_temp_2 = L_shr(t0,1), which is used in */
/* the calculation of *ener_l (see next operation) */
L_temp_2 = t0 >> 1;
L_temp_3 = (Word32)(ener_h << 15);
L_temp = L_temp_2 - L_temp_3;
ener_l = (Word16)L_temp;
t0 = Mpy_32(max_h, max_l, ener_h, ener_l, pOverflow);
if (scal_flag)
{
t0 = L_shr(t0, scal_fac, pOverflow);
if (t0 > (Word32) 0X0000FFFFL)
{
*cor_max = MAX_16;
}
else if (t0 < (Word32) 0xFFFF0000L)
{
*cor_max = MIN_16;
}
else
{
*cor_max = (Word16)(t0 >> 1);
}
}
else
{
static Word16 Lag_max ( /* o : lag found */
vadState *vadSt, /* i/o : VAD state struct */
Word32 corr[], /* i : correlation vector. */
Word16 scal_sig[], /* i : scaled signal. */
Word16 scal_fac, /* i : scaled signal factor. */
Word16 scal_flag, /* i : if 1 use EFR compatible scaling */
Word16 L_frame, /* i : length of frame to compute pitch */
Word16 lag_max, /* i : maximum lag */
Word16 lag_min, /* i : minimum lag */
Word16 *cor_max, /* o : normalized correlation of selected lag */
Flag dtx /* i : dtx flag; use dtx=1, do not use dtx=0 */
)
#endif
{
Word16 i, j;
Word16 *p;
Word32 max, t0;
Word16 max_h, max_l, ener_h, ener_l;
Word16 p_max = 0; /* initialization only needed to keep gcc silent */
max = MIN_32; move32 ();
p_max = lag_max; move16 ();
for (i = lag_max, j = (PIT_MAX-lag_max-1); i >= lag_min; i--, j--)
{
test ();
if (L_sub (corr[-i], max) >= 0)
{
max = corr[-i]; move32 ();
p_max = i; move16 ();
}
}
/* compute energy */
t0 = 0; move32 ();
p = &scal_sig[-p_max]; move16 ();
for (i = 0; i < L_frame; i++, p++)
{
t0 = L_mac (t0, *p, *p);
}
/* 1/sqrt(energy) */
if (dtx)
{ /* no test() call since this if is only in simulation env */
#ifdef VAD2
*rmax = max; move32();
*r0 = t0; move32();
#else
/* check tone */
vad_tone_detection (vadSt, max, t0);
#endif
}
t0 = Inv_sqrt (t0); move32 (); /* function result */
test();
if (scal_flag)
{
t0 = L_shl (t0, 1);
}
/* max = max/sqrt(energy) */
L_Extract (max, &max_h, &max_l);
L_Extract (t0, &ener_h, &ener_l);
t0 = Mpy_32 (max_h, max_l, ener_h, ener_l);
test();
if (scal_flag)
{
t0 = L_shr (t0, scal_fac);
*cor_max = extract_h (L_shl (t0, 15)); /* divide by 2 */
}
else
{
*cor_max = extract_l(t0);
}
return (p_max);
}
/*
**************************************************************************
*
* Function : agc
* Purpose : Scales the postfilter output on a subframe basis
*
**************************************************************************
*/
int agc (
agcState *st, /* i/o : agc state */
Word16 *sig_in, /* i : postfilter input signal (l_trm) */
Word16 *sig_out, /* i/o : postfilter output signal (l_trm) */
Word16 agc_fac, /* i : AGC factor */
Word16 l_trm /* i : subframe size */
)
{
Word16 i, exp;
Word16 gain_in, gain_out, g0, gain;
Word32 s;
/* calculate gain_out with exponent */
s = energy_new(sig_out, l_trm); /* function result */
if (s == 0)
{
st->past_gain = 0;
return 0;
}
exp = sub (norm_l (s), 1);
gain_out = round (L_shl (s, exp));
/* calculate gain_in with exponent */
s = energy_new(sig_in, l_trm); /* function result */
if (s == 0)
{
g0 = 0;
}
else
{
i = norm_l (s);
gain_in = round (L_shl (s, i));
exp = sub (exp, i);
/*---------------------------------------------------*
* g0 = (1-agc_fac) * sqrt(gain_in/gain_out); *
*---------------------------------------------------*/
s = L_deposit_l (div_s (gain_out, gain_in));
s = L_shl (s, 7); /* s = gain_out / gain_in */
s = L_shr (s, exp); /* add exponent */
s = Inv_sqrt (s); /* function result */
i = round (L_shl (s, 9));
/* g0 = i * (1-agc_fac) */
g0 = mult (i, sub (32767, agc_fac));
}
/* compute gain[n] = agc_fac * gain[n-1]
+ (1-agc_fac) * sqrt(gain_in/gain_out) */
/* sig_out[n] = gain[n] * sig_out[n] */
gain = st->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));
}
st->past_gain = gain;
return 0;
}
void set_sign12k2(
Word16 dn[], /* i/o : correlation between target and h[] */
Word16 cn[], /* i : residual after long term prediction */
Word16 sign[], /* o : sign of d[n] */
Word16 pos_max[], /* o : position of maximum correlation */
Word16 nb_track, /* i : number of tracks tracks */
Word16 ipos[], /* o : starting position for each pulse */
Word16 step, /* i : the step size in the tracks */
Flag *pOverflow /* i/o: overflow flag */
)
{
Word16 i, j;
Word16 val;
Word16 cor;
Word16 k_cn;
Word16 k_dn;
Word16 max;
Word16 max_of_all;
Word16 pos = 0; /* initialization only needed to keep gcc silent */
Word16 en[L_CODE]; /* correlation vector */
Word32 s;
Word32 t;
Word32 L_temp;
Word16 *p_cn;
Word16 *p_dn;
Word16 *p_sign;
Word16 *p_en;
/* calculate energy for normalization of cn[] and dn[] */
s = 256;
t = 256;
p_cn = cn;
p_dn = dn; /* crosscorrelation values do not have strong peaks, so
scaling applied in cor_h_x (sf=2) guaranteed that the
mac of the energy for this vector will not overflow */
for (i = L_CODE; i != 0; i--)
{
val = *(p_cn++);
s = L_mac(s, val, val, pOverflow);
val = *(p_dn++);
t += ((Word32) val * val) << 1;
}
s = Inv_sqrt(s, pOverflow);
k_cn = (Word16)((L_shl(s, 5, pOverflow)) >> 16);
t = Inv_sqrt(t, pOverflow);
k_dn = (Word16)(t >> 11);
p_cn = &cn[L_CODE-1];
p_sign = &sign[L_CODE-1];
p_en = &en[L_CODE-1];
for (i = L_CODE - 1; i >= 0; i--)
{
L_temp = ((Word32)k_cn * *(p_cn--)) << 1;
val = dn[i];
s = L_mac(L_temp, k_dn, val, pOverflow);
L_temp = L_shl(s, 10, pOverflow);
cor = pv_round(L_temp, pOverflow);
if (cor >= 0)
{
*(p_sign--) = 32767; /* sign = +1 */
}
else
{
*(p_sign--) = -32767; /* sign = -1 */
cor = - (cor);
/* modify dn[] according to the fixed sign */
dn[i] = - val;
}
*(p_en--) = cor;
}
max_of_all = -1;
for (i = 0; i < nb_track; i++)
{
max = -1;
for (j = i; j < L_CODE; j += step)
{
cor = en[j];
if (cor > max)
{
max = cor;
pos = j;
}
}
/* store maximum correlation position */
pos_max[i] = pos;
if (max > max_of_all)
{
max_of_all = max;
/* starting position for i0 */
ipos[0] = i;
}
}
/*----------------------------------------------------------------*
* Set starting position of each pulse. *
*----------------------------------------------------------------*/
pos = ipos[0];
ipos[nb_track] = pos;
for (i = 1; i < nb_track; i++)
{
pos++;
if (pos >= nb_track)
{
pos = 0;
}
ipos[ i] = pos;
ipos[ i + nb_track] = pos;
}
return;
}
