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 11e806e..b7a2e0c 100644
--- a/arch/arm/cpu/armv8/fsl-layerscape/cpu.c
+++ b/arch/arm/cpu/armv8/fsl-layerscape/cpu.c
@@ -203,6 +203,27 @@
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 df7ffb8..f607c39 100644
--- a/arch/arm/cpu/armv8/fsl-layerscape/mp.c
+++ b/arch/arm/cpu/armv8/fsl-layerscape/mp.c
@@ -104,6 +104,11 @@
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 @@
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 @@
}
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 e46e076..f7306ff 100644
--- a/arch/arm/include/asm/arch-fsl-layerscape/mp.h
+++ b/arch/arm/include/asm/arch-fsl-layerscape/mp.h
@@ -34,5 +34,6 @@
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 */