oc_saipll.c

#include <oc_saipll.h>
#include <oc_machine.h>
#include <oc_lsf.h>
#include <oc_clock_lld.h>
#include <oc_array.h>
#include <oc_math.h>
#include <oc_mcs.h>

#define _________________________________________MACROS_SECTION_____________________________________________________________________________

#define IsRam(Address)                      (oC_LSF_IsRamAddress(Address) || oC_LSF_IsExternalAddress(Address))
#define IsRom(Address)                      oC_LSF_IsRomAddress(Address)
#define RCC_CR                              oC_Register(RCC,RCC_CR)
#define RCC_PLLSAICFGR                      oC_Register(RCC,RCC_PLLSAICFGR)
#define RCC_PLLCFGR                         oC_Register(RCC,RCC_PLLCFGR)
#define RCC_DCKCFGR                        oC_Register(RCC,RCC_DCKCFGR)
#define IsOutputLineUsed(OutputLineIndex)   (WantedFrequencies[OutputLineIndex] != 0)
#define IsAnyOutputLineUsed()               (IsOutputLineUsed(oC_SaiPll_OutputLine_LTDC)        || \
                                             IsOutputLineUsed(oC_SaiPll_OutputLine_PLL48CLK)    || \
                                             IsOutputLineUsed(oC_SaiPll_OutputLine_SAICLK)     )
#define SetOutputLineUsed(OutputLineIndex,Freq,PermissibleDifference)       \
                                            WantedFrequencies[OutputLineIndex]      = Freq;\
                                            PermissibleDifferencies[OutputLineIndex]= PermissibleDifference
#define GetPllSaiPDivisor(RegisterValue)    ( ((RegisterValue+1)<<1) )
#define GetPllSaiDivRDivisor(RegisterValue) ( 0x2 <<(RegisterValue) )
#define CountFrequency(Input,Divisor)       ( (oC_Frequency_t) (((oC_Frequency_t)(Input))/( (oC_Frequency_t)(Divisor) )))
#define IsFrequencyAcceptable(WantedFrequency,PermissibleDifference,Frequency)      ( oC_ABS(WantedFrequency,Frequency) <= PermissibleDifference )
#define IsPLLSAIxCorrect(PLLSAIx,OutputLineIndex)           ( (PLLSAIx) >= PLLSAIxRanges[OutputLineIndex].Min && (PLLSAIx) <= PLLSAIxRanges[OutputLineIndex].Max )
#define IsPLLSAIDIVxCorrect(PLLSAIDIVx,OutputLineIndex)     ( (PLLSAIDIVx) >= PLLSAIDIVxRanges[OutputLineIndex].Min && (PLLSAIDIVx) <= PLLSAIDIVxRanges[OutputLineIndex].Max )

#undef  _________________________________________MACROS_SECTION_____________________________________________________________________________

#define _________________________________________TYPES_SECTION______________________________________________________________________________

typedef struct
{
    uint8_t         PLLM;
    uint16_t        PLLSAIN;

    union
    {
        struct
        {
            uint16_t        PLLSAIR;
            uint16_t        PLLSAIP;
            uint16_t        PLLSAIQ;
        };
        uint16_t    PLLSAIx[oC_SaiPll_OutputLine_NumberOfElements];
    };

    union
    {
        struct
        {
            uint16_t        PLLSAIDIVR;
            uint16_t        PLLSAIDIVP;
            uint16_t        PLLSAIDIVQ;
        };
        uint16_t    PLLSAIDIVx[oC_SaiPll_OutputLine_NumberOfElements];
    };
} PllConfig_t;

typedef struct
{
    oC_Frequency_t  PllSaiInput;
    oC_Frequency_t  VcoOutput;

    union
    {
        struct
        {
            oC_Frequency_t  PLLSAIR;
            oC_Frequency_t  PLLSAIP;
            oC_Frequency_t  PLLSAIQ;
        };
        oC_Frequency_t PLLSAIx[oC_SaiPll_OutputLine_NumberOfElements];
    };

    union
    {
        struct
        {
            oC_Frequency_t  LTDC;
            oC_Frequency_t  PLL48CLK;
            oC_Frequency_t  SAICLK;
        };
        oC_Frequency_t OutputLinesFrequencies[oC_SaiPll_OutputLine_NumberOfElements];
    };
} LineFrequencies_t;

typedef struct
{
    uint16_t    Min;
    uint16_t    Max;
} Range_t;

typedef struct
{
    uint16_t                ValueForRegister;
    oC_SaiPll_OutputLine_t  OutputLine;
} Divisor_t;

#undef  _________________________________________TYPES_SECTION______________________________________________________________________________

#define _________________________________________LOCAL_PROTOTYPES_SECTION___________________________________________________________________

static oC_ErrorCode_t    ReadCurrentPllConfig      ( PllConfig_t * outPllConfig );
static void              CountFrequencies          ( PllConfig_t * PllConfig , LineFrequencies_t * outFrequencies );
static bool              FindDivisorForOutputLine  ( PllConfig_t * PllConfig  , oC_SaiPll_OutputLine_t OutputLineIndex , oC_Frequency_t Frequency , oC_Frequency_t PermissibleDifference , oC_Frequency_t * outRealFrequency );
static oC_ErrorCode_t    FindNewPllConfig          ( PllConfig_t * PllConfig , oC_SaiPll_OutputLine_t OutputLineIndex , oC_Frequency_t Frequency , oC_Frequency_t PermissibleDifference , oC_Frequency_t * outRealFrequency );
static oC_ErrorCode_t    ConfigurePll              ( PllConfig_t * PllConfig , oC_SaiPll_OutputLine_t OutputLine );

#undef  _________________________________________LOCAL_PROTOTYPES_SECTION___________________________________________________________________

#define _________________________________________VARIABLES_SECTION__________________________________________________________________________

static oC_Frequency_t   WantedFrequencies[oC_SaiPll_OutputLine_NumberOfElements]        = {0};
static oC_Frequency_t   PermissibleDifferencies[oC_SaiPll_OutputLine_NumberOfElements]  = {0};
static const Range_t    PLLSAIxRanges[oC_SaiPll_OutputLine_NumberOfElements]            = {
                [oC_SaiPll_OutputLine_LTDC]     = { .Min = 2 , .Max =  7 },
                [oC_SaiPll_OutputLine_PLL48CLK] = { .Min = 0 , .Max =  3 },
                [oC_SaiPll_OutputLine_SAICLK]   = { .Min = 2 , .Max = 15 },
};
static const Range_t    PLLSAIDIVxRanges[oC_SaiPll_OutputLine_NumberOfElements]         = {
                [oC_SaiPll_OutputLine_LTDC]     = { .Min = 0 , .Max =  3 },
                [oC_SaiPll_OutputLine_PLL48CLK] = { .Min = 0 , .Max =  1 },
                [oC_SaiPll_OutputLine_SAICLK]   = { .Min = 0 , .Max = 31 },
};


#undef  _________________________________________VARIABLES_SECTION__________________________________________________________________________


#define _________________________________________INTERFACE_FUNCTIONS_SECTION________________________________________________________________

//==========================================================================================================================================
//==========================================================================================================================================
oC_ErrorCode_t oC_SaiPll_Configure(  oC_SaiPll_OutputLine_t OutputLine , oC_Frequency_t Frequency , oC_Frequency_t PermissibleDifference , oC_Frequency_t * outRealFrequency )
{
    oC_ErrorCode_t          errorCode       = oC_ErrorCode_ImplementError;
    oC_SaiPll_OutputLine_t  outputLineIndex = OutputLine & oC_SaiPll_OutputLine_IndexMask;

    if(
        ErrorCondition( outputLineIndex < oC_SaiPll_OutputLine_NumberOfElements , oC_ErrorCode_WrongParameters       ) &&
        ErrorCondition( Frequency > 0                                           , oC_ErrorCode_WrongFrequency        ) &&
        ErrorCondition( IsRam(outRealFrequency)                                 , oC_ErrorCode_OutputAddressNotInRAM )
        )
    {
        PllConfig_t pllConfig = {0};

        if(
            oC_AssignErrorCode(&errorCode , ReadCurrentPllConfig(&pllConfig)                                                              ) &&
            oC_AssignErrorCode(&errorCode , FindNewPllConfig(&pllConfig,outputLineIndex,Frequency,PermissibleDifference,outRealFrequency) ) &&
            oC_AssignErrorCode(&errorCode , ConfigurePll(&pllConfig,OutputLine)                                                           )
            )
        {
            SetOutputLineUsed(outputLineIndex,Frequency,PermissibleDifference);
            errorCode = oC_ErrorCode_None;
        }
    }

    return errorCode;
}

#undef  _________________________________________INTERFACE_FUNCTIONS_SECTION________________________________________________________________

#define _________________________________________LOCAL_FUNCTIONS_SECTION____________________________________________________________________

//==========================================================================================================================================
//==========================================================================================================================================
static oC_ErrorCode_t ReadCurrentPllConfig( PllConfig_t * outPllConfig )
{
    oC_ErrorCode_t errorCode = oC_ErrorCode_None;

    oC_MCS_EnterCriticalSection();

    outPllConfig->PLLM      = RCC_PLLCFGR->PLLM;
    outPllConfig->PLLSAIN   = RCC_PLLSAICFGR->PLLSAIN;
    outPllConfig->PLLSAIP   = RCC_PLLSAICFGR->PLLSAIP;
    outPllConfig->PLLSAIQ   = RCC_PLLSAICFGR->PLLSAIQ;
    outPllConfig->PLLSAIR   = RCC_PLLSAICFGR->PLLSAIR;

    outPllConfig->PLLSAIDIVQ= RCC_DCKCFGR->PLLSAIDIVQ;
    outPllConfig->PLLSAIDIVR= RCC_DCKCFGR->PLLSAIDIVR;
    outPllConfig->PLLSAIDIVP= 1;

    oC_MCS_ExitCriticalSection();

    return errorCode;
}


//==========================================================================================================================================
//==========================================================================================================================================
static void CountFrequencies( PllConfig_t * PllConfig , LineFrequencies_t * outFrequencies )
{
    oC_Frequency_t oscillatorFrequency = oC_CLOCK_LLD_GetOscillatorFrequency();

    /* ********************************************************************
     *                                                                    *
     *      If these assertions failed, then something goes really wrong. *
     * Probably registers addresses are not set correctly, cause this     *
     * cannot be 0.                                                       *
     *      The other option is that the local functions are not called   *
     * correctly - this is mean, that PllConfig is not initialized before *
     * usage. It should be read from the machine registers by using       *
     * function 'ReadCurrentPllConfig'                                    *
     *                                                                    *
     * ********************************************************************/
    oC_ASSERT(PllConfig->PLLSAIQ > 0);
    oC_ASSERT(PllConfig->PLLSAIR > 0);

    outFrequencies->PllSaiInput = CountFrequency(oscillatorFrequency , PllConfig->PLLM);
    outFrequencies->VcoOutput   = outFrequencies->PllSaiInput * ((oC_Frequency_t)PllConfig->PLLSAIN);
    outFrequencies->PLLSAIP     = CountFrequency(outFrequencies->VcoOutput , GetPllSaiPDivisor(PllConfig->PLLSAIP));
    outFrequencies->PLLSAIQ     = CountFrequency(outFrequencies->VcoOutput , PllConfig->PLLSAIQ);
    outFrequencies->PLLSAIR     = CountFrequency(outFrequencies->VcoOutput , PllConfig->PLLSAIR);
    outFrequencies->LTDC        = CountFrequency(outFrequencies->PLLSAIR   , GetPllSaiDivRDivisor(PllConfig->PLLSAIDIVR));
    outFrequencies->SAICLK      = CountFrequency(outFrequencies->PLLSAIQ   , PllConfig->PLLSAIDIVQ + 1);
    outFrequencies->PLL48CLK    = outFrequencies->PLLSAIP;

}

//==========================================================================================================================================
//==========================================================================================================================================
static bool FindDivisorForOutputLine(PllConfig_t * PllConfig , oC_SaiPll_OutputLine_t OutputLineIndex , oC_Frequency_t Frequency , oC_Frequency_t PermissibleDifference , oC_Frequency_t * outRealFrequency )
{
    bool                found           = false;
    LineFrequencies_t   newFrequencies  = {0};

    for(uint16_t PLLSAIx = PLLSAIxRanges[OutputLineIndex].Min ; !found && PLLSAIx <= PLLSAIxRanges[OutputLineIndex].Max ; PLLSAIx++)
    {
        PllConfig->PLLSAIx[OutputLineIndex] = PLLSAIx;

        for(uint16_t PLLSAIDIVx = PLLSAIDIVxRanges[OutputLineIndex].Min ; PLLSAIDIVx <= PLLSAIDIVxRanges[OutputLineIndex].Max ; PLLSAIDIVx++)
        {
            PllConfig->PLLSAIDIVx[OutputLineIndex] = PLLSAIDIVx;

            CountFrequencies(PllConfig , &newFrequencies);

            if(IsFrequencyAcceptable(Frequency,PermissibleDifference,newFrequencies.OutputLinesFrequencies[OutputLineIndex]))
            {
                found             = true;
                if(outRealFrequency)
                {
                    *outRealFrequency = newFrequencies.OutputLinesFrequencies[OutputLineIndex];
                }
                break;
            }
        }
    }

    return found;
}

//==========================================================================================================================================
//==========================================================================================================================================
static oC_ErrorCode_t FindNewPllConfig( PllConfig_t * PllConfig , oC_SaiPll_OutputLine_t OutputLineIndex , oC_Frequency_t Frequency , oC_Frequency_t PermissibleDifference , oC_Frequency_t * outRealFrequency )
{
    oC_ErrorCode_t errorCode = oC_ErrorCode_FrequencyNotPossible;

    oC_MCS_EnterCriticalSection();

    if(IsAnyOutputLineUsed() && FindDivisorForOutputLine(PllConfig,OutputLineIndex,Frequency,PermissibleDifference,outRealFrequency))
    {
        /* **************************************
         * Some PLL output is used, so we must  *
         * try to reconfigure the PLL first     *
         * **************************************/
        errorCode = oC_ErrorCode_None;
    }

    if(oC_ErrorOccur(errorCode))
    {
        /* ****************************************
         * Frequency not found, not possible or
         * none of output lines is used, so
         * we must/can to try reconfigure PLL
         * ****************************************/

        for(PllConfig->PLLSAIN = 49 ; PllConfig->PLLSAIN < 433 ; PllConfig->PLLSAIN++)
        {
            bool foundAll = true;

            for( oC_SaiPll_OutputLine_t outputLineIndex = 0 ; outputLineIndex < oC_SaiPll_OutputLine_NumberOfElements && foundAll ; outputLineIndex++ )
            {
                if(OutputLineIndex == outputLineIndex)
                {
                    foundAll = foundAll && FindDivisorForOutputLine(PllConfig,OutputLineIndex,Frequency,PermissibleDifference,outRealFrequency);
                }
                else if(IsOutputLineUsed(outputLineIndex))
                {
                    foundAll = foundAll && FindDivisorForOutputLine(PllConfig,outputLineIndex,WantedFrequencies[outputLineIndex],PermissibleDifferencies[outputLineIndex],NULL);
                }
            }

            if(foundAll)
            {
                /* We found all needed frequencies, so we can stop here */
                errorCode = oC_ErrorCode_None;
                break;
            }
        }
    }

    oC_MCS_ExitCriticalSection();

    return errorCode;
}

//==========================================================================================================================================
//==========================================================================================================================================
static oC_ErrorCode_t ConfigurePll( PllConfig_t * PllConfig , oC_SaiPll_OutputLine_t OutputLine )
{
    oC_ErrorCode_t errorCode = oC_ErrorCode_None;

    for(oC_SaiPll_OutputLine_t outputLineIndex = 0; outputLineIndex < oC_SaiPll_OutputLine_NumberOfElements ; outputLineIndex++ )
    {
        uint16_t PLLSAIx    = PllConfig->PLLSAIx[outputLineIndex];
        uint16_t PLLSAIDIVx = PllConfig->PLLSAIDIVx[outputLineIndex];

        if( !IsPLLSAIxCorrect(PLLSAIx , outputLineIndex) )
        {
            errorCode = oC_ErrorCode_MachineCanBeDamaged;
        }
        if( !IsPLLSAIDIVxCorrect(PLLSAIDIVx , outputLineIndex) )
        {
            errorCode = oC_ErrorCode_MachineCanBeDamaged;
        }
    }

    if(!oC_ErrorOccur(errorCode))
    {
        oC_MCS_EnterCriticalSection();

        RCC_CR->PLLSAION = 0;

        while(RCC_CR->PLLSAIRDY == 1);

        RCC_PLLSAICFGR->PLLSAIN = PllConfig->PLLSAIN;
        RCC_PLLSAICFGR->PLLSAIP = PllConfig->PLLSAIP;
        RCC_PLLSAICFGR->PLLSAIQ = PllConfig->PLLSAIQ;
        RCC_PLLSAICFGR->PLLSAIR = PllConfig->PLLSAIR;

        RCC_DCKCFGR->PLLSAIDIVR = PllConfig->PLLSAIDIVR;
        RCC_DCKCFGR->PLLSAIDIVQ = PllConfig->PLLSAIDIVQ;

        if(OutputLine & oC_SaiPll_OutputLine_SAI1CLK)
        {
            RCC_DCKCFGR->SAI1SEL = 0;
        }
        if(OutputLine & oC_SaiPll_OutputLine_SAI2CLK)
        {
            RCC_DCKCFGR->SAI2SEL = 0;
        }

        RCC_CR->PLLSAION = 1;

        while(RCC_CR->PLLSAIRDY == 0);

        oC_MCS_ExitCriticalSection();
    }

    return errorCode;
}

#undef  _________________________________________LOCAL_FUNCTIONS_SECTION____________________________________________________________________