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
@@ -111,6 +111,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
-  //
-  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
-  }
-
   //
   // Get original IDT address and size.
   //
-  AsmReadIdtr ((IA32_DESCRIPTOR *) &Idtr);
+  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;
+  }
 
   //
-  // Copy original IDT entry.
+  // Intialize IDT
   //
-  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();
+  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;
+    }
+
+    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
   //