3 files changed, 34 insertions, 4 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);
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/mp.c b/arch/arm/cpu/armv8/fsl-layerscape/mp.c
index df7ffb88f6..f607c3900a 100644
--- 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 e46e076f16..f7306ff266 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 @@ 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 */