1 files changed, 34 insertions, 22 deletions

diff --git a/arch/arm/mach-rockchip/platsmp.c b/arch/arm/mach-rockchip/platsmp.c
index 8fcec1cc101e..3e7a4b761a95 100644
--- a/arch/arm/mach-rockchip/platsmp.c
+++ b/arch/arm/mach-rockchip/platsmp.c
@@ -72,29 +72,22 @@ static struct reset_control *rockchip_get_core_reset(int cpu)
 static int pmu_set_power_domain(int pd, bool on)
 {
         u32 val = (on) ? 0 : BIT(pd);
+        struct reset_control *rstc = rockchip_get_core_reset(pd);
         int ret;
 
+        if (IS_ERR(rstc) && read_cpuid_part() != ARM_CPU_PART_CORTEX_A9) {
+                pr_err("%s: could not get reset control for core %d\n",
+                       __func__, pd);
+                return PTR_ERR(rstc);
+        }
+
         /*
          * We need to soft reset the cpu when we turn off the cpu power domain,
          * or else the active processors might be stalled when the individual
          * processor is powered down.
          */
-        if (read_cpuid_part() != ARM_CPU_PART_CORTEX_A9) {
-                struct reset_control *rstc = rockchip_get_core_reset(pd);
-
-                if (IS_ERR(rstc)) {
-                        pr_err("%s: could not get reset control for core %d\n",
-                               __func__, pd);
-                        return PTR_ERR(rstc);
-                }
-
-                if (on)
-                        reset_control_deassert(rstc);
-                else
-                        reset_control_assert(rstc);
-
-                reset_control_put(rstc);
-        }
+        if (!IS_ERR(rstc) && !on)
+                reset_control_assert(rstc);
 
         ret = regmap_update_bits(pmu, PMU_PWRDN_CON, BIT(pd), val);
         if (ret < 0) {
@@ -107,11 +100,17 @@ static int pmu_set_power_domain(int pd, bool on)
                 ret = pmu_power_domain_is_on(pd);
                 if (ret < 0) {
                         pr_err("%s: could not read power domain state\n",
-                                 __func__);
+                               __func__);
                         return ret;
                 }
         }
 
+        if (!IS_ERR(rstc)) {
+                if (on)
+                        reset_control_deassert(rstc);
+                reset_control_put(rstc);
+        }
+
         return 0;
 }
 
@@ -130,7 +129,7 @@ static int rockchip_boot_secondary(unsigned int cpu, struct task_struct *idle)
 
         if (cpu >= ncores) {
                 pr_err("%s: cpu %d outside maximum number of cpus %d\n",
-                                                        __func__, cpu, ncores);
+                       __func__, cpu, ncores);
                 return -ENXIO;
         }
 
@@ -140,14 +139,19 @@ static int rockchip_boot_secondary(unsigned int cpu, struct task_struct *idle)
                 return ret;
 
         if (read_cpuid_part() != ARM_CPU_PART_CORTEX_A9) {
-                /* We communicate with the bootrom to active the cpus other
+                /*
+                 * We communicate with the bootrom to active the cpus other
                  * than cpu0, after a blob of initialize code, they will
                  * stay at wfe state, once they are actived, they will check
                  * the mailbox:
                  * sram_base_addr + 4: 0xdeadbeaf
                  * sram_base_addr + 8: start address for pc
-                 * */
-                udelay(10);
+                 * The cpu0 need to wait the other cpus other than cpu0 entering
+                 * the wfe state.The wait time is affected by many aspects.
+                 * (e.g: cpu frequency, bootrom frequency, sram frequency, ...)
+                 */
+                mdelay(1); /* ensure the cpus other than cpu0 to startup */
+
                 writel(virt_to_phys(secondary_startup), sram_base_addr + 8);
                 writel(0xDEADBEAF, sram_base_addr + 4);
                 dsb_sev();
@@ -317,6 +321,13 @@ static void __init rockchip_smp_prepare_cpus(unsigned int max_cpus)
 #ifdef CONFIG_HOTPLUG_CPU
 static int rockchip_cpu_kill(unsigned int cpu)
 {
+        /*
+         * We need a delay here to ensure that the dying CPU can finish
+         * executing v7_coherency_exit() and reach the WFI/WFE state
+         * prior to having the power domain disabled.
+         */
+        mdelay(1);
+
         pmu_set_power_domain(0 + cpu, false);
         return 1;
 }
@@ -324,7 +335,7 @@ static int rockchip_cpu_kill(unsigned int cpu)
 static void rockchip_cpu_die(unsigned int cpu)
 {
         v7_exit_coherency_flush(louis);
-        while(1)
+        while (1)
                 cpu_do_idle();
 }
 #endif
@@ -337,4 +348,5 @@ static struct smp_operations rockchip_smp_ops __initdata = {
         .cpu_die                = rockchip_cpu_die,
 #endif
 };
+
 CPU_METHOD_OF_DECLARE(rk3066_smp, "rockchip,rk3066-smp", &rockchip_smp_ops);