smp

Author: ZR233

Cargo.lock

@@ -15,6 +15,7 @@ smp = ["axplat/smp"]
 
 [dependencies]
 aarch64-cpu = "10"
+aarch64-cpu-ext = "0.1"
 any-uart = "0.2.11"
 axconfig-macros = "0.2"
 axcpu = {workspace = true}

platforms/axplat-aarch64-dyn/Cargo.toml

@@ -1,12 +1,5 @@
-use core::ops::Deref;
-
 use pie_boot::boot_info;
-pub use rdrive::probe_all;
-use rdrive::{
-    Platform, init, probe_pre_kernel,
-    register::{DriverRegister, DriverRegisterSlice},
-    register_append,
-};
+use rdrive::{Platform, init, probe_pre_kernel};
 
 pub fn setup() {
     let fdt = boot_info().fdt.expect("FDT must be present");
@@ -15,22 +8,3 @@ pub fn setup() {
 
     probe_pre_kernel().unwrap();
 }
-
-fn driver_registers() -> impl Deref<Target = [DriverRegister]> {
-    unsafe extern "C" {
-        fn __sdriver_register();
-        fn __edriver_register();
-    }
-
-    unsafe {
-        let len = __edriver_register as usize - __sdriver_register as usize;
-
-        if len == 0 {
-            return DriverRegisterSlice::empty();
-        }
-
-        let data = core::slice::from_raw_parts(__sdriver_register as _, len);
-
-        DriverRegisterSlice::from_raw(data)
-    }
-}

platforms/axplat-aarch64-dyn/src/cache.rs

@@ -1,6 +1,7 @@
+use aarch64_cpu_ext::cache::{CacheOp, dcache_all};
 use axplat::init::InitIf;
 
-use crate::{cache, console, driver};
+use crate::{console, driver};
 
 struct InitIfImpl;
 
@@ -30,7 +31,7 @@ impl InitIf for InitIfImpl {
     /// * Exception & interrupt handlers are set up.
     /// * Early console is initialized.
     /// * Current monotonic time and wall time can be obtained.
-    fn init_early(cpu_id: usize, arg: usize) {
+    fn init_early(_cpu_id: usize, _arg: usize) {
         axcpu::init::init_trap();
         crate::mem::setup();
         console::setup_early();
@@ -40,7 +41,7 @@ impl InitIf for InitIfImpl {
     ///
     /// See [`init_early`] for details.
     #[cfg(feature = "smp")]
-    fn init_early_secondary(cpu_id: usize) {
+    fn init_early_secondary(_cpu_id: usize) {
         axcpu::init::init_trap();
     }
 
@@ -68,8 +69,10 @@ impl InitIf for InitIfImpl {
     /// * Interrupt controller is initialized (if applicable).
     /// * Timer interrupts are enabled (if applicable).
     /// * Other platform devices are initialized.
-    fn init_later(cpu_id: usize, arg: usize) {
-        unsafe { cache::dcache_all(cache::DcacheOp::CleanAndInvalidate) };
+    fn init_later(_cpu_id: usize, _arg: usize) {
+        dcache_all(CacheOp::CleanAndInvalidate);
+        #[cfg(feature = "smp")]
+        crate::smp::init();
         driver::setup();
         #[cfg(feature = "irq")]
         {
@@ -83,8 +86,8 @@ impl InitIf for InitIfImpl {
     ///
     /// See [`init_later`] for details.
     #[cfg(feature = "smp")]
-    fn init_later_secondary(cpu_id: usize) {
-        unsafe { cache::dcache_all(cache::DcacheOp::CleanAndInvalidate) };
+    fn init_later_secondary(_cpu_id: usize) {
+        dcache_all(CacheOp::CleanAndInvalidate);
         #[cfg(feature = "irq")]
         {
             crate::irq::init_current_cpu();

platforms/axplat-aarch64-dyn/src/driver.rs

@@ -85,7 +85,7 @@ pub(crate) fn init_current_cpu() {
     let mut cpu_if = intc.lock().unwrap().cpu_local().unwrap();
     cpu_if.open().unwrap();
     cpu_if.set_eoi_mode(true);
-    CPU_IF.init_once(cpu_if);
+    CPU_IF.call_once(move || cpu_if);
     debug!("GIC initialized for current CPU");
 }
 
@@ -97,11 +97,7 @@ fn current_cpu() -> usize {
     MPIDR_EL1.get() as usize & 0xffffff
 }
 
-pub(crate) fn set_enable(
-    irq_raw: usize,
-    trigger: Option<Trigger>,
-    enabled: bool,
-) {
+pub(crate) fn set_enable(irq_raw: usize, trigger: Option<Trigger>, enabled: bool) {
     debug!(
         "IRQ({:#x}) set enable: {}, {}",
         irq_raw,
@@ -131,7 +127,7 @@ pub(crate) fn set_enable(
             if !is_irq_private(irq_raw) {
                 // For private IRQs, we need to acknowledge the interrupt
                 // controller.
-                intc.set_target_cpu(irq, current_cpu().into());
+                intc.set_target_cpu(irq, current_cpu().into()).unwrap();
             }
 
             if let Some(t) = trigger {

platforms/axplat-aarch64-dyn/src/init.rs

@@ -5,6 +5,10 @@
 extern crate axplat;
 extern crate alloc;
 
+use core::ptr::NonNull;
+
+use axplat::mem::phys_to_virt;
+use fdt_parser::Fdt;
 use pie_boot::BootInfo;
 
 mod console;
@@ -14,8 +18,9 @@ mod init;
 mod irq;
 mod mem;
 mod power;
+#[cfg(feature = "smp")]
+mod smp;
 mod time;
-mod cache;
 
 pub mod config {
     axconfig_macros::include_configs!(path_env = "AX_CONFIG_PATH", fallback = "axconfig.toml");
@@ -25,3 +30,12 @@ pub mod config {
 fn main(args: &BootInfo) -> ! {
     axplat::call_main(0, args.fdt.map(|p| p.as_ptr() as usize).unwrap_or_default());
 }
+
+fn fdt() -> Fdt<'static> {
+    let paddr = pie_boot::boot_info()
+        .fdt
+        .expect("FDT is not available, please check the bootloader configuration");
+    let addr = phys_to_virt((paddr.as_ptr() as usize).into());
+
+    Fdt::from_ptr(NonNull::new(addr.as_mut_ptr()).unwrap()).expect("Failed to parse FDT")
+}

platforms/axplat-aarch64-dyn/src/irq.rs

@@ -20,8 +20,18 @@ impl PowerIf for PowerImpl {
     /// Where `cpu_id` is the logical CPU ID (0, 1, ..., N-1, N is the number of
     /// CPU cores on the platform).
     #[cfg(feature = "smp")]
-    fn cpu_boot(cpu_id: usize, stack_top_paddr: usize) {
-        todo!()
+    fn cpu_boot(cpu_idx: usize, stack_top_paddr: usize) {
+        use aarch64_cpu_ext::cache::{CacheOp, dcache_all};
+        use log::info;
+
+        let cpu_id = crate::smp::cpu_idx_to_id(cpu_idx);
+        let entry = crate::smp::secondary_entry_phys_addr();
+        info!(
+            "booting CPU {cpu_id} with entry {:#x} and stack top {:#x}",
+            entry, stack_top_paddr
+        );
+        dcache_all(CacheOp::CleanAndInvalidate);
+        cpu_on(cpu_id as _, entry.as_usize() as _, stack_top_paddr as _).unwrap();
     }
 
     /// Shutdown the whole system.
@@ -82,15 +92,11 @@ fn probe(fdt: FdtInfo<'_>, _dev: PlatformDevice) -> Result<(), OnProbeError> {
     Ok(())
 }
 
-// fn cpu_on(
-//     cpu_id: CpuId,
-//     entry: usize,
-//     stack_top: PhysAddr,
-// ) -> Result<(), alloc::boxed::Box<dyn Error>> {
-//     let method = *METHOD;
-//     match method {
-//         Method::Smc => psci::cpu_on::<Smc>(cpu_id.raw() as _, entry as _, stack_top.raw() as _)?,
-//         Method::Hvc => psci::cpu_on::<Hvc>(cpu_id.raw() as _, entry as _, stack_top.raw() as _)?,
-//     };
-//     Ok(())
-// }
+fn cpu_on(cpu_id: u64, entry: u64, stack_top: u64) -> Result<(), alloc::boxed::Box<dyn Error>> {
+    let method = METHOD.wait();
+    match method {
+        Method::Smc => psci::cpu_on::<Smc>(cpu_id, entry, stack_top)?,
+        Method::Hvc => psci::cpu_on::<Hvc>(cpu_id, entry, stack_top)?,
+    };
+    Ok(())
+}