void pitch_unquant_3tap(
spx_word16_t exc[], /* Input excitation */
spx_word32_t exc_out[], /* Output excitation */
int start, /* Smallest pitch value allowed */
int end, /* Largest pitch value allowed */
spx_word16_t pitch_coef, /* Voicing (pitch) coefficient */
const void *par,
int nsf, /* Number of samples in subframe */
int *pitch_val,
spx_word16_t *gain_val,
SpeexBits *bits,
char *stack,
int count_lost,
int subframe_offset,
spx_word16_t last_pitch_gain,
int cdbk_offset
)
{
int i;
int pitch;
int gain_index;
spx_word16_t gain[3];
const signed char *gain_cdbk;
int gain_cdbk_size;
const ltp_params *params;
params = (const ltp_params*) par;
gain_cdbk_size = 1<<params->gain_bits;
gain_cdbk = params->gain_cdbk + 4*gain_cdbk_size*cdbk_offset;
pitch = speex_bits_unpack_unsigned(bits, params->pitch_bits);
pitch += start;
gain_index = speex_bits_unpack_unsigned(bits, params->gain_bits);
/*printf ("decode pitch: %d %d\n", pitch, gain_index);*/
#ifdef FIXED_POINT
gain[0] = ADD16(32,(spx_word16_t)gain_cdbk[gain_index*4]);
gain[1] = ADD16(32,(spx_word16_t)gain_cdbk[gain_index*4+1]);
gain[2] = ADD16(32,(spx_word16_t)gain_cdbk[gain_index*4+2]);
#else
gain[0] = 0.015625*gain_cdbk[gain_index*4]+.5;
gain[1] = 0.015625*gain_cdbk[gain_index*4+1]+.5;
gain[2] = 0.015625*gain_cdbk[gain_index*4+2]+.5;
#endif
if (count_lost && pitch > subframe_offset)
{
spx_word16_t gain_sum;
if (1) {
#ifdef FIXED_POINT
spx_word16_t tmp = count_lost < 4 ? last_pitch_gain : SHR16(last_pitch_gain,1);
if (tmp>62)
tmp=62;
#else
spx_word16_t tmp = count_lost < 4 ? last_pitch_gain : 0.5 * last_pitch_gain;
if (tmp>.95)
tmp=.95;
#endif
gain_sum = gain_3tap_to_1tap(gain);
if (gain_sum > tmp)
{
spx_word16_t fact = DIV32_16(SHL32(EXTEND32(tmp),14),gain_sum);
for (i=0;i<3;i++)
gain[i]=MULT16_16_Q14(fact,gain[i]);
}
}
}
*pitch_val = pitch;
gain_val[0]=gain[0];
gain_val[1]=gain[1];
gain_val[2]=gain[2];
gain[0] = SHL16(gain[0],7);
gain[1] = SHL16(gain[1],7);
gain[2] = SHL16(gain[2],7);
SPEEX_MEMSET(exc_out, 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_out[j]=MAC16_16(exc_out[j],gain[2-i],exc[j-pp]);
tmp3=nsf;
if (tmp3>pp+pitch)
tmp3=pp+pitch;
for (j=tmp1;j<tmp3;j++)
exc_out[j]=MAC16_16(exc_out[j],gain[2-i],exc[j-pp-pitch]);
}
/*for (i=0;i<nsf;i++)
exc[i]=PSHR32(exc32[i],13);*/
}
void open_loop_nbest_pitch(spx_word16_t *sw, int start, int end, int len, int *pitch, spx_word16_t *gain, int N, char *stack)
{
int i,j,k;
VARDECL(spx_word32_t *best_score);
VARDECL(spx_word32_t *best_ener);
spx_word32_t e0;
VARDECL(spx_word32_t *corr);
#ifdef FIXED_POINT
/* In fixed-point, we need only one (temporary) array of 32-bit values and two (corr16, ener16)
arrays for (normalized) 16-bit values */
VARDECL(spx_word16_t *corr16);
VARDECL(spx_word16_t *ener16);
spx_word32_t *energy;
int cshift=0, eshift=0;
int scaledown = 0;
ALLOC(corr16, end-start+1, spx_word16_t);
ALLOC(ener16, end-start+1, spx_word16_t);
ALLOC(corr, end-start+1, spx_word32_t);
energy = corr;
#else
/* In floating-point, we need to float arrays and no normalized copies */
VARDECL(spx_word32_t *energy);
spx_word16_t *corr16;
spx_word16_t *ener16;
ALLOC(energy, end-start+2, spx_word32_t);
ALLOC(corr, end-start+1, spx_word32_t);
corr16 = corr;
ener16 = energy;
#endif
ALLOC(best_score, N, spx_word32_t);
ALLOC(best_ener, N, spx_word32_t);
for (i=0;i<N;i++)
{
best_score[i]=-1;
best_ener[i]=0;
pitch[i]=start;
}
#ifdef FIXED_POINT
for (i=-end;i<len;i++)
{
if (ABS16(sw[i])>16383)
{
scaledown=1;
break;
}
}
/* If the weighted input is close to saturation, then we scale it down */
if (scaledown)
{
for (i=-end;i<len;i++)
{
sw[i]=SHR16(sw[i],1);
}
}
#endif
energy[0]=inner_prod(sw-start, sw-start, len);
e0=inner_prod(sw, sw, len);
for (i=start;i<end;i++)
{
/* Update energy for next pitch*/
energy[i-start+1] = SUB32(ADD32(energy[i-start],SHR32(MULT16_16(sw[-i-1],sw[-i-1]),6)), SHR32(MULT16_16(sw[-i+len-1],sw[-i+len-1]),6));
if (energy[i-start+1] < 0)
energy[i-start+1] = 0;
}
#ifdef FIXED_POINT
eshift = normalize16(energy, ener16, 32766, end-start+1);
#endif
/* In fixed-point, this actually overrites the energy array (aliased to corr) */
pitch_xcorr(sw, sw-end, corr, len, end-start+1, stack);
#ifdef FIXED_POINT
/* Normalize to 180 so we can square it and it still fits in 16 bits */
cshift = normalize16(corr, corr16, 180, end-start+1);
/* If we scaled weighted input down, we need to scale it up again (OK, so we've just lost the LSB, who cares?) */
if (scaledown)
{
for (i=-end;i<len;i++)
{
sw[i]=SHL16(sw[i],1);
}
}
#endif
/* Search for the best pitch prediction gain */
for (i=start;i<=end;i++)
{
spx_word16_t tmp = MULT16_16_16(corr16[i-start],corr16[i-start]);
/* Instead of dividing the tmp by the energy, we multiply on the other side */
if (MULT16_16(tmp,best_ener[N-1])>MULT16_16(best_score[N-1],ADD16(1,ener16[i-start])))
{
/* We can safely put it last and then check */
best_score[N-1]=tmp;
best_ener[N-1]=ener16[i-start]+1;
pitch[N-1]=i;
/* Check if it comes in front of others */
for (j=0;j<N-1;j++)
{
if (MULT16_16(tmp,best_ener[j])>MULT16_16(best_score[j],ADD16(1,ener16[i-start])))
{
for (k=N-1;k>j;k--)
{
best_score[k]=best_score[k-1];
best_ener[k]=best_ener[k-1];
pitch[k]=pitch[k-1];
}
best_score[j]=tmp;
best_ener[j]=ener16[i-start]+1;
pitch[j]=i;
break;
}
}
}
}
/* Compute open-loop gain if necessary */
if (gain)
{
for (j=0;j<N;j++)
{
spx_word16_t g;
i=pitch[j];
g = DIV32(SHL32(EXTEND32(corr16[i-start]),cshift), 10+SHR32(MULT16_16(spx_sqrt(e0),spx_sqrt(SHL32(EXTEND32(ener16[i-start]),eshift))),6));
/* FIXME: g = max(g,corr/energy) */
if (g<0)
g = 0;
gain[j]=g;
}
}
}
/** 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;
}
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*/
if (have_sign)
vq_nbest_sign(x, resp2, subvect_size, shape_cb_size, E, 1, &best_index, &best_dist, stack);
else
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));
}
}
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;
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);
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 */
/* Allocate memory space for polynomials */
ALLOC(Q, (m+1), spx_word32_t);
ALLOC(P, (m+1), spx_word32_t);
/* 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;
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++);
}
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 */
ALLOC(P16, m+1, spx_word16_t);
ALLOC(Q16, m+1, spx_word16_t);
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);
}
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;
}
}
/*
* Create a DNS message.
*
* If AAAA is non NULL:
* - create a AAAA update message for Name.Domain with value AAAA.
*
* If AAAA is NULL:
* - create an update message for the deletion of all RRsets that belongs
* to Name.Domain.
*
* *Len_p is set with the length of the created message.
*
*/
static tDNSRCode
DNSMessageCreate(char *Buffer, uint16_t DNSid, char *Domain, char *Name, uint32_t TTL, char *AAAA, size_t *Len_p)
{
char *p;
size_t Domain_len;
size_t Name_len;
struct in6_addr AAAA_addr;
struct in6_addr *AAAA_addr_p = NULL;
tDNSRCode ret;
if (NULL == Buffer ||
NULL == Name ||
NULL == Domain ||
NULL == Len_p)
return DNS_RCODE_FORMERR;
Name_len = strlen(Name);
Domain_len = strlen(Domain);
/* Validate Name.Domain length */
if (Name_len + 1 + Domain_len > DNS_NAME_SIZE ||
Name_len > DNS_LABEL_SIZE)
return DNS_RCODE_FORMERR;
/* Convert AAAA name */
if (NULL != AAAA) {
AAAA_addr_p = NetText2Addr6(AAAA, &AAAA_addr);
if (NULL == AAAA_addr_p)
return DNS_RCODE_FORMERR;
}
/* Skip TCPMSGLENGTH (16 bits) - message length no known yet */
p = Buffer + DNS16SZ;
/* DNS id (16 bits) */
ADD16(p, DNSid);
/*
* Fields QR (1 bit) = 0, Opcode (4 bits) = UPDATE,
* Reserved (7 bits) = 0, RCode (4 bits) = 0.
*/
ADD16(p, DNS_OPCODE_UPDATE << 11);
/* Zone count (16 bits) = 1 */
ADD16(p, 1);
/* Prerequisite count (16 bits) = 0 */
ADD16(p, 0);
/* Update count (16 bits) = 1 */
ADD16(p, 1);
/* Additionnal data count (16 bits) = 0 */
ADD16(p, 0);
/*
* ZONE section
*/
ret = DNSNameEncode(&p, Domain, 0); /* ZNAME */
if (DNS_RCODE_NOERROR != ret)
return ret;
ADD16(p, DNS_TYPE_SOA); /* ZTYPE */
ADD16(p, DNS_CLASS_IN); /* CLASS */
/*
* Ressource Record Section
*
* For the RRNAME, DNS compression is used.
* DNS_HEADER_SIZE is the offset of the Zone Name.
*/
ret = DNSNameEncode(&p, Name, DNS_HEADER_SIZE); /* RRNAME */
if (DNS_RCODE_NOERROR != ret)
return ret;
if ( NULL != AAAA_addr_p) {
/* Addition of AAAA record */
ADD16(p, DNS_TYPE_AAAA); /* RRTYPE */
ADD16(p, DNS_CLASS_IN); /* RRCLASS */
ADD32(p, TTL); /* RRTTL */
ADD16(p, DNS_TYPE_AAAA_SIZE); /* RRDLENGTH */
memcpy(p, AAAA_addr_p, DNS_TYPE_AAAA_SIZE); /* RRDATA */
p += DNS_TYPE_AAAA_SIZE;
} else {
/* Deletion of all RRSETS */
ADD16(p, DNS_TYPE_ANY); /* RRTYPE */
ADD16(p, DNS_CLASS_ANY); /* RRCLASS */
ADD32(p, 0); /* RRTTL */
ADD16(p, 0); /* RRDLENGTH */
}
/* Update *Len_p and the TCPMSGLENGTH field */
*Len_p = p - Buffer;
p = Buffer;
ADD16(p, (u_short)(*Len_p) - DNS16SZ);
return DNS_RCODE_NOERROR;
}
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]);
}
}
/** 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 open_loop_nbest_pitch(spx_word16_t *sw, int start, int end, int len, int *pitch, spx_word16_t *gain, int N, char *stack)
{
int i,j,k;
VARDECL(spx_word32_t *best_score);
VARDECL(spx_word32_t *best_ener);
spx_word32_t e0;
VARDECL(spx_word32_t *corr);
VARDECL(spx_word32_t *energy);
ALLOC(best_score, N, spx_word32_t);
ALLOC(best_ener, N, spx_word32_t);
ALLOC(corr, end-start+1, spx_word32_t);
ALLOC(energy, end-start+2, spx_word32_t);
for (i=0;i<N;i++)
{
best_score[i]=-1;
best_ener[i]=0;
pitch[i]=start;
}
energy[0]=inner_prod(sw-start, sw-start, len);
e0=inner_prod(sw, sw, len);
for (i=start;i<end;i++)
{
/* Update energy for next pitch*/
energy[i-start+1] = SUB32(ADD32(energy[i-start],SHR32(MULT16_16(sw[-i-1],sw[-i-1]),6)), SHR32(MULT16_16(sw[-i+len-1],sw[-i+len-1]),6));
if (energy[i-start+1] < 0)
energy[i-start+1] = 0;
}
pitch_xcorr(sw, sw-end, corr, len, end-start+1, stack);
/* FIXME: Fixed-point and floating-point code should be merged */
#ifdef FIXED_POINT
{
VARDECL(spx_word16_t *corr16);
VARDECL(spx_word16_t *ener16);
ALLOC(corr16, end-start+1, spx_word16_t);
ALLOC(ener16, end-start+1, spx_word16_t);
/* Normalize to 180 so we can square it and it still fits in 16 bits */
normalize16(corr, corr16, 180, end-start+1);
normalize16(energy, ener16, 180, end-start+1);
for (i=start;i<=end;i++)
{
spx_word16_t tmp = MULT16_16_16(corr16[i-start],corr16[i-start]);
/* Instead of dividing the tmp by the energy, we multiply on the other side */
if (MULT16_16(tmp,best_ener[N-1])>MULT16_16(best_score[N-1],ADD16(1,ener16[i-start])))
{
/* We can safely put it last and then check */
best_score[N-1]=tmp;
best_ener[N-1]=ener16[i-start]+1;
pitch[N-1]=i;
/* Check if it comes in front of others */
for (j=0;j<N-1;j++)
{
if (MULT16_16(tmp,best_ener[j])>MULT16_16(best_score[j],ADD16(1,ener16[i-start])))
{
for (k=N-1;k>j;k--)
{
best_score[k]=best_score[k-1];
best_ener[k]=best_ener[k-1];
pitch[k]=pitch[k-1];
}
best_score[j]=tmp;
best_ener[j]=ener16[i-start]+1;
pitch[j]=i;
break;
}
}
}
}
}
#else
for (i=start;i<=end;i++)
{
float tmp = corr[i-start]*corr[i-start];
if (tmp*best_ener[N-1]>best_score[N-1]*(1+energy[i-start]))
{
for (j=0;j<N;j++)
{
if (tmp*best_ener[j]>best_score[j]*(1+energy[i-start]))
{
for (k=N-1;k>j;k--)
{
best_score[k]=best_score[k-1];
best_ener[k]=best_ener[k-1];
pitch[k]=pitch[k-1];
}
best_score[j]=tmp;
best_ener[j]=energy[i-start]+1;
pitch[j]=i;
break;
}
}
}
}
#endif
/* Compute open-loop gain */
if (gain)
{
for (j=0;j<N;j++)
{
spx_word16_t g;
i=pitch[j];
g = DIV32(corr[i-start], 10+SHR32(MULT16_16(spx_sqrt(e0),spx_sqrt(energy[i-start])),6));
/* FIXME: g = max(g,corr/energy) */
if (g<0)
g = 0;
gain[j]=g;
}
}
}
/*
** Opcode 0xDD/0xFD
** IX/IY related instructions
*/
UBYTE Z80::indexInstructions(UWORD& I, UBYTE origOpcode)
{
UBYTE opcode = READ_MEM(PC++);
switch (opcode) {
case 0x8E: ADD8(A, READ_MEM(I + READ_MEM(PC++)), F_C); return 19;
case 0x86: ADD8(A, READ_MEM(I + READ_MEM(PC++)), 0); return 19;
case 0x09: ADD16(I, BC); return 15;
case 0x19: ADD16(I, DE); return 15;
case 0x29: ADD16(I, I); return 15;
case 0x39: ADD16(I, SP); return 15;
case 0xA6: AND(A, READ_MEM(I + READ_MEM(PC++))); return 19;
case 0xBE: CP(A, READ_MEM(I + READ_MEM(PC++))); return 19;
case 0x96: SUB8(A, READ_MEM(I + READ_MEM(PC++)), 0); return 19;
case 0xAE: XOR(A, READ_MEM(I + READ_MEM(PC++))); return 19;
case 0x35: {
UBYTE v = I + READ_MEM(PC++); UBYTE s = READ_MEM(v); DEC8(s); WRITE_MEM(v, s);
} return 23;
case 0x2B: DEC16(I); return 10;
case 0xE3: { UWORD s = READ_MEM16(SP); EX(s, I); WRITE_MEM16(SP, s); } return 23;
case 0x23: INC16(I); return 10;
case 0x34: {
UBYTE v = I + READ_MEM(PC++); UBYTE s = READ_MEM(v); INC8(s); WRITE_MEM(v, s);
} return 23;
/* JP */
case 0xE9: PC = READ_MEM16(I); return 8;
/* LD */
case 0x7E: A = READ_MEM(I + READ_MEM(PC++)); return 19;
case 0x46: B = READ_MEM(I + READ_MEM(PC++)); return 19;
case 0x4E: C = READ_MEM(I + READ_MEM(PC++)); return 19;
case 0x56: D = READ_MEM(I + READ_MEM(PC++)); return 19;
case 0x5E: E = READ_MEM(I + READ_MEM(PC++)); return 19;
case 0x66: H = READ_MEM(I + READ_MEM(PC++)); return 19;
case 0x6E: L = READ_MEM(I + READ_MEM(PC++)); return 19;
case 0xF9: SP = I; return 19;
case 0x2A: I = READ_MEM16(READ_MEM16(PC)); PC += 2; return 20;
case 0x21: I = READ_MEM16(PC); PC += 2; return 14;
case 0x22: WRITE_MEM16(READ_MEM16(PC), I); PC += 2; return 20;
case 0x70 + 7: WRITE_MEM(I + READ_MEM(PC++), A); return 19;
case 0x70 + 0: WRITE_MEM(I + READ_MEM(PC++), B); return 19;
case 0x70 + 1: WRITE_MEM(I + READ_MEM(PC++), C); return 19;
case 0x70 + 2: WRITE_MEM(I + READ_MEM(PC++), D); return 19;
case 0x70 + 3: WRITE_MEM(I + READ_MEM(PC++), E); return 19;
case 0x70 + 4: WRITE_MEM(I + READ_MEM(PC++), H); return 19;
case 0x70 + 5: WRITE_MEM(I + READ_MEM(PC++), L); return 19;
case 0x36: WRITE_MEM(I + READ_MEM(PC), READ_MEM(PC + 1)); PC += 2; return 19;
case 0xB6: OR(A, READ_MEM(I + READ_MEM(PC++))); return 19;
case 0xE1: I = pop16(); return 14;
case 0xE5: push16(I); return 15;
case 0x9E: SUB8(A, READ_MEM(I + READ_MEM(PC++)), F_C); return 19;
case 0xCB: { // DD CB
UBYTE arg = READ_MEM(PC++);
UBYTE extOpcode = READ_MEM(PC++);
switch (extOpcode) {
#define RES_I(b) case 0x86 + 8 * b: { UBYTE s = READ_MEM(I + arg); \
RES(s, b); WRITE_MEM(I + arg, s); } return 23;
#define SET_I(b) case 0xC6 + 8 * b: { UBYTE s = READ_MEM(I + arg); \
SET(s, b); WRITE_MEM(I + arg, s); } return 23;
#define BIT_I(b) case 0x46 + 8 * b: { UBYTE s = READ_MEM(I + arg); \
BIT(s, b); WRITE_MEM(I + arg, s); } return 23;
/* BIT b,(I+N) */
BIT_I(0); BIT_I(1); BIT_I(2); BIT_I(3);
BIT_I(4); BIT_I(5); BIT_I(6); BIT_I(7);
/* RES b,(I+N) */
RES_I(0); RES_I(1); RES_I(2); RES_I(3);
RES_I(4); RES_I(5); RES_I(6); RES_I(7);
/* SET b,(I+N) */
SET_I(0); SET_I(1); SET_I(2); SET_I(3);
SET_I(4); SET_I(5); SET_I(6); SET_I(7);
case 0x16: { /* RL (I+N) */
UBYTE s = READ_MEM(I + arg); RL(s); WRITE_MEM(I + arg, s);
} return 23;
case 0x06: { /* RLC (I+N) */
UBYTE s = READ_MEM(I + arg); RLC(s); WRITE_MEM(I + arg, s);
} return 23;
case 0x1E: { /* RR (I+N) */
UBYTE s = READ_MEM(I + arg); RR(s); WRITE_MEM(I + arg, s);
} return 23;
case 0x0E: { /* RRC (I+N) */
UBYTE s = READ_MEM(I + arg); RRC(s); WRITE_MEM(I + arg, s);
} return 23;
case 0x26: { /* SLA (I+N) */
UBYTE s = READ_MEM(I + arg); SLA(s); WRITE_MEM(I + arg, s);
} return 23;
case 0x2E: { /* SRA (I+N) */
UBYTE s = READ_MEM(I + arg); SRA(s); WRITE_MEM(I + arg, s);
} return 23;
case 0x36: { /* SLL (I+N) */
UBYTE s = READ_MEM(I + arg); SLL(s); WRITE_MEM(I + arg, s);
} return 23;
case 0x3E: { /* SRL (I+N) */
UBYTE s = READ_MEM(I + arg); SRL(s); WRITE_MEM(I + arg, s);
} return 23;
default:
std::cout << std::hex << "Unknown extended opcode ("
<< origOpcode << ":" << opcode << "): " << static_cast<int>(extOpcode) << std::endl;
return 0;
}
} break;
default:
std::cout << std::hex << "Unknown extended opcode ("
<< origOpcode << "): " << static_cast<int>(opcode) << std::endl;
break;
}
return 0;
}
void decodeGains (bcg729DecoderChannelContextStruct *decoderChannelContext, uint16_t GA, uint16_t GB, word16_t *fixedCodebookVector, uint8_t frameErasureFlag,
word16_t *adaptativeCodebookGain, word16_t *fixedCodebookGain)
{
word32_t predictedFixedCodebookGain;
word16_t fixedCodebookGainCorrectionFactor;
/* check the erasure flag */
if (frameErasureFlag != 0) { /* we have a frame erasure, proceed as described in spec 4.4.2 */
int i;
word32_t currentGainPredictionError =0;
/* adaptativeCodebookGain as in eq94 */
if (*adaptativeCodebookGain < 16384) { /* last subframe gain < 1 in Q14 */
*adaptativeCodebookGain = MULT16_16_Q15(*adaptativeCodebookGain, 29491 ); /* *0.9 in Q15 */
} else { /* bound current subframe gain to 0.9 (14746 in Q14) */
*adaptativeCodebookGain = 14746;
}
/* fixedCodebookGain as in eq93 */
*fixedCodebookGain = MULT16_16_Q15(*fixedCodebookGain, 32113 ); /* *0.98 in Q15 */
/* And update the previousGainPredictionError according to spec 4.4.3 */
for (i=0; i<4; i++) {
currentGainPredictionError = ADD32(currentGainPredictionError, decoderChannelContext->previousGainPredictionError[i]); /* previousGainPredictionError in Q3.10-> Sum in Q5.10 (on 32 bits) */
}
currentGainPredictionError = PSHR(currentGainPredictionError,2); /* /4 -> Q3.10 */
if (currentGainPredictionError < -10240) { /* final result is low bounded by -14, so check before doing -4 if it's over -10(-10240 in Q10) or not */
currentGainPredictionError = -14336; /* set to -14 in Q10 */
} else { /* substract 4 */
currentGainPredictionError = SUB32(currentGainPredictionError,4096); /* in Q10 */
}
/* shift the array and insert the current Prediction Error */
decoderChannelContext->previousGainPredictionError[3] = decoderChannelContext->previousGainPredictionError[2];
decoderChannelContext->previousGainPredictionError[2] = decoderChannelContext->previousGainPredictionError[1];
decoderChannelContext->previousGainPredictionError[1] = decoderChannelContext->previousGainPredictionError[0];
decoderChannelContext->previousGainPredictionError[0] = (word16_t)currentGainPredictionError;
return;
}
/* First recover the GA and GB real index from their mapping tables(spec 3.9.3) */
GA = reverseIndexMappingGA[GA];
GB = reverseIndexMappingGB[GB];
/* Compute the adaptativeCodebookGain from the tables according to eq73 in spec3.9.2 */
/* adaptativeCodebookGain = GACodebook[GA][0] + GBCodebook[GB][0] */
*adaptativeCodebookGain = ADD16(GACodebook[GA][0], GBCodebook[GB][0]); /* result in Q1.14 */
/* Fixed Codebook: MA code-gain prediction */
predictedFixedCodebookGain = MACodeGainPrediction(decoderChannelContext->previousGainPredictionError, fixedCodebookVector); /* predictedFixedCodebookGain on 32 bits in Q11.16 */
/* get fixed codebook gain correction factor(gama) from the codebooks GA and GB according to eq74 */
fixedCodebookGainCorrectionFactor = ADD16(GACodebook[GA][1], GBCodebook[GB][1]); /* result in Q3.12 (range [0.185, 5.05])*/
/* compute fixedCodebookGain according to eq74 */
*fixedCodebookGain = (word16_t)PSHR(MULT16_32_Q12(fixedCodebookGainCorrectionFactor, predictedFixedCodebookGain), 15); /* Q11.16*Q3.12 -> Q14.16, shift by 15 to get a Q14.1 which fits on 16 bits */
/* use eq72 to compute current prediction error in order to update the previousGainPredictionError array */
computeGainPredictionError(fixedCodebookGainCorrectionFactor, decoderChannelContext->previousGainPredictionError);
}
/*ADD HL,BC*/
static void op_0x09(Z80EX_CONTEXT *cpu)
{
ADD16(HL,BC);
T_WAIT_UNTIL(11);
return;
}
int sb_encode(void *state, void *vin, SpeexBits *bits)
{
SBEncState *st;
int i, roots, sub;
char *stack;
VARDECL(spx_mem_t *mem);
VARDECL(spx_sig_t *innov);
VARDECL(spx_word16_t *target);
VARDECL(spx_word16_t *syn_resp);
VARDECL(spx_word32_t *low_pi_gain);
spx_word16_t *low;
spx_word16_t *high;
VARDECL(spx_word16_t *low_exc_rms);
VARDECL(spx_word16_t *low_innov_rms);
const SpeexSBMode *mode;
spx_int32_t dtx;
spx_word16_t *in = (spx_word16_t*)vin;
spx_word16_t e_low=0, e_high=0;
VARDECL(spx_coef_t *lpc);
VARDECL(spx_coef_t *interp_lpc);
VARDECL(spx_coef_t *bw_lpc1);
VARDECL(spx_coef_t *bw_lpc2);
VARDECL(spx_lsp_t *lsp);
VARDECL(spx_lsp_t *qlsp);
VARDECL(spx_lsp_t *interp_lsp);
VARDECL(spx_lsp_t *interp_qlsp);
st = (SBEncState*)state;
stack=st->stack;
mode = (const SpeexSBMode*)(st->mode->mode);
low = in;
high = in+st->frame_size;
/* High-band buffering / sync with low band */
/* Compute the two sub-bands by filtering with QMF h0*/
qmf_decomp(in, h0, low, high, st->full_frame_size, QMF_ORDER, st->h0_mem, stack);
#ifndef DISABLE_VBR
if (st->vbr_enabled || st->vad_enabled)
{
/* Need to compute things here before the signal is trashed by the encoder */
/*FIXME: Are the two signals (low, high) in sync? */
e_low = compute_rms16(low, st->frame_size);
e_high = compute_rms16(high, st->frame_size);
}
#endif /* #ifndef DISABLE_VBR */
ALLOC(low_innov_rms, st->nbSubframes, spx_word16_t);
speex_encoder_ctl(st->st_low, SPEEX_SET_INNOVATION_SAVE, low_innov_rms);
/* Encode the narrowband part*/
speex_encode_native(st->st_low, low, bits);
high = high - (st->windowSize-st->frame_size);
SPEEX_COPY(high, st->high, st->windowSize-st->frame_size);
SPEEX_COPY(st->high, &high[st->frame_size], st->windowSize-st->frame_size);
ALLOC(low_pi_gain, st->nbSubframes, spx_word32_t);
ALLOC(low_exc_rms, st->nbSubframes, spx_word16_t);
speex_encoder_ctl(st->st_low, SPEEX_GET_PI_GAIN, low_pi_gain);
speex_encoder_ctl(st->st_low, SPEEX_GET_EXC, low_exc_rms);
speex_encoder_ctl(st->st_low, SPEEX_GET_LOW_MODE, &dtx);
if (dtx==0)
dtx=1;
else
dtx=0;
ALLOC(lpc, st->lpcSize, spx_coef_t);
ALLOC(interp_lpc, st->lpcSize, spx_coef_t);
ALLOC(bw_lpc1, st->lpcSize, spx_coef_t);
ALLOC(bw_lpc2, st->lpcSize, spx_coef_t);
ALLOC(lsp, st->lpcSize, spx_lsp_t);
ALLOC(qlsp, st->lpcSize, spx_lsp_t);
ALLOC(interp_lsp, st->lpcSize, spx_lsp_t);
ALLOC(interp_qlsp, st->lpcSize, spx_lsp_t);
{
VARDECL(spx_word16_t *autocorr);
VARDECL(spx_word16_t *w_sig);
ALLOC(autocorr, st->lpcSize+1, spx_word16_t);
ALLOC(w_sig, st->windowSize, spx_word16_t);
/* Window for analysis */
/* FIXME: This is a kludge */
if (st->subframeSize==80)
{
for (i=0;i<st->windowSize;i++)
w_sig[i] = EXTRACT16(SHR32(MULT16_16(high[i],st->window[i>>1]),SIG_SHIFT));
} else {
for (i=0;i<st->windowSize;i++)
w_sig[i] = EXTRACT16(SHR32(MULT16_16(high[i],st->window[i]),SIG_SHIFT));
}
/* Compute auto-correlation */
_spx_autocorr(w_sig, autocorr, st->lpcSize+1, st->windowSize);
autocorr[0] = ADD16(autocorr[0],MULT16_16_Q15(autocorr[0],st->lpc_floor)); /* Noise floor in auto-correlation domain */
/* Lag windowing: equivalent to filtering in the power-spectrum domain */
for (i=0;i<st->lpcSize+1;i++)
autocorr[i] = MULT16_16_Q14(autocorr[i],st->lagWindow[i]);
/* Levinson-Durbin */
_spx_lpc(lpc, autocorr, st->lpcSize);
}
void 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;
}
/*ADD IX,IX*/
static void op_DD_0x29(Z80EX_CONTEXT *cpu)
{
ADD16(IX,IX);
T_WAIT_UNTIL(11);
return;
}
