static spx_word16_t spx_cos(spx_word16_t x)
{
spx_word16_t x2;
if (x<12868)
{
x2 = MULT16_16_P13(x,x);
return ADD32(C1, MULT16_16_P13(x2, ADD32(C2, MULT16_16_P13(x2, ADD32(C3, MULT16_16_P13(C4, x2))))));
} else {
x = SUB16(25736,x);
x2 = MULT16_16_P13(x,x);
return SUB32(-C1, MULT16_16_P13(x2, ADD32(C2, MULT16_16_P13(x2, ADD32(C3, MULT16_16_P13(C4, x2))))));
/*return SUB32(-C1, MULT16_16_Q13(x2, ADD32(C2, MULT16_16_Q13(C3, x2))));*/
}
}
void lsp_quant_high(spx_lsp_t *lsp, spx_lsp_t *qlsp, int order, SpeexBits *bits)
{
int i;
int id;
spx_word16_t quant_weight[10];
for (i=0;i<order;i++)
qlsp[i]=lsp[i];
compute_quant_weights(qlsp, quant_weight, order);
/* quant_weight[0] = 10/(qlsp[1]-qlsp[0]);
quant_weight[order-1] = 10/(qlsp[order-1]-qlsp[order-2]);
for (i=1;i<order-1;i++)
{
tmp1 = 10/(qlsp[i]-qlsp[i-1]);
tmp2 = 10/(qlsp[i+1]-qlsp[i]);
quant_weight[i] = tmp1 > tmp2 ? tmp1 : tmp2;
}*/
for (i=0;i<order;i++)
qlsp[i]=SUB16(qlsp[i],LSP_LINEAR_HIGH(i));
#ifndef FIXED_POINT
for (i=0;i<order;i++)
qlsp[i] = qlsp[i]*LSP_SCALE;
#endif
id = lsp_quant(qlsp, high_lsp_cdbk, 64, order);
speex_bits_pack(bits, id, 6);
for (i=0;i<order;i++)
qlsp[i]*=2;
id = lsp_weight_quant(qlsp, quant_weight, high_lsp_cdbk2, 64, order);
speex_bits_pack(bits, id, 6);
#ifdef FIXED_POINT
for (i=0;i<order;i++)
qlsp[i] = PSHR16(qlsp[i],1);
#else
for (i=0;i<order;i++)
qlsp[i] = qlsp[i]*0.0019531;
#endif
for (i=0;i<order;i++)
qlsp[i]=lsp[i]-qlsp[i];
}
static inline spx_word32_t cheb_poly_eva(spx_word32_t *coef,spx_word16_t x,int m,char *stack)
/* float coef[] coefficients of the polynomial to be evaluated */
/* float x the point where polynomial is to be evaluated */
/* int m order of the polynomial */
{
int i;
VARDECL(spx_word16_t *T);
spx_word32_t sum;
int m2=m>>1;
VARDECL(spx_word16_t *coefn);
/*Prevents overflows*/
if (x>16383)
x = 16383;
if (x<-16383)
x = -16383;
/* Allocate memory for Chebyshev series formulation */
ALLOC(T, m2+1, spx_word16_t);
ALLOC(coefn, m2+1, spx_word16_t);
for (i=0;i<m2+1;i++)
{
coefn[i] = coef[i];
/*printf ("%f ", coef[i]);*/
}
/*printf ("\n");*/
/* Initialise values */
T[0]=16384;
T[1]=x;
/* Evaluate Chebyshev series formulation using iterative approach */
/* Evaluate polynomial and return value also free memory space */
sum = ADD32(coefn[m2], MULT16_16_P14(coefn[m2-1],x));
/*x *= 2;*/
for(i=2;i<=m2;i++)
{
T[i] = SUB16(MULT16_16_Q13(x,T[i-1]), T[i-2]);
sum = ADD32(sum, MULT16_16_P14(coefn[m2-i],T[i]));
/*printf ("%f ", sum);*/
}
/*printf ("\n");*/
return sum;
}
/* returns minimum mean square error */
spx_word32_t _spx_lpc(
spx_coef_t *lpc, /* out: [0...p-1] LPC coefficients */
const spx_word16_t *ac, /* in: [0...p] autocorrelation values */
int p
)
{
int i, j;
spx_word16_t r;
spx_word16_t error = ac[0];
if (ac[0] == 0)
{
for (i = 0; i < p; i++)
lpc[i] = 0;
return 0;
}
for (i = 0; i < p; i++) {
/* Sum up this iteration's reflection coefficient */
spx_word32_t rr = NEG32(SHL32(EXTEND32(ac[i + 1]),13));
for (j = 0; j < i; j++)
rr = SUB32(rr,MULT16_16(lpc[j],ac[i - j]));
#ifdef FIXED_POINT
r = DIV32_16(rr+PSHR32(error,1),ADD16(error,8));
#else
r = rr/(error+.003*ac[0]);
#endif
/* Update LPC coefficients and total error */
lpc[i] = r;
for (j = 0; j < i>>1; j++)
{
spx_word16_t tmp = lpc[j];
lpc[j] = MAC16_16_P13(lpc[j],r,lpc[i-1-j]);
lpc[i-1-j] = MAC16_16_P13(lpc[i-1-j],r,tmp);
}
if (i & 1)
lpc[j] = MAC16_16_P13(lpc[j],lpc[j],r);
error = SUB16(error,MULT16_16_Q13(r,MULT16_16_Q13(error,r)));
}
return error;
}
void lsp_quant_lbr(spx_lsp_t *lsp, spx_lsp_t *qlsp, int order, SpeexBits *bits)
{
int i;
int id;
spx_word16_t quant_weight[10];
for (i=0;i<order;i++)
qlsp[i]=lsp[i];
compute_quant_weights(qlsp, quant_weight, order);
for (i=0;i<order;i++)
qlsp[i]=SUB16(qlsp[i],LSP_LINEAR(i));
#ifndef FIXED_POINT
for (i=0;i<order;i++)
qlsp[i]=qlsp[i]*LSP_SCALE;
#endif
id = lsp_quant(qlsp, cdbk_nb, NB_CDBK_SIZE, order);
speex_bits_pack(bits, id, 6);
for (i=0;i<order;i++)
qlsp[i]*=2;
id = lsp_weight_quant(qlsp, quant_weight, cdbk_nb_low1, NB_CDBK_SIZE_LOW1, 5);
speex_bits_pack(bits, id, 6);
id = lsp_weight_quant(qlsp+5, quant_weight+5, cdbk_nb_high1, NB_CDBK_SIZE_HIGH1, 5);
speex_bits_pack(bits, id, 6);
#ifdef FIXED_POINT
for (i=0;i<order;i++)
qlsp[i] = PSHR16(qlsp[i],1);
#else
for (i=0;i<order;i++)
qlsp[i] = qlsp[i]*0.0019531;
#endif
for (i=0;i<order;i++)
qlsp[i]=lsp[i]-qlsp[i];
}
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));
}
}
void decodeAdaptativeCodeVector(bcg729DecoderChannelContextStruct *decoderChannelContext, int subFrameIndex, uint16_t adaptativeCodebookIndex, uint8_t parityFlag, uint8_t frameErasureFlag,
int16_t *intPitchDelay, word16_t *excitationVector)
{
int16_t fracPitchDelay;
/*** Compute the Pitch Delay from the Codebook index ***/
/* fracPitchDelay is computed in the range -1,0,1 */
if (subFrameIndex == 0 ) { /* first subframe */
if (parityFlag|frameErasureFlag) { /* there is an error (either parity or frame erased) */
*intPitchDelay = decoderChannelContext->previousIntPitchDelay; /* set the integer part of Pitch Delay to the last second subframe Pitch Delay computed spec: 4.1.2 */
/* Note: unable to find anything regarding this part in the spec, just copied it from the ITU source code */
fracPitchDelay = 0;
decoderChannelContext->previousIntPitchDelay++;
if (decoderChannelContext->previousIntPitchDelay>MAXIMUM_INT_PITCH_DELAY) decoderChannelContext->previousIntPitchDelay=MAXIMUM_INT_PITCH_DELAY;
} else { /* parity and frameErasure flags are off, do the normal computation (doc 4.1.3) */
if (adaptativeCodebookIndex<197) {
/* *intPitchDelay = (P1 + 2 )/ 3 + 19 */
*intPitchDelay = ADD16(MULT16_16_Q15(ADD16(adaptativeCodebookIndex,2), 10923), 19); /* MULT in Q15: 1/3 in Q15: 10923 */
/* fracPitchDelay = P1 − 3*intPitchDelay + 58 : fracPitchDelay in -1, 0, 1 */
fracPitchDelay = ADD16(SUB16(adaptativeCodebookIndex, MULT16_16(*intPitchDelay, 3)), 58);
} else {/* adaptativeCodebookIndex>= 197 */
*intPitchDelay = SUB16(adaptativeCodebookIndex, 112);
fracPitchDelay = 0;
}
/* backup the intPitchDelay */
decoderChannelContext->previousIntPitchDelay = *intPitchDelay;
}
} else { /* second subframe */
if (frameErasureFlag) { /* there is an error : frame erased, in case of parity error, it has been taken in account at first subframe */
/* unable to find anything regarding this part in the spec, just copied it from the ITU source code */
*intPitchDelay = decoderChannelContext->previousIntPitchDelay;
fracPitchDelay = 0;
decoderChannelContext->previousIntPitchDelay++;
if (decoderChannelContext->previousIntPitchDelay>MAXIMUM_INT_PITCH_DELAY) decoderChannelContext->previousIntPitchDelay=MAXIMUM_INT_PITCH_DELAY;
} else { /* frameErasure flags are off, do the normal computation (doc 4.1.3) */
int16_t tMin = SUB16(*intPitchDelay,5); /* intPitchDelay contains the intPitch computed for subframe one */
if (tMin<20) {
tMin = 20;
}
if (tMin>134) {
tMin = 134;
}
/* intPitchDelay = (P2 + 2 )/ 3 − 1 */
*intPitchDelay = SUB16(MULT16_16_Q15(ADD16(adaptativeCodebookIndex, 2), 10923), 1);
/* fracPitchDelay = P2 − 2 − 3((P 2 + 2 )/ 3 − 1) */
fracPitchDelay = SUB16(SUB16(adaptativeCodebookIndex, MULT16_16(*intPitchDelay, 3)), 2);
/* *intPitchDelay = (P2 + 2 )/ 3 − 1 + tMin */
*intPitchDelay = ADD16(*intPitchDelay,tMin);
/* backup the intPitchDelay */
decoderChannelContext->previousIntPitchDelay = *intPitchDelay;
}
}
/* now compute the adaptative codebook vector using the pitch delay we just get and the past excitation vector */
/* from spec 4.1.3 and 3.7.1 */
/* shall compute v(n ) = ∑ u (n - k + i )b30 (t + 3i ) + ∑ u (n - k + 1 + i )b30 (3 - t + 3i ) for i=0,...,9 and n = 0,...,39 (t in 0, 1, 2) */
/* with k = intPitchDelay and t = fracPitchDelay wich must be converted from range -1,0,1 to 0,1,2 */
/* u the past excitation vector */
/* v the adaptative codebook vector */
/* b30 an interpolation filter */
word16_t *excitationVectorMinusK; /* pointer to u(-k) */
/* scale fracPichDelay from -1,0.1 to 0,1,2 */
if (fracPitchDelay==1) {
excitationVectorMinusK = &(excitationVector[-(*intPitchDelay+1)]); /* fracPitchDelay being positive -> increase by one the integer part and set to 2 the fractional part : -(k+1/3) -> -(k+1)+2/3 */
fracPitchDelay = 2;
} else {
fracPitchDelay = -fracPitchDelay; /* 0 unchanged, -1 -> +1 */
excitationVectorMinusK = &(excitationVector[-(*intPitchDelay)]); /* -(k-1/3) -> -k+1/3 or -(k) -> -k*/
}
int n;
for (n=0; n<L_SUBFRAME; n++) { /* loop over the whole subframe */
word16_t *excitationVectorNMinusK = &(excitationVectorMinusK[n]); /* point to u(n-k), unscaled value, full range */
word16_t *excitationVectorNMinusKPlusOne = &(excitationVectorMinusK[n+1]); /* point to u(n-k+1), unscaled value, full range */
word16_t *b301 = &(b30[fracPitchDelay]); /* point to b30(t) in Q0.15 : sums of all b30 coeffs is < 2, no overflow possible on 32 bits */
word16_t *b302 = &(b30[3-fracPitchDelay]); /* point to b30(3-t) in Q0.15*/
int i,j; /* j will store 3i */
word32_t acc = 0; /* in Q15 */
for (i=0, j=0; i<10; i++, j+=3) {
acc = MAC16_16(acc, excitationVectorNMinusK[-i], b301[j]); /* Note : the spec says: u(n−k+i)b30(t+3i) but the ITU code do (and here too) u(n-k-i )b30(t+3i) */
acc = MAC16_16(acc, excitationVectorNMinusKPlusOne[i], b302[j]); /* u(n-k+1+i)b30(3-t+3i) */
}
excitationVector[n] = SATURATE(PSHR(acc, 15), MAXINT16); /* acc in Q15, shift/round to unscaled value and check overflow on 16 bits */
}
return;
}
int lpc_to_lsp (spx_coef_t *a,int lpcrdr,spx_lsp_t *freq,int nb,spx_word16_t delta, char *stack)
/* float *a lpc coefficients */
/* int lpcrdr order of LPC coefficients (10) */
/* float *freq LSP frequencies in the x domain */
/* int nb number of sub-intervals (4) */
/* float delta grid spacing interval (0.02) */
{
spx_word16_t temp_xr,xl,xr,xm=0;
spx_word32_t psuml,psumr,psumm,temp_psumr/*,temp_qsumr*/;
int i,j,m,flag,k;
#ifndef FIXED_LPC_SIZE
VARDECL(spx_word32_t *Q); /* ptrs for memory allocation */
VARDECL(spx_word32_t *P);
VARDECL(spx_word16_t *Q16); /* ptrs for memory allocation */
VARDECL(spx_word16_t *P16);
#else
spx_word32_t Q[(FIXED_LPC_SIZE/2)+1]; /* ptrs for memory allocation */
spx_word32_t P[(FIXED_LPC_SIZE/2)+1];
spx_word16_t Q16[(FIXED_LPC_SIZE/2)+1]; /* ptrs for memory allocation */
spx_word16_t P16[(FIXED_LPC_SIZE/2)+1];
#endif
spx_word32_t *px; /* ptrs of respective P'(z) & Q'(z) */
spx_word32_t *qx;
spx_word32_t *p;
spx_word32_t *q;
spx_word16_t *pt; /* ptr used for cheb_poly_eval()
whether P' or Q' */
int roots=0; /* DR 8/2/94: number of roots found */
flag = 1; /* program is searching for a root when,
1 else has found one */
m = lpcrdr/2; /* order of P'(z) & Q'(z) polynomials */
#ifndef FIXED_LPC_SIZE
/* Allocate memory space for polynomials */
ALLOC(Q, (m+1), spx_word32_t);
ALLOC(P, (m+1), spx_word32_t);
#endif
/* determine P'(z)'s and Q'(z)'s coefficients where
P'(z) = P(z)/(1 + z^(-1)) and Q'(z) = Q(z)/(1-z^(-1)) */
px = P; /* initialise ptrs */
qx = Q;
p = px;
q = qx;
#ifdef FIXED_POINT
*px++ = LPC_SCALING;
*qx++ = LPC_SCALING;
#ifndef FIXED_LPC_SIZE
for(i=0;i<m;i++){
*px++ = SUB32(ADD32(EXTEND32(a[i]),EXTEND32(a[lpcrdr-i-1])), *p++);
*qx++ = ADD32(SUB32(EXTEND32(a[i]),EXTEND32(a[lpcrdr-i-1])), *q++);
}
#else
for(i=0;i<(FIXED_LPC_SIZE/2);i++){
*px++ = SUB32(ADD32(EXTEND32(a[i]),EXTEND32(a[FIXED_LPC_SIZE-i-1])), *p++);
*qx++ = ADD32(SUB32(EXTEND32(a[i]),EXTEND32(a[FIXED_LPC_SIZE-i-1])), *q++);
}
#endif
px = P;
qx = Q;
for(i=0;i<m;i++)
{
/*if (fabs(*px)>=32768)
speex_warning_int("px", *px);
if (fabs(*qx)>=32768)
speex_warning_int("qx", *qx);*/
*px = PSHR32(*px,2);
*qx = PSHR32(*qx,2);
px++;
qx++;
}
/* The reason for this lies in the way cheb_poly_eva() is implemented for fixed-point */
P[m] = PSHR32(P[m],3);
Q[m] = PSHR32(Q[m],3);
#else
*px++ = LPC_SCALING;
*qx++ = LPC_SCALING;
for(i=0;i<m;i++){
*px++ = (a[i]+a[lpcrdr-1-i]) - *p++;
*qx++ = (a[i]-a[lpcrdr-1-i]) + *q++;
}
px = P;
qx = Q;
for(i=0;i<m;i++){
*px = 2**px;
*qx = 2**qx;
px++;
qx++;
}
#endif
px = P; /* re-initialise ptrs */
qx = Q;
/* now that we have computed P and Q convert to 16 bits to
speed up cheb_poly_eval */
#ifndef FIXED_LPC_SIZE
ALLOC(P16, m+1, spx_word16_t);
ALLOC(Q16, m+1, spx_word16_t);
#endif
for (i=0;i<m+1;i++)
{
P16[i] = P[i];
Q16[i] = Q[i];
}
/* Search for a zero in P'(z) polynomial first and then alternate to Q'(z).
Keep alternating between the two polynomials as each zero is found */
xr = 0; /* initialise xr to zero */
xl = FREQ_SCALE; /* start at point xl = 1 */
for(j=0;j<lpcrdr;j++){
if(j&1) /* determines whether P' or Q' is eval. */
pt = Q16;
else
pt = P16;
psuml = cheb_poly_eva(pt,xl,m,stack); /* evals poly. at xl */
flag = 1;
while(flag && (xr >= -FREQ_SCALE)){
spx_word16_t dd;
/* Modified by JMV to provide smaller steps around x=+-1 */
#ifdef FIXED_POINT
dd = MULT16_16_Q15(delta,SUB16(FREQ_SCALE, MULT16_16_Q14(MULT16_16_Q14(xl,xl),14000)));
if (psuml<512 && psuml>-512)
dd = PSHR16(dd,1);
#else
dd=delta*(1-.9*xl*xl);
if (fabs(psuml)<.2)
dd *= .5;
#endif
xr = SUB16(xl, dd); /* interval spacing */
psumr = cheb_poly_eva(pt,xr,m,stack);/* poly(xl-delta_x) */
temp_psumr = psumr;
temp_xr = xr;
/* if no sign change increment xr and re-evaluate poly(xr). Repeat til
sign change.
if a sign change has occurred the interval is bisected and then
checked again for a sign change which determines in which
interval the zero lies in.
If there is no sign change between poly(xm) and poly(xl) set interval
between xm and xr else set interval between xl and xr and repeat till
root is located within the specified limits */
if(SIGN_CHANGE(psumr,psuml))
{
roots++;
psumm=psuml;
for(k=0;k<=nb;k++){
#ifdef FIXED_POINT
xm = ADD16(PSHR16(xl,1),PSHR16(xr,1)); /* bisect the interval */
#else
xm = .5*(xl+xr); /* bisect the interval */
#endif
psumm=cheb_poly_eva(pt,xm,m,stack);
/*if(psumm*psuml>0.)*/
if(!SIGN_CHANGE(psumm,psuml))
{
psuml=psumm;
xl=xm;
} else {
psumr=psumm;
xr=xm;
}
}
/* once zero is found, reset initial interval to xr */
freq[j] = X2ANGLE(xm);
xl = xm;
flag = 0; /* reset flag for next search */
}
else{
psuml=temp_psumr;
xl=temp_xr;
}
}
}
return(roots);
}
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;
}
void DEC16(reg_t r1)
{
SUB16(r1, r1, R_ONE);
}
/** 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;
}
void comb_filter(
spx_sig_t *exc, /*decoded excitation*/
spx_sig_t *new_exc, /*enhanced excitation*/
spx_coef_t *ak, /*LPC filter coefs*/
int p, /*LPC order*/
int nsf, /*sub-frame size*/
int pitch, /*pitch period*/
spx_word16_t *pitch_gain, /*pitch gain (3-tap)*/
spx_word16_t comb_gain, /*gain of comb filter*/
CombFilterMem *mem
)
{
int i;
spx_word16_t exc_energy=0, new_exc_energy=0;
spx_word16_t gain;
spx_word16_t step;
spx_word16_t fact;
/*Compute excitation amplitude prior to enhancement*/
exc_energy = compute_rms(exc, nsf);
/*for (i=0;i<nsf;i++)
exc_energy+=((float)exc[i])*exc[i];*/
/*Some gain adjustment if pitch is too high or if unvoiced*/
#ifdef FIXED_POINT
{
spx_word16_t g = gain_3tap_to_1tap(pitch_gain)+gain_3tap_to_1tap(mem->last_pitch_gain);
if (g > 166)
comb_gain = MULT16_16_Q15(DIV32_16(SHL(165,15),g), comb_gain);
if (g < 64)
comb_gain = MULT16_16_Q15(SHL(g, 9), comb_gain);
}
#else
{
float g=0;
g = GAIN_SCALING_1*.5*(gain_3tap_to_1tap(pitch_gain)+gain_3tap_to_1tap(mem->last_pitch_gain));
if (g>1.3)
comb_gain*=1.3/g;
if (g<.5)
comb_gain*=2.*g;
}
#endif
step = DIV32(COMB_STEP, nsf);
fact=0;
/*Apply pitch comb-filter (filter out noise between pitch harmonics)*/
for (i=0;i<nsf;i++)
{
spx_word32_t exc1, exc2;
fact += step;
exc1 = SHL(MULT16_32_Q15(SHL(pitch_gain[0],7),exc[i-pitch+1]) +
MULT16_32_Q15(SHL(pitch_gain[1],7),exc[i-pitch]) +
MULT16_32_Q15(SHL(pitch_gain[2],7),exc[i-pitch-1]) , 2);
exc2 = SHL(MULT16_32_Q15(SHL(mem->last_pitch_gain[0],7),exc[i-mem->last_pitch+1]) +
MULT16_32_Q15(SHL(mem->last_pitch_gain[1],7),exc[i-mem->last_pitch]) +
MULT16_32_Q15(SHL(mem->last_pitch_gain[2],7),exc[i-mem->last_pitch-1]),2);
new_exc[i] = exc[i] + MULT16_32_Q15(comb_gain,MULT16_32_Q15(fact,exc1) + MULT16_32_Q15(SUB16(COMB_STEP,fact), exc2));
}
mem->last_pitch_gain[0] = pitch_gain[0];
mem->last_pitch_gain[1] = pitch_gain[1];
mem->last_pitch_gain[2] = pitch_gain[2];
mem->last_pitch = pitch;
/*Amplitude after enhancement*/
new_exc_energy = compute_rms(new_exc, nsf);
if (exc_energy > new_exc_energy)
exc_energy = new_exc_energy;
gain = DIV32_16(SHL(exc_energy,15),1+new_exc_energy);
#ifdef FIXED_POINT
if (gain < 16384)
gain = 16384;
#else
if (gain < .5)
gain=.5;
#endif
#ifdef FIXED_POINT
for (i=0;i<nsf;i++)
{
mem->smooth_gain = MULT16_16_Q15(31457,mem->smooth_gain) + MULT16_16_Q15(1311,gain);
new_exc[i] = MULT16_32_Q15(mem->smooth_gain, new_exc[i]);
}
#else
for (i=0;i<nsf;i++)
{
mem->smooth_gain = .96*mem->smooth_gain + .04*gain;
new_exc[i] *= mem->smooth_gain;
}
#endif
}
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;
}
void postFilter(bcg729DecoderChannelContextStruct *decoderChannelContext, word16_t *LPCoefficients, word16_t *reconstructedSpeech, int16_t intPitchDelay, int subframeIndex,
word16_t *postFilteredSignal)
{
int i,j;
/********************************************************************/
/* Long Term Post Filter */
/********************************************************************/
/*** Compute LPGammaN and LPGammaD coefficients : LPGamma[0] = LP[0]*Gamma^(i+1) (i=0..9) ***/
word16_t LPGammaNCoefficients[NB_LSP_COEFF]; /* in Q12 */
/* GAMMA_XX constants are in Q15 */
LPGammaNCoefficients[0] = MULT16_16_P15(LPCoefficients[0], GAMMA_N1);
LPGammaNCoefficients[1] = MULT16_16_P15(LPCoefficients[1], GAMMA_N2);
LPGammaNCoefficients[2] = MULT16_16_P15(LPCoefficients[2], GAMMA_N3);
LPGammaNCoefficients[3] = MULT16_16_P15(LPCoefficients[3], GAMMA_N4);
LPGammaNCoefficients[4] = MULT16_16_P15(LPCoefficients[4], GAMMA_N5);
LPGammaNCoefficients[5] = MULT16_16_P15(LPCoefficients[5], GAMMA_N6);
LPGammaNCoefficients[6] = MULT16_16_P15(LPCoefficients[6], GAMMA_N7);
LPGammaNCoefficients[7] = MULT16_16_P15(LPCoefficients[7], GAMMA_N8);
LPGammaNCoefficients[8] = MULT16_16_P15(LPCoefficients[8], GAMMA_N9);
LPGammaNCoefficients[9] = MULT16_16_P15(LPCoefficients[9], GAMMA_N10);
/*** Compute the residual signal as described in spec 4.2.1 eq79 ***/
/* Compute also a scaled residual signal: shift right by 2 to avoid overflows on 32 bits when computing correlation and energy */
/* pointers to current subframe beginning */
word16_t *residualSignal = &(decoderChannelContext->residualSignalBuffer[MAXIMUM_INT_PITCH_DELAY+subframeIndex]);
word16_t *scaledResidualSignal = &(decoderChannelContext->scaledResidualSignalBuffer[MAXIMUM_INT_PITCH_DELAY+subframeIndex]);
for (i=0; i<L_SUBFRAME; i++) {
word32_t acc = SHL((word32_t)reconstructedSpeech[i], 12); /* reconstructedSpeech in Q0 shifted to set acc in Q12 */
for (j=0; j<NB_LSP_COEFF; j++) {
acc = MAC16_16(acc, LPGammaNCoefficients[j],reconstructedSpeech[i-j-1]); /* LPGammaNCoefficients in Q12, reconstructedSpeech in Q0 -> acc in Q12 */
}
residualSignal[i] = (word16_t)SATURATE(PSHR(acc, 12), MAXINT16); /* shift back acc to Q0 and saturate it to avoid overflow when going back to 16 bits */
scaledResidualSignal[i] = PSHR(residualSignal[i], 2); /* shift acc to Q-2 and saturate it to get the scaled version of the signal */
}
/*** Compute the maximum correlation on scaledResidualSignal delayed by intPitchDelay +/- 3 to get the best delay. Spec 4.2.1 eq80 ***/
/* using a scaled(Q-2) signals gives correlation in Q-4. */
word32_t correlationMax = (word32_t)MININT32;
int16_t intPitchDelayMax = intPitchDelay+3; /* intPitchDelayMax shall be < MAXIMUM_INT_PITCH_DELAY(143) */
int16_t bestIntPitchDelay = 0;
word16_t *delayedResidualSignal;
if (intPitchDelayMax>MAXIMUM_INT_PITCH_DELAY) {
intPitchDelayMax = MAXIMUM_INT_PITCH_DELAY;
}
for (i=intPitchDelay-3; i<=intPitchDelayMax; i++) {
word32_t correlation = 0;
delayedResidualSignal = &(scaledResidualSignal[-i]); /* delayedResidualSignal points to scaledResidualSignal[-i] */
/* compute correlation: ∑r(n)*rk(n) */
for (j=0; j<L_SUBFRAME; j++) {
correlation = MAC16_16(correlation, delayedResidualSignal[j], scaledResidualSignal[j]);
}
/* if we have a maximum correlation */
if (correlation>correlationMax) {
correlationMax = correlation;
bestIntPitchDelay = i; /* get the intPitchDelay */
}
}
/* saturate correlation to a positive integer */
if (correlationMax<0) {
correlationMax = 0;
}
/*** Compute the signal energy ∑r(n)*r(n) and delayed signal energy ∑rk(n)*rk(n) which shall be used to compute gl spec 4.2.1 eq81, eq 82 and eq83 ***/
word32_t residualSignalEnergy = 0; /* in Q-4 */
word32_t delayedResidualSignalEnergy = 0; /* in Q-4 */
delayedResidualSignal = &(scaledResidualSignal[-bestIntPitchDelay]); /* in Q-2, points to the residual signal delayed to give the higher correlation: rk(n) */
for (i=0; i<L_SUBFRAME; i++) {
residualSignalEnergy = MAC16_16(residualSignalEnergy, scaledResidualSignal[i], scaledResidualSignal[i]);
delayedResidualSignalEnergy = MAC16_16(delayedResidualSignalEnergy, delayedResidualSignal[i], delayedResidualSignal[i]);
}
/*** Scale correlationMax, residualSignalEnergy and delayedResidualSignalEnergy to the best fit on 16 bits ***/
/* these variables must fit on 16bits for the following computation, to avoid loosing information, scale them */
/* at best fit: scale the higher of three to get the value over 2^14 and shift the other two from the same amount */
/* Note: all three value are >= 0 */
word32_t maximumThree = correlationMax;
if (maximumThree<residualSignalEnergy) {
maximumThree = residualSignalEnergy;
}
if (maximumThree<delayedResidualSignalEnergy) {
maximumThree = delayedResidualSignalEnergy;
}
int16_t leadingZeros = 0;
word16_t correlationMaxWord16 = 0;
word16_t residualSignalEnergyWord16 = 0;
word16_t delayedResidualSignalEnergyWord16 = 0;
if (maximumThree>0) { /* if all of them a null, just do nothing otherwise shift right to get the max number in range [0x4000,0x8000[ */
leadingZeros = countLeadingZeros(maximumThree);
if (leadingZeros<16) {
correlationMaxWord16 = (word16_t)SHR32(correlationMax, 16-leadingZeros);
residualSignalEnergyWord16 = (word16_t)SHR32(residualSignalEnergy, 16-leadingZeros);
delayedResidualSignalEnergyWord16 = (word16_t)SHR32(delayedResidualSignalEnergy, 16-leadingZeros);
} else { /* if the values already fit on 16 bits, no need to shift */
correlationMaxWord16 = (word16_t)correlationMax;
residualSignalEnergyWord16 = (word16_t)residualSignalEnergy;
delayedResidualSignalEnergyWord16 = (word16_t)delayedResidualSignalEnergy;
}
}
/* eq78: Hp(z)=(1 + γp*gl*z(−T))/(1 + γp*gl) -> (with g=γp*gl) Hp(z)=1/(1+g) + (g/(1+g))*z(-T) = g0 + g1*z(-T) */
/* g = gl/2 (as γp=0.5)= (eq83) correlationMax/(2*delayedResidualSignalEnergy) */
/* compute g0 = 1/(1+g)= delayedResidualSignalEnergy/(delayedResidualSignalEnergy+correlationMax/2) = 1-g1*/
/* compute g1 = g/(1+g) = correlationMax/(2*delayedResidualSignalEnergy+correlationMax) = 1-g0 */
/*** eq82 -> (correlationMax^2)/(residualSignalEnergy*delayedResidualSignalEnergy)<0.5 ***/
/* (correlationMax^2) < (residualSignalEnergy*delayedResidualSignalEnergy)*0.5 */
if ((MULT16_16(correlationMaxWord16, correlationMaxWord16) < SHR(MULT16_16(residualSignalEnergyWord16, delayedResidualSignalEnergyWord16), 1)) /* eq82 */
|| ((correlationMaxWord16==0) && (delayedResidualSignalEnergyWord16==0))) { /* correlationMax and delayedResidualSignalEnergy values are 0 -> unable to compute g0 and g1 -> disable filter */
/* long term post filter disabled */
for (i=0; i<L_SUBFRAME; i++) {
decoderChannelContext->longTermFilteredResidualSignal[i] = residualSignal[i];
}
} else { /* eq82 gives long term filter enabled, */
word16_t g0, g1;
/* eq83: gl = correlationMax/delayedResidualSignalEnergy bounded in ]0,1] */
/* check if gl > 1 -> gl=1 -> g=1/2 -> g0=2/3 and g1=1/3 */
if (correlationMax > delayedResidualSignalEnergy) {
g0 = 21845; /* 2/3 in Q15 */
g1 = 10923; /* 1/3 in Q15 */
} else {
/* g1 = correlationMax/(2*delayedResidualSignalEnergy+correlationMax) */
g1 = DIV32((word32_t)SHL32(correlationMaxWord16,15),(word32_t)ADD32(SHL32(delayedResidualSignalEnergyWord16,1), correlationMaxWord16)); /* g1 in Q15 */
g0 = SUB16(32767, g1); /* g0 = 1 - g1 in Q15 */
}
/* longTermFilteredResidualSignal[i] = g0*residualSignal[i] + g1*delayedResidualSignal[i]*/
delayedResidualSignal = &(residualSignal[-bestIntPitchDelay]);
for (i=0; i<L_SUBFRAME; i++) {
decoderChannelContext->longTermFilteredResidualSignal[i] = (word16_t)SATURATE(PSHR(ADD32(MULT16_16(g0, residualSignal[i]), MULT16_16(g1, delayedResidualSignal[i])), 15), MAXINT16);
}
}
/********************************************************************/
/* Tilt Compensation Filter */
/********************************************************************/
/* compute hf the truncated (to 22 coefficients) impulse response of the filter A(z/γn)/A(z/γd) described in spec 4.2.2 eq84 */
/* hf(i) = LPGammaNCoeff[i] - ∑[j:0..9]LPGammaDCoeff[j]*hf[i-j-1]) */
word16_t LPGammaDCoefficients[NB_LSP_COEFF]; /* in Q12 */
/* GAMMA_XX constants are in Q15 */
LPGammaDCoefficients[0] = MULT16_16_P15(LPCoefficients[0], GAMMA_D1);
LPGammaDCoefficients[1] = MULT16_16_P15(LPCoefficients[1], GAMMA_D2);
LPGammaDCoefficients[2] = MULT16_16_P15(LPCoefficients[2], GAMMA_D3);
LPGammaDCoefficients[3] = MULT16_16_P15(LPCoefficients[3], GAMMA_D4);
LPGammaDCoefficients[4] = MULT16_16_P15(LPCoefficients[4], GAMMA_D5);
LPGammaDCoefficients[5] = MULT16_16_P15(LPCoefficients[5], GAMMA_D6);
LPGammaDCoefficients[6] = MULT16_16_P15(LPCoefficients[6], GAMMA_D7);
LPGammaDCoefficients[7] = MULT16_16_P15(LPCoefficients[7], GAMMA_D8);
LPGammaDCoefficients[8] = MULT16_16_P15(LPCoefficients[8], GAMMA_D9);
LPGammaDCoefficients[9] = MULT16_16_P15(LPCoefficients[9], GAMMA_D10);
word16_t hf[22]; /* the truncated impulse response to short term filter Hf in Q12 */
hf[0] = 4096; /* 1 in Q12 as LPGammaNCoefficients and LPGammaDCoefficient doesn't contain the first element which is 1 and past values of hf are 0 */
for (i=1; i<11; i++) {
word32_t acc = (word32_t)SHL(LPGammaNCoefficients[i-1],12); /* LPGammaNCoefficients in Q12 -> acc in Q24 */
for (j=0; j<NB_LSP_COEFF && j<i; j++) { /* j<i to avoid access to negative index of hf(past values are 0 anyway) */
acc = MSU16_16(acc, LPGammaDCoefficients[j], hf[i-j-1]); /* LPGammaDCoefficient in Q12, hf in Q12 -> Q24 TODO: Possible overflow?? */
}
hf[i] = (word16_t)SATURATE(PSHR(acc, 12), MAXINT16); /* get result back in Q12 and saturate on 16 bits */
}
for (i=11; i<22; i++) {
word32_t acc = 0;
for (j=0; j<NB_LSP_COEFF; j++) { /* j<i to avoid access to negative index of hf(past values are 0 anyway) */
acc = MSU16_16(acc, LPGammaDCoefficients[j], hf[i-j-1]); /* LPGammaDCoefficient in Q12, hf in Q12 -> Q24 TODO: Possible overflow?? */
}
hf[i] = (word16_t)SATURATE(PSHR(acc, 12), MAXINT16); /* get result back in Q12 and saturate on 16 bits */
}
/* hf is then used to compute k'1 spec 4.2.3 eq87: k'1 = -rh1/rh0 */
/* rh0 = ∑[i:0..21]hf[i]*hf[i] */
/* rh1 = ∑[i:0..20]hf[i]*hf[i+1] */
word32_t rh1 = MULT16_16(hf[0], hf[1]);
for (i=1; i<21; i++) {
rh1 = MAC16_16(rh1, hf[i], hf[i+1]); /* rh1 in Q24 */
}
/* tiltCompensationGain is set to 0 if k'1>0 -> rh1<0 (as rh0 is always>0) */
word16_t tiltCompensatedSignal[L_SUBFRAME]; /* in Q0 */
if (rh1<0) { /* tiltCompensationGain = 0 -> no gain filter is off, just copy the input */
memcpy(tiltCompensatedSignal, decoderChannelContext->longTermFilteredResidualSignal, L_SUBFRAME*sizeof(word16_t));
} else { /*compute tiltCompensationGain = k'1*γt */
word32_t rh0 = MULT16_16(hf[0], hf[0]);
for (i=1; i<22; i++) {
rh0 = MAC16_16(rh0, hf[i], hf[i]); /* rh0 in Q24 */
}
rh1 = MULT16_32_Q15(GAMMA_T, rh1); /* GAMMA_T in Q15, rh1 in Q24*/
word16_t tiltCompensationGain = (word16_t)SATURATE((word32_t)(DIV32(rh1,PSHR(rh0,12))), MAXINT16); /* rh1 in Q24, PSHR(rh0,12) in Q12 -> tiltCompensationGain in Q12 */
/* compute filter Ht (spec A.4.2.3 eqA14) = 1 + gain*z(-1) */
for (i=0; i<L_SUBFRAME; i++) {
tiltCompensatedSignal[i] = MSU16_16_Q12(decoderChannelContext->longTermFilteredResidualSignal[i], tiltCompensationGain, decoderChannelContext->longTermFilteredResidualSignal[i-1]);
}
}
/* update memory word of longTermFilteredResidualSignal for next subframe */
decoderChannelContext->longTermFilteredResidualSignal[-1] = decoderChannelContext->longTermFilteredResidualSignal[L_SUBFRAME-1];
/********************************************************************/
/* synthesis filter 1/[Â(z /γd)] spec A.4.2.2 */
/* */
/* Note: Â(z/γn) was done before when computing residual signal */
/********************************************************************/
/* shortTermFilteredResidualSignal is accessed in range [-NB_LSP_COEFF,L_SUBFRAME[ */
synthesisFilter(tiltCompensatedSignal, LPGammaDCoefficients, decoderChannelContext->shortTermFilteredResidualSignal);
/* get the last NB_LSP_COEFF of shortTermFilteredResidualSignal and set them as memory for next subframe(they do not overlap so use memcpy) */
memcpy(decoderChannelContext->shortTermFilteredResidualSignalBuffer, &(decoderChannelContext->shortTermFilteredResidualSignalBuffer[L_SUBFRAME]), NB_LSP_COEFF*sizeof(word16_t));
/********************************************************************/
/* Adaptive Gain Control spec A.4.2.4 */
/* */
/********************************************************************/
/*** compute G(gain scaling factor) according to eqA15 : G = Sqrt((∑s(n)^2)/∑sf(n)^2 ) ***/
word16_t gainScalingFactor; /* in Q12 */
/* compute ∑sf(n)^2 scale the signal shifting left by 2 to avoid overflow on 32 bits sum */
word32_t shortTermFilteredResidualSignalSquareSum = 0;
for (i=0; i<L_SUBFRAME; i++) {
shortTermFilteredResidualSignalSquareSum = MAC16_16_Q4(shortTermFilteredResidualSignalSquareSum, decoderChannelContext->shortTermFilteredResidualSignal[i], decoderChannelContext->shortTermFilteredResidualSignal[i]);
}
/* if the sum is null we can't compute gain -> output of postfiltering is the output of shortTermFilter and previousAdaptativeGain is set to 0 */
/* the reset of previousAdaptativeGain is not mentionned in the spec but in ITU code only */
if (shortTermFilteredResidualSignalSquareSum == 0) {
decoderChannelContext->previousAdaptativeGain = 0;
for (i=0; i<L_SUBFRAME; i++) {
postFilteredSignal[i] = decoderChannelContext->shortTermFilteredResidualSignal[i];
}
} else { /* we can compute adaptativeGain and output signal */
/* compute ∑s(n)^2 scale the signal shifting left by 2 to avoid overflow on 32 bits sum */
word32_t reconstructedSpeechSquareSum = 0;
for (i=0; i<L_SUBFRAME; i++) {
reconstructedSpeechSquareSum = MAC16_16_Q4(reconstructedSpeechSquareSum, reconstructedSpeech[i], reconstructedSpeech[i]);
}
if (reconstructedSpeechSquareSum==0) { /* numerator is null -> current gain is null */
gainScalingFactor = 0;
} else {
/* Compute ∑s(n)^2)/∑sf(n)^2 result shall be in Q10 */
/* normalise the numerator on 32 bits */
word16_t numeratorShift = countLeadingZeros(reconstructedSpeechSquareSum);
reconstructedSpeechSquareSum = SHL(reconstructedSpeechSquareSum, numeratorShift); /* reconstructedSpeechSquareSum*2^numeratorShift */
/* normalise denominator to get the result directly in Q10 if possible */
word32_t fractionResult; /* stores ∑s(n)^2)/∑sf(n)^2 */
word32_t scaledShortTermFilteredResidualSignalSquareSum = VSHR32(shortTermFilteredResidualSignalSquareSum, 10-numeratorShift); /* shortTermFilteredResidualSignalSquareSum*2^(numeratorShift-10)*/
if (scaledShortTermFilteredResidualSignalSquareSum==0) {/* shift might have sent to zero the denominator */
fractionResult = DIV32(reconstructedSpeechSquareSum, shortTermFilteredResidualSignalSquareSum); /* result in QnumeratorShift */
fractionResult = VSHR32(fractionResult, numeratorShift-10); /* result in Q10 */
} else { /* ok denominator is still > 0 */
fractionResult = DIV32(reconstructedSpeechSquareSum, scaledShortTermFilteredResidualSignalSquareSum); /* result in Q10 */
}
/* now compute current Gain = Sqrt((∑s(n)^2)/∑sf(n)^2 ) */
/* g729Sqrt_Q0Q7(Q0)->Q7, by giving a Q10 as input, output is in Q12 */
gainScalingFactor = (word16_t)SATURATE(g729Sqrt_Q0Q7(fractionResult), MAXINT16);
/* multiply by 0.1 as described in spec A.4.2.4 */
gainScalingFactor = MULT16_16_P15(gainScalingFactor, 3277); /* in Q12, 3277 = 0.1 in Q15*/
}
/* Compute the signal according to eq89 (spec 4.2.4 and section A4.2.4) */
/* currentGain = 0.9*previousGain + 0.1*gainScalingFactor the 0.1 factor has already been integrated in the variable gainScalingFactor */
/* outputsignal = currentGain*shortTermFilteredResidualSignal */
word16_t currentAdaptativeGain = decoderChannelContext->previousAdaptativeGain;
for (i=0; i<L_SUBFRAME; i++) {
currentAdaptativeGain = ADD16(gainScalingFactor, MULT16_16_P15(currentAdaptativeGain, 29491)); /* 29492 = 0.9 in Q15, result in Q12 */
postFilteredSignal[i] = MULT16_16_Q12(currentAdaptativeGain, decoderChannelContext->shortTermFilteredResidualSignal[i]);
}
decoderChannelContext->previousAdaptativeGain = currentAdaptativeGain;
}
/* shift buffers if needed */
if (subframeIndex>0) { /* only after 2nd subframe treatment */
/* shift left by L_FRAME the residualSignal and scaledResidualSignal buffers */
memmove(decoderChannelContext->residualSignalBuffer, &(decoderChannelContext->residualSignalBuffer[L_FRAME]), MAXIMUM_INT_PITCH_DELAY*sizeof(word16_t));
memmove(decoderChannelContext->scaledResidualSignalBuffer, &(decoderChannelContext->scaledResidualSignalBuffer[L_FRAME]), MAXIMUM_INT_PITCH_DELAY*sizeof(word16_t));
}
return;
}
int MinxCPU_Exec(void)
{
uint8_t I8A, I8B;
uint16_t I16;
// Shift U
if (MinxCPU.Shift_U) {
MinxCPU.U1 = MinxCPU.U2;
MinxCPU.U2 = MinxCPU.PC.B.I;
MinxCPU.Shift_U--;
MinxCPU_OnIRQHandle(MinxCPU.F, MinxCPU.Shift_U);
}
// Check HALT or STOP status
if (MinxCPU.Status != MINX_STATUS_NORMAL) {
if (MinxCPU.Status == MINX_STATUS_IRQ) {
MinxCPU.Status = MINX_STATUS_NORMAL; // Return to normal
CALLI(MinxCPU.IRQ_Vector); // Jump to IRQ vector
return 20;
} else {
return 8; // Cause short NOPs
}
}
// Read IR
MinxCPU.IR = Fetch8();
// Process instruction
switch(MinxCPU.IR) {
case 0x00: // ADD A, A
MinxCPU.BA.B.L = ADD8(MinxCPU.BA.B.L, MinxCPU.BA.B.L);
return 8;
case 0x01: // ADD A, B
MinxCPU.BA.B.L = ADD8(MinxCPU.BA.B.L, MinxCPU.BA.B.H);
return 8;
case 0x02: // ADD A, #nn
I8A = Fetch8();
MinxCPU.BA.B.L = ADD8(MinxCPU.BA.B.L, I8A);
return 8;
case 0x03: // ADD A, [HL]
MinxCPU.BA.B.L = ADD8(MinxCPU.BA.B.L, MinxCPU_OnRead(1, MinxCPU.HL.D));
return 8;
case 0x04: // ADD A, [N+#nn]
I8A = Fetch8();
MinxCPU.BA.B.L = ADD8(MinxCPU.BA.B.L, MinxCPU_OnRead(1, MinxCPU.N.D + I8A));
return 12;
case 0x05: // ADD A, [#nnnn]
I16 = Fetch16();
MinxCPU.BA.B.L = ADD8(MinxCPU.BA.B.L, MinxCPU_OnRead(1, (MinxCPU.HL.B.I << 16) | I16));
return 16;
case 0x06: // ADD A, [X]
MinxCPU.BA.B.L = ADD8(MinxCPU.BA.B.L, MinxCPU_OnRead(1, MinxCPU.X.D));
return 8;
case 0x07: // ADD A, [Y]
MinxCPU.BA.B.L = ADD8(MinxCPU.BA.B.L, MinxCPU_OnRead(1, MinxCPU.Y.D));
return 8;
case 0x08: // ADC A, A
MinxCPU.BA.B.L = ADC8(MinxCPU.BA.B.L, MinxCPU.BA.B.L);
return 8;
case 0x09: // ADC A, B
MinxCPU.BA.B.L = ADC8(MinxCPU.BA.B.L, MinxCPU.BA.B.H);
return 8;
case 0x0A: // ADC A, #nn
I8A = Fetch8();
MinxCPU.BA.B.L = ADC8(MinxCPU.BA.B.L, I8A);
return 8;
case 0x0B: // ADC A, [HL]
MinxCPU.BA.B.L = ADC8(MinxCPU.BA.B.L, MinxCPU_OnRead(1, MinxCPU.HL.D));
return 8;
case 0x0C: // ADC A, [N+#nn]
I8A = Fetch8();
MinxCPU.BA.B.L = ADC8(MinxCPU.BA.B.L, MinxCPU_OnRead(1, MinxCPU.N.D + I8A));
return 12;
case 0x0D: // ADC A, [#nnnn]
I16 = Fetch16();
MinxCPU.BA.B.L = ADC8(MinxCPU.BA.B.L, MinxCPU_OnRead(1, (MinxCPU.HL.B.I << 16) | I16));
return 16;
case 0x0E: // ADC A, [X]
MinxCPU.BA.B.L = ADC8(MinxCPU.BA.B.L, MinxCPU_OnRead(1, MinxCPU.X.D));
return 8;
case 0x0F: // ADC A, [Y]
MinxCPU.BA.B.L = ADC8(MinxCPU.BA.B.L, MinxCPU_OnRead(1, MinxCPU.Y.D));
return 8;
case 0x10: // SUB A, A
MinxCPU.BA.B.L = SUB8(MinxCPU.BA.B.L, MinxCPU.BA.B.L);
return 8;
case 0x11: // SUB A, B
MinxCPU.BA.B.L = SUB8(MinxCPU.BA.B.L, MinxCPU.BA.B.H);
return 8;
case 0x12: // SUB A, #nn
I8A = Fetch8();
MinxCPU.BA.B.L = SUB8(MinxCPU.BA.B.L, I8A);
return 8;
case 0x13: // SUB A, [HL]
MinxCPU.BA.B.L = SUB8(MinxCPU.BA.B.L, MinxCPU_OnRead(1, MinxCPU.HL.D));
return 8;
case 0x14: // SUB A, [N+#nn]
I8A = Fetch8();
MinxCPU.BA.B.L = SUB8(MinxCPU.BA.B.L, MinxCPU_OnRead(1, MinxCPU.N.D + I8A));
return 12;
case 0x15: // SUB A, [#nnnn]
I16 = Fetch16();
MinxCPU.BA.B.L = SUB8(MinxCPU.BA.B.L, MinxCPU_OnRead(1, (MinxCPU.HL.B.I << 16) | I16));
return 16;
case 0x16: // SUB A, [X]
MinxCPU.BA.B.L = SUB8(MinxCPU.BA.B.L, MinxCPU_OnRead(1, MinxCPU.X.D));
return 8;
case 0x17: // SUB A, [Y]
MinxCPU.BA.B.L = SUB8(MinxCPU.BA.B.L, MinxCPU_OnRead(1, MinxCPU.Y.D));
return 8;
case 0x18: // SBC A, A
MinxCPU.BA.B.L = SBC8(MinxCPU.BA.B.L, MinxCPU.BA.B.L);
return 8;
case 0x19: // SBC A, B
MinxCPU.BA.B.L = SBC8(MinxCPU.BA.B.L, MinxCPU.BA.B.H);
return 8;
case 0x1A: // SBC A, #nn
I8A = Fetch8();
MinxCPU.BA.B.L = SBC8(MinxCPU.BA.B.L, I8A);
return 8;
case 0x1B: // SBC A, [HL]
MinxCPU.BA.B.L = SBC8(MinxCPU.BA.B.L, MinxCPU_OnRead(1, MinxCPU.HL.D));
return 8;
case 0x1C: // SBC A, [N+#nn]
I8A = Fetch8();
MinxCPU.BA.B.L = SBC8(MinxCPU.BA.B.L, MinxCPU_OnRead(1, MinxCPU.N.D + I8A));
return 12;
case 0x1D: // SBC A, [#nnnn]
I16 = Fetch16();
MinxCPU.BA.B.L = SBC8(MinxCPU.BA.B.L, MinxCPU_OnRead(1, (MinxCPU.HL.B.I << 16) | I16));
return 16;
case 0x1E: // SBC A, [X]
MinxCPU.BA.B.L = SBC8(MinxCPU.BA.B.L, MinxCPU_OnRead(1, MinxCPU.X.D));
return 8;
case 0x1F: // SBC A, [Y]
MinxCPU.BA.B.L = SBC8(MinxCPU.BA.B.L, MinxCPU_OnRead(1, MinxCPU.Y.D));
return 8;
case 0x20: // AND A, A
MinxCPU.BA.B.L = AND8(MinxCPU.BA.B.L, MinxCPU.BA.B.L);
return 8;
case 0x21: // AND A, B
MinxCPU.BA.B.L = AND8(MinxCPU.BA.B.L, MinxCPU.BA.B.H);
return 8;
case 0x22: // AND A, #nn
I8A = Fetch8();
MinxCPU.BA.B.L = AND8(MinxCPU.BA.B.L, I8A);
return 8;
case 0x23: // AND A, [HL]
MinxCPU.BA.B.L = AND8(MinxCPU.BA.B.L, MinxCPU_OnRead(1, MinxCPU.HL.D));
return 8;
case 0x24: // AND A, [N+#nn]
I8A = Fetch8();
MinxCPU.BA.B.L = AND8(MinxCPU.BA.B.L, MinxCPU_OnRead(1, MinxCPU.N.D + I8A));
return 12;
case 0x25: // AND A, [#nnnn]
I16 = Fetch16();
MinxCPU.BA.B.L = AND8(MinxCPU.BA.B.L, MinxCPU_OnRead(1, (MinxCPU.HL.B.I << 16) | I16));
return 16;
case 0x26: // AND A, [X]
MinxCPU.BA.B.L = AND8(MinxCPU.BA.B.L, MinxCPU_OnRead(1, MinxCPU.X.D));
return 8;
case 0x27: // AND A, [Y]
MinxCPU.BA.B.L = AND8(MinxCPU.BA.B.L, MinxCPU_OnRead(1, MinxCPU.Y.D));
return 8;
case 0x28: // OR A, A
MinxCPU.BA.B.L = OR8(MinxCPU.BA.B.L, MinxCPU.BA.B.L);
return 8;
case 0x29: // OR A, B
MinxCPU.BA.B.L = OR8(MinxCPU.BA.B.L, MinxCPU.BA.B.H);
return 8;
case 0x2A: // OR A, #nn
I8A = Fetch8();
MinxCPU.BA.B.L = OR8(MinxCPU.BA.B.L, I8A);
return 8;
case 0x2B: // OR A, [HL]
MinxCPU.BA.B.L = OR8(MinxCPU.BA.B.L, MinxCPU_OnRead(1, MinxCPU.HL.D));
return 8;
case 0x2C: // OR A, [N+#nn]
I8A = Fetch8();
MinxCPU.BA.B.L = OR8(MinxCPU.BA.B.L, MinxCPU_OnRead(1, MinxCPU.N.D + I8A));
return 12;
case 0x2D: // OR A, [#nnnn]
I16 = Fetch16();
MinxCPU.BA.B.L = OR8(MinxCPU.BA.B.L, MinxCPU_OnRead(1, (MinxCPU.HL.B.I << 16) | I16));
return 16;
case 0x2E: // OR A, [X]
MinxCPU.BA.B.L = OR8(MinxCPU.BA.B.L, MinxCPU_OnRead(1, MinxCPU.X.D));
return 8;
case 0x2F: // OR A, [Y]
MinxCPU.BA.B.L = OR8(MinxCPU.BA.B.L, MinxCPU_OnRead(1, MinxCPU.Y.D));
return 8;
case 0x30: // CMP A, A
SUB8(MinxCPU.BA.B.L, MinxCPU.BA.B.L);
return 8;
case 0x31: // CMP A, B
SUB8(MinxCPU.BA.B.L, MinxCPU.BA.B.H);
return 8;
case 0x32: // CMP A, #nn
I8A = Fetch8();
SUB8(MinxCPU.BA.B.L, I8A);
return 8;
case 0x33: // CMP A, [HL]
SUB8(MinxCPU.BA.B.L, MinxCPU_OnRead(1, MinxCPU.HL.D));
return 8;
case 0x34: // CMP A, [N+#nn]
I8A = Fetch8();
SUB8(MinxCPU.BA.B.L, MinxCPU_OnRead(1, MinxCPU.N.D + I8A));
return 12;
case 0x35: // CMP A, [#nnnn]
I16 = Fetch16();
SUB8(MinxCPU.BA.B.L, MinxCPU_OnRead(1, (MinxCPU.HL.B.I << 16) | I16));
return 16;
case 0x36: // CMP A, [X]
SUB8(MinxCPU.BA.B.L, MinxCPU_OnRead(1, MinxCPU.X.D));
return 8;
case 0x37: // CMP A, [Y]
SUB8(MinxCPU.BA.B.L, MinxCPU_OnRead(1, MinxCPU.Y.D));
return 8;
case 0x38: // XOR A, A
MinxCPU.BA.B.L = XOR8(MinxCPU.BA.B.L, MinxCPU.BA.B.L);
return 8;
case 0x39: // XOR A, B
MinxCPU.BA.B.L = XOR8(MinxCPU.BA.B.L, MinxCPU.BA.B.H);
return 8;
case 0x3A: // XOR A, #nn
I8A = Fetch8();
MinxCPU.BA.B.L = XOR8(MinxCPU.BA.B.L, I8A);
return 8;
case 0x3B: // XOR A, [HL]
MinxCPU.BA.B.L = XOR8(MinxCPU.BA.B.L, MinxCPU_OnRead(1, MinxCPU.HL.D));
return 8;
case 0x3C: // XOR A, [N+#nn]
I8A = Fetch8();
MinxCPU.BA.B.L = XOR8(MinxCPU.BA.B.L, MinxCPU_OnRead(1, MinxCPU.N.D + I8A));
return 12;
case 0x3D: // XOR A, [#nnnn]
I16 = Fetch16();
MinxCPU.BA.B.L = XOR8(MinxCPU.BA.B.L, MinxCPU_OnRead(1, (MinxCPU.HL.B.I << 16) | I16));
return 16;
case 0x3E: // XOR A, [X]
MinxCPU.BA.B.L = XOR8(MinxCPU.BA.B.L, MinxCPU_OnRead(1, MinxCPU.X.D));
return 8;
case 0x3F: // XOR A, [Y]
MinxCPU.BA.B.L = XOR8(MinxCPU.BA.B.L, MinxCPU_OnRead(1, MinxCPU.Y.D));
return 8;
case 0x40: // MOV A, A
return 4;
case 0x41: // MOV A, B
MinxCPU.BA.B.L = MinxCPU.BA.B.H;
return 4;
case 0x42: // MOV A, L
MinxCPU.BA.B.L = MinxCPU.HL.B.L;
return 4;
case 0x43: // MOV A, H
MinxCPU.BA.B.L = MinxCPU.HL.B.H;
return 4;
case 0x44: // MOV A, [N+#nn]
I8A = Fetch8();
MinxCPU.BA.B.L = MinxCPU_OnRead(1, MinxCPU.N.D + I8A);
return 12;
case 0x45: // MOV A, [HL]
MinxCPU.BA.B.L = MinxCPU_OnRead(1, MinxCPU.HL.D);
return 8;
case 0x46: // MOV A, [X]
MinxCPU.BA.B.L = MinxCPU_OnRead(1, MinxCPU.X.D);
return 8;
case 0x47: // MOV A, [Y]
MinxCPU.BA.B.L = MinxCPU_OnRead(1, MinxCPU.Y.D);
return 8;
case 0x48: // MOV B, A
MinxCPU.BA.B.H = MinxCPU.BA.B.L;
return 4;
case 0x49: // MOV B, B
return 4;
case 0x4A: // MOV B, L
MinxCPU.BA.B.H = MinxCPU.HL.B.L;
return 4;
case 0x4B: // MOV B, H
MinxCPU.BA.B.H = MinxCPU.HL.B.H;
return 4;
case 0x4C: // MOV B, [N+#nn]
I8A = Fetch8();
MinxCPU.BA.B.H = MinxCPU_OnRead(1, MinxCPU.N.D + I8A);
return 12;
case 0x4D: // MOV B, [HL]
MinxCPU.BA.B.H = MinxCPU_OnRead(1, MinxCPU.HL.D);
return 8;
case 0x4E: // MOV B, [X]
MinxCPU.BA.B.H = MinxCPU_OnRead(1, MinxCPU.X.D);
return 8;
case 0x4F: // MOV B, [Y]
MinxCPU.BA.B.H = MinxCPU_OnRead(1, MinxCPU.Y.D);
return 8;
case 0x50: // MOV L, A
MinxCPU.HL.B.L = MinxCPU.BA.B.L;
return 4;
case 0x51: // MOV L, B
MinxCPU.HL.B.L = MinxCPU.BA.B.H;
return 4;
case 0x52: // MOV L, L
return 4;
case 0x53: // MOV L, H
MinxCPU.HL.B.L = MinxCPU.HL.B.H;
return 4;
case 0x54: // MOV L, [N+#nn]
I8A = Fetch8();
MinxCPU.HL.B.L = MinxCPU_OnRead(1, MinxCPU.N.D + I8A);
return 12;
case 0x55: // MOV L, [HL]
MinxCPU.HL.B.L = MinxCPU_OnRead(1, MinxCPU.HL.D);
return 8;
case 0x56: // MOV L, [X]
MinxCPU.HL.B.L = MinxCPU_OnRead(1, MinxCPU.X.D);
return 8;
case 0x57: // MOV L, [Y]
MinxCPU.HL.B.L = MinxCPU_OnRead(1, MinxCPU.Y.D);
return 8;
case 0x58: // MOV H, A
MinxCPU.HL.B.H = MinxCPU.BA.B.L;
return 4;
case 0x59: // MOV H, B
MinxCPU.HL.B.H = MinxCPU.BA.B.H;
return 4;
case 0x5A: // MOV H, L
MinxCPU.HL.B.H = MinxCPU.HL.B.L;
return 4;
case 0x5B: // MOV H, H
return 4;
case 0x5C: // MOV H, [N+#nn]
I8A = Fetch8();
MinxCPU.HL.B.H = MinxCPU_OnRead(1, MinxCPU.N.D + I8A);
return 12;
case 0x5D: // MOV H, [HL]
MinxCPU.HL.B.H = MinxCPU_OnRead(1, MinxCPU.HL.D);
return 8;
case 0x5E: // MOV H, [X]
MinxCPU.HL.B.H = MinxCPU_OnRead(1, MinxCPU.X.D);
return 8;
case 0x5F: // MOV H, [Y]
MinxCPU.HL.B.H = MinxCPU_OnRead(1, MinxCPU.Y.D);
return 8;
case 0x60: // MOV [X], A
MinxCPU_OnWrite(1, MinxCPU.X.D, MinxCPU.BA.B.L);
return 8;
case 0x61: // MOV [X], B
MinxCPU_OnWrite(1, MinxCPU.X.D, MinxCPU.BA.B.H);
return 8;
case 0x62: // MOV [X], L
MinxCPU_OnWrite(1, MinxCPU.X.D, MinxCPU.HL.B.L);
return 8;
case 0x63: // MOV [X], H
MinxCPU_OnWrite(1, MinxCPU.X.D, MinxCPU.HL.B.H);
return 8;
case 0x64: // MOV [X], [N+#nn]
I8A = Fetch8();
MinxCPU_OnWrite(1, MinxCPU.X.D, MinxCPU_OnRead(1, MinxCPU.N.D + I8A));
return 16;
case 0x65: // MOV [X], [HL]
MinxCPU_OnWrite(1, MinxCPU.X.D, MinxCPU_OnRead(1, MinxCPU.HL.D));
return 12;
case 0x66: // MOV [X], [X]
MinxCPU_OnWrite(1, MinxCPU.X.D, MinxCPU_OnRead(1, MinxCPU.X.D));
return 12;
case 0x67: // MOV [X], [Y]
MinxCPU_OnWrite(1, MinxCPU.X.D, MinxCPU_OnRead(1, MinxCPU.Y.D));
return 12;
case 0x68: // MOV [HL], A
MinxCPU_OnWrite(1, MinxCPU.HL.D, MinxCPU.BA.B.L);
return 8;
case 0x69: // MOV [HL], B
MinxCPU_OnWrite(1, MinxCPU.HL.D, MinxCPU.BA.B.H);
return 8;
case 0x6A: // MOV [HL], L
MinxCPU_OnWrite(1, MinxCPU.HL.D, MinxCPU.HL.B.L);
return 8;
case 0x6B: // MOV [HL], H
MinxCPU_OnWrite(1, MinxCPU.HL.D, MinxCPU.HL.B.H);
return 8;
case 0x6C: // MOV [HL], [N+#nn]
I8A = Fetch8();
MinxCPU_OnWrite(1, MinxCPU.HL.D, MinxCPU_OnRead(1, MinxCPU.N.D + I8A));
return 16;
case 0x6D: // MOV [HL], [HL]
MinxCPU_OnWrite(1, MinxCPU.HL.D, MinxCPU_OnRead(1, MinxCPU.HL.D));
return 12;
case 0x6E: // MOV [HL], [X]
MinxCPU_OnWrite(1, MinxCPU.HL.D, MinxCPU_OnRead(1, MinxCPU.X.D));
return 12;
case 0x6F: // MOV [HL], [Y]
MinxCPU_OnWrite(1, MinxCPU.HL.D, MinxCPU_OnRead(1, MinxCPU.Y.D));
return 12;
case 0x70: // MOV [Y], A
MinxCPU_OnWrite(1, MinxCPU.Y.D, MinxCPU.BA.B.L);
return 8;
case 0x71: // MOV [Y], B
MinxCPU_OnWrite(1, MinxCPU.Y.D, MinxCPU.BA.B.H);
return 8;
case 0x72: // MOV [Y], L
MinxCPU_OnWrite(1, MinxCPU.Y.D, MinxCPU.HL.B.L);
return 8;
case 0x73: // MOV [Y], H
MinxCPU_OnWrite(1, MinxCPU.Y.D, MinxCPU.HL.B.H);
return 8;
case 0x74: // MOV [Y], [N+#nn]
I8A = Fetch8();
MinxCPU_OnWrite(1, MinxCPU.Y.D, MinxCPU_OnRead(1, MinxCPU.N.D + I8A));
return 16;
case 0x75: // MOV [Y], [HL]
MinxCPU_OnWrite(1, MinxCPU.Y.D, MinxCPU_OnRead(1, MinxCPU.HL.D));
return 12;
case 0x76: // MOV [Y], [X]
MinxCPU_OnWrite(1, MinxCPU.Y.D, MinxCPU_OnRead(1, MinxCPU.X.D));
return 12;
case 0x77: // MOV [Y], [Y]
MinxCPU_OnWrite(1, MinxCPU.Y.D, MinxCPU_OnRead(1, MinxCPU.Y.D));
return 12;
case 0x78: // MOV [N+#nn], A
I8A = Fetch8();
MinxCPU_OnWrite(1, MinxCPU.N.D + I8A, MinxCPU.BA.B.L);
return 8;
case 0x79: // MOV [N+#nn], B
I8A = Fetch8();
MinxCPU_OnWrite(1, MinxCPU.N.D + I8A, MinxCPU.BA.B.H);
return 8;
case 0x7A: // MOV [N+#nn], L
I8A = Fetch8();
MinxCPU_OnWrite(1, MinxCPU.N.D + I8A, MinxCPU.HL.B.L);
return 8;
case 0x7B: // MOV [N+#nn], H
I8A = Fetch8();
MinxCPU_OnWrite(1, MinxCPU.N.D + I8A, MinxCPU.HL.B.H);
return 8;
case 0x7C: // NOTHING #nn
I8A = Fetch8();
return 64;
case 0x7D: // MOV [N+#nn], [HL]
I8A = Fetch8();
MinxCPU_OnWrite(1, MinxCPU.N.D + I8A, MinxCPU_OnRead(1, MinxCPU.HL.D));
return 16;
case 0x7E: // MOV [N+#nn], [X]
I8A = Fetch8();
MinxCPU_OnWrite(1, MinxCPU.N.D + I8A, MinxCPU_OnRead(1, MinxCPU.X.D));
return 16;
case 0x7F: // MOV [N+#nn], [Y]
I8A = Fetch8();
MinxCPU_OnWrite(1, MinxCPU.N.D + I8A, MinxCPU_OnRead(1, MinxCPU.Y.D));
return 16;
case 0x80: // INC A
MinxCPU.BA.B.L = INC8(MinxCPU.BA.B.L);
return 8;
case 0x81: // INC B
MinxCPU.BA.B.H = INC8(MinxCPU.BA.B.H);
return 8;
case 0x82: // INC L
MinxCPU.HL.B.L = INC8(MinxCPU.HL.B.L);
return 8;
case 0x83: // INC H
MinxCPU.HL.B.H = INC8(MinxCPU.HL.B.H);
return 8;
case 0x84: // INC N
MinxCPU.N.B.H = INC8(MinxCPU.N.B.H);
return 8;
case 0x85: // INC [N+#nn]
I8A = Fetch8();
MinxCPU_OnWrite(1, MinxCPU.N.D + I8A, INC8(MinxCPU_OnRead(1, MinxCPU.N.D + I8A)));
return 16;
case 0x86: // INC [HL]
MinxCPU_OnWrite(1, MinxCPU.HL.D, INC8(MinxCPU_OnRead(1, MinxCPU.HL.D)));
return 12;
case 0x87: // INC SP
MinxCPU.SP.W.L = INC16(MinxCPU.SP.W.L);
return 8;
case 0x88: // DEC A
MinxCPU.BA.B.L = DEC8(MinxCPU.BA.B.L);
return 8;
case 0x89: // DEC B
MinxCPU.BA.B.H = DEC8(MinxCPU.BA.B.H);
return 8;
case 0x8A: // DEC L
MinxCPU.HL.B.L = DEC8(MinxCPU.HL.B.L);
return 8;
case 0x8B: // DEC H
MinxCPU.HL.B.H = DEC8(MinxCPU.HL.B.H);
return 8;
case 0x8C: // DEC N
MinxCPU.N.B.H = DEC8(MinxCPU.N.B.H);
return 8;
case 0x8D: // DEC [N+#nn]
I8A = Fetch8();
MinxCPU_OnWrite(1, MinxCPU.N.D + I8A, DEC8(MinxCPU_OnRead(1, MinxCPU.N.D + I8A)));
return 16;
case 0x8E: // DEC [HL]
MinxCPU_OnWrite(1, MinxCPU.HL.D, DEC8(MinxCPU_OnRead(1, MinxCPU.HL.D)));
return 12;
case 0x8F: // DEC SP
MinxCPU.SP.W.L = DEC16(MinxCPU.SP.W.L);
return 8;
case 0x90: // INC BA
MinxCPU.BA.W.L = INC16(MinxCPU.BA.W.L);
return 8;
case 0x91: // INC HL
MinxCPU.HL.W.L = INC16(MinxCPU.HL.W.L);
return 8;
case 0x92: // INC X
MinxCPU.X.W.L = INC16(MinxCPU.X.W.L);
return 8;
case 0x93: // INC Y
MinxCPU.Y.W.L = INC16(MinxCPU.Y.W.L);
return 8;
case 0x94: // TST A, B
AND8(MinxCPU.BA.B.L, MinxCPU.BA.B.H);
return 8;
case 0x95: // TST [HL], #nn
I8A = Fetch8();
AND8(MinxCPU_OnRead(1, MinxCPU.HL.D), I8A);
return 12;
case 0x96: // TST A, #nn
I8A = Fetch8();
AND8(MinxCPU.BA.B.L, I8A);
return 8;
case 0x97: // TST B, #nn
I8A = Fetch8();
AND8(MinxCPU.BA.B.H, I8A);
return 8;
case 0x98: // DEC BA
MinxCPU.BA.W.L = DEC16(MinxCPU.BA.W.L);
return 8;
case 0x99: // DEC HL
MinxCPU.HL.W.L = DEC16(MinxCPU.HL.W.L);
return 8;
case 0x9A: // DEC X
MinxCPU.X.W.L = DEC16(MinxCPU.X.W.L);
return 8;
case 0x9B: // DEC Y
MinxCPU.Y.W.L = DEC16(MinxCPU.Y.W.L);
return 8;
case 0x9C: // AND F, #nn
I8A = Fetch8();
MinxCPU.F = MinxCPU.F & I8A;
MinxCPU_OnIRQHandle(MinxCPU.F, MinxCPU.Shift_U);
return 12;
case 0x9D: // OR F, #nn
I8A = Fetch8();
MinxCPU.F = MinxCPU.F | I8A;
MinxCPU_OnIRQHandle(MinxCPU.F, MinxCPU.Shift_U);
return 12;
case 0x9E: // XOR F, #nn
I8A = Fetch8();
MinxCPU.F = MinxCPU.F ^ I8A;
MinxCPU_OnIRQHandle(MinxCPU.F, MinxCPU.Shift_U);
return 12;
case 0x9F: // MOV F, #nn
I8A = Fetch8();
MinxCPU.F = I8A;
MinxCPU_OnIRQHandle(MinxCPU.F, MinxCPU.Shift_U);
return 12;
case 0xA0: // PUSH BA
PUSH(MinxCPU.BA.B.H);
PUSH(MinxCPU.BA.B.L);
return 16;
case 0xA1: // PUSH HL
PUSH(MinxCPU.HL.B.H);
PUSH(MinxCPU.HL.B.L);
return 16;
case 0xA2: // PUSH X
PUSH(MinxCPU.X.B.H);
PUSH(MinxCPU.X.B.L);
return 16;
case 0xA3: // PUSH Y
PUSH(MinxCPU.Y.B.H);
PUSH(MinxCPU.Y.B.L);
return 16;
case 0xA4: // PUSH N
PUSH(MinxCPU.N.B.H);
return 12;
case 0xA5: // PUSH I
PUSH(MinxCPU.HL.B.I);
return 12;
case 0xA6: // PUSHX
PUSH(MinxCPU.X.B.I);
PUSH(MinxCPU.Y.B.I);
return 16;
case 0xA7: // PUSH F
PUSH(MinxCPU.F);
return 12;
case 0xA8: // POP BA
MinxCPU.BA.B.L = POP();
MinxCPU.BA.B.H = POP();
return 12;
case 0xA9: // POP HL
MinxCPU.HL.B.L = POP();
MinxCPU.HL.B.H = POP();
return 12;
case 0xAA: // POP X
MinxCPU.X.B.L = POP();
MinxCPU.X.B.H = POP();
return 12;
case 0xAB: // POP Y
MinxCPU.Y.B.L = POP();
MinxCPU.Y.B.H = POP();
return 12;
case 0xAC: // POP N
MinxCPU.N.B.H = POP();
return 8;
case 0xAD: // POP I
MinxCPU.HL.B.I = POP();
MinxCPU.N.B.I = MinxCPU.HL.B.I;
return 8;
case 0xAE: // POPX
MinxCPU.Y.B.I = POP();
MinxCPU.X.B.I = POP();
return 12;
case 0xAF: // POP F
MinxCPU.F = POP();
MinxCPU_OnIRQHandle(MinxCPU.F, MinxCPU.Shift_U);
return 8;
case 0xB0: // MOV A, #nn
I8A = Fetch8();
MinxCPU.BA.B.L = I8A;
return 8;
case 0xB1: // MOV B, #nn
I8A = Fetch8();
MinxCPU.BA.B.H = I8A;
return 8;
case 0xB2: // MOV L, #nn
I8A = Fetch8();
MinxCPU.HL.B.L = I8A;
return 8;
case 0xB3: // MOV H, #nn
I8A = Fetch8();
MinxCPU.HL.B.H = I8A;
return 8;
case 0xB4: // MOV N, #nn
I8A = Fetch8();
MinxCPU.N.B.H = I8A;
return 8;
case 0xB5: // MOV [HL], #nn
I8A = Fetch8();
MinxCPU_OnWrite(1, MinxCPU.HL.D, I8A);
return 12;
case 0xB6: // MOV [X], #nn
I8A = Fetch8();
MinxCPU_OnWrite(1, MinxCPU.X.D, I8A);
return 12;
case 0xB7: // MOV [Y], #nn
I8A = Fetch8();
MinxCPU_OnWrite(1, MinxCPU.Y.D, I8A);
return 12;
case 0xB8: // MOV BA, [#nnnn]
I16 = Fetch16();
MinxCPU.BA.B.L = MinxCPU_OnRead(1, (MinxCPU.HL.B.I << 16) | I16++);
MinxCPU.BA.B.H = MinxCPU_OnRead(1, (MinxCPU.HL.B.I << 16) | I16);
return 20;
case 0xB9: // MOV HL, [#nnnn]
I16 = Fetch16();
MinxCPU.HL.B.L = MinxCPU_OnRead(1, (MinxCPU.HL.B.I << 16) | I16++);
MinxCPU.HL.B.H = MinxCPU_OnRead(1, (MinxCPU.HL.B.I << 16) | I16);
return 20;
case 0xBA: // MOV X, [#nnnn]
I16 = Fetch16();
MinxCPU.X.B.L = MinxCPU_OnRead(1, (MinxCPU.HL.B.I << 16) | I16++);
MinxCPU.X.B.H = MinxCPU_OnRead(1, (MinxCPU.HL.B.I << 16) | I16);
return 20;
case 0xBB: // MOV Y, [#nnnn]
I16 = Fetch16();
MinxCPU.Y.B.L = MinxCPU_OnRead(1, (MinxCPU.HL.B.I << 16) | I16++);
MinxCPU.Y.B.H = MinxCPU_OnRead(1, (MinxCPU.HL.B.I << 16) | I16);
return 20;
case 0xBC: // MOV [#nnnn], BA
I16 = Fetch16();
MinxCPU_OnWrite(1, (MinxCPU.HL.B.I << 16) | I16++, MinxCPU.BA.B.L);
MinxCPU_OnWrite(1, (MinxCPU.HL.B.I << 16) | I16, MinxCPU.BA.B.H);
return 20;
case 0xBD: // MOV [#nnnn], HL
I16 = Fetch16();
MinxCPU_OnWrite(1, (MinxCPU.HL.B.I << 16) | I16++, MinxCPU.HL.B.L);
MinxCPU_OnWrite(1, (MinxCPU.HL.B.I << 16) | I16, MinxCPU.HL.B.H);
return 20;
case 0xBE: // MOV [#nnnn], X
I16 = Fetch16();
MinxCPU_OnWrite(1, (MinxCPU.HL.B.I << 16) | I16++, MinxCPU.X.B.L);
MinxCPU_OnWrite(1, (MinxCPU.HL.B.I << 16) | I16, MinxCPU.X.B.H);
return 20;
case 0xBF: // MOV [#nnnn], Y
I16 = Fetch16();
MinxCPU_OnWrite(1, (MinxCPU.HL.B.I << 16) | I16++, MinxCPU.Y.B.L);
MinxCPU_OnWrite(1, (MinxCPU.HL.B.I << 16) | I16, MinxCPU.Y.B.H);
return 20;
case 0xC0: // ADD BA, #nnnn
I16 = Fetch16();
MinxCPU.BA.W.L = ADD16(MinxCPU.BA.W.L, I16);
return 12;
case 0xC1: // ADD HL, #nnnn
I16 = Fetch16();
MinxCPU.HL.W.L = ADD16(MinxCPU.HL.W.L, I16);
return 12;
case 0xC2: // ADD X, #nnnn
I16 = Fetch16();
MinxCPU.X.W.L = ADD16(MinxCPU.X.W.L, I16);
return 12;
case 0xC3: // ADD Y, #nnnn
I16 = Fetch16();
MinxCPU.Y.W.L = ADD16(MinxCPU.Y.W.L, I16);
return 12;
case 0xC4: // MOV BA, #nnnn
I16 = Fetch16();
MinxCPU.BA.W.L = I16;
return 12;
case 0xC5: // MOV HL, #nnnn
I16 = Fetch16();
MinxCPU.HL.W.L = I16;
return 12;
case 0xC6: // MOV X, #nnnn
I16 = Fetch16();
MinxCPU.X.W.L = I16;
return 12;
case 0xC7: // MOV Y, #nnnn
I16 = Fetch16();
MinxCPU.Y.W.L = I16;
return 12;
case 0xC8: // XCHG BA, HL
I16 = MinxCPU.HL.W.L;
MinxCPU.HL.W.L = MinxCPU.BA.W.L;
MinxCPU.BA.W.L = I16;
return 12;
case 0xC9: // XCHG BA, X
I16 = MinxCPU.X.W.L;
MinxCPU.X.W.L = MinxCPU.BA.W.L;
MinxCPU.BA.W.L = I16;
return 12;
case 0xCA: // XCHG BA, Y
I16 = MinxCPU.Y.W.L;
MinxCPU.Y.W.L = MinxCPU.BA.W.L;
MinxCPU.BA.W.L = I16;
return 12;
case 0xCB: // XCHG BA, SP
I16 = MinxCPU.SP.W.L;
MinxCPU.SP.W.L = MinxCPU.BA.W.L;
MinxCPU.BA.W.L = I16;
return 12;
case 0xCC: // XCHG A, B
I8A = MinxCPU.BA.B.H;
MinxCPU.BA.B.H = MinxCPU.BA.B.L;
MinxCPU.BA.B.L = I8A;
return 8;
case 0xCD: // XCHG A, [HL]
I8A = MinxCPU_OnRead(1, MinxCPU.HL.D);
MinxCPU_OnWrite(1, MinxCPU.HL.D, MinxCPU.BA.B.L);
MinxCPU.BA.B.L = I8A;
return 12;
case 0xCE: // Expand 0
return MinxCPU_ExecCE();
case 0xCF: // Expand 1
return MinxCPU_ExecCF();
case 0xD0: // SUB BA, #nnnn
I16 = Fetch16();
MinxCPU.BA.W.L = SUB16(MinxCPU.BA.W.L, I16);
return 12;
case 0xD1: // SUB HL, #nnnn
I16 = Fetch16();
MinxCPU.HL.W.L = SUB16(MinxCPU.HL.W.L, I16);
return 12;
case 0xD2: // SUB X, #nnnn
I16 = Fetch16();
MinxCPU.X.W.L = SUB16(MinxCPU.X.W.L, I16);
return 12;
case 0xD3: // SUB Y, #nnnn
I16 = Fetch16();
MinxCPU.Y.W.L = SUB16(MinxCPU.Y.W.L, I16);
return 12;
case 0xD4: // CMP BA, #nnnn
I16 = Fetch16();
SUB16(MinxCPU.BA.W.L, I16);
return 12;
case 0xD5: // CMP HL, #nnnn
I16 = Fetch16();
SUB16(MinxCPU.HL.W.L, I16);
return 12;
case 0xD6: // CMP X, #nnnn
I16 = Fetch16();
SUB16(MinxCPU.X.W.L, I16);
return 12;
case 0xD7: // CMP Y, #nnnn
I16 = Fetch16();
SUB16(MinxCPU.Y.W.L, I16);
return 12;
case 0xD8: // AND [N+#nn], #nn
I8A = Fetch8();
I8B = Fetch8();
MinxCPU_OnWrite(1, MinxCPU.N.D + I8A, AND8(MinxCPU_OnRead(1, MinxCPU.N.D + I8A), I8B));
return 20;
case 0xD9: // OR [N+#nn], #nn
I8A = Fetch8();
I8B = Fetch8();
MinxCPU_OnWrite(1, MinxCPU.N.D + I8A, OR8(MinxCPU_OnRead(1, MinxCPU.N.D + I8A), I8B));
return 20;
case 0xDA: // XOR [N+#nn], #nn
I8A = Fetch8();
I8B = Fetch8();
MinxCPU_OnWrite(1, MinxCPU.N.D + I8A, XOR8(MinxCPU_OnRead(1, MinxCPU.N.D + I8A), I8B));
return 20;
case 0xDB: // CMP [N+#nn], #nn
I8A = Fetch8();
I8B = Fetch8();
SUB8(MinxCPU_OnRead(1, MinxCPU.N.D + I8A), I8B);
return 16;
case 0xDC: // TST [N+#nn], #nn
I8A = Fetch8();
I8B = Fetch8();
AND8(MinxCPU_OnRead(1, MinxCPU.N.D + I8A), I8B);
return 16;
case 0xDD: // MOV [N+#nn], #nn
I8A = Fetch8();
I8B = Fetch8();
MinxCPU_OnWrite(1, MinxCPU.N.D + I8A, I8B);
return 16;
case 0xDE: // PACK
MinxCPU.BA.B.L = (MinxCPU.BA.B.L & 0x0F) | (MinxCPU.BA.B.H << 4);
return 8;
case 0xDF: // UNPACK
MinxCPU.BA.B.H = (MinxCPU.BA.B.L >> 4);
MinxCPU.BA.B.L = MinxCPU.BA.B.L & 0x0F;
return 8;
case 0xE0: // CALLC #ss
I8A = Fetch8();
if (MinxCPU.F & MINX_FLAG_CARRY) {
CALLS(S8_TO_16(I8A));
return 20;
}
return 8;
case 0xE1: // CALLNC #ss
I8A = Fetch8();
if (!(MinxCPU.F & MINX_FLAG_CARRY)) {
CALLS(S8_TO_16(I8A));
return 20;
}
return 8;
case 0xE2: // CALLZ #ss
I8A = Fetch8();
if (MinxCPU.F & MINX_FLAG_ZERO) {
CALLS(S8_TO_16(I8A));
return 20;
}
return 8;
case 0xE3: // CALLNZ #ss
I8A = Fetch8();
if (!(MinxCPU.F & MINX_FLAG_ZERO)) {
CALLS(S8_TO_16(I8A));
return 20;
}
return 8;
case 0xE4: // JC #ss
I8A = Fetch8();
if (MinxCPU.F & MINX_FLAG_CARRY) {
JMPS(S8_TO_16(I8A));
}
return 8;
case 0xE5: // JNC #ss
I8A = Fetch8();
if (!(MinxCPU.F & MINX_FLAG_CARRY)) {
JMPS(S8_TO_16(I8A));
}
return 8;
case 0xE6: // JZ #ss
I8A = Fetch8();
if (MinxCPU.F & MINX_FLAG_ZERO) {
JMPS(S8_TO_16(I8A));
}
return 8;
case 0xE7: // JNZ #ss
I8A = Fetch8();
if (!(MinxCPU.F & MINX_FLAG_ZERO)) {
JMPS(S8_TO_16(I8A));
}
return 8;
case 0xE8: // CALLC #ssss
I16 = Fetch16();
if (MinxCPU.F & MINX_FLAG_CARRY) {
CALLS(I16);
return 24;
}
return 12;
case 0xE9: // CALLNC #ssss
I16 = Fetch16();
if (!(MinxCPU.F & MINX_FLAG_CARRY)) {
CALLS(I16);
return 24;
}
return 12;
case 0xEA: // CALLZ #ssss
I16 = Fetch16();
if (MinxCPU.F & MINX_FLAG_ZERO) {
CALLS(I16);
return 24;
}
return 12;
case 0xEB: // CALLNZ #ssss
I16 = Fetch16();
if (!(MinxCPU.F & MINX_FLAG_ZERO)) {
CALLS(I16);
return 24;
}
return 12;
case 0xEC: // JC #ssss
I16 = Fetch16();
if (MinxCPU.F & MINX_FLAG_CARRY) {
JMPS(I16);
}
return 12;
case 0xED: // JNC #ssss
I16 = Fetch16();
if (!(MinxCPU.F & MINX_FLAG_CARRY)) {
JMPS(I16);
}
return 12;
case 0xEE: // JZ #ssss
I16 = Fetch16();
if (MinxCPU.F & MINX_FLAG_ZERO) {
JMPS(I16);
}
return 12;
case 0xEF: // JNZ #ssss
I16 = Fetch16();
if (!(MinxCPU.F & MINX_FLAG_ZERO)) {
JMPS(I16);
}
return 12;
case 0xF0: // CALL #ss
I8A = Fetch8();
CALLS(S8_TO_16(I8A));
return 20;
case 0xF1: // JMP #ss
I8A = Fetch8();
JMPS(S8_TO_16(I8A));
return 8;
case 0xF2: // CALL #ssss
I16 = Fetch16();
CALLS(I16);
return 24;
case 0xF3: // JMP #ssss
I16 = Fetch16();
JMPS(I16);
return 12;
case 0xF4: // JMP HL
JMPU(MinxCPU.HL.W.L);
return 8;
case 0xF5: // JDBNZ #ss
I8A = Fetch8();
JDBNZ(S8_TO_16(I8A));
return 16;
case 0xF6: // SWAP A
MinxCPU.BA.B.L = SWAP(MinxCPU.BA.B.L);
return 8;
case 0xF7: // SWAP [HL]
MinxCPU_OnWrite(1, MinxCPU.HL.D, SWAP(MinxCPU_OnRead(1, MinxCPU.HL.D)));
return 12;
case 0xF8: // RET
RET();
return 16;
case 0xF9: // RETI
RETI();
return 16;
case 0xFA: // RETSKIP
RET();
MinxCPU.PC.W.L = MinxCPU.PC.W.L + 2;
return 16;
case 0xFB: // CALL [#nnnn]
I16 = Fetch16();
CALLX(I16);
return 20;
case 0xFC: // CINT #nn
I16 = Fetch8();
CALLI(I16);
return 20;
case 0xFD: // JINT #nn
I16 = Fetch8();
JMPI(I16);
return 8;
case 0xFE: // CRASH
MinxCPU_OnException(EXCEPTION_CRASH_INSTRUCTION, 0xFE);
return 4;
case 0xFF: // NOP
return 8;
default:
MinxCPU_OnException(EXCEPTION_UNKNOWN_INSTRUCTION, MinxCPU.IR);
return 4;
}
}
/** 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_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 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));
}
}
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]);
}
static inline void target_update(spx_word16_t *t, spx_word16_t g, spx_word16_t *r, int len)
{
int n;
for (n=0;n<len;n++)
t[n] = SUB16(t[n],PSHR32(MULT16_16(g,r[n]),13));
}
/** 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];*/
}
}
