parisc: Add CPU topology support

Add topology support, including multi-core scheduler support on
PA8800/PA8900 CPUs and enhanced output in /proc/cpuinfo, e.g.
lscpu now reports on a single-socket, dual-core machine:

Architecture:          parisc64
CPU(s):                2
On-line CPU(s) list:   0,1
Thread(s) per core:    1
Core(s) per socket:    2
Socket(s):             1
CPU family:            PA-RISC 2.0
Model name:            PA8800 (Mako)

Signed-off-by: Helge Deller <deller@gmx.de>
diff --git a/arch/parisc/Kconfig b/arch/parisc/Kconfig
index 1fd3eb5..7171c21 100644
--- a/arch/parisc/Kconfig
+++ b/arch/parisc/Kconfig
@@ -32,6 +32,7 @@
 	select GENERIC_PCI_IOMAP
 	select ARCH_HAVE_NMI_SAFE_CMPXCHG
 	select GENERIC_SMP_IDLE_THREAD
+	select GENERIC_CPU_DEVICES
 	select GENERIC_STRNCPY_FROM_USER
 	select SYSCTL_ARCH_UNALIGN_ALLOW
 	select SYSCTL_EXCEPTION_TRACE
@@ -288,6 +289,21 @@
 
 	  If you don't know what to do here, say N.
 
+config PARISC_CPU_TOPOLOGY
+	bool "Support cpu topology definition"
+	depends on SMP
+	default y
+	help
+	  Support PARISC cpu topology definition.
+
+config SCHED_MC
+	bool "Multi-core scheduler support"
+	depends on PARISC_CPU_TOPOLOGY && PA8X00
+	help
+	  Multi-core scheduler support improves the CPU scheduler's decision
+	  making when dealing with multi-core CPU chips at a cost of slightly
+	  increased overhead in some places. If unsure say N here.
+
 config IRQSTACKS
 	bool "Use separate kernel stacks when processing interrupts"
 	default y
diff --git a/arch/parisc/include/asm/topology.h b/arch/parisc/include/asm/topology.h
new file mode 100644
index 0000000..6f0750c
--- /dev/null
+++ b/arch/parisc/include/asm/topology.h
@@ -0,0 +1,36 @@
+#ifndef _ASM_PARISC_TOPOLOGY_H
+#define _ASM_PARISC_TOPOLOGY_H
+
+#ifdef CONFIG_PARISC_CPU_TOPOLOGY
+
+#include <linux/cpumask.h>
+
+struct cputopo_parisc {
+	int thread_id;
+	int core_id;
+	int socket_id;
+	cpumask_t thread_sibling;
+	cpumask_t core_sibling;
+};
+
+extern struct cputopo_parisc cpu_topology[NR_CPUS];
+
+#define topology_physical_package_id(cpu)	(cpu_topology[cpu].socket_id)
+#define topology_core_id(cpu)		(cpu_topology[cpu].core_id)
+#define topology_core_cpumask(cpu)	(&cpu_topology[cpu].core_sibling)
+#define topology_sibling_cpumask(cpu)	(&cpu_topology[cpu].thread_sibling)
+
+void init_cpu_topology(void);
+void store_cpu_topology(unsigned int cpuid);
+const struct cpumask *cpu_coregroup_mask(int cpu);
+
+#else
+
+static inline void init_cpu_topology(void) { }
+static inline void store_cpu_topology(unsigned int cpuid) { }
+
+#endif
+
+#include <asm-generic/topology.h>
+
+#endif /* _ASM_ARM_TOPOLOGY_H */
diff --git a/arch/parisc/kernel/Makefile b/arch/parisc/kernel/Makefile
index 649dc3e..eafd06a 100644
--- a/arch/parisc/kernel/Makefile
+++ b/arch/parisc/kernel/Makefile
@@ -9,8 +9,7 @@
 		   pa7300lc.o syscall.o entry.o sys_parisc.o firmware.o \
 		   ptrace.o hardware.o inventory.o drivers.o \
 		   signal.o hpmc.o real2.o parisc_ksyms.o unaligned.o \
-		   process.o processor.o pdc_cons.o pdc_chassis.o unwind.o \
-		   topology.o
+		   process.o processor.o pdc_cons.o pdc_chassis.o unwind.o
 
 ifdef CONFIG_FUNCTION_TRACER
 # Do not profile debug and lowlevel utilities
@@ -30,5 +29,6 @@
 obj64-$(CONFIG_AUDIT)	+= compat_audit.o
 # only supported for PCX-W/U in 64-bit mode at the moment
 obj-$(CONFIG_64BIT)	+= perf.o perf_asm.o $(obj64-y)
+obj-$(CONFIG_PARISC_CPU_TOPOLOGY)	+= topology.o
 obj-$(CONFIG_FUNCTION_TRACER)		+= ftrace.o
 obj-$(CONFIG_FUNCTION_GRAPH_TRACER)	+= ftrace.o
diff --git a/arch/parisc/kernel/processor.c b/arch/parisc/kernel/processor.c
index e120d63..45cc659 100644
--- a/arch/parisc/kernel/processor.c
+++ b/arch/parisc/kernel/processor.c
@@ -184,6 +184,9 @@ static int __init processor_probe(struct parisc_device *dev)
 	p->txn_addr = txn_addr;	/* save CPU IRQ address */
 	p->cpu_num = cpu_info.cpu_num;
 	p->cpu_loc = cpu_info.cpu_loc;
+
+	store_cpu_topology(cpuid);
+
 #ifdef CONFIG_SMP
 	/*
 	** FIXME: review if any other initialization is clobbered
@@ -325,6 +328,8 @@ int __init init_per_cpu(int cpunum)
 	set_firmware_width();
 	ret = pdc_coproc_cfg(&coproc_cfg);
 
+	store_cpu_topology(cpunum);
+
 	if(ret >= 0 && coproc_cfg.ccr_functional) {
 		mtctl(coproc_cfg.ccr_functional, 10);  /* 10 == Coprocessor Control Reg */
 
@@ -388,6 +393,14 @@ show_cpuinfo (struct seq_file *m, void *v)
 				 boot_cpu_data.cpu_hz / 1000000,
 				 boot_cpu_data.cpu_hz % 1000000  );
 
+#ifdef CONFIG_PARISC_CPU_TOPOLOGY
+		seq_printf(m, "physical id\t: %d\n",
+				topology_physical_package_id(cpu));
+		seq_printf(m, "siblings\t: %d\n",
+				cpumask_weight(topology_core_cpumask(cpu)));
+		seq_printf(m, "core id\t\t: %d\n", topology_core_id(cpu));
+#endif
+
 		seq_printf(m, "capabilities\t:");
 		if (boot_cpu_data.pdc.capabilities & PDC_MODEL_OS32)
 			seq_puts(m, " os32");
diff --git a/arch/parisc/kernel/setup.c b/arch/parisc/kernel/setup.c
index f7d0c3b3..0e9675f 100644
--- a/arch/parisc/kernel/setup.c
+++ b/arch/parisc/kernel/setup.c
@@ -408,6 +408,8 @@ void __init start_parisc(void)
 
 	cpunum = smp_processor_id();
 
+	init_cpu_topology();
+
 	set_firmware_width_unlocked();
 
 	ret = pdc_coproc_cfg_unlocked(&coproc_cfg);
diff --git a/arch/parisc/kernel/topology.c b/arch/parisc/kernel/topology.c
index f515938..0a10e4d 100644
--- a/arch/parisc/kernel/topology.c
+++ b/arch/parisc/kernel/topology.c
@@ -1,37 +1,142 @@
 /*
- * arch/parisc/kernel/topology.c - Populate sysfs with topology information
+ * arch/parisc/kernel/topology.c
  *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
+ * Copyright (C) 2017 Helge Deller <deller@gmx.de>
  *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
- * NON INFRINGEMENT.  See the GNU General Public License for more
- * details.
+ * based on arch/arm/kernel/topology.c
  *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License.  See the file "COPYING" in the main directory of this archive
+ * for more details.
  */
 
-#include <linux/init.h>
-#include <linux/smp.h>
-#include <linux/cpu.h>
-#include <linux/cache.h>
+#include <linux/percpu.h>
+#include <linux/sched.h>
+#include <linux/sched/topology.h>
 
-static DEFINE_PER_CPU(struct cpu, cpu_devices);
+#include <asm/topology.h>
 
-static int __init topology_init(void)
+ /*
+  * cpu topology table
+  */
+struct cputopo_parisc cpu_topology[NR_CPUS] __read_mostly;
+EXPORT_SYMBOL_GPL(cpu_topology);
+
+const struct cpumask *cpu_coregroup_mask(int cpu)
 {
-	int num;
-
-	for_each_present_cpu(num) {
-		register_cpu(&per_cpu(cpu_devices, num), num);
-	}
-	return 0;
+	return &cpu_topology[cpu].core_sibling;
 }
 
-subsys_initcall(topology_init);
+static void update_siblings_masks(unsigned int cpuid)
+{
+	struct cputopo_parisc *cpu_topo, *cpuid_topo = &cpu_topology[cpuid];
+	int cpu;
+
+	/* update core and thread sibling masks */
+	for_each_possible_cpu(cpu) {
+		cpu_topo = &cpu_topology[cpu];
+
+		if (cpuid_topo->socket_id != cpu_topo->socket_id)
+			continue;
+
+		cpumask_set_cpu(cpuid, &cpu_topo->core_sibling);
+		if (cpu != cpuid)
+			cpumask_set_cpu(cpu, &cpuid_topo->core_sibling);
+
+		if (cpuid_topo->core_id != cpu_topo->core_id)
+			continue;
+
+		cpumask_set_cpu(cpuid, &cpu_topo->thread_sibling);
+		if (cpu != cpuid)
+			cpumask_set_cpu(cpu, &cpuid_topo->thread_sibling);
+	}
+	smp_wmb();
+}
+
+static int dualcores_found __initdata;
+
+/*
+ * store_cpu_topology is called at boot when only one cpu is running
+ * and with the mutex cpu_hotplug.lock locked, when several cpus have booted,
+ * which prevents simultaneous write access to cpu_topology array
+ */
+void __init store_cpu_topology(unsigned int cpuid)
+{
+	struct cputopo_parisc *cpuid_topo = &cpu_topology[cpuid];
+	struct cpuinfo_parisc *p;
+	int max_socket = -1;
+	unsigned long cpu;
+
+	/* If the cpu topology has been already set, just return */
+	if (cpuid_topo->core_id != -1)
+		return;
+
+	/* create cpu topology mapping */
+	cpuid_topo->thread_id = -1;
+	cpuid_topo->core_id = 0;
+
+	p = &per_cpu(cpu_data, cpuid);
+	for_each_online_cpu(cpu) {
+		const struct cpuinfo_parisc *cpuinfo = &per_cpu(cpu_data, cpu);
+
+		if (cpu == cpuid) /* ignore current cpu */
+			continue;
+
+		if (cpuinfo->cpu_loc == p->cpu_loc) {
+			cpuid_topo->core_id = cpu_topology[cpu].core_id;
+			if (p->cpu_loc) {
+				cpuid_topo->core_id++;
+				cpuid_topo->socket_id = cpu_topology[cpu].socket_id;
+				dualcores_found = 1;
+				continue;
+			}
+		}
+
+		if (cpuid_topo->socket_id == -1)
+			max_socket = max(max_socket, cpu_topology[cpu].socket_id);
+	}
+
+	if (cpuid_topo->socket_id == -1)
+		cpuid_topo->socket_id = max_socket + 1;
+
+	update_siblings_masks(cpuid);
+
+	pr_info("CPU%u: thread %d, cpu %d, socket %d\n",
+		cpuid, cpu_topology[cpuid].thread_id,
+		cpu_topology[cpuid].core_id,
+		cpu_topology[cpuid].socket_id);
+}
+
+static struct sched_domain_topology_level parisc_mc_topology[] = {
+#ifdef CONFIG_SCHED_MC
+	{ cpu_coregroup_mask, cpu_core_flags, SD_INIT_NAME(MC) },
+#endif
+
+	{ cpu_cpu_mask, SD_INIT_NAME(DIE) },
+	{ NULL, },
+};
+
+/*
+ * init_cpu_topology is called at boot when only one cpu is running
+ * which prevent simultaneous write access to cpu_topology array
+ */
+void __init init_cpu_topology(void)
+{
+	unsigned int cpu;
+
+	/* init core mask and capacity */
+	for_each_possible_cpu(cpu) {
+		struct cputopo_parisc *cpu_topo = &(cpu_topology[cpu]);
+
+		cpu_topo->thread_id = -1;
+		cpu_topo->core_id =  -1;
+		cpu_topo->socket_id = -1;
+		cpumask_clear(&cpu_topo->core_sibling);
+		cpumask_clear(&cpu_topo->thread_sibling);
+	}
+	smp_wmb();
+
+	/* Set scheduler topology descriptor */
+	if (dualcores_found)
+		set_sched_topology(parisc_mc_topology);
+}