From ec826732fccf070b4de1369d0ee50d653689bb9c Mon Sep 17 00:00:00 2001
From: Simon Kueppers <simon.kueppers@skueppers.de>
Date: Tue, 25 Oct 2022 18:32:10 +0200
Subject: [PATCH] Fixed non-aligned allocated memory sizes

---
 src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c | 11 +++--
 .../st/stm32_fsdev/dcd_stm32_fsdev_pvt_st.h   | 44 +++++++++----------
 2 files changed, 26 insertions(+), 29 deletions(-)

diff --git a/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c b/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
index 7ebaa826c..cb2b5a029 100644
--- a/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
+++ b/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
@@ -723,7 +723,7 @@ static uint16_t dcd_pma_alloc(uint8_t ep_addr, size_t length)
     return epXferCtl->pma_ptr;
   }
   
-  uint16_t addr = ep_buf_ptr; 
+  uint16_t addr = ep_buf_ptr;
   ep_buf_ptr = (uint16_t)(ep_buf_ptr + length); // increment buffer pointer
   
   // Verify no overflow
@@ -772,7 +772,7 @@ bool dcd_edpt_open (uint8_t rhport, tusb_desc_endpoint_t const * p_endpoint_desc
   /* TODO: This hardware endpoint allocation could be more sensible. For now, simple allocation or manual allocation using callback */
   uint8_t const epnum = tu_stm32_edpt_number_cb ? tu_stm32_edpt_number_cb(p_endpoint_desc->bEndpointAddress) : tu_edpt_number(p_endpoint_desc->bEndpointAddress);
   uint8_t const dir   = tu_edpt_dir(p_endpoint_desc->bEndpointAddress);
-  const uint16_t epMaxPktSize = tu_edpt_packet_size(p_endpoint_desc);
+  const uint16_t buffer_size = pcd_aligned_buffer_size(tu_edpt_packet_size(p_endpoint_desc));
   uint16_t pma_addr;
   uint32_t wType;
 
@@ -809,7 +809,7 @@ bool dcd_edpt_open (uint8_t rhport, tusb_desc_endpoint_t const * p_endpoint_desc
 
   /* Create a packet memory buffer area. For isochronous endpoints,
    * use the same buffer as the double buffer, essentially disabling double buffering */
-  pma_addr = dcd_pma_alloc(p_endpoint_desc->bEndpointAddress, epMaxPktSize);
+  pma_addr = dcd_pma_alloc(p_endpoint_desc->bEndpointAddress, buffer_size);
 
 #if defined(ISOCHRONOUS_DOUBLEBUFFER)
   if( (dir == TUSB_DIR_IN) || (wType == USB_EP_ISOCHRONOUS) )
@@ -818,7 +818,6 @@ bool dcd_edpt_open (uint8_t rhport, tusb_desc_endpoint_t const * p_endpoint_desc
 #endif
   {
     *pcd_ep_tx_address_ptr(USB, epnum) = pma_addr;
-    pcd_set_ep_tx_cnt(USB, epnum, epMaxPktSize);
     pcd_clear_tx_dtog(USB, epnum);
   }
 #if defined(ISOCHRONOUS_DOUBLEBUFFER)
@@ -828,7 +827,7 @@ bool dcd_edpt_open (uint8_t rhport, tusb_desc_endpoint_t const * p_endpoint_desc
 #endif
   {
     *pcd_ep_rx_address_ptr(USB, epnum) = pma_addr;
-    pcd_set_ep_rx_cnt(USB, epnum, epMaxPktSize);
+    pcd_set_ep_rx_cnt(USB, epnum, buffer_size);
     pcd_clear_rx_dtog(USB, epnum);
   }
 
@@ -849,7 +848,7 @@ bool dcd_edpt_open (uint8_t rhport, tusb_desc_endpoint_t const * p_endpoint_desc
     }
   }
 
-  xfer_ctl_ptr(p_endpoint_desc->bEndpointAddress)->max_packet_size = epMaxPktSize;
+  xfer_ctl_ptr(p_endpoint_desc->bEndpointAddress)->max_packet_size = buffer_size;
   xfer_ctl_ptr(p_endpoint_desc->bEndpointAddress)->epnum = epnum;
 
   return true;
diff --git a/src/portable/st/stm32_fsdev/dcd_stm32_fsdev_pvt_st.h b/src/portable/st/stm32_fsdev/dcd_stm32_fsdev_pvt_st.h
index 5d26d3525..17726e521 100644
--- a/src/portable/st/stm32_fsdev/dcd_stm32_fsdev_pvt_st.h
+++ b/src/portable/st/stm32_fsdev/dcd_stm32_fsdev_pvt_st.h
@@ -131,6 +131,18 @@ static inline __IO uint16_t* pcd_ep_rx_cnt_ptr(USB_TypeDef * USBx, uint32_t bEpN
 static inline __IO uint16_t* pcd_ep_tx_cnt_ptr(USB_TypeDef * USBx, uint32_t bEpNum);
 static inline void pcd_set_endpoint(USB_TypeDef * USBx, uint32_t bEpNum, uint32_t wRegValue);
 
+/* Aligned buffer size according to hardware */
+static inline uint16_t pcd_aligned_buffer_size(uint16_t size)
+{
+  /* The STM32 full speed USB peripheral supports only a limited set of
+   * buffer sizes given by the RX buffer entry format in the USB_BTABLE. */
+  uint16_t blocksize = (size > 62) ? 32 : 2;
+
+  // Round up while dividing requested size by blocksize
+  uint16_t numblocks = (size + blocksize - 1) / blocksize ;
+
+  return numblocks * blocksize;
+}
 
 /* SetENDPOINT */
 static inline void pcd_set_endpoint(USB_TypeDef * USBx, uint32_t bEpNum, uint32_t wRegValue)
@@ -207,32 +219,18 @@ static inline uint32_t pcd_get_ep_rx_cnt(USB_TypeDef * USBx, uint32_t bEpNum)
   * @param  wNBlocks no. of Blocks.
   * @retval None
   */
+static inline void pcd_set_ep_cnt_rx_reg(__O uint16_t * pdwReg, size_t wCount)
+{
+  /* We assume that the buffer size is already aligned to hardware requirements. */
+  uint16_t blocksize = (wCount > 62) ? 1 : 0;
+  uint16_t numblocks = wCount / (blocksize ? 32 : 2);
 
-static inline void pcd_set_ep_cnt_rx_reg(__O uint16_t * pdwReg, size_t wCount)  {
-  uint32_t wNBlocks;
-  if(wCount > 62u)
-  {
-    wNBlocks = wCount >> 5u;
-    if((wCount & 0x1fU) == 0u)
-    {
-      wNBlocks--;
-    }
-    wNBlocks = wNBlocks << 10u;
-    wNBlocks |= 0x8000u; // Mark block size as 32byte
-    *pdwReg = (uint16_t)wNBlocks;
-  }
-  else
-  {
-    wNBlocks = wCount >> 1u;
-    if((wCount & 0x1U) != 0u)
-    {
-      wNBlocks++;
-    }
-    *pdwReg = (uint16_t)((wNBlocks) << 10u);
-  }
+  /* There should be no remainder in the above calculation */
+  TU_VERIFY((wCount - (numblocks * (blocksize ? 32 : 2))) == 0, /**/);
+
+  *pdwReg = (blocksize << 15) | (numblocks << 10);
 }
 
-
 /**
   * @brief  Sets address in an endpoint register.
   * @param  USBx USB peripheral instance register address.