diff --git a/payload_stage1/Makefile b/payload_stage1/Makefile
index bb20211..64867ba 100644
--- a/payload_stage1/Makefile
+++ b/payload_stage1/Makefile
@@ -20,7 +20,7 @@ TARGET 		:=  $(notdir $(CURDIR))
 BUILD		:=	build
 SOURCES		:=	source
 DATA		:=	data
-INCLUDES	:=	include source source/fatfs
+INCLUDES	:=	source 
 
 #---------------------------------------------------------------------------------
 # Setup some defines
@@ -32,11 +32,11 @@ INCLUDES	:=	include source source/fatfs
 ARCH	:=	-mthumb -mthumb-interwork
 
 CFLAGS	:=	-g -Wall -O2\
-			-march=armv5te -mtune=arm946e-s -fomit-frame-pointer\
+			-flto -march=armv5te -mtune=arm946e-s -fomit-frame-pointer\
 			-ffast-math -std=c99\
 			$(ARCH)
 
-CFLAGS	+=	$(INCLUDE)
+CFLAGS	+=	$(INCLUDE) -pedantic -Wall -Wextra -Wcast-align -Wcast-qual -Wdisabled-optimization -Wformat=2 -Winit-self -Wlogical-op -Wmissing-declarations -Wmissing-include-dirs -Wredundant-decls -Wshadow -Wsign-conversion -Wstrict-overflow=5 -Wswitch-default -Wundef -Wno-unused
 
 CXXFLAGS	:= $(CFLAGS) -fno-rtti -fno-exceptions
 
diff --git a/payload_stage1/source/i2c.c b/payload_stage1/source/i2c.c
new file mode 100644
index 0000000..8249d23
--- /dev/null
+++ b/payload_stage1/source/i2c.c
@@ -0,0 +1,98 @@
+#include "i2c.h"
+
+//-----------------------------------------------------------------------------
+
+static const struct { u8 bus_id, reg_addr; } dev_data[] = {
+    {0, 0x4A}, {0, 0x7A}, {0, 0x78},
+    {1, 0x4A}, {1, 0x78}, {1, 0x2C},
+    {1, 0x2E}, {1, 0x40}, {1, 0x44},
+    {2, 0xD6}, {2, 0xD0}, {2, 0xD2},
+    {2, 0xA4}, {2, 0x9A}, {2, 0xA0},
+};
+
+static inline u8 i2cGetDeviceBusId(u8 device_id) {
+    return dev_data[device_id].bus_id;
+}
+
+static inline u8 i2cGetDeviceRegAddr(u8 device_id) {
+    return dev_data[device_id].reg_addr;
+}
+
+//-----------------------------------------------------------------------------
+
+static vu8* reg_data_addrs[] = {
+    (vu8*)(I2C1_REG_OFF + I2C_REG_DATA),
+    (vu8*)(I2C2_REG_OFF + I2C_REG_DATA),
+    (vu8*)(I2C3_REG_OFF + I2C_REG_DATA),
+};
+
+static inline vu8* i2cGetDataReg(u8 bus_id) {
+    return reg_data_addrs[bus_id];
+}
+
+//-----------------------------------------------------------------------------
+
+static vu8* reg_cnt_addrs[] = {
+    (vu8*)(I2C1_REG_OFF + I2C_REG_CNT),
+    (vu8*)(I2C2_REG_OFF + I2C_REG_CNT),
+    (vu8*)(I2C3_REG_OFF + I2C_REG_CNT),
+};
+
+static inline vu8* i2cGetCntReg(u8 bus_id) {
+    return reg_cnt_addrs[bus_id];
+}
+
+//-----------------------------------------------------------------------------
+
+static inline void i2cWaitBusy(u8 bus_id) {
+    while (*i2cGetCntReg(bus_id) & 0x80);
+}
+
+static inline bool i2cGetResult(u8 bus_id) {
+    i2cWaitBusy(bus_id);
+    return (*i2cGetCntReg(bus_id) >> 4) & 1;
+}
+
+static void i2cStop(u8 bus_id, u8 arg0) {
+    *i2cGetCntReg(bus_id) = (arg0 << 5) | 0xC0;
+    i2cWaitBusy(bus_id);
+    *i2cGetCntReg(bus_id) = 0xC5;
+}
+
+//-----------------------------------------------------------------------------
+
+static bool i2cSelectDevice(u8 bus_id, u8 dev_reg) {
+    i2cWaitBusy(bus_id);
+    *i2cGetDataReg(bus_id) = dev_reg;
+    *i2cGetCntReg(bus_id) = 0xC2;
+    return i2cGetResult(bus_id);
+}
+
+static bool i2cSelectRegister(u8 bus_id, u8 reg) {
+    i2cWaitBusy(bus_id);
+    *i2cGetDataReg(bus_id) = reg;
+    *i2cGetCntReg(bus_id) = 0xC0;
+    return i2cGetResult(bus_id);
+}
+
+//-----------------------------------------------------------------------------
+
+u8 i2cReadRegister(u8 dev_id, u8 reg) {
+    u8 bus_id = i2cGetDeviceBusId(dev_id);
+    u8 dev_addr = i2cGetDeviceRegAddr(dev_id);
+
+    for (size_t i = 0; i < 8; i++) {
+        if (i2cSelectDevice(bus_id, dev_addr) && i2cSelectRegister(bus_id, reg)) {
+            if (i2cSelectDevice(bus_id, dev_addr | 1)) {
+                i2cWaitBusy(bus_id);
+                i2cStop(bus_id, 1);
+                i2cWaitBusy(bus_id);
+                return *i2cGetDataReg(bus_id);
+            }
+        }
+        *i2cGetCntReg(bus_id) = 0xC5;
+        i2cWaitBusy(bus_id);
+    }
+    return 0xff;
+}
+
diff --git a/payload_stage1/source/i2c.h b/payload_stage1/source/i2c.h
new file mode 100644
index 0000000..b4b86fe
--- /dev/null
+++ b/payload_stage1/source/i2c.h
@@ -0,0 +1,19 @@
+#pragma once
+
+#include "common.h"
+
+#define I2C1_REG_OFF 0x10161000
+#define I2C2_REG_OFF 0x10144000
+#define I2C3_REG_OFF 0x10148000
+
+#define I2C_REG_DATA  0
+#define I2C_REG_CNT   1
+#define I2C_REG_CNTEX 2
+#define I2C_REG_SCL   4
+
+#define I2C_DEV_MCU  3
+#define I2C_DEV_GYRO 10
+#define I2C_DEV_IR   13
+
+u8 i2cReadRegister(u8 dev_id, u8 reg);
+
diff --git a/payload_stage1/source/main.c b/payload_stage1/source/main.c
index 7cc48c3..628d5cd 100644
--- a/payload_stage1/source/main.c
+++ b/payload_stage1/source/main.c
@@ -1,5 +1,6 @@
 #include "common.h"
 #include "sdmmc.h"
+#include "i2c.h"
 
 int main()
 {
@@ -7,9 +8,20 @@ int main()
     *(vu32*)0x10000020 = 0;
     *(vu32*)0x10000020 = 0x340;
     sdmmc_sdcard_init();
-    sdmmc_nand_readsectors(0x5C000, 0x20, (vu8*)0x08006000);
+
+    if(i2cReadRegister(I2C_DEV_MCU, 0x10) & 0x4) //if home button is pressed
+    {
+        //Jump to alternate stage2
+        sdmmc_nand_readsectors(0x5A000, 0x20, (u8*)0x08006000);
+    }
+    else
+    {
+        //jump to normal stage2
+        sdmmc_nand_readsectors(0x5C000, 0x20, (u8*)0x08006000);
+    }
     
     // Jump to secondary payload
     ((void (*)())0x08006000)();
     return 0;
 }
+
diff --git a/payload_stage1/source/sdmmc.c b/payload_stage1/source/sdmmc.c
index a623dfc..2c3549a 100644
--- a/payload_stage1/source/sdmmc.c
+++ b/payload_stage1/source/sdmmc.c
@@ -7,10 +7,6 @@
 #include "sdmmc.h"
 #include "delay.h"
 
-//Uncomment to enable 32bit fifo support?
-//not currently working
-//#define DATA32_SUPPORT
-
 static inline u16 sdmmc_read16(u16 reg) {
     return *(vu16*)(SDMMC_BASE + reg);
 }
@@ -19,14 +15,6 @@ static inline void sdmmc_write16(u16 reg, u16 val) {
     *(vu16*)(SDMMC_BASE + reg) = val;
 }
 
-static inline u32 sdmmc_read32(u16 reg) {
-    return *(vu32*)(SDMMC_BASE + reg);
-}
-
-static inline void sdmmc_write32(u16 reg, u32 val) {
-    *(vu32*)(SDMMC_BASE + reg) = val;
-}
-
 static inline void sdmmc_mask16(u16 reg, const u16 clear, const u16 set) {
     u16 val = sdmmc_read16(reg);
     val &= ~clear;
@@ -42,23 +30,13 @@ static inline void setckl(u32 data)
 }
 
 static struct mmcdevice handleNAND;
-static struct mmcdevice handleSD;
-
-mmcdevice *getMMCDevice(int drive)
-{
-    if(drive==0) return &handleNAND;
-    return &handleSD;
-}
 
-int __attribute__((noinline)) geterror(struct mmcdevice *ctx)
+static u32 __attribute__((noinline)) geterror(struct mmcdevice *ctx)
 {
-    //if(ctx->error == 0x4) return -1;
-    //else return 0;
     return (ctx->error << 29) >> 31;
 }
 
-
-void __attribute__((noinline)) inittarget(struct mmcdevice *ctx)
+static void __attribute__((noinline)) inittarget(struct mmcdevice *ctx)
 {
     sdmmc_mask16(REG_SDPORTSEL,0x3,(u16)ctx->devicenumber);
     setckl(ctx->clk);
@@ -70,8 +48,7 @@ void __attribute__((noinline)) inittarget(struct mmcdevice *ctx)
 
 }
 
-
-void __attribute__((noinline)) sdmmc_send_command(struct mmcdevice *ctx, u32 cmd, u32 args)
+static void __attribute__((noinline)) sdmmc_send_command(struct mmcdevice *ctx, u32 cmd, u32 args)
 {
     bool getSDRESP = (cmd << 15) >> 31;
     u16 flags = (cmd << 15) >> 31;
@@ -88,29 +65,16 @@ void __attribute__((noinline)) sdmmc_send_command(struct mmcdevice *ctx, u32 cmd
     sdmmc_write16(REG_SDSTATUS0,0);
     sdmmc_write16(REG_SDSTATUS1,0);
 
-#ifdef DATA32_SUPPORT
-    if (readdata)
-        sdmmc_mask16(REG_SDDATACTL32, 0x1000, 0x800);
-    if (writedata)
-        sdmmc_mask16(REG_SDDATACTL32, 0x800, 0x1000);
-#else
     sdmmc_mask16(REG_SDDATACTL32,0x1800,0);
-#endif
 
     sdmmc_write16(REG_SDCMDARG0,args &0xFFFF);
     sdmmc_write16(REG_SDCMDARG1,args >> 16);
     sdmmc_write16(REG_SDCMD,cmd &0xFFFF);
 
     u32 size = ctx->size;
-    u16 *dataPtr = (u16*)ctx->data;
-#ifdef DATA32_SUPPORT
-    u32 *dataPtr32 = (u32*)ctx->data;
-#endif
+    u8 *dataPtr = ctx->data;
 
     bool useBuf = ( NULL != dataPtr );
-#ifdef DATA32_SUPPORT
-    bool useBuf32 = (useBuf && (0 == (3 & ((u32)dataPtr))));
-#endif
 
     u16 status0 = 0;
     while(true) {
@@ -118,19 +82,12 @@ void __attribute__((noinline)) sdmmc_send_command(struct mmcdevice *ctx, u32 cmd
         if (status1 & TMIO_STAT1_RXRDY) {
             if (readdata && useBuf) {
                 sdmmc_mask16(REG_SDSTATUS1, TMIO_STAT1_RXRDY, 0);
-                //sdmmc_write16(REG_SDSTATUS1,~TMIO_STAT1_RXRDY);
                 if (size > 0x1FF) {
-#ifdef DATA32_SUPPORT
-                    if (useBuf32) {
-                        for(int i = 0; i<0x200; i+=4)
-                            *dataPtr32++ = sdmmc_read32(REG_SDFIFO32);
-                    } else {
-#endif
-                        for(int i = 0; i<0x200; i+=2)
-                            *dataPtr++ = sdmmc_read16(REG_SDFIFO);
-#ifdef DATA32_SUPPORT
+                    for(int i = 0; i<0x200; i+=2) {
+                        u16 data = sdmmc_read16(REG_SDFIFO);
+                        *dataPtr++ = (u8)data;
+                        *dataPtr++ = (u8)(data >> 8);
                     }
-#endif
                     size -= 0x200;
                 }
             }
@@ -139,15 +96,12 @@ void __attribute__((noinline)) sdmmc_send_command(struct mmcdevice *ctx, u32 cmd
         if (status1 & TMIO_STAT1_TXRQ) {
             if (writedata && useBuf) {
                 sdmmc_mask16(REG_SDSTATUS1, TMIO_STAT1_TXRQ, 0);
-                //sdmmc_write16(REG_SDSTATUS1,~TMIO_STAT1_TXRQ);
                 if (size > 0x1FF) {
-#ifdef DATA32_SUPPORT
-                    for (int i = 0; i<0x200; i+=4)
-                        sdmmc_write32(REG_SDFIFO32,*dataPtr32++);
-#else
-                    for (int i = 0; i<0x200; i+=2)
-                        sdmmc_write16(REG_SDFIFO,*dataPtr++);
-#endif
+                    for (int i = 0; i<0x200; i+=2) {
+                        u16 data = *dataPtr++;
+                        data |= (u16)(*dataPtr++) << 8;
+                        sdmmc_write16(REG_SDFIFO, data);
+                    }
                     size -= 0x200;
                 }
             }
@@ -174,54 +128,29 @@ void __attribute__((noinline)) sdmmc_send_command(struct mmcdevice *ctx, u32 cmd
     sdmmc_write16(REG_SDSTATUS1,0);
 
     if (getSDRESP != 0) {
-        ctx->ret[0] = sdmmc_read16(REG_SDRESP0) | (sdmmc_read16(REG_SDRESP1) << 16);
-        ctx->ret[1] = sdmmc_read16(REG_SDRESP2) | (sdmmc_read16(REG_SDRESP3) << 16);
-        ctx->ret[2] = sdmmc_read16(REG_SDRESP4) | (sdmmc_read16(REG_SDRESP5) << 16);
-        ctx->ret[3] = sdmmc_read16(REG_SDRESP6) | (sdmmc_read16(REG_SDRESP7) << 16);
+        ctx->ret[0] = (u32)(sdmmc_read16(REG_SDRESP0) | (sdmmc_read16(REG_SDRESP1) << 16));
+        ctx->ret[1] = (u32)(sdmmc_read16(REG_SDRESP2) | (sdmmc_read16(REG_SDRESP3) << 16));
+        ctx->ret[2] = (u32)(sdmmc_read16(REG_SDRESP4) | (sdmmc_read16(REG_SDRESP5) << 16));
+        ctx->ret[3] = (u32)(sdmmc_read16(REG_SDRESP6) | (sdmmc_read16(REG_SDRESP7) << 16));
     }
 }
 
-int __attribute__((noinline)) sdmmc_nand_readsectors(u32 sector_no, u32 numsectors, u8 *out)
+u32 __attribute__((noinline)) sdmmc_nand_readsectors(u32 sector_no, u32 numsectors, u8 *out)
 {
     if (handleNAND.isSDHC == 0)
         sector_no <<= 9;
     inittarget(&handleNAND);
     sdmmc_write16(REG_SDSTOP,0x100);
 
-#ifdef DATA32_SUPPORT
-    sdmmc_write32(REG_SDBLKCOUNT32,numsectors);
-#else
     sdmmc_write16(REG_SDBLKCOUNT,numsectors);
-#endif
 
     handleNAND.data = out;
     handleNAND.size = numsectors << 9;
     sdmmc_send_command(&handleNAND,0x33C12,sector_no);
-    inittarget(&handleSD);
-    return geterror(&handleNAND);
-}
-
-int __attribute__((noinline)) sdmmc_nand_writesectors(u32 sector_no, u32 numsectors, u8 *in) //experimental
-{
-    if (handleNAND.isSDHC == 0)
-        sector_no <<= 9;
-    inittarget(&handleNAND);
-    sdmmc_write16(REG_SDSTOP,0x100);
-
-#ifdef DATA32_SUPPORT
-    sdmmc_write32(REG_SDBLKCOUNT32,numsectors);
-#else
-    sdmmc_write16(REG_SDBLKCOUNT,numsectors);
-#endif
-
-    handleNAND.data = in;
-    handleNAND.size = numsectors << 9;
-    sdmmc_send_command(&handleNAND,0x52C19,sector_no);
-    inittarget(&handleSD);
     return geterror(&handleNAND);
 }
 
-void InitSD()
+static void InitSD()
 {
     //NAND
     handleNAND.isSDHC = 0;
@@ -231,46 +160,13 @@ void InitSD()
     handleNAND.clk = 0x80;
     handleNAND.devicenumber = 1;
 
-    //sdmmc_mask16(0x100,0x800,0);
-    //sdmmc_mask16(0x100,0x1000,0);
-    //sdmmc_mask16(0x100,0x0,0x402);
-    //sdmmc_mask16(0xD8,0x22,0x2);
-    //sdmmc_mask16(0x100,0x2,0);
-    //sdmmc_mask16(0xD8,0x22,0);
-    //sdmmc_write16(0x104,0);
-    //sdmmc_write16(0x108,1);
-    //sdmmc_mask16(REG_SDRESET,1,0); //not in new Version -- nintendo's code does this
-    //sdmmc_mask16(REG_SDRESET,0,1); //not in new Version -- nintendo's code does this
-    //sdmmc_mask16(0x20,0,0x31D);
-    //sdmmc_mask16(0x22,0,0x837F);
-    //sdmmc_mask16(0xFC,0,0xDB);
-    //sdmmc_mask16(0xFE,0,0xDB);
-    ////sdmmc_write16(REG_SDCLKCTL,0x20);
-    ////sdmmc_write16(REG_SDOPT,0x40EE);
-    ////sdmmc_mask16(0x02,0x3,0);
-    //sdmmc_write16(REG_SDCLKCTL,0x40);
-    //sdmmc_write16(REG_SDOPT,0x40EB);
-    //sdmmc_mask16(0x02,0x3,0);
-    //sdmmc_write16(REG_SDBLKLEN,0x200);
-    //sdmmc_write16(REG_SDSTOP,0);
-
     *(vu16*)0x10006100 &= 0xF7FFu; //SDDATACTL32
     *(vu16*)0x10006100 &= 0xEFFFu; //SDDATACTL32
-#ifdef DATA32_SUPPORT
-    *(vu16*)0x10006100 |= 0x402u; //SDDATACTL32
-#else
     *(vu16*)0x10006100 |= 0x402u; //SDDATACTL32
-#endif
     *(vu16*)0x100060D8 = (*(vu16*)0x100060D8 & 0xFFDD) | 2;
-#ifdef DATA32_SUPPORT
-    *(vu16*)0x10006100 &= 0xFFFFu; //SDDATACTL32
-    *(vu16*)0x100060D8 &= 0xFFDFu; //SDDATACTL
-    *(vu16*)0x10006104 = 512; //SDBLKLEN32
-#else
     *(vu16*)0x10006100 &= 0xFFFDu; //SDDATACTL32
     *(vu16*)0x100060D8 &= 0xFFDDu; //SDDATACTL
     *(vu16*)0x10006104 = 0; //SDBLKLEN32
-#endif
     *(vu16*)0x10006108 = 1; //SDBLKCOUNT32
     *(vu16*)0x100060E0 &= 0xFFFEu; //SDRESET
     *(vu16*)0x100060E0 |= 1u; //SDRESET
@@ -279,13 +175,8 @@ void InitSD()
     *(vu16*)0x100060FC |= 0xDBu; //SDCTL_RESERVED7
     *(vu16*)0x100060FE |= 0xDBu; //SDCTL_RESERVED8
     *(vu16*)0x10006002 &= 0xFFFCu; //SDPORTSEL
-#ifdef DATA32_SUPPORT
-    *(vu16*)0x10006024 = 0x20;
-    *(vu16*)0x10006028 = 0x40EE;
-#else
     *(vu16*)0x10006024 = 0x40; //Nintendo sets this to 0x20
     *(vu16*)0x10006028 = 0x40EB; //Nintendo sets this to 0x40EE
-#endif
     *(vu16*)0x10006002 &= 0xFFFCu; ////SDPORTSEL
     *(vu16*)0x10006026 = 512; //SDBLKLEN
     *(vu16*)0x10006008 = 0; //SDSTOP
@@ -299,9 +190,9 @@ static u32 calcSDSize(u8* csd, int type)
         case 0:
             {
                 u32 block_len = csd[9] & 0xf;
-                block_len = 1 << block_len;
-                u32 mult = (csd[4] >> 7) | ((csd[5] & 3) << 1);
-                mult = 1 << (mult + 2);
+                block_len = 1u << block_len;
+                u32 mult = (u32)((csd[4] >> 7) | ((csd[5] & 3) << 1));
+                mult = 1u << (mult + 2);
                 result = csd[8] & 3;
                 result = (result << 8) | csd[7];
                 result = (result << 2) | (csd[6] >> 6);
@@ -314,11 +205,13 @@ static u32 calcSDSize(u8* csd, int type)
             result = (result << 8) | csd[5];
             result = (result + 1) * 1024;
             break;
+        default:
+            break; //Do nothing
     }
     return result;
 }
 
-int Nand_Init()
+static int Nand_Init()
 {
     inittarget(&handleNAND);
     ioDelay(0xF000);
@@ -363,14 +256,12 @@ int Nand_Init()
 
     handleNAND.clk |= 0x200;
 
-    inittarget(&handleSD);
-
     return 0;
 }
 
 int sdmmc_sdcard_init()
 {
     InitSD();
-    Nand_Init();
-    return 0;
+    return Nand_Init();
 }
+
diff --git a/payload_stage1/source/sdmmc.h b/payload_stage1/source/sdmmc.h
index aa37429..0732c61 100644
--- a/payload_stage1/source/sdmmc.h
+++ b/payload_stage1/source/sdmmc.h
@@ -127,11 +127,5 @@ void sdmmc_blktransferinit();*/
 
 int sdmmc_sdcard_init();
 
-int sdmmc_nand_readsectors(u32 sector_no, u32 numsectors, u8 *out);
-int sdmmc_nand_writesectors(u32 sector_no, u32 numsectors, u8 *in);
-
-mmcdevice *getMMCDevice(int drive);
-
-void InitSDMMC();
-int Nand_Init();
+u32 sdmmc_nand_readsectors(u32 sector_no, u32 numsectors, u8 *out);
 
diff --git a/payload_stage2/Makefile b/payload_stage2/Makefile
index 2e0145c..8fa679e 100644
--- a/payload_stage2/Makefile
+++ b/payload_stage2/Makefile
@@ -20,7 +20,7 @@ TARGET 		:=  $(notdir $(CURDIR))
 BUILD		:=	build
 SOURCES		:=	source source/fatfs
 DATA		:=	data
-INCLUDES	:=	include source source/fatfs
+INCLUDES	:=	source source/fatfs
 
 #---------------------------------------------------------------------------------
 # Setup some defines
@@ -33,10 +33,10 @@ ARCH	:=	-mthumb -mthumb-interwork
 
 CFLAGS	:=	-g -Wall -O2\
 			-march=armv5te -mtune=arm946e-s -fomit-frame-pointer\
-			-ffast-math -std=c11\
+			-ffast-math -std=c11 -flto\
 			$(ARCH)
 
-CFLAGS	+=	$(INCLUDE)
+CFLAGS	+=	$(INCLUDE) -pedantic -Wall -Wextra -Wcast-align -Wcast-qual -Wdisabled-optimization -Wformat=2 -Winit-self -Wlogical-op -Wmissing-declarations -Wmissing-include-dirs -Wredundant-decls -Wshadow -Wsign-conversion -Wstrict-overflow=5 -Wswitch-default -Wundef -Wno-unused
 
 CXXFLAGS	:= $(CFLAGS) -fno-rtti -fno-exceptions
 
diff --git a/payload_stage2/source/common.h b/payload_stage2/source/common.h
index b47eb07..ab6ac7a 100644
--- a/payload_stage2/source/common.h
+++ b/payload_stage2/source/common.h
@@ -28,14 +28,14 @@
 
 inline char* strupper(const char* str) {
     char* buffer = (char*)malloc(strlen(str) + 1);
-    for (int i = 0; i < strlen(str); ++i)
+    for (size_t i = 0; i < strlen(str); ++i)
         buffer[i] = toupper((unsigned)str[i]);
     return buffer;
 }
 
 inline char* strlower(const char* str) {
     char* buffer = (char*)malloc(strlen(str) + 1);
-    for (int i = 0; i < strlen(str); ++i)
+    for (size_t i = 0; i < strlen(str); ++i)
         buffer[i] = tolower((unsigned)str[i]);
     return buffer;
 }
diff --git a/payload_stage2/source/fatfs/diskio.c b/payload_stage2/source/fatfs/diskio.c
index c053e55..addad9c 100644
--- a/payload_stage2/source/fatfs/diskio.c
+++ b/payload_stage2/source/fatfs/diskio.c
@@ -64,9 +64,11 @@ DRESULT disk_read (
     {
         switch(pdrv){
         case 0:
-            if (sdmmc_sdcard_readsectors(sector,count,buff))
+            if (sdmmc_sdcard_readsectors(sector, count, buff))
             return RES_PARERR;
             break;
+	default:
+	    return RES_PARERR; //Bad drive parameter given to us
         }
         return RES_OK;
     }
@@ -91,9 +93,11 @@ DRESULT disk_write (
     {
         switch(pdrv){
             case 0:
-                if (sdmmc_sdcard_writesectors(sector,count,buff))
+                if (sdmmc_sdcard_writesectors(sector, count, buff))
                     return RES_PARERR;
                 break;
+	    default:
+	        return RES_PARERR; //Bad drive parameter given to us
         }
         return RES_OK;
     }
diff --git a/payload_stage2/source/fatfs/ff.c b/payload_stage2/source/fatfs/ff.c
index 3041e4c..a43d053 100644
--- a/payload_stage2/source/fatfs/ff.c
+++ b/payload_stage2/source/fatfs/ff.c
@@ -138,9 +138,9 @@ const unsigned short Tbl[] = {  /*  CP437(0x80-0xFF) to Unicode conversion table
     0x00B0, 0x2219, 0x00B7, 0x221A, 0x207F, 0x00B2, 0x25A0, 0x00A0
 };
 
-DWORD get_fattime (void) {
-    //NO
-    return 0;
+static DWORD get_fattime (void) {
+    // fixed valid time of 1980-01-01 00:00:00
+    return 0x210000;
 }
 
 WCHAR ff_convert (  /* Converted character, Returns zero on error */
@@ -908,7 +908,7 @@ FRESULT sync_fs (   /* FR_OK: successful, FR_DISK_ERR: failed */
 /*-----------------------------------------------------------------------*/
 
 
-DWORD clust2sect (  /* !=0: Sector number, 0: Failed - invalid cluster# */
+static DWORD clust2sect (  /* !=0: Sector number, 0: Failed - invalid cluster# */
     FATFS* fs,      /* File system object */
     DWORD clst      /* Cluster# to be converted */
 )
@@ -926,7 +926,7 @@ DWORD clust2sect (  /* !=0: Sector number, 0: Failed - invalid cluster# */
 /*-----------------------------------------------------------------------*/
 
 
-DWORD get_fat ( /* 0xFFFFFFFF:Disk error, 1:Internal error, Else:Cluster status */
+static DWORD get_fat ( /* 0xFFFFFFFF:Disk error, 1:Internal error, Else:Cluster status */
     FATFS* fs,  /* File system object */
     DWORD clst  /* Cluster# to get the link information */
 )
@@ -946,7 +946,7 @@ DWORD get_fat ( /* 0xFFFFFFFF:Disk error, 1:Internal error, Else:Cluster status
         wc = fs->win[bc % SS(fs)];
         bc++;
         if (move_window(fs, fs->fatbase + (bc / SS(fs)))) break;
-        wc |= fs->win[bc % SS(fs)] << 8;
+        wc |= (UINT)(fs->win[bc % SS(fs)] << 8);
         return clst & 1 ? wc >> 4 : (wc & 0xFFF);
 
     case FS_FAT16 :
@@ -1245,7 +1245,7 @@ FRESULT dir_next (  /* FR_OK:Succeeded, FR_NO_FILE:End of table, FR_DENIED:Could
     UINT i;
 
 
-    i = dp->index + 1;
+    i = (UINT)dp->index + 1;
     if (!(i & 0xFFFF) || !dp->sect) /* Report EOT when index has reached 65535 */
         return FR_NO_FILE;
 
@@ -1257,7 +1257,7 @@ FRESULT dir_next (  /* FR_OK:Succeeded, FR_NO_FILE:End of table, FR_DENIED:Could
                 return FR_NO_FILE;
         }
         else {                  /* Dynamic table */
-            if (((i / (SS(dp->fs) / SZ_DIR)) & (dp->fs->csize - 1)) == 0) { /* Cluster changed? */
+            if (((i / (SS(dp->fs) / SZ_DIR)) & (UINT)(dp->fs->csize - 1)) == 0) { /* Cluster changed? */
                 clst = get_fat(dp->fs, dp->clust);              /* Get next cluster */
                 if (clst <= 1) return FR_INT_ERR;
                 if (clst == 0xFFFFFFFF) return FR_DISK_ERR;
@@ -1389,7 +1389,7 @@ int cmp_lfn (           /* 1:Matched, 0:Not matched */
     WCHAR wc, uc;
 
 
-    i = ((dir[LDIR_Ord] & ~LLE) - 1) * 13;  /* Get offset in the LFN buffer */
+    i = (UINT)(((dir[LDIR_Ord] & ~LLE) - 1) * 13);  /* Get offset in the LFN buffer */
     s = 0;
     wc = 1;
     do {
@@ -1421,7 +1421,7 @@ int pick_lfn (          /* 1:Succeeded, 0:Buffer overflow */
     WCHAR wc, uc;
 
 
-    i = ((dir[LDIR_Ord] & 0x3F) - 1) * 13;  /* Offset in the LFN buffer */
+    i = (UINT)(((dir[LDIR_Ord] & 0x3F) - 1) * 13);  /* Offset in the LFN buffer */
 
     s = 0;
     wc = 1;
@@ -2200,7 +2200,7 @@ int get_ldnumber (      /* Returns logical drive number (-1:invalid drive) */
         for (tt = *path; (UINT)*tt >= (_USE_LFN ? ' ' : '!') && *tt != ':'; tt++) ; /* Find ':' in the path */
         if (*tt == ':') {   /* If a ':' is exist in the path name */
             tp = *path;
-            i = *tp++ - '0';
+            i = (UINT)(*tp++ - '0');
             if (i < 10 && tp == tt) {   /* Is there a numeric drive id? */
                 if (i < _VOLUMES) { /* If a drive id is found, get the value and strip it */
                     vol = (int)i;
@@ -2677,7 +2677,7 @@ FRESULT f_read (
 )
 {
     FRESULT res;
-    DWORD clst, sect, remain;
+    DWORD clst, sect = 0, remain;
     UINT rcnt, cc;
     BYTE csect, *rbuff = (BYTE*)buff;
 
@@ -2696,7 +2696,7 @@ FRESULT f_read (
     for ( ;  btr;                               /* Repeat until all data read */
             rbuff += rcnt, fp->fptr += rcnt, *br += rcnt, btr -= rcnt) {
         if ((fp->fptr % SS(fp->fs)) == 0) {     /* On the sector boundary? */
-            csect = (BYTE)(fp->fptr / SS(fp->fs) & (fp->fs->csize - 1));    /* Sector offset in the cluster */
+            csect = (BYTE)(fp->fptr / SS(fp->fs) & (UINT)(fp->fs->csize - 1));    /* Sector offset in the cluster */
             if (!csect) {                       /* On the cluster boundary? */
                 if (fp->fptr == 0) {            /* On the top of the file? */
                     clst = fp->sclust;          /* Follow from the origin */
@@ -2718,7 +2718,7 @@ FRESULT f_read (
             cc = btr / SS(fp->fs);              /* When remaining bytes >= sector size, */
             if (cc) {                           /* Read maximum contiguous sectors directly */
                 if (csect + cc > fp->fs->csize) /* Clip at cluster boundary */
-                    cc = fp->fs->csize - csect;
+                    cc = (UINT)(fp->fs->csize - csect);
                 if (disk_read(fp->fs->drv, rbuff, sect, cc))
                     ABORT(fp->fs, FR_DISK_ERR);
 #if !_FS_READONLY && _FS_MINIMIZE <= 2          /* Replace one of the read sectors with cached data if it contains a dirty sector */
diff --git a/payload_stage2/source/fatfs/ff.h b/payload_stage2/source/fatfs/ff.h
index 0da5ab5..ae17552 100644
--- a/payload_stage2/source/fatfs/ff.h
+++ b/payload_stage2/source/fatfs/ff.h
@@ -332,7 +332,7 @@ int ff_del_syncobj (_SYNC_t sobj);              /* Delete a sync object */
 #define ST_DWORD(ptr,val)   *(DWORD*)(BYTE*)(ptr)=(DWORD)(val)
 #else                   /* Use byte-by-byte access to the FAT structure */
 #define LD_WORD(ptr)        (WORD)(((WORD)*((BYTE*)(ptr)+1)<<8)|(WORD)*(BYTE*)(ptr))
-#define LD_DWORD(ptr)       (DWORD)(((DWORD)*((BYTE*)(ptr)+3)<<24)|((DWORD)*((BYTE*)(ptr)+2)<<16)|((WORD)*((BYTE*)(ptr)+1)<<8)|*(BYTE*)(ptr))
+#define LD_DWORD(ptr)       (DWORD)(((DWORD)*((BYTE*)(ptr)+3)<<24)|((DWORD)*((BYTE*)(ptr)+2)<<16)|(DWORD)((WORD)*((BYTE*)(ptr)+1)<<8)|*(BYTE*)(ptr))
 #define ST_WORD(ptr,val)    *(BYTE*)(ptr)=(BYTE)(val); *((BYTE*)(ptr)+1)=(BYTE)((WORD)(val)>>8)
 #define ST_DWORD(ptr,val)   *(BYTE*)(ptr)=(BYTE)(val); *((BYTE*)(ptr)+1)=(BYTE)((WORD)(val)>>8); *((BYTE*)(ptr)+2)=(BYTE)((DWORD)(val)>>16); *((BYTE*)(ptr)+3)=(BYTE)((DWORD)(val)>>24)
 #endif
diff --git a/payload_stage2/source/i2c.c b/payload_stage2/source/i2c.c
index a848eeb..f0beefd 100644
--- a/payload_stage2/source/i2c.c
+++ b/payload_stage2/source/i2c.c
@@ -2,7 +2,7 @@
 
 //-----------------------------------------------------------------------------
 
-static const struct { u8 bus_id, reg_addr } dev_data[] = {
+static const struct { u8 bus_id, reg_addr; } dev_data[] = {
     {0, 0x4A}, {0, 0x7A}, {0, 0x78},
     {1, 0x4A}, {1, 0x78}, {1, 0x2C},
     {1, 0x2E}, {1, 0x40}, {1, 0x44},
@@ -10,35 +10,35 @@ static const struct { u8 bus_id, reg_addr } dev_data[] = {
     {2, 0xA4}, {2, 0x9A}, {2, 0xA0},
 };
 
-const inline u8 i2cGetDeviceBusId(u8 device_id) {
+inline u8 i2cGetDeviceBusId(u8 device_id) {
     return dev_data[device_id].bus_id;
 }
 
-const inline u8 i2cGetDeviceRegAddr(u8 device_id) {
+inline u8 i2cGetDeviceRegAddr(u8 device_id) {
     return dev_data[device_id].reg_addr;
 }
 
 //-----------------------------------------------------------------------------
 
-static vu8* const reg_data_addrs[] = {
+static vu8* reg_data_addrs[] = {
     (vu8*)(I2C1_REG_OFF + I2C_REG_DATA),
     (vu8*)(I2C2_REG_OFF + I2C_REG_DATA),
     (vu8*)(I2C3_REG_OFF + I2C_REG_DATA),
 };
 
-inline vu8* const i2cGetDataReg(u8 bus_id) {
+inline vu8* i2cGetDataReg(u8 bus_id) {
     return reg_data_addrs[bus_id];
 }
 
 //-----------------------------------------------------------------------------
 
-static vu8* const reg_cnt_addrs[] = {
+static vu8* reg_cnt_addrs[] = {
     (vu8*)(I2C1_REG_OFF + I2C_REG_CNT),
     (vu8*)(I2C2_REG_OFF + I2C_REG_CNT),
     (vu8*)(I2C3_REG_OFF + I2C_REG_CNT),
 };
 
-inline vu8* const i2cGetCntReg(u8 bus_id) {
+inline vu8* i2cGetCntReg(u8 bus_id) {
     return reg_cnt_addrs[bus_id];
 }
 
@@ -113,7 +113,7 @@ bool i2cReadRegisterBuffer(unsigned int dev_id, int reg, u8* buffer, size_t buf_
     }
 
     if (buf_size != 1) {
-        for (int i = 0; i < buf_size - 1; i++) {
+        for (size_t i = 0; i < buf_size - 1; i++) {
             i2cWaitBusy(bus_id);
             *i2cGetCntReg(bus_id) = 0xF0;
             i2cWaitBusy(bus_id);
@@ -146,4 +146,4 @@ bool i2cWriteRegister(u8 dev_id, u8 reg, u8 data) {
     }
 
     return false;
-}
\ No newline at end of file
+}
diff --git a/payload_stage2/source/i2c.h b/payload_stage2/source/i2c.h
index 3e1bfea..f047c17 100644
--- a/payload_stage2/source/i2c.h
+++ b/payload_stage2/source/i2c.h
@@ -15,11 +15,11 @@
 #define I2C_DEV_GYRO 10
 #define I2C_DEV_IR   13
 
-const u8 i2cGetDeviceBusId(u8 device_id);
-const u8 i2cGetDeviceRegAddr(u8 device_id);
+u8 i2cGetDeviceBusId(u8 device_id);
+u8 i2cGetDeviceRegAddr(u8 device_id);
 
-vu8* const i2cGetDataReg(u8 bus_id);
-vu8* const i2cGetCntReg(u8 bus_id);
+vu8* i2cGetDataReg(u8 bus_id);
+vu8* i2cGetCntReg(u8 bus_id);
 
 void i2cWaitBusy(u8 bus_id);
 bool i2cGetResult(u8 bus_id);
@@ -32,4 +32,4 @@ bool i2cSelectRegister(u8 bus_id, u8 reg);
 u8 i2cReadRegister(u8 dev_id, u8 reg);
 bool i2cWriteRegister(u8 dev_id, u8 reg, u8 data);
 
-bool i2cReadRegisterBuffer(unsigned int dev_id, int reg, u8* buffer, size_t buf_size);
\ No newline at end of file
+bool i2cReadRegisterBuffer(unsigned int dev_id, int reg, u8* buffer, size_t buf_size);
diff --git a/payload_stage2/source/main.c b/payload_stage2/source/main.c
index d0bb17c..efd1194 100644
--- a/payload_stage2/source/main.c
+++ b/payload_stage2/source/main.c
@@ -12,7 +12,7 @@
 extern u8 screen_init_bin[];
 extern u32 screen_init_bin_size;
 
-void ownArm11()
+static void ownArm11()
 {
 	memcpy((void*)A11_PAYLOAD_LOC, screen_init_bin, screen_init_bin_size);
 	*((u32*)0x1FFAED80) = 0xE51FF004;
@@ -26,7 +26,7 @@ void ownArm11()
 }
 
 //fixes the snow issue
-void clearScreen(void)
+static void clearScreen(void)
 {
 	for(int i = 0; i < (SCREEN_SIZE); i++)
 	{
@@ -54,17 +54,17 @@ int main()
 
 	FATFS fs;
 	FIL payload;
-	u32 br;
+	unsigned int br;
 	
+	screenInit();
 		
 	f_mount(&fs, "0:", 0); //This never fails due to deferred mounting
 	if(f_open(&payload, "arm9loaderhax.bin", FA_READ | FA_OPEN_EXISTING) == FR_OK)
 	{
 		ownArm11();
 		clearScreen();
-		screenInit();
 
-		f_read(&payload, PAYLOAD_ADDRESS, PAYLOAD_SIZE, &br);
+		f_read(&payload, (void*)PAYLOAD_ADDRESS, PAYLOAD_SIZE, &br);
 		((void (*)())PAYLOAD_ADDRESS)();
 	}
 	
diff --git a/payload_stage2/source/screen_init.c b/payload_stage2/source/screen_init.c
index ee86c8d..69c52f2 100644
--- a/payload_stage2/source/screen_init.c
+++ b/payload_stage2/source/screen_init.c
@@ -1,3 +1,4 @@
+#include "screen_init.h"
 #include "i2c.h"
 
 void screenInit()
diff --git a/payload_stage2/source/screen_init.h b/payload_stage2/source/screen_init.h
index 37bad3b..33afe5c 100644
--- a/payload_stage2/source/screen_init.h
+++ b/payload_stage2/source/screen_init.h
@@ -1 +1,4 @@
+#pragma once 
+
 void screenInit();
+
diff --git a/payload_stage2/source/sdmmc.c b/payload_stage2/source/sdmmc.c
index e5c96d6..720fdab 100644
--- a/payload_stage2/source/sdmmc.c
+++ b/payload_stage2/source/sdmmc.c
@@ -7,9 +7,21 @@
 #include "sdmmc.h"
 #include "delay.h"
 
-//Uncomment to enable 32bit fifo support?
-//not currently working
-//#define DATA32_SUPPORT
+typedef struct mmcdevice {
+    vu8* data;
+    u32 size;
+    u32 error;
+    u16 stat0;
+    u16 stat1;
+    u32 ret[4];
+    u32 initarg;
+    u32 isSDHC;
+    u32 clk;
+    u32 SDOPT;
+    u32 devicenumber;
+    u32 total_size; //size in sectors of the device
+    u32 res;
+} mmcdevice;
 
 static struct mmcdevice handleNAND;
 static struct mmcdevice handleSD;
@@ -45,21 +57,18 @@ static inline void setckl(u32 data)
 }
 
 
-mmcdevice *getMMCDevice(int drive)
+static mmcdevice *getMMCDevice(int drive)
 {
     if(drive==0) return &handleNAND;
     return &handleSD;
 }
 
-int __attribute__((noinline)) geterror(struct mmcdevice *ctx)
+static u32 __attribute__((noinline)) geterror(struct mmcdevice *ctx)
 {
-    //if(ctx->error == 0x4) return -1;
-    //else return 0;
     return (ctx->error << 29) >> 31;
 }
 
-
-void __attribute__((noinline)) inittarget(struct mmcdevice *ctx)
+static void __attribute__((noinline)) inittarget(struct mmcdevice *ctx)
 {
     sdmmc_mask16(REG_SDPORTSEL,0x3,(u16)ctx->devicenumber);
     setckl(ctx->clk);
@@ -71,8 +80,7 @@ void __attribute__((noinline)) inittarget(struct mmcdevice *ctx)
 
 }
 
-
-void __attribute__((noinline)) sdmmc_send_command(struct mmcdevice *ctx, u32 cmd, u32 args)
+static void __attribute__((noinline)) sdmmc_send_command(struct mmcdevice *ctx, u32 cmd, u32 args)
 {
     bool getSDRESP = (cmd << 15) >> 31;
     u16 flags = (cmd << 15) >> 31;
@@ -88,30 +96,16 @@ void __attribute__((noinline)) sdmmc_send_command(struct mmcdevice *ctx, u32 cmd
     sdmmc_write16(REG_SDIRMASK1,0);
     sdmmc_write16(REG_SDSTATUS0,0);
     sdmmc_write16(REG_SDSTATUS1,0);
-
-#ifdef DATA32_SUPPORT
-    if (readdata)
-        sdmmc_mask16(REG_SDDATACTL32, 0x1000, 0x800);
-    if (writedata)
-        sdmmc_mask16(REG_SDDATACTL32, 0x800, 0x1000);
-#else
     sdmmc_mask16(REG_SDDATACTL32,0x1800,0);
-#endif
 
     sdmmc_write16(REG_SDCMDARG0,args &0xFFFF);
     sdmmc_write16(REG_SDCMDARG1,args >> 16);
     sdmmc_write16(REG_SDCMD,cmd &0xFFFF);
 
     u32 size = ctx->size;
-    u16 *dataPtr = (u16*)ctx->data;
-#ifdef DATA32_SUPPORT
-    u32 *dataPtr32 = (u32*)ctx->data;
-#endif
+    vu8 *dataPtr = ctx->data;
 
     bool useBuf = ( NULL != dataPtr );
-#ifdef DATA32_SUPPORT
-    bool useBuf32 = (useBuf && (0 == (3 & ((u32)dataPtr))));
-#endif
 
     u16 status0 = 0;
     while(true) {
@@ -121,17 +115,11 @@ void __attribute__((noinline)) sdmmc_send_command(struct mmcdevice *ctx, u32 cmd
                 sdmmc_mask16(REG_SDSTATUS1, TMIO_STAT1_RXRDY, 0);
                 //sdmmc_write16(REG_SDSTATUS1,~TMIO_STAT1_RXRDY);
                 if (size > 0x1FF) {
-#ifdef DATA32_SUPPORT
-                    if (useBuf32) {
-                        for(int i = 0; i<0x200; i+=4)
-                            *dataPtr32++ = sdmmc_read32(REG_SDFIFO32);
-                    } else {
-#endif
-                        for(int i = 0; i<0x200; i+=2)
-                            *dataPtr++ = sdmmc_read16(REG_SDFIFO);
-#ifdef DATA32_SUPPORT
+                    for(int i = 0; i<0x200; i+=2) {
+                        u16 data = sdmmc_read16(REG_SDFIFO);
+                        *dataPtr++ = data & 0xFF;
+                        *dataPtr++ = data >> 8;
                     }
-#endif
                     size -= 0x200;
                 }
             }
@@ -142,13 +130,11 @@ void __attribute__((noinline)) sdmmc_send_command(struct mmcdevice *ctx, u32 cmd
                 sdmmc_mask16(REG_SDSTATUS1, TMIO_STAT1_TXRQ, 0);
                 //sdmmc_write16(REG_SDSTATUS1,~TMIO_STAT1_TXRQ);
                 if (size > 0x1FF) {
-#ifdef DATA32_SUPPORT
-                    for (int i = 0; i<0x200; i+=4)
-                        sdmmc_write32(REG_SDFIFO32,*dataPtr32++);
-#else
-                    for (int i = 0; i<0x200; i+=2)
-                        sdmmc_write16(REG_SDFIFO,*dataPtr++);
-#endif
+                    for (int i = 0; i<0x200; i+=2) {
+                        u16 data = *dataPtr++;
+                        data |= *dataPtr++ << 8;
+                        sdmmc_write16(REG_SDFIFO, data);
+                    }
                     size -= 0x200;
                 }
             }
@@ -175,24 +161,20 @@ void __attribute__((noinline)) sdmmc_send_command(struct mmcdevice *ctx, u32 cmd
     sdmmc_write16(REG_SDSTATUS1,0);
 
     if (getSDRESP != 0) {
-        ctx->ret[0] = sdmmc_read16(REG_SDRESP0) | (sdmmc_read16(REG_SDRESP1) << 16);
-        ctx->ret[1] = sdmmc_read16(REG_SDRESP2) | (sdmmc_read16(REG_SDRESP3) << 16);
-        ctx->ret[2] = sdmmc_read16(REG_SDRESP4) | (sdmmc_read16(REG_SDRESP5) << 16);
-        ctx->ret[3] = sdmmc_read16(REG_SDRESP6) | (sdmmc_read16(REG_SDRESP7) << 16);
+        ctx->ret[0] = (u32)sdmmc_read16(REG_SDRESP0) | (u32)(sdmmc_read16(REG_SDRESP1) << 16);
+        ctx->ret[1] = (u32)sdmmc_read16(REG_SDRESP2) | (u32)(sdmmc_read16(REG_SDRESP3) << 16);
+        ctx->ret[2] = (u32)sdmmc_read16(REG_SDRESP4) | (u32)(sdmmc_read16(REG_SDRESP5) << 16);
+        ctx->ret[3] = (u32)sdmmc_read16(REG_SDRESP6) | (u32)(sdmmc_read16(REG_SDRESP7) << 16);
     }
 }
 
-int __attribute__((noinline)) sdmmc_sdcard_writesectors(u32 sector_no, u32 numsectors, u8 *in)
+u32 __attribute__((noinline)) sdmmc_sdcard_writesectors(u32 sector_no, u32 numsectors, const vu8 *in)
 {
     if (handleSD.isSDHC == 0)
         sector_no <<= 9;
     inittarget(&handleSD);
     sdmmc_write16(REG_SDSTOP,0x100);
 
-#ifdef DATA32_SUPPORT
-    sdmmc_write16(REG_SDBLKCOUNT32,numsectors);
-#endif
-
     sdmmc_write16(REG_SDBLKCOUNT,numsectors);
     handleSD.data = in;
     handleSD.size = numsectors << 9;
@@ -200,18 +182,13 @@ int __attribute__((noinline)) sdmmc_sdcard_writesectors(u32 sector_no, u32 numse
     return geterror(&handleSD);
 }
 
-int __attribute__((noinline)) sdmmc_sdcard_readsectors(u32 sector_no, u32 numsectors, u8 *out)
+u32 __attribute__((noinline)) sdmmc_sdcard_readsectors(u32 sector_no, u32 numsectors, vu8 *out)
 {
     if (handleSD.isSDHC == 0)
         sector_no <<= 9;
     inittarget(&handleSD);
     sdmmc_write16(REG_SDSTOP,0x100);
 
-#ifdef DATA32_SUPPORT
-    sdmmc_write16(REG_SDBLKCOUNT32,numsectors);
-    sdmmc_write16(REG_SDBLKLEN32,0x200);
-#endif
-
     sdmmc_write16(REG_SDBLKCOUNT,numsectors);
     handleSD.data = out;
     handleSD.size = numsectors << 9;
@@ -219,20 +196,14 @@ int __attribute__((noinline)) sdmmc_sdcard_readsectors(u32 sector_no, u32 numsec
     return geterror(&handleSD);
 }
 
-
-
-int __attribute__((noinline)) sdmmc_nand_readsectors(u32 sector_no, u32 numsectors, u8 *out)
+u32 __attribute__((noinline)) sdmmc_nand_readsectors(u32 sector_no, u32 numsectors, vu8 *out)
 {
     if (handleNAND.isSDHC == 0)
         sector_no <<= 9;
     inittarget(&handleNAND);
     sdmmc_write16(REG_SDSTOP,0x100);
 
-#ifdef DATA32_SUPPORT
-    sdmmc_write32(REG_SDBLKCOUNT32,numsectors);
-#else
     sdmmc_write16(REG_SDBLKCOUNT,numsectors);
-#endif
 
     handleNAND.data = out;
     handleNAND.size = numsectors << 9;
@@ -241,18 +212,14 @@ int __attribute__((noinline)) sdmmc_nand_readsectors(u32 sector_no, u32 numsecto
     return geterror(&handleNAND);
 }
 
-int __attribute__((noinline)) sdmmc_nand_writesectors(u32 sector_no, u32 numsectors, u8 *in) //experimental
+u32 __attribute__((noinline)) sdmmc_nand_writesectors(u32 sector_no, u32 numsectors, const vu8 *in) //experimental
 {
     if (handleNAND.isSDHC == 0)
         sector_no <<= 9;
     inittarget(&handleNAND);
     sdmmc_write16(REG_SDSTOP,0x100);
 
-#ifdef DATA32_SUPPORT
-    sdmmc_write32(REG_SDBLKCOUNT32,numsectors);
-#else
     sdmmc_write16(REG_SDBLKCOUNT,numsectors);
-#endif
 
     handleNAND.data = in;
     handleNAND.size = numsectors << 9;
@@ -269,9 +236,9 @@ static u32 calcSDSize(u8* csd, int type)
         case 0:
             {
                 u32 block_len = csd[9] & 0xf;
-                block_len = 1 << block_len;
-                u32 mult = (csd[4] >> 7) | ((csd[5] & 3) << 1);
-                mult = 1 << (mult + 2);
+                block_len = 1u << block_len;
+                u32 mult = (u32)(csd[4] >> 7) | (u32)((csd[5] & 3) << 1);
+                mult = 1u << (mult + 2);
                 result = csd[8] & 3;
                 result = (result << 8) | csd[7];
                 result = (result << 2) | (csd[6] >> 6);
@@ -284,11 +251,13 @@ static u32 calcSDSize(u8* csd, int type)
             result = (result << 8) | csd[5];
             result = (result + 1) * 1024;
             break;
+    default:
+        break; //Do nothing otherwise
     }
     return result;
 }
 
-void InitSD()
+static void InitSD()
 {
     //NAND
     handleNAND.isSDHC = 0;
@@ -306,46 +275,13 @@ void InitSD()
     handleSD.clk = 0x80;
     handleSD.devicenumber = 0;
 
-    //sdmmc_mask16(0x100,0x800,0);
-    //sdmmc_mask16(0x100,0x1000,0);
-    //sdmmc_mask16(0x100,0x0,0x402);
-    //sdmmc_mask16(0xD8,0x22,0x2);
-    //sdmmc_mask16(0x100,0x2,0);
-    //sdmmc_mask16(0xD8,0x22,0);
-    //sdmmc_write16(0x104,0);
-    //sdmmc_write16(0x108,1);
-    //sdmmc_mask16(REG_SDRESET,1,0); //not in new Version -- nintendo's code does this
-    //sdmmc_mask16(REG_SDRESET,0,1); //not in new Version -- nintendo's code does this
-    //sdmmc_mask16(0x20,0,0x31D);
-    //sdmmc_mask16(0x22,0,0x837F);
-    //sdmmc_mask16(0xFC,0,0xDB);
-    //sdmmc_mask16(0xFE,0,0xDB);
-    ////sdmmc_write16(REG_SDCLKCTL,0x20);
-    ////sdmmc_write16(REG_SDOPT,0x40EE);
-    ////sdmmc_mask16(0x02,0x3,0);
-    //sdmmc_write16(REG_SDCLKCTL,0x40);
-    //sdmmc_write16(REG_SDOPT,0x40EB);
-    //sdmmc_mask16(0x02,0x3,0);
-    //sdmmc_write16(REG_SDBLKLEN,0x200);
-    //sdmmc_write16(REG_SDSTOP,0);
-
     *(vu16*)0x10006100 &= 0xF7FFu; //SDDATACTL32
     *(vu16*)0x10006100 &= 0xEFFFu; //SDDATACTL32
-#ifdef DATA32_SUPPORT
     *(vu16*)0x10006100 |= 0x402u; //SDDATACTL32
-#else
-    *(vu16*)0x10006100 |= 0x402u; //SDDATACTL32
-#endif
     *(vu16*)0x100060D8 = (*(vu16*)0x100060D8 & 0xFFDD) | 2;
-#ifdef DATA32_SUPPORT
-    *(vu16*)0x10006100 &= 0xFFFFu; //SDDATACTL32
-    *(vu16*)0x100060D8 &= 0xFFDFu; //SDDATACTL
-    *(vu16*)0x10006104 = 512; //SDBLKLEN32
-#else
     *(vu16*)0x10006100 &= 0xFFFDu; //SDDATACTL32
     *(vu16*)0x100060D8 &= 0xFFDDu; //SDDATACTL
     *(vu16*)0x10006104 = 0; //SDBLKLEN32
-#endif
     *(vu16*)0x10006108 = 1; //SDBLKCOUNT32
     *(vu16*)0x100060E0 &= 0xFFFEu; //SDRESET
     *(vu16*)0x100060E0 |= 1u; //SDRESET
@@ -354,13 +290,8 @@ void InitSD()
     *(vu16*)0x100060FC |= 0xDBu; //SDCTL_RESERVED7
     *(vu16*)0x100060FE |= 0xDBu; //SDCTL_RESERVED8
     *(vu16*)0x10006002 &= 0xFFFCu; //SDPORTSEL
-#ifdef DATA32_SUPPORT
-    *(vu16*)0x10006024 = 0x20;
-    *(vu16*)0x10006028 = 0x40EE;
-#else
     *(vu16*)0x10006024 = 0x40; //Nintendo sets this to 0x20
     *(vu16*)0x10006028 = 0x40EB; //Nintendo sets this to 0x40EE
-#endif
     *(vu16*)0x10006002 &= 0xFFFCu; ////SDPORTSEL
     *(vu16*)0x10006026 = 512; //SDBLKLEN
     *(vu16*)0x10006008 = 0; //SDSTOP
@@ -368,7 +299,7 @@ void InitSD()
     inittarget(&handleSD);
 }
 
-int Nand_Init()
+static int Nand_Init()
 {
     inittarget(&handleNAND);
     ioDelay(0xF000);
@@ -418,13 +349,11 @@ int Nand_Init()
     return 0;
 }
 
-int SD_Init()
+static int SD_Init()
 {
     inittarget(&handleSD);
 
-    //ioDelay(0x3E8);
-    //ioDelay(0xF000);
-    ioDelay(1u << 18);
+    ioDelay(1u << 18); //Card needs a little bit of time to be detected, it seems
     
     //If not inserted
     if (!(*((vu16*)0x1000601c) & TMIO_STAT0_SIGSTATE)) return -1;
@@ -444,19 +373,11 @@ int SD_Init()
         } while ( !(handleSD.error & 1) );
 
     } while((handleSD.ret[0] & 0x80000000) == 0);
-    //do
-    //{
-    //  sdmmc_send_command(&handleSD,0x10437,handleSD.initarg << 0x10);
-    //  sdmmc_send_command(&handleSD,0x10769,0x00FF8000 | temp);
-    //
-    //}
-    //while(!(handleSD.ret[0] & 0x80000000));
 
     if(!((handleSD.ret[0] >> 30) & 1) || !temp)
         temp2 = 0;
 
     handleSD.isSDHC = temp2;
-    //handleSD.isSDHC = (handleSD.ret[0] & 0x40000000);
 
     sdmmc_send_command(&handleSD,0x10602,0);
     if (handleSD.error & 0x4) return -1;
@@ -495,6 +416,7 @@ int SD_Init()
 int sdmmc_sdcard_init()
 {
     InitSD();
-    Nand_Init();
-    return SD_Init();
+    int result = Nand_Init();
+    return result | SD_Init();
 }
+
diff --git a/payload_stage2/source/sdmmc.h b/payload_stage2/source/sdmmc.h
index c8efed9..ea2471a 100644
--- a/payload_stage2/source/sdmmc.h
+++ b/payload_stage2/source/sdmmc.h
@@ -102,41 +102,12 @@
 #define TMIO_MASK_READOP  (TMIO_STAT1_RXRDY | TMIO_STAT1_DATAEND)
 #define TMIO_MASK_WRITEOP (TMIO_STAT1_TXRQ | TMIO_STAT1_DATAEND)
 
-typedef struct mmcdevice {
-    u8* data;
-    u32 size;
-    u32 error;
-    u16 stat0;
-    u16 stat1;
-    u32 ret[4];
-    u32 initarg;
-    u32 isSDHC;
-    u32 clk;
-    u32 SDOPT;
-    u32 devicenumber;
-    u32 total_size; //size in sectors of the device
-    u32 res;
-} mmcdevice;
-
-/*int sdmmc_sdcard_init();
-void sdmmc_sdcard_readsector(uint32_t sector_no, void *out);
-void sdmmc_sdcard_readsectors(uint32_t sector_no, uint32_t numsectors, void *out);
-void sdmmc_sdcard_writesector(uint32_t sector_no, void *in);
-void sdmmc_sdcard_writesectors(uint32_t sector_no, uint32_t numsectors, void *in);
-void sdmmc_blktransferinit();*/
-
 int sdmmc_sdcard_init();
-int sdmmc_sdcard_readsector(u32 sector_no, u8 *out);
-int sdmmc_sdcard_readsectors(u32 sector_no, u32 numsectors, u8 *out);
-int sdmmc_sdcard_writesector(u32 sector_no, u8 *in);
-int sdmmc_sdcard_writesectors(u32 sector_no, u32 numsectors, u8 *in);
-
-int sdmmc_nand_readsectors(u32 sector_no, u32 numsectors, u8 *out);
-int sdmmc_nand_writesectors(u32 sector_no, u32 numsectors, u8 *in);
-
-mmcdevice *getMMCDevice(int drive);
+u32 sdmmc_sdcard_readsector(u32 sector_no, vu32 *out);
+u32 sdmmc_sdcard_readsectors(u32 sector_no, u32 numsectors, vu8 *out);
+u32 sdmmc_sdcard_writesector(u32 sector_no, const vu32 *in);
+u32 sdmmc_sdcard_writesectors(u32 sector_no, u32 numsectors, const vu8 *in);
 
-void InitSDMMC();
-int Nand_Init();
-int SD_Init();
+u32 sdmmc_nand_readsectors(u32 sector_no, u32 numsectors, vu8 *out);
+u32 sdmmc_nand_writesectors(u32 sector_no, u32 numsectors, const vu8 *in);
 
diff --git a/screen_init/source/screen_init/screen_init.c b/screen_init/source/screen_init/screen_init.c
index 5faf97e..986d6c0 100644
--- a/screen_init/source/screen_init/screen_init.c
+++ b/screen_init/source/screen_init/screen_init.c
@@ -5,13 +5,14 @@
 #define FB_TOP_RIGHT 0x18300000
 #define FB_BOTTOM 0x18346500
 
+static inline void regSet();
 void __attribute__ ((naked)) a11Entry()
 {
     __asm__ ("ldr r0,=_stack\n\t mov sp, r0");
     regSet();
 }
 
-void regSet()
+static inline void regSet()
 {
 
     volatile uint32_t *entry = (uint32_t *)0x1FFFFFF8;