diff --git a/src/machine/machine_atsamd21_usb.go b/src/machine/machine_atsamd21_usb.go
index 28e8c6d22..45ba18d45 100644
--- a/src/machine/machine_atsamd21_usb.go
+++ b/src/machine/machine_atsamd21_usb.go
@@ -175,7 +175,7 @@ func handleUSBIRQ(intr interrupt.Interrupt) {
 
 	// Now the actual transfer handlers, ignore endpoint number 0 (setup)
 	var i uint32
-	for i = 1; i < uint32(len(endPoints)); i++ {
+	for i = 1; i < NumberOfUSBEndpoints; i++ {
 		// Check if endpoint has a pending interrupt
 		epFlags := getEPINTFLAG(i)
 		setEPINTFLAG(i, epFlags)
@@ -193,6 +193,8 @@ func handleUSBIRQ(intr interrupt.Interrupt) {
 }
 
 func initEndpoint(ep, config uint32) {
+	// Note: Both IN (Bank 1) and OUT (Bank 0) configurations share the same EPCFG register.
+	// We must use getEPCFG(ep) | ... to avoid clearing/disabling the opposite direction.
 	switch config {
 	case usb.ENDPOINT_TYPE_INTERRUPT | usb.EndpointIn:
 		// set packet size
@@ -202,7 +204,7 @@ func initEndpoint(ep, config uint32) {
 		usbEndpointDescriptors[ep].DeviceDescBank[1].ADDR.Set(uint32(uintptr(unsafe.Pointer(&udd_ep_in_cache_buffer[ep]))))
 
 		// set endpoint type
-		setEPCFG(ep, ((usb.ENDPOINT_TYPE_INTERRUPT + 1) << sam.USB_DEVICE_EPCFG_EPTYPE1_Pos))
+		setEPCFG(ep, getEPCFG(ep)|((usb.ENDPOINT_TYPE_INTERRUPT+1)<<sam.USB_DEVICE_EPCFG_EPTYPE1_Pos))
 
 		setEPINTENSET(ep, sam.USB_DEVICE_EPINTENSET_TRCPT1)
 
@@ -214,7 +216,7 @@ func initEndpoint(ep, config uint32) {
 		usbEndpointDescriptors[ep].DeviceDescBank[0].ADDR.Set(uint32(uintptr(unsafe.Pointer(&udd_ep_out_cache_buffer[ep]))))
 
 		// set endpoint type
-		setEPCFG(ep, ((usb.ENDPOINT_TYPE_BULK + 1) << sam.USB_DEVICE_EPCFG_EPTYPE0_Pos))
+		setEPCFG(ep, getEPCFG(ep)|((usb.ENDPOINT_TYPE_BULK+1)<<sam.USB_DEVICE_EPCFG_EPTYPE0_Pos))
 
 		// receive interrupts when current transfer complete
 		setEPINTENSET(ep, sam.USB_DEVICE_EPINTENSET_TRCPT0)
@@ -233,7 +235,7 @@ func initEndpoint(ep, config uint32) {
 		usbEndpointDescriptors[ep].DeviceDescBank[0].ADDR.Set(uint32(uintptr(unsafe.Pointer(&udd_ep_out_cache_buffer[ep]))))
 
 		// set endpoint type
-		setEPCFG(ep, ((usb.ENDPOINT_TYPE_INTERRUPT + 1) << sam.USB_DEVICE_EPCFG_EPTYPE0_Pos))
+		setEPCFG(ep, getEPCFG(ep)|((usb.ENDPOINT_TYPE_INTERRUPT+1)<<sam.USB_DEVICE_EPCFG_EPTYPE0_Pos))
 
 		// receive interrupts when current transfer complete
 		setEPINTENSET(ep, sam.USB_DEVICE_EPINTENSET_TRCPT0)
@@ -252,7 +254,7 @@ func initEndpoint(ep, config uint32) {
 		usbEndpointDescriptors[ep].DeviceDescBank[1].ADDR.Set(uint32(uintptr(unsafe.Pointer(&udd_ep_in_cache_buffer[ep]))))
 
 		// set endpoint type
-		setEPCFG(ep, ((usb.ENDPOINT_TYPE_BULK + 1) << sam.USB_DEVICE_EPCFG_EPTYPE1_Pos))
+		setEPCFG(ep, getEPCFG(ep)|((usb.ENDPOINT_TYPE_BULK+1)<<sam.USB_DEVICE_EPCFG_EPTYPE1_Pos))
 
 		// NAK on endpoint IN, the bank is not yet filled in.
 		setEPSTATUSCLR(ep, sam.USB_DEVICE_EPSTATUSCLR_BK1RDY)
@@ -650,3 +652,19 @@ func setEPINTENSET(ep uint32, val uint8) {
 		return
 	}
 }
+
+func (dev *USBDevice) SetStallEPIn(ep uint32) {
+	setEPSTATUSSET(ep, sam.USB_DEVICE_EPSTATUSSET_STALLRQ1)
+}
+
+func (dev *USBDevice) SetStallEPOut(ep uint32) {
+	setEPSTATUSSET(ep, sam.USB_DEVICE_EPSTATUSSET_STALLRQ0)
+}
+
+func (dev *USBDevice) ClearStallEPIn(ep uint32) {
+	setEPSTATUSCLR(ep, sam.USB_DEVICE_EPSTATUSCLR_STALLRQ1)
+}
+
+func (dev *USBDevice) ClearStallEPOut(ep uint32) {
+	setEPSTATUSCLR(ep, sam.USB_DEVICE_EPSTATUSCLR_STALLRQ0)
+}
diff --git a/src/machine/machine_atsamd51_usb.go b/src/machine/machine_atsamd51_usb.go
index 263393c3a..186ba4ce0 100644
--- a/src/machine/machine_atsamd51_usb.go
+++ b/src/machine/machine_atsamd51_usb.go
@@ -178,7 +178,7 @@ func handleUSBIRQ(intr interrupt.Interrupt) {
 
 	// Now the actual transfer handlers, ignore endpoint number 0 (setup)
 	var i uint32
-	for i = 1; i < uint32(len(endPoints)); i++ {
+	for i = 1; i < NumberOfUSBEndpoints; i++ {
 		// Check if endpoint has a pending interrupt
 		epFlags := getEPINTFLAG(i)
 		setEPINTFLAG(i, epFlags)
@@ -196,6 +196,8 @@ func handleUSBIRQ(intr interrupt.Interrupt) {
 }
 
 func initEndpoint(ep, config uint32) {
+	// Note: Both IN (Bank 1) and OUT (Bank 0) configurations share the same EPCFG register.
+	// We must use getEPCFG(ep) | ... to avoid clearing/disabling the opposite direction.
 	switch config {
 	case usb.ENDPOINT_TYPE_INTERRUPT | usb.EndpointIn:
 		// set packet size
@@ -205,7 +207,7 @@ func initEndpoint(ep, config uint32) {
 		usbEndpointDescriptors[ep].DeviceDescBank[1].ADDR.Set(uint32(uintptr(unsafe.Pointer(&udd_ep_in_cache_buffer[ep]))))
 
 		// set endpoint type
-		setEPCFG(ep, ((usb.ENDPOINT_TYPE_INTERRUPT + 1) << sam.USB_DEVICE_ENDPOINT_EPCFG_EPTYPE1_Pos))
+		setEPCFG(ep, getEPCFG(ep)|((usb.ENDPOINT_TYPE_INTERRUPT+1)<<sam.USB_DEVICE_ENDPOINT_EPCFG_EPTYPE1_Pos))
 
 		setEPINTENSET(ep, sam.USB_DEVICE_ENDPOINT_EPINTENSET_TRCPT1)
 
@@ -217,7 +219,7 @@ func initEndpoint(ep, config uint32) {
 		usbEndpointDescriptors[ep].DeviceDescBank[0].ADDR.Set(uint32(uintptr(unsafe.Pointer(&udd_ep_out_cache_buffer[ep]))))
 
 		// set endpoint type
-		setEPCFG(ep, ((usb.ENDPOINT_TYPE_BULK + 1) << sam.USB_DEVICE_ENDPOINT_EPCFG_EPTYPE0_Pos))
+		setEPCFG(ep, getEPCFG(ep)|((usb.ENDPOINT_TYPE_BULK+1)<<sam.USB_DEVICE_ENDPOINT_EPCFG_EPTYPE0_Pos))
 
 		// receive interrupts when current transfer complete
 		setEPINTENSET(ep, sam.USB_DEVICE_ENDPOINT_EPINTENSET_TRCPT0)
@@ -236,7 +238,7 @@ func initEndpoint(ep, config uint32) {
 		usbEndpointDescriptors[ep].DeviceDescBank[0].ADDR.Set(uint32(uintptr(unsafe.Pointer(&udd_ep_out_cache_buffer[ep]))))
 
 		// set endpoint type
-		setEPCFG(ep, ((usb.ENDPOINT_TYPE_INTERRUPT + 1) << sam.USB_DEVICE_ENDPOINT_EPCFG_EPTYPE0_Pos))
+		setEPCFG(ep, getEPCFG(ep)|((usb.ENDPOINT_TYPE_INTERRUPT+1)<<sam.USB_DEVICE_ENDPOINT_EPCFG_EPTYPE0_Pos))
 
 		// receive interrupts when current transfer complete
 		setEPINTENSET(ep, sam.USB_DEVICE_ENDPOINT_EPINTENSET_TRCPT0)
@@ -255,7 +257,7 @@ func initEndpoint(ep, config uint32) {
 		usbEndpointDescriptors[ep].DeviceDescBank[1].ADDR.Set(uint32(uintptr(unsafe.Pointer(&udd_ep_in_cache_buffer[ep]))))
 
 		// set endpoint type
-		setEPCFG(ep, ((usb.ENDPOINT_TYPE_BULK + 1) << sam.USB_DEVICE_ENDPOINT_EPCFG_EPTYPE1_Pos))
+		setEPCFG(ep, getEPCFG(ep)|((usb.ENDPOINT_TYPE_BULK+1)<<sam.USB_DEVICE_ENDPOINT_EPCFG_EPTYPE1_Pos))
 
 		// NAK on endpoint IN, the bank is not yet filled in.
 		setEPSTATUSCLR(ep, sam.USB_DEVICE_ENDPOINT_EPSTATUSCLR_BK1RDY)
@@ -481,3 +483,19 @@ func setEPINTENCLR(ep uint32, val uint8) {
 func setEPINTENSET(ep uint32, val uint8) {
 	sam.USB_DEVICE.DEVICE_ENDPOINT[ep].EPINTENSET.Set(val)
 }
+
+func (dev *USBDevice) SetStallEPIn(ep uint32) {
+	setEPSTATUSSET(ep, sam.USB_DEVICE_ENDPOINT_EPSTATUSSET_STALLRQ1)
+}
+
+func (dev *USBDevice) SetStallEPOut(ep uint32) {
+	setEPSTATUSSET(ep, sam.USB_DEVICE_ENDPOINT_EPSTATUSSET_STALLRQ0)
+}
+
+func (dev *USBDevice) ClearStallEPIn(ep uint32) {
+	setEPSTATUSCLR(ep, sam.USB_DEVICE_ENDPOINT_EPSTATUSCLR_STALLRQ1)
+}
+
+func (dev *USBDevice) ClearStallEPOut(ep uint32) {
+	setEPSTATUSCLR(ep, sam.USB_DEVICE_ENDPOINT_EPSTATUSCLR_STALLRQ0)
+}
diff --git a/src/machine/machine_rp2_usb.go b/src/machine/machine_rp2_usb.go
index bf0e20b41..8a90aacea 100644
--- a/src/machine/machine_rp2_usb.go
+++ b/src/machine/machine_rp2_usb.go
@@ -20,38 +20,56 @@ var (
 
 func initEndpoint(ep, config uint32) {
 	val := uint32(usbEpControlEnable) | uint32(usbEpControlInterruptPerBuff)
+
+	// Each endpoint has 128 bytes of DPRAM buffer space allocated (2 * usbBufferLen).
+	// To support bidirectional configurations using the same endpoint number,
+	// we allocate the first 64 bytes (Buffer0) to OUT transfers, and the remaining
+	// 64 bytes (Buffer1) to IN transfers by shifting the IN offset by usbBufferLen.
 	offset := ep*2*usbBufferLen + 0x100
-	val |= offset
 
 	// Bulk and interrupt endpoints must have their Packet ID reset to DATA0 when un-stalled.
-	epXPIDReset[ep] = false // Default to false in case an endpoint is re-initialized.
+	// Since both directions share the same ep, we reset their PID flags independently.
+	if (config & usb.EndpointIn) != 0 {
+		epXPIDResetIn[ep] = false
+	} else {
+		epXPIDResetOut[ep] = false
+	}
 
 	switch config {
 	case usb.ENDPOINT_TYPE_INTERRUPT | usb.EndpointIn:
+		epXPIDResetIn[ep] = true
+		epXdata0In[ep] = false
+		val |= offset + usbBufferLen
 		val |= usbEpControlEndpointTypeInterrupt
 		_usbDPSRAM.EPxControl[ep].In.Set(val)
-		epXPIDReset[ep] = true
 
 	case usb.ENDPOINT_TYPE_BULK | usb.EndpointOut:
+		epXPIDResetOut[ep] = true
+		epXdata0Out[ep] = false
+		val |= offset
 		val |= usbEpControlEndpointTypeBulk
 		_usbDPSRAM.EPxControl[ep].Out.Set(val)
 		_usbDPSRAM.EPxBufferControl[ep].Out.Set(usbBufferLen & usbBuf0CtrlLenMask)
 		_usbDPSRAM.EPxBufferControl[ep].Out.SetBits(usbBuf0CtrlAvail)
-		epXPIDReset[ep] = true
 
 	case usb.ENDPOINT_TYPE_INTERRUPT | usb.EndpointOut:
+		epXPIDResetOut[ep] = true
+		epXdata0Out[ep] = false
+		val |= offset
 		val |= usbEpControlEndpointTypeInterrupt
 		_usbDPSRAM.EPxControl[ep].Out.Set(val)
 		_usbDPSRAM.EPxBufferControl[ep].Out.Set(usbBufferLen & usbBuf0CtrlLenMask)
 		_usbDPSRAM.EPxBufferControl[ep].Out.SetBits(usbBuf0CtrlAvail)
-		epXPIDReset[ep] = true
 
 	case usb.ENDPOINT_TYPE_BULK | usb.EndpointIn:
+		epXPIDResetIn[ep] = true
+		epXdata0In[ep] = false
+		val |= offset + usbBufferLen
 		val |= usbEpControlEndpointTypeBulk
 		_usbDPSRAM.EPxControl[ep].In.Set(val)
-		epXPIDReset[ep] = true
 
 	case usb.ENDPOINT_TYPE_CONTROL:
+		val |= offset
 		val |= usbEpControlEndpointTypeControl
 		_usbDPSRAM.EPxBufferControl[ep].Out.Set(usbBuf0CtrlData1Pid)
 		_usbDPSRAM.EPxBufferControl[ep].Out.SetBits(usbBuf0CtrlAvail)
@@ -79,7 +97,7 @@ func sendUSBPacket(ep uint32, data []byte) {
 		} else {
 			sendOnEP0DATADONE.offset = 0
 		}
-		epXdata0[ep] = true
+		epXdata0In[ep] = true
 	}
 
 	sendViaEPIn(ep, data, count)
@@ -116,19 +134,29 @@ func handleEndpointRx(ep uint32) []byte {
 // AckUsbOutTransfer is called to acknowledge the completion of a USB OUT transfer.
 func AckUsbOutTransfer(ep uint32) {
 	ep = ep & 0x7F
-	setEPDataPID(ep, !epXdata0[ep])
+	setEPDataPIDOut(ep, !epXdata0Out[ep])
 }
 
-// Set the USB endpoint Packet ID to DATA0 or DATA1.
-func setEPDataPID(ep uint32, dataOne bool) {
-	epXdata0[ep] = dataOne
-	if epXdata0[ep] || ep == 0 {
+// Set the USB endpoint Packet ID to DATA0 or DATA1 for OUT direction.
+func setEPDataPIDOut(ep uint32, dataOne bool) {
+	epXdata0Out[ep] = dataOne
+	if epXdata0Out[ep] || ep == 0 {
 		_usbDPSRAM.EPxBufferControl[ep].Out.SetBits(usbBuf0CtrlData1Pid)
 	}
 
 	_usbDPSRAM.EPxBufferControl[ep].Out.SetBits(usbBuf0CtrlAvail)
 }
 
+// Set the USB endpoint Packet ID to DATA0 or DATA1 for IN direction.
+func setEPDataPIDIn(ep uint32, dataOne bool) {
+	epXdata0In[ep] = dataOne
+	if epXdata0In[ep] || ep == 0 {
+		_usbDPSRAM.EPxBufferControl[ep].In.SetBits(usbBuf0CtrlData1Pid)
+	}
+
+	_usbDPSRAM.EPxBufferControl[ep].In.SetBits(usbBuf0CtrlAvail)
+}
+
 func SendZlp() {
 	sendUSBPacket(0, nil)
 }
@@ -138,15 +166,19 @@ func sendViaEPIn(ep uint32, data []byte, count int) {
 	val := uint32(count) | usbBuf0CtrlAvail
 
 	// DATA0 or DATA1
-	epXdata0[ep&0x7F] = !epXdata0[ep&0x7F]
-	if !epXdata0[ep&0x7F] {
+	epXdata0In[ep&0x7F] = !epXdata0In[ep&0x7F]
+	if !epXdata0In[ep&0x7F] {
 		val |= usbBuf0CtrlData1Pid
 	}
 
 	// Mark as full
 	val |= usbBuf0CtrlFull
 
-	copy(_usbDPSRAM.EPxBuffer[ep&0x7F].Buffer0[:], data[:count])
+	if (ep & 0x7F) == 0 {
+		copy(_usbDPSRAM.EPxBuffer[0].Buffer0[:], data[:count])
+	} else {
+		copy(_usbDPSRAM.EPxBuffer[ep&0x7F].Buffer1[:], data[:count])
+	}
 	_usbDPSRAM.EPxBufferControl[ep&0x7F].In.Set(val)
 }
 
@@ -178,9 +210,9 @@ func (dev *USBDevice) ClearStallEPIn(ep uint32) {
 	ep = ep & 0x7F
 	val := uint32(usbBuf0CtrlStall)
 	_usbDPSRAM.EPxBufferControl[ep].In.ClearBits(val)
-	if epXPIDReset[ep] {
+	if epXPIDResetIn[ep] {
 		// Reset the PID to DATA0
-		setEPDataPID(ep, false)
+		setEPDataPIDIn(ep, false)
 	}
 }
 
@@ -189,9 +221,9 @@ func (dev *USBDevice) ClearStallEPOut(ep uint32) {
 	ep = ep & 0x7F
 	val := uint32(usbBuf0CtrlStall)
 	_usbDPSRAM.EPxBufferControl[ep].Out.ClearBits(val)
-	if epXPIDReset[ep] {
+	if epXPIDResetOut[ep] {
 		// Reset the PID to DATA0
-		setEPDataPID(ep, false)
+		setEPDataPIDOut(ep, false)
 	}
 }
 
@@ -219,10 +251,12 @@ type usbBuffer struct {
 }
 
 var (
-	_usbDPSRAM  = (*usbDPSRAM)(unsafe.Pointer(uintptr(0x50100000)))
-	epXdata0    [16]bool
-	epXPIDReset [16]bool
-	setupBytes  [8]byte
+	_usbDPSRAM     = (*usbDPSRAM)(unsafe.Pointer(uintptr(0x50100000)))
+	epXdata0In     [16]bool
+	epXdata0Out    [16]bool
+	epXPIDResetIn  [16]bool
+	epXPIDResetOut [16]bool
+	setupBytes     [8]byte
 )
 
 func (d *usbDPSRAM) setupBytes() []byte {
diff --git a/src/machine/usb/cdc/cdc.go b/src/machine/usb/cdc/cdc.go
index 44bd7f7e0..78b9c74e9 100644
--- a/src/machine/usb/cdc/cdc.go
+++ b/src/machine/usb/cdc/cdc.go
@@ -2,10 +2,14 @@
 
 package cdc
 
+import (
+	"machine/usb"
+)
+
 const (
-	cdcEndpointACM = 1
-	cdcEndpointOut = 2
-	cdcEndpointIn  = 3
+	cdcEndpointACM = usb.CDC_ENDPOINT_ACM
+	cdcEndpointOut = usb.CDC_ENDPOINT_OUT
+	cdcEndpointIn  = usb.CDC_ENDPOINT_IN
 )
 
 // New returns USBCDC struct.
diff --git a/src/machine/usb/msc/msc.go b/src/machine/usb/msc/msc.go
index 420a06ed9..cc46dc669 100644
--- a/src/machine/usb/msc/msc.go
+++ b/src/machine/usb/msc/msc.go
@@ -166,6 +166,7 @@ func (m *msc) sendCSW(status csw.Status) {
 	}
 	m.cbw.CSW(status, residue, m.cswBuf)
 	m.state = mscStateStatusSent
+	m.queuedBytes = csw.MsgLen
 	m.sendUSBPacket(m.cswBuf)
 }
 
@@ -235,9 +236,9 @@ func (m *msc) run(b []byte, isEpOut bool) bool {
 			// 6.6.1 CBW Not Valid
 			// https://usb.org/sites/default/files/usbmassbulk_10.pdf
 			m.state = mscStateNeedReset
-			m.stallEndpoint(usb.MSC_ENDPOINT_IN)
-			m.stallEndpoint(usb.MSC_ENDPOINT_OUT)
-			m.stallEndpoint(usb.CONTROL_ENDPOINT)
+			m.stallEndpointIn(usb.MSC_ENDPOINT_IN)
+			m.stallEndpointOut(usb.MSC_ENDPOINT_OUT)
+			m.stallEndpointIn(usb.CONTROL_ENDPOINT)
 			return ack
 		}
 
@@ -284,7 +285,7 @@ func (m *msc) run(b []byte, isEpOut bool) bool {
 	if m.state == mscStateStatus && !m.txStalled {
 		if m.cbw.transferLength() > m.sentBytes && m.cbw.isIn() {
 			// 6.7.2 The Thirteen Cases - Case 5 (Hi > Di): STALL before status
-			m.stallEndpoint(usb.MSC_ENDPOINT_IN)
+			m.stallEndpointIn(usb.MSC_ENDPOINT_IN)
 		} else if m.sendZLP {
 			// Send a zero-length packet to force the end of the transfer before we send a CSW
 			m.queuedBytes = 0
diff --git a/src/machine/usb/msc/scsi.go b/src/machine/usb/msc/scsi.go
index 4cec23e2f..562a718ea 100644
--- a/src/machine/usb/msc/scsi.go
+++ b/src/machine/usb/msc/scsi.go
@@ -298,9 +298,9 @@ func (m *msc) sendScsiError(status csw.Status, key scsi.Sense, code scsi.SenseCo
 
 	if expected > 0 && residue > 0 {
 		if m.cbw.isIn() {
-			m.stallEndpoint(usb.MSC_ENDPOINT_IN)
+			m.stallEndpointIn(usb.MSC_ENDPOINT_IN)
 		} else {
-			m.stallEndpoint(usb.MSC_ENDPOINT_OUT)
+			m.stallEndpointOut(usb.MSC_ENDPOINT_OUT)
 		}
 	}
 }
diff --git a/src/machine/usb/msc/setup.go b/src/machine/usb/msc/setup.go
index 00507aac6..fb4aaf2c2 100644
--- a/src/machine/usb/msc/setup.go
+++ b/src/machine/usb/msc/setup.go
@@ -52,32 +52,30 @@ func (m *msc) handleClearFeature(setup usb.Setup, wValue uint16) bool {
 	// (c) a Clear Feature HALT to the Bulk-Out endpoint (clear stall OUT)
 	// https://usb.org/sites/default/files/usbmassbulk_10.pdf
 	if m.state == mscStateNeedReset {
-		wIndex := setup.WIndex & 0x7F // Clear the direction bit from the endpoint address for comparison
+		wIndex := uint8(setup.WIndex & 0x7F)
 		if wIndex == usb.MSC_ENDPOINT_IN {
-			m.stallEndpoint(usb.MSC_ENDPOINT_IN)
-		} else if wIndex == usb.MSC_ENDPOINT_OUT {
-			m.stallEndpoint(usb.MSC_ENDPOINT_OUT)
+			if (setup.WIndex & 0x80) != 0 {
+				m.stallEndpointIn(wIndex)
+			} else {
+				m.stallEndpointOut(wIndex)
+			}
 		}
 		machine.SendZlp()
 		return true
 	}
 
-	// Clear the direction bit from the endpoint address for comparison
-	wIndex := setup.WIndex & 0x7F
-
-	// Clear the IN/OUT stalls if addressed to the endpoint
+	wIndex := uint8(setup.WIndex & 0x7F)
 	if wIndex == usb.MSC_ENDPOINT_IN {
-		m.clearStallEndpoint(usb.MSC_ENDPOINT_IN)
-		ok = true
-	}
-	if wIndex == usb.MSC_ENDPOINT_OUT {
-		m.clearStallEndpoint(usb.MSC_ENDPOINT_OUT)
-		ok = true
-	}
-	// Send a CSW if needed to resume after the IN endpoint stall is cleared
-	if m.state == mscStateStatus && wIndex == usb.MSC_ENDPOINT_IN {
-		m.sendCSW(m.respStatus)
-		ok = true
+		if (setup.WIndex & 0x80) != 0 {
+			m.clearStallEndpointIn(wIndex)
+			ok = true
+			if m.state == mscStateStatus {
+				m.sendCSW(m.respStatus)
+			}
+		} else {
+			m.clearStallEndpointOut(wIndex)
+			ok = true
+		}
 	}
 
 	if ok {
@@ -120,26 +118,28 @@ func (m *msc) handleReset(setup usb.Setup, wValue uint16) bool {
 	return true
 }
 
-func (m *msc) stallEndpoint(ep uint8) {
-	if ep == usb.MSC_ENDPOINT_IN {
-		m.txStalled = true
-		machine.USBDev.SetStallEPIn(usb.MSC_ENDPOINT_IN)
-	} else if ep == usb.MSC_ENDPOINT_OUT {
-		m.rxStalled = true
-		machine.USBDev.SetStallEPOut(usb.MSC_ENDPOINT_OUT)
-	} else if ep == usb.CONTROL_ENDPOINT {
+func (m *msc) stallEndpointIn(ep uint8) {
+	if ep == usb.CONTROL_ENDPOINT {
 		machine.USBDev.SetStallEPIn(usb.CONTROL_ENDPOINT)
+		return
 	}
+	m.txStalled = true
+	machine.USBDev.SetStallEPIn(uint32(ep))
 }
 
-func (m *msc) clearStallEndpoint(ep uint8) {
-	if ep == usb.MSC_ENDPOINT_IN {
-		machine.USBDev.ClearStallEPIn(usb.MSC_ENDPOINT_IN)
-		m.txStalled = false
-	} else if ep == usb.MSC_ENDPOINT_OUT {
-		machine.USBDev.ClearStallEPOut(usb.MSC_ENDPOINT_OUT)
-		m.rxStalled = false
-	}
+func (m *msc) stallEndpointOut(ep uint8) {
+	m.rxStalled = true
+	machine.USBDev.SetStallEPOut(uint32(ep))
+}
+
+func (m *msc) clearStallEndpointIn(ep uint8) {
+	machine.USBDev.ClearStallEPIn(uint32(ep))
+	m.txStalled = false
+}
+
+func (m *msc) clearStallEndpointOut(ep uint8) {
+	machine.USBDev.ClearStallEPOut(uint32(ep))
+	m.rxStalled = false
 }
 
 func (m *msc) setStringField(field []byte, value string) {