Fix single step on AARCH64 processors (#32)

## Description

On AARCH64 processors single step was not working on real hardware
because either 1. the OS lock register was set, or 2. the DEBUG
exceptions were masked by the DAIF configuration. This makes sure
neither are true in debugger configuration.

- [x] Impacts functionality?
- [ ] Impacts security?
- [ ] Breaking change?
- [ ] Includes tests?
- [ ] Includes documentation?

## How This Was Tested

Manually tested on an AARCH64 machine.

## Integration Instructions

N/A

DebuggerFeaturePkg/DebuggerFeaturePkg.ci.yaml

@@ -73,7 +73,9 @@
             "fioff",
             "fiseg",
             "fooff",
-            "foseg"],
+            "foseg",
+            "oslar",
+            "oslsr"],
         "IgnoreStandardPaths": [],   # Standard Plugin defined paths that should be ignore
         "AdditionalIncludePaths": [] # Additional paths to spell check (wildcards supported)
     },

DebuggerFeaturePkg/Library/DebugAgent/AARCH64/DebugAarch64.c

@@ -22,10 +22,14 @@
 // Debug register definitions.
 //
 
-#define ARM64_MDSCR_MDE   0x00008000
-#define ARM64_MDSCR_KDE   0x00002000
-#define ARM64_MDSCR_TDCC  0x00001000
-#define ARM64_MDSCR_SS    0x00000001
+#define MDSCR_MDE   0x00008000
+#define MDSCR_KDE   0x00002000
+#define MDSCR_TDCC  0x00001000
+#define MDSCR_SS    0x00000001
+
+#define OSLSR_LOCKED  0x2
+
+#define DAIF_DEBUG  0x200
 
 //
 // Assembly routines.
@@ -41,6 +45,26 @@ DebugWriteMdscrEl1 (
   IN UINT64  Value
   );
 
+UINT64
+DebugReadDaif (
+  VOID
+  );
+
+VOID
+DebugWriteDaif (
+  IN UINT64  Value
+  );
+
+UINT64
+DebugReadOslsrEl1 (
+  VOID
+  );
+
+VOID
+DebugWriteOslarEl1 (
+  IN UINT64  Value
+  );
+
 UINT64
 DebugGetTCR (
   VOID
@@ -141,7 +165,7 @@ DebuggerExceptionHandler (
 
       // Clear the step flag is present.
       Value  = DebugReadMdscrEl1 ();
-      Value &= ~ARM64_MDSCR_SS;
+      Value &= ~MDSCR_SS;
       DebugWriteMdscrEl1 (Value);
 
       ExceptionInfo.ExceptionType    = ExceptionDebugStep;
@@ -202,7 +226,7 @@ AddSingleStep (
 
   // Set the Software Step bit in the MDSCR.
   Value  = DebugReadMdscrEl1 ();
-  Value |= (ARM64_MDSCR_SS | ARM64_MDSCR_MDE | ARM64_MDSCR_KDE);
+  Value |= (MDSCR_SS | MDSCR_MDE | MDSCR_KDE);
   DebugWriteMdscrEl1 (Value);
 }
 
@@ -242,9 +266,39 @@ DebugArchInit (
 {
   UINT64  Value;
 
+  //
+  // For AARCH64 debugging to work, the following must be true.
+  //    1. OS Lock is unlocked.
+  //    2. Enabled the kernel and monitor debug bits in the MDSCR
+  //    3. Enabled debug exceptions in the DAIF
+  //
+
+  // Make sure debug exceptions are disable in the DAIF while configuring in case
+  // there is some latent configuration.
+  Value  = DebugReadDaif ();
+  Value |= DAIF_DEBUG;
+  DebugWriteDaif (Value);
+  SpeculationBarrier ();
+
+  // Clear the OS lock if needed.
+  Value = DebugReadOslsrEl1 ();
+  if (Value & OSLSR_LOCKED) {
+    DebugWriteOslarEl1 (0);
+  }
+
+  SpeculationBarrier ();
+
+  // Enable kernel and monitor debug bits.
   Value  = DebugReadMdscrEl1 ();
-  Value |= (ARM64_MDSCR_MDE | ARM64_MDSCR_KDE);
+  Value |= (MDSCR_MDE | MDSCR_KDE);
   DebugWriteMdscrEl1 (Value);
+  SpeculationBarrier ();
+
+  // Make sure debug exceptions are enabled in the DAIF.
+  Value  = DebugReadDaif ();
+  Value &= ~DAIF_DEBUG;
+  DebugWriteDaif (Value);
+  SpeculationBarrier ();
 }
 
 #define MIN_T0SZ        16

DebuggerFeaturePkg/Library/DebugAgent/AARCH64/Registers.S

@@ -12,6 +12,10 @@
 
 GCC_ASM_EXPORT(DebugReadMdscrEl1)
 GCC_ASM_EXPORT(DebugWriteMdscrEl1)
+GCC_ASM_EXPORT(DebugReadOslsrEl1)
+GCC_ASM_EXPORT(DebugWriteOslarEl1)
+GCC_ASM_EXPORT(DebugReadDaif)
+GCC_ASM_EXPORT(DebugWriteDaif)
 GCC_ASM_EXPORT(DebugGetTCR)
 GCC_ASM_EXPORT(DebugGetTTBR0BaseAddress)
 
@@ -23,6 +27,22 @@ ASM_PFX(DebugWriteMdscrEl1):
     msr mdscr_el1, x0
     ret
 
+ASM_PFX(DebugReadOslsrEl1):
+    mrs x0, oslsr_el1
+    ret
+
+ASM_PFX(DebugWriteOslarEl1):
+    msr oslar_el1, x0
+    ret
+
+ASM_PFX(DebugReadDaif):
+    mrs x0, daif
+    ret
+
+ASM_PFX(DebugWriteDaif):
+    msr daif, x0
+    ret
+
 ASM_PFX(DebugGetTCR):
     mrs x1, CurrentEL
     cmp x1, #0x8

DebuggerFeaturePkg/Library/DebugAgent/AARCH64/Registers.masm

@@ -9,6 +9,10 @@
 
     EXPORT DebugReadMdscrEl1
     EXPORT DebugWriteMdscrEl1
+    EXPORT DebugReadOslsrEl1
+    EXPORT DebugWriteOslarEl1
+    EXPORT DebugReadDaif
+    EXPORT DebugWriteDaif
     EXPORT DebugGetTCR
     EXPORT DebugGetTTBR0BaseAddress
 
@@ -25,6 +29,30 @@ DebugWriteMdscrEl1 PROC
 DebugWriteMdscrEl1 ENDP
 
 
+DebugReadOslsrEl1 PROC
+  mrs x0, oslsr_el1
+  ret
+DebugReadOslsrEl1 ENDP
+
+
+DebugWriteOslarEl1 PROC
+  msr oslar_el1, x0
+  ret
+DebugWriteOslarEl1 ENDP
+
+
+DebugReadDaif PROC
+  mrs x0, daif
+  ret
+DebugReadDaif ENDP
+
+
+DebugWriteDaif PROC
+  msr daif, x0
+  ret
+DebugWriteDaif ENDP
+
+
 DebugGetTCR PROC
   mrs   x1, CurrentEL
   cmp   x1, #0x8

DebuggerFeaturePkg/Library/DebugAgent/GdbStub/GdbStub.c

@@ -133,6 +133,44 @@ SendGdbAck (
   }
 }
 
+/**
+  Sends a GDB notification packet with log information.
+
+  @param[in] FormatString  Format string of the log message.
+  @param[in] VA_LIST       Additional arguments for the formatting.
+
+**/
+/*
+STATIC
+VOID
+GdbNotifyLog (
+  IN  CONST CHAR8  *FormatString,
+  ...
+  )
+{
+  VA_LIST  Marker;
+  UINTN    Length;
+  UINT8    Checksum;
+  CHAR8    String[64];
+
+  Length = sizeof ("%log:") - 1;
+  CopyMem (String, "%log:", Length);
+
+  VA_START (Marker, FormatString);
+  Length += AsciiVSPrint (&String[Length], sizeof (String) - Length - 4, FormatString, Marker);
+  VA_END (Marker);
+
+  Checksum           = CalculateSum8 ((UINT8 *)&String[1], Length - 1);
+  String[Length]     = '#';
+  String[Length + 1] = HexChars[Checksum >> 4];
+  String[Length + 2] = HexChars[Checksum & 0xF];
+  String[Length + 3] = 0;
+  Length            += 3;
+
+  DebugTransportWrite ((UINT8 *)&String[0], Length);
+}
+*/
+
 /**
   Sends a checksummed GDB packet response.