diff --git a/Makefile b/Makefile
index 7d511e8..fb9bfaf 100644
--- a/Makefile
+++ b/Makefile
@@ -12,10 +12,10 @@ OUT=out/
 
 # Source files
 SRCBOTH=misc.c stacks.c pmm.c output.c util.c block.c floppy.c ata.c mouse.c \
-        kbd.c pci.c serial.c clock.c pic.c cdrom.c ps2port.c smp.c resume.c \
-        pnpbios.c pirtable.c vgahooks.c ramdisk.c pcibios.c blockcmd.c \
-        usb.c usb-uhci.c usb-ohci.c usb-ehci.c usb-hid.c usb-msc.c \
-        virtio-ring.c virtio-pci.c virtio-blk.c apm.c ahci.c
+    kbd.c pci.c serial.c clock.c pic.c cdrom.c ps2port.c smp.c resume.c \
+    pnpbios.c pirtable.c vgahooks.c ramdisk.c pcibios.c blockcmd.c \
+    usb.c usb-uhci.c usb-ohci.c usb-ehci.c usb-hid.c usb-msc.c \
+    virtio-ring.c virtio-pci.c virtio-blk.c virtio-scsi.c apm.c ahci.c
 SRC16=$(SRCBOTH) system.c disk.c font.c
 SRC32FLAT=$(SRCBOTH) post.c shadow.c memmap.c coreboot.c boot.c \
       acpi.c smm.c mptable.c smbios.c pciinit.c optionroms.c mtrr.c \
diff --git a/src/Kconfig b/src/Kconfig
index 338f51a..8de3503 100644
--- a/src/Kconfig
+++ b/src/Kconfig
@@ -109,10 +109,16 @@ menu "Hardware support"
             Support for AHCI disk code.
     config VIRTIO_BLK
         depends on DRIVES && !COREBOOT
-        bool "VirtIO controllers"
+        bool "virtio-blk controllers"
         default y
         help
-            Support boot from virtio storage.
+            Support boot from virtio-blk storage.
+    config VIRTIO_SCSI
+        depends on DRIVES && !COREBOOT
+        bool "virtio-scsi controllers"
+        default y
+        help
+            Support boot from virtio-scsi storage.
     config FLOPPY
         depends on DRIVES
         bool "Floppy controller"
diff --git a/src/block.c b/src/block.c
index f7e7851..e607d67 100644
--- a/src/block.c
+++ b/src/block.c
@@ -11,8 +11,8 @@
 #include "util.h" // dprintf
 #include "ata.h" // process_ata_op
 #include "ahci.h" // process_ahci_op
-#include "usb-msc.h" // process_usb_op
-#include "virtio-blk.h" // process_virtio_op
+#include "virtio-blk.h" // process_virtio_blk_op
+#include "blockcmd.h" // cdb_*
 
 u8 FloppyCount VAR16VISIBLE;
 u8 CDCount;
@@ -276,6 +276,28 @@ map_floppy_drive(struct drive_s *drive_g)
  * 16bit calling interface
  ****************************************************************/
 
+int
+process_scsi_op(struct disk_op_s *op)
+{
+    if (!CONFIG_VIRTIO_SCSI && !CONFIG_USB_MSC)
+        return 0;
+    switch (op->command) {
+    case CMD_READ:
+        return cdb_read(op);
+    case CMD_WRITE:
+        return cdb_write(op);
+    case CMD_FORMAT:
+    case CMD_RESET:
+    case CMD_ISREADY:
+    case CMD_VERIFY:
+    case CMD_SEEK:
+        return DISK_RET_SUCCESS;
+    default:
+        op->count = 0;
+        return DISK_RET_EPARAM;
+    }
+}
+
 // Execute a disk_op request.
 int
 process_op(struct disk_op_s *op)
@@ -293,12 +315,13 @@ process_op(struct disk_op_s *op)
         return process_ramdisk_op(op);
     case DTYPE_CDEMU:
         return process_cdemu_op(op);
-    case DTYPE_USB:
-        return process_usb_op(op);
-    case DTYPE_VIRTIO:
-	return process_virtio_op(op);
+    case DTYPE_VIRTIO_BLK:
+        return process_virtio_blk_op(op);
     case DTYPE_AHCI:
 	return process_ahci_op(op);
+    case DTYPE_USB:
+    case DTYPE_VIRTIO_SCSI:
+        return process_scsi_op(op);
     default:
         op->count = 0;
         return DISK_RET_EPARAM;
diff --git a/src/blockcmd.c b/src/blockcmd.c
index c9c6845..b2a8d71 100644
--- a/src/blockcmd.c
+++ b/src/blockcmd.c
@@ -12,6 +12,7 @@
 #include "ata.h" // atapi_cmd_data
 #include "ahci.h" // atapi_cmd_data
 #include "usb-msc.h" // usb_cmd_data
+#include "virtio-scsi.h" // virtio_scsi_cmd_data
 
 // Route command to low-level handler.
 static int
@@ -25,6 +26,8 @@ cdb_cmd_data(struct disk_op_s *op, void *cdbcmd, u16 blocksize)
         return usb_cmd_data(op, cdbcmd, blocksize);
     case DTYPE_AHCI:
         return ahci_cmd_data(op, cdbcmd, blocksize);
+    case DTYPE_VIRTIO_SCSI:
+        return virtio_scsi_cmd_data(op, cdbcmd, blocksize);
     default:
         op->count = 0;
         return DISK_RET_EPARAM;
@@ -32,6 +35,128 @@ cdb_cmd_data(struct disk_op_s *op, void *cdbcmd, u16 blocksize)
 }
 
 int
+scsi_is_ready(struct disk_op_s *op)
+{
+    dprintf(6, "scsi_is_ready (drive=%p)\n", op->drive_g);
+
+    /* Retry TEST UNIT READY for 5 seconds unless MEDIUM NOT PRESENT is
+     * reported by the device.  If the device reports "IN PROGRESS",
+     * 30 seconds is added. */
+    int in_progress = 0;
+    u64 end = calc_future_tsc(5000);
+    for (;;) {
+        if (check_tsc(end)) {
+            dprintf(1, "test unit ready failed\n");
+            return -1;
+        }
+
+        int ret = cdb_test_unit_ready(op);
+        if (!ret)
+            // Success
+            break;
+
+        struct cdbres_request_sense sense;
+        ret = cdb_get_sense(op, &sense);
+        if (ret)
+            // Error - retry.
+            continue;
+
+        // Sense succeeded.
+        if (sense.asc == 0x3a) { /* MEDIUM NOT PRESENT */
+            dprintf(1, "Device reports MEDIUM NOT PRESENT\n");
+            return -1;
+        }
+
+        if (sense.asc == 0x04 && sense.ascq == 0x01 && !in_progress) {
+            /* IN PROGRESS OF BECOMING READY */
+            printf("Waiting for device to detect medium... ");
+            /* Allow 30 seconds more */
+            end = calc_future_tsc(30000);
+            in_progress = 1;
+        }
+    }
+    return 0;
+}
+
+// Validate drive and find block size and sector count.
+int
+scsi_init_drive(struct drive_s *drive, const char *s, int *pdt, char **desc)
+{
+    if (!CONFIG_USB_MSC && !CONFIG_VIRTIO_SCSI)
+        return 0;
+
+    struct disk_op_s dop;
+    memset(&dop, 0, sizeof(dop));
+    dop.drive_g = drive;
+    struct cdbres_inquiry data;
+    int ret = cdb_get_inquiry(&dop, &data);
+    if (ret)
+        return ret;
+    char vendor[sizeof(data.vendor)+1], product[sizeof(data.product)+1];
+    char rev[sizeof(data.rev)+1];
+    strtcpy(vendor, data.vendor, sizeof(vendor));
+    nullTrailingSpace(vendor);
+    strtcpy(product, data.product, sizeof(product));
+    nullTrailingSpace(product);
+    strtcpy(rev, data.rev, sizeof(rev));
+    nullTrailingSpace(rev);
+    *pdt = data.pdt & 0x1f;
+    int removable = !!(data.removable & 0x80);
+    dprintf(1, "%s vendor='%s' product='%s' rev='%s' type=%d removable=%d\n"
+            , s, vendor, product, rev, *pdt, removable);
+    drive->removable = removable;
+
+    if (*pdt == SCSI_TYPE_CDROM) {
+        drive->blksize = CDROM_SECTOR_SIZE;
+        drive->sectors = (u64)-1;
+
+        *desc = znprintf(MAXDESCSIZE, "DVD/CD [%s Drive %s %s %s]"
+                              , s, vendor, product, rev);
+        return 0;
+    }
+
+    ret = scsi_is_ready(&dop);
+    if (ret) {
+        dprintf(1, "scsi_is_ready returned %d\n", ret);
+        return ret;
+    }
+
+    struct cdbres_read_capacity capdata;
+    ret = cdb_read_capacity(&dop, &capdata);
+    if (ret)
+        return ret;
+
+    // READ CAPACITY returns the address of the last block.
+    // We do not bother with READ CAPACITY(16) because BIOS does not support
+    // 64-bit LBA anyway.
+    drive->blksize = ntohl(capdata.blksize);
+    drive->sectors = (u64)ntohl(capdata.sectors) + 1;
+    dprintf(1, "%s blksize=%d sectors=%d\n"
+            , s, drive->blksize, (unsigned)drive->sectors);
+
+    struct cdbres_mode_sense_geom geomdata;
+    ret = cdb_mode_sense_geom(&dop, &geomdata);
+    if (ret == 0) {
+        u32 cylinders;
+        cylinders = geomdata.cyl[0] << 16;
+        cylinders |= geomdata.cyl[1] << 8;
+        cylinders |= geomdata.cyl[2];
+        if (cylinders && geomdata.heads &&
+            drive->sectors <= 0xFFFFFFFFULL &&
+            ((u32)drive->sectors % (geomdata.heads * cylinders) == 0)) {
+            drive->pchs.cylinders = cylinders;
+            drive->pchs.heads = geomdata.heads;
+            drive->pchs.spt = (u32)drive->sectors
+     / (geomdata.heads * cylinders);
+        }
+    }
+
+    *desc = znprintf(MAXDESCSIZE, "%s Drive %s %s %s"
+                      , s, vendor, product, rev);
+    return 0;
+}
+
+int
 cdb_get_inquiry(struct disk_op_s *op, struct cdbres_inquiry *data)
 {
     struct cdb_request_sense cmd;
@@ -56,6 +181,18 @@ cdb_get_sense(struct disk_op_s *op, struct cdbres_request_sense *data)
     return cdb_cmd_data(op, &cmd, sizeof(*data));
 }
 
+// Test unit ready
+int
+cdb_test_unit_ready(struct disk_op_s *op)
+{
+    struct cdb_request_sense cmd;
+    memset(&cmd, 0, sizeof(cmd));
+    cmd.command = CDB_CMD_TEST_UNIT_READY;
+    op->count = 0;
+    op->buf_fl = NULL;
+    return cdb_cmd_data(op, &cmd, 0);
+}
+
 // Request capacity
 int
 cdb_read_capacity(struct disk_op_s *op, struct cdbres_read_capacity *data)
@@ -68,6 +205,21 @@ cdb_read_capacity(struct disk_op_s *op, struct cdbres_read_capacity *data)
     return cdb_cmd_data(op, &cmd, sizeof(*data));
 }
 
+// Mode sense, geometry page.
+int
+cdb_mode_sense_geom(struct disk_op_s *op, struct cdbres_mode_sense_geom *data)
+{
+    struct cdb_mode_sense cmd;
+    memset(&cmd, 0, sizeof(cmd));
+    cmd.command = CDB_CMD_MODE_SENSE;
+    cmd.flags = 8; /* DBD */
+    cmd.page = MODE_PAGE_HD_GEOMETRY;
+    cmd.count = htons(sizeof(*data));
+    op->count = 1;
+    op->buf_fl = data;
+    return cdb_cmd_data(op, &cmd, sizeof(*data));
+}
+
 // Read sectors.
 int
 cdb_read(struct disk_op_s *op)
@@ -79,3 +231,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 903c435..bace649 100644
--- a/src/blockcmd.h
+++ b/src/blockcmd.h
@@ -32,8 +32,9 @@ struct cdbres_read_capacity {
     u32 blksize;
 } PACKED;
 
-#define CDB_CMD_INQUIRY 0x12
-#define CDB_CMD_REQUEST_SENSE 0x03
+#define CDB_CMD_TEST_UNIT_READY  0x00
+#define CDB_CMD_INQUIRY          0x12
+#define CDB_CMD_REQUEST_SENSE    0x03
 
 struct cdb_request_sense {
     u8 command;
@@ -56,6 +57,9 @@ struct cdbres_request_sense {
     u32 reserved_0e;
 } PACKED;
 
+#define SCSI_TYPE_DISK  0x00
+#define SCSI_TYPE_CDROM 0x05
+
 struct cdbres_inquiry {
     u8 pdt;
     u8 removable;
@@ -67,11 +71,45 @@ struct cdbres_inquiry {
     char rev[4];
 } PACKED;
 
+#define CDB_CMD_MODE_SENSE    0x5A
+#define MODE_PAGE_HD_GEOMETRY 0x04
+
+struct cdb_mode_sense {
+    u8 command;
+    u8 flags;
+    u8 page;
+    u32 reserved_03;
+    u16 count;
+    u8 reserved_09;
+    u8 pad[6];
+} PACKED;
+
+struct cdbres_mode_sense_geom {
+    u8 unused_00[3];
+    u8 read_only;
+    u32 unused_04;
+    u8 page;
+    u8 length;
+    u8 cyl[3];
+    u8 heads;
+    u8 precomp[3];
+    u8 reduced[3];
+    u16 step_rate;
+    u8 landing[3];
+    u16 rpm;
+} PACKED;
+
 // blockcmd.c
 int cdb_get_inquiry(struct disk_op_s *op, struct cdbres_inquiry *data);
 int cdb_get_sense(struct disk_op_s *op, struct cdbres_request_sense *data);
+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_mode_sense_geom(struct disk_op_s *op, struct cdbres_mode_sense_geom *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);
+
+int scsi_is_ready(struct disk_op_s *op);
+int scsi_init_drive(struct drive_s *drive, const char *s, int *pdt, char **desc);
 
 #endif // blockcmd.h
diff --git a/src/boot.c b/src/boot.c
index 93928d3..c0991cd 100644
--- a/src/boot.c
+++ b/src/boot.c
@@ -128,6 +128,20 @@ int bootprio_find_pci_device(struct pci_device *pci)
     return find_prio(desc);
 }
 
+int bootprio_find_scsi_device(struct pci_device *pci, int target, int lun)
+{
+    if (!CONFIG_BOOTORDER)
+        return -1;
+    if (!pci)
+        // support only pci machine for now
+        return -1;
+    // Find scsi drive - for example: /pci@i0cf8/scsi@5/channel@0/disk@1,0
+    char desc[256], *p;
+    p = build_pci_path(desc, sizeof(desc), "*", pci);
+    snprintf(p, desc+sizeof(desc)-p, "/*@0/*@%d,%d", target, lun);
+    return find_prio(desc);
+}
+
 int bootprio_find_ata_device(struct pci_device *pci, int chanid, int slave)
 {
     if (!CONFIG_BOOTORDER)
diff --git a/src/boot.h b/src/boot.h
index d776aa1..686f04d 100644
--- a/src/boot.h
+++ b/src/boot.h
@@ -14,6 +14,7 @@ void boot_add_cbfs(void *data, const char *desc, int prio);
 void boot_prep(void);
 struct pci_device;
 int bootprio_find_pci_device(struct pci_device *pci);
+int bootprio_find_scsi_device(struct pci_device *pci, int target, int lun);
 int bootprio_find_ata_device(struct pci_device *pci, int chanid, int slave);
 int bootprio_find_fdc_device(struct pci_device *pci, int port, int fdid);
 int bootprio_find_pci_rom(struct pci_device *pci, int instance);
diff --git a/src/cdrom.c b/src/cdrom.c
index 6351fec..170ffc4 100644
--- a/src/cdrom.c
+++ b/src/cdrom.c
@@ -184,60 +184,6 @@ cdemu_134b(struct bregs *regs)
  * CD booting
  ****************************************************************/
 
-static int
-atapi_is_ready(struct disk_op_s *op)
-{
-    dprintf(6, "atapi_is_ready (drive=%p)\n", op->drive_g);
-
-    /* Retry READ CAPACITY for 5 seconds unless MEDIUM NOT PRESENT is
-     * reported by the device.  If the device reports "IN PROGRESS",
-     * 30 seconds is added. */
-    struct cdbres_read_capacity info;
-    int in_progress = 0;
-    u64 end = calc_future_tsc(5000);
-    for (;;) {
-        if (check_tsc(end)) {
-            dprintf(1, "read capacity failed\n");
-            return -1;
-        }
-
-        int ret = cdb_read_capacity(op, &info);
-        if (!ret)
-            // Success
-            break;
-
-        struct cdbres_request_sense sense;
-        ret = cdb_get_sense(op, &sense);
-        if (ret)
-            // Error - retry.
-            continue;
-
-        // Sense succeeded.
-        if (sense.asc == 0x3a) { /* MEDIUM NOT PRESENT */
-            dprintf(1, "Device reports MEDIUM NOT PRESENT\n");
-            return -1;
-        }
-
-        if (sense.asc == 0x04 && sense.ascq == 0x01 && !in_progress) {
-            /* IN PROGRESS OF BECOMING READY */
-            printf("Waiting for device to detect medium... ");
-            /* Allow 30 seconds more */
-            end = calc_future_tsc(30000);
-            in_progress = 1;
-        }
-    }
-
-    u32 blksize = ntohl(info.blksize), sectors = ntohl(info.sectors);
-    if (blksize != GET_GLOBAL(op->drive_g->blksize)) {
-        printf("Unsupported sector size %u\n", blksize);
-        return -1;
-    }
-
-    dprintf(6, "sectors=%u\n", sectors);
-    printf("%dMB medium detected\n", sectors>>(20-11));
-    return 0;
-}
-
 int
 cdrom_boot(struct drive_s *drive_g)
 {
@@ -248,9 +194,9 @@ cdrom_boot(struct drive_s *drive_g)
     if (!dop.drive_g || cdid < 0)
         return 1;
 
-    int ret = atapi_is_ready(&dop);
+    int ret = scsi_is_ready(&dop);
     if (ret)
-        dprintf(1, "atapi_is_ready returned %d\n", ret);
+        dprintf(1, "scsi_is_ready returned %d\n", ret);
 
     // Read the Boot Record Volume Descriptor
     u8 buffer[2048];
diff --git a/src/disk.c b/src/disk.c
index 8f7c61f..6a170fd 100644
--- a/src/disk.c
+++ b/src/disk.c
@@ -546,7 +546,8 @@ disk_1348(struct bregs *regs, struct drive_s *drive_g)
     SET_INT13DPT(regs, blksize, blksize);
 
     if (size < 30 ||
-        (type != DTYPE_ATA && type != DTYPE_ATAPI && type != DTYPE_VIRTIO)) {
+        (type != DTYPE_ATA && type != DTYPE_ATAPI &&
+         type != DTYPE_VIRTIO_BLK && type != DTYPE_VIRTIO_SCSI)) {
         disk_ret(regs, DISK_RET_SUCCESS);
         return;
     }
@@ -651,7 +652,7 @@ disk_1348(struct bregs *regs, struct drive_s *drive_g)
         SET_INT13DPT(regs, iface_path, iobase1);
     }
 
-    if (type != DTYPE_VIRTIO) {
+    if (type != DTYPE_VIRTIO_BLK) {
         SET_INT13DPT(regs, iface_type[0], 'A');
         SET_INT13DPT(regs, iface_type[1], 'T');
         SET_INT13DPT(regs, iface_type[2], 'A');
diff --git a/src/disk.h b/src/disk.h
index ac33518..d344399 100644
--- a/src/disk.h
+++ b/src/disk.h
@@ -198,15 +198,16 @@ struct drive_s {
 #define DISK_SECTOR_SIZE  512
 #define CDROM_SECTOR_SIZE 2048
 
-#define DTYPE_NONE     0x00
-#define DTYPE_FLOPPY   0x01
-#define DTYPE_ATA      0x02
-#define DTYPE_ATAPI    0x03
-#define DTYPE_RAMDISK  0x04
-#define DTYPE_CDEMU    0x05
-#define DTYPE_USB      0x06
-#define DTYPE_VIRTIO   0x07
-#define DTYPE_AHCI     0x08
+#define DTYPE_NONE         0x00
+#define DTYPE_FLOPPY       0x01
+#define DTYPE_ATA          0x02
+#define DTYPE_ATAPI        0x03
+#define DTYPE_RAMDISK      0x04
+#define DTYPE_CDEMU        0x05
+#define DTYPE_USB          0x06
+#define DTYPE_VIRTIO_BLK   0x07
+#define DTYPE_AHCI         0x08
+#define DTYPE_VIRTIO_SCSI  0x09
 
 #define MAXDESCSIZE 80
 
diff --git a/src/pci_ids.h b/src/pci_ids.h
index e1cded2..4b59585 100644
--- a/src/pci_ids.h
+++ b/src/pci_ids.h
@@ -2608,3 +2608,4 @@
 
 #define PCI_VENDOR_ID_REDHAT_QUMRANET	0x1af4
 #define PCI_DEVICE_ID_VIRTIO_BLK	0x1001
+#define PCI_DEVICE_ID_VIRTIO_SCSI	0x1004
diff --git a/src/post.c b/src/post.c
index b4ad1fa..d7bbcdd 100644
--- a/src/post.c
+++ b/src/post.c
@@ -26,6 +26,7 @@
 #include "xen.h" // xen_probe_hvm_info
 #include "ps2port.h" // ps2port_setup
 #include "virtio-blk.h" // virtio_blk_setup
+#include "virtio-scsi.h" // virtio_scsi_setup
 
 
 /****************************************************************
@@ -190,6 +191,7 @@ init_hw(void)
     cbfs_payload_setup();
     ramdisk_setup();
     virtio_blk_setup();
+    virtio_scsi_setup();
 }
 
 // Begin the boot process by invoking an int0x19 in 16bit mode.
diff --git a/src/usb-ehci.c b/src/usb-ehci.c
index a60c607..9bdd638 100644
--- a/src/usb-ehci.c
+++ b/src/usb-ehci.c
@@ -303,22 +303,12 @@ ehci_init(struct pci_device *pci, int busid, struct pci_device *comppci)
  * End point communication
  ****************************************************************/
 
-static int
-ehci_wait_qh(struct usb_ehci_s *cntl, struct ehci_qh *qh)
-{
-    // XXX - 500ms just a guess
-    u64 end = calc_future_tsc(500);
-    for (;;) {
-        if (qh->qtd_next & EHCI_PTR_TERM)
-            // XXX - confirm
-            return 0;
-        if (check_tsc(end)) {
-            warn_timeout();
-            return -1;
-        }
-        yield();
-    }
-}
+struct ehci_pipe {
+    struct ehci_qh qh;
+    struct ehci_qtd *next_td, *tds;
+    void *data;
+    struct usb_pipe pipe;
+};
 
 // Wait for next USB async frame to start - for ensuring safe memory release.
 static void
@@ -362,12 +352,46 @@ ehci_waittick(struct usb_ehci_s *cntl)
     writel(&cntl->regs->usbsts, STS_IAA);
 }
 
-struct ehci_pipe {
-    struct ehci_qh qh;
-    struct ehci_qtd *next_td, *tds;
-    void *data;
-    struct usb_pipe pipe;
-};
+static void
+ehci_reset_pipe(struct ehci_pipe *pipe)
+{
+    SET_FLATPTR(pipe->qh.qtd_next, EHCI_PTR_TERM);
+    SET_FLATPTR(pipe->qh.alt_next, EHCI_PTR_TERM);
+    barrier();
+    SET_FLATPTR(pipe->qh.token, GET_FLATPTR(pipe->qh.token) & QTD_TOGGLE);
+}
+
+static int
+ehci_wait_td(struct ehci_pipe *pipe, struct ehci_qtd *td, int timeout)
+{
+    u64 end = calc_future_tsc(timeout);
+    u32 status;
+    for (;;) {
+        status = td->token;
+        if (!(status & QTD_STS_ACTIVE))
+            break;
+        if (check_tsc(end)) {
+            u32 cur = GET_FLATPTR(pipe->qh.current);
+            u32 tok = GET_FLATPTR(pipe->qh.token);
+            u32 next = GET_FLATPTR(pipe->qh.qtd_next);
+            warn_timeout();
+            dprintf(1, "ehci pipe=%p cur=%08x tok=%08x next=%x td=%p status=%x\n"
+                    , pipe, cur, tok, next, td, status);
+            ehci_reset_pipe(pipe);
+            struct usb_ehci_s *cntl = container_of(
+                GET_FLATPTR(pipe->pipe.cntl), struct usb_ehci_s, usb);
+            ehci_waittick(cntl);
+            return -1;
+        }
+        yield();
+    }
+    if (status & QTD_STS_HALT) {
+        dprintf(1, "ehci_wait_td error - status=%x\n", status);
+        ehci_reset_pipe(pipe);
+        return -2;
+    }
+    return 0;
+}
 
 void
 ehci_free_pipe(struct usb_pipe *p)
@@ -416,7 +440,6 @@ ehci_alloc_control_pipe(struct usb_pipe *dummy)
     memset(pipe, 0, sizeof(*pipe));
     memcpy(&pipe->pipe, dummy, sizeof(pipe->pipe));
     pipe->qh.qtd_next = pipe->qh.alt_next = EHCI_PTR_TERM;
-    pipe->qh.token = QTD_STS_HALT;
 
     // Add queue head to controller list.
     struct ehci_qh *async_qh = cntl->async_qh;
@@ -455,15 +478,14 @@ ehci_control(struct usb_pipe *p, int dir, const void *cmd, int cmdsize
     ASSERT32FLAT();
     if (! CONFIG_USB_EHCI)
         return -1;
-    dprintf(5, "ehci_control %p\n", p);
+    dprintf(5, "ehci_control %p (dir=%d cmd=%d data=%d)\n"
+            , p, dir, cmdsize, datasize);
     if (datasize > 4*4096 || cmdsize > 4*4096) {
         // XXX - should support larger sizes.
         warn_noalloc();
         return -1;
     }
     struct ehci_pipe *pipe = container_of(p, struct ehci_pipe, pipe);
-    struct usb_ehci_s *cntl = container_of(
-        pipe->pipe.cntl, struct usb_ehci_s, usb);
 
     u16 maxpacket = pipe->pipe.maxpacket;
     int speed = pipe->pipe.speed;
@@ -513,14 +535,12 @@ ehci_control(struct usb_pipe *p, int dir, const void *cmd, int cmdsize
     // Transfer data
     barrier();
     pipe->qh.qtd_next = (u32)tds;
-    barrier();
-    pipe->qh.token = 0;
-    int ret = ehci_wait_qh(cntl, &pipe->qh);
-    pipe->qh.token = QTD_STS_HALT;
-    if (ret) {
-        pipe->qh.qtd_next = pipe->qh.alt_next = EHCI_PTR_TERM;
-        // XXX - halt qh?
-        ehci_waittick(cntl);
+    int i, ret=0;
+    for (i=0; i<3; i++) {
+        struct ehci_qtd *td = &tds[i];
+        ret = ehci_wait_td(pipe, td, 500);
+        if (ret)
+            break;
     }
     free(tds);
     return ret;
@@ -545,7 +565,6 @@ ehci_alloc_bulk_pipe(struct usb_pipe *dummy)
     memset(pipe, 0, sizeof(*pipe));
     memcpy(&pipe->pipe, dummy, sizeof(pipe->pipe));
     pipe->qh.qtd_next = pipe->qh.alt_next = EHCI_PTR_TERM;
-    pipe->qh.token = QTD_STS_HALT;
 
     // Add queue head to controller list.
     struct ehci_qh *async_qh = cntl->async_qh;
@@ -555,28 +574,6 @@ ehci_alloc_bulk_pipe(struct usb_pipe *dummy)
     return &pipe->pipe;
 }
 
-static int
-ehci_wait_td(struct ehci_qtd *td)
-{
-    u64 end = calc_future_tsc(5000); // XXX - lookup real time.
-    u32 status;
-    for (;;) {
-        status = td->token;
-        if (!(status & QTD_STS_ACTIVE))
-            break;
-        if (check_tsc(end)) {
-            warn_timeout();
-            return -1;
-        }
-        yield();
-    }
-    if (status & QTD_STS_HALT) {
-        dprintf(1, "ehci_wait_td error - status=%x\n", status);
-        return -2;
-    }
-    return 0;
-}
-
 #define STACKQTDS 4
 
 int
@@ -607,15 +604,13 @@ ehci_send_bulk(struct usb_pipe *p, int dir, void *data, int datasize)
                    | (GET_FLATPTR(pipe->pipe.tt_devaddr) << QH_HUBADDR_SHIFT)));
     barrier();
     SET_FLATPTR(pipe->qh.qtd_next, (u32)MAKE_FLATPTR(GET_SEG(SS), tds));
-    barrier();
-    SET_FLATPTR(pipe->qh.token, GET_FLATPTR(pipe->qh.token) & QTD_TOGGLE);
 
     int tdpos = 0;
     while (datasize) {
         struct ehci_qtd *td = &tds[tdpos++ % STACKQTDS];
-        int ret = ehci_wait_td(td);
+        int ret = ehci_wait_td(pipe, td, 5000);
         if (ret)
-            goto fail;
+            return -1;
 
         struct ehci_qtd *nexttd_fl = MAKE_FLATPTR(GET_SEG(SS)
                                                  , &tds[tdpos % STACKQTDS]);
@@ -633,21 +628,12 @@ ehci_send_bulk(struct usb_pipe *p, int dir, void *data, int datasize)
     int i;
     for (i=0; i<STACKQTDS; i++) {
         struct ehci_qtd *td = &tds[tdpos++ % STACKQTDS];
-        int ret = ehci_wait_td(td);
+        int ret = ehci_wait_td(pipe, td, 5000);
         if (ret)
-            goto fail;
+            return -1;
     }
 
     return 0;
-fail:
-    dprintf(1, "ehci_send_bulk failed\n");
-    SET_FLATPTR(pipe->qh.qtd_next, EHCI_PTR_TERM);
-    SET_FLATPTR(pipe->qh.alt_next, EHCI_PTR_TERM);
-    // XXX - halt qh?
-    struct usb_ehci_s *cntl = container_of(
-        GET_FLATPTR(pipe->pipe.cntl), struct usb_ehci_s, usb);
-    ehci_waittick(cntl);
-    return -1;
 }
 
 struct usb_pipe *
diff --git a/src/usb-msc.c b/src/usb-msc.c
index 13ef93e..4a09972 100644
--- a/src/usb-msc.c
+++ b/src/usb-msc.c
@@ -46,6 +46,17 @@ struct csw_s {
     u8 bCSWStatus;
 } PACKED;
 
+static int
+usb_msc_send(struct usbdrive_s *udrive_g, int dir, void *buf, u32 bytes)
+{
+    struct usb_pipe *pipe;
+    if (dir == USB_DIR_OUT)
+        pipe = GET_GLOBAL(udrive_g->bulkout);
+    else
+        pipe = GET_GLOBAL(udrive_g->bulkin);
+    return usb_send_bulk(pipe, dir, buf, bytes);
+}
+
 // Low-level usb command transmit function.
 int
 usb_cmd_data(struct disk_op_s *op, void *cdbcmd, u16 blocksize)
@@ -57,35 +68,35 @@ usb_cmd_data(struct disk_op_s *op, void *cdbcmd, u16 blocksize)
             , 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);
 
     // Setup command block wrapper.
     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_send_bulk(bulkout, USB_DIR_OUT
+    int ret = usb_msc_send(udrive_g, USB_DIR_OUT
                             , MAKE_FLATPTR(GET_SEG(SS), &cbw), sizeof(cbw));
     if (ret)
         goto fail;
 
-    // Transfer data from device.
-    ret = usb_send_bulk(bulkin, USB_DIR_IN, op->buf_fl, bytes);
-    if (ret)
-        goto fail;
+    // Transfer data to/from device.
+    if (bytes) {
+        ret = usb_msc_send(udrive_g, cbw.bmCBWFlags, op->buf_fl, bytes);
+        if (ret)
+            goto fail;
+    }
 
     // Transfer csw info.
     struct csw_s csw;
-    ret = usb_send_bulk(bulkin, USB_DIR_IN
+    ret = usb_msc_send(udrive_g, USB_DIR_IN
                         , MAKE_FLATPTR(GET_SEG(SS), &csw), sizeof(csw));
     if (ret)
         goto fail;
@@ -95,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 (blocksize)
+        op->count -= csw.dCSWDataResidue / blocksize;
     return DISK_RET_EBADTRACK;
 
 fail:
@@ -107,75 +119,33 @@ fail:
 
 
 /****************************************************************
- * Drive ops
- ****************************************************************/
-
-// 16bit command demuxer for ATAPI cdroms.
-int
-process_usb_op(struct disk_op_s *op)
-{
-    if (!CONFIG_USB_MSC)
-        return 0;
-    switch (op->command) {
-    case CMD_READ:
-        return cdb_read(op);
-    case CMD_FORMAT:
-    case CMD_WRITE:
-        return DISK_RET_EWRITEPROTECT;
-    case CMD_RESET:
-    case CMD_ISREADY:
-    case CMD_VERIFY:
-    case CMD_SEEK:
-        return DISK_RET_SUCCESS;
-    default:
-        op->count = 0;
-        return DISK_RET_EPARAM;
-    }
-}
-
-
-/****************************************************************
  * Setup
  ****************************************************************/
 
 static int
-setup_drive_cdrom(struct disk_op_s *op, char *desc)
+setup_drive_cdrom(struct drive_s *drive, char *desc)
 {
-    op->drive_g->blksize = CDROM_SECTOR_SIZE;
-    op->drive_g->sectors = (u64)-1;
+    drive->sectors = (u64)-1;
     struct usb_pipe *pipe = container_of(
-        op->drive_g, struct usbdrive_s, drive)->bulkout;
+        drive, struct usbdrive_s, drive)->bulkout;
     int prio = bootprio_find_usb(pipe->cntl->pci, pipe->path);
-    boot_add_cd(op->drive_g, desc, prio);
+    boot_add_cd(drive, desc, prio);
     return 0;
 }
 
 static int
-setup_drive_hd(struct disk_op_s *op, char *desc)
+setup_drive_hd(struct drive_s *drive, char *desc)
 {
-    struct cdbres_read_capacity info;
-    int ret = cdb_read_capacity(op, &info);
-    if (ret)
-        return ret;
-    // XXX - retry for some timeout?
-
-    u32 blksize = ntohl(info.blksize), sectors = ntohl(info.sectors);
-    if (blksize != DISK_SECTOR_SIZE) {
-        if (blksize == CDROM_SECTOR_SIZE)
-            return setup_drive_cdrom(op, desc);
-        dprintf(1, "Unsupported USB MSC block size %d\n", blksize);
+    if (drive->blksize != DISK_SECTOR_SIZE) {
+        dprintf(1, "Unsupported USB MSC block size %d\n", drive->blksize);
         return -1;
     }
-    op->drive_g->blksize = blksize;
-    op->drive_g->sectors = sectors;
-    dprintf(1, "USB MSC blksize=%d sectors=%d\n", blksize, sectors);
 
     // Register with bcv system.
     struct usb_pipe *pipe = container_of(
-        op->drive_g, struct usbdrive_s, drive)->bulkout;
+        drive, struct usbdrive_s, drive)->bulkout;
     int prio = bootprio_find_usb(pipe->cntl->pci, pipe->path);
-    boot_add_hd(op->drive_g, desc, prio);
-
+    boot_add_hd(drive, desc, prio);
     return 0;
 }
 
@@ -218,37 +188,17 @@ usb_msc_init(struct usb_pipe *pipe
     if (!udrive_g->bulkin || !udrive_g->bulkout)
         goto fail;
 
-    // Validate drive and find block size and sector count.
-    struct disk_op_s dop;
-    memset(&dop, 0, sizeof(dop));
-    dop.drive_g = &udrive_g->drive;
-    struct cdbres_inquiry data;
-    int ret = cdb_get_inquiry(&dop, &data);
+    int ret, pdt;
+    char *desc = NULL;
+    ret = scsi_init_drive(&udrive_g->drive, "USB MSC", &pdt, &desc);
     if (ret)
         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));
-    nullTrailingSpace(vendor);
-    strtcpy(product, data.product, sizeof(product));
-    nullTrailingSpace(product);
-    strtcpy(rev, data.rev, sizeof(rev));
-    nullTrailingSpace(rev);
-    int pdt = data.pdt & 0x1f;
-    int removable = !!(data.removable & 0x80);
-    dprintf(1, "USB MSC vendor='%s' product='%s' rev='%s' type=%d removable=%d\n"
-            , vendor, product, rev, pdt, removable);
-    udrive_g->drive.removable = removable;
-
-    if (pdt == USB_MSC_TYPE_CDROM) {
-        char *desc = znprintf(MAXDESCSIZE, "DVD/CD [USB Drive %s %s %s]"
-                              , vendor, product, rev);
-        ret = setup_drive_cdrom(&dop, desc);
-    } else {
-        char *desc = znprintf(MAXDESCSIZE, "USB Drive %s %s %s"
-                              , vendor, product, rev);
-        ret = setup_drive_hd(&dop, desc);
-    }
+
+    if (pdt == SCSI_TYPE_CDROM)
+        ret = setup_drive_cdrom(&udrive_g->drive, desc);
+    else
+        ret = setup_drive_hd(&udrive_g->drive, desc);
+
     if (ret)
         goto fail;
 
diff --git a/src/usb-msc.h b/src/usb-msc.h
index 71adb20..a8686a3 100644
--- a/src/usb-msc.h
+++ b/src/usb-msc.h
@@ -21,7 +21,4 @@ int process_usb_op(struct disk_op_s *op);
 
 #define US_PR_BULK         0x50
 
-#define USB_MSC_TYPE_DISK  0x00
-#define USB_MSC_TYPE_CDROM 0x05
-
 #endif // ush-msc.h
diff --git a/src/usb-uhci.c b/src/usb-uhci.c
index f3680d3..a78dbca 100644
--- a/src/usb-uhci.c
+++ b/src/usb-uhci.c
@@ -212,26 +212,13 @@ uhci_init(struct pci_device *pci, int busid)
  * End point communication
  ****************************************************************/
 
-static int
-wait_qh(struct usb_uhci_s *cntl, struct uhci_qh *qh)
-{
-    // XXX - 500ms just a guess
-    u64 end = calc_future_tsc(500);
-    for (;;) {
-        if (qh->element & UHCI_PTR_TERM)
-            return 0;
-        if (check_tsc(end)) {
-            warn_timeout();
-            struct uhci_td *td = (void*)(qh->element & ~UHCI_PTR_BITS);
-            dprintf(1, "Timeout on wait_qh %p (td=%p s=%x c=%x/%x)\n"
-                    , qh, td, td->status
-                    , inw(cntl->iobase + USBCMD)
-                    , inw(cntl->iobase + USBSTS));
-            return -1;
-        }
-        yield();
-    }
-}
+struct uhci_pipe {
+    struct uhci_qh qh;
+    struct uhci_td *next_td;
+    struct usb_pipe pipe;
+    u16 iobase;
+    u8 toggle;
+};
 
 // Wait for next USB frame to start - for ensuring safe memory release.
 static void
@@ -251,13 +238,29 @@ uhci_waittick(u16 iobase)
     }
 }
 
-struct uhci_pipe {
-    struct uhci_qh qh;
-    struct uhci_td *next_td;
-    struct usb_pipe pipe;
-    u16 iobase;
-    u8 toggle;
-};
+static int
+wait_pipe(struct uhci_pipe *pipe, int timeout)
+{
+    u64 end = calc_future_tsc(timeout);
+    for (;;) {
+        u32 el_link = GET_FLATPTR(pipe->qh.element);
+        if (el_link & UHCI_PTR_TERM)
+            return 0;
+        if (check_tsc(end)) {
+            warn_timeout();
+            u16 iobase = GET_FLATPTR(pipe->iobase);
+            struct uhci_td *td = (void*)(el_link & ~UHCI_PTR_BITS);
+            dprintf(1, "Timeout on wait_pipe %p (td=%p s=%x c=%x/%x)\n"
+                    , pipe, (void*)el_link, GET_FLATPTR(td->status)
+                    , inw(iobase + USBCMD)
+                    , inw(iobase + USBSTS));
+            SET_FLATPTR(pipe->qh.element, UHCI_PTR_TERM);
+            uhci_waittick(iobase);
+            return -1;
+        }
+        yield();
+    }
+}
 
 void
 uhci_free_pipe(struct usb_pipe *p)
@@ -331,8 +334,6 @@ uhci_control(struct usb_pipe *p, int dir, const void *cmd, int cmdsize
         return -1;
     dprintf(5, "uhci_control %p\n", p);
     struct uhci_pipe *pipe = container_of(p, struct uhci_pipe, pipe);
-    struct usb_uhci_s *cntl = container_of(
-        pipe->pipe.cntl, struct usb_uhci_s, usb);
 
     int maxpacket = pipe->pipe.maxpacket;
     int lowspeed = pipe->pipe.speed;
@@ -376,11 +377,7 @@ uhci_control(struct usb_pipe *p, int dir, const void *cmd, int cmdsize
     // Transfer data
     barrier();
     pipe->qh.element = (u32)&tds[0];
-    int ret = wait_qh(cntl, &pipe->qh);
-    if (ret) {
-        pipe->qh.element = UHCI_PTR_TERM;
-        uhci_waittick(pipe->iobase);
-    }
+    int ret = wait_pipe(pipe, 500);
     free(tds);
     return ret;
 }
@@ -487,16 +484,8 @@ uhci_send_bulk(struct usb_pipe *p, int dir, void *data, int datasize)
         data += transfer;
         datasize -= transfer;
     }
-    int i;
-    for (i=0; i<STACKTDS; i++) {
-        struct uhci_td *td = &tds[tdpos++ % STACKTDS];
-        int ret = wait_td(td);
-        if (ret)
-            goto fail;
-    }
-
     SET_FLATPTR(pipe->toggle, !!toggle);
-    return 0;
+    return wait_pipe(pipe, 5000);
 fail:
     dprintf(1, "uhci_send_bulk failed\n");
     SET_FLATPTR(pipe->qh.element, UHCI_PTR_TERM);
diff --git a/src/virtio-blk.c b/src/virtio-blk.c
index b1274fc..b869189 100644
--- a/src/virtio-blk.c
+++ b/src/virtio-blk.c
@@ -75,7 +75,7 @@ virtio_blk_op(struct disk_op_s *op, int write)
 }
 
 int
-process_virtio_op(struct disk_op_s *op)
+process_virtio_blk_op(struct disk_op_s *op)
 {
     if (! CONFIG_VIRTIO_BLK || CONFIG_COREBOOT)
         return 0;
@@ -103,27 +103,17 @@ init_virtio_blk(struct pci_device *pci)
     dprintf(1, "found virtio-blk at %x:%x\n", pci_bdf_to_bus(bdf),
             pci_bdf_to_dev(bdf));
     struct virtiodrive_s *vdrive_g = malloc_fseg(sizeof(*vdrive_g));
-    struct vring_virtqueue *vq = memalign_low(PAGE_SIZE, sizeof(*vq));
-    if (!vdrive_g || !vq) {
+    if (!vdrive_g) {
         warn_noalloc();
-        goto fail;
+        return;
     }
     memset(vdrive_g, 0, sizeof(*vdrive_g));
-    memset(vq, 0, sizeof(*vq));
-    vdrive_g->drive.type = DTYPE_VIRTIO;
+    vdrive_g->drive.type = DTYPE_VIRTIO_BLK;
     vdrive_g->drive.cntl_id = bdf;
-    vdrive_g->vq = vq;
-
-    u16 ioaddr = pci_config_readl(bdf, PCI_BASE_ADDRESS_0) &
-        PCI_BASE_ADDRESS_IO_MASK;
 
+    u16 ioaddr = vp_init_simple(bdf);
     vdrive_g->ioaddr = ioaddr;
-
-    vp_reset(ioaddr);
-    vp_set_status(ioaddr, VIRTIO_CONFIG_S_ACKNOWLEDGE |
-                  VIRTIO_CONFIG_S_DRIVER );
-
-    if (vp_find_vq(ioaddr, 0, vdrive_g->vq) < 0 ) {
+    if (vp_find_vq(ioaddr, 0, &vdrive_g->vq) < 0 ) {
         dprintf(1, "fail to find vq for virtio-blk %x:%x\n",
                 pci_bdf_to_bus(bdf), pci_bdf_to_dev(bdf));
         goto fail;
@@ -161,8 +151,8 @@ init_virtio_blk(struct pci_device *pci)
     return;
 
 fail:
+    free(vdrive_g->vq);
     free(vdrive_g);
-    free(vq);
 }
 
 void
diff --git a/src/virtio-blk.h b/src/virtio-blk.h
index 7243704..0825f09 100644
--- a/src/virtio-blk.h
+++ b/src/virtio-blk.h
@@ -37,7 +37,7 @@ struct virtio_blk_outhdr {
 #define VIRTIO_BLK_S_UNSUPP	2
 
 struct disk_op_s;
-int process_virtio_op(struct disk_op_s *op);
+int process_virtio_blk_op(struct disk_op_s *op);
 void virtio_blk_setup(void);
 
 #endif /* _VIRTIO_BLK_H */
diff --git a/src/virtio-pci.c b/src/virtio-pci.c
index db19e97..7e0c1a5 100644
--- a/src/virtio-pci.c
+++ b/src/virtio-pci.c
@@ -21,12 +21,18 @@
 #include "util.h" // dprintf
 
 int vp_find_vq(unsigned int ioaddr, int queue_index,
-               struct vring_virtqueue *vq)
+               struct vring_virtqueue **p_vq)
 {
-   struct vring * vr = &vq->vring;
    u16 num;
 
    ASSERT32FLAT();
+   struct vring_virtqueue *vq = *p_vq = memalign_low(PAGE_SIZE, sizeof(*vq));
+   if (!vq) {
+       warn_noalloc();
+       goto fail;
+   }
+   memset(vq, 0, sizeof(*vq));
+
    /* select the queue */
 
    outw(queue_index, ioaddr + VIRTIO_PCI_QUEUE_SEL);
@@ -36,25 +42,26 @@ int vp_find_vq(unsigned int ioaddr, int queue_index,
    num = inw(ioaddr + VIRTIO_PCI_QUEUE_NUM);
    if (!num) {
        dprintf(1, "ERROR: queue size is 0\n");
-       return -1;
+       goto fail;
    }
 
    if (num > MAX_QUEUE_NUM) {
        dprintf(1, "ERROR: queue size %d > %d\n", num, MAX_QUEUE_NUM);
-       return -1;
+       goto fail;
    }
 
    /* check if the queue is already active */
 
    if (inl(ioaddr + VIRTIO_PCI_QUEUE_PFN)) {
        dprintf(1, "ERROR: queue already active\n");
-       return -1;
+       goto fail;
    }
 
    vq->queue_index = queue_index;
 
    /* initialize the queue */
 
+   struct vring * vr = &vq->vring;
    vring_init(vr, num, (unsigned char*)&vq->queue);
 
    /* activate the queue
@@ -66,4 +73,20 @@ int vp_find_vq(unsigned int ioaddr, int queue_index,
         ioaddr + VIRTIO_PCI_QUEUE_PFN);
 
    return num;
+
+fail:
+   free(vq);
+   *p_vq = NULL;
+   return -1;
+}
+
+u16 vp_init_simple(u16 bdf)
+{
+    u16 ioaddr = pci_config_readl(bdf, PCI_BASE_ADDRESS_0) &
+        PCI_BASE_ADDRESS_IO_MASK;
+
+    vp_reset(ioaddr);
+    vp_set_status(ioaddr, VIRTIO_CONFIG_S_ACKNOWLEDGE |
+                  VIRTIO_CONFIG_S_DRIVER );
+    return ioaddr;
 }
diff --git a/src/virtio-pci.h b/src/virtio-pci.h
index d21d5a5..e1d972d 100644
--- a/src/virtio-pci.h
+++ b/src/virtio-pci.h
@@ -99,6 +99,7 @@ static inline void vp_del_vq(unsigned int ioaddr, int queue_index)
 }
 
 struct vring_virtqueue;
+u16 vp_init_simple(u16 bdf);
 int vp_find_vq(unsigned int ioaddr, int queue_index,
-               struct vring_virtqueue *vq);
+               struct vring_virtqueue **p_vq);
 #endif /* _VIRTIO_PCI_H_ */
diff --git a/src/virtio-scsi.c b/src/virtio-scsi.c
new file mode 100644
index 0000000..2f6c48a
--- /dev/null
+++ b/src/virtio-scsi.c
@@ -0,0 +1,211 @@
+// Virtio SCSI boot support.
+//
+// Copyright (C) 2011 Red Hat Inc.
+//
+// Authors:
+//  Paolo Bonzini <pbonzini@redhat.com>
+//
+// This file may be distributed under the terms of the GNU LGPLv3 license.
+
+#include "util.h" // dprintf
+#include "pci.h" // foreachpci
+#include "config.h" // CONFIG_*
+#include "biosvar.h" // GET_GLOBAL
+#include "pci_ids.h" // PCI_DEVICE_ID_VIRTIO_BLK
+#include "pci_regs.h" // PCI_VENDOR_ID
+#include "boot.h" // boot_add_hd
+#include "blockcmd.h" // CDB_CMD_WRITE_10, SCSI_TYPE_CDROM
+#include "virtio-pci.h"
+#include "virtio-ring.h"
+#include "virtio-scsi.h"
+#include "disk.h"
+
+struct virtio_lun_s {
+    struct drive_s drive;
+    struct pci_device *pci;
+    struct vring_virtqueue *vq;
+    u16 ioaddr;
+    u16 target;
+    u16 lun;
+};
+
+static int
+virtio_scsi_cmd(u16 ioaddr, struct vring_virtqueue *vq, struct disk_op_s *op,
+                void *cdbcmd, u16 target, u16 lun, u32 len)
+{
+    struct virtio_scsi_req_cmd req;
+    struct virtio_scsi_resp_cmd resp;
+    struct vring_list sg[3];
+
+    memset(&req, 0, sizeof(req));
+    req.lun[0] = 1;
+    req.lun[1] = target;
+    req.lun[2] = (lun >> 8) | 0x40;
+    req.lun[3] = (lun & 0xff);
+    memcpy(req.cdb, cdbcmd, 16);
+
+    int datain = (req.cdb[0] != CDB_CMD_WRITE_10);
+    int data_idx = (datain ? 2 : 1);
+    int out_num = (datain ? 1 : 2);
+    int in_num = (len ? 3 : 2) - out_num;
+
+    sg[0].addr   = MAKE_FLATPTR(GET_SEG(SS), &req);
+    sg[0].length = sizeof(req);
+
+    sg[out_num].addr   = MAKE_FLATPTR(GET_SEG(SS), &resp);
+    sg[out_num].length = sizeof(resp);
+
+    sg[data_idx].addr   = op->buf_fl;
+    sg[data_idx].length = len;
+
+    /* Add to virtqueue and kick host */
+    vring_add_buf(vq, sg, out_num, in_num, 0, 0);
+    vring_kick(ioaddr, vq, 1);
+
+    /* Wait for reply */
+    while (!vring_more_used(vq))
+        usleep(5);
+
+    /* Reclaim virtqueue element */
+    vring_get_buf(vq, NULL);
+
+    /* Clear interrupt status register.  Avoid leaving interrupts stuck if
+     * VRING_AVAIL_F_NO_INTERRUPT was ignored and interrupts were raised.
+     */
+    vp_get_isr(ioaddr);
+
+    if (resp.response == VIRTIO_SCSI_S_OK && resp.status == 0) {
+        return DISK_RET_SUCCESS;
+    }
+    return DISK_RET_EBADTRACK;
+}
+
+int
+virtio_scsi_cmd_data(struct disk_op_s *op, void *cdbcmd, u16 blocksize)
+{
+    struct virtio_lun_s *vlun =
+        container_of(op->drive_g, struct virtio_lun_s, drive);
+
+    return virtio_scsi_cmd(GET_GLOBAL(vlun->ioaddr),
+                           GET_GLOBAL(vlun->vq), op, cdbcmd,
+                           GET_GLOBAL(vlun->target), GET_GLOBAL(vlun->lun),
+                           blocksize * op->count);
+}
+
+static int
+setup_lun_cdrom(struct virtio_lun_s *vlun, char *desc)
+{
+    int prio = bootprio_find_scsi_device(vlun->pci, vlun->target, vlun->lun);
+    boot_add_cd(&vlun->drive, desc, prio);
+    return 0;
+}
+
+static int
+setup_lun_hd(struct virtio_lun_s *vlun, char *desc)
+{
+    if (vlun->drive.blksize != DISK_SECTOR_SIZE) {
+        dprintf(1, "Unsupported block size %d\n", vlun->drive.blksize);
+        return -1;
+    }
+
+    // Register with bcv system.
+    int prio = bootprio_find_scsi_device(vlun->pci, vlun->target, vlun->lun);
+    boot_add_hd(&vlun->drive, desc, prio);
+
+    return 0;
+}
+
+static int
+virtio_scsi_add_lun(struct pci_device *pci, u16 ioaddr,
+                    struct vring_virtqueue *vq, u16 target, u16 lun)
+{
+    struct virtio_lun_s *vlun = malloc_fseg(sizeof(*vlun));
+    if (!vlun) {
+        warn_noalloc();
+        return -1;
+    }
+    memset(vlun, 0, sizeof(*vlun));
+    vlun->drive.type = DTYPE_VIRTIO_SCSI;
+    vlun->drive.cntl_id = pci->bdf;
+    vlun->pci = pci;
+    vlun->ioaddr = ioaddr;
+    vlun->vq = vq;
+    vlun->target = target;
+    vlun->lun = lun;
+
+    int pdt, ret;
+    char *desc = NULL;
+    ret = scsi_init_drive(&vlun->drive, "virtio-scsi", &pdt, &desc);
+    if (ret)
+        goto fail;
+
+    if (pdt == SCSI_TYPE_CDROM)
+        ret = setup_lun_cdrom(vlun, desc);
+    else
+        ret = setup_lun_hd(vlun, desc);
+    if (ret)
+        goto fail;
+    return ret;
+
+fail:
+    free(vlun);
+    return -1;
+}
+
+static int
+virtio_scsi_scan_target(struct pci_device *pci, u16 ioaddr,
+                        struct vring_virtqueue *vq, u16 target)
+{
+    /* TODO: send REPORT LUNS.  For now, only LUN 0 is recognized.  */
+    int ret = virtio_scsi_add_lun(pci, ioaddr, vq, target, 0);
+    return ret < 0 ? ret : 1;
+}
+
+static void
+init_virtio_scsi(struct pci_device *pci)
+{
+    u16 bdf = pci->bdf;
+    dprintf(1, "found virtio-scsi at %x:%x\n", pci_bdf_to_bus(bdf),
+            pci_bdf_to_dev(bdf));
+    struct vring_virtqueue *vq = NULL;
+    u16 ioaddr = vp_init_simple(bdf);
+    if (vp_find_vq(ioaddr, 2, &vq) < 0 ) {
+        if (vq) {
+            dprintf(1, "fail to find vq for virtio-scsi %x:%x\n",
+                    pci_bdf_to_bus(bdf), pci_bdf_to_dev(bdf));
+        }
+        goto fail;
+    }
+
+    int i, tot;
+    for (tot = 0, i = 0; i < 256; i++)
+        tot += virtio_scsi_scan_target(pci, ioaddr, vq, i);
+
+    if (!tot)
+        goto fail;
+
+    vp_set_status(ioaddr, VIRTIO_CONFIG_S_ACKNOWLEDGE |
+                  VIRTIO_CONFIG_S_DRIVER | VIRTIO_CONFIG_S_DRIVER_OK);
+    return;
+
+fail:
+    free(vq);
+}
+
+void
+virtio_scsi_setup(void)
+{
+    ASSERT32FLAT();
+    if (! CONFIG_VIRTIO_SCSI || CONFIG_COREBOOT)
+        return;
+
+    dprintf(3, "init virtio-scsi\n");
+
+    struct pci_device *pci;
+    foreachpci(pci) {
+        if (pci->vendor != PCI_VENDOR_ID_REDHAT_QUMRANET
+            || pci->device != PCI_DEVICE_ID_VIRTIO_SCSI)
+            continue;
+        init_virtio_scsi(pci);
+    }
+}
diff --git a/src/virtio-scsi.h b/src/virtio-scsi.h
new file mode 100644
index 0000000..28d48b0
--- /dev/null
+++ b/src/virtio-scsi.h
@@ -0,0 +1,48 @@
+#ifndef _VIRTIO_SCSI_H
+#define _VIRTIO_SCSI_H
+
+#define VIRTIO_SCSI_CDB_SIZE      32
+#define VIRTIO_SCSI_SENSE_SIZE    96
+
+struct virtio_scsi_config
+{
+    u32 num_queues;
+    u32 seg_max;
+    u32 max_sectors;
+    u32 cmd_per_lun;
+    u32 event_info_size;
+    u32 sense_size;
+    u32 cdb_size;
+    u16 max_channel;
+    u16 max_target;
+    u32 max_lun;
+} __attribute__((packed));
+
+/* This is the first element of the "out" scatter-gather list. */
+struct virtio_scsi_req_cmd {
+    u8 lun[8];
+    u64 id;
+    u8 task_attr;
+    u8 prio;
+    u8 crn;
+    char cdb[VIRTIO_SCSI_CDB_SIZE];
+};
+
+/* This is the first element of the "in" scatter-gather list. */
+struct virtio_scsi_resp_cmd {
+    u32 sense_len;
+    u32 residual;
+    u16 status_qualifier;
+    u8 status;
+    u8 response;
+    u8 sense[VIRTIO_SCSI_SENSE_SIZE];
+};
+
+#define VIRTIO_SCSI_S_OK            0
+
+struct disk_op_s;
+int process_virtio_scsi_op(struct disk_op_s *op);
+int virtio_scsi_cmd_data(struct disk_op_s *op, void *cdbcmd, u16 blocksize);
+void virtio_scsi_setup(void);
+
+#endif /* _VIRTIO_SCSI_H */