boot: reset core1 before start

This commit is contained in:
Astro 2019-11-18 00:38:03 +01:00
parent 0bc941d789
commit ef6d0ff3f1
3 changed files with 42 additions and 18 deletions

View File

@ -1,7 +1,8 @@
use r0::zero_bss; use r0::zero_bss;
use crate::regs::{RegisterR, RegisterW}; use vcell::VolatileCell;
use crate::regs::{RegisterR, RegisterW, RegisterRW};
use crate::cortex_a9::{asm, regs::*, mmu}; use crate::cortex_a9::{asm, regs::*, mmu};
use crate::zynq::mpcore; use crate::zynq::{slcr, mpcore};
extern "C" { extern "C" {
static mut __bss_start: u32; static mut __bss_start: u32;
@ -9,7 +10,7 @@ extern "C" {
static mut __stack_start: u32; static mut __stack_start: u32;
} }
static mut CORE1_STACK: u32 = 0; static mut CORE1_STACK: VolatileCell<u32> = VolatileCell::new(0);
#[link_section = ".text.boot"] #[link_section = ".text.boot"]
#[no_mangle] #[no_mangle]
@ -23,15 +24,11 @@ pub unsafe extern "C" fn _boot_cores() -> ! {
boot_core0(); boot_core0();
} }
1 => { 1 => {
// Wait for a first `sev` so that `CORE1_STACK` is cleared while CORE1_STACK.get() == 0 {
// by `zero_bss()` on core 0.
asm::wfe();
while CORE1_STACK == 0 {
asm::wfe(); asm::wfe();
} }
SP.write(CORE1_STACK); SP.write(CORE1_STACK.get());
boot_core1(); boot_core1();
} }
_ => unreachable!(), _ => unreachable!(),
@ -102,13 +99,33 @@ fn l1_cache_init() {
dciall(); dciall();
} }
pub fn start_core1<T: AsMut<[u32]>>(mut stack: T) { pub struct Core1;
let stack = stack.as_mut();
let stack_start = &mut stack[stack.len() - 1]; impl Core1 {
unsafe { pub fn stop() {
CORE1_STACK = stack_start as *mut _ as u32; slcr::RegisterBlock::unlocked(|slcr| {
slcr.a9_cpu_rst_ctrl.modify(|_, w| w.a9_rst1(true));
slcr.a9_cpu_rst_ctrl.modify(|_, w| w.a9_clkstop1(true));
slcr.a9_cpu_rst_ctrl.modify(|_, w| w.a9_rst1(false));
});
} }
// wake up core1 pub fn start<T: AsMut<[u32]>>(mut stack: T) {
asm::sev(); // reset and stop (safe to repeat)
Self::stop();
let stack = stack.as_mut();
let stack_start = &mut stack[stack.len() - 1];
unsafe {
CORE1_STACK.set(stack_start as *mut _ as u32);
}
// Ensure stack pointer has been written
asm::dmb();
// wake up core1
slcr::RegisterBlock::unlocked(|slcr| {
slcr.a9_cpu_rst_ctrl.modify(|_, w| w.a9_rst1(false));
slcr.a9_cpu_rst_ctrl.modify(|_, w| w.a9_clkstop1(false));
});
}
} }

View File

@ -38,7 +38,7 @@ pub fn main() {
let core1_stack = vec![0; 2048]; let core1_stack = vec![0; 2048];
println!("{} bytes stack for core1", core1_stack.len()); println!("{} bytes stack for core1", core1_stack.len());
boot::start_core1(core1_stack); boot::Core1::start(core1_stack);
let eth = zynq::eth::Eth::default(HWADDR.clone()); let eth = zynq::eth::Eth::default(HWADDR.clone());
println!("Eth on"); println!("Eth on");

View File

@ -129,7 +129,7 @@ pub struct RegisterBlock {
pub ocm_rst_ctrl: RW<u32>, pub ocm_rst_ctrl: RW<u32>,
reserved4: [u32; 1], reserved4: [u32; 1],
pub fpga_rst_ctrl: RW<u32>, pub fpga_rst_ctrl: RW<u32>,
pub a9_cpu_rst_ctrl: RW<u32>, pub a9_cpu_rst_ctrl: A9CpuRstCtrl,
reserved5: [u32; 1], reserved5: [u32; 1],
pub rs_awdt_ctrl: RW<u32>, pub rs_awdt_ctrl: RW<u32>,
reserved6: [u32; 2], reserved6: [u32; 2],
@ -440,6 +440,13 @@ impl UartRstCtrl {
} }
} }
register!(a9_cpu_rst_ctrl, A9CpuRstCtrl, RW, u32);
register_bit!(a9_cpu_rst_ctrl, peri_rst, 8);
register_bit!(a9_cpu_rst_ctrl, a9_clkstop1, 5);
register_bit!(a9_cpu_rst_ctrl, a9_clkstop0, 4);
register_bit!(a9_cpu_rst_ctrl, a9_rst1, 1);
register_bit!(a9_cpu_rst_ctrl, a9_rst0, 0);
register!(pss_rst_ctrl, PssRstCtrl, RW, u32); register!(pss_rst_ctrl, PssRstCtrl, RW, u32);
register_bit!(pss_rst_ctrl, soft_rst, 1); register_bit!(pss_rst_ctrl, soft_rst, 1);