1 files changed, 36 insertions, 2 deletions

diff --git a/drivers/virtio/virtio.c b/drivers/virtio/virtio.c
index b535483bc556..13866789b356 100644
--- a/drivers/virtio/virtio.c
+++ b/drivers/virtio/virtio.c
@@ -80,19 +80,51 @@ static void add_status(struct virtio_device *dev, unsigned status)
         dev->config->set_status(dev, dev->config->get_status(dev) | status);
 }
 
+void virtio_check_driver_offered_feature(const struct virtio_device *vdev,
+                                         unsigned int fbit)
+{
+        unsigned int i;
+        struct virtio_driver *drv = container_of(vdev->dev.driver,
+                                                 struct virtio_driver, driver);
+
+        for (i = 0; i < drv->feature_table_size; i++)
+                if (drv->feature_table[i] == fbit)
+                        return;
+        BUG();
+}
+EXPORT_SYMBOL_GPL(virtio_check_driver_offered_feature);
+
 static int virtio_dev_probe(struct device *_d)
 {
-        int err;
+        int err, i;
         struct virtio_device *dev = container_of(_d,struct virtio_device,dev);
         struct virtio_driver *drv = container_of(dev->dev.driver,
                                                  struct virtio_driver, driver);
+        u32 device_features;
 
+        /* We have a driver! */
         add_status(dev, VIRTIO_CONFIG_S_DRIVER);
+
+        /* Figure out what features the device supports. */
+        device_features = dev->config->get_features(dev);
+
+        /* Features supported by both device and driver into dev->features. */
+        memset(dev->features, 0, sizeof(dev->features));
+        for (i = 0; i < drv->feature_table_size; i++) {
+                unsigned int f = drv->feature_table[i];
+                BUG_ON(f >= 32);
+                if (device_features & (1 << f))
+                        set_bit(f, dev->features);
+        }
+
         err = drv->probe(dev);
         if (err)
                 add_status(dev, VIRTIO_CONFIG_S_FAILED);
-        else
+        else {
                 add_status(dev, VIRTIO_CONFIG_S_DRIVER_OK);
+                /* They should never have set feature bits beyond 32 */
+                dev->config->set_features(dev, dev->features[0]);
+        }
         return err;
 }
 
@@ -114,6 +146,8 @@ static int virtio_dev_remove(struct device *_d)
 
 int register_virtio_driver(struct virtio_driver *driver)
 {
+        /* Catch this early. */
+        BUG_ON(driver->feature_table_size && !driver->feature_table);
         driver->driver.bus = &virtio_bus;
         driver->driver.probe = virtio_dev_probe;
         driver->driver.remove = virtio_dev_remove;