Merge branch 'feat/thread' into feat/smp

Makefile

@@ -34,6 +34,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 \
@@ -68,7 +69,7 @@ endif
 all: kernel initrd
 
 kernel:
-	cd kernel && cargo build $(CARGO_BUILD_OPTS)
+	cd kernel && ARCH=$(ARCH) MACH=$(MACH) cargo build $(CARGO_BUILD_OPTS)
 ifeq ($(ARCH),aarch64)
 	$(LLVM_BASE)/llvm-strip -o $(O)/kernel.strip $(O)/kernel
 	$(LLVM_BASE)/llvm-size $(O)/kernel.strip

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/build.rs

@@ -0,0 +1,12 @@
+use std::env;
+
+fn main() -> Result<(), i32> {
+    let arch = env::var("ARCH").expect("$ARCH is not set");
+    let mach = if arch == "aarch64" {
+        env::var("MACH").expect("$MACH is not set")
+    } else {
+        "none".to_owned()
+    };
+    println!("cargo:rerun-if-changed=../etc/{}-{}.ld", arch, mach);
+    Ok(())
+}

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 libsys::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);
 
@@ -84,10 +86,34 @@ extern "C" fn __aa64_bsp_main(fdt_base: usize) -> ! {
         isb(barrier::SY);
     }
 
+    unsafe {
+        cpu::init_self();
+    }
+}
+
+#[no_mangle]
+extern "C" fn __aa64_secondary_main() -> ! {
+    cpu_setup_common();
+
+    unsafe {
+        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,21 @@ 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 {
         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,72 @@
+#![allow(missing_docs)]
+
+use crate::proc::Scheduler;
+use crate::util::InitOnce;
+use core::mem::MaybeUninit;
+use core::ptr::null_mut;
+use core::sync::atomic::{AtomicUsize, Ordering};
+use cortex_a::registers::{MPIDR_EL1, TPIDR_EL1};
+use tock_registers::interfaces::{Readable, Writeable};
+
+#[repr(C)]
+pub struct Cpu {
+    counter: AtomicUsize, // 0x08
+
+    id: usize,
+    scheduler: Scheduler,
+}
+
+impl Cpu {
+    pub fn new(id: usize) -> Self {
+        Self {
+            counter: AtomicUsize::new(0),
+
+            id,
+            scheduler: Scheduler::new(),
+        }
+    }
+
+    pub fn id(&self) -> usize {
+        self.id
+    }
+
+    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(0);
+static mut CPUS: [MaybeUninit<Cpu>; 8] = MaybeUninit::uninit_array();
+
+pub unsafe fn init_self() {
+    let cpu_index = CPU_COUNT.load(Ordering::Acquire);
+    let mpidr_id = (MPIDR_EL1.get() & 0xF) as usize;
+
+    CPUS[cpu_index].write(Cpu::new(mpidr_id));
+    CPUS[cpu_index].assume_init_mut().set();
+
+    CPU_COUNT.store(cpu_index + 1, Ordering::Release);
+}

kernel/src/arch/aarch64/exception.rs

@@ -90,7 +90,8 @@ extern "C" fn __aa64_exc_sync_handler(exc: &mut ExceptionFrame) {
             let far = FAR_EL1.get() as usize;
             let iss = esr & 0x1FFFFFF;
 
-            if iss & (1 << 6) != 0 && far < mem::KERNEL_OFFSET && sched::is_ready() {
+            // TODO handle scenarios when sheduler is not yet initialized
+            if iss & (1 << 6) != 0 && far < mem::KERNEL_OFFSET {
                 let thread = Thread::current();
                 let proc = thread.owner().unwrap();
 
@@ -147,14 +148,14 @@ extern "C" fn __aa64_exc_sync_handler(exc: &mut ExceptionFrame) {
         _ => {}
     }
 
-    if sched::is_ready() {
-        let thread = Thread::current();
-        errorln!(
-            "Unhandled exception in thread {}, {:?}",
-            thread.id(),
-            thread.owner().map(|e| e.id())
-        );
-    }
+    // if sched::is_ready() {
+    //     let thread = Thread::current();
+    //     errorln!(
+    //         "Unhandled exception in thread {}, {:?}",
+    //         thread.id(),
+    //         thread.owner().map(|e| e.id())
+    //     );
+    // }
 
     errorln!(
         "Unhandled exception at ELR={:#018x}, ESR={:#010x}",

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

@@ -1,7 +1,7 @@
 use crate::mem::virt::DeviceMemoryIo;
 use crate::sync::IrqSafeSpinLock;
 use tock_registers::interfaces::{Readable, Writeable};
-use tock_registers::registers::{ReadOnly, ReadWrite};
+use tock_registers::registers::{ReadOnly, WriteOnly, ReadWrite};
 use tock_registers::{register_bitfields, register_structs};
 
 register_bitfields! {
@@ -31,7 +31,9 @@ register_structs! {
         (0x820 => ITARGETSR: [ReadWrite<u32, ITARGETSR::Register>; 248]),
         (0xC00 => _res2),
         (0xC08 => ICFGR: [ReadWrite<u32>; 62]),
-        (0xC0C => @END),
+        (0xD00 => _res3),
+        (0xF00 => SGIR: WriteOnly<u32>),
+        (0xF04 => @END),
     }
 }
 
@@ -103,6 +105,14 @@ impl Gicd {
         }
     }
 
+    pub fn set_sgir(&self, filter: bool, mask: u32, intid: u32) {
+        let mut value = (mask << 16) | intid;
+        if filter {
+            value |= 1 << 24;
+        }
+        self.shared_regs.lock().SGIR.set(value);
+    }
+
     pub fn enable_irq(&self, irq: super::IrqNumber) {
         let irq = irq.get();
 

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

@@ -1,7 +1,7 @@
 //! ARM Generic Interrupt Controller
 
 use crate::dev::{
-    irq::{IntController, IntSource, IrqContext},
+    irq::{IntController, IntSource, IrqContext, IpiSender},
     Device,
 };
 use crate::mem::virt::{DeviceMemory, DeviceMemoryIo};
@@ -17,6 +17,8 @@ use gicd::Gicd;
 /// Maximum available IRQ number
 pub const MAX_IRQ: usize = 300;
 
+const SGI_IRQ: u32 = 2;
+
 /// Range-checked IRQ number type
 #[repr(transparent)]
 #[derive(Copy, Clone)]
@@ -61,11 +63,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(())
     }
 }
@@ -85,9 +88,30 @@ impl IntController for Gic {
             return;
         }
 
+//<<<<<<< HEAD
+        if irq_number == 1 {
+            gicc.clear_irq(irq_number as u32, ic);
+            debugln!("Received IPI");
+            loop {}
+        }
+//
+//        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;
+//        }
+//=======
         gicc.clear_irq(irq_number as u32, ic);
+//>>>>>>> feat/thread
 
         {
+            // 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),
@@ -117,7 +141,18 @@ impl IntController for Gic {
     }
 }
 
+impl IpiSender for Gic {
+    fn send_to_mask(&self, exclude_self: bool, target: u32, data: u64) {
+        self.gicd.get().set_sgir(exclude_self, target, 1);
+    }
+}
+
 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,13 @@ 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
+}
+
+/// Returns CPU's IPI sender device
+#[inline]
+pub fn ipi_sender() -> &'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,123 @@
+#![allow(missing_docs)]
+
+use crate::arch::{aarch64::cpu, machine};
+use crate::dev::{
+    fdt::{self, DeviceTree},
+    irq::IpiSender,
+};
+use crate::mem::{
+    self,
+    phys::{self, PageUsage},
+};
+use cortex_a::registers::MPIDR_EL1;
+use libsys::error::Errno;
+use fdt_rs::prelude::*;
+use tock_registers::interfaces::Readable;
+
+pub type NodeAddress = u32;
+
+#[derive(Clone, Copy, Debug)]
+pub enum PsciError {
+    NotSupported,
+    InvalidParameters,
+    Denied,
+    AlreadyOn,
+    OnPending,
+    InternalFailure,
+    NotPresent,
+    Disabled,
+    InvalidAddress,
+}
+
+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,
+        ))
+    }
+}
+
+const SECONDARY_STACK_PAGES: usize = 4;
+
+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
+}
+
+pub unsafe fn send_ipi(exclude_self: bool, target_mask: u32, data: u64) {
+    machine::ipi_sender().send_to_mask(exclude_self, target_mask, data);
+}
+
+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!(),
+    }
+}
+
+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,9 @@ 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();
+        use crate::proc::{wait, sched};
+        wait::tick();
+        sched::switch(false);
         Ok(())
     }
 

kernel/src/arch/aarch64/vectors.S

@@ -74,8 +74,18 @@ __aa\bits\()_el\el\ht\()_\kind:
 
     EXC_SAVE_STATE
     mov x0, sp
+.if \el == 0
+    sub sp, sp, #16
+    stp fp, lr, [sp, #0]
+.else
     mov lr, xzr
+.endif
     bl __aa64_exc_\kind\()_handler
+
+.if \el == 0
+    add sp, sp, #16
+.endif
+
     EXC_RESTORE_STATE
     eret
 .endm

kernel/src/debug.rs

@@ -12,6 +12,7 @@
 //! * [errorln!]
 
 use crate::dev::serial::SerialDevice;
+use crate::sync::IrqSafeSpinLock;
 use libsys::debug::TraceLevel;
 use core::fmt;
 
@@ -115,9 +116,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

@@ -3,7 +3,9 @@ use crate::debug::Level;
 use fdt_rs::prelude::*;
 use fdt_rs::{
     base::DevTree,
-    index::{DevTreeIndex, DevTreeIndexNode, DevTreeIndexProp},
+    index::{
+        iters::DevTreeIndexCompatibleNodeIter, DevTreeIndex, DevTreeIndexNode, DevTreeIndexProp,
+    },
 };
 use libsys::{error::Errno, path::path_component_left};
 
@@ -44,7 +46,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, "<");
@@ -115,6 +117,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/dev/irq.rs

@@ -1,4 +1,5 @@
 //! Interrupt controller and handler interfaces
+use crate::arch::platform::smp::NodeAddress;
 use crate::dev::Device;
 use core::marker::PhantomData;
 use libsys::error::Errno;
@@ -27,6 +28,12 @@ pub trait IntController: Device {
     fn handle_pending_irqs<'irq_context>(&'irq_context self, ic: &IrqContext<'irq_context>);
 }
 
+/// Inter-processor interrupt delivery method
+pub trait IpiSender: Device {
+    /// Raise an IPI for the target CPU mask, optionally excluding source CPU
+    fn send_to_mask(&self, except_self: bool, target: u32, data: u64);
+}
+
 /// Interface for peripherals capable of emitting IRQs
 pub trait IntSource: Device {
     /// Handles pending IRQs, if any, of this [IntSource].

kernel/src/dev/serial/pl011.rs

@@ -131,7 +131,6 @@ impl IntSource for Pl011 {
     fn handle_irq(&self) -> Result<(), Errno> {
         let inner = self.inner.get().lock();
         inner.regs.ICR.write(ICR::ALL::CLEAR);
-
         let byte = inner.regs.DR.get();
         drop(inner);
 

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]
@@ -44,9 +45,19 @@ pub mod util;
 fn panic_handler(pi: &core::panic::PanicInfo) -> ! {
     unsafe {
         asm!("msr daifset, #2");
+        use crate::arch::platform::cpu::{self, Cpu};
+
+        crate::arch::platform::smp::send_ipi(true, (1 << cpu::count()) - 1, 0);
     }
 
-    errorln!("Panic: {:?}", pi);
+    use cortex_a::registers::MPIDR_EL1;
+    use tock_registers::interfaces::Readable;
+
+    errorln!("Panic on node{}: {:?}", MPIDR_EL1.get() & 0xF, pi);
     // TODO
-    loop {}
+    loop {
+        unsafe {
+            asm!("wfe");
+        }
+    }
 }

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};
 use libsys::proc::Pid;
 
 pub mod elf;
@@ -18,30 +24,91 @@ pub mod wait;
 
 pub mod sched;
 pub use sched::Scheduler;
-pub(self) use sched::SCHED;
+//pub(self) use sched::SCHED;
 
-/// Performs a task switch.
-///
-/// See [Scheduler::switch]
-pub fn switch() {
-    SCHED.switch(false);
+//<<<<<<< HEAD
+// <<<<<<< HEAD
+// 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;
+//             {
+//                 #![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))
+// }
+
+///// 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
+// // =======
+// /// Performs a task switch.
+// ///
+// /// See [Scheduler::switch]
+// pub fn switch() {
+//     SCHED.switch(false);
+// }
+
+// >>>>>>> feat/thread
 pub(self) static PROCESSES: IrqSafeSpinLock<BTreeMap<Pid, ProcessRef>> =
     IrqSafeSpinLock::new(BTreeMap::new());
 
 pub(self) static THREADS: IrqSafeSpinLock<BTreeMap<u32, ThreadRef>> =
     IrqSafeSpinLock::new(BTreeMap::new());
 
-/// Sets up initial process and enters it.
-///
-/// See [Scheduler::enter]
-///
-/// # Safety
-///
-/// Unsafe: May only be called once.
-pub unsafe fn enter() -> ! {
-    SCHED.init();
-    Process::new_kernel(init::init_fn, 0).unwrap().enqueue();
-    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 {
+        Process::new_kernel(init::init_fn, 0).unwrap().enqueue();
+    }
+
+    sched.enter();
 }

kernel/src/proc/process.rs

@@ -6,7 +6,7 @@ use crate::mem::{
     virt::{MapAttributes, Space},
 };
 use crate::proc::{
-    wait::Wait, Context, ProcessIo, Thread, ThreadRef, ThreadState, PROCESSES, SCHED,
+    wait::Wait, Context, ProcessIo, Thread, ThreadRef, ThreadState, PROCESSES, sched,
 };
 use crate::sync::IrqSafeSpinLock;
 use alloc::{rc::Rc, vec::Vec};
@@ -137,7 +137,7 @@ impl Process {
     pub fn enqueue(&self) {
         let inner = self.inner.lock();
         for &tid in inner.threads.iter() {
-            SCHED.enqueue(tid);
+            sched::enqueue(tid);
         }
     }
 
@@ -146,43 +146,114 @@ impl Process {
         PROCESSES.lock().get(&pid).cloned()
     }
 
+// <<<<<<< HEAD
+    // /// Schedules an initial thread for execution
+    // ///
+    // /// # Safety
+    // ///
+    // /// Unsafe: only allowed to be called once, repeated calls
+    // ///         will generate undefined behavior
+    // 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();
+
+    //     // I don't think this is bad: process can't be dropped fully unless
+    //     // it's been reaped (and this function won't run for such process)
+    //     // drop(proc);
+    //     (&mut *ctx).enter()
+    // }
+// =======
     /// Sets a pending signal for a process
     pub fn set_signal(&self, signal: Signal) {
-        let mut lock = self.inner.lock();
-        let ttbr0 = lock.space.as_mut().unwrap().address_phys() | ((lock.id.asid() as usize) << 48);
-        let main_thread = Thread::get(lock.threads[0]).unwrap();
-        drop(lock);
+        todo!();
+        // let mut lock = self.inner.lock();
+        // let ttbr0 = lock.space.as_mut().unwrap().address_phys() | ((lock.id.asid() as usize) << 48);
+        // let main_thread = Thread::get(lock.threads[0]).unwrap();
+        // drop(lock);
 
-        // TODO check that `signal` is not a fault signal
-        //      it is illegal to call this function with
-        //      fault signals
-
-        match main_thread.state() {
-            ThreadState::Running => {
-                main_thread.enter_signal(signal, ttbr0);
-            }
-            ThreadState::Waiting => {
-                main_thread.clone().setup_signal(signal, ttbr0);
-                main_thread.interrupt_wait(true);
-            }
-            ThreadState::Ready => {
-                main_thread.clone().setup_signal(signal, ttbr0);
-                main_thread.interrupt_wait(false);
-            }
-            ThreadState::Finished => {
-                // TODO report error back
-                todo!()
-            }
-        }
+        // // TODO check that `signal` is not a fault signal
+        // //      it is illegal to call this function with
+        // //      fault signals
+        // match main_thread.state() {
+        //     ThreadState::Running => {
+        //         main_thread.enter_signal(signal, ttbr0);
+        //     }
+        //     ThreadState::Waiting => {
+        //         main_thread.clone().setup_signal(signal, ttbr0);
+        //         main_thread.interrupt_wait(true);
+        //     }
+        //     ThreadState::Ready => {
+        //         main_thread.clone().setup_signal(signal, ttbr0);
+        //         main_thread.interrupt_wait(false);
+        //     }
+        //     ThreadState::Finished => {
+        //         // TODO report error back
+        //         todo!()
+        //     }
+        // }
     }
 
     /// Immediately delivers a signal to requested thread
     pub fn enter_fault_signal(&self, thread: ThreadRef, signal: Signal) {
-        let mut lock = self.inner.lock();
-        let ttbr0 = lock.space.as_mut().unwrap().address_phys() | ((lock.id.asid() as usize) << 48);
-        thread.enter_signal(signal, ttbr0);
+        todo!();
+        // let mut lock = self.inner.lock();
+        // let ttbr0 = lock.space.as_mut().unwrap().address_phys() | ((lock.id.asid() as usize) << 48);
+        // thread.enter_signal(signal, ttbr0);
     }
 
+    // /// Schedules a next thread for execution
+    // ///
+    // /// # Safety
+    // ///
+    // /// Unsafe:
+    // ///
+    // /// * Does not ensure src and dst threads are not the same thread
+    // /// * Does not ensure src is actually current context
+    // 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();
+
+    //         if !discard {
+    //             assert_eq!(src_lock.state, State::Running);
+    //             src_lock.state = State::Ready;
+    //         }
+    //         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();
+    //     let dst_ctx = dst.ctx.get();
+
+    //     // See "drop" note in Process::enter()
+    //     // drop(src);
+    //     // drop(dst);
+
+    //     (&mut *src_ctx).switch(&mut *dst_ctx);
+    // }
+
+    // /// Suspends current process with a "waiting" status
+    // pub fn enter_wait(&self) {
+    //     let drop = {
+    //         let mut lock = self.inner.lock();
+    //         let drop = lock.state == State::Running;
+    //         lock.state = State::Waiting;
+    //         sched::dequeue(lock.id);
+    //         // SCHED.dequeue(lock.id);
+    //         drop
+    //     };
+    //     if drop {
+    //         sched::switch(true);
+    //         // todo!();
+    //         // SCHED.switch(true);
+    //     }
+    // }
+
     /// Crates a new thread in the process
     pub fn new_user_thread(&self, entry: usize, stack: usize, arg: usize) -> Result<u32, Errno> {
         let mut lock = self.inner.lock();
@@ -193,11 +264,32 @@ impl Process {
         let thread = Thread::new_user(lock.id, entry, stack, arg, ttbr0)?;
         let tid = thread.id();
         lock.threads.push(tid);
-        SCHED.enqueue(tid);
+        sched::enqueue(tid);
 
         Ok(tid)
     }
 
+    // /// Creates a new kernel process
+    // pub fn new_kernel(entry: extern "C" fn(usize) -> !, arg: usize) -> Result<ProcessRef, Errno> {
+    //     let id = Pid::new_kernel();
+    //     let res = Rc::new(Self {
+    //         ctx: UnsafeCell::new(Context::kernel(entry as usize, arg)),
+    //         io: IrqSafeSpinLock::new(ProcessIo::new()),
+    //         exit_wait: Wait::new(),
+    //         inner: IrqSafeSpinLock::new(ProcessInner {
+    //             id,
+    //             exit: None,
+    //             space: None,
+    //             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());
+    //     Ok(res)
+    // }
+
     /// Creates a "fork" of the process, cloning its address space and
     /// resources
     pub fn fork(&self, frame: &mut ExceptionFrame) -> Result<Pid, Errno> {
@@ -232,7 +324,8 @@ impl Process {
         debugln!("Process {:?} forked into {:?}", src_inner.id, dst_id);
         assert!(PROCESSES.lock().insert(dst_id, dst).is_none());
 
-        SCHED.enqueue(tid);
+        sched::enqueue(tid);
+        // SCHED.enqueue(dst_id);
 
         Ok(dst_id)
     }
@@ -250,7 +343,8 @@ impl Process {
 
         for &tid in lock.threads.iter() {
             Thread::get(tid).unwrap().terminate(status);
-            SCHED.dequeue(tid);
+            sched::dequeue(tid);
+            // SCHED.dequeue(tid);
         }
 
         if let Some(space) = lock.space.take() {
@@ -269,7 +363,7 @@ impl Process {
         self.exit_wait.wakeup_all();
 
         if is_running {
-            SCHED.switch(true);
+            sched::switch(true);
             panic!("This code should never run");
         }
     }
@@ -293,7 +387,8 @@ impl Process {
             lock.threads.retain(|&e| e != tid);
 
             thread.terminate(status);
-            SCHED.dequeue(tid);
+            todo!();
+            // SCHED.dequeue(tid);
             debugln!("Thread {} terminated", tid);
 
             switch
@@ -302,7 +397,8 @@ impl Process {
         if switch {
             // TODO retain thread ID in process "finished" list and
             //      drop it when process finishes
-            SCHED.switch(true);
+            // SCHED.switch(true);
+            todo!();
             panic!("This code should not run");
         } else {
             // Can drop this thread: it's not running
@@ -428,21 +524,50 @@ impl Process {
         Ok(base + offset)
     }
 
-    /// Loads a new program into current process address space
-    pub fn execve<F: FnOnce(&mut Space) -> Result<usize, Errno>>(
-        loader: F,
-        argv: &[&str],
-    ) -> Result<(), Errno> {
-        unsafe {
-            // Run with interrupts disabled
-            asm!("msr daifset, #2");
-        }
+   /// Loads a new program into current process address space
+   pub fn execve<F: FnOnce(&mut Space) -> Result<usize, Errno>>(
+       loader: F,
+       argv: &[&str],
+   ) -> Result<(), Errno> {
+       unsafe {
+           // Run with interrupts disabled
+           asm!("msr daifset, #2");
+       }
 
+// <<<<<<< HEAD
+//         let proc = sched::current_process();
+//         let mut lock = proc.inner.lock();
+//         if lock.id.is_kernel() {
+//             let mut proc_lock = PROCESSES.lock();
+//             let old_pid = lock.id;
+//             assert!(
+//                 proc_lock.remove(&old_pid).is_some(),
+//                 "Failed to downgrade kernel process (remove kernel pid)"
+//             );
+//             lock.id = Pid::new_user();
+//             debugln!(
+//                 "Process downgrades from kernel to user: {} -> {}",
+//                 old_pid,
+//                 lock.id
+//             );
+//             assert!(proc_lock.insert(lock.id, proc.clone()).is_none());
+//             unsafe {
+//                 use crate::arch::platform::cpu::Cpu;
+//                 Cpu::get().scheduler().hack_current_pid(lock.id);
+//             }
+//         } else {
+//             // Invalidate user ASID
+//             let input = (lock.id.asid() as usize) << 48;
+//             unsafe {
+//                 asm!("tlbi aside1, {}", in(reg) input);
+//             }
+// =======
         let proc = Process::current();
         let mut process_lock = proc.inner.lock();
 
         if process_lock.threads.len() != 1 {
             todo!();
+// >>>>>>> feat/thread
         }
 
         let thread = Thread::get(process_lock.threads[0]).unwrap();

kernel/src/proc/sched.rs

@@ -1,8 +1,12 @@
 //!
 use crate::proc::{Thread, ThreadRef, THREADS};
-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<u32>,
@@ -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
@@ -67,7 +89,7 @@ impl Scheduler {
         };
 
         asm!("msr daifset, #2");
-        Thread::enter(thread)
+        Thread::enter((MPIDR_EL1.get() & 0xF) as u32, thread)
     }
 
     /// This hack is required to be called from execve() when downgrading current
@@ -88,14 +110,20 @@ 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 != 0 {
+//<<<<<<< HEAD
+            if !discard && current != inner.idle.unwrap() {
+//=======
+//            if !discard && current != 0 {
+//>>>>>>> feat/thread
                 // 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() {
@@ -118,8 +146,13 @@ impl Scheduler {
 
         if !Rc::ptr_eq(&from, &to) {
             unsafe {
+//<<<<<<< HEAD
+                drop(sched_lock);
+//                Process::switch((MPIDR_EL1.get() & 0xF) as u32, from, to, discard);
+//=======
                 asm!("msr daifset, #2");
-                Thread::switch(from, to, discard);
+                Thread::switch((MPIDR_EL1.get() & 0xF) as u32, from, to, discard);
+//>>>>>>> feat/thread
             }
         }
     }
@@ -139,10 +172,16 @@ impl Scheduler {
     // }
 }
 
-/// Returns `true` if the scheduler has been initialized
-pub fn is_ready() -> bool {
-    SCHED.inner.is_initialized()
-}
+// <<<<<<< HEAD
+// // pub fn is_ready() -> bool {
+// //     SCHED.inner.is_initialized()
+// // }
+// =======
+// /// Returns `true` if the scheduler has been initialized
+// pub fn is_ready() -> bool {
+//     SCHED.inner.is_initialized()
+// }
+// >>>>>>> feat/thread
 
 #[inline(never)]
 extern "C" fn idle_fn(_a: usize) -> ! {
@@ -151,8 +190,94 @@ extern "C" fn idle_fn(_a: usize) -> ! {
     }
 }
 
+pub fn current_thread() -> ThreadRef {
+    let guard = SCHED_LOCK.lock();
+    unsafe { Cpu::get().scheduler().current_thread() }
+}
+
+/// 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, tid: u32) {
+    todo!()
+    //let _lock = SCHED_LOCK.lock();
+    //debugln!("Queue {} to cpu{}", pid, cpu);
+    //unsafe {
+    //    cpu::by_index(cpu).scheduler().enqueue(pid)
+    //}
+}
+
+///
+pub fn enqueue(tid: u32) {
+    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(tid)
+    }
+}
+
+///
+pub fn enqueue_somewhere_else(ignore: usize, tid: u32, _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(tid)
+        }
+        true
+    }
+}
+
+///
+pub fn dequeue(tid: u32) {
+    // TODO process can be rescheduled to other CPU between scheduler locks
+    let lock = SCHED_LOCK.lock();
+    let cpu_id = Thread::get(tid).unwrap().cpu();
+    unsafe {
+        cpu::by_index(cpu_id as usize).scheduler().dequeue(tid);
+    }
+}
+
+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/thread.rs

@@ -3,7 +3,7 @@
 use crate::arch::aarch64::exception::ExceptionFrame;
 use crate::proc::{
     wait::{Wait, WaitStatus},
-    Process, ProcessRef, SCHED, THREADS,
+    Process, ProcessRef, sched, THREADS,
 };
 use crate::sync::IrqSafeSpinLock;
 use crate::util::InitOnce;
@@ -52,13 +52,16 @@ pub struct Thread {
     pub(super) ctx: UnsafeCell<Context>,
     signal_ctx: UnsafeCell<Context>,
     signal_pending: AtomicU32,
+    cpu: AtomicU32,
 }
 
 impl Thread {
+    const CPU_NONE: u32 = u32::MAX;
+
     /// Returns currently active thread [Rc]-reference
     #[inline]
     pub fn current() -> ThreadRef {
-        SCHED.current_thread()
+        sched::current_thread()
     }
 
     /// Returns a reference to thread `tid`, if it exists
@@ -87,6 +90,7 @@ impl Thread {
         let id = new_tid();
 
         let res = Rc::new(Self {
+            cpu: AtomicU32::new(Self::CPU_NONE),
             ctx: UnsafeCell::new(Context::kernel(entry as usize, arg)),
             signal_ctx: UnsafeCell::new(Context::empty()),
             signal_pending: AtomicU32::new(0),
@@ -118,6 +122,7 @@ impl Thread {
         let id = new_tid();
 
         let res = Rc::new(Self {
+            cpu: AtomicU32::new(Self::CPU_NONE),
             ctx: UnsafeCell::new(Context::user(entry, arg, ttbr0, stack)),
             signal_ctx: UnsafeCell::new(Context::empty()),
             signal_pending: AtomicU32::new(0),
@@ -147,6 +152,7 @@ impl Thread {
         let id = new_tid();
 
         let res = Rc::new(Self {
+            cpu: AtomicU32::new(Self::CPU_NONE),
             ctx: UnsafeCell::new(Context::fork(frame, ttbr0)),
             signal_ctx: UnsafeCell::new(Context::empty()),
             signal_pending: AtomicU32::new(0),
@@ -179,12 +185,18 @@ impl Thread {
     ///
     /// Unsafe: only allowed to be called once, repeated calls
     ///         will generate undefined behavior
-    pub unsafe fn enter(thread: ThreadRef) -> ! {
+    pub unsafe fn enter(cpu: u32, thread: ThreadRef) -> ! {
         // FIXME use some global lock to guarantee atomicity of thread entry?
         thread.inner.lock().state = State::Running;
+        thread.cpu.store(cpu, Ordering::SeqCst);
         thread.current_context().enter()
     }
 
+    ///
+    pub fn cpu(&self) -> u32 {
+        self.cpu.load(Ordering::SeqCst)
+    }
+
     /// Schedules a next thread for execution
     ///
     /// # Safety
@@ -193,7 +205,7 @@ impl Thread {
     ///
     /// * 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: ThreadRef, dst: ThreadRef, discard: bool) {
+    pub unsafe fn switch(cpu: u32, src: ThreadRef, dst: ThreadRef, discard: bool) {
         {
             let mut src_lock = src.inner.lock();
             let mut dst_lock = dst.inner.lock();
@@ -204,6 +216,9 @@ impl Thread {
             }
             // 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.current_context();
@@ -227,11 +242,13 @@ impl Thread {
             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);
+            // SCHED.switch(true);
         }
     }
 

kernel/src/proc/wait.rs

@@ -2,7 +2,7 @@
 
 use crate::arch::machine;
 use crate::dev::timer::TimestampSource;
-use crate::proc::{sched::SCHED, Thread, ThreadRef};
+use crate::proc::{self, sched, Thread, ThreadRef};
 use crate::sync::IrqSafeSpinLock;
 use alloc::collections::LinkedList;
 use core::time::Duration;
@@ -47,7 +47,12 @@ pub fn tick() {
         if time > item.deadline {
             let tid = item.tid;
             cursor.remove_current();
-            SCHED.enqueue(tid);
+            todo!();
+//<<<<<<< HEAD
+//            sched::enqueue(pid);
+//=======
+//            SCHED.enqueue(tid);
+//>>>>>>> feat/thread
         } else {
             cursor.move_next();
         }
@@ -149,7 +154,8 @@ impl Wait {
                 let thread = Thread::get(tid).unwrap();
                 thread.set_wait_status(WaitStatus::Interrupted);
                 if enqueue {
-                    SCHED.enqueue(tid);
+                    sched::enqueue(tid);
+                    // SCHED.enqueue(tid);
                 }
                 break;
             } else {
@@ -178,7 +184,7 @@ impl Wait {
                 drop(tick_lock);
 
                 Thread::get(tid).unwrap().set_wait_status(WaitStatus::Done);
-                SCHED.enqueue(tid);
+                sched::enqueue(tid);
             }
 
             limit -= 1;

kernel/src/sync.rs

@@ -4,12 +4,14 @@ use crate::arch::platform::{irq_mask_save, irq_restore};
 use core::cell::UnsafeCell;
 use core::fmt;
 use core::ops::{Deref, DerefMut};
-use core::sync::atomic::{AtomicBool, Ordering};
+use core::sync::atomic::{AtomicUsize, Ordering};
+use cortex_a::registers::MPIDR_EL1;
+use tock_registers::interfaces::Readable;
 
 /// Lock structure ensuring IRQs are disabled when inner value is accessed
 pub struct IrqSafeSpinLock<T> {
     value: UnsafeCell<T>,
-    state: AtomicBool,
+    state: AtomicUsize,
 }
 
 /// Guard-structure wrapping a reference to value owned by [IrqSafeSpinLock].
@@ -25,28 +27,25 @@ impl<T> IrqSafeSpinLock<T> {
     pub const fn new(value: T) -> Self {
         Self {
             value: UnsafeCell::new(value),
-            state: AtomicBool::new(false),
+            state: AtomicUsize::new(usize::MAX),
         }
     }
 
-    #[inline(always)]
-    fn try_lock(&self) -> Result<bool, bool> {
-        self.state
-            .compare_exchange_weak(false, true, Ordering::Acquire, Ordering::Relaxed)
-    }
-
-    #[inline(always)]
-    unsafe fn force_release(&self) {
-        self.state.store(false, Ordering::Release);
-        cortex_a::asm::sev();
-    }
-
     /// Returns [IrqSafeSpinLockGuard] for this lock
     #[inline]
     pub fn lock(&self) -> IrqSafeSpinLockGuard<T> {
         let irq_state = unsafe { irq_mask_save() };
+        let id = MPIDR_EL1.get() & 0xF;
 
-        while self.try_lock().is_err() {
+        while let Err(e) = self.state.compare_exchange_weak(
+            usize::MAX,
+            id as usize,
+            Ordering::Acquire,
+            Ordering::Relaxed,
+        ) {
+            // if e == id as usize {
+            //     break;
+            // }
             cortex_a::asm::wfe();
         }
 
@@ -55,11 +54,17 @@ impl<T> IrqSafeSpinLock<T> {
             irq_state,
         }
     }
+
+    pub unsafe fn force_release(&self) {
+        self.state.store(usize::MAX, Ordering::Release);
+        cortex_a::asm::sev();
+    }
 }
 
 impl<T> Deref for IrqSafeSpinLockGuard<'_, T> {
     type Target = T;
 
+    #[inline(always)]
     fn deref(&self) -> &Self::Target {
         unsafe { &*self.lock.value.get() }
     }

kernel/src/syscall/mod.rs

@@ -5,7 +5,7 @@ use crate::mem::{virt::MapAttributes, phys::PageUsage};
 use crate::debug::Level;
 use crate::dev::timer::TimestampSource;
 use crate::fs::create_filesystem;
-use crate::proc::{self, elf, wait, Process, ProcessIo, Thread};
+use crate::proc::{self, sched, elf, wait, Process, ProcessIo, Thread};
 use core::mem::size_of;
 use core::ops::DerefMut;
 use core::time::Duration;
@@ -348,7 +348,7 @@ pub fn syscall(num: SystemCall, args: &[usize]) -> Result<usize, Errno> {
             Ok(0)
         }
         SystemCall::Yield => {
-            proc::switch();
+            sched::switch(false);
             Ok(0)
         }
         SystemCall::GetSid => {