2 files changed, 20 insertions, 32 deletions

diff --git a/arch/mips/kernel/mips-cm.c b/arch/mips/kernel/mips-cm.c
index 96c607dbb3a0..7c9c5dc38823 100644
--- a/arch/mips/kernel/mips-cm.c
+++ b/arch/mips/kernel/mips-cm.c
@@ -10,6 +10,7 @@
 #include <linux/spinlock.h>
 
 #include <asm/mips-cps.h>
+#include <asm/smp-cps.h>
 #include <asm/mipsregs.h>
 
 void __iomem *mips_gcr_base;
@@ -534,39 +535,23 @@ void mips_cm_error_report(void)
 	write_gcr_error_cause(cm_error);
 }
 
-unsigned int mips_cps_first_online_in_cluster(void)
+unsigned int mips_cps_first_online_in_cluster(int *first_cpu)
 {
-	unsigned int local_cl;
-	int i;
-
-	local_cl = cpu_cluster(&current_cpu_data);
+	unsigned int local_cl = cpu_cluster(&current_cpu_data);
+	struct cpumask *local_cl_mask;
 
 	/*
-	 * We rely upon knowledge that CPUs are numbered sequentially by
-	 * cluster - ie. CPUs 0..X will be in cluster 0, CPUs X+1..Y in cluster
-	 * 1, CPUs Y+1..Z in cluster 2 etc. This means that CPUs in the same
-	 * cluster will immediately precede or follow one another.
-	 *
-	 * First we scan backwards, until we find an online CPU in the cluster
-	 * or we move on to another cluster.
+	 * mips_cps_cluster_bootcfg is allocated in cps_prepare_cpus. If it is
+	 * not yet done, then we are so early that only one CPU is running, so
+	 * it is the first online CPU in the cluster.
 	 */
-	for (i = smp_processor_id() - 1; i >= 0; i--) {
-		if (cpu_cluster(&cpu_data[i]) != local_cl)
-			break;
-		if (!cpu_online(i))
-			continue;
-		return false;
-	}
-
-	/* Then do the same for higher numbered CPUs */
-	for (i = smp_processor_id() + 1; i < nr_cpu_ids; i++) {
-		if (cpu_cluster(&cpu_data[i]) != local_cl)
-			break;
-		if (!cpu_online(i))
-			continue;
-		return false;
-	}
-
-	/* We found no online CPUs in the local cluster */
-	return true;
+	if  (IS_ENABLED(CONFIG_MIPS_CPS) && mips_cps_cluster_bootcfg)
+		local_cl_mask = &mips_cps_cluster_bootcfg[local_cl].cpumask;
+	else
+		return true;
+
+	*first_cpu = cpumask_any_and_but(local_cl_mask,
+					 cpu_online_mask,
+					 smp_processor_id());
+	return (*first_cpu >= nr_cpu_ids);
 }
diff --git a/arch/mips/kernel/smp-cps.c b/arch/mips/kernel/smp-cps.c
index 7b0e69af4097..6c5f15293a8e 100644
--- a/arch/mips/kernel/smp-cps.c
+++ b/arch/mips/kernel/smp-cps.c
@@ -283,7 +283,7 @@ static void __init cps_smp_setup(void)
 
 static void __init cps_prepare_cpus(unsigned int max_cpus)
 {
-	unsigned int nclusters, ncores, core_vpes, c, cl, cca;
+	unsigned int nclusters, ncores, core_vpes, nvpe = 0, c, cl, cca;
 	bool cca_unsuitable, cores_limited;
 	struct cluster_boot_config *cluster_bootcfg;
 	struct core_boot_config *core_bootcfg;
@@ -356,10 +356,13 @@ static void __init cps_prepare_cpus(unsigned int max_cpus)
 
 		/* Allocate VPE boot configuration structs */
 		for (c = 0; c < ncores; c++) {
+			int v;
 			core_vpes = core_vpe_count(cl, c);
 			core_bootcfg[c].vpe_config = kcalloc(core_vpes,
 					sizeof(*core_bootcfg[c].vpe_config),
 					GFP_KERNEL);
+			for (v = 0; v < core_vpes; v++)
+				cpumask_set_cpu(nvpe++, &mips_cps_cluster_bootcfg[cl].cpumask);
 			if (!core_bootcfg[c].vpe_config)
 				goto err_out;
 		}