view libgsmefr/vad.c @ 282:9ee8ad3d4d30

frtest: rm gsmfr-hand-test and gsmfr-max-out utils These hack programs were never properly documented and were written only as part of a debug chase, in pursuit of a bug that ultimately turned out to be in our then-hacky patch to osmo-bts-sysmo, before beginning of proper patches in Osmocom. These hack programs need to be dropped from the present sw package because they depend on old libgsm, and we are eliminating that dependency.
author Mychaela Falconia <falcon@freecalypso.org>
date Sun, 14 Apr 2024 05:44:47 +0000
parents 1c108dd5b33f
children
line wrap: on
line source

/***************************************************************************
 *
 *   File Name: vad.c
 *
 *   Purpose:   Contains all functions for voice activity detection, as
 *              described in the high level specification of VAD.
 *
 *     Below is a listing of all the functions appearing in the file.
 *     The functions are arranged according to their purpose.  Under
 *     each heading, the ordering is hierarchical.
 *
 *     Resetting of static variables of VAD:
 *       reset_vad()
 *
 *     Main routine of VAD (called by the speech encoder):
 *       vad_computation()
 *       Adaptive filtering and energy computation:
 *         energy_computation()
 *       Averaging of autocorrelation function values:
 *         acf_averaging()
 *       Computation of predictor values:
 *         predictor_values()
 *           schur_recursion()
 *           step_up()
 *           compute_rav1()
 *       Spectral comparison:
 *         spectral_comparison()
 *       Information tone detection:
 *         tone_detection()
 *           step_up()
 *       Threshold adaptation:
 *         threshold_adaptation()
 *       VAD decision:
 *         vad_decision()
 *       VAD hangover addition:
 *         vad_hangover()
 *
 *     Periodicity detection routine (called by the speech encoder):
 *       periodicity_detection()
 *
 **************************************************************************/

#include "gsm_efr.h"
#include "typedef.h"
#include "namespace.h"
#include "cnst.h"
#include "basic_op.h"
#include "oper_32b.h"
#include "no_count.h"
#include "vad.h"
#include "enc_state.h"

/* Constants of VAD hangover addition */

#define HANGCONST 10
#define BURSTCONST 3

/* Constant of spectral comparison */

#define STAT_THRESH 3670L       /* 0.056 */

/* Constants of periodicity detection */

#define LTHRESH 2
#define NTHRESH 4

/* Pseudo floating point representations of constants
   for threshold adaptation */

#define M_PTH    32500          /*** 130000.0 ***/
#define E_PTH    17
#define M_PLEV   21667          /*** 346666.7 ***/
#define E_PLEV   19
#define M_MARGIN 16927          /*** 69333340 ***/
#define E_MARGIN 27

#define FAC 17203               /* 2.1 */

/* Constants of tone detection */

#define FREQTH 3189
#define PREDTH 1464

/* forward declarations for internal functions */

static void energy_computation (
    Word16 r_h[],
    Word16 scal_acf,
    Word16 rvad[],
    Word16 scal_rvad,
    Pfloat * acf0,
    Pfloat * pvad
);

static void acf_averaging (
    struct EFR_encoder_state *st,
    Word16 r_h[],
    Word16 r_l[],
    Word16 scal_acf,
    Word32 L_av0[],
    Word32 L_av1[]
);

static void predictor_values (
    Word32 L_av1[],
    Word16 rav1[],
    Word16 *scal_rav1
);

static void schur_recursion (
    Word32 L_av1[],
    Word16 vpar[]
);

static void step_up (
    Word16 np,
    Word16 vpar[],
    Word16 aav1[]
);

static void compute_rav1 (
    Word16 aav1[],
    Word16 rav1[],
    Word16 *scal_rav1
);

static Word16 spectral_comparison (
    struct EFR_encoder_state *st,
    Word16 rav1[],
    Word16 scal_rav1,
    Word32 L_av0[]
);

static void threshold_adaptation (
    struct EFR_encoder_state *st,
    Word16 stat,
    Word16 ptch,
    Word16 tone,
    Word16 rav1[],
    Word16 scal_rav1,
    Pfloat pvad,
    Pfloat acf0,
    Word16 rvad[],
    Word16 *scal_rvad,
    Pfloat * thvad
);

static void tone_detection (
    Word16 rc[],
    Word16 *tone
);

static Word16 vad_decision (
    Pfloat pvad,
    Pfloat thvad
);

static Word16 vad_hangover (
    struct EFR_encoder_state *st,
    Word16 vvad
);

/*************************************************************************
 *
 *   FUNCTION NAME: vad_reset
 *
 *   PURPOSE:  Resets the static variables of the VAD to their
 *             initial values
 *
 *************************************************************************/

void vad_reset (struct EFR_encoder_state *st)
{
    struct vad_state *vst = &st->vad;
    Word16 i;

    /* Initialize rvad variables */
    vst->rvad[0] = 0x6000;
    for (i = 1; i < 9; i++)
    {
        vst->rvad[i] = 0;
    }
    vst->scal_rvad = 7;

    /* Initialize threshold level */
    vst->thvad.e = 20;               /*** exponent ***/
    vst->thvad.m = 27083;            /*** mantissa ***/

    /* Initialize ACF averaging variables */
    for (i = 0; i < 27; i++)
    {
        vst->L_sacf[i] = 0L;
    }
    for (i = 0; i < 36; i++)
    {
        vst->L_sav0[i] = 0L;
    }
    vst->pt_sacf = 0;
    vst->pt_sav0 = 0;

    /* Initialize spectral comparison variable */
    vst->L_lastdm = 0L;

    /* Initialize threshold adaptation variable */
    vst->adaptcount = 0;

    /* Initialize VAD hangover addition variables */
    vst->burstcount = 0;
    vst->hangcount = -1;

    /* Initialize periodicity detection variables */
    vst->oldlagcount = 0;
    vst->veryoldlagcount = 0;
    vst->oldlag = 18;

    st->ptch = 1;

    return;
}

/****************************************************************************
 *
 *     FUNCTION:  vad_computation
 *
 *     PURPOSE:   Returns a decision as to whether the current frame being
 *                processed by the speech encoder contains speech or not.
 *
 *     INPUTS:    r_h[0..8]     autocorrelation of input signal frame (msb)
 *                r_l[0..8]     autocorrelation of input signal frame (lsb)
 *                scal_acf      scaling factor for the autocorrelations
 *                rc[0..3]      speech encoder reflection coefficients
 *                ptch          flag to indicate a periodic signal component
 *
 *     OUTPUTS:   none
 *
 *     RETURN VALUE: vad decision
 *
 ***************************************************************************/

Word16 vad_computation (
    struct EFR_encoder_state *st,
    Word16 r_h[],
    Word16 r_l[],
    Word16 scal_acf,
    Word16 rc[],
    Word16 ptch
)
{
    struct vad_state *vst = &st->vad;
    Word32 L_av0[9], L_av1[9];
    Word16 vad, vvad, rav1[9], scal_rav1, stat, tone;
    Pfloat acf0, pvad;

    energy_computation (r_h, scal_acf, vst->rvad, vst->scal_rvad, &acf0, &pvad);
    acf_averaging (st, r_h, r_l, scal_acf, L_av0, L_av1);
    predictor_values (L_av1, rav1, &scal_rav1);
    stat = spectral_comparison (st, rav1, scal_rav1, L_av0);
    tone_detection (rc, &tone);
    threshold_adaptation (st, stat, ptch, tone, rav1, scal_rav1, pvad, acf0,
                          vst->rvad, &vst->scal_rvad, &vst->thvad);
    vvad = vad_decision (pvad, vst->thvad);
    vad = vad_hangover (st, vvad);

    return vad;
}

/****************************************************************************
 *
 *     FUNCTION:  energy_computation
 *
 *     PURPOSE:   Computes the input and residual energies of the adaptive
 *                filter in a floating point representation.
 *
 *     INPUTS:    r_h[0..8]      autocorrelation of input signal frame (msb)
 *                scal_acf       scaling factor for the autocorrelations
 *                rvad[0..8]     autocorrelated adaptive filter coefficients
 *                scal_rvad      scaling factor for rvad[]
 *
 *     OUTPUTS:   *acf0          signal frame energy (mantissa+exponent)
 *                *pvad          filtered signal energy (mantissa+exponent)
 *
 *     RETURN VALUE: none
 *
 ***************************************************************************/

void energy_computation (
    Word16 r_h[],
    Word16 scal_acf,
    Word16 rvad[],
    Word16 scal_rvad,
    Pfloat * acf0,
    Pfloat * pvad
)
{
    Word16 i, temp, norm_prod;
    Word32 L_temp;

    /* r[0] is always greater than zero (no need to test for r[0] == 0) */

    /* Computation of acf0 (exponent and mantissa) */

    acf0->e = sub (32, scal_acf);       move16 (); 
    acf0->m = r_h[0] & 0x7ff8;          move16 (); logic16 (); 

    /* Computation of pvad (exponent and mantissa) */

    pvad->e = add (acf0->e, 14);        move16 (); 
    pvad->e = sub (pvad->e, scal_rvad); move16 (); 

    L_temp = 0L;                        move32 (); 

    for (i = 1; i <= 8; i++)
    {
        temp = shr (r_h[i], 3);
        L_temp = L_mac (L_temp, temp, rvad[i]);
    }

    temp = shr (r_h[0], 3);
    L_temp = L_add (L_temp, L_shr (L_mult (temp, rvad[0]), 1));

    test (); 
    if (L_temp <= 0L)
    {
        L_temp = 1L;                    move32 (); 
    }
    norm_prod = norm_l (L_temp);
    pvad->e = sub (pvad->e, norm_prod); move16 (); 
    pvad->m = extract_h (L_shl (L_temp, norm_prod));
                                        move16 (); 

    return;
}

/****************************************************************************
 *
 *     FUNCTION:  acf_averaging
 *
 *     PURPOSE:   Computes the arrays L_av0[0..8] and L_av1[0..8].
 *
 *     INPUTS:    r_h[0..8]     autocorrelation of input signal frame (msb)
 *                r_l[0..8]     autocorrelation of input signal frame (lsb)
 *                scal_acf      scaling factor for the autocorrelations
 *
 *     OUTPUTS:   L_av0[0..8]   ACF averaged over last four frames
 *                L_av1[0..8]   ACF averaged over previous four frames
 *
 *     RETURN VALUE: none
 *
 ***************************************************************************/

void acf_averaging (
    struct EFR_encoder_state *st,
    Word16 r_h[],
    Word16 r_l[],
    Word16 scal_acf,
    Word32 L_av0[],
    Word32 L_av1[]
)
{
    struct vad_state *vst = &st->vad;
    Word32 L_temp;
    Word16 scale;
    Word16 i;

    scale = add (9, scal_acf);

    for (i = 0; i <= 8; i++)
    {
        L_temp = L_shr (L_Comp (r_h[i], r_l[i]), scale);
        L_av0[i] = L_add (vst->L_sacf[i], L_temp);
        L_av0[i] = L_add (vst->L_sacf[i + 9], L_av0[i]);
        L_av0[i] = L_add (vst->L_sacf[i + 18], L_av0[i]);
        vst->L_sacf[vst->pt_sacf + i] = L_temp;
        L_av1[i] = vst->L_sav0[vst->pt_sav0 + i];
        vst->L_sav0[vst->pt_sav0 + i] = L_av0[i];
    }

    /* Update the array pointers */

    if (vst->pt_sacf == 18)
    {
        vst->pt_sacf = 0;
    }
    else
    {
        vst->pt_sacf += 9;
    }

    if (vst->pt_sav0 == 27)
    {
        vst->pt_sav0 = 0;
    }
    else
    {
        vst->pt_sav0 += 9;
    }

    return;
}

/****************************************************************************
 *
 *     FUNCTION:  predictor_values
 *
 *     PURPOSE:   Computes the array rav[0..8] needed for the spectral
 *                comparison and the threshold adaptation.
 *
 *     INPUTS:    L_av1[0..8]   ACF averaged over previous four frames
 *
 *     OUTPUTS:   rav1[0..8]    ACF obtained from L_av1
 *                *scal_rav1    rav1[] scaling factor
 *
 *     RETURN VALUE: none
 *
 ***************************************************************************/

void predictor_values (
    Word32 L_av1[],
    Word16 rav1[],
    Word16 *scal_rav1
)
{
    Word16 vpar[8], aav1[9];

    schur_recursion (L_av1, vpar);
    step_up (8, vpar, aav1);
    compute_rav1 (aav1, rav1, scal_rav1);

    return;
}

/****************************************************************************
 *
 *     FUNCTION:  schur_recursion
 *
 *     PURPOSE:   Uses the Schur recursion to compute adaptive filter
 *                reflection coefficients from an autorrelation function.
 *
 *     INPUTS:    L_av1[0..8]    autocorrelation function
 *
 *     OUTPUTS:   vpar[0..7]     reflection coefficients
 *
 *     RETURN VALUE: none
 *
 ***************************************************************************/

void schur_recursion (
    Word32 L_av1[],
    Word16 vpar[]
)
{
    Word16 acf[9], pp[9], kk[9], temp;
    Word16 i, k, m, n;

    /*** Schur recursion with 16-bit arithmetic ***/

    test (); move32 (); 
    if (L_av1[0] == 0)
    {
        for (i = 0; i < 8; i++)
        {
            vpar[i] = 0;                                move16 (); 
        }
        return;
    }
    temp = norm_l (L_av1[0]);

    for (k = 0; k <= 8; k++)
    {
        acf[k] = extract_h (L_shl (L_av1[k], temp));    move16 (); 
    }

    /*** Initialize arrays pp[..] and kk[..] for the recursion: ***/

    for (i = 1; i <= 7; i++)
    {
        kk[9 - i] = acf[i];                             move16 (); 
    }

    for (i = 0; i <= 8; i++)
    {
        pp[i] = acf[i];                                 move16 (); 
    }

    /*** Compute Parcor coefficients: ***/

    for (n = 0; n < 8; n++)
    {
        test (); 
        if ((pp[0] == 0) ||
            (pp[0] < abs_s (pp[1])))
        {
            for (i = n; i < 8; i++)
            {
                vpar[i] = 0;                            move16 (); 
            }
            return;
        }
        vpar[n] = div_s (abs_s (pp[1]), pp[0]);         move16 (); 

        test ();                                        move16 (); 
        if (pp[1] > 0)
        {
            vpar[n] = negate (vpar[n]);                 move16 (); 
        }
        test (); 
        if (n == 7)
        {
            return;
        }
        /*** Schur recursion: ***/

        pp[0] = add (pp[0], mult_r (pp[1], vpar[n]));   move16 (); 

        for (m = 1; m <= 7 - n; m++)
        {
            pp[m] = add (pp[1 + m], mult_r (kk[9 - m], vpar[n]));
                                                        move16 (); 
            kk[9 - m] = add (kk[9 - m], mult_r (pp[1 + m], vpar[n]));
                                                        move16 (); 
        }
    }

    return;
}

/****************************************************************************
 *
 *     FUNCTION:  step_up
 *
 *     PURPOSE:   Computes the transversal filter coefficients from the
 *                reflection coefficients.
 *
 *     INPUTS:    np               filter order (2..8)
 *                vpar[0..np-1]    reflection coefficients
 *
 *     OUTPUTS:   aav1[0..np]      transversal filter coefficients
 *
 *     RETURN VALUE: none
 *
 ***************************************************************************/

void step_up (
    Word16 np,
    Word16 vpar[],
    Word16 aav1[]
)
{
    Word32 L_coef[9], L_work[9];
    Word16 temp;
    Word16 i, m;

    /*** Initialization of the step-up recursion ***/

    L_coef[0] = 0x20000000L;    move32 (); 
    L_coef[1] = L_shl (L_deposit_l (vpar[0]), 14);              move32 (); 

    /*** Loop on the LPC analysis order: ***/

    for (m = 2; m <= np; m++)
    {
        for (i = 1; i < m; i++)
        {
            temp = extract_h (L_coef[m - i]);
            L_work[i] = L_mac (L_coef[i], vpar[m - 1], temp);   move32 (); 
        }

        for (i = 1; i < m; i++)
        {
            L_coef[i] = L_work[i];                              move32 (); 
        }

        L_coef[m] = L_shl (L_deposit_l (vpar[m - 1]), 14);      move32 (); 
    }

    /*** Keep the aav1[0..np] in 15 bits ***/

    for (i = 0; i <= np; i++)
    {
        aav1[i] = extract_h (L_shr (L_coef[i], 3));             move32 (); 
    }

    return;
}

/****************************************************************************
 *
 *     FUNCTION:  compute_rav1
 *
 *     PURPOSE:   Computes the autocorrelation function of the adaptive
 *                filter coefficients.
 *
 *     INPUTS:    aav1[0..8]     adaptive filter coefficients
 *
 *     OUTPUTS:   rav1[0..8]     ACF of aav1
 *                *scal_rav1     rav1[] scaling factor
 *
 *     RETURN VALUE: none
 *
 ***************************************************************************/

void compute_rav1 (
    Word16 aav1[],
    Word16 rav1[],
    Word16 *scal_rav1
)
{
    Word32 L_work[9];
    Word16 i, k;

    /*** Computation of the rav1[0..8] ***/

    for (i = 0; i <= 8; i++)
    {
        L_work[i] = 0L;                                           move32 (); 

        for (k = 0; k <= 8 - i; k++)
        {
            L_work[i] = L_mac (L_work[i], aav1[k], aav1[k + i]);  move32 (); 
        }
    }

    test (); move32 (); 
    if (L_work[0] == 0L)
    {
        *scal_rav1 = 0;                                           move16 (); 
    }
    else
    {
        *scal_rav1 = norm_l (L_work[0]);
    }

    for (i = 0; i <= 8; i++)
    {
        rav1[i] = extract_h (L_shl (L_work[i], *scal_rav1));      move16 (); 
    }

    return;
}

/****************************************************************************
 *
 *     FUNCTION:  spectral_comparison
 *
 *     PURPOSE:   Computes the stat flag needed for the threshold
 *                adaptation decision.
 *
 *     INPUTS:    rav1[0..8]      ACF obtained from L_av1
 *                *scal_rav1      rav1[] scaling factor
 *                L_av0[0..8]     ACF averaged over last four frames
 *
 *     OUTPUTS:   none
 *
 *     RETURN VALUE: flag to indicate spectral stationarity
 *
 ***************************************************************************/

Word16 spectral_comparison (
    struct EFR_encoder_state *st,
    Word16 rav1[],
    Word16 scal_rav1,
    Word32 L_av0[]
)
{
    struct vad_state *vst = &st->vad;
    Word32 L_dm, L_sump, L_temp;
    Word16 stat, sav0[9], shift, divshift, temp;
    Word16 i;

    /*** Re-normalize L_av0[0..8] ***/

    test (); move32 (); 
    if (L_av0[0] == 0L)
    {
        for (i = 0; i <= 8; i++)
        {
            sav0[i] = 0x0fff;   /* 4095 */                      move16 (); 
        }
    }
    else
    {
        shift = sub (norm_l (L_av0[0]), 3);
        for (i = 0; i <= 8; i++)
        {
            sav0[i] = extract_h (L_shl (L_av0[i], shift));      move16 (); 
        }
    }

    /*** Compute partial sum of dm ***/

    L_sump = 0L;                                                move32 (); 
    for (i = 1; i <= 8; i++)
    {
        L_sump = L_mac (L_sump, rav1[i], sav0[i]);
    }

    /*** Compute the division of the partial sum by sav0[0] ***/

    test (); 
    if (L_sump < 0L)
    {
        L_temp = L_negate (L_sump);
    }
    else
    {
        L_temp = L_sump;                                        move32 (); 
    }

    test (); 
    if (L_temp == 0L)
    {
        L_dm = 0L;                                              move32 (); 
        shift = 0;                                              move16 (); 
    }
    else
    {
        sav0[0] = shl (sav0[0], 3);                             move16 (); 
        shift = norm_l (L_temp);
        temp = extract_h (L_shl (L_temp, shift));

        test (); 
        if (sav0[0] >= temp)
        {
            divshift = 0;                                       move16 (); 
            temp = div_s (temp, sav0[0]);
        }
        else
        {
            divshift = 1;                                       move16 (); 
            temp = sub (temp, sav0[0]);
            temp = div_s (temp, sav0[0]);
        }

        test (); 
        if (divshift == 1)
        {
            L_dm = 0x8000L;                                     move32 (); 
        }
        else
        {
            L_dm = 0L;                                          move32 (); 
        }

        L_dm = L_shl (L_add (L_dm, L_deposit_l (temp)), 1);

        test (); 
        if (L_sump < 0L)
        {
            L_dm = L_negate (L_dm);
        }
    }

    /*** Re-normalization and final computation of L_dm ***/

    L_dm = L_shl (L_dm, 14);
    L_dm = L_shr (L_dm, shift);
    L_dm = L_add (L_dm, L_shl (L_deposit_l (rav1[0]), 11));
    L_dm = L_shr (L_dm, scal_rav1);

    /*** Compute the difference and save L_dm ***/

    L_temp = L_sub (L_dm, vst->L_lastdm);
    vst->L_lastdm = L_dm;

    test (); 
    if (L_temp < 0L)
    {
        L_temp = L_negate (L_temp);
    }
    /*** Evaluation of the stat flag ***/

    L_temp = L_sub (L_temp, STAT_THRESH);

    test (); 
    if (L_temp < 0L)
    {
        stat = 1;                                               move16 (); 
    }
    else
    {
        stat = 0;                                               move16 (); 
    }

    return stat;
}

/****************************************************************************
 *
 *     FUNCTION:  threshold_adaptation
 *
 *     PURPOSE:   Evaluates the secondary VAD decision.  If speech is not
 *                present then the noise model rvad and adaptive threshold
 *                thvad are updated.
 *
 *     INPUTS:    stat          flag to indicate spectral stationarity
 *                ptch          flag to indicate a periodic signal component
 *                tone          flag to indicate a tone signal component
 *                rav1[0..8]    ACF obtained from L_av1
 *                scal_rav1     rav1[] scaling factor
 *                pvad          filtered signal energy (mantissa+exponent)
 *                acf0          signal frame energy (mantissa+exponent)
 *
 *     OUTPUTS:   rvad[0..8]    autocorrelated adaptive filter coefficients
 *                *scal_rvad    rvad[] scaling factor
 *                *thvad        decision threshold (mantissa+exponent)
 *
 *     RETURN VALUE: none
 *
 ***************************************************************************/

void threshold_adaptation (
    struct EFR_encoder_state *st,
    Word16 stat,
    Word16 ptch,
    Word16 tone,
    Word16 rav1[],
    Word16 scal_rav1,
    Pfloat pvad,
    Pfloat acf0,
    Word16 rvad[],
    Word16 *scal_rvad,
    Pfloat * thvad
)
{
    struct vad_state *vst = &st->vad;
    Word16 comp, comp2;
    Word32 L_temp;
    Word16 temp;
    Pfloat p_temp;
    Word16 i;

    comp = 0;                                           move16 (); 

    /*** Test if acf0 < pth; if yes set thvad to plev ***/

    test (); 
    if (acf0.e < E_PTH)
    {
        comp = 1;                                       move16 (); 
    }
    test (); test (); 
    if ((acf0.e == E_PTH) && (acf0.m < M_PTH))
    {
        comp = 1;                                       move16 (); 
    }
    test (); 
    if (comp == 1)
    {
        thvad->e = E_PLEV;                              move16 (); 
        thvad->m = M_PLEV;                              move16 (); 

        return;
    }
    /*** Test if an adaption is required ***/

    test (); 
    if (ptch == 1)
    {
        comp = 1;                                       move16 (); 
    }
    test (); 
    if (stat == 0)
    {
        comp = 1;                                       move16 (); 
    }
    test (); 
    if (tone == 1)
    {
        comp = 1;                                       move16 (); 
    }
    test (); 
    if (comp == 1)
    {
        vst->adaptcount = 0;
        return;
    }
    /*** Increment adaptcount ***/

    vst->adaptcount = add (vst->adaptcount, 1);
    if (vst->adaptcount <= 8)
    {
        return;
    }
    /*** computation of thvad-(thvad/dec) ***/

    thvad->m = sub (thvad->m, shr (thvad->m, 5));       move16 (); 

    test (); 
    if (thvad->m < 0x4000)
    {
        thvad->m = shl (thvad->m, 1);                   move16 (); 
        thvad->e = sub (thvad->e, 1);                   move16 (); 
    }
    /*** computation of pvad*fac ***/

    L_temp = L_mult (pvad.m, FAC);
    L_temp = L_shr (L_temp, 15);
    p_temp.e = add (pvad.e, 1);                         move16 (); 

    test (); 
    if (L_temp > 0x7fffL)
    {
        L_temp = L_shr (L_temp, 1);
        p_temp.e = add (p_temp.e, 1);                   move16 (); 
    }
    p_temp.m = extract_l (L_temp);                      move16 (); 

    /*** test if thvad < pvad*fac ***/

    test (); 
    if (thvad->e < p_temp.e)
    {
        comp = 1;                                       move16 (); 
    }
    test (); test (); 
    if ((thvad->e == p_temp.e) &&
        (thvad->m < p_temp.m))
    {
        comp = 1;                                       move16 (); 
    }
    /*** compute minimum(thvad+(thvad/inc), pvad*fac) when comp = 1 ***/

    test (); 
    if (comp == 1)
    {
        /*** compute thvad + (thvad/inc) ***/

        L_temp = L_add (L_deposit_l (thvad->m),
                        L_deposit_l (shr (thvad->m, 4)));

        test (); 
        if (L_temp > 0x7fffL)
        {
            thvad->m = extract_l (L_shr (L_temp, 1));   move16 (); 
            thvad->e = add (thvad->e, 1);               move16 (); 
        }
        else
        {
            thvad->m = extract_l (L_temp);              move16 (); 
        }

        comp2 = 0;                                      move16 (); 

        test (); 
        if (p_temp.e < thvad->e)
        {
            comp2 = 1;                                  move16 (); 
        }
        test (); test (); 
        if ((p_temp.e == thvad->e) &&
            (p_temp.m < thvad->m))
        {
            comp2 = 1;                                  move16 (); 
        }
        test (); 
        if (comp2 == 1)
        {
            thvad->e = p_temp.e;move16 (); 
            thvad->m = p_temp.m;move16 (); 
        }
    }
    /*** compute pvad + margin ***/

    test (); 
    if (pvad.e == E_MARGIN)
    {
        L_temp = L_add (L_deposit_l (pvad.m), L_deposit_l (M_MARGIN));
        p_temp.m = extract_l (L_shr (L_temp, 1));       move16 (); 
        p_temp.e = add (pvad.e, 1);     move16 (); 
    }
    else
    {
        test (); 
        if (pvad.e > E_MARGIN)
        {
            temp = sub (pvad.e, E_MARGIN);
            temp = shr (M_MARGIN, temp);
            L_temp = L_add (L_deposit_l (pvad.m), L_deposit_l (temp));

            test (); 
            if (L_temp > 0x7fffL)
            {
                p_temp.e = add (pvad.e, 1);             move16 (); 
                p_temp.m = extract_l (L_shr (L_temp, 1));
                                                        move16 (); 
            }
            else
            {
                p_temp.e = pvad.e;                      move16 (); 
                p_temp.m = extract_l (L_temp);          move16 (); 
            }
        }
        else
        {
            temp = sub (E_MARGIN, pvad.e);
            temp = shr (pvad.m, temp);
            L_temp = L_add (L_deposit_l (M_MARGIN), L_deposit_l (temp));

            test (); 
            if (L_temp > 0x7fffL)
            {
                p_temp.e = add (E_MARGIN, 1);           move16 (); 
                p_temp.m = extract_l (L_shr (L_temp, 1));
                                                        move16 (); 
            }
            else
            {
                p_temp.e = E_MARGIN;                    move16 (); 
                p_temp.m = extract_l (L_temp);          move16 (); 
            }
        }
    }

    /*** Test if thvad > pvad + margin ***/

    comp = 0;                                           move16 (); 

    test (); 
    if (thvad->e > p_temp.e)
    {
        comp = 1;                                       move16 (); 
    }
    test (); test (); 
    if ((thvad->e == p_temp.e) &&
        (thvad->m > p_temp.m))
    {
        comp = 1;                                       move16 (); 
    }
    test (); 
    if (comp == 1)
    {
        thvad->e = p_temp.e;                            move16 (); 
        thvad->m = p_temp.m;                            move16 (); 
    }
    /*** Normalise and retain rvad[0..8] in memory ***/

    *scal_rvad = scal_rav1;                             move16 (); 

    for (i = 0; i <= 8; i++)
    {
        rvad[i] = rav1[i];                              move16 (); 
    }

    /*** Set adaptcount to adp + 1 ***/

    vst->adaptcount = 9;

    return;
}

/****************************************************************************
 *
 *     FUNCTION:  tone_detection
 *
 *     PURPOSE:   Computes the tone flag needed for the threshold
 *                adaptation decision.
 *
 *     INPUTS:    rc[0..3]    reflection coefficients calculated in the
 *                            speech encoder short term predictor
 *
 *     OUTPUTS:   *tone       flag to indicate a periodic signal component
 *
 *     RETURN VALUE: none
 *
 ***************************************************************************/

void tone_detection (
    Word16 rc[],
    Word16 *tone
)
{
    Word32 L_num, L_den, L_temp;
    Word16 temp, prederr, a[3];
    Word16 i;

    *tone = 0;                  move16 (); 

    /*** Calculate filter coefficients ***/

    step_up (2, rc, a);

    /*** Calculate ( a[1] * a[1] ) ***/

    temp = shl (a[1], 3);
    L_den = L_mult (temp, temp);

    /*** Calculate ( 4*a[2] - a[1]*a[1] ) ***/

    L_temp = L_shl (L_deposit_h (a[2]), 3);
    L_num = L_sub (L_temp, L_den);

    /*** Check if pole frequency is less than 385 Hz ***/

    test (); 
    if (L_num <= 0)
    {
        return;
    }
    test (); move16 (); 
    if (a[1] < 0)
    {
        temp = extract_h (L_den);
        L_den = L_mult (temp, FREQTH);

        L_temp = L_sub (L_num, L_den);

        test (); 
        if (L_temp < 0)
        {
            return;
        }
    }
    /*** Calculate normalised prediction error ***/

    prederr = 0x7fff;           move16 (); 

    for (i = 0; i < 4; i++)
    {
        temp = mult (rc[i], rc[i]);
        temp = sub (0x7fff, temp);
        prederr = mult (prederr, temp);
    }

    /*** Test if prediction error is smaller than threshold ***/

    temp = sub (prederr, PREDTH);

    test (); 
    if (temp < 0)
    {
        *tone = 1;              move16 (); 
    }
    return;
}

/****************************************************************************
 *
 *     FUNCTION:  vad_decision
 *
 *     PURPOSE:   Computes the VAD decision based on the comparison of the
 *                floating point representations of pvad and thvad.
 *
 *     INPUTS:    pvad          filtered signal energy (mantissa+exponent)
 *                thvad         decision threshold (mantissa+exponent)
 *
 *     OUTPUTS:   none
 *
 *     RETURN VALUE: vad decision before hangover is added
 *
 ***************************************************************************/

Word16 vad_decision (
    Pfloat pvad,
    Pfloat thvad
)
{
    Word16 vvad;

    test (); test (); test (); 
    if (pvad.e > thvad.e)
    {
        vvad = 1;               move16 (); 
    }
    else if ((pvad.e == thvad.e) &&
             (pvad.m > thvad.m))
    {
        vvad = 1;               move16 (); 
    }
    else
    {
        vvad = 0;               move16 (); 
    }

    return vvad;
}

/****************************************************************************
 *
 *     FUNCTION:  vad_hangover
 *
 *     PURPOSE:   Computes the final VAD decision for the current frame
 *                being processed.
 *
 *     INPUTS:    vvad           vad decision before hangover is added
 *
 *     OUTPUTS:   none
 *
 *     RETURN VALUE: vad decision after hangover is added
 *
 ***************************************************************************/

Word16 vad_hangover (
    struct EFR_encoder_state *st,
    Word16 vvad
)
{
    struct vad_state *vst = &st->vad;

    if (vvad == 1)
    {
        vst->burstcount++;
    }
    else
    {
        vst->burstcount = 0;
    }

    if (vst->burstcount >= BURSTCONST)
    {
        vst->hangcount = HANGCONST;
        vst->burstcount = BURSTCONST;
    }
    if (vst->hangcount >= 0)
    {
        vst->hangcount--;
        return 1;               /* vad = 1 */
    }
    return vvad;                /* vad = vvad */
}

/****************************************************************************
 *
 *     FUNCTION:  periodicity_update
 *
 *     PURPOSE:   Computes the ptch flag needed for the threshold
 *                adaptation decision for the next frame.
 *
 *     INPUTS:    lags[0..1]       speech encoder long term predictor lags
 *
 *     OUTPUTS:   *ptch            Boolean voiced / unvoiced decision
 *
 *     RETURN VALUE: none
 *
 ***************************************************************************/

void periodicity_update (
    struct EFR_encoder_state *st,
    Word16 lags[]
)
{
    struct vad_state *vst = &st->vad;
    Word16 minlag, maxlag, lagcount, temp;
    Word16 i;

    /*** Run loop for the two halves in the frame ***/

    lagcount = 0;

    for (i = 0; i <= 1; i++)
    {
        /*** Search the maximum and minimum of consecutive lags ***/

        if (vst->oldlag > lags[i])
        {
            minlag = lags[i];
            maxlag = vst->oldlag;
        }
        else
        {
            minlag = vst->oldlag;
            maxlag = lags[i];
        }

        temp = sub (maxlag, minlag);

        if (temp < LTHRESH)
        {
            lagcount = add (lagcount, 1);
        }
        /*** Save the current LTP lag ***/

        vst->oldlag = lags[i];
    }

    /*** Update the veryoldlagcount and oldlagcount ***/

    vst->veryoldlagcount = vst->oldlagcount;
    vst->oldlagcount = lagcount;

    /*** Make ptch decision ready for next frame ***/

    temp = add (vst->oldlagcount, vst->veryoldlagcount);

    if (temp >= NTHRESH)
    {
        st->ptch = 1;
    }
    else
    {
        st->ptch = 0;
    }

    return;
}