#include "stm32l0xx.h"

// The current clock frequency
uint32_t SystemCoreClock=2097000;

// Change system clock to 32 MHz using 8 MHz crystal
void init_clock() {

    // Wait until voltage regulator is stabilized
    while(READ_BIT(PWR->CSR, PWR_CSR_VOSF)) {}

    // Switch to voltage scaling range 1
    MODIFY_REG(PWR->CR, PWR_CR_VOS, 1 << PWR_CR_VOS_Pos);

    // Wait until voltage regulator is stabilized
    while(READ_BIT(PWR->CSR, PWR_CSR_VOSF)) {}

    // Enable HSE oscillator
    SET_BIT(RCC->CR, RCC_CR_HSEON);

    // Wait until HSE oscillator is ready
    while(!READ_BIT(RCC->CR, RCC_CR_HSERDY)) {}

    // Switch to HSE oscillator
    MODIFY_REG(RCC->CFGR, RCC_CFGR_SW, RCC_CFGR_SW_HSE);

    // Wait until the switch is done
    while ((RCC->CFGR & RCC_CFGR_SWS_Msk) != RCC_CFGR_SWS_HSE) {}

    // Disable the PLL
    CLEAR_BIT(RCC->CR, RCC_CR_PLLON);

    // Wait until the PLL is fully stopped
    while(READ_BIT(RCC->CR, RCC_CR_PLLRDY)) {}

    // Flash latency 1 wait state
    SET_BIT(FLASH->ACR, FLASH_ACR_LATENCY);

    // 32 MHz using the 8 MHz HSE oscillator multiply by 8 divide by 2
    RCC->CFGR = RCC_CFGR_PLLSRC_HSE + RCC_CFGR_PLLMUL8 + RCC_CFGR_PLLDIV2;

    // Enable PLL
    SET_BIT(RCC->CR, RCC_CR_PLLON);

    // Wait until PLL is ready
    while(!READ_BIT(RCC->CR, RCC_CR_PLLRDY)) {}

    // Select PLL as clock source
    MODIFY_REG(RCC->CFGR, RCC_CFGR_SW, RCC_CFGR_SW_PLL);

    // Update variable
    SystemCoreClock=32000000;

    // Switch the MSI oscillator off
    CLEAR_BIT(RCC->CR, RCC_CR_MSION);
}

// Counts milliseconds
volatile uint32_t systick_count=0;

// Interrupt handler
void SysTick_Handler() {
    systick_count++;
}

// Delay some milliseconds.
void delay(int ms) {
    uint32_t start=systick_count;
    while (systick_count-start < ms);
}

int main() {
	init_clock();

	// Initialize the timer for 1 ms intervals
	SysTick_Config(SystemCoreClock/1000);

    // Enable Port A
    SET_BIT(RCC->IOPENR, RCC_IOPENR_GPIOAEN);

    // PA5 = Output
    MODIFY_REG(GPIOA->MODER, GPIO_MODER_MODE5, 0b01 << GPIO_MODER_MODE5_Pos);

    while(1) {
        // Set LED pin to HIGH
        GPIOA->BSRR = GPIO_BSRR_BS_5;
        delay(500);

        // Reset LED pin to LOW
        GPIOA->BSRR = GPIO_BSRR_BR_5;
        delay(500);
    }
}