diff --git a/sw/n64/src/fatfs/diskio.c b/sw/n64/src/fatfs/diskio.c
index d8c42f9..6774919 100644
--- a/sw/n64/src/fatfs/diskio.c
+++ b/sw/n64/src/fatfs/diskio.c
@@ -12,23 +12,21 @@ DSTATUS disk_initialize (BYTE pdrv) {
 }
 
 DRESULT disk_read (BYTE pdrv, BYTE *buff, LBA_t sector, UINT count) {
-    sc64_storage_type_t storage_type;
-
-    if (pdrv == 0) {
-        storage_type = SC64_STORAGE_TYPE_SD;
-    } else if (pdrv == 1) {
-        storage_type = SC64_STORAGE_TYPE_USB;
-    } else {
-        return RES_PARERR;
-    }
-
-    if (sc64_storage_read(storage_type, buff, sector, count) != SC64_STORAGE_OK) {
+    if (sc64_storage_read(pdrv, buff, sector, count)) {
         return RES_ERROR;
     }
-
     return RES_OK;
 }
 
+#ifndef FF_FS_READONLY
+DRESULT disk_write (BYTE pdrv, const BYTE* buff, LBA_t sector, UINT count) {
+    if (sc64_storage_write(pdrv, buff, sector, count)) {
+        return RES_ERROR;
+    }
+    return RES_OK;
+}
+#endif
+
 DRESULT disk_ioctl (BYTE pdrv, BYTE cmd, void *buff) {
     return RES_PARERR;
 }
diff --git a/sw/n64/src/sc64.c b/sw/n64/src/sc64.c
index c82589b..744570a 100644
--- a/sw/n64/src/sc64.c
+++ b/sw/n64/src/sc64.c
@@ -1,19 +1,22 @@
 #include "sc64.h"
 
 
+#define SECTOR_SIZE     (512)
+
+
 bool sc64_check_presence (void) {
     uint32_t version = pi_io_read(&SC64->VERSION);
     return (version == SC64_VERSION_2);
 }
 
-void sc64_wait_cpu_ready (void) {
+static void sc64_wait_cpu_ready (void) {
     uint32_t sr;
     do {
         sr = pi_io_read(&SC64->SR_CMD);
     } while (!(sr & SC64_SR_CPU_READY));
 }
 
-bool sc64_wait_cpu_busy (void) {
+static bool sc64_wait_cpu_busy (void) {
     uint32_t sr;
     do {
         sr = pi_io_read(&SC64->SR_CMD);
@@ -21,7 +24,7 @@ bool sc64_wait_cpu_busy (void) {
     return sr & SC64_SR_CMD_ERROR;
 }
 
-bool sc64_perform_cmd (uint8_t cmd, uint32_t *args, uint32_t *result) {
+static bool sc64_perform_cmd (uint8_t cmd, uint32_t *args, uint32_t *result) {
     if (args != NULL) {
         pi_io_write(&SC64->DATA[0], args[0]);
         pi_io_write(&SC64->DATA[1], args[1]);
@@ -53,7 +56,7 @@ void sc64_get_info (sc64_info_t *info) {
     info->boot_mode = (boot_mode_t) sc64_query_config(CFG_ID_BOOT_MODE);
 }
 
-void sc64_uart_put_char (char c) {
+static void sc64_uart_put_char (char c) {
 #ifdef DEBUG
     uint32_t args[2] = { (uint32_t) (c), 0 };
     sc64_perform_cmd(SC64_CMD_UART_PUT, args, NULL);
@@ -72,3 +75,77 @@ void sc64_init (void) {
     sc64_change_config(CFG_ID_SDRAM_SWITCH, true);
     sc64_uart_print_string("\033c");
 }
+
+static bool sc64_wait_drive_ready (uint8_t drive) {
+    bool error;
+    uint32_t args[2] = { drive, 0 };
+    uint32_t result[2];
+    do {
+        error = sc64_perform_cmd(SC64_CMD_DRIVE_BUSY, args, result);
+    } while (!result[0]);
+    return error;
+}
+
+static bool sc64_drive_read_sector (uint8_t drive, void *buffer, uint32_t sector) {
+    io32_t *src = SC64->BUFFER;
+    uint8_t *dst = (uint8_t *) (buffer);
+    uint32_t args[2] = { drive, sector };
+    if (sc64_perform_cmd(SC64_CMD_DRIVE_READ, args, NULL)) {
+        return true;
+    }
+    if (sc64_wait_drive_ready(drive)) {
+        return true;
+    }
+    for (int i = 0; i < SECTOR_SIZE; i += sizeof(uint32_t)) {
+        uint32_t data = pi_io_read(src++);
+        *dst++ = ((data >> 24) & 0xFF);
+        *dst++ = ((data >> 16) & 0xFF);
+        *dst++ = ((data >> 8) & 0xFF);
+        *dst++ = ((data >> 0) & 0xFF);
+    }
+    return false;
+}
+
+bool sc64_storage_read (uint8_t drive, void *buffer, uint32_t sector, uint32_t count) {
+    if (sc64_wait_drive_ready(drive)) {
+        return true;
+    }
+    for (int i = 0; i < count; i++) {
+        if (sc64_drive_read_sector(drive, buffer, (sector + i))) {
+            return true;
+        }
+        buffer += SECTOR_SIZE;
+    }
+    return false;
+}
+
+static bool sc64_drive_write_sector (uint8_t drive, void *buffer, uint32_t sector) {
+    uint8_t *src = (uint8_t *) (buffer);
+    io32_t *dst = SC64->BUFFER;
+    uint32_t args[2] = { drive, sector };
+    uint32_t data;
+    if (sc64_wait_drive_ready(drive)) {
+        return true;
+    }
+    for (int i = 0; i < SECTOR_SIZE; i += sizeof(uint32_t)) {
+        data = ((*src++) << 24);
+        data |= ((*src++) << 16);
+        data |= ((*src++) << 8);
+        data |= ((*src++) << 0);
+        pi_io_write(dst++, data);
+    }
+    if (sc64_perform_cmd(SC64_CMD_DRIVE_WRITE, args, NULL)) {
+        return true;
+    }
+    return false;
+}
+
+bool sc64_storage_write (uint8_t drive, void *buffer, uint32_t sector, uint32_t count) {
+    for (int i = 0; i < count; i++) {
+        if (sc64_drive_write_sector(drive, buffer, (sector + i))) {
+            return true;
+        }
+        buffer += SECTOR_SIZE;
+    }
+    return false;
+}
diff --git a/sw/n64/src/sc64.h b/sw/n64/src/sc64.h
index 7bb17f7..2b858f7 100644
--- a/sw/n64/src/sc64.h
+++ b/sw/n64/src/sc64.h
@@ -9,7 +9,10 @@
 
 #define SC64_CMD_QUERY              ('Q')
 #define SC64_CMD_CONFIG             ('C')
-#define SC64_CMD_UART_PUT           ('Z')
+#define SC64_CMD_DRIVE_BUSY         (0xF0)
+#define SC64_CMD_DRIVE_READ         (0xF1)
+#define SC64_CMD_DRIVE_WRITE        (0xF2)
+#define SC64_CMD_UART_PUT           (0xFF)
 
 #define SC64_VERSION_2              (0x53437632)
 
@@ -72,15 +75,13 @@ typedef struct {
 
 
 bool sc64_check_presence (void);
-void sc64_wait_cpu_ready (void);
-bool sc64_wait_cpu_busy (void);
-bool sc64_perform_cmd (uint8_t cmd, uint32_t *args, uint32_t *result);
 uint32_t sc64_query_config (cfg_id_t id);
 void sc64_change_config (cfg_id_t id, uint32_t value);
 void sc64_get_info (sc64_info_t *info);
-void sc64_uart_put_char (char c);
 void sc64_uart_print_string (const char *text);
 void sc64_init (void);
+bool sc64_storage_read (uint8_t drive, void *buffer, uint32_t sector, uint32_t count);
+bool sc64_storage_write (uint8_t drive, void *buffer, uint32_t sector, uint32_t count);
 
 
 #endif