ARM: versatile: convert sched_clock() to use new infrastructure

Convert versatile platforms to use the new sched_clock() infrastructure
for extending 32bit counters to full 64-bit nanoseconds.

Tested-by: Will Deacon <will.deacon@arm.com>
Signed-off-by: Russell King <rmk+kernel@arm.linux.org.uk>

arch/arm/Kconfig

@@ -236,6 +236,7 @@ config ARCH_REALVIEW
 	bool "ARM Ltd. RealView family"
 	select ARM_AMBA
 	select COMMON_CLKDEV
+	select HAVE_SCHED_CLOCK
 	select ICST
 	select GENERIC_CLOCKEVENTS
 	select ARCH_WANT_OPTIONAL_GPIOLIB
@@ -250,6 +251,7 @@ config ARCH_VERSATILE
 	select ARM_AMBA
 	select ARM_VIC
 	select COMMON_CLKDEV
+	select HAVE_SCHED_CLOCK
 	select ICST
 	select GENERIC_CLOCKEVENTS
 	select ARCH_WANT_OPTIONAL_GPIOLIB

arch/arm/mach-realview/core.c

@@ -52,6 +52,8 @@
 #include <mach/irqs.h>
 #include <plat/timer-sp.h>
 
+#include <plat/sched_clock.h>
+
 #include "core.h"
 
 /* used by entry-macro.S and platsmp.c */
@@ -657,6 +659,12 @@ void realview_leds_event(led_event_t ledevt)
 }
 #endif	/* CONFIG_LEDS */
 
+/*
+ * The sched_clock counter
+ */
+#define REFCOUNTER		(__io_address(REALVIEW_SYS_BASE) + \
+				 REALVIEW_SYS_24MHz_OFFSET)
+
 /*
  * Where is the timer (VA)?
  */
@@ -672,6 +680,8 @@ void __init realview_timer_init(unsigned int timer_irq)
 {
 	u32 val;
 
+	versatile_sched_clock_init(REFCOUNTER, 24000000);
+
 	/* 
 	 * set clock frequency: 
 	 *	REALVIEW_REFCLK is 32KHz

arch/arm/mach-versatile/core.c

@@ -51,6 +51,8 @@
 #include <mach/platform.h>
 #include <plat/timer-sp.h>
 
+#include <plat/sched_clock.h>
+
 #include "core.h"
 
 /*
@@ -885,6 +887,12 @@ void __init versatile_init(void)
 #endif
 }
 
+/*
+ * The sched_clock counter
+ */
+#define REFCOUNTER		(__io_address(VERSATILE_SYS_BASE) + \
+				 VERSATILE_SYS_24MHz_OFFSET)
+
 /*
  * Where is the timer (VA)?
  */
@@ -900,6 +908,8 @@ static void __init versatile_timer_init(void)
 {
 	u32 val;
 
+	versatile_sched_clock_init(REFCOUNTER, 24000000);
+
 	/* 
 	 * set clock frequency: 
 	 *	VERSATILE_REFCLK is 32KHz

arch/arm/plat-versatile/include/plat/sched_clock.h

@@ -0,0 +1,6 @@
+#ifndef ARM_PLAT_SCHED_CLOCK_H
+#define ARM_PLAT_SCHED_CLOCK_H
+
+void versatile_sched_clock_init(void __iomem *, unsigned long);
+
+#endif

arch/arm/plat-versatile/sched-clock.c

@@ -18,37 +18,41 @@
  * along with this program; if not, write to the Free Software
  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  */
-#include <linux/cnt32_to_63.h>
 #include <linux/io.h>
 #include <linux/sched.h>
-#include <asm/div64.h>
 
-#include <mach/hardware.h>
-#include <mach/platform.h>
+#include <asm/sched_clock.h>
+#include <plat/sched_clock.h>
 
-#ifdef VERSATILE_SYS_BASE
-#define REFCOUNTER	(__io_address(VERSATILE_SYS_BASE) + VERSATILE_SYS_24MHz_OFFSET)
-#endif
-
-#ifdef REALVIEW_SYS_BASE
-#define REFCOUNTER	(__io_address(REALVIEW_SYS_BASE) + REALVIEW_SYS_24MHz_OFFSET)
-#endif
+static DEFINE_CLOCK_DATA(cd);
+static void __iomem *ctr;
 
 /*
- * This is the Realview and Versatile sched_clock implementation.  This
- * has a resolution of 41.7ns, and a maximum value of about 35583 days.
- *
- * The return value is guaranteed to be monotonic in that range as
- * long as there is always less than 89 seconds between successive
- * calls to this function.
+ * Constants generated by clocks_calc_mult_shift(m, s, 24MHz, NSEC_PER_SEC, 60).
+ * This gives a resolution of about 41ns and a wrap period of about 178s.
  */
+#define SC_MULT		2796202667u
+#define SC_SHIFT	26
+
 unsigned long long notrace sched_clock(void)
 {
-	unsigned long long v = cnt32_to_63(readl(REFCOUNTER));
+	if (ctr) {
+		u32 cyc = readl(ctr);
+		return cyc_to_fixed_sched_clock(&cd, cyc, (u32)~0,
+						SC_MULT, SC_SHIFT);
+	} else
+		return 0;
+}
 
-	/* the <<1 gets rid of the cnt_32_to_63 top bit saving on a bic insn */
-	v *= 125<<1;
-	do_div(v, 3<<1);
+static void notrace versatile_update_sched_clock(void)
+{
+	u32 cyc = readl(ctr);
+	update_sched_clock(&cd, cyc, (u32)~0);
+}
 
-	return v;
+void __init versatile_sched_clock_init(void __iomem *reg, unsigned long rate)
+{
+	ctr = reg;
+	init_fixed_sched_clock(&cd, versatile_update_sched_clock,
+			       32, rate, SC_MULT, SC_SHIFT);
 }