#define STM32_CLOCKSOURCE_DEFAULT_TIMEOUT 100000

#define stm32_rccEnableHSI() do { STM32_RCC->CR |= RCC_CR_HSION; } while (0)
#define stm32_rccDisableHSI() do { STM32_RCC->CR &= ~RCC_CR_HSION; } while (0)
#define stm32_rccEnableHSE() do { STM32_RCC->CR |= RCC_CR_HSEON; } while (0)
#define stm32_rccDisableHSE() do { STM32_RCC->CR &= ~RCC_CR_HSEON; } while (0)
#define stm32_rccEnablePLL() do { STM32_RCC->CR |= RCC_CR_PLLON; } while (0)
#define stm32_rccDisablePLL() do { STM32_RCC->CR &= ~RCC_CR_PLLON; } while (0)
#define stm32_rccEnableLSI() do { STM32_RCC->CSR |= RCC_CSR_LSION; } while (0)
#define stm32_rccDisableLSI() do { STM32_RCC->CSR &= ~RCC_CSR_LSION; } while (0)
#define stm32_rccEnableLSE() do { STM32_RCC->BDCR |= RCC_BDCR_LSEON; } while (0)
#define stm32_rccDisableLSE() do { STM32_RCC->BDCR |= ~RCC_BDCR_LSEON; } while (0)

#define stm32_rccHSIReady() (STM32_RCC->CR & RCC_CR_HSIRDY)
#define stm32_rccHSEReady() (STM32_RCC->CR & RCC_CR_HSERDY)
#define stm32_rccPLLReady() (STM32_RCC->CR & RCC_CR_PLLRDY)
#define stm32_rccLSIReady() (STM32_RCC->CSR & RCC_CSR_LSIRDY)
#define stm32_rccLSEReady() (STM32_RCC->BDCR & RCC_BDCR_LSERDY)

#define stm32_rccSelectHSI() do { STM32_RCC->CFGR = (STM32_RCC->CFGR & ~(RCC_CFGR_SW0 | RCC_CFGR_SW1)) | 0; } while (0)
#define stm32_rccSelectHSE() do { STM32_RCC->CFGR = (STM32_RCC->CFGR & ~(RCC_CFGR_SW0 | RCC_CFGR_SW1)) | 1; } while (0)
#define stm32_rccSelectPLL() do { STM32_RCC->CFGR = (STM32_RCC->CFGR & ~(RCC_CFGR_SW0 | RCC_CFGR_SW1)) | 2; } while (0)

#define stm32_rccHSISelected() ((STM32_RCC->CFGR & (RCC_CFGR_SW0 | RCC_CFGR_SW1)) == 0)
#define stm32_rccHSESelected() ((STM32_RCC->CFGR & (RCC_CFGR_SW0 | RCC_CFGR_SW1)) == 1)
#define stm32_rccPLLSelected() ((STM32_RCC->CFGR & (RCC_CFGR_SW0 | RCC_CFGR_SW1)) == 2)

void stm32_rccSetAHBPrescaler(STM32AHBPrescaler psc) {
    STM32_RCC->CFGR = (STM32_RCC->CFGR & ~RCC_CFGR_HPRE_MASK) | (psc << RCC_CFGR_HPRE_OFFSET);
}

void stm32_rccSetAPB1Prescaler(STM32APBPrescaler psc) {
    STM32_RCC->CFGR = (STM32_RCC->CFGR & ~RCC_CFGR_PPRE1_MASK) | (psc << RCC_CFGR_PPRE1_OFFSET);
}

void stm32_rccSetAPB2Prescaler(STM32APBPrescaler psc) {
    STM32_RCC->CFGR = (STM32_RCC->CFGR & ~RCC_CFGR_PPRE2_MASK) | (psc << RCC_CFGR_PPRE2_OFFSET);
}

void stm32_rccSetADCPrescaler(STM32ADCPrescaler psc) {
    STM32_RCC->CFGR = (STM32_RCC->CFGR & ~RCC_CFGR_ADCPRE_MASK) | (psc << RCC_CFGR_ADCPRE_OFFSET);
}

bool stm32_rccWaitHSIReady(uint32_t timeout) {
    while ((stm32_rccHSIReady() == 0) && (timeout--));
    return timeout > 0;
}

bool stm32_rccWaitHSEReady(uint32_t timeout) {
    while ((stm32_rccHSEReady() == 0) && (timeout--));
    return timeout > 0;
}

bool stm32_rccWaitPLLReady(uint32_t timeout) {
    while ((stm32_rccPLLReady() == 0) && (timeout--));
    return timeout > 0;
}

bool stm32_rccWaitLSIReady(uint32_t timeout) {
    while ((stm32_rccLSIReady() == 0) && (timeout--));
    return timeout > 0;
}

bool stm32_rccWaitLSEReady(uint32_t timeout) {
    while ((stm32_rccLSEReady() == 0) && (timeout--));
    return timeout > 0;
}

bool stm32_rccWaitHSISelected(uint32_t timeout) {
    while ((stm32_rccHSISelected() == 0) && (timeout--));
    return timeout > 0;
}

bool stm32_rccWaitHSESelected(uint32_t timeout) {
    while ((stm32_rccHSESelected() == 0) && (timeout--));
    return timeout > 0;
}

bool stm32_rccWaitPLLSelected(uint32_t timeout) {
    while ((stm32_rccPLLSelected() == 0) && (timeout--));
    return timeout > 0;
}

void stm32_rccSetBusPrescalers(uint32_t sysClk, STM32AHBPrescaler ahbPsc, STM32APBPrescaler apb1Psc, STM32APBPrescaler apb2Psc) {
    stm32_rccSetAHBPrescaler(ahbPsc);
    stm32_rccSetAPB1Prescaler(apb1Psc);
    stm32_rccSetAPB2Prescaler(apb2Psc);
    if (ahbPsc == 0) {
        _stm32_rccAHBFrequency = sysClk;
    } else if (ahbPsc <= 11) {
        _stm32_rccAHBFrequency = sysClk / (1 << (ahbPsc - 7));
    } else {
        _stm32_rccAHBFrequency = sysClk / (1 << (ahbPsc - 6));
    }
    if (apb1Psc == 0) {
        _stm32_rccAPB1Frequency = _stm32_rccAHBFrequency;
    } else {
        _stm32_rccAPB1Frequency = _stm32_rccAHBFrequency / (1 << (apb1Psc - 3));
    }
    if (apb2Psc == 0) {
        _stm32_rccAPB2Frequency = _stm32_rccAHBFrequency;
    } else {
        _stm32_rccAPB2Frequency = _stm32_rccAHBFrequency / (1 << (apb2Psc - 3));
    }
}

void stm32_rccAutoSetPrescalers(uint32_t sysClk) {
    if (sysClk <= 36000000) {
        stm32_rccSetBusPrescalers(sysClk, RCC_AHB_NODIV, RCC_APB_NODIV, RCC_APB_NODIV);
    } else {
        stm32_rccSetBusPrescalers(sysClk, RCC_AHB_NODIV, RCC_APB_DIV2, RCC_APB_NODIV);
    }
    stm32_rccSetADCPrescaler((sysClk - 1) / 28000000);
    FLASH_ACR = (FLASH_ACR & ~FLASH_ACR_LATENCY_MASK) | FLASH_ACR_PRFTBE | ((sysClk - 1) / 24000000);
}

void stm32_rccUseHSI(void) {
    stm32_rccEnableHSI();
    stm32_rccWaitHSIReady(STM32_CLOCKSOURCE_DEFAULT_TIMEOUT);
    stm32_rccSelectHSI();
    stm32_rccWaitHSISelected(STM32_CLOCKSOURCE_DEFAULT_TIMEOUT);
    stm32_rccAutoSetPrescalers(8000000);
}

bool stm32_rccUseHSE(uint32_t clk) {
    stm32_rccEnableHSE();
    if (stm32_rccWaitHSEReady(STM32_CLOCKSOURCE_DEFAULT_TIMEOUT)) {
        stm32_rccSelectHSE();
        if (stm32_rccWaitHSESelected(STM32_CLOCKSOURCE_DEFAULT_TIMEOUT)) {
            stm32_rccAutoSetPrescalers(clk);
            return true;
        }
        stm32_rccUseHSI();
    }
    stm32_rccDisableHSE();
    return false;
}

bool stm32_rccSetupPLL(uint32_t inClk, uint32_t outClk, bool extClk) {
    assert(outClk >= inClk);
    uint32_t cfg = extClk ? RCC_CFGR_PLLSRC : 0;
    uint32_t mul = outClk / inClk;
    if (!extClk) {
        mul *= 2;
    }
    if (mul && (mul <= 15)) {
        if (mul < 8) {
            cfg |= RCC_CFGR_PLLXTPRE;
            mul *= 2;
        }
        cfg |= (mul - 2) << RCC_CFGR_PLLMUL_OFFSET;
        if (outClk <= 48000000) {
            cfg |= RCC_CFGR_OTGFSPRE;
        }
        STM32_RCC->CFGR = (STM32_RCC->CFGR & ~(RCC_CFGR_PLLMUL_MASK | RCC_CFGR_PLLXTPRE | RCC_CFGR_OTGFSPRE)) | cfg;
        return true;
    }
    return false;
}

bool stm32_rccUsePLL(uint32_t clk) {
    stm32_rccEnablePLL();
    if (stm32_rccWaitPLLReady(STM32_CLOCKSOURCE_DEFAULT_TIMEOUT)) {
        stm32_rccAutoSetPrescalers(clk);
        stm32_rccSelectPLL();
        if (stm32_rccWaitPLLSelected(STM32_CLOCKSOURCE_DEFAULT_TIMEOUT)) {
            return true;
        }
        stm32_rccUseHSI();
    }
    stm32_rccDisablePLL();
    return false;
}

bool stm32_rccUsePLLFromHSI(uint32_t clk) {
    stm32_rccUseHSI();
    if (stm32_rccSetupPLL(8000000, clk, false)) {
        if (stm32_rccUsePLL(clk)) {
            return true;
        }
    }
    return false;
}

bool stm32_rccUsePLLFromHSE(uint32_t clk, uint32_t hseClk) {
    stm32_rccUseHSI();
    stm32_rccEnableHSE();
    if (stm32_rccWaitHSEReady(STM32_CLOCKSOURCE_DEFAULT_TIMEOUT)) {
        if (stm32_rccSetupPLL(hseClk, clk, true)) {
            if (stm32_rccUsePLL(clk)) {
                stm32_rccDisableHSI();
                return true;
            }
        }
    }
    stm32_rccDisableHSE();
    return false;
}