Fix/devfs rxcount (#70)

* fix devfs rxcount calculations

issue #69

* fix devfs rxcount calculations (asm)

issue #69

7 files changed, 63 insertions, 110 deletions

diff --git a/src/usbd_stm32f103_devfs.c b/src/usbd_stm32f103_devfs.c
index 312a881..0ed54d4 100644
--- a/src/usbd_stm32f103_devfs.c
+++ b/src/usbd_stm32f103_devfs.c
@@ -269,8 +269,8 @@ static void setaddr (uint8_t addr) {
 static bool ep_config(uint8_t ep, uint8_t eptype, uint16_t epsize) {
     volatile uint16_t *reg = EPR(ep);
     pma_table *tbl = EPT(ep);
-    /* epsize should be 16-bit aligned */
-    if (epsize & 0x01) epsize++;
+    /* epsize must be 2-byte aligned */
+    epsize = (~0x01U) & (epsize + 1);
 
     switch (eptype) {
     case USB_EPTYPE_CONTROL:
@@ -311,11 +311,9 @@ static bool ep_config(uint8_t ep, uint8_t eptype, uint16_t epsize) {
         uint16_t _rxcnt;
         uint16_t _pma;
         if (epsize > 62) {
-            if (epsize & 0x1F) {
-                epsize &= ~0x1F;
-                epsize += 0x20;
-            }
-            _rxcnt = 0x8000 - 0x20 + (epsize << 5);
+            /* using 32-byte blocks. epsize must be 32-byte aligned */
+            epsize = (~0x1FU) & (epsize + 0x1FU);
+            _rxcnt = 0x8000U - 0x400U + (epsize << 5);
         } else {
             _rxcnt = epsize << 9;
         }
diff --git a/src/usbd_stm32f103_devfs_asm.S b/src/usbd_stm32f103_devfs_asm.S
index 308551e..ae1e0c5 100644
--- a/src/usbd_stm32f103_devfs_asm.S
+++ b/src/usbd_stm32f103_devfs_asm.S
@@ -53,15 +53,7 @@
     #define RXCOUNT1    0x06
     #define USB_PMASZ   0x300
     #define USB_PMASTP  0x02
-
-    .macro GetEPT
-    add     r4, r4, r0, LSR #25     //*EPT -> R4
-    .endm
-
-    .macro GetPMA
-    ldr     r4, =#USB_PMABASE
-    adds    r5, r4          // PMA BUFFER -> R5
-    .endm
+    #define EPT_SHIFT   3
 
 #else
     #define RXADDR0     0x00
@@ -70,16 +62,7 @@
     #define RXCOUNT1    0x0C
     #define USB_PMASZ   0x200
     #define USB_PMASTP  0x04
-
-    .macro GetEPT
-    add     r4, r4, r0, LSR #24     //*EPT -> R4
-    .endm
-
-    .macro GetPMA
-    ldr     r4, =#USB_PMABASE
-    lsls    r5, #1
-    adds    r5, r4          // PMA BUFFER -> R5
-    .endm
+    #define EPT_SHIFT   4
 
 #endif
 
@@ -430,12 +413,12 @@ _ep_isstalled:
  * out length of the recieved data -> R0 or 0 on error
  */
 _ep_read:
-    push    {r4, r5, lr}
+    push    {r4, r5, r6, lr}
     ldr     r3, =#USB_EPBASE
-    ldr     r4, =#USB_PMABASE
+    ldr     r6, =#USB_PMABASE
     lsls    r0, #28
     add     r3, r3, r0, LSR #26     //*EPR -> R3
-    GetEPT                          //*EPT -> R4
+    add     r4, r6, r0, LSR (28 - EPT_SHIFT) //*EPT -> R4
     ldrh    r5, [r3]                // reading epr
 /* validating endpoint */
     movs    r0, #0x37
@@ -468,16 +451,16 @@ _ep_read:
 .L_epr_iso:
     lsrs    r5, #15         // DTOG_RX -> CF
     bcs     .L_epr_sngl
-    subs    r4, #0x08       // set RXADDR0
+    subs    r4, #RXADDR1    // set RXADDR0
 .L_epr_sngl:
     ldrh    r0, [r4, #RXCOUNT]
     lsrs    r5, r0, #0x0A
     lsls    r5, #0x0A       // r5 = r5 & ~0x03FF
     strh    r5, [r4, #RXCOUNT]
-    lsls    r0, #22
-    lsrs    r0, #22         // r0 &= 0x3FF (RX count)
+    eors    r0, r5          // r0 &= 0x3FF (RX count)
     ldrh    r5, [r4, #RXADDR]
-    GetPMA
+//    ldr     r4, =USB_PMABASE
+    adds    r5, r4, r5, LSL (EPT_SHIFT - 3)
     cmp     r2, r0
     blo     .L_epr_read
     mov     r2, r0          // if buffer is larger
@@ -506,7 +489,7 @@ _ep_read:
     and     r5, r5, r2, LSR #16
     strh    r5, [r3]        // set ep to VALID state
 .L_epr_exit:
-    pop     {r4, r5, pc}
+    pop     {r4, r5, r6, pc}
     .size   _ep_read, . - _ep_read
 
 
@@ -521,10 +504,10 @@ _ep_read:
 _ep_write:
     push    {r4, r5, r6, lr}
     ldr     r3, =#USB_EPBASE
-    ldr     r4, =#USB_PMABASE
+    ldr     r6, =#USB_PMABASE
     lsls    r0, #28
     add     r3, r3, r0, LSR #26     //*EPR -> R3
-    GetEPT
+    add     r4, r6, r0, LSR (28 - EPT_SHIFT) //*EPT -> R4
     ldrh    r5, [r3]        // reading epr
     movs    r0, #0x73
     and     r0, r0, r5, LSR #4
@@ -547,20 +530,19 @@ _ep_write:
 .L_epw_iso:
     lsrs    r5, #7          // DTOG_TX -> CF
     bcs     .L_epw_sngl
-    adds    r4, #8          // TXADDR1 -> R4
+    adds    r4, #RXADDR1    // TXADDR1 -> R4
 .L_epw_sngl:
     strh    r2, [r4, #TXCOUNT]
     mov     r0, r2          // save count for return
     ldrh    r5, [r4, #TXADDR]
-    GetPMA
+    adds    r5, r6, r5, LSL (EPT_SHIFT - 3)
 .L_epw_write:
     cmp     r2, #1
     blo     .L_epw_write_end
     ldrb    r4, [r1]
     beq     .L_epw_store
     ldrb    r6, [r1, #1]
-    lsls    r6, #8
-    orrs    r4, r6
+    orr     r4, r4, r6, LSL #8
 .L_epw_store:
     strh    r4, [r5]
     adds    r5, #USB_PMASTP
@@ -605,7 +587,7 @@ _get_next_pma:
     blo     .L_gnp_nxtaddr
     mov     r3, r4
 .L_gnp_nxtaddr:
-    adds    r0, #8
+    adds    r0, #RXADDR1
     subs    r1, #1
     bne     .L_gnp_chkaddr
     subs    r0, r3, r2
@@ -613,9 +595,6 @@ _get_next_pma:
     cmp     r0, #0x20       //check for the pma table overlap
 .L_gnp_exit:
     pop     {r1, r3, r4, pc}
-
-
-
     .size   _get_next_pma, . - _get_next_pma
 
     .thumb_func
@@ -628,26 +607,25 @@ _get_next_pma:
  */
 _ep_config:
     push    {r4, r5, lr}
-    movs    r3, 0x01
-    ands    r3, r2
-    adds    r2, r3      //R2 -> halfword aligned epsize
-    movs    r3, #0x00   //BULK
-    cmp     r1, #0x02   // is eptype bulk ?
+    movs    r3, #0x01
+    adds    r2, r3
+    bics    r2, r3          //R2 -> halfword aligned epsize
+    cmp     r1, #0x06       //DBLBULK (0x01)
     beq     .L_epc_settype
-    movs    r3, #0x01   //DBLBULK
-    cmp     r1, #0x06
+    movs    r3, #0x00
+    cmp     r1, #0x02       //BULK
     beq     .L_epc_settype
-    movs    r3, #0x02   //CONTROL
-    cmp     r1, #0x00
+    movs    r3, #0x02
+    cmp     r1, #0x00       //CONTROL
     beq     .L_epc_settype
-    movs    r3, #0x04   //ISO
-    cmp     r1, #0x01
+    movs    r3, #0x04
+    cmp     r1, #0x01       //ISO
     beq     .L_epc_settype
-    movs    r3, #0x06   //INTERRUPT
+    movs    r3, #0x06       //INTERRUPT
 .L_epc_settype:
     lsls    r3, #8
-    lsls    r4, r0, #28
-    lsrs    r4, #28
+    movs    r4, #0x07
+    ands    r4, r0
     orrs    r3, r4
     lsls    r4, #2
     ldr     r5, =#USB_EPBASE
@@ -658,8 +636,7 @@ _ep_config:
     blo     .L_epc_setuprx
 .L_epc_setuptx:
     ldr     r5, =#USB_PMABASE
-    lsls    r4, #2
-    adds    r5, r4
+    adds    r5, r5, r4, LSL (EPT_SHIFT - 2)
     bl      _get_next_pma
     bcc     .L_epc_fail
     strh    r0, [r5, #TXADDR]    //store txaddr or txaddr0
@@ -679,7 +656,6 @@ _ep_config:
     strh    r0, [r5, #TXCOUNT1]    //store txcnt
 .L_epc_txsetstate:
     ldr     r5, =#USB_EPBASE
-    lsrs    r4, #2
     ldrh    r0, [r5, r4]
     eors    r0, r3
     lsrs    r3, #16
@@ -688,23 +664,18 @@ _ep_config:
     cmp     r1, #0x00       //is a control ep ?
     bne     .L_epc_exit
 .L_epc_setuprx:
-    mov     r3, r2
+    movs    r3, r2
     cmp     r2, #62
     bls     .L_epc_rxbb
     movs    r3, #0x1F
-    ands    r3, r2
-    bne     .L_epc_rxaa
-    subs    r2, #0x20
-.L_epc_rxaa:
+    adds    r2, r3
     bics    r2, r3
     lsrs    r3, r2, #4
-    adds    r3, #0x40
-    adds    r2, #0x20
+    adds    r3, #0x3E
 .L_epc_rxbb:
     lsls    r3, #9
     ldr     r5, =#USB_PMABASE
-    lsls    r4, #2
-    adds    r5, r4
+    adds    r5, r5, r4, LSL (EPT_SHIFT - 2)
 /* RX or RX1 */
     bl      _get_next_pma
     bcc     .L_epc_fail
@@ -724,7 +695,6 @@ _ep_config:
     ldr     r0, =#DRX_USTALL
 .L_epc_rxsetstate:
     ldr     r5, =#USB_EPBASE
-    lsrs    r4, #2
     ldrh    r3, [r5, r4]
     eors    r3, r0
     lsrs    r0, #16
@@ -752,8 +722,7 @@ _ep_deconfig:
     ldr     r2, =#USB_EPBASE
     ldr     r3, =#USB_PMABASE
     adds    r2, r1
-    lsls    r1, #1
-    adds    r3, r1
+    adds    r3, r3, r1, LSL (EPT_SHIFT - 2)
 /* clearing endpoint register */
     ldr     r1, =#EP_NOTOG
     ldrh    r0, [r2]
diff --git a/src/usbd_stm32l052_devfs.c b/src/usbd_stm32l052_devfs.c
index e6b7f71..1b9e0eb 100644
--- a/src/usbd_stm32l052_devfs.c
+++ b/src/usbd_stm32l052_devfs.c
@@ -195,8 +195,8 @@ static void setaddr (uint8_t addr) {
 static bool ep_config(uint8_t ep, uint8_t eptype, uint16_t epsize) {
     volatile uint16_t *reg = EPR(ep);
     pma_table *tbl = EPT(ep);
-    /* epsize should be 16-bit aligned */
-    if (epsize & 0x01) epsize++;
+    /* epsize must be 2-byte aligned */
+    epsize = (~0x01U) & (epsize + 1);
 
     switch (eptype) {
     case USB_EPTYPE_CONTROL:
@@ -237,11 +237,9 @@ static bool ep_config(uint8_t ep, uint8_t eptype, uint16_t epsize) {
         uint16_t _rxcnt;
         uint16_t _pma;
         if (epsize > 62) {
-            if (epsize & 0x1F) {
-                epsize &= ~0x1F;
-                epsize += 0x20;
-            }
-            _rxcnt = 0x8000 - 0x20 + (epsize << 5);
+            /* using 32-byte blocks. epsize must be 32-byte aligned */
+            epsize = (~0x1FU) & (epsize + 0x1FU);
+            _rxcnt = 0x8000 - 0x400 + (epsize << 5);
         } else {
             _rxcnt = epsize << 9;
         }
diff --git a/src/usbd_stm32l052_devfs_asm.S b/src/usbd_stm32l052_devfs_asm.S
index 1f1c2fa..0bc10c3 100644
--- a/src/usbd_stm32l052_devfs_asm.S
+++ b/src/usbd_stm32l052_devfs_asm.S
@@ -219,7 +219,6 @@ _enable:
     ldr     r1, =#USB_REGBASE     //USB->CNTR
     ldr     r2, =#RCC_BASE        //RCC
     movs    r3, #0x01
-//    lsls    r3, #23             //USBEN or USBRST
     lsls    r3, #RCC_USBEN        //USBEN or USBRST
     tst     r0, r0
     beq     .L_disable
@@ -647,18 +646,15 @@ _ep_config:
     bne     .L_epc_exit
 .L_epc_setuprx:
 /* calculating RX_COUNT field. result in R3*/
-    mov     r3, r2
+    movs    r3, r2
     cmp     r2, #62
     bls     .L_epc_rxbb
+    /* ep size must be 32-byte aligned if >= 64 */
     movs    r3, #0x1F
-    ands    r3, r2
-    bne     .L_epc_rxaa
-    subs    r2, #0x20
-.L_epc_rxaa:
+    adds    r2, r3
     bics    r2, r3
     lsrs    r3, r2, #4
-    adds    r3, #0x40
-    adds    r2, #0x20
+    adds    r3, #0x3E
 .L_epc_rxbb:
     lsls    r3, #9
     ldr     r5, =#USB_PMABASE
diff --git a/src/usbd_stm32l100_devfs.c b/src/usbd_stm32l100_devfs.c
index c2c978f..29bd259 100644
--- a/src/usbd_stm32l100_devfs.c
+++ b/src/usbd_stm32l100_devfs.c
@@ -179,8 +179,8 @@ static void setaddr (uint8_t addr) {
 static bool ep_config(uint8_t ep, uint8_t eptype, uint16_t epsize) {
     volatile uint16_t *reg = EPR(ep);
     pma_table *tbl = EPT(ep);
-    /* epsize should be 16-bit aligned */
-    if (epsize & 0x01) epsize++;
+    /* epsize must be 2-byte aligned */
+    epsize = (~0x01U) & (epsize + 1);
 
     switch (eptype) {
     case USB_EPTYPE_CONTROL:
@@ -221,11 +221,9 @@ static bool ep_config(uint8_t ep, uint8_t eptype, uint16_t epsize) {
         uint16_t _rxcnt;
         uint16_t _pma;
         if (epsize > 62) {
-            if (epsize & 0x1F) {
-                epsize &= ~0x1F;
-                epsize += 0x20;
-            }
-            _rxcnt = 0x8000 - 0x20 + (epsize << 5);
+            /* using 32-byte blocks. epsize must be 32-byte aligned */
+            epsize = (~0x1FU) & (epsize + 0x1FU);
+            _rxcnt = 0x8000 - 0x400 + (epsize << 5);
         } else {
             _rxcnt = epsize << 9;
         }
diff --git a/src/usbd_stm32l100_devfs_asm.S b/src/usbd_stm32l100_devfs_asm.S
index cc0a2f3..956d942 100644
--- a/src/usbd_stm32l100_devfs_asm.S
+++ b/src/usbd_stm32l100_devfs_asm.S
@@ -623,18 +623,14 @@ _ep_config:
     cmp     r1, #0x00       //is a control ep ?
     bne     .L_epc_exit
 .L_epc_setuprx:
-    mov     r3, r2
+    movs    r3, r2
     cmp     r2, #62
     bls     .L_epc_rxbb
     movs    r3, #0x1F
-    ands    r3, r2
-    bne     .L_epc_rxaa
-    subs    r2, #0x20
-.L_epc_rxaa:
+    adds    r2, r3
     bics    r2, r3
-    lsrs    r3, r2, #4
-    adds    r3, #0x40
-    adds    r2, #0x20
+    lsrs    r3, r2, #5
+    adds    r3, #0x3E
 .L_epc_rxbb:
     lsls    r3, #9
     ldr     r5, =#USB_PMABASE
diff --git a/src/usbd_stm32l433_devfs.c b/src/usbd_stm32l433_devfs.c
index 538de90..c738c67 100644
--- a/src/usbd_stm32l433_devfs.c
+++ b/src/usbd_stm32l433_devfs.c
@@ -196,8 +196,8 @@ static void setaddr (uint8_t addr) {
 static bool ep_config(uint8_t ep, uint8_t eptype, uint16_t epsize) {
     volatile uint16_t *reg = EPR(ep);
     pma_table *tbl = EPT(ep);
-    /* epsize should be 16-bit aligned */
-    if (epsize & 0x01) epsize++;
+    /* epsize must be 2-byte aligned */
+    epsize = (~0x01U) & (epsize + 1);
 
     switch (eptype) {
     case USB_EPTYPE_CONTROL:
@@ -238,11 +238,9 @@ static bool ep_config(uint8_t ep, uint8_t eptype, uint16_t epsize) {
         uint16_t _rxcnt;
         uint16_t _pma;
         if (epsize > 62) {
-            if (epsize & 0x1F) {
-                epsize &= ~0x1F;
-                epsize += 0x20;
-            }
-            _rxcnt = 0x8000 - 0x20 + (epsize << 5);
+            /* using 32-byte blocks. epsize must be 32-byte aligned */
+            epsize = (~0x1FU) & (epsize + 0x1FU);
+            _rxcnt = 0x8000 - 0x400 + (epsize << 5);
         } else {
             _rxcnt = epsize << 9;
         }