1 files changed, 97 insertions, 26 deletions

diff --git a/arch/x86/kvm/i8259.c b/arch/x86/kvm/i8259.c
index 19fe855e7953..cac4746d7ffb 100644
--- a/arch/x86/kvm/i8259.c
+++ b/arch/x86/kvm/i8259.c
@@ -34,6 +34,9 @@
 #include <linux/kvm_host.h>
 #include "trace.h"
 
+#define pr_pic_unimpl(fmt, ...) \
+        pr_err_ratelimited("kvm: pic: " fmt, ## __VA_ARGS__)
+
 static void pic_irq_request(struct kvm *kvm, int level);
 
 static void pic_lock(struct kvm_pic *s)
@@ -306,10 +309,10 @@ static void pic_ioport_write(void *opaque, u32 addr, u32 val)
                         }
                         s->init_state = 1;
                         if (val & 0x02)
-                                printk(KERN_ERR "single mode not supported");
+                                pr_pic_unimpl("single mode not supported");
                         if (val & 0x08)
-                                printk(KERN_ERR
-                                       "level sensitive irq not supported");
+                                pr_pic_unimpl(
+                                        "level sensitive irq not supported");
                 } else if (val & 0x08) {
                         if (val & 0x04)
                                 s->poll = 1;
@@ -459,22 +462,15 @@ static int picdev_in_range(gpa_t addr)
         }
 }
 
-static inline struct kvm_pic *to_pic(struct kvm_io_device *dev)
-{
-        return container_of(dev, struct kvm_pic, dev);
-}
-
-static int picdev_write(struct kvm_io_device *this,
+static int picdev_write(struct kvm_pic *s,
                          gpa_t addr, int len, const void *val)
 {
-        struct kvm_pic *s = to_pic(this);
         unsigned char data = *(unsigned char *)val;
         if (!picdev_in_range(addr))
                 return -EOPNOTSUPP;
 
         if (len != 1) {
-                if (printk_ratelimit())
-                        printk(KERN_ERR "PIC: non byte write\n");
+                pr_pic_unimpl("non byte write\n");
                 return 0;
         }
         pic_lock(s);
@@ -494,17 +490,15 @@ static int picdev_write(struct kvm_io_device *this,
         return 0;
 }
 
-static int picdev_read(struct kvm_io_device *this,
+static int picdev_read(struct kvm_pic *s,
                        gpa_t addr, int len, void *val)
 {
-        struct kvm_pic *s = to_pic(this);
         unsigned char data = 0;
         if (!picdev_in_range(addr))
                 return -EOPNOTSUPP;
 
         if (len != 1) {
-                if (printk_ratelimit())
-                        printk(KERN_ERR "PIC: non byte read\n");
+                pr_pic_unimpl("non byte read\n");
                 return 0;
         }
         pic_lock(s);
@@ -525,6 +519,48 @@ static int picdev_read(struct kvm_io_device *this,
         return 0;
 }
 
+static int picdev_master_write(struct kvm_io_device *dev,
+                               gpa_t addr, int len, const void *val)
+{
+        return picdev_write(container_of(dev, struct kvm_pic, dev_master),
+                            addr, len, val);
+}
+
+static int picdev_master_read(struct kvm_io_device *dev,
+                              gpa_t addr, int len, void *val)
+{
+        return picdev_read(container_of(dev, struct kvm_pic, dev_master),
+                            addr, len, val);
+}
+
+static int picdev_slave_write(struct kvm_io_device *dev,
+                              gpa_t addr, int len, const void *val)
+{
+        return picdev_write(container_of(dev, struct kvm_pic, dev_slave),
+                            addr, len, val);
+}
+
+static int picdev_slave_read(struct kvm_io_device *dev,
+                             gpa_t addr, int len, void *val)
+{
+        return picdev_read(container_of(dev, struct kvm_pic, dev_slave),
+                            addr, len, val);
+}
+
+static int picdev_eclr_write(struct kvm_io_device *dev,
+                             gpa_t addr, int len, const void *val)
+{
+        return picdev_write(container_of(dev, struct kvm_pic, dev_eclr),
+                            addr, len, val);
+}
+
+static int picdev_eclr_read(struct kvm_io_device *dev,
+                            gpa_t addr, int len, void *val)
+{
+        return picdev_read(container_of(dev, struct kvm_pic, dev_eclr),
+                            addr, len, val);
+}
+
 /*
  * callback when PIC0 irq status changed
  */
@@ -537,9 +573,19 @@ static void pic_irq_request(struct kvm *kvm, int level)
         s->output = level;
 }
 
-static const struct kvm_io_device_ops picdev_ops = {
-        .read     = picdev_read,
-        .write    = picdev_write,
+static const struct kvm_io_device_ops picdev_master_ops = {
+        .read     = picdev_master_read,
+        .write    = picdev_master_write,
+};
+
+static const struct kvm_io_device_ops picdev_slave_ops = {
+        .read     = picdev_slave_read,
+        .write    = picdev_slave_write,
+};
+
+static const struct kvm_io_device_ops picdev_eclr_ops = {
+        .read     = picdev_eclr_read,
+        .write    = picdev_eclr_write,
 };
 
 struct kvm_pic *kvm_create_pic(struct kvm *kvm)
@@ -560,16 +606,39 @@ struct kvm_pic *kvm_create_pic(struct kvm *kvm)
         /*
          * Initialize PIO device
          */
-        kvm_iodevice_init(&s->dev, &picdev_ops);
+        kvm_iodevice_init(&s->dev_master, &picdev_master_ops);
+        kvm_iodevice_init(&s->dev_slave, &picdev_slave_ops);
+        kvm_iodevice_init(&s->dev_eclr, &picdev_eclr_ops);
         mutex_lock(&kvm->slots_lock);
-        ret = kvm_io_bus_register_dev(kvm, KVM_PIO_BUS, &s->dev);
+        ret = kvm_io_bus_register_dev(kvm, KVM_PIO_BUS, 0x20, 2,
+                                      &s->dev_master);
+        if (ret < 0)
+                goto fail_unlock;
+
+        ret = kvm_io_bus_register_dev(kvm, KVM_PIO_BUS, 0xa0, 2, &s->dev_slave);
+        if (ret < 0)
+                goto fail_unreg_2;
+
+        ret = kvm_io_bus_register_dev(kvm, KVM_PIO_BUS, 0x4d0, 2, &s->dev_eclr);
+        if (ret < 0)
+                goto fail_unreg_1;
+
         mutex_unlock(&kvm->slots_lock);
-        if (ret < 0) {
-                kfree(s);
-                return NULL;
-        }
 
         return s;
+
+fail_unreg_1:
+        kvm_io_bus_unregister_dev(kvm, KVM_PIO_BUS, &s->dev_slave);
+
+fail_unreg_2:
+        kvm_io_bus_unregister_dev(kvm, KVM_PIO_BUS, &s->dev_master);
+
+fail_unlock:
+        mutex_unlock(&kvm->slots_lock);
+
+        kfree(s);
+
+        return NULL;
 }
 
 void kvm_destroy_pic(struct kvm *kvm)
@@ -577,7 +646,9 @@ void kvm_destroy_pic(struct kvm *kvm)
         struct kvm_pic *vpic = kvm->arch.vpic;
 
         if (vpic) {
-                kvm_io_bus_unregister_dev(kvm, KVM_PIO_BUS, &vpic->dev);
+                kvm_io_bus_unregister_dev(kvm, KVM_PIO_BUS, &vpic->dev_master);
+                kvm_io_bus_unregister_dev(kvm, KVM_PIO_BUS, &vpic->dev_slave);
+                kvm_io_bus_unregister_dev(kvm, KVM_PIO_BUS, &vpic->dev_eclr);
                 kvm->arch.vpic = NULL;
                 kfree(vpic);
         }