Add support for starting cores with PSCI

src/kernel/Cargo.toml

@@ -40,6 +40,7 @@ acpi = "4.1.1"
 arm64 = { package = "aarch64-cpu", version = "9.3.1" }
 registers = { package = "tock-registers", version = "0.8.x" }
 fdt = "0.1.5"
+smccc = "0.1.1"
 
 [dependencies.lazy_static]
 version = "1.0"

src/kernel/src/arch/aarch64/interrupt.rs

@@ -179,13 +179,11 @@ impl Drop for DynamicInterrupt {
     }
 }
 
-pub fn init_interrupts() {
-    // we don't want to use logln since it enables interrupts
-    // in the future we should not use logging until mm us up
-    emerglogln!("[arch::interrupt] initializing interrupts");
-
+pub fn init_interrupts() {    
     let cpu = current_processor();
 
+    emerglogln!("[arch::interrupt] processor {} initializing interrupts", cpu.id);
+
     // initialize interrupt controller
     if cpu.is_bsp() {
         INTERRUPT_CONTROLLER.configure_global();

src/kernel/src/arch/aarch64/mod.rs

@@ -79,9 +79,46 @@ pub fn init<B: BootInfo>(boot_info: &B) {
 }
 
 pub fn init_secondary() {
-    // TODO: Initialize secondary processors:
-    // - set up exception handling
-    // - configure the local CPU interrupt controller interface
+    // initialize exceptions by setting up our exception vectors
+    exception::init();
+
+    // check if SPSel is already set to use SP_EL1
+    let spsel: InMemoryRegister<u64, SPSel::Register> = InMemoryRegister::new(SPSel.get());
+    if spsel.matches_all(SPSel::SP::EL0) {
+        // make it so that we use SP_EL1 in the kernel
+        // when taking an exception.
+        spsel.write(SPSel::SP::ELx);
+        let sp: u64;
+        unsafe {
+            core::arch::asm!(
+                // save the stack pointer from before
+                "mov {0}, sp",
+                // change usage of sp from SP_EL0 to SP_EL1
+                "msr spsel, {1}",
+                // set current stack pointer to previous,
+                // sp is now aliased to SP_EL1
+                "mov sp, {0}",
+                // scrub the value stored in SP_EL0
+                // "msr sp_el0, xzr",
+                out(reg) sp,
+                in(reg) spsel.get(),
+            );
+        }
+
+        // make it so that the boot stack is in higher half memory
+        if !VirtAddr::new(sp).unwrap().is_kernel() {
+            unsafe {
+                // we convert it to higher memory that has r/w permissions
+                let new_sp = PhysAddr::new_unchecked(sp).kernel_vaddr().raw();
+                core::arch::asm!(
+                    "mov sp, {}",
+                    in(reg) new_sp,
+                );
+            }
+        }
+    }
+    // initialize the (local) settings for the interrupt controller
+    init_interrupts();
 }
 
 pub fn start_clock(_statclock_hz: u64, _stat_cb: fn(Nanoseconds)) {
@@ -113,6 +150,6 @@ pub fn debug_shutdown(_code: u32) {
 /// Start up a CPU.
 /// # Safety
 /// The tcb_base and kernel stack must both be valid memory regions for each thing.
-pub unsafe fn poke_cpu(_cpu: u32, _tcb_base: crate::memory::VirtAddr, _kernel_stack: *mut u8) {
-    todo!("start up a cpu")
+pub unsafe fn poke_cpu(cpu: u32, tcb_base: crate::memory::VirtAddr, kernel_stack: *mut u8) {
+    crate::machine::processor::poke_cpu(cpu, tcb_base, kernel_stack);
 }

src/kernel/src/arch/aarch64/processor.rs

@@ -1,13 +1,16 @@
 use alloc::vec::Vec;
+use core::sync::atomic::{AtomicBool, Ordering::SeqCst};
 
-use arm64::registers::TPIDR_EL1;
+use arm64::registers::{TPIDR_EL1, MPIDR_EL1};
+use arm64::asm::{wfe, sev};
 use registers::interfaces::{Readable, Writeable};
 
 use crate::{
-    machine::processor::BootMethod,
+    machine::processor::{BootMethod, BootArgs},
     memory::VirtAddr,
     processor::Processor,
     once::Once,
+    current_processor,
 };
 
 #[allow(unused_imports)] // DEBUG
@@ -49,25 +52,38 @@ pub fn get_topology() -> Vec<(usize, bool)> {
     // using something like information in MPIDR_EL1,
     // Device Tree, or ACPI
 
-    // For now we simply return a single core, the boot core.
-    alloc::vec![(*BOOT_CORE_ID.wait() as usize, false)]
+    // For now we simply return a the ID of this core.
+    alloc::vec![((MPIDR_EL1.get() & 0xff) as usize, true)]
 }
 
 // arch specific implementation of processor specific state
 #[derive(Default, Debug)]
 pub struct ArchProcessor {
     pub boot: BootMethod,
+    pub args: BootArgs,
+    pub mpidr: u64,
+    pub wait_flag: AtomicBool,
 }
 
 pub fn halt_and_wait() {
     /* TODO: spin a bit */
-    /* TODO: actually put the cpu into deeper and deeper sleep */
-    todo!()
+    /* TODO: actually put the cpu into deeper and deeper sleep, see PSCI */
+    let core = current_processor();
+    // set the wait condition
+    core.arch.wait_flag.store(true, SeqCst);
+
+    // wait until someone wakes us up
+    while core.arch.wait_flag.load(SeqCst) {
+        wfe();
+    }
 }
 
 impl Processor {
     pub fn wakeup(&self, _signal: bool) {
-        todo!()
+        // remove the wait condition
+        self.arch.wait_flag.store(false, SeqCst);
+        // wakeup the processor
+        sev();
     }
 }
 

src/kernel/src/machine/arm/common/boot/mod.rs

@@ -0,0 +1,121 @@
+/// The method of starting a CPU on ARM devices is machine specific
+/// and usually implemented by the firmware.
+
+mod psci;
+
+use core::str::FromStr;
+
+use arm64::registers::{PAR_EL1, Readable};
+
+use twizzler_abi::upcall::MemoryAccessKind;
+
+use crate::memory::{VirtAddr, PhysAddr};
+
+/// Possible boot protocols used to start a CPU.
+#[derive(Debug, Default, Copy, Clone, PartialEq)]
+pub enum BootMethod {
+    Psci,
+    SpinTable,
+    #[default]
+    Unknown,
+}
+
+impl BootMethod {
+    fn as_str(&self) -> &'static str {
+        match self {
+            Self::Psci => "psci",
+            Self::SpinTable => "spintable",
+            Self::Unknown => "unknown",
+        }
+    }
+}
+
+impl FromStr for BootMethod {
+    type Err = ();
+
+    // Required method
+    fn from_str(s: &str) -> Result<Self, Self::Err> {
+        match s {
+            "psci" => Ok(BootMethod::Psci),
+            "spin-table" => Ok(BootMethod::SpinTable),
+            _ => Err(())
+        }
+    }
+}
+
+/// The arguments needed to start a CPU.
+#[derive(Debug, Default, Copy, Clone)]
+pub struct BootArgs {
+    /// System-wide ID of this CPU core
+    cpu: u32,
+    /// TCB base use for TLS data
+    tcb_base: u64,
+    /// The stack of this kernel thread
+    kernel_stack: u64,
+    /// The entry point of this CPU core
+    entry: u64,
+    // system register state used to start core
+    mair: u64,
+    ttbr1: u64,
+    ttbr0: u64,
+    tcr: u64,
+    sctlr: u64,
+    spsr: u64,
+    cpacr: u64,
+}
+
+/// Start up a CPU.
+/// # Safety
+/// The tcb_base and kernel stack must both be valid memory regions for each thing.
+pub unsafe fn poke_cpu(cpu: u32, tcb_base: VirtAddr, kernel_stack: *mut u8) {
+    let core = unsafe {
+        crate::processor::get_processor_mut(cpu)
+    };
+
+    match core.arch.boot {
+        BootMethod::Psci => psci::boot_core(core, tcb_base, kernel_stack),
+        _ => unimplemented!("boot method: {}", core.arch.boot.as_str())
+    }
+}
+
+// Translate a virtual to a physical address if it is mapped in with the desired access rights
+fn translate(va: VirtAddr, access: MemoryAccessKind) -> Option<PhysAddr> {
+    if !va.is_kernel() {
+        unimplemented!("address is in user memory: {:?}", va)
+    }
+    unsafe {
+        // AT <operation>, <Xt>
+        // <operation>: <stage><level><r/w>
+        // - S1,E1,R/W (stage 1, EL1, R or Write)
+        // <Xt>: address
+        match access {
+            MemoryAccessKind::Read => core::arch::asm!(
+                "AT S1E1R, {}",
+                in(reg) va.raw(),
+                options(nostack, nomem),
+            ),
+            // given the way address translation works
+            // writeable implies readable ...
+            MemoryAccessKind::Write => core::arch::asm!(
+                "AT S1E1W, {}",
+                in(reg) va.raw(),
+                options(nostack, nomem),
+            ),
+            _ => unimplemented!("translation for {:?}", access)
+        }
+    }
+    // PAR_EL1 holds result of AT instruction
+    // - FST: fault status info
+    // - PA: output address
+    if PAR_EL1.matches_all(PAR_EL1::F::TranslationSuccessfull) {
+        let pa = unsafe { 
+            // PAR_EL1.PA returns bits 47:12
+            let base_phys = PAR_EL1.read(PAR_EL1::PA) << 12;
+            // the lower 12 bit offset resides in the VA
+            let block_offset = va.raw() & 0xFFF;
+            PhysAddr::new_unchecked(base_phys | block_offset)
+        };
+        return Some(pa)
+    }
+    None
+}