语音压缩

开发平台：
C/C++

fs_lib.c：源码内容
							/*
  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;
	Longword L_temp,L_shift;
	Shortword temp1;
    
    MEM_ALLOC(MALLOC, L_fsmag, num_harm, Longword);
    /* Find normalization factor of frame and */
    /* scale input to maximum precision.      */
    max = 0;    //   data_move();mark del
    for (i = 0; i < length; i++) {
      /* temp = abs_s(input[i]); */
	   L_temp = _abs2((Longword)input[i]);
	   temp = (Shortword) (0x0000ffffL & L_temp);
//      compare_nonzero();  mark del
      if (temp > max)
	max = temp;
    }
   /*  shift = norm_s(max); */
   L_temp = (Longword) max << 16;
   L_shift = _norm(L_temp);
   if (L_temp == 0){
    L_shift = (Longword)0;
   }
   shift = (Shortword) (0x0000ffffL & L_shift);
    /* 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);  
       /*  find_hbuf[i] = (shift >=0)? (shl_a(input[i/2],shift)) : (shr_a(input[i/2],shift)); */
      // data_move();mark del
    }
    fft(find_hbuf,FFTLENGTH,MONE_Q15);
	
    /* Implement pitch dependent staircase function		*/
    /* Harmonic bin width */
    /* fwidth=Q6 */
    /* fwidth = shr(divide_s((Shortword)FFTLENGTH,pitch),2); */
       temp1 = divide_s((Shortword)FFTLENGTH,pitch);
	   L_temp = _shr2((Longword)temp1,2);
	   fwidth = (Shortword) (0x0000ffffL & L_temp);
    /* iwidth = (int) fwidth */
  /*   iwidth = shr(fwidth,6); */
       L_temp = _shr2((Longword)fwidth,6);
	   iwidth = (Shortword) (0x0000ffffL & L_temp);
//    compare_nonzero();mark del
    if (iwidth < 2) {
      iwidth = 2;    
      //  data_move();  mark del
    }
    /* i2 = shr(iwidth,1); */
	  L_temp = _shr2((Longword)iwidth,1);
	  i2 = (Shortword) (0x0000ffffL & L_temp);
    /* if (num_harm > 0.25*pitch) num_harm = 0.25*pitch */
    /* temp = 0.25*pitch in Q0 */
   /*  temp = shr(pitch,9); */
      L_temp = _shr2((Longword)pitch,9);
	  temp = (Shortword) (0x0000ffffL & L_temp);
//    compare_nonzero();  mark del
    if (num_harm > temp) {
      num_harm = temp;
    }
    /* initialize avg to make sure that it is non-zero */
    avg = 1;    //   L_data_move();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);    */
        L_temp = _sadd2((Longword)k,(Longword)1);
		temp1 = (Shortword) (0x0000ffffL & L_temp);
		L_temp = _smpy(temp1,fwidth);
		temp = (Shortword) (0x0000ffffL & L_temp);
		temp1 = sub(temp,shl_a(i2,7));
		L_temp = _sadd2((Longword)temp1,(Longword)X05_Q7);
		L_temp = _shr2(L_temp,7);
		i = (Shortword) (0x0000ffffL & L_temp);
      /* 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]));	  */
       mag[j] = _sadd(_smpy(find_hbuf[2*j],find_hbuf[2*j]),
		       _smpy(find_hbuf[(2*j)+1],find_hbuf[(2*j)+1]));
//	L_data_move();   mark del
      }
      /* j = add(i,findmax(&mag[i],iwidth)); */
      temp1 = findmax(&mag[i],iwidth);
	  L_temp = _sadd2((Longword)i,(Longword)temp1);
	  j = (Shortword) (0x0000ffffL & L_temp);
      L_fsmag[k] = mag[j];    
//         L_data_move();   mark del
      /* avg = L_add(avg,mag[j]); */
	  avg = _sadd(avg,mag[j]);
    }
    /* Normalize Fourier series values to average magnitude */
    num_harm_Q11 = shl_a(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);                      */   
         temp = L_divider2(L_fsmag[i],avg,0,0);
		 L_temp = _smpy(num_harm_Q11,temp);
		 temp = (Shortword) (0x0000ffffL & (L_temp >> 16));
		 fsmag[i] = shl_a(sqrt_fxp(temp,11),2);
		
		 
      //   data_move();   mark del
    }
    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;
	register Longword  L_tempr,L_tempi;
	Shortword *data;
	Longword L_temp1,L_temp2;
	Shortword index,index_step;
	Longword L_temp;
	/*  Use pointer indexed from 1 instead of 0	*/
	data = &datam1[-1];
	n = shl_a(nn,1);
	j = 1;
	for( i = 1; i < n; i+=2 ) {
	  if ( j > i) {
	    SWAP(data[j],data[i]);  
//	      data_move();    data_move();  mark del
	    SWAP(data[j+1],data[i+1]);  
//	      data_move();    data_move();  mark del
	  }
	  m = nn;
	  while ( m >= 2 && j > m ) {
		j = sub(j,m);
		/* m = shr(m,1); */
		L_temp = _shr2((Longword)m,1);
		m = (Shortword) (0x0000ffffL & L_temp);
	  }
	 /*  j = add(j,m); */
	    L_temp = _sadd2((Longword)j,(Longword)m);
		j = (Shortword) (0x0000ffffL & L_temp);
	}
	mmax = 2;
	/* initialize index step */
	index_step = nn;
	while ( n > mmax) {
	  istep = shl_a(mmax,1);  /* istep = 2 * mmax */
	  index = 0;
	  /* index_step = shr(index_step,1); */
	  L_temp = _shr2((Longword)index_step,1);
      index_step = (Shortword) (0x0000ffffL & L_temp);
	  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);	     */
        L_temp1 = _sshvr(_smpy(wr,data[j]),1);
		L_temp2 = _sshvr(_smpy(wi,data[j+1]),1);
		L_tempr = _ssub(L_temp1,L_temp2);
//		complexity -= (2+2+2);   mark del
		/* 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);	 */
        L_temp1 = _sshvr(_smpy(wr,data[j+1]),1);
		L_temp2 = _sshvr(_smpy(wi,data[j]),1);
	    L_tempi = _sadd(L_temp1,L_temp2);
//		complexity -= (2+2+2);   mark del
	   	/* 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));    */
        L_temp = (Longword)(data[i] << 16);
        L_temp1 = _sshvr(L_temp,1);
		L_temp = _ssub(L_temp1,L_tempr);
        data[j] = (Shortword) (0x0000ffffL & (L_temp >> 16));
//		complexity -= 2;    mark del
		/* data[i] += tempr */
		/* data[i] = extract_h(L_add(L_temp1,L_tempr)); */
		L_temp = _sadd(L_temp1,L_tempr);
        data[i] = (Shortword) (0x0000ffffL & (L_temp >> 16));
		/* 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));	  */
        L_temp = (Longword) (data[i+1] << 16);
        L_temp1 = _sshvr(L_temp,1);
		L_temp = _ssub(L_temp1,L_tempi);
        data[j+1] = (Shortword) (0x0000ffffL & (L_temp >> 16));
 
//		complexity -= 2;    mark del
		/* data[i+1] += tempi */
		/* data[i+1] = extract_h(L_add(L_temp1,L_tempi)); */
		 L_temp = _sadd(L_temp1,L_tempi);
         data[i+1] = (Shortword) (0x0000ffffL & (L_temp >> 16));
	    }
	   /*  index = add(index,index_step); */
	    L_temp = _sadd2((Longword)index,(Longword)index_step);
		index = (Shortword) (0x0000ffffL & L_temp);
	    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) */
/* {                                                  */
/*   register Shortword     i, maxloc;                */
/*   register Longword  maxval, *p_in;                */
/*                                                    */
/*   p_in = &input[0];                                */
/*   maxloc = 0;                                      */
/* //     data_move();   mark del                     */
/*   maxval = input[maxloc];                          */
/* //   data_move();  mark del                        */
/*   for (i = 1; i < npts; i++ ) {                    */
/* //    compare_nonzero();mark del                   */
/*     if (*(++p_in) > maxval) {                      */
/*       maxloc = i;                                  */
/* //         data_move();   mark del                 */
/*       maxval = *p_in;                              */
/* //       data_move();   mark del                   */
/*     }                                              */
/*   }                                                */
/*   return(maxloc);                                  */
/* }                                                  */
/* Initialization of wr_array and wi_array */
void fs_init()
{
  Shortword i;
  Shortword fft_len2,shift,step,theta;
  Longword L_temp,L_shift;
/*   fft_len2 = shr(FFTLENGTH,1); */
/*   shift = norm_s(fft_len2);    */
  L_temp = _shr2((Longword)FFTLENGTH,1);
  fft_len2 = (Shortword) (0x0000ffffL & L_temp);
  L_temp = (Longword) fft_len2 << 16;
  L_shift = _norm(L_temp);
  if (L_temp == 0){
    L_shift = (Longword)0;
  }
  shift = (Shortword) (0x0000ffffL & L_shift);
  /* step = shl(2,shift); */
  step = (shift >= 0)?(shl_a(2,shift)) : (shr_a(2,shift));
  theta = 0;
  for (i = 0; i <= fft_len2; i++) {
    wr_array[i] = cos_fxp(theta);   
//     data_move();    mark del
    wi_array[i] = sin_fxp(theta);   
//     data_move();   mark del
    if (i >= (fft_len2-1))
      theta = ONE_Q15;
    else{
      /* theta = add(theta,step); */
	  L_temp = _sadd2((Longword)theta,(Longword)step);
	  theta = (Shortword) (0x0000ffffL & L_temp);
      }
  }
}
/*								*/
/*	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;
	Longword L_temp1,L_temp2,L_temp3;
#if (PRINT)
    if (length > DFTMAX) {
	printf("****ERROR: IDFT size too large **** n");
	exit(1);
    }
#endif
    
   /*  length2 = add(shr(length,1),1); */
    L_temp1 = _shr2((Longword)length,1);
	L_temp1 = _sadd2(L_temp1,(Longword)1);
	length2 = (Shortword) (0x0000ffffL & L_temp1);
    /* 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  */
	   L_temp = _smpy(w,i);
	/* make sure argument for cos function is less than 1 */
//	L_compare_nonzero();    mark del
	if (L_temp > (Longword)ONE_Q19) {
	  /* cos(pi+x) = cos(pi-x) */
	  /* L_temp = L_sub((Longword)TWO_Q19,L_temp); */
	   L_temp = _ssub((Longword)TWO_Q19,L_temp);
	}
	else if (L_temp == (Longword)ONE_Q19)
	  L_temp = _ssub(L_temp,1);
	L_temp = _sshvr(L_temp,4);      /* L_temp in Q15 */
/* 	temp = extract_l(L_temp);	 */
    temp = (Shortword) (0x0000ffffL & 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  */
     L_temp1 = _shr2((Longword)w,1);
	 w = (Shortword) (0x0000ffffL & L_temp1);
     L_temp2 = _shr2((Longword)w,1);
	 w2 = (Shortword) (0x0000ffffL & L_temp2);
	 L_temp3 = _smpy(real[0],w2);
     real[0] = (Shortword) (0x0000ffffL & (L_temp3 >> 16));
    temp = sub(length2,1);
    for (i = 1; i < temp; i++ ) {
      /* real[i] *= (2.0/length); */
      /* real[i] = mult(real[i],w); */
	   L_temp1 = _smpy(real[i],w);
       real[i] = (Shortword) (0x0000ffffL & (L_temp1 >> 16));
    }
    temp = shl_a(i,1);
    if (temp == length){
      /* real[i] *= (1.0/length); */
    /*   real[i] = mult(real[i],w2); */
	  L_temp2 = _smpy(real[i],w2);
	  real[i] = (Shortword) (0x0000ffffL & (L_temp2 >> 16));
     }
    else{
      /* real[i] *= (2.0/length);*/
     /*  real[i] = mult(real[i],w); */
	  L_temp3 = _smpy(real[i],w);
	  real[i] = (Shortword) (0x0000ffffL & (L_temp3 >> 16));
	}
    for (i = 0; i < length; i++ ) {
/*	L_temp = L_deposit_h(real[0]);     L_temp in Q31 */
    L_temp = (Longword)(real[0]) << 16;
	k_inc = i;
	k = k_inc;
	for (j = 1; j < length2; j++ ) {
	   /*  L_temp = L_mac(L_temp,real[j],idftc[k]); */
	    L_temp = _sadd(L_temp,_smpy(real[j],idftc[k]));
	    k += k_inc;
	    if (k >= length)
		k -= length;
	}
	/* signal[i] = round(L_temp); */
    L_temp1 = _sadd(L_temp, 0x00008000L); /* round MSP */
    signal[i] = (Shortword)(0x0000ffffL & (L_temp1 >> 16));
    }
} /* IDFT_REAL */

						