[PATCH v2] sunxi: psci: Fix sunxi_power_switch on sun8i-r40 platform

[qianfanguijin]

From: qianfan Zhao <qianfangui...@163.com>

linux system will die if we offline one of the cpu on R40 based board:
eg: echo 0 > /sys/devices/system/cpu/cpu3/online

Fixed sunxi_power_switch based on allwinner lichee 3.10 kernel driver.

Signed-off-by: qianfan Zhao <qianfangui...@163.com>
---
v2 changes: Fix the commit message, the source code doesn't change.

 arch/arm/cpu/armv7/sunxi/psci.c | 24 +++++++++++++++++++-----
 1 file changed, 19 insertions(+), 5 deletions(-)

diff --git a/arch/arm/cpu/armv7/sunxi/psci.c b/arch/arm/cpu/armv7/sunxi/psci.c
index 1ac50f558a..63186a9388 100644
--- a/arch/arm/cpu/armv7/sunxi/psci.c
+++ b/arch/arm/cpu/armv7/sunxi/psci.c
@@ -79,8 +79,7 @@ static void __secure __mdelay(u32 ms)
 static void __secure clamp_release(u32 __maybe_unused *clamp)
 {
 #if defined(CONFIG_MACH_SUN6I) || defined(CONFIG_MACH_SUN7I) || \
-       defined(CONFIG_MACH_SUN8I_H3) || \
-       defined(CONFIG_MACH_SUN8I_R40)
+       defined(CONFIG_MACH_SUN8I_H3)
        u32 tmp = 0x1ff;
        do {
                tmp >>= 1;
@@ -88,15 +87,30 @@ static void __secure clamp_release(u32 __maybe_unused 
*clamp)
        } while (tmp);
 
        __mdelay(10);
+#elif defined(CONFIG_MACH_SUN8I_R40)
+       u8 i, tmp = 0xfe;
+
+       for (i = 0; i < 5; i++) { /* 0xfe, 0xf8, 0xe0, 0x80, 0x00 */
+               writel(tmp, clamp);
+               tmp <<= 2;
+       }
+
+       while (0x00 != readl(clamp)) {
+               ;
+       }
 #endif
 }
 
 static void __secure clamp_set(u32 __maybe_unused *clamp)
 {
 #if defined(CONFIG_MACH_SUN6I) || defined(CONFIG_MACH_SUN7I) || \
-       defined(CONFIG_MACH_SUN8I_H3) || \
-       defined(CONFIG_MACH_SUN8I_R40)
+       defined(CONFIG_MACH_SUN8I_H3)
        writel(0xff, clamp);
+#elif defined(CONFIG_MACH_SUN8I_R40)
+       writel(0xff, clamp);
+       while (0xff != readl(clamp)) {
+               ;
+       }
 #endif
 }
 
@@ -153,7 +167,7 @@ static void __secure sunxi_cpu_set_power(int cpu, bool on)
 
        sunxi_power_switch((void *)cpucfg + SUN8I_R40_PWR_CLAMP(cpu),
                           (void *)cpucfg + SUN8I_R40_PWROFF,
-                          on, 0);
+                          on, cpu);
 }
 #else /* ! CONFIG_MACH_SUN7I && ! CONFIG_MACH_SUN8I_R40 */
 static void __secure sunxi_cpu_set_power(int cpu, bool on)
-- 
2.25.1