ARM: tegra: timer: Add suspend and resume support

Change-Id: I4d431037e83742e123ad81e4614354b14e32d453
Signed-off-by: Colin Cross <ccross@android.com>

1 files changed, 39 insertions, 31 deletions

diff --git a/arch/arm/mach-tegra/timer.c b/arch/arm/mach-tegra/timer.c
index cb83ca8a7d37..732a60cab93f 100644
--- a/arch/arm/mach-tegra/timer.c
+++ b/arch/arm/mach-tegra/timer.c
@@ -60,6 +60,8 @@ static void __iomem *rtc_base = IO_ADDRESS(TEGRA_RTC_BASE);
 
 static struct timespec persistent_ts;
 static u64 persistent_ms, last_persistent_ms;
+static u32 usec_offset;
+static bool usec_suspended;
 
 #define timer_writel(value, reg) \
 	__raw_writel(value, (u32)timer_reg_base + (reg))
@@ -115,15 +117,23 @@ static DEFINE_CLOCK_DATA(cd);
 #define SC_MULT		4194304000u
 #define SC_SHIFT	22
 
+static u32 notrace tegra_read_usec(void)
+{
+	u32 cyc = usec_offset;
+	if (!usec_suspended)
+		cyc += timer_readl(TIMERUS_CNTR_1US);
+	return cyc;
+}
+
 unsigned long long notrace sched_clock(void)
 {
-	u32 cyc = timer_readl(TIMERUS_CNTR_1US);
+	u32 cyc = tegra_read_usec();
 	return cyc_to_fixed_sched_clock(&cd, cyc, (u32)~0, SC_MULT, SC_SHIFT);
 }
 
 static void notrace tegra_update_sched_clock(void)
 {
-	u32 cyc = timer_readl(TIMERUS_CNTR_1US);
+	u32 cyc = tegra_read_usec();
 	update_sched_clock(&cd, cyc, (u32)~0);
 }
 
@@ -179,6 +189,31 @@ static struct irqaction tegra_timer_irq = {
 	.irq		= INT_TMR3,
 };
 
+static u32 usec_config;
+
+static int tegra_timer_suspend(void)
+{
+	usec_config = timer_readl(TIMERUS_USEC_CFG);
+
+	usec_offset += timer_readl(TIMERUS_CNTR_1US);
+	usec_suspended = true;
+
+	return 0;
+}
+
+static void tegra_timer_resume(void)
+{
+	timer_writel(usec_config, TIMERUS_USEC_CFG);
+
+	usec_offset -= timer_readl(TIMERUS_CNTR_1US);
+	usec_suspended = false;
+}
+
+static struct syscore_ops tegra_timer_syscore_ops = {
+	.suspend = tegra_timer_suspend,
+	.resume = tegra_timer_resume,
+};
+
 static void __init tegra_init_timer(void)
 {
 	struct clk *clk;
@@ -241,37 +276,10 @@ static void __init tegra_init_timer(void)
 	tegra_clockevent.cpumask = cpu_all_mask;
 	tegra_clockevent.irq = tegra_timer_irq.irq;
 	clockevents_register_device(&tegra_clockevent);
+
+	register_syscore_ops(&tegra_timer_syscore_ops);
 }
 
 struct sys_timer tegra_timer = {
 	.init = tegra_init_timer,
 };
-
-#ifdef CONFIG_PM
-static u32 usec_config;
-
-static int tegra_timer_suspend(void)
-{
-	usec_config = timer_readl(TIMERUS_USEC_CFG);
-
-	return 0;
-}
-
-static void tegra_timer_resume(void)
-{
-	timer_writel(usec_config, TIMERUS_USEC_CFG);
-}
-
-static struct syscore_ops tegra_timer_syscore_ops = {
-	.suspend = tegra_timer_suspend,
-	.resume = tegra_timer_resume,
-};
-
-static int tegra_timer_syscore_init(void)
-{
-	register_syscore_ops(&tegra_timer_syscore_ops);
-
-	return 0;
-}
-subsys_initcall(tegra_timer_syscore_init);
-#endif