clkrvfilter.c

st40 clock driver source code. 用于st40 linux

    ApplyWeighingFilter(Instance_p);

#if defined (CLKRV_FILTERDEBUG)
    GetHPTimer(Time2);
    FD.ExecutionTime = Time2 - Time1;
#endif

                /*************************************************************
                * CALCULATE AVERAGE ERROR, Correction in MASTER/SLAVE clocks *
                *************************************************************/

    /* Recalculate average */
    FD.AverageError = DivideAndRound(Instance_p->ActvContext.ErrorSum,
                                  Instance_p->ActvContext.ErrorStoreCount);


    /* Apply Correction gradually, divide the avg. error by a multiple of the
       number of sample in the window. AEPS may be too small to correct, So it
       carried forward in out standing error */

    /* Multilpied by PRECISION_FACTOR=128 to increase the accuracy to two decimal digits */

    FD.AverageErrorPerSample = ((FD.AverageError * PRECISION_FACTOR)
                                 / (Instance_p->ActvContext.ErrorStoreCount
                                   * GRADUAL_CORRECTION_FACTOR));


    /* Find new control value +/- step in the current frequency to apply. */
    /* SD_STEP is already a multiple of */

    CorrectionValue = ((FD.AverageErrorPerSample + Instance_p->ActvContext.OutStandingErr)
                            / SD_STC_Step);

    /* SD_STEP is already a multiple of PRECIOSION factor */
    Instance_p->ActvContext.OutStandingErr += (FD.AverageErrorPerSample
                                                 - (CorrectionValue * SD_STC_Step));

    /* Upper and Lower Limit controlled inside */

    /* If there is change necessary... */
    if (CorrectionValue != 0)
    {
        /* Correct Pixel clock */
        /* DDTS 51128 fix */
        clkrv_ChangeClockFreqBy( Instance_p,
                             Instance_p->HWContext.Clocks[0].ClockType,
                             CorrectionValue);
        /* Correct other clocks if valid */
        Instance_p->SlaveCorrectionBeingApplied = TRUE;
        for ( SlaveClock = 1; SlaveClock < Instance_p->HWContext.ClockIndex ; SlaveClock++)
        {
            if (Instance_p->HWContext.Clocks[SlaveClock].Valid)
            {
                clkrv_CorrectSlaveClocks( Instance_p,
                                          CorrectionValue,
                                          Instance_p->HWContext.Clocks[SlaveClock].ClockType);
            }
        }
        Instance_p->SlaveCorrectionBeingApplied = FALSE;
    }

#ifdef CLKRV_FILTERDEBUG
    Dump_Debug_Data(555, &FD, Instance_p);
#endif

    return ReturnCode;

}

/****************************************************************************
Name         : CalculateFrequencyError()

Description  : Calculation of Frequency Error between Encoder and Decoder.

                1. tick error = (S1-S0) - (P1-P0)

                2. time = (P1-P0) / 27000000

                3. Frequency Error = Tick Error / time

                3a. Frequency Error = Tick Error * ( 27000000 / time )

Parameters   : FilterData * Pointer to local filter data
               STCLKRV_ControlBlock_t *Instance_p) instance to operate on

Return Value : Return code
               PCR_NO_UPDATE specifies unvalid pcr
****************************************************************************/

static ST_ErrorCode_t CalculateFrequencyError(FilterData *FilterData_p,
                                                STCLKRV_ControlBlock_t *CB_p)
{
    U32 ElapsedPCRTime = 0;

    /* Find current relative difference */
    FilterData_p->Difference = STOS_time_minus(FilterData_p->TotalSTCTime,
                                                FilterData_p->TotalPCRTime );

    /* Time elapsed can only be positive */
    FilterData_p->PCRTickDiff = FilterData_p->TotalPCRTime - CB_p->ActvContext.PrevPcr;

    /* If PCR events are very close then Error calculation gives correct a quite large
       frequency error which is less than threshold. see DDTS - 28442 */

    if((FilterData_p->PCRTickDiff < MINIMUM_TIME_BETWEEN_PCR) &&
            ((FilterData_p->TotalSTCTime - CB_p->ActvRefPcr.Time) < MINIMUM_TIME_BETWEEN_PCR))
    {
#ifdef CLKRV_FILTERDEBUG
        Dump_Debug_Data(6001, FilterData_p, CB_p);
#endif
        return (ST_ErrorCode_t) PCR_BURST;
    }

    /* Tick Error */
    FilterData_p->CurrentTicksErrorValue = (S32)(CB_p->ActvContext.PrevDiff
                                                    - FilterData_p->Difference);
    /*
     * To Handle the OVERFLOW in the equation, 27MHz is
     *  first divided by time and then multiplied by Tick error
    */

    /* Handle Divide by Zero */
    if (FilterData_p->PCRTickDiff != 0)
    {
        /* encoder frequency assumed 27MHz Nominal */
        ElapsedPCRTime = DivideAndRound((S32)ENCODER_FREQUENCY, FilterData_p->PCRTickDiff);
    }
    else
    {
#ifdef CLKRV_FILTERDEBUG
        Dump_Debug_Data(7001, FilterData_p, CB_p);
#endif
        return (ST_ErrorCode_t) PCR_NO_UPDATE;
    }

    FilterData_p->CurrentFreqErrorValue = ((S32)ElapsedPCRTime * FilterData_p->CurrentTicksErrorValue);

    return ST_NO_ERROR;
}

/****************************************************************************
Name         : LowPassFilter()

Description  : Discard any Frequency Error sample which is Larger
               than maximum Error. Maximum error is calculated on the basis of
               value of PCRDriftThres init parameter.
               Maximum Error =  PCRDriftThres * 40,
               See details in API document

               It takes new Reference Point if PCRMaxGlitch number of
               continous large error found.

Parameters   : FilterData * Pointer to local filter data
               STCLKRV_ControlBlock_t *Instance_p) instance to operate on

Return Value : Return code
               PCR_UPDATE_OK specifies valid pcr
               PCR_NO_UPDATE specifies unvalid pcr
               PCR_UPDATE_THIS_LAST_GLITCH specifies Glitch due to jitter
****************************************************************************/

static ST_ErrorCode_t LowPassFilter(FilterData *FilterData_p, STCLKRV_ControlBlock_t *CB_p)
{

    U32 Error = 0 ;
    U32 ErrorThres = 0;
    U32 Glitches =0;

    /* Load in local variables */

    Error      = abs(FilterData_p->CurrentFreqErrorValue);
    ErrorThres = CB_p->ActvContext.MaxFrequencyError;
    Glitches   = CB_p->ActvRefPcr.GlitchCount;

    /* reference thought OK so glitch in PCR - if we have
       too many of these we should check the value of PCRDrfit */


    if (CB_p->ActvRefPcr.Valid)
    {
        if((Error >= ErrorThres) && (Glitches == 0))
        {

#ifdef CLKRV_FILTERDEBUG
            Dump_Debug_Data(1001, FilterData_p, CB_p);
#endif
            /* first large error, Discard this ERROR and no updation of the PCR */
            return (ST_ErrorCode_t) PCR_NO_UPDATE;

        }
        else if((Error < ErrorThres) && (Glitches == 1))
        {
            /* single spike of large error, Process this ERROR and update PCR */
             return (ST_ErrorCode_t) PCR_UPDATE_THIS_LAST_GLITCH;
        }
        else if( (Error >= ErrorThres) && (Glitches == 1))
        {

#ifdef CLKRV_FILTERDEBUG
            Dump_Debug_Data(1002, FilterData_p, CB_p);
#endif
            /* Second continous large error, Discard this ERROR and no updation of the PCR */
            return (ST_ErrorCode_t) PCR_NO_UPDATE;

        }
        else if( (Error < ErrorThres) && (Glitches >= CB_p->InitPars.PCRMaxGlitch))
        {
            /* +/- type of frequently occuring large errors */
            /* Process the ERROR and Update the PCR */
            return (ST_ErrorCode_t) PCR_UPDATE_THIS_LAST_GLITCH;
        }
        else if( (Error >= ErrorThres) && (Glitches >= CB_p->InitPars.PCRMaxGlitch))
        {

#ifdef CLKRV_FILTERDEBUG
            Dump_Debug_Data(3333, FilterData_p, CB_p);
#endif
            /* too many of large errors, take new reference point */
            return (ST_ErrorCode_t) PCR_INVALID;
        }
        else
        {
            return (ST_ErrorCode_t) PCR_UPDATE_OK;
        }
    }
    else
    {
        /* take new reference point */
        return (ST_ErrorCode_t) PCR_INVALID;
    }

}

/****************************************************************************
Name         : ApplyWeighingFilter()

Description  : A weighting pattern is applied to the error sample
               data. defined frac = 1 / half the window size, then
               val[0] weighting is frac, val[1] weighting is frac*2, etc.,
               up to the middle, then it starts going down again.
               Frac = 1/(Window Size/2)

               From start to middle

                i=0; i<mid; i++
                    sample = (sample * 2 * ( i - start))/ windowsize

               and from middle to end

                i=mid; i<end; i++
                    sample = (sample * 2 * ( end - i))/ windowsize


Parameters   : STCLKRV_ControlBlock_t *Instance_p) instance to operate on

Return Value : Return code
               NONE
****************************************************************************/

static void ApplyWeighingFilter(STCLKRV_ControlBlock_t *CB_p)
{

    S32 TempI     = 0;
    S32 WinOldest = 0;      /* index of oldest element */
    S32 WinLatest = 0;      /* index of latest element */
    S32 WinMid    = 0;
    S32 WinCount  = 0;      /* error sample count in the window */
    S32 WinEnd    = 0;
    S32 WinSum    = 0;      /* Final sum after weighing */

    WinCount = CB_p->ActvContext.ErrorStoreCount;
    WinSum   = 0;

    if (WinCount < (S32)CB_p->InitPars.MaxWindowSize)
    {
        WinOldest = 0;
        WinLatest = WinCount -1;
        WinMid    = WinCount / 2;
        WinEnd    = WinCount - 1;
    }
    else
    {
        WinLatest = CB_p->ActvContext.Head;

        WinOldest = WinLatest + 1;
        if (WinOldest > ((S32)CB_p->InitPars.MaxWindowSize-1))
            WinOldest = 0;

        WinMid   = WinCount / 2;
        WinEnd   = WinCount - 1;
    }

    /* Starting from element 2, Since first element is ZERO anyway */
    for(TempI=1;TempI<WinMid;TempI++)
    {
        WinSum += (CB_p->ActvContext.ErrorStore_p[(WinOldest + TempI) % WinCount]* TempI);
    }

    /* Last element is ZERO too */
    for(TempI=WinMid;TempI<(WinCount-1);TempI++)
    {
        WinSum += (CB_p->ActvContext.ErrorStore_p[(WinOldest + TempI) % WinCount]* (WinEnd - TempI));
    }

    WinSum *= 2;
    WinSum /= ((S32)CB_p->InitPars.MaxWindowSize);

    /* Store the Sum in context and Return */
    CB_p->ActvContext.ErrorSum = WinSum;
}

/****************************************************************************
Name         : DivideAndRound()

Description  : Divides SIGNED Numerator by UNSIGNED denominator and returns
               value rounded to nearest whole value.
               ASSUMES a postive denom always.

Parameters   : S32 Num : Numerator
               U32 Denom : Denominator

Return Value : S32 to rounded nearest whole number
****************************************************************************/
static S32 DivideAndRound( S32 Num, U32 Denom )
{
    S32 Quo;
    S32 Rem;

    BOOL Neg = FALSE;

    /* If Num is negative */
    if (Num < 0)
    {
        /* set neagitve flag and force all values to positive for calculation */
        Neg = TRUE;
        Num = abs(Num);
    }

    /* Calc whole value */
    Quo = (Num / (S32)Denom);

    /* Calc remainder */
    Rem = (Num % (S32)Denom);
    /* If remainder >= 0.5, round up */
    if ((2 * Rem) >= (S32)Denom)
    {
        Quo++;
    }

    /* If the Num was negative, quo is neg too */
    if (Neg == TRUE)
    {
        Quo = -Quo;
    }

    return (Quo);
}


/****************************************************************************
Name         : Dump_Debug_Data()

Description  : Dumps the key variable of filter

Parameters   : U32 Code specifies execution path
               FilterData *FilterData_p filter data structure
               STCLKRV_ControlBlock_t *Instance_p instance to operate on

Return Value : Return code
               NONE
****************************************************************************/

#ifdef CLKRV_FILTERDEBUG
void Dump_Debug_Data(U32 Code, FilterData *FilterData_p,
                        STCLKRV_ControlBlock_t *CB_p)
{
    if (SampleCount < CLKRV_MAX_SAMPLES)
    {
        ClkrvDbg[0][SampleCount]  = Code;
        ClkrvDbg[2][SampleCount]  = FilterData_p->TotalSTCTime;
        ClkrvDbg[3][SampleCount]  = FilterData_p->TotalPCRTime;
        ClkrvDbg[1][SampleCount]  = CB_p->HWContext.Clocks[STCLKRV_CLOCK_SD_0].Frequency;
                                    /* May be SD frequency*/;
        ClkrvDbg[2][SampleCount]  = FilterData_p->TotalSTCTime;
        ClkrvDbg[3][SampleCount]  = FilterData_p->TotalPCRTime;
        ClkrvDbg[4][SampleCount]  = FilterData_p->CurrentFreqErrorValue;
        ClkrvDbg[5][SampleCount]  = CB_p->ActvContext.ErrorStoreCount;
        ClkrvDbg[6][SampleCount]  = CB_p->ActvContext.ErrorSum;
        ClkrvDbg[7][SampleCount]  = FilterData_p->AverageError;
        ClkrvDbg[8][SampleCount]  = FilterData_p->CurrentTicksErrorValue;
        ClkrvDbg[9][SampleCount]  = FilterData_p->AverageErrorPerSample;
        ClkrvDbg[10][SampleCount] = CB_p->ActvContext.OutStandingErr;