armv7/ls102xa: Fix non-boot cpus cannot correctly fall in spin table

[oweals/u-boot.git] / arch / arm / cpu / armv7 / ls102xa / cpu.c

diff --git a/arch/arm/cpu/armv7/ls102xa/cpu.c b/arch/arm/cpu/armv7/ls102xa/cpu.c
index 18665a32d238a25a2387ba87179ded47ffac0df0..8dd95d98791d763789f530d393a12563aa358282 100644 (file)
--- a/arch/arm/cpu/armv7/ls102xa/cpu.c
+++ b/arch/arm/cpu/armv7/ls102xa/cpu.c
@@ -16,6 +16,13 @@
 
 #include "fsl_epu.h"
 
+#define DCSR_RCPM2_BLOCK_OFFSET        0x223000
+#define DCSR_RCPM2_CPMFSMCR0   0x400
+#define DCSR_RCPM2_CPMFSMSR0   0x404
+#define DCSR_RCPM2_CPMFSMCR1   0x414
+#define DCSR_RCPM2_CPMFSMSR1   0x418
+#define CPMFSMSR_FSM_STATE_MASK        0x7f
+
 DECLARE_GLOBAL_DATA_PTR;
 
 #ifndef CONFIG_SYS_DCACHE_OFF
@@ -290,6 +297,27 @@ int cpu_eth_init(bd_t *bis)
 int arch_cpu_init(void)
 {
        void *epu_base = (void *)(CONFIG_SYS_DCSRBAR + EPU_BLOCK_OFFSET);
+       void *rcpm2_base =
+               (void *)(CONFIG_SYS_DCSRBAR + DCSR_RCPM2_BLOCK_OFFSET);
+       u32 state;
+
+       /*
+        * The RCPM FSM state may not be reset after power-on.
+        * So, reset them.
+        */
+       state = in_be32(rcpm2_base + DCSR_RCPM2_CPMFSMSR0) &
+               CPMFSMSR_FSM_STATE_MASK;
+       if (state != 0) {
+               out_be32(rcpm2_base + DCSR_RCPM2_CPMFSMCR0, 0x80);
+               out_be32(rcpm2_base + DCSR_RCPM2_CPMFSMCR0, 0x0);
+       }
+
+       state = in_be32(rcpm2_base + DCSR_RCPM2_CPMFSMSR1) &
+               CPMFSMSR_FSM_STATE_MASK;
+       if (state != 0) {
+               out_be32(rcpm2_base + DCSR_RCPM2_CPMFSMCR1, 0x80);
+               out_be32(rcpm2_base + DCSR_RCPM2_CPMFSMCR1, 0x0);
+       }
 
        /*
         * After wakeup from deep sleep, Clear EPU registers
@@ -301,7 +329,7 @@ int arch_cpu_init(void)
        return 0;
 }
 
-#if defined(CONFIG_ARMV7_NONSEC) || defined(CONFIG_ARMV7_VIRT)
+#ifdef CONFIG_ARMV7_NONSEC
 /* Set the address at which the secondary core starts from.*/
 void smp_set_core_boot_addr(unsigned long addr, int corenr)
 {
@@ -316,5 +344,13 @@ void smp_kick_all_cpus(void)
        struct ccsr_gur __iomem *gur = (void *)(CONFIG_SYS_FSL_GUTS_ADDR);
 
        out_be32(&gur->brrl, 0x2);
+
+       /*
+        * LS1 STANDBYWFE is not captured outside the ARM module in the soc.
+        * So add a delay to wait bootrom execute WFE.
+        */
+       udelay(1);
+
+       asm volatile("sev");
 }
 #endif