From 5e0491122d7d60baf14f51e4516288c689031068 Mon Sep 17 00:00:00 2001
From: Alex Ionescu <aionescu@gmail.com>
Date: Mon, 5 Mar 2007 17:09:44 +0000
Subject: [PATCH] - Implement simple breakpoints (KdWriteBreakPointApi,
 KdpWriteBreakpoint, KdpAddBreakpoint). - Stepping out with WinDBG now works,
 but not adding breakpoints (that's KdWriteBreakpointExApi, coming up soon). -
 This was supposed to be 26000 but oh well, back porting features to a locked
 branch seems to have won out :)

svn path=/trunk/; revision=26004
---
 reactos/ntoskrnl/include/internal/kd64.h |  6 +++
 reactos/ntoskrnl/kd64/kdapi.c            | 36 ++++++++++++++++--
 reactos/ntoskrnl/kd64/kdbreak.c          | 48 ++++++++++++++++++++++++
 3 files changed, 87 insertions(+), 3 deletions(-)

diff --git a/reactos/ntoskrnl/include/internal/kd64.h b/reactos/ntoskrnl/include/internal/kd64.h
index fe849dcbafc..6387e6fc1bd 100644
--- a/reactos/ntoskrnl/include/internal/kd64.h
+++ b/reactos/ntoskrnl/include/internal/kd64.h
@@ -228,6 +228,12 @@ KdpDeleteBreakpointRange(
     IN PVOID Limit
 );
 
+ULONG
+NTAPI
+KdpAddBreakpoint(
+    IN PVOID Address
+);
+
 //
 // Global KD Data
 //
diff --git a/reactos/ntoskrnl/kd64/kdapi.c b/reactos/ntoskrnl/kd64/kdapi.c
index 2307dda0719..5e5c044ad3d 100644
--- a/reactos/ntoskrnl/kd64/kdapi.c
+++ b/reactos/ntoskrnl/kd64/kdapi.c
@@ -14,6 +14,37 @@
 
 /* PRIVATE FUNCTIONS *********************************************************/
 
+VOID
+NTAPI
+KdpWriteBreakpoint(IN PDBGKD_MANIPULATE_STATE64 State,
+                   IN PSTRING Data,
+                   IN PCONTEXT Context)
+{
+    PDBGKD_WRITE_BREAKPOINT64 Breakpoint = &State->u.WriteBreakPoint;
+    STRING Header;
+    NTSTATUS Status;
+
+    /* Build header */
+    Header.Length = sizeof(DBGKD_MANIPULATE_STATE64);
+    Header.Buffer = (PCHAR)State;
+    ASSERT(Data->Length == 0);
+
+    /* Create the breakpoint */
+    Breakpoint->BreakPointHandle =
+        KdpAddBreakpoint((PVOID)(LONG_PTR)Breakpoint->BreakPointAddress);
+    if (!Breakpoint->BreakPointHandle)
+    {
+        /* We failed */
+        Status = STATUS_UNSUCCESSFUL;
+    }
+
+    /* Send the packet */
+    KdSendPacket(PACKET_TYPE_KD_STATE_MANIPULATE,
+                 &Header,
+                 NULL,
+                 &KdpContext);
+}
+
 VOID
 NTAPI
 DumpTraceData(IN PSTRING TraceData)
@@ -545,9 +576,8 @@ SendPacket:
 
             case DbgKdWriteBreakPointApi:
 
-                /* FIXME: TODO */
-                Ke386SetCr2(DbgKdWriteBreakPointApi);
-                while (TRUE);
+                /* Write the breakpoint */
+                KdpWriteBreakpoint(&ManipulateState, &Data, Context);
                 break;
 
             case DbgKdRestoreBreakPointApi:
diff --git a/reactos/ntoskrnl/kd64/kdbreak.c b/reactos/ntoskrnl/kd64/kdbreak.c
index 4b9b9af12f7..31c8b68e7a2 100644
--- a/reactos/ntoskrnl/kd64/kdbreak.c
+++ b/reactos/ntoskrnl/kd64/kdbreak.c
@@ -14,6 +14,54 @@
 
 /* FUNCTIONS *****************************************************************/
 
+ULONG
+NTAPI
+KdpAddBreakpoint(IN PVOID Address)
+{
+    UCHAR Content;
+    ULONG i;
+
+    /* Loop current breakpoints */
+    for (i = 0; i < 20; i++)
+    {
+        /* Check if the breakpoint is valid */
+        if ((KdpBreakpointTable[i].Flags & KdpBreakpointActive) &&
+            (KdpBreakpointTable[i].Address == Address))
+        {
+            /* Check if it's pending */
+            if ((KdpBreakpointTable[i].Flags & KdpBreakpointPending))
+            {
+                /* It's not pending anymore now */
+                KdpBreakpointTable[i].Flags &= ~KdpBreakpointPending;
+                return i + 1;
+            }
+            else
+            {
+                /* Fail */
+                return 0;
+            }
+        }
+    }
+
+    /* Find a free entry */
+    for (i = 0; i < 20; i++) if (!(KdpBreakpointTable[i].Flags)) break;
+
+    /* Fail if no free entry was found */
+    if (i == 20) return 0;
+
+    /* Save the old instruction */
+    RtlCopyMemory(&Content, Address, sizeof(UCHAR));
+
+    /* Write the entry */
+    KdpBreakpointTable[i].Address = Address;
+    KdpBreakpointTable[i].Content = Content;
+    KdpBreakpointTable[i].Flags = KdpBreakpointActive;
+
+    /* Write the INT3 and return the handle */
+    RtlCopyMemory(Address, &KdpBreakpointInstruction, sizeof(UCHAR));
+    return i + 1;
+}
+
 BOOLEAN
 NTAPI
 KdpLowWriteContent(IN ULONG BpIndex)