From d58a3ef12c017546f8c2e7e249a7a301c3b580ac Mon Sep 17 00:00:00 2001
From: pca006132 <john.lck40@gmail.com>
Date: Tue, 4 Aug 2020 10:17:19 +0800
Subject: [PATCH] runtime/comms: restart core1 before kernel load.

---
 src/runtime/Cargo.toml            |  2 +-
 src/runtime/src/comms.rs          |  1 +
 src/runtime/src/irq.rs            | 46 +++++++++++++++++++++++++++++++
 src/runtime/src/kernel/control.rs | 11 ++++++++
 src/runtime/src/main.rs           |  1 +
 5 files changed, 60 insertions(+), 1 deletion(-)
 create mode 100644 src/runtime/src/irq.rs

diff --git a/src/runtime/Cargo.toml b/src/runtime/Cargo.toml
index 6ffc889..1c5d6a3 100644
--- a/src/runtime/Cargo.toml
+++ b/src/runtime/Cargo.toml
@@ -26,7 +26,7 @@ log_buffer = { version = "1.2" }
 libm = { version = "0.2", features = ["unstable"] }
 
 libboard_zynq = { git = "https://git.m-labs.hk/M-Labs/zc706.git" }
-libsupport_zynq = { default-features = false, features = ["alloc_core", "dummy_irq_handler"], git = "https://git.m-labs.hk/M-Labs/zc706.git" }
+libsupport_zynq = { default-features = false, features = ["alloc_core"], git = "https://git.m-labs.hk/M-Labs/zc706.git" }
 libcortex_a9 = { git = "https://git.m-labs.hk/M-Labs/zc706.git" }
 libasync = { git = "https://git.m-labs.hk/M-Labs/zc706.git" }
 libregister = { git = "https://git.m-labs.hk/M-Labs/zc706.git" }
diff --git a/src/runtime/src/comms.rs b/src/runtime/src/comms.rs
index 30df324..4340656 100644
--- a/src/runtime/src/comms.rs
+++ b/src/runtime/src/comms.rs
@@ -232,6 +232,7 @@ async fn handle_run_kernel(stream: Option<&TcpStream>, control: &Rc<RefCell<kern
 
 async fn load_kernel(buffer: Vec<u8>, control: &Rc<RefCell<kernel::Control>>, stream: Option<&TcpStream>) -> Result<()> {
     let mut control = control.borrow_mut();
+    control.restart();
     control.tx.async_send(kernel::Message::LoadRequest(buffer)).await;
     let reply = control.rx.async_recv().await;
     match reply {
diff --git a/src/runtime/src/irq.rs b/src/runtime/src/irq.rs
new file mode 100644
index 0000000..972604f
--- /dev/null
+++ b/src/runtime/src/irq.rs
@@ -0,0 +1,46 @@
+use libboard_zynq::{gic, mpcore, println, stdio};
+use libcortex_a9::{
+    asm,
+    regs::{MPIDR, SP},
+};
+use libregister::{RegisterR, RegisterW};
+use core::sync::atomic::{AtomicBool, Ordering};
+
+extern "C" {
+    static mut __stack1_start: u32;
+    fn main_core1() -> !;
+}
+
+static CORE1_RESTART: AtomicBool = AtomicBool::new(false);
+
+#[link_section = ".text.boot"]
+#[no_mangle]
+#[naked]
+pub unsafe extern "C" fn IRQ() {
+    if MPIDR.read().cpu_id() == 1 {
+        let mpcore = mpcore::RegisterBlock::new();
+        let mut gic = gic::InterruptController::new(mpcore);
+        let id = gic.get_interrupt_id();
+        if id.0 == 0 {
+            gic.end_interrupt(id);
+            asm::exit_irq();
+            SP.write(&mut __stack1_start as *mut _ as u32);
+            asm::enable_irq();
+            CORE1_RESTART.store(false, Ordering::Relaxed);
+            asm::sev();
+            main_core1();
+        }
+    }
+    stdio::drop_uart();
+    println!("IRQ");
+    loop {}
+}
+
+pub fn restart_core1() {
+    let mut interrupt_controller = gic::InterruptController::new(mpcore::RegisterBlock::new());
+    CORE1_RESTART.store(true, Ordering::Relaxed);
+    interrupt_controller.send_sgi(gic::InterruptId(0), gic::CPUCore::Core1.into());
+    while CORE1_RESTART.load(Ordering::Relaxed) {
+        asm::wfe();
+    }
+}
diff --git a/src/runtime/src/kernel/control.rs b/src/runtime/src/kernel/control.rs
index 1793022..9a7d1a9 100644
--- a/src/runtime/src/kernel/control.rs
+++ b/src/runtime/src/kernel/control.rs
@@ -2,6 +2,9 @@ use libcortex_a9::sync_channel::{Sender, Receiver};
 use libsupport_zynq::boot::Core1;
 
 use super::{CHANNEL_0TO1, CHANNEL_1TO0, Message};
+use crate::irq::restart_core1;
+
+use core::mem::{forget, replace};
 
 pub struct Control {
     pub tx: Sender<'static, Message>,
@@ -34,5 +37,13 @@ impl Control {
             rx: core0_rx,
         }
     }
+
+    pub fn restart(&mut self) {
+        restart_core1();
+        let (core0_tx, core0_rx) = get_channels();
+        // dangling pointer here, so we forget it
+        forget(replace(&mut self.tx, core0_tx));
+        forget(replace(&mut self.rx, core0_rx));
+    }
 }
 
diff --git a/src/runtime/src/main.rs b/src/runtime/src/main.rs
index 5842b34..0854ce3 100644
--- a/src/runtime/src/main.rs
+++ b/src/runtime/src/main.rs
@@ -40,6 +40,7 @@ mod panic;
 mod logger;
 mod mgmt;
 mod analyzer;
+mod irq;
 
 fn init_gateware() {
     // Set up PS->PL clocks