[mirror_edk2.git] / EmbeddedPkg / Library / GdbDebugAgent / Ia32 / Processor.c

/** @file\r
  Processor specific parts of the GDB stub\r
\r
  Copyright (c) 2008 - 2009, Apple Inc. All rights reserved.<BR>\r
  \r
  This program and the accompanying materials\r
  are licensed and made available under the terms and conditions of the BSD License\r
  which accompanies this distribution.  The full text of the license may be found at\r
  http://opensource.org/licenses/bsd-license.php\r
\r
  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,\r
  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.\r
\r
**/\r
\r
#include <GdbDebugAgent.h>\r
\r
//\r
// Array of exception types that need to be hooked by the debugger\r
// {EFI mapping, GDB mapping}\r
//\r
EFI_EXCEPTION_TYPE_ENTRY gExceptionType[] = {\r
  { EXCEPT_IA32_DIVIDE_ERROR,     GDB_SIGFPE  }, \r
  { EXCEPT_IA32_DEBUG,            GDB_SIGTRAP },\r
  { EXCEPT_IA32_NMI,              GDB_SIGEMT  },\r
  { EXCEPT_IA32_BREAKPOINT,       GDB_SIGTRAP },\r
  { EXCEPT_IA32_OVERFLOW,         GDB_SIGSEGV },\r
  { EXCEPT_IA32_BOUND,            GDB_SIGSEGV },\r
  { EXCEPT_IA32_INVALID_OPCODE,   GDB_SIGILL  },\r
  { EXCEPT_IA32_DOUBLE_FAULT,     GDB_SIGEMT  },\r
  { EXCEPT_IA32_STACK_FAULT,      GDB_SIGSEGV },\r
  { EXCEPT_IA32_GP_FAULT,         GDB_SIGSEGV },\r
  { EXCEPT_IA32_PAGE_FAULT,       GDB_SIGSEGV },\r
  { EXCEPT_IA32_FP_ERROR,         GDB_SIGEMT  },\r
  { EXCEPT_IA32_ALIGNMENT_CHECK,  GDB_SIGEMT  },\r
  { EXCEPT_IA32_MACHINE_CHECK,    GDB_SIGEMT  }\r
};\r
\r
\r
// The offsets of registers SystemContext.\r
// The fields in the array are in the gdb ordering.\r
//\r
//16 regs\r
UINTN gRegisterOffsets[] = {\r
  OFFSET_OF(EFI_SYSTEM_CONTEXT_IA32, Eax),\r
  OFFSET_OF(EFI_SYSTEM_CONTEXT_IA32, Ecx),\r
  OFFSET_OF(EFI_SYSTEM_CONTEXT_IA32, Edx),\r
  OFFSET_OF(EFI_SYSTEM_CONTEXT_IA32, Ebx),\r
  OFFSET_OF(EFI_SYSTEM_CONTEXT_IA32, Esp),\r
  OFFSET_OF(EFI_SYSTEM_CONTEXT_IA32, Ebp),\r
  OFFSET_OF(EFI_SYSTEM_CONTEXT_IA32, Esi),\r
  OFFSET_OF(EFI_SYSTEM_CONTEXT_IA32, Edi),\r
  OFFSET_OF(EFI_SYSTEM_CONTEXT_IA32, Eip),\r
  OFFSET_OF(EFI_SYSTEM_CONTEXT_IA32, Eflags),\r
  OFFSET_OF(EFI_SYSTEM_CONTEXT_IA32, Cs),\r
  OFFSET_OF(EFI_SYSTEM_CONTEXT_IA32, Ss),\r
  OFFSET_OF(EFI_SYSTEM_CONTEXT_IA32, Ds),\r
  OFFSET_OF(EFI_SYSTEM_CONTEXT_IA32, Es),\r
  OFFSET_OF(EFI_SYSTEM_CONTEXT_IA32, Fs),\r
  OFFSET_OF(EFI_SYSTEM_CONTEXT_IA32, Gs)\r
};\r
\r
\r
//Debug only..\r
VOID \r
PrintReg (\r
  IN EFI_SYSTEM_CONTEXT SystemContext\r
  )\r
{\r
  Print ((CHAR16 *)L"EAX: %x ", SystemContext.SystemContextIa32->Eax);\r
  Print ((CHAR16 *)L"ECX: %x ", SystemContext.SystemContextIa32->Ecx);\r
  Print ((CHAR16 *)L"EDX: %x ", SystemContext.SystemContextIa32->Edx);\r
  Print ((CHAR16 *)L"EBX: %x ", SystemContext.SystemContextIa32->Ebx);\r
  Print ((CHAR16 *)L"ESP: %x ", SystemContext.SystemContextIa32->Esp);\r
  Print ((CHAR16 *)L"EBP: %x ", SystemContext.SystemContextIa32->Ebp);\r
  Print ((CHAR16 *)L"ESI: %x ", SystemContext.SystemContextIa32->Esi);\r
  Print ((CHAR16 *)L"EDI: %x ", SystemContext.SystemContextIa32->Edi);\r
  Print ((CHAR16 *)L"EIP: %x\n", SystemContext.SystemContextIa32->Eip);\r
  Print ((CHAR16 *)L"EFlags: %x\n", SystemContext.SystemContextIa32->Eflags);\r
}\r
\r
//Debug only..\r
VOID \r
PrintDRreg (\r
  IN EFI_SYSTEM_CONTEXT SystemContext\r
  )\r
{\r
  Print ((CHAR16 *)L"DR0: %x ", SystemContext.SystemContextIa32->Dr0);\r
  Print ((CHAR16 *)L"DR1: %x ", SystemContext.SystemContextIa32->Dr1);\r
  Print ((CHAR16 *)L"DR2: %x ", SystemContext.SystemContextIa32->Dr2);\r
  Print ((CHAR16 *)L"DR3: %x ", SystemContext.SystemContextIa32->Dr3);\r
  Print ((CHAR16 *)L"DR6: %x ", SystemContext.SystemContextIa32->Dr6);\r
  Print ((CHAR16 *)L"DR7: %x\n", SystemContext.SystemContextIa32->Dr7);\r
}\r
\r
\r
/**\r
 Return the number of entries in the gExceptionType[]\r
 \r
 @retval  UINTN, the number of entries in the gExceptionType[] array.    \r
 **/\r
UINTN\r
MaxEfiException (\r
  VOID\r
  )\r
{\r
  return sizeof (gExceptionType)/sizeof (EFI_EXCEPTION_TYPE_ENTRY);\r
}\r
\r
\r
/**\r
  Check to see if the ISA is supported. \r
  ISA = Instruction Set Architecture\r
 \r
  @retval TRUE if Isa is supported,\r
      FALSE otherwise.\r
**/\r
BOOLEAN\r
CheckIsa (\r
  IN  EFI_INSTRUCTION_SET_ARCHITECTURE  Isa\r
  )\r
{\r
  return (BOOLEAN)(Isa == IsaIa32);\r
}\r
\r
\r
/**\r
 This takes in the register number and the System Context, and returns a pointer to the RegNumber-th register in gdb ordering\r
 It is, by default, set to find the register pointer of the IA32 member\r
\r
 @param   SystemContext     Register content at time of the exception \r
 @param   RegNumber       The register to which we want to find a pointer\r
 @retval  the pointer to the RegNumber-th pointer\r
 **/\r
UINTN *\r
FindPointerToRegister(\r
  IN  EFI_SYSTEM_CONTEXT  SystemContext,\r
  IN  UINTN               RegNumber  \r
  )\r
{\r
  UINT8 *TempPtr;\r
  TempPtr = ((UINT8 *)SystemContext.SystemContextIa32) + gRegisterOffsets[RegNumber];\r
  return (UINTN *)TempPtr;\r
}\r
\r
\r
/**\r
 Adds the RegNumber-th register's value to the output buffer, starting at the given OutBufPtr\r
\r
 @param SystemContext     Register content at time of the exception\r
 @param   RegNumber       the number of the register that we want to read\r
 @param   OutBufPtr       pointer to the output buffer's end. the new data will be added from this point on.\r
 @retval  the pointer to the next character of the output buffer that is available to be written on.\r
 **/\r
CHAR8 *\r
BasicReadRegister (\r
  IN  EFI_SYSTEM_CONTEXT      SystemContext,\r
  IN  UINTN           RegNumber,\r
  IN  CHAR8           *OutBufPtr\r
  )\r
{\r
  UINTN RegSize;\r
  \r
  RegSize = 0;\r
  while (RegSize < REG_SIZE) {\r
    *OutBufPtr++ = mHexToStr[((*FindPointerToRegister(SystemContext, RegNumber) >> (RegSize+4)) & 0xf)];\r
    *OutBufPtr++ = mHexToStr[((*FindPointerToRegister(SystemContext, RegNumber) >> RegSize) & 0xf)];\r
    RegSize = RegSize + 8;\r
  }\r
  return OutBufPtr;\r
}\r
\r
\r
/** ‘p n’ \r
 Reads the n-th register's value into an output buffer and sends it as a packet \r
\r
 @param   SystemContext   Register content at time of the exception\r
 @param   InBuffer      Pointer to the input buffer received from gdb server\r
 **/\r
VOID\r
EFIAPI\r
ReadNthRegister (\r
  IN  EFI_SYSTEM_CONTEXT   SystemContext,\r
  IN  CHAR8                *InBuffer\r
  )\r
{\r
  UINTN RegNumber;\r
  CHAR8 OutBuffer[9]; // 1 reg=8 hex chars, and the end '\0' (escape seq)\r
  CHAR8 *OutBufPtr;   // pointer to the output buffer\r
  \r
  RegNumber = AsciiStrHexToUintn (&InBuffer[1]);\r
\r
  if ((RegNumber < 0) || (RegNumber >= sizeof (gRegisterOffsets)/sizeof (UINTN))) {\r
    SendError (GDB_EINVALIDREGNUM);\r
    return;\r
  }\r
\r
  OutBufPtr = OutBuffer;\r
  OutBufPtr = BasicReadRegister(SystemContext, RegNumber, OutBufPtr);\r
\r
  *OutBufPtr = '\0';  // the end of the buffer\r
  SendPacket(OutBuffer);\r
}\r
\r
\r
/** ‘g’ \r
 Reads the general registers into an output buffer  and sends it as a packet \r
\r
 @param   SystemContext     Register content at time of the exception\r
 **/\r
VOID\r
EFIAPI\r
ReadGeneralRegisters (  \r
  IN  EFI_SYSTEM_CONTEXT      SystemContext\r
  )\r
{\r
  UINTN   i;\r
  CHAR8 OutBuffer[129]; // 16 regs, 8 hex chars each, and the end '\0' (escape seq)\r
  CHAR8 *OutBufPtr;   // pointer to the output buffer\r
\r
  OutBufPtr = OutBuffer;\r
  for(i = 0 ; i < sizeof (gRegisterOffsets)/sizeof (UINTN) ; i++) {  // there are only 16 registers to read   \r
    OutBufPtr = BasicReadRegister(SystemContext, i, OutBufPtr);\r
  }\r
\r
  *OutBufPtr = '\0';  // the end of the buffer\r
  SendPacket(OutBuffer);\r
}\r
\r
\r
/**\r
 Adds the RegNumber-th register's value to the output buffer, starting at the given OutBufPtr\r
\r
 @param   SystemContext       Register content at time of the exception\r
 @param   RegNumber         the number of the register that we want to write\r
 @param   InBufPtr          pointer to the output buffer. the new data will be extracted from the input buffer from this point on.\r
 @retval  the pointer to the next character of the input buffer that can be used\r
 **/\r
CHAR8 *\r
BasicWriteRegister (\r
  IN  EFI_SYSTEM_CONTEXT      SystemContext,\r
  IN  UINTN           RegNumber,\r
  IN  CHAR8           *InBufPtr\r
  )\r
{\r
  UINTN RegSize;\r
  UINTN TempValue; // the value transferred from a hex char\r
  UINT32 NewValue; // the new value of the RegNumber-th Register\r
  \r
  NewValue = 0;\r
  RegSize = 0;\r
  while (RegSize < REG_SIZE) {\r
    TempValue = HexCharToInt(*InBufPtr++);\r
    \r
   if (TempValue < 0) {\r
      SendError (GDB_EBADMEMDATA); \r
      return NULL;\r
    }\r
\r
    NewValue += (TempValue << (RegSize+4));\r
    TempValue = HexCharToInt(*InBufPtr++);\r
    \r
    if (TempValue < 0) {\r
      SendError (GDB_EBADMEMDATA); \r
      return NULL;\r
    }\r
    \r
    NewValue += (TempValue << RegSize); \r
    RegSize = RegSize + 8;\r
  }\r
  *(FindPointerToRegister(SystemContext, RegNumber)) = NewValue;\r
  return InBufPtr;\r
}\r
\r
\r
/** ‘P n...=r...’\r
 Writes the new value of n-th register received into the input buffer to the n-th register\r
\r
 @param   SystemContext   Register content at time of the exception\r
 @param   InBuffer      Ponter to the input buffer received from gdb server\r
 **/\r
VOID\r
EFIAPI\r
WriteNthRegister (\r
  IN  EFI_SYSTEM_CONTEXT      SystemContext,\r
  IN  CHAR8           *InBuffer\r
  )\r
{\r
  UINTN RegNumber;\r
  CHAR8 RegNumBuffer[MAX_REG_NUM_BUF_SIZE];  // put the 'n..' part of the message into this array\r
  CHAR8 *RegNumBufPtr;\r
  CHAR8 *InBufPtr; // pointer to the input buffer\r
  \r
  // find the register number to write\r
  InBufPtr = &InBuffer[1];\r
  RegNumBufPtr = RegNumBuffer;\r
  while (*InBufPtr != '=') {\r
    *RegNumBufPtr++ = *InBufPtr++;\r
  } \r
  *RegNumBufPtr = '\0';\r
  RegNumber = AsciiStrHexToUintn (RegNumBuffer); \r
\r
  // check if this is a valid Register Number\r
  if ((RegNumber < 0) || (RegNumber >= sizeof (gRegisterOffsets)/sizeof (UINTN))) {\r
	SendError (GDB_EINVALIDREGNUM); \r
    return;\r
  }\r
  InBufPtr++;  // skips the '=' character\r
  BasicWriteRegister (SystemContext, RegNumber, InBufPtr);\r
  SendSuccess();\r
}\r
\r
\r
/** ‘G XX...’\r
 Writes the new values received into the input buffer to the general registers\r
\r
 @param   SystemContext       Register content at time of the exception\r
 @param   InBuffer          Pointer to the input buffer received from gdb server\r
 **/\r
VOID\r
EFIAPI\r
WriteGeneralRegisters (\r
  IN  EFI_SYSTEM_CONTEXT        SystemContext,\r
  IN  CHAR8             *InBuffer\r
  )\r
{\r
  UINTN  i;\r
  CHAR8 *InBufPtr; /// pointer to the input buffer\r
\r
  // check to see if the buffer is the right size which is \r
  // 1 (for 'G') + 16 (for 16 registers) * 8 ( for 8 hex chars each) = 129 \r
  if (AsciiStrLen(InBuffer) != 129) { // 16 regs, 8 hex chars each, and the end '\0' (escape seq)\r
	//Bad message. Message is not the right length \r
    SendError (GDB_EBADBUFSIZE); \r
    return;\r
  }\r
\r
  InBufPtr = &InBuffer[1];\r
  \r
  // Read the new values for the registers from the input buffer to an array, NewValueArray.\r
  // The values in the array are in the gdb ordering\r
  for(i=0; i < sizeof (gRegisterOffsets)/sizeof (UINTN); i++) {  // there are only 16 registers to write\r
    InBufPtr = BasicWriteRegister(SystemContext, i, InBufPtr);\r
  }\r
\r
  SendSuccess();\r
}\r
\r
/** ‘c [addr ]’ \r
 Continue. addr is Address to resume. If addr is omitted, resume at current \r
 Address.\r
 \r
 @param   SystemContext     Register content at time of the exception  \r
 **/\r
VOID\r
EFIAPI\r
ContinueAtAddress (\r
  IN  EFI_SYSTEM_CONTEXT      SystemContext,\r
  IN    CHAR8                 *PacketData\r
  )\r
{\r
  if (PacketData[1] != '\0') {\r
    SystemContext.SystemContextIa32->Eip = AsciiStrHexToUintn (&PacketData[1]);\r
  } \r
}\r
\r
\r
/** ‘s [addr ]’\r
 Single step. addr is the Address at which to resume. If addr is omitted, resume \r
 at same Address.\r
 \r
 @param   SystemContext     Register content at time of the exception  \r
 **/\r
VOID\r
EFIAPI\r
SingleStep (\r
  IN  EFI_SYSTEM_CONTEXT      SystemContext,\r
  IN    CHAR8                 *PacketData\r
  )\r
{\r
  SendNotSupported();\r
}\r
\r
\r
/**\r
  Returns breakpoint data address from DR0-DR3 based on the input breakpoint number\r
\r
  @param  SystemContext      Register content at time of the exception\r
  @param  BreakpointNumber   Breakpoint number\r
\r
  @retval Address            Data address from DR0-DR3 based on the breakpoint number.\r
\r
**/\r
UINTN\r
GetBreakpointDataAddress (\r
  IN  EFI_SYSTEM_CONTEXT  SystemContext,\r
  IN  UINTN               BreakpointNumber\r
  )\r
{\r
  UINTN Address;\r
\r
  if (BreakpointNumber == 1) {\r
    Address = SystemContext.SystemContextIa32->Dr0;\r
  } else if (BreakpointNumber == 2) {\r
    Address = SystemContext.SystemContextIa32->Dr1;\r
  } else if (BreakpointNumber == 3) {\r
    Address = SystemContext.SystemContextIa32->Dr2;\r
  } else if (BreakpointNumber == 4) {\r
    Address = SystemContext.SystemContextIa32->Dr3;\r
  } else {\r
    Address = 0;\r
  }\r
\r
  return Address;\r
}\r
\r
\r
/**\r
  Returns currently detected breakpoint value based on the register DR6 B0-B3 field.\r
  If no breakpoint is detected then it returns 0.\r
\r
  @param  SystemContext  Register content at time of the exception\r
\r
  @retval {1-4}          Currently detected breakpoint value   \r
  @retval 0              No breakpoint detected.\r
 \r
**/\r
UINTN\r
GetBreakpointDetected (\r
  IN  EFI_SYSTEM_CONTEXT  SystemContext\r
  )\r
{\r
  IA32_DR6 Dr6;\r
  UINTN BreakpointNumber;\r
\r
  Dr6.UintN = SystemContext.SystemContextIa32->Dr6;\r
\r
  if (Dr6.Bits.B0 == 1) {\r
    BreakpointNumber = 1;\r
  } else if (Dr6.Bits.B1 == 1) {\r
    BreakpointNumber = 2;\r
  } else if (Dr6.Bits.B2 == 1) {\r
    BreakpointNumber = 3;\r
  } else if (Dr6.Bits.B3 == 1) {\r
    BreakpointNumber = 4;\r
  } else {\r
    BreakpointNumber = 0;  //No breakpoint detected \r
  }\r
\r
  return BreakpointNumber;\r
}\r
\r
\r
/**\r
  Returns Breakpoint type (InstructionExecution, DataWrite, DataRead or DataReadWrite)\r
  based on the Breakpoint number\r
  \r
  @param  SystemContext        Register content at time of the exception\r
  @param  BreakpointNumber     Breakpoint number\r
  \r
  @retval BREAK_TYPE           Breakpoint type value read from register DR7 RWn field\r
                               For unknown value, it returns NotSupported.\r
  \r
**/\r
BREAK_TYPE\r
GetBreakpointType (\r
  IN  EFI_SYSTEM_CONTEXT  SystemContext,\r
  IN  UINTN               BreakpointNumber\r
  )\r
{\r
  IA32_DR7 Dr7;\r
  BREAK_TYPE Type = NotSupported;  //Default is NotSupported type\r
\r
  Dr7.UintN = SystemContext.SystemContextIa32->Dr7;\r
\r
  if (BreakpointNumber == 1) {\r
    Type = (BREAK_TYPE) Dr7.Bits.RW0;  \r
  } else if (BreakpointNumber == 2) {\r
    Type = (BREAK_TYPE) Dr7.Bits.RW1;  \r
  } else if (BreakpointNumber == 3) {\r
    Type = (BREAK_TYPE) Dr7.Bits.RW2;  \r
  } else if (BreakpointNumber == 4) {\r
    Type = (BREAK_TYPE) Dr7.Bits.RW3;  \r
  }\r
\r
  return Type;\r
}\r
\r
\r
/** \r
  Parses Length and returns the length which DR7 LENn field accepts.\r
  For example: If we receive 1-Byte length then we should return 0. \r
               Zero gets written to DR7 LENn field.\r
\r
  @param  Length  Breakpoint length in Bytes (1 byte, 2 byte, 4 byte)\r
\r
  @retval Length  Appropriate converted values which DR7 LENn field accepts.\r
\r
**/\r
UINTN\r
ConvertLengthData (\r
  IN     UINTN   Length\r
  )\r
{\r
  if (Length == 1) {         //1-Byte length \r
    return 0;\r
  } else if (Length == 2) {  //2-Byte length\r
    return 1;\r
  } else if (Length == 4) {  //4-Byte length\r
    return 3;\r
  } else {                   //Undefined or 8-byte length\r
    return 2;\r
  }\r
}\r
\r
\r
/**\r
  Finds the next free debug register. If all the registers are occupied then \r
  EFI_OUT_OF_RESOURCES is returned.  \r
\r
  @param  SystemContext   Register content at time of the exception\r
  @param  Register        Register value (0 - 3 for the first free debug register)\r
\r
  @retval EFI_STATUS      Appropriate status value.\r
\r
**/\r
EFI_STATUS\r
FindNextFreeDebugRegister (\r
  IN  EFI_SYSTEM_CONTEXT  SystemContext,\r
  OUT UINTN               *Register\r
  )\r
{\r
  IA32_DR7 Dr7;\r
\r
  Dr7.UintN = SystemContext.SystemContextIa32->Dr7;\r
\r
  if (Dr7.Bits.G0 == 0) {\r
    *Register = 0;\r
  } else if (Dr7.Bits.G1 == 0) {\r
    *Register = 1;\r
  } else if (Dr7.Bits.G2 == 0) {\r
    *Register = 2;\r
  } else if (Dr7.Bits.G3 == 0) {\r
    *Register = 3;\r
  } else {\r
    return EFI_OUT_OF_RESOURCES;\r
  }\r
\r
  return EFI_SUCCESS;\r
}\r
\r
\r
/**\r
  Enables the debug register. Writes Address value to appropriate DR0-3 register.\r
  Sets LENn, Gn, RWn bits in DR7 register.\r
 \r
  @param  SystemContext   Register content at time of the exception\r
  @param  Register        Register value (0 - 3) \r
  @param  Address         Breakpoint address value\r
  @param  Type            Breakpoint type (Instruction, Data write, Data read \r
                          or write etc.)\r
\r
  @retval EFI_STATUS      Appropriate status value.\r
\r
**/\r
EFI_STATUS\r
EnableDebugRegister (\r
  IN  EFI_SYSTEM_CONTEXT  SystemContext,\r
  IN  UINTN               Register,\r
  IN  UINTN               Address,\r
  IN  UINTN               Length,\r
  IN  UINTN               Type\r
  )\r
{\r
  IA32_DR7  Dr7;\r
\r
  //Convert length data\r
  Length = ConvertLengthData (Length);\r
\r
  //For Instruction execution, length should be 0 \r
  //(Ref. Intel reference manual 18.2.4)\r
  if ((Type == 0) && (Length != 0)) {\r
    return EFI_INVALID_PARAMETER;\r
  }\r
 \r
  //Hardware doesn't support ReadWatch (z3 packet) type. GDB can handle\r
  //software breakpoint. We should send empty packet in both these cases.\r
  if ((Type == (BREAK_TYPE)DataRead) || \r
      (Type == (BREAK_TYPE)SoftwareBreakpoint)) {\r
    return EFI_UNSUPPORTED;\r
  }\r
\r
  //Read DR7 so appropriate Gn, RWn and LENn bits can be modified.\r
  Dr7.UintN = SystemContext.SystemContextIa32->Dr7;\r
\r
  if (Register == 0) {\r
    SystemContext.SystemContextIa32->Dr0 = Address;\r
    Dr7.Bits.G0 = 1;\r
    Dr7.Bits.RW0 = Type;\r
    Dr7.Bits.LEN0 = Length;\r
  } else if (Register == 1) {\r
    SystemContext.SystemContextIa32->Dr1 = Address;\r
    Dr7.Bits.G1 = 1;\r
    Dr7.Bits.RW1 = Type;\r
    Dr7.Bits.LEN1 = Length;\r
  } else if (Register == 2) {\r
    SystemContext.SystemContextIa32->Dr2 = Address;\r
    Dr7.Bits.G2 = 1;\r
    Dr7.Bits.RW2 = Type;\r
    Dr7.Bits.LEN2 = Length;\r
  } else if (Register == 3) {\r
    SystemContext.SystemContextIa32->Dr3 = Address;\r
    Dr7.Bits.G3 = 1;\r
    Dr7.Bits.RW3 = Type;\r
    Dr7.Bits.LEN3 = Length;\r
  } else {\r
    return EFI_INVALID_PARAMETER;\r
  }\r
\r
  //Update Dr7 with appropriate Gn, RWn and LENn bits \r
  SystemContext.SystemContextIa32->Dr7 = Dr7.UintN;\r
\r
  return EFI_SUCCESS;\r
}\r
\r
\r
/**\r
  Returns register number 0 - 3 for the maching debug register.\r
  This function compares incoming Address, Type, Length and\r
  if there is a match then it returns the appropriate register number.\r
  In case of mismatch, function returns EFI_NOT_FOUND message.\r
\r
  @param  SystemContext  Register content at time of the exception\r
  @param  Address        Breakpoint address value\r
  @param  Length         Breakpoint length value\r
  @param  Type           Breakpoint type (Instruction, Data write, \r
                         Data read or write etc.)\r
  @param  Register       Register value to be returned\r
\r
  @retval EFI_STATUS     Appropriate status value.\r
\r
**/\r
EFI_STATUS\r
FindMatchingDebugRegister (\r
 IN  EFI_SYSTEM_CONTEXT  SystemContext,\r
 IN  UINTN               Address,\r
 IN  UINTN               Length,\r
 IN  UINTN               Type,\r
 OUT UINTN               *Register\r
 )\r
{\r
  IA32_DR7 Dr7;\r
\r
  //Hardware doesn't support ReadWatch (z3 packet) type. GDB can handle\r
  //software breakpoint. We should send empty packet in both these cases.\r
  if ((Type == (BREAK_TYPE)DataRead) || \r
      (Type == (BREAK_TYPE)SoftwareBreakpoint)) {\r
    return EFI_UNSUPPORTED;\r
  }\r
\r
  //Convert length data\r
  Length = ConvertLengthData(Length);\r
\r
  Dr7.UintN = SystemContext.SystemContextIa32->Dr7;\r
\r
  if ((Dr7.Bits.G0 == 1) && \r
      (Dr7.Bits.LEN0 == Length) &&\r
      (Dr7.Bits.RW0 == Type) && \r
      (Address == SystemContext.SystemContextIa32->Dr0)) {\r
    *Register = 0;\r
  } else if ((Dr7.Bits.G1 == 1) && \r
             (Dr7.Bits.LEN1 == Length) &&\r
             (Dr7.Bits.RW1 == Type) && \r
             (Address == SystemContext.SystemContextIa32->Dr1)) {\r
    *Register = 1;\r
  } else if ((Dr7.Bits.G2 == 1) && \r
             (Dr7.Bits.LEN2 == Length) &&\r
             (Dr7.Bits.RW2 == Type) && \r
             (Address == SystemContext.SystemContextIa32->Dr2)) {\r
    *Register = 2;\r
  } else if ((Dr7.Bits.G3 == 1) && \r
             (Dr7.Bits.LEN3 == Length) &&\r
             (Dr7.Bits.RW3 == Type) && \r
             (Address == SystemContext.SystemContextIa32->Dr3)) {\r
    *Register = 3;\r
  } else {\r
    Print ((CHAR16 *)L"No match found..\n");\r
    return EFI_NOT_FOUND;\r
  }\r
\r
  return EFI_SUCCESS;\r
}\r
\r
\r
/**\r
  Disables the particular debug register.\r
\r
  @param  SystemContext   Register content at time of the exception\r
  @param  Register        Register to be disabled\r
\r
  @retval EFI_STATUS      Appropriate status value.\r
\r
**/\r
EFI_STATUS\r
DisableDebugRegister (\r
 IN  EFI_SYSTEM_CONTEXT  SystemContext,\r
 IN  UINTN               Register  \r
 )\r
{\r
  IA32_DR7  Dr7;\r
  UINTN Address = 0;\r
 \r
  //Read DR7 register so appropriate Gn, RWn and LENn bits can be turned off.\r
  Dr7.UintN = SystemContext.SystemContextIa32->Dr7;\r
\r
  if (Register == 0) {\r
    SystemContext.SystemContextIa32->Dr0 = Address;\r
    Dr7.Bits.G0 = 0;\r
    Dr7.Bits.RW0 = 0;\r
    Dr7.Bits.LEN0 = 0;\r
  } else if (Register == 1) {\r
    SystemContext.SystemContextIa32->Dr1 = Address;\r
    Dr7.Bits.G1 = 0;\r
    Dr7.Bits.RW1 = 0;\r
    Dr7.Bits.LEN1 = 0;\r
  } else if (Register == 2) {\r
    SystemContext.SystemContextIa32->Dr2 = Address;\r
    Dr7.Bits.G2 = 0;\r
    Dr7.Bits.RW2 = 0;\r
    Dr7.Bits.LEN2 = 0;\r
  } else if (Register == 3) {\r
    SystemContext.SystemContextIa32->Dr3 = Address;\r
    Dr7.Bits.G3 = 0;\r
    Dr7.Bits.RW3 = 0;\r
    Dr7.Bits.LEN3 = 0;\r
  } else {\r
    return EFI_INVALID_PARAMETER;\r
  }\r
\r
  //Update DR7 register so appropriate Gn, RWn and LENn bits can be turned off.\r
  SystemContext.SystemContextIa32->Dr7 = Dr7.UintN;\r
\r
  return EFI_SUCCESS;\r
}\r
\r
\r
/**\r
  ‘Z1, [addr], [length]’\r
  ‘Z2, [addr], [length]’\r
  ‘Z3, [addr], [length]’\r
  ‘Z4, [addr], [length]’\r
\r
  Insert hardware breakpoint/watchpoint at address addr of size length\r
\r
  @param SystemContext  Register content at time of the exception\r
  @param *PacketData    Pointer to the Payload data for the packet\r
\r
**/\r
VOID\r
EFIAPI\r
InsertBreakPoint (\r
  IN  EFI_SYSTEM_CONTEXT  SystemContext,\r
  IN  CHAR8              *PacketData\r
  )\r
{\r
  UINTN Type;\r
  UINTN Address;\r
  UINTN Length;\r
  UINTN Register;\r
  EFI_STATUS Status;\r
  BREAK_TYPE BreakType = NotSupported;\r
  UINTN ErrorCode;\r
\r
  ErrorCode = ParseBreakpointPacket (PacketData, &Type, &Address, &Length);\r
  if (ErrorCode > 0) {\r
    SendError ((UINT8)ErrorCode);\r
    return;\r
  }\r
\r
  switch (Type) {\r
\r
    case    0:   //Software breakpoint\r
      BreakType = SoftwareBreakpoint;\r
      break;\r
\r
    case    1:   //Hardware breakpoint\r
      BreakType = InstructionExecution;\r
      break;\r
\r
    case    2:   //Write watchpoint\r
      BreakType = DataWrite;\r
      break;\r
\r
    case    3:   //Read watchpoint\r
      BreakType = DataRead;\r
      break;\r
\r
    case    4:   //Access watchpoint\r
      BreakType = DataReadWrite;\r
      break;\r
\r
    default  :\r
      Print ((CHAR16 *)L"Insert breakpoint default: %x\n", Type);\r
      SendError (GDB_EINVALIDBRKPOINTTYPE);\r
      return;\r
  }\r
\r
  // Find next free debug register\r
  Status = FindNextFreeDebugRegister (SystemContext, &Register);\r
  if (EFI_ERROR(Status)) {\r
    Print ((CHAR16 *)L"No space left on device\n");\r
    SendError (GDB_ENOSPACE);\r
    return;\r
  }\r
\r
  // Write Address, length data at particular DR register\r
  Status = EnableDebugRegister (SystemContext, Register, Address, Length, (UINTN)BreakType);\r
  if (EFI_ERROR(Status)) {\r
    if (Status == EFI_UNSUPPORTED) {\r
      SendNotSupported();\r
      return;\r
    }\r
\r
    SendError (GDB_EINVALIDARG);\r
    return;\r
  }\r
\r
  SendSuccess ();\r
}\r
\r
\r
/**\r
  ‘z1, [addr], [length]’\r
  ‘z2, [addr], [length]’\r
  ‘z3, [addr], [length]’\r
  ‘z4, [addr], [length]’\r
\r
  Remove hardware breakpoint/watchpoint at address addr of size length\r
\r
  @param *PacketData    Pointer to the Payload data for the packet\r
\r
**/\r
VOID\r
EFIAPI\r
RemoveBreakPoint (\r
  IN  EFI_SYSTEM_CONTEXT  SystemContext,\r
  IN  CHAR8               *PacketData\r
  )\r
{\r
  UINTN      Type;\r
  UINTN      Address;\r
  UINTN      Length;\r
  UINTN      Register;\r
  BREAK_TYPE BreakType = NotSupported;\r
  EFI_STATUS Status;\r
  UINTN      ErrorCode;\r
\r
  //Parse breakpoint packet data\r
  ErrorCode = ParseBreakpointPacket (PacketData, &Type, &Address, &Length);\r
  if (ErrorCode > 0) {\r
    SendError ((UINT8)ErrorCode);\r
    return;\r
  }\r
\r
  switch (Type) {\r
  \r
    case    0:   //Software breakpoint\r
      BreakType = SoftwareBreakpoint;\r
      break;\r
      \r
    case    1:   //Hardware breakpoint\r
      BreakType = InstructionExecution;\r
      break;\r
      \r
    case    2:   //Write watchpoint\r
      BreakType = DataWrite;\r
      break;\r
\r
    case    3:   //Read watchpoint\r
      BreakType = DataRead;\r
      break;\r
\r
    case    4:   //Access watchpoint\r
      BreakType = DataReadWrite;\r
      break;\r
\r
    default  :\r
      SendError (GDB_EINVALIDBRKPOINTTYPE);\r
      return;\r
  }\r
\r
  //Find matching debug register\r
  Status = FindMatchingDebugRegister (SystemContext, Address, Length, (UINTN)BreakType, &Register);\r
  if (EFI_ERROR(Status)) {\r
    if (Status == EFI_UNSUPPORTED) {\r
      SendNotSupported();\r
      return;\r
    }\r
\r
    SendError (GDB_ENOSPACE);\r
    return;\r
  }\r
\r
  //Remove breakpoint\r
  Status = DisableDebugRegister(SystemContext, Register);\r
  if (EFI_ERROR(Status)) {\r
    SendError (GDB_EINVALIDARG);\r
    return;\r
  }\r
\r
  SendSuccess ();\r
}\r
\r
\r
/**\r
  Initialize debug agent.\r
\r
  This function is used to set up debug environment to support source level debugging.\r
  If certain Debug Agent Library instance has to save some private data in the stack,\r
  this function must work on the mode that doesn't return to the caller, then\r
  the caller needs to wrap up all rest of logic after InitializeDebugAgent() into one\r
  function and pass it into InitializeDebugAgent(). InitializeDebugAgent() is\r
  responsible to invoke the passing-in function at the end of InitializeDebugAgent().\r
\r
  If the parameter Function is not NULL, Debug Agent Libary instance will invoke it by\r
  passing in the Context to be its parameter.\r
\r
  If Function() is NULL, Debug Agent Library instance will return after setup debug\r
  environment.\r
\r
  @param[in] InitFlag     Init flag is used to decide the initialize process.\r
  @param[in] Context      Context needed according to InitFlag; it was optional.\r
  @param[in] Function     Continue function called by debug agent library; it was\r
                          optional.\r
\r
**/\r
VOID\r
EFIAPI\r
InitializeDebugAgent (\r
  IN UINT32                InitFlag,\r
  IN VOID                  *Context, OPTIONAL\r
  IN DEBUG_AGENT_CONTINUE  Function  OPTIONAL\r
  )\r
{\r
  // BugBug: Add the code to build an GDT/IDT\r
\r
  if (Function != NULL) {\r
    Function (Context);\r
  }\r
}\r
\r