SourceLevelDebugPkg/Library/DebugAgent/DebugAgentCommon/DebugAgent.c

   1 /** @file
   2   Commond Debug Agent library implementition. It mainly includes
   3   the first C function called by exception/interrupt handlers,
   4   read/write debug packet to communication with HOST based on transfer
   5   protocol.
   6
   7   Copyright (c) 2010 - 2014, Intel Corporation. All rights reserved.<BR>
   8   This program and the accompanying materials
   9   are licensed and made available under the terms and conditions of the BSD License
  10   which accompanies this distribution.  The full text of the license may be found at
  11   http://opensource.org/licenses/bsd-license.php.
  12
  13   THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
  14   WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
  15
  16 **/
  17
  18 #include "DebugAgent.h"
  19 #include "Ia32/DebugException.h"
  20
  21 GLOBAL_REMOVE_IF_UNREFERENCED CHAR8 mErrorMsgVersionAlert[]       = "\rThe SourceLevelDebugPkg you are using requires a newer version of the Intel(R) UDK Debugger Tool.\r\n";
  22 GLOBAL_REMOVE_IF_UNREFERENCED CHAR8 mErrorMsgSendInitPacket[]     = "\rSend INIT break packet and try to connect the HOST (Intel(R) UDK Debugger Tool v1.4) ...\r\n";
  23 GLOBAL_REMOVE_IF_UNREFERENCED CHAR8 mErrorMsgConnectOK[]          = "HOST connection is successful!\r\n";
  24 GLOBAL_REMOVE_IF_UNREFERENCED CHAR8 mErrorMsgConnectFail[]        = "HOST connection is failed!\r\n";
  25 GLOBAL_REMOVE_IF_UNREFERENCED CHAR8 mWarningMsgIngoreBreakpoint[] = "Ignore break point in SMM for SMI issued during DXE debugging!\r\n";
  26
  27 //
  28 // Vector Handoff Info list used by Debug Agent for persist
  29 //
  30 GLOBAL_REMOVE_IF_UNREFERENCED EFI_VECTOR_HANDOFF_INFO mVectorHandoffInfoDebugAgent[] = {
  31   {
  32     DEBUG_EXCEPT_DIVIDE_ERROR,         // Vector 0
  33     EFI_VECTOR_HANDOFF_HOOK_BEFORE,
  34     EFI_DEBUG_AGENT_GUID
  35   },
  36   {
  37     DEBUG_EXCEPT_DEBUG,                // Vector 1
  38     EFI_VECTOR_HANDOFF_DO_NOT_HOOK,
  39     EFI_DEBUG_AGENT_GUID
  40   },
  41   {
  42     DEBUG_EXCEPT_NMI,                  // Vector 2
  43     EFI_VECTOR_HANDOFF_HOOK_BEFORE,
  44     EFI_DEBUG_AGENT_GUID
  45   },
  46   {
  47     DEBUG_EXCEPT_BREAKPOINT,           // Vector 3
  48     EFI_VECTOR_HANDOFF_DO_NOT_HOOK,
  49     EFI_DEBUG_AGENT_GUID
  50   },
  51   {
  52     DEBUG_EXCEPT_OVERFLOW,             // Vector 4
  53     EFI_VECTOR_HANDOFF_HOOK_BEFORE,
  54     EFI_DEBUG_AGENT_GUID
  55   },
  56   {
  57     DEBUG_EXCEPT_BOUND,                // Vector 5
  58     EFI_VECTOR_HANDOFF_HOOK_BEFORE,
  59     EFI_DEBUG_AGENT_GUID
  60   },
  61   {
  62     DEBUG_EXCEPT_INVALID_OPCODE,       // Vector 6
  63     EFI_VECTOR_HANDOFF_HOOK_BEFORE,
  64     EFI_DEBUG_AGENT_GUID
  65   },
  66   {
  67     DEBUG_EXCEPT_DOUBLE_FAULT,         // Vector 8
  68     EFI_VECTOR_HANDOFF_HOOK_BEFORE,
  69     EFI_DEBUG_AGENT_GUID
  70   },
  71   {
  72     DEBUG_EXCEPT_INVALID_TSS,          // Vector 10
  73     EFI_VECTOR_HANDOFF_HOOK_BEFORE,
  74     EFI_DEBUG_AGENT_GUID
  75   },
  76   {
  77     DEBUG_EXCEPT_SEG_NOT_PRESENT,      // Vector 11
  78     EFI_VECTOR_HANDOFF_HOOK_BEFORE,
  79     EFI_DEBUG_AGENT_GUID
  80   },
  81   {
  82     DEBUG_EXCEPT_STACK_FAULT,          // Vector 12
  83     EFI_VECTOR_HANDOFF_HOOK_BEFORE,
  84     EFI_DEBUG_AGENT_GUID
  85   },
  86   {
  87     DEBUG_EXCEPT_GP_FAULT,             // Vector 13
  88     EFI_VECTOR_HANDOFF_HOOK_BEFORE,
  89     EFI_DEBUG_AGENT_GUID
  90   },
  91     {
  92     DEBUG_EXCEPT_PAGE_FAULT,           // Vector 14
  93     EFI_VECTOR_HANDOFF_HOOK_BEFORE,
  94     EFI_DEBUG_AGENT_GUID
  95   },
  96   {
  97     DEBUG_EXCEPT_FP_ERROR,             // Vector 16
  98     EFI_VECTOR_HANDOFF_HOOK_BEFORE,
  99     EFI_DEBUG_AGENT_GUID
 100   },
 101   {
 102     DEBUG_EXCEPT_ALIGNMENT_CHECK,      // Vector 17
 103     EFI_VECTOR_HANDOFF_HOOK_BEFORE,
 104     EFI_DEBUG_AGENT_GUID
 105   },
 106   {
 107     DEBUG_EXCEPT_MACHINE_CHECK,        // Vector 18
 108     EFI_VECTOR_HANDOFF_HOOK_BEFORE,
 109     EFI_DEBUG_AGENT_GUID
 110   },
 111   {
 112     DEBUG_EXCEPT_SIMD,                 // Vector 19
 113     EFI_VECTOR_HANDOFF_HOOK_BEFORE,
 114     EFI_DEBUG_AGENT_GUID
 115   },
 116   {
 117     DEBUG_TIMER_VECTOR,                // Vector 32
 118     EFI_VECTOR_HANDOFF_DO_NOT_HOOK,
 119     EFI_DEBUG_AGENT_GUID
 120   },
 121   {
 122     DEBUG_MAILBOX_VECTOR,              // Vector 33
 123     EFI_VECTOR_HANDOFF_DO_NOT_HOOK,
 124     EFI_DEBUG_AGENT_GUID
 125   },
 126   {
 127     0,
 128     EFI_VECTOR_HANDOFF_LAST_ENTRY,
 129     { 0 }
 130   }
 131 };
 132
 133 GLOBAL_REMOVE_IF_UNREFERENCED UINTN mVectorHandoffInfoCount = sizeof (mVectorHandoffInfoDebugAgent) / sizeof (EFI_VECTOR_HANDOFF_INFO);
 134
 135 /**
 136   Calculate CRC16 for target data.
 137
 138   @param[in]  Data              The target data.
 139   @param[in]  DataSize          The target data size.
 140   @param[in]  Crc               Initial CRC.
 141
 142   @return UINT16     The CRC16 value.
 143
 144 **/
 145 UINT16
 146 CalculateCrc16 (
 147   IN UINT8   *Data,
 148   IN UINTN   DataSize,
 149   IN UINT16  Crc
 150   )
 151 {
 152   UINTN  Index;
 153   UINTN  BitIndex;
 154
 155   for (Index = 0; Index < DataSize; Index++) {
 156     Crc ^= (UINT16)Data[Index];
 157     for (BitIndex = 0; BitIndex < 8; BitIndex++) {
 158       if ((Crc & 0x8000) != 0) {
 159         Crc <<= 1;
 160         Crc ^= 0x1021;
 161       } else {
 162         Crc <<= 1;
 163       }
 164     }
 165   }
 166   return Crc;
 167 }
 168
 169
 170 /**
 171   Read IDT entry to check if IDT entries are setup by Debug Agent.
 172
 173   @retval  TRUE     IDT entries were setup by Debug Agent.
 174   @retval  FALSE    IDT entries were not setup by Debug Agent.
 175
 176 **/
 177 BOOLEAN
 178 IsDebugAgentInitialzed (
 179   VOID
 180   )
 181 {
 182   UINTN                      InterruptHandler;
 183
 184   InterruptHandler = (UINTN) GetExceptionHandlerInIdtEntry (0);
 185   if (InterruptHandler >= 4 &&  *(UINT32 *)(InterruptHandler - 4) == AGENT_HANDLER_SIGNATURE) {
 186     return TRUE;
 187   } else {
 188     return FALSE;
 189   }
 190 }
 191
 192 /**
 193   Find and report module image info to HOST.
 194
 195   @param[in] AlignSize      Image aligned size.
 196
 197 **/
 198 VOID
 199 FindAndReportModuleImageInfo (
 200   IN UINTN          AlignSize
 201   )
 202 {
 203   UINTN                                Pe32Data;
 204   EFI_IMAGE_DOS_HEADER                 *DosHdr;
 205   EFI_IMAGE_OPTIONAL_HEADER_PTR_UNION  Hdr;
 206   PE_COFF_LOADER_IMAGE_CONTEXT         ImageContext;
 207
 208   //
 209   // Find Image Base
 210   //
 211   Pe32Data = ((UINTN)mErrorMsgVersionAlert) & ~(AlignSize - 1);
 212   while (Pe32Data != 0) {
 213     DosHdr = (EFI_IMAGE_DOS_HEADER *) Pe32Data;
 214     if (DosHdr->e_magic == EFI_IMAGE_DOS_SIGNATURE) {
 215       //
 216       // DOS image header is present, so read the PE header after the DOS image header.
 217       //
 218       Hdr.Pe32 = (EFI_IMAGE_NT_HEADERS32 *)(Pe32Data + (UINTN) ((DosHdr->e_lfanew) & 0x0ffff));
 219       //
 220       // Make sure PE header address does not overflow and is less than the initial address.
 221       //
 222       if (((UINTN)Hdr.Pe32 > Pe32Data) && ((UINTN)Hdr.Pe32 < (UINTN)mErrorMsgVersionAlert)) {
 223         if (Hdr.Pe32->Signature == EFI_IMAGE_NT_SIGNATURE) {
 224           //
 225           // It's PE image.
 226           //
 227           break;
 228         }
 229       }
 230     } else {
 231       //
 232       // DOS image header is not present, TE header is at the image base.
 233       //
 234       Hdr.Pe32 = (EFI_IMAGE_NT_HEADERS32 *)Pe32Data;
 235       if ((Hdr.Te->Signature == EFI_TE_IMAGE_HEADER_SIGNATURE) &&
 236           ((Hdr.Te->Machine == IMAGE_FILE_MACHINE_I386) || Hdr.Te->Machine == IMAGE_FILE_MACHINE_X64)) {
 237         //
 238         // It's TE image, it TE header and Machine type match
 239         //
 240         break;
 241       }
 242     }
 243
 244     //
 245     // Not found the image base, check the previous aligned address
 246     //
 247     Pe32Data -= AlignSize;
 248   }
 249
 250   ImageContext.ImageAddress = Pe32Data;
 251   ImageContext.PdbPointer = PeCoffLoaderGetPdbPointer ((VOID*) (UINTN) ImageContext.ImageAddress);
 252   PeCoffLoaderRelocateImageExtraAction (&ImageContext);
 253 }
 254
 255 /**
 256   Trigger one software interrupt to debug agent to handle it.
 257
 258   @param[in] Signature       Software interrupt signature.
 259
 260 **/
 261 VOID
 262 TriggerSoftInterrupt (
 263   IN UINT32                 Signature
 264   )
 265 {
 266   UINTN                  Dr0;
 267   UINTN                  Dr1;
 268
 269   //
 270   // Save Debug Register State
 271   //
 272   Dr0 = AsmReadDr0 ();
 273   Dr1 = AsmReadDr1 ();
 274
 275   //
 276   // DR0 = Signature
 277   //
 278   AsmWriteDr0 (SOFT_INTERRUPT_SIGNATURE);
 279   AsmWriteDr1 (Signature);
 280
 281   //
 282   // Do INT3 to communicate with HOST side
 283   //
 284   CpuBreakpoint ();
 285
 286   //
 287   // Restore Debug Register State only when Host didn't change it inside exception handler.
 288   //   Dr registers can only be changed by setting the HW breakpoint.
 289   //
 290   AsmWriteDr0 (Dr0);
 291   AsmWriteDr1 (Dr1);
 292
 293 }
 294
 295 /**
 296   Caculate Mailbox checksum and update the checksum field.
 297
 298   @param[in]  Mailbox  Debug Agent Mailbox pointer.
 299
 300 **/
 301 VOID
 302 UpdateMailboxChecksum (
 303   IN DEBUG_AGENT_MAILBOX    *Mailbox
 304   )
 305 {
 306   Mailbox->CheckSum = CalculateCheckSum8 ((UINT8 *)Mailbox, sizeof (DEBUG_AGENT_MAILBOX) - 2);
 307 }
 308
 309 /**
 310   Verify Mailbox checksum.
 311
 312   If checksum error, print debug message and run init dead loop.
 313
 314   @param[in]  Mailbox  Debug Agent Mailbox pointer.
 315
 316 **/
 317 VOID
 318 VerifyMailboxChecksum (
 319   IN DEBUG_AGENT_MAILBOX    *Mailbox
 320   )
 321 {
 322   UINT8                     CheckSum;
 323
 324   CheckSum = CalculateCheckSum8 ((UINT8 *) Mailbox, sizeof (DEBUG_AGENT_MAILBOX) - 2);
 325   //
 326   // The checksum updating process may be disturbed by hardware SMI, we need to check CheckSum field
 327   // and ToBeCheckSum field to validate the mail box.
 328   //
 329   if (CheckSum != Mailbox->CheckSum && CheckSum != Mailbox->ToBeCheckSum) {
 330     DEBUG ((EFI_D_ERROR, "DebugAgent: Mailbox checksum error, stack or heap crashed!\n"));
 331     DEBUG ((EFI_D_ERROR, "DebugAgent: CheckSum = %x, Mailbox->CheckSum = %x, Mailbox->ToBeCheckSum = %x\n", CheckSum, Mailbox->CheckSum, Mailbox->ToBeCheckSum));
 332     CpuDeadLoop ();
 333   }
 334 }
 335
 336 /**
 337   Update Mailbox content by index.
 338
 339   @param[in]  Mailbox  Debug Agent Mailbox pointer.
 340   @param[in]  Index    Mailbox content index.
 341   @param[in]  Value    Value to be set into Mailbox.
 342
 343 **/
 344 VOID
 345 UpdateMailboxContent (
 346   IN DEBUG_AGENT_MAILBOX    *Mailbox,
 347   IN UINTN                  Index,
 348   IN UINT64                 Value
 349   )
 350 {
 351   AcquireMpSpinLock (&mDebugMpContext.MailboxSpinLock);
 352   switch (Index) {
 353   case DEBUG_MAILBOX_DEBUG_FLAG_INDEX:
 354     Mailbox->ToBeCheckSum = Mailbox->CheckSum + CalculateSum8 ((UINT8 *)&Mailbox->DebugFlag.Uint64, sizeof(UINT64))
 355                                               - CalculateSum8 ((UINT8 *)&Value, sizeof(UINT64));
 356     Mailbox->DebugFlag.Uint64 = Value;
 357     break;
 358   case DEBUG_MAILBOX_DEBUG_PORT_HANDLE_INDEX:
 359     Mailbox->ToBeCheckSum = Mailbox->CheckSum + CalculateSum8 ((UINT8 *)&Mailbox->DebugPortHandle, sizeof(UINTN))
 360                                               - CalculateSum8 ((UINT8 *)&Value, sizeof(UINTN));
 361     Mailbox->DebugPortHandle = (UINTN) Value;
 362     break;
 363   case DEBUG_MAILBOX_EXCEPTION_BUFFER_POINTER_INDEX:
 364     Mailbox->ToBeCheckSum = Mailbox->CheckSum + CalculateSum8 ((UINT8 *)&Mailbox->ExceptionBufferPointer, sizeof(UINTN))
 365                                               - CalculateSum8 ((UINT8 *)&Value, sizeof(UINTN));
 366     Mailbox->ExceptionBufferPointer = (UINTN) Value;
 367     break;
 368   case DEBUG_MAILBOX_LAST_ACK:
 369     Mailbox->ToBeCheckSum = Mailbox->CheckSum + CalculateSum8 ((UINT8 *)&Mailbox->LastAck, sizeof(UINT8))
 370                                               - CalculateSum8 ((UINT8 *)&Value, sizeof(UINT8));
 371     Mailbox->LastAck = (UINT8) Value;
 372     break;
 373   case DEBUG_MAILBOX_SEQUENCE_NO_INDEX:
 374     Mailbox->ToBeCheckSum = Mailbox->CheckSum + CalculateSum8 ((UINT8 *)&Mailbox->SequenceNo, sizeof(UINT8))
 375                                               - CalculateSum8 ((UINT8 *)&Value, sizeof(UINT8));
 376     Mailbox->SequenceNo = (UINT8) Value;
 377     break;
 378   case DEBUG_MAILBOX_HOST_SEQUENCE_NO_INDEX:
 379     Mailbox->ToBeCheckSum = Mailbox->CheckSum + CalculateSum8 ((UINT8 *)&Mailbox->HostSequenceNo, sizeof(UINT8))
 380                                               - CalculateSum8 ((UINT8 *)&Value, sizeof(UINT8));
 381     Mailbox->HostSequenceNo = (UINT8) Value;
 382     break;
 383   }
 384   UpdateMailboxChecksum (Mailbox);
 385   ReleaseMpSpinLock (&mDebugMpContext.MailboxSpinLock);
 386 }
 387 /**
 388   Set debug flag in mailbox.
 389
 390   @param[in]  FlagMask      Debug flag mask value.
 391   @param[in]  FlagValue     Debug flag value.
 392
 393 **/
 394 VOID
 395 SetDebugFlag (
 396   IN UINT64                 FlagMask,
 397   IN UINT32                 FlagValue
 398   )
 399 {
 400   DEBUG_AGENT_MAILBOX    *Mailbox;
 401   UINT64                 Data64;
 402
 403   Mailbox = GetMailboxPointer ();
 404   Data64 = (Mailbox->DebugFlag.Uint64 & ~FlagMask) |
 405            (LShiftU64 ((UINT64)FlagValue, LowBitSet64 (FlagMask)) & FlagMask);
 406   UpdateMailboxContent (Mailbox, DEBUG_MAILBOX_DEBUG_FLAG_INDEX, Data64);
 407 }
 408
 409 /**
 410   Get debug flag in mailbox.
 411
 412   @param[in]  FlagMask      Debug flag mask value.
 413
 414   @return Debug flag value.
 415
 416 **/
 417 UINT32
 418 GetDebugFlag (
 419   IN UINT64                 FlagMask
 420   )
 421 {
 422   DEBUG_AGENT_MAILBOX    *Mailbox;
 423   UINT32                 DebugFlag;
 424
 425   Mailbox = GetMailboxPointer ();
 426   DebugFlag = (UINT32) RShiftU64 (Mailbox->DebugFlag.Uint64 & FlagMask, LowBitSet64 (FlagMask));
 427
 428   return DebugFlag;
 429 }
 430
 431 /**
 432   Send a debug message packet to the debug port.
 433
 434   @param[in] Buffer  The debug message.
 435   @param[in] Length  The length of debug message.
 436
 437 **/
 438 VOID
 439 SendDebugMsgPacket (
 440   IN CHAR8         *Buffer,
 441   IN UINTN         Length
 442   )
 443 {
 444   DEBUG_PACKET_HEADER  DebugHeader;
 445   DEBUG_PORT_HANDLE    Handle;
 446
 447   Handle = GetDebugPortHandle();
 448
 449   DebugHeader.StartSymbol = DEBUG_STARTING_SYMBOL_NORMAL;
 450   DebugHeader.Command     = DEBUG_COMMAND_PRINT_MESSAGE;
 451   DebugHeader.Length      = sizeof (DEBUG_PACKET_HEADER) + (UINT8) Length;
 452   DebugHeader.SequenceNo  = 0xEE;
 453   DebugHeader.Crc         = 0;
 454   DebugHeader.Crc         = CalculateCrc16 (
 455                               (UINT8 *)Buffer, Length,
 456                               CalculateCrc16 ((UINT8 *)&DebugHeader, sizeof (DEBUG_PACKET_HEADER), 0)
 457                               );
 458
 459   DebugPortWriteBuffer (Handle, (UINT8 *)&DebugHeader, sizeof (DEBUG_PACKET_HEADER));
 460   DebugPortWriteBuffer (Handle, (UINT8 *)Buffer, Length);
 461 }
 462
 463 /**
 464   Prints a debug message to the debug port if the specified error level is enabled.
 465
 466   If any bit in ErrorLevel is also set in Mainbox, then print the message specified
 467   by Format and the associated variable argument list to the debug port.
 468
 469   @param[in] ErrorLevel  The error level of the debug message.
 470   @param[in] Format      Format string for the debug message to print.
 471   @param[in] ...         Variable argument list whose contents are accessed
 472                          based on the format string specified by Format.
 473
 474 **/
 475 VOID
 476 EFIAPI
 477 DebugAgentMsgPrint (
 478   IN UINT8         ErrorLevel,
 479   IN CHAR8         *Format,
 480   ...
 481   )
 482 {
 483   CHAR8                Buffer[DEBUG_DATA_MAXIMUM_REAL_DATA];
 484   VA_LIST              Marker;
 485
 486   //
 487   // Check driver debug mask value and global mask
 488   //
 489   if ((ErrorLevel & GetDebugFlag (DEBUG_AGENT_FLAG_PRINT_ERROR_LEVEL)) == 0) {
 490     return;
 491   }
 492
 493   //
 494   // Convert the DEBUG() message to an ASCII String
 495   //
 496   VA_START (Marker, Format);
 497   AsciiVSPrint (Buffer, sizeof (Buffer), Format, Marker);
 498   VA_END (Marker);
 499
 500   SendDebugMsgPacket (Buffer, AsciiStrLen (Buffer));
 501 }
 502
 503 /**
 504   Prints a debug message to the debug output device if the specified error level is enabled.
 505
 506   If any bit in ErrorLevel is also set in DebugPrintErrorLevelLib function
 507   GetDebugPrintErrorLevel (), then print the message specified by Format and the
 508   associated variable argument list to the debug output device.
 509
 510   If Format is NULL, then ASSERT().
 511
 512   @param[in] ErrorLevel  The error level of the debug message.
 513   @param[in] IsSend      Flag of debug message to declare that the data is being sent or being received.
 514   @param[in] Data        Variable argument list whose contents are accessed
 515   @param[in] Length      based on the format string specified by Format.
 516
 517 **/
 518 VOID
 519 EFIAPI
 520 DebugAgentDataMsgPrint (
 521   IN UINT8             ErrorLevel,
 522   IN BOOLEAN           IsSend,
 523   IN UINT8             *Data,
 524   IN UINT8             Length
 525   )
 526 {
 527   CHAR8                Buffer[DEBUG_DATA_MAXIMUM_REAL_DATA];
 528   CHAR8                *DestBuffer;
 529   UINTN                Index;
 530
 531   //
 532   // Check driver debug mask value and global mask
 533   //
 534   if ((ErrorLevel & GetDebugFlag (DEBUG_AGENT_FLAG_PRINT_ERROR_LEVEL)) == 0) {
 535     return;
 536   }
 537
 538   DestBuffer = Buffer;
 539   if (IsSend) {
 540     DestBuffer += AsciiSPrint (DestBuffer, DEBUG_DATA_MAXIMUM_REAL_DATA, "Sent data [ ");
 541   } else {
 542     DestBuffer += AsciiSPrint (DestBuffer, DEBUG_DATA_MAXIMUM_REAL_DATA, "Received data [ ");
 543   }
 544
 545   Index = 0;
 546   while (TRUE) {
 547     if (DestBuffer - Buffer > DEBUG_DATA_MAXIMUM_REAL_DATA - 6) {
 548       //
 549       // If there was no enough space in buffer, send out the debug message,
 550       // reserving 6 bytes is for the last data and end characters "]\n".
 551       //
 552       SendDebugMsgPacket (Buffer, DestBuffer - Buffer);
 553       DestBuffer = Buffer;
 554     }
 555     DestBuffer += AsciiSPrint (DestBuffer, DEBUG_DATA_MAXIMUM_REAL_DATA - (DestBuffer - Buffer), "%02x ", Data[Index]);
 556     Index ++;
 557     if (Index >= Length) {
 558       //
 559       // The last character of debug message has been foramtted in buffer
 560       //
 561       DestBuffer += AsciiSPrint(DestBuffer, DEBUG_DATA_MAXIMUM_REAL_DATA - (DestBuffer - Buffer), "]\n");
 562       SendDebugMsgPacket (Buffer, DestBuffer - Buffer);
 563       break;
 564     }
 565   }
 566 }
 567
 568 /**
 569   Read remaing debug packet except for the start symbol
 570
 571   @param[in]      Handle        Pointer to Debug Port handle.
 572   @param[in, out] DebugHeader   Debug header buffer including start symbol.
 573
 574   @retval EFI_SUCCESS        Read the symbol in BreakSymbol.
 575   @retval EFI_CRC_ERROR      CRC check fail.
 576   @retval EFI_TIMEOUT        Timeout occurs when reading debug packet.
 577   @retval EFI_DEVICE_ERROR   Receive the old or responsed packet.
 578
 579 **/
 580 EFI_STATUS
 581 ReadRemainingBreakPacket (
 582   IN     DEBUG_PORT_HANDLE      Handle,
 583   IN OUT DEBUG_PACKET_HEADER    *DebugHeader
 584   )
 585 {
 586   UINT16                     Crc;
 587   DEBUG_AGENT_MAILBOX        *Mailbox;
 588
 589   //
 590   // Has received start symbol, try to read the rest part
 591   //
 592   if (DebugPortReadBuffer (Handle, (UINT8 *)DebugHeader + OFFSET_OF (DEBUG_PACKET_HEADER, Command), sizeof (DEBUG_PACKET_HEADER) - OFFSET_OF (DEBUG_PACKET_HEADER, Command), READ_PACKET_TIMEOUT) == 0) {
 593     //
 594     // Timeout occur, exit
 595     //
 596     DebugAgentMsgPrint (DEBUG_AGENT_WARNING, "Timeout in Debug Timer interrupt\n");
 597     return EFI_TIMEOUT;
 598   }
 599
 600   Crc = DebugHeader->Crc;
 601   DebugHeader->Crc = 0;
 602   if (CalculateCrc16 ((UINT8 *)DebugHeader, DebugHeader->Length, 0) != Crc) {
 603     DebugAgentMsgPrint (DEBUG_AGENT_WARNING, "Debug Timer CRC (%x) against (%x)\n", Crc, CalculateCrc16 ((UINT8 *) &DebugHeader, DebugHeader->Length, 0));
 604     DebugAgentDataMsgPrint (DEBUG_AGENT_VERBOSE, FALSE, (UINT8 *)DebugHeader, DebugHeader->Length);
 605     return EFI_CRC_ERROR;
 606   }
 607   Mailbox = GetMailboxPointer();
 608   if (IS_REQUEST (DebugHeader)) {
 609     if (DebugHeader->SequenceNo == (UINT8) (Mailbox->HostSequenceNo + 1)) {
 610       //
 611       // Only updagte HostSequenceNo for new command packet
 612       //
 613       UpdateMailboxContent (Mailbox, DEBUG_MAILBOX_HOST_SEQUENCE_NO_INDEX, DebugHeader->SequenceNo);
 614       return EFI_SUCCESS;
 615     }
 616     if (DebugHeader->SequenceNo == Mailbox->HostSequenceNo) {
 617       return EFI_SUCCESS;
 618     }
 619   }
 620
 621   return EFI_DEVICE_ERROR;
 622 }
 623
 624 /**
 625   Check if HOST is attached based on Mailbox.
 626
 627   @retval TRUE        HOST is attached.
 628   @retval FALSE       HOST is not attached.
 629
 630 **/
 631 BOOLEAN
 632 IsHostAttached (
 633   VOID
 634   )
 635 {
 636   return (BOOLEAN) (GetDebugFlag (DEBUG_AGENT_FLAG_HOST_ATTACHED) == 1);
 637 }
 638
 639 /**
 640   Set HOST connect flag in Mailbox.
 641
 642   @param[in] Attached        Attach status.
 643
 644 **/
 645 VOID
 646 SetHostAttached (
 647   IN BOOLEAN                      Attached
 648   )
 649 {
 650   DebugAgentMsgPrint (DEBUG_AGENT_INFO, "Attach status is %d\n", Attached);
 651   SetDebugFlag (DEBUG_AGENT_FLAG_HOST_ATTACHED, (UINT32)Attached);
 652 }
 653
 654 /**
 655   Set debug setting of Debug Agent in Mailbox.
 656
 657   @param DebugSetting         Pointer to Debug Setting defined by transfer protocol.
 658
 659   @retval RETURN_SUCCESS      The setting is set successfully.
 660   @retval RETURN_UNSUPPORTED  The Key value is not supported.
 661
 662 **/
 663 RETURN_STATUS
 664 SetDebugSetting (
 665   IN DEBUG_DATA_SET_DEBUG_SETTING  *DebugSetting
 666   )
 667 {
 668   RETURN_STATUS                Status;
 669
 670   Status = RETURN_SUCCESS;
 671   switch (DebugSetting->Key) {
 672   case DEBUG_AGENT_SETTING_SMM_ENTRY_BREAK:
 673     SetDebugFlag (DEBUG_AGENT_FLAG_BREAK_ON_NEXT_SMI, DebugSetting->Value);
 674     break;
 675   case DEBUG_AGENT_SETTING_PRINT_ERROR_LEVEL:
 676     SetDebugFlag (DEBUG_AGENT_FLAG_PRINT_ERROR_LEVEL, DebugSetting->Value);
 677     break;
 678   case DEBUG_AGENT_SETTING_BOOT_SCRIPT_ENTRY_BREAK:
 679     SetDebugFlag (DEBUG_AGENT_FLAG_BREAK_BOOT_SCRIPT, DebugSetting->Value);
 680     break;
 681   default:
 682     Status = RETURN_UNSUPPORTED;
 683   }
 684   return Status;
 685 }
 686
 687 /**
 688   Exectue GO command.
 689
 690   @param[in] CpuContext        Pointer to saved CPU context.
 691
 692 **/
 693 VOID
 694 CommandGo (
 695   IN DEBUG_CPU_CONTEXT         *CpuContext
 696   )
 697 {
 698   IA32_EFLAGS32                *Eflags;
 699
 700   Eflags = (IA32_EFLAGS32 *) &CpuContext->Eflags;
 701   Eflags->Bits.TF = 0;
 702   Eflags->Bits.RF = 1;
 703 }
 704
 705 /**
 706   Exectue Stepping command.
 707
 708   @param[in] CpuContext        Pointer to saved CPU context.
 709
 710 **/
 711 VOID
 712 CommandStepping (
 713   IN DEBUG_CPU_CONTEXT          *CpuContext
 714   )
 715 {
 716   IA32_EFLAGS32                *Eflags;
 717
 718   Eflags = (IA32_EFLAGS32 *) &CpuContext->Eflags;
 719   Eflags->Bits.TF = 1;
 720   Eflags->Bits.RF = 1;
 721 }
 722
 723 /**
 724   Set debug register for hardware breakpoint.
 725
 726   @param[in] CpuContext      Pointer to saved CPU context.
 727   @param[in] SetHwBreakpoint Hardware breakpoint to be set.
 728
 729 **/
 730 VOID
 731 SetDebugRegister (
 732   IN DEBUG_CPU_CONTEXT             *CpuContext,
 733   IN DEBUG_DATA_SET_HW_BREAKPOINT  *SetHwBreakpoint
 734   )
 735 {
 736   UINT8                      RegisterIndex;
 737   UINTN                      Dr7Value;
 738
 739   RegisterIndex = SetHwBreakpoint->Type.Index;
 740
 741   //
 742   // Set debug address
 743   //
 744   * ((UINTN *) &CpuContext->Dr0 + RegisterIndex) = (UINTN) SetHwBreakpoint->Address;
 745
 746   Dr7Value = CpuContext->Dr7;
 747
 748   //
 749   // Enable Gx, Lx
 750   //
 751   Dr7Value |= (UINTN) (0x3 << (RegisterIndex * 2));
 752   //
 753   // Set RWx and Lenx
 754   //
 755   Dr7Value &= (UINTN) (~(0xf << (16 + RegisterIndex * 4)));
 756   Dr7Value |= (UINTN) ((SetHwBreakpoint->Type.Length << 2) | SetHwBreakpoint->Type.Access) << (16 + RegisterIndex * 4);
 757   //
 758   // Enable GE, LE
 759   //
 760   Dr7Value |= 0x300;
 761
 762   CpuContext->Dr7 = Dr7Value;
 763 }
 764
 765 /**
 766   Clear debug register for hardware breakpoint.
 767
 768   @param[in] CpuContext        Pointer to saved CPU context.
 769   @param[in] ClearHwBreakpoint Hardware breakpoint to be cleared.
 770
 771 **/
 772 VOID
 773 ClearDebugRegister (
 774   IN DEBUG_CPU_CONTEXT                 *CpuContext,
 775   IN DEBUG_DATA_CLEAR_HW_BREAKPOINT    *ClearHwBreakpoint
 776   )
 777 {
 778   if ((ClearHwBreakpoint->IndexMask & BIT0) != 0) {
 779     CpuContext->Dr0 = 0;
 780     CpuContext->Dr7 &= (UINTN)(~(0x3 << 0));
 781   }
 782   if ((ClearHwBreakpoint->IndexMask & BIT1) != 0) {
 783     CpuContext->Dr1 = 0;
 784     CpuContext->Dr7 &= (UINTN)(~(0x3 << 2));
 785   }
 786   if ((ClearHwBreakpoint->IndexMask & BIT2) != 0) {
 787     CpuContext->Dr2 = 0;
 788     CpuContext->Dr7 &= (UINTN)(~(0x3 << 4));
 789   }
 790   if ((ClearHwBreakpoint->IndexMask & BIT3) != 0) {
 791     CpuContext->Dr3 = 0;
 792     CpuContext->Dr7 &= (UINTN)(~(0x3 << 6));
 793   }
 794 }
 795
 796
 797 /**
 798   Return the offset of FP / MMX / XMM registers in the FPU saved state by register index.
 799
 800   @param[in]  Index    Register index.
 801   @param[out] Width    Register width returned.
 802
 803   @return Offset in the FPU Save State.
 804
 805 **/
 806 UINT16
 807 ArchReadFxStatOffset (
 808   IN  UINT8                     Index,
 809   OUT UINT8                     *Width
 810   )
 811 {
 812   if (Index < SOFT_DEBUGGER_REGISTER_ST0) {
 813     switch (Index) {
 814     case SOFT_DEBUGGER_REGISTER_FP_FCW:
 815       *Width = (UINT8) sizeof (UINT16);
 816       return OFFSET_OF(DEBUG_DATA_FX_SAVE_STATE, Fcw);
 817
 818     case SOFT_DEBUGGER_REGISTER_FP_FSW:
 819       *Width = (UINT8) sizeof (UINT16);
 820       return OFFSET_OF(DEBUG_DATA_FX_SAVE_STATE, Fsw);
 821
 822     case SOFT_DEBUGGER_REGISTER_FP_FTW:
 823       *Width = (UINT8) sizeof (UINT16);
 824       return OFFSET_OF(DEBUG_DATA_FX_SAVE_STATE, Ftw);
 825
 826     case SOFT_DEBUGGER_REGISTER_FP_OPCODE:
 827       *Width = (UINT8) sizeof (UINT16);
 828       return OFFSET_OF(DEBUG_DATA_FX_SAVE_STATE, Opcode);
 829
 830     case SOFT_DEBUGGER_REGISTER_FP_EIP:
 831       *Width = (UINT8) sizeof (UINT32);
 832       return OFFSET_OF(DEBUG_DATA_FX_SAVE_STATE, Eip);
 833
 834     case SOFT_DEBUGGER_REGISTER_FP_CS:
 835       *Width = (UINT8) sizeof (UINT16);
 836       return OFFSET_OF(DEBUG_DATA_FX_SAVE_STATE, Cs);
 837
 838     case SOFT_DEBUGGER_REGISTER_FP_DATAOFFSET:
 839       *Width = (UINT8) sizeof (UINT32);
 840       return OFFSET_OF(DEBUG_DATA_FX_SAVE_STATE, DataOffset);
 841
 842     case SOFT_DEBUGGER_REGISTER_FP_DS:
 843       *Width = (UINT8) sizeof (UINT16);
 844       return OFFSET_OF(DEBUG_DATA_FX_SAVE_STATE, Ds);
 845
 846     case SOFT_DEBUGGER_REGISTER_FP_MXCSR:
 847       *Width = (UINT8) sizeof (UINT32);
 848       return OFFSET_OF(DEBUG_DATA_FX_SAVE_STATE, Mxcsr);
 849
 850     case SOFT_DEBUGGER_REGISTER_FP_MXCSR_MASK:
 851       *Width = (UINT8) sizeof (UINT32);
 852       return OFFSET_OF(DEBUG_DATA_FX_SAVE_STATE, Mxcsr_Mask);
 853     }
 854   }
 855
 856   if (Index <= SOFT_DEBUGGER_REGISTER_ST7) {
 857     *Width = 10;
 858   } else if (Index <= SOFT_DEBUGGER_REGISTER_XMM15) {
 859     *Width = 16;
 860   } else {
 861     //
 862     // MMX register
 863     //
 864     *Width = 8;
 865     Index -= SOFT_DEBUGGER_REGISTER_MM0 - SOFT_DEBUGGER_REGISTER_ST0;
 866   }
 867
 868   return OFFSET_OF (DEBUG_DATA_FX_SAVE_STATE, St0Mm0) + (Index - SOFT_DEBUGGER_REGISTER_ST0) * 16;
 869 }
 870
 871 /**
 872   Return the pointer of the register value in the CPU saved context.
 873
 874   @param[in]  CpuContext         Pointer to saved CPU context.
 875   @param[in]  Index              Register index value.
 876   @param[out] Width              Data width to read.
 877
 878   @return The pointer in the CPU saved context.
 879
 880 **/
 881 UINT8 *
 882 ArchReadRegisterBuffer (
 883   IN DEBUG_CPU_CONTEXT               *CpuContext,
 884   IN UINT8                           Index,
 885   OUT UINT8                          *Width
 886   )
 887 {
 888   UINT8           *Buffer;
 889
 890   if (Index < SOFT_DEBUGGER_REGISTER_FP_BASE) {
 891     Buffer = (UINT8 *) CpuContext + OFFSET_OF (DEBUG_CPU_CONTEXT, Dr0) + Index * sizeof (UINTN);
 892     *Width = (UINT8) sizeof (UINTN);
 893   } else {
 894     //
 895     // FPU/MMX/XMM registers
 896     //
 897     Buffer = (UINT8 *) CpuContext + OFFSET_OF (DEBUG_CPU_CONTEXT, FxSaveState) + ArchReadFxStatOffset (Index, Width);
 898   }
 899
 900   return Buffer;
 901 }
 902
 903 /**
 904   Send the packet without data to HOST.
 905
 906   @param[in] CommandType    Type of Command.
 907   @param[in] SequenceNo     Sequence number.
 908
 909 **/
 910 VOID
 911 SendPacketWithoutData (
 912   IN UINT8                  CommandType,
 913   IN UINT8                  SequenceNo
 914   )
 915 {
 916   DEBUG_PACKET_HEADER       DebugHeader;
 917   DEBUG_PORT_HANDLE         Handle;
 918
 919   Handle = GetDebugPortHandle();
 920
 921   DebugHeader.StartSymbol = DEBUG_STARTING_SYMBOL_NORMAL;
 922   DebugHeader.Command     = CommandType;
 923   DebugHeader.Length      = sizeof (DEBUG_PACKET_HEADER);
 924   DebugHeader.SequenceNo  = SequenceNo;
 925   DebugHeader.Crc         = 0;
 926   DebugHeader.Crc         = CalculateCrc16 ((UINT8 *)&DebugHeader, sizeof (DEBUG_PACKET_HEADER), 0);
 927
 928   DebugAgentDataMsgPrint (DEBUG_AGENT_VERBOSE, TRUE, (UINT8 *) &DebugHeader, DebugHeader.Length);
 929   DebugPortWriteBuffer (Handle, (UINT8 *) &DebugHeader, DebugHeader.Length);
 930 }
 931
 932 /**
 933   Send acknowledge packet to HOST.
 934
 935   @param[in] AckCommand    Type of Acknowledge packet.
 936
 937 **/
 938 VOID
 939 SendAckPacket (
 940   IN UINT8                AckCommand
 941   )
 942 {
 943   UINT8                   SequenceNo;
 944   DEBUG_AGENT_MAILBOX     *Mailbox;
 945
 946   if (AckCommand != DEBUG_COMMAND_OK) {
 947     //
 948     // This is not ACK OK packet
 949     //
 950     DebugAgentMsgPrint (DEBUG_AGENT_ERROR, "Send ACK(%d)\n", AckCommand);
 951   }
 952   Mailbox = GetMailboxPointer();
 953   SequenceNo = Mailbox->HostSequenceNo;
 954   DebugAgentMsgPrint (DEBUG_AGENT_INFO, "SendAckPacket: SequenceNo = %x\n", SequenceNo);
 955   SendPacketWithoutData (AckCommand, SequenceNo);
 956   UpdateMailboxContent (Mailbox, DEBUG_MAILBOX_LAST_ACK, AckCommand);
 957 }
 958
 959 /**
 960   Receive valid packet from HOST.
 961
 962   @param[out] InputPacket         Buffer to receive packet.
 963   @param[out] BreakReceived       TRUE means break-in symbol received.
 964                                   FALSE means break-in symbol not received.
 965   @param[out] IncompatibilityFlag If IncompatibilityFlag is not NULL, return
 966                                   TRUE:  Compatible packet received.
 967                                   FALSE: Incompatible packet received.
 968   @param[in]  Timeout             Time out value to wait for acknowlege from HOST.
 969                                   The unit is microsecond.
 970   @param[in]  SkipStartSymbol     TRUE:  Skip time out when reading start symbol.
 971                                   FALSE: Does not Skip time out when reading start symbol.
 972
 973   @retval RETURN_SUCCESS   A valid package was reveived in InputPacket.
 974   @retval RETURN_TIMEOUT   Timeout occurs.
 975
 976 **/
 977 RETURN_STATUS
 978 ReceivePacket (
 979   OUT UINT8             *InputPacket,
 980   OUT BOOLEAN           *BreakReceived,
 981   OUT BOOLEAN           *IncompatibilityFlag, OPTIONAL
 982   IN  UINTN             Timeout,
 983   IN  BOOLEAN           SkipStartSymbol
 984   )
 985 {
 986   DEBUG_PACKET_HEADER   *DebugHeader;
 987   UINTN                 Received;
 988   DEBUG_PORT_HANDLE     Handle;
 989   UINT16                Crc;
 990   UINTN                 TimeoutForStartSymbol;
 991
 992   Handle = GetDebugPortHandle();
 993   if (SkipStartSymbol) {
 994     TimeoutForStartSymbol = 0;
 995   } else {
 996     TimeoutForStartSymbol = Timeout;
 997   }
 998
 999   DebugHeader = (DEBUG_PACKET_HEADER *) InputPacket;
1000   while (TRUE) {
1001     //
1002     // Find the valid start symbol
1003     //
1004     Received = DebugPortReadBuffer (Handle, &DebugHeader->StartSymbol, sizeof (DebugHeader->StartSymbol), TimeoutForStartSymbol);
1005     if (Received < sizeof (DebugHeader->StartSymbol)) {
1006       DebugAgentMsgPrint (DEBUG_AGENT_WARNING, "DebugPortReadBuffer(StartSymbol) timeout\n");
1007       return RETURN_TIMEOUT;
1008     }
1009
1010     if (DebugHeader->StartSymbol != DEBUG_STARTING_SYMBOL_NORMAL) {
1011       DebugAgentMsgPrint (DEBUG_AGENT_WARNING, "Invalid start symbol received [%02x]\n", DebugHeader->StartSymbol);
1012       continue;
1013     }
1014
1015     //
1016     // Read Package header till field Length
1017     //
1018     Received = DebugPortReadBuffer (
1019                  Handle,
1020                  (UINT8 *)DebugHeader + OFFSET_OF (DEBUG_PACKET_HEADER, Command),
1021                  OFFSET_OF (DEBUG_PACKET_HEADER, Length) + sizeof (DebugHeader->Length) - sizeof (DebugHeader->StartSymbol),
1022                  Timeout
1023                  );
1024     if (Received == 0) {
1025       DebugAgentMsgPrint (DEBUG_AGENT_ERROR, "DebugPortReadBuffer(Command) timeout\n");
1026       return RETURN_TIMEOUT;
1027     }
1028     if (DebugHeader->Length < sizeof (DEBUG_PACKET_HEADER)) {
1029       if (IncompatibilityFlag != NULL) {
1030         //
1031         // This is one old version debug packet format, set Incompatibility flag
1032         //
1033         *IncompatibilityFlag = TRUE;
1034       } else {
1035         //
1036         // Skip the bad small packet
1037         //
1038         continue;
1039       }
1040     } else {
1041       //
1042       // Read the payload data include the CRC field
1043       //
1044       Received = DebugPortReadBuffer (Handle, &DebugHeader->SequenceNo, (UINT8) (DebugHeader->Length - OFFSET_OF (DEBUG_PACKET_HEADER, SequenceNo)), Timeout);
1045       if (Received == 0) {
1046         DebugAgentMsgPrint (DEBUG_AGENT_ERROR, "DebugPortReadBuffer(SequenceNo) timeout\n");
1047         return RETURN_TIMEOUT;
1048       }
1049       //
1050       // Calculate the CRC of Debug Packet
1051       //
1052       Crc = DebugHeader->Crc;
1053       DebugHeader->Crc = 0;
1054       if (Crc == CalculateCrc16 ((UINT8 *) DebugHeader, DebugHeader->Length, 0)) {
1055         break;
1056       }
1057       DebugAgentMsgPrint (DEBUG_AGENT_WARNING, "CRC Error (received CRC is %x)\n", Crc);
1058       DebugAgentDataMsgPrint (DEBUG_AGENT_VERBOSE, FALSE, (UINT8 *) DebugHeader, DebugHeader->Length);
1059     }
1060   }
1061
1062   DebugAgentDataMsgPrint (DEBUG_AGENT_VERBOSE, FALSE, (UINT8 *) DebugHeader, DebugHeader->Length);
1063
1064   return RETURN_SUCCESS;
1065 }
1066
1067 /**
1068   Receive acknowledge packet OK from HOST in specified time.
1069
1070   @param[in]  Command             The command type issued by TARGET.
1071   @param[in]  Timeout             Time out value to wait for acknowlege from HOST.
1072                                   The unit is microsecond.
1073   @param[out] BreakReceived       If BreakReceived is not NULL,
1074                                   TRUE is retured if break-in symbol received.
1075                                   FALSE is retured if break-in symbol not received.
1076   @param[out] IncompatibilityFlag If IncompatibilityFlag is not NULL, return
1077                                   TRUE:  Compatible packet received.
1078                                   FALSE: Incompatible packet received.
1079
1080   @retval  RETRUEN_SUCCESS  Succeed to receive acknowlege packet from HOST,
1081                             the type of acknowlege packet saved in Ack.
1082   @retval  RETURN_TIMEOUT   Specified timeout value was up.
1083
1084 **/
1085 RETURN_STATUS
1086 SendCommandAndWaitForAckOK (
1087   IN  UINT8               Command,
1088   IN  UINTN               Timeout,
1089   OUT BOOLEAN             *BreakReceived, OPTIONAL
1090   OUT BOOLEAN             *IncompatibilityFlag OPTIONAL
1091   )
1092 {
1093   RETURN_STATUS           Status;
1094   UINT8                   InputPacketBuffer[DEBUG_DATA_UPPER_LIMIT];
1095   DEBUG_PACKET_HEADER     *DebugHeader;
1096   UINT8                   SequenceNo;
1097   UINT8                   HostSequenceNo;
1098   UINT8                   RetryCount;
1099
1100   RetryCount  = 3;
1101   DebugHeader = (DEBUG_PACKET_HEADER *) InputPacketBuffer;
1102   Status      = RETURN_TIMEOUT;
1103   while (RetryCount > 0) {
1104     SequenceNo = GetMailboxPointer()->SequenceNo;
1105     HostSequenceNo = GetMailboxPointer()->HostSequenceNo;
1106     SendPacketWithoutData (Command, SequenceNo);
1107     Status = ReceivePacket ((UINT8 *) DebugHeader, BreakReceived, IncompatibilityFlag, Timeout, FALSE);
1108     if (Status == RETURN_TIMEOUT) {
1109       if (Command == DEBUG_COMMAND_INIT_BREAK) {
1110         RetryCount--;
1111       } else {
1112         DebugAgentMsgPrint (DEBUG_AGENT_WARNING, "TARGET: Timeout when waiting for ACK packet.\n");
1113       }
1114       continue;
1115     }
1116     ASSERT_EFI_ERROR (Status);
1117     //
1118     // Status == RETURN_SUCCESS
1119     //
1120     if (DebugHeader->Command == DEBUG_COMMAND_OK && DebugHeader->SequenceNo == SequenceNo) {
1121       //
1122       // Received Ack OK
1123       //
1124       UpdateMailboxContent (GetMailboxPointer(), DEBUG_MAILBOX_SEQUENCE_NO_INDEX, ++SequenceNo);
1125       return Status;
1126     }
1127     if (DebugHeader->Command == DEBUG_COMMAND_GO && (DebugHeader->SequenceNo == HostSequenceNo || Command == DEBUG_COMMAND_INIT_BREAK)) {
1128       //
1129       // Received Old GO
1130       //
1131       if (Command == DEBUG_COMMAND_INIT_BREAK) {
1132         DebugAgentMsgPrint (DEBUG_AGENT_WARNING, "TARGET: Receive GO() in last boot\n");
1133       }
1134       SendPacketWithoutData (DEBUG_COMMAND_OK, DebugHeader->SequenceNo);
1135     }
1136   }
1137
1138   ASSERT (Command == DEBUG_COMMAND_INIT_BREAK);
1139   return Status;
1140 }
1141
1142 /**
1143   Get current break cause.
1144
1145   @param[in] Vector      Vector value of exception or interrupt.
1146   @param[in] CpuContext  Pointer to save CPU context.
1147
1148   @return The type of break cause defined by XXXX
1149
1150 **/
1151 UINT8
1152 GetBreakCause (
1153   IN UINTN                    Vector,
1154   IN DEBUG_CPU_CONTEXT        *CpuContext
1155   )
1156 {
1157   UINT8                    Cause;
1158
1159   Cause = DEBUG_DATA_BREAK_CAUSE_UNKNOWN;
1160
1161   switch (Vector) {
1162   case DEBUG_INT1_VECTOR:
1163   case DEBUG_INT3_VECTOR:
1164
1165     if (Vector == DEBUG_INT1_VECTOR) {
1166       //
1167       // INT 1
1168       //
1169       if ((CpuContext->Dr6 & BIT14) != 0) {
1170         Cause = DEBUG_DATA_BREAK_CAUSE_STEPPING;
1171         //
1172         // If it's single step, no need to check DR0, to ensure single step work in PeCoffExtraActionLib
1173         // (right after triggering a breakpoint to report image load/unload).
1174         //
1175         return Cause;
1176
1177       } else {
1178         Cause = DEBUG_DATA_BREAK_CAUSE_HW_BREAKPOINT;
1179       }
1180     } else {
1181       //
1182       // INT 3
1183       //
1184       Cause = DEBUG_DATA_BREAK_CAUSE_SW_BREAKPOINT;
1185     }
1186
1187     switch (CpuContext->Dr0) {
1188     case IMAGE_LOAD_SIGNATURE:
1189     case IMAGE_UNLOAD_SIGNATURE:
1190
1191       if (CpuContext->Dr3 == IO_PORT_BREAKPOINT_ADDRESS) {
1192
1193         Cause = (UINT8) ((CpuContext->Dr0 == IMAGE_LOAD_SIGNATURE) ?
1194           DEBUG_DATA_BREAK_CAUSE_IMAGE_LOAD : DEBUG_DATA_BREAK_CAUSE_IMAGE_UNLOAD);
1195       }
1196       break;
1197
1198     case SOFT_INTERRUPT_SIGNATURE:
1199
1200       if (CpuContext->Dr1 == MEMORY_READY_SIGNATURE) {
1201         Cause = DEBUG_DATA_BREAK_CAUSE_MEMORY_READY;
1202         CpuContext->Dr0 = 0;
1203       } else if (CpuContext->Dr1 == SYSTEM_RESET_SIGNATURE) {
1204         Cause = DEBUG_DATA_BREAK_CAUSE_SYSTEM_RESET;
1205         CpuContext->Dr0 = 0;
1206       }
1207       break;
1208
1209     default:
1210       break;
1211
1212     }
1213
1214     break;
1215
1216   case DEBUG_TIMER_VECTOR:
1217     Cause = DEBUG_DATA_BREAK_CAUSE_USER_HALT;
1218     break;
1219
1220   default:
1221     if (Vector < 20) {
1222       if (GetDebugFlag (DEBUG_AGENT_FLAG_STEPPING) == 1) {
1223         //
1224         // If stepping command is executing
1225         //
1226         Cause = DEBUG_DATA_BREAK_CAUSE_STEPPING;
1227       } else {
1228         Cause = DEBUG_DATA_BREAK_CAUSE_EXCEPTION;
1229       }
1230     }
1231     break;
1232   }
1233
1234   return Cause;
1235 }
1236
1237 /**
1238   Copy memory from source to destination with specified width.
1239
1240   @param[out] Dest        A pointer to the destination buffer of the memory copy.
1241   @param[in]  Src         A pointer to the source buffer of the memory copy.
1242   @param[in]  Count       The number of data with specified width to copy from source to destination.
1243   @param[in]  Width       Data width in byte.
1244
1245 **/
1246 VOID
1247 CopyMemByWidth (
1248   OUT UINT8               *Dest,
1249   IN  UINT8               *Src,
1250   IN  UINT16              Count,
1251   IN  UINT8               Width
1252   )
1253 {
1254   UINT8                   *Destination;
1255   UINT8                   *Source;
1256   INT8                    Step;
1257
1258   if (Src > Dest) {
1259     Destination = Dest;
1260     Source      = Src;
1261     Step        = Width;
1262   } else {
1263     //
1264     // Copy memory from tail to avoid memory overlap
1265     //
1266     Destination = Dest + (Count - 1) * Width;
1267     Source      = Src  + (Count - 1) * Width;
1268     Step        = -Width;
1269   }
1270
1271   while (Count-- != 0) {
1272     switch (Width) {
1273     case 1:
1274       *(UINT8 *) Destination = MmioRead8 ((UINTN) Source);
1275       break;
1276     case 2:
1277       *(UINT16 *) Destination = MmioRead16 ((UINTN) Source);
1278       break;
1279     case 4:
1280       *(UINT32 *) Destination = MmioRead32 ((UINTN) Source);
1281       break;
1282     case 8:
1283       *(UINT64 *) Destination = MmioRead64 ((UINTN) Source);
1284       break;
1285     default:
1286       ASSERT (FALSE);
1287     }
1288     Source      += Step;
1289     Destination += Step;
1290   }
1291 }
1292
1293 /**
1294   Read memory with speicifed width and send packet with response data to HOST.
1295
1296   @param[in] Data        Pointer to response data buffer.
1297   @param[in] Count       The number of data with specified Width.
1298   @param[in] Width       Data width in byte.
1299
1300   @retval RETURN_SUCCESS      Response data was sent successfully.
1301
1302 **/
1303 RETURN_STATUS
1304 ReadMemoryAndSendResponsePacket (
1305   IN UINT8                *Data,
1306   IN UINT16               Count,
1307   IN UINT8                Width
1308   )
1309 {
1310   RETURN_STATUS        Status;
1311   DEBUG_PACKET_HEADER  *DebugHeader;
1312   BOOLEAN              LastPacket;
1313   DEBUG_PACKET_HEADER  *AckDebugHeader;
1314   UINT8                DebugPacket[DEBUG_DATA_UPPER_LIMIT + sizeof (UINT64) - 1];
1315   UINT8                InputPacketBuffer[DEBUG_DATA_UPPER_LIMIT];
1316   DEBUG_PORT_HANDLE    Handle;
1317   UINT8                SequenceNo;
1318   UINTN                RemainingDataSize;
1319   UINTN                CurrentDataSize;
1320
1321   Handle = GetDebugPortHandle();
1322
1323   //
1324   // Data is appended end of Debug Packet header,  make sure data address
1325   // in Debug Packet 8-byte alignment always
1326   //
1327   DebugHeader = (DEBUG_PACKET_HEADER *) (ALIGN_VALUE ((UINTN)&DebugPacket + sizeof (DEBUG_PACKET_HEADER), sizeof (UINT64))
1328                                          - sizeof (DEBUG_PACKET_HEADER));
1329   DebugHeader->StartSymbol = DEBUG_STARTING_SYMBOL_NORMAL;
1330
1331   RemainingDataSize = Count * Width;
1332   while (TRUE) {
1333     SequenceNo = GetMailboxPointer()->HostSequenceNo;
1334     if (RemainingDataSize <= DEBUG_DATA_MAXIMUM_REAL_DATA) {
1335       //
1336       // If the remaining data is less one real packet size, this is the last data packet
1337       //
1338       CurrentDataSize = RemainingDataSize;
1339       LastPacket = TRUE;
1340       DebugHeader->Command = DEBUG_COMMAND_OK;
1341     } else {
1342       //
1343       // Data is too larger to be sent in one packet, calculate the actual data size could
1344       // be sent in one Maximum data packet
1345       //
1346       CurrentDataSize = (DEBUG_DATA_MAXIMUM_REAL_DATA / Width) * Width;
1347       LastPacket = FALSE;
1348       DebugHeader->Command = DEBUG_COMMAND_IN_PROGRESS;
1349     }
1350     //
1351     // Construct the rest Debug header
1352     //
1353     DebugHeader->Length     = (UINT8)(CurrentDataSize + sizeof (DEBUG_PACKET_HEADER));
1354     DebugHeader->SequenceNo = SequenceNo;
1355     DebugHeader->Crc        = 0;
1356     CopyMemByWidth ((UINT8 *)(DebugHeader + 1), Data, (UINT16) CurrentDataSize / Width, Width);
1357     //
1358     // Calculate and fill the checksum, DebugHeader->Crc should be 0 before invoking CalculateCrc16 ()
1359     //
1360     DebugHeader->Crc = CalculateCrc16 ((UINT8 *) DebugHeader, DebugHeader->Length, 0);
1361
1362     DebugAgentDataMsgPrint (DEBUG_AGENT_VERBOSE, TRUE, (UINT8 *) DebugHeader, DebugHeader->Length);
1363
1364     DebugPortWriteBuffer (Handle, (UINT8 *) DebugHeader, DebugHeader->Length);
1365
1366     while (TRUE) {
1367       Status = ReceivePacket (InputPacketBuffer, NULL, NULL, READ_PACKET_TIMEOUT, FALSE);
1368       if (Status == RETURN_TIMEOUT) {
1369         DebugAgentMsgPrint (DEBUG_AGENT_WARNING, "TARGET: Timeout in SendDataResponsePacket()\n");
1370         break;
1371       }
1372       AckDebugHeader = (DEBUG_PACKET_HEADER *) InputPacketBuffer;
1373       SequenceNo = AckDebugHeader->SequenceNo;
1374       if (AckDebugHeader->Command == DEBUG_COMMAND_OK &&
1375           SequenceNo == DebugHeader->SequenceNo &&
1376           LastPacket) {
1377         //
1378         // If this is the last packet, return RETURN_SUCCESS.
1379         //
1380         return RETURN_SUCCESS;
1381       }
1382       if ((SequenceNo == (UINT8) (DebugHeader->SequenceNo + 1)) && (AckDebugHeader->Command == DEBUG_COMMAND_CONTINUE)) {
1383         //
1384         // Calculate the rest data size
1385         //
1386         Data              += CurrentDataSize;
1387         RemainingDataSize -= CurrentDataSize;
1388         UpdateMailboxContent (GetMailboxPointer(), DEBUG_MAILBOX_HOST_SEQUENCE_NO_INDEX, (UINT8) SequenceNo);
1389         break;
1390       }
1391       if (SequenceNo >= DebugHeader->SequenceNo) {
1392         DebugAgentMsgPrint (DEBUG_AGENT_WARNING, "TARGET: Received one old or new command(SequenceNo is %x, last SequenceNo is %x)\n", SequenceNo, DebugHeader->SequenceNo);
1393         break;
1394       }
1395     }
1396   }
1397 }
1398
1399 /**
1400   Send packet with response data to HOST.
1401
1402   @param[in] Data        Pointer to response data buffer.
1403   @param[in] DataSize    Size of response data in byte.
1404
1405   @retval RETURN_SUCCESS      Response data was sent successfully.
1406
1407 **/
1408 RETURN_STATUS
1409 SendDataResponsePacket (
1410   IN UINT8                *Data,
1411   IN UINT16               DataSize
1412   )
1413 {
1414   return ReadMemoryAndSendResponsePacket (Data, DataSize, 1);
1415 }
1416
1417 /**
1418   Send break cause packet to HOST.
1419
1420   @param[in] Vector      Vector value of exception or interrutp.
1421   @param[in] CpuContext  Pointer to save CPU context.
1422
1423   @retval RETURN_SUCCESS      Response data was sent successfully.
1424   @retval RETURN_DEVICE_ERROR Cannot receive DEBUG_COMMAND_OK from HOST.
1425
1426 **/
1427 RETURN_STATUS
1428 SendBreakCausePacket (
1429   IN UINTN                    Vector,
1430   IN DEBUG_CPU_CONTEXT        *CpuContext
1431   )
1432 {
1433   DEBUG_DATA_RESPONSE_BREAK_CAUSE    DebugDataBreakCause;
1434
1435   DebugDataBreakCause.StopAddress = CpuContext->Eip;
1436   DebugDataBreakCause.Cause       = GetBreakCause (Vector, CpuContext);
1437
1438   return SendDataResponsePacket ((UINT8 *) &DebugDataBreakCause, (UINT16) sizeof (DEBUG_DATA_RESPONSE_BREAK_CAUSE));
1439 }
1440
1441 /**
1442   Try to attach the HOST.
1443
1444   Send init break packet to HOST:
1445   If no acknowlege received in specified Timeout, return RETURN_TIMEOUT.
1446   If received acknowlege, check the revision of HOST.
1447   Set Attach Flag if attach successfully.
1448
1449   @param[in]  BreakCause     Break cause of this break event.
1450   @param[in]  Timeout        Time out value to wait for acknowlege from HOST.
1451                              The unit is microsecond.
1452   @param[out] BreakReceived  If BreakReceived is not NULL,
1453                              TRUE is retured if break-in symbol received.
1454                              FALSE is retured if break-in symbol not received.
1455 **/
1456 RETURN_STATUS
1457 AttachHost (
1458   IN  UINT8                BreakCause,
1459   IN  UINTN                Timeout,
1460   OUT BOOLEAN              *BreakReceived
1461   )
1462 {
1463   RETURN_STATUS                    Status;
1464   DEBUG_PORT_HANDLE                Handle;
1465   BOOLEAN                          IncompatibilityFlag;
1466
1467   IncompatibilityFlag = FALSE;
1468   Handle = GetDebugPortHandle();
1469
1470   //
1471   // Send init break and wait ack in Timeout
1472   //
1473   DebugPortWriteBuffer (Handle, (UINT8 *) mErrorMsgSendInitPacket, AsciiStrLen (mErrorMsgSendInitPacket));
1474   if (BreakCause == DEBUG_DATA_BREAK_CAUSE_SYSTEM_RESET) {
1475     Status = SendCommandAndWaitForAckOK (DEBUG_COMMAND_INIT_BREAK, Timeout, BreakReceived, &IncompatibilityFlag);
1476   } else {
1477     Status = SendCommandAndWaitForAckOK (DEBUG_COMMAND_ATTACH_BREAK, Timeout, BreakReceived, &IncompatibilityFlag);
1478   }
1479   if (IncompatibilityFlag) {
1480     //
1481     // If the incompatible Debug Packet received, the HOST should be running transfer protocol before DEBUG_AGENT_REVISION.
1482     // It could be UDK Debugger for Windows v1.1/v1.2 or for Linux v0.8/v1.2.
1483     //
1484     DebugPortWriteBuffer (Handle, (UINT8 *) mErrorMsgVersionAlert, AsciiStrLen (mErrorMsgVersionAlert));
1485     CpuDeadLoop ();
1486   }
1487
1488   if (RETURN_ERROR (Status)) {
1489     DebugPortWriteBuffer (Handle, (UINT8 *) mErrorMsgConnectFail, AsciiStrLen (mErrorMsgConnectFail));
1490   } else {
1491     DebugPortWriteBuffer (Handle, (UINT8 *) mErrorMsgConnectOK, AsciiStrLen (mErrorMsgConnectOK));
1492     //
1493     // Set Attach flag
1494     //
1495     SetHostAttached (TRUE);
1496   }
1497   return Status;
1498 }
1499
1500 /**
1501   Send Break point packet to HOST.
1502
1503   Only the first breaking processor could sent BREAK_POINT packet.
1504
1505   @param[in]  BreakCause     Break cause of this break event.
1506   @param[in]  ProcessorIndex Processor index value.
1507   @param[out] BreakReceived  If BreakReceived is not NULL,
1508                              TRUE is retured if break-in symbol received.
1509                              FALSE is retured if break-in symbol not received.
1510
1511 **/
1512 VOID
1513 SendBreakPacketToHost (
1514   IN  UINT8                BreakCause,
1515   IN  UINT32               ProcessorIndex,
1516   OUT BOOLEAN              *BreakReceived
1517   )
1518 {
1519   UINT8                 InputCharacter;
1520   DEBUG_PORT_HANDLE     Handle;
1521
1522   Handle = GetDebugPortHandle();
1523
1524   if (IsHostAttached ()) {
1525     DebugAgentMsgPrint (DEBUG_AGENT_INFO, "processor[%x]:Send Break Packet to HOST.\n", ProcessorIndex);
1526     SendCommandAndWaitForAckOK (DEBUG_COMMAND_BREAK_POINT, READ_PACKET_TIMEOUT, BreakReceived, NULL);
1527   } else {
1528     DebugAgentMsgPrint (DEBUG_AGENT_INFO, "processor[%x]:Try to attach HOST.\n", ProcessorIndex);
1529     //
1530     // If HOST is not attached, try to attach it firstly.
1531     //
1532     //
1533     // Poll Attach symbols from HOST and ack OK
1534     //
1535     do {
1536       DebugPortReadBuffer (Handle, &InputCharacter, 1, 0);
1537     } while (InputCharacter != DEBUG_STARTING_SYMBOL_ATTACH);
1538     SendAckPacket (DEBUG_COMMAND_OK);
1539
1540     //
1541     // Try to attach HOST
1542     //
1543     while (AttachHost (BreakCause, 0, NULL) != RETURN_SUCCESS);
1544
1545   }
1546 }
1547
1548 /**
1549   The main function to process communication with HOST.
1550
1551   It received the command packet from HOST, and sent response data packet to HOST.
1552
1553   @param[in]      Vector         Vector value of exception or interrutp.
1554   @param[in, out] CpuContext     Pointer to saved CPU context.
1555   @param[in]      BreakReceived  TRUE means break-in symbol received.
1556                                  FALSE means break-in symbol not received.
1557
1558 **/
1559 VOID
1560 CommandCommunication (
1561   IN     UINTN                   Vector,
1562   IN OUT DEBUG_CPU_CONTEXT       *CpuContext,
1563   IN     BOOLEAN                 BreakReceived
1564   )
1565 {
1566   RETURN_STATUS                     Status;
1567   UINT8                             InputPacketBuffer[DEBUG_DATA_UPPER_LIMIT + sizeof (UINT64) - 1];
1568   DEBUG_PACKET_HEADER               *DebugHeader;
1569   UINT8                             Width;
1570   UINT8                             Data8;
1571   UINT32                            Data32;
1572   UINT64                            Data64;
1573   DEBUG_DATA_READ_MEMORY            *MemoryRead;
1574   DEBUG_DATA_WRITE_MEMORY           *MemoryWrite;
1575   DEBUG_DATA_READ_IO                *IoRead;
1576   DEBUG_DATA_WRITE_IO               *IoWrite;
1577   DEBUG_DATA_READ_REGISTER          *RegisterRead;
1578   DEBUG_DATA_WRITE_REGISTER         *RegisterWrite;
1579   UINT8                             *RegisterBuffer;
1580   DEBUG_DATA_READ_MSR               *MsrRegisterRead;
1581   DEBUG_DATA_WRITE_MSR              *MsrRegisterWrite;
1582   DEBUG_DATA_CPUID                  *Cpuid;
1583   DEBUG_DATA_RESPONSE_CPUID         CpuidResponse;
1584   DEBUG_DATA_SEARCH_SIGNATURE       *SearchSignature;
1585   DEBUG_DATA_RESPONSE_GET_EXCEPTION Exception;
1586   DEBUG_DATA_RESPONSE_GET_REVISION  DebugAgentRevision;
1587   DEBUG_DATA_SET_VIEWPOINT          *SetViewPoint;
1588   BOOLEAN                           HaltDeferred;
1589   UINT32                            ProcessorIndex;
1590   DEBUG_PORT_HANDLE                 Handle;
1591   DEBUG_AGENT_EXCEPTION_BUFFER      AgentExceptionBuffer;
1592   UINT32                            IssuedViewPoint;
1593   DEBUG_AGENT_MAILBOX               *Mailbox;
1594   UINT8                             *AlignedDataPtr;
1595
1596   ProcessorIndex  = 0;
1597   IssuedViewPoint = 0;
1598   HaltDeferred    = BreakReceived;
1599
1600   if (MultiProcessorDebugSupport()) {
1601     ProcessorIndex = GetProcessorIndex ();
1602     SetCpuStopFlagByIndex (ProcessorIndex, TRUE);
1603     if (mDebugMpContext.ViewPointIndex == ProcessorIndex) {
1604       //
1605       // Only the current view processor could set AgentInProgress Flag.
1606       //
1607       IssuedViewPoint = ProcessorIndex;
1608     }
1609   }
1610
1611   if (IssuedViewPoint == ProcessorIndex) {
1612     //
1613     // Set AgentInProgress Flag.
1614     //
1615     SetDebugFlag (DEBUG_AGENT_FLAG_AGENT_IN_PROGRESS, 1);
1616   }
1617
1618   Handle = GetDebugPortHandle();
1619
1620   while (TRUE) {
1621
1622     if (MultiProcessorDebugSupport()) {
1623       //
1624       // Check if the current processor is HOST view point
1625       //
1626       if (mDebugMpContext.ViewPointIndex != ProcessorIndex) {
1627         if (mDebugMpContext.RunCommandSet) {
1628           //
1629           // If HOST view point sets RUN flag, run GO command to leave
1630           //
1631           SetCpuStopFlagByIndex (ProcessorIndex, FALSE);
1632           CommandGo (CpuContext);
1633           break;
1634         } else {
1635           //
1636           // Run into loop again
1637           //
1638           CpuPause ();
1639           continue;
1640         }
1641       }
1642     }
1643
1644     AcquireMpSpinLock (&mDebugMpContext.DebugPortSpinLock);
1645
1646     DebugHeader =(DEBUG_PACKET_HEADER *) InputPacketBuffer;
1647
1648     DebugAgentMsgPrint (DEBUG_AGENT_INFO, "TARGET: Try to get command from HOST...\n");
1649     Status = ReceivePacket ((UINT8 *)DebugHeader, &BreakReceived, NULL, READ_PACKET_TIMEOUT, TRUE);
1650     if (Status != RETURN_SUCCESS || !IS_REQUEST (DebugHeader)) {
1651       DebugAgentMsgPrint (DEBUG_AGENT_WARNING, "TARGET: Get command[%x] sequenceno[%x] returned status is [%x] \n", DebugHeader->Command, DebugHeader->SequenceNo, Status);
1652       DebugAgentMsgPrint (DEBUG_AGENT_WARNING, "TARGET: Get command failed or it's response packet not expected! \n");
1653       ReleaseMpSpinLock (&mDebugMpContext.DebugPortSpinLock);
1654       continue;
1655     }
1656
1657     Mailbox = GetMailboxPointer ();
1658     if (DebugHeader->SequenceNo == Mailbox->HostSequenceNo) {
1659       DebugAgentMsgPrint (DEBUG_AGENT_WARNING, "TARGET: Receive one old command[%x] agaist command[%x]\n", DebugHeader->SequenceNo, Mailbox->HostSequenceNo);
1660       SendAckPacket (Mailbox->LastAck);
1661       ReleaseMpSpinLock (&mDebugMpContext.DebugPortSpinLock);
1662       continue;
1663     } else if (DebugHeader->SequenceNo == (UINT8) (Mailbox->HostSequenceNo + 1)) {
1664       UpdateMailboxContent (Mailbox, DEBUG_MAILBOX_HOST_SEQUENCE_NO_INDEX, (UINT8) DebugHeader->SequenceNo);
1665     } else {
1666       DebugAgentMsgPrint (DEBUG_AGENT_WARNING, "Receive one invalid comamnd[%x] agaist command[%x]\n", DebugHeader->SequenceNo, Mailbox->HostSequenceNo);
1667       ReleaseMpSpinLock (&mDebugMpContext.DebugPortSpinLock);
1668       continue;
1669     }
1670
1671     //
1672     // Save CPU content before executing HOST commond
1673     //
1674     UpdateMailboxContent (Mailbox, DEBUG_MAILBOX_EXCEPTION_BUFFER_POINTER_INDEX, (UINT64)(UINTN) &AgentExceptionBuffer.JumpBuffer);
1675     if (SetJump (&AgentExceptionBuffer.JumpBuffer) != 0) {
1676       //
1677       // If HOST command failed, continue to wait for HOST's next command
1678       // If needed, agent could send exception info to HOST.
1679       //
1680       SendAckPacket (DEBUG_COMMAND_ABORT);
1681       ReleaseMpSpinLock (&mDebugMpContext.DebugPortSpinLock);
1682       continue;
1683     }
1684
1685     DebugAgentMsgPrint (DEBUG_AGENT_INFO, "Processor[%x]:Received one command(%x)\n", mDebugMpContext.ViewPointIndex, DebugHeader->Command);
1686
1687     switch (DebugHeader->Command) {
1688
1689     case DEBUG_COMMAND_HALT:
1690       SendAckPacket (DEBUG_COMMAND_HALT_DEFERRED);
1691       HaltDeferred = TRUE;
1692       BreakReceived = FALSE;
1693       Status = RETURN_SUCCESS;
1694       break;
1695
1696     case DEBUG_COMMAND_RESET:
1697       SendAckPacket (DEBUG_COMMAND_OK);
1698       SendAckPacket (DEBUG_COMMAND_OK);
1699       SendAckPacket (DEBUG_COMMAND_OK);
1700       ReleaseMpSpinLock (&mDebugMpContext.DebugPortSpinLock);
1701
1702       ResetCold ();
1703       //
1704       // Assume system resets in 2 seconds, otherwise send TIMEOUT packet.
1705       // PCD can be used if 2 seconds isn't long enough for some platforms.
1706       //
1707       MicroSecondDelay (2000000);
1708       UpdateMailboxContent (Mailbox, DEBUG_MAILBOX_HOST_SEQUENCE_NO_INDEX, Mailbox->HostSequenceNo + 1);
1709       SendAckPacket (DEBUG_COMMAND_TIMEOUT);
1710       SendAckPacket (DEBUG_COMMAND_TIMEOUT);
1711       SendAckPacket (DEBUG_COMMAND_TIMEOUT);
1712       break;
1713
1714     case DEBUG_COMMAND_GO:
1715       CommandGo (CpuContext);
1716       //
1717       // Clear Dr0 to avoid to be recognized as IMAGE_LOAD/_UNLOAD again when hitting a breakpoint after GO
1718       // If HOST changed Dr0 before GO, we will not change Dr0 here
1719       //
1720       Data8 = GetBreakCause (Vector, CpuContext);
1721       if (Data8 == DEBUG_DATA_BREAK_CAUSE_IMAGE_LOAD || Data8 == DEBUG_DATA_BREAK_CAUSE_IMAGE_UNLOAD) {
1722         CpuContext->Dr0 = 0;
1723       }
1724       //
1725       // Clear Stepping Flag
1726       //
1727       SetDebugFlag (DEBUG_AGENT_FLAG_STEPPING, 0);
1728
1729       if (!HaltDeferred) {
1730         //
1731         // If no HALT command received when being in-active mode
1732         //
1733         if (MultiProcessorDebugSupport()) {
1734           Data32 = FindNextPendingBreakCpu ();
1735           if (Data32 != -1) {
1736             //
1737             // If there are still others processors being in break state,
1738             // send OK packet to HOST to finish this go command
1739             //
1740             SendAckPacket (DEBUG_COMMAND_OK);
1741             CpuPause ();
1742             //
1743             // Set current view to the next breaking processor
1744             //
1745             mDebugMpContext.ViewPointIndex = Data32;
1746             mDebugMpContext.BreakAtCpuIndex = mDebugMpContext.ViewPointIndex;
1747             SetCpuBreakFlagByIndex (mDebugMpContext.ViewPointIndex, FALSE);
1748             //
1749             // Send break packet to HOST to let HOST break again
1750             //
1751             SendBreakPacketToHost (DEBUG_DATA_BREAK_CAUSE_UNKNOWN, mDebugMpContext.BreakAtCpuIndex, &BreakReceived);
1752             //
1753             // Continue to run into loop to read command packet from HOST
1754             //
1755             ReleaseMpSpinLock (&mDebugMpContext.DebugPortSpinLock);
1756             break;
1757           }
1758
1759           //
1760           // If no else processor break, set stop bitmask,
1761           // and set Running flag for all processors.
1762           //
1763           SetCpuStopFlagByIndex (ProcessorIndex, FALSE);
1764           SetCpuRunningFlag (TRUE);
1765           CpuPause ();
1766           //
1767           // Wait for all processors are in running state
1768           //
1769           while (TRUE) {
1770             if (IsAllCpuRunning ()) {
1771               break;
1772             }
1773           }
1774           //
1775           // Set BSP to be current view point.
1776           //
1777           SetDebugViewPoint (mDebugMpContext.BspIndex);
1778           CpuPause ();
1779           //
1780           // Clear breaking processor index and running flag
1781           //
1782           mDebugMpContext.BreakAtCpuIndex = (UINT32) (-1);
1783           SetCpuRunningFlag (FALSE);
1784         }
1785
1786         //
1787         // Send OK packet to HOST to finish this go command
1788         //
1789         SendAckPacket (DEBUG_COMMAND_OK);
1790
1791         ReleaseMpSpinLock (&mDebugMpContext.DebugPortSpinLock);
1792
1793         if (!IsHostAttached()) {
1794           UpdateMailboxContent (Mailbox, DEBUG_MAILBOX_SEQUENCE_NO_INDEX, 0);
1795           UpdateMailboxContent (Mailbox, DEBUG_MAILBOX_HOST_SEQUENCE_NO_INDEX, 0);
1796         }
1797         return;
1798
1799       } else {
1800         //
1801         // If reveived HALT command, need to defer the GO command
1802         //
1803         SendAckPacket (DEBUG_COMMAND_HALT_PROCESSED);
1804         HaltDeferred = FALSE;
1805
1806         Vector = DEBUG_TIMER_VECTOR;
1807       }
1808       break;
1809
1810     case DEBUG_COMMAND_BREAK_CAUSE:
1811
1812       if (MultiProcessorDebugSupport() && ProcessorIndex != mDebugMpContext.BreakAtCpuIndex) {
1813         Status = SendBreakCausePacket (DEBUG_TIMER_VECTOR, CpuContext);
1814
1815       } else {
1816         Status = SendBreakCausePacket (Vector, CpuContext);
1817       }
1818
1819       break;
1820
1821     case DEBUG_COMMAND_SET_HW_BREAKPOINT:
1822       SetDebugRegister (CpuContext, (DEBUG_DATA_SET_HW_BREAKPOINT *) (DebugHeader + 1));
1823       SendAckPacket (DEBUG_COMMAND_OK);
1824       break;
1825
1826     case DEBUG_COMMAND_CLEAR_HW_BREAKPOINT:
1827       ClearDebugRegister (CpuContext, (DEBUG_DATA_CLEAR_HW_BREAKPOINT *) (DebugHeader + 1));
1828       SendAckPacket (DEBUG_COMMAND_OK);
1829       break;
1830
1831     case DEBUG_COMMAND_SINGLE_STEPPING:
1832       CommandStepping (CpuContext);
1833       //
1834       // Clear Dr0 to avoid to be recognized as IMAGE_LOAD/_UNLOAD again when hitting a breakpoint after GO
1835       // If HOST changed Dr0 before GO, we will not change Dr0 here
1836       //
1837       Data8 = GetBreakCause (Vector, CpuContext);
1838       if (Data8 == DEBUG_DATA_BREAK_CAUSE_IMAGE_LOAD || Data8 == DEBUG_DATA_BREAK_CAUSE_IMAGE_UNLOAD) {
1839         CpuContext->Dr0 = 0;
1840       }
1841
1842       mDebugMpContext.BreakAtCpuIndex = (UINT32) (-1);
1843       //
1844       // Set Stepping Flag
1845       //
1846       SetDebugFlag (DEBUG_AGENT_FLAG_STEPPING, 1);
1847       ReleaseMpSpinLock (&mDebugMpContext.DebugPortSpinLock);
1848       //
1849       // Executing stepping command directly without sending ACK packet,
1850       // ACK packet will be sent after stepping done.
1851       //
1852       return;
1853
1854     case DEBUG_COMMAND_SET_SW_BREAKPOINT:
1855       Data64 = (UINTN) (((DEBUG_DATA_SET_SW_BREAKPOINT *) (DebugHeader + 1))->Address);
1856       Data8 = *(UINT8 *) (UINTN) Data64;
1857       *(UINT8 *) (UINTN) Data64 = DEBUG_SW_BREAKPOINT_SYMBOL;
1858       Status = SendDataResponsePacket ((UINT8 *) &Data8, (UINT16) sizeof (UINT8));
1859       break;
1860
1861     case DEBUG_COMMAND_READ_MEMORY:
1862       MemoryRead = (DEBUG_DATA_READ_MEMORY *) (DebugHeader + 1);
1863       Status = ReadMemoryAndSendResponsePacket ((UINT8 *) (UINTN) MemoryRead->Address, MemoryRead->Count, MemoryRead->Width);
1864       break;
1865
1866     case DEBUG_COMMAND_WRITE_MEMORY:
1867       MemoryWrite = (DEBUG_DATA_WRITE_MEMORY *) (DebugHeader + 1);
1868       //
1869       // Copy data into one memory with 8-byte alignment address
1870       //
1871       AlignedDataPtr = ALIGN_POINTER ((UINT8 *) &MemoryWrite->Data, sizeof (UINT64));
1872       if (AlignedDataPtr != (UINT8 *) &MemoryWrite->Data) {
1873         CopyMem (AlignedDataPtr, (UINT8 *) &MemoryWrite->Data, MemoryWrite->Count * MemoryWrite->Width);
1874       }
1875       CopyMemByWidth ((UINT8 *) (UINTN) MemoryWrite->Address, AlignedDataPtr, MemoryWrite->Count, MemoryWrite->Width);
1876       SendAckPacket (DEBUG_COMMAND_OK);
1877       break;
1878
1879     case DEBUG_COMMAND_READ_IO:
1880       IoRead = (DEBUG_DATA_READ_IO *) (DebugHeader + 1);
1881       switch (IoRead->Width) {
1882       case 1:
1883         Data64  = IoRead8 ((UINTN) IoRead->Port);
1884         break;
1885       case 2:
1886         Data64  = IoRead16 ((UINTN) IoRead->Port);
1887         break;
1888       case 4:
1889         Data64  = IoRead32 ((UINTN) IoRead->Port);
1890         break;
1891       case 8:
1892         Data64  = IoRead64 ((UINTN) IoRead->Port);
1893         break;
1894       default:
1895         Data64  = (UINT64) -1;
1896       }
1897       Status = SendDataResponsePacket ((UINT8 *) &Data64, IoRead->Width);
1898       break;
1899
1900     case DEBUG_COMMAND_WRITE_IO:
1901       IoWrite = (DEBUG_DATA_WRITE_IO *) (DebugHeader + 1);
1902       switch (IoWrite->Width) {
1903       case 1:
1904         Data64  = IoWrite8 ((UINTN) IoWrite->Port, *(UINT8 *) &IoWrite->Data);
1905         break;
1906       case 2:
1907         Data64  = IoWrite16 ((UINTN) IoWrite->Port, *(UINT16 *) &IoWrite->Data);
1908         break;
1909       case 4:
1910         Data64  = IoWrite32 ((UINTN) IoWrite->Port, *(UINT32 *) &IoWrite->Data);
1911         break;
1912       case 8:
1913         Data64  = IoWrite64 ((UINTN) IoWrite->Port, *(UINT64 *) &IoWrite->Data);
1914         break;
1915       default:
1916         Data64  = (UINT64) -1;
1917       }
1918       SendAckPacket (DEBUG_COMMAND_OK);
1919       break;
1920
1921     case DEBUG_COMMAND_READ_ALL_REGISTERS:
1922       Status = SendDataResponsePacket ((UINT8 *) CpuContext, sizeof (*CpuContext));
1923       break;
1924
1925     case DEBUG_COMMAND_READ_REGISTER:
1926       RegisterRead = (DEBUG_DATA_READ_REGISTER *) (DebugHeader + 1);
1927
1928       if (RegisterRead->Index <= SOFT_DEBUGGER_REGISTER_MAX) {
1929         RegisterBuffer = ArchReadRegisterBuffer (CpuContext, RegisterRead->Index, &Width);
1930         Status = SendDataResponsePacket (RegisterBuffer, Width);
1931       } else {
1932         Status = RETURN_UNSUPPORTED;
1933       }
1934       break;
1935
1936     case DEBUG_COMMAND_WRITE_REGISTER:
1937       RegisterWrite = (DEBUG_DATA_WRITE_REGISTER *) (DebugHeader + 1);
1938       if (RegisterWrite->Index <= SOFT_DEBUGGER_REGISTER_MAX) {
1939         RegisterBuffer = ArchReadRegisterBuffer (CpuContext, RegisterWrite->Index, &Width);
1940         ASSERT (Width == RegisterWrite->Length);
1941         CopyMem (RegisterBuffer, RegisterWrite->Data, Width);
1942         SendAckPacket (DEBUG_COMMAND_OK);
1943       } else {
1944         Status = RETURN_UNSUPPORTED;
1945       }
1946       break;
1947
1948     case DEBUG_COMMAND_ARCH_MODE:
1949       Data8 = DEBUG_ARCH_SYMBOL;
1950       Status = SendDataResponsePacket ((UINT8 *) &Data8, (UINT16) sizeof (UINT8));
1951       break;
1952
1953     case DEBUG_COMMAND_READ_MSR:
1954       MsrRegisterRead = (DEBUG_DATA_READ_MSR *) (DebugHeader + 1);
1955       Data64 = AsmReadMsr64 (MsrRegisterRead->Index);
1956       Status = SendDataResponsePacket ((UINT8 *) &Data64, (UINT16) sizeof (UINT64));
1957       break;
1958
1959     case DEBUG_COMMAND_WRITE_MSR:
1960       MsrRegisterWrite = (DEBUG_DATA_WRITE_MSR *) (DebugHeader + 1);
1961       AsmWriteMsr64 (MsrRegisterWrite->Index, MsrRegisterWrite->Value);
1962       SendAckPacket (DEBUG_COMMAND_OK);
1963       break;
1964
1965     case DEBUG_COMMAND_SET_DEBUG_SETTING:
1966       Status = SetDebugSetting ((DEBUG_DATA_SET_DEBUG_SETTING *)(DebugHeader + 1));
1967       if (Status == RETURN_SUCCESS) {
1968         SendAckPacket (DEBUG_COMMAND_OK);
1969       }
1970       break;
1971
1972     case DEBUG_COMMAND_GET_REVISION:
1973       DebugAgentRevision.Revision = DEBUG_AGENT_REVISION;
1974       DebugAgentRevision.Capabilities = DEBUG_AGENT_CAPABILITIES;
1975       Status = SendDataResponsePacket ((UINT8 *) &DebugAgentRevision, (UINT16) sizeof (DEBUG_DATA_RESPONSE_GET_REVISION));
1976       break;
1977
1978     case DEBUG_COMMAND_GET_EXCEPTION:
1979       Exception.ExceptionNum  = (UINT8) Vector;
1980       Exception.ExceptionData = (UINT32) CpuContext->ExceptionData;
1981       Status = SendDataResponsePacket ((UINT8 *) &Exception, (UINT16) sizeof (DEBUG_DATA_RESPONSE_GET_EXCEPTION));
1982       break;
1983
1984     case DEBUG_COMMAND_SET_VIEWPOINT:
1985       SetViewPoint = (DEBUG_DATA_SET_VIEWPOINT *) (DebugHeader + 1);
1986       if (MultiProcessorDebugSupport()) {
1987         if (IsCpuStopped (SetViewPoint->ViewPoint)) {
1988           SetDebugViewPoint (SetViewPoint->ViewPoint);
1989           SendAckPacket (DEBUG_COMMAND_OK);
1990         } else {
1991           //
1992           // If CPU is not halted
1993           //
1994           SendAckPacket (DEBUG_COMMAND_NOT_SUPPORTED);
1995         }
1996       } else if (SetViewPoint->ViewPoint == 0) {
1997         SendAckPacket (DEBUG_COMMAND_OK);
1998
1999       } else {
2000         SendAckPacket (DEBUG_COMMAND_NOT_SUPPORTED);
2001       }
2002
2003       break;
2004
2005     case DEBUG_COMMAND_GET_VIEWPOINT:
2006       Data32 = mDebugMpContext.ViewPointIndex;
2007       SendDataResponsePacket((UINT8 *) &Data32, (UINT16) sizeof (UINT32));
2008       break;
2009
2010     case DEBUG_COMMAND_MEMORY_READY:
2011       Data8 = (UINT8) GetDebugFlag (DEBUG_AGENT_FLAG_MEMORY_READY);
2012       SendDataResponsePacket (&Data8, (UINT16) sizeof (UINT8));
2013       break;
2014
2015     case DEBUG_COMMAND_DETACH:
2016       SetHostAttached (FALSE);
2017       SendAckPacket (DEBUG_COMMAND_OK);
2018       break;
2019
2020     case DEBUG_COMMAND_CPUID:
2021       Cpuid = (DEBUG_DATA_CPUID *) (DebugHeader + 1);
2022       AsmCpuidEx (
2023         Cpuid->Eax, Cpuid->Ecx,
2024         &CpuidResponse.Eax, &CpuidResponse.Ebx,
2025         &CpuidResponse.Ecx, &CpuidResponse.Edx
2026         );
2027       SendDataResponsePacket ((UINT8 *) &CpuidResponse, (UINT16) sizeof (CpuidResponse));
2028       break;
2029
2030    case DEBUG_COMMAND_SEARCH_SIGNATURE:
2031       SearchSignature = (DEBUG_DATA_SEARCH_SIGNATURE *) (DebugHeader + 1);
2032       if ((SearchSignature->Alignment != 0) &&
2033           (SearchSignature->Alignment == GetPowerOfTwo32 (SearchSignature->Alignment))
2034          ) {
2035         if (SearchSignature->Positive) {
2036           for (
2037             Data64 = ALIGN_VALUE ((UINTN) SearchSignature->Start, SearchSignature->Alignment);
2038             Data64 <= SearchSignature->Start + SearchSignature->Count - SearchSignature->DataLength;
2039             Data64 += SearchSignature->Alignment
2040               ) {
2041             if (CompareMem ((VOID *) (UINTN) Data64, &SearchSignature->Data, SearchSignature->DataLength) == 0) {
2042               break;
2043             }
2044           }
2045           if (Data64 > SearchSignature->Start + SearchSignature->Count - SearchSignature->DataLength) {
2046             Data64 = (UINT64) -1;
2047           }
2048         } else {
2049           for (
2050             Data64 = ALIGN_VALUE ((UINTN) SearchSignature->Start - SearchSignature->Alignment, SearchSignature->Alignment);
2051             Data64 >= SearchSignature->Start - SearchSignature->Count;
2052             Data64 -= SearchSignature->Alignment
2053               ) {
2054             if (CompareMem ((VOID *) (UINTN) Data64, &SearchSignature->Data, SearchSignature->DataLength) == 0) {
2055               break;
2056             }
2057           }
2058           if (Data64 < SearchSignature->Start - SearchSignature->Count) {
2059             Data64 = (UINT64) -1;
2060           }
2061         }
2062         SendDataResponsePacket ((UINT8 *) &Data64, (UINT16) sizeof (Data64));
2063       } else {
2064         Status = RETURN_UNSUPPORTED;
2065       }
2066       break;
2067
2068     default:
2069       SendAckPacket (DEBUG_COMMAND_NOT_SUPPORTED);
2070       break;
2071     }
2072
2073     if (Status == RETURN_UNSUPPORTED) {
2074       SendAckPacket (DEBUG_COMMAND_NOT_SUPPORTED);
2075     } else if (Status != RETURN_SUCCESS) {
2076       SendAckPacket (DEBUG_COMMAND_ABORT);
2077     }
2078
2079     ReleaseMpSpinLock (&mDebugMpContext.DebugPortSpinLock);
2080     CpuPause ();
2081   }
2082 }
2083
2084 /**
2085   C function called in interrupt handler.
2086
2087   @param[in] Vector      Vector value of exception or interrutp.
2088   @param[in] CpuContext  Pointer to save CPU context.
2089
2090 **/
2091 VOID
2092 EFIAPI
2093 InterruptProcess (
2094   IN UINT32                          Vector,
2095   IN DEBUG_CPU_CONTEXT               *CpuContext
2096   )
2097 {
2098   UINT8                            InputCharacter;
2099   UINT8                            BreakCause;
2100   UINTN                            SavedEip;
2101   BOOLEAN                          BreakReceived;
2102   UINT32                           ProcessorIndex;
2103   UINT32                           CurrentDebugTimerInitCount;
2104   DEBUG_PORT_HANDLE                Handle;
2105   UINT8                            Data8;
2106   UINT8                            *Al;
2107   UINT32                           IssuedViewPoint;
2108   DEBUG_AGENT_EXCEPTION_BUFFER     *ExceptionBuffer;
2109
2110   InputCharacter  = 0;
2111   ProcessorIndex  = 0;
2112   IssuedViewPoint = 0;
2113   BreakReceived   = FALSE;
2114
2115   if (mSkipBreakpoint) {
2116     //
2117     // If Skip Breakpoint flag is set, means communication is disturbed by hardware SMI, we need to ignore the break points in SMM
2118     //
2119     if ((Vector == DEBUG_INT1_VECTOR) || (Vector == DEBUG_INT3_VECTOR)) {
2120       DebugPortWriteBuffer (GetDebugPortHandle(), (UINT8 *) mWarningMsgIngoreBreakpoint, AsciiStrLen (mWarningMsgIngoreBreakpoint));
2121       return;
2122     }
2123   }
2124
2125   if (MultiProcessorDebugSupport()) {
2126     ProcessorIndex = GetProcessorIndex ();
2127     //
2128     // If this processor has alreay halted before, need to check it later
2129     //
2130     if (IsCpuStopped (ProcessorIndex)) {
2131       IssuedViewPoint = ProcessorIndex;
2132     }
2133   }
2134
2135   if (IssuedViewPoint == ProcessorIndex && GetDebugFlag (DEBUG_AGENT_FLAG_STEPPING) != 1) {
2136     //
2137     // Check if this exception is issued by Debug Agent itself
2138     // If yes, fill the debug agent exception buffer and LongJump() back to
2139     // the saved CPU content in CommandCommunication()
2140     //
2141     if (GetDebugFlag (DEBUG_AGENT_FLAG_AGENT_IN_PROGRESS) == 1) {
2142       DebugAgentMsgPrint (DEBUG_AGENT_ERROR, "Debug agent meet one Exception, ExceptionNum is %d, EIP = 0x%x.\n", Vector, (UINTN)CpuContext->Eip);
2143       ExceptionBuffer = (DEBUG_AGENT_EXCEPTION_BUFFER *) (UINTN) GetMailboxPointer()->ExceptionBufferPointer;
2144       ExceptionBuffer->ExceptionContent.ExceptionNum  = (UINT8) Vector;
2145       ExceptionBuffer->ExceptionContent.ExceptionData = (UINT32) CpuContext->ExceptionData;
2146       LongJump ((BASE_LIBRARY_JUMP_BUFFER *)(UINTN)(ExceptionBuffer), 1);
2147     }
2148   }
2149
2150   if (MultiProcessorDebugSupport()) {
2151     //
2152     // If RUN commmand is executing, wait for it done.
2153     //
2154     while (mDebugMpContext.RunCommandSet) {
2155       CpuPause ();
2156     }
2157   }
2158
2159   Handle     = GetDebugPortHandle();
2160   BreakCause = GetBreakCause (Vector, CpuContext);
2161   switch (Vector) {
2162   case DEBUG_INT1_VECTOR:
2163   case DEBUG_INT3_VECTOR:
2164     switch (BreakCause) {
2165     case DEBUG_DATA_BREAK_CAUSE_SYSTEM_RESET:
2166       if (AttachHost (BreakCause, READ_PACKET_TIMEOUT, &BreakReceived) != RETURN_SUCCESS) {
2167         //
2168         // Try to connect HOST, return if fails
2169         //
2170         break;
2171       }
2172       CommandCommunication (Vector, CpuContext, BreakReceived);
2173       break;
2174
2175     case DEBUG_DATA_BREAK_CAUSE_STEPPING:
2176       //
2177       // Stepping is finished, send Ack package.
2178       //
2179       if (MultiProcessorDebugSupport()) {
2180         mDebugMpContext.BreakAtCpuIndex = ProcessorIndex;
2181       }
2182       SendAckPacket (DEBUG_COMMAND_OK);
2183       CommandCommunication (Vector, CpuContext, BreakReceived);
2184       break;
2185
2186     case DEBUG_DATA_BREAK_CAUSE_MEMORY_READY:
2187       //
2188       // Memory is ready
2189       //
2190       SendCommandAndWaitForAckOK (DEBUG_COMMAND_MEMORY_READY, READ_PACKET_TIMEOUT, &BreakReceived, NULL);
2191       CommandCommunication (Vector, CpuContext, BreakReceived);
2192       break;
2193
2194     case DEBUG_DATA_BREAK_CAUSE_IMAGE_LOAD:
2195     case DEBUG_DATA_BREAK_CAUSE_IMAGE_UNLOAD:
2196       //
2197       // Set AL to DEBUG_AGENT_IMAGE_CONTINUE
2198       //
2199       Al = ArchReadRegisterBuffer (CpuContext, SOFT_DEBUGGER_REGISTER_AX, &Data8);
2200       *Al = DEBUG_AGENT_IMAGE_CONTINUE;
2201
2202       if (!IsHostAttached ()) {
2203         //
2204         // If HOST is not connected for image load/unload, return
2205         //
2206         break;
2207       }
2208       //
2209       // Continue to run the following common code
2210       //
2211
2212     case DEBUG_DATA_BREAK_CAUSE_HW_BREAKPOINT:
2213     case DEBUG_DATA_BREAK_CAUSE_SW_BREAKPOINT:
2214     default:
2215       //
2216       // Send Break packet to HOST
2217       //
2218       AcquireMpSpinLock (&mDebugMpContext.DebugPortSpinLock);
2219       //
2220       // Only the first breaking processor could send BREAK_POINT to HOST
2221       //
2222       if (IsFirstBreakProcessor (ProcessorIndex)) {
2223         SendBreakPacketToHost (BreakCause, ProcessorIndex, &BreakReceived);
2224       }
2225       ReleaseMpSpinLock (&mDebugMpContext.DebugPortSpinLock);
2226
2227       if (Vector == DEBUG_INT3_VECTOR) {
2228         //
2229         // go back address located "0xCC"
2230         //
2231         CpuContext->Eip--;
2232         SavedEip = CpuContext->Eip;
2233         CommandCommunication (Vector, CpuContext, BreakReceived);
2234         if ((SavedEip == CpuContext->Eip) &&
2235             (*(UINT8 *) (UINTN) CpuContext->Eip == DEBUG_SW_BREAKPOINT_SYMBOL)) {
2236           //
2237           // If this is not a software breakpoint set by HOST,
2238           // restore EIP
2239           //
2240           CpuContext->Eip++;
2241         }
2242       } else {
2243         CommandCommunication (Vector, CpuContext, BreakReceived);
2244       }
2245       break;
2246     }
2247
2248     break;
2249
2250   case DEBUG_TIMER_VECTOR:
2251
2252     AcquireMpSpinLock (&mDebugMpContext.DebugPortSpinLock);
2253
2254     if (MultiProcessorDebugSupport()) {
2255       if (IsBsp (ProcessorIndex)) {
2256         //
2257         // If current processor is BSP, check Apic timer's init count if changed,
2258         // it may be re-written when switching BSP.
2259         // If it changed, re-initialize debug timer
2260         //
2261         CurrentDebugTimerInitCount = GetApicTimerInitCount ();
2262         if (mDebugMpContext.DebugTimerInitCount != CurrentDebugTimerInitCount) {
2263           InitializeDebugTimer ();
2264         }
2265       }
2266
2267       if (!IsBsp (ProcessorIndex) || mDebugMpContext.IpiSentByAp) {
2268         ReleaseMpSpinLock (&mDebugMpContext.DebugPortSpinLock);
2269         //
2270         // If current processor is not BSP or this is one IPI sent by AP
2271         //
2272         if (mDebugMpContext.BreakAtCpuIndex != (UINT32) (-1)) {
2273           CommandCommunication (Vector, CpuContext, FALSE);
2274         }
2275
2276         //
2277         // Clear EOI before exiting interrupt process routine.
2278         //
2279         SendApicEoi ();
2280         break;
2281       }
2282     }
2283
2284     //
2285     // Only BSP could run here
2286     //
2287     while (TRUE) {
2288       //
2289       // If there is data in debug port, will check whether it is break(attach/break-in) symbol,
2290       // If yes, go into communication mode with HOST.
2291       // If no, exit interrupt process.
2292       //
2293       if (DebugReadBreakSymbol (Handle, &InputCharacter) == EFI_NOT_FOUND) {
2294         break;
2295       }
2296
2297       if ((!IsHostAttached () && (InputCharacter == DEBUG_STARTING_SYMBOL_ATTACH)) ||
2298           (IsHostAttached () && (InputCharacter == DEBUG_COMMAND_HALT)) ||
2299           (IsHostAttached () && (InputCharacter == DEBUG_COMMAND_GO))
2300          ) {
2301         DebugAgentMsgPrint (DEBUG_AGENT_VERBOSE, "Received data [%02x]\n", InputCharacter);
2302         //
2303         // Ack OK for break-in symbol
2304         //
2305         SendAckPacket (DEBUG_COMMAND_OK);
2306
2307         //
2308         // If receive GO command in Debug Timer, means HOST may lost ACK packet before.
2309         //
2310         if (InputCharacter == DEBUG_COMMAND_GO) {
2311           break;
2312         }
2313
2314         if (!IsHostAttached ()) {
2315           //
2316           // Try to attach HOST, if no ack received after 200ms, return
2317           //
2318           if (AttachHost (BreakCause, READ_PACKET_TIMEOUT, &BreakReceived) != RETURN_SUCCESS) {
2319             break;
2320           }
2321         }
2322
2323         if (MultiProcessorDebugSupport()) {
2324           if(FindNextPendingBreakCpu  () != -1) {
2325             SetCpuBreakFlagByIndex (ProcessorIndex, TRUE);
2326           } else {
2327             HaltOtherProcessors (ProcessorIndex);
2328           }
2329         }
2330         ReleaseMpSpinLock (&mDebugMpContext.DebugPortSpinLock);
2331         CommandCommunication (Vector, CpuContext, BreakReceived);
2332         AcquireMpSpinLock (&mDebugMpContext.DebugPortSpinLock);
2333         break;
2334       }
2335     }
2336
2337     //
2338     // Clear EOI before exiting interrupt process routine.
2339     //
2340     SendApicEoi ();
2341
2342     ReleaseMpSpinLock (&mDebugMpContext.DebugPortSpinLock);
2343
2344     break;
2345
2346   default:
2347     if (Vector <= DEBUG_EXCEPT_SIMD) {
2348       if (BreakCause == DEBUG_DATA_BREAK_CAUSE_STEPPING) {
2349         //
2350         // Stepping is finished, send Ack package.
2351         //
2352         if (MultiProcessorDebugSupport()) {
2353           mDebugMpContext.BreakAtCpuIndex = ProcessorIndex;
2354         }
2355         SendAckPacket (DEBUG_COMMAND_OK);
2356       } else {
2357         //
2358         // Exception occurs, send Break packet to HOST
2359         //
2360         AcquireMpSpinLock (&mDebugMpContext.DebugPortSpinLock);
2361         //
2362         // Only the first breaking processor could send BREAK_POINT to HOST
2363         //
2364         if (IsFirstBreakProcessor (ProcessorIndex)) {
2365           SendBreakPacketToHost (BreakCause, ProcessorIndex, &BreakReceived);
2366         }
2367         ReleaseMpSpinLock (&mDebugMpContext.DebugPortSpinLock);
2368       }
2369
2370       CommandCommunication (Vector, CpuContext, BreakReceived);
2371     }
2372     break;
2373   }
2374
2375   if (MultiProcessorDebugSupport()) {
2376     //
2377     // Clear flag and wait for all processors run here
2378     //
2379     SetIpiSentByApFlag (FALSE);
2380     while (mDebugMpContext.RunCommandSet) {
2381       CpuPause ();
2382     }
2383
2384     //
2385     // Only current (view) processor could clean up AgentInProgress flag.
2386     //
2387     if (mDebugMpContext.ViewPointIndex == ProcessorIndex) {
2388       IssuedViewPoint = mDebugMpContext.ViewPointIndex;
2389     }
2390   }
2391
2392   if (IssuedViewPoint == ProcessorIndex && GetDebugFlag (DEBUG_AGENT_FLAG_STEPPING) != 1) {
2393     //
2394     // If the command is not stepping, clean up AgentInProgress flag
2395     //
2396     SetDebugFlag (DEBUG_AGENT_FLAG_AGENT_IN_PROGRESS, 0);
2397   }
2398
2399   return;
2400 }
2401