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@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));