powerpc/85xx: fix pw20&altivec idle can not work after cpuhotplug restore

PW20 & AltiVec idle should be enabled, when the cpuhotplug restore.

Signed-off-by: Wang Dongsheng <dongsheng.wang@freescale.com>
Change-Id: Ib4d2ac24eedde0a6a814f3b5f0b42ca03e20494c
Reviewed-on: http://git.am.freescale.net:8181/4269
Tested-by: Review Code-CDREVIEW <CDREVIEW@freescale.com>
Reviewed-by: Wood Scott-B07421 <scottwood@freescale.com>
Reviewed-by: Rivera Jose-B46482 <German.Rivera@freescale.com>

3 files changed, 80 insertions, 109 deletions

diff --git a/arch/powerpc/include/asm/reg_booke.h b/arch/powerpc/include/asm/reg_booke.h
index 5152c36..8987ca3 100644
--- a/arch/powerpc/include/asm/reg_booke.h
+++ b/arch/powerpc/include/asm/reg_booke.h
@@ -227,8 +227,12 @@
 #define	CCR1_TCS	0x00000080 /* Timer Clock Select */
 
 /* Bit definitions for PWRMGTCR0. */
-#define PWRMGTCR0_ALTIVEC_IDLE	(1 << 22) /* Altivec idle enable */
-#define PWRMGTCR0_PW20_WAIT	(1 << 14) /* PW20 state enable bit */
+#define PWRMGTCR0_PW20_WAIT		(1 << 14) /* PW20 state enable bit */
+#define PWRMGTCR0_PW20_ENT_SHIFT	8
+#define PWRMGTCR0_PW20_ENT		0x3F00
+#define PWRMGTCR0_AV_IDLE_PD_EN		(1 << 22) /* Altivec idle enable */
+#define PWRMGTCR0_AV_IDLE_CNT_SHIFT	16
+#define PWRMGTCR0_AV_IDLE_CNT		0x3F0000
 
 /* Bit definitions for the MCSR. */
 #define MCSR_MCS	0x80000000 /* Machine Check Summary */
diff --git a/arch/powerpc/kernel/cpu_setup_fsl_booke.S b/arch/powerpc/kernel/cpu_setup_fsl_booke.S
index dbeff8c..acba3dfd 100644
--- a/arch/powerpc/kernel/cpu_setup_fsl_booke.S
+++ b/arch/powerpc/kernel/cpu_setup_fsl_booke.S
@@ -53,12 +53,84 @@ _GLOBAL(__e500_dcache_setup)
 	isync
 	blr
 
+_GLOBAL(has_pw20_altivec_idle)
+	/* 0 false, 1 true */
+	li	r3, 0
+
+	/* PW20 & AltiVec idle feature only exists for E6500 */
+	mfspr	r0, SPRN_PVR
+	rlwinm	r4, r0, 16, 16, 31
+	lis	r12, 0
+	ori	r12, r12, PVR_VER_E6500@l
+	cmpw	r4, r12
+	bne	2f
+
+	/* Fix erratum, e6500 rev1 does not support PW20 & AltiVec idle */
+	rlwinm	r4, r0, 0, 16, 31
+	cmpwi	r4, 0x20
+	blt	2f
+	li	r3, 1
+2:
+	blr
+
+/*
+ * FIXME - we haven't yet done testing to determine a reasonable default
+ * value for PW20_WAIT_IDLE_BIT.
+ */
+#define PW20_WAIT_IDLE_BIT		50 /* 1ms, TB frequency is 41.66MHZ */
+_GLOBAL(setup_pw20_idle)
+	mflr	r10
+	bl	.has_pw20_altivec_idle
+	mtlr	r10
+	cmpwi	r3, 0
+	beq	2f
+
+	mfspr	r3, SPRN_PWRMGTCR0
+
+	/* Set PW20_WAIT bit, enable pw20 state*/
+	ori	r3, r3, PWRMGTCR0_PW20_WAIT
+	li	r4, PW20_WAIT_IDLE_BIT
+
+	/* Set Automatic PW20 Core Idle Count */
+	rlwimi	r3, r4, PWRMGTCR0_PW20_ENT_SHIFT, PWRMGTCR0_PW20_ENT
+
+	mtspr	SPRN_PWRMGTCR0, r3
+2:
+	blr
+
+/*
+ * FIXME - we haven't yet done testing to determine a reasonable default
+ * value for AV_WAIT_IDLE_BIT.
+ */
+#define AV_WAIT_IDLE_BIT		50 /* 1ms, TB frequency is 41.66MHZ */
+_GLOBAL(setup_altivec_idle)
+	mflr	r10
+	bl	.has_pw20_altivec_idle
+	mtlr	r10
+	cmpwi	r3, 0
+	beq	2f
+
+	mfspr	r3, SPRN_PWRMGTCR0
+
+	/* Enable Altivec Idle */
+	oris	r3, r3, PWRMGTCR0_AV_IDLE_PD_EN@h
+	li	r4, AV_WAIT_IDLE_BIT
+
+	/* Set Automatic AltiVec Idle Count */
+	rlwimi	r3, r4, PWRMGTCR0_AV_IDLE_CNT_SHIFT, PWRMGTCR0_AV_IDLE_CNT
+
+	mtspr	SPRN_PWRMGTCR0, r3
+2:
+	blr
+
 #ifdef CONFIG_PPC_BOOK3E_64
 _GLOBAL(__setup_cpu_e6500)
 	mflr	r6
 #ifdef CONFIG_PPC64
 	bl	.setup_altivec_ivors
 #endif
+	bl	.setup_pw20_idle
+	bl	.setup_altivec_idle
 	bl	__setup_cpu_e5500
 	mtlr	r6
 	blr
@@ -121,6 +193,8 @@ _GLOBAL(__setup_cpu_e5500)
 _GLOBAL(__restore_cpu_e6500)
 	mflr	r5
 	bl	.setup_altivec_ivors
+	bl	.setup_pw20_idle
+	bl	.setup_altivec_idle
 	bl	__restore_cpu_e5500
 	mtlr	r5
 	blr
diff --git a/arch/powerpc/platforms/85xx/common.c b/arch/powerpc/platforms/85xx/common.c
index 09978f5..f63c10d 100644
--- a/arch/powerpc/platforms/85xx/common.c
+++ b/arch/powerpc/platforms/85xx/common.c
@@ -7,32 +7,10 @@
  */
 #include <linux/of_platform.h>
 
-#include <asm/time.h>
-
 #include <sysdev/cpm2_pic.h>
 
 #include "mpc85xx.h"
 
-#define MAX_BIT				64
-
-#define ALTIVEC_COUNT_OFFSET		16
-#define ALTIVEC_IDLE_COUNT_MASK		0x003f0000
-#define PW20_COUNT_OFFSET		8
-#define PW20_IDLE_COUNT_MASK		0x00003f00
-
-/*
- * FIXME - We don't know the AltiVec application scenarios.
- */
-#define ALTIVEC_IDLE_TIME	1000 /* 1ms */
-
-/*
- * FIXME - We don't know, what time should we let the core into PW20 state.
- * because we don't know the current state of the cpu load. And threads are
- * independent, so we can not know the state of different thread has been
- * idle.
- */
-#define	PW20_IDLE_TIME		1000 /* 1ms */
-
 static struct of_device_id __initdata mpc85xx_common_ids[] = {
 	{ .type = "soc", },
 	{ .compatible = "soc", },
@@ -80,7 +58,6 @@ static void cpm2_cascade(unsigned int irq, struct irq_desc *desc)
 	chip->irq_eoi(&desc->irq_data);
 }
 
-
 void __init mpc85xx_cpm2_pic_init(void)
 {
 	struct device_node *np;
@@ -104,87 +81,3 @@ void __init mpc85xx_cpm2_pic_init(void)
 	irq_set_chained_handler(irq, cpm2_cascade);
 }
 #endif
-
-static bool has_pw20_altivec_idle(void)
-{
-	u32 pvr;
-
-	pvr = mfspr(SPRN_PVR);
-
-	/* PW20 & AltiVec idle feature only exists for E6500 */
-	if (PVR_VER(pvr) != PVR_VER_E6500)
-		return false;
-
-	/* Fix erratum, e6500 rev1 does not support PW20 & AltiVec idle */
-	if (PVR_REV(pvr) < 0x20)
-		return false;
-
-	return true;
-}
-
-static unsigned int get_idle_ticks_bit(unsigned int us)
-{
-	unsigned int cycle;
-
-	/*
-	 * The time control by TB turn over bit, so we need
-	 * to be divided by 2.
-	 */
-	cycle = (us / 2) * tb_ticks_per_usec;
-
-	return ilog2(cycle) + 1;
-}
-
-static void setup_altivec_idle(void *unused)
-{
-	u32 altivec_idle, bit;
-
-	if (!has_pw20_altivec_idle())
-		return;
-
-	/* Enable Altivec Idle */
-	altivec_idle = mfspr(SPRN_PWRMGTCR0);
-	altivec_idle |= PWRMGTCR0_ALTIVEC_IDLE;
-
-	/* Set Automatic AltiVec Idle Count */
-	/* clear count */
-	altivec_idle &= ~ALTIVEC_IDLE_COUNT_MASK;
-
-	/* set count */
-	bit = get_idle_ticks_bit(ALTIVEC_IDLE_TIME);
-	altivec_idle |= ((MAX_BIT - bit) << ALTIVEC_COUNT_OFFSET);
-
-	mtspr(SPRN_PWRMGTCR0, altivec_idle);
-}
-
-static void setup_pw20_idle(void *unused)
-{
-	u32 pw20_idle, bit;
-
-	if (!has_pw20_altivec_idle())
-		return;
-
-	pw20_idle = mfspr(SPRN_PWRMGTCR0);
-
-	/* set PW20_WAIT bit, enable pw20 */
-	pw20_idle |= PWRMGTCR0_PW20_WAIT;
-
-	/* Set Automatic PW20 Core Idle Count */
-	/* clear count */
-	pw20_idle &= ~PW20_IDLE_COUNT_MASK;
-
-	/* set count */
-	bit = get_idle_ticks_bit(PW20_IDLE_TIME);
-	pw20_idle |= ((MAX_BIT - bit) << PW20_COUNT_OFFSET);
-
-	mtspr(SPRN_PWRMGTCR0, pw20_idle);
-}
-
-static int __init setup_idle_hw_governor(void)
-{
-	on_each_cpu(setup_altivec_idle, NULL, 1);
-	on_each_cpu(setup_pw20_idle, NULL, 1);
-
-	return 0;
-}
-late_initcall(setup_idle_hw_governor);