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