3 files changed, 64 insertions, 34 deletions

diff --git a/drivers/input/serio/i8042.c b/drivers/input/serio/i8042.c
index c3fdfc1f342a..b90a2cddc8a1 100644
--- a/drivers/input/serio/i8042.c
+++ b/drivers/input/serio/i8042.c
@@ -724,7 +724,7 @@ static int i8042_controller_init(void)
         if (~i8042_read_status() & I8042_STR_KEYLOCK) {
                 if (i8042_unlock)
                         i8042_ctr |= I8042_CTR_IGNKEYLOCK;
-                 else
+                else
                         printk(KERN_WARNING "i8042.c: Warning: Keylock active.\n");
         }
         spin_unlock_irqrestore(&i8042_lock, flags);
@@ -791,27 +791,6 @@ static void i8042_controller_reset(void)
 
 
 /*
- * Here we try to reset everything back to a state in which the BIOS will be
- * able to talk to the hardware when rebooting.
- */
-
-static void i8042_controller_cleanup(void)
-{
-        int i;
-
-/*
- * Reset anything that is connected to the ports.
- */
-
-        for (i = 0; i < I8042_NUM_PORTS; i++)
-                if (i8042_ports[i].serio)
-                        serio_cleanup(i8042_ports[i].serio);
-
-        i8042_controller_reset();
-}
-
-
-/*
  * i8042_panic_blink() will flash the keyboard LEDs and is called when
  * kernel panics. Flashing LEDs is useful for users running X who may
  * not see the console and will help distingushing panics from "real"
@@ -857,13 +836,22 @@ static long i8042_panic_blink(long count)
 
 #undef DELAY
 
+#ifdef CONFIG_PM
 /*
- * Here we try to restore the original BIOS settings
+ * Here we try to restore the original BIOS settings. We only want to
+ * do that once, when we really suspend, not when we taking memory
+ * snapshot for swsusp (in this case we'll perform required cleanup
+ * as part of shutdown process).
  */
 
 static int i8042_suspend(struct platform_device *dev, pm_message_t state)
 {
-        i8042_controller_cleanup();
+        if (dev->dev.power.power_state.event != state.event) {
+                if (state.event == PM_EVENT_SUSPEND)
+                        i8042_controller_reset();
+
+                dev->dev.power.power_state = state;
+        }
 
         return 0;
 }
@@ -877,6 +865,12 @@ static int i8042_resume(struct platform_device *dev)
 {
         int error;
 
+/*
+ * Do not bother with restoring state if we haven't suspened yet
+ */
+        if (dev->dev.power.power_state.event == PM_EVENT_ON)
+                return 0;
+
         error = i8042_controller_check();
         if (error)
                 return error;
@@ -886,9 +880,12 @@ static int i8042_resume(struct platform_device *dev)
                 return error;
 
 /*
- * Restore pre-resume CTR value and disable all ports
+ * Restore original CTR value and disable all ports
  */
 
+        i8042_ctr = i8042_initial_ctr;
+        if (i8042_direct)
+                i8042_ctr &= ~I8042_CTR_XLATE;
         i8042_ctr |= I8042_CTR_AUXDIS | I8042_CTR_KBDDIS;
         i8042_ctr &= ~(I8042_CTR_AUXINT | I8042_CTR_KBDINT);
         if (i8042_command(&i8042_ctr, I8042_CMD_CTL_WCTR)) {
@@ -909,8 +906,11 @@ static int i8042_resume(struct platform_device *dev)
 
         i8042_interrupt(0, NULL);
 
+        dev->dev.power.power_state = PMSG_ON;
+
         return 0;
 }
+#endif /* CONFIG_PM */
 
 /*
  * We need to reset the 8042 back to original mode on system shutdown,
@@ -919,7 +919,7 @@ static int i8042_resume(struct platform_device *dev)
 
 static void i8042_shutdown(struct platform_device *dev)
 {
-        i8042_controller_cleanup();
+        i8042_controller_reset();
 }
 
 static int __devinit i8042_create_kbd_port(void)
@@ -1154,9 +1154,11 @@ static struct platform_driver i8042_driver = {
         },
         .probe          = i8042_probe,
         .remove         = __devexit_p(i8042_remove),
+        .shutdown       = i8042_shutdown,
+#ifdef CONFIG_PM
         .suspend        = i8042_suspend,
         .resume         = i8042_resume,
-        .shutdown       = i8042_shutdown,
+#endif
 };
 
 static int __init i8042_init(void)
diff --git a/drivers/input/serio/serio.c b/drivers/input/serio/serio.c
index 17c8c63cbe1a..a15e531ec755 100644
--- a/drivers/input/serio/serio.c
+++ b/drivers/input/serio/serio.c
@@ -778,6 +778,19 @@ static int serio_driver_remove(struct device *dev)
         return 0;
 }
 
+static void serio_cleanup(struct serio *serio)
+{
+        if (serio->drv && serio->drv->cleanup)
+                serio->drv->cleanup(serio);
+}
+
+static void serio_shutdown(struct device *dev)
+{
+        struct serio *serio = to_serio_port(dev);
+
+        serio_cleanup(serio);
+}
+
 static void serio_attach_driver(struct serio_driver *drv)
 {
         int error;
@@ -910,11 +923,25 @@ static int serio_uevent(struct device *dev, char **envp, int num_envp, char *buf
 
 #endif /* CONFIG_HOTPLUG */
 
+#ifdef CONFIG_PM
+static int serio_suspend(struct device *dev, pm_message_t state)
+{
+        if (dev->power.power_state.event != state.event) {
+                if (state.event == PM_EVENT_SUSPEND)
+                        serio_cleanup(to_serio_port(dev));
+
+                dev->power.power_state = state;
+        }
+
+        return 0;
+}
+
 static int serio_resume(struct device *dev)
 {
         struct serio *serio = to_serio_port(dev);
 
-        if (serio_reconnect_driver(serio)) {
+        if (dev->power.power_state.event != PM_EVENT_ON &&
+            serio_reconnect_driver(serio)) {
                 /*
                  * Driver re-probing can take a while, so better let kseriod
                  * deal with it.
@@ -922,8 +949,11 @@ static int serio_resume(struct device *dev)
                 serio_rescan(serio);
         }
 
+        dev->power.power_state = PMSG_ON;
+
         return 0;
 }
+#endif /* CONFIG_PM */
 
 /* called from serio_driver->connect/disconnect methods under serio_mutex */
 int serio_open(struct serio *serio, struct serio_driver *drv)
@@ -974,7 +1004,11 @@ static struct bus_type serio_bus = {
         .uevent         = serio_uevent,
         .probe          = serio_driver_probe,
         .remove         = serio_driver_remove,
+        .shutdown       = serio_shutdown,
+#ifdef CONFIG_PM
+        .suspend        = serio_suspend,
         .resume         = serio_resume,
+#endif
 };
 
 static int __init serio_init(void)
diff --git a/include/linux/serio.h b/include/linux/serio.h
index ac2c70e7f760..1ebf0455e224 100644
--- a/include/linux/serio.h
+++ b/include/linux/serio.h
@@ -108,12 +108,6 @@ static inline void serio_drv_write_wakeup(struct serio *serio)
                 serio->drv->write_wakeup(serio);
 }
 
-static inline void serio_cleanup(struct serio *serio)
-{
-        if (serio->drv && serio->drv->cleanup)
-                serio->drv->cleanup(serio);
-}
-
 /*
  * Use the following functions to manipulate serio's per-port
  * driver-specific data.