5 files changed, 84 insertions, 1 deletions

diff --git a/src/boot.c b/src/boot.c
index f2f084be..cc87b1d9 100644
--- a/src/boot.c
+++ b/src/boot.c
@@ -316,6 +316,15 @@ int bootprio_find_pci_device(struct pci_device *pci)
     return find_prio(desc);
 }
 
+int bootprio_find_mmio_device(void *mmio)
+{
+    if (!CONFIG_BOOTORDER)
+        return -1;
+    char desc[256];
+    snprintf(desc, sizeof(desc), "/virtio-mmio@%016x/*", (u32)mmio);
+    return find_prio(desc);
+}
+
 int bootprio_find_scsi_device(struct pci_device *pci, int target, int lun)
 {
     if (!CONFIG_BOOTORDER)
diff --git a/src/hw/virtio-blk.c b/src/hw/virtio-blk.c
index a5e28fc8..3b198965 100644
--- a/src/hw/virtio-blk.c
+++ b/src/hw/virtio-blk.c
@@ -20,6 +20,7 @@
 #include "string.h" // memset
 #include "util.h" // usleep, bootprio_find_pci_device, is_bootprio_strict
 #include "virtio-pci.h"
+#include "virtio-mmio.h"
 #include "virtio-ring.h"
 #include "virtio-blk.h"
 
@@ -194,6 +195,76 @@ fail:
 }
 
 void
+init_virtio_blk_mmio(void *mmio)
+{
+    u8 status = VIRTIO_CONFIG_S_ACKNOWLEDGE | VIRTIO_CONFIG_S_DRIVER;
+    dprintf(1, "found virtio-blk-mmio at %p\n", mmio);
+    struct virtiodrive_s *vdrive = malloc_low(sizeof(*vdrive));
+    if (!vdrive) {
+        warn_noalloc();
+        return;
+    }
+    memset(vdrive, 0, sizeof(*vdrive));
+    vdrive->drive.type = DTYPE_VIRTIO_BLK;
+    vdrive->drive.cntl_id = (u32)mmio;
+
+    vp_init_mmio(&vdrive->vp, mmio);
+    if (vp_find_vq(&vdrive->vp, 0, &vdrive->vq) < 0 ) {
+        dprintf(1, "fail to find vq for virtio-blk-mmio %p\n", mmio);
+        goto fail;
+    }
+
+    struct vp_device *vp = &vdrive->vp;
+    u64 features = vp_get_features(vp);
+    u64 version1 = 1ull << VIRTIO_F_VERSION_1;
+    u64 blk_size = 1ull << VIRTIO_BLK_F_BLK_SIZE;
+
+    features = features & (version1 | blk_size);
+    vp_set_features(vp, features);
+    status |= VIRTIO_CONFIG_S_FEATURES_OK;
+    vp_set_status(vp, status);
+    if (!(vp_get_status(vp) & VIRTIO_CONFIG_S_FEATURES_OK)) {
+        dprintf(1, "device didn't accept features: %p\n", mmio);
+        goto fail;
+    }
+
+    vdrive->drive.sectors =
+        vp_read(&vp->device, struct virtio_blk_config, capacity);
+    if (features & blk_size) {
+        vdrive->drive.blksize =
+            vp_read(&vp->device, struct virtio_blk_config, blk_size);
+    } else {
+        vdrive->drive.blksize = DISK_SECTOR_SIZE;
+    }
+    if (vdrive->drive.blksize != DISK_SECTOR_SIZE) {
+        dprintf(1, "virtio-blk-mmio %p block size %d is unsupported\n",
+                mmio, vdrive->drive.blksize);
+        goto fail;
+    }
+    dprintf(1, "virtio-blk-mmio %p blksize=%d sectors=%u\n",
+            mmio, vdrive->drive.blksize, (u32)vdrive->drive.sectors);
+
+    vdrive->drive.pchs.cylinder =
+        vp_read(&vp->device, struct virtio_blk_config, cylinders);
+    vdrive->drive.pchs.head =
+        vp_read(&vp->device, struct virtio_blk_config, heads);
+    vdrive->drive.pchs.sector =
+        vp_read(&vp->device, struct virtio_blk_config, sectors);
+
+    char *desc = znprintf(MAXDESCSIZE, "Virtio disk mmio:%p", mmio);
+    boot_add_hd(&vdrive->drive, desc, bootprio_find_mmio_device(mmio));
+
+    status |= VIRTIO_CONFIG_S_DRIVER_OK;
+    vp_set_status(&vdrive->vp, status);
+    return;
+
+fail:
+    vp_reset(&vdrive->vp);
+    free(vdrive->vq);
+    free(vdrive);
+}
+
+void
 virtio_blk_setup(void)
 {
     u8 skip_nonbootable = is_bootprio_strict();
diff --git a/src/hw/virtio-blk.h b/src/hw/virtio-blk.h
index 157bed62..d20461a2 100644
--- a/src/hw/virtio-blk.h
+++ b/src/hw/virtio-blk.h
@@ -39,5 +39,6 @@ struct virtio_blk_outhdr {
 struct disk_op_s;
 int virtio_blk_process_op(struct disk_op_s *op);
 void virtio_blk_setup(void);
+void init_virtio_blk_mmio(void *mmio);
 
 #endif /* _VIRTIO_BLK_H */
diff --git a/src/hw/virtio-mmio.c b/src/hw/virtio-mmio.c
index daca8a09..adb28f73 100644
--- a/src/hw/virtio-mmio.c
+++ b/src/hw/virtio-mmio.c
@@ -4,6 +4,7 @@
 #include "stacks.h" // run_thread
 #include "string.h" // memset
 #include "virtio-pci.h"
+#include "virtio-blk.h"
 #include "virtio-scsi.h"
 #include "virtio-ring.h"
 #include "virtio-mmio.h"
@@ -35,7 +36,7 @@ void virtio_mmio_setup_one(u64 addr)
             addr, devid, version == 1 ? " (legacy)" : "");
     switch (devid) {
     case 2: /* blk */
-        /* TODO */
+        run_thread(init_virtio_blk_mmio, mmio);
         break;
     case 8: /* scsi */
         run_thread(init_virtio_scsi_mmio, mmio);
diff --git a/src/util.h b/src/util.h
index 1c82e09e..4f27fc30 100644
--- a/src/util.h
+++ b/src/util.h
@@ -30,6 +30,7 @@ void bcv_prepboot(void);
 u8 is_bootprio_strict(void);
 struct pci_device;
 int bootprio_find_pci_device(struct pci_device *pci);
+int bootprio_find_mmio_device(void *mmio);
 int bootprio_find_scsi_device(struct pci_device *pci, int target, int lun);
 int bootprio_find_scsi_mmio_device(void *mmio, int target, int lun);
 int bootprio_find_ata_device(struct pci_device *pci, int chanid, int slave);