From eb3665ffbb7b07931adc012a402dcc61ef35b5b3 Mon Sep 17 00:00:00 2001
From: Damien George <damien.p.george@gmail.com>
Date: Wed, 26 Sep 2018 11:56:45 +1000
Subject: [PATCH] F7_HAL/pcd: Optimise writing to USB FIFO to fill it as much
 as possible.

The existing HAL code had two issues:

- It used < instead of <= to compare INEPTFSAV with len32b, which meant
  that data would not be written to the FIFO if there was exactly enough
  room for the packet (it would require enough room plus one additional
  FIFO slot).  So for IN endpoints that had a FIFO size the same as the
  maximum packet size, packets of maximum size could never be written.

- If the write loop went around again it would use the old len32b to check
  if there was enough space, thereby missing some opportunities to write
  packets that were smaller than the previous len32b.

This patch fixes these two issues.
---
 STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_pcd.c | 32 ++++++++++++++++++++
 1 file changed, 32 insertions(+)

diff --git a/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_pcd.c b/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_pcd.c
index 5622c80..ca63df1 100644
--- a/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_pcd.c
+++ b/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_pcd.c
@@ -1256,6 +1256,7 @@ static HAL_StatusTypeDef PCD_WriteEmptyTxFifo(PCD_HandleTypeDef *hpcd, uint32_t
   uint32_t fifoemptymsk = 0;
 
   ep = &hpcd->IN_ep[epnum];
+  #if 0
   len = ep->xfer_len - ep->xfer_count;
 
   if (len > ep->maxpacket)
@@ -1285,6 +1286,37 @@ static HAL_StatusTypeDef PCD_WriteEmptyTxFifo(PCD_HandleTypeDef *hpcd, uint32_t
     ep->xfer_count += len;
   }
 
+  #else
+  /* dpgeorge: modified above loop to:
+   *    - allow to write the packet in the case it exactly fills the FIFO
+   *    - recompute len32b before each check of this value against FIFO free space
+   */
+  for (;;)
+  {
+    len = ep->xfer_len - ep->xfer_count;
+    if (len <= 0U)
+    {
+      /* Finished sending all data */
+      break;
+    }
+    if (len > ep->maxpacket)
+    {
+      len = ep->maxpacket;
+    }
+
+    len32b = (len + 3U) / 4U;
+    if  (len32b > (USBx_INEP(epnum)->DTXFSTS & USB_OTG_DTXFSTS_INEPTFSAV))
+    {
+      /* No room left in FIFO */
+      break;
+    }
+
+    USB_WritePacket(USBx, ep->xfer_buff, epnum, len, hpcd->Init.dma_enable);
+    ep->xfer_buff  += len;
+    ep->xfer_count += len;
+  }
+  #endif
+
   if(len <= 0)
   {
     fifoemptymsk = 0x1 << epnum;