From 3415ccd68c14e5783f30d503671b76dc8c0f4b2b Mon Sep 17 00:00:00 2001
From: Berkus Decker <berkus+github@metta.systems>
Date: Mon, 2 Nov 2020 20:39:21 +0200
Subject: [PATCH] Add JTAG helpers

---
 .github/workflows/build.yml      |  3 +++
 nucleus/Cargo.toml               |  3 +++
 nucleus/src/arch/aarch64/jtag.rs | 19 +++++++++++++++++++
 nucleus/src/arch/aarch64/mod.rs  |  2 ++
 nucleus/src/main.rs              |  3 +++
 5 files changed, 30 insertions(+)
 create mode 100644 nucleus/src/arch/aarch64/jtag.rs

diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml
index 916d193..25cdd92 100644
--- a/.github/workflows/build.yml
+++ b/.github/workflows/build.yml
@@ -119,6 +119,9 @@ jobs:
           "noserial",
           "qemu",
           "noserial,qemu",
+          "jtag",
+          "noserial,jtag",
+          # jtag and qemu together don't make much sense
         ]
     runs-on: ubuntu-latest
     timeout-minutes: 10
diff --git a/nucleus/Cargo.toml b/nucleus/Cargo.toml
index 8eca98b..1b66511 100644
--- a/nucleus/Cargo.toml
+++ b/nucleus/Cargo.toml
@@ -17,6 +17,9 @@ maintenance = { status = "experimental" }
 
 [features]
 noserial = []
+# Enable JTAG debugging of kernel - enable jtag helpers and
+# block waiting for JTAG probe attach at the start of kernel main.
+jtag = []
 # Build for running under QEMU with semihosting, so various halt/reboot options would for example quit QEMU instead.
 qemu = ["qemu-exit"]
 
diff --git a/nucleus/src/arch/aarch64/jtag.rs b/nucleus/src/arch/aarch64/jtag.rs
new file mode 100644
index 0000000..d264a86
--- /dev/null
+++ b/nucleus/src/arch/aarch64/jtag.rs
@@ -0,0 +1,19 @@
+//! JTAG helper functions.
+
+use cortex_a::asm;
+
+#[no_mangle]
+static mut WAIT_FLAG: bool = true;
+
+/// Wait for debugger to attach.
+/// Then in gdb issue `> set var *(&WAIT_FLAG) = 0`
+/// from inside this function's frame to contiue running.
+pub fn wait_debugger() {
+    use core::ptr::{read_volatile, write_volatile};
+
+    while unsafe { read_volatile(&WAIT_FLAG) } {
+        asm::nop();
+    }
+    // Reset the flag so that next jtag::wait_debugger() would block again.
+    unsafe { write_volatile(&mut WAIT_FLAG, true) }
+}
diff --git a/nucleus/src/arch/aarch64/mod.rs b/nucleus/src/arch/aarch64/mod.rs
index 2435c9e..c0fb793 100644
--- a/nucleus/src/arch/aarch64/mod.rs
+++ b/nucleus/src/arch/aarch64/mod.rs
@@ -8,6 +8,8 @@
 use cortex_a::asm;
 
 mod boot;
+#[cfg(feature = "jtag")]
+pub mod jtag;
 pub mod memory;
 pub mod traps;
 
diff --git a/nucleus/src/main.rs b/nucleus/src/main.rs
index 7f7e915..2b85277 100644
--- a/nucleus/src/main.rs
+++ b/nucleus/src/main.rs
@@ -111,6 +111,9 @@ fn init_uart_serial() {
 /// `arch` crate is responsible for calling it.
 #[inline]
 pub fn kmain() -> ! {
+    #[cfg(feature = "jtag")]
+    jtag::wait_debugger();
+
     #[cfg(not(feature = "noserial"))]
     init_uart_serial();