diff --git a/arm11/Makefile b/arm11/Makefile
index e1308a4..3618de3 100644
--- a/arm11/Makefile
+++ b/arm11/Makefile
@@ -35,13 +35,13 @@
 #---------------------------------------------------------------------------------
 ARCH	:=	-march=armv6k -mtune=mpcore -mfloat-abi=hard -mtp=soft -marm -mthumb-interwork
 
-CFLAGS	:=	$(ARCH) -std=c11 -O2 -g -flto -fstrict-aliasing -mword-relocations -fomit-frame-pointer \
-			-ffast-math -ffunction-sections -Wall -Wextra -Wno-unused-function
+CFLAGS	:=	$(ARCH) -std=c11 -O2 -g -flto -mword-relocations -ffunction-sections \
+			-Wall -Wextra -Wno-unused-function
 
 CFLAGS	+=	$(INCLUDE) $(DEFINES)
 
-CXXFLAGS	:=	$(ARCH) -std=c11 -O2 -g -flto -fno-rtti -fno-exceptions -fstrict-aliasing \
-				-mword-relocations -fomit-frame-pointer -ffast-math -ffunction-sections -Wall -Wextra \
+CXXFLAGS	:=	$(ARCH) -std=c++14 -O2 -g -flto -fno-rtti -fno-exceptions \
+				-mword-relocations -ffunction-sections -Wall -Wextra \
 				-Wno-unused-function
 
 CXXFLAGS	+=	$(INCLUDE) $(DEFINES)
diff --git a/arm9/Makefile b/arm9/Makefile
index 8cabf4d..f202795 100644
--- a/arm9/Makefile
+++ b/arm9/Makefile
@@ -34,13 +34,14 @@
 #---------------------------------------------------------------------------------
 ARCH	:=	-march=armv5te -mtune=arm946e-s -mfloat-abi=soft -mtp=soft -marm -mthumb-interwork
 
-CFLAGS	:=	$(ARCH) -std=c11 -O2 -g -flto -fstrict-aliasing -mword-relocations -fomit-frame-pointer \
-			-ffast-math -ffunction-sections -Wall -Wextra
+CFLAGS	:=	$(ARCH) -std=c11 -O2 -g -flto -mword-relocations -ffunction-sections \
+			-Wall -Wextra -Wno-unused-function
 
 CFLAGS	+=	$(INCLUDE) $(DEFINES)
 
-CXXFLAGS	:=	$(ARCH) -std=c11 -O2 -g -flto -fno-rtti -fno-exceptions -fstrict-aliasing \
-				-mword-relocations -fomit-frame-pointer -ffast-math -ffunction-sections -Wall -Wextra
+CXXFLAGS	:=	$(ARCH) -std=c++14 -O2 -g -flto -fno-rtti -fno-exceptions \
+				-mword-relocations -ffunction-sections -Wall -Wextra \
+				-Wno-unused-function
 
 CXXFLAGS	+=	$(INCLUDE) $(DEFINES)
 
diff --git a/source/arm11/hardware/i2c.c b/source/arm11/hardware/i2c.c
index a40a275..ac816e5 100644
--- a/source/arm11/hardware/i2c.c
+++ b/source/arm11/hardware/i2c.c
@@ -35,30 +35,37 @@
 	vu16 I2C_SCL;
 } I2cRegs;
 
+enum
+{
+	I2C_BUS1 = 0,
+	I2C_BUS2 = 1,
+	I2C_BUS3 = 2
+};
+
 static const struct
 {
 	u8 busId;
 	u8 devAddr;
 } i2cDevTable[] =
 {
-	{0, 0x4A},
-	{0, 0x7A},
-	{0, 0x78},
-	{1, 0x4A},
-	{1, 0x78},
-	{1, 0x2C},
-	{1, 0x2E},
-	{1, 0x40},
-	{1, 0x44},
-	{2, 0xA6}, // TODO: Find out if 0xA6 or 0xD6 is correct
-	{2, 0xD0},
-	{2, 0xD2},
-	{2, 0xA4},
-	{2, 0x9A},
-	{2, 0xA0},
-	{1, 0xEE},
-	{0, 0x40},
-	{2, 0x54}
+	{I2C_BUS1, 0x4A},
+	{I2C_BUS1, 0x7A},
+	{I2C_BUS1, 0x78},
+	{I2C_BUS2, 0x4A},
+	{I2C_BUS2, 0x78},
+	{I2C_BUS2, 0x2C},
+	{I2C_BUS2, 0x2E},
+	{I2C_BUS2, 0x40},
+	{I2C_BUS2, 0x44},
+	{I2C_BUS3, 0xA6}, // TODO: Find out if 0xA6 or 0xD6 is correct
+	{I2C_BUS3, 0xD0},
+	{I2C_BUS3, 0xD2},
+	{I2C_BUS3, 0xA4},
+	{I2C_BUS3, 0x9A},
+	{I2C_BUS3, 0xA0},
+	{I2C_BUS2, 0xEE},
+	{I2C_BUS1, 0x40},
+	{I2C_BUS3, 0x54}
 };
 
 
@@ -68,13 +75,13 @@
 	I2cRegs *base;
 	switch(busId)
 	{
-		case 0:
+		case I2C_BUS1:
 			base = (I2cRegs*)I2C1_REGS_BASE;
 			break;
-		case 1:
+		case I2C_BUS2:
 			base = (I2cRegs*)I2C2_REGS_BASE;
 			break;
-		case 2:
+		case I2C_BUS3:
 			base = (I2cRegs*)I2C3_REGS_BASE;
 			break;
 		default:
@@ -84,12 +91,7 @@
 	return base;
 }
 
-static void i2cWaitBusy(I2cRegs *const regs)
-{
-	while(regs->I2C_CNT & I2C_ENABLE);
-}
-
-static void i2cWaitBusyIrq(I2cRegs *const regs)
+static void i2cWaitBusyIrq(const I2cRegs *const regs)
 {
 	while(regs->I2C_CNT & I2C_ENABLE) __wfi();
 }
@@ -106,18 +108,18 @@
 	IRQ_registerHandler(IRQ_I2C2, 14, 0, true, NULL);
 	IRQ_registerHandler(IRQ_I2C3, 14, 0, true, NULL);
 
-	I2cRegs *regs = i2cGetBusRegsBase(0); // Bus 1
-	i2cWaitBusy(regs);
+	I2cRegs *regs = i2cGetBusRegsBase(I2C_BUS1);
+	while(regs->I2C_CNT & I2C_ENABLE);
 	regs->I2C_CNTEX = 2;  // TODO: Find out what this does. If bit 1 is not set I2C hangs.
 	regs->I2C_SCL = 1280; // TODO: How to calculate the frequency.
 
-	regs = i2cGetBusRegsBase(1); // Bus 2
-	i2cWaitBusy(regs);
+	regs = i2cGetBusRegsBase(I2C_BUS2);
+	while(regs->I2C_CNT & I2C_ENABLE);
 	regs->I2C_CNTEX = 2;
 	regs->I2C_SCL = 1280;
 
-	regs = i2cGetBusRegsBase(2); // Bus 3
-	i2cWaitBusy(regs);
+	regs = i2cGetBusRegsBase(I2C_BUS3);
+	while(regs->I2C_CNT & I2C_ENABLE);
 	regs->I2C_CNTEX = 2;
 	regs->I2C_SCL = 1280;
 }
@@ -127,7 +129,10 @@
 	u32 tries = 8;
 	do
 	{
-		i2cWaitBusyIrq(regs);
+		// This is a special case where we can't predict when or if
+		// the IRQ has already fired. If it fires after checking but
+		// before a wfi this would hang.
+		while(regs->I2C_CNT & I2C_ENABLE) __wfe();
 
 		// Select device and start.
 		regs->I2C_DATA = devAddr;
diff --git a/source/arm11/system.c b/source/arm11/system.c
index c933dce..3b0d7a4 100644
--- a/source/arm11/system.c
+++ b/source/arm11/system.c
@@ -29,25 +29,17 @@
 
 
 
-static void systemRestoreHwState(void)
-{
-	MCU_disableLEDs();
-}
-
 void WEAK __systemInit(void)
 {
 	IRQ_init();
 	TIMER_init();
 
-	const u32 cpuId = __getCpuId();
-	if(!cpuId)
+	if(!__getCpuId())
 	{
-		IRQ_registerHandler(IRQ_PDN, 0, 0b1111, true, NULL);
 		I2C_init();
 		hidInit();
 		PXI_init();
 		MCU_init();
-		systemRestoreHwState();
 	}
 	else
 	{
@@ -58,9 +50,6 @@
 	}
 
 	__cpsie(i); // Enables interrupts
-#ifdef CORE123_INIT
-	CPU_poweroffCore23();
-#endif
 }
 
 void WEAK __systemDeinit(void)