int32_t WebRtcSpl_Energy(int16_t* vector, int vector_length, int* scale_factor)
{
int32_t en = 0;
int i;
int scaling = WebRtcSpl_GetScalingSquare(vector, vector_length, vector_length);
int looptimes = vector_length;
int16_t *vectorptr = vector;
for (i = 0; i < looptimes; i++)
{
en += WEBRTC_SPL_MUL_16_16_RSFT(*vectorptr, *vectorptr, scaling);
vectorptr++;
}
*scale_factor = scaling;
return en;
}
int32_t WebRtcSpl_Energy(int16_t* vector,
size_t vector_length,
int* scale_factor)
{
int32_t en = 0;
size_t i;
int scaling =
WebRtcSpl_GetScalingSquare(vector, vector_length, vector_length);
size_t looptimes = vector_length;
int16_t *vectorptr = vector;
for (i = 0; i < looptimes; i++)
{
en += (*vectorptr * *vectorptr) >> scaling;
vectorptr++;
}
*scale_factor = scaling;
return en;
}
static void PCorr2Q32(const int16_t *in, int32_t *logcorQ8)
{
int16_t scaling,n,k;
int32_t ysum32,csum32, lys, lcs;
int32_t oneQ8;
const int16_t *x, *inptr;
oneQ8 = WEBRTC_SPL_LSHIFT_W32((int32_t)1, 8); // 1.00 in Q8
x = in + PITCH_MAX_LAG/2 + 2;
scaling = WebRtcSpl_GetScalingSquare ((int16_t *) in, PITCH_CORR_LEN2, PITCH_CORR_LEN2);
ysum32 = 1;
csum32 = 0;
x = in + PITCH_MAX_LAG/2 + 2;
for (n = 0; n < PITCH_CORR_LEN2; n++) {
ysum32 += WEBRTC_SPL_MUL_16_16_RSFT( (int16_t) in[n],(int16_t) in[n], scaling); // Q0
csum32 += WEBRTC_SPL_MUL_16_16_RSFT((int16_t) x[n],(int16_t) in[n], scaling); // Q0
}
logcorQ8 += PITCH_LAG_SPAN2 - 1;
lys=Log2Q8((uint32_t) ysum32); // Q8
lys=WEBRTC_SPL_RSHIFT_W32(lys, 1); //sqrt(ysum);
if (csum32>0) {
lcs=Log2Q8((uint32_t) csum32); // 2log(csum) in Q8
if (lcs>(lys + oneQ8) ){ // csum/sqrt(ysum) > 2 in Q8
*logcorQ8 = lcs - lys; // log2(csum/sqrt(ysum))
} else {
*logcorQ8 = oneQ8; // 1.00
}
} else {
*logcorQ8 = 0;
}
for (k = 1; k < PITCH_LAG_SPAN2; k++) {
inptr = &in[k];
ysum32 -= WEBRTC_SPL_MUL_16_16_RSFT( (int16_t) in[k-1],(int16_t) in[k-1], scaling);
ysum32 += WEBRTC_SPL_MUL_16_16_RSFT( (int16_t) in[PITCH_CORR_LEN2 + k - 1],(int16_t) in[PITCH_CORR_LEN2 + k - 1], scaling);
#ifdef WEBRTC_ARCH_ARM_NEON
{
int32_t vbuff[4];
int32x4_t int_32x4_sum = vmovq_n_s32(0);
// Can't shift a Neon register to right with a non-constant shift value.
int32x4_t int_32x4_scale = vdupq_n_s32(-scaling);
// Assert a codition used in loop unrolling at compile-time.
COMPILE_ASSERT(PITCH_CORR_LEN2 %4 == 0);
for (n = 0; n < PITCH_CORR_LEN2; n += 4) {
int16x4_t int_16x4_x = vld1_s16(&x[n]);
int16x4_t int_16x4_in = vld1_s16(&inptr[n]);
int32x4_t int_32x4 = vmull_s16(int_16x4_x, int_16x4_in);
int_32x4 = vshlq_s32(int_32x4, int_32x4_scale);
int_32x4_sum = vaddq_s32(int_32x4_sum, int_32x4);
}
// Use vector store to avoid long stall from data trasferring
// from vector to general register.
vst1q_s32(vbuff, int_32x4_sum);
csum32 = vbuff[0] + vbuff[1];
csum32 += vbuff[2];
csum32 += vbuff[3];
}
#else
csum32 = 0;
if(scaling == 0) {
for (n = 0; n < PITCH_CORR_LEN2; n++) {
csum32 += x[n] * inptr[n];
}
} else {
for (n = 0; n < PITCH_CORR_LEN2; n++) {
csum32 += (x[n] * inptr[n]) >> scaling;
}
}
#endif
logcorQ8--;
lys=Log2Q8((uint32_t)ysum32); // Q8
lys=WEBRTC_SPL_RSHIFT_W32(lys, 1); //sqrt(ysum);
if (csum32>0) {
lcs=Log2Q8((uint32_t) csum32); // 2log(csum) in Q8
if (lcs>(lys + oneQ8) ){ // csum/sqrt(ysum) > 2
*logcorQ8 = lcs - lys; // log2(csum/sqrt(ysum))
} else {
*logcorQ8 = oneQ8; // 1.00
}
} else {
*logcorQ8 = 0;
}
}
}
