static void Calc_RCoeff(Word16 *Coeff, Word16 *RCoeff, Word16 *sh_RCoeff)
{
Word16 i, j;
Word16 sh1;
Word32 L_acc;
/* RCoeff[0] = SUM(j=0->M) Coeff[j] ** 2 */
L_acc = 0L;
for(j=0; j <= M; j++) {
L_acc = L_mac(L_acc, Coeff[j], Coeff[j]);
}
/* Compute exponent RCoeff */
sh1 = norm_l(L_acc);
L_acc = L_shl(L_acc, sh1);
RCoeff[0] = round(L_acc);
/* RCoeff[i] = SUM(j=0->M-i) Coeff[j] * Coeff[j+i] */
for(i=1; i<=M; i++) {
L_acc = 0L;
for(j=0; j<=M-i; j++) {
L_acc = L_mac(L_acc, Coeff[j], Coeff[j+i]);
}
L_acc = L_shl(L_acc, sh1);
RCoeff[i] = round(L_acc);
}
*sh_RCoeff = sh1;
return;
}
void Deemph_32_(
Word16 x_hi[], /* (i) : input signal (bit31..16) */
Word16 x_lo[], /* (i) : input signal (bit15..4) */
Word16 y[], /* (o) : output signal (x16) */
Word16 mu, /* (i) Q15 : deemphasis factor */
Word16 L, /* (i) : vector size */
Word16 * mem /* (i/o) : memory (y[-1]) */
)
{
Word16 i;
Word32 L_tmp;
/* L_tmp = hi<<16 + lo<<4 */
L_tmp = (Word32)x_hi[0] << 16;
L_tmp = L_tmp + ((Word32)x_lo[0] << 4);
L_tmp = L_shl(L_tmp, 4);
L_tmp = L_mac(L_tmp, *mem, mu); /* saturation can occur here */
y[0] = round16(L_tmp);
for (i = 1; i < L; i++)
{
L_tmp = (Word32)x_hi[i] << 16;
L_tmp = L_tmp + ((Word32)x_lo[i] << 4);
L_tmp = L_shl(L_tmp, 4);
L_tmp = L_mac(L_tmp, y[i - 1], mu); /* saturation can occur here */
y[i] = round16(L_tmp);
}
*mem = y[L - 1];
return;
}
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;
}
void Post_Process(
PopStatus *handle,
Word16 signal[], /* input/output signal */
Word16 lg) /* length of signal */
{
Word16 i, x2;
Word32 L_tmp;
for(i=0; i<lg; i++)
{
x2 = handle->x1;
handle->x1 = handle->x0;
handle->x0 = signal[i];
/* y[i] = b[0]*x[i] + b[1]*x[i-1] + b[2]*x[i-2] */
/* + a[1]*y[i-1] + a[2] * y[i-2]; */
L_tmp = Mpy_32_16(handle->y1_hi, handle->y1_lo, a100[1]);
L_tmp = L_add(L_tmp, Mpy_32_16(handle->y2_hi, handle->y2_lo, a100[2]));
L_tmp = L_mac(L_tmp, handle->x0, b100[0]);
L_tmp = L_mac(L_tmp, handle->x1, b100[1]);
L_tmp = L_mac(L_tmp, x2, b100[2]);
L_tmp = L_shl(L_tmp, 2); /* Q29 --> Q31 (Q13 --> Q15) */
/* Multiplication by two of output speech with saturation. */
signal[i] = round(L_shl(L_tmp, 1));
handle->y2_hi = handle->y1_hi;
handle->y2_lo = handle->y1_lo;
L_Extract(L_tmp, &(handle->y1_hi), &(handle->y1_lo));
}
return;
}
void Post_Process(
int16_t signal[], /* input/output signal */
int16_t lg) /* length of signal */
{
int16_t i, x2;
int32_t L_tmp;
for(i=0; i<lg; i++)
{
x2 = x1;
x1 = x0;
x0 = signal[i];
/* y[i] = b[0]*x[i] + b[1]*x[i-1] + b[2]*x[i-2] */
/* + a[1]*y[i-1] + a[2] * y[i-2]; */
L_tmp = Mpy_32_16(y1_hi, y1_lo, a100[1]);
L_tmp = L_add(L_tmp, Mpy_32_16(y2_hi, y2_lo, a100[2]));
L_tmp = L_mac(L_tmp, x0, b100[0]);
L_tmp = L_mac(L_tmp, x1, b100[1]);
L_tmp = L_mac(L_tmp, x2, b100[2]);
L_tmp = L_shl(L_tmp, 2); /* Q29 --> Q31 (Q13 --> Q15) */
/* Multiplication by two of output speech with saturation. */
signal[i] = _round(L_shl(L_tmp, 1));
y2_hi = y1_hi;
y2_lo = y1_lo;
L_Extract(L_tmp, &y1_hi, &y1_lo);
}
return;
}
void Get_wegt(
Word16 flsp[], /* (i) Q13 : M LSP parameters */
Word16 wegt[] /* (o) Q11->norm : M weighting coefficients */
)
{
Word16 i;
Word16 tmp;
Word32 L_acc;
Word16 sft;
Word16 buf[M]; /* in Q13 */
buf[0] = sub( flsp[1], (PI04+8192) ); /* 8192:1.0(Q13) */
for ( i = 1 ; i < M-1 ; i++ ) {
tmp = sub( flsp[i+1], flsp[i-1] );
buf[i] = sub( tmp, 8192 );
}
buf[M-1] = sub( (PI92-8192), flsp[M-2] );
/* */
for ( i = 0 ; i < M ; i++ ) {
if ( buf[i] > 0 ){
wegt[i] = 2048; /* 2048:1.0(Q11) */
}
else {
L_acc = L_mult( buf[i], buf[i] ); /* L_acc in Q27 */
tmp = extract_h( L_shl( L_acc, 2 ) ); /* tmp in Q13 */
L_acc = L_mult( tmp, CONST10 ); /* L_acc in Q25 */
tmp = extract_h( L_shl( L_acc, 2 ) ); /* tmp in Q11 */
wegt[i] = add( tmp, 2048 ); /* wegt in Q11 */
}
}
/* */
L_acc = L_mult( wegt[4], CONST12 ); /* L_acc in Q26 */
wegt[4] = extract_h( L_shl( L_acc, 1 ) ); /* wegt in Q11 */
L_acc = L_mult( wegt[5], CONST12 ); /* L_acc in Q26 */
wegt[5] = extract_h( L_shl( L_acc, 1 ) ); /* wegt in Q11 */
/* wegt: Q11 -> normalized */
tmp = 0;
for ( i = 0; i < M; i++ ) {
if ( sub(wegt[i], tmp) > 0 ) {
tmp = wegt[i];
}
}
sft = norm_s(tmp);
for ( i = 0; i < M; i++ ) {
wegt[i] = shl(wegt[i], sft); /* wegt in Q(11+sft) */
}
return;
}
/*-----------------------------------------------------*
* Function Autocorr() *
* *
* Compute autocorrelations of signal with windowing *
* *
*-----------------------------------------------------*/
void Autocorr(
Word16 x[], /* (i) : Input signal */
Word16 m, /* (i) : LPC order */
Word16 r_h[], /* (o) : Autocorrelations (msb) */
Word16 r_l[] /* (o) : Autocorrelations (lsb) */
)
{
Word16 i, j, norm;
Word16 y[L_WINDOW];
Word32 sum;
extern Flag Overflow;
/* Windowing of signal */
for(i=0; i<L_WINDOW; i++)
{
y[i] = mult_r(x[i], hamwindow[i]);
}
/* Compute r[0] and test for overflow */
do {
Overflow = 0;
sum = 1; /* Avoid case of all zeros */
for(i=0; i<L_WINDOW; i++)
sum = L_mac(sum, y[i], y[i]);
/* If overflow divide y[] by 4 */
if(Overflow != 0)
{
for(i=0; i<L_WINDOW; i++)
{
y[i] = shr(y[i], 2);
}
}
}while (Overflow != 0);
/* Normalization of r[0] */
norm = norm_l(sum);
sum = L_shl(sum, norm);
L_Extract(sum, &r_h[0], &r_l[0]); /* Put in DPF format (see oper_32b) */
/* r[1] to r[m] */
for (i = 1; i <= m; i++)
{
sum = 0;
for(j=0; j<L_WINDOW-i; j++)
sum = L_mac(sum, y[j], y[j+i]);
sum = L_shl(sum, norm);
L_Extract(sum, &r_h[i], &r_l[i]);
}
return;
}
void Corr_xy2(
Word16 xn[], /* (i) Q0 :Target vector. */
Word16 y1[], /* (i) Q0 :Adaptive codebook. */
Word16 y2[], /* (i) Q12 :Filtered innovative vector. */
Word16 g_coeff[], /* (o) Q[exp]:Correlations between xn,y1,y2 */
Word16 exp_g_coeff[] /* (o) :Q-format of g_coeff[] */
)
{
Word16 i,exp;
Word16 exp_y2y2,exp_xny2,exp_y1y2;
Word16 y2y2, xny2, y1y2;
Word32 L_acc;
Word16 scaled_y2[L_SUBFR]; /* Q9 */
/*------------------------------------------------------------------*
* Scale down y2[] from Q12 to Q9 to avoid overflow *
*------------------------------------------------------------------*/
for(i=0; i<L_SUBFR; i++) {
scaled_y2[i] = shr(y2[i], 3); }
/* Compute scalar product <y2[],y2[]> */
L_acc = 1; /* Avoid case of all zeros */
for(i=0; i<L_SUBFR; i++)
L_acc = L_mac(L_acc, scaled_y2[i], scaled_y2[i]); /* L_acc:Q19 */
exp = norm_l(L_acc);
y2y2 = round( L_shl(L_acc, exp) );
exp_y2y2 = add(exp, 19-16); /* Q[19+exp-16] */
g_coeff[2] = y2y2;
exp_g_coeff[2] = exp_y2y2;
/* Compute scalar product <xn[],y2[]> */
L_acc = 1; /* Avoid case of all zeros */
for(i=0; i<L_SUBFR; i++)
L_acc = L_mac(L_acc, xn[i], scaled_y2[i]); /* L_acc:Q10 */
exp = norm_l(L_acc);
xny2 = round( L_shl(L_acc, exp) );
exp_xny2 = add(exp, 10-16); /* Q[10+exp-16] */
g_coeff[3] = negate(xny2);
exp_g_coeff[3] = sub(exp_xny2,1); /* -2<xn,y2> */
/* Compute scalar product <y1[],y2[]> */
L_acc = 1; /* Avoid case of all zeros */
for(i=0; i<L_SUBFR; i++)
L_acc = L_mac(L_acc, y1[i], scaled_y2[i]); /* L_acc:Q10 */
exp = norm_l(L_acc);
y1y2 = round( L_shl(L_acc, exp) );
exp_y1y2 = add(exp, 10-16); /* Q[10+exp-16] */
g_coeff[4] = y1y2;
exp_g_coeff[4] = sub(exp_y1y2,1); ; /* 2<y1,y2> */
return;
}
/*-------------------------------------------------------------------*
* Function set_sign() *
* ~~~~~~~~~~~~~~~~~~~~ *
* Set the sign of each pulse position. *
*-------------------------------------------------------------------*/
static void set_sign(
Word16 fac_cn, /* (i) Q15: residual weight for sign determination */
Word16 cn[], /* (i) Q0 : residual after long term prediction */
Word16 dn[], /* (i) Q0 : correlation between target and h[] */
Word16 sign[], /* (o) Q15: sign vector (sign of each position) */
Word16 inv_sign[], /* (o) Q15: inverse of sign[] */
Word16 pos_max[], /* (o) : pos of max of correlation */
Word32 corr[] /* (o) : correlation of each track */
)
{
Word16 i, k, pos, k_cn, k_dn, val;
Word32 s, max;
/* calculate energy for normalization of cn[] and dn[] */
s = 0;
for (i=0; i<L_SUBFR; i++) s = L_mac(s, cn[i], cn[i]);
if (s < 512) s = 512;
s = Inv_sqrt(s);
k_cn = extract_h(L_shl(s, 5)); /* k_cn = 11..23170 */
k_cn = mult(k_cn, fac_cn);
s = 0;
for (i=0; i<L_SUBFR; i++) s = L_mac(s, dn[i], dn[i]);
if (s < 512) s = 512;
s = Inv_sqrt(s);
k_dn = extract_h(L_shl(s, 5)); /* k_dn = 11..23170 */
/* set sign according to en[] = k_cn*cn[] + k_dn*dn[] */
/* find position of maximum of correlation in each track */
for (k=0; k<NB_TRACK; k++) {
max = -1;
for (i=k; i<L_SUBFR; i+=STEP) {
val = dn[i];
s = L_mac(L_mult(k_cn, cn[i]), k_dn, val);
if (s >= 0) {
sign[i] = 32767L; /* sign = +1 (Q15) */
inv_sign[i] = -32768L;
}
else {
sign[i] = -32768L; /* sign = -1 (Q15) */
inv_sign[i] = 32767L;
val = negate(val);
}
dn[i] = val; /* modify dn[] according to the fixed sign */
s = L_abs(s);
if (s > max) {
max = s;
pos = i;
}
}
pos_max[k] = pos;
corr[k] = max;
}
return;
}
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;
}
/*-------------------------------------------------------------------*
* Function Qua_Sidgain *
* ~~~~~~~~~~~ *
*-------------------------------------------------------------------*/
void Qua_Sidgain(
Word16 *ener, /* (i) array of energies */
Word16 *sh_ener, /* (i) corresponding scaling factors */
Word16 nb_ener, /* (i) number of energies or */
Word16 *enerq, /* (o) decoded energies in dB */
Word16 *idx /* (o) SID gain quantization index */
)
{
Word16 i;
Word32 L_x;
Word16 sh1, temp;
Word16 hi, lo;
Word32 L_acc;
if(nb_ener == 0) {
/* Quantize energy saved for frame erasure case */
/* L_x = average_ener */
L_acc = L_deposit_l(*ener);
L_acc = L_shl(L_acc, *sh_ener); /* >> if *sh_ener < 0 */
L_Extract(L_acc, &hi, &lo);
L_x = Mpy_32_16(hi, lo, fact[0]);
sh1 = 0;
}
else {
/*
* Compute weighted average of energies
* ener[i] = enerR[i] x 2**sh_ener[i]
* L_x = k[nb_ener] x SUM(i=0->nb_ener-1) enerR[i]
* with k[nb_ener] = fact_ener / nb_ener x L_FRAME x nbAcf
*/
sh1 = sh_ener[0];
for(i=1; i<nb_ener; i++) {
if(sh_ener[i] < sh1) sh1 = sh_ener[i];
}
sh1 = add(sh1, (16-marg[nb_ener]));
L_x = 0L;
for(i=0; i<nb_ener; i++) {
temp = sub(sh1, sh_ener[i]);
L_acc = L_deposit_l(ener[i]);
L_acc = L_shl(L_acc, temp);
L_x = L_add(L_x, L_acc);
}
L_Extract(L_x, &hi, &lo);
L_x = Mpy_32_16(hi, lo, fact[i]);
}
*idx = Quant_Energy(L_x, sh1, enerq);
return;
}
void Post_Process(
Word16 signal[], /* input/output signal */
Word16 lg) /* length of signal */
{
Word16 i, x2;
Word32 L_tmp;
Word16 y2_hi, y2_lo, y1_hi, y1_lo, x0, x1;
y2_hi = pg729dec->ppost_pro->y2_hi;
y2_lo = pg729dec->ppost_pro->y2_lo;
y1_hi = pg729dec->ppost_pro->y1_hi;
y1_lo = pg729dec->ppost_pro->y1_lo;
x0 = pg729dec->ppost_pro->x0;
x1 = pg729dec->ppost_pro->x1;
for(i=0; i<lg; i++)
{
x2 = x1;
x1 = x0;
x0 = signal[i];
/* y[i] = b[0]*x[i] + b[1]*x[i-1] + b[2]*x[i-2] */
/* + a[1]*y[i-1] + a[2] * y[i-2]; */
L_tmp = Mpy_32_16(y1_hi, y1_lo, a100[1]);
L_tmp = L_add(L_tmp, Mpy_32_16(y2_hi, y2_lo, a100[2]));
L_tmp = L_mac(L_tmp, x0, b100[0]);
L_tmp = L_mac(L_tmp, x1, b100[1]);
L_tmp = L_mac(L_tmp, x2, b100[2]);
L_tmp = L_shl(L_tmp, 2); /* Q29 --> Q31 (Q13 --> Q15) */
/* Multiplication by two of output speech with saturation. */
signal[i] = round(L_shl(L_tmp, 1));
y2_hi = y1_hi;
y2_lo = y1_lo;
L_Extract(L_tmp, &y1_hi, &y1_lo);
}
pg729dec->ppost_pro->y2_hi = y2_hi;
pg729dec->ppost_pro->y2_lo = y2_lo;
pg729dec->ppost_pro->y1_hi = y1_hi;
pg729dec->ppost_pro->y1_lo = y1_lo;
pg729dec->ppost_pro->x0 = x0;
pg729dec->ppost_pro->x1 = x1;
return;
}
void rmlt_coefs_to_samples(int16_t coefs[],
int16_t old_samples[],
int16_t out_samples[],
int dct_length,
int16_t mag_shift)
{
int i;
int half_dct_length;
int last;
int16_t new_samples[MAX_DCT_LENGTH];
const int16_t *win;
int32_t sum;
half_dct_length = dct_length >> 1;
/* Perform a Type IV (inverse) DCT on the coefficients */
dct_type_iv_s(coefs, new_samples, dct_length);
if (mag_shift > 0)
{
for (i = 0; i < dct_length; i++)
new_samples[i] = shr(new_samples[i], mag_shift);
}
else if (mag_shift < 0)
{
mag_shift = negate(mag_shift);
for (i = 0; i < dct_length; i++)
new_samples[i] = shl(new_samples[i], mag_shift);
}
win = (dct_length == DCT_LENGTH) ? rmlt_to_samples_window : max_rmlt_to_samples_window;
last = half_dct_length - 1;
for (i = 0; i < half_dct_length; i++)
{
/* Get the first half of the windowed samples */
sum = L_mult(win[i], new_samples[last - i]);
sum = L_mac(sum, win[dct_length - i - 1], old_samples[i]);
out_samples[i] = xround(L_shl(sum, 2));
/* Get the second half of the windowed samples */
sum = L_mult(win[half_dct_length + i], new_samples[i]);
sum = L_mac(sum, negate(win[last - i]), old_samples[last - i]);
out_samples[half_dct_length + i] = xround(L_shl(sum, 2));
}
/* Save the second half of the new samples for
next time, when they will be the old samples. */
vec_copyi16(old_samples, &new_samples[half_dct_length], half_dct_length);
}
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 Pre_Process(
Word16 signal[], /* input/output signal */
Word16 lg) /* length of signal */
{
Word16 i, x2;
Word32 L_tmp;
for(i=0; i<lg; i++)
{
x2 = x1;
x1 = x0;
x0 = signal[i];
/* y[i] = b[0]*x[i]/2 + b[1]*x[i-1]/2 + b140[2]*x[i-2]/2 */
/* + a[1]*y[i-1] + a[2] * y[i-2]; */
L_tmp = Mpy_32_16(y1_hi, y1_lo, a140[1]);
L_tmp = L_add(L_tmp, Mpy_32_16(y2_hi, y2_lo, a140[2]));
L_tmp = L_mac(L_tmp, x0, b140[0]);
L_tmp = L_mac(L_tmp, x1, b140[1]);
L_tmp = L_mac(L_tmp, x2, b140[2]);
L_tmp = L_shl(L_tmp, 3); /* Q28 --> Q31 (Q12 --> Q15) */
signal[i] = round(L_tmp);
y2_hi = y1_hi;
y2_lo = y1_lo;
L_Extract(L_tmp, &y1_hi, &y1_lo);
}
return;
}
Word32 Inv_sqrt( /* (o) Q30 : output value (range: 0<=val<1) */
Word32 L_x /* (i) Q0 : input value (range: 0<=val<=7fffffff) */
)
{
Word16 exp, i, a, tmp;
Word32 L_y;
if( L_x <= (Word32)0) return ( (Word32)0x3fffffffL);
exp = norm_l(L_x);
L_x = L_shl(L_x, exp ); /* L_x is normalize */
exp = sub(30, exp);
if( (exp & 1) == 0 ) /* If exponent even -> shift right */
L_x = L_shr(L_x, 1);
exp = shr(exp, 1);
exp = add(exp, 1);
L_x = L_shr(L_x, 9);
i = extract_h(L_x); /* Extract b25-b31 */
L_x = L_shr(L_x, 1);
a = extract_l(L_x); /* Extract b10-b24 */
a = a & (Word16)0x7fff;
i = sub(i, 16);
L_y = L_deposit_h(tabsqr[i]); /* tabsqr[i] << 16 */
tmp = sub(tabsqr[i], tabsqr[i+1]); /* tabsqr[i] - tabsqr[i+1]) */
L_y = L_msu(L_y, tmp, a); /* L_y -= tmp*a*2 */
L_y = L_shr(L_y, exp); /* denormalization */
return(L_y);
}
long interpolation_cos129( short freq )
{
short sin_data,cos_data,count,temp ;
long Ltemp,Lresult;
/* cos(x)=cos(a)+(x-a)sin(a)-pow((a-x),2)*cos(a) */
count=shr(abs_s(freq ),7 );
temp=sub( extract_l(L_mult( count,64)) , freq );
/* (a-x)sin a */
/* Scale factor for (a-x): 3217=pi2/64 */
sin_data=sin129_table [ count];
cos_data=cos129_table [count];
Ltemp=L_mpy_ls(L_mult(3217,temp),sin_data);
/* (a-x) sin(a) - [(a-x)*(a-x)*cos(a)] /2 */
/* Scale factor for (a-x)*(a-x): 20213=pi2*pi2/64 */
Ltemp=L_sub(Ltemp,
L_mpy_ls(L_mult(mult_r(10106,temp),temp),cos_data));
/* Scaled up by 64/2 times */
Ltemp=L_shl( Ltemp ,6 );
Lresult= L_add(L_deposit_h(cos_data), (Ltemp)) ;
return(Lresult);
}
void
Pre_Process (CodState *coder,
Word16 signal[], /* input/output signal */
Word16 lg)
{ /* length of signal */
Word16 i, x2;
Word32 L_tmp;
for (i = 0; i < lg; i++) {
x2 = coder->x1;
coder->x1 = coder->x0;
coder->x0 = signal[i];
/* y[i] = b[0]*x[i]/2 + b[1]*x[i-1]/2 + b140[2]*x[i-2]/2 */
/* + a[1]*y[i-1] + a[2] * y[i-2]; */
L_tmp = Mpy_32_16 (coder->y1_hi, coder->y1_lo, a140[1]);
L_tmp = L_add (L_tmp, Mpy_32_16 (coder->y2_hi, coder->y2_lo, a140[2]));
L_tmp = L_mac (L_tmp, coder->x0, b140[0]);
L_tmp = L_mac (L_tmp, coder->x1, b140[1]);
L_tmp = L_mac (L_tmp, x2, b140[2]);
L_tmp = L_shl (L_tmp, 3); /* Q28 --> Q31 (Q12 --> Q15) */
signal[i] = wround (L_tmp);
coder->y2_hi = coder->y1_hi;
coder->y2_lo = coder->y1_lo;
L_Extract (L_tmp, &coder->y1_hi, &coder->y1_lo);
}
return;
}
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 WebRtcG729fix_Log2(
int32_t L_x, /* (i) Q0 : input value */
int16_t *exponent, /* (o) Q0 : Integer part of Log2. (range: 0<=val<=30) */
int16_t *fraction /* (o) Q15: Fractional part of Log2. (range: 0<=val<1) */
)
{
int16_t exp, i, a, tmp;
int32_t L_y;
if( L_x <= (int32_t)0 )
{
*exponent = 0;
*fraction = 0;
return;
}
exp = WebRtcSpl_NormW32(L_x);
L_x = L_shl(L_x, exp ); /* L_x is normalized */
*exponent = WebRtcSpl_SubSatW16(30, exp);
L_x = L_shr(L_x, 9);
i = extract_h(L_x); /* Extract b25-b31 */
L_x = L_shr(L_x, 1);
a = extract_l(L_x); /* Extract b10-b24 of fraction */
a = a & (int16_t)0x7fff;
i = WebRtcSpl_SubSatW16(i, 32);
L_y = L_deposit_h(WebRtcG729fix_tablog[i]); /* tablog[i] << 16 */
tmp = WebRtcSpl_SubSatW16(WebRtcG729fix_tablog[i], WebRtcG729fix_tablog[i+1]); /* tablog[i] - tablog[i+1] */
L_y = L_msu(L_y, tmp, a); /* L_y -= tmp*a*2 */
*fraction = extract_h( L_y);
}
int32_t WebRtcG729fix_Inv_sqrt( /* (o) Q30 : output value (range: 0<=val<1) */
int32_t L_x /* (i) Q0 : input value (range: 0<=val<=7fffffff) */
)
{
int16_t exp, i, a, tmp;
int32_t L_y;
if( L_x <= (int32_t)0) return ( (int32_t)0x3fffffffL);
exp = WebRtcSpl_NormW32(L_x);
L_x = L_shl(L_x, exp ); /* L_x is normalize */
exp = WebRtcSpl_SubSatW16(30, exp);
if( (exp & 1) == 0 ) /* If exponent even -> shift right */
L_x = L_shr(L_x, 1);
exp = shr(exp, 1);
exp = WebRtcSpl_AddSatW16(exp, 1);
L_x = L_shr(L_x, 9);
i = extract_h(L_x); /* Extract b25-b31 */
L_x = L_shr(L_x, 1);
a = extract_l(L_x); /* Extract b10-b24 */
a = a & (int16_t)0x7fff;
i = WebRtcSpl_SubSatW16(i, 16);
L_y = L_deposit_h(WebRtcG729fix_tabsqr[i]); /* tabsqr[i] << 16 */
tmp = WebRtcSpl_SubSatW16(WebRtcG729fix_tabsqr[i], WebRtcG729fix_tabsqr[i+1]); /* tabsqr[i] - tabsqr[i+1]) */
L_y = L_msu(L_y, tmp, a); /* L_y -= tmp*a*2 */
L_y = L_shr(L_y, exp); /* denormalization */
return(L_y);
}
/*
extract elementary LSP from composed LSP with previous LSP
*/
void Lsp_prev_extract(
Word16 lsp[M], /* (i) Q13 : unquantized LSP parameters */
Word16 lsp_ele[M], /* (o) Q13 : target vector */
Word16 fg[MA_NP][M], /* (i) Q15 : MA prediction coef. */
Word16 freq_prev[MA_NP][M], /* (i) Q13 : previous LSP vector */
Word16 fg_sum_inv[M] /* (i) Q12 : inverse previous LSP vector */
)
{
Word16 j, k;
Word32 L_temp; /* Q19 */
Word16 temp; /* Q13 */
for ( j = 0 ; j < M ; j++ ) {
L_temp = L_deposit_h(lsp[j]);
for ( k = 0 ; k < MA_NP ; k++ )
L_temp = L_msu( L_temp, freq_prev[k][j], fg[k][j] );
temp = extract_h(L_temp);
L_temp = L_mult( temp, fg_sum_inv[j] );
lsp_ele[j] = extract_h( L_shl( L_temp, 3 ) );
}
return;
}
Word32 fnExp10(Word32 L_Input)
{
/*_________________________________________________________________________
| |
| Local Static Variables |
|_________________________________________________________________________|
*/
static Word16
InvScale = 27213; /* (1/log10(2))/4 */
/*_________________________________________________________________________
| |
| Automatic Variables |
|_________________________________________________________________________|
*/
Word32 LwIn;
/*_________________________________________________________________________
| |
| Executable Code |
|_________________________________________________________________________|
*/
LwIn = L_mpy_ls(L_Input, InvScale);
LwIn = L_shl(LwIn, 2);
LwIn = fnExp2(LwIn);
/* return result */
/* ------------- */
return (LwIn);
}
void Lsp_get_tdist(
Word16 wegt[], /* (i) norm: weight coef. */
Word16 buf[], /* (i) Q13 : candidate LSP vector */
Word32 *L_tdist, /* (o) Q27 : distortion */
Word16 rbuf[], /* (i) Q13 : target vector */
Word16 fg_sum[] /* (i) Q15 : present MA prediction coef. */
)
{
Word16 j;
Word16 tmp, tmp2; /* Q13 */
Word32 L_acc; /* Q25 */
*L_tdist = 0;
for ( j = 0 ; j < M ; j++ ) {
/* tmp = (buf - rbuf)*fg_sum */
tmp = sub( buf[j], rbuf[j] );
tmp = mult( tmp, fg_sum[j] );
/* *L_tdist += wegt * tmp * tmp */
L_acc = L_mult( wegt[j], tmp );
tmp2 = extract_h( L_shl( L_acc, 4 ) );
*L_tdist = L_mac( *L_tdist, tmp2, tmp );
}
return;
}
void Bitpack(
INT16 in,
UINT16 *TrWords,
INT16 NoOfBits,
INT16 *ptr
)
{
INT16 temp;
UINT16 *WordPtr;
WordPtr = TrWords + ptr[1];
*ptr = sub(*ptr, NoOfBits);
if (*ptr >= 0)
{
/* NOTE: Creating *WordPtr requires an unsigned shift. There isn't a fixed
* point prototype for an unsigned shift, so I kept the C version. */
*WordPtr = *WordPtr | (in << *ptr);
}
else
{
temp = shr(in, negate(*ptr));
*WordPtr = *WordPtr | temp;
WordPtr++;
*ptr = add(*ptr, 16);
*WordPtr = (INT16) ((INT32) (L_shl((INT32) in, *ptr)) & 0xffff);
ptr[1] += 1;
}
}
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;
}
void Lsp_lsf(
Word16 lsp[], /* (i) Q15 : lsp[m] (range: -1<=val<1) */
Word16 lsf[], /* (o) Q15 : lsf[m] normalized (range: 0.0<=val<=0.5) */
Word16 m /* (i) : LPC order */
)
{
Word16 i, ind, tmp;
Word32 L_tmp;
ind = 63; /* begin at end of table -1 */
for(i= m-(Word16)1; i >= 0; i--)
{
/* find value in table that is just greater than lsp[i] */
while( sub(table[ind], lsp[i]) < 0 )
{
ind = sub(ind,1);
}
/* acos(lsp[i])= ind*256 + ( ( lsp[i]-table[ind] ) * slope[ind] )/4096 */
L_tmp = L_mult( sub(lsp[i], table[ind]) , slope[ind] );
tmp = round(L_shl(L_tmp, 3)); /*(lsp[i]-table[ind])*slope[ind])>>12*/
lsf[i] = add(tmp, shl(ind, 8));
}
return;
}
void azfilter(
Word16 a[], /* (i) Q12 : prediction coefficients */
Word16 m, /* (i) : LPC order */
Word16 x[], /* (i) Q0 : input signal samples, incl. past */
Word16 y[], /* (o) Q0 : filtered output signal */
Word16 lg /* (i) : size of filtering */
)
{
Word16 i, n;
Word32 a0;
Word16 *fp1;
/* loop through every element of the current vector */
for (n = 0; n < lg; n++) {
/* perform multiply-adds along the delay line of filter */
fp1 = x + n;
a0 = L_mult0(a[0], *fp1--); // Q12
for (i = 1; i <= m; i++)
a0 = L_mac0(a0, a[i], *fp1--); // Q12
/* get the output with rounding */
y[n] = intround(L_shl(a0, 4)); // Q0
}
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 */
}
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;
}