conv_cor.c

手机加密通话软件

/* Copyright 2001,2002,2003 NAH6
 * All Rights Reserved
 *
 * Parts Copyright DoD, Parts Copyright Starium
 *
 */
          /*LINTLIBRARY*/
          /*PROTOLIB1*/

#include <math.h>
#include <stdlib.h>
#include "main.h"
#include "conv_cor.h"

#include "rint.h"

static void CalcStochConv(
float	ExcVec[],
int	ExVecLen,
float	LPImpResp[],
int	LenTruncH,
float	Conv[MAX_CW_VEC_LEN]);

static void EndCorrectStoch(
float	ExcVal,
float	LPImpResp[],
int	LenTruncH,
float	Conv[]);

static void CalcCorEngStoch(
float	Residual[RES_LEN],
float	Conv[],
float	ExcVal1,
float	ExcVal2,
int	First,
float	*Cor,
float 	*Energy);

static void CompGainErrStoch(
float	Energy,
float	Cor,
float	*Gain,
float	*Error);

/**************************************************************************
*
* ROUTINE
*		ConvCor
*
* FUNCTION
*		Find codeword gain and error (TERNARY CODE BOOK ASSUMED!)
*
* SYNOPSIS
*               ConvCor(ExcVec, ExVecLen, First, LPImpResp, LenTruncH, NegErr)
*
*   formal 
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       ExcVec		float	 i	excitation vector (ternary codeword)
*       ExcVecLen	int	 i	size of ex (dimension of codeword)
*       First		int	 i	first call flag
*	LPImpResp	float	 i	LPC Impulse Response
*	LenTruncH	int	 i	Length to truncate impulse response
*       NegErr		float	 o	negative partial squared error
*
*       ConvCor		float	fun	optimal gain for ex
*
*
*==========================================================================
*	
* DESCRIPTION
*
* (The calculations below may be valid for version 3.2, but may not be 
*  correct for version 3.3).
*
*	For each code word find its gain and error:
*	   a.  Filter code words through impulse response
*	       of perceptual weighting filter (LPC filter with
*	       bandwidth broadening).
*	   b.  Correlate filtered result with actual second error
*	       signal (e0).
*	   c.  Compute MSPE gain and error for code book vector.
*
*	Notes:  Codewords may contain many zeros (i.e., ex(1)=0).  The
*		code book could be accessed by a pointer to nonzero samples.
*		Because the code book is static, it`s silly to test its
*		samples as in the code below.
*
*		Proper selection of the convolution length (len) depends on
*		the perceptual weighting filter's expansion factor (gamma)
*		which controls the damping of the impulse response.
*
*		This is one of CELP's most computationally intensive
*		routines.  Neglecting overhead, the approximate number of
*		DSP instructions (add, multiply, multiply accumulate, or
*		compare) per second (IPS) is:
*
*	               Code book size   MIPS
*	               --------------   ----
*	                   64           1.1
*	                  128           2.1
*	                  256           4.2
*	                  512 (max)     8.3
*
*	        C:  convolution (recursive truncated end-point correction)
*	        R:  correlation
*	        E:  energy (recursive end-point correction)
*	        G:  gain quantization
*
*	celp code book search complexity (doesn't fully exploit ternary values!):
*	implicit undefined(a-z)
c
c	DSP chip instructions/operation:
*	integer MUL, ADD, SUB, MAC, MAD, CMP
*	parameter (MUL=1)	!multiply
*	parameter (ADD=1)	!add
*	parameter (SUB=1)	!subtract
*	parameter (MAC=1)	!multiply & accumulate
*	parameter (MAD=2)	!multiply & add
*	parameter (CMP=1)	!compare
c
c	CELP algorithm parameters:
*	integer L, len, K, shift, g_bits
*	real p, F
*	parameter (L=60)	!subframe length
*	parameter (len=30)	!length to truncate calculations (<= L)
*	parameter (p=0.77)	!code book sparsity
*	parameter (shift=2)	!shift between code words
*	parameter (K=4)		!number of subframes/frame
*	parameter (F=30.e-3)	!time (seconds)/frame
*	parameter (g_bits=5)	!cbgain bit allocation
c
*	integer j
*	parameter (j=10)
*	integer N(j), i
*	real C, R, E, G, IPS
*	data N/1, 2, 4, 8, 16, 32, 64, 128, 256, 512/
*	print 1
*1	format(10x,'N',10x,'C',13x,'R',12x,'E',15x,'G',13x,'MIPS')
*	do i = 1, j
*	   C = (335)*MAD + (N(i)-1)*shift*(1.-p)*len*ADD
*	   R = N(i)*L*MAC
*	   E = L*MAC + (N(i)-1)*((1.-p*p)*L*MAC + (p*p)*2*MAD)
*	   G = N(i)*(g_bits*(CMP+MUL+ADD) + 3*MUL+1*SUB)
*	   IPS = (C+R+E+G)*K/F
*	   print *,N(i),C*K/1.e6/F,R*K/1.e6/F,E*K/1.e6/F,G*K/1.e6/F,IPS/1.e6
*	end do
*	end
*
*    N        C             R             E               G           MIPS
*    1  8.9333333E-02  8.0000004E-03  8.0000004E-03  2.5333334E-03  0.1078667   
*    2  9.1173328E-02  1.6000001E-02  1.1573013E-02  5.0666668E-03  0.1238130   
*    4  9.4853334E-02  3.2000002E-02  1.8719040E-02  1.0133334E-02  0.1557057   
*    8  0.1022133      6.4000003E-02  3.3011094E-02  2.0266667E-02  0.2194911   
*   16  0.1169333      0.1280000      6.1595201E-02  4.0533334E-02  0.3470618   
*   32  0.1463733      0.2560000      0.1187634      8.1066668E-02  0.6022034   
*   64  0.2052534      0.5120000      0.2330998      0.1621333       1.112487   
*  128  0.3230133       1.024000      0.4617727      0.3242667       2.133053   
*  256  0.5585334       2.048000      0.9191184      0.6485333       4.174185   
*  512   1.029573       4.096000       1.833810       1.297067       8.256450  
*
*==========================================================================
*
* CALLED BY
*
*	cbsearch
*
* CALLS
*
*	gainencode2
*
*==========================================================================
*
* REFERENCES
*
*	See REFERENCES in celp.c...and:
*
*	Lin, Daniel, "New Approaches to Stochastic Coding of Speech
*	Sources at Very Low Bit Rates," Signal Processing III:  Theories
*	and Applications (Proceedings of EUSIPCO-86), 1986, p.445.
*
*	Xydeas, C.S., M.A. Ireton and D.K. Baghbadrani, "Theory and
*	Real Time Implementation of a CELP Coder at 4.8 and 6.0 kbits/s
*	Using Ternary Code Excitation," Fifth International Conference on
*	Digital Processing of Signals in Communications, 1988, p. 167.
*
*	Ess, Mike, "Simple Convolution on the Cray X-MP,"
*	Supercomputer, March 1988, p. 35
*
*	Supercomputer, July 1988, p. 24
*
***************************************************************************/
float ConvCor(
float	ExcVec[],
int	ExVecLen,
int	First,
float	LPImpResp[SF_LEN],
int 	LenTruncH,
float	Residual[RES_LEN],
float	*NegErr)
{
static float 	Conv[MAX_CW_VEC_LEN];
static float 	Energy=0.0;
float		Cor;
float		Gain;

	if (First)	{

/*  For first codeword, calculate and save convolution of codeword with 
    truncated impulse response */
	  CalcStochConv(ExcVec, ExVecLen, LPImpResp, LenTruncH, Conv);

	}
	else	{
	  
/*  End correct the convolution sum on subsequent code words */
/*  First Shift */
	  EndCorrectStoch(ExcVec[1], LPImpResp, LenTruncH, Conv);

/*  Second Shift */
	  EndCorrectStoch(ExcVec[0], LPImpResp, LenTruncH, Conv);

	}

/*  Calculate correlation and energy */
	CalcCorEngStoch(Residual, Conv, ExcVec[0], ExcVec[1], First, &Cor, &Energy);

/*  Compute Gain and Error */
	CompGainErrStoch(Energy, Cor, &Gain, NegErr);

	return Gain;
}
/*

*************************************************************************
*
* ROUTINE
*		CalcStochConv
*
* FUNCTION
*		Calculate and save convolution of codeword with 
*		truncated impulse response
*
* SYNOPSIS
*               CalcStochConv(ExcVec, ExcVecLen, LPImpulseResponse, Conv)
*
*   formal 
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       ExcVec		float	 i	excitation vector (ternary codeword)
*       ExcVecLen	int	 i	excitation vector length
*	LPImpResp	float	 i	LPC Impulse Response
*	LenTruncH	int	 i	length to truncate impulse response
*	Conv		float	 o	Convolution of codeword with impulse
*					response
*
***************************************************************************
*	   For first code word, calculate and save convolution
*	   of code word with truncated (to len) impulse response:
*
*	   NOTES: A standard convolution of two L point sequences
*	          produces 2L-1 points, however, this convolution
*	          generates only the first L points.
*
*	          A "scalar times vector accumulation" method is used
*	          to exploit (skip) zero samples of the code words:
*
*	          min(L-i+1, len)
*	   y         =  SUM  ex * h   , where i = 1, ..., L points
*	    i+j-1, t    j=1    i   j
*
*	                ex |1 2  .  .  .  L|
*	   h |x x len ... 2 1|               = y(1)
*	     h |x x len ... 2 1|             = y(2)
*	                      :                 :
*	                 h |x x len ... 2 1| = y(L)
*
***************************************************************************/

void CalcStochConv(
float	ExcVec[],
int	ExcVecLen,
float	LPImpResp[],
int	LenTruncH,
float	Conv[MAX_CW_VEC_LEN])
{
int	i, j;

/*  Initialize */
	for(i=0; i< SF_LEN; i++)	{
	  Conv[i] = 0.0;
	}

	for(i=0; i< ExcVecLen; i++)	{
	  if (rint((double)(ExcVec[i])) != 0.0)	{
	    for(j=0; j<min(SF_LEN - i, LenTruncH); j++)	{
	      Conv[i+j] += ExcVec[i] * LPImpResp[j];
	    }
	  }
	}

}
/*

*************************************************************************
*
* ROUTINE
*		EndCorrectStoch
*
* FUNCTION
*		End correct the convolution sum
*
* SYNOPSIS
*               EndCorrectStoch(ExcVal, LPImpResp, LenTruncH, Conv)
*
*   formal 
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*	ExcVal		float	 i	Excitation value
*	LPImpResp	float	 i	LPC impulse response
*	LenTruncH	int	 i	Length to truncate impulse response
*	Conv		float	 o	Convolution sum
*
***************************************************************************
*
*	   End correct the convolution sum on subsequent code words:
*	   (Do two end corrections for a shift by 2 code book)
*
*	   y     =  0
*	    0, 0
*	   y     =  y        + ex * h   where i = 1, ..., L points
*	    i, m     i-1, m-1   -m   i  and   m = 1, ..., cbsize-1 code words
*
*	   NOTE:  The data movements in loops "do 59 ..." and "do 69 ..."
*	          are performed many times and can be quite time consuming.
*	          Therefore, special precautions should be taken when
*	          implementing this.  Some implementation suggestions:
*	          1.  Circular buffers with pointers to eliminate data moves.
*	          2.  Fast "block move" operation as offered on some DSPs.
*
*
***************************************************************************/

void EndCorrectStoch(
float	ExcVal,
float	LPImpResp[],
int	LenTruncH,
float	Conv[])
{
int 	i;

	if(rint((double)(ExcVal)) != 0.0)	{
	
/*  Ternary stochastic code book (-1, 0, +1) */
	  if(rint((double)(ExcVal)) == 1.0)	{
	    for(i=LenTruncH-1; i>0; i--)	{
	      Conv[i-1] += LPImpResp[i];
	    }
	  }
	  else	{
	    for(i=LenTruncH-1; i>0; i--)	{
	      Conv[i-1] -= LPImpResp[i];
	    }
	  }
	}

	for(i=SF_LEN-1; i>0; i--)	{
	  Conv[i] = Conv[i-1];
	}
	Conv[0] = ExcVal*LPImpResp[0];

}
/*

*************************************************************************
*
* 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	float	 i	Spectrum and adaptive residual
*	Conv		float	 i	Convolution sum
*	ExcVal1		float	 i	First excitation vector value
*	ExcVal2		float	 i	Second excitation vector value
*	First		int	 i	First subframe flag
*	Cor		float	 o	Correlation
*	Energy		float	 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(
float	Residual[RES_LEN],
float	Conv[],
float	ExcVal1,
float	ExcVal2,
int	First,
float	*Cor,
float 	*Energy)
{
int i;
static float NextLastConv=0.0;
static float LastConv=0.0;

/*  Initialize */
	*Cor = 0.0;

/*  Calculate correlation */
	for(i=0; i<SF_LEN; i++)	{
	  *Cor += Conv[i]*Residual[i];
	}

/*  End correct energy on subsequent code words */
	if ((rint((double)(ExcVal1)) == 0.0) && (rint((double)(ExcVal2)) == 0.0) && (!First))	{
	  *Energy -= NextLastConv*NextLastConv + LastConv*LastConv;
	}
	else	{
	  *Energy = 0.0;
	  for(i=0; i<SF_LEN; i++)	{
	    *Energy += Conv[i] * Conv[i];
	  }
	}
	NextLastConv = Conv[SF_LEN-2];
	LastConv = Conv[SF_LEN-1];

}
/*

*************************************************************************
*
* ROUTINE
*		CompGainErrStoch
*
* FUNCTION
*		Compute gain and error
*
* SYNOPSIS
*               CompGainErrStoch(Energy, Cor, Gain, Error)
*
*   formal 
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*	Energy		float	 i	Energy
*	Cor		float	 i	Correlation
*	Gain		float	 o	Codebook Gain
*	Error		float	 o	Codebook Error
*
***************************************************************************
*
*	Compute gain and error:
*	  NOTE: Actual MSPE = e0.e0 - gain(2*cor-gain*eng)
*		since e0.e0 is independent of the code word,
*		minimizing MSPE is equivalent to maximizing:
*		     match = gain(2*cor-gain*eng)
*		If unquantized gain is used, this simplifies:
*		     match = cor*gain
*
*	Independent (open-loop) quantization of gain and match (index):
*
***************************************************************************/

void CompGainErrStoch(
float	Energy,
float	Cor,
float	*Gain,
float	*Error)
{

	if(Energy <= 0.0)
	  *Gain = Cor;
	else
	  *Gain = Cor/Energy;

	*Error = Cor * *Gain;

}