iir-lib.c

Reference Implementation of G.711 standard and other voice codecs

	T[n][0] = T[n][1];
	T[n][1] = Ttmp;
      }
    }
    (*k0)++;
  }
  *k0 %= idown;			  /* avoid overflow by (*k0)++ */
  return ky;
}
/* .............. End of scd_parallel_form_iir_down_kernel() .............. */



/*
  ============================================================================

        long scd_parallel_form_iir_up_kernel(long lenx, float *x,
        ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ float *y, long *k0,
                                               long idown, long nblocks,
                                               double direct_cof,
                                               double gain, float (*b)[3],
                                               float (*c)[2], float (*T)[2]);

        Description:
        ~~~~~~~~~~~~

        Function for filtering a sequence of input samples by a
        parallel-form IIR-filter with up-sampling.

        Parameters:
        ~~~~~~~~~~~
        lenx: ........ (In) length of input array x[]
        x: ........... (In) array with input samples
        y: ........... (Out) array with output samples
        iup: ......... (In) up-sampling factor
        nblocks: ..... (In) number of coeff. sets
        direct_cof: .. (In) direct path coefficient
        gain: ........ (In) gain factor
        b: ........... (In) numerator coefficients
        c: ........... (In) denominator coefficients
        T: ........... (In/Out) state variables

        Return value:
        ~~~~~~~~~~~~~
        Returns the number of samples filtered.

        Author: <[e-mail]>
        ~~~~~~~

        History:
        ~~~~~~~~
        28.Feb.92 v1.0 Release of 1st version <[e-mail]>

 ============================================================================
*/
static long     scd_parallel_form_iir_up_kernel(lenx, x, y, iup, nblocks,
				                  direct_cof, gain, b, c, T)
  long            lenx;
  float          *x, *y;
  long            iup, nblocks;
  double          direct_cof, gain;
  float           (*b)[3], (*c)[2], (*T)[2];
{
  long            kx, ky, n;
  float           Ttmp;


  kx = 0;			  /* starting index in input array (x) */
  for (ky = 0; ky < iup * lenx; ky++)	/* loop over all input samples */
  {
    if (ky % iup == 0)		  /* compute output only every "iup" */
    {				  /* samples by taking one input sample */
      y[ky] = direct_cof * x[kx]; /* direct path */
      for (n = 0; n < nblocks; n++)	/* loop over all second order filter */
      {
	Ttmp = 2. * (x[kx] - c[n][0] * T[n][0] - c[n][1] * T[n][1]);
	y[ky] += b[n][2] * Ttmp + b[n][1] * T[n][1] + b[n][0] * T[n][0];
	T[n][0] = T[n][1];
	T[n][1] = Ttmp;
      }
      y[ky] *= gain;
      kx++;
    }
    else
    {
      y[ky] = 0.0;		  /* at other instants feed zero-valued
				   * samples */
      for (n = 0; n < nblocks; n++)
      {
	Ttmp = 2. * (0.0 - c[n][0] * T[n][0] - c[n][1] * T[n][1]);
	y[ky] += b[n][2] * Ttmp + b[n][1] * T[n][1] + b[n][0] * T[n][0];
	T[n][0] = T[n][1];
	T[n][1] = Ttmp;
      }
      y[ky] *= gain;
    }
  }
  return ky;
}
/* ............... End of scd_parallel_form_iir_up_kernel() ............... */



/* *************************************************************************
   ******** THE ROUTINES TO FOLLOW HAVE BEEN ADDED AFTER THE STL92 *********
 * ************************************************************************* */

/*
  ============================================================================

  void cascade_iir_reset (CASCADE_IIR *iir_ptr);
  ~~~~~~~~~~~~~~~~~~~~~~
  
  Description:
  ~~~~~~~~~~~~

  Clear state variables in CASCADE_IIR structure, what has been
  initialized by a previous call to one of the init functions.

  Parameters:
  ~~~~~~~~~~~
  CASCADE_IIR *iir_ptr: ... pointer to struct CASCADE_IIR previously 
                            initialized by a call to one of the initialization 
                            routines.

  Return value:
  ~~~~~~~~~~~~~
  Nothing.

  Author: <[e-mail]>
  ~~~~~~~

  History:
  ~~~~~~~~
  30.Oct.94 v1.0 Release of 1st version <[e-mail]>

 ============================================================================
*/
void            cascade_iir_reset(iir_ptr)
  CASCADE_IIR        *iir_ptr;
{
  long            n;
  float           (*T_ptr)[4];


  T_ptr = iir_ptr->T;
  for (n = 0; n < iir_ptr->nblocks; n++)
  {
    T_ptr[n][0] = 0.0;
    T_ptr[n][1] = 0.0;
    T_ptr[n][2] = 0.0;
    T_ptr[n][3] = 0.0;
  }

  iir_ptr->k0 = iir_ptr->idown;	  /* modulo counter for down-sampling */
}
/* .................... End of cascade_iir_reset() ...................... */


/*
  ============================================================================

  long cascade_iir_kernel (long lseg, float *x_ptr, CASCADE_IIR *iir_ptr,
  ~~~~~~~~~~~~~~~~~~~~~~~  float *y_ptr);

  Description:
  ~~~~~~~~~~~~

  Basic cascade-form IIR filtering routine, for both up- and 
  down-sampling.

  Parameters:
  ~~~~~~~~~~~
  lseg: ...... number of input samples
  x_ptr: ..... array with input samples
  iir_ptr: ... pointer to IIR-struct (CASCADE_IIR *)
  y_ptr: ..... output samples

  Return value:
  ~~~~~~~~~~~~~
  Returns the number of output samples.
  
  Author: <[e-mail]>
  ~~~~~~~
  
  History:
  ~~~~~~~~
  30.Oct.94 v1.0 Release of 1st version <[e-mail]>

 ============================================================================
*/
long            cascade_iir_kernel(lseg, x_ptr, iir_ptr, y_ptr)
  long            lseg;
  float          *x_ptr;
  CASCADE_IIR    *iir_ptr;
  float          *y_ptr;
{
  if (iir_ptr->hswitch == 'U')
    return
      cascade_form_iir_up_kernel( /* returns number of output samples */
				  lseg,	          /* In : input signal leng. */
				  x_ptr,	  /* In : input sample array */
				  y_ptr,	  /* Out: outp. sample array */
				  iir_ptr->idown, /* In : dwnsmpl.factor */
				  iir_ptr->nblocks, /*In: no.IIR-coeffs */
				  iir_ptr->gain,  /* In : gain factor*/
				  iir_ptr->a,	  /* In : num.coeffs */
				  iir_ptr->b,	  /* In : denom.coeffs */
				  iir_ptr->T	  /* I/O: state vars */
      );
  else
    return
      cascade_form_iir_down_kernel(/* returns number of output samples */
				   lseg,	  /* In : input signal leng. */
				   x_ptr,	  /* In : input sample array */
				   y_ptr,	  /* Out: outp. sample array */
				   &(iir_ptr->k0), /*I/O: start idx,x-array */
				   iir_ptr->idown, /*In : dwnsmpl.factor */
				   iir_ptr->nblocks, /*In:no.of IIR-coeffs */
				   iir_ptr->gain, /* In : gain factor */
				   iir_ptr->a,	  /* In : numerator coeffs */
				   iir_ptr->b,	  /* In : denom.coeffs */
				   iir_ptr->T	  /* I/O: state vars */
      );
}
/* .................... End of cascade_iir_kernel() ....................... */


/*
  ============================================================================

  long cascade_form_iir_down_kernel(long lenx, float *x,
  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ float *y, long *k0,
                                    long idown, long nblocks,
                                    double gain, float (*a)[2],
                                    float (*b)[2], float (*T)[4]);
  
  Description:
  ~~~~~~~~~~~~

  Function for filtering a sequence of input samples by a
  cascade-form IIR-filter with down-sampling.
  
  Parameters:
  ~~~~~~~~~~~
  lenx: ........ (In) length of input array x[]
  x: ........... (In) array with input samples
  y: ........... (Out) array with output samples
  k0: .......... (In/Out) pointer to modulo counter
  idown: ....... (In) down-sampling factor
  nblocks: ..... (In) number of coeff. sets
  gain: ........ (In) gain factor
  b: ........... (In) numerator coefficients
  c: ........... (In) denominator coefficients
  T: ........... (In/Out) state variables
  
  Return value:
  ~~~~~~~~~~~~~
  Returns the number of samples filtered.
  
  Author: <[e-mail]>
  ~~~~~~~
  
  History:
  ~~~~~~~~
  30.Oct.94 v1.0 Release of 1st version <[e-mail]>

 ============================================================================
*/
static long     cascade_form_iir_down_kernel(lenx, x, y, k0, idown, 
                                             nblocks, gain, a, b, T)
  long            lenx;
  float          *x, *y;
  long           *k0, idown, nblocks;
  double          gain;
  float           (*a)[2], (*b)[2], (*T)[4];
{
  long            kx, ky, n;
  double   xj,yj;


  ky = 0;			  /* starting index in output array (y) */
  for (kx = 0; kx < lenx; kx++)	  /* loop over all input samples */
  {
    xj = x[kx]; /* direct path */
    for (n = 0; n < nblocks; n++)	/* loop over all second order filter */
    {
      yj =  xj + a[n][0] * T[n][0] + a[n][1] * T[n][1];
      yj -= (b[n][0] * T[n][2] + b[n][1] * T[n][3]);

      /* Save samples in memory */
      T[n][1] = T[n][0];
      T[n][0] = xj;
      T[n][3] = T[n][2];
      T[n][2] = yj;

      /* The yj of this stage is the xj of the next */
      xj = yj;
    }

    if (*k0 % idown == 0)	  /* compute output only every "idown"
				   * samples */
    {
      /* Apply gain and update y-samples' counter */
      y[ky] = yj * gain;
      ky++;
    }
    (*k0)++;
  }
  *k0 %= idown;			  /* avoid overflow by (*k0)++ */
  return ky;
}
/* .............. End of cascade_form_iir_down_kernel() .............. */


/*
  ============================================================================

  long cascade_form_iir_up_kernel(long lenx, float *x,
  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ float *y, long *k0,
                                  long idown, long nblocks,
                                  double gain, float (*a)[2],
                                  float (*b)[2], float (*T)[4]);

  Description:
  ~~~~~~~~~~~~

  Function for filtering a sequence of input samples by a
  cascade-form IIR-filter with up-sampling.

  Parameters:
  ~~~~~~~~~~~
  lenx: ........ (In) length of input array x[]
  x: ........... (In) array with input samples
  y: ........... (Out) array with output samples
  iup: ......... (In) up-sampling factor
  nblocks: ..... (In) number of coeff. sets
  gain: ........ (In) gain factor
  b: ........... (In) numerator coefficients
  c: ........... (In) denominator coefficients
  T: ........... (In/Out) state variables
  
  Return value:
  ~~~~~~~~~~~~~
  Returns the number of samples filtered.
  
  Author: <[e-mail]>
  ~~~~~~~
  
  History:
  ~~~~~~~~
  30.Oct.94 v1.0 Release of 1st version <[e-mail]>

 ============================================================================
*/
static long     cascade_form_iir_up_kernel(lenx, x, y, iup, nblocks,
					   gain, a, b, T)
  long            lenx;
  float          *x, *y;
  long            iup, nblocks;
  double          gain;
  float           (*a)[2], (*b)[2], (*T)[4];
{
  long            kx, ky, n;
  double xj, yj;

  kx = 0;			  /* starting index in input array (x) */
  for (ky = 0; ky < iup * lenx; ky++)	/* loop over all input samples */
  {
    /* Compute output only every "iup" compute output only every "iup" 
     * samples by taking one input sample direct path OR by using a 
     * zero-valued sample */
    if (ky % iup == 0)		  
      xj = x[kx];                
    else                          
      xj = 0.;

    /* Filter samples through all cascade stages */
    for (n = 0; n < nblocks; n++)    
    {
      yj =  xj + a[n][0] * T[n][0] + a[n][1] * T[n][1];
      yj -= (b[n][0] * T[n][2] + b[n][1] * T[n][3]);

      /* Save samples in memory */
      T[n][1] = T[n][0];
      T[n][0] = xj;
      T[n][3] = T[n][2];
      T[n][2] = yj;

      /* The yj of this stage is the xj of the next */
      xj = yj;
    }
 
    /* Apply the gain and update x counter if needed */
    y[ky] = yj * gain;
    if (ky % iup == 0) kx++;
  }
  return ky;
}
/* ............... End of cascade_form_iir_up_kernel() ............... */


/*
  ============================================================================

  CASCADE_IIR *cascade_iir_init (long nblocks, float (*a)[2],
  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~  float (*b)[2], double gain, 
	                         long idwnup, char hswitch);

  Description:
  ~~~~~~~~~~~~
  Allocate & initialize struct for down/up-sampling procedures.

  Parameters:
  ~~~~~~~~~~~
  long nblocks: ....... number of 2'nd order blocks
  float (*a)[2]: ...... pointer to numerator coeffic.
  float (*b)[2]: ...... pointer to denominator coeffic.
  double gain: ........ gain factor for filter
  long idown: ......... Down-sampling factor
  char hswitch: ....... to up-/downsampling routine

  Return value:
  ~~~~~~~~~~~~~
  Returns a pointer to struct CASCADE_IIR;

  Author: <[e-mail]>
  ~~~~~~~
  
  History:
  ~~~~~~~~
  30.Oct.94 v1.0 Release of 1st version <[e-mail]>

 ============================================================================
*/
CASCADE_IIR *cascade_iir_init(nblocks, a, b, gain, idown, hswitch)
  long            nblocks;
  float           (*a)[2], (*b)[2];
  double          gain;
  long            idown;
  char            hswitch;
{
  static CASCADE_IIR *ptrIIR;	  /* pointer to the new struct */
  float           fak;
  float           (*T_ptr)[4];
  long            n;


  /* Allocate memory for a new struct */
  ptrIIR = (CASCADE_IIR *) malloc((long) sizeof(CASCADE_IIR));
  if (ptrIIR == (CASCADE_IIR *) 0L)
  {
    return 0;
  }


  /* Allocate memory for state variables */
  if ((ptrIIR->T = (float (*)[4]) malloc((nblocks * 4) * sizeof(fak)))
      == (float (*)[4]) 0)
  {
    free(ptrIIR);
    return 0;
  }


  /* fill coefficient sets */
  ptrIIR->nblocks = nblocks;	  /* store number of 2'nd order blocks */
  ptrIIR->a = a;
  ptrIIR->b = b;


  /* store down-sampling factor/gain/direct-path coefficient */
  ptrIIR->idown = idown;
  ptrIIR->gain = gain;


  /* Store switch to IIR-kernel procedure */
  ptrIIR->hswitch = hswitch;

  /* Clear state variables */
  T_ptr = ptrIIR->T;
  for (n = 0; n < nblocks; n++)
  {
    T_ptr[n][0] = 0.0;
    T_ptr[n][1] = 0.0;
    T_ptr[n][2] = 0.0;
    T_ptr[n][3] = 0.0;
  }

  ptrIIR->k0 = idown;		  /* modulo counter for down-sampling */


  /* Exit returning pointer to struct */
  return (ptrIIR);