Fix e6500-based platform CPU hotplug issue

Fix thread 1 of core 0 online issue. Besides, put offline
thread into PW10 state, give it a chance to enter PW20
state when threads of the same core are in PW10 state.

Signed-off-by: Zhang Zhuoyu <Zhuoyu.Zhang@freescale.com>
Change-Id: I9018c4499f02b79f2ec684798c54bf3cfe6723de
Reviewed-on: http://git.am.freescale.net:8181/21206
Tested-by: Review Code-CDREVIEW <CDREVIEW@freescale.com>
Reviewed-by: Yang Li <LeoLi@freescale.com>

6 files changed, 75 insertions, 25 deletions

diff --git a/arch/powerpc/include/asm/fsl_pm.h b/arch/powerpc/include/asm/fsl_pm.h
index 9811cb2..4e6fd0e 100644
--- a/arch/powerpc/include/asm/fsl_pm.h
+++ b/arch/powerpc/include/asm/fsl_pm.h
@@ -16,9 +16,10 @@
 #include <linux/suspend.h>
 
 #define E500_PM_PH10	1
-#define E500_PM_PH15	2
-#define E500_PM_PH20	3
-#define E500_PM_PH30	4
+#define E500_PM_PW10	2
+#define E500_PM_PH15	3
+#define E500_PM_PH20	4
+#define E500_PM_PH30	5
 #define E500_PM_DOZE	E500_PM_PH10
 #define E500_PM_NAP	E500_PM_PH15
 
diff --git a/arch/powerpc/include/asm/machdep.h b/arch/powerpc/include/asm/machdep.h
index eb9dec0..bb96a21 100644
--- a/arch/powerpc/include/asm/machdep.h
+++ b/arch/powerpc/include/asm/machdep.h
@@ -278,6 +278,7 @@ extern void power4_idle(void);
 extern void power7_idle(void);
 extern void ppc6xx_idle(void);
 extern void book3e_idle(void);
+extern void book3e_die(void);
 
 /*
  * ppc_md contains a copy of the machine description structure for the
diff --git a/arch/powerpc/kernel/idle_book3e.S b/arch/powerpc/kernel/idle_book3e.S
index bfb73cc..e7d8ab3 100644
--- a/arch/powerpc/kernel/idle_book3e.S
+++ b/arch/powerpc/kernel/idle_book3e.S
@@ -79,6 +79,12 @@ _GLOBAL(\name)
 	b	1b
 .endm
 
+_GLOBAL(book3e_die)
+	wrteei  1
+	PPC_WAIT(0)
+1:
+	b	1b
+
 /* epapr_ev_idle_start below is patched with the proper hcall
    opcodes during kernel initialization */
 .macro EPAPR_EV_IDLE_LOOP
diff --git a/arch/powerpc/platforms/85xx/smp.c b/arch/powerpc/platforms/85xx/smp.c
index a65c5d3..5b12f4b 100644
--- a/arch/powerpc/platforms/85xx/smp.c
+++ b/arch/powerpc/platforms/85xx/smp.c
@@ -48,8 +48,6 @@ static int tb_req;
 static int tb_valid;
 static u32 cur_booting_core;
 
-extern void fsl_enable_threads(void);
-
 /* specify the cpu PM state when cpu dies, PH15/NAP is the default */
 int qoriq_cpu_die_state = E500_PM_PH15;
 
@@ -212,11 +210,8 @@ static void __cpuinit smp_85xx_mach_cpu_die(void)
 	if (cur_cpu_spec && cur_cpu_spec->cpu_flush_caches)
 		cur_cpu_spec->cpu_flush_caches();
 
-	if (is_core_down(cpu))
+	if (qoriq_pm_ops->cpu_enter_state)
 		qoriq_pm_ops->cpu_enter_state(cpu, qoriq_cpu_die_state);
-
-	while (1)
-		;
 }
 
 #else
@@ -291,15 +286,33 @@ static int smp_85xx_kick_cpu(int nr)
 		if (cpu_online(cpu_first_thread_sibling(nr))) {
 
 			local_irq_save(flags);
-			/*
-			 * In cpu hotplug case, Thread 1 of Core 0 must
-			 * start by calling fsl_enable_threads(). Thread 1
-			 * of other cores can be started by Thread 0
-			 * after reset.
-			 */
-			if (nr == 1 && system_state == SYSTEM_RUNNING)
-				fsl_enable_threads();
-
+#ifdef CONFIG_PPC_E500MC
+			if (system_state == SYSTEM_RUNNING) {
+				/*
+				 * In cpu hotplug case, Thread 1 of Core0
+				 * must start by Thread0 of Core0 calling
+				 * fsl_enable_threads().
+				 */
+				if (nr == 1) {
+					if (get_cpu() == boot_cpuid)
+						fsl_enable_threads(&ret);
+					else
+						work_on_cpu(boot_cpuid,
+						    fsl_enable_threads, NULL);
+				}
+
+				/*
+				 * Thread 1 of other cores can be waken up from
+				 * low-power state when Thread 0 is online,
+				 * or restart by Thread 0 after core reset.
+				 */
+				if (!strcmp(cur_cpu_spec->cpu_name, "e6500"))
+					arch_send_call_function_single_ipi(nr);
+
+				if (qoriq_pm_ops->irq_unmask)
+					qoriq_pm_ops->irq_unmask(nr);
+			}
+#endif
 			smp_generic_kick_cpu(nr);
 
 			generic_set_cpu_up(nr);
@@ -365,6 +378,13 @@ static int smp_85xx_kick_cpu(int nr)
 
 #ifdef CONFIG_PPC_E500MC
 		/*
+		 * For e6500-based platform, wake up core from
+		 * low-power state before reset core.
+		 */
+		if (!strcmp(cur_cpu_spec->cpu_name, "e6500"))
+			arch_send_call_function_single_ipi(nr);
+
+		/*
 		 * Errata-A-006568. If SOC-rcpm is V1, we need enable
 		 * cpu first, T4240rev2 and later Soc has been fixed.
 		 * But before this errata is still needed.
@@ -606,8 +626,13 @@ void __init mpc85xx_smp_init(void)
 #ifdef CONFIG_HOTPLUG_CPU
 		ppc_md.cpu_die = smp_85xx_mach_cpu_die;
 #endif
+		/*
+		 * For e6500-based platform, put thread into PW10 state gives
+		 * it a chance to enter PW20 state when threads of the same
+		 * core are in PW10 state.
+		 */
 		if (!strcmp(cur_cpu_spec->cpu_name, "e6500"))
-			qoriq_cpu_die_state = E500_PM_PH20;
+			qoriq_cpu_die_state = E500_PM_PW10;
 	}
 
 	smp_ops = &smp_85xx_ops;
diff --git a/arch/powerpc/platforms/85xx/smp.h b/arch/powerpc/platforms/85xx/smp.h
index e2b4493..8983009 100644
--- a/arch/powerpc/platforms/85xx/smp.h
+++ b/arch/powerpc/platforms/85xx/smp.h
@@ -12,4 +12,6 @@ static inline void mpc85xx_smp_init(void)
 }
 #endif
 
+extern long fsl_enable_threads(void *);
+
 #endif /* not POWERPC_85XX_SMP_H_ */
diff --git a/arch/powerpc/sysdev/fsl_rcpm.c b/arch/powerpc/sysdev/fsl_rcpm.c
index a21235c..6b26678 100644
--- a/arch/powerpc/sysdev/fsl_rcpm.c
+++ b/arch/powerpc/sysdev/fsl_rcpm.c
@@ -19,6 +19,7 @@
 #include <asm/fsl_guts.h>
 #include <asm/cputhreads.h>
 #include <asm/fsl_pm.h>
+#include <asm/machdep.h>
 
 const struct fsl_pm_ops *qoriq_pm_ops;
 
@@ -152,7 +153,9 @@ static void rcpm_v2_irq_mask(int cpu)
 	int hw_cpu = get_hard_smp_processor_id(cpu);
 	unsigned int mask = 1 << hw_cpu;
 
-	setbits32(&rcpm_v2_regs->tpmimr0, mask);
+	if (strcmp(cur_cpu_spec->cpu_name, "e6500"))
+		setbits32(&rcpm_v2_regs->tpmimr0, mask);
+
 	setbits32(&rcpm_v2_regs->tpmcimr0, mask);
 	setbits32(&rcpm_v2_regs->tpmmcmr0, mask);
 	setbits32(&rcpm_v2_regs->tpmnmimr0, mask);
@@ -163,7 +166,9 @@ static void rcpm_v2_irq_unmask(int cpu)
 	int hw_cpu = get_hard_smp_processor_id(cpu);
 	unsigned int mask = 1 << hw_cpu;
 
-	clrbits32(&rcpm_v2_regs->tpmimr0, mask);
+	if (strcmp(cur_cpu_spec->cpu_name, "e6500"))
+		clrbits32(&rcpm_v2_regs->tpmimr0, mask);
+
 	clrbits32(&rcpm_v2_regs->tpmcimr0, mask);
 	clrbits32(&rcpm_v2_regs->tpmmcmr0, mask);
 	clrbits32(&rcpm_v2_regs->tpmnmimr0, mask);
@@ -188,6 +193,12 @@ static void rcpm_v2_cpu_enter_state(int cpu, int state)
 		/* one bit corresponds to one thread for PH10 of 6500 */
 		setbits32(&rcpm_v2_regs->tph10setr0, 1 << hw_cpu);
 		break;
+#ifdef CONFIG_PPC_BOOK3E_64
+	case E500_PM_PW10:
+		/* one bit corresponds to one thread for PW10 of 6500 */
+		book3e_die();
+		break;
+#endif
 	case E500_PM_PH15:
 		setbits32(&rcpm_v2_regs->pcph15setr, mask);
 		break;
@@ -207,18 +218,22 @@ static void rcpm_v2_cpu_exit_state(int cpu, int state)
 	unsigned int hw_cpu = get_hard_smp_processor_id(cpu);
 	u32 mask = 1 << cpu_core_index_of_thread(hw_cpu);
 
+	/*
+	 * The true value read from these registers only means
+	 * there is a pending request.
+	 */
 	switch (state) {
 	case E500_PM_PH10:
-		setbits32(&rcpm_v2_regs->tph10clrr0, 1 << hw_cpu);
+		out_be32(&rcpm_v2_regs->tph10clrr0, 1 << hw_cpu);
 		break;
 	case E500_PM_PH15:
-		setbits32(&rcpm_v2_regs->pcph15clrr, mask);
+		out_be32(&rcpm_v2_regs->pcph15clrr, mask);
 		break;
 	case E500_PM_PH20:
-		setbits32(&rcpm_v2_regs->pcph20clrr, mask);
+		out_be32(&rcpm_v2_regs->pcph20clrr, mask);
 		break;
 	case E500_PM_PH30:
-		setbits32(&rcpm_v2_regs->pcph30clrr, mask);
+		out_be32(&rcpm_v2_regs->pcph30clrr, mask);
 		break;
 	default:
 		pr_err("%s: Unknown cpu PM state (%d)\n", __func__, state);