void speex_decode_stereo_int(spx_int16_t *data, int frame_size, SpeexStereoState *_stereo)
{
int i;
spx_word32_t balance;
spx_word16_t e_left, e_right, e_ratio;
RealSpeexStereoState *stereo = (RealSpeexStereoState*)_stereo;
/* COMPATIBILITY_HACK(stereo); */
balance=stereo->balance;
e_ratio=stereo->e_ratio;
/* These two are Q14, with max value just below 2. */
e_right = DIV32(QCONST32(1., 22), spx_sqrt(MULT16_32_Q15(e_ratio, ADD32(QCONST32(1., 16), balance))));
e_left = SHR32(MULT16_16(spx_sqrt(balance), e_right), 8);
for (i=frame_size-1;i>=0;i--)
{
spx_int16_t tmp=data[i];
stereo->smooth_left = EXTRACT16(PSHR32(MAC16_16(MULT16_16(stereo->smooth_left, QCONST16(0.98, 15)), e_left, QCONST16(0.02, 15)), 15));
stereo->smooth_right = EXTRACT16(PSHR32(MAC16_16(MULT16_16(stereo->smooth_right, QCONST16(0.98, 15)), e_right, QCONST16(0.02, 15)), 15));
data[2*i] = (spx_int16_t)MULT16_16_P14(stereo->smooth_left, tmp);
data[2*i+1] = (spx_int16_t)MULT16_16_P14(stereo->smooth_right, tmp);
}
}
static inline opus_val16 tansig_approx(opus_val32 _x)
{ /* Q19 */
int i;
opus_val16 xx; /* Q11 */
/*double x, y; */
opus_val16 dy, yy; /* Q14 */
/*x = 1.9073e-06*_x; */
if (_x >= QCONST32(8, 19))
return QCONST32(1., 14);
if (_x <= -QCONST32(8, 19))
return -QCONST32(1., 14);
xx = EXTRACT16(SHR32(_x, 8));
/*i = lrint(25*x); */
i = SHR32(ADD32(1024, MULT16_16(25, xx)), 11);
/*x -= .04*i; */
xx -= EXTRACT16(SHR32(MULT16_16(20972, i), 8));
/*x = xx*(1./2048); */
/*y = tansig_table[250+i]; */
yy = tansig_table[250 + i];
/*y = yy*(1./16384); */
dy = 16384 - MULT16_16_Q14(yy, yy);
yy = yy + MULT16_16_Q14(MULT16_16_Q11(xx, dy),
(16384 - MULT16_16_Q11(yy, xx)));
return yy;
}
static void exp_rotation1(celt_norm *X, int len, int stride, opus_val16 c, opus_val16 s)
{
int i;
opus_val16 ms;
celt_norm *Xptr;
Xptr = X;
ms = NEG16(s);
for (i=0; i<len-stride; i++)
{
celt_norm x1, x2;
x1 = Xptr[0];
x2 = Xptr[stride];
Xptr[stride] = EXTRACT16(PSHR32(MAC16_16(MULT16_16(c, x2), s, x1), 15));
*Xptr++ = EXTRACT16(PSHR32(MAC16_16(MULT16_16(c, x1), ms, x2), 15));
}
Xptr = &X[len-2*stride-1];
for (i=len-2*stride-1; i>=0; i--)
{
celt_norm x1, x2;
x1 = Xptr[0];
x2 = Xptr[stride];
Xptr[stride] = EXTRACT16(PSHR32(MAC16_16(MULT16_16(c, x2), s, x1), 15));
*Xptr-- = EXTRACT16(PSHR32(MAC16_16(MULT16_16(c, x1), ms, x2), 15));
}
}
static inline void mdf_adjust_prop(const spx_word32_t *W, int N, int M, int P, spx_word16_t *prop)
{
int i, j, p;
spx_word16_t max_sum = 1;
spx_word32_t prop_sum = 1;
for (i=0;i<M;i++)
{
spx_word32_t tmp = 1;
for (p=0;p<P;p++)
for (j=0;j<N;j++)
tmp += MULT16_16(EXTRACT16(SHR32(W[p*N*M + i*N+j],18)), EXTRACT16(SHR32(W[p*N*M + i*N+j],18)));
#ifdef FIXED_POINT
/* Just a security in case an overflow were to occur */
tmp = MIN32(ABS32(tmp), 536870912);
#endif
prop[i] = spx_sqrt(tmp);
if (prop[i] > max_sum)
max_sum = prop[i];
}
for (i=0;i<M;i++)
{
prop[i] += MULT16_16_Q15(QCONST16(.1f,15),max_sum);
prop_sum += EXTEND32(prop[i]);
}
for (i=0;i<M;i++)
{
prop[i] = DIV32(MULT16_16(QCONST16(.99f,15), prop[i]),prop_sum);
/*printf ("%f ", prop[i]);*/
}
/*printf ("\n");*/
}
/* Compute the amplitude (sqrt energy) in each of the bands */
void compute_band_energies(const CELTMode *m, const celt_sig *X, celt_ener *bandE, int end, int C, int M)
{
int i, c, N;
const opus_int16 *eBands = m->eBands;
N = M*m->shortMdctSize;
c=0; do {
for (i=0;i<end;i++)
{
int j;
opus_val32 maxval=0;
opus_val32 sum = 0;
j=M*eBands[i]; do {
maxval = MAX32(maxval, X[j+c*N]);
maxval = MAX32(maxval, -X[j+c*N]);
} while (++j<M*eBands[i+1]);
if (maxval > 0)
{
int shift = celt_ilog2(maxval)-10;
j=M*eBands[i]; do {
sum = MAC16_16(sum, EXTRACT16(VSHR32(X[j+c*N],shift)),
EXTRACT16(VSHR32(X[j+c*N],shift)));
} while (++j<M*eBands[i+1]);
/* We're adding one here to make damn sure we never end up with a pitch vector that's
larger than unity norm */
bandE[i+c*m->nbEBands] = EPSILON+VSHR32(EXTEND32(celt_sqrt(sum)),-shift);
} else {
bandE[i+c*m->nbEBands] = EPSILON;
}
/*printf ("%f ", bandE[i+c*m->nbEBands]);*/
}
} while (++c<C);
/*printf ("\n");*/
}
/** Forced pitch delay and gain */
int forced_pitch_quant(spx_word16_t target[], /* Target vector */
spx_word16_t * sw, spx_coef_t ak[], /* LPCs for this subframe */
spx_coef_t awk1[], /* Weighted LPCs #1 for this subframe */
spx_coef_t awk2[], /* Weighted LPCs #2 for this subframe */
spx_sig_t exc[], /* Excitation */
const void *par, int start, /* Smallest pitch value allowed */
int end, /* Largest pitch value allowed */
spx_word16_t pitch_coef, /* Voicing (pitch) coefficient */
int p, /* Number of LPC coeffs */
int nsf, /* Number of samples in subframe */
SpeexBits * bits,
char *stack,
spx_word16_t * exc2,
spx_word16_t * r,
int complexity,
int cdbk_offset,
int plc_tuning, spx_word32_t * cumul_gain)
{
(void)sw;
(void)par;
(void)end;
(void)bits;
(void)r;
(void)complexity;
(void)cdbk_offset;
(void)plc_tuning;
(void)cumul_gain;
int i;
spx_word16_t res[nsf];
#ifdef FIXED_POINT
if (pitch_coef > 63)
pitch_coef = 63;
#else
if (pitch_coef > .99)
pitch_coef = .99;
#endif
for (i = 0; i < nsf && i < start; i++) {
exc[i] = MULT16_16(SHL16(pitch_coef, 7), exc2[i - start]);
}
for (; i < nsf; i++) {
exc[i] = MULT16_32_Q15(SHL16(pitch_coef, 9), exc[i - start]);
}
for (i = 0; i < nsf; i++)
res[i] = EXTRACT16(PSHR32(exc[i], SIG_SHIFT - 1));
syn_percep_zero16(res, ak, awk1, awk2, res, nsf, p, stack);
for (i = 0; i < nsf; i++)
target[i] =
EXTRACT16(SATURATE
(SUB32(EXTEND32(target[i]), EXTEND32(res[i])),
32700));
return start;
}
spx_word32_t speex_rand(spx_word16_t std, spx_int32_t *seed)
{
spx_word32_t res;
*seed = 1664525 * *seed + 1013904223;
res = MULT16_16(EXTRACT16(SHR32(*seed,16)),std);
return SUB32(res, SHR(res, 3));
}
/** Unquantize forced pitch delay and gain */
void forced_pitch_unquant(
spx_word16_t exc[], /* Input excitation */
spx_word32_t exc_out[], /* Output excitation */
int start, /* Smallest pitch value allowed */
int end, /* Largest pitch value allowed */
spx_word16_t pitch_coef, /* Voicing (pitch) coefficient */
const void* par,
int nsf, /* Number of samples in subframe */
int* pitch_val,
spx_word16_t* gain_val,
SpeexBits* bits,
char* stack,
int count_lost,
int subframe_offset,
spx_word16_t last_pitch_gain,
int cdbk_offset
) {
int i;
#ifdef FIXED_POINT
if (pitch_coef > 63)
pitch_coef = 63;
#else
if (pitch_coef > .99)
pitch_coef = .99;
#endif
for (i = 0; i < nsf; i++) {
exc_out[i] = MULT16_16(exc[i - start], SHL16(pitch_coef, 7));
exc[i] = EXTRACT16(PSHR32(exc_out[i], 13));
}
*pitch_val = start;
gain_val[0] = gain_val[2] = 0;
gain_val[1] = pitch_coef;
}
static void compute_weighted_codebook(const signed char *shape_cb, const spx_word16_t *r, spx_word16_t *resp, spx_word16_t *resp2, spx_word32_t *E, int shape_cb_size, int subvect_size, char *stack)
{
int i, j, k;
VARDECL(spx_word16_t *shape);
ALLOC(shape, subvect_size, spx_word16_t);
for (i=0;i<shape_cb_size;i++)
{
spx_word16_t *res;
res = resp+i*subvect_size;
for (k=0;k<subvect_size;k++)
shape[k] = (spx_word16_t)shape_cb[i*subvect_size+k];
E[i]=0;
/* Compute codeword response using convolution with impulse response */
for(j=0;j<subvect_size;j++)
{
spx_word32_t resj=0;
spx_word16_t res16;
for (k=0;k<=j;k++)
resj = MAC16_16(resj,shape[k],r[j-k]);
#ifdef FIXED_POINT
res16 = EXTRACT16(SHR32(resj, 13));
#else
res16 = 0.03125f*resj;
#endif
/* Compute codeword energy */
E[i]=MAC16_16(E[i],res16,res16);
res[j] = res16;
/*printf ("%d\n", (int)res[j]);*/
}
}
}
static void find_best_pitch(opus_val32 *xcorr, opus_val16 *y, int len,
int max_pitch, int *best_pitch
#ifdef FIXED_POINT
, int yshift, opus_val32 maxcorr
#endif
)
{
int i, j;
opus_val32 Syy=1;
opus_val16 best_num[2];
opus_val32 best_den[2];
#ifdef FIXED_POINT
int xshift;
xshift = celt_ilog2(maxcorr)-14;
#endif
best_num[0] = -1;
best_num[1] = -1;
best_den[0] = 0;
best_den[1] = 0;
best_pitch[0] = 0;
best_pitch[1] = 1;
for (j=0;j<len;j++)
Syy = ADD32(Syy, SHR32(MULT16_16(y[j],y[j]), yshift));
for (i=0;i<max_pitch;i++)
{
if (xcorr[i]>0)
{
opus_val16 num;
opus_val32 xcorr16;
xcorr16 = EXTRACT16(VSHR32(xcorr[i], xshift));
#ifndef FIXED_POINT
/* Considering the range of xcorr16, this should avoid both underflows
and overflows (inf) when squaring xcorr16 */
xcorr16 *= 1e-12f;
#endif
num = MULT16_16_Q15(xcorr16,xcorr16);
if (MULT16_32_Q15(num,best_den[1]) > MULT16_32_Q15(best_num[1],Syy))
{
if (MULT16_32_Q15(num,best_den[0]) > MULT16_32_Q15(best_num[0],Syy))
{
best_num[1] = best_num[0];
best_den[1] = best_den[0];
best_pitch[1] = best_pitch[0];
best_num[0] = num;
best_den[0] = Syy;
best_pitch[0] = i;
} else {
best_num[1] = num;
best_den[1] = Syy;
best_pitch[1] = i;
}
}
}
Syy += SHR32(MULT16_16(y[i+len],y[i+len]),yshift) - SHR32(MULT16_16(y[i],y[i]),yshift);
Syy = MAX32(1, Syy);
}
}
EXPORT int speex_echo_ctl(SpeexEchoState *st, int request, void *ptr)
{
switch(request)
{
case SPEEX_ECHO_GET_FRAME_SIZE:
(*(int*)ptr) = st->frame_size;
break;
case SPEEX_ECHO_SET_SAMPLING_RATE:
st->sampling_rate = (*(int*)ptr);
st->spec_average = DIV32_16(SHL32(EXTEND32(st->frame_size), 15), st->sampling_rate);
#ifdef FIXED_POINT
st->beta0 = DIV32_16(SHL32(EXTEND32(st->frame_size), 16), st->sampling_rate);
st->beta_max = DIV32_16(SHL32(EXTEND32(st->frame_size), 14), st->sampling_rate);
#else
st->beta0 = (2.0f*st->frame_size)/st->sampling_rate;
st->beta_max = (.5f*st->frame_size)/st->sampling_rate;
#endif
if (st->sampling_rate<12000)
st->notch_radius = QCONST16(.9, 15);
else if (st->sampling_rate<24000)
st->notch_radius = QCONST16(.982, 15);
else
st->notch_radius = QCONST16(.992, 15);
break;
case SPEEX_ECHO_GET_SAMPLING_RATE:
(*(int*)ptr) = st->sampling_rate;
break;
case SPEEX_ECHO_GET_IMPULSE_RESPONSE_SIZE:
/*FIXME: Implement this for multiple channels */
*((spx_int32_t *)ptr) = st->M * st->frame_size;
break;
case SPEEX_ECHO_GET_IMPULSE_RESPONSE:
{
int M = st->M, N = st->window_size, n = st->frame_size, i, j;
spx_int32_t *filt = (spx_int32_t *) ptr;
for(j=0;j<M;j++)
{
/*FIXME: Implement this for multiple channels */
#ifdef FIXED_POINT
for (i=0;i<N;i++)
st->wtmp2[i] = EXTRACT16(PSHR32(st->W[j*N+i],16+NORMALIZE_SCALEDOWN));
spx_ifft(st->fft_table, st->wtmp2, st->wtmp);
#else
spx_ifft(st->fft_table, &st->W[j*N], st->wtmp);
#endif
for(i=0;i<n;i++)
filt[j*n+i] = PSHR32(MULT16_16(32767,st->wtmp[i]), WEIGHT_SHIFT-NORMALIZE_SCALEDOWN);
}
}
break;
default:
speex_warning_int("Unknown speex_echo_ctl request: ", request);
return -1;
}
return 0;
}
static __inline celt_word16_t SIG2WORD16(celt_sig_t x)
{
#ifdef FIXED_POINT
x = PSHR32(x, SIG_SHIFT);
x = MAX32(x, -32768);
x = MIN32(x, 32767);
return EXTRACT16(x);
#else
return (celt_word16_t)x;
#endif
}
static void find_best_pitch(opus_val32 *xcorr, opus_val16 *y, int len,
int max_pitch, int *best_pitch
#ifdef FIXED_POINT
, int yshift, opus_val32 maxcorr
#endif
)
{
int i, j;
opus_val32 Syy=1;
opus_val16 best_num[2];
opus_val32 best_den[2];
#ifdef FIXED_POINT
int xshift;
xshift = celt_ilog2(maxcorr)-14;
#endif
best_num[0] = -1;
best_num[1] = -1;
best_den[0] = 0;
best_den[1] = 0;
best_pitch[0] = 0;
best_pitch[1] = 1;
for (j=0;j<len;j++)
Syy = MAC16_16(Syy, y[j],y[j]);
for (i=0;i<max_pitch;i++)
{
if (xcorr[i]>0)
{
opus_val16 num;
opus_val32 xcorr16;
xcorr16 = EXTRACT16(VSHR32(xcorr[i], xshift));
num = MULT16_16_Q15(xcorr16,xcorr16);
if (MULT16_32_Q15(num,best_den[1]) > MULT16_32_Q15(best_num[1],Syy))
{
if (MULT16_32_Q15(num,best_den[0]) > MULT16_32_Q15(best_num[0],Syy))
{
best_num[1] = best_num[0];
best_den[1] = best_den[0];
best_pitch[1] = best_pitch[0];
best_num[0] = num;
best_den[0] = Syy;
best_pitch[0] = i;
} else {
best_num[1] = num;
best_den[1] = Syy;
best_pitch[1] = i;
}
}
}
Syy += SHR32(MULT16_16(y[i+len],y[i+len]),yshift) - SHR32(MULT16_16(y[i],y[i]),yshift);
Syy = MAX32(1, Syy);
}
}
static inline opus_val16 SIG2WORD16(celt_sig x)
{
#ifdef FIXED_POINT
x = PSHR32(x, SIG_SHIFT);
x = MAX32(x, -32768);
x = MIN32(x, 32767);
return EXTRACT16(x);
#else
return (opus_val16)x;
#endif
}
void filterbank_compute_psd16(FilterBank *bank, spx_word16_t *mel, spx_word16_t *ps)
{
int i;
for (i=0;i<bank->len;i++)
{
spx_word32_t tmp;
int id1, id2;
id1 = bank->bank_left[i];
id2 = bank->bank_right[i];
tmp = MULT16_16(mel[id1],bank->filter_left[i]);
tmp += MULT16_16(mel[id2],bank->filter_right[i]);
ps[i] = EXTRACT16(PSHR32(tmp,15));
}
}
/* Compute the amplitude (sqrt energy) in each of the bands */
void compute_band_energies(const CELTMode *m, const celt_sig *X, celt_ener *bank, int _C)
{
int i, c, N;
const celt_int16 *eBands = m->eBands;
const int C = CHANNELS(_C);
N = FRAMESIZE(m);
for (c=0;c<C;c++)
{
for (i=0;i<m->nbEBands;i++)
{
int j;
celt_word32 maxval=0;
celt_word32 sum = 0;
j=eBands[i]; do {
maxval = MAX32(maxval, X[j+c*N]);
maxval = MAX32(maxval, -X[j+c*N]);
} while (++j<eBands[i+1]);
if (maxval > 0)
{
int shift = celt_ilog2(maxval)-10;
j=eBands[i]; do {
sum = MAC16_16(sum, EXTRACT16(VSHR32(X[j+c*N],shift)),
EXTRACT16(VSHR32(X[j+c*N],shift)));
} while (++j<eBands[i+1]);
/* We're adding one here to make damn sure we never end up with a pitch vector that's
larger than unity norm */
bank[i+c*m->nbEBands] = EPSILON+VSHR32(EXTEND32(celt_sqrt(sum)),-shift);
} else {
bank[i+c*m->nbEBands] = EPSILON;
}
/*printf ("%f ", bank[i+c*m->nbEBands]);*/
}
}
/*printf ("\n");*/
}
static void cubic_coef(spx_word16_t x, spx_word16_t interp[4])
{
/* Compute interpolation coefficients. I'm not sure whether this corresponds to cubic interpolation
but I know it's MMSE-optimal on a sinc */
spx_word16_t x2, x3;
x2 = MULT16_16_P15(x, x);
x3 = MULT16_16_P15(x, x2);
interp[0] = PSHR32(MULT16_16(QCONST16(-0.16667f, 15),x) + MULT16_16(QCONST16(0.16667f, 15),x3),15);
interp[1] = EXTRACT16(EXTEND32(x) + SHR32(SUB32(EXTEND32(x2),EXTEND32(x3)),1));
interp[3] = PSHR32(MULT16_16(QCONST16(-0.33333f, 15),x) + MULT16_16(QCONST16(.5f,15),x2) - MULT16_16(QCONST16(0.16667f, 15),x3),15);
/* Just to make sure we don't have rounding problems */
interp[2] = Q15_ONE-interp[0]-interp[1]-interp[3];
if (interp[2]<32767)
interp[2]+=1;
}
void mlp_process(const MLP * m, const opus_val16 * in, opus_val16 * out)
{
int j;
opus_val16 hidden[MAX_NEURONS];
const opus_val16 *W = m->weights;
/* Copy to tmp_in */
for (j = 0; j < m->topo[1]; j++) {
int k;
opus_val32 sum = SHL32(EXTEND32(*W++), 8);
for (k = 0; k < m->topo[0]; k++)
sum = MAC16_16(sum, in[k], *W++);
hidden[j] = tansig_approx(sum);
}
for (j = 0; j < m->topo[2]; j++) {
int k;
opus_val32 sum = SHL32(EXTEND32(*W++), 14);
for (k = 0; k < m->topo[1]; k++)
sum = MAC16_16(sum, hidden[k], *W++);
out[j] = tansig_approx(EXTRACT16(PSHR32(sum, 17)));
}
}
void split_cb_search_shape_sign(
spx_word16_t target[], /* target vector */
spx_coef_t ak[], /* LPCs for this subframe */
spx_coef_t awk1[], /* Weighted LPCs for this subframe */
spx_coef_t awk2[], /* Weighted LPCs for this subframe */
const void *par, /* Codebook/search parameters*/
int p, /* number of LPC coeffs */
int nsf, /* number of samples in subframe */
spx_sig_t *exc,
spx_word16_t *r,
SpeexBits *bits,
char *stack,
int complexity,
int update_target
)
{
int i,j,m,q;
const signed char *shape_cb;
int shape_cb_size = 32, subvect_size = 10;
int best_index;
spx_word32_t best_dist;
spx_word16_t resp[320];
spx_word16_t *resp2 = resp;
spx_word32_t E[32];
spx_word16_t t[40];
spx_sig_t e[40];
shape_cb=exc_10_32_table;
/* FIXME: Do we still need to copy the target? */
SPEEX_COPY(t, target, nsf);
//compute_weighted_codebook
{
int i, k;
spx_word16_t shape[10];
for (i=0;i<shape_cb_size;i++)
{
spx_word16_t *res;
res = resp+i*subvect_size;
for (k=0;k<subvect_size;k++)
shape[k] = (spx_word16_t)shape_cb[i*subvect_size+k];
E[i]=0;
/* Compute codeword response using convolution with impulse response */
{
spx_word32_t resj;
spx_word16_t res16;
// 0
resj = MULT16_16(shape[0],r[0]);
res16 = EXTRACT16(SHR32(resj, 13));
// Compute codeword energy
E[i]=MAC16_16(E[i],res16,res16);
res[0] = res16;
//++++++++++++++++++++++++++
// 1
resj = MULT16_16(shape[0],r[1]);
resj = MAC16_16(resj,shape[1],r[0]);
res16 = EXTRACT16(SHR32(resj, 13));
// Compute codeword energy
E[i]=MAC16_16(E[i],res16,res16);
res[1] = res16;
//++++++++++++++++++++++++++
// 2
resj = MULT16_16(shape[0],r[2]);
resj = MAC16_16(resj,shape[1],r[1]);
resj = MAC16_16(resj,shape[2],r[0]);
res16 = EXTRACT16(SHR32(resj, 13));
// Compute codeword energy
E[i]=MAC16_16(E[i],res16,res16);
res[2] = res16;
//++++++++++++++++++++++++++
// 3
resj = MULT16_16(shape[0],r[3]);
resj = MAC16_16(resj,shape[1],r[2]);
resj = MAC16_16(resj,shape[2],r[1]);
resj = MAC16_16(resj,shape[3],r[0]);
res16 = EXTRACT16(SHR32(resj, 13));
// Compute codeword energy
E[i]=MAC16_16(E[i],res16,res16);
res[3] = res16;
//++++++++++++++++++++++++++
// 4
resj = MULT16_16(shape[0],r[4]);
resj = MAC16_16(resj,shape[1],r[3]);
resj = MAC16_16(resj,shape[2],r[2]);
resj = MAC16_16(resj,shape[3],r[1]);
resj = MAC16_16(resj,shape[4],r[0]);
res16 = EXTRACT16(SHR32(resj, 13));
// Compute codeword energy
E[i]=MAC16_16(E[i],res16,res16);
res[4] = res16;
//++++++++++++++++++++++++++
// 5
resj = MULT16_16(shape[0],r[5]);
resj = MAC16_16(resj,shape[1],r[4]);
resj = MAC16_16(resj,shape[2],r[3]);
resj = MAC16_16(resj,shape[3],r[2]);
resj = MAC16_16(resj,shape[4],r[1]);
resj = MAC16_16(resj,shape[5],r[0]);
res16 = EXTRACT16(SHR32(resj, 13));
// Compute codeword energy
E[i]=MAC16_16(E[i],res16,res16);
res[5] = res16;
//++++++++++++++++++++++++++
// 6
resj = MULT16_16(shape[0],r[6]);
resj = MAC16_16(resj,shape[1],r[5]);
resj = MAC16_16(resj,shape[2],r[4]);
resj = MAC16_16(resj,shape[3],r[3]);
resj = MAC16_16(resj,shape[4],r[2]);
resj = MAC16_16(resj,shape[5],r[1]);
resj = MAC16_16(resj,shape[6],r[0]);
res16 = EXTRACT16(SHR32(resj, 13));
// Compute codeword energy
E[i]=MAC16_16(E[i],res16,res16);
res[6] = res16;
//++++++++++++++++++++++++++
// 7
resj = MULT16_16(shape[0],r[7]);
resj = MAC16_16(resj,shape[1],r[6]);
resj = MAC16_16(resj,shape[2],r[5]);
resj = MAC16_16(resj,shape[3],r[4]);
resj = MAC16_16(resj,shape[4],r[3]);
resj = MAC16_16(resj,shape[5],r[2]);
resj = MAC16_16(resj,shape[6],r[1]);
resj = MAC16_16(resj,shape[7],r[0]);
res16 = EXTRACT16(SHR32(resj, 13));
// Compute codeword energy
E[i]=MAC16_16(E[i],res16,res16);
res[7] = res16;
//++++++++++++++++++++++++++
// 8
resj = MULT16_16(shape[0],r[8]);
resj = MAC16_16(resj,shape[1],r[7]);
resj = MAC16_16(resj,shape[2],r[6]);
resj = MAC16_16(resj,shape[3],r[5]);
resj = MAC16_16(resj,shape[4],r[4]);
resj = MAC16_16(resj,shape[5],r[3]);
resj = MAC16_16(resj,shape[6],r[2]);
resj = MAC16_16(resj,shape[7],r[1]);
resj = MAC16_16(resj,shape[8],r[0]);
res16 = EXTRACT16(SHR32(resj, 13));
// Compute codeword energy
E[i]=MAC16_16(E[i],res16,res16);
res[8] = res16;
//++++++++++++++++++++++++++
// 9
resj = MULT16_16(shape[0],r[9]);
resj = MAC16_16(resj,shape[1],r[8]);
resj = MAC16_16(resj,shape[2],r[7]);
resj = MAC16_16(resj,shape[3],r[6]);
resj = MAC16_16(resj,shape[4],r[5]);
resj = MAC16_16(resj,shape[5],r[4]);
resj = MAC16_16(resj,shape[6],r[3]);
resj = MAC16_16(resj,shape[7],r[2]);
resj = MAC16_16(resj,shape[8],r[1]);
resj = MAC16_16(resj,shape[9],r[0]);
res16 = EXTRACT16(SHR32(resj, 13));
// Compute codeword energy
E[i]=MAC16_16(E[i],res16,res16);
res[9] = res16;
//++++++++++++++++++++++++++
}
}
}
for (i=0;i<4;i++)
{
spx_word16_t *x=t+subvect_size*i;
/*Find new n-best based on previous n-best j*/
vq_nbest(x, resp2, subvect_size, shape_cb_size, E, 1, &best_index, &best_dist, stack);
speex_bits_pack(bits,best_index,5);
{
int rind;
spx_word16_t *res;
spx_word16_t sign=1;
rind = best_index;
if (rind>=shape_cb_size)
{
sign=-1;
rind-=shape_cb_size;
}
res = resp+rind*subvect_size;
if (sign>0)
for (m=0;m<subvect_size;m++)
t[subvect_size*i+m] = SUB16(t[subvect_size*i+m], res[m]);
else
for (m=0;m<subvect_size;m++)
t[subvect_size*i+m] = ADD16(t[subvect_size*i+m], res[m]);
if (sign==1)
{
for (j=0;j<subvect_size;j++)
e[subvect_size*i+j]=SHL32(EXTEND32(shape_cb[rind*subvect_size+j]),SIG_SHIFT-5);
} else {
for (j=0;j<subvect_size;j++)
e[subvect_size*i+j]=NEG32(SHL32(EXTEND32(shape_cb[rind*subvect_size+j]),SIG_SHIFT-5));
}
}
for (m=0;m<subvect_size;m++)
{
spx_word16_t g;
int rind;
spx_word16_t sign=1;
rind = best_index;
if (rind>=shape_cb_size)
{
sign=-1;
rind-=shape_cb_size;
}
q=subvect_size-m;
g=sign*shape_cb[rind*subvect_size+m];
target_update(t+subvect_size*(i+1), g, r+q, nsf-subvect_size*(i+1));
}
}
/* Update excitation */
/* FIXME: We could update the excitation directly above */
for (j=0;j<nsf;j++)
exc[j]=ADD32(exc[j],e[j]);
}
/** Finds the best quantized 3-tap pitch predictor by analysis by synthesis */
static spx_word64_t pitch_gain_search_3tap(
const spx_sig_t target[], /* Target vector */
const spx_coef_t ak[], /* LPCs for this subframe */
const spx_coef_t awk1[], /* Weighted LPCs #1 for this subframe */
const spx_coef_t awk2[], /* Weighted LPCs #2 for this subframe */
spx_sig_t exc[], /* Excitation */
const void *par,
int pitch, /* Pitch value */
int p, /* Number of LPC coeffs */
int nsf, /* Number of samples in subframe */
SpeexBits *bits,
char *stack,
const spx_sig_t *exc2,
const spx_word16_t *r,
spx_sig_t *new_target,
int *cdbk_index,
int cdbk_offset,
int plc_tuning
)
{
int i,j;
VARDECL(spx_sig_t *tmp1);
VARDECL(spx_sig_t *tmp2);
spx_sig_t *x[3];
spx_sig_t *e[3];
spx_word32_t corr[3];
spx_word32_t A[3][3];
int gain_cdbk_size;
const signed char *gain_cdbk;
spx_word16_t gain[3];
spx_word64_t err;
const ltp_params *params;
params = (const ltp_params*) par;
gain_cdbk_size = 1<<params->gain_bits;
gain_cdbk = params->gain_cdbk + 3*gain_cdbk_size*cdbk_offset;
ALLOC(tmp1, 3*nsf, spx_sig_t);
ALLOC(tmp2, 3*nsf, spx_sig_t);
x[0]=tmp1;
x[1]=tmp1+nsf;
x[2]=tmp1+2*nsf;
e[0]=tmp2;
e[1]=tmp2+nsf;
e[2]=tmp2+2*nsf;
for (i=2; i>=0; i--)
{
int pp=pitch+1-i;
for (j=0; j<nsf; j++)
{
if (j-pp<0)
e[i][j]=exc2[j-pp];
else if (j-pp-pitch<0)
e[i][j]=exc2[j-pp-pitch];
else
e[i][j]=0;
}
if (i==2)
syn_percep_zero(e[i], ak, awk1, awk2, x[i], nsf, p, stack);
else {
for (j=0; j<nsf-1; j++)
x[i][j+1]=x[i+1][j];
x[i][0]=0;
for (j=0; j<nsf; j++)
{
x[i][j]=ADD32(x[i][j],SHL32(MULT16_32_Q15(r[j], e[i][0]),1));
}
}
}
#ifdef FIXED_POINT
{
/* If using fixed-point, we need to normalize the signals first */
spx_word16_t *y[3];
VARDECL(spx_word16_t *ytmp);
VARDECL(spx_word16_t *t);
spx_sig_t max_val=1;
int sig_shift;
ALLOC(ytmp, 3*nsf, spx_word16_t);
#if 0
ALLOC(y[0], nsf, spx_word16_t);
ALLOC(y[1], nsf, spx_word16_t);
ALLOC(y[2], nsf, spx_word16_t);
#else
y[0] = ytmp;
y[1] = ytmp+nsf;
y[2] = ytmp+2*nsf;
#endif
ALLOC(t, nsf, spx_word16_t);
for (j=0; j<3; j++)
{
for (i=0; i<nsf; i++)
{
spx_sig_t tmp = x[j][i];
if (tmp<0)
tmp = -tmp;
if (tmp > max_val)
max_val = tmp;
}
}
for (i=0; i<nsf; i++)
{
spx_sig_t tmp = target[i];
if (tmp<0)
tmp = -tmp;
if (tmp > max_val)
max_val = tmp;
}
sig_shift=0;
while (max_val>16384)
{
sig_shift++;
max_val >>= 1;
}
for (j=0; j<3; j++)
{
for (i=0; i<nsf; i++)
{
y[j][i] = EXTRACT16(SHR32(x[j][i],sig_shift));
}
}
for (i=0; i<nsf; i++)
{
t[i] = EXTRACT16(SHR32(target[i],sig_shift));
}
for (i=0; i<3; i++)
corr[i]=inner_prod(y[i],t,nsf);
for (i=0; i<3; i++)
for (j=0; j<=i; j++)
A[i][j]=A[j][i]=inner_prod(y[i],y[j],nsf);
}
#else
{
for (i=0; i<3; i++)
corr[i]=inner_prod(x[i],target,nsf);
for (i=0; i<3; i++)
for (j=0; j<=i; j++)
A[i][j]=A[j][i]=inner_prod(x[i],x[j],nsf);
}
#endif
{
spx_word32_t C[9];
const signed char *ptr=gain_cdbk;
int best_cdbk=0;
spx_word32_t best_sum=0;
C[0]=corr[2];
C[1]=corr[1];
C[2]=corr[0];
C[3]=A[1][2];
C[4]=A[0][1];
C[5]=A[0][2];
C[6]=A[2][2];
C[7]=A[1][1];
C[8]=A[0][0];
/*plc_tuning *= 2;*/
if (plc_tuning<2)
plc_tuning=2;
#ifdef FIXED_POINT
C[0] = MAC16_32_Q15(C[0],MULT16_16_16(plc_tuning,-327),C[0]);
C[1] = MAC16_32_Q15(C[1],MULT16_16_16(plc_tuning,-327),C[1]);
C[2] = MAC16_32_Q15(C[2],MULT16_16_16(plc_tuning,-327),C[2]);
#else
C[0]*=1-.01*plc_tuning;
C[1]*=1-.01*plc_tuning;
C[2]*=1-.01*plc_tuning;
C[6]*=.5*(1+.01*plc_tuning);
C[7]*=.5*(1+.01*plc_tuning);
C[8]*=.5*(1+.01*plc_tuning);
#endif
for (i=0; i<gain_cdbk_size; i++)
{
spx_word32_t sum=0;
spx_word16_t g0,g1,g2;
spx_word16_t pitch_control=64;
spx_word16_t gain_sum;
ptr = gain_cdbk+3*i;
g0=ADD16((spx_word16_t)ptr[0],32);
g1=ADD16((spx_word16_t)ptr[1],32);
g2=ADD16((spx_word16_t)ptr[2],32);
gain_sum = g1;
if (g0>0)
gain_sum += g0;
if (g2>0)
gain_sum += g2;
if (gain_sum > 64)
{
gain_sum = SUB16(gain_sum, 64);
if (gain_sum > 127)
gain_sum = 127;
#ifdef FIXED_POINT
pitch_control = SUB16(64,EXTRACT16(PSHR32(MULT16_16(64,MULT16_16_16(plc_tuning, gain_sum)),10)));
#else
pitch_control = 64*(1.-.001*plc_tuning*gain_sum);
#endif
if (pitch_control < 0)
pitch_control = 0;
}
sum = ADD32(sum,MULT16_32_Q14(MULT16_16_16(g0,pitch_control),C[0]));
sum = ADD32(sum,MULT16_32_Q14(MULT16_16_16(g1,pitch_control),C[1]));
sum = ADD32(sum,MULT16_32_Q14(MULT16_16_16(g2,pitch_control),C[2]));
sum = SUB32(sum,MULT16_32_Q14(MULT16_16_16(g0,g1),C[3]));
sum = SUB32(sum,MULT16_32_Q14(MULT16_16_16(g2,g1),C[4]));
sum = SUB32(sum,MULT16_32_Q14(MULT16_16_16(g2,g0),C[5]));
sum = SUB32(sum,MULT16_32_Q15(MULT16_16_16(g0,g0),C[6]));
sum = SUB32(sum,MULT16_32_Q15(MULT16_16_16(g1,g1),C[7]));
sum = SUB32(sum,MULT16_32_Q15(MULT16_16_16(g2,g2),C[8]));
/* We could force "safe" pitch values to handle packet loss better */
if (sum>best_sum || i==0)
{
best_sum=sum;
best_cdbk=i;
}
}
#ifdef FIXED_POINT
gain[0] = ADD16(32,(spx_word16_t)gain_cdbk[best_cdbk*3]);
gain[1] = ADD16(32,(spx_word16_t)gain_cdbk[best_cdbk*3+1]);
gain[2] = ADD16(32,(spx_word16_t)gain_cdbk[best_cdbk*3+2]);
/*printf ("%d %d %d %d\n",gain[0],gain[1],gain[2], best_cdbk);*/
#else
gain[0] = 0.015625*gain_cdbk[best_cdbk*3] + .5;
gain[1] = 0.015625*gain_cdbk[best_cdbk*3+1]+ .5;
gain[2] = 0.015625*gain_cdbk[best_cdbk*3+2]+ .5;
#endif
*cdbk_index=best_cdbk;
}
#ifdef FIXED_POINT
for (i=0; i<nsf; i++)
exc[i]=SHL32(ADD32(ADD32(MULT16_32_Q15(SHL16(gain[0],7),e[2][i]), MULT16_32_Q15(SHL16(gain[1],7),e[1][i])),
MULT16_32_Q15(SHL16(gain[2],7),e[0][i])), 2);
err=0;
for (i=0; i<nsf; i++)
{
spx_word16_t perr2;
spx_sig_t tmp = SHL32(ADD32(ADD32(MULT16_32_Q15(SHL16(gain[0],7),x[2][i]),MULT16_32_Q15(SHL16(gain[1],7),x[1][i])),
MULT16_32_Q15(SHL16(gain[2],7),x[0][i])),2);
spx_sig_t perr=SUB32(target[i],tmp);
new_target[i] = SUB32(target[i], tmp);
perr2 = EXTRACT16(PSHR32(perr,15));
err = ADD64(err,MULT16_16(perr2,perr2));
}
#else
for (i=0; i<nsf; i++)
exc[i]=gain[0]*e[2][i]+gain[1]*e[1][i]+gain[2]*e[0][i];
err=0;
for (i=0; i<nsf; i++)
{
spx_sig_t tmp = gain[2]*x[0][i]+gain[1]*x[1][i]+gain[0]*x[2][i];
new_target[i] = target[i] - tmp;
err+=new_target[i]*new_target[i];
}
#endif
return err;
}
void split_cb_search_shape_sign(
spx_word16_t target[], /* target vector */
spx_coef_t ak[], /* LPCs for this subframe */
spx_coef_t awk1[], /* Weighted LPCs for this subframe */
spx_coef_t awk2[], /* Weighted LPCs for this subframe */
const void *par, /* Codebook/search parameters*/
int p, /* number of LPC coeffs */
int nsf, /* number of samples in subframe */
spx_sig_t *exc,
spx_word16_t *r,
SpeexBits *bits,
char *stack,
int complexity,
int update_target
)
{
int i,j,k,m,n,q;
VARDECL(spx_word16_t *resp);
#ifdef _USE_SSE
VARDECL(__m128 *resp2);
VARDECL(__m128 *E);
#else
spx_word16_t *resp2;
VARDECL(spx_word32_t *E);
#endif
VARDECL(spx_word16_t *t);
VARDECL(spx_sig_t *e);
VARDECL(spx_word16_t *tmp);
VARDECL(spx_word32_t *ndist);
VARDECL(spx_word32_t *odist);
VARDECL(int *itmp);
VARDECL(spx_word16_t **ot2);
VARDECL(spx_word16_t **nt2);
spx_word16_t **ot, **nt;
VARDECL(int **nind);
VARDECL(int **oind);
VARDECL(int *ind);
const signed char *shape_cb;
int shape_cb_size, subvect_size, nb_subvect;
const split_cb_params *params;
int N=2;
VARDECL(int *best_index);
VARDECL(spx_word32_t *best_dist);
VARDECL(int *best_nind);
VARDECL(int *best_ntarget);
int have_sign;
N=complexity;
if (N>10)
N=10;
/* Complexity isn't as important for the codebooks as it is for the pitch */
N=(2*N)/3;
if (N<1)
N=1;
if (N==1)
{
split_cb_search_shape_sign_N1(target,ak,awk1,awk2,par,p,nsf,exc,r,bits,stack,update_target);
return;
}
ALLOC(ot2, N, spx_word16_t*);
ALLOC(nt2, N, spx_word16_t*);
ALLOC(oind, N, int*);
ALLOC(nind, N, int*);
params = (const split_cb_params *) par;
subvect_size = params->subvect_size;
nb_subvect = params->nb_subvect;
shape_cb_size = 1<<params->shape_bits;
shape_cb = params->shape_cb;
have_sign = params->have_sign;
ALLOC(resp, shape_cb_size*subvect_size, spx_word16_t);
#ifdef _USE_SSE
ALLOC(resp2, (shape_cb_size*subvect_size)>>2, __m128);
ALLOC(E, shape_cb_size>>2, __m128);
#else
resp2 = resp;
ALLOC(E, shape_cb_size, spx_word32_t);
#endif
ALLOC(t, nsf, spx_word16_t);
ALLOC(e, nsf, spx_sig_t);
ALLOC(ind, nb_subvect, int);
ALLOC(tmp, 2*N*nsf, spx_word16_t);
for (i=0;i<N;i++)
{
ot2[i]=tmp+2*i*nsf;
nt2[i]=tmp+(2*i+1)*nsf;
}
ot=ot2;
nt=nt2;
ALLOC(best_index, N, int);
ALLOC(best_dist, N, spx_word32_t);
ALLOC(best_nind, N, int);
ALLOC(best_ntarget, N, int);
ALLOC(ndist, N, spx_word32_t);
ALLOC(odist, N, spx_word32_t);
ALLOC(itmp, 2*N*nb_subvect, int);
for (i=0;i<N;i++)
{
nind[i]=itmp+2*i*nb_subvect;
oind[i]=itmp+(2*i+1)*nb_subvect;
}
SPEEX_COPY(t, target, nsf);
for (j=0;j<N;j++)
SPEEX_COPY(&ot[j][0], t, nsf);
/* Pre-compute codewords response and energy */
compute_weighted_codebook(shape_cb, r, resp, resp2, E, shape_cb_size, subvect_size, stack);
for (j=0;j<N;j++)
odist[j]=0;
/*For all subvectors*/
for (i=0;i<nb_subvect;i++)
{
/*"erase" nbest list*/
for (j=0;j<N;j++)
ndist[j]=VERY_LARGE32;
/* This is not strictly necessary, but it provides an additonal safety
to prevent crashes in case something goes wrong in the previous
steps (e.g. NaNs) */
for (j=0;j<N;j++)
best_nind[j] = best_ntarget[j] = 0;
/*For all n-bests of previous subvector*/
for (j=0;j<N;j++)
{
spx_word16_t *x=ot[j]+subvect_size*i;
spx_word32_t tener = 0;
for (m=0;m<subvect_size;m++)
tener = MAC16_16(tener, x[m],x[m]);
#ifdef FIXED_POINT
tener = SHR32(tener,1);
#else
tener *= .5;
#endif
/*Find new n-best based on previous n-best j*/
#ifndef DISABLE_WIDEBAND
if (have_sign)
vq_nbest_sign(x, resp2, subvect_size, shape_cb_size, E, N, best_index, best_dist, stack);
else
#endif /* DISABLE_WIDEBAND */
vq_nbest(x, resp2, subvect_size, shape_cb_size, E, N, best_index, best_dist, stack);
/*For all new n-bests*/
for (k=0;k<N;k++)
{
/* Compute total distance (including previous sub-vectors */
spx_word32_t err = ADD32(ADD32(odist[j],best_dist[k]),tener);
/*update n-best list*/
if (err<ndist[N-1])
{
for (m=0;m<N;m++)
{
if (err < ndist[m])
{
for (n=N-1;n>m;n--)
{
ndist[n] = ndist[n-1];
best_nind[n] = best_nind[n-1];
best_ntarget[n] = best_ntarget[n-1];
}
/* n is equal to m here, so they're interchangeable */
ndist[m] = err;
best_nind[n] = best_index[k];
best_ntarget[n] = j;
break;
}
}
}
}
if (i==0)
break;
}
for (j=0;j<N;j++)
{
/*previous target (we don't care what happened before*/
for (m=(i+1)*subvect_size;m<nsf;m++)
nt[j][m]=ot[best_ntarget[j]][m];
/* New code: update the rest of the target only if it's worth it */
for (m=0;m<subvect_size;m++)
{
spx_word16_t g;
int rind;
spx_word16_t sign=1;
rind = best_nind[j];
if (rind>=shape_cb_size)
{
sign=-1;
rind-=shape_cb_size;
}
q=subvect_size-m;
#ifdef FIXED_POINT
g=sign*shape_cb[rind*subvect_size+m];
#else
g=sign*0.03125*shape_cb[rind*subvect_size+m];
#endif
target_update(nt[j]+subvect_size*(i+1), g, r+q, nsf-subvect_size*(i+1));
}
for (q=0;q<nb_subvect;q++)
nind[j][q]=oind[best_ntarget[j]][q];
nind[j][i]=best_nind[j];
}
/*update old-new data*/
/* just swap pointers instead of a long copy */
{
spx_word16_t **tmp2;
tmp2=ot;
ot=nt;
nt=tmp2;
}
for (j=0;j<N;j++)
for (m=0;m<nb_subvect;m++)
oind[j][m]=nind[j][m];
for (j=0;j<N;j++)
odist[j]=ndist[j];
}
/*save indices*/
for (i=0;i<nb_subvect;i++)
{
ind[i]=nind[0][i];
speex_bits_pack(bits,ind[i],params->shape_bits+have_sign);
}
/* Put everything back together */
for (i=0;i<nb_subvect;i++)
{
int rind;
spx_word16_t sign=1;
rind = ind[i];
if (rind>=shape_cb_size)
{
sign=-1;
rind-=shape_cb_size;
}
#ifdef FIXED_POINT
if (sign==1)
{
for (j=0;j<subvect_size;j++)
e[subvect_size*i+j]=SHL32(EXTEND32(shape_cb[rind*subvect_size+j]),SIG_SHIFT-5);
} else {
for (j=0;j<subvect_size;j++)
e[subvect_size*i+j]=NEG32(SHL32(EXTEND32(shape_cb[rind*subvect_size+j]),SIG_SHIFT-5));
}
#else
for (j=0;j<subvect_size;j++)
e[subvect_size*i+j]=sign*0.03125*shape_cb[rind*subvect_size+j];
#endif
}
/* Update excitation */
for (j=0;j<nsf;j++)
exc[j]=ADD32(exc[j],e[j]);
/* Update target: only update target if necessary */
if (update_target)
{
VARDECL(spx_word16_t *r2);
ALLOC(r2, nsf, spx_word16_t);
for (j=0;j<nsf;j++)
r2[j] = EXTRACT16(PSHR32(e[j] ,6));
syn_percep_zero16(r2, ak, awk1, awk2, r2, nsf,p, stack);
for (j=0;j<nsf;j++)
target[j]=SUB16(target[j],PSHR16(r2[j],2));
}
}
static void split_cb_search_shape_sign_N1(
spx_word16_t target[], /* target vector */
spx_coef_t ak[], /* LPCs for this subframe */
spx_coef_t awk1[], /* Weighted LPCs for this subframe */
spx_coef_t awk2[], /* Weighted LPCs for this subframe */
const void *par, /* Codebook/search parameters*/
int p, /* number of LPC coeffs */
int nsf, /* number of samples in subframe */
spx_sig_t *exc,
spx_word16_t *r,
SpeexBits *bits,
char *stack,
int update_target
)
{
int i,j,m,q;
VARDECL(spx_word16_t *resp);
#ifdef _USE_SSE
VARDECL(__m128 *resp2);
VARDECL(__m128 *E);
#else
spx_word16_t *resp2;
VARDECL(spx_word32_t *E);
#endif
VARDECL(spx_word16_t *t);
VARDECL(spx_sig_t *e);
const signed char *shape_cb;
int shape_cb_size, subvect_size, nb_subvect;
const split_cb_params *params;
int best_index;
spx_word32_t best_dist;
int have_sign;
params = (const split_cb_params *) par;
subvect_size = params->subvect_size;
nb_subvect = params->nb_subvect;
shape_cb_size = 1<<params->shape_bits;
shape_cb = params->shape_cb;
have_sign = params->have_sign;
ALLOC(resp, shape_cb_size*subvect_size, spx_word16_t);
#ifdef _USE_SSE
ALLOC(resp2, (shape_cb_size*subvect_size)>>2, __m128);
ALLOC(E, shape_cb_size>>2, __m128);
#else
resp2 = resp;
ALLOC(E, shape_cb_size, spx_word32_t);
#endif
ALLOC(t, nsf, spx_word16_t);
ALLOC(e, nsf, spx_sig_t);
/* FIXME: Do we still need to copy the target? */
SPEEX_COPY(t, target, nsf);
compute_weighted_codebook(shape_cb, r, resp, resp2, E, shape_cb_size, subvect_size, stack);
for (i=0;i<nb_subvect;i++)
{
spx_word16_t *x=t+subvect_size*i;
/*Find new n-best based on previous n-best j*/
#ifndef DISABLE_WIDEBAND
if (have_sign)
vq_nbest_sign(x, resp2, subvect_size, shape_cb_size, E, 1, &best_index, &best_dist, stack);
else
#endif /* DISABLE_WIDEBAND */
vq_nbest(x, resp2, subvect_size, shape_cb_size, E, 1, &best_index, &best_dist, stack);
speex_bits_pack(bits,best_index,params->shape_bits+have_sign);
{
int rind;
spx_word16_t *res;
spx_word16_t sign=1;
rind = best_index;
if (rind>=shape_cb_size)
{
sign=-1;
rind-=shape_cb_size;
}
res = resp+rind*subvect_size;
if (sign>0)
for (m=0;m<subvect_size;m++)
t[subvect_size*i+m] = SUB16(t[subvect_size*i+m], res[m]);
else
for (m=0;m<subvect_size;m++)
t[subvect_size*i+m] = ADD16(t[subvect_size*i+m], res[m]);
#ifdef FIXED_POINT
if (sign==1)
{
for (j=0;j<subvect_size;j++)
e[subvect_size*i+j]=SHL32(EXTEND32(shape_cb[rind*subvect_size+j]),SIG_SHIFT-5);
} else {
for (j=0;j<subvect_size;j++)
e[subvect_size*i+j]=NEG32(SHL32(EXTEND32(shape_cb[rind*subvect_size+j]),SIG_SHIFT-5));
}
#else
for (j=0;j<subvect_size;j++)
e[subvect_size*i+j]=sign*0.03125*shape_cb[rind*subvect_size+j];
#endif
}
for (m=0;m<subvect_size;m++)
{
spx_word16_t g;
int rind;
spx_word16_t sign=1;
rind = best_index;
if (rind>=shape_cb_size)
{
sign=-1;
rind-=shape_cb_size;
}
q=subvect_size-m;
#ifdef FIXED_POINT
g=sign*shape_cb[rind*subvect_size+m];
#else
g=sign*0.03125*shape_cb[rind*subvect_size+m];
#endif
target_update(t+subvect_size*(i+1), g, r+q, nsf-subvect_size*(i+1));
}
}
/* Update excitation */
/* FIXME: We could update the excitation directly above */
for (j=0;j<nsf;j++)
exc[j]=ADD32(exc[j],e[j]);
/* Update target: only update target if necessary */
if (update_target)
{
VARDECL(spx_word16_t *r2);
ALLOC(r2, nsf, spx_word16_t);
for (j=0;j<nsf;j++)
r2[j] = EXTRACT16(PSHR32(e[j] ,6));
syn_percep_zero16(r2, ak, awk1, awk2, r2, nsf,p, stack);
for (j=0;j<nsf;j++)
target[j]=SUB16(target[j],PSHR16(r2[j],2));
}
}
FilterBank *filterbank_new(int banks, spx_word32_t sampling, int len, int type)
{
FilterBank *bank;
spx_word32_t df;
spx_word32_t max_mel, mel_interval;
int i;
int id1;
int id2;
df = DIV32(SHL32(sampling,15),MULT16_16(2,len));
max_mel = toBARK(EXTRACT16(sampling/2));
mel_interval = PDIV32(max_mel,banks-1);
bank = (FilterBank*)speex_alloc(sizeof(FilterBank));
bank->nb_banks = banks;
bank->len = len;
bank->bank_left = (int*)speex_alloc(len*sizeof(int));
bank->bank_right = (int*)speex_alloc(len*sizeof(int));
bank->filter_left = (spx_word16_t*)speex_alloc(len*sizeof(spx_word16_t));
bank->filter_right = (spx_word16_t*)speex_alloc(len*sizeof(spx_word16_t));
/* Think I can safely disable normalisation that for fixed-point (and probably float as well) */
#ifndef FIXED_POINT
bank->scaling = (float*)speex_alloc(banks*sizeof(float));
#endif
for (i=0;i<len;i++)
{
spx_word16_t curr_freq;
spx_word32_t mel;
spx_word16_t val;
curr_freq = EXTRACT16(MULT16_32_P15(i,df));
mel = toBARK(curr_freq);
if (mel > max_mel)
break;
#ifdef FIXED_POINT
id1 = DIV32(mel,mel_interval);
#else
id1 = (int)(floor(mel/mel_interval));
#endif
if (id1>banks-2)
{
id1 = banks-2;
val = Q15_ONE;
} else {
val = DIV32_16(mel - id1*mel_interval,EXTRACT16(PSHR32(mel_interval,15)));
}
id2 = id1+1;
bank->bank_left[i] = id1;
bank->filter_left[i] = SUB16(Q15_ONE,val);
bank->bank_right[i] = id2;
bank->filter_right[i] = val;
}
/* Think I can safely disable normalisation for fixed-point (and probably float as well) */
#ifndef FIXED_POINT
for (i=0;i<bank->nb_banks;i++)
bank->scaling[i] = 0;
for (i=0;i<bank->len;i++)
{
int id = bank->bank_left[i];
bank->scaling[id] += bank->filter_left[i];
id = bank->bank_right[i];
bank->scaling[id] += bank->filter_right[i];
}
for (i=0;i<bank->nb_banks;i++)
bank->scaling[i] = Q15_ONE/(bank->scaling[i]);
#endif
return bank;
}
static int quant_coarse_energy_impl(const CELTMode *m, int start, int end,
const opus_val16 *eBands, opus_val16 *oldEBands,
opus_int32 budget, opus_int32 tell,
const unsigned char *prob_model, opus_val16 *error, ec_enc *enc,
int C, int LM, int intra, opus_val16 max_decay)
{
int i, c;
int badness = 0;
opus_val32 prev[2] = {0,0};
opus_val16 coef;
opus_val16 beta;
if (tell+3 <= budget)
ec_enc_bit_logp(enc, intra, 3);
if (intra)
{
coef = 0;
beta = beta_intra;
} else {
beta = beta_coef[LM];
coef = pred_coef[LM];
}
/* Encode at a fixed coarse resolution */
for (i=start;i<end;i++)
{
c=0;
do {
int bits_left;
int qi, qi0;
opus_val32 q;
opus_val16 x;
opus_val32 f, tmp;
opus_val16 oldE;
opus_val16 decay_bound;
x = eBands[i+c*m->nbEBands];
oldE = MAX16(-QCONST16(9.f,DB_SHIFT), oldEBands[i+c*m->nbEBands]);
#ifdef FIXED_POINT
f = SHL32(EXTEND32(x),7) - PSHR32(MULT16_16(coef,oldE), 8) - prev[c];
/* Rounding to nearest integer here is really important! */
qi = (f+QCONST32(.5f,DB_SHIFT+7))>>(DB_SHIFT+7);
decay_bound = EXTRACT16(MAX32(-QCONST16(28.f,DB_SHIFT),
SUB32((opus_val32)oldEBands[i+c*m->nbEBands],max_decay)));
#else
f = x-coef*oldE-prev[c];
/* Rounding to nearest integer here is really important! */
qi = (int)floor(.5f+f);
decay_bound = MAX16(-QCONST16(28.f,DB_SHIFT), oldEBands[i+c*m->nbEBands]) - max_decay;
#endif
/* Prevent the energy from going down too quickly (e.g. for bands
that have just one bin) */
if (qi < 0 && x < decay_bound)
{
qi += (int)SHR16(SUB16(decay_bound,x), DB_SHIFT);
if (qi > 0)
qi = 0;
}
qi0 = qi;
/* If we don't have enough bits to encode all the energy, just assume
something safe. */
tell = ec_tell(enc);
bits_left = budget-tell-3*C*(end-i);
if (i!=start && bits_left < 30)
{
if (bits_left < 24)
qi = IMIN(1, qi);
if (bits_left < 16)
qi = IMAX(-1, qi);
}
if (budget-tell >= 15)
{
int pi;
pi = 2*IMIN(i,20);
ec_laplace_encode(enc, &qi,
prob_model[pi]<<7, prob_model[pi+1]<<6);
}
else if(budget-tell >= 2)
{
qi = IMAX(-1, IMIN(qi, 1));
ec_enc_icdf(enc, 2*qi^-(qi<0), small_energy_icdf, 2);
}
else if(budget-tell >= 1)
{
qi = IMIN(0, qi);
ec_enc_bit_logp(enc, -qi, 1);
}
else
qi = -1;
error[i+c*m->nbEBands] = PSHR32(f,7) - SHL16(qi,DB_SHIFT);
badness += abs(qi0-qi);
q = (opus_val32)SHL32(EXTEND32(qi),DB_SHIFT);
tmp = PSHR32(MULT16_16(coef,oldE),8) + prev[c] + SHL32(q,7);
#ifdef FIXED_POINT
tmp = MAX32(-QCONST32(28.f, DB_SHIFT+7), tmp);
#endif
oldEBands[i+c*m->nbEBands] = PSHR32(tmp, 7);
prev[c] = prev[c] + SHL32(q,7) - MULT16_16(beta,PSHR32(q,8));
} while (++c < C);
}
return badness;
}
/** Finds the best quantized 3-tap pitch predictor by analysis by synthesis */
static spx_word32_t pitch_gain_search_3tap(
const spx_word16_t target[], /* Target vector */
const spx_coef_t ak[], /* LPCs for this subframe */
const spx_coef_t awk1[], /* Weighted LPCs #1 for this subframe */
const spx_coef_t awk2[], /* Weighted LPCs #2 for this subframe */
spx_sig_t exc[], /* Excitation */
const signed char* gain_cdbk,
int gain_cdbk_size,
int pitch, /* Pitch value */
int p, /* Number of LPC coeffs */
int nsf, /* Number of samples in subframe */
SpeexBits* bits,
char* stack,
const spx_word16_t* exc2,
const spx_word16_t* r,
spx_word16_t* new_target,
int* cdbk_index,
int plc_tuning,
spx_word32_t cumul_gain,
int scaledown
) {
int i, j;
VARDECL(spx_word16_t * tmp1);
VARDECL(spx_word16_t * e);
spx_word16_t* x[3];
spx_word32_t corr[3];
spx_word32_t A[3][3];
spx_word16_t gain[3];
spx_word32_t err;
spx_word16_t max_gain = 128;
int best_cdbk = 0;
ALLOC(tmp1, 3 * nsf, spx_word16_t);
ALLOC(e, nsf, spx_word16_t);
if (cumul_gain > 262144)
max_gain = 31;
x[0] = tmp1;
x[1] = tmp1 + nsf;
x[2] = tmp1 + 2 * nsf;
for (j = 0; j < nsf; j++)
new_target[j] = target[j];
{
VARDECL(spx_mem_t * mm);
int pp = pitch - 1;
ALLOC(mm, p, spx_mem_t);
for (j = 0; j < nsf; j++) {
if (j - pp < 0)
e[j] = exc2[j - pp];
else if (j - pp - pitch < 0)
e[j] = exc2[j - pp - pitch];
else
e[j] = 0;
}
#ifdef FIXED_POINT
/* Scale target and excitation down if needed (avoiding overflow) */
if (scaledown) {
for (j = 0; j < nsf; j++)
e[j] = SHR16(e[j], 1);
for (j = 0; j < nsf; j++)
new_target[j] = SHR16(new_target[j], 1);
}
#endif
for (j = 0; j < p; j++)
mm[j] = 0;
iir_mem16(e, ak, e, nsf, p, mm, stack);
for (j = 0; j < p; j++)
mm[j] = 0;
filter_mem16(e, awk1, awk2, e, nsf, p, mm, stack);
for (j = 0; j < nsf; j++)
x[2][j] = e[j];
}
for (i = 1; i >= 0; i--) {
spx_word16_t e0 = exc2[-pitch - 1 + i];
#ifdef FIXED_POINT
/* Scale excitation down if needed (avoiding overflow) */
if (scaledown)
e0 = SHR16(e0, 1);
#endif
x[i][0] = MULT16_16_Q14(r[0], e0);
for (j = 0; j < nsf - 1; j++)
x[i][j + 1] = ADD32(x[i + 1][j], MULT16_16_P14(r[j + 1], e0));
}
for (i = 0; i < 3; i++)
corr[i] = inner_prod(x[i], new_target, nsf);
for (i = 0; i < 3; i++)
for (j = 0; j <= i; j++)
A[i][j] = A[j][i] = inner_prod(x[i], x[j], nsf);
{
spx_word32_t C[9];
#ifdef FIXED_POINT
spx_word16_t C16[9];
#else
spx_word16_t* C16 = C;
#endif
C[0] = corr[2];
C[1] = corr[1];
C[2] = corr[0];
C[3] = A[1][2];
C[4] = A[0][1];
C[5] = A[0][2];
C[6] = A[2][2];
C[7] = A[1][1];
C[8] = A[0][0];
/*plc_tuning *= 2;*/
if (plc_tuning < 2)
plc_tuning = 2;
if (plc_tuning > 30)
plc_tuning = 30;
#ifdef FIXED_POINT
C[0] = SHL32(C[0], 1);
C[1] = SHL32(C[1], 1);
C[2] = SHL32(C[2], 1);
C[3] = SHL32(C[3], 1);
C[4] = SHL32(C[4], 1);
C[5] = SHL32(C[5], 1);
C[6] = MAC16_32_Q15(C[6], MULT16_16_16(plc_tuning, 655), C[6]);
C[7] = MAC16_32_Q15(C[7], MULT16_16_16(plc_tuning, 655), C[7]);
C[8] = MAC16_32_Q15(C[8], MULT16_16_16(plc_tuning, 655), C[8]);
normalize16(C, C16, 32767, 9);
#else
C[6] *= .5 * (1 + .02 * plc_tuning);
C[7] *= .5 * (1 + .02 * plc_tuning);
C[8] *= .5 * (1 + .02 * plc_tuning);
#endif
best_cdbk = pitch_gain_search_3tap_vq(gain_cdbk, gain_cdbk_size, C16, max_gain);
#ifdef FIXED_POINT
gain[0] = ADD16(32, (spx_word16_t)gain_cdbk[best_cdbk * 4]);
gain[1] = ADD16(32, (spx_word16_t)gain_cdbk[best_cdbk * 4 + 1]);
gain[2] = ADD16(32, (spx_word16_t)gain_cdbk[best_cdbk * 4 + 2]);
/*printf ("%d %d %d %d\n",gain[0],gain[1],gain[2], best_cdbk);*/
#else
gain[0] = 0.015625 * gain_cdbk[best_cdbk * 4] + .5;
gain[1] = 0.015625 * gain_cdbk[best_cdbk * 4 + 1] + .5;
gain[2] = 0.015625 * gain_cdbk[best_cdbk * 4 + 2] + .5;
#endif
*cdbk_index = best_cdbk;
}
SPEEX_MEMSET(exc, 0, nsf);
for (i = 0; i < 3; i++) {
int j;
int tmp1, tmp3;
int pp = pitch + 1 - i;
tmp1 = nsf;
if (tmp1 > pp)
tmp1 = pp;
for (j = 0; j < tmp1; j++)
exc[j] = MAC16_16(exc[j], SHL16(gain[2 - i], 7), exc2[j - pp]);
tmp3 = nsf;
if (tmp3 > pp + pitch)
tmp3 = pp + pitch;
for (j = tmp1; j < tmp3; j++)
exc[j] = MAC16_16(exc[j], SHL16(gain[2 - i], 7), exc2[j - pp - pitch]);
}
for (i = 0; i < nsf; i++) {
spx_word32_t tmp = ADD32(ADD32(MULT16_16(gain[0], x[2][i]), MULT16_16(gain[1], x[1][i])),
MULT16_16(gain[2], x[0][i]));
new_target[i] = SUB16(new_target[i], EXTRACT16(PSHR32(tmp, 6)));
}
err = inner_prod(new_target, new_target, nsf);
return err;
}
int sb_encode(void *state, void *vin, SpeexBits *bits)
{
SBEncState *st;
int i, roots, sub;
char *stack;
VARDECL(spx_mem_t *mem);
VARDECL(spx_sig_t *innov);
VARDECL(spx_word16_t *target);
VARDECL(spx_word16_t *syn_resp);
VARDECL(spx_word32_t *low_pi_gain);
spx_word16_t *low;
spx_word16_t *high;
VARDECL(spx_word16_t *low_exc_rms);
VARDECL(spx_word16_t *low_innov_rms);
const SpeexSBMode *mode;
spx_int32_t dtx;
spx_word16_t *in = (spx_word16_t*)vin;
spx_word16_t e_low=0, e_high=0;
VARDECL(spx_coef_t *lpc);
VARDECL(spx_coef_t *interp_lpc);
VARDECL(spx_coef_t *bw_lpc1);
VARDECL(spx_coef_t *bw_lpc2);
VARDECL(spx_lsp_t *lsp);
VARDECL(spx_lsp_t *qlsp);
VARDECL(spx_lsp_t *interp_lsp);
VARDECL(spx_lsp_t *interp_qlsp);
st = (SBEncState*)state;
stack=st->stack;
mode = (const SpeexSBMode*)(st->mode->mode);
low = in;
high = in+st->frame_size;
/* High-band buffering / sync with low band */
/* Compute the two sub-bands by filtering with QMF h0*/
qmf_decomp(in, h0, low, high, st->full_frame_size, QMF_ORDER, st->h0_mem, stack);
#ifndef DISABLE_VBR
if (st->vbr_enabled || st->vad_enabled)
{
/* Need to compute things here before the signal is trashed by the encoder */
/*FIXME: Are the two signals (low, high) in sync? */
e_low = compute_rms16(low, st->frame_size);
e_high = compute_rms16(high, st->frame_size);
}
#endif /* #ifndef DISABLE_VBR */
ALLOC(low_innov_rms, st->nbSubframes, spx_word16_t);
speex_encoder_ctl(st->st_low, SPEEX_SET_INNOVATION_SAVE, low_innov_rms);
/* Encode the narrowband part*/
speex_encode_native(st->st_low, low, bits);
high = high - (st->windowSize-st->frame_size);
SPEEX_COPY(high, st->high, st->windowSize-st->frame_size);
SPEEX_COPY(st->high, &high[st->frame_size], st->windowSize-st->frame_size);
ALLOC(low_pi_gain, st->nbSubframes, spx_word32_t);
ALLOC(low_exc_rms, st->nbSubframes, spx_word16_t);
speex_encoder_ctl(st->st_low, SPEEX_GET_PI_GAIN, low_pi_gain);
speex_encoder_ctl(st->st_low, SPEEX_GET_EXC, low_exc_rms);
speex_encoder_ctl(st->st_low, SPEEX_GET_LOW_MODE, &dtx);
if (dtx==0)
dtx=1;
else
dtx=0;
ALLOC(lpc, st->lpcSize, spx_coef_t);
ALLOC(interp_lpc, st->lpcSize, spx_coef_t);
ALLOC(bw_lpc1, st->lpcSize, spx_coef_t);
ALLOC(bw_lpc2, st->lpcSize, spx_coef_t);
ALLOC(lsp, st->lpcSize, spx_lsp_t);
ALLOC(qlsp, st->lpcSize, spx_lsp_t);
ALLOC(interp_lsp, st->lpcSize, spx_lsp_t);
ALLOC(interp_qlsp, st->lpcSize, spx_lsp_t);
{
VARDECL(spx_word16_t *autocorr);
VARDECL(spx_word16_t *w_sig);
ALLOC(autocorr, st->lpcSize+1, spx_word16_t);
ALLOC(w_sig, st->windowSize, spx_word16_t);
/* Window for analysis */
/* FIXME: This is a kludge */
if (st->subframeSize==80)
{
for (i=0;i<st->windowSize;i++)
w_sig[i] = EXTRACT16(SHR32(MULT16_16(high[i],st->window[i>>1]),SIG_SHIFT));
} else {
for (i=0;i<st->windowSize;i++)
w_sig[i] = EXTRACT16(SHR32(MULT16_16(high[i],st->window[i]),SIG_SHIFT));
}
/* Compute auto-correlation */
_spx_autocorr(w_sig, autocorr, st->lpcSize+1, st->windowSize);
autocorr[0] = ADD16(autocorr[0],MULT16_16_Q15(autocorr[0],st->lpc_floor)); /* Noise floor in auto-correlation domain */
/* Lag windowing: equivalent to filtering in the power-spectrum domain */
for (i=0;i<st->lpcSize+1;i++)
autocorr[i] = MULT16_16_Q14(autocorr[i],st->lagWindow[i]);
/* Levinson-Durbin */
_spx_lpc(lpc, autocorr, st->lpcSize);
}
void split_cb_search_shape_sign(
spx_sig_t target[], /* target vector */
spx_coef_t ak[], /* LPCs for this subframe */
spx_coef_t awk1[], /* Weighted LPCs for this subframe */
spx_coef_t awk2[], /* Weighted LPCs for this subframe */
const void *par, /* Codebook/search parameters*/
int p, /* number of LPC coeffs */
int nsf, /* number of samples in subframe */
spx_sig_t *exc,
spx_word16_t *r,
SpeexBits *bits,
char *stack,
int complexity,
int update_target
)
{
int i,j,k,m,n,q;
VARDECL(spx_word16_t *resp);
#ifdef _USE_SSE
VARDECL(__m128 *resp2);
VARDECL(__m128 *E);
#else
spx_word16_t *resp2;
VARDECL(spx_word32_t *E);
#endif
VARDECL(spx_word16_t *t);
VARDECL(spx_sig_t *e);
VARDECL(spx_sig_t *r2);
VARDECL(spx_word16_t *tmp);
VARDECL(spx_word32_t *ndist);
VARDECL(spx_word32_t *odist);
VARDECL(int *itmp);
VARDECL(spx_word16_t **ot2);
VARDECL(spx_word16_t **nt2);
spx_word16_t **ot, **nt;
VARDECL(int **nind);
VARDECL(int **oind);
VARDECL(int *ind);
const signed char *shape_cb;
int shape_cb_size, subvect_size, nb_subvect;
const split_cb_params *params;
int N=2;
VARDECL(int *best_index);
VARDECL(spx_word32_t *best_dist);
int have_sign;
N=complexity;
if (N>10)
N=10;
if (N<1)
N=1;
if (N==1)
{
split_cb_search_shape_sign_N1(target,ak,awk1,awk2,par,p,nsf,exc,r,bits,stack,complexity,update_target);
return;
}
ALLOC(ot2, N, spx_word16_t*);
ALLOC(nt2, N, spx_word16_t*);
ALLOC(oind, N, int*);
ALLOC(nind, N, int*);
params = (const split_cb_params *) par;
subvect_size = params->subvect_size;
nb_subvect = params->nb_subvect;
shape_cb_size = 1<<params->shape_bits;
shape_cb = params->shape_cb;
have_sign = params->have_sign;
ALLOC(resp, shape_cb_size*subvect_size, spx_word16_t);
#ifdef _USE_SSE
ALLOC(resp2, (shape_cb_size*subvect_size)>>2, __m128);
ALLOC(E, shape_cb_size>>2, __m128);
#else
resp2 = resp;
ALLOC(E, shape_cb_size, spx_word32_t);
#endif
ALLOC(t, nsf, spx_word16_t);
ALLOC(e, nsf, spx_sig_t);
ALLOC(r2, nsf, spx_sig_t);
ALLOC(ind, nb_subvect, int);
ALLOC(tmp, 2*N*nsf, spx_word16_t);
for (i=0;i<N;i++)
{
ot2[i]=tmp+2*i*nsf;
nt2[i]=tmp+(2*i+1)*nsf;
}
ot=ot2;
nt=nt2;
ALLOC(best_index, N, int);
ALLOC(best_dist, N, spx_word32_t);
ALLOC(ndist, N, spx_word32_t);
ALLOC(odist, N, spx_word32_t);
ALLOC(itmp, 2*N*nb_subvect, int);
for (i=0;i<N;i++)
{
nind[i]=itmp+2*i*nb_subvect;
oind[i]=itmp+(2*i+1)*nb_subvect;
for (j=0;j<nb_subvect;j++)
nind[i][j]=oind[i][j]=-1;
}
/* FIXME: make that adaptive? */
for (i=0;i<nsf;i++)
t[i]=EXTRACT16(PSHR32(target[i],6));
for (j=0;j<N;j++)
for (i=0;i<nsf;i++)
ot[j][i]=t[i];
/*for (i=0;i<nsf;i++)
printf ("%d\n", (int)t[i]);*/
/* Pre-compute codewords response and energy */
compute_weighted_codebook(shape_cb, r, resp, resp2, E, shape_cb_size, subvect_size, stack);
for (j=0;j<N;j++)
odist[j]=0;
/*For all subvectors*/
for (i=0;i<nb_subvect;i++)
{
/*"erase" nbest list*/
for (j=0;j<N;j++)
ndist[j]=-2;
/*For all n-bests of previous subvector*/
for (j=0;j<N;j++)
{
spx_word16_t *x=ot[j]+subvect_size*i;
/*Find new n-best based on previous n-best j*/
if (have_sign)
vq_nbest_sign(x, resp2, subvect_size, shape_cb_size, E, N, best_index, best_dist, stack);
else
vq_nbest(x, resp2, subvect_size, shape_cb_size, E, N, best_index, best_dist, stack);
/*For all new n-bests*/
for (k=0;k<N;k++)
{
spx_word16_t *ct;
spx_word32_t err=0;
ct = ot[j];
/*update target*/
/*previous target*/
for (m=i*subvect_size;m<(i+1)*subvect_size;m++)
t[m]=ct[m];
/* New code: update only enough of the target to calculate error*/
{
int rind;
spx_word16_t *res;
spx_word16_t sign=1;
rind = best_index[k];
if (rind>=shape_cb_size)
{
sign=-1;
rind-=shape_cb_size;
}
res = resp+rind*subvect_size;
if (sign>0)
for (m=0;m<subvect_size;m++)
t[subvect_size*i+m] = SUB16(t[subvect_size*i+m], res[m]);
else
for (m=0;m<subvect_size;m++)
t[subvect_size*i+m] = ADD16(t[subvect_size*i+m], res[m]);
}
/*compute error (distance)*/
err=odist[j];
for (m=i*subvect_size;m<(i+1)*subvect_size;m++)
err = MAC16_16(err, t[m],t[m]);
/*update n-best list*/
if (err<ndist[N-1] || ndist[N-1]<-1)
{
/*previous target (we don't care what happened before*/
for (m=(i+1)*subvect_size;m<nsf;m++)
t[m]=ct[m];
/* New code: update the rest of the target only if it's worth it */
for (m=0;m<subvect_size;m++)
{
spx_word16_t g;
int rind;
spx_word16_t sign=1;
rind = best_index[k];
if (rind>=shape_cb_size)
{
sign=-1;
rind-=shape_cb_size;
}
q=subvect_size-m;
#ifdef FIXED_POINT
g=sign*shape_cb[rind*subvect_size+m];
for (n=subvect_size*(i+1);n<nsf;n++,q++)
t[n] = SUB32(t[n],MULT16_16_Q11_32(g,r[q]));
#else
g=sign*0.03125*shape_cb[rind*subvect_size+m];
for (n=subvect_size*(i+1);n<nsf;n++,q++)
t[n] = SUB32(t[n],g*r[q]);
#endif
}
for (m=0;m<N;m++)
{
if (err < ndist[m] || ndist[m]<-1)
{
for (n=N-1;n>m;n--)
{
for (q=(i+1)*subvect_size;q<nsf;q++)
nt[n][q]=nt[n-1][q];
for (q=0;q<nb_subvect;q++)
nind[n][q]=nind[n-1][q];
ndist[n]=ndist[n-1];
}
for (q=(i+1)*subvect_size;q<nsf;q++)
nt[m][q]=t[q];
for (q=0;q<nb_subvect;q++)
nind[m][q]=oind[j][q];
nind[m][i]=best_index[k];
ndist[m]=err;
break;
}
}
}
}
if (i==0)
break;
}
/*update old-new data*/
/* just swap pointers instead of a long copy */
{
spx_word16_t **tmp2;
tmp2=ot;
ot=nt;
nt=tmp2;
}
for (j=0;j<N;j++)
for (m=0;m<nb_subvect;m++)
oind[j][m]=nind[j][m];
for (j=0;j<N;j++)
odist[j]=ndist[j];
}
/*save indices*/
for (i=0;i<nb_subvect;i++)
{
ind[i]=nind[0][i];
speex_bits_pack(bits,ind[i],params->shape_bits+have_sign);
}
/* Put everything back together */
for (i=0;i<nb_subvect;i++)
{
int rind;
spx_word16_t sign=1;
rind = ind[i];
if (rind>=shape_cb_size)
{
sign=-1;
rind-=shape_cb_size;
}
#ifdef FIXED_POINT
if (sign==1)
{
for (j=0;j<subvect_size;j++)
e[subvect_size*i+j]=SHL32(EXTEND32(shape_cb[rind*subvect_size+j]),SIG_SHIFT-5);
} else {
for (j=0;j<subvect_size;j++)
e[subvect_size*i+j]=NEG32(SHL32(EXTEND32(shape_cb[rind*subvect_size+j]),SIG_SHIFT-5));
}
#else
for (j=0;j<subvect_size;j++)
e[subvect_size*i+j]=sign*0.03125*shape_cb[rind*subvect_size+j];
#endif
}
/* Update excitation */
for (j=0;j<nsf;j++)
exc[j]=ADD32(exc[j],e[j]);
/* Update target: only update target if necessary */
if (update_target)
{
syn_percep_zero(e, ak, awk1, awk2, r2, nsf,p, stack);
for (j=0;j<nsf;j++)
target[j]=SUB32(target[j],r2[j]);
}
}
void lsp_to_lpc(spx_lsp_t *freq,spx_coef_t *ak,int lpcrdr, char *stack)
/* float *freq array of LSP frequencies in the x domain */
/* float *ak array of LPC coefficients */
/* int lpcrdr order of LPC coefficients */
{
int i,j;
spx_word32_t xout1,xout2,xin1,xin2;
VARDECL(spx_word32_t *Wp);
spx_word32_t *pw,*n1,*n2,*n3,*n4=NULL;
VARDECL(spx_word16_t *freqn);
int m = lpcrdr>>1;
ALLOC(freqn, lpcrdr, spx_word16_t);
for (i=0;i<lpcrdr;i++)
freqn[i] = ANGLE2X(freq[i]);
ALLOC(Wp, 4*m+2, spx_word32_t);
pw = Wp;
/* initialise contents of array */
for(i=0;i<=4*m+1;i++){ /* set contents of buffer to 0 */
*pw++ = 0;
}
/* Set pointers up */
pw = Wp;
xin1 = 1048576;
xin2 = 1048576;
/* reconstruct P(z) and Q(z) by cascading second order
polynomials in form 1 - 2xz(-1) +z(-2), where x is the
LSP coefficient */
for(j=0;j<=lpcrdr;j++){
spx_word16_t *fr=freqn;
for(i=0;i<m;i++){
n1 = pw+(i<<2);
n2 = n1 + 1;
n3 = n2 + 1;
n4 = n3 + 1;
xout1 = ADD32(SUB32(xin1, MULT16_32_Q14(*fr,*n1)), *n2);
fr++;
xout2 = ADD32(SUB32(xin2, MULT16_32_Q14(*fr,*n3)), *n4);
fr++;
*n2 = *n1;
*n4 = *n3;
*n1 = xin1;
*n3 = xin2;
xin1 = xout1;
xin2 = xout2;
}
xout1 = xin1 + *(n4+1);
xout2 = xin2 - *(n4+2);
/* FIXME: perhaps apply bandwidth expansion in case of overflow? */
/*FIXME: Is it OK to have a long constant? */
if (xout1 + xout2>SHL(32766,8))
ak[j] = 32767;
else if (xout1 + xout2 < -SHL(32766,8))
ak[j] = -32767;
else
ak[j] = EXTRACT16(PSHR32(ADD32(xout1,xout2),8));
*(n4+1) = xin1;
*(n4+2) = xin2;
xin1 = 0;
xin2 = 0;
}
}
EXPORT void speex_encode_stereo_int(spx_int16_t * data, int frame_size,
SpeexBits * bits)
{
int i, tmp;
spx_word32_t e_left = 0, e_right = 0, e_tot = 0;
spx_word32_t balance, e_ratio;
spx_word32_t largest, smallest;
int balance_id;
#ifdef FIXED_POINT
int shift;
#endif
/* In band marker */
speex_bits_pack(bits, 14, 5);
/* Stereo marker */
speex_bits_pack(bits, SPEEX_INBAND_STEREO, 4);
for (i = 0; i < frame_size; i++) {
e_left += SHR32(MULT16_16(data[2 * i], data[2 * i]), 8);
e_right +=
SHR32(MULT16_16(data[2 * i + 1], data[2 * i + 1]), 8);
#ifdef FIXED_POINT
/* I think this is actually unbiased */
data[i] = SHR16(data[2 * i], 1) + PSHR16(data[2 * i + 1], 1);
#else
data[i] = .5 * (((float)data[2 * i]) + data[2 * i + 1]);
#endif
e_tot += SHR32(MULT16_16(data[i], data[i]), 8);
}
if (e_left > e_right) {
speex_bits_pack(bits, 0, 1);
largest = e_left;
smallest = e_right;
} else {
speex_bits_pack(bits, 1, 1);
largest = e_right;
smallest = e_left;
}
/* Balance quantization */
#ifdef FIXED_POINT
shift = spx_ilog2(largest) - 15;
largest = VSHR32(largest, shift - 4);
smallest = VSHR32(smallest, shift);
balance = DIV32(largest, ADD32(smallest, 1));
if (balance > 32767)
balance = 32767;
balance_id = scal_quant(EXTRACT16(balance), balance_bounds, 32);
#else
balance = (largest + 1.) / (smallest + 1.);
balance = 4 * log(balance);
balance_id = floor(.5 + fabs(balance));
if (balance_id > 30)
balance_id = 31;
#endif
speex_bits_pack(bits, balance_id, 5);
/* "coherence" quantisation */
#ifdef FIXED_POINT
shift = spx_ilog2(e_tot);
e_tot = VSHR32(e_tot, shift - 25);
e_left = VSHR32(e_left, shift - 10);
e_right = VSHR32(e_right, shift - 10);
e_ratio = DIV32(e_tot, e_left + e_right + 1);
#else
e_ratio = e_tot / (1. + e_left + e_right);
#endif
tmp = scal_quant(EXTRACT16(e_ratio), e_ratio_quant_bounds, 4);
/*fprintf (stderr, "%d %d %d %d\n", largest, smallest, balance_id, e_ratio); */
speex_bits_pack(bits, tmp, 2);
}
/** Performs echo cancellation on a frame */
EXPORT void speex_echo_cancellation(SpeexEchoState *st, const spx_int16_t *in, const spx_int16_t *far_end, spx_int16_t *out)
{
int i,j, chan, speak;
int N,M, C, K;
spx_word32_t Syy,See,Sxx,Sdd, Sff;
#ifdef TWO_PATH
spx_word32_t Dbf;
int update_foreground;
#endif
spx_word32_t Sey;
spx_word16_t ss, ss_1;
spx_float_t Pey = FLOAT_ONE, Pyy=FLOAT_ONE;
spx_float_t alpha, alpha_1;
spx_word16_t RER;
spx_word32_t tmp32;
N = st->window_size;
M = st->M;
C = st->C;
K = st->K;
st->cancel_count++;
#ifdef FIXED_POINT
ss=DIV32_16(11469,M);
ss_1 = SUB16(32767,ss);
#else
ss=.35/M;
ss_1 = 1-ss;
#endif
for (chan = 0; chan < C; chan++)
{
/* Apply a notch filter to make sure DC doesn't end up causing problems */
filter_dc_notch16(in+chan, st->notch_radius, st->input+chan*st->frame_size, st->frame_size, st->notch_mem+2*chan, C);
/* Copy input data to buffer and apply pre-emphasis */
/* Copy input data to buffer */
for (i=0;i<st->frame_size;i++)
{
spx_word32_t tmp32;
/* FIXME: This core has changed a bit, need to merge properly */
tmp32 = SUB32(EXTEND32(st->input[chan*st->frame_size+i]), EXTEND32(MULT16_16_P15(st->preemph, st->memD[chan])));
#ifdef FIXED_POINT
if (tmp32 > 32767)
{
tmp32 = 32767;
if (st->saturated == 0)
st->saturated = 1;
}
if (tmp32 < -32767)
{
tmp32 = -32767;
if (st->saturated == 0)
st->saturated = 1;
}
#endif
st->memD[chan] = st->input[chan*st->frame_size+i];
st->input[chan*st->frame_size+i] = EXTRACT16(tmp32);
}
}
for (speak = 0; speak < K; speak++)
{
for (i=0;i<st->frame_size;i++)
{
spx_word32_t tmp32;
st->x[speak*N+i] = st->x[speak*N+i+st->frame_size];
tmp32 = SUB32(EXTEND32(far_end[i*K+speak]), EXTEND32(MULT16_16_P15(st->preemph, st->memX[speak])));
#ifdef FIXED_POINT
/*FIXME: If saturation occurs here, we need to freeze adaptation for M frames (not just one) */
if (tmp32 > 32767)
{
tmp32 = 32767;
st->saturated = M+1;
}
if (tmp32 < -32767)
{
tmp32 = -32767;
st->saturated = M+1;
}
#endif
st->x[speak*N+i+st->frame_size] = EXTRACT16(tmp32);
st->memX[speak] = far_end[i*K+speak];
}
}
for (speak = 0; speak < K; speak++)
{
/* Shift memory: this could be optimized eventually*/
for (j=M-1;j>=0;j--)
{
for (i=0;i<N;i++)
st->X[(j+1)*N*K+speak*N+i] = st->X[j*N*K+speak*N+i];
}
/* Convert x (echo input) to frequency domain */
spx_fft(st->fft_table, st->x+speak*N, &st->X[speak*N]);
}
Sxx = 0;
for (speak = 0; speak < K; speak++)
{
Sxx += mdf_inner_prod(st->x+speak*N+st->frame_size, st->x+speak*N+st->frame_size, st->frame_size);
power_spectrum_accum(st->X+speak*N, st->Xf, N);
}
Sff = 0;
for (chan = 0; chan < C; chan++)
{
#ifdef TWO_PATH
/* Compute foreground filter */
spectral_mul_accum16(st->X, st->foreground+chan*N*K*M, st->Y+chan*N, N, M*K);
spx_ifft(st->fft_table, st->Y+chan*N, st->e+chan*N);
for (i=0;i<st->frame_size;i++)
st->e[chan*N+i] = SUB16(st->input[chan*st->frame_size+i], st->e[chan*N+i+st->frame_size]);
Sff += mdf_inner_prod(st->e+chan*N, st->e+chan*N, st->frame_size);
#endif
}
/* Adjust proportional adaption rate */
/* FIXME: Adjust that for C, K*/
if (st->adapted)
mdf_adjust_prop (st->W, N, M, C*K, st->prop);
/* Compute weight gradient */
if (st->saturated == 0)
{
for (chan = 0; chan < C; chan++)
{
for (speak = 0; speak < K; speak++)
{
for (j=M-1;j>=0;j--)
{
weighted_spectral_mul_conj(st->power_1, FLOAT_SHL(PSEUDOFLOAT(st->prop[j]),-15), &st->X[(j+1)*N*K+speak*N], st->E+chan*N, st->PHI, N);
for (i=0;i<N;i++)
st->W[chan*N*K*M + j*N*K + speak*N + i] += st->PHI[i];
}
}
}
} else {
st->saturated--;
}
/* FIXME: MC conversion required */
/* Update weight to prevent circular convolution (MDF / AUMDF) */
for (chan = 0; chan < C; chan++)
{
for (speak = 0; speak < K; speak++)
{
for (j=0;j<M;j++)
{
/* This is a variant of the Alternatively Updated MDF (AUMDF) */
/* Remove the "if" to make this an MDF filter */
if (j==0 || st->cancel_count%(M-1) == j-1)
{
#ifdef FIXED_POINT
for (i=0;i<N;i++)
st->wtmp2[i] = EXTRACT16(PSHR32(st->W[chan*N*K*M + j*N*K + speak*N + i],NORMALIZE_SCALEDOWN+16));
spx_ifft(st->fft_table, st->wtmp2, st->wtmp);
for (i=0;i<st->frame_size;i++)
{
st->wtmp[i]=0;
}
for (i=st->frame_size;i<N;i++)
{
st->wtmp[i]=SHL16(st->wtmp[i],NORMALIZE_SCALEUP);
}
spx_fft(st->fft_table, st->wtmp, st->wtmp2);
/* The "-1" in the shift is a sort of kludge that trades less efficient update speed for decrease noise */
for (i=0;i<N;i++)
st->W[chan*N*K*M + j*N*K + speak*N + i] -= SHL32(EXTEND32(st->wtmp2[i]),16+NORMALIZE_SCALEDOWN-NORMALIZE_SCALEUP-1);
#else
spx_ifft(st->fft_table, &st->W[chan*N*K*M + j*N*K + speak*N], st->wtmp);
for (i=st->frame_size;i<N;i++)
{
st->wtmp[i]=0;
}
spx_fft(st->fft_table, st->wtmp, &st->W[chan*N*K*M + j*N*K + speak*N]);
#endif
}
}
}
}
/* So we can use power_spectrum_accum */
for (i=0;i<=st->frame_size;i++)
st->Rf[i] = st->Yf[i] = st->Xf[i] = 0;
Dbf = 0;
See = 0;
#ifdef TWO_PATH
/* Difference in response, this is used to estimate the variance of our residual power estimate */
for (chan = 0; chan < C; chan++)
{
spectral_mul_accum(st->X, st->W+chan*N*K*M, st->Y+chan*N, N, M*K);
spx_ifft(st->fft_table, st->Y+chan*N, st->y+chan*N);
for (i=0;i<st->frame_size;i++)
st->e[chan*N+i] = SUB16(st->e[chan*N+i+st->frame_size], st->y[chan*N+i+st->frame_size]);
Dbf += 10+mdf_inner_prod(st->e+chan*N, st->e+chan*N, st->frame_size);
for (i=0;i<st->frame_size;i++)
st->e[chan*N+i] = SUB16(st->input[chan*st->frame_size+i], st->y[chan*N+i+st->frame_size]);
See += mdf_inner_prod(st->e+chan*N, st->e+chan*N, st->frame_size);
}
#endif
#ifndef TWO_PATH
Sff = See;
#endif
#ifdef TWO_PATH
/* Logic for updating the foreground filter */
/* For two time windows, compute the mean of the energy difference, as well as the variance */
st->Davg1 = ADD32(MULT16_32_Q15(QCONST16(.6f,15),st->Davg1), MULT16_32_Q15(QCONST16(.4f,15),SUB32(Sff,See)));
st->Davg2 = ADD32(MULT16_32_Q15(QCONST16(.85f,15),st->Davg2), MULT16_32_Q15(QCONST16(.15f,15),SUB32(Sff,See)));
st->Dvar1 = FLOAT_ADD(FLOAT_MULT(VAR1_SMOOTH, st->Dvar1), FLOAT_MUL32U(MULT16_32_Q15(QCONST16(.4f,15),Sff), MULT16_32_Q15(QCONST16(.4f,15),Dbf)));
st->Dvar2 = FLOAT_ADD(FLOAT_MULT(VAR2_SMOOTH, st->Dvar2), FLOAT_MUL32U(MULT16_32_Q15(QCONST16(.15f,15),Sff), MULT16_32_Q15(QCONST16(.15f,15),Dbf)));
/* Equivalent float code:
st->Davg1 = .6*st->Davg1 + .4*(Sff-See);
st->Davg2 = .85*st->Davg2 + .15*(Sff-See);
st->Dvar1 = .36*st->Dvar1 + .16*Sff*Dbf;
st->Dvar2 = .7225*st->Dvar2 + .0225*Sff*Dbf;
*/
update_foreground = 0;
/* Check if we have a statistically significant reduction in the residual echo */
/* Note that this is *not* Gaussian, so we need to be careful about the longer tail */
if (FLOAT_GT(FLOAT_MUL32U(SUB32(Sff,See),ABS32(SUB32(Sff,See))), FLOAT_MUL32U(Sff,Dbf)))
update_foreground = 1;
else if (FLOAT_GT(FLOAT_MUL32U(st->Davg1, ABS32(st->Davg1)), FLOAT_MULT(VAR1_UPDATE,(st->Dvar1))))
update_foreground = 1;
else if (FLOAT_GT(FLOAT_MUL32U(st->Davg2, ABS32(st->Davg2)), FLOAT_MULT(VAR2_UPDATE,(st->Dvar2))))
update_foreground = 1;
/* Do we update? */
if (update_foreground)
{
st->Davg1 = st->Davg2 = 0;
st->Dvar1 = st->Dvar2 = FLOAT_ZERO;
/* Copy background filter to foreground filter */
for (i=0;i<N*M*C*K;i++)
st->foreground[i] = EXTRACT16(PSHR32(st->W[i],16));
/* Apply a smooth transition so as to not introduce blocking artifacts */
for (chan = 0; chan < C; chan++)
for (i=0;i<st->frame_size;i++)
st->e[chan*N+i+st->frame_size] = MULT16_16_Q15(st->window[i+st->frame_size],st->e[chan*N+i+st->frame_size]) + MULT16_16_Q15(st->window[i],st->y[chan*N+i+st->frame_size]);
} else {
int reset_background=0;
/* Otherwise, check if the background filter is significantly worse */
if (FLOAT_GT(FLOAT_MUL32U(NEG32(SUB32(Sff,See)),ABS32(SUB32(Sff,See))), FLOAT_MULT(VAR_BACKTRACK,FLOAT_MUL32U(Sff,Dbf))))
reset_background = 1;
if (FLOAT_GT(FLOAT_MUL32U(NEG32(st->Davg1), ABS32(st->Davg1)), FLOAT_MULT(VAR_BACKTRACK,st->Dvar1)))
reset_background = 1;
if (FLOAT_GT(FLOAT_MUL32U(NEG32(st->Davg2), ABS32(st->Davg2)), FLOAT_MULT(VAR_BACKTRACK,st->Dvar2)))
reset_background = 1;
if (reset_background)
{
/* Copy foreground filter to background filter */
for (i=0;i<N*M*C*K;i++)
st->W[i] = SHL32(EXTEND32(st->foreground[i]),16);
/* We also need to copy the output so as to get correct adaptation */
for (chan = 0; chan < C; chan++)
{
for (i=0;i<st->frame_size;i++)
st->y[chan*N+i+st->frame_size] = st->e[chan*N+i+st->frame_size];
for (i=0;i<st->frame_size;i++)
st->e[chan*N+i] = SUB16(st->input[chan*st->frame_size+i], st->y[chan*N+i+st->frame_size]);
}
See = Sff;
st->Davg1 = st->Davg2 = 0;
st->Dvar1 = st->Dvar2 = FLOAT_ZERO;
}
}
#endif
Sey = Syy = Sdd = 0;
for (chan = 0; chan < C; chan++)
{
/* Compute error signal (for the output with de-emphasis) */
for (i=0;i<st->frame_size;i++)
{
spx_word32_t tmp_out;
#ifdef TWO_PATH
tmp_out = SUB32(EXTEND32(st->input[chan*st->frame_size+i]), EXTEND32(st->e[chan*N+i+st->frame_size]));
#else
tmp_out = SUB32(EXTEND32(st->input[chan*st->frame_size+i]), EXTEND32(st->y[chan*N+i+st->frame_size]));
#endif
tmp_out = ADD32(tmp_out, EXTEND32(MULT16_16_P15(st->preemph, st->memE[chan])));
/* This is an arbitrary test for saturation in the microphone signal */
if (in[i*C+chan] <= -32000 || in[i*C+chan] >= 32000)
{
if (st->saturated == 0)
st->saturated = 1;
}
out[i*C+chan] = WORD2INT(tmp_out);
st->memE[chan] = tmp_out;
}
#ifdef DUMP_ECHO_CANCEL_DATA
dump_audio(in, far_end, out, st->frame_size);
#endif
/* Compute error signal (filter update version) */
for (i=0;i<st->frame_size;i++)
{
st->e[chan*N+i+st->frame_size] = st->e[chan*N+i];
st->e[chan*N+i] = 0;
}
/* Compute a bunch of correlations */
/* FIXME: bad merge */
Sey += mdf_inner_prod(st->e+chan*N+st->frame_size, st->y+chan*N+st->frame_size, st->frame_size);
Syy += mdf_inner_prod(st->y+chan*N+st->frame_size, st->y+chan*N+st->frame_size, st->frame_size);
Sdd += mdf_inner_prod(st->input+chan*st->frame_size, st->input+chan*st->frame_size, st->frame_size);
/* Convert error to frequency domain */
spx_fft(st->fft_table, st->e+chan*N, st->E+chan*N);
for (i=0;i<st->frame_size;i++)
st->y[i+chan*N] = 0;
spx_fft(st->fft_table, st->y+chan*N, st->Y+chan*N);
/* Compute power spectrum of echo (X), error (E) and filter response (Y) */
power_spectrum_accum(st->E+chan*N, st->Rf, N);
power_spectrum_accum(st->Y+chan*N, st->Yf, N);
}
/*printf ("%f %f %f %f\n", Sff, See, Syy, Sdd, st->update_cond);*/
/* Do some sanity check */
if (!(Syy>=0 && Sxx>=0 && See >= 0)
#ifndef FIXED_POINT
|| !(Sff < N*1e9 && Syy < N*1e9 && Sxx < N*1e9)
#endif
)
{
/* Things have gone really bad */
st->screwed_up += 50;
for (i=0;i<st->frame_size*C;i++)
out[i] = 0;
} else if (SHR32(Sff, 2) > ADD32(Sdd, SHR32(MULT16_16(N, 10000),6)))
{
/* AEC seems to add lots of echo instead of removing it, let's see if it will improve */
st->screwed_up++;
} else {
/* Everything's fine */
st->screwed_up=0;
}
if (st->screwed_up>=50)
{
speex_warning("The echo canceller started acting funny and got slapped (reset). It swears it will behave now.");
speex_echo_state_reset(st);
return;
}
/* Add a small noise floor to make sure not to have problems when dividing */
See = MAX32(See, SHR32(MULT16_16(N, 100),6));
for (speak = 0; speak < K; speak++)
{
Sxx += mdf_inner_prod(st->x+speak*N+st->frame_size, st->x+speak*N+st->frame_size, st->frame_size);
power_spectrum_accum(st->X+speak*N, st->Xf, N);
}
/* Smooth far end energy estimate over time */
for (j=0;j<=st->frame_size;j++)
st->power[j] = MULT16_32_Q15(ss_1,st->power[j]) + 1 + MULT16_32_Q15(ss,st->Xf[j]);
/* Compute filtered spectra and (cross-)correlations */
for (j=st->frame_size;j>=0;j--)
{
spx_float_t Eh, Yh;
Eh = PSEUDOFLOAT(st->Rf[j] - st->Eh[j]);
Yh = PSEUDOFLOAT(st->Yf[j] - st->Yh[j]);
Pey = FLOAT_ADD(Pey,FLOAT_MULT(Eh,Yh));
Pyy = FLOAT_ADD(Pyy,FLOAT_MULT(Yh,Yh));
#ifdef FIXED_POINT
st->Eh[j] = MAC16_32_Q15(MULT16_32_Q15(SUB16(32767,st->spec_average),st->Eh[j]), st->spec_average, st->Rf[j]);
st->Yh[j] = MAC16_32_Q15(MULT16_32_Q15(SUB16(32767,st->spec_average),st->Yh[j]), st->spec_average, st->Yf[j]);
#else
st->Eh[j] = (1-st->spec_average)*st->Eh[j] + st->spec_average*st->Rf[j];
st->Yh[j] = (1-st->spec_average)*st->Yh[j] + st->spec_average*st->Yf[j];
#endif
}
Pyy = FLOAT_SQRT(Pyy);
Pey = FLOAT_DIVU(Pey,Pyy);
/* Compute correlation updatete rate */
tmp32 = MULT16_32_Q15(st->beta0,Syy);
if (tmp32 > MULT16_32_Q15(st->beta_max,See))
tmp32 = MULT16_32_Q15(st->beta_max,See);
alpha = FLOAT_DIV32(tmp32, See);
alpha_1 = FLOAT_SUB(FLOAT_ONE, alpha);
/* Update correlations (recursive average) */
st->Pey = FLOAT_ADD(FLOAT_MULT(alpha_1,st->Pey) , FLOAT_MULT(alpha,Pey));
st->Pyy = FLOAT_ADD(FLOAT_MULT(alpha_1,st->Pyy) , FLOAT_MULT(alpha,Pyy));
if (FLOAT_LT(st->Pyy, FLOAT_ONE))
st->Pyy = FLOAT_ONE;
/* We don't really hope to get better than 33 dB (MIN_LEAK-3dB) attenuation anyway */
if (FLOAT_LT(st->Pey, FLOAT_MULT(MIN_LEAK,st->Pyy)))
st->Pey = FLOAT_MULT(MIN_LEAK,st->Pyy);
if (FLOAT_GT(st->Pey, st->Pyy))
st->Pey = st->Pyy;
/* leak_estimate is the linear regression result */
st->leak_estimate = FLOAT_EXTRACT16(FLOAT_SHL(FLOAT_DIVU(st->Pey, st->Pyy),14));
/* This looks like a stupid bug, but it's right (because we convert from Q14 to Q15) */
if (st->leak_estimate > 16383)
st->leak_estimate = 32767;
else
st->leak_estimate = SHL16(st->leak_estimate,1);
/*printf ("%f\n", st->leak_estimate);*/
/* Compute Residual to Error Ratio */
#ifdef FIXED_POINT
tmp32 = MULT16_32_Q15(st->leak_estimate,Syy);
tmp32 = ADD32(SHR32(Sxx,13), ADD32(tmp32, SHL32(tmp32,1)));
/* Check for y in e (lower bound on RER) */
{
spx_float_t bound = PSEUDOFLOAT(Sey);
bound = FLOAT_DIVU(FLOAT_MULT(bound, bound), PSEUDOFLOAT(ADD32(1,Syy)));
if (FLOAT_GT(bound, PSEUDOFLOAT(See)))
tmp32 = See;
else if (tmp32 < FLOAT_EXTRACT32(bound))
tmp32 = FLOAT_EXTRACT32(bound);
}
if (tmp32 > SHR32(See,1))
tmp32 = SHR32(See,1);
RER = FLOAT_EXTRACT16(FLOAT_SHL(FLOAT_DIV32(tmp32,See),15));
#else
RER = (.0001*Sxx + 3.*MULT16_32_Q15(st->leak_estimate,Syy)) / See;
/* Check for y in e (lower bound on RER) */
if (RER < Sey*Sey/(1+See*Syy))
RER = Sey*Sey/(1+See*Syy);
if (RER > .5)
RER = .5;
#endif
/* We consider that the filter has had minimal adaptation if the following is true*/
if (!st->adapted && st->sum_adapt > SHL32(EXTEND32(M),15) && MULT16_32_Q15(st->leak_estimate,Syy) > MULT16_32_Q15(QCONST16(.03f,15),Syy))
{
st->adapted = 1;
}
if (st->adapted)
{
/* Normal learning rate calculation once we're past the minimal adaptation phase */
for (i=0;i<=st->frame_size;i++)
{
spx_word32_t r, e;
/* Compute frequency-domain adaptation mask */
r = MULT16_32_Q15(st->leak_estimate,SHL32(st->Yf[i],3));
e = SHL32(st->Rf[i],3)+1;
#ifdef FIXED_POINT
if (r>SHR32(e,1))
r = SHR32(e,1);
#else
if (r>.5*e)
r = .5*e;
#endif
r = MULT16_32_Q15(QCONST16(.7,15),r) + MULT16_32_Q15(QCONST16(.3,15),(spx_word32_t)(MULT16_32_Q15(RER,e)));
/*st->power_1[i] = adapt_rate*r/(e*(1+st->power[i]));*/
st->power_1[i] = FLOAT_SHL(FLOAT_DIV32_FLOAT(r,FLOAT_MUL32U(e,st->power[i]+10)),WEIGHT_SHIFT+16);
}
} else {
/* Temporary adaption rate if filter is not yet adapted enough */
spx_word16_t adapt_rate=0;
if (Sxx > SHR32(MULT16_16(N, 1000),6))
{
tmp32 = MULT16_32_Q15(QCONST16(.25f, 15), Sxx);
#ifdef FIXED_POINT
if (tmp32 > SHR32(See,2))
tmp32 = SHR32(See,2);
#else
if (tmp32 > .25*See)
tmp32 = .25*See;
#endif
adapt_rate = FLOAT_EXTRACT16(FLOAT_SHL(FLOAT_DIV32(tmp32, See),15));
}
for (i=0;i<=st->frame_size;i++)
st->power_1[i] = FLOAT_SHL(FLOAT_DIV32(EXTEND32(adapt_rate),ADD32(st->power[i],10)),WEIGHT_SHIFT+1);
/* How much have we adapted so far? */
st->sum_adapt = ADD32(st->sum_adapt,adapt_rate);
}
/* FIXME: MC conversion required */
for (i=0;i<st->frame_size;i++)
st->last_y[i] = st->last_y[st->frame_size+i];
if (st->adapted)
{
/* If the filter is adapted, take the filtered echo */
for (i=0;i<st->frame_size;i++)
st->last_y[st->frame_size+i] = in[i]-out[i];
} else {
/* If filter isn't adapted yet, all we can do is take the far end signal directly */
/* moved earlier: for (i=0;i<N;i++)
st->last_y[i] = st->x[i];*/
}
}
