diff --git a/sw/bootloader/Makefile b/sw/bootloader/Makefile
index 1156cc2..ef6c45a 100644
--- a/sw/bootloader/Makefile
+++ b/sw/bootloader/Makefile
@@ -29,8 +29,8 @@ SRC_FILES = \
 	io.c \
 	ipl2.S \
 	main.c \
+	menu.c \
 	sc64.c \
-	storage.c \
 	syscalls.c \
 	test.c \
 	version.c \
diff --git a/sw/bootloader/src/fatfs/diskio.c b/sw/bootloader/src/fatfs/diskio.c
index 9d61a65..848d2e6 100644
--- a/sw/bootloader/src/fatfs/diskio.c
+++ b/sw/bootloader/src/fatfs/diskio.c
@@ -4,7 +4,11 @@
 #include "../sc64.h"
 #include "../error.h"
 
-DSTATUS status = STA_NOINIT;
+
+#define FROM_BCD(x)     ((((x >> 4) & 0x0F) * 10) + (x & 0x0F))
+
+
+static DSTATUS status = STA_NOINIT;
 
 
 DSTATUS disk_status (BYTE pdrv) {
@@ -18,7 +22,7 @@ DSTATUS disk_initialize (BYTE pdrv) {
     if (pdrv > 0) {
         return STA_NODISK;
     }
-    if (!sc64_sd_card_initialize()) {
+    if (!sc64_sd_card_init()) {
         status &= ~(STA_NOINIT);
     }
     return status;
@@ -28,67 +32,61 @@ DRESULT disk_read (BYTE pdrv, BYTE *buff, LBA_t sector, UINT count) {
     if (pdrv > 0) {
         return RES_PARERR;
     }
-    uint32_t physical_address = ((uint32_t) (buff)) & 0x1FFFFFFF;
-    uint32_t block = 0;
-    while (count > 0) {
-        if (physical_address < (8 * 1024 * 1024)) {
-            block = ((count > 16) ? 16 : count);
-            if (sc64_sd_read_sectors(sector, 0x1FFE0000UL, block * 512)) {
+    uint32_t *physical_address = (uint32_t *) (PHYSICAL(buff));
+    if (physical_address < (uint32_t *) (N64_RAM_SIZE)) {
+        while (count > 0) {
+            uint32_t block = ((count > 16) ? 16 : count);
+            if (sc64_sd_read_sectors((uint32_t *) (SC64_BUFFERS->BUFFER), sector, block)) {
                 return RES_ERROR;
             }
             for (uint32_t i = 0; i < (block * 512); i += 4) {
-                uint32_t data = pi_io_read((uint32_t *) (0x1FFE0000UL + i));
+                // TODO: use dma
+                uint32_t data = pi_io_read((uint32_t *) (&SC64_BUFFERS->BUFFER[i]));
                 uint8_t *ptr = (uint8_t *) (&data);
                 for (int j = 0; j < 4; j++) {
                     *buff++ = *ptr++;
                 }
             }
-        } else {
-            block = ((count > 256) ? 256 : count);
-            if (sc64_sd_read_sectors(sector, physical_address, block * 512)) {
-                error_display("Kurwa 0x%08X, 0x%08X\n", physical_address, block);
-                return RES_ERROR;
-            }
-            physical_address += (block * 512);
+            count -= block;
+            sector += block;
+        }
+    } else {
+        if (sc64_sd_read_sectors(physical_address, sector, count)) {
+            return RES_ERROR;
         }
-        count -= block;
-        sector += block;
     }
     return RES_OK;
 }
 
 #if !FF_FS_READONLY
 DRESULT disk_write (BYTE pdrv, const BYTE* buff, LBA_t sector, UINT count) {
-    // if (pdrv >= __DRIVE_COUNT) {
-        // return RES_PARERR;
-    // }
-    // if (sc64_storage_write((drive_id_t) (pdrv), buff, sector, count)) {
-        return RES_ERROR;
-    // }
-    // return RES_OK;
+    if (pdrv > 0) {
+        return RES_PARERR;
+    }
+    // TODO: write
+    return RES_ERROR;
 }
 #endif
 
 DRESULT disk_ioctl (BYTE pdrv, BYTE cmd, void *buff) {
+    if (pdrv > 0) {
+        return RES_PARERR;
+    }
     if (cmd == CTRL_SYNC) {
         return RES_OK;
     }
     return RES_PARERR;
 }
 
-static uint8_t from_bcd (uint8_t bcd) {
-    return ((((bcd >> 4) & 0x0F) * 10) + (bcd & 0x0F));
-}
-
 DWORD get_fattime(void) {
     rtc_time_t t;
     sc64_get_time(&t);
     return (
-        ((from_bcd(t.year) + 20) << 25) |
-        (from_bcd(t.month) << 21) |
-        (from_bcd(t.day) << 16) |
-        (from_bcd(t.hour) << 11) |
-        (from_bcd(t.minute) << 5) |
-        (from_bcd(t.second) >> 1)
+        ((FROM_BCD(t.year) + 20) << 25) |
+        (FROM_BCD(t.month) << 21) |
+        (FROM_BCD(t.day) << 16) |
+        (FROM_BCD(t.hour) << 11) |
+        (FROM_BCD(t.minute) << 5) |
+        (FROM_BCD(t.second) >> 1)
     );
 }
diff --git a/sw/bootloader/src/io.h b/sw/bootloader/src/io.h
index 542b924..1780c0e 100644
--- a/sw/bootloader/src/io.h
+++ b/sw/bootloader/src/io.h
@@ -12,8 +12,11 @@ typedef volatile uint32_t io32_t;
 
 #define	ALIGN(value, align)         (((value) + ((typeof(value))(align) - 1)) & ~((typeof(value))(align) - 1))
 
+#define PHYSICAL(address)           ((typeof(address)) (((io32_t) (address)) & (0x1FFFFFFFUL)))
 #define UNCACHED(address)           ((typeof(address)) (((io32_t) (address)) | (0xA0000000UL)))
 
+#define N64_RAM_SIZE                (0x00800000UL)
+
 
 typedef struct {
     io32_t DMEM[1024];
diff --git a/sw/bootloader/src/main.c b/sw/bootloader/src/main.c
index 1b791f8..88e1944 100644
--- a/sw/bootloader/src/main.c
+++ b/sw/bootloader/src/main.c
@@ -2,8 +2,8 @@
 #include "error.h"
 #include "init.h"
 #include "io.h"
+#include "menu.h"
 #include "sc64.h"
-#include "storage.h"
 
 
 void main (void) {
@@ -13,12 +13,8 @@ void main (void) {
     sc64_get_boot_info(&sc64_boot_info);
 
     switch (sc64_boot_info.boot_mode) {
-        case BOOT_MODE_MENU_SD:
-            storage_run_menu(STORAGE_BACKEND_SD);
-            break;
-
-        case BOOT_MODE_MENU_USB:
-            storage_run_menu(STORAGE_BACKEND_USB);
+        case BOOT_MODE_MENU:
+            menu_load_and_run();
             break;
 
         case BOOT_MODE_ROM:
diff --git a/sw/bootloader/src/storage.c b/sw/bootloader/src/menu.c
similarity index 76%
rename from sw/bootloader/src/storage.c
rename to sw/bootloader/src/menu.c
index 7ed59c7..4b1f0aa 100644
--- a/sw/bootloader/src/storage.c
+++ b/sw/bootloader/src/menu.c
@@ -1,7 +1,7 @@
 #include "error.h"
-#include "init.h"
-#include "storage.h"
 #include "fatfs/ff.h"
+#include "init.h"
+#include "menu.h"
 
 
 #define ROM_ENTRY_OFFSET    (8)
@@ -41,28 +41,16 @@ static const char *fatfs_error_codes[] = {
 }
 
 
-void storage_run_menu (storage_backend_t storage_backend) {
+void menu_load_and_run (void) {
     void (* menu)(void);
     FRESULT fresult;
     FATFS fs;
     FIL fil;
     UINT br;
     FSIZE_t size = ROM_MAX_LOAD_SIZE;
-    const TCHAR *path = "";
 
-    if (storage_backend == STORAGE_BACKEND_SD) {
-        path = "0:";
-    } else if (storage_backend == STORAGE_BACKEND_USB) {
-        path = "1:";
-    } else {
-        error_display("Unknown storage backend [%d]\n", storage_backend);
-    }
-
-    FF_CHECK(f_mount(&fs, path, 1), "Couldn't mount drive");
-    FF_CHECK(f_chdrive(path), "Couldn't chdrive");
+    FF_CHECK(f_mount(&fs, "", 1), "Couldn't mount drive");
     FF_CHECK(f_open(&fil, "sc64menu.n64", FA_READ), "Couldn't open menu file");
-    FF_CHECK(f_lseek(&fil, 0), "debug 1");
-    FF_CHECK(f_read(&fil, (void *) (0x10000000UL), f_size(&fil), &br), "debug 2");
     FF_CHECK(f_lseek(&fil, ROM_ENTRY_OFFSET), "Couldn't seek to entry point offset");
     FF_CHECK(f_read(&fil, &menu, sizeof(menu), &br), "Couldn't read entry point");
     FF_CHECK(f_lseek(&fil, ROM_CODE_OFFSET), "Couldn't seek to code start offset");
@@ -71,8 +59,11 @@ void storage_run_menu (storage_backend_t storage_backend) {
     }
     FF_CHECK(f_read(&fil, menu, size, &br), "Couldn't read menu file");
     FF_CHECK(br != size, "Read size is different than expected");
+    
+    FF_CHECK(f_lseek(&fil, 0), "Couldn't seek to the beginning of file");
+    FF_CHECK(f_read(&fil, (void *) (0x10000000UL), f_size(&fil), &br), "Couldn't read file contents to SDRAM");
     FF_CHECK(f_close(&fil), "Couldn't close menu file");
-    FF_CHECK(f_unmount(path), "Couldn't unmount drive");
+    FF_CHECK(f_unmount(""), "Couldn't unmount drive");
 
     deinit();
 
diff --git a/sw/bootloader/src/menu.h b/sw/bootloader/src/menu.h
new file mode 100644
index 0000000..505608b
--- /dev/null
+++ b/sw/bootloader/src/menu.h
@@ -0,0 +1,8 @@
+#ifndef MENU_H__
+#define MENU_H__
+
+
+void menu_load_and_run (void);
+
+
+#endif
diff --git a/sw/bootloader/src/sc64.c b/sw/bootloader/src/sc64.c
index f1e674b..440fe84 100644
--- a/sw/bootloader/src/sc64.c
+++ b/sw/bootloader/src/sc64.c
@@ -15,7 +15,7 @@ typedef enum {
     SC64_CMD_USB_WRITE          = 'M',
     SC64_CMD_USB_READ_STATUS    = 'u',
     SC64_CMD_USB_READ           = 'm',
-    SC64_CMD_SD_CARD_INITIALIZE = 'i',
+    SC64_CMD_SD_CARD_INIT       = 'i',
     SC64_CMD_SD_SECTOR_SET      = 'I',
     SC64_CMD_SD_READ            = 's',
     SC64_CMD_SD_WRITE           = 'S',
@@ -27,7 +27,7 @@ static bool sc64_wait_cpu_busy (void) {
     do {
         sr = pi_io_read(&SC64_REGS->SR_CMD);
     } while (sr & SC64_SR_CPU_BUSY);
-    return sr & SC64_SR_CMD_ERROR;
+    return (sr & SC64_SR_CMD_ERROR);
 }
 
 static bool sc64_execute_cmd (uint8_t cmd, uint32_t *args, uint32_t *result) {
@@ -126,7 +126,7 @@ bool sc64_usb_read_ready (uint8_t *type, uint32_t *length) {
     if (length != NULL) {
         *length = result[1];
     }
-    return result[1] > 0;
+    return (result[1] > 0);
 }
 
 bool sc64_usb_read (uint32_t *address, uint32_t length) {
@@ -141,16 +141,25 @@ bool sc64_usb_read (uint32_t *address, uint32_t length) {
     return false;
 }
 
-bool sc64_sd_card_initialize (void) {
-    if (sc64_execute_cmd(SC64_CMD_SD_CARD_INITIALIZE, NULL, NULL)) {
+bool sc64_sd_card_init (void) {
+    uint32_t args[2] = { 0, true };
+    if (sc64_execute_cmd(SC64_CMD_SD_CARD_INIT, args, NULL)) {
         return true;
     }
     return false;
 }
 
-bool sc64_sd_read_sectors (uint32_t starting_sector, uint32_t address, uint32_t length) {
-    uint32_t sector_set_args[2] = { starting_sector, 0 };
-    uint32_t read_args[2] = { address, length };
+bool sc64_sd_card_deinit (void) {
+    uint32_t args[2] = { 0, false };
+    if (sc64_execute_cmd(SC64_CMD_SD_CARD_INIT, args, NULL)) {
+        return true;
+    }
+    return false;
+}
+
+bool sc64_sd_read_sectors (uint32_t *address, uint32_t sector, uint32_t count) {
+    uint32_t sector_set_args[2] = { sector, 0 };
+    uint32_t read_args[2] = { (uint32_t) (address), count };
     if (sc64_execute_cmd(SC64_CMD_SD_SECTOR_SET, sector_set_args, NULL)) {
         return true;
     }
diff --git a/sw/bootloader/src/sc64.h b/sw/bootloader/src/sc64.h
index 199c6c2..985c9e8 100644
--- a/sw/bootloader/src/sc64.h
+++ b/sw/bootloader/src/sc64.h
@@ -39,11 +39,10 @@ typedef enum {
 } dd_mode_t;
 
 typedef enum {
-    BOOT_MODE_MENU_SD = 0,
-    BOOT_MODE_MENU_USB = 1,
-    BOOT_MODE_ROM = 2,
-    BOOT_MODE_DDIPL = 3,
-    BOOT_MODE_DIRECT = 4
+    BOOT_MODE_MENU = 0,
+    BOOT_MODE_ROM = 1,
+    BOOT_MODE_DDIPL = 2,
+    BOOT_MODE_DIRECT = 3
 } boot_mode_t;
 
 typedef enum {
@@ -104,8 +103,9 @@ bool sc64_usb_write_ready (void);
 bool sc64_usb_write (uint32_t *address, uint32_t length);
 bool sc64_usb_read_ready (uint8_t *type, uint32_t *length);
 bool sc64_usb_read (uint32_t *address, uint32_t length);
-bool sc64_sd_card_initialize (void);
-bool sc64_sd_read_sectors (uint32_t starting_sector, uint32_t address, uint32_t length);
+bool sc64_sd_card_init (void);
+bool sc64_sd_card_deinit (void);
+bool sc64_sd_read_sectors (uint32_t *address, uint32_t sector, uint32_t count);
 
 
 #endif
diff --git a/sw/bootloader/src/storage.h b/sw/bootloader/src/storage.h
deleted file mode 100644
index 75d532d..0000000
--- a/sw/bootloader/src/storage.h
+++ /dev/null
@@ -1,14 +0,0 @@
-#ifndef STORAGE_H__
-#define STORAGE_H__
-
-
-typedef enum {
-    STORAGE_BACKEND_SD = 0,
-    STORAGE_BACKEND_USB = 1,
-} storage_backend_t;
-
-
-void storage_run_menu (storage_backend_t storage_backend);
-
-
-#endif
diff --git a/sw/controller/src/cfg.c b/sw/controller/src/cfg.c
index b9d0db8..a046ee2 100644
--- a/sw/controller/src/cfg.c
+++ b/sw/controller/src/cfg.c
@@ -35,11 +35,10 @@ typedef enum {
 } dd_mode_t;
 
 typedef enum {
-    BOOT_MODE_MENU_SD = 0,
-    BOOT_MODE_MENU_USB = 1,
-    BOOT_MODE_ROM = 2,
-    BOOT_MODE_DD = 3,
-    BOOT_MODE_DIRECT = 4
+    BOOT_MODE_MENU = 0,
+    BOOT_MODE_ROM = 1,
+    BOOT_MODE_DD = 2,
+    BOOT_MODE_DIRECT = 3
 } boot_mode_t;
 
 typedef enum {
@@ -315,7 +314,7 @@ void cfg_init (void) {
 
     p.cic_seed = CIC_SEED_UNKNOWN;
     p.tv_type = TV_TYPE_UNKNOWN;
-    p.boot_mode = BOOT_MODE_MENU_SD;
+    p.boot_mode = BOOT_MODE_MENU;
     p.usb_output_ready = true;
 }
 
@@ -393,8 +392,12 @@ void cfg_process (void) {
                 break;
 
             case 'i':
-                if (sd_card_initialize()) {
-                    cfg_set_error(CFG_ERROR_SD);
+                if (args[1]) {
+                    if (sd_card_init()) {
+                        cfg_set_error(CFG_ERROR_SD);
+                    }
+                } else {
+                    sd_card_deinit();
                 }
                 break;
 
@@ -407,7 +410,7 @@ void cfg_process (void) {
                     cfg_set_error(CFG_ERROR_BAD_ADDRESS);
                     return;
                 }
-                if (sd_read_sectors(p.sd_card_sector, args[0], args[1])) {
+                if (sd_read_sectors(args[0], p.sd_card_sector, args[1])) {
                     cfg_set_error(CFG_ERROR_SD);
                 }
                 break;
diff --git a/sw/controller/src/fpga.h b/sw/controller/src/fpga.h
index 6b955bd..f23cbdc 100644
--- a/sw/controller/src/fpga.h
+++ b/sw/controller/src/fpga.h
@@ -55,6 +55,9 @@ typedef enum {
 } fpga_reg_t;
 
 
+#define SWAP16(x)                       (((x & 0xFF) << 8) | ((x & 0xFF00) >> 8))
+#define SWAP32(x)                       ((x & 0xFF) << 24 | (x & 0xFF00) << 8 | (x & 0xFF0000) >> 8 | (x & 0xFF000000) >> 24)
+
 #define FPGA_ID                         (0x64)
 
 #define FPGA_MAX_MEM_TRANSFER           (1024)
@@ -174,8 +177,6 @@ typedef enum {
 #define DD_HEAD_TRACK_MASK              (DD_HEAD_MASK | DD_TRACK_MASK)
 #define DD_HEAD_TRACK_INDEX_LOCK        (1 << 13)
 
-#define SWAP32(x)                       ((x & 0xFF) << 24 | (x & 0xFF00) << 8 | (x & 0xFF0000) >> 8 | (x & 0xFF000000) >> 24)
-
 
 uint8_t fpga_id_get (void);
 uint32_t fpga_reg_get (fpga_reg_t reg);
diff --git a/sw/controller/src/sd.c b/sw/controller/src/sd.c
index 0e541db..34b6a99 100644
--- a/sw/controller/src/sd.c
+++ b/sw/controller/src/sd.c
@@ -1,22 +1,35 @@
 #include <stdbool.h>
 #include <stdint.h>
-#include "sd.h"
 #include "fpga.h"
-#include "debug.h"
 #include "hw.h"
+#include "sd.h"
 
 
+#define SD_BUFFER_ADDRESS               (0x05000000UL)
+
+#define CMD6_ARG_CHECK_HS               (0x00FFFFF1UL)
+#define CMD6_ARG_SWITCH_HS              (0x80FFFFF1UL)
+
 #define CMD8_ARG_SUPPLY_VOLTAGE_27_36_V (1 << 8)
 #define CMD8_ARG_CHECK_PATTERN          (0xAA << 0)
 
+#define ACMD6_ARG_BUS_WIDTH_4BIT        (2 << 0)
+
+#define ACMD41_ARG_OCR                  (0xFF8000 << 0)
 #define ACMD41_ARG_HCS                  (1 << 30)
 
+#define R3_OCR                          (0xFF8000 << 0)
 #define R3_CCS                          (1 << 30)
 #define R3_BUSY                         (1 << 31)
 
+#define R6_RCA_MASK                     (0xFFFF0000UL)
+
 #define R7_SUPPLY_VOLTAGE_27_36_V       (1 << 8)
 #define R7_CHECK_PATTERN                (0xAA << 0)
 
+#define SD_BLOCK_SIZE                   (512)
+#define DAT_BLOCK_MAX_COUNT             (256)
+
 
 typedef enum {
     CLOCK_STOP,
@@ -35,15 +48,39 @@ typedef enum {
     RSP_R7,
 } rsp_type_t;
 
+typedef enum {
+    DAT_READ,
+    DAT_WRITE,
+} dat_mode_t;
 
-static bool sd_card_initialized;
-static bool sd_card_type_block;
-static uint32_t sd_rca = 0;
-static volatile bool timeout = false;
+
+struct process {
+    bool card_initialized;
+    bool card_type_block;
+    uint32_t rca;
+    volatile bool timeout;
+};
+
+
+static struct process p;
 
 
 static void sd_trigger_timeout (void) {
-    timeout = true;
+    p.timeout = true;
+}
+
+static void sd_prepare_timeout (uint16_t value) {
+    p.timeout = false;
+    hw_tim_setup(TIM_ID_GVR, value, sd_trigger_timeout);
+}
+
+static bool sd_did_timeout (void) {
+    return p.timeout;
+}
+
+static void sd_clear_timeout (void) {
+    hw_tim_stop(TIM_ID_GVR);
+    p.timeout = false;
 }
 
 static void sd_set_clock (sd_clock_t mode) {
@@ -91,22 +128,15 @@ static bool sd_cmd (uint8_t cmd, uint32_t arg, rsp_type_t rsp_type, void *rsp) {
     } while (scr & SD_SCR_CMD_BUSY);
 
     if (rsp != NULL) {
-        fpga_reg_t rsp_regs[4] = {
-            REG_SD_RSP_3,
-            REG_SD_RSP_2,
-            REG_SD_RSP_1,
-            REG_SD_RSP_0
-        };
-        bool rsp_long = rsp_type & SD_CMD_LONG_RESPONSE;
-        fpga_reg_t rsp_reg = (rsp_long ? 0 : (sizeof(rsp_regs) - 1));
+        bool rsp_long = (cmd_data & SD_CMD_LONG_RESPONSE);
         uint8_t *rsp_8 = (uint8_t *) (rsp);
-        while (rsp_reg < sizeof(rsp_regs)) {
-            uint32_t rsp_data = fpga_reg_get(rsp_regs[rsp_reg++]);
+        for (int i = 0; i < (rsp_long ? 4 : 1); i++) {
+            uint32_t rsp_data = fpga_reg_get(REG_SD_RSP_0 + i);
             uint8_t *rsp_data_8 = (uint8_t *) (&rsp_data);
             for (int i = 0; i < 4; i++) {
                 *rsp_8++ = *rsp_data_8++;
             }
-        };
+        }
     }
 
     if (rsp_type == RSP_R1b) {
@@ -119,7 +149,7 @@ static bool sd_cmd (uint8_t cmd, uint32_t arg, rsp_type_t rsp_type, void *rsp) {
 }
 
 static bool sd_acmd (uint8_t acmd, uint32_t arg, rsp_type_t rsp_type, void *rsp) {
-    if (sd_cmd(55, sd_rca, RSP_R1, NULL)) {
+    if (sd_cmd(55, p.rca, RSP_R1, NULL)) {
         return true;
     }
     if (sd_cmd(acmd, arg, rsp_type, rsp)) {
@@ -128,158 +158,201 @@ static bool sd_acmd (uint8_t acmd, uint32_t arg, rsp_type_t rsp_type, void *rsp)
     return false;
 }
 
+static void sd_dat_prepare (uint32_t address, uint32_t count, dat_mode_t mode) {
+    uint32_t length = (count * SD_BLOCK_SIZE);
+    uint32_t sd_dat = (((count - 1) << SD_DAT_BLOCKS_BIT) | SD_DAT_FIFO_FLUSH);
+    uint32_t sd_dma_scr = DMA_SCR_START;
 
-bool sd_read_sectors (uint32_t starting_sector, uint32_t address, uint32_t length) {
-    if (!sd_card_initialized) {
-        return true;
+    if (mode == DAT_READ) {
+        sd_dat |= SD_DAT_START_READ;
+        sd_dma_scr |= DMA_SCR_DIRECTION;
+    } else {
+        sd_dat |= SD_DAT_START_WRITE;
     }
 
-    if ((length == 0) || (length % 512 != 0) || (length > (256 * 512))) {
-        return true;
-    }
-
-    do {
-        uint32_t blocks = (length / 512);
-
-        timeout = false;
-        hw_tim_setup(TIM_ID_GVR, 5000, sd_trigger_timeout);
-
-        if (!sd_card_type_block) {
-            starting_sector *= 512;
-        }
-
-        // fpga_reg_set(REG_SD_DMA_SCR, DMA_SCR_STOP);
-        // fpga_reg_set(REG_SD_DAT, SD_DAT_STOP | SD_DAT_FIFO_FLUSH);
-
-        fpga_reg_set(REG_SD_DMA_ADDRESS, address);
-        fpga_reg_set(REG_SD_DMA_LENGTH, length);
-        fpga_reg_set(REG_SD_DMA_SCR, DMA_SCR_DIRECTION | DMA_SCR_START);
-
-        fpga_reg_set(REG_SD_DAT, (((blocks - 1) << SD_DAT_BLOCKS_BIT) | SD_DAT_START_READ | SD_DAT_FIFO_FLUSH));
-
-        if (sd_cmd(23, blocks, RSP_R1, NULL)) {
-            break;
-        }
-
-        if (sd_cmd(18, starting_sector, RSP_R1, NULL)) {
-            break;
-        }
-
-        bool error = false;
-
-        while (!timeout) {
-            uint32_t sd_scr = fpga_reg_get(REG_SD_SCR);
-            if (!(sd_scr & SD_SCR_CARD_INSERTED)) {
-                error = true;
-                break;
-            }
-            uint32_t sd_dat = fpga_reg_get(REG_SD_DAT);
-            uint32_t sd_dma_scr = fpga_reg_get(REG_SD_DMA_SCR);
-            if (sd_dat & SD_DAT_ERROR) {
-                error = true;
-                break;
-            }
-            if ((!(sd_dma_scr & DMA_SCR_BUSY)) && (!(sd_dat & SD_DAT_BUSY))) {
-                break;
-            }
-        }
-
-        if (timeout) {
-            break;
-        }
-
-        hw_tim_stop(TIM_ID_GVR);
-
-        if (error) {
-            break;
-        }
-
-        return false;
-    } while (0);
+    fpga_reg_set(REG_SD_DAT, sd_dat);
+    fpga_reg_set(REG_SD_DMA_ADDRESS, address);
+    fpga_reg_set(REG_SD_DMA_LENGTH, length);
+    fpga_reg_set(REG_SD_DMA_SCR, sd_dma_scr);
+}
 
+static void sd_dat_abort (void) {
     fpga_reg_set(REG_SD_DMA_SCR, DMA_SCR_STOP);
     fpga_reg_set(REG_SD_DAT, SD_DAT_STOP | SD_DAT_FIFO_FLUSH);
+}
+
+static bool sd_dat_wait (uint16_t timeout) {
+    sd_prepare_timeout(timeout);
+
+    do {
+        uint32_t sd_dat = fpga_reg_get(REG_SD_DAT);
+        uint32_t sd_dma_scr = fpga_reg_get(REG_SD_DMA_SCR);
+        if ((!(sd_dat & SD_DAT_BUSY)) && (!(sd_dma_scr & DMA_SCR_BUSY))) {
+            sd_clear_timeout();
+            return (sd_dat & SD_DAT_ERROR);
+        }
+    } while (!sd_did_timeout());
+
+    sd_dat_abort();
 
     return true;
 }
 
-bool sd_card_initialize (void) {
-    bool error;
+
+bool sd_read_sectors (uint32_t address, uint32_t sector, uint32_t count) {
+    if (!p.card_initialized || (count == 0)) {
+        return true;
+    }
+
+    if (!p.card_type_block) {
+        sector *= SD_BLOCK_SIZE;
+    }
+
+    while (count > 0) {
+        uint32_t blocks = ((count > DAT_BLOCK_MAX_COUNT) ? DAT_BLOCK_MAX_COUNT : count);
+        sd_dat_prepare(address, blocks, DAT_READ);
+        if (sd_cmd(23, blocks, RSP_R1, NULL)) {
+            sd_dat_abort();
+            return true;
+        }
+        if (sd_cmd(18, sector, RSP_R1, NULL)) {
+            sd_dat_abort();
+            return true;
+        }
+        if (sd_dat_wait(1000)) {
+            if (sd_did_timeout()) {
+                sd_cmd(12, 0, RSP_R1b, NULL);
+            }
+            return true;
+        }
+        address += (blocks * SD_BLOCK_SIZE);
+        sector += (blocks * (p.card_type_block ? 1 : SD_BLOCK_SIZE));
+        count -= blocks;
+    }
+
+    return false;
+}
+
+bool sd_card_init (void) {
     uint32_t arg;
     uint32_t rsp;
-    bool version_2_or_later = false;
+    uint16_t sd_function;
 
-    if (sd_card_initialized) {
+    if (p.card_initialized) {
         return false;
     }
 
+    p.card_initialized = true;
+    p.rca = 0;
+
     sd_set_clock(CLOCK_400KHZ);
 
-    do {
-        sd_cmd(0, 0, RSP_NONE, NULL);
-
-        arg = (CMD8_ARG_SUPPLY_VOLTAGE_27_36_V | CMD8_ARG_CHECK_PATTERN);
-        if (!sd_cmd(8, arg, RSP_R7, &rsp)) {
-            version_2_or_later = true;
-            if (rsp != (R7_SUPPLY_VOLTAGE_27_36_V | R7_CHECK_PATTERN)) {
-                break;
-            }
-        }
-
-        arg = ((version_2_or_later ? ACMD41_ARG_HCS : 0) | 0x00FF8000);
-        for (int i = 0; i < 4000; i++) {
-            error = sd_acmd(41, arg, RSP_R3, &rsp);
-            if (error || (rsp & R3_BUSY)) {
-                break;
-            }
-        }
-        if (error || ((rsp & 0x00FF8000) == 0)) {
-            break;
-        }
-        sd_card_type_block = (rsp & R3_CCS);
-
-        if (sd_cmd(2, 0, RSP_R2, NULL)) {
-            break;
-        }
-
-        if (sd_cmd(3, 0, RSP_R6, &rsp)) {
-            break;
-        }
-        sd_rca = rsp & 0xFFFF0000;
-
-        if (sd_cmd(7, sd_rca, RSP_R1b, NULL)) {
-            break;
-        }
-
-        if (sd_acmd(6, 2, RSP_R1, NULL)) {
-            break;
-        }
-
-        sd_set_clock(CLOCK_50MHZ);
-
-        sd_card_initialized = true;
-
-        return false;
-    } while (0);
-
-    sd_rca = 0;
     sd_cmd(0, 0, RSP_NONE, NULL);
-    sd_set_clock(CLOCK_STOP);
 
-    return true;
+    arg = (CMD8_ARG_SUPPLY_VOLTAGE_27_36_V | CMD8_ARG_CHECK_PATTERN);
+    if (sd_cmd(8, arg, RSP_R7, &rsp)) {
+        arg = ACMD41_ARG_OCR;
+    } else {
+        if (rsp != (R7_SUPPLY_VOLTAGE_27_36_V | R7_CHECK_PATTERN)) {
+            sd_card_deinit();
+            return true;
+        }
+        arg = (ACMD41_ARG_HCS | ACMD41_ARG_OCR);
+    }
+
+    sd_prepare_timeout(1000);
+    do {
+        if (sd_did_timeout()) {
+            sd_card_deinit();
+            return true;            
+        }
+        if (sd_acmd(41, arg, RSP_R3, &rsp)) {
+            sd_card_deinit();
+            return true;
+        }
+        if (rsp & R3_BUSY) {
+            if ((rsp & R3_OCR) == 0) {
+                sd_card_deinit();
+                return true;
+            }
+            p.card_type_block = (rsp & R3_CCS);
+            break;
+        }
+    } while (1);
+    sd_clear_timeout();
+
+    if (sd_cmd(2, 0, RSP_R2, NULL)) {
+        sd_card_deinit();
+        return true;
+    }
+
+    if (sd_cmd(3, 0, RSP_R6, &rsp)) {
+        sd_card_deinit();
+        return true;
+    }
+    p.rca = (rsp & R6_RCA_MASK);
+
+    if (sd_cmd(7, p.rca, RSP_R1b, NULL)) {
+        sd_card_deinit();
+        return true;
+    }
+
+    sd_set_clock(CLOCK_25MHZ);
+
+    if (sd_acmd(6, ACMD6_ARG_BUS_WIDTH_4BIT, RSP_R1, NULL)) {
+        sd_card_deinit();
+        return true;
+    }
+
+    sd_dat_prepare(SD_BUFFER_ADDRESS, 1, DAT_READ);
+    if (sd_cmd(6, CMD6_ARG_CHECK_HS, RSP_R1, NULL)) {
+        sd_dat_abort();
+        sd_card_deinit();
+        return true;
+    }
+    sd_dat_wait(1000);
+    if (sd_did_timeout()) {
+        sd_card_deinit();
+        return true;
+    }
+    fpga_mem_read(SD_BUFFER_ADDRESS + 12, 2, (uint8_t *) (&sd_function));
+    if (SWAP16(sd_function) & (1 << 1)) {
+        sd_dat_prepare(SD_BUFFER_ADDRESS, 1, DAT_READ);
+        if (sd_cmd(6, CMD6_ARG_SWITCH_HS, RSP_R1, NULL)) {
+            sd_dat_abort();
+            sd_card_deinit();
+            return true;
+        }
+        sd_dat_wait(1000);
+        if (sd_did_timeout()) {
+            sd_card_deinit();
+            return true;
+        }
+        fpga_mem_read(SD_BUFFER_ADDRESS + 12, 2, (uint8_t *) (&sd_function));
+        if (SWAP16(sd_function) & (1 << 1)) {
+            sd_set_clock(CLOCK_50MHZ);
+        }
+    }
+
+    return false;
+}
+
+void sd_card_deinit (void) {
+    if (p.card_initialized) {
+        p.card_initialized = false;
+        sd_set_clock(CLOCK_400KHZ);
+        sd_cmd(0, 0, RSP_NONE, NULL);
+        sd_set_clock(CLOCK_STOP);
+    }
 }
 
 void sd_init (void) {
-    sd_card_initialized = false;
+    p.card_initialized = false;
     sd_set_clock(CLOCK_STOP);
 }
 
 void sd_process (void) {
     if (!(fpga_reg_get(REG_SD_SCR) & SD_SCR_CARD_INSERTED)) {
-        if (sd_card_initialized) {
-            sd_card_initialized = false;
-            sd_rca = 0;
-            sd_set_clock(CLOCK_STOP);
-        }
+        sd_card_deinit();
     }
 }
diff --git a/sw/controller/src/sd.h b/sw/controller/src/sd.h
index 90afd36..5a68e36 100644
--- a/sw/controller/src/sd.h
+++ b/sw/controller/src/sd.h
@@ -5,8 +5,9 @@
 #include <stdbool.h>
 
 
-bool sd_read_sectors (uint32_t starting_sector, uint32_t address, uint32_t length);
-bool sd_card_initialize (void);
+bool sd_read_sectors (uint32_t address, uint32_t sector, uint32_t count);
+bool sd_card_init (void);
+void sd_card_deinit (void);
 void sd_init (void);
 void sd_process (void);
 
diff --git a/sw/pc/sc64.py b/sw/pc/sc64.py
index 704fe4a..3c77c63 100755
--- a/sw/pc/sc64.py
+++ b/sw/pc/sc64.py
@@ -276,11 +276,10 @@ class SC64:
         DD_DISK_SWAP = 3
 
     class BootMode(IntEnum):
-        SD = 0
-        USB = 1
-        ROM = 2
-        DDIPL = 3
-        DIRECT = 4
+        MENU = 0
+        ROM = 1
+        DDIPL = 2
+        DIRECT = 3
 
     class SaveType(IntEnum):
         NONE = 0
@@ -357,7 +356,7 @@ class SC64:
         self.__set_config(self.__CfgId.ROM_SHADOW_ENABLE, False)
         self.__set_config(self.__CfgId.DD_MODE, self.__DDMode.NONE)
         self.__set_config(self.__CfgId.ISV_ENABLE, False)
-        self.__set_config(self.__CfgId.BOOT_MODE, self.BootMode.USB)
+        self.__set_config(self.__CfgId.BOOT_MODE, self.BootMode.MENU)
         self.__set_config(self.__CfgId.SAVE_TYPE, self.SaveType.NONE)
         self.__set_config(self.__CfgId.CIC_SEED, self.CICSeed.AUTO)
         self.__set_config(self.__CfgId.TV_TYPE, self.TVType.AUTO)