Changeset 58459 in vbox for trunk/src/VBox/Devices/EFI/Firmware/SourceLevelDebugPkg/Library/DebugAgent

Timestamp:

Oct 28, 2015 8:17:18 PM (9 years ago)

Author:

vboxsync

Message:

EFI/Firmware: 'svn merge ^{/vendor/edk2/UDK2010.SR1} /vendor/edk2/current .', reverting and removing files+dirs listed in ReadMe.vbox, resolving conflicts with help from ../UDK2014.SP1/. This is a raw untested merge.

Location:

trunk/src/VBox/Devices/EFI/Firmware

Files:

• . (modified) (1 prop)
• SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/DebugAgent.c (modified) (39 diffs)
• SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/DebugAgent.h (modified) (12 diffs)
• SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/DebugMp.c (modified) (19 diffs)
• SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/DebugMp.h (modified) (8 diffs)
• SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/DebugTimer.c (modified) (4 diffs)
• SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/DebugTimer.h (modified) (2 diffs)
• SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/Ia32/ArchDebugSupport.c (modified) (5 diffs)
• SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/Ia32/ArchDebugSupport.h (modified) (2 diffs)
• SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/Ia32/ArchReadGroupRegister.c (deleted)
• SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/Ia32/ArchRegisters.h (deleted)
• SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/Ia32/AsmFuncs.S (modified) (25 diffs)
• SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/Ia32/AsmFuncs.asm (modified) (25 diffs)
• SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/X64/ArchDebugSupport.c (modified) (4 diffs)
• SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/X64/ArchDebugSupport.h (modified) (2 diffs)
• SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/X64/ArchReadGroupRegister.c (deleted)
• SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/X64/ArchRegisters.h (deleted)
• SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/X64/AsmFuncs.S (modified) (27 diffs)
• SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/X64/AsmFuncs.asm (modified) (25 diffs)
• SourceLevelDebugPkg/Library/DebugAgent/DxeDebugAgent/DxeDebugAgentLib.c (modified) (9 diffs)
• SourceLevelDebugPkg/Library/DebugAgent/DxeDebugAgent/DxeDebugAgentLib.h (modified) (2 diffs)
• SourceLevelDebugPkg/Library/DebugAgent/DxeDebugAgentLib.inf (modified) (5 diffs)
• SourceLevelDebugPkg/Library/DebugAgent/SecPeiDebugAgent/SecPeiDebugAgentLib.c (modified) (14 diffs)
• SourceLevelDebugPkg/Library/DebugAgent/SecPeiDebugAgent/SecPeiDebugAgentLib.h (modified) (2 diffs)
• SourceLevelDebugPkg/Library/DebugAgent/SecPeiDebugAgentLib.inf (modified) (4 diffs)
• SourceLevelDebugPkg/Library/DebugAgent/SmmDebugAgent/SmmDebugAgentLib.c (modified) (7 diffs)
• SourceLevelDebugPkg/Library/DebugAgent/SmmDebugAgent/SmmDebugAgentLib.h (modified) (2 diffs)
• SourceLevelDebugPkg/Library/DebugAgent/SmmDebugAgentLib.inf (modified) (4 diffs)

trunk/src/VBox/Devices/EFI/Firmware/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/DebugAgent.c

@@ -5 +5 @@
   protocol.
 
-  Copyright (c) 2010, Intel Corporation. All rights reserved.<BR>
+  Copyright (c) 2010 - 2014, Intel Corporation. All rights reserved.<BR>
   This program and the accompanying materials
   are licensed and made available under the terms and conditions of the BSD License
@@ -19 +19 @@
 #include "Ia32/DebugException.h"
 
-/**
-  Check if HOST is connected based on Mailbox.
-
-  @retval TRUE        HOST is connected.
-  @retval FALSE       HOST is not connected.
+GLOBAL_REMOVE_IF_UNREFERENCED CHAR8 mErrorMsgVersionAlert[]       = "\rThe SourceLevelDebugPkg you are using requires a newer version of the Intel(R) UDK Debugger Tool.\r\n";
+GLOBAL_REMOVE_IF_UNREFERENCED CHAR8 mErrorMsgSendInitPacket[]     = "\rSend INIT break packet and try to connect the HOST (Intel(R) UDK Debugger Tool v1.4) ...\r\n";
+GLOBAL_REMOVE_IF_UNREFERENCED CHAR8 mErrorMsgConnectOK[]          = "HOST connection is successful!\r\n";
+GLOBAL_REMOVE_IF_UNREFERENCED CHAR8 mErrorMsgConnectFail[]        = "HOST connection is failed!\r\n";
+GLOBAL_REMOVE_IF_UNREFERENCED CHAR8 mWarningMsgIngoreBreakpoint[] = "Ignore break point in SMM for SMI issued during DXE debugging!\r\n";
+
+//
+// Vector Handoff Info list used by Debug Agent for persist
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_VECTOR_HANDOFF_INFO mVectorHandoffInfoDebugAgent[] = {
+  {
+    DEBUG_EXCEPT_DIVIDE_ERROR,         // Vector 0
+    EFI_VECTOR_HANDOFF_HOOK_BEFORE,
+    EFI_DEBUG_AGENT_GUID
+  },
+  {
+    DEBUG_EXCEPT_DEBUG,                // Vector 1
+    EFI_VECTOR_HANDOFF_DO_NOT_HOOK,
+    EFI_DEBUG_AGENT_GUID
+  },
+  {
+    DEBUG_EXCEPT_NMI,                  // Vector 2
+    EFI_VECTOR_HANDOFF_HOOK_BEFORE,
+    EFI_DEBUG_AGENT_GUID
+  },
+  {
+    DEBUG_EXCEPT_BREAKPOINT,           // Vector 3
+    EFI_VECTOR_HANDOFF_DO_NOT_HOOK,
+    EFI_DEBUG_AGENT_GUID
+  },
+  {
+    DEBUG_EXCEPT_OVERFLOW,             // Vector 4
+    EFI_VECTOR_HANDOFF_HOOK_BEFORE,
+    EFI_DEBUG_AGENT_GUID
+  },
+  {
+    DEBUG_EXCEPT_BOUND,                // Vector 5
+    EFI_VECTOR_HANDOFF_HOOK_BEFORE,
+    EFI_DEBUG_AGENT_GUID
+  },
+  {
+    DEBUG_EXCEPT_INVALID_OPCODE,       // Vector 6
+    EFI_VECTOR_HANDOFF_HOOK_BEFORE,
+    EFI_DEBUG_AGENT_GUID
+  },
+  {
+    DEBUG_EXCEPT_DOUBLE_FAULT,         // Vector 8
+    EFI_VECTOR_HANDOFF_HOOK_BEFORE,
+    EFI_DEBUG_AGENT_GUID
+  },
+  {
+    DEBUG_EXCEPT_INVALID_TSS,          // Vector 10
+    EFI_VECTOR_HANDOFF_HOOK_BEFORE,
+    EFI_DEBUG_AGENT_GUID
+  },
+  {
+    DEBUG_EXCEPT_SEG_NOT_PRESENT,      // Vector 11
+    EFI_VECTOR_HANDOFF_HOOK_BEFORE,
+    EFI_DEBUG_AGENT_GUID
+  },
+  {
+    DEBUG_EXCEPT_STACK_FAULT,          // Vector 12
+    EFI_VECTOR_HANDOFF_HOOK_BEFORE,
+    EFI_DEBUG_AGENT_GUID
+  },
+  {
+    DEBUG_EXCEPT_GP_FAULT,             // Vector 13
+    EFI_VECTOR_HANDOFF_HOOK_BEFORE,
+    EFI_DEBUG_AGENT_GUID
+  },
+    {
+    DEBUG_EXCEPT_PAGE_FAULT,           // Vector 14
+    EFI_VECTOR_HANDOFF_HOOK_BEFORE,
+    EFI_DEBUG_AGENT_GUID
+  },
+  {
+    DEBUG_EXCEPT_FP_ERROR,             // Vector 16
+    EFI_VECTOR_HANDOFF_HOOK_BEFORE,
+    EFI_DEBUG_AGENT_GUID
+  },
+  {
+    DEBUG_EXCEPT_ALIGNMENT_CHECK,      // Vector 17
+    EFI_VECTOR_HANDOFF_HOOK_BEFORE,
+    EFI_DEBUG_AGENT_GUID
+  },
+  {
+    DEBUG_EXCEPT_MACHINE_CHECK,        // Vector 18
+    EFI_VECTOR_HANDOFF_HOOK_BEFORE,
+    EFI_DEBUG_AGENT_GUID
+  },
+  {
+    DEBUG_EXCEPT_SIMD,                 // Vector 19
+    EFI_VECTOR_HANDOFF_HOOK_BEFORE,
+    EFI_DEBUG_AGENT_GUID
+  },
+  {
+    DEBUG_TIMER_VECTOR,                // Vector 32
+    EFI_VECTOR_HANDOFF_DO_NOT_HOOK,
+    EFI_DEBUG_AGENT_GUID
+  },
+  {
+    DEBUG_MAILBOX_VECTOR,              // Vector 33
+    EFI_VECTOR_HANDOFF_DO_NOT_HOOK,
+    EFI_DEBUG_AGENT_GUID
+  },
+  {
+    0,
+    EFI_VECTOR_HANDOFF_LAST_ENTRY,
+    { 0 }
+  }
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED UINTN mVectorHandoffInfoCount = sizeof (mVectorHandoffInfoDebugAgent) / sizeof (EFI_VECTOR_HANDOFF_INFO);
+
+/**
+  Calculate CRC16 for target data.
+
+  @param[in]  Data              The target data.
+  @param[in]  DataSize          The target data size.
+  @param[in]  Crc               Initial CRC.
+
+  @return UINT16     The CRC16 value.
+
+**/
+UINT16
+CalculateCrc16 (
+  IN UINT8   *Data,
+  IN UINTN   DataSize,
+  IN UINT16  Crc
+  )
+{
+  UINTN  Index;
+  UINTN  BitIndex;
+
+  for (Index = 0; Index < DataSize; Index++) {
+    Crc ^= (UINT16)Data[Index];
+    for (BitIndex = 0; BitIndex < 8; BitIndex++) {
+      if ((Crc & 0x8000) != 0) {
+        Crc <<= 1;
+        Crc ^= 0x1021;
+      } else {
+        Crc <<= 1;
+      }
+    }
+  }
+  return Crc;
+}
+
+
+/**
+  Read IDT entry to check if IDT entries are setup by Debug Agent.
+
+  @retval  TRUE     IDT entries were setup by Debug Agent.
+  @retval  FALSE    IDT entries were not setup by Debug Agent.
 
 **/
 BOOLEAN
-IsHostConnected (
+IsDebugAgentInitialzed (
   VOID
   )
 {
-  DEBUG_AGENT_MAILBOX          *Mailbox;
-
-  Mailbox = GetMailboxPointer ();
-
-  if (Mailbox->DebugFlag.Bits.HostPresent == 1) {
+  UINTN                      InterruptHandler;
+
+  InterruptHandler = (UINTN) GetExceptionHandlerInIdtEntry (0);
+  if (InterruptHandler >= 4 &&  *(UINT32 *)(InterruptHandler - 4) == AGENT_HANDLER_SIGNATURE) {
     return TRUE;
   } else {
@@ -43 +191 @@
 
 /**
-  Set HOST connect flag in Mailbox.
+  Find and report module image info to HOST.
+
+  @param[in] AlignSize      Image aligned size.
 
 **/
 VOID
-SetHostConnectedFlag (
-  VOID
-  )
-{
-  DEBUG_AGENT_MAILBOX          *Mailbox;
-
-  Mailbox = GetMailboxPointer ();
-
-  Mailbox->DebugFlag.Bits.HostPresent = 1;
-}
-
-/**
-  Set debug flag of Debug Agent in Mailbox.
-
-  @param DebugFlag       Debug Flag defined by transfer protocol.
+FindAndReportModuleImageInfo (
+  IN UINTN          AlignSize
+  )
+{
+  UINTN                                Pe32Data;
+  EFI_IMAGE_DOS_HEADER                 *DosHdr;
+  EFI_IMAGE_OPTIONAL_HEADER_PTR_UNION  Hdr;
+  PE_COFF_LOADER_IMAGE_CONTEXT         ImageContext;
+
+  //
+  // Find Image Base
+  //
+  Pe32Data = ((UINTN)mErrorMsgVersionAlert) & ~(AlignSize - 1);
+  while (Pe32Data != 0) {
+    DosHdr = (EFI_IMAGE_DOS_HEADER *) Pe32Data;
+    if (DosHdr->e_magic == EFI_IMAGE_DOS_SIGNATURE) {
+      //
+      // DOS image header is present, so read the PE header after the DOS image header.
+      //
+      Hdr.Pe32 = (EFI_IMAGE_NT_HEADERS32 *)(Pe32Data + (UINTN) ((DosHdr->e_lfanew) & 0x0ffff));
+      //
+      // Make sure PE header address does not overflow and is less than the initial address.
+      //
+      if (((UINTN)Hdr.Pe32 > Pe32Data) && ((UINTN)Hdr.Pe32 < (UINTN)mErrorMsgVersionAlert)) {
+        if (Hdr.Pe32->Signature == EFI_IMAGE_NT_SIGNATURE) {
+          //
+          // It's PE image.
+          //
+          break;
+        }
+      }
+    } else {
+      //
+      // DOS image header is not present, TE header is at the image base.
+      //
+      Hdr.Pe32 = (EFI_IMAGE_NT_HEADERS32 *)Pe32Data;
+      if ((Hdr.Te->Signature == EFI_TE_IMAGE_HEADER_SIGNATURE) &&
+          ((Hdr.Te->Machine == IMAGE_FILE_MACHINE_I386) || Hdr.Te->Machine == IMAGE_FILE_MACHINE_X64)) {
+        //
+        // It's TE image, it TE header and Machine type match
+        //
+        break;
+      }
+    }
+
+    //
+    // Not found the image base, check the previous aligned address
+    //
+    Pe32Data -= AlignSize;
+  }
+
+  ImageContext.ImageAddress = Pe32Data;
+  ImageContext.PdbPointer = PeCoffLoaderGetPdbPointer ((VOID*) (UINTN) ImageContext.ImageAddress);
+  PeCoffLoaderRelocateImageExtraAction (&ImageContext);
+}
+
+/**
+  Trigger one software interrupt to debug agent to handle it.
+
+  @param[in] Signature       Software interrupt signature.
+
+**/
+VOID
+TriggerSoftInterrupt (
+  IN UINT32                 Signature
+  )
+{
+  UINTN                  Dr0;
+  UINTN                  Dr1;
+
+  //
+  // Save Debug Register State
+  //
+  Dr0 = AsmReadDr0 ();
+  Dr1 = AsmReadDr1 ();
+
+  //
+  // DR0 = Signature
+  //
+  AsmWriteDr0 (SOFT_INTERRUPT_SIGNATURE);
+  AsmWriteDr1 (Signature);
+
+  //
+  // Do INT3 to communicate with HOST side
+  //
+  CpuBreakpoint ();
+
+  //
+  // Restore Debug Register State only when Host didn't change it inside exception handler.
+  //   Dr registers can only be changed by setting the HW breakpoint.
+  //
+  AsmWriteDr0 (Dr0);
+  AsmWriteDr1 (Dr1);
+
+}
+
+/**
+  Caculate Mailbox checksum and update the checksum field.
+
+  @param[in]  Mailbox  Debug Agent Mailbox pointer.
+
+**/
+VOID
+UpdateMailboxChecksum (
+  IN DEBUG_AGENT_MAILBOX    *Mailbox
+  )
+{
+  Mailbox->CheckSum = CalculateCheckSum8 ((UINT8 *)Mailbox, sizeof (DEBUG_AGENT_MAILBOX) - 2);
+}
+
+/**
+  Verify Mailbox checksum.
+
+  If checksum error, print debug message and run init dead loop.
+
+  @param[in]  Mailbox  Debug Agent Mailbox pointer.
+
+**/
+VOID
+VerifyMailboxChecksum (
+  IN DEBUG_AGENT_MAILBOX    *Mailbox
+  )
+{
+  UINT8                     CheckSum;
+
+  CheckSum = CalculateCheckSum8 ((UINT8 *) Mailbox, sizeof (DEBUG_AGENT_MAILBOX) - 2);
+  //
+  // The checksum updating process may be disturbed by hardware SMI, we need to check CheckSum field
+  // and ToBeCheckSum field to validate the mail box.
+  //
+  if (CheckSum != Mailbox->CheckSum && CheckSum != Mailbox->ToBeCheckSum) {
+    DEBUG ((EFI_D_ERROR, "DebugAgent: Mailbox checksum error, stack or heap crashed!\n"));
+    DEBUG ((EFI_D_ERROR, "DebugAgent: CheckSum = %x, Mailbox->CheckSum = %x, Mailbox->ToBeCheckSum = %x\n", CheckSum, Mailbox->CheckSum, Mailbox->ToBeCheckSum));
+    CpuDeadLoop ();
+  }
+}
+
+/**
+  Update Mailbox content by index.
+
+  @param[in]  Mailbox  Debug Agent Mailbox pointer.
+  @param[in]  Index    Mailbox content index.
+  @param[in]  Value    Value to be set into Mailbox.
+
+**/
+VOID
+UpdateMailboxContent (
+  IN DEBUG_AGENT_MAILBOX    *Mailbox,
+  IN UINTN                  Index,
+  IN UINT64                 Value
+  )
+{
+  AcquireMpSpinLock (&mDebugMpContext.MailboxSpinLock);
+  switch (Index) {
+  case DEBUG_MAILBOX_DEBUG_FLAG_INDEX:
+    Mailbox->ToBeCheckSum = Mailbox->CheckSum + CalculateSum8 ((UINT8 *)&Mailbox->DebugFlag.Uint64, sizeof(UINT64))
+                                              - CalculateSum8 ((UINT8 *)&Value, sizeof(UINT64));
+    Mailbox->DebugFlag.Uint64 = Value;
+    break;
+  case DEBUG_MAILBOX_DEBUG_PORT_HANDLE_INDEX:
+    Mailbox->ToBeCheckSum = Mailbox->CheckSum + CalculateSum8 ((UINT8 *)&Mailbox->DebugPortHandle, sizeof(UINTN))
+                                              - CalculateSum8 ((UINT8 *)&Value, sizeof(UINTN));
+    Mailbox->DebugPortHandle = (UINTN) Value;
+    break;
+  case DEBUG_MAILBOX_EXCEPTION_BUFFER_POINTER_INDEX:
+    Mailbox->ToBeCheckSum = Mailbox->CheckSum + CalculateSum8 ((UINT8 *)&Mailbox->ExceptionBufferPointer, sizeof(UINTN))
+                                              - CalculateSum8 ((UINT8 *)&Value, sizeof(UINTN));
+    Mailbox->ExceptionBufferPointer = (UINTN) Value;
+    break;
+  case DEBUG_MAILBOX_LAST_ACK:
+    Mailbox->ToBeCheckSum = Mailbox->CheckSum + CalculateSum8 ((UINT8 *)&Mailbox->LastAck, sizeof(UINT8))
+                                              - CalculateSum8 ((UINT8 *)&Value, sizeof(UINT8));
+    Mailbox->LastAck = (UINT8) Value;
+    break;
+  case DEBUG_MAILBOX_SEQUENCE_NO_INDEX:
+    Mailbox->ToBeCheckSum = Mailbox->CheckSum + CalculateSum8 ((UINT8 *)&Mailbox->SequenceNo, sizeof(UINT8))
+                                              - CalculateSum8 ((UINT8 *)&Value, sizeof(UINT8));
+    Mailbox->SequenceNo = (UINT8) Value;
+    break;
+  case DEBUG_MAILBOX_HOST_SEQUENCE_NO_INDEX:
+    Mailbox->ToBeCheckSum = Mailbox->CheckSum + CalculateSum8 ((UINT8 *)&Mailbox->HostSequenceNo, sizeof(UINT8))
+                                              - CalculateSum8 ((UINT8 *)&Value, sizeof(UINT8));
+    Mailbox->HostSequenceNo = (UINT8) Value;
+    break;
+  }
+  UpdateMailboxChecksum (Mailbox);
+  ReleaseMpSpinLock (&mDebugMpContext.MailboxSpinLock);
+}
+/**
+  Set debug flag in mailbox.
+
+  @param[in]  FlagMask      Debug flag mask value.
+  @param[in]  FlagValue     Debug flag value.
 
 **/
 VOID
 SetDebugFlag (
-  IN UINT32               DebugFlag
-  )
-{
-  DEBUG_AGENT_MAILBOX          *Mailbox;
+  IN UINT64                 FlagMask,
+  IN UINT32                 FlagValue
+  )
+{
+  DEBUG_AGENT_MAILBOX    *Mailbox;
+  UINT64                 Data64;
 
   Mailbox = GetMailboxPointer ();
-
-  if ((DebugFlag & SOFT_DEBUGGER_SETTING_SMM_ENTRY_BREAK) != 0) {
-    Mailbox->DebugFlag.Bits.BreakOnNextSmi = 1;
+  Data64 = (Mailbox->DebugFlag.Uint64 & ~FlagMask) |
+           (LShiftU64 ((UINT64)FlagValue, LowBitSet64 (FlagMask)) & FlagMask);
+  UpdateMailboxContent (Mailbox, DEBUG_MAILBOX_DEBUG_FLAG_INDEX, Data64);
+}
+
+/**
+  Get debug flag in mailbox.
+
+  @param[in]  FlagMask      Debug flag mask value.
+
+  @return Debug flag value.
+
+**/
+UINT32
+GetDebugFlag (
+  IN UINT64                 FlagMask
+  )
+{
+  DEBUG_AGENT_MAILBOX    *Mailbox;
+  UINT32                 DebugFlag;
+
+  Mailbox = GetMailboxPointer ();
+  DebugFlag = (UINT32) RShiftU64 (Mailbox->DebugFlag.Uint64 & FlagMask, LowBitSet64 (FlagMask));
+
+  return DebugFlag;
+}
+
+/**
+  Send a debug message packet to the debug port.
+
+  @param[in] Buffer  The debug message.
+  @param[in] Length  The length of debug message.
+
+**/
+VOID
+SendDebugMsgPacket (
+  IN CHAR8         *Buffer,
+  IN UINTN         Length
+  )
+{
+  DEBUG_PACKET_HEADER  DebugHeader;
+  DEBUG_PORT_HANDLE    Handle;
+
+  Handle = GetDebugPortHandle();
+
+  DebugHeader.StartSymbol = DEBUG_STARTING_SYMBOL_NORMAL;
+  DebugHeader.Command     = DEBUG_COMMAND_PRINT_MESSAGE;
+  DebugHeader.Length      = sizeof (DEBUG_PACKET_HEADER) + (UINT8) Length;
+  DebugHeader.SequenceNo  = 0xEE;
+  DebugHeader.Crc         = 0;
+  DebugHeader.Crc         = CalculateCrc16 (
+                              (UINT8 *)Buffer, Length,
+                              CalculateCrc16 ((UINT8 *)&DebugHeader, sizeof (DEBUG_PACKET_HEADER), 0)
+                              );
+
+  DebugPortWriteBuffer (Handle, (UINT8 *)&DebugHeader, sizeof (DEBUG_PACKET_HEADER));
+  DebugPortWriteBuffer (Handle, (UINT8 *)Buffer, Length);
+}
+
+/**
+  Prints a debug message to the debug port if the specified error level is enabled.
+
+  If any bit in ErrorLevel is also set in Mainbox, then print the message specified
+  by Format and the associated variable argument list to the debug port.
+
+  @param[in] ErrorLevel  The error level of the debug message.
+  @param[in] Format      Format string for the debug message to print.
+  @param[in] ...         Variable argument list whose contents are accessed
+                         based on the format string specified by Format.
+
+**/
+VOID
+EFIAPI
+DebugAgentMsgPrint (
+  IN UINT8         ErrorLevel,
+  IN CHAR8         *Format,
+  ...
+  )
+{
+  CHAR8                Buffer[DEBUG_DATA_MAXIMUM_REAL_DATA];
+  VA_LIST              Marker;
+
+  //
+  // Check driver debug mask value and global mask
+  //
+  if ((ErrorLevel & GetDebugFlag (DEBUG_AGENT_FLAG_PRINT_ERROR_LEVEL)) == 0) {
+    return;
+  }
+
+  //
+  // Convert the DEBUG() message to an ASCII String
+  //
+  VA_START (Marker, Format);
+  AsciiVSPrint (Buffer, sizeof (Buffer), Format, Marker);
+  VA_END (Marker);
+
+  SendDebugMsgPacket (Buffer, AsciiStrLen (Buffer));
+}
+
+/**
+  Prints a debug message to the debug output device if the specified error level is enabled.
+
+  If any bit in ErrorLevel is also set in DebugPrintErrorLevelLib function
+  GetDebugPrintErrorLevel (), then print the message specified by Format and the
+  associated variable argument list to the debug output device.
+
+  If Format is NULL, then ASSERT().
+
+  @param[in] ErrorLevel  The error level of the debug message.
+  @param[in] IsSend      Flag of debug message to declare that the data is being sent or being received.
+  @param[in] Data        Variable argument list whose contents are accessed
+  @param[in] Length      based on the format string specified by Format.
+
+**/
+VOID
+EFIAPI
+DebugAgentDataMsgPrint (
+  IN UINT8             ErrorLevel,
+  IN BOOLEAN           IsSend,
+  IN UINT8             *Data,
+  IN UINT8             Length
+  )
+{
+  CHAR8                Buffer[DEBUG_DATA_MAXIMUM_REAL_DATA];
+  CHAR8                *DestBuffer;
+  UINTN                Index;
+
+  //
+  // Check driver debug mask value and global mask
+  //
+  if ((ErrorLevel & GetDebugFlag (DEBUG_AGENT_FLAG_PRINT_ERROR_LEVEL)) == 0) {
+    return;
+  }
+
+  DestBuffer = Buffer;
+  if (IsSend) {
+    DestBuffer += AsciiSPrint (DestBuffer, DEBUG_DATA_MAXIMUM_REAL_DATA, "Sent data [ ");
   } else {
-    Mailbox->DebugFlag.Bits.BreakOnNextSmi = 0;
-  }
+    DestBuffer += AsciiSPrint (DestBuffer, DEBUG_DATA_MAXIMUM_REAL_DATA, "Received data [ ");
+  }
+
+  Index = 0;
+  while (TRUE) {
+    if (DestBuffer - Buffer > DEBUG_DATA_MAXIMUM_REAL_DATA - 6) {
+      //
+      // If there was no enough space in buffer, send out the debug message,
+      // reserving 6 bytes is for the last data and end characters "]\n".
+      //
+      SendDebugMsgPacket (Buffer, DestBuffer - Buffer);
+      DestBuffer = Buffer;
+    }
+    DestBuffer += AsciiSPrint (DestBuffer, DEBUG_DATA_MAXIMUM_REAL_DATA - (DestBuffer - Buffer), "%02x ", Data[Index]);
+    Index ++;
+    if (Index >= Length) {
+      //
+      // The last character of debug message has been foramtted in buffer
+      //
+      DestBuffer += AsciiSPrint(DestBuffer, DEBUG_DATA_MAXIMUM_REAL_DATA - (DestBuffer - Buffer), "]\n");
+      SendDebugMsgPacket (Buffer, DestBuffer - Buffer);
+      break;
+    }
+  }
+}
+
+/**
+  Read remaing debug packet except for the start symbol
+
+  @param[in]      Handle        Pointer to Debug Port handle.
+  @param[in, out] DebugHeader   Debug header buffer including start symbol.
+
+  @retval EFI_SUCCESS        Read the symbol in BreakSymbol.
+  @retval EFI_CRC_ERROR      CRC check fail.
+  @retval EFI_TIMEOUT        Timeout occurs when reading debug packet.
+  @retval EFI_DEVICE_ERROR   Receive the old or responsed packet.
+
+**/
+EFI_STATUS
+ReadRemainingBreakPacket (
+  IN     DEBUG_PORT_HANDLE      Handle,
+  IN OUT DEBUG_PACKET_HEADER    *DebugHeader
+  )
+{
+  UINT16                     Crc;
+  DEBUG_AGENT_MAILBOX        *Mailbox;
+
+  //
+  // Has received start symbol, try to read the rest part
+  //
+  if (DebugPortReadBuffer (Handle, (UINT8 *)DebugHeader + OFFSET_OF (DEBUG_PACKET_HEADER, Command), sizeof (DEBUG_PACKET_HEADER) - OFFSET_OF (DEBUG_PACKET_HEADER, Command), READ_PACKET_TIMEOUT) == 0) {
+    //
+    // Timeout occur, exit
+    //
+    DebugAgentMsgPrint (DEBUG_AGENT_WARNING, "Timeout in Debug Timer interrupt\n");
+    return EFI_TIMEOUT;
+  }
+
+  Crc = DebugHeader->Crc;
+  DebugHeader->Crc = 0;
+  if (CalculateCrc16 ((UINT8 *)DebugHeader, DebugHeader->Length, 0) != Crc) {
+    DebugAgentMsgPrint (DEBUG_AGENT_WARNING, "Debug Timer CRC (%x) against (%x)\n", Crc, CalculateCrc16 ((UINT8 *) &DebugHeader, DebugHeader->Length, 0));
+    DebugAgentDataMsgPrint (DEBUG_AGENT_VERBOSE, FALSE, (UINT8 *)DebugHeader, DebugHeader->Length);
+    return EFI_CRC_ERROR;
+  }
+  Mailbox = GetMailboxPointer();
+  if (((DebugHeader->Command & DEBUG_COMMAND_RESPONSE) == 0) &&
+       (DebugHeader->SequenceNo == (UINT8) (Mailbox->HostSequenceNo + 1))) {
+    //
+    // Only updagte HostSequenceNo for new command packet
+    //
+    UpdateMailboxContent (Mailbox, DEBUG_MAILBOX_HOST_SEQUENCE_NO_INDEX, DebugHeader->SequenceNo);
+    return EFI_SUCCESS;
+  } else {
+    //
+    // If one old command or response packet received, skip it
+    //
+    return EFI_DEVICE_ERROR;
+  }
+}
+
+/**
+  Check if HOST is attached based on Mailbox.
+
+  @retval TRUE        HOST is attached.
+  @retval FALSE       HOST is not attached.
+
+**/
+BOOLEAN
+IsHostAttached (
+  VOID
+  )
+{
+  return (BOOLEAN) (GetDebugFlag (DEBUG_AGENT_FLAG_HOST_ATTACHED) == 1);
+}
+
+/**
+  Set HOST connect flag in Mailbox.
+
+  @param[in] Attached        Attach status.
+
+**/
+VOID
+SetHostAttached (
+  IN BOOLEAN                      Attached
+  )
+{
+  DebugAgentMsgPrint (DEBUG_AGENT_INFO, "Attach status is %d\n", Attached);
+  SetDebugFlag (DEBUG_AGENT_FLAG_HOST_ATTACHED, (UINT32)Attached);
+}
+
+/**
+  Set debug setting of Debug Agent in Mailbox.
+
+  @param DebugSetting         Pointer to Debug Setting defined by transfer protocol.
+
+  @retval RETURN_SUCCESS      The setting is set successfully.
+  @retval RETURN_UNSUPPORTED  The Key value is not supported.
+
+**/
+RETURN_STATUS
+SetDebugSetting (
+  IN DEBUG_DATA_SET_DEBUG_SETTING  *DebugSetting
+  )
+{
+  RETURN_STATUS                Status;
+
+  Status = RETURN_SUCCESS;
+  switch (DebugSetting->Key) {
+  case DEBUG_AGENT_SETTING_SMM_ENTRY_BREAK:
+    SetDebugFlag (DEBUG_AGENT_FLAG_BREAK_ON_NEXT_SMI, DebugSetting->Value);
+    break;
+  case DEBUG_AGENT_SETTING_PRINT_ERROR_LEVEL:
+    SetDebugFlag (DEBUG_AGENT_FLAG_PRINT_ERROR_LEVEL, DebugSetting->Value);
+    break;
+  case DEBUG_AGENT_SETTING_BOOT_SCRIPT_ENTRY_BREAK:
+    SetDebugFlag (DEBUG_AGENT_FLAG_BREAK_BOOT_SCRIPT, DebugSetting->Value);
+    break;
+  default:
+    Status = RETURN_UNSUPPORTED;
+  }
+  return Status;
 }
 
@@ -144 +748 @@
   // Enable Gx, Lx
   //
-  Dr7Value |= 0x3 << (RegisterIndex * 2);
+  Dr7Value |= (UINTN) (0x3 << (RegisterIndex * 2));
   //
   // Set RWx and Lenx
   //
-  Dr7Value &= ~(0xf0000 << (RegisterIndex * 4));
-  Dr7Value |= (SetHwBreakpoint->Type.Length | SetHwBreakpoint->Type.Access) << (RegisterIndex * 4);
+  Dr7Value &= (UINTN) (~(0xf << (16 + RegisterIndex * 4)));
+  Dr7Value |= (UINTN) ((SetHwBreakpoint->Type.Length << 2) | SetHwBreakpoint->Type.Access) << (16 + RegisterIndex * 4);
   //
   // Enable GE, LE
@@ -173 +777 @@
   if ((ClearHwBreakpoint->IndexMask & BIT0) != 0) {
     CpuContext->Dr0 = 0;
-    CpuContext->Dr7 &= ~(0x3 << 0);
+    CpuContext->Dr7 &= (UINTN)(~(0x3 << 0));
   }
   if ((ClearHwBreakpoint->IndexMask & BIT1) != 0) {
     CpuContext->Dr1 = 0;
-    CpuContext->Dr7 &= ~(0x3 << 2);
+    CpuContext->Dr7 &= (UINTN)(~(0x3 << 2));
   }
   if ((ClearHwBreakpoint->IndexMask & BIT2) != 0) {
     CpuContext->Dr2 = 0;
-    CpuContext->Dr7 &= ~(0x3 << 4);
+    CpuContext->Dr7 &= (UINTN)(~(0x3 << 4));
   }
   if ((ClearHwBreakpoint->IndexMask & BIT3) != 0) {
     CpuContext->Dr3 = 0;
-    CpuContext->Dr7 &= ~(0x3 << 6);
-  }
+    CpuContext->Dr7 &= (UINTN)(~(0x3 << 6));
+  }
+}
+
+
+/**
+  Return the offset of FP / MMX / XMM registers in the FPU saved state by register index.
+
+  @param[in]  Index    Register index.
+  @param[out] Width    Register width returned.
+
+  @return Offset in the FPU Save State.
+
+**/
+UINT16
+ArchReadFxStatOffset (
+  IN  UINT8                     Index,
+  OUT UINT8                     *Width
+  )
+{
+  if (Index < SOFT_DEBUGGER_REGISTER_ST0) {
+    switch (Index) {
+    case SOFT_DEBUGGER_REGISTER_FP_FCW:
+      *Width = (UINT8) sizeof (UINT16);
+      return OFFSET_OF(DEBUG_DATA_FX_SAVE_STATE, Fcw);
+
+    case SOFT_DEBUGGER_REGISTER_FP_FSW:
+      *Width = (UINT8) sizeof (UINT16);
+      return OFFSET_OF(DEBUG_DATA_FX_SAVE_STATE, Fsw);
+
+    case SOFT_DEBUGGER_REGISTER_FP_FTW:
+      *Width = (UINT8) sizeof (UINT16);
+      return OFFSET_OF(DEBUG_DATA_FX_SAVE_STATE, Ftw);
+
+    case SOFT_DEBUGGER_REGISTER_FP_OPCODE:
+      *Width = (UINT8) sizeof (UINT16);
+      return OFFSET_OF(DEBUG_DATA_FX_SAVE_STATE, Opcode);
+
+    case SOFT_DEBUGGER_REGISTER_FP_EIP:
+      *Width = (UINT8) sizeof (UINT32);
+      return OFFSET_OF(DEBUG_DATA_FX_SAVE_STATE, Eip);
+
+    case SOFT_DEBUGGER_REGISTER_FP_CS:
+      *Width = (UINT8) sizeof (UINT16);
+      return OFFSET_OF(DEBUG_DATA_FX_SAVE_STATE, Cs);
+
+    case SOFT_DEBUGGER_REGISTER_FP_DATAOFFSET:
+      *Width = (UINT8) sizeof (UINT32);
+      return OFFSET_OF(DEBUG_DATA_FX_SAVE_STATE, DataOffset);
+
+    case SOFT_DEBUGGER_REGISTER_FP_DS:
+      *Width = (UINT8) sizeof (UINT16);
+      return OFFSET_OF(DEBUG_DATA_FX_SAVE_STATE, Ds);
+
+    case SOFT_DEBUGGER_REGISTER_FP_MXCSR:
+      *Width = (UINT8) sizeof (UINT32);
+      return OFFSET_OF(DEBUG_DATA_FX_SAVE_STATE, Mxcsr);
+
+    case SOFT_DEBUGGER_REGISTER_FP_MXCSR_MASK:
+      *Width = (UINT8) sizeof (UINT32);
+      return OFFSET_OF(DEBUG_DATA_FX_SAVE_STATE, Mxcsr_Mask);
+    }
+  }
+
+  if (Index <= SOFT_DEBUGGER_REGISTER_ST7) {
+    *Width = 10;
+  } else if (Index <= SOFT_DEBUGGER_REGISTER_XMM15) {
+    *Width = 16;
+  } else {
+    //
+    // MMX register
+    //
+    *Width = 8;
+    Index -= SOFT_DEBUGGER_REGISTER_MM0 - SOFT_DEBUGGER_REGISTER_ST0;
+  }
+
+  return OFFSET_OF (DEBUG_DATA_FX_SAVE_STATE, St0Mm0) + (Index - SOFT_DEBUGGER_REGISTER_ST0) * 16;
+}
+
+/**
+  Return the pointer of the register value in the CPU saved context.
+
+  @param[in]  CpuContext         Pointer to saved CPU context.
+  @param[in]  Index              Register index value.
+  @param[out] Width              Data width to read.
+
+  @return The pointer in the CPU saved context.
+
+**/
+UINT8 *
+ArchReadRegisterBuffer (
+  IN DEBUG_CPU_CONTEXT               *CpuContext,
+  IN UINT8                           Index,
+  OUT UINT8                          *Width
+  )
+{
+  UINT8           *Buffer;
+
+  if (Index < SOFT_DEBUGGER_REGISTER_FP_BASE) {
+    Buffer = (UINT8 *) CpuContext + OFFSET_OF (DEBUG_CPU_CONTEXT, Dr0) + Index * sizeof (UINTN);
+    *Width = (UINT8) sizeof (UINTN);
+  } else {
+    //
+    // FPU/MMX/XMM registers
+    //
+    Buffer = (UINT8 *) CpuContext + OFFSET_OF (DEBUG_CPU_CONTEXT, FxSaveState) + ArchReadFxStatOffset (Index, Width);
+  }
+
+  return Buffer;
+}
+
+/**
+  Send the packet without data to HOST.
+
+  @param[in] CommandType    Type of Command.
+  @param[in] SequenceNo     Sequence number.
+
+**/
+VOID
+SendPacketWithoutData (
+  IN UINT8                  CommandType,
+  IN UINT8                  SequenceNo
+  )
+{
+  DEBUG_PACKET_HEADER       DebugHeader;
+  DEBUG_PORT_HANDLE         Handle;
+
+  Handle = GetDebugPortHandle();
+
+  DebugHeader.StartSymbol = DEBUG_STARTING_SYMBOL_NORMAL;
+  DebugHeader.Command     = CommandType;
+  DebugHeader.Length      = sizeof (DEBUG_PACKET_HEADER);
+  DebugHeader.SequenceNo  = SequenceNo;
+  DebugHeader.Crc         = 0;
+  DebugHeader.Crc         = CalculateCrc16 ((UINT8 *)&DebugHeader, sizeof (DEBUG_PACKET_HEADER), 0);
+
+  DebugAgentDataMsgPrint (DEBUG_AGENT_VERBOSE, TRUE, (UINT8 *) &DebugHeader, DebugHeader.Length);
+  DebugPortWriteBuffer (Handle, (UINT8 *) &DebugHeader, DebugHeader.Length);
 }
 
@@ -200 +940 @@
   )
 {
-  DEBUG_COMMAND_HEADER      DebugCommonHeader;
-  DEBUG_PORT_HANDLE         Handle;
+  UINT8                   SequenceNo;
+  DEBUG_AGENT_MAILBOX     *Mailbox;
+
+  if (AckCommand != DEBUG_COMMAND_OK) {
+    //
+    // This is not ACK OK packet
+    //
+    DebugAgentMsgPrint (DEBUG_AGENT_ERROR, "Send ACK(%d)\n", AckCommand);
+  }
+  Mailbox = GetMailboxPointer();
+  SequenceNo = Mailbox->HostSequenceNo;
+  DebugAgentMsgPrint (DEBUG_AGENT_INFO, "SendAckPacket: SequenceNo = %x\n", SequenceNo);
+  SendPacketWithoutData (AckCommand, SequenceNo);
+  UpdateMailboxContent (Mailbox, DEBUG_MAILBOX_LAST_ACK, AckCommand);
+}
+
+/**
+  Receive valid packet from HOST.
+
+  @param[out] InputPacket         Buffer to receive packet.
+  @param[out] BreakReceived       TRUE means break-in symbol received.
+                                  FALSE means break-in symbol not received.
+  @param[out] IncompatibilityFlag If IncompatibilityFlag is not NULL, return
+                                  TRUE:  Compatible packet received.
+                                  FALSE: Incompatible packet received.
+  @param[in]  Timeout             Time out value to wait for acknowlege from HOST.
+                                  The unit is microsecond.
+  @param[in]  SkipStartSymbol     TRUE:  Skip time out when reading start symbol.
+                                  FALSE: Does not Skip time out when reading start symbol.
+
+  @retval RETURN_SUCCESS   A valid package was reveived in InputPacket.
+  @retval RETURN_TIMEOUT   Timeout occurs.
+
+**/
+RETURN_STATUS
+ReceivePacket (
+  OUT UINT8             *InputPacket,
+  OUT BOOLEAN           *BreakReceived,
+  OUT BOOLEAN           *IncompatibilityFlag, OPTIONAL
+  IN  UINTN             Timeout,
+  IN  BOOLEAN           SkipStartSymbol
+  )
+{
+  DEBUG_PACKET_HEADER   *DebugHeader;
+  UINTN                 Received;
+  DEBUG_PORT_HANDLE     Handle;
+  UINT16                Crc;
+  UINTN                 TimeoutForStartSymbol;
 
   Handle = GetDebugPortHandle();
-
-  DebugCommonHeader.StartSymbol = DEBUG_STARTING_SYMBOL_NORMAL;
-  DebugCommonHeader.Command     = AckCommand;
-  DebugCommonHeader.DataLength  = 0;
-
-  DebugPortWriteBuffer (Handle, (UINT8 *) &DebugCommonHeader, sizeof (DEBUG_COMMAND_HEADER));
-}
-
-/**
-  Receive acknowledge packet from HOST in specified time.
-
-  @param[out] Ack           Returned acknowlege type from HOST.
-  @param[in]  Timeout       Time out value to wait for acknowlege from HOST.
-                            The unit is microsecond.
-  @param[out] BreakReceived If BreakReceived is not NULL,
-                            TRUE is retured if break-in symbol received.
-                            FALSE is retured if break-in symbol not received.
+  if (SkipStartSymbol) {
+    TimeoutForStartSymbol = 0;
+  } else {
+    TimeoutForStartSymbol = Timeout;
+  }
+
+  DebugHeader = (DEBUG_PACKET_HEADER *) InputPacket;
+  while (TRUE) {
+    //
+    // Find the valid start symbol
+    //
+    Received = DebugPortReadBuffer (Handle, &DebugHeader->StartSymbol, sizeof (DebugHeader->StartSymbol), TimeoutForStartSymbol);
+    if (Received < sizeof (DebugHeader->StartSymbol)) {
+      DebugAgentMsgPrint (DEBUG_AGENT_WARNING, "DebugPortReadBuffer(StartSymbol) timeout\n");
+      return RETURN_TIMEOUT;
+    }
+
+    if (DebugHeader->StartSymbol != DEBUG_STARTING_SYMBOL_NORMAL) {
+      DebugAgentMsgPrint (DEBUG_AGENT_WARNING, "Invalid start symbol received [%02x]\n", DebugHeader->StartSymbol);
+      continue;
+    }
+
+    //
+    // Read Package header till field Length
+    //
+    Received = DebugPortReadBuffer (
+                 Handle,
+                 (UINT8 *)DebugHeader + OFFSET_OF (DEBUG_PACKET_HEADER, Command),
+                 OFFSET_OF (DEBUG_PACKET_HEADER, Length) + sizeof (DebugHeader->Length) - sizeof (DebugHeader->StartSymbol),
+                 Timeout
+                 );
+    if (Received == 0) {
+      DebugAgentMsgPrint (DEBUG_AGENT_ERROR, "DebugPortReadBuffer(Command) timeout\n");
+      return RETURN_TIMEOUT;
+    }
+    if (DebugHeader->Length < sizeof (DEBUG_PACKET_HEADER)) {
+      if (IncompatibilityFlag != NULL) {
+        //
+        // This is one old version debug packet format, set Incompatibility flag
+        //
+        *IncompatibilityFlag = TRUE;
+      } else {
+        //
+        // Skip the bad small packet
+        //
+        continue;
+      }
+    } else {
+      //
+      // Read the payload data include the CRC field
+      //
+      Received = DebugPortReadBuffer (Handle, &DebugHeader->SequenceNo, (UINT8) (DebugHeader->Length - OFFSET_OF (DEBUG_PACKET_HEADER, SequenceNo)), Timeout);
+      if (Received == 0) {
+        DebugAgentMsgPrint (DEBUG_AGENT_ERROR, "DebugPortReadBuffer(SequenceNo) timeout\n");
+        return RETURN_TIMEOUT;
+      }
+      //
+      // Calculate the CRC of Debug Packet
+      //
+      Crc = DebugHeader->Crc;
+      DebugHeader->Crc = 0;
+      if (Crc == CalculateCrc16 ((UINT8 *) DebugHeader, DebugHeader->Length, 0)) {
+        break;
+      }
+      DebugAgentMsgPrint (DEBUG_AGENT_WARNING, "CRC Error (received CRC is %x)\n", Crc);
+      DebugAgentDataMsgPrint (DEBUG_AGENT_VERBOSE, FALSE, (UINT8 *) DebugHeader, DebugHeader->Length);
+    }
+  }
+
+  DebugAgentDataMsgPrint (DEBUG_AGENT_VERBOSE, FALSE, (UINT8 *) DebugHeader, DebugHeader->Length);
+
+  return RETURN_SUCCESS;
+}
+
+/**
+  Receive acknowledge packet OK from HOST in specified time.
+
+  @param[in]  Command             The command type issued by TARGET.
+  @param[in]  Timeout             Time out value to wait for acknowlege from HOST.
+                                  The unit is microsecond.
+  @param[out] BreakReceived       If BreakReceived is not NULL,
+                                  TRUE is retured if break-in symbol received.
+                                  FALSE is retured if break-in symbol not received.
+  @param[out] IncompatibilityFlag If IncompatibilityFlag is not NULL, return
+                                  TRUE:  Compatible packet received.
+                                  FALSE: Incompatible packet received.
 
   @retval  RETRUEN_SUCCESS  Succeed to receive acknowlege packet from HOST,
@@ -228 +1083 @@
 **/
 RETURN_STATUS
-ReceiveAckPacket (
-  OUT UINT8                     *Ack,
-  IN  UINTN                     Timeout,
-  OUT BOOLEAN                   *BreakReceived OPTIONAL
-  )
-{
-  DEBUG_COMMAND_HEADER      DebugCommonHeader;
-  DEBUG_PORT_HANDLE         Handle;
-
-  Handle = GetDebugPortHandle();
-
-  while (TRUE) {
-    if (DebugPortReadBuffer (Handle, (UINT8 *) &DebugCommonHeader.StartSymbol, 1, Timeout) == 0) {
-      return RETURN_TIMEOUT;
-    }
-    if (DebugCommonHeader.StartSymbol == DEBUG_STARTING_SYMBOL_BREAK) {
-      if (BreakReceived != NULL) {
-        SendAckPacket (DEBUG_COMMAND_HALT_DEFERRED);
-        *BreakReceived = TRUE;
-      }
-    }
-    if (DebugCommonHeader.StartSymbol == DEBUG_STARTING_SYMBOL_NORMAL) {
-      break;
-    }
-  }
-  if (DebugPortReadBuffer (Handle, (UINT8 *)&DebugCommonHeader.Command, sizeof (DEBUG_COMMAND_HEADER) - 1, Timeout) == 0) {
-    return RETURN_TIMEOUT;
-  }
-
-  *Ack = DebugCommonHeader.Command;
-  return RETURN_SUCCESS;
-}
-
-/**
-  Receive acknowledge packet OK from HOST in specified time.
-
-  @param[in]  Timeout       Time out value to wait for acknowlege from HOST.
-                            The unit is microsecond.
-  @param[out] BreakReceived If BreakReceived is not NULL,
-                            TRUE is retured if break-in symbol received.
-                            FALSE is retured if break-in symbol not received.
-
-  @retval  RETRUEN_SUCCESS  Succeed to receive acknowlege packet from HOST,
-                            the type of acknowlege packet saved in Ack.
-  @retval  RETURN_TIMEOUT   Specified timeout value was up.
-
-**/
-RETURN_STATUS
-WaitForAckPacketOK (
-  IN  UINTN                     Timeout,
-  OUT BOOLEAN                   *BreakReceived OPTIONAL
-  )
-{
-  RETURN_STATUS             Status;
-  UINT8                     Ack;
-
-  while (TRUE) {
-    Status = ReceiveAckPacket (&Ack, Timeout, BreakReceived);
-    if ((Status == RETURN_SUCCESS && Ack == DEBUG_COMMAND_OK) ||
-         Status == RETURN_TIMEOUT) {
-      break;
-    }
-  }
-
+SendCommandAndWaitForAckOK (
+  IN  UINT8               Command,
+  IN  UINTN               Timeout,
+  OUT BOOLEAN             *BreakReceived, OPTIONAL
+  OUT BOOLEAN             *IncompatibilityFlag OPTIONAL
+  )
+{
+  RETURN_STATUS           Status;
+  UINT8                   InputPacketBuffer[DEBUG_DATA_UPPER_LIMIT];
+  DEBUG_PACKET_HEADER     *DebugHeader;
+  UINT8                   SequenceNo;
+  UINT8                   HostSequenceNo;
+  UINT8                   RetryCount;
+
+  RetryCount  = 3;
+  DebugHeader = (DEBUG_PACKET_HEADER *) InputPacketBuffer;
+  Status      = RETURN_TIMEOUT;
+  while (RetryCount > 0) {
+    SequenceNo = GetMailboxPointer()->SequenceNo;
+    HostSequenceNo = GetMailboxPointer()->HostSequenceNo;
+    SendPacketWithoutData (Command, SequenceNo);
+    Status = ReceivePacket ((UINT8 *) DebugHeader, BreakReceived, IncompatibilityFlag, Timeout, FALSE);
+    if (Status == RETURN_TIMEOUT) {
+      if (Command == DEBUG_COMMAND_INIT_BREAK) {
+        RetryCount--;
+      } else {
+        DebugAgentMsgPrint (DEBUG_AGENT_WARNING, "TARGET: Timeout when waiting for ACK packet.\n");
+      }
+      continue;
+    }
+    ASSERT_EFI_ERROR (Status);
+    //
+    // Status == RETURN_SUCCESS
+    //
+    if (DebugHeader->Command == DEBUG_COMMAND_OK && DebugHeader->SequenceNo == SequenceNo) {
+      //
+      // Received Ack OK
+      //
+      UpdateMailboxContent (GetMailboxPointer(), DEBUG_MAILBOX_SEQUENCE_NO_INDEX, ++SequenceNo);
+      return Status;
+    }
+    if (DebugHeader->Command == DEBUG_COMMAND_GO && (DebugHeader->SequenceNo == HostSequenceNo || Command == DEBUG_COMMAND_INIT_BREAK)) {
+      //
+      // Received Old GO
+      //
+      if (Command == DEBUG_COMMAND_INIT_BREAK) {
+        DebugAgentMsgPrint (DEBUG_AGENT_WARNING, "TARGET: Receive GO() in last boot\n");
+      }
+      SendPacketWithoutData (DEBUG_COMMAND_OK, DebugHeader->SequenceNo);
+    }
+  }
+
+  ASSERT (Command == DEBUG_COMMAND_INIT_BREAK);
   return Status;
-}
-
-/**
-  Receive valid packet from HOST.
-
-  @param[out] InputPacket    Buffer to receive packet.
-  @param[out] BreakReceived  TRUE means break-in symbol received.
-                             FALSE means break-in symbol not received.
-
-  @retval RETURN_SUCCESS   A valid package was reveived in InputPacket.
-  @retval RETURN_NOT_READY No valid start symbol received.
-  @retval RETURN_TIMEOUT   Timeout occurs.
-
-**/
-RETURN_STATUS
-ReceivePacket (
-  OUT UINT8             *InputPacket,
-  OUT BOOLEAN           *BreakReceived
-  )
-{
-  DEBUG_COMMAND_HEADER  *DebugHeader;
-  UINTN                 Received;
-  DEBUG_PORT_HANDLE     Handle;
-
-  Handle = GetDebugPortHandle();
-  //
-  // Find the valid start symbol
-  //
-  DebugPortReadBuffer (Handle, InputPacket, 1, 0);
-
-  if (*InputPacket == DEBUG_STARTING_SYMBOL_BREAK) {
-    *BreakReceived = TRUE;
-    SendAckPacket (DEBUG_COMMAND_HALT_DEFERRED);
-  }
-
-  if (*InputPacket != DEBUG_STARTING_SYMBOL_NORMAL) {
-    return RETURN_NOT_READY;
-  }
-
-  //
-  // Read Package header
-  //
-  Received = DebugPortReadBuffer (Handle, InputPacket + 1, sizeof(DEBUG_COMMAND_HEADER_NO_START_SYMBOL), 0);
-  if (Received == 0) {
-    return RETURN_TIMEOUT;
-  }
-
-  DebugHeader = (DEBUG_COMMAND_HEADER *) InputPacket;
-  //
-  // Read the payload if has
-  //
-  if (DebugHeader->DataLength > 0 && DebugHeader->DataLength < (DEBUG_DATA_MAXIMUM_REAL_DATA - sizeof(DEBUG_COMMAND_HEADER))) {
-    InputPacket = InputPacket + 1 + Received;
-    Received = DebugPortReadBuffer (Handle, InputPacket, DebugHeader->DataLength, 0);
-
-    if (Received == 0) {
-      return RETURN_TIMEOUT;
-    }
-  }
-
-  return RETURN_SUCCESS;
 }
 
@@ -407 +1190 @@
       if (CpuContext->Dr3 == IO_PORT_BREAKPOINT_ADDRESS) {
 
-        Cause = (UINT8) ((CpuContext->Dr0 == IMAGE_LOAD_SIGNATURE) ? 
+        Cause = (UINT8) ((CpuContext->Dr0 == IMAGE_LOAD_SIGNATURE) ?
           DEBUG_DATA_BREAK_CAUSE_IMAGE_LOAD : DEBUG_DATA_BREAK_CAUSE_IMAGE_UNLOAD);
       }
@@ -413 +1196 @@
 
     case SOFT_INTERRUPT_SIGNATURE:
-   
+
       if (CpuContext->Dr1 == MEMORY_READY_SIGNATURE) {
         Cause = DEBUG_DATA_BREAK_CAUSE_MEMORY_READY;
@@ -436 +1219 @@
   default:
     if (Vector < 20) {
-      Cause = DEBUG_DATA_BREAK_CAUSE_EXCEPTION;
+      if (GetDebugFlag (DEBUG_AGENT_FLAG_STEPPING) == 1) {
+        //
+        // If stepping command is executing
+        //
+        Cause = DEBUG_DATA_BREAK_CAUSE_STEPPING;
+      } else {
+        Cause = DEBUG_DATA_BREAK_CAUSE_EXCEPTION;
+      }
     }
     break;
@@ -445 +1235 @@
 
 /**
+  Copy memory from source to destination with specified width.
+
+  @param[out] Dest        A pointer to the destination buffer of the memory copy.
+  @param[in]  Src         A pointer to the source buffer of the memory copy.
+  @param[in]  Count       The number of data with specified width to copy from source to destination.
+  @param[in]  Width       Data width in byte.
+
+**/
+VOID
+CopyMemByWidth (
+  OUT UINT8               *Dest,
+  IN  UINT8               *Src,
+  IN  UINT16              Count,
+  IN  UINT8               Width
+  )
+{
+  UINT8                   *Destination;
+  UINT8                   *Source;
+  INT8                    Step;
+
+  if (Src > Dest) {
+    Destination = Dest;
+    Source      = Src;
+    Step        = Width;
+  } else {
+    //
+    // Copy memory from tail to avoid memory overlap
+    //
+    Destination = Dest + (Count - 1) * Width;
+    Source      = Src  + (Count - 1) * Width;
+    Step        = -Width;
+  }
+
+  while (Count-- != 0) {
+    switch (Width) {
+    case 1:
+      *(UINT8 *) Destination = MmioRead8 ((UINTN) Source);
+      break;
+    case 2:
+      *(UINT16 *) Destination = MmioRead16 ((UINTN) Source);
+      break;
+    case 4:
+      *(UINT32 *) Destination = MmioRead32 ((UINTN) Source);
+      break;
+    case 8:
+      *(UINT64 *) Destination = MmioRead64 ((UINTN) Source);
+      break;
+    default:
+      ASSERT (FALSE);
+    }
+    Source      += Step;
+    Destination += Step;
+  }
+}
+
+/**
+  Read memory with speicifed width and send packet with response data to HOST.
+
+  @param[in] Data        Pointer to response data buffer.
+  @param[in] Count       The number of data with specified Width.
+  @param[in] Width       Data width in byte.
+
+  @retval RETURN_SUCCESS      Response data was sent successfully.
+
+**/
+RETURN_STATUS
+ReadMemoryAndSendResponsePacket (
+  IN UINT8                *Data,
+  IN UINT16               Count,
+  IN UINT8                Width
+  )
+{
+  RETURN_STATUS        Status;
+  DEBUG_PACKET_HEADER  *DebugHeader;
+  BOOLEAN              LastPacket;
+  DEBUG_PACKET_HEADER  *AckDebugHeader;
+  UINT8                DebugPacket[DEBUG_DATA_UPPER_LIMIT + sizeof (UINT64) - 1];
+  UINT8                InputPacketBuffer[DEBUG_DATA_UPPER_LIMIT];
+  DEBUG_PORT_HANDLE    Handle;
+  UINT8                SequenceNo;
+  UINTN                RemainingDataSize;
+  UINTN                CurrentDataSize;
+
+  Handle = GetDebugPortHandle();
+
+  //
+  // Data is appended end of Debug Packet header,  make sure data address
+  // in Debug Packet 8-byte alignment always
+  //
+  DebugHeader = (DEBUG_PACKET_HEADER *) (ALIGN_VALUE ((UINTN)&DebugPacket + sizeof (DEBUG_PACKET_HEADER), sizeof (UINT64))
+                                         - sizeof (DEBUG_PACKET_HEADER));
+  DebugHeader->StartSymbol = DEBUG_STARTING_SYMBOL_NORMAL;
+
+  RemainingDataSize = Count * Width;
+  while (TRUE) {
+    SequenceNo = GetMailboxPointer()->HostSequenceNo;
+    if (RemainingDataSize <= DEBUG_DATA_MAXIMUM_REAL_DATA) {
+      //
+      // If the remaining data is less one real packet size, this is the last data packet
+      //
+      CurrentDataSize = RemainingDataSize;
+      LastPacket = TRUE;
+      DebugHeader->Command = DEBUG_COMMAND_OK;
+    } else {
+      //
+      // Data is too larger to be sent in one packet, calculate the actual data size could
+      // be sent in one Maximum data packet
+      //
+      CurrentDataSize = (DEBUG_DATA_MAXIMUM_REAL_DATA / Width) * Width;
+      LastPacket = FALSE;
+      DebugHeader->Command = DEBUG_COMMAND_IN_PROGRESS;
+    }
+    //
+    // Construct the rest Debug header
+    //
+    DebugHeader->Length     = (UINT8)(CurrentDataSize + sizeof (DEBUG_PACKET_HEADER));
+    DebugHeader->SequenceNo = SequenceNo;
+    DebugHeader->Crc        = 0;
+    CopyMemByWidth ((UINT8 *)(DebugHeader + 1), Data, (UINT16) CurrentDataSize / Width, Width);
+    //
+    // Calculate and fill the checksum, DebugHeader->Crc should be 0 before invoking CalculateCrc16 ()
+    //
+    DebugHeader->Crc = CalculateCrc16 ((UINT8 *) DebugHeader, DebugHeader->Length, 0);
+
+    DebugAgentDataMsgPrint (DEBUG_AGENT_VERBOSE, TRUE, (UINT8 *) DebugHeader, DebugHeader->Length);
+
+    DebugPortWriteBuffer (Handle, (UINT8 *) DebugHeader, DebugHeader->Length);
+
+    while (TRUE) {
+      Status = ReceivePacket (InputPacketBuffer, NULL, NULL, READ_PACKET_TIMEOUT, FALSE);
+      if (Status == RETURN_TIMEOUT) {
+        DebugAgentMsgPrint (DEBUG_AGENT_WARNING, "TARGET: Timeout in SendDataResponsePacket()\n");
+        break;
+      }
+      AckDebugHeader = (DEBUG_PACKET_HEADER *) InputPacketBuffer;
+      SequenceNo = AckDebugHeader->SequenceNo;
+      if (AckDebugHeader->Command == DEBUG_COMMAND_OK &&
+          SequenceNo == DebugHeader->SequenceNo &&
+          LastPacket) {
+        //
+        // If this is the last packet, return RETURN_SUCCESS.
+        //
+        return RETURN_SUCCESS;
+      }
+      if ((SequenceNo == (UINT8) (DebugHeader->SequenceNo + 1)) && (AckDebugHeader->Command == DEBUG_COMMAND_CONTINUE)) {
+        //
+        // Calculate the rest data size
+        //
+        Data              += CurrentDataSize;
+        RemainingDataSize -= CurrentDataSize;
+        UpdateMailboxContent (GetMailboxPointer(), DEBUG_MAILBOX_HOST_SEQUENCE_NO_INDEX, (UINT8) SequenceNo);
+        break;
+      }
+      if (SequenceNo >= DebugHeader->SequenceNo) {
+        DebugAgentMsgPrint (DEBUG_AGENT_WARNING, "TARGET: Received one old or new command(SequenceNo is %x, last SequenceNo is %x)\n", SequenceNo, DebugHeader->SequenceNo);
+        break;
+      }
+    }
+  }
+}
+
+/**
   Send packet with response data to HOST.
 
-  @param[in] CpuContext  Pointer to saved CPU context.
   @param[in] Data        Pointer to response data buffer.
   @param[in] DataSize    Size of response data in byte.
 
   @retval RETURN_SUCCESS      Response data was sent successfully.
-  @retval RETURN_DEVICE_ERROR Cannot receive DEBUG_COMMAND_OK from HOST.
 
 **/
 RETURN_STATUS
 SendDataResponsePacket (
-  IN DEBUG_CPU_CONTEXT    *CpuContext,
   IN UINT8                *Data,
   IN UINT16               DataSize
   )
 {
-  UINT8                PacketHeader[DEBUG_DATA_MAXIMUM_LENGTH_FOR_SMALL_COMMANDS];
-  BOOLEAN              LastPacket;
-  UINT8                Ack;
-  UINT8                PacketData[DEBUG_DATA_MAXIMUM_REAL_DATA];
-  DEBUG_PORT_HANDLE    Handle;
-
-  Handle = GetDebugPortHandle();
-
-  ((DEBUG_COMMAND_HEADER *)PacketHeader)->StartSymbol = DEBUG_STARTING_SYMBOL_NORMAL;
-
-  while (TRUE) {
-    if (DataSize <= DEBUG_DATA_MAXIMUM_REAL_DATA) {
-      LastPacket = TRUE;
-      ((DEBUG_COMMAND_HEADER *)PacketHeader)->Command     = DEBUG_COMMAND_OK;
-      ((DEBUG_COMMAND_HEADER *)PacketHeader)->DataLength  = (UINT8) DataSize;
-      CopyMem (PacketData, Data, DataSize);
-
-    } else {
-      LastPacket = FALSE;
-      ((DEBUG_COMMAND_HEADER *)PacketHeader)->Command     = DEBUG_COMMAND_IN_PROGRESS;
-      ((DEBUG_COMMAND_HEADER *)PacketHeader)->DataLength  = DEBUG_DATA_MAXIMUM_REAL_DATA;
-      CopyMem (PacketData, Data, DEBUG_DATA_MAXIMUM_REAL_DATA);
-    }
-
-    DebugPortWriteBuffer (Handle, PacketHeader, sizeof (DEBUG_COMMAND_HEADER));
-    DebugPortWriteBuffer (Handle, PacketData, ((DEBUG_COMMAND_HEADER *)PacketHeader)->DataLength);
-
-    ReceiveAckPacket(&Ack, 0, NULL);
-    switch (Ack) {
-    case DEBUG_COMMAND_RESEND:
-      //
-      // Send the packet again
-      //
-      break;
-
-    case DEBUG_COMMAND_CONTINUE:
-      //
-      // Send the rest packet
-      //
-      Data     += DEBUG_DATA_MAXIMUM_REAL_DATA;
-      DataSize -= DEBUG_DATA_MAXIMUM_REAL_DATA;
-      break;
-
-    case DEBUG_COMMAND_OK:
-      if (LastPacket) {
-        //
-        // If this is the last packet, return RETURN_SUCCESS.
-        //
-        return RETURN_SUCCESS;
-      } else {
-        return RETURN_DEVICE_ERROR;
-      }
-
-    default:
-      return RETURN_DEVICE_ERROR;
-
-    }
-  }
+  return ReadMemoryAndSendResponsePacket (Data, DataSize, 1);
 }
 
@@ -543 +1435 @@
   DebugDataBreakCause.Cause       = GetBreakCause (Vector, CpuContext);
 
-  return SendDataResponsePacket (CpuContext, (UINT8 *) &DebugDataBreakCause, (UINT16) sizeof (DEBUG_DATA_RESPONSE_BREAK_CAUSE));
-}
-
+  return SendDataResponsePacket ((UINT8 *) &DebugDataBreakCause, (UINT16) sizeof (DEBUG_DATA_RESPONSE_BREAK_CAUSE));
+}
+
+/**
+  Try to attach the HOST.
+
+  Send init break packet to HOST:
+  If no acknowlege received in specified Timeout, return RETURN_TIMEOUT.
+  If received acknowlege, check the revision of HOST.
+  Set Attach Flag if attach successfully.
+
+  @param[in]  BreakCause     Break cause of this break event.
+  @param[in]  Timeout        Time out value to wait for acknowlege from HOST.
+                             The unit is microsecond.
+  @param[out] BreakReceived  If BreakReceived is not NULL,
+                             TRUE is retured if break-in symbol received.
+                             FALSE is retured if break-in symbol not received.
+**/
+RETURN_STATUS
+AttachHost (
+  IN  UINT8                BreakCause,
+  IN  UINTN                Timeout,
+  OUT BOOLEAN              *BreakReceived
+  )
+{
+  RETURN_STATUS                    Status;
+  DEBUG_PORT_HANDLE                Handle;
+  BOOLEAN                          IncompatibilityFlag;
+
+  IncompatibilityFlag = FALSE;
+  Handle = GetDebugPortHandle();
+
+  //
+  // Send init break and wait ack in Timeout
+  //
+  DebugPortWriteBuffer (Handle, (UINT8 *) mErrorMsgSendInitPacket, AsciiStrLen (mErrorMsgSendInitPacket));
+  if (BreakCause == DEBUG_DATA_BREAK_CAUSE_SYSTEM_RESET) {
+    Status = SendCommandAndWaitForAckOK (DEBUG_COMMAND_INIT_BREAK, Timeout, BreakReceived, &IncompatibilityFlag);
+  } else {
+    Status = SendCommandAndWaitForAckOK (DEBUG_COMMAND_ATTACH_BREAK, Timeout, BreakReceived, &IncompatibilityFlag);
+  }
+  if (IncompatibilityFlag) {
+    //
+    // If the incompatible Debug Packet received, the HOST should be running transfer protocol before DEBUG_AGENT_REVISION.
+    // It could be UDK Debugger for Windows v1.1/v1.2 or for Linux v0.8/v1.2.
+    //
+    DebugPortWriteBuffer (Handle, (UINT8 *) mErrorMsgVersionAlert, AsciiStrLen (mErrorMsgVersionAlert));
+    CpuDeadLoop ();
+  }
+
+  if (RETURN_ERROR (Status)) {
+    DebugPortWriteBuffer (Handle, (UINT8 *) mErrorMsgConnectFail, AsciiStrLen (mErrorMsgConnectFail));
+  } else {
+    DebugPortWriteBuffer (Handle, (UINT8 *) mErrorMsgConnectOK, AsciiStrLen (mErrorMsgConnectOK));
+    //
+    // Set Attach flag
+    //
+    SetHostAttached (TRUE);
+  }
+  return Status;
+}
+
+/**
+  Send Break point packet to HOST.
+
+  Only the first breaking processor could sent BREAK_POINT packet.
+
+  @param[in]  BreakCause     Break cause of this break event.
+  @param[in]  ProcessorIndex Processor index value.
+  @param[out] BreakReceived  If BreakReceived is not NULL,
+                             TRUE is retured if break-in symbol received.
+                             FALSE is retured if break-in symbol not received.
+
+**/
+VOID
+SendBreakPacketToHost (
+  IN  UINT8                BreakCause,
+  IN  UINT32               ProcessorIndex,
+  OUT BOOLEAN              *BreakReceived
+  )
+{
+  UINT8                 InputCharacter;
+  DEBUG_PORT_HANDLE     Handle;
+
+  Handle = GetDebugPortHandle();
+
+  if (IsHostAttached ()) {
+    DebugAgentMsgPrint (DEBUG_AGENT_INFO, "processor[%x]:Send Break Packet to HOST.\n", ProcessorIndex);
+    SendCommandAndWaitForAckOK (DEBUG_COMMAND_BREAK_POINT, READ_PACKET_TIMEOUT, BreakReceived, NULL);
+  } else {
+    DebugAgentMsgPrint (DEBUG_AGENT_INFO, "processor[%x]:Try to attach HOST.\n", ProcessorIndex);
+    //
+    // If HOST is not attached, try to attach it firstly.
+    //
+    //
+    // Poll Attach symbols from HOST and ack OK
+    //
+    do {
+      DebugPortReadBuffer (Handle, &InputCharacter, 1, 0);
+    } while (InputCharacter != DEBUG_STARTING_SYMBOL_ATTACH);
+    SendAckPacket (DEBUG_COMMAND_OK);
+
+    //
+    // Try to attach HOST
+    //
+    while (AttachHost (BreakCause, 0, NULL) != RETURN_SUCCESS);
+
+  }
+}
 
 /**
@@ -565 +1563 @@
   )
 {
-  RETURN_STATUS                 Status;
-  UINT8                         InputPacketBuffer[DEBUG_DATA_MAXIMUM_LENGTH_FOR_SMALL_COMMANDS];
-  DEBUG_COMMAND_HEADER          *DebugHeader;
-  UINT8                         Data8;
-  UINT32                        Data32;
-  UINT64                        Data64;
-  UINTN                         DataN;
-  DEBUG_DATA_READ_MEMORY_8      *MemoryRead;
-  DEBUG_DATA_WRITE_MEMORY_8     *MemoryWrite;
-  DEBUG_DATA_READ_IO            *IoRead;
-  DEBUG_DATA_WRITE_IO           *IoWrite;
-  DEBUG_DATA_READ_REGISTER      *RegisterRead;
-  DEBUG_DATA_WRITE_REGISTER     *RegisterWrite;
-  UINT8                         *RegisterBuffer;
-  DEBUG_DATA_READ_MSR           *MsrRegisterRead;
-  DEBUG_DATA_WRITE_MSR          *MsrRegisterWrite;
-  DEBUG_DATA_REPONSE_READ_REGISTER_GROUP_SEGLIM   RegisterGroupSegLim;
-  DEBUG_DATA_REPONSE_READ_REGISTER_GROUP_SEGBASE  RegisterGroupSegBase;
-  DEBUG_DATA_RESPONSE_GET_REVISION DebugAgentRevision;
-  BOOLEAN                       HaltDeferred;
-  DEBUG_DATA_RESPONSE_GET_EXCEPTION  Exception;
-  UINT32                        ProcessorIndex;
-  DEBUG_PORT_HANDLE             Handle;
-
-  Handle = GetDebugPortHandle();
+  RETURN_STATUS                     Status;
+  UINT8                             InputPacketBuffer[DEBUG_DATA_UPPER_LIMIT + sizeof (UINT64) - 1];
+  DEBUG_PACKET_HEADER               *DebugHeader;
+  UINT8                             Width;
+  UINT8                             Data8;
+  UINT32                            Data32;
+  UINT64                            Data64;
+  DEBUG_DATA_READ_MEMORY            *MemoryRead;
+  DEBUG_DATA_WRITE_MEMORY           *MemoryWrite;
+  DEBUG_DATA_READ_IO                *IoRead;
+  DEBUG_DATA_WRITE_IO               *IoWrite;
+  DEBUG_DATA_READ_REGISTER          *RegisterRead;
+  DEBUG_DATA_WRITE_REGISTER         *RegisterWrite;
+  UINT8                             *RegisterBuffer;
+  DEBUG_DATA_READ_MSR               *MsrRegisterRead;
+  DEBUG_DATA_WRITE_MSR              *MsrRegisterWrite;
+  DEBUG_DATA_CPUID                  *Cpuid;
+  DEBUG_DATA_RESPONSE_CPUID         CpuidResponse;
+  DEBUG_DATA_SEARCH_SIGNATURE       *SearchSignature;
+  DEBUG_DATA_RESPONSE_GET_EXCEPTION Exception;
+  DEBUG_DATA_RESPONSE_GET_REVISION  DebugAgentRevision;
+  DEBUG_DATA_SET_VIEWPOINT          *SetViewPoint;
+  BOOLEAN                           HaltDeferred;
+  UINT32                            ProcessorIndex;
+  DEBUG_PORT_HANDLE                 Handle;
+  DEBUG_AGENT_EXCEPTION_BUFFER      AgentExceptionBuffer;
+  UINT32                            IssuedViewPoint;
+  DEBUG_AGENT_MAILBOX               *Mailbox;
+  UINT8                             *AlignedDataPtr;
 
   ProcessorIndex  = 0;
-  HaltDeferred = BreakReceived;
-
-  if (MultiProcessorDebugSupport) {
+  IssuedViewPoint = 0;
+  HaltDeferred    = BreakReceived;
+
+  if (MultiProcessorDebugSupport()) {
     ProcessorIndex = GetProcessorIndex ();
     SetCpuStopFlagByIndex (ProcessorIndex, TRUE);
-  }
+    if (mDebugMpContext.ViewPointIndex == ProcessorIndex) {
+      //
+      // Only the current view processor could set AgentInProgress Flag.
+      //
+      IssuedViewPoint = ProcessorIndex;
+    }
+  }
+
+  if (IssuedViewPoint == ProcessorIndex) {
+    //
+    // Set AgentInProgress Flag.
+    //
+    SetDebugFlag (DEBUG_AGENT_FLAG_AGENT_IN_PROGRESS, 1);
+  }
+
+  Handle = GetDebugPortHandle();
 
   while (TRUE) {
 
-    if (MultiProcessorDebugSupport) {
+    if (MultiProcessorDebugSupport()) {
+      //
+      // Check if the current processor is HOST view point
+      //
       if (mDebugMpContext.ViewPointIndex != ProcessorIndex) {
         if (mDebugMpContext.RunCommandSet) {
+          //
+          // If HOST view point sets RUN flag, run GO command to leave
+          //
           SetCpuStopFlagByIndex (ProcessorIndex, FALSE);
           CommandGo (CpuContext);
           break;
         } else {
+          //
+          // Run into loop again
+          //
+          CpuPause ();
           continue;
         }
@@ -613 +1641 @@
     }
 
-    AcquireDebugPortControl ();
-
-    Status = ReceivePacket (InputPacketBuffer, &BreakReceived);
-
-    if (BreakReceived) {
+    AcquireMpSpinLock (&mDebugMpContext.DebugPortSpinLock);
+
+    DebugHeader =(DEBUG_PACKET_HEADER *) InputPacketBuffer;
+
+    DebugAgentMsgPrint (DEBUG_AGENT_INFO, "TARGET: Try to get command from HOST...\n");
+    Status = ReceivePacket ((UINT8 *)DebugHeader, &BreakReceived, NULL, READ_PACKET_TIMEOUT, TRUE);
+    if (Status != RETURN_SUCCESS || (DebugHeader->Command & DEBUG_COMMAND_RESPONSE) != 0) {
+      DebugAgentMsgPrint (DEBUG_AGENT_WARNING, "TARGET: Get command[%x] sequenceno[%x] returned status is [%x] \n", DebugHeader->Command, DebugHeader->SequenceNo, Status);
+      DebugAgentMsgPrint (DEBUG_AGENT_WARNING, "TARGET: Get command failed or it's response packet not expected! \n");
+      ReleaseMpSpinLock (&mDebugMpContext.DebugPortSpinLock);
+      continue;
+    }
+
+    Mailbox = GetMailboxPointer ();
+    if (DebugHeader->SequenceNo == Mailbox->HostSequenceNo) {
+      DebugAgentMsgPrint (DEBUG_AGENT_WARNING, "TARGET: Receive one old command[%x] agaist command[%x]\n", DebugHeader->SequenceNo, Mailbox->HostSequenceNo);
+      SendAckPacket (Mailbox->LastAck);
+      ReleaseMpSpinLock (&mDebugMpContext.DebugPortSpinLock);
+      continue;
+    } else if (DebugHeader->SequenceNo == (UINT8) (Mailbox->HostSequenceNo + 1)) {
+      UpdateMailboxContent (Mailbox, DEBUG_MAILBOX_HOST_SEQUENCE_NO_INDEX, (UINT8) DebugHeader->SequenceNo);
+    } else {
+      DebugAgentMsgPrint (DEBUG_AGENT_WARNING, "Receive one invalid comamnd[%x] agaist command[%x]\n", DebugHeader->SequenceNo, Mailbox->HostSequenceNo);
+      ReleaseMpSpinLock (&mDebugMpContext.DebugPortSpinLock);
+      continue;
+    }
+
+    //
+    // Save CPU content before executing HOST commond
+    //
+    UpdateMailboxContent (Mailbox, DEBUG_MAILBOX_EXCEPTION_BUFFER_POINTER_INDEX, (UINT64)(UINTN) &AgentExceptionBuffer.JumpBuffer);
+    if (SetJump (&AgentExceptionBuffer.JumpBuffer) != 0) {
+      //
+      // If HOST command failed, continue to wait for HOST's next command
+      // If needed, agent could send exception info to HOST.
+      //
+      SendAckPacket (DEBUG_COMMAND_ABORT);
+      ReleaseMpSpinLock (&mDebugMpContext.DebugPortSpinLock);
+      continue;
+    }
+
+    DebugAgentMsgPrint (DEBUG_AGENT_INFO, "Processor[%x]:Received one command(%x)\n", mDebugMpContext.ViewPointIndex, DebugHeader->Command);
+
+    switch (DebugHeader->Command) {
+
+    case DEBUG_COMMAND_HALT:
+      SendAckPacket (DEBUG_COMMAND_HALT_DEFERRED);
       HaltDeferred = TRUE;
       BreakReceived = FALSE;
-    }
-
-    if (Status != RETURN_SUCCESS) {
-      ReleaseDebugPortControl ();
-      continue;
-    }
-
-    Data8 = 1;
-
-    DebugHeader =(DEBUG_COMMAND_HEADER *) InputPacketBuffer;
-    switch (DebugHeader->Command) {
+      Status = RETURN_SUCCESS;
+      break;
 
     case DEBUG_COMMAND_RESET:
       SendAckPacket (DEBUG_COMMAND_OK);
-      ReleaseDebugPortControl ();
+      SendAckPacket (DEBUG_COMMAND_OK);
+      SendAckPacket (DEBUG_COMMAND_OK);
+      ReleaseMpSpinLock (&mDebugMpContext.DebugPortSpinLock);
 
       ResetCold ();
       //
-      // Wait for reset
-      //
-      CpuDeadLoop ();
+      // Assume system resets in 2 seconds, otherwise send TIMEOUT packet.
+      // PCD can be used if 2 seconds isn't long enough for some platforms.
+      //
+      MicroSecondDelay (2000000);
+      UpdateMailboxContent (Mailbox, DEBUG_MAILBOX_HOST_SEQUENCE_NO_INDEX, Mailbox->HostSequenceNo + 1);
+      SendAckPacket (DEBUG_COMMAND_TIMEOUT);
+      SendAckPacket (DEBUG_COMMAND_TIMEOUT);
+      SendAckPacket (DEBUG_COMMAND_TIMEOUT);
       break;
 
     case DEBUG_COMMAND_GO:
       CommandGo (CpuContext);
+      //
+      // Clear Dr0 to avoid to be recognized as IMAGE_LOAD/_UNLOAD again when hitting a breakpoint after GO
+      // If HOST changed Dr0 before GO, we will not change Dr0 here
+      //
+      Data8 = GetBreakCause (Vector, CpuContext);
+      if (Data8 == DEBUG_DATA_BREAK_CAUSE_IMAGE_LOAD || Data8 == DEBUG_DATA_BREAK_CAUSE_IMAGE_UNLOAD) {
+        CpuContext->Dr0 = 0;
+      }
+      //
+      // Clear Stepping Flag
+      //
+      SetDebugFlag (DEBUG_AGENT_FLAG_STEPPING, 0);
+
       if (!HaltDeferred) {
         //
         // If no HALT command received when being in-active mode
         //
-        if (MultiProcessorDebugSupport) {
-          Data32 = FindCpuNotRunning ();
+        if (MultiProcessorDebugSupport()) {
+          Data32 = FindNextPendingBreakCpu ();
           if (Data32 != -1) {
             //
-            // If there are still others processors being in break state,          
+            // If there are still others processors being in break state,
             // send OK packet to HOST to finish this go command
             //
@@ -665 +1746 @@
             SetCpuBreakFlagByIndex (mDebugMpContext.ViewPointIndex, FALSE);
             //
-            // Send break packet to HOST and exit to wait for command packet from HOST.
+            // Send break packet to HOST to let HOST break again
             //
-            SendAckPacket (DEBUG_COMMAND_BREAK_POINT);
-            WaitForAckPacketOK (0, &BreakReceived);
-            ReleaseDebugPortControl (); 
+            SendBreakPacketToHost (DEBUG_DATA_BREAK_CAUSE_UNKNOWN, mDebugMpContext.BreakAtCpuIndex, &BreakReceived);
+            //
+            // Continue to run into loop to read command packet from HOST
+            //
+            ReleaseMpSpinLock (&mDebugMpContext.DebugPortSpinLock);
             break;
           }
@@ -681 +1764 @@
           CpuPause ();
           //
-          // Wait for all processors are in running state 
+          // Wait for all processors are in running state
           //
           while (TRUE) {
@@ -705 +1788 @@
         SendAckPacket (DEBUG_COMMAND_OK);
 
-        ReleaseDebugPortControl ();
-
+        ReleaseMpSpinLock (&mDebugMpContext.DebugPortSpinLock);
+
+        if (!IsHostAttached()) {
+          UpdateMailboxContent (Mailbox, DEBUG_MAILBOX_SEQUENCE_NO_INDEX, 0);
+          UpdateMailboxContent (Mailbox, DEBUG_MAILBOX_HOST_SEQUENCE_NO_INDEX, 0);
+        }
         return;
 
@@ -715 +1802 @@
         SendAckPacket (DEBUG_COMMAND_HALT_PROCESSED);
         HaltDeferred = FALSE;
-        Data8 = GetBreakCause (Vector, CpuContext);
-        if (Data8 == DEBUG_DATA_BREAK_CAUSE_IMAGE_LOAD || Data8 == DEBUG_DATA_BREAK_CAUSE_IMAGE_UNLOAD) {
-          CpuContext->Dr0 = 0;
-          CpuContext->Dr3 = 0;
-        }
 
         Vector = DEBUG_TIMER_VECTOR;
@@ -727 +1809 @@
     case DEBUG_COMMAND_BREAK_CAUSE:
 
-      if (MultiProcessorDebugSupport && ProcessorIndex != mDebugMpContext.BreakAtCpuIndex) {
+      if (MultiProcessorDebugSupport() && ProcessorIndex != mDebugMpContext.BreakAtCpuIndex) {
         Status = SendBreakCausePacket (DEBUG_TIMER_VECTOR, CpuContext);
 
@@ -748 +1830 @@
     case DEBUG_COMMAND_SINGLE_STEPPING:
       CommandStepping (CpuContext);
+      //
+      // Clear Dr0 to avoid to be recognized as IMAGE_LOAD/_UNLOAD again when hitting a breakpoint after GO
+      // If HOST changed Dr0 before GO, we will not change Dr0 here
+      //
+      Data8 = GetBreakCause (Vector, CpuContext);
+      if (Data8 == DEBUG_DATA_BREAK_CAUSE_IMAGE_LOAD || Data8 == DEBUG_DATA_BREAK_CAUSE_IMAGE_UNLOAD) {
+        CpuContext->Dr0 = 0;
+      }
 
       mDebugMpContext.BreakAtCpuIndex = (UINT32) (-1);
-
-      ReleaseDebugPortControl ();
-      //
-      // Executing stepping command directly without sending ACK packet.
+      //
+      // Set Stepping Flag
+      //
+      SetDebugFlag (DEBUG_AGENT_FLAG_STEPPING, 1);
+      ReleaseMpSpinLock (&mDebugMpContext.DebugPortSpinLock);
+      //
+      // Executing stepping command directly without sending ACK packet,
+      // ACK packet will be sent after stepping done.
       //
       return;
@@ -761 +1855 @@
       Data8 = *(UINT8 *) (UINTN) Data64;
       *(UINT8 *) (UINTN) Data64 = DEBUG_SW_BREAKPOINT_SYMBOL;
-      Status = SendDataResponsePacket (CpuContext, (UINT8 *) &Data8, (UINT16) sizeof (UINT8));
-      break;
-
-    case DEBUG_COMMAND_READ_MEMORY_64:
-      Data8 *= 2;
-    case DEBUG_COMMAND_READ_MEMORY_32:
-      Data8 *= 2;
-    case DEBUG_COMMAND_READ_MEMORY_16:
-      Data8 *= 2;
-          case DEBUG_COMMAND_READ_MEMORY_8:
-      MemoryRead = (DEBUG_DATA_READ_MEMORY_8 *) (DebugHeader + 1);
-      Status = SendDataResponsePacket (CpuContext, (UINT8 *) (UINTN) MemoryRead->Address, (UINT16) (MemoryRead->Count * Data8));
-      break;
-
-    case DEBUG_COMMAND_WRITE_MEMORY_64:
-      Data8 *= 2;
-    case DEBUG_COMMAND_WRITE_MEMORY_32:
-      Data8 *= 2;
-    case DEBUG_COMMAND_WRITE_MEMORY_16:
-      Data8 *= 2;
-    case DEBUG_COMMAND_WRITE_MEMORY_8:
-      MemoryWrite = (DEBUG_DATA_WRITE_MEMORY_8 *) (DebugHeader + 1);
-      CopyMem ((VOID *) (UINTN) MemoryWrite->Address, &MemoryWrite->Data, MemoryWrite->Count * Data8);
+      Status = SendDataResponsePacket ((UINT8 *) &Data8, (UINT16) sizeof (UINT8));
+      break;
+
+    case DEBUG_COMMAND_READ_MEMORY:
+      MemoryRead = (DEBUG_DATA_READ_MEMORY *) (DebugHeader + 1);
+      Status = ReadMemoryAndSendResponsePacket ((UINT8 *) (UINTN) MemoryRead->Address, MemoryRead->Count, MemoryRead->Width);
+      break;
+
+    case DEBUG_COMMAND_WRITE_MEMORY:
+      MemoryWrite = (DEBUG_DATA_WRITE_MEMORY *) (DebugHeader + 1);
+      //
+      // Copy data into one memory with 8-byte alignment address
+      //
+      AlignedDataPtr = ALIGN_POINTER ((UINT8 *) &MemoryWrite->Data, sizeof (UINT64));
+      if (AlignedDataPtr != (UINT8 *) &MemoryWrite->Data) {
+        CopyMem (AlignedDataPtr, (UINT8 *) &MemoryWrite->Data, MemoryWrite->Count * MemoryWrite->Width);
+      }
+      CopyMemByWidth ((UINT8 *) (UINTN) MemoryWrite->Address, AlignedDataPtr, MemoryWrite->Count, MemoryWrite->Width);
       SendAckPacket (DEBUG_COMMAND_OK);
       break;
@@ -791 +1880 @@
       switch (IoRead->Width) {
       case 1:
-        Data64  = IoRead8 (IoRead->Port);
+        Data64  = IoRead8 ((UINTN) IoRead->Port);
         break;
       case 2:
-        Data64  = IoRead16 (IoRead->Port);
+        Data64  = IoRead16 ((UINTN) IoRead->Port);
         break;
       case 4:
-        Data64  = IoRead32 (IoRead->Port);
+        Data64  = IoRead32 ((UINTN) IoRead->Port);
         break;
       case 8:
-        Data64  = IoRead64 (IoRead->Port);
+        Data64  = IoRead64 ((UINTN) IoRead->Port);
         break;
       default:
         Data64  = (UINT64) -1;
       }
-      Status = SendDataResponsePacket (CpuContext, (UINT8 *) &Data64, IoRead->Width);
+      Status = SendDataResponsePacket ((UINT8 *) &Data64, IoRead->Width);
       break;
 
@@ -812 +1901 @@
       switch (IoWrite->Width) {
       case 1:
-        Data64  = IoWrite8 (IoWrite->Port, *(UINT8 *) &IoWrite->Data);
+        Data64  = IoWrite8 ((UINTN) IoWrite->Port, *(UINT8 *) &IoWrite->Data);
         break;
       case 2:
-        Data64  = IoWrite16 (IoWrite->Port, *(UINT16 *) &IoWrite->Data);
+        Data64  = IoWrite16 ((UINTN) IoWrite->Port, *(UINT16 *) &IoWrite->Data);
         break;
       case 4:
-        Data64  = IoWrite32 (IoWrite->Port, *(UINT32 *) &IoWrite->Data);
+        Data64  = IoWrite32 ((UINTN) IoWrite->Port, *(UINT32 *) &IoWrite->Data);
         break;
       case 8:
-        Data64  = IoWrite64 (IoWrite->Port, *(UINT64 *) &IoWrite->Data);
+        Data64  = IoWrite64 ((UINTN) IoWrite->Port, *(UINT64 *) &IoWrite->Data);
         break;
       default:
@@ -829 +1918 @@
       break;
 
+    case DEBUG_COMMAND_READ_ALL_REGISTERS:
+      Status = SendDataResponsePacket ((UINT8 *) CpuContext, sizeof (*CpuContext));
+      break;
+
     case DEBUG_COMMAND_READ_REGISTER:
       RegisterRead = (DEBUG_DATA_READ_REGISTER *) (DebugHeader + 1);
 
-      if (RegisterRead->Index < SOFT_DEBUGGER_REGISTER_OTHERS_BASE) {
-        Data8 = RegisterRead->Length;
-        RegisterBuffer = ArchReadRegisterBuffer (CpuContext, RegisterRead->Index, RegisterRead->Offset, &Data8);
-        Status = SendDataResponsePacket (CpuContext, RegisterBuffer, Data8);
-        break;
-      }
-
-      if (RegisterRead->Index <= SOFT_DEBUGGER_REGISTER_TSS_LIM) {
-        ReadRegisterGroupSegLim (CpuContext, &RegisterGroupSegLim);
-        DataN = * ((UINTN *) &RegisterGroupSegLim + (RegisterRead->Index - SOFT_DEBUGGER_REGISTER_CS_LIM));
-        Status = SendDataResponsePacket (CpuContext, (UINT8 *) &DataN, (UINT16) sizeof (UINTN));
-      } else if (RegisterRead->Index <= SOFT_DEBUGGER_REGISTER_TSS_BAS) {
-        ReadRegisterGroupSegBase (CpuContext, &RegisterGroupSegBase);
-        DataN = * ((UINTN *) &RegisterGroupSegBase + (RegisterRead->Index - SOFT_DEBUGGER_REGISTER_CS_BAS));
-        Status = SendDataResponsePacket (CpuContext, (UINT8 *) &DataN, (UINT16) sizeof (UINTN));
-      } else if (RegisterRead->Index < SOFT_DEBUGGER_REGISTER_IDT_LIM) {
-        Data64 = ReadRegisterSelectorByIndex (CpuContext, RegisterRead->Index);
-        Status = SendDataResponsePacket (CpuContext, (UINT8 *) &Data64, (UINT16) sizeof (UINT64));
+      if (RegisterRead->Index <= SOFT_DEBUGGER_REGISTER_MAX) {
+        RegisterBuffer = ArchReadRegisterBuffer (CpuContext, RegisterRead->Index, &Width);
+        Status = SendDataResponsePacket (RegisterBuffer, Width);
       } else {
-        switch (RegisterRead->Index) {
-        case SOFT_DEBUGGER_REGISTER_IDT_LIM:
-          DataN = (UINTN) (CpuContext->Idtr[0] & 0xffff);
-          SendDataResponsePacket (CpuContext, (UINT8 *) &DataN, (UINT16) sizeof (UINTN));
-          break;
-        case SOFT_DEBUGGER_REGISTER_GDT_LIM:
-          DataN = (UINTN) (CpuContext->Gdtr[0] & 0xffff);
-          SendDataResponsePacket (CpuContext, (UINT8 *) &DataN, (UINT16) sizeof (UINTN));
-          break;
-        case SOFT_DEBUGGER_REGISTER_IDT_BAS:
-          DataN = (UINTN) RShiftU64 (CpuContext->Idtr[0], 16);
-          DataN |= (UINTN) LShiftU64 (CpuContext->Idtr[1], (UINT16) (sizeof (UINTN) * 8 - 16));
-          SendDataResponsePacket (CpuContext, (UINT8 *) &DataN, (UINT16) sizeof (UINTN));
-          break;
-        case SOFT_DEBUGGER_REGISTER_GDT_BAS:
-          DataN = (UINTN) RShiftU64 (CpuContext->Gdtr[0], 16);
-          DataN |= (UINTN) LShiftU64 (CpuContext->Gdtr[1], (UINT16) (sizeof (UINTN) * 8 - 16));
-          SendDataResponsePacket (CpuContext, (UINT8 *) &DataN, (UINT16) sizeof (UINTN));
-          break;
-        }
+        Status = RETURN_UNSUPPORTED;
       }
       break;
@@ -876 +1935 @@
     case DEBUG_COMMAND_WRITE_REGISTER:
       RegisterWrite = (DEBUG_DATA_WRITE_REGISTER *) (DebugHeader + 1);
-      ArchWriteRegisterBuffer (CpuContext, RegisterWrite->Index, RegisterWrite->Offset, RegisterWrite->Length, (UINT8 *)&RegisterWrite->Value);
-      SendAckPacket (DEBUG_COMMAND_OK);
+      if (RegisterWrite->Index <= SOFT_DEBUGGER_REGISTER_MAX) {
+        RegisterBuffer = ArchReadRegisterBuffer (CpuContext, RegisterWrite->Index, &Width);
+        ASSERT (Width == RegisterWrite->Length);
+        CopyMem (RegisterBuffer, RegisterWrite->Data, Width);
+        SendAckPacket (DEBUG_COMMAND_OK);
+      } else {
+        Status = RETURN_UNSUPPORTED;
+      }
       break;
 
     case DEBUG_COMMAND_ARCH_MODE:
       Data8 = DEBUG_ARCH_SYMBOL;
-      Status = SendDataResponsePacket (CpuContext, (UINT8 *) &Data8, (UINT16) sizeof (UINT8));
+      Status = SendDataResponsePacket ((UINT8 *) &Data8, (UINT16) sizeof (UINT8));
       break;
 
@@ -888 +1953 @@
       MsrRegisterRead = (DEBUG_DATA_READ_MSR *) (DebugHeader + 1);
       Data64 = AsmReadMsr64 (MsrRegisterRead->Index);
-      Status = SendDataResponsePacket (CpuContext, (UINT8 *) &Data64, (UINT16) sizeof (UINT64));
+      Status = SendDataResponsePacket ((UINT8 *) &Data64, (UINT16) sizeof (UINT64));
       break;
 
@@ -897 +1962 @@
       break;
 
-    case DEBUG_COMMAND_READ_REGISTER_GROUP:
-      Data8 = *(UINT8 *) (DebugHeader + 1);
-      Status = ArchReadRegisterGroup (CpuContext, Data8);
-      break;
-
-    case DEBUG_COMMAND_SET_DEBUG_FLAG:
-      Data32 = *(UINT32 *) (DebugHeader + 1);
-      SetDebugFlag (Data32);
-      SendAckPacket (DEBUG_COMMAND_OK);
+    case DEBUG_COMMAND_SET_DEBUG_SETTING:
+      Status = SetDebugSetting ((DEBUG_DATA_SET_DEBUG_SETTING *)(DebugHeader + 1));
+      if (Status == RETURN_SUCCESS) {
+        SendAckPacket (DEBUG_COMMAND_OK);
+      }
       break;
 
@@ -911 +1972 @@
       DebugAgentRevision.Revision = DEBUG_AGENT_REVISION;
       DebugAgentRevision.Capabilities = DEBUG_AGENT_CAPABILITIES;
-      Status = SendDataResponsePacket (CpuContext, (UINT8 *) &DebugAgentRevision, (UINT16) sizeof (DEBUG_DATA_RESPONSE_GET_REVISION));
+      Status = SendDataResponsePacket ((UINT8 *) &DebugAgentRevision, (UINT16) sizeof (DEBUG_DATA_RESPONSE_GET_REVISION));
       break;
 
     case DEBUG_COMMAND_GET_EXCEPTION:
       Exception.ExceptionNum  = (UINT8) Vector;
-      Exception.ExceptionData = 0;
-      Status = SendDataResponsePacket (CpuContext, (UINT8 *) &Exception, (UINT16) sizeof (DEBUG_DATA_RESPONSE_GET_EXCEPTION));
+      Exception.ExceptionData = (UINT32) CpuContext->ExceptionData;
+      Status = SendDataResponsePacket ((UINT8 *) &Exception, (UINT16) sizeof (DEBUG_DATA_RESPONSE_GET_EXCEPTION));
       break;
 
     case DEBUG_COMMAND_SET_VIEWPOINT:
-      Data32 = *(UINT32 *) (DebugHeader + 1);
-
-      if (MultiProcessorDebugSupport) {
-        if (IsCpuStopped (Data32)) {
-          SetDebugViewPoint (Data32);
+      SetViewPoint = (DEBUG_DATA_SET_VIEWPOINT *) (DebugHeader + 1);
+      if (MultiProcessorDebugSupport()) {
+        if (IsCpuStopped (SetViewPoint->ViewPoint)) {
+          SetDebugViewPoint (SetViewPoint->ViewPoint);
           SendAckPacket (DEBUG_COMMAND_OK);
         } else {
@@ -933 +1993 @@
           SendAckPacket (DEBUG_COMMAND_NOT_SUPPORTED);
         }
-      } else if (Data32 == 0) {
+      } else if (SetViewPoint->ViewPoint == 0) {
         SendAckPacket (DEBUG_COMMAND_OK);
 
@@ -944 +2004 @@
     case DEBUG_COMMAND_GET_VIEWPOINT:
       Data32 = mDebugMpContext.ViewPointIndex;
-      SendDataResponsePacket(CpuContext, (UINT8 *) &Data32, (UINT16) sizeof (UINT32));
+      SendDataResponsePacket((UINT8 *) &Data32, (UINT16) sizeof (UINT32));
+      break;
+
+    case DEBUG_COMMAND_MEMORY_READY:
+      Data8 = (UINT8) GetDebugFlag (DEBUG_AGENT_FLAG_MEMORY_READY);
+      SendDataResponsePacket (&Data8, (UINT16) sizeof (UINT8));
+      break;
+
+    case DEBUG_COMMAND_DETACH:
+      SetHostAttached (FALSE);
+      SendAckPacket (DEBUG_COMMAND_OK);
+      break;
+
+    case DEBUG_COMMAND_CPUID:
+      Cpuid = (DEBUG_DATA_CPUID *) (DebugHeader + 1);
+      AsmCpuidEx (
+        Cpuid->Eax, Cpuid->Ecx,
+        &CpuidResponse.Eax, &CpuidResponse.Ebx,
+        &CpuidResponse.Ecx, &CpuidResponse.Edx
+        );
+      SendDataResponsePacket ((UINT8 *) &CpuidResponse, (UINT16) sizeof (CpuidResponse));
+      break;
+
+   case DEBUG_COMMAND_SEARCH_SIGNATURE:
+      SearchSignature = (DEBUG_DATA_SEARCH_SIGNATURE *) (DebugHeader + 1);
+      if ((SearchSignature->Alignment != 0) &&
+          (SearchSignature->Alignment == GetPowerOfTwo32 (SearchSignature->Alignment))
+         ) {
+        if (SearchSignature->Positive) {
+          for (
+            Data64 = ALIGN_VALUE ((UINTN) SearchSignature->Start, SearchSignature->Alignment);
+            Data64 <= SearchSignature->Start + SearchSignature->Count - SearchSignature->DataLength;
+            Data64 += SearchSignature->Alignment
+              ) {
+            if (CompareMem ((VOID *) (UINTN) Data64, &SearchSignature->Data, SearchSignature->DataLength) == 0) {
+              break;
+            }
+          }
+          if (Data64 > SearchSignature->Start + SearchSignature->Count - SearchSignature->DataLength) {
+            Data64 = (UINT64) -1;
+          }
+        } else {
+          for (
+            Data64 = ALIGN_VALUE ((UINTN) SearchSignature->Start - SearchSignature->Alignment, SearchSignature->Alignment);
+            Data64 >= SearchSignature->Start - SearchSignature->Count;
+            Data64 -= SearchSignature->Alignment
+              ) {
+            if (CompareMem ((VOID *) (UINTN) Data64, &SearchSignature->Data, SearchSignature->DataLength) == 0) {
+              break;
+            }
+          }
+          if (Data64 < SearchSignature->Start - SearchSignature->Count) {
+            Data64 = (UINT64) -1;
+          }
+        }
+        SendDataResponsePacket ((UINT8 *) &Data64, (UINT16) sizeof (Data64));
+      } else {
+        Status = RETURN_UNSUPPORTED;
+      }
       break;
 
@@ -958 +2076 @@
     }
 
-    ReleaseDebugPortControl ();
+    ReleaseMpSpinLock (&mDebugMpContext.DebugPortSpinLock);
     CpuPause ();
   }
@@ -977 +2095 @@
   )
 {
-  UINT8                     InputCharacter;
-  UINT8                     BreakCause;
-  UINTN                     SavedEip;
-  BOOLEAN                   BreakReceived;
-  UINT32                    ProcessorIndex;
-  UINT32                    CurrentDebugTimerInitCount;
-  DEBUG_PORT_HANDLE         Handle;
-  UINT8                     Data8;
-
-  Handle = GetDebugPortHandle();
-
-  ProcessorIndex = 0;
-  BreakReceived  = FALSE;
-
-  if (MultiProcessorDebugSupport) {
+  UINT8                            InputCharacter;
+  UINT8                            BreakCause;
+  UINTN                            SavedEip;
+  BOOLEAN                          BreakReceived;
+  UINT32                           ProcessorIndex;
+  UINT32                           CurrentDebugTimerInitCount;
+  DEBUG_PORT_HANDLE                Handle;
+  UINT8                            Data8;
+  UINT8                            *Al;
+  UINT32                           IssuedViewPoint;
+  DEBUG_AGENT_EXCEPTION_BUFFER     *ExceptionBuffer;
+
+  InputCharacter  = 0;
+  ProcessorIndex  = 0;
+  IssuedViewPoint = 0;
+  BreakReceived   = FALSE;
+
+  if (mSkipBreakpoint) {
+    //
+    // If Skip Breakpoint flag is set, means communication is disturbed by hardware SMI, we need to ignore the break points in SMM
+    //
+    if ((Vector == DEBUG_INT1_VECTOR) || (Vector == DEBUG_INT3_VECTOR)) {
+      DebugPortWriteBuffer (GetDebugPortHandle(), (UINT8 *) mWarningMsgIngoreBreakpoint, AsciiStrLen (mWarningMsgIngoreBreakpoint));
+      return;
+    }
+  }
+
+  if (MultiProcessorDebugSupport()) {
     ProcessorIndex = GetProcessorIndex ();
-    while (mDebugMpContext.RunCommandSet);
-  }
-
+    //
+    // If this processor has alreay halted before, need to check it later
+    //
+    if (IsCpuStopped (ProcessorIndex)) {
+      IssuedViewPoint = ProcessorIndex;
+    }
+  }
+
+  if (IssuedViewPoint == ProcessorIndex && GetDebugFlag (DEBUG_AGENT_FLAG_STEPPING) != 1) {
+    //
+    // Check if this exception is issued by Debug Agent itself
+    // If yes, fill the debug agent exception buffer and LongJump() back to
+    // the saved CPU content in CommandCommunication()
+    //
+    if (GetDebugFlag (DEBUG_AGENT_FLAG_AGENT_IN_PROGRESS) == 1) {
+      DebugAgentMsgPrint (DEBUG_AGENT_ERROR, "Debug agent meet one Exception, ExceptionNum is %d, EIP = 0x%x.\n", Vector, (UINTN)CpuContext->Eip);
+      ExceptionBuffer = (DEBUG_AGENT_EXCEPTION_BUFFER *) (UINTN) GetMailboxPointer()->ExceptionBufferPointer;
+      ExceptionBuffer->ExceptionContent.ExceptionNum  = (UINT8) Vector;
+      ExceptionBuffer->ExceptionContent.ExceptionData = (UINT32) CpuContext->ExceptionData;
+      LongJump ((BASE_LIBRARY_JUMP_BUFFER *)(UINTN)(ExceptionBuffer), 1);
+    }
+  }
+
+  if (MultiProcessorDebugSupport()) {
+    //
+    // If RUN commmand is executing, wait for it done.
+    //
+    while (mDebugMpContext.RunCommandSet) {
+      CpuPause ();
+    }
+  }
+
+  Handle     = GetDebugPortHandle();
+  BreakCause = GetBreakCause (Vector, CpuContext);
   switch (Vector) {
   case DEBUG_INT1_VECTOR:
   case DEBUG_INT3_VECTOR:
-
-    BreakCause = GetBreakCause (Vector, CpuContext);
-
-    if (BreakCause == DEBUG_DATA_BREAK_CAUSE_SYSTEM_RESET) {
-
-      //
-      // Init break, if no ack received after 200ms, return
-      //
-      SendAckPacket (DEBUG_COMMAND_INIT_BREAK);
-      if (WaitForAckPacketOK (200 * 1000, &BreakReceived) != RETURN_SUCCESS) {
+    switch (BreakCause) {
+    case DEBUG_DATA_BREAK_CAUSE_SYSTEM_RESET:
+      if (AttachHost (BreakCause, READ_PACKET_TIMEOUT, &BreakReceived) != RETURN_SUCCESS) {
+        //
+        // Try to connect HOST, return if fails
+        //
         break;
       }
-
-      SetHostConnectedFlag ();
       CommandCommunication (Vector, CpuContext, BreakReceived);
-
-    } else if (BreakCause == DEBUG_DATA_BREAK_CAUSE_STEPPING) {
-
+      break;
+
+    case DEBUG_DATA_BREAK_CAUSE_STEPPING:
       //
       // Stepping is finished, send Ack package.
       //
-      if (MultiProcessorDebugSupport) {
+      if (MultiProcessorDebugSupport()) {
         mDebugMpContext.BreakAtCpuIndex = ProcessorIndex;
       }
       SendAckPacket (DEBUG_COMMAND_OK);
       CommandCommunication (Vector, CpuContext, BreakReceived);
-
-    } else if (BreakCause == DEBUG_DATA_BREAK_CAUSE_MEMORY_READY) {
-
+      break;
+
+    case DEBUG_DATA_BREAK_CAUSE_MEMORY_READY:
       //
       // Memory is ready
       //
-      SendAckPacket (DEBUG_COMMAND_MEMORY_READY);
-      WaitForAckPacketOK (0, &BreakReceived);
+      SendCommandAndWaitForAckOK (DEBUG_COMMAND_MEMORY_READY, READ_PACKET_TIMEOUT, &BreakReceived, NULL);
       CommandCommunication (Vector, CpuContext, BreakReceived);
-
-    } else {
-
-      if (BreakCause == DEBUG_DATA_BREAK_CAUSE_IMAGE_LOAD || BreakCause == DEBUG_DATA_BREAK_CAUSE_IMAGE_UNLOAD) {
-        
-        //
-        // Set AL to DEBUG_AGENT_IMAGE_CONTINUE
-        //
-        Data8 = DEBUG_AGENT_IMAGE_CONTINUE;
-        ArchWriteRegisterBuffer (CpuContext, SOFT_DEBUGGER_REGISTER_AX, 0, 1, &Data8);
-
-        if (!IsHostConnected ()) {
-          //
-          // If HOST is not connected, return
-          //
-          break;
-        }
-      }
-
-      AcquireDebugPortControl ();
-
-      if (MultiProcessorDebugSupport) {
-        if(!IsAllCpuRunning ()) {
-          //
-          // If other processors have been stopped
-          //
-          SetCpuBreakFlagByIndex (ProcessorIndex, TRUE);
-        } else {
-          //
-          // If no any processor was stopped, try to halt other processors
-          //
-          HaltOtherProcessors (ProcessorIndex);
-          SendAckPacket (DEBUG_COMMAND_BREAK_POINT);
-          WaitForAckPacketOK (0, &BreakReceived);
-        }
-      } else {
-        SendAckPacket (DEBUG_COMMAND_BREAK_POINT);
-        WaitForAckPacketOK (0, &BreakReceived);
-      }
-
-      ReleaseDebugPortControl ();
+      break;
+
+    case DEBUG_DATA_BREAK_CAUSE_IMAGE_LOAD:
+    case DEBUG_DATA_BREAK_CAUSE_IMAGE_UNLOAD:
+      //
+      // Set AL to DEBUG_AGENT_IMAGE_CONTINUE
+      //
+      Al = ArchReadRegisterBuffer (CpuContext, SOFT_DEBUGGER_REGISTER_AX, &Data8);
+      *Al = DEBUG_AGENT_IMAGE_CONTINUE;
+
+      if (!IsHostAttached ()) {
+        //
+        // If HOST is not connected for image load/unload, return
+        //
+        break;
+      }
+      //
+      // Continue to run the following common code
+      //
+
+    case DEBUG_DATA_BREAK_CAUSE_HW_BREAKPOINT:
+    case DEBUG_DATA_BREAK_CAUSE_SW_BREAKPOINT:
+    default:
+      //
+      // Send Break packet to HOST
+      //
+      AcquireMpSpinLock (&mDebugMpContext.DebugPortSpinLock);
+      //
+      // Only the first breaking processor could send BREAK_POINT to HOST
+      //
+      if (IsFirstBreakProcessor (ProcessorIndex)) {
+        SendBreakPacketToHost (BreakCause, ProcessorIndex, &BreakReceived);
+      }
+      ReleaseMpSpinLock (&mDebugMpContext.DebugPortSpinLock);
 
       if (Vector == DEBUG_INT3_VECTOR) {
@@ -1094 +2242 @@
         CommandCommunication (Vector, CpuContext, BreakReceived);
       }
+      break;
     }
 
@@ -1100 +2249 @@
   case DEBUG_TIMER_VECTOR:
 
-    if (MultiProcessorDebugSupport) {
+    AcquireMpSpinLock (&mDebugMpContext.DebugPortSpinLock);
+
+    if (MultiProcessorDebugSupport()) {
       if (IsBsp (ProcessorIndex)) {
         //
@@ -1114 +2265 @@
 
       if (!IsBsp (ProcessorIndex) || mDebugMpContext.IpiSentByAp) {
+        ReleaseMpSpinLock (&mDebugMpContext.DebugPortSpinLock);
         //
         // If current processor is not BSP or this is one IPI sent by AP
@@ -1132 +2284 @@
     // Only BSP could run here
     //
-
-    AcquireDebugPortControl ();
-    
-    while (DebugPortPollBuffer (Handle)) {
-      //
-      // If there is data in debug port, will check whether it is break-in symbol,
+    while (TRUE) {
+      //
+      // If there is data in debug port, will check whether it is break(attach/break-in) symbol,
       // If yes, go into communication mode with HOST.
       // If no, exit interrupt process.
       //
-      DebugPortReadBuffer (Handle, &InputCharacter, 1, 0);
-      if (InputCharacter == DEBUG_STARTING_SYMBOL_BREAK) {
+      if (DebugReadBreakSymbol (Handle, &InputCharacter) == EFI_NOT_FOUND) {
+        break;
+      }
+
+      if ((!IsHostAttached () && (InputCharacter == DEBUG_STARTING_SYMBOL_ATTACH)) ||
+          (IsHostAttached () && (InputCharacter == DEBUG_COMMAND_HALT)) ||
+          (IsHostAttached () && (InputCharacter == DEBUG_COMMAND_GO))
+         ) {
+        DebugAgentMsgPrint (DEBUG_AGENT_VERBOSE, "Received data [%02x]\n", InputCharacter);
+        //
+        // Ack OK for break-in symbol
+        //
         SendAckPacket (DEBUG_COMMAND_OK);
-        if (MultiProcessorDebugSupport) {
-          if(FindCpuNotRunning () != -1) {
+
+        //
+        // If receive GO command in Debug Timer, means HOST may lost ACK packet before.
+        //
+        if (InputCharacter == DEBUG_COMMAND_GO) {
+          break;
+        }
+
+        if (!IsHostAttached ()) {
+          //
+          // Try to attach HOST, if no ack received after 200ms, return
+          //
+          if (AttachHost (BreakCause, READ_PACKET_TIMEOUT, &BreakReceived) != RETURN_SUCCESS) {
+            break;
+          }
+        }
+
+        if (MultiProcessorDebugSupport()) {
+          if(FindNextPendingBreakCpu  () != -1) {
             SetCpuBreakFlagByIndex (ProcessorIndex, TRUE);
           } else {
@@ -1151 +2327 @@
           }
         }
-        ReleaseDebugPortControl ();
+        ReleaseMpSpinLock (&mDebugMpContext.DebugPortSpinLock);
         CommandCommunication (Vector, CpuContext, BreakReceived);
-        AcquireDebugPortControl ();
+        AcquireMpSpinLock (&mDebugMpContext.DebugPortSpinLock);
         break;
       }
@@ -1163 +2339 @@
     SendApicEoi ();
 
-    ReleaseDebugPortControl ();
+    ReleaseMpSpinLock (&mDebugMpContext.DebugPortSpinLock);
 
     break;
 
   default:
-
     if (Vector <= DEBUG_EXCEPT_SIMD) {
-
-      AcquireDebugPortControl ();
-
-      if (MultiProcessorDebugSupport) {
-        if(FindCpuNotRunning () != -1) {
-          SetCpuBreakFlagByIndex (ProcessorIndex, TRUE);
-        } else {
-          HaltOtherProcessors (ProcessorIndex);
+      if (BreakCause == DEBUG_DATA_BREAK_CAUSE_STEPPING) {
+        //
+        // Stepping is finished, send Ack package.
+        //
+        if (MultiProcessorDebugSupport()) {
+          mDebugMpContext.BreakAtCpuIndex = ProcessorIndex;
         }
-      }
-      SendAckPacket (DEBUG_COMMAND_BREAK_POINT);
-      WaitForAckPacketOK (0, &BreakReceived);
-      ReleaseDebugPortControl ();
+        SendAckPacket (DEBUG_COMMAND_OK);
+      } else {
+        //
+        // Exception occurs, send Break packet to HOST
+        //
+        AcquireMpSpinLock (&mDebugMpContext.DebugPortSpinLock);
+        //
+        // Only the first breaking processor could send BREAK_POINT to HOST
+        //
+        if (IsFirstBreakProcessor (ProcessorIndex)) {
+          SendBreakPacketToHost (BreakCause, ProcessorIndex, &BreakReceived);
+        }
+        ReleaseMpSpinLock (&mDebugMpContext.DebugPortSpinLock);
+      }
+
       CommandCommunication (Vector, CpuContext, BreakReceived);
     }
@@ -1188 +2372 @@
   }
 
-  if (MultiProcessorDebugSupport) {
+  if (MultiProcessorDebugSupport()) {
     //
     // Clear flag and wait for all processors run here
     //
     SetIpiSentByApFlag (FALSE);
-    while (mDebugMpContext.RunCommandSet);
+    while (mDebugMpContext.RunCommandSet) {
+      CpuPause ();
+    }
+
+    //
+    // Only current (view) processor could clean up AgentInProgress flag.
+    //
+    if (mDebugMpContext.ViewPointIndex == ProcessorIndex) {
+      IssuedViewPoint = mDebugMpContext.ViewPointIndex;
+    }
+  }
+
+  if (IssuedViewPoint == ProcessorIndex && GetDebugFlag (DEBUG_AGENT_FLAG_STEPPING) != 1) {
+    //
+    // If the command is not stepping, clean up AgentInProgress flag
+    //
+    SetDebugFlag (DEBUG_AGENT_FLAG_AGENT_IN_PROGRESS, 0);
   }
 

trunk/src/VBox/Devices/EFI/Firmware/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/DebugAgent.h

@@ -2 +2 @@
   Command header of for Debug Agent library instance.
 
-  Copyright (c) 2010 - 2011, Intel Corporation. All rights reserved.<BR>
+  Copyright (c) 2010 - 2013, Intel Corporation. All rights reserved.<BR>
   This program and the accompanying materials
   are licensed and made available under the terms and conditions of the BSD License
@@ -17 +17 @@
 
 #include <Register/LocalApic.h>
-
 #include <Guid/DebugAgentGuid.h>
-
+#include <Guid/VectorHandoffTable.h>
+#include <Ppi/VectorHandoffInfo.h>
 #include <Library/BaseLib.h>
 #include <Library/BaseMemoryLib.h>
@@ -31 +31 @@
 #include <Library/LocalApicLib.h>
 #include <Library/DebugLib.h>
+#include <Library/TimerLib.h>
+#include <Library/PrintLib.h>
+#include <Library/PeCoffGetEntryPointLib.h>
+#include <Library/PeCoffExtraActionLib.h>
 
 #include <TransferProtocol.h>
@@ -38 +42 @@
 #include "DebugTimer.h"
 #include "ArchDebugSupport.h"
-
-#define DEBUG_AGENT_REVISION            ((0 << 16) | 01)
-#define DEBUG_AGENT_CAPABILITIES        0
-
-#define DEBUG_INT1_VECTOR               1
-#define DEBUG_INT3_VECTOR               3
+#include "DebugException.h"
+
+//
+// These macros may be already defined in DebugAgentLib.h
+//
+#define DEBUG_AGENT_INIT_PEI                     9
+#define DEBUG_AGENT_INIT_DXE_LOAD               10
+#define DEBUG_AGENT_INIT_DXE_UNLOAD             11
+#define DEBUG_AGENT_INIT_THUNK_PEI_IA32TOX64    12
+
+#define DEBUG_INT1_VECTOR               DEBUG_EXCEPT_DEBUG
+#define DEBUG_INT3_VECTOR               DEBUG_EXCEPT_BREAKPOINT
 #define DEBUG_TIMER_VECTOR              32
 #define DEBUG_MAILBOX_VECTOR            33
+
+//
+//  Timeout value for reading packet (unit is microsecond)
+//
+#define READ_PACKET_TIMEOUT     (500 * 1000)
 
 #define SOFT_INTERRUPT_SIGNATURE    SIGNATURE_32('S','O','F','T')
@@ -53 +68 @@
 extern UINTN  Exception0Handle;
 extern UINTN  TimerInterruptHandle;
-extern UINT16 ExceptionStubHeaderSize;
-
+extern UINT32 ExceptionStubHeaderSize;
+extern BOOLEAN mSkipBreakpoint;
+extern EFI_VECTOR_HANDOFF_INFO mVectorHandoffInfoDebugAgent[];
+extern UINTN                   mVectorHandoffInfoCount;
+
+//
+// CPU exception information issued by debug agent
+//
+typedef struct {
+  //
+  // This field is used to save CPU content before executing HOST command
+  //
+  BASE_LIBRARY_JUMP_BUFFER            JumpBuffer;
+  //
+  // This field returns the exception information issued by the HOST command
+  //
+  DEBUG_DATA_RESPONSE_GET_EXCEPTION   ExceptionContent;
+} DEBUG_AGENT_EXCEPTION_BUFFER;
+
+#define DEBUG_AGENT_FLAG_HOST_ATTACHED         BIT0
+#define DEBUG_AGENT_FLAG_AGENT_IN_PROGRESS     BIT1
+#define DEBUG_AGENT_FLAG_MEMORY_READY          BIT2
+#define DEBUG_AGENT_FLAG_STEPPING              BIT3
+#define DEBUG_AGENT_FLAG_CHECK_MAILBOX_IN_HOB  BIT4
+#define DEBUG_AGENT_FLAG_INIT_ARCH             BIT5|BIT6
+#define DEBUG_AGENT_FLAG_BREAK_ON_NEXT_SMI     BIT32
+#define DEBUG_AGENT_FLAG_PRINT_ERROR_LEVEL     (BIT33|BIT34|BIT35|BIT36)
+#define DEBUG_AGENT_FLAG_BREAK_BOOT_SCRIPT     BIT37
+
+#define DEBUG_MAILBOX_DEBUG_FLAG_INDEX                1
+#define DEBUG_MAILBOX_DEBUG_PORT_HANDLE_INDEX         2
+#define DEBUG_MAILBOX_EXCEPTION_BUFFER_POINTER_INDEX  3
+#define DEBUG_MAILBOX_LAST_ACK                        4
+#define DEBUG_MAILBOX_SEQUENCE_NO_INDEX               5
+#define DEBUG_MAILBOX_HOST_SEQUENCE_NO_INDEX          6
+
+#pragma pack(1)
 typedef union {
   struct {
-    UINT32  HostPresent      : 1;
-    UINT32  BreakOnNextSmi   : 1;
-    UINT32  Reserved         : 30;
+    //
+    // Lower 32 bits to store the status of DebugAgent
+    //
+    UINT32  HostAttached      : 1;   // 1: HOST is attached
+    UINT32  AgentInProgress   : 1;   // 1: Debug Agent is communicating with HOST
+    UINT32  MemoryReady       : 1;   // 1: Memory is ready
+    UINT32  SteppingFlag      : 1;   // 1: Agent is running stepping command
+    UINT32  CheckMailboxInHob : 1;   // 1: Need to check mailbox saved in HOB
+    UINT32  InitArch          : 2;   // value of DEBUG_DATA_RESPONSE_ARCH_MODE
+    UINT32  Reserved1         : 25;
+    //
+    // Higher 32bits to control the behavior of DebugAgent
+    //
+    UINT32  BreakOnNextSmi    : 1;   // 1: Break on next SMI
+    UINT32  PrintErrorLevel   : 4;   // Bitmask of print error level for debug message
+    UINT32  BreakOnBootScript : 1;   // 1: Break before executing boot script 
+    UINT32  Reserved2         : 26;
   } Bits;
-  UINT32  Uint32;
+  UINT64  Uint64;
 } DEBUG_AGENT_FLAG;
 
-#pragma pack(1)
 typedef struct {
   DEBUG_AGENT_FLAG           DebugFlag;
   UINT64                     DebugPortHandle;
+  //
+  // Pointer to DEBUG_AGENT_EXCEPTION_BUFFER
+  //
+  UINT64                     ExceptionBufferPointer;
+  UINT8                      LastAck;      // The last ack packet type
+  UINT8                      SequenceNo;
+  UINT8                      HostSequenceNo;
+  UINT8                      CheckSum;     // Mailbox checksum
+  UINT8                      ToBeCheckSum; // To be Mailbox checksum at the next
 } DEBUG_AGENT_MAILBOX;
 #pragma pack()
+
+///
+/// Byte packed structure for an IA-32 Interrupt Gate Descriptor.
+///
+typedef union {
+  struct {
+    UINT32  OffsetLow:16;   ///< Offset bits 15..0.
+    UINT32  Selector:16;    ///< Selector.
+    UINT32  Reserved_0:8;   ///< Reserved.
+    UINT32  GateType:8;     ///< Gate Type.  See #defines above.
+    UINT32  OffsetHigh:16;  ///< Offset bits 31..16.
+  } Bits;
+  UINT64  Uint64;
+} IA32_IDT_ENTRY;
+
 
 typedef union {
@@ -91 +178 @@
 
 /**
-  Caller provided function to be invoked at the end of DebugPortInitialize().
-
-  Refer to the descrption for DebugPortInitialize() for more details.
-
-  @param[in] Context           The first input argument of DebugPortInitialize().
-  @param[in] DebugPortHandle   Debug port handle created by Debug Communication Libary.
-
-**/
-VOID
-EFIAPI
-InitializeDebugAgentPhase2 (
-  IN VOID                  *Context,
-  IN DEBUG_PORT_HANDLE     DebugPortHandle
-  );
-
-/**
   Initialize IDT entries to support source level debug.
 
@@ -116 +187 @@
 
 /**
-  Write specified register into save CPU context.
+  Read register value from saved CPU context.
 
   @param[in] CpuContext         Pointer to saved CPU context.
   @param[in] Index              Register index value.
-  @param[in] Offset             Offset in register address range
-  @param[in] Width              Data width to read.
-  @param[in] RegisterBuffer     Pointer to input buffer with data.
-
-**/
-VOID
-ArchWriteRegisterBuffer (
-  IN DEBUG_CPU_CONTEXT               *CpuContext,
-  IN UINT8                           Index,
-  IN UINT8                           Offset,
-  IN UINT8                           Width,
-  IN UINT8                           *RegisterBuffer
-  );
-
-/**
-  Read register value from saved CPU context.
-
-  @param[in] CpuContext         Pointer to saved CPU context.
-  @param[in] Index              Register index value.
-  @param[in] Offset             Offset in register address range
   @param[in] Width              Data width to read.
 
@@ -149 +200 @@
   IN DEBUG_CPU_CONTEXT               *CpuContext,
   IN UINT8                           Index,
-  IN UINT8                           Offset,
   IN UINT8                           *Width
   );
@@ -156 +206 @@
   Send packet with response data to HOST.
 
-  @param[in] CpuContext  Pointer to saved CPU context.
   @param[in] Data        Pointer to response data buffer.
   @param[in] DataSize    Size of response data in byte.
@@ -166 +215 @@
 RETURN_STATUS
 SendDataResponsePacket (
-  IN DEBUG_CPU_CONTEXT    *CpuContext,
   IN UINT8                *Data,
   IN UINT16               DataSize
@@ -172 +220 @@
 
 /**
-  Read segment selector by register index.
-
-  @param[in] CpuContext           Pointer to saved CPU context.
-  @param[in] RegisterIndex        Register Index.
-
-  @return Value of segment selector.
-
-**/
-UINT64
-ReadRegisterSelectorByIndex (
-  IN DEBUG_CPU_CONTEXT                       *CpuContext,
-  IN UINT8                                   RegisterIndex
-  );
-
-/**
-  Read group register of common registers.
-
-  @param[in] CpuContext           Pointer to saved CPU context.
-  @param[in] RegisterGroup        Pointer to Group registers.
-
-**/
-VOID
-ReadRegisterGroup (
-  IN DEBUG_CPU_CONTEXT                       *CpuContext,
-  IN DEBUG_DATA_REPONSE_READ_REGISTER_GROUP  *RegisterGroup
-  );
-
-/**
-  Read group register of Segment Base.
-
-  @param[in] CpuContext           Pointer to saved CPU context.
-  @param[in] RegisterGroupSegBase Pointer to Group registers.
-
-**/
-VOID
-ReadRegisterGroupSegBase (
-  IN DEBUG_CPU_CONTEXT                              *CpuContext,
-  IN DEBUG_DATA_REPONSE_READ_REGISTER_GROUP_SEGBASE *RegisterGroupSegBase
-  );
-
-/**
-  Read gourp register of Segment Limit.
-
-  @param[in] CpuContext           Pointer to saved CPU context.
-  @param[in] RegisterGroupSegLim  Pointer to Group registers.
-
-**/
-VOID
-ReadRegisterGroupSegLim (
-  IN DEBUG_CPU_CONTEXT                             *CpuContext,
-  IN DEBUG_DATA_REPONSE_READ_REGISTER_GROUP_SEGLIM *RegisterGroupSegLim
-  );
-
-/**
-  Read group register by group index.
-
-  @param[in] CpuContext           Pointer to saved CPU context.
-  @param[in] GroupIndex           Group Index.
-
-  @retval RETURN_SUCCESS         Read successfully.
-  @retval RETURN_NOT_SUPPORTED   Group index cannot be supported.
-
-**/
-RETURN_STATUS
-ArchReadRegisterGroup (
-  IN DEBUG_CPU_CONTEXT                             *CpuContext,
-  IN UINT8                                         GroupIndex
-  );
-
-/**
-  Send acknowledge packet to HOST.
-
-  @param AckCommand    Type of Acknowledge packet.
-
-**/
-VOID
-SendAckPacket (
-  IN UINT8                AckCommand
-  );
-
-/**
-  Receive acknowledge packet OK from HOST in specified time.
-
-  @param[in]  Timeout       Time out value to wait for acknowlege from HOST.
-                            The unit is microsecond.
-  @param[out] BreakReceived If BreakReceived is not NULL,
-                            TRUE is retured if break-in symbol received.
-                            FALSE is retured if break-in symbol not received.
-
-  @retval  RETRUEN_SUCCESS  Succeed to receive acknowlege packet from HOST,
-                            the type of acknowlege packet saved in Ack.
-  @retval  RETURN_TIMEOUT   Specified timeout value was up.
-
-**/
-RETURN_STATUS
-WaitForAckPacketOK (
-  IN  UINTN                     Timeout,
-  OUT BOOLEAN                   *BreakReceived OPTIONAL
-  );
-
-/**
-  Check if HOST is connected based on Mailbox.
-
-  @retval TRUE        HOST is connected.
-  @retval FALSE       HOST is not connected.
+  Check if HOST is attached based on Mailbox.
+
+  @retval TRUE        HOST is attached.
+  @retval FALSE       HOST is not attached.
 
 **/
 BOOLEAN
-IsHostConnected (
+IsHostAttached (
   VOID
   );
@@ -306 +253 @@
   );
 
+/**
+  Read the Attach/Break-in symbols from the debug port.
+
+  @param[in]  Handle         Pointer to Debug Port handle.
+  @param[out] BreakSymbol    Returned break symbol.
+
+  @retval EFI_SUCCESS        Read the symbol in BreakSymbol.
+  @retval EFI_NOT_FOUND      No read the break symbol.
+
+**/
+EFI_STATUS
+DebugReadBreakSymbol (
+  IN  DEBUG_PORT_HANDLE      Handle,
+  OUT UINT8                  *BreakSymbol
+  );
+
+/**
+  Prints a debug message to the debug port if the specified error level is enabled.
+
+  If any bit in ErrorLevel is also set in Mainbox, then print the message specified
+  by Format and the associated variable argument list to the debug port.
+
+  @param[in] ErrorLevel  The error level of the debug message.
+  @param[in] Format      Format string for the debug message to print.
+  @param[in] ...         Variable argument list whose contents are accessed 
+                         based on the format string specified by Format.
+
+**/
+VOID
+EFIAPI
+DebugAgentMsgPrint (
+  IN UINT8         ErrorLevel,
+  IN CHAR8         *Format,
+  ...
+  );
+
+/**
+  Trigger one software interrupt to debug agent to handle it.
+
+  @param[in] Signature       Software interrupt signature.
+
+**/
+VOID
+TriggerSoftInterrupt (
+  IN UINT32                 Signature
+  );
+
+/**
+  Check if debug agent support multi-processor.
+
+  @retval TRUE    Multi-processor is supported.
+  @retval FALSE   Multi-processor is not supported.
+
+**/
+BOOLEAN
+MultiProcessorDebugSupport (
+  VOID
+  );
+
+/**
+  Find and report module image info to HOST.
+  
+  @param[in] AlignSize      Image aligned size.
+  
+**/
+VOID 
+FindAndReportModuleImageInfo (
+  IN UINTN          AlignSize                   
+  );
+
+/**
+  Read IDT entry to check if IDT entries are setup by Debug Agent.
+
+  @retval  TRUE     IDT entries were setup by Debug Agent.
+  @retval  FALSE    IDT entries were not setup by Debug Agent.
+
+**/
+BOOLEAN 
+IsDebugAgentInitialzed (
+  VOID
+  );
+
+/**
+  Caculate Mailbox checksum and update the checksum field.
+
+  @param[in]  Mailbox  Debug Agent Mailbox pointer.
+
+**/
+VOID
+UpdateMailboxChecksum (
+  IN DEBUG_AGENT_MAILBOX    *Mailbox
+  );
+
+/**
+  Verify Mailbox checksum.
+
+  If checksum error, print debug message and run init dead loop.
+
+  @param[in]  Mailbox  Debug Agent Mailbox pointer.
+
+**/
+VOID 
+VerifyMailboxChecksum (
+  IN DEBUG_AGENT_MAILBOX    *Mailbox
+  );
+
+/**
+  Set debug flag in mailbox.
+
+  @param[in]  FlagMask      Debug flag mask value.
+  @param[in]  FlagValue     Debug flag value.
+
+**/
+VOID 
+SetDebugFlag (
+  IN UINT64                 FlagMask,
+  IN UINT32                 FlagValue                          
+  );
+
+/**
+  Get debug flag in mailbox.
+
+  @param[in]  FlagMask      Debug flag mask value.
+  
+  @return Debug flag value.
+
+**/
+UINT32
+GetDebugFlag (
+  IN UINT64                 FlagMask
+  );
+
+/**
+  Update Mailbox content by index.
+
+  @param[in]  Mailbox  Debug Agent Mailbox pointer.
+  @param[in]  Index    Mailbox content index.
+  @param[in]  Value    Value to be set into mail box.
+  
+**/
+VOID
+UpdateMailboxContent ( 
+  IN DEBUG_AGENT_MAILBOX    *Mailbox,
+  IN UINTN                  Index,
+  IN UINT64                 Value
+  );
+
+/**
+  Retrieve exception handler from IDT table by ExceptionNum.
+
+  @param[in]  ExceptionNum    Exception number
+ 
+  @return Exception handler
+
+**/
+VOID *
+GetExceptionHandlerInIdtEntry (
+  IN UINTN             ExceptionNum
+  );
+
+/**
+  Set exception handler in IDT table by ExceptionNum.
+
+  @param[in]  ExceptionNum      Exception number
+  @param[in]  ExceptionHandler  Exception Handler to be set 
+
+**/
+VOID
+SetExceptionHandlerInIdtEntry (
+  IN UINTN             ExceptionNum,
+  IN VOID              *ExceptionHandler
+  );
+
+/**
+  Prints a debug message to the debug output device if the specified error level is enabled.
+
+  If any bit in ErrorLevel is also set in DebugPrintErrorLevelLib function 
+  GetDebugPrintErrorLevel (), then print the message specified by Format and the 
+  associated variable argument list to the debug output device.
+
+  If Format is NULL, then ASSERT().
+
+  @param[in] ErrorLevel  The error level of the debug message.
+  @param[in] IsSend      Flag of debug message to declare that the data is being sent or being received.
+  @param[in] Data        Variable argument list whose contents are accessed 
+  @param[in] Length      based on the format string specified by Format.
+
+**/
+VOID
+EFIAPI
+DebugAgentDataMsgPrint (
+  IN UINT8             ErrorLevel,
+  IN BOOLEAN           IsSend,
+  IN UINT8             *Data,
+  IN UINT8             Length  
+  );
+
+/**
+  Read remaing debug packet except for the start symbol
+
+  @param[in]      Handle        Pointer to Debug Port handle.
+  @param[in, out] DebugHeader   Debug header buffer including start symbol.
+
+  @retval EFI_SUCCESS        Read the symbol in BreakSymbol.
+  @retval EFI_CRC_ERROR      CRC check fail.
+  @retval EFI_TIMEOUT        Timeout occurs when reading debug packet.
+
+**/
+EFI_STATUS
+ReadRemainingBreakPacket (
+  IN     DEBUG_PORT_HANDLE      Handle,
+  IN OUT DEBUG_PACKET_HEADER    *DebugHeader
+  );
+
 #endif
 

trunk/src/VBox/Devices/EFI/Firmware/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/DebugMp.c

@@ -2 +2 @@
   Multi-Processor support functions implementation.
 
-  Copyright (c) 2010 - 2011, Intel Corporation. All rights reserved.<BR>
+  Copyright (c) 2010 - 2014, Intel Corporation. All rights reserved.<BR>
   This program and the accompanying materials
   are licensed and made available under the terms and conditions of the BSD License
@@ -15 +15 @@
 #include "DebugAgent.h"
 
-DEBUG_MP_CONTEXT volatile  mDebugMpContext = {0,0,0,0,0,0,0,0,FALSE,FALSE};
-
-DEBUG_CPU_DATA volatile  mDebugCpuData = {0};
-
-/**
-  Acquire access control on debug port.
+GLOBAL_REMOVE_IF_UNREFERENCED DEBUG_MP_CONTEXT volatile  mDebugMpContext = {0,0,0,{0},{0},0,0,0,0,FALSE,FALSE};
+
+GLOBAL_REMOVE_IF_UNREFERENCED DEBUG_CPU_DATA volatile  mDebugCpuData = {0};
+
+/**
+  Acquire a spin lock when Multi-processor supported.
 
   It will block in the function if cannot get the access control.
-
-**/
-VOID
-AcquireDebugPortControl (
-  VOID
-  )
-{
-  if (!MultiProcessorDebugSupport) {
+  If Multi-processor is not supported, return directly.
+
+  @param[in, out] MpSpinLock      A pointer to the spin lock.
+
+**/
+VOID
+AcquireMpSpinLock (
+  IN OUT SPIN_LOCK           *MpSpinLock
+  )
+{
+  if (!MultiProcessorDebugSupport()) {
     return;
   }
 
   while (TRUE) {
-    if (AcquireSpinLockOrFail (&mDebugMpContext.DebugPortSpinLock)) {
+    if (AcquireSpinLockOrFail (MpSpinLock)) {
       break;
     }
@@ -44 +47 @@
 
 /**
-  Release access control on debug port.
-
-**/
-VOID
-ReleaseDebugPortControl (
-  VOID
-  )
-{
-  if (!MultiProcessorDebugSupport) {
+  Release a spin lock when Multi-processor supported.
+
+  @param[in, out] MpSpinLock      A pointer to the spin lock.
+
+**/
+VOID
+ReleaseMpSpinLock (
+  IN OUT SPIN_LOCK           *MpSpinLock
+  )
+{
+  if (!MultiProcessorDebugSupport()) {
     return;
   }
 
-  ReleaseSpinLock (&mDebugMpContext.DebugPortSpinLock);
-}
-
-/**
-  Acquire access control on MP context.
-
-  It will block in the function if cannot get the access control.
-
-**/
-VOID
-AcquireMpContextControl (
-  VOID
-  )
-{
-  while (TRUE) {
-    if (AcquireSpinLockOrFail (&mDebugMpContext.MpContextSpinLock)) {
-      break;
-    }
-    CpuPause ();
-    continue;
-  }
-}
-
-/**
-  Release access control on MP context.
-
-**/
-VOID
-ReleaseMpContextControl (
-  VOID
-  )
-{
-  ReleaseSpinLock (&mDebugMpContext.MpContextSpinLock);
+  ReleaseSpinLock (MpSpinLock);
 }
 
@@ -102 +75 @@
   )
 {
-
+  DebugAgentMsgPrint (DEBUG_AGENT_INFO, "processor[%x]:Try to halt other processors.\n", CurrentProcessorIndex);
   if (!IsBsp (CurrentProcessorIndex)) {
     SetIpiSentByApFlag (TRUE);;
@@ -118 +91 @@
   //
   SendFixedIpiAllExcludingSelf (DEBUG_TIMER_VECTOR);
-  
+
 }
 
@@ -137 +110 @@
   LocalApicID = (UINT16) GetApicId ();
 
-  AcquireMpContextControl ();
+  AcquireMpSpinLock (&mDebugMpContext.MpContextSpinLock);
 
   for (Index = 0; Index < mDebugCpuData.CpuCount; Index ++) {
@@ -150 +123 @@
   }
 
-  ReleaseMpContextControl ();
+  ReleaseMpSpinLock (&mDebugMpContext.MpContextSpinLock);
 
   return Index;
@@ -171 +144 @@
   if (AsmMsrBitFieldRead64 (MSR_IA32_APIC_BASE_ADDRESS, 8, 8) == 1) {
     if (mDebugMpContext.BspIndex != ProcessorIndex) {
-      AcquireMpContextControl ();
+      AcquireMpSpinLock (&mDebugMpContext.MpContextSpinLock);
       mDebugMpContext.BspIndex = ProcessorIndex;
-      ReleaseMpContextControl ();
+      ReleaseMpSpinLock (&mDebugMpContext.MpContextSpinLock);
     }
     return TRUE;
@@ -198 +171 @@
   UINTN                 Index;
 
-  AcquireMpContextControl ();
+  AcquireMpSpinLock (&mDebugMpContext.MpContextSpinLock);
 
   Value = mDebugMpContext.CpuStopStatusMask[ProcessorIndex / 8];
@@ -209 +182 @@
   mDebugMpContext.CpuStopStatusMask[ProcessorIndex / 8] = Value;
 
-  ReleaseMpContextControl ();
+  ReleaseMpSpinLock (&mDebugMpContext.MpContextSpinLock);
 }
 
@@ -229 +202 @@
   UINTN                 Index;
 
-  AcquireMpContextControl ();
+  AcquireMpSpinLock (&mDebugMpContext.MpContextSpinLock);
 
   Value = mDebugMpContext.CpuBreakMask[ProcessorIndex / 8];
@@ -240 +213 @@
   mDebugMpContext.CpuBreakMask[ProcessorIndex / 8] = Value;
 
-  ReleaseMpContextControl ();
+  ReleaseMpSpinLock (&mDebugMpContext.MpContextSpinLock);
 }
 
@@ -280 +253 @@
   )
 {
-  AcquireMpContextControl ();
-
+  AcquireMpSpinLock (&mDebugMpContext.MpContextSpinLock);
   mDebugMpContext.RunCommandSet = RunningFlag;
-
-  ReleaseMpContextControl ();
+  ReleaseMpSpinLock (&mDebugMpContext.MpContextSpinLock);
 }
 
@@ -298 +269 @@
   )
 {
-  AcquireMpContextControl ();
-
+  AcquireMpSpinLock (&mDebugMpContext.MpContextSpinLock);
   mDebugMpContext.ViewPointIndex = ProcessorIndex;
-
-  ReleaseMpContextControl ();
-}
-
-/**
-  Initialize debug timer.
+  ReleaseMpSpinLock (&mDebugMpContext.MpContextSpinLock);
+}
+
+/**
+  Set the IPI send by BPS/AP flag.
 
   @param[in] IpiSentByApFlag   TRUE means this IPI is sent by AP.
@@ -317 +286 @@
   )
 {
-  AcquireMpContextControl ();
-
+  AcquireMpSpinLock (&mDebugMpContext.MpContextSpinLock);
   mDebugMpContext.IpiSentByAp = IpiSentByApFlag;
-
-  ReleaseMpContextControl ();
-}
-
-/**
-  Check if any processor breaks.
+  ReleaseMpSpinLock (&mDebugMpContext.MpContextSpinLock);
+}
+
+/**
+  Check the next pending breaking CPU.
 
   @retval others      There is at least one processor broken, the minimum
@@ -333 +300 @@
 **/
 UINT32
-FindCpuNotRunning (
+FindNextPendingBreakCpu (
   VOID
   )
 {
   UINT32               Index;
-  
+
   for (Index = 0; Index < DEBUG_CPU_MAX_COUNT / 8; Index ++) {
     if (mDebugMpContext.CpuBreakMask[Index] != 0) {
@@ -346 +313 @@
   return (UINT32)-1;
 }
-  
+
 /**
   Check if all processors are in running status.
@@ -360 +327 @@
 {
   UINTN              Index;
-  
+
   for (Index = 0; Index < DEBUG_CPU_MAX_COUNT / 8; Index ++) {
     if (mDebugMpContext.CpuStopStatusMask[Index] != 0) {
@@ -369 +336 @@
 }
 
+/**
+  Check if the current processor is the first breaking processor.
+
+  If yes, halt other processors.
+
+  @param[in] ProcessorIndex   Processor index value.
+
+  @return TRUE       This processor is the first breaking processor.
+  @return FALSE      This processor is not the first breaking processor.
+
+**/
+BOOLEAN
+IsFirstBreakProcessor (
+  IN UINT32              ProcessorIndex
+  )
+{
+  if (MultiProcessorDebugSupport()) {
+    if (mDebugMpContext.BreakAtCpuIndex != (UINT32) -1) {
+      //
+      // The current processor is not the first breaking one.
+      //
+      SetCpuBreakFlagByIndex (ProcessorIndex, TRUE);
+      return FALSE;
+    } else {
+      //
+      // If no any processor breaks, try to halt other processors
+      //
+      HaltOtherProcessors (ProcessorIndex);
+      return TRUE;
+    }
+  }
+  return TRUE;
+}
+

trunk/src/VBox/Devices/EFI/Firmware/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/DebugMp.h

@@ -2 +2 @@
   Header file for Multi-Processor support.
 
-  Copyright (c) 2010, Intel Corporation. All rights reserved.<BR>
+  Copyright (c) 2010 - 2013, Intel Corporation. All rights reserved.<BR>
   This program and the accompanying materials
   are licensed and made available under the terms and conditions of the BSD License
@@ -26 +26 @@
   SPIN_LOCK                 MpContextSpinLock;   ///< Lock for writting MP context
   SPIN_LOCK                 DebugPortSpinLock;   ///< Lock for access debug port
+  SPIN_LOCK                 MailboxSpinLock;     ///< Lock for accessing mail box
   UINT8                     CpuBreakMask[DEBUG_CPU_MAX_COUNT/8];        ///< Bitmask of all breaking CPUs
   UINT8                     CpuStopStatusMask[DEBUG_CPU_MAX_COUNT/8];   ///< Bitmask of CPU stop status
@@ -33 +34 @@
   UINT32                    DebugTimerInitCount; ///< Record BSP's init timer count
   BOOLEAN                   IpiSentByAp;         ///< TRUR: IPI is sent by AP. TALSE: IPI is sent by BSP
-  BOOLEAN                   RunCommandSet;       ///< TRUE: RUN commmand is not executed. FALSE : RUN command is executed.
+  BOOLEAN                   RunCommandSet;       ///< TRUE: RUN commmand is executing. FALSE : RUN command has been executed.
 } DEBUG_MP_CONTEXT;
 
-extern CONST BOOLEAN               MultiProcessorDebugSupport;
 extern DEBUG_MP_CONTEXT volatile   mDebugMpContext;
 extern DEBUG_CPU_DATA   volatile   mDebugCpuData;
@@ -63 +63 @@
 
 /**
-  Acquire access control on MP context.
+  Acquire a spin lock when Multi-processor supported.
 
   It will block in the function if cannot get the access control.
-
-**/
-VOID
-AcquireMpContextControl (
-  VOID
-  );
-
-/**
-  Release access control on MP context.
-
-**/
-VOID
-ReleaseMpContextControl (
-  VOID
-  );
-
-/**
-  Acquire access control on debug port.
-
-  It will block in the function if cannot get the access control.
-
-**/
-VOID
-AcquireDebugPortControl (
-  VOID
-  );
-
-/**
-  Release access control on debug port.
-
-**/
-VOID
-ReleaseDebugPortControl (
-  VOID
+  If Multi-processor is not supported, return directly.
+
+  @param[in, out] MpSpinLock      A pointer to the spin lock.
+
+**/
+VOID
+AcquireMpSpinLock (
+  IN OUT SPIN_LOCK           *MpSpinLock
+  );
+
+/**
+  Release a spin lock when Multi-processor supported.
+
+  @param[in, out] MpSpinLock      A pointer to the spin lock.
+
+**/
+VOID
+ReleaseMpSpinLock (
+  IN OUT SPIN_LOCK           *MpSpinLock
   );
 
@@ -182 +167 @@
 
 /**
-  Initialize debug timer.
+  Set the IPI send by BPS/AP flag.
 
   @param[in] IpiSentByApFlag   TRUE means this IPI is sent by AP.
@@ -194 +179 @@
 
 /**
-  Check if any processor breaks.
+  Check the next pending breaking CPU.
 
   @retval others      There is at least one processor broken, the minimum
@@ -202 +187 @@
 **/
 UINT32
-FindCpuNotRunning (
+FindNextPendingBreakCpu (
   VOID
   );
@@ -218 +203 @@
   );
 
+/**
+  Check if the current processor is the first breaking processor.
+
+  If yes, halt other processors.  
+  
+  @param[in] ProcessorIndex   Processor index value.
+  
+  @return TRUE       This processor is the first breaking processor.
+  @return FALSE      This processor is not the first breaking processor.
+                            
+**/
+BOOLEAN
+IsFirstBreakProcessor (
+  IN UINT32              ProcessorIndex
+  );
+  
 #endif
 

trunk/src/VBox/Devices/EFI/Firmware/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/DebugTimer.c

@@ -2 +2 @@
   Code for debug timer to support debug agent library implementation.
 
-  Copyright (c) 2010, Intel Corporation. All rights reserved.<BR>
+  Copyright (c) 2010 - 2014, Intel Corporation. All rights reserved.<BR>
   This program and the accompanying materials
   are licensed and made available under the terms and conditions of the BSD License
@@ -18 +18 @@
   Initialize CPU local APIC timer.
 
+  @return   32-bit Local APIC timer init count.
 **/
-VOID
+UINT32
 InitializeDebugTimer (
   VOID
@@ -42 +43 @@
   InitializeApicTimer (ApicTimerDivisor, InitialCount, TRUE, DEBUG_TIMER_VECTOR);
 
-  if (MultiProcessorDebugSupport) {
-    mDebugMpContext.DebugTimerInitCount = InitialCount;
-  }
+  return InitialCount;
 }
 
@@ -66 +65 @@
   )
 {
-  BOOLEAN     OldInterruptState;
   BOOLEAN     OldDebugTimerInterruptState;
 
-  OldInterruptState = SaveAndDisableInterrupts ();
   OldDebugTimerInterruptState = GetApicTimerInterruptState ();
-  
-  if (EnableStatus) {
-    EnableApicTimerInterrupt ();
-  } else {
-    DisableApicTimerInterrupt ();
+
+  if (OldDebugTimerInterruptState != EnableStatus) {
+    if (EnableStatus) {
+      EnableApicTimerInterrupt ();
+    } else {
+      DisableApicTimerInterrupt ();
+    }
+    //
+    // Validate the Debug Timer interrupt state
+    // This will make additional delay after Local Apic Timer interrupt state is changed.
+    // Thus, CPU could handle the potential pending interrupt of Local Apic timer.
+    //
+    while (GetApicTimerInterruptState () != EnableStatus) {
+      CpuPause ();
+    }
   }
 
-  SetInterruptState (OldInterruptState);
   return OldDebugTimerInterruptState;
 }

trunk/src/VBox/Devices/EFI/Firmware/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/DebugTimer.h

@@ -2 +2 @@
   Header file for debug timer to support debug agent library implementation.
 
-  Copyright (c) 2010, Intel Corporation. All rights reserved.<BR>
+  Copyright (c) 2010 - 2014, Intel Corporation. All rights reserved.<BR>
   This program and the accompanying materials
   are licensed and made available under the terms and conditions of the BSD License
@@ -17 +17 @@
 
 /**
-  Initialize debug timer.
+  Initialize CPU local APIC timer.
 
+  @return   32-bit Local APIC timer init count.
 **/
-VOID
+UINT32
 InitializeDebugTimer (
   VOID

trunk/src/VBox/Devices/EFI/Firmware/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/Ia32/ArchDebugSupport.c

@@ -1 +1 @@
 /** @file
-  Public include file for Debug Port Library.
+  Supporting functions for IA32 architecture.
 
-  Copyright (c) 2010, Intel Corporation. All rights reserved.<BR>
+  Copyright (c) 2010 - 2013, Intel Corporation. All rights reserved.<BR>
   This program and the accompanying materials
   are licensed and made available under the terms and conditions of the BSD License
@@ -14 +14 @@
 
 #include "DebugAgent.h"
-
-/**
-  Read the offset of FP / MMX / XMM registers by register index.
-
-  @param[in]  Index    Register index.
-  @param[out] Width    Register width returned.
-
-  @return Offset in register address range.
-
-**/
-UINT16
-ArchReadFxStatOffset (
-  IN  UINT8                     Index,
-  OUT UINT8                     *Width
-  )
-{
-  if (Index < SOFT_DEBUGGER_REGISTER_ST0) {
-    switch (Index) {
-    case SOFT_DEBUGGER_REGISTER_FP_FCW:
-      *Width = (UINT8) sizeof (UINT16);
-      return (UINT16)OFFSET_OF(DEBUG_DATA_IA32_FX_SAVE_STATE, Fcw);
-
-    case SOFT_DEBUGGER_REGISTER_FP_FSW:
-          *Width = (UINT8) sizeof (UINT16);
-      return (UINT16)OFFSET_OF(DEBUG_DATA_IA32_FX_SAVE_STATE, Fsw);
-
-    case SOFT_DEBUGGER_REGISTER_FP_FTW:
-      *Width = (UINT8) sizeof (UINT16);
-      return (UINT16)OFFSET_OF(DEBUG_DATA_IA32_FX_SAVE_STATE, Ftw);
-
-    case SOFT_DEBUGGER_REGISTER_FP_OPCODE:
-      *Width = (UINT8) sizeof (UINT16);
-      return (UINT16)OFFSET_OF(DEBUG_DATA_IA32_FX_SAVE_STATE, Opcode);
-
-    case SOFT_DEBUGGER_REGISTER_FP_EIP:
-      *Width = (UINT8) sizeof (UINTN);
-      return (UINT16)OFFSET_OF(DEBUG_DATA_IA32_FX_SAVE_STATE, Eip);
-
-    case SOFT_DEBUGGER_REGISTER_FP_CS:
-      *Width = (UINT8) sizeof (UINT16);
-      return (UINT16)OFFSET_OF(DEBUG_DATA_IA32_FX_SAVE_STATE, Cs);
-
-    case SOFT_DEBUGGER_REGISTER_FP_DATAOFFSET:
-      *Width = (UINT8) sizeof (UINTN);
-      return (UINT16)OFFSET_OF(DEBUG_DATA_IA32_FX_SAVE_STATE, DataOffset);
-
-    case SOFT_DEBUGGER_REGISTER_FP_DS:
-      *Width = (UINT8) sizeof (UINT16);
-      return (UINT16)OFFSET_OF(DEBUG_DATA_IA32_FX_SAVE_STATE, Ds);
-
-    case SOFT_DEBUGGER_REGISTER_FP_MXCSR:
-      *Width = (UINT8) sizeof (UINTN);
-      return (UINT16)OFFSET_OF(DEBUG_DATA_IA32_FX_SAVE_STATE, Mxcsr);
-
-    case SOFT_DEBUGGER_REGISTER_FP_MXCSR_MASK:
-      *Width = (UINT8) sizeof (UINTN);
-      return (UINT16)OFFSET_OF(DEBUG_DATA_IA32_FX_SAVE_STATE, Mxcsr_Mask);
-    }
-  }
-
-  if (Index < SOFT_DEBUGGER_REGISTER_XMM0) {
-    *Width = 10;
-  } else if (Index < SOFT_DEBUGGER_REGISTER_MM0 ) {
-    *Width = 16;
-  } else {
-    *Width = 8;
-    Index -= SOFT_DEBUGGER_REGISTER_MM0 - SOFT_DEBUGGER_REGISTER_ST0;
-  }
-
-  return (UINT16)(OFFSET_OF(DEBUG_DATA_IA32_FX_SAVE_STATE, St0Mm0) + (Index - SOFT_DEBUGGER_REGISTER_ST0) * 16);
-}
-
-/**
-  Write specified register into save CPU context.
-
-  @param[in] CpuContext         Pointer to saved CPU context.
-  @param[in] Index              Register index value.
-  @param[in] Offset             Offset in register address range.
-  @param[in] Width              Data width to read.
-  @param[in] RegisterBuffer     Pointer to input buffer with data.
-
-**/
-VOID
-ArchWriteRegisterBuffer (
-  IN DEBUG_CPU_CONTEXT               *CpuContext,
-  IN UINT8                           Index,
-  IN UINT8                           Offset,
-  IN UINT8                           Width,
-  IN UINT8                           *RegisterBuffer
-  )
-{
-  UINT8           *Buffer;
-  if (Index < SOFT_DEBUGGER_REGISTER_FP_BASE) {
-    Buffer = (UINT8 *) CpuContext + sizeof (DEBUG_DATA_IA32_FX_SAVE_STATE) + Index * 4;
-  } else {
-    //
-    // If it is MMX register, adjust its index position
-    //
-    if (Index >= SOFT_DEBUGGER_REGISTER_MM0) {
-      Index -= SOFT_DEBUGGER_REGISTER_MM0 - SOFT_DEBUGGER_REGISTER_ST0;
-    }
-    //
-    // FPU/MMX/XMM registers
-    //
-    Buffer = (UINT8 *) CpuContext + ArchReadFxStatOffset (Index, &Width);
-  }
-
-  CopyMem (Buffer + Offset, RegisterBuffer, Width);
-}
-
-/**
-  Read register value from saved CPU context.
-
-  @param[in] CpuContext         Pointer to saved CPU context.
-  @param[in] Index              Register index value.
-  @param[in] Offset             Offset in register address range
-  @param[in] Width              Data width to read.
-
-  @return The address of register value.
-
-**/
-UINT8 *
-ArchReadRegisterBuffer (
-  IN DEBUG_CPU_CONTEXT               *CpuContext,
-  IN UINT8                           Index,
-  IN UINT8                           Offset,
-  IN UINT8                           *Width
-  )
-{
-  UINT8           *Buffer;
-
-  if (Index < SOFT_DEBUGGER_REGISTER_FP_BASE) {
-    Buffer = (UINT8 *) CpuContext + sizeof (DEBUG_DATA_IA32_FX_SAVE_STATE) + Index * 4;
-    if (*Width == 0) {
-      *Width = (UINT8) sizeof (UINTN);
-    }
-  } else {
-    //
-    // FPU/MMX/XMM registers
-    //
-    Buffer = (UINT8 *) CpuContext + ArchReadFxStatOffset (Index, Width);
-  }
-
-  return Buffer;
-}
-
-/**
-  Read group register of common registers.
-
-  @param[in] CpuContext           Pointer to saved CPU context.
-  @param[in] RegisterGroup        Pointer to Group registers.
-
-**/
-VOID
-ReadRegisterGroup (
-  IN DEBUG_CPU_CONTEXT                       *CpuContext,
-  IN DEBUG_DATA_REPONSE_READ_REGISTER_GROUP  *RegisterGroup
-  )
-{
-  RegisterGroup->Cs     = (UINT16) CpuContext->Cs;
-  RegisterGroup->Ds     = (UINT16) CpuContext->Ds;
-  RegisterGroup->Es     = (UINT16) CpuContext->Es;
-  RegisterGroup->Fs     = (UINT16) CpuContext->Fs;
-  RegisterGroup->Gs     = (UINT16) CpuContext->Gs;
-  RegisterGroup->Ss     = (UINT16) CpuContext->Ss;
-  RegisterGroup->Eflags = CpuContext->Eflags;
-  RegisterGroup->Ebp    = CpuContext->Ebp;
-  RegisterGroup->Eip    = CpuContext->Eip;
-  RegisterGroup->Esp    = CpuContext->Esp;
-  RegisterGroup->Eax    = CpuContext->Eax;
-  RegisterGroup->Ebx    = CpuContext->Ebx;
-  RegisterGroup->Ecx    = CpuContext->Ecx;
-  RegisterGroup->Edx    = CpuContext->Edx;
-  RegisterGroup->Esi    = CpuContext->Esi;
-  RegisterGroup->Edi    = CpuContext->Edi;
-  RegisterGroup->Dr0    = CpuContext->Dr0;
-  RegisterGroup->Dr1    = CpuContext->Dr1;
-  RegisterGroup->Dr2    = CpuContext->Dr2;
-  RegisterGroup->Dr3    = CpuContext->Dr3;
-  RegisterGroup->Dr6    = CpuContext->Dr6;
-  RegisterGroup->Dr7    = CpuContext->Dr7;
-}
 
 /**
@@ -222 +40 @@
 
   for (Index = 0; Index < 20; Index ++) {
-    if ((PcdGet32 (PcdExceptionsIgnoredByDebugger) & (1 << Index)) != 0) {
+    if (((PcdGet32 (PcdExceptionsIgnoredByDebugger) & ~(BIT1 | BIT3)) & (1 << Index)) != 0) {
       //
-      // If the exception is masked to be reserved, skip it
+      // If the exception is masked to be reserved except for INT1 and INT3, skip it
       //
       continue;
@@ -231 +49 @@
     IdtEntry[Index].Bits.OffsetLow  = (UINT16)(UINTN)InterruptHandler;
     IdtEntry[Index].Bits.OffsetHigh = (UINT16)((UINTN)InterruptHandler >> 16);
-    IdtEntry[Index].Bits.Selector        = CodeSegment;
-    IdtEntry[Index].Bits.GateType        = IA32_IDT_GATE_TYPE_INTERRUPT_32;
+    IdtEntry[Index].Bits.Selector   = CodeSegment;
+    IdtEntry[Index].Bits.GateType   = IA32_IDT_GATE_TYPE_INTERRUPT_32;
   }
 
@@ -238 +56 @@
   IdtEntry[DEBUG_TIMER_VECTOR].Bits.OffsetLow  = (UINT16)(UINTN)InterruptHandler;
   IdtEntry[DEBUG_TIMER_VECTOR].Bits.OffsetHigh = (UINT16)((UINTN)InterruptHandler >> 16);
-  IdtEntry[Index].Bits.Selector        = CodeSegment;
-  IdtEntry[Index].Bits.GateType        = IA32_IDT_GATE_TYPE_INTERRUPT_32;
+  IdtEntry[DEBUG_TIMER_VECTOR].Bits.Selector   = CodeSegment;
+  IdtEntry[DEBUG_TIMER_VECTOR].Bits.GateType   = IA32_IDT_GATE_TYPE_INTERRUPT_32;
+  
+  //
+  // Set DE flag in CR4 to enable IO breakpoint
+  //
+  AsmWriteCr4 (AsmReadCr4 () | BIT3);
 }
+
+/**
+  Retrieve exception handler from IDT table by ExceptionNum.
+
+  @param[in]  ExceptionNum    Exception number
+ 
+  @return Exception handler
+
+**/
+VOID *
+GetExceptionHandlerInIdtEntry (
+  IN UINTN             ExceptionNum
+  )
+{
+  IA32_IDT_GATE_DESCRIPTOR   *IdtEntry;
+  IA32_DESCRIPTOR            IdtDescriptor;
+
+  AsmReadIdtr (&IdtDescriptor);
+  IdtEntry = (IA32_IDT_GATE_DESCRIPTOR *) IdtDescriptor.Base;
+
+  return (VOID *) (((UINTN)IdtEntry[ExceptionNum].Bits.OffsetLow) |
+                  (((UINTN)IdtEntry[ExceptionNum].Bits.OffsetHigh) << 16));
+} 
+
+/**
+  Set exception handler in IDT table by ExceptionNum.
+
+  @param[in]  ExceptionNum      Exception number
+  @param[in]  ExceptionHandler  Exception Handler to be set 
+
+**/
+VOID
+SetExceptionHandlerInIdtEntry (
+  IN UINTN             ExceptionNum,
+  IN VOID              *ExceptionHandler
+  )
+{
+  IA32_IDT_GATE_DESCRIPTOR   *IdtEntry;
+  IA32_DESCRIPTOR            IdtDescriptor;
+
+  AsmReadIdtr (&IdtDescriptor);
+  IdtEntry = (IA32_IDT_GATE_DESCRIPTOR *) IdtDescriptor.Base;
+
+  IdtEntry[ExceptionNum].Bits.OffsetLow   = (UINT16)(UINTN)ExceptionHandler;
+  IdtEntry[ExceptionNum].Bits.OffsetHigh  = (UINT16)((UINTN)ExceptionHandler >> 16);
+}

trunk/src/VBox/Devices/EFI/Firmware/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/Ia32/ArchDebugSupport.h

@@ -2 +2 @@
   IA32 specific defintions for debug agent library instance.
 
-  Copyright (c) 2010, Intel Corporation. All rights reserved.<BR>
+  Copyright (c) 2010 - 2012, Intel Corporation. All rights reserved.<BR>
   This program and the accompanying materials
   are licensed and made available under the terms and conditions of the BSD License
@@ -16 +16 @@
 #define _ARCH_DEBUG_SUPPORT_H_
 
-#include "ArchRegisters.h"
+#include "ProcessorContext.h"
 #include "TransferProtocol.h"
 
-typedef DEBUG_DATA_REPONSE_READ_REGISTER_GROUP_IA32          DEBUG_DATA_REPONSE_READ_REGISTER_GROUP;
-typedef DEBUG_DATA_REPONSE_READ_REGISTER_GROUP_SEGLIM_IA32   DEBUG_DATA_REPONSE_READ_REGISTER_GROUP_SEGLIM;
-typedef DEBUG_DATA_REPONSE_READ_REGISTER_GROUP_SEGBASE_IA32  DEBUG_DATA_REPONSE_READ_REGISTER_GROUP_SEGBASE;
-
 #define DEBUG_SW_BREAKPOINT_SYMBOL       0xcc
-
 #define DEBUG_ARCH_SYMBOL                DEBUG_DATA_BREAK_CPU_ARCH_IA32
 
+typedef DEBUG_DATA_IA32_FX_SAVE_STATE    DEBUG_DATA_FX_SAVE_STATE;
 typedef DEBUG_DATA_IA32_SYSTEM_CONTEXT   DEBUG_CPU_CONTEXT;
 

trunk/src/VBox/Devices/EFI/Firmware/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/Ia32/AsmFuncs.S

@@ -1 +1 @@
 #------------------------------------------------------------------------------
 #
-# Copyright (c) 2010, Intel Corporation. All rights reserved.<BR>
+# Copyright (c) 2010 - 2013, Intel Corporation. All rights reserved.<BR>
 # This program and the accompanying materials
 # are licensed and made available under the terms and conditions of the BSD License
@@ -28 +28 @@
 ASM_GLOBAL ASM_PFX(CommonEntry)
 
+.macro  AGENT_HANDLER_SIGNATURE
+  .byte 0x41, 0x47, 0x54, 0x48   # AGENT_HANDLER_SIGNATURE     SIGNATURE_32('A','G','T','H')
+.endm
+
 .data
 
-ASM_PFX(ExceptionStubHeaderSize):  .word     ASM_PFX(Exception1Handle) - ASM_PFX(Exception0Handle)
+ASM_PFX(ExceptionStubHeaderSize):  .long     ASM_PFX(Exception1Handle) - ASM_PFX(Exception0Handle)
 
 .text
 
+AGENT_HANDLER_SIGNATURE
 ASM_PFX(Exception0Handle):
    cli
@@ -39 +44 @@
    mov   $0, %eax
    jmp   ASM_PFX(CommonEntry)
+AGENT_HANDLER_SIGNATURE
 ASM_PFX(Exception1Handle):
    cli
@@ -44 +50 @@
    mov   $1, %eax
    jmp   ASM_PFX(CommonEntry)
+AGENT_HANDLER_SIGNATURE
 ASM_PFX(Exception2Handle):
    cli
@@ -49 +56 @@
    mov   $2, %eax
    jmp   ASM_PFX(CommonEntry)
+AGENT_HANDLER_SIGNATURE
 ASM_PFX(Exception3Handle):
    cli
@@ -54 +62 @@
    mov   $3, %eax
    jmp   ASM_PFX(CommonEntry)
+AGENT_HANDLER_SIGNATURE
 ASM_PFX(Exception4Handle):
    cli
@@ -59 +68 @@
    mov   $4, %eax
    jmp   ASM_PFX(CommonEntry)
+AGENT_HANDLER_SIGNATURE
 ASM_PFX(Exception5Handle):
    cli
@@ -64 +74 @@
    mov   $5, %eax
    jmp   ASM_PFX(CommonEntry)
+AGENT_HANDLER_SIGNATURE
 ASM_PFX(Exception6Handle):
    cli
@@ -69 +80 @@
    mov   $6, %eax
    jmp   ASM_PFX(CommonEntry)
+AGENT_HANDLER_SIGNATURE
 ASM_PFX(Exception7Handle):
    cli
@@ -74 +86 @@
    mov   $7, %eax
    jmp   ASM_PFX(CommonEntry)
+AGENT_HANDLER_SIGNATURE
 ASM_PFX(Exception8Handle):
    cli
@@ -79 +92 @@
    mov   $8, %eax
    jmp   ASM_PFX(CommonEntry)
+AGENT_HANDLER_SIGNATURE
 ASM_PFX(Exception9Handle):
    cli
@@ -84 +98 @@
    mov   $9, %eax
    jmp   ASM_PFX(CommonEntry)
+AGENT_HANDLER_SIGNATURE
 ASM_PFX(Exception10Handle):
    cli
@@ -89 +104 @@
    mov   $10, %eax
    jmp   ASM_PFX(CommonEntry)
+AGENT_HANDLER_SIGNATURE
 ASM_PFX(Exception11Handle):
    cli
@@ -94 +110 @@
    mov   $11, %eax
    jmp   ASM_PFX(CommonEntry)
+AGENT_HANDLER_SIGNATURE
 ASM_PFX(Exception12Handle):
    cli
@@ -99 +116 @@
    mov   $12, %eax
    jmp   ASM_PFX(CommonEntry)
+AGENT_HANDLER_SIGNATURE
 ASM_PFX(Exception13Handle):
    cli
@@ -104 +122 @@
    mov   $13, %eax
    jmp   ASM_PFX(CommonEntry)
+AGENT_HANDLER_SIGNATURE
 ASM_PFX(Exception14Handle):
    cli
@@ -109 +128 @@
    mov   $14, %eax
    jmp   ASM_PFX(CommonEntry)
+AGENT_HANDLER_SIGNATURE
 ASM_PFX(Exception15Handle):
    cli
@@ -114 +134 @@
    mov   $15, %eax
    jmp   ASM_PFX(CommonEntry)
+AGENT_HANDLER_SIGNATURE
 ASM_PFX(Exception16Handle):
    cli
@@ -119 +140 @@
    mov   $16, %eax
    jmp   ASM_PFX(CommonEntry)
+AGENT_HANDLER_SIGNATURE
 ASM_PFX(Exception17Handle):
    cli
@@ -124 +146 @@
    mov   $17, %eax
    jmp   ASM_PFX(CommonEntry)
+AGENT_HANDLER_SIGNATURE
 ASM_PFX(Exception18Handle):
    cli
@@ -129 +152 @@
    mov   $18, %eax
    jmp   ASM_PFX(CommonEntry)
+AGENT_HANDLER_SIGNATURE
 ASM_PFX(Exception19Handle):
    cli
@@ -134 +158 @@
    mov   $19, %eax
    jmp   ASM_PFX(CommonEntry)
-
+AGENT_HANDLER_SIGNATURE
 ASM_PFX(TimerInterruptHandle):
    cli
@@ -260 +284 @@
 ## clear Dr7 while executing debugger itself
   xorl    %eax,%eax
-#  movl    %eax, %dr7
+  movl    %eax, %dr7
 
   movl    %dr6, %eax
@@ -281 +305 @@
   movl    %esp,%edi
   .byte 0x0f, 0xae, 0x07   # fxsave [edi]
+
+## save the exception data
+  pushl   8(%esp)
 
 ## Clear Direction Flag
@@ -290 +317 @@
   call    ASM_PFX(InterruptProcess)
   addl    $8,%esp
+
+## skip the exception data
+  addl    $4,%esp
 
 ## FX_SAVE_STATE_IA32 FxSaveState;

trunk/src/VBox/Devices/EFI/Firmware/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/Ia32/AsmFuncs.asm

@@ -1 +1 @@
 ;------------------------------------------------------------------------------
 ;
-; Copyright (c) 2010, Intel Corporation. All rights reserved.<BR>
+; Copyright (c) 2010 - 2013, Intel Corporation. All rights reserved.<BR>
 ; This program and the accompanying materials
 ; are licensed and made available under the terms and conditions of the BSD License
@@ -33 +33 @@
 public    Exception0Handle, TimerInterruptHandle, ExceptionStubHeaderSize
 
+AGENT_HANDLER_SIGNATURE  MACRO
+  db   41h, 47h, 54h, 48h       ; SIGNATURE_32('A','G','T','H')
+ENDM
+
 .data
 
-ExceptionStubHeaderSize   DW    Exception1Handle - Exception0Handle
+ExceptionStubHeaderSize   DD    Exception1Handle - Exception0Handle
 CommonEntryAddr           DD    CommonEntry
 
 .code
 
+AGENT_HANDLER_SIGNATURE
 Exception0Handle:
     cli
@@ -45 +50 @@
     mov     eax, 0
     jmp     dword ptr [CommonEntryAddr]
+AGENT_HANDLER_SIGNATURE
 Exception1Handle:
     cli
@@ -50 +56 @@
     mov     eax, 1
     jmp     dword ptr [CommonEntryAddr]
+AGENT_HANDLER_SIGNATURE
 Exception2Handle:
     cli
@@ -55 +62 @@
     mov     eax, 2
     jmp     dword ptr [CommonEntryAddr]
+AGENT_HANDLER_SIGNATURE
 Exception3Handle:
     cli
@@ -60 +68 @@
     mov     eax, 3
     jmp     dword ptr [CommonEntryAddr]
+AGENT_HANDLER_SIGNATURE
 Exception4Handle:
     cli
@@ -65 +74 @@
     mov     eax, 4
     jmp     dword ptr [CommonEntryAddr]
+AGENT_HANDLER_SIGNATURE
 Exception5Handle:
     cli
@@ -70 +80 @@
     mov     eax, 5
     jmp     dword ptr [CommonEntryAddr]
+AGENT_HANDLER_SIGNATURE
 Exception6Handle:
     cli
@@ -75 +86 @@
     mov     eax, 6
     jmp     dword ptr [CommonEntryAddr]
+AGENT_HANDLER_SIGNATURE
 Exception7Handle:
     cli
@@ -80 +92 @@
     mov     eax, 7
     jmp     dword ptr [CommonEntryAddr]
+AGENT_HANDLER_SIGNATURE
 Exception8Handle:
     cli
@@ -85 +98 @@
     mov     eax, 8
     jmp     dword ptr [CommonEntryAddr]
+AGENT_HANDLER_SIGNATURE
 Exception9Handle:
     cli
@@ -90 +104 @@
     mov     eax, 9
     jmp     dword ptr [CommonEntryAddr]
+AGENT_HANDLER_SIGNATURE
 Exception10Handle:
     cli
@@ -95 +110 @@
     mov     eax, 10
     jmp     dword ptr [CommonEntryAddr]
+AGENT_HANDLER_SIGNATURE
 Exception11Handle:
     cli
@@ -100 +116 @@
     mov     eax, 11
     jmp     dword ptr [CommonEntryAddr]
+AGENT_HANDLER_SIGNATURE
 Exception12Handle:
     cli
@@ -105 +122 @@
     mov     eax, 12
     jmp     dword ptr [CommonEntryAddr]
+AGENT_HANDLER_SIGNATURE
 Exception13Handle:
     cli
@@ -110 +128 @@
     mov     eax, 13
     jmp     dword ptr [CommonEntryAddr]
+AGENT_HANDLER_SIGNATURE
 Exception14Handle:
     cli
@@ -115 +134 @@
     mov     eax, 14
     jmp     dword ptr [CommonEntryAddr]
+AGENT_HANDLER_SIGNATURE
 Exception15Handle:
     cli
@@ -120 +140 @@
     mov     eax, 15
     jmp     dword ptr [CommonEntryAddr]
+AGENT_HANDLER_SIGNATURE
 Exception16Handle:
     cli
@@ -125 +146 @@
     mov     eax, 16
     jmp     dword ptr [CommonEntryAddr]
+AGENT_HANDLER_SIGNATURE
 Exception17Handle:
     cli
@@ -130 +152 @@
     mov     eax, 17
     jmp     dword ptr [CommonEntryAddr]
+AGENT_HANDLER_SIGNATURE
 Exception18Handle:
     cli
@@ -135 +158 @@
     mov     eax, 18
     jmp     dword ptr [CommonEntryAddr]
+AGENT_HANDLER_SIGNATURE
 Exception19Handle:
     cli
@@ -140 +164 @@
     mov     eax, 19
     jmp     dword ptr [CommonEntryAddr]
-
+AGENT_HANDLER_SIGNATURE
 TimerInterruptHandle:
     cli
@@ -260 +284 @@
     ;; clear Dr7 while executing debugger itself
     xor     eax, eax
- ;; mov     dr7, eax
+    mov     dr7, eax
 
     ;; Dr6
@@ -283 +307 @@
     mov edi, esp
     db 0fh, 0aeh, 00000111y ;fxsave [edi]
+
+    ;; save the exception data    
+    push    dword ptr [ebp + 8]
 
     ;; Clear Direction Flag
@@ -292 +319 @@
     call    InterruptProcess
     add     esp, 8
+
+    ; skip the exception data
+    add     esp, 4
 
     ;; FX_SAVE_STATE_IA32 FxSaveState;

trunk/src/VBox/Devices/EFI/Firmware/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/X64/ArchDebugSupport.c

@@ -1 +1 @@
 /** @file
-  Supporting functions for x64 architecture.
+  Supporting functions for X64 architecture.
 
-  Copyright (c) 2010, Intel Corporation. All rights reserved.<BR>
+  Copyright (c) 2010 - 2013, Intel Corporation. All rights reserved.<BR>
   This program and the accompanying materials
   are licensed and made available under the terms and conditions of the BSD License
@@ -14 +14 @@
 
 #include "DebugAgent.h"
-
-/**
-  Read the offset of FP / MMX / XMM registers by register index.
-
-  @param[in]  Index    Register index.
-  @param[out] Width    Register width returned.
-
-  @return Offset in register address range.
-
-**/
-UINT16
-ArchReadFxStatOffset (
-  IN  UINT8                     Index,
-  OUT UINT8                     *Width
-  )
-{
-  if (Index < SOFT_DEBUGGER_REGISTER_ST0) {
-    switch (Index) {
-    case SOFT_DEBUGGER_REGISTER_FP_FCW:
-      *Width = (UINT8) sizeof (UINT16);
-      return OFFSET_OF(DEBUG_DATA_X64_FX_SAVE_STATE, Fcw);
-
-    case SOFT_DEBUGGER_REGISTER_FP_FSW:
-      *Width = (UINT8) sizeof (UINT16);
-      return OFFSET_OF(DEBUG_DATA_X64_FX_SAVE_STATE, Fsw);
-
-    case SOFT_DEBUGGER_REGISTER_FP_FTW:
-      *Width = (UINT8) sizeof (UINT16);
-      return OFFSET_OF(DEBUG_DATA_X64_FX_SAVE_STATE, Ftw);
-
-    case SOFT_DEBUGGER_REGISTER_FP_OPCODE:
-      *Width = (UINT8) sizeof (UINT16);
-      return OFFSET_OF(DEBUG_DATA_X64_FX_SAVE_STATE, Opcode);
-
-    case SOFT_DEBUGGER_REGISTER_FP_EIP:
-      *Width = (UINT8) sizeof (UINTN);
-      return OFFSET_OF(DEBUG_DATA_X64_FX_SAVE_STATE, Rip);
-
-    case SOFT_DEBUGGER_REGISTER_FP_DATAOFFSET:
-      *Width = (UINT8) sizeof (UINTN);
-      return OFFSET_OF(DEBUG_DATA_X64_FX_SAVE_STATE, DataOffset);
-
-    case SOFT_DEBUGGER_REGISTER_FP_MXCSR:
-      *Width = (UINT8) sizeof (UINT32);
-      return OFFSET_OF(DEBUG_DATA_X64_FX_SAVE_STATE, Mxcsr);
-
-    case SOFT_DEBUGGER_REGISTER_FP_MXCSR_MASK:
-      *Width = (UINT8) sizeof (UINT32);
-      return OFFSET_OF(DEBUG_DATA_X64_FX_SAVE_STATE, Mxcsr_Mask);
-
-    default:
-      return (UINT16) (-1);
-    }
-  }
-
-  if (Index < SOFT_DEBUGGER_REGISTER_XMM0) {
-    *Width = 10;
-  } else if (Index < SOFT_DEBUGGER_REGISTER_MM0 ) {
-    *Width = 16;
-  } else {
-    *Width = 8;
-    Index -= SOFT_DEBUGGER_REGISTER_MM0 - SOFT_DEBUGGER_REGISTER_ST0;
-  }
-
-  return OFFSET_OF(DEBUG_DATA_X64_FX_SAVE_STATE, St0Mm0) + (Index - SOFT_DEBUGGER_REGISTER_ST0) * 16;
-}
-
-/**
-  Write specified register into save CPU context.
-
-  @param[in] CpuContext         Pointer to saved CPU context.
-  @param[in] Index              Register index value.
-  @param[in] Offset             Offset in register address range
-  @param[in] Width              Data width to read.
-  @param[in] RegisterBuffer     Pointer to input buffer with data.
-
-**/
-VOID
-ArchWriteRegisterBuffer (
-  IN DEBUG_CPU_CONTEXT               *CpuContext,
-  IN UINT8                           Index,
-  IN UINT8                           Offset,
-  IN UINT8                           Width,
-  IN UINT8                           *RegisterBuffer
-  )
-{
-  UINT8           *Buffer;
-  if (Index < SOFT_DEBUGGER_REGISTER_FP_BASE) {
-    Buffer = (UINT8 *) CpuContext + sizeof (DEBUG_DATA_X64_FX_SAVE_STATE) + Index * 8;
-  } else {
-    //
-    // If it is MMX register, adjust its index position
-    //
-    if (Index >= SOFT_DEBUGGER_REGISTER_MM0) {
-      Index -= SOFT_DEBUGGER_REGISTER_MM0 - SOFT_DEBUGGER_REGISTER_ST0;
-    }
-
-    //
-    // FPU/MMX/XMM registers
-    //
-    Buffer = (UINT8 *) CpuContext + ArchReadFxStatOffset (Index, &Width);
-  }
-
-  CopyMem (Buffer + Offset, RegisterBuffer, Width);
-}
-
-/**
-  Read register value from saved CPU context.
-
-  @param[in] CpuContext         Pointer to saved CPU context.
-  @param[in] Index              Register index value.
-  @param[in] Offset             Offset in register address range
-  @param[in] Width              Data width to read.
-
-  @return The address of register value.
-
-**/
-UINT8 *
-ArchReadRegisterBuffer (
-  IN DEBUG_CPU_CONTEXT               *CpuContext,
-  IN UINT8                           Index,
-  IN UINT8                           Offset,
-  IN UINT8                           *Width
-  )
-{
-  UINT8           *Buffer;
-
-  if (Index < SOFT_DEBUGGER_REGISTER_FP_BASE) {
-    Buffer = (UINT8 *) CpuContext + sizeof (DEBUG_DATA_X64_FX_SAVE_STATE) + Index * 8;
-    if (*Width == 0) {
-      *Width = (UINT8) sizeof (UINTN);
-    }
-  } else {
-    //
-    // FPU/MMX/XMM registers
-    //
-    Buffer = (UINT8 *) CpuContext + ArchReadFxStatOffset (Index, Width);
-  }
-
-  return Buffer;
-}
-
-/**
-  Read group register of common registers.
-
-  @param[in] CpuContext           Pointer to saved CPU context.
-  @param[in] RegisterGroup        Pointer to Group registers.
-
-**/
-VOID
-ReadRegisterGroup (
-  IN DEBUG_CPU_CONTEXT                       *CpuContext,
-  IN DEBUG_DATA_REPONSE_READ_REGISTER_GROUP  *RegisterGroup
-  )
-{
-  RegisterGroup->Cs     = (UINT16) CpuContext->Cs;
-  RegisterGroup->Ds     = (UINT16) CpuContext->Ds;
-  RegisterGroup->Es     = (UINT16) CpuContext->Es;
-  RegisterGroup->Fs     = (UINT16) CpuContext->Fs;
-  RegisterGroup->Gs     = (UINT16) CpuContext->Gs;
-  RegisterGroup->Ss     = (UINT16) CpuContext->Ss;
-  RegisterGroup->Eflags = (UINT32) CpuContext->Eflags;
-  RegisterGroup->Rbp    = CpuContext->Rbp;
-  RegisterGroup->Eip    = CpuContext->Eip;
-  RegisterGroup->Rsp    = CpuContext->Rsp;
-  RegisterGroup->Eax    = CpuContext->Rax;
-  RegisterGroup->Rbx    = CpuContext->Rbx;
-  RegisterGroup->Rcx    = CpuContext->Rcx;
-  RegisterGroup->Rdx    = CpuContext->Rdx;
-  RegisterGroup->Rsi    = CpuContext->Rsi;
-  RegisterGroup->Rdi    = CpuContext->Rdi;
-  RegisterGroup->R8     = CpuContext->R8;
-  RegisterGroup->R9     = CpuContext->R9;
-  RegisterGroup->R10    = CpuContext->R10;
-  RegisterGroup->R11    = CpuContext->R11;
-  RegisterGroup->R12    = CpuContext->R12;
-  RegisterGroup->R13    = CpuContext->R13;
-  RegisterGroup->R14    = CpuContext->R14;
-  RegisterGroup->R15    = CpuContext->R15;
-  RegisterGroup->Dr0    = CpuContext->Dr0;
-  RegisterGroup->Dr1    = CpuContext->Dr1;
-  RegisterGroup->Dr2    = CpuContext->Dr2;
-  RegisterGroup->Dr3    = CpuContext->Dr3;
-  RegisterGroup->Dr6    = CpuContext->Dr6;
-  RegisterGroup->Dr7    = CpuContext->Dr7;
-  RegisterGroup->Cr0    = CpuContext->Cr0;
-  RegisterGroup->Cr2    = CpuContext->Cr2;
-  RegisterGroup->Cr3    = CpuContext->Cr3;
-  RegisterGroup->Cr4    = CpuContext->Cr4;
-  RegisterGroup->Cr8    = CpuContext->Cr8;
-
-  CopyMem ((UINT8 *) &RegisterGroup->Xmm0[0], (UINT8 *) &CpuContext->FxSaveState.Xmm0[0], 16 * 10);
-}
 
 /**
@@ -233 +40 @@
 
   for (Index = 0; Index < 20; Index ++) {
-    if ((PcdGet32 (PcdExceptionsIgnoredByDebugger) & (1 << Index)) != 0) {
+    if (((PcdGet32 (PcdExceptionsIgnoredByDebugger) & ~(BIT1 | BIT3)) & (1 << Index)) != 0) {
       //
-      // If the exception is masked to be reserved, skip it
+      // If the exception is masked to be reserved except for INT1 and INT3, skip it
       //
       continue;
@@ -253 +60 @@
   IdtEntry[DEBUG_TIMER_VECTOR].Bits.Selector        = CodeSegment;
   IdtEntry[DEBUG_TIMER_VECTOR].Bits.GateType        = IA32_IDT_GATE_TYPE_INTERRUPT_32;
+
+  //
+  // Set DE flag in CR4 to enable IO breakpoint
+  //
+  AsmWriteCr4 (AsmReadCr4 () | BIT3);
 }
+
+/**
+  Retrieve exception handler from IDT table by ExceptionNum.
+
+  @param[in]  ExceptionNum    Exception number
+ 
+  @return Exception handler
+
+**/
+VOID *
+GetExceptionHandlerInIdtEntry (
+  IN UINTN             ExceptionNum
+  )
+{
+  IA32_IDT_GATE_DESCRIPTOR   *IdtEntry;
+  IA32_DESCRIPTOR            IdtDescriptor;
+
+  AsmReadIdtr (&IdtDescriptor);
+  IdtEntry = (IA32_IDT_GATE_DESCRIPTOR *) IdtDescriptor.Base;
+
+  return (VOID *) (IdtEntry[ExceptionNum].Bits.OffsetLow |
+                  (((UINTN)IdtEntry[ExceptionNum].Bits.OffsetHigh) << 16) |
+                  (((UINTN)IdtEntry[ExceptionNum].Bits.OffsetUpper) << 32));
+} 
+
+/**
+  Set exception handler in IDT table by ExceptionNum.
+
+  @param[in]  ExceptionNum      Exception number
+  @param[in]  ExceptionHandler  Exception Handler to be set 
+
+**/
+VOID
+SetExceptionHandlerInIdtEntry (
+  IN UINTN             ExceptionNum,
+  IN VOID              *ExceptionHandler
+  )
+{
+  IA32_IDT_GATE_DESCRIPTOR   *IdtEntry;
+  IA32_DESCRIPTOR            IdtDescriptor;
+
+  AsmReadIdtr (&IdtDescriptor);
+  IdtEntry = (IA32_IDT_GATE_DESCRIPTOR *) IdtDescriptor.Base;
+
+  IdtEntry[ExceptionNum].Bits.OffsetLow   = (UINT16)(UINTN)ExceptionHandler;
+  IdtEntry[ExceptionNum].Bits.OffsetHigh  = (UINT16)((UINTN)ExceptionHandler >> 16);
+  IdtEntry[ExceptionNum].Bits.OffsetUpper = (UINT32)((UINTN)ExceptionHandler >> 32);
+}

trunk/src/VBox/Devices/EFI/Firmware/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/X64/ArchDebugSupport.h

@@ -2 +2 @@
   X64 specific defintions for debug agent library instance.
 
-  Copyright (c) 2010, Intel Corporation. All rights reserved.<BR>
+  Copyright (c) 2010 - 2012, Intel Corporation. All rights reserved.<BR>
   This program and the accompanying materials
   are licensed and made available under the terms and conditions of the BSD License
@@ -16 +16 @@
 #define _ARCH_DEBUG_SUPPORT_H_
 
-#include "ArchRegisters.h"
+#include "ProcessorContext.h"
 #include "TransferProtocol.h"
 
-typedef DEBUG_DATA_REPONSE_READ_REGISTER_GROUP_X64          DEBUG_DATA_REPONSE_READ_REGISTER_GROUP;
-typedef DEBUG_DATA_REPONSE_READ_REGISTER_GROUP_SEGLIM_X64   DEBUG_DATA_REPONSE_READ_REGISTER_GROUP_SEGLIM;
-typedef DEBUG_DATA_REPONSE_READ_REGISTER_GROUP_SEGBASE_X64  DEBUG_DATA_REPONSE_READ_REGISTER_GROUP_SEGBASE;
-
 #define DEBUG_SW_BREAKPOINT_SYMBOL       0xcc
-
 #define DEBUG_ARCH_SYMBOL                DEBUG_DATA_BREAK_CPU_ARCH_X64
 
+typedef DEBUG_DATA_X64_FX_SAVE_STATE     DEBUG_DATA_FX_SAVE_STATE;
 typedef DEBUG_DATA_X64_SYSTEM_CONTEXT    DEBUG_CPU_CONTEXT;
 

trunk/src/VBox/Devices/EFI/Firmware/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/X64/AsmFuncs.S

@@ -1 +1 @@
 #------------------------------------------------------------------------------
 #
-# Copyright (c) 2006 - 2008, Intel Corporation. All rights reserved.<BR>
+# Copyright (c) 2006 - 2014, Intel Corporation. All rights reserved.<BR>
 # This program and the accompanying materials
 # are licensed and made available under the terms and conditions of the BSD License
@@ -29 +29 @@
 ASM_GLOBAL ASM_PFX(CommonEntry)
 
+.macro  AGENT_HANDLER_SIGNATURE
+  .byte 0x41, 0x47, 0x54, 0x48   # AGENT_HANDLER_SIGNATURE     SIGNATURE_32('A','G','T','H')
+.endm
+
 .data
 
-ASM_PFX(ExceptionStubHeaderSize):  .word     ASM_PFX(Exception1Handle) - ASM_PFX(Exception0Handle)
+ASM_PFX(ExceptionStubHeaderSize):  .long     ASM_PFX(Exception1Handle) - ASM_PFX(Exception0Handle)
 
 
 .text
 
+AGENT_HANDLER_SIGNATURE
 ASM_PFX(Exception0Handle):
    cli
@@ -41 +46 @@
    mov   $0, %rcx
    jmp   ASM_PFX(CommonEntry)
+AGENT_HANDLER_SIGNATURE
 ASM_PFX(Exception1Handle):
    cli
@@ -46 +52 @@
    mov   $1, %rcx
    jmp   ASM_PFX(CommonEntry)
+AGENT_HANDLER_SIGNATURE
 ASM_PFX(Exception2Handle):
    cli
@@ -51 +58 @@
    mov   $2, %rcx
    jmp   ASM_PFX(CommonEntry)
+AGENT_HANDLER_SIGNATURE
 ASM_PFX(Exception3Handle):
    cli
@@ -56 +64 @@
    mov   $3, %rcx
    jmp   ASM_PFX(CommonEntry)
+AGENT_HANDLER_SIGNATURE
 ASM_PFX(Exception4Handle):
    cli
@@ -61 +70 @@
    mov   $4, %rcx
    jmp   ASM_PFX(CommonEntry)
+AGENT_HANDLER_SIGNATURE
 ASM_PFX(Exception5Handle):
    cli
@@ -66 +76 @@
    mov   $5, %rcx
    jmp   ASM_PFX(CommonEntry)
+AGENT_HANDLER_SIGNATURE
 ASM_PFX(Exception6Handle):
    cli
@@ -71 +82 @@
    mov   $6, %rcx
    jmp   ASM_PFX(CommonEntry)
+AGENT_HANDLER_SIGNATURE
 ASM_PFX(Exception7Handle):
    cli
@@ -76 +88 @@
    mov   $7, %rcx
    jmp   ASM_PFX(CommonEntry)
+AGENT_HANDLER_SIGNATURE
 ASM_PFX(Exception8Handle):
    cli
@@ -81 +94 @@
    mov   $8, %rcx
    jmp   ASM_PFX(CommonEntry)
+AGENT_HANDLER_SIGNATURE
 ASM_PFX(Exception9Handle):
    cli
@@ -86 +100 @@
    mov   $9, %rcx
    jmp   ASM_PFX(CommonEntry)
+AGENT_HANDLER_SIGNATURE
 ASM_PFX(Exception10Handle):
    cli
@@ -91 +106 @@
    mov   $10, %rcx
    jmp   ASM_PFX(CommonEntry)
+AGENT_HANDLER_SIGNATURE
 ASM_PFX(Exception11Handle):
    cli
@@ -96 +112 @@
    mov   $11, %rcx
    jmp   ASM_PFX(CommonEntry)
+AGENT_HANDLER_SIGNATURE
 ASM_PFX(Exception12Handle):
    cli
@@ -101 +118 @@
    mov   $12, %rcx
    jmp   ASM_PFX(CommonEntry)
+AGENT_HANDLER_SIGNATURE
 ASM_PFX(Exception13Handle):
    cli
@@ -106 +124 @@
    mov   $13, %rcx
    jmp   ASM_PFX(CommonEntry)
+AGENT_HANDLER_SIGNATURE
 ASM_PFX(Exception14Handle):
    cli
@@ -111 +130 @@
    mov   $14, %rcx
    jmp   ASM_PFX(CommonEntry)
+AGENT_HANDLER_SIGNATURE
 ASM_PFX(Exception15Handle):
    cli
@@ -116 +136 @@
    mov   $15, %rcx
    jmp   ASM_PFX(CommonEntry)
+AGENT_HANDLER_SIGNATURE
 ASM_PFX(Exception16Handle):
    cli
@@ -121 +142 @@
    mov   $16, %rcx
    jmp   ASM_PFX(CommonEntry)
+AGENT_HANDLER_SIGNATURE
 ASM_PFX(Exception17Handle):
    cli
@@ -126 +148 @@
    mov   $17, %rcx
    jmp   ASM_PFX(CommonEntry)
+AGENT_HANDLER_SIGNATURE
 ASM_PFX(Exception18Handle):
    cli
@@ -131 +154 @@
    mov   $18, %rcx
    jmp   ASM_PFX(CommonEntry)
+AGENT_HANDLER_SIGNATURE
 ASM_PFX(Exception19Handle):
    cli
@@ -136 +160 @@
    mov   $19, %rcx
    jmp   ASM_PFX(CommonEntry)
-
+AGENT_HANDLER_SIGNATURE
 ASM_PFX(TimerInterruptHandle):
    cli
@@ -248 +272 @@
 #  movq     %cs, %rax
   pushq    %rax
-  movq     %ds, %rax
-  pushq    %rax
-  movq     %es, %rax
-  pushq    %rax
-  movq     %fs, %rax
-  pushq    %rax
-  movq     %gs, %rax
+  mov      %ds, %rax
+  pushq    %rax
+  mov      %es, %rax
+  pushq    %rax
+  mov      %fs, %rax
+  pushq    %rax
+  mov      %gs, %rax
   pushq    %rax
 
@@ -281 +305 @@
 ## clear Dr7 while executing debugger itself
    xorq    %rax, %rax
-#debug   movq    %rax, %dr7
+   movq    %rax, %dr7
 
    movq    %dr6, %rax
@@ -302 +326 @@
    movq    %rsp, %rdi
    .byte   0x0f, 0xae, 0b00000111
+
+## save the exception data;
+   pushq   16(%rbp)
 
 ## Clear Direction Flag
@@ -314 +341 @@
   # and make sure RSP is 16-byte aligned
   #
-   subq    $(4 * 8), %rsp
+   subq    $(32 + 8), %rsp
    call    ASM_PFX(InterruptProcess)
-   addq    $(4 * 8), %rsp
+   addq    $(32 + 8), %rsp
+
+## skip the exception data;
+   addq    $8, %rsp
 
 ## FX_SAVE_STATE_X64 FxSaveState;
@@ -356 +386 @@
   # (X64 will not use fs and gs, so we do not restore it)
    popq     %rax
-  movw    %rax, %es
-   popq     %rax
-  movw    %rax, %ds
+   mov      %rax, %es
+   popq     %rax
+   mov      %rax, %ds
    popq     32(%rbp)
    popq     56(%rbp)

trunk/src/VBox/Devices/EFI/Firmware/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/X64/AsmFuncs.asm

@@ -1 +1 @@
 ;------------------------------------------------------------------------------
 ;
-; Copyright (c) 2010, Intel Corporation. All rights reserved.<BR>
+; Copyright (c) 2010 - 2013, Intel Corporation. All rights reserved.<BR>
 ; This program and the accompanying materials
 ; are licensed and made available under the terms and conditions of the BSD License
@@ -29 +29 @@
 public          Exception0Handle, TimerInterruptHandle, ExceptionStubHeaderSize
 
-ExceptionStubHeaderSize        dw      Exception1Handle - Exception0Handle ;
+AGENT_HANDLER_SIGNATURE  MACRO
+  db   41h, 47h, 54h, 48h       ; SIGNATURE_32('A','G','T','H')
+ENDM
+
+ExceptionStubHeaderSize        dd      Exception1Handle - Exception0Handle ;
 CommonEntryAddr                dq      CommonEntry ;
 
 .code
 
+AGENT_HANDLER_SIGNATURE
 Exception0Handle:
     cli
@@ -39 +44 @@
     mov     rcx, 0
     jmp     qword ptr [CommonEntryAddr]
+AGENT_HANDLER_SIGNATURE 
 Exception1Handle:
     cli
@@ -44 +50 @@
     mov     rcx, 1
     jmp     qword ptr [CommonEntryAddr]
+AGENT_HANDLER_SIGNATURE
 Exception2Handle:
     cli
@@ -49 +56 @@
     mov     rcx, 2
     jmp     qword ptr [CommonEntryAddr]
+AGENT_HANDLER_SIGNATURE
 Exception3Handle:
     cli
@@ -54 +62 @@
     mov     rcx, 3
     jmp     qword ptr [CommonEntryAddr]
+AGENT_HANDLER_SIGNATURE
 Exception4Handle:
     cli
@@ -59 +68 @@
     mov     rcx, 4
     jmp     qword ptr [CommonEntryAddr]
+AGENT_HANDLER_SIGNATURE
 Exception5Handle:
     cli
@@ -64 +74 @@
     mov     rcx, 5
     jmp     qword ptr [CommonEntryAddr]
+AGENT_HANDLER_SIGNATURE
 Exception6Handle:
     cli
@@ -69 +80 @@
     mov     rcx, 6
     jmp     qword ptr [CommonEntryAddr]
+AGENT_HANDLER_SIGNATURE
 Exception7Handle:
     cli
@@ -74 +86 @@
     mov     rcx, 7
     jmp     qword ptr [CommonEntryAddr]
+AGENT_HANDLER_SIGNATURE
 Exception8Handle:
     cli
@@ -79 +92 @@
     mov     rcx, 8
     jmp     qword ptr [CommonEntryAddr]
+AGENT_HANDLER_SIGNATURE
 Exception9Handle:
     cli
@@ -84 +98 @@
     mov     rcx, 9
     jmp     qword ptr [CommonEntryAddr]
+AGENT_HANDLER_SIGNATURE
 Exception10Handle:
     cli
@@ -89 +104 @@
     mov     rcx, 10
     jmp     qword ptr [CommonEntryAddr]
+AGENT_HANDLER_SIGNATURE
 Exception11Handle:
     cli
@@ -94 +110 @@
     mov     rcx, 11
     jmp     qword ptr [CommonEntryAddr]
+AGENT_HANDLER_SIGNATURE
 Exception12Handle:
     cli
@@ -99 +116 @@
     mov     rcx, 12
     jmp     qword ptr [CommonEntryAddr]
+AGENT_HANDLER_SIGNATURE
 Exception13Handle:
     cli
@@ -104 +122 @@
     mov     rcx, 13
     jmp     qword ptr [CommonEntryAddr]
+AGENT_HANDLER_SIGNATURE
 Exception14Handle:
     cli
@@ -109 +128 @@
     mov     rcx, 14
     jmp     qword ptr [CommonEntryAddr]
+AGENT_HANDLER_SIGNATURE
 Exception15Handle:
     cli
@@ -114 +134 @@
     mov     rcx, 15
     jmp     qword ptr [CommonEntryAddr]
+AGENT_HANDLER_SIGNATURE
 Exception16Handle:
     cli
@@ -119 +140 @@
     mov     rcx, 16
     jmp     qword ptr [CommonEntryAddr]
+AGENT_HANDLER_SIGNATURE
 Exception17Handle:
     cli
@@ -124 +146 @@
     mov     rcx, 17
     jmp     qword ptr [CommonEntryAddr]
+AGENT_HANDLER_SIGNATURE
 Exception18Handle:
     cli
@@ -129 +152 @@
     mov     rcx, 18
     jmp     qword ptr [CommonEntryAddr]
+AGENT_HANDLER_SIGNATURE
 Exception19Handle:
     cli
@@ -134 +158 @@
     mov     rcx, 19
     jmp     qword ptr [CommonEntryAddr]
-
+AGENT_HANDLER_SIGNATURE
 TimerInterruptHandle:
     cli
@@ -199 +223 @@
     mov     rax, cr2
     push    rax
-    push    0         ; cr0 will not saved???
+    push    0
     mov     rax, cr0
     push    rax
@@ -264 +288 @@
 
     sub     rsp, 512
-    mov rdi, rsp
+    mov     rdi, rsp
     db 0fh, 0aeh, 00000111y ;fxsave [rdi]
+
+    ;; save the exception data
+    push    qword ptr [rbp + 16]
 
     ;; Clear Direction Flag
@@ -274 +301 @@
     mov     r15, rcx      ; save vector in r15
     
-    sub     rsp, 32
+    ;
+    ; Per X64 calling convention, allocate maximum parameter stack space
+    ; and make sure RSP is 16-byte aligned
+    ;
+    sub     rsp, 32 + 8
     call    InterruptProcess
-    add     rsp, 32
+    add     rsp, 32 + 8
+
+    ;; skip the exception data
+    add     rsp, 8
     
     mov     rsi, rsp

trunk/src/VBox/Devices/EFI/Firmware/SourceLevelDebugPkg/Library/DebugAgent/DxeDebugAgent/DxeDebugAgentLib.c

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

trunk/src/VBox/Devices/EFI/Firmware/SourceLevelDebugPkg/Library/DebugAgent/DxeDebugAgent/DxeDebugAgentLib.h

@@ -2 +2 @@
   Header file for Dxe Core Debug Agent Library instance.
 
-  Copyright (c) 2010, Intel Corporation. All rights reserved.<BR>
+  Copyright (c) 2010 - 2013, Intel Corporation. All rights reserved.<BR>
   This program and the accompanying materials
   are licensed and made available under the terms and conditions of the BSD License
@@ -18 +18 @@
 #include <PiDxe.h>
 
+#include <Protocol/SerialIo.h>
+#include <Protocol/DevicePath.h>
+#include <Protocol/PiPcd.h>
+
 #include <Library/UefiBootServicesTableLib.h>
 #include <Library/UefiLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/MemoryAllocationLib.h>
 
 #include "DebugAgent.h"
 
+/**
+  Install EFI Serial IO protocol based on Debug Communication Library. 
+
+**/
+VOID
+InstallSerialIo (
+  VOID
+  );
+  
 #endif

trunk/src/VBox/Devices/EFI/Firmware/SourceLevelDebugPkg/Library/DebugAgent/DxeDebugAgentLib.inf

@@ -2 +2 @@
 #  Debug Agent library instance for Dxe Core and Dxe modules.
 #
-#  Copyright (c) 2010, Intel Corporation. All rights reserved.<BR>
+#  Copyright (c) 2010 - 2014, Intel Corporation. All rights reserved.<BR>
 #
 #  This program and the accompanying materials
@@ -17 +17 @@
   INF_VERSION                    = 0x00010005
   BASE_NAME                      = DxeDebugAgentLib
+  MODULE_UNI_FILE                = DxeDebugAgentLib.uni
   FILE_GUID                      = BA6BAD25-B814-4747-B0B0-0FBB61D40B90
   MODULE_TYPE                    = DXE_DRIVER
@@ -33 +34 @@
   DxeDebugAgent/DxeDebugAgentLib.c
   DxeDebugAgent/DxeDebugAgentLib.h
+  DxeDebugAgent/SerialIo.c
   DebugAgentCommon/DebugAgent.c
   DebugAgentCommon/DebugAgent.h
@@ -42 +44 @@
 [Sources.Ia32]
   DebugAgentCommon/Ia32/AsmFuncs.S     | GCC
-  DebugAgentCommon/Ia32/AsmFuncs.asm   | MSFT
+  DebugAgentCommon/Ia32/AsmFuncs.asm
   DebugAgentCommon/Ia32/ArchDebugSupport.h
   DebugAgentCommon/Ia32/ArchDebugSupport.c
-  DebugAgentCommon/Ia32/ArchReadGroupRegister.c
-  DebugAgentCommon/Ia32/ArchRegisters.h
   DebugAgentCommon/Ia32/DebugException.h
 
 [Sources.X64]
   DebugAgentCommon/X64/AsmFuncs.S      | GCC
-  DebugAgentCommon/X64/AsmFuncs.asm    | MSFT
+  DebugAgentCommon/X64/AsmFuncs.asm
   DebugAgentCommon/X64/ArchDebugSupport.h
   DebugAgentCommon/X64/ArchDebugSupport.c
-  DebugAgentCommon/X64/ArchReadGroupRegister.c
-  DebugAgentCommon/X64/ArchRegisters.h
   DebugAgentCommon/X64/DebugException.h
 
@@ -77 +75 @@
   MemoryAllocationLib
   LocalApicLib
+  TimerLib
+  PrintLib
+  PeCoffGetEntryPointLib
+  PeCoffExtraActionLib
+  MemoryAllocationLib
 
 [Guids]
-  gEfiDebugAgentGuid                            ## PRODUCES ## Configuration Table
-  gEfiDebugAgentGuid                            ## CONSUMES ## HOB
+  ## SOMETIMES_PRODUCES ## SystemTable
+  ## CONSUMES ## HOB
+  gEfiDebugAgentGuid
+  ## SOMETIMES_CONSUMES ## SystemTable
+  ## SOMETIMES_PRODUCES ## SystemTable
+  gEfiVectorHandoffTableGuid
+
+[Ppis]
+  gEfiVectorHandoffInfoPpiGuid                  ## UNDEFINED
+
+[Protocols]
+  gEfiSerialIoProtocolGuid                      ## SOMETIMES_PRODUCES
+  gEfiDevicePathProtocolGuid                    ## SOMETIMES_PRODUCES 
 
 [Pcd]
-  gEfiMdePkgTokenSpaceGuid.PcdFSBClock                                  ## CONSUMES
-  gEfiSourceLevelDebugPkgTokenSpaceGuid.PcdExceptionsIgnoredByDebugger  ## CONSUMES
+  gEfiMdePkgTokenSpaceGuid.PcdFSBClock                                  ## SOMETIMES_CONSUMES
+  gEfiSourceLevelDebugPkgTokenSpaceGuid.PcdExceptionsIgnoredByDebugger  ## SOMETIMES_CONSUMES
+  gEfiSourceLevelDebugPkgTokenSpaceGuid.PcdDebugPortHandleBufferSize    ## CONSUMES
 

trunk/src/VBox/Devices/EFI/Firmware/SourceLevelDebugPkg/Library/DebugAgent/SecPeiDebugAgent/SecPeiDebugAgentLib.c

@@ -2 +2 @@
   SEC Core Debug Agent Library instance implementition.
 
-  Copyright (c) 2010 - 2011, Intel Corporation. All rights reserved.<BR>
+  Copyright (c) 2010 - 2014, Intel Corporation. All rights reserved.<BR>
   This program and the accompanying materials
   are licensed and made available under the terms and conditions of the BSD License
@@ -15 +15 @@
 #include "SecPeiDebugAgentLib.h"
 
-CONST BOOLEAN                MultiProcessorDebugSupport = FALSE;
-
-/**
-  Get pointer to Mailbox from IDT entry before memory is ready.
+GLOBAL_REMOVE_IF_UNREFERENCED BOOLEAN  mSkipBreakpoint = FALSE;
+
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_PEI_VECTOR_HANDOFF_INFO_PPI mVectorHandoffInfoPpi = {
+  &mVectorHandoffInfoDebugAgent[0]
+};
+
+//
+// Ppis to be installed
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_PEI_PPI_DESCRIPTOR           mVectorHandoffInfoPpiList[] = {
+  {
+    (EFI_PEI_PPI_DESCRIPTOR_PPI | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST),
+    &gEfiVectorHandoffInfoPpiGuid,
+    &mVectorHandoffInfoPpi
+  }
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_PEI_NOTIFY_DESCRIPTOR mMemoryDiscoveredNotifyList[1] = {
+  {
+    (EFI_PEI_PPI_DESCRIPTOR_NOTIFY_CALLBACK | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST),
+    &gEfiPeiMemoryDiscoveredPpiGuid,
+    DebugAgentCallbackMemoryDiscoveredPpi
+  }
+};
+
+/**
+  Check if debug agent support multi-processor.
+
+  @retval TRUE    Multi-processor is supported.
+  @retval FALSE   Multi-processor is not supported.
+
+**/
+BOOLEAN
+MultiProcessorDebugSupport (
+  VOID
+  )
+{
+  return FALSE;
+}
+
+/**
+  Read the Attach/Break-in symbols from the debug port.
+
+  @param[in]  Handle         Pointer to Debug Port handle.
+  @param[out] BreakSymbol    Returned break symbol.
+
+  @retval EFI_SUCCESS        Read the symbol in BreakSymbol.
+  @retval EFI_NOT_FOUND      No read the break symbol.
+
+**/
+EFI_STATUS
+DebugReadBreakSymbol (
+  IN  DEBUG_PORT_HANDLE      Handle,
+  OUT UINT8                  *BreakSymbol
+  )
+{
+  EFI_STATUS                 Status;
+  DEBUG_PACKET_HEADER        DebugHeader;
+  UINT8                      *Data8;
+
+  *BreakSymbol = 0;
+  //
+  // If Debug Port buffer has data, read it till it was break symbol or Debug Port buffer empty.
+  //
+  Data8 = (UINT8 *) &DebugHeader;
+  while (TRUE) {
+    //
+    // If start symbol is not received
+    //
+    if (!DebugPortPollBuffer (Handle)) {
+      //
+      // If no data in Debug Port, exit
+      //
+      break;
+    }
+    //
+    // Try to read the start symbol
+    //
+    DebugPortReadBuffer (Handle, Data8, 1, 0);
+    if (*Data8 == DEBUG_STARTING_SYMBOL_ATTACH) {
+      *BreakSymbol = *Data8;
+      DebugAgentMsgPrint (DEBUG_AGENT_INFO, "Debug Timer attach symbol received %x", *BreakSymbol);
+      return EFI_SUCCESS;
+    }
+    if (*Data8 == DEBUG_STARTING_SYMBOL_NORMAL) {
+      Status = ReadRemainingBreakPacket (Handle, &DebugHeader);
+      if (Status == EFI_SUCCESS) {
+        *BreakSymbol = DebugHeader.Command;
+        DebugAgentMsgPrint (DEBUG_AGENT_INFO, "Debug Timer break symbol received %x", *BreakSymbol);
+        return EFI_SUCCESS;
+      }
+      if (Status == EFI_TIMEOUT) {
+        break;
+      }
+    }
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Get the pointer to location saved Mailbox pointer from IDT entry.
 
 **/
 VOID *
-GetMailboxPointerInIdtEntry (
+GetLocationSavedMailboxPointerInIdtEntry (
   VOID
   )
 {
-  IA32_IDT_GATE_DESCRIPTOR   *IdtEntry;
-  IA32_DESCRIPTOR            IdtDescriptor;
-  UINTN                      Mailbox;
-
-  AsmReadIdtr (&IdtDescriptor);
-  IdtEntry = (IA32_IDT_GATE_DESCRIPTOR *) IdtDescriptor.Base;
-
-  Mailbox = IdtEntry[DEBUG_MAILBOX_VECTOR].Bits.OffsetLow + (IdtEntry[DEBUG_MAILBOX_VECTOR].Bits.OffsetHigh << 16);
-  return (VOID *) Mailbox;
+  UINTN                     *MailboxLocation;
+
+  MailboxLocation = (UINTN *) GetExceptionHandlerInIdtEntry (DEBUG_MAILBOX_VECTOR);
+  //
+  // *MailboxLocation is the pointer to Mailbox
+  //
+  VerifyMailboxChecksum ((DEBUG_AGENT_MAILBOX *) (*MailboxLocation));
+  return MailboxLocation;
 }
 
@@ -40 +138 @@
   Set the pointer of Mailbox into IDT entry before memory is ready.
 
-  @param[in]  Mailbox       The pointer of Mailbox.
+  @param[in]  MailboxLocation    Pointer to location saved Mailbox pointer.
 
 **/
 VOID
-SetMailboxPointerInIdtEntry (
-  IN VOID                    *Mailbox
-  )
-{
-  IA32_IDT_GATE_DESCRIPTOR   *IdtEntry;
-  IA32_DESCRIPTOR            IdtDescriptor;
-
-  AsmReadIdtr (&IdtDescriptor);
-  IdtEntry = (IA32_IDT_GATE_DESCRIPTOR *) IdtDescriptor.Base;
-
-  IdtEntry[DEBUG_MAILBOX_VECTOR].Bits.OffsetLow  = (UINT16)(UINTN)Mailbox;
-  IdtEntry[DEBUG_MAILBOX_VECTOR].Bits.OffsetHigh = (UINT16)((UINTN)Mailbox >> 16);
-}
-
-/**
-  Get the pointer to Mailbox from IDT entry and build the Mailbox into GUIDed Hob
-  after memory is ready.
-
-  @return Pointer to Mailbox.
-
-**/
-DEBUG_AGENT_MAILBOX *
-BuildMailboxHob (
+SetLocationSavedMailboxPointerInIdtEntry (
+  IN VOID                  *MailboxLocation
+  )
+{
+  SetExceptionHandlerInIdtEntry (DEBUG_MAILBOX_VECTOR, MailboxLocation);
+}
+
+/**
+  Get the location of Mailbox pointer from the GUIDed HOB.
+
+  @return Pointer to the location saved Mailbox pointer.
+
+**/
+UINT64 *
+GetMailboxLocationFromHob (
   VOID
   )
 {
-  DEBUG_AGENT_MAILBOX       *Mailbox;
-
-  Mailbox = (DEBUG_AGENT_MAILBOX *) GetMailboxPointerInIdtEntry ();
-
-  return BuildGuidDataHob (
-           &gEfiDebugAgentGuid,
-           Mailbox,
-           sizeof (DEBUG_AGENT_MAILBOX)
-           );
+  EFI_HOB_GUID_TYPE        *GuidHob;
+
+  GuidHob = GetFirstGuidHob (&gEfiDebugAgentGuid);
+  if (GuidHob == NULL) {
+    return NULL;
+  }
+  return (UINT64 *) (GET_GUID_HOB_DATA(GuidHob));
 }
 
@@ -92 +180 @@
   )
 {
-  return (DEBUG_AGENT_MAILBOX *) GetMailboxPointerInIdtEntry ();
+  UINT64               DebugPortHandle;
+  UINT64               *MailboxLocationInIdt;
+  UINT64               *MailboxLocationInHob;
+  DEBUG_AGENT_MAILBOX  *Mailbox;
+
+  //
+  // Get mailbox from IDT entry firstly
+  //
+  MailboxLocationInIdt = GetLocationSavedMailboxPointerInIdtEntry ();
+  Mailbox = (DEBUG_AGENT_MAILBOX *)(UINTN)(*MailboxLocationInIdt);
+  //
+  // Cannot used GetDebugFlag() to get Debug Flag to avoid GetMailboxPointer() nested
+  //
+  if (Mailbox->DebugFlag.Bits.CheckMailboxInHob != 1 ||
+      Mailbox->DebugFlag.Bits.InitArch != DEBUG_ARCH_SYMBOL) {
+    //
+    // If mailbox was setup in SEC or the current CPU arch is different from the init arch
+    // Debug Agent initialized, return the mailbox from IDT entry directly.
+    // Otherwise, we need to check the mailbox location saved in GUIDed HOB further.
+    //
+    return Mailbox;
+  }
+
+  MailboxLocationInHob = GetMailboxLocationFromHob ();
+  //
+  // Compare mailbox in IDT enry with mailbox in HOB,
+  // need to fix mailbox location if HOB moved by PEI CORE
+  //
+  if (MailboxLocationInHob != MailboxLocationInIdt && MailboxLocationInHob != NULL) {
+    Mailbox = (DEBUG_AGENT_MAILBOX *)(UINTN)(*MailboxLocationInHob);
+    //
+    // Fix up Debug Port handler and save new mailbox in IDT entry
+    //
+    Mailbox = (DEBUG_AGENT_MAILBOX *)((UINTN)Mailbox + ((UINTN)(MailboxLocationInHob) - (UINTN)MailboxLocationInIdt));
+    DebugPortHandle = (UINT64)((UINTN)Mailbox->DebugPortHandle + ((UINTN)(MailboxLocationInHob) - (UINTN)MailboxLocationInIdt));
+    UpdateMailboxContent (Mailbox, DEBUG_MAILBOX_DEBUG_PORT_HANDLE_INDEX, DebugPortHandle);
+    *MailboxLocationInHob = (UINT64)(UINTN)Mailbox;
+    SetLocationSavedMailboxPointerInIdtEntry (MailboxLocationInHob);
+    //
+    // Clean CheckMailboxInHob flag
+    //
+    Mailbox->DebugFlag.Bits.CheckMailboxInHob = 0;
+    UpdateMailboxChecksum (Mailbox);
+  }
+
+  return Mailbox;
 }
 
@@ -107 +240 @@
 {
   DEBUG_AGENT_MAILBOX    *DebugAgentMailbox;
-  
-  DebugAgentMailbox = (DEBUG_AGENT_MAILBOX *)GetMailboxPointerInIdtEntry ();
+
+  DebugAgentMailbox = GetMailboxPointer ();
 
   return (DEBUG_PORT_HANDLE) (UINTN)(DebugAgentMailbox->DebugPortHandle);
@@ -114 +247 @@
 
 /**
-  Trigger one software interrupt to debug agent to handle it.
-
-  @param Signature       Software interrupt signature.
-
-**/
-VOID
-TriggerSoftInterrupt (
-  UINT32                 Signature
-  )
-{
-  UINTN                  Dr0;
-  UINTN                  Dr1;
-
-  //
-  // Save Debug Register State
-  //
-  Dr0 = AsmReadDr0 ();
-  Dr1 = AsmReadDr1 ();
-
-  //
-  // DR0 = Signature
-  //
-  AsmWriteDr0 (SOFT_INTERRUPT_SIGNATURE);
-  AsmWriteDr1 (Signature);
-
-  //
-  // Do INT3 to communicate with HOST side
-  //
-  CpuBreakpoint ();
-
-  //
-  // Restore Debug Register State only when Host didn't change it inside exception handler.
-  //   Dr registers can only be changed by setting the HW breakpoint.
-  //
-  AsmWriteDr0 (Dr0);
-  AsmWriteDr1 (Dr1);
-
+  Debug Agent provided notify callback function on Memory Discovered PPI.
+
+  @param[in] PeiServices      Indirect reference to the PEI Services Table.
+  @param[in] NotifyDescriptor Address of the notification descriptor data structure.
+  @param[in] Ppi              Address of the PPI that was installed.
+
+  @retval EFI_SUCCESS If the function completed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+DebugAgentCallbackMemoryDiscoveredPpi (
+  IN EFI_PEI_SERVICES                     **PeiServices,
+  IN EFI_PEI_NOTIFY_DESCRIPTOR            *NotifyDescriptor,
+  IN VOID                                 *Ppi
+  )
+{
+  EFI_STATUS                     Status;
+  DEBUG_AGENT_MAILBOX            *Mailbox;
+  BOOLEAN                        InterruptStatus;
+  EFI_PHYSICAL_ADDRESS           Address;
+  DEBUG_AGENT_MAILBOX            *NewMailbox;
+  UINT64                         *MailboxLocationInHob;
+
+  //
+  // Save and disable original interrupt status
+  //
+  InterruptStatus = SaveAndDisableInterrupts ();
+
+  //
+  // Allocate ACPI NVS memory for new Mailbox and Debug Port Handle buffer
+  //
+  Status = PeiServicesAllocatePages (
+             EfiACPIMemoryNVS,
+             EFI_SIZE_TO_PAGES (sizeof(DEBUG_AGENT_MAILBOX) + PcdGet16(PcdDebugPortHandleBufferSize)),
+             &Address
+             );
+  ASSERT_EFI_ERROR (Status);
+  NewMailbox = (DEBUG_AGENT_MAILBOX *) (UINTN) Address;
+  //
+  // Copy Mailbox and Debug Port Handle buffer to new location in ACPI NVS memory, because original Mailbox
+  // and Debug Port Handle buffer in the allocated pool that may be marked as free by DXE Core after DXE Core
+  // reallocates the HOB.
+  //
+  Mailbox = GetMailboxPointer ();
+  CopyMem (NewMailbox, Mailbox, sizeof (DEBUG_AGENT_MAILBOX));
+  CopyMem (NewMailbox + 1, (VOID *)(UINTN)Mailbox->DebugPortHandle, PcdGet16(PcdDebugPortHandleBufferSize));
+  //
+  // Update Mailbox Location pointer in GUIDed HOB and IDT entry with new one
+  //
+  MailboxLocationInHob = GetMailboxLocationFromHob ();
+  ASSERT (MailboxLocationInHob != NULL);
+  *MailboxLocationInHob = (UINT64)(UINTN)NewMailbox;
+  SetLocationSavedMailboxPointerInIdtEntry (MailboxLocationInHob);
+  //
+  // Update Debug Port Handle in new Mailbox
+  //
+  UpdateMailboxContent (NewMailbox, DEBUG_MAILBOX_DEBUG_PORT_HANDLE_INDEX, (UINT64)(UINTN)(NewMailbox + 1));
+  //
+  // Set physical memory ready flag
+  //
+  SetDebugFlag (DEBUG_AGENT_FLAG_MEMORY_READY, 1);
+
+  if (IsHostAttached ()) {
+    //
+    // Trigger one software interrupt to inform HOST
+    //
+    TriggerSoftInterrupt (MEMORY_READY_SIGNATURE);
+  }
+
+  //
+  // Restore interrupt state.
+  //
+  SetInterruptState (InterruptStatus);
+
+  return EFI_SUCCESS;
 }
 
@@ -193 +365 @@
 {
   DEBUG_AGENT_MAILBOX              *Mailbox;
+  DEBUG_AGENT_MAILBOX              *NewMailbox;
   DEBUG_AGENT_MAILBOX              MailboxInStack;
   DEBUG_AGENT_PHASE2_CONTEXT       Phase2Context;
   DEBUG_AGENT_CONTEXT_POSTMEM_SEC  *DebugAgentContext;
+  EFI_STATUS                       Status;
+  IA32_DESCRIPTOR                  *Ia32Idtr;
+  IA32_IDT_ENTRY                   *Ia32IdtEntry;
+  UINT64                           DebugPortHandle;
+  UINT64                           MailboxLocation;
+  UINT64                           *MailboxLocationPointer;
+  EFI_PHYSICAL_ADDRESS             Address;
 
   DisableInterrupts ();
@@ -205 +385 @@
     InitializeDebugIdt ();
 
+    MailboxLocation = (UINT64)(UINTN)&MailboxInStack;
     Mailbox = &MailboxInStack;
     ZeroMem ((VOID *) Mailbox, sizeof (DEBUG_AGENT_MAILBOX));
-
     //
     // Get and save debug port handle and set the length of memory block.
     //
-    SetMailboxPointerInIdtEntry ((VOID *) Mailbox);
+    SetLocationSavedMailboxPointerInIdtEntry (&MailboxLocation);
+    //
+    // Force error message could be printed during the first shakehand between Target/HOST.
+    //
+    SetDebugFlag (DEBUG_AGENT_FLAG_PRINT_ERROR_LEVEL, DEBUG_AGENT_ERROR);
+    //
+    // Save init arch type when debug agent initialized
+    //
+    SetDebugFlag (DEBUG_AGENT_FLAG_INIT_ARCH, DEBUG_ARCH_SYMBOL);
 
     InitializeDebugTimer ();
 
+    Phase2Context.InitFlag = InitFlag;
     Phase2Context.Context  = Context;
     Phase2Context.Function = Function;
     DebugPortInitialize ((VOID *) &Phase2Context, InitializeDebugAgentPhase2);
-
     //
     // If reaches here, it means Debug Port initialization failed.
@@ -227 +415 @@
 
   case DEBUG_AGENT_INIT_POSTMEM_SEC:
-
+    Mailbox = GetMailboxPointer ();
     //
     // Memory has been ready
     //
-    if (IsHostConnected()) {
+    SetDebugFlag (DEBUG_AGENT_FLAG_MEMORY_READY, 1);
+    if (IsHostAttached ()) {
       //
       // Trigger one software interrupt to inform HOST
@@ -237 +426 @@
       TriggerSoftInterrupt (MEMORY_READY_SIGNATURE);
     }
-
+    //
+    // Install Vector Handoff Info PPI to persist vectors used by Debug Agent
+    //
+    Status = PeiServicesInstallPpi (&mVectorHandoffInfoPpiList[0]);
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "DebugAgent: Failed to install Vector Handoff Info PPI!\n"));
+      CpuDeadLoop ();
+    }
+    //
+    // Fix up Debug Port handle address and mailbox address
+    //
     DebugAgentContext = (DEBUG_AGENT_CONTEXT_POSTMEM_SEC *) Context;
-
-    Mailbox = (DEBUG_AGENT_MAILBOX *) GetMailboxPointerInIdtEntry ();
-    Mailbox->DebugPortHandle = Mailbox->DebugPortHandle + DebugAgentContext->StackMigrateOffset;
-
-    Mailbox = BuildMailboxHob ();
-    Mailbox = (DEBUG_AGENT_MAILBOX *) ((UINTN) Mailbox + DebugAgentContext->HeapMigrateOffset);
-
-    SetMailboxPointerInIdtEntry ((VOID *) Mailbox);
-
-    EnableInterrupts ();
-
+    if (DebugAgentContext != NULL) {
+      DebugPortHandle = (UINT64)(UINT32)(Mailbox->DebugPortHandle + DebugAgentContext->StackMigrateOffset);
+      UpdateMailboxContent (Mailbox, DEBUG_MAILBOX_DEBUG_PORT_HANDLE_INDEX, DebugPortHandle);
+      Mailbox = (DEBUG_AGENT_MAILBOX *) ((UINTN) Mailbox + DebugAgentContext->StackMigrateOffset);
+      MailboxLocation = (UINT64)(UINTN)Mailbox;
+      //
+      // Build mailbox location in HOB and fix-up its address
+      //
+      MailboxLocationPointer = BuildGuidDataHob (
+                                 &gEfiDebugAgentGuid,
+                                 &MailboxLocation,
+                                 sizeof (UINT64)
+                                 );
+      MailboxLocationPointer = (UINT64 *) ((UINTN) MailboxLocationPointer + DebugAgentContext->HeapMigrateOffset);
+    } else {
+      //
+      // DebugAgentContext is NULL. Then, Mailbox can directly be copied into memory.
+      // Allocate ACPI NVS memory for new Mailbox and Debug Port Handle buffer
+      //
+      Status = PeiServicesAllocatePages (
+                 EfiACPIMemoryNVS,
+                 EFI_SIZE_TO_PAGES (sizeof(DEBUG_AGENT_MAILBOX) + PcdGet16(PcdDebugPortHandleBufferSize)),
+                 &Address
+                 );
+      if (EFI_ERROR (Status)) {
+        DEBUG ((EFI_D_ERROR, "DebugAgent: Failed to allocate pages!\n"));
+        CpuDeadLoop ();
+      }
+      NewMailbox = (DEBUG_AGENT_MAILBOX *) (UINTN) Address;
+      //
+      // Copy Mailbox and Debug Port Handle buffer to new location in ACPI NVS memory, because original Mailbox
+      // and Debug Port Handle buffer in the allocated pool that may be marked as free by DXE Core after DXE Core
+      // reallocates the HOB.
+      //
+      CopyMem (NewMailbox, Mailbox, sizeof (DEBUG_AGENT_MAILBOX));
+      CopyMem (NewMailbox + 1, (VOID *)(UINTN)Mailbox->DebugPortHandle, PcdGet16(PcdDebugPortHandleBufferSize));
+      UpdateMailboxContent (NewMailbox, DEBUG_MAILBOX_DEBUG_PORT_HANDLE_INDEX, (UINT64)(UINTN)(NewMailbox + 1));
+      MailboxLocation = (UINT64)(UINTN)NewMailbox;
+      //
+      // Build mailbox location in HOB
+      //
+      MailboxLocationPointer = BuildGuidDataHob (
+                                 &gEfiDebugAgentGuid,
+                                 &MailboxLocation,
+                                 sizeof (UINT64)
+                                 );
+    }
+    //
+    // Update IDT entry to save the location saved mailbox pointer
+    //
+    SetLocationSavedMailboxPointerInIdtEntry (MailboxLocationPointer);
     break;
 
+  case DEBUG_AGENT_INIT_PEI:
+    if (Context == NULL) {
+      DEBUG ((EFI_D_ERROR, "DebugAgent: Input parameter Context cannot be NULL!\n"));
+      CpuDeadLoop ();
+    }
+    //
+    // Check if Debug Agent has initialized before
+    //
+    if (IsDebugAgentInitialzed()) {
+      DEBUG ((EFI_D_WARN, "Debug Agent: It has already initialized in SEC Core!\n"));
+      break;
+    }
+    //
+    // Install Vector Handoff Info PPI to persist vectors used by Debug Agent
+    //
+    Status = PeiServicesInstallPpi (&mVectorHandoffInfoPpiList[0]);
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "DebugAgent: Failed to install Vector Handoff Info PPI!\n"));
+      CpuDeadLoop ();
+    }
+    //
+    // Set up IDT entries
+    //
+    InitializeDebugIdt ();
+    //
+    // Build mailbox in HOB and setup Mailbox Set In Pei flag
+    //
+    Mailbox = AllocateZeroPool (sizeof (DEBUG_AGENT_MAILBOX));
+    MailboxLocation = (UINT64)(UINTN)Mailbox;
+    MailboxLocationPointer = BuildGuidDataHob (
+                               &gEfiDebugAgentGuid,
+                               &MailboxLocation,
+                               sizeof (UINT64)
+                               );
+
+    InitializeDebugTimer ();
+    //
+    // Update IDT entry to save the location pointer saved mailbox pointer
+    //
+    SetLocationSavedMailboxPointerInIdtEntry (MailboxLocationPointer);
+    //
+    // Save init arch type when debug agent initialized
+    //
+    SetDebugFlag (DEBUG_AGENT_FLAG_INIT_ARCH, DEBUG_ARCH_SYMBOL);
+    //
+    // Register for a callback once memory has been initialized.
+    // If memery has been ready, the callback funtion will be invoked immediately
+    //
+    Status = PeiServicesNotifyPpi (&mMemoryDiscoveredNotifyList[0]);
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "DebugAgent: Failed to register memory discovered callback function!\n"));
+      CpuDeadLoop ();
+    }
+    //
+    // Set HOB check flag if memory has not been ready yet
+    //
+    if (GetDebugFlag (DEBUG_AGENT_FLAG_MEMORY_READY) == 0) {
+      SetDebugFlag (DEBUG_AGENT_FLAG_CHECK_MAILBOX_IN_HOB, 1);
+    }
+
+    Phase2Context.InitFlag = InitFlag;
+    Phase2Context.Context  = Context;
+    Phase2Context.Function = Function;
+    DebugPortInitialize ((VOID *) &Phase2Context, InitializeDebugAgentPhase2);
+
+    FindAndReportModuleImageInfo (4);
+
+    break;
+
+  case DEBUG_AGENT_INIT_THUNK_PEI_IA32TOX64:
+    if (Context == NULL) {
+      DEBUG ((EFI_D_ERROR, "DebugAgent: Input parameter Context cannot be NULL!\n"));
+      CpuDeadLoop ();
+    } else {
+      Ia32Idtr =  (IA32_DESCRIPTOR *) Context;
+      Ia32IdtEntry = (IA32_IDT_ENTRY *)(Ia32Idtr->Base);
+      MailboxLocationPointer = (UINT64 *) (UINTN) (Ia32IdtEntry[DEBUG_MAILBOX_VECTOR].Bits.OffsetLow +
+                                                (Ia32IdtEntry[DEBUG_MAILBOX_VECTOR].Bits.OffsetHigh << 16));
+      Mailbox = (DEBUG_AGENT_MAILBOX *) (UINTN)(*MailboxLocationPointer);
+      //
+      // Mailbox should valid and setup before executing thunk code
+      //
+      VerifyMailboxChecksum (Mailbox);
+
+      DebugPortHandle = (UINT64) (UINTN)DebugPortInitialize ((VOID *)(UINTN)Mailbox->DebugPortHandle, NULL);
+      UpdateMailboxContent (Mailbox, DEBUG_MAILBOX_DEBUG_PORT_HANDLE_INDEX, DebugPortHandle);
+      //
+      // Set up IDT entries
+      //
+      InitializeDebugIdt ();
+      //
+      // Update IDT entry to save location pointer saved the mailbox pointer
+      //
+      SetLocationSavedMailboxPointerInIdtEntry (MailboxLocationPointer);
+
+      FindAndReportModuleImageInfo (4);
+    }
+    break;
+
   default:
-
-    //
-    // Only DEBUG_AGENT_INIT_PREMEM_SEC and DEBUG_AGENT_INIT_POSTMEM_SEC are allowed for this 
+    //
+    // Only DEBUG_AGENT_INIT_PREMEM_SEC and DEBUG_AGENT_INIT_POSTMEM_SEC are allowed for this
     // Debug Agent library instance.
     //
@@ -261 +598 @@
     CpuDeadLoop ();
     break;
-
-  }
+  }
+
+  //
+  // Enable CPU interrupts so debug timer interrupts can be delivered
+  //
+  EnableInterrupts ();
 
   //
@@ -269 +610 @@
   if (Function != NULL) {
     Function (Context);
+  }
+  //
+  // Set return status for DEBUG_AGENT_INIT_PEI
+  //
+  if (InitFlag == DEBUG_AGENT_INIT_PEI && Context != NULL) {
+    *(EFI_STATUS *)Context = EFI_SUCCESS;
   }
 }
@@ -289 +636 @@
 {
   DEBUG_AGENT_PHASE2_CONTEXT *Phase2Context;
+  UINT64                     *MailboxLocation;
   DEBUG_AGENT_MAILBOX        *Mailbox;
   EFI_SEC_PEI_HAND_OFF       *SecCoreData;
-
-  Mailbox = GetMailboxPointerInIdtEntry ();
-  Mailbox->DebugPortHandle = (UINT64) (UINTN)DebugPortHandle;
+  UINT16                     BufferSize;
+  UINT64                     NewDebugPortHandle;
+
+  Phase2Context = (DEBUG_AGENT_PHASE2_CONTEXT *) Context;
+  MailboxLocation = GetLocationSavedMailboxPointerInIdtEntry ();
+  Mailbox = (DEBUG_AGENT_MAILBOX *)(UINTN)(*MailboxLocation);
+  BufferSize = PcdGet16(PcdDebugPortHandleBufferSize);
+  if (Phase2Context->InitFlag == DEBUG_AGENT_INIT_PEI) {
+    NewDebugPortHandle = (UINT64)(UINTN)AllocateCopyPool (BufferSize, DebugPortHandle);
+  } else {
+    NewDebugPortHandle = (UINT64)(UINTN)DebugPortHandle;
+  }
+  UpdateMailboxContent (Mailbox, DEBUG_MAILBOX_DEBUG_PORT_HANDLE_INDEX, NewDebugPortHandle);
 
   //
@@ -300 +658 @@
   TriggerSoftInterrupt (SYSTEM_RESET_SIGNATURE);
 
-  //
-  // If Temporary RAM region is below 128 MB, then send message to 
-  // host to disable low memory filtering.
-  //
-  Phase2Context = (DEBUG_AGENT_PHASE2_CONTEXT *) Context;
-  SecCoreData = (EFI_SEC_PEI_HAND_OFF *)Phase2Context->Context;
-  if ((UINTN)SecCoreData->TemporaryRamBase < BASE_128MB && IsHostConnected ()) {
-    TriggerSoftInterrupt (MEMORY_READY_SIGNATURE);
-  }
-
-  //
-  // Enable CPU interrupts so debug timer interrupts can be delivered
-  //
-  EnableInterrupts ();
-
-  //
-  // Call continuation function if it is not NULL.
-  //
-  if (Phase2Context->Function != NULL) {
+  if (Phase2Context->InitFlag == DEBUG_AGENT_INIT_PREMEM_SEC) {
+    //
+    // If Temporary RAM region is below 128 MB, then send message to
+    // host to disable low memory filtering.
+    //
+    SecCoreData = (EFI_SEC_PEI_HAND_OFF *)Phase2Context->Context;
+    if ((UINTN)SecCoreData->TemporaryRamBase < BASE_128MB && IsHostAttached ()) {
+      SetDebugFlag (DEBUG_AGENT_FLAG_MEMORY_READY, 1);
+      TriggerSoftInterrupt (MEMORY_READY_SIGNATURE);
+    }
+    //
+    // Enable CPU interrupts so debug timer interrupts can be delivered
+    //
+    EnableInterrupts ();
+    //
+    // Call continuation function if it is not NULL.
+    //
     Phase2Context->Function (Phase2Context->Context);
   }

trunk/src/VBox/Devices/EFI/Firmware/SourceLevelDebugPkg/Library/DebugAgent/SecPeiDebugAgent/SecPeiDebugAgentLib.h

@@ -2 +2 @@
   Header file for Sec Core Debug Agent Library instance.
 
-  Copyright (c) 2010, Intel Corporation. All rights reserved.<BR>
+  Copyright (c) 2010 - 2013, Intel Corporation. All rights reserved.<BR>
   This program and the accompanying materials
   are licensed and made available under the terms and conditions of the BSD License
@@ -17 +17 @@
 
 #include <PiPei.h>
-
+#include <Ppi/MemoryDiscovered.h>
+#include <Library/PeiServicesLib.h>
+#include <Library/MemoryAllocationLib.h>
 #include "DebugAgent.h"
 
 typedef struct {
+  UINT32                  InitFlag;
   VOID                    *Context;
   DEBUG_AGENT_CONTINUE    Function;
 } DEBUG_AGENT_PHASE2_CONTEXT;
 
+/**
+  Caller provided function to be invoked at the end of DebugPortInitialize().
+
+  Refer to the descrption for DebugPortInitialize() for more details.
+
+  @param[in] Context           The first input argument of DebugPortInitialize().
+  @param[in] DebugPortHandle   Debug port handle created by Debug Communication Libary.
+
+**/
+VOID
+EFIAPI
+InitializeDebugAgentPhase2 (
+  IN VOID                  *Context,
+  IN DEBUG_PORT_HANDLE     DebugPortHandle
+  );
+
+/**
+  Debug Agent provided notify callback function on Memory Discovered PPI.
+
+  @param[in] PeiServices      Indirect reference to the PEI Services Table.
+  @param[in] NotifyDescriptor Address of the notification descriptor data structure.
+  @param[in] Ppi              Address of the PPI that was installed.
+
+  @retval EFI_SUCCESS If the function completed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+DebugAgentCallbackMemoryDiscoveredPpi (
+  IN EFI_PEI_SERVICES                     **PeiServices,
+  IN EFI_PEI_NOTIFY_DESCRIPTOR            *NotifyDescriptor,
+  IN VOID                                 *Ppi
+  );
+
 #endif
 

trunk/src/VBox/Devices/EFI/Firmware/SourceLevelDebugPkg/Library/DebugAgent/SecPeiDebugAgentLib.inf

@@ -2 +2 @@
 #  Debug Agent library instance for SEC Core and PEI modules.
 #
-#  Copyright (c) 2010 - 2011, Intel Corporation. All rights reserved.<BR>
+#  Copyright (c) 2010 - 2014, Intel Corporation. All rights reserved.<BR>
 #
 #  This program and the accompanying materials
@@ -17 +17 @@
   INF_VERSION                    = 0x00010005
   BASE_NAME                      = SecPeiDebugAgentLib
+  MODULE_UNI_FILE                = SecPeiDebugAgentLib.uni
   FILE_GUID                      = 508B7D59-CD4E-4a6b-A45B-6D3B2D90111E
   MODULE_TYPE                    = PEIM
@@ -40 +41 @@
 [Sources.Ia32]
   DebugAgentCommon/Ia32/AsmFuncs.S     | GCC
-  DebugAgentCommon/Ia32/AsmFuncs.asm   | MSFT
+  DebugAgentCommon/Ia32/AsmFuncs.asm
   DebugAgentCommon/Ia32/ArchDebugSupport.h
   DebugAgentCommon/Ia32/ArchDebugSupport.c
-  DebugAgentCommon/Ia32/ArchReadGroupRegister.c
-  DebugAgentCommon/Ia32/ArchRegisters.h
   DebugAgentCommon/Ia32/DebugException.h
 
 [Sources.X64]
   DebugAgentCommon/X64/AsmFuncs.S      | GCC
-  DebugAgentCommon/X64/AsmFuncs.asm    | MSFT
+  DebugAgentCommon/X64/AsmFuncs.asm
   DebugAgentCommon/X64/ArchDebugSupport.h
   DebugAgentCommon/X64/ArchDebugSupport.c
-  DebugAgentCommon/X64/ArchReadGroupRegister.c
-  DebugAgentCommon/X64/ArchRegisters.h
   DebugAgentCommon/X64/DebugException.h
 
@@ -73 +70 @@
   LocalApicLib
   DebugLib
+  TimerLib
+  PrintLib
+  PeiServicesLib
+  MemoryAllocationLib
+  PeCoffGetEntryPointLib
+  PeCoffExtraActionLib
+
+[Ppis]
+  gEfiPeiMemoryDiscoveredPpiGuid                ## NOTIFY
+  gEfiVectorHandoffInfoPpiGuid                  ## PRODUCES
 
 [Guids]
-  gEfiDebugAgentGuid                            ## PRODUCES ## HOB
+  ## PRODUCES ## HOB
+  ## CONSUMES ## HOB
+  gEfiDebugAgentGuid
 
 [Pcd]
-  gEfiMdePkgTokenSpaceGuid.PcdFSBClock                                  ## CONSUMES
-  gEfiSourceLevelDebugPkgTokenSpaceGuid.PcdExceptionsIgnoredByDebugger  ## CONSUMES
+  gEfiMdePkgTokenSpaceGuid.PcdFSBClock                                  ## SOMETIMES_CONSUMES
+  gEfiSourceLevelDebugPkgTokenSpaceGuid.PcdExceptionsIgnoredByDebugger  ## SOMETIMES_CONSUMES
+  gEfiSourceLevelDebugPkgTokenSpaceGuid.PcdDebugPortHandleBufferSize    ## SOMETIMES_CONSUMES
 

trunk/src/VBox/Devices/EFI/Firmware/SourceLevelDebugPkg/Library/DebugAgent/SmmDebugAgent/SmmDebugAgentLib.c

@@ -2 +2 @@
   Debug Agent library implementition.
 
-  Copyright (c) 2010, Intel Corporation. All rights reserved.<BR>
+  Copyright (c) 2010 - 2013, Intel Corporation. All rights reserved.<BR>
   This program and the accompanying materials
   are licensed and made available under the terms and conditions of the BSD License
@@ -18 +18 @@
 DEBUG_AGENT_MAILBOX         mLocalMailbox;
 UINTN                       mSavedDebugRegisters[6];
-CONST BOOLEAN               MultiProcessorDebugSupport = FALSE;
-
+IA32_IDT_GATE_DESCRIPTOR    mIdtEntryTable[33];
+BOOLEAN                     mSkipBreakpoint = FALSE;
+
+CHAR8 mWarningMsgIgnoreSmmEntryBreak[] = "Ignore smmentrybreak setting for SMI issued during DXE debugging!\r\n";
+
+/**
+  Check if debug agent support multi-processor.
+
+  @retval TRUE    Multi-processor is supported.
+  @retval FALSE   Multi-processor is not supported.
+
+**/
+BOOLEAN
+MultiProcessorDebugSupport (
+  VOID
+  )
+{
+  return FALSE;
+}
+
+/**
+  Read the Attach/Break-in symbols from the debug port.
+
+  @param[in]  Handle         Pointer to Debug Port handle.
+  @param[out] BreakSymbol    Returned break symbol.
+
+  @retval EFI_SUCCESS        Read the symbol in BreakSymbol.
+  @retval EFI_NOT_FOUND      No read the break symbol.
+
+**/
+EFI_STATUS
+DebugReadBreakSymbol (
+  IN  DEBUG_PORT_HANDLE      Handle,
+  OUT UINT8                  *BreakSymbol
+  )
+{
+  //
+  // Smm instance has no debug timer to poll break symbol.
+  //
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Get the pointer to Mailbox from the GUIDed HOB.
+
+  @return Pointer to Mailbox.
+
+**/
+DEBUG_AGENT_MAILBOX *
+GetMailboxFromHob (
+  VOID
+  )
+{
+  EFI_HOB_GUID_TYPE        *GuidHob;
+  UINT64                   *MailboxLocation;  
+  DEBUG_AGENT_MAILBOX      *Mailbox;
+
+  GuidHob = GetFirstGuidHob (&gEfiDebugAgentGuid);
+  if (GuidHob == NULL) {
+    return NULL;
+  }
+  MailboxLocation = (UINT64 *) (GET_GUID_HOB_DATA(GuidHob));
+  Mailbox = (DEBUG_AGENT_MAILBOX *)(UINTN)(*MailboxLocation);
+  VerifyMailboxChecksum (Mailbox);
+  
+  return Mailbox;
+}
 
 /**
@@ -32 +97 @@
   )
 {
+  VerifyMailboxChecksum (mMailboxPointer);
   return mMailboxPointer;
 }
@@ -46 +112 @@
   )
 {
-  return (DEBUG_PORT_HANDLE) (UINTN)(mMailboxPointer->DebugPortHandle);
+  return (DEBUG_PORT_HANDLE) (UINTN)(GetMailboxPointer()->DebugPortHandle);
 }
 
@@ -114 +180 @@
   )
 {
-  EFI_STATUS               Status;
-  UINT64                   DebugPortHandle;
+  EFI_STATUS                    Status;
+  UINT64                        DebugPortHandle;
+  IA32_IDT_GATE_DESCRIPTOR      IdtEntry[33];
+  IA32_DESCRIPTOR               IdtDescriptor;
+  IA32_DESCRIPTOR               *Ia32Idtr;
+  IA32_IDT_ENTRY                *Ia32IdtEntry;
+  IA32_DESCRIPTOR               Idtr;
+  UINT16                        IdtEntryCount;
+  DEBUG_AGENT_MAILBOX           *Mailbox;
+  UINT64                        *MailboxLocation;
 
   switch (InitFlag) {
   case DEBUG_AGENT_INIT_SMM:
-    Status = EfiGetSystemConfigurationTable (&gEfiDebugAgentGuid, (VOID **) &mMailboxPointer);
-    if (EFI_ERROR (Status) || mMailboxPointer == NULL) {
-      ZeroMem (&mLocalMailbox, sizeof (DEBUG_AGENT_MAILBOX));
-      mMailboxPointer = &mLocalMailbox;
-    }
+    //
+    // Install configuration table for persisted vector handoff info
+    //
+    Status = gSmst->SmmInstallConfigurationTable (
+                      gSmst,
+                      &gEfiVectorHandoffTableGuid,
+                      (VOID *) &mVectorHandoffInfoDebugAgent[0],
+                      sizeof (EFI_VECTOR_HANDOFF_INFO) * mVectorHandoffInfoCount
+                      );
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "DebugAgent: Cannot install configuration table for persisted vector handoff info!\n"));
+      CpuDeadLoop ();
+    }
+    //
+    // Check if Debug Agent initialized in DXE phase
+    //
+    Status = EfiGetSystemConfigurationTable (&gEfiDebugAgentGuid, (VOID **) &Mailbox);
+    if (Status == EFI_SUCCESS && Mailbox != NULL) {
+      VerifyMailboxChecksum (Mailbox);
+      mMailboxPointer = Mailbox;
+      break;
+    }
+    //
+    // Check if Debug Agent initialized in SEC/PEI phase
+    //
+    Mailbox = GetMailboxFromHob (); 
+    if (Mailbox != NULL) {
+      mMailboxPointer = Mailbox;
+      break;
+    }
+    //
+    // Debug Agent was not initialized before, use the local mailbox.
+    //
+    ZeroMem (&mLocalMailbox, sizeof (DEBUG_AGENT_MAILBOX));
+    Mailbox = &mLocalMailbox;
+    //
+    // Save original IDT entries
+    //
+    AsmReadIdtr (&IdtDescriptor);      
+    CopyMem (&IdtEntry, (VOID *)IdtDescriptor.Base, 33 * sizeof(IA32_IDT_GATE_DESCRIPTOR));
+    //
+    // Initialized Debug Agent
+    //
+    InitializeDebugIdt ();
+    DebugPortHandle = (UINT64) (UINTN)DebugPortInitialize ((DEBUG_PORT_HANDLE) (UINTN)Mailbox->DebugPortHandle, NULL);
+    UpdateMailboxContent (Mailbox, DEBUG_MAILBOX_DEBUG_PORT_HANDLE_INDEX, DebugPortHandle);
+    mMailboxPointer = Mailbox;
+    //
+    // Trigger one software interrupt to inform HOST
+    //
+    TriggerSoftInterrupt (SYSTEM_RESET_SIGNATURE);
+
+    SetDebugFlag (DEBUG_AGENT_FLAG_MEMORY_READY, 1);
+    //
+    // Memory has been ready
+    //
+    if (IsHostAttached ()) {
+      //
+      // Trigger one software interrupt to inform HOST
+      //
+      TriggerSoftInterrupt (MEMORY_READY_SIGNATURE);
+    }
+    //
+    // Find and report PE/COFF image info to HOST
+    //  
+    FindAndReportModuleImageInfo (SIZE_4KB);
+    //
+    // Restore saved IDT entries
+    //     
+    CopyMem ((VOID *)IdtDescriptor.Base, &IdtEntry, 33 * sizeof(IA32_IDT_GATE_DESCRIPTOR));
 
     break;
@@ -131 +270 @@
     InitializeDebugIdt ();
 
-    if (mMailboxPointer != NULL) {
-      //
-      // Initialize debug communication port
-      //
-      DebugPortHandle = (UINT64) (UINTN)DebugPortInitialize ((DEBUG_PORT_HANDLE) (UINTN)mMailboxPointer->DebugPortHandle, NULL);
-      mMailboxPointer->DebugPortHandle = DebugPortHandle;
-
-      if (mMailboxPointer->DebugFlag.Bits.BreakOnNextSmi == 1) {
+    Mailbox = GetMailboxPointer ();
+    if (GetDebugFlag (DEBUG_AGENT_FLAG_AGENT_IN_PROGRESS) == 1) {
+      //
+      // If Debug Agent has been communicaton state with HOST, we need skip
+      // any break points set in SMM, set Skip Breakpoint flag
+      //
+      mSkipBreakpoint = TRUE;
+    }
+    if (GetDebugFlag (DEBUG_AGENT_FLAG_BREAK_ON_NEXT_SMI) == 1) {
+      if (mSkipBreakpoint) {
+        //
+        // Print warning message if ignore smm entry break
+        //
+        DebugPortWriteBuffer ((DEBUG_PORT_HANDLE) (UINTN)Mailbox->DebugPortHandle,
+                               (UINT8 *)mWarningMsgIgnoreSmmEntryBreak,
+                               AsciiStrLen (mWarningMsgIgnoreSmmEntryBreak)
+                               );
+      } else {
         //
         // If SMM entry break is set, SMM code will be break at here.
@@ -148 +297 @@
 
   case DEBUG_AGENT_INIT_EXIT_SMI:
+    Mailbox = GetMailboxPointer ();
+    //
+    // Clear Skip Breakpoint flag
+    //
+    mSkipBreakpoint = FALSE;
     RestoreDebugRegister ();
     break;
+
+  case DEBUG_AGENT_INIT_THUNK_PEI_IA32TOX64:
+    if (Context == NULL) {
+      DEBUG ((EFI_D_ERROR, "DebugAgent: Input parameter Context cannot be NULL!\n"));
+      CpuDeadLoop ();
+    } else {
+      Ia32Idtr =  (IA32_DESCRIPTOR *) Context;
+      Ia32IdtEntry = (IA32_IDT_ENTRY *)(Ia32Idtr->Base);
+      MailboxLocation = (UINT64 *) (UINTN) (Ia32IdtEntry[DEBUG_MAILBOX_VECTOR].Bits.OffsetLow + 
+                                           (Ia32IdtEntry[DEBUG_MAILBOX_VECTOR].Bits.OffsetHigh << 16));
+      mMailboxPointer = (DEBUG_AGENT_MAILBOX *)(UINTN)(*MailboxLocation);
+      VerifyMailboxChecksum (mMailboxPointer);
+      //
+      // Get original IDT address and size.
+      //
+      AsmReadIdtr ((IA32_DESCRIPTOR *) &Idtr);
+      IdtEntryCount = (UINT16) ((Idtr.Limit + 1) / sizeof (IA32_IDT_GATE_DESCRIPTOR));
+      if (IdtEntryCount < 33) {
+        Idtr.Limit = (UINT16) (sizeof (IA32_IDT_GATE_DESCRIPTOR) * 33 - 1);
+        Idtr.Base  = (UINTN) &mIdtEntryTable;
+        ZeroMem (&mIdtEntryTable, Idtr.Limit + 1);
+        AsmWriteIdtr ((IA32_DESCRIPTOR *) &Idtr);
+      }
+
+      InitializeDebugIdt ();
+      //
+      // Initialize Debug Timer hardware and enable interrupt.
+      //
+      InitializeDebugTimer ();
+      EnableInterrupts ();
+
+      FindAndReportModuleImageInfo (SIZE_4KB);
+    }
+    break;
+
+  default:
+    //
+    // Only DEBUG_AGENT_INIT_PREMEM_SEC and DEBUG_AGENT_INIT_POSTMEM_SEC are allowed for this 
+    // Debug Agent library instance.
+    //
+    DEBUG ((EFI_D_ERROR, "Debug Agent: The InitFlag value is not allowed!\n"));
+    CpuDeadLoop ();
+    break;    
   }
 }

trunk/src/VBox/Devices/EFI/Firmware/SourceLevelDebugPkg/Library/DebugAgent/SmmDebugAgent/SmmDebugAgentLib.h

@@ -2 +2 @@
   Header file for Smm Debug Agent Library instance.
 
-  Copyright (c) 2010, Intel Corporation. All rights reserved.<BR>
+  Copyright (c) 2010 - 2013, Intel Corporation. All rights reserved.<BR>
   This program and the accompanying materials
   are licensed and made available under the terms and conditions of the BSD License
@@ -19 +19 @@
 
 #include <Library/UefiLib.h>
+#include <Library/SmmServicesTableLib.h>
 
 #include "DebugAgent.h"

trunk/src/VBox/Devices/EFI/Firmware/SourceLevelDebugPkg/Library/DebugAgent/SmmDebugAgentLib.inf

@@ -2 +2 @@
 #  Debug Agent library instance for SMM modules.
 #
-#  Copyright (c) 2010, Intel Corporation. All rights reserved.<BR>
+#  Copyright (c) 2010 - 2015, Intel Corporation. All rights reserved.<BR>
 #
 #  This program and the accompanying materials
@@ -16 +16 @@
 [Defines]
   INF_VERSION                    = 0x00010005
-  BASE_NAME                      = SecDebugAgentLib
+  BASE_NAME                      = SmmDebugAgentLib
+  MODULE_UNI_FILE                = SmmDebugAgentLib.uni
   FILE_GUID                      = CB07D74C-598F-4268-A5D1-644FB4A481E8
   MODULE_TYPE                    = DXE_SMM_DRIVER
@@ -40 +41 @@
 [Sources.Ia32]
   DebugAgentCommon/Ia32/AsmFuncs.S     | GCC
-  DebugAgentCommon/Ia32/AsmFuncs.asm   | MSFT
+  DebugAgentCommon/Ia32/AsmFuncs.asm
   DebugAgentCommon/Ia32/ArchDebugSupport.h
   DebugAgentCommon/Ia32/ArchDebugSupport.c
-  DebugAgentCommon/Ia32/ArchReadGroupRegister.c
-  DebugAgentCommon/Ia32/ArchRegisters.h
   DebugAgentCommon/Ia32/DebugException.h
 
 [Sources.X64]
   DebugAgentCommon/X64/AsmFuncs.S      | GCC
-  DebugAgentCommon/X64/AsmFuncs.asm    | MSFT
+  DebugAgentCommon/X64/AsmFuncs.asm
   DebugAgentCommon/X64/ArchDebugSupport.h
   DebugAgentCommon/X64/ArchDebugSupport.c
-  DebugAgentCommon/X64/ArchReadGroupRegister.c
-  DebugAgentCommon/X64/ArchRegisters.h
   DebugAgentCommon/X64/DebugException.h
 
@@ -72 +69 @@
   SynchronizationLib
   LocalApicLib
+  TimerLib
+  PrintLib
+  PeCoffExtraActionLib
+  PeCoffGetEntryPointLib
+  SmmServicesTableLib
 
 [Guids]
-  gEfiDebugAgentGuid                            ## CONSUMES ## Configuration Table
+  ## CONSUMES ## SystemTable
+  ## CONSUMES ## HOB
+  gEfiDebugAgentGuid
+  gEfiVectorHandoffTableGuid            ## PRODUCES ## GUID # SMM Configuration Table
 
 [Pcd]
-  gEfiMdePkgTokenSpaceGuid.PcdFSBClock                                  ## CONSUMES
-  gEfiSourceLevelDebugPkgTokenSpaceGuid.PcdExceptionsIgnoredByDebugger  ## CONSUMES
+  gEfiMdePkgTokenSpaceGuid.PcdFSBClock                                             ## SOMETIMES_CONSUMES
+  # Skip Page Fault exception (14) by default in SMM
+  gEfiSourceLevelDebugPkgTokenSpaceGuid.PcdExceptionsIgnoredByDebugger|0x00004000  ## SOMETIMES_CONSUMES