diff --git a/address/src/phys.rs b/address/src/phys.rs index 34fadec..b679e67 100644 --- a/address/src/phys.rs +++ b/address/src/phys.rs @@ -87,6 +87,14 @@ impl From for VirtualAddress< } } +#[cfg(target_pointer_width = "64")] +impl From for u64 { + #[inline(always)] + fn from(p: PhysicalAddress) -> Self { + p.0 as u64 + } +} + // Formatting impl fmt::Debug for PhysicalAddress { #[inline] diff --git a/kernel/src/boot/entry.S b/kernel/src/boot/entry.S index ba20ba3..960bb06 100644 --- a/kernel/src/boot/entry.S +++ b/kernel/src/boot/entry.S @@ -28,11 +28,58 @@ _entry: mrs x0, mpidr_el1 ands x0, x0, #3 beq _entry_bsp + 1: - b 1b + 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 + ldr x0, [x0] + + mov x1, #0xFFFFFF8000000000 + ldr x10, [x0, #8] + add x0, x0, x1 + msr tpidr_el1, x0 + adr x11, kernel_ap_main + add x11, x11, x1 + + b _entry_ap .section .text _entry_bsp: + // Setup paging tables + // This is done once for all PEs + adr x0, kernel_l1 + mov x1, #(1 << 0) // Present + orr x1, x1, #(1 << 10) // Accessed + orr x1, x1, #(3 << 8) // Inner shareable + // orr x2, x2, #(0 << 2) // MAIR[0] + str x1, [x0] + + mov x1, #(1 << 0) // Present + orr x1, x1, #(1 << 10) // Accessed + orr x1, x1, #(3 << 8) // Inner shareable + orr x1, x1, #(1 << 2) // MAIR[1] + str x1, [x0, #8] + + // Load BSP stack + mov x0, #0xFFFFFF8000000000 + adr x10, bsp_stack_top + adr x11, kernel_bsp_main + add x10, x10, x0 + add x11, x11, x0 +_entry_ap: + // NOTE the following code must not clobber: x10, x11 // EL3 check mrs x0, CurrentEL lsr x0, x0, #2 @@ -68,32 +115,6 @@ _entry_bsp: eret 1: - // Setup paging tables - adr x8, kernel_l1 - mov x2, #(1 << 0) // Present - orr x2, x2, #(1 << 10) // Accessed - orr x2, x2, #(3 << 8) // Inner shareable - // orr x2, x2, #(0 << 2) // MAIR[0] - str x2, [x8] - - mov x2, #(1 << 0) // Present - orr x2, x2, #(1 << 10) // Accessed - orr x2, x2, #(3 << 8) // Inner shareable - orr x2, x2, #(1 << 2) // MAIR[1] - str x2, [x8, #8] - -// mov x1, #512 -//1: -// sub x1, x1, #1 -// -// lsl x0, x1, #30 -// orr x0, x0, x2 -// -// str x0, [x8, x1, lsl #3] -// -// cmp x1, xzr -// bne 1b - // Setup the MMU mov x0, #(MAIR_EL1_INNER_NC | MAIR_EL1_OUTER_NC) msr mair_el1, x0 @@ -139,15 +160,12 @@ upper_half: orr x0, x0, CPACR_EL1_FPEN_TRAP_NONE msr cpacr_el1, x0 - adr x0, bsp_stack_top - mov sp, x0 + mov sp, x10 adr x0, el1_vectors msr vbar_el1, x0 - bl kernel_main - - b . + br x11 .section .bss @@ -158,3 +176,16 @@ bsp_stack_top: .p2align 12 kernel_l1: .skip 4096 + +.p2align 4 +.global ap_init_value +ap_init_value: + // AP stack pointer + .skip 8 + +.section .data +.p2align 4 +.global ap_wakeup_lock +ap_wakeup_lock: + // Locked by default + .quad 1 diff --git a/kernel/src/cpu.rs b/kernel/src/cpu.rs new file mode 100644 index 0000000..42377b3 --- /dev/null +++ b/kernel/src/cpu.rs @@ -0,0 +1,105 @@ +use crate::{ + mem::phys::{self, PageUsage}, + proc::{self, Scheduler}, + KernelSpace, +}; +use address::{PhysicalAddress, VirtualAddress}; +use core::mem::MaybeUninit; +use core::sync::atomic::{Ordering, AtomicUsize}; + +#[repr(C)] +pub struct Cpu { + pub cpu_id: u32, // 0x00 + _pad0: u32, // 0x04 + stack_top: usize, // 0x08 + // + pub scheduler: Scheduler +} + +const MAX_CPU: usize = 4; +static mut CPUS: [MaybeUninit; MAX_CPU] = MaybeUninit::uninit_array(); +static CPU_COUNT: AtomicUsize = AtomicUsize::new(1); + +#[inline(always)] +pub fn get_cpu() -> &'static mut Cpu { + unsafe { + let mut out: usize; + llvm_asm!("mrs $0, tpidr_el1":"=r"(out)); + &mut *(out as *mut _) + } +} + +fn set_cpu(cpu: *mut Cpu) { + unsafe { + llvm_asm!("msr tpidr_el1, $0"::"r"(cpu)); + } +} + +#[no_mangle] +extern "C" fn kernel_ap_main() -> ! { + let cpu = get_cpu(); + debug!("cpu{} awakened\n", cpu.cpu_id); + + CPU_COUNT.fetch_add(1, Ordering::SeqCst); + + wakeup_single_ap(); + + proc::enter(); +} + +pub fn wakeup_single_ap() { + extern "C" { + static mut ap_wakeup_lock: u64; + static mut ap_init_value: u64; + } + + let this_cpu = get_cpu(); + + // Allocate a stack for the AP + let stack_bottom_phys = phys::alloc_contiguous_pages(PageUsage::Kernel, 4).unwrap(); + let stack_bottom = VirtualAddress::::from(stack_bottom_phys); + + // Allocate a new CPU struct + let index = CPU_COUNT.load(Ordering::Acquire); + + if index == MAX_CPU { + debug!("cpu{}: reached cpu limit\n", this_cpu.cpu_id); + return; + } + + debug!("cpu{}: waking up cpu{}\n", this_cpu.cpu_id, index); + + unsafe { + CPUS[index].write(Cpu { + cpu_id: index as u32, + _pad0: 0, + stack_top: (stack_bottom + 4 * 4096).into(), + scheduler: Scheduler::new() + }); + + let cpu_addr = VirtualAddress::::from(CPUS[index].as_mut_ptr() as u64); + let cpu_addr_phys = PhysicalAddress::from(cpu_addr); + + // Wakeup a single AP for test + core::ptr::write_volatile(&mut ap_init_value, cpu_addr_phys.into()); + // Ensure all writes are complete before waking up an AP + llvm_asm!("dsb sy"); + core::ptr::write_volatile(&mut ap_wakeup_lock, 0); + } + + // Any further CPUs will be waken up by the newly started AP +} + +pub fn bsp_init() { + unsafe { + CPUS[0].write(Cpu { + cpu_id: 0, + _pad0: 0, + stack_top: 0, + scheduler: Scheduler::new() + }); + set_cpu(CPUS[0].as_mut_ptr()); + + debug!("BSP cpu: {:p}\n", get_cpu()); + } +} diff --git a/kernel/src/main.rs b/kernel/src/main.rs index 50119a9..fada6f8 100644 --- a/kernel/src/main.rs +++ b/kernel/src/main.rs @@ -1,4 +1,4 @@ -#![feature(global_asm, llvm_asm, const_panic)] +#![feature(global_asm, llvm_asm, const_panic, maybe_uninit_uninit_array)] #![no_std] #![no_main] @@ -9,6 +9,8 @@ pub mod arch; pub mod boot; pub mod mem; pub mod proc; +// XXX XXX +pub mod cpu; pub use mem::KernelSpace; @@ -31,8 +33,10 @@ impl Iterator for SimpleMemoryIterator<'_> { } #[no_mangle] -extern "C" fn kernel_main() -> ! { +extern "C" fn kernel_bsp_main() -> ! { // TODO determine VC/ARM split + cpu::bsp_init(); + let memory = UsableMemory { start: PhysicalAddress::from(0usize), end: PhysicalAddress::from(0x30000000usize), @@ -44,6 +48,9 @@ extern "C" fn kernel_main() -> ! { mem::phys::initialize(iter); } + debug!("BSP init finished\n"); + + cpu::wakeup_single_ap(); proc::enter(); } diff --git a/kernel/src/proc/context.S b/kernel/src/proc/context.S index 524c672..bdd9e84 100644 --- a/kernel/src/proc/context.S +++ b/kernel/src/proc/context.S @@ -1,4 +1,7 @@ .section .text +.global context_switch_to +.global context_switch + context_enter_kernel: ldp x0, xzr, [sp, #0] ldp lr, x1, [sp, #16] diff --git a/kernel/src/proc/context.rs b/kernel/src/proc/context.rs new file mode 100644 index 0000000..4f20864 --- /dev/null +++ b/kernel/src/proc/context.rs @@ -0,0 +1,77 @@ +use crate::{ + mem::phys::{self, PageUsage}, + KernelSpace, +}; +use address::VirtualAddress; +use core::mem::size_of; + +global_asm!(include_str!("context.S")); + +#[repr(C)] +pub(super) struct Context { + kernel_sp: VirtualAddress, // 0x00 + cpu_id: u32, // 0x08 +} + +struct StackBuilder { + bp: VirtualAddress, + sp: VirtualAddress, +} + +impl Context { + pub fn new_kernel(entry: usize, arg: usize) -> Context { + let kstack_phys = phys::alloc_contiguous_pages(PageUsage::Kernel, 4).unwrap(); + let mut stack = unsafe { StackBuilder::new(kstack_phys.into(), 4096 * 4) }; + let stack_top = stack.sp; + + stack.push(stack_top); // popped into stack register before ERET + stack.push(entry); // ELR before ERET + stack.push(0usize); // padding + stack.push(arg); + + stack.push(context_enter_kernel as usize); // x30 LR + stack.push(0usize); // padding + stack.push(0usize); // x29 + stack.push(0usize); // x27 + stack.push(0usize); // x26 + stack.push(0usize); // x25 + stack.push(0usize); // x24 + stack.push(0usize); // x23 + stack.push(0usize); // x22 + stack.push(0usize); // x21 + stack.push(0usize); // x20 + stack.push(0usize); // x19 + + Context { + kernel_sp: stack.sp, + cpu_id: u32::MAX, + } + } +} + +impl StackBuilder { + pub unsafe fn new(bp: VirtualAddress, size: usize) -> Self { + Self { + bp, + sp: bp + size, + } + } + + pub fn push>(&mut self, value: A) { + if self.sp == self.bp { + panic!("Stack overflow"); + } + self.sp -= size_of::(); + unsafe { + core::ptr::write(self.sp.as_mut_ptr(), value.into()); + } + } +} + +extern "C" { + pub(super) fn context_switch_to(dst: *mut Context); + pub(super) fn context_switch(dst: *mut Context, src: *mut Context); + + fn context_enter_kernel(); +} + diff --git a/kernel/src/proc/mod.rs b/kernel/src/proc/mod.rs index d776b9d..dbf7333 100644 --- a/kernel/src/proc/mod.rs +++ b/kernel/src/proc/mod.rs @@ -1,101 +1,162 @@ -use crate::{ - mem::phys::{self, PageUsage}, - KernelSpace, -}; -use address::VirtualAddress; -use core::mem::{size_of, MaybeUninit}; +use crate::cpu::get_cpu; +use core::mem::MaybeUninit; +use core::ptr::null_mut; +use spin::Mutex; -global_asm!(include_str!("context.S")); +pub mod context; +use context::{context_switch, context_switch_to, Context}; -#[repr(C)] -struct Context { - kernel_sp: VirtualAddress, +struct Process { + context: Context, + + sched_prev: *mut Process, + sched_next: *mut Process, } -struct Stack { - bp: VirtualAddress, - sp: VirtualAddress, - tp: VirtualAddress, +pub struct Scheduler { + queue_head: Mutex<*mut Process>, + current: Mutex<*mut Process>, + idle: MaybeUninit, } -impl Context { - fn new_kernel(entry: usize, arg: usize) -> Context { - let kstack_phys = phys::alloc_contiguous_pages(PageUsage::Kernel, 4).unwrap(); - let mut stack = unsafe { Stack::new(kstack_phys.into(), 4096 * 4) }; +impl Process { + fn new_kernel(entry: usize, arg: usize) -> Self { + Self { + context: Context::new_kernel(entry, arg), - stack.push(stack.tp); // popped into stack register before ERET - stack.push(entry); // ELR before ERET - stack.push(0usize); // padding - stack.push(arg); - - stack.push(context_enter_kernel as usize); // x30 LR - stack.push(0usize); // padding - stack.push(0usize); // x29 - stack.push(0usize); // x27 - stack.push(0usize); // x26 - stack.push(0usize); // x25 - stack.push(0usize); // x24 - stack.push(0usize); // x23 - stack.push(0usize); // x22 - stack.push(0usize); // x21 - stack.push(0usize); // x20 - stack.push(0usize); // x19 - - Context { - kernel_sp: stack.sp, - } - } -} - -impl Stack { - pub unsafe fn new(bp: VirtualAddress, size: usize) -> Stack { - Stack { - bp, - sp: bp + size, - tp: bp + size, + sched_prev: null_mut(), + sched_next: null_mut(), } } - pub fn push>(&mut self, value: A) { - if self.sp == self.bp { - panic!("Stack overflow"); + //pub unsafe fn enter_initial(&mut self) -> ! { + // context_switch_to(&mut self.context); + // panic!("This code should not run"); + //} + + //pub unsafe fn switch_to(&mut self, next: &mut Process) { + // if self as *mut _ == next as *mut _ { + // return; + // } + // // TODO &mut self.context argument can be eliminated + // // if extracted from Cpu struct in assembly + // context_switch(&mut next.context, &mut self.context); + //} +} + +impl Scheduler { + pub fn new() -> Self { + Self { + queue_head: Mutex::new(null_mut()), + current: Mutex::new(null_mut()), + idle: MaybeUninit::uninit(), } - self.sp -= size_of::(); - unsafe { - core::ptr::write(self.sp.as_mut_ptr(), value.into()); + } + + unsafe fn queue(&mut self, proc: *mut Process) { + let mut lock = self.queue_head.lock(); + + if !(*lock).is_null() { + let queue_tail = (**lock).sched_prev; + (*queue_tail).sched_next = proc; + (*proc).sched_prev = queue_tail; + (**lock).sched_prev = proc; + (*proc).sched_next = *lock; + } else { + (*proc).sched_prev = proc; + (*proc).sched_next = proc; + *lock = proc; } } + + unsafe fn enter_process(&mut self, proc: *mut Process) -> ! { + *self.current.lock() = proc; + context_switch_to(&mut (*proc).context); + panic!("This code should not run"); + } + + unsafe fn switch_process(&mut self, from: *mut Process, to: *mut Process) { + *self.current.lock() = to; + context_switch(&mut (*to).context, &mut (*from).context); + } + + unsafe fn enter(&mut self) -> ! { + let mut lock = self.queue_head.lock(); + + let proc = if let Some(first) = (*lock).as_mut() { + first + } else { + self.idle.as_mut_ptr() + }; + drop(lock); + self.enter_process(proc); + } + + unsafe fn init_idle(&mut self) { + self.idle.write(Process::new_kernel(idle_fn as usize, 0)); + } + + unsafe fn sched(&mut self) { + let queue_lock = self.queue_head.lock(); + let current_lock = self.current.lock(); + let from = *current_lock; + assert!(!from.is_null()); + let from = &mut *from; + + let to = if !from.sched_next.is_null() { + from.sched_next + } else if !(*queue_lock).is_null() { + *queue_lock + } else { + self.idle.as_mut_ptr() + }; + + assert!(!to.is_null()); + drop(queue_lock); + drop(current_lock); + + self.switch_process(from, to); + } } -extern "C" { - fn context_switch_to(dst: *mut Context); - fn context_switch(dst: *mut Context, src: *mut Context); +fn idle_fn(arg: usize) { + loop {} +} - fn context_enter_kernel(); +#[no_mangle] +pub extern "C" fn sched_yield() { + let cpu = get_cpu(); + unsafe { + cpu.scheduler.sched(); + } } fn f0(arg: usize) { loop { + debug!("{}", arg); unsafe { - debug!("{}", arg); - if arg == 0 { - context_switch(C1.as_mut_ptr(), C0.as_mut_ptr()); - } else { - context_switch(C0.as_mut_ptr(), C1.as_mut_ptr()); - } + sched_yield(); } } } -static mut C0: MaybeUninit = MaybeUninit::uninit(); -static mut C1: MaybeUninit = MaybeUninit::uninit(); +static mut C0: MaybeUninit = MaybeUninit::uninit(); +static mut C1: MaybeUninit = MaybeUninit::uninit(); pub fn enter() -> ! { unsafe { - C0.write(Context::new_kernel(f0 as usize, 0)); - C1.write(Context::new_kernel(f0 as usize, 1)); + let cpu = get_cpu(); - context_switch_to(C0.as_mut_ptr()); + if cpu.cpu_id == 0 { + debug!("Setting up a task for cpu0\n"); + C0.write(Process::new_kernel(f0 as usize, 0)); + cpu.scheduler.queue(C0.assume_init_mut()); + } + + // Initialize the idle task + cpu.scheduler.init_idle(); + + debug!("Entering scheduler on cpu{}\n", cpu.cpu_id); + cpu.scheduler.enter(); } - loop {} }