From 18b144ea424e476f14839e9d9d3b81fb4820a613 Mon Sep 17 00:00:00 2001
From: vanjeff <vanjeff@6f19259b-4bc3-4df7-8a09-765794883524>
Date: Sun, 12 Sep 2010 06:43:36 +0000
Subject: Import SourceLevelDebugPkg.

git-svn-id: https://edk2.svn.sourceforge.net/svnroot/edk2/trunk/edk2@10867 6f19259b-4bc3-4df7-8a09-765794883524
---
 .../DebugAgent/DebugAgentCommon/DebugAgent.c       | 1195 ++++++++++++++++++++
 .../DebugAgent/DebugAgentCommon/DebugAgent.h       |  308 +++++
 .../Library/DebugAgent/DebugAgentCommon/DebugMp.c  |  365 ++++++
 .../Library/DebugAgent/DebugAgentCommon/DebugMp.h  |  221 ++++
 .../DebugAgent/DebugAgentCommon/DebugTimer.c       |   83 ++
 .../DebugAgent/DebugAgentCommon/DebugTimer.h       |   28 +
 .../DebugAgentCommon/Ia32/ArchDebugSupport.c       |  242 ++++
 .../DebugAgentCommon/Ia32/ArchDebugSupport.h       |   31 +
 .../DebugAgentCommon/Ia32/ArchReadGroupRegister.c  |  210 ++++
 .../DebugAgentCommon/Ia32/ArchRegisters.h          |  156 +++
 .../DebugAgent/DebugAgentCommon/Ia32/AsmFuncs.S    |  360 ++++++
 .../DebugAgent/DebugAgentCommon/Ia32/AsmFuncs.asm  |  365 ++++++
 .../DebugAgentCommon/Ia32/DebugException.h         |   36 +
 .../DebugAgentCommon/X64/ArchDebugSupport.c        |  255 +++++
 .../DebugAgentCommon/X64/ArchDebugSupport.h        |   31 +
 .../DebugAgentCommon/X64/ArchReadGroupRegister.c   |  259 +++++
 .../DebugAgentCommon/X64/ArchRegisters.h           |  329 ++++++
 .../DebugAgent/DebugAgentCommon/X64/AsmFuncs.S     |  401 +++++++
 .../DebugAgent/DebugAgentCommon/X64/AsmFuncs.asm   |  364 ++++++
 .../DebugAgentCommon/X64/DebugException.h          |   36 +
 .../DebugAgent/DxeDebugAgent/DxeDebugAgentLib.c    |  247 ++++
 .../DebugAgent/DxeDebugAgent/DxeDebugAgentLib.h    |   25 +
 .../Library/DebugAgent/DxeDebugAgentLib.inf        |   87 ++
 .../SecPeiDebugAgent/SecPeiDebugAgentLib.c         |  307 +++++
 .../SecPeiDebugAgent/SecPeiDebugAgentLib.h         |   28 +
 .../Library/DebugAgent/SecPeiDebugAgentLib.inf     |   81 ++
 .../DebugAgent/SmmDebugAgent/SmmDebugAgentLib.c    |  157 +++
 .../DebugAgent/SmmDebugAgent/SmmDebugAgentLib.h    |   24 +
 .../Library/DebugAgent/SmmDebugAgentLib.inf        |   81 ++
 29 files changed, 6312 insertions(+)
 create mode 100644 SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/DebugAgent.c
 create mode 100644 SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/DebugAgent.h
 create mode 100644 SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/DebugMp.c
 create mode 100644 SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/DebugMp.h
 create mode 100644 SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/DebugTimer.c
 create mode 100644 SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/DebugTimer.h
 create mode 100644 SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/Ia32/ArchDebugSupport.c
 create mode 100644 SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/Ia32/ArchDebugSupport.h
 create mode 100644 SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/Ia32/ArchReadGroupRegister.c
 create mode 100644 SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/Ia32/ArchRegisters.h
 create mode 100644 SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/Ia32/AsmFuncs.S
 create mode 100644 SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/Ia32/AsmFuncs.asm
 create mode 100644 SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/Ia32/DebugException.h
 create mode 100644 SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/X64/ArchDebugSupport.c
 create mode 100644 SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/X64/ArchDebugSupport.h
 create mode 100644 SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/X64/ArchReadGroupRegister.c
 create mode 100644 SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/X64/ArchRegisters.h
 create mode 100644 SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/X64/AsmFuncs.S
 create mode 100644 SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/X64/AsmFuncs.asm
 create mode 100644 SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/X64/DebugException.h
 create mode 100644 SourceLevelDebugPkg/Library/DebugAgent/DxeDebugAgent/DxeDebugAgentLib.c
 create mode 100644 SourceLevelDebugPkg/Library/DebugAgent/DxeDebugAgent/DxeDebugAgentLib.h
 create mode 100644 SourceLevelDebugPkg/Library/DebugAgent/DxeDebugAgentLib.inf
 create mode 100644 SourceLevelDebugPkg/Library/DebugAgent/SecPeiDebugAgent/SecPeiDebugAgentLib.c
 create mode 100644 SourceLevelDebugPkg/Library/DebugAgent/SecPeiDebugAgent/SecPeiDebugAgentLib.h
 create mode 100644 SourceLevelDebugPkg/Library/DebugAgent/SecPeiDebugAgentLib.inf
 create mode 100644 SourceLevelDebugPkg/Library/DebugAgent/SmmDebugAgent/SmmDebugAgentLib.c
 create mode 100644 SourceLevelDebugPkg/Library/DebugAgent/SmmDebugAgent/SmmDebugAgentLib.h
 create mode 100644 SourceLevelDebugPkg/Library/DebugAgent/SmmDebugAgentLib.inf

(limited to 'SourceLevelDebugPkg/Library/DebugAgent')

diff --git a/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/DebugAgent.c b/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/DebugAgent.c
new file mode 100644
index 0000000000..39b24c39c6
--- /dev/null
+++ b/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/DebugAgent.c
@@ -0,0 +1,1195 @@
+/** @file
+  Commond Debug Agent library implementition. It mainly includes
+  the first C function called by exception/interrupt handlers,
+  read/write debug packet to communication with HOST based on transfer
+  protocol.
+
+  Copyright (c) 2010, 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
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php.
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#include "DebugAgent.h"
+#include "Ia32/DebugException.h"
+
+/**
+  Check if HOST is connected based on Mailbox.
+
+  @retval TRUE        HOST is connected.
+  @retval FALSE       HOST is not connected.
+
+**/
+BOOLEAN
+IsHostConnected (
+  VOID
+  )
+{
+  DEBUG_AGENT_MAILBOX          *Mailbox;
+
+  Mailbox = GetMailboxPointer ();
+
+  if (Mailbox->DebugFlag.Bits.HostPresent == 1) {
+    return TRUE;
+  } else {
+    return FALSE;
+  }
+}
+
+/**
+  Set HOST connect flag in Mailbox.
+
+**/
+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.
+
+**/
+VOID
+SetDebugFlag (
+  IN UINT32               DebugFlag
+  )
+{
+  DEBUG_AGENT_MAILBOX          *Mailbox;
+
+  Mailbox = GetMailboxPointer ();
+
+  if ((DebugFlag & SOFT_DEBUGGER_SETTING_SMM_ENTRY_BREAK) != 0) {
+    Mailbox->DebugFlag.Bits.BreakOnNextSmi = 1;
+  } else {
+    Mailbox->DebugFlag.Bits.BreakOnNextSmi = 0;
+  }
+}
+
+/**
+  Exectue GO command.
+
+  @param[in] CpuContext        Pointer to saved CPU context.
+
+**/
+VOID
+CommandGo (
+  IN DEBUG_CPU_CONTEXT         *CpuContext
+  )
+{
+  IA32_EFLAGS32                *Eflags;
+
+  Eflags = (IA32_EFLAGS32 *) &CpuContext->Eflags;
+  Eflags->Bits.TF = 0;
+  Eflags->Bits.RF = 1;
+}
+
+/**
+  Exectue Stepping command.
+
+  @param[in] CpuContext        Pointer to saved CPU context.
+
+**/
+VOID
+CommandStepping (
+  IN DEBUG_CPU_CONTEXT          *CpuContext
+  )
+{
+  IA32_EFLAGS32                *Eflags;
+
+  Eflags = (IA32_EFLAGS32 *) &CpuContext->Eflags;
+  Eflags->Bits.TF = 1;
+  Eflags->Bits.RF = 1;
+}
+
+/**
+  Set debug register for hardware breakpoint.
+
+  @param[in] CpuContext      Pointer to saved CPU context.
+  @param[in] SetHwBreakpoint Hardware breakpoint to be set.
+
+**/
+VOID
+SetDebugRegister (
+  IN DEBUG_CPU_CONTEXT             *CpuContext,
+  IN DEBUG_DATA_SET_HW_BREAKPOINT  *SetHwBreakpoint
+  )
+{
+  UINT8                      RegisterIndex;
+  UINTN                      Dr7Value;
+
+  RegisterIndex = SetHwBreakpoint->Type.Index;
+
+  //
+  // Set debug address
+  //
+  * ((UINTN *) &CpuContext->Dr0 + RegisterIndex) = (UINTN) SetHwBreakpoint->Address;
+
+  Dr7Value = CpuContext->Dr7;
+
+  //
+  // Enable Gx, Lx
+  //
+  Dr7Value |= 0x3 << (RegisterIndex * 2);
+  //
+  // Set RWx and Lenx
+  //
+  Dr7Value &= ~(0xf0000 << (RegisterIndex * 4));
+  Dr7Value |= (SetHwBreakpoint->Type.Length | SetHwBreakpoint->Type.Access) << (RegisterIndex * 4);
+  //
+  // Enable GE, LE
+  //
+  Dr7Value |= 0x300;
+
+  CpuContext->Dr7 = Dr7Value;
+}
+
+/**
+  Clear debug register for hardware breakpoint.
+
+  @param[in] CpuContext        Pointer to saved CPU context.
+  @param[in] ClearHwBreakpoint Hardware breakpoint to be cleared.
+
+**/
+VOID
+ClearDebugRegister (
+  IN DEBUG_CPU_CONTEXT                 *CpuContext,
+  IN DEBUG_DATA_CLEAR_HW_BREAKPOINT    *ClearHwBreakpoint
+  )
+{
+  if ((ClearHwBreakpoint->IndexMask & BIT0) != 0) {
+    CpuContext->Dr0 = 0;
+    CpuContext->Dr7 &= ~(0x3 << 0);
+  }
+  if ((ClearHwBreakpoint->IndexMask & BIT1) != 0) {
+    CpuContext->Dr1 = 0;
+    CpuContext->Dr7 &= ~(0x3 << 2);
+  }
+  if ((ClearHwBreakpoint->IndexMask & BIT2) != 0) {
+    CpuContext->Dr2 = 0;
+    CpuContext->Dr7 &= ~(0x3 << 4);
+  }
+  if ((ClearHwBreakpoint->IndexMask & BIT3) != 0) {
+    CpuContext->Dr3 = 0;
+    CpuContext->Dr7 &= ~(0x3 << 6);
+  }
+}
+
+/**
+  Send acknowledge packet to HOST.
+
+  @param[in] AckCommand    Type of Acknowledge packet.
+
+**/
+VOID
+SendAckPacket (
+  IN UINT8                AckCommand
+  )
+{
+  DEBUG_COMMAND_HEADER      DebugCommonHeader;
+  DEBUG_PORT_HANDLE         Handle;
+
+  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.
+
+  @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
+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;
+    }
+  }
+
+  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;
+}
+
+/**
+  Get current break cause.
+
+  @param[in] Vector      Vector value of exception or interrupt.
+  @param[in] CpuContext  Pointer to save CPU context.
+
+  @return The type of break cause defined by XXXX
+
+**/
+UINT8
+GetBreakCause (
+  IN UINTN                    Vector,
+  IN DEBUG_CPU_CONTEXT        *CpuContext
+  )
+{
+  UINT8                    Cause;
+
+  Cause = DEBUG_DATA_BREAK_CAUSE_UNKNOWN;
+
+  switch (Vector) {
+  case DEBUG_INT1_VECTOR:
+  case DEBUG_INT3_VECTOR:
+
+    if (Vector == DEBUG_INT1_VECTOR) {
+      //
+      // INT 1
+      //
+      if ((CpuContext->Dr6 & BIT14) != 0) {
+        Cause = DEBUG_DATA_BREAK_CAUSE_STEPPING;
+
+      } else {
+        Cause = DEBUG_DATA_BREAK_CAUSE_HW_BREAKPOINT;
+      }
+    } else {
+      //
+      // INT 3
+      //
+      Cause = DEBUG_DATA_BREAK_CAUSE_SW_BREAKPOINT;
+    }
+
+    switch (CpuContext->Dr0) {
+    case IMAGE_LOAD_SIGNATURE:
+    case IMAGE_UNLOAD_SIGNATURE:
+
+      if (CpuContext->Dr3 == IO_PORT_BREAKPOINT_ADDRESS) {
+
+        Cause = (UINT8) ((CpuContext->Dr0 == IMAGE_LOAD_SIGNATURE) ? 
+          DEBUG_DATA_BREAK_CAUSE_IMAGE_LOAD : DEBUG_DATA_BREAK_CAUSE_IMAGE_UNLOAD);
+      }
+      break;
+
+    case SOFT_INTERRUPT_SIGNATURE:
+   
+      if (CpuContext->Dr1 == MEMORY_READY_SIGNATURE) {
+        Cause = DEBUG_DATA_BREAK_CAUSE_MEMORY_READY;
+        CpuContext->Dr0 = 0;
+      } else if (CpuContext->Dr1 == SYSTEM_RESET_SIGNATURE) {
+        Cause = DEBUG_DATA_BREAK_CAUSE_SYSTEM_RESET;
+        CpuContext->Dr0 = 0;
+      }
+      break;
+
+    default:
+      break;
+
+    }
+
+    break;
+
+  case DEBUG_TIMER_VECTOR:
+    Cause = DEBUG_DATA_BREAK_CAUSE_USER_HALT;
+    break;
+
+  default:
+    if (Vector < 20) {
+      Cause = DEBUG_DATA_BREAK_CAUSE_EXCEPTION;
+    }
+    break;
+  }
+
+  return Cause;
+}
+
+/**
+  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;
+
+    }
+  }
+}
+
+/**
+  Send break cause packet to HOST.
+
+  @param[in] Vector      Vector value of exception or interrutp.
+  @param[in] CpuContext  Pointer to save CPU context.
+
+  @retval RETURN_SUCCESS      Response data was sent successfully.
+  @retval RETURN_DEVICE_ERROR Cannot receive DEBUG_COMMAND_OK from HOST.
+
+**/
+RETURN_STATUS
+SendBreakCausePacket (
+  IN UINTN                    Vector,
+  IN DEBUG_CPU_CONTEXT        *CpuContext
+  )
+{
+  DEBUG_DATA_RESPONSE_BREAK_CAUSE    DebugDataBreakCause;
+
+  DebugDataBreakCause.StopAddress = CpuContext->Eip;
+  DebugDataBreakCause.Cause       = GetBreakCause (Vector, CpuContext);
+
+  return SendDataResponsePacket (CpuContext, (UINT8 *) &DebugDataBreakCause, (UINT16) sizeof (DEBUG_DATA_RESPONSE_BREAK_CAUSE));
+}
+
+
+/**
+  The main function to process communication with HOST.
+
+  It received the command packet from HOST, and sent response data packet to HOST.
+
+  @param[in]      Vector         Vector value of exception or interrutp.
+  @param[in, out] CpuContext     Pointer to saved CPU context.
+  @param[in]      BreakReceived  TRUE means break-in symbol received.
+                                 FALSE means break-in symbol not received.
+
+**/
+VOID
+CommandCommunication (
+  IN     UINTN                   Vector,
+  IN OUT DEBUG_CPU_CONTEXT       *CpuContext,
+  IN     BOOLEAN                 BreakReceived
+  )
+{
+  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();
+
+  ProcessorIndex  = 0;
+  HaltDeferred = BreakReceived;
+
+  if (MultiProcessorDebugSupport) {
+    ProcessorIndex = GetProcessorIndex ();
+    SetCpuStopFlagByIndex (ProcessorIndex, TRUE);
+  }
+
+  while (TRUE) {
+
+    if (MultiProcessorDebugSupport) {
+      if (mDebugMpContext.ViewPointIndex != ProcessorIndex) {
+        if (mDebugMpContext.RunCommandSet) {
+          SetCpuStopFlagByIndex (ProcessorIndex, FALSE);
+          CommandGo (CpuContext);
+          break;
+        } else {
+          continue;
+        }
+      }
+    }
+
+    AcquireDebugPortControl ();
+
+    Status = ReceivePacket (InputPacketBuffer, &BreakReceived);
+
+    if (BreakReceived) {
+      HaltDeferred = TRUE;
+      BreakReceived = FALSE;
+    }
+
+    if (Status != RETURN_SUCCESS) {
+      ReleaseDebugPortControl ();
+      continue;
+    }
+
+    Data8 = 1;
+
+    DebugHeader =(DEBUG_COMMAND_HEADER *) InputPacketBuffer;
+    switch (DebugHeader->Command) {
+
+    case DEBUG_COMMAND_RESET:
+      SendAckPacket (DEBUG_COMMAND_OK);
+      ReleaseDebugPortControl ();
+
+      ResetCold ();
+      //
+      // Wait for reset
+      //
+      CpuDeadLoop ();
+      break;
+
+    case DEBUG_COMMAND_GO:
+      CommandGo (CpuContext);
+      if (!HaltDeferred) {
+        //
+        // If no HALT command received when being in-active mode
+        //
+        if (MultiProcessorDebugSupport) {
+          Data32 = FindCpuNotRunning ();
+          if (Data32 != -1) {
+            //
+            // If there are still others processors being in break state,          
+            // send OK packet to HOST to finish this go command
+            //
+            SendAckPacket (DEBUG_COMMAND_OK);
+            CpuPause ();
+            //
+            // Set current view to the next breaking processor
+            //
+            mDebugMpContext.ViewPointIndex = Data32;
+            mDebugMpContext.BreakAtCpuIndex = mDebugMpContext.ViewPointIndex;
+            SetCpuBreakFlagByIndex (mDebugMpContext.ViewPointIndex, FALSE);
+            //
+            // Send break packet to HOST and exit to wait for command packet from HOST.
+            //
+            SendAckPacket (DEBUG_COMMAND_BREAK_POINT);
+            WaitForAckPacketOK (0, &BreakReceived);
+            ReleaseDebugPortControl (); 
+            break;
+          }
+
+          //
+          // If no else processor break, set stop bitmask,
+          // and set Running flag for all processors.
+          //
+          SetCpuStopFlagByIndex (ProcessorIndex, FALSE);
+          SetCpuRunningFlag (TRUE);
+          CpuPause ();
+          //
+          // Wait for all processors are in running state 
+          //
+          while (TRUE) {
+            if (IsAllCpuRunning ()) {
+              break;
+            }
+          }
+          //
+          // Set BSP to be current view point.
+          //
+          SetDebugViewPoint (mDebugMpContext.BspIndex);
+          CpuPause ();
+          //
+          // Clear breaking processor index and running flag
+          //
+          mDebugMpContext.BreakAtCpuIndex = (UINT32) (-1);
+          SetCpuRunningFlag (FALSE);
+        }
+
+        //
+        // Send OK packet to HOST to finish this go command
+        //
+        SendAckPacket (DEBUG_COMMAND_OK);
+
+        ReleaseDebugPortControl ();
+
+        return;
+
+      } else {
+        //
+        // If reveived HALT command, need to defer the GO command
+        //
+        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;
+      }
+      break;
+
+    case DEBUG_COMMAND_BREAK_CAUSE:
+
+      if (MultiProcessorDebugSupport && ProcessorIndex != mDebugMpContext.BreakAtCpuIndex) {
+        Status = SendBreakCausePacket (DEBUG_TIMER_VECTOR, CpuContext);
+
+      } else {
+        Status = SendBreakCausePacket (Vector, CpuContext);
+      }
+
+      break;
+
+    case DEBUG_COMMAND_SET_HW_BREAKPOINT:
+      SetDebugRegister (CpuContext, (DEBUG_DATA_SET_HW_BREAKPOINT *) (DebugHeader + 1));
+      SendAckPacket (DEBUG_COMMAND_OK);
+      break;
+
+    case DEBUG_COMMAND_CLEAR_HW_BREAKPOINT:
+      ClearDebugRegister (CpuContext, (DEBUG_DATA_CLEAR_HW_BREAKPOINT *) (DebugHeader + 1));
+      SendAckPacket (DEBUG_COMMAND_OK);
+      break;
+
+    case DEBUG_COMMAND_SINGLE_STEPPING:
+      CommandStepping (CpuContext);
+
+      mDebugMpContext.BreakAtCpuIndex = (UINT32) (-1);
+
+      ReleaseDebugPortControl ();
+      //
+      // Executing stepping command directly without sending ACK packet.
+      //
+      return;
+
+    case DEBUG_COMMAND_SET_SW_BREAKPOINT:
+      Data64 = (UINTN) (((DEBUG_DATA_SET_SW_BREAKPOINT *) (DebugHeader + 1))->Address);
+      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);
+      SendAckPacket (DEBUG_COMMAND_OK);
+      break;
+
+    case DEBUG_COMMAND_READ_IO:
+      IoRead = (DEBUG_DATA_READ_IO *) (DebugHeader + 1);
+      switch (IoRead->Width) {
+      case 1:
+        Data64  = IoRead8 (IoRead->Port);
+        break;
+      case 2:
+        Data64  = IoRead16 (IoRead->Port);
+        break;
+      case 4:
+        Data64  = IoRead32 (IoRead->Port);
+        break;
+      case 8:
+        Data64  = IoRead64 (IoRead->Port);
+        break;
+      default:
+        Data64  = (UINT64) -1;
+      }
+      Status = SendDataResponsePacket (CpuContext, (UINT8 *) &Data64, IoRead->Width);
+      break;
+
+    case DEBUG_COMMAND_WRITE_IO:
+      IoWrite = (DEBUG_DATA_WRITE_IO *) (DebugHeader + 1);
+      switch (IoWrite->Width) {
+      case 1:
+        Data64  = IoWrite8 (IoWrite->Port, *(UINT8 *) &IoWrite->Data);
+        break;
+      case 2:
+        Data64  = IoWrite16 (IoWrite->Port, *(UINT16 *) &IoWrite->Data);
+        break;
+      case 4:
+        Data64  = IoWrite32 (IoWrite->Port, *(UINT32 *) &IoWrite->Data);
+        break;
+      case 8:
+        Data64  = IoWrite64 (IoWrite->Port, *(UINT64 *) &IoWrite->Data);
+        break;
+      default:
+        Data64  = (UINT64) -1;
+      }
+      SendAckPacket (DEBUG_COMMAND_OK);
+      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));
+      } 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;
+        }
+      }
+      break;
+
+    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);
+      break;
+
+    case DEBUG_COMMAND_ARCH_MODE:
+      Data8 = DEBUG_ARCH_SYMBOL;
+      Status = SendDataResponsePacket (CpuContext, (UINT8 *) &Data8, (UINT16) sizeof (UINT8));
+      break;
+
+    case DEBUG_COMMAND_READ_MSR:
+      MsrRegisterRead = (DEBUG_DATA_READ_MSR *) (DebugHeader + 1);
+      Data64 = AsmReadMsr64 (MsrRegisterRead->Index);
+      Status = SendDataResponsePacket (CpuContext, (UINT8 *) &Data64, (UINT16) sizeof (UINT64));
+      break;
+
+    case DEBUG_COMMAND_WRITE_MSR:
+      MsrRegisterWrite = (DEBUG_DATA_WRITE_MSR *) (DebugHeader + 1);
+      AsmWriteMsr64 (MsrRegisterWrite->Index, MsrRegisterWrite->Value);
+      SendAckPacket (DEBUG_COMMAND_OK);
+      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);
+      break;
+
+    case DEBUG_COMMAND_GET_REVISION:
+      DebugAgentRevision.Revision = DEBUG_AGENT_REVISION;
+      DebugAgentRevision.Capabilities = DEBUG_AGENT_CAPABILITIES;
+      Status = SendDataResponsePacket (CpuContext, (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));
+      break;
+
+    case DEBUG_COMMAND_SET_VIEWPOINT:
+      Data32 = *(UINT32 *) (DebugHeader + 1);
+
+      if (MultiProcessorDebugSupport) {
+        if (IsCpuStopped (Data32)) {
+          SetDebugViewPoint (Data32);
+          SendAckPacket (DEBUG_COMMAND_OK);
+        } else {
+          //
+          // If CPU is not halted
+          //
+          SendAckPacket (DEBUG_COMMAND_NOT_SUPPORTED);
+        }
+      } else if (Data32 == 0) {
+        SendAckPacket (DEBUG_COMMAND_OK);
+
+      } else {
+        SendAckPacket (DEBUG_COMMAND_NOT_SUPPORTED);
+      }
+
+      break;
+
+    case DEBUG_COMMAND_GET_VIEWPOINT:
+      Data32 = mDebugMpContext.ViewPointIndex;
+      SendDataResponsePacket(CpuContext, (UINT8 *) &Data32, (UINT16) sizeof (UINT32));
+      break;
+
+    default:
+      SendAckPacket (DEBUG_COMMAND_NOT_SUPPORTED);
+      break;
+    }
+
+    if (Status == RETURN_UNSUPPORTED) {
+      SendAckPacket (DEBUG_COMMAND_NOT_SUPPORTED);
+    } else if (Status != RETURN_SUCCESS) {
+      SendAckPacket (DEBUG_COMMAND_ABORT);
+    }
+
+    ReleaseDebugPortControl ();
+    CpuPause ();
+  }
+}
+
+/**
+  C function called in interrupt handler.
+
+  @param[in] Vector      Vector value of exception or interrutp.
+  @param[in] CpuContext  Pointer to save CPU context.
+
+**/
+VOID
+EFIAPI
+InterruptProcess (
+  IN UINT32                          Vector,
+  IN DEBUG_CPU_CONTEXT               *CpuContext
+  )
+{
+  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) {
+    ProcessorIndex = GetProcessorIndex ();
+    while (mDebugMpContext.RunCommandSet);
+  }
+
+  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) {
+        break;
+      }
+
+      SetHostConnectedFlag ();
+      CommandCommunication (Vector, CpuContext, BreakReceived);
+
+    } else if (BreakCause == DEBUG_DATA_BREAK_CAUSE_STEPPING) {
+
+      //
+      // Stepping is finished, send Ack package.
+      //
+      if (MultiProcessorDebugSupport) {
+        mDebugMpContext.BreakAtCpuIndex = ProcessorIndex;
+      }
+      SendAckPacket (DEBUG_COMMAND_OK);
+      CommandCommunication (Vector, CpuContext, BreakReceived);
+
+    } else if (BreakCause == DEBUG_DATA_BREAK_CAUSE_MEMORY_READY) {
+
+      //
+      // Memory is ready
+      //
+      SendAckPacket (DEBUG_COMMAND_MEMORY_READY);
+      WaitForAckPacketOK (0, &BreakReceived);
+      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 ();
+
+      if (Vector == DEBUG_INT3_VECTOR) {
+        //
+        // go back address located "0xCC"
+        //
+        CpuContext->Eip--;
+        SavedEip = CpuContext->Eip;
+        CommandCommunication (Vector, CpuContext, BreakReceived);
+        if ((SavedEip == CpuContext->Eip) &&
+            (*(UINT8 *) (UINTN) CpuContext->Eip == DEBUG_SW_BREAKPOINT_SYMBOL)) {
+          //
+          // If this is not a software breakpoint set by HOST,
+          // restore EIP
+          //
+          CpuContext->Eip++;
+        }
+      } else {
+        CommandCommunication (Vector, CpuContext, BreakReceived);
+      }
+    }
+
+    break;
+
+  case DEBUG_TIMER_VECTOR:
+
+    if (MultiProcessorDebugSupport) {
+      if (IsBsp (ProcessorIndex)) {
+        //
+        // If current processor is BSP, check Apic timer's init count if changed,
+        // it may be re-written when switching BSP.
+        // If it changed, re-initialize debug timer
+        //
+        CurrentDebugTimerInitCount = GetApicTimerInitCount ();
+        if (mDebugMpContext.DebugTimerInitCount != CurrentDebugTimerInitCount) {
+          InitializeDebugTimer ();
+        }
+      }
+
+      if (!IsBsp (ProcessorIndex) || mDebugMpContext.IpiSentByAp) {
+        //
+        // If current processor is not BSP or this is one IPI sent by AP
+        //
+        if (mDebugMpContext.BreakAtCpuIndex != (UINT32) (-1)) {
+          CommandCommunication (Vector, CpuContext, FALSE);
+        }
+
+        //
+        // Clear EOI before exiting interrupt process routine.
+        //
+        SendApicEoi ();
+        break;
+      }
+    }
+
+    //
+    // Only BSP could run here
+    //
+
+    AcquireDebugPortControl ();
+    
+    while (DebugPortPollBuffer (Handle)) {
+      //
+      // If there is data in debug port, will check whether it is 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) {
+        SendAckPacket (DEBUG_COMMAND_OK);
+        if (MultiProcessorDebugSupport) {
+          if(FindCpuNotRunning () != -1) {
+            SetCpuBreakFlagByIndex (ProcessorIndex, TRUE);
+          } else {
+            HaltOtherProcessors (ProcessorIndex);
+          }
+        }
+        ReleaseDebugPortControl ();
+        CommandCommunication (Vector, CpuContext, BreakReceived);
+        AcquireDebugPortControl ();
+        break;
+      }
+    }
+
+    //
+    // Clear EOI before exiting interrupt process routine.
+    //
+    SendApicEoi ();
+
+    ReleaseDebugPortControl ();
+
+    break;
+
+  default:
+
+    if (Vector <= DEBUG_EXCEPT_SIMD) {
+
+      AcquireDebugPortControl ();
+
+      if (MultiProcessorDebugSupport) {
+        if(FindCpuNotRunning () != -1) {
+          SetCpuBreakFlagByIndex (ProcessorIndex, TRUE);
+        } else {
+          HaltOtherProcessors (ProcessorIndex);
+        }
+      }
+      SendAckPacket (DEBUG_COMMAND_BREAK_POINT);
+      WaitForAckPacketOK (0, &BreakReceived);
+      ReleaseDebugPortControl ();
+      CommandCommunication (Vector, CpuContext, BreakReceived);
+    }
+    break;
+  }
+
+  if (MultiProcessorDebugSupport) {
+    //
+    // Clear flag and wait for all processors run here
+    //
+    SetIpiSentByApFlag (FALSE);
+    while (mDebugMpContext.RunCommandSet);
+  }
+
+  return;
+}
+
diff --git a/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/DebugAgent.h b/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/DebugAgent.h
new file mode 100644
index 0000000000..9671ca4efe
--- /dev/null
+++ b/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/DebugAgent.h
@@ -0,0 +1,308 @@
+/** @file
+  Command header of for Debug Agent library instance.
+
+  Copyright (c) 2010, 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
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php.
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _DEBUG_AGENT_H_
+#define _DEBUG_AGENT_H_
+
+#include <Register/LocalApic.h>
+
+#include <Guid/DebugAgentGuid.h>
+
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/ResetSystemLib.h>
+#include <Library/IoLib.h>
+#include <Library/HobLib.h>
+#include <Library/DebugCommunicationLib.h>
+#include <Library/DebugAgentLib.h>
+#include <Library/PcdLib.h>
+#include <Library/SynchronizationLib.h>
+#include <Library/LocalApicLib.h>
+
+#include <TransferProtocol.h>
+#include <ImageDebugSupport.h>
+
+#include "DebugMp.h"
+#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
+#define DEBUG_TIMER_VECTOR              32
+#define DEBUG_MAILBOX_VECTOR            33
+
+#define SOFT_INTERRUPT_SIGNATURE    SIGNATURE_32('S','O','F','T')
+#define SYSTEM_RESET_SIGNATURE      SIGNATURE_32('S','Y','S','R')
+#define MEMORY_READY_SIGNATURE      SIGNATURE_32('M','E','M','R')
+
+extern UINTN  Exception0Handle;
+extern UINTN  TimerInterruptHandle;
+extern UINT16 ExceptionStubHeaderSize;
+
+typedef union {
+  struct {
+    UINT32  HostPresent      : 1;
+    UINT32  BreakOnNextSmi   : 1;
+    UINT32  Reserved         : 30;
+  } Bits;
+  UINT32  Uint32;
+} DEBUG_AGENT_FLAG;
+
+#pragma pack(1)
+typedef struct {
+  DEBUG_AGENT_FLAG           DebugFlag;
+  UINT64                     DebugPortHandle;
+} DEBUG_AGENT_MAILBOX;
+#pragma pack()
+
+typedef union {
+  struct {
+    UINT32  LimitLow    : 16;
+    UINT32  BaseLow     : 16;
+    UINT32  BaseMid     : 8;
+    UINT32  Type        : 4;
+    UINT32  System      : 1;
+    UINT32  Dpl         : 2;
+    UINT32  Present     : 1;
+    UINT32  LimitHigh   : 4;
+    UINT32  Software    : 1;
+    UINT32  Reserved    : 1;
+    UINT32  DefaultSize : 1;
+    UINT32  Granularity : 1;
+    UINT32  BaseHigh    : 8;
+  } Bits;
+  UINT64  Uint64;
+} IA32_GDT;
+
+/**
+  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.
+
+**/
+VOID
+InitializeDebugIdt (
+  VOID
+  );
+
+/**
+  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
+  );
+
+/**
+  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
+  );
+
+/**
+  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
+  );
+
+/**
+  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.
+
+**/
+BOOLEAN
+IsHostConnected (
+  VOID
+  );
+
+/**
+  Get Debug Agent Mailbox pointer.
+
+  @return Mailbox pointer.
+
+**/
+DEBUG_AGENT_MAILBOX *
+GetMailboxPointer (
+  VOID
+  );
+
+/**
+  Get debug port handle.
+
+  @return Debug port handle.
+
+**/
+DEBUG_PORT_HANDLE
+GetDebugPortHandle (
+  VOID
+  );
+
+#endif
+
diff --git a/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/DebugMp.c b/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/DebugMp.c
new file mode 100644
index 0000000000..7d053fadc0
--- /dev/null
+++ b/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/DebugMp.c
@@ -0,0 +1,365 @@
+/** @file
+  Multi-Processor support functions implementation.
+
+  Copyright (c) 2010, 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
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php.
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#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.
+
+  It will block in the function if cannot get the access control.
+
+**/
+VOID
+AcquireDebugPortControl (
+  VOID
+  )
+{
+  if (!MultiProcessorDebugSupport) {
+    return;
+  }
+
+  while (TRUE) {
+    if (AcquireSpinLockOrFail (&mDebugMpContext.DebugPortSpinLock)) {
+      break;
+    }
+    CpuPause ();
+    continue;
+  }
+}
+
+/**
+  Release access control on debug port.
+
+**/
+VOID
+ReleaseDebugPortControl (
+  VOID
+  )
+{
+  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);
+}
+
+/**
+  Break the other processor by send IPI.
+
+  @param[in] CurrentProcessorIndex  Current processor index value.
+
+**/
+VOID
+HaltOtherProcessors (
+  IN UINT32             CurrentProcessorIndex
+  )
+{
+
+  if (!IsBsp (CurrentProcessorIndex)) {
+    SetIpiSentByApFlag (TRUE);;
+  }
+
+  mDebugMpContext.BreakAtCpuIndex = CurrentProcessorIndex;
+
+  //
+  // Send fixed IPI to other processors.
+  //
+  SendFixedIpiAllExcludingSelf (DEBUG_TIMER_VECTOR);
+  
+}
+
+/**
+  Get the current processor's index.
+
+  @return Processor index value.
+
+**/
+UINT32
+GetProcessorIndex (
+  VOID
+  )
+{
+  UINT32                Index;
+  UINT16                LocalApicID;
+
+  LocalApicID = (UINT16) GetApicId ();
+
+  AcquireMpContextControl ();
+
+  for (Index = 0; Index < mDebugCpuData.CpuCount; Index ++) {
+    if (mDebugCpuData.ApicID[Index] == LocalApicID) {
+      break;
+    }
+  }
+
+  if (Index == mDebugCpuData.CpuCount) {
+    mDebugCpuData.ApicID[Index] = LocalApicID;
+    mDebugCpuData.CpuCount ++ ;
+  }
+
+  ReleaseMpContextControl ();
+
+  return Index;
+}
+
+/**
+  Check if the specified processor is BSP or not.
+
+  @param[in] ProcessorIndex Processor index value.
+
+  @retval TRUE    It is BSP.
+  @retval FALSE   It isn't BSP.
+
+**/
+BOOLEAN
+IsBsp (
+  IN UINT32             ProcessorIndex
+  )
+{
+  if (AsmMsrBitFieldRead64 (MSR_IA32_APIC_BASE_ADDRESS, 8, 8) == 1) {
+    if (mDebugMpContext.BspIndex != ProcessorIndex) {
+      AcquireMpContextControl ();
+      mDebugMpContext.BspIndex = ProcessorIndex;
+      ReleaseMpContextControl ();
+    }
+    return TRUE;
+  } else {
+    return FALSE;
+  }
+}
+
+/**
+  Set processor stop flag bitmask in MP context.
+
+  @param[in] ProcessorIndex Processor index value.
+  @param[in] StopFlag       TRUE means set stop flag.
+                            FALSE means clean break flag.
+
+**/
+VOID
+SetCpuStopFlagByIndex (
+  IN UINT32             ProcessorIndex,
+  IN BOOLEAN            StopFlag
+  )
+{
+  UINT8                 Value;
+  UINTN                 Index;
+
+  AcquireMpContextControl ();
+
+  Value = mDebugMpContext.CpuStopStatusMask[ProcessorIndex / 8];
+  Index = ProcessorIndex % 8;
+  if (StopFlag) {
+    Value = BitFieldWrite8 (Value, Index, Index, 1);
+  } else {
+    Value = BitFieldWrite8 (Value, Index, Index, 0);
+  }
+  mDebugMpContext.CpuStopStatusMask[ProcessorIndex / 8] = Value;
+
+  ReleaseMpContextControl ();
+}
+
+/**
+  Set processor break flag bitmask in MP context.
+
+  @param[in] ProcessorIndex Processor index value.
+  @param[in] BreakFlag      TRUE means set break flag.
+                            FALSE means clean break flag.
+
+**/
+VOID
+SetCpuBreakFlagByIndex (
+  IN UINT32             ProcessorIndex,
+  IN BOOLEAN            BreakFlag
+  )
+{
+  UINT8                 Value;
+  UINTN                 Index;
+
+  AcquireMpContextControl ();
+
+  Value = mDebugMpContext.CpuBreakMask[ProcessorIndex / 8];
+  Index = ProcessorIndex % 8;
+  if (BreakFlag) {
+    Value = BitFieldWrite8 (Value, Index, Index, 1);
+  } else {
+    Value = BitFieldWrite8 (Value, Index, Index, 0);
+  }
+  mDebugMpContext.CpuBreakMask[ProcessorIndex / 8] = Value;
+
+  ReleaseMpContextControl ();
+}
+
+/**
+  Check if processor is stopped already.
+
+  @param[in] ProcessorIndex   Processor index value.
+
+  @retval TRUE        Processor is stopped already.
+  @retval TRUE        Processor isn't stopped.
+
+**/
+BOOLEAN
+IsCpuStopped (
+  IN UINT32              ProcessorIndex
+  )
+{
+  UINT8                 CpuMask;
+
+  CpuMask = (UINT8) (1 << (ProcessorIndex % 8));
+
+  if ((mDebugMpContext.CpuStopStatusMask[ProcessorIndex / 8] & CpuMask) != 0) {
+    return TRUE;
+  } else {
+    return FALSE;
+  }
+}
+
+/**
+  Set the run command flag.
+
+  @param[in] RunningFlag   TRUE means run command flag is set.
+                           FALSE means run command flag is cleared.
+
+**/
+VOID
+SetCpuRunningFlag (
+  IN BOOLEAN            RunningFlag
+  )
+{
+  AcquireMpContextControl ();
+
+  mDebugMpContext.RunCommandSet = RunningFlag;
+
+  ReleaseMpContextControl ();
+}
+
+/**
+  Set the current view point to be debugged.
+
+  @param[in] ProcessorIndex   Processor index value.
+
+**/
+VOID
+SetDebugViewPoint (
+  IN UINT32             ProcessorIndex
+  )
+{
+  AcquireMpContextControl ();
+
+  mDebugMpContext.ViewPointIndex = ProcessorIndex;
+
+  ReleaseMpContextControl ();
+}
+
+/**
+  Initialize debug timer.
+
+  @param[in] IpiSentByApFlag   TRUE means this IPI is sent by AP.
+                               FALSE means this IPI is sent by BSP.
+
+**/
+VOID
+SetIpiSentByApFlag (
+  IN BOOLEAN            IpiSentByApFlag
+  )
+{
+  AcquireMpContextControl ();
+
+  mDebugMpContext.IpiSentByAp = IpiSentByApFlag;
+
+  ReleaseMpContextControl ();
+}
+
+/**
+  Check if any processor breaks.
+
+  @retval others      There is at least one processor broken, the minimum
+                      index number of Processor returned.
+  @retval -1          No any processor broken.
+
+**/
+UINT32
+FindCpuNotRunning (
+  VOID
+  )
+{
+  UINT32               Index;
+  
+  for (Index = 0; Index < DEBUG_CPU_MAX_COUNT / 8; Index ++) {
+    if (mDebugMpContext.CpuBreakMask[Index] != 0) {
+      return  (UINT32) LowBitSet32 (mDebugMpContext.CpuBreakMask[Index]) + Index * 8;
+    }
+  }
+  return (UINT32)-1;
+}
+  
+/**
+  Check if all processors are in running status.
+
+  @retval TRUE        All processors run.
+  @retval FALSE       At least one processor does not run.
+
+**/
+BOOLEAN
+IsAllCpuRunning (
+  VOID
+  )
+{
+  UINTN              Index;
+  
+  for (Index = 0; Index < DEBUG_CPU_MAX_COUNT / 8; Index ++) {
+    if (mDebugMpContext.CpuStopStatusMask[Index] != 0) {
+      return FALSE;
+    }
+  }
+  return TRUE;
+}
+
diff --git a/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/DebugMp.h b/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/DebugMp.h
new file mode 100644
index 0000000000..db124599a5
--- /dev/null
+++ b/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/DebugMp.h
@@ -0,0 +1,221 @@
+/** @file
+  Header file for Multi-Processor support.
+
+  Copyright (c) 2010, 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
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php.
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _DEBUG_MP_H_
+#define _DEBUG_MP_H_
+
+#define DEBUG_CPU_MAX_COUNT             256
+
+typedef struct {
+  UINT32    CpuCount;                            ///< Processor count
+  UINT16    ApicID[DEBUG_CPU_MAX_COUNT];         ///< Record the local apic id for each processor
+} DEBUG_CPU_DATA;
+
+typedef struct {
+  SPIN_LOCK                 MpContextSpinLock;   ///< Lock for writting MP context
+  SPIN_LOCK                 DebugPortSpinLock;   ///< Lock for access debug port
+  UINT8                     CpuBreakMask[DEBUG_CPU_MAX_COUNT/8];        ///< Bitmask of all breaking CPUs
+  UINT8                     CpuStopStatusMask[DEBUG_CPU_MAX_COUNT/8];   ///< Bitmask of CPU stop status
+  UINT32                    ViewPointIndex;      ///< Current view point to be debugged
+  UINT32                    BspIndex;            ///< Processor index value of BSP
+  UINT32                    BreakAtCpuIndex;     ///< Processor index value of the current breaking CPU
+  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.
+} DEBUG_MP_CONTEXT;
+
+extern CONST BOOLEAN               MultiProcessorDebugSupport;
+extern DEBUG_MP_CONTEXT volatile   mDebugMpContext;
+extern DEBUG_CPU_DATA   volatile   mDebugCpuData;
+
+/**
+  Break the other processor by send IPI.
+
+  @param[in] CurrentProcessorIndex  Current processor index value.
+
+**/
+VOID
+HaltOtherProcessors (
+  IN UINT32             CurrentProcessorIndex
+  );
+
+/**
+  Get the current processor's index.
+
+  @return Processor index value.
+
+**/
+UINT32
+GetProcessorIndex (
+  VOID
+  );
+
+/**
+  Acquire access control on MP context.
+
+  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
+  );
+
+/**
+  Check if the specified processor is BSP or not.
+
+  @param[in] ProcessorIndex Processor index value.
+
+  @retval TRUE    It is BSP.
+  @retval FALSE   It isn't BSP.
+
+**/
+BOOLEAN
+IsBsp (
+  IN UINT32             ProcessorIndex
+  );
+
+/**
+  Set processor stop flag bitmask in MP context.
+
+  @param[in] ProcessorIndex Processor index value.
+  @param[in] StopFlag       TRUE means set stop flag.
+                            FALSE means clean break flag.
+
+**/
+VOID
+SetCpuStopFlagByIndex (
+  IN UINT32             ProcessorIndex,
+  IN BOOLEAN            StopFlag
+  );
+
+/**
+  Set processor break flag bitmask in MP context.
+
+  @param[in] ProcessorIndex Processor index value.
+  @param[in] BreakFlag      TRUE means set break flag.
+                            FALSE means clean break flag.
+
+**/
+VOID
+SetCpuBreakFlagByIndex (
+  IN UINT32             ProcessorIndex,
+  IN BOOLEAN            BreakFlag
+  );
+
+/**
+  Check if processor is stopped already.
+
+  @param[in] ProcessorIndex   Processor index value.
+
+  @retval TRUE        Processor is stopped already.
+  @retval FALSE       Processor isn't stopped.
+
+**/
+BOOLEAN
+IsCpuStopped (
+  IN UINT32              ProcessorIndex
+  );
+
+/**
+  Set the run command flag.
+
+  @param[in] RunningFlag   TRUE means run command flag is set.
+                           FALSE means run command flag is cleared.
+
+**/
+VOID
+SetCpuRunningFlag (
+  IN BOOLEAN            RunningFlag
+  );
+
+/**
+  Set the current view point to be debugged.
+
+  @param[in] ProcessorIndex   Processor index value.
+
+**/
+VOID
+SetDebugViewPoint (
+  IN UINT32             ProcessorIndex
+  );
+
+/**
+  Initialize debug timer.
+
+  @param[in] IpiSentByApFlag   TRUE means this IPI is sent by AP.
+                               FALSE means this IPI is sent by BSP.
+
+**/
+VOID
+SetIpiSentByApFlag (
+  IN BOOLEAN            IpiSentByApFlag
+  );
+
+/**
+  Check if any processor breaks.
+
+  @retval others      There is at least one processor broken, the minimum
+                      index number of Processor returned.
+  @retval -1          No any processor broken.
+
+**/
+UINT32
+FindCpuNotRunning (
+  VOID
+  );
+  
+/**
+  Check if all processors are in running status.
+
+  @retval TRUE        All processors run.
+  @retval FALSE       At least one processor does not run.
+
+**/
+BOOLEAN
+IsAllCpuRunning (
+  VOID
+  );
+
+#endif
+
diff --git a/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/DebugTimer.c b/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/DebugTimer.c
new file mode 100644
index 0000000000..d473df8678
--- /dev/null
+++ b/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/DebugTimer.c
@@ -0,0 +1,83 @@
+/** @file
+  Code for debug timer to support debug agent library implementation.
+
+  Copyright (c) 2010, 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
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php.
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#include "DebugAgent.h"
+
+/**
+  Initialize CPU local APIC timer.
+
+**/
+VOID
+InitializeDebugTimer (
+  VOID
+  )
+{
+  UINTN       ApicTimerDivisor;
+  UINT32      InitialCount;
+
+  GetApicTimerState (&ApicTimerDivisor, NULL, NULL);
+
+  //
+  // Cpu Local Apic timer interrupt frequency, it is set to 0.1s
+  //
+  InitialCount = (UINT32)DivU64x32 (
+                   MultU64x64 (
+                     PcdGet32(PcdFSBClock) / (UINT32)ApicTimerDivisor,
+                     100
+                     ),
+                   1000
+                   );
+
+  InitializeApicTimer (ApicTimerDivisor, InitialCount, TRUE, DEBUG_TIMER_VECTOR);
+
+  if (MultiProcessorDebugSupport) {
+    mDebugMpContext.DebugTimerInitCount = InitialCount;
+  }
+}
+
+/**
+  Enable/Disable the interrupt of debug timer and return the interrupt state
+  prior to the operation.
+
+  If EnableStatus is TRUE, enable the interrupt of debug timer.
+  If EnableStatus is FALSE, disable the interrupt of debug timer.
+
+  @param[in] EnableStatus    Enable/Disable.
+
+  @retval TRUE  Debug timer interrupt were enabled on entry to this call.
+  @retval FALSE Debug timer interrupt were disabled on entry to this call.
+
+**/
+BOOLEAN
+EFIAPI
+SaveAndSetDebugTimerInterrupt (
+  IN BOOLEAN                EnableStatus
+  )
+{
+  BOOLEAN     OldInterruptState;
+  BOOLEAN     OldDebugTimerInterruptState;
+
+  OldInterruptState = SaveAndDisableInterrupts ();
+  OldDebugTimerInterruptState = GetApicTimerInterruptState ();
+  
+  if (EnableStatus) {
+    EnableApicTimerInterrupt ();
+  } else {
+    DisableApicTimerInterrupt ();
+  }
+
+  SetInterruptState (OldInterruptState);
+  return OldDebugTimerInterruptState;
+}
+
diff --git a/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/DebugTimer.h b/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/DebugTimer.h
new file mode 100644
index 0000000000..4970f93da3
--- /dev/null
+++ b/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/DebugTimer.h
@@ -0,0 +1,28 @@
+/** @file
+  Header file for debug timer to support debug agent library implementation.
+
+  Copyright (c) 2010, 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
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php.
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _DEBUG_TIMER_H_
+#define _DEBUG_TIMER_H_
+
+/**
+  Initialize debug timer.
+
+**/
+VOID
+InitializeDebugTimer (
+  VOID
+  );
+
+#endif
+
diff --git a/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/Ia32/ArchDebugSupport.c b/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/Ia32/ArchDebugSupport.c
new file mode 100644
index 0000000000..ab724ff3d6
--- /dev/null
+++ b/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/Ia32/ArchDebugSupport.c
@@ -0,0 +1,242 @@
+/** @file
+  Public include file for Debug Port Library.
+
+  Copyright (c) 2010, 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
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php.
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#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;
+}
+
+/**
+  Initialize IDT entries to support source level debug.
+
+**/
+VOID
+InitializeDebugIdt (
+  VOID
+  )
+{
+  IA32_IDT_GATE_DESCRIPTOR   *IdtEntry;
+  UINTN                      InterruptHandler;
+  IA32_DESCRIPTOR            IdtDescriptor;
+  UINTN                      Index;
+  UINT16                     CodeSegment;
+
+  AsmReadIdtr (&IdtDescriptor);
+
+  //
+  // Use current CS as the segment selector of interrupt gate in IDT
+  //
+  CodeSegment = AsmReadCs ();
+
+  IdtEntry = (IA32_IDT_GATE_DESCRIPTOR *) IdtDescriptor.Base;
+
+  for (Index = 0; Index < 20; Index ++) {
+    if ((PcdGet32 (PcdExceptionsIgnoredByDebugger) & (1 << Index)) != 0) {
+      //
+      // If the exception is masked to be reserved, skip it
+      //
+      continue;
+    }
+    InterruptHandler = (UINTN)&Exception0Handle + Index * ExceptionStubHeaderSize;
+    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;
+  }
+
+  InterruptHandler = (UINTN) &TimerInterruptHandle;
+  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;
+}
diff --git a/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/Ia32/ArchDebugSupport.h b/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/Ia32/ArchDebugSupport.h
new file mode 100644
index 0000000000..7df3cb9415
--- /dev/null
+++ b/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/Ia32/ArchDebugSupport.h
@@ -0,0 +1,31 @@
+/** @file
+  IA32 specific defintions for debug agent library instance.
+
+  Copyright (c) 2010, 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
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php.
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _ARCH_DEBUG_SUPPORT_H_
+#define _ARCH_DEBUG_SUPPORT_H_
+
+#include "ArchRegisters.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_SYSTEM_CONTEXT   DEBUG_CPU_CONTEXT;
+
+#endif
diff --git a/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/Ia32/ArchReadGroupRegister.c b/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/Ia32/ArchReadGroupRegister.c
new file mode 100644
index 0000000000..59ebd00255
--- /dev/null
+++ b/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/Ia32/ArchReadGroupRegister.c
@@ -0,0 +1,210 @@
+/** @file
+  IA32 Group registers read support functions.
+
+  Copyright (c) 2010, 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
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php.
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#include "DebugAgent.h"
+
+/**
+  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
+  )
+{
+  IA32_DESCRIPTOR               *Ia32Descriptor;
+  IA32_GDT                      *Ia32Gdt;
+  UINTN                         Index;
+
+  Ia32Descriptor = (IA32_DESCRIPTOR *) CpuContext->Gdtr;
+  Ia32Gdt = (IA32_GDT *) (Ia32Descriptor->Base);
+
+  Index = CpuContext->Cs / 8;
+  RegisterGroupSegBase->CsBas = (Ia32Gdt[Index].Bits.BaseLow) + (Ia32Gdt[Index].Bits.BaseMid << 16) + (Ia32Gdt[Index].Bits.BaseMid << 24);
+  Index = CpuContext->Ss / 8;
+  RegisterGroupSegBase->SsBas = (Ia32Gdt[Index].Bits.BaseLow) + (Ia32Gdt[Index].Bits.BaseMid << 16) + (Ia32Gdt[Index].Bits.BaseMid << 24);
+  Index = CpuContext->Gs / 8;
+  RegisterGroupSegBase->GsBas = (Ia32Gdt[Index].Bits.BaseLow) + (Ia32Gdt[Index].Bits.BaseMid << 16) + (Ia32Gdt[Index].Bits.BaseMid << 24);
+  Index = CpuContext->Fs / 8;
+  RegisterGroupSegBase->FsBas = (Ia32Gdt[Index].Bits.BaseLow) + (Ia32Gdt[Index].Bits.BaseMid << 16) + (Ia32Gdt[Index].Bits.BaseMid << 24);
+  Index = CpuContext->Es / 8;
+  RegisterGroupSegBase->EsBas = (Ia32Gdt[Index].Bits.BaseLow) + (Ia32Gdt[Index].Bits.BaseMid << 16) + (Ia32Gdt[Index].Bits.BaseMid << 24);
+  Index = CpuContext->Ds / 8;
+  RegisterGroupSegBase->DsBas = (Ia32Gdt[Index].Bits.BaseLow) + (Ia32Gdt[Index].Bits.BaseMid << 16) + (Ia32Gdt[Index].Bits.BaseMid << 24);
+
+  RegisterGroupSegBase->LdtBas = 0;
+  RegisterGroupSegBase->TssBas = 0;
+}
+
+/**
+  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
+  )
+{
+  IA32_DESCRIPTOR               *Ia32Descriptor;
+  IA32_GDT                      *Ia32Gdt;
+  UINTN                         Index;
+
+  Ia32Descriptor = (IA32_DESCRIPTOR *) CpuContext->Gdtr;
+  Ia32Gdt = (IA32_GDT *) (Ia32Descriptor->Base);
+
+  Index = CpuContext->Cs / 8;
+  RegisterGroupSegLim->CsLim = Ia32Gdt[Index].Bits.LimitLow + (Ia32Gdt[Index].Bits.LimitHigh << 16);
+  if (Ia32Gdt[Index].Bits.Granularity == 1) {
+    RegisterGroupSegLim->CsLim = (RegisterGroupSegLim->CsLim << 12) | 0xfff;
+  }
+
+  Index = CpuContext->Ss / 8;
+  RegisterGroupSegLim->SsLim = Ia32Gdt[Index].Bits.LimitLow + (Ia32Gdt[Index].Bits.LimitHigh << 16);
+  if (Ia32Gdt[Index].Bits.Granularity == 1) {
+    RegisterGroupSegLim->SsLim = (RegisterGroupSegLim->SsLim << 12) | 0xfff;
+  }
+
+  Index = CpuContext->Gs / 8;
+  RegisterGroupSegLim->GsLim = Ia32Gdt[Index].Bits.LimitLow + (Ia32Gdt[Index].Bits.LimitHigh << 16);
+  if (Ia32Gdt[Index].Bits.Granularity == 1) {
+    RegisterGroupSegLim->GsLim = (RegisterGroupSegLim->GsLim << 12) | 0xfff;
+  }
+
+  Index = CpuContext->Fs / 8;
+  RegisterGroupSegLim->FsLim = Ia32Gdt[Index].Bits.LimitLow + (Ia32Gdt[Index].Bits.LimitHigh << 16);
+  if (Ia32Gdt[Index].Bits.Granularity == 1) {
+    RegisterGroupSegLim->FsLim = (RegisterGroupSegLim->FsLim << 12) | 0xfff;
+  }
+
+  Index = CpuContext->Es / 8;
+  RegisterGroupSegLim->EsLim = Ia32Gdt[Index].Bits.LimitLow + (Ia32Gdt[Index].Bits.LimitHigh << 16);
+  if (Ia32Gdt[Index].Bits.Granularity == 1) {
+    RegisterGroupSegLim->EsLim = (RegisterGroupSegLim->EsLim << 12) | 0xfff;
+  }
+
+  Index = CpuContext->Ds / 8;
+  RegisterGroupSegLim->DsLim = Ia32Gdt[Index].Bits.LimitLow + (Ia32Gdt[Index].Bits.LimitHigh << 16);
+  if (Ia32Gdt[Index].Bits.Granularity == 1) {
+    RegisterGroupSegLim->DsLim = (RegisterGroupSegLim->DsLim << 12) | 0xfff;
+  }
+
+  RegisterGroupSegLim->LdtLim = 0xffff;
+  RegisterGroupSegLim->TssLim = 0xffff;
+}
+
+/**
+  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
+  )
+{
+  DEBUG_DATA_REPONSE_READ_REGISTER_GROUP  RegisterGroup;
+  DEBUG_DATA_REPONSE_READ_REGISTER_GROUP_SEGLIM   RegisterGroupSegLim;
+  DEBUG_DATA_REPONSE_READ_REGISTER_GROUP_SEGBASE  RegisterGroupSegBase;
+
+  switch (GroupIndex) {
+  case SOFT_DEBUGGER_REGISTER_GROUP_GPDRS32:
+    ReadRegisterGroup (CpuContext, &RegisterGroup);
+    SendDataResponsePacket (CpuContext, (UINT8 *) &RegisterGroup, (UINT16) sizeof (DEBUG_DATA_REPONSE_READ_REGISTER_GROUP));
+    break;
+
+  case SOFT_DEBUGGER_REGISTER_GROUP_SEGMENT_LIMITS32:
+    ReadRegisterGroupSegLim (CpuContext, &RegisterGroupSegLim);
+    SendDataResponsePacket (CpuContext, (UINT8 *) &RegisterGroupSegLim, (UINT16) sizeof (DEBUG_DATA_REPONSE_READ_REGISTER_GROUP_SEGLIM));
+    break;
+
+  case SOFT_DEBUGGER_REGISTER_GROUP_SEGMENT_BASES32:
+    ReadRegisterGroupSegBase (CpuContext, &RegisterGroupSegBase);
+    SendDataResponsePacket (CpuContext, (UINT8 *) &RegisterGroupSegBase, (UINT16) sizeof (DEBUG_DATA_REPONSE_READ_REGISTER_GROUP_SEGBASE));
+    break;
+
+  default:
+    return RETURN_UNSUPPORTED;
+  }
+
+  return RETURN_SUCCESS;
+}
+
+/**
+  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
+  )
+{
+  IA32_DESCRIPTOR               *Ia32Descriptor;
+  IA32_GDT                      *Ia32Gdt;
+  UINT16                        Selector;
+  UINT32                        Data32;
+
+  Ia32Descriptor = (IA32_DESCRIPTOR *) CpuContext->Gdtr;
+  Ia32Gdt = (IA32_GDT *) (Ia32Descriptor->Base);
+
+  Selector = 0;
+
+  switch (RegisterIndex) {
+  case SOFT_DEBUGGER_REGISTER_CSAS:
+    Selector = (UINT16) CpuContext->Cs;
+    break;
+  case SOFT_DEBUGGER_REGISTER_SSAS:
+    Selector = (UINT16) CpuContext->Ss;
+    break;
+  case SOFT_DEBUGGER_REGISTER_GSAS:
+    Selector = (UINT16) CpuContext->Gs;
+    break;
+  case SOFT_DEBUGGER_REGISTER_FSAS:
+    Selector = (UINT16) CpuContext->Fs;
+    break;
+  case SOFT_DEBUGGER_REGISTER_ESAS:
+    Selector = (UINT16) CpuContext->Es;
+    break;
+  case SOFT_DEBUGGER_REGISTER_DSAS:
+    Selector = (UINT16) CpuContext->Ds;
+  case SOFT_DEBUGGER_REGISTER_LDTAS:
+  case SOFT_DEBUGGER_REGISTER_TSSAS:
+    return 0x00820000;
+    break;
+  }
+
+  Data32 = (UINT32) RShiftU64 (Ia32Gdt[Selector / 8].Uint64, 24);
+  return (Data32 & (UINT32)(~0xff)) | Selector;
+
+}
+
diff --git a/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/Ia32/ArchRegisters.h b/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/Ia32/ArchRegisters.h
new file mode 100644
index 0000000000..3c279589e7
--- /dev/null
+++ b/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/Ia32/ArchRegisters.h
@@ -0,0 +1,156 @@
+/** @file
+  IA32 register defintions needed by debug transfer protocol.
+
+  Copyright (c) 2010, 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
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php.
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _ARCH_REGISTERS_H_
+#define _ARCH_REGISTERS_H_
+
+///
+/// FXSAVE_STATE
+/// FP / MMX / XMM registers (see fxrstor instruction definition)
+///
+typedef struct {
+  UINT16  Fcw;
+  UINT16  Fsw;
+  UINT16  Ftw;
+  UINT16  Opcode;
+  UINT32  Eip;
+  UINT16  Cs;
+  UINT16  Reserved1;
+  UINT32  DataOffset;
+  UINT16  Ds;
+  UINT8   Reserved2[2];
+  UINT32  Mxcsr;
+  UINT32  Mxcsr_Mask;
+  UINT8   St0Mm0[10];
+  UINT8   Reserved3[6];
+  UINT8   St1Mm1[10];
+  UINT8   Reserved4[6];
+  UINT8   St2Mm2[10];
+  UINT8   Reserved5[6];
+  UINT8   St3Mm3[10];
+  UINT8   Reserved6[6];
+  UINT8   St4Mm4[10];
+  UINT8   Reserved7[6];
+  UINT8   St5Mm5[10];
+  UINT8   Reserved8[6];
+  UINT8   St6Mm6[10];
+  UINT8   Reserved9[6];
+  UINT8   St7Mm7[10];
+  UINT8   Reserved10[6];
+  UINT8   Xmm0[16];
+  UINT8   Xmm1[16];
+  UINT8   Xmm2[16];
+  UINT8   Xmm3[16];
+  UINT8   Xmm4[16];
+  UINT8   Xmm5[16];
+  UINT8   Xmm6[16];
+  UINT8   Xmm7[16];
+  UINT8   Reserved11[14 * 16];
+} DEBUG_DATA_IA32_FX_SAVE_STATE;
+
+///
+///  IA-32 processor context definition
+///
+typedef struct {
+  DEBUG_DATA_IA32_FX_SAVE_STATE  FxSaveState;
+  UINT32                         Dr0;
+  UINT32                         Dr1;
+  UINT32                         Dr2;
+  UINT32                         Dr3;
+  UINT32                         Dr6;
+  UINT32                         Dr7;
+  UINT32                         Eflags;
+  UINT32                         Ldtr;
+  UINT32                         Tr;
+  UINT32                         Gdtr[2];
+  UINT32                         Idtr[2];
+  UINT32                         Eip;
+  UINT32                         Gs;
+  UINT32                         Fs;
+  UINT32                         Es;
+  UINT32                         Ds;
+  UINT32                         Cs;
+  UINT32                         Ss;
+  UINT32                         Cr0;
+  UINT32                         Cr1;  ///< Reserved
+  UINT32                         Cr2;
+  UINT32                         Cr3;
+  UINT32                         Cr4;
+  UINT32                         Edi;
+  UINT32                         Esi;
+  UINT32                         Ebp;
+  UINT32                         Esp;
+  UINT32                         Edx;
+  UINT32                         Ecx;
+  UINT32                         Ebx;
+  UINT32                         Eax;
+} DEBUG_DATA_IA32_SYSTEM_CONTEXT;
+
+///
+///  IA32 GROUP register
+///
+typedef struct {
+  UINT16         Cs;
+  UINT16         Ds;
+  UINT16         Es;
+  UINT16         Fs;
+  UINT16         Gs;
+  UINT16         Ss;
+  UINT32         Eflags;
+  UINT32         Ebp;
+  UINT32         Eip;
+  UINT32         Esp;
+  UINT32         Eax;
+  UINT32         Ebx;
+  UINT32         Ecx;
+  UINT32         Edx;
+  UINT32         Esi;
+  UINT32         Edi;
+  UINT32         Dr0;
+  UINT32         Dr1;
+  UINT32         Dr2;
+  UINT32         Dr3;
+  UINT32         Dr6;
+  UINT32         Dr7;
+} DEBUG_DATA_REPONSE_READ_REGISTER_GROUP_IA32;
+
+///
+///  IA32 Segment Limit GROUP register
+///
+typedef struct {
+  UINT32         CsLim;
+  UINT32         SsLim;
+  UINT32         GsLim;
+  UINT32         FsLim;
+  UINT32         EsLim;
+  UINT32         DsLim;
+  UINT32         LdtLim;
+  UINT32         TssLim;
+} DEBUG_DATA_REPONSE_READ_REGISTER_GROUP_SEGLIM_IA32;
+
+///
+///  IA32 Segment Base GROUP register
+///
+typedef struct {
+  UINT32         CsBas;
+  UINT32         SsBas;
+  UINT32         GsBas;
+  UINT32         FsBas;
+  UINT32         EsBas;
+  UINT32         DsBas;
+  UINT32         LdtBas;
+  UINT32         TssBas;
+} DEBUG_DATA_REPONSE_READ_REGISTER_GROUP_SEGBASE_IA32;
+
+#endif
diff --git a/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/Ia32/AsmFuncs.S b/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/Ia32/AsmFuncs.S
new file mode 100644
index 0000000000..365947f4d5
--- /dev/null
+++ b/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/Ia32/AsmFuncs.S
@@ -0,0 +1,360 @@
+#------------------------------------------------------------------------------
+#
+# Copyright (c) 2010, 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
+# which accompanies this distribution.  The full text of the license may be found at
+# http://opensource.org/licenses/bsd-license.php.
+#
+# THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+# WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+#
+# Module Name:
+#
+#   AsmFuncs.S
+#
+# Abstract:
+#
+#   Debug interrupt handle functions.
+#
+#------------------------------------------------------------------------------
+
+#include "DebugException.h"
+
+ASM_GLOBAL ASM_PFX(InterruptProcess)
+ASM_GLOBAL ASM_PFX(Exception0Handle)
+ASM_GLOBAL ASM_PFX(ExceptionStubHeaderSize)
+ASM_GLOBAL ASM_PFX(TimerInterruptHandle)
+ASM_GLOBAL ASM_PFX(CommonEntry)
+
+.data
+
+ASM_PFX(ExceptionStubHeaderSize):  .word     ASM_PFX(Exception1Handle) - ASM_PFX(Exception0Handle)
+
+.text
+
+ASM_PFX(Exception0Handle):
+   cli
+   pushl %eax
+   mov   $0, %eax
+   jmp   ASM_PFX(CommonEntry)
+ASM_PFX(Exception1Handle):
+   cli
+   pushl %eax
+   mov   $1, %eax
+   jmp   ASM_PFX(CommonEntry)
+ASM_PFX(Exception2Handle):
+   cli
+   pushl %eax
+   mov   $2, %eax
+   jmp   ASM_PFX(CommonEntry)
+ASM_PFX(Exception3Handle):
+   cli
+   pushl %eax
+   mov   $3, %eax
+   jmp   ASM_PFX(CommonEntry)
+ASM_PFX(Exception4Handle):
+   cli
+   pushl %eax
+   mov   $4, %eax
+   jmp   ASM_PFX(CommonEntry)
+ASM_PFX(Exception5Handle):
+   cli
+   pushl %eax
+   mov   $5, %eax
+   jmp   ASM_PFX(CommonEntry)
+ASM_PFX(Exception6Handle):
+   cli
+   pushl %eax
+   mov   $6, %eax
+   jmp   ASM_PFX(CommonEntry)
+ASM_PFX(Exception7Handle):
+   cli
+   pushl %eax
+   mov   $7, %eax
+   jmp   ASM_PFX(CommonEntry)
+ASM_PFX(Exception8Handle):
+   cli
+   pushl %eax
+   mov   $8, %eax
+   jmp   ASM_PFX(CommonEntry)
+ASM_PFX(Exception9Handle):
+   cli
+   pushl %eax
+   mov   $9, %eax
+   jmp   ASM_PFX(CommonEntry)
+ASM_PFX(Exception10Handle):
+   cli
+   pushl %eax
+   mov   $10, %eax
+   jmp   ASM_PFX(CommonEntry)
+ASM_PFX(Exception11Handle):
+   cli
+   pushl %eax
+   mov   $11, %eax
+   jmp   ASM_PFX(CommonEntry)
+ASM_PFX(Exception12Handle):
+   cli
+   pushl %eax
+   mov   $12, %eax
+   jmp   ASM_PFX(CommonEntry)
+ASM_PFX(Exception13Handle):
+   cli
+   pushl %eax
+   mov   $13, %eax
+   jmp   ASM_PFX(CommonEntry)
+ASM_PFX(Exception14Handle):
+   cli
+   pushl %eax
+   mov   $14, %eax
+   jmp   ASM_PFX(CommonEntry)
+ASM_PFX(Exception15Handle):
+   cli
+   pushl %eax
+   mov   $15, %eax
+   jmp   ASM_PFX(CommonEntry)
+ASM_PFX(Exception16Handle):
+   cli
+   pushl %eax
+   mov   $16, %eax
+   jmp   ASM_PFX(CommonEntry)
+ASM_PFX(Exception17Handle):
+   cli
+   pushl %eax
+   mov   $17, %eax
+   jmp   ASM_PFX(CommonEntry)
+ASM_PFX(Exception18Handle):
+   cli
+   pushl %eax
+   mov   $18, %eax
+   jmp   ASM_PFX(CommonEntry)
+ASM_PFX(Exception19Handle):
+   cli
+   pushl %eax
+   mov   $19, %eax
+   jmp   ASM_PFX(CommonEntry)
+
+ASM_PFX(TimerInterruptHandle):
+   cli
+   pushl %eax
+   mov   $32, %eax
+   jmp   ASM_PFX(CommonEntry)
+
+
+ASM_PFX(CommonEntry):
+
+#---------------------------------------;
+# _CommonEntry                  ;
+#----------------------------------------------------------------------------;
+# The follow algorithm is used for the common interrupt routine.
+# Entry from each interrupt with a push eax and eax=interrupt number
+#
+# +---------------------+
+# +    EFlags           +
+# +---------------------+
+# +    CS               +
+# +---------------------+
+# +    EIP              +
+# +---------------------+
+# +    Error Code       +
+# +---------------------+
+# + EAX / Vector Number +
+# +---------------------+
+# +    EBP              +
+# +---------------------+ <-- EBP
+#
+
+# We need to determine if any extra data was pushed by the exception
+  cmpl    $DEBUG_EXCEPT_DOUBLE_FAULT, %eax
+  je      NoExtrPush
+  cmpl    $DEBUG_EXCEPT_INVALID_TSS, %eax
+  je      NoExtrPush
+  cmpl    $DEBUG_EXCEPT_SEG_NOT_PRESENT, %eax
+  je      NoExtrPush
+  cmpl    $DEBUG_EXCEPT_STACK_FAULT, %eax
+  je      NoExtrPush
+  cmpl    $DEBUG_EXCEPT_GP_FAULT, %eax
+  je      NoExtrPush
+  cmpl    $DEBUG_EXCEPT_PAGE_FAULT, %eax
+  je      NoExtrPush
+  cmpl    $DEBUG_EXCEPT_ALIGNMENT_CHECK, %eax
+  je      NoExtrPush
+
+  pushl    (%esp)
+  movl     $0, 4(%esp)
+
+NoExtrPush:
+
+  pushl   %ebp
+  movl    %esp,%ebp
+
+  #
+  # Align stack to make sure that EFI_FX_SAVE_STATE_IA32 of EFI_SYSTEM_CONTEXT_IA32
+  # is 16-byte aligned
+  #
+  andl    $0xfffffff0,%esp
+  subl    $12,%esp
+
+## UINT32  Edi, Esi, Ebp, Esp, Ebx, Edx, Ecx, Eax;
+  pushl   0x4(%ebp)
+  pushl   %ebx
+  pushl   %ecx
+  pushl   %edx
+  mov     %eax, %ebx                   # save vector in ebx
+  leal    24(%ebp),%ecx
+  pushl   %ecx                         # save original ESP
+  pushl   (%ebp)
+  pushl   %esi
+  pushl   %edi
+
+## UINT32  Cr0, Cr1, Cr2, Cr3, Cr4;
+  movl    %cr4, %eax
+  orl     $0x208,%eax
+  movl    %eax, %cr4
+  pushl   %eax
+  movl    %cr3, %eax
+  pushl   %eax
+  movl    %cr2, %eax
+  pushl   %eax
+  xorl    %eax,%eax
+  pushl   %eax
+  movl    %cr0, %eax
+  pushl   %eax
+
+## UINT32  Gs, Fs, Es, Ds, Cs, Ss;
+  movl    %ss,%eax
+  pushl   %eax
+  movzwl  16(%ebp), %eax
+  pushl   %eax
+  movl    %ds,%eax
+  pushl   %eax
+  movl    %es,%eax
+  pushl   %eax
+  movl    %fs,%eax
+  pushl   %eax
+  movl    %gs,%eax
+  pushl   %eax
+
+## UINT32  Eip;
+  pushl   12(%ebp)
+
+## UINT32  Gdtr[2], Idtr[2];
+  subl    $8,%esp
+  sidt    (%esp)
+  subl    $8,%esp
+  sgdt    (%esp)
+
+## UINT32  Ldtr, Tr;
+  xorl    %eax,%eax
+  strl    %eax
+  pushl   %eax
+  sldtl   %eax
+  pushl   %eax
+
+## UINT32  EFlags;
+  pushl   20(%ebp)
+
+## UINT32  Dr0, Dr1, Dr2, Dr3, Dr6, Dr7;
+  movl    %dr7, %eax
+  pushl   %eax
+## clear Dr7 while executing debugger itself
+  xorl    %eax,%eax
+#  movl    %eax, %dr7
+
+  movl    %dr6, %eax
+  pushl   %eax
+## insure all status bits in dr6 are clear...
+  xorl    %eax,%eax
+  movl    %eax, %dr6
+
+  movl    %dr3, %eax
+  pushl   %eax
+  movl    %dr2, %eax
+  pushl   %eax
+  movl    %dr1, %eax
+  pushl   %eax
+  movl    %dr0, %eax
+  pushl   %eax
+
+## FX_SAVE_STATE_IA32 FxSaveState;
+  subl    $512,%esp
+  movl    %esp,%edi
+  .byte 0x0f, 0xae, 0x07   # fxsave [edi]
+
+## Clear Direction Flag
+  cld
+	
+## Prepare parameter and call C function
+  pushl   %esp
+  pushl   %ebx
+  call    ASM_PFX(InterruptProcess)
+  addl    $8,%esp
+
+## FX_SAVE_STATE_IA32 FxSaveState;
+  movl    %esp,%esi
+  .byte 0x0f, 0xae, 0x0e   # fxrstor [esi]
+  addl    $512,%esp
+
+## UINT32  Dr0, Dr1, Dr2, Dr3, Dr6, Dr7;
+  popl    %eax
+  movl    %eax, %dr0
+  popl    %eax
+  movl    %eax, %dr1
+  popl    %eax
+  movl    %eax, %dr2
+  popl    %eax
+  movl    %eax, %dr3
+## skip restore of dr6.  We cleared dr6 during the context save.
+  addl    $4,%esp
+  popl    %eax
+  movl    %eax, %dr7
+
+## UINT32  EFlags;
+  popl    20(%ebp)
+
+## UINT32  Ldtr, Tr;
+## UINT32  Gdtr[2], Idtr[2];
+## Best not let anyone mess with these particular registers...
+  addl     $24,%esp
+
+## UINT32  Eip;
+   pop     12(%ebp)
+
+## UINT32  Gs, Fs, Es, Ds, Cs, Ss;
+## NOTE - modified segment registers could hang the debugger...  We
+##        could attempt to insulate ourselves against this possibility,
+##        but that poses risks as well.
+##
+  popl    %gs
+  popl    %fs
+  popl    %es
+  popl    %ds
+  popl    16(%ebp)
+  popl    %ss
+
+## UINT32  Cr0, Cr1, Cr2, Cr3, Cr4;
+  popl    %eax
+  movl    %eax, %cr0
+  addl    $4,%esp   # not for Cr1
+  popl    %eax
+  movl    %eax, %cr2
+  popl    %eax
+  movl    %eax, %cr3
+  popl    %eax
+  movl    %eax, %cr4
+
+## UINT32  Edi, Esi, Ebp, Esp, Ebx, Edx, Ecx, Eax;
+  popl    %edi
+  popl    %esi
+  addl    $4,%esp  # not for ebp
+  addl    $4,%esp  # not for esp
+  popl    %edx
+  popl    %ecx
+  popl    %ebx
+  popl    %eax
+
+  movl    %ebp,%esp
+  popl    %ebp
+  addl    $8,%esp   # skip eax
+  iretl
+  
diff --git a/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/Ia32/AsmFuncs.asm b/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/Ia32/AsmFuncs.asm
new file mode 100644
index 0000000000..2d2fa5fc7a
--- /dev/null
+++ b/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/Ia32/AsmFuncs.asm
@@ -0,0 +1,365 @@
+;------------------------------------------------------------------------------
+;
+; Copyright (c) 2010, 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
+; which accompanies this distribution.  The full text of the license may be found at
+; http://opensource.org/licenses/bsd-license.php.
+;
+; THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+; WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+;
+; Module Name:
+;
+;   AsmFuncs.asm
+;
+; Abstract:
+;
+;   Debug interrupt handle functions.
+;
+;------------------------------------------------------------------------------
+
+#include "DebugException.h"
+
+.686p
+.xmm
+.model  flat,c
+
+;
+; InterruptProcess()
+;
+InterruptProcess                 PROTO   C
+
+public    Exception0Handle, TimerInterruptHandle, ExceptionStubHeaderSize
+
+.data
+
+ExceptionStubHeaderSize   DW    Exception1Handle - Exception0Handle
+CommonEntryAddr           DD    CommonEntry
+
+.code
+
+Exception0Handle:
+    cli
+    push    eax
+    mov     eax, 0
+    jmp     dword ptr [CommonEntryAddr]
+Exception1Handle:
+    cli
+    push    eax
+    mov     eax, 1
+    jmp     dword ptr [CommonEntryAddr]
+Exception2Handle:
+    cli
+    push    eax
+    mov     eax, 2
+    jmp     dword ptr [CommonEntryAddr]
+Exception3Handle:
+    cli
+    push    eax
+    mov     eax, 3
+    jmp     dword ptr [CommonEntryAddr]
+Exception4Handle:
+    cli
+    push    eax
+    mov     eax, 4
+    jmp     dword ptr [CommonEntryAddr]
+Exception5Handle:
+    cli
+    push    eax
+    mov     eax, 5
+    jmp     dword ptr [CommonEntryAddr]
+Exception6Handle:
+    cli
+    push    eax
+    mov     eax, 6
+    jmp     dword ptr [CommonEntryAddr]
+Exception7Handle:
+    cli
+    push    eax
+    mov     eax, 7
+    jmp     dword ptr [CommonEntryAddr]
+Exception8Handle:
+    cli
+    push    eax
+    mov     eax, 8
+    jmp     dword ptr [CommonEntryAddr]
+Exception9Handle:
+    cli
+    push    eax
+    mov     eax, 9
+    jmp     dword ptr [CommonEntryAddr]
+Exception10Handle:
+    cli
+    push    eax
+    mov     eax, 10
+    jmp     dword ptr [CommonEntryAddr]
+Exception11Handle:
+    cli
+    push    eax
+    mov     eax, 11
+    jmp     dword ptr [CommonEntryAddr]
+Exception12Handle:
+    cli
+    push    eax
+    mov     eax, 12
+    jmp     dword ptr [CommonEntryAddr]
+Exception13Handle:
+    cli
+    push    eax
+    mov     eax, 13
+    jmp     dword ptr [CommonEntryAddr]
+Exception14Handle:
+    cli
+    push    eax
+    mov     eax, 14
+    jmp     dword ptr [CommonEntryAddr]
+Exception15Handle:
+    cli
+    push    eax
+    mov     eax, 15
+    jmp     dword ptr [CommonEntryAddr]
+Exception16Handle:
+    cli
+    push    eax
+    mov     eax, 16
+    jmp     dword ptr [CommonEntryAddr]
+Exception17Handle:
+    cli
+    push    eax
+    mov     eax, 17
+    jmp     dword ptr [CommonEntryAddr]
+Exception18Handle:
+    cli
+    push    eax
+    mov     eax, 18
+    jmp     dword ptr [CommonEntryAddr]
+Exception19Handle:
+    cli
+    push    eax
+    mov     eax, 19
+    jmp     dword ptr [CommonEntryAddr]
+
+TimerInterruptHandle:
+    cli
+    push    eax
+    mov     eax, 32
+    jmp     dword ptr [CommonEntryAddr]
+
+CommonEntry:
+;
+; +---------------------+
+; +    EFlags           +
+; +---------------------+
+; +    CS               +
+; +---------------------+
+; +    EIP              +
+; +---------------------+
+; +    Error Code       +
+; +---------------------+
+; + EAX / Vector Number +
+; +---------------------+
+; +    EBP              +
+; +---------------------+ <-- EBP
+;
+    cmp     eax, DEBUG_EXCEPT_DOUBLE_FAULT
+    je      NoExtrPush
+    cmp     eax, DEBUG_EXCEPT_INVALID_TSS
+    je      NoExtrPush
+    cmp     eax, DEBUG_EXCEPT_SEG_NOT_PRESENT
+    je      NoExtrPush
+    cmp     eax, DEBUG_EXCEPT_STACK_FAULT
+    je      NoExtrPush
+    cmp     eax, DEBUG_EXCEPT_GP_FAULT
+    je      NoExtrPush
+    cmp     eax, DEBUG_EXCEPT_PAGE_FAULT
+    je      NoExtrPush
+    cmp     eax, DEBUG_EXCEPT_ALIGNMENT_CHECK
+    je      NoExtrPush
+
+    push    [esp]
+    mov     dword ptr [esp + 4], 0
+
+NoExtrPush:
+
+    push    ebp
+    mov     ebp, esp        ; save esp in ebp
+    ;
+    ; Make stack 16-byte alignment to make sure save fxrstor later
+    ;
+    and     esp, 0fffffff0h
+    sub     esp, 12
+
+    ; store UINT32  Edi, Esi, Ebp, Ebx, Edx, Ecx, Eax;
+    push    dword ptr [ebp + 4]  ; original eax
+    push    ebx
+    push    ecx
+    push    edx
+    mov     ebx, eax         ; save vector in ebx
+    mov     eax, ebp
+    add     eax, 4 * 6
+    push    eax              ; original ESP
+    push    dword ptr [ebp]  ; EBP
+    push    esi
+    push    edi
+
+    ;; UINT32  Cr0, Cr1, Cr2, Cr3, Cr4;
+    ;; insure FXSAVE/FXRSTOR is enabled in CR4...
+    ;; ... while we're at it, make sure DE is also enabled...
+    mov     eax, cr4
+    push    eax       ; push cr4 firstly
+    or      eax, 208h
+    mov     cr4, eax
+    mov     eax, cr3
+    push    eax
+    mov     eax, cr2
+    push    eax
+    push    0         ; cr0 will not saved???
+    mov     eax, cr0
+    push    eax
+
+    xor     ecx, ecx
+    mov     ecx, Ss
+    push    ecx
+    mov     ecx, Cs
+    push    ecx
+    mov     ecx, Ds
+    push    ecx
+    mov     ecx, Es
+    push    ecx
+    mov     ecx, Fs
+    push    ecx
+    mov     ecx, Gs
+    push    ecx
+
+    ;; EIP
+    mov     ecx, [ebp + 4 * 3] ; EIP
+    push    ecx
+
+    ;; UINT32  Gdtr[2], Idtr[2];
+    sub  esp, 8
+    sidt fword ptr [esp]
+    sub  esp, 8
+    sgdt fword ptr [esp]
+
+    ;; UINT32  Ldtr, Tr;
+    xor  eax, eax
+    str  ax
+    push eax
+    sldt ax
+    push eax
+
+    ;; EFlags
+    mov     ecx, [ebp + 4 * 5]
+    push    ecx
+
+    ;; UINT32  Dr0, Dr1, Dr2, Dr3, Dr6, Dr7;
+    mov     eax, dr7
+    push    eax
+
+    ;; clear Dr7 while executing debugger itself
+    xor     eax, eax
+ ;; mov     dr7, eax
+
+    ;; Dr6
+    mov     eax, dr6
+    push    eax
+
+    ;; insure all status bits in dr6 are clear...
+    xor     eax, eax
+    mov     dr6, eax
+
+    mov     eax, dr3
+    push    eax
+    mov     eax, dr2
+    push    eax
+    mov     eax, dr1
+    push    eax
+    mov     eax, dr0
+    push    eax
+
+    ;; FX_SAVE_STATE_IA32 FxSaveState;
+    sub esp, 512
+    mov edi, esp
+    db 0fh, 0aeh, 00000111y ;fxsave [edi]
+
+    ;; Clear Direction Flag
+    cld
+    	
+    ; call the C interrupt process function
+    push    esp     ; Structure
+    push    ebx     ; vector
+    call    InterruptProcess
+    add     esp, 8
+
+    ;; FX_SAVE_STATE_IA32 FxSaveState;
+    mov esi, esp
+    db 0fh, 0aeh, 00001110y ; fxrstor [esi]
+    add esp, 512
+
+    ;; UINT32  Dr0, Dr1, Dr2, Dr3, Dr6, Dr7;
+    pop     eax
+    mov     dr0, eax
+    pop     eax
+    mov     dr1, eax
+    pop     eax
+    mov     dr2, eax
+    pop     eax
+    mov     dr3, eax
+    ;; skip restore of dr6.  We cleared dr6 during the context save.
+    add     esp, 4
+    pop     eax
+    mov     dr7, eax
+
+    ;; set EFlags
+    pop     dword ptr [ebp + 4 * 5]  ; set EFLAGS in stack
+
+    ;; UINT32  Ldtr, Tr;
+    ;; UINT32  Gdtr[2], Idtr[2];
+    ;; Best not let anyone mess with these particular registers...
+    add     esp, 24
+
+    ;; UINT32  Eip;
+    pop     dword ptr [ebp + 4 * 3]   ; set EIP in stack
+
+    ;; UINT32  Gs, Fs, Es, Ds, Cs, Ss;
+    ;; NOTE - modified segment registers could hang the debugger...  We
+    ;;        could attempt to insulate ourselves against this possibility,
+    ;;        but that poses risks as well.
+    ;;
+    pop     gs
+    pop     fs
+    pop     es
+    pop     ds
+    pop     dword ptr [ebp + 4 * 4]    ; set CS in stack
+    pop     ss
+
+    ;; UINT32  Cr0, Cr1, Cr2, Cr3, Cr4;
+    pop     eax
+    mov     cr0, eax
+    add     esp, 4    ; skip for Cr1
+    pop     eax
+    mov     cr2, eax
+    pop     eax
+    mov     cr3, eax
+    pop     eax
+    mov     cr4, eax
+
+    ;; restore general register
+    pop     edi
+    pop     esi
+    pop     dword ptr [ebp]         ; save updated ebp
+    pop     dword ptr [ebp + 4]     ; save updated esp
+    pop     edx
+    pop     ecx
+    pop     ebx
+    pop     eax
+
+    mov     esp, ebp
+    pop     ebp         ; restore ebp maybe updated
+    pop     esp         ; restore esp maybe updated
+    sub     esp, 4 * 3  ; restore interupt pushced stack
+
+    iretd
+
+END
diff --git a/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/Ia32/DebugException.h b/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/Ia32/DebugException.h
new file mode 100644
index 0000000000..13a7d23a0d
--- /dev/null
+++ b/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/Ia32/DebugException.h
@@ -0,0 +1,36 @@
+/** @file
+  Exception defintions.
+
+  Copyright (c) 2010, 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
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php.
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _DEBUG_EXCEPTION_H_
+#define _DEBUG_EXCEPTION_H_
+
+#define DEBUG_EXCEPT_DIVIDE_ERROR             0
+#define DEBUG_EXCEPT_DEBUG                    1
+#define DEBUG_EXCEPT_NMI                      2
+#define DEBUG_EXCEPT_BREAKPOINT               3
+#define DEBUG_EXCEPT_OVERFLOW                 4
+#define DEBUG_EXCEPT_BOUND                    5
+#define DEBUG_EXCEPT_INVALID_OPCODE           6
+#define DEBUG_EXCEPT_DOUBLE_FAULT             8
+#define DEBUG_EXCEPT_INVALID_TSS             10
+#define DEBUG_EXCEPT_SEG_NOT_PRESENT         11
+#define DEBUG_EXCEPT_STACK_FAULT             12
+#define DEBUG_EXCEPT_GP_FAULT                13
+#define DEBUG_EXCEPT_PAGE_FAULT              14
+#define DEBUG_EXCEPT_FP_ERROR                16
+#define DEBUG_EXCEPT_ALIGNMENT_CHECK         17
+#define DEBUG_EXCEPT_MACHINE_CHECK           18
+#define DEBUG_EXCEPT_SIMD                    19
+
+#endif
diff --git a/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/X64/ArchDebugSupport.c b/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/X64/ArchDebugSupport.c
new file mode 100644
index 0000000000..3c18b8676f
--- /dev/null
+++ b/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/X64/ArchDebugSupport.c
@@ -0,0 +1,255 @@
+/** @file
+  Supporting functions for x64 architecture.
+
+  Copyright (c) 2010, 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
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php.
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#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);
+}
+
+/**
+  Initialize IDT entries to support source level debug.
+
+**/
+VOID
+InitializeDebugIdt (
+  VOID
+  )
+{
+  IA32_IDT_GATE_DESCRIPTOR   *IdtEntry;
+  UINTN                      InterruptHandler;
+  IA32_DESCRIPTOR            IdtDescriptor;
+  UINTN                      Index;
+  UINT16                     CodeSegment;
+
+  AsmReadIdtr (&IdtDescriptor);
+
+  //
+  // Use current CS as the segment selector of interrupt gate in IDT
+  //
+  CodeSegment = AsmReadCs ();
+
+  IdtEntry = (IA32_IDT_GATE_DESCRIPTOR *) IdtDescriptor.Base;
+
+  for (Index = 0; Index < 20; Index ++) {
+    if ((PcdGet32 (PcdExceptionsIgnoredByDebugger) & (1 << Index)) != 0) {
+      //
+      // If the exception is masked to be reserved, skip it
+      //
+      continue;
+    }
+    InterruptHandler = (UINTN)&Exception0Handle + Index * ExceptionStubHeaderSize;
+    IdtEntry[Index].Bits.OffsetLow       = (UINT16)(UINTN)InterruptHandler;
+    IdtEntry[Index].Bits.OffsetHigh      = (UINT16)((UINTN)InterruptHandler >> 16);
+    IdtEntry[Index].Bits.OffsetUpper     = (UINT32)((UINTN)InterruptHandler >> 32);
+    IdtEntry[Index].Bits.Selector        = CodeSegment;
+    IdtEntry[Index].Bits.GateType        = IA32_IDT_GATE_TYPE_INTERRUPT_32;
+  }
+
+  InterruptHandler = (UINTN) &TimerInterruptHandle;
+  IdtEntry[DEBUG_TIMER_VECTOR].Bits.OffsetLow       = (UINT16)(UINTN)InterruptHandler;
+  IdtEntry[DEBUG_TIMER_VECTOR].Bits.OffsetHigh      = (UINT16)((UINTN)InterruptHandler >> 16);
+  IdtEntry[DEBUG_TIMER_VECTOR].Bits.OffsetUpper     = (UINT32)((UINTN)InterruptHandler >> 32);
+  IdtEntry[DEBUG_TIMER_VECTOR].Bits.Selector        = CodeSegment;
+  IdtEntry[DEBUG_TIMER_VECTOR].Bits.GateType        = IA32_IDT_GATE_TYPE_INTERRUPT_32;
+}
diff --git a/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/X64/ArchDebugSupport.h b/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/X64/ArchDebugSupport.h
new file mode 100644
index 0000000000..0b25a78d13
--- /dev/null
+++ b/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/X64/ArchDebugSupport.h
@@ -0,0 +1,31 @@
+/** @file
+  X64 specific defintions for debug agent library instance.
+
+  Copyright (c) 2010, 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
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php.
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _ARCH_DEBUG_SUPPORT_H_
+#define _ARCH_DEBUG_SUPPORT_H_
+
+#include "ArchRegisters.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_SYSTEM_CONTEXT    DEBUG_CPU_CONTEXT;
+
+#endif
diff --git a/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/X64/ArchReadGroupRegister.c b/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/X64/ArchReadGroupRegister.c
new file mode 100644
index 0000000000..72d30fcf53
--- /dev/null
+++ b/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/X64/ArchReadGroupRegister.c
@@ -0,0 +1,259 @@
+/** @file
+  x64 Group registers read support functions.
+
+  Copyright (c) 2010, 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
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php.
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#include "DebugAgent.h"
+
+/**
+  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
+  )
+{
+  IA32_DESCRIPTOR               *Ia32Descriptor;
+  IA32_GDT                      *Ia32Gdt;
+  UINT16                        Selector;
+  UINT32                        Data32;
+
+  Ia32Descriptor = (IA32_DESCRIPTOR *) CpuContext->Gdtr;
+  Ia32Gdt = (IA32_GDT *) (Ia32Descriptor->Base);
+
+  Selector = 0;
+
+  switch (RegisterIndex) {
+  case SOFT_DEBUGGER_REGISTER_CSAS:
+    Selector = (UINT16) CpuContext->Cs;
+    break;
+  case SOFT_DEBUGGER_REGISTER_SSAS:
+    Selector = (UINT16) CpuContext->Ss;
+    break;
+  case SOFT_DEBUGGER_REGISTER_GSAS:
+    Selector = (UINT16) CpuContext->Gs;
+    break;
+  case SOFT_DEBUGGER_REGISTER_FSAS:
+    Selector = (UINT16) CpuContext->Fs;
+    break;
+  case SOFT_DEBUGGER_REGISTER_ESAS:
+    Selector = (UINT16) CpuContext->Es;
+    break;
+  case SOFT_DEBUGGER_REGISTER_DSAS:
+    Selector = (UINT16) CpuContext->Ds;
+  case SOFT_DEBUGGER_REGISTER_LDTAS:
+  case SOFT_DEBUGGER_REGISTER_TSSAS:
+    return 0x00820000;
+    break;
+  }
+
+  Data32 = (UINT32) RShiftU64 (Ia32Gdt[Selector / 8].Uint64, 24);
+  return (Data32 & (UINT32)(~0xff)) | Selector;
+
+}
+
+/**
+  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
+  )
+{
+  IA32_DESCRIPTOR               *Ia32Descriptor;
+  IA32_GDT                      *Ia32Gdt;
+  UINTN                         Index;
+
+  Ia32Descriptor = (IA32_DESCRIPTOR *) CpuContext->Gdtr;
+  Ia32Gdt = (IA32_GDT *) (Ia32Descriptor->Base);
+
+  Index = CpuContext->Cs / 8;
+  RegisterGroupSegBase->CsBas = (Ia32Gdt[Index].Bits.BaseLow) + (Ia32Gdt[Index].Bits.BaseMid << 16) + (Ia32Gdt[Index].Bits.BaseMid << 24);
+  Index = CpuContext->Ss / 8;
+  RegisterGroupSegBase->SsBas = (Ia32Gdt[Index].Bits.BaseLow) + (Ia32Gdt[Index].Bits.BaseMid << 16) + (Ia32Gdt[Index].Bits.BaseMid << 24);
+  Index = CpuContext->Gs / 8;
+  RegisterGroupSegBase->GsBas = (Ia32Gdt[Index].Bits.BaseLow) + (Ia32Gdt[Index].Bits.BaseMid << 16) + (Ia32Gdt[Index].Bits.BaseMid << 24);
+  Index = CpuContext->Fs / 8;
+  RegisterGroupSegBase->FsBas = (Ia32Gdt[Index].Bits.BaseLow) + (Ia32Gdt[Index].Bits.BaseMid << 16) + (Ia32Gdt[Index].Bits.BaseMid << 24);
+  Index = CpuContext->Es / 8;
+  RegisterGroupSegBase->EsBas = (Ia32Gdt[Index].Bits.BaseLow) + (Ia32Gdt[Index].Bits.BaseMid << 16) + (Ia32Gdt[Index].Bits.BaseMid << 24);
+  Index = CpuContext->Ds / 8;
+  RegisterGroupSegBase->DsBas = (Ia32Gdt[Index].Bits.BaseLow) + (Ia32Gdt[Index].Bits.BaseMid << 16) + (Ia32Gdt[Index].Bits.BaseMid << 24);
+
+  RegisterGroupSegBase->LdtBas = 0;
+  RegisterGroupSegBase->TssBas = 0;
+}
+
+/**
+  Read group 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
+  )
+{
+  IA32_DESCRIPTOR               *Ia32Descriptor;
+  IA32_GDT                      *Ia32Gdt;
+  UINTN                         Index;
+
+  Ia32Descriptor = (IA32_DESCRIPTOR *) CpuContext->Gdtr;
+  Ia32Gdt = (IA32_GDT *) (Ia32Descriptor->Base);
+
+  Index = CpuContext->Cs / 8;
+  RegisterGroupSegLim->CsLim = Ia32Gdt[Index].Bits.LimitLow + (Ia32Gdt[Index].Bits.LimitHigh << 16);
+  if (Ia32Gdt[Index].Bits.Granularity == 1) {
+    RegisterGroupSegLim->CsLim = (RegisterGroupSegLim->CsLim << 12) | 0xfff;
+  }
+
+  Index = CpuContext->Ss / 8;
+  RegisterGroupSegLim->SsLim = Ia32Gdt[Index].Bits.LimitLow + (Ia32Gdt[Index].Bits.LimitHigh << 16);
+  if (Ia32Gdt[Index].Bits.Granularity == 1) {
+    RegisterGroupSegLim->SsLim = (RegisterGroupSegLim->SsLim << 12) | 0xfff;
+  }
+
+  Index = CpuContext->Gs / 8;
+  RegisterGroupSegLim->GsLim = Ia32Gdt[Index].Bits.LimitLow + (Ia32Gdt[Index].Bits.LimitHigh << 16);
+  if (Ia32Gdt[Index].Bits.Granularity == 1) {
+    RegisterGroupSegLim->GsLim = (RegisterGroupSegLim->GsLim << 12) | 0xfff;
+  }
+
+  Index = CpuContext->Fs / 8;
+  RegisterGroupSegLim->FsLim = Ia32Gdt[Index].Bits.LimitLow + (Ia32Gdt[Index].Bits.LimitHigh << 16);
+  if (Ia32Gdt[Index].Bits.Granularity == 1) {
+    RegisterGroupSegLim->FsLim = (RegisterGroupSegLim->FsLim << 12) | 0xfff;
+  }
+
+  Index = CpuContext->Es / 8;
+  RegisterGroupSegLim->EsLim = Ia32Gdt[Index].Bits.LimitLow + (Ia32Gdt[Index].Bits.LimitHigh << 16);
+  if (Ia32Gdt[Index].Bits.Granularity == 1) {
+    RegisterGroupSegLim->EsLim = (RegisterGroupSegLim->EsLim << 12) | 0xfff;
+  }
+
+  Index = CpuContext->Ds / 8;
+  RegisterGroupSegLim->DsLim = Ia32Gdt[Index].Bits.LimitLow + (Ia32Gdt[Index].Bits.LimitHigh << 16);
+  if (Ia32Gdt[Index].Bits.Granularity == 1) {
+    RegisterGroupSegLim->DsLim = (RegisterGroupSegLim->DsLim << 12) | 0xfff;
+  }
+
+  RegisterGroupSegLim->LdtLim = 0xffff;
+  RegisterGroupSegLim->TssLim = 0xffff;
+}
+
+/**
+  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
+  )
+{
+  UINTN                                                    DataN;
+  DEBUG_DATA_REPONSE_READ_REGISTER_GROUP                   RegisterGroup;
+  DEBUG_DATA_REPONSE_READ_REGISTER_GROUP_SEGMENT_BAS_LIM   RegisterGroupBasLim;
+  DEBUG_DATA_REPONSE_READ_REGISTER_GROUP_SEGMENT_BASES_X64 RegisterGroupBases64;
+
+  switch (GroupIndex) {
+  case SOFT_DEBUGGER_REGISTER_GROUP_SEGMENT64:
+    ReadRegisterGroup (CpuContext, &RegisterGroup);
+    SendDataResponsePacket (CpuContext, (UINT8 *) &RegisterGroup, (UINT16) sizeof (DEBUG_DATA_REPONSE_READ_REGISTER_GROUP_SEGMENT));
+    break;
+
+  case SOFT_DEBUGGER_REGISTER_GROUP_SEGMENT_BAS_LIM64:
+    DataN = (UINTN) (CpuContext->Idtr[0] & 0xffff);
+    RegisterGroupBasLim.IdtLim = DataN;
+    DataN = (UINTN) (CpuContext->Gdtr[0] & 0xffff);
+    RegisterGroupBasLim.GdtLim = DataN;
+    DataN = (UINTN) RShiftU64 (CpuContext->Idtr[0], 16);
+    DataN |= (UINTN) LShiftU64 (CpuContext->Idtr[1], sizeof (UINTN) * 8 - 16);
+    RegisterGroupBasLim.IdtBas = DataN;
+    DataN = (UINTN) RShiftU64 (CpuContext->Gdtr[0], 16);
+    DataN |= (UINTN) LShiftU64 (CpuContext->Gdtr[1], sizeof (UINTN) * 8 - 16);
+    RegisterGroupBasLim.GdtBas = DataN;
+
+    ReadRegisterGroupSegLim (CpuContext, (DEBUG_DATA_REPONSE_READ_REGISTER_GROUP_SEGLIM *) &RegisterGroupBasLim.CsLim);
+    ReadRegisterGroupSegBase (CpuContext, (DEBUG_DATA_REPONSE_READ_REGISTER_GROUP_SEGBASE *) &RegisterGroupBasLim.CsBas);
+
+    SendDataResponsePacket (CpuContext, (UINT8 *) &RegisterGroupBasLim, (UINT16) sizeof (DEBUG_DATA_REPONSE_READ_REGISTER_GROUP_SEGMENT_BAS_LIM));
+    break;
+
+  case SOFT_DEBUGGER_REGISTER_GROUP_GP2_64:
+    ReadRegisterGroup (CpuContext, &RegisterGroup);
+    SendDataResponsePacket (CpuContext, (UINT8 *) &RegisterGroup.Eflags, (UINT16) sizeof (DEBUG_DATA_REPONSE_READ_REGISTER_GROUP_GP2));
+    break;
+
+  case SOFT_DEBUGGER_REGISTER_GROUP_GP64:
+    ReadRegisterGroup (CpuContext, &RegisterGroup);
+    SendDataResponsePacket (CpuContext, (UINT8 *) &RegisterGroup.Eax, (UINT16) sizeof (DEBUG_DATA_REPONSE_READ_REGISTER_GROUP_GP));
+    break;
+
+  case SOFT_DEBUGGER_REGISTER_GROUP_DR64:
+    ReadRegisterGroup (CpuContext, &RegisterGroup);
+    SendDataResponsePacket (CpuContext, (UINT8 *) &RegisterGroup.Dr0, (UINT16) sizeof (DEBUG_DATA_REPONSE_READ_REGISTER_GROUP_DR));
+    break;
+
+  case SOFT_DEBUGGER_REGISTER_GROUP_SEGMENT_BASES64:
+    RegisterGroupBases64.Ldtr = (UINT16) CpuContext->Ldtr;
+    RegisterGroupBases64.Tr   = (UINT16) CpuContext->Tr;
+
+    RegisterGroupBases64.Csas = ReadRegisterSelectorByIndex (CpuContext, SOFT_DEBUGGER_REGISTER_CSAS);
+    RegisterGroupBases64.Ssas = ReadRegisterSelectorByIndex (CpuContext, SOFT_DEBUGGER_REGISTER_SSAS);
+    RegisterGroupBases64.Gsas = ReadRegisterSelectorByIndex (CpuContext, SOFT_DEBUGGER_REGISTER_GSAS);
+    RegisterGroupBases64.Fsas = ReadRegisterSelectorByIndex (CpuContext, SOFT_DEBUGGER_REGISTER_FSAS);
+    RegisterGroupBases64.Esas = ReadRegisterSelectorByIndex (CpuContext, SOFT_DEBUGGER_REGISTER_ESAS);
+    RegisterGroupBases64.Dsas = ReadRegisterSelectorByIndex (CpuContext, SOFT_DEBUGGER_REGISTER_DSAS);
+    RegisterGroupBases64.Ldtas = ReadRegisterSelectorByIndex (CpuContext, SOFT_DEBUGGER_REGISTER_LDTAS);
+    RegisterGroupBases64.Tssas = ReadRegisterSelectorByIndex (CpuContext, SOFT_DEBUGGER_REGISTER_TSSAS);
+
+    SendDataResponsePacket (CpuContext, (UINT8 *) &RegisterGroupBases64, (UINT16) sizeof (DEBUG_DATA_REPONSE_READ_REGISTER_GROUP_SEGMENT_BASES_X64));
+    break;
+
+  case SOFT_DEBUGGER_REGISTER_GROUP_CR64:
+    ReadRegisterGroup (CpuContext, &RegisterGroup);
+    SendDataResponsePacket (CpuContext, (UINT8 *) &RegisterGroup.Dr7 + 8, (UINT16) sizeof (DEBUG_DATA_REPONSE_READ_REGISTER_GROUP_CR));
+    break;
+
+  case SOFT_DEBUGGER_REGISTER_GROUP_XMM64:
+    ReadRegisterGroup (CpuContext, &RegisterGroup);
+    SendDataResponsePacket (CpuContext, (UINT8 *) &RegisterGroup.Dr7 + 8 * 6, (UINT16) sizeof (DEBUG_DATA_REPONSE_READ_REGISTER_GROUP_XMM));
+    break;
+
+  default:
+    return RETURN_UNSUPPORTED;
+  }
+
+  return RETURN_SUCCESS;
+}
diff --git a/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/X64/ArchRegisters.h b/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/X64/ArchRegisters.h
new file mode 100644
index 0000000000..e7a5a33089
--- /dev/null
+++ b/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/X64/ArchRegisters.h
@@ -0,0 +1,329 @@
+/** @file
+  X64 register defintions needed by debug transfer protocol.
+
+  Copyright (c) 2010, 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
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php.
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _ARCH_REGISTERS_H_
+#define _ARCH_REGISTERS_H_
+
+///
+/// FXSAVE_STATE (promoted operation)
+/// FP / MMX / XMM registers (see fxrstor instruction definition)
+///
+typedef struct {
+  UINT16  Fcw;
+  UINT16  Fsw;
+  UINT16  Ftw;
+  UINT16  Opcode;
+  UINT64  Rip;
+  UINT64  DataOffset;
+  UINT32  Mxcsr;
+  UINT32  Mxcsr_Mask;
+  UINT8   St0Mm0[10];
+  UINT8   Reserved2[6];
+  UINT8   St1Mm1[10];
+  UINT8   Reserved3[6];
+  UINT8   St2Mm2[10];
+  UINT8   Reserved4[6];
+  UINT8   St3Mm3[10];
+  UINT8   Reserved5[6];
+  UINT8   St4Mm4[10];
+  UINT8   Reserved6[6];
+  UINT8   St5Mm5[10];
+  UINT8   Reserved7[6];
+  UINT8   St6Mm6[10];
+  UINT8   Reserved8[6];
+  UINT8   St7Mm7[10];
+  UINT8   Reserved9[6];
+  UINT8   Xmm0[16];
+  UINT8   Xmm1[16];
+  UINT8   Xmm2[16];
+  UINT8   Xmm3[16];
+  UINT8   Xmm4[16];
+  UINT8   Xmm5[16];
+  UINT8   Xmm6[16];
+  UINT8   Xmm7[16];
+  UINT8   Xmm8[16];
+  UINT8   Xmm9[16];
+  UINT8   Xmm10[16];
+  UINT8   Xmm11[16];
+  UINT8   Xmm12[16];
+  UINT8   Xmm13[16];
+  UINT8   Xmm14[16];
+  UINT8   Xmm15[16];
+  UINT8   Reserved11[6 * 16];
+} DEBUG_DATA_X64_FX_SAVE_STATE;
+
+///
+///  x64 processor context definition
+///
+typedef struct {
+  DEBUG_DATA_X64_FX_SAVE_STATE   FxSaveState;
+  UINT64                         Dr0;
+  UINT64                         Dr1;
+  UINT64                         Dr2;
+  UINT64                         Dr3;
+  UINT64                         Dr6;
+  UINT64                         Dr7;
+  UINT64                         Eflags;
+  UINT64                         Ldtr;
+  UINT64                         Tr;
+  UINT64                         Gdtr[2];
+  UINT64                         Idtr[2];
+  UINT64                         Eip;
+  UINT64                         Gs;
+  UINT64                         Fs;
+  UINT64                         Es;
+  UINT64                         Ds;
+  UINT64                         Cs;
+  UINT64                         Ss;
+  UINT64                         Cr0;
+  UINT64                         Cr1;  /* Reserved */
+  UINT64                         Cr2;
+  UINT64                         Cr3;
+  UINT64                         Cr4;
+  UINT64                         Rdi;
+  UINT64                         Rsi;
+  UINT64                         Rbp;
+  UINT64                         Rsp;
+  UINT64                         Rdx;
+  UINT64                         Rcx;
+  UINT64                         Rbx;
+  UINT64                         Rax;
+  UINT64                         Cr8;
+  UINT64                         R8;
+  UINT64                         R9;
+  UINT64                         R10;
+  UINT64                         R11;
+  UINT64                         R12;
+  UINT64                         R13;
+  UINT64                         R14;
+  UINT64                         R15;
+} DEBUG_DATA_X64_SYSTEM_CONTEXT;
+
+
+///
+///  x64 GROUP register
+///
+typedef struct {
+  UINT16         Cs;
+  UINT16         Ds;
+  UINT16         Es;
+  UINT16         Fs;
+  UINT16         Gs;
+  UINT16         Ss;
+  UINT32         Eflags;
+  UINT64         Rbp;
+  UINT64         Eip;
+  UINT64         Rsp;
+  UINT64         Eax;
+  UINT64         Rbx;
+  UINT64         Rcx;
+  UINT64         Rdx;
+  UINT64         Rsi;
+  UINT64         Rdi;
+  UINT64         R8;
+  UINT64         R9;
+  UINT64         R10;
+  UINT64         R11;
+  UINT64         R12;
+  UINT64         R13;
+  UINT64         R14;
+  UINT64         R15;
+  UINT64         Dr0;
+  UINT64         Dr1;
+  UINT64         Dr2;
+  UINT64         Dr3;
+  UINT64         Dr6;
+  UINT64         Dr7;
+  UINT64         Cr0;
+  UINT64         Cr2;
+  UINT64         Cr3;
+  UINT64         Cr4;
+  UINT64         Cr8;
+  UINT8          Xmm0[16];
+  UINT8          Xmm1[16];
+  UINT8          Xmm2[16];
+  UINT8          Xmm3[16];
+  UINT8          Xmm4[16];
+  UINT8          Xmm5[16];
+  UINT8          Xmm6[16];
+  UINT8          Xmm7[16];
+  UINT8          Xmm8[16];
+  UINT8          Xmm9[16];
+  UINT8          Xmm10[16];
+  UINT8          Xmm11[16];
+  UINT8          Xmm12[16];
+  UINT8          Xmm13[16];
+  UINT8          Xmm14[16];
+  UINT8          Xmm15[16];
+} DEBUG_DATA_REPONSE_READ_REGISTER_GROUP_X64;
+
+///
+///  x64 Segment Limit GROUP register
+///
+typedef struct {
+  UINT64         CsLim;
+  UINT64         SsLim;
+  UINT64         GsLim;
+  UINT64         FsLim;
+  UINT64         EsLim;
+  UINT64         DsLim;
+  UINT64         LdtLim;
+  UINT64         TssLim;
+} DEBUG_DATA_REPONSE_READ_REGISTER_GROUP_SEGLIM_X64;
+
+///
+///  x64 Segment Base GROUP register
+///
+typedef struct {
+  UINT64         CsBas;
+  UINT64         SsBas;
+  UINT64         GsBas;
+  UINT64         FsBas;
+  UINT64         EsBas;
+  UINT64         DsBas;
+  UINT64         LdtBas;
+  UINT64         TssBas;
+} DEBUG_DATA_REPONSE_READ_REGISTER_GROUP_SEGBASE_X64;
+
+///
+///  x64 Segment Base/Limit GROUP register
+///
+typedef struct {
+  UINT64         IdtBas;
+  UINT64         IdtLim;
+  UINT64         GdtBas;
+  UINT64         GdtLim;
+  UINT64         CsLim;
+  UINT64         SsLim;
+  UINT64         GsLim;
+  UINT64         FsLim;
+  UINT64         EsLim;
+  UINT64         DsLim;
+  UINT64         LdtLim;
+  UINT64         TssLim;
+  UINT64         CsBas;
+  UINT64         SsBas;
+  UINT64         GsBas;
+  UINT64         FsBas;
+  UINT64         EsBas;
+  UINT64         DsBas;
+  UINT64         LdtBas;
+  UINT64         TssBas;
+} DEBUG_DATA_REPONSE_READ_REGISTER_GROUP_SEGMENT_BAS_LIM;
+
+///
+///  x64 register GROUP register
+///
+typedef struct {
+  UINT32         Eflags;
+  UINT64         Rbp;
+  UINT64         Eip;
+  UINT64         Rsp;
+} DEBUG_DATA_REPONSE_READ_REGISTER_GROUP_GP2;
+
+///
+///  x64 general register GROUP register
+///
+typedef struct {
+  UINT64         Eax;
+  UINT64         Rbx;
+  UINT64         Rcx;
+  UINT64         Rdx;
+  UINT64         Rsi;
+  UINT64         Rdi;
+  UINT64         R8;
+  UINT64         R9;
+  UINT64         R10;
+  UINT64         R11;
+  UINT64         R12;
+  UINT64         R13;
+  UINT64         R14;
+  UINT64         R15;
+} DEBUG_DATA_REPONSE_READ_REGISTER_GROUP_GP;
+
+///
+///  x64 Segment GROUP register
+///
+typedef struct {
+  UINT16         Cs;
+  UINT16         Ds;
+  UINT16         Es;
+  UINT16         Fs;
+  UINT16         Gs;
+  UINT16         Ss;
+} DEBUG_DATA_REPONSE_READ_REGISTER_GROUP_SEGMENT;
+
+///
+///  x64 Debug Register GROUP register
+///
+typedef struct {
+  UINT64         Dr0;
+  UINT64         Dr1;
+  UINT64         Dr2;
+  UINT64         Dr3;
+  UINT64         Dr6;
+  UINT64         Dr7;
+} DEBUG_DATA_REPONSE_READ_REGISTER_GROUP_DR;
+
+///
+///  x64 Control Register GROUP register
+///
+typedef struct {
+  UINT64         Cr0;
+  UINT64         Cr2;
+  UINT64         Cr3;
+  UINT64         Cr4;
+  UINT64         Cr8;
+} DEBUG_DATA_REPONSE_READ_REGISTER_GROUP_CR;
+
+///
+///  x64 XMM Register GROUP register
+///
+typedef struct {
+  UINT8          Xmm0[16];
+  UINT8          Xmm1[16];
+  UINT8          Xmm2[16];
+  UINT8          Xmm3[16];
+  UINT8          Xmm4[16];
+  UINT8          Xmm5[16];
+  UINT8          Xmm6[16];
+  UINT8          Xmm7[16];
+  UINT8          Xmm8[16];
+  UINT8          Xmm9[16];
+  UINT8          Xmm10[16];
+  UINT8          Xmm11[16];
+  UINT8          Xmm12[16];
+  UINT8          Xmm13[16];
+  UINT8          Xmm14[16];
+  UINT8          Xmm15[16];
+} DEBUG_DATA_REPONSE_READ_REGISTER_GROUP_XMM;
+
+///
+///  x64 Segment Base GROUP register
+///
+typedef struct {
+  UINT16         Ldtr;
+  UINT16         Tr;
+  UINT64         Csas;
+  UINT64         Ssas;
+  UINT64         Gsas;
+  UINT64         Fsas;
+  UINT64         Esas;
+  UINT64         Dsas;
+  UINT64         Ldtas;
+  UINT64         Tssas;
+} DEBUG_DATA_REPONSE_READ_REGISTER_GROUP_SEGMENT_BASES_X64;
+
+
+#endif
diff --git a/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/X64/AsmFuncs.S b/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/X64/AsmFuncs.S
new file mode 100644
index 0000000000..0d78e7d774
--- /dev/null
+++ b/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/X64/AsmFuncs.S
@@ -0,0 +1,401 @@
+#------------------------------------------------------------------------------
+#
+# Copyright (c) 2006 - 2008, 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
+# which accompanies this distribution.  The full text of the license may be found at
+# http://opensource.org/licenses/bsd-license.php.
+#
+# THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+# WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+#
+# Module Name:
+#
+#   AsmFuncs.S
+#
+# Abstract:
+#
+#   Debug interrupt handle functions.
+#
+#------------------------------------------------------------------------------
+
+#include "DebugException.h"
+
+ASM_GLOBAL ASM_PFX(InterruptProcess)
+
+ASM_GLOBAL ASM_PFX(Exception0Handle)
+ASM_GLOBAL ASM_PFX(ExceptionStubHeaderSize)
+ASM_GLOBAL ASM_PFX(TimerInterruptHandle)
+ASM_GLOBAL ASM_PFX(CommonEntry)
+
+.data
+
+ASM_PFX(ExceptionStubHeaderSize):  .word     ASM_PFX(Exception1Handle) - ASM_PFX(Exception0Handle)
+
+
+.text
+
+ASM_PFX(Exception0Handle):
+   cli
+   pushq %rcx
+   mov   $0, %rcx
+   jmp   ASM_PFX(CommonEntry)
+ASM_PFX(Exception1Handle):
+   cli
+   pushq %rcx
+   mov   $1, %rcx
+   jmp   ASM_PFX(CommonEntry)
+ASM_PFX(Exception2Handle):
+   cli
+   pushq %rcx
+   mov   $2, %rcx
+   jmp   ASM_PFX(CommonEntry)
+ASM_PFX(Exception3Handle):
+   cli
+   pushq %rcx
+   mov   $3, %rcx
+   jmp   ASM_PFX(CommonEntry)
+ASM_PFX(Exception4Handle):
+   cli
+   pushq %rcx
+   mov   $4, %rcx
+   jmp   ASM_PFX(CommonEntry)
+ASM_PFX(Exception5Handle):
+   cli
+   pushq %rcx
+   mov   $5, %rcx
+   jmp   ASM_PFX(CommonEntry)
+ASM_PFX(Exception6Handle):
+   cli
+   pushq %rcx
+   mov   $6, %rcx
+   jmp   ASM_PFX(CommonEntry)
+ASM_PFX(Exception7Handle):
+   cli
+   pushq %rcx
+   mov   $7, %rcx
+   jmp   ASM_PFX(CommonEntry)
+ASM_PFX(Exception8Handle):
+   cli
+   pushq %rcx
+   mov   $8, %rcx
+   jmp   ASM_PFX(CommonEntry)
+ASM_PFX(Exception9Handle):
+   cli
+   pushq %rcx
+   mov   $9, %rcx
+   jmp   ASM_PFX(CommonEntry)
+ASM_PFX(Exception10Handle):
+   cli
+   pushq %rcx
+   mov   $10, %rcx
+   jmp   ASM_PFX(CommonEntry)
+ASM_PFX(Exception11Handle):
+   cli
+   pushq %rcx
+   mov   $11, %rcx
+   jmp   ASM_PFX(CommonEntry)
+ASM_PFX(Exception12Handle):
+   cli
+   pushq %rcx
+   mov   $12, %rcx
+   jmp   ASM_PFX(CommonEntry)
+ASM_PFX(Exception13Handle):
+   cli
+   pushq %rcx
+   mov   $13, %rcx
+   jmp   ASM_PFX(CommonEntry)
+ASM_PFX(Exception14Handle):
+   cli
+   pushq %rcx
+   mov   $14, %rcx
+   jmp   ASM_PFX(CommonEntry)
+ASM_PFX(Exception15Handle):
+   cli
+   pushq %rcx
+   mov   $15, %rcx
+   jmp   ASM_PFX(CommonEntry)
+ASM_PFX(Exception16Handle):
+   cli
+   pushq %rcx
+   mov   $16, %rcx
+   jmp   ASM_PFX(CommonEntry)
+ASM_PFX(Exception17Handle):
+   cli
+   pushq %rcx
+   mov   $17, %rcx
+   jmp   ASM_PFX(CommonEntry)
+ASM_PFX(Exception18Handle):
+   cli
+   pushq %rcx
+   mov   $18, %rcx
+   jmp   ASM_PFX(CommonEntry)
+ASM_PFX(Exception19Handle):
+   cli
+   pushq %rcx
+   mov   $19, %rcx
+   jmp   ASM_PFX(CommonEntry)
+
+ASM_PFX(TimerInterruptHandle):
+   cli
+   pushq %rcx
+   mov   $32, %rcx
+   jmp   ASM_PFX(CommonEntry)
+
+
+ASM_PFX(CommonEntry):
+
+#---------------------------------------;
+# CommonInterruptEntry                  ;
+#---------------------------------------;
+# The follow algorithm is used for the common interrupt routine.
+
+#
+# +---------------------+ <-- 16-byte aligned ensured by processor
+# +    Old SS           +
+# +---------------------+
+# +    Old RSP          +
+# +---------------------+
+# +    RFlags           +
+# +---------------------+
+# +    CS               +
+# +---------------------+
+# +    RIP              +
+# +---------------------+
+# +    Error Code       +
+# +---------------------+
+# + RCX / Vector Number +
+# +---------------------+
+# +    RBP              +
+# +---------------------+ <-- RBP, 16-byte aligned
+#
+
+# We need to determine if any extra data was pushed by the exception
+  cmpq    $DEBUG_EXCEPT_DOUBLE_FAULT, %rcx
+  je      NoExtrPush
+  cmpq    $DEBUG_EXCEPT_INVALID_TSS, %rcx
+  je      NoExtrPush
+  cmpq    $DEBUG_EXCEPT_SEG_NOT_PRESENT, %rcx
+  je      NoExtrPush
+  cmpq    $DEBUG_EXCEPT_STACK_FAULT, %rcx
+  je      NoExtrPush
+  cmpq    $DEBUG_EXCEPT_GP_FAULT, %rcx
+  je      NoExtrPush
+  cmpq    $DEBUG_EXCEPT_PAGE_FAULT, %rcx
+  je      NoExtrPush
+  cmpq    $DEBUG_EXCEPT_ALIGNMENT_CHECK, %rcx
+  je      NoExtrPush
+
+  pushq   (%rsp)
+  movq    $0, 8(%rsp)
+
+NoExtrPush:
+  #
+  # All interrupt handlers are invoked through interrupt gates, so
+  # IF flag automatically cleared at the entry point
+  pushq   %rbp
+  movq    %rsp, %rbp
+
+  #
+  # Since here the stack pointer is 16-byte aligned, so
+  # EFI_FX_SAVE_STATE_X64 of EFI_SYSTEM_CONTEXT_x64
+  # is 16-byte aligned
+  #
+
+## UINT64  R8, R9, R10, R11, R12, R13, R14, R15;
+  pushq %r15
+  pushq %r14
+  pushq %r13
+  pushq %r12
+  pushq %r11
+  pushq %r10
+  pushq %r9
+  pushq %r8
+
+  movq  %cr8, %r8
+  pushq %r8
+
+## UINT64  Rdi, Rsi, Rbp, Rsp, Rbx, Rdx, Rcx, Rax;
+  pushq %rax
+  pushq %rbx
+  pushq 8(%rbp)      # original rcx
+  pushq %rdx
+  pushq 48(%rbp)     # original rsp
+  pushq (%rbp)       # original rbp
+  pushq %rsi
+  pushq %rdi
+
+## UINT64  Cr0, Cr1, Cr2, Cr3, Cr4;
+  movq    %cr4, %rax
+  orq     $0x208, %rax
+  movq    %rax, %cr4
+  pushq   %rax
+  movq    %cr3, %rax
+  pushq   %rax
+  movq    %cr2, %rax
+  pushq   %rax
+  xorq    %rax, %rax
+  pushq   %rax
+  movq    %cr0, %rax
+  pushq   %rax
+
+## UINT64  Gs, Fs, Es, Ds, Cs, Ss;  insure high 16 bits of each is zero
+  xorq     %rax, %rax      # set rax to 0
+  movzwq   56(%rbp), %rax
+#  movq     %ss, %rax
+  pushq    %rax
+  movzwq   32(%rbp), %rax
+#  movq     %cs, %rax
+  pushq    %rax
+  movq     %ds, %rax
+  pushq    %rax
+  movq     %es, %rax
+  pushq    %rax
+  movq     %fs, %rax
+  pushq    %rax
+  movq     %gs, %rax
+  pushq    %rax
+
+## UINT64  Rip;
+  pushq    24(%rbp)
+
+## UINT64  Gdtr[2], Idtr[2];
+  subq     $16, %rsp
+  sidt    (%rsp)
+  subq     $16, %rsp
+  sgdt    (%rsp)
+
+## UINT64  Ldtr, Tr;
+  xorq    %rax, %rax
+  strw    %ax
+  pushq   %rax
+  sldtw   %ax
+  pushq   %rax
+
+## UINT64  RFlags;
+  pushq   40(%rbp)
+
+## UINT64  Dr0, Dr1, Dr2, Dr3, Dr6, Dr7;
+   movq    %dr7, %rax
+  pushq   %rax
+## clear Dr7 while executing debugger itself
+   xorq    %rax, %rax
+#debug   movq    %rax, %dr7
+
+   movq    %dr6, %rax
+  pushq    %rax
+## insure all status bits in dr6 are clear...
+   xorq    %rax, %rax
+   movq    %rax, %dr6
+
+   movq    %dr3, %rax
+  pushq    %rax
+   movq    %dr2, %rax
+  pushq    %rax
+   movq    %dr1, %rax
+  pushq    %rax
+   movq    %dr0, %rax
+  pushq    %rax
+
+## FX_SAVE_STATE_X64 FxSaveState;
+   subq    $512, %rsp
+   movq    %rsp, %rdi
+   .byte   0x0f, 0xae, 0b00000111
+
+## Clear Direction Flag
+  cld
+
+## Prepare parameter and call
+#  movq    8(%rbp), %rcx
+   movq    %rsp, %rdx
+   movq    %rcx, %r15   # save vector in r15
+  #
+  # Per X64 calling convention, allocate maximum parameter stack space
+  # and make sure RSP is 16-byte aligned
+  #
+   subq    $(4 * 8), %rsp
+   call    ASM_PFX(InterruptProcess)
+   addq    $(4 * 8), %rsp
+
+## FX_SAVE_STATE_X64 FxSaveState;
+
+   movq    %rsp, %rsi
+   .byte   0x0f, 0xae, 0b00001110
+   addq    $512, %rsp
+
+## UINT64  Dr0, Dr1, Dr2, Dr3, Dr6, Dr7;
+   popq     %rax
+   movq     %rax, %dr0
+   popq     %rax
+   movq     %rax, %dr1
+   popq     %rax
+   movq     %rax, %dr2
+   popq     %rax
+   movq     %rax, %dr3
+## skip restore of dr6.  We cleared dr6 during the context save.
+   addq     $8, %rsp
+   popq     %rax
+   movq     %rax, %dr7
+
+## UINT64  RFlags;
+   popq    40(%rbp)
+
+## UINT64  Ldtr, Tr;
+## UINT64  Gdtr[2], Idtr[2];
+## Best not let anyone mess with these particular registers...
+   addq    $48, %rsp
+
+## UINT64  Rip;
+   popq    24(%rbp)
+
+## UINT64  Gs, Fs, Es, Ds, Cs, Ss;
+   popq     %rax
+  # mov     gs, rax ; not for gs
+   popq     %rax
+  # mov     fs, rax ; not for fs
+  # (X64 will not use fs and gs, so we do not restore it)
+   popq     %rax
+  movw    %rax, %es
+   popq     %rax
+  movw    %rax, %ds
+   popq     32(%rbp)
+   popq     56(%rbp)
+
+## UINT64  Cr0, Cr1, Cr2, Cr3, Cr4, Cr8;
+   popq     %rax
+   movq     %rax, %cr0
+   addq     $8, %rsp
+   popq     %rax
+   movq     %rax, %cr2
+   popq     %rax
+   movq     %rax, %cr3
+   popq     %rax
+   movq     %rax, %cr4
+
+## UINT64  Rdi, Rsi, Rbp, Rsp, Rbx, Rdx, Rcx, Rax;
+## UINT64  R8, R9, R10, R11, R12, R13, R14, R15;
+   popq     %rdi
+   popq     %rsi
+   addq     $8, %rsp
+   addq     $8, %rsp
+   popq     %rdx
+   popq     %rcx
+   popq     %rbx
+   popq     %rax
+
+   popq     %r8
+   movq     %r8, %cr8
+
+   popq     %r8
+   popq     %r9
+   popq     %r10
+   popq     %r11
+   popq     %r12
+   popq     %r13
+   popq     %r14
+   popq     %r15
+
+   movq     %rbp, %rsp
+   popq     %rbp
+   addq     $16,  %rsp
+   iretq
diff --git a/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/X64/AsmFuncs.asm b/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/X64/AsmFuncs.asm
new file mode 100644
index 0000000000..b712c428c8
--- /dev/null
+++ b/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/X64/AsmFuncs.asm
@@ -0,0 +1,364 @@
+;------------------------------------------------------------------------------
+;
+; Copyright (c) 2010, 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
+; which accompanies this distribution.  The full text of the license may be found at
+; http://opensource.org/licenses/bsd-license.php.
+;
+; THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+; WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+;
+; Module Name:
+;
+;   AsmFuncs.asm
+;
+; Abstract:
+;
+;   Debug interrupt handle functions.
+;
+;------------------------------------------------------------------------------
+
+#include "DebugException.h"
+
+
+externdef InterruptProcess:near
+
+data SEGMENT
+
+public          Exception0Handle, TimerInterruptHandle, ExceptionStubHeaderSize
+
+ExceptionStubHeaderSize        dw      Exception1Handle - Exception0Handle ;
+CommonEntryAddr                dq      CommonEntry ;
+
+.code
+
+Exception0Handle:
+    cli
+    push    rcx
+    mov     rcx, 0
+    jmp     qword ptr [CommonEntryAddr]
+Exception1Handle:
+    cli
+    push    rcx
+    mov     rcx, 1
+    jmp     qword ptr [CommonEntryAddr]
+Exception2Handle:
+    cli
+    push    rcx
+    mov     rcx, 2
+    jmp     qword ptr [CommonEntryAddr]
+Exception3Handle:
+    cli
+    push    rcx
+    mov     rcx, 3
+    jmp     qword ptr [CommonEntryAddr]
+Exception4Handle:
+    cli
+    push    rcx
+    mov     rcx, 4
+    jmp     qword ptr [CommonEntryAddr]
+Exception5Handle:
+    cli
+    push    rcx
+    mov     rcx, 5
+    jmp     qword ptr [CommonEntryAddr]
+Exception6Handle:
+    cli
+    push    rcx
+    mov     rcx, 6
+    jmp     qword ptr [CommonEntryAddr]
+Exception7Handle:
+    cli
+    push    rcx
+    mov     rcx, 7
+    jmp     qword ptr [CommonEntryAddr]
+Exception8Handle:
+    cli
+    push    rcx
+    mov     rcx, 8
+    jmp     qword ptr [CommonEntryAddr]
+Exception9Handle:
+    cli
+    push    rcx
+    mov     rcx, 9
+    jmp     qword ptr [CommonEntryAddr]
+Exception10Handle:
+    cli
+    push    rcx
+    mov     rcx, 10
+    jmp     qword ptr [CommonEntryAddr]
+Exception11Handle:
+    cli
+    push    rcx
+    mov     rcx, 11
+    jmp     qword ptr [CommonEntryAddr]
+Exception12Handle:
+    cli
+    push    rcx
+    mov     rcx, 12
+    jmp     qword ptr [CommonEntryAddr]
+Exception13Handle:
+    cli
+    push    rcx
+    mov     rcx, 13
+    jmp     qword ptr [CommonEntryAddr]
+Exception14Handle:
+    cli
+    push    rcx
+    mov     rcx, 14
+    jmp     qword ptr [CommonEntryAddr]
+Exception15Handle:
+    cli
+    push    rcx
+    mov     rcx, 15
+    jmp     qword ptr [CommonEntryAddr]
+Exception16Handle:
+    cli
+    push    rcx
+    mov     rcx, 16
+    jmp     qword ptr [CommonEntryAddr]
+Exception17Handle:
+    cli
+    push    rcx
+    mov     rcx, 17
+    jmp     qword ptr [CommonEntryAddr]
+Exception18Handle:
+    cli
+    push    rcx
+    mov     rcx, 18
+    jmp     qword ptr [CommonEntryAddr]
+Exception19Handle:
+    cli
+    push    rcx
+    mov     rcx, 19
+    jmp     qword ptr [CommonEntryAddr]
+
+TimerInterruptHandle:
+    cli
+    push    rcx
+    mov     rcx, 32
+    jmp     qword ptr [CommonEntryAddr]
+
+CommonEntry:
+    ; We need to determine if any extra data was pushed by the exception
+    cmp     rcx, DEBUG_EXCEPT_DOUBLE_FAULT
+    je      NoExtrPush
+    cmp     rcx, DEBUG_EXCEPT_INVALID_TSS
+    je      NoExtrPush
+    cmp     rcx, DEBUG_EXCEPT_SEG_NOT_PRESENT
+    je      NoExtrPush
+    cmp     rcx, DEBUG_EXCEPT_STACK_FAULT
+    je      NoExtrPush
+    cmp     rcx, DEBUG_EXCEPT_GP_FAULT
+    je      NoExtrPush
+    cmp     rcx, DEBUG_EXCEPT_PAGE_FAULT
+    je      NoExtrPush
+    cmp     rcx, DEBUG_EXCEPT_ALIGNMENT_CHECK
+    je      NoExtrPush
+
+    push    [rsp]
+    mov     qword ptr [rsp + 8], 0
+
+NoExtrPush:
+    push    rbp
+    mov     rbp, rsp
+
+    ; store UINT64  r8, r9, r10, r11, r12, r13, r14, r15;
+    push    r15
+    push    r14
+    push    r13
+    push    r12
+    push    r11
+    push    r10
+    push    r9
+    push    r8
+
+    mov     r8, cr8
+    push    r8
+
+    ; store UINT64  Rdi, Rsi, Rbp, Rsp, Rdx, Rcx, Rbx, Rax;
+    push    rax
+    push    rbx
+    push    qword ptr [rbp + 8]       ; original rcx
+    push    rdx
+    push    qword ptr [rbp + 6 * 8]   ; original rsp
+    push    qword ptr [rbp]           ; original rbp
+    push    rsi
+    push    rdi
+
+    ;; UINT32  Cr0, Cr1, Cr2, Cr3, Cr4;
+    ;; insure FXSAVE/FXRSTOR is enabled in CR4...
+    ;; ... while we're at it, make sure DE is also enabled...
+    mov     rax, cr4
+    or      rax, 208h
+    mov     cr4, rax
+    push    rax
+    mov     rax, cr3
+    push    rax
+    mov     rax, cr2
+    push    rax
+    push    0         ; cr0 will not saved???
+    mov     rax, cr0
+    push    rax
+
+    xor     rax, rax
+    mov     rax, Ss
+    push    rax
+    mov     rax, Cs
+    push    rax
+    mov     rax, Ds
+    push    rax
+    mov     rax, Es
+    push    rax
+    mov     rax, Fs
+    push    rax
+    mov     rax, Gs
+    push    rax
+
+    ;; EIP
+    mov     rax, [rbp + 8 * 3] ; EIP
+    push    rax
+
+    ;; UINT64  Gdtr[2], Idtr[2];
+    sub  rsp, 16
+    sidt fword ptr [rsp]
+    sub  rsp, 16
+    sgdt fword ptr [rsp]
+
+    ;; UINT64  Ldtr, Tr;
+    xor  rax, rax
+    str  ax
+    push rax
+    sldt ax
+    push rax
+
+    ;; EFlags
+    mov     rax, [rbp + 8 * 5]
+    push    rax
+
+    ;; UINT64  Dr0, Dr1, Dr2, Dr3, Dr6, Dr7;
+    mov     rax, dr7
+    push    rax
+
+    ;; clear Dr7 while executing debugger itself
+    xor     rax, rax
+    mov     dr7, rax
+
+    ;; Dr6
+    mov     rax, dr6
+    push    rax
+
+    ;; insure all status bits in dr6 are clear...
+    xor     rax, rax
+    mov     dr6, rax
+
+    mov     rax, dr3
+    push    rax
+    mov     rax, dr2
+    push    rax
+    mov     rax, dr1
+    push    rax
+    mov     rax, dr0
+    push    rax
+
+    sub     rsp, 512
+    mov rdi, rsp
+    db 0fh, 0aeh, 00000111y ;fxsave [rdi]
+
+    ;; Clear Direction Flag
+    cld
+	
+    ; call the C interrupt process function
+    mov     rdx, rsp      ; Structure
+    mov     r15, rcx      ; save vector in r15
+    
+    sub     rsp, 32
+    call    InterruptProcess
+    add     rsp, 32
+    
+    mov     rsi, rsp
+    db 0fh, 0aeh, 00001110y ; fxrstor [rsi]
+    add     rsp, 512
+
+    ;; UINT64  Dr0, Dr1, Dr2, Dr3, Dr6, Dr7;
+    pop     rax
+    mov     dr0, rax
+    pop     rax
+    mov     dr1, rax
+    pop     rax
+    mov     dr2, rax
+    pop     rax
+    mov     dr3, rax
+    ;; skip restore of dr6.  We cleared dr6 during the context save.
+    add     rsp, 8
+    pop     rax
+    mov     dr7, rax
+
+    ;; set EFlags
+    pop     qword ptr [rbp + 8 * 5]
+
+    ;; UINT64  Ldtr, Tr;
+    ;; UINT64  Gdtr[2], Idtr[2];
+    ;; Best not let anyone mess with these particular registers...
+    add     rsp, 24 * 2
+
+    ;; UINT64  Eip;
+    pop     qword ptr [rbp + 8 * 3]   ; set EIP in stack
+
+    ;; UINT64  Gs, Fs, Es, Ds, Cs, Ss;
+    ;; NOTE - modified segment registers could hang the debugger...  We
+    ;;        could attempt to insulate ourselves against this possibility,
+    ;;        but that poses risks as well.
+    ;;
+    pop     rax
+    pop     rax
+    pop     rax
+    mov     es, rax
+    pop     rax
+    mov     ds, rax
+    pop     qword ptr [rbp + 8 * 4]    ; Set CS in stack
+    pop     rax
+    mov     ss, rax
+
+    ;; UINT64  Cr0, Cr1, Cr2, Cr3, Cr4;
+    pop     rax
+    mov     cr0, rax
+    add     rsp, 8    ; skip for Cr1
+    pop     rax
+    mov     cr2, rax
+    pop     rax
+    mov     cr3, rax
+    pop     rax
+    mov     cr4, rax
+
+    ;; restore general register
+    pop    rdi
+    pop    rsi
+    add    rsp, 8  ; skip rbp
+    add    rsp, 8  ; skip rsp
+    pop    rdx
+    pop    rcx
+    pop    rbx
+    pop    rax
+
+    pop    r8
+    mov    cr8, r8
+
+    ; store UINT64  r8, r9, r10, r11, r12, r13, r14, r15;
+    pop     r8
+    pop     r9
+    pop     r10
+    pop     r11
+    pop     r12
+    pop     r13
+    pop     r14
+    pop     r15
+
+    mov     rsp, rbp
+    pop     rbp
+    add     rsp, 16      ; skip rcx and error code
+
+    iretq
+
+END
diff --git a/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/X64/DebugException.h b/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/X64/DebugException.h
new file mode 100644
index 0000000000..13a7d23a0d
--- /dev/null
+++ b/SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/X64/DebugException.h
@@ -0,0 +1,36 @@
+/** @file
+  Exception defintions.
+
+  Copyright (c) 2010, 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
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php.
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _DEBUG_EXCEPTION_H_
+#define _DEBUG_EXCEPTION_H_
+
+#define DEBUG_EXCEPT_DIVIDE_ERROR             0
+#define DEBUG_EXCEPT_DEBUG                    1
+#define DEBUG_EXCEPT_NMI                      2
+#define DEBUG_EXCEPT_BREAKPOINT               3
+#define DEBUG_EXCEPT_OVERFLOW                 4
+#define DEBUG_EXCEPT_BOUND                    5
+#define DEBUG_EXCEPT_INVALID_OPCODE           6
+#define DEBUG_EXCEPT_DOUBLE_FAULT             8
+#define DEBUG_EXCEPT_INVALID_TSS             10
+#define DEBUG_EXCEPT_SEG_NOT_PRESENT         11
+#define DEBUG_EXCEPT_STACK_FAULT             12
+#define DEBUG_EXCEPT_GP_FAULT                13
+#define DEBUG_EXCEPT_PAGE_FAULT              14
+#define DEBUG_EXCEPT_FP_ERROR                16
+#define DEBUG_EXCEPT_ALIGNMENT_CHECK         17
+#define DEBUG_EXCEPT_MACHINE_CHECK           18
+#define DEBUG_EXCEPT_SIMD                    19
+
+#endif
diff --git a/SourceLevelDebugPkg/Library/DebugAgent/DxeDebugAgent/DxeDebugAgentLib.c b/SourceLevelDebugPkg/Library/DebugAgent/DxeDebugAgent/DxeDebugAgentLib.c
new file mode 100644
index 0000000000..cc49dc0a59
--- /dev/null
+++ b/SourceLevelDebugPkg/Library/DebugAgent/DxeDebugAgent/DxeDebugAgentLib.c
@@ -0,0 +1,247 @@
+/** @file
+  Debug Agent library implementition for Dxe Core and Dxr modules.
+
+  Copyright (c) 2010, 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
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php.
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#include "DxeDebugAgentLib.h"
+
+DEBUG_AGENT_MAILBOX          mMailbox;
+
+DEBUG_AGENT_MAILBOX          *mMailboxPointer;
+
+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
+  in system table to store its pointer.
+
+  @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.
+
+**/
+RETURN_STATUS
+EFIAPI
+DxeDebugAgentLibConstructor (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+  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);
+}
+
+/**
+  Get the pointer to Mailbox from the GUIDed HOB.
+
+  @param[in]  HobStart      The starting HOB pointer to search from.
+
+  @return Pointer to Mailbox.
+
+**/
+DEBUG_AGENT_MAILBOX *
+GetMailboxFromHob (
+  IN VOID                  *HobStart
+  )
+{
+  EFI_HOB_GUID_TYPE        *GuidHob;
+
+  GuidHob = GetNextGuidHob (&gEfiDebugAgentGuid, HobStart);
+  if (GuidHob == NULL) {
+    return NULL;
+  }
+
+  return (DEBUG_AGENT_MAILBOX *) (GET_GUID_HOB_DATA(GuidHob));
+}
+
+/**
+  Get Debug Agent Mailbox pointer.
+
+  @return Mailbox pointer.
+
+**/
+DEBUG_AGENT_MAILBOX *
+GetMailboxPointer (
+  VOID
+  )
+{
+  return mMailboxPointer;
+}
+
+/**
+  Get debug port handle.
+
+  @return Debug port handle.
+
+**/
+DEBUG_PORT_HANDLE
+GetDebugPortHandle (
+  VOID
+  )
+{
+  return (DEBUG_PORT_HANDLE) (UINTN)(mMailboxPointer->DebugPortHandle);
+}
+
+ 
+/**
+  Initialize debug agent.
+
+  This function is used to set up debug enviroment for DXE phase.
+
+  If this function is called by DXE Core, Context must be the pointer
+  to HOB list which will be used to get GUIDed HOB. It will enable
+  interrupt to support break-in feature.
+  If this function is called by DXE module, Context must be NULL. It
+  will enable interrupt to support break-in feature.
+
+  @param[in] InitFlag     Init flag is used to decide initialize process.
+  @param[in] Context      Context needed according to InitFlag.
+  @param[in] Function     Continue function called by debug agent library; it was
+                          optional.
+
+**/
+VOID
+EFIAPI
+InitializeDebugAgent (
+  IN UINT32                InitFlag,
+  IN VOID                  *Context, OPTIONAL
+  IN DEBUG_AGENT_CONTINUE  Function  OPTIONAL
+  )
+{
+  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;
+  }
+
+  //
+  // Save and disable original interrupt status
+  //
+  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) {
+
+    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 ();
+    EnableInterrupts ();
+
+    return;
+  } else {
+    //
+    // Disable Debug Timer interrupt in S3 path.
+    //
+    SaveAndSetDebugTimerInterrupt (FALSE);
+
+    //
+    // Restore interrupt state.
+    //
+    SetInterruptState (InterruptStatus);
+  }
+
+}
+
diff --git a/SourceLevelDebugPkg/Library/DebugAgent/DxeDebugAgent/DxeDebugAgentLib.h b/SourceLevelDebugPkg/Library/DebugAgent/DxeDebugAgent/DxeDebugAgentLib.h
new file mode 100644
index 0000000000..7d78c5183b
--- /dev/null
+++ b/SourceLevelDebugPkg/Library/DebugAgent/DxeDebugAgent/DxeDebugAgentLib.h
@@ -0,0 +1,25 @@
+/** @file
+  Header file for Dxe Core Debug Agent Library instance.
+
+  Copyright (c) 2010, 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
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php.
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _DXE_CORE_DEBUG_AGENT_LIB_H_
+#define _DXE_CORE_DEBUG_AGENT_LIB_H_
+
+#include <PiDxe.h>
+
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiLib.h>
+
+#include "DebugAgent.h"
+
+#endif
diff --git a/SourceLevelDebugPkg/Library/DebugAgent/DxeDebugAgentLib.inf b/SourceLevelDebugPkg/Library/DebugAgent/DxeDebugAgentLib.inf
new file mode 100644
index 0000000000..b932437258
--- /dev/null
+++ b/SourceLevelDebugPkg/Library/DebugAgent/DxeDebugAgentLib.inf
@@ -0,0 +1,87 @@
+## @file
+#  Debug Agent library instance for Dxe Core and Dxe modules.
+#
+#  Copyright (c) 2010, 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
+#  which accompanies this distribution. The full text of the license may be found at
+#  http://opensource.org/licenses/bsd-license.php.
+#  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = DxeDebugAgentLib
+  FILE_GUID                      = BA6BAD25-B814-4747-B0B0-0FBB61D40B90
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 0.7
+  LIBRARY_CLASS                  = DebugAgentLib|DXE_CORE DXE_DRIVER
+
+  CONSTRUCTOR                    = DxeDebugAgentLibConstructor
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64
+#
+
+[Sources.common]
+  DxeDebugAgent/DxeDebugAgentLib.c
+  DxeDebugAgent/DxeDebugAgentLib.h
+  DebugAgentCommon/DebugAgent.c
+  DebugAgentCommon/DebugAgent.h
+  DebugAgentCommon/DebugTimer.c
+  DebugAgentCommon/DebugTimer.h
+  DebugAgentCommon/DebugMp.c
+  DebugAgentCommon/DebugMp.h
+
+[Sources.Ia32]
+  DebugAgentCommon/Ia32/AsmFuncs.S     | GCC
+  DebugAgentCommon/Ia32/AsmFuncs.asm   | MSFT
+  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/ArchDebugSupport.h
+  DebugAgentCommon/X64/ArchDebugSupport.c
+  DebugAgentCommon/X64/ArchReadGroupRegister.c
+  DebugAgentCommon/X64/ArchRegisters.h
+  DebugAgentCommon/X64/DebugException.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+  UefiCpuPkg/UefiCpuPkg.dec
+  SourceLevelDebugPkg/SourceLevelDebugPkg.dec
+
+[LibraryClasses]
+  BaseLib
+  BaseMemoryLib
+  ResetSystemLib
+  IoLib
+  HobLib
+  DebugCommunicationLib
+  UefiBootServicesTableLib
+  UefiLib
+  PcdLib
+  SynchronizationLib
+  MemoryAllocationLib
+  LocalApicLib
+
+[Guids]
+  gEfiDebugAgentGuid                            ## PRODUCES ## Configuration Table
+  gEfiDebugAgentGuid                            ## CONSUMES ## HOB
+
+[Pcd]
+  gEfiMdePkgTokenSpaceGuid.PcdFSBClock                                  ## CONSUMES
+  gEfiSourceLevelDebugPkgTokenSpaceGuid.PcdExceptionsIgnoredByDebugger  ## CONSUMES
+
diff --git a/SourceLevelDebugPkg/Library/DebugAgent/SecPeiDebugAgent/SecPeiDebugAgentLib.c b/SourceLevelDebugPkg/Library/DebugAgent/SecPeiDebugAgent/SecPeiDebugAgentLib.c
new file mode 100644
index 0000000000..0d4169ccc7
--- /dev/null
+++ b/SourceLevelDebugPkg/Library/DebugAgent/SecPeiDebugAgent/SecPeiDebugAgentLib.c
@@ -0,0 +1,307 @@
+/** @file
+  SEC Core Debug Agent Library instance implementition.
+
+  Copyright (c) 2010, 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
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php.
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#include "SecPeiDebugAgentLib.h"
+
+CONST BOOLEAN                MultiProcessorDebugSupport = FALSE;
+
+/**
+  Get pointer to Mailbox from IDT entry before memory is ready.
+
+**/
+VOID *
+GetMailboxPointerInIdtEntry (
+  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;
+}
+
+/**
+  Set the pointer of Mailbox into IDT entry before memory is ready.
+
+  @param[in]  Mailbox       The pointer of Mailbox.
+
+**/
+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 (
+  VOID
+  )
+{
+  DEBUG_AGENT_MAILBOX       *Mailbox;
+
+  Mailbox = (DEBUG_AGENT_MAILBOX *) GetMailboxPointerInIdtEntry ();
+
+  return BuildGuidDataHob (
+           &gEfiDebugAgentGuid,
+           Mailbox,
+           sizeof (DEBUG_AGENT_MAILBOX)
+           );
+}
+
+/**
+  Get Debug Agent Mailbox pointer.
+
+  @return Mailbox pointer.
+
+**/
+DEBUG_AGENT_MAILBOX *
+GetMailboxPointer (
+  VOID
+  )
+{
+  return (DEBUG_AGENT_MAILBOX *) GetMailboxPointerInIdtEntry ();
+}
+
+/**
+  Get debug port handle.
+
+  @return Debug port handle.
+
+**/
+DEBUG_PORT_HANDLE
+GetDebugPortHandle (
+  VOID
+  )
+{
+  DEBUG_AGENT_MAILBOX    *DebugAgentMailbox;
+  
+  DebugAgentMailbox = (DEBUG_AGENT_MAILBOX *)GetMailboxPointerInIdtEntry ();
+
+  return (DEBUG_PORT_HANDLE) (UINTN)(DebugAgentMailbox->DebugPortHandle);
+}
+
+/**
+  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);
+
+}
+
+/**
+  Initialize debug agent.
+
+  This function is used to set up debug environment for SEC and PEI phase.
+
+  If InitFlag is DEBUG_AGENT_INIT_PREMEM_SEC, it will overirde IDT table entries
+  and initialize debug port. It will enable interrupt to support break-in feature.
+  It will set up debug agent Mailbox in cache-as-ramfrom. It will be called before
+  physical memory is ready.
+  If InitFlag is DEBUG_AGENT_INIT_POSTMEM_SEC, debug agent will build one GUIDed
+  HOB to copy debug agent Mailbox. It will be called after physical memory is ready.
+
+  This function is used to set up debug environment to support source level debugging.
+  If certain Debug Agent Library instance has to save some private data in the stack,
+  this function must work on the mode that doesn't return to the caller, then
+  the caller needs to wrap up all rest of logic after InitializeDebugAgent() into one
+  function and pass it into InitializeDebugAgent(). InitializeDebugAgent() is
+  responsible to invoke the passing-in function at the end of InitializeDebugAgent().
+
+  If the parameter Function is not NULL, Debug Agent Libary instance will invoke it by
+  passing in the Context to be its parameter.
+
+  If Function() is NULL, Debug Agent Library instance will return after setup debug
+  environment.
+
+  @param[in] InitFlag     Init flag is used to decide the initialize process.
+  @param[in] Context      Context needed according to InitFlag; it was optional.
+  @param[in] Function     Continue function called by debug agent library; it was
+                          optional.
+
+**/
+VOID
+EFIAPI
+InitializeDebugAgent (
+  IN UINT32                InitFlag,
+  IN VOID                  *Context, OPTIONAL
+  IN DEBUG_AGENT_CONTINUE  Function  OPTIONAL
+  )
+{
+  DEBUG_AGENT_MAILBOX              *Mailbox;
+  DEBUG_AGENT_MAILBOX              MailboxInStack;
+  DEBUG_AGENT_PHASE2_CONTEXT       Phase2Context;
+  DEBUG_AGENT_CONTEXT_POSTMEM_SEC  *DebugAgentContext;
+
+  if (InitFlag != DEBUG_AGENT_INIT_PREMEM_SEC &&
+      InitFlag != DEBUG_AGENT_INIT_POSTMEM_SEC) {
+    return;
+  }
+
+  DisableInterrupts ();
+
+  if (InitFlag == DEBUG_AGENT_INIT_POSTMEM_SEC) {
+
+    //
+    // Memory has been ready
+    //
+    if (IsHostConnected()) {
+      //
+      // Trigger one software interrupt to inform HOST
+      //
+      TriggerSoftInterrupt (MEMORY_READY_SIGNATURE);
+    }
+
+    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 (Function != NULL) {
+      Function (Context);
+    }
+
+    return;
+
+  } else {
+
+    InitializeDebugIdt ();
+
+    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);
+
+    InitializeDebugTimer ();
+
+    Phase2Context.Context  = Context;
+    Phase2Context.Function = Function;
+    DebugPortInitialize ((VOID *) &Phase2Context, InitializeDebugAgentPhase2);
+
+    return;
+  }
+}
+
+/**
+  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_PHASE2_CONTEXT *Phase2Context;
+  DEBUG_AGENT_MAILBOX        *Mailbox;
+  EFI_SEC_PEI_HAND_OFF       *SecCoreData;
+
+  Mailbox = GetMailboxPointerInIdtEntry ();
+  Mailbox->DebugPortHandle = (UINT64) (UINTN)DebugPortHandle;
+
+  //
+  // Trigger one software interrupt to inform HOST
+  //
+  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) {
+    TriggerSoftInterrupt (MEMORY_READY_SIGNATURE);
+  }
+
+  //
+  // Enable CPU interrupts so debug timer interrupts can be delivered
+  //
+  EnableInterrupts ();
+
+  //
+  // Call continuation function is it is not NULL.
+  //
+  if (Phase2Context->Function != NULL) {
+    Phase2Context->Function (Phase2Context->Context);
+  }
+}
diff --git a/SourceLevelDebugPkg/Library/DebugAgent/SecPeiDebugAgent/SecPeiDebugAgentLib.h b/SourceLevelDebugPkg/Library/DebugAgent/SecPeiDebugAgent/SecPeiDebugAgentLib.h
new file mode 100644
index 0000000000..9ca5e87fb3
--- /dev/null
+++ b/SourceLevelDebugPkg/Library/DebugAgent/SecPeiDebugAgent/SecPeiDebugAgentLib.h
@@ -0,0 +1,28 @@
+/** @file
+  Header file for Sec Core Debug Agent Library instance.
+
+  Copyright (c) 2010, 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
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php.
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _SEC_CORE_DEBUG_AGENT_LIB_H_
+#define _SEC_CORE_DEBUG_AGENT_LIB_H_
+
+#include <PiPei.h>
+
+#include "DebugAgent.h"
+
+typedef struct {
+  VOID                    *Context;
+  DEBUG_AGENT_CONTINUE    Function;
+} DEBUG_AGENT_PHASE2_CONTEXT;
+
+#endif
+
diff --git a/SourceLevelDebugPkg/Library/DebugAgent/SecPeiDebugAgentLib.inf b/SourceLevelDebugPkg/Library/DebugAgent/SecPeiDebugAgentLib.inf
new file mode 100644
index 0000000000..e2b8ad60af
--- /dev/null
+++ b/SourceLevelDebugPkg/Library/DebugAgent/SecPeiDebugAgentLib.inf
@@ -0,0 +1,81 @@
+## @file
+#  Debug Agent library instance for SEC Core and PEI modules.
+#
+#  Copyright (c) 2010, 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
+#  which accompanies this distribution. The full text of the license may be found at
+#  http://opensource.org/licenses/bsd-license.php.
+#  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = SecPeiDebugAgentLib
+  FILE_GUID                      = 508B7D59-CD4E-4a6b-A45B-6D3B2D90111E
+  MODULE_TYPE                    = PEIM
+  VERSION_STRING                 = 0.7
+  LIBRARY_CLASS                  = DebugAgentLib|SEC PEIM
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64
+#
+
+[Sources.common]
+  SecPeiDebugAgent/SecPeiDebugAgentLib.c
+  SecPeiDebugAgent/SecPeiDebugAgentLib.h
+  DebugAgentCommon/DebugAgent.c
+  DebugAgentCommon/DebugAgent.h
+  DebugAgentCommon/DebugTimer.c
+  DebugAgentCommon/DebugTimer.h
+  DebugAgentCommon/DebugMp.c
+  DebugAgentCommon/DebugMp.h
+
+[Sources.Ia32]
+  DebugAgentCommon/Ia32/AsmFuncs.S     | GCC
+  DebugAgentCommon/Ia32/AsmFuncs.asm   | MSFT
+  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/ArchDebugSupport.h
+  DebugAgentCommon/X64/ArchDebugSupport.c
+  DebugAgentCommon/X64/ArchReadGroupRegister.c
+  DebugAgentCommon/X64/ArchRegisters.h
+  DebugAgentCommon/X64/DebugException.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+  UefiCpuPkg/UefiCpuPkg.dec
+  SourceLevelDebugPkg/SourceLevelDebugPkg.dec
+
+[LibraryClasses]
+  BaseLib
+  BaseMemoryLib
+  ResetSystemLib
+  IoLib
+  HobLib
+  PcdLib
+  DebugCommunicationLib
+  SynchronizationLib
+  LocalApicLib
+
+[Guids]
+  gEfiDebugAgentGuid                            ## PRODUCES ## HOB
+
+[Pcd]
+  gEfiMdePkgTokenSpaceGuid.PcdFSBClock                                  ## CONSUMES
+  gEfiSourceLevelDebugPkgTokenSpaceGuid.PcdExceptionsIgnoredByDebugger  ## CONSUMES
+
diff --git a/SourceLevelDebugPkg/Library/DebugAgent/SmmDebugAgent/SmmDebugAgentLib.c b/SourceLevelDebugPkg/Library/DebugAgent/SmmDebugAgent/SmmDebugAgentLib.c
new file mode 100644
index 0000000000..fa42311659
--- /dev/null
+++ b/SourceLevelDebugPkg/Library/DebugAgent/SmmDebugAgent/SmmDebugAgentLib.c
@@ -0,0 +1,157 @@
+/** @file
+  Debug Agent library implementition.
+
+  Copyright (c) 2010, 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
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php.
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#include "SmmDebugAgentLib.h"
+
+DEBUG_AGENT_MAILBOX         *mMailboxPointer = NULL;
+
+GLOBAL_REMOVE_IF_UNREFERENCED DEBUG_AGENT_MAILBOX         mLocalMailbox;
+
+GLOBAL_REMOVE_IF_UNREFERENCED UINTN    mSavedDebugRegisters[6];
+
+CONST BOOLEAN                    MultiProcessorDebugSupport = FALSE;
+
+
+/**
+  Get Debug Agent Mailbox pointer.
+
+  @return Mailbox pointer.
+
+**/
+DEBUG_AGENT_MAILBOX *
+GetMailboxPointer (
+  VOID
+  )
+{
+  return mMailboxPointer;
+}
+
+/**
+  Get debug port handle.
+
+  @return Debug port handle.
+
+**/
+DEBUG_PORT_HANDLE
+GetDebugPortHandle (
+  VOID
+  )
+{
+  return (DEBUG_PORT_HANDLE) (UINTN)(mMailboxPointer->DebugPortHandle);
+}
+
+/**
+  Store debug register when SMI exit.
+
+**/
+VOID
+SaveDebugRegister (
+  VOID
+  )
+{
+  mSavedDebugRegisters[0] = AsmReadDr0 ();
+  mSavedDebugRegisters[1] = AsmReadDr1 ();
+  mSavedDebugRegisters[2] = AsmReadDr2 ();
+  mSavedDebugRegisters[3] = AsmReadDr3 ();
+  mSavedDebugRegisters[4] = AsmReadDr6 ();
+  mSavedDebugRegisters[5] = AsmReadDr7 ();
+}
+
+/**
+  Restore debug register when SMI exit.
+
+**/
+VOID
+RestoreDebugRegister (
+  VOID
+  )
+{
+  AsmWriteDr7 (0);
+  AsmWriteDr0 (mSavedDebugRegisters[0]);
+  AsmWriteDr1 (mSavedDebugRegisters[1]);
+  AsmWriteDr2 (mSavedDebugRegisters[2]);
+  AsmWriteDr3 (mSavedDebugRegisters[3]);
+  AsmWriteDr6 (mSavedDebugRegisters[4]);
+  AsmWriteDr7 (mSavedDebugRegisters[5]);
+}
+
+/**
+  Initialize debug agent.
+
+  This function is used to set up debug enviroment for source level debug
+  in SMM code.
+
+  If InitFlag is DEBUG_AGENT_INIT_SMM, it will overirde IDT table entries
+  and initialize debug port. It will get debug agent Mailbox from GUIDed HOB,
+  it it exists, debug agent wiil copied it into the local Mailbox in SMM space.
+  it will overirde IDT table entries and initialize debug port. Context will be
+  NULL.
+  If InitFlag is DEBUG_AGENT_INIT_ENTER_SMI, debug agent will save Debug
+  Registers and get local Mailbox in SMM space. Context will be NULL.
+  If InitFlag is DEBUG_AGENT_INIT_EXIT_SMI, debug agent will restore Debug
+  Registers. Context will be NULL.
+
+  @param[in] InitFlag     Init flag is used to decide initialize process.
+  @param[in] Context      Context needed according to InitFlag.
+  @param[in] Function     Continue function called by debug agent library; it was
+                          optional.
+
+**/
+VOID
+EFIAPI
+InitializeDebugAgent (
+  IN UINT32                InitFlag,
+  IN VOID                  *Context, OPTIONAL
+  IN DEBUG_AGENT_CONTINUE  Function  OPTIONAL
+  )
+{
+  EFI_STATUS               Status;
+  UINT64                   DebugPortHandle;
+
+  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;
+    }
+
+    break;
+
+  case DEBUG_AGENT_INIT_ENTER_SMI:
+    SaveDebugRegister ();
+    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) {
+        //
+        // If SMM entry break is set, SMM code will be break at here.
+        //
+        CpuBreakpoint ();
+      }
+    }
+    break;
+
+  case DEBUG_AGENT_INIT_EXIT_SMI:
+    RestoreDebugRegister ();
+    break;
+  }
+}
+
diff --git a/SourceLevelDebugPkg/Library/DebugAgent/SmmDebugAgent/SmmDebugAgentLib.h b/SourceLevelDebugPkg/Library/DebugAgent/SmmDebugAgent/SmmDebugAgentLib.h
new file mode 100644
index 0000000000..ccb55f830d
--- /dev/null
+++ b/SourceLevelDebugPkg/Library/DebugAgent/SmmDebugAgent/SmmDebugAgentLib.h
@@ -0,0 +1,24 @@
+/** @file
+  Header file for Smm Debug Agent Library instance.
+
+  Copyright (c) 2010, 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
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php.
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _SMM_DEBUG_AGENT_LIB_H_
+#define _SMM_DEBUG_AGENT_LIB_H_
+
+#include <PiDxe.h>
+
+#include <Library/UefiLib.h>
+
+#include "DebugAgent.h"
+
+#endif
diff --git a/SourceLevelDebugPkg/Library/DebugAgent/SmmDebugAgentLib.inf b/SourceLevelDebugPkg/Library/DebugAgent/SmmDebugAgentLib.inf
new file mode 100644
index 0000000000..9dfdec1d32
--- /dev/null
+++ b/SourceLevelDebugPkg/Library/DebugAgent/SmmDebugAgentLib.inf
@@ -0,0 +1,81 @@
+## @file
+#  Debug Agent library instance for SMM modules.
+#
+#  Copyright (c) 2010, 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
+#  which accompanies this distribution. The full text of the license may be found at
+#  http://opensource.org/licenses/bsd-license.php.
+#  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = SecDebugAgentLib
+  FILE_GUID                      = CB07D74C-598F-4268-A5D1-644FB4A481E8
+  MODULE_TYPE                    = DXE_SMM_DRIVER
+  VERSION_STRING                 = 0.7
+  LIBRARY_CLASS                  = DebugAgentLib|DXE_SMM_DRIVER
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64
+#
+
+[Sources.common]
+  SmmDebugAgent/SmmDebugAgentLib.c
+  SmmDebugAgent/SmmDebugAgentLib.h
+  DebugAgentCommon/DebugAgent.c
+  DebugAgentCommon/DebugAgent.h
+  DebugAgentCommon/DebugTimer.c
+  DebugAgentCommon/DebugTimer.h
+  DebugAgentCommon/DebugMp.c
+  DebugAgentCommon/DebugMp.h
+
+[Sources.Ia32]
+  DebugAgentCommon/Ia32/AsmFuncs.S     | GCC
+  DebugAgentCommon/Ia32/AsmFuncs.asm   | MSFT
+  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/ArchDebugSupport.h
+  DebugAgentCommon/X64/ArchDebugSupport.c
+  DebugAgentCommon/X64/ArchReadGroupRegister.c
+  DebugAgentCommon/X64/ArchRegisters.h
+  DebugAgentCommon/X64/DebugException.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+  UefiCpuPkg/UefiCpuPkg.dec
+  SourceLevelDebugPkg/SourceLevelDebugPkg.dec
+
+[LibraryClasses]
+  BaseLib
+  BaseMemoryLib
+  ResetSystemLib
+  IoLib
+  DebugCommunicationLib
+  UefiLib
+  PcdLib
+  SynchronizationLib
+  LocalApicLib
+
+[Guids]
+  gEfiDebugAgentGuid                            ## CONSUMES ## Configuration Table
+
+[Pcd]
+  gEfiMdePkgTokenSpaceGuid.PcdFSBClock                                  ## CONSUMES
+  gEfiSourceLevelDebugPkgTokenSpaceGuid.PcdExceptionsIgnoredByDebugger  ## CONSUMES
+
-- 
cgit v1.2.3