From bc898d1f92e3c2d9ec8bd9f3d672b819e2b8ee74 Mon Sep 17 00:00:00 2001
From: Karl Palsson <karlp@tweak.net.au>
Date: Mon, 1 May 2017 23:17:31 +0000
Subject: [PATCH] stm32l0: rcc: Add clock struct setup helper

Based on l1, l4 and friends.
---
 include/libopencm3/stm32/l0/rcc.h | 20 ++++++++++++-
 lib/stm32/l0/rcc.c                | 48 +++++++++++++++++++++++++++++++
 2 files changed, 67 insertions(+), 1 deletion(-)

diff --git a/include/libopencm3/stm32/l0/rcc.h b/include/libopencm3/stm32/l0/rcc.h
index fe31b3a3..6fb3e3d7 100644
--- a/include/libopencm3/stm32/l0/rcc.h
+++ b/include/libopencm3/stm32/l0/rcc.h
@@ -40,6 +40,9 @@
 #ifndef LIBOPENCM3_RCC_H
 #define LIBOPENCM3_RCC_H
 
+#include <stdbool.h>
+#include <stdint.h>
+#include <libopencm3/stm32/pwr.h>
 
 /* --- RCC registers ------------------------------------------------------- */
 
@@ -496,6 +499,21 @@
 #define RCC_CSR_LSIRDY				(1 << 1)
 #define RCC_CSR_LSION				(1 << 0)
 
+struct rcc_clock_scale {
+	uint8_t pll_mul;
+	uint16_t pll_div;
+	uint8_t pll_source;
+	uint8_t flash_waitstates;
+	enum pwr_vos_scale voltage_scale;
+	uint8_t hpre;
+	uint8_t ppre1;
+	uint8_t ppre2;
+	// FIXME enum pwr_vos_scale voltage_scale;
+	uint32_t ahb_frequency;
+	uint32_t apb1_frequency;
+	uint32_t apb2_frequency;
+	uint8_t msi_range;
+};
 
 /* --- Variable definitions ------------------------------------------------ */
 extern uint32_t rcc_ahb_frequency;
@@ -672,7 +690,7 @@ void rcc_set_pll_divider(uint32_t factor);
 void rcc_set_ppre2(uint32_t ppre2);
 void rcc_set_ppre1(uint32_t ppre1);
 void rcc_set_hpre(uint32_t hpre);
-/* TODO */
+void rcc_clock_setup_pll(const struct rcc_clock_scale *clock);
 
 END_DECLS
 
diff --git a/lib/stm32/l0/rcc.c b/lib/stm32/l0/rcc.c
index f7d5c207..d851c39a 100644
--- a/lib/stm32/l0/rcc.c
+++ b/lib/stm32/l0/rcc.c
@@ -35,8 +35,15 @@
 /**@{*/
 
 #include <libopencm3/cm3/assert.h>
+#include <libopencm3/stm32/flash.h>
+#include <libopencm3/stm32/pwr.h>
 #include <libopencm3/stm32/rcc.h>
 
+/* Set the default clock frequencies after reset. */
+uint32_t rcc_ahb_frequency = 2097000;
+uint32_t rcc_apb1_frequency = 2097000;
+uint32_t rcc_apb2_frequency = 2097000;
+
 void rcc_osc_on(enum rcc_osc osc)
 {
 	switch (osc) {
@@ -371,5 +378,46 @@ void rcc_set_hpre(uint32_t hpre)
 	RCC_CFGR = reg | (hpre << RCC_CFGR_HPRE_SHIFT);
 }
 
+/**
+ * Set up sysclock with PLL from HSI16
+ * @param clock full struct with desired parameters
+ */
+void rcc_clock_setup_pll(const struct rcc_clock_scale *clock)
+{
+	/* Turn on the appropriate source for the PLL */
+	if (clock->pll_source == RCC_CFGR_PLLSRC_HSE_CLK) {
+		rcc_osc_on(RCC_HSE);
+		rcc_wait_for_osc_ready(RCC_HSE);
+	} else {
+		rcc_osc_on(RCC_HSI16);
+		rcc_wait_for_osc_ready(RCC_HSI16);
+	}
+
+	rcc_set_hpre(clock->hpre);
+	rcc_set_ppre1(clock->ppre1);
+	rcc_set_ppre2(clock->ppre2);
+
+	rcc_periph_clock_enable(RCC_PWR);
+	pwr_set_vos_scale(clock->voltage_scale);
+
+	rcc_osc_off(RCC_PLL);
+	while (rcc_is_osc_ready(RCC_PLL));
+
+	flash_prefetch_enable();
+	flash_set_ws(clock->flash_waitstates);
+
+	/* Set up the PLL */
+	rcc_set_pll_multiplier(clock->pll_mul);
+	rcc_set_pll_divider(clock->pll_div);
+
+	rcc_osc_on(RCC_PLL);
+	rcc_wait_for_osc_ready(RCC_PLL);
+	rcc_set_sysclk_source(RCC_PLL);
+
+	/* Set the peripheral clock frequencies used. */
+	rcc_ahb_frequency = clock->ahb_frequency;
+	rcc_apb1_frequency = clock->apb1_frequency;
+	rcc_apb2_frequency = clock->apb2_frequency;
+}
 
 /**@}*/