3 files changed, 22 insertions, 1 deletions
diff --git a/arch/powerpc/platforms/pasemi/cpufreq.c b/arch/powerpc/platforms/pasemi/cpufreq.c
index 8caa1667cf34..58556b028a4c 100644
--- a/arch/powerpc/platforms/pasemi/cpufreq.c
+++ b/arch/powerpc/platforms/pasemi/cpufreq.c
@@ -32,6 +32,7 @@
 #include <asm/io.h>
 #include <asm/prom.h>
 #include <asm/time.h>
+#include <asm/smp.h>
 
 #define SDCASR_REG		0x0100
 #define SDCASR_REG_STRIDE	0x1000
@@ -124,6 +125,11 @@ static void set_astate(int cpu, unsigned int astate)
 	local_irq_restore(flags);
 }
 
+int check_astate(void)
+{
+	return get_cur_astate(hard_smp_processor_id());
+}
+
 void restore_astate(int cpu)
 {
 	set_astate(cpu, current_astate);
diff --git a/arch/powerpc/platforms/pasemi/pasemi.h b/arch/powerpc/platforms/pasemi/pasemi.h
index 516acabb4e96..c96127b029b6 100644
--- a/arch/powerpc/platforms/pasemi/pasemi.h
+++ b/arch/powerpc/platforms/pasemi/pasemi.h
@@ -16,8 +16,14 @@ extern void idle_doze(void);
 
 /* Restore astate to last set */
 #ifdef CONFIG_PPC_PASEMI_CPUFREQ
+extern int check_astate(void);
 extern void restore_astate(int cpu);
 #else
+static inline int check_astate(void)
+{
+	/* Always return >0 so we never power save */
+	return 1;
+}
 static inline void restore_astate(int cpu)
 {
 }
diff --git a/arch/powerpc/platforms/pasemi/powersave.S b/arch/powerpc/platforms/pasemi/powersave.S
index 6d0fba6aab17..56f45adcd089 100644
--- a/arch/powerpc/platforms/pasemi/powersave.S
+++ b/arch/powerpc/platforms/pasemi/powersave.S
@@ -62,7 +62,16 @@ sleep_common:
 	mflr	r0
 	std	r0, 16(r1)
 	stdu	r1,-64(r1)
+#ifdef CONFIG_PPC_PASEMI_CPUFREQ
+	std	r3, 48(r1)
 
+	/* Only do power savings when in astate 0 */
+	bl	.check_astate
+	cmpwi	r3,0
+	bne	1f
+
+	ld	r3, 48(r1)
+#endif
 	LOAD_REG_IMMEDIATE(r6,MSR_DR|MSR_IR|MSR_ME|MSR_EE)
 	mfmsr	r4
 	andc	r5,r4,r6
@@ -73,7 +82,7 @@ sleep_common:
 
 	mtmsrd	r4,0
 
-	addi	r1,r1,64
+1:	addi	r1,r1,64
 	ld	r0,16(r1)
 	mtlr	r0
 	blr