From 8aa959869060cb5ffbc06c4c25cfb0e6d81bc8b4 Mon Sep 17 00:00:00 2001
From: Max Filippov <jcmvbkbc@gmail.com>
Date: Mon, 25 Jul 2016 10:58:10 +0300
Subject: [PATCH] xtensa: xtfpga: add clock provider

Signed-off-by: Max Filippov <jcmvbkbc@gmail.com>
---
 arch/xtensa/boot/dts/xtfpga.dtsi     | 11 ++++++-----
 arch/xtensa/platforms/xtfpga/setup.c | 32 ++++++++++++++++++++++++++++----
 2 files changed, 34 insertions(+), 9 deletions(-)

diff --git a/arch/xtensa/boot/dts/xtfpga.dtsi b/arch/xtensa/boot/dts/xtfpga.dtsi
index cd45f9c..b49b5d4 100644
--- a/arch/xtensa/boot/dts/xtfpga.dtsi
+++ b/arch/xtensa/boot/dts/xtfpga.dtsi
@@ -36,11 +36,6 @@
 	};
 
 	clocks {
-		osc: main-oscillator {
-			#clock-cells = <0>;
-			compatible = "fixed-clock";
-		};
-
 		clk54: clk54 {
 			#clock-cells = <0>;
 			compatible = "fixed-clock";
@@ -54,6 +49,12 @@
 		compatible = "simple-bus";
 		ranges = <0x00000000 0xf0000000 0x10000000>;
 
+		osc: main-oscillator {
+			reg = <0x0d020004 0x4>;
+			#clock-cells = <0>;
+			compatible = "cdns,xtfpga-clock";
+		};
+
 		serial0: serial@0d050020 {
 			device_type = "serial";
 			compatible = "ns16550a";
diff --git a/arch/xtensa/platforms/xtfpga/setup.c b/arch/xtensa/platforms/xtfpga/setup.c
index b509d1f..24f22e8 100644
--- a/arch/xtensa/platforms/xtfpga/setup.c
+++ b/arch/xtensa/platforms/xtfpga/setup.c
@@ -26,6 +26,8 @@
 #include <linux/console.h>
 #include <linux/delay.h>
 #include <linux/of.h>
+#include <linux/clk-provider.h>
+#include <linux/of_address.h>
 
 #include <asm/timex.h>
 #include <asm/processor.h>
@@ -135,14 +137,36 @@ static void __init update_local_mac(struct device_node *node)
 	of_update_property(node, newmac);
 }
 
+static void __init xtfpga_clk_setup(struct device_node *np)
+{
+	void __iomem *base = of_iomap(np, 0);
+	struct clk *clk;
+	u32 freq;
+
+	if (!base) {
+		pr_err("%s: invalid address\n", np->name);
+		return;
+	}
+
+	freq = __raw_readl(base);
+	clk = clk_register_fixed_rate(NULL, np->name, NULL, 0, freq);
+
+	if (IS_ERR(clk)) {
+		pr_err("%s: clk registration failed\n", np->name);
+		return;
+	}
+
+	if (of_clk_add_provider(np, of_clk_src_simple_get, clk)) {
+		pr_err("%s: clk provider registration failed\n", np->name);
+		return;
+	}
+}
+CLK_OF_DECLARE(xtfpga_clk, "cdns,xtfpga-clock", xtfpga_clk_setup);
+
 static int __init machine_setup(void)
 {
-	struct device_node *clock;
 	struct device_node *eth = NULL;
 
-	for_each_node_by_name(clock, "main-oscillator")
-		update_clock_frequency(clock);
-
 	if ((eth = of_find_compatible_node(eth, NULL, "opencores,ethoc")))
 		update_local_mac(eth);
 	return 0;
-- 
2.1.4