#include <math.h>

#include "stm32h7xx.h"

uint32_t      SystemCoreClock;
uint32_t      SystemD2Clock;
const uint8_t D1CorePrescTable[16] = {0, 0, 0, 0, 1, 2, 3, 4, 1, 2, 3, 4, 6, 7, 8, 9};

void SystemInit(void) {
/* FPU settings ------------------------------------------------------------*/
#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
    SCB->CPACR |= ((3UL << (10 * 2)) | (3UL << (11 * 2))); /* set CP10 and CP11 Full Access */
#endif
}

void SystemCoreClockUpdate(void) {
    uint32_t pllp, pllsource, pllm, pllfracen, hsivalue, tmp;
    uint32_t common_system_clock;
    float_t  fracn1, pllvco;

    /* Get SYSCLK source -------------------------------------------------------*/

    switch (RCC->CFGR & RCC_CFGR_SWS) {
        case RCC_CFGR_SWS_HSI: /* HSI used as system clock source */
            common_system_clock = (uint32_t)(HSI_VALUE >> ((RCC->CR & RCC_CR_HSIDIV) >> 3));
            break;

        case RCC_CFGR_SWS_CSI: /* CSI used as system clock  source */
            common_system_clock = CSI_VALUE;
            break;

        case RCC_CFGR_SWS_HSE: /* HSE used as system clock  source */
            common_system_clock = HSE_VALUE;
            break;

        case RCC_CFGR_SWS_PLL1: /* PLL1 used as system clock  source */

            /* PLL_VCO = (HSE_VALUE or HSI_VALUE or CSI_VALUE/ PLLM) * PLLN
            SYSCLK = PLL_VCO / PLLR
            */
            pllsource = (RCC->PLLCKSELR & RCC_PLLCKSELR_PLLSRC);
            pllm      = ((RCC->PLLCKSELR & RCC_PLLCKSELR_DIVM1) >> 4);
            pllfracen = ((RCC->PLLCFGR & RCC_PLLCFGR_PLL1FRACEN) >> RCC_PLLCFGR_PLL1FRACEN_Pos);
            fracn1    = (float_t)(uint32_t)(pllfracen * ((RCC->PLL1FRACR & RCC_PLL1FRACR_FRACN1) >> 3));

            if (pllm != 0U) {
                switch (pllsource) {
                    case RCC_PLLCKSELR_PLLSRC_HSI: /* HSI used as PLL clock source */

                        hsivalue = (HSI_VALUE >> ((RCC->CR & RCC_CR_HSIDIV) >> 3));
                        pllvco   = ((float_t)hsivalue / (float_t)pllm) *
                                 ((float_t)(uint32_t)(RCC->PLL1DIVR & RCC_PLL1DIVR_N1) + (fracn1 / (float_t)0x2000) +
                                  (float_t)1);

                        break;

                    case RCC_PLLCKSELR_PLLSRC_CSI: /* CSI used as PLL clock source */
                        pllvco = ((float_t)CSI_VALUE / (float_t)pllm) *
                                 ((float_t)(uint32_t)(RCC->PLL1DIVR & RCC_PLL1DIVR_N1) + (fracn1 / (float_t)0x2000) +
                                  (float_t)1);
                        break;

                    case RCC_PLLCKSELR_PLLSRC_HSE: /* HSE used as PLL clock source */
                        pllvco = ((float_t)HSE_VALUE / (float_t)pllm) *
                                 ((float_t)(uint32_t)(RCC->PLL1DIVR & RCC_PLL1DIVR_N1) + (fracn1 / (float_t)0x2000) +
                                  (float_t)1);
                        break;

                    default:
                        hsivalue = (HSI_VALUE >> ((RCC->CR & RCC_CR_HSIDIV) >> 3));
                        pllvco   = ((float_t)hsivalue / (float_t)pllm) *
                                 ((float_t)(uint32_t)(RCC->PLL1DIVR & RCC_PLL1DIVR_N1) + (fracn1 / (float_t)0x2000) +
                                  (float_t)1);
                        break;
                }
                pllp                = (((RCC->PLL1DIVR & RCC_PLL1DIVR_P1) >> 9) + 1U);
                common_system_clock = (uint32_t)(float_t)(pllvco / (float_t)pllp);
            } else {
                common_system_clock = 0U;
            }
            break;

        default:
            common_system_clock = (uint32_t)(HSI_VALUE >> ((RCC->CR & RCC_CR_HSIDIV) >> 3));
            break;
    }

        /* Compute SystemClock frequency --------------------------------------------------*/
#if defined(RCC_D1CFGR_D1CPRE)
    tmp = D1CorePrescTable[(RCC->D1CFGR & RCC_D1CFGR_D1CPRE) >> RCC_D1CFGR_D1CPRE_Pos];

    /* common_system_clock frequency : CM7 CPU frequency  */
    common_system_clock >>= tmp;

    /* SystemD2Clock frequency : CM4 CPU, AXI and AHBs Clock frequency  */
    SystemD2Clock =
        (common_system_clock >> ((D1CorePrescTable[(RCC->D1CFGR & RCC_D1CFGR_HPRE) >> RCC_D1CFGR_HPRE_Pos]) & 0x1FU));

#else
    tmp = D1CorePrescTable[(RCC->CDCFGR1 & RCC_CDCFGR1_CDCPRE) >> RCC_CDCFGR1_CDCPRE_Pos];

    /* common_system_clock frequency : CM7 CPU frequency  */
    common_system_clock >>= tmp;

    /* SystemD2Clock frequency : AXI and AHBs Clock frequency  */
    SystemD2Clock   = (common_system_clock >>
                     ((D1CorePrescTable[(RCC->CDCFGR1 & RCC_CDCFGR1_HPRE) >> RCC_CDCFGR1_HPRE_Pos]) & 0x1FU));

#endif

#if defined(DUAL_CORE) && defined(CORE_CM4)
    SystemCoreClock = SystemD2Clock;
#else
    SystemCoreClock = common_system_clock;
#endif /* DUAL_CORE && CORE_CM4 */
}