AA

kernel/src/arch/cpu.rs

@@ -1,4 +1,4 @@
-use crate::arch::{intrin, smp};
+use crate::arch::intrin;
 use core::mem::MaybeUninit;
 use core::ops::{Deref, DerefMut};
 use core::sync::atomic::AtomicUsize;
@@ -10,17 +10,18 @@ pub struct Cpu {
 
 pub struct CpuRef {
     inner: &'static mut Cpu,
-    irq_state: usize
 }
 
 impl Cpu {
     fn new(index: u32) -> Self {
-        Self { index }
+        Self {
+            index,
+        }
     }
 
     #[inline(always)]
     pub unsafe fn get_raw() -> &'static mut Cpu {
-        &mut *(intrin::read_tpidr_el1() as *mut _)
+        CPU.assume_init_mut()
     }
 
     #[inline(always)]
@@ -30,19 +31,13 @@ impl Cpu {
 
     #[inline]
     pub fn get() -> CpuRef {
-        let irq_state = unsafe { intrin::save_irq() };
         CpuRef {
             inner: unsafe { Self::get_raw() },
-            irq_state
         }
     }
-}
 
-impl Drop for CpuRef {
-    fn drop(&mut self) {
-        unsafe {
-            intrin::restore_irq(self.irq_state);
-        }
+    pub unsafe fn init(index: u32) {
+        CPU.write(Self::new(index));
     }
 }
 
@@ -60,14 +55,7 @@ impl DerefMut for CpuRef {
     }
 }
 
-pub static mut CPUS: [MaybeUninit<Cpu>; smp::MAX_CPU] = MaybeUninit::uninit_array();
-pub static CPU_COUNT: AtomicUsize = AtomicUsize::new(1);
-
-fn set_cpu(cpu: *mut Cpu) {
-    unsafe {
-        intrin::write_tpidr_el1(cpu as usize);
-    }
-}
+static mut CPU: MaybeUninit<Cpu> = MaybeUninit::uninit();
 
 #[inline(always)]
 pub fn get_phys_id() -> usize {
@@ -76,7 +64,6 @@ pub fn get_phys_id() -> usize {
 
 pub fn init(index: usize) {
     unsafe {
-        CPUS[index].write(Cpu::new(index as u32));
-        set_cpu(CPUS[index].as_mut_ptr());
+        Cpu::init(index as u32);
     }
 }

kernel/src/arch/exception.rs

@@ -28,9 +28,20 @@ struct ExceptionContext {
     far: usize,
 }
 
+impl ExceptionContext {
+    pub fn dump(&self) {
+        debugln!(" x0 = {:#018x},  x1 = {:#018x}", self.x0, self.x1);
+        debugln!(" x2 = {:#018x},  x3 = {:#018x}", self.x2, self.x3);
+        debugln!(" x4 = {:#018x},  x5 = {:#018x}", self.x4, self.x5);
+        debugln!(" x6 = {:#018x},  x7 = {:#018x}", self.x6, self.x7);
+        debugln!(" x8 = {:#018x},  x9 = {:#018x}", self.x8, self.x9);
+    }
+}
+
 #[no_mangle]
-extern "C" fn exc_handler(_context: ExceptionContext) -> ! {
-    debug!("Unhandled exception\n");
+extern "C" fn exc_handler(ctx: ExceptionContext) -> ! {
+    debugln!("Unhandled exception");
+    debugln!("FAR = {:#018x}, ESR = {:#018x}", ctx.far, ctx.esr);
 
     loop {}
 }

kernel/src/arch/intrin.rs

@@ -1,3 +1,10 @@
+pub fn delay(mut p: usize) {
+    while p != 0 {
+        nop();
+        p -= 1;
+    }
+}
+
 #[inline(always)]
 pub unsafe fn disable_irq() {
     llvm_asm!("msr daifset, #0xF");

kernel/src/arch/mach_bcm283x/mbox.rs

@@ -1,8 +1,5 @@
 use crate::{
-    arch::{
-        cpu, intrin, mmio_read, mmio_write,
-        smp::{self, IpiDelivery, IpiMessage},
-    },
+    arch::{cpu, intrin, mmio_read, mmio_write},
     KernelSpace,
 };
 use address::{PhysicalAddress, VirtualAddress};
@@ -31,59 +28,59 @@ struct Message {
 }
 
 static mut MESSAGE: Message = Message { data: [0; 36] };
-pub static CORE_MBOX0: CoreMailbox = CoreMailbox { index: 0 };
-
-pub struct CoreMailbox {
-    index: usize,
-}
-
-impl IpiDelivery for CoreMailbox {
-    fn enable(&self) {
-        let phys_core_id = cpu::get_phys_id();
-        unsafe {
-            mmio_write(Self::REG_INTC + phys_core_id * 4, 1 << self.index);
-        }
-    }
-
-    fn send_ipi(target_id: u32, message: IpiMessage) {
-        unsafe {
-            mmio_write(
-                Self::REG_SET + target_id as usize * 16,
-                1 << (message as u32),
-            );
-        }
-    }
-}
-
-impl CoreMailbox {
-    const REG_INTC: PhysicalAddress = PhysicalAddress::new(0x40000050);
-    const REG_SET: PhysicalAddress = PhysicalAddress::new(0x40000080);
-    const REG_RDCLR: PhysicalAddress = PhysicalAddress::new(0x400000C0);
-
-    pub fn do_irq(&self) {
-        let phys_core_id = cpu::get_phys_id();
-        let value: u32 = unsafe { mmio_read(Self::REG_RDCLR + phys_core_id * 16 + self.index * 4) };
-        if value != 0 {
-            macro_rules! test_ipi {
-                ($value:expr, $msg:expr) => {
-                    if $value & (1 << ($msg as u32)) != 0 {
-                        smp::handle_ipi($msg);
-                    }
-                };
-            }
-
-            test_ipi!(value, IpiMessage::Halt);
-            test_ipi!(value, IpiMessage::Tick);
-
-            unsafe {
-                mmio_write::<u32>(
-                    Self::REG_RDCLR + phys_core_id * 16 + self.index * 4,
-                    0xFFFFFFFF,
-                );
-            }
-        }
-    }
-}
+//pub static CORE_MBOX0: CoreMailbox = CoreMailbox { index: 0 };
+//
+//pub struct CoreMailbox {
+//    index: usize,
+//}
+//
+//impl IpiDelivery for CoreMailbox {
+//    fn enable(&self) {
+//        let phys_core_id = cpu::get_phys_id();
+//        unsafe {
+//            mmio_write(Self::REG_INTC + phys_core_id * 4, 1 << self.index);
+//        }
+//    }
+//
+//    fn send_ipi(target_id: u32, message: IpiMessage) {
+//        unsafe {
+//            mmio_write(
+//                Self::REG_SET + target_id as usize * 16,
+//                1 << (message as u32),
+//            );
+//        }
+//    }
+//}
+//
+//impl CoreMailbox {
+//    const REG_INTC: PhysicalAddress = PhysicalAddress::new(0x40000050);
+//    const REG_SET: PhysicalAddress = PhysicalAddress::new(0x40000080);
+//    const REG_RDCLR: PhysicalAddress = PhysicalAddress::new(0x400000C0);
+//
+//    pub fn do_irq(&self) {
+//        let phys_core_id = cpu::get_phys_id();
+//        let value: u32 = unsafe { mmio_read(Self::REG_RDCLR + phys_core_id * 16 + self.index * 4) };
+//        if value != 0 {
+//            macro_rules! test_ipi {
+//                ($value:expr, $msg:expr) => {
+//                    if $value & (1 << ($msg as u32)) != 0 {
+//                        smp::handle_ipi($msg);
+//                    }
+//                };
+//            }
+//
+//            test_ipi!(value, IpiMessage::Halt);
+//            test_ipi!(value, IpiMessage::Tick);
+//
+//            unsafe {
+//                mmio_write::<u32>(
+//                    Self::REG_RDCLR + phys_core_id * 16 + self.index * 4,
+//                    0xFFFFFFFF,
+//                );
+//            }
+//        }
+//    }
+//}
 
 unsafe fn call(ch: u32) -> Result<(), ()> {
     let value = (usize::from(PhysicalAddress::from(

kernel/src/arch/mach_bcm283x/mod.rs

@@ -4,7 +4,6 @@ use address::PhysicalAddress;
 pub mod aux;
 pub mod intc;
 pub mod mbox;
-pub mod smp;
 pub mod timer;
 
 pub const IRQ_LOCAL_TIMER: u32 = 1;

kernel/src/arch/mach_bcm283x/smp.rs

@@ -1,42 +0,0 @@
-use crate::{
-    arch::{cpu::CPU_COUNT, intrin},
-    mem::phys::{self, PageUsage},
-    KernelSpace,
-};
-use address::VirtualAddress;
-use core::hint;
-use core::sync::atomic::Ordering;
-
-pub const MAX_CPU: usize = 4;
-
-pub fn cpu_ready(_index: usize) {
-    CPU_COUNT.fetch_add(1, Ordering::SeqCst);
-}
-
-fn wakeup_single_ap() {
-    extern "C" {
-        static mut ap_wakeup_lock: u64;
-        static mut ap_init_value: u64;
-    }
-
-    let stack_bottom_phys = phys::alloc_contiguous_pages(PageUsage::Kernel, 4).unwrap();
-    let stack_bottom = VirtualAddress::<KernelSpace>::from(stack_bottom_phys);
-
-    let old_count = CPU_COUNT.load(Ordering::SeqCst);
-
-    unsafe {
-        core::ptr::write_volatile(&mut ap_init_value, stack_bottom.into());
-        intrin::dsb_sy();
-        core::ptr::write_volatile(&mut ap_wakeup_lock, 0);
-    }
-
-    while CPU_COUNT.load(Ordering::SeqCst) == old_count {
-        hint::spin_loop();
-    }
-}
-
-pub fn wakeup_ap_cpus() {
-    for _ in 1..4 {
-        wakeup_single_ap();
-    }
-}

kernel/src/arch/mod.rs

@@ -2,7 +2,6 @@ use crate::KernelSpace;
 use address::{PhysicalAddress, VirtualAddress};
 
 pub mod cpu;
-pub mod smp;
 pub mod exception;
 pub mod intrin;
 pub mod timer;

kernel/src/arch/smp.rs

@@ -1,99 +0,0 @@
-use crate::{
-    arch::{
-        cpu::{self, Cpu, CPU_COUNT},
-        intrin,
-    },
-    entry_common,
-};
-use core::sync::atomic::Ordering;
-
-pub trait IpiDelivery {
-    fn enable(&self);
-    fn send_ipi(target_id: u32, message: IpiMessage);
-}
-
-cfg_if! {
-    if #[cfg(feature = "mach_rpi3b")] {
-        use super::mach_bcm283x::{mbox, smp as smp_impl};
-
-        pub type IpiDeliveryImpl = mbox::CoreMailbox;
-        use mbox::CORE_MBOX0 as IPI_IMPL;
-    } else {
-        // Dummy SMP implementation
-        mod smp_impl {
-            use super::{IpiDelivery, IpiMessage};
-            pub const MAX_CPU: usize = 1;
-
-            pub struct IpiDeliveryImpl;
-
-            impl IpiDelivery for IpiDeliveryImpl {
-                fn enable(&self) {}
-                fn send_ipi(_target_id: u32, _message: IpiMessage) {}
-            }
-
-            pub(super) fn cpu_ready(_index: usize) {}
-            pub(super) fn wakeup_ap_cpus() {}
-
-            pub static IPI_IMPL: IpiDeliveryImpl = IpiDeliveryImpl;
-        }
-
-        pub use smp_impl::{IpiDeliveryImpl, IPI_IMPL};
-    }
-}
-
-pub use smp_impl::MAX_CPU;
-#[derive(Clone, Copy, PartialEq, Debug)]
-#[repr(u32)]
-pub enum IpiMessage {
-    Halt = 0,
-    Tick = 1,
-}
-
-#[no_mangle]
-extern "C" fn kernel_ap_main() -> ! {
-    let index = cpu::get_phys_id();
-    debugln!("cpu{}: ap wake up", index);
-    cpu::init(index);
-
-    smp_impl::cpu_ready(index);
-
-    entry_common();
-}
-
-pub unsafe fn send_ipi(mask: usize, message: IpiMessage) {
-    let self_index = Cpu::get().index();
-
-    for index in 0..CPU_COUNT.load(Ordering::Relaxed) {
-        if (1 << index) & mask != 0 && self_index != index as u32 {
-            // TODO cpus must be in phys order?
-            IpiDeliveryImpl::send_ipi(index as u32, message);
-        }
-    }
-}
-
-pub fn handle_ipi(message: IpiMessage) {
-    debugln!("cpu{} received ipi: {:?}", Cpu::get().index(), message);
-
-    match message {
-        IpiMessage::Halt => {
-            unsafe {
-                intrin::disable_irq();
-            }
-            loop {
-                unsafe {
-                    intrin::disable_irq();
-                }
-                intrin::nop();
-            }
-        }
-        IpiMessage::Tick => {}
-    }
-}
-
-pub fn wakeup_ap_cpus() {
-    smp_impl::wakeup_ap_cpus();
-}
-
-pub fn init_ipi_delivery() {
-    IPI_IMPL.enable();
-}

kernel/src/arch/timer.rs

@@ -1,5 +1,6 @@
 use crate::{
-    arch::{intrin, machine},
+    arch::{intrin, machine, cpu::Cpu},
+    proc::sched,
     dev::{
         irq::{self, InterruptController, InterruptHandler},
         Device,
@@ -25,6 +26,7 @@ impl Device for ArmTimer {
 impl InterruptHandler for ArmTimer {
     fn do_irq(&self, _irq: u32) {
         unsafe {
+            sched::sched_yield();
             intrin::write_cntp_tval_el0(100000);
         }
     }

kernel/src/arch/vectors.S

@@ -1,3 +1,4 @@
+// vi:ft=asm :
 .include "kernel/src/arch/macros.S"
 
 .macro __exc_save_ctx
@@ -36,28 +37,6 @@
     add sp, sp, #192
 .endm
 
-.macro __callee_save_ctx
-    sub sp, sp, #96
-
-    stp x19, x20, [sp, #0]
-    stp x21, x22, [sp, #16]
-    stp x23, x24, [sp, #32]
-    stp x25, x26, [sp, #48]
-    stp x27, x29, [sp, #64]
-    stp xzr, lr, [sp, #80]
-.endm
-
-.macro __callee_restore_ctx
-    ldp x19, x20, [sp, #0]
-    ldp x21, x22, [sp, #16]
-    ldp x23, x24, [sp, #32]
-    ldp x25, x26, [sp, #48]
-    ldp x27, x29, [sp, #64]
-    ldp xzr, lr, [sp, #80]
-
-    add sp, sp, #96
-.endm
-
 .section .rodata
 .global el1_vectors
 .p2align 7

kernel/src/boot/entry.S

@@ -8,32 +8,9 @@ _entry:
     ands x1, x1, #3
     beq _entry_bsp
 
-1:
-    adr x8, ap_wakeup_lock
-    mov x9, #1
 _ap_loop:
-    // Will acquire exclusive access to [x8]
-    ldaxr x0, [x8]
-    // Will try to write 1 into [x8], failing if
-    // any other PE has acquired exclusive access at this point
-    stxr w1, x9, [x8]
-    // Store failed, jump back
-    cbnz w1, _ap_loop
-    // [x8] data wasn't zero, jump back
-    cbnz x0, _ap_loop
-_ap_wakeup:
-    adr x0, ap_init_value
-
-    mov x1, #KERNEL_OFFSET
-
-    // Kernel stack
-    ldr x10, [x0]
-
-    // Entry
-    adr x11, kernel_ap_main
-    add x11, x11, x1
-
-    b _entry_ap
+1:
+    b 1b
 
 .section .text
 _entry_bsp:
@@ -44,14 +21,21 @@ _entry_bsp:
     // Setup paging tables
     // This is done once for all PEs
     adr x0, kernel_l1
-    mov x1, #PAGE_PRESENT
-    orr x1, x1, #PAGE_ACCESSED
-    orr x1, x1, #PAGE_ISH
+
+    mov x2, #PAGE_PRESENT
+    orr x2, x2, #PAGE_ACCESSED
+    orr x2, x2, #PAGE_ISH
+
+    mov x1, x2
     str x1, [x0]
 
-    orr x1, x1, #1 << 30
+    orr x1, x2, #1 << 30
     str x1, [x0, #8]
 
+    ldr x1, =0x4010000000
+    orr x1, x2, x1
+    str x1, [x0, #16]
+
     // Load BSP stack
     mov x0, #KERNEL_OFFSET
     adr x10, bsp_stack_top

kernel/src/debug.rs

@@ -1,6 +1,9 @@
-use crate::dev::serial::{SerialDevice, SERIAL0};
+use crate::{
+    arch::intrin,
+    dev::serial::{SerialDevice, SERIAL0},
+    sync::Spin as Mutex,
+};
 use core::fmt;
-use spin::Mutex;
 
 struct Debug;
 
@@ -33,5 +36,9 @@ macro_rules! debugln {
 
 pub fn debug_fmt(args: fmt::Arguments<'_>) {
     use fmt::Write;
+    let u = unsafe { intrin::save_irq() };
     write!(DEBUG.lock(), "{}", args).unwrap();
+    unsafe {
+        intrin::restore_irq(u);
+    }
 }

kernel/src/dev/mod.rs

@@ -3,6 +3,8 @@ pub mod serial;
 
 pub mod pl011;
 pub mod pl031;
+pub mod virtio;
+pub mod pcie;
 
 pub trait Device {
     fn name(&self) -> &'static str;

kernel/src/dev/pcie.rs

@@ -0,0 +1,58 @@
+use crate::{arch::mmio_read, dev::Device, mem::EcamSpace};
+use address::{PhysicalAddress, VirtualAddress};
+use core::ops::Index;
+
+pub struct HostPci {
+    base: PhysicalAddress,
+}
+
+pub struct PcieFunctionConfig {
+    base: PhysicalAddress,
+}
+
+impl Device for HostPci {
+    fn name(&self) -> &'static str {
+        "Host PCI(e) Controller"
+    }
+
+    unsafe fn enable(&self) {
+        let func = PcieFunctionConfig { base: self.base };
+        debugln!("{:#010x}", func.readl(0));
+        debugln!(":");
+    }
+
+    unsafe fn disable(&self) {}
+}
+
+impl HostPci {
+    pub const fn new(base: PhysicalAddress) -> Self {
+        Self { base }
+    }
+}
+
+impl PcieFunctionConfig {
+    #[inline(always)]
+    pub unsafe fn readl(&self, off: usize) -> u32 {
+        let addr = VirtualAddress::<EcamSpace>::from(self.base + off);
+        core::ptr::read_volatile(addr.as_ptr())
+    }
+
+    #[inline(always)]
+    pub unsafe fn readw(&self, off: usize) -> u16 {
+        (self.readl(off & !0x3) | (0xFFFF << (off & 0x3))) as u16
+    }
+
+    #[inline(always)]
+    pub fn device_id(&self) -> u16 {
+        unsafe { self.readw(2) }
+    }
+
+    #[inline(always)]
+    pub fn vendor_id(&self) -> u16 {
+        unsafe { self.readw(0) }
+    }
+}
+
+pub const fn func_offset(bus: u8, dev: u8, func: u8) -> usize {
+    ((bus as usize) << 20) | ((dev as usize) << 15) | ((func as usize) << 12)
+}

kernel/src/dev/virtio.rs

@@ -0,0 +1,25 @@
+use crate::dev::Device;
+use address::PhysicalAddress;
+
+pub struct Display {
+    base: PhysicalAddress
+}
+
+impl Device for Display {
+    fn name(&self) -> &'static str {
+        "VirtIO GPU"
+    }
+
+    unsafe fn enable(&self) {
+    }
+
+    unsafe fn disable(&self) {
+
+    }
+}
+
+impl Display {
+    pub const fn new(base: PhysicalAddress) -> Self {
+        Self { base }
+    }
+}

kernel/src/fdt.rs

@@ -9,7 +9,7 @@ use address::{PhysicalAddress, VirtualAddress};
 use core::mem::MaybeUninit;
 use fdt_rs::{
     base::{DevTree, DevTreeProp},
-    index::DevTreeIndex,
+    index::{DevTreeIndex, DevTreeIndexNode, DevTreeIndexProp},
     prelude::*,
 };
 
@@ -17,10 +17,72 @@ use fdt_rs::{
 struct FdtManager {
     fdt: DevTree<'static>,
     index: DevTreeIndex<'static, 'static>,
+    address_cells: u32,
+    size_cells: u32,
+}
+
+impl FdtManager {
+    fn dump(&self) {
+        self.dump_node(&self.index.root(), 0);
+    }
+
+    fn dump_node(&self, node: &DevTreeIndexNode, depth: usize) {
+        for _ in 0..depth {
+            debug!(" ");
+        }
+        debugln!("\"{}\" {{", node.name().unwrap());
+
+        for prop in node.props() {
+            for _ in 0..=depth {
+                debug!(" ");
+            }
+            let name = prop.name().unwrap();
+            debug!("\"{}\" = ", name);
+            match name {
+                "compatible" => debug!("\"{}\"", prop.str().unwrap()),
+                "#size-cells" | "#address-cells" => debug!("{}", prop.u32(0).unwrap()),
+                "reg" => {
+                    debug!("<");
+                    for i in 0..prop.length() / 4 {
+                        debug!("{:#010x}", prop.u32(i).unwrap());
+                        if i != prop.length() / 4 - 1 {
+                            debug!(", ");
+                        }
+                    }
+                    debug!(">");
+                }
+                _ => debug!("..."),
+            }
+            debugln!(";");
+        }
+
+        for child in node.children() {
+            self.dump_node(&child, depth + 1);
+        }
+
+        for _ in 0..depth {
+            debug!(" ");
+        }
+        debugln!("}}");
+    }
 }
 
 static mut FDT_MANAGER: MaybeUninit<FdtManager> = MaybeUninit::uninit();
 
+fn read_cell_index_prop(prop: &DevTreeIndexProp, offset: usize, cells: u32) -> Option<usize> {
+    if cells == 2 {
+        // Read as two u32s
+        let high = prop.u32(offset).ok()? as usize;
+        let low = prop.u32(offset + 1).ok()? as usize;
+
+        Some(low | (high << 32))
+    } else {
+        let val = prop.u32(offset).ok()?;
+
+        Some(val as usize)
+    }
+}
+
 fn read_cell_prop(prop: &DevTreeProp, offset: usize, cells: u32) -> Option<usize> {
     if cells == 2 {
         // Read as two u32s
@@ -47,8 +109,25 @@ pub fn init(fdt_base_phys: PhysicalAddress) {
     let index =
         DevTreeIndex::new(fdt, unsafe { pages_virt.as_slice_mut(page_count * 0x1000) }).unwrap();
 
+    let root = index.root();
+    let mut address_cells = None;
+    let mut size_cells = None;
+    for prop in root.props() {
+        match prop.name().unwrap() {
+            "#address-cells" => address_cells = Some(prop.u32(0).unwrap()),
+            "#size-cells" => size_cells = Some(prop.u32(0).unwrap()),
+            _ => {}
+        }
+    }
+
     unsafe {
-        FDT_MANAGER.write(FdtManager { fdt, index });
+        FDT_MANAGER.write(FdtManager {
+            address_cells: address_cells.unwrap(),
+            size_cells: size_cells.unwrap(),
+            fdt,
+            index,
+        });
+        FDT_MANAGER.assume_init_ref().dump();
     }
 }
 

kernel/src/main.rs

@@ -3,7 +3,8 @@
     llvm_asm,
     const_panic,
     maybe_uninit_uninit_array,
-    alloc_error_handler
+    alloc_error_handler,
+    const_fn_trait_bound
 )]
 #![no_std]
 #![no_main]
@@ -22,24 +23,16 @@ pub mod dev;
 pub mod fdt;
 pub mod mem;
 pub mod time;
+pub mod proc;
+pub mod sync;
 
 pub use mem::KernelSpace;
 
 use address::PhysicalAddress;
-use arch::{timer, cpu, intrin, smp};
+use arch::{timer, cpu, intrin};
 use dev::irq::{self, InterruptController};
-
-pub fn entry_common() -> ! {
-    smp::init_ipi_delivery();
-
-    unsafe {
-        irq::init();
-        timer::enable_local_timer();
-        intrin::enable_irq();
-    }
-
-    loop {}
-}
+use dev::virtio::Display;
+use dev::pcie::HostPci;
 
 #[no_mangle]
 extern "C" fn kernel_bsp_main(fdt_base: PhysicalAddress) -> ! {
@@ -67,8 +60,20 @@ extern "C" fn kernel_bsp_main(fdt_base: PhysicalAddress) -> ! {
     }
 
     debug!("BSP init finished\n");
-    // smp::wakeup_ap_cpus();
-    entry_common();
+    unsafe {
+        irq::init();
+        timer::enable_local_timer();
+    }
+
+    let mut bus = HostPci::new(PhysicalAddress::new(0x10000000));
+    unsafe {
+        use dev::Device;
+        bus.enable();
+    }
+    //let mut display = Display::new();
+
+    loop {}
+    //proc::enter();
 }
 
 use core::panic::PanicInfo;
@@ -78,8 +83,5 @@ fn panic_handler(pi: &PanicInfo) -> ! {
         intrin::disable_irq();
     }
     debug!("PANIC: {:?}\n", pi);
-    unsafe {
-        smp::send_ipi(usize::MAX, smp::IpiMessage::Halt);
-    }
     loop {}
 }

kernel/src/mem/mod.rs

@@ -12,6 +12,15 @@ impl AddressSpace for KernelSpace {
 }
 impl TrivialConvert for KernelSpace {}
 
+#[derive(Copy, Clone, PartialEq, PartialOrd)]
+pub struct EcamSpace;
+impl AddressSpace for EcamSpace {
+    const NAME: &'static str = "ecam";
+    const OFFSET: usize = 0xFFFFFF8080000000;
+    const LIMIT: usize = 0xFFFFFF8080000000 + (1 << 30);
+}
+impl TrivialConvert for EcamSpace {}
+
 pub const PAGE_SIZE: usize = 0x1000;
 
 pub fn kernel_end_phys() -> PhysicalAddress {

kernel/src/mem/phys/manager.rs

@@ -2,8 +2,8 @@ use super::{PageInfo, PageUsage};
 use crate::{mem::PAGE_SIZE, KernelSpace};
 use address::{PhysicalAddress, VirtualAddress};
 use core::mem;
-use error::Errno;
 use spin::Mutex;
+use error::Errno;
 
 pub unsafe trait Manager {
     fn alloc_page(&mut self, pu: PageUsage) -> Result<PhysicalAddress, Errno>;
@@ -18,7 +18,7 @@ pub unsafe trait Manager {
 }
 
 pub struct SimpleManager {
-    pages: &'static mut [Mutex<PageInfo>],
+    pages: &'static mut [PageInfo],
     base_index: usize,
 }
 
@@ -28,17 +28,17 @@ impl SimpleManager {
         at: PhysicalAddress,
         count: usize,
     ) -> Self {
-        let pages: &'static mut [Mutex<PageInfo>] =
+        let pages: &'static mut [PageInfo] =
             VirtualAddress::<KernelSpace>::from(at).as_slice_mut(count);
 
         // Initialize uninit pages
         for index in 0..count {
             mem::forget(mem::replace(
                 &mut pages[index],
-                Mutex::new(PageInfo {
+                PageInfo {
                     refcount: 0,
                     usage: PageUsage::Reserved,
-                }),
+                },
             ));
         }
 
@@ -49,7 +49,7 @@ impl SimpleManager {
     }
 
     pub(super) unsafe fn add_page(&mut self, addr: PhysicalAddress) {
-        let mut page = self.pages[addr.page_index() - self.base_index].lock();
+        let page = &mut self.pages[addr.page_index() - self.base_index];
         assert!(page.refcount == 0 && page.usage == PageUsage::Reserved);
         page.usage = PageUsage::Available;
 
@@ -62,7 +62,7 @@ impl SimpleManager {
 unsafe impl Manager for SimpleManager {
     fn alloc_page(&mut self, pu: PageUsage) -> Result<PhysicalAddress, Errno> {
         for index in 0..self.pages.len() {
-            let mut page = self.pages[index].lock();
+            let page = &mut self.pages[index];
 
             if page.usage == PageUsage::Available {
                 page.usage = pu;
@@ -79,13 +79,13 @@ unsafe impl Manager for SimpleManager {
     ) -> Result<PhysicalAddress, Errno> {
         'l0: for i in 0..self.pages.len() {
             for j in 0..count {
-                if self.pages[i + j].lock().usage != PageUsage::Available {
+                if self.pages[i + j].usage != PageUsage::Available {
                     continue 'l0;
                 }
             }
 
             for j in 0..count {
-                let mut page = self.pages[i + j].lock();
+                let page = &mut self.pages[i + j];
                 assert!(page.usage == PageUsage::Available);
                 page.usage = pu;
                 page.refcount = 1;

kernel/src/mem/phys/mod.rs

@@ -1,4 +1,5 @@
 use super::PAGE_SIZE;
+use core::panic::Location;
 use address::PhysicalAddress;
 use core::convert::TryFrom;
 use core::mem::size_of;

kernel/src/proc/context.S

@@ -0,0 +1,54 @@
+.section .text
+.global context_enter_kernel
+.global context_switch_to
+.global context_switch
+
+.macro __callee_save_ctx
+    sub sp, sp, #96
+
+    stp x19, x20, [sp, #0]
+    stp x21, x22, [sp, #16]
+    stp x23, x24, [sp, #32]
+    stp x25, x26, [sp, #48]
+    stp x27, x29, [sp, #64]
+    stp xzr, lr, [sp, #80]
+.endm
+
+.macro __callee_restore_ctx
+    ldp x19, x20, [sp, #0]
+    ldp x21, x22, [sp, #16]
+    ldp x23, x24, [sp, #32]
+    ldp x25, x26, [sp, #48]
+    ldp x27, x29, [sp, #64]
+    ldp xzr, lr, [sp, #80]
+
+    add sp, sp, #96
+.endm
+
+context_enter_kernel:
+    mov x0, #5
+    msr spsr_el1, x0
+    ldp x0, x1, [sp]
+    msr elr_el1, x1
+    eret
+
+context_switch:
+    msr daifset, #0xF
+
+    // Store old callee-saved regs
+    __callee_save_ctx
+
+    // Store old stack
+    mov x19, sp
+    str x19, [x1]
+context_switch_to:
+    msr daifset, #0xF
+
+    // Load new stack
+    ldr x0, [x0]
+    mov sp, x0
+
+    // Load new callee-saved regs from stack
+    __callee_restore_ctx
+
+    ret

kernel/src/proc/context.rs

@@ -0,0 +1,68 @@
+use crate::{KernelSpace, mem::phys::{self, PageUsage}};
+use address::{VirtualAddress};
+
+global_asm!(include_str!("context.S"));
+
+#[repr(C)]
+pub struct Context {
+    sp: VirtualAddress<KernelSpace>,      // 0x00
+}
+
+struct WriteStack {
+    bp: VirtualAddress<KernelSpace>,
+    sp: VirtualAddress<KernelSpace>
+}
+
+extern "C" {
+    fn context_enter_kernel();
+
+    pub(super) fn context_switch_to(dst: *mut Context);
+    pub(super) fn context_switch(dst: *mut Context, src: *mut Context);
+}
+
+impl Context {
+    pub fn new_kernel(entry: usize, arg: usize) -> Self {
+        let pages = phys::alloc_contiguous_pages(PageUsage::Kernel, 4).unwrap();
+        let stack_bottom = VirtualAddress::<KernelSpace>::from(pages);
+        let mut stack = WriteStack::new(stack_bottom, 4 * 4096);
+
+        stack.push(entry);
+        stack.push(arg);
+
+        stack.push(context_enter_kernel as usize);      // x30 LR
+        stack.push(0);                                  // xzr
+        stack.push(0);                                  // x29
+        stack.push(0);                                  // x27
+        stack.push(0);                                  // x26
+        stack.push(0);                                  // x25
+        stack.push(0);                                  // x24
+        stack.push(0);                                  // x23
+        stack.push(0);                                  // x22
+        stack.push(0);                                  // x21
+        stack.push(0);                                  // x20
+        stack.push(0);                                  // x19
+
+        Context {
+            sp: stack.sp,
+        }
+    }
+}
+
+impl WriteStack {
+    pub fn new(bottom: VirtualAddress<KernelSpace>, size: usize) -> Self {
+        Self {
+            bp: bottom,
+            sp: bottom + size,
+        }
+    }
+
+    pub fn push(&mut self, value: usize) {
+        if self.sp == self.bp {
+            panic!("Stack overflow");
+        }
+        self.sp -= 8;
+        unsafe {
+            core::ptr::write(self.sp.as_mut_ptr(), value);
+        }
+    }
+}

kernel/src/proc/mod.rs

@@ -1,3 +1,72 @@
+use crate::{
+    arch::{cpu::Cpu, intrin},
+};
+use core::mem::MaybeUninit;
+use core::ptr::null_mut;
+use core::sync::atomic::{AtomicU32, Ordering};
+use spin::Mutex;
+
+pub mod context;
 pub mod sched;
 
-pub struct Process {}
+pub use context::Context;
+pub use sched::Scheduler;
+
+#[repr(C)]
+pub struct Process {
+    context: Context,
+
+    id: u32,
+    cpu_id: Option<u32>,
+
+    queue_prev: *mut Process,
+    queue_next: *mut Process,
+}
+
+impl Process {
+    pub fn new_kernel(entry: usize, arg: usize) -> Self {
+        Self {
+            context: Context::new_kernel(entry, arg),
+
+            id: new_pid(),
+            cpu_id: None,
+
+            queue_prev: null_mut(),
+            queue_next: null_mut(),
+        }
+    }
+}
+
+pub fn new_pid() -> u32 {
+    LAST_PID.fetch_add(1, Ordering::SeqCst)
+}
+
+extern "C" fn f0(arg: usize) {
+    loop {
+        debug!("{}", arg);
+    }
+}
+
+static LAST_PID: AtomicU32 = AtomicU32::new(1);
+static mut S: [MaybeUninit<Process>; 2] = MaybeUninit::uninit_array();
+
+pub fn enter() -> ! {
+    unsafe {
+        //let mut cpu = Cpu::get();
+        //cpu.scheduler.init();
+        let mut sched = Scheduler::get();
+
+        S[0].write(Process::new_kernel(f0 as usize, 0));
+        S[1].write(Process::new_kernel(f0 as usize, 1));
+
+        sched.queue(S[0].as_mut_ptr());
+        //cpu.scheduler.queue(S[0].as_mut_ptr());
+        intrin::delay(4000);
+        sched.queue(S[1].as_mut_ptr());
+        //cpu.scheduler.queue(S[1].as_mut_ptr());
+
+        sched.enter();
+        //cpu.scheduler.enter();
+    }
+    loop {}
+}

kernel/src/proc/sched.rs

@@ -1,5 +1,174 @@
-pub struct Queue {}
+use crate::{
+    arch::{cpu, intrin},
+    proc::{
+        context::{context_switch, context_switch_to},
+        Process,
+    },
+    sync::Spin as Mutex
+};
+use core::mem::MaybeUninit;
+use core::ptr::null_mut;
+use core::sync::atomic::{AtomicBool, Ordering};
+
+pub struct Queue {
+    head: *mut Process,
+    current: *mut Process,
+    size: usize,
+}
 
 pub struct Scheduler {
-    queue: Queue,
+    queue: Mutex<Queue>,
+    ready: AtomicBool,
+    idle: MaybeUninit<Process>,
+    cpu_index: usize,
+}
+
+impl Queue {
+    pub const fn new() -> Self {
+        Self {
+            head: null_mut(),
+            current: null_mut(),
+            size: 0,
+        }
+    }
+}
+
+impl Scheduler {
+    pub const fn new() -> Self {
+        Self {
+            queue: Mutex::new(Queue::new()),
+            ready: AtomicBool::new(false),
+            idle: MaybeUninit::uninit(),
+            cpu_index: 0,
+        }
+    }
+
+    pub unsafe fn init(&mut self) {
+        self.idle.write(Process::new_kernel(idle_fn as usize, 0));
+    }
+
+    pub fn is_ready(&self) -> bool {
+        self.ready.load(Ordering::Acquire)
+    }
+
+    pub fn queue(&mut self, proc: *mut Process) {
+        let irq_state = unsafe { intrin::save_irq() };
+        let mut queue = self.queue.lock();
+
+        unsafe {
+            if !queue.head.is_null() {
+                let queue_tail = (*queue.head).queue_prev;
+                (*queue_tail).queue_next = proc;
+                (*proc).queue_prev = queue_tail;
+                (*queue.head).queue_prev = proc;
+                (*proc).queue_next = queue.head;
+            } else {
+                (*proc).queue_prev = proc;
+                (*proc).queue_next = proc;
+                queue.head = proc;
+            }
+        }
+
+        queue.size += 1;
+        unsafe {
+            intrin::restore_irq(irq_state);
+        }
+    }
+
+    pub unsafe fn switch_to(&mut self, dst: *mut Process) {
+        intrin::disable_irq();
+
+        let mut queue = self.queue.lock();
+        let src = queue.current;
+
+        if src == dst {
+            return;
+        }
+        assert!((*dst).cpu_id.is_none());
+
+        queue.current = dst;
+        (*dst).cpu_id = Some(self.cpu_index as u32);
+
+        if src.is_null() {
+            drop(queue);
+            context_switch_to(&mut (*dst).context);
+        } else {
+            (*src).cpu_id = None;
+            drop(queue);
+            context_switch(&mut (*dst).context, &mut (*src).context);
+        }
+    }
+
+    pub unsafe fn enter(&mut self) -> ! {
+        intrin::disable_irq();
+
+        let queue = self.queue.lock();
+        self.ready.store(true, Ordering::Release);
+
+        let initial = if let Some(first) = queue.head.as_mut() {
+            first
+        } else {
+            self.idle.as_mut_ptr()
+        };
+
+        drop(queue);
+        debugln!("cpu{}: N -> {:p}", self.cpu_index, initial);
+        self.switch_to(initial);
+        panic!("This code should not run");
+    }
+
+    pub unsafe fn switch(&mut self) {
+        intrin::disable_irq();
+
+        let mut queue = self.queue.lock();
+
+        let src = queue.current;
+        assert!(!src.is_null());
+        let src = &mut *src;
+
+        let dst = if !src.queue_next.is_null() {
+            src.queue_next
+        } else if !queue.head.is_null() {
+            queue.head
+        } else {
+            self.idle.as_mut_ptr()
+        };
+
+        assert!(!dst.is_null());
+        //debugln!("cpu{}: {:p} -> {:p}", self.cpu_index, src, dst);
+        drop(queue);
+
+        self.switch_to(dst);
+    }
+
+    pub unsafe fn get() -> &'static mut Self {
+        &mut SCHEDULER
+    }
+}
+
+unsafe impl Send for Queue {}
+
+extern "C" fn idle_fn(_arg: usize) {
+    loop {}
+}
+
+static mut SCHEDULER: Scheduler = Scheduler::new();
+
+pub fn queue(proc: *mut Process) {
+    unsafe {
+        Scheduler::get().queue(proc);
+    }
+}
+
+pub fn sched_yield() {
+    unsafe {
+        let mut sched = Scheduler::get();
+        if sched.is_ready() {
+            sched.switch();
+        }
+    }
+}
+
+pub fn unqueue(proc: *mut Process) {
+    todo!()
 }

kernel/src/sync.rs

@@ -0,0 +1,78 @@
+use crate::arch::{intrin, cpu::{self, Cpu}};
+use core::cell::UnsafeCell;
+use core::ops::{Deref, DerefMut};
+use core::sync::atomic::{AtomicUsize, Ordering};
+
+pub struct Spin<T: ?Sized + Send> {
+    inner: AtomicUsize,
+    value: UnsafeCell<T>,
+}
+
+pub struct SpinGuard<'a, T: ?Sized + Send> {
+    inner: &'a AtomicUsize,
+    value: &'a mut T,
+    irq_state: usize,
+}
+
+impl<T: Send> Spin<T> {
+    #[inline(always)]
+    pub const fn new(value: T) -> Self {
+        Self {
+            inner: AtomicUsize::new(usize::MAX),
+            value: UnsafeCell::new(value),
+        }
+    }
+}
+
+impl<T: ?Sized + Send> Spin<T> {
+    #[inline]
+    pub fn lock(&self) -> SpinGuard<T> {
+        let irq_state = unsafe { intrin::save_irq() };
+        let cpu_id = cpu::get_phys_id();
+
+        while self
+            .inner
+            .compare_exchange_weak(usize::MAX, cpu_id as usize, Ordering::Acquire, Ordering::Relaxed)
+            .is_err()
+        {
+            while self.inner.load(Ordering::Relaxed) != usize::MAX {
+                intrin::nop();
+            }
+        }
+
+        SpinGuard {
+            inner: &self.inner,
+            value: unsafe { &mut *self.value.get() },
+            irq_state,
+        }
+    }
+}
+
+unsafe impl<T: ?Sized + Send> Sync for Spin<T> {}
+unsafe impl<T: ?Sized + Send> Send for Spin<T> {}
+
+impl<'a, T: ?Sized + Send> Drop for SpinGuard<'a, T> {
+    #[inline]
+    fn drop(&mut self) {
+        self.inner.store(usize::MAX, Ordering::Release);
+        unsafe {
+            intrin::restore_irq(self.irq_state);
+        }
+    }
+}
+
+impl<'a, T: ?Sized + Send> Deref for SpinGuard<'a, T> {
+    type Target = T;
+
+    #[inline(always)]
+    fn deref(&self) -> &Self::Target {
+        self.value
+    }
+}
+
+impl<'a, T: ?Sized + Send> DerefMut for SpinGuard<'a, T> {
+    #[inline(always)]
+    fn deref_mut(&mut self) -> &mut Self::Target {
+        self.value
+    }
+}