From b96c5390889bc40ce45ca9a3a69315d684f42018 Mon Sep 17 00:00:00 2001
From: Robert Fisk <robert_fisk@fastmail.fm>
Date: Tue, 31 May 2016 22:43:57 +1200
Subject: [PATCH] Implemented FIFO support in Downstream host, significant
 speed increase!

Each USB transaction passed to the driver now consists of multiple
64-byte packets. 8 packets when receiving, 4 packets when transmitting.
The STM32 silicon bugs out when more than 4 packets are scheduled to
write at a time :(

Reads 1.0MB/sec, writes 967kB/sec, not CPU limited :)
---
 Downstream/.cproject                          |   6 +-
 .../Inc/stm32f4xx_hal_hcd.h                   |   7 +-
 .../Inc/stm32f4xx_ll_usb.h                    |  43 ++--
 .../Src/stm32f4xx_hal_hcd.c                   | 232 +++++++++++-------
 .../Src/stm32f4xx_ll_usb.c                    |  96 ++++++--
 .../Class/MSC/Src/usbh_msc_bot.c              |  28 +--
 .../Core/Inc/usbh_def.h                       |   7 +-
 .../Core/Src/usbh_pipes.c                     |   7 +-
 Upstream/.cproject                            |   2 +-
 9 files changed, 276 insertions(+), 152 deletions(-)

diff --git a/Downstream/.cproject b/Downstream/.cproject
index d86772c..bd2671c 100644
--- a/Downstream/.cproject
+++ b/Downstream/.cproject
@@ -47,7 +47,7 @@
 							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.warnings.logicalop.1966851850" name="Warn if suspicious logical ops (-Wlogical-op)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.warnings.logicalop" value="true" valueType="boolean"/>
 							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.warnings.pointerarith.1529172556" name="Warn if pointer arithmetic (-Wpointer-arith)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.warnings.pointerarith" value="true" valueType="boolean"/>
 							<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="ilg.gnuarmeclipse.managedbuild.cross.targetPlatform.1042769951" isAbstract="false" osList="all" superClass="ilg.gnuarmeclipse.managedbuild.cross.targetPlatform"/>
-							<builder buildPath="${workspace_loc:/Downstream}/Debug" id="ilg.gnuarmeclipse.managedbuild.cross.builder.2070257479" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="optimal" superClass="ilg.gnuarmeclipse.managedbuild.cross.builder"/>
+							<builder buildPath="${workspace_loc:/Downstream}/Debug" id="ilg.gnuarmeclipse.managedbuild.cross.builder.2070257479" keepEnvironmentInBuildfile="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="optimal" superClass="ilg.gnuarmeclipse.managedbuild.cross.builder"/>
 							<tool id="ilg.gnuarmeclipse.managedbuild.cross.tool.assembler.789837881" name="Cross ARM GNU Assembler" superClass="ilg.gnuarmeclipse.managedbuild.cross.tool.assembler">
 								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.assembler.usepreprocessor.1524992420" name="Use preprocessor" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.assembler.usepreprocessor" value="true" valueType="boolean"/>
 								<inputType id="ilg.gnuarmeclipse.managedbuild.cross.tool.assembler.input.1659311775" superClass="ilg.gnuarmeclipse.managedbuild.cross.tool.assembler.input"/>
@@ -128,6 +128,8 @@
 					<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
 					<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
 					<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
 					<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
 				</extensions>
 			</storageModule>
@@ -164,7 +166,7 @@
 							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.warnings.pointerarith.1192110404" name="Warn if pointer arithmetic (-Wpointer-arith)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.warnings.pointerarith" value="true" valueType="boolean"/>
 							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.warnings.logicalop.322411367" name="Warn if suspicious logical ops (-Wlogical-op)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.warnings.logicalop" value="true" valueType="boolean"/>
 							<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="ilg.gnuarmeclipse.managedbuild.cross.targetPlatform.777514892" isAbstract="false" osList="all" superClass="ilg.gnuarmeclipse.managedbuild.cross.targetPlatform"/>
-							<builder autoBuildTarget="all" buildPath="${workspace_loc:/Downstream}/Release" cleanBuildTarget="clean" command="${cross_make}" id="org.eclipse.cdt.build.core.internal.builder.1921336679" incrementalBuildTarget="all" keepEnvironmentInBuildfile="false" managedBuildOn="true" name="CDT Internal Builder" parallelBuildOn="true" parallelizationNumber="optimal" superClass="org.eclipse.cdt.build.core.internal.builder"/>
+							<builder buildPath="${workspace_loc:/Downstream}/Release" id="ilg.gnuarmeclipse.managedbuild.cross.builder.1006388187" keepEnvironmentInBuildfile="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="optimal" superClass="ilg.gnuarmeclipse.managedbuild.cross.builder"/>
 							<tool id="ilg.gnuarmeclipse.managedbuild.cross.tool.assembler.314526004" name="Cross ARM GNU Assembler" superClass="ilg.gnuarmeclipse.managedbuild.cross.tool.assembler">
 								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.assembler.usepreprocessor.2143421487" name="Use preprocessor" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.assembler.usepreprocessor" value="true" valueType="boolean"/>
 								<inputType id="ilg.gnuarmeclipse.managedbuild.cross.tool.assembler.input.2143048963" superClass="ilg.gnuarmeclipse.managedbuild.cross.tool.assembler.input"/>
diff --git a/Downstream/Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_hcd.h b/Downstream/Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_hcd.h
index a2bc45f..d436361 100644
--- a/Downstream/Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_hcd.h
+++ b/Downstream/Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_hcd.h
@@ -82,12 +82,15 @@ typedef USB_OTG_HCStateTypeDef  HCD_HCStateTypeDef ;
 
 /** @defgroup HCD_Exported_Types_Group2 HCD Handle Structure definition   
   * @{
-  */ 
+  */
+
+#define HOST_CHANNEL_COUNT      15
+
 typedef struct
 {
   HCD_TypeDef               *Instance;  /*!< Register base address    */ 
   HCD_InitTypeDef           Init;       /*!< HCD required parameters  */
-  HCD_HCTypeDef             hc[15];     /*!< Host channels parameters */
+  HCD_HCTypeDef             hc[HOST_CHANNEL_COUNT];     /*!< Host channels parameters */
   HAL_LockTypeDef           Lock;       /*!< HCD peripheral status    */
   __IO HCD_StateTypeDef     State;      /*!< HCD communication state  */
   void                      *pData;     /*!< Pointer Stack Handler    */     
diff --git a/Downstream/Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_usb.h b/Downstream/Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_usb.h
index 246108a..bb1c611 100644
--- a/Downstream/Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_usb.h
+++ b/Downstream/Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_usb.h
@@ -172,6 +172,25 @@ typedef struct
 
 typedef struct
 {
+  uint8_t   *xfer_buff;    /*!< Pointer to transfer buffer.                                                */
+
+  uint32_t  xfer_len;      /*!< Current transfer length.                                                   */
+
+  uint32_t  xfer_count;    /*!< Partial transfer length in case of multi packet transfer.                  */
+
+  uint32_t  dma_addr;      /*!< 32 bits aligned transfer buffer address.                                   */
+
+  uint32_t  ErrCnt;        /*!< Host channel error count.*/
+
+  USB_OTG_URBStateTypeDef  urb_state;  /*!< URB state.
+                                           This parameter can be any value of @ref USB_OTG_URBStateTypeDef */
+
+  USB_OTG_HCStateTypeDef   state;     /*!< Host Channel state.
+                                           This parameter can be any value of @ref USB_OTG_HCStateTypeDef  */
+
+  uint16_t  max_packet;    /*!< Endpoint Max packet size.
+                                This parameter must be a number between Min_Data = 0 and Max_Data = 64KB   */
+
   uint8_t   dev_addr ;     /*!< USB device address.
                                 This parameter must be a number between Min_Data = 1 and Max_Data = 255    */ 
 
@@ -194,17 +213,8 @@ typedef struct
   uint8_t   ep_type;       /*!< Endpoint Type.
                                 This parameter can be any value of @ref USB_EP_Type_                       */
                                 
-  uint16_t  max_packet;    /*!< Endpoint Max packet size.
-                                This parameter must be a number between Min_Data = 0 and Max_Data = 64KB   */
-                                
   uint8_t   data_pid;      /*!< Initial data PID.
                                 This parameter must be a number between Min_Data = 0 and Max_Data = 1      */
-                                
-  uint8_t   *xfer_buff;    /*!< Pointer to transfer buffer.                                                */
-  
-  uint32_t  xfer_len;      /*!< Current transfer length.                                                   */
-  
-  uint32_t  xfer_count;    /*!< Partial transfer length in case of multi packet transfer.                  */
   
   uint8_t   toggle_in;     /*!< IN transfer current toggle flag.
                                 This parameter must be a number between Min_Data = 0 and Max_Data = 1      */
@@ -212,15 +222,8 @@ typedef struct
   uint8_t   toggle_out;    /*!< OUT transfer current toggle flag
                                 This parameter must be a number between Min_Data = 0 and Max_Data = 1      */
   
-  uint32_t  dma_addr;      /*!< 32 bits aligned transfer buffer address.                                   */
-  
-  uint32_t  ErrCnt;        /*!< Host channel error count.*/
-  
-  USB_OTG_URBStateTypeDef  urb_state;  /*!< URB state. 
-                                           This parameter can be any value of @ref USB_OTG_URBStateTypeDef */ 
+  uint8_t   packet_count;
   
-  USB_OTG_HCStateTypeDef   state;     /*!< Host Channel state. 
-                                           This parameter can be any value of @ref USB_OTG_HCStateTypeDef  */ 
                                              
 }USB_OTG_HCTypeDef;
   
@@ -355,7 +358,7 @@ typedef struct
 #define HPRT0_PRTSPD_LOW_SPEED                 2
 /**
   * @}
-  */  
+  */
    
 #define HCCHAR_CTRL                            0
 #define HCCHAR_ISOC                            1
@@ -371,6 +374,9 @@ typedef struct
 #define GRXSTS_PKTSTS_IN_XFER_COMP             3
 #define GRXSTS_PKTSTS_DATA_TOGGLE_ERR          5
 #define GRXSTS_PKTSTS_CH_HALTED                7
+
+#define HxTXSTS_xTXQSAV_SHIFT                  16
+#define HxTXSTS_xTXQSAV_MASK                   0xFF
     
 #define USBx_PCGCCTL    *(__IO uint32_t *)((uint32_t)USBx + USB_OTG_PCGCCTL_BASE)
 #define USBx_HPRT0      *(__IO uint32_t *)((uint32_t)USBx + USB_OTG_HOST_PORT_BASE)
@@ -440,6 +446,7 @@ HAL_StatusTypeDef USB_HC_Init(USB_OTG_GlobalTypeDef *USBx,
                                   uint8_t ep_type,
                                   uint16_t mps);
 HAL_StatusTypeDef USB_HC_StartXfer(USB_OTG_GlobalTypeDef *USBx, USB_OTG_HCTypeDef *hc, uint8_t dma);
+HAL_StatusTypeDef USB_HC_WriteEmptyTxFifo(USB_OTG_GlobalTypeDef *USBx, USB_OTG_HCTypeDef *hc, uint8_t periodic);
 uint32_t          USB_HC_ReadInterrupt (USB_OTG_GlobalTypeDef *USBx);
 HAL_StatusTypeDef USB_HC_Halt(USB_OTG_GlobalTypeDef *USBx , uint8_t hc_num);
 HAL_StatusTypeDef USB_DoPing(USB_OTG_GlobalTypeDef *USBx , uint8_t ch_num);
diff --git a/Downstream/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_hcd.c b/Downstream/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_hcd.c
index 3b58821..b4c2677 100644
--- a/Downstream/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_hcd.c
+++ b/Downstream/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_hcd.c
@@ -202,6 +202,8 @@ HAL_StatusTypeDef HAL_HCD_HC_Init(HCD_HandleTypeDef *hhcd,
   hhcd->hc[ch_num].ep_num = epnum & 0x7F;
   hhcd->hc[ch_num].ep_is_in = ((epnum & 0x80) == 0x80);
   hhcd->hc[ch_num].speed = speed;
+  hhcd->hc[ch_num].xfer_len = 0;
+  hhcd->hc[ch_num].xfer_count = 0;
   
   status =  USB_HC_Init(hhcd->Instance, 
                         ch_num,
@@ -462,30 +464,16 @@ void HAL_HCD_IRQHandler(HCD_HandleTypeDef *hhcd)
       return;
     }
     
-    if(__HAL_HCD_GET_FLAG(hhcd, USB_OTG_GINTSTS_PXFR_INCOMPISOOUT))
-    {
-      /* Incorrect mode, acknowledge the interrupt */
-      __HAL_HCD_CLEAR_FLAG(hhcd, USB_OTG_GINTSTS_PXFR_INCOMPISOOUT);
-    }
-    
-    if(__HAL_HCD_GET_FLAG(hhcd, USB_OTG_GINTSTS_IISOIXFR))
+    /* Handle Rx Queue Level Interrupts */
+    if(__HAL_HCD_GET_FLAG(hhcd, USB_OTG_GINTSTS_RXFLVL))
     {
-      /* Incorrect mode, acknowledge the interrupt */
-      __HAL_HCD_CLEAR_FLAG(hhcd, USB_OTG_GINTSTS_IISOIXFR);
+    USB_MASK_INTERRUPT(hhcd->Instance, USB_OTG_GINTSTS_RXFLVL);
+
+    HCD_RXQLVL_IRQHandler (hhcd);
+
+    USB_UNMASK_INTERRUPT(hhcd->Instance, USB_OTG_GINTSTS_RXFLVL);
     }
-    
-    if(__HAL_HCD_GET_FLAG(hhcd, USB_OTG_GINTSTS_PTXFE))
-    {
-      /* Incorrect mode, acknowledge the interrupt */
-      __HAL_HCD_CLEAR_FLAG(hhcd, USB_OTG_GINTSTS_PTXFE);
-    }   
-    
-    if(__HAL_HCD_GET_FLAG(hhcd, USB_OTG_GINTSTS_MMIS))
-    {
-      /* Incorrect mode, acknowledge the interrupt */
-      __HAL_HCD_CLEAR_FLAG(hhcd, USB_OTG_GINTSTS_MMIS);
-    }     
-    
+
     /* Handle Host Disconnect Interrupts */
     if(__HAL_HCD_GET_FLAG(hhcd, USB_OTG_GINTSTS_DISCINT))
     {
@@ -532,17 +520,85 @@ void HAL_HCD_IRQHandler(HCD_HandleTypeDef *hhcd)
         }
       }
       __HAL_HCD_CLEAR_FLAG(hhcd, USB_OTG_GINTSTS_HCINT);
-    } 
-    
-    /* Handle Rx Queue Level Interrupts */
-    if(__HAL_HCD_GET_FLAG(hhcd, USB_OTG_GINTSTS_RXFLVL))
+    }
+
+    if(__HAL_HCD_GET_FLAG(hhcd, USB_OTG_GINTSTS_PXFR_INCOMPISOOUT))
     {
-      USB_MASK_INTERRUPT(hhcd->Instance, USB_OTG_GINTSTS_RXFLVL);
-      
-      HCD_RXQLVL_IRQHandler (hhcd);
-      
-      USB_UNMASK_INTERRUPT(hhcd->Instance, USB_OTG_GINTSTS_RXFLVL);
+      /* Incorrect mode, acknowledge the interrupt */
+      __HAL_HCD_CLEAR_FLAG(hhcd, USB_OTG_GINTSTS_PXFR_INCOMPISOOUT);
     }
+
+    if(__HAL_HCD_GET_FLAG(hhcd, USB_OTG_GINTSTS_IISOIXFR))
+    {
+      /* Incorrect mode, acknowledge the interrupt */
+      __HAL_HCD_CLEAR_FLAG(hhcd, USB_OTG_GINTSTS_IISOIXFR);
+    }
+
+    if(__HAL_HCD_GET_FLAG(hhcd, USB_OTG_GINTSTS_MMIS))
+    {
+      /* Incorrect mode, acknowledge the interrupt */
+      __HAL_HCD_CLEAR_FLAG(hhcd, USB_OTG_GINTSTS_MMIS);
+    }
+
+//If I needed FIFO (half-)empty interrupt handlers, this is what they would look like:
+//
+//    if(__HAL_HCD_GET_FLAG(hhcd, USB_OTG_GINTSTS_NPTXFE))
+//    {
+//        halResult = HAL_OK;
+//
+//        //Find a channel that wants to write something:
+//        for (i = 0; i < hhcd->Init.Host_channels; i++)
+//        {
+//            if (hhcd->hc[i].ep_is_in == 0)
+//            {
+//                if ((hhcd->hc[i].ep_type == EP_TYPE_CTRL) ||
+//                    (hhcd->hc[i].ep_type == EP_TYPE_BULK))
+//                {
+//                    if (hhcd->hc[i].xfer_count < hhcd->hc[i].xfer_len)
+//                    {
+//                        halResult = USB_HC_WriteEmptyTxFifo(hhcd->Instance, &hhcd->hc[i], 0);
+//                        if (halResult == HAL_BUSY) break;       //Fifo is full, so bail out now
+//                    }
+//                }
+//            }
+//        }
+//        if (halResult != HAL_BUSY)
+//        {
+//            //Fifo is not full, ergo we have sent all the data we have to transmit.
+//            //So we can disable the fifo interrupt now:
+//            USB_MASK_INTERRUPT(hhcd->Instance, USB_OTG_GINTMSK_NPTXFEM);
+//        }
+//    }
+//
+//    if(__HAL_HCD_GET_FLAG(hhcd, USB_OTG_GINTSTS_PTXFE))
+//    {
+//        halResult = HAL_OK;
+//
+//        //Find a channel that wants to write something:
+//        for (i = 0; i < hhcd->Init.Host_channels; i++)
+//        {
+//            if (hhcd->hc[i].ep_is_in == 0)
+//            {
+//                if ((hhcd->hc[i].ep_type == EP_TYPE_INTR) ||
+//                    (hhcd->hc[i].ep_type == EP_TYPE_ISOC))
+//                {
+//                    if (hhcd->hc[i].xfer_count < hhcd->hc[i].xfer_len)
+//                    {
+//                        halResult = USB_HC_WriteEmptyTxFifo(hhcd->Instance, &hhcd->hc[i], 1);
+//                        if (halResult == HAL_BUSY) break;       //Fifo is full, so bail out now
+//                    }
+//                }
+//            }
+//        }
+//
+//        if (halResult != HAL_BUSY)
+//        {
+//            //Fifo is not full, ergo we have sent all the data we have to transmit.
+//            //So we can disable the fifo interrupt now:
+//            USB_MASK_INTERRUPT(hhcd->Instance, USB_OTG_GINTMSK_PTXFEM);
+//        }
+//    }
+
   }
 }
 
@@ -796,36 +852,38 @@ static void HCD_HC_IN_IRQHandler(HCD_HandleTypeDef *hhcd, uint8_t chnum)
   {
     __HAL_HCD_CLEAR_HC_INT(chnum, USB_OTG_HCINT_AHBERR);
     __HAL_HCD_UNMASK_HALT_HC_INT(chnum);
-  }  
+  }
+
   else if ((USBx_HC(chnum)->HCINT) &  USB_OTG_HCINT_ACK)
   {
     __HAL_HCD_CLEAR_HC_INT(chnum, USB_OTG_HCINT_ACK);
   }
-  
-  else if ((USBx_HC(chnum)->HCINT) &  USB_OTG_HCINT_STALL)  
+
+  else if ((USBx_HC(chnum)->HCINT) &  USB_OTG_HCINT_STALL)
   {
     __HAL_HCD_UNMASK_HALT_HC_INT(chnum);
     hhcd->hc[chnum].state = HC_STALL;
     __HAL_HCD_CLEAR_HC_INT(chnum, USB_OTG_HCINT_NAK);
-    __HAL_HCD_CLEAR_HC_INT(chnum, USB_OTG_HCINT_STALL);    
-    USB_HC_Halt(hhcd->Instance, chnum);    
+    __HAL_HCD_CLEAR_HC_INT(chnum, USB_OTG_HCINT_STALL);
+    USB_HC_Halt(hhcd->Instance, chnum);
   }
+
   else if ((USBx_HC(chnum)->HCINT) &  USB_OTG_HCINT_DTERR)
   {
     __HAL_HCD_UNMASK_HALT_HC_INT(chnum);
-    USB_HC_Halt(hhcd->Instance, chnum);  
-    __HAL_HCD_CLEAR_HC_INT(chnum, USB_OTG_HCINT_NAK);    
+    USB_HC_Halt(hhcd->Instance, chnum);
+    __HAL_HCD_CLEAR_HC_INT(chnum, USB_OTG_HCINT_NAK);
     hhcd->hc[chnum].state = HC_DATATGLERR;
     __HAL_HCD_CLEAR_HC_INT(chnum, USB_OTG_HCINT_DTERR);
-  }    
-  
-  if ((USBx_HC(chnum)->HCINT) &  USB_OTG_HCINT_FRMOR)
+  }
+
+  else if ((USBx_HC(chnum)->HCINT) &  USB_OTG_HCINT_FRMOR)
   {
-    __HAL_HCD_UNMASK_HALT_HC_INT(chnum); 
-    USB_HC_Halt(hhcd->Instance, chnum);  
+    __HAL_HCD_UNMASK_HALT_HC_INT(chnum);
+    USB_HC_Halt(hhcd->Instance, chnum);
     __HAL_HCD_CLEAR_HC_INT(chnum, USB_OTG_HCINT_FRMOR);
   }
-  
+
   else if ((USBx_HC(chnum)->HCINT) &  USB_OTG_HCINT_XFRC)
   {
     if (hhcd->Init.dma_enable)
@@ -853,9 +911,9 @@ static void HCD_HC_IN_IRQHandler(HCD_HandleTypeDef *hhcd, uint8_t chnum)
       hhcd->hc[chnum].urb_state = URB_DONE; 
       HAL_HCD_HC_NotifyURBChange_Callback(hhcd, chnum, hhcd->hc[chnum].urb_state);
     }
-    hhcd->hc[chnum].toggle_in ^= 1;
-    
+//    hhcd->hc[chnum].toggle_in ^= 1;
   }
+
   else if ((USBx_HC(chnum)->HCINT) &  USB_OTG_HCINT_CHH)
   {
     __HAL_HCD_MASK_HALT_HC_INT(chnum); 
@@ -905,6 +963,7 @@ static void HCD_HC_IN_IRQHandler(HCD_HandleTypeDef *hhcd, uint8_t chnum)
     USB_HC_Halt(hhcd->Instance, chnum);     
     __HAL_HCD_CLEAR_HC_INT(chnum, USB_OTG_HCINT_TXERR);
   }
+
   else if ((USBx_HC(chnum)->HCINT) &  USB_OTG_HCINT_NAK)
   {  
       hhcd->hc[chnum].ErrCnt = 0;
@@ -943,16 +1002,17 @@ static void HCD_HC_OUT_IRQHandler  (HCD_HandleTypeDef *hhcd, uint8_t chnum)
   {
     __HAL_HCD_CLEAR_HC_INT(chnum, USB_OTG_HCINT_AHBERR);
     __HAL_HCD_UNMASK_HALT_HC_INT(chnum);
-  }  
+  }
+
   else if ((USBx_HC(chnum)->HCINT) &  USB_OTG_HCINT_ACK)
   {
     __HAL_HCD_CLEAR_HC_INT(chnum, USB_OTG_HCINT_ACK);
-    
+
     if( hhcd->hc[chnum].do_ping == 1)
     {
-      hhcd->hc[chnum].state = HC_NYET;     
-      __HAL_HCD_UNMASK_HALT_HC_INT(chnum); 
-      USB_HC_Halt(hhcd->Instance, chnum); 
+      hhcd->hc[chnum].state = HC_NYET;
+      __HAL_HCD_UNMASK_HALT_HC_INT(chnum);
+      USB_HC_Halt(hhcd->Instance, chnum);
       hhcd->hc[chnum].urb_state  = URB_NOTREADY;
     }
   }
@@ -960,12 +1020,11 @@ static void HCD_HC_OUT_IRQHandler  (HCD_HandleTypeDef *hhcd, uint8_t chnum)
   else if ((USBx_HC(chnum)->HCINT) &  USB_OTG_HCINT_NYET)
   {
     hhcd->hc[chnum].state = HC_NYET;
-    hhcd->hc[chnum].ErrCnt= 0;    
-    __HAL_HCD_UNMASK_HALT_HC_INT(chnum); 
-    USB_HC_Halt(hhcd->Instance, chnum);      
+    hhcd->hc[chnum].ErrCnt= 0;
+    __HAL_HCD_UNMASK_HALT_HC_INT(chnum);
+    USB_HC_Halt(hhcd->Instance, chnum);
     __HAL_HCD_CLEAR_HC_INT(chnum, USB_OTG_HCINT_NYET);
-    
-  }  
+  }
   
   else if ((USBx_HC(chnum)->HCINT) &  USB_OTG_HCINT_FRMOR)
   {
@@ -984,41 +1043,40 @@ static void HCD_HC_OUT_IRQHandler  (HCD_HandleTypeDef *hhcd, uint8_t chnum)
 
   }  
 
-  else if ((USBx_HC(chnum)->HCINT) &  USB_OTG_HCINT_STALL)  
+  else if ((USBx_HC(chnum)->HCINT) &  USB_OTG_HCINT_STALL)
   {
-    __HAL_HCD_CLEAR_HC_INT(chnum, USB_OTG_HCINT_STALL);  
+    __HAL_HCD_CLEAR_HC_INT(chnum, USB_OTG_HCINT_STALL);
     __HAL_HCD_UNMASK_HALT_HC_INT(chnum);
-    USB_HC_Halt(hhcd->Instance, chnum);   
-    hhcd->hc[chnum].state = HC_STALL;    
+    USB_HC_Halt(hhcd->Instance, chnum);
+    hhcd->hc[chnum].state = HC_STALL;
   }
 
   else if ((USBx_HC(chnum)->HCINT) &  USB_OTG_HCINT_NAK)
-  {  
-    hhcd->hc[chnum].ErrCnt = 0;  
-    __HAL_HCD_UNMASK_HALT_HC_INT(chnum); 
-    USB_HC_Halt(hhcd->Instance, chnum);   
+  {
+    hhcd->hc[chnum].ErrCnt = 0;
+    __HAL_HCD_UNMASK_HALT_HC_INT(chnum);
+    USB_HC_Halt(hhcd->Instance, chnum);
     hhcd->hc[chnum].state = HC_NAK;
     __HAL_HCD_CLEAR_HC_INT(chnum, USB_OTG_HCINT_NAK);
   }
 
   else if ((USBx_HC(chnum)->HCINT) &  USB_OTG_HCINT_TXERR)
   {
-    __HAL_HCD_UNMASK_HALT_HC_INT(chnum); 
-    USB_HC_Halt(hhcd->Instance, chnum);      
-    hhcd->hc[chnum].state = HC_XACTERR;  
+    __HAL_HCD_UNMASK_HALT_HC_INT(chnum);
+    USB_HC_Halt(hhcd->Instance, chnum);
+    hhcd->hc[chnum].state = HC_XACTERR;
      __HAL_HCD_CLEAR_HC_INT(chnum, USB_OTG_HCINT_TXERR);
   }
-  
+
   else if ((USBx_HC(chnum)->HCINT) &  USB_OTG_HCINT_DTERR)
   {
-    __HAL_HCD_UNMASK_HALT_HC_INT(chnum); 
-    USB_HC_Halt(hhcd->Instance, chnum);      
+    __HAL_HCD_UNMASK_HALT_HC_INT(chnum);
+    USB_HC_Halt(hhcd->Instance, chnum);
     __HAL_HCD_CLEAR_HC_INT(chnum, USB_OTG_HCINT_NAK);
-    __HAL_HCD_CLEAR_HC_INT(chnum, USB_OTG_HCINT_DTERR);    
+    __HAL_HCD_CLEAR_HC_INT(chnum, USB_OTG_HCINT_DTERR);
     hhcd->hc[chnum].state = HC_DATATGLERR;
   }
-  
-  
+
   else if ((USBx_HC(chnum)->HCINT) &  USB_OTG_HCINT_CHH)
   {
     __HAL_HCD_MASK_HALT_HC_INT(chnum); 
@@ -1028,7 +1086,10 @@ static void HCD_HC_OUT_IRQHandler  (HCD_HandleTypeDef *hhcd, uint8_t chnum)
       hhcd->hc[chnum].urb_state  = URB_DONE;
       if (hhcd->hc[chnum].ep_type == EP_TYPE_BULK)
       {
-        hhcd->hc[chnum].toggle_out ^= 1; 
+          if (hhcd->hc[chnum].packet_count & 1)
+          {
+              hhcd->hc[chnum].toggle_out ^= 1;
+          }
       }      
     }
     else if (hhcd->hc[chnum].state == HC_NAK)
@@ -1094,25 +1155,24 @@ static void HCD_RXQLVL_IRQHandler(HCD_HandleTypeDef *hhcd)
   switch (pktsts)
   {
   case GRXSTS_PKTSTS_IN:
+    if((USBx_HC(channelnum)->HCTSIZ & USB_OTG_HCTSIZ_PKTCNT) > 0)
+    {
+    /* re-activate the channel when more packets are expected */
+    tmpreg = USBx_HC(channelnum)->HCCHAR;
+    tmpreg &= ~USB_OTG_HCCHAR_CHDIS;
+    tmpreg |= USB_OTG_HCCHAR_CHENA;
+    USBx_HC(channelnum)->HCCHAR = tmpreg;
+    }
+
     /* Read the data into the host buffer. */
     if ((pktcnt > 0) && (hhcd->hc[channelnum].xfer_buff != (void  *)0))
-    {  
-      
+    {
       USB_ReadPacket(hhcd->Instance, hhcd->hc[channelnum].xfer_buff, pktcnt);   //Todo: buffer overflow here!
+      hhcd->hc[channelnum].toggle_in ^= 1;
       
       /*manage multiple Xfer */
       hhcd->hc[channelnum].xfer_buff += pktcnt;           
       hhcd->hc[channelnum].xfer_count  += pktcnt;
-      
-      if((USBx_HC(channelnum)->HCTSIZ & USB_OTG_HCTSIZ_PKTCNT) > 0)
-      {
-        /* re-activate the channel when more packets are expected */
-        tmpreg = USBx_HC(channelnum)->HCCHAR;
-        tmpreg &= ~USB_OTG_HCCHAR_CHDIS;
-        tmpreg |= USB_OTG_HCCHAR_CHENA;
-        USBx_HC(channelnum)->HCCHAR = tmpreg;
-        hhcd->hc[channelnum].toggle_in ^= 1;
-      }
     }
     break;
     
diff --git a/Downstream/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_usb.c b/Downstream/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_usb.c
index 971edfa..544c711 100644
--- a/Downstream/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_usb.c
+++ b/Downstream/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_usb.c
@@ -1433,19 +1433,18 @@ HAL_StatusTypeDef USB_HC_Init(USB_OTG_GlobalTypeDef *USBx,
   *           1 : DMA feature used  
   * @retval HAL state
   */
-#if defined   (__CC_ARM) /*!< ARM Compiler */
-#pragma O0
-#elif defined (__GNUC__) /*!< GNU Compiler */
-#pragma GCC optimize ("O0")
-#endif /* __CC_ARM */
+//#if defined   (__CC_ARM) /*!< ARM Compiler */
+//#pragma O0
+//#elif defined (__GNUC__) /*!< GNU Compiler */
+//#pragma GCC optimize ("O0")
+//#endif /* __CC_ARM */
 HAL_StatusTypeDef USB_HC_StartXfer(USB_OTG_GlobalTypeDef *USBx, USB_OTG_HCTypeDef *hc, uint8_t dma)
 {
   uint8_t  is_oddframe = 0; 
-  uint16_t len_words = 0;   
   uint16_t num_packets = 0;
   uint16_t max_hc_pkt_count = 256;
   uint32_t tmpreg = 0;
-    
+
   if((USBx != USB_OTG_FS) && (hc->speed == USB_OTG_SPEED_HIGH))
   {
     if((dma == 0) && (hc->do_ping == 1))
@@ -1475,6 +1474,8 @@ HAL_StatusTypeDef USB_HC_StartXfer(USB_OTG_GlobalTypeDef *USBx, USB_OTG_HCTypeDe
   {
     num_packets = 1;
   }
+  hc->packet_count = (uint8_t)num_packets;
+
   if (hc->ep_is_in)
   {
     hc->xfer_len = num_packets * hc->max_packet;
@@ -1505,45 +1506,90 @@ HAL_StatusTypeDef USB_HC_StartXfer(USB_OTG_GlobalTypeDef *USBx, USB_OTG_HCTypeDe
   {  
     if((hc->ep_is_in == 0) && (hc->xfer_len > 0))
     {
-      switch(hc->ep_type) 
+
+      switch(hc->ep_type)
       {
         /* Non periodic transfer */
       case EP_TYPE_CTRL:
       case EP_TYPE_BULK:
-        
-        len_words = (hc->xfer_len + 3) / 4;
-        
-        /* check if there is enough space in FIFO space */
-        if(len_words > (USBx->HNPTXSTS & 0xFFFF))
+
+        if (USB_HC_WriteEmptyTxFifo(USBx, hc, 0) == HAL_BUSY)
         {
-          /* need to process data in nptxfempty interrupt */
-          USBx->GINTMSK |= USB_OTG_GINTMSK_NPTXFEM;
+            while (1);
+//If transaction doesn't fit in fifo, enable interrupt like so:
+//            USB_UNMASK_INTERRUPT(USBx, USB_OTG_GINTMSK_NPTXFEM);
         }
         break;
-        /* Periodic transfer */
+
+      /* Periodic transfer */
       case EP_TYPE_INTR:
       case EP_TYPE_ISOC:
-        len_words = (hc->xfer_len + 3) / 4;
-        /* check if there is enough space in FIFO space */
-        if(len_words > (USBx_HOST->HPTXSTS & 0xFFFF)) /* split the transfer */
+
+        if (USB_HC_WriteEmptyTxFifo(USBx, hc, 1) == HAL_BUSY)
         {
-          /* need to process data in ptxfempty interrupt */
-          USBx->GINTMSK |= USB_OTG_GINTMSK_PTXFEM;          
+            while (1);
+//If transaction doesn't fit in fifo, enable interrupt like so:
+//            USB_UNMASK_INTERRUPT(USBx, USB_OTG_GINTMSK_PTXFEM);
         }
         break;
         
       default:
         break;
       }
-      
-      /* Write packet into the Tx FIFO. */
-      USB_WritePacket(USBx, hc->xfer_buff, hc->ch_num, hc->xfer_len, 0);
     }
   }
-  
+
   return HAL_OK;
 }
 
+
+
+
+HAL_StatusTypeDef USB_HC_WriteEmptyTxFifo(USB_OTG_GlobalTypeDef *USBx, USB_OTG_HCTypeDef *hc, uint8_t periodic)
+{
+    int32_t len;
+    uint32_t len32b;
+
+
+    while (1)
+    {
+        len = hc->xfer_len - hc->xfer_count;
+        if (len > hc->max_packet)
+        {
+            len = hc->max_packet;
+        }
+        if (len <= 0)
+        {
+            return HAL_OK;              //transfer complete
+        }
+
+        len32b = (len + 3) / 4;
+        if (periodic == 0)
+        {
+            if ((len32b > (USBx->HNPTXSTS & 0xFFFF)) ||
+                (((USBx->HNPTXSTS >> HxTXSTS_xTXQSAV_SHIFT) & HxTXSTS_xTXQSAV_MASK) == 0))
+            {
+                return HAL_BUSY;        //fifo is full
+            }
+        }
+        else
+        {
+            if ((len32b > (USBx_HOST->HPTXSTS & 0xFFFF)) ||
+                (((USBx_HOST->HPTXSTS >> HxTXSTS_xTXQSAV_SHIFT) & HxTXSTS_xTXQSAV_MASK) == 0))
+            {
+                return HAL_BUSY;        //fifo is full
+            }
+        }
+
+        /* Write packet into the Tx FIFO. */
+        USB_WritePacket(USBx, hc->xfer_buff, hc->ch_num, len, 0);
+
+        hc->xfer_count += len;
+        hc->xfer_buff += len;
+    }
+}
+
+
 /**
   * @brief Read all host channel interrupts status
   * @param  USBx : Selected device
diff --git a/Downstream/Middlewares/ST/STM32_USB_Host_Library/Class/MSC/Src/usbh_msc_bot.c b/Downstream/Middlewares/ST/STM32_USB_Host_Library/Class/MSC/Src/usbh_msc_bot.c
index 65f7f13..b96563f 100644
--- a/Downstream/Middlewares/ST/STM32_USB_Host_Library/Class/MSC/Src/usbh_msc_bot.c
+++ b/Downstream/Middlewares/ST/STM32_USB_Host_Library/Class/MSC/Src/usbh_msc_bot.c
@@ -239,8 +239,8 @@ USBH_StatusTypeDef USBH_MSC_BOT_Process (USBH_HandleTypeDef *phost, uint8_t lun)
             else
             {
                 //Still more data to receive
-                if (MSC_Handle->hbot.bot_packet_bytes_remaining == 0)
-                {
+//                if (MSC_Handle->hbot.bot_packet_bytes_remaining == 0)
+//                {
                     //Dispatch current bot_packet, then get a new one
                     if (Downstream_MSC_PutStreamDataPacket(MSC_Handle->hbot.bot_packet,
                                                            BOT_PAGE_LENGTH) != HAL_OK)
@@ -254,12 +254,12 @@ USBH_StatusTypeDef USBH_MSC_BOT_Process (USBH_HandleTypeDef *phost, uint8_t lun)
                         MSC_Handle->hbot.state  = BOT_ERROR_IN;
                         break;
                     }
-                }
-                else
-                {
-                    //Continue filling the current bot_packet
-                    USBH_MSC_BOT_Read_Multipacket_PrepareURB(phost);
-                }
+//                }
+//                else
+//                {
+//                    //Continue filling the current bot_packet
+//                    USBH_MSC_BOT_Read_Multipacket_PrepareURB(phost);
+//                }
             }
         }
     }
@@ -488,10 +488,10 @@ void USBH_MSC_BOT_Read_Multipacket_PrepareURB(USBH_HandleTypeDef *phost)
     {
         temp_URB_size = MSC_Handle->hbot.bot_packet_bytes_remaining;
     }
-    if (temp_URB_size > MSC_Handle->InEpSize)
-    {
-        temp_URB_size = MSC_Handle->InEpSize;
-    }
+//    if (temp_URB_size > MSC_Handle->InEpSize)
+//    {
+//        temp_URB_size = MSC_Handle->InEpSize;
+//    }
     MSC_Handle->hbot.this_URB_size = (uint16_t)temp_URB_size;
 
     USBH_BulkReceiveData(phost,
@@ -530,9 +530,9 @@ void USBH_MSC_BOT_Write_Multipacket_PrepareURB(USBH_HandleTypeDef *phost)
     {
         temp_URB_size = MSC_Handle->hbot.bot_packet_bytes_remaining;
     }
-    if (temp_URB_size > MSC_Handle->OutEpSize)
+    if (temp_URB_size > MSC_Handle->OutEpSize * 4)          //4 x 64-byte packets is the magic number. Anything more than this will fail.
     {
-        temp_URB_size = MSC_Handle->OutEpSize;
+        temp_URB_size = MSC_Handle->OutEpSize * 4;
     }
     MSC_Handle->hbot.this_URB_size = (uint16_t)temp_URB_size;
 
diff --git a/Downstream/Middlewares/ST/STM32_USB_Host_Library/Core/Inc/usbh_def.h b/Downstream/Middlewares/ST/STM32_USB_Host_Library/Core/Inc/usbh_def.h
index 0b74acd..c654544 100644
--- a/Downstream/Middlewares/ST/STM32_USB_Host_Library/Core/Inc/usbh_def.h
+++ b/Downstream/Middlewares/ST/STM32_USB_Host_Library/Core/Inc/usbh_def.h
@@ -454,6 +454,11 @@ typedef struct
   void*                pData;
 } USBH_ClassTypeDef;
 
+
+
+#define HOST_PIPE_COUNT     15
+
+
 /* USB Host handle structure */
 typedef struct _USBH_HandleTypeDef
 {
@@ -465,7 +470,7 @@ typedef struct _USBH_HandleTypeDef
   USBH_ClassTypeDef*    pClass[USBH_MAX_NUM_SUPPORTED_CLASS];
   USBH_ClassTypeDef*    pActiveClass;
   uint32_t              ClassNumber;
-  uint32_t              Pipes[15];
+  uint32_t              Pipes[HOST_PIPE_COUNT];
   __IO uint32_t         Timer;
   uint8_t               id;  
   void*                 pData;                  
diff --git a/Downstream/Middlewares/ST/STM32_USB_Host_Library/Core/Src/usbh_pipes.c b/Downstream/Middlewares/ST/STM32_USB_Host_Library/Core/Src/usbh_pipes.c
index 79e026d..7d24e1b 100644
--- a/Downstream/Middlewares/ST/STM32_USB_Host_Library/Core/Src/usbh_pipes.c
+++ b/Downstream/Middlewares/ST/STM32_USB_Host_Library/Core/Src/usbh_pipes.c
@@ -157,7 +157,7 @@ uint8_t USBH_AllocPipe  (USBH_HandleTypeDef *phost, uint8_t ep_addr)
   */
 USBH_StatusTypeDef USBH_FreePipe  (USBH_HandleTypeDef *phost, uint8_t idx)
 {
-   if(idx < 11)
+   if (idx < HOST_PIPE_COUNT)
    {
      phost->Pipes[idx] &= 0x7FFF;
    }
@@ -173,8 +173,9 @@ USBH_StatusTypeDef USBH_FreePipe  (USBH_HandleTypeDef *phost, uint8_t idx)
 static uint16_t USBH_GetFreePipe (USBH_HandleTypeDef *phost)
 {
   uint8_t idx = 0;
-  
-  for (idx = 0 ; idx < 11 ; idx++)
+
+  //We need to limit allocated pipes to the number actually provided by hardware!
+  for (idx = 0 ; idx < ((HCD_HandleTypeDef*)phost->pData)->Init.Host_channels; idx++)
   {
     if ((phost->Pipes[idx] & 0x8000) == 0)
     {
diff --git a/Upstream/.cproject b/Upstream/.cproject
index b42e19e..6127309 100755
--- a/Upstream/.cproject
+++ b/Upstream/.cproject
@@ -178,7 +178,7 @@
 							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.warnings.pointerarith.1615061633" name="Warn if pointer arithmetic (-Wpointer-arith)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.warnings.pointerarith" value="true" valueType="boolean"/>
 							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.warnings.logicalop.1292304015" name="Warn if suspicious logical ops (-Wlogical-op)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.warnings.logicalop" value="true" valueType="boolean"/>
 							<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="ilg.gnuarmeclipse.managedbuild.cross.targetPlatform.1338907066" isAbstract="false" osList="all" superClass="ilg.gnuarmeclipse.managedbuild.cross.targetPlatform"/>
-							<builder buildPath="${workspace_loc:/Upstream}/Release" command="make" id="ilg.gnuarmeclipse.managedbuild.cross.builder.836101662" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="optimal" superClass="ilg.gnuarmeclipse.managedbuild.cross.builder"/>
+							<builder buildPath="${workspace_loc:/Upstream}/Release" id="ilg.gnuarmeclipse.managedbuild.cross.builder.836101662" keepEnvironmentInBuildfile="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="optimal" superClass="ilg.gnuarmeclipse.managedbuild.cross.builder"/>
 							<tool id="ilg.gnuarmeclipse.managedbuild.cross.tool.assembler.290553164" name="Cross ARM GNU Assembler" superClass="ilg.gnuarmeclipse.managedbuild.cross.tool.assembler">
 								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.assembler.usepreprocessor.1693864736" name="Use preprocessor" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.assembler.usepreprocessor" value="true" valueType="boolean"/>
 								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.assembler.include.paths.1982648457" name="Include paths (-I)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.assembler.include.paths" valueType="includePath">