From 4858ae01590b402463552feb179f84e6ef1a6cb6 Mon Sep 17 00:00:00 2001
From: Berkus Decker <berkus+github@metta.systems>
Date: Sat, 23 Feb 2019 22:57:59 +0200
Subject: [PATCH] Add JTAG debugger wait function

---
 src/arch/aarch64/boot.rs |  2 ++
 src/arch/aarch64/mod.rs  | 16 ++++++++++++++++
 2 files changed, 18 insertions(+)

diff --git a/src/arch/aarch64/boot.rs b/src/arch/aarch64/boot.rs
index 2423735..793d1e7 100644
--- a/src/arch/aarch64/boot.rs
+++ b/src/arch/aarch64/boot.rs
@@ -126,6 +126,8 @@ fn setup_and_enter_el1_from_el2() -> ! {
 pub unsafe extern "C" fn _boot_cores() -> ! {
     use cortex_a::{asm, regs::*};
 
+    // crate::arch::aarch64::jtag_dbg_wait();
+
     const CORE_0: u64 = 0;
     const CORE_MASK: u64 = 0x3;
     const EL1: u32 = CurrentEL::EL::EL1.value;
diff --git a/src/arch/aarch64/mod.rs b/src/arch/aarch64/mod.rs
index 0a21bf4..a96b1bd 100644
--- a/src/arch/aarch64/mod.rs
+++ b/src/arch/aarch64/mod.rs
@@ -8,6 +8,22 @@ pub use mmu::*;
 
 use cortex_a::{asm, barrier, regs::*};
 
+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.
+#[inline]
+pub fn jtag_dbg_wait() {
+    use core::ptr::{read_volatile, write_volatile};
+
+    while unsafe { read_volatile(&WAIT_FLAG) } {
+        asm::nop();
+    }
+    // Reset the flag so that next jtag_dbg_wait() would block again.
+    unsafe { write_volatile(&mut WAIT_FLAG, true) }
+}
+
 // Data memory barrier
 #[inline]
 pub fn dmb() {