dsp_sub.c

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

	output[i] = mult(amplitude,shl(temp,1));
    }
}

/****************************************************************************/
/* 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     */
     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);
     /* mask bit 15 to bit 31 */
     L_temp1 = L_shl((L_temp1 & (Longword)0x00007fff),16);     L_logic();
     L_temp2 = L_mpyu(A,x0);
     q = L_add(L_temp1,L_temp2);
     compare_zero();
     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);
     }

     /*---------------------------------------------------------------------*/
     /* 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();
     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();

     /* 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;
  int old_complexity;
  
  old_complexity = complexity;

  L_product = (ULongword) var1 *var2; /* integer multiply */
      
  complexity = old_complexity + 1;
  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;

	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(shr((Shortword)((Shortword)*ch_word & shl(1,ch_bit)),ch_bit),i);

		/* Check for end of 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;


    /* 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;

    for (i = 0; i < npts; i++ ) {
      output[i] = mult(win_cof[i],input[i]);    data_move();
    }
}



/*								*/
/*	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;

    shift = sub(15,Qin);
    for (i = 0; i < npts; i++ ) {
      output[i] = extract_h(L_shl(L_mult(win_cof[i],input[i]),shift));
    }
}


/*								*/
/*	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,j;
    Longword  accum;
    
    
    for (i = npts-1; i >= 0; i-- ) {
      accum = 0;    data_move();
      for (j = 0; j <= order; j++ )
	accum = L_mac(accum, input[i-j],coeff[j]);
      /* Round off output */
      accum = L_shl(accum,3);
      output[i] = round(accum);
    }
}


/*                                                              */
/*      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;
    
    scale = sub(15,Q_coeff);
    for (i = npts-1; i >= 0; i-- ) {
      accum = 0;    data_move();
      for (j = 0; j <= order; j++ )
	accum = L_mac(accum, input[i-j],coeff[j]);
      /* Round off output */
      accum = L_shl(accum,scale);
      output[i] = round(accum);
    }
}


/*                                                              */
/*      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;

  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 = 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]);

    /* shift result to correct Q value */
    accum = L_shl(accum,2);      /* assume coefficients in Q13 */

    /* update input delay buffer */
    delin[1] = delin[0];    data_move();
    delin[0] = shr(input[i],1);    data_move();

    /* update output delay buffer */
    delout_hi[1] = delout_hi[0];    data_move();
    delout_lo[1] = delout_lo[0];    data_move();
    delout_hi[0] = extract_h(accum);
    temp = shr(extract_l(accum),2);
    delout_lo[0] = temp & (Shortword)0x3FFF;    logic();

    /* round off result */
    accum = L_shl(accum,1);
    output[i] = round(accum);
  }
}

/*                                                              */
/*      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;

  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]);

    /* shift result to correct Q value */
    accum = L_shl(accum,2);      /* assume coefficients in Q13 */

    /* update input delay buffer */
    delin[1] = delin[0];    data_move();
    delin[0] = input[i];    data_move();

    /* round off result */
    output[i] = round(accum);

    /* update output delay buffer */
    delout[1] = delout[0];    data_move();
    delout[0] = output[i];    data_move();
  }
}