语音压缩

开发平台：
C/C++

dsp_sub.c：源码内容
							/*  
  dsp_sub.c: general 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 "dsp_sub.h"
#include "constant.h"
#define PRINT 1 
/*								*/
/*	Subroutine envelope: calculate time envelope of signal. */
/*      Note: the delay history requires one previous sample    */
/*      of the input signal and two previous output samples.    */
/*      Output is scaled down by 4 bits from input signal       */
/*								*/
#define C2_Q14 -15415    /* (-0.9409*(1<<14)) */
#define C1_Q14 31565     /* (1.9266*(1<<14)) */
/* void envelope(Shortword input[], Shortword prev_in, Shortword output[],  */
/* 	      Shortword npts)                                                   */
/* {                                                                        */
/*     Shortword i;                                                         */
/*     Shortword curr_abs, prev_abs;                                        */
/*     Longword L_temp;                                                     */
/*                                                                          */
/*     prev_abs = abs_s(prev_in);                                           */
/*     for (i = 0; i < npts; i++) {                                         */
/*         curr_abs = abs_s(input[i]);                                      */
/*                                                                          */
/* 	 output[i] = curr_abs - prev_abs + C2*output[i-2] + C1*output[i-1] */ 
/* 	L_temp = L_shr(L_deposit_h(sub(curr_abs,prev_abs)),5);                  */
/* 	L_temp = L_mac(L_temp,(Shortword)C1_Q14,output[i-1]);                   */
/* 	L_temp = L_mac(L_temp,(Shortword)C2_Q14,output[i-2]);                   */
/* 	L_temp = L_shl(L_temp,1);                                               */
/* 	output[i] = round(L_temp);                                              */
/*                                                                          */
/*         prev_abs = curr_abs;                                             */
/*     }                                                                    */
/* }                                                                        */
/*								*/
/*  Subroutine fill: fill an input array with a value.		*/
/*								*/
/* void fill(Shortword output[], Shortword fillval, Shortword npts) */
/* {                                                                */
/*   Shortword i;                                                   */
/*                                                                  */
/*   for (i = 0; i < npts; i++ )                                    */
/*     output[i] = fillval;                                         */
/*                                                                  */
/* }                                                                */
/*								*/
/*	Subroutine interp_array: interpolate array              */
/*                                                              */
/*  Q values:
    ifact - Q15
*/
void interp_array(Shortword prev[],Shortword curr[],Shortword out[],
		  Shortword ifact,Shortword size)
{
    Shortword i;
    Shortword ifact2;
    Shortword temp1,temp2;
	Longword L_temp,L_temp1,L_temp2;
    if (ifact == 0) {
      for (i = 0; i < size; i++) {
	out[i] = prev[i];   //  data_move();mark del
      }
    }
    else if (ifact == ONE_Q15) {
      for (i = 0; i < size; i++) {
	out[i] = curr[i];  //   data_move();mark del
      }
    }
    else {
      ifact2 = sub(ONE_Q15,ifact);
      for (i = 0; i < size; i++) {
/* 	temp1 = mult(ifact,curr[i]);  */
/* 	temp2 = mult(ifact2,prev[i]); */
/* 	out[i] = add(temp1,temp2);	  */
        L_temp = _smpy(ifact,curr[i]);
		/* temp1 = (Shortword) (0x0000ffffL & (L_temp >> 16)); */
		L_temp1 = _smpy(ifact2,prev[i]);
		/* temp2 = (Shortword) (0x0000ffffL & (L_temp1 >> 16)); */
		L_temp2 = _sadd2(L_temp,L_temp1);
		out[i] = (Shortword) (0x0000ffffL & (L_temp2 >> 16));
 
      }
    }
}
/*								*/
/*	Subroutine median: calculate median value               */
/*								*/
#define MAXSORT 5
Shortword median(Shortword input[], Shortword npts)
{
    Shortword i,j,loc;
    Shortword insert_val;
    Shortword sorted[MAXSORT];
    /* sort data in temporary array */
#ifdef PRINT
    if (npts > MAXSORT) {
	printf("ERROR: median size too large.n");
	exit(1);
    }
#endif
    v_equ(sorted,input,npts);
    for (i = 1; i < npts; i++) {
	/* for each data point */
	for (j = 0; j < i; j++) {
	    /* find location in current sorted list */
	//    compare_nonzero();mark del
	    if (sorted[i] < sorted[j])
	      break;
	}
	/* insert new value */
	loc = j;    //  data_move();mark del
	insert_val = sorted[i];    // data_move();mark del
	for (j = i; j > loc; j--) {
	  sorted[j] = sorted[j-1];    // data_move();mark del
	}
	sorted[loc] = insert_val;    // data_move();mark del
    }
    return(sorted[npts/2]);
}
#undef MAXSORT
/*								*/
/*	Subroutine PACK_CODE: Pack bit code into channel.	*/
/*								*/
void pack_code(Shortword code,UShortword **p_ch_beg,Shortword *p_ch_bit, 
	       Shortword numbits,Shortword wsize)
{
    Shortword	i,ch_bit;
    UShortword *ch_word;
	ch_bit = *p_ch_bit;
	ch_word = *p_ch_beg;
	for (i = 0; i < numbits; i++) {
		/* Mask in bit from code to channel word	*/
		if (ch_bit == 0)
		  *ch_word = (UShortword)(shr((Shortword)(code & (shl(1,i))),i));
		else
		  *ch_word |= (UShortword)(shl((shr((Shortword)(code & (shl(1,i))),i)),ch_bit));
		/* Check for full channel word			*/
		ch_bit = add(ch_bit,1);
		if (ch_bit >= wsize) {
			ch_bit = 0;
			(*p_ch_beg)++ ;
			ch_word++ ;
		}
	}
	/* Save updated bit counter	*/
	*p_ch_bit = ch_bit;
}
/*								*/
/*	Subroutine peakiness: estimate peakiness of input       */
/*      signal using ratio of L2 to L1 norms.                   */
/*								*/
/* Q_values
   --------
   peak_fact - Q12
   input - Q0
   
*/
/* Upper limit of (magsq/sum_abs) to prevent saturation of peak_fact */
#define LIMIT_PEAKI    20723
Shortword peakiness(Shortword input[], Shortword npts)
{
    Shortword i;
    Shortword peak_fact;
    Longword  sum_abs;
    Shortword scale;
    Shortword temp1, temp2;
    Longword L_temp;
    Shortword *temp_buf;
	Shortword temp;
	Longword L_temp1;
    
    MEM_ALLOC(MALLOC,temp_buf,npts,Shortword);
    scale = 4;
    v_equ_shr(temp_buf,input,scale,npts);
    L_temp = L_v_magsq(temp_buf,npts,0,1);
//    compare_zero();mark del
    if (L_temp) {
     /*  temp1 = norm_l(L_temp); */
	  L_temp1 = _norm(L_temp);
	  if (L_temp ==0){
	    L_temp1 = (Longword)0;
	  }
	  temp1 = (Shortword) (0x0000ffffL & L_temp1);
	  L_temp1 = _shr2(L_temp1,1);
	  temp = (Shortword) (0x0000ffffL & L_temp1);
      scale = sub(scale,temp);
      if (scale < 0)
	scale = 0;
    }
    else
      scale = 0;
    sum_abs = 0;   // data_move();mark del
    for (i = 0; i < npts; i++) {
      /* L_temp = L_deposit_l(abs_s(input[i])); */
/*       sum_abs = L_add(sum_abs, L_temp);      */
	  L_temp1 = _abs2(input[i]);
	  temp = (Shortword) (0x0000ffffL & L_temp1);
	  L_temp = (Longword)temp;
	  sum_abs = _sadd(sum_abs, L_temp);
    }
    /* Right shift input signal and put in temp buffer */
    /* increment complexity for if statement */
//    compare_zero();mark del
    if (scale)
      v_equ_shr(temp_buf,input,scale,npts);
    /* increment complexity for if statement */
 //   compare_zero();mark del
    if (sum_abs > 0)
      {
	/* peak_fact = sqrt(npts*v_magsq(input,npts)) / sum_abs */
	/*           = sqrt(npts)*(sqrt(v_magsq(input,npts))/sum_abs) */
	/* increment complexity for if statement */
//	compare_zero();mark del
	if (scale)
	  L_temp = L_v_magsq(temp_buf,npts,0,0);
	else
	  L_temp = L_v_magsq(input,npts,0,0);
	L_temp = L_deposit_l(L_sqrt_fxp(L_temp,0));    /* L_temp in Q0 */
	peak_fact = L_divider2(L_temp,sum_abs,0,0);
//	compare_nonzero();mark del
	if (peak_fact > LIMIT_PEAKI) {
	  peak_fact = SW_MAX;    //    data_move();mark del
	}
	else {
	  /* shl 7 is mult , other shift is Q7->Q12 */
	 /*  temp1 = add(scale,5);          */
/* 	  temp2 = shl(npts,7);              */
/* 	  temp2 = sqrt_fxp(temp2,7);        */
/* 	  L_temp = L_mult(peak_fact,temp2); */
/* 	  L_temp = L_shl(L_temp,temp1);     */
/* 	  peak_fact = extract_h(L_temp);    */
      L_temp1 = _sadd2((Longword)scale,(Longword)5);
	  temp1 = (Shortword) (0x0000ffffL & L_temp1);
	  temp2 = shl_a(npts,7);
	  temp2 = sqrt_fxp(temp2,7);
	  L_temp = _smpy(peak_fact,temp2);
	  L_temp = _sshvl(L_temp,temp1);
	  peak_fact = (Shortword) (0x0000ffffL & (L_temp >> 16));
	}
      }
    else
      peak_fact = 0;
    MEM_FREE(FREE,temp_buf);
    return(peak_fact);
} /* peakiness */
#undef LIMIT_PEAKI
/*								*/
/*	Subroutine QUANT_U: quantize positive input value with 	*/
/*	symmetrical uniform quantizer over given positive	*/
/*	input range.						*/
/*								*/
void quant_u(Shortword *p_data, Shortword *p_index, Shortword qmin, 
	     Shortword qmax, Shortword nlev, Shortword nlev_q, 
	     Shortword double_flag, Shortword scale)
{
  Shortword i;
  Shortword step, half_step, qbnd, *p_in;
  Longword L_step, L_half_step, L_qbnd, L_qmin, L_p_in;
  Shortword temp;
  Longword L_temp;
  p_in = p_data;
  /*  Define symmetrical quantizer stepsize	*/
  /* step = (qmax - qmin) / (nlev - 1); */
  temp = sub(qmax,qmin);
  step = divide_s(temp,nlev_q);
  if (double_flag) {
    /* double precision specified */
    /*  Search quantizer boundaries			*/
    /*qbnd = qmin + (0.5 * step); */
    L_step = L_deposit_l(step);
    L_half_step = L_shr(L_step,1);
    L_qmin = L_shl(L_deposit_l(qmin),scale);
    L_qbnd = L_add(L_qmin,L_half_step);
    L_p_in = L_shl(L_deposit_l(*p_in),scale);
    for (i = 0; i < nlev; i++) {
      if (L_p_in < L_qbnd)
	break;
      else
	L_qbnd = L_add(L_qbnd,L_step);
    }
    /* Quantize input to correct level */
    /* *p_in = qmin + (i * step); */
    L_temp = L_sub(L_qbnd,L_half_step);
    *p_in = extract_l(L_shr(L_temp,scale));
    *p_index = i;
  }
  else {
    /*  Search quantizer boundaries			*/
    /* qbnd = qmin + (0.5 * step); */
    step = shr(step,scale);
    half_step = shr(step,1);
    qbnd = add(qmin,half_step);
    for (i = 0; i < nlev; i ++ ) {
      if (*p_in < qbnd)
	break;
      else
	qbnd = add(qbnd,step);
    }
    /*  Quantize input to correct level		*/
    /* *p_in = qmin + (i * step); */
    *p_in = sub(qbnd,half_step);
    *p_index = i;
  }
} /* quant_u */
/*								*/
/*	Subroutine QUANT_U_DEC: decode uniformly quantized	*/
/*	value.							*/
/*								*/
void quant_u_dec(Shortword index, Shortword *p_data, Shortword qmin, 
		 Shortword qmax, Shortword nlev_q, Shortword scale)
{
  Shortword step,temp;
  Longword L_qmin, L_temp;
  Longword L_temp1;
  /*  Define symmetrical quantizer stepsize	*/
  /* step = (qmax - qmin) / (nlev - 1); */
  temp = sub(qmax,qmin);
  step = divide_s(temp,nlev_q);
  /*  Decode quantized level			*/
  /* double precision specified */
  /* L_temp = L_shr(L_mult(step,index),1);     */
/*   L_qmin = L_shl(L_deposit_l(qmin),scale);  */
/*   L_temp = L_add(L_qmin,L_temp);            */
/*   *p_data = extract_l(L_shr(L_temp,scale)); */
   L_temp = _sshvr(_smpy(step,index),1);
   L_qmin = _sshvl((Longword)qmin,scale);
   L_temp = _sadd(L_qmin,L_temp);
   L_temp1 = _sshvr(L_temp,scale);
   *p_data = (Shortword) (0x0000ffffL & L_temp1);
}
/*								*/
/*	Subroutine rand_num: generate random numbers to fill    */
/*      array using "minimal standard" random number generator. */
/*                                                              */
void	rand_num(Shortword output[], Shortword amplitude, Shortword npts)
{
    Shortword i,temp;
    Shortword rand_minstdgen(void);
	Longword L_temp;
    for (i = 0; i < npts; i++ ) {
	/* rand_minstdgen returns 0 <= x < 1 */
        /* -0.5 <= temp < 0.5 */
        temp = sub(rand_minstdgen(),(Shortword)X05_Q15);
/* 	output[i] = mult(amplitude,shl(temp,1));    */
      L_temp = _smpy(amplitude,shl_a(temp,1));
      output[i] = (Shortword) (0x0000ffffL & (L_temp >> 16));
    }
}
/****************************************************************************/
/* RAND() - COMPUTE THE NEXT VALUE IN THE RANDOM NUMBER SEQUENCE.           */
/*                                                                          */
/*     The sequence used is x' = (A*x) mod M,  (A = 16807, M = 2^31 - 1).   */
/*     This is the "minimal standard" generator from CACM Oct 1988, p. 1192.*/
/*     The implementation is based on an algorithm using 2 31-bit registers */
/*     to represent the product (A*x), from CACM Jan 1990, p. 87.           */
/*                                                                          */ 
/****************************************************************************/ 
#define A 16807u                        /* MULTIPLIER VALUE    */
static ULongword next = 1;   /* seed; must not be zero!!! */
Shortword rand_minstdgen()
{
     extern int saturation;
     int old_saturation;
     /* UShortword x0 = extract_l(next);       16 LSBs OF SEED      */
/*      UShortword x1 = extract_h(next);      16 MSBs OF SEED      */
        UShortword x0 = (Shortword) (0x0000ffffL & next);
        UShortword x1 = (Shortword) (0x0000ffffL & (next >> 16));
     ULongword p, q;                       /* MSW, LSW OF PRODUCT */
     ULongword L_temp1,L_temp2;
     ULongword L_mpyu(UShortword var1, UShortword var2);
     /*---------------------------------------------------------------------*/
     /* COMPUTE THE PRODUCT (A * next) USING CROSS MULTIPLICATION OF        */
     /* 16-BIT HALVES OF THE INPUT VALUES.  THE RESULT IS REPRESENTED AS 2  */
     /* 31-BIT VALUES.  SINCE 'A' FITS IN 15 BITS, ITS UPPER HALF CAN BE    */
     /* DISREGARDED.  USING THE NOTATION val[m::n] TO MEAN "BITS n THROUGH  */
     /* m OF val", THE PRODUCT IS COMPUTED AS:                              */
     /*   q = (A * x)[0::30]  = ((A * x1)[0::14] << 16) + (A * x0)[0::30]   */
     /*   p = (A * x)[31::60] =  (A * x1)[15::30]       + (A * x0)[31]  + C */
     /* WHERE C = q[31] (CARRY BIT FROM q).  NOTE THAT BECAUSE A < 2^15,    */
     /* (A * x0)[31] IS ALWAYS 0.                                           */
     /*---------------------------------------------------------------------*/
     /* save and reset saturation count */
     old_saturation = saturation;
     saturation = 0;
     /* q = ((A * x1) << 17 >> 1) + (A * x0); */
     /* p = ((A * x1) >> 15) + (q >> 31); */
     /* q = q << 1 >> 1; */                        /* CLEAR CARRY */
     L_temp1 = L_mpyu(A,x1);
     /* store bit 15 to bit 31 in p */
    /*  p = L_shr(L_temp1,15); */
	  p = _sshvr(L_temp1,15);
     /* mask bit 15 to bit 31 */
    /*  L_temp1 = L_shl((L_temp1 & (Longword)0x00007fff),16); //    L_logic();mark del */
	  L_temp1 = _sshvl((L_temp1 & (Longword)0x00007fff),16); //    L_logic();mark del
     L_temp2 = L_mpyu(A,x0);
     /* q = L_add(L_temp1,L_temp2); */
	 q = _sadd(L_temp1,L_temp2);
 //    compare_zero();mark del
     if (saturation > 0) {
       /* reset saturation */
       saturation = 0;
       /* subtract 0x80000000 from sum */
     /*   L_temp1 = L_sub(L_temp1,(Longword)0x7fffffff); */
/*        L_temp1 = L_sub(L_temp1,1);                    */
/*        q = L_add(L_temp1,L_temp2);                    */
/*        p = L_add(p,1);                                */
       L_temp1 = _ssub(L_temp1,(Longword)0x7fffffff);
       L_temp1 = _ssub(L_temp1,1);
       q = _sadd(L_temp1,L_temp2);
       p = _sadd(p,1);
     }
     /*---------------------------------------------------------------------*/
     /* IF (p + q) < 2^31, RESULT IS (p + q).  OTHERWISE, RESULT IS         */
     /* (p + q) - 2^31 + 1.  (SEE REFERENCES).                              */
     /*---------------------------------------------------------------------*/
     /* p += q; */
     /* next = ((p + (p >> 31)) << 1) >> 1; */ /* ADD CARRY, THEN CLEAR IT */
     L_temp1 = L_add(p,q);
 //    compare_zero();mark del
     if (saturation > 0) {
       /* subtract 0x7fffffff from sum */
       L_temp1 = L_sub(p,(Longword)0x7fffffff);
       L_temp1 = L_add(L_temp1,q);
     }
     next = L_temp1;   //  L_data_move();mark del
     /* restore saturation count */
     saturation = old_saturation;
     x1 = extract_h(next);
     return (x1);
} /* rand_minstdgen */
/***************************************************************************
 *
 *   FUNCTION NAME: L_mpyu
 *
 *   PURPOSE:
 *
 *     Perform an unsigned fractional multipy of the two unsigned 16 bit 
 *     input numbers with saturation.  Output a 32 bit unsigned number.
 *
 *   INPUTS:
 *
 *     var1
 *                     16 bit short unsigned integer (Shortword) whose value
 *                     falls in the range 0xffff 0000 <= var1 <= 0x0000 ffff.
 *     var2
 *                     16 bit short unsigned integer (Shortword) whose value
 *                     falls in the range 0xffff 0000 <= var2 <= 0x0000 ffff.
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     L_Out
 *                     32 bit long unsigned integer (Longword) whose value
 *                     falls in the range
 *                     0x0000 0000 <= L_var1 <= 0xffff ffff.
 *
 *   IMPLEMENTATION:
 *
 *     Multiply the two unsigned 16 bit input numbers. 
 *
 *   KEYWORDS: multiply, mult, mpy
 *
 *************************************************************************/
ULongword L_mpyu(UShortword var1, UShortword var2)
{
  ULongword L_product;
//  extern int complexity; mark del
//  int old_complexity;  mark del
  
//  old_complexity = complexity;  mark del
  L_product = (ULongword) var1 *var2; /* integer multiply */
      
//  complexity = old_complexity + 1; mark del
  return (L_product);
}
/*								*/
/*	Subroutine READBL: read block of input data		*/
/*								*/
#define MAXSIZE 1024
Shortword readbl(Shortword input[], FILE *fp_in, Shortword size)
{
    Shortword i, length;
    Shortword int_sp[MAXSIZE]; /*  integer input array		*/
#ifdef PRINT
    if (size > MAXSIZE) {
	printf("****ERROR: read block size too large **** n");
	exit(1);
    }
#endif
    length = fread(int_sp,sizeof(Shortword),size,fp_in);
    for (i = 0; i < length; i++ )
      input[i] = int_sp[i];
    for (i = length; i < size; i++ )
      input[i] = 0;
    return length;
}
#undef MAXSIZE
/*								*/
/*	Subroutine UNPACK_CODE: Unpack bit code from channel.	*/
/*      Return 1 if erasure, otherwise 0.                       */
/*								*/
Shortword unpack_code(UShortword **p_ch_beg, Shortword *p_ch_bit, 
		      Shortword *p_code, Shortword numbits, Shortword wsize, 
		      UShortword ERASE_MASK)
{
    Shortword ret_code;
    Shortword i,ch_bit;
    UShortword *ch_word;
	Longword L_temp;
	ch_bit = *p_ch_bit;
	ch_word = *p_ch_beg;
	*p_code = 0;
        ret_code = *ch_word & ERASE_MASK;    
	for (i = 0; i < numbits; i++) {
		/* Mask in bit from channel word to code	*/
		*p_code |= shl_a(shr((Shortword)((Shortword)*ch_word & shl(1,ch_bit)),ch_bit),i);
		/* Check for end of channel word		*/
	/* 	ch_bit = add(ch_bit,1);	   */
	    L_temp = _sadd2((Longword)ch_bit,(Longword)1);
		ch_bit = (Shortword) (0x0000ffffL & L_temp);
		if (ch_bit >= wsize) {
			ch_bit = 0;
			(*p_ch_beg)++ ;
			ch_word++ ;
		}
	}
	/*  Save updated bit counter	*/
	*p_ch_bit = ch_bit;
    /* Catch erasure in new word if read */
    if (ch_bit != 0)
      ret_code |= *ch_word & ERASE_MASK;    
    return(ret_code);
}
/*								*/
/*	Subroutine window: multiply signal by window            */
/*								*/
/* Q values:
   input - Q0
   win_cof - Q15
   output - Q0 */
void window(Shortword input[], Shortword win_cof[], 
	    Shortword output[], Shortword npts)
{
    Shortword i;
	Longword L_temp;
    for (i = 0; i < npts; i++ ) {
     /*  output[i] = mult(win_cof[i],input[i]); //   data_move(); */
	   L_temp = _smpy(win_cof[i],input[i]);
       output[i] = (Shortword) (0x0000ffffL & (L_temp >> 16));
    }
}
/*								*/
/*	Subroutine window: multiply signal by window            */
/*								*/
/* Q values:
   win_cof - Qin
   output = input */
void window_Q(Shortword input[], Shortword win_cof[], 
		  Shortword output[], Shortword npts, Shortword Qin)
{
    Shortword i;
    Shortword shift;
	Longword L_temp;
    shift = sub(15,Qin);
    for (i = 0; i < npts; i++ ) {
     /*  output[i] = extract_h(L_shl(L_mult(win_cof[i],input[i]),shift)); */
	  L_temp = _sshvl(_smpy(win_cof[i],input[i]),shift);
      output[i] = (Shortword) (0x0000ffffL & (L_temp >> 16));
	   
    }
}
/*								*/
/*	Subroutine WRITEBL: write block of output data		*/
/*								*/
#define MAXSIZE 1024
void writebl(Shortword output[], FILE *fp_out, Shortword size)
{
    fwrite(output,sizeof(Shortword),size,fp_out);
}
#undef MAXSIZE
/*                                                              */
/*      Subroutine zerflt: all zero (FIR) filter.               */
/*      Note: the output array can overlay the input.           */
/*                                                              */
/* Q values:
   input - Q0
   coeff - Q12
   output - Q0 */
void zerflt(Shortword input[], Shortword coeff[], 
		Shortword output[], Shortword order, Shortword npts)
{
    Shortword i;
    Longword  accum;
    Longword  L_mul,L_Prod,L_temp;
    
    for (i = npts-1; i >= 0; i-- ) {
      accum = 0; //   data_move();mark del
     /*  for (j = 0; j <= order; j++ ){      */
/* 	     L_mul = _smpy(input[i-j],coeff[j]); */
/* 		 accum = _sadd(accum,L_mul);         */
/* 	  }	                                     */
/* 	accum = L_mac(accum, input[i-j],coeff[j]);	  */
      /* Round off output */
   /*    accum = L_shl(accum,3);   */
/*       output[i] = round(accum); */
       L_mul = _smpy(input[i-0],coeff[0]); 
 	   accum = _sadd(accum,L_mul); 
       L_mul = _smpy(input[i-1],coeff[1]); 
 	   accum = _sadd(accum,L_mul); 
	   L_mul = _smpy(input[i-2],coeff[2]); 
 	   accum = _sadd(accum,L_mul); 
	   L_mul = _smpy(input[i-3],coeff[3]); 
 	   accum = _sadd(accum,L_mul); 
	   L_mul = _smpy(input[i-4],coeff[4]); 
 	   accum = _sadd(accum,L_mul); 
	   L_mul = _smpy(input[i-5],coeff[5]); 
 	   accum = _sadd(accum,L_mul); 
       L_mul = _smpy(input[i-6],coeff[6]); 
 	   accum = _sadd(accum,L_mul); 
	   L_mul = _smpy(input[i-7],coeff[7]); 
 	   accum = _sadd(accum,L_mul); 
	   L_mul = _smpy(input[i-8],coeff[8]); 
 	   accum = _sadd(accum,L_mul); 
	   L_mul = _smpy(input[i-9],coeff[9]); 
 	   accum = _sadd(accum,L_mul);
	   L_mul = _smpy(input[i-10],coeff[10]); 
 	   accum = _sadd(accum,L_mul);
       accum = _sshl(accum,3);
	   L_temp = 0x00008000L;
	   L_Prod = _sadd(accum,L_temp);
	   output[i] = (Shortword) (0x0000ffffL & L_Prod >> 16);
    }
}
/*                                                              */
/*      Subroutine zerflt_Q: all zero (FIR) filter.             */
/*      Note: the output array can overlay the input.           */
/*                                                              */
/* Q values:
   coeff - specified by Q_coeff
   output - same as input */
void zerflt_Q(Shortword input[], Shortword coeff[], Shortword output[], 
	      Shortword order, Shortword npts, Shortword Q_coeff)
{
    Shortword i,j,scale;
    Longword  accum;
	Longword L_temp,L_Prod;
    
    scale = sub(15,Q_coeff);
    for (i = npts-1; i >= 0; i-- ) {
      accum = 0; //   data_move();mark del
      for (j = 0; j <= order; j++ ){
/* 	accum = L_mac(accum, input[i-j],coeff[j]);	  */
          accum = _sadd(accum, _smpy(input[i-j],coeff[j]));
    	}
      /* Round off output */
     /*  accum = L_shl(accum,scale); */
	    accum = _sshvl(accum,scale);
     /*  output[i] = round(accum); */
	  L_Prod = _sadd(accum, 0x00008000L); /* round MSP */
      output[i] =(Shortword)(0x0000ffffL & (L_Prod >> 16));
    }
}
/*                                                              */
/*      Subroutine iir_2nd_d: Second order IIR filter           */
/*                            (Double precision)                */
/*      Note: the output array can overlay the input.           */
/*                                                              */
/* Input scaled down by a factor of 2 to prevent overflow */
void iir_2nd_d(Shortword input[],Shortword den[],Shortword num[],
	       Shortword output[],Shortword delin[],Shortword delout_hi[],
	       Shortword delout_lo[],Shortword npts)
{
  Shortword i,temp;
  Longword accum,multemp,L_Prod;
  for (i = 0; i < npts; i++) {
/*  accum = L_mult(delout_lo[0],den[1]);      */
/*  accum = L_mac(accum,delout_lo[1],den[2]); */
/*  accum = L_shr(accum,14);                  */
/*  accum = L_mac(accum,delout_hi[0],den[1]); */
/*  accum = L_mac(accum,delout_hi[1],den[2]); */
    accum = _smpy(delout_lo[0],den[1]);
	multemp = _smpy(delout_lo[1],den[2]);
	accum = _sadd(accum,multemp);
/* 	accum =  L_shr(accum,14);	 */
    accum = _sshvr(accum,14);
	multemp = _smpy(delout_hi[0],den[1]);
	accum = _sadd(accum,multemp);
	multemp = _smpy(delout_hi[1],den[2]);
	accum = _sadd(accum,multemp);
/*     accum = L_mac(accum,shr(input[i],1),num[0]); */
/*     accum = L_mac(accum,delin[0],num[1]);        */
/*     accum = L_mac(accum,delin[1],num[2]);        */
    multemp = (Longword)(input[i]);               
    multemp = _shr2(multemp,1);                   
    temp = (Shortword)(0x0000ffffL & multemp);   
 /*    temp = shr(input[i],1);  */
	multemp = _smpy(temp,num[0]);
	accum = _sadd(accum,multemp);
	multemp = _smpy(delin[0],num[1]);
	accum = _sadd(accum,multemp);
	multemp = _smpy(delin[1],num[2]);
	accum = _sadd(accum,multemp);
    
    /* shift result to correct Q value */
   /*  accum = L_shl(accum,2); */      /* assume coefficients in Q13 */
    accum = _sshl(accum,2);
    /* update input delay buffer */
    delin[1] = delin[0];//    data_move();mark del
/*     delin[0] = shr(input[i],1);  //  data_move();mark del */
    multemp = (Longword)(input[i]);                
    multemp = _shr2(multemp,1);                    
    delin[0] = (Shortword)(0x0000ffffL & multemp);  
    /* update output delay buffer */
/*  delout_hi[1] = delout_hi[0]; //   data_move();mark del */
/* 	delout_lo[1] = delout_lo[0];   // data_move();mark del    */
/*  delout_hi[0] = extract_h(accum);                       */
/* 	temp = shr(extract_l(accum),2);   	                  */
/*  delout_lo[0] = temp & (Shortword)0x3FFF;   // logic();mark del */
    delout_hi[1] = delout_hi[0]; //   data_move();mark del
	delout_lo[1] = delout_lo[0];   // data_move();mark del
    delout_hi[0] = (Shortword) (0x0000ffffL & (accum >> 16));
/* 	temp =  (Shortword) (0x0000ffffL & accum);	*/ 
    multemp = _shr2(accum,2);                      
  	temp = (Shortword) (0x0000ffffL & multemp);	   
    /*temp = shr(temp,2); */
    delout_lo[0] = temp & (Shortword)0x3FFF;   // logic();mark del
    /* round off result */
/*    accum = L_shl(accum,1); */
/*     output[i] = round(accum); */
    accum = _sshl(accum,1);
    temp = 0x00008000L;
    L_Prod = _sadd(temp,accum);
    output[i] = (Shortword) (0x0000ffffL & (L_Prod >> 16));
  }
}
/*                                                              */
/*      Subroutine iir_2nd_s: Second order IIR filter           */
/*                            (Single precision)                */
/*      Note: the output array can overlay the input.           */
/*                                                              */
/* Input scaled down by a factor of 2 to prevent overflow */
/* void iir_2nd_s(Shortword input[],Shortword den[],Shortword num[],        */
/*  	       Shortword output[],Shortword delin[],Shortword delout[],     */
/*  	       Shortword npts)                                              */
/*  {                                                                       */
/*    Shortword i;                                                          */
/*    Longword accum,temp,L_Prod;                                           */
/*                                                                          */
/*    for (i = 0; i < npts; i++) {                                          */
/*     // accum = L_mult(input[i],num[0]);                                */
/*     // accum = L_mac(accum,delin[0],num[1]);                           */
/*      //accum = L_mac(accum,delin[1],num[2]);                           */
/*                                                                          */
/*      //accum = L_mac(accum,delout[0],den[1]);                         */
/*      //accum = L_mac(accum,delout[1],den[2]);                        */
/*  	accum = _smpy(input[i],num[0]);                                     */
/*      temp  = _smpy(delin[0],num[1]);                                     */
/*  	accum = _sadd(accum,temp);                                          */
/*      temp  = _smpy(delin[1],num[2]);                                     */
/*  	accum = _sadd(accum,temp);                                          */
/*  	temp  = _smpy(delout[0],den[1]);                                    */
/*  	accum = _sadd(accum,temp);                                          */
/*  	temp  = _smpy(delout[1],den[2]);                                    */
/*  	accum = _sadd(accum,temp);                                          */
/*  	                                                                    */
/*                                                                          */
/*       shift result to correct Q value */                              
/*      //accum = L_shl(accum,2);       assume coefficients in Q13 */     
/*      accum =_sshl(accum,2);                                              */
/*                                                                          */
/*       update input delay buffer */                                     
/*      delin[1] = delin[0]; //   data_move();mark del                  */  
/*      delin[0] = input[i];   // data_move();mark del                  */ 
/*  	printf("%dn",delin[0]);*/                                          
/*                                                                          */
/*     round off result */                                             
/*      //output[i] = round(accum);                                     */  
/*      temp = 0x00008000L;                                                 */
/*      L_Prod = _sadd(temp,accum);                                         */
/*     output[i] = (Shortword) (0x0000ffffL & (L_Prod >> 16));              */
/*                                                                          */
/*       update output delay buffer */                                    
/*      delout[1] = delout[0];  //  data_move();mark del                */  
/*      delout[0] = output[i];  //  data_move();mark del                */  
/*    }                                                                     */
/*  }                                                                       */                                                               

						