merge: smp

Makefile

@@ -31,6 +31,7 @@ else
 ifeq ($(MACH),qemu)
 QEMU_OPTS+=-kernel $(O)/kernel.bin \
 		   -initrd $(O)/initrd.img \
+		   -smp cpus=4 \
 		   -M virt,virtualization=on \
 		   -cpu cortex-a72 \
 		   -m 512 \

init/src/main.rs

@@ -0,0 +1,17 @@
+#![feature(asm)]
+#![no_std]
+#![no_main]
+
+#[macro_use]
+extern crate libusr;
+
+#[no_mangle]
+fn main() -> i32 {
+    loop {
+        trace!("Hello from userspace");
+        for _ in 0..100000 {
+            unsafe { asm!("nop"); }
+        }
+    }
+    123
+}

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

@@ -4,12 +4,15 @@ use crate::arch::{
     aarch64::reg::{CNTKCTL_EL1, CPACR_EL1},
     machine,
 };
-use crate::config::{ConfigKey, CONFIG};
-use crate::dev::{
-    fdt::{find_prop, DeviceTree},
-    irq::IntSource,
-    Device,
+use crate::arch::{
+    aarch64::{
+        cpu,
+        smp,
+    },
 };
+use crate::config::{ConfigKey, CONFIG};
+use crate::dev::fdt::find_prop;
+use crate::dev::{fdt::DeviceTree, irq::IntSource, Device};
 use crate::fs::devfs;
 use error::Errno;
 //use crate::debug::Level;
@@ -20,17 +23,17 @@ use crate::mem::{
 };
 use crate::proc;
 use cortex_a::asm::barrier::{self, dsb, isb};
-use cortex_a::registers::{SCTLR_EL1, VBAR_EL1};
-use tock_registers::interfaces::{ReadWriteable, Writeable};
+use cortex_a::registers::{MPIDR_EL1, SCTLR_EL1, VBAR_EL1};
+use tock_registers::interfaces::{ReadWriteable, Readable, Writeable};
 
-fn init_device_tree(fdt_base_phys: usize) -> Result<(), Errno> {
+fn init_device_tree(fdt_base_phys: usize) -> Result<Option<DeviceTree>, Errno> {
     use fdt_rs::prelude::*;
 
     let fdt = if fdt_base_phys != 0 {
         DeviceTree::from_phys(fdt_base_phys + 0xFFFFFF8000000000)?
     } else {
         warnln!("No FDT present");
-        return Ok(());
+        return Ok(None);
     };
 
     #[cfg(feature = "verbose")]
@@ -56,11 +59,10 @@ fn init_device_tree(fdt_base_phys: usize) -> Result<(), Errno> {
         }
     }
 
-    Ok(())
+    Ok(Some(fdt))
 }
 
-#[no_mangle]
-extern "C" fn __aa64_bsp_main(fdt_base: usize) -> ! {
+fn cpu_setup_common() {
     // Disable FP instruction trapping
     CPACR_EL1.modify(CPACR_EL1::FPEN::TrapNone);
 
@@ -83,11 +85,35 @@ extern "C" fn __aa64_bsp_main(fdt_base: usize) -> ! {
         dsb(barrier::SY);
         isb(barrier::SY);
     }
+}
+
+#[no_mangle]
+extern "C" fn __aa64_secondary_main() -> ! {
+    cpu_setup_common();
+
+    infoln!("cpu{} is online!", MPIDR_EL1.get() & 0xF);
+
+    unsafe {
+        cpu::init_self();
+
+        use crate::dev::irq::IntController;
+        machine::local_timer().enable().unwrap();
+        machine::intc().enable_secondary();
+        machine::intc().enable_irq(machine::IrqNumber::new(30));
+
+        proc::enter(false);
+    }
+}
+
+#[no_mangle]
+extern "C" fn __aa64_bsp_main(fdt_base: usize) -> ! {
+    // Boot CPU is MPDIR_EL1 = 0
+    cpu_setup_common();
 
     // Enable MMU
     virt::enable().expect("Failed to initialize virtual memory");
 
-    init_device_tree(fdt_base).expect("Device tree init failed");
+    let fdt = init_device_tree(fdt_base).expect("Device tree init failed");
 
     // Most basic machine init: initialize proper debug output
     // physical memory
@@ -102,16 +128,23 @@ extern "C" fn __aa64_bsp_main(fdt_base: usize) -> ! {
     }
 
     devfs::init();
-
     machine::init_board().unwrap();
 
+    if let Some(fdt) = &fdt {
+        unsafe {
+            smp::enable_secondary_cpus(fdt);
+        }
+    }
+
     infoln!("Machine init finished");
 
     unsafe {
+        cpu::init_bsp();
+
         machine::local_timer().enable().unwrap();
         machine::local_timer().init_irqs().unwrap();
 
-        proc::enter();
+        proc::enter(true);
     }
 }
 

kernel/src/arch/aarch64/boot/uboot.S

@@ -15,6 +15,22 @@
 _entry:
     mov x8, x0
 
+    // Zero .bss
+    ADR_ABS x0, __bss_start_phys
+    ADR_ABS x1, __bss_end_phys
+
+1:
+    cmp x0, x1
+    beq 2f
+
+    str xzr, [x0], #8
+
+    b 1b
+2:
+    ADR_ABS x9, __aa64_entry_upper
+    ADR_REL x10, __aa64_enter_upper
+
+_entry_common:
     // Test for EL2
     mrs x0, CurrentEL
     lsr x0, x0, #2
@@ -46,32 +62,34 @@ _entry:
     dsb sy
     isb
 
-    // Zero .bss
-    ADR_ABS x0, __bss_start_phys
-    ADR_ABS x1, __bss_end_phys
-1:
-    cmp x0, x1
-    beq 2f
-
-    str xzr, [x0], #8
-
-    b 1b
-2:
-
-    ADR_ABS x9, __aa64_entry_upper
-    b __aa64_enter_upper
+    br x10
 
 .section .text._entry_upper
 __aa64_entry_upper:
     // x0 -- fdt address
 
-    ADR_REL x1, bsp_stack_top
+    ADR_ABS x1, bsp_stack_top
     mov sp, x1
 
     mov lr, xzr
     bl __aa64_bsp_main
     b .
 
+__aa64_entry_upper_secondary:
+    // x0 -- stack
+    mov sp, x0
+    mov lr, xzr
+    bl __aa64_secondary_main
+    b .
+
+.section .text._entry_secondary
+.global _entry_secondary
+_entry_secondary:
+    mov x8, x0
+    ADR_ABS x9, __aa64_entry_upper_secondary
+    ADR_ABS x10, __aa64_enter_upper_secondary
+    b _entry_common
+
 .section .bss
 .p2align 12
 bsp_stack_bottom:

kernel/src/arch/aarch64/boot/upper.S

@@ -61,7 +61,13 @@ __aa64_enter_upper:
 
     cbnz x2, 1b
 
-.init_mmu_regs:
+__aa64_enter_upper_secondary:
+    ADR_ABS x5, KERNEL_TTBR1
+    ADR_ABS x6, KERNEL_OFFSET
+
+    // x5 = KERNEL_TTBR1 physical address
+    sub x5, x5, x6
+
     mov x0, #(MAIR_EL1_Attr0_Normal_Outer_NC | MAIR_EL1_Attr0_Normal_Inner_NC | MAIR_EL1_Attr1_Device | MAIR_EL1_Attr1_Device_nGnRE)
     msr mair_el1, x0
 

kernel/src/arch/aarch64/cpu.rs

@@ -0,0 +1,79 @@
+#![allow(missing_docs)]
+
+use cortex_a::registers::{TPIDR_EL1, MPIDR_EL1};
+use tock_registers::interfaces::{Readable, Writeable};
+use core::ptr::null_mut;
+use core::mem::MaybeUninit;
+use core::sync::atomic::{Ordering, AtomicUsize};
+use crate::proc::{Scheduler, process::Context};
+use crate::util::InitOnce;
+
+#[repr(C)]
+pub struct Cpu {
+    active_context: *mut Context,           // 0x00
+    counter: AtomicUsize,                   // 0x08
+
+    scheduler: Scheduler
+}
+
+impl Cpu {
+    pub fn new() -> Self {
+        Self {
+            active_context: null_mut(),
+            counter: AtomicUsize::new(0),
+
+            scheduler: Scheduler::new()
+        }
+    }
+
+    pub fn tick(&self) {
+        self.counter.fetch_add(1, Ordering::SeqCst);
+        if self.counter.load(Ordering::Acquire) >= 100 {
+            debugln!("{} TICK", MPIDR_EL1.get() & 0xF);
+            self.counter.store(0, Ordering::Release);
+        }
+    }
+
+    pub fn scheduler(&mut self) -> &Scheduler {
+        &self.scheduler
+    }
+
+    pub unsafe fn set(&mut self) {
+        TPIDR_EL1.set(self as *mut _ as u64);
+    }
+
+    pub unsafe fn get() -> &'static mut Self {
+        &mut *(TPIDR_EL1.get() as *mut Self)
+    }
+}
+
+pub unsafe fn cpus() -> impl Iterator<Item = &'static mut Cpu> {
+    CPUS[..CPU_COUNT.load(Ordering::Acquire)].iter_mut().map(|c| c.assume_init_mut())
+}
+
+pub unsafe fn by_index(idx: usize) -> &'static mut Cpu {
+    assert!(idx < CPU_COUNT.load(Ordering::Acquire));
+    CPUS[idx].assume_init_mut()
+}
+
+pub fn count() -> usize {
+    CPU_COUNT.load(Ordering::Acquire)
+}
+
+static CPU_COUNT: AtomicUsize = AtomicUsize::new(1);
+static mut CPUS: [MaybeUninit<Cpu>; 8] = MaybeUninit::uninit_array();
+
+pub unsafe fn init_bsp() {
+    // TODO cpu id different than 0?
+    CPUS[0].write(Cpu::new());
+    CPUS[0].assume_init_mut().set();
+}
+
+pub unsafe fn init_self() {
+    let cpu_index = CPU_COUNT.load(Ordering::Acquire);
+
+    CPUS[cpu_index].write(Cpu::new());
+    CPUS[cpu_index].assume_init_mut().set();
+
+    CPU_COUNT.store(cpu_index + 1, Ordering::Release);
+}

kernel/src/arch/aarch64/exception.rs

@@ -88,8 +88,9 @@ extern "C" fn __aa64_exc_sync_handler(exc: &mut ExceptionFrame) {
     match err_code {
         EC_DATA_ABORT_EL0 | EC_DATA_ABORT_ELX => {
             let far = FAR_EL1.get() as usize;
+            // TODO handle scenarios when sheduler is not yet initialized
 
-            if far < mem::KERNEL_OFFSET && sched::is_ready() {
+            if far < mem::KERNEL_OFFSET {
                 let proc = Process::current();
 
                 if let Err(e) = proc.manipulate_space(|space| space.try_cow_copy(far)) {

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

@@ -62,11 +62,12 @@ impl Device for Gic {
         let gicc = Gicc::new(gicc_mmio);
 
         gicd.enable();
-        gicc.enable();
 
         self.gicd.init(gicd);
         self.gicc.init(gicc);
 
+        self.enable_secondary();
+
         Ok(())
     }
 }
@@ -87,10 +88,19 @@ impl IntController for Gic {
         }
 
         if self.scheduler_irq.0 == irq_number {
+            use crate::proc::sched;
+            use cortex_a::registers::{CNTP_TVAL_EL0, CNTP_CTL_EL0};
+            use tock_registers::interfaces::Writeable;
+            use crate::arch::platform::cpu::Cpu;
             gicc.clear_irq(irq_number as u32, ic);
+            CNTP_TVAL_EL0.set(1000000);
+            CNTP_CTL_EL0.write(CNTP_CTL_EL0::ENABLE::SET);
+            sched::switch(false);
+            return;
         }
 
         {
+            // TODO make timer interrupt a special case and drop table lock
             let table = self.table.lock();
             match table[irq_number] {
                 None => panic!("No handler registered for irq{}", irq_number),
@@ -123,6 +133,11 @@ impl IntController for Gic {
 }
 
 impl Gic {
+    ///
+    pub unsafe fn enable_secondary(&self) {
+        self.gicc.get().enable();
+    }
+
     /// Constructs an instance of GICv2.
     ///
     /// # Safety

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

@@ -72,7 +72,7 @@ pub fn local_timer() -> &'static GenericTimer {
 
 /// Returns CPU's interrupt controller device
 #[inline]
-pub fn intc() -> &'static impl IntController<IrqNumber = IrqNumber> {
+pub fn intc() -> &'static Gic {
     &GIC
 }
 

kernel/src/arch/aarch64/mod.rs

@@ -5,9 +5,11 @@ use tock_registers::interfaces::{Readable, Writeable};
 
 pub mod boot;
 pub mod context;
+pub mod cpu;
 pub mod exception;
 pub mod irq;
 pub mod reg;
+pub mod smp;
 pub mod timer;
 
 cfg_if! {

kernel/src/arch/aarch64/smp.rs

@@ -0,0 +1,118 @@
+#![allow(missing_docs)]
+
+use crate::dev::fdt::{self, DeviceTree};
+use crate::arch::aarch64::cpu;
+use crate::mem::{
+    self,
+    phys::{self, PageUsage},
+};
+use cortex_a::registers::MPIDR_EL1;
+use error::Errno;
+use fdt_rs::prelude::*;
+use tock_registers::interfaces::Readable;
+
+#[derive(Clone, Copy, Debug)]
+pub enum PsciError {
+    NotSupported,
+    InvalidParameters,
+    Denied,
+    AlreadyOn,
+    OnPending,
+    InternalFailure,
+    NotPresent,
+    Disabled,
+    InvalidAddress,
+}
+
+const SECONDARY_STACK_PAGES: usize = 4;
+
+pub fn get_cpu_id() -> usize {
+    (MPIDR_EL1.get() & 0xF) as usize
+}
+
+unsafe fn call_smc(mut x0: usize, x1: usize, x2: usize, x3: usize) -> usize {
+    asm!("smc #0", inout("x0") x0, in("x1") x1, in("x2") x2, in("x3") x3);
+    x0
+}
+
+fn wrap_psci_ok(a: usize) -> Result<(), PsciError> {
+    const NOT_SUPPORTED: isize = -1;
+    const INVALID_PARAMETERS: isize = -2;
+    const DENIED: isize = -3;
+    const ALREADY_ON: isize = -4;
+
+    match a as isize {
+        0 => Ok(()),
+        NOT_SUPPORTED => Err(PsciError::NotSupported),
+        INVALID_PARAMETERS => Err(PsciError::InvalidParameters),
+        DENIED => Err(PsciError::Denied),
+        ALREADY_ON => Err(PsciError::AlreadyOn),
+        _ => unimplemented!(),
+    }
+}
+
+struct Psci {
+    use_smc: bool,
+}
+
+impl Psci {
+    const PSCI_VERSION: usize = 0x84000000;
+    const PSCI_CPU_OFF: usize = 0x84000002;
+    const PSCI_CPU_ON: usize = 0xC4000003;
+
+    pub const fn new() -> Self {
+        Self { use_smc: true }
+    }
+
+    unsafe fn call(&self, x0: usize, x1: usize, x2: usize, x3: usize) -> usize {
+        if self.use_smc {
+            call_smc(x0, x1, x2, x3)
+        } else {
+            todo!()
+        }
+    }
+
+    pub unsafe fn cpu_on(
+        &self,
+        target_cpu: usize,
+        entry_point_address: usize,
+        context_id: usize,
+    ) -> Result<(), PsciError> {
+        wrap_psci_ok(self.call(
+            Self::PSCI_CPU_ON,
+            target_cpu,
+            entry_point_address,
+            context_id,
+        ))
+    }
+}
+
+pub unsafe fn enable_secondary_cpus(dt: &DeviceTree) {
+    extern "C" {
+        fn _entry_secondary();
+    }
+
+    let cpus = dt.node_by_path("/cpus").unwrap();
+    let psci = Psci::new();
+
+    for cpu_node in cpus.children() {
+        let reg = fdt::find_prop(cpu_node, "reg").unwrap().u32(0).unwrap();
+        if reg == 0 {
+            continue;
+        }
+        infoln!("Enabling cpu{}", reg);
+        let stack_pages =
+            phys::alloc_contiguous_pages(PageUsage::Kernel, SECONDARY_STACK_PAGES).unwrap();
+        let count_old = cpu::count();
+        psci.cpu_on(
+            reg as usize,
+            _entry_secondary as usize - mem::KERNEL_OFFSET,
+            mem::virtualize(stack_pages + SECONDARY_STACK_PAGES * mem::PAGE_SIZE),
+        )
+        .unwrap();
+        while cpu::count() == count_old {
+            cortex_a::asm::wfe();
+        }
+        debugln!("Done");
+    }
+}

kernel/src/arch/aarch64/timer.rs

@@ -34,9 +34,15 @@ impl IntSource for GenericTimer {
     fn handle_irq(&self) -> Result<(), Errno> {
         CNTP_TVAL_EL0.set(TIMER_TICK);
         CNTP_CTL_EL0.write(CNTP_CTL_EL0::ENABLE::SET);
-        use crate::proc;
-        proc::wait::tick();
-        proc::switch();
+        todo!();
+//<<<<<<< HEAD
+//        use crate::proc;
+//        proc::wait::tick();
+//        proc::switch();
+//=======
+//        use crate::proc::sched;
+//        sched::switch(false);
+//>>>>>>> 61fa9d1 (feat: dirty smp)
         Ok(())
     }
 

kernel/src/debug.rs

@@ -12,6 +12,7 @@
 //! * [errorln!]
 
 use crate::dev::serial::SerialDevice;
+use crate::sync::IrqSafeSpinLock;
 use core::fmt;
 
 /// Kernel logging levels
@@ -102,9 +103,11 @@ macro_rules! errorln {
 
 #[doc(hidden)]
 pub fn _debug(_level: Level, args: fmt::Arguments) {
+    static LOCK: IrqSafeSpinLock<()> = IrqSafeSpinLock::new(());
     use crate::arch::machine;
     use fmt::Write;
 
+    let _lock = LOCK.lock();
     SerialOutput {
         inner: machine::console(),
     }

kernel/src/dev/fdt.rs

@@ -4,7 +4,9 @@ use error::Errno;
 use fdt_rs::prelude::*;
 use fdt_rs::{
     base::DevTree,
-    index::{DevTreeIndex, DevTreeIndexNode, DevTreeIndexProp},
+    index::{
+        iters::DevTreeIndexCompatibleNodeIter, DevTreeIndex, DevTreeIndexNode, DevTreeIndexProp,
+    },
 };
 use libcommon::path_component_left;
 
@@ -45,7 +47,7 @@ fn dump_node(level: Level, node: &INode, depth: usize) {
         print!(level, "{:?} = ", name);
 
         match name {
-            "compatible" => print!(level, "{:?}", prop.str().unwrap()),
+            "compatible" | "enable-method" => print!(level, "{:?}", prop.str().unwrap()),
             "#address-cells" | "#size-cells" => print!(level, "{}", prop.u32(0).unwrap()),
             "reg" => {
                 print!(level, "<");
@@ -116,6 +118,20 @@ impl DeviceTree {
 
     /// Loads a device tree from physical `base` address and
     /// creates an index for it
+    pub fn compatible<'a, 's>(&'a self, compat: &'s str) -> DevTreeIndexCompatibleNodeIter<'s, 'a, 'a, 'a> {
+        self.index.compatible_nodes(compat)
+    }
+
+    pub fn initrd(&self) -> Option<(usize, usize)> {
+        let chosen = self.node_by_path("/chosen")?;
+        let initrd_start = find_prop(chosen.clone(), "linux,initrd-start")?
+            .u32(0)
+            .ok()?;
+        let initrd_end = find_prop(chosen, "linux,initrd-end")?.u32(0).ok()?;
+
+        Some((initrd_start as usize, initrd_end as usize))
+    }
+
     pub fn from_phys(base: usize) -> Result<DeviceTree, Errno> {
         // TODO virtualize address
         let tree = unsafe { DevTree::from_raw_pointer(base as *const _) }

kernel/src/main.rs

@@ -12,7 +12,8 @@
     panic_info_message,
     alloc_error_handler,
     linked_list_cursors,
-    const_btree_new
+    const_btree_new,
+    maybe_uninit_uninit_array,
 )]
 #![no_std]
 #![no_main]

kernel/src/proc/mod.rs

@@ -2,7 +2,13 @@
 
 use crate::init;
 use crate::sync::IrqSafeSpinLock;
-use alloc::collections::BTreeMap;
+use crate::mem;
+use alloc::{
+    boxed::Box,
+    collections::{BTreeMap},
+};
+use core::sync::atomic::{AtomicUsize, Ordering};
+use crate::arch::platform::cpu::{self, Cpu};
 
 pub mod elf;
 pub mod process;
@@ -14,8 +20,9 @@ pub mod wait;
 
 pub mod sched;
 pub use sched::Scheduler;
-pub(self) use sched::SCHED;
+//pub(self) use sched::SCHED;
 
+// <<<<<<< HEAD
 // macro_rules! spawn {
 //     (fn ($dst_arg:ident : usize) $body:block, $src_arg:expr) => {{
 //         #[inline(never)]
@@ -35,18 +42,38 @@ pub(self) use sched::SCHED;
 //     (fn () $body:block) => (spawn!(fn (_arg: usize) $body, 0usize))
 // }
 
-/// Performs a task switch.
-///
-/// See [Scheduler::switch]
-pub fn switch() {
-    SCHED.switch(false);
-}
+///// Performs a task switch.
+/////
+///// See [Scheduler::switch]
+//pub fn switch() {
+//    SCHED.switch(false);
+//}
 
 ///
 pub fn process(id: Pid) -> ProcessRef {
     PROCESSES.lock().get(&id).unwrap().clone()
 }
 
+macro_rules! spawn {
+    (fn ($dst_arg:ident : usize) $body:block, $src_arg:expr) => {{
+        #[inline(never)]
+        extern "C" fn __inner_func($dst_arg : usize) -> ! {
+            let __res = $body;
+            {
+                todo!();
+                // #![allow(unreachable_code)]
+                // SCHED.current_process().exit(__res);
+                panic!();
+            }
+        }
+
+        let __proc = $crate::proc::Process::new_kernel(__inner_func, $src_arg).unwrap();
+        $crate::proc::sched::enqueue(__proc.id());
+    }};
+
+    (fn () $body:block) => (spawn!(fn (_arg: usize) $body, 0usize))
+}
+
 /// Global list of all processes in the system
 pub(self) static PROCESSES: IrqSafeSpinLock<BTreeMap<Pid, ProcessRef>> =
     IrqSafeSpinLock::new(BTreeMap::new());
@@ -58,8 +85,36 @@ pub(self) static PROCESSES: IrqSafeSpinLock<BTreeMap<Pid, ProcessRef>> =
 /// # Safety
 ///
 /// Unsafe: May only be called once.
-pub unsafe fn enter() -> ! {
-    SCHED.init();
-    SCHED.enqueue(Process::new_kernel(init::init_fn, 0).unwrap().id());
-    SCHED.enter();
+pub unsafe fn enter(is_bsp: bool) -> ! {
+    static COUNTER: AtomicUsize = AtomicUsize::new(0);
+
+    let sched = Cpu::get().scheduler();
+    sched.init();
+
+    COUNTER.fetch_add(1, Ordering::Release);
+    while COUNTER.load(Ordering::Acquire) != cpu::count() {
+        cortex_a::asm::nop();
+    }
+
+    if is_bsp {
+        sched::enqueue(Process::new_kernel(init::init_fn, 0).unwrap().id());
+    } else {
+        spawn!(fn () {
+            loop {}
+        });
+    }
+    // if let Some((start, end)) = initrd {
+    //     let initrd = Box::into_raw(Box::new((mem::virtualize(start), mem::virtualize(end))));
+
+    //     spawn!(fn (initrd_ptr: usize) {
+    //         loop {}
+    //         // debugln!("Running kernel init process");
+
+    //         // let (start, _end) = unsafe { *(initrd_ptr as *const (usize, usize)) };
+    //         // Process::execve(|space| elf::load_elf(space, start as *const u8), 0).unwrap();
+    //         // panic!("This code should not run");
+    //     }, initrd as usize);
+    // }
+
+    sched.enter();
 }

kernel/src/proc/process.rs

@@ -5,7 +5,7 @@ use crate::mem::{
     phys::{self, PageUsage},
     virt::{MapAttributes, Space},
 };
-use crate::proc::{wait::Wait, ProcessIo, PROCESSES, SCHED};
+use crate::proc::{wait::Wait, ProcessIo, PROCESSES, sched};
 use crate::sync::IrqSafeSpinLock;
 use alloc::rc::Rc;
 use core::cell::UnsafeCell;
@@ -57,6 +57,7 @@ pub struct Process {
     exit_wait: Wait,
     /// Process I/O context
     pub io: IrqSafeSpinLock<ProcessIo>,
+    cpu: AtomicU32,
 }
 
 impl From<i32> for ExitCode {
@@ -144,10 +145,11 @@ impl fmt::Display for Pid {
 impl Process {
     const USTACK_VIRT_TOP: usize = 0x100000000;
     const USTACK_PAGES: usize = 4;
+    const CPU_NONE: u32 = u32::MAX;
 
     /// Returns currently executing process
     pub fn current() -> ProcessRef {
-        SCHED.current_process()
+        sched::current_process()
     }
 
     pub fn get(pid: Pid) -> Option<ProcessRef> {
@@ -160,9 +162,10 @@ impl Process {
     ///
     /// Unsafe: only allowed to be called once, repeated calls
     ///         will generate undefined behavior
-    pub unsafe fn enter(proc: ProcessRef) -> ! {
+    pub unsafe fn enter(cpu: u32, proc: ProcessRef) -> ! {
         // FIXME use some global lock to guarantee atomicity of thread entry?
         proc.inner.lock().state = State::Running;
+        proc.cpu.store(cpu, Ordering::SeqCst);
         let ctx = proc.ctx.get();
 
         (&mut *ctx).enter()
@@ -184,7 +187,7 @@ impl Process {
     ///
     /// * Does not ensure src and dst threads are not the same thread
     /// * Does not ensure src is actually current context
-    pub unsafe fn switch(src: ProcessRef, dst: ProcessRef, discard: bool) {
+    pub unsafe fn switch(cpu: u32, src: ProcessRef, dst: ProcessRef, discard: bool) {
         {
             let mut src_lock = src.inner.lock();
             let mut dst_lock = dst.inner.lock();
@@ -195,6 +198,9 @@ impl Process {
             }
             assert!(dst_lock.state == State::Ready || dst_lock.state == State::Waiting);
             dst_lock.state = State::Running;
+
+            src.cpu.store(Self::CPU_NONE, Ordering::SeqCst);
+            dst.cpu.store(cpu, Ordering::SeqCst);
         }
 
         let src_ctx = src.ctx.get();
@@ -209,11 +215,14 @@ impl Process {
             let mut lock = self.inner.lock();
             let drop = lock.state == State::Running;
             lock.state = State::Waiting;
-            SCHED.dequeue(lock.id);
+            sched::dequeue(lock.id);
+            // SCHED.dequeue(lock.id);
             drop
         };
         if drop {
-            SCHED.switch(true);
+            sched::switch(true);
+            // todo!();
+            // SCHED.switch(true);
         }
     }
 
@@ -232,6 +241,11 @@ impl Process {
         self.inner.lock().id
     }
 
+    ///
+    pub fn cpu(&self) -> u32 {
+        self.cpu.load(Ordering::SeqCst)
+    }
+
     /// Creates a new kernel process
     pub fn new_kernel(entry: extern "C" fn(usize) -> !, arg: usize) -> Result<ProcessRef, Errno> {
         let id = Pid::new_kernel();
@@ -246,6 +260,7 @@ impl Process {
                 wait_flag: false,
                 state: State::Ready,
             }),
+            cpu: AtomicU32::new(Self::CPU_NONE)
         });
         debugln!("New kernel process: {}", id);
         assert!(PROCESSES.lock().insert(id, res.clone()).is_none());
@@ -274,10 +289,12 @@ impl Process {
                 state: State::Ready,
                 wait_flag: false,
             }),
+            cpu: AtomicU32::new(Self::CPU_NONE)
         });
         debugln!("Process {} forked into {}", src_inner.id, dst_id);
         assert!(PROCESSES.lock().insert(dst_id, dst).is_none());
-        SCHED.enqueue(dst_id);
+        sched::enqueue(dst_id);
+        // SCHED.enqueue(dst_id);
 
         Ok(dst_id)
     }
@@ -302,12 +319,13 @@ impl Process {
 
             self.io.lock().handle_exit();
 
-            SCHED.dequeue(lock.id);
+            sched::dequeue(lock.id);
+
             drop
         };
         self.exit_wait.wakeup_all();
         if drop {
-            SCHED.switch(true);
+            sched::switch(true);
             panic!("This code should never run");
         }
     }
@@ -351,7 +369,7 @@ impl Process {
             asm!("msr daifset, #2");
         }
 
-        let proc = SCHED.current_process();
+        let proc = sched::current_process();
         let mut lock = proc.inner.lock();
         if lock.id.is_kernel() {
             let mut proc_lock = PROCESSES.lock();
@@ -368,7 +386,8 @@ impl Process {
             );
             assert!(proc_lock.insert(lock.id, proc.clone()).is_none());
             unsafe {
-                SCHED.hack_current_pid(lock.id);
+                use crate::arch::platform::cpu::Cpu;
+                Cpu::get().scheduler().hack_current_pid(lock.id);
             }
         } else {
             // Invalidate user ASID

kernel/src/proc/sched.rs

@@ -1,8 +1,12 @@
 //!
 use crate::proc::{Pid, Process, ProcessRef, PROCESSES};
-use crate::sync::IrqSafeSpinLock;
 use crate::util::InitOnce;
 use alloc::{collections::VecDeque, rc::Rc};
+use crate::sync::{IrqSafeSpinLock, IrqSafeSpinLockGuard};
+use crate::arch::platform::cpu::{self, Cpu};
+use cortex_a::registers::{MPIDR_EL1, DAIF};
+use core::ops::Deref;
+use tock_registers::interfaces::Readable;
 
 struct SchedulerInner {
     queue: VecDeque<Pid>,
@@ -30,6 +34,24 @@ impl SchedulerInner {
 }
 
 impl Scheduler {
+    ///
+    pub const fn new() -> Self {
+        Self {
+            inner: InitOnce::new()
+        }
+    }
+
+    ///
+    pub fn queue_size(&self) -> usize {
+        let lock = self.inner.get().lock();
+        let c = if lock.current.is_some() {
+            1
+        } else {
+            0
+        };
+        lock.queue.len() + c
+    }
+
     /// Initializes inner data structure:
     ///
     /// * idle thread
@@ -66,8 +88,7 @@ impl Scheduler {
             PROCESSES.lock().get(&id).unwrap().clone()
         };
 
-        asm!("msr daifset, #2");
-        Process::enter(thread)
+        Process::enter((MPIDR_EL1.get() & 0xF) as u32, thread)
     }
 
     /// This hack is required to be called from execve() when downgrading current
@@ -82,14 +103,16 @@ impl Scheduler {
 
     /// Switches to the next task scheduled for execution. If there're
     /// none present in the queue, switches to the idle task.
-    pub fn switch(&self, discard: bool) {
+    pub fn switch(&self, discard: bool, sched_lock: IrqSafeSpinLockGuard<()>) {
         let (from, to) = {
             let mut inner = self.inner.get().lock();
             let current = inner.current.unwrap();
 
-            if !discard && current != Pid::IDLE {
+            if !discard && current != inner.idle.unwrap() {
                 // Put the process into the back of the queue
-                inner.queue.push_back(current);
+                if !enqueue_somewhere_else((MPIDR_EL1.get() & 0xF) as usize, current, &sched_lock) {
+                    inner.queue.push_back(current);
+                }
             }
 
             let next = if inner.queue.is_empty() {
@@ -112,8 +135,8 @@ impl Scheduler {
 
         if !Rc::ptr_eq(&from, &to) {
             unsafe {
-                asm!("msr daifset, #2");
-                Process::switch(from, to, discard);
+                drop(sched_lock);
+                Process::switch((MPIDR_EL1.get() & 0xF) as u32, from, to, discard);
             }
         }
     }
@@ -126,9 +149,9 @@ impl Scheduler {
     }
 }
 
-pub fn is_ready() -> bool {
-    SCHED.inner.is_initialized()
-}
+// pub fn is_ready() -> bool {
+//     SCHED.inner.is_initialized()
+// }
 
 #[inline(never)]
 extern "C" fn idle_fn(_a: usize) -> ! {
@@ -137,8 +160,94 @@ extern "C" fn idle_fn(_a: usize) -> ! {
     }
 }
 
+/// Performs a task switch.
+///
+/// See [Scheduler::switch]
+pub fn switch(discard: bool) {
+    assert!(DAIF.matches_all(DAIF::I::SET));
+    let guard = SCHED_LOCK.lock();
+    unsafe { Cpu::get().scheduler().switch(discard, guard); }
+}
+
+///
+pub fn enqueue_to(cpu: usize, pid: Pid) {
+    let _lock = SCHED_LOCK.lock();
+    debugln!("Queue {} to cpu{}", pid, cpu);
+    unsafe {
+        cpu::by_index(cpu).scheduler().enqueue(pid)
+    }
+}
+
+///
+pub fn enqueue(pid: Pid) {
+    let _lock = SCHED_LOCK.lock();
+    let mut min_idx = 0;
+    let mut min_cnt = usize::MAX;
+    for (i, cpu) in unsafe { cpu::cpus() }.enumerate() {
+        let size = cpu.scheduler().queue_size();
+        if size < min_cnt {
+            min_cnt = size;
+            min_idx = i;
+        }
+    }
+
+    // debugln!("Queue {} to cpu{}", pid, min_idx);
+    unsafe {
+        cpu::by_index(min_idx).scheduler().enqueue(pid)
+    }
+}
+
+///
+pub fn enqueue_somewhere_else(ignore: usize, pid: Pid, _guard: &IrqSafeSpinLockGuard<()>) -> bool {
+    let mut min_idx = 0;
+    //let mut min_cnt = usize::MAX;
+    static mut LAST: usize = 0;
+    //for (i, cpu) in unsafe { cpu::cpus() }.enumerate() {
+    //for (i, cpu) in wacky_cpu_iterate() {
+    //    if i == ignore {
+    //        continue;
+    //    }
+    //    let size = cpu.scheduler().queue_size();
+    //    if size < min_cnt {
+    //        min_cnt = size;
+    //        min_idx = i;
+    //    }
+    //}
+    unsafe {
+        LAST = (LAST + 1) % cpu::count();
+        min_idx = LAST;
+    }
+
+    if min_idx == ignore {
+        false
+    } else {
+        unsafe {
+            cpu::by_index(min_idx).scheduler().enqueue(pid)
+        }
+        true
+    }
+}
+
+///
+pub fn dequeue(pid: Pid) {
+    // TODO process can be rescheduled to other CPU between scheduler locks
+    let lock = SCHED_LOCK.lock();
+    let cpu_id = PROCESSES.lock().get(&pid).unwrap().cpu();
+    unsafe {
+        cpu::by_index(cpu_id as usize).scheduler().dequeue(pid);
+    }
+}
+
+///
+pub fn current_process() -> ProcessRef {
+    let _lock = SCHED_LOCK.lock();
+    unsafe { Cpu::get().scheduler().current_process() }
+}
+
+static SCHED_LOCK: IrqSafeSpinLock<()> = IrqSafeSpinLock::new(());
+
 // TODO maybe move this into a per-CPU struct
-/// Global scheduler struct
-pub static SCHED: Scheduler = Scheduler {
-    inner: InitOnce::new(),
-};
+// /// Global scheduler struct
+// pub static SCHED: Scheduler = Scheduler {
+//     inner: InitOnce::new(),
+// };

kernel/src/proc/wait.rs

@@ -2,7 +2,7 @@
 
 use crate::arch::machine;
 use crate::dev::timer::TimestampSource;
-use crate::proc::{self, sched::SCHED, Pid, Process};
+use crate::proc::{self, sched, Pid, Process};
 use crate::sync::IrqSafeSpinLock;
 use alloc::collections::LinkedList;
 use core::time::Duration;
@@ -31,7 +31,7 @@ pub fn tick() {
         if time > item.deadline {
             let pid = item.pid;
             cursor.remove_current();
-            SCHED.enqueue(pid);
+            sched::enqueue(pid);
         } else {
             cursor.move_next();
         }
@@ -82,7 +82,7 @@ impl Wait {
                 drop(tick_lock);
 
                 proc::process(pid).set_wait_flag(false);
-                SCHED.enqueue(pid);
+                sched::enqueue(pid);
             }
 
             limit -= 1;

kernel/src/sync.rs

@@ -1,5 +1,7 @@
 //! Synchronization facilities module
 
+#![allow(missing_docs)]
+
 use crate::arch::platform::{irq_mask_save, irq_restore};
 use core::cell::UnsafeCell;
 use core::fmt;
@@ -60,6 +62,7 @@ impl<T> IrqSafeSpinLock<T> {
 impl<T> Deref for IrqSafeSpinLockGuard<'_, T> {
     type Target = T;
 
+    #[inline(always)]
     fn deref(&self) -> &Self::Target {
         unsafe { &*self.lock.value.get() }
     }