static inline opus_val16 tansig_approx(opus_val32 _x)
{ /* Q19 */
int i;
opus_val16 xx; /* Q11 */
/*double x, y; */
opus_val16 dy, yy; /* Q14 */
/*x = 1.9073e-06*_x; */
if (_x >= QCONST32(8, 19))
return QCONST32(1., 14);
if (_x <= -QCONST32(8, 19))
return -QCONST32(1., 14);
xx = EXTRACT16(SHR32(_x, 8));
/*i = lrint(25*x); */
i = SHR32(ADD32(1024, MULT16_16(25, xx)), 11);
/*x -= .04*i; */
xx -= EXTRACT16(SHR32(MULT16_16(20972, i), 8));
/*x = xx*(1./2048); */
/*y = tansig_table[250+i]; */
yy = tansig_table[250 + i];
/*y = yy*(1./16384); */
dy = 16384 - MULT16_16_Q14(yy, yy);
yy = yy + MULT16_16_Q14(MULT16_16_Q11(xx, dy),
(16384 - MULT16_16_Q11(yy, xx)));
return yy;
}
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;
spx_word32_t *Q; /* ptrs for memory allocation */
spx_word32_t *P;
spx_word32_t *px; /* ptrs of respective P'(z) & Q'(z) */
spx_word32_t *qx;
spx_word32_t *p;
spx_word32_t *q;
spx_word32_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 */
Q = PUSH(stack, (m+1), spx_word32_t);
P = PUSH(stack, (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=1;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++)
{
/*if (fabs(*px)>=32768)
speex_warning_int("px", *px);
if (fabs(*qx)>=32768)
speex_warning_int("qx", *qx);*/
*px = (2+*px)>>2;
*qx = (2+*qx)>>2;
px++;
qx++;
}
P[m] = PSHR(P[m],3);
Q[m] = PSHR(Q[m],3);
#else
*px++ = LPC_SCALING;
*qx++ = LPC_SCALING;
for(i=1;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;
/* 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 = qx;
else
pt = px;
psuml = cheb_poly_eva(pt,xl,lpcrdr,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,(FREQ_SCALE - MULT16_16_Q14(MULT16_16_Q14(xl,xl),14000)));
if (psuml<512 && psuml>-512)
dd = PSHR(dd,1);
#else
dd=delta*(1-.9*xl*xl);
if (fabs(psuml)<.2)
dd *= .5;
#endif
xr = xl - dd; /* interval spacing */
psumr = cheb_poly_eva(pt,xr,lpcrdr,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(PSHR(xl,1),PSHR(xr,1)); /* bisect the interval */
#else
xm = .5*(xl+xr); /* bisect the interval */
#endif
psumm=cheb_poly_eva(pt,xm,lpcrdr,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);
}
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);*/
}
/** Finds the best quantized 3-tap pitch predictor by analysis by synthesis */
static spx_word32_t pitch_gain_search_3tap(
const spx_word16_t target[], /* Target vector */
const spx_coef_t ak[], /* LPCs for this subframe */
const spx_coef_t awk1[], /* Weighted LPCs #1 for this subframe */
const spx_coef_t awk2[], /* Weighted LPCs #2 for this subframe */
spx_sig_t exc[], /* Excitation */
const signed char* gain_cdbk,
int gain_cdbk_size,
int pitch, /* Pitch value */
int p, /* Number of LPC coeffs */
int nsf, /* Number of samples in subframe */
SpeexBits* bits,
char* stack,
const spx_word16_t* exc2,
const spx_word16_t* r,
spx_word16_t* new_target,
int* cdbk_index,
int plc_tuning,
spx_word32_t cumul_gain,
int scaledown
) {
int i, j;
VARDECL(spx_word16_t * tmp1);
VARDECL(spx_word16_t * e);
spx_word16_t* x[3];
spx_word32_t corr[3];
spx_word32_t A[3][3];
spx_word16_t gain[3];
spx_word32_t err;
spx_word16_t max_gain = 128;
int best_cdbk = 0;
ALLOC(tmp1, 3 * nsf, spx_word16_t);
ALLOC(e, nsf, spx_word16_t);
if (cumul_gain > 262144)
max_gain = 31;
x[0] = tmp1;
x[1] = tmp1 + nsf;
x[2] = tmp1 + 2 * nsf;
for (j = 0; j < nsf; j++)
new_target[j] = target[j];
{
VARDECL(spx_mem_t * mm);
int pp = pitch - 1;
ALLOC(mm, p, spx_mem_t);
for (j = 0; j < nsf; j++) {
if (j - pp < 0)
e[j] = exc2[j - pp];
else if (j - pp - pitch < 0)
e[j] = exc2[j - pp - pitch];
else
e[j] = 0;
}
#ifdef FIXED_POINT
/* Scale target and excitation down if needed (avoiding overflow) */
if (scaledown) {
for (j = 0; j < nsf; j++)
e[j] = SHR16(e[j], 1);
for (j = 0; j < nsf; j++)
new_target[j] = SHR16(new_target[j], 1);
}
#endif
for (j = 0; j < p; j++)
mm[j] = 0;
iir_mem16(e, ak, e, nsf, p, mm, stack);
for (j = 0; j < p; j++)
mm[j] = 0;
filter_mem16(e, awk1, awk2, e, nsf, p, mm, stack);
for (j = 0; j < nsf; j++)
x[2][j] = e[j];
}
for (i = 1; i >= 0; i--) {
spx_word16_t e0 = exc2[-pitch - 1 + i];
#ifdef FIXED_POINT
/* Scale excitation down if needed (avoiding overflow) */
if (scaledown)
e0 = SHR16(e0, 1);
#endif
x[i][0] = MULT16_16_Q14(r[0], e0);
for (j = 0; j < nsf - 1; j++)
x[i][j + 1] = ADD32(x[i + 1][j], MULT16_16_P14(r[j + 1], e0));
}
for (i = 0; i < 3; i++)
corr[i] = inner_prod(x[i], new_target, nsf);
for (i = 0; i < 3; i++)
for (j = 0; j <= i; j++)
A[i][j] = A[j][i] = inner_prod(x[i], x[j], nsf);
{
spx_word32_t C[9];
#ifdef FIXED_POINT
spx_word16_t C16[9];
#else
spx_word16_t* C16 = C;
#endif
C[0] = corr[2];
C[1] = corr[1];
C[2] = corr[0];
C[3] = A[1][2];
C[4] = A[0][1];
C[5] = A[0][2];
C[6] = A[2][2];
C[7] = A[1][1];
C[8] = A[0][0];
/*plc_tuning *= 2;*/
if (plc_tuning < 2)
plc_tuning = 2;
if (plc_tuning > 30)
plc_tuning = 30;
#ifdef FIXED_POINT
C[0] = SHL32(C[0], 1);
C[1] = SHL32(C[1], 1);
C[2] = SHL32(C[2], 1);
C[3] = SHL32(C[3], 1);
C[4] = SHL32(C[4], 1);
C[5] = SHL32(C[5], 1);
C[6] = MAC16_32_Q15(C[6], MULT16_16_16(plc_tuning, 655), C[6]);
C[7] = MAC16_32_Q15(C[7], MULT16_16_16(plc_tuning, 655), C[7]);
C[8] = MAC16_32_Q15(C[8], MULT16_16_16(plc_tuning, 655), C[8]);
normalize16(C, C16, 32767, 9);
#else
C[6] *= .5 * (1 + .02 * plc_tuning);
C[7] *= .5 * (1 + .02 * plc_tuning);
C[8] *= .5 * (1 + .02 * plc_tuning);
#endif
best_cdbk = pitch_gain_search_3tap_vq(gain_cdbk, gain_cdbk_size, C16, max_gain);
#ifdef FIXED_POINT
gain[0] = ADD16(32, (spx_word16_t)gain_cdbk[best_cdbk * 4]);
gain[1] = ADD16(32, (spx_word16_t)gain_cdbk[best_cdbk * 4 + 1]);
gain[2] = ADD16(32, (spx_word16_t)gain_cdbk[best_cdbk * 4 + 2]);
/*printf ("%d %d %d %d\n",gain[0],gain[1],gain[2], best_cdbk);*/
#else
gain[0] = 0.015625 * gain_cdbk[best_cdbk * 4] + .5;
gain[1] = 0.015625 * gain_cdbk[best_cdbk * 4 + 1] + .5;
gain[2] = 0.015625 * gain_cdbk[best_cdbk * 4 + 2] + .5;
#endif
*cdbk_index = best_cdbk;
}
SPEEX_MEMSET(exc, 0, nsf);
for (i = 0; i < 3; i++) {
int j;
int tmp1, tmp3;
int pp = pitch + 1 - i;
tmp1 = nsf;
if (tmp1 > pp)
tmp1 = pp;
for (j = 0; j < tmp1; j++)
exc[j] = MAC16_16(exc[j], SHL16(gain[2 - i], 7), exc2[j - pp]);
tmp3 = nsf;
if (tmp3 > pp + pitch)
tmp3 = pp + pitch;
for (j = tmp1; j < tmp3; j++)
exc[j] = MAC16_16(exc[j], SHL16(gain[2 - i], 7), exc2[j - pp - pitch]);
}
for (i = 0; i < nsf; i++) {
spx_word32_t tmp = ADD32(ADD32(MULT16_16(gain[0], x[2][i]), MULT16_16(gain[1], x[1][i])),
MULT16_16(gain[2], x[0][i]));
new_target[i] = SUB16(new_target[i], EXTRACT16(PSHR32(tmp, 6)));
}
err = inner_prod(new_target, new_target, nsf);
return err;
}
int sb_encode(void *state, void *vin, SpeexBits *bits)
{
SBEncState *st;
int i, roots, sub;
char *stack;
VARDECL(spx_mem_t *mem);
VARDECL(spx_sig_t *innov);
VARDECL(spx_word16_t *target);
VARDECL(spx_word16_t *syn_resp);
VARDECL(spx_word32_t *low_pi_gain);
spx_word16_t *low;
spx_word16_t *high;
VARDECL(spx_word16_t *low_exc_rms);
VARDECL(spx_word16_t *low_innov_rms);
const SpeexSBMode *mode;
spx_int32_t dtx;
spx_word16_t *in = (spx_word16_t*)vin;
spx_word16_t e_low=0, e_high=0;
VARDECL(spx_coef_t *lpc);
VARDECL(spx_coef_t *interp_lpc);
VARDECL(spx_coef_t *bw_lpc1);
VARDECL(spx_coef_t *bw_lpc2);
VARDECL(spx_lsp_t *lsp);
VARDECL(spx_lsp_t *qlsp);
VARDECL(spx_lsp_t *interp_lsp);
VARDECL(spx_lsp_t *interp_qlsp);
st = (SBEncState*)state;
stack=st->stack;
mode = (const SpeexSBMode*)(st->mode->mode);
low = in;
high = in+st->frame_size;
/* High-band buffering / sync with low band */
/* Compute the two sub-bands by filtering with QMF h0*/
qmf_decomp(in, h0, low, high, st->full_frame_size, QMF_ORDER, st->h0_mem, stack);
#ifndef DISABLE_VBR
if (st->vbr_enabled || st->vad_enabled)
{
/* Need to compute things here before the signal is trashed by the encoder */
/*FIXME: Are the two signals (low, high) in sync? */
e_low = compute_rms16(low, st->frame_size);
e_high = compute_rms16(high, st->frame_size);
}
#endif /* #ifndef DISABLE_VBR */
ALLOC(low_innov_rms, st->nbSubframes, spx_word16_t);
speex_encoder_ctl(st->st_low, SPEEX_SET_INNOVATION_SAVE, low_innov_rms);
/* Encode the narrowband part*/
speex_encode_native(st->st_low, low, bits);
high = high - (st->windowSize-st->frame_size);
SPEEX_COPY(high, st->high, st->windowSize-st->frame_size);
SPEEX_COPY(st->high, &high[st->frame_size], st->windowSize-st->frame_size);
ALLOC(low_pi_gain, st->nbSubframes, spx_word32_t);
ALLOC(low_exc_rms, st->nbSubframes, spx_word16_t);
speex_encoder_ctl(st->st_low, SPEEX_GET_PI_GAIN, low_pi_gain);
speex_encoder_ctl(st->st_low, SPEEX_GET_EXC, low_exc_rms);
speex_encoder_ctl(st->st_low, SPEEX_GET_LOW_MODE, &dtx);
if (dtx==0)
dtx=1;
else
dtx=0;
ALLOC(lpc, st->lpcSize, spx_coef_t);
ALLOC(interp_lpc, st->lpcSize, spx_coef_t);
ALLOC(bw_lpc1, st->lpcSize, spx_coef_t);
ALLOC(bw_lpc2, st->lpcSize, spx_coef_t);
ALLOC(lsp, st->lpcSize, spx_lsp_t);
ALLOC(qlsp, st->lpcSize, spx_lsp_t);
ALLOC(interp_lsp, st->lpcSize, spx_lsp_t);
ALLOC(interp_qlsp, st->lpcSize, spx_lsp_t);
{
VARDECL(spx_word16_t *autocorr);
VARDECL(spx_word16_t *w_sig);
ALLOC(autocorr, st->lpcSize+1, spx_word16_t);
ALLOC(w_sig, st->windowSize, spx_word16_t);
/* Window for analysis */
/* FIXME: This is a kludge */
if (st->subframeSize==80)
{
for (i=0;i<st->windowSize;i++)
w_sig[i] = EXTRACT16(SHR32(MULT16_16(high[i],st->window[i>>1]),SIG_SHIFT));
} else {
for (i=0;i<st->windowSize;i++)
w_sig[i] = EXTRACT16(SHR32(MULT16_16(high[i],st->window[i]),SIG_SHIFT));
}
/* Compute auto-correlation */
_spx_autocorr(w_sig, autocorr, st->lpcSize+1, st->windowSize);
autocorr[0] = ADD16(autocorr[0],MULT16_16_Q15(autocorr[0],st->lpc_floor)); /* Noise floor in auto-correlation domain */
/* Lag windowing: equivalent to filtering in the power-spectrum domain */
for (i=0;i<st->lpcSize+1;i++)
autocorr[i] = MULT16_16_Q14(autocorr[i],st->lagWindow[i]);
/* Levinson-Durbin */
_spx_lpc(lpc, autocorr, st->lpcSize);
}
void gainQuantization(bcg729EncoderChannelContextStruct *encoderChannelContext, word16_t targetSignal[], word16_t filteredAdaptativeCodebookVector[], word16_t convolvedFixedCodebookVector[], word16_t fixedCodebookVector[], word64_t xy64, word64_t yy64,
word16_t *quantizedAdaptativeCodebookGain, word16_t *quantizedFixedCodebookGain, uint16_t *gainCodebookStage1, uint16_t *gainCodebookStage2)
{
int i,j;
word64_t xz64=0, yz64=0, zz64=0;
word32_t xy;
word32_t yy;
word32_t xz;
word32_t yz;
word32_t zz;
uint16_t minNormalization = 31;
uint16_t currentNormalization;
word32_t bestAdaptativeCodebookGain, bestFixedCodebookGain;
word64_t denominator;
word16_t predictedFixedCodebookGain;
uint16_t indexBaseGa=0;
uint16_t indexBaseGb=0;
uint16_t indexGa=0, indexGb=0;
word64_t distanceMin = MAXINT64;
/*** compute spec 3.9 eq63 terms first on 64 bits and then scale them if needed to fit on 32 ***/
/* Xy64 and Yy64 already computed during adaptativeCodebookGain computation */
for (i=0; i<L_SUBFRAME; i++) {
xz64 = MAC64(xz64, targetSignal[i], convolvedFixedCodebookVector[i]); /* in Q12 */
yz64 = MAC64(yz64, filteredAdaptativeCodebookVector[i], convolvedFixedCodebookVector[i]); /* in Q12 */
zz64 = MAC64(zz64, convolvedFixedCodebookVector[i], convolvedFixedCodebookVector[i]); /* in Q24 */
}
/* now scale this terms to have them fit on 32 bits - terms Xy, Xz and Yz shall fit on 31 bits because used in eq63 with a factor 2 */
xy = SHR64(((xy64<0)?-xy64:xy64),30);
yy = SHR64(yy64,31);
xz = SHR64(((xz64<0)?-xz64:xz64),30);
yz = SHR64(((yz64<0)?-yz64:yz64),30);
zz = SHR64(zz64,31);
currentNormalization = countLeadingZeros(xy);
if (currentNormalization<minNormalization) {
minNormalization = currentNormalization;
}
currentNormalization = countLeadingZeros(xz);
if (currentNormalization<minNormalization) {
minNormalization = currentNormalization;
}
currentNormalization = countLeadingZeros(yz);
if (currentNormalization<minNormalization) {
minNormalization = currentNormalization;
}
currentNormalization = countLeadingZeros(yy);
if (currentNormalization<minNormalization) {
minNormalization = currentNormalization;
}
currentNormalization = countLeadingZeros(zz);
if (currentNormalization<minNormalization) {
minNormalization = currentNormalization;
}
if (minNormalization<31) { /* we shall normalise, values are over 32 bits */
minNormalization = 31 - minNormalization;
xy = (word32_t)SHR64(xy64, minNormalization);
yy = (word32_t)SHR64(yy64, minNormalization);
xz = (word32_t)SHR64(xz64, minNormalization);
yz = (word32_t)SHR64(yz64, minNormalization);
zz = (word32_t)SHR64(zz64, minNormalization);
} else { /* no need to normalise, values already fit on 32 bits, just cast them */
xy = (word32_t)xy64; /* in Q0 */
yy = (word32_t)yy64; /* in Q0 */
xz = (word32_t)xz64; /* in Q12 */
yz = (word32_t)yz64; /* in Q12 */
zz = (word32_t)zz64; /* in Q24 */
}
/*** compute the best gains minimizinq eq63 ***/
/* Note this bestgain computation is not at all described in the spec, got it from ITU code */
/* bestAdaptativeCodebookGain = (zz.xy - xz.yz) / (yy*zz) - yz^2) */
/* bestfixedCodebookGain = (yy*xz - xy*yz) / (yy*zz) - yz^2) */
/* best gain are computed in Q9 and Q2 and fits on 16 bits */
denominator = MAC64(MULT32_32(yy, zz), -yz, yz); /* (yy*zz) - yz^2) in Q24 (always >= 0)*/
/* avoid division by zero */
if (denominator==0) { /* consider it to be one */
bestAdaptativeCodebookGain = (word32_t)(SHR64(MAC64(MULT32_32(zz, xy), -xz, yz), 15)); /* MAC in Q24 -> Q9 */
bestFixedCodebookGain = (word32_t)(SHR64(MAC64(MULT32_32(yy, xz), -xy, yz), 10)); /* MAC in Q12 -> Q2 */
} else {
/* bestAdaptativeCodebookGain in Q9 */
uint16_t numeratorNorm;
word64_t numerator = MAC64(MULT32_32(zz, xy), -xz, yz); /* in Q24 */
/* check if we can shift it by 9 without overflow as the bestAdaptativeCodebookGain in computed in Q9 */
word32_t numeratorH = (word32_t)(SHR64(numerator,32));
numeratorH = (numeratorH>0)?numeratorH:-numeratorH;
numeratorNorm = countLeadingZeros(numeratorH);
if (numeratorNorm >= 9) {
bestAdaptativeCodebookGain = (word32_t)(DIV64(SHL64(numerator,9), denominator)); /* bestAdaptativeCodebookGain in Q9 */
} else {
word64_t shiftedDenominator = SHR64(denominator, 9-numeratorNorm);
if (shiftedDenominator>0) { /* can't shift left by 9 the numerator, can we shift right by 9-numeratorNorm the denominator without hiting 0 */
bestAdaptativeCodebookGain = (word32_t)(DIV64(SHL64(numerator, numeratorNorm),shiftedDenominator)); /* bestAdaptativeCodebookGain in Q9 */
} else {
bestAdaptativeCodebookGain = SHL((word32_t)(DIV64(SHL64(numerator, numeratorNorm), denominator)), 9-numeratorNorm); /* shift left the division result to reach Q9 */
}
}
numerator = MAC64(MULT32_32(yy, xz), -xy, yz); /* in Q12 */
/* check if we can shift it by 14(it's in Q12 and denominator in Q24) without overflow as the bestFixedCodebookGain in computed in Q2 */
numeratorH = (word32_t)(SHR64(numerator,32));
numeratorH = (numeratorH>0)?numeratorH:-numeratorH;
numeratorNorm = countLeadingZeros(numeratorH);
if (numeratorNorm >= 14) {
bestFixedCodebookGain = (word32_t)(DIV64(SHL64(numerator,14), denominator));
} else {
word64_t shiftedDenominator = SHR64(denominator, 14-numeratorNorm); /* bestFixedCodebookGain in Q14 */
if (shiftedDenominator>0) { /* can't shift left by 9 the numerator, can we shift right by 9-numeratorNorm the denominator without hiting 0 */
bestFixedCodebookGain = (word32_t)(DIV64(SHL64(numerator, numeratorNorm),shiftedDenominator)); /* bestFixedCodebookGain in Q14 */
} else {
bestFixedCodebookGain = SHL((word32_t)(DIV64(SHL64(numerator, numeratorNorm), denominator)), 14-numeratorNorm); /* shift left the division result to reach Q14 */
}
}
}
/*** Compute the predicted gain as in spec 3.9.1 eq71 in Q6 ***/
predictedFixedCodebookGain = (word16_t)(SHR32(MACodeGainPrediction(encoderChannelContext->previousGainPredictionError, fixedCodebookVector), 12)); /* in Q16 -> Q4 range [3,1830] */
/*** preselection spec 3.9.2 ***/
/* Note: spec just says to select the best 50% of each vector, ITU code go through magical constant computation to select the begining of a continuous range */
/* much more simple here : vector are ordened in growing order so just select 2 (4 for Gb) indexes before the first value to be superior to the best gain previously computed */
while (indexBaseGa<6 && bestFixedCodebookGain>(MULT16_16_Q14(GACodebook[indexBaseGa][1],predictedFixedCodebookGain))) { /* bestFixedCodebookGain> in Q2, GACodebook in Q12 *predictedFixedCodebookGain in Q4 -> Q16-14 */
indexBaseGa++;
}
if (indexBaseGa>0) indexBaseGa--;
if (indexBaseGa>0) indexBaseGa--;
while (indexBaseGb<12 && bestAdaptativeCodebookGain>(SHR(GBCodebook[indexBaseGb][0],5))) {
indexBaseGb++;
}
if (indexBaseGb>0) indexBaseGb--;
if (indexBaseGb>0) indexBaseGb--;
if (indexBaseGb>0) indexBaseGb--;
if (indexBaseGb>0) indexBaseGb--;
/*** test all possibilities of Ga and Gb indexes and select the best one ***/
xy = -SHL(xy,1); /* xy term is always used with a -2 factor */
xz = -SHL(xz,1); /* xz term is always used with a -2 factor */
yz = SHL(yz,1); /* yz term is always used with a 2 factor */
for (i=0; i<4; i++) {
for (j=0; j<8; j++) {
/* compute gamma->gc and gp */
word16_t gp = ADD16(GACodebook[i+indexBaseGa][0], GBCodebook[j+indexBaseGb][0]); /* result in Q14 */
word16_t gamma = ADD16(GACodebook[i+indexBaseGa][1], GBCodebook[j+indexBaseGb][1]); /* result in Q3.12 (range [0.185, 5.05])*/
word32_t gc = MULT16_16_Q14(gamma, predictedFixedCodebookGain); /* gamma in Q12, predictedFixedCodebookGain in Q4 -> Q16 -14 -> Q2 */
/* compute E as in eq63 (first term excluded) */
word64_t acc = MULT32_32(MULT16_16(gp, gp), yy); /* acc = gp^2*yy gp in Q14, yy in Q0 -> acc in Q28 */
acc = MAC64(acc, MULT16_16(gc, gc), zz); /* gc in Q2, zz in Q24 -> acc in Q28, note gc is on 32 bits but in a range making gc^2 fitting on 32 bits */
acc = MAC64(acc, SHL32((word32_t)gp, 14), xy); /* gp in Q14 shifted to Q28, xy in Q0 -> acc in Q28 */
acc = MAC64(acc, SHL32(gc, 14), xz); /* gc in Q2 shifted to Q16, xz in Q12 -> acc in Q28 */
acc = MAC64(acc, MULT16_16(gp,gc), yz); /* gp in Q14, gc in Q2 yz in Q12 -> acc in Q28 */
if (acc<distanceMin) {
distanceMin = acc;
indexGa = i+indexBaseGa;
indexGb = j+indexBaseGb;
*quantizedAdaptativeCodebookGain = gp;
*quantizedFixedCodebookGain = (word16_t)SHR(gc, 1);
}
}
}
/* update the previous gain prediction error */
computeGainPredictionError(ADD16(GACodebook[indexGa][1], GBCodebook[indexGb][1]), encoderChannelContext->previousGainPredictionError);
/* mapping of indexes */
*gainCodebookStage1 = indexMappingGA[indexGa];
*gainCodebookStage2 = indexMappingGB[indexGb];
return;
}
