void w_Lsf_wt(int16_t * lsf, /* input : LSF vector */
int16_t * wf)
{ /* output: square of weighting factors */
int16_t temp;
int16_t i;
/* wf[0] = lsf[1] - 0 */
wf[0] = lsf[1];
for (i = 1; i < 9; i++) {
wf[i] = w_sub(lsf[i + 1], lsf[i - 1]);
}
/* wf[9] = 0.5 - lsf[8] */
wf[9] = w_sub(16384, lsf[8]);
for (i = 0; i < 10; i++) {
temp = w_sub(wf[i], 1843);
if (temp < 0) {
wf[i] = w_sub(3427, w_mult(wf[i], 28160));
} else {
wf[i] = w_sub(1843, w_mult(temp, 6242));
}
wf[i] = w_shl(wf[i], 3);
}
return;
}
void w_cor_h(int16_t h[], /* (i) : impulse response of weighted w_w_synthesis
filter */
int16_t sign[], /* (i) : sign of d[n] */
int16_t rr[][L_CODE] /* (o) : matrix of autocorrelation */
)
{
int16_t i, j, k, dec, h2[L_CODE];
int32_t s;
/* Scaling for maximum precision */
s = 2;
for (i = 0; i < L_CODE; i++)
s = w_L_mac(s, h[i], h[i]);
j = w_sub(w_extract_h(s), 32767);
if (j == 0) {
for (i = 0; i < L_CODE; i++) {
h2[i] = w_shr(h[i], 1);
}
} else {
s = w_L_w_shr(s, 1);
k = w_extract_h(w_L_w_shl(w_Inv_sqrt(s), 7));
k = w_mult(k, 32440); /* k = 0.99*k */
for (i = 0; i < L_CODE; i++) {
h2[i] = w_round(w_L_w_shl(w_L_w_mult(h[i], k), 9));
}
}
/* build matrix rr[] */
s = 0;
i = L_CODE - 1;
for (k = 0; k < L_CODE; k++, i--) {
s = w_L_mac(s, h2[k], h2[k]);
rr[i][i] = w_round(s);
}
for (dec = 1; dec < L_CODE; dec++) {
s = 0;
j = L_CODE - 1;
i = w_sub(j, dec);
for (k = 0; k < (L_CODE - dec); k++, i--, j--) {
s = w_L_mac(s, h2[k], h2[k + dec]);
rr[j][i] = w_mult(w_round(s), w_mult(sign[i], sign[j]));
rr[i][j] = rr[j][i];
}
}
}
int16_t Vq_w_subvec(int16_t * lsf_r1, /* input : 1st LSF residual vector */
int16_t * lsf_r2, /* input : and LSF residual vector */
const int16_t * dico, /* input : quantization codebook */
int16_t * wf1, /* input : 1st LSF weighting factors */
int16_t * wf2, /* input : 2nd LSF weighting factors */
int16_t dico_size /* input : size of quantization codebook */
)
{
int16_t i, index = 0, temp = 0;
const int16_t *p_dico;
int32_t dist_min, dist;
dist_min = MAX_32;
p_dico = dico;
for (i = 0; i < dico_size; i++) {
temp = w_sub(lsf_r1[0], *p_dico++);
temp = w_mult(wf1[0], temp);
dist = w_L_w_mult(temp, temp);
temp = w_sub(lsf_r1[1], *p_dico++);
temp = w_mult(wf1[1], temp);
dist = w_L_mac(dist, temp, temp);
temp = w_sub(lsf_r2[0], *p_dico++);
temp = w_mult(wf2[0], temp);
dist = w_L_mac(dist, temp, temp);
temp = w_sub(lsf_r2[1], *p_dico++);
temp = w_mult(wf2[1], temp);
dist = w_L_mac(dist, temp, temp);
if (w_L_w_sub(dist, dist_min) < (int32_t) 0) {
dist_min = dist;
index = i;
}
}
/* Reading the selected vector */
p_dico = &dico[w_shl(index, 2)];
lsf_r1[0] = *p_dico++;
lsf_r1[1] = *p_dico++;
lsf_r2[0] = *p_dico++;
lsf_r2[1] = *p_dico++;
return index;
}
/*************************************************************************
*
* FUNCTION: er_w_cor_h_x()
*
* PURPOSE: Computes correlation between target signal "x[]" and
* impulse response"h[]".
*
* DESCRIPTION:
* The correlation is given by:
* d[n] = sum_{i=n}^{L-1} x[i] h[i-n] n=0,...,L-1
*
* d[n] is normalized such that the sum of 5 maxima of d[n] corresponding
* to each position track does not w_saturate.
*
*************************************************************************/
void er_w_cor_h_x(int16_t h[], /* (i) : impulse response of weighted w_w_synthesis filter */
int16_t x[], /* (i) : target */
int16_t dn[] /* (o) : correlation between target and h[] */
)
{
int16_t i, j, k;
int32_t s, y32[L_CODE], max, tot;
/* 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_CODE; i += STEP) {
s = 0;
for (j = i; j < L_CODE; j++)
s = w_L_mac(s, x[j], h[j - i]);
y32[i] = s;
s = w_L_abs(s);
if (w_L_w_sub(s, max) > (int32_t) 0L)
max = s;
}
tot = L_w_add(tot, w_L_w_shr(max, 1));
}
j = w_sub(w_norm_l(tot), 2); /* w_multiply tot by 4 */
for (i = 0; i < L_CODE; i++) {
dn[i] = w_round(w_L_w_shl(y32[i], j));
}
}
void w_dec_10i40_35bits(int16_t index[], /* (i) : index of 10 pulses (sign+position) */
int16_t cod[] /* (o) : algebraic (fixed) codebook w_excitation */
)
{
static const int16_t dgray[8] = { 0, 1, 3, 2, 5, 6, 4, 7 };
int16_t i, j, pos1, pos2, sign, tmp;
for (i = 0; i < L_CODE; i++) {
cod[i] = 0;
}
/* decode the positions and signs of pulses and build the codeword */
for (j = 0; j < NB_TRACK; j++) {
/* compute index i */
tmp = index[j];
i = tmp & 7;
i = dgray[i];
i = w_extract_l(w_L_w_shr(w_L_w_mult(i, 5), 1));
pos1 = w_add(i, j); /* position of pulse "j" */
i = w_shr(tmp, 3) & 1;
if (i == 0) {
sign = 4096; /* +1.0 */
} else {
sign = -4096; /* -1.0 */
}
cod[pos1] = sign;
/* compute index i */
i = index[w_add(j, 5)] & 7;
i = dgray[i];
i = w_extract_l(w_L_w_shr(w_L_w_mult(i, 5), 1));
pos2 = w_add(i, j); /* position of pulse "j+5" */
if (w_sub(pos2, pos1) < 0) {
sign = w_negate(sign);
}
cod[pos2] = w_add(cod[pos2], sign);
}
return;
}
void w_q_p(int16_t * ind, /* Pulse position */
int16_t n /* Pulse number */
)
{
static const int16_t gray[8] = { 0, 1, 3, 2, 6, 4, 5, 7 };
int16_t tmp;
tmp = *ind;
if (w_sub(n, 5) < 0) {
tmp = (tmp & 0x8) | gray[tmp & 0x7];
} else {
tmp = gray[tmp & 0x7];
}
*ind = tmp;
}
inline
Eigen::Matrix<typename boost::math::tools::promote_args<T1, T2>::type,
Eigen::Dynamic, 1>
csr_matrix_times_vector(const int& m,
const int& n,
const Eigen::Matrix<T1, Eigen::Dynamic, 1>& w,
const std::vector<int>& v,
const std::vector<int>& u,
const Eigen::Matrix<T2, Eigen::Dynamic, 1>& b) {
typedef typename boost::math::tools::promote_args<T1, T2>::type
result_t;
check_positive("csr_matrix_times_vector", "m", m);
check_positive("csr_matrix_times_vector", "n", n);
check_size_match("csr_matrix_times_vector", "n", n, "b", b.size());
check_size_match("csr_matrix_times_vector", "m", m, "u", u.size() - 1);
check_size_match("csr_matrix_times_vector", "w", w.size(), "v", v.size());
check_size_match("csr_matrix_times_vector", "u/z",
u[m - 1] + csr_u_to_z(u, m - 1) - 1, "v", v.size());
for (unsigned int i = 0; i < v.size(); ++i)
check_range("csr_matrix_times_vector", "v[]", n, v[i]);
Eigen::Matrix<result_t, Eigen::Dynamic, 1> result(m);
result.setZero();
for (int row = 0; row < m; ++row) {
int idx = csr_u_to_z(u, row);
int row_end_in_w = (u[row] - stan::error_index::value) + idx;
int i = 0; // index into dot-product segment entries.
Eigen::Matrix<result_t, Eigen::Dynamic, 1> b_sub(idx);
b_sub.setZero();
for (int nze = u[row] - stan::error_index::value;
nze < row_end_in_w; ++nze, ++i) {
check_range("csr_matrix_times_vector", "j", n, v[nze]);
b_sub.coeffRef(i) = b.coeffRef(v[nze] - stan::error_index::value);
} // loop skipped when z is zero.
Eigen::Matrix<T1, Eigen::Dynamic, 1>
w_sub(w.segment(u[row] - stan::error_index::value, idx));
result.coeffRef(row) = dot_product(w_sub, b_sub);
}
return result;
}
int16_t w_Pitch_ol(int16_t signal[], /* input : signal used to compute the open loop pitch */
/* signal[-pit_max] to signal[-1] should be known */
int16_t pit_min, /* input : minimum pitch lag */
int16_t pit_max, /* input : maximum pitch lag */
int16_t L_frame /* input : length of frame to compute pitch */
)
{
int16_t i, j;
int16_t max1, max2, max3;
int16_t p_max1, p_max2, p_max3;
int32_t t0;
/* Scaled signal */
/* Can be allocated with memory allocation of(pit_max+L_frame) */
int16_t scaled_signal[512];
int16_t *scal_sig, scal_fac;
scal_sig = &scaled_signal[pit_max];
t0 = 0L;
for (i = -pit_max; i < L_frame; i++) {
t0 = w_L_mac(t0, signal[i], signal[i]);
}
/*--------------------------------------------------------*
* Scaling of input signal. *
* *
* if w_Overflow -> scal_sig[i] = signal[i]>>2 *
* else if t0 < 1^22 -> scal_sig[i] = signal[i]<<2 *
* else -> scal_sig[i] = signal[i] *
*--------------------------------------------------------*/
/*--------------------------------------------------------*
* Verification for risk of overflow. *
*--------------------------------------------------------*/
if (w_L_w_sub(t0, MAX_32) == 0L) { /* Test for overflow */
for (i = -pit_max; i < L_frame; i++) {
scal_sig[i] = w_shr(signal[i], 3);
}
scal_fac = 3;
} else if (w_L_w_sub(t0, (int32_t) 1048576L) < (int32_t) 0)
/* if (t0 < 2^20) */
{
for (i = -pit_max; i < L_frame; i++) {
scal_sig[i] = w_shl(signal[i], 3);
}
scal_fac = -3;
} else {
for (i = -pit_max; i < L_frame; i++) {
scal_sig[i] = signal[i];
}
scal_fac = 0;
}
/*--------------------------------------------------------------------*
* The pitch lag search is divided in three sections. *
* Each section cannot have a pitch w_multiple. *
* We find a maximum for each section. *
* We compare the maximum of each section by favoring small lags. *
* *
* 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 = w_shl(pit_min, 2);
p_max1 = w_Lag_max(scal_sig, scal_fac, L_frame, pit_max, j, &max1);
i = w_sub(j, 1);
j = w_shl(pit_min, 1);
p_max2 = w_Lag_max(scal_sig, scal_fac, L_frame, i, j, &max2);
i = w_sub(j, 1);
p_max3 = w_Lag_max(scal_sig, scal_fac, L_frame, i, pit_min, &max3);
/*--------------------------------------------------------------------*
* Compare the 3 sections maximum, and favor small lag. *
*-------------------------------------------------------------------*/
if (w_sub(w_mult(max1, THRESHOLD), max2) < 0) {
max1 = max2;
p_max1 = p_max2;
}
if (w_sub(w_mult(max1, THRESHOLD), max3) < 0) {
p_max1 = p_max3;
}
return (p_max1);
}
void w_Q_plsf_5(int16_t * lsp1, /* input : 1st LSP vector */
int16_t * lsp2, /* input : 2nd LSP vector */
int16_t * lsp1_q, /* output: quantized 1st LSP vector */
int16_t * lsp2_q, /* output: quantized 2nd LSP vector */
int16_t * indice, /* output: quantization indices of 5 matrices */
int16_t w_txdtx_ctrl /* input : tx dtx control word */
)
{
int16_t i;
int16_t lsf1[M], lsf2[M], wf1[M], wf2[M], lsf_p[M], lsf_r1[M], lsf_r2[M];
int16_t lsf1_q[M], lsf2_q[M];
int16_t lsf_aver[M];
static int16_t w_lsf_p_CN[M];
/* convert LSFs to normalize frequency domain 0..16384 */
w_Lsp_lsf(lsp1, lsf1, M);
w_Lsp_lsf(lsp2, lsf2, M);
/* Update LSF CN quantizer "memory" */
if ((w_txdtx_ctrl & TX_SP_FLAG) == 0
&& (w_txdtx_ctrl & TX_PREV_HANGOVER_ACTIVE) != 0) {
update_w_lsf_p_CN(w_lsf_old_tx, w_lsf_p_CN);
}
if ((w_txdtx_ctrl & TX_SID_UPDATE) != 0) {
/* New SID frame is to be sent:
Compute average of the current LSFs and the LSFs in the history */
w_aver_lsf_history(w_lsf_old_tx, lsf1, lsf2, lsf_aver);
}
/* Update LSF history with unquantized LSFs when no w_speech activity
is present */
if ((w_txdtx_ctrl & TX_SP_FLAG) == 0) {
w_update_lsf_history(lsf1, lsf2, w_lsf_old_tx);
}
if ((w_txdtx_ctrl & TX_SID_UPDATE) != 0) {
/* Compute LSF weighting factors for lsf2, using averaged LSFs */
/* Set LSF weighting factors for lsf1 to w_zero */
/* Replace lsf1 and lsf2 by the averaged LSFs */
w_Lsf_wt(lsf_aver, wf2);
for (i = 0; i < M; i++) {
wf1[i] = 0;
lsf1[i] = lsf_aver[i];
lsf2[i] = lsf_aver[i];
}
} else {
/* Compute LSF weighting factors */
w_Lsf_wt(lsf1, wf1);
w_Lsf_wt(lsf2, wf2);
}
/* Compute w_predicted LSF and w_prediction w_error */
if ((w_txdtx_ctrl & TX_SP_FLAG) != 0) {
for (i = 0; i < M; i++) {
lsf_p[i] =
w_add(w_mean_lsf[i],
w_mult(w_past_r2_q[i], PRED_FAC));
lsf_r1[i] = w_sub(lsf1[i], lsf_p[i]);
lsf_r2[i] = w_sub(lsf2[i], lsf_p[i]);
}
} else {
for (i = 0; i < M; i++) {
lsf_r1[i] = w_sub(lsf1[i], w_lsf_p_CN[i]);
lsf_r2[i] = w_sub(lsf2[i], w_lsf_p_CN[i]);
}
}
/*---- Split-VQ of w_prediction w_error ----*/
indice[0] = Vq_w_subvec(&lsf_r1[0], &lsf_r2[0], w_dico1_lsf,
&wf1[0], &wf2[0], DICO1_SIZE);
indice[1] = Vq_w_subvec(&lsf_r1[2], &lsf_r2[2], w_dico2_lsf,
&wf1[2], &wf2[2], DICO2_SIZE);
indice[2] = Vq_w_subvec_s(&lsf_r1[4], &lsf_r2[4], w_dico3_lsf,
&wf1[4], &wf2[4], DICO3_SIZE);
indice[3] = Vq_w_subvec(&lsf_r1[6], &lsf_r2[6], w_dico4_lsf,
&wf1[6], &wf2[6], DICO4_SIZE);
indice[4] = Vq_w_subvec(&lsf_r1[8], &lsf_r2[8], w_dico5_lsf,
&wf1[8], &wf2[8], DICO5_SIZE);
/* Compute quantized LSFs and update the past quantized residual */
/* In case of no w_speech activity, skip computing the quantized LSFs,
and set w_past_r2_q to w_zero (initial value) */
if ((w_txdtx_ctrl & TX_SP_FLAG) != 0) {
for (i = 0; i < M; i++) {
lsf1_q[i] = w_add(lsf_r1[i], lsf_p[i]);
lsf2_q[i] = w_add(lsf_r2[i], lsf_p[i]);
w_past_r2_q[i] = lsf_r2[i];
}
/* verification that LSFs has minimum distance of LSF_GAP */
w_Reorder_lsf(lsf1_q, LSF_GAP, M);
w_Reorder_lsf(lsf2_q, LSF_GAP, M);
/* Update LSF history with quantized LSFs
when hangover period is active */
if ((w_txdtx_ctrl & TX_HANGOVER_ACTIVE) != 0) {
w_update_lsf_history(lsf1_q, lsf2_q, w_lsf_old_tx);
}
/* convert LSFs to the cosine domain */
w_Lsf_lsp(lsf1_q, lsp1_q, M);
w_Lsf_lsp(lsf2_q, lsp2_q, M);
} else {
for (i = 0; i < M; i++) {
w_past_r2_q[i] = 0;
}
}
return;
}
int16_t Vq_w_subvec_s(int16_t * lsf_r1, /* input : 1st LSF residual vector */
int16_t * lsf_r2, /* input : and LSF residual vector */
const int16_t * dico, /* input : quantization codebook */
int16_t * wf1, /* input : 1st LSF weighting factors */
int16_t * wf2, /* input : 2nd LSF weighting factors */
int16_t dico_size)
{ /* input : size of quantization codebook */
int16_t i, index = 0, sign = 0, temp = 0;
const int16_t *p_dico;
int32_t dist_min, dist;
dist_min = MAX_32;
p_dico = dico;
for (i = 0; i < dico_size; i++) {
/* w_test positive */
temp = w_sub(lsf_r1[0], *p_dico++);
temp = w_mult(wf1[0], temp);
dist = w_L_w_mult(temp, temp);
temp = w_sub(lsf_r1[1], *p_dico++);
temp = w_mult(wf1[1], temp);
dist = w_L_mac(dist, temp, temp);
temp = w_sub(lsf_r2[0], *p_dico++);
temp = w_mult(wf2[0], temp);
dist = w_L_mac(dist, temp, temp);
temp = w_sub(lsf_r2[1], *p_dico++);
temp = w_mult(wf2[1], temp);
dist = w_L_mac(dist, temp, temp);
if (w_L_w_sub(dist, dist_min) < (int32_t) 0) {
dist_min = dist;
index = i;
sign = 0;
}
/* w_test negative */
p_dico -= 4;
temp = w_add(lsf_r1[0], *p_dico++);
temp = w_mult(wf1[0], temp);
dist = w_L_w_mult(temp, temp);
temp = w_add(lsf_r1[1], *p_dico++);
temp = w_mult(wf1[1], temp);
dist = w_L_mac(dist, temp, temp);
temp = w_add(lsf_r2[0], *p_dico++);
temp = w_mult(wf2[0], temp);
dist = w_L_mac(dist, temp, temp);
temp = w_add(lsf_r2[1], *p_dico++);
temp = w_mult(wf2[1], temp);
dist = w_L_mac(dist, temp, temp);
if (w_L_w_sub(dist, dist_min) < (int32_t) 0) {
dist_min = dist;
index = i;
sign = 1;
}
}
/* Reading the selected vector */
p_dico = &dico[w_shl(index, 2)];
if (sign == 0) {
lsf_r1[0] = *p_dico++;
lsf_r1[1] = *p_dico++;
lsf_r2[0] = *p_dico++;
lsf_r2[1] = *p_dico++;
} else {
lsf_r1[0] = w_negate(*p_dico++);
lsf_r1[1] = w_negate(*p_dico++);
lsf_r2[0] = w_negate(*p_dico++);
lsf_r2[1] = w_negate(*p_dico++);
}
index = w_shl(index, 1);
index = w_add(index, sign);
return index;
}
void w_D_plsf_5(int16_t * indice, /* input : quantization indices of 5 w_submatrices */
int16_t * lsp1_q, /* output: quantized 1st LSP vector */
int16_t * lsp2_q, /* output: quantized 2nd LSP vector */
int16_t bfi, /* input : bad frame indicator (set to 1 if a bad
frame is received) */
int16_t w_rxdtx_ctrl, /* input : RX DTX control word */
int16_t w_w_rx_dtx_w_state /* input : w_state of the comfort noise insertion
period */
)
{
int16_t i;
const int16_t *p_dico;
int16_t temp, sign;
int16_t lsf1_r[M], lsf2_r[M];
int16_t lsf1_q[M], lsf2_q[M];
/* Update comfort noise LSF quantizer memory */
if ((w_rxdtx_ctrl & RX_UPD_SID_QUANT_MEM) != 0) {
update_w_lsf_p_CN(w_lsf_old_rx, w_lsf_p_CN);
}
/* Handle cases of comfort noise LSF decoding in which past
valid SID frames are repeated */
if (((w_rxdtx_ctrl & RX_NO_TRANSMISSION) != 0)
|| ((w_rxdtx_ctrl & RX_INVALID_SID_FRAME) != 0)
|| ((w_rxdtx_ctrl & RX_LOST_SID_FRAME) != 0)) {
if ((w_rxdtx_ctrl & RX_NO_TRANSMISSION) != 0) {
/* DTX active: no transmission. Interpolate LSF values in memory */
w_interpolate_CN_lsf(w_lsf_old_CN, w_lsf_new_CN, lsf2_q,
w_w_rx_dtx_w_state);
} else { /* Invalid or lost SID frame: use LSFs
from last good SID frame */
for (i = 0; i < M; i++) {
w_lsf_old_CN[i] = w_lsf_new_CN[i];
lsf2_q[i] = w_lsf_new_CN[i];
v_past_r2_q[i] = 0;
}
}
for (i = 0; i < M; i++) {
w_past_lsf_q[i] = lsf2_q[i];
}
/* convert LSFs to the cosine domain */
w_Lsf_lsp(lsf2_q, lsp2_q, M);
return;
}
if (bfi != 0) { /* if bad frame */
/* use the past LSFs slightly shifted towards their mean */
for (i = 0; i < M; i++) {
/* lsfi_q[i] = ALPHA*w_past_lsf_q[i] + ONE_ALPHA*w_mean_lsf[i]; */
lsf1_q[i] = w_add(w_mult(w_past_lsf_q[i], ALPHA),
w_mult(w_mean_lsf[i], ONE_ALPHA));
lsf2_q[i] = lsf1_q[i];
}
/* estimate past quantized residual to be used in next frame */
for (i = 0; i < M; i++) {
/* temp = w_mean_lsf[i] + v_past_r2_q[i] * PRED_FAC; */
temp =
w_add(w_mean_lsf[i],
w_mult(v_past_r2_q[i], PRED_FAC));
v_past_r2_q[i] = w_sub(lsf2_q[i], temp);
}
} else
/* if good LSFs received */
{
/* decode w_prediction residuals from 5 received indices */
p_dico = &w_dico1_lsf[w_shl(indice[0], 2)];
lsf1_r[0] = *p_dico++;
lsf1_r[1] = *p_dico++;
lsf2_r[0] = *p_dico++;
lsf2_r[1] = *p_dico;
p_dico = &w_dico2_lsf[w_shl(indice[1], 2)];
lsf1_r[2] = *p_dico++;
lsf1_r[3] = *p_dico++;
lsf2_r[2] = *p_dico++;
lsf2_r[3] = *p_dico;
sign = indice[2] & 1;
i = w_shr(indice[2], 1);
p_dico = &w_dico3_lsf[w_shl(i, 2)];
if (sign == 0) {
lsf1_r[4] = *p_dico++;
lsf1_r[5] = *p_dico++;
lsf2_r[4] = *p_dico++;
lsf2_r[5] = *p_dico++;
} else {
lsf1_r[4] = w_negate(*p_dico++);
lsf1_r[5] = w_negate(*p_dico++);
lsf2_r[4] = w_negate(*p_dico++);
lsf2_r[5] = w_negate(*p_dico++);
}
p_dico = &w_dico4_lsf[w_shl(indice[3], 2)];
lsf1_r[6] = *p_dico++;
lsf1_r[7] = *p_dico++;
lsf2_r[6] = *p_dico++;
lsf2_r[7] = *p_dico;
p_dico = &w_dico5_lsf[w_shl(indice[4], 2)];
lsf1_r[8] = *p_dico++;
lsf1_r[9] = *p_dico++;
lsf2_r[8] = *p_dico++;
lsf2_r[9] = *p_dico++;
/* Compute quantized LSFs and update the past quantized residual */
/* Use w_lsf_p_CN as w_predicted LSF vector in case of no w_speech
activity */
if ((w_rxdtx_ctrl & RX_SP_FLAG) != 0) {
for (i = 0; i < M; i++) {
temp =
w_add(w_mean_lsf[i],
w_mult(v_past_r2_q[i], PRED_FAC));
lsf1_q[i] = w_add(lsf1_r[i], temp);
lsf2_q[i] = w_add(lsf2_r[i], temp);
v_past_r2_q[i] = lsf2_r[i];
}
} else { /* Valid SID frame */
for (i = 0; i < M; i++) {
lsf2_q[i] = w_add(lsf2_r[i], w_lsf_p_CN[i]);
/* Use the dequantized values of lsf2 also for lsf1 */
lsf1_q[i] = lsf2_q[i];
v_past_r2_q[i] = 0;
}
}
}
/* verification that LSFs have minimum distance of LSF_GAP Hz */
w_Reorder_lsf(lsf1_q, LSF_GAP, M);
w_Reorder_lsf(lsf2_q, LSF_GAP, M);
if ((w_rxdtx_ctrl & RX_FIRST_SID_UPDATE) != 0) {
for (i = 0; i < M; i++) {
w_lsf_new_CN[i] = lsf2_q[i];
}
}
if ((w_rxdtx_ctrl & RX_CONT_SID_UPDATE) != 0) {
for (i = 0; i < M; i++) {
w_lsf_old_CN[i] = w_lsf_new_CN[i];
w_lsf_new_CN[i] = lsf2_q[i];
}
}
if ((w_rxdtx_ctrl & RX_SP_FLAG) != 0) {
/* Update lsf history with quantized LSFs
when w_speech activity is present. If the current frame is
a bad one, update with most recent good comfort noise LSFs */
if (bfi == 0) {
w_update_lsf_history(lsf1_q, lsf2_q, w_lsf_old_rx);
} else {
w_update_lsf_history(w_lsf_new_CN, w_lsf_new_CN,
w_lsf_old_rx);
}
for (i = 0; i < M; i++) {
w_lsf_old_CN[i] = lsf2_q[i];
}
} else {
w_interpolate_CN_lsf(w_lsf_old_CN, w_lsf_new_CN, lsf2_q,
w_w_rx_dtx_w_state);
}
for (i = 0; i < M; i++) {
w_past_lsf_q[i] = lsf2_q[i];
}
/* convert LSFs to the cosine domain */
w_Lsf_lsp(lsf1_q, lsp1_q, M);
w_Lsf_lsp(lsf2_q, lsp2_q, M);
return;
}
int16_t w_Autocorr(int16_t x[], /* (i) : Input signal */
int16_t m, /* (i) : LPC order */
int16_t r_h[], /* (o) : w_Autocorrelations (msb) */
int16_t r_l[], /* (o) : w_Autocorrelations (lsb) */
int16_t wind[] /* (i) : window for LPC analysis */
)
{
int16_t i, j, norm;
int16_t y[L_WINDOW];
int32_t sum;
int16_t overfl, overfl_shft;
/* Windowing of signal */
for (i = 0; i < L_WINDOW; i++) {
y[i] = w_w_mult_r(x[i], wind[i]);
}
/* Compute r[0] and w_test for overflow */
overfl_shft = 0;
do {
overfl = 0;
sum = 0L;
for (i = 0; i < L_WINDOW; i++) {
sum = w_L_mac(sum, y[i], y[i]);
}
/* If overflow divide y[] by 4 */
if (w_L_w_sub(sum, MAX_32) == 0L) {
overfl_shft = w_add(overfl_shft, 4);
overfl = 1; /* Set the overflow flag */
for (i = 0; i < L_WINDOW; i++) {
y[i] = w_shr(y[i], 2);
}
}
}
while (overfl != 0);
sum = L_w_add(sum, 1L); /* Avoid the case of all w_zeros */
/* Normalization of r[0] */
norm = w_norm_l(sum);
sum = w_L_w_shl(sum, norm);
w_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 = w_L_mac(sum, y[j], y[j + i]);
}
sum = w_L_w_shl(sum, norm);
w_L_Extract(sum, &r_h[i], &r_l[i]);
}
norm = w_sub(norm, overfl_shft);
return norm;
}
void w_build_code(int16_t codvec[], /* (i) : position of pulses */
int16_t sign[], /* (i) : sign of d[n] */
int16_t cod[], /* (o) : innovative code vector */
int16_t h[], /* (i) : impulse response of weighted w_w_synthesis filter */
int16_t y[], /* (o) : filtered innovative code */
int16_t indx[] /* (o) : index of 10 pulses (sign+position) */
)
{
int16_t i, j, k, track, index, _sign[NB_PULSE];
int16_t *p0, *p1, *p2, *p3, *p4, *p5, *p6, *p7, *p8, *p9;
int32_t s;
for (i = 0; i < L_CODE; i++) {
cod[i] = 0;
}
for (i = 0; i < NB_TRACK; i++) {
indx[i] = -1;
}
for (k = 0; k < NB_PULSE; k++) {
/* read pulse position */
i = codvec[k];
/* read sign */
j = sign[i];
index = w_mult(i, 6554); /* index = pos/5 */
/* track = pos%5 */
track =
w_sub(i, w_extract_l(w_L_w_shr(w_L_w_mult(index, 5), 1)));
if (j > 0) {
cod[i] = w_add(cod[i], 4096);
_sign[k] = 8192;
} else {
cod[i] = w_sub(cod[i], 4096);
_sign[k] = -8192;
index = w_add(index, 8);
}
if (indx[track] < 0) {
indx[track] = index;
} else {
if (((index ^ indx[track]) & 8) == 0) {
/* sign of 1st pulse == sign of 2nd pulse */
if (w_sub(indx[track], index) <= 0) {
indx[track + 5] = index;
} else {
indx[track + 5] = indx[track];
indx[track] = index;
}
} else {
/* sign of 1st pulse != sign of 2nd pulse */
if (w_sub((indx[track] & 7), (index & 7)) <= 0) {
indx[track + 5] = indx[track];
indx[track] = index;
} else {
indx[track + 5] = index;
}
}
}
}
p0 = h - codvec[0];
p1 = h - codvec[1];
p2 = h - codvec[2];
p3 = h - codvec[3];
p4 = h - codvec[4];
p5 = h - codvec[5];
p6 = h - codvec[6];
p7 = h - codvec[7];
p8 = h - codvec[8];
p9 = h - codvec[9];
for (i = 0; i < L_CODE; i++) {
s = 0;
s = w_L_mac(s, *p0++, _sign[0]);
s = w_L_mac(s, *p1++, _sign[1]);
s = w_L_mac(s, *p2++, _sign[2]);
s = w_L_mac(s, *p3++, _sign[3]);
s = w_L_mac(s, *p4++, _sign[4]);
s = w_L_mac(s, *p5++, _sign[5]);
s = w_L_mac(s, *p6++, _sign[6]);
s = w_L_mac(s, *p7++, _sign[7]);
s = w_L_mac(s, *p8++, _sign[8]);
s = w_L_mac(s, *p9++, _sign[9]);
y[i] = w_round(s);
}
}
void w_set_sign(int16_t dn[], /* (i/o): correlation between target and h[] */
int16_t cn[], /* (i) : residual after long term w_prediction */
int16_t sign[], /* (o) : sign of d[n] */
int16_t pos_max[], /* (o) : position of maximum correlation */
int16_t ipos[] /* (o) : starting position for each pulse */
)
{
int16_t i, j;
int16_t val, cor, k_cn, k_dn, max, max_of_all, pos = 0;
int16_t en[L_CODE]; /* correlation vector */
int32_t s;
/* calculate energy for normalization of cn[] and dn[] */
s = 256;
for (i = 0; i < L_CODE; i++) {
s = w_L_mac(s, cn[i], cn[i]);
}
s = w_Inv_sqrt(s);
k_cn = w_extract_h(w_L_w_shl(s, 5));
s = 256;
for (i = 0; i < L_CODE; i++) {
s = w_L_mac(s, dn[i], dn[i]);
}
s = w_Inv_sqrt(s);
k_dn = w_extract_h(w_L_w_shl(s, 5));
for (i = 0; i < L_CODE; i++) {
val = dn[i];
cor =
w_round(w_L_w_shl
(w_L_mac(w_L_w_mult(k_cn, cn[i]), k_dn, val), 10));
if (cor >= 0) {
sign[i] = 32767; /* sign = +1 */
} else {
sign[i] = -32767; /* sign = -1 */
cor = w_negate(cor);
val = w_negate(val);
}
/* modify dn[] according to the fixed sign */
dn[i] = val;
en[i] = cor;
}
max_of_all = -1;
for (i = 0; i < NB_TRACK; i++) {
max = -1;
for (j = i; j < L_CODE; j += STEP) {
cor = en[j];
val = w_sub(cor, max);
if (val > 0) {
max = cor;
pos = j;
}
}
/* store maximum correlation position */
pos_max[i] = pos;
val = w_sub(max, max_of_all);
if (val > 0) {
max_of_all = max;
/* starting position for i0 */
ipos[0] = i;
}
}
/*----------------------------------------------------------------*
* Set starting position of each pulse. *
*----------------------------------------------------------------*/
pos = ipos[0];
ipos[5] = pos;
for (i = 1; i < NB_TRACK; i++) {
pos = w_add(pos, 1);
if (w_sub(pos, NB_TRACK) >= 0) {
pos = 0;
}
ipos[i] = pos;
ipos[i + 5] = pos;
}
}
void SeniorVMHandle::w_cmp(long _register1,long _register2)
{
w_sub(_register1,_register2);
pop(T_EFLAG);
pop(T_INVALID16);
}
