postfilt.c

手机加密通话软件

/* Copyright 2001,2002,2003 NAH6
 * All Rights Reserved
 *
 * Parts Copyright DoD, Parts Copyright Starium
 *
 */
/*LINTLIBRARY*/
/*PROTOLIB1*/

#include "main.h"
#include "filter.h"
/*#include <math.h>*/
#include "bwexp.h"
#include "lsftopc.h"
#include "move_array16.h"
#include "pctorc.h"
#include "postfilt.h"

FILTER  PostP, PostZ, PostZ2;

/**************************************************************************
*                                                                         *
* ROUTINE
*               PostFilter
*
* FUNCTION
*               Filter input signal
* SYNOPSIS
*               PostFilter(speech_in, lsf, speech_out)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       speech_in       fxpt_16  i      input speech
*       lsf             fxpt_16  i      input line spectral frequencies
*       speech          fxpt_16  o      output speech
*
***************************************************************************
*
* CALLED BY
*
*       Synthesis
*
* CALLS
*
*       BWExpand do_zfilt_dynamic do_zpfilt_dynamic PCtoRC MoveArray16
*
**************************************************************************/

#ifndef FLOATING_POINT
#define POSTFIL2                /* only support POSTFIL2 in fixed point mode */
#endif
#define TC      0.01

void PostFilter(
fxpt_16 speech_in[],            /* 15.0 format */
fxpt_16 lsf[],                  /* 0.15 format */
fxpt_16 speech_out[])           /* 15.0 format */
{
        int             n, precision;
        static fxpt_32  powerin=0;      /* 30.1 format */
        static fxpt_32  powerout=0;     /* 30.1 format */
        static fxpt_32  num, den;
        fxpt_16         ast[2];
        fxpt_16         pcexp1[ORDER + 1], pcexp2[ORDER + 1], rcexp2[ORDER];
        fxpt_16         alpha = 26214;  /* 0.8 in 0.15 format */
        fxpt_16         beta = 16384;   /* 0.5 in 0.15 format */
        fxpt_16         pc[ORDER+1];

#ifdef POSTFIL2
        fxpt_16         scale;          /* 3.12 format */
#else
        fxpt_32         newpowerin[MAX_CW_VEC_LEN + 1], newpowerout[MAX_CW_VEC_LEN + 1];
#endif

        /* estimate input power */

#ifdef POSTFIL2
        for (n = 0; n < SF_LEN; n++) {
                /*powerin = powerin * (1.0 - TC) + TC * speech_in[n] * speech_in[n];*/
                /* Multiply powerin by 63/64 */
                powerin = fxpt_shr32_fast(powerin, 6);
                powerin *= 63;

                /* Add (1/8) * speech_in[n] * (1/8) * speech_in[n]  */
                powerin = fxpt_mac32(powerin, fxpt_shr16_round(speech_in[n], 3),
                    fxpt_shr16_round(speech_in[n], 3));
        }
#else
        newpowerin[0] = powerin;
        for (n = 0; n < SF_LEN; n++)
                newpowerin[n + 1] = (1.0 - TC) * newpowerin[n] +
                    TC * speech_in[n] * speech_in[n];
        powerin = newpowerin[SF_LEN];
#endif

        /* Convert input LSFs to PCs */
        LSFtoPC(lsf, pc);

        /* BW expansion */
        BWExpand(beta, pc, pcexp1);
        BWExpand(alpha, pc, pcexp2);

        /* pole-zero postfilter */
        MoveArray16(speech_out, speech_in, SF_LEN);
        do_zpfilt_dynamic(&PostZ, &PostP, pcexp1, pcexp2, speech_out);

        /* find spectral tilt (1st order fit) of postfilter
         * (denominator dominates the tilt)
         */
        PCtoRC(pcexp2, rcexp2);

        /* tilt compensation by a scaled zero
         * (don't allow hF roll-off)
         */
        ast[0] = 8192;          /* 1.0 in 2.13 format */
        ast[1] = (rcexp2[0] > 0) ? fxpt_mult16_fast(-16384, rcexp2[0]) : 0;
        do_zfilt_dynamic(&PostZ2, ast, speech_out);

        /*  estimate output power       */

#ifdef POSTFIL2
        for (n = 0; n < SF_LEN; n++) {
                /*powerout = powerout * (1.0 - TC) + TC * speech_out[n] * speech_out[n];*/
                /* Multiply powerout by 63/64 */
                powerout = fxpt_shr32_fast(powerout, 6);
                powerout *= 63;

                /* Add (1/8) * speech_out[n] * (1/8) * speech_out[n] */
                powerout = fxpt_mac32(powerout,
                    fxpt_shr16_round(speech_out[n], 3),
                    fxpt_shr16_round(speech_out[n], 3));
        }

        /*  block wise automatic gain control   */

        if (powerout > 1) {
                /*scale = sqrt(powerin/powerout);*/
                /*scale = fxpt_shl16_fast(fxpt_extract_l(integer_sqrt(
                    powerin/fxpt_extract_h(powerout))), 6);*/
                /*
                 * Compute scale
                 */
                precision = 0;
                num = powerin;
                den = powerout;
                while ((num & 0x4000000) == 0) {
                        num = fxpt_shl32_fast(num, 1);
                        if (++precision == 12)
                                break;
                }
                if (precision < 12)
                        den = fxpt_shr32_fast(den, 12-precision);
                scale = fxpt_shl16_fast(integer_sqrt(num/den), 6);
                scale = fxpt_mult16_fix(scale, 18022, 14);

                for (n=0; n<SF_LEN; n++)
                        /*speech_out[n] *= scale;*/
                        speech_out[n] = fxpt_mult16_fix(speech_out[n],
                            scale, 12);
        }
#else
        newpowerout[0] = powerout;
        for (n = 0; n < SF_LEN; n++)
                newpowerout[n + 1] = (1.0 - TC) * newpowerout[n] +
                    TC * speech_out[n] * speech_out[n];
        powerout = newpowerout[SF_LEN];

        /*  sample wise automatic gain control  */

        for (n = 0; n < SF_LEN; n++) {
                if (newpowerout[n + 1] > 0.0)
                        speech_out[n] *= (float)sqrt((double)(newpowerin[n + 1] / newpowerout[n + 1]));
        }
#endif
}