1 files changed, 31 insertions, 7 deletions

diff --git a/drivers/input/serio/i8042.c b/drivers/input/serio/i8042.c
index bc56e52b945f..1df02d25aca5 100644
--- a/drivers/input/serio/i8042.c
+++ b/drivers/input/serio/i8042.c
@@ -609,6 +609,8 @@ static irqreturn_t __init i8042_aux_test_irq(int irq, void *dev_id)
         str = i8042_read_status();
         if (str & I8042_STR_OBF) {
                 data = i8042_read_data();
+                dbg("%02x <- i8042 (aux_test_irq, %s)",
+                        data, str & I8042_STR_AUXDATA ? "aux" : "kbd");
                 if (i8042_irq_being_tested &&
                     data == 0xa5 && (str & I8042_STR_AUXDATA))
                         complete(&i8042_aux_irq_delivered);
@@ -750,6 +752,7 @@ static int __init i8042_check_aux(void)
  * AUX IRQ was never delivered so we need to flush the controller to
  * get rid of the byte we put there; otherwise keyboard may not work.
  */
+                dbg("     -- i8042 (aux irq test timeout)");
                 i8042_flush();
                 retval = -1;
         }
@@ -833,17 +836,32 @@ static int i8042_controller_selftest(void)
 static int i8042_controller_init(void)
 {
         unsigned long flags;
+        int n = 0;
+        unsigned char ctr[2];
 
 /*
- * Save the CTR for restoral on unload / reboot.
+ * Save the CTR for restore on unload / reboot.
  */
 
-        if (i8042_command(&i8042_ctr, I8042_CMD_CTL_RCTR)) {
-                printk(KERN_ERR "i8042.c: Can't read CTR while initializing i8042.\n");
-                return -EIO;
-        }
+        do {
+                if (n >= 10) {
+                        printk(KERN_ERR
+                                "i8042.c: Unable to get stable CTR read.\n");
+                        return -EIO;
+                }
+
+                if (n != 0)
+                        udelay(50);
+
+                if (i8042_command(&ctr[n++ % 2], I8042_CMD_CTL_RCTR)) {
+                        printk(KERN_ERR
+                                "i8042.c: Can't read CTR while initializing i8042.\n");
+                        return -EIO;
+                }
 
-        i8042_initial_ctr = i8042_ctr;
+        } while (n < 2 || ctr[0] != ctr[1]);
+
+        i8042_initial_ctr = i8042_ctr = ctr[0];
 
 /*
  * Disable the keyboard interface and interrupt.
@@ -892,6 +910,12 @@ static int i8042_controller_init(void)
                 return -EIO;
         }
 
+/*
+ * Flush whatever accumulated while we were disabling keyboard port.
+ */
+
+        i8042_flush();
+
         return 0;
 }
 
@@ -911,7 +935,7 @@ static void i8042_controller_reset(void)
         i8042_ctr |= I8042_CTR_KBDDIS | I8042_CTR_AUXDIS;
         i8042_ctr &= ~(I8042_CTR_KBDINT | I8042_CTR_AUXINT);
 
-        if (i8042_command(&i8042_initial_ctr, I8042_CMD_CTL_WCTR))
+        if (i8042_command(&i8042_ctr, I8042_CMD_CTL_WCTR))
                 printk(KERN_WARNING "i8042.c: Can't write CTR while resetting.\n");
 
 /*