/* The conversion is implemented by the step-down algorithm */
void WebRtcSpl_AToK_JSK(
WebRtc_Word16 *a16, /* Q11 */
WebRtc_Word16 useOrder,
WebRtc_Word16 *k16 /* Q15 */
)
{
int m, k;
WebRtc_Word32 tmp32[MAX_AR_MODEL_ORDER];
WebRtc_Word32 tmp32b;
WebRtc_Word32 tmp_inv_denum32;
WebRtc_Word16 tmp_inv_denum16;
k16[useOrder-1]= WEBRTC_SPL_LSHIFT_W16(a16[useOrder], 4); //Q11<<4 => Q15
for (m=useOrder-1; m>0; m--) {
tmp_inv_denum32 = ((WebRtc_Word32) 1073741823) - WEBRTC_SPL_MUL_16_16(k16[m], k16[m]); // (1 - k^2) in Q30
tmp_inv_denum16 = (WebRtc_Word16) WEBRTC_SPL_RSHIFT_W32(tmp_inv_denum32, 15); // (1 - k^2) in Q15
for (k=1; k<=m; k++) {
tmp32b = WEBRTC_SPL_LSHIFT_W32((WebRtc_Word32)a16[k], 16) -
WEBRTC_SPL_LSHIFT_W32(WEBRTC_SPL_MUL_16_16(k16[m], a16[m-k+1]), 1);
tmp32[k] = WebRtcSpl_DivW32W16(tmp32b, tmp_inv_denum16); //Q27/Q15 = Q12
}
for (k=1; k<m; k++) {
a16[k] = (WebRtc_Word16) WEBRTC_SPL_RSHIFT_W32(tmp32[k], 1); //Q12>>1 => Q11
}
tmp32[m] = WEBRTC_SPL_SAT(4092, tmp32[m], -4092);
k16[m-1] = (WebRtc_Word16) WEBRTC_SPL_LSHIFT_W32(tmp32[m], 3); //Q12<<3 => Q15
}
return;
}
void WebRtcVad_Allpass(WebRtc_Word16 *in_vector,
WebRtc_Word16 *out_vector,
WebRtc_Word16 filter_coefficients,
int vector_length,
WebRtc_Word16 *filter_state)
{
// The filter can only cause overflow (in the w16 output variable)
// if more than 4 consecutive input numbers are of maximum value and
// has the the same sign as the impulse responses first taps.
// First 6 taps of the impulse response: 0.6399 0.5905 -0.3779
// 0.2418 -0.1547 0.0990
int n;
WebRtc_Word16 tmp16;
WebRtc_Word32 tmp32, in32, state32;
state32 = WEBRTC_SPL_LSHIFT_W32(((WebRtc_Word32)(*filter_state)), 16); // Q31
for (n = 0; n < vector_length; n++)
{
tmp32 = state32 + WEBRTC_SPL_MUL_16_16(filter_coefficients, (*in_vector));
tmp16 = (WebRtc_Word16)WEBRTC_SPL_RSHIFT_W32(tmp32, 16);
*out_vector++ = tmp16;
in32 = WEBRTC_SPL_LSHIFT_W32(((WebRtc_Word32)(*in_vector)), 14);
state32 = in32 - WEBRTC_SPL_MUL_16_16(filter_coefficients, tmp16);
state32 = WEBRTC_SPL_LSHIFT_W32(state32, 1);
in_vector += 2;
}
*filter_state = (WebRtc_Word16)WEBRTC_SPL_RSHIFT_W32(state32, 16);
}
void WebRtcIlbcfix_Vq4(int16_t * Xq, /* quantized vector (Q13) */
int16_t * index, int16_t * CB, /* codebook in Q13 */
int16_t * X, /* vector to quantize (Q13) */
int16_t n_cb)
{
int16_t i, j;
int16_t pos, minindex = 0;
int16_t tmp;
int32_t dist, mindist;
pos = 0;
mindist = WEBRTC_SPL_WORD32_MAX; /* start value */
/* Find the codebook with the lowest square distance */
for (j = 0; j < n_cb; j++) {
tmp = X[0] - CB[pos];
dist = WEBRTC_SPL_MUL_16_16(tmp, tmp);
for (i = 1; i < 4; i++) {
tmp = X[i] - CB[pos + i];
dist += WEBRTC_SPL_MUL_16_16(tmp, tmp);
}
if (dist < mindist) {
mindist = dist;
minindex = j;
}
pos += 4;
}
/* Store the quantized codebook and the index */
for (i = 0; i < 4; i++) {
Xq[i] = CB[minindex * 4 + i];
}
*index = minindex;
}
/* Filter ar_g_Q0[] and ar_f_Q0[] through an AR filter with coefficients
* cth_Q15[] and sth_Q15[].
*/
void WebRtcIsacfix_FilterArLoop(int16_t* ar_g_Q0, // Input samples
int16_t* ar_f_Q0, // Input samples
int16_t* cth_Q15, // Filter coefficients
int16_t* sth_Q15, // Filter coefficients
int16_t order_coef) { // order of the filter
int n = 0;
for (n = 0; n < HALF_SUBFRAMELEN - 1; n++) {
int k = 0;
int16_t tmpAR = 0;
int32_t tmp32 = 0;
int32_t tmp32_2 = 0;
tmpAR = ar_f_Q0[n + 1];
for (k = order_coef - 1; k >= 0; k--) {
tmp32 = WEBRTC_SPL_RSHIFT_W32(((WEBRTC_SPL_MUL_16_16(cth_Q15[k], tmpAR))
- (WEBRTC_SPL_MUL_16_16(sth_Q15[k], ar_g_Q0[k])) + 16384), 15);
tmp32_2 = WEBRTC_SPL_RSHIFT_W32(((WEBRTC_SPL_MUL_16_16(sth_Q15[k], tmpAR))
+ (WEBRTC_SPL_MUL_16_16(cth_Q15[k], ar_g_Q0[k])) + 16384), 15);
tmpAR = (WebRtc_Word16)WebRtcSpl_SatW32ToW16(tmp32);
ar_g_Q0[k + 1] = (WebRtc_Word16)WebRtcSpl_SatW32ToW16(tmp32_2);
}
ar_f_Q0[n + 1] = tmpAR;
ar_g_Q0[0] = tmpAR;
}
}
void WebRtcIsacfix_PitchFilterCore(int loopNumber,
int16_t gain,
int index,
int16_t sign,
int16_t* inputState,
int16_t* outputBuf2,
const int16_t* coefficient,
int16_t* inputBuf,
int16_t* outputBuf,
int* index2) {
int i = 0, j = 0; /* Loop counters. */
int16_t* ubufQQpos2 = &outputBuf2[PITCH_BUFFSIZE - (index + 2)];
int16_t tmpW16 = 0;
for (i = 0; i < loopNumber; i++) {
int32_t tmpW32 = 0;
/* Filter to get fractional pitch. */
for (j = 0; j < PITCH_FRACORDER; j++) {
tmpW32 += WEBRTC_SPL_MUL_16_16(ubufQQpos2[*index2 + j], coefficient[j]);
}
/* Saturate to avoid overflow in tmpW16. */
tmpW32 = WEBRTC_SPL_SAT(536862719, tmpW32, -536879104);
tmpW32 += 8192;
tmpW16 = (int16_t)WEBRTC_SPL_RSHIFT_W32(tmpW32, 14);
/* Shift low pass filter state. */
memmove(&inputState[1], &inputState[0],
(PITCH_DAMPORDER - 1) * sizeof(int16_t));
inputState[0] = (int16_t)WEBRTC_SPL_MUL_16_16_RSFT_WITH_ROUND(
gain, tmpW16, 12);
/* Low pass filter. */
tmpW32 = 0;
/* TODO(kma): Define a static inline function WebRtcSpl_DotProduct()
in spl_inl.h to replace this and other similar loops. */
for (j = 0; j < PITCH_DAMPORDER; j++) {
tmpW32 += WEBRTC_SPL_MUL_16_16(inputState[j], kDampFilter[j]);
}
/* Saturate to avoid overflow in tmpW16. */
tmpW32 = WEBRTC_SPL_SAT(1073725439, tmpW32, -1073758208);
tmpW32 += 16384;
tmpW16 = (int16_t)WEBRTC_SPL_RSHIFT_W32(tmpW32, 15);
/* Subtract from input and update buffer. */
tmpW32 = inputBuf[*index2] - WEBRTC_SPL_MUL_16_16(sign, tmpW16);
outputBuf[*index2] = WebRtcSpl_SatW32ToW16(tmpW32);
tmpW32 = inputBuf[*index2] + outputBuf[*index2];
outputBuf2[*index2 + PITCH_BUFFSIZE] = WebRtcSpl_SatW32ToW16(tmpW32);
(*index2)++;
}
}
/* 1D parabolic interpolation . All input and output values are in Q8 */
static __inline void Intrp1DQ8(int32_t *x, int32_t *fx, int32_t *y, int32_t *fy) {
int16_t sign1=1, sign2=1;
int32_t r32, q32, t32, nom32, den32;
int16_t t16, tmp16, tmp16_1;
if ((fx[0]>0) && (fx[2]>0)) {
r32=fx[1]-fx[2];
q32=fx[0]-fx[1];
nom32=q32+r32;
den32=WEBRTC_SPL_MUL_32_16((q32-r32), 2);
if (nom32<0)
sign1=-1;
if (den32<0)
sign2=-1;
/* t = (q32+r32)/(2*(q32-r32)) = (fx[0]-fx[1] + fx[1]-fx[2])/(2 * fx[0]-fx[1] - (fx[1]-fx[2]))*/
/* (Signs are removed because WebRtcSpl_DivResultInQ31 can't handle negative numbers) */
t32=WebRtcSpl_DivResultInQ31(WEBRTC_SPL_MUL_32_16(nom32, sign1),WEBRTC_SPL_MUL_32_16(den32, sign2)); /* t in Q31, without signs */
t16=(int16_t)WEBRTC_SPL_RSHIFT_W32(t32, 23); /* Q8 */
t16=t16*sign1*sign2; /* t in Q8 with signs */
*y = x[0]+t16; /* Q8 */
// *y = x[1]+t16; /* Q8 */
/* The following code calculates fy in three steps */
/* fy = 0.5 * t * (t-1) * fx[0] + (1-t*t) * fx[1] + 0.5 * t * (t+1) * fx[2]; */
/* Part I: 0.5 * t * (t-1) * fx[0] */
tmp16_1=(int16_t)WEBRTC_SPL_MUL_16_16(t16,t16); /* Q8*Q8=Q16 */
tmp16_1 = WEBRTC_SPL_RSHIFT_W16(tmp16_1,2); /* Q16>>2 = Q14 */
t16 = (int16_t)WEBRTC_SPL_MUL_16_16(t16, 64); /* Q8<<6 = Q14 */
tmp16 = tmp16_1-t16;
*fy = WEBRTC_SPL_MUL_16_32_RSFT15(tmp16, fx[0]); /* (Q14 * Q8 >>15)/2 = Q8 */
/* Part II: (1-t*t) * fx[1] */
tmp16 = 16384-tmp16_1; /* 1 in Q14 - Q14 */
*fy += WEBRTC_SPL_MUL_16_32_RSFT14(tmp16, fx[1]);/* Q14 * Q8 >> 14 = Q8 */
/* Part III: 0.5 * t * (t+1) * fx[2] */
tmp16 = tmp16_1+t16;
*fy += WEBRTC_SPL_MUL_16_32_RSFT15(tmp16, fx[2]);/* (Q14 * Q8 >>15)/2 = Q8 */
} else {
*y = x[0];
*fy= fx[1];
}
}
void WebRtcIlbcfix_MyCorr(
int32_t *corr, /* (o) correlation of seq1 and seq2 */
int16_t *seq1, /* (i) first sequence */
int16_t dim1, /* (i) dimension first seq1 */
const int16_t *seq2, /* (i) second sequence */
int16_t dim2 /* (i) dimension seq2 */
){
int16_t max, scale, loops;
/* Calculate correlation between the two sequences. Scale the
result of the multiplcication to maximum 26 bits in order
to avoid overflow */
max=WebRtcSpl_MaxAbsValueW16(seq1, dim1);
scale=WebRtcSpl_GetSizeInBits(max);
scale = (int16_t)(WEBRTC_SPL_MUL_16_16(2,scale)-26);
if (scale<0) {
scale=0;
}
loops=dim1-dim2+1;
/* Calculate the cross correlations */
WebRtcSpl_CrossCorrelation(corr, (int16_t*)seq2, seq1, dim2, loops, scale, 1);
return;
}
void WebRtcIlbcfix_SimpleLsfDeQ(int16_t * lsfdeq, /* (o) dequantized lsf coefficients */
int16_t * index, /* (i) quantization index */
int16_t lpc_n /* (i) number of LPCs */
)
{
int i, j, pos, cb_pos;
/* decode first LSF */
pos = 0;
cb_pos = 0;
for (i = 0; i < LSF_NSPLIT; i++) {
for (j = 0; j < WebRtcIlbcfix_kLsfDimCb[i]; j++) {
lsfdeq[pos + j] = WebRtcIlbcfix_kLsfCb[cb_pos +
WEBRTC_SPL_MUL_16_16
(index[i],
WebRtcIlbcfix_kLsfDimCb
[i]) + j];
}
pos += WebRtcIlbcfix_kLsfDimCb[i];
cb_pos +=
WEBRTC_SPL_MUL_16_16(WebRtcIlbcfix_kLsfSizeCb[i],
WebRtcIlbcfix_kLsfDimCb[i]);
}
if (lpc_n > 1) {
/* decode last LSF */
pos = 0;
cb_pos = 0;
for (i = 0; i < LSF_NSPLIT; i++) {
for (j = 0; j < WebRtcIlbcfix_kLsfDimCb[i]; j++) {
lsfdeq[LPC_FILTERORDER + pos + j] =
WebRtcIlbcfix_kLsfCb[cb_pos +
WEBRTC_SPL_MUL_16_16
(index[LSF_NSPLIT + i],
WebRtcIlbcfix_kLsfDimCb
[i]) + j];
}
pos += WebRtcIlbcfix_kLsfDimCb[i];
cb_pos +=
WEBRTC_SPL_MUL_16_16(WebRtcIlbcfix_kLsfSizeCb[i],
WebRtcIlbcfix_kLsfDimCb[i]);
}
}
return;
}
/* Compute the energy of the rest of the cb memory
* by step wise adding and subtracting the next
* sample and the last sample respectively */
void WebRtcIlbcfix_CbMemEnergyCalc(
WebRtc_Word32 energy, /* (i) input start energy */
WebRtc_Word16 range, /* (i) number of iterations */
WebRtc_Word16 *ppi, /* (i) input pointer 1 */
WebRtc_Word16 *ppo, /* (i) input pointer 2 */
WebRtc_Word16 *energyW16, /* (o) Energy in the CB vectors */
WebRtc_Word16 *energyShifts, /* (o) Shift value of the energy */
WebRtc_Word16 scale, /* (i) The scaling of all energy values */
WebRtc_Word16 base_size /* (i) Index to where the energy values should be stored */
)
{
WebRtc_Word16 j,shft;
WebRtc_Word32 tmp;
WebRtc_Word16 *eSh_ptr;
WebRtc_Word16 *eW16_ptr;
eSh_ptr = &energyShifts[1+base_size];
eW16_ptr = &energyW16[1+base_size];
for(j=0;j<range-1;j++) {
/* Calculate next energy by a +/-
operation on the edge samples */
tmp = WEBRTC_SPL_MUL_16_16(*ppi, *ppi);
tmp -= WEBRTC_SPL_MUL_16_16(*ppo, *ppo);
energy += WEBRTC_SPL_RSHIFT_W32(tmp, scale);
energy = WEBRTC_SPL_MAX(energy, 0);
ppi--;
ppo--;
/* Normalize the energy into a WebRtc_Word16 and store
the number of shifts */
shft = (WebRtc_Word16)WebRtcSpl_NormW32(energy);
*eSh_ptr++ = shft;
tmp = WEBRTC_SPL_LSHIFT_W32(energy, shft);
*eW16_ptr++ = (WebRtc_Word16)WEBRTC_SPL_RSHIFT_W32(tmp, 16);
}
}
void WebRtcIlbcfix_CbConstruct(
WebRtc_Word16 *decvector, /* (o) Decoded vector */
WebRtc_Word16 *index, /* (i) Codebook indices */
WebRtc_Word16 *gain_index, /* (i) Gain quantization indices */
WebRtc_Word16 *mem, /* (i) Buffer for codevector construction */
WebRtc_Word16 lMem, /* (i) Length of buffer */
WebRtc_Word16 veclen /* (i) Length of vector */
){
int j;
WebRtc_Word16 gain[CB_NSTAGES];
/* Stack based */
WebRtc_Word16 cbvec0[SUBL];
WebRtc_Word16 cbvec1[SUBL];
WebRtc_Word16 cbvec2[SUBL];
WebRtc_Word32 a32;
WebRtc_Word16 *gainPtr;
/* gain de-quantization */
gain[0] = WebRtcIlbcfix_GainDequant(gain_index[0], 16384, 0);
gain[1] = WebRtcIlbcfix_GainDequant(gain_index[1], gain[0], 1);
gain[2] = WebRtcIlbcfix_GainDequant(gain_index[2], gain[1], 2);
/* codebook vector construction and construction of total vector */
/* Stack based */
WebRtcIlbcfix_GetCbVec(cbvec0, mem, index[0], lMem, veclen);
WebRtcIlbcfix_GetCbVec(cbvec1, mem, index[1], lMem, veclen);
WebRtcIlbcfix_GetCbVec(cbvec2, mem, index[2], lMem, veclen);
gainPtr = &gain[0];
for (j=0;j<veclen;j++) {
a32 = WEBRTC_SPL_MUL_16_16(*gainPtr++, cbvec0[j]);
a32 += WEBRTC_SPL_MUL_16_16(*gainPtr++, cbvec1[j]);
a32 += WEBRTC_SPL_MUL_16_16(*gainPtr, cbvec2[j]);
gainPtr -= 2;
decvector[j] = (WebRtc_Word16) WEBRTC_SPL_RSHIFT_W32(a32 + 8192, 14);
}
return;
}
void WebRtcNetEQ_MixVoiceUnvoice(WebRtc_Word16 *pw16_outData, WebRtc_Word16 *pw16_voicedVec,
WebRtc_Word16 *pw16_unvoicedVec,
WebRtc_Word16 *w16_current_vfraction,
WebRtc_Word16 w16_vfraction_change, WebRtc_Word16 N)
{
int i;
WebRtc_Word16 w16_tmp2;
WebRtc_Word16 vfraction = *w16_current_vfraction;
w16_tmp2 = 16384 - vfraction;
for (i = 0; i < N; i++)
{
pw16_outData[i] = (WebRtc_Word16) WEBRTC_SPL_RSHIFT_W32(
WEBRTC_SPL_MUL_16_16(vfraction, pw16_voicedVec[i]) +
WEBRTC_SPL_MUL_16_16(w16_tmp2, pw16_unvoicedVec[i]) + 8192,
14);
vfraction -= w16_vfraction_change;
w16_tmp2 += w16_vfraction_change;
}
*w16_current_vfraction = vfraction;
}
int WebRtcSpl_DownsampleFast(WebRtc_Word16 *in_ptr, WebRtc_Word16 in_length,
WebRtc_Word16 *out_ptr, WebRtc_Word16 out_length,
WebRtc_Word16 *B, WebRtc_Word16 B_length, WebRtc_Word16 factor,
WebRtc_Word16 delay)
{
WebRtc_Word32 o;
int i, j;
WebRtc_Word16 *downsampled_ptr = out_ptr;
WebRtc_Word16 *b_ptr;
WebRtc_Word16 *x_ptr;
WebRtc_Word16 endpos = delay
+ (WebRtc_Word16)WEBRTC_SPL_MUL_16_16(factor, (out_length - 1)) + 1;
if (in_length < endpos)
{
return -1;
}
for (i = delay; i < endpos; i += factor)
{
b_ptr = &B[0];
x_ptr = &in_ptr[i];
o = (WebRtc_Word32)2048; // Round val
for (j = 0; j < B_length; j++)
{
o += WEBRTC_SPL_MUL_16_16(*b_ptr++, *x_ptr--);
}
o = WEBRTC_SPL_RSHIFT_W32(o, 12);
// If output is higher than 32768, saturate it. Same with negative side
*downsampled_ptr++ = WebRtcSpl_SatW32ToW16(o);
}
return 0;
}
static inline void DotProductWithScaleNeon(int32_t* cross_correlation,
const int16_t* vector1,
const int16_t* vector2,
size_t length,
int scaling) {
size_t i = 0;
size_t len1 = length >> 3;
size_t len2 = length & 7;
int64x2_t sum0 = vdupq_n_s64(0);
int64x2_t sum1 = vdupq_n_s64(0);
for (i = len1; i > 0; i -= 1) {
int16x8_t seq1_16x8 = vld1q_s16(vector1);
int16x8_t seq2_16x8 = vld1q_s16(vector2);
#if defined(WEBRTC_ARCH_ARM64)
int32x4_t tmp0 = vmull_s16(vget_low_s16(seq1_16x8),
vget_low_s16(seq2_16x8));
int32x4_t tmp1 = vmull_high_s16(seq1_16x8, seq2_16x8);
#else
int32x4_t tmp0 = vmull_s16(vget_low_s16(seq1_16x8),
vget_low_s16(seq2_16x8));
int32x4_t tmp1 = vmull_s16(vget_high_s16(seq1_16x8),
vget_high_s16(seq2_16x8));
#endif
sum0 = vpadalq_s32(sum0, tmp0);
sum1 = vpadalq_s32(sum1, tmp1);
vector1 += 8;
vector2 += 8;
}
// Calculate the rest of the samples.
int64_t sum_res = 0;
for (i = len2; i > 0; i -= 1) {
sum_res += WEBRTC_SPL_MUL_16_16(*vector1, *vector2);
vector1++;
vector2++;
}
sum0 = vaddq_s64(sum0, sum1);
#if defined(WEBRTC_ARCH_ARM64)
int64_t sum2 = vaddvq_s64(sum0);
*cross_correlation = (int32_t)((sum2 + sum_res) >> scaling);
#else
int64x1_t shift = vdup_n_s64(-scaling);
int64x1_t sum2 = vadd_s64(vget_low_s64(sum0), vget_high_s64(sum0));
sum2 = vadd_s64(sum2, vdup_n_s64(sum_res));
sum2 = vshl_s64(sum2, shift);
vst1_lane_s32(cross_correlation, vreinterpret_s32_s64(sum2), 0);
#endif
}
void WebRtcIlbcfix_Interpolate(
WebRtc_Word16 *out, /* (o) output vector */
WebRtc_Word16 *in1, /* (i) first input vector */
WebRtc_Word16 *in2, /* (i) second input vector */
WebRtc_Word16 coef, /* (i) weight coefficient in Q14 */
WebRtc_Word16 length) /* (i) number of sample is vectors */
{
int i;
WebRtc_Word16 invcoef;
/*
Performs the operation out[i] = in[i]*coef + (1-coef)*in2[i] (with rounding)
*/
invcoef = 16384 - coef; /* 16384 = 1.0 (Q14)*/
for (i = 0; i < length; i++) {
out[i] = (WebRtc_Word16) WEBRTC_SPL_RSHIFT_W32(
(WEBRTC_SPL_MUL_16_16(coef, in1[i]) + WEBRTC_SPL_MUL_16_16(invcoef, in2[i]))+8192,
14);
}
return;
}
WebRtc_Word32 WebRtcVad_GaussianProbability(WebRtc_Word16 in_sample,
WebRtc_Word16 mean,
WebRtc_Word16 std,
WebRtc_Word16 *delta)
{
WebRtc_Word16 tmp16, tmpDiv, tmpDiv2, expVal, tmp16_1, tmp16_2;
WebRtc_Word32 tmp32, y32;
// Calculate tmpDiv=1/std, in Q10
tmp32 = (WebRtc_Word32)WEBRTC_SPL_RSHIFT_W16(std,1) + (WebRtc_Word32)131072; // 1 in Q17
tmpDiv = (WebRtc_Word16)WebRtcSpl_DivW32W16(tmp32, std); // Q17/Q7 = Q10
// Calculate tmpDiv2=1/std^2, in Q14
tmp16 = WEBRTC_SPL_RSHIFT_W16(tmpDiv, 2); // From Q10 to Q8
tmpDiv2 = (WebRtc_Word16)WEBRTC_SPL_MUL_16_16_RSFT(tmp16, tmp16, 2); // (Q8 * Q8)>>2 = Q14
tmp16 = WEBRTC_SPL_LSHIFT_W16(in_sample, 3); // Q7
tmp16 = tmp16 - mean; // Q7 - Q7 = Q7
// To be used later, when updating noise/speech model
// delta = (x-m)/std^2, in Q11
*delta = (WebRtc_Word16)WEBRTC_SPL_MUL_16_16_RSFT(tmpDiv2, tmp16, 10); //(Q14*Q7)>>10 = Q11
// Calculate tmp32=(x-m)^2/(2*std^2), in Q10
tmp32 = (WebRtc_Word32)WEBRTC_SPL_MUL_16_16_RSFT(*delta, tmp16, 9); // One shift for /2
// Calculate expVal ~= exp(-(x-m)^2/(2*std^2)) ~= exp2(-log2(exp(1))*tmp32)
if (tmp32 < kCompVar)
{
// Calculate tmp16 = log2(exp(1))*tmp32 , in Q10
tmp16 = (WebRtc_Word16)WEBRTC_SPL_MUL_16_16_RSFT((WebRtc_Word16)tmp32,
kLog10Const, 12);
tmp16 = -tmp16;
tmp16_2 = (WebRtc_Word16)(0x0400 | (tmp16 & 0x03FF));
tmp16_1 = (WebRtc_Word16)(tmp16 ^ 0xFFFF);
tmp16 = (WebRtc_Word16)WEBRTC_SPL_RSHIFT_W16(tmp16_1, 10);
tmp16 += 1;
// Calculate expVal=log2(-tmp32), in Q10
expVal = (WebRtc_Word16)WEBRTC_SPL_RSHIFT_W32((WebRtc_Word32)tmp16_2, tmp16);
} else
{
expVal = 0;
}
// Calculate y32=(1/std)*exp(-(x-m)^2/(2*std^2)), in Q20
y32 = WEBRTC_SPL_MUL_16_16(tmpDiv, expVal); // Q10 * Q10 = Q20
return y32; // Q20
}
WebRtc_Word32 WebRtcIlbcfix_Smooth_odata(
WebRtc_Word16 *odata,
WebRtc_Word16 *psseq,
WebRtc_Word16 *surround,
WebRtc_Word16 C)
{
int i;
WebRtc_Word16 err;
WebRtc_Word32 errs;
for(i=0;i<80;i++) {
odata[i]= (WebRtc_Word16)WEBRTC_SPL_RSHIFT_W32(
(WEBRTC_SPL_MUL_16_16(C, surround[i])+1024), 11);
}
errs=0;
for(i=0;i<80;i++) {
err=(WebRtc_Word16)WEBRTC_SPL_RSHIFT_W16((psseq[i]-odata[i]), 3);
errs+=WEBRTC_SPL_MUL_16_16(err, err); /* errs in Q-6 */
}
return errs;
}
void WebRtcSpl_LpcToReflCoef(int16_t* a16, int use_order, int16_t* k16)
{
int m, k;
int32_t tmp32[SPL_LPC_TO_REFL_COEF_MAX_AR_MODEL_ORDER];
int32_t tmp_inv_denom32;
int16_t tmp_inv_denom16;
k16[use_order - 1] = a16[use_order] << 3; // Q12<<3 => Q15
for (m = use_order - 1; m > 0; m--)
{
// (1 - k^2) in Q30
tmp_inv_denom32 = ((int32_t)1073741823) - WEBRTC_SPL_MUL_16_16(k16[m], k16[m]);
// (1 - k^2) in Q15
tmp_inv_denom16 = (int16_t)(tmp_inv_denom32 >> 15);
for (k = 1; k <= m; k++)
{
// tmp[k] = (a[k] - RC[m] * a[m-k+1]) / (1.0 - RC[m]*RC[m]);
// [Q12<<16 - (Q15*Q12)<<1] = [Q28 - Q28] = Q28
tmp32[k] = WEBRTC_SPL_LSHIFT_W32((int32_t)a16[k], 16)
- WEBRTC_SPL_LSHIFT_W32(WEBRTC_SPL_MUL_16_16(k16[m], a16[m-k+1]), 1);
tmp32[k] = WebRtcSpl_DivW32W16(tmp32[k], tmp_inv_denom16); //Q28/Q15 = Q13
}
for (k = 1; k < m; k++)
{
a16[k] = (int16_t)(tmp32[k] >> 1); // Q13>>1 => Q12
}
tmp32[m] = WEBRTC_SPL_SAT(8191, tmp32[m], -8191);
k16[m - 1] = (int16_t)WEBRTC_SPL_LSHIFT_W32(tmp32[m], 2); //Q13<<2 => Q15
}
return;
}
int32_t WebRtcIlbcfix_Smooth_odata(
int16_t *odata,
int16_t *psseq,
int16_t *surround,
int16_t C)
{
int i;
int16_t err;
int32_t errs;
for(i=0;i<80;i++) {
odata[i]= (int16_t)WEBRTC_SPL_RSHIFT_W32(
(WEBRTC_SPL_MUL_16_16(C, surround[i])+1024), 11);
}
errs=0;
for(i=0;i<80;i++) {
err=(int16_t)WEBRTC_SPL_RSHIFT_W16((psseq[i]-odata[i]), 3);
errs+=WEBRTC_SPL_MUL_16_16(err, err); /* errs in Q-6 */
}
return errs;
}
void WebRtcVad_HpOutput(WebRtc_Word16 *in_vector,
WebRtc_Word16 in_vector_length,
WebRtc_Word16 *out_vector,
WebRtc_Word16 *filter_state)
{
WebRtc_Word16 i, *pi, *outPtr;
WebRtc_Word32 tmpW32;
pi = &in_vector[0];
outPtr = &out_vector[0];
// The sum of the absolute values of the impulse response:
// The zero/pole-filter has a max amplification of a single sample of: 1.4546
// Impulse response: 0.4047 -0.6179 -0.0266 0.1993 0.1035 -0.0194
// The all-zero section has a max amplification of a single sample of: 1.6189
// Impulse response: 0.4047 -0.8094 0.4047 0 0 0
// The all-pole section has a max amplification of a single sample of: 1.9931
// Impulse response: 1.0000 0.4734 -0.1189 -0.2187 -0.0627 0.04532
for (i = 0; i < in_vector_length; i++)
{
// all-zero section (filter coefficients in Q14)
tmpW32 = (WebRtc_Word32)WEBRTC_SPL_MUL_16_16(kHpZeroCoefs[0], (*pi));
tmpW32 += (WebRtc_Word32)WEBRTC_SPL_MUL_16_16(kHpZeroCoefs[1], filter_state[0]);
tmpW32 += (WebRtc_Word32)WEBRTC_SPL_MUL_16_16(kHpZeroCoefs[2], filter_state[1]); // Q14
filter_state[1] = filter_state[0];
filter_state[0] = *pi++;
// all-pole section
tmpW32 -= (WebRtc_Word32)WEBRTC_SPL_MUL_16_16(kHpPoleCoefs[1], filter_state[2]); // Q14
tmpW32 -= (WebRtc_Word32)WEBRTC_SPL_MUL_16_16(kHpPoleCoefs[2], filter_state[3]);
filter_state[3] = filter_state[2];
filter_state[2] = (WebRtc_Word16)WEBRTC_SPL_RSHIFT_W32 (tmpW32, 14);
*outPtr++ = filter_state[2];
}
}
void WebRtcIsacfix_DecimateAllpass32(const WebRtc_Word16 *in,
WebRtc_Word32 *state_in, /* array of size: 2*ALLPASSSECTIONS+1 */
WebRtc_Word16 N, /* number of input samples */
WebRtc_Word16 *out) /* array of size N/2 */
{
int n;
WebRtc_Word16 data_vec[PITCH_FRAME_LEN];
/* copy input */
memcpy(data_vec+1, in, WEBRTC_SPL_MUL_16_16(sizeof(WebRtc_Word16), (N-1)));
data_vec[0] = (WebRtc_Word16) WEBRTC_SPL_RSHIFT_W32(state_in[WEBRTC_SPL_MUL_16_16(2, ALLPASSSECTIONS)],16); //the z^(-1) state
state_in[WEBRTC_SPL_MUL_16_16(2, ALLPASSSECTIONS)] = WEBRTC_SPL_LSHIFT_W32((WebRtc_UWord32)in[N-1],16);
AllpassFilterForDec32(data_vec+1, kApUpperQ15, N, state_in);
AllpassFilterForDec32(data_vec, kApLowerQ15, N, state_in+ALLPASSSECTIONS);
for (n=0;n<N/2;n++) {
out[n]=WEBRTC_SPL_ADD_SAT_W16(data_vec[WEBRTC_SPL_MUL_16_16(2, n)], data_vec[WEBRTC_SPL_MUL_16_16(2, n)+1]);
}
}
void WebRtcSpl_AffineTransformVector(WebRtc_Word16 *out, WebRtc_Word16 *in,
WebRtc_Word16 gain, WebRtc_Word32 add_constant,
WebRtc_Word16 right_shifts, int vector_length)
{
WebRtc_Word16 *inPtr;
WebRtc_Word16 *outPtr;
int i;
inPtr = in;
outPtr = out;
for (i = 0; i < vector_length; i++)
{
(*outPtr++) = (WebRtc_Word16)((WEBRTC_SPL_MUL_16_16((*inPtr++), gain)
+ (WebRtc_Word32)add_constant) >> right_shifts);
}
}
void WebRtcSpl_AffineTransformVector(int16_t *out, int16_t *in,
int16_t gain, int32_t add_constant,
int16_t right_shifts, int vector_length)
{
int16_t *inPtr;
int16_t *outPtr;
int i;
inPtr = in;
outPtr = out;
for (i = 0; i < vector_length; i++)
{
(*outPtr++) = (int16_t)((WEBRTC_SPL_MUL_16_16((*inPtr++), gain)
+ (int32_t)add_constant) >> right_shifts);
}
}
/* The output is in the same domain as the input */
void WebRtcIlbcfix_BwExpand(
WebRtc_Word16 *out, /* (o) the bandwidth expanded lpc coefficients */
WebRtc_Word16 *in, /* (i) the lpc coefficients before bandwidth
expansion */
WebRtc_Word16 *coef, /* (i) the bandwidth expansion factor Q15 */
WebRtc_Word16 length /* (i) the length of lpc coefficient vectors */
) {
int i;
out[0] = in[0];
for (i = 1; i < length; i++) {
/* out[i] = coef[i] * in[i] with rounding.
in[] and out[] are in Q12 and coef[] is in Q15
*/
out[i] = (WebRtc_Word16)((WEBRTC_SPL_MUL_16_16(coef[i], in[i])+16384)>>15);
}
}
void WebRtcIsacfix_InitPitchAnalysis(PitchAnalysisStruct *State)
{
int k;
for (k = 0; k < PITCH_CORR_LEN2+PITCH_CORR_STEP2+PITCH_MAX_LAG/2-PITCH_FRAME_LEN/2+2; k++) {
State->dec_buffer16[k] = 0;
}
for (k = 0; k < WEBRTC_SPL_MUL_16_16(2, ALLPASSSECTIONS)+1; k++) {
State->decimator_state32[k] = 0;
}
for (k = 0; k < QLOOKAHEAD; k++)
State->inbuf[k] = 0;
WebRtcIsacfix_InitPitchFilter(&(State->PFstr_wght));
WebRtcIsacfix_InitPitchFilter(&(State->PFstr));
}
int16_t WebRtcIlbcfix_GainDequant(
/* (o) quantized gain value (Q14) */
int16_t index, /* (i) quantization index */
int16_t maxIn, /* (i) maximum of unquantized gain (Q14) */
int16_t stage /* (i) The stage of the search */
){
int16_t scale;
const int16_t *gain;
/* obtain correct scale factor */
scale=WEBRTC_SPL_ABS_W16(maxIn);
scale = WEBRTC_SPL_MAX(1638, scale); /* if lower than 0.1, set it to 0.1 */
/* select the quantization table and return the decoded value */
gain = WebRtcIlbcfix_kGain[stage];
return((int16_t)((WEBRTC_SPL_MUL_16_16(scale, gain[index])+8192)>>14));
}
void WebRtcIsacfix_InitPostFilterbank(PostFiltBankstr *postfiltdata)
{
int k;
for (k = 0; k < WEBRTC_SPL_MUL_16_16(2, POSTQORDER); k++) {
postfiltdata->STATE_0_LOWER_fix[k] = 0;
postfiltdata->STATE_0_UPPER_fix[k] = 0;
}
/* High pass filter states */
postfiltdata->HPstates1_fix[0] = 0;
postfiltdata->HPstates1_fix[1] = 0;
postfiltdata->HPstates2_fix[0] = 0;
postfiltdata->HPstates2_fix[1] = 0;
return;
}
void WebRtcIsacfix_InitPreFilterbank(PreFiltBankstr *prefiltdata)
{
int k;
for (k = 0; k < QLOOKAHEAD; k++) {
prefiltdata->INLABUF1_fix[k] = 0;
prefiltdata->INLABUF2_fix[k] = 0;
}
for (k = 0; k < WEBRTC_SPL_MUL_16_16(2,(QORDER-1)); k++) {
prefiltdata->INSTAT1_fix[k] = 0;
prefiltdata->INSTAT2_fix[k] = 0;
}
/* High pass filter states */
prefiltdata->HPstates_fix[0] = 0;
prefiltdata->HPstates_fix[1] = 0;
return;
}
void WebRtcIlbcfix_NearestNeighbor(
int16_t *index, /* (o) index of array element closest to value */
int16_t *array, /* (i) data array (Q2) */
int16_t value, /* (i) value (Q2) */
int16_t arlength /* (i) dimension of data array (==8) */
){
int i;
int16_t diff;
/* Stack based */
int32_t crit[8];
/* Calculate square distance */
for(i=0;i<arlength;i++){
diff=array[i]-value;
crit[i]=WEBRTC_SPL_MUL_16_16(diff, diff);
}
/* Find the minimum square distance */
*index=WebRtcSpl_MinIndexW32(crit, (int16_t)arlength);
}
/* This routine calculates the residual energy for LPC.
* Formula as shown in comments inside.
*/
int32_t WebRtcIsacfix_CalculateResidualEnergyC(int lpc_order,
int32_t q_val_corr,
int q_val_polynomial,
int16_t* a_polynomial,
int32_t* corr_coeffs,
int* q_val_residual_energy) {
int i = 0, j = 0;
int shift_internal = 0, shift_norm = 0;
int32_t tmp32 = 0, word32_high = 0, word32_low = 0, residual_energy = 0;
int64_t sum64 = 0, sum64_tmp = 0;
for (i = 0; i <= lpc_order; i++) {
for (j = i; j <= lpc_order; j++) {
/* For the case of i == 0: residual_energy +=
* a_polynomial[j] * corr_coeffs[i] * a_polynomial[j - i];
* For the case of i != 0: residual_energy +=
* a_polynomial[j] * corr_coeffs[i] * a_polynomial[j - i] * 2;
*/
tmp32 = WEBRTC_SPL_MUL_16_16(a_polynomial[j], a_polynomial[j - i]);
/* tmp32 in Q(q_val_polynomial * 2). */
if (i != 0) {
tmp32 <<= 1;
}
sum64_tmp = (int64_t)tmp32 * (int64_t)corr_coeffs[i];
sum64_tmp >>= shift_internal;
/* Test overflow and sum the result. */
if(((sum64_tmp > 0 && sum64 > 0) && (LLONG_MAX - sum64 < sum64_tmp)) ||
((sum64_tmp < 0 && sum64 < 0) && (LLONG_MIN - sum64 > sum64_tmp))) {
/* Shift right for overflow. */
shift_internal += 1;
sum64 >>= 1;
sum64 += sum64_tmp >> 1;
} else {
sum64 += sum64_tmp;
}
}
void WebRtcIlbcfix_NearestNeighbor(
iLBC_Dec_Inst_t *iLBCdec_inst,
/* (i) Decoder state */
WebRtc_Word16 *index, /* (o) index of array element closest to value */
WebRtc_Word16 *array, /* (i) data array (Q2) */
WebRtc_Word16 value, /* (i) value (Q2) */
WebRtc_Word16 arlength /* (i) dimension of data array (==8) */
){
int i;
WebRtc_Word16 diff;
/* Stack based */
WebRtc_Word32 crit[8];
/* The input variable iLBCdec_inst is unused if not using scratch memory */
iLBCdec_inst = iLBCdec_inst;
/* Calculate square distance */
for(i=0;i<arlength;i++){
diff=array[i]-value;
crit[i]=WEBRTC_SPL_MUL_16_16(diff, diff);
}
/* Find the minimum square distance */
*index=WebRtcSpl_MinIndexW32(crit, (WebRtc_Word16)arlength);
}