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