1 files changed, 71 insertions, 8 deletions

diff --git a/drivers/virtio/virtio_mmio.c b/drivers/virtio/virtio_mmio.c
index 9c877d2375a5..6010d7ec0a0f 100644
--- a/drivers/virtio/virtio_mmio.c
+++ b/drivers/virtio/virtio_mmio.c
@@ -156,22 +156,85 @@ static void vm_get(struct virtio_device *vdev, unsigned offset,
                    void *buf, unsigned len)
 {
         struct virtio_mmio_device *vm_dev = to_virtio_mmio_device(vdev);
-        u8 *ptr = buf;
-        int i;
+        void __iomem *base = vm_dev->base + VIRTIO_MMIO_CONFIG;
+        u8 b;
+        __le16 w;
+        __le32 l;
 
-        for (i = 0; i < len; i++)
-                ptr[i] = readb(vm_dev->base + VIRTIO_MMIO_CONFIG + offset + i);
+        if (vm_dev->version == 1) {
+                u8 *ptr = buf;
+                int i;
+
+                for (i = 0; i < len; i++)
+                        ptr[i] = readb(base + offset + i);
+                return;
+        }
+
+        switch (len) {
+        case 1:
+                b = readb(base + offset);
+                memcpy(buf, &b, sizeof b);
+                break;
+        case 2:
+                w = cpu_to_le16(readw(base + offset));
+                memcpy(buf, &w, sizeof w);
+                break;
+        case 4:
+                l = cpu_to_le32(readl(base + offset));
+                memcpy(buf, &l, sizeof l);
+                break;
+        case 8:
+                l = cpu_to_le32(readl(base + offset));
+                memcpy(buf, &l, sizeof l);
+                l = cpu_to_le32(ioread32(base + offset + sizeof l));
+                memcpy(buf + sizeof l, &l, sizeof l);
+                break;
+        default:
+                BUG();
+        }
 }
 
 static void vm_set(struct virtio_device *vdev, unsigned offset,
                    const void *buf, unsigned len)
 {
         struct virtio_mmio_device *vm_dev = to_virtio_mmio_device(vdev);
-        const u8 *ptr = buf;
-        int i;
+        void __iomem *base = vm_dev->base + VIRTIO_MMIO_CONFIG;
+        u8 b;
+        __le16 w;
+        __le32 l;
 
-        for (i = 0; i < len; i++)
-                writeb(ptr[i], vm_dev->base + VIRTIO_MMIO_CONFIG + offset + i);
+        if (vm_dev->version == 1) {
+                const u8 *ptr = buf;
+                int i;
+
+                for (i = 0; i < len; i++)
+                        writeb(ptr[i], base + offset + i);
+
+                return;
+        }
+
+        switch (len) {
+        case 1:
+                memcpy(&b, buf, sizeof b);
+                writeb(b, base + offset);
+                break;
+        case 2:
+                memcpy(&w, buf, sizeof w);
+                writew(le16_to_cpu(w), base + offset);
+                break;
+        case 4:
+                memcpy(&l, buf, sizeof l);
+                writel(le32_to_cpu(l), base + offset);
+                break;
+        case 8:
+                memcpy(&l, buf, sizeof l);
+                writel(le32_to_cpu(l), base + offset);
+                memcpy(&l, buf + sizeof l, sizeof l);
+                writel(le32_to_cpu(l), base + offset + sizeof l);
+                break;
+        default:
+                BUG();
+        }
 }
 
 static u32 vm_generation(struct virtio_device *vdev)