1 files changed, 51 insertions, 42 deletions

diff --git a/drivers/lguest/lguest_device.c b/drivers/lguest/lguest_device.c
index 2bc9bf7e88e5..1a8de57289eb 100644
--- a/drivers/lguest/lguest_device.c
+++ b/drivers/lguest/lguest_device.c
@@ -20,14 +20,11 @@
 /* The pointer to our (page) of device descriptions. */
 static void *lguest_devices;
 
-/* Unique numbering for lguest devices. */
-static unsigned int dev_index;
-
 /* For Guests, device memory can be used as normal memory, so we cast away the
  * __iomem to quieten sparse. */
 static inline void *lguest_map(unsigned long phys_addr, unsigned long pages)
 {
-        return (__force void *)ioremap(phys_addr, PAGE_SIZE*pages);
+        return (__force void *)ioremap_cache(phys_addr, PAGE_SIZE*pages);
 }
 
 static inline void lguest_unmap(void *addr)
@@ -85,27 +82,34 @@ static unsigned desc_size(const struct lguest_device_desc *desc)
                 + desc->config_len;
 }
 
-/* This tests (and acknowleges) a feature bit. */
-static bool lg_feature(struct virtio_device *vdev, unsigned fbit)
+/* This gets the device's feature bits. */
+static u32 lg_get_features(struct virtio_device *vdev)
 {
+        unsigned int i;
+        u32 features = 0;
         struct lguest_device_desc *desc = to_lgdev(vdev)->desc;
-        u8 *features;
-
-        /* Obviously if they ask for a feature off the end of our feature
-         * bitmap, it's not set. */
-        if (fbit / 8 > desc->feature_len)
-                return false;
-
-        /* The feature bitmap comes after the virtqueues. */
-        features = lg_features(desc);
-        if (!(features[fbit / 8] & (1 << (fbit % 8))))
-                return false;
-
-        /* We set the matching bit in the other half of the bitmap to tell the
-         * Host we want to use this feature.  We don't use this yet, but we
-         * could in future. */
-        features[desc->feature_len + fbit / 8] |= (1 << (fbit % 8));
-        return true;
+        u8 *in_features = lg_features(desc);
+
+        /* We do this the slow but generic way. */
+        for (i = 0; i < min(desc->feature_len * 8, 32); i++)
+                if (in_features[i / 8] & (1 << (i % 8)))
+                        features |= (1 << i);
+
+        return features;
+}
+
+static void lg_set_features(struct virtio_device *vdev, u32 features)
+{
+        unsigned int i;
+        struct lguest_device_desc *desc = to_lgdev(vdev)->desc;
+        /* Second half of bitmap is features we accept. */
+        u8 *out_features = lg_features(desc) + desc->feature_len;
+
+        memset(out_features, 0, desc->feature_len);
+        for (i = 0; i < min(desc->feature_len * 8, 32); i++) {
+                if (features & (1 << i))
+                        out_features[i / 8] |= (1 << (i % 8));
+        }
 }
 
 /* Once they've found a field, getting a copy of it is easy. */
@@ -137,20 +141,26 @@ static u8 lg_get_status(struct virtio_device *vdev)
         return to_lgdev(vdev)->desc->status;
 }
 
+/* To notify on status updates, we (ab)use the NOTIFY hypercall, with the
+ * descriptor address of the device.  A zero status means "reset". */
+static void set_status(struct virtio_device *vdev, u8 status)
+{
+        unsigned long offset = (void *)to_lgdev(vdev)->desc - lguest_devices;
+
+        /* We set the status. */
+        to_lgdev(vdev)->desc->status = status;
+        hcall(LHCALL_NOTIFY, (max_pfn<<PAGE_SHIFT) + offset, 0, 0);
+}
+
 static void lg_set_status(struct virtio_device *vdev, u8 status)
 {
         BUG_ON(!status);
-        to_lgdev(vdev)->desc->status = status;
+        set_status(vdev, status);
 }
 
-/* To reset the device, we (ab)use the NOTIFY hypercall, with the descriptor
- * address of the device.  The Host will zero the status and all the
- * features. */
 static void lg_reset(struct virtio_device *vdev)
 {
-        unsigned long offset = (void *)to_lgdev(vdev)->desc - lguest_devices;
-
-        hcall(LHCALL_NOTIFY, (max_pfn<<PAGE_SHIFT) + offset, 0, 0);
+        set_status(vdev, 0);
 }
 
 /*
@@ -286,7 +296,8 @@ static void lg_del_vq(struct virtqueue *vq)
 
 /* The ops structure which hooks everything together. */
 static struct virtio_config_ops lguest_config_ops = {
-        .feature = lg_feature,
+        .get_features = lg_get_features,
+        .set_features = lg_set_features,
         .get = lg_get,
         .set = lg_set,
         .get_status = lg_get_status,
@@ -311,8 +322,10 @@ static struct device lguest_root = {
  * As Andrew Tridgell says, "Untested code is buggy code".
  *
  * It's worth reading this carefully: we start with a pointer to the new device
- * descriptor in the "lguest_devices" page. */
-static void add_lguest_device(struct lguest_device_desc *d)
+ * descriptor in the "lguest_devices" page, and the offset into the device
+ * descriptor page so we can uniquely identify it if things go badly wrong. */
+static void add_lguest_device(struct lguest_device_desc *d,
+                              unsigned int offset)
 {
         struct lguest_device *ldev;
 
@@ -320,18 +333,14 @@ static void add_lguest_device(struct lguest_device_desc *d)
          * it. */
         ldev = kzalloc(sizeof(*ldev), GFP_KERNEL);
         if (!ldev) {
-                printk(KERN_EMERG "Cannot allocate lguest dev %u\n",
-                       dev_index++);
+                printk(KERN_EMERG "Cannot allocate lguest dev %u type %u\n",
+                       offset, d->type);
                 return;
         }
 
         /* This devices' parent is the lguest/ dir. */
         ldev->vdev.dev.parent = &lguest_root;
         /* We have a unique device index thanks to the dev_index counter. */
-        ldev->vdev.index = dev_index++;
-        /* The device type comes straight from the descriptor.  There's also a
-         * device vendor field in the virtio_device struct, which we leave as
-         * 0. */
         ldev->vdev.id.device = d->type;
         /* We have a simple set of routines for querying the device's
          * configuration information and setting its status. */
@@ -343,8 +352,8 @@ static void add_lguest_device(struct lguest_device_desc *d)
          * virtio_device and calls device_register().  This makes the bus
          * infrastructure look for a matching driver. */
         if (register_virtio_device(&ldev->vdev) != 0) {
-                printk(KERN_ERR "Failed to register lguest device %u\n",
-                       ldev->vdev.index);
+                printk(KERN_ERR "Failed to register lguest dev %u type %u\n",
+                       offset, d->type);
                 kfree(ldev);
         }
 }
@@ -365,7 +374,7 @@ static void scan_devices(void)
                         break;
 
                 printk("Device at %i has size %u\n", i, desc_size(d));
-                add_lguest_device(d);
+                add_lguest_device(d, i);
         }
 }