diff --git a/source/fatfs/sdmmc/common.h b/source/fatfs/sdmmc/common.h
deleted file mode 100644
index 3b77ae6..0000000
--- a/source/fatfs/sdmmc/common.h
+++ /dev/null
@@ -1,3 +0,0 @@
-#pragma once
-
-#include "../../types.h"
\ No newline at end of file
diff --git a/source/fatfs/sdmmc/delay.h b/source/fatfs/sdmmc/delay.h
index afad438..543794a 100644
--- a/source/fatfs/sdmmc/delay.h
+++ b/source/fatfs/sdmmc/delay.h
@@ -1,9 +1,5 @@
-// Copyright 2014 Normmatt
-// Licensed under GPLv2 or any later version
-// Refer to the license.txt file included.
-
 #pragma once
 
-#include "common.h"
+#include "../../types.h"
 
-void ioDelay(u32 us);
+void waitcycles(u32 us);
diff --git a/source/fatfs/sdmmc/delay.s b/source/fatfs/sdmmc/delay.s
index b3baccd..3a2cfdf 100644
--- a/source/fatfs/sdmmc/delay.s
+++ b/source/fatfs/sdmmc/delay.s
@@ -1,17 +1,15 @@
-// Copyright 2014 Normmatt
-// Licensed under GPLv2 or any later version
-// Refer to the license.txt file included.
-
 .arm
-.global ioDelay
-.type   ioDelay STT_FUNC
+.global waitcycles
+.type   waitcycles STT_FUNC
 
-@ioDelay ( u32 us )
-ioDelay:
-	ldr r1, =0x18000000  @ VRAM
-1:
-	@ Loop doing uncached reads from VRAM to make loop timing more reliable
-	ldr r2, [r1]
-	subs r0, #1
-	bgt 1b
-	bx lr
+@waitcycles ( u32 us )
+waitcycles:
+	PUSH    {R0-R2,LR}
+	STR     R0, [SP,#4]
+	waitcycles_loop:
+		LDR     R3, [SP,#4]
+		SUBS    R2, R3, #1
+		STR     R2, [SP,#4]
+		CMP     R3, #0
+		BNE     waitcycles_loop
+	POP     {R0-R2,PC}
diff --git a/source/fatfs/sdmmc/sdmmc.c b/source/fatfs/sdmmc/sdmmc.c
index 9eb7f50..27e2031 100644
--- a/source/fatfs/sdmmc/sdmmc.c
+++ b/source/fatfs/sdmmc/sdmmc.c
@@ -1,6 +1,26 @@
-// Copyright 2014 Normmatt
-// Licensed under GPLv2 or any later version
-// Refer to the license.txt file included.
+/*
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file, 
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Copyright (c) 2014-2015, Normmatt
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 2, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 2 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
 
 #include "sdmmc.h"
 #include "delay.h"
@@ -8,23 +28,28 @@
 struct mmcdevice handleNAND;
 struct mmcdevice handleSD;
 
-static inline u16 sdmmc_read16(u16 reg) {
-    return *(vu16*)(SDMMC_BASE + reg);
+static inline u16 sdmmc_read16(u16 reg)
+{
+    return *(vu16 *)(SDMMC_BASE + reg);
 }
 
-static inline void sdmmc_write16(u16 reg, u16 val) {
-    *(vu16*)(SDMMC_BASE + reg) = val;
+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 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_write32(u16 reg, u32 val)
+{
+    *(vu32 *)(SDMMC_BASE + reg) = val;
 }
 
-static inline void sdmmc_mask16(u16 reg, const u16 clear, const u16 set) {
+static inline void sdmmc_mask16(u16 reg, const u16 clear, const u16 set)
+{
     u16 val = sdmmc_read16(reg);
     val &= ~clear;
     val |= set;
@@ -38,172 +63,215 @@ static inline void setckl(u32 data)
     sdmmc_mask16(REG_SDCLKCTL, 0x0, 0x100);
 }
 
-
 mmcdevice *getMMCDevice(int drive)
 {
-    if(drive==0) return &handleNAND;
+    if(drive == 0) return &handleNAND;
     return &handleSD;
 }
 
-static u32 __attribute__((noinline)) geterror(struct mmcdevice *ctx)
+static int geterror(struct mmcdevice *ctx)
 {
-    return (ctx->error << 29) >> 31;
+    return (int)((ctx->error << 29) >> 31);
 }
 
-static void __attribute__((noinline)) inittarget(struct mmcdevice *ctx)
+static void inittarget(struct mmcdevice *ctx)
 {
-    sdmmc_mask16(REG_SDPORTSEL,0x3,(u16)ctx->devicenumber);
+    sdmmc_mask16(REG_SDPORTSEL, 0x3, (u16)ctx->devicenumber);
     setckl(ctx->clk);
-    if (ctx->SDOPT == 0) {
-        sdmmc_mask16(REG_SDOPT, 0, 0x8000);
-    } else {
-        sdmmc_mask16(REG_SDOPT, 0x8000, 0);
-    }
-
+    if(ctx->SDOPT == 0) sdmmc_mask16(REG_SDOPT, 0, 0x8000);
+    else sdmmc_mask16(REG_SDOPT, 0x8000, 0);
 }
 
-static 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;
+    u32 getSDRESP = (cmd << 15) >> 31;
     u16 flags = (cmd << 15) >> 31;
-    const bool readdata = cmd & 0x20000;
-    const bool writedata = cmd & 0x40000;
+    const int readdata = cmd & 0x20000;
+    const int writedata = cmd & 0x40000;
 
-    if (readdata || writedata)
+    if(readdata || writedata)
         flags |= TMIO_STAT0_DATAEND;
 
     ctx->error = 0;
-    while (sdmmc_read16(REG_SDSTATUS1) & TMIO_STAT1_CMD_BUSY); //mmc working?
-    sdmmc_write16(REG_SDIRMASK0,0);
-    sdmmc_write16(REG_SDIRMASK1,0);
-    sdmmc_write16(REG_SDSTATUS0,0);
-    sdmmc_write16(REG_SDSTATUS1,0);
-    sdmmc_mask16(REG_SDDATACTL32,0x1800,0);
-
-    sdmmc_write16(REG_SDCMDARG0,args &0xFFFF);
-    sdmmc_write16(REG_SDCMDARG1,args >> 16);
-    sdmmc_write16(REG_SDCMD,cmd &0xFFFF);
+    while((sdmmc_read16(REG_SDSTATUS1) & TMIO_STAT1_CMD_BUSY)); //mmc working?
+    sdmmc_write16(REG_SDIRMASK0, 0);
+    sdmmc_write16(REG_SDIRMASK1, 0);
+    sdmmc_write16(REG_SDSTATUS0, 0);
+    sdmmc_write16(REG_SDSTATUS1, 0);
+    sdmmc_mask16(REG_DATACTL32, 0x1800, 0);
+    sdmmc_write16(REG_SDCMDARG0, args & 0xFFFF);
+    sdmmc_write16(REG_SDCMDARG1, args >> 16);
+    sdmmc_write16(REG_SDCMD, cmd & 0xFFFF);
 
     u32 size = ctx->size;
-    vu8 *dataPtr = ctx->data;
+    u8 *rDataPtr = ctx->rData;
+    const u8 *tDataPtr = ctx->tData;
 
-    bool useBuf = ( NULL != dataPtr );
+    int rUseBuf = NULL != rDataPtr;
+    int tUseBuf = NULL != tDataPtr;
 
     u16 status0 = 0;
-    while(true) {
-        u16 status1 = sdmmc_read16(REG_SDSTATUS1);
-        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) {
-                    for(int i = 0; i<0x200; i+=2) {
-                        u16 data = sdmmc_read16(REG_SDFIFO);
-                        *dataPtr++ = data & 0xFF;
-                        *dataPtr++ = data >> 8;
+    while(true)
+    {
+        vu16 status1 = sdmmc_read16(REG_SDSTATUS1);
+        vu16 ctl32 = sdmmc_read16(REG_DATACTL32);
+        if((ctl32 & 0x100))
+        {
+            if(readdata)
+            {
+                if(rUseBuf)
+                {
+                    sdmmc_mask16(REG_SDSTATUS1, TMIO_STAT1_RXRDY, 0);
+                    if(size > 0x1FF)
+                    {
+                        //Gabriel Marcano: This implementation doesn't assume alignment.
+                        //I've removed the alignment check doen with former rUseBuf32 as a result
+                        for(int i = 0; i < 0x200; i += 4)
+                        {
+                            u32 data = sdmmc_read32(REG_SDFIFO32);
+                            *rDataPtr++ = data;
+                            *rDataPtr++ = data >> 8;
+                            *rDataPtr++ = data >> 16;
+                            *rDataPtr++ = data >> 24;
+                        }
+                        size -= 0x200;
                     }
-                    size -= 0x200;
                 }
-            }
-        }
 
-        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) {
-                    for (int i = 0; i<0x200; i+=2) {
-                        u16 data = *dataPtr++;
-                        data |= *dataPtr++ << 8;
-                        sdmmc_write16(REG_SDFIFO, data);
-                    }
-                    size -= 0x200;
-                }
+                sdmmc_mask16(REG_DATACTL32, 0x800, 0);
             }
         }
-        if (status1 & TMIO_MASK_GW) {
+        if(!(ctl32 & 0x200))
+        {
+            if(writedata)
+            {
+                if(tUseBuf)
+                {
+                    sdmmc_mask16(REG_SDSTATUS1, TMIO_STAT1_TXRQ, 0);
+                    if(size > 0x1FF)
+                    {
+                        for(int i = 0; i < 0x200; i += 4)
+                        {
+                            u32 data = *tDataPtr++;
+                            data |= (u32)*tDataPtr++ << 8;
+                            data |= (u32)*tDataPtr++ << 16;
+                            data |= (u32)*tDataPtr++ << 24;
+                            sdmmc_write32(REG_SDFIFO32, data);
+                        }
+                        size -= 0x200;
+                    }
+                }
+
+                sdmmc_mask16(REG_DATACTL32, 0x1000, 0);
+            }
+        }
+        if(status1 & TMIO_MASK_GW)
+        {
             ctx->error |= 4;
             break;
         }
 
-        if (!(status1 & TMIO_STAT1_CMD_BUSY)) {
+        if(!(status1 & TMIO_STAT1_CMD_BUSY))
+        {
             status0 = sdmmc_read16(REG_SDSTATUS0);
-            if (sdmmc_read16(REG_SDSTATUS0) & TMIO_STAT0_CMDRESPEND)
+            if(sdmmc_read16(REG_SDSTATUS0) & TMIO_STAT0_CMDRESPEND)
+            {
                 ctx->error |= 0x1;
-            if (status0 & TMIO_STAT0_DATAEND)
+            }
+            if(status0 & TMIO_STAT0_DATAEND)
+            {
                 ctx->error |= 0x2;
+            }
 
-            if ((status0 & flags) == flags)
+            if((status0 & flags) == flags)
                 break;
         }
     }
     ctx->stat0 = sdmmc_read16(REG_SDSTATUS0);
     ctx->stat1 = sdmmc_read16(REG_SDSTATUS1);
-    sdmmc_write16(REG_SDSTATUS0,0);
-    sdmmc_write16(REG_SDSTATUS1,0);
+    sdmmc_write16(REG_SDSTATUS0, 0);
+    sdmmc_write16(REG_SDSTATUS1, 0);
 
-    if (getSDRESP != 0) {
-        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);
+    if(getSDRESP != 0)
+    {
+        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));
     }
 }
 
-u32 __attribute__((noinline)) sdmmc_sdcard_writesectors(u32 sector_no, u32 numsectors, vu8 *in)
+int __attribute__ ((noinline)) sdmmc_sdcard_writesectors(u32 sector_no, u32 numsectors, const u8 *in)
 {
-    if (handleSD.isSDHC == 0)
-        sector_no <<= 9;
+    if(handleSD.isSDHC == 0) sector_no <<= 9;
     inittarget(&handleSD);
-    sdmmc_write16(REG_SDSTOP,0x100);
-
-    sdmmc_write16(REG_SDBLKCOUNT,numsectors);
-    handleSD.data = in;
+    sdmmc_write16(REG_SDSTOP, 0x100);
+    sdmmc_write16(REG_SDBLKCOUNT32, numsectors);
+    sdmmc_write16(REG_SDBLKLEN32, 0x200);
+    sdmmc_write16(REG_SDBLKCOUNT, numsectors);
+    handleSD.tData = in;
     handleSD.size = numsectors << 9;
-    sdmmc_send_command(&handleSD,0x52C19,sector_no);
+    sdmmc_send_command(&handleSD, 0x52C19, sector_no);
     return geterror(&handleSD);
 }
 
-u32 __attribute__((noinline)) sdmmc_sdcard_readsectors(u32 sector_no, u32 numsectors, vu8 *out)
+int __attribute__ ((noinline)) sdmmc_sdcard_readsectors(u32 sector_no, u32 numsectors, u8 *out)
 {
-    if (handleSD.isSDHC == 0)
-        sector_no <<= 9;
+    if(handleSD.isSDHC == 0) sector_no <<= 9;
     inittarget(&handleSD);
-    sdmmc_write16(REG_SDSTOP,0x100);
-
-    sdmmc_write16(REG_SDBLKCOUNT,numsectors);
-    handleSD.data = out;
+    sdmmc_write16(REG_SDSTOP, 0x100);
+    sdmmc_write16(REG_SDBLKCOUNT32, numsectors);
+    sdmmc_write16(REG_SDBLKLEN32, 0x200);
+    sdmmc_write16(REG_SDBLKCOUNT, numsectors);
+    handleSD.rData = out;
     handleSD.size = numsectors << 9;
-    sdmmc_send_command(&handleSD,0x33C12,sector_no);
+    sdmmc_send_command(&handleSD, 0x33C12, sector_no);
     return geterror(&handleSD);
 }
 
-u32 __attribute__((noinline)) sdmmc_nand_readsectors(u32 sector_no, u32 numsectors, vu8 *out)
+int __attribute__ ((noinline)) sdmmc_nand_readsectors(u32 sector_no, u32 numsectors, u8 *out)
 {
-    if (handleNAND.isSDHC == 0)
-        sector_no <<= 9;
+    if(handleNAND.isSDHC == 0) sector_no <<= 9;
     inittarget(&handleNAND);
-    sdmmc_write16(REG_SDSTOP,0x100);
-
-    sdmmc_write16(REG_SDBLKCOUNT,numsectors);
-
-    handleNAND.data = out;
+    sdmmc_write16(REG_SDSTOP, 0x100);
+    sdmmc_write16(REG_SDBLKCOUNT32, numsectors);
+    sdmmc_write16(REG_SDBLKLEN32, 0x200);
+    sdmmc_write16(REG_SDBLKCOUNT, numsectors);
+    handleNAND.rData = out;
     handleNAND.size = numsectors << 9;
-    sdmmc_send_command(&handleNAND,0x33C12,sector_no);
+    sdmmc_send_command(&handleNAND, 0x33C12, sector_no);
     inittarget(&handleSD);
     return geterror(&handleNAND);
 }
 
-static u32 calcSDSize(u8* csd, int type)
+/*
+int __attribute__ ((noinline)) sdmmc_nand_writesectors(u32 sector_no, u32 numsectors, const u8 *in) //experimental
+{
+    if(handleNAND.isSDHC == 0) sector_no <<= 9;
+    inittarget(&handleNAND);
+    sdmmc_write16(REG_SDSTOP, 0x100);
+    sdmmc_write16(REG_SDBLKCOUNT32, numsectors);
+    sdmmc_write16(REG_SDBLKLEN32, 0x200);
+    sdmmc_write16(REG_SDBLKCOUNT, numsectors);
+    handleNAND.tData = in;
+    handleNAND.size = numsectors << 9;
+    sdmmc_send_command(&handleNAND, 0x52C19, sector_no);
+    inittarget(&handleSD);
+    return geterror(&handleNAND);
+}
+*/
+
+static u32 calcSDSize(u8 *csd, int type)
 {
     u32 result = 0;
-    if (type == -1) type = csd[14] >> 6;
-    switch (type) {
+    if(type == -1) type = csd[14] >> 6;
+    switch(type)
+    {
         case 0:
             {
-                u32 block_len = csd[9] & 0xf;
+                u32 block_len = csd[9] & 0xF;
                 block_len = 1u << block_len;
-                u32 mult = (u32)(csd[4] >> 7) | (u32)((csd[5] & 3) << 1);
+                u32 mult = (u32)((csd[4] >> 7) | ((csd[5] & 3) << 1));
                 mult = 1u << (mult + 2);
                 result = csd[8] & 3;
                 result = (result << 8) | csd[7];
@@ -212,18 +280,42 @@ static u32 calcSDSize(u8* csd, int type)
             }
             break;
         case 1:
-            result = csd[7] & 0x3f;
+            result = csd[7] & 0x3F;
             result = (result << 8) | csd[6];
             result = (result << 8) | csd[5];
             result = (result + 1) * 1024;
             break;
-    default:
-        break; //Do nothing otherwise
+        default:
+            break; //Do nothing otherwise FIXME perhaps return some error?
     }
     return result;
 }
 
 static void InitSD()
+{
+    *(vu16 *)0x10006100 &= 0xF7FFu; //SDDATACTL32
+    *(vu16 *)0x10006100 &= 0xEFFFu; //SDDATACTL32
+    *(vu16 *)0x10006100 |= 0x402u; //SDDATACTL32
+    *(vu16 *)0x100060D8 = (*(vu16 *)0x100060D8 & 0xFFDD) | 2;
+    *(vu16 *)0x10006100 &= 0xFFFFu; //SDDATACTL32
+    *(vu16 *)0x100060D8 &= 0xFFDFu; //SDDATACTL
+    *(vu16 *)0x10006104 = 512; //SDBLKLEN32
+    *(vu16 *)0x10006108 = 1; //SDBLKCOUNT32
+    *(vu16 *)0x100060E0 &= 0xFFFEu; //SDRESET
+    *(vu16 *)0x100060E0 |= 1u; //SDRESET
+    *(vu16 *)0x10006020 |= TMIO_MASK_ALL; //SDIR_MASK0
+    *(vu16 *)0x10006022 |= TMIO_MASK_ALL>>16; //SDIR_MASK1
+    *(vu16 *)0x100060FC |= 0xDBu; //SDCTL_RESERVED7
+    *(vu16 *)0x100060FE |= 0xDBu; //SDCTL_RESERVED8
+    *(vu16 *)0x10006002 &= 0xFFFCu; //SDPORTSEL
+    *(vu16 *)0x10006024 = 0x20;
+    *(vu16 *)0x10006028 = 0x40EE;
+    *(vu16 *)0x10006002 &= 0xFFFCu; ////SDPORTSEL
+    *(vu16 *)0x10006026 = 512; //SDBLKLEN
+    *(vu16 *)0x10006008 = 0; //SDSTOP
+}
+
+static int Nand_Init()
 {
     //NAND
     handleNAND.isSDHC = 0;
@@ -233,80 +325,50 @@ static void InitSD()
     handleNAND.clk = 0x80;
     handleNAND.devicenumber = 1;
 
-    //SD
-    handleSD.isSDHC = 0;
-    handleSD.SDOPT = 0;
-    handleSD.res = 0;
-    handleSD.initarg = 0;
-    handleSD.clk = 0x80;
-    handleSD.devicenumber = 0;
-
-    *(vu16*)0x10006100 &= 0xF7FFu; //SDDATACTL32
-    *(vu16*)0x10006100 &= 0xEFFFu; //SDDATACTL32
-    *(vu16*)0x10006100 |= 0x402u; //SDDATACTL32
-    *(vu16*)0x100060D8 = (*(vu16*)0x100060D8 & 0xFFDD) | 2;
-    *(vu16*)0x10006100 &= 0xFFFDu; //SDDATACTL32
-    *(vu16*)0x100060D8 &= 0xFFDDu; //SDDATACTL
-    *(vu16*)0x10006104 = 0; //SDBLKLEN32
-    *(vu16*)0x10006108 = 1; //SDBLKCOUNT32
-    *(vu16*)0x100060E0 &= 0xFFFEu; //SDRESET
-    *(vu16*)0x100060E0 |= 1u; //SDRESET
-    *(vu16*)0x10006020 |= TMIO_MASK_ALL; //SDIR_MASK0
-    *(vu16*)0x10006022 |= TMIO_MASK_ALL>>16; //SDIR_MASK1
-    *(vu16*)0x100060FC |= 0xDBu; //SDCTL_RESERVED7
-    *(vu16*)0x100060FE |= 0xDBu; //SDCTL_RESERVED8
-    *(vu16*)0x10006002 &= 0xFFFCu; //SDPORTSEL
-    *(vu16*)0x10006024 = 0x40; //Nintendo sets this to 0x20
-    *(vu16*)0x10006028 = 0x40EB; //Nintendo sets this to 0x40EE
-    *(vu16*)0x10006002 &= 0xFFFCu; ////SDPORTSEL
-    *(vu16*)0x10006026 = 512; //SDBLKLEN
-    *(vu16*)0x10006008 = 0; //SDSTOP
-
-    inittarget(&handleSD);
-}
-
-static int Nand_Init()
-{
     inittarget(&handleNAND);
-    ioDelay(0xF000);
+    waitcycles(0xF000);
 
-    sdmmc_send_command(&handleNAND,0,0);
+    sdmmc_send_command(&handleNAND, 0, 0);
 
-    do {
-        do {
-            sdmmc_send_command(&handleNAND,0x10701,0x100000);
-        } while ( !(handleNAND.error & 1) );
-    } while((handleNAND.ret[0] & 0x80000000) == 0);
+    do
+    {
+        do
+        {
+            sdmmc_send_command(&handleNAND, 0x10701, 0x100000);
+        }
+        while(!(handleNAND.error & 1));
+    }
+    while((handleNAND.ret[0] & 0x80000000) == 0);
 
-    sdmmc_send_command(&handleNAND,0x10602,0x0);
-    if (handleNAND.error & 0x4) return -1;
+    sdmmc_send_command(&handleNAND, 0x10602, 0x0);
+    if((handleNAND.error & 0x4))return -1;
 
-    sdmmc_send_command(&handleNAND,0x10403,handleNAND.initarg << 0x10);
-    if (handleNAND.error & 0x4) return -1;
+    sdmmc_send_command(&handleNAND, 0x10403, handleNAND.initarg << 0x10);
+    if((handleNAND.error & 0x4))return -1;
 
-    sdmmc_send_command(&handleNAND,0x10609,handleNAND.initarg << 0x10);
-    if (handleNAND.error & 0x4) return -1;
+    sdmmc_send_command(&handleNAND, 0x10609, handleNAND.initarg << 0x10);
+    if((handleNAND.error & 0x4))return -1;
 
-    handleNAND.total_size = calcSDSize((u8*)&handleNAND.ret[0],0);
+    handleNAND.total_size = calcSDSize((u8*)&handleNAND.ret[0], 0);
     handleNAND.clk = 1;
     setckl(1);
 
-    sdmmc_send_command(&handleNAND,0x10407,handleNAND.initarg << 0x10);
-    if (handleNAND.error & 0x4) return -1;
+    sdmmc_send_command(&handleNAND, 0x10407, handleNAND.initarg << 0x10);
+    if((handleNAND.error & 0x4))return -1;
 
     handleNAND.SDOPT = 1;
 
-    sdmmc_send_command(&handleNAND,0x10506,0x3B70100);
-    if (handleNAND.error & 0x4) return -1;
+    sdmmc_send_command(&handleNAND, 0x10506, 0x3B70100);
+    if((handleNAND.error & 0x4))return -1;
 
-    sdmmc_send_command(&handleNAND,0x10506,0x3B90100);
-    if (handleNAND.error & 0x4) return -1;
+    sdmmc_send_command(&handleNAND, 0x10506, 0x3B90100);
+    if((handleNAND.error & 0x4))return -1;
 
-    sdmmc_send_command(&handleNAND,0x1040D,handleNAND.initarg << 0x10);
-    if (handleNAND.error & 0x4) return -1;
+    sdmmc_send_command(&handleNAND, 0x1040D, handleNAND.initarg << 0x10);
+    if((handleNAND.error & 0x4))return -1;
 
-    sdmmc_send_command(&handleNAND,0x10410,0x200);
-    if (handleNAND.error & 0x4) return -1;
+    sdmmc_send_command(&handleNAND, 0x10410, 0x200);
+    if((handleNAND.error & 0x4))return -1;
 
     handleNAND.clk |= 0x200;
 
@@ -317,113 +379,102 @@ static int Nand_Init()
 
 static int SD_Init()
 {
+    //SD
+    handleSD.isSDHC = 0;
+    handleSD.SDOPT = 0;
+    handleSD.res = 0;
+    handleSD.initarg = 0;
+    handleSD.clk = 0x80;
+    handleSD.devicenumber = 0;
+
     inittarget(&handleSD);
 
-    ioDelay(1u << 18); //Card needs a little bit of time to be detected, it seems
-    
+    waitcycles(1u << 22); //Card needs a little bit of time to be detected, it seems FIXME test again to see what a good number is for the delay
+
     //If not inserted
-    if (!(*((vu16*)0x1000601c) & TMIO_STAT0_SIGSTATE)) return -1;
-    
-    sdmmc_send_command(&handleSD,0,0);
-    sdmmc_send_command(&handleSD,0x10408,0x1AA);
-    //u32 temp = (handleSD.ret[0] == 0x1AA) << 0x1E;
+    if(!(*((vu16 *)(SDMMC_BASE + REG_SDSTATUS0)) & TMIO_STAT0_SIGSTATE)) return 5;
+
+    sdmmc_send_command(&handleSD, 0, 0);
+    sdmmc_send_command(&handleSD, 0x10408, 0x1AA);
     u32 temp = (handleSD.error & 0x1) << 0x1E;
 
-    //int count = 0;
     u32 temp2 = 0;
-    do {
-        do {
-            sdmmc_send_command(&handleSD,0x10437,handleSD.initarg << 0x10);
-            sdmmc_send_command(&handleSD,0x10769,0x00FF8000 | temp);
+    do
+    {
+        do
+        {
+            sdmmc_send_command(&handleSD, 0x10437, handleSD.initarg << 0x10);
+            sdmmc_send_command(&handleSD, 0x10769, 0x00FF8000 | temp);
             temp2 = 1;
-        } while ( !(handleSD.error & 1) );
-
-    } while((handleSD.ret[0] & 0x80000000) == 0);
+        }
+        while(!(handleSD.error & 1));
+    }
+    while((handleSD.ret[0] & 0x80000000) == 0);
 
     if(!((handleSD.ret[0] >> 30) & 1) || !temp)
         temp2 = 0;
 
     handleSD.isSDHC = temp2;
 
-    sdmmc_send_command(&handleSD,0x10602,0);
-    if (handleSD.error & 0x4) return -1;
+    sdmmc_send_command(&handleSD, 0x10602, 0);
+    if((handleSD.error & 0x4)) return -1;
 
-    sdmmc_send_command(&handleSD,0x10403,0);
-    if (handleSD.error & 0x4) return -1;
+    sdmmc_send_command(&handleSD, 0x10403, 0);
+    if((handleSD.error & 0x4)) return -2;
     handleSD.initarg = handleSD.ret[0] >> 0x10;
 
-    sdmmc_send_command(&handleSD,0x10609,handleSD.initarg << 0x10);
-    if (handleSD.error & 0x4) return -1;
+    sdmmc_send_command(&handleSD, 0x10609, handleSD.initarg << 0x10);
+    if((handleSD.error & 0x4)) return -3;
 
-    handleSD.total_size = calcSDSize((u8*)&handleSD.ret[0],-1);
+    handleSD.total_size = calcSDSize((u8*)&handleSD.ret[0], -1);
     handleSD.clk = 1;
     setckl(1);
 
-    sdmmc_send_command(&handleSD,0x10507,handleSD.initarg << 0x10);
-    if (handleSD.error & 0x4) return -1;
+    sdmmc_send_command(&handleSD, 0x10507, handleSD.initarg << 0x10);
+    if((handleSD.error & 0x4)) return -4;
 
-    sdmmc_send_command(&handleSD,0x10437,handleSD.initarg << 0x10);
-    if (handleSD.error & 0x4) return -1;
+    sdmmc_send_command(&handleSD, 0x10437, handleSD.initarg << 0x10);
+    if((handleSD.error & 0x4)) return -5;
 
     handleSD.SDOPT = 1;
-    sdmmc_send_command(&handleSD,0x10446,0x2);
-    if (handleSD.error & 0x4) return -1;
+    sdmmc_send_command(&handleSD, 0x10446, 0x2);
+    if((handleSD.error & 0x4)) return -6;
 
-    sdmmc_send_command(&handleSD,0x1040D,handleSD.initarg << 0x10);
-    if (handleSD.error & 0x4) return -1;
+    sdmmc_send_command(&handleSD, 0x1040D, handleSD.initarg << 0x10);
+    if((handleSD.error & 0x4)) return -7;
 
-    sdmmc_send_command(&handleSD,0x10410,0x200);
-    if (handleSD.error & 0x4) return -1;
+    sdmmc_send_command(&handleSD, 0x10410, 0x200);
+    if((handleSD.error & 0x4)) return -8;
     handleSD.clk |= 0x200;
 
     return 0;
 }
 
+void sdmmc_get_cid(bool isNand, u32 *info)
+{
+    struct mmcdevice *device = isNand ? &handleNAND : &handleSD;
+
+    inittarget(device);
+
+    // use cmd7 to put sd card in standby mode
+    // CMD7
+    sdmmc_send_command(device, 0x10507, 0);
+
+    // get sd card info
+    // use cmd10 to read CID
+    sdmmc_send_command(device, 0x1060A, device->initarg << 0x10);
+
+    for(int i = 0; i < 4; ++i)
+        info[i] = device->ret[i];
+
+    // put sd card back to transfer mode
+    // CMD7
+    sdmmc_send_command(device, 0x10507, device->initarg << 0x10);
+}
+
 void sdmmc_sdcard_init()
 {
     InitSD();
     Nand_Init();
     SD_Init();
-}
-
-int sdmmc_get_cid(int isNand, uint32_t *info)
-{
-	struct mmcdevice *device;
-	if(isNand)
-		device = &handleNAND;
-	else
-		device = &handleSD;
-	
-	inittarget(device);
-	// use cmd7 to put sd card in standby mode
-	// CMD7
-	{
-		sdmmc_send_command(device,0x10507,0);
-		//if((device->error & 0x4)) return -1;
-	}
-
-	// get sd card info
-	// use cmd10 to read CID
-	{
-		sdmmc_send_command(device,0x1060A,device->initarg << 0x10);
-		//if((device->error & 0x4)) return -2;
-
-		for( int i = 0; i < 4; ++i ) {
-			info[i] = device->ret[i];
-		}
-	}
-
-	// put sd card back to transfer mode
-	// CMD7
-	{
-		sdmmc_send_command(device,0x10507,device->initarg << 0x10);
-		//if((device->error & 0x4)) return -3;
-	}
-
-	if(isNand)
-	{
-		inittarget(&handleSD);
-	}
-	
-	return 0;
 }
\ No newline at end of file
diff --git a/source/fatfs/sdmmc/sdmmc.h b/source/fatfs/sdmmc/sdmmc.h
index f666a1c..44449b7 100644
--- a/source/fatfs/sdmmc/sdmmc.h
+++ b/source/fatfs/sdmmc/sdmmc.h
@@ -1,52 +1,48 @@
-// Copyright 2014 Normmatt
-// Licensed under GPLv2 or any later version
-// Refer to the license.txt file included.
-
 #pragma once
 
-#include "common.h"
+#include "../../types.h"
 
-#define SDMMC_BASE              0x10006000u
+#define SDMMC_BASE		0x10006000
 
-#define REG_SDCMD               0x00
-#define REG_SDPORTSEL           0x02
-#define REG_SDCMDARG            0x04
-#define REG_SDCMDARG0           0x04
-#define REG_SDCMDARG1           0x06
-#define REG_SDSTOP              0x08
-#define REG_SDBLKCOUNT          0x0a
+#define REG_SDCMD		0x00
+#define REG_SDPORTSEL		0x02
+#define REG_SDCMDARG		0x04
+#define REG_SDCMDARG0		0x04
+#define REG_SDCMDARG1		0x06
+#define REG_SDSTOP		0x08
+#define REG_SDBLKCOUNT		0x0a
 
-#define REG_SDRESP0             0x0c
-#define REG_SDRESP1             0x0e
-#define REG_SDRESP2             0x10
-#define REG_SDRESP3             0x12
-#define REG_SDRESP4             0x14
-#define REG_SDRESP5             0x16
-#define REG_SDRESP6             0x18
-#define REG_SDRESP7             0x1a
+#define REG_SDRESP0		0x0c
+#define REG_SDRESP1		0x0e
+#define REG_SDRESP2		0x10
+#define REG_SDRESP3		0x12
+#define REG_SDRESP4		0x14
+#define REG_SDRESP5		0x16
+#define REG_SDRESP6		0x18
+#define REG_SDRESP7		0x1a
 
-#define REG_SDSTATUS0           0x1c
-#define REG_SDSTATUS1           0x1e
+#define REG_SDSTATUS0		0x1c
+#define REG_SDSTATUS1		0x1e
 
-#define REG_SDIRMASK0           0x20
-#define REG_SDIRMASK1           0x22
-#define REG_SDCLKCTL            0x24
+#define REG_SDIRMASK0		0x20
+#define REG_SDIRMASK1		0x22
+#define REG_SDCLKCTL		0x24
 
-#define REG_SDBLKLEN            0x26
-#define REG_SDOPT               0x28
-#define REG_SDFIFO              0x30
+#define REG_SDBLKLEN		0x26
+#define REG_SDOPT		0x28
+#define REG_SDFIFO		0x30
 
-#define REG_SDDATACTL           0xd8
-#define REG_SDRESET             0xe0
-#define REG_SDPROTECTED         0xf6 //bit 0 determines if sd is protected or not?
+#define REG_DATACTL 		0xd8
+#define REG_SDRESET		0xe0
+#define REG_SDPROTECTED		0xf6 //bit 0 determines if sd is protected or not?
 
-#define REG_SDDATACTL32         0x100
-#define REG_SDBLKLEN32          0x104
-#define REG_SDBLKCOUNT32        0x108
-#define REG_SDFIFO32            0x10C
+#define REG_DATACTL32 		0x100
+#define REG_SDBLKLEN32		0x104
+#define REG_SDBLKCOUNT32	0x108
+#define REG_SDFIFO32		0x10C
 
-#define REG_CLK_AND_WAIT_CTL    0x138
-#define REG_RESET_SDIO          0x1e0
+#define REG_CLK_AND_WAIT_CTL	0x138
+#define REG_RESET_SDIO		0x1e0
 
 #define TMIO_STAT0_CMDRESPEND    0x0001
 #define TMIO_STAT0_DATAEND       0x0004
@@ -70,40 +66,17 @@
 #define TMIO_STAT1_CMD_BUSY      0x4000
 #define TMIO_STAT1_ILL_ACCESS    0x8000
 
-//Comes from TWLSDK mongoose.tef DWARF info
-#define SDMC_NORMAL              0x00000000
-#define SDMC_ERR_COMMAND         0x00000001
-#define SDMC_ERR_CRC             0x00000002
-#define SDMC_ERR_END             0x00000004
-#define SDMC_ERR_TIMEOUT         0x00000008
-#define SDMC_ERR_FIFO_OVF        0x00000010
-#define SDMC_ERR_FIFO_UDF        0x00000020
-#define SDMC_ERR_WP              0x00000040
-#define SDMC_ERR_ABORT           0x00000080
-#define SDMC_ERR_FPGA_TIMEOUT    0x00000100
-#define SDMC_ERR_PARAM           0x00000200
-#define SDMC_ERR_R1_STATUS       0x00000800
-#define SDMC_ERR_NUM_WR_SECTORS  0x00001000
-#define SDMC_ERR_RESET           0x00002000
-#define SDMC_ERR_ILA             0x00004000
-#define SDMC_ERR_INFO_DETECT     0x00008000
-
-#define SDMC_STAT_ERR_UNKNOWN    0x00080000
-#define SDMC_STAT_ERR_CC         0x00100000
-#define SDMC_STAT_ERR_ECC_FAILED 0x00200000
-#define SDMC_STAT_ERR_CRC        0x00800000
-#define SDMC_STAT_ERR_OTHER      0xf9c70008
-
 #define TMIO_MASK_ALL           0x837f031d
 
 #define TMIO_MASK_GW            (TMIO_STAT1_ILL_ACCESS | TMIO_STAT1_CMDTIMEOUT | TMIO_STAT1_TXUNDERRUN | TMIO_STAT1_RXOVERFLOW | \
-                                 TMIO_STAT1_DATATIMEOUT | TMIO_STAT1_STOPBIT_ERR | TMIO_STAT1_CRCFAIL | TMIO_STAT1_CMD_IDX_ERR)
+								 TMIO_STAT1_DATATIMEOUT | TMIO_STAT1_STOPBIT_ERR | TMIO_STAT1_CRCFAIL | TMIO_STAT1_CMD_IDX_ERR)
 
 #define TMIO_MASK_READOP  (TMIO_STAT1_RXRDY | TMIO_STAT1_DATAEND)
 #define TMIO_MASK_WRITEOP (TMIO_STAT1_TXRQ | TMIO_STAT1_DATAEND)
 
 typedef struct mmcdevice {
-    vu8* data;
+    u8* rData;
+    const u8* tData;
     u32 size;
     u32 error;
     u16 stat0;
@@ -118,12 +91,10 @@ typedef struct mmcdevice {
     u32 res;
 } mmcdevice;
 
-mmcdevice *getMMCDevice(int drive);
-
 void sdmmc_sdcard_init();
-u32 sdmmc_sdcard_readsectors(u32 sector_no, u32 numsectors, vu8 *out);
-u32 sdmmc_sdcard_writesectors(u32 sector_no, u32 numsectors, vu8 *in);
-
-u32 sdmmc_nand_readsectors(u32 sector_no, u32 numsectors, vu8 *out);
-
-int sdmmc_get_cid( int isNand, uint32_t *info);
\ No newline at end of file
+int sdmmc_sdcard_readsectors(u32 sector_no, u32 numsectors, u8 *out);
+int sdmmc_sdcard_writesectors(u32 sector_no, u32 numsectors, const u8 *in);
+int sdmmc_nand_readsectors(u32 sector_no, u32 numsectors, u8 *out);
+//int sdmmc_nand_writesectors(u32 sector_no, u32 numsectors, const u8 *in);
+void sdmmc_get_cid(bool isNand, u32 *info);
+mmcdevice *getMMCDevice(int drive);
\ No newline at end of file