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

1 files changed, 77 insertions, 30 deletions

diff --git a/UefiCpuPkg/CpuDxe/CpuDxe.c b/UefiCpuPkg/CpuDxe/CpuDxe.c
index 52741fcb26..9e40b4d038 100644
--- a/UefiCpuPkg/CpuDxe/CpuDxe.c
+++ b/UefiCpuPkg/CpuDxe/CpuDxe.c
@@ -112,6 +112,15 @@ EFI_CPU_ARCH_PROTOCOL  gCpu = {
 UINT32 mErrorCodeFlag = 0x00027d00;

 

 //

+// Local function prototypes

+//

+VOID

+SetInterruptDescriptorTableHandlerAddress (

+  IN UINTN Index,

+  IN VOID  *Handler  OPTIONAL

+  );

+

+//

 // CPU Arch Protocol Functions

 //

 

@@ -504,6 +513,7 @@ CpuRegisterInterruptHandler (
     return EFI_ALREADY_STARTED;

   }

 

+  SetInterruptDescriptorTableHandlerAddress (InterruptType, NULL);

   ExternalVectorTable[InterruptType] = InterruptHandler;

   return EFI_SUCCESS;

 }

@@ -1005,6 +1015,31 @@ RefreshGcdMemoryAttributes (
 }

 

 

+VOID

+SetInterruptDescriptorTableHandlerAddress (

+  IN UINTN Index,

+  IN VOID  *Handler  OPTIONAL

+  )

+{

+  UINTN                     UintnHandler;

+

+  if (Handler != NULL) {

+    UintnHandler = (UINTN) Handler;

+  } else {

+    UintnHandler = ((UINTN) AsmIdtVector00) + (8 * Index);

+  }

+

+  gIdtTable[Index].Bits.OffsetLow   = (UINT16)UintnHandler;

+  gIdtTable[Index].Bits.Reserved_0  = 0;

+  gIdtTable[Index].Bits.GateType    = IA32_IDT_GATE_TYPE_INTERRUPT_32;

+  gIdtTable[Index].Bits.OffsetHigh  = (UINT16)(UintnHandler >> 16);

+#if defined (MDE_CPU_X64)

+  gIdtTable[Index].Bits.OffsetUpper = (UINT32)(UintnHandler >> 32);

+  gIdtTable[Index].Bits.Reserved_1  = 0;

+#endif

+}

+

+

 /**

   Initialize Interrupt Descriptor Table for interrupt handling.

 

@@ -1014,47 +1049,59 @@ InitInterruptDescriptorTable (
   VOID

   )

 {

-  EFI_STATUS       Status;

-  VOID             *IdtPtrAlignmentBuffer;

-  IA32_DESCRIPTOR  *IdtPtr;

-  UINTN            Index;

-  UINTN            CurrentHandler;

-  IA32_DESCRIPTOR  Idtr;

+  EFI_STATUS                Status;

+  IA32_DESCRIPTOR           OldIdtPtr;

+  IA32_IDT_GATE_DESCRIPTOR  *OldIdt;

+  UINTN                     OldIdtSize;

+  VOID                      *IdtPtrAlignmentBuffer;

+  IA32_DESCRIPTOR           *IdtPtr;

+  UINTN                     Index;

+  UINT16                    CurrentCs;

+  VOID                      *IntHandler;

 

   SetMem (ExternalVectorTable, sizeof(ExternalVectorTable), 0);

 

   //

-  // Intialize IDT

+  // Get original IDT address and size.

   //

-  CurrentHandler = (UINTN)AsmIdtVector00;

-  for (Index = 0; Index < INTERRUPT_VECTOR_NUMBER; Index ++, CurrentHandler += 0x08) {

-    gIdtTable[Index].Bits.OffsetLow   = (UINT16)CurrentHandler;

-    gIdtTable[Index].Bits.Reserved_0  = 0;

-    gIdtTable[Index].Bits.GateType    = IA32_IDT_GATE_TYPE_INTERRUPT_32;

-    gIdtTable[Index].Bits.OffsetHigh  = (UINT16)(CurrentHandler >> 16);

-#if defined (MDE_CPU_X64)

-    gIdtTable[Index].Bits.OffsetUpper = (UINT32)(CurrentHandler >> 32);

-    gIdtTable[Index].Bits.Reserved_1  = 0;

-#endif

+  AsmReadIdtr ((IA32_DESCRIPTOR *) &OldIdtPtr);

+

+  if ((OldIdtPtr.Base != 0) && ((OldIdtPtr.Limit & 7) == 7)) {

+    OldIdt = (IA32_IDT_GATE_DESCRIPTOR*) OldIdtPtr.Base;

+    OldIdtSize = (OldIdtPtr.Limit + 1) / 8;

+  } else {

+    OldIdt = NULL;

+    OldIdtSize = 0;

   }

 

   //

-  // Get original IDT address and size.

+  // Intialize IDT

   //

-  AsmReadIdtr ((IA32_DESCRIPTOR *) &Idtr);

+  CurrentCs = AsmReadCs();

+  for (Index = 0; Index < INTERRUPT_VECTOR_NUMBER; Index ++) {

+    //

+    // If the old IDT had a handler for this interrupt, then

+    // preserve it.

+    //

+    if (Index < OldIdtSize) {

+      IntHandler = 

+        (VOID*) (

+          OldIdt[Index].Bits.OffsetLow +

+          (OldIdt[Index].Bits.OffsetHigh << 16)

+#if defined (MDE_CPU_X64)

+            + (((UINTN) OldIdt[Index].Bits.OffsetUpper) << 32)

+#endif

+          );

+    } else {

+      IntHandler = NULL;

+    }

 

-  //

-  // Copy original IDT entry.

-  //

-  CopyMem (&gIdtTable[0], (VOID *) Idtr.Base, Idtr.Limit + 1);

-  

-  //

-  // Update all IDT enties to use cuurent CS value

-  //

-  for (Index = 0; Index < INTERRUPT_VECTOR_NUMBER; Index ++, CurrentHandler += 0x08) {

-    gIdtTable[Index].Bits.Selector    = AsmReadCs();

+    gIdtTable[Index].Bits.Selector    = CurrentCs;

+    gIdtTable[Index].Bits.Reserved_0  = 0;

+    gIdtTable[Index].Bits.GateType    = IA32_IDT_GATE_TYPE_INTERRUPT_32;

+    SetInterruptDescriptorTableHandlerAddress (Index, IntHandler);

   }

-  

+

   //

   // Load IDT Pointer

   //