dcd_msp430x5xx: Add STALL support for nonzero endpoints.

1 files changed, 12 insertions, 0 deletions

diff --git a/src/portable/ti/msp430x5xx/dcd_msp430x5xx.c b/src/portable/ti/msp430x5xx/dcd_msp430x5xx.c
index 121dd03ac..5202f156a 100644
--- a/src/portable/ti/msp430x5xx/dcd_msp430x5xx.c
+++ b/src/portable/ti/msp430x5xx/dcd_msp430x5xx.c
@@ -346,6 +346,11 @@ void dcd_edpt_stall (uint8_t rhport, uint8_t ep_addr)
       USBIEPCNF_0 |= STALL;
     }
   }
+  else
+  {
+    ep_regs_t ep_regs = EP_REGS(epnum, dir);
+    ep_regs[CNF] |= STALL;
+  }
 }
 
 void dcd_edpt_clear_stall (uint8_t rhport, uint8_t ep_addr)
@@ -366,6 +371,13 @@ void dcd_edpt_clear_stall (uint8_t rhport, uint8_t ep_addr)
       USBIEPCNT_0 &= ~STALL;
     }
   }
+  else
+  {
+    ep_regs_t ep_regs = EP_REGS(epnum, dir);
+    // Required by USB spec to reset DATA toggle bit to DATA0 on interrupt
+    // and bulk endpoints.
+    ep_regs[CNF] &= ~(STALL + TOGGLE);
+  }
 }
 
 /*------------------------------------------------------------------*/