[SeaBIOS] [PATCH 06/16] usb-msc: support WRITE commands

[Paolo Bonzini]

Writes only require building the CDB and some care with the direction
in the USB packet.

Signed-off-by: Paolo Bonzini <pbonzini at redhat.com>
---
 src/blockcmd.c |   12 ++++++++++++
 src/blockcmd.h |    1 +
 src/usb-msc.c  |   15 ++++++++-------
 3 files changed, 21 insertions(+), 7 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 5e38b01..65b33a2 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,9 +87,9 @@ usb_cmd_data(struct disk_op_s *op, void *cdbcmd, u16 blocksize)
     if (ret)
         goto fail;
 
-    // Transfer data from device.
+    // Transfer data to/from device.
     if (bytes) {
-        ret = usb_msc_send(udrive_g, USB_DIR_IN, op->buf_fl, bytes);
+        ret = usb_msc_send(udrive_g, cbw.bmCBWFlags, op->buf_fl, bytes);
         if (ret)
             goto fail;
     }
@@ -106,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:
@@ -130,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:
-- 
1.7.7.1