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