clkrvfilter.c

st40 clock driver source code. 用于st40 linux

        ClkrvDbg[11][SampleCount] = FilterData_p->ExecutionTime;

        Clkrv_Slave_Dbg[0][SampleCount]  = CB_p->HWContext.Clocks[1].Frequency;
        Clkrv_Slave_Dbg[1][SampleCount]  = CB_p->HWContext.Clocks[2].Frequency;

        SampleCount++;
    }
}
#endif

/****************************************************************************
Name         : clkrv_CorrectSlaveClocks()

Description  : Calculates the Freq Change required by Slave Clocks depending
               on the Freq. change in the Master Clock.

Parameters   : Poniter to STCLKRV_ControlBlock_t instance
               Frequency Error to correct
               Clock Type to apply correction to.

Return Value :
****************************************************************************/

void clkrv_CorrectSlaveClocks( STCLKRV_ControlBlock_t *CB_p,
                                       S32 ControlVal,
                                       STCLKRV_ClockSource_t  Clock )
{
    U32 Index;

    Index = FindClockIndex(CB_p,Clock);

    if(CB_p->HWContext.Clocks[Index].Valid == 0 )
    {
        return ;
    }

    if (CB_p->HWContext.Clocks[Index].DriftError > 0)
    {
        ControlVal--;
        /* Error is corrected */
        CB_p->HWContext.Clocks[Index].DriftError = 0;
    }
    else if (CB_p->HWContext.Clocks[Index].DriftError < 0)
    {
        ControlVal++;
        /* Error is corrected */
        CB_p->HWContext.Clocks[Index].DriftError = 0;
    }
    else
    {/* Nothing */}

    if(ControlVal != 0)
    {
        clkrv_ChangeClockFreqBy(CB_p,Clock, ControlVal);
    }

    return ;
}

#endif /* STCLKRV_NO_PTI */

/****************************************************************************
Name         : clkrv_ChangeClockFreqBy

Description  : Changes a clock freq. by +/- freq (multiple of signed Control)

Parameters   : Pointer to STCLKRV_ControlBlock_t
               STCLKRV_ClockSource_t type clock
               S32 Control value

Return Value : None.

See Also     : clkrv_ProgramFSRegs
****************************************************************************/
void clkrv_ChangeClockFreqBy( STCLKRV_ControlBlock_t *CB_P,
                              STCLKRV_ClockSource_t Clock,
                              S32 Control )
{
    U16 ChangePos_ipe = 0;
    S16 ChangeNeg_ipe = 0;
    U16 ChangeBy      = 0;
    U32 Index;
    S8  md;
    U16 ipe;
    U32 Step;

    STSYS_DU32 *BaseAddress = NULL;

    Index = FindClockIndex(CB_P,Clock);

    /* SD can not change */
    md    = CB_P->HWContext.Clocks[Index].MD;
    ipe   = CB_P->HWContext.Clocks[Index].IPE;
    Step  = CB_P->HWContext.Clocks[Index].Step;

    BaseAddress = CB_P->InitPars.FSBaseAddress_p;

    /*
                 <- range  ->
      Min............0............Max
    */

    ChangeBy = abs(Control);

    if( Control > 0)
    {
        ChangePos_ipe = (ipe + ChangeBy);
        if (ChangePos_ipe  > 32767)
        {
            ipe = (ChangeBy  - (32768 - ipe));
            md--;
        }
        else
            ipe += ChangeBy;
    }
    else
    {
        ChangeNeg_ipe = ipe - ChangeBy;
        if (ChangeNeg_ipe < 0)
        {
            ipe = (32768 - (ChangeBy - ipe));
            md++;
        }
        else
            ipe -= ChangeBy;
    }

    /* Update last control value and frequency */
    CB_P->HWContext.Clocks[Index].Frequency += ((Control * (S32)Step)/ACCURACY_FACTOR);
    /* Only MD and IPE are supposed to be chnaged */
    CB_P->HWContext.Clocks[Index].MD   = md;
    CB_P->HWContext.Clocks[Index].IPE  = ipe;

    /* Switch to new freq. */
    clkrv_setClockFreq(CB_P, Clock, Index);

}

/****************************************************************************
Name         : clkrv_setClockFreq()

Description  : Program the FS registers with the Sdiv, Md and Ipe values

Parameters   : Pointer to STCLKRV_ControlBlock_t
               STCLKRV_ClockSource_t type clock

Return Value : ST_ErrorCode_t specified as
               ST_NO_ERROR                     No errors determined
               ST_ERROR_BAD_PARAMETER          Invalid Handle


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

#if defined (ST_5100) || defined (ST_5105) || defined (ST_5301) || defined (ST_5107)
ST_ErrorCode_t clkrv_setClockFreq( STCLKRV_ControlBlock_t *CB_p,
                                   STCLKRV_ClockSource_t Clock,
                                   U32 Index )
{

    U32 ClockOffset0 = 0;
    U32 ClockOffset1 = 0;
    U32 RegValue     = 0;
    U16 i = 0;
    U16 Sdiv;
    S8  Md;
    U16 Ipe;
    STSYS_DU32 *BaseAddress = NULL;

    BaseAddress = (STSYS_DU32*)(CB_p->InitPars.FSBaseAddress_p);

    Sdiv = CB_p->HWContext.Clocks[Index].SDIV;
    Md   = CB_p->HWContext.Clocks[Index].MD;
    Ipe  = CB_p->HWContext.Clocks[Index].IPE;

    if(!((Md  <= -1) && (Md  >= -17)))
        return (ST_ERROR_BAD_PARAMETER);

    if(!((Sdiv == 2)||(Sdiv==4) ||(Sdiv==8)||
         (Sdiv==16) ||(Sdiv==32)||(Sdiv==64)||
         (Sdiv==128)||(Sdiv==256)))
    {
        return (ST_ERROR_BAD_PARAMETER);
    }

    switch(Clock)
    {
        case STCLKRV_CLOCK_SD_0:
            ClockOffset0 = CLK_SD_0_SETUP0;
            ClockOffset1 = CLK_SD_0_SETUP1;
            break;
        case STCLKRV_CLOCK_PCM_0:
            ClockOffset0 = CLK_PCM_0_SETUP0;
            ClockOffset1 = CLK_PCM_0_SETUP1;
            break;
        case STCLKRV_CLOCK_SPDIF_0:
            ClockOffset0 = CLK_SPDIF_0_SETUP0;
            ClockOffset1 = CLK_SPDIF_0_SETUP1;
            break;
        default:
            return (ST_ERROR_BAD_PARAMETER);
    }

    /**************************************
    **************************************/

    /* Program the DCO_MODE_CFG 0x00000000 */
    clkrv_writeReg(BaseAddress, DCO_MODE_CFG, 0x00);

    /* Update sdiv / md / ipe */

    /* Update Sdiv */

    RegValue = 0;

    clkrv_readReg(BaseAddress, ClockOffset0, &RegValue);

    RegValue &= (~SDIV_MASK); /*0xFFFFE3F */

    i = 0;
    while(!((Sdiv >>= 1) & 0x01))
    {
        i++;
    }

    RegValue |= ((i << SD_BIT_OFFSET)& SDIV_MASK);

    /* Update Md */
    RegValue &= (~MD_MASK); /*0xFFFFFFE0*/
    RegValue |= (COMPLEMENT2S_FOR_5BIT_VALUE - abs(Md));

    /* "Freq synth normal use" & "Dig. Algo Works Normaly" & "O/P enabled" */
    RegValue |= (U32)FS_OUTPUT_NORMAL;

    /* Program SDIV & MD  value */
    clkrv_writeRegUnLock(BaseAddress, ClockOffset0, RegValue);

    /* Assumed that reserved Bits should be ZERO */
    RegValue = (Ipe & IPE_MASK);
    clkrv_writeRegUnLock(BaseAddress, ClockOffset1, RegValue);


    /* Program the DCO_MODE_CFG 0x00000001 */
    clkrv_writeReg(BaseAddress, DCO_MODE_CFG, 0x01);

    /* 10 neno-seconds Delay */
    /* DO NOT TOUCH these following Instruction they there to Give
     * required delay DDTS -HW for 5100 & 5105,5107
    */

    CLKRV_DELAY(10);

    /* Glitch free clock */
    /* Program the DCO_MODE_CFG 0x00000000 */
    clkrv_writeReg(BaseAddress, DCO_MODE_CFG, 0x00);

    return ST_NO_ERROR;
}


#elif defined (ST_5188) || defined (ST_5525)

ST_ErrorCode_t clkrv_setClockFreq( STCLKRV_ControlBlock_t *CB_p,
                                   STCLKRV_ClockSource_t Clock,
                                   U32 Index )
{

    U32 ClockOffset0 = 0;
    U32 ClockOffset1 = 0;
    U32 RegValue     = 0;
    U16 i = 0;
    U16 Sdiv;
    S8  Md;
    U16 Ipe;
    STSYS_DU32 *BaseAddress = NULL;

    BaseAddress = (STSYS_DU32*)(CB_p->InitPars.FSBaseAddress_p);

    Sdiv = CB_p->HWContext.Clocks[Index].SDIV;
    Md   = CB_p->HWContext.Clocks[Index].MD;
    Ipe  = CB_p->HWContext.Clocks[Index].IPE;

    if(!((Md  <= -1) && (Md  >= -17)))
        return (ST_ERROR_BAD_PARAMETER);

    if(!((Sdiv == 2)||(Sdiv==4) ||(Sdiv==8)||
         (Sdiv==16) ||(Sdiv==32)||(Sdiv==64)||
         (Sdiv==128)||(Sdiv==256)))
    {
        return (ST_ERROR_BAD_PARAMETER);
    }

    switch(Clock)
    {
        case STCLKRV_CLOCK_SD_0:
            ClockOffset0 = CLK_SD_0_SETUP0;
            ClockOffset1 = CLK_SD_0_SETUP1;
            break;
        case STCLKRV_CLOCK_PCM_0:
            ClockOffset0 = CLK_PCM_0_SETUP0;
            ClockOffset1 = CLK_PCM_0_SETUP1;
            break;
#if defined (ST_5525)
        case STCLKRV_CLOCK_SPDIF_0:
            ClockOffset0 = CLK_SPDIF_0_SETUP0;
            ClockOffset1 = CLK_SPDIF_0_SETUP1;
            break;
        case STCLKRV_CLOCK_SD_1:
            ClockOffset0 = CLK_SD_1_SETUP0;
            ClockOffset1 = CLK_SD_1_SETUP1;
            break;
        case STCLKRV_CLOCK_PCM_1:
            ClockOffset0 = CLK_PCM_1_SETUP0;
            ClockOffset1 = CLK_PCM_1_SETUP1;
            break;
        case STCLKRV_CLOCK_PCM_2:
            ClockOffset0 = CLK_PCM_2_SETUP0;
            ClockOffset1 = CLK_PCM_2_SETUP1;
            break;
        case STCLKRV_CLOCK_PCM_3:
            ClockOffset0 = CLK_PCM_3_SETUP0;
            ClockOffset1 = CLK_PCM_3_SETUP1;
            break;
#endif
        default:
            return (ST_ERROR_BAD_PARAMETER);
    }

    /**************************************
    **************************************/

    /* EN_PRG is ANDed with DCO_CFG_BITS
       DCO_CFG bits are enabled for all clocks
       EN_PRG = 0;
    */

    RegValue = 0;

    clkrv_readReg(BaseAddress, ClockOffset0, &RegValue);
    clkrv_writeReg(BaseAddress, ClockOffset0, RegValue&0xFFFFFFDF );

    /* Update sdiv / md / ipe */

    /* Update Sdiv */

    RegValue = 0;

    clkrv_readReg(BaseAddress, ClockOffset0, &RegValue);

    RegValue &= (~SDIV_MASK);

    i = 0;
    while(!((Sdiv >>= 1) & 0x01))
    {
        i++;
    }

    RegValue |= ((i << SD_BIT_OFFSET)& SDIV_MASK);

    /* Update Md */
    RegValue &= (~MD_MASK);
    RegValue |= (COMPLEMENT2S_FOR_5BIT_VALUE - abs(Md));

    /* "Freq synth normal use" & "Dig. Algo Works Normaly" & "O/P enabled" */
    RegValue |= (U32)FS_OUTPUT_NORMAL;

    /* Program SDIV & MD  value */
    clkrv_writeRegUnLock(BaseAddress, ClockOffset0, RegValue);

    /* Assumed that reserved Bits should be ZERO */
    RegValue = (Ipe & IPE_MASK);
    clkrv_writeRegUnLock(BaseAddress, ClockOffset1, RegValue);

    /* SET EN_PRG = 1 */
    clkrv_readReg(BaseAddress, ClockOffset0, &RegValue);
    clkrv_writeReg(BaseAddress, ClockOffset0, RegValue|0x20 );

    /* 10 nano-seconds Delay */
    /* DO NOT TOUCH these following Instruction they there to Give
     * required delay DDTS -HW for 5100 & 5105 5107
    */

    CLKRV_DELAY(10);

    /* Glitch free clock */
    /* EN_PRG = 0 */

    clkrv_readReg(BaseAddress, ClockOffset0, &RegValue);
    clkrv_writeReg(Ba