[PATCH 1/3] MIPS: Loongson64: Add Loongson-2K1000 SMP support

[Qing Zhang <zhangqing@loongson.cn>]

Add SMP part of dt parsing about Loongson-2K1000 to multiplex
loongson3_smp_ops, CONFIG_NUMA is not currently supported,
so of_numa is not used to parse relevant information.

Signed-off-by: Qing Zhang <zhangqing@loongson.cn>
---
 arch/mips/loongson64/env.c |   3 +
 arch/mips/loongson64/smp.c | 130 +++++++++++++++++++++++++++++++++++--
 2 files changed, 129 insertions(+), 4 deletions(-)

diff --git a/arch/mips/loongson64/env.c b/arch/mips/loongson64/env.c
index c961e2999f15..90552f6b00ca 100644
--- a/arch/mips/loongson64/env.c
+++ b/arch/mips/loongson64/env.c
@@ -52,6 +52,9 @@ void __init prom_dtb_init_env(void)
 		loongson_fdt_blob = __dtb_loongson64_2core_2k1000_begin;
 	else
 		loongson_fdt_blob = (void *)fw_arg2;
+
+	loongson_sysconf.dma_mask_bits = 32;
+	loongson_sysconf.workarounds = WORKAROUND_CPUHOTPLUG;
 }
 
 void __init prom_lefi_init_env(void)
diff --git a/arch/mips/loongson64/smp.c b/arch/mips/loongson64/smp.c
index 660e1de4412a..a7d2ad87bae0 100644
--- a/arch/mips/loongson64/smp.c
+++ b/arch/mips/loongson64/smp.c
@@ -13,6 +13,7 @@
 #include <linux/smp.h>
 #include <linux/cpufreq.h>
 #include <linux/kexec.h>
+#include <linux/of_address.h>
 #include <asm/processor.h>
 #include <asm/time.h>
 #include <asm/tlbflush.h>
@@ -476,10 +477,59 @@ static void loongson3_smp_finish(void)
 			smp_processor_id(), read_c0_status());
 }
 
+#define INVALID_HWID	ULONG_MAX
+static void parse_dt_cpus_init(void)
+{
+	struct device_node *dn, *np;
+	int i, hwids[NR_CPUS];
+	u64 hwid;
+
+	for_each_of_cpu_node(dn) {
+		hwid = of_get_cpu_hwid(dn, 0);
+		if (hwid >= INVALID_HWID)
+			continue;
+
+		for (i = 0; i < loongson_sysconf.nr_cpus; i++) {
+			if (hwids[i] == hwid) {
+				pr_err("%pOF: duplicate cpu reg properties in the DT\n", dn);
+				continue;
+			}
+		}
+
+		if (loongson_sysconf.nr_cpus >= NR_CPUS)
+			break;
+
+		hwids[loongson_sysconf.nr_cpus] = hwid;
+		loongson_sysconf.nr_cpus++;
+
+		np = of_find_compatible_node(NULL, NULL, "loongson, mmio-ipi");
+		if (!np) {
+			pr_info("Failed to get ipi node\n");
+			return;
+		}
+
+		smp_group[0] = (unsigned long long)of_iomap(np, 0);
+		if (!smp_group[0]) {
+			pr_info("Failed to map ipi register base address\n");
+			return;
+		}
+	}
+
+	of_node_put(np);
+	loongson_sysconf.reserved_cpus_mask = 0;
+	loongson_sysconf.boot_cpu_id = 0;
+	loongson_sysconf.nr_nodes = 1;
+	loongson_sysconf.cores_per_node = loongson_sysconf.nr_cpus;
+	loongson_sysconf.cores_per_package = loongson_sysconf.nr_cpus;
+}
+
 static void __init loongson3_smp_setup(void)
 {
 	int i = 0, num = 0; /* i: physical id, num: logical id */
 
+	if (loongson_sysconf.fw_interface == LOONGSON_DTB)
+		parse_dt_cpus_init();
+
 	init_cpu_possible(cpu_none_mask);
 
 	/* For unified kernel, NR_CPUS is the maximum possible value,
@@ -781,6 +831,68 @@ static void loongson3_type3_play_dead(int *state_addr)
 		: "a1");
 }
 
+static void loongson3_type4_play_dead(int *state_addr)
+{
+	register int val;
+	register long cpuid, core, node, count;
+	register void *addr, *base, *initfunc;
+
+	__asm__ __volatile__(
+		"   .set push                     \n"
+		"   .set noreorder                \n"
+		"   li %[addr], 0x80000000        \n" /* KSEG0 */
+		"1: cache 0, 0(%[addr])           \n" /* flush L1 ICache */
+		"   cache 0, 1(%[addr])           \n"
+		"   cache 0, 2(%[addr])           \n"
+		"   cache 0, 3(%[addr])           \n"
+		"   cache 1, 0(%[addr])           \n" /* flush L1 DCache */
+		"   cache 1, 1(%[addr])           \n"
+		"   cache 1, 2(%[addr])           \n"
+		"   cache 1, 3(%[addr])           \n"
+		"   addiu %[sets], %[sets], -1    \n"
+		"   bnez  %[sets], 1b             \n"
+		"   addiu %[addr], %[addr], 0x40  \n"
+		"   li    %[val], 0x7             \n" /* *state_addr = CPU_DEAD; */
+		"   sw    %[val], (%[state_addr]) \n"
+		"   sync                          \n"
+		"   cache 21, (%[state_addr])     \n" /* flush entry of *state_addr */
+		"   .set pop                      \n"
+		: [addr] "=&r" (addr), [val] "=&r" (val)
+		: [state_addr] "r" (state_addr),
+		  [sets] "r" (cpu_data[smp_processor_id()].dcache.sets));
+
+	__asm__ __volatile__(
+		"   .set push                         \n"
+		"   .set noreorder                    \n"
+		"   .set mips64                       \n"
+		"   mfc0  %[cpuid], $15, 1            \n"
+		"   andi  %[cpuid], 0x3ff             \n"
+		"   dli   %[base], 0x900000001fe11000 \n"
+		"   andi  %[core], %[cpuid], 0x3      \n"
+		"   sll   %[core], 8                  \n" /* get core id */
+		"   or    %[base], %[base], %[core]   \n"
+		"   andi  %[node], %[cpuid], 0xc      \n"
+		"   dsll  %[node], 42                 \n" /* get node id */
+		"   or    %[base], %[base], %[node]   \n"
+		"1: li    %[count], 0x100             \n" /* wait for init loop */
+		"2: bnez  %[count], 2b                \n" /* limit mailbox access */
+		"   addiu %[count], -1                \n"
+		"   ld    %[initfunc], 0x20(%[base])  \n" /* get PC via mailbox */
+		"   beqz  %[initfunc], 1b             \n"
+		"   nop                               \n"
+		"   ld    $sp, 0x28(%[base])          \n" /* get SP via mailbox */
+		"   ld    $gp, 0x30(%[base])          \n" /* get GP via mailbox */
+		"   ld    $a1, 0x38(%[base])          \n"
+		"   jr    %[initfunc]                 \n" /* jump to initial PC */
+		"   nop                               \n"
+		"   .set pop                          \n"
+		: [core] "=&r" (core), [node] "=&r" (node),
+		  [base] "=&r" (base), [cpuid] "=&r" (cpuid),
+		  [count] "=&r" (count), [initfunc] "=&r" (initfunc)
+		: /* No Input */
+		: "a1");
+}
+
 void play_dead(void)
 {
 	int prid_imp, prid_rev, *state_addr;
@@ -816,6 +928,13 @@ void play_dead(void)
 		play_dead_at_ckseg1 =
 			(void *)CKSEG1ADDR((unsigned long)loongson3_type3_play_dead);
 		break;
+	case PRID_REV_LOONGSON2K_R1_0:
+	case PRID_REV_LOONGSON2K_R1_1:
+	case PRID_REV_LOONGSON2K_R1_2:
+	case PRID_REV_LOONGSON2K_R1_3:
+		play_dead_at_ckseg1 =
+			(void *)CKSEG1ADDR((unsigned long)loongson3_type4_play_dead);
+		break;
 	}
 
 out:
@@ -854,10 +973,13 @@ static int loongson3_enable_clock(unsigned int cpu)
 
 static int register_loongson3_notifier(void)
 {
-	return cpuhp_setup_state_nocalls(CPUHP_MIPS_SOC_PREPARE,
-					 "mips/loongson:prepare",
-					 loongson3_enable_clock,
-					 loongson3_disable_clock);
+	if (loongson_sysconf.fw_interface != LOONGSON_DTB)
+		return cpuhp_setup_state_nocalls(CPUHP_MIPS_SOC_PREPARE,
+						 "mips/loongson:prepare",
+						 loongson3_enable_clock,
+						 loongson3_disable_clock);
+	else
+		return 0;
 }
 early_initcall(register_loongson3_notifier);
 
-- 
2.20.1