/*************************************************************************
*
* FUNCTION: Lag_max
*
* PURPOSE: Find the lag that has maximum correlation of scal_sig[] in a
* given delay range.
*
* DESCRIPTION:
* The correlation is given by
* cor[t] = <scal_sig[n],scal_sig[n-t]>, t=lag_min,...,lag_max
* The functions outputs the maximum correlation after normalization
* and the corresponding lag.
*
*************************************************************************/
Word16 Lag_max ( /* o : lag found */
vadState1 *vadSt, /* i/o : VAD state struct */
Word32 corr[], /* i : correlation vector. */
Word16 scal_sig[], /* i : scaled signal. */
Word16 scal_fac, /* i : scaled signal factor. */
Word16 scal_flag, /* i : if 1 use EFR compatible scaling */
Word16 L_frame, /* i : length of frame to compute pitch */
Word16 lag_max, /* i : maximum lag */
Word16 lag_min, /* i : minimum lag */
Word16 *cor_max, /* o : normalized correlation of selected lag */
Flag dtx /* i : dtx flag; use dtx=1, do not use dtx=0 */
)
{
Word16 i, j;
Word16 *p;
Word32 max, t0;
Word16 max_h, max_l, ener_h, ener_l;
Word16 p_max = 0; /* initialization only needed to keep gcc silent */
long lTemp;
max = MIN_32;
p_max = lag_max;
for (i = lag_max, j = (PIT_MAX-lag_max-1); i >= lag_min; i--, j--)
{
if (corr[-i] >= max)
{
max = corr[-i];
p_max = i;
}
}
/* compute energy */
t0 = 0;
p = &scal_sig[-p_max];
for (i = 0; i < L_frame; i++, p++)
{
//t0 = L_mac (t0, *p, *p);
lTemp = (*p) * (*p);
if (lTemp != 0x40000000)
{
lTemp = lTemp * 2;
}
else
{
lTemp = MAX_32;
}
lTemp = lTemp + t0;
if (lTemp > MAX_32)
{
t0 = MAX_32;
}
else if (lTemp < MIN_32)
{
t0 = MIN_32;
}
else
{
t0 = (Word32)lTemp;
}
}
/* 1/sqrt(energy) */
if (dtx)
{ /* no test() call since this if is only in simulation env */
/* check tone */
vad_tone_detection (vadSt, max, t0);
}
t0 = Inv_sqrt (t0); /* function result */
if (scal_flag)
{
t0 = L_shl (t0, 1);
}
/* max = max/sqrt(energy) */
L_Extract (max, &max_h, &max_l);
L_Extract (t0, &ener_h, &ener_l);
t0 = Mpy_32 (max_h, max_l, ener_h, ener_l);
if (scal_flag)
{
t0 = L_shr (t0, scal_fac);
*cor_max = extract_h (L_shl (t0, 15)); /* divide by 2 */
}
else
{
*cor_max = extract_l(t0);
}
return (p_max);
}
static Word16 Lag_max( /* o : lag found */
vadState *vadSt, /* i/o : VAD state struct */
Word32 corr[], /* i : correlation vector. */
Word16 scal_sig[], /* i : scaled signal. */
Word16 scal_fac, /* i : scaled signal factor. */
Word16 scal_flag, /* i : if 1 use EFR compatible scaling */
Word16 L_frame, /* i : length of frame to compute pitch */
Word16 lag_max, /* i : maximum lag */
Word16 lag_min, /* i : minimum lag */
Word16 *cor_max, /* o : normalized correlation of selected lag */
Flag dtx, /* i : dtx flag; use dtx=1, do not use dtx=0 */
Flag *pOverflow /* i/o : overflow Flag */
)
#endif
{
register Word16 i;
Word16 *p;
Word32 max;
Word32 t0;
Word16 max_h;
Word16 max_l;
Word16 ener_h;
Word16 ener_l;
Word16 p_max = 0; /* initialization only needed to keep gcc silent */
Word32 L_temp;
Word32 L_temp_2;
Word32 L_temp_3;
Word32 *p_corr = &corr[-lag_max];
max = MIN_32;
p_max = lag_max;
for (i = lag_max; i >= lag_min; i--)
{
/* The negative array index is equivalent to a negative */
/* address offset, i.e., corr[-i] == *(corr - i) */
if (*(p_corr++) >= max)
{
p_corr--;
max = *(p_corr++);
p_max = i;
}
}
/* compute energy */
t0 = 0;
/* The negative array index is equivalent to a negative */
/* address offset, i.e., scal_sig[-p_max] == *(scal_sig - p_max) */
p = &scal_sig[-p_max];
for (i = (L_frame >> 2); i != 0; i--)
{
t0 = amrnb_fxp_mac_16_by_16bb((Word32) * (p), (Word32) * (p), t0);
p++;
t0 = amrnb_fxp_mac_16_by_16bb((Word32) * (p), (Word32) * (p), t0);
p++;
t0 = amrnb_fxp_mac_16_by_16bb((Word32) * (p), (Word32) * (p), t0);
p++;
t0 = amrnb_fxp_mac_16_by_16bb((Word32) * (p), (Word32) * (p), t0);
p++;
}
t0 <<= 1;
/* 1/sqrt(energy) */
if (dtx)
{ /* no test() call since this if is only in simulation env */
/* check tone */
#ifdef VAD2
*rmax = max;
*r0 = t0;
#else
/* check tone */
vad_tone_detection(vadSt, max, t0, pOverflow);
#endif
}
t0 = Inv_sqrt(t0, pOverflow);
if (scal_flag)
{
if (t0 > (Word32) 0x3fffffffL)
{
t0 = MAX_32;
}
else
{
t0 = t0 << 1;
}
}
/* max = max/sqrt(energy) */
/* The following code is an inlined version of */
/* L_Extract (max, &max_h, &max_l), i.e. */
/* */
/* *max_h = extract_h (max); */
max_h = (Word16)(max >> 16);
/* L_temp_2 = L_shr(max,1), which is used in */
/* the calculation of *max_l (see next operation) */
L_temp_2 = max >> 1;
/* *max_l = extract_l (L_msu (L_shr (max, 1), *max_h, 16384)); */
L_temp_3 = (Word32)(max_h << 15);
L_temp = L_temp_2 - L_temp_3;
max_l = (Word16)L_temp;
/* The following code is an inlined version of */
/* L_Extract (t0, &ener_h, &ener_l), i.e. */
/* */
/* *ener_h = extract_h (t0); */
ener_h = (Word16)(t0 >> 16);
/* L_temp_2 = L_shr(t0,1), which is used in */
/* the calculation of *ener_l (see next operation) */
L_temp_2 = t0 >> 1;
L_temp_3 = (Word32)(ener_h << 15);
L_temp = L_temp_2 - L_temp_3;
ener_l = (Word16)L_temp;
t0 = Mpy_32(max_h, max_l, ener_h, ener_l, pOverflow);
if (scal_flag)
{
t0 = L_shr(t0, scal_fac, pOverflow);
if (t0 > (Word32) 0X0000FFFFL)
{
*cor_max = MAX_16;
}
else if (t0 < (Word32) 0xFFFF0000L)
{
*cor_max = MIN_16;
}
else
{
*cor_max = (Word16)(t0 >> 1);
}
}
else
{
static Word16 Lag_max( /* o : lag found */
vadState *vadSt, /* i/o : VAD state struct */
Word32 corr[], /* i : correlation vector. */
Word16 scal_sig[], /* i : scaled signal. */
Word16 L_frame, /* i : length of frame to compute pitch */
Word16 lag_max, /* i : maximum lag */
Word16 lag_min, /* i : minimum lag */
Word16 old_lag, /* i : old open-loop lag */
Word16 *cor_max, /* o : normalized correlation of selected lag */
Word16 wght_flg, /* i : is weighting function used */
Word16 *gain_flg, /* o : open-loop flag */
Flag dtx, /* i : dtx flag; use dtx=1, do not use dtx=0 */
Flag *pOverflow /* o : overflow flag */
)
{
Word16 i;
Word16 j;
Word16 *p;
Word16 *p1;
Word32 max;
Word32 t0;
Word16 t0_h;
Word16 t0_l;
Word16 p_max;
const Word16 *ww;
const Word16 *we;
Word32 t1;
Word16 temp;
ww = &corrweight[250];
we = &corrweight[123 + lag_max - old_lag];
max = MIN_32;
p_max = lag_max;
for (i = lag_max; i >= lag_min; i--)
{
t0 = corr[-i];
/* Weighting of the correlation function. */
L_Extract(corr[-i], &t0_h, &t0_l, pOverflow);
t0 = Mpy_32_16(t0_h, t0_l, *ww, pOverflow);
ww--;
if (wght_flg > 0)
{
/* Weight the neighbourhood of the old lag. */
L_Extract(t0, &t0_h, &t0_l, pOverflow);
t0 = Mpy_32_16(t0_h, t0_l, *we, pOverflow);
we--;
}
/* if (L_sub (t0, max) >= 0) */
if (t0 >= max)
{
max = t0;
p_max = i;
}
}
p = &scal_sig[0];
p1 = &scal_sig[-p_max];
t0 = 0;
t1 = 0;
for (j = 0; j < L_frame; j++, p++, p1++)
{
t0 = L_mac(t0, *p, *p1, pOverflow);
t1 = L_mac(t1, *p1, *p1, pOverflow);
}
if (dtx)
{ /* no test() call since this if is only in simulation env */
#ifdef VAD2
/* Save max correlation */
vadSt->L_Rmax = L_add(vadSt->L_Rmax, t0, pOverflow);
/* Save max energy */
vadSt->L_R0 = L_add(vadSt->L_R0, t1, pOverflow);
#else
/* update and detect tone */
vad_tone_detection_update(vadSt, 0, pOverflow);
vad_tone_detection(vadSt, t0, t1, pOverflow);
#endif
}
/* gain flag is set according to the open_loop gain */
/* is t2/t1 > 0.4 ? */
temp = pv_round(t1, pOverflow);
t1 = L_msu(t0, temp, 13107, pOverflow);
*gain_flg = pv_round(t1, pOverflow);
*cor_max = 0;
return (p_max);
}
static Word16 Lag_max ( /* o : lag found */
vadState *vadSt, /* i/o : VAD state struct */
Word32 corr[], /* i : correlation vector. */
Word16 scal_sig[], /* i : scaled signal. */
Word16 scal_fac, /* i : scaled signal factor. */
Word16 scal_flag, /* i : if 1 use EFR compatible scaling */
Word16 L_frame, /* i : length of frame to compute pitch */
Word16 lag_max, /* i : maximum lag */
Word16 lag_min, /* i : minimum lag */
Word16 *cor_max, /* o : normalized correlation of selected lag */
Flag dtx /* i : dtx flag; use dtx=1, do not use dtx=0 */
)
#endif
{
Word16 i, j;
Word16 *p;
Word32 max, t0;
Word16 max_h, max_l, ener_h, ener_l;
Word16 p_max = 0; /* initialization only needed to keep gcc silent */
max = MIN_32; move32 ();
p_max = lag_max; move16 ();
for (i = lag_max, j = (PIT_MAX-lag_max-1); i >= lag_min; i--, j--)
{
test ();
if (L_sub (corr[-i], max) >= 0)
{
max = corr[-i]; move32 ();
p_max = i; move16 ();
}
}
/* compute energy */
t0 = 0; move32 ();
p = &scal_sig[-p_max]; move16 ();
for (i = 0; i < L_frame; i++, p++)
{
t0 = L_mac (t0, *p, *p);
}
/* 1/sqrt(energy) */
if (dtx)
{ /* no test() call since this if is only in simulation env */
#ifdef VAD2
*rmax = max; move32();
*r0 = t0; move32();
#else
/* check tone */
vad_tone_detection (vadSt, max, t0);
#endif
}
t0 = Inv_sqrt (t0); move32 (); /* function result */
test();
if (scal_flag)
{
t0 = L_shl (t0, 1);
}
/* max = max/sqrt(energy) */
L_Extract (max, &max_h, &max_l);
L_Extract (t0, &ener_h, &ener_l);
t0 = Mpy_32 (max_h, max_l, ener_h, ener_l);
test();
if (scal_flag)
{
t0 = L_shr (t0, scal_fac);
*cor_max = extract_h (L_shl (t0, 15)); /* divide by 2 */
}
else
{
*cor_max = extract_l(t0);
}
return (p_max);
}