Add DEBUG() macros for DEBUG_CACHE to MTRR Library show all changes memory caches...

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

diff --git a/UefiCpuPkg/CpuDxe/CpuDxe.c b/UefiCpuPkg/CpuDxe/CpuDxe.c
index 52741fcb267c08cd49968940ee23c57c1dd41615..99fdbd7757055c34c3d0172c6321f3cad3a1d6c7 100644 (file)
--- a/UefiCpuPkg/CpuDxe/CpuDxe.c
+++ b/UefiCpuPkg/CpuDxe/CpuDxe.c
@@ -1,7 +1,7 @@
 /** @file\r
   CPU DXE Module.\r
 \r
-  Copyright (c) 2008 - 2010, Intel Corporation. All rights reserved.<BR>\r
+  Copyright (c) 2008 - 2011, Intel Corporation. All rights reserved.<BR>\r
   This program and the accompanying materials\r
   are licensed and made available under the terms and conditions of the BSD License\r
   which accompanies this distribution.  The full text of the license may be found at\r
@@ -111,6 +111,23 @@ EFI_CPU_ARCH_PROTOCOL  gCpu = {
 //\r
 UINT32 mErrorCodeFlag = 0x00027d00;\r
 \r
+//\r
+// Local function prototypes\r
+//\r
+\r
+/**\r
+  Set Interrupt Descriptor Table Handler Address.\r
+\r
+  @param Index        The Index of the interrupt descriptor table handle.\r
+  @param Handler      Handler address.\r
+\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 +521,7 @@ CpuRegisterInterruptHandler (
     return EFI_ALREADY_STARTED;\r
   }\r
 \r
+  SetInterruptDescriptorTableHandlerAddress ((UINTN)InterruptType, NULL);\r
   ExternalVectorTable[InterruptType] = InterruptHandler;\r
   return EFI_SUCCESS;\r
 }\r
@@ -594,8 +612,6 @@ CpuSetMemoryAttributes (
     return EFI_UNSUPPORTED;\r
   }\r
 \r
-  DEBUG((EFI_D_ERROR, "CpuAp: SetMemorySpaceAttributes(BA=%08x, Len=%08x, Attr=%08x)\n", BaseAddress, Length, Attributes));\r
-\r
   //\r
   // If this function is called because GCD SetMemorySpaceAttributes () is called\r
   // by RefreshGcdMemoryAttributes (), then we are just synchronzing GCD memory\r
@@ -634,15 +650,12 @@ CpuSetMemoryAttributes (
   //\r
   // call MTRR libary function\r
   //\r
-  DEBUG((EFI_D_ERROR, "  MtrrSetMemoryAttribute()\n"));\r
-  Status = MtrrSetMemoryAttribute(\r
+  Status = MtrrSetMemoryAttribute (\r
              BaseAddress,\r
              Length,\r
              CacheType\r
              );\r
 \r
-  MtrrDebugPrintAllMtrrs ();\r
-\r
   return (EFI_STATUS) Status;\r
 }\r
 \r
@@ -1004,6 +1017,37 @@ RefreshGcdMemoryAttributes (
   mIsFlushingGCD = FALSE;\r
 }\r
 \r
+/**\r
+  Set Interrupt Descriptor Table Handler Address.\r
+\r
+  @param Index        The Index of the interrupt descriptor table handle.\r
+  @param Handler      Handler address.\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
@@ -1014,47 +1058,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) / sizeof (IA32_IDT_GATE_DESCRIPTOR);\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