From 49901d1b8a626f1b8cb477b2a13588933488d3d3 Mon Sep 17 00:00:00 2001 From: Astro Date: Fri, 15 Nov 2019 23:54:26 +0100 Subject: [PATCH] boot: prepare core1 bootup --- src/boot.rs | 39 ++++++++++++++++++++++++++++++++++++--- src/cortex_a9/asm.rs | 6 ++++++ src/zynq/mpcore.rs | 36 +++++++++++++++++++++++++++++++++--- 3 files changed, 75 insertions(+), 6 deletions(-) diff --git a/src/boot.rs b/src/boot.rs index 8312699..d1e15c7 100644 --- a/src/boot.rs +++ b/src/boot.rs @@ -1,6 +1,7 @@ use r0::zero_bss; use crate::regs::{RegisterR, RegisterW}; use crate::cortex_a9::{asm, regs::*, mmu}; +use crate::zynq::mpcore; extern "C" { static mut __bss_start: u32; @@ -8,6 +9,8 @@ extern "C" { static mut __stack_start: u32; } +static mut CORE1_STACK: u32 = 0; + #[link_section = ".text.boot"] #[no_mangle] #[naked] @@ -19,10 +22,19 @@ pub unsafe extern "C" fn _boot_cores() -> ! { SP.write(&mut __stack_start as *mut _ as u32); boot_core0(); } - _ => loop { - // if not core0, infinitely wait for events + 1 => { + // Wait for a first `sev` so that `CORE1_STACK` is cleared + // by `zero_bss()` on core 0. asm::wfe(); - }, + + while CORE1_STACK == 0 { + asm::wfe(); + } + + SP.write(CORE1_STACK); + boot_core1(); + } + _ => unreachable!(), } } @@ -30,16 +42,37 @@ pub unsafe extern "C" fn _boot_cores() -> ! { #[inline(never)] unsafe fn boot_core0() -> ! { l1_cache_init(); + + let mpcore = mpcore::RegisterBlock::new(); + mpcore.scu_invalidate.invalidate_all_cores(); + zero_bss(&mut __bss_start, &mut __bss_end); let mmu_table = mmu::L1Table::get() .setup_flat_layout(); mmu::with_mmu(mmu_table, || { + mpcore.scu_control.start(); + crate::main(); panic!("return from main"); }); } +#[naked] +#[inline(never)] +unsafe fn boot_core1() -> ! { + l1_cache_init(); + + let mpcore = mpcore::RegisterBlock::new(); + mpcore.scu_invalidate.invalidate_core1(); + + let mmu_table = mmu::L1Table::get(); + mmu::with_mmu(mmu_table, || { + crate::main_core1(); + panic!("return from main_core1"); + }); +} + fn l1_cache_init() { use crate::cortex_a9::cache::*; diff --git a/src/cortex_a9/asm.rs b/src/cortex_a9/asm.rs index 022530e..c7a92b2 100644 --- a/src/cortex_a9/asm.rs +++ b/src/cortex_a9/asm.rs @@ -10,6 +10,12 @@ pub fn wfe() { unsafe { asm!("wfe" :::: "volatile") } } +/// Send Event +#[inline] +pub fn sev() { + unsafe { asm!("sev" :::: "volatile") } +} + /// Data Memory Barrier #[inline] pub fn dmb() { diff --git a/src/zynq/mpcore.rs b/src/zynq/mpcore.rs index 36e503c..bb3b3da 100644 --- a/src/zynq/mpcore.rs +++ b/src/zynq/mpcore.rs @@ -1,14 +1,15 @@ ///! Register definitions for Application Processing Unit (mpcore) -use volatile_register::{RO, RW, WO}; -use crate::{register, register_at, register_bit}; +use volatile_register::{RO, RW}; +use crate::{register, register_at, register_bit, register_bits, + regs::RegisterW, regs::RegisterRW}; #[repr(C)] pub struct RegisterBlock { pub scu_control: ScuControl, pub scu_config: RO, pub scu_cpu_power: RW, - pub scu_invalidate: WO, + pub scu_invalidate: ScuInvalidate, reserved0: [u32; 12], pub filter_start: RW, pub filter_end: RW, @@ -27,3 +28,32 @@ register_bit!(scu_control, scu_speculative_linefill_enable, 3); register_bit!(scu_control, scu_rams_parity_enable, 2); register_bit!(scu_control, address_filtering_enable, 1); register_bit!(scu_control, enable, 0); + +impl ScuControl { + pub fn start(&mut self) { + self.modify(|_, w| w.enable(true)); + } +} + +register!(scu_invalidate, ScuInvalidate, WO, u32); +register_bits!(scu_invalidate, cpu0_ways, u8, 0, 3); +register_bits!(scu_invalidate, cpu1_ways, u8, 4, 7); +register_bits!(scu_invalidate, cpu2_ways, u8, 8, 11); +register_bits!(scu_invalidate, cpu3_ways, u8, 12, 15); + +impl ScuInvalidate { + pub fn invalidate_all_cores(&mut self) { + self.write(ScuInvalidate::zeroed() + .cpu0_ways(0xf) + .cpu1_ways(0xf) + .cpu2_ways(0xf) + .cpu3_ways(0xf) + ); + } + + pub fn invalidate_core1(&mut self) { + self.write(ScuInvalidate::zeroed() + .cpu1_ways(0xf) + ); + } +}