diff --git a/demos/ARMCM3-STM32F103ZG-FATFS/main.c b/demos/ARMCM3-STM32F103ZG-FATFS/main.c
index f8608ffeb..49fa093f0 100644
--- a/demos/ARMCM3-STM32F103ZG-FATFS/main.c
+++ b/demos/ARMCM3-STM32F103ZG-FATFS/main.c
@@ -18,9 +18,16 @@
     along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
 
+#include <stdio.h>
+#include <string.h>
+
 #include "ch.h"
 #include "hal.h"
 #include "test.h"
+#include "shell.h"
+#include "evtimer.h"
+
+#include "ff.h"
 
 /*===========================================================================*/
 /* Card insertion monitor.                                                   */
@@ -114,10 +121,210 @@ static void tmr_init(SDCDriver *sdcp) {
   chSysUnlock();
 }
 
+/*===========================================================================*/
+/* FatFs related.                                                            */
+/*===========================================================================*/
+
+/**
+ * @brief FS object.
+ */
+FATFS SDC_FS;
+
+/* FS mounted and ready.*/
+static bool_t fs_ready = FALSE;
+
+/* Generic large buffer.*/
+uint8_t fbuff[1024];
+
+static FRESULT scan_files(char *path)
+{
+  FRESULT res;
+  FILINFO fno;
+  DIR dir;
+  int i;
+  char *fn;
+
+  res = f_opendir(&dir, path);
+  if (res == FR_OK) {
+    i = strlen(path);
+    for (;;) {
+      res = f_readdir(&dir, &fno);
+      if (res != FR_OK || fno.fname[0] == 0)
+        break;
+      if (fno.fname[0] == '.')
+        continue;
+      fn = fno.fname;
+      if (fno.fattrib & AM_DIR) {
+        sprintf(&path[i], "/%s", fn);
+        res = scan_files(path);
+        if (res != FR_OK)
+          break;
+        path[i] = 0;
+      }
+      else {
+//        iprintf("%s/%s\r\n", path, fn);
+      }
+    }
+  }
+  return res;
+}
+
+
+/*===========================================================================*/
+/* Command line related.                                                     */
+/*===========================================================================*/
+
+#define SHELL_WA_SIZE   THD_WA_SIZE(2048)
+#define TEST_WA_SIZE    THD_WA_SIZE(256)
+
+static void cmd_mem(BaseChannel *chp, int argc, char *argv[]) {
+  size_t n, size;
+  char buf[52];
+
+  (void)argv;
+  if (argc > 0) {
+    shellPrintLine(chp, "Usage: mem");
+    return;
+  }
+  n = chHeapStatus(NULL, &size);
+  sprintf(buf, "core free memory : %u bytes", chCoreStatus());
+  shellPrintLine(chp, buf);
+  sprintf(buf, "heap fragments   : %u", n);
+  shellPrintLine(chp, buf);
+  sprintf(buf, "heap free total  : %u bytes", size);
+  shellPrintLine(chp, buf);
+}
+
+static void cmd_threads(BaseChannel *chp, int argc, char *argv[]) {
+  static const char *states[] = {
+    "READY",
+    "CURRENT",
+    "SUSPENDED",
+    "WTSEM",
+    "WTMTX",
+    "WTCOND",
+    "SLEEPING",
+    "WTEXIT",
+    "WTOREVT",
+    "WTANDEVT",
+    "SNDMSGQ",
+    "SNDMSG",
+    "WTMSG",
+    "FINAL"
+  };
+  Thread *tp;
+  char buf[60];
+
+  (void)argv;
+  if (argc > 0) {
+    shellPrintLine(chp, "Usage: threads");
+    return;
+  }
+  shellPrintLine(chp, "    addr    stack prio refs     state time");
+  tp = chRegFirstThread();
+  do {
+    sprintf(buf, "%8lx %8lx %4u %4i %9s %u",
+            (uint32_t)tp, (uint32_t)tp->p_ctx.r13,
+            (unsigned int)tp->p_prio, tp->p_refs - 1,
+            states[tp->p_state], (unsigned int)tp->p_time);
+    shellPrintLine(chp, buf);
+    tp = chRegNextThread(tp);
+  } while (tp != NULL);
+}
+
+static void cmd_test(BaseChannel *chp, int argc, char *argv[]) {
+  Thread *tp;
+
+  (void)argv;
+  if (argc > 0) {
+    shellPrintLine(chp, "Usage: test");
+    return;
+  }
+  tp = chThdCreateFromHeap(NULL, TEST_WA_SIZE, chThdGetPriority(),
+                           TestThread, chp);
+  if (tp == NULL) {
+    shellPrintLine(chp, "out of memory");
+    return;
+  }
+  chThdWait(tp);
+}
+
+static void cmd_tree(BaseChannel *chp, int argc, char *argv[]) {
+  FRESULT err;
+  DWORD clusters;
+  FATFS *fsp;
+
+  (void)argv;
+  if (argc > 0) {
+    shellPrintLine(chp, "Usage: tree");
+    return;
+  }
+  if (!fs_ready) {
+    shellPrintLine(chp, "File System not mounted");
+    return;
+  }
+  err = f_getfree("/", &clusters, &fsp);
+  if (err != FR_OK) {
+    shellPrintLine(chp, "FS: f_getfree() failed");
+    return;
+  }
+  sprintf((void *)fbuff,
+          "FS: %lu free clusters, %lu sectors per cluster, %lu bytes free",
+          clusters, (uint32_t)SDC_FS.csize,
+          clusters * (uint32_t)SDC_FS.csize * (uint32_t)SDC_BLOCK_SIZE);
+  shellPrintLine(chp, (void *)fbuff);
+  fbuff[0] = 0;
+  scan_files((char *)fbuff);
+}
+
+static const ShellCommand commands[] = {
+  {"mem", cmd_mem},
+  {"threads", cmd_threads},
+  {"test", cmd_test},
+  {"tree", cmd_tree},
+  {NULL, NULL}
+};
+
+static const ShellConfig shell_cfg1 = {
+  (BaseChannel *)&SD2,
+  commands
+};
+
 /*===========================================================================*/
 /* Main and generic code.                                                    */
 /*===========================================================================*/
 
+/*
+ * MMC card insertion event.
+ */
+static void InsertHandler(eventid_t id) {
+  FRESULT err;
+
+  (void)id;
+  /*
+   * On insertion MMC initialization and FS mount.
+   */
+  if (sdcConnect(&SDCD1)) {
+    return;
+  }
+  err = f_mount(0, &SDC_FS);
+  if (err != FR_OK) {
+    sdcDisconnect(&SDCD1);
+    return;
+  }
+  fs_ready = TRUE;
+}
+
+/*
+ * MMC card removal event.
+ */
+static void RemoveHandler(eventid_t id) {
+
+  (void)id;
+  sdcDisconnect(&SDCD1);
+  fs_ready = FALSE;
+}
+
 /*
  * Red LED blinker thread, times are in milliseconds.
  */
diff --git a/os/hal/dox/sdc.dox b/os/hal/dox/sdc.dox
index 2a18977ff..df88796af 100644
--- a/os/hal/dox/sdc.dox
+++ b/os/hal/dox/sdc.dox
@@ -52,9 +52,10 @@
     stop -> stop [label="\nsdcStop()"];
     stop -> ready [label="\nsdcStart()"];
     ready -> stop [label="\nsdcStop()"];
-    ready -> ready [label="\nsdcStart()"];
+    ready -> ready [label="\nsdcStart()\nsdcDisconnect()"];
     ready -> connecting [label="\nsdcConnect()"];
     connecting -> active [label="\nconnection\nsuccessful"];
+    connecting -> active [label="\nsdcConnect()", dir="back"];
     connecting -> ready [label="\nconnection\nfailed"];
     disconnecting -> active [label="\nsdcDisconnect()", dir="back"];
     ready -> disconnecting [label="\ndisconnection\nfinished", dir="back"];
@@ -85,9 +86,10 @@
     stop -> stop [label="\nsdcStop()"];
     stop -> ready [label="\nsdcStart()"];
     ready -> stop [label="\nsdcStop()"];
-    ready -> ready [label="\nsdcStart()"];
+    ready -> ready [label="\nsdcStart()\nsdcDisconnect()"];
     ready -> connecting [label="\nsdcConnect()"];
     connecting -> active [label="\nconnection\nsuccessful"];
+    connecting -> active [label="\nsdcConnect()", dir="back"];
     connecting -> ready [label="\nconnection\nfailed"];
     disconnecting -> active [label="\nsdcDisconnect()", dir="back"];
     ready -> disconnecting [label="\ndisconnection\nfinished", dir="back"];
diff --git a/os/hal/src/sdc.c b/os/hal/src/sdc.c
index a7a39c268..08c667df4 100644
--- a/os/hal/src/sdc.c
+++ b/os/hal/src/sdc.c
@@ -169,7 +169,8 @@ bool_t sdcConnect(SDCDriver *sdcp) {
   chDbgCheck(sdcp != NULL, "sdcConnect");
 
   chSysLock();
-  chDbgAssert(sdcp->state == SDC_READY, "mmcConnect(), #1", "invalid state");
+  chDbgAssert((sdcp->state == SDC_READY) || (sdcp->state == SDC_ACTIVE),
+              "mmcConnect(), #1", "invalid state");
   sdcp->state = SDC_CONNECTING;
   chSysUnlock();
 
@@ -302,6 +303,10 @@ bool_t sdcDisconnect(SDCDriver *sdcp) {
   chSysLock();
   chDbgAssert(sdcp->state == SDC_ACTIVE,
               "sdcDisconnect(), #1", "invalid state");
+  if (sdcp->state == SDC_READY) {
+    chSysUnlock();
+    return FALSE;
+  }
   sdcp->state = SDC_DISCONNECTING;
   chSysUnlock();