diff --git a/stm/main.c b/stm/main.c
index 3425435942f528714e3203f9d84309a07d98a59c..3484fd28d0607338aa4dd84388553586ea803aed 100644
--- a/stm/main.c
+++ b/stm/main.c
@@ -10,6 +10,7 @@
 #include <stm32f4xx_rtc.h>
 #include <stm32f4xx_usart.h>
 #include <stm32f4xx_rng.h>
+#include <usbd_storage_msd.h>
 #include <stm_misc.h>
 #include "std.h"
 
@@ -815,14 +816,6 @@ soft_reset:
         flash_error(4);
     }
 
-#ifdef USE_HOST_MODE
-    // USB host
-    pyb_usb_host_init();
-#elif defined(USE_DEVICE_MODE)
-    // USB device
-    pyb_usb_dev_init();
-#endif
-
     if (first_soft_reset) {
 #if MICROPY_HW_HAS_MMA7660
         // MMA: init and reset address to zero
@@ -839,10 +832,23 @@ soft_reset:
         FRESULT res = f_mount(&fatfs1, "1:", 1);
         if (res != FR_OK) {
             printf("[SD] could not mount SD card\n");
+        } else {
+            if (first_soft_reset) {
+                // use SD card as medium for the USB MSD
+                usbd_storage_select_medium(USBD_STORAGE_MEDIUM_SDCARD);
+            }
         }
     }
 #endif
 
+#ifdef USE_HOST_MODE
+    // USB host
+    pyb_usb_host_init();
+#elif defined(USE_DEVICE_MODE)
+    // USB device
+    pyb_usb_dev_init();
+#endif
+
     // run main script
     {
         vstr_t *vstr = vstr_new();
diff --git a/stm/sdcard.c b/stm/sdcard.c
index 119027efa78f16d13a1e3829a3b2879a1e3fea20..d0ec45a2362faef10b279c3d2cddbb431e4f96df 100644
--- a/stm/sdcard.c
+++ b/stm/sdcard.c
@@ -95,6 +95,12 @@ void sdcard_power_off(void) {
     SD_DeInit();
 }
 
+uint64_t sdcard_get_capacity_in_bytes(void) {
+    SD_CardInfo SDCardInfo;
+    SD_GetCardInfo(&SDCardInfo);
+    return SDCardInfo.CardCapacity;
+}
+
 bool sdcard_read_block(uint8_t *dest, uint32_t block_num) {
     // TODO return error if not powered on
 
diff --git a/stm/sdcard.h b/stm/sdcard.h
index 6461d0f9b59f1e081558e44ebdbf5ecb10cbbff1..da7dbddabb813b5f5f64b3f4cdf0eaa96e23225a 100644
--- a/stm/sdcard.h
+++ b/stm/sdcard.h
@@ -5,6 +5,7 @@ void sdcard_init(void);
 bool sdcard_is_present(void);
 bool sdcard_power_on(void);
 void sdcard_power_off(void);
+uint64_t sdcard_get_capacity_in_bytes(void);
 bool sdcard_read_block(uint8_t *dest, uint32_t block_num);
 bool sdcard_write_block(const uint8_t *src, uint32_t block_num);
 
diff --git a/stm/stmusbd/usbd_storage_msd.c b/stm/stmusbd/usbd_storage_msd.c
index c22abbd11db7b6e560205857de025e21e122942d..d8650280c7ebc133ba027326c869146a96b62dde 100644
--- a/stm/stmusbd/usbd_storage_msd.c
+++ b/stm/stmusbd/usbd_storage_msd.c
@@ -22,142 +22,166 @@
   * See the License for the specific language governing permissions and
   * limitations under the License.
   *
+  * Heavily modified by dpgeorge for Micro Python.
+  *
   ******************************************************************************
-  */ 
+  */
 
-/* Includes ------------------------------------------------------------------*/
 #include "usbd_msc_mem.h"
 #include "usb_conf.h"
+#include "usbd_storage_msd.h"
 
 #include "misc.h"
 #include "storage.h"
 #include "diskio.h"
+#include "sdcard.h"
+
+/******************************************************************************/
+// Callback functions for when the internal flash is the mass storage device
+
+static const int8_t FLASH_STORAGE_Inquirydata[] = { // 36 bytes
+    /* LUN 0 */
+    0x00,
+    0x00, // 0x00 for a fixed drive, 0x80 for a removable drive
+    0x02,
+    0x02,
+    (USBD_STD_INQUIRY_LENGTH - 5),
+    0x00,
+    0x00,
+    0x00,
+    'S', 'T', 'M', ' ', ' ', ' ', ' ', ' ', /* Manufacturer : 8 bytes */
+    'm', 'i', 'c', 'r', 'o', 'S', 'D', ' ', /* Product      : 16 Bytes */
+    'F', 'l', 'a', 's', 'h', ' ', ' ', ' ',
+    '1', '.', '0' ,'0',                     /* Version      : 4 Bytes */
+};
 
-/** @addtogroup STM32_USB_OTG_DEVICE_LIBRARY
-  * @{
+/**
+  * @brief  Initialize the storage medium
+  * @param  lun : logical unit number
+  * @retval Status
   */
+int8_t FLASH_STORAGE_Init(uint8_t lun) {
+    storage_init();
+    return 0;
+}
 
-
-/** @defgroup STORAGE 
-  * @brief media storage application module
-  * @{
-  */ 
-
-/** @defgroup STORAGE_Private_TypesDefinitions
-  * @{
-  */ 
 /**
-  * @}
-  */ 
-
-
-/** @defgroup STORAGE_Private_Defines
-  * @{
-  */ 
+  * @brief  return medium capacity and block size
+  * @param  lun : logical unit number
+  * @param  block_num :  number of physical block
+  * @param  block_size : size of a physical block
+  * @retval Status
+  */
+int8_t FLASH_STORAGE_GetCapacity(uint8_t lun, uint32_t *block_num, uint32_t *block_size) {
+    *block_size = storage_get_block_size();
+    *block_num = storage_get_block_count();
+    return 0;
+}
 
-#define STORAGE_LUN_NBR                  1 
 /**
-  * @}
-  */ 
-
+  * @brief  check whether the medium is ready
+  * @param  lun : logical unit number
+  * @retval Status
+  */
+int8_t FLASH_STORAGE_IsReady(uint8_t lun) {
+    return 0;
+}
 
-/** @defgroup STORAGE_Private_Macros
-  * @{
-  */ 
 /**
-  * @}
-  */ 
-
-
-/** @defgroup STORAGE_Private_Variables
-  * @{
-  */ 
-/* USB Mass storage Standard Inquiry Data */
-const int8_t  STORAGE_Inquirydata[] = {//36
-  
-  /* LUN 0 */
-  0x00,		
-  0x00,		// make it 0x80 for a removable drive
-  0x02,		
-  0x02,
-  (USBD_STD_INQUIRY_LENGTH - 5),
-  0x00,
-  0x00,	
-  0x00,
-  'S', 'T', 'M', ' ', ' ', ' ', ' ', ' ', /* Manufacturer : 8 bytes */
-  'm', 'i', 'c', 'r', 'o', 'S', 'D', ' ', /* Product      : 16 Bytes */
-  'F', 'l', 'a', 's', 'h', ' ', ' ', ' ',
-  '1', '.', '0' ,'0',                     /* Version      : 4 Bytes */
-}; 
+  * @brief  check whether the medium is write-protected
+  * @param  lun : logical unit number
+  * @retval Status
+  */
+int8_t FLASH_STORAGE_IsWriteProtected(uint8_t lun) {
+    return  0;
+}
 
 /**
-  * @}
-  */ 
-
-
-/** @defgroup STORAGE_Private_FunctionPrototypes
-  * @{
-  */ 
-int8_t STORAGE_Init (uint8_t lun);
-
-int8_t STORAGE_GetCapacity (uint8_t lun, 
-                           uint32_t *block_num, 
-                           uint32_t *block_size);
-
-int8_t  STORAGE_IsReady (uint8_t lun);
-
-int8_t  STORAGE_IsWriteProtected (uint8_t lun);
-
-int8_t STORAGE_Read (uint8_t lun, 
-                        uint8_t *buf, 
-                        uint32_t blk_addr,
-                        uint16_t blk_len);
-
-int8_t STORAGE_Write (uint8_t lun, 
-                        uint8_t *buf, 
-                        uint32_t blk_addr,
-                        uint16_t blk_len);
-
-int8_t STORAGE_GetMaxLun (void);
-
-
-USBD_STORAGE_cb_TypeDef USBD_MICRO_SDIO_fops =
-{
-  STORAGE_Init,
-  STORAGE_GetCapacity,
-  STORAGE_IsReady,
-  STORAGE_IsWriteProtected,
-  STORAGE_Read,
-  STORAGE_Write,
-  STORAGE_GetMaxLun,
-  (int8_t *)STORAGE_Inquirydata,
-};
-
-USBD_STORAGE_cb_TypeDef  *USBD_STORAGE_fops = &USBD_MICRO_SDIO_fops;
-/*
-#ifndef USE_STM3210C_EVAL   
-extern SD_CardInfo SDCardInfo;
-#endif
-*/
+  * @brief  Read data from the medium
+  * @param  lun : logical unit number
+  * @param  buf : Pointer to the buffer to save data
+  * @param  blk_addr :  address of 1st block to be read
+  * @param  blk_len : nmber of blocks to be read
+  * @retval Status
+  */
+int8_t FLASH_STORAGE_Read(uint8_t lun, uint8_t *buf, uint32_t blk_addr, uint16_t blk_len) {
+    disk_read(0, buf, blk_addr, blk_len);
+    /*
+    for (int i = 0; i < blk_len; i++) {
+        if (!storage_read_block(buf + i * FLASH_BLOCK_SIZE, blk_addr + i)) {
+            return -1;
+        }
+    }
+    */
+    return 0;
+}
 
 /**
-  * @}
-  */ 
+  * @brief  Write data to the medium
+  * @param  lun : logical unit number
+  * @param  buf : Pointer to the buffer to write from
+  * @param  blk_addr :  address of 1st block to be written
+  * @param  blk_len : nmber of blocks to be read
+  * @retval Status
+  */
+int8_t FLASH_STORAGE_Write (uint8_t lun, uint8_t *buf, uint32_t blk_addr, uint16_t blk_len) {
+    disk_write(0, buf, blk_addr, blk_len);
+    /*
+    for (int i = 0; i < blk_len; i++) {
+        if (!storage_write_block(buf + i * FLASH_BLOCK_SIZE, blk_addr + i)) {
+            return -1;
+        }
+    }
+    */
+    storage_flush(); // XXX hack for now so that the cache is always flushed
+    return 0;
+}
 
+/**
+  * @brief  Return number of supported logical unit
+  * @param  None
+  * @retval number of logical unit
+  */
+int8_t FLASH_STORAGE_GetMaxLun(void) {
+    return 0;
+}
 
-/** @defgroup STORAGE_Private_Functions
-  * @{
-  */ 
+static const USBD_STORAGE_cb_TypeDef USBD_FLASH_STORAGE_fops = {
+    FLASH_STORAGE_Init,
+    FLASH_STORAGE_GetCapacity,
+    FLASH_STORAGE_IsReady,
+    FLASH_STORAGE_IsWriteProtected,
+    FLASH_STORAGE_Read,
+    FLASH_STORAGE_Write,
+    FLASH_STORAGE_GetMaxLun,
+    (int8_t *)FLASH_STORAGE_Inquirydata,
+};
 
+/******************************************************************************/
+// Callback functions for when the SD card is the mass storage device
+
+static const int8_t SDCARD_STORAGE_Inquirydata[] = { // 36 bytes
+    /* LUN 0 */
+    0x00,
+    0x80, // 0x00 for a fixed drive, 0x80 for a removable drive
+    0x02,
+    0x02,
+    (USBD_STD_INQUIRY_LENGTH - 5),
+    0x00,
+    0x00,
+    0x00,
+    'S', 'T', 'M', ' ', ' ', ' ', ' ', ' ', /* Manufacturer : 8 bytes */
+    'm', 'i', 'c', 'r', 'o', 'S', 'D', ' ', /* Product      : 16 Bytes */
+    'S', 'D', ' ', 'c', 'a', 'r', 'd', ' ',
+    '1', '.', '0' ,'0',                     /* Version      : 4 Bytes */
+};
 
 /**
   * @brief  Initialize the storage medium
   * @param  lun : logical unit number
   * @retval Status
   */
-
-int8_t STORAGE_Init (uint8_t lun)
-{
+int8_t SDCARD_STORAGE_Init(uint8_t lun) {
     /*
 #ifndef USE_STM3210C_EVAL 
   NVIC_InitTypeDef NVIC_InitStructure;
@@ -172,8 +196,11 @@ int8_t STORAGE_Init (uint8_t lun)
     return (-1);
   }
   */
+    if (!sdcard_power_on()) {
+        return -1;
+    }
 
-  return (0);
+    return 0;
 
 }
 
@@ -184,29 +211,22 @@ int8_t STORAGE_Init (uint8_t lun)
   * @param  block_size : size of a physical block
   * @retval Status
   */
-int8_t STORAGE_GetCapacity (uint8_t lun, uint32_t *block_num, uint32_t *block_size)
-{
+int8_t SDCARD_STORAGE_GetCapacity(uint8_t lun, uint32_t *block_num, uint32_t *block_size) {
 /*
 #ifdef USE_STM3210C_EVAL   
   SD_CardInfo SDCardInfo;
-  
   SD_GetCardInfo(&SDCardInfo);  
-  
 #else
-  if(SD_GetStatus() != 0 )
-  {
+  if(SD_GetStatus() != 0 ) {
     return (-1); 
   }   
 #endif  
   */
 
-  
-  *block_size = storage_get_block_size();
-  //*block_num =  SDCardInfo.CardCapacity / 512;  
-  *block_num = storage_get_block_count();
-  
-  return (0);
-  
+    *block_size = SDCARD_BLOCK_SIZE;
+    *block_num =  sdcard_get_capacity_in_bytes() / SDCARD_BLOCK_SIZE;
+
+    return 0;
 }
 
 /**
@@ -214,8 +234,7 @@ int8_t STORAGE_GetCapacity (uint8_t lun, uint32_t *block_num, uint32_t *block_si
   * @param  lun : logical unit number
   * @retval Status
   */
-int8_t  STORAGE_IsReady (uint8_t lun)
-{
+int8_t SDCARD_STORAGE_IsReady(uint8_t lun) {
     /*
 #ifndef USE_STM3210C_EVAL  
   
@@ -239,7 +258,7 @@ int8_t  STORAGE_IsReady (uint8_t lun)
   }  
 #endif
 */
-  return (0);
+    return 0;
 }
 
 /**
@@ -247,9 +266,8 @@ int8_t  STORAGE_IsReady (uint8_t lun)
   * @param  lun : logical unit number
   * @retval Status
   */
-int8_t  STORAGE_IsWriteProtected (uint8_t lun)
-{
-  return  0;
+int8_t SDCARD_STORAGE_IsWriteProtected(uint8_t lun) {
+    return 0;
 }
 
 /**
@@ -260,27 +278,25 @@ int8_t  STORAGE_IsWriteProtected (uint8_t lun)
   * @param  blk_len : nmber of blocks to be read
   * @retval Status
   */
-int8_t STORAGE_Read (uint8_t lun, 
-                 uint8_t *buf, 
-                 uint32_t blk_addr,                       
-                 uint16_t blk_len)
-{
+int8_t SDCARD_STORAGE_Read(uint8_t lun, uint8_t *buf, uint32_t blk_addr, uint16_t blk_len) {
+    // TODO replace with call to sdcard_read_multi_blocks
+    for (int i = 0; i < blk_len; i++) {
+        if (!sdcard_read_block(buf + i * SDCARD_BLOCK_SIZE, blk_addr + i)) {
+            return -1;
+        }
+    }
     /*
-  if( SD_ReadMultiBlocks (buf, 
-                          blk_addr * 512, 
-                          512,
-                          blk_len) != 0)
-  {
-    return -1;
-  }
-#ifndef USE_STM3210C_EVAL 
-  SD_WaitReadOperation();
-  while (SD_GetStatus() != SD_TRANSFER_OK);
-#endif    
+    if (SD_ReadMultiBlocks(buf, blk_addr * 512, 512, blk_len) != 0) {
+        return -1;
+    }
+#ifndef USE_STM3210C_EVAL
+    SD_WaitReadOperation();
+    while (SD_GetStatus() != SD_TRANSFER_OK);
+#endif
 */
-    disk_read(0, buf, blk_addr, blk_len);
-  return 0;
+   return 0;
 }
+
 /**
   * @brief  Write data to the medium
   * @param  lun : logical unit number
@@ -289,28 +305,23 @@ int8_t STORAGE_Read (uint8_t lun,
   * @param  blk_len : nmber of blocks to be read
   * @retval Status
   */
-int8_t STORAGE_Write (uint8_t lun, 
-                  uint8_t *buf, 
-                  uint32_t blk_addr,
-                  uint16_t blk_len)
-{
-  
+int8_t SDCARD_STORAGE_Write(uint8_t lun, uint8_t *buf, uint32_t blk_addr, uint16_t blk_len) {
+    // TODO replace with call to sdcard_write_multi_blocks
+    for (int i = 0; i < blk_len; i++) {
+        if (!sdcard_write_block(buf + i * SDCARD_BLOCK_SIZE, blk_addr + i)) {
+            return -1;
+        }
+    }
     /*
-  if( SD_WriteMultiBlocks (buf, 
-                           blk_addr * 512, 
-                           512,
-                           blk_len) != 0)
-  {
+  if( SD_WriteMultiBlocks (buf, blk_addr * 512, 512, blk_len) != 0) {
     return -1;
   }
-#ifndef USE_STM3210C_EVAL  
+#ifndef USE_STM3210C_EVAL
   SD_WaitWriteOperation();
-  while (SD_GetStatus() != SD_TRANSFER_OK);  
-#endif  
+  while (SD_GetStatus() != SD_TRANSFER_OK);
+#endif
 */
-    disk_write(0, buf, blk_addr, blk_len);
-    storage_flush(); // XXX hack for now so that the cache is always flushed
-  return (0);
+    return 0;
 }
 
 /**
@@ -318,24 +329,35 @@ int8_t STORAGE_Write (uint8_t lun,
   * @param  None
   * @retval number of logical unit
   */
-
-int8_t STORAGE_GetMaxLun (void)
-{
-  return (STORAGE_LUN_NBR - 1);
+int8_t SDCARD_STORAGE_GetMaxLun(void) {
+    return 0;
 }
-/**
-  * @}
-  */ 
-
 
-/**
-  * @}
-  */ 
+static const USBD_STORAGE_cb_TypeDef USBD_SDCARD_STORAGE_fops = {
+    SDCARD_STORAGE_Init,
+    SDCARD_STORAGE_GetCapacity,
+    SDCARD_STORAGE_IsReady,
+    SDCARD_STORAGE_IsWriteProtected,
+    SDCARD_STORAGE_Read,
+    SDCARD_STORAGE_Write,
+    SDCARD_STORAGE_GetMaxLun,
+    (int8_t *)SDCARD_STORAGE_Inquirydata,
+};
 
+/******************************************************************************/
+// Callback functions for when the SD card is the mass storage device
 
-/**
-  * @}
-  */ 
+// default to flash as the storage device
+USBD_STORAGE_cb_TypeDef *USBD_STORAGE_fops = (USBD_STORAGE_cb_TypeDef*)&USBD_FLASH_STORAGE_fops;
 
-/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
+void usbd_storage_select_medium(usbd_storage_medium_kind_t medium_kind) {
+    switch (medium_kind) {
+        case USBD_STORAGE_MEDIUM_FLASH:
+            USBD_STORAGE_fops = (USBD_STORAGE_cb_TypeDef*)&USBD_FLASH_STORAGE_fops;
+            break;
 
+        case USBD_STORAGE_MEDIUM_SDCARD:
+            USBD_STORAGE_fops = (USBD_STORAGE_cb_TypeDef*)&USBD_SDCARD_STORAGE_fops;
+            break;
+    }
+}
diff --git a/stm/stmusbd/usbd_storage_msd.h b/stm/stmusbd/usbd_storage_msd.h
new file mode 100644
index 0000000000000000000000000000000000000000..fac7b0469c0cdf53fa36041a175c8594b42edd84
--- /dev/null
+++ b/stm/stmusbd/usbd_storage_msd.h
@@ -0,0 +1,6 @@
+typedef enum {
+    USBD_STORAGE_MEDIUM_FLASH,
+    USBD_STORAGE_MEDIUM_SDCARD,
+} usbd_storage_medium_kind_t;
+
+void usbd_storage_select_medium(usbd_storage_medium_kind_t medium_kind);