simplify read/write 16-bit packet

3 files changed, 85 insertions, 111 deletions

diff --git a/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c b/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
index 346cf8164..cff328418 100644
--- a/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
+++ b/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
@@ -171,8 +171,8 @@ static bool edpt_xfer(uint8_t rhport, uint8_t ep_num, uint8_t dir);
 static uint16_t ep_buf_ptr; ///< Points to first free memory location
 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);
+static bool dcd_write_packet_memory(uint16_t dst, const void *__restrict src, uint16_t nbytes);
+static bool dcd_read_packet_memory(void *__restrict dst, uint16_t src, uint16_t nbytes);
 
 static bool dcd_write_packet_memory_ff(tu_fifo_t *ff, uint16_t dst, uint16_t wNBytes);
 static bool dcd_read_packet_memory_ff(tu_fifo_t *ff, uint16_t src, uint16_t wNBytes);
@@ -828,10 +828,10 @@ void dcd_edpt_clear_stall(uint8_t rhport, uint8_t ep_addr) {
 #ifdef FSDEV_BUS_32BIT
 static bool dcd_write_packet_memory(uint16_t dst, const void *__restrict src, uint16_t wNBytes) {
   const uint8_t *src8 = src;
-  volatile uint32_t *dst32 = (volatile uint32_t *)(USB_PMAADDR + dst);
+  volatile uint32_t *pma32 = (volatile uint32_t *)(USB_PMAADDR + dst);
 
   for (uint32_t n = wNBytes / 4; n > 0; --n) {
-    *dst32++ = tu_unaligned_read32(src8);
+    *pma32++ = tu_unaligned_read32(src8);
     src8 += 4;
   }
 
@@ -849,11 +849,41 @@ static bool dcd_write_packet_memory(uint16_t dst, const void *__restrict src, ui
       }
     }
 
-    *dst32 = wrVal;
+    *pma32 = wrVal;
   }
 
   return true;
 }
+
+static bool dcd_read_packet_memory(void *__restrict dst, uint16_t src, uint16_t wNBytes) {
+  uint8_t *dst8 = dst;
+  volatile uint32_t *src32 = (volatile uint32_t *)(USB_PMAADDR + src);
+
+  for (uint32_t n = wNBytes / 4; n > 0; --n) {
+    tu_unaligned_write32(dst8, *src32++);
+    dst8 += 4;
+  }
+
+  uint16_t odd = wNBytes & 0x03;
+  if (odd) {
+    uint32_t rdVal = *src32;
+
+    *dst8 = tu_u32_byte0(rdVal);
+    odd--;
+
+    if (odd) {
+      *++dst8 = tu_u32_byte1(rdVal);
+      odd--;
+
+      if (odd) {
+        *++dst8 = tu_u32_byte2(rdVal);
+      }
+    }
+  }
+
+  return true;
+}
+
 #else
 // Packet buffer access can only be 8- or 16-bit.
 /**
@@ -862,26 +892,52 @@ static bool dcd_write_packet_memory(uint16_t dst, const void *__restrict src, ui
  * @param   dst, byte address in PMA; must be 16-bit aligned
  * @param   src pointer to user memory area.
  * @param   wPMABufAddr address into PMA.
- * @param   wNBytes no. of bytes to be copied.
+ * @param   nbytes no. of bytes to be copied.
  * @retval None
  */
-static bool dcd_write_packet_memory(uint16_t dst, const void *__restrict src, uint16_t wNBytes) {
-  uint32_t n = (uint32_t)wNBytes >> 1U;
+static bool dcd_write_packet_memory(uint16_t dst, const void *__restrict src, uint16_t nbytes) {
+  uint32_t n16 = (uint32_t)nbytes >> 1U;
   const uint8_t *src8 = src;
-  volatile uint16_t *pdw16 = &pma[FSDEV_PMA_STRIDE * (dst >> 1)];
+  fsdev_pma16_t* pma16 = (fsdev_pma16_t*) (USB_PMAADDR + FSDEV_PMA_STRIDE * dst);
 
-  while (n--) {
-    *pdw16 = tu_unaligned_read16(src8);
+  while (n16--) {
+    pma16->u16 = tu_unaligned_read16(src8);
     src8 += 2;
-    pdw16 += FSDEV_PMA_STRIDE;
+    pma16++;
+  }
+
+  if (nbytes & 0x01) {
+    pma16->u16 = (uint16_t) *src8;
+  }
+
+  return true;
+}
+
+/**
+ * @brief Copy a buffer from packet memory area (PMA) to user memory area.
+ *        Uses unaligned for system memory and 16-bit access of packet memory
+ * @param   nbytes no. of bytes to be copied.
+ * @retval None
+ */
+static bool dcd_read_packet_memory(void *__restrict dst, uint16_t src, uint16_t nbytes) {
+  uint32_t n16 = (uint32_t)nbytes >> 1U;
+  fsdev_pma16_t* pma16 = (fsdev_pma16_t*) (USB_PMAADDR + FSDEV_PMA_STRIDE * src);
+  uint8_t *dst8 = (uint8_t *)dst;
+
+  while (n16--) {
+    uint16_t temp16 = pma16->u16;
+    tu_unaligned_write16(dst8, temp16);
+    dst8 += 2;
+    pma16++;
   }
 
-  if (wNBytes & 0x01) {
-    *pdw16 = (uint16_t) *src8;
+  if (nbytes & 0x01) {
+    *dst8++ = tu_u16_low(pma16->u16);
   }
 
   return true;
 }
+
 #endif
 
 /**
@@ -959,61 +1015,6 @@ static bool dcd_write_packet_memory_ff(tu_fifo_t *ff, uint16_t dst, uint16_t wNB
   return true;
 }
 
-#ifdef FSDEV_BUS_32BIT
-static bool dcd_read_packet_memory(void *__restrict dst, uint16_t src, uint16_t wNBytes) {
-  uint8_t *dst8 = dst;
-  volatile uint32_t *src32 = (volatile uint32_t *)(USB_PMAADDR + src);
-
-  for (uint32_t n = wNBytes / 4; n > 0; --n) {
-    tu_unaligned_write32(dst8, *src32++);
-    dst8 += 4;
-  }
-
-  uint16_t odd = wNBytes & 0x03;
-  if (odd) {
-    uint32_t rdVal = *src32;
-
-    *dst8 = tu_u32_byte0(rdVal);
-    odd--;
-
-    if (odd) {
-      *++dst8 = tu_u32_byte1(rdVal);
-      odd--;
-
-      if (odd) {
-        *++dst8 = tu_u32_byte2(rdVal);
-      }
-    }
-  }
-
-  return true;
-}
-#else
-/**
- * @brief Copy a buffer from packet memory area (PMA) to user memory area.
- *        Uses unaligned for system memory and 16-bit access of packet memory
- * @param   wNBytes no. of bytes to be copied.
- * @retval None
- */
-static bool dcd_read_packet_memory(void *__restrict dst, uint16_t src, uint16_t wNBytes) {
-  uint32_t n = (uint32_t)wNBytes >> 1U;
-  volatile const uint16_t *pdw16 = &pma[FSDEV_PMA_STRIDE * (src >> 1)];
-  uint8_t *dst8 = (uint8_t *)dst;
-
-  while (n--) {
-    tu_unaligned_write16(dst8, *pdw16);
-    dst8 += 2;
-    pdw16 += FSDEV_PMA_STRIDE;
-  }
-
-  if (wNBytes & 0x01) {
-    *dst8++ = tu_u16_low(*pdw16);
-  }
-
-  return true;
-}
-#endif
-
 /**
  * @brief Copy a buffer from user packet memory area (PMA) to FIFO.
  *        Uses byte-access of system memory and 16-bit access of packet memory
diff --git a/src/portable/st/stm32_fsdev/fsdev_ch32.h b/src/portable/st/stm32_fsdev/fsdev_ch32.h
index 81ef08b8a..8c0961bb9 100644
--- a/src/portable/st/stm32_fsdev/fsdev_ch32.h
+++ b/src/portable/st/stm32_fsdev/fsdev_ch32.h
@@ -54,29 +54,6 @@
 #endif
 
 #define FSDEV_PMA_SIZE (512u)
-
-#if 1
-// volatile 32-bit aligned
-#define _vaa32     volatile TU_ATTR_ALIGNED(4)
-
-typedef struct {
-  _vaa32 uint16_t EP0R;            // 00: USB Endpoint 0 register
-  _vaa32 uint16_t EP1R;            // 04: USB Endpoint 1 register
-  _vaa32 uint16_t EP2R;            // 08: USB Endpoint 2 register
-  _vaa32 uint16_t EP3R;            // 0C: USB Endpoint 3 register
-  _vaa32 uint16_t EP4R;            // 10: USB Endpoint 4 register
-  _vaa32 uint16_t EP5R;            // 14: USB Endpoint 5 register
-  _vaa32 uint16_t EP6R;            // 18: USB Endpoint 6 register
-  _vaa32 uint16_t EP7R;            // 1C: USB Endpoint 7 register
-  _vaa32 uint16_t RESERVED7[16];   // Reserved
-  _vaa32 uint16_t CNTR;            // 40: Control register
-  _vaa32 uint16_t ISTR;            // 44: Interrupt status register
-  _vaa32 uint16_t FNR;             // 48: Frame number register
-  _vaa32 uint16_t DADDR;           // 4C: Device address register
-  _vaa32 uint16_t BTABLE;          // 50: Buffer Table address register
-} USB_TypeDef;
-#endif
-
 #define FSDEV_REG_BASE  0x40005C00UL
 
 #define USB_BASE            (APB1PERIPH_BASE + 0x00005C00UL) /*!< USB_IP Peripheral Registers base address */
diff --git a/src/portable/st/stm32_fsdev/fsdev_type.h b/src/portable/st/stm32_fsdev/fsdev_type.h
index 116017f63..e4a0f3f28 100644
--- a/src/portable/st/stm32_fsdev/fsdev_type.h
+++ b/src/portable/st/stm32_fsdev/fsdev_type.h
@@ -75,21 +75,19 @@ enum {
 
 // Buffer Table is located in Packet Memory Area (PMA) and therefore its address access is forced to either
 // 16-bit or 32-bit depending on FSDEV_BUS_32BIT.
-typedef struct {
-  union {
-    // 0: TX (IN), 1: RX (OUT)
+typedef union {
+  // 0: TX (IN), 1: RX (OUT)
 
-    // strictly 16-bit access (could be 32-bit aligned)
-    struct {
-      volatile pma_aligned uint16_t addr;
-      volatile pma_aligned uint16_t count;
-    } ep16[FSDEV_EP_COUNT][2];
+  // strictly 16-bit access (could be 32-bit aligned)
+  struct {
+    volatile pma_aligned uint16_t addr;
+    volatile pma_aligned uint16_t count;
+  } ep16[FSDEV_EP_COUNT][2];
 
-    // strictly 32-bit access
-    struct {
-      volatile uint32_t count_addr;
-    } ep32[FSDEV_EP_COUNT][2];
-  };
+  // strictly 32-bit access
+  struct {
+    volatile uint32_t count_addr;
+  } ep32[FSDEV_EP_COUNT][2];
 } fsdev_btable_t;
 
 TU_VERIFY_STATIC(sizeof(fsdev_btable_t) == FSDEV_EP_COUNT*8*FSDEV_PMA_STRIDE, "size is not correct");
@@ -97,6 +95,10 @@ 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))
 
+typedef struct {
+  volatile pma_aligned uint16_t u16;
+} fsdev_pma16_t;
+
 //--------------------------------------------------------------------+
 // Registers Typedef
 //--------------------------------------------------------------------+
@@ -105,16 +107,10 @@ TU_VERIFY_STATIC(FSDEV_BTABLE_BASE + FSDEV_EP_COUNT*8 <= FSDEV_PMA_SIZE, "BTABLE
 #define _va32     volatile TU_ATTR_ALIGNED(4)
 
 // The fsdev_bus_t type can be used for both register and PMA access necessities
-// For type-safety create a new macro for the volatile address of PMAADDR
-// The compiler should warn us if we cast it to a non-volatile type?
 #ifdef FSDEV_BUS_32BIT
 typedef uint32_t fsdev_bus_t;
-static volatile uint32_t * const pma32 = (volatile uint32_t*)USB_PMAADDR;
-
 #else
 typedef uint16_t fsdev_bus_t;
-// Volatile is also needed to prevent the optimizer from changing access to 32-bit (as 32-bit access is forbidden)
-static volatile uint16_t * const pma = (volatile uint16_t*)USB_PMAADDR;
 #endif
 
 typedef struct {