Word32 Div_32(Word32 L_num, Word16 denom_hi, Word16 denom_lo)
{
Word16 approx, hi, lo, n_hi, n_lo;
Word32 L_32;
/* First approximation: 1 / L_denom = 1/denom_hi */
approx = div_s( (Word16)0x3fff, denom_hi); /* result in Q14 */
/* Note: 3fff = 0.5 in Q15 */
/* 1/L_denom = approx * (2.0 - L_denom * approx) */
L_32 = Mpy_32_16(denom_hi, denom_lo, approx); /* result in Q30 */
L_32 = L_sub( (Word32)0x7fffffffL, L_32); /* result in Q30 */
L_Extract(L_32, &hi, &lo);
L_32 = Mpy_32_16(hi, lo, approx); /* = 1/L_denom in Q29 */
/* L_num * (1/L_denom) */
L_Extract(L_32, &hi, &lo);
L_Extract(L_num, &n_hi, &n_lo);
L_32 = Mpy_32(n_hi, n_lo, hi, lo); /* result in Q29 */
L_32 = L_shl(L_32, 2); /* From Q29 to Q31 */
return( L_32 );
}
void sqrt_i(Word16 x_man, Word16 x_exp, Word16 *y_man, Word16 *y_exp)
{
Word16 x_manb, x_expb, y, exp, idx, sub_frac, sub_tab;
Word32 a0;
if(x_man <= 0){
*y_man = 0;
*y_exp = 0;
}
else{
exp = norm_s(x_man);
x_manb = shl(x_man, exp); // normalize to Q15 in [0; 1] for table look-up
x_expb = add(x_exp, exp); // update exponent for x (left-shifting by additionl exp)
x_expb = sub(x_expb, 15); // we need to take sqrt of 0-32767 number but table is for 0-1
idx = shr(x_manb, 9); // for 65 entry table
a0 = L_deposit_h(tabsqrt[idx]); // Q31 table look-up value
sub_frac = shl((Word16)(x_manb & 0x01FF), 6);// Q15 sub-fraction
sub_tab = sub(tabsqrt[idx+1], tabsqrt[idx]); // Q15 table interval for interpolation
a0 = L_mac(a0, sub_frac, sub_tab); // Q31 linear interpolation between table entries
exp = norm_l(a0); // exponent of a0
y = intround(L_shl(a0,exp)); // normalize sqrt-root and drop 16 LBSs
exp = add(15, exp); // exponent of a0 taking Q15 of y into account
if(x_expb & 0x0001){
if(y < 0x5A82){ // 1*sqrt(2) in Q14) - with div_s(y1, y2), y2 must be >= y1
exp = add(exp, shr(add(x_expb,1), 1)); // normalization for sqrt()
*y_man = div_s(0x2D41, y); // 0x2D41 is 1/sqrt(2) in Q14
}
else{
exp = add(exp, shr(sub(x_expb,1), 1)); // normalization for sqrt()
*y_man = div_s(0x5A82, y); // 0x5A82 is 1*sqrt(2) in Q14
}
*y_exp = sub(29, exp); // ...and div_s returns fraction divide in Q15
}
else{
exp = add(exp, shr(x_expb, 1)); // normalization for sqrt()
*y_man = div_s(0x4000, y);
*y_exp = sub(29, exp); // 0x4000 is 1 in Q14 and div_s returns fraction divide in Q15
}
}
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;
}
Word16 G_pitch( /* (o) Q14 : Gain of pitch lag saturated to 1.2 */
Word16 xn[], /* (i) : Pitch target. */
Word16 y1[], /* (i) : filtered adaptive codebook. */
Word16 g_coeff[], /* : Correlations need for gain quantization. */
Word16 L_subfr /* : Length of subframe. */
)
{
Word16 i;
Word16 xy, yy, exp_xy, exp_yy, gain;
Word32 L_tmp;
/* Compute scalar product <y1[],y1[]> */
L_tmp = Dot_product12(y1, y1, L_subfr, &exp_yy);
yy = (Word16)(L_tmp >> 16);
/* Compute scalar product <xn[],y1[]> */
L_tmp = Dot_product12(xn, y1, L_subfr, &exp_xy);
xy = (Word16)(L_tmp >> 16);
g_coeff[0] = yy;
g_coeff[1] = exp_yy;
g_coeff[2] = xy;
g_coeff[3] = exp_xy;
xy = xy >> 1; /* Be sure xy < yy */
/* If (xy < 0) gain = 0 */
if (xy <= 0)
return ((Word16) 0);
/* compute gain = xy/yy */
gain = div_s(xy, yy);
i = add(exp_xy, 1 - 1); /* -1 -> gain in Q14 */
i = sub(i, exp_yy);
gain = shl(gain, i); /* saturation can occur here */
/* if (gain > 1.2) gain = 1.2 in Q14 */
if (gain > 19661)
{
gain = 19661;
}
return (gain);
}
/*----------------------------------------------------------------------------
* 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;
}
/*----------------------------------------------------------------------------
* calc_st_filt - computes impulse response of A(gamma2) / A(gamma1)
* controls gain : computation of energy impulse response as
* SUMn (abs (h[n])) and computes parcor0
*----------------------------------------------------------------------------
*/
static void calc_st_filte(
Word16 *apond2, /* input : coefficients of numerator */
Word16 *apond1, /* input : coefficients of denominator */
Word16 *parcor0, /* output: 1st parcor calcul. on composed filter */
Word16 *sig_ltp_ptr, /* in/out: input of 1/A(gamma1) : scaled by 1/g0 */
Word16 long_h_st, /* input : impulse response length */
Word16 m_pst /* input : LPC order */
)
{
Word16 h[LONG_H_ST_E];
Word32 L_temp, L_g0;
Word16 g0, temp;
Word16 i;
/* compute i.r. of composed filter apond2 / apond1 */
Syn_filte(m_pst, apond1, apond2, h, long_h_st, mem_zero, 0);
/* compute 1st parcor */
calc_rc0_he(h, parcor0, long_h_st);
/* compute g0 */
L_g0 = 0L;
for(i=0; i<long_h_st; i++) {
L_temp = L_deposit_l(abs_s(h[i]));
L_g0 = L_add(L_g0, L_temp);
}
g0 = extract_h(L_shl(L_g0, 14));
/* Scale signal input of 1/A(gamma1) */
if(sub(g0, 1024)>0) {
temp = div_s(1024, g0); /* temp = 2**15 / gain0 */
for(i=0; i<L_SUBFR; i++) {
sig_ltp_ptr[i] = mult_r(sig_ltp_ptr[i], temp);
}
}
return;
}
/*---------------------------------------------------------------------------*
* Function Qua_gain *
* ~~~~~~~~~~~~~~~~~~ *
* Inputs: *
* code[] :Innovative codebook. *
* g_coeff[] :Correlations compute for pitch. *
* L_subfr :Subframe length. *
* *
* Outputs: *
* gain_pit :Quantized pitch gain. *
* gain_cod :Quantized code gain. *
* *
* Return: *
* Index of quantization. *
* *
*--------------------------------------------------------------------------*/
Word16 Qua_gain(
Word16 code[], /* (i) Q13 :Innovative vector. */
Word16 g_coeff[], /* (i) :Correlations <xn y1> -2<y1 y1> */
/* <y2,y2>, -2<xn,y2>, 2<y1,y2> */
Word16 exp_coeff[], /* (i) :Q-Format g_coeff[] */
Word16 L_subfr, /* (i) :Subframe length. */
Word16 *gain_pit, /* (o) Q14 :Pitch gain. */
Word16 *gain_cod, /* (o) Q1 :Code gain. */
Word16 tameflag /* (i) : set to 1 if taming is needed */
)
{
Word16 i, j, index1, index2;
Word16 cand1, cand2;
Word16 exp, gcode0, exp_gcode0, gcode0_org, e_min ;
Word16 nume, denom, inv_denom;
Word16 exp1,exp2,exp_nume,exp_denom,exp_inv_denom,sft,tmp;
Word16 g_pitch, g2_pitch, g_code, g2_code, g_pit_cod;
Word16 coeff[5], coeff_lsf[5];
Word16 exp_min[5];
Word32 L_gbk12;
Word32 L_tmp, L_dist_min, L_tmp1, L_tmp2, L_acc, L_accb;
Word16 best_gain[2];
/* Gain predictor, Past quantized energies = -14.0 in Q10 */
static Word16 past_qua_en[4] = { -14336, -14336, -14336, -14336 };
/*---------------------------------------------------*
*- energy due to innovation -*
*- predicted energy -*
*- predicted codebook gain => gcode0[exp_gcode0] -*
*---------------------------------------------------*/
Gain_predict( past_qua_en, code, L_subfr, &gcode0, &exp_gcode0 );
/*-----------------------------------------------------------------*
* pre-selection *
*-----------------------------------------------------------------*/
/*-----------------------------------------------------------------*
* calculate best gain *
* *
* tmp = -1./(4.*coeff[0]*coeff[2]-coeff[4]*coeff[4]) ; *
* best_gain[0] = (2.*coeff[2]*coeff[1]-coeff[3]*coeff[4])*tmp ; *
* best_gain[1] = (2.*coeff[0]*coeff[3]-coeff[1]*coeff[4])*tmp ; *
* gbk_presel(best_gain,&cand1,&cand2,gcode0) ; *
* *
*-----------------------------------------------------------------*/
/*-----------------------------------------------------------------*
* tmp = -1./(4.*coeff[0]*coeff[2]-coeff[4]*coeff[4]) ; *
*-----------------------------------------------------------------*/
L_tmp1 = L_mult( g_coeff[0], g_coeff[2] );
exp1 = add( add( exp_coeff[0], exp_coeff[2] ), 1-2 );
L_tmp2 = L_mult( g_coeff[4], g_coeff[4] );
exp2 = add( add( exp_coeff[4], exp_coeff[4] ), 1 );
//if( sub(exp1, exp2)>0 ){
if( exp1 > exp2 ){
L_tmp = L_sub( L_shr( L_tmp1, sub(exp1,exp2) ), L_tmp2 );
exp = exp2;
}
else{
L_tmp = L_sub( L_tmp1, L_shr( L_tmp2, sub(exp2,exp1) ) );
exp = exp1;
}
sft = norm_l( L_tmp );
denom = extract_h( L_shl(L_tmp, sft) );
exp_denom = sub( add( exp, sft ), 16 );
inv_denom = div_s(16384,denom);
inv_denom = negate( inv_denom );
exp_inv_denom = sub( 14+15, exp_denom );
/*-----------------------------------------------------------------*
* best_gain[0] = (2.*coeff[2]*coeff[1]-coeff[3]*coeff[4])*tmp ; *
*-----------------------------------------------------------------*/
L_tmp1 = L_mult( g_coeff[2], g_coeff[1] );
exp1 = add( exp_coeff[2], exp_coeff[1] );
L_tmp2 = L_mult( g_coeff[3], g_coeff[4] );
exp2 = add( add( exp_coeff[3], exp_coeff[4] ), 1 );
//if( sub(exp1, exp2)>0 ){
if (exp1 > exp2){
L_tmp = L_sub( L_shr( L_tmp1, add(sub(exp1,exp2),1 )), L_shr( L_tmp2,1 ) );
exp = sub(exp2,1);
}
else{
L_tmp = L_sub( L_shr( L_tmp1,1 ), L_shr( L_tmp2, add(sub(exp2,exp1),1 )) );
exp = sub(exp1,1);
}
sft = norm_l( L_tmp );
nume = extract_h( L_shl(L_tmp, sft) );
exp_nume = sub( add( exp, sft ), 16 );
sft = sub( add( exp_nume, exp_inv_denom ), (9+16-1) );
L_acc = L_shr( L_mult( nume,inv_denom ), sft );
best_gain[0] = extract_h( L_acc ); /*-- best_gain[0]:Q9 --*/
if (tameflag == 1){
//if(sub(best_gain[0], GPCLIP2) > 0) best_gain[0] = GPCLIP2;
if(best_gain[0] > GPCLIP2) best_gain[0] = GPCLIP2;
}
/*-----------------------------------------------------------------*
* best_gain[1] = (2.*coeff[0]*coeff[3]-coeff[1]*coeff[4])*tmp ; *
*-----------------------------------------------------------------*/
L_tmp1 = L_mult( g_coeff[0], g_coeff[3] );
exp1 = add( exp_coeff[0], exp_coeff[3] ) ;
L_tmp2 = L_mult( g_coeff[1], g_coeff[4] );
exp2 = add( add( exp_coeff[1], exp_coeff[4] ), 1 );
//if( sub(exp1, exp2)>0 ){
if( exp1 > exp2 ){
L_tmp = L_sub( L_shr( L_tmp1, add(sub(exp1,exp2),1) ), L_shr( L_tmp2,1 ) );
exp = sub(exp2,1);
//exp = exp2--;
}
else{
L_tmp = L_sub( L_shr( L_tmp1,1 ), L_shr( L_tmp2, add(sub(exp2,exp1),1) ) );
exp = sub(exp1,1);
//exp = exp1--;
}
sft = norm_l( L_tmp );
nume = extract_h( L_shl(L_tmp, sft) );
exp_nume = sub( add( exp, sft ), 16 );
sft = sub( add( exp_nume, exp_inv_denom ), (2+16-1) );
L_acc = L_shr( L_mult( nume,inv_denom ), sft );
best_gain[1] = extract_h( L_acc ); /*-- best_gain[1]:Q2 --*/
/*--- Change Q-format of gcode0 ( Q[exp_gcode0] -> Q4 ) ---*/
//if( sub(exp_gcode0,4) >= 0 ){
if (exp_gcode0 >=4) {
gcode0_org = shr( gcode0, sub(exp_gcode0,4) );
}
else{
L_acc = L_deposit_l( gcode0 );
L_acc = L_shl( L_acc, sub( (4+16), exp_gcode0 ) );
gcode0_org = extract_h( L_acc ); /*-- gcode0_org:Q4 --*/
}
/*----------------------------------------------*
* - presearch for gain codebook - *
*----------------------------------------------*/
Gbk_presel(best_gain, &cand1, &cand2, gcode0_org );
/*---------------------------------------------------------------------------*
* *
* Find the best quantizer. *
* *
* dist_min = MAX_32; *
* for ( i=0 ; i<NCAN1 ; i++ ){ *
* for ( j=0 ; j<NCAN2 ; j++ ){ *
* g_pitch = gbk1[cand1+i][0] + gbk2[cand2+j][0]; *
* g_code = gcode0 * (gbk1[cand1+i][1] + gbk2[cand2+j][1]); *
* dist = g_pitch*g_pitch * coeff[0] *
* + g_pitch * coeff[1] *
* + g_code*g_code * coeff[2] *
* + g_code * coeff[3] *
* + g_pitch*g_code * coeff[4] ; *
* *
* if (dist < dist_min){ *
* dist_min = dist; *
* indice1 = cand1 + i ; *
* indice2 = cand2 + j ; *
* } *
* } *
* } *
* *
* g_pitch = Q13 *
* g_pitch*g_pitch = Q11:(13+13+1-16) *
* g_code = Q[exp_gcode0-3]:(exp_gcode0+(13-1)+1-16) *
* g_code*g_code = Q[2*exp_gcode0-21]:(exp_gcode0-3+exp_gcode0-3+1-16) *
* g_pitch*g_code = Q[exp_gcode0-5]:(13+exp_gcode0-3+1-16) *
* *
* term 0: g_pitch*g_pitch*coeff[0] ;exp_min0 = 13 +exp_coeff[0] *
* term 1: g_pitch *coeff[1] ;exp_min1 = 14 +exp_coeff[1] *
* term 2: g_code*g_code *coeff[2] ;exp_min2 = 2*exp_gcode0-21+exp_coeff[2] *
* term 3: g_code *coeff[3] ;exp_min3 = exp_gcode0 - 3+exp_coeff[3] *
* term 4: g_pitch*g_code *coeff[4] ;exp_min4 = exp_gcode0 - 4+exp_coeff[4] *
* *
*---------------------------------------------------------------------------*/
exp_min[0] = add( exp_coeff[0], 13 );
exp_min[1] = add( exp_coeff[1], 14 );
exp_min[2] = add( exp_coeff[2], sub( shl( exp_gcode0, 1 ), 21 ) );
exp_min[3] = add( exp_coeff[3], sub( exp_gcode0, 3 ) );
exp_min[4] = add( exp_coeff[4], sub( exp_gcode0, 4 ) );
e_min = exp_min[0];
for(i=1; i<5; i++){
//if( sub(exp_min[i], e_min) < 0 ){
if (exp_min[i] < e_min) {
e_min = exp_min[i];
}
}
/* align coeff[] and save in special 32 bit double precision */
for(i=0; i<5; i++){
j = sub( exp_min[i], e_min );
L_tmp = (Word32)g_coeff[i] << 16;
L_tmp = L_shr( L_tmp, j ); /* L_tmp:Q[exp_g_coeff[i]+16-j] */
L_Extract( L_tmp, &coeff[i], &coeff_lsf[i] ); /* DPF */
}
/* Codebook search */
L_dist_min = MAX_32;
/* initialization used only to suppress Microsoft Visual C++ warnings */
index1 = cand1;
index2 = cand2;
if(tameflag == 1){
for(i=0; i<NCAN1; i++){
for(j=0; j<NCAN2; j++){
g_pitch = add( gbk1[cand1+i][0], gbk2[cand2+j][0] ); /* Q14 */
if(g_pitch < GP0999) {
L_acc = L_deposit_l( gbk1[cand1+i][1] );
L_accb = L_deposit_l( gbk2[cand2+j][1] ); /* Q13 */
L_tmp = L_add( L_acc,L_accb );
tmp = extract_l( L_shr( L_tmp,1 ) ); /* Q12 */
g_code = mult( gcode0, tmp ); /* Q[exp_gcode0+12-15] */
g2_pitch = mult(g_pitch, g_pitch); /* Q13 */
g2_code = mult(g_code, g_code); /* Q[2*exp_gcode0-6-15] */
g_pit_cod= mult(g_code, g_pitch); /* Q[exp_gcode0-3+14-15] */
L_tmp = Mpy_32_16(coeff[0], coeff_lsf[0], g2_pitch);
//L_tmp = L_add(L_tmp, Mpy_32_16(coeff[1], coeff_lsf[1], g_pitch) );
//L_tmp = L_add(L_tmp, Mpy_32_16(coeff[2], coeff_lsf[2], g2_code) );
//L_tmp = L_add(L_tmp, Mpy_32_16(coeff[3], coeff_lsf[3], g_code) );
//L_tmp = L_add(L_tmp, Mpy_32_16(coeff[4], coeff_lsf[4], g_pit_cod) );
L_tmp += Mpy_32_16(coeff[1], coeff_lsf[1], g_pitch);
L_tmp += Mpy_32_16(coeff[2], coeff_lsf[2], g2_code);
L_tmp += Mpy_32_16(coeff[3], coeff_lsf[3], g_code);
L_tmp += Mpy_32_16(coeff[4], coeff_lsf[4], g_pit_cod);
//L_temp = L_sub(L_tmp, L_dist_min);
//if( L_temp < 0L ){
if( L_tmp < L_dist_min ){
L_dist_min = L_tmp;
index1 = add(cand1,i);
index2 = add(cand2,j);
}
}
}
}
}
else{
for(i=0; i<NCAN1; i++){
for(j=0; j<NCAN2; j++){
g_pitch = add( gbk1[cand1+i][0], gbk2[cand2+j][0] ); /* Q14 */
L_acc = L_deposit_l( gbk1[cand1+i][1] );
L_accb = L_deposit_l( gbk2[cand2+j][1] ); /* Q13 */
L_tmp = L_add( L_acc,L_accb );
tmp = extract_l( L_shr( L_tmp,1 ) ); /* Q12 */
g_code = mult( gcode0, tmp ); /* Q[exp_gcode0+12-15] */
g2_pitch = mult(g_pitch, g_pitch); /* Q13 */
g2_code = mult(g_code, g_code); /* Q[2*exp_gcode0-6-15] */
g_pit_cod= mult(g_code, g_pitch); /* Q[exp_gcode0-3+14-15] */
L_tmp = Mpy_32_16(coeff[0], coeff_lsf[0], g2_pitch);
//L_tmp = L_add(L_tmp, Mpy_32_16(coeff[1], coeff_lsf[1], g_pitch) );
//L_tmp = L_add(L_tmp, Mpy_32_16(coeff[2], coeff_lsf[2], g2_code) );
//L_tmp = L_add(L_tmp, Mpy_32_16(coeff[3], coeff_lsf[3], g_code) );
//L_tmp = L_add(L_tmp, Mpy_32_16(coeff[4], coeff_lsf[4], g_pit_cod) );
L_tmp += Mpy_32_16(coeff[1], coeff_lsf[1], g_pitch);
L_tmp += Mpy_32_16(coeff[2], coeff_lsf[2], g2_code);
L_tmp += Mpy_32_16(coeff[3], coeff_lsf[3], g_code);
L_tmp += Mpy_32_16(coeff[4], coeff_lsf[4], g_pit_cod);
if( L_tmp < L_dist_min ){
L_dist_min = L_tmp;
index1 = add(cand1,i);
index2 = add(cand2,j);
}
}
}
}
/* Read the quantized gain */
/*-----------------------------------------------------------------*
* *gain_pit = gbk1[indice1][0] + gbk2[indice2][0]; *
*-----------------------------------------------------------------*/
*gain_pit = add( gbk1[index1][0], gbk2[index2][0] ); /* Q14 */
/*-----------------------------------------------------------------*
* *gain_code = (gbk1[indice1][1]+gbk2[indice2][1]) * gcode0; *
*-----------------------------------------------------------------*/
L_gbk12 = (Word32)gbk1[index1][1] + (Word32)gbk2[index2][1]; /* Q13 */
tmp = extract_l( L_shr( L_gbk12,1 ) ); /* Q12 */
L_acc = L_mult(tmp, gcode0); /* Q[exp_gcode0+12+1] */
L_acc = L_shl(L_acc, add( negate(exp_gcode0),(-12-1+1+16) ));
*gain_cod = extract_h( L_acc ); /* Q1 */
/*----------------------------------------------*
* update table of past quantized energies *
*----------------------------------------------*/
Gain_update( past_qua_en, L_gbk12 );
return( add( map1[index1]*(Word16)NCODE2, map2[index2] ) );
}
void Post_Filter(
Word16 *syn, /* in/out: synthesis speech (postfiltered is output) */
Word16 *Az_4, /* input : interpolated LPC parameters in all subframes */
Word16 *T, /* input : decoded pitch lags in all subframes */
Word16 Vad
)
{
/*-------------------------------------------------------------------*
* Declaration of parameters *
*-------------------------------------------------------------------*/
Word16 res2_pst[L_SUBFR]; /* res2[] after pitch postfiltering */
Word16 syn_pst[L_FRAME]; /* post filtered synthesis speech */
Word16 Ap3[MP1], Ap4[MP1]; /* bandwidth expanded LP parameters */
Word16 *Az; /* pointer to Az_4: */
/* LPC parameters in each subframe */
Word16 t0_max, t0_min; /* closed-loop pitch search range */
Word16 i_subfr; /* index for beginning of subframe */
Word16 h[L_H];
Word16 i, j;
Word16 temp1, temp2;
Word32 L_tmp;
postfilt_type* ppost_filt = pg729dec->ppost_filt;
/*-----------------------------------------------------*
* Post filtering *
*-----------------------------------------------------*/
Az = Az_4;
for (i_subfr = 0; i_subfr < L_FRAME; i_subfr += L_SUBFR)
{
/* Find pitch range t0_min - t0_max */
t0_min = sub(*T++, 3);
t0_max = add(t0_min, 6);
if (sub(t0_max, PIT_MAX) > 0) {
t0_max = PIT_MAX;
t0_min = sub(t0_max, 6);
}
/* Find weighted filter coefficients Ap3[] and ap[4] */
Weight_Az(Az, GAMMA2_PST, M, Ap3);
Weight_Az(Az, GAMMA1_PST, M, Ap4);
/* filtering of synthesis speech by A(z/GAMMA2_PST) to find res2[] */
Residu(Ap3, &syn[i_subfr], ppost_filt->res2, L_SUBFR);
/* scaling of "res2[]" to avoid energy overflow */
for (j=0; j<L_SUBFR; j++)
{
ppost_filt->scal_res2[j] = shr(ppost_filt->res2[j], 2);
}
/* pitch postfiltering */
if (sub(Vad, 1) == 0)
pit_pst_filt(ppost_filt->res2, ppost_filt->scal_res2, t0_min, t0_max, L_SUBFR, res2_pst);
else
for (j=0; j<L_SUBFR; j++)
res2_pst[j] = ppost_filt->res2[j];
/* tilt compensation filter */
/* impulse response of A(z/GAMMA2_PST)/A(z/GAMMA1_PST) */
Copy(Ap3, h, M+1);
Set_zero(&h[M+1], L_H-M-1);
Syn_filt(Ap4, h, h, L_H, &h[M+1], 0);
/* 1st correlation of h[] */
L_tmp = L_mult(h[0], h[0]);
for (i=1; i<L_H; i++) L_tmp = L_mac(L_tmp, h[i], h[i]);
temp1 = extract_h(L_tmp);
L_tmp = L_mult(h[0], h[1]);
for (i=1; i<L_H-1; i++) L_tmp = L_mac(L_tmp, h[i], h[i+1]);
temp2 = extract_h(L_tmp);
if(temp2 <= 0) {
temp2 = 0;
}
else {
temp2 = mult(temp2, MU);
temp2 = div_s(temp2, temp1);
}
preemphasis(res2_pst, temp2, L_SUBFR);
/* filtering through 1/A(z/GAMMA1_PST) */
Syn_filt(Ap4, res2_pst, &syn_pst[i_subfr], L_SUBFR, ppost_filt->mem_syn_pst, 1);
/* scale output to input */
agc(&syn[i_subfr], &syn_pst[i_subfr], L_SUBFR);
/* update res2[] buffer; shift by L_SUBFR */
Copy(&ppost_filt->res2[L_SUBFR-PIT_MAX], &ppost_filt->res2[-PIT_MAX], PIT_MAX);
Copy(&ppost_filt->scal_res2[L_SUBFR-PIT_MAX], &ppost_filt->scal_res2[-PIT_MAX], PIT_MAX);
Az += MP1;
}
/* update syn[] buffer */
Copy(&syn[L_FRAME-M], &syn[-M], M);
/* overwrite synthesis speech by postfiltered synthesis speech */
Copy(syn_pst, syn, L_FRAME);
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;
}
/* Standard Long-Term Postfilter */
void postfilter(
Word16 *s, /* input : quantized speech signal */
Word16 pp, /* input : pitch period */
Word16 *ma_a,
Word16 *b_prv,
Word16 *pp_prv,
Word16 *e) /* output: enhanced speech signal */
{
int n;
Word16 len, t0, t1, t2, t3, shift, aa, R0norm, R0_exp;
Word32 a0, a1, R0, R1, R01, R01max, Rx;
Word16 *fp1;
Word16 ppt, pptmin, pptmax, ppnew;
Word16 bb[2];
Word16 R1max_exp, R1max, R01Sqmax_exp, R01Sqmax, R01Sq_exp, R01Sq, R1_exp, R1n;
Word16 gainn, Rx_exp;
Word16 buf[MAXPP+FRSZ];
Word16 *ps, ww1, ww2;
Word32 step, delta;
Word16 bi0, bi1c, bi1p;
ps = s+XQOFF;
/********************************************************************/
/* pitch search around decoded pitch */
/********************************************************************/
pptmin = sub(pp, DPPQNS);
pptmax = add(pp, DPPQNS);
if (pptmin<MINPP)
{
pptmin = MINPP;
pptmax = add(pptmin, 2*DPPQNS);
}
else if (pptmax>MAXPP)
{
pptmax = MAXPP;
pptmin = sub(pptmax, 2*DPPQNS);
}
fp1 = &s[XQOFF-pptmax];
len = add(FRSZ, pptmax);
a0 = 0;
for (n=0;n<len;n++)
{
t1 = shr(*fp1++, 3);
a0 = L_mac0(a0,t1,t1);
}
shift = norm_l(a0);
if (a0==0) shift=31;
shift = sub(6, shift);
if (shift > 0)
{
ps = buf+pptmax;
fp1 = &s[XQOFF-pptmax];
shift = shr(add(shift, 1), 1);
for (n=0;n<len;n++)
{
buf[n] = shr(fp1[n], shift);
}
}
else shift=0;
R0 = 0;
R1 = 0;
R01 = 0;
for(n=0; n<FRSZ; n++)
{
R0 = L_mac0(R0, ps[n], ps[n]);
R1 = L_mac0(R1, ps[n-pptmin], ps[n-pptmin]);
R01 = L_mac0(R01,ps[n], ps[n-pptmin]);
}
R0_exp = norm_l(R0);
R0norm = extract_h(L_shl(R0, R0_exp));
R0_exp = R0_exp-16;
ppnew = pptmin;
R1max_exp = norm_l(R1);
R1max = extract_h(L_shl(R1, R1max_exp));
R01Sqmax_exp = norm_l(R01);
t1 = extract_h(L_shl(R01, R01Sqmax_exp));
R01Sqmax_exp = shl(R01Sqmax_exp, 1);
R01Sqmax = extract_h(L_mult(t1, t1));
R01max = R01;
for(ppt=pptmin+1; ppt<=pptmax; ppt++)
{
R1 = L_msu0(R1,ps[FRSZ-ppt], ps[FRSZ-ppt]);
R1 = L_mac0(R1,ps[-ppt], ps[-ppt]);
R01= 0;
for(n=0; n<FRSZ; n++)
{
R01 = L_mac0(R01, ps[n], ps[n-ppt]);
}
R01Sq_exp = norm_l(R01);
t1 = extract_h(L_shl(R01, R01Sq_exp));
R01Sq_exp = shl(R01Sq_exp, 1);
R01Sq = extract_h(L_mult(t1, t1));
R1_exp = norm_l(R1);
R1n = extract_h(L_shl(R1, R1_exp));
a0 = L_mult(R01Sq, R1max);
a1 = L_mult(R01Sqmax, R1n);
t1 = add(R01Sq_exp, R1max_exp);
t2 = add(R01Sqmax_exp, R1_exp);
t2 = sub(t1, t2);
if (t2>=0) a0 = L_shr(a0, t2);
if (t2<0) a1 = L_shl(a1, t2);
if (L_sub(a0, a1)>0)
{
R01Sqmax = R01Sq;
R01Sqmax_exp = R01Sq_exp;
R1max = R1n; R1max_exp = R1_exp;
ppnew = ppt;
R01max = R01;
}
}
/******************************************************************/
/* calculate all-zero pitch postfilter */
/******************************************************************/
if (R1max==0 || R0==0 || R01max <= 0)
{
aa = 0;
}
else
{
a0 = R1max_exp-16;
t1 = mult(R1max, R0norm);
a0 = a0+R0_exp-15;
sqrt_i(t1, (Word16)a0, &t1, &t2);
t0 = norm_l(R01max);
t3 = extract_h(L_shl(R01max, t0));
t0 = t0-16;
aa = mult(t3, t1);
t0 = t0+t2-15;
t0 = t0-15;
if (t0<0) aa = shl(aa, sub(0,t0));
else aa = shr(aa, t0);
}
a0 = L_mult(8192, aa);
a0 = L_mac(a0, 24576, *ma_a);
*ma_a = intround(a0);
if((*ma_a < ATHLD1) && (aa < (ATHLD2)))
aa = 0;
bb[1] = mult(ScLTPF, aa);
/******************************************************************/
/* calculate normalization energies */
/******************************************************************/
Rx = 0;
R0 = 0;
for(n=0; n<FRSZ; n++)
{
a0 = L_shl(s[XQOFF+n], 15);
a0 = L_add(a0, L_mult0(bb[1], s[XQOFF+n-ppnew]));
e[n] = intround(a0);
t1 = shr(e[n], shift);
t2 = shr(s[XQOFF+n], shift);
Rx = L_mac0(Rx, t1, t1);
R0 = L_mac0(R0, t2, t2);
}
R0 = L_shr(R0, 2);
if(R0 == 0 || Rx == 0)
gainn = 32767;
else
{
Rx_exp = norm_l(Rx);
t1 = extract_h(L_shl(Rx, Rx_exp));
t2 = extract_h(L_shl(R0, Rx_exp));
if (t2>= t1)
gainn = 32767;
else
{
t1 = div_s(t2, t1);
gainn = sqrts(t1);
}
}
/******************************************************************/
/* interpolate from the previous postfilter to the current */
/******************************************************************/
bb[0] = gainn;
bb[1] = mult(gainn, bb[1]);
step = (Word32)((1.0/(NINT+1))*(2147483648.0));
delta = 0;
for(n=0; n<NINT; n++)
{
delta = L_add(delta, step);
ww1 = intround(delta);
ww2 = add(sub(32767, ww1), 1);
/* interpolate between two filters */
bi0 = intround(L_mac(L_mult(ww1, bb[0]), ww2, b_prv[0]));
bi1c= mult(ww1, bb[1]);
bi1p= mult(ww2, b_prv[1]);
e[n] = intround(L_mac(L_mac(L_mult(bi1c, s[XQOFF+n-ppnew]), bi1p, s[XQOFF+n-(*pp_prv)]), bi0, s[XQOFF+n]));
}
for(n=NINT; n<FRSZ; n++)
{
e[n] = intround(L_shl(L_mult(gainn, e[n]),1));
}
/******************************************************************/
/* save state memory */
/******************************************************************/
*pp_prv = ppnew;
b_prv[0] = bb[0];
b_prv[1] = bb[1];
return;
}
/*************************************************************************
*
* FUNCTION: calc_unfilt_energies
*
* PURPOSE: calculation of several energy coefficients for unfiltered
* excitation signals and the LTP coding gain
*
* frac_en[0]*2^exp_en[0] = <res res> // LP residual energy
* frac_en[1]*2^exp_en[1] = <exc exc> // LTP residual energy
* frac_en[2]*2^exp_en[2] = <exc code> // LTP/CB innovation dot product
* frac_en[3]*2^exp_en[3] = <lres lres> // LTP residual energy
* // (lres = res - gain_pit*exc)
* ltpg = log2(LP_res_en / LTP_res_en)
*
*************************************************************************/
void
calc_unfilt_energies(
Word16 res[], /* i : LP residual, Q0 */
Word16 exc[], /* i : LTP excitation (unfiltered), Q0 */
Word16 code[], /* i : CB innovation (unfiltered), Q13 */
Word16 gain_pit, /* i : pitch gain, Q14 */
Word16 L_subfr, /* i : Subframe length */
Word16 frac_en[], /* o : energy coefficients (4), fraction part, Q15 */
Word16 exp_en[], /* o : energy coefficients (4), exponent part, Q0 */
Word16 *ltpg /* o : LTP coding gain (log2()), Q13 */
)
{
Word32 s, L_temp;
Word16 i, exp, tmp;
Word16 ltp_res_en, pred_gain;
Word16 ltpg_exp, ltpg_frac;
/* Compute residual energy */
s = L_mac((Word32) 0, res[0], res[0]);
for (i = 1; i < L_subfr; i++)
s = L_mac(s, res[i], res[i]);
/* ResEn := 0 if ResEn < 200.0 (= 400 Q1) */
test();
if (L_sub (s, 400L) < 0)
{
frac_en[0] = 0; move16 ();
exp_en[0] = -15; move16 ();
}
else
{
exp = norm_l(s);
frac_en[0] = extract_h(L_shl(s, exp)); move16 ();
exp_en[0] = sub(15, exp); move16 ();
}
/* Compute ltp excitation energy */
s = L_mac((Word32) 0, exc[0], exc[0]);
for (i = 1; i < L_subfr; i++)
s = L_mac(s, exc[i], exc[i]);
exp = norm_l(s);
frac_en[1] = extract_h(L_shl(s, exp)); move16 ();
exp_en[1] = sub(15, exp); move16 ();
/* Compute scalar product <exc[],code[]> */
s = L_mac((Word32) 0, exc[0], code[0]);
for (i = 1; i < L_subfr; i++)
s = L_mac(s, exc[i], code[i]);
exp = norm_l(s);
frac_en[2] = extract_h(L_shl(s, exp)); move16 ();
exp_en[2] = sub(16-14, exp); move16 ();
/* Compute energy of LTP residual */
s = 0L; move32 ();
for (i = 0; i < L_subfr; i++)
{
L_temp = L_mult(exc[i], gain_pit);
L_temp = L_shl(L_temp, 1);
tmp = sub(res[i], round(L_temp)); /* LTP residual, Q0 */
s = L_mac (s, tmp, tmp);
}
exp = norm_l(s);
ltp_res_en = extract_h (L_shl (s, exp));
exp = sub (15, exp);
frac_en[3] = ltp_res_en; move16 ();
exp_en[3] = exp; move16 ();
/* calculate LTP coding gain, i.e. energy reduction LP res -> LTP res */
test (); test ();
if (ltp_res_en > 0 && frac_en[0] != 0)
{
/* gain = ResEn / LTPResEn */
pred_gain = div_s (shr (frac_en[0], 1), ltp_res_en);
exp = sub (exp, exp_en[0]);
/* L_temp = ltpGain * 2^(30 + exp) */
L_temp = L_deposit_h (pred_gain);
/* L_temp = ltpGain * 2^27 */
L_temp = L_shr (L_temp, add (exp, 3));
/* Log2 = log2() + 27 */
Log2(L_temp, <pg_exp, <pg_frac);
/* ltpg = log2(LtpGain) * 2^13 --> range: +- 4 = +- 12 dB */
L_temp = L_Comp (sub (ltpg_exp, 27), ltpg_frac);
*ltpg = round (L_shl (L_temp, 13)); /* Q13 */
}
else
{
*ltpg = 0; move16 ();
}
}
/*************************************************************************
*
* FUNCTION: calc_filt_energies
*
* PURPOSE: calculation of several energy coefficients for filtered
* excitation signals
*
* Compute coefficients need for the quantization and the optimum
* codebook gain gcu (for MR475 only).
*
* coeff[0] = y1 y1
* coeff[1] = -2 xn y1
* coeff[2] = y2 y2
* coeff[3] = -2 xn y2
* coeff[4] = 2 y1 y2
*
*
* gcu = <xn2, y2> / <y2, y2> (0 if <xn2, y2> <= 0)
*
* Product <y1 y1> and <xn y1> have been computed in G_pitch() and
* are in vector g_coeff[].
*
*************************************************************************/
void
calc_filt_energies(
enum Mode mode, /* i : coder mode */
Word16 xn[], /* i : LTP target vector, Q0 */
Word16 xn2[], /* i : CB target vector, Q0 */
Word16 y1[], /* i : Adaptive codebook, Q0 */
Word16 Y2[], /* i : Filtered innovative vector, Q12 */
Word16 g_coeff[], /* i : Correlations <xn y1> <y1 y1> */
/* computed in G_pitch() */
Word16 frac_coeff[],/* o : energy coefficients (5), fraction part, Q15 */
Word16 exp_coeff[], /* o : energy coefficients (5), exponent part, Q0 */
Word16 *cod_gain_frac,/* o: optimum codebook gain (fraction part), Q15 */
Word16 *cod_gain_exp /* o: optimum codebook gain (exponent part), Q0 */
)
{
Word32 s, ener_init;
Word16 i, exp, frac;
Word16 y2[L_SUBFR];
if (test(), sub(mode, MR795) == 0 || sub(mode, MR475) == 0)
{
ener_init = 0L; move32 ();
}
else
{
ener_init = 1L; move32 ();
}
for (i = 0; i < L_SUBFR; i++) {
y2[i] = shr(Y2[i], 3); move16 ();
}
frac_coeff[0] = g_coeff[0]; move16 ();
exp_coeff[0] = g_coeff[1]; move16 ();
frac_coeff[1] = negate(g_coeff[2]); move16 (); /* coeff[1] = -2 xn y1 */
exp_coeff[1] = add(g_coeff[3], 1); move16 ();
/* Compute scalar product <y2[],y2[]> */
s = L_mac(ener_init, y2[0], y2[0]);
for (i = 1; i < L_SUBFR; i++)
s = L_mac(s, y2[i], y2[i]);
exp = norm_l(s);
frac_coeff[2] = extract_h(L_shl(s, exp)); move16 ();
exp_coeff[2] = sub(15 - 18, exp); move16();
/* Compute scalar product -2*<xn[],y2[]> */
s = L_mac(ener_init, xn[0], y2[0]);
for (i = 1; i < L_SUBFR; i++)
s = L_mac(s, xn[i], y2[i]);
exp = norm_l(s);
frac_coeff[3] = negate(extract_h(L_shl(s, exp))); move16 ();
exp_coeff[3] = sub(15 - 9 + 1, exp); move16 ();
/* Compute scalar product 2*<y1[],y2[]> */
s = L_mac(ener_init, y1[0], y2[0]);
for (i = 1; i < L_SUBFR; i++)
s = L_mac(s, y1[i], y2[i]);
exp = norm_l(s);
frac_coeff[4] = extract_h(L_shl(s, exp)); move16 ();
exp_coeff[4] = sub(15 - 9 + 1, exp); move16();
if (test(), test (), sub(mode, MR475) == 0 || sub(mode, MR795) == 0)
{
/* Compute scalar product <xn2[],y2[]> */
s = L_mac(ener_init, xn2[0], y2[0]);
for (i = 1; i < L_SUBFR; i++)
s = L_mac(s, xn2[i], y2[i]);
exp = norm_l(s);
frac = extract_h(L_shl(s, exp));
exp = sub(15 - 9, exp);
if (test (), frac <= 0)
{
*cod_gain_frac = 0; move16 ();
*cod_gain_exp = 0; move16 ();
}
else
{
/*
gcu = <xn2, y2> / c[2]
= (frac>>1)/frac[2] * 2^(exp+1-exp[2])
= div_s(frac>>1, frac[2])*2^-15 * 2^(exp+1-exp[2])
= div_s * 2^(exp-exp[2]-14)
*/
*cod_gain_frac = div_s (shr (frac,1), frac_coeff[2]); move16 ();
*cod_gain_exp = sub (sub (exp, exp_coeff[2]), 14); move16 ();
}
}
}
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;
}
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;
}
/*
**
** Function: Lsp_Qnt()
**
** Description: Vector quantizes the LSP frequencies. The LSP
** vector is divided into 3 sub-vectors, or
** bands, of dimension 3, 3, and 4. Each band is
** quantized separately using a different VQ
** table. Each table has 256 entries, so the
** quantization generates three indices of 8 bits
** each. (Only the LSP vector for subframe 3 is
** quantized per frame.)
**
** Links to text: Section 2.5
**
** Arguments:
**
** Word16 CurrLsp[] Unquantized LSP frequencies for the current frame (10 words)
** Word16 PrevLsp[] LSP frequencies from the previous frame (10 words)
**
** Outputs: Quantized LSP frequencies for the current frame (10 words)
**
** Return value:
**
** Word32 Long word packed with the 3 VQ indices. Band 0
** corresponds to bits [23:16], band 1 corresponds
** to bits [15:8], and band 2 corresponds to bits [7:0].
** (Bit 0 is the least significant.)
**
*/
Word32 Lsp_Qnt( Word16 *CurrLsp, Word16 *PrevLsp )
{
int i ;
Word16 Wvect[LpcOrder] ;
Word16 Tmp0,Tmp1 ;
Word16 Exp ;
/*
* Compute the VQ weighting vector. The weights assign greater
* precision to those frequencies that are closer together.
*/
/* Compute the end differences */
Wvect[0] = sub( CurrLsp[1], CurrLsp[0] ) ;
Wvect[LpcOrder-1] = sub( CurrLsp[LpcOrder-1], CurrLsp[LpcOrder-2] ) ;
/* Compute the rest of the differences */
for ( i = 1 ; i < LpcOrder-1 ; i ++ ) {
Tmp0 = sub( CurrLsp[i+1], CurrLsp[i] ) ;
Tmp1 = sub( CurrLsp[i], CurrLsp[i-1] ) ;
if ( Tmp0 > Tmp1 )
Wvect[i] = Tmp1 ;
else
Wvect[i] = Tmp0 ;
}
/* Invert the differences */
Tmp0 = (Word16) 0x0020 ;
for ( i = 0 ; i < LpcOrder ; i ++ ) {
if ( Wvect[i] > Tmp0 )
Wvect[i] = div_s( Tmp0, Wvect[i] ) ;
else
Wvect[i] = MAX_16 ;
}
/* Normalize the weight vector */
Tmp0 = (Word16) 0 ;
for ( i = 0 ; i < LpcOrder ; i ++ )
if ( Wvect[i] > Tmp0 )
Tmp0 = Wvect[i] ;
Exp = norm_s( Tmp0 ) ;
for ( i = 0 ; i < LpcOrder ; i ++ )
Wvect[i] = shl( Wvect[i], Exp ) ;
/*
* Compute the VQ target vector. This is the residual that remains
* after subtracting both the DC and predicted
* components.
*/
/*
* Subtract the DC component from both the current and previous LSP
* vectors.
*/
for ( i = 0 ; i < LpcOrder ; i ++ ) {
CurrLsp[i] = sub( CurrLsp[i], LspDcTable[i] ) ;
PrevLsp[i] = sub( PrevLsp[i], LspDcTable[i] ) ;
}
/*
* Generate the prediction vector and subtract it. Use a constant
* first-order predictor based on the previous (DC-free) LSP
* vector.
*/
for ( i = 0 ; i < LpcOrder ; i ++ ) {
Tmp0 = mult_r( PrevLsp[i], (Word16) LspPrd0 ) ;
CurrLsp[i] = sub( CurrLsp[i], Tmp0 ) ;
}
/*
* Add the DC component back to the previous LSP vector. This
* vector is needed in later routines.
*/
for ( i = 0 ; i < LpcOrder ; i ++ )
PrevLsp[i] = add( PrevLsp[i], LspDcTable[i] ) ;
/*
* Do the vector quantization for all three bands
*/
return Lsp_Svq( CurrLsp, Wvect ) ;
}
void a2lsp(
Word16 pc[], /* (i) Q12: predictor coefficients */
Word16 lsp[], /* (o) Q15: line spectral pairs */
Word16 old_lsp[]) /* (i) Q15: old lsp */
{
Word16 i, j, exp;
Word16 fa_man[NAB], fa_exp[NAB], fb_man[NAB], fb_exp[NAB];
Word16 ta_man[NAB], ta_exp[NAB], tb_man[NAB], tb_exp[NAB];
Word16 *t_man, *t_exp;
Word32 a0;
Word16 nd2, nf, ngrd;
Word16 xroot, xlow, ylow, ind, xhigh, yhigh, xmid, ymid, dx, dy, dxdy, x, sign;
/* Find normalization for fa and fb */
/* fb[0] = fa[0] = 1.0; */
/* for (i = 1, j = LPCO; i <= (LPCO/2); i++, j--) { */
/* fa[i] = pc[i] + pc[j] - fa[i-1]; */
/* fb[i] = pc[i] - pc[j] + fb[i-1]; */
/* } */
fa_man[0] = 16384;
fa_exp[0] = 6; // fa_man[0] in high 16-bits >> fa_exp[0] = 1.0 in Q24
fb_man[0] = 16384;
fb_exp[0] = 6; // fb_man[0] in high 16-bits >> fb_exp[0] = 1.0 in Q24
for (i = 1, j = LPCO; i <= (LPCO/2); i++, j--) {
a0 = L_mult0(pc[i], 4096); // Q24
a0 = L_mac0(a0, pc[j], 4096); // Q24
a0 = L_sub(a0, L_shr(L_deposit_h(fa_man[i-1]),fa_exp[i-1])); // Q24
fa_exp[i] = norm_l(a0);
fa_man[i] = intround(L_shl(a0, fa_exp[i])); // Q(8+fb_exp[i])
a0 = L_mult0(pc[i], 4096); // Q24
a0 = L_msu0(a0, pc[j], 4096); // Q24
a0 = L_add(a0, L_shr(L_deposit_h(fb_man[i-1]),fb_exp[i-1])); // Q24
fb_exp[i] = norm_l(a0);
fb_man[i] = intround(L_shl(a0, fb_exp[i])); // Q(8+fb_exp[i])
}
nd2 = (LPCO)/2;
/* ta[] and tb[] in Q(7+exp) */
/* ta[0] = fa[nab-1]; ta[i] = 2.0 * fa[j]; */
/* tb[0] = fb[nab-1]; tb[i] = 2.0 * fb[j]; */
ta_man[0] = fa_man[NAB-1];
ta_exp[0] = add(fa_exp[NAB-1], 1);
tb_man[0] = fb_man[NAB-1];
tb_exp[0] = add(fb_exp[NAB-1], 1);
for (i = 1, j = NAB - 2; i < NAB; ++i, --j) {
ta_man[i] = fa_man[j];
ta_exp[i] = fa_exp[j];
tb_man[i] = fb_man[j];
tb_exp[i] = fb_exp[j];
}
nf = 0;
t_man = ta_man;
t_exp = ta_exp;
xroot = 0x7fff;
ngrd = 0;
xlow = grid[0]; // Q15
ylow = FNevChebP(xlow, t_man, t_exp, nd2);
ind = 0;
/* Root search loop */
while (ngrd<(Ngrd-1) && nf < LPCO) {
ngrd++;
xhigh = xlow;
yhigh = ylow;
xlow = grid[ngrd];
ylow = FNevChebP(xlow, t_man, t_exp, nd2);
if ( L_mult(ylow ,yhigh) <= 0) {
/* Bisections of the interval containing a sign change */
dx = xhigh - xlow;
for (i = 1; i <= NBIS; ++i) {
dx = shr(dx, 1);
xmid = add(xlow, dx);
ymid = FNevChebP(xmid, t_man, t_exp, nd2);
if (L_mult(ylow,ymid) <= 0) {
yhigh = ymid;
xhigh = xmid;
} else {
ylow = ymid;
xlow = xmid;
}
}
/*
* Linear interpolation in the subinterval with a sign change
* (take care if yhigh=ylow=0)
*/
dx = sub(xhigh, xlow);
dy = sub(ylow, yhigh);
if (dy != 0) {
sign = dy;
dy = abs_s(dy);
exp = norm_s(dy);
dy = shl(dy, exp);
/* The maximum grid distance is 1629 => */
/* Maximum dx=1629/2^4=101.8125, i.e. 16384/101.8125=160.92~128 (7 bits) */
/* However, due to the starting point for the search of a new root, */
/* xlow = xroot, 1 more bit of headroom for the division is required. */
dxdy = div_s(shl(dx,6), dy);
a0 = L_mult(dxdy, ylow);
a0 = L_shr(a0, sub(6, exp));
x = intround(a0);
if(sign < 0) x = negate(x);
xmid = add(xlow, x);
}
else {
xmid = add(xlow, shr(dx,1));
}
/* acos mapping for New lsp component */
while (( costable[ind] >= xmid ) && (ind < 63)) ind++;
ind--;
a0 = L_mult( sub(xmid, costable[ind]) , acosslope[ind] );
x = intround(L_shl(a0, 4));
lsp[nf] = add(x, shl(ind, 9));
++nf;
/* Start the search for the roots of next polynomial at the estimated
* location of the root just found. We have to catch the case that the
* two polynomials have roots at the same place to avoid getting stuck at
* that root.
*/
if (xmid >= xroot) xmid = xlow - dx;
xroot = xmid;
if (t_man == ta_man){
t_man = tb_man;
t_exp = tb_exp;
}
else{
t_man = ta_man;
t_exp = ta_exp;
}
xlow = xmid;
ylow = FNevChebP(xlow, t_man, t_exp, nd2);
}
}
/* Check if all LSPs are found */
if( sub(nf, LPCO) < 0)
{
W16copy(lsp, old_lsp, LPCO);
}
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;
}
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 () */
/*
**************************************************************************
* Function: Post_Filter
* Purpose: postfiltering of synthesis speech.
* Description:
* The postfiltering process is described as follows:
*
* - inverse filtering of syn[] through A(z/0.7) to get res2[]
* - tilt compensation filtering; 1 - MU*k*z^-1
* - synthesis filtering through 1/A(z/0.75)
* - adaptive gain control
*
**************************************************************************
*/
int Post_Filter (
Post_FilterState *st, /* i/o : post filter states */
enum Mode mode, /* i : AMR mode */
Word16 *syn, /* i/o : synthesis speech (postfiltered is output) */
Word16 *Az_4 /* i : interpolated LPC parameters in all subfr. */
)
{
/*-------------------------------------------------------------------*
* Declaration of parameters *
*-------------------------------------------------------------------*/
Word16 Ap3[MP1], Ap4[MP1]; /* bandwidth expanded LP parameters */
Word16 *Az; /* pointer to Az_4: */
/* LPC parameters in each subframe */
Word16 i_subfr; /* index for beginning of subframe */
Word16 h[L_H];
Word16 i;
Word16 temp1, temp2;
Word32 L_tmp;
Word16 *syn_work = &st->synth_buf[M];
move16 ();
/*-----------------------------------------------------*
* Post filtering *
*-----------------------------------------------------*/
Copy (syn, syn_work , L_FRAME);
Az = Az_4;
for (i_subfr = 0; i_subfr < L_FRAME; i_subfr += L_SUBFR)
{
/* Find weighted filter coefficients Ap3[] and ap[4] */
test ();
test ();
if (sub(mode, MR122) == 0 || sub(mode, MR102) == 0)
{
Weight_Ai (Az, gamma3_MR122, Ap3);
Weight_Ai (Az, gamma4_MR122, Ap4);
}
else
{
Weight_Ai (Az, gamma3, Ap3);
Weight_Ai (Az, gamma4, Ap4);
}
/* filtering of synthesis speech by A(z/0.7) to find res2[] */
Residu (Ap3, &syn_work[i_subfr], st->res2, L_SUBFR);
/* tilt compensation filter */
/* impulse response of A(z/0.7)/A(z/0.75) */
Copy (Ap3, h, M + 1);
Set_zero (&h[M + 1], L_H - M - 1);
Syn_filt (Ap4, h, h, L_H, &h[M + 1], 0);
/* 1st correlation of h[] */
L_tmp = L_mult (h[0], h[0]);
for (i = 1; i < L_H; i++)
{
L_tmp = L_mac (L_tmp, h[i], h[i]);
}
temp1 = extract_h (L_tmp);
L_tmp = L_mult (h[0], h[1]);
for (i = 1; i < L_H - 1; i++)
{
L_tmp = L_mac (L_tmp, h[i], h[i + 1]);
}
temp2 = extract_h (L_tmp);
test ();
if (temp2 <= 0)
{
temp2 = 0;
move16 ();
}
else
{
temp2 = mult (temp2, MU);
temp2 = div_s (temp2, temp1);
}
preemphasis (st->preemph_state, st->res2, temp2, L_SUBFR);
/* filtering through 1/A(z/0.75) */
Syn_filt (Ap4, st->res2, &syn[i_subfr], L_SUBFR, st->mem_syn_pst, 1);
/* scale output to input */
agc (st->agc_state, &syn_work[i_subfr], &syn[i_subfr],
AGC_FAC, L_SUBFR);
Az += MP1;
}
/* update syn_work[] buffer */
Copy (&syn_work[L_FRAME - M], &syn_work[-M], M);
return 0;
}
void DIV_S(void)
{
if (check_cop1_unusable()) return;
div_s(reg_cop1_simple[cffs], reg_cop1_simple[cfft], reg_cop1_simple[cffd]);
PC++;
}
void Az_lsp (
INT16 a[], /* (i) : predictor coefficients */
INT16 lsp[], /* (o) : line spectral pairs */
INT16 old_lsp[] /* (i) : old lsp[] (in case not found 10 roots) */
)
{
INT16 i, j, nf, ip;
INT16 xlow, ylow, xhigh, yhigh, xmid, ymid, xint;
INT16 x, y, sign, exp;
INT16 *coef;
INT16 f1[M / 2 + 1], f2[M / 2 + 1];
INT32 t0=0;
VPP_EFR_PROFILE_FUNCTION_ENTER(Az_lsp);
/*-------------------------------------------------------------*
* 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; /* f1[0] = 1.0 */
f2[0] = 1024; /* f2[0] = 1.0 */
for (i = 0; i < NC; i++)
{
//VPP_MLX16 (t0_hi,t0_lo,a[i + 1], 8192);
//VPP_MLA16 ( t0_hi, t0_lo, a[M - i], 8192);
//t0 = VPP_SCALE64_TO_16( t0_hi, t0_lo);
//x = EXTRACT_H(t0);
t0 = (INT32) a[i + 1] + (INT32)a[M - i];
x = (INT16)(L_SHR_D(t0,2));
/* f1[i+1] = a[i+1] + a[M-i] - f1[i] */
f1[i + 1] = SUB (x, f1[i]);
//VPP_MLX16(t0_hi, t0_lo, a[i + 1], 8192);
//VPP_MLA16(t0_hi, t0_lo, a[M - i], -8192);
//x = EXTRACT_H(VPP_SCALE64_TO_16(t0_hi, t0_lo));
t0 = (INT32) a[i + 1] - (INT32)a[M - i];
x = (INT16)(L_SHR_D(t0,2));
//f2[i + 1] = add (x, f2[i]);
f2[i + 1] = ADD(x, f2[i]);
}
/*-------------------------------------------------------------*
* find the LSPs using the Chebychev pol. evaluation *
*-------------------------------------------------------------*/
nf = 0; /* number of found frequencies */
ip = 0; /* indicator for f1 or f2 */
coef = f1;
xlow = grid[0];
ylow = Chebps (xlow, coef, NC);
j = 0;
/* while ( (nf < M) && (j < grid_points) ) */
//while ((sub (nf, M) < 0) && (sub (j, grid_points) < 0))
while ((SUB (nf, M) < 0) && (SUB (j, grid_points) < 0))
{
j++;
xhigh = xlow;
yhigh = ylow;
xlow = grid[j];
ylow = Chebps (xlow, coef, NC);
//if (L_mult (ylow, yhigh) <= (INT32) 0L)
if (L_MULT(ylow, yhigh) <= (INT32) 0L)
{
/* divide 4 times the interval */
for (i = 0; i < 4; i++)
{
/* xmid = (xlow + xhigh)/2 */
// xmid = add (shr (xlow, 1), shr (xhigh, 1));
xmid = ADD ((SHR_D(xlow, 1)),(SHR_D(xhigh, 1)));
ymid = Chebps (xmid, coef, NC);
//if (L_mult (ylow, ymid) <= (INT32) 0L)
if (L_MULT(ylow, ymid) <= (INT32) 0L)
{
yhigh = ymid;
xhigh = xmid;
}
else
{
ylow = ymid;
xlow = xmid;
}
}
/*-------------------------------------------------------------*
* Linear interpolation *
* xint = xlow - ylow*(xhigh-xlow)/(yhigh-ylow); *
*-------------------------------------------------------------*/
//x = sub (xhigh, xlow);
x = SUB (xhigh, xlow);
//y = sub (yhigh, ylow);
y = SUB (yhigh, ylow);
if (y == 0)
{
xint = xlow;
}
else
{
sign = y;
//y = abs_s (y);
y = ABS_S(y);
exp = norm_s (y);
//y = shl (y, exp);
y = SHL(y, exp);
y = div_s ((INT16) 16383, y);
//t0 = L_mult (x, y);
t0 = L_MULT(x, y);
//t0 = L_shr (t0, sub (20, exp));
t0 = L_SHR_V(t0, SUB (20, exp));
//y = extract_l (t0); /* y= (xhigh-xlow)/(yhigh-ylow) */
y = EXTRACT_L(t0);
if (sign < 0)
{
//y = negate (y);
y = NEGATE(y);
}
//t0 = L_mult (ylow, y);
t0 = L_MULT(ylow, y);
//t0 = L_shr (t0, 11);
t0 = L_SHR_D(t0, 11);
//xint = sub (xlow, extract_l (t0)); /* xint = xlow - ylow*y */
xint = SUB (xlow, EXTRACT_L(t0));
}
lsp[nf] = xint;
xlow = xint;
nf++;
if (ip == 0)
{
ip = 1;
coef = f2;
}
else
{
ip = 0;
coef = f1;
}
ylow = Chebps (xlow, coef, NC);
}
}
/* Check if M roots found */
//if (sub (nf, M) < 0)
if (SUB (nf, M) < 0)
{
for (i = 0; i < M; i++)
{
lsp[i] = old_lsp[i];
}
}
VPP_EFR_PROFILE_FUNCTION_EXIT(Az_lsp);
return;
}
void Post_Filter (
INT16 *syn, /* in/out: synthesis speech (postfiltered is output) */
INT16 *Az_4 /* input: interpolated LPC parameters in all subframes */
)
{
/*-------------------------------------------------------------------*
* Declaration of parameters *
*-------------------------------------------------------------------*/
INT16 syn_pst[L_FRAME]; /* post filtered synthesis speech */
INT16 Ap3[MP1], Ap4[MP1]; /* bandwidth expanded LP parameters */
INT16 *Az; /* pointer to Az_4: */
/* LPC parameters in each subframe */
INT16 i_subfr; /* index for beginning of subframe */
INT16 h[L_H];
INT16 i;
INT16 temp1, temp2;
//INT32 L_tmp;
register INT32 tmp_hi=0;
register UINT32 tmp_lo=0;
VPP_EFR_PROFILE_FUNCTION_ENTER(Post_Filter);
/*-----------------------------------------------------*
* Post filtering *
*-----------------------------------------------------*/
Az = Az_4;
for (i_subfr = 0; i_subfr < L_FRAME; i_subfr += L_SUBFR)
{
/* Find weighted filter coefficients Ap3[] and ap[4] */
Weight_Ai (Az, F_gamma3, Ap3);
Weight_Ai (Az, F_gamma4, Ap4);
/* filtering of synthesis speech by A(z/0.7) to find res2[] */
Residu (Ap3, &syn[i_subfr], res2, L_SUBFR);
/* tilt compensation filter */
/* impulse response of A(z/0.7)/A(z/0.75) */
Copy (Ap3, h, M + 1);
//Set_zero (&h[M + 1], L_H - M - 1);
memset ((INT8*)&h[M + 1], 0, (L_H - M - 1)<<1);
Syn_filt (Ap4, h, h, L_H, &h[M + 1], 0);
/* 1st correlation of h[] */
//L_tmp = L_MULT(h[0], h[0]);
VPP_MLX16(tmp_hi,tmp_lo, h[0], h[0]);
for (i = 1; i < L_H; i++)
{
//L_tmp = L_MAC(L_tmp, h[i], h[i]);
VPP_MLA16(tmp_hi,tmp_lo, h[i], h[i]);
}
//temp1 = extract_h (L_tmp);
//temp1 = EXTRACT_H(VPP_SCALE64_TO_16(tmp_hi,tmp_lo));
temp1 = L_SHR_D((INT32)tmp_lo, 15);
//L_tmp = L_MULT(h[0], h[1]);
VPP_MLX16(tmp_hi,tmp_lo, h[0], h[1]);
for (i = 1; i < L_H - 1; i++)
{
//L_tmp = L_MAC(L_tmp, h[i], h[i + 1]);
VPP_MLA16(tmp_hi,tmp_lo, h[i], h[i + 1]);
}
//temp2 = extract_h (L_tmp);
//temp2 = EXTRACT_H(VPP_SCALE64_TO_16(tmp_hi,tmp_lo));
temp2 = L_SHR_D((INT32)tmp_lo, 15);
if (temp2 <= 0)
{
temp2 = 0;
}
else
{
//temp2 = mult (temp2, MU);
temp2 = MULT(temp2, MU);
temp2 = div_s (temp2, temp1);
}
preemphasis (res2, temp2, L_SUBFR);
/* filtering through 1/A(z/0.75) */
Syn_filt (Ap4, res2, &syn_pst[i_subfr], L_SUBFR, mem_syn_pst, 1);
/* scale output to input */
agc (&syn[i_subfr], &syn_pst[i_subfr], AGC_FAC, L_SUBFR);
Az += MP1;
}
/* update syn[] buffer */
Copy (&syn[L_FRAME - M], &syn[-M], M);
/* overwrite synthesis speech by postfiltered synthesis speech */
Copy (syn_pst, syn, L_FRAME);
VPP_EFR_PROFILE_FUNCTION_EXIT(Post_Filter);
return;
}
Word16 G_pitch( /* (o) Q14 : Gain of pitch lag saturated to 1.2 */
Word16 xn[], /* (i) : Pitch target. */
Word16 y1[], /* (i) : Filtered adaptive codebook. */
Word16 g_coeff[], /* (i) : Correlations need for gain quantization. */
Word16 L_subfr /* (i) : Length of subframe. */
)
{
Word16 i;
Word16 xy, yy, exp_xy, exp_yy, gain;
Word32 s;
Word16 scaled_y1[L_SUBFR];
/* divide "y1[]" by 4 to avoid overflow */
for(i=0; i<L_subfr; i++)
scaled_y1[i] = shr(y1[i], 2);
/* Compute scalar product <y1[],y1[]> */
Overflow = 0;
s = 1; /* Avoid case of all zeros */
for(i=0; i<L_subfr; i++)
s = L_mac(s, y1[i], y1[i]);
if (Overflow == 0) {
exp_yy = norm_l(s);
yy = round( L_shl(s, exp_yy) );
}
else {
s = 1; /* Avoid case of all zeros */
for(i=0; i<L_subfr; i++)
s = L_mac(s, scaled_y1[i], scaled_y1[i]);
exp_yy = norm_l(s);
yy = round( L_shl(s, exp_yy) );
exp_yy = sub(exp_yy, 4);
}
/* Compute scalar product <xn[],y1[]> */
Overflow = 0;
s = 0;
for(i=0; i<L_subfr; i++)
s = L_mac(s, xn[i], y1[i]);
if (Overflow == 0) {
exp_xy = norm_l(s);
xy = round( L_shl(s, exp_xy) );
}
else {
s = 0;
for(i=0; i<L_subfr; i++)
s = L_mac(s, xn[i], scaled_y1[i]);
exp_xy = norm_l(s);
xy = round( L_shl(s, exp_xy) );
exp_xy = sub(exp_xy, 2);
}
g_coeff[0] = yy;
g_coeff[1] = sub(15, exp_yy);
g_coeff[2] = xy;
g_coeff[3] = sub(15, exp_xy);
/* If (xy <= 0) gain = 0 */
if (xy <= 0)
{
g_coeff[3] = -15; /* Force exp_xy to -15 = (15-30) */
return( (Word16) 0);
}
/* compute gain = xy/yy */
xy = shr(xy, 1); /* Be sure xy < yy */
gain = div_s( xy, yy);
i = sub(exp_xy, exp_yy);
gain = shr(gain, i); /* saturation if > 1.99 in Q14 */
/* if(gain >1.2) gain = 1.2 in Q14 */
if( sub(gain, 19661) > 0)
{
gain = 19661;
}
return(gain);
}
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 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;
}
/*
********************************************************************************
* PUBLIC PROGRAM CODE
********************************************************************************
*/
Word16 hp_max (
Word32 corr[], /* i : correlation vector. */
Word16 scal_sig[], /* i : scaled signal. */
Word16 L_frame, /* i : length of frame to compute pitch */
Word16 lag_max, /* i : maximum lag */
Word16 lag_min, /* i : minimum lag */
Word16 *cor_hp_max) /* o : max high-pass filtered norm. correlation */
{
Word16 i;
Word16 *p, *p1;
Word32 max, t0, t1;
Word16 max16, t016, cor_max;
Word16 shift, shift1, shift2;
max = MIN_32; move32 ();
t0 = 0L; move32 ();
for (i = lag_max-1; i > lag_min; i--)
{
/* high-pass filtering */
t0 = L_sub (L_sub(L_shl(corr[-i], 1), corr[-i-1]), corr[-i+1]);
t0 = L_abs (t0);
test ();
if (L_sub (t0, max) >= 0)
{
max = t0; move32 ();
}
}
/* compute energy */
p = scal_sig; move16 ();
p1 = &scal_sig[0]; move16 ();
t0 = 0L; move32 ();
for (i = 0; i < L_frame; i++, p++, p1++)
{
t0 = L_mac (t0, *p, *p1);
}
p = scal_sig; move16 ();
p1 = &scal_sig[-1]; move16 ();
t1 = 0L; move32 ();
for (i = 0; i < L_frame; i++, p++, p1++)
{
t1 = L_mac (t1, *p, *p1);
}
/* high-pass filtering */
t0 = L_sub(L_shl(t0, 1), L_shl(t1, 1));
t0 = L_abs (t0);
/* max/t0 */
shift1 = sub(norm_l(max), 1);
max16 = extract_h(L_shl(max, shift1));
shift2 = norm_l(t0);
t016 = extract_h(L_shl(t0, shift2));
test ();
if (t016 != 0)
{
cor_max = div_s(max16, t016);
}
else
{
cor_max = 0; move16 ();
}
shift = sub(shift1, shift2);
test ();
if (shift >= 0)
{
*cor_hp_max = shr(cor_max, shift); move16 (); /* Q15 */
}
else
{
*cor_hp_max = shl(cor_max, negate(shift)); move16 (); /* Q15 */
}
return 0;
}
int exe(FILE* program) //program指向存有待执行程序机器码的文件
{
char* tmp_instru=(char*)malloc(33*sizeof(char)); //读机器码
programTail=programHead;
while(fscanf(program,"%s",tmp_instru)!=EOF)
{
instru=0;
int i=0;
unsigned j=1;
for(i=31;i>=0;i--)
{
if(tmp_instru[i]=='1')
{
instru+=j;
j*=2;
}
else
{
j*=2;
}
}//将机器码转为unsi
unsigned char* tmp_R=&instru;
for(i=0;i<4;i++)
{
writeMymemory(programTail+i,tmp_R+i);//装载指令
}
programTail+=4;//最后一条指令的下一条指令的地址，用来判断程序是否执行完
}
pcShort=programHead;
pc=pcShort;
while(pcShort!=programTail)
{
instru=0; //指令寄存器清零
unsigned char* tmp_R=&instru;
unsigned short addr=addrToMyAddr(pc);
int i;
for(i=0;i<4;i++)
{
readMymemory(addr+i,tmp_R+i);//取指令
}
unsigned tmp=instru>>26;//得到指令op
//printf("the op is : %u\n",tmp);
unsigned numRs=0,numRt=0,numRd=0,numFs=0,numFt=0,numFd=0,tmp_fuc=0;
switch(tmp)
{
case 0x00000023:
numRs=instru<<6>>27;
numRt=instru<<11>>27;
RS1=myRegister+numRt;
RS2=myRegister+numRs;
lig=instru<<16>>16;
pc=lw(pc);
break;
case 0x0000002B:
numRs=instru<<6>>27;
numRt=instru<<11>>27;
RS1=myRegister+numRt;
RS2=myRegister+numRs;
lig=instru<<16>>16;
pc=sw(pc);
break;
case 0x00000008:
numRs=instru<<6>>27;
numRt=instru<<11>>27;
RS1=myRegister+numRt;
RS2=myRegister+numRs;
lig=instru<<16>>16;
pc=addi(pc);
break;
case 0x00000009:
numRs=instru<<6>>27;
numRt=instru<<11>>27;
RS1=myRegister+numRt;
RS2=myRegister+numRs;
lig=instru<<16>>16;
pc=addiu(pc);
break;
case 0x0000000A:
numRs=instru<<6>>27;
numRt=instru<<11>>27;
RS1=myRegister+numRt;
RS2=myRegister+numRs;
lig=instru<<16>>16;
pc=slti(pc);
break;
case 0x0000000B:
numRs=instru<<6>>27;
numRt=instru<<11>>27;
RS1=myRegister+numRt;
RS2=myRegister+numRs;
lig=instru<<16>>16;
pc=sltiu(pc);
break;
case 0x0000000C:
numRs=instru<<6>>27;
numRt=instru<<11>>27;
RS1=myRegister+numRt;
RS2=myRegister+numRs;
lig=instru<<16>>16;
pc=andi(pc);
break;
case 0x0000000D:
numRs=instru<<6>>27;
numRt=instru<<11>>27;
RS1=myRegister+numRt;
RS2=myRegister+numRs;
lig=instru<<16>>16;
pc=ori(pc);
break;
case 0x0000000E:
numRs=instru<<6>>27;
numRt=instru<<11>>27;
RS1=myRegister+numRt;
RS2=myRegister+numRs;
lig=instru<<16>>16;
pc=xori(pc);
break;
case 0x00000024:
numRs=instru<<6>>27;
numRt=instru<<11>>27;
RS1=myRegister+numRt;
RS2=myRegister+numRs;
lig=instru<<16>>16;
pc=lbu(pc);
break;
case 0x00000020:
numRs=instru<<6>>27;
numRt=instru<<11>>27;
RS1=myRegister+numRt;
RS2=myRegister+numRs;
lig=instru<<16>>16;
pc=lb(pc);
break;
case 0x00000028:
numRs=instru<<6>>27;
numRt=instru<<11>>27;
RS1=myRegister+numRt;
RS2=myRegister+numRs;
lig=instru<<16>>16;
pc=sb(pc);
break;
case 0x0000000F:
numRs=instru<<6>>27;
numRt=instru<<11>>27;
RS1=myRegister+numRt;
RS2=myRegister+numRs;
lig=instru<<16>>16;
pc=lui(pc);
break;
case 0x00000004:
numRs=instru<<6>>27;
numRt=instru<<11>>27;
RS1=myRegister+numRt;
RS2=myRegister+numRs;
lig=instru<<16>>16;
pc=beq(pc);
break;
case 0x00000005:
numRs=instru<<6>>27;
numRt=instru<<11>>27;
RS1=myRegister+numRt;
RS2=myRegister+numRs;
//printf("%u,%u,%u,%u\n",numRt,numRs,*RS1,*RS2);
lig=instru<<16>>16;
// printf("%u\n",lig);
pc=bne(pc);
break;
case 0x00000006:
numRs=instru<<6>>27;
numRt=instru<<11>>27;
RS1=myRegister+numRt;
RS2=myRegister+numRs;
lig=instru<<16>>16;
pc=blez(pc);
break;
case 0x00000007:
numRs=instru<<6>>27;
numRt=instru<<11>>27;
RS1=myRegister+numRt;
RS2=myRegister+numRs;
lig=instru<<16>>16;
pc=bgtz(pc);
break;
case 0x00000001:
numRs=instru<<6>>27;
numRt=instru<<11>>27;
RS1=myRegister+numRt;
RS2=myRegister+numRs;
lig=instru<<16>>16;
pc=bltz(pc);
break;
case 0x00000002:
pc=j(pc);
break;
case 0x00000003:
pc=jal(pc);
break;
case 0x00000000:
numRs=instru<<6>>27;
numRt=instru<<11>>27;
numRd=instru<<16>>27;
RS1=myRegister+numRt;
RS2=myRegister+numRs;
RD=myRegister+numRd;
tmp_fuc=instru%64;
switch(tmp_fuc)
{
case 32:
pc=add(pc);
break;
case 33:
pc=addu(pc);
break;
case 34:
pc=sub(pc);
break;
case 35:
pc=subu(pc);
break;
case 24:
pc=mul(pc);
break;
case 25:
pc=mulu(pc);
break;
case 26:
pc=myDiv(pc);
break;
case 27:
pc=divu(pc);
break;
case 42:
pc=slt(pc);
break;
case 43:
pc=sltu(pc);
break;
case 36:
pc=myAnd(pc);
break;
case 37:
pc=myOr(pc);
break;
case 39:
pc=nor(pc);
break;
case 40:
pc=myXor(pc);
break;
case 8:
pc=jr(pc);
break;
case 9:
pc=jalr(pc);
break;
case 0:
pc=nop(pc);
break;
case 16:
pc=mfhi(pc);
break;
case 18:
pc=mflo(pc);
break;
default:
break;
}
break;
case 0x00000010:
numRt=instru<<11>>27;
numRd=instru<<16>>27;
RS1=myRegister+numRt;
if(numRd==14)
{
pc=mfepc(pc);
}
else if(numRd==13)
{
pc=mfco(pc);
}
else return -1;
break;
case 0x00000031:
numRs=instru<<6>>27;
numFt=instru<<11>>27;
RS2=myRegister+numRs;
FS1=myFloatReg+numFt;
lig=instru<<16>>16;
pc=lwc1(pc);
//printf("/********\nL.S %u %u\n****************/\n",numFt,numRs);
break;
case 0x0000001F:
numRs=instru<<6>>27;
numFt=instru<<11>>27;
RS2=myRegister+numRs;
FS1=myFloatReg+numFt;
lig=instru<<16>>16;
pc=S_D(pc);
//printf("/********\nL.D %u %u\n****************/\n",numFt,numRs);
break;
case 0x0000001E:
numRs=instru<<6>>27;
numFt=instru<<11>>27;
RS2=myRegister+numRs;
FS1=myFloatReg+numFt;
lig=instru<<16>>16;
//printf("/********\nS.D %u %u\n****************/\n",numFt,numRs);
pc=S_D(pc);
break;
case 0x00000039:
numRs=instru<<6>>27;
numFt=instru<<11>>27;
RS2=myRegister+numRs;
FS1=myFloatReg+numFt;
lig=instru<<16>>16;
//printf("/********\nS.S %u %u\n****************/\n",numFt,numRs);
pc=swc1(pc);
break;
case 0x00000011:
numFt=instru<<11>>27;
numFs=instru<<16>>27;
numFd=instru<<21>>27;
FS1=myFloatReg+numFt;
FS2=myFloatReg+numFs;
FD=myFloatReg+numFd;
numRs=instru<<6>>27;
tmp_fuc=instru%64;
//printf("%u %u\n",tmp_fuc,numRs);
if(numRs==0)
{
switch(tmp_fuc)
{
case 0:
pc=add_s(pc);
break;
case 1:
pc=sub_s(pc);
break;
case 2:
pc=mul_s(pc);
case 3:
pc=div_s(pc);
default:
break;
}
}
else if(numRs==1)
{
switch(tmp_fuc)
{
case 0:
pc=add_d(pc);
//printf("/****************\nADD.D %u %u %u\n*****************/\n",numFd,numFt,numFs);
break;
case 1:
pc=sub_d(pc);
break;
case 2:
pc=mul_d(pc);
case 3:
pc=div_d(pc);
default:
break;
}
}
default:break;
}
pcShort=pc%0x00010000;
//printf("%u %u\n",pc,pcShort);
//printf("%u %u\n",pcShort,programTail);
}
return 0;
}
/*
**************************************************************************
*
* 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;
}
/*
**************************************************************************
*
* 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;
}
/*----------------------------------------------------------------------------
; FUNCTION CODE
----------------------------------------------------------------------------*/
Word16 hp_max(
Word32 corr[], /* i : correlation vector. */
Word16 scal_sig[], /* i : scaled signal. */
Word16 L_frame, /* i : length of frame to compute pitch */
Word16 lag_max, /* i : maximum lag */
Word16 lag_min, /* i : minimum lag */
Word16 *cor_hp_max, /* o : max high-pass filtered norm. correlation */
Flag *pOverflow /* i/o : overflow Flag */
)
{
Word16 i;
Word16 *p, *p1;
Word32 max, t0, t1;
Word16 max16, t016, cor_max;
Word16 shift, shift1, shift2;
Word32 L_temp;
max = MIN_32;
t0 = 0L;
for (i = lag_max - 1; i > lag_min; i--)
{
/* high-pass filtering */
t0 = L_shl(corr[-i], 1, pOverflow);
L_temp = L_sub(t0, corr[-i-1], pOverflow);
t0 = L_sub(L_temp, corr[-i+1], pOverflow);
t0 = L_abs(t0);
if (t0 >= max)
{
max = t0;
}
}
/* compute energy */
p = scal_sig;
p1 = &scal_sig[0];
t0 = 0L;
for (i = 0; i < L_frame; i++, p++, p1++)
{
t0 = L_mac(t0, *p, *p1, pOverflow);
}
p = scal_sig;
p1 = &scal_sig[-1];
t1 = 0L;
for (i = 0; i < L_frame; i++, p++, p1++)
{
t1 = L_mac(t1, *p, *p1, pOverflow);
}
/* high-pass filtering */
L_temp = L_shl(t0, 1, pOverflow);
t1 = L_shl(t1, 1, pOverflow);
t0 = L_sub(L_temp, t1, pOverflow);
t0 = L_abs(t0);
/* max/t0 */
/* shift1 = sub(norm_l(max), 1);
max16 = extract_h(L_shl(max, shift1));
shift2 = norm_l(t0);
t016 = extract_h(L_shl(t0, shift2)); */
t016 = norm_l(max);
shift1 = sub(t016, 1, pOverflow);
L_temp = L_shl(max, shift1, pOverflow);
max16 = (Word16)(L_temp >> 16);
shift2 = norm_l(t0);
L_temp = L_shl(t0, shift2, pOverflow);
t016 = (Word16)(L_temp >> 16);
if (t016 != 0)
{
cor_max = div_s(max16, t016);
}
else
{
cor_max = 0;
}
shift = sub(shift1, shift2, pOverflow);
if (shift >= 0)
{
*cor_hp_max = shr(cor_max, shift, pOverflow); /* Q15 */
}
else
{
*cor_hp_max = shl(cor_max, negate(shift), pOverflow); /* Q15 */
}
return 0;
}