ARM: clps711x: Add clocksource driver

This patch adds clocksource driver for CLPS711X targets and adds
support to platform to use this new driver.

Signed-off-by: Alexander Shiyan <shc_work@mail.ru>
Signed-off-by: Sascha Hauer <s.hauer@pengutronix.de>

1 files changed, 25 insertions, 26 deletions

diff --git a/arch/arm/mach-clps711x/clock.c b/arch/arm/mach-clps711x/clock.c
index 09cbaf961c..e9576622d9 100644
--- a/arch/arm/mach-clps711x/clock.c
+++ b/arch/arm/mach-clps711x/clock.c
@@ -9,25 +9,18 @@
 
 #include <common.h>
 #include <init.h>
-#include <clock.h>
+#include <sizes.h>
 #include <asm/io.h>
 #include <linux/clkdev.h>
 
 #include <mach/clps711x.h>
 
+#define CLPS711X_OSC_FREQ	3686400
+#define CLPS711X_EXT_FREQ	13000000
+
 static struct clk {
 	unsigned long	rate;
-} uart_clk, bus_clk;
-
-static uint64_t clocksource_read(void)
-{
-	return ~readw(TC2D);
-}
-
-static struct clocksource cs = {
-	.read	= clocksource_read,
-	.mask	= CLOCKSOURCE_MASK(16),
-};
+} uart_clk, bus_clk, timer_clk;
 
 unsigned long clk_get_rate(struct clk *clk)
 {
@@ -50,22 +43,19 @@ EXPORT_SYMBOL(clk_disable);
 
 static int clocks_init(void)
 {
-	int osc, ext, pll, cpu, timer;
+	int pll, cpu;
 	u32 tmp;
 
-	osc = 3686400;
-	ext = 13000000;
-
 	tmp = readl(PLLR) >> 24;
 	if (tmp)
-		pll = (osc * tmp) / 2;
+		pll = (CLPS711X_OSC_FREQ * tmp) / 2;
 	else
 		pll = 73728000; /* Default value for old CPUs */
 
 	tmp = readl(SYSFLG2);
 	if (tmp & SYSFLG2_CKMODE) {
-		cpu = ext;
-		bus_clk.rate = cpu;
+		cpu = CLPS711X_EXT_FREQ;
+		bus_clk.rate = CLPS711X_EXT_FREQ;
 	} else {
 		cpu = pll;
 		if (cpu >= 36864000)
@@ -74,25 +64,23 @@ static int clocks_init(void)
 			bus_clk.rate = 36864000 / 2;
 	}
 
-	uart_clk.rate = bus_clk.rate / 10;
+	uart_clk.rate = DIV_ROUND_CLOSEST(bus_clk.rate, 10);
 
 	if (tmp & SYSFLG2_CKMODE) {
 		tmp = readw(SYSCON2);
 		if (tmp & SYSCON2_OSTB)
-			timer = ext / 26;
+			timer_clk.rate = DIV_ROUND_CLOSEST(CLPS711X_EXT_FREQ, 26);
 		else
-			timer = 541440;
+			timer_clk.rate = DIV_ROUND_CLOSEST(CLPS711X_EXT_FREQ, 24);
 	} else
-		timer = cpu / 144;
+		timer_clk.rate = DIV_ROUND_CLOSEST(cpu, 144);
 
 	tmp = readl(SYSCON1);
 	tmp &= ~SYSCON1_TC2M;	/* Free running mode */
 	tmp |= SYSCON1_TC2S;	/* High frequency source */
 	writel(tmp, SYSCON1);
 
-	clocks_calc_mult_shift(&cs.mult, &cs.shift, timer, NSEC_PER_SEC, 10);
-
-	return init_clock(&cs);
+	return 0;
 }
 core_initcall(clocks_init);
 
@@ -100,6 +88,7 @@ static struct clk_lookup clocks_lookups[] = {
 	CLKDEV_CON_ID("bus", &bus_clk),
 	CLKDEV_DEV_ID("clps711x_serial0", &uart_clk),
 	CLKDEV_DEV_ID("clps711x_serial1", &uart_clk),
+	CLKDEV_DEV_ID("clps711x-cs", &timer_clk),
 };
 
 static int clkdev_init(void)
@@ -109,3 +98,13 @@ static int clkdev_init(void)
 	return 0;
 }
 postcore_initcall(clkdev_init);
+
+static const char *clps711x_clocksrc_name = "clps711x-cs";
+
+static __init int clps711x_core_init(void)
+{
+	add_generic_device(clps711x_clocksrc_name, DEVICE_ID_SINGLE, NULL,
+			   TC2D, SZ_2, IORESOURCE_MEM, NULL);
+	return 0;
+}
+coredevice_initcall(clps711x_core_init);