[dgorbachev] 25745: Fix "keyboard does not respond" bug. Thanks to Peter Krawies (breakoutbox /AT/ web /DOT/ de)

Author: dgorbachev
Date: Wed Feb  7 03:29:50 2007
New Revision: 25745

URL: http://svn.reactos.org/svn/reactos?rev=25745&view=rev
Log:
Fix "keyboard does not respond" bug. Thanks to Peter Krawies (breakoutbox /AT/ web /DOT/ de)

Modified:
    trunk/reactos/boot/freeldr/freeldr/arch/i386/hardware.c

Modified: trunk/reactos/boot/freeldr/freeldr/arch/i386/hardware.c
URL: http://svn.reactos.org/svn/reactos/trunk/reactos/boot/freeldr/freeldr/arch/i...
==============================================================================
--- trunk/reactos/boot/freeldr/freeldr/arch/i386/hardware.c (original)
+++ trunk/reactos/boot/freeldr/freeldr/arch/i386/hardware.c Wed Feb  7 03:29:50 2007
@@ -1925,6 +1925,8 @@
 {
   UCHAR Scancode;
   UCHAR Status;
+  ULONG Loops;
+  BOOLEAN Result = TRUE;

PS2ControllerWait();
   WRITE_PORT_UCHAR((PUCHAR)CONTROLLER_REGISTER_CONTROL,
@@ -1935,31 +1937,43 @@
   WRITE_PORT_UCHAR((PUCHAR)CONTROLLER_REGISTER_DATA,
    	   0xF2);

-  StallExecutionProcessor(10000);
+  /* Wait for reply */
+  for (Loops = 0; Loops < 10; Loops++)
+    {
+      StallExecutionProcessor(10000);
+      Status = READ_PORT_UCHAR((PUCHAR)CONTROLLER_REGISTER_STATUS);
+      if ((Status & CONTROLLER_STATUS_OUTPUT_BUFFER_FULL) != 0)
+        break;
+    }

Status = READ_PORT_UCHAR((PUCHAR)CONTROLLER_REGISTER_STATUS);
   if ((Status & CONTROLLER_STATUS_MOUSE_OUTPUT_BUFFER_FULL) == 0)
-    {
-      return FALSE;
-    }
+    Result = FALSE;

Scancode = READ_PORT_UCHAR((PUCHAR)CONTROLLER_REGISTER_DATA);
   if (Scancode != 0xFA)
-    return FALSE;
+    Result = FALSE;

StallExecutionProcessor(10000);

Status = READ_PORT_UCHAR((PUCHAR)CONTROLLER_REGISTER_STATUS);
   if ((Status & CONTROLLER_STATUS_MOUSE_OUTPUT_BUFFER_FULL) == 0)
-    {
-      return FALSE;
-    }
+    Result = FALSE;

Scancode = READ_PORT_UCHAR((PUCHAR)CONTROLLER_REGISTER_DATA);
   if (Scancode != 0x00)
-    return FALSE;
-
-  return TRUE;
+    Result = FALSE;
+
+  /* Flush output buffer */
+  for (Loops = 0; Loops < 10; Loops++)
+    {
+      Status = READ_PORT_UCHAR((PUCHAR)CONTROLLER_REGISTER_STATUS);
+      if ((Status & CONTROLLER_STATUS_OUTPUT_BUFFER_FULL) == 0)
+        break;
+      StallExecutionProcessor(10000);
+    }
+
+  return Result;
 }