Merge branch 'mvebu/fixes' into mvebu/soc-cpuidle

[firefly-linux-kernel-4.4.55.git] / arch / arm / mach-mvebu / pmsu.c

diff --git a/arch/arm/mach-mvebu/pmsu.c b/arch/arm/mach-mvebu/pmsu.c
index 53a55c8520bf9a1fbab97919457af791f202ca2d..b31a8293a347c04a1974db7ba1644eb94498a309 100644 (file)
--- a/arch/arm/mach-mvebu/pmsu.c
+++ b/arch/arm/mach-mvebu/pmsu.c
@@ -66,6 +66,8 @@ static void __iomem *pmsu_mp_base;
 extern void ll_disable_coherency(void);
 extern void ll_enable_coherency(void);
 
+extern void armada_370_xp_cpu_resume(void);
+
 static struct platform_device armada_xp_cpuidle_device = {
        .name = "cpuidle-armada-370-xp",
 };
@@ -140,21 +142,14 @@ static void armada_370_xp_pmsu_enable_l2_powerdown_onidle(void)
        writel(reg, pmsu_mp_base + L2C_NFABRIC_PM_CTL);
 }
 
-static void armada_370_xp_cpu_resume(void)
-{
-       asm volatile("bl    ll_add_cpu_to_smp_group\n\t"
-                    "bl    ll_enable_coherency\n\t"
-                    "b     cpu_resume\n\t");
-}
-
 /* No locking is needed because we only access per-CPU registers */
-void armada_370_xp_pmsu_idle_prepare(bool deepidle)
+int armada_370_xp_pmsu_idle_enter(unsigned long deepidle)
 {
        unsigned int hw_cpu = cpu_logical_map(smp_processor_id());
        u32 reg;
 
        if (pmsu_mp_base == NULL)
-               return;
+               return -EINVAL;
 
        /*
         * Adjust the PMSU configuration to wait for WFI signal, enable
@@ -183,11 +178,6 @@ void armada_370_xp_pmsu_idle_prepare(bool deepidle)
        reg = readl(pmsu_mp_base + PMSU_CPU_POWER_DOWN_CONTROL(hw_cpu));
        reg |= PMSU_CPU_POWER_DOWN_DIS_SNP_Q_SKIP;
        writel(reg, pmsu_mp_base + PMSU_CPU_POWER_DOWN_CONTROL(hw_cpu));
-}
-
-static noinline int do_armada_370_xp_cpu_suspend(unsigned long deepidle)
-{
-       armada_370_xp_pmsu_idle_prepare(deepidle);
 
        v7_exit_coherency_flush(all);
 
@@ -206,12 +196,12 @@ static noinline int do_armada_370_xp_cpu_suspend(unsigned long deepidle)
 
        /* Test the CR_C bit and set it if it was cleared */
        asm volatile(
-       "mrc    p15, 0, %0, c1, c0, 0 \n\t"
-       "tst    %0, #(1 << 2) \n\t"
-       "orreq  %0, %0, #(1 << 2) \n\t"
-       "mcreq  p15, 0, %0, c1, c0, 0 \n\t"
+       "mrc    p15, 0, r0, c1, c0, 0 \n\t"
+       "tst    r0, #(1 << 2) \n\t"
+       "orreq  r0, r0, #(1 << 2) \n\t"
+       "mcreq  p15, 0, r0, c1, c0, 0 \n\t"
        "isb    "
-       : : "r" (0));
+       : : : "r0");
 
        pr_warn("Failed to suspend the system\n");
 
@@ -220,11 +210,11 @@ static noinline int do_armada_370_xp_cpu_suspend(unsigned long deepidle)
 
 static int armada_370_xp_cpu_suspend(unsigned long deepidle)
 {
-       return cpu_suspend(deepidle, do_armada_370_xp_cpu_suspend);
+       return cpu_suspend(deepidle, armada_370_xp_pmsu_idle_enter);
 }
 
 /* No locking is needed because we only access per-CPU registers */
-static noinline void armada_370_xp_pmsu_idle_restore(void)
+void armada_370_xp_pmsu_idle_exit(void)
 {
        unsigned int hw_cpu = cpu_logical_map(smp_processor_id());
        u32 reg;
@@ -253,7 +243,7 @@ static int armada_370_xp_cpu_pm_notify(struct notifier_block *self,
                unsigned int hw_cpu = cpu_logical_map(smp_processor_id());
                mvebu_pmsu_set_cpu_boot_addr(hw_cpu, armada_370_xp_cpu_resume);
        } else if (action == CPU_PM_EXIT) {
-               armada_370_xp_pmsu_idle_restore();
+               armada_370_xp_pmsu_idle_exit();
        }
 
        return NOTIFY_OK;
@@ -263,7 +253,7 @@ static struct notifier_block armada_370_xp_cpu_pm_notifier = {
        .notifier_call = armada_370_xp_cpu_pm_notify,
 };
 
-int __init armada_370_xp_cpu_pm_init(void)
+static int __init armada_370_xp_cpu_pm_init(void)
 {
        struct device_node *np;