clids.c

This R2.9 revision of the CLID detector provides the TYPE 1 (on-hook, between first and second ring,

/*-------------------------------------------------------------------------*
*                                                                         *
*   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"          *
*                                                                         *
*-------------------------------------------------------------------------*/
#if ! defined (_dsp)
#include <stdlib.h> // abs()
#endif

#include "stddefs.h"
#include "clidi.h"

/*--------------------- local defs ----------------------------------------*/

#define _CORR   ICLID_DB(22.2)

/*--------------------- public vars ---------------------------------------*/
/*--------------------- local vars ----------------------------------------*/
/*--------------------- local functions -----------------------------------*/
/*-------------------------------------------------------------------------*/
S16                     _en2logC
/*-------------------------------------------------------------------------*/
(
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 += t0*512;
    return (S16)(ac0+0); 
}
/*-------------------------------------------------------------------------*/
void 					clid_move_data_inC
/*-------------------------------------------------------------------------*/
(
CLID_tDb *pDb,
CLID_tSc *pSc,
S16 *psIn
)
{
    S16 k;
    S16 sNormShift = pDb->Cfg.sNormShift;

    pSc->sCurrBitLen = pDb->sCurrBitLen;
    pSc->sBits = pDb->sBits;
    pSc->sAlignment = pDb->sAlignment;

    /* band-pass data */
    for (k = 0; k < CLID_BPF_SSZ; k++)
    {
        pSc->asBp[k] = pDb->asBpSav[k];
    }
    /* add new data */
    for (k = 0; k < ICLID_FR_SZ; k++)
    {
        S16 x;
        if (sNormShift > 0)
            x = psIn[k] << sNormShift;
        else
            x = psIn[k] >> (-sNormShift);

        pSc->asBp[k+CLID_BPF_SSZ] = x;
    }
    /* lp data */
    for (k = 0; k < CLID_LPF_SSZ; k++)
    {
        pSc->asLpI[k] = pDb->asLpISav[k];
        pSc->asLpQ[k] = pDb->asLpQSav[k];
    }
    /* mf data */
    for (k = 0; k < CLID_MF_SSZ; k++)
    {
        pSc->asMfI[k] = pDb->asMfISav[k];
        pSc->asMfQ[k] = pDb->asMfQSav[k];
    }

    for (k = 0; k < CLID_OUT_SSZ; k++)
    {
        pSc->asOut[k] = pDb->asOutSav[k];
    }

}
/*-------------------------------------------------------------------------*/
void                    clid_bp_filterC
/*-------------------------------------------------------------------------*/
(
CLID_tDb *pDb,
CLID_tSc *pSc
)
{
    S16 *psFrom = pSc->asBp;
    S16 *psToI = pSc->asLpI + CLID_LPF_SSZ;
    S16 *psToQ = pSc->asLpQ + CLID_LPF_SSZ;
    U16 k;

    for (k = 0; k < ICLID_FR_SZ; k++)
    {
        U16 m = 0;
        S32 ac0 = 0;
        S32 ac1 = 0;

        for (m = 0; m < CLID_BPF_SZ; m++)
        {
            ac0 += *psFrom * (S32)(CLID_asBpfI[m]);
            ac1 += *psFrom * (S32)(CLID_asBpfQ[m]);
            psFrom++;
        }
        ac0 += 1<<14; ac0 >>= 15; *psToI++ = (S16)ac0;
        ac1 += 1<<14; ac1 >>= 15; *psToQ++ = (S16)ac1;
        psFrom -= (CLID_BPF_SZ-1);
    }
}
/*-------------------------------------------------------------------------*/
void                    clid_convert_basebandC
/*-------------------------------------------------------------------------*/
(
CLID_tDb *pDb,
CLID_tSc *pSc
)
{
    U16 k;
    S16 *psI = &pSc->asLpI[CLID_LPF_SSZ];
    S16 *psQ = &pSc->asLpQ[CLID_LPF_SSZ];
    const S16 *ps1700 = CLID_as1700;

    if (pDb->sEven) 
    {
        ps1700 += 40;
    }

    for (k = 0; k < ICLID_FR_SZ; k++)
    {
        S32 ac0 = *psQ * (S32) (*ps1700);
        S32 ac1 = *psI * (S32) (*ps1700);
        ps1700 += 20;

        ac0 -= *psI * (S32) (*ps1700);
        ac1 += *psQ * (S32) (*ps1700);
        ps1700 -= (20-1);

        ac0 += (1<<14);
        ac0 >>= 15;
        *psI++ = (S16)ac0;
        
        ac1 += (1<<14);
        ac1 >>= 15;
        *psQ++ = (S16)ac1;
    }
    pDb->sEven ^= 1;
}
/*-------------------------------------------------------------------------*/
void                    clid_resample_6kC
/*-------------------------------------------------------------------------*/
(
CLID_tDb *pDb,
CLID_tSc *pSc
)
{
    S16 *psFromI = pSc->asLpI;
    S16 *psFromQ = pSc->asLpQ;
    S16 *psToI   = pSc->asMfI+CLID_MF_SSZ;
    S16 *psToQ   = pSc->asMfQ+CLID_MF_SSZ;
    S16 k;

    // resample here.... 8 kHz -> 6 kHz

    for (k = 0; k < ICLID_FR_SZ/4; k++)
    {
        S32 ac0 = 0;
        S32 ac1 = 0;
        S16 m;
        S16 m0;

        m0 = 2;
        for (m = 0; m < CLID_LPF_SZ; m++)
        {
            ac0 += *psFromI++ * (S32)(CLID_asLpf24[m*3+m0]);
            ac1 += *psFromQ++ * (S32)(CLID_asLpf24[m*3+m0]);
        }
        psFromI -= (CLID_LPF_SZ-1);
        psFromQ -= (CLID_LPF_SZ-1);
        ac0 += (1<<14); ac0 >>= 15; *psToI++ = (S16)ac0;
        ac1 += (1<<14); ac1 >>= 15; *psToQ++ = (S16)ac1;

        ac0 = 0;
        ac1 = 0;
        m0 = 1;
        for (m = 0; m < CLID_LPF_SZ; m++)
        {
            ac0 += *psFromI++ * (S32)(CLID_asLpf24[m*3+m0]);
            ac1 += *psFromQ++ * (S32)(CLID_asLpf24[m*3+m0]);
        }
        psFromI -= (CLID_LPF_SZ-1);
        psFromQ -= (CLID_LPF_SZ-1);
        ac0 += (1<<14); ac0 >>= 15; *psToI++ = (S16)ac0;
        ac1 += (1<<14); ac1 >>= 15; *psToQ++ = (S16)ac1;

        m0 = 0;
        ac0 = 0;
        ac1 = 0;
        for (m = 0; m < CLID_LPF_SZ; m++)
        {
            ac0 += *psFromI++ * (S32)(CLID_asLpf24[m*3+m0]);
            ac1 += *psFromQ++ * (S32)(CLID_asLpf24[m*3+m0]);
        }
        psFromI -= (CLID_LPF_SZ-2);
        psFromQ -= (CLID_LPF_SZ-2);
        ac0 += (1<<14); ac0 >>= 15; *psToI++ = (S16)ac0;
        ac1 += (1<<14); ac1 >>= 15; *psToQ++ = (S16)ac1;
    }
}
/*-------------------------------------------------------------------------*/
void                    clid_matched_filterC
/*-------------------------------------------------------------------------*/
(
CLID_tDb *pDb,
CLID_tSc *pSc
)
{
    S16 k;
    S16 *psFromI = pSc->asMfI;
    S16 *psFromQ = pSc->asMfQ;
    S16 *psTo = pSc->asOut + CLID_OUT_SSZ;

    for (k = 0; k < CLID6_FR_SZ; k++)
    {
        S32 ac0 = 0; // I * C
        S32 ac1 = 0; // Q * C
        S32 ac2 = 0; // I * S
        S32 ac3 = 0; // Q * S
        S16 m;

        for (m = 0; m < CLID_MF_SZ; m++)
        {
            ac0 += *psFromI++ * (S32)CLID_asMfI[m];
            ac1 += *psFromQ++ * (S32)CLID_asMfI[m];
        }
        psFromI -= (CLID_MF_SZ);
        psFromQ -= (CLID_MF_SZ);

        for (m = 0; m < CLID_MF_SZ; m++)
        {
            ac2 += *psFromI++ * (S32)CLID_asMfQ[m];
            ac3 += *psFromQ++ * (S32)CLID_asMfQ[m];
        }
        psFromI -= (CLID_MF_SZ-1);
        psFromQ -= (CLID_MF_SZ-1);


        // filters for +500 and -500 Hz are conjugate
        // mf1 = (IC - QS) + j * (QC + IS);
        // mf2 = (IC + QS) + j * (QC - IS);
        // |mf1|^2 - |mf2|^2 = 4*(QC * IS - IC * QS);
        ac0 >>= 16; ac1 >>= 16; ac2 >>= 16; ac3 >>= 16;
        ac1 = ac1 * ac2;
        ac1 = ac1 - (ac0 * ac3);
        // compress dymanic range
#if 0
        *psTo++ = (S16) (ac1 >> 7);
#else        
        ac0 = absval(ac1);
        {
            S16 t0 = 0;
            ac0 += 1;
            // mant aco :: nexp ac0, t0
            for (;;)
            {
                if (ac0 &0x40000000L)
                    break;
                ac0 <<= 1;
                t0--;
            }
            t0 += 30;
            ac0 >>= 21;
            ac0 += t0 * 512;
        }

        if (ac1 < 0) ac0 = -ac0;
        if (ac1 == 0) ac0 = 0;
        *psTo++ = (S16) (ac0 + pSc->sAlignment);
#endif        
    }
    {
        S32 ac0 = 0;
        psTo--;
        for (k = 0; k < CLID6_FR_SZ; k++)
        {
            ac0 += *psTo--;
        }
        pSc->sAverage = (S16)((ac0 + (1<<8))>>9);
    }
}
/*-------------------------------------------------------------------------*/
S16                         clid_match_byteC
/*-------------------------------------------------------------------------*/
(
S16 *psOut,
U16 *puByte
)
{
    S32 ac0 = 0;
    U16 uByte = 0;
    S16 sOffset = 0;
    S16 k;

    uByte = 0;
    for (k = 0; k < 10; k++)
    {
        S16 m;