Refactor setting a USB EP rx count

Signed-off-by: Manuel Odendahl <wesen@ruinwesen.com>

1 files changed, 7 insertions, 18 deletions

diff --git a/libmaple/usb/stm32f1/usb_reg_map.c b/libmaple/usb/stm32f1/usb_reg_map.c
index fbfca86..71bbe0d 100644
--- a/libmaple/usb/stm32f1/usb_reg_map.c
+++ b/libmaple/usb/stm32f1/usb_reg_map.c
@@ -58,8 +58,7 @@ void usb_copy_from_pma(uint8 *buf, uint16 len, uint16 pma_offset) {
     }
 }
 
-void usb_set_ep_rx_buf0_count(uint8 ep, uint16 count) {
-    uint32 *rxc = usb_ep_tx_count_ptr(ep);
+static void usb_set_ep_rx_count_common(uint32 *rxc, uint16 count) {
     uint16 nblocks;
     if (count > 62) {
         /* use 32-byte memory block size */
@@ -78,22 +77,12 @@ void usb_set_ep_rx_buf0_count(uint8 ep, uint16 count) {
     }
 }
 
+void usb_set_ep_rx_buf0_count(uint8 ep, uint16 count) {
+    uint32 *rxc = usb_ep_tx_count_ptr(ep);
+    usb_set_ep_rx_count_common(rxc, count);
+}
+
 void usb_set_ep_rx_count(uint8 ep, uint16 count) {
     uint32 *rxc = usb_ep_rx_count_ptr(ep);
-    uint16 nblocks;
-    if (count > 62) {
-        /* use 32-byte memory block size */
-        nblocks = count >> 5;
-        if ((count & 0x1F) == 0) {
-            nblocks--;
-        }
-        *rxc = (nblocks << 10) | 0x8000;
-    } else {
-        /* use 2-byte memory block size */
-        nblocks = count >> 1;
-        if ((count & 0x1) != 0) {
-            nblocks++;
-        }
-        *rxc = nblocks << 10;
-    }
+    usb_set_ep_rx_count_common(rxc, count);
 }