[PATCH 5/7] MIPS: smp: Process NUMA information

[Jiaxun Yang <jiaxun.yang@flygoat.com>]

Performing numa_add_cpu and numa_remove_cpu as necessary, store
NUMA map from devicetree and fix up if platform SMP code didn't
do mapping properly.

Signed-off-by: Jiaxun Yang <jiaxun.yang@flygoat.com>
---
 arch/mips/include/asm/smp-ops.h |  7 +------
 arch/mips/kernel/smp-cps.c      |  6 ++++++
 arch/mips/kernel/smp.c          | 26 ++++++++++++++++++++++++++
 3 files changed, 33 insertions(+), 6 deletions(-)

diff --git a/arch/mips/include/asm/smp-ops.h b/arch/mips/include/asm/smp-ops.h
index 1617b207723f..4d984f0088cb 100644
--- a/arch/mips/include/asm/smp-ops.h
+++ b/arch/mips/include/asm/smp-ops.h
@@ -40,12 +40,7 @@ struct plat_smp_ops {
 
 extern void register_smp_ops(const struct plat_smp_ops *ops);
 
-static inline void plat_smp_setup(void)
-{
-	extern const struct plat_smp_ops *mp_ops;	/* private */
-
-	mp_ops->smp_setup();
-}
+extern void __init plat_smp_setup(void);
 
 extern void mips_smp_send_ipi_single(int cpu, unsigned int action);
 extern void mips_smp_send_ipi_mask(const struct cpumask *mask,
diff --git a/arch/mips/kernel/smp-cps.c b/arch/mips/kernel/smp-cps.c
index 395622c37325..be9f13c0b058 100644
--- a/arch/mips/kernel/smp-cps.c
+++ b/arch/mips/kernel/smp-cps.c
@@ -8,6 +8,7 @@
 #include <linux/delay.h>
 #include <linux/io.h>
 #include <linux/memblock.h>
+#include <linux/of.h>
 #include <linux/sched/task_stack.h>
 #include <linux/sched/hotplug.h>
 #include <linux/slab.h>
@@ -179,10 +180,14 @@ static void __init cps_smp_setup(void)
 
 	/* Indicate present CPUs (CPU being synonymous with VPE) */
 	for (v = 0; v < min_t(unsigned, nvpes, NR_CPUS); v++) {
+		struct device_node *np = of_get_cpu_node(v, NULL);
+
 		set_cpu_possible(v, cpu_cluster(&cpu_data[v]) == 0);
 		set_cpu_present(v, cpu_cluster(&cpu_data[v]) == 0);
 		__cpu_number_map[v] = v;
 		__cpu_logical_map[v] = v;
+		if (np)
+			early_map_cpu_to_node(v, of_node_to_nid(np));
 	}
 
 	/* Set a coherent default CCA (CWB) */
@@ -553,6 +558,7 @@ static int cps_cpu_disable(void)
 	atomic_sub(1 << cpu_vpe_id(&current_cpu_data), &core_cfg->vpe_mask);
 	smp_mb__after_atomic();
 	set_cpu_online(cpu, false);
+	numa_remove_cpu(cpu);
 	calculate_cpu_foreign_map();
 	irq_migrate_all_off_this_cpu();
 
diff --git a/arch/mips/kernel/smp.c b/arch/mips/kernel/smp.c
index 0362fc5df7b0..17dd97ad3cbb 100644
--- a/arch/mips/kernel/smp.c
+++ b/arch/mips/kernel/smp.c
@@ -153,6 +153,11 @@ void calculate_cpu_foreign_map(void)
 			       &temp_foreign_map, &cpu_sibling_map[i]);
 }
 
+bool arch_match_cpu_phys_id(int cpu, u64 phys_id)
+{
+	return phys_id == cpu_logical_map(cpu);
+}
+
 const struct plat_smp_ops *mp_ops;
 EXPORT_SYMBOL(mp_ops);
 
@@ -164,6 +169,19 @@ void register_smp_ops(const struct plat_smp_ops *ops)
 	mp_ops = ops;
 }
 
+void __init plat_smp_setup(void)
+{
+	unsigned int cpu;
+
+	mp_ops->smp_setup();
+
+	for_each_possible_cpu(cpu) {
+		/* Workaround platform SMP code not handling NUMA map */
+		if (early_cpu_to_node(cpu) == NUMA_NO_NODE)
+			early_map_cpu_to_node(cpu, 0);
+	}
+}
+
 #ifdef CONFIG_GENERIC_IRQ_IPI
 void mips_smp_send_ipi_single(int cpu, unsigned int action)
 {
@@ -372,6 +390,7 @@ asmlinkage void start_secondary(void)
 
 	set_cpu_sibling_map(cpu);
 	set_cpu_core_map(cpu);
+	numa_add_cpu(cpu);
 
 	cpumask_set_cpu(cpu, &cpu_coherent_mask);
 	notify_cpu_starting(cpu);
@@ -426,6 +445,8 @@ void __init smp_cpus_done(unsigned int max_cpus)
 /* called from main before smp_init() */
 void __init smp_prepare_cpus(unsigned int max_cpus)
 {
+	unsigned int cpu;
+
 	init_new_context(current, &init_mm);
 	current_thread_info()->cpu = 0;
 	mp_ops->prepare_cpus(max_cpus);
@@ -436,6 +457,11 @@ void __init smp_prepare_cpus(unsigned int max_cpus)
 	init_cpu_present(cpu_possible_mask);
 #endif
 	cpumask_copy(&cpu_coherent_mask, cpu_possible_mask);
+
+	for_each_possible_cpu(cpu)
+		numa_store_cpu_info(cpu);
+
+	numa_add_cpu(0);
 }
 
 /* preload SMP state for boot cpu */

-- 
2.46.0