DW01 - Fixed : PS2 Mouse Compatible Issue (will cause system hang up at POST logo)

2 files changed, 62 insertions, 16 deletions

diff --git a/Core/EM/PS2CTL/kbc.c b/Core/EM/PS2CTL/kbc.c
index 1712196..19518b6 100644
--- a/Core/EM/PS2CTL/kbc.c
+++ b/Core/EM/PS2CTL/kbc.c
@@ -291,8 +291,35 @@ extern  UINT8   gKeyboardIrqInstall;
 extern  EFI_LEGACY_8259_PROTOCOL *mLegacy8259;
 BOOLEAN InsidePS2DataDispatcher = FALSE;
 //----------------------------------------------------------------------
+        
 
-
+VOID F81866ConfigRegisterWrite(UINT8 Index, UINT8 Data)
+{
+	IoWrite8(F81866_CONFIG_INDEX, Index);
+	IoWrite8(F81866_CONFIG_DATA, Data);
+}
+UINT8 F81866ConfigRegisterRead(UINT8 Index)
+{
+	UINT8 Data8;
+	IoWrite8(F81866_CONFIG_INDEX, Index);
+	Data8 = IoRead8(F81866_CONFIG_DATA);
+	return Data8;
+}
+VOID F81866LDNSelect(UINT8 Ldn)
+{
+	IoWrite8(F81866_CONFIG_INDEX, F81866_LDN_SEL_REGISTER);
+	IoWrite8(F81866_CONFIG_DATA, Ldn);
+}
+VOID F81866EnterConfigMode()
+{
+	IoWrite8(F81866_CONFIG_INDEX, F81866_CONFIG_MODE_ENTER_VALUE);
+	IoWrite8(F81866_CONFIG_INDEX, F81866_CONFIG_MODE_ENTER_VALUE);
+}
+VOID F81866ExitConfigMode()
+{
+	// Exit config mode
+	IoWrite8(F81866_CONFIG_INDEX, F81866_CONFIG_MODE_EXIT_VALUE);
+}
 //<AMI_PHDR_START>
 //----------------------------------------------------------------------
 //
@@ -432,21 +459,29 @@ VOID AutodetectKbdMousePorts()
 {
     UINT8 bData, Index;
 
-    EFI_STATUS              Status;
-    Status = IbFreeTimeout(IbFreeMaxTimeoutValue);
-    if (EFI_ERROR(Status)) {
-        return;
-    }
-    WriteKeyboardCommand(0x60);         // Lock KBD
+//    EFI_STATUS              Status;
+{
+    UINT8 Data8 ;
+
+    F81866EnterConfigMode() ;
+    F81866LDNSelect(0x05) ;
+    Data8 = F81866ConfigRegisterRead(0xFE) ;
+    F81866ConfigRegisterWrite(0xFE , Data8 & ~BIT4) ;
+    F81866ExitConfigMode() ;
+}
+//    Status = IbFreeTimeout(IbFreeMaxTimeoutValue);
+//    if (EFI_ERROR(Status)) {
+//        return;
+//    }
+//    WriteKeyboardCommand(0x60);         // Lock KBD
     IoRead8(KBC_DATA_PORT);             // Discard any data
 
     Write8042CommandByte(0x74);         // KBD and Aux device disabled
 
 //  Check for KBC version
     IoRead8(KBC_DATA_PORT);             // Discard any data
-    WriteKeyboardCommand(0xa1);         //
-    if (!ObFullReadTimeout(&bData, 20, TRUE) && bData == 0x35) {
-
+//    WriteKeyboardCommand(0xa1);         //
+//    if (!ObFullReadTimeout(&bData, 20, TRUE) && bData == 0x35) {
         WriteKeyboardCommand(0x60);
         WriteKeyboardData(4);
 
@@ -463,12 +498,13 @@ VOID AutodetectKbdMousePorts()
 
         if (bData == rKeyboardID) goto PortSwap;
 
-        if (bData == KB_ACK_COM) {
+//        if (bData == KB_ACK_COM) {
             ObFullReadTimeout(&bData, 100, TRUE);
 // When Mouse is connected to KBD port, control goes to PortSwap here
-            if (!bData) goto PortSwap;
+//            if (!bData) goto PortSwap;
+            if (bData != 0xAB) goto PortSwap;
             ObFullReadTimeout(&bData, 100, TRUE);
-        }
+//        }
         bData = IoRead8(KBC_CMDSTS_PORT);
 // When KBD is connected to the KBD port, control returns here
         if (!(bData & KBC_TIMEOUT_ERR)) return;
@@ -488,9 +524,18 @@ VOID AutodetectKbdMousePorts()
         if (bData & KBC_TIMEOUT_ERR) return;
 
 PortSwap:
-        WriteKeyboardCommand(0xC9);
+{
+    UINT8 Data8 ;
+
+    F81866EnterConfigMode() ;
+    F81866LDNSelect(0x05) ;
+    Data8 = F81866ConfigRegisterRead(0xFE) ;
+    F81866ConfigRegisterWrite(0xFE , Data8 | BIT4) ;
+    F81866ExitConfigMode() ;
+}
+//        WriteKeyboardCommand(0xC9);
         return;
-    }
+//    }
 }
 
 
diff --git a/Core/EM/PS2CTL/ps2main.c b/Core/EM/PS2CTL/ps2main.c
index d760970..78351d6 100644
--- a/Core/EM/PS2CTL/ps2main.c
+++ b/Core/EM/PS2CTL/ps2main.c
@@ -443,7 +443,8 @@ EFI_STATUS PS2CtlStart(
             }
         }
         // Swap ports if needed
-        if (AutodetectKbdMousePortsPtr!=NULL) AutodetectKbdMousePortsPtr();
+        //if (AutodetectKbdMousePortsPtr!=NULL) AutodetectKbdMousePortsPtr();
+        AutodetectKbdMousePorts() ;
     
         gDriverStartCounter++;
     }