armv8: fsl-layerscape: Add NXP LS2088A SoC support

[u-boot] / arch / arm / cpu / armv8 / fsl-layerscape / mp.c

diff --git a/arch/arm/cpu/armv8/fsl-layerscape/mp.c b/arch/arm/cpu/armv8/fsl-layerscape/mp.c
index 1b13d3271916e3b54dd34d26ee90c388d62ee497..f607c3900ad53514b309a5d210eec3a6da477c56 100644 (file)
--- a/arch/arm/cpu/armv8/fsl-layerscape/mp.c
+++ b/arch/arm/cpu/armv8/fsl-layerscape/mp.c
@@ -25,7 +25,11 @@ phys_addr_t determine_mp_bootpg(void)
 int fsl_layerscape_wake_seconday_cores(void)
 {
        struct ccsr_gur __iomem *gur = (void *)(CONFIG_SYS_FSL_GUTS_ADDR);
+#ifdef CONFIG_FSL_LSCH3
        struct ccsr_reset __iomem *rst = (void *)(CONFIG_SYS_FSL_RST_ADDR);
+#elif defined(CONFIG_FSL_LSCH2)
+       struct ccsr_scfg __iomem *scfg = (void *)(CONFIG_SYS_FSL_SCFG_ADDR);
+#endif
        u32 cores, cpu_up_mask = 1;
        int i, timeout = 10;
        u64 *table = get_spin_tbl_addr();
@@ -48,13 +52,23 @@ int fsl_layerscape_wake_seconday_cores(void)
 
        printf("Waking secondary cores to start from %lx\n", gd->relocaddr);
 
+#ifdef CONFIG_FSL_LSCH3
        gur_out32(&gur->bootlocptrh, (u32)(gd->relocaddr >> 32));
        gur_out32(&gur->bootlocptrl, (u32)gd->relocaddr);
        gur_out32(&gur->scratchrw[6], 1);
        asm volatile("dsb st" : : : "memory");
        rst->brrl = cores;
        asm volatile("dsb st" : : : "memory");
+#elif defined(CONFIG_FSL_LSCH2)
+       scfg_out32(&scfg->scratchrw[0], (u32)(gd->relocaddr >> 32));
+       scfg_out32(&scfg->scratchrw[1], (u32)gd->relocaddr);
+       asm volatile("dsb st" : : : "memory");
+       gur_out32(&gur->brrl, cores);
+       asm volatile("dsb st" : : : "memory");
 
+       /* Bootup online cores */
+       scfg_out32(&scfg->corebcr, cores);
+#endif
        /* This is needed as a precautionary measure.
         * If some code before this has accidentally  released the secondary
         * cores then the pre-bootloader code will trap them in a "wfe" unless
@@ -90,6 +104,11 @@ int is_core_valid(unsigned int core)
        return !!((1 << core) & cpu_mask());
 }
 
+static int is_pos_valid(unsigned int pos)
+{
+       return !!((1 << pos) & cpu_pos_mask());
+}
+
 int is_core_online(u64 cpu_id)
 {
        u64 *table;
@@ -112,9 +131,9 @@ int cpu_disable(int nr)
        return 0;
 }
 
-int core_to_pos(int nr)
+static int core_to_pos(int nr)
 {
-       u32 cores = cpu_mask();
+       u32 cores = cpu_pos_mask();
        int i, count = 0;
 
        if (nr == 0) {
@@ -125,14 +144,17 @@ int core_to_pos(int nr)
        }
 
        for (i = 1; i < 32; i++) {
-               if (is_core_valid(i)) {
+               if (is_pos_valid(i)) {
                        count++;
                        if (count == nr)
                                break;
                }
        }
 
-       return count;
+       if (count != nr)
+               return -1;
+
+       return i;
 }
 
 int cpu_status(int nr)
@@ -178,6 +200,12 @@ int cpu_release(int nr, int argc, char * const argv[])
                           (unsigned long)table + SPIN_TABLE_ELEM_SIZE);
        asm volatile("dsb st");
        smp_kick_all_cpus();    /* only those with entry addr set will run */
+       /*
+        * When the first release command runs, all cores are set to go. Those
+        * without a valid entry address will be trapped by "wfe". "sev" kicks
+        * them off to check the address again. When set, they continue to run.
+        */
+       asm volatile("sev");
 
        return 0;
 }