diff --git a/conts/libl4/src/arm/mutex.S b/conts/libl4/src/arm/mutex.S
index 6118b83..144f312 100644
--- a/conts/libl4/src/arm/mutex.S
+++ b/conts/libl4/src/arm/mutex.S
@@ -63,6 +63,9 @@ BEGIN_PROC(__l4_mutex_unlock)
 	mov	pc, lr
 END_PROC(__l4_mutex_unlock)
 
+/*
+ * r0 = byte address to read from.
+ */
 BEGIN_PROC(l4_atomic_dest_readb)
 	mov	r1, r0		@ Move byte address to r1
 	mov	r2, #0		@ Move 0 to r2
diff --git a/include/l4/arch/arm/exception.h b/include/l4/arch/arm/exception.h
index 4d9d053..953b20f 100644
--- a/include/l4/arch/arm/exception.h
+++ b/include/l4/arch/arm/exception.h
@@ -42,17 +42,20 @@ static inline void disable_irqs()
 /* Disable the irqs unconditionally, but also keep the previous state such that
  * if it was already disabled before the call, the restore call would retain
  * this state. */
-static inline void irq_local_disable_save(unsigned long *state)
+void irq_local_disable_save(unsigned long *state);
+#if 0
 {
 	unsigned long temp;
 	__asm__ __volatile__ (
 		"mrs	%0, cpsr_fc\n"
-		"orr	%1, %0, #0x80\n"
-		"msr	cpsr_fc, %1\n"
+		"orr	%2, %0, #0x80\n"
+		"msr	cpsr_fc, %2\n"
 		: "=r" (*state)
 		: "r" (*state),"r" (temp)
 	);
 }
+#endif
+
 /* Simply change it back to original state supplied in @flags. This might enable
  * or retain disabled state of the irqs for example. Useful for nested calls. */
 static inline void irq_local_restore(unsigned long state)
diff --git a/include/l4/arch/arm/v5/irq.h b/include/l4/arch/arm/v5/irq.h
new file mode 100644
index 0000000..9de39dd
--- /dev/null
+++ b/include/l4/arch/arm/v5/irq.h
@@ -0,0 +1,11 @@
+#ifndef __ARM_V5_IRQ_H__
+#define __ARM_V5_IRQ_H__
+
+/*
+ * Destructive atomic-read.
+ *
+ * Write 0 to byte at @location as its contents are read back.
+ */
+char l4_atomic_dest_readb(void *location);
+
+#endif
diff --git a/include/l4/glue/arm/message.h b/include/l4/glue/arm/message.h
index f89dd23..e04af8d 100644
--- a/include/l4/glue/arm/message.h
+++ b/include/l4/glue/arm/message.h
@@ -73,7 +73,8 @@
 #define MR0_REGISTER		r3
 #define MR_RETURN_REGISTER	r3
 
-#define TASK_NOTIFY_MAX		8
+#define TASK_NOTIFY_SLOTS	8
+#define TASK_NOTIFY_MAXVALUE	255
 
 /* Primaries aren't used for memcopy. Those ops use this as a parameter */
 #define L4_UTCB_FULL_BUFFER_SIZE	(MR_REST * sizeof(int))
@@ -85,7 +86,7 @@ struct utcb {
 	u32 mr[MR_TOTAL];	/* MRs that are mapped to real registers */
 	u32 saved_tag;		/* Saved tag field for stacked ipcs */
 	u32 saved_sender;	/* Saved sender field for stacked ipcs */
-	u8  notify[TASK_NOTIFY_MAX]; /* Irq notification slots */
+	u8  notify[TASK_NOTIFY_SLOTS]; /* Irq notification slots */
 	u32 mr_rest[MR_REST];	/* Complete the utcb for up to 64 words */
 };
 #endif
diff --git a/src/api/irq.c b/src/api/irq.c
index 3fd2e6d..1a6f9b2 100644
--- a/src/api/irq.c
+++ b/src/api/irq.c
@@ -12,6 +12,7 @@
 #include <l4/generic/tcb.h>
 #include INC_GLUE(message.h)
 #include <l4/lib/wait.h>
+#include INC_SUBARCH(irq.h)
 
 /*
  * Default function that handles userspace
@@ -57,12 +58,13 @@ int irq_thread_notify(struct irq_desc *desc)
 	utcb = (struct utcb *)desc->task->utcb_address;
 
 	/* Atomic increment (See above comments) with no wraparound */
-	if (utcb->notify[desc->task_notify_slot] != TASK_NOTIFY_MAX)
+	if (utcb->notify[desc->task_notify_slot] != TASK_NOTIFY_MAXVALUE)
 		utcb->notify[desc->task_notify_slot]++;
 
 	/* Async wake up any waiter irq threads */
-	wake_up(&desc->task->wqh_notify, WAKEUP_ASYNC);
+	wake_up(&desc->wqh_irq, WAKEUP_ASYNC);
 
+	BUG_ON(!irqs_enabled());
 	return 0;
 }
 
@@ -111,10 +113,13 @@ int irq_wait(l4id_t irq_index)
 
 	/* Wait until the irq changes slot value */
 	WAIT_EVENT(&desc->wqh_irq,
-		   !utcb->notify[desc->task_notify_slot],
+		   utcb->notify[desc->task_notify_slot] != 0,
 		   ret);
-
-	return ret;
+	printk("Didn't sleep. utcb->notify[%d]=%d\n", desc->task_notify_slot, utcb->notify[desc->task_notify_slot]);
+	if (ret < 0)
+		return ret;
+	else
+		return l4_atomic_dest_readb(&utcb->notify[desc->task_notify_slot]);
 }
 
 
diff --git a/src/arch/arm/v5/SConscript b/src/arch/arm/v5/SConscript
index 4fd282a..74b0345 100644
--- a/src/arch/arm/v5/SConscript
+++ b/src/arch/arm/v5/SConscript
@@ -4,7 +4,7 @@
 Import('env')
 
 # The set of source files associated with this SConscript file.
-src_local = ['mm.c', 'mmu_ops.S', 'mutex.S']
+src_local = ['mm.c', 'mmu_ops.S', 'mutex.S', 'irq.S']
 
 obj = env.Object(src_local)
 Return('obj')
diff --git a/src/arch/arm/v5/irq.S b/src/arch/arm/v5/irq.S
new file mode 100644
index 0000000..4bc607f
--- /dev/null
+++ b/src/arch/arm/v5/irq.S
@@ -0,0 +1,27 @@
+
+
+
+#include INC_ARCH(asm.h)
+
+/*
+ * r0 = unsigned long *state, stores current irq state.
+ */
+BEGIN_PROC(irq_local_disable_save)
+	mov	r1, r0		@ Move address of state to r1
+	mrs	r2, cpsr_fc	@ Read cpsr
+	orr	r3, r2, #0x80	@ Disable irq bit in cpsr
+	msr	cpsr_fc, r3	@ Write to cpsr
+	str	r2, [r1]	@ Store original cpsr in r2 to state
+	mov	pc, lr
+END_PROC(irq_local_disable_save)
+
+/*
+ * r0 = byte address to read from.
+ */
+BEGIN_PROC(l4_atomic_dest_readb)
+	mov	r1, r0		@ Move byte address to r1
+	mov	r2, #0		@ Move 0 to r2
+	swpb	r0, r2, [r1]	@ Write 0 to byte location, while reading its value to r0
+	mov	pc, lr		@ Return byte location value
+END_PROC(l4_atomic_dest_readb)
+
diff --git a/src/generic/irq.c b/src/generic/irq.c
index a027350..00f5026 100644
--- a/src/generic/irq.c
+++ b/src/generic/irq.c
@@ -42,6 +42,9 @@ int irq_register(struct ktcb *task, int notify_slot, l4id_t irq_index)
 	this_desc->task = task;
 	this_desc->task_notify_slot = notify_slot;
 
+	/* Setup irq desc waitqueue */
+	waitqueue_head_init(&this_desc->wqh_irq);
+
 	/* Enable the irq */
 	irq_enable(irq_index);
 
diff --git a/src/generic/scheduler.c b/src/generic/scheduler.c
index 287a67e..9989e1d 100644
--- a/src/generic/scheduler.c
+++ b/src/generic/scheduler.c
@@ -38,6 +38,7 @@ void sched_lock_runqueues(unsigned long *irqflags)
 {
 	spin_lock_irq(&scheduler.sched_rq[0].lock, irqflags);
 	spin_lock(&scheduler.sched_rq[1].lock);
+	BUG_ON(irqs_enabled());
 }
 
 void sched_unlock_runqueues(unsigned long irqflags)
diff --git a/src/lib/wait.c b/src/lib/wait.c
index a4d3a9d..b3f2e2c 100644
--- a/src/lib/wait.c
+++ b/src/lib/wait.c
@@ -165,7 +165,7 @@ void wake_up(struct waitqueue_head *wqh, unsigned int flags)
 		task_unset_wqh(sleeper);
 		if (flags & WAKEUP_INTERRUPT)
 			sleeper->flags |= TASK_INTERRUPTED;
-		//printk("(%d) Waking up (%d)\n", current->tid, sleeper->tid);
+		printk("(%d) Waking up (%d)\n", current->tid, sleeper->tid);
 		spin_unlock_irq(&wqh->slock, irqflags);
 
 		if (flags & WAKEUP_SYNC)