Refine code to remove type converting warning.

[mirror_edk2.git] / UefiCpuPkg / CpuDxe / CpuDxe.c

diff --git a/UefiCpuPkg/CpuDxe/CpuDxe.c b/UefiCpuPkg/CpuDxe/CpuDxe.c
index 454659150fea14c883dab6b18246ad2706535a3a..35fba39b43aeb192cbf3f0ac9dffbaf00d946f01 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 ((UINTN)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,28 +1049,57 @@ InitInterruptDescriptorTable (
   VOID\r
   )\r
 {\r
-  EFI_STATUS      Status;\r
-  VOID            *IdtPtrAlignmentBuffer;\r
-  IA32_DESCRIPTOR *IdtPtr;\r
-  UINTN           Index;\r
-  UINTN           CurrentHandler;\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
+  // Get original IDT address and size.\r
+  //\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
   // Intialize IDT\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.Selector    = AsmReadCs();\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
+  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
-    gIdtTable[Index].Bits.OffsetUpper = (UINT32)(CurrentHandler >> 32);\r
-    gIdtTable[Index].Bits.Reserved_1  = 0;\r
+            + (((UINTN) OldIdt[Index].Bits.OffsetUpper) << 32)\r
 #endif\r
+          );\r
+    } else {\r
+      IntHandler = NULL;\r
+    }\r
+\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
@@ -1044,7 +1108,7 @@ InitInterruptDescriptorTable (
   IdtPtrAlignmentBuffer = AllocatePool (sizeof (*IdtPtr) + 16);\r
   IdtPtr = ALIGN_POINTER (IdtPtrAlignmentBuffer, 16);\r
   IdtPtr->Base = (UINT32)(((UINTN)(VOID*) gIdtTable) & (BASE_4GB-1));\r
-  IdtPtr->Limit = sizeof (gIdtTable) - 1;\r
+  IdtPtr->Limit = (UINT16) (sizeof (gIdtTable) - 1);\r
 \r
   AsmWriteIdtr (IdtPtr);\r
 \r