fs_lib.c

2400bps MELP语音编解码源程序。

/*

2.4 kbps MELP Proposed Federal Standard speech coder

Fixed-point C code, version 1.0

Copyright (c) 1998, Texas Instruments, Inc.  

Texas Instruments has intellectual property rights on the MELP
algorithm.  The Texas Instruments contact for licensing issues for
commercial and non-government use is William Gordon, Director,
Government Contracts, Texas Instruments Incorporated, Semiconductor
Group (phone [phone]).

The fixed-point version of the voice codec Mixed Excitation Linear
Prediction (MELP) is based on specifications on the C-language software
simulation contained in GSM 06.06 which is protected by copyright and
is the property of the European Telecommunications Standards Institute
(ETSI). This standard is available from the ETSI publication office
tel. [phone]. ETSI has granted a license to United States
Department of Defense to use the C-language software simulation contained
in GSM 06.06 for the purposes of the development of a fixed-point
version of the voice codec Mixed Excitation Linear Prediction (MELP).
Requests for authorization to make other use of the GSM 06.06 or
otherwise distribute or modify them need to be addressed to the ETSI
Secretariat fax: [phone].

*/
/*

  fs_lib.c: Fourier series subroutines 

*/

/*  compiler include files  */
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "spbstd.h"
#include "mathhalf.h"
#include "mathdp31.h"
#include "wmops.h"
#include "mat.h"
#include "math_lib.h"
#include "fs.h"
#include "constant.h"

/*  compiler constants */
#define PRINT 1

/*  external variable */
extern int complexity;

/*								*/
/*	Subroutine FIND_HARM: find Fourier coefficients using	*/
/*	FFT of input signal divided into pitch dependent bins.	*/
/*								*/
/*  Q values:
    input - Q0
    fsmag - Q13
    pitch - Q7 */

#define	FFTLENGTH	512

/* Memory definition		*/
static Shortword find_hbuf[2*FFTLENGTH];
static Longword mag[FFTLENGTH];
static Shortword wr_array[FFTLENGTH/2+1];
static Shortword wi_array[FFTLENGTH/2+1];

void find_harm(Shortword input[],Shortword fsmag[],Shortword pitch,
	       Shortword num_harm,Shortword length)
{
    Shortword i, j, k, iwidth, i2;
    Shortword fwidth;
    Shortword num_harm_Q11;
    Longword  avg;
    Shortword temp;
    Shortword shift, max;

    Longword *L_fsmag;

    
    MEM_ALLOC(MALLOC, L_fsmag, num_harm, Longword);

    /* Find normalization factor of frame and */
    /* scale input to maximum precision.      */
    max = 0;       data_move();
    for (i = 0; i < length; i++) {
      temp = abs_s(input[i]);
      compare_nonzero();
      if (temp > max)
	max = temp;
    }
    shift = norm_s(max);

    /* initialize fsmag */
    for (i = 0; i < num_harm; i++)
      fsmag[i] = ONE_Q13;

    /* Perform peak-picking on FFT of input signal */

    /* Calculate FFT of complex signal in scratch buffer	*/
    v_zap(find_hbuf,2*FFTLENGTH);
    for (i = 0; i < 2*length; i+=2) {
      find_hbuf[i] = shl(input[i/2],shift);    data_move();
    }
    fft(find_hbuf,FFTLENGTH,MONE_Q15);
	
    /* Implement pitch dependent staircase function		*/
    /* Harmonic bin width */
    /* fwidth=Q6 */
    fwidth = shr(divide_s((Shortword)FFTLENGTH,pitch),2);
    /* iwidth = (int) fwidth */
    iwidth = shr(fwidth,6);
    compare_nonzero();
    if (iwidth < 2) {
      iwidth = 2;      data_move();
    }
    i2 = shr(iwidth,1);

    /* if (num_harm > 0.25*pitch) num_harm = 0.25*pitch */
    /* temp = 0.25*pitch in Q0 */
    temp = shr(pitch,9);
    compare_nonzero();
    if (num_harm > temp) {
      num_harm = temp;
    }

    /* initialize avg to make sure that it is non-zero */
    avg = 1;       L_data_move();
    for (k = 0; k < num_harm; k++) {
      /* i = ((k+1)*fwidth) - i2 + 0.5 */ /* Start at peak-i2 */
      temp = extract_l(L_mult(add(k,1),fwidth));  /* temp=Q7 */
      i = shr(add(sub(temp,shl(i2,7)),(Shortword)X05_Q7),7);

      /* Calculate magnitude squared of coefficients		*/
      for (j = i; j < i+iwidth; j++) {
	mag[j] = L_add(L_mult(find_hbuf[2*j],find_hbuf[2*j]),
		       L_mult(find_hbuf[(2*j)+1],find_hbuf[(2*j)+1]));
	L_data_move();
      }

      j = add(i,findmax(&mag[i],iwidth));
      L_fsmag[k] = mag[j];       L_data_move();
      avg = L_add(avg,mag[j]);
    }

    /* Normalize Fourier series values to average magnitude */
    num_harm_Q11 = shl(num_harm,11);
    for (i = 0; i < num_harm; i++) {
      /* temp = num_harm/(avg+ .0001) */
      /* fsmag[i] = sqrt(temp*fsmag[i]) */
      temp = L_divider2(L_fsmag[i],avg,0,0);     /* temp=Q15 */
      temp = mult(num_harm_Q11,temp);    /* temp=Q11 */
      fsmag[i] = shl(sqrt_fxp(temp,11),2);      data_move();
    }

    MEM_FREE(FREE,L_fsmag);
} /* find_harm */

    

/*	Subroutine FFT: Fast Fourier Transform 		*/
/**************************************************************
* Replaces data by its DFT, if isign is 1, or replaces data   *
* by inverse DFT times nn if isign is -1.  data is a complex  *
* array of length nn, input as a real array of length 2*nn.   *
* nn MUST be an integer power of two.  This is not checked    *
* The real part of the number should be in the zeroeth        *
* of data , and the imaginary part should be in the next      *
* element.  Hence all the real parts should have even indeces *
* and the imaginary parts, odd indeces.			      *

* Data is passed in an array starting in position 0, but the  *
* code is copied from Fortran so uses an internal pointer     *
* which accesses position 0 as position 1, etc.		      *

* This code uses e+jwt sign convention, so isign should be    *
* reversed for e-jwt.                                         *
***************************************************************/
/* Q values:
   datam1 - Q14
   isign - Q15 */

#define	SWAP(a,b) temp1 = (a);(a) = (b); (b) = temp1

void fft(Shortword *datam1,Shortword nn,Shortword isign)
{
	Shortword n,mmax,m,j,istep,i;
	Shortword wr,wi,temp1;
	Longword register L_tempr,L_tempi;
	Shortword *data;
	Longword L_temp1,L_temp2;
	Shortword index,index_step;

	/*  Use pointer indexed from 1 instead of 0	*/
	data = &datam1[-1];

	n = shl(nn,1);
	j = 1;
	for( i = 1; i < n; i+=2 ) {
	  if ( j > i) {
	    SWAP(data[j],data[i]);    data_move();    data_move();
	    SWAP(data[j+1],data[i+1]);    data_move();    data_move();
	  }
	  m = nn;
	  while ( m >= 2 && j > m ) {
		j = sub(j,m);
		m = shr(m,1);
	  }
	  j = add(j,m);
	}
	mmax = 2;

	/* initialize index step */
	index_step = nn;

	while ( n > mmax) {
	  istep = shl(mmax,1);  /* istep = 2 * mmax */

	  index = 0;
	  index_step = shr(index_step,1);

	  wr = ONE_Q15;
	  wi = 0;
	  for ( m = 1; m < mmax;m+=2) {
	    for ( i = m; i <= n; i += istep) {
		j = i + mmax;

		/* note: complexity is reduced since L_shr is not necessary 
		   on TMS32C5x with SPM=0, mac and msu can be used */

		/*tempr = wr * data[j] - wi * data[j+1] */
		L_temp1 = L_shr(L_mult(wr,data[j]),1);
		L_temp2 = L_shr(L_mult(wi,data[j+1]),1);
		L_tempr = L_sub(L_temp1,L_temp2);
		complexity -= (2+2+2);

		/* tempi = wr * data[j+1] + wi * data[j] */
		L_temp1 = L_shr(L_mult(wr,data[j+1]),1);
		L_temp2 = L_shr(L_mult(wi,data[j]),1);
	      	L_tempi = L_add(L_temp1,L_temp2);
		complexity -= (2+2+2);

	   	/* data[j] = data[i] - tempr */
		L_temp1 = L_shr(L_deposit_h(data[i]),1);
	   	data[j] = extract_h(L_sub(L_temp1,L_tempr));
		complexity -= 2;

		/* data[i] += tempr */
		data[i] = extract_h(L_add(L_temp1,L_tempr));

		/* data[j+1] = data[i+1] - tempi */
		L_temp1 = L_shr(L_deposit_h(data[i+1]),1);
		data[j+1] = extract_h(L_sub(L_temp1,L_tempi));
		complexity -= 2;

		/* data[i+1] += tempi */
		data[i+1] = extract_h(L_add(L_temp1,L_tempi));
	    }
	    index = add(index,index_step);
	    wr = wr_array[index];
	    if (isign < 0)
	      wi = negate(wi_array[index]);
	    else
	      wi = wi_array[index];
	  }
	  mmax = istep;
	}
} /* fft */



/*                                                              */
/*      Subroutine FINDMAX: find maximum value in an            */
/*      input array.                                            */
/*                                                              */
Shortword findmax(Longword input[],Shortword npts)
{
  Shortword register    i, maxloc;
  Longword register  maxval, *p_in;

  p_in = &input[0];
  maxloc = 0;    data_move();
  maxval = input[maxloc];    data_move();
  for (i = 1; i < npts; i++ ) {
    compare_nonzero();
    if (*(++p_in) > maxval) {
      maxloc = i;    data_move();
      maxval = *p_in;    data_move();
    }
  }
  return(maxloc);
}



/* Initialization of wr_array and wi_array */
void fs_init()
{
  Shortword i;
  Shortword fft_len2,shift,step,theta;

  fft_len2 = shr(FFTLENGTH,1);
  shift = norm_s(fft_len2);
  step = shl(2,shift);
  theta = 0;

  for (i = 0; i <= fft_len2; i++) {
    wr_array[i] = cos_fxp(theta);    data_move();
    wi_array[i] = sin_fxp(theta);    data_move();
    if (i >= (fft_len2-1))
      theta = ONE_Q15;
    else
      theta = add(theta,step);
  }
}


/*								*/
/*	Subroutine IDFT_REAL: take inverse discrete Fourier 	*/
/*	transform of real input coefficients.			*/
/*	Assume real time signal, so reduce computation		*/
/*	using symmetry between lower and upper DFT		*/
/*	coefficients.						*/
/*								*/
/*  Q values:
    real - Q13
    signal - Q15
*/
#define DFTMAX 160

/* Memory definition	*/
static Shortword	idftc[DFTMAX];

void	idft_real(Shortword real[], Shortword signal[], Shortword length)

{
    Shortword i, j, k, k_inc, length2;
    Shortword w, w2;
    Shortword temp;
    Longword L_temp;

#if (PRINT)
    if (length > DFTMAX) {
	printf("****ERROR: IDFT size too large **** \n");
	exit(1);
    }
#endif
    
    length2 = add(shr(length,1),1);
    /* w = TWOPI / length; */
    w = divide_s(TWO_Q3,length);   /* w = 2/length in Q18 */

    for (i = 0; i < length; i++ ) {
        L_temp = L_mult(w,i);      /* L_temp in Q19 */

	/* make sure argument for cos function is less than 1 */
	L_compare_nonzero();
	if (L_temp > (Longword)ONE_Q19) {
	  /* cos(pi+x) = cos(pi-x) */
	  L_temp = L_sub((Longword)TWO_Q19,L_temp);
	}
	else if (L_temp == (Longword)ONE_Q19)
	  L_temp = L_sub(L_temp,1);
	L_temp = L_shr(L_temp,4);      /* L_temp in Q15 */
	temp = extract_l(L_temp);
	idftc[i] = cos_fxp(temp);      /* idftc in Q15 */
    }

    w = shr(w,1);        /* w = 2/length in Q17 */
    w2 = shr(w,1);       /* w2 = 1/length in Q17 */
    real[0] = mult(real[0],w2);     /* real in Q15 */
    temp = sub(length2,1);
    for (i = 1; i < temp; i++ ) {
      /* real[i] *= (2.0/length); */
      real[i] = mult(real[i],w);
    }
    temp = shl(i,1);
    if (temp == length)
      /* real[i] *= (1.0/length); */
      real[i] = mult(real[i],w2);
    else
      /* real[i] *= (2.0/length);*/
      real[i] = mult(real[i],w);

    for (i = 0; i < length; i++ ) {
	L_temp = L_deposit_h(real[0]);    /* L_temp in Q31 */
	k_inc = i;
	k = k_inc;
	for (j = 1; j < length2; j++ ) {
	    L_temp = L_mac(L_temp,real[j],idftc[k]);
	    k += k_inc;
	    if (k >= length)
		k -= length;
	}
	signal[i] = round(L_temp);
    }
} /* IDFT_REAL */