move align buffer to pma_alloc()

2 files changed, 85 insertions, 87 deletions

diff --git a/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c b/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
index cd00e6395..2be6ef031 100644
--- a/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
+++ b/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
@@ -170,7 +170,7 @@ static void dcd_ep_ctr_handler(void);
 
 // PMA allocation/access
 static uint16_t ep_buf_ptr; ///< Points to first free memory location
-static uint32_t dcd_pma_alloc(uint16_t length, bool dbuf);
+static uint32_t dcd_pma_alloc(uint16_t len, bool dbuf);
 static uint8_t dcd_ep_alloc(uint8_t ep_addr, uint8_t ep_type);
 static bool dcd_write_packet_memory(uint16_t dst, const void *__restrict src, uint16_t wNBytes);
 static bool dcd_read_packet_memory(void *__restrict dst, uint16_t src, uint16_t wNBytes);
@@ -551,21 +551,19 @@ void dcd_edpt0_status_complete(uint8_t rhport, tusb_control_request_t const *req
  * In case of double buffering, high 16bit is the address of 2nd buffer
  * During failure, TU_ASSERT is used. If this happens, rework/reallocate memory manually.
  */
-static uint32_t dcd_pma_alloc(uint16_t length, bool dbuf)
+static uint32_t dcd_pma_alloc(uint16_t len, bool dbuf)
 {
-  // Ensure allocated buffer is aligned
-#ifdef FSDEV_BUS_32BIT
-  length = (length + 3) & ~0x03;
-#else
-  length = (length + 1) & ~0x01;
-#endif
+  uint8_t blsize, num_block;
+  uint16_t aligned_len = pma_align_buffer_size(len, &blsize, &num_block);
+  (void) blsize;
+  (void) num_block;
 
   uint32_t addr = ep_buf_ptr;
-  ep_buf_ptr = (uint16_t)(ep_buf_ptr + length); // increment buffer pointer
+  ep_buf_ptr = (uint16_t)(ep_buf_ptr + aligned_len); // increment buffer pointer
 
   if (dbuf) {
     addr |= ((uint32_t)ep_buf_ptr) << 16;
-    ep_buf_ptr = (uint16_t)(ep_buf_ptr + length); // increment buffer pointer
+    ep_buf_ptr = (uint16_t)(ep_buf_ptr + aligned_len); // increment buffer pointer
   }
 
   // Verify packet buffer is not overflowed
@@ -621,11 +619,9 @@ bool dcd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const *desc_ep) {
   uint8_t const ep_idx = dcd_ep_alloc(ep_addr, desc_ep->bmAttributes.xfer);
   uint8_t const dir = tu_edpt_dir(ep_addr);
   const uint16_t packet_size = tu_edpt_packet_size(desc_ep);
-  const uint16_t buffer_size = pcd_aligned_buffer_size(packet_size);
   uint32_t wType;
 
   TU_ASSERT(ep_idx < FSDEV_EP_COUNT);
-  TU_ASSERT(buffer_size <= 64);
 
   // Set type
   switch (desc_ep->bmAttributes.xfer) {
@@ -649,7 +645,7 @@ bool dcd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const *desc_ep) {
   pcd_set_ep_address(USB, ep_idx, tu_edpt_number(ep_addr));
 
   /* Create a packet memory buffer area. */
-  uint16_t pma_addr = dcd_pma_alloc(buffer_size, false);
+  uint16_t pma_addr = dcd_pma_alloc(packet_size, false);
 
   if (dir == TUSB_DIR_IN) {
     btable_set_addr(ep_idx, BTABLE_BUF_TX, pma_addr);
@@ -707,22 +703,20 @@ void dcd_edpt_close(uint8_t rhport, uint8_t ep_addr)
   }
 }
 
-bool dcd_edpt_iso_alloc(uint8_t rhport, uint8_t ep_addr, uint16_t largest_packet_size)
-{
+bool dcd_edpt_iso_alloc(uint8_t rhport, uint8_t ep_addr, uint16_t largest_packet_size) {
   (void)rhport;
 
   uint8_t const ep_idx = dcd_ep_alloc(ep_addr, TUSB_XFER_ISOCHRONOUS);
-  const uint16_t buffer_size = pcd_aligned_buffer_size(largest_packet_size);
 
   /* Create a packet memory buffer area. Enable double buffering for devices with 2048 bytes PMA,
      for smaller devices double buffering occupy too much space. */
+  uint32_t pma_addr = dcd_pma_alloc(largest_packet_size, true);
 #if FSDEV_PMA_SIZE > 1024u
-  uint32_t pma_addr = dcd_pma_alloc(buffer_size, true);
   uint16_t pma_addr2 = pma_addr >> 16;
 #else
-  uint32_t pma_addr = dcd_pma_alloc(buffer_size, true);
   uint16_t pma_addr2 = pma_addr;
 #endif
+
   btable_set_addr(ep_idx, 0, pma_addr);
   btable_set_addr(ep_idx, 1, pma_addr2);
   pcd_set_eptype(USB, ep_idx, USB_EP_ISOCHRONOUS);
@@ -803,7 +797,7 @@ static bool edpt_xfer(uint8_t rhport, uint8_t ep_addr)
   if (dir == TUSB_DIR_IN) {
     dcd_transmit_packet(xfer, ep_idx);
   } else {
-    uint32_t cnt = (uint32_t ) tu_min16(xfer->total_len, xfer->max_packet_size);
+    uint32_t cnt = (uint32_t) tu_min16(xfer->total_len, xfer->max_packet_size);
     uint16_t ep_reg = pcd_get_endpoint(USB, ep_idx);
 
     if ((ep_reg & USB_EP_TYPE_MASK) == USB_EP_ISOCHRONOUS) {
diff --git a/src/portable/st/stm32_fsdev/fsdev_common.h b/src/portable/st/stm32_fsdev/fsdev_common.h
index 05354d705..c8d259cfa 100644
--- a/src/portable/st/stm32_fsdev/fsdev_common.h
+++ b/src/portable/st/stm32_fsdev/fsdev_common.h
@@ -96,7 +96,7 @@ TU_VERIFY_STATIC(FSDEV_BTABLE_BASE + FSDEV_EP_COUNT*8 <= FSDEV_PMA_SIZE, "BTABLE
 #define FSDEV_BTABLE ((volatile fsdev_btable_t*) (USB_PMAADDR+FSDEV_BTABLE_BASE))
 
 //--------------------------------------------------------------------+
-// Helper
+// BTable
 //--------------------------------------------------------------------+
 
 // The fsdev_bus_t type can be used for both register and PMA access necessities
@@ -112,18 +112,84 @@ typedef uint16_t fsdev_bus_t;
 static volatile uint16_t * const pma = (volatile uint16_t*)USB_PMAADDR;
 #endif
 
+TU_ATTR_ALWAYS_INLINE static inline uint32_t btable_get_addr(uint32_t ep_id, uint8_t buf_id) {
+#ifdef FSDEV_BUS_32BIT
+  return FSDEV_BTABLE->ep32[ep_id][buf_id].count_addr & 0x0000FFFFu;
+#else
+  return FSDEV_BTABLE->ep16[ep_id][buf_id].addr;
+#endif
+}
+
+TU_ATTR_ALWAYS_INLINE static inline void btable_set_addr(uint32_t ep_id, uint8_t buf_id, uint16_t addr) {
+#ifdef FSDEV_BUS_32BIT
+  uint32_t count_addr = FSDEV_BTABLE->ep32[ep_id][buf_id].count_addr;
+  count_addr = (count_addr & 0xFFFF0000u) | (addr & 0x0000FFFCu);
+  FSDEV_BTABLE->ep32[ep_id][buf_id].count_addr = count_addr;
+#else
+  FSDEV_BTABLE->ep16[ep_id][buf_id].addr = addr;
+#endif
+}
+
+TU_ATTR_ALWAYS_INLINE static inline uint32_t btable_get_count(uint32_t ep_id, uint8_t buf_id) {
+  uint16_t count;
+#ifdef FSDEV_BUS_32BIT
+  count = (FSDEV_BTABLE->ep32[ep_id][buf_id].count_addr >> 16);
+#else
+  count = FSDEV_BTABLE->ep16[ep_id][buf_id].count;
+#endif
+  return count & 0x3FFU;
+}
+
+TU_ATTR_ALWAYS_INLINE static inline void btable_set_count(uint32_t ep_id, uint8_t buf_id, uint16_t byte_count) {
+#ifdef FSDEV_BUS_32BIT
+  uint32_t count_addr = FSDEV_BTABLE->ep32[ep_id][buf_id].count_addr;
+  count_addr = (count_addr & ~0x03FF0000u) | ((byte_count & 0x3FFu) << 16);
+  FSDEV_BTABLE->ep32[ep_id][buf_id].count_addr = count_addr;
+#else
+  uint16_t cnt = FSDEV_BTABLE->ep16[ep_id][buf_id].count;
+  cnt = (cnt & ~0x3FFU) | (byte_count & 0x3FFU);
+  FSDEV_BTABLE->ep16[ep_id][buf_id].count = cnt;
+#endif
+}
+
 /* Aligned buffer size according to hardware */
-TU_ATTR_ALWAYS_INLINE static inline uint16_t pcd_aligned_buffer_size(uint16_t size) {
+TU_ATTR_ALWAYS_INLINE static inline uint16_t pma_align_buffer_size(uint16_t size, uint8_t* blsize, uint8_t* num_block) {
   /* 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;
+  uint16_t block_in_bytes;
+  if (size > 62) {
+    block_in_bytes = 32;
+    *blsize = 1;
+  } else {
+    block_in_bytes = 2;
+    *blsize = 0;
+  }
+
+  *num_block = tu_div_ceil(size, block_in_bytes);
 
-  // Round up while dividing requested size by blocksize
-  uint16_t numblocks = (size + blocksize - 1) / blocksize ;
+  return (*num_block) * block_in_bytes;
+}
+
+TU_ATTR_ALWAYS_INLINE static inline void btable_set_rx_bufsize(uint32_t ep_id, uint8_t buf_id, uint32_t wCount) {
+  uint8_t blsize, num_block;
+  (void) pma_align_buffer_size(wCount, &blsize, &num_block);
 
-  return numblocks * blocksize;
+  /* Encode into register. When BLSIZE==1, we need to subtract 1 block count */
+  uint16_t bl_nb = (blsize << 15) | ((num_block - blsize) << 10);
+
+#ifdef FSDEV_BUS_32BIT
+  uint32_t count_addr = FSDEV_BTABLE->ep32[ep_id][buf_id].count_addr;
+  count_addr = (bl_nb << 16) | (count_addr & 0x0000FFFFu);
+  FSDEV_BTABLE->ep32[ep_id][buf_id].count_addr = count_addr;
+#else
+  FSDEV_BTABLE->ep16[ep_id][buf_id].count = bl_nb;
+#endif
 }
 
+//--------------------------------------------------------------------+
+// Endpoint
+//--------------------------------------------------------------------+
+
 TU_ATTR_ALWAYS_INLINE static inline void pcd_set_endpoint(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t wRegValue) {
 #ifdef FSDEV_BUS_32BIT
   (void) USBx;
@@ -196,68 +262,6 @@ TU_ATTR_ALWAYS_INLINE static inline void pcd_clear_tx_ep_ctr(USB_TypeDef * USBx,
   pcd_set_endpoint(USBx, bEpIdx,regVal);
 }
 
-TU_ATTR_ALWAYS_INLINE static inline uint32_t btable_get_count(uint32_t ep_id, uint8_t buf_id) {
-  uint16_t count;
-#ifdef FSDEV_BUS_32BIT
-  count = (FSDEV_BTABLE->ep32[ep_id][buf_id].count_addr >> 16);
-#else
-  count = FSDEV_BTABLE->ep16[ep_id][buf_id].count;
-#endif
-  return count & 0x3FFU;
-}
-
-TU_ATTR_ALWAYS_INLINE static inline uint32_t btable_get_addr(uint32_t ep_id, uint8_t buf_id) {
-#ifdef FSDEV_BUS_32BIT
-  return FSDEV_BTABLE->ep32[ep_id][buf_id].count_addr & 0x0000FFFFu;
-#else
-  return FSDEV_BTABLE->ep16[ep_id][buf_id].addr;
-#endif
-}
-
-TU_ATTR_ALWAYS_INLINE static inline void btable_set_addr(uint32_t ep_id, uint8_t buf_id, uint16_t addr) {
-#ifdef FSDEV_BUS_32BIT
-  uint32_t count_addr = FSDEV_BTABLE->ep32[ep_id][buf_id].count_addr;
-  count_addr = (count_addr & 0xFFFF0000u) | (addr & 0x0000FFFCu);
-  FSDEV_BTABLE->ep32[ep_id][buf_id].count_addr = count_addr;
-#else
-  FSDEV_BTABLE->ep16[ep_id][buf_id].addr = addr;
-#endif
-}
-
-TU_ATTR_ALWAYS_INLINE static inline void btable_set_count(uint32_t ep_id, uint8_t buf_id, uint16_t byte_count) {
-#ifdef FSDEV_BUS_32BIT
-  uint32_t count_addr = FSDEV_BTABLE->ep32[ep_id][buf_id].count_addr;
-  count_addr = (count_addr & ~0x03FF0000u) | ((byte_count & 0x3FFu) << 16);
-  FSDEV_BTABLE->ep32[ep_id][buf_id].count_addr = count_addr;
-#else
-  uint16_t cnt = FSDEV_BTABLE->ep16[ep_id][buf_id].count;
-  cnt = (cnt & ~0x3FFU) | (byte_count & 0x3FFU);
-  FSDEV_BTABLE->ep16[ep_id][buf_id].count = cnt;
-#endif
-}
-
-TU_ATTR_ALWAYS_INLINE static inline void btable_set_rx_bufsize(uint32_t ep_id, uint8_t buf_id, uint32_t wCount) {
-  wCount = pcd_aligned_buffer_size(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);
-
-  /* There should be no remainder in the above calculation */
-  TU_ASSERT((wCount - (numblocks * (blocksize ? 32 : 2))) == 0, /**/);
-
-  /* Encode into register. When BLSIZE==1, we need to subtract 1 block count */
-  uint16_t bl_nb = (blocksize << 15) | ((numblocks - blocksize) << 10);
-
-#ifdef FSDEV_BUS_32BIT
-  uint32_t count_addr = FSDEV_BTABLE->ep32[ep_id][buf_id].count_addr;
-  count_addr = (bl_nb << 16) | (count_addr & 0x0000FFFFu);
-  FSDEV_BTABLE->ep32[ep_id][buf_id].count_addr = count_addr;
-#else
-  FSDEV_BTABLE->ep16[ep_id][buf_id].count = bl_nb;
-#endif
-}
-
 /**
   * @brief  sets the status for tx transfer (bits STAT_TX[1:0]).
   * @param  USBx USB peripheral instance register address.