/*------------------------------------------------------------------------------
--  This file is a part of the libuc, microcontroler library
--  Copyright (C) 2011, Alexis Jeandet
--
--  This program is free software; you can redistribute it and/or modify
--  it under the terms of the GNU General Public License as published by
--  the Free Software Foundation; either version 3 of the License, or
--  (at your option) any later version.
--
--  This program is distributed in the hope that it will be useful,
--  but WITHOUT ANY WARRANTY; without even the implied warranty of
--  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
--  GNU General Public License for more details.
--
--  You should have received a copy of the GNU General Public License
--  along with this program; if not, write to the Free Software
--  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA 
-------------------------------------------------------------------------------
--                 Author : Alexis Jeandet
--                   Mail : alexis.jeandet@gmail.com
-------------------------------------------------------------------------------*/
#include <core.h>
#include <stm32f4xx_rcc.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdio.h>
#include <core_cm4.h>

extern uint32_t OSC0;
extern uint32_t INTOSC;
extern uint32_t RTCOSC;

volatile uint32_t tickCounter=0;


void SysTick_Handler(void)
{
    tickCounter+=1;
}

void delay_us(uint32_t value)
{
    extern uint32_t currentCpuFreq;
    if(value)
    {
        uint32_t tickperus=currentCpuFreq/(1000*10);
        uint32_t tickCounterSnap = SysTick->VAL+((value%100)*tickperus);
        uint32_t targetVal=tickCounterSnap +(value/100);
        if(targetVal < tickCounterSnap)
        {
            while(tickCounter > targetVal);
        }
        while((tickCounter < targetVal) | (SysTick->VAL<tickCounterSnap));
    }
}

void delay_100us(uint32_t value)
{
    if(value)
    {
        uint32_t tickCounterSnap = tickCounter;
        uint32_t SysTickSnap = SysTick->VAL;
        uint32_t targetVal=tickCounterSnap +(value);
        if(targetVal < tickCounterSnap)
        {
            while(tickCounter > targetVal);
        }
        while((tickCounter < targetVal) | (SysTick->VAL<SysTickSnap));
    }
}

uint32_t getAPB1Freq()
{
    RCC_ClocksTypeDef RCC_ClocksStatus;
    RCC_GetClocksFreq(&RCC_ClocksStatus);
    return RCC_ClocksStatus.PCLK1_Frequency;
}

uint32_t getAPB2Freq()
{
    RCC_ClocksTypeDef RCC_ClocksStatus;
    RCC_GetClocksFreq(&RCC_ClocksStatus);
    return RCC_ClocksStatus.PCLK2_Frequency;
}


uint32_t getCpuFreq()
{
    uint32_t cpufreq = OSC0;
    uint32_t PLLN,PLLM,PLLP;

    if((RCC->CFGR & 0xC) == 8) //PLL used as sys clk
    {
        uint32_t pllinput=INTOSC;
        if((RCC->PLLCFGR & (1<<22)) == (1<<22))
        {
            pllinput=OSC0;
        }
        PLLN = (RCC->PLLCFGR>>6) & 0x1FF;
        PLLM = RCC->PLLCFGR & 0x3F;
        PLLP = 1<<(((RCC->PLLCFGR>>16) & 3 )+1);
        cpufreq = (pllinput * PLLN )/(PLLM*PLLP);
    }
    else if((RCC->CFGR & 0xC) == 0) //HSI used as sys clk
    {
        cpufreq=INTOSC;
    }
    if((RCC->CFGR & (1<<7))==1<<7)
    {
        return cpufreq>>((RCC->CFGR & (7<<4))>>4);
    }
    return cpufreq;
}

void reset_AHB1()
{
    RCC->AHB1RSTR = -1;
    RCC->AHB1RSTR = 0;
}

void reset_AHB2()
{
    RCC->AHB2RSTR = -1;
    RCC->AHB2RSTR = 0;
}

void reset_APB1()
{
    RCC->APB1RSTR = -1;
    RCC->APB1RSTR = 0;
}

void reset_APB2()
{
    RCC->APB2RSTR = -1;
    RCC->APB2RSTR = 0;
}



/*
       | 2.7->3.6V
-------------------------
  0WS  | 0<HCLK<=30
  1WS  | 30<HCLK<=60
  2WS  | 60<HCLK<=90
  3WS  | 90<HCLK<=120
  4WS  | 120<HCLK<=150
  5WS  | 150<HCLK<=168

f(VCO clock) = f(PLL clock input) × (PLLN / PLLM) [1]
64MHz <= f(VCO clock) <= 432MHz [2]

f(VCO clock input) must be between 1MHz and 2MHz and as close to 2MHz as possible!! [3]

f(PLL general clock output) = f(VCO clock) / PLLP  [4]

CPU<168MHz  AHB1<168MHz AHB2<168MHz  APB1<42MHz APB2<84MHz  [5]

! 63<=PLLN<=432  [6]
!  2<=PLLM<=63   [7]
!  PLLP=2,4,6,8  [8]
   4<=PLLM*PLLP<=504

F= f(PLL clock input) * A/B with
    63<=A<=432
     4<=B<=504

*/


int optimizePLLcfg(uint32_t freq, uint32_t srcfreq,uint32_t PLLM,uint32_t* PLLP, uint32_t* PLLN,uint8_t* AHBPRindx)
{
    uint32_t AHBPRtbl[9]={1,2,4,8,16,64,128,256,512};
    uint32_t AHBPR=0,AHBPR_r=0,PLLN_r=0,PLLP_r=0;
    uint32_t Fplli=0;
    int32_t f_error=100000000;
    int32_t f_errornw=100000000;
    Fplli = srcfreq / PLLM;
    //not efficient but should find the best parameters
    for((*AHBPRindx)=0;(*AHBPRindx)<9;(*AHBPRindx)++) //lowest priority
    {
        AHBPR = AHBPRtbl[(*AHBPRindx)];
        for(*PLLP=2;*PLLP<9;*PLLP+=2)
        {
            *PLLN = (freq*(*PLLP)*AHBPR)/Fplli;
            if(((*PLLN)>62) && ((*PLLN)<433) && ((Fplli * (*PLLN)) < 433000000))
            {
                f_errornw = abs((int32_t)((int32_t)((Fplli*(*PLLN))/((*PLLP)*AHBPR))-freq));
                if( ( (f_error)>(f_errornw) ) || ( (*AHBPRindx==0)&&(*PLLP==2)&&(*PLLN==63) ) )
                {
                    f_error=f_errornw;
                    PLLN_r = *PLLN;
                    PLLP_r = *PLLP;
                    AHBPR_r=*AHBPRindx;
                    if(f_error==0)
                    {
                        *PLLN = PLLN_r;
                        *PLLP = PLLP_r;
                        *AHBPRindx = AHBPR_r;
                        return 1;
                    }
                }
            }
        }
    }
    *PLLN = PLLN_r;
    *PLLP = PLLP_r;
    *AHBPRindx = AHBPR_r;
    return 1;
}


int setPll(uint32_t freq)
{
    extern uint32_t OSC0;
    extern uint32_t INTOSC;
    uint32_t srcfreq = INTOSC;
    uint8_t AHBPRindx;
    uint32_t AHBPRtbl[9]={1,2,4,8,16,64,128,256,512};
    uint32_t PLLN=0,PLLM=0,PLLP=0,AHBPR=0;
    uint32_t Fplli=0;
    if((RCC->PLLCFGR & (1<<22))==(1<<22))
    {
        srcfreq = OSC0;
    }
    PLLM = srcfreq / 1500000;   // [3]
    Fplli = srcfreq / PLLM;
    optimizePLLcfg(freq,srcfreq,PLLM,&PLLP,&PLLN,&AHBPRindx);
    srcfreq = (Fplli*PLLN)/(PLLP*AHBPRtbl[AHBPRindx]);   //Put real clk freq in srcfreq for return value
    //now switch to HSIs
    if((RCC->CR & 1)==0)RCC->CR |= 1;                      //turn ON HSI
    while((RCC->CR & 2)!=2);                               //wait for HSI Ready
    RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_SW));
    RCC->CFGR |= RCC_CFGR_SW_HSI;                          //set HSI as main clk
    while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS ) != RCC_CFGR_SWS_HSI);
    RCC->CR &= ~(1<<24);                                   //Turn OFF PLL
    RCC->PLLCFGR &= ~0x37FFF;                              //clear PLLP PLLM PLLN
    RCC->PLLCFGR |= PLLM + (PLLN<<6) + (((PLLP>>1) -1)<<16);
    RCC->CR |= RCC_CR_PLLON;                                      //Turn ON PLL
    while((RCC->CR & (1<<25))!=(1<<25));                   //wait for PLL Ready
    if(AHBPRindx!=0)AHBPRindx|=0x8;
    RCC->CFGR &= ~(0xF<<4);
    RCC->CFGR |= (uint32_t)(AHBPRindx<<4);
    AHBPR=0;
    while((srcfreq>>AHBPR)>42000000)AHBPR++; //[5]             //Thune APB1 prescaler to keep APB1 CLK below 42MHz
    if(AHBPR!=0)
    {
        AHBPR-=1;
        AHBPR|=0x4;
    }
    RCC->CFGR &= ~(0x7<<10);
    RCC->CFGR |= (uint32_t)(AHBPR<<10);
    AHBPR=0;
    while((srcfreq>>AHBPR)>84000000)AHBPR++; //[5]             //Thune APB2 prescaler to keep APB2 CLK below 42MHz
    if(AHBPR!=0)
    {
        AHBPR-=1;
        AHBPR|=0x4;
    }
    RCC->CFGR &= ~(0x7<<13);
    RCC->CFGR |= (uint32_t)(AHBPR<<13);
    FLASH->ACR |= FLASH_ACR_LATENCY_7WS;
    RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_SW));//Switch to PLL as main clk source
    RCC->CFGR |= RCC_CFGR_SW_PLL;
    /* Wait till the main PLL is used as system clock source */
    while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS ) != RCC_CFGR_SWS_PLL);
    if(srcfreq>150000000)
    {
        FLASH->ACR &= (~7)|FLASH_ACR_LATENCY_5WS;
    }
    if((srcfreq<150000000) && (srcfreq>=120000000))
    {
        FLASH->ACR &= (~7)|FLASH_ACR_LATENCY_4WS;
    }
    if((srcfreq<120000000) && (srcfreq>=90000000))
    {
        FLASH->ACR &= (~7)|FLASH_ACR_LATENCY_3WS;
    }
    if((srcfreq<90000000) && (srcfreq>=60000000))
    {
        FLASH->ACR &= (~7)|FLASH_ACR_LATENCY_2WS;
    }
    if((srcfreq<60000000) && (srcfreq>=30000000))
    {
        FLASH->ACR &= (~7)|FLASH_ACR_LATENCY_1WS;
    }
    if(srcfreq<30000000)
    {
        FLASH->ACR &= (~7)|FLASH_ACR_LATENCY_0WS;
    }
    return srcfreq;
}

void configureSysTick()
{
    extern uint32_t currentCpuFreq;
    uint32_t us=currentCpuFreq/(1000*10);
    SysTick_Config(us);
}

int setCpuFreq(uint32_t freq)
{
    extern uint32_t OSC0;
    extern uint32_t INTOSC;
    uint8_t i=0;
    uint32_t curentFeq = getCpuFreq();
    if(curentFeq==freq)return curentFeq;
    if((freq>2000000) && (freq<=250000000)) //be carefull with 250MHz!!!
    {
        if((RCC->CFGR & 0xC) == 8) //PLL used as sys clk
        {
            return setPll(freq);
        }
        else if((RCC->CFGR & 0xC) == 0) //HSI used as sys clk
        {
            if((INTOSC%freq)==0) //now check if we can directly divide HSI
            {
                if(freq==INTOSC)
                {
                    RCC->CFGR &= ~(0xF<<4);
                    return freq;
                }
                for(i=1;i<8;i++)
                {
                    if((freq<<i)==INTOSC)
                    {
                        RCC->CFGR &= ~(0xF<<4);
                        RCC->CFGR |= ((0x8|i)<<4);
                        return freq;
                    }
                }
            }
            else
                return setPll(freq);
        }
        else //HSE used as sys clk
        {
            if((OSC0%freq)==0) //now check if we can directly divide HSI
            {
                if(freq==OSC0)
                {
                    RCC->CFGR &= ~(0xF<<4);
                    return freq;
                }
                for(i=1;i<8;i++)
                {
                    if((freq<<i)==OSC0)
                    {
                        RCC->CFGR &= ~(0xF<<4);
                        RCC->CFGR |= ((0x8|i)<<4);
                        return freq;
                    }
                }
            }
            else
                return setPll(freq);
        }
    }
    return 0;
}


void enable_FPU()
{
    SCB->CPACR |= ((3UL << 10*2)|(3UL << 11*2));  /* set CP10 and CP11 Full Access */
    __asm__("dsb");
    __asm__("isb");
}