filters.c

手机加密通话软件

/* Copyright 2001,2002,2003 NAH6
 * All Rights Reserved
 *
 * Parts Copyright DoD, Parts Copyright Starium
 *
 */
#include "alloc.h"
#include <string.h>
/*#include "main.h"*/
#include "celpfilt.h"
#include "move_array16.h"
#include "zero_array16.h"

/* LP Analysis Filter Parameters */
BIGFILTER  InputHPFZ, InputHPFP;           /* filter coefs in 2.29 format */
fxpt_32 InputHPFCoefsZ[3] = {507879882L, -1015759765L, 507879882L};
fxpt_32 InputHPFCoefsP[3] = {536870912L, -1014166869L, 480431981L};
int     InputHPFOrder = 2;
int     InputHPFLength = F_LEN;

/* LP residual calculation filter parameters */
FILTER  LP_ResZ, LP_ResP, LP_ResP2;

/* Adaptive analysis residual calculation filter parameters */
FILTER  Adapt_ResZ, Adapt_ResP, Adapt_ResP2;

/* Update filters for residual calculations */
FILTER  Update_ResZ, Update_ResP, Update_ResP2;

/*  HPF output filters for synthesis */
BIGFILTER  OutputHPFZ, OutputHPFP;
/*  Filter coefficients for 2nd order Butterworth 275 Hz HPF:           */
fxpt_16 OutputHPFCoefsZ[3] = {7750, -15499, 7750};
fxpt_16 OutputHPFCoefsP[3] = {8192, -15475, 7331};

/**************************************************************************
*                                                                         *
* ROUTINE
*               do_zpfilt_dynamic
*
* FUNCTION
*
* SYNOPSIS
*               do_zpfilt_dynamic(zflt, pflt, alpha, coef, data)
*
**************************************************************************/

void do_zpfilt_dynamic(
FILTER *zflt,
FILTER *pflt,
fxpt_16 zcoef[],
fxpt_16 pcoef[],
fxpt_16 data[])
{
	fxpt_32 tmpdata[RES_LEN];
FXPT_PUSHSTATE("do_zpfilt_dynamic", -1.0, -1.0);
#if 1

//        tmpdata = local_malloc(zflt->data_length * sizeof(*tmpdata));
//        if (!tmpdata) {
//                CELP_PRINTF(("malloc failed in do_zpfilt_dynamic\n"));
//                CELP_ABORT();
//        }

		ASSERT( zflt->data_length <= RES_LEN );

        do_zfilt_dynamic16to32(zflt, zcoef, data, tmpdata);
        do_pfilt_dynamic32to16(pflt, pcoef, tmpdata, data);
//        local_free(tmpdata);
#else
        do_zfilt_dynamic(zflt, zcoef, data);
        do_pfilt_dynamic(pflt, pcoef, data);
#endif
FXPT_POPSTATE();
}

void do_zpfilt_dynamic32to16(
FILTER *zflt,
FILTER *pflt,
fxpt_16 zcoef[],
fxpt_16 pcoef[],
fxpt_32 indata[],
fxpt_16 outdata[])
{
FXPT_PUSHSTATE("do_zpfilt_dynamic32to16", -1.0, -1.0);
        do_zfilt_dynamic32to32(zflt, zcoef, indata);
        do_pfilt_dynamic32to16(pflt, pcoef, indata, outdata);
FXPT_POPSTATE();
}


/**************************************************************************
*                                                                         *
* ROUTINE
*               HPF_Speech
*
* FUNCTION
*               High pass filter input speech
* SYNOPSIS
*               HPF_Speech(speech_in)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       speech_in       fxpt_16  i/o    Frame of input speech
*
**************************************************************************/

void HPF_InSpeech(
fxpt_16 speech[F_LEN])          /* 15.0 format */
{
        fxpt_32 tmpspeech[F_LEN];	/* 17.14 format!!! */

FXPT_PUSHSTATE("HPF_InSpeech", -1.0, -1.0);

        do_zfilt(&InputHPFZ, speech, tmpspeech);

//		DUMPARR(tmpspeech,14,F_LEN)

        do_pfilt(&InputHPFP, tmpspeech, speech);

//		DUMPARR0(speech,F_LEN)

FXPT_POPSTATE();
}


/**************************************************************************
*                                                                         *
* ROUTINE
*               HPF_OutSpeech
*
* FUNCTION
*               High pass filter output speech
* SYNOPSIS
*               HPF_Outspeech(speech_in, speech_out)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       speech_in       fxpt_16   i     Frame of input speech
*       speech_in       fxpt_16   o     Frame of output speech
*
**************************************************************************/

void HPF_OutSpeech(
fxpt_16 speech_in[SF_LEN],      /* 15.0 format */
fxpt_16 speech_out[SF_LEN])     /* 15.0 format */
{
        fxpt_32 tmpspeech[F_LEN];

        /* Perform filtering on input speech to produce output speech */
        MoveArray16(speech_out, speech_in, SF_LEN);
        do_zfilt(&OutputHPFZ, speech_out, tmpspeech);
        do_pfilt(&OutputHPFP, tmpspeech, speech_out);
}


/**************************************************************************
*                                                                         *
* ROUTINE
*               FilterImpulseResponse
*
* FUNCTION
*               Filter Impulse Response with varying coefficients and
                  no history from past frames
* SYNOPSIS
*               FilterImpulseResponse(h, coef)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       h               fxpt_16  i/o    Impulse response
*       coef            fxpt_16   i     Filter coefficients
*
**************************************************************************/

#if 1 /* routine now implemented it's own file */
void FilterImpulseResponse(
fxpt_16 h[SF_LEN],                      /* 2.13 format */
fxpt_16 coef[ORDER+1])                  /* 2.13 format */
{
        int     t, j;
        fxpt_32 FiltMem[ORDER+1];       /* 3.28 format */
		fxpt_32 *pf;
		fxpt_16 *pc;

FXPT_PUSHSTATE("FilterImpulseResponse", -1.0, -1.0);

        /* Initialize memory */
		ZeroArray32( FiltMem, ORDER+1 );

        /* Filter impulse response */
        for (t=0; t<SF_LEN; t++) {
                /*FiltMem[0] = h[t];*/
                //FiltMem[0] = fxpt_shr32_fast(fxpt_deposit_h(h[t]), 1);
                FiltMem[0] = fxpt_shl32_fast(h[t], 15);
				for (j=ORDER, pf= FiltMem+j, pc= coef+j; j>0; --j, --pf, --pc) {
					FiltMem[0] = fxpt_sub32(FiltMem[0],
							fxpt_mult64_round_good(*pf, *pc, 13));
					pf[0] = pf[-1]; // FiltMem[j]  = FiltMem[j-1];
				}
                /*h[t] = FiltMem[0];*/
//                h[t] = fxpt_extract_h(fxpt_shl32_fast(FiltMem[0], 1));
                h[t] = fxpt_saturate16_fix(pf[0], 15);
        }
FXPT_POPSTATE();
}
#endif