diff --git a/testhal/STM32/STM32F4xx/SDC/Makefile b/testhal/STM32/STM32F4xx/SDC/Makefile
index c60993d6e..977110756 100644
--- a/testhal/STM32/STM32F4xx/SDC/Makefile
+++ b/testhal/STM32/STM32F4xx/SDC/Makefile
@@ -5,7 +5,7 @@
 
 # Compiler options here.
 ifeq ($(USE_OPT),)
-  USE_OPT = -O2 -ggdb -fomit-frame-pointer -falign-functions=16
+  USE_OPT = -O0 -ggdb -fomit-frame-pointer -falign-functions=16
 endif
 
 # C specific options here (added to USE_OPT).
@@ -43,6 +43,8 @@ ifeq ($(USE_VERBOSE_COMPILE),)
   USE_VERBOSE_COMPILE = no
 endif
 
+USE_SMART_BUILD = yes
+
 #
 # Build global options
 ##############################################################################
diff --git a/testhal/STM32/STM32F4xx/SDC/debug/STM32F4xx-SDC (OpenOCD, Flash and Run).launch b/testhal/STM32/STM32F4xx/SDC/debug/STM32F4xx-SDC (OpenOCD, Flash and Run).launch
index 2d6a432db..54dd80e2e 100644
--- a/testhal/STM32/STM32F4xx/SDC/debug/STM32F4xx-SDC (OpenOCD, Flash and Run).launch	
+++ b/testhal/STM32/STM32F4xx/SDC/debug/STM32F4xx-SDC (OpenOCD, Flash and Run).launch	
@@ -35,7 +35,7 @@
 <stringAttribute key="org.eclipse.cdt.launch.DEBUGGER_REGISTER_GROUPS" value=""/>
 <stringAttribute key="org.eclipse.cdt.launch.FORMAT" value="&lt;?xml version=&quot;1.0&quot; encoding=&quot;UTF-8&quot; standalone=&quot;no&quot;?&gt;&lt;contentList&gt;&lt;content id=&quot;CR2-adc-adcp-adc_lld_start_conversion-(format)&quot; val=&quot;4&quot;/&gt;&lt;content id=&quot;CR2-adc-null-port_wait_for_interrupt-(format)&quot; val=&quot;4&quot;/&gt;&lt;content id=&quot;cr2-adc_lld_start_conversion-(format)&quot; val=&quot;4&quot;/&gt;&lt;/contentList&gt;"/>
 <stringAttribute key="org.eclipse.cdt.launch.GLOBAL_VARIABLES" value="&lt;?xml version=&quot;1.0&quot; encoding=&quot;UTF-8&quot; standalone=&quot;no&quot;?&gt;&#13;&#10;&lt;globalVariableList/&gt;&#13;&#10;"/>
-<stringAttribute key="org.eclipse.cdt.launch.MEMORY_BLOCKS" value="&lt;?xml version=&quot;1.0&quot; encoding=&quot;UTF-8&quot; standalone=&quot;no&quot;?&gt;&#13;&#10;&lt;memoryBlockExpressionList/&gt;&#13;&#10;"/>
+<stringAttribute key="org.eclipse.cdt.launch.MEMORY_BLOCKS" value="&lt;?xml version=&quot;1.0&quot; encoding=&quot;UTF-8&quot; standalone=&quot;no&quot;?&gt;&#13;&#10;&lt;memoryBlockExpressionList&gt;&#13;&#10;&lt;memoryBlockExpressionItem&gt;&#13;&#10;&lt;expression text=&quot;0x200014e8&quot;/&gt;&#13;&#10;&lt;/memoryBlockExpressionItem&gt;&#13;&#10;&lt;/memoryBlockExpressionList&gt;&#13;&#10;"/>
 <stringAttribute key="org.eclipse.cdt.launch.PROGRAM_NAME" value="./build/ch.elf"/>
 <stringAttribute key="org.eclipse.cdt.launch.PROJECT_ATTR" value="STM32F4xx-SDC"/>
 <booleanAttribute key="org.eclipse.cdt.launch.PROJECT_BUILD_CONFIG_AUTO_ATTR" value="true"/>
diff --git a/testhal/STM32/STM32F4xx/SDC/main.c b/testhal/STM32/STM32F4xx/SDC/main.c
index ac7be48ee..93538ae5c 100644
--- a/testhal/STM32/STM32F4xx/SDC/main.c
+++ b/testhal/STM32/STM32F4xx/SDC/main.c
@@ -59,9 +59,10 @@ static THD_FUNCTION(Thread1, arg) {
 
 #define SDC_BURST_SIZE      16
 
-/* Buffer for block read/write operations, note that an extra byte is
+/* Buffer for block read/write operations, note that extra bytes are
    allocated in order to support unaligned operations.*/
-static uint8_t buf[MMCSD_BLOCK_SIZE * SDC_BURST_SIZE + 1];
+static uint8_t buf[MMCSD_BLOCK_SIZE * SDC_BURST_SIZE + 4];
+
 /* Additional buffer for sdcErase() test */
 static uint8_t buf2[MMCSD_BLOCK_SIZE * SDC_BURST_SIZE ];
 
@@ -93,7 +94,7 @@ void cmd_sdc(BaseSequentialStream *chp, int argc, char *argv[]) {
            SDCD1.csd[3], SDCD1.csd[2], SDCD1.csd[1], SDCD1.csd[0]);
   chprintf(chp, "CID      : %08X %8X %08X %08X \r\n",
            SDCD1.cid[3], SDCD1.cid[2], SDCD1.cid[1], SDCD1.cid[0]);
-  chprintf(chp, "Mode     : %s\r\n", mode[SDCD1.cardmode]);
+  chprintf(chp, "Mode     : %s\r\n", mode[SDCD1.cardmode & 3U]);
   chprintf(chp, "Capacity : %DMB\r\n", SDCD1.capacity / 2048);
 
   /* The test is performed in the middle of the flash area.*/
@@ -161,6 +162,43 @@ void cmd_sdc(BaseSequentialStream *chp, int argc, char *argv[]) {
 #endif /* STM32_SDC_SDIO_UNALIGNED_SUPPORT */
   }
 
+  if ((strcmp(argv[0], "write") == 0) ||
+      (strcmp(argv[0], "all") == 0)) {
+    unsigned i;
+
+    memset(buf, 0xAA, MMCSD_BLOCK_SIZE * 2);
+    chprintf(chp, "Writing...");
+    if(sdcWrite(&SDCD1, startblk, buf, 2)) {
+      chprintf(chp, "failed\r\n");
+      goto exittest;
+    }
+    chprintf(chp, "OK\r\n");
+
+    memset(buf, 0x55, MMCSD_BLOCK_SIZE * 2);
+    chprintf(chp, "Reading...");
+    if (blkRead(&SDCD1, startblk, buf, 1)) {
+      chprintf(chp, "failed\r\n");
+      goto exittest;
+    }
+    chprintf(chp, "OK\r\n");
+
+    for (i = 0; i < MMCSD_BLOCK_SIZE; i++)
+      buf[i] = i + 8;
+    chprintf(chp, "Writing...");
+    if(sdcWrite(&SDCD1, startblk, buf, 2)) {
+      chprintf(chp, "failed\r\n");
+      goto exittest;
+    }
+    chprintf(chp, "OK\r\n");
+
+    memset(buf, 0, MMCSD_BLOCK_SIZE * 2);
+    chprintf(chp, "Reading...");
+    if (blkRead(&SDCD1, startblk, buf, 1)) {
+      chprintf(chp, "failed\r\n");
+      goto exittest;
+    }
+    chprintf(chp, "OK\r\n");
+  }
 
   if ((strcmp(argv[0], "erase") == 0) ||
       (strcmp(argv[0], "all") == 0)) {
@@ -235,12 +273,6 @@ void cmd_sdc(BaseSequentialStream *chp, int argc, char *argv[]) {
     /* END of sdcErase() test */
   }
   
-
-  if ((strcmp(argv[0], "write") == 0) ||
-      (strcmp(argv[0], "all") == 0)) {
-
-  }
-
   /* Card disconnect and command end.*/
 exittest:
   sdcDisconnect(&SDCD1);