dsp_sub.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].

*/
/*  

  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;

    if (ifact == 0) {
      for (i = 0; i < size; i++) {
	out[i] = prev[i];     data_move();
      }
    }
    else if (ifact == ONE_Q15) {
      for (i = 0; i < size; i++) {
	out[i] = curr[i];     data_move();
      }
    }
    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);
      }
    }
}


/*								*/
/*	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();
	    if (sorted[i] < sorted[j])
	      break;
	}

	/* insert new value */
	loc = j;      data_move();
	insert_val = sorted[i];     data_move();
	for (j = i; j > loc; j--) {
	  sorted[j] = sorted[j-1];     data_move();
	}
	sorted[loc] = insert_val;     data_move();
    }

    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;
    
    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();
    if (L_temp) {
      temp1 = norm_l(L_temp);
      scale = sub(scale,shr(temp1,1));
      if (scale < 0)
	scale = 0;
    }
    else
      scale = 0;

    sum_abs = 0;    data_move();
    for (i = 0; i < npts; i++) {
      L_temp = L_deposit_l(abs_s(input[i]));
      sum_abs = L_add(sum_abs, L_temp);
    }

    /* Right shift input signal and put in temp buffer */
    /* increment complexity for if statement */
    compare_zero();
    if (scale)
      v_equ_shr(temp_buf,input,scale,npts);

    /* increment complexity for if statement */
    compare_zero();
    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();
	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();
	if (peak_fact > LIMIT_PEAKI) {
	  peak_fact = SW_MAX;        data_move();
	}
	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);
	}
      }
    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;

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

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

    for (i = 0; i < npts; i++ ) {

	/* rand_minstdgen returns 0 <= x < 1 */
        /* -0.5 <= temp < 0.5 */
        temp = sub(rand_minstdgen(),(Shortword)X05_Q15);