feat: add discard/erase block support

Author: mikesmitty

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
+}

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

@@ -0,0 +1,67 @@
+package fakefs
+
+/* READ10 Callback - pico-littlefs-usb
+int32_t tud_msc_read10_cb(uint8_t lun, uint32_t lba, uint32_t offset, void* buffer, uint32_t bufsize) {
+    (void)lun;
+    (void)offset;
+
+    if (!is_initialized) {
+        mimic_fat_init(&lfs_pico_flash_config);
+        mimic_fat_update_usb_device_is_enabled(true);
+        mimic_fat_create_cache();
+        is_initialized = true;
+    }
+    mimic_fat_read(lun, lba, buffer, bufsize);
+
+    return (int32_t)bufsize;
+*/
+
+type FAT12 struct {
+	readonly bool
+}
+
+func NewFAT12() *FAT12 {
+	return &FAT12{}
+}
+
+func (f *FAT12) ReadOnly() bool {
+	return f.readonly
+}
+
+func (f *FAT12) ReadAt(p []byte, off int64) (n int, err error) {
+	// FIXME: Implement this function
+	/* ReadAt reads len(p) bytes into p starting at offset off in the underlying input source. It returns the number of bytes read (0 <= n <= len(p)) and any error encountered.
+	When ReadAt returns n < len(p), it returns a non-nil error explaining why more bytes were not returned. In this respect, ReadAt is stricter than Read.
+	Even if ReadAt returns n < len(p), it may use all of p as scratch space during the call. If some data is available but not len(p) bytes, ReadAt blocks until either all the data is available or an error occurs. In this respect ReadAt is different from Read.
+	If the n = len(p) bytes returned by ReadAt are at the end of the input source, ReadAt may return either err == EOF or err == nil.
+	If ReadAt is reading from an input source with a seek offset, ReadAt should not affect nor be affected by the underlying seek offset.
+	Clients of ReadAt can execute parallel ReadAt calls on the same input source.
+	Implementations must not retain p.
+	*/
+
+	return 0, nil
+}
+
+func (f *FAT12) WriteAt(p []byte, off int64) (n int, err error) {
+	// FIXME: Implement this function
+	/* WriteAt writes len(p) bytes from p to the underlying data stream at offset off. It returns the number of bytes written from p (0 <= n <= len(p)) and any error encountered that caused the write to stop early. WriteAt must return a non-nil error if it returns n < len(p).
+	If WriteAt is writing to a destination with a seek offset, WriteAt should not affect nor be affected by the underlying seek offset.
+	Clients of WriteAt can execute parallel WriteAt calls on the same destination if the ranges do not overlap.
+	Implementations must not retain p.
+	*/
+
+	return 0, nil
+}
+
+/* FIXME: Cleanup?
+func (f *FAT12) SectorCount() uint32 {
+	// FIXME: Implement this:
+	// uint64_t storage_size = littlefs_lfs_config->block_count * littlefs_lfs_config->block_size;
+	// return (double)storage_size / DISK_SECTOR_SIZE;
+	return f.sectors
+}
+
+func (f *FAT12) SectorSize() uint16 {
+	return 512
+}
+*/