[SeaBIOS] [PATCH] USB MSC: send START/STOP UNIT command before accessing device

[Sven Schnelle]

Some USB Mass storage devices need this command before being
accessed. request sense in error case and retry the last command
in case it says 'Unit attention'

Signed-off-by: Sven Schnelle <svens at stackframe.org>
---
 src/blockcmd.c |   12 ++++++++++++
 src/blockcmd.h |   11 +++++++++++
 src/usb-msc.c  |   44 ++++++++++++++++++++++++++++++++++----------
 3 files changed, 57 insertions(+), 10 deletions(-)

diff --git a/src/blockcmd.c b/src/blockcmd.c
index c9c6845..571f7cf 100644
--- a/src/blockcmd.c
+++ b/src/blockcmd.c
@@ -56,6 +56,18 @@ cdb_get_sense(struct disk_op_s *op, struct cdbres_request_sense *data)
     return cdb_cmd_data(op, &cmd, sizeof(*data));
 }
 
+int
+cdb_start_stop_unit(struct disk_op_s *op, int stop, int start)
+{
+    struct cdb_start_stop_unit cmd;
+    memset(&cmd, 0, sizeof(cmd));
+    cmd.command = CDB_CMD_START_STOP_UNIT;
+    cmd.loj_start = ((stop & 1) << 1) | (start & 1);
+    op->buf_fl = NULL;
+    op->count = 0;
+    return cdb_cmd_data(op, &cmd, 0);
+}
+
 // Request capacity
 int
 cdb_read_capacity(struct disk_op_s *op, struct cdbres_read_capacity *data)
diff --git a/src/blockcmd.h b/src/blockcmd.h
index 903c435..ebc1161 100644
--- a/src/blockcmd.h
+++ b/src/blockcmd.h
@@ -34,6 +34,7 @@ struct cdbres_read_capacity {
 
 #define CDB_CMD_INQUIRY 0x12
 #define CDB_CMD_REQUEST_SENSE 0x03
+#define CDB_CMD_START_STOP_UNIT 0x1b
 
 struct cdb_request_sense {
     u8 command;
@@ -44,6 +45,16 @@ struct cdb_request_sense {
     u8 pad[10];
 } PACKED;
 
+struct cdb_start_stop_unit {
+    u8 command;
+    u8 lun;
+    u8 reserved_02;
+    u8 reserved_03;
+    u8 loj_start;
+    u8 control;
+    u8 pad[10];
+} PACKED;
+
 struct cdbres_request_sense {
     u8 errcode;
     u8 segment;
diff --git a/src/usb-msc.c b/src/usb-msc.c
index 13ef93e..deccf35 100644
--- a/src/usb-msc.c
+++ b/src/usb-msc.c
@@ -53,12 +53,14 @@ usb_cmd_data(struct disk_op_s *op, void *cdbcmd, u16 blocksize)
     if (!CONFIG_USB_MSC)
         return 0;
 
+restart:
     dprintf(16, "usb_cmd_data id=%p write=%d count=%d bs=%d buf=%p\n"
             , op->drive_g, 0, op->count, blocksize, op->buf_fl);
     struct usbdrive_s *udrive_g = container_of(
         op->drive_g, struct usbdrive_s, drive);
     struct usb_pipe *bulkin = GET_GLOBAL(udrive_g->bulkin);
     struct usb_pipe *bulkout = GET_GLOBAL(udrive_g->bulkout);
+    struct cdbres_request_sense sense;
 
     // Setup command block wrapper.
     u32 bytes = blocksize * op->count;
@@ -78,24 +80,42 @@ usb_cmd_data(struct disk_op_s *op, void *cdbcmd, u16 blocksize)
     if (ret)
         goto fail;
 
-    // Transfer data from device.
-    ret = usb_send_bulk(bulkin, USB_DIR_IN, op->buf_fl, bytes);
-    if (ret)
-        goto fail;
-
+    if (blocksize) {
+	    // Transfer data from device.
+	    ret = usb_send_bulk(bulkin, USB_DIR_IN, op->buf_fl, bytes);
+	    if (ret)
+		    goto fail;
+    }
     // Transfer csw info.
     struct csw_s csw;
     ret = usb_send_bulk(bulkin, USB_DIR_IN
                         , MAKE_FLATPTR(GET_SEG(SS), &csw), sizeof(csw));
     if (ret)
-        goto fail;
+	    goto fail;
 
-    if (!csw.bCSWStatus)
+    switch(csw.bCSWStatus) {
+    case 0:
         return DISK_RET_SUCCESS;
-    if (csw.bCSWStatus == 2)
-        goto fail;
+    case 2:
+        break;
+    case 1:
+        if (!cdb_get_sense(op, &sense)) {
+            dprintf(1, "SENSE: error %x, key %x, asc %x ascq %x\n",
+                    sense.errcode, sense.flags & 0x0f, sense.asc, sense.ascq);
+
+            switch(sense.flags & 0x0f) {
+                case 6: /* Unit Attention */
+                    goto restart;
+                default:
+                    goto fail;
+            }
+        }
+        break;
+    };
 
-    op->count -= csw.dCSWDataResidue / blocksize;
+    if(blocksize)
+	    op->count -= csw.dCSWDataResidue / blocksize;
+    dprintf(1, "EBADTRACK\n");
     return DISK_RET_EBADTRACK;
 
 fail:
@@ -226,6 +246,10 @@ usb_msc_init(struct usb_pipe *pipe
     int ret = cdb_get_inquiry(&dop, &data);
     if (ret)
         goto fail;
+
+    if (cdb_start_stop_unit(&dop, 0, 1))
+        goto fail;
+
     char vendor[sizeof(data.vendor)+1], product[sizeof(data.product)+1];
     char rev[sizeof(data.rev)+1];
     strtcpy(vendor, data.vendor, sizeof(vendor));
-- 
1.7.7