calc_cor_eng_stoch.c

手机加密通话软件

/* Copyright 2001,2002,2003 NAH6
 * All Rights Reserved
 *
 * Parts Copyright DoD, Parts Copyright Starium
 *
 */
#include "calc_cor_eng_stoch.h"

/**************************************************************************
*
* ROUTINE
*               CalcCorEngStoch
*
* FUNCTION
*               Calculate correlation and energy
*
* SYNOPSIS
*               CalcCorEngStoch(Residual, Conv, ExcVal1, ExcVal2, First, Cor, Energy)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       Residual        fxpt_16  i      Spectrum and adaptive residual
*       Conv            fxpt_16  i      Convolution sum
*       ExcVal1         fxpt_16  i      First excitation vector value
*       ExcVal2         fxpt_16  i      Second excitation vector value
*       First           int      i      First subframe flag
*       Cor             fxpt_32  o      Correlation
*       Energy          fxpt_32  o      Energy
*
***************************************************************************
*
*       Calculate correlation and energy:
*       e0 = spectrum & pitch prediction residual
*       y  = error weighting filtered code words
*
*       \/\/\/  CELP's computations are focused in this correlation \/\/\/
*               - For a 512 code book this correlation takes 4 MIPS!
*               - Decimation?, Down-sample & decimate?, FEC codes?
*
***************************************************************************/

void CalcCorEngStoch(
fxpt_16 Residual[RES_LEN],              /* 15.0 format */
fxpt_16 Conv[],                         /* 15.0 format */
fxpt_16 ExcVal1,                        /* 15.0 format */
fxpt_16 ExcVal2,                        /* 15.0 format */
int     First,
fxpt_32 *Cor,                           /* 30.1 format */
fxpt_32 *Energy)                        /* 30.1 format */
{
        int i;
        static fxpt_16 NextLastConv=0;
        static fxpt_16 LastConv=0;
        int64 acc;
	fxpt_16 *pc, *pr;


FXPT_PUSHSTATE("CalcCorEngStoch", -1.0, -1.0);
        /*  Initialize */
        acc = 0;

        /*  Calculate correlation */
/*      for (i=0; i<SF_LEN; i++) {
                acc += Conv[i] * Residual[i];
        }
*/
		for (i=SF_LEN, pc= Conv, pr=Residual ; i ; --i, ++pc, ++pr) {
			acc += *pc * *pr;
		}

        *Cor = fxpt_saturate32( acc );

        /*  End correct energy on subsequent code words */
        if ((ExcVal1 == 0) && (ExcVal2 == 0) && (!First))
		{
                acc = NextLastConv * NextLastConv;
                acc += LastConv * LastConv;
//                *Energy -= fxpt_saturate32( acc );
                *Energy = fxpt_sub32( *Energy, fxpt_saturate32( acc ) );
        }
        else {
                acc = 0;
/*
                for (i=0; i<SF_LEN; i++) {
                        acc += Conv[i] * Conv[i];
                }
*/
				for (i=SF_LEN, pc= Conv; i; --i, ++pc) {
					acc += *pc * *pc;
				}

                *Energy = fxpt_saturate32( acc );
        }
        NextLastConv = Conv[SF_LEN-2];
        LastConv = Conv[SF_LEN-1];
FXPT_POPSTATE();
}