void acf_averaging (
Word16 r_h[],
Word16 r_l[],
Word16 scal_acf,
Word32 L_av0[],
Word32 L_av1[]
)
{
Word32 L_temp;
Word16 scale;
Word16 i;
scale = add (9, scal_acf);
for (i = 0; i <= 8; i++)
{
if(scale<0)
L_temp = L_shl (L_Comp (r_h[i], r_l[i]), -scale);
else
L_temp = L_shr (L_Comp (r_h[i], r_l[i]), scale);
L_av0[i] = L_add (L_sacf[i], L_temp);
L_av0[i] = L_add (L_sacf[i + 9], L_av0[i]);
L_av0[i] = L_add (L_sacf[i + 18], L_av0[i]);
L_sacf[pt_sacf + i] = L_temp;
L_av1[i] = L_sav0[pt_sav0 + i];
L_sav0[pt_sav0 + i] = L_av0[i];
}
/* Update the array pointers */
if (sub (pt_sacf, 18) == 0)
{
pt_sacf = 0;
}
else
{
pt_sacf = add (pt_sacf, 9);
}
if (sub (pt_sav0, 27) == 0)
{
pt_sav0 = 0;
}
else
{
pt_sav0 = add (pt_sav0, 9);
}
return;
}
/*---------------------------------------------------------------------------*
* Function Gain_update *
* ~~~~~~~~~~~~~~~~~~~~~~ *
* update table of past quantized energies *
*---------------------------------------------------------------------------*/
void Gain_update(
Word16 past_qua_en[], /* (io) Q10 :Past quantized energies */
Word32 L_gbk12 /* (i) Q13 : gbk1[indice1][1]+gbk2[indice2][1] */
)
{
Word16 i, tmp;
Word16 exp, frac;
Word32 L_acc;
for(i=3; i>0; i--){
past_qua_en[i] = past_qua_en[i-1]; /* Q10 */
}
/*----------------------------------------------------------------------*
* -- past_qua_en[0] = 20*log10(gbk1[index1][1]+gbk2[index2][1]); -- *
* 2 * 10 log10( gbk1[index1][1]+gbk2[index2][1] ) *
* = 2 * 3.0103 log2( gbk1[index1][1]+gbk2[index2][1] ) *
* = 2 * 3.0103 log2( gbk1[index1][1]+gbk2[index2][1] ) *
* 24660:Q12(6.0205) *
*----------------------------------------------------------------------*/
Log2( L_gbk12, &exp, &frac ); /* L_gbk12:Q13 */
L_acc = L_Comp( sub(exp,13), frac); /* L_acc:Q16 */
tmp = extract_h( L_shl( L_acc,13 ) ); /* tmp:Q13 */
past_qua_en[0] = mult( tmp, 24660 ); /* past_qua_en[]:Q10 */
}
/*-----------------------------------------------------------------*
* Functions vad *
* ~~~ *
* Input: *
* rc : reflection coefficient *
* lsf[] : unquantized lsf vector *
* r_h[] : upper 16-bits of the autocorrelation vector *
* r_l[] : lower 16-bits of the autocorrelation vector *
* exp_R0 : exponent of the autocorrelation vector *
* sigpp[] : preprocessed input signal *
* frm_count : frame counter *
* prev_marker : VAD decision of the last frame *
* pprev_marker : VAD decision of the frame before last frame *
* *
* Output: *
* *
* marker : VAD decision of the current frame *
* *
*-----------------------------------------------------------------*/
void vadg(
Word16 rc,
Word16 *lsf,
Word16 *r_h,
Word16 *r_l,
Word16 exp_R0,
Word16 *sigpp,
Word16 frm_count,
Word16 prev_marker,
Word16 pprev_marker,
Word16 *marker,
Word16 *ENERGY_db)
{
/* scalar */
Word32 acc0;
Word16 i, j, exp, frac;
Word16 ENERGY, ENERGY_low, SD, ZC, dSE, dSLE, dSZC;
Word16 COEF, C_COEF, COEFZC, C_COEFZC, COEFSD, C_COEFSD;
/* compute the frame energy */
acc0 = L_Comp(r_h[0], r_l[0]);
Log2(acc0, &exp, &frac);
acc0 = Mpy_32_16(exp, frac, 9864);
i = sub(exp_R0, 1);
i = sub(i, 1);
acc0 = L_mac(acc0, 9864, i);
acc0 = L_shl(acc0, 11);
ENERGY = extract_h(acc0);
ENERGY = sub(ENERGY, 4875);
*ENERGY_db = ENERGY;
/* compute the low band energy */
acc0 = 0;
for (i=1; i<=NP; i++)
acc0 = L_mac(acc0, r_h[i], lbf_corr[i]);
acc0 = L_shl(acc0, 1);
acc0 = L_mac(acc0, r_h[0], lbf_corr[0]);
Log2(acc0, &exp, &frac);
acc0 = Mpy_32_16(exp, frac, 9864);
i = sub(exp_R0, 1);
i = sub(i, 1);
acc0 = L_mac(acc0, 9864, i);
acc0 = L_shl(acc0, 11);
ENERGY_low = extract_h(acc0);
ENERGY_low = sub(ENERGY_low, 4875);
/* compute SD */
acc0 = 0;
for (i=0; i<M; i++){
j = sub(lsf[i], MeanLSF[i]);
acc0 = L_mac(acc0, j, j);
}
SD = extract_h(acc0); /* Q15 */
/* compute # zero crossing */
ZC = 0;
for (i=ZC_START+1; i<=ZC_END; i++)
if (mult(sigpp[i-1], sigpp[i]) < 0){
ZC = add(ZC, 410); /* Q15 */
}
/* Initialize and update Mins */
if(sub(frm_count, 129) < 0){
if (sub(ENERGY, Min) < 0){
Min = ENERGY;
Prev_Min = ENERGY;
}
if((frm_count & 0x0007) == 0){
i = sub(shr(frm_count,3),1);
Min_buffer[i] = Min;
Min = MAX_16;
}
}
if((frm_count & 0x0007) == 0){
Prev_Min = Min_buffer[0];
for (i=1; i<16; i++){
if (sub(Min_buffer[i], Prev_Min) < 0){
Prev_Min = Min_buffer[i];
}
}
}
if(sub(frm_count, 129) >= 0){
if(((frm_count & 0x0007) ^ (0x0001)) == 0){
Min = Prev_Min;
Next_Min = MAX_16;
}
if (sub(ENERGY, Min) < 0){
Min = ENERGY;
}
if (sub(ENERGY, Next_Min) < 0){
Next_Min = ENERGY;
}
if((frm_count & 0x0007) == 0){
for (i=0; i<15; i++)
Min_buffer[i] = Min_buffer[i+1];
Min_buffer[15] = Next_Min;
Prev_Min = Min_buffer[0];
for (i=1; i<16; i++)
if (sub(Min_buffer[i], Prev_Min) < 0){
Prev_Min = Min_buffer[i];
}
}
}
if (sub(frm_count, INIT_FRAME) <= 0){
if(sub(ENERGY, 3072) < 0){
*marker = NOISE;
less_count++;
}
else{
*marker = VOICE;
acc0 = L_deposit_h(MeanE);
acc0 = L_mac(acc0, ENERGY, 1024);
MeanE = extract_h(acc0);
acc0 = L_deposit_h(MeanSZC);
acc0 = L_mac(acc0, ZC, 1024);
MeanSZC = extract_h(acc0);
for (i=0; i<M; i++){
acc0 = L_deposit_h(MeanLSF[i]);
acc0 = L_mac(acc0, lsf[i], 1024);
MeanLSF[i] = extract_h(acc0);
}
}
}
if (sub(frm_count, INIT_FRAME) >= 0){
if (sub(frm_count, INIT_FRAME) == 0){
acc0 = L_mult(MeanE, factor_fx[less_count]);
acc0 = L_shl(acc0, shift_fx[less_count]);
MeanE = extract_h(acc0);
acc0 = L_mult(MeanSZC, factor_fx[less_count]);
acc0 = L_shl(acc0, shift_fx[less_count]);
MeanSZC = extract_h(acc0);
for (i=0; i<M; i++){
acc0 = L_mult(MeanLSF[i], factor_fx[less_count]);
acc0 = L_shl(acc0, shift_fx[less_count]);
MeanLSF[i] = extract_h(acc0);
}
MeanSE = sub(MeanE, 2048); /* Q11 */
MeanSLE = sub(MeanE, 2458); /* Q11 */
}
dSE = sub(MeanSE, ENERGY);
dSLE = sub(MeanSLE, ENERGY_low);
dSZC = sub(MeanSZC, ZC);
if(sub(ENERGY, 3072) < 0){
*marker = NOISE;
}
else {
*marker = MakeDec(dSLE, dSE, SD, dSZC);
}
v_flag = 0;
if((prev_marker==VOICE) && (*marker==NOISE) && (add(dSE,410)<0)
&& (sub(ENERGY, 3072)>0)){
*marker = VOICE;
v_flag = 1;
}
if(flag == 1){
if((pprev_marker == VOICE) &&
(prev_marker == VOICE) &&
(*marker == NOISE) &&
(sub(abs_s(sub(prev_energy,ENERGY)), 614) <= 0)){
count_ext++;
*marker = VOICE;
v_flag = 1;
if(sub(count_ext, 4) <= 0){
flag=1;
}
else{
count_ext=0;
flag=0;
}
}
}
else{
flag=1;
}
if(*marker == NOISE){
count_sil++;
}
if((*marker == VOICE) && (sub(count_sil, 10) > 0) &&
(sub(sub(ENERGY,prev_energy), 614) <= 0)){
*marker = NOISE;
count_sil=0;
}
if(*marker == VOICE){
count_sil=0;
}
if ((sub(sub(ENERGY, 614), MeanSE)<0) && (sub(frm_count, 128) > 0)
&& (!v_flag) && (sub(rc, 19661) < 0)){
*marker = NOISE;
}
if ((sub(sub(ENERGY,614),MeanSE) < 0) && (sub(rc, 24576) < 0)
&& (sub(SD, 83) < 0)){
count_update++;
if (sub(count_update, INIT_COUNT) < 0){
COEF = 24576;
C_COEF = 8192;
COEFZC = 26214;
C_COEFZC = 6554;
COEFSD = 19661;
C_COEFSD = 13017;
}
else
if (sub(count_update, INIT_COUNT+10) < 0){
COEF = 31130;
C_COEF = 1638;
COEFZC = 30147;
C_COEFZC = 2621;
COEFSD = 21299;
C_COEFSD = 11469;
}
else
if (sub(count_update, INIT_COUNT+20) < 0){
COEF = 31785;
C_COEF = 983;
COEFZC = 30802;
C_COEFZC = 1966;
COEFSD = 22938;
C_COEFSD = 9830;
}
else
if (sub(count_update, INIT_COUNT+30) < 0){
COEF = 32440;
C_COEF = 328;
COEFZC = 31457;
C_COEFZC = 1311;
COEFSD = 24576;
C_COEFSD = 8192;
}
else
if (sub(count_update, INIT_COUNT+40) < 0){
COEF = 32604;
C_COEF = 164;
COEFZC = 32440;
C_COEFZC = 328;
COEFSD = 24576;
C_COEFSD = 8192;
}
else{
COEF = 32604;
C_COEF = 164;
COEFZC = 32702;
C_COEFZC = 66;
COEFSD = 24576;
C_COEFSD = 8192;
}
/* compute MeanSE */
acc0 = L_mult(COEF, MeanSE);
acc0 = L_mac(acc0, C_COEF, ENERGY);
MeanSE = extract_h(acc0);
/* compute MeanSLE */
acc0 = L_mult(COEF, MeanSLE);
acc0 = L_mac(acc0, C_COEF, ENERGY_low);
MeanSLE = extract_h(acc0);
/* compute MeanSZC */
acc0 = L_mult(COEFZC, MeanSZC);
acc0 = L_mac(acc0, C_COEFZC, ZC);
MeanSZC = extract_h(acc0);
/* compute MeanLSF */
for (i=0; i<M; i++){
acc0 = L_mult(COEFSD, MeanLSF[i]);
acc0 = L_mac(acc0, C_COEFSD, lsf[i]);
MeanLSF[i] = extract_h(acc0);
}
}
if((sub(frm_count, 128) > 0) && (((sub(MeanSE,Min) < 0) && (sub(SD, 83) < 0)
) || (sub(MeanSE,Min) > 2048))){
MeanSE = Min;
count_update = 0;
}
}
prev_energy = ENERGY;
}
/*************************************************************************
*
* FUNCTION: gc_pred()
*
* PURPOSE: MA prediction of the innovation energy
* (in dB/(20*log10(2))) with mean removed).
*
*************************************************************************/
void
gc_pred(
gc_predState *st, /* i/o: State struct */
enum Mode mode, /* i : AMR mode */
Word16 *code, /* i : innovative codebook vector (L_SUBFR) */
/* MR122: Q12, other modes: Q13 */
Word16 *exp_gcode0, /* o : exponent of predicted gain factor, Q0 */
Word16 *frac_gcode0,/* o : fraction of predicted gain factor Q15 */
Word16 *exp_en, /* o : exponent of innovation energy, Q0 */
/* (only calculated for MR795) */
Word16 *frac_en /* o : fraction of innovation energy, Q15 */
/* (only calculated for MR795) */
)
{
Word16 i;
Word32 ener_code;
Word16 exp, frac;
/*-------------------------------------------------------------------*
* energy of code: *
* ~~~~~~~~~~~~~~~ *
* ener_code = sum(code[i]^2) *
*-------------------------------------------------------------------*/
ener_code = L_mac((Word32) 0, code[0], code[0]);
/* MR122: Q12*Q12 -> Q25 */
/* others: Q13*Q13 -> Q27 */
for (i = 1; i < L_SUBFR; i++)
ener_code = L_mac(ener_code, code[i], code[i]);
test ();
if (sub (mode, MR122) == 0)
{
Word32 ener;
/* ener_code = ener_code / lcode; lcode = 40; 1/40 = 26214 Q20 */
ener_code = L_mult (round (ener_code), 26214); /* Q9 * Q20 -> Q30 */
/*-------------------------------------------------------------------*
* energy of code: *
* ~~~~~~~~~~~~~~~ *
* ener_code(Q17) = 10 * Log10(energy) / constant *
* = 1/2 * Log2(energy) *
* constant = 20*Log10(2) *
*-------------------------------------------------------------------*/
/* ener_code = 1/2 * Log2(ener_code); Note: Log2=log2+30 */
Log2(ener_code, &exp, &frac);
ener_code = L_Comp (sub (exp, 30), frac); /* Q16 for log() */
/* ->Q17 for 1/2 log()*/
/*-------------------------------------------------------------------*
* predicted energy: *
* ~~~~~~~~~~~~~~~~~ *
* ener(Q24) = (Emean + sum{pred[i]*past_en[i]})/constant *
* = MEAN_ENER + sum(pred[i]*past_qua_en[i]) *
* constant = 20*Log10(2) *
*-------------------------------------------------------------------*/
ener = MEAN_ENER_MR122;
move32 (); /* Q24 (Q17) */
for (i = 0; i < NPRED; i++)
{
ener = L_mac (ener, st->past_qua_en_MR122[i], pred_MR122[i]);
/* Q10 * Q13 -> Q24 */
/* Q10 * Q6 -> Q17 */
}
/*-------------------------------------------------------------------*
* predicted codebook gain *
* ~~~~~~~~~~~~~~~~~~~~~~~ *
* gc0 = Pow10( (ener*constant - ener_code*constant) / 20 ) *
* = Pow2(ener-ener_code) *
* = Pow2(int(d)+frac(d)) *
* *
* (store exp and frac for pow2()) *
*-------------------------------------------------------------------*/
ener = L_shr (L_sub (ener, ener_code), 1); /* Q16 */
L_Extract(ener, exp_gcode0, frac_gcode0);
}
else /* all modes except 12.2 */
{
Word32 L_tmp;
Word16 exp_code, gcode0;
/*-----------------------------------------------------------------*
* Compute: means_ener - 10log10(ener_code/ L_sufr) *
*-----------------------------------------------------------------*/
exp_code = norm_l (ener_code);
ener_code = L_shl (ener_code, exp_code);
/* Log2 = log2 + 27 */
Log2_norm (ener_code, exp_code, &exp, &frac);
/* fact = 10/log2(10) = 3.01 = 24660 Q13 */
L_tmp = Mpy_32_16(exp, frac, -24660); /* Q0.Q15 * Q13 -> Q14 */
/* L_tmp = means_ener - 10log10(ener_code/L_SUBFR)
* = means_ener - 10log10(ener_code) + 10log10(L_SUBFR)
* = K - fact * Log2(ener_code)
* = K - fact * log2(ener_code) - fact*27
*
* ==> K = means_ener + fact*27 + 10log10(L_SUBFR)
*
* means_ener = 33 = 540672 Q14 (MR475, MR515, MR59)
* means_ener = 28.75 = 471040 Q14 (MR67)
* means_ener = 30 = 491520 Q14 (MR74)
* means_ener = 36 = 589824 Q14 (MR795)
* means_ener = 33 = 540672 Q14 (MR102)
* 10log10(L_SUBFR) = 16.02 = 262481.51 Q14
* fact * 27 = 1331640 Q14
* -----------------------------------------
* (MR475, MR515, MR59) K = 2134793.51 Q14 ~= 16678 * 64 * 2
* (MR67) K = 2065161.51 Q14 ~= 32268 * 32 * 2
* (MR74) K = 2085641.51 Q14 ~= 32588 * 32 * 2
* (MR795) K = 2183945.51 Q14 ~= 17062 * 64 * 2
* (MR102) K = 2134793.51 Q14 ~= 16678 * 64 * 2
*/
if (test (), sub (mode, MR102) == 0)
{
/* mean = 33 dB */
L_tmp = L_mac(L_tmp, 16678, 64); /* Q14 */
}
else if (test (), sub (mode, MR795) == 0)
{
/* ener_code = <xn xn> * 2^27*2^exp_code
frac_en = ener_code / 2^16
= <xn xn> * 2^11*2^exp_code
<xn xn> = <xn xn>*2^11*2^exp * 2^exp_en
:= frac_en * 2^exp_en
==> exp_en = -11-exp_code;
*/
*frac_en = extract_h (ener_code);
move16 ();
*exp_en = sub (-11, exp_code);
move16 ();
/* mean = 36 dB */
L_tmp = L_mac(L_tmp, 17062, 64); /* Q14 */
}
else if (test (), sub (mode, MR74) == 0)
{
/* mean = 30 dB */
L_tmp = L_mac(L_tmp, 32588, 32); /* Q14 */
}
else if (test (), sub (mode, MR67) == 0)
{
/* mean = 28.75 dB */
L_tmp = L_mac(L_tmp, 32268, 32); /* Q14 */
}
else /* MR59, MR515, MR475 */
{
/* mean = 33 dB */
L_tmp = L_mac(L_tmp, 16678, 64); /* Q14 */
}
/*-----------------------------------------------------------------*
* Compute gcode0. *
* = Sum(i=0,3) pred[i]*past_qua_en[i] - ener_code + mean_ener *
*-----------------------------------------------------------------*/
L_tmp = L_shl(L_tmp, 10); /* Q24 */
for (i = 0; i < 4; i++)
L_tmp = L_mac(L_tmp, pred[i], st->past_qua_en[i]);
/* Q13 * Q10 -> Q24 */
gcode0 = extract_h(L_tmp); /* Q8 */
/*-----------------------------------------------------------------*
* gcode0 = pow(10.0, gcode0/20) *
* = pow(2, 3.3219*gcode0/20) *
* = pow(2, 0.166*gcode0) *
*-----------------------------------------------------------------*/
/* 5439 Q15 = 0.165985 */
/* (correct: 1/(20*log10(2)) 0.166096 = 5443 Q15) */
test ();
if (sub (mode, MR74) == 0) /* For IS641 bitexactness */
L_tmp = L_mult(gcode0, 5439); /* Q8 * Q15 -> Q24 */
else
L_tmp = L_mult(gcode0, 5443); /* Q8 * Q15 -> Q24 */
L_tmp = L_shr(L_tmp, 8); /* -> Q16 */
L_Extract(L_tmp, exp_gcode0, frac_gcode0); /* -> Q0.Q15 */
}
}
/*************************************************************************
*
* 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 ();
}
}
void Levinson(int32_t r32[], /* (i) : r32[] double precision vector of autocorrelation coefficients */
int16_t a[], /* (o) : a[] in Q12 - LPC coefficients */
int16_t old_a[], /* (i/o): old_a[] in Q12 - previous LPC coefficients */
int16_t m /* (i) : LPC order */
)
{
int16_t i, j, high, low, alpha_hi, alpha_lo, alpha_exp;
int16_t exp, r_hi[LPCO + 1], r_lo[LPCO + 1];
int16_t a_hi[LPCO + 1], a_lo[LPCO + 1], anew_hi[LPCO + 1],
anew_lo[LPCO + 1];
int16_t rc_hi, rc_lo;
int32_t a0, a1, alpha_man;
memzero(r_hi, (LPCO + 1) * sizeof(int16_t));
memzero(r_lo, (LPCO + 1) * sizeof(int16_t));
memzero(anew_hi, (LPCO + 1) * sizeof(int16_t));
memzero(anew_lo, (LPCO + 1) * sizeof(int16_t));
/* Normalization of autocorrelation coefficients */
exp = bv_norm_l(r32[0]);
for (i = 0; i <= m; i++) {
r32[i] = L_bv_shl(r32[i], exp);
L_Extract(r32[i], r_hi + i, r_lo + i);
}
/* a[1] = rc = -r[1]/r[0] */
a1 = bv_L_abs(r32[1]);
a0 = Div_32(a1, r_hi[0], r_lo[0]); // rc in Q31
if (r32[1] > 0)
a0 = L_bv_negate(a0);
L_Extract(L_bv_shr(a0, 4), a_hi + 1, a_lo + 1); // Q27
/* alpha = r[0]*(1-rc*rc) */
L_Extract(a0, &high, &low);
a0 = Mpy_32(high, low, high, low); // rc^2 in Q31
a0 = bv_L_abs(a0); // Lesson from G.729
a0 = L_bv_sub(0x40000000, L_bv_shr(a0, 1)); // 1-rc*rc in Q30
L_Extract(a0, &high, &low);
a0 = Mpy_32(r_hi[0], r_lo[0], high, low); // alpha in Q30
alpha_exp = bv_norm_l(a0);
alpha_man = L_bv_shl(a0, alpha_exp);
alpha_exp = bv_sub(alpha_exp, 1); // alpha: Q(31+alpha_exp)
/* Recursive solution of Yule-Walker equations */
for (i = 2; i <= m; i++) {
/* s = r[i] + sum{r[j]*a[i-j], j=1,2,...,i-1} */
a0 = 0;
for (j = 1; j < i; j++) {
a1 = Mpy_32(r_hi[j], r_lo[j], a_hi[i - j], a_lo[i - j]); // Q27
a0 = L_bv_add(a0, a1); // Q27
}
a0 = L_bv_shl(a0, 4); // Q31
a0 = L_bv_add(a0, r32[i]); // Q31
/* rc = -s/alpha */
exp = bv_norm_l(a0);
a0 = L_bv_shl(a0, exp);
a1 = bv_L_abs(a0);
if (L_bv_sub(a1, alpha_man) >= 0) {
a1 = L_bv_shr(a1, 1);
exp = bv_sub(exp, 1);
}
L_Extract(alpha_man, &alpha_hi, &alpha_lo);
a1 = Div_32(a1, alpha_hi, alpha_lo);
if (a0 > 0)
a1 = L_bv_negate(a1); // rc in Q(31+exp-alpha_exp)
a1 = L_bv_shr(a1, bv_sub(exp, alpha_exp)); // rc in Q31
L_Extract(a1, &rc_hi, &rc_lo); // rc in Q31
/* Check for absolute value of reflection coefficient - stability */
if (bv_sub(bv_abs_s(intround(a1)), 32750) > 0) {
a[0] = 4096;
for (j = 1; j <= m; j++)
a[j] = old_a[j];
return;
}
/* anew[j]=a[j]+rc*a[i-j], j=1,2,...i-1 */
/* anew[i]=rc */
for (j = 1; j < i; j++) {
a0 = Mpy_32(a_hi[i - j], a_lo[i - j], rc_hi, rc_lo); // Q27
a0 = L_bv_add(a0, L_Comp(a_hi[j], a_lo[j])); // Q27
L_Extract(a0, anew_hi + j, anew_lo + j); // Q27
}
L_Extract(L_bv_shr(a1, 4), anew_hi + i, anew_lo + i); // Q27
/* alpha = alpha*(1-rc*rc) */
a0 = Mpy_32(rc_hi, rc_lo, rc_hi, rc_lo); // rc*rc in Q31
a0 = bv_L_abs(a0); // Lesson from G.729
a0 = L_bv_shr(a0, 1); // rc*rc in Q30
a0 = L_bv_sub(0x40000000, a0); // 1-rc*rc in Q30
L_Extract(a0, &high, &low);
a0 = Mpy_32(alpha_hi, alpha_lo, high, low); // Q(30+alpha_exp)
exp = bv_norm_l(a0);
alpha_man = L_bv_shl(a0, exp); // Q(30+exp+alpha_exp)
alpha_exp = bv_sub(bv_add(alpha_exp, exp), 1); // alpha: Q(31+alpha_exp)
/* a[j] = anew[j] in Q(12+a1_exp) */
for (j = 1; j <= i; j++) {
a_hi[j] = anew_hi[j];
a_lo[j] = anew_lo[j];
}
}
/* convert lpc coefficients to Q12 and save new lpc as old lpc for next frame */
a[0] = 4096;
for (j = 1; j <= m; j++) {
a[j] = intround(L_bv_shl(L_Comp(a_hi[j], a_lo[j]), 1)); // Q12
old_a[j] = a[j];
}
return;
}
void Levinsone(
Word16 m, /* (i) : LPC order */
Word16 Rh[], /* (i) : Rh[M+1] Vector of autocorrelations (msb) */
Word16 Rl[], /* (i) : Rl[M+1] Vector of autocorrelations (lsb) */
Word16 A[], /* (o) Q12 : A[M] LPC coefficients (m = 10) */
Word16 rc[], /* (o) Q15 : rc[M] Reflection coefficients. */
Word16 old_A[], /* (i/o) Q12 : last stable filter LPC coefficients */
Word16 old_rc[] /* (i/o) Q15 : last stable filter Reflection coefficients. */
)
{
Word16 i, j;
Word16 hi, lo;
Word16 Kh, Kl; /* reflection coefficient; hi and lo */
Word16 alp_h, alp_l, alp_exp; /* Prediction gain; hi lo and exponent */
Word16 Ah[M_BWDP1], Al[M_BWDP1]; /* LPC coef. in double prec. */
Word16 Anh[M_BWDP1], Anl[M_BWDP1]; /* LPC coef.for next iteration in double prec. */
Word32 t0, t1, t2; /* temporary variable */
Word32 tmp;
/* K = A[1] = -R[1] / R[0] */
t1 = L_Comp(Rh[1], Rl[1]); /* R[1] in Q31 */
tmp = L_Comp(Rh[0], Rl[0]);
if (t1 > tmp) t1 = tmp;
t2 = L_abs(t1); /* abs R[1] */
t0 = Div_32(t2, Rh[0], Rl[0]); /* R[1]/R[0] in Q31 */
if(t1 > 0) t0= L_negate(t0); /* -R[1]/R[0] */
L_Extract(t0, &Kh, &Kl); /* K in DPF */
rc[0] = Kh;
t0 = L_shr(t0,4); /* A[1] in Q27 */
L_Extract(t0, &Ah[1], &Al[1]); /* A[1] in DPF */
/* Alpha = R[0] * (1-K**2) */
t0 = Mpy_32(Kh ,Kl, Kh, Kl); /* K*K in Q31 */
t0 = L_abs(t0); /* Some case <0 !! */
t0 = L_sub( (Word32)0x7fffffffL, t0 ); /* 1 - K*K in Q31 */
L_Extract(t0, &hi, &lo); /* DPF format */
t0 = Mpy_32(Rh[0] ,Rl[0], hi, lo); /* Alpha in Q31 */
/* Normalize Alpha */
alp_exp = norm_l(t0);
t0 = L_shl(t0, alp_exp);
L_Extract(t0, &alp_h, &alp_l); /* DPF format */
/*--------------------------------------*
* ITERATIONS I=2 to m *
*--------------------------------------*/
for(i= 2; i<=m; i++)
{
/* t0 = SUM ( R[j]*A[i-j] ,j=1,i-1 ) + R[i] */
t0 = 0;
for(j=1; j<i; j++)
t0 = L_add(t0, Mpy_32(Rh[j], Rl[j], Ah[i-j], Al[i-j]));
t0 = L_shl(t0,4); /* result in Q27 -> convert to Q31 */
/* No overflow possible */
t1 = L_Comp(Rh[i],Rl[i]);
t0 = L_add(t0, t1); /* add R[i] in Q31 */
/* K = -t0 / Alpha */
t1 = L_abs(t0);
tmp = L_Comp(alp_h, alp_l);
if (t1 > tmp) t1 = tmp;
t2 = Div_32(t1, alp_h, alp_l); /* abs(t0)/Alpha */
if(t0 > 0) t2= L_negate(t2); /* K =-t0/Alpha */
t2 = L_shl(t2, alp_exp); /* denormalize; compare to Alpha */
L_Extract(t2, &Kh, &Kl); /* K in DPF */
rc[i-1] = Kh;
/* Test for unstable filter. If unstable keep old A(z) */
if (sub(abs_s(Kh), 32750) > 0)
{
for(j=0; j<=m; j++)
{
A[j] = old_A[j];
}
rc[0] = old_rc[0]; /* only two rc coefficients are needed */
rc[1] = old_rc[1];
return;
}
/*------------------------------------------*
* Compute new LPC coeff. -> An[i] *
* An[j]= A[j] + K*A[i-j] , j=1 to i-1 *
* An[i]= K *
*------------------------------------------*/
for(j=1; j<i; j++)
{
t0 = Mpy_32(Kh, Kl, Ah[i-j], Al[i-j]);
t0 = L_add(t0, L_Comp(Ah[j], Al[j]));
L_Extract(t0, &Anh[j], &Anl[j]);
}
t2 = L_shr(t2, 4); /* t2 = K in Q31 ->convert to Q27 */
L_Extract(t2, &Anh[i], &Anl[i]); /* An[i] in Q27 */
/* Alpha = Alpha * (1-K**2) */
t0 = Mpy_32(Kh ,Kl, Kh, Kl); /* K*K in Q31 */
t0 = L_abs(t0); /* Some case <0 !! */
t0 = L_sub( (Word32)0x7fffffffL, t0 ); /* 1 - K*K in Q31 */
L_Extract(t0, &hi, &lo); /* DPF format */
t0 = Mpy_32(alp_h , alp_l, hi, lo); /* Alpha in Q31 */
/* Normalize Alpha */
j = norm_l(t0);
t0 = L_shl(t0, j);
L_Extract(t0, &alp_h, &alp_l); /* DPF format */
alp_exp = add(alp_exp, j); /* Add normalization to alp_exp */
/* A[j] = An[j] */
for(j=1; j<=i; j++)
{
Ah[j] =Anh[j];
Al[j] =Anl[j];
}
}
/* Truncate A[i] in Q27 to Q12 with rounding */
A[0] = 4096;
for(i=1; i<=m; i++)
{
t0 = L_Comp(Ah[i], Al[i]);
old_A[i] = A[i] = round(L_shl(t0, 1));
}
old_rc[0] = rc[0];
old_rc[1] = rc[1];
return;
}
/*----------------------------------------------------------------------------
* search_del: computes best (shortest) integer LTP delay + fine search
*----------------------------------------------------------------------------
*/
static void search_del(
Word16 t0, /* input : pitch delay given by coder */
Word16 *ptr_sig_in, /* input : input signal (with delay line) */
Word16 *ltpdel, /* output: delay = *ltpdel - *phase / f_up */
Word16 *phase, /* output: phase */
Word16 *num_gltp, /* output: 16 bits numerator of LTP gain */
Word16 *den_gltp, /* output: 16 bits denominator of LTP gain */
Word16 *sh_num_gltp, /* output: justification for num_gltp */
Word16 *sh_den_gltp, /* output: justification for den_gltp */
Word16 *y_up, /* output: LT delayed signal if fract. delay */
Word16 *off_yup /* output: offset in y_up */
)
{
/* Tables of constants */
extern Word16 tab_hup_s[SIZ_TAB_HUP_S];
Word32 L_den0[F_UP_PST-1];
Word32 L_den1[F_UP_PST-1];
Word32 *ptr_L_den0, *ptr_L_den1;
int i, n;
Word16 *ptr_h;
Word16 *ptr_sig_past, *ptr_sig_past0;
Word16 *ptr1, *ptr_y_up;
Word16 num, den0, den1;
Word16 den_max, num_max;
Word32 L_num_int, L_den_int, L_den_max;
Word16 hi_numsq, hi_numsq_max;
Word16 lo_numsq, lo_numsq_max;
Word16 ener;
Word16 sh_num, sh_den, sh_ener;
Word16 i_max, lambda, phi, phi_max, ioff;
Word16 temp;
Word32 L_temp0, L_temp1;
Word32 L_acc;
Word32 L_temp;
/* Computes energy of current signal */
/*************************************/
L_acc = 0L;
for(i=0; i<L_SUBFR; i++) {
L_acc = L_mac( L_acc, ptr_sig_in[i] , ptr_sig_in[i]);
}
if(L_acc == 0) {
*num_gltp = 0;
*den_gltp = 1;
*ltpdel = 0;
*phase = 0;
return;
}
sh_ener = sub(16, norm_l(L_acc));
/* save energy for final decision */
if(sh_ener > 0) {
ener = extract_l(L_shr(L_acc, sh_ener));
}
else {
sh_ener = 0;
ener = extract_l(L_acc);
}
/*************************************/
/* Selects best of 3 integer delays */
/* Maximum of 3 numerators around t0 */
/*************************************/
lambda = sub(t0,1);
ptr_sig_past = ptr_sig_in - lambda;
L_num_int = -1L;
/* initialization used only to suppress Microsoft Visual C++ warnings */
i_max = (Word16)0;
for(i=0; i<3; i++) {
L_acc = 0L;
for(n=0; n<L_SUBFR; n++) {
L_acc = L_mac( L_acc, ptr_sig_in[n] , ptr_sig_past[n]);
}
if(L_acc < 0) {
L_acc = 0L;
}
L_temp =L_sub(L_acc ,L_num_int);
if(L_temp > 0L) {
L_num_int = L_acc;
i_max = (Word16)i;
}
ptr_sig_past--;
}
if(L_num_int == 0) {
*num_gltp = 0;
*den_gltp = 1;
*ltpdel = 0;
*phase = 0;
return;
}
/* Compute den for i_max */
lambda = add(lambda, (Word16)i_max);
ptr_sig_past = ptr_sig_in - lambda;
L_acc = 0L;
for(i=0; i<L_SUBFR; i++) {
temp = *ptr_sig_past++;
L_acc = L_mac( L_acc, temp, temp);
}
if(L_acc == 0L) {
*num_gltp = 0;
*den_gltp = 1;
*ltpdel = 0;
*phase = 0;
return;
}
L_den_int = L_acc;
/***********************************/
/* Select best phase around lambda */
/***********************************/
/* Compute y_up & denominators */
/*******************************/
ptr_y_up = y_up;
L_den_max = L_den_int;
ptr_L_den0 = L_den0;
ptr_L_den1 = L_den1;
ptr_h = tab_hup_s;
temp = sub(lambda, LH_UP_SM1);
ptr_sig_past0 = ptr_sig_in - temp;
/* Loop on phase */
for(phi=1; phi<F_UP_PST; phi++) {
/* Compute y_up for lambda+1 - phi/F_UP_PST */
/* and lambda - phi/F_UP_PST */
ptr_sig_past = ptr_sig_past0;
for(n = 0; n<=L_SUBFR; n++) {
ptr1 = ptr_sig_past++;
L_acc = 0L;
for(i=0; i<LH2_S; i++) {
L_acc = L_mac(L_acc, ptr_h[i], ptr1[-i]);
}
ptr_y_up[n] = round(L_acc);
}
/* compute den0 (lambda+1) and den1 (lambda) */
/* part common to den0 and den1 */
L_acc = 0L;
for(n=1; n<L_SUBFR; n++) {
L_acc = L_mac(L_acc, ptr_y_up[n] ,ptr_y_up[n]);
}
L_temp0 = L_acc; /* saved for den1 */
/* den0 */
L_acc = L_mac(L_acc, ptr_y_up[0] ,ptr_y_up[0]);
*ptr_L_den0 = L_acc;
/* den1 */
L_acc = L_mac(L_temp0, ptr_y_up[L_SUBFR] ,ptr_y_up[L_SUBFR]);
*ptr_L_den1 = L_acc;
if(sub(abs_s(ptr_y_up[0]),abs_s(ptr_y_up[L_SUBFR])) >0) {
L_temp =L_sub(*ptr_L_den0 ,L_den_max );
if(L_temp> 0L) {
L_den_max = *ptr_L_den0;
}
}
else {
L_temp =L_sub(*ptr_L_den1 ,L_den_max );
if(L_temp> 0L) {
L_den_max = *ptr_L_den1;
}
}
ptr_L_den0++;
ptr_L_den1++;
ptr_y_up += L_SUBFRP1;
ptr_h += LH2_S;
}
if(L_den_max == 0) {
*num_gltp = 0;
*den_gltp = 1;
*ltpdel = 0;
*phase = 0;
return;
}
sh_den = sub(16, norm_l(L_den_max));
/* if sh_den <= 0 : dynamic between current frame */
/* and delay line too high */
if(sh_den <= 0) {
*num_gltp = 0;
*den_gltp = 1;
*ltpdel = 0;
*phase = 0;
return;
}
/* search sh_num to justify correlations */
/* sh_num = Max(sh_den, sh_ener) */
sh_num = (sub( sh_den , sh_ener)>=0) ? sh_den : sh_ener;
/* Computation of the numerators */
/* and selection of best num*num/den */
/* for non null phases */
/* Initialize with null phase */
L_acc = L_shr(L_den_int, sh_den); /* sh_den > 0 */
den_max = extract_l(L_acc);
L_acc = L_shr(L_num_int, sh_num); /* sh_num > 0 */
num_max = extract_l(L_acc);
L_acc = L_mult(num_max, num_max);
L_Extract(L_acc, &hi_numsq_max, &lo_numsq_max);
phi_max = 0;
ioff = 1;
ptr_L_den0 = L_den0;
ptr_L_den1 = L_den1;
ptr_y_up = y_up;
/* if den_max = 0 : will be selected and declared unvoiced */
/* if num!=0 & den=0 : will be selected and declared unvoiced */
/* degenerated seldom cases, switch off LT is OK */
/* Loop on phase */
for(phi=1; phi<F_UP_PST; phi++) {
/* compute num for lambda+1 - phi/F_UP_PST */
L_acc = 0L;
for(n = 0; n<L_SUBFR; n++) {
L_acc = L_mac(L_acc, ptr_sig_in[n] ,ptr_y_up[n]);
}
L_acc = L_shr(L_acc, sh_num); /* sh_num > 0 */
if(L_acc < 0L) {
num = 0;
}
else {
num = extract_l(L_acc);
}
/* selection if num**2/den0 max */
L_acc = L_mult(num, num);
L_Extract(L_acc, &hi_numsq, &lo_numsq);
L_temp0 = Mpy_32_16(hi_numsq, lo_numsq, den_max);
L_acc = *ptr_L_den0++;
L_acc = L_shr(L_acc, sh_den); /* sh_den > 0 */
den0 = extract_l(L_acc);
L_temp1 = Mpy_32_16(hi_numsq_max, lo_numsq_max, den0);
L_temp = L_sub(L_temp0, L_temp1);
if(L_temp>0L) {
num_max = num;
hi_numsq_max = hi_numsq;
lo_numsq_max = lo_numsq;
den_max = den0;
ioff = 0;
phi_max = phi;
}
/* compute num for lambda - phi/F_UP_PST */
ptr_y_up++;
L_acc = 0L;
for(n = 0; n<L_SUBFR; n++) {
L_acc = L_mac(L_acc, ptr_sig_in[n] ,ptr_y_up[n]);
}
L_acc = L_shr(L_acc, sh_num); /* sh_num > 0 */
if(L_acc < 0L) {
num = 0;
}
else {
num = extract_l(L_acc);
}
/* selection if num**2/den1 max */
L_acc = L_mult(num, num);
L_Extract(L_acc, &hi_numsq, &lo_numsq);
L_temp0 = Mpy_32_16(hi_numsq, lo_numsq, den_max);
L_acc = *ptr_L_den1++;
L_acc = L_shr(L_acc, sh_den); /* sh_den > 0 */
den1 = extract_l(L_acc);
L_temp1 = Mpy_32_16(hi_numsq_max, lo_numsq_max, den1);
L_temp = L_sub(L_temp0,L_temp1);
if(L_temp> 0L) {
num_max = num;
hi_numsq_max = hi_numsq;
lo_numsq_max = lo_numsq;
den_max = den1;
ioff = 1;
phi_max = phi;
}
ptr_y_up += L_SUBFR;
}
/***************************************************/
/*** test if normalized crit0[iopt] > THRESHCRIT ***/
/***************************************************/
if((num_max == 0) || (sub(den_max,1) <= 0)) {
*num_gltp = 0;
*den_gltp = 1;
*ltpdel = 0;
*phase = 0;
return;
}
/* compare num**2 */
/* to ener * den * 0.5 */
/* (THRESHCRIT = 0.5) */
L_temp1 = L_mult(den_max, ener);
L_temp0 = L_Comp(hi_numsq_max, lo_numsq_max);
/* temp = 2 * sh_num - sh_den - sh_ener + 1 */
/* 16 bits with no overflows */
temp = shl(sh_num,1);
temp = sub(temp, sh_den);
temp = sub(temp, sh_ener);
temp = add(temp, 1);
if(temp < 0) {
temp = negate(temp); /* no overflow */
L_temp0 = L_shr(L_temp0, temp);
}
else {
if(temp > 0) L_temp1 = L_shr(L_temp1, temp);
}
L_temp = L_sub(L_temp0 ,L_temp1);
if(L_temp >= 0L) {
temp = add(lambda, 1);
*ltpdel = sub(temp, ioff);
*off_yup = ioff;
*phase = phi_max;
*num_gltp = num_max;
*den_gltp = den_max;
*sh_den_gltp = sh_den;
*sh_num_gltp = sh_num;
}
else {
*num_gltp = 0;
*den_gltp = 1;
*ltpdel = 0;
*phase = 0;
}
return;
}
/*************************************************************************
*
* FUNCTION: Levinson()
*
* PURPOSE: Levinson-Durbin algorithm in double precision. To compute the
* LP filter parameters from the speech autocorrelations.
*
* DESCRIPTION:
* R[i] autocorrelations.
* A[i] filter coefficients.
* K reflection coefficients.
* Alpha prediction gain.
*
* Initialisation:
* A[0] = 1
* K = -R[1]/R[0]
* A[1] = K
* Alpha = R[0] * (1-K**2]
*
* Do for i = 2 to M
*
* S = SUM ( R[j]*A[i-j] ,j=1,i-1 ) + R[i]
*
* K = -S / Alpha
*
* An[j] = A[j] + K*A[i-j] for j=1 to i-1
* where An[i] = new A[i]
* An[i]=K
*
* Alpha=Alpha * (1-K**2)
*
* END
*
*************************************************************************/
int Levinson (
LevinsonState *st,
Word16 Rh[], /* i : Rh[m+1] Vector of autocorrelations (msb) */
Word16 Rl[], /* i : Rl[m+1] Vector of autocorrelations (lsb) */
Word16 A[], /* o : A[m] LPC coefficients (m = 10) */
Word16 rc[] /* o : rc[4] First 4 reflection coefficients */
)
{
Word16 i, j;
Word16 hi, lo;
Word16 Kh, Kl; /* reflexion coefficient; hi and lo */
Word16 alp_h, alp_l, alp_exp; /* Prediction gain; hi lo and exponent */
Word16 Ah[M + 1], Al[M + 1]; /* LPC coef. in double prec. */
Word16 Anh[M + 1], Anl[M + 1];/* LPC coef.for next iteration in double
prec. */
Word32 t0, t1, t2; /* temporary variable */
/* K = A[1] = -R[1] / R[0] */
t1 = L_Comp (Rh[1], Rl[1]);
t2 = L_abs (t1); /* abs R[1] */
t0 = Div_32 (t2, Rh[0], Rl[0]); /* R[1]/R[0] */
if (t1 > 0)
t0 = L_negate (t0); /* -R[1]/R[0] */
L_Extract (t0, &Kh, &Kl); /* K in DPF */
rc[0] = round (t0);
t0 = L_shr (t0, 4); /* A[1] in */
L_Extract (t0, &Ah[1], &Al[1]); /* A[1] in DPF */
/* Alpha = R[0] * (1-K**2) */
t0 = Mpy_32 (Kh, Kl, Kh, Kl); /* K*K */
t0 = L_abs (t0); /* Some case <0 !! */
t0 = L_sub ((Word32) 0x7fffffffL, t0); /* 1 - K*K */
L_Extract (t0, &hi, &lo); /* DPF format */
t0 = Mpy_32 (Rh[0], Rl[0], hi, lo); /* Alpha in */
/* Normalize Alpha */
alp_exp = norm_l (t0);
t0 = L_shl (t0, alp_exp);
L_Extract (t0, &alp_h, &alp_l); /* DPF format */
/*--------------------------------------*
* ITERATIONS I=2 to M *
*--------------------------------------*/
for (i = 2; i <= M; i++)
{
/* t0 = SUM ( R[j]*A[i-j] ,j=1,i-1 ) + R[i] */
t0 = 0;
for (j = 1; j < i; j++)
{
t0 = L_add (t0, Mpy_32 (Rh[j], Rl[j], Ah[i - j], Al[i - j]));
}
t0 = L_shl (t0, 4);
t1 = L_Comp (Rh[i], Rl[i]);
t0 = L_add (t0, t1); /* add R[i] */
/* K = -t0 / Alpha */
t1 = L_abs (t0);
t2 = Div_32 (t1, alp_h, alp_l); /* abs(t0)/Alpha */
if (t0 > 0)
t2 = L_negate (t2); /* K =-t0/Alpha */
t2 = L_shl (t2, alp_exp); /* denormalize; compare to Alpha */
L_Extract (t2, &Kh, &Kl); /* K in DPF */
if (sub (i, 5) < 0)
{
rc[i - 1] = round (t2);
}
/* Test for unstable filter. If unstable keep old A(z) */
if (sub (abs_s (Kh), 32750) > 0)
{
for (j = 0; j <= M; j++)
{
A[j] = st->old_A[j];
}
for (j = 0; j < 4; j++)
{
rc[j] = 0;
}
return 0;
}
/*------------------------------------------*
* Compute new LPC coeff. -> An[i] *
* An[j]= A[j] + K*A[i-j] , j=1 to i-1 *
* An[i]= K *
*------------------------------------------*/
for (j = 1; j < i; j++)
{
t0 = Mpy_32 (Kh, Kl, Ah[i - j], Al[i - j]);
t0 = L_add(t0, L_Comp(Ah[j], Al[j]));
L_Extract (t0, &Anh[j], &Anl[j]);
}
t2 = L_shr (t2, 4);
L_Extract (t2, &Anh[i], &Anl[i]);
/* Alpha = Alpha * (1-K**2) */
t0 = Mpy_32 (Kh, Kl, Kh, Kl); /* K*K */
t0 = L_abs (t0); /* Some case <0 !! */
t0 = L_sub ((Word32) 0x7fffffffL, t0); /* 1 - K*K */
L_Extract (t0, &hi, &lo); /* DPF format */
t0 = Mpy_32 (alp_h, alp_l, hi, lo);
/* Normalize Alpha */
j = norm_l (t0);
t0 = L_shl (t0, j);
L_Extract (t0, &alp_h, &alp_l); /* DPF format */
alp_exp = add (alp_exp, j); /* Add normalization to
alp_exp */
/* A[j] = An[j] */
for (j = 1; j <= i; j++)
{
Ah[j] = Anh[j];
Al[j] = Anl[j];
}
}
A[0] = 4096;
for (i = 1; i <= M; i++)
{
t0 = L_Comp (Ah[i], Al[i]);
st->old_A[i] = A[i] = round (L_shl (t0, 1));
}
return 0;
}
INT16 q_gain_code ( /* Return quantization index */
INT16 code[], /* (i) : fixed codebook excitation */
INT16 lcode, /* (i) : codevector size */
INT16 *gain, /* (i/o) : quantized fixed codebook gain */
INT16 txdtx_ctrl,
INT16 i_subfr
)
{
INT16 i, index=0;
INT16 gcode0, err, err_min, exp, frac;
INT32 ener, ener_code;
register INT32 ener_code_hi=0;
register UINT32 ener_code_lo=0;
INT16 aver_gain;
static INT16 gcode0_CN;
VPP_EFR_PROFILE_FUNCTION_ENTER(q_gain_code);
if ((txdtx_ctrl & TX_SP_FLAG) != 0)
{
/*-------------------------------------------------------------------*
* energy of code: *
* ~~~~~~~~~~~~~~~ *
* ener_code(Q17) = 10 * Log10(energy/lcode) / constant *
* = 1/2 * Log2(energy/lcode) *
* constant = 20*Log10(2) *
*-------------------------------------------------------------------*/
/* ener_code = log10(ener_code/lcode) / (20*log10(2)) */
//ener_code = 0;
ener_code_lo = 0;
for (i = 0; i < lcode; i++)
{
//ener_code = L_mac (ener_code, code[i], code[i]);
//ener_code = L_MAC(ener_code, code[i], code[i]);
VPP_MLA16(ener_code_hi,ener_code_lo,code[i], code[i]);
}
/* ener_code = ener_code / lcode */
ener_code = VPP_SCALE64_TO_16(ener_code_hi,ener_code_lo);
//ener_code = L_mult (round(ener_code), 26214);
ener_code = L_MULT(ROUND(ener_code), 26214);
/* ener_code = 1/2 * Log2(ener_code) */
Log2 (ener_code, &exp, &frac);
//ener_code = L_Comp (sub (exp, 30), frac);
ener_code = L_Comp (SUB (exp, 30), frac);
/* predicted energy */
//ener = MEAN_ENER;
ener_code_lo = MEAN_ENER>>1;
ener_code_hi =0;
for (i = 0; i < 4; i++)
{
//ener = L_mac (ener, past_qua_en[i], pred[i]);
//ener = L_MAC(ener, past_qua_en[i], pred[i]);
VPP_MLA16(ener_code_hi,ener_code_lo, past_qua_en[i], pred[i]);
}
ener = VPP_SCALE64_TO_16(ener_code_hi,ener_code_lo);
/*-------------------------------------------------------------------*
* predicted codebook gain *
* ~~~~~~~~~~~~~~~~~~~~~~~ *
* gcode0(Qx) = Pow10( (ener*constant - ener_code*constant) / 20 ) *
* = Pow2(ener-ener_code) *
* constant = 20*Log10(2) *
*-------------------------------------------------------------------*/
//ener = L_shr (L_SUB(ener, ener_code), 1);
ener = L_SHR_D(L_SUB(ener, ener_code), 1);
L_Extract (ener, &exp, &frac);
//gcode0 = extract_l (Pow2 (exp, frac)); /* predicted gain */
gcode0 = EXTRACT_L(Pow2 (exp, frac));
//gcode0 = shl (gcode0, 4);
gcode0 = SHL(gcode0, 4);
/*-------------------------------------------------------------------*
* Search for best quantizer *
*-------------------------------------------------------------------*/
//err_min = abs_s (sub (*gain, mult (gcode0, qua_gain_code[0])));
err_min = ABS_S(SUB (*gain, MULT(gcode0, qua_gain_code[0])));
index = 0;
for (i = 1; i < NB_QUA_CODE; i++)
{
//err = abs_s (sub (*gain, mult (gcode0, qua_gain_code[i])));
err = ABS_S(SUB (*gain, MULT (gcode0, qua_gain_code[i])));
//if (sub (err, err_min) < 0)
if (SUB (err, err_min) < 0)
{
err_min = err;
index = i;
}
}
//*gain = mult (gcode0, qua_gain_code[index]);
*gain = MULT(gcode0, qua_gain_code[index]);
/*------------------------------------------------------------------*
* update table of past quantized energies *
* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ *
* past_qua_en(Q12) = 20 * Log10(qua_gain_code) / constant *
* = Log2(qua_gain_code) *
* constant = 20*Log10(2) *
*------------------------------------------------------------------*/
for (i = 3; i > 0; i--)
{
past_qua_en[i] = past_qua_en[i - 1];
}
//Log2 (L_deposit_l (qua_gain_code[index]), &exp, &frac);
Log2 (L_DEPOSIT_L(qua_gain_code[index]), &exp, &frac);
//past_qua_en[0] = shr (frac, 5);
past_qua_en[0] = SHR_D(frac, 5);
//past_qua_en[0] = add (past_qua_en[0], shl (sub (exp, 11), 10));
past_qua_en[0] = ADD (past_qua_en[0], SHL(SUB (exp, 11), 10));
update_gain_code_history_tx (*gain, gain_code_old_tx);
}
