dtmfs.c

Express DSP compliant C55x DTMF detector software is proposed in two versions: one with a 5 ms frame

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

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

#if ! defined(_dsp)
#if (IDTMF_FR_SZ == 40)
S16 DBG_sFrameEn;
S32 DBG_slDialEn;
#endif
#include <stdlib.h>
#endif

/*--------------------- local vars ----------------------------------------*/
static const S16 dtmf_as2k[DTMF_2KFLT_HSZ] = 
{
     1230, 
    -2060, 
     3472, 
    -6516, 
    20712
};
static const S16 *__apsLoDev[] =
{
    dtmf_a697,
    dtmf_a770,
    dtmf_a852,
    dtmf_a941
};

static const S16 *__apsHiDev[] =
{
    dtmf_a1209,
    dtmf_a1336,
    dtmf_a1477,
    dtmf_a1633
};
static const S16 __aasLoDevFlt[4][2] = 
{
    {23220,  77},
    {18493,  90},
    {14473,  112},
    {11353,  139}
};
static const S16 __aasHiDevFlt[4][2] = 
{
    {16916,  100},
    {12640,  115},
    {9888,   139},
    {7614,   175}
};
/*--------------------- local functions -----------------------------------*/
/*--------------------- public  functions ---------------------------------*/
/*-------------------------------------------------------------------------*/
static S16                     dtmf_en2logS
/*-------------------------------------------------------------------------*/
(
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;
    ac0 += t0*512;
    return (S16)ac0; 
}   
/*-------------------------------------------------------------------------*/
static void 					move_data
/*-------------------------------------------------------------------------*/
(
DTMF_tDb *pDb,
DTMF_tSc *pSc,
S16 *psIn
)
{
	S16 k;
	
	pSc->sTestFailed = 0;
    /* input data buffer */
    for (k = 0; k < DTMF_2KFLT_SZ; k++)
    {
        pSc->as2kData[k] = pDb->as2kDataSav[k];
    }
    for (k = 0; k < IDTMF_FR_SZ; k++)
    {
        pSc->as2kData[k+DTMF_2KFLT_SZ] = (psIn[k]>>3);
    }
    /* 2k data buffer */
    for (k = 0; k < DTMF_BPFLT_SZ; k++)
    {
        pSc->asBpData[k] = pDb->asBpDataSav[k];
    }
    /* band pass filtered data buffers */
    for (k = 0; k < DTMF_LOSD_SZ; k++)
    {
        pSc->asLoData[k] = pDb->asLoDataSav[k];
    }
#if (IDTMF_FR_SZ != 80)
    for (k = 0; k < DTMF_HISD_SZ; k++)
    {
        pSc->asHiData[k] = pDb->asHiDataSav[k];
    }
#endif
    for (k = 5; k > 0; k--)
    {
        pDb->v.asSumEn[k] = pDb->v.asSumEn[k-1];
    }
}
/*-------------------------------------------------------------------------*/
static void 					filter_2k
/*-------------------------------------------------------------------------*/
(
DTMF_tDb *pDb,
DTMF_tSc *pSc
)
{
    S16 *psPos = pSc->as2kData;
    S16 *psNeg = psPos + DTMF_2KFLT_HSZ*4-2;
    S16 *psOut = &pSc->asBpData[DTMF_BPFLT_SZ];
    const S16 *psCDP = dtmf_as2k;
    S32 ac0;
    S32 ac1;
    S32 ac2;
    S32 ac3;
	S16 sSample;
    S16 k;

    ac0 = 0;
    ac3 = 0;
    for (sSample = 0; sSample < IDTMF_FR_SZ/2; sSample++)
    {
        ac1 = 0; 
        ac2 = 0;
        for (k = 0; k < DTMF_2KFLT_HSZ; k++)
        {
            ac1 += *psPos * (S32)*psCDP;
            ac2 += *psNeg * (S32)*psCDP;
            psNeg -= 2;
            psPos += 2;
            psCDP++;
        }
        ac1 += ac2;
        ac2 = ac1;

        psPos--;                    /* points to the centre */
        ac1 += ((S32)(*psPos))<<15; /* central coeff (lo half) =  32768 */
        ac2 -= ((S32)(*psPos))<<15; /* central coeff (hi half) = -32768 */
        psPos++;

        ac1 >>= 16; ac0 += ac1*ac1; /* lo-half freq (0000...2000 Hz) energy */
        ac2 >>= 16; ac3 += ac2*ac2; /* hi-half freq (2000...4000 Hz) energy */

        *psOut++ = (S16)ac1;

        psPos -= DTMF_2KFLT_HSZ*2-2; /* back and 2 forward */
        psNeg += DTMF_2KFLT_HSZ*2+2; /* -(back) and 2 forward */
        psCDP -= DTMF_2KFLT_HSZ;     /* circular */  
    }

    pSc->s2kEnDlt = (dtmf_en2logS(ac0) - dtmf_en2logS(ac3));
}
/*-------------------------------------------------------------------------*/
static void 					filter_lo
/*-------------------------------------------------------------------------*/
(
DTMF_tDb *pDb,
DTMF_tSc *pSc
)
{
    S16 *psOut  = &pSc->asLoData[DTMF_LOSD_SZ];
    S16 *psCDP  = pSc->asBpData;
    const S16 *psIflt = dtmf_aLoBpI;
    const S16 *psQflt = dtmf_aLoBpQ;
    S32 ac0;
    S32 ac1;
    S32 ac2;
	S16 sSample;
    S16 k;

    ac0 = 0;
    for (sSample = 0; sSample < DTMF_DFR_SZ; sSample++)
    {
        ac1 = 0;
        ac2 = 0;
        for (k = 0; k < DTMF_BPFLT_SZ; k++)
        {
            ac1 += *psCDP * (S32) *psIflt;
            ac2 += *psCDP * (S32) *psQflt;
            psCDP++;
            psIflt++;
            psQflt++;
        }
        ac1 >>= 16;
        ac2 >>= 16;
        *psOut++ = (S16)(ac1);
        *psOut++ = (S16)(ac2);
        ac0 += (ac1)*(ac1);
        ac0 += (ac2)*(ac2);
        psCDP  -= DTMF_BPFLT_SZ - DTMF_BPDR; 

        psIflt -= DTMF_BPFLT_SZ; /* circular ? */
        psQflt -= DTMF_BPFLT_SZ; /* circular ? */
    }
#if (IDTMF_FR_SZ == 40)
#else
    ac0 >>= 1;
#endif
    pSc->slLoBpEn = ac0;
    pSc->sLoBpEn = dtmf_en2logS(pSc->slLoBpEn);
}
/*-------------------------------------------------------------------------*/
static void 					filter_hi
/*-------------------------------------------------------------------------*/
(
DTMF_tDb *pDb,
DTMF_tSc *pSc
)
{
    S16 *psOut  = &pSc->asHiData[DTMF_HISD_SZ];
    S16 *psCDP  = pSc->asBpData;
    const S16 *psIflt = dtmf_aHiBpI;
    const S16 *psQflt = dtmf_aHiBpQ;
    S32 ac0;
    S32 ac1;
    S32 ac2;
	S16 sSample;
    S16 k;

    ac0 = 0;
    for (sSample = 0; sSample < DTMF_DFR_SZ; sSample++)
    {
        ac1 = 0;
        ac2 = 0;
        for (k = 0; k < DTMF_BPFLT_SZ; k++)
        {
            ac1 += *psCDP * (S32) *psIflt;
            ac2 += *psCDP * (S32) *psQflt;
            psCDP++;
            psIflt++;
            psQflt++;
        }
        ac1 >>= 16;
        ac2 >>= 16;
        *psOut++ = (S16)(ac1);
        *psOut++ = (S16)(ac2);
        ac0 += (ac1)*(ac1);
        ac0 += (ac2)*(ac2);
        psCDP  -= DTMF_BPFLT_SZ - DTMF_BPDR; 

        psIflt -= DTMF_BPFLT_SZ; /* circular ? */
        psQflt -= DTMF_BPFLT_SZ; /* circular ? */
    }
#if (IDTMF_FR_SZ == 40)
#else
    ac0 >>= 1;
#endif
    pSc->slHiBpEn = ac0;
    pSc->sHiBpEn = dtmf_en2logS(pSc->slHiBpEn);
}
/*-------------------------------------------------------------------------*/
static void 					filter_dial
/*-------------------------------------------------------------------------*/
(
DTMF_tDb *pDb,
DTMF_tSc *pSc
)
{
    const S16 *psIflt = dtmf_aDialI;
    const S16 *psQflt = dtmf_aDialQ;
    S16 *psCDP  = pSc->asBpData;
    S32 ac0;
    S32 ac1;
    S32 ac2;
    S16 sSample;
    S16 k;

    ac0 = 0;
    for (sSample = 0; sSample < DTMF_DFR_SZ/2; sSample++)
    {
        ac1 = 0;
        ac2 = 0;
        for (k = 0; k < DTMF_BPFLT_SZ; k++)
        {
            ac1 += *psCDP * (S32) *psIflt;
            ac2 += *psCDP * (S32) *psQflt;
            psCDP++;
            psIflt++;
            psQflt++;
        } 
        ac1 >>= 16;
        ac2 >>=16;
        ac0 += (ac1)*(ac1);
        ac0 += (ac2)*(ac2);
        psCDP  -= DTMF_BPFLT_SZ - DTMF_BPDR*2; 

        psIflt -= DTMF_BPFLT_SZ; /* circular */
        psQflt -= DTMF_BPFLT_SZ; /* circular */
    }

    pSc->slDialEn = ac0;


#if (IDTMF_FR_SZ == 40)                                  
#if ! defined (_dsp)
    DBG_slDialEn = pDb->v.slDialEn + pSc->slDialEn;
#endif
#else
    pSc->sDialEn = dtmf_en2logS(pSc->slDialEn);
#endif

    pSc->sSumDialEn = dtmf_en2logS(pSc->slLoBpEn + 
                                  pSc->slHiBpEn +
#if (IDTMF_FR_SZ == 40)                                  
                                  pDb->v.slDialEn +
#endif
                                  pSc->slDialEn);
	/* update for the next frame */
#if (IDTMF_FR_SZ == 40)                                  
    pDb->v.slDialEn = pSc->slDialEn;
#endif

}
/*-------------------------------------------------------------------------*/
static void 					dft_lo_main
/*-------------------------------------------------------------------------*/
(
DTMF_tDb *pDb,
DTMF_tSc *pSc
)
{
    const S16 *psCDP = dtmf_aLo;
    S16 *psI = pSc->asLoData;
    S16 *psQ = psI + 1;
    S16 *psOut = pSc->asLoMainEn;
    S16 sFilterNo;
    S16 k;
    S32 ac0;
    S32 ac1;

    for (sFilterNo = 0; sFilterNo < 4; sFilterNo++)
    {
        ac0 = 0;
        ac1 = 0;
        /* psCDP points to Cos coeffs */
        for (k = 0; k < DTMF_LO_DFT_SZ; k++)
        {
            ac0 += *psCDP * (S32) *psI;
            ac1 += *psCDP * (S32) *psQ;
            psI += 2;
            psQ += 2;
            psCDP++;
        }
        /* psCDP points to Sin coeffs */
        /* return data pointers back */
        psI -= DTMF_LO_DFT_SZ*2;
        psQ -= DTMF_LO_DFT_SZ*2;
        for (k = 0; k < DTMF_LO_DFT_SZ; k++)
        {
            ac1 += *psCDP * (S32) *psI;
            ac0 -= *psCDP * (S32) *psQ;
            psI += 2;
            psQ += 2;
            psCDP++;
        }
        /* return data pointers back */
        psI -= DTMF_LO_DFT_SZ*2;
        psQ -= DTMF_LO_DFT_SZ*2;
        /* psCDP points to Cos coeffs of the next filter */
        /* ac0 = I * c - Q * s;
           ac1 = I * s + Q * c;
         */

        ac0 >>= 16; ac0  = ac0 * ac0;
        ac1 >>= 16; ac0 += ac1 * ac1;
        *psOut++ = dtmf_en2logS(ac0);;
    }
}
/*-------------------------------------------------------------------------*/
static void 					sort_lo
/*-------------------------------------------------------------------------*/
(
DTMF_tDb *pDb,
DTMF_tSc *pSc
)
{
    S16 *psIn = pSc->asLoMainEn;
    S16 sFreqNo = 0;
    S16 sMaxFreqEn = psIn[0];
    S16 k;

    for (k = 1; k < 4; k++)
    {
        if (psIn[k] > sMaxFreqEn)
        {
            sMaxFreqEn = psIn[k];
            sFreqNo = k;
        }
    }

    sMaxFreqEn = -32767;
    for (k = 0; k < 4; k++)
    {
        if (k != sFreqNo)
        {
            if (psIn[k] > sMaxFreqEn)
            {
                sMaxFreqEn = psIn[k];
            }
        }
    }
    pSc->sLoNextEn = sMaxFreqEn;
    pSc->sLoFreqNo = sFreqNo;
}
/*-------------------------------------------------------------------------*/
static void 					dft_lo_dev
/*-------------------------------------------------------------------------*/
(
DTMF_tDb *pDb,
DTMF_tSc *pSc
)
{
    const S16 *psCDP = __apsLoDev[pSc->sLoFreqNo];
    S16 *psI = pSc->asLoData;
    S16 *psQ = psI + 1;
    S16 *psOut = pSc->asLoDevEn;
    S16 sFilterNo;
    S16 k;
    S32 ac0;
    S32 ac1;

    for (sFilterNo = 0; sFilterNo < 4; sFilterNo++)
    {
        ac0 = 0;
        ac1 = 0;
        /* psCDP points to Cos coeffs */
        for (k = 0; k < DTMF_LO_DFT_SZ; k++)
        {
            ac0 += *psCDP * (S32) *psI;
            ac1 += *psCDP * (S32) *psQ;
            psI += 2;
            psQ += 2;
            psCDP++;
        }
        /* psCDP points to Sin coeffs */
        /* return data pointers back */
        psI -= DTMF_LO_DFT_SZ*2;
        psQ -= DTMF_LO_DFT_SZ*2;
        for (k = 0; k < DTMF_LO_DFT_SZ; k++)
        {
            ac1 += *psCDP * (S32) *psI;
            ac0 -= *psCDP * (S32) *psQ;
            psI += 2;
            psQ += 2;
            psCDP++;
        }
        /* return data pointers back */
        psI -= DTMF_LO_DFT_SZ*2;
        psQ -= DTMF_LO_DFT_SZ*2;
        /* psCDP points to Cos coeffs of the next filter */
        /* ac0 = I * c - Q * s;
           ac1 = I * s + Q * c;
         */

        ac0 >>= 16; ac0  = ac0 * ac0;