void Lsp_Az(
Word16 lsp[], /* (i) Q15 : line spectral frequencies */
Word16 a[] /* (o) Q12 : predictor coefficients (order = 10) */
)
{
Word16 i, j;
Word32 f1[6], f2[6];
Word32 t0;
Get_lsp_pol(&lsp[0],f1);
Get_lsp_pol(&lsp[1],f2);
for (i = 5; i > 0; i--)
{
f1[i] = L_add(f1[i], f1[i-1]); /* f1[i] += f1[i-1]; */
f2[i] = L_sub(f2[i], f2[i-1]); /* f2[i] -= f2[i-1]; */
}
a[0] = 4096;
for (i = 1, j = 10; i <= 5; i++, j--)
{
t0 = L_add(f1[i], f2[i]); /* f1[i] + f2[i] */
a[i] = extract_l( L_shr_r(t0, 13) ); /* from Q24 to Q12 and * 0.5 */
t0 = L_sub(f1[i], f2[i]); /* f1[i] - f2[i] */
a[j] = extract_l( L_shr_r(t0, 13) ); /* from Q24 to Q12 and * 0.5 */
}
return;
}
Word16 Test_Err(Word16 Lag1, Word16 Lag2,ENC_HANDLE *handle)
{
int i, i1, i2;
Word16 zone1, zone2;
Word32 Acc, Err_max;
Word16 iTest;
i2 = Lag2 + ClPitchOrd/2;
zone2 = mult( (Word16) i2, (Word16) 1092);
i1 = - SubFrLen + 1 + Lag1 - ClPitchOrd/2;
if(i1 <= 0) i1 = 1;
zone1 = mult( (Word16) i1, (Word16) 1092);
Err_max = -1L;
for(i=zone2; i>=zone1; i--) {
Acc = L_sub(handle->CodStat.Err[i], Err_max);
if(Acc > 0L) {
Err_max = handle->CodStat.Err[i];
}
}
Acc = L_sub(Err_max, ThreshErr);
if((Acc > 0L) || (handle->CodStat.SinDet < 0 ) ) {
iTest = 0;
}
else {
Acc = L_negate(Acc);
Acc = L_shr(Acc, DEC);
iTest = extract_l(Acc);
}
return(iTest);
}
void update_exc_err(
Word32 *L_exc_err,
Word16 gain_pit, /* (i) pitch gain */
Word16 T0 /* (i) integer part of pitch delay */
)
{
Word16 i, zone1, zone2, n;
Word32 L_worst, L_temp, L_acc;
Word16 hi, lo;
L_worst = -1L;
n = sub(T0, L_SUBFR);
if(n < 0) {
L_Extract(L_exc_err[0], &hi, &lo);
L_temp = Mpy_32_16(hi, lo, gain_pit);
L_temp = L_shl(L_temp, 1);
L_temp = L_add(0x00004000L, L_temp);
L_acc = L_sub(L_temp, L_worst);
if(L_acc > 0L) {
L_worst = L_temp;
}
L_Extract(L_temp, &hi, &lo);
L_temp = Mpy_32_16(hi, lo, gain_pit);
L_temp = L_shl(L_temp, 1);
L_temp = L_add(0x00004000L, L_temp);
L_acc = L_sub(L_temp, L_worst);
if(L_acc > 0L) {
L_worst = L_temp;
}
}
else {
zone1 = tab_zone[n];
i = sub(T0, 1);
zone2 = tab_zone[i];
for(i = zone1; i <= zone2; i++) {
L_Extract(L_exc_err[i], &hi, &lo);
L_temp = Mpy_32_16(hi, lo, gain_pit);
L_temp = L_shl(L_temp, 1);
L_temp = L_add(0x00004000L, L_temp);
L_acc = L_sub(L_temp, L_worst);
if(L_acc > 0L) L_worst = L_temp;
}
}
for(i=3; i>=1; i--) {
L_exc_err[i] = L_exc_err[i-1];
}
L_exc_err[0] = L_worst;
return;
}
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 Log2(
Word32 x, /* (i) input */
Word16 *int_comp, /* Q0 integer part */
Word16 *frac_comp /* Q15 fractional part */
)
{
Word16 exp, idx_man, sub_man, sub_tab;
Word32 a0;
if(x <= 0){
*int_comp = 0;
*frac_comp = 0;
}
else{
exp = norm_l(x); // normalization
a0 = L_shl(x, exp); // Q30 mantissa, i.e. 1.xxx Q30
/* use table look-up of man in [1.0, 2.0[ Q30 */
a0 = L_shr(L_sub(a0, (Word32)0x40000000), 8); // Q16 index into table - note zero'ing of leading 1
idx_man = extract_h(a0); // Q0 index into table
sub_man = extract_l(L_shr((a0 & 0xFFFF), 1)); // Q15 fractional sub_man
a0 = L_deposit_h(tablog[idx_man]); // Q31
sub_tab = sub(tablog[idx_man+1], tablog[idx_man]); // Q15
a0 = L_mac(a0, sub_man, sub_tab); // Q31
*frac_comp = intround(a0); // Q15
*int_comp = sub(30, exp); // Q0
}
return;
}
void Lsp_pre_select(
Word16 rbuf[], /* (i) Q13 : target vetor */
Word16 lspcb1[][M], /* (i) Q13 : first stage LSP codebook */
Word16 *cand /* (o) : selected code */
)
{
Word16 i, j;
Word16 tmp; /* Q13 */
Word32 L_dmin; /* Q26 */
Word32 L_tmp; /* Q26 */
Word32 L_temp;
/* avoid the worst case. (all over flow) */
*cand = 0;
L_dmin = MAX_32;
for ( i = 0 ; i < NC0 ; i++ ) {
L_tmp = 0;
for ( j = 0 ; j < M ; j++ ) {
tmp = sub(rbuf[j], lspcb1[i][j]);
L_tmp = L_mac( L_tmp, tmp, tmp );
}
L_temp = L_sub(L_tmp,L_dmin);
if ( L_temp< 0L) {
L_dmin = L_tmp;
*cand = i;
}
}
return;
}
long interpolation_cos129( short freq )
{
short sin_data,cos_data,count,temp ;
long Ltemp,Lresult;
/* cos(x)=cos(a)+(x-a)sin(a)-pow((a-x),2)*cos(a) */
count=shr(abs_s(freq ),7 );
temp=sub( extract_l(L_mult( count,64)) , freq );
/* (a-x)sin a */
/* Scale factor for (a-x): 3217=pi2/64 */
sin_data=sin129_table [ count];
cos_data=cos129_table [count];
Ltemp=L_mpy_ls(L_mult(3217,temp),sin_data);
/* (a-x) sin(a) - [(a-x)*(a-x)*cos(a)] /2 */
/* Scale factor for (a-x)*(a-x): 20213=pi2*pi2/64 */
Ltemp=L_sub(Ltemp,
L_mpy_ls(L_mult(mult_r(10106,temp),temp),cos_data));
/* Scaled up by 64/2 times */
Ltemp=L_shl( Ltemp ,6 );
Lresult= L_add(L_deposit_h(cos_data), (Ltemp)) ;
return(Lresult);
}
Word32 L_divide(Word32 L_num, Word32 L_denom)
{
Word16 approx;
Word32 L_div;
if (L_num < 0 || L_denom < 0 || L_num > L_denom)
{
printf("ERROR: Invalid input into L_divide!\n");
return (0);
}
/* First approximation: 1 / L_denom = 1/extract_h(L_denom) */
approx = divide_s((Word16) 0x3fff, extract_h(L_denom));
/* 1/L_denom = approx * (2.0 - L_denom * approx) */
L_div = L_mpy_ls(L_denom, approx);
L_div = L_sub((Word32) 0x7fffffffL, L_div);
L_div = L_mpy_ls(L_div, approx);
/* L_num * (1/L_denom) */
L_div = L_mpy_ll(L_num, L_div);
L_div = L_shl(L_div, 2);
return (L_div);
}
void get_pq_polynomials(
Word32 *f, /* Q23 */
Word16 *lsp) /* Q15 */
{
Word16 i, n, hi, lo;
Word16 index, offset, coslsp, c;
Word32 a0;
f[0] = L_mult(2048, 2048); // 1.0 Q23
for(i = 1; i <= LPCO ; i++)
f[i]= 0;
for(n=1; n<=(LPCO>>1); n++) {
/* cosine mapping */
index = shr(lsp[2*n-2],9); // Q6
offset = lsp[2*n-2]&(Word16)0x01ff; // Q9
a0 = L_mult(sub(costable[index+1], costable[index]), offset); // Q10
coslsp = add(costable[index], intround(L_shl(a0, 6))); // Q15 cos((double)PI*lsp[2*n-2])
c = coslsp; // Q14 c = 2. * cos((double)PI*lsp[2*n-2])
for(i = 2*n; i >= 2; i--) {
L_Extract(f[i-1], &hi, &lo);
f[i] = L_add(f[i], f[i-2]); // Q23 f[i] += f[i-2]
a0 = Mpy_32_16(hi, lo, c); // Q22
f[i] = L_sub(f[i], L_shl(a0,1)); // Q23 f[i] += f[i-2] - c*f[i-1];
}
f[1] = L_msu(f[1], c, 256); // Q23 f[1] -= c;
}
return;
}
static void Get_lsp_pol(Word16 *lsp, Word32 *f)
{
Word16 i,j, hi, lo;
Word32 t0;
/* All computation in Q24 */
*f = L_mult(4096, 2048); /* f[0] = 1.0; in Q24 */
f++;
*f = L_msu((Word32)0, *lsp, 512); /* f[1] = -2.0 * lsp[0]; in Q24 */
f++;
lsp += 2; /* Advance lsp pointer */
for(i=2; i<=5; i++)
{
*f = f[-2];
for(j=1; j<i; j++, f--)
{
L_Extract(f[-1] ,&hi, &lo);
t0 = Mpy_32_16(hi, lo, *lsp); /* t0 = f[-1] * lsp */
t0 = L_shl(t0, 1);
*f = L_add(*f, f[-2]); /* *f += f[-2] */
*f = L_sub(*f, t0); /* *f -= t0 */
}
*f = L_msu(*f, *lsp, 512); /* *f -= lsp<<9 */
f += i; /* Advance f pointer */
lsp += 2; /* Advance lsp pointer */
}
return;
}
static void cor_h_x_e(
Word16 h[], /* (i) Q12 : impulse response of weighted synthesis filter */
Word16 x[], /* (i) Q0 : correlation between target and h[] */
Word16 dn[] /* (o) Q0 : correlation between target and h[] */
)
{
Word16 i, j, k;
Word32 s, y32[L_SUBFR], max, tot, L_tmp;
/* first keep the result on 32 bits and find absolute maximum */
tot = 5;
for (k=0; k<NB_TRACK; k++) {
max = 0;
for (i=k; i<L_SUBFR; i+=STEP) {
s = 0;
for (j=i; j<L_SUBFR; j++) s = L_mac(s, x[j], h[j-i]);
y32[i] = s;
s = L_abs(s);
L_tmp = L_sub(s, max);
if (L_tmp > (Word32)0) max = s;
}
tot = L_add(tot, L_shr(max, 1)); /* tot += (2.0 x max) / 4.0 */
}
/* Find the number of right shifts to do on y32[] so that */
/* 2.0 x sumation of all max of dn[] in each track not saturate. */
j = sub(norm_l(tot), 2); /* multiply tot by 4 */
for (i=0; i<L_SUBFR; i++) {
dn[i] = round(L_shl(y32[i], j));
}
return;
}
void LTP_flag_update (vadState2 * st, Word16 mode)
{
Word16 thresh;
Word16 hi1;
Word16 lo1;
Word32 Ltmp;
test(); test();
if ((sub(mode, MR475) == 0) || (sub(mode, MR515) == 0))
{
thresh = (Word16)(32768.0*0.55); move16();
}
else if (sub(mode, MR102) == 0)
{
thresh = (Word16)(32768.0*0.60); move16();
}
else
{
thresh = (Word16)(32768.0*0.65); move16();
}
L_Extract (st->L_R0, &hi1, &lo1);
Ltmp = Mpy_32_16(hi1, lo1, thresh); test();
if (L_sub(st->L_Rmax, Ltmp) > 0)
{
st->LTP_flag = TRUE; move16();
}
else
{
st->LTP_flag = FALSE; move16();
}
return;
}
static Word16 Lag_max( /* output: lag found */
Word16 signal[], /* input : signal used to compute the open loop pitch */
Word16 L_frame, /* input : length of frame to compute pitch */
Word16 lag_max, /* input : maximum lag */
Word16 lag_min, /* input : minimum lag */
Word16 *cor_max) /* output: normalized correlation of selected lag */
{
Word16 i, j;
Word16 *p, *p1;
Word32 max, t0, L_temp;
Word16 max_h, max_l, ener_h, ener_l;
Word16 p_max;
max = MIN_32;
/* initialization used only to suppress Microsoft Visual C++ warnings */
p_max = lag_max;
for (i = lag_max; i >= lag_min; i--)
{
p = signal;
p1 = &signal[-i];
t0 = 0;
for (j=0; j<L_frame; j++, p++, p1++)
t0 = L_mac(t0, *p, *p1);
L_temp = L_sub(t0,max);
if (L_temp >= 0L)
{
max = t0;
p_max = i;
}
}
/* compute energy */
t0 = 0;
p = &signal[-p_max];
for(i=0; i<L_frame; i++, p++)
t0 = L_mac(t0, *p, *p);
/* 1/sqrt(energy), result in Q30 */
t0 = Inv_sqrt(t0);
/* max = max/sqrt(energy) */
/* This result will always be on 16 bits !! */
L_Extract(max, &max_h, &max_l);
L_Extract(t0, &ener_h, &ener_l);
t0 = Mpy_32(max_h, max_l, ener_h, ener_l);
*cor_max = extract_l(t0);
return(p_max);
}
void Cor_h_X(
Word16 h[], /* (i) Q12 :Impulse response of filters */
Word16 X[], /* (i) :Target vector */
Word16 D[] /* (o) :Correlations between h[] and D[] */
/* Normalized to 13 bits */
)
{
Word16 i, j;
Word32 s, max, L_temp;
Word32 y32[L_SUBFR];
/* first keep the result on 32 bits and find absolute maximum */
max = 0;
for (i = 0; i < L_SUBFR; i++)
{
s = 0;
for (j = i; j < L_SUBFR; j++)
s = L_mac(s, X[j], h[j-i]);
y32[i] = s;
s = L_abs(s);
L_temp =L_sub(s,max);
if(L_temp>0L) {
max = s;
}
}
/* Find the number of right shifts to do on y32[] */
/* so that maximum is on 13 bits */
j = norm_l(max);
if( sub(j,16) > 0) {
j = 16;
}
j = sub(18, j);
if(j>=0)
{
for(i=0; i<L_SUBFR; i++) {
D[i] = extract_l( L_shr(y32[i], j) );
}
}
else
{
Word16 pj = abs_s(j);
for(i=0; i<L_SUBFR; i++) {
D[i] = extract_l( L_shr(y32[i], pj) );
}
}
return;
}
/*----------------------------------------------------------------------------
* select_ltp : selects best of (gain1, gain2)
* with gain1 = num1 * 2** sh_num1 / den1 * 2** sh_den1
* and gain2 = num2 * 2** sh_num2 / den2 * 2** sh_den2
*----------------------------------------------------------------------------
*/
static Word16 select_ltp( /* output : 1 = 1st gain, 2 = 2nd gain */
Word16 num1, /* input : numerator of gain1 */
Word16 den1, /* input : denominator of gain1 */
Word16 sh_num1, /* input : just. factor for num1 */
Word16 sh_den1, /* input : just. factor for den1 */
Word16 num2, /* input : numerator of gain2 */
Word16 den2, /* input : denominator of gain2 */
Word16 sh_num2, /* input : just. factor for num2 */
Word16 sh_den2) /* input : just. factor for den2 */
{
Word32 L_temp1, L_temp2;
Word16 temp1, temp2;
Word16 hi, lo;
Word32 L_temp;
if(den2 == 0) {
return(1);
}
/* compares criteria = num**2/den */
L_temp1 = L_mult(num1, num1);
L_Extract(L_temp1, &hi, &lo);
L_temp1 = Mpy_32_16(hi, lo, den2);
L_temp2 = L_mult(num2, num2);
L_Extract(L_temp2, &hi, &lo);
L_temp2 = Mpy_32_16(hi, lo, den1);
/* temp1 = sh_den2 + 2 * sh_num1 */
temp1 = shl(sh_num1, 1);
temp1 = add(temp1, sh_den2);
/* temp2 = sh_den1 + 2 * sh_num2; */
temp2 = shl(sh_num2, 1);
temp2 = add(temp2, sh_den1);
if(sub(temp2 ,temp1)>0) {
temp2 = sub(temp2, temp1);
L_temp1 = L_shr(L_temp1, temp2); /* temp2 > 0 */
}
else {
if(sub(temp1 ,temp2) >0) {
temp1 = sub(temp1, temp2);
L_temp2 = L_shr(L_temp2, temp1); /* temp1 > 0 */
}
}
L_temp = L_sub(L_temp2,L_temp1);
if(L_temp>0L) {
return(2);
}
else {
return(1);
}
}
void Lsp_stability(
Word16 buf[] /* (i/o) Q13 : quantized LSP parameters */
)
{
Word16 j;
Word16 tmp;
Word32 L_diff;
Word32 L_acc, L_accb;
for(j=0; j<M-1; j++) {
L_acc = L_deposit_l( buf[j+1] );
L_accb = L_deposit_l( buf[j] );
L_diff = L_sub( L_acc, L_accb );
if( L_diff < 0L ) {
/* exchange buf[j]<->buf[j+1] */
tmp = buf[j+1];
buf[j+1] = buf[j];
buf[j] = tmp;
}
}
if( sub(buf[0], L_LIMIT) <0 ) {
buf[0] = L_LIMIT;
printf("lsp_stability warning Low \n");
}
for(j=0; j<M-1; j++) {
L_acc = L_deposit_l( buf[j+1] );
L_accb = L_deposit_l( buf[j] );
L_diff = L_sub( L_acc, L_accb );
if( L_sub(L_diff, GAP3)<0L ) {
buf[j+1] = add( buf[j], GAP3 );
}
}
if( sub(buf[M-1],M_LIMIT)>0 ) {
buf[M-1] = M_LIMIT;
printf("lsp_stability warning High \n");
}
return;
}
static Word16 Vq_subvec (/* o : quantization index, Q0 */
Word16 *lsf_r1, /* i : 1st LSF residual vector, Q15 */
Word16 *lsf_r2, /* i : 2nd LSF residual vector, Q15 */
const Word16 *dico, /* i : quantization codebook, Q15 */
Word16 *wf1, /* i : 1st LSF weighting factors Q13 */
Word16 *wf2, /* i : 2nd LSF weighting factors Q13 */
Word16 dico_size /* i : size of quantization codebook, Q0 */
)
{
Word16 index = 0; /* initialization only needed to keep gcc silent */
Word16 i, temp;
const Word16 *p_dico;
Word32 dist_min, dist;
dist_min = MAX_32; move32 ();
p_dico = dico; move16 ();
for (i = 0; i < dico_size; i++)
{
temp = sub (lsf_r1[0], *p_dico++);
temp = mult (wf1[0], temp);
dist = L_mult (temp, temp);
temp = sub (lsf_r1[1], *p_dico++);
temp = mult (wf1[1], temp);
dist = L_mac (dist, temp, temp);
temp = sub (lsf_r2[0], *p_dico++);
temp = mult (wf2[0], temp);
dist = L_mac (dist, temp, temp);
temp = sub (lsf_r2[1], *p_dico++);
temp = mult (wf2[1], temp);
dist = L_mac (dist, temp, temp);
test ();
if (L_sub (dist, dist_min) < (Word32) 0)
{
dist_min = dist; move32 ();
index = i; move16 ();
}
}
/* Reading the selected vector */
p_dico = &dico[shl (index, 2)]; move16 ();
lsf_r1[0] = *p_dico++; move16 ();
lsf_r1[1] = *p_dico++; move16 ();
lsf_r2[0] = *p_dico++; move16 ();
lsf_r2[1] = *p_dico++; move16 ();
return index;
}
Word16 test_err( /* (o) flag set to 1 if taming is necessary */
Word32 *L_exc_err,
Word16 T0, /* (i) integer part of pitch delay */
Word16 T0_frac /* (i) fractional part of pitch delay */
)
{
Word16 i, t1, zone1, zone2, flag;
Word32 L_maxloc, L_acc;
if(T0_frac > 0) {
t1 = add(T0, 1);
}
else {
t1 = T0;
}
i = sub(t1, (L_SUBFR+L_INTER10));
if(i < 0) {
i = 0;
}
zone1 = tab_zone[i];
i = add(t1, (L_INTER10 - 2));
zone2 = tab_zone[i];
L_maxloc = -1L;
flag = 0 ;
for(i=zone2; i>=zone1; i--) {
L_acc = L_sub(L_exc_err[i], L_maxloc);
if(L_acc > 0L) {
L_maxloc = L_exc_err[i];
}
}
L_acc = L_sub(L_maxloc, L_THRESH_ERR);
if(L_acc > 0L) {
flag = 1;
}
return(flag);
}
Word16 FNevChebP(
Word16 x, /* (i) Q15: value */
Word16 *t_man, /* (i) Q7: mantissa of coefficients */
Word16 *t_exp, /* (i): exponent fo cofficients */
Word16 nd2) /* (i): order */
{
Word16 i;
Word16 x2;
Word16 b_man[NAB], b_exp[NAB];
Word16 y;
Word32 a0;
x2 = x; // 2x in Q14
b_man[0] = t_man[nd2];
b_exp[0] = t_exp[nd2]; // b[0] in Q(7+t_exp)
a0 = L_mult(x2, b_man[0]);
a0 = L_shr(a0, sub(b_exp[0], 1)); // t*b[0] in Q23
a0 = L_add(a0, L_shr(L_deposit_h(t_man[nd2-1]), t_exp[nd2-1])); // c[nd2-1] + t*b[0] in Q23
b_exp[1] = norm_l(a0);
b_man[1] = intround(L_shl(a0, b_exp[1])); // b[1] = c[nd2-1] + t * b[0]
for (i=2;i<nd2;i++){
a0 = L_mult(x2, b_man[i-1]);
a0 = L_shr(a0, sub(b_exp[i-1], 1)); // t*b[i-1] in Q23
a0 = L_add(a0, L_shr(L_deposit_h(t_man[nd2-i]), t_exp[nd2-i]));// c[nd2-i] + t*b[i-1] in Q23
a0 = L_sub(a0, L_shr(L_deposit_h(b_man[i-2]), b_exp[i-2])); // c[nd2-i] + t*b[i-1] - b[i-2] in Q23
b_exp[i] = norm_l(a0);
b_man[i] = intround(L_shl(a0, b_exp[i])); // b[i] = c[nd2-i] - b[i-2] + t * b[i-1]
}
a0 = L_mult(x, b_man[nd2-1]);
a0 = L_shr(a0, b_exp[nd2-1]); // x*b[nd2-1] in Q23
a0 = L_add(a0, L_shr(L_deposit_h(t_man[0]), t_exp[0])); // c[0] + x*b[nd2-1] in Q23
a0 = L_sub(a0, L_shr(L_deposit_h(b_man[nd2-2]), b_exp[nd2-2])); // c[0] + x*b[nd2-1] - b[nd2-2] in Q23
y = intround(L_shl(a0, 6)); // Q13
return y; // Q13
}
void Lsp_last_select(
Word32 L_tdist[], /* (i) Q27 : distortion */
Word16 *mode_index /* (o) : the selected mode */
)
{
Word32 L_temp;
*mode_index = 0;
L_temp =L_sub(L_tdist[1] ,L_tdist[0]);
if ( L_temp<0L){
*mode_index = 1;
}
return;
}
static Word16 Sqrt( Word32 Num )
{
Word16 i ;
Word16 Rez = (Word16) 0 ;
Word16 Exp = (Word16) 0x4000 ;
Word32 Acc, L_temp;
for ( i = 0 ; i < 14 ; i ++ ) {
Acc = L_mult(add(Rez, Exp), add(Rez, Exp) );
L_temp = L_sub(Num, Acc);
if(L_temp >= 0L) Rez = add( Rez, Exp);
Exp = shr( Exp, (Word16) 1 ) ;
}
return Rez ;
}
static Word16 Cmp_filt(Word16 *RCoeff, Word16 sh_RCoeff, Word16 *acf,
Word16 alpha, Word16 FracThresh)
{
Word32 L_temp0, L_temp1;
Word16 temp1, temp2, sh[2], ind;
Word16 i;
Word16 diff, flag;
extern Flag Overflow;
sh[0] = 0;
sh[1] = 0;
ind = 1;
flag = 0;
do {
Overflow = 0;
temp1 = shr(RCoeff[0], sh[0]);
temp2 = shr(acf[0], sh[1]);
L_temp0 = L_shr(L_mult(temp1, temp2),1);
for(i=1; i <= M; i++) {
temp1 = shr(RCoeff[i], sh[0]);
temp2 = shr(acf[i], sh[1]);
L_temp0 = L_mac(L_temp0, temp1, temp2);
}
if(Overflow != 0) {
sh[(int)ind] = add(sh[(int)ind], 1);
ind = sub(1, ind);
}
else flag = 1;
} while (flag == 0);
temp1 = mult_r(alpha, FracThresh);
L_temp1 = L_add(L_deposit_l(temp1), L_deposit_l(alpha));
temp1 = add(sh_RCoeff, 9); /* 9 = Lpc_justif. * 2 - 16 + 1 */
temp2 = add(sh[0], sh[1]);
temp1 = sub(temp1, temp2);
L_temp1 = L_shl(L_temp1, temp1);
L_temp0 = L_sub(L_temp0, L_temp1);
if(L_temp0 > 0L) diff = 1;
else diff = 0;
return(diff);
}
void lsp2a(
Word16 lsp[], /* (i) Q15 : line spectral pairs */
Word16 a[]) /* (o) Q12 : predictor coefficients (order = 10) */
{
Word32 p[LPCO+1], q[LPCO+1]; // Q23
Word32 a0;
Word16 i, n;
get_pq_polynomials(p, lsp);
get_pq_polynomials(q, lsp+1);
a[0] = 4096; // Q12
a0 = L_add(p[1], q[1]); // Q23
a[1] = intround(L_shl(a0,4)); // Q12 - includes 0.5 factor of a[1] = 0.5*(p[1]+q[1])
for(i=1, n=2; i<LPCO; i++, n++) {
a0 = L_add(p[i], p[n]); // Q23
a0 = L_add(a0, q[n]); // Q23
a0 = L_sub(a0, q[i]); // Q23
a[n] = intround(L_shl(a0,4)); // Q12 a[n] = 0.5 * (p[i] + p[n] + q[n] - q[i]);
}
return;
}
void Lsp_select_2(
Word16 rbuf[], /* (i) Q13 : target vector */
Word16 lspcb1[], /* (i) Q13 : first stage lsp codebook */
Word16 wegt[], /* (i) norm: weighting coef. */
Word16 lspcb2[][M], /* (i) Q13 : second stage lsp codebook */
Word16 *index /* (o) : selected codebook index */
)
{
Word16 j, k1;
Word16 buf[M]; /* Q13 */
Word32 L_dist; /* Q26 */
Word32 L_dmin; /* Q26 */
Word16 tmp,tmp2; /* Q13 */
Word32 L_temp;
for ( j = NC ; j < M ; j++ )
buf[j] = sub(rbuf[j], lspcb1[j]);
/* avoid the worst case. (all over flow) */
*index = 0;
L_dmin = MAX_32;
for ( k1 = 0 ; k1 < NC1 ; k1++ ) {
L_dist = 0;
for ( j = NC ; j < M ; j++ ) {
tmp = sub(buf[j], lspcb2[k1][j]);
tmp2 = mult( wegt[j], tmp );
L_dist = L_mac( L_dist, tmp2, tmp );
}
L_temp = L_sub(L_dist, L_dmin);
if ( L_temp <0L ) {
L_dmin = L_dist;
*index = k1;
}
}
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] ) );
}
/*---------------------------------------------------------------------------*
* Function Gbk_presel *
* ~~~~~~~~~~~~~~~~~~~ *
* - presearch for gain codebook - *
*---------------------------------------------------------------------------*/
static void Gbk_presel(
Word16 best_gain[], /* (i) [0] Q9 : unquantized pitch gain */
/* (i) [1] Q2 : unquantized code gain */
Word16 *cand1, /* (o) : index of best 1st stage vector */
Word16 *cand2, /* (o) : index of best 2nd stage vector */
Word16 gcode0 /* (i) Q4 : presearch for gain codebook */
)
{
Word16 acc_h;
Word16 sft_x,sft_y;
Word32 L_acc,L_preg,L_cfbg,L_tmp,L_tmp_x,L_tmp_y;
Word32 L_temp;
/*--------------------------------------------------------------------------*
x = (best_gain[1]-(coef[0][0]*best_gain[0]+coef[1][1])*gcode0) * inv_coef;
*--------------------------------------------------------------------------*/
L_cfbg = L_mult( coef[0][0], best_gain[0] ); /* L_cfbg:Q20 -> !!y */
L_acc = L_shr( L_coef[1][1], 15 ); /* L_acc:Q20 */
L_acc = L_add( L_cfbg , L_acc );
acc_h = extract_h( L_acc ); /* acc_h:Q4 */
L_preg = L_mult( acc_h, gcode0 ); /* L_preg:Q9 */
L_acc = L_shl( L_deposit_l( best_gain[1] ), 7 ); /* L_acc:Q9 */
L_acc = L_sub( L_acc, L_preg );
acc_h = extract_h( L_shl( L_acc,2 ) ); /* L_acc_h:Q[-5] */
L_tmp_x = L_mult( acc_h, INV_COEF ); /* L_tmp_x:Q15 */
/*--------------------------------------------------------------------------*
y = (coef[1][0]*(-coef[0][1]+best_gain[0]*coef[0][0])*gcode0
-coef[0][0]*best_gain[1]) * inv_coef;
*--------------------------------------------------------------------------*/
L_acc = L_shr( L_coef[0][1], 10 ); /* L_acc:Q20 */
L_acc = L_sub( L_cfbg, L_acc ); /* !!x -> L_cfbg:Q20 */
acc_h = extract_h( L_acc ); /* acc_h:Q4 */
acc_h = mult( acc_h, gcode0 ); /* acc_h:Q[-7] */
L_tmp = L_mult( acc_h, coef[1][0] ); /* L_tmp:Q10 */
L_preg = L_mult( coef[0][0], best_gain[1] ); /* L_preg:Q13 */
L_acc = L_sub( L_tmp, L_shr(L_preg,3) ); /* L_acc:Q10 */
acc_h = extract_h( L_shl( L_acc,2 ) ); /* acc_h:Q[-4] */
L_tmp_y = L_mult( acc_h, INV_COEF ); /* L_tmp_y:Q16 */
sft_y = (14+4+1)-16; /* (Q[thr1]+Q[gcode0]+1)-Q[L_tmp_y] */
sft_x = (15+4+1)-15; /* (Q[thr2]+Q[gcode0]+1)-Q[L_tmp_x] */
if(gcode0>0){
/*-- pre select codebook #1 --*/
*cand1 = 0 ;
do{
L_temp = L_sub( L_tmp_y, L_shr(L_mult(thr1[*cand1],gcode0),sft_y));
if(L_temp >0L ){
//(*cand1) =add(*cand1,1);
*cand1 += 1;
}
else break ;
//} while(sub((*cand1),(NCODE1-NCAN1))<0) ;
} while ((*cand1) < (NCODE1-NCAN1));
/*-- pre select codebook #2 --*/
*cand2 = 0 ;
do{
L_temp = L_sub( L_tmp_x , L_shr(L_mult(thr2[*cand2],gcode0),sft_x));
if( L_temp >0L) {
//(*cand2) =add(*cand2,1);
*cand2 += 1;
}
else break ;
//} while(sub((*cand2),(NCODE2-NCAN2))<0) ;
} while((*cand2) < (NCODE2-NCAN2)) ;
}
else{
/*-- pre select codebook #1 --*/
*cand1 = 0 ;
do{
L_temp = L_sub(L_tmp_y ,L_shr(L_mult(thr1[*cand1],gcode0),sft_y));
if( L_temp <0L){
//(*cand1) =add(*cand1,1);
*cand1 += 1;
}
else break ;
//} while(sub((*cand1),(NCODE1-NCAN1))) ;
} while (*cand1 != (NCODE1-NCAN1)) ;
/*-- pre select codebook #2 --*/
*cand2 = 0 ;
do{
L_temp =L_sub(L_tmp_x ,L_shr(L_mult(thr2[*cand2],gcode0),sft_x));
if( L_temp <0L){
//(*cand2) =add(*cand2,1);
*cand2 += 1;
}
else break ;
//} while(sub( (*cand2),(NCODE2-NCAN2))) ;
} while(*cand2 != (NCODE2-NCAN2)) ;
}
return ;
}
Word16 coarsepitch(
Word16 *xw, /* (i) (normalized) weighted signal */
struct BV16_Encoder_State *cstate) /* (i/o) Coder State */
{
Word16 s; /* Q2 */
Word16 a, b;
Word16 im;
Word16 maxdev, flag, mpflag;
Word32 eni, deltae;
Word32 cc;
Word16 ah,al, bh, bl;
Word32 a0, a1, a2, a3;
Word32 *lp0;
Word16 exp, new_exp;
Word16 *fp0, *fp1, *fp2, *fp3, *sp;
Word16 *fp1_h, *fp1_l, *fp2_h, *fp2_l;
Word16 cor2max, cor2max_exp;
Word16 cor2m, cor2m_exp;
Word16 s0, t0, t1, exp0, exp1, e2, e3;
Word16 threshold;
Word16 mplth; /* Q2 */
Word16 i, j, k, n, npeaks, imax, idx[MAXPPD-MINPPD+1];
Word16 cpp;
Word16 plag[HMAXPPD], cor2[MAXPPD1], cor2_exp[MAXPPD1];
Word16 cor2i[HMAXPPD], cor2i_exp[HMAXPPD], xwd[LXD];
Word16 tmp_h[DFO+FRSZ], tmp_l[DFO+FRSZ]; /* DPF Q7 */
Word32 cor[MAXPPD1], energy[MAXPPD1], lxwd[FRSZD];
Word16 energy_man[MAXPPD1], energy_exp[MAXPPD1];
Word16 energyi_man[HMAXPPD], energyi_exp[HMAXPPD];
Word16 energym_man, energym_exp;
Word16 energymax_man, energymax_exp;
/* Lowpass filter xw() to 800 hz; shift & output into xwd() */
/* AP and AZ filtering and decimation */
fp1_h = tmp_h + DFO;
fp1_l = tmp_l + DFO;
sp = xw;
a1 = 1;
for (i=0;i<DFO;i++) tmp_h[i] = cstate->dfm_h[2*i+1];
for (i=0;i<DFO;i++) tmp_l[i] = cstate->dfm_h[2*i];
lp0 = lxwd;
for (i=0;i<FRSZD;i++) {
for (k=0;k<DECF;k++) {
a0 = L_shr(L_deposit_h(*sp++),10);
fp2_h = fp1_h-1;
fp2_l = fp1_l-1;
for (j=0;j<DFO;j++)
a0=L_sub(a0,Mpy_32(*fp2_h--,*fp2_l--,adf_h[j+1],adf_l[j+1]));
a0 = L_shl(a0, 2); /* adf Q13 */
L_Extract(a0, fp1_h++, fp1_l++);
}
fp2_h = fp1_h-1;
fp2_l = fp1_l-1;
a0 = Mpy_32_16(*fp2_h--, *fp2_l--, bdf[0]);
for (j=0;j<DFO;j++)
a0=L_add(a0,Mpy_32_16(*fp2_h--,*fp2_l--,bdf[j+1]));
*lp0++ = a0;
a0 = L_abs(a0);
if (a1 < a0)
a1 = a0;
}
/* copy temp buffer to memory */
fp1_h -= DFO;
fp1_l -= DFO;
for (i=0;i<DFO;i++) {
cstate->dfm_h[2*i+1] = fp1_h[i];
cstate->dfm_h[2*i] = fp1_l[i];
}
lp0 = lxwd;
new_exp = sub(norm_l(a1), 3); /* headroom to avoid overflow */
exp = sub(cstate->xwd_exp,new_exp); /* increase in bit-resolution */
if (exp < 0) { /* Descending signal level */
new_exp = cstate->xwd_exp;
exp = 0;
}
for (i=0;i<XDOFF;i++)
xwd[i] = shr(cstate->xwd[i], exp);
/* fill-in new exponent */
fp0 = xwd + XDOFF;
for (i=0;i<FRSZD;i++)
fp0[i] = intround(L_shl(lp0[i],new_exp));
/* update signal memory for next frame */
exp0 = 1;
for (i=0;i<XDOFF;i++) {
exp1 = abs_s(xwd[FRSZD+i]);
if (exp1 > exp0)
exp0 = exp1;
}
exp0 = sub(norm_s(exp0),3); /* extra exponent for next frame */
exp = sub(exp0, exp);
if (exp >=0)
{
for (i=0;i<XDOFF-FRSZD;i++)
cstate->xwd[i] = shl(cstate->xwd[i+FRSZD], exp);
}
else
{
exp = -exp;
if (exp >=15)
exp = 15;
for (i=0;i<XDOFF-FRSZD;i++)
cstate->xwd[i] = shr(cstate->xwd[i+FRSZD], exp);
}
for (;i<XDOFF;i++)
cstate->xwd[i] = shl(xwd[FRSZD+i],exp0);
cstate->xwd_exp = add(new_exp, exp0);
/* Compute correlation & energy of prediction basis vector */
/* reset local buffers */
for (i=0;i<MAXPPD1;i++)
cor[i] = energy[i] = 0;
fp0 = xwd+MAXPPD1;
fp1 = xwd+MAXPPD1-M1;
a0 = a1 = 0;
for (i=0;i<(LXD-MAXPPD1);i++) {
a0 = L_mac0(a0, *fp1, *fp1);
a1 = L_mac0(a1, *fp0++, *fp1++);
}
cor[M1-1] = a1;
energy[M1-1] = a0;
energy_exp[M1-1] = norm_l(energy[M1-1]);
energy_man[M1-1] = extract_h(L_shl(energy[M1-1], energy_exp[M1-1]));
s0 = cor2_exp[M1-1] = norm_l(a1);
t0 = extract_h(L_shl(a1, s0));
cor2[M1-1] = extract_h(L_mult(t0, t0));
if (a1 < 0)
cor2[M1-1] = negate(cor2[M1-1]);
fp2 = xwd+LXD-M1-1;
fp3 = xwd+MAXPPD1-M1-1;
for (i=M1;i<M2;i++) {
fp0 = xwd+MAXPPD1;
fp1 = xwd+MAXPPD1-i-1;
a1 = 0;
for (j=0;j<(LXD-MAXPPD1);j++)
a1 = L_mac0(a1,*fp0++,*fp1++);
cor[i] = a1;
a0 = L_msu0(a0, *fp2, *fp2);
a0 = L_mac0(a0, *fp3, *fp3);
fp2--; fp3--;
energy[i] = a0;
energy_exp[i] = norm_l(energy[i]);
energy_man[i] = extract_h(L_shl(energy[i], energy_exp[i]));
s0 = cor2_exp[i] = norm_l(a1);
t0 = extract_h(L_shl(a1, s0));
cor2[i] = extract_h(L_mult(t0, t0));
if (a1 < 0)
cor2[i] = negate(cor2[i]);
}
/* Find positive correlation peaks */
/* Find maximum of cor*cor/energy among positive correlation peaks */
npeaks = 0;
n = MINPPD-1;
while ((npeaks < MAX_NPEAKS) && (n<MAXPPD)) {
if (cor[n]>0) {
a0 = L_mult(energy_man[n-1],cor2[n]);
a1 = L_mult(energy_man[n], cor2[n-1]);
exp0 = shl(sub(cor2_exp[n], cor2_exp[n-1]),1);
exp0 = add(exp0, energy_exp[n-1]);
exp0 = sub(exp0, energy_exp[n]);
if (exp0>=0)
a0 = L_shr(a0, exp0);
else
a1 = L_shl(a1, exp0);
if (a0 > a1) {
a0 = L_mult(energy_man[n+1],cor2[n]);
a1 = L_mult(energy_man[n], cor2[n+1]);
exp0 = shl(sub(cor2_exp[n], cor2_exp[n+1]),1);
exp0 = add(exp0, energy_exp[n+1]);
exp0 = sub(exp0, energy_exp[n]);
if (exp0>=0)
a0 = L_shr(a0, exp0);
else
a1 = L_shl(a1, exp0);
if (a0 > a1) {
idx[npeaks] = n;
npeaks++;
}
}
}
n++;
}
/* Return early if there is no peak or only one peak */
if (npeaks == 0){ /* if there are no positive peak, */
return MINPPD*DECF; /* return minimum pitch period in decimated domain */
}
if (npeaks == 1){ /* if there is exactly one peak, */
return (idx[0]+1)*DECF; /* return the time lag for this single peak */
}
/* If program proceeds to here, there are 2 or more peaks */
cor2max=(Word16) 0x8000;
cor2max_exp= (Word16) 0;
energymax_man=1;
energymax_exp=0;
imax=0;
for (i=0; i < npeaks; i++) {
/* Use quadratic interpolation to find the interpolated cor[] and
energy[] corresponding to interpolated peak of cor2[]/energy[] */
/* first calculate coefficients of y(x)=ax^2+bx+c; */
n=idx[i];
a0=L_sub(L_shr(L_add(cor[n+1],cor[n-1]),1),cor[n]);
L_Extract(a0, &ah, &al);
a0=L_shr(L_sub(cor[n+1],cor[n-1]),1);
L_Extract(a0, &bh, &bl);
cc=cor[n];
/* Initialize variables before searching for interpolated peak */
im=0;
cor2m_exp = cor2_exp[n];
cor2m = cor2[n];
energym_exp = energy_exp[n];
energym_man = energy_man[n];
eni=energy[n];
/* Determine which side the interpolated peak falls in, then
do the search in the appropriate range */
a0 = L_mult(energy_man[n-1],cor2[n+1]);
a1 = L_mult(energy_man[n+1], cor2[n-1]);
exp0 = shl(sub(cor2_exp[n+1], cor2_exp[n-1]),1);
exp0 = add(exp0, energy_exp[n-1]);
exp0 = sub(exp0, energy_exp[n+1]);
if (exp0>=0)
a0 = L_shr(a0, exp0);
else
a1 = L_shl(a1, exp0);
if (a0 > a1) { /* if right side */
deltae = L_shr(L_sub(energy[n+1], eni), 2);
for (k = 0; k < HDECF; k++) {
a0=L_add(L_add(Mpy_32_16(ah,al,x2[k]),Mpy_32_16(bh,bl,x[k])),cc);
eni = L_add(eni, deltae);
a1 = eni;
exp0 = norm_l(a0);
s0 = extract_h(L_shl(a0, exp0));
s0 = extract_h(L_mult(s0, s0));
e2 = energym_exp;
t0 = energym_man;
a2 = L_mult(t0, s0);
e3 = norm_l(a1);
t1 = extract_h(L_shl(a1, e3));
a3 = L_mult(t1, cor2m);
exp1 = shl(sub(exp0, cor2m_exp),1);
exp1 = add(exp1, e2);
exp1 = sub(exp1, e3);
if (exp1>=0)
a2 = L_shr(a2, exp1);
else
a3 = L_shl(a3, exp1);
if (a2 > a3) {
im = k+1;
cor2m = s0;
cor2m_exp = exp0;
energym_exp = e3;
energym_man = t1;
}
}
} else { /* if interpolated peak is on the left side */
deltae = L_shr(L_sub(energy[n-1], eni), 2);
for (k = 0; k < HDECF; k++) {
a0=L_add(L_sub(Mpy_32_16(ah,al,x2[k]),Mpy_32_16(bh,bl,x[k])),cc);
eni = L_add(eni, deltae);
a1=eni;
exp0 = norm_l(a0);
s0 = extract_h(L_shl(a0, exp0));
s0 = extract_h(L_mult(s0, s0));
e2 = energym_exp;
t0 = energym_man;
a2 = L_mult(t0, s0);
e3 = norm_l(a1);
t1 = extract_h(L_shl(a1, e3));
a3 = L_mult(t1, cor2m);
exp1 = shl(sub(exp0, cor2m_exp),1);
exp1 = add(exp1, e2);
exp1 = sub(exp1, e3);
if (exp1>=0)
a2 = L_shr(a2, exp1);
else
a3 = L_shl(a3, exp1);
if (a2 > a3) {
im = -k-1;
cor2m = s0;
cor2m_exp = exp0;
energym_exp = e3;
energym_man = t1;
}
}
}
/* Search done; assign cor2[] and energy[] corresponding to
interpolated peak */
plag[i]=add(shl(add(idx[i],1),2),im); /* lag of interp. peak */
cor2i[i]=cor2m;
cor2i_exp[i]=cor2m_exp;
/* interpolated energy[] of i-th interpolated peak */
energyi_exp[i] = energym_exp;
energyi_man[i] = energym_man;
/* Search for global maximum of interpolated cor2[]/energy[] peak */
a0 = L_mult(cor2m,energymax_man);
a1 = L_mult(cor2max, energyi_man[i]);
exp0 = shl(sub(cor2m_exp, cor2max_exp),1);
exp0 = add(exp0, energymax_exp);
exp0 = sub(exp0, energyi_exp[i]);
if (exp0 >=0)
a0 = L_shr(a0, exp0);
else
a1 = L_shl(a1, exp0);
if (a0 > a1) {
imax=i;
cor2max=cor2m;
cor2max_exp=cor2m_exp;
energymax_exp = energyi_exp[i];
energymax_man = energyi_man[i];
}
}
cpp=plag[imax]; /* first candidate for coarse pitch period */
mplth=plag[npeaks-1]; /* set mplth to the lag of last peak */
/* Find the largest peak (if there is any) around the last pitch */
maxdev= shr(cstate->cpplast,2); /* maximum deviation from last pitch */
im = -1;
cor2m=(Word16) 0x8000;
cor2m_exp= (Word16) 0;
energym_man = 1;
energym_exp = 0;
for (i=0;i<npeaks;i++) { /* loop thru the peaks before the largest peak */
if (abs_s(sub(plag[i],cstate->cpplast)) <= maxdev) {
a0 = L_mult(cor2i[i],energym_man);
a1 = L_mult(cor2m, energyi_man[i]);
exp0 = shl(sub(cor2i_exp[i], cor2m_exp),1);
exp0 = add(exp0, energym_exp);
exp0 = sub(exp0, energyi_exp[i]);
if (exp0 >=0)
a0 = L_shr(a0, exp0);
else
a1 = L_shl(a1, exp0);
if (a0 > a1) {
im=i;
cor2m=cor2i[i];
cor2m_exp=cor2i_exp[i];
energym_man = energyi_man[i];
energym_exp = energyi_exp[i];
}
}
} /* if there is no peaks around last pitch, then im is still -1 */
/* Now see if we should pick any alternatice peak */
/* first, search first half of pitch range, see if any qualified peak
has large enough peaks at every multiple of its lag */
i=0;
while (2*plag[i] < mplth) {
/* Determine the appropriate threshold for this peak */
if (i != im) { /* if not around last pitch, */
threshold = TH1; /* use a higher threshold */
} else { /* if around last pitch */
threshold = TH2; /* use a lower threshold */
}
/* If threshold exceeded, test peaks at multiples of this lag */
a0 = L_mult(cor2i[i],energymax_man);
t1 = extract_h(L_mult(energyi_man[i], threshold));
a1 = L_mult(cor2max, t1);
exp0 = shl(sub(cor2i_exp[i], cor2max_exp),1);
exp0 = add(exp0, energymax_exp);
exp0 = sub(exp0, energyi_exp[i]);
if (exp0 >=0)
a0 = L_shr(a0, exp0);
else
a1 = L_shl(a1, exp0);
if (a0 > a1) {
flag=1;
j=i+1;
k=0;
s=shl(plag[i],1); /* initialize t to twice the current lag */
while (s<=mplth) { /* loop thru all multiple lag <= mplth */
mpflag=0; /* initialize multiple pitch flag to 0 */
t0 = mult_r(s,MPDTH);
a=sub(s, t0); /* multiple pitch range lower bound */
b=add(s, t0); /* multiple pitch range upper bound */
while (j < npeaks) { /* loop thru peaks with larger lags */
if (plag[j] > b) { /* if range exceeded, */
break; /* break the innermost while loop */
} /* if didn't break, then plag[j] <= b */
if (plag[j] > a) { /* if current peak lag within range, */
/* then check if peak value large enough */
a0 = L_mult(cor2i[j],energymax_man);
if (k<4)
t1 = MPTH[k];
else
t1 = MPTH4;
t1 = extract_h(L_mult(t1, energyi_man[j]));
a1 = L_mult(cor2max, t1);
exp0 = shl(sub(cor2i_exp[j], cor2max_exp),1);
exp0 = add(exp0, energymax_exp);
exp0 = sub(exp0, energyi_exp[j]);
if (exp0 >=0)
a0 = L_shr(a0, exp0);
else
a1 = L_shl(a1, exp0);
if (a0 > a1) {
mpflag=1; /* if peak large enough, set mpflag, */
break; /* and break the innermost while loop */
}
}
j++;
}
/* if no qualified peak found at this multiple lag */
if (mpflag == 0) {
flag=0; /* disqualify the lag plag[i] */
break; /* and break the while (s<=mplth) loop */
}
k++;
s = add(s, plag[i]); /* update s to the next multiple pitch lag */
}
/* if there is a qualified peak at every multiple of plag[i], */
if (flag == 1) {
cpp = plag[i]; /* then accept this as final pitch */
return cpp; /* and return to calling function */
}
}
i++;
if (i == npeaks)
break; /* to avoid out of array bound error */
}
/* If program proceeds to here, none of the peaks with lags < 0.5*mplth
qualifies as the final pitch. in this case, check if
there is any peak large enough around last pitch. if so, use its
lag as the final pitch. */
if (im != -1) { /* if there is at least one peak around last pitch */
if (im == imax) { /* if this peak is also the global maximum, */
return cpp; /* return first pitch candidate at global max */
}
if (im < imax) { /* if lag of this peak < lag of global max, */
a0 = L_mult(cor2m,energymax_man);
t1 = extract_h(L_mult(energym_man, LPTH2));
a1 = L_mult(cor2max, t1);
exp0 = shl(sub(cor2m_exp, cor2max_exp),1);
exp0 = add(exp0, energymax_exp);
exp0 = sub(exp0, energym_exp);
if (exp0 >=0)
a0 = L_shr(a0, exp0);
else
a1 = L_shl(a1, exp0);
if (a0 > a1) {
if (plag[im] > HMAXPPD*DECF) {
cpp=plag[im];
return cpp;
}
for (k=2; k<=5;k++) { /* check if current candidate pitch */
s=mult(plag[imax],invk[k-2]); /* is a sub-multiple of */
t0 = mult_r(s,SMDTH);
a=sub(s, t0); /* the time lag of */
b=add(s, t0); /* the global maximum peak */
if (plag[im]>a && plag[im]<b) { /* if so, */
cpp=plag[im]; /* accept this peak, */
return cpp; /* and return as pitch */
}
}
}
} else { /* if lag of this peak > lag of global max, */
a0 = L_mult(cor2m,energymax_man);
t1 = extract_h(L_mult(energym_man, LPTH1));
a1 = L_mult(cor2max, t1);
exp0 = shl(sub(cor2m_exp, cor2max_exp),1);
exp0 = add(exp0, energymax_exp);
exp0 = sub(exp0, energym_exp);
if (exp0 >=0)
a0 = L_shr(a0, exp0);
else
a1 = L_shl(a1, exp0);
if (a0 > a1) {
cpp = plag[im]; /* if this peak is large enough, */
return cpp; /* accept its lag */
}
}
}
/* If program proceeds to here, we have no choice but to accept the
lag of the global maximum */
return cpp;
}
void 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;
}
Word16 Pitch_ol( /* output: open loop pitch lag */
Word16 signal[], /* input : signal used to compute the open loop pitch */
/* signal[-pit_max] to signal[-1] should be known */
Word16 pit_min, /* input : minimum pitch lag */
Word16 pit_max, /* input : maximum pitch lag */
Word16 L_frame /* input : length of frame to compute pitch */
)
{
Word16 i, j;
Word16 max1, max2, max3;
Word16 p_max1, p_max2, p_max3;
Word32 t0, L_temp;
/* Scaled signal */
Word16 scaled_signal[L_FRAME+PIT_MAX];
Word16 *scal_sig;
scal_sig = &scaled_signal[pit_max];
/*--------------------------------------------------------*
* Verification for risk of overflow. *
*--------------------------------------------------------*/
Overflow = 0;
t0 = 0;
for(i= -pit_max; i< L_frame; i++)
t0 = L_mac(t0, signal[i], signal[i]);
/*--------------------------------------------------------*
* Scaling of input signal. *
* *
* if Overflow -> scal_sig[i] = signal[i]>>3 *
* else if t0 < 1^20 -> scal_sig[i] = signal[i]<<3 *
* else -> scal_sig[i] = signal[i] *
*--------------------------------------------------------*/
if(Overflow == 1)
{
for(i=-pit_max; i<L_frame; i++)
scal_sig[i] = shr(signal[i], 3);
}
else {
L_temp = L_sub(t0, (Word32)1048576L);
if ( L_temp < (Word32)0 ) /* if (t0 < 2^20) */
{
for(i=-pit_max; i<L_frame; i++)
scal_sig[i] = shl(signal[i], 3);
}
else
{
for(i=-pit_max; i<L_frame; i++)
scal_sig[i] = signal[i];
}
}
/*--------------------------------------------------------------------*
* The pitch lag search is divided in three sections. *
* Each section cannot have a pitch multiple. *
* We find a maximum for each section. *
* We compare the maximum of each section by favoring small lag. *
* *
* First section: lag delay = pit_max downto 4*pit_min *
* Second section: lag delay = 4*pit_min-1 downto 2*pit_min *
* Third section: lag delay = 2*pit_min-1 downto pit_min *
*--------------------------------------------------------------------*/
j = shl(pit_min, 2);
p_max1 = Lag_max(scal_sig, L_frame, pit_max, j, &max1);
i = sub(j, 1); j = shl(pit_min, 1);
p_max2 = Lag_max(scal_sig, L_frame, i, j, &max2);
i = sub(j, 1);
p_max3 = Lag_max(scal_sig, L_frame, i, pit_min , &max3);
/*--------------------------------------------------------------------*
* Compare the 3 sections maximum, and favor small lag. *
*--------------------------------------------------------------------*/
if( sub(mult(max1, THRESHPIT), max2) < 0)
{
max1 = max2;
p_max1 = p_max2;
}
if( sub(mult(max1, THRESHPIT), max3) < 0)
{
p_max1 = p_max3;
}
return (p_max1);
}
static void Norm_Corr(Word16 exc[], Word16 xn[], Word16 h[], Word16 L_subfr,
Word16 t_min, Word16 t_max, Word16 corr_norm[])
{
Word16 i,j,k;
Word16 corr_h, corr_l, norm_h, norm_l;
Word32 s, L_temp;
Word16 excf[L_SUBFR];
Word16 scaling, h_fac, *s_excf, scaled_excf[L_SUBFR];
k = negate(t_min);
/* compute the filtered excitation for the first delay t_min */
Convolve(&exc[k], h, excf, L_subfr);
/* scaled "excf[]" to avoid overflow */
for(j=0; j<L_subfr; j++)
scaled_excf[j] = shr(excf[j], 2);
/* Compute energy of excf[] for danger of overflow */
s = 0;
for (j = 0; j < L_subfr; j++)
s = L_mac(s, excf[j], excf[j]);
L_temp = L_sub(s, 67108864L);
if (L_temp <= 0L) /* if (s <= 2^26) */
{
s_excf = excf;
h_fac = 15-12; /* h in Q12 */
scaling = 0;
}
else {
s_excf = scaled_excf; /* "excf[]" is divide by 2 */
h_fac = 15-12-2; /* h in Q12, divide by 2 */
scaling = 2;
}
/* loop for every possible period */
for (i = t_min; i <= t_max; i++)
{
/* Compute 1/sqrt(energy of excf[]) */
s = 0;
for (j = 0; j < L_subfr; j++)
s = L_mac(s, s_excf[j], s_excf[j]);
s = Inv_sqrt(s); /* Result in Q30 */
L_Extract(s, &norm_h, &norm_l);
/* Compute correlation between xn[] and excf[] */
s = 0;
for (j = 0; j < L_subfr; j++)
s = L_mac(s, xn[j], s_excf[j]);
L_Extract(s, &corr_h, &corr_l);
/* Normalize correlation = correlation * (1/sqrt(energy)) */
s = Mpy_32(corr_h, corr_l, norm_h, norm_l);
corr_norm[i] = extract_h(L_shl(s, 16)); /* Result is on 16 bits */
/* modify the filtered excitation excf[] for the next iteration */
if( sub(i, t_max) != 0)
{
k=sub(k,1);
for (j = L_subfr-(Word16)1; j > 0; j--)
{
s = L_mult(exc[k], h[j]);
s = L_shl(s, h_fac); /* h is in Q(12-scaling) */
s_excf[j] = add(extract_h(s), s_excf[j-1]);
}
s_excf[0] = shr(exc[k], scaling);
}
}
return;
}