Input: mice - handle errors when registering input devices

Signed-off-by: Dmitry Torokhov <dtor@mail.ru>

1 files changed, 59 insertions, 25 deletions

diff --git a/drivers/input/mouse/psmouse-base.c b/drivers/input/mouse/psmouse-base.c
index 6f9b2c7..9144df6 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);