From 4e1f46b3e232609b7bc8d818f60ce07fa769d93b Mon Sep 17 00:00:00 2001
From: Sebastien Bourdeauducq <sb@m-labs.hk>
Date: Wed, 6 May 2020 22:05:34 +0800
Subject: [PATCH] core1: support redirecting vectors to sdram

---
 experiments/src/main.rs     |  2 +-
 libsupport_zynq/src/boot.rs | 19 +++++++++++++++++--
 2 files changed, 18 insertions(+), 3 deletions(-)

diff --git a/experiments/src/main.rs b/experiments/src/main.rs
index 56682ea..db1bb24 100644
--- a/experiments/src/main.rs
+++ b/experiments/src/main.rs
@@ -113,7 +113,7 @@ pub fn main_core0() {
         flash = flash_io.stop();
     }
 
-    let core1 = boot::Core1::start();
+    let core1 = boot::Core1::start(false);
 
     let (mut core1_req, rx) = sync_channel(10);
     *CORE1_REQ.lock() = Some(rx);
diff --git a/libsupport_zynq/src/boot.rs b/libsupport_zynq/src/boot.rs
index 507ad66..045ac49 100644
--- a/libsupport_zynq/src/boot.rs
+++ b/libsupport_zynq/src/boot.rs
@@ -1,4 +1,5 @@
 use r0::zero_bss;
+use core::ptr::write_volatile;
 use libregister::{
     VolatileCell,
     RegisterR, RegisterW, RegisterRW,
@@ -108,7 +109,7 @@ pub struct Core1 {
 
 impl Core1 {
     /// Reset and start core1
-    pub fn start() -> Self {
+    pub fn start(sdram: bool) -> Self {
         // reset and stop (safe to repeat)
         slcr::RegisterBlock::unlocked(|slcr| {
             slcr.a9_cpu_rst_ctrl.modify(|_, w| w.a9_rst1(true));
@@ -116,13 +117,27 @@ impl Core1 {
             slcr.a9_cpu_rst_ctrl.modify(|_, w| w.a9_rst1(false));
         });
 
+        if sdram {
+            // Cores always start from OCM no matter what you do.
+            // Make up a vector table there that just jumps to SDRAM.
+            for i in 0..8 {
+                unsafe {
+                    // this is the ARM instruction "b +0x00100000"
+                    write_volatile((i*4) as *mut u32, 0xea03fffe);
+                }
+            }
+        }
+
         unsafe {
             CORE1_ENABLED.set(true);
         }
-        // Ensure stack pointer has been written to cache
+        // Ensure values have been written to cache
         asm::dmb();
         // Flush cache-line
         cache::dccmvac(unsafe { &CORE1_ENABLED } as *const _ as usize);
+        if sdram {
+            cache::dccmvac(0);
+        }
 
         // wake up core1
         slcr::RegisterBlock::unlocked(|slcr| {