1 files changed, 59 insertions, 25 deletions

diff --git a/drivers/input/mouse/psmouse-base.c b/drivers/input/mouse/psmouse-base.c
index 6f9b2c7cc9c2..9144df65e703 100644
--- a/drivers/input/mouse/psmouse-base.c
+++ b/drivers/input/mouse/psmouse-base.c
@@ -1102,7 +1102,7 @@ static int psmouse_connect(struct serio *serio, struct serio_driver *drv)
 {
         struct psmouse *psmouse, *parent = NULL;
         struct input_dev *input_dev;
-        int retval = -ENOMEM;
+        int retval = 0, error = -ENOMEM;
 
         mutex_lock(&psmouse_mutex);
 
@@ -1118,7 +1118,7 @@ static int psmouse_connect(struct serio *serio, struct serio_driver *drv)
         psmouse = kzalloc(sizeof(struct psmouse), GFP_KERNEL);
         input_dev = input_allocate_device();
         if (!psmouse || !input_dev)
-                goto out;
+                goto err_free;
 
         ps2_init(&psmouse->ps2dev, serio);
         INIT_WORK(&psmouse->resync_work, psmouse_resync, psmouse);
@@ -1129,14 +1129,13 @@ static int psmouse_connect(struct serio *serio, struct serio_driver *drv)
 
         serio_set_drvdata(serio, psmouse);
 
-        retval = serio_open(serio, drv);
-        if (retval)
-                goto out;
+        error = serio_open(serio, drv);
+        if (error)
+                goto err_clear_drvdata;
 
         if (psmouse_probe(psmouse) < 0) {
-                serio_close(serio);
-                retval = -ENODEV;
-                goto out;
+                error = -ENODEV;
+                goto err_close_serio;
         }
 
         psmouse->rate = psmouse_rate;
@@ -1150,30 +1149,44 @@ static int psmouse_connect(struct serio *serio, struct serio_driver *drv)
         psmouse_set_state(psmouse, PSMOUSE_CMD_MODE);
         psmouse_initialize(psmouse);
 
-        input_register_device(psmouse->dev);
+        error = input_register_device(psmouse->dev);
+        if (error)
+                goto err_protocol_disconnect;
 
         if (parent && parent->pt_activate)
                 parent->pt_activate(parent);
 
-        sysfs_create_group(&serio->dev.kobj, &psmouse_attribute_group);
+        error = sysfs_create_group(&serio->dev.kobj, &psmouse_attribute_group);
+        if (error)
+                goto err_pt_deactivate;
 
         psmouse_activate(psmouse);
 
-        retval = 0;
-
-out:
-        if (retval) {
-                serio_set_drvdata(serio, NULL);
-                input_free_device(input_dev);
-                kfree(psmouse);
-        }
-
+ out:
         /* If this is a pass-through port the parent needs to be re-activated */
         if (parent)
                 psmouse_activate(parent);
 
         mutex_unlock(&psmouse_mutex);
         return retval;
+
+ err_pt_deactivate:
+        if (parent && parent->pt_deactivate)
+                parent->pt_deactivate(parent);
+ err_protocol_disconnect:
+        if (psmouse->disconnect)
+                psmouse->disconnect(psmouse);
+        psmouse_set_state(psmouse, PSMOUSE_IGNORE);
+ err_close_serio:
+        serio_close(serio);
+ err_clear_drvdata:
+        serio_set_drvdata(serio, NULL);
+ err_free:
+        input_free_device(input_dev);
+        kfree(psmouse);
+
+        retval = error;
+        goto out;
 }
 
 
@@ -1365,17 +1378,20 @@ static ssize_t psmouse_attr_set_protocol(struct psmouse *psmouse, void *data, co
 {
         struct serio *serio = psmouse->ps2dev.serio;
         struct psmouse *parent = NULL;
-        struct input_dev *new_dev;
-        const struct psmouse_protocol *proto;
+        struct input_dev *old_dev, *new_dev;
+        const struct psmouse_protocol *proto, *old_proto;
+        int error;
         int retry = 0;
 
-        if (!(proto = psmouse_protocol_by_name(buf, count)))
+        proto = psmouse_protocol_by_name(buf, count);
+        if (!proto)
                 return -EINVAL;
 
         if (psmouse->type == proto->type)
                 return count;
 
-        if (!(new_dev = input_allocate_device()))
+        new_dev = input_allocate_device();
+        if (!new_dev)
                 return -ENOMEM;
 
         while (serio->child) {
@@ -1408,11 +1424,13 @@ static ssize_t psmouse_attr_set_protocol(struct psmouse *psmouse, void *data, co
                         parent->pt_deactivate(parent);
         }
 
+        old_dev = psmouse->dev;
+        old_proto = psmouse_protocol_by_type(psmouse->type);
+
         if (psmouse->disconnect)
                 psmouse->disconnect(psmouse);
 
         psmouse_set_state(psmouse, PSMOUSE_IGNORE);
-        input_unregister_device(psmouse->dev);
 
         psmouse->dev = new_dev;
         psmouse_set_state(psmouse, PSMOUSE_INITIALIZING);
@@ -1426,7 +1444,23 @@ static ssize_t psmouse_attr_set_protocol(struct psmouse *psmouse, void *data, co
         psmouse_initialize(psmouse);
         psmouse_set_state(psmouse, PSMOUSE_CMD_MODE);
 
-        input_register_device(psmouse->dev);
+        error = input_register_device(psmouse->dev);
+        if (error) {
+                if (psmouse->disconnect)
+                        psmouse->disconnect(psmouse);
+
+                psmouse_set_state(psmouse, PSMOUSE_IGNORE);
+                input_free_device(new_dev);
+                psmouse->dev = old_dev;
+                psmouse_set_state(psmouse, PSMOUSE_INITIALIZING);
+                psmouse_switch_protocol(psmouse, old_proto);
+                psmouse_initialize(psmouse);
+                psmouse_set_state(psmouse, PSMOUSE_CMD_MODE);
+
+                return error;
+        }
+
+        input_unregister_device(old_dev);
 
         if (parent && parent->pt_activate)
                 parent->pt_activate(parent);