[PATCH 02/12] MIPS: prom: Add helper to parse CPU node in dt

[Jiaxun Yang]

Mostly identical with arm one. The only difference is that we allow
to mark a CPU Node as status = "disabled" in dt, which means the core
is physicaly present, but not possible for the kernel. It will occupy
a bit in cpumask as well.

Signed-off-by: Jiaxun Yang <jiaxun.yang@xxxxxxxxxxx>
---
 arch/mips/include/asm/prom.h |  2 +
 arch/mips/kernel/prom.c      | 96 ++++++++++++++++++++++++++++++++++++
 2 files changed, 98 insertions(+)

diff --git a/arch/mips/include/asm/prom.h b/arch/mips/include/asm/prom.h
index c42e07671934..1ec46e09dc8b 100644
--- a/arch/mips/include/asm/prom.h
+++ b/arch/mips/include/asm/prom.h
@@ -19,9 +19,11 @@ struct boot_param_header;
 
 extern void __dt_setup_arch(void *bph);
 extern int __dt_register_buses(const char *bus0, const char *bus1);
+extern void mips_dt_init_cpu_maps(void);
 
 #else /* CONFIG_OF */
 static inline void device_tree_init(void) { }
+static inline void mips_dt_init_cpu_maps(void) { }
 #endif /* CONFIG_OF */
 
 extern char *mips_get_machine_name(void);
diff --git a/arch/mips/kernel/prom.c b/arch/mips/kernel/prom.c
index 9e50dc8df2f6..ebeb22ffa76a 100644
--- a/arch/mips/kernel/prom.c
+++ b/arch/mips/kernel/prom.c
@@ -89,4 +89,100 @@ int __init __dt_register_buses(const char *bus0, const char *bus1)
 	return 0;
 }
 
+void __init mips_dt_init_cpu_maps(void)
+{
+	struct device_node *cpu, *cpus;
+	u32 i, j, cpuidx = 1;
+	u32 cpunum;
+	u32 tmp_map[NR_CPUS] = { [0 ... NR_CPUS-1] = U32_MAX };
+	bool cpu_possible[NR_CPUS] = { [0 ... NR_CPUS-1] = false };
+	bool bootcpu_valid = false;
+
+	cpus = of_find_node_by_path("/cpus");
+	if (!cpus)
+		return;
+
+	if (cpu_has_mips_r2_r6)
+		cpunum = get_ebase_cpunum();
+	else
+		cpunum = 0; /* For legacy system we assume boot from CPU 0 */
+
+	for_each_of_cpu_node(cpu) {
+		u32 hwid;
+
+		pr_debug(" * %pOF...\n", cpu);
+		/*
+		 * A device tree containing CPU nodes with missing "reg"
+		 * properties is considered invalid to build the
+		 * cpu_logical_map.
+		 */
+
+		if (of_property_read_u32(cpu, "reg", &hwid)) {
+			pr_debug(" * %pOF missing reg property\n", cpu);
+			of_node_put(cpu);
+			return;
+		}
+
+		/*
+		 * Duplicate hwid are a recipe for disaster.
+		 * Scan all initialized entries and check for
+		 * duplicates. If any is found just bail out.
+		 */
+		for (j = 0; j < cpuidx; j++)
+			if (WARN(tmp_map[j] == hwid,
+				 "Duplicate /cpu reg properties in the DT\n")) {
+				of_node_put(cpu);
+				return;
+			}
+
+		/*
+		 * Build a stashed array of hwid values. Numbering scheme
+		 * requires that if detected the boot CPU must be assigned
+		 * logical id 0. Other CPUs get sequential indexes starting
+		 * from 1. If a CPU node with a reg property matching the
+		 * boot CPU hwid is detected, this is recorded so that the
+		 * logical map built from DT is validated.
+		 */
+		if (hwid == cpunum) {
+			i = 0;
+			if (of_device_is_available(cpu))
+				bootcpu_valid = true;
+		} else {
+			i = cpuidx++;
+		}
+
+		if (WARN(cpuidx > nr_cpu_ids, "DT /cpu %u nodes greater than "
+						   "max cores %u, capping them\n",
+						   cpuidx, nr_cpu_ids)) {
+			cpuidx = nr_cpu_ids;
+			of_node_put(cpu);
+			break;
+		}
+
+		tmp_map[i] = hwid;
+
+		if (of_device_is_available(cpu))
+			cpu_possible[i] = true;
+	}
+
+	if (!bootcpu_valid) {
+		pr_warn("DT missing boot CPU, fall back to default cpu_logical_map\n");
+		return;
+	}
+
+	init_cpu_possible(cpu_none_mask);
+	init_cpu_present(cpu_none_mask);
+
+	for (i = 0; i < cpuidx; i++) {
+		set_cpu_possible(i, cpu_possible[i]);
+		cpu_logical_map(i) = tmp_map[i];
+		pr_debug("cpu logical map 0x%x\n", cpu_logical_map(i));
+	}
+}
+
+bool arch_match_cpu_phys_id(int cpu, u64 phys_id)
+{
+	return phys_id == cpu_logical_map(cpu);
+}
+
 #endif
-- 
2.26.0.rc2