#include "board.h"

#include "app_config.h"
#include <stdbool.h>
#include "stm32g474xx.h"

#define CLOCK_STARTUP_TIMEOUT       1000000UL

static volatile uint32_t s_ms_ticks;

static void SystemClock_Config(void);
static bool wait_mask_set(volatile uint32_t *reg, uint32_t mask);
static bool wait_mask_clear(volatile uint32_t *reg, uint32_t mask);
static bool wait_mask_value(volatile uint32_t *reg, uint32_t mask, uint32_t value);
static bool Flash_SetLatency(uint32_t latency);
static void Clock_UseHsi16(void);

void Board_Init(void)
{
    SystemClock_Config();

    if (SysTick_Config(SystemCoreClock / 1000U) != 0U)
    {
        Board_FatalError();
    }
}

uint32_t Board_Millis(void)
{
    return s_ms_ticks;
}

void Board_DelayMs(uint32_t delay_ms)
{
    uint32_t start = Board_Millis();
    while ((Board_Millis() - start) < delay_ms)
    {
        __NOP();
    }
}

void Board_FatalError(void)
{
    __disable_irq();
    while (1)
    {
        __NOP();
    }
}

void SysTick_Handler(void)
{
    s_ms_ticks++;
}

static void SystemClock_Config(void)
{
    Clock_UseHsi16();

#if CLOCK_USE_PLL_170MHZ
    /* Enable PWR and switch the STM32G474 regulator to Range 1 boost. */
    RCC->APB1ENR1 |= RCC_APB1ENR1_PWREN;
    (void)RCC->APB1ENR1;

    PWR->CR1 = (PWR->CR1 & ~PWR_CR1_VOS) | PWR_CR1_VOS_0;
    PWR->CR5 &= ~PWR_CR5_R1MODE;
    if (!wait_mask_clear(&PWR->SR2, PWR_SR2_VOSF))
    {
        Clock_UseHsi16();
        return;
    }

    /*
     * ST G474 power examples use FLASH_LATENCY_4 for 170 MHz.
     * Set only the latency field first, then switch SYSCLK, then enable cache.
     */
    if (!Flash_SetLatency(FLASH_ACR_LATENCY_4WS))
    {
        Clock_UseHsi16();
        return;
    }

    RCC->CR |= RCC_CR_HSION;
    if (!wait_mask_set(&RCC->CR, RCC_CR_HSIRDY))
    {
        Clock_UseHsi16();
        return;
    }

    /* HSI16 / 4 * 85 / 2 = 170 MHz. */
    RCC->CR &= ~RCC_CR_PLLON;
    if (!wait_mask_clear(&RCC->CR, RCC_CR_PLLRDY))
    {
        Clock_UseHsi16();
        return;
    }

    RCC->PLLCFGR =
        RCC_PLLCFGR_PLLSRC_HSI |
        (3UL << RCC_PLLCFGR_PLLM_Pos) |
        (85UL << RCC_PLLCFGR_PLLN_Pos) |
        RCC_PLLCFGR_PLLREN;

    RCC->CR |= RCC_CR_PLLON;
    if (!wait_mask_set(&RCC->CR, RCC_CR_PLLRDY))
    {
        Clock_UseHsi16();
        return;
    }

    RCC->CFGR =
        RCC_CFGR_HPRE_DIV1 |
        RCC_CFGR_PPRE1_DIV1 |
        RCC_CFGR_PPRE2_DIV1 |
        RCC_CFGR_SW_PLL;

    if (!wait_mask_value(&RCC->CFGR, RCC_CFGR_SWS, RCC_CFGR_SWS_PLL))
    {
        Clock_UseHsi16();
        return;
    }

    FLASH->ACR |= FLASH_ACR_ICEN | FLASH_ACR_DCEN | FLASH_ACR_PRFTEN;
    SystemCoreClockUpdate();
#endif
}

static bool wait_mask_set(volatile uint32_t *reg, uint32_t mask)
{
    uint32_t timeout = CLOCK_STARTUP_TIMEOUT;
    while (((*reg & mask) != mask) && (timeout > 0U))
    {
        timeout--;
    }

    return timeout > 0U;
}

static bool wait_mask_clear(volatile uint32_t *reg, uint32_t mask)
{
    uint32_t timeout = CLOCK_STARTUP_TIMEOUT;
    while (((*reg & mask) != 0U) && (timeout > 0U))
    {
        timeout--;
    }

    return timeout > 0U;
}

static bool wait_mask_value(volatile uint32_t *reg, uint32_t mask, uint32_t value)
{
    uint32_t timeout = CLOCK_STARTUP_TIMEOUT;
    while (((*reg & mask) != value) && (timeout > 0U))
    {
        timeout--;
    }

    return timeout > 0U;
}

static bool Flash_SetLatency(uint32_t latency)
{
    uint32_t timeout = CLOCK_STARTUP_TIMEOUT;

    FLASH->ACR = (FLASH->ACR & ~FLASH_ACR_LATENCY) | latency;
    while (((FLASH->ACR & FLASH_ACR_LATENCY) != latency) && (timeout > 0U))
    {
        timeout--;
    }

    return timeout > 0U;
}

static void Clock_UseHsi16(void)
{
    RCC->CR |= RCC_CR_HSION;
    (void)wait_mask_set(&RCC->CR, RCC_CR_HSIRDY);

    (void)Flash_SetLatency(FLASH_ACR_LATENCY_0WS);

    RCC->CFGR =
        RCC_CFGR_HPRE_DIV1 |
        RCC_CFGR_PPRE1_DIV1 |
        RCC_CFGR_PPRE2_DIV1 |
        RCC_CFGR_SW_HSI;

    (void)wait_mask_value(&RCC->CFGR, RCC_CFGR_SWS, RCC_CFGR_SWS_HSI);
    SystemCoreClockUpdate();
}