diff --git a/include/arm9/interrupt.h b/include/arm9/interrupt.h
index d58e671..1767aca 100644
--- a/include/arm9/interrupt.h
+++ b/include/arm9/interrupt.h
@@ -3,40 +3,59 @@
 #include "io.h"
 
 
-
 #define waitForIrq()      __asm__ __volatile__("mcr p15, 0, %0, c7, c0, 4\n\t" : : "r" (0))
 
 
 typedef enum
 {
-	IRQ_DMAC_1_0      = (1<<0 ), // DMAC_1 = NDMA
-	IRQ_DMAC_1_1      = (1<<1 ),
-	IRQ_DMAC_1_2      = (1<<2 ),
-	IRQ_DMAC_1_3      = (1<<3 ),
-	IRQ_DMAC_1_4      = (1<<4 ),
-	IRQ_DMAC_1_5      = (1<<5 ),
-	IRQ_DMAC_1_6      = (1<<6 ),
-	IRQ_DMAC_1_7      = (1<<7 ),
-	IRQ_TIMER_0       = (1<<8 ),
-	IRQ_TIMER_1       = (1<<9 ),
-	IRQ_TIMER_2       = (1<<10),
-	IRQ_TIMER_3       = (1<<11),
-	IRQ_PXI_SYNC      = (1<<12),
-	IRQ_PXI_NOT_FULL  = (1<<13),
-	IRQ_PXI_NOT_EMPTY = (1<<14),
-	IRQ_AES           = (1<<15),
-	IRQ_SDIO_1        = (1<<16),
-	IRQ_SDIO_1_ASYNC  = (1<<17),
-	IRQ_SDIO_3        = (1<<18),
-	IRQ_SDIO_3_ASYNC  = (1<<19),
-	IRQ_DEBUG_RECV    = (1<<20),
-	IRQ_DEBUG_SEND    = (1<<21),
-	IRQ_RSA           = (1<<22),
-	IRQ_CTR_CARD_1    = (1<<23),
-	IRQ_CTR_CARD_2    = (1<<24),
-	IRQ_CGC           = (1<<25),
-	IRQ_CGC_DET       = (1<<26),
-	IRQ_DS_CARD       = (1<<27),
-	IRQ_DMAC_2        = (1<<28),
-	IRQ_DMAC_2_ABORT  = (1<<29)
+	IRQ_DMAC_1_0      = 0,  // DMAC_1 = NDMA
+	IRQ_DMAC_1_1      = 1,
+	IRQ_DMAC_1_2      = 2,
+	IRQ_DMAC_1_3      = 3,
+	IRQ_DMAC_1_4      = 4,
+	IRQ_DMAC_1_5      = 5,
+	IRQ_DMAC_1_6      = 6,
+	IRQ_DMAC_1_7      = 7,
+	IRQ_TIMER_0       = 8,
+	IRQ_TIMER_1       = 9,
+	IRQ_TIMER_2       = 10,
+	IRQ_TIMER_3       = 11,
+	IRQ_PXI_SYNC      = 12,
+	IRQ_PXI_NOT_FULL  = 13,
+	IRQ_PXI_NOT_EMPTY = 14,
+	IRQ_AES           = 15,
+	IRQ_SDIO_1        = 16,
+	IRQ_SDIO_1_ASYNC  = 17,
+	IRQ_SDIO_3        = 18,
+	IRQ_SDIO_3_ASYNC  = 19,
+	IRQ_DEBUG_RECV    = 20,
+	IRQ_DEBUG_SEND    = 21,
+	IRQ_RSA           = 22,
+	IRQ_CTR_CARD_1    = 23,
+	IRQ_CTR_CARD_2    = 24,
+	IRQ_CGC           = 25,
+	IRQ_CGC_DET       = 26,
+	IRQ_DS_CARD       = 27,
+	IRQ_DMAC_2        = 28,
+	IRQ_DMAC_2_ABORT  = 29
 } Interrupt;
+
+
+
+void IRQ_init(void);
+void IRQ_registerHandler(Interrupt num, void (*irqHandler)(void));
+void IRQ_unregisterHandler(Interrupt num);
+
+static void enableIrq(void)
+{
+	u32 tmp;
+	__asm__ __volatile__("mrs %0, cpsr\n\t" : "=r" (tmp) : );
+	__asm__ __volatile__("msr cpsr_c, %0\n\t" : : "r" (tmp & ~(0x80u)));
+}
+
+static void disableIrq(void)
+{
+	u32 tmp;
+	__asm__ __volatile__("mrs %0, cpsr\n\t" : "=r" (tmp) : );
+	__asm__ __volatile__("msr cpsr_c, %0\n\t" : : "r" (tmp | 0x80u));
+}
diff --git a/source/arm9/cpu.s b/source/arm9/cpu.s
index 4a4e2a2..bcb35a4 100644
--- a/source/arm9/cpu.s
+++ b/source/arm9/cpu.s
@@ -16,6 +16,7 @@
 .extern undefInstrHandler
 .extern prefetchAbortHandler
 .extern dataAbortHandler
+.extern irqHandler
 .extern flushDCache
 
 .section ".init"
@@ -48,7 +49,12 @@
 	bl setupExceptionVectors    @ Setup the vectors in ARM9 mem bootrom vectors jump to
 	bl setupMpu
 	bl setupTcms                @ Setup and enable DTCM and ITCM
+
+	msr cpsr_cxsf, #0xD2        @ IRQ mode
+	ldr sp, =(A9_STACK_END - 0x3000)
+	msr cpsr_cxsf, #0xDF
 	ldr sp, =A9_STACK_END
+
 	bx r10
 .pool
 
@@ -63,20 +69,20 @@
 	ldmia r0, {r2-r5}
 	stmia r1, {r2-r5}
 	bx lr
-.pool
 __vectorStubs:
-	.word MAKE_BRANCH(A9_VECTORS_START + 0x00, A9_VECTORS_START + 0x00) // IRQ
-	.word 0
-	.word MAKE_BRANCH(A9_VECTORS_START + 0x08, A9_VECTORS_START + 0x08) // FIQ
-	.word 0
-	.word MAKE_BRANCH(A9_VECTORS_START + 0x10, A9_VECTORS_START + 0x10) // SVC
-	.word 0
+	ldr pc, irqHandlerPtr
+	irqHandlerPtr:                  .word irqHandler
+	ldr pc, fiqHandlerPtr
+	fiqHandlerPtr:                  .word (A9_VECTORS_START + 0x08)
+	ldr pc, svcHandlerPtr
+	svcHandlerPtr:                  .word (A9_VECTORS_START + 0x10)
 	ldr pc, undefInstrHandlerPtr
 	undefInstrHandlerPtr:           .word undefInstrHandler
 	ldr pc, prefetchAbortHandlerPtr
 	prefetchAbortHandlerPtr:        .word prefetchAbortHandler
 	ldr pc, dataAbortHandlerPtr
 	dataAbortHandlerPtr:            .word dataAbortHandler
+.pool
 
 
 setupTcms:
diff --git a/source/arm9/crypto.c b/source/arm9/crypto.c
index 4848801..e319ae2 100644
--- a/source/arm9/crypto.c
+++ b/source/arm9/crypto.c
@@ -193,11 +193,10 @@
 		// Setup the AES engine and wait for it to finish
 		REG_AESBLKCNT = (blockSize>>4)<<16;
 		REG_AESCNT = aesParams | (aesDmaFifoSize<<14) | ((3 - aesDmaFifoSize)<<12);
-		while(!(REG_IRQ_IF & (u32)IRQ_AES))
+		while(REG_AESCNT & AES_ENABLE)
 		{
 			waitForIrq();
 		}
-		REG_IRQ_IF = (u32)IRQ_AES; // Aknowledge interrupt
 
 
 		if(mode == 2) // AES_MODE_CTR
diff --git a/source/arm9/exception.s b/source/arm9/exception.s
index 5b8eb82..9164794 100644
--- a/source/arm9/exception.s
+++ b/source/arm9/exception.s
@@ -7,14 +7,17 @@
 .global undefInstrHandler
 .global prefetchAbortHandler
 .global dataAbortHandler
+.global irqHandler
 
 .type undefInstrHandler STT_FUNC
 .type prefetchAbortHandler STT_FUNC
 .type dataAbortHandler STT_FUNC
 .type exceptionHandler STT_FUNC
+.type irqHandler STT_FUNC
 
 .extern deinitCpu
 .extern guruMeditation
+.extern irqHandlerTable
 
 .section ".text"
 
@@ -42,3 +45,26 @@
 	mov sp, r6
 	mov r1, r6
 	b guruMeditation            @ r0 = exception type, r1 = reg dump ptr {cpsr, pc (unmodified), r0-r14}
+.pool
+
+
+irqHandler:
+	stmfd sp!, {r0-r3, r12, lr}
+	ldr r12, =0x10001000        //REG_IRQ_IE
+	ldmia r12, {r1, r2}
+	and r1, r1, r2
+	mov r3, #0x80000000
+irqHandler_find_first_lp:
+	clz r0, r1
+	bics r1, r1, r3, lsr r0
+	bne irqHandler_find_first_lp
+	mov r1, r3, lsr r0
+	str r1, [r12, #4]           @ REG_IRQ_IF
+	rsb r2, r0, #31             @ r2 = 31 - r0
+	ldr r1, =irqHandlerTable
+	ldr r0, [r1, r2, lsl #2]
+	cmp r0, #0
+	blxne r0
+	ldmfd sp!, {r0-r3, r12, lr}
+	subs pc, lr, #4
+.pool
diff --git a/source/arm9/interrupt.c b/source/arm9/interrupt.c
new file mode 100644
index 0000000..6061d7d
--- /dev/null
+++ b/source/arm9/interrupt.c
@@ -0,0 +1,36 @@
+#include "types.h"
+#include "arm9/interrupt.h"
+
+
+void (*irqHandlerTable[32])(void);
+
+
+
+void IRQ_init(void)
+{
+	// Atomic IRQ disable and aknowledge
+	__asm__ __volatile__("stmia %0, {%1, %2}" : : "r" (&REG_IRQ_IE), "r" (0u), "r" (0xFFFFFFFFu) : "memory");
+
+	for(u32 i = 0; i < 32; i++)
+	{
+		irqHandlerTable[i] = (void (*)(void))NULL;
+	}
+
+	enableIrq();
+}
+
+void IRQ_registerHandler(Interrupt num, void (*irqHandler)(void))
+{
+	disableIrq();
+	irqHandlerTable[num] = irqHandler;
+	REG_IRQ_IE |= (1u<<num);
+	enableIrq();
+}
+
+void IRQ_unregisterHandler(Interrupt num)
+{
+	disableIrq();
+	REG_IRQ_IE &= ~(1u<<num);
+	irqHandlerTable[num] = (void (*)(void))NULL;
+	enableIrq();
+}
diff --git a/source/arm9/loader_init.c b/source/arm9/loader_init.c
index 8b87e3e..7891104 100644
--- a/source/arm9/loader_init.c
+++ b/source/arm9/loader_init.c
@@ -8,8 +8,7 @@
 
 void hardwareInit(void)
 {
-	// Atomic IRQ disable and aknowledge
-	__asm__ __volatile__("stmia %0, {%1, %2}" : : "r" (&REG_IRQ_IE), "r" (0u), "r" (0xFFFFFFFFu) : "memory");
+	IRQ_init();
 	NDMA_init();
 	TIMER_init();
 	PXI_init();
diff --git a/source/arm9/ndma.c b/source/arm9/ndma.c
index aed976e..87711a3 100644
--- a/source/arm9/ndma.c
+++ b/source/arm9/ndma.c
@@ -20,8 +20,8 @@
 		REG_NDMA_CNT(i) = 0;
 	}
 
-	REG_IRQ_IE |= (IRQ_DMAC_1_0 | IRQ_DMAC_1_1 | IRQ_DMAC_1_2 | IRQ_DMAC_1_3 |
-					IRQ_DMAC_1_4 | IRQ_DMAC_1_5 | IRQ_DMAC_1_6 | IRQ_DMAC_1_7);
+	REG_IRQ_IE |= ((1u<<IRQ_DMAC_1_0) | (1u<<IRQ_DMAC_1_1) | (1u<<IRQ_DMAC_1_2) | (1u<<IRQ_DMAC_1_3) |
+					(1u<<IRQ_DMAC_1_4) | (1u<<IRQ_DMAC_1_5) | (1u<<IRQ_DMAC_1_6) | (1u<<IRQ_DMAC_1_7));
 }
 
 void NDMA_copy(u32 *dest, const u32 *source, u32 num)
@@ -35,11 +35,10 @@
 	REG_NDMA7_BLOCK_CNT = NDMA_BLOCK_SYS_FREQ;
 	REG_NDMA7_CNT = NDMA_ENABLE | NDMA_IRQ_ENABLE | NDMA_IMMEDIATE_MODE | NDMA_SRC_UPDATE_INC | NDMA_DST_UPDATE_INC;
 
-	while(!(REG_IRQ_IF & (u32)IRQ_DMAC_1_7))
+	while(REG_NDMA7_CNT & NDMA_ENABLE)
 	{
 		waitForIrq();
 	}
-	REG_IRQ_IF = (u32)IRQ_DMAC_1_7;
 }
 
 void NDMA_fill(u32 *dest, u32 value, u32 num)
@@ -52,9 +51,8 @@
 	REG_NDMA7_FILL_DATA = value;
 	REG_NDMA7_CNT = NDMA_ENABLE | NDMA_IRQ_ENABLE | NDMA_IMMEDIATE_MODE | NDMA_SRC_UPDATE_FILL | NDMA_DST_UPDATE_INC;
 
-	while(!(REG_IRQ_IF & (u32)IRQ_DMAC_1_7))
+	while(REG_NDMA7_CNT & NDMA_ENABLE)
 	{
 		waitForIrq();
 	}
-	REG_IRQ_IF = (u32)IRQ_DMAC_1_7;
 }
diff --git a/source/arm9/pxi.c b/source/arm9/pxi.c
index 428b2a9..1158e8d 100644
--- a/source/arm9/pxi.c
+++ b/source/arm9/pxi.c
@@ -12,7 +12,7 @@
 	REG_PXI_SYNC9 |= 9u<<8;
 	while((REG_PXI_SYNC9 & 0xFFu) != 11u);
 
-	REG_IRQ_IE |= (IRQ_PXI_SYNC | IRQ_PXI_NOT_FULL | IRQ_PXI_NOT_EMPTY);
+	REG_IRQ_IE |= ((1u<<IRQ_PXI_SYNC) | (1u<<IRQ_PXI_NOT_FULL) | (1u<<IRQ_PXI_NOT_EMPTY));
 }
 
 void PXI_sendWord(u32 val)
diff --git a/source/arm9/timer.c b/source/arm9/timer.c
index 05c80f5..ad3bdf4 100644
--- a/source/arm9/timer.c
+++ b/source/arm9/timer.c
@@ -11,7 +11,7 @@
 		REG_TIMER_CNT(i) = 0;
 	}
 
-	REG_IRQ_IE |= (IRQ_TIMER_0 | IRQ_TIMER_1 | IRQ_TIMER_2 | IRQ_TIMER_3);
+	REG_IRQ_IE |= ((1u<<IRQ_TIMER_0) | (1u<<IRQ_TIMER_1) | (1u<<IRQ_TIMER_2) | (1u<<IRQ_TIMER_3));
 }
 
 void TIMER_start(Timer timer, TimerPrescaler prescaler, u16 ticks, bool enableIrq)
@@ -23,7 +23,12 @@
 void TIMER_stop(Timer timer)
 {
 	REG_TIMER_CNT(timer) = 0;
-	REG_IRQ_IF = ((u32)IRQ_TIMER_0<<timer);
+}
+
+static void timerSleepHandler(void)
+{
+	REG_TIMER3_CNT = REG_TIMER2_CNT = 0;
+	IRQ_unregisterHandler(IRQ_TIMER_3);
 }
 
 void _timerSleep(u32 ticks)
@@ -31,15 +36,14 @@
 	REG_TIMER2_VAL = (u16)ticks;
 	REG_TIMER3_VAL = (u16)(ticks>>16);
 
+	IRQ_registerHandler(IRQ_TIMER_3, timerSleepHandler);
+
 	REG_TIMER3_CNT = TIMER_ENABLE | TIMER_IRQ_ENABLE | TIMER_COUNT_UP;
 	REG_TIMER2_CNT = TIMER_ENABLE | TIMER_PRESCALER_1024;
 
-	while(!(REG_IRQ_IF & (u32)IRQ_TIMER_3))
+	while(REG_TIMER3_CNT & TIMER_ENABLE)
 	{
 		waitForIrq();
 	}
-
-	REG_TIMER3_CNT = REG_TIMER2_CNT = 0;
-	REG_IRQ_IF = (u32)IRQ_TIMER_3;
 }