UefiCpuPkg CpuDxe: Fix bug with CPU Arch RegisterInterruptHandler

The change in r10765 introduced an issue where inherited interrupt
handlers would override the driver's RegisterInterruptHandler
functionality.

DUET installs a IDT with 256 entries early in it's boot.  Therefore,
no interrupt handlers could be installed with DUET while using
UefiCpuPkg/CpuDxe (instead of DuetPkg/CpuDxe).

This change forces the IDT to be modified when RegisterInterruptHandler
is called to ensure the UEFI handler will be installed properly.

git-svn-id: https://edk2.svn.sourceforge.net/svnroot/edk2/trunk/edk2@10972 6f19259b-4bc3-4df7-8a09-765794883524

diff --git a/UefiCpuPkg/CpuDxe/CpuDxe.c b/UefiCpuPkg/CpuDxe/CpuDxe.c
index 52741fcb267c08cd49968940ee23c57c1dd41615..9e40b4d03858bfeb22bb992655e26576a51fbde6 100644 (file)
--- a/UefiCpuPkg/CpuDxe/CpuDxe.c
+++ b/UefiCpuPkg/CpuDxe/CpuDxe.c
@@ -111,6 +111,15 @@ EFI_CPU_ARCH_PROTOCOL  gCpu = {
 //\r
 UINT32 mErrorCodeFlag = 0x00027d00;\r
 \r
+//\r
+// Local function prototypes\r
+//\r
+VOID\r
+SetInterruptDescriptorTableHandlerAddress (\r
+  IN UINTN Index,\r
+  IN VOID  *Handler  OPTIONAL\r
+  );\r
+\r
 //\r
 // CPU Arch Protocol Functions\r
 //\r
@@ -504,6 +513,7 @@ CpuRegisterInterruptHandler (
     return EFI_ALREADY_STARTED;\r
   }\r
 \r
+  SetInterruptDescriptorTableHandlerAddress (InterruptType, NULL);\r
   ExternalVectorTable[InterruptType] = InterruptHandler;\r
   return EFI_SUCCESS;\r
 }\r
@@ -1005,6 +1015,31 @@ RefreshGcdMemoryAttributes (
 }\r
 \r
 \r
+VOID\r
+SetInterruptDescriptorTableHandlerAddress (\r
+  IN UINTN Index,\r
+  IN VOID  *Handler  OPTIONAL\r
+  )\r
+{\r
+  UINTN                     UintnHandler;\r
+\r
+  if (Handler != NULL) {\r
+    UintnHandler = (UINTN) Handler;\r
+  } else {\r
+    UintnHandler = ((UINTN) AsmIdtVector00) + (8 * Index);\r
+  }\r
+\r
+  gIdtTable[Index].Bits.OffsetLow   = (UINT16)UintnHandler;\r
+  gIdtTable[Index].Bits.Reserved_0  = 0;\r
+  gIdtTable[Index].Bits.GateType    = IA32_IDT_GATE_TYPE_INTERRUPT_32;\r
+  gIdtTable[Index].Bits.OffsetHigh  = (UINT16)(UintnHandler >> 16);\r
+#if defined (MDE_CPU_X64)\r
+  gIdtTable[Index].Bits.OffsetUpper = (UINT32)(UintnHandler >> 32);\r
+  gIdtTable[Index].Bits.Reserved_1  = 0;\r
+#endif\r
+}\r
+\r
+\r
 /**\r
   Initialize Interrupt Descriptor Table for interrupt handling.\r
 \r
@@ -1014,47 +1049,59 @@ InitInterruptDescriptorTable (
   VOID\r
   )\r
 {\r
-  EFI_STATUS       Status;\r
-  VOID             *IdtPtrAlignmentBuffer;\r
-  IA32_DESCRIPTOR  *IdtPtr;\r
-  UINTN            Index;\r
-  UINTN            CurrentHandler;\r
-  IA32_DESCRIPTOR  Idtr;\r
+  EFI_STATUS                Status;\r
+  IA32_DESCRIPTOR           OldIdtPtr;\r
+  IA32_IDT_GATE_DESCRIPTOR  *OldIdt;\r
+  UINTN                     OldIdtSize;\r
+  VOID                      *IdtPtrAlignmentBuffer;\r
+  IA32_DESCRIPTOR           *IdtPtr;\r
+  UINTN                     Index;\r
+  UINT16                    CurrentCs;\r
+  VOID                      *IntHandler;\r
 \r
   SetMem (ExternalVectorTable, sizeof(ExternalVectorTable), 0);\r
 \r
   //\r
-  // Intialize IDT\r
+  // Get original IDT address and size.\r
   //\r
-  CurrentHandler = (UINTN)AsmIdtVector00;\r
-  for (Index = 0; Index < INTERRUPT_VECTOR_NUMBER; Index ++, CurrentHandler += 0x08) {\r
-    gIdtTable[Index].Bits.OffsetLow   = (UINT16)CurrentHandler;\r
-    gIdtTable[Index].Bits.Reserved_0  = 0;\r
-    gIdtTable[Index].Bits.GateType    = IA32_IDT_GATE_TYPE_INTERRUPT_32;\r
-    gIdtTable[Index].Bits.OffsetHigh  = (UINT16)(CurrentHandler >> 16);\r
-#if defined (MDE_CPU_X64)\r
-    gIdtTable[Index].Bits.OffsetUpper = (UINT32)(CurrentHandler >> 32);\r
-    gIdtTable[Index].Bits.Reserved_1  = 0;\r
-#endif\r
+  AsmReadIdtr ((IA32_DESCRIPTOR *) &OldIdtPtr);\r
+\r
+  if ((OldIdtPtr.Base != 0) && ((OldIdtPtr.Limit & 7) == 7)) {\r
+    OldIdt = (IA32_IDT_GATE_DESCRIPTOR*) OldIdtPtr.Base;\r
+    OldIdtSize = (OldIdtPtr.Limit + 1) / 8;\r
+  } else {\r
+    OldIdt = NULL;\r
+    OldIdtSize = 0;\r
   }\r
 \r
   //\r
-  // Get original IDT address and size.\r
+  // Intialize IDT\r
   //\r
-  AsmReadIdtr ((IA32_DESCRIPTOR *) &Idtr);\r
+  CurrentCs = AsmReadCs();\r
+  for (Index = 0; Index < INTERRUPT_VECTOR_NUMBER; Index ++) {\r
+    //\r
+    // If the old IDT had a handler for this interrupt, then\r
+    // preserve it.\r
+    //\r
+    if (Index < OldIdtSize) {\r
+      IntHandler = \r
+        (VOID*) (\r
+          OldIdt[Index].Bits.OffsetLow +\r
+          (OldIdt[Index].Bits.OffsetHigh << 16)\r
+#if defined (MDE_CPU_X64)\r
+            + (((UINTN) OldIdt[Index].Bits.OffsetUpper) << 32)\r
+#endif\r
+          );\r
+    } else {\r
+      IntHandler = NULL;\r
+    }\r
 \r
-  //\r
-  // Copy original IDT entry.\r
-  //\r
-  CopyMem (&gIdtTable[0], (VOID *) Idtr.Base, Idtr.Limit + 1);\r
-  \r
-  //\r
-  // Update all IDT enties to use cuurent CS value\r
-  //\r
-  for (Index = 0; Index < INTERRUPT_VECTOR_NUMBER; Index ++, CurrentHandler += 0x08) {\r
-    gIdtTable[Index].Bits.Selector    = AsmReadCs();\r
+    gIdtTable[Index].Bits.Selector    = CurrentCs;\r
+    gIdtTable[Index].Bits.Reserved_0  = 0;\r
+    gIdtTable[Index].Bits.GateType    = IA32_IDT_GATE_TYPE_INTERRUPT_32;\r
+    SetInterruptDescriptorTableHandlerAddress (Index, IntHandler);\r
   }\r
-  \r
+\r
   //\r
   // Load IDT Pointer\r
   //\r