tegra RM: Stabilized local timers input frequency.

Utilized local timers prescaler to keep input frequency at calibration
(boot) level while CPU clock is scaled by DVFS. The local timer tick is
lowered to 2.0MHz/1.5MHz on T20/AP20 (was 250MHz/187.5MHz respectively).
Still it is better than 1MHz used as a base for jiffies counting. The
LP2 dead time is not compensated by this commit, nevertheless it should
address bug 660382 (no LP2 during video playback).

Change-Id: I9fd0e1238afa4cf1b44339bf30c37a2a84e97ae9
Reviewed-on: http://git-master/r/865
Tested-by: Aleksandr Frid <afrid@nvidia.com>
Reviewed-by: Gary King <gking@nvidia.com>

8 files changed, 154 insertions, 3 deletions

diff --git a/arch/arm/Kconfig b/arch/arm/Kconfig
index d63633f566b3..8065d1c7502f 100644
--- a/arch/arm/Kconfig
+++ b/arch/arm/Kconfig
@@ -947,6 +947,12 @@ config HAVE_ARM_TWD
 	help
 	  This options enables support for the ARM timer and watchdog unit
 
+config USE_ARM_TWD_PRESCALER
+	bool
+	depends on HAVE_ARM_TWD
+	help
+	  This options enables support for the ARM timer prescaler
+
 choice
 	prompt "Memory split"
 	default VMSPLIT_3G
@@ -988,6 +994,7 @@ config LOCAL_TIMERS
 	depends on SMP && (REALVIEW_EB_ARM11MP || MACH_REALVIEW_PB11MP || REALVIEW_EB_A9MP || MACH_REALVIEW_PBX || ARCH_TEGRA)
 	default y
 	select HAVE_ARM_TWD if (ARCH_REALVIEW || ARCH_TEGRA)
+	select USE_ARM_TWD_PRESCALER if (ARCH_TEGRA)
 	help
 	  Enable support for local timers on SMP platforms, rather then the
 	  legacy IPI broadcast method.  Local timers allows the system
diff --git a/arch/arm/include/asm/smp_twd.h b/arch/arm/include/asm/smp_twd.h
index 7be0978b2625..45d2ed6d87ab 100644
--- a/arch/arm/include/asm/smp_twd.h
+++ b/arch/arm/include/asm/smp_twd.h
@@ -5,6 +5,15 @@ struct clock_event_device;
 
 extern void __iomem *twd_base;
 
+#ifdef CONFIG_USE_ARM_TWD_PRESCALER
+extern unsigned long timer_prescaler;
+void twd_set_prescaler(void* unused);
+#else
+static inline void twd_set_prescaler(void* unused)
+{
+}
+#endif
+
 void twd_timer_stop(void);
 int twd_timer_ack(void);
 void twd_timer_setup(struct clock_event_device *);
diff --git a/arch/arm/kernel/smp_twd.c b/arch/arm/kernel/smp_twd.c
index aabd62d6bd19..e7177d921b32 100644
--- a/arch/arm/kernel/smp_twd.c
+++ b/arch/arm/kernel/smp_twd.c
@@ -38,9 +38,22 @@
 #define TWD_TIMER_CONTROL_PERIODIC	(1 << 1)
 #define TWD_TIMER_CONTROL_IT_ENABLE	(1 << 2)
 
+#define TWD_TIMER_CONTROL_PRESCALER_LSB		8
+#define TWD_TIMER_CONTROL_PRESCALER_MASK	0xFF
+#define TWD_TIMER_CONTROL_PRESCALER_FIELD \
+	(TWD_TIMER_CONTROL_PRESCALER_MASK << TWD_TIMER_CONTROL_PRESCALER_LSB)
+
 /* set up by the platform code */
 void __iomem *twd_base;
 
+#ifdef CONFIG_USE_ARM_TWD_PRESCALER
+/* dynamically updated by the platform code */
+unsigned long timer_prescaler = 0;
+
+static spinlock_t timer_control_lock = SPIN_LOCK_UNLOCKED;
+static unsigned long flags;
+#endif
+
 static unsigned long twd_timer_rate;
 
 static void twd_set_mode(enum clock_event_mode mode,
@@ -63,21 +76,64 @@ static void twd_set_mode(enum clock_event_mode mode,
 	default:
 		ctrl = 0;
 	}
+#ifdef CONFIG_USE_ARM_TWD_PRESCALER
+	spin_lock_irqsave(&timer_control_lock, flags);
 
+	ctrl |= (timer_prescaler << TWD_TIMER_CONTROL_PRESCALER_LSB);
 	__raw_writel(ctrl, twd_base + TWD_TIMER_CONTROL);
+
+	spin_unlock_irqrestore(&timer_control_lock, flags);
+#else
+	__raw_writel(ctrl, twd_base + TWD_TIMER_CONTROL);
+#endif
 }
 
 static int twd_set_next_event(unsigned long evt,
 			struct clock_event_device *unused)
 {
+#ifdef CONFIG_USE_ARM_TWD_PRESCALER
+	unsigned long ctrl;
+
+	spin_lock_irqsave(&timer_control_lock, flags);
+
+	ctrl = __raw_readl(twd_base + TWD_TIMER_CONTROL);
+	ctrl &= (~TWD_TIMER_CONTROL_PRESCALER_FIELD);
+	ctrl |= (timer_prescaler << TWD_TIMER_CONTROL_PRESCALER_LSB);
+	ctrl |= TWD_TIMER_CONTROL_ENABLE;
+
+	__raw_writel(evt, twd_base + TWD_TIMER_COUNTER);
+	__raw_writel(ctrl, twd_base + TWD_TIMER_CONTROL);
+
+	spin_unlock_irqrestore(&timer_control_lock, flags);
+#else
 	unsigned long ctrl = __raw_readl(twd_base + TWD_TIMER_CONTROL);
 
 	__raw_writel(evt, twd_base + TWD_TIMER_COUNTER);
 	__raw_writel(ctrl | TWD_TIMER_CONTROL_ENABLE, twd_base + TWD_TIMER_CONTROL);
-
+#endif
 	return 0;
 }
 
+#ifdef CONFIG_USE_ARM_TWD_PRESCALER
+/*
+ * Loads prescaler settings into control register
+ * (matches smp inter-processor call signature).
+ */
+void twd_set_prescaler(void* unused)
+{
+	unsigned long ctrl;
+
+	spin_lock_irqsave(&timer_control_lock, flags);
+
+	ctrl = __raw_readl(twd_base + TWD_TIMER_CONTROL);
+	ctrl &= (~TWD_TIMER_CONTROL_PRESCALER_FIELD);
+	ctrl |= (timer_prescaler << TWD_TIMER_CONTROL_PRESCALER_LSB);
+	__raw_writel(ctrl, twd_base + TWD_TIMER_CONTROL);
+
+	spin_unlock_irqrestore(&timer_control_lock, flags);
+}
+#endif
+
 /*
  * local_timer_ack: checks for a local timer interrupt.
  *
@@ -115,9 +171,14 @@ static void __cpuinit twd_calibrate_rate(void)
 		/* OK, now the tick has started, let's get the timer going */
 		waitjiffies += 5;
 
+#ifdef CONFIG_USE_ARM_TWD_PRESCALER
+		/* set prescaler, enable, no interrupt or reload */
+		load = (timer_prescaler << TWD_TIMER_CONTROL_PRESCALER_LSB);
+		__raw_writel(0x1 | load, twd_base + TWD_TIMER_CONTROL);
+#else
 				 /* enable, no interrupt or reload */
 		__raw_writel(0x1, twd_base + TWD_TIMER_CONTROL);
-
+#endif
 				 /* maximum value */
 		__raw_writel(0xFFFFFFFFU, twd_base + TWD_TIMER_COUNTER);
 
@@ -129,7 +190,7 @@ static void __cpuinit twd_calibrate_rate(void)
 		twd_timer_rate = (0xFFFFFFFFU - count) * (HZ / 5);
 
 		printk("%lu.%02luMHz.\n", twd_timer_rate / 1000000,
-			(twd_timer_rate / 100000) % 100);
+			(twd_timer_rate / 10000) % 100);
 	}
 
 	load = twd_timer_rate / HZ;
diff --git a/arch/arm/mach-tegra/include/mach/timex.h b/arch/arm/mach-tegra/include/mach/timex.h
index b4f0b3f8c740..d59ade633b8c 100644
--- a/arch/arm/mach-tegra/include/mach/timex.h
+++ b/arch/arm/mach-tegra/include/mach/timex.h
@@ -23,4 +23,12 @@
 
 #define CLOCK_TICK_RATE		1000000
 
+#if defined(CONFIG_USE_ARM_TWD_PRESCALER)
+void local_timer_rescale(unsigned long cpu_freq_khz);
+#else
+static inline void local_timer_rescale(unsigned long cpu_freq_khz)
+{
+}
+#endif
+
 #endif
diff --git a/arch/arm/mach-tegra/include/nvrm_power_private.h b/arch/arm/mach-tegra/include/nvrm_power_private.h
index 11b7f777c9d0..87a6ca6ef69b 100644
--- a/arch/arm/mach-tegra/include/nvrm_power_private.h
+++ b/arch/arm/mach-tegra/include/nvrm_power_private.h
@@ -464,6 +464,15 @@ NvRmFreqKHz NvRmPrivDfsGetMaxKHz(NvRmDfsClockId ClockId);
 NvRmFreqKHz NvRmPrivDfsGetMinKHz(NvRmDfsClockId ClockId);
 
 /**
+ * Gets current frequency for the specified DFS clock domain.
+ *
+ * @param ClockId The DFS ID of the targeted clock domain.
+ *
+ * @return Current domain frequency in kHz
+ */
+NvRmFreqKHz NvRmPrivDfsGetCurrentKHz(NvRmDfsClockId ClockId);
+
+/**
  * Signals DFS clock control thread
  * 
  * @param Mode Synchronization mode. In synchronous mode this function returns
diff --git a/arch/arm/mach-tegra/localtimer.c b/arch/arm/mach-tegra/localtimer.c
index 9bb5407bb49a..58bef261ab0a 100644
--- a/arch/arm/mach-tegra/localtimer.c
+++ b/arch/arm/mach-tegra/localtimer.c
@@ -33,18 +33,52 @@
 #include <linux/percpu.h>
 #include <linux/clockchips.h>
 #include <asm/smp_twd.h>
+#include "mach/timex.h"
 
 #if defined(CONFIG_ARCH_TEGRA_2x_SOC)
 #define LOCAL_TIMER_ADDR 0x50040600UL
 #define LOCAL_TIMER_IRQ  29
+#define CPU_FREQ_SCALE_SHIFT	24
+#define CPU_FREQ_SCALE_DIVIDER	(0x1 << CPU_FREQ_SCALE_SHIFT)
+#define CPU_FREQ_SCALE_INIT	125
 #else
 #error "Unsupported Tegra SoC family"
 #endif
 
 #if defined(CONFIG_LOCAL_TIMERS) && defined(CONFIG_HAVE_ARM_TWD)
 
+#if defined(CONFIG_USE_ARM_TWD_PRESCALER)
+/*
+ * Find new prescaler value for cpu frequency changes, so that local timer
+ * input frequency is kept at calibration level. Save new value in shadow
+ * variable - do not update h/w.
+ */
+void local_timer_rescale(unsigned long cpu_freq_khz)
+{
+	static unsigned long cpu_freq_scale_mult = 0;
+	unsigned long scale;
+
+	/* 1st call at boot/calibration frequency */
+	if (cpu_freq_scale_mult == 0) {
+		cpu_freq_scale_mult = ((timer_prescaler + 1) <<
+			CPU_FREQ_SCALE_SHIFT) / cpu_freq_khz;
+		printk("Local timer scaling factor %d, shift %d\n",
+			cpu_freq_scale_mult, CPU_FREQ_SCALE_SHIFT);
+		return;
+	}
+
+	scale = (unsigned long)((((uint64_t)cpu_freq_khz * cpu_freq_scale_mult)
+			+ CPU_FREQ_SCALE_DIVIDER -1) >> CPU_FREQ_SCALE_SHIFT);
+	timer_prescaler = scale - 1;
+}
+#endif
+
 void __cpuinit local_timer_setup(struct clock_event_device *clk)
 {
+#if defined(CONFIG_USE_ARM_TWD_PRESCALER)
+	if (timer_prescaler == 0)
+		timer_prescaler = CPU_FREQ_SCALE_INIT - 1;
+#endif
 	twd_base = IO_ADDRESS(LOCAL_TIMER_ADDR);
 	clk->irq = LOCAL_TIMER_IRQ;
 	twd_timer_setup(clk);
diff --git a/arch/arm/mach-tegra/nvrm/core/common/nvrm_power_dfs.c b/arch/arm/mach-tegra/nvrm/core/common/nvrm_power_dfs.c
index c19998409856..ce8601c06e23 100644
--- a/arch/arm/mach-tegra/nvrm/core/common/nvrm_power_dfs.c
+++ b/arch/arm/mach-tegra/nvrm/core/common/nvrm_power_dfs.c
@@ -2029,6 +2029,13 @@ NvRmFreqKHz NvRmPrivDfsGetMinKHz(NvRmDfsClockId ClockId)
     return pDfs->DfsParameters[ClockId].MinKHz;
 }
 
+NvRmFreqKHz NvRmPrivDfsGetCurrentKHz(NvRmDfsClockId ClockId)
+{
+    NvRmDfs* pDfs = &s_Dfs;
+    NV_ASSERT((0 < ClockId) && (ClockId < NvRmDfsClockId_Num));
+    return pDfs->CurrentKHz.Domains[ClockId];
+}
+
 void NvRmPrivDfsSignal(NvRmDfsBusyHintSyncMode Mode)
 {
     NvRmDfs* pDfs = &s_Dfs;
diff --git a/arch/arm/mach-tegra/nvrm_user.c b/arch/arm/mach-tegra/nvrm_user.c
index 6ebee8c58562..13540ca9f926 100644
--- a/arch/arm/mach-tegra/nvrm_user.c
+++ b/arch/arm/mach-tegra/nvrm_user.c
@@ -34,6 +34,8 @@
 #ifdef CONFIG_HAS_EARLYSUSPEND
 #include <linux/earlysuspend.h>
 #endif
+#include <linux/smp.h>
+#include <asm/smp_twd.h>
 #include <asm/cpu.h>
 #include "nvcommon.h"
 #include "nvassert.h"
@@ -44,6 +46,7 @@
 #include "linux/nvos_ioctl.h"
 #include "nvrm_power_private.h"
 #include "nvreftrack.h"
+#include "mach/timex.h"
 
 pid_t s_nvrm_daemon_pid = 0;
 int s_nvrm_daemon_sig = 0;
@@ -111,11 +114,23 @@ static void NvRmDfsThread(void *args)
 
     if (NvRmDfsGetState(hRm) > NvRmDfsRunState_Disabled)
     {
+        NvRmFreqKHz CpuKHz, f;
+        CpuKHz = NvRmPrivDfsGetCurrentKHz(NvRmDfsClockId_Cpu);
+        local_timer_rescale(CpuKHz);
+
         NvRmDfsSetState(hRm, NvRmDfsRunState_ClosedLoop);
 
         for (;;)
         {
             NvRmPmRequest Request = NvRmPrivPmThread();
+            f = NvRmPrivDfsGetCurrentKHz(NvRmDfsClockId_Cpu);
+            if (CpuKHz != f)
+            {
+                CpuKHz = f;
+                local_timer_rescale(CpuKHz);
+                twd_set_prescaler(NULL);
+                smp_call_function(twd_set_prescaler, NULL, NV_TRUE);
+            }
             if (Request & NvRmPmRequest_ExitFlag)
             {
                 break;
@@ -126,6 +141,7 @@ static void NvRmDfsThread(void *args)
                 printk("DFS requested CPU1 ON\n");
                 preset_lpj = per_cpu(cpu_data, 0).loops_per_jiffy;
                 cpu_up(1);
+                smp_call_function(twd_set_prescaler, NULL, NV_TRUE);
 #endif
             }