From 6cefe92f2991c2bcd8f3a16faa9f4e3c91be48f2 Mon Sep 17 00:00:00 2001
From: Russell King <rmk+kernel@arm.linux.org.uk>
Date: Fri, 29 Nov 2013 01:03:35 +0000
Subject: [PATCH] ARM: footbridge: add sched_clock implementation

Signed-off-by: Russell King <rmk+kernel@arm.linux.org.uk>
---
 arch/arm/mach-footbridge/common.h        |  2 ++
 arch/arm/mach-footbridge/dc21285-timer.c | 17 +++++++++++++++++
 arch/arm/mach-footbridge/ebsa285.c       |  1 +
 3 files changed, 20 insertions(+)

diff --git a/arch/arm/mach-footbridge/common.h b/arch/arm/mach-footbridge/common.h
index 56607b3a773e..b911e5587ecf 100644
--- a/arch/arm/mach-footbridge/common.h
+++ b/arch/arm/mach-footbridge/common.h
@@ -10,3 +10,5 @@ extern void footbridge_init_irq(void);
 
 extern void isa_init_irq(unsigned int irq);
 extern void footbridge_restart(enum reboot_mode, const char *);
+
+extern void footbridge_sched_clock(void);
diff --git a/arch/arm/mach-footbridge/dc21285-timer.c b/arch/arm/mach-footbridge/dc21285-timer.c
index 9ee78f7b4990..785e4199f9cd 100644
--- a/arch/arm/mach-footbridge/dc21285-timer.c
+++ b/arch/arm/mach-footbridge/dc21285-timer.c
@@ -9,6 +9,7 @@
 #include <linux/init.h>
 #include <linux/interrupt.h>
 #include <linux/irq.h>
+#include <linux/sched_clock.h>
 
 #include <asm/irq.h>
 
@@ -104,3 +105,19 @@ void __init footbridge_timer_init(void)
 	ce->cpumask = cpumask_of(smp_processor_id());
 	clockevents_config_and_register(ce, mem_fclk_21285, 0x4, 0xffffff);
 }
+
+static u32 notrace footbridge_read_sched_clock(void)
+{
+	return ~*CSR_TIMER3_VALUE;
+}
+
+void __init footbridge_sched_clock(void)
+{
+	unsigned rate = DIV_ROUND_CLOSEST(mem_fclk_21285, 16);
+
+	*CSR_TIMER3_LOAD = 0;
+	*CSR_TIMER3_CLR = 0;
+	*CSR_TIMER3_CNTL = TIMER_CNTL_ENABLE | TIMER_CNTL_DIV16;
+
+	setup_sched_clock(footbridge_read_sched_clock, 24, rate);
+}
diff --git a/arch/arm/mach-footbridge/ebsa285.c b/arch/arm/mach-footbridge/ebsa285.c
index 1a7235fb52ac..3ff7a0c30a0f 100644
--- a/arch/arm/mach-footbridge/ebsa285.c
+++ b/arch/arm/mach-footbridge/ebsa285.c
@@ -104,6 +104,7 @@ MACHINE_START(EBSA285, "EBSA285")
 	.video_start	= 0x000a0000,
 	.video_end	= 0x000bffff,
 	.map_io		= footbridge_map_io,
+	.init_early	= footbridge_sched_clock,
 	.init_irq	= footbridge_init_irq,
 	.init_time	= footbridge_timer_init,
 	.restart	= footbridge_restart,