[SeaBIOS] [PATCH] USB MSC: send START/STOP UNIT command before accessing device
Sven Schnelle
svens at stackframe.org
Sun Oct 16 11:57:27 CEST 2011
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
More information about the SeaBIOS
mailing list