[SeaBIOS] [RFC PATCH 04/16] usb-msc: support WRITE and TEST UNIT READY

[Paolo Bonzini]

Writes only require building the CDB and some care with the direction
in the USB packet.  TEST UNIT READY is special because it has no data,
so it cannot have any residue either.

Signed-off-by: Paolo Bonzini 
---
 src/blockcmd.c |   12 ++++++++++++
 src/blockcmd.h |    1 +
 src/usb-msc.c  |   21 ++++++++++++---------
 3 files changed, 25 insertions(+), 9 deletions(-)

diff --git a/src/blockcmd.c b/src/blockcmd.c
index c127729..f5e2ce3 100644
--- a/src/blockcmd.c
+++ b/src/blockcmd.c
@@ -91,3 +91,15 @@ cdb_read(struct disk_op_s *op)
     cmd.count = htons(op->count);
     return cdb_cmd_data(op, &cmd, GET_GLOBAL(op->drive_g->blksize));
 }
+
+// Write sectors.
+int
+cdb_write(struct disk_op_s *op)
+{
+    struct cdb_rwdata_10 cmd;
+    memset(&cmd, 0, sizeof(cmd));
+    cmd.command = CDB_CMD_WRITE_10;
+    cmd.lba = htonl(op->lba);
+    cmd.count = htons(op->count);
+    return cdb_cmd_data(op, &cmd, GET_GLOBAL(op->drive_g->blksize));
+}
diff --git a/src/blockcmd.h b/src/blockcmd.h
index 49921b0..84ab659 100644
--- a/src/blockcmd.h
+++ b/src/blockcmd.h
@@ -75,5 +75,6 @@ int cdb_test_unit_ready(struct disk_op_s *op);
 int cdb_read_capacity(struct disk_op_s *op, struct cdbres_read_capacity *data);
 int cdb_inquiry(struct disk_op_s *op, struct cdbres_inquiry *data);
 int cdb_read(struct disk_op_s *op);
+int cdb_write(struct disk_op_s *op);

#endif // blockcmd.h
diff --git a/src/usb-msc.c b/src/usb-msc.c
index 07a7ea2..c8b9fbe 100644
--- a/src/usb-msc.c
+++ b/src/usb-msc.c
@@ -73,13 +73,13 @@ usb_cmd_data(struct disk_op_s *op, void *cdbcmd, u16 blocksize)
     u32 bytes = blocksize * op->count;
     struct cbw_s cbw;
     memset(&cbw, 0, sizeof(cbw));
+    memcpy(cbw.CBWCB, cdbcmd, USB_CDB_SIZE);
     cbw.dCBWSignature = CBW_SIGNATURE;
     cbw.dCBWTag = 999; // XXX
     cbw.dCBWDataTransferLength = bytes;
-    cbw.bmCBWFlags = USB_DIR_IN; // XXX
+    cbw.bmCBWFlags = (cbw.CBWCB[0] == CDB_CMD_WRITE_10) ? USB_DIR_OUT : USB_DIR_IN;
     cbw.bCBWLUN = 0; // XXX
     cbw.bCBWCBLength = USB_CDB_SIZE;
-    memcpy(cbw.CBWCB, cdbcmd, USB_CDB_SIZE);

// Transfer cbw to device.
     int ret = usb_msc_send(udrive_g, USB_DIR_OUT
@@ -87,10 +87,12 @@ usb_cmd_data(struct disk_op_s *op, void *cdbcmd, u16 blocksize)
     if (ret)
         goto fail;

-    // Transfer data from device.
-    ret = usb_msc_send(udrive_g, USB_DIR_IN, op->buf_fl, bytes);
-    if (ret)
-        goto fail;
+    // Transfer data to/from device.
+    if (op->count) {
+        ret = usb_msc_send(udrive_g, cbw.bmCBWFlags, op->buf_fl, bytes);
+        if (ret)
+            goto fail;
+    }

// Transfer csw info.
     struct csw_s csw;
@@ -104,7 +106,8 @@ usb_cmd_data(struct disk_op_s *op, void *cdbcmd, u16 blocksize)
     if (csw.bCSWStatus == 2)
         goto fail;

-    op->count -= csw.dCSWDataResidue / blocksize;
+    if (csw.dCSWDataResidue)
+        op->count -= csw.dCSWDataResidue / blocksize;
     return DISK_RET_EBADTRACK;

fail:
@@ -128,9 +131,9 @@ process_usb_op(struct disk_op_s *op)
     switch (op->command) {
     case CMD_READ:
         return cdb_read(op);
-    case CMD_FORMAT:
     case CMD_WRITE:
-        return DISK_RET_EWRITEPROTECT;
+        return cdb_write(op);
+    case CMD_FORMAT:
     case CMD_RESET:
     case CMD_ISREADY:
     case CMD_VERIFY: