From fb52ad00e465259b445ea69c448182417a0489b0 Mon Sep 17 00:00:00 2001
From: Vincent Palatin <vpalatin@chromium.org>
Date: Fri, 27 Jan 2012 23:08:56 +0000
Subject: [PATCH] stm32l: initialize clocks

Run from internal clock at 16Mhz, but enable PLL to get a better
precision.

Signed-off-by: Vincent Palatin <vpalatin@chromium.org>

BUG=None
TEST=run on discovery board and check software is still alive after
clock initialization.

Change-Id: I8425482825015adf96c30e67a9320d0df2f4f2b7
---
 board/discovery/board.h |  3 +++
 chip/stm32l/clock.c     | 40 ++++++++++++++++++++++++++++++++++++++++
 2 files changed, 43 insertions(+)

diff --git a/board/discovery/board.h b/board/discovery/board.h
index 3e4ea80d12..48a1e280da 100644
--- a/board/discovery/board.h
+++ b/board/discovery/board.h
@@ -8,6 +8,9 @@
 #ifndef __BOARD_H
 #define __BOARD_H
 
+/* 16 MHz SYSCLK clock frequency */
+#define CPU_CLOCK 16000000
+
 #define USB_CHARGE_PORT_COUNT 0
 
 /* GPIO signal list */
diff --git a/chip/stm32l/clock.c b/chip/stm32l/clock.c
index 0f4f5f9cdb..1db7c02894 100644
--- a/chip/stm32l/clock.c
+++ b/chip/stm32l/clock.c
@@ -7,8 +7,11 @@
 
 #include <stdint.h>
 
+#include "board.h"
 #include "clock.h"
 #include "common.h"
+#include "registers.h"
+#include "util.h"
 
 /**
  * Idle task
@@ -25,5 +28,42 @@ void __idle(void)
 
 int clock_init(void)
 {
+	/*
+	 * The initial state :
+	 *  SYSCLK from HSI (=16MHz), no divider on AHB, APB1, APB2
+	 *  PLL unlocked, RTC enabled on LSE
+	 */
+
+	/* Ensure that HSI is ON */
+	if (!(STM32L_RCC_CR & (1 << 1))) {
+		/* Enable HSI */
+		STM32L_RCC_CR |= 1 << 0;
+		/* Wait for HSI to be ready */
+		while (!(STM32L_RCC_CR & (1 << 1)))
+			;
+	}
+
+	/*
+	 * stays on HSI, no prescaler, PLLSRC = HSI, PLLMUL = x3, PLLDIV = /3,
+	 * no MCO                      => PLLVCO = 48 MHz and PLLCLK = 16 Mhz
+	 */
+	BUILD_ASSERT(CPU_CLOCK == 16000000);
+	STM32L_RCC_CFGR = 0x00800001;
+	/* Enable the PLL */
+	STM32L_RCC_CR |= 1 << 24;
+	/* Wait for the PLL to lock */
+	while (!(STM32L_RCC_CR & (1 << 25)))
+		;
+	/* switch to SYSCLK to the PLL */
+	STM32L_RCC_CFGR = 0x00800003;
+
+	/* switch on LSI */
+	STM32L_RCC_CSR |= 1 << 0;
+	/* Wait for LSI to be ready */
+	while (!(STM32L_RCC_CSR & (1 << 1)))
+		;
+	/* Enable RTC and use LSI as clock source */
+	STM32L_RCC_CSR = (STM32L_RCC_CSR & ~0x00430000) | 0x00420000;
+
 	return EC_SUCCESS;
 }