feat: add discard/erase block support

Author: mikesmitty

src/machine/machine_rp2350_usb.go

@@ -142,6 +142,14 @@ func initEndpoint(ep, config uint32) {
 	offset := ep*2*usbBufferLen + 0x100
 	val |= offset
 
+	// Bulk and interrupt endpoints must have their Packet ID reset to DATA0 when un-stalled
+	if config&(usb.ENDPOINT_TYPE_BULK|usb.ENDPOINT_TYPE_INTERRUPT) != 0 {
+		epXPIDReset[ep] = true
+	} else {
+		// In case we're reconfiguring an endpoint, change the PID reset behavior
+		epXPIDReset[ep] = false
+	}
+
 	switch config {
 	case usb.ENDPOINT_TYPE_INTERRUPT | usb.EndpointIn:
 		val |= usbEpControlEndpointTypeInterrupt
@@ -248,11 +256,11 @@ func handleEndpointRx(ep uint32) []byte {
 }
 
 func handleEndpointRxComplete(ep uint32) {
-	SetEPDataPID(ep, !epXdata0[ep])
+	setEPDataPID(ep, !epXdata0[ep])
 }
 
 // Set the USB endpoint Packet ID to DATA0 or DATA1
-func SetEPDataPID(ep uint32, dataOne bool) {
+func setEPDataPID(ep uint32, dataOne bool) {
 	epXdata0[ep&0x7F] = dataOne
 	if epXdata0[ep] || ep == 0 {
 		_usbDPSRAM.EPxBufferControl[ep].Out.SetBits(usbBuf0CtrlData1Pid)
@@ -307,12 +315,20 @@ func SetStallEPOut(ep uint32) {
 func ClearStallEPIn(ep uint32) {
 	val := uint32(usbBuf0CtrlStall)
 	_usbDPSRAM.EPxBufferControl[ep&0x7F].In.ClearBits(val)
+	if epXPIDReset[ep] {
+		// Reset the PID to DATA0
+		setEPDataPID(ep, false)
+	}
 }
 
 // Clear the endpoint stall bit on a USB OUT endpoint
 func ClearStallEPOut(ep uint32) {
 	val := uint32(usbBuf0CtrlStall)
 	_usbDPSRAM.EPxBufferControl[ep&0x7F].Out.ClearBits(val)
+	if epXPIDReset[ep] {
+		// Reset the PID to DATA0
+		setEPDataPID(ep, false)
+	}
 }
 
 type usbDPSRAM struct {
@@ -339,9 +355,10 @@ type usbBuffer struct {
 }
 
 var (
-	_usbDPSRAM = (*usbDPSRAM)(unsafe.Pointer(uintptr(0x50100000)))
-	epXdata0   [16]bool
-	setupBytes [8]byte
+	_usbDPSRAM  = (*usbDPSRAM)(unsafe.Pointer(uintptr(0x50100000)))
+	epXdata0    [16]bool
+	epXPIDReset [16]bool
+	setupBytes  [8]byte
 )
 
 func (d *usbDPSRAM) setupBytes() []byte {

src/machine/usb.go

@@ -123,6 +123,7 @@ var (
 	usbTxHandler    [usb.NumberOfEndpoints]func()
 	usbRxHandler    [usb.NumberOfEndpoints]func([]byte)
 	usbSetupHandler [usb.NumberOfInterfaces]func(usb.Setup) bool
+	usbStallHandler [usb.NumberOfEndpoints]func(usb.Setup) bool
 
 	endPoints = []uint32{
 		usb.CONTROL_ENDPOINT:  usb.ENDPOINT_TYPE_CONTROL,
@@ -214,7 +215,11 @@ func handleStandardSetup(setup usb.Setup) bool {
 		if setup.WValueL == 1 { // DEVICEREMOTEWAKEUP
 			isRemoteWakeUpEnabled = false
 		} else if setup.WValueL == 0 { // ENDPOINTHALT
-			isEndpointHalt = false
+			if usbStallHandler[setup.WIndex&0x7F] != nil {
+				return usbStallHandler[setup.WIndex&0x7F](setup)
+			} else {
+				isEndpointHalt = false
+			}
 		}
 		SendZlp()
 		return true
@@ -223,7 +228,11 @@ func handleStandardSetup(setup usb.Setup) bool {
 		if setup.WValueL == 1 { // DEVICEREMOTEWAKEUP
 			isRemoteWakeUpEnabled = true
 		} else if setup.WValueL == 0 { // ENDPOINTHALT
-			isEndpointHalt = true
+			if usbStallHandler[setup.WIndex&0x7F] != nil {
+				return usbStallHandler[setup.WIndex&0x7F](setup)
+			} else {
+				isEndpointHalt = true
+			}
 		}
 		SendZlp()
 		return true
@@ -322,6 +331,9 @@ func ConfigureUSBEndpoint(desc descriptor.Descriptor, epSettings []usb.EndpointC
 				usbRxHandler[ep.Index] = ep.RxHandler
 			}
 		}
+		if ep.StallHandler != nil {
+			usbStallHandler[ep.Index] = ep.StallHandler
+		}
 	}
 
 	for _, s := range setup {

src/machine/usb/config.go

@@ -1,11 +1,12 @@
 package usb
 
 type EndpointConfig struct {
-	Index     uint8
-	IsIn      bool
-	TxHandler func()
-	RxHandler func([]byte)
-	Type      uint8
+	Index        uint8
+	IsIn         bool
+	TxHandler    func()
+	RxHandler    func([]byte)
+	StallHandler func(Setup) bool
+	Type         uint8
 }
 
 type SetupConfig struct {

src/machine/usb/descriptor/endpoint.go

@@ -150,3 +150,7 @@ func (d EndpointType) MaxPacketSize(v uint16) {
 func (d EndpointType) Interval(v uint8) {
 	d.data[6] = byte(v)
 }
+
+func (d EndpointType) GetMaxPacketSize() uint16 {
+	return binary.LittleEndian.Uint16(d.data[4:6])
+}

src/machine/usb/msc/cbw.go

@@ -1,12 +1,14 @@
 package msc
 
 import (
+	"encoding/binary"
 	"machine/usb/msc/csw"
 	"machine/usb/msc/scsi"
 )
 
 const (
-	cbwMsgLen = 31 // Command Block Wrapper (CBW) message length
+	cbwMsgLen = 31         // Command Block Wrapper (CBW) message length
+	Signature = 0x43425355 // "USBC" in little endian
 )
 
 type CBW struct {
@@ -22,15 +24,15 @@ func (c *CBW) validLength() bool {
 }
 
 func (c *CBW) validSignature() bool {
-	return c.Data[0] == 0x55 && c.Data[1] == 0x53 && c.Data[2] == 0x42 && c.Data[3] == 0x43
+	return binary.LittleEndian.Uint32(c.Data[:4]) == Signature
 }
 
 func (c *CBW) scsiCmd() scsi.Cmd {
 	return scsi.Cmd{Data: c.Data[15:]}
 }
 
 func (c *CBW) transferLength() uint32 {
-	return uint32(c.Data[8]) | uint32(c.Data[9])<<8 | uint32(c.Data[10])<<16 | uint32(c.Data[11])<<24
+	return binary.LittleEndian.Uint32(c.Data[8:12])
 }
 
 // isIn returns true if the command direction is from the device to the host.
@@ -44,21 +46,12 @@ func (c *CBW) isOut() bool {
 }
 
 func (c *CBW) CSW(status csw.Status, residue uint32, b []byte) {
-	// Signature: 53425355h (little endian)
-	b[3] = 0x53
-	b[2] = 0x42
-	b[1] = 0x53
-	b[0] = 0x55
-	// Tag:
-	b[4] = c.Data[4]
-	b[5] = c.Data[5]
-	b[6] = c.Data[6]
-	b[7] = c.Data[7]
+	// Signature: "USBS" 53425355h (little endian)
+	binary.LittleEndian.PutUint32(b[:4], csw.Signature)
+	// Tag: (same as CBW)
+	copy(b[4:8], c.Data[4:8])
 	// Data Residue: (untransferred bytes)
-	b[8] = byte(residue)
-	b[9] = byte(residue >> 8)
-	b[10] = byte(residue >> 16)
-	b[11] = byte(residue >> 24)
+	binary.LittleEndian.PutUint32(b[8:12], residue)
 	// Status:
 	b[12] = byte(status)
 }

src/machine/usb/msc/csw/csw.go

@@ -9,5 +9,6 @@ const (
 )
 
 const (
-	MsgLen = 13
+	MsgLen    = 13
+	Signature = 0x53425355 // "USBS" in little endian
 )

src/machine/usb/msc/disk.go

@@ -1,13 +1,11 @@
 package msc
 
-type Disk interface {
-	Ready() bool
-	ReadOnly() bool
-	BlockCount() uint32
-	BlockSize() uint32
-	Read(offset uint32, buffer []byte) (uint32, error)
-	Write(offset uint32, buffer []byte) (uint32, error)
-}
+import (
+	"encoding/binary"
+	"machine"
+)
+
+var _ machine.BlockDevice = (*DefaultDisk)(nil)
 
 // DefaultDisk is a placeholder disk implementation
 type DefaultDisk struct {
@@ -18,36 +16,58 @@ func NewDefaultDisk() *DefaultDisk {
 	return &DefaultDisk{}
 }
 
-func (d *DefaultDisk) Ready() bool {
-	return true
+func (d *DefaultDisk) Size() int64 {
+	return 4096 * int64(d.WriteBlockSize()) // 2MB
 }
 
-func (d *DefaultDisk) ReadOnly() bool {
-	return false
+func (d *DefaultDisk) WriteBlockSize() int64 {
+	return 512 // 512 bytes
 }
 
-func (d *DefaultDisk) BlockCount() uint32 {
-	return 4096 // 2MB
+func (d *DefaultDisk) EraseBlockSize() int64 {
+	return 2048 // 4 blocks of 512 bytes
 }
 
-func (d *DefaultDisk) BlockSize() uint32 {
-	return 512 // 512 bytes
+func (d *DefaultDisk) EraseBlocks(startBlock, numBlocks int64) error {
+	return nil
 }
 
-func (d *DefaultDisk) Read(offset uint32, buffer []byte) (uint32, error) {
+func (d *DefaultDisk) ReadAt(buffer []byte, offset int64) (int, error) {
 	n := uint8(offset)
 	for i := range buffer {
 		n++
 		buffer[i] = n
 	}
-	return uint32(len(buffer)), nil
+	return len(buffer), nil
 }
 
-func (d *DefaultDisk) Write(offset uint32, buffer []byte) (uint32, error) {
-	return uint32(len(buffer)), nil
+func (d *DefaultDisk) WriteAt(buffer []byte, offset int64) (int, error) {
+	return len(buffer), nil
 }
 
-// RegisterDisk registers a disk provider with the MSC driver
-func (m *msc) RegisterDisk(disk Disk) {
-	m.disk = disk
+// RegisterBlockDevice registers a BlockDevice provider with the MSC driver
+func (m *msc) RegisterBlockDevice(dev machine.BlockDevice) {
+	m.dev = dev
+
+	// Set VPD UNMAP fields
+	for i := range vpdPages {
+		if vpdPages[i].PageCode == 0xb0 {
+			// 0xb0 - 5.4.5 Block Limits VPD page (B0h)
+			if len(vpdPages[i].Data) >= 28 {
+				// Set the OPTIMAL UNMAP GRANULARITY (write blocks per erase block)
+				granularity := uint32(dev.EraseBlockSize()) / uint32(dev.WriteBlockSize())
+				binary.BigEndian.PutUint32(vpdPages[i].Data[24:28], granularity)
+			}
+			/* TODO: Add method for working out the optimal unmap granularity alignment
+			if len(vpdPages[i].Data) >= 32 {
+				// Set the UNMAP GRANULARITY ALIGNMENT (first sector of first full erase block)
+				// The unmap granularity alignment is used to calculate an optimal unmap request starting LBA as follows:
+				// optimal unmap request starting LBA = (n * OPTIMAL UNMAP GRANULARITY) + UNMAP GRANULARITY ALIGNMENT
+				// where n is zero or any positive integer value
+				// https://www.seagate.com/files/staticfiles/support/docs/manual/Interface%20manuals/100293068j.pdf
+			}
+			*/
+			break
+		}
+	}
 }

src/machine/usb/msc/fakefs/fakefs.go

@@ -0,0 +1,83 @@
+package fakefs
+
+type MockFS interface {
+	ReadAt(p []byte, off int64) (n int, err error)
+	WriteAt(p []byte, off int64) (n int, err error)
+	ReadOnly() bool
+	/* FIXME: Cleanup?
+	SectorCount() uint32
+	SectorSize() uint16
+	*/
+}
+
+type FakeFS struct {
+	fs    MockFS
+	ready bool
+}
+
+func NewFakeFS() *FakeFS {
+	// FIXME: Initialize the fake FAT12 file system
+	return &FakeFS{fs: NewFAT12(), ready: true}
+}
+
+func (fs *FakeFS) TestUnitReady() bool {
+	// Check if the disk is ready
+	return fs.ready
+}
+
+func (fs *FakeFS) SetReady(ready bool) {
+	// Set the disk ready state
+	fs.ready = ready
+}
+
+func (fs *FakeFS) Read(offset uint32, buffer []byte) (uint32, error) {
+	return uint32(len(buffer)), nil // FIXME: Cleanup
+	// Read data from the file system
+	n, err := fs.fs.ReadAt(buffer, int64(offset))
+	if err != nil {
+		return 0, err
+	}
+	return uint32(n), nil
+}
+
+func (fs *FakeFS) Write(offset uint32, buffer []byte) (uint32, error) {
+	return uint32(len(buffer)), nil // FIXME: Cleanup
+	// Write data to the file system
+	n, err := fs.fs.WriteAt(buffer, int64(offset))
+	if err != nil {
+		return 0, err
+	}
+	return uint32(n), nil
+}
+
+/* FIXME: Cleanup
+func (fs *FakeFS) ReadCapacity() (uint32, uint32) {
+	// Return the total number of blocks and block size
+	return 0, 0 // FIXME: Implement finding actual values
+}
+
+func (fs *FakeFS) ReadFormatCapacity() (uint32, uint32) {
+	// Return the total number of blocks and block size
+	return 0, 0 // FIXME: Implement finding actual values
+}
+
+func (fs *FakeFS) Inquiry() ([]byte, error) {
+	// Return the inquiry data
+	return nil, nil // FIXME: Implement finding actual values
+}
+*/
+
+func (fs *FakeFS) ReadOnly() bool {
+	// If the file system is read-only, return true to indicate the hardware is read-only also
+	return fs.fs.ReadOnly()
+}
+
+func (fs *FakeFS) BlockCount() uint32 {
+	// Return the total number of blocks
+	return 4096 // FIXME: Implement finding actual values
+}
+
+func (fs *FakeFS) BlockSize() uint32 {
+	// Return the block size
+	return 512 // FIXME: Implement finding actual values
+}