1 files changed, 59 insertions, 64 deletions

diff --git a/arch/sparc64/kernel/vio.c b/arch/sparc64/kernel/vio.c
index d487be093b4a..8d3cc4fdb557 100644
--- a/arch/sparc64/kernel/vio.c
+++ b/arch/sparc64/kernel/vio.c
@@ -172,36 +172,6 @@ struct device_node *cdev_node;
 static struct vio_dev *root_vdev;
 static u64 cdev_cfg_handle;
 
-static void vio_add(struct mdesc_handle *hp, u64 node)
-{
-        const char *name = mdesc_get_property(hp, node, "name", NULL);
-        const u64 *id = mdesc_get_property(hp, node, "id", NULL);
-
-        printk(KERN_ERR "VIO: Device add (%s) ID[%lx]\n",
-               name, *id);
-}
-
-static void vio_remove(struct mdesc_handle *hp, u64 node)
-{
-        const char *name = mdesc_get_property(hp, node, "name", NULL);
-        const u64 *id = mdesc_get_property(hp, node, "id", NULL);
-
-        printk(KERN_ERR "VIO: Device remove (%s) ID[%lx]\n",
-               name, *id);
-}
-
-static struct mdesc_notifier_client vio_device_notifier = {
-        .add            = vio_add,
-        .remove         = vio_remove,
-        .node_name      = "virtual-device-port",
-};
-
-static struct mdesc_notifier_client vio_ds_notifier = {
-        .add            = vio_add,
-        .remove         = vio_remove,
-        .node_name      = "domain-services-port",
-};
-
 static void vio_fill_channel_info(struct mdesc_handle *hp, u64 mp,
                                   struct vio_dev *vdev)
 {
@@ -231,10 +201,11 @@ static void vio_fill_channel_info(struct mdesc_handle *hp, u64 mp,
 static struct vio_dev *vio_create_one(struct mdesc_handle *hp, u64 mp,
                                       struct device *parent)
 {
-        const char *type, *compat;
+        const char *type, *compat, *bus_id_name;
         struct device_node *dp;
         struct vio_dev *vdev;
         int err, tlen, clen;
+        const u64 *id;
 
         type = mdesc_get_property(hp, mp, "device-type", &tlen);
         if (!type) {
@@ -250,6 +221,16 @@ static struct vio_dev *vio_create_one(struct mdesc_handle *hp, u64 mp,
                 return NULL;
         }
 
+        bus_id_name = type;
+        if (!strcmp(type, "domain-services-port"))
+                bus_id_name = "ds";
+
+        if (strlen(bus_id_name) >= KOBJ_NAME_LEN - 4) {
+                printk(KERN_ERR "VIO: bus_id_name [%s] is too long.\n",
+                       bus_id_name);
+                return NULL;
+        }
+
         compat = mdesc_get_property(hp, mp, "device-type", &clen);
         if (!compat) {
                 clen = 0;
@@ -279,7 +260,14 @@ static struct vio_dev *vio_create_one(struct mdesc_handle *hp, u64 mp,
 
         vio_fill_channel_info(hp, mp, vdev);
 
-        snprintf(vdev->dev.bus_id, BUS_ID_SIZE, "%lx", mp);
+        id = mdesc_get_property(hp, mp, "id", NULL);
+        if (!id)
+                snprintf(vdev->dev.bus_id, BUS_ID_SIZE, "%s",
+                         bus_id_name);
+        else
+                snprintf(vdev->dev.bus_id, BUS_ID_SIZE, "%s-%lu",
+                         bus_id_name, *id);
+
         vdev->dev.parent = parent;
         vdev->dev.bus = &vio_bus_type;
         vdev->dev.release = vio_dev_release;
@@ -299,6 +287,8 @@ static struct vio_dev *vio_create_one(struct mdesc_handle *hp, u64 mp,
         }
         vdev->dp = dp;
 
+        printk(KERN_ERR "VIO: Adding device %s\n", vdev->dev.bus_id);
+
         err = device_register(&vdev->dev);
         if (err) {
                 printk(KERN_ERR "VIO: Could not register device %s, err=%d\n",
@@ -313,46 +303,46 @@ static struct vio_dev *vio_create_one(struct mdesc_handle *hp, u64 mp,
         return vdev;
 }
 
-static void walk_tree(struct mdesc_handle *hp, u64 n, struct vio_dev *parent)
+static void vio_add(struct mdesc_handle *hp, u64 node)
 {
-        u64 a;
-
-        mdesc_for_each_arc(a, hp, n, MDESC_ARC_TYPE_FWD) {
-                struct vio_dev *vdev;
-                u64 target;
-
-                target = mdesc_arc_target(hp, a);
-                vdev = vio_create_one(hp, target, &parent->dev);
-                if (vdev)
-                        walk_tree(hp, target, vdev);
-        }
+        (void) vio_create_one(hp, node, &root_vdev->dev);
 }
 
-static void create_devices(struct mdesc_handle *hp, u64 root)
+static int vio_md_node_match(struct device *dev, void *arg)
 {
-        u64 mp;
+        struct vio_dev *vdev = to_vio_dev(dev);
 
-        root_vdev = vio_create_one(hp, root, NULL);
-        if (!root_vdev) {
-                printk(KERN_ERR "VIO: Coult not create root device.\n");
-                return;
-        }
+        if (vdev->mp == (u64) arg)
+                return 1;
 
-        walk_tree(hp, root, root_vdev);
+        return 0;
+}
 
-        /* Domain services is odd as it doesn't sit underneath the
-         * channel-devices node, so we plug it in manually.
-         */
-        mp = mdesc_node_by_name(hp, MDESC_NODE_NULL, "domain-services");
-        if (mp != MDESC_NODE_NULL) {
-                struct vio_dev *parent = vio_create_one(hp, mp,
-                                                        &root_vdev->dev);
+static void vio_remove(struct mdesc_handle *hp, u64 node)
+{
+        struct device *dev;
 
-                if (parent)
-                        walk_tree(hp, mp, parent);
+        dev = device_find_child(&root_vdev->dev, (void *) node,
+                                vio_md_node_match);
+        if (dev) {
+                printk(KERN_INFO "VIO: Removing device %s\n", dev->bus_id);
+
+                device_unregister(dev);
         }
 }
 
+static struct mdesc_notifier_client vio_device_notifier = {
+        .add            = vio_add,
+        .remove         = vio_remove,
+        .node_name      = "virtual-device-port",
+};
+
+static struct mdesc_notifier_client vio_ds_notifier = {
+        .add            = vio_add,
+        .remove         = vio_remove,
+        .node_name      = "domain-services-port",
+};
+
 const char *channel_devices_node = "channel-devices";
 const char *channel_devices_compat = "SUNW,sun4v-channel-devices";
 const char *cfg_handle_prop = "cfg-handle";
@@ -411,14 +401,19 @@ static int __init vio_init(void)
 
         cdev_cfg_handle = *cfg_handle;
 
+        root_vdev = vio_create_one(hp, root, NULL);
+        err = -ENODEV;
+        if (!root_vdev) {
+                printk(KERN_ERR "VIO: Coult not create root device.\n");
+                goto out_release;
+        }
+
         mdesc_register_notifier(&vio_device_notifier);
         mdesc_register_notifier(&vio_ds_notifier);
 
-        create_devices(hp, root);
-
         mdesc_release(hp);
 
-        return 0;
+        return err;
 
 out_release:
         mdesc_release(hp);