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