#include "hardware_init.h"
#include "config.h"
#include "gd32f4xx.h"
#include "gd32f4xx_fmc.h"

namespace HardwareInit {

RetCode init()
{
    systemClockConfig();
    peripheralClockEnable();
    gpioInit();
    nvicConfig();
    mpuConfig();
    return RET_OK;
}

void systemClockConfig()
{
    uint32_t timeout = 0U;

    if ((RCU_CTL & RCU_CTL_PLLEN) && (RCU_CFG0 & RCU_SCSS_PLLP)) {
        uint32_t pll_val = RCU_PLL;
        uint32_t pll_n = (pll_val >> 6) & 0x1FF;
        uint32_t pll_psc = pll_val & 0x3F;

        if (pll_n == 336 && pll_psc == 25) {
            SystemCoreClock = 168000000U;
            return;
        }
    }

    fmc_wscnt_set(WS_WSCNT_7);

    RCU_CTL |= RCU_CTL_HXTALEN;
    timeout = 0xFFFFFF;
    while ((0U == (RCU_CTL & RCU_CTL_HXTALSTB)) && (timeout > 0)) {
        timeout--;
    }

    if (0U == (RCU_CTL & RCU_CTL_HXTALSTB)) {
        RCU_CTL |= RCU_CTL_IRC16MEN;
        timeout = 0xFFFF;
        while (0U == (RCU_CTL & RCU_CTL_IRC16MSTB) && timeout > 0) {
            timeout--;
        }
        if (0U == (RCU_CTL & RCU_CTL_IRC16MSTB)) {
            while (1) { }
        }
        RCU_CFG0 |= RCU_AHB_CKSYS_DIV1;
        RCU_CFG0 |= RCU_APB2_CKAHB_DIV2;
        RCU_CFG0 |= RCU_APB1_CKAHB_DIV4;
        RCU_CFG0 &= ~RCU_CFG0_SCS;
        RCU_CFG0 |= RCU_CKSYSSRC_IRC16M;
        timeout = 0xFFFF;
        while (0U == (RCU_CFG0 & RCU_SCSS_IRC16M) && timeout > 0) {
            timeout--;
        }
        SystemCoreClock = 16000000U;
        return;
    }

    RCU_APB1EN |= RCU_APB1EN_PMUEN;
    PMU_CTL |= PMU_CTL_LDOVS;

    RCU_CFG0 |= RCU_AHB_CKSYS_DIV1;
    RCU_CFG0 |= RCU_APB2_CKAHB_DIV2;
    RCU_CFG0 |= RCU_APB1_CKAHB_DIV4;

    RCU_PLL = (25U | (336 << 6U) | (((2 >> 1U) - 1U) << 16U) |
                   (RCU_PLLSRC_HXTAL) | (7 << 24U));

    RCU_CTL |= RCU_CTL_PLLEN;
    timeout = 0xFFFF;
    while (0U == (RCU_CTL & RCU_CTL_PLLSTB) && timeout > 0) {
        timeout--;
    }

    if (0U == (RCU_CTL & RCU_CTL_PLLSTB)) {
        RCU_CFG0 &= ~RCU_CFG0_SCS;
        RCU_CFG0 |= RCU_CKSYSSRC_HXTAL;
        timeout = 0xFFFF;
        while (0U == (RCU_CFG0 & RCU_SCSS_HXTAL) && timeout > 0) {
            timeout--;
        }
        SystemCoreClock = 25000000U;
        return;
    }

    PMU_CTL |= PMU_CTL_HDEN;
    timeout = 0xFFFF;
    while (0U == (PMU_CS & PMU_CS_HDRF) && timeout > 0) {
        timeout--;
    }
    PMU_CTL |= PMU_CTL_HDS;
    timeout = 0xFFFF;
    while (0U == (PMU_CS & PMU_CS_HDSRF) && timeout > 0) {
        timeout--;
    }

    RCU_CFG0 &= ~RCU_CFG0_SCS;
    RCU_CFG0 |= RCU_CKSYSSRC_PLLP;
    timeout = 0xFFFF;
    while (0U == (RCU_CFG0 & RCU_SCSS_PLLP) && timeout > 0) {
        timeout--;
    }

    SystemCoreClock = 168000000U;
}

void peripheralClockEnable()
{
    rcu_periph_clock_enable(RCU_GPIOA);
    rcu_periph_clock_enable(RCU_GPIOB);
    rcu_periph_clock_enable(RCU_GPIOC);
    rcu_periph_clock_enable(RCU_GPIOD);
    rcu_periph_clock_enable(RCU_GPIOE);
    rcu_periph_clock_enable(RCU_GPIOF);
    rcu_periph_clock_enable(RCU_GPIOG);

    rcu_periph_clock_enable(RCU_USART1);

    rcu_periph_clock_enable(RCU_I2C1);

    rcu_periph_clock_enable(RCU_SPI1);

    rcu_periph_clock_enable(RCU_ADC0);

    rcu_periph_clock_enable(RCU_TIMER2);

    rcu_periph_clock_enable(RCU_EXMC);
}

void gpioInit()
{
}

void nvicConfig()
{
}

void mpuConfig()
{
    MPU->CTRL = 0U;

    __DSB();
    __ISB();

    MPU->RNR = 0U;
    MPU->RBAR = (0xC0000000U & MPU_RBAR_ADDR_Msk)
              | (1UL << MPU_RBAR_VALID_Pos)
              | (0UL << MPU_RBAR_REGION_Pos);

    MPU->RASR = (1UL << MPU_RASR_XN_Pos)
              | (3UL << MPU_RASR_AP_Pos)
              | (1UL << MPU_RASR_TEX_Pos)
              | (0UL << MPU_RASR_S_Pos)
              | (0UL << MPU_RASR_C_Pos)
              | (0UL << MPU_RASR_B_Pos)
              | (0UL << MPU_RASR_SRD_Pos)
              | (27UL << MPU_RASR_SIZE_Pos)
              | (1UL << MPU_RASR_ENABLE_Pos);

    __DSB();
    __ISB();

    MPU->RNR = 1U;
    MPU->RBAR = (0x6C000000U & MPU_RBAR_ADDR_Msk)
              | (1UL << MPU_RBAR_VALID_Pos)
              | (1UL << MPU_RBAR_REGION_Pos);

    MPU->RASR = (3UL << MPU_RASR_AP_Pos)
              | (0UL << MPU_RASR_TEX_Pos)
              | (0UL << MPU_RASR_S_Pos)
              | (0UL << MPU_RASR_C_Pos)
              | (1UL << MPU_RASR_B_Pos)
              | (0UL << MPU_RASR_SRD_Pos)
              | (25UL << MPU_RASR_SIZE_Pos)
              | (1UL << MPU_RASR_ENABLE_Pos);

    __DSB();
    __ISB();

    MPU->CTRL = MPU_CTRL_ENABLE_Msk | MPU_CTRL_PRIVDEFENA_Msk;

    __DSB();
    __ISB();
}

}