armv8: fsl-layerscape: Fix "cpu status" command

The core position is not continuous for some SoCs. For example,
valid cores may present at position 0, 1, 4, 5, 8, 9, etc. Some
registers (including boot release register) only count existing
cores. Current implementation of cpu_mask() complies with the
continuous numbering. However, command "cpu status" queries the
spin table with actual core position. Add functions to calculate
core position from core number, to correctly calculate offsets.

Tested on LS2080ARDB and LS1043ARDB.

Signed-off-by: York Sun <york.sun@nxp.com>

diff --git a/arch/arm/cpu/armv8/fsl-layerscape/cpu.c b/arch/arm/cpu/armv8/fsl-layerscape/cpu.c
index 11e806ece01ffd2a6f2633c0eefa114b4c8fc9c3..b7a2e0c946081904e124701932f9c84f9d3a67cf 100644 (file)
--- a/arch/arm/cpu/armv8/fsl-layerscape/cpu.c
+++ b/arch/arm/cpu/armv8/fsl-layerscape/cpu.c
@@ -203,6 +203,27 @@ static inline u32 initiator_type(u32 cluster, int init_id)
        return 0;
 }
 
+u32 cpu_pos_mask(void)
+{
+       struct ccsr_gur __iomem *gur = (void *)(CONFIG_SYS_FSL_GUTS_ADDR);
+       int i = 0;
+       u32 cluster, type, mask = 0;
+
+       do {
+               int j;
+
+               cluster = gur_in32(&gur->tp_cluster[i].lower);
+               for (j = 0; j < TP_INIT_PER_CLUSTER; j++) {
+                       type = initiator_type(cluster, j);
+                       if (type && (TP_ITYP_TYPE(type) == TP_ITYP_TYPE_ARM))
+                               mask |= 1 << (i * TP_INIT_PER_CLUSTER + j);
+               }
+               i++;
+       } while ((cluster & TP_CLUSTER_EOC) == 0x0);
+
+       return mask;
+}
+
 u32 cpu_mask(void)
 {
        struct ccsr_gur __iomem *gur = (void *)(CONFIG_SYS_FSL_GUTS_ADDR);

diff --git a/arch/arm/cpu/armv8/fsl-layerscape/mp.c b/arch/arm/cpu/armv8/fsl-layerscape/mp.c
index df7ffb88f6a6ae2572040ea53ca1728a18980da8..f607c3900ad53514b309a5d210eec3a6da477c56 100644 (file)
--- a/arch/arm/cpu/armv8/fsl-layerscape/mp.c
+++ b/arch/arm/cpu/armv8/fsl-layerscape/mp.c
@@ -104,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;
@@ -126,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) {
@@ -139,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)

diff --git a/arch/arm/include/asm/arch-fsl-layerscape/mp.h b/arch/arm/include/asm/arch-fsl-layerscape/mp.h
index e46e076f16b09c737fb3521f8dd1ec62a05f0bf8..f7306ff266717b9c8ac2d17e1d5c2fb00b5712fb 100644 (file)
--- a/arch/arm/include/asm/arch-fsl-layerscape/mp.h
+++ b/arch/arm/include/asm/arch-fsl-layerscape/mp.h
@@ -34,5 +34,6 @@ void *get_spin_tbl_addr(void);
 phys_addr_t determine_mp_bootpg(void);
 void secondary_boot_func(void);
 int is_core_online(u64 cpu_id);
+u32 cpu_pos_mask(void);
 #endif
 #endif /* _FSL_LAYERSCAPE_MP_H */