#include <fe_global.h>
#include <fe_clocks.h>

unsigned int sysclk_speed;
unsigned int ahb_speed;
unsigned int apb1_speed;
unsigned int apb2_speed;
unsigned int apb1_timer_speed;
unsigned int apb2_timer_speed;


void delay_ms(int ms) {
    uint32_t init_val = SysTick->VAL;
    uint32_t wait_end = sys_time_millis + ms;

    while (sys_time_millis < wait_end)
        ;

    while (SysTick->VAL >= init_val) {
        if (sys_time_millis > wait_end)
            return;
    }
}

void fe_config_clocks()
{
    /* 8MHz HSI clock as PLL source. */
#define HSI_SPEED 8000000
    /* PLL output = HSI / 2 * PLL_MUL */
#define PLL_MUL 16

    /* Check that we came out of reset correctly */
    if (((RCC->CFGR & RCC_CFGR_SWS_Msk) >> RCC_CFGR_SW_Pos) != 0)
        asm volatile ("bkpt");
    if (RCC->CR & RCC_CR_HSEON)
        asm volatile ("bkpt");
    if (RCC->CR & RCC_CR_PLLON)
        asm volatile ("bkpt");

    RCC->CFGR = 0;
    RCC->CFGR |= (0<<RCC_CFGR_PLLSRC_Pos); /* PLL input: HSI /2 */
    RCC->CFGR |= ((PLL_MUL-2)<<RCC_CFGR_PLLMUL_Pos);
    sysclk_speed = HSI_SPEED / 2 * PLL_MUL;

    /* set AHB prescaler to /1 */
    RCC->CFGR |= (0 << RCC_CFGR_HPRE_Pos);
    ahb_speed = sysclk_speed;

    /* set ABP1 prescaler to 2 -> 36MHz */
    RCC->CFGR |= (4 << RCC_CFGR_PPRE1_Pos);
    apb1_speed = sysclk_speed / 2;
    apb1_timer_speed = apb1_speed * 2;

    /* set ABP2 prescaler to 2 -> 36MHz */
    RCC->CFGR |= (4 << RCC_CFGR_PPRE2_Pos);
    apb2_speed = sysclk_speed / 2;
    apb2_timer_speed = apb2_speed * 2;

    /* Configure PLL */
    RCC->CR |= RCC_CR_PLLON;

    /* Wait for main PLL */
    while(!(RCC->CR & RCC_CR_PLLRDY))
        ;

    /* Configure Flash: enable prefetch; set latency = 2 wait states
     * See reference manual (RM0365), Section 4.5.1
     */
    FLASH->ACR = FLASH_ACR_PRFTBE | (2<<FLASH_ACR_LATENCY_Pos);

    /* Select PLL as system clock source */
    RCC->CFGR &= ~RCC_CFGR_SW_Msk;
    RCC->CFGR |= 2 << RCC_CFGR_SW_Pos;

    /* Wait for clock to switch over */
    while ((RCC->CFGR & RCC_CFGR_SWS_Msk)>>RCC_CFGR_SWS_Pos != 2)
        ;

    SystemCoreClockUpdate();
    SysTick_Config(SystemCoreClock / 1000); /* 1 ms ticks */

    NVIC_SetPriority(SysTick_IRQn, 32);
}