ARM: zynq: PM: Enable A9 internal 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 31a6fa40ba37ef0b37da4d9439258ba3e2b8726b..3cb7c198615a713ad44752a601b7281983a5c86b 100644 (file)
--- a/arch/arm/mach-zynq/common.c
+++ b/arch/arm/mach-zynq/common.c
@@ -98,6 +98,11 @@ static int __init zynq_get_revision(void)
        return revision;
 }
 
+static void __init zynq_init_late(void)
+{
+       zynq_core_pm_init();
+}
+
 /**
  * zynq_init_machine - System specific initialization, intended to be
  *                    called from board specific initialization.
@@ -204,6 +209,7 @@ DT_MACHINE_START(XILINX_EP107, "Xilinx Zynq Platform")
        .map_io         = zynq_map_io,
        .init_irq       = zynq_irq_init,
        .init_machine   = zynq_init_machine,
+       .init_late      = zynq_init_late,
        .init_time      = zynq_timer_init,
        .dt_compat      = zynq_dt_match,
        .reserve        = zynq_memory_init,

diff --git a/arch/arm/mach-zynq/common.h b/arch/arm/mach-zynq/common.h
index f652f0a884a67d34da78bfd27966540b97a12312..596ef0b5067cdbc202a16cbb440a6ad7999f3cc5 100644 (file)
--- a/arch/arm/mach-zynq/common.h
+++ b/arch/arm/mach-zynq/common.h
@@ -40,4 +40,15 @@ extern void __iomem *zynq_scu_base;
 /* Hotplug */
 extern void zynq_platform_cpu_die(unsigned int cpu);
 
+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 abc82ef085c1617011fa3174bfaea2b7d8cb6244..6c7843108c7f58f202d974111de21628af1f2078 100644 (file)
--- a/arch/arm/mach-zynq/platsmp.c
+++ b/arch/arm/mach-zynq/platsmp.c
@@ -112,6 +112,18 @@ static void __init zynq_smp_prepare_cpus(unsigned int max_cpus)
        scu_enable(zynq_scu_base);
 }
 
+/**
+ * zynq_secondary_init - Initialize secondary CPU cores
+ * @cpu:       CPU that is initialized
+ *
+ * This function is in the hotplug path. Don't move it into the
+ * init section!!
+ */
+static void zynq_secondary_init(unsigned int cpu)
+{
+       zynq_core_pm_init();
+}
+
 #ifdef CONFIG_HOTPLUG_CPU
 static int zynq_cpu_kill(unsigned cpu)
 {
@@ -124,6 +136,7 @@ struct smp_operations zynq_smp_ops __initdata = {
        .smp_init_cpus          = zynq_smp_init_cpus,
        .smp_prepare_cpus       = zynq_smp_prepare_cpus,
        .smp_boot_secondary     = zynq_boot_secondary,
+       .smp_secondary_init     = zynq_secondary_init,
 #ifdef CONFIG_HOTPLUG_CPU
        .cpu_die                = zynq_platform_cpu_die,
        .cpu_kill               = zynq_cpu_kill,