diff --git a/sw/bootloader/src/fatfs/diskio.c b/sw/bootloader/src/fatfs/diskio.c
index 861d86a..d198360 100644
--- a/sw/bootloader/src/fatfs/diskio.c
+++ b/sw/bootloader/src/fatfs/diskio.c
@@ -46,10 +46,8 @@ DRESULT disk_read (BYTE pdrv, BYTE *buff, LBA_t sector, UINT count) {
             }
             if (((uint32_t) (buff) % 8) == 0) {
                 pi_dma_read((io32_t *) (SC64_BUFFERS->BUFFER), buff, length);
-                cache_data_hit_invalidate(buff, length);
             } else {
                 pi_dma_read((io32_t *) (SC64_BUFFERS->BUFFER), aligned_buffer, length);
-                cache_data_hit_invalidate(aligned_buffer, length);
                 memcpy(buff, aligned_buffer, length);
             }
             buff += length;
@@ -76,11 +74,9 @@ DRESULT disk_write (BYTE pdrv, const BYTE* buff, LBA_t sector, UINT count) {
             uint32_t blocks = ((count > BUFFER_BLOCKS_MAX) ? BUFFER_BLOCKS_MAX : count);
             size_t length = (blocks * SD_BLOCK_SIZE);
             if (((uint32_t) (buff) % 8) == 0) {
-                cache_data_hit_writeback((void *) (buff), length);
                 pi_dma_write((io32_t *) (SC64_BUFFERS->BUFFER), (void *) (buff), length);
             } else {
                 memcpy(aligned_buffer, buff, length);
-                cache_data_hit_writeback(aligned_buffer, length);
                 pi_dma_write((io32_t *) (SC64_BUFFERS->BUFFER), aligned_buffer, length);
             }
             if (sc64_sd_write_sectors((uint32_t *) (SC64_BUFFERS->BUFFER), sector, blocks)) {
diff --git a/sw/bootloader/src/io.c b/sw/bootloader/src/io.c
index aff1879..39bc4ef 100644
--- a/sw/bootloader/src/io.c
+++ b/sw/bootloader/src/io.c
@@ -1,6 +1,30 @@
 #include "io.h"
 
 
+static void cache_operation (uint8_t operation, uint8_t line_size, void *address, size_t length) {
+    uint32_t cache_address = (((uint32_t) (address)) & (~(line_size - 1)));
+    while (cache_address < ((uint32_t) (address) + length)) {
+        asm volatile (
+            "cache %[operation], (%[cache_address]) \n" ::
+            [operation] "i" (operation),
+            [cache_address] "r" (cache_address)
+        );
+        cache_address += line_size;
+    }    
+}
+
+void cache_data_hit_invalidate (void *address, size_t length) {
+    cache_operation(0x11, 16, address, length);
+}
+
+void cache_data_hit_writeback (void *address, size_t length) {
+    cache_operation(0x19, 16, address, length);
+}
+
+void cache_inst_hit_invalidate (void *address, size_t length) {
+    cache_operation(0x10, 32, address, length);
+}
+
 uint32_t io_read (io32_t *address) {
     io32_t *uncached = UNCACHED(address);
     uint32_t value = *uncached;
@@ -30,9 +54,11 @@ void pi_dma_read (io32_t *address, void *buffer, size_t length) {
     io_write(&PI->MADDR, (uint32_t) (PHYSICAL(buffer)));
     io_write(&PI->WDMA, length - 1);
     while (pi_busy());
+    cache_data_hit_invalidate(buffer, length);
 }
 
 void pi_dma_write (io32_t *address, void *buffer, size_t length) {
+    cache_data_hit_writeback(buffer, length);
     io_write(&PI->PADDR, (uint32_t) (PHYSICAL(address)));
     io_write(&PI->MADDR, (uint32_t) (PHYSICAL(buffer)));
     io_write(&PI->RDMA, length - 1);
@@ -51,27 +77,3 @@ void si_io_write (io32_t *address, uint32_t value) {
     io_write(address, value);
     while (si_busy());
 }
-
-static void cache_operation (uint8_t operation, uint8_t line_size, void *address, size_t length) {
-    uint32_t cache_address = (((uint32_t) (address)) & (~(line_size - 1)));
-    while (cache_address < ((uint32_t) (address) + length)) {
-        asm volatile (
-            "cache %[operation], (%[cache_address]) \n" ::
-            [operation] "i" (operation),
-            [cache_address] "r" (cache_address)
-        );
-        cache_address += line_size;
-    }    
-}
-
-void cache_data_hit_invalidate (void *address, size_t length) {
-    cache_operation(0x11, 16, address, length);
-}
-
-void cache_data_hit_writeback (void *address, size_t length) {
-    cache_operation(0x19, 16, address, length);
-}
-
-void cache_inst_hit_invalidate (void *address, size_t length) {
-    cache_operation(0x10, 32, address, length);
-}
diff --git a/sw/bootloader/src/sc64.c b/sw/bootloader/src/sc64.c
index 36e65ae..67864b7 100644
--- a/sw/bootloader/src/sc64.c
+++ b/sw/bootloader/src/sc64.c
@@ -15,12 +15,19 @@ typedef enum {
     SC64_CMD_USB_WRITE          = 'M',
     SC64_CMD_USB_READ_STATUS    = 'u',
     SC64_CMD_USB_READ           = 'm',
-    SC64_CMD_SD_CARD_INIT       = 'i',
+    SC64_CMD_SD_CARD_OP         = 'i',
     SC64_CMD_SD_SECTOR_SET      = 'I',
     SC64_CMD_SD_WRITE           = 'S',
     SC64_CMD_SD_READ            = 's',
 } cmd_id_t;
 
+typedef enum {
+    SD_CARD_OP_DEINIT = 0,
+    SD_CARD_OP_INIT = 1,
+    SD_CARD_OP_GET_STATUS = 2,
+    SD_CARD_OP_GET_INFO = 3,
+} sd_card_op_t;
+
 
 static bool sc64_wait_cpu_busy (void) {
     uint32_t sr;
@@ -142,16 +149,33 @@ bool sc64_usb_read (uint32_t *address, uint32_t length) {
 }
 
 bool sc64_sd_card_init (void) {
-    uint32_t args[2] = { 0, true };
-    if (sc64_execute_cmd(SC64_CMD_SD_CARD_INIT, args, NULL)) {
+    uint32_t args[2] = { 0, SD_CARD_OP_INIT };
+    if (sc64_execute_cmd(SC64_CMD_SD_CARD_OP, args, NULL)) {
         return true;
     }
     return false;
 }
 
 bool sc64_sd_card_deinit (void) {
-    uint32_t args[2] = { 0, false };
-    if (sc64_execute_cmd(SC64_CMD_SD_CARD_INIT, args, NULL)) {
+    uint32_t args[2] = { 0, SD_CARD_OP_DEINIT };
+    if (sc64_execute_cmd(SC64_CMD_SD_CARD_OP, args, NULL)) {
+        return true;
+    }
+    return false;
+}
+
+bool sc64_sd_card_get_status (void) {
+    uint32_t args[2] = { 0, SD_CARD_OP_GET_STATUS };
+    uint32_t result[2];
+    if (sc64_execute_cmd(SC64_CMD_SD_CARD_OP, args, result)) {
+        return false;
+    }
+    return result[1];
+}
+
+bool sc64_sd_card_get_info (uint32_t *address) {
+    uint32_t args[2] = { (uint32_t) (address), SD_CARD_OP_GET_INFO };
+    if (sc64_execute_cmd(SC64_CMD_SD_CARD_OP, args, NULL)) {
         return true;
     }
     return false;
diff --git a/sw/bootloader/src/sc64.h b/sw/bootloader/src/sc64.h
index 1d5ad63..9554cf8 100644
--- a/sw/bootloader/src/sc64.h
+++ b/sw/bootloader/src/sc64.h
@@ -105,6 +105,8 @@ 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_init (void);
 bool sc64_sd_card_deinit (void);
+bool sc64_sd_card_get_status (void);
+bool sc64_sd_card_get_info (uint32_t *address);
 bool sc64_sd_write_sectors (uint32_t *address, uint32_t sector, uint32_t count);
 bool sc64_sd_read_sectors (uint32_t *address, uint32_t sector, uint32_t count);
 
diff --git a/sw/bootloader/src/test.c b/sw/bootloader/src/test.c
index 75d07ea..76fc098 100644
--- a/sw/bootloader/src/test.c
+++ b/sw/bootloader/src/test.c
@@ -1,5 +1,6 @@
 #include <stddef.h>
 #include "display.h"
+#include "io.h"
 #include "sc64.h"
 #include "test.h"
 
@@ -14,9 +15,30 @@ bool test_check (void) {
 void test_execute (void) {
     display_init(NULL);
 
-    display_printf("SC64 Test suite\n");
+    display_printf("SC64 Test suite\n\n");
 
-    // TODO: implement tests
+    if (sc64_sd_card_init()) {
+        display_printf("SD card init error!\n");
+        while (1);
+    }
+
+    if (sc64_sd_card_get_info((uint32_t *) (SC64_BUFFERS->BUFFER))) {
+        display_printf("SD card get info error!\n");
+        while (1);
+    }
+
+    uint8_t card_info[32] __attribute__((aligned(8)));
+
+    pi_dma_read((io32_t *) (SC64_BUFFERS->BUFFER), card_info, sizeof(card_info));
+
+    display_printf("CSD: 0x");
+    for (int i = 0; i < 16; i++) {
+        display_printf("%02X", card_info[i]);
+    }
+    display_printf("\nCID: 0x");
+    for (int i = 16; i < 32; i++) {
+        display_printf("%02X", card_info[i]);
+    }
 
     while (1);
 }
diff --git a/sw/controller/src/cfg.c b/sw/controller/src/cfg.c
index 0e60bf2..7132126 100644
--- a/sw/controller/src/cfg.c
+++ b/sw/controller/src/cfg.c
@@ -63,10 +63,11 @@ typedef enum {
 
 typedef enum {
     CFG_ERROR_OK = 0,
-    CFG_ERROR_BAD_ADDRESS = 1,
-    CFG_ERROR_BAD_CONFIG_ID = 2,
-    CFG_ERROR_TIMEOUT = 3,
-    CFG_ERROR_SD = 4,
+    CFG_ERROR_BAD_ARGUMENT = 1,
+    CFG_ERROR_BAD_ADDRESS = 2,
+    CFG_ERROR_BAD_CONFIG_ID = 3,
+    CFG_ERROR_TIMEOUT = 4,
+    CFG_ERROR_SD_CARD = 5,
     CFG_ERROR_UNKNOWN_CMD = -1,
 } cfg_error_t;
 
@@ -392,12 +393,33 @@ void cfg_process (void) {
                 break;
 
             case 'i':
-                if (args[1]) {
-                    if (sd_card_init()) {
-                        cfg_set_error(CFG_ERROR_SD);
-                    }
-                } else {
-                    sd_card_deinit();
+                switch (args[1]) {
+                    case 0:
+                        sd_card_deinit();
+                        break;
+                    case 1:
+                        if (sd_card_init()) {
+                            cfg_set_error(CFG_ERROR_SD_CARD);
+                            return;
+                        }
+                        break;
+                    case 2:
+                        args[1] = sd_card_get_status();
+                        break;
+                    case 3:
+                        args[1] = 32;
+                        if (cfg_translate_address(args)) {
+                            cfg_set_error(CFG_ERROR_BAD_ADDRESS);
+                            return;
+                        }
+                        if (sd_card_get_info(args[0])) {
+                            cfg_set_error(CFG_ERROR_SD_CARD);
+                            return;
+                        }
+                        break;
+                    default:
+                        cfg_set_error(CFG_ERROR_BAD_ARGUMENT);
+                        return;
                 }
                 break;
 
@@ -411,7 +433,8 @@ void cfg_process (void) {
                     return;
                 }
                 if (sd_read_sectors(args[0], p.sd_card_sector, args[1])) {
-                    cfg_set_error(CFG_ERROR_SD);
+                    cfg_set_error(CFG_ERROR_SD_CARD);
+                    return;
                 }
                 break;
 
@@ -421,7 +444,8 @@ void cfg_process (void) {
                     return;
                 }
                 if (sd_write_sectors(args[0], p.sd_card_sector, args[1])) {
-                    cfg_set_error(CFG_ERROR_SD);
+                    cfg_set_error(CFG_ERROR_SD_CARD);
+                    return;
                 }
                 break;
 
diff --git a/sw/controller/src/fpga.h b/sw/controller/src/fpga.h
index f23cbdc..b7dfe65 100644
--- a/sw/controller/src/fpga.h
+++ b/sw/controller/src/fpga.h
@@ -55,8 +55,8 @@ 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 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)
 
diff --git a/sw/controller/src/sd.c b/sw/controller/src/sd.c
index 1cde4dd..318b70a 100644
--- a/sw/controller/src/sd.c
+++ b/sw/controller/src/sd.c
@@ -16,10 +16,10 @@
 
 #define ACMD6_ARG_BUS_WIDTH_4BIT        (2 << 0)
 
-#define ACMD41_ARG_OCR                  (0xFF8000 << 0)
+#define ACMD41_ARG_OCR                  (0x300000 << 0)
 #define ACMD41_ARG_HCS                  (1 << 30)
 
-#define R3_OCR                          (0xFF8000 << 0)
+#define R3_OCR                          (0x300000 << 0)
 #define R3_CCS                          (1 << 30)
 #define R3_BUSY                         (1 << 31)
 
@@ -59,6 +59,8 @@ struct process {
     bool card_initialized;
     bool card_type_block;
     uint32_t rca;
+    uint8_t csd[16];
+    uint8_t cid[16];
     volatile bool timeout;
 };
 
@@ -129,14 +131,18 @@ 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) {
-        bool rsp_long = (cmd_data & SD_CMD_LONG_RESPONSE);
-        uint8_t *rsp_8 = (uint8_t *) (rsp);
-        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);
+        if (cmd_data & SD_CMD_LONG_RESPONSE) {
+            uint8_t *rsp_8 = (uint8_t *) (rsp);
             for (int i = 0; i < 4; i++) {
-                *rsp_8++ = *rsp_data_8++;
+                uint32_t rsp_data = fpga_reg_get(REG_SD_RSP_3 - i);
+                uint8_t *rsp_data_8 = (uint8_t *) (&rsp_data);
+                rsp_data = SWAP32(rsp_data);
+                for (int i = 0; i < 4; i++) {
+                    *rsp_8++ = *rsp_data_8++;
+                }
             }
+        } else {
+            (*(uint32_t *) (rsp)) = fpga_reg_get(REG_SD_RSP_0);
         }
     }
 
@@ -200,74 +206,6 @@ static bool sd_dat_wait (uint16_t timeout) {
 }
 
 
-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);
-        led_blink_act();
-        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_write_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);
-        led_blink_act();
-        if (sd_cmd(23, blocks, RSP_R1, NULL)) {
-            return true;
-        }
-        if (sd_cmd(25, sector, RSP_R1, NULL)) {
-            return true;
-        }
-        sd_dat_prepare(address, blocks, DAT_WRITE);
-        bool error = sd_dat_wait(1000);
-        sd_cmd(12, 0, RSP_R1b, NULL);
-        if (error) {
-            sd_dat_abort();
-            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;
@@ -329,6 +267,16 @@ bool sd_card_init (void) {
     }
     p.rca = (rsp & R6_RCA_MASK);
 
+    if (sd_cmd(9, p.rca, RSP_R2, p.csd)) {
+        sd_card_deinit();
+        return true;
+    }
+
+    if (sd_cmd(10, p.rca, RSP_R2, p.cid)) {
+        sd_card_deinit();
+        return true;
+    }
+
     if (sd_cmd(7, p.rca, RSP_R1b, NULL)) {
         sd_card_deinit();
         return true;
@@ -383,6 +331,89 @@ void sd_card_deinit (void) {
     }
 }
 
+bool sd_card_get_status (void) {
+    return p.card_initialized;
+}
+
+bool sd_card_get_info (uint32_t address) {
+    if (!p.card_initialized) {
+        return true;
+    }
+    fpga_mem_write(address, sizeof(p.csd), p.csd);
+    address += sizeof(p.csd);
+    fpga_mem_write(address, sizeof(p.cid), p.cid);
+    address += sizeof(p.cid);
+    return false;
+}
+
+bool sd_write_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);
+        led_blink_act();
+        if (sd_cmd(23, blocks, RSP_R1, NULL)) {
+            return true;
+        }
+        if (sd_cmd(25, sector, RSP_R1, NULL)) {
+            return true;
+        }
+        sd_dat_prepare(address, blocks, DAT_WRITE);
+        bool error = sd_dat_wait(1000);
+        sd_cmd(12, 0, RSP_R1b, NULL);
+        if (error) {
+            sd_dat_abort();
+            return true;
+        }
+        address += (blocks * SD_BLOCK_SIZE);
+        sector += (blocks * (p.card_type_block ? 1 : SD_BLOCK_SIZE));
+        count -= blocks;
+    }
+
+    return false;
+}
+
+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);
+        led_blink_act();
+        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;
+}
+
 void sd_init (void) {
     p.card_initialized = false;
     sd_set_clock(CLOCK_STOP);
diff --git a/sw/controller/src/sd.h b/sw/controller/src/sd.h
index c7f34ed..5648d8d 100644
--- a/sw/controller/src/sd.h
+++ b/sw/controller/src/sd.h
@@ -5,10 +5,12 @@
 #include <stdbool.h>
 
 
-bool sd_read_sectors (uint32_t address, uint32_t sector, uint32_t count);
-bool sd_write_sectors (uint32_t address, uint32_t sector, uint32_t count);
 bool sd_card_init (void);
 void sd_card_deinit (void);
+bool sd_card_get_status (void);
+bool sd_card_get_info (uint32_t address);
+bool sd_write_sectors (uint32_t address, uint32_t sector, uint32_t count);
+bool sd_read_sectors (uint32_t address, uint32_t sector, uint32_t count);
 void sd_init (void);
 void sd_process (void);