Input: resume support for i8042 (atkbd & psmouse)

diff -Nru a/drivers/input/serio/i8042.c b/drivers/input/serio/i8042.c
@@ -17,6 +17,7 @@
 #include <linux/config.h>
 #include <linux/reboot.h>
 #include <linux/init.h>
+#include <linux/sysdev.h>
 #include <linux/serio.h>
 
 #include <asm/io.h>

@@ -61,6 +62,8 @@
 static unsigned char i8042_mux_open;
 struct timer_list i8042_timer;
 
+static int i8042_sysdev_initialized;
+
 /*
  * Shared IRQ's require a device pointer, but this driver doesn't support
  * multiple devices

@@ -214,16 +217,35 @@
 }
 
 /*
- * i8042_open() is called when a port is open by the higher layer.
- * It allocates the interrupt and enables it in the chip.
+ * i8042_activate_port() enables port on a chip.
  */
 
-static int i8042_open(struct serio *port)
+static int i8042_activate_port(struct serio *port)
 {
 	struct i8042_values *values = port->driver;
 
 	i8042_flush();
 
+	i8042_ctr |= values->irqen;
+
+	if (i8042_command(&i8042_ctr, I8042_CMD_CTL_WCTR)) {
+		i8042_ctr &= ~values->irqen;
+		return -1;
+	}
+
+	return 0;
+}
+
+
+/*
+ * i8042_open() is called when a port is open by the higher layer.
+ * It allocates the interrupt and calls i8042_enable_port.
+ */
+
+static int i8042_open(struct serio *port)
+{
+	struct i8042_values *values = port->driver;
+
 	if (values->mux != -1)
 		if (i8042_mux_open++)
 			return 0;

@@ -234,10 +256,8 @@
 		goto irq_fail;
 	}
 
-	i8042_ctr |= values->irqen;
-
-	if (i8042_command(&i8042_ctr, I8042_CMD_CTL_WCTR)) {
-		printk(KERN_ERR "i8042.c: Can't write CTR while opening %s, unregistering the port\n", values->name);
+	if (i8042_activate_port(port)) {
+		printk(KERN_ERR "i8042.c: Can't activate %s, unregistering the port\n", values->name);
 		goto ctr_fail;
 	}
 

@@ -246,7 +266,6 @@
 	return 0;
 
 ctr_fail:
-	i8042_ctr &= ~values->irqen;
 	free_irq(values->irq, i8042_request_irq_cookie);
 
 irq_fail:

@@ -406,7 +425,7 @@
  * desired.
  */
 	
-static int __init i8042_controller_init(void)
+static int i8042_controller_init(void)
 {
 
 /*

@@ -541,6 +560,37 @@
 }
 
 /*
+ * Here we try to reset everything back to a state in which suspended
+ */
+
+static int i8042_controller_resume(struct sys_device *dev)
+{
+	int i;
+
+	if (i8042_controller_init()) {
+		printk(KERN_ERR "i8042: resume failed\n");
+		return -1;
+	}
+
+/*
+ * Reconnect anything that was connected to the ports.
+ */
+
+	if (i8042_kbd_values.exists && i8042_activate_port(&i8042_kbd_port) == 0)
+		serio_reconnect(&i8042_kbd_port);
+
+	if (i8042_aux_values.exists && i8042_activate_port(&i8042_aux_port) == 0)
+		serio_reconnect(&i8042_aux_port);
+
+	for (i = 0; i < 4; i++)
+		if (i8042_mux_values[i].exists && i8042_activate_port(i8042_mux_port + i) == 0)
+			serio_reconnect(i8042_mux_port + i);
+
+	return 0;
+}
+
+
+/*
  * i8042_check_mux() checks whether the controller supports the PS/2 Active
  * Multiplexing specification by Synaptics, Phoenix, Insyde and
  * LCS/Telegraphics.

@@ -791,6 +841,16 @@
 	values->mux = index;
 }
 
+static struct sysdev_class kbc_sysclass = {
+       set_kset_name("i8042"),
+       .resume = i8042_controller_resume,
+};
+
+static struct sys_device device_i8042 = {
+       .id     = 0,
+       .cls    = &kbc_sysclass,
+};
+
 int __init i8042_init(void)
 {
 	int i;

@@ -825,6 +885,13 @@
 	i8042_timer.function = i8042_timer_func;
 	mod_timer(&i8042_timer, jiffies + I8042_POLL_PERIOD);
 
+        if (sysdev_class_register(&kbc_sysclass) == 0) {
+                if (sys_device_register(&device_i8042) == 0)
+			i8042_sysdev_initialized = 1;
+		else
+			sysdev_class_unregister(&kbc_sysclass);
+        }
+
 	register_reboot_notifier(&i8042_notifier);
 
 	return 0;

@@ -835,6 +902,11 @@
 	int i;
 
 	unregister_reboot_notifier(&i8042_notifier);
+
+	if (i8042_sysdev_initialized) {
+		sys_device_unregister(&device_i8042);
+		sysdev_class_unregister(&kbc_sysclass);
+	}
 
 	del_timer(&i8042_timer);
 

diff -Nru a/drivers/input/keyboard/atkbd.c b/drivers/input/keyboard/atkbd.c
@@ -686,10 +686,60 @@
 	printk(KERN_INFO "input: %s on %s\n", atkbd->name, serio->phys);
 }
 
+/*
+ * atkbd_reconnect() tries to restore keyboard in a sane state and is
+ * most likely called on resume.
+ */
+
+static int atkbd_reconnect(struct serio *serio)
+{
+	struct atkbd *atkbd = serio->private;
+	struct serio_dev *dev = serio->dev;
+	int i;
+
+        if (!dev) {
+                printk(KERN_DEBUG "atkbd: reconnect request, but serio is disconnected, ignoring...\n");
+                return -1;
+        }
+
+	if (atkbd->write) {
+		if (atkbd_probe(atkbd))
+			return -1;
+		
+		atkbd->set = atkbd_set_3(atkbd);
+		atkbd_enable(atkbd);
+	} else {
+		atkbd->set = 2;
+		atkbd->id = 0xab00;
+	}
+
+	/* 
+	 * Here we probably should check if the keyboard has the same set that
+         * it had before and bail out if it's different. But this will most likely
+         * cause new keyboard device be created... and for the user it will look
+         * like keyboard is lost
+	 */
+
+	if (atkbd->set == 3)
+		memcpy(atkbd->keycode, atkbd_set3_keycode, sizeof(atkbd->keycode));
+	else
+		memcpy(atkbd->keycode, atkbd_set2_keycode, sizeof(atkbd->keycode));
+
+	for (i = 0; i < 512; i++)
+		if (atkbd->keycode[i] && atkbd->keycode[i] < 255)
+			set_bit(atkbd->keycode[i], atkbd->dev.keybit);
+
+	printk(KERN_INFO "input: %s on %s (reconnected)\n", atkbd->name, serio->phys);
+
+	return 0;
+}
+
+
 
 static struct serio_dev atkbd_dev = {
 	.interrupt =	atkbd_interrupt,
 	.connect =	atkbd_connect,
+	.reconnect = 	atkbd_reconnect,
 	.disconnect =	atkbd_disconnect,
 	.cleanup =	atkbd_cleanup,
 };

diff -Nru a/drivers/input/mouse/psmouse-base.c b/drivers/input/mouse/psmouse-base.c
@@ -638,16 +638,6 @@
 		return -1;
 	}
 	
-	/* We need to reopen the serio port to reinitialize the i8042 controller */
-	serio_close(serio);
-	if (serio_open(serio, dev)) {
-		/* do a disconnect here as serio_open leaves dev as NULL so disconnect 
-		 * will not be called automatically later
-		 */
-		psmouse_disconnect(serio);
-		return -1;
-	}
-	
 	psmouse->state = PSMOUSE_NEW_DEVICE;
 	psmouse->type = psmouse->acking = psmouse->cmdcnt = psmouse->pktcnt = 0;
 	if (psmouse->reconnect) {

diff -Nru a/drivers/input/serio/i8042.c b/drivers/input/serio/i8042.c
@@ -226,6 +226,12 @@
 
 	i8042_flush();
 
+	/* 
+	 * Enable port again here because it is disabled if we are
+	 * resuming (normally it is enabled already).
+	 */
+	i8042_ctr &= ~values->disable;
+
 	i8042_ctr |= values->irqen;
 
 	if (i8042_command(&i8042_ctr, I8042_CMD_CTL_WCTR)) {