do_zfilt.c

手机加密通话软件

/* Copyright 2001,2002,2003 NAH6
 * All Rights Reserved
 *
 * Parts Copyright DoD, Parts Copyright Starium
 *
 */
/**************************************************************************
*
* 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
*
***************************************************************************/

#include "celpfilt.h"

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

#if 0  /* function now isolated in do_zfilt2.c */
void do_zfilt(
FILTER *flt,
fxpt_16 indata[],       /* 15.0  format */
fxpt_32 outdata[])      /* 16.15 format */
{
        fxpt_32 ar;     /* 16.15 format */
        int t, j;

FXPT_PUSHSTATE("do_zfilt", -1.0, -1.0);
        for (t = 0; t < flt->data_length; t++) {
                flt->memory[0] = fxpt_shr32_fast(fxpt_deposit_h(indata[t]), 1);
                ar = 0;
                for (j = flt->order; j > 0; j--) {
                        /*ar  += flt->memory[j] * flt->coef[j];*/
//                        ar = fxpt_add32(ar,
//                            fxpt_shl32_fast(fxpt_mult32(flt->coef[j],
//                            fxpt_extract_h(flt->memory[j])), 2));
                        /*ar = fxpt_add16(ar,
                            fxpt_mult16_fix(flt->memory[j], flt->coef[j], 13));*/
                        ar = fxpt_add32(ar,
                            fxpt_mult64_fix( fxpt_deposit_l(flt->coef[j]),
							flt->memory[j], 13 ) );
                        flt->memory[j] = flt->memory[j-1];
                }
                /*data[t] = ar + flt->memory[0] * flt->coef[0];*/
//                ar = fxpt_add32(ar,
//                    fxpt_shl32_fast(fxpt_mult32(flt->coef[0],
//                    fxpt_extract_h(flt->memory[0])), 2));
                ar = fxpt_add32(ar,
                    fxpt_mult64_fix( fxpt_deposit_l(flt->coef[0]),
                    flt->memory[0], 13) );
                outdata[t] = ar;
                /*data[t] = fxpt_extract_h(fxpt_shl32_fast(ar, 1));*/
                /*data[t] = fxpt_add16(ar,
                    fxpt_mult16_fix(flt->memory[0], flt->coef[0], 13));*/
        }
FXPT_POPSTATE();
}
#endif


void do_zfilt_dynamic(
FILTER  *flt,
fxpt_16 coef[],         /* 2.13 format */
fxpt_16 data[])         /* 15.0 format */
{
        fxpt_32 ar;
        int t, j;

FXPT_PUSHSTATE("do_zfilt_dynamic", -1.0, -1.0);
        for (t = 0; t < flt->data_length; t++) {
                flt->memory[0] = fxpt_shr32_fast(fxpt_deposit_h(data[t]), 1);
                ar = 0;
                for (j = flt->order; j > 0; j--) {
                        /*ar  += flt->memory[j] * coef[j];*/
                        ar = fxpt_add32(ar,
                            fxpt_shl32_fast(fxpt_mult32(coef[j],
                            fxpt_extract_h(flt->memory[j])), 2));
                        /*ar = fxpt_add16(ar,
                            fxpt_mult16_fix(flt->memory[j], coef[j], 13));*/
                        flt->memory[j] = flt->memory[j-1];
                }
                /*data[t] = ar + flt->memory[0] * coef[0];*/
                ar = fxpt_add32(ar,
                    fxpt_shl32_fast(fxpt_mult32(coef[0],
                    fxpt_extract_h(flt->memory[0])), 2));
                data[t] = fxpt_extract_h(fxpt_shl32_fast(ar, 1));
                /*data[t] = fxpt_add16(ar,
                    fxpt_mult16_fix(flt->memory[0], coef[0], 13));*/
        }
FXPT_POPSTATE();
}

#if 0  /* routine now in its own file */
void do_zfilt_dynamic16to32(
FILTER  *flt,
fxpt_16 coef[],         /*  2.13 format */
fxpt_16 indata[],       /* 15.0  format */
fxpt_32 outdata[])      /* 16.15 format */
{
        fxpt_32 ar;     /* 16.15 format */
        int t, j;

FXPT_PUSHSTATE("do_zfilt_dynamic16to32", -1.0, -1.0);
        for (t = 0; t < flt->data_length; t++) {
                flt->memory[0] = fxpt_shr32_fast(fxpt_deposit_h(indata[t]), 1);
                ar = 0;
                for (j = flt->order; j > 0; j--) {
                        /*ar  += flt->memory[j] * coef[j];*/
                        ar = fxpt_add32(ar,
                            fxpt_shl32_fast(fxpt_mult32(coef[j],
                            fxpt_extract_h(flt->memory[j])), 2));
                        flt->memory[j] = flt->memory[j-1];
                }
                /*data[t] = ar + flt->memory[0] * coef[0];*/
                ar = fxpt_add32(ar,
                    fxpt_shl32_fast(fxpt_mult32(coef[0],
                    fxpt_extract_h(flt->memory[0])), 2));
                outdata[t] = ar;
        }
FXPT_POPSTATE();
}
#endif


#if 0  /* routine now in its own file */
void do_zfilt_dynamic32to32(
FILTER  *flt,
fxpt_16 coef[],         /*  2.13 format */
fxpt_32 data[])         /* 16.15 format */
{
        fxpt_32 ar;
        int t, j;

FXPT_PUSHSTATE("do_zfilt_dynamic32to32", -1.0, -1.0);
        for (t = 0; t < flt->data_length; t++) {
                flt->memory[0] = data[t];
                ar = 0;
                for (j = flt->order; j > 0; j--) {
                        /*ar  += flt->memory[j] * coef[j];*/
                        ar = fxpt_add32(ar,
                            fxpt_shl32_fast(fxpt_mult32(coef[j],
                            fxpt_extract_h(flt->memory[j])), 2));
                        flt->memory[j] = flt->memory[j-1];
                }
                /*data[t] = ar + flt->memory[0] * coef[0];*/
                ar = fxpt_add32(ar,
                    fxpt_shl32_fast(fxpt_mult32(coef[0],
                    fxpt_extract_h(flt->memory[0])), 2));
                data[t] = ar;
        }
FXPT_POPSTATE();
}
#endif