code_lsf.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 "lsftable.h"
#include "code_lsf.h"

int	BITS[10] = {3, 4, 4, 4, 4, 3, 3, 3, 3, 3};

#define FSCALE	8000.0
#ifdef NEW_LSF_CODE
#define MIN_LSF_DIST	39
#else
#define MIN_LSF_DIST	0
#endif

static void AssignLSFFreqs(
int	findex[ORDER],
float	qlsp[ORDER]);

static void CalcLSFFreq(
float	lsp[ORDER],
int	frequency,
float	lsf[ORDER]);

static void CheckMono(
int	table[LSFTABLE_LENGTH][LSFTABLE_WIDTH],
int	index[],
int	num,
float	lsf[],
int	level,
float	*errorup,
float	*errordn,
int	*nonmono);

static void CorrectNonMono(
float	errorup,
float	errordn,
int	findex[],
int	where,
int	level);

static void InitLSFQuant(
int	table[LSFTABLE_LENGTH][LSFTABLE_WIDTH],
int	lsf[],
int	level,
int	lsfnum);

static void QuantizeLSF(
float 	input,
int	level[],
int 	nlevel,
int	*findex);

/**************************************************************************
*                                                                         *
* ROUTINE
*		QuanLSF
*
* FUNCTION
*		Quantize line spectral frequencies
* SYNOPSIS
*		QuanLSF(lsf, qlsf, freq_index)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*	lsf		float	 i	Unquantized LSFs
*	qlsf		float 	 o	Quantized LSFs
*	freq_index	int	 o	Indexes into frequency array
*
**************************************************************************/
void QuanLSF(
float	lsf[ORDER],
float	qlsf[ORDER],
int	freq_index[ORDER])
{
float	freqs[ORDER];
int	i, level;
int	LSFInit[16];
float	errorup, errordn;
int	nonmonotonic=FALSE;


/*  Calculate frequencies for LSPs */
	CalcLSFFreq(lsf, 8000, freqs);

/*  For each LSF */
	for(i=0;i<ORDER;i++)	{
/*  Calculate quantization level for ith LSF based on bit allocation */
	  level = (int)pow(2.0, (double)BITS[i]) - 1;

/*  Assign initial quantized LSFs */
	  InitLSFQuant(LSFTable, LSFInit, level, i);

/*  Quantize LSFs */
	  QuantizeLSF(freqs[i], LSFInit, level+1, &freq_index[i]);

/*  Check for monotonicity */
	  if(i>0)
	    CheckMono(LSFTable, freq_index, i, freqs, level, &errorup, &errordn, &nonmonotonic);

/*  Correct if nonmonotonic */
	  if (nonmonotonic)
	    CorrectNonMono(errorup, errordn, freq_index, i, level);

	}

/*  Quantize LSF frequencies using indices found above */
	AssignLSFFreqs(freq_index, qlsf);

}

/*

*************************************************************************
*                                                                         *
* ROUTINE
*		CalcLSFFreq
*
* FUNCTION
*		Calculate LSFs from LSPs using input frequency
* SYNOPSIS
*		CalcLSFFreq(lsp, frequency, lsf)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*	lsp		float	  i	Frame of input line spectral pairs
*	frequency	int	  i	Sampling frequency
*	lsf		float	  o	Frame of output line spectral frequencies
*
**************************************************************************/

void CalcLSFFreq(
float	lsf[ORDER],
int	frequency,
float	freqs[ORDER])
{
int	i;

/*  Calculate LSF based on sampling frequency */
	for(i=0;i<ORDER;i++)
	{
	  freqs[i] = (float)frequency * lsf[i];
	}
}

/*

*************************************************************************
*                                                                         *
* ROUTINE
*		InitLSFQuant
*
* FUNCTION
*		Initialize Quantized LSFs
* SYNOPSIS
*		InitLSFQuant(table, lsf, n)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*	table		int	  i	Table of LSFs
*	lsf		float	  o	Frame of output line spectral frequencies
*	level		int	  i	Number of levels
*	lsfnum		int	  i	LSF number (not amount, but item number) 
*
**************************************************************************/

void InitLSFQuant(
int	table[LSFTABLE_LENGTH][LSFTABLE_WIDTH],
int	lsf[],
int	level,
int	lsfnum)
{
int	i;

/*  Assign values from table */
	for(i=0;i<=level;i++)	{
	  lsf[i] = table[lsfnum][i];
	}

}

/*

*************************************************************************
*                                                                         *
* ROUTINE
*		CheckMono
*
* FUNCTION
*		Check frequencies indices for monotonicity
* SYNOPSIS
*		CheckMono(table, index, num, lsf, errorup, errordn, nonmono)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*	table		int	  i	Table of LSFs
*	index		int	  i	Index into table
*	num		int	  i	LSF number
*	lsf		float	  i	Line spectral frequencies
*	level		int	  i	Quantization level
*	errorup		float	  o	Upward error correction
*	errordn		float	  o	Downward error correction
*	nonmono		int	  o	Flag indicating lack of monotonicity
*
**************************************************************************/

void CheckMono(
int	table[LSFTABLE_LENGTH][LSFTABLE_WIDTH],
int	index[],
int	num,
float	lsf[],
int	level,
float	*errorup,
float	*errordn,
int	*nonmono)
{

/*  Check this frequency agains the previous one */
	if(table[num][index[num]] - MIN_LSF_DIST <= table[num-1][index[num-1]])
	{
	  *errorup = fabs(lsf[num] - 
			table[num][min(index[num]+1, level)])
		 +   fabs(lsf[num-1] -
			table[num-1][index[num-1]]);

	  *errordn = fabs(lsf[num] -
			table[num][index[num]])
		 +   fabs(lsf[num-1] -
			table[num-1][max(index[num-1]-1, 0)]);
	  *nonmono = TRUE;
	}
	else
	  *nonmono = FALSE;
}


/*

*************************************************************************
*                                                                         *
* ROUTINE
*		CorrectNonMono
*
* FUNCTION
*		Correct index for minimum error if nonmonotonic
* SYNOPSIS
*		CorrectNonMono(errorup, errordn, findex, where, level)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*	errorup		float	 i	Error adjustment measurement
*	errordn		float	 i	Error adjustment measurement
*	findex		float	i/o	Frequency index into LSFTable
*	where		int	 i	Current LSF level
*	level		int	 i	Number of quantization levels available
*
**************************************************************************/

void CorrectNonMono(
float	errorup,
float	errordn,
int	findex[],
int	where,
int	level)
{

	if (errorup < errordn) {
	  findex[where] = min(findex[where]+1, level);

	  while (LSFTable[where][findex[where]] - MIN_LSF_DIST < 
			LSFTable[where-1][findex[where-1]])	{
	    findex[where] = min(findex[where]+1, level);
	  }
	}
	else	{
	  if (where == 1) {
	    findex[where-1] = max(findex[where-1]-1, 0);
	  }
	  else	{
	    if (LSFTable[where-1][max(findex[where-1]-1,0)] - MIN_LSF_DIST >
			LSFTable[where-2][findex[where-2]]) 	{
	      findex[where-1] = max(findex[where-1]-1, 0);
	    }
	    else	{
	      findex[where] = min(findex[where]+1, level);

	      while(LSFTable[where][findex[where]] - MIN_LSF_DIST < 
			LSFTable[where-1][findex[where-1]])	{
	        findex[where] = min(findex[where]+1, level);
	      }
	    }
		    
	  }
	}
}


/*

*************************************************************************
*                                                                         *
* ROUTINE
*		AssignLSFFreqs
*
* FUNCTION
*		Correct index for minimum error if nonmonotonic
* SYNOPSIS
*		AssignLSFFreqs(findex, qlsp)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*	findex		int	 i	Frequency index into LSF Table
*	qlsp		int	 o	Quantized line spectral parameter
*
**************************************************************************/


void AssignLSFFreqs(
int	findex[ORDER],
float	qlsp[ORDER])
{
int	i;

/* Quantize LSF frequencies using indices passed in */
	for(i=0;i<ORDER;i++)	{
	  qlsp[i] =  ((float)LSFTable[i][findex[i]] / FSCALE);
	}
}

/*

*************************************************************************
*
* ROUTINE
*               UnquanLSF
*
* FUNCTION
*                
*               Independent nonuniform scalar lsf decoder 
*
* SYNOPSIS
*               UnquanLSF(findex,order,lsf)
*
*   formal 
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       findex		int	i	lsf frequency index
*	no		int	i	lpc order
*	lsf		float	o	lsf quantized frequency
*
***************************************************************************
*
* CALLED BY
*
*	codeparm
*
**************************************************************************/
void UnquanLSF(
int 	findex[], 
int	order,
float 	lsf[])
{
  int i;

  /* *** choose appropriate frequency by findex */
  
  for (i = 0; i < order; i++)
  {
    lsf[i] = LSFTable[i][findex[i]]/FSCALE;
  }
}

/*

*************************************************************************
*
* ROUTINE
*		QuantizeLSF
*
* FUNCTION
*		Nearest output level scalar quantizer
*
* SYNOPSIS
*		QuantizeLSF(input, level, nlevel, findex)
*
*   formal 
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*	input		real	i	Scalar to be quantized
*	level(nlevel)	real	i	Output level table
*	nlevel		int	i	Number of quantizer levels
*	findex		int	o	Index to quantized output (zero based!)
*
****************************************************************************/
void QuantizeLSF(
float 	input,
int	level[],
int 	nlevel,
int	*findex)
{
int 	i;
float 	dist, low=0.0;

/*	*** Quantize to nearest output level:	*/

	low = (float)fabs((double)(input - level[0]));
	*findex = 0;
	for(i = 1; i< nlevel; i++)	{
	   dist = (float)fabs((double)(input - level[i]));
	   if ((dist < low) || (i == 0)) 
	   {
	      low = dist;
	      *findex = i;
	   }
	}
	
}