From fe57308e71412c73f24d4c92d775f494c6e5d13d Mon Sep 17 00:00:00 2001
From: Sebastien Bourdeauducq <sb@m-labs.hk>
Date: Mon, 27 Jul 2015 20:11:31 +0800
Subject: [PATCH] runtime: support for RTIO PLL

---
 soc/runtime/main.c    |  4 ++++
 soc/runtime/session.c | 18 +++++++++++++++++-
 2 files changed, 21 insertions(+), 1 deletion(-)

diff --git a/soc/runtime/main.c b/soc/runtime/main.c
index 306f26b20..3a639b145 100644
--- a/soc/runtime/main.c
+++ b/soc/runtime/main.c
@@ -253,6 +253,10 @@ int main(void)
     puts("Press 't' to enter test mode...");
     blink_led();
 
+#ifdef CSR_RTIO_CRG_PLL_RESET_ADDR
+    rtio_crg_pll_reset_write(0);
+#endif
+
     if(check_test_mode()) {
         puts("Entering test mode.");
         test_main();
diff --git a/soc/runtime/session.c b/soc/runtime/session.c
index 9b3591e30..d0f896583 100644
--- a/soc/runtime/session.c
+++ b/soc/runtime/session.c
@@ -128,6 +128,22 @@ static int check_flash_storage_key_len(char *key, unsigned int key_len)
     return 1;
 }
 
+static void switch_clock(int clk)
+{
+    int current_clk;
+
+    current_clk = rtio_crg_clock_sel_read();
+    if(clk == current_clk)
+        return;
+#ifdef CSR_RTIO_CRG_PLL_RESET_ADDR
+    rtio_crg_pll_reset_write(1);
+#endif
+    rtio_crg_clock_sel_write(clk);
+#ifdef CSR_RTIO_CRG_PLL_RESET_ADDR
+    rtio_crg_pll_reset_write(0);
+#endif
+}
+
 static int process_input(void)
 {
     switch(buffer_in[8]) {
@@ -154,7 +170,7 @@ static int process_input(void)
                 submit_output(9);
                 break;
             }
-            rtio_crg_clock_sel_write(buffer_in[9]);
+            switch_clock(buffer_in[9]);
             buffer_out[8] = REMOTEMSG_TYPE_CLOCK_SWITCH_COMPLETED;
             submit_output(9);
             break;