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