clkrvfilter.c

st40 clock driver source code. 用于st40 linux

    while(1)
    {
        STOS_SemaphoreWait(CB_p->FilterSemaphore_p);

        if (CB_p->FilterTaskActive != TRUE)
        break;

        ActionPCR(CB_p);

    }

    STOS_TaskExit (NULL);

    STCLKRV_Print(("ClockRecoveryFilterTask Quit\n"));
}
#else /* Linux */
int ClockRecoveryFilterTask(STCLKRV_ControlBlock_t *CB_p)
{
    U32 event = STPTI_EVENT_PCR_RECEIVED_EVT;
    U32 eventID;

    STCLKRV_Print(("ClockRecoveryFilterTask Created..\n"));

    STOS_TaskEnter (NULL);

    while (CB_p->FilterTaskActive)
    {
        if (STPTI_WaitEventList( CB_p->PcrPtiHandle, 1, &event, &eventID, sizeof(CB_p->InstancePcrData),
                                 (void*)&(CB_p->InstancePcrData) ) != ST_NO_ERROR)
            break;

        /* Check handling control status */
        if (CB_p->ActvRefPcr.PCRHandlingActive != TRUE)
        {
            continue;
        }

        /* Check Slot validity */
        if ( (CB_p->SlotEnabled != TRUE) ||
             (CB_p->InstancePcrData.SlotHandle != CB_p->Slot) )   /* GNBVd06147 */
        {
            STTBX_Report( (STTBX_REPORT_LEVEL_ERROR, "Invalid handle or slot not enabled"));
            continue;
        }

        ActionPCR(CB_p);
    }

    /* Don't stop without permission */
    STOS_TaskExit (NULL);

    STCLKRV_Print(("ClockRecoveryFilterTask Quit\n"));

    return 0;
}
#endif


/****************************************************************************
Name         : ActionPCR()

Description  : Performs the Clock Recovery function from the clock values
               passed with the call, and previous history. Note that since
               the generation of the TP3 implementation on which this code
               is based, extended PTI functions have been added to return
               bit32 for both PCR and STC clock values. The extra bit will
               only become significant when there is a large difference
               (over 13 hours for b32 on 90kHz ticks) between the two
               clock values; this is unlikely to affect real-time Transport
               Streams.  The extended PCR function returns an extra boolean
               value, flagging PCR discontinuities. Its use should give
               faster recovery from (PTI expected) PCR dis-continuities, by
               effecting immediate re-basing.  The implementation would
               still have to sensibly handle unexpected PCR glitches and
               discontinuities anyway.

Parameters   : STCLKRV_ControlBlock_t InstanceHandle_p

Return Value : None

See Also     :
 ****************************************************************************/

void ActionPCR( STCLKRV_ControlBlock_t *InstanceHandle_p )
{
    /* For local storage of pcr event data passed on callback */
    BOOL DiscontinuousPCR;
    U32 PCRBaseBit32;
    U32 PCRBaseValue;
    U32 PCRExtension;
    U32 STCBaseBit32;
    U32 STCBaseValue;
    U32 STCExtension;


    STCLKRV_ControlBlock_t *TmpCB_p;
    U32     PCRTimeTotal;
    U32     STCTimeTotal;
    U32     RetVal = 0;


#ifdef CLKRV_FILTERDEBUG
    /* Debug use only */
    FilterData FData;
#endif

    TmpCB_p = (STCLKRV_ControlBlock_t *)InstanceHandle_p;

    /* Take semap. STPTI callbakck from event queue, so semap ok */
    STOS_SemaphoreWait(TmpCB_p->InstanceSemaphore_p);

    /* Load passed data to local vars */
#ifdef ST_5188
    DiscontinuousPCR =  TmpCB_p->InstancePcrData.member.PCREventData.DiscontinuityFlag;
    PCRBaseBit32 =      (U32) TmpCB_p->InstancePcrData.member.PCREventData.PCRBase.Bit32;
    PCRBaseValue =      (U32) TmpCB_p->InstancePcrData.member.PCREventData.PCRBase.LSW;
    PCRExtension =      (U32) TmpCB_p->InstancePcrData.member.PCREventData.PCRExtension;
    STCBaseBit32 =      (U32) TmpCB_p->InstancePcrData.member.PCREventData.PCRArrivalTime.Bit32;
    STCBaseValue =      (U32) TmpCB_p->InstancePcrData.member.PCREventData.PCRArrivalTime.LSW;
    STCExtension =      (U32) TmpCB_p->InstancePcrData.member.PCREventData.PCRArrivalTimeExtension;
#else
    DiscontinuousPCR =  TmpCB_p->InstancePcrData.u.PCREventData.DiscontinuityFlag;
    PCRBaseBit32 =      (U32) TmpCB_p->InstancePcrData.u.PCREventData.PCRBase.Bit32;
    PCRBaseValue =      (U32) TmpCB_p->InstancePcrData.u.PCREventData.PCRBase.LSW;
    PCRExtension =      (U32) TmpCB_p->InstancePcrData.u.PCREventData.PCRExtension;
    STCBaseBit32 =      (U32) TmpCB_p->InstancePcrData.u.PCREventData.PCRArrivalTime.Bit32;
    STCBaseValue =      (U32) TmpCB_p->InstancePcrData.u.PCREventData.PCRArrivalTime.LSW;
    STCExtension =      (U32) TmpCB_p->InstancePcrData.u.PCREventData.PCRArrivalTimeExtension;
#endif

    PCRTimeTotal = ( PCRBaseValue * CONV90K_TO_27M ) + PCRExtension;
    STCTimeTotal = ( STCBaseValue * CONV90K_TO_27M ) + STCExtension;

    /* determine whether we have a PCR glitch */
    if (DiscontinuousPCR || TmpCB_p->ActvRefPcr.AwaitingPCR)
    {
        /* First PCR so can't make comparison.  Use first
           PCR as reference - it may not be correct though */
        EnterCriticalSection();
        TmpCB_p->ActvRefPcr.Valid        = FALSE;
        TmpCB_p->ActvRefPcr.PcrBaseBit32 = PCRBaseBit32;
        TmpCB_p->ActvRefPcr.PcrBaseValue = PCRBaseValue;
        TmpCB_p->ActvRefPcr.PcrExtension = PCRExtension;
        TmpCB_p->ActvRefPcr.TotalValue   = PCRTimeTotal;
        TmpCB_p->ActvRefPcr.PcrArrivalBaseBit32 = STCBaseBit32;
        TmpCB_p->ActvRefPcr.PcrArrivalBaseValue = STCBaseValue;
        TmpCB_p->ActvRefPcr.PcrArrivalExtension = STCExtension;
        TmpCB_p->ActvRefPcr.Time = STCTimeTotal;
        TmpCB_p->ActvRefPcr.GlitchCount = 0;
        TmpCB_p->ActvRefPcr.Glitch      = FALSE;
        TmpCB_p->ActvRefPcr.AwaitingPCR = FALSE;
        TmpCB_p->ActvContext.Reset      = TRUE;       /* tell PWMFilter */
        LeaveCriticalSection();

#ifdef CLKRV_FILTERDEBUG
        /* Debug use only */

        /* Initialize the local filter data */
        memset(&FData, 0, sizeof(FilterData));

        FData.TotalSTCTime = STCTimeTotal;
        FData.TotalPCRTime = PCRTimeTotal;

        Dump_Debug_Data(1111, &FData, TmpCB_p);
#endif

    }
    else
    {

        /* PCR & reference seem plausible */
        TmpCB_p->ActvRefPcr.Valid = TRUE;

        /* adjust decode VCXO */
        RetVal = ClockRecoveryFilter(PCRTimeTotal, STCTimeTotal, TmpCB_p );

        EnterCriticalSection();
        switch(RetVal)
        {

            case PCR_NO_UPDATE:
                TmpCB_p->ActvRefPcr.GlitchCount++;
                break;

            case PCR_UPDATE_THIS_LAST_GLITCH:
                TmpCB_p->ActvRefPcr.Valid        = TRUE; /* ??? Added*/
                TmpCB_p->ActvRefPcr.PcrBaseBit32 = PCRBaseBit32;
                TmpCB_p->ActvRefPcr.PcrBaseValue = PCRBaseValue;
                TmpCB_p->ActvRefPcr.PcrExtension = PCRExtension;
                TmpCB_p->ActvRefPcr.TotalValue   = PCRTimeTotal;
                TmpCB_p->ActvRefPcr.PcrArrivalBaseBit32 = STCBaseBit32;
                TmpCB_p->ActvRefPcr.PcrArrivalBaseValue = STCBaseValue;
                TmpCB_p->ActvRefPcr.PcrArrivalExtension = STCExtension;
                TmpCB_p->ActvRefPcr.Time        = STCTimeTotal;
                TmpCB_p->ActvRefPcr.GlitchCount = 0;
                TmpCB_p->ActvRefPcr.Glitch      = TRUE; /* ??? Added*/
                break;

            case PCR_UPDATE_OK:
                /*Pcr Ok Update Reference*/
                TmpCB_p->ActvRefPcr.Valid        = TRUE; /*Added*/
                TmpCB_p->ActvRefPcr.PcrBaseBit32 = PCRBaseBit32;
                TmpCB_p->ActvRefPcr.PcrBaseValue = PCRBaseValue;
                TmpCB_p->ActvRefPcr.PcrExtension = PCRExtension;
                TmpCB_p->ActvRefPcr.TotalValue   = PCRTimeTotal;
                TmpCB_p->ActvRefPcr.PcrArrivalBaseBit32 = STCBaseBit32;
                TmpCB_p->ActvRefPcr.PcrArrivalBaseValue = STCBaseValue;
                TmpCB_p->ActvRefPcr.PcrArrivalExtension = STCExtension;
                TmpCB_p->ActvRefPcr.Time        = STCTimeTotal;
                TmpCB_p->ActvRefPcr.GlitchCount = 0;
                TmpCB_p->ActvRefPcr.Glitch      = FALSE;
                break;

            case PCR_INVALID:
                TmpCB_p->ActvRefPcr.Valid        = TRUE;
                TmpCB_p->ActvRefPcr.PcrBaseBit32 = PCRBaseBit32;
                TmpCB_p->ActvRefPcr.PcrBaseValue = PCRBaseValue;
                TmpCB_p->ActvRefPcr.PcrExtension = PCRExtension;
                TmpCB_p->ActvRefPcr.TotalValue   = PCRTimeTotal;
                TmpCB_p->ActvRefPcr.PcrArrivalBaseBit32 = STCBaseBit32;
                TmpCB_p->ActvRefPcr.PcrArrivalBaseValue = STCBaseValue;
                TmpCB_p->ActvRefPcr.PcrArrivalExtension = STCExtension;
                TmpCB_p->ActvRefPcr.Time = STCTimeTotal;
                TmpCB_p->ActvRefPcr.GlitchCount = 0;
                TmpCB_p->ActvRefPcr.Glitch      = TRUE;
                TmpCB_p->ActvRefPcr.AwaitingPCR = FALSE;
                TmpCB_p->ActvContext.Reset      = TRUE;       /* tell PWMFilter */

            case PCR_BURST:
                /* Ignore this PCR altogether, since
                   Previous reference PCR is OK anyway */
                break;

            default:
                break;
        }
        LeaveCriticalSection();
    }

    UpdateStateMachine( TmpCB_p, DiscontinuousPCR );

    STOS_SemaphoreSignal(TmpCB_p->InstanceSemaphore_p);

}

/****************************************************************************
Name         : ClockRecoveryFilter()

Description  : performs filter function for Clock Recovery, programming
               FS registers

               Filtering is performed in three steps

               1. Error sample calculation
                    a. CalculateFrequencyError()
                    b. Pass the error through the LowPassfilter

               2. Averaging the error samples
                    a. ApplyWeighingFilter over moving window
                    b. Calculate Average error

               3. Calculate Correction & apply using FS registers


Parameters   : U32 PCR Total Time and
               U32 Captured Time of the Transport Stream in which PCR arrived
               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
****************************************************************************/

U32 ClockRecoveryFilter( U32 TotalPCRTime,
                         U32 TotalSTCTime,
                         STCLKRV_ControlBlock_t *Instance_p )
{
    U32 ReturnCode    = (U32) PCR_UPDATE_OK;
    U8  SlaveClock = 0;
    FilterData FD;
    S32 CorrectionValue = 0;
    S32 SD_STC_Step = (S32) Instance_p->HWContext.Clocks[STCLKRV_CLOCK_SD_0].Step;

#ifdef CLKRV_FILTERDEBUG
    U32 Time2                   = 0;
    U32 Time1                   = 0;
#endif


    /* Initialize the local filter data */
    memset(&FD, 0, sizeof(FilterData));

    /* Store Current STC,PCR sample */
    FD.TotalSTCTime = TotalSTCTime;
    FD.TotalPCRTime = TotalPCRTime;

    /* Find current relative difference */
    FD.Difference = STOS_time_minus(FD.TotalSTCTime, FD.TotalPCRTime);

    /* If reset request then just reinitialise the pcr data */
    if( Instance_p->ActvContext.Reset )
    {
        Instance_p->ActvContext.Reset = FALSE;
        Instance_p->ActvContext.PrevDiff = FD.Difference;
        /* to find PCR time for frequeny error calculation */
        Instance_p->ActvContext.PrevPcr = FD.TotalPCRTime;

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

        return  (U32) PCR_UPDATE_OK;
    }

                /****************************
                * CALCULATE AN ERROR SAMPLE *
                ****************************/

    ReturnCode = CalculateFrequencyError(&FD, Instance_p);

    if( ReturnCode != ST_NO_ERROR )
    {
        /* Reutrns in two cases either 1.Glitch or 2.PCR Burst */
        return ReturnCode;
    }

                /*********************************************
                * DISCARD ANY LARGE ERRORS (LOW PASS FILTER) *
                *********************************************/

    /* This return code to be preserved */
    ReturnCode = LowPassFilter(&FD, Instance_p);

    /* If error is too large than Discard the (PCR, STC) sample */
    if((ReturnCode == (U32)PCR_INVALID) || (ReturnCode ==  (U32)PCR_NO_UPDATE))
    {
        return ReturnCode;
    }

    /* If error sample is good then Process the error sample */

    /* Store the current difference for use in next call */
    Instance_p->ActvContext.PrevDiff = FD.Difference;

    /* store the current pcr for use in next call*/
    Instance_p->ActvContext.PrevPcr = FD.TotalPCRTime ;


                /*********************************************
                * MAINTAIN CIRCULAR LIST FOR MOVING AVERAGE  *
                *********************************************/

    /* Store current error value  */
    Instance_p->ActvContext.ErrorStore_p[Instance_p->ActvContext.Head] = FD.CurrentFreqErrorValue;
    if (Instance_p->ActvContext.ErrorStoreCount < (S32)Instance_p->InitPars.MaxWindowSize)
    {
        Instance_p->ActvContext.ErrorStoreCount++;
    }

    if (Instance_p->ActvContext.ErrorStoreCount < (S32)Instance_p->InitPars.MinSampleThres)
    {
        /* Maintain circular list head pointer */
        Instance_p->ActvContext.Head++;

        /* Store current error for averaging later */
        Instance_p->ActvContext.ErrorSum += FD.CurrentFreqErrorValue;

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

        /* Not got enough data to average now, so exit */
        return  (U32) PCR_UPDATE_OK;
    }

    /* Maintain circular list head pointer */
    Instance_p->ActvContext.Head++;
    if (Instance_p->ActvContext.Head == Instance_p->InitPars.MaxWindowSize)
    {
        Instance_p->ActvContext.Head = 0;
    }

                /**********************************************
                * APPLY WEIGHING FILTER TO THE ERROR SAMPLES  *
                **********************************************/

#if defined (CLKRV_FILTERDEBUG)
    GetHPTimer(Time1);
#endif

    /* Apply Weighing filter to maintain the stability in PWM with Jittery Data */