Index: uspace/drv/bus/usb/usbmast/scsi_ms.c
===================================================================
--- uspace/drv/bus/usb/usbmast/scsi_ms.c	(revision 2aceec5d8eb06e4d99d0b4a64ba26b6db7f1938f)
+++ uspace/drv/bus/usb/usbmast/scsi_ms.c	(revision de3432be3bd5cf920867c570f84e50d62887f870)
@@ -61,4 +61,23 @@
 }
 
+static void usbmast_dump_sense(usbmast_fun_t *mfun)
+{
+	scsi_sense_data_t sense_buf;
+	unsigned sense_key;
+	int rc;
+
+	rc = usbmast_request_sense(mfun, &sense_buf, sizeof(sense_buf));
+	if (rc == EOK) {
+		sense_key = sense_buf.flags_key & 0x0f;
+		printf("Got sense data. Sense key: 0x%x (%s), ASC 0x%02x, "
+		    "ASCQ 0x%02x.\n", sense_key,
+		    scsi_get_sense_key_str(sense_key),
+		    sense_buf.additional_code,
+		    sense_buf.additional_cqual);
+	} else {
+		printf("Failed to read sense data.\n");
+	}
+}
+
 /** Perform SCSI Inquiry command on USB mass storage device.
  *
@@ -70,5 +89,5 @@
 {
 	scsi_std_inquiry_data_t inq_data;
-	size_t response_len;
+	scsi_cmd_t cmd;
 	scsi_cdb_inquiry_t cdb;
 	int rc;
@@ -78,16 +97,28 @@
 	cdb.alloc_len = host2uint16_t_be(sizeof(inq_data));
 
-	rc = usb_massstor_data_in(mfun, 0xDEADBEEF, (uint8_t *) &cdb,
-	    sizeof(cdb), &inq_data, sizeof(inq_data), &response_len);
+	memset(&cmd, 0, sizeof(cmd));
+	cmd.cdb = &cdb;
+	cmd.cdb_size = sizeof(cdb);
+	cmd.data_in = &inq_data;
+	cmd.data_in_size = sizeof(inq_data);
+
+	rc = usb_massstor_cmd(mfun, 0xDEADBEEF, &cmd);
 
 	if (rc != EOK) {
-		usb_log_error("Inquiry failed, device %s: %s.\n",
-		   mfun->mdev->ddf_dev->name, str_error(rc));
-		return rc;
-	}
-
-	if (response_len < SCSI_STD_INQUIRY_DATA_MIN_SIZE) {
+		usb_log_error("Inquiry transport failed, device %s: %s.\n",
+		   mfun->mdev->ddf_dev->name, str_error(rc));
+		return rc;
+	}
+
+	if (cmd.status != CMDS_GOOD) {
+		usb_log_error("Inquiry command failed, device %s.\n",
+		   mfun->mdev->ddf_dev->name);
+		usbmast_dump_sense(mfun);
+		return EIO;
+	}
+
+	if (cmd.rcvd_size < SCSI_STD_INQUIRY_DATA_MIN_SIZE) {
 		usb_log_error("SCSI Inquiry response too short (%zu).\n",
-		    response_len);
+		    cmd.rcvd_size);
 		return EIO;
 	}
@@ -127,6 +158,6 @@
 int usbmast_request_sense(usbmast_fun_t *mfun, void *buf, size_t size)
 {
+	scsi_cmd_t cmd;
 	scsi_cdb_request_sense_t cdb;
-	size_t data_len;
 	int rc;
 
@@ -135,8 +166,13 @@
 	cdb.alloc_len = min(size, SCSI_SENSE_DATA_MAX_SIZE);
 
-	rc = usb_massstor_data_in(mfun, 0xDEADBEEF, (uint8_t *) &cdb,
-	    sizeof(cdb), buf, size, &data_len);
-
-        if (rc != EOK) {
+	memset(&cmd, 0, sizeof(cmd));
+	cmd.cdb = &cdb;
+	cmd.cdb_size = sizeof(cdb);
+	cmd.data_in = buf;
+	cmd.data_in_size = size;
+
+	rc = usb_massstor_cmd(mfun, 0xDEADBEEF, &cmd);
+
+        if (rc != EOK || cmd.status != CMDS_GOOD) {
 		usb_log_error("Request Sense failed, device %s: %s.\n",
 		   mfun->mdev->ddf_dev->name, str_error(rc));
@@ -144,8 +180,8 @@
 	}
 
-	if (data_len < SCSI_SENSE_DATA_MIN_SIZE) {
+	if (cmd.rcvd_size < SCSI_SENSE_DATA_MIN_SIZE) {
 		/* The missing bytes should be considered to be zeroes. */
-		memset((uint8_t *)buf + data_len, 0,
-		    SCSI_SENSE_DATA_MIN_SIZE - data_len);
+		memset((uint8_t *)buf + cmd.rcvd_size, 0,
+		    SCSI_SENSE_DATA_MIN_SIZE - cmd.rcvd_size);
 	}
 
@@ -164,7 +200,7 @@
     uint32_t *block_size)
 {
+	scsi_cmd_t cmd;
 	scsi_cdb_read_capacity_10_t cdb;
 	scsi_read_capacity_10_data_t data;
-	size_t data_len;
 	int rc;
 
@@ -172,16 +208,28 @@
 	cdb.op_code = SCSI_CMD_READ_CAPACITY_10;
 
-	rc = usb_massstor_data_in(mfun, 0xDEADBEEF, (uint8_t *) &cdb,
-	    sizeof(cdb), &data, sizeof(data), &data_len);
+	memset(&cmd, 0, sizeof(cmd));
+	cmd.cdb = &cdb;
+	cmd.cdb_size = sizeof(cdb);
+	cmd.data_in = &data;
+	cmd.data_in_size = sizeof(data);
+
+	rc = usb_massstor_cmd(mfun, 0xDEADBEEF, &cmd);
 
         if (rc != EOK) {
-		usb_log_error("Read Capacity (10) failed, device %s: %s.\n",
-		   mfun->mdev->ddf_dev->name, str_error(rc));
-		return rc;
-	}
-
-	if (data_len < sizeof(data)) {
+		usb_log_error("Read Capacity (10) transport failed, device %s: %s.\n",
+		   mfun->mdev->ddf_dev->name, str_error(rc));
+		return rc;
+	}
+
+	if (cmd.status != CMDS_GOOD) {
+		usb_log_error("Read Capacity (10) command failed, device %s.\n",
+		   mfun->mdev->ddf_dev->name);
+		usbmast_dump_sense(mfun);
+		return EIO;
+	}
+
+	if (cmd.rcvd_size < sizeof(data)) {
 		usb_log_error("SCSI Read Capacity response too short (%zu).\n",
-		    data_len);
+		    cmd.rcvd_size);
 		return EIO;
 	}
@@ -203,9 +251,7 @@
 int usbmast_read(usbmast_fun_t *mfun, uint64_t ba, size_t nblocks, void *buf)
 {
+	scsi_cmd_t cmd;
 	scsi_cdb_read_12_t cdb;
-	size_t data_len;
-	int rc;
-
-	/* XXX Need softstate to store block size. */
+	int rc;
 
 	if (ba > UINT32_MAX)
@@ -220,16 +266,28 @@
 	cdb.xfer_len = host2uint32_t_be(nblocks);
 
-	rc = usb_massstor_data_in(mfun, 0xDEADBEEF, (uint8_t *) &cdb,
-	    sizeof(cdb), buf, nblocks * mfun->block_size, &data_len);
+	memset(&cmd, 0, sizeof(cmd));
+	cmd.cdb = &cdb;
+	cmd.cdb_size = sizeof(cdb);
+	cmd.data_in = buf;
+	cmd.data_in_size = nblocks * mfun->block_size;
+
+	rc = usb_massstor_cmd(mfun, 0xDEADBEEF, &cmd);
 
         if (rc != EOK) {
-		usb_log_error("Read (12) failed, device %s: %s.\n",
-		   mfun->mdev->ddf_dev->name, str_error(rc));
-		return rc;
-	}
-
-	if (data_len < nblocks * mfun->block_size) {
+		usb_log_error("Read (12) transport failed, device %s: %s.\n",
+		   mfun->mdev->ddf_dev->name, str_error(rc));
+		return rc;
+	}
+
+	if (cmd.status != CMDS_GOOD) {
+		usb_log_error("Read (12) command failed, device %s.\n",
+		   mfun->mdev->ddf_dev->name);
+		usbmast_dump_sense(mfun);
+		return EIO;
+	}
+
+	if (cmd.rcvd_size < nblocks * mfun->block_size) {
 		usb_log_error("SCSI Read response too short (%zu).\n",
-		    data_len);
+		    cmd.rcvd_size);
 		return EIO;
 	}
@@ -250,6 +308,6 @@
     const void *data)
 {
+	scsi_cmd_t cmd;
 	scsi_cdb_write_12_t cdb;
-	size_t sent_len;
 	int rc;
 
@@ -265,16 +323,22 @@
 	cdb.xfer_len = host2uint32_t_be(nblocks);
 
-	rc = usb_massstor_data_out(mfun, 0xDEADBEEF, (uint8_t *) &cdb,
-	    sizeof(cdb), data, nblocks * mfun->block_size, &sent_len);
+	memset(&cmd, 0, sizeof(cmd));
+	cmd.cdb = &cdb;
+	cmd.cdb_size = sizeof(cdb);
+	cmd.data_out = data;
+	cmd.data_out_size = nblocks * mfun->block_size;
+
+	rc = usb_massstor_cmd(mfun, 0xDEADBEEF, &cmd);
 
         if (rc != EOK) {
-		usb_log_error("Write (12) failed, device %s: %s.\n",
-		   mfun->mdev->ddf_dev->name, str_error(rc));
-		return rc;
-	}
-
-	if (sent_len < nblocks * mfun->block_size) {
-		usb_log_error("SCSI Write not all bytes transferred (%zu).\n",
-		    sent_len);
+		usb_log_error("Write (12) transport failed, device %s: %s.\n",
+		   mfun->mdev->ddf_dev->name, str_error(rc));
+		return rc;
+	}
+
+	if (cmd.status != CMDS_GOOD) {
+		usb_log_error("Write (12) command failed, device %s.\n",
+		   mfun->mdev->ddf_dev->name);
+		usbmast_dump_sense(mfun);
 		return EIO;
 	}
