diff --git a/Downstream/Src/downstream_statemachine.c b/Downstream/Src/downstream_statemachine.c
index b09cf4d..ad70681 100644
--- a/Downstream/Src/downstream_statemachine.c
+++ b/Downstream/Src/downstream_statemachine.c
@@ -49,6 +49,7 @@ void Downstream_InitStateMachine(void)
 }
 
 
+
 void Downstream_PacketProcessor(DownstreamPacketTypeDef* receivedPacket)
 {
     if (DownstreamState >= STATE_ERROR)
@@ -57,35 +58,31 @@ void Downstream_PacketProcessor(DownstreamPacketTypeDef* receivedPacket)
         return;
     }
 
-    if (receivedPacket->CommandClass == COMMAND_CLASS_INTERFACE)
+    //Device not connected yet
+    if (DownstreamState != STATE_ACTIVE)
     {
-        if (DownstreamState > STATE_DEVICE_READY)
+        if (receivedPacket->CommandClass == COMMAND_CLASS_INTERFACE)
         {
-            DOWNSTREAM_STATEMACHINE_FREAKOUT;
-            return;
+            Downstream_PacketProcessor_Interface(receivedPacket);
+        }
+        else
+        {
+            //If we get a class-specific message when our device is disconnected,
+            //we need to tell Upstream of the fact (and not freak out).
+            Downstream_PacketProcessor_NotifyDisconnectReply(receivedPacket);
         }
-        Downstream_PacketProcessor_Interface(receivedPacket);
-        return;
-    }
-
-    //If we get a class-specific message when our device is disconnected,
-    //we need to tell Upstream of the fact (and not freak out).
-    if (DownstreamState == STATE_DEVICE_NOT_READY)
-    {
-        Downstream_PacketProcessor_NotifyDisconnectReply(receivedPacket);
         return;
     }
 
-    //We should only receive class-specific messages when we are in the Active state,
+    //Device is connected: expect only class-specific messages,
     //and only to our currently active device class.
-    if ((DownstreamState != STATE_ACTIVE) ||
-        (receivedPacket->CommandClass != ConfiguredDeviceClass))
+    if ((ConfiguredDeviceClass == COMMAND_CLASS_INTERFACE) ||
+        (ConfiguredDeviceClass != receivedPacket->CommandClass))
     {
         DOWNSTREAM_STATEMACHINE_FREAKOUT;
         return;
     }
 
-
     switch (ConfiguredDeviceClass)
     {
 
diff --git a/Upstream/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Inc/usbd_msc.h b/Upstream/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Inc/usbd_msc.h
index 139af0e..1ac6f88 100755
--- a/Upstream/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Inc/usbd_msc.h
+++ b/Upstream/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Inc/usbd_msc.h
@@ -85,6 +85,7 @@ typedef struct
   UpstreamPacketTypeDef*     bot_packet;            //Not NULL indicates we currently own an upstream packet buffer, and should free it when we are done.
   USBD_MSC_BOT_CBWTypeDef    cbw;
   USBD_MSC_BOT_CSWTypeDef    csw;
+  HAL_StatusTypeDef          last_test_unit_result;
   
   USBD_SCSI_SenseTypeDef   scsi_sense [SENSE_LIST_DEPTH];
   uint8_t                  scsi_sense_head;
diff --git a/Upstream/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Src/usbd_msc.c b/Upstream/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Src/usbd_msc.c
index 45ebf0e..8a60f58 100755
--- a/Upstream/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Src/usbd_msc.c
+++ b/Upstream/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Src/usbd_msc.c
@@ -354,7 +354,6 @@ uint8_t  USBD_MSC_Init (USBD_HandleTypeDef *pdev,
   {
     /* Init the BOT  layer */
     MSC_BOT_Init(pdev);
-    ((USBD_MSC_BOT_HandleTypeDef*)pdev->pClassData)->bot_packet = NULL;
     ret = 0;
   }
   
diff --git a/Upstream/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Src/usbd_msc_bot.c b/Upstream/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Src/usbd_msc_bot.c
index 59ba677..3636f35 100755
--- a/Upstream/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Src/usbd_msc_bot.c
+++ b/Upstream/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Src/usbd_msc_bot.c
@@ -117,6 +117,8 @@ void MSC_BOT_Init (USBD_HandleTypeDef  *pdev)
     
   hmsc->bot_state  = USBD_BOT_IDLE;
   hmsc->bot_status = USBD_BOT_STATUS_NORMAL;
+  hmsc->bot_packet = NULL;
+  hmsc->last_test_unit_result = HAL_ERROR;
   
   hmsc->scsi_sense_tail = 0;
   hmsc->scsi_sense_head = 0;
@@ -173,6 +175,7 @@ void MSC_BOT_DeInit (USBD_HandleTypeDef  *pdev)
 void MSC_BOT_DataIn (USBD_HandleTypeDef  *pdev, 
                      uint8_t epnum)
 {
+  UNUSED(epnum);
   USBD_MSC_BOT_HandleTypeDef  *hmsc = (USBD_MSC_BOT_HandleTypeDef*)pdev->pClassData;  
   
   switch (hmsc->bot_state)
@@ -214,6 +217,7 @@ void MSC_BOT_DataIn_Callback(int8_t result)
 void MSC_BOT_DataOut (USBD_HandleTypeDef  *pdev, 
                       uint8_t epnum)
 {
+  UNUSED(epnum);
   USBD_MSC_BOT_HandleTypeDef  *hmsc = (USBD_MSC_BOT_HandleTypeDef*)pdev->pClassData;
   
   switch (hmsc->bot_state)
diff --git a/Upstream/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Src/usbd_msc_scsi.c b/Upstream/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Src/usbd_msc_scsi.c
index 57f2163..d42bfe5 100755
--- a/Upstream/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Src/usbd_msc_scsi.c
+++ b/Upstream/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Src/usbd_msc_scsi.c
@@ -258,6 +258,8 @@ static void SCSI_TestUnitReady(void)
 
 static void SCSI_TestUnitReadyCallback(HAL_StatusTypeDef result)
 {
+    SCSI_ProcessCmd_hmsc->last_test_unit_result = result;
+
     if (result != HAL_OK)
     {
         SCSI_SenseCode(SCSI_ProcessCmd_pdev,
@@ -553,6 +555,7 @@ static void SCSI_StartStopUnit(void)
 {
     if ((SCSI_ProcessCmd_params[4] & START_STOP_DATA_MASK) == START_STOP_DATA_EJECT_STOP_MOTOR)
     {
+        SCSI_ProcessCmd_hmsc->last_test_unit_result = HAL_ERROR;
         if (Upstream_MSC_RequestDisconnect(SCSI_StartStopUnitCallback) != HAL_OK)     //Host is signalling us to disconnect
         {
             SCSI_StartStopUnitCallback(HAL_ERROR);
@@ -605,12 +608,21 @@ static void SCSI_Read10(void)
             SCSI_ProcessCmd_callback(-1);
             return;
         }
+
+        if (SCSI_ProcessCmd_hmsc->last_test_unit_result != HAL_OK)
+        {
+            SCSI_SenseCode(SCSI_ProcessCmd_pdev,
+                         SCSI_ProcessCmd_hmsc->cbw.bLUN,
+                         NOT_READY,
+                         MEDIUM_NOT_PRESENT);
+            SCSI_ProcessCmd_callback(-1);
+            return;
+        }
 
         SCSI_ProcessCmd_hmsc->scsi_blk_addr = (SCSI_ProcessCmd_params[2] << 24) | \
                                               (SCSI_ProcessCmd_params[3] << 16) | \
                                               (SCSI_ProcessCmd_params[4] <<  8) | \
                                               SCSI_ProcessCmd_params[5];
-
         SCSI_ProcessCmd_hmsc->scsi_blk_len =  (SCSI_ProcessCmd_params[7] <<  8) | \
                                               SCSI_ProcessCmd_params[8];
 
@@ -732,6 +744,16 @@ static void SCSI_Write10(void)
             return;
         }
 
+        if (SCSI_ProcessCmd_hmsc->last_test_unit_result != HAL_OK)
+        {
+            SCSI_SenseCode(SCSI_ProcessCmd_pdev,
+                         SCSI_ProcessCmd_hmsc->cbw.bLUN,
+                         NOT_READY,
+                         MEDIUM_NOT_PRESENT);
+            SCSI_ProcessCmd_callback(-1);
+            return;
+        }
+
         SCSI_ProcessCmd_hmsc->scsi_blk_addr = (SCSI_ProcessCmd_params[2] << 24) | \
                                               (SCSI_ProcessCmd_params[3] << 16) | \
                                               (SCSI_ProcessCmd_params[4] <<  8) | \