diff --git a/include/arm11/hardware/interrupt.h b/include/arm11/hardware/interrupt.h
index add54db..912a09f 100644
--- a/include/arm11/hardware/interrupt.h
+++ b/include/arm11/hardware/interrupt.h
@@ -112,12 +112,17 @@
 	__asm__ __volatile__("wfi" : : : "memory");
 }
 
-static inline void enterCriticalSection(void)
+static inline u32 enterCriticalSection(void)
 {
-	__asm__ __volatile__("cpsid i" : : : "memory");
+	u32 tmp;
+	__asm__ __volatile__("mrs %0, cpsr\n"
+	                     "cpsid i" : "=r" (tmp) : : "memory");
+	return tmp & 0x80;
 }
 
-static inline void leaveCriticalSection(void)
+static inline void leaveCriticalSection(u32 oldState)
 {
-	__asm__ __volatile__("cpsie i" : : : "memory");
+	u32 tmp;
+	__asm__ __volatile__("mrs %0, cpsr" : "=r" (tmp) : );
+	__asm__ __volatile__("msr cpsr_c, %0" : : "r" ((tmp & ~0x80u) | oldState) : "memory");
 }
diff --git a/include/arm9/hardware/interrupt.h b/include/arm9/hardware/interrupt.h
index dd6c6eb..e0770e1 100644
--- a/include/arm9/hardware/interrupt.h
+++ b/include/arm9/hardware/interrupt.h
@@ -92,13 +92,13 @@
 {
 	u32 tmp;
 	__asm__ __volatile__("mrs %0, cpsr" : "=r" (tmp) : );
-	__asm__ __volatile__("msr cpsr_c, %0" : : "r" (tmp | 0x80u) : "memory");
-	return tmp & 0x80u;
+	__asm__ __volatile__("msr cpsr_c, %0" : : "r" (tmp | 0x80) : "memory");
+	return tmp & 0x80;
 }
 
 static inline void leaveCriticalSection(u32 oldState)
 {
 	u32 tmp;
 	__asm__ __volatile__("mrs %0, cpsr" : "=r" (tmp) : );
-	__asm__ __volatile__("msr cpsr_c, %0" : : "r" ((tmp & ~(0x80u)) | oldState) : "memory");
+	__asm__ __volatile__("msr cpsr_c, %0" : : "r" ((tmp & ~0x80u) | oldState) : "memory");
 }
diff --git a/source/arm11/hardware/hardware.c b/source/arm11/hardware/hardware.c
index c417d40..53c11dc 100644
--- a/source/arm11/hardware/hardware.c
+++ b/source/arm11/hardware/hardware.c
@@ -34,8 +34,8 @@
 	if(!getCpuId())
 	{
 		I2C_init();
-		PXI_init();
 		hidInit();
+		PXI_init();
 	}
 	else
 	{
@@ -44,7 +44,7 @@
 		((void (*)(void))0x0001004C)();
 	}
 
-	leaveCriticalSection(); // Enables interrupts
+	leaveCriticalSection(0); // Enables interrupts
 }
 
 /*void hardwareDeinit(void)
diff --git a/source/arm11/hardware/i2c.c b/source/arm11/hardware/i2c.c
index 13b874d..771c767 100644
--- a/source/arm11/hardware/i2c.c
+++ b/source/arm11/hardware/i2c.c
@@ -19,7 +19,7 @@
 #include "types.h"
 #include "mem_map.h"
 #include "arm11/hardware/i2c.h"
-#include "arm11/hardware/interrupt.h"
+#include "arm11/spinlock.h"
 
 
 #define I2C1_REGS_BASE  (IO_MEM_ARM9_ARM11 + 0x61000)
@@ -161,14 +161,15 @@
 
 bool I2C_readRegBuf(I2cDevice devId, u8 regAddr, u8 *out, u32 size)
 {
-	enterCriticalSection(); // TODO: Instead of blocking other interrupts we need locks.
+	static u32 lock;
+	spinlockLock(&lock); // TODO: Don't block interrupts.
 	const u8 busId = i2cDevTable[devId].busId;
 	I2cRegs *const regs = i2cGetBusRegsBase(busId);
 
 
 	if(!i2cStartTransfer(devId, regAddr, true, regs))
 	{
-		leaveCriticalSection();
+		spinlockUnlock(&lock);
 		return false;
 	}
 
@@ -183,20 +184,21 @@
 	i2cWaitBusy(regs);
 	*out = regs->REG_I2C_DATA; // Last byte
 
-	leaveCriticalSection();
+	spinlockUnlock(&lock);
 	return true;
 }
 
 bool I2C_writeRegBuf(I2cDevice devId, u8 regAddr, const u8 *in, u32 size)
 {
-	enterCriticalSection(); // TODO: Instead of blocking other interrupts we need locks.
+	static u32 lock;
+	spinlockLock(&lock); // TODO: Don't block interrupts.
 	const u8 busId = i2cDevTable[devId].busId;
 	I2cRegs *const regs = i2cGetBusRegsBase(busId);
 
 
 	if(!i2cStartTransfer(devId, regAddr, false, regs))
 	{
-		leaveCriticalSection();
+		spinlockUnlock(&lock);
 		return false;
 	}
 
@@ -208,7 +210,7 @@
 		if(!I2C_GET_ACK(regs->REG_I2C_CNT)) // If ack flag is 0 it failed.
 		{
 			regs->REG_I2C_CNT = I2C_ENABLE | I2C_IRQ_ENABLE | I2C_ERROR | I2C_STOP;
-			leaveCriticalSection();
+			spinlockUnlock(&lock);
 			return false;
 		}
 	}
@@ -219,11 +221,11 @@
 	if(!I2C_GET_ACK(regs->REG_I2C_CNT)) // If ack flag is 0 it failed.
 	{
 		regs->REG_I2C_CNT = I2C_ENABLE | I2C_IRQ_ENABLE | I2C_ERROR | I2C_STOP;
-		leaveCriticalSection();
+		spinlockUnlock(&lock);
 		return false;
 	}
 
-	leaveCriticalSection();
+	spinlockUnlock(&lock);
 	return true;
 }
 
diff --git a/source/arm11/hardware/interrupt.c b/source/arm11/hardware/interrupt.c
index 0be6e69..844009a 100644
--- a/source/arm11/hardware/interrupt.c
+++ b/source/arm11/hardware/interrupt.c
@@ -133,7 +133,7 @@
 	const u32 cpuId = getCpuId();
 	if(!cpuMask) cpuMask = 1u<<cpuId;
 
-	enterCriticalSection();
+	u32 oldState = enterCriticalSection();
 
 	if(handler)
 	{
@@ -155,30 +155,30 @@
 
 	REGs_GID_ENA_SET[id>>5] = 1u<<(id % 32);
 
-	leaveCriticalSection();
+	leaveCriticalSection(oldState);
 }
 
 void IRQ_unregisterHandler(Interrupt id)
 {
-	enterCriticalSection();
+	u32 oldState = enterCriticalSection();
 
 	REGs_GID_ENA_CLR[id>>5] = 1u<<(id % 32);
 
 	if(id < 32) privIrqHandlerTable[getCpuId()][id] = (IrqHandler)NULL;
 	else irqHandlerTable[id - 32] = (IrqHandler)NULL;
 
-	leaveCriticalSection();
+	leaveCriticalSection(oldState);
 }
 
 void IRQ_setPriority(Interrupt id, u8 prio)
 {
-	enterCriticalSection();
+	u32 oldState = enterCriticalSection();
 
 	u32 shift = (id % 4 * 8) + 4;
 	u32 tmp = REGs_GID_IPRIO[id>>2] & ~(0xFu<<shift);
 	REGs_GID_IPRIO[id>>2] = tmp | (u32)prio<<shift;
 
-	leaveCriticalSection();
+	leaveCriticalSection(oldState);
 }
 
 void IRQ_softwareInterrupt(Interrupt id, u8 cpuMask)
diff --git a/source/arm9/hardware/hardware.c b/source/arm9/hardware/hardware.c
index dabf6b9..69b0e09 100644
--- a/source/arm9/hardware/hardware.c
+++ b/source/arm9/hardware/hardware.c
@@ -34,7 +34,7 @@
 	AES_init();
 	RSA_init();
 
-	leaveCriticalSection(0u); // Enables interrupts
+	leaveCriticalSection(0); // Enables interrupts
 }
 
 void hardwareDeinit(void)