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>

1 files changed, 21 insertions, 0 deletions

diff --git a/arch/arm/cpu/armv8/fsl-layerscape/cpu.c b/arch/arm/cpu/armv8/fsl-layerscape/cpu.c
index 11e806ece0..b7a2e0c946 100644
--- 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);