gaecus.c

TI公司DSP/tms320c55x/tms320c54x系列的声学回声消除代码

/*-------------------------------------------------------------------------*
 *                                                                         *
 *   THIS IS AN UNPUBLISHED WORK CONTAINING CONFIDENTIAL AND PROPRIETARY   *
 *   INFORMATION.  IF PUBLICATION OCCURS, THE FOLLOWING NOTICE APPLIES:    *
 *      "COPYRIGHT 2002 MIKET DSP SOLUTIONS, ALL RIGHTS RESERVED"          *
 *                                                                         *
 *-------------------------------------------------------------------------*/

#include "gaeci.h"

/*--------------------- local defs ----------------------------------------*/
/*--------------------- public vars ---------------------------------------*/
/*--------------------- local vars ----------------------------------------*/
/*--------------------- local functions -----------------------------------*/
/*--------------------- public  functions ---------------------------------*/
/*-------------------------------------------------------------------------*/


/* ------------------------------------------------------------------------ */
#if !defined(_dsp)
void		                    gaec_utl_avrg
#else
void		                    gaec_utl_avrgC
#endif
/* ------------------------------------------------------------------------ */
(
S16 *psVal, 
S16 sNew,
S16 sCoeff
)
/* Vk+1 = Vk + (y - Vk) * coeff/32768; */
{
    S32 acc;

    acc = (sNew - *psVal);
    acc *= sCoeff;
    acc += ((S32)(*psVal))<<15;
    acc += 0x4000;
    acc >>= 15;
    *psVal = (S16)acc;
}

#if 0
#define MULT    (31821)
#define INC     (13849)
/* ------------------------------------------------------------------------ */
void                            gaec_utl_pkt_rand
/* ------------------------------------------------------------------------ */
(
S16 *psTo, 
S16 *psSeed
)
{
    int k;
    S16 t = MULT;
    S16 b = INC;
    for (k = 0; k < GAEC_FR_SZ; k++)
    {
        S32 acc = *psSeed * (S32) t;
        acc += b;
        *psTo++ = (S16) acc;
        *psSeed = (S16)acc;
    }
}
#endif

/* ------------------------------------------------------------------------ */
#if ! defined (_dsp)
S16                             gaec_utl_pkt_energy
#else
S16                             gaec_utl_pkt_energyC
#endif
/* ------------------------------------------------------------------------ */
(
S16 *psIn /* --- ar2, pointer to the start of array */
)
{
     S32 ac0 = 0; /* --- acc */
     S32 ac1 = 0; /* --- accb */
     int k;

     for (k = 0; k < GAEC_FR_SZ/2; k++, psIn++)
     {
          ac1 += *psIn * (S32)*psIn;
     }
     for (k = 0; k < GAEC_FR_SZ/2; k++, psIn++)
     {
          ac0 += *psIn * (S32)*psIn;
     }
     ac0 >>= 2;
     ac0 += (ac1>>2);
     return gaec_utl_pen2log(ac0);
}
/* ------------------------------------------------------------------------ */
#if ! defined (_dsp)
S32                             gaec_utl_pkt_norm
#else
S32                             gaec_utl_pkt_normC
#endif
/* ------------------------------------------------------------------------ */
(
S16 *psIn /* --- ar2, pointer to the start of array */
)
{
     S32 ac0 = 0; 
     S32 ac1 = 0; 
     int k;

     for (k = 0; k < GAEC_FR_SZ/2; k++, psIn++)
     {
          ac1 += *psIn * (S32)*psIn;
     }
     for (k = 0; k < GAEC_FR_SZ/2; k++, psIn++)
     {
          ac0 += *psIn * (S32)*psIn;
     }
     ac0 >>= 2;
     ac0 += (ac1>>2);
     return (ac0);
}
/*-------------------------------------------------------------------------*/
#if ! defined (_dsp)
S16                             gaec_utl_pen2log
#else
S16                             gaec_utl_pen2logC
#endif
/*-------------------------------------------------------------------------*/
(
S32 ac0    /* but it is positive */
)
{
    S16 t0 = 0;

    ac0 += 1; // min val -13952
    // mant aco :: nexp ac0, t0
    for (;;)
    {
        if (ac0 &0x40000000L)
            break;
        ac0 <<= 1;
        t0--;
    }
    ac0 >>= 21;
    ac0 += 0x780-1024;
    ac0 += t0*512;
    return (S16)(ac0); 
}
/*-------------------------------------------------------------------------*/
#if !defined(_dsp)
S16                             gaec_utl_en2log
#else
S16                             gaec_utl_en2logC
#endif
/*-------------------------------------------------------------------------*/
(
S32 ac0    /* but it is positive */
)
{
    S16 t0 = 0;

    ac0 += 1; // min val -13952
    // mant aco :: nexp ac0, t0
    for (;;)
    {
        if (ac0 &0x40000000L)
            break;
        ac0 <<= 1;
        t0--;
    }
    ac0 >>= 21;
    ac0 += 0x780-1024 - GAEC_DB(6.0);
    ac0 += t0*512;
    return (S16)(ac0); 
}
#if 0
/* ------------------------------------------------------------------------ */
S16                             lec_dB2Lin
/* ------------------------------------------------------------------------ */
(
S16 level /* in DB*100, shall be negative  */
)
{
	
    /* linear approximation, max err ~ 6% */
    if (level > 0) return 4096;
    if (level < -73*100) return 0;
    S32 acc = -level*109L;
	 /* number of right shifts in accH */
    /* reminder in accL */

    S16 rem = (S16)((acc & 0xffff)>>2);
    S16 val = 4096>>((S16)(acc >> 16));
    S16 mult = 0x7fff - rem;
    return (S16)((val * (S32)mult + 0x4000)>>15);

}
#endif

// short exp too???
/* ------------------------------------------------------------------------ */
#if !defined(_dsp)
S16                             gaec_utl_exp
#else
S16                             gaec_utl_expC
#endif
/* ------------------------------------------------------------------------ */
(
S16 log /* in dBm0*170.1 */
)
{
    S16 x, exp;
    S32 a = ((S32)log)<<2;
    S16 t = (S16)(a>>12);
    S16 d = a&4095;
    a = (d * (S32) d)>>13;
    a += d;
    a *= 21845;
    a >>= 15;
    a += 4096;
    x = (S16) a;/* 4096 + (d + (S16)((d*(S32)d)>>13))*0.66667; */

    if (t > 0) exp = x<<t; else exp = x>>(-t);
    return exp; 
}  
#if 0
/* ------------------------------------------------------------------------ */
#if !defined(_dsp)
S16                             gaec_utl_pkt_peak
#else
S16                             gaec_utl_pkt_peakC
#endif
/* ------------------------------------------------------------------------ */
(
S16 *p
)
{
     int i;
     S16 max = 0;
     for (i = 0; i < GAEC_FR_SZ; i++,p++)
     {
          S16 t = *p;
          if (t < 0) t = -t;
          if (t > max) max = t;
     }
     return max;

}
#endif

#if !defined(_dsp)
#include <memory.h>
/* ------------------------------------------------------------------------ */
void                             memclr
/* ------------------------------------------------------------------------ */
(
void *p,
U16 sz
)
{
	memset(p, 0, sz);
}
#endif