[AVR32] Put cpu in sleep 0 when idle.

This patch puts the CPU in sleep 0 when doing nothing, idle. This will
turn of the CPU clock and thus save power. The CPU is waken again when
an interrupt occurs.

Signed-off-by: Hans-Christian Egtvedt <hcegtvedt@atmel.com>
Signed-off-by: Haavard Skinnemoen <hskinnemoen@atmel.com>

3 files changed, 42 insertions, 7 deletions

diff --git a/arch/avr32/kernel/entry-avr32b.S b/arch/avr32/kernel/entry-avr32b.S
index eeb66792bc37..5f5f7e42f51b 100644
--- a/arch/avr32/kernel/entry-avr32b.S
+++ b/arch/avr32/kernel/entry-avr32b.S
@@ -630,9 +630,12 @@ irq_level\level:
         rcall   do_IRQ
 
         lddsp   r4, sp[REG_SR]
-        andh    r4, (MODE_MASK >> 16), COH
+        bfextu  r4, r4, SYSREG_M0_OFFSET, 3
+        cp.w    r4, MODE_SUPERVISOR >> SYSREG_M0_OFFSET
+        breq    2f
+        cp.w    r4, MODE_USER >> SYSREG_M0_OFFSET
 #ifdef CONFIG_PREEMPT
-        brne    2f
+        brne    3f
 #else
         brne    1f
 #endif
@@ -649,9 +652,18 @@ irq_level\level:
         sub     sp, -4          /* ignore r12_orig */
         rete
 
+2:      get_thread_info r0
+        ld.w    r1, r0[TI_flags]
+        bld     r1, TIF_CPU_GOING_TO_SLEEP
 #ifdef CONFIG_PREEMPT
-2:
-        get_thread_info r0
+        brcc    3f
+#else
+        brcc    1b
+#endif
+        sub     r1, pc, . - cpu_idle_skip_sleep
+        stdsp   sp[REG_PC], r1
+#ifdef CONFIG_PREEMPT
+3:      get_thread_info r0
         ld.w    r2, r0[TI_preempt_count]
         cp.w    r2, 0
         brne    1b
@@ -662,12 +674,32 @@ irq_level\level:
         bld     r4, SYSREG_GM_OFFSET
         brcs    1b
         rcall   preempt_schedule_irq
-        rjmp    1b
 #endif
+        rjmp    1b
         .endm
 
         .section .irq.text,"ax",@progbits
 
+.global cpu_idle_sleep
+cpu_idle_sleep:
+        mask_interrupts
+        get_thread_info r8
+        ld.w    r9, r8[TI_flags]
+        bld     r9, TIF_NEED_RESCHED
+        brcs    cpu_idle_enable_int_and_exit
+        sbr     r9, TIF_CPU_GOING_TO_SLEEP
+        st.w    r8[TI_flags], r9
+        unmask_interrupts
+        sleep 0
+cpu_idle_skip_sleep:
+        mask_interrupts
+        ld.w    r9, r8[TI_flags]
+        cbr     r9, TIF_CPU_GOING_TO_SLEEP
+        st.w    r8[TI_flags], r9
+cpu_idle_enable_int_and_exit:
+        unmask_interrupts
+        retal   r12
+
         .global irq_level0
         .global irq_level1
         .global irq_level2
diff --git a/arch/avr32/kernel/process.c b/arch/avr32/kernel/process.c
index 0b4325946a41..4f8d2d474740 100644
--- a/arch/avr32/kernel/process.c
+++ b/arch/avr32/kernel/process.c
@@ -19,6 +19,8 @@
 void (*pm_power_off)(void) = NULL;
 EXPORT_SYMBOL(pm_power_off);
 
+extern void cpu_idle_sleep(void);
+
 /*
  * This file handles the architecture-dependent parts of process handling..
  */
@@ -27,9 +29,8 @@ void cpu_idle(void)
 {
         /* endless idle loop with no priority at all */
         while (1) {
-                /* TODO: Enter sleep mode */
                 while (!need_resched())
-                        cpu_relax();
+                        cpu_idle_sleep();
                 preempt_enable_no_resched();
                 schedule();
                 preempt_disable();
diff --git a/include/asm-avr32/thread_info.h b/include/asm-avr32/thread_info.h
index d1f5b35ebd54..a2e606dd4f4a 100644
--- a/include/asm-avr32/thread_info.h
+++ b/include/asm-avr32/thread_info.h
@@ -83,6 +83,7 @@ static inline struct thread_info *current_thread_info(void)
 #define TIF_SINGLE_STEP         6       /* single step after next break */
 #define TIF_MEMDIE              7
 #define TIF_RESTORE_SIGMASK     8       /* restore signal mask in do_signal */
+#define TIF_CPU_GOING_TO_SLEEP  9       /* CPU is entering sleep 0 mode */
 #define TIF_USERSPACE           31      /* true if FS sets userspace */
 
 #define _TIF_SYSCALL_TRACE      (1 << TIF_SYSCALL_TRACE)
@@ -94,6 +95,7 @@ static inline struct thread_info *current_thread_info(void)
 #define _TIF_SINGLE_STEP        (1 << TIF_SINGLE_STEP)
 #define _TIF_MEMDIE             (1 << TIF_MEMDIE)
 #define _TIF_RESTORE_SIGMASK    (1 << TIF_RESTORE_SIGMASK)
+#define _TIF_CPU_GOING_TO_SLEEP (1 << TIF_CPU_GOING_TO_SLEEP)
 
 /* XXX: These two masks must never span more than 16 bits! */
 /* work to do on interrupt/exception return */