1 files changed, 48 insertions, 8 deletions

diff --git a/chip/stm32/clock-stm32f0.c b/chip/stm32/clock-stm32f0.c
index df09e5b67b..3f503caad0 100644
--- a/chip/stm32/clock-stm32f0.c
+++ b/chip/stm32/clock-stm32f0.c
@@ -17,22 +17,18 @@
 /* use 48Mhz USB-synchronized High-speed oscillator */
 #define HSI48_CLOCK 48000000
 
+/* use PLL at 38.4MHz as system clock. */
+#define PLL_CLOCK 38400000
+
 int clock_get_freq(void)
 {
-	return HSI48_CLOCK;
+	return CPU_CLOCK;
 }
 
 void clock_enable_module(enum module_id module, int enable)
 {
 }
 
-/*
- * system closk is HSI48 = 48MHz,
- * no prescaler, no MCO, no PLL
- * USB clock = HSI48
- */
-BUILD_ASSERT(CPU_CLOCK == HSI48_CLOCK);
-
 void clock_init(void)
 {
 	/*
@@ -52,10 +48,54 @@ void clock_init(void)
 		while (!(STM32_RCC_CR2 & (1 << 17)))
 			;
 	}
+
+#if (CPU_CLOCK == HSI48_CLOCK)
+	/*
+	 * HSI48 = 48MHz, no prescaler, no MCO, no PLL
+	 * therefore PCLK = FCLK = SYSCLK = 48MHz
+	 * USB uses HSI48 = 48MHz
+	 */
+
 	/* switch SYSCLK to HSI48 */
 	STM32_RCC_CFGR = 0x00000003;
 
 	/* wait until the HSI48 is the clock source */
 	while ((STM32_RCC_CFGR & 0xc) != 0xc)
 		;
+
+#elif (CPU_CLOCK == PLL_CLOCK)
+	/*
+	 * HSI48 = 48MHz, no prescalar, no MCO, with PLL *4/5 => 38.4MHz SYSCLK
+	 * therefore PCLK = FCLK = SYSCLK = 38.4MHz
+	 * USB uses HSI48 = 48MHz
+	 */
+
+	/* If PLL is the clock source, PLL has already been set up. */
+	if ((STM32_RCC_CFGR & 0xc) == 0x8)
+		return;
+
+	/*
+	 * Specify HSI48 clock as input clock to PLL and set PLL multiplier
+	 * and divider.
+	 */
+	STM32_RCC_CFGR = 0x00098000;
+	STM32_RCC_CFGR2 = 0x4;
+
+	/* Enable the PLL. */
+	STM32_RCC_CR |= 0x01000000;
+
+	/* Wait until PLL is ready. */
+	while (!(STM32_RCC_CR & 0x02000000))
+		;
+
+	/* Switch SYSCLK to PLL. */
+	STM32_RCC_CFGR |= 0x2;
+
+	/* wait until the PLL is the clock source */
+	while ((STM32_RCC_CFGR & 0xc) != 0x8)
+		;
+
+#else
+#error "CPU_CLOCK must be either 48MHz or 38.4MHz"
+#endif
 }