reactos/ntoskrnl/kd/wrappers/gdbstub.c

   1 /****************************************************************************
   2
   3    THIS SOFTWARE IS NOT COPYRIGHTED
   4
   5    HP offers the following for use in the public domain.  HP makes no
   6    warranty with regard to the software or it's performance and the
   7    user accepts the software "AS IS" with all faults.
   8
   9    HP DISCLAIMS ANY WARRANTIES, EXPRESS OR IMPLIED, WITH REGARD
  10    TO THIS SOFTWARE INCLUDING BUT NOT LIMITED TO THE WARRANTIES
  11    OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
  12
  13 ****************************************************************************/
  14 /****************************************************************************
  15  *  Contributor:     Lake Stevens Instrument Division$
  16  *
  17  *  Description:     low level support for gdb debugger. $
  18  *
  19  *  Written by:      Glenn Engel $
  20  *  ModuleState:     Experimental $
  21  *
  22  *  Modified for 386 by Jim Kingdon, Cygnus Support.
  23  *  Modified for ReactOS by Casper S. Hornstrup <chorns@users.sourceforge.net>
  24  *
  25  ****************************************************************************/
  26
  27 #include <ntoskrnl.h>
  28 #define NDEBUG
  29 #include <debug.h>
  30
  31 /************************************************************************/
  32
  33 static BOOLEAN GspInitialized;
  34 static BOOLEAN GspRemoteDebug;
  35
  36 static CONST CHAR HexChars[] = "0123456789abcdef";
  37
  38 static PETHREAD GspRunThread;   /* NULL means run all threads */
  39 static PETHREAD GspDbgThread;
  40 static PETHREAD GspEnumThread;
  41
  42 static FAST_MUTEX GspLock;
  43
  44 extern LIST_ENTRY PsActiveProcessHead;
  45
  46 /* FIXME hardcoded for COM2, 115200 baud */
  47 KD_PORT_INFORMATION GdbPortInfo = { 2, 115200, 0 };
  48
  49 static CHAR GspInBuffer[1000];
  50 static CHAR GspOutBuffer[1000];
  51
  52 /* Number of Registers.  */
  53 #define NUMREGS 16
  54
  55 enum REGISTER_NAMES
  56 {
  57     EAX, ECX, EDX, EBX, ESP, EBP, ESI, EDI,
  58     PC /* also known as eip */ ,
  59     PS /* also known as eflags */ ,
  60     CS, SS, DS, ES, FS, GS
  61 };
  62
  63 typedef struct _CPU_REGISTER
  64 {
  65     ULONG Size;
  66     ULONG OffsetInTF;
  67     ULONG OffsetInContext;
  68     BOOLEAN SetInContext;
  69 } CPU_REGISTER, *PCPU_REGISTER;
  70
  71 static CPU_REGISTER GspRegisters[NUMREGS] =
  72 {
  73     { 4, FIELD_OFFSET(KTRAP_FRAME, Eax), FIELD_OFFSET(CONTEXT, Eax), TRUE },
  74     { 4, FIELD_OFFSET(KTRAP_FRAME, Ecx), FIELD_OFFSET(CONTEXT, Ecx), TRUE },
  75     { 4, FIELD_OFFSET(KTRAP_FRAME, Edx), FIELD_OFFSET(CONTEXT, Edx), FALSE },
  76     { 4, FIELD_OFFSET(KTRAP_FRAME, Ebx), FIELD_OFFSET(CONTEXT, Ebx), TRUE },
  77     { 4, FIELD_OFFSET(KTRAP_FRAME, HardwareEsp), FIELD_OFFSET(CONTEXT, Esp), TRUE },
  78     { 4, FIELD_OFFSET(KTRAP_FRAME, DbgEbp), FIELD_OFFSET(CONTEXT, Ebp), TRUE },
  79     { 4, FIELD_OFFSET(KTRAP_FRAME, Esi), FIELD_OFFSET(CONTEXT, Esi), TRUE },
  80     { 4, FIELD_OFFSET(KTRAP_FRAME, Edi), FIELD_OFFSET(CONTEXT, Edi), TRUE },
  81     { 4, FIELD_OFFSET(KTRAP_FRAME, DbgEip), FIELD_OFFSET(CONTEXT, Eip), TRUE },
  82     { 4, FIELD_OFFSET(KTRAP_FRAME, EFlags), FIELD_OFFSET(CONTEXT, EFlags), TRUE },
  83     { 4, FIELD_OFFSET(KTRAP_FRAME, SegCs), FIELD_OFFSET(CONTEXT, SegCs), TRUE },
  84     { 4, FIELD_OFFSET(KTRAP_FRAME, HardwareSegSs), FIELD_OFFSET(CONTEXT, SegSs), TRUE },
  85     { 4, FIELD_OFFSET(KTRAP_FRAME, SegDs), FIELD_OFFSET(CONTEXT, SegDs), TRUE },
  86     { 4, FIELD_OFFSET(KTRAP_FRAME, SegEs), FIELD_OFFSET(CONTEXT, SegEs), TRUE },
  87     { 4, FIELD_OFFSET(KTRAP_FRAME, SegFs), FIELD_OFFSET(CONTEXT, SegFs), TRUE },
  88     { 4, FIELD_OFFSET(KTRAP_FRAME, SegGs), FIELD_OFFSET(CONTEXT, SegGs), TRUE }
  89 };
  90
  91 static PCHAR GspThreadStates[DeferredReady + 1] =
  92 {
  93     "Initialized",
  94     "Ready",
  95     "Running",
  96     "Standby",
  97     "Terminated",
  98     "Waiting",
  99     "Transition",
 100     "DeferredReady"
 101 };
 102
 103 LONG
 104 HexValue(CHAR ch)
 105 {
 106     if ((ch >= '0') && (ch <= '9'))
 107         return (ch - '0');
 108
 109     if ((ch >= 'a') && (ch <= 'f'))
 110         return (ch - 'a' + 10);
 111
 112     if ((ch >= 'A') && (ch <= 'F'))
 113         return (ch - 'A' + 10);
 114
 115     return -1;
 116 }
 117
 118 VOID
 119 GdbPutChar(UCHAR Value)
 120 {
 121     KdPortPutByteEx(&GdbPortInfo, Value);
 122 }
 123
 124 UCHAR
 125 GdbGetChar(VOID)
 126 {
 127     UCHAR Value;
 128
 129     while (!KdPortGetByteEx(&GdbPortInfo, &Value))
 130         ;
 131
 132     return Value;
 133 }
 134
 135 /* scan for the sequence $<data>#<Checksum> */
 136 PCHAR
 137 GspGetPacket()
 138 {
 139     PCHAR Buffer = &GspInBuffer[0];
 140     CHAR Checksum;
 141     CHAR XmitChecksum;
 142     ULONG Count;
 143     CHAR ch;
 144
 145     while (TRUE)
 146     {
 147         /* wait around for the start character, ignore all other characters */
 148         while ((ch = GdbGetChar()) != '$')
 149             ;
 150
 151 retry:
 152         Checksum = 0;
 153         XmitChecksum = -1;
 154         Count = 0;
 155
 156         /* now, read until a # or end of Buffer is found */
 157         while (Count < sizeof(GspInBuffer) - 1)
 158         {
 159             ch = GdbGetChar();
 160             if (ch == '$')
 161                 goto retry;
 162
 163             if (ch == '#')
 164                 break;
 165
 166             Checksum = Checksum + ch;
 167             Buffer[Count] = ch;
 168             Count = Count + 1;
 169         }
 170         Buffer[Count] = 0;
 171
 172         if (ch == '#')
 173         {
 174             ch = GdbGetChar();
 175             XmitChecksum = (CHAR)(HexValue(ch) << 4);
 176             ch = GdbGetChar();
 177             XmitChecksum += (CHAR)(HexValue(ch));
 178
 179             if (Checksum != XmitChecksum)
 180             {
 181                 GdbPutChar('-'); /* failed checksum */
 182             }
 183             else
 184             {
 185                 GdbPutChar('+'); /* successful transfer */
 186                 return &Buffer[0];
 187             }
 188         }
 189     }
 190 }
 191
 192 /* send the packet in Buffer. */
 193 VOID
 194 GspPutPacket(PCHAR Buffer)
 195 {
 196     CHAR Checksum;
 197     LONG Count;
 198     CHAR ch;
 199
 200     /* $<packet info>#<Checksum>. */
 201     do
 202     {
 203         GdbPutChar('$');
 204         Checksum = 0;
 205         Count = 0;
 206
 207         while ((ch = Buffer[Count]))
 208         {
 209             GdbPutChar(ch);
 210             Checksum += ch;
 211             Count += 1;
 212         }
 213
 214         GdbPutChar('#');
 215         GdbPutChar(HexChars[(Checksum >> 4) & 0xf]);
 216         GdbPutChar(HexChars[Checksum & 0xf]);
 217     }
 218     while (GdbGetChar() != '+');
 219 }
 220
 221 VOID
 222 GspPutPacketNoWait(PCHAR Buffer)
 223 {
 224     CHAR Checksum;
 225     LONG Count;
 226     CHAR ch;
 227
 228     /* $<packet info>#<Checksum>. */
 229     GdbPutChar('$');
 230     Checksum = 0;
 231     Count = 0;
 232
 233     while ((ch = Buffer[Count]))
 234     {
 235         GdbPutChar(ch);
 236         Checksum += ch;
 237         Count += 1;
 238     }
 239
 240     GdbPutChar('#');
 241     GdbPutChar(HexChars[(Checksum >> 4) & 0xf]);
 242     GdbPutChar(HexChars[Checksum & 0xf]);
 243 }
 244
 245 /* Indicate to caller of GspMem2Hex or GspHex2Mem that there has been an error. */
 246 static volatile BOOLEAN GspMemoryError = FALSE;
 247
 248 static CHAR
 249 GspReadMemSafe(PCHAR Address)
 250 {
 251     CHAR ch = 0;
 252
 253     if (!KdpSafeReadMemory((ULONG_PTR)Address, 1, &ch))
 254         GspMemoryError = TRUE;
 255
 256     return ch;
 257 }
 258
 259 static void
 260 GspWriteMemSafe(PCHAR Address, CHAR Ch)
 261 {
 262     if (!KdpSafeWriteMemory((ULONG_PTR)Address, 1, Ch))
 263         GspMemoryError = TRUE;
 264 }
 265
 266 /* Convert the memory pointed to by Address into hex, placing result in Buffer
 267  * Return a pointer to the last char put in Buffer (null)
 268  * If MayFault is TRUE, then we should set GspMemoryError in response to
 269  * a fault; if FALSE treat a fault like any other fault in the stub.
 270  */
 271 static PCHAR
 272 GspMem2Hex(PCHAR Address, PCHAR Buffer, LONG Count, BOOLEAN MayFault)
 273 {
 274     ULONG i;
 275     CHAR ch;
 276
 277     for (i = 0; i < (ULONG)Count; i++)
 278     {
 279         if (MayFault)
 280         {
 281             ch = GspReadMemSafe(Address);
 282             if (GspMemoryError)
 283                 return Buffer;
 284         }
 285         else
 286         {
 287             ch = *Address;
 288         }
 289         *Buffer++ = HexChars[(ch >> 4) & 0xf];
 290         *Buffer++ = HexChars[ch & 0xf];
 291         Address++;
 292     }
 293
 294     *Buffer = 0;
 295     return Buffer;
 296 }
 297
 298 static ULONG
 299 GspWriteMem(PCHAR Address, ULONG Count, BOOLEAN MayFault,
 300             CHAR (*GetContent)(PVOID Context, ULONG Offset), PVOID Context)
 301 {
 302     PCHAR Current;
 303     PCHAR Page;
 304     ULONG CountInPage;
 305     ULONG i;
 306     CHAR ch;
 307
 308     Current = Address;
 309     while (Current < Address + Count)
 310     {
 311         Page = (PCHAR)PAGE_ROUND_DOWN(Current);
 312         if (Address + Count <= Page + PAGE_SIZE)
 313         {
 314             /* Fits in this page */
 315             CountInPage = Count;
 316         }
 317         else
 318         {
 319             /* Flows into next page, handle only current page in this iteration */
 320             CountInPage = PAGE_SIZE - (Address - Page);
 321         }
 322
 323         for (i = 0; i < CountInPage && !GspMemoryError; i++)
 324         {
 325             ch = (*GetContent)(Context, Current - Address);
 326
 327             if (MayFault)
 328                 GspWriteMemSafe(Current, ch);
 329             else
 330                 *Current = ch;
 331
 332             Current++;
 333         }
 334         if (MayFault)
 335         {
 336             if (GspMemoryError)
 337                 return Current - Address;
 338         }
 339     }
 340
 341     return Current - Address;
 342 }
 343
 344 static CHAR
 345 GspHex2MemGetContent(PVOID Context, ULONG Offset)
 346 {
 347     return (CHAR)((HexValue(*((PCHAR)Context + 2 * Offset)) << 4) +
 348                    HexValue(*((PCHAR)Context + 2 * Offset + 1)));
 349 }
 350
 351 /* Convert the hex array pointed to by Buffer into binary to be placed at Address
 352  * Return a pointer to the character AFTER the last byte read from Buffer */
 353 static PCHAR
 354 GspHex2Mem(PCHAR Buffer, PCHAR Address, ULONG Count, BOOLEAN MayFault)
 355 {
 356     Count = GspWriteMem(Address, Count, MayFault, GspHex2MemGetContent, Buffer);
 357     return Buffer + 2 * Count;
 358 }
 359
 360 /**********************************************/
 361 /* WHILE WE FIND NICE HEX CHARS, BUILD A LONG */
 362 /* RETURN NUMBER OF CHARS PROCESSED           */
 363 /**********************************************/
 364 LONG
 365 GspHex2Long(PCHAR *Address, PLONG Value)
 366 {
 367     LONG NumChars = 0;
 368     LONG Hex;
 369
 370     *Value = 0;
 371
 372     while (**Address)
 373     {
 374         Hex = HexValue(**Address);
 375         if (Hex >= 0)
 376         {
 377             *Value = (*Value << 4) | Hex;
 378             NumChars++;
 379         }
 380         else
 381         {
 382             break;
 383         }
 384
 385         (*Address)++;
 386     }
 387
 388     return NumChars;
 389 }
 390
 391 VOID
 392 GspLong2Hex(PCHAR *Address, LONG Value)
 393 {
 394     LONG Save;
 395
 396     Save = (((Value >> 0) & 0xff) << 24) | (((Value >> 8) & 0xff) << 16) |
 397            (((Value >> 16) & 0xff) << 8) | (((Value >> 24) & 0xff) << 0);
 398
 399     *Address = GspMem2Hex((PCHAR)&Save, *Address, 4, FALSE);
 400 }
 401
 402 /*
 403  * When coming from kernel mode, Esp is not stored in the trap frame.
 404  * Instead, it was pointing to the location of the TrapFrame Esp member
 405  * when the exception occured. When coming from user mode, Esp is just
 406  * stored in the TrapFrame Esp member.
 407  */
 408 static LONG
 409 GspGetEspFromTrapFrame(PKTRAP_FRAME TrapFrame)
 410 {
 411     return KeGetPreviousMode() == KernelMode ?
 412            (LONG)&TrapFrame->HardwareEsp : (LONG)TrapFrame->HardwareEsp;
 413 }
 414
 415 static VOID
 416 GspGetRegisters(PCHAR Address, PKTRAP_FRAME TrapFrame)
 417 {
 418     ULONG_PTR Value;
 419     PULONG p;
 420     ULONG i;
 421     PETHREAD Thread;
 422     ULONG_PTR *KernelStack;
 423
 424     if (NULL == GspDbgThread)
 425     {
 426         Thread = PsGetCurrentThread();
 427     }
 428     else
 429     {
 430         TrapFrame = GspDbgThread->Tcb.TrapFrame;
 431         Thread = GspDbgThread;
 432     }
 433
 434     if (Waiting == Thread->Tcb.State)
 435     {
 436         KernelStack = Thread->Tcb.KernelStack;
 437         for (i = 0; i < sizeof(GspRegisters) / sizeof(GspRegisters[0]); i++)
 438         {
 439             switch (i)
 440             {
 441                 case EBP:
 442                     Value = KernelStack[3];
 443                     break;
 444                 case EDI:
 445                     Value = KernelStack[4];
 446                     break;
 447                 case ESI:
 448                     Value = KernelStack[5];
 449                     break;
 450                 case EBX:
 451                     Value = KernelStack[6];
 452                     break;
 453                 case PC:
 454                     Value = KernelStack[7];
 455                     break;
 456                 case ESP:
 457                     Value = (ULONG_PTR)(KernelStack + 8);
 458                     break;
 459                 case CS:
 460                     Value = KGDT_R0_CODE;
 461                     break;
 462                 case DS:
 463                     Value = KGDT_R0_DATA;
 464                     break;
 465                 default:
 466                     Value = 0;
 467                     break;
 468             }
 469
 470             Address = GspMem2Hex((PCHAR)&Value, Address, GspRegisters[i].Size, FALSE);
 471         }
 472     }
 473     else
 474     {
 475         for (i = 0; i < sizeof(GspRegisters) / sizeof(GspRegisters[0]); i++)
 476         {
 477             if (TrapFrame)
 478             {
 479                 if (ESP == i)
 480                 {
 481                     Value = GspGetEspFromTrapFrame(TrapFrame);
 482                 }
 483                 else
 484                 {
 485                     p = (PULONG)((ULONG_PTR)TrapFrame + GspRegisters[i].OffsetInTF);
 486                     Value = *p;
 487                 }
 488             }
 489             else if (i == PC)
 490             {
 491                 /*
 492                  * This thread has not been sheduled yet so assume it
 493                  * is still in PsBeginThreadWithContextInternal().
 494                  */
 495                 Value = (ULONG)KiThreadStartup;
 496             }
 497             else
 498             {
 499                 Value = 0;
 500             }
 501
 502             Address = GspMem2Hex((PCHAR)&Value, Address, GspRegisters[i].Size, FALSE);
 503         }
 504     }
 505 }
 506
 507 VOID
 508 GspSetRegistersInTrapFrame(PCHAR Address, PCONTEXT Context, PKTRAP_FRAME TrapFrame)
 509 {
 510     ULONG Value;
 511     PCHAR Buffer;
 512     PULONG p;
 513     ULONG i;
 514
 515     if (!TrapFrame)
 516         return;
 517
 518     Buffer = Address;
 519     for (i = 0; i < NUMREGS; i++)
 520     {
 521         if (GspRegisters[i].SetInContext)
 522         {
 523             p = (PULONG)((ULONG_PTR)Context + GspRegisters[i].OffsetInContext);
 524         }
 525         else
 526         {
 527             p = (PULONG)((ULONG_PTR)TrapFrame + GspRegisters[i].OffsetInTF);
 528         }
 529
 530         Value = 0;
 531         Buffer = GspHex2Mem(Buffer, (PCHAR)&Value, GspRegisters[i].Size, FALSE);
 532         *p = Value;
 533     }
 534 }
 535
 536 VOID
 537 GspSetSingleRegisterInTrapFrame(PCHAR Address, LONG Number, PCONTEXT Context, PKTRAP_FRAME TrapFrame)
 538 {
 539     ULONG Value;
 540     PULONG p;
 541
 542     if (!TrapFrame)
 543         return;
 544
 545     if (GspRegisters[Number].SetInContext)
 546     {
 547         p = (PULONG)((ULONG_PTR)Context + GspRegisters[Number].OffsetInContext);
 548     }
 549     else
 550     {
 551         p = (PULONG)((ULONG_PTR)TrapFrame + GspRegisters[Number].OffsetInTF);
 552     }
 553
 554     Value = 0;
 555     GspHex2Mem(Address, (PCHAR)&Value, GspRegisters[Number].Size, FALSE);
 556     *p = Value;
 557 }
 558
 559 BOOLEAN
 560 GspFindThread(PCHAR Data, PETHREAD *Thread)
 561 {
 562     PETHREAD ThreadInfo = NULL;
 563
 564     if (strcmp(Data, "-1") == 0)
 565     {
 566         /* All threads */
 567         ThreadInfo = NULL;
 568     }
 569     else
 570     {
 571         ULONG uThreadId;
 572         HANDLE ThreadId;
 573         PCHAR ptr = &Data[0];
 574
 575         GspHex2Long(&ptr, (PLONG)&uThreadId);
 576         ThreadId = (HANDLE)uThreadId;
 577
 578         if (!NT_SUCCESS(PsLookupThreadByThreadId(ThreadId, &ThreadInfo)))
 579         {
 580             *Thread = NULL;
 581             return FALSE;
 582         }
 583     }
 584
 585     *Thread = ThreadInfo;
 586     return TRUE;
 587 }
 588
 589 VOID
 590 GspSetThread(PCHAR Request)
 591 {
 592     PETHREAD ThreadInfo;
 593     PCHAR ptr = &Request[1];
 594
 595     switch (Request[0])
 596     {
 597         case 'c': /* Run thread */
 598             if (GspFindThread(ptr, &ThreadInfo))
 599             {
 600                 GspOutBuffer[0] = 'O';
 601                 GspOutBuffer[1] = 'K';
 602
 603                 if (NULL != GspRunThread)
 604                     ObDereferenceObject(GspRunThread);
 605
 606                 GspRunThread = ThreadInfo;
 607
 608                 if (NULL != GspRunThread)
 609                     ObReferenceObject(GspRunThread);
 610             }
 611             else
 612             {
 613                 GspOutBuffer[0] = 'E';
 614             }
 615             break;
 616
 617         case 'g': /* Debug thread */
 618             if (GspFindThread(ptr, &ThreadInfo))
 619             {
 620                 GspOutBuffer[0] = 'O';
 621                 GspOutBuffer[1] = 'K';
 622
 623                 if (NULL != GspDbgThread)
 624                     ObDereferenceObject(GspDbgThread);
 625
 626                 if (ThreadInfo == PsGetCurrentThread())
 627                 {
 628                     GspDbgThread = NULL;
 629                     ObDereferenceObject(ThreadInfo);
 630                 }
 631                 else
 632                 {
 633                     GspDbgThread = ThreadInfo;
 634                 }
 635             }
 636             else
 637             {
 638                 GspOutBuffer[0] = 'E';
 639             }
 640             break;
 641
 642         default:
 643             break;
 644     }
 645 }
 646
 647 VOID
 648 GspQuery(PCHAR Request)
 649 {
 650     ULONG Value;
 651
 652     if (strncmp(Request, "C", 1) == 0)
 653     {
 654         PCHAR ptr = &GspOutBuffer[2];
 655
 656         /* Get current thread id */
 657         GspOutBuffer[0] = 'Q';
 658         GspOutBuffer[1] = 'C';
 659
 660         if (NULL != GspDbgThread)
 661             Value = (ULONG)GspDbgThread->Cid.UniqueThread;
 662         else
 663             Value = (ULONG)PsGetCurrentThread()->Cid.UniqueThread;
 664
 665         GspLong2Hex(&ptr, Value);
 666     }
 667     else if (strncmp(Request, "fThreadInfo", 11) == 0)
 668     {
 669         PEPROCESS Process;
 670         PLIST_ENTRY AThread, AProcess;
 671         PCHAR ptr = &GspOutBuffer[1];
 672
 673         /* Get first thread id */
 674         GspEnumThread = NULL;
 675         AProcess = PsActiveProcessHead.Flink;
 676         while (AProcess != &PsActiveProcessHead)
 677         {
 678             Process = CONTAINING_RECORD(AProcess, EPROCESS, ActiveProcessLinks);
 679             AThread = Process->ThreadListHead.Flink;
 680             if (AThread != &Process->ThreadListHead)
 681             {
 682                 GspEnumThread = CONTAINING_RECORD(Process->ThreadListHead.Flink,
 683                                                   ETHREAD, ThreadListEntry);
 684                 break;
 685             }
 686             AProcess = AProcess->Flink;
 687         }
 688         if (GspEnumThread != NULL)
 689         {
 690             GspOutBuffer[0] = 'm';
 691             Value = (ULONG)GspEnumThread->Cid.UniqueThread;
 692             GspLong2Hex(&ptr, Value);
 693         }
 694         else
 695         {
 696             /* FIXME - what to do here? This case should never happen though, there
 697                should always be at least one thread on the system... */
 698             /* GspOutBuffer[0] = 'l'; */
 699         }
 700     }
 701     else if (strncmp(Request, "sThreadInfo", 11) == 0)
 702     {
 703         PEPROCESS Process;
 704         PLIST_ENTRY AThread, AProcess;
 705         PCHAR ptr = &GspOutBuffer[1];
 706
 707         /* Get next thread id */
 708         if (GspEnumThread != NULL)
 709         {
 710             /* find the next thread */
 711             Process = GspEnumThread->ThreadsProcess;
 712             if (GspEnumThread->ThreadListEntry.Flink != &Process->ThreadListHead)
 713             {
 714                 GspEnumThread = CONTAINING_RECORD(GspEnumThread->ThreadListEntry.Flink,
 715                                                   ETHREAD, ThreadListEntry);
 716             }
 717             else
 718             {
 719                 PETHREAD Thread = NULL;
 720                 AProcess = Process->ActiveProcessLinks.Flink;
 721                 while (AProcess != &PsActiveProcessHead)
 722                 {
 723                     Process = CONTAINING_RECORD(AProcess, EPROCESS, ActiveProcessLinks);
 724                     AThread = Process->ThreadListHead.Flink;
 725                     if (AThread != &Process->ThreadListHead)
 726                     {
 727                         Thread = CONTAINING_RECORD(Process->ThreadListHead.Flink,
 728                                                    ETHREAD, ThreadListEntry);
 729                         break;
 730                     }
 731                     AProcess = AProcess->Flink;
 732                 }
 733                 GspEnumThread = Thread;
 734             }
 735
 736             if (GspEnumThread != NULL)
 737             {
 738                 /* return the ID */
 739                 GspOutBuffer[0] = 'm';
 740                 Value = (ULONG)GspEnumThread->Cid.UniqueThread;
 741                 GspLong2Hex(&ptr, Value);
 742             }
 743             else
 744             {
 745                 GspOutBuffer[0] = 'l';
 746             }
 747         }
 748         else
 749         {
 750             GspOutBuffer[0] = 'l';
 751         }
 752     }
 753     else if (strncmp(Request, "ThreadExtraInfo", 15) == 0)
 754     {
 755         PETHREAD ThreadInfo;
 756
 757         /* Get thread information */
 758         if (GspFindThread(Request + 16, &ThreadInfo))
 759         {
 760             char Buffer[64];
 761             PEPROCESS Proc;
 762
 763             Proc = (PEPROCESS)ThreadInfo->ThreadsProcess;
 764
 765             Buffer[0] = '\0';
 766
 767             if (NULL != Proc)
 768                 sprintf(Buffer, "%s [%d:0x%x], ",
 769                         Proc->ImageFileName,
 770                         (int)Proc->UniqueProcessId,
 771                         (int)ThreadInfo->Cid.UniqueThread);
 772
 773             strcpy(Buffer + strlen(Buffer), GspThreadStates[ThreadInfo->Tcb.State]);
 774
 775             ObDereferenceObject(ThreadInfo);
 776
 777             GspMem2Hex(Buffer, &GspOutBuffer[0], strlen(Buffer), FALSE);
 778         }
 779     }
 780     else if (strncmp(Request, "Supported", 9) == 0)
 781     {
 782         /* tell maximum incoming packet size */
 783         sprintf(GspOutBuffer, "PacketSize=%u", sizeof(GspInBuffer) - 1);
 784     }
 785     else if (strncmp(Request, "Rcmd,", 5) == 0)
 786     {
 787         ;
 788     }
 789 }
 790
 791 VOID
 792 GspQueryThreadStatus(PCHAR Request)
 793 {
 794     PETHREAD ThreadInfo;
 795     PCHAR ptr = &Request[0];
 796
 797     if (GspFindThread(ptr, &ThreadInfo))
 798     {
 799         ObDereferenceObject(ThreadInfo);
 800
 801         GspOutBuffer[0] = 'O';
 802         GspOutBuffer[1] = 'K';
 803         GspOutBuffer[2] = '\0';
 804     }
 805     else
 806     {
 807         GspOutBuffer[0] = 'E';
 808         GspOutBuffer[1] = '\0';
 809     }
 810 }
 811
 812 #define DR6_BS         0x00004000       /* Single step */
 813
 814 #define DR7_L0         0x00000001       /* Local breakpoint 0 enable */
 815 #define DR7_G0         0x00000002       /* Global breakpoint 0 enable */
 816 #define DR7_L1         0x00000004       /* Local breakpoint 1 enable */
 817 #define DR7_G1         0x00000008       /* Global breakpoint 1 enable */
 818 #define DR7_L2         0x00000010       /* Local breakpoint 2 enable */
 819 #define DR7_G2         0x00000020       /* Global breakpoint 2 enable */
 820 #define DR7_L3         0x00000040       /* Local breakpoint 3 enable */
 821 #define DR7_G3         0x00000080       /* Global breakpoint 3 enable */
 822 #define DR7_LE         0x00000100       /* Local exact breakpoint enable (old) */
 823 #define DR7_GE         0x00000200       /* Global exact breakpoint enable (old) */
 824 #define DR7_GD         0x00002000       /* General detect enable */
 825 #define DR7_TYPE0_MASK 0x00030000       /* Breakpoint 0 condition */
 826 #define DR7_LEN0_MASK  0x000c0000       /* Breakpoint 0 length */
 827 #define DR7_TYPE1_MASK 0x00300000       /* Breakpoint 1 condition */
 828 #define DR7_LEN1_MASK  0x00c00000       /* Breakpoint 1 length */
 829 #define DR7_TYPE2_MASK 0x03000000       /* Breakpoint 2 condition */
 830 #define DR7_LEN2_MASK  0x0c000000       /* Breakpoint 2 length */
 831 #define DR7_TYPE3_MASK 0x30000000       /* Breakpoint 3 condition */
 832 #define DR7_LEN3_MASK  0xc0000000       /* Breakpoint 3 length */
 833 #define DR7_GLOBAL_ENABLE(Bp) (2 << (2 * (Bp)))
 834 #define DR7_TYPE(Bp, Type)    ((Type) << (16 + 4 * (Bp)))
 835 #define DR7_LEN(Bp, Len)      ((Len) << (18 + 4 * (Bp)))
 836
 837 #define I386_BP_TYPE_EXECUTE        0
 838 #define I386_BP_TYPE_DATA_WRITE     1
 839 #define I386_BP_TYPE_DATA_READWRITE 3
 840
 841 #define I386_OPCODE_INT3 0xcc
 842
 843 #define GDB_ZTYPE_MEMORY_BREAKPOINT   0
 844 #define GDB_ZTYPE_HARDWARE_BREAKPOINT 1
 845 #define GDB_ZTYPE_WRITE_WATCHPOINT    2
 846 #define GDB_ZTYPE_READ_WATCHPOINT     3
 847 #define GDB_ZTYPE_ACCESS_WATCHPOINT   4
 848
 849 typedef struct _GSPHWBREAKPOINT
 850 {
 851     ULONG Type;
 852     ULONG_PTR Address;
 853     ULONG Length;
 854 } GSPHWBREAKPOINT;
 855
 856 #define MAX_HW_BREAKPOINTS 4
 857 static unsigned GspHwBreakpointCount = 0;
 858 static GSPHWBREAKPOINT GspHwBreakpoints[MAX_HW_BREAKPOINTS];
 859
 860 typedef struct _GSPSWBREAKPOINT
 861 {
 862     ULONG_PTR Address;
 863     CHAR PrevContent;
 864     BOOLEAN Active;
 865 } GSPSWBREAKPOINT;
 866
 867 #define MAX_SW_BREAKPOINTS 64
 868 static unsigned GspSwBreakpointCount = 0;
 869 static GSPSWBREAKPOINT GspSwBreakpoints[MAX_SW_BREAKPOINTS];
 870
 871 static void
 872 GspSetHwBreakpoint(ULONG Type, ULONG_PTR Address, ULONG Length)
 873 {
 874     DPRINT("GspSetHwBreakpoint(%lu, 0x%p, %lu)\n", Type, Address, Length);
 875
 876     if (GDB_ZTYPE_READ_WATCHPOINT == Type)
 877     {
 878         DPRINT1("Read watchpoint not supported\n");
 879         strcpy(GspOutBuffer, "E22");
 880     }
 881     else if (GDB_ZTYPE_HARDWARE_BREAKPOINT == Type && 1 != Length)
 882     {
 883         DPRINT1("Invalid length %lu for hardware breakpoint\n", Length);
 884         strcpy(GspOutBuffer, "E22");
 885     }
 886     else if (1 != Length && 2 != Length && 4 != Length)
 887     {
 888         DPRINT1("Invalid length %lu for GDB Z type %lu\n", Length, Type);
 889         strcpy(GspOutBuffer, "E22");
 890     }
 891     else if (0 != (Address & (Length - 1)))
 892     {
 893         DPRINT1("Invalid alignment for address 0x%p and length %d\n", Address, Length);
 894         strcpy(GspOutBuffer, "E22");
 895     }
 896     else if (MAX_HW_BREAKPOINTS == GspHwBreakpointCount)
 897     {
 898         DPRINT1("Trying to set too many hardware breakpoints\n");
 899         strcpy(GspOutBuffer, "E22");
 900     }
 901     else
 902     {
 903         DPRINT("Stored at index %u\n", GspHwBreakpointCount);
 904         GspHwBreakpoints[GspHwBreakpointCount].Type = Type;
 905         GspHwBreakpoints[GspHwBreakpointCount].Address = Address;
 906         GspHwBreakpoints[GspHwBreakpointCount].Length = Length;
 907         GspHwBreakpointCount++;
 908         strcpy(GspOutBuffer, "OK");
 909     }
 910 }
 911
 912 static void
 913 GspRemoveHwBreakpoint(ULONG Type, ULONG_PTR Address, ULONG Length)
 914 {
 915     unsigned Index;
 916
 917     DPRINT("GspRemoveHwBreakpoint(%lu, 0x%p, %lu)\n", Type, Address, Length);
 918     for (Index = 0; Index < GspHwBreakpointCount; Index++)
 919     {
 920         if (GspHwBreakpoints[Index].Type == Type &&
 921             GspHwBreakpoints[Index].Address == Address &&
 922             GspHwBreakpoints[Index].Length == Length)
 923         {
 924             DPRINT("Found match at index %u\n", Index);
 925             if (Index + 1 < GspHwBreakpointCount)
 926                 memmove(GspHwBreakpoints + Index,
 927                         GspHwBreakpoints + (Index + 1),
 928                         (GspHwBreakpointCount - Index - 1) * sizeof(GSPHWBREAKPOINT));
 929
 930             GspHwBreakpointCount--;
 931             strcpy(GspOutBuffer, "OK");
 932             return;
 933         }
 934     }
 935
 936     DPRINT1("Not found\n");
 937     strcpy(GspOutBuffer, "E22");
 938 }
 939
 940 static BOOLEAN
 941 GspFindSwBreakpoint(ULONG_PTR Address, PULONG PIndex)
 942 {
 943     ULONG Index;
 944
 945     for (Index = 0; Index < GspSwBreakpointCount; Index++)
 946         if (GspSwBreakpoints[Index].Address == Address)
 947         {
 948             if (PIndex)
 949                 *PIndex = Index;
 950             return TRUE;
 951         }
 952
 953     return FALSE;
 954 }
 955
 956 static void
 957 GspSetSwBreakpoint(ULONG_PTR Address)
 958 {
 959     DPRINT("GspSetSwBreakpoint(0x%p)\n", Address);
 960
 961     if (MAX_SW_BREAKPOINTS == GspSwBreakpointCount)
 962     {
 963         DPRINT1("Trying to set too many software breakpoints\n");
 964         strcpy(GspOutBuffer, "E22");
 965         return;
 966     }
 967
 968     if (GspFindSwBreakpoint(Address, NULL))
 969     {
 970         strcpy(GspOutBuffer, "E22");
 971         return;
 972     }
 973
 974     DPRINT("Stored at index %u\n", GspSwBreakpointCount);
 975     GspSwBreakpoints[GspSwBreakpointCount].Address = Address;
 976     GspSwBreakpoints[GspSwBreakpointCount].Active = FALSE;
 977     GspSwBreakpointCount++;
 978     strcpy(GspOutBuffer, "OK");
 979 }
 980
 981 static void
 982 GspRemoveSwBreakpoint(ULONG_PTR Address)
 983 {
 984     ULONG Index;
 985
 986     DPRINT("GspRemoveSwBreakpoint(0x%p)\n", Address);
 987
 988     if (GspFindSwBreakpoint(Address, &Index))
 989     {
 990         DPRINT("Found match at index %u\n", Index);
 991         ASSERT(!GspSwBreakpoints[Index].Active);
 992
 993         if (Index + 1 < GspSwBreakpointCount)
 994             memmove(GspSwBreakpoints + Index,
 995                     GspSwBreakpoints + (Index + 1),
 996                     (GspSwBreakpointCount - Index - 1) * sizeof(GSPSWBREAKPOINT));
 997
 998         GspSwBreakpointCount--;
 999         strcpy(GspOutBuffer, "OK");
1000         return;
1001     }
1002
1003     DPRINT1("Not found\n");
1004     strcpy(GspOutBuffer, "E22");
1005 }
1006
1007 static void
1008 GspLoadHwBreakpoint(PKTRAP_FRAME TrapFrame, unsigned BpIndex,
1009                     ULONG_PTR Address, ULONG Length, ULONG Type)
1010 {
1011     DPRINT("GspLoadHwBreakpoint(0x%p, %d, 0x%p, %d)\n",
1012            TrapFrame, BpIndex, Address, Type);
1013
1014     /* Set the DR7_Gx bit to globally enable the breakpoint */
1015     TrapFrame->Dr7 |= DR7_GLOBAL_ENABLE(BpIndex) |
1016                       DR7_LEN(BpIndex, Length) |
1017                       DR7_TYPE(BpIndex, Type);
1018
1019     switch (BpIndex)
1020     {
1021         case 0:
1022             DPRINT("Setting DR0 to 0x%p\n", Address);
1023             TrapFrame->Dr0 = Address;
1024             break;
1025
1026         case 1:
1027             DPRINT("Setting DR1 to 0x%p\n", Address);
1028             TrapFrame->Dr1 = Address;
1029             break;
1030
1031         case 2:
1032             DPRINT("Setting DR2 to 0x%p\n", Address);
1033             TrapFrame->Dr2 = Address;
1034             break;
1035
1036         case 3:
1037             DPRINT("Setting DR3 to 0x%p\n", Address);
1038             TrapFrame->Dr3 = Address;
1039             break;
1040     }
1041 }
1042
1043 static void
1044 GspLoadSwBreakpoint(ULONG Index)
1045 {
1046     GspMemoryError = FALSE;
1047
1048     GspSwBreakpoints[Index].PrevContent = GspReadMemSafe((PCHAR)GspSwBreakpoints[Index].Address);
1049
1050     if (!GspMemoryError)
1051         GspWriteMemSafe((PCHAR)GspSwBreakpoints[Index].Address, I386_OPCODE_INT3);
1052
1053     GspSwBreakpoints[Index].Active = !GspMemoryError;
1054
1055     if (GspMemoryError)
1056         DPRINT1("Failed to set software breakpoint at 0x%p\n", GspSwBreakpoints[Index].Address);
1057     else
1058         DPRINT("Successfully set software breakpoint at 0x%p\n", GspSwBreakpoints[Index].Address);
1059 }
1060
1061 static void
1062 GspLoadBreakpoints(PKTRAP_FRAME TrapFrame)
1063 {
1064     unsigned Index;
1065     ULONG i386Type;
1066
1067     DPRINT("GspLoadBreakpoints\n");
1068     DPRINT("DR7 on entry: 0x%08x\n", TrapFrame->Dr7);
1069
1070     /* Remove all breakpoints */
1071     TrapFrame->Dr7 &= ~(DR7_L0 | DR7_L1 | DR7_L2 | DR7_L3 |
1072                         DR7_G0 | DR7_G1 | DR7_G2 | DR7_G3 |
1073                         DR7_TYPE0_MASK | DR7_LEN0_MASK |
1074                         DR7_TYPE1_MASK | DR7_LEN1_MASK |
1075                         DR7_TYPE2_MASK | DR7_LEN2_MASK |
1076                         DR7_TYPE3_MASK | DR7_LEN3_MASK);
1077
1078     for (Index = 0; Index < GspHwBreakpointCount; Index++)
1079     {
1080         switch (GspHwBreakpoints[Index].Type)
1081         {
1082             case GDB_ZTYPE_HARDWARE_BREAKPOINT:
1083                 i386Type = I386_BP_TYPE_EXECUTE;
1084                 break;
1085             case GDB_ZTYPE_WRITE_WATCHPOINT:
1086                 i386Type = I386_BP_TYPE_DATA_WRITE;
1087                 break;
1088             case GDB_ZTYPE_ACCESS_WATCHPOINT:
1089                 i386Type = I386_BP_TYPE_DATA_READWRITE;
1090                 break;
1091             default:
1092                 ASSERT(FALSE);
1093                 i386Type = I386_BP_TYPE_EXECUTE;
1094                 break;
1095         }
1096
1097         GspLoadHwBreakpoint(TrapFrame, Index, GspHwBreakpoints[Index].Address,
1098                             GspHwBreakpoints[Index].Length - 1, i386Type);
1099     }
1100
1101     for (Index = 0; Index < GspSwBreakpointCount; Index++)
1102     {
1103         DPRINT("Using real software breakpoint\n");
1104         GspLoadSwBreakpoint(Index);
1105     }
1106
1107     DPRINT("Final DR7 value 0x%08x\n", TrapFrame->Dr7);
1108 }
1109
1110 static void
1111 GspUnloadBreakpoints(void)
1112 {
1113     unsigned Index;
1114
1115     DPRINT("GspUnloadBreakpoints\n");
1116
1117     /* Disable hardware debugging while we are inside the stub */
1118     __writedr(7, 0);
1119
1120     for (Index = 0; Index < GspSwBreakpointCount; Index++)
1121     {
1122         if (GspSwBreakpoints[Index].Active)
1123         {
1124             GspMemoryError = FALSE;
1125             GspWriteMemSafe((PCHAR)GspSwBreakpoints[Index].Address,
1126                             GspSwBreakpoints[Index].PrevContent);
1127             GspSwBreakpoints[Index].Active = FALSE;
1128             if (GspMemoryError)
1129             {
1130                 DPRINT1("Failed to remove software breakpoint from 0x%p\n",
1131                         GspSwBreakpoints[Index].Address);
1132             }
1133             else
1134             {
1135                 DPRINT("Successfully removed software breakpoint from 0x%p\n",
1136                        GspSwBreakpoints[Index].Address);
1137             }
1138         }
1139     }
1140 }
1141
1142 static void
1143 GspStopReply(NTSTATUS ExceptionCode, PKTRAP_FRAME TrapFrame)
1144 {
1145     PCHAR ptr = GspOutBuffer;
1146     ULONG SigVal;
1147     LONG Esp;
1148
1149     switch (ExceptionCode)
1150     {
1151         case STATUS_INTEGER_DIVIDE_BY_ZERO:
1152             SigVal = 8; /* divide by zero */
1153             break;
1154         case STATUS_SINGLE_STEP:
1155         case STATUS_BREAKPOINT:
1156             SigVal = 5; /* breakpoint */
1157             break;
1158         case STATUS_INTEGER_OVERFLOW:
1159         case STATUS_ARRAY_BOUNDS_EXCEEDED:
1160             SigVal = 16; /* bound instruction */
1161             break;
1162         case STATUS_ILLEGAL_INSTRUCTION:
1163             SigVal = 4; /* Invalid opcode */
1164             break;
1165         case STATUS_STACK_OVERFLOW:
1166         case STATUS_DATATYPE_MISALIGNMENT:
1167         case STATUS_ACCESS_VIOLATION:
1168             SigVal = 11; /* access violation */
1169             break;
1170         default:
1171             SigVal = 7; /* "software generated" */
1172     }
1173
1174     ptr = GspOutBuffer;
1175
1176     *ptr++ = 'T'; /* notify gdb with signo, PC, FP and SP */
1177     *ptr++ = HexChars[(SigVal >> 4) & 0xf];
1178     *ptr++ = HexChars[SigVal & 0xf];
1179
1180     *ptr++ = HexChars[ESP];
1181     *ptr++ = ':';
1182
1183     Esp = GspGetEspFromTrapFrame(TrapFrame); /* SP */
1184     ptr = GspMem2Hex((PCHAR)&Esp, ptr, 4, 0);
1185     *ptr++ = ';';
1186
1187     *ptr++ = HexChars[EBP];
1188     *ptr++ = ':';
1189     ptr = GspMem2Hex((PCHAR)&TrapFrame->Ebp, ptr, 4, 0); /* FP */
1190     *ptr++ = ';';
1191
1192     *ptr++ = HexChars[PC];
1193     *ptr++ = ':';
1194     ptr = GspMem2Hex((PCHAR)&TrapFrame->Eip, ptr, 4, 0); /* PC */
1195     *ptr++ = ';';
1196
1197     *ptr = '\0';
1198 }
1199
1200 /*
1201  * This function does all command procesing for interfacing to GDB.
1202  */
1203 KD_CONTINUE_TYPE
1204 NTAPI
1205 KdpGdbEnterDebuggerException(PEXCEPTION_RECORD ExceptionRecord,
1206                              PCONTEXT Context,
1207                              PKTRAP_FRAME TrapFrame)
1208 {
1209     static BOOLEAN GdbAttached = FALSE;
1210     BOOLEAN Stepping = FALSE;
1211     NTSTATUS ExceptionCode;
1212     LONG Address;
1213     LONG Length;
1214     PCHAR ptr;
1215
1216     /* FIXME: Stop on other CPUs too */
1217
1218     DPRINT("Thread %p entering stub\n", PsGetCurrentThread());
1219     ExceptionCode = (NTSTATUS)ExceptionRecord->ExceptionCode;
1220
1221     /* Can only debug 1 thread at a time... */
1222     ExAcquireFastMutex(&GspLock);
1223     DPRINT("Thread %p acquired mutex\n", PsGetCurrentThread());
1224
1225     GspUnloadBreakpoints();
1226
1227     /* Make sure we're debugging the current thread. */
1228     if (NULL != GspDbgThread)
1229     {
1230         DPRINT1("Internal error: entering stub with non-NULL GspDbgThread\n");
1231         ObDereferenceObject(GspDbgThread);
1232         GspDbgThread = NULL;
1233     }
1234
1235     if (GdbAttached)
1236     {
1237         GspStopReply(ExceptionCode, TrapFrame);
1238         GspPutPacket(GspOutBuffer);
1239         // DbgPrint(">>> (%s) >>>\n", GspOutBuffer);
1240     }
1241     else
1242     {
1243         GdbAttached = TRUE;
1244     }
1245
1246     while (TRUE)
1247     {
1248         /* Zero the buffer now so we don't have to worry about the terminating zero character */
1249         memset(GspOutBuffer, 0, sizeof(GspOutBuffer));
1250         ptr = GspGetPacket();
1251         // DbgPrint("<<< (%s) <<<\n", ptr);
1252
1253         switch (*ptr++)
1254         {
1255             case '?':
1256                 /* a little hack to send more complete status information */
1257                 GspStopReply(ExceptionCode, TrapFrame);
1258                 break;
1259
1260             case 'd':
1261                 GspRemoteDebug = !GspRemoteDebug; /* toggle debug flag */
1262                 break;
1263
1264             case 'g': /* return the value of the CPU Registers */
1265                 GspGetRegisters(GspOutBuffer, TrapFrame);
1266                 break;
1267
1268             case 'G': /* set the value of the CPU Registers - return OK */
1269                 if (NULL != GspDbgThread)
1270                     GspSetRegistersInTrapFrame(ptr, Context, GspDbgThread->Tcb.TrapFrame);
1271                 else
1272                     GspSetRegistersInTrapFrame(ptr, Context, TrapFrame);
1273
1274                 strcpy(GspOutBuffer, "OK");
1275                 break;
1276
1277             case 'P': /* set the value of a single CPU register - return OK */
1278             {
1279                 LONG Register;
1280
1281                 if ((GspHex2Long(&ptr, &Register)) && (*ptr++ == '='))
1282                 {
1283                     if ((Register >= 0) && (Register < NUMREGS))
1284                     {
1285                         if (GspDbgThread)
1286                         {
1287                             GspSetSingleRegisterInTrapFrame(ptr, Register, Context,
1288                                                             GspDbgThread->Tcb.TrapFrame);
1289                         }
1290                         else
1291                         {
1292                             GspSetSingleRegisterInTrapFrame(ptr, Register, Context, TrapFrame);
1293                         }
1294                         strcpy(GspOutBuffer, "OK");
1295                         break;
1296                     }
1297                 }
1298
1299                 strcpy(GspOutBuffer, "E01");
1300                 break;
1301             }
1302
1303             /* mAA..AA,LLLL  Read LLLL bytes at address AA..AA */
1304             case 'm':
1305             {
1306                 /* TRY TO READ %x,%x.  IF SUCCEED, SET PTR = 0 */
1307                 if (GspHex2Long(&ptr, &Address) && *(ptr++) == ',' && GspHex2Long(&ptr, &Length))
1308                 {
1309                     PEPROCESS DbgProcess = NULL;
1310
1311                     ptr = NULL;
1312                     if (NULL != GspDbgThread &&
1313                         PsGetCurrentProcess() != GspDbgThread->ThreadsProcess)
1314                     {
1315                         DbgProcess = GspDbgThread->ThreadsProcess;
1316                         KeAttachProcess(&DbgProcess->Pcb);
1317                     }
1318
1319                     GspMemoryError = FALSE;
1320                     GspMem2Hex((PCHAR)Address, GspOutBuffer, Length, 1);
1321
1322                     if (NULL != DbgProcess)
1323                         KeDetachProcess();
1324
1325                     if (GspMemoryError)
1326                     {
1327                         strcpy(GspOutBuffer, "E03");
1328                         DPRINT1("Fault during memory read\n");
1329                     }
1330                     ptr = NULL;
1331                 }
1332
1333                 if (NULL != ptr)
1334                     strcpy(GspOutBuffer, "E01");
1335
1336                 break;
1337             }
1338
1339             /* MAA..AA,LLLL: Write LLLL bytes at address AA.AA return OK */
1340             case 'M':
1341             {
1342                 /* TRY TO READ '%x,%x:'.  IF SUCCEED, SET PTR = 0 */
1343                 if (GspHex2Long(&ptr, &Address))
1344                 {
1345                     if (*(ptr++) == ',' && GspHex2Long(&ptr, &Length) && *(ptr++) == ':')
1346                     {
1347                         PEPROCESS DbgProcess = NULL;
1348
1349                         if (NULL != GspDbgThread &&
1350                             PsGetCurrentProcess() != GspDbgThread->ThreadsProcess)
1351                         {
1352                             DbgProcess = GspDbgThread->ThreadsProcess;
1353                             KeAttachProcess(&DbgProcess->Pcb);
1354                         }
1355                         GspMemoryError = FALSE;
1356                         GspHex2Mem(ptr, (PCHAR)Address, Length, TRUE);
1357                         if (NULL != DbgProcess)
1358                             KeDetachProcess();
1359
1360                         if (GspMemoryError)
1361                         {
1362                             strcpy(GspOutBuffer, "E03");
1363                             DPRINT1("Fault during memory write\n");
1364                         }
1365                         else
1366                         {
1367                             strcpy(GspOutBuffer, "OK");
1368                         }
1369                         ptr = NULL;
1370                     }
1371                 }
1372
1373                 if (NULL != ptr)
1374                     strcpy(GspOutBuffer, "E02");
1375
1376                 break;
1377             }
1378
1379             /* cAA..AA   Continue at address AA..AA */
1380             /* sAA..AA   Step one instruction from AA..AA */
1381             case 's':
1382                 Stepping = TRUE;
1383             case 'c':
1384             {
1385                 ULONG BreakpointNumber;
1386                 ULONG Dr6;
1387
1388                 /* try to read optional parameter, pc changed if param */
1389                 if (GspHex2Long(&ptr, &Address))
1390                 {
1391                     Context->Eip = Address;
1392                 }
1393                 else if (ExceptionCode == STATUS_BREAKPOINT)
1394                 {
1395                     if (GspReadMemSafe((PCHAR)Context->Eip) == (CHAR)I386_OPCODE_INT3)
1396                         Context->Eip++;
1397                 }
1398
1399                 /* clear the trace bit */
1400                 Context->EFlags &= ~EFLAGS_TF;
1401
1402                 /* set the trace bit if we're Stepping */
1403                 if (Stepping)
1404                     Context->EFlags |= EFLAGS_TF;
1405
1406                 Dr6 = __readdr(6);
1407                 if (!(Dr6 & DR6_BS))
1408                 {
1409                     for (BreakpointNumber = 0;
1410                          BreakpointNumber < MAX_HW_BREAKPOINTS;
1411                          BreakpointNumber++)
1412                     {
1413                         if (Dr6 & (1 << BreakpointNumber))
1414                         {
1415                             if (GspHwBreakpoints[BreakpointNumber].Type == I386_BP_TYPE_EXECUTE)
1416                             {
1417                                 /* Set restore flag */
1418                                 Context->EFlags |= EFLAGS_RF;
1419                                 break;
1420                             }
1421                         }
1422                     }
1423                 }
1424
1425                 GspLoadBreakpoints(TrapFrame);
1426                 __writedr(6, 0);
1427
1428                 if (NULL != GspDbgThread)
1429                 {
1430                     ObDereferenceObject(GspDbgThread);
1431                     GspDbgThread = NULL;
1432                 }
1433
1434                 DPRINT("Thread %p releasing mutex\n", PsGetCurrentThread());
1435                 ExReleaseFastMutex(&GspLock);
1436                 DPRINT("Thread %p leaving stub\n", PsGetCurrentThread());
1437
1438                 if (ExceptionCode == STATUS_BREAKPOINT ||
1439                     ExceptionCode == STATUS_SINGLE_STEP)
1440                     return kdContinue;
1441
1442                 return kdHandleException;
1443             }
1444
1445             case 'k': /* kill the program */
1446                 strcpy(GspOutBuffer, "OK");
1447                 break;
1448
1449             case 'H': /* Set thread */
1450                 GspSetThread(ptr);
1451                 break;
1452
1453             case 'q': /* Query */
1454                 GspQuery(ptr);
1455                 break;
1456
1457             case 'T': /* Query thread status */
1458                 GspQueryThreadStatus(ptr);
1459                 break;
1460
1461             case 'Z':
1462             {
1463                 LONG Type;
1464                 LONG Address;
1465                 LONG Length;
1466
1467                 GspHex2Long(&ptr, &Type);
1468                 ptr++;
1469                 GspHex2Long(&ptr, &Address);
1470                 ptr++;
1471                 GspHex2Long(&ptr, &Length);
1472
1473                 if (0 == Type)
1474                     GspSetSwBreakpoint((ULONG_PTR)Address);
1475                 else
1476                     GspSetHwBreakpoint(Type, (ULONG_PTR)Address, Length);
1477
1478                 break;
1479             }
1480
1481             case 'z':
1482             {
1483                 LONG Type;
1484                 LONG Address;
1485                 LONG Length;
1486
1487                 GspHex2Long(&ptr, &Type);
1488                 ptr++;
1489                 GspHex2Long(&ptr, &Address);
1490                 ptr++;
1491                 GspHex2Long(&ptr, &Length);
1492
1493                 if (0 == Type)
1494                     GspRemoveSwBreakpoint((ULONG_PTR)Address);
1495                 else
1496                     GspRemoveHwBreakpoint(Type, (ULONG_PTR)Address, Length);
1497
1498                 break;
1499             }
1500
1501             default:
1502                 break;
1503         }
1504
1505         /* reply to the request */
1506         GspPutPacket(GspOutBuffer);
1507         // DbgPrint(">>> (%s) >>>\n",  GspOutBuffer);
1508     }
1509 }
1510
1511 BOOLEAN
1512 NTAPI
1513 GspBreakIn(PKINTERRUPT Interrupt, PVOID ServiceContext)
1514 {
1515     PKTRAP_FRAME TrapFrame;
1516     BOOLEAN DoBreakIn;
1517     CONTEXT Context;
1518     KIRQL OldIrql;
1519     UCHAR Value;
1520
1521     DPRINT("Break In\n");
1522
1523     DoBreakIn = FALSE;
1524     while (KdPortGetByteEx(&GdbPortInfo, &Value))
1525     {
1526         if (Value == 0x03)
1527             DoBreakIn = TRUE;
1528     }
1529
1530     if (!DoBreakIn)
1531         return TRUE;
1532
1533     KeRaiseIrql(HIGH_LEVEL, &OldIrql);
1534
1535     TrapFrame = PsGetCurrentThread()->Tcb.TrapFrame;
1536
1537     KeTrapFrameToContext(TrapFrame, NULL, &Context);
1538
1539     KdpGdbEnterDebuggerException(NULL, &Context, TrapFrame);
1540
1541     KeContextToTrapFrame(&Context, NULL, TrapFrame, Context.ContextFlags, KernelMode);
1542
1543     KeLowerIrql(OldIrql);
1544
1545     return TRUE;
1546 }
1547
1548 VOID
1549 NTAPI
1550 KdpGdbDebugPrint(PCH Message, ULONG Length)
1551 {
1552 }
1553
1554 /* Initialize the GDB stub */
1555 VOID
1556 NTAPI
1557 KdpGdbStubInit(PKD_DISPATCH_TABLE WrapperTable, ULONG BootPhase)
1558 {
1559     if (!KdDebuggerEnabled || !KdpDebugMode.Gdb)
1560         return;
1561
1562     if (BootPhase == 0)
1563     {
1564         ExInitializeFastMutex(&GspLock);
1565
1566         /* Write out the functions that we support for now */
1567         WrapperTable->KdpInitRoutine = KdpGdbStubInit;
1568         WrapperTable->KdpPrintRoutine = KdpGdbDebugPrint;
1569         WrapperTable->KdpExceptionRoutine = KdpGdbEnterDebuggerException;
1570
1571         /* Initialize the Port */
1572         KdPortInitializeEx(&GdbPortInfo, 0, 0);
1573     }
1574     else if (BootPhase == 1)
1575     {
1576         GspInitialized = TRUE;
1577
1578         GspRunThread = NULL;
1579         GspDbgThread = NULL;
1580         GspEnumThread = NULL;
1581
1582         HalDisplayString("Waiting for GDB to attach\n");
1583         DbgBreakPointWithStatus(DBG_STATUS_CONTROL_C);
1584     }
1585     else if (BootPhase == 2)
1586     {
1587         HalDisplayString("\n   GDB debugging enabled\n\n");
1588     }
1589 }
1590
1591 /* EOF */