dsp_sub.c

MELPe 1200 bps, fixed point

	/*----------------------------------------------------------------------*/
	/* 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_temp2 = L_mpyu(A, x0);
	L_temp3 = L_sub(LW_MAX, L_temp1);
	if (L_temp2 > L_temp3){
		/* 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);
	} else
		q = L_add(L_temp1, L_temp2);

	/*---------------------------------------------------------------------*/
	/* 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_temp3 = L_sub(LW_MAX, p);
	if (q > L_temp3){
		/* subtract 0x7fffffff from sum */
		L_temp1 = L_sub(p, (Longword) 0x7fffffff);
		L_temp1 = L_add(L_temp1, q);
	} else
		L_temp1 = L_add(p, q);
	next = L_temp1;

	/* restore saturation count */
	saturation = old_saturation;

	x1 = extract_h(next);
	return (x1);
}


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

static ULongword	L_mpyu(UShortword var1, UShortword var2)
{
	ULongword	L_product;


	L_product = (ULongword) var1 *var2;                   /* integer multiply */
	return (L_product);
}


/* ========================================================================== */
/* This function reads a block of input data.  It returns the actual number   */
/*    of samples read.  Zeros are filled into input[] if there are not suffi- */
/*    cient data samples.                                                     */
/* ========================================================================== */
Shortword readbl(Shortword input[], FILE *fp_in, Shortword size)
{
	Shortword	length;


	length = (Shortword) fread(input, sizeof(Shortword), size, fp_in);
	v_zap(&(input[length]), (Shortword) (size - length));

	return(length);
}


/* ========================================================================== */
/* This function unpacks bits of "code" from channel.  It returns 1 if an     */
/*    erasure is encountered, or 0 otherwise.  "numbits" bits of "code" */
/*    is used and they are packed into the array pointed by "ptr_ch_begin".   */
/*    "ptr_ch_bit" points to the position of the next bit being copied onto.  */
/* ========================================================================== */
BOOLEAN	unpack_code(unsigned char **ptr_ch_begin, Shortword *ptr_ch_bit,
					Shortword *code, Shortword numbits, Shortword wsize,
					UShortword erase_mask)
{
	register Shortword	i;
	unsigned char	*ch_word;
	Shortword	ret_code;
	Shortword	ch_bit;


	ch_bit = *ptr_ch_bit;
	ch_word = *ptr_ch_begin;
	*code = 0;
	ret_code = (Shortword) (*ch_word & erase_mask); 

	for (i = 0; i < numbits; i++){
		/* Mask in bit from channel word to code */
		*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;
			(*ptr_ch_begin) ++;
			ch_word++ ;
		}
	}

	/* Save updated bit counter */
	*ptr_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_coeff - Q15, output - Q0  */
/* ============================================ */

void window(Shortword input[], const Shortword win_coeff[], Shortword output[],
			Shortword npts)
{
	register Shortword	i;

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


/* ============================================== */
/* Subroutine window_Q: multiply signal by window */
/*                                                */
/* Q values:                                      */
/*    win_coeff - Qin, output = input             */
/* ============================================== */

void window_Q(Shortword input[], Shortword win_coeff[], Shortword output[],
			  Shortword npts, Shortword Qin)
{
	register Shortword	i;
	Shortword	shift;

	/* After computing "shift", win_coeff[]*2^(-shift) is considered Q15.     */
	shift = sub(15, Qin);
	for (i = 0; i < npts; i++){
		output[i] = extract_h(L_shl(L_mult(win_coeff[i], input[i]), shift));
	}
}


/* ============================================ */
/* This function writes a block of output data. */
/* ============================================ */
void writebl(Shortword output[], FILE *fp_out, Shortword size)
{
	fwrite(output, sizeof(Shortword), size, fp_out);
}


/* Subroutine polflt(): all pole (IIR) filter.                                */
/*   Note: The filter coefficients (Q13) represent the denominator only, and  */
/*         the leading coefficient is assumed to be 1.  The output array can  */
/*         overlay the input.                                                 */
/* Q value:                                                                   */
/*   input[], output[]: Q0, coeff[]: Q12                                      */

void polflt(Shortword input[], Shortword coeff[], Shortword output[],
			Shortword order, Shortword npts)
{
	register Shortword	i, j;
	Longword	accum;                                                 /* Q12 */


	for (i = 0; i < npts; i++ ){
		accum = L_shl(L_deposit_l(input[i]), 12);
		for (j = 1; j <= order; j++)
			accum = L_msu(accum, output[i - j], coeff[j]);
		/* Round off output */
		accum = L_shl(accum, 3);
		output[i] = round(accum);
	}
}


/* Subroutine zerflt(): all zero (FIR) filter.                                */
/*   Note: the output array can overlay the input.                            */
/*                                                                            */
/* Q values:                                                                  */
/*   input[], output[] - Q0, coeff[] - Q12                                    */

void zerflt(Shortword input[], const Shortword coeff[], Shortword output[],
			Shortword order, Shortword npts)
{
	register Shortword	i, j;
	Longword	accum;


	for (i = sub(npts, 1); i >= 0; i--){ 
		accum = 0;
		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[], const Shortword coeff[], Shortword output[],
			  Shortword order, Shortword npts, Shortword Q_coeff)
{
	register Shortword	i, j;
	Shortword	scale;
	Longword	accum;


	scale = sub(15, Q_coeff);
	for (i = sub(npts, 1); i >= 0; i--){
		accum = 0;
		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[], const Shortword den[], const Shortword num[],
			   Shortword output[], Shortword delin[], Shortword delout_hi[],
			   Shortword delout_lo[], Shortword npts)
{
	register Shortword	i;
	Shortword	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];
		delin[0] = shr(input[i], 1);

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

		/* 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[], const Shortword den[], const Shortword num[],
			   Shortword output[], Shortword delin[], Shortword delout[],
			   Shortword npts)
{
	register 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];
		delin[0] = input[i];

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

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


/* ========================================================================== */
/* This function interpolates two scalars to obtain the output.  It calls     */
/* interp_array() to do the job.  "ifact" is Q15, and "prev", "curr" and the  */
/* returned value are of the same Q value.                                    */
/* ========================================================================== */

Shortword interp_scalar(Shortword prev, Shortword curr, Shortword ifact)
{
	Shortword	out;


	interp_array(&prev, &curr, &out, ifact, 1);
	return(out);
}