1 files changed, 50 insertions, 46 deletions

diff --git a/arch/x86/boot/a20.c b/arch/x86/boot/a20.c
index 31348d054fc..64a31a6d751 100644
--- a/arch/x86/boot/a20.c
+++ b/arch/x86/boot/a20.c
@@ -1,7 +1,8 @@
 /* -*- linux-c -*- ------------------------------------------------------- *
  *
  *   Copyright (C) 1991, 1992 Linus Torvalds
- *   Copyright 2007 rPath, Inc. - All Rights Reserved
+ *   Copyright 2007-2008 rPath, Inc. - All Rights Reserved
+ *   Copyright 2009 Intel Corporation; author H. Peter Anvin
  *
  *   This file is part of the Linux kernel, and is made available under
  *   the terms of the GNU General Public License version 2.
@@ -9,24 +10,29 @@
  * ----------------------------------------------------------------------- */
 
 /*
- * arch/i386/boot/a20.c
- *
  * Enable A20 gate (return -1 on failure)
  */
 
 #include "boot.h"
 
 #define MAX_8042_LOOPS	100000
+#define MAX_8042_FF	32
 
 static int empty_8042(void)
 {
 	u8 status;
 	int loops = MAX_8042_LOOPS;
+	int ffs   = MAX_8042_FF;
 
 	while (loops--) {
 		io_delay();
 
 		status = inb(0x64);
+		if (status == 0xff) {
+			/* FF is a plausible, but very unlikely status */
+			if (!--ffs)
+				return -1; /* Assume no KBC present */
+		}
 		if (status & 1) {
 			/* Read and discard input data */
 			io_delay();
@@ -84,8 +90,11 @@ static int a20_test_long(void)
 
 static void enable_a20_bios(void)
 {
-	asm volatile("pushfl; int $0x15; popfl"
-		     : : "a" ((u16)0x2401));
+	struct biosregs ireg;
+
+	initregs(&ireg);
+	ireg.ax = 0x2401;
+	intcall(0x15, &ireg, NULL);
 }
 
 static void enable_a20_kbc(void)
@@ -97,6 +106,9 @@ static void enable_a20_kbc(void)
 
 	outb(0xdf, 0x60);	/* A20 on */
 	empty_8042();
+
+	outb(0xff, 0x64);	/* Null command, but UHCI wants it */
+	empty_8042();
 }
 
 static void enable_a20_fast(void)
@@ -117,45 +129,37 @@ static void enable_a20_fast(void)
 
 int enable_a20(void)
 {
-	int loops = A20_ENABLE_LOOPS;
-
-#if defined(CONFIG_X86_ELAN)
-	/* Elan croaks if we try to touch the KBC */
-	enable_a20_fast();
-	while (!a20_test_long())
-		;
-	return 0;
-#elif defined(CONFIG_X86_VOYAGER)
-	/* On Voyager, a20_test() is unsafe? */
-	enable_a20_kbc();
-	return 0;
-#else
-	while (loops--) {
-		/* First, check to see if A20 is already enabled
-		   (legacy free, etc.) */
-		if (a20_test_short())
-			return 0;
-
-		/* Next, try the BIOS (INT 0x15, AX=0x2401) */
-		enable_a20_bios();
-		if (a20_test_short())
-			return 0;
-
-		/* Try enabling A20 through the keyboard controller */
-		empty_8042();
-		if (a20_test_short())
-			return 0; /* BIOS worked, but with delayed reaction */
-
-		enable_a20_kbc();
-		if (a20_test_long())
-			return 0;
-
-		/* Finally, try enabling the "fast A20 gate" */
-		enable_a20_fast();
-		if (a20_test_long())
-			return 0;
-	}
-
-	return -1;
-#endif
+       int loops = A20_ENABLE_LOOPS;
+       int kbc_err;
+
+       while (loops--) {
+	       /* First, check to see if A20 is already enabled
+		  (legacy free, etc.) */
+	       if (a20_test_short())
+		       return 0;
+	       
+	       /* Next, try the BIOS (INT 0x15, AX=0x2401) */
+	       enable_a20_bios();
+	       if (a20_test_short())
+		       return 0;
+	       
+	       /* Try enabling A20 through the keyboard controller */
+	       kbc_err = empty_8042();
+
+	       if (a20_test_short())
+		       return 0; /* BIOS worked, but with delayed reaction */
+	
+	       if (!kbc_err) {
+		       enable_a20_kbc();
+		       if (a20_test_long())
+			       return 0;
+	       }
+	       
+	       /* Finally, try enabling the "fast A20 gate" */
+	       enable_a20_fast();
+	       if (a20_test_long())
+		       return 0;
+       }
+       
+       return -1;
 }