]> git.proxmox.com Git - mirror_edk2.git/blobdiff - SourceLevelDebugPkg/Library/DebugAgent/DxeDebugAgent/DxeDebugAgentLib.c
Not to copy mailbox into new space when debug agent initialization for S3 Boot Script.
[mirror_edk2.git] / SourceLevelDebugPkg / Library / DebugAgent / DxeDebugAgent / DxeDebugAgentLib.c
index f8e3f6c1abe888565ca5d88f927b1e4e83086c8e..e424c0f8d8f2eebb24e1d22a8b96d62a8a983b02 100644 (file)
@@ -1,7 +1,7 @@
 /** @file\r
   Debug Agent library implementition for Dxe Core and Dxr modules.\r
 \r
-  Copyright (c) 2010 - 2012, Intel Corporation. All rights reserved.<BR>\r
+  Copyright (c) 2010 - 2014, 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
 #include "DxeDebugAgentLib.h"\r
 \r
 DEBUG_AGENT_MAILBOX          mMailbox;\r
-DEBUG_AGENT_MAILBOX          *mMailboxPointer;\r
+DEBUG_AGENT_MAILBOX          *mMailboxPointer = NULL;\r
 IA32_IDT_GATE_DESCRIPTOR     mIdtEntryTable[33];\r
-BOOLEAN                      mDxeCoreFlag               = FALSE;\r
-CONST BOOLEAN                MultiProcessorDebugSupport = TRUE;\r
+BOOLEAN                      mDxeCoreFlag                = FALSE;\r
+BOOLEAN                      mMultiProcessorDebugSupport = FALSE;\r
+VOID                         *mSavedIdtTable             = NULL;\r
+UINTN                        mSaveIdtTableSize           = 0;\r
+BOOLEAN                      mDebugAgentInitialized      = FALSE;\r
+BOOLEAN                      mSkipBreakpoint             = FALSE;\r
 \r
 /**\r
-  Constructor allocates the NVS memory to store Mailbox and install configuration table\r
-  in system table to store its pointer.\r
+  Check if debug agent support multi-processor.\r
 \r
-  @param[in]  ImageHandle   The firmware allocated handle for the EFI image.\r
-  @param[in]  SystemTable   A pointer to the EFI System Table.\r
+  @retval TRUE    Multi-processor is supported.\r
+  @retval FALSE   Multi-processor is not supported.\r
+\r
+**/\r
+BOOLEAN\r
+MultiProcessorDebugSupport (\r
+  VOID\r
+  )\r
+{\r
+  return mMultiProcessorDebugSupport;\r
+}\r
 \r
-  @retval  RETURN_SUCCESS   Allocate the global memory space to store guid and function tables.\r
+/**\r
+  Internal constructor worker function.\r
+\r
+  It will register one callback function on EFI PCD Protocol.\r
+  It will allocate the NVS memory to store Mailbox and install configuration table\r
+  in system table to store its pointer.\r
 \r
 **/\r
-RETURN_STATUS\r
-EFIAPI\r
-DxeDebugAgentLibConstructor (\r
-  IN EFI_HANDLE        ImageHandle,\r
-  IN EFI_SYSTEM_TABLE  *SystemTable\r
+VOID\r
+InternalConstructorWorker (\r
+  VOID\r
   )\r
 {\r
   EFI_STATUS                  Status;\r
   EFI_PHYSICAL_ADDRESS        Address;\r
-  EFI_EVENT                   Event;\r
-  VOID                        *EventRegistration;\r
+  BOOLEAN                     DebugTimerInterruptState;\r
+  DEBUG_AGENT_MAILBOX         *Mailbox;\r
+  DEBUG_AGENT_MAILBOX         *NewMailbox;\r
+  EFI_HOB_GUID_TYPE           *GuidHob;\r
+  EFI_VECTOR_HANDOFF_INFO     *VectorHandoffInfo;\r
 \r
-  if (!mDxeCoreFlag) {\r
-    return RETURN_SUCCESS;\r
+  //\r
+  // Check persisted vector handoff info\r
+  //\r
+  Status = EFI_SUCCESS;\r
+  GuidHob = GetFirstGuidHob (&gEfiVectorHandoffInfoPpiGuid);\r
+  if (GuidHob != NULL && !mDxeCoreFlag) {\r
+    //\r
+    // Check if configuration table is installed or not if GUIDed HOB existed,\r
+    // only when Debug Agent is not linked by DXE Core\r
+    //\r
+    Status = EfiGetSystemConfigurationTable (&gEfiVectorHandoffTableGuid, (VOID **) &VectorHandoffInfo);\r
+  }\r
+  if (GuidHob == NULL || Status != EFI_SUCCESS) {\r
+    //\r
+    // Install configuration table for persisted vector handoff info if GUIDed HOB cannot be found or\r
+    // configuration table does not exist\r
+    //\r
+    Status = gBS->InstallConfigurationTable (&gEfiVectorHandoffTableGuid, (VOID *) &mVectorHandoffInfoDebugAgent[0]);\r
+    if (EFI_ERROR (Status)) {\r
+      DEBUG ((EFI_D_ERROR, "DebugAgent: Cannot install configuration table for persisted vector handoff info!\n"));\r
+      CpuDeadLoop ();\r
+    }\r
   }\r
-\r
-  Status = gBS->CreateEvent (\r
-                  EVT_NOTIFY_SIGNAL,\r
-                  TPL_CALLBACK,\r
-                  InstallSerialIoNotification,\r
-                  NULL,\r
-                  &Event\r
-                  );\r
-  ASSERT_EFI_ERROR (Status);\r
 \r
   //\r
-  // Register for protocol notifications on this event\r
+  // Install EFI Serial IO protocol on debug port\r
   //\r
-\r
-  Status = gBS->RegisterProtocolNotify (\r
-                  &gEfiPcdProtocolGuid,\r
-                  Event,\r
-                  &EventRegistration\r
-                  );\r
-\r
-  ASSERT_EFI_ERROR (Status);\r
+  InstallSerialIo ();\r
 \r
   Address = 0;\r
   Status = gBS->AllocatePages (\r
                   AllocateAnyPages,\r
                   EfiACPIMemoryNVS,\r
-                  EFI_SIZE_TO_PAGES (sizeof (DEBUG_AGENT_MAILBOX)),\r
+                  EFI_SIZE_TO_PAGES (sizeof(DEBUG_AGENT_MAILBOX) + PcdGet16(PcdDebugPortHandleBufferSize)),\r
                   &Address\r
                   );\r
-  ASSERT_EFI_ERROR (Status);\r
+  if (EFI_ERROR (Status)) {\r
+    DEBUG ((EFI_D_ERROR, "DebugAgent: Cannot install configuration table for mailbox!\n"));\r
+    CpuDeadLoop ();\r
+  }\r
+\r
+  DebugTimerInterruptState = SaveAndSetDebugTimerInterrupt (FALSE);\r
 \r
-  CopyMem (\r
-    (UINT8 *) (UINTN) Address,\r
-    (UINT8 *) (UINTN) mMailboxPointer,\r
-    sizeof (DEBUG_AGENT_MAILBOX)\r
-    );\r
+  NewMailbox = (DEBUG_AGENT_MAILBOX *) (UINTN) Address;\r
+  //\r
+  // Copy Mailbox and Debug Port Handle buffer to new location in ACPI NVS memory, because original Mailbox\r
+  // and Debug Port Handle buffer may be free at runtime, SMM debug agent needs to access them\r
+  //\r
+  Mailbox = GetMailboxPointer ();\r
+  CopyMem (NewMailbox, Mailbox, sizeof (DEBUG_AGENT_MAILBOX));\r
+  CopyMem (NewMailbox + 1, (VOID *)(UINTN)Mailbox->DebugPortHandle, PcdGet16(PcdDebugPortHandleBufferSize));\r
+  //\r
+  // Update Debug Port Handle in new Mailbox\r
+  //\r
+  UpdateMailboxContent (NewMailbox, DEBUG_MAILBOX_DEBUG_PORT_HANDLE_INDEX, (UINT64)(UINTN)(NewMailbox + 1));\r
+  mMailboxPointer = NewMailbox;\r
 \r
-  mMailboxPointer = (DEBUG_AGENT_MAILBOX *) (UINTN) Address;\r
+  DebugTimerInterruptState = SaveAndSetDebugTimerInterrupt (DebugTimerInterruptState);\r
 \r
   Status = gBS->InstallConfigurationTable (&gEfiDebugAgentGuid, (VOID *) mMailboxPointer);\r
-  ASSERT_EFI_ERROR (Status);\r
+  if (EFI_ERROR (Status)) {\r
+    DEBUG ((EFI_D_ERROR, "DebugAgent: Failed to install configuration for mailbox!\n"));\r
+    CpuDeadLoop ();\r
+  }\r
+}\r
+\r
+/**\r
+  Debug Agent constructor function.\r
+\r
+  @param[in]  ImageHandle   The firmware allocated handle for the EFI image.\r
+  @param[in]  SystemTable   A pointer to the EFI System Table.\r
+\r
+  @retval  RETURN_SUCCESS  When this function completed.\r
+\r
+**/\r
+RETURN_STATUS\r
+EFIAPI\r
+DxeDebugAgentLibConstructor (\r
+  IN EFI_HANDLE        ImageHandle,\r
+  IN EFI_SYSTEM_TABLE  *SystemTable\r
+  )\r
+{\r
+  if (mDxeCoreFlag) {\r
+    //\r
+    // Invoke internal constructor function only when DXE core links this library instance\r
+    //\r
+    InternalConstructorWorker ();\r
+  }\r
+\r
+  return RETURN_SUCCESS;\r
+}\r
+\r
+/**\r
+  Get the pointer to Mailbox from the configuration table.\r
+\r
+  @return Pointer to Mailbox.\r
+\r
+**/\r
+DEBUG_AGENT_MAILBOX *\r
+GetMailboxFromConfigurationTable (\r
+  VOID\r
+  )\r
+{\r
+  EFI_STATUS               Status;\r
+  DEBUG_AGENT_MAILBOX      *Mailbox;\r
   \r
-  return Status;\r
+  Status = EfiGetSystemConfigurationTable (&gEfiDebugAgentGuid, (VOID **) &Mailbox);\r
+  if (Status == EFI_SUCCESS && Mailbox != NULL) {\r
+    VerifyMailboxChecksum (Mailbox);\r
+    return Mailbox;\r
+  } else {\r
+    return NULL;\r
+  }\r
 }\r
 \r
 /**\r
@@ -104,13 +189,18 @@ GetMailboxFromHob (
   )\r
 {\r
   EFI_HOB_GUID_TYPE        *GuidHob;\r
+  UINT64                   *MailboxLocation;\r
+  DEBUG_AGENT_MAILBOX      *Mailbox;\r
 \r
   GuidHob = GetNextGuidHob (&gEfiDebugAgentGuid, HobStart);\r
   if (GuidHob == NULL) {\r
     return NULL;\r
   }\r
+  MailboxLocation = (UINT64 *) (GET_GUID_HOB_DATA(GuidHob));\r
+  Mailbox = (DEBUG_AGENT_MAILBOX *)(UINTN)(*MailboxLocation);\r
+  VerifyMailboxChecksum (Mailbox);\r
 \r
-  return (DEBUG_AGENT_MAILBOX *) (GET_GUID_HOB_DATA(GuidHob));\r
+  return Mailbox;\r
 }\r
 \r
 /**\r
@@ -124,6 +214,9 @@ GetMailboxPointer (
   VOID\r
   )\r
 {\r
+  AcquireMpSpinLock (&mDebugMpContext.MailboxSpinLock);\r
+  VerifyMailboxChecksum (mMailboxPointer);\r
+  ReleaseMpSpinLock (&mDebugMpContext.MailboxSpinLock);\r
   return mMailboxPointer;\r
 }\r
 \r
@@ -138,10 +231,98 @@ GetDebugPortHandle (
   VOID\r
   )\r
 {\r
-  return (DEBUG_PORT_HANDLE) (UINTN)(mMailboxPointer->DebugPortHandle);\r
+  return (DEBUG_PORT_HANDLE) (UINTN)(GetMailboxPointer ()->DebugPortHandle);\r
+}\r
+\r
+/**\r
+  Worker function to setup IDT table and initialize the IDT entries.\r
+\r
+  @param[in] Mailbox        Pointer to Mailbox.\r
+\r
+**/\r
+VOID\r
+SetupDebugAgentEnviroment (\r
+  IN DEBUG_AGENT_MAILBOX       *Mailbox\r
+  )\r
+{\r
+  IA32_DESCRIPTOR              Idtr;\r
+  UINT16                       IdtEntryCount;\r
+  UINT64                       DebugPortHandle;\r
+\r
+  if (mMultiProcessorDebugSupport) {\r
+    InitializeSpinLock (&mDebugMpContext.MpContextSpinLock);\r
+    InitializeSpinLock (&mDebugMpContext.DebugPortSpinLock);\r
+    InitializeSpinLock (&mDebugMpContext.MailboxSpinLock);\r
+    //\r
+    // Clear Break CPU index value\r
+    //\r
+    mDebugMpContext.BreakAtCpuIndex = (UINT32) -1;\r
+  }\r
+\r
+  //\r
+  // Get original IDT address and size.\r
+  //\r
+  AsmReadIdtr ((IA32_DESCRIPTOR *) &Idtr);\r
+  IdtEntryCount = (UINT16) ((Idtr.Limit + 1) / sizeof (IA32_IDT_GATE_DESCRIPTOR));\r
+  if (IdtEntryCount < 33) {\r
+    ZeroMem (&mIdtEntryTable, sizeof (IA32_IDT_GATE_DESCRIPTOR) * 33);\r
+    //\r
+    // Copy original IDT table into new one\r
+    //\r
+    CopyMem (&mIdtEntryTable, (VOID *) Idtr.Base, Idtr.Limit + 1);\r
+    //\r
+    // Load new IDT table\r
+    //\r
+    Idtr.Limit = (UINT16) (sizeof (IA32_IDT_GATE_DESCRIPTOR) * 33 - 1);\r
+    Idtr.Base  = (UINTN) &mIdtEntryTable;\r
+    AsmWriteIdtr ((IA32_DESCRIPTOR *) &Idtr);\r
+  }\r
+\r
+  //\r
+  // Initialize the IDT table entries to support source level debug.\r
+  //\r
+  InitializeDebugIdt ();\r
+\r
+  //\r
+  // If mMailboxPointer is not set before, set it\r
+  //\r
+  if (mMailboxPointer == NULL) {\r
+    if (Mailbox != NULL) {\r
+      //\r
+      // If Mailbox exists, copy it into one global variable\r
+      //\r
+      CopyMem (&mMailbox, Mailbox, sizeof (DEBUG_AGENT_MAILBOX));\r
+    } else {\r
+      ZeroMem (&mMailbox, sizeof (DEBUG_AGENT_MAILBOX));\r
+    }\r
+    mMailboxPointer = &mMailbox;\r
+  }\r
+\r
+  //\r
+  // Initialize debug communication port\r
+  //\r
+  DebugPortHandle = (UINT64) (UINTN)DebugPortInitialize ((VOID *)(UINTN)mMailboxPointer->DebugPortHandle, NULL);\r
+  UpdateMailboxContent (mMailboxPointer, DEBUG_MAILBOX_DEBUG_PORT_HANDLE_INDEX, DebugPortHandle);\r
+\r
+  if (Mailbox == NULL) {\r
+    //\r
+    // Trigger one software interrupt to inform HOST\r
+    //\r
+    TriggerSoftInterrupt (SYSTEM_RESET_SIGNATURE);\r
+    SetDebugFlag (DEBUG_AGENT_FLAG_MEMORY_READY, 1);\r
+    //\r
+    // Memory has been ready\r
+    //\r
+    if (IsHostAttached ()) {\r
+      //\r
+      // Trigger one software interrupt to inform HOST\r
+      //\r
+      TriggerSoftInterrupt (MEMORY_READY_SIGNATURE);\r
+    }\r
+  }\r
 }\r
 \r
\r
+\r
 /**\r
   Initialize debug agent.\r
 \r
@@ -167,105 +348,211 @@ InitializeDebugAgent (
   IN DEBUG_AGENT_CONTINUE  Function  OPTIONAL\r
   )\r
 {\r
+  UINT64                       *MailboxLocation;\r
   DEBUG_AGENT_MAILBOX          *Mailbox;\r
-  IA32_DESCRIPTOR              Idtr;\r
-  UINT16                       IdtEntryCount;\r
   BOOLEAN                      InterruptStatus;\r
+  VOID                         *HobList;\r
+  IA32_DESCRIPTOR              IdtDescriptor;\r
+  IA32_DESCRIPTOR              *Ia32Idtr;\r
+  IA32_IDT_ENTRY               *Ia32IdtEntry;\r
 \r
-  if (InitFlag != DEBUG_AGENT_INIT_DXE_CORE &&\r
-      InitFlag != DEBUG_AGENT_INIT_S3 &&\r
-      InitFlag != DEBUG_AGENT_INIT_DXE_AP) {\r
-    return;\r
+  if (InitFlag == DEBUG_AGENT_INIT_DXE_AP) {\r
+    //\r
+    // Invoked by AP, enable interrupt to let AP could receive IPI from other processors\r
+    //\r
+    EnableInterrupts ();\r
+    return ;\r
   }\r
 \r
+  // \r
+  // Disable Debug Timer interrupt\r
+  //\r
+  SaveAndSetDebugTimerInterrupt (FALSE);\r
   //\r
   // Save and disable original interrupt status\r
   //\r
   InterruptStatus = SaveAndDisableInterrupts ();\r
 \r
-  if (InitFlag == DEBUG_AGENT_INIT_DXE_CORE) {\r
+  //\r
+  // Try to get mailbox firstly\r
+  //\r
+  HobList         = NULL;\r
+  Mailbox         = NULL;\r
+  MailboxLocation = NULL;\r
+\r
+  switch (InitFlag) {\r
+\r
+  case DEBUG_AGENT_INIT_DXE_LOAD:\r
     //\r
-    // Try to get Mailbox from GUIDed HOB.\r
+    // Check if Debug Agent has been initialized before\r
     //\r
-    mDxeCoreFlag = TRUE;\r
-    Mailbox = GetMailboxFromHob (Context);\r
-    \r
+    if (IsDebugAgentInitialzed ()) {\r
+      DEBUG ((EFI_D_INFO, "Debug Agent: The former agent will be overwritten by the new one!\n"));\r
+    }\r
+\r
+    mMultiProcessorDebugSupport = TRUE;\r
     //\r
-    // Clear Break CPU index value\r
+    // Save original IDT table\r
+    //\r
+    AsmReadIdtr (&IdtDescriptor);\r
+    mSaveIdtTableSize = IdtDescriptor.Limit + 1;\r
+    mSavedIdtTable    = AllocateCopyPool (mSaveIdtTableSize, (VOID *) IdtDescriptor.Base);\r
+    //\r
+    // Initialize Debug Timer hardware\r
+    //\r
+    InitializeDebugTimer ();\r
+    //\r
+    // Check if Debug Agent initialized in DXE phase\r
+    //\r
+    Mailbox = GetMailboxFromConfigurationTable ();\r
+    if (Mailbox == NULL) {\r
+      //\r
+      // Try to get mailbox from GUIDed HOB build in PEI \r
+      //\r
+      HobList = GetHobList ();\r
+      Mailbox = GetMailboxFromHob (HobList);\r
+    }\r
+    //\r
+    // Set up IDT table and prepare for IDT entries\r
+    //\r
+    SetupDebugAgentEnviroment (Mailbox);\r
+    //\r
+    // For DEBUG_AGENT_INIT_S3, needn't to install configuration table and EFI Serial IO protocol\r
+    // For DEBUG_AGENT_INIT_DXE_CORE, InternalConstructorWorker() will invoked in Constructor()\r
+    //\r
+    InternalConstructorWorker ();\r
+    //\r
+    // Enable interrupt to receive Debug Timer interrupt\r
     //\r
-    mDebugMpContext.BreakAtCpuIndex = (UINT32) -1;\r
-\r
-  } else if (InitFlag == DEBUG_AGENT_INIT_DXE_AP) {\r
-\r
     EnableInterrupts ();\r
 \r
-    return;\r
+    mDebugAgentInitialized = TRUE;\r
+    FindAndReportModuleImageInfo (SIZE_4KB);\r
+\r
+    *(EFI_STATUS *)Context = EFI_SUCCESS;\r
+\r
+    if (gST->ConOut != NULL) {\r
+      Print (L"Debug Agent: Initialized successfully!\r\n");\r
+      Print (L"If the Debug Port is serial port, please make sure this serial port isn't connected by ISA Serial driver\r\n");\r
+      Print (L"You could do the following steps to disconnect the serial port:\r\n");\r
+      Print (L"1: Shell> drivers\r\n");\r
+      Print (L"   ...\r\n");\r
+      Print (L"   V  VERSION  E G G #D #C DRIVER NAME                         IMAGE NAME\r\n");\r
+      Print (L"   == ======== = = = == == =================================== ===================\r\n");\r
+      Print (L"   8F 0000000A B - -  1 14 PCI Bus Driver                      PciBusDxe\r\n");\r
+      Print (L"   91 00000010 ? - -  -  - ATA Bus Driver                      AtaBusDxe\r\n");\r
+      Print (L"   ...\r\n");\r
+      Print (L"   A7 0000000A B - -  1  1 ISA Serial Driver                   IsaSerialDxe\r\n");\r
+      Print (L"   ...\r\n");\r
+      Print (L"2: Shell> dh -d A7\r\n");\r
+      Print (L"   A7: Image(IsaSerialDxe) ImageDevPath (..9FB3-11D4-9A3A-0090273FC14D))DriverBinding ComponentName ComponentName2\r\n");\r
+      Print (L"        Driver Name    : ISA Serial Driver\r\n");\r
+      Print (L"        Image Name     : FvFile(93B80003-9FB3-11D4-9A3A-0090273FC14D)\r\n");\r
+      Print (L"        Driver Version : 0000000A\r\n");\r
+      Print (L"        Driver Type    : BUS\r\n");\r
+      Print (L"        Configuration  : NO\r\n");\r
+      Print (L"        Diagnostics    : NO\r\n");\r
+      Print (L"        Managing       :\r\n");\r
+      Print (L"          Ctrl[EA] : PciRoot(0x0)/Pci(0x1F,0x0)/Serial(0x0)\r\n");\r
+      Print (L"            Child[EB] : PciRoot(0x0)/Pci(0x1F,0x0)/Serial(0x0)/Uart(115200,8,N,1)\r\n");\r
+      Print (L"3: Shell> disconnect EA\r\n");\r
+      Print (L"4: Shell> load -nc DebugAgentDxe.efi\r\n\r\n");\r
+    }\r
+    break;\r
+\r
+  case DEBUG_AGENT_INIT_DXE_UNLOAD:\r
+    if (mDebugAgentInitialized) {\r
+      if (IsHostAttached ()) {\r
+        Print (L"Debug Agent: Host is still connected, please de-attach TARGET firstly!\r\n");\r
+        *(EFI_STATUS *)Context = EFI_ACCESS_DENIED;\r
+        // \r
+        // Enable Debug Timer interrupt again\r
+        //\r
+        SaveAndSetDebugTimerInterrupt (TRUE);\r
+      } else {\r
+        //\r
+        // Restore original IDT table\r
+        //\r
+        AsmReadIdtr (&IdtDescriptor);\r
+        IdtDescriptor.Limit = (UINT16) (mSaveIdtTableSize - 1);\r
+        CopyMem ((VOID *) IdtDescriptor.Base, mSavedIdtTable, mSaveIdtTableSize);\r
+        AsmWriteIdtr (&IdtDescriptor);\r
+        FreePool (mSavedIdtTable);\r
+        mDebugAgentInitialized = FALSE;\r
+        *(EFI_STATUS *)Context = EFI_SUCCESS;\r
+      }\r
+    } else {\r
+      Print (L"Debug Agent: It hasn't been initialized, cannot unload it!\r\n");\r
+      *(EFI_STATUS *)Context = EFI_NOT_STARTED;\r
+    }\r
 \r
-  } else {\r
     //\r
-    // If it is in S3 path, needn't to install configuration table.\r
+    // Restore interrupt state.\r
     //\r
-    Mailbox = NULL;\r
-  }\r
+    SetInterruptState (InterruptStatus);\r
+    break;\r
 \r
-  if (Mailbox != NULL) {\r
+  case DEBUG_AGENT_INIT_DXE_CORE:\r
+    mDxeCoreFlag                = TRUE;\r
+    mMultiProcessorDebugSupport = TRUE;\r
     //\r
-    // If Mailbox exists, copy it into one global variable.\r
+    // Initialize Debug Timer hardware\r
     //\r
-    CopyMem (&mMailbox, Mailbox, sizeof (DEBUG_AGENT_MAILBOX));\r
-  } else {\r
+    InitializeDebugTimer ();\r
     //\r
-    // If Mailbox not exists, used the local Mailbox.\r
+    // Try to get mailbox from GUIDed HOB build in PEI \r
     //\r
-    ZeroMem (&mMailbox, sizeof (DEBUG_AGENT_MAILBOX));\r
-  }\r
-\r
-  mMailboxPointer = &mMailbox;\r
-\r
-  //\r
-  // Get original IDT address and size.\r
-  //\r
-  AsmReadIdtr ((IA32_DESCRIPTOR *) &Idtr);\r
-  IdtEntryCount = (UINT16) ((Idtr.Limit + 1) / sizeof (IA32_IDT_GATE_DESCRIPTOR));\r
-  if (IdtEntryCount < 33) {\r
-    Idtr.Limit = (UINT16) (sizeof (IA32_IDT_GATE_DESCRIPTOR) * 33 - 1);\r
-    Idtr.Base  = (UINTN) &mIdtEntryTable;\r
-    ZeroMem (&mIdtEntryTable, Idtr.Limit + 1);\r
-    AsmWriteIdtr ((IA32_DESCRIPTOR *) &Idtr);\r
-  }\r
+    HobList = Context;\r
+    Mailbox = GetMailboxFromHob (HobList);\r
+    //\r
+    // Set up IDT table and prepare for IDT entries\r
+    //\r
+    SetupDebugAgentEnviroment (Mailbox);\r
+    //\r
+    // Enable interrupt to receive Debug Timer interrupt\r
+    //\r
+    EnableInterrupts ();\r
 \r
-  //\r
-  // Initialize the IDT table entries to support source level debug.\r
-  //\r
-  InitializeDebugIdt ();\r
+    break;\r
 \r
-  //\r
-  // Initialize debug communication port\r
-  //\r
-  mMailboxPointer->DebugPortHandle = (UINT64) (UINTN)DebugPortInitialize ((VOID *)(UINTN)mMailbox.DebugPortHandle, NULL);\r
+  case DEBUG_AGENT_INIT_S3:\r
 \r
-  InitializeSpinLock (&mDebugMpContext.MpContextSpinLock);\r
-  InitializeSpinLock (&mDebugMpContext.DebugPortSpinLock);\r
\r
-  if (InitFlag == DEBUG_AGENT_INIT_DXE_CORE) {\r
+    if (Context != NULL) {\r
+      Ia32Idtr =  (IA32_DESCRIPTOR *) Context;\r
+      Ia32IdtEntry = (IA32_IDT_ENTRY *)(Ia32Idtr->Base);\r
+      MailboxLocation = (UINT64 *) (UINTN) (Ia32IdtEntry[DEBUG_MAILBOX_VECTOR].Bits.OffsetLow + \r
+                                           (Ia32IdtEntry[DEBUG_MAILBOX_VECTOR].Bits.OffsetHigh << 16));\r
+      Mailbox = (DEBUG_AGENT_MAILBOX *)(UINTN)(*MailboxLocation);\r
+      VerifyMailboxChecksum (Mailbox);\r
+    }\r
     //\r
-    // Initialize Debug Timer hardware and enable interrupt.\r
+    // Save Mailbox pointer in global variable\r
     //\r
-    InitializeDebugTimer ();\r
-    EnableInterrupts ();\r
-\r
-    return;\r
-  } else {\r
+    mMailboxPointer = Mailbox;\r
     //\r
-    // Disable Debug Timer interrupt in S3 path.\r
+    // Set up IDT table and prepare for IDT entries\r
     //\r
-    SaveAndSetDebugTimerInterrupt (FALSE);\r
-\r
+    SetupDebugAgentEnviroment (Mailbox);\r
     //\r
-    // Restore interrupt state.\r
+    // Disable interrupt\r
     //\r
-    SetInterruptState (InterruptStatus);\r
+    DisableInterrupts ();\r
+    FindAndReportModuleImageInfo (SIZE_4KB);\r
+    if (GetDebugFlag (DEBUG_AGENT_FLAG_BREAK_BOOT_SCRIPT) == 1) {\r
+      //\r
+      // If Boot Script entry break is set, code will be break at here.\r
+      //\r
+      CpuBreakpoint ();\r
+    }\r
+    break;\r
+\r
+  default:\r
+    //\r
+    // Only DEBUG_AGENT_INIT_PREMEM_SEC and DEBUG_AGENT_INIT_POSTMEM_SEC are allowed for this \r
+    // Debug Agent library instance.\r
+    //\r
+    DEBUG ((EFI_D_ERROR, "Debug Agent: The InitFlag value is not allowed!\n"));\r
+    CpuDeadLoop ();\r
+    break;\r
   }\r
-\r
 }\r
-\r