[PATCH 3/3] PowerPC/mpc85xx: Add hotplug support on E6500 cores

[<b29983@freescale.com>]

From: Tang Yuantian <Yuantian.Tang@freescale.com>

Freescale E6500 core-based platforms, like t4240, support
disabling/enabling CPU dynamically.
This patch adds this feature on those platforms.

Signed-off-by: Chenhui Zhao <chenhui.zhao@freescale.com>
Signed-off-by: Tang Yuantian <Yuantian.Tang@feescale.com>
---
 arch/powerpc/include/asm/smp.h    |  1 +
 arch/powerpc/kernel/head_64.S     | 10 +++++--
 arch/powerpc/platforms/85xx/smp.c | 63 ++++++++++++++++++++++++++-------------
 3 files changed, 52 insertions(+), 22 deletions(-)

diff --git a/arch/powerpc/include/asm/smp.h b/arch/powerpc/include/asm/smp.h
index bf37d17..c7bd27d 100644
--- a/arch/powerpc/include/asm/smp.h
+++ b/arch/powerpc/include/asm/smp.h
@@ -198,6 +198,7 @@ extern void generic_secondary_thread_init(void);
 extern unsigned long __secondary_hold_spinloop;
 extern unsigned long __secondary_hold_acknowledge;
 extern char __secondary_hold;
+extern unsigned int booting_cpu_hwid;
 
 extern void __early_start(void);
 #endif /* __ASSEMBLY__ */
diff --git a/arch/powerpc/kernel/head_64.S b/arch/powerpc/kernel/head_64.S
index d48125d..c47544d 100644
--- a/arch/powerpc/kernel/head_64.S
+++ b/arch/powerpc/kernel/head_64.S
@@ -181,6 +181,10 @@ exception_marker:
 #endif
 
 #ifdef CONFIG_PPC_BOOK3E
+	.globl	booting_cpu_hwid
+booting_cpu_hwid:
+	.long  0x0
+	.align 3
 _GLOBAL(fsl_secondary_thread_init)
 	/* Enable branch prediction */
 	lis     r3,BUCSR_INIT@h
@@ -197,8 +201,10 @@ _GLOBAL(fsl_secondary_thread_init)
 	 * but the low bit right by two bits so that the cpu numbering is
 	 * continuous.
 	 */
-	mfspr	r3, SPRN_PIR
-	rlwimi	r3, r3, 30, 2, 30
+	bl	10f
+10:	mflr	r22
+	addi	r22,r22,(booting_cpu_hwid - 10b)
+	lwz	r3,0(r22)
 	mtspr	SPRN_PIR, r3
 #endif
 
diff --git a/arch/powerpc/platforms/85xx/smp.c b/arch/powerpc/platforms/85xx/smp.c
index 7f0dadb..8652a49 100644
--- a/arch/powerpc/platforms/85xx/smp.c
+++ b/arch/powerpc/platforms/85xx/smp.c
@@ -189,15 +189,22 @@ static inline u32 read_spin_table_addr_l(void *spin_table)
 static void wake_hw_thread(void *info)
 {
 	void fsl_secondary_thread_init(void);
-	unsigned long imsr1, inia1;
+	unsigned long imsr, inia;
 	int nr = *(const int *)info;
-
-	imsr1 = MSR_KERNEL;
-	inia1 = *(unsigned long *)fsl_secondary_thread_init;
-
-	mttmr(TMRN_IMSR1, imsr1);
-	mttmr(TMRN_INIA1, inia1);
-	mtspr(SPRN_TENS, TEN_THREAD(1));
+	int hw_cpu = get_hard_smp_processor_id(nr);
+	int thread_idx = cpu_thread_in_core(hw_cpu);
+
+	booting_cpu_hwid = (u32)hw_cpu;
+	imsr = MSR_KERNEL;
+	inia = *(unsigned long *)fsl_secondary_thread_init;
+	if (thread_idx == 0) {
+		mttmr(TMRN_IMSR0, imsr);
+		mttmr(TMRN_INIA0, inia);
+	} else {
+		mttmr(TMRN_IMSR1, imsr);
+		mttmr(TMRN_INIA1, inia);
+	}
+	mtspr(SPRN_TENS, TEN_THREAD(thread_idx));
 
 	smp_generic_kick_cpu(nr);
 }
@@ -219,27 +226,43 @@ static int smp_85xx_kick_cpu(int nr)
 	pr_debug("smp_85xx_kick_cpu: kick CPU #%d\n", nr);
 
 #ifdef CONFIG_PPC64
-	/* Threads don't use the spin table */
-	if (cpu_thread_in_core(nr) != 0) {
+	if (threads_per_core > 1) {
 		int primary = cpu_first_thread_sibling(nr);
 
 		if (WARN_ON_ONCE(!cpu_has_feature(CPU_FTR_SMT)))
 			return -ENOENT;
 
-		if (cpu_thread_in_core(nr) != 1) {
-			pr_err("%s: cpu %d: invalid hw thread %d\n",
-			       __func__, nr, cpu_thread_in_core(nr));
-			return -ENOENT;
+		/*
+		 * If either one of threads in the same core is online,
+		 * use the online one to start the other.
+		 */
+		if (cpu_online(primary) || cpu_online(primary + 1)) {
+			qoriq_pm_ops->cpu_up(nr);
+			if (cpu_online(primary))
+				smp_call_function_single(primary,
+						wake_hw_thread, &nr, 1);
+			else
+				smp_call_function_single(primary + 1,
+						wake_hw_thread, &nr, 1);
+			return 0;
 		}
 
-		if (!cpu_online(primary)) {
-			pr_err("%s: cpu %d: primary %d not online\n",
-			       __func__, nr, primary);
-			return -ENOENT;
+		/*
+		 * If both threads are offline, reset core to start.
+		 * When core is up, Thread 0 always gets up first,
+		 * so bind the current logical cpu with Thread 0.
+		 */
+		if (hw_cpu != cpu_first_thread_sibling(hw_cpu)) {
+			int hw_cpu1, hw_cpu2;
+
+			hw_cpu1 = get_hard_smp_processor_id(primary);
+			hw_cpu2 = get_hard_smp_processor_id(primary + 1);
+			set_hard_smp_processor_id(primary, hw_cpu2);
+			set_hard_smp_processor_id(primary + 1, hw_cpu1);
+			/* get new physical cpu id */
+			hw_cpu = get_hard_smp_processor_id(nr);
 		}
 
-		smp_call_function_single(primary, wake_hw_thread, &nr, 0);
-		return 0;
 	}
 #endif
 
-- 
2.1.0.27.g96db324