SourceLevelDebugPkg/SecPeiDebugAgentLib: Restore CPU interrupt state

[mirror_edk2.git] / SourceLevelDebugPkg / Library / DebugAgent / SecPeiDebugAgent / SecPeiDebugAgentLib.c

diff --git a/SourceLevelDebugPkg/Library/DebugAgent/SecPeiDebugAgent/SecPeiDebugAgentLib.c b/SourceLevelDebugPkg/Library/DebugAgent/SecPeiDebugAgent/SecPeiDebugAgentLib.c
index 20b6a370d6bb104aabd1849ea1c0818812567cca..faec574fa4171d585ea6b9587c5455a73a821ada 100644 (file)
--- a/SourceLevelDebugPkg/Library/DebugAgent/SecPeiDebugAgent/SecPeiDebugAgentLib.c
+++ b/SourceLevelDebugPkg/Library/DebugAgent/SecPeiDebugAgent/SecPeiDebugAgentLib.c
@@ -376,8 +376,12 @@ InitializeDebugAgent (
   UINT64                           *MailboxLocationPointer;\r
   EFI_PHYSICAL_ADDRESS             Address;\r
   UINT32                           DebugTimerFrequency;\r
+  BOOLEAN                          CpuInterruptState;\r
 \r
-  DisableInterrupts ();\r
+  //\r
+  // Disable interrupts and save current interrupt state\r
+  //\r
+  CpuInterruptState = SaveAndDisableInterrupts();\r
 \r
   switch (InitFlag) {\r
 \r
@@ -579,7 +583,7 @@ InitializeDebugAgent (
       Ia32Idtr =  (IA32_DESCRIPTOR *) Context;\r
       Ia32IdtEntry = (IA32_IDT_ENTRY *)(Ia32Idtr->Base);\r
       MailboxLocationPointer = (UINT64 *) (UINTN) (Ia32IdtEntry[DEBUG_MAILBOX_VECTOR].Bits.OffsetLow +\r
-                                                (Ia32IdtEntry[DEBUG_MAILBOX_VECTOR].Bits.OffsetHigh << 16));\r
+                                         (UINT32) (Ia32IdtEntry[DEBUG_MAILBOX_VECTOR].Bits.OffsetHigh << 16));\r
       Mailbox = (DEBUG_AGENT_MAILBOX *) (UINTN)(*MailboxLocationPointer);\r
       //\r
       // Mailbox should valid and setup before executing thunk code\r
@@ -611,11 +615,22 @@ InitializeDebugAgent (
     break;\r
   }\r
 \r
-  //\r
-  // Enable CPU interrupts so debug timer interrupts can be delivered\r
-  //\r
-  EnableInterrupts ();\r
-\r
+  if (InitFlag == DEBUG_AGENT_INIT_POSTMEM_SEC) {\r
+    //\r
+    // Restore CPU Interrupt state and keep debug timer interrupt state as is\r
+    // in DEBUG_AGENT_INIT_POSTMEM_SEC case\r
+    //\r
+    SetInterruptState (CpuInterruptState);\r
+  } else {\r
+    //\r
+    // Enable Debug Timer interrupt\r
+    //\r
+    SaveAndSetDebugTimerInterrupt (TRUE);\r
+    //\r
+    // Enable CPU interrupts so debug timer interrupts can be delivered\r
+    //\r
+    EnableInterrupts ();\r
+  }\r
   //\r
   // If Function is not NULL, invoke it always whatever debug agent was initialized sucesssfully or not.\r
   //\r
@@ -680,6 +695,10 @@ InitializeDebugAgentPhase2 (
       TriggerSoftInterrupt (MEMORY_READY_SIGNATURE);\r
     }\r
     //\r
+    // Enable Debug Timer interrupt\r
+    //\r
+    SaveAndSetDebugTimerInterrupt (TRUE);\r
+    //\r
     // Enable CPU interrupts so debug timer interrupts can be delivered\r
     //\r
     EnableInterrupts ();\r