ARM: zynq: Enable A9 clock gating feature

Signed-off-by: Soren Brinkmann <soren.brinkmann@xilinx.com>
Signed-off-by: Michal Simek <michal.simek@xilinx.com>

diff --git a/arch/arm/mach-zynq/common.c b/arch/arm/mach-zynq/common.c
index 8ce276c21f1b2bc327126a2ca63aad3bd33a323f..baefd698e00dec4bcee0099311e2d8338ba765f9 100644 (file)
--- a/arch/arm/mach-zynq/common.c
+++ b/arch/arm/mach-zynq/common.c
@@ -90,6 +90,7 @@ early_initcall(zynq_l2c_init);
 static void __init zynq_init_late(void)
 {
        zynq_pm_late_init();
+       zynq_core_pm_init();
        zynq_prefetch_init();
 }
 

diff --git a/arch/arm/mach-zynq/common.h b/arch/arm/mach-zynq/common.h
index 5daee792a98456e66a0dde906796c63769b19a3a..a2607936f504dbf84787b57c0a8550c183d6bac7 100644 (file)
--- a/arch/arm/mach-zynq/common.h
+++ b/arch/arm/mach-zynq/common.h
@@ -72,4 +72,15 @@ static inline void zynq_prefetch_init(void)
                      : : : "r1");
 }
 
+static inline void zynq_core_pm_init(void)
+{
+       /* A9 clock gating */
+       asm volatile ("mrc  p15, 0, r12, c15, c0, 0\n"
+                     "orr  r12, r12, #1\n"
+                     "mcr  p15, 0, r12, c15, c0, 0\n"
+                     : /* no outputs */
+                     : /* no inputs */
+                     : "r12");
+}
+
 #endif

diff --git a/arch/arm/mach-zynq/platsmp.c b/arch/arm/mach-zynq/platsmp.c
index 3dd4bb6e930e5d4687a09fedbe54896ef3e2e4c1..d42e8bbc7d70581f8a94b3140600ad5058b31528 100644 (file)
--- a/arch/arm/mach-zynq/platsmp.c
+++ b/arch/arm/mach-zynq/platsmp.c
@@ -117,6 +117,7 @@ static void __init zynq_smp_prepare_cpus(unsigned int max_cpus)
  */
 static void zynq_secondary_init(unsigned int cpu)
 {
+       zynq_core_pm_init();
        zynq_prefetch_init();
 }
 

diff --git a/arch/arm/mach-zynq/pm.c b/arch/arm/mach-zynq/pm.c
index 248ef3d71aa1da5f52474b23fa8e4b2c619e7de8..0524c15a597363e7c8bd81c3b0cfe26b7806cd0f 100644 (file)
--- a/arch/arm/mach-zynq/pm.c
+++ b/arch/arm/mach-zynq/pm.c
@@ -65,13 +65,6 @@ static int zynq_pm_suspend(unsigned long arg)
        /* Topswitch clock stop disable */
        zynq_clk_topswitch_disable();
 
-       /* A9 clock gating */
-       asm volatile ("mrc  p15, 0, r12, c15, c0, 0\n"
-                     "orr  r12, r12, #1\n"
-                     "mcr  p15, 0, r12, c15, c0, 0\n"
-                     : /* no outputs */
-                     : /* no inputs */
-                     : "r12");
 
        if (!ocm_base || !ddrc_base)
                do_ddrpll_bypass = 0;
@@ -94,14 +87,6 @@ static int zynq_pm_suspend(unsigned long arg)
        /* Topswitch clock stop enable */
        zynq_clk_topswitch_enable();
 
-       /* A9 clock gating */
-       asm volatile ("mrc  p15, 0, r12, c15, c0, 0\n"
-                     "bic  r12, r12, #1\n"
-                     "mcr  p15, 0, r12, c15, c0, 0\n"
-                     : /* no outputs */
-                     : /* no inputs */
-                     : "r12");
-
        return 0;
 }