ARM: tegra: power: Program CPU power control register

Bug 755621

Original-Change-Id: I7263e9f6e72b9eefc6e775ef646fcfde5290a129
Reviewed-on: http://git-master/r/25043
Reviewed-by: Scott Williams <scwilliams@nvidia.com>
Tested-by: Scott Williams <scwilliams@nvidia.com>
Change-Id: I5ae5fe008ec548c65bba1049dcfeee5454abcb93

6 files changed, 58 insertions, 3 deletions

diff --git a/arch/arm/mach-tegra/common.c b/arch/arm/mach-tegra/common.c
index 352a688308e1..900f6bd0be0a 100644
--- a/arch/arm/mach-tegra/common.c
+++ b/arch/arm/mach-tegra/common.c
@@ -284,6 +284,11 @@ void __init tegra_common_init(void)
 	   late. For non-SMP systems, the function that initializes the
 	   reset dispatcher is not called, so do it here for non-SMP. */
 	tegra_cpu_reset_handler_init();
+
+	/* The same goes for initialization of CPU dynamic power gating.
+	   For SMP systems it's done in SMP initialization. For non-SMP
+	   systems it's done here. */
+	tegra_cpu_dynamic_power_init();
 #endif
 	tegra_init_fuse();
 	tegra_init_clock();
diff --git a/arch/arm/mach-tegra/cortex-a9.S b/arch/arm/mach-tegra/cortex-a9.S
index 76dd9f7656e8..27ffe84daf67 100644
--- a/arch/arm/mach-tegra/cortex-a9.S
+++ b/arch/arm/mach-tegra/cortex-a9.S
@@ -3,7 +3,7 @@
  *
  * CPU state save & restore routines for CPU hotplug
  *
- * Copyright (c) 2010, NVIDIA Corporation.
+ * Copyright (c) 2010-2011, NVIDIA Corporation.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -408,6 +408,13 @@ ENTRY(__cortex_a9_restore)
 	tst	r2, #(0x1 << 6)
 	orrne	r2, r2, #(1 << 0)	@ sync FW bit with SMP state
 	mcr	p15, 0, r2, c1, c0, 1	@ actlr
+#ifndef CONFIG_ARCH_TEGRA_2x_SOC
+	mrc	p15, 0, r0, c0, c0, 5	@ MPIDR
+	tst	r0, #(0xF<<8)		@ cluster mask
+	bic	r3, r3, #(7<<8)		@ clear MAXCLKLATENCY field
+	orreq	r3, r3, #(3<<8)		@ set MAXCLKLATENCY to 3 on G
+	orrne	r3, r3, #(2<<8)		@ set MAXCLKLATENCY to 2 on LP
+#endif
 #ifndef CONFIG_TRUSTED_FOUNDATIONS
 	//TL : moved to secure 
 	mcr	p15, 0, r3, c15, c0, 0	@ pctlr
diff --git a/arch/arm/mach-tegra/headsmp-t2.S b/arch/arm/mach-tegra/headsmp-t2.S
index b71ff090918d..013ee6bfcbf2 100644
--- a/arch/arm/mach-tegra/headsmp-t2.S
+++ b/arch/arm/mach-tegra/headsmp-t2.S
@@ -3,7 +3,7 @@
  *
  * SMP initialization routines for Tegra2 SoCs
  *
- * Copyright (c) 2009-2010, NVIDIA Corporation.
+ * Copyright (c) 2009-2011, NVIDIA Corporation.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -170,3 +170,19 @@ __tegra_hotplug_data:
 	.long	__cortex_a9_restore
 	.size	__tegra_hotplug_data, . - __tegra_hotplug_data
 #endif
+
+/*
+ *	tegra_cpu_dynamic_power_init
+ *
+ *	  Confirgure tegra-specific CPU power-gating.
+ */
+
+	.align L1_CACHE_SHIFT
+ENTRY(tegra_cpu_dynamic_power_init)
+	mrc	p15, 0, r0, c15, c0, 0	@ power control
+	bic	r0, r0, #(7<<8)		@ clear MAXCLKLATENCY field
+	orr	r0, r0, #1		@ enable dynamic clock gating
+	orr	r0, r0, #(3<<8)		@ set MAXCLKLATENCY to 3
+	mcr	p15, 0, r0, c15, c0, 0	@ power control
+	bx	lr
+ENDPROC(tegra_cpu_dynamic_power_init)
diff --git a/arch/arm/mach-tegra/headsmp-t3.S b/arch/arm/mach-tegra/headsmp-t3.S
index f656e41f37a3..41160dc59155 100644
--- a/arch/arm/mach-tegra/headsmp-t3.S
+++ b/arch/arm/mach-tegra/headsmp-t3.S
@@ -3,7 +3,7 @@
  *
  * SMP initialization routines for Tegra3 SoCs
  *
- * Copyright (c) 2009-2010, NVIDIA Corporation.
+ * Copyright (c) 2009-2011, NVIDIA Corporation.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -207,3 +207,22 @@ __tegra_hotplug_data:
 	.long	__cortex_a9_restore
 	.size	__tegra_hotplug_data, . - __tegra_hotplug_data
 #endif
+
+/*
+ *	tegra_cpu_dynamic_power_init
+ *
+ *	  Confirgure tegra-specific CPU power-gating.
+ */
+
+	.align L1_CACHE_SHIFT
+ENTRY(tegra_cpu_dynamic_power_init)
+	mrc	p15, 0, r0, c0, c0, 5	@ MPIDR
+	tst	r0, #(0xF<<8)		@ cluster mask
+	mrc	p15, 0, r0, c15, c0, 0	@ power control
+	bic	r0, r0, #(7<<8)		@ clear MAXCLKLATENCY field
+	orr	r0, r0, #1		@ enable dynamic clock gating
+	orreq	r0, r0, #(3<<8)		@ set MAXCLKLATENCY to 3 on G
+	orrne	r0, r0, #(2<<8)		@ set MAXCLKLATENCY to 2 on LP
+	mcr	p15, 0, r0, c15, c0, 0	@ power control
+	bx	lr
+ENDPROC(tegra_cpu_dynamic_power_init)
diff --git a/arch/arm/mach-tegra/platsmp.c b/arch/arm/mach-tegra/platsmp.c
index bf3122ad8f7c..d2c735164b2e 100644
--- a/arch/arm/mach-tegra/platsmp.c
+++ b/arch/arm/mach-tegra/platsmp.c
@@ -79,6 +79,10 @@ void __cpuinit platform_secondary_init(unsigned int cpu)
 {
 	trace_hardirqs_off();
 	gic_cpu_init(0, IO_ADDRESS(TEGRA_ARM_PERIF_BASE) + 0x100);
+
+	/* Initialize CPU0 dynamic power gating (n > 0). */
+	tegra_cpu_dynamic_power_init();
+
 	/*
 	 * Synchronise with the boot thread.
 	 */
@@ -221,6 +225,9 @@ void __init smp_init_cpus(void)
 
 	/* Initialize the reset dispatcher. */
 	tegra_cpu_reset_handler_init();
+
+	/* Initialize CPU0 dynamic power gating. */
+	tegra_cpu_dynamic_power_init();
 }
 
 void __init smp_prepare_cpus(unsigned int max_cpus)
diff --git a/arch/arm/mach-tegra/power.h b/arch/arm/mach-tegra/power.h
index daa05533f121..ff668c2749fb 100644
--- a/arch/arm/mach-tegra/power.h
+++ b/arch/arm/mach-tegra/power.h
@@ -145,6 +145,7 @@ void tegra_idle_enter_lp2_cpu_0(struct cpuidle_device *dev,
 	struct cpuidle_state *state);
 void tegra_idle_enter_lp2_cpu_n(struct cpuidle_device *dev,
 	struct cpuidle_state *state);
+void tegra_cpu_dynamic_power_init(void);
 
 #if defined(CONFIG_TEGRA_AUTO_HOTPLUG) && !defined(CONFIG_ARCH_TEGRA_2x_SOC)
 int tegra_auto_hotplug_init(struct mutex *cpu_lock);