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 d42bfe5..b9347b6 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
@@ -122,7 +122,8 @@ static void SCSI_Read10ReplyCallback(UpstreamPacketTypeDef* upstreamPacket,
 static void SCSI_Write10BeginCallback(HAL_StatusTypeDef result);
 static void SCSI_Write10FreePacketCallback(UpstreamPacketTypeDef* freePacket);
 static void SCSI_StartStopUnitCallback(HAL_StatusTypeDef result);
-
+static void SCSI_ModeSense6_FreePacketCallback(UpstreamPacketTypeDef* freePacket);
+static void SCSI_ModeSense10_FreePacketCallback(UpstreamPacketTypeDef* freePacket);
 
 
 /**
@@ -433,12 +434,16 @@ static void SCSI_ReadFormatCapacityCallback(UpstreamPacketTypeDef* upstreamPacke
 * @param  params: Command parameters
 * @retval status
 */
-static void SCSI_ModeSense6 (void)
+static void SCSI_ModeSense6(void)
+{
+    Upstream_GetFreePacket(SCSI_ModeSense6_FreePacketCallback);
+}
+
+
+static void SCSI_ModeSense6_FreePacketCallback(UpstreamPacketTypeDef* freePacket)
 {
     uint16_t len = 4;
-    UpstreamPacketTypeDef* freePacket;
 
-    freePacket = Upstream_GetFreePacketImmediately();
     SCSI_ProcessCmd_hmsc->bot_packet = freePacket;
     SCSI_ProcessCmd_hmsc->bot_data = freePacket->Data;
 
@@ -460,11 +465,15 @@ static void SCSI_ModeSense6 (void)
 * @retval status
 */
 static void SCSI_ModeSense10(void)
+{
+    Upstream_GetFreePacket(SCSI_ModeSense10_FreePacketCallback);
+}
+
+
+static void SCSI_ModeSense10_FreePacketCallback(UpstreamPacketTypeDef* freePacket)
 {
     uint16_t len = 8;
-    UpstreamPacketTypeDef* freePacket;
 
-    freePacket = Upstream_GetFreePacketImmediately();
     SCSI_ProcessCmd_hmsc->bot_packet = freePacket;
     SCSI_ProcessCmd_hmsc->bot_data = freePacket->Data;
 
diff --git a/Upstream/Src/main.c b/Upstream/Src/main.c
index b6516d0..bbf3b4a 100755
--- a/Upstream/Src/main.c
+++ b/Upstream/Src/main.c
@@ -67,7 +67,6 @@ int main(void)
     /* Initialize all configured peripherals */
     GPIO_Init();
     LED_Init();
-    USB_Device_Init();
     Upstream_InitStateMachine();
 
     HAL_Delay(200);         //Delay executing WFI to avoid destroying JTAG access
diff --git a/Upstream/Src/upstream_msc.c b/Upstream/Src/upstream_msc.c
index b7306ce..aac61d2 100644
--- a/Upstream/Src/upstream_msc.c
+++ b/Upstream/Src/upstream_msc.c
@@ -38,6 +38,7 @@ uint8_t                                 ReadStreamBusy;
 
 static void Upstream_MSC_TestReadyFreePacketCallback(UpstreamPacketTypeDef* freePacket);
 static void Upstream_MSC_TestReadyReplyCallback(UpstreamPacketTypeDef* replyPacket);
+static void Upstream_MSC_GetCapacityFreePacketCallback(UpstreamPacketTypeDef* freePacket);
 static void Upstream_MSC_GetCapacityReplyCallback(UpstreamPacketTypeDef* replyPacket);
 static void Upstream_MSC_GetStreamDataPacketCallback(UpstreamPacketTypeDef* replyPacket);
 static void Upstream_MSC_BeginReadFreePacketCallback(UpstreamPacketTypeDef* freePacket);
@@ -61,7 +62,7 @@ HAL_StatusTypeDef Upstream_MSC_TestReady(UpstreamMSCCallbackTypeDef callback)
 
 
 
-void Upstream_MSC_TestReadyFreePacketCallback(UpstreamPacketTypeDef* freePacket)
+static void Upstream_MSC_TestReadyFreePacketCallback(UpstreamPacketTypeDef* freePacket)
 {
     freePacket->Length16 = UPSTREAM_PACKET_HEADER_LEN_16;
     freePacket->CommandClass = COMMAND_CLASS_MASS_STORAGE;
@@ -84,7 +85,7 @@ void Upstream_MSC_TestReadyFreePacketCallback(UpstreamPacketTypeDef* freePacket)
 
 
 
-void Upstream_MSC_TestReadyReplyCallback(UpstreamPacketTypeDef* replyPacket)
+static void Upstream_MSC_TestReadyReplyCallback(UpstreamPacketTypeDef* replyPacket)
 {
     if (Upstream_StateMachine_CheckActiveClass() != COMMAND_CLASS_MASS_STORAGE)
     {
@@ -113,34 +114,38 @@ void Upstream_MSC_TestReadyReplyCallback(UpstreamPacketTypeDef* replyPacket)
 
 HAL_StatusTypeDef Upstream_MSC_GetCapacity(UpstreamMSCCallbackUintPacketTypeDef callback)
 {
-    UpstreamPacketTypeDef* freePacket;
-
     if (Upstream_StateMachine_CheckActiveClass() != COMMAND_CLASS_MASS_STORAGE)
     {
         return HAL_ERROR;
     }
 
     GetCapacityCallback = callback;
-    freePacket = Upstream_GetFreePacketImmediately();
-    if (freePacket == NULL)
-    {
-        return HAL_ERROR;
-    }
+    return Upstream_GetFreePacket(Upstream_MSC_GetCapacityFreePacketCallback);
+}
+
 
+
+static void Upstream_MSC_GetCapacityFreePacketCallback(UpstreamPacketTypeDef* freePacket)
+{
     freePacket->Length16 = UPSTREAM_PACKET_HEADER_LEN_16;
     freePacket->CommandClass = COMMAND_CLASS_MASS_STORAGE;
     freePacket->Command = COMMAND_MSC_GET_CAPACITY;
     if (Upstream_TransmitPacket(freePacket) == HAL_OK)
     {
-        return Upstream_ReceivePacket(Upstream_MSC_GetCapacityReplyCallback);
+        if (Upstream_ReceivePacket(Upstream_MSC_GetCapacityReplyCallback) != HAL_OK)
+        {
+            GetCapacityCallback(NULL, 0, 0);
+        }
+        return;
     }
+
     //else:
     Upstream_ReleasePacket(freePacket);
-    return HAL_ERROR;
+    GetCapacityCallback(NULL, 0, 0);
 }
 
 
-void Upstream_MSC_GetCapacityReplyCallback(UpstreamPacketTypeDef* replyPacket)
+static void Upstream_MSC_GetCapacityReplyCallback(UpstreamPacketTypeDef* replyPacket)
 {
     uint32_t block_count;
     uint32_t block_size;
@@ -246,7 +251,7 @@ HAL_StatusTypeDef Upstream_MSC_GetStreamDataPacket(UpstreamMSCCallbackPacketType
 
 
 
-void Upstream_MSC_GetStreamDataPacketCallback(UpstreamPacketTypeDef* replyPacket)
+static void Upstream_MSC_GetStreamDataPacketCallback(UpstreamPacketTypeDef* replyPacket)
 {
     uint32_t dataLength8;
 
@@ -307,7 +312,7 @@ HAL_StatusTypeDef Upstream_MSC_BeginWrite(UpstreamMSCCallbackTypeDef callback,
 
 
 
-void Upstream_MSC_BeginWriteFreePacketCallback(UpstreamPacketTypeDef* freePacket)
+static void Upstream_MSC_BeginWriteFreePacketCallback(UpstreamPacketTypeDef* freePacket)
 {
     freePacket->Length16 = UPSTREAM_PACKET_HEADER_LEN_16 + ((4 * 3) / 2);
     freePacket->CommandClass = COMMAND_CLASS_MASS_STORAGE;
diff --git a/Upstream/Src/upstream_statemachine.c b/Upstream/Src/upstream_statemachine.c
index b8e9be3..650fffd 100644
--- a/Upstream/Src/upstream_statemachine.c
+++ b/Upstream/Src/upstream_statemachine.c
@@ -76,7 +76,7 @@ void Upstream_StateMachine_SetErrorState(void)
     if ((ConfiguredDeviceClass > COMMAND_CLASS_INTERFACE) &&
         (ConfiguredDeviceClass < COMMAND_CLASS_ERROR))
     {
-        USBD_Stop(&hUsbDeviceFS);
+        USBD_DeInit(&hUsbDeviceFS);
     }
 }
 
@@ -228,6 +228,7 @@ void Upstream_StateMachine_NotifyDeviceReplyCallback(UpstreamPacketTypeDef* repl
 
     UpstreamState = STATE_DEVICE_ACTIVE;
     ConfiguredDeviceClass = newActiveClass;
+    USB_Device_Init();
     USBD_RegisterClass(&hUsbDeviceFS, newClassPointer);
     USBD_Start(&hUsbDeviceFS);
 
@@ -245,7 +246,7 @@ void Upstream_StateMachine_DeviceDisconnected(void)
         return;
     }
 
-    USBD_Stop(&hUsbDeviceFS);
+    USBD_DeInit(&hUsbDeviceFS);
     Upstream_GetFreePacket(Upstream_StateMachine_NotifyDevice);
 }