1 files changed, 83 insertions, 0 deletions
diff --git a/src/clocks.c b/src/clocks.c
new file mode 100644
index 0000000..a8cb8e4
--- /dev/null
+++ b/src/clocks.c
@@ -0,0 +1,83 @@
+#include <fe_global.h>
+#include <fe_clocks.h>
+
+unsigned int sysclk_speed;
+unsigned int ahb_speed;
+unsigned int apb1_speed;
+unsigned int apb2_speed;
+unsigned int apb1_timer_speed;
+unsigned int apb2_timer_speed;
+
+
+void delay_ms(int ms) {
+    uint32_t init_val = SysTick->VAL;
+    uint32_t wait_end = sys_time_millis + ms;
+
+    while (sys_time_millis < wait_end)
+        ;
+
+    while (SysTick->VAL >= init_val) {
+        if (sys_time_millis > wait_end)
+            return;
+    }
+}
+
+void fe_config_clocks()
+{
+    /* 8MHz HSI clock as PLL source. */
+#define HSI_SPEED 8000000
+    /* PLL output = HSI / 2 * PLL_MUL */
+#define PLL_MUL 16
+
+    /* Check that we came out of reset correctly */
+    if (((RCC->CFGR & RCC_CFGR_SWS_Msk) >> RCC_CFGR_SW_Pos) != 0)
+        asm volatile ("bkpt");
+    if (RCC->CR & RCC_CR_HSEON)
+        asm volatile ("bkpt");
+    if (RCC->CR & RCC_CR_PLLON)
+        asm volatile ("bkpt");
+
+    RCC->CFGR = 0;
+    RCC->CFGR |= (0<<RCC_CFGR_PLLSRC_Pos); /* PLL input: HSI /2 */
+    RCC->CFGR |= ((PLL_MUL-2)<<RCC_CFGR_PLLMUL_Pos);
+    sysclk_speed = HSI_SPEED / 2 * PLL_MUL;
+
+    /* set AHB prescaler to /1 */
+    RCC->CFGR |= (0 << RCC_CFGR_HPRE_Pos);
+    ahb_speed = sysclk_speed;
+
+    /* set ABP1 prescaler to 2 -> 36MHz */
+    RCC->CFGR |= (4 << RCC_CFGR_PPRE1_Pos);
+    apb1_speed = sysclk_speed / 2;
+    apb1_timer_speed = apb1_speed * 2;
+
+    /* set ABP2 prescaler to 2 -> 36MHz */
+    RCC->CFGR |= (4 << RCC_CFGR_PPRE2_Pos);
+    apb2_speed = sysclk_speed / 2;
+    apb2_timer_speed = apb2_speed * 2;
+
+    /* Configure PLL */
+    RCC->CR |= RCC_CR_PLLON;
+
+    /* Wait for main PLL */
+    while(!(RCC->CR & RCC_CR_PLLRDY))
+        ;
+
+    /* Configure Flash: enable prefetch; set latency = 2 wait states
+     * See reference manual (RM0365), Section 4.5.1
+     */
+    FLASH->ACR = FLASH_ACR_PRFTBE | (2<<FLASH_ACR_LATENCY_Pos);
+
+    /* Select PLL as system clock source */
+    RCC->CFGR &= ~RCC_CFGR_SW_Msk;
+    RCC->CFGR |= 2 << RCC_CFGR_SW_Pos;
+
+    /* Wait for clock to switch over */
+    while ((RCC->CFGR & RCC_CFGR_SWS_Msk)>>RCC_CFGR_SWS_Pos != 2)
+        ;
+
+    SystemCoreClockUpdate();
+    SysTick_Config(SystemCoreClock / 1000); /* 1 ms ticks */
+
+    NVIC_SetPriority(SysTick_IRQn, 32);
+}