diff --git a/arch/avr32/kernel/entry-avr32b.S b/arch/avr32/kernel/entry-avr32b.S
index eeb66792bc37887e382640c297a116d8de5d22b4..5f5f7e42f51b3c9d722636e1f90f4bf3802d879f 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 0b4325946a41629cc7a4755a7c679311e85e688f..4f8d2d4747406d0aa4210d50a40424324adcc6d8 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 d1f5b35ebd54212f7cfec40cb2458a648c305dd3..a2e606dd4f4a8bc3ba465fbd205a12cf6475925a 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 */