[PATCH 2/2] MIPS: csrc-r4k: get cp0 counter rate from devicetree

[Antony Pavlov]

Signed-off-by: Antony Pavlov <antonynpavlov at gmail.com>
---
 arch/mips/lib/csrc-r4k.c | 24 ++++++++++++++++++++++--
 1 file changed, 22 insertions(+), 2 deletions(-)

diff --git a/arch/mips/lib/csrc-r4k.c b/arch/mips/lib/csrc-r4k.c
index 5c3f18f..6f6e18c 100644
--- a/arch/mips/lib/csrc-r4k.c
+++ b/arch/mips/lib/csrc-r4k.c
@@ -21,6 +21,8 @@
  */
 
 #include <init.h>
+#include <of.h>
+#include <linux/clk.h>
 #include <clock.h>
 #include <io.h>
 #include <asm/mipsregs.h>
@@ -37,8 +39,26 @@ static struct clocksource cs = {
 
 static int clocksource_init(void)
 {
-	cs.mult = clocksource_hz2mult(100000000, cs.shift);
+	unsigned int mips_hpt_frequency;
+	struct device_node *np;
+	struct clk *clk;
+
+	/* default rate: 100 MHz */
+	mips_hpt_frequency = 100000000;
+
+	if (IS_ENABLED(CONFIG_OFTREE)) {
+		np = of_get_cpu_node(0, NULL);
+		if (np) {
+			clk = of_clk_get(np, 0);
+			if (!IS_ERR(clk)) {
+				mips_hpt_frequency = clk_get_rate(clk) / 2;
+			}
+		}
+	}
+
+	clocks_calc_mult_shift(&cs.mult, &cs.shift,
+		mips_hpt_frequency, NSEC_PER_SEC, 10);
 
 	return init_clock(&cs);
 }
-core_initcall(clocksource_init);
+postcore_initcall(clocksource_init);
-- 
2.8.1