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