do_zfilt2.c

手机加密通话软件

/* Copyright 2001,2002,2003 NAH6
 * All Rights Reserved
 *
 * Parts Copyright DoD, Parts Copyright Starium
 *
 */
#include "celpfilt.h"
//#include <assert.h>

/**************************************************************************
*
* ROUTINE
*               do_zfilt
*               do_zfilt_dynamic
*
* FUNCTION
*               pole filter - given a filter structure
*               do_zfilt assumes static coefficients
*               do_zfilt_dynamic assumes changing coefficients
*
* SYNOPSIS
*               subroutine do_zfilt(flt, coef, data)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       flt             FILTER  i/o     FILTER structure
*       coef            fxpt_16  i      coefficients
*       data            fxpt_xx i/o     input/filtered output data
*
***************************************************************************
*
* DESCRIPTION
*
*       Nonrecursive all-zero in-place time-domain filter.
*       The filter is implemented in a direct form realisation.
*
*               N       -i
*       H(z) = SUM b(i)z
*              i=0
*
*       x(t) ->---(z0)----- b0 >------+-----> y(t)
*                  |                  |
*                  z1------ b1 >------+
*                  |                  |
*                  z2------ b2 >------+
*                  |                  |
*                  :                  :
*                  |                  |
*                  zN------ bN >------+
*
*
***************************************************************************
*
* CALLED BY
*
*       FindAdaptResidual FindLPCResidual UpdateEverything
*       HPF_InSpeech HPF_OutSpeech PostFilter
*
***************************************************************************/


/* flt->memory          16.15 format */
/* flt->coef            2.13 format */

// we need to go 17.14 due to overflows!!!

#if 1

void do_zfilt(
  BIGFILTER *flt,
  fxpt_16 indata[],     /* 15.0  format */
  fxpt_32 outdata[])    /* 17.14 format */
{
  fxpt_16 *in;
  fxpt_32 *out;
  fxpt_32 m0, m1, m2;
  fxpt_32 c0, c1, c2;

  fxpt_64 ar;   /* x.27 format */

  ASSERT(flt->data_length == 240);
  ASSERT(flt->order == 2);

#if 1
  c0 = flt->coef[0];
  c1 = flt->coef[1];
  c2 = flt->coef[2];

  m1 = flt->memory[1];
  m2 = flt->memory[2];
#else
  c0 = flt->coef[0] << 3;
  c1 = flt->coef[1] << 3;
  c2 = flt->coef[2] << 3;

  m1 = flt->memory[1] >> 16;
  m2 = flt->memory[2] >> 16;
#endif

  in = indata;
  out = outdata;

  do {
#if 1
    m0 = ((fxpt_32)*in++) << 14;

    ar= (fxpt_64)c2 * m2;
    ar += (fxpt_64)c1 * m1;
    ar += (fxpt_64)c0 * m0;
    m2 = m1;
    m1 = m0;
//    *out++ = (fxpt_32)(ar>>29);
    *out++ = fxpt_saturate32_round(ar,29);
#else
    ar = 0;
    m0 = *in++ >> 1;

    ar += c2 * m2;
    m2 = m1;

    ar += c1 * m1;
    m1 = m0;

    ar += c0 * m0;

    *out++ = ar;
#endif
  } while (out != &outdata[240]);

#if 0
  m0 = m1 = indata[239] << 15;
  m2      = indata[238] << 15;
#endif
  flt->memory[0] = m0;
  flt->memory[1] = m1;
  flt->memory[2] = m2;
}

#endif