diff --git a/crate/memory/src/memory_set.rs b/crate/memory/src/memory_set.rs index 9731c27..3c631eb 100644 --- a/crate/memory/src/memory_set.rs +++ b/crate/memory/src/memory_set.rs @@ -222,6 +222,7 @@ pub struct MemoryAttr { user: bool, readonly: bool, execute: bool, + mmio: bool, hide: bool, } @@ -250,6 +251,10 @@ impl MemoryAttr { self.execute = true; self } + pub fn mmio(mut self) -> Self { + self.mmio = true; + self + } /* ** @brief set the memory attribute's hide bit ** @retval MemoryAttr the memory attribute itself @@ -268,8 +273,9 @@ impl MemoryAttr { if self.user { entry.set_user(true); } if self.readonly { entry.set_writable(false); } if self.execute { entry.set_execute(true); } + if self.mmio { entry.set_mmio(true); } if self.hide { entry.set_present(false); } - if self.user || self.readonly || self.execute || self.hide { entry.update(); } + if self.user || self.readonly || self.execute || self.mmio || self.hide { entry.update(); } } } @@ -325,6 +331,9 @@ impl MemorySet { pub fn iter(&self) -> impl Iterator { self.areas.iter() } + pub fn edit(&mut self, f: impl FnOnce(&mut T::Active)) { + self.page_table.edit(f); + } /* ** @brief execute function with the associated page table ** @param f: impl FnOnce() the function to be executed diff --git a/crate/memory/src/paging/mod.rs b/crate/memory/src/paging/mod.rs index ceda06e..6bbe9b2 100644 --- a/crate/memory/src/paging/mod.rs +++ b/crate/memory/src/paging/mod.rs @@ -195,4 +195,6 @@ pub trait Entry { ** @retval none */ fn set_execute(&mut self, value: bool); + fn mmio(&self) -> bool; + fn set_mmio(&mut self, value: bool); } diff --git a/kernel/Cargo.toml b/kernel/Cargo.toml index 00fcf23..e8ba981 100644 --- a/kernel/Cargo.toml +++ b/kernel/Cargo.toml @@ -51,7 +51,7 @@ riscv = { git = "https://github.com/riscv-and-rust-and-decaf/riscv" } bbl = { path = "../crate/bbl" } [target.'cfg(target_arch = "aarch64")'.dependencies] -cortex-a = "2.2.1" +aarch64 = { git = "https://github.com/equation314/aarch64" } atags = { path = "../crate/atags" } bcm2837 = { path = "../crate/bcm2837", features = ["use_generic_timer"] } diff --git a/kernel/Makefile b/kernel/Makefile index 5ab7d19..e96afbf 100644 --- a/kernel/Makefile +++ b/kernel/Makefile @@ -142,7 +142,7 @@ ifeq ($(arch), riscv32) ifeq ($(board), fpga) @cp $(kernel) $@ else - @cd ../riscv-pk && \ + @cd ../tools/riscv-pk && \ mkdir -p build && \ cd build && \ ../configure \ @@ -152,7 +152,7 @@ else --host=riscv64-unknown-elf \ --with-payload=$(abspath $(kernel)) && \ make && \ - cp bbl ../../kernel/$@ + cp bbl ../../../kernel/$@ endif else ifeq ($(arch), aarch64) @$(objcopy) $(kernel) --strip-all -O binary $@ diff --git a/kernel/src/arch/aarch64/board/raspi3/mod.rs b/kernel/src/arch/aarch64/board/raspi3/mod.rs index 4276dd9..26c2ff6 100644 --- a/kernel/src/arch/aarch64/board/raspi3/mod.rs +++ b/kernel/src/arch/aarch64/board/raspi3/mod.rs @@ -4,13 +4,13 @@ pub mod irq; pub mod timer; pub mod serial; +pub const IO_REMAP_BASE: usize = bcm2837::IO_BASE; +pub const IO_REMAP_END: usize = 0x40001000; + pub fn init() { - // FIXME - // assert_has_not_been_called!("board::init must be called only once"); + assert_has_not_been_called!("board::init must be called only once"); - unsafe { - serial::SERIAL_PORT.init(); - } + serial::SERIAL_PORT.lock().init(); println!("Hello Raspberry Pi!"); } diff --git a/kernel/src/arch/aarch64/board/raspi3/serial.rs b/kernel/src/arch/aarch64/board/raspi3/serial.rs index 5170195..a7856b8 100644 --- a/kernel/src/arch/aarch64/board/raspi3/serial.rs +++ b/kernel/src/arch/aarch64/board/raspi3/serial.rs @@ -19,8 +19,7 @@ impl SerialPort { /// Init a newly created SerialPort, can only be called once. pub fn init(&mut self) { - // FIXME - // assert_has_not_been_called!("SerialPort::init must be called only once"); + assert_has_not_been_called!("SerialPort::init must be called only once"); self.mu = Some(MiniUart::new()); } @@ -70,7 +69,4 @@ impl fmt::Write for SerialPort { } } -// FIXME -// pub static SERIAL_PORT: Mutex = Mutex::new(SerialPort::new()); -pub static mut SERIAL_PORT: SerialPort = SerialPort::new(); - +pub static SERIAL_PORT: Mutex = Mutex::new(SerialPort::new()); diff --git a/kernel/src/arch/aarch64/boot/boot.S b/kernel/src/arch/aarch64/boot/boot.S index b8a142e..7a126bd 100644 --- a/kernel/src/arch/aarch64/boot/boot.S +++ b/kernel/src/arch/aarch64/boot/boot.S @@ -2,8 +2,7 @@ .section .text.boot -.global _start -_start: +boot: # read cpu affinity, start core 0, halt rest mrs x1, mpidr_el1 and x1, x1, #3 @@ -16,7 +15,7 @@ halt: setup: # store the desired EL1 stack pointer in x1 - adr x1, _start + ldr x1, =_start # use SP_ELx for Exception level ELx msr SPsel, #1 @@ -94,17 +93,22 @@ set_stack: zero_bss: # load the start address and number of bytes in BSS section - ldr x1, =__bss_start + ldr x1, =sbss ldr x2, =__bss_length zero_bss_loop: # zero out the BSS section, 64-bits at a time - cbz x2, go_kmain + cbz x2, zero_bss_loop_end str xzr, [x1], #8 sub x2, x2, #8 cbnz x2, zero_bss_loop -go_kmain: +zero_bss_loop_end: + b _start + +.section .text.entry +.globl _start +_start: # jump to rust_main, which shouldn't return. halt if it does bl rust_main b halt diff --git a/kernel/src/arch/aarch64/boot/linker.ld b/kernel/src/arch/aarch64/boot/linker.ld index ce4c0c2..2c7bdce 100644 --- a/kernel/src/arch/aarch64/boot/linker.ld +++ b/kernel/src/arch/aarch64/boot/linker.ld @@ -3,36 +3,49 @@ ENTRY(_start) SECTIONS { . = 0x80000; /* Raspbery Pi 3 Aarch64 (kernel8.img) load address */ - /* start of the binary */ - _start = .; + .boot : { + KEEP(*(.text.boot)) /* from boot.S */ + } + + . = 0x100000; /* Load the kernel at this address. It's also kernel stack top address */ + bootstacktop = .; .text : { - KEEP(*(.text.boot)) /* from boot.S */ + stext = .; + *(.text.entry) *(.text .text.* .gnu.linkonce.t*) + . = ALIGN(4K); + etext = .; } .rodata : { + srodata = .; *(.rodata .rodata.* .gnu.linkonce.r*) + . = ALIGN(4K); + erodata = .; } .data : { + sdata = .; *(.data .data.* .gnu.linkonce.d*) + . = ALIGN(4K); + edata = .; } .bss (NOLOAD) : { . = ALIGN(32); - __bss_start = .; + sbss = .; *(.bss .bss.*) *(COMMON) - . = ALIGN(8); - __bss_end = .; + . = ALIGN(4K); + ebss = .; } /* end of the binary */ _end = ALIGN(8); /* number of bytes in BSS section and complete binary */ - __bss_length = (__bss_end - __bss_start); + __bss_length = (ebss - sbss); __binary_length = (_end - _start); /DISCARD/ : { *(.comment) *(.gnu*) *(.note*) *(.eh_frame*) } diff --git a/kernel/src/arch/aarch64/interrupt/context.rs b/kernel/src/arch/aarch64/interrupt/context.rs index e494453..f5f4eda 100644 --- a/kernel/src/arch/aarch64/interrupt/context.rs +++ b/kernel/src/arch/aarch64/interrupt/context.rs @@ -1,12 +1,20 @@ //! TrapFrame and context definitions for aarch64. +extern crate aarch64; + +use spin::{Mutex}; +use aarch64::barrier; +use aarch64::addr::PhysAddr; +use aarch64::paging::PhysFrame; +use aarch64::asm::{tlb_invalidate_all, ttbr_el1_write_asid}; + #[repr(C)] #[derive(Default, Debug, Copy, Clone)] pub struct TrapFrame { pub elr: usize, pub spsr: usize, pub sp: usize, - pub tpidr: usize, + pub tpidr: usize, // currently unused // pub q0to31: [u128; 32], // disable SIMD/FP registers pub x1to29: [usize; 29], pub __reserved: usize, @@ -47,10 +55,14 @@ pub struct InitStack { } impl InitStack { - unsafe fn push_at(self, stack_top: usize) -> Context { + unsafe fn push_at(self, stack_top: usize, ttbr: usize) -> Context { let ptr = (stack_top as *mut Self).offset(-1); *ptr = self; - Context(ptr as usize) + Context { + stack_top: ptr as usize, + ttbr: PhysFrame::containing_address(PhysAddr::new(ttbr as u64)), + asid: Asid::default(), + } } } @@ -63,18 +75,20 @@ extern { struct ContextData { x19to29: [usize; 11], lr: usize, - ttbr0: usize, } impl ContextData { - fn new(ttbr0: usize) -> Self { - ContextData { lr: __trapret as usize, ttbr0, ..ContextData::default() } + fn new() -> Self { + ContextData { lr: __trapret as usize, ..ContextData::default() } } } - #[derive(Debug)] -pub struct Context(usize); +pub struct Context { + stack_top: usize, + ttbr: PhysFrame, + asid: Asid, +} impl Context { /// Switch to another kernel thread. @@ -86,10 +100,10 @@ impl Context { /// Pop all callee-saved registers, then return to the target. #[naked] #[inline(never)] - pub unsafe extern fn switch(&mut self, target: &mut Self) { + unsafe extern fn __switch(self_stack: &mut usize, target_stack: &mut usize) { asm!( " - mov x10, #-(13 * 8) + mov x10, #-(12 * 8) add x8, sp, x10 str x8, [x0] stp x19, x20, [x8], #16 // store callee-saved registers @@ -98,8 +112,6 @@ impl Context { stp x25, x26, [x8], #16 stp x27, x28, [x8], #16 stp x29, lr, [x8], #16 - mrs x9, ttbr0_el1 - str x9, [x8], #8 ldr x8, [x1] ldp x19, x20, [x8], #16 // restore callee-saved registers @@ -108,45 +120,51 @@ impl Context { ldp x25, x26, [x8], #16 ldp x27, x28, [x8], #16 ldp x29, lr, [x8], #16 - ldr x9, [x8], #8 mov sp, x8 - msr ttbr0_el1, x9 // set new page directory - dsb ishst // ensure write has completed - tlbi vmalle1is // invalidate the TLB entry for the entry that changes - dsb ish // ensure TLB invalidation is complete - isb // synchronize context on this processor - str xzr, [x1] ret" : : : : "volatile" ); } + pub unsafe fn switch(&mut self, target: &mut Self) { + target.asid = ASID_ALLOCATOR.lock().alloc(target.asid); + + // with ASID we needn't flush TLB frequently + ttbr_el1_write_asid(1, target.asid.value, target.ttbr); + barrier::dsb(barrier::ISH); + Self::__switch(&mut self.stack_top, &mut target.stack_top); + } + pub unsafe fn null() -> Self { - Context(0) + Context { + stack_top: 0, + ttbr: PhysFrame::containing_address(PhysAddr::new(0)), + asid: Asid::default(), + } } - pub unsafe fn new_kernel_thread(entry: extern fn(usize) -> !, arg: usize, kstack_top: usize, ttbr0: usize) -> Self { + pub unsafe fn new_kernel_thread(entry: extern fn(usize) -> !, arg: usize, kstack_top: usize, ttbr: usize) -> Self { InitStack { - context: ContextData::new(ttbr0), + context: ContextData::new(), tf: TrapFrame::new_kernel_thread(entry, arg, kstack_top), - }.push_at(kstack_top) + }.push_at(kstack_top, ttbr) } - pub unsafe fn new_user_thread(entry_addr: usize, ustack_top: usize, kstack_top: usize, is32: bool, ttbr0: usize) -> Self { + pub unsafe fn new_user_thread(entry_addr: usize, ustack_top: usize, kstack_top: usize, is32: bool, ttbr: usize) -> Self { InitStack { - context: ContextData::new(ttbr0), + context: ContextData::new(), tf: TrapFrame::new_user_thread(entry_addr, ustack_top), - }.push_at(kstack_top) + }.push_at(kstack_top, ttbr) } - pub unsafe fn new_fork(tf: &TrapFrame, kstack_top: usize, ttbr0: usize) -> Self { + pub unsafe fn new_fork(tf: &TrapFrame, kstack_top: usize, ttbr: usize) -> Self { InitStack { - context: ContextData::new(ttbr0), + context: ContextData::new(), tf: { let mut tf = tf.clone(); tf.x0 = 0; tf }, - }.push_at(kstack_top) + }.push_at(kstack_top, ttbr) } /// Called at a new user context /// To get the init TrapFrame in sys_exec @@ -155,3 +173,41 @@ impl Context { } } + +const ASID_MASK: u16 = 0xffff; + +#[derive(Debug, Copy, Clone, Default)] +#[repr(C)] +struct Asid { + value: u16, + generation: u16, +} + +struct AsidAllocator(Asid); + +impl AsidAllocator { + fn new() -> Self { + AsidAllocator(Asid { value: 0, generation: 1 }) + } + + fn alloc(&mut self, old_asid: Asid) -> Asid { + if self.0.generation == old_asid.generation { + return old_asid; + } + + if self.0.value == ASID_MASK { + self.0.value = 0; + self.0.generation = self.0.generation.wrapping_add(1); + if self.0.generation == 0 { + self.0.generation += 1; + } + tlb_invalidate_all(); + } + self.0.value += 1; + return self.0; + } +} + +lazy_static! { + static ref ASID_ALLOCATOR: Mutex = Mutex::new(AsidAllocator::new()); +} diff --git a/kernel/src/arch/aarch64/interrupt/handler.rs b/kernel/src/arch/aarch64/interrupt/handler.rs index e4a768c..2b5adcc 100644 --- a/kernel/src/arch/aarch64/interrupt/handler.rs +++ b/kernel/src/arch/aarch64/interrupt/handler.rs @@ -2,7 +2,9 @@ use crate::arch::board::irq::handle_irq; use super::context::TrapFrame; -use super::syndrome::Syndrome; +use super::syndrome::{Fault, Syndrome}; + +use aarch64::regs::*; use log::*; global_asm!(include_str!("trap.S")); @@ -39,14 +41,22 @@ pub struct Info { /// the trap frame for the exception. #[no_mangle] pub extern "C" fn rust_trap(info: Info, esr: u32, tf: &mut TrapFrame) { - let syndrome = Syndrome::from(esr); - trace!("Interrupt: {:?} from: {:?}", syndrome, info); + trace!("Interrupt: {:?}, ELR: {:#x?}", info, tf.elr); match info.kind { Kind::Synchronous => { + let syndrome = Syndrome::from(esr); + trace!("ESR: {:#x?}, Syndrome: {:?}", esr, syndrome); // syndrome is only valid with sync match syndrome { Syndrome::Brk(brk) => handle_break(brk, tf), - Syndrome::Svc(_) => handle_syscall(tf), + Syndrome::Svc(svc) => handle_syscall(svc, tf), + Syndrome::DataAbort { kind, level: _ } + | Syndrome::InstructionAbort { kind, level: _ } => match kind { + Fault::Translation | Fault::AccessFlag | Fault::Permission => { + handle_page_fault(tf) + } + _ => ::trap::error(tf), + }, _ => crate::trap::error(tf), } } @@ -56,12 +66,16 @@ pub extern "C" fn rust_trap(info: Info, esr: u32, tf: &mut TrapFrame) { trace!("Interrupt end"); } -fn handle_break(num: u16, tf: &mut TrapFrame) { +fn handle_break(_num: u16, tf: &mut TrapFrame) { // Skip the current brk instruction (ref: J1.1.2, page 6147) tf.elr += 4; } -fn handle_syscall(tf: &mut TrapFrame) { +fn handle_syscall(num: u16, tf: &mut TrapFrame) { + if num != 0 { + ::trap::error(tf); + } + // svc instruction has been skipped in syscall (ref: J1.1.2, page 6152) let ret = crate::syscall::syscall( tf.x1to29[7] as usize, @@ -77,3 +91,10 @@ fn handle_syscall(tf: &mut TrapFrame) { ); tf.x0 = ret as usize; } + +fn handle_page_fault(tf: &mut TrapFrame) { + let addr = FAR_EL1.get(); + trace!("\nEXCEPTION: Page Fault @ {:#x}", addr); + + ::trap::error(tf); +} diff --git a/kernel/src/arch/aarch64/interrupt/mod.rs b/kernel/src/arch/aarch64/interrupt/mod.rs index 606654a..bba925a 100644 --- a/kernel/src/arch/aarch64/interrupt/mod.rs +++ b/kernel/src/arch/aarch64/interrupt/mod.rs @@ -4,7 +4,8 @@ mod handler; mod context; mod syndrome; -use cortex_a::regs::*; +use aarch64::regs::*; + pub use self::context::*; pub use self::handler::*; diff --git a/kernel/src/arch/aarch64/io.rs b/kernel/src/arch/aarch64/io.rs index deda39c..1a9d1d4 100644 --- a/kernel/src/arch/aarch64/io.rs +++ b/kernel/src/arch/aarch64/io.rs @@ -4,15 +4,11 @@ use core::fmt::{Arguments, Write}; use super::board::serial::{SerialRead, SERIAL_PORT}; pub fn getchar() -> char { - // FIXME - unsafe { - SERIAL_PORT.receive() as char - } + unsafe { SERIAL_PORT.force_unlock(); } + SERIAL_PORT.lock().receive() as char } pub fn putfmt(fmt: Arguments) { - // FIXME - unsafe { - SERIAL_PORT.write_fmt(fmt).unwrap() - } + unsafe { SERIAL_PORT.force_unlock(); } + SERIAL_PORT.lock().write_fmt(fmt).unwrap() } diff --git a/kernel/src/arch/aarch64/memory.rs b/kernel/src/arch/aarch64/memory.rs index edc1d5b..29f3009 100644 --- a/kernel/src/arch/aarch64/memory.rs +++ b/kernel/src/arch/aarch64/memory.rs @@ -1,29 +1,131 @@ //! Memory initialization for aarch64. use ucore_memory::PAGE_SIZE; -use atags::atags::Atags; -use crate::HEAP_ALLOCATOR; -use log::*; +use memory::{FRAME_ALLOCATOR, init_heap, MemoryArea, MemoryAttr, MemorySet, Stack}; +use super::atags::atags::Atags; +use aarch64::{barrier, regs::*, addr::*}; +use aarch64::paging::{PhysFrame as Frame, memory_attribute::*}; /// Memory initialization. pub fn init() { + init_frame_allocator(); + init_heap(); + remap_the_kernel(); + info!("memory: init end"); +} + +/// initialize temporary paging and enable mmu immediately after boot. Serial port is disabled at this time. +pub fn init_mmu_early() { + #[repr(align(4096))] + struct PageData([u8; PAGE_SIZE]); + static PAGE_TABLE_LVL4: PageData = PageData([0; PAGE_SIZE]); + static PAGE_TABLE_LVL3: PageData = PageData([0; PAGE_SIZE]); + static PAGE_TABLE_LVL2: PageData = PageData([0; PAGE_SIZE]); + + let frame_lvl4 = Frame::containing_address(PhysAddr::new(&PAGE_TABLE_LVL4 as *const _ as u64)); + let frame_lvl3 = Frame::containing_address(PhysAddr::new(&PAGE_TABLE_LVL3 as *const _ as u64)); + let frame_lvl2 = Frame::containing_address(PhysAddr::new(&PAGE_TABLE_LVL2 as *const _ as u64)); + super::paging::setup_temp_page_table(frame_lvl4, frame_lvl3, frame_lvl2); + + // device. + MAIR_EL1.write( + MAIR_EL1::Attr0.val(MairDevice::config_value()) + + MAIR_EL1::Attr1.val(MairNormal::config_value()) + + MAIR_EL1::Attr2.val(MairNormalNonCacheable::config_value()), + ); + + // Configure various settings of stage 1 of the EL1 translation regime. + let ips = ID_AA64MMFR0_EL1.read(ID_AA64MMFR0_EL1::PARange); + TCR_EL1.write( + TCR_EL1::TBI1::Ignored + + TCR_EL1::TBI0::Ignored + + TCR_EL1::AS::Bits_16 + + TCR_EL1::IPS.val(ips) + + + TCR_EL1::TG1::KiB_4 + + TCR_EL1::SH1::Inner + + TCR_EL1::ORGN1::WriteBack_ReadAlloc_WriteAlloc_Cacheable + + TCR_EL1::IRGN1::WriteBack_ReadAlloc_WriteAlloc_Cacheable + + TCR_EL1::EPD1::EnableTTBR1Walks + + TCR_EL1::A1::UseTTBR1ASID + + TCR_EL1::T1SZ.val(16) + + + TCR_EL1::TG0::KiB_4 + + TCR_EL1::SH0::Inner + + TCR_EL1::ORGN0::WriteBack_ReadAlloc_WriteAlloc_Cacheable + + TCR_EL1::IRGN0::WriteBack_ReadAlloc_WriteAlloc_Cacheable + + TCR_EL1::EPD0::EnableTTBR0Walks + + TCR_EL1::T0SZ.val(16), + ); + + // Switch the MMU on. + // + // First, force all previous changes to be seen before the MMU is enabled. + unsafe { barrier::isb(barrier::SY); } + + // Enable the MMU and turn on data and instruction caching. + SCTLR_EL1.modify(SCTLR_EL1::M::Enable + SCTLR_EL1::C::Cacheable + SCTLR_EL1::I::Cacheable); + + // Force MMU init to complete before next instruction + unsafe { barrier::isb(barrier::SY); } +} + +fn init_frame_allocator() { + use bit_allocator::BitAlloc; + use core::ops::Range; + use consts::MEMORY_OFFSET; + let (start, end) = memory_map().expect("failed to find memory map"); - unsafe { - HEAP_ALLOCATOR.lock().init(start, end - start); + let mut ba = FRAME_ALLOCATOR.lock(); + ba.insert(to_range(start, end)); + info!("FrameAllocator init end"); + + /* + * @param: + * start: start address + * end: end address + * @brief: + * transform the memory address to the page number + * @retval: + * the page number range from start address to end address + */ + fn to_range(start: usize, end: usize) -> Range { + let page_start = (start - MEMORY_OFFSET) / PAGE_SIZE; + let page_end = (end - MEMORY_OFFSET - 1) / PAGE_SIZE + 1; + page_start..page_end } - info!("memory: init end"); } -extern "C" { - static _end: u8; +/// remap kernel page table after all initialization. +fn remap_the_kernel() { + let (bottom, top) = (0, bootstacktop as usize); + let kstack = Stack { + top, + bottom, + }; + static mut SPACE: [u8; 0x1000] = [0; 0x1000]; + let mut ms = unsafe { MemorySet::new_from_raw_space(&mut SPACE, kstack) }; + ms.push(MemoryArea::new_identity(bottom, top, MemoryAttr::default(), "kstack")); + ms.push(MemoryArea::new_identity(stext as usize, etext as usize, MemoryAttr::default().execute().readonly(), "text")); + ms.push(MemoryArea::new_identity(sdata as usize, edata as usize, MemoryAttr::default(), "data")); + ms.push(MemoryArea::new_identity(srodata as usize, erodata as usize, MemoryAttr::default().readonly(), "rodata")); + ms.push(MemoryArea::new_identity(sbss as usize, ebss as usize, MemoryAttr::default(), "bss")); + + use arch::board::{IO_REMAP_BASE, IO_REMAP_END}; + ms.push(MemoryArea::new_identity(IO_REMAP_BASE, IO_REMAP_END, MemoryAttr::default().mmio(), "io_remap")); + + unsafe { ms.activate(); } + use core::mem::forget; + forget(ms); + info!("kernel remap end"); } /// Returns the (start address, end address) of the available memory on this /// system if it can be determined. If it cannot, `None` is returned. /// /// This function is expected to return `Some` under all normal cirumstances. -pub fn memory_map() -> Option<(usize, usize)> { - let binary_end = unsafe { (&_end as *const u8) as u32 }; +fn memory_map() -> Option<(usize, usize)> { + let binary_end = _end as u32; let mut atags: Atags = Atags::get(); while let Some(atag) = atags.next() { @@ -34,3 +136,17 @@ pub fn memory_map() -> Option<(usize, usize)> { None } + +extern { + fn bootstacktop(); + fn stext(); + fn etext(); + fn sdata(); + fn edata(); + fn srodata(); + fn erodata(); + fn sbss(); + fn ebss(); + fn _start(); + fn _end(); +} diff --git a/kernel/src/arch/aarch64/mod.rs b/kernel/src/arch/aarch64/mod.rs index de86725..dabdc1f 100644 --- a/kernel/src/arch/aarch64/mod.rs +++ b/kernel/src/arch/aarch64/mod.rs @@ -18,9 +18,13 @@ global_asm!(include_str!("boot/boot.S")); /// The entry point of kernel #[no_mangle] // don't mangle the name of this function pub extern "C" fn rust_main() -> ! { + // Enable mmu and paging + memory::init_mmu_early(); + // Init board to enable serial port. board::init(); - crate::logging::init(); // FIXME + + crate::logging::init(); interrupt::init(); memory::init(); timer::init(); diff --git a/kernel/src/arch/aarch64/paging.rs b/kernel/src/arch/aarch64/paging.rs index 53b9045..c429a68 100644 --- a/kernel/src/arch/aarch64/paging.rs +++ b/kernel/src/arch/aarch64/paging.rs @@ -1,222 +1,297 @@ //! Page table implementations for aarch64. - +// Depends on kernel +use consts::{KERNEL_PML4, RECURSIVE_INDEX}; +use memory::{active_table, alloc_frame, alloc_stack, dealloc_frame}; use ucore_memory::memory_set::*; +use ucore_memory::PAGE_SIZE; use ucore_memory::paging::*; +use aarch64::asm::{tlb_invalidate, tlb_invalidate_all, flush_icache_all, ttbr_el1_read, ttbr_el1_write}; +use aarch64::{PhysAddr, VirtAddr}; +use aarch64::paging::{Mapper, PageTable as Aarch64PageTable, PageTableEntry, PageTableFlags as EF, RecursivePageTable}; +use aarch64::paging::{FrameAllocator, FrameDeallocator, Page, PhysFrame as Frame, Size4KiB, Size2MiB, Size1GiB}; +use aarch64::paging::memory_attribute::*; + +// need 3 page +pub fn setup_temp_page_table(frame_lvl4: Frame, frame_lvl3: Frame, frame_lvl2: Frame) { + let p4 = unsafe { &mut *(frame_lvl4.start_address().as_u64() as *mut Aarch64PageTable) }; + let p3 = unsafe { &mut *(frame_lvl3.start_address().as_u64() as *mut Aarch64PageTable) }; + let p2 = unsafe { &mut *(frame_lvl2.start_address().as_u64() as *mut Aarch64PageTable) }; + p4.zero(); + p3.zero(); + p2.zero(); + + let (start_addr, end_addr) = (0, 0x40000000); + let block_flags = EF::VALID | EF::AF | EF::WRITE | EF::XN; + for page in Page::::range_of(start_addr, end_addr) { + let paddr = PhysAddr::new(page.start_address().as_u64()); + + use arch::board::IO_REMAP_BASE; + if paddr.as_u64() >= IO_REMAP_BASE as u64 { + p2[page.p2_index()].set_block::(paddr, block_flags | EF::PXN, MairDevice::attr_value()); + } else { + p2[page.p2_index()].set_block::(paddr, block_flags, MairNormal::attr_value()); + } + } -type VirtAddr = usize; -type PhysAddr = usize; + p3[0].set_frame(frame_lvl2, EF::default(), MairNormal::attr_value()); + p3[1].set_block::(PhysAddr::new(0x40000000), block_flags | EF::PXN, MairDevice::attr_value()); -use alloc::alloc::{alloc, Layout}; -use crate::memory::{active_table, alloc_frame, dealloc_frame}; + p4[0].set_frame(frame_lvl3, EF::default(), MairNormal::attr_value()); + p4[RECURSIVE_INDEX].set_frame(frame_lvl4, EF::default(), MairNormal::attr_value()); -/// TODO -pub struct ActivePageTable { - // TODO + ttbr_el1_write(0, frame_lvl4); + tlb_invalidate_all(); } -impl ActivePageTable { - /// TODO - pub unsafe fn new() -> Self { - unimplemented!() - } +pub struct ActivePageTable(RecursivePageTable<'static>); - pub fn token() -> usize { - unimplemented!() - } -} +pub struct PageEntry(PageTableEntry); impl PageTable for ActivePageTable { type Entry = PageEntry; - fn map(&mut self, addr: VirtAddr, target: PhysAddr) -> &mut Self::Entry { - unimplemented!() + fn map(&mut self, addr: usize, target: usize) -> &mut PageEntry { + let flags = EF::default(); + let attr = MairNormal::attr_value(); + self.0.map_to(Page::of_addr(addr), Frame::of_addr(target), flags, attr, &mut FrameAllocatorForAarch64) + .unwrap().flush(); + self.get_entry(addr) } - fn unmap(&mut self, addr: VirtAddr) { - unimplemented!() + + fn unmap(&mut self, addr: usize) { + let (frame, flush) = self.0.unmap(Page::of_addr(addr)).unwrap(); + flush.flush(); } - fn get_entry(&mut self, addr: VirtAddr) -> Option<&mut Self::Entry> { - unimplemented!() + fn get_entry(&mut self, addr: usize) -> Option<&mut PageEntry> { + let entry_addr = ((addr >> 9) & 0o777_777_777_7770) | (RECURSIVE_INDEX << 39); + Some(unsafe { &mut *(entry_addr as *mut PageEntry) }) } - // For testing with mock - fn get_page_slice_mut<'a, 'b>(&'a mut self, addr: VirtAddr) -> &'b mut [u8] { - unimplemented!() + fn get_page_slice_mut<'a, 'b>(&'a mut self, addr: usize) -> &'b mut [u8] { + use core::slice; + unsafe { slice::from_raw_parts_mut((addr & !0xfffusize) as *mut u8, PAGE_SIZE) } } - fn read(&mut self, addr: VirtAddr) -> u8 { - unimplemented!() + fn read(&mut self, addr: usize) -> u8 { + unsafe { *(addr as *const u8) } } - fn write(&mut self, addr: VirtAddr, data: u8) { - unimplemented!() + fn write(&mut self, addr: usize, data: u8) { + unsafe { *(addr as *mut u8) = data; } } } -/// TODO -pub struct PageEntry { - // TODO +const ROOT_PAGE_TABLE: *mut Aarch64PageTable = + ((RECURSIVE_INDEX << 39) | (RECURSIVE_INDEX << 30) | (RECURSIVE_INDEX << 21) | (RECURSIVE_INDEX << 12)) as *mut Aarch64PageTable; + +impl ActivePageTable { + pub unsafe fn new() -> Self { + ActivePageTable(RecursivePageTable::new(&mut *(ROOT_PAGE_TABLE as *mut _)).unwrap()) + } + fn with_temporary_map(&mut self, frame: &Frame, f: impl FnOnce(&mut ActivePageTable, &mut Aarch64PageTable)) { + // Create a temporary page + let page = Page::of_addr(0xcafebabe); + assert!(self.0.translate_page(page).is_none(), "temporary page is already mapped"); + // Map it to table + self.map(page.start_address().as_u64() as usize, frame.start_address().as_u64() as usize); + // Call f + let table = unsafe { &mut *page.start_address().as_mut_ptr() }; + f(self, table); + // Unmap the page + self.unmap(0xcafebabe); + } } impl Entry for PageEntry { - /// IMPORTANT! - /// This must be called after any change to ensure it become effective. - /// Usually this will make a flush to TLB/MMU. fn update(&mut self) { - unimplemented!() - } - - /// Will be set when accessed - fn accessed(&self) -> bool { - unimplemented!() - } - - /// Will be set when written - fn dirty(&self) -> bool { - unimplemented!() - } - - /// Will PageFault when try to write page where writable=0 - fn writable(&self) -> bool { - unimplemented!() + let addr = VirtAddr::new_unchecked((self as *const _ as u64) << 9); + tlb_invalidate(addr); } - /// Will PageFault when try to access page where present=0 - fn present(&self) -> bool { - unimplemented!() - } - - fn clear_accessed(&mut self) { - unimplemented!() - } - - fn clear_dirty(&mut self) { - unimplemented!() - } + fn present(&self) -> bool { self.0.flags().contains(EF::VALID) } + fn accessed(&self) -> bool { self.0.flags().contains(EF::AF) } + fn writable(&self) -> bool { self.0.flags().contains(EF::WRITE) } + fn dirty(&self) -> bool { self.hw_dirty() && self.sw_dirty() } - fn set_writable(&mut self, value: bool) { - unimplemented!() + fn clear_accessed(&mut self) { self.as_flags().remove(EF::AF); } + fn clear_dirty(&mut self) + { + self.as_flags().remove(EF::DIRTY); + self.as_flags().insert(EF::AP_RO); } - - fn set_present(&mut self, value: bool) { - unimplemented!() + fn set_writable(&mut self, value: bool) + { + self.as_flags().set(EF::AP_RO, !value); + self.as_flags().set(EF::WRITE, value); } - - fn target(&self) -> PhysAddr { - unimplemented!() + fn set_present(&mut self, value: bool) { self.as_flags().set(EF::VALID, value); } + fn target(&self) -> usize { self.0.addr().as_u64() as usize } + fn set_target(&mut self, target: usize) { + self.0.modify_addr(PhysAddr::new(target as u64)); } - - fn set_target(&mut self, target: PhysAddr) { - unimplemented!() - } - - // For Copy-on-write extension - fn writable_shared(&self) -> bool { - unimplemented!() - } - - fn readonly_shared(&self) -> bool { - unimplemented!() - } - + fn writable_shared(&self) -> bool { self.0.flags().contains(EF::WRITABLE_SHARED) } + fn readonly_shared(&self) -> bool { self.0.flags().contains(EF::READONLY_SHARED) } fn set_shared(&mut self, writable: bool) { - unimplemented!() - } - - fn clear_shared(&mut self) { - unimplemented!() - } - - // For Swap extension - fn swapped(&self) -> bool { - unimplemented!() - } - - fn set_swapped(&mut self, value: bool) { - unimplemented!() - } - - fn user(&self) -> bool { - unimplemented!() - } - + let flags = self.as_flags(); + flags.set(EF::WRITABLE_SHARED, writable); + flags.set(EF::READONLY_SHARED, !writable); + } + fn clear_shared(&mut self) { self.as_flags().remove(EF::WRITABLE_SHARED | EF::READONLY_SHARED); } + fn user(&self) -> bool { self.0.flags().contains(EF::AP_EL0) } + fn swapped(&self) -> bool { self.0.flags().contains(EF::SWAPPED) } + fn set_swapped(&mut self, value: bool) { self.as_flags().set(EF::SWAPPED, value); } fn set_user(&mut self, value: bool) { - unimplemented!() + self.as_flags().set(EF::AP_EL0, value); + self.as_flags().set(EF::nG, value); // set non-global to use ASID } - fn execute(&self) -> bool { - unimplemented!() + if self.user() { + !self.0.flags().contains(EF::XN) + } else { + !self.0.flags().contains(EF::PXN) + } } - fn set_execute(&mut self, value: bool) { - unimplemented!() - } -} - -#[derive(Debug, Clone, PartialEq, Eq, PartialOrd, Ord)] -pub struct MockFrame(PhysAddr); - -impl MockFrame { - pub fn of_addr(addr: PhysAddr) -> Self { - MockFrame(addr) - } - pub fn start_address(&self) -> PhysAddr { - unimplemented!() + if self.user() { + self.as_flags().set(EF::XN, !value) + } else { + self.as_flags().set(EF::PXN, !value) + } } - pub fn p2_index(&self) -> usize { - unimplemented!() + fn mmio(&self) -> bool { + self.0.attr().value == MairDevice::attr_value().value } - pub fn p1_index(&self) -> usize { - unimplemented!() + fn set_mmio(&mut self, value: bool) { + if value { + self.0.modify_attr(MairDevice::attr_value()) + } else { + self.0.modify_attr(MairNormal::attr_value()) + } } - pub fn number(&self) -> usize { - unimplemented!() +} + +impl PageEntry { + fn read_only(&self) -> bool { self.0.flags().contains(EF::AP_RO) } + fn hw_dirty(&self) -> bool { self.writable() && !self.read_only() } + fn sw_dirty(&self) -> bool { self.0.flags().contains(EF::DIRTY) } + fn as_flags(&mut self) -> &mut EF { + unsafe { &mut *(self as *mut _ as *mut EF) } } } -/// TODO +#[derive(Debug)] pub struct InactivePageTable0 { - p4_frame: MockFrame, + p4_frame: Frame, } -/// TODO impl InactivePageTable for InactivePageTable0 { type Active = ActivePageTable; fn new() -> Self { - unsafe { - let layout = Layout::new::(); - let ptr = alloc(layout); - let frame = MockFrame::of_addr(*ptr as usize); - InactivePageTable0 { p4_frame: frame } - } + // When the new InactivePageTable is created for the user MemorySet, it's use ttbr1 as the + // TTBR. And the kernel TTBR ttbr0 will never changed, so we needn't call map_kernel() + Self::new_bare() } fn new_bare() -> Self { - unimplemented!() + let frame = Self::alloc_frame().map(|target| Frame::of_addr(target)) + .expect("failed to allocate frame"); + active_table().with_temporary_map(&frame, |_, table: &mut Aarch64PageTable| { + table.zero(); + // set up recursive mapping for the table + table[RECURSIVE_INDEX].set_frame(frame.clone(), EF::default(), MairNormal::attr_value()); + }); + InactivePageTable0 { p4_frame: frame } } fn edit(&mut self, f: impl FnOnce(&mut Self::Active)) { - unimplemented!() + active_table().with_temporary_map(&ttbr_el1_read(0), |active_table, p4_table: &mut Aarch64PageTable| { + let backup = p4_table[RECURSIVE_INDEX].clone(); + + // overwrite recursive mapping + p4_table[RECURSIVE_INDEX].set_frame(self.p4_frame.clone(), EF::default(), MairNormal::attr_value()); + tlb_invalidate_all(); + + // execute f in the new context + f(active_table); + + // restore recursive mapping to original p4 table + p4_table[RECURSIVE_INDEX] = backup; + tlb_invalidate_all(); + }); } unsafe fn activate(&self) { - unimplemented!() + let old_frame = ttbr_el1_read(0); + let new_frame = self.p4_frame.clone(); + debug!("switch TTBR0 {:?} -> {:?}", old_frame, new_frame); + if old_frame != new_frame { + ttbr_el1_write(0, new_frame); + tlb_invalidate_all(); + } } unsafe fn with(&self, f: impl FnOnce() -> T) -> T { - unimplemented!() + // Just need to switch the user TTBR + let old_frame = ttbr_el1_read(1); + let new_frame = self.p4_frame.clone(); + debug!("switch TTBR1 {:?} -> {:?}", old_frame, new_frame); + if old_frame != new_frame { + ttbr_el1_write(1, new_frame); + tlb_invalidate_all(); + } + f(); + debug!("switch TTBR1 {:?} -> {:?}", new_frame, old_frame); + if old_frame != new_frame { + ttbr_el1_write(1, old_frame); + tlb_invalidate_all(); + flush_icache_all(); + } } fn token(&self) -> usize { - 0 + self.p4_frame.start_address().as_u64() as usize // as TTBRx_EL1 } - fn alloc_frame() -> Option { + fn alloc_frame() -> Option { alloc_frame() } - fn dealloc_frame(target: PhysAddr) { + fn dealloc_frame(target: usize) { dealloc_frame(target) } } + +impl InactivePageTable0 { + fn map_kernel(&mut self) { + let table = unsafe { &mut *ROOT_PAGE_TABLE }; + let e0 = table[KERNEL_PML4].clone(); + assert!(!e0.is_unused()); + + self.edit(|_| { + table[KERNEL_PML4].set_frame(Frame::containing_address(e0.addr()), EF::default(), MairNormal::attr_value()); + }); + } +} + +impl Drop for InactivePageTable0 { + fn drop(&mut self) { + info!("PageTable dropping: {:?}", self); + Self::dealloc_frame(self.p4_frame.start_address().as_u64() as usize); + } +} + +struct FrameAllocatorForAarch64; + +impl FrameAllocator for FrameAllocatorForAarch64 { + fn alloc(&mut self) -> Option { + alloc_frame().map(|addr| Frame::of_addr(addr)) + } +} + +impl FrameDeallocator for FrameAllocatorForAarch64 { + fn dealloc(&mut self, frame: Frame) { + dealloc_frame(frame.start_address().as_u64() as usize); + } +} diff --git a/kernel/src/arch/riscv32/paging.rs b/kernel/src/arch/riscv32/paging.rs index b4d1744..2f2c0f1 100644 --- a/kernel/src/arch/riscv32/paging.rs +++ b/kernel/src/arch/riscv32/paging.rs @@ -200,6 +200,8 @@ impl Entry for PageEntry { fn set_user(&mut self, value: bool) { self.as_flags().set(EF::USER, value); } fn execute(&self) -> bool { self.0.flags().contains(EF::EXECUTABLE) } fn set_execute(&mut self, value: bool) { self.as_flags().set(EF::EXECUTABLE, value); } + fn mmio(&self) -> bool { unimplemented!() } + fn set_mmio(&mut self, value: bool) { unimplemented!() } } impl PageEntry { diff --git a/kernel/src/arch/x86_64/paging.rs b/kernel/src/arch/x86_64/paging.rs index 4799482..5c5c896 100644 --- a/kernel/src/arch/x86_64/paging.rs +++ b/kernel/src/arch/x86_64/paging.rs @@ -145,6 +145,8 @@ impl Entry for PageEntry { } fn execute(&self) -> bool { !self.0.flags().contains(EF::NO_EXECUTE) } fn set_execute(&mut self, value: bool) { self.as_flags().set(EF::NO_EXECUTE, !value); } + fn mmio(&self) -> bool { unimplemented!() } + fn set_mmio(&mut self, value: bool) { unimplemented!() } } fn get_entry_ptr(addr: usize, level: u8) -> *mut PageEntry { @@ -271,4 +273,4 @@ impl FrameDeallocator for FrameAllocatorForX86 { fn dealloc(&mut self, frame: Frame) { dealloc_frame(frame.start_address().as_u64() as usize); } -} \ No newline at end of file +} diff --git a/kernel/src/fs.rs b/kernel/src/fs.rs index 7ef2208..e0fc3d9 100644 --- a/kernel/src/fs.rs +++ b/kernel/src/fs.rs @@ -20,6 +20,18 @@ _user_img_start: _user_img_end: "#); +// Hard link user program +#[cfg(target_arch = "aarch64")] +global_asm!(r#" + .section .rodata + .align 12 + .global _user_img_start + .global _user_img_end +_user_img_start: + .incbin "../user/user-aarch64.img" +_user_img_end: +"#); + lazy_static! { pub static ref ROOT_INODE: Arc = { #[cfg(target_arch = "riscv32")] diff --git a/kernel/src/memory.rs b/kernel/src/memory.rs index 27b85fe..e0a209a 100644 --- a/kernel/src/memory.rs +++ b/kernel/src/memory.rs @@ -1,5 +1,5 @@ pub use crate::arch::paging::*; -use bit_allocator::{BitAlloc, BitAlloc4K, BitAlloc64K}; +use bit_allocator::{BitAlloc, BitAlloc4K, BitAlloc64K, BitAlloc1M}; use crate::consts::MEMORY_OFFSET; use super::HEAP_ALLOCATOR; use ucore_memory::{*, paging::PageTable}; @@ -29,7 +29,7 @@ pub type FrameAlloc = BitAlloc4K; // Raspberry Pi 3 has 1G memory #[cfg(target_arch = "aarch64")] -pub type FrameAlloc = BitAlloc64K; +pub type FrameAlloc = BitAlloc1M; lazy_static! { pub static ref FRAME_ALLOCATOR: SpinNoIrqLock = SpinNoIrqLock::new(FrameAlloc::default()); @@ -170,4 +170,4 @@ pub fn page_fault_handler(_addr: usize) -> bool { // use ucore_memory::cow::test::test_with; // test_with(&mut active_table()); // } -//} \ No newline at end of file +//} diff --git a/kernel/src/process/context.rs b/kernel/src/process/context.rs index f5d7587..82d0ed4 100644 --- a/kernel/src/process/context.rs +++ b/kernel/src/process/context.rs @@ -51,20 +51,6 @@ impl ContextImpl { }) } - /// Temp for aarch64 - pub fn new_user_test(entry: extern fn(usize) -> !) -> Box { - let memory_set = MemorySet::new(); - let kstack = KernelStack::new(); - let ustack = KernelStack::new(); - Box::new(ContextImpl { - arch: unsafe { ArchContext::new_user_thread(entry as usize, ustack.top(), kstack.top(), false, memory_set.token()) }, - memory_set, - kstack, - files: BTreeMap::default(), - cwd: String::new(), - }) - } - /// Make a new user thread from ELF data pub fn new_user<'a, Iter>(data: &[u8], args: Iter) -> Box where Iter: Iterator @@ -228,6 +214,10 @@ fn memory_set_from<'a>(elf: &'a ElfFile<'a>) -> (MemorySet, usize) { let offset = ph.offset() as usize; let file_size = ph.file_size() as usize; let mem_size = ph.mem_size() as usize; + + #[cfg(target_arch = "aarch64")] + assert_eq!((virt_addr >> 48), 0xffff, "Segment Fault"); + // Get target slice #[cfg(feature = "no_mmu")] let target = ms.push(mem_size); diff --git a/tools/raspi-firmware/config.txt b/tools/raspi-firmware/config.txt new file mode 100755 index 0000000..89ebfb4 --- /dev/null +++ b/tools/raspi-firmware/config.txt @@ -0,0 +1,59 @@ +# For more options and information see +# http://rpf.io/configtxt +# Some settings may impact device functionality. See link above for details + +# uncomment if you get no picture on HDMI for a default "safe" mode +#hdmi_safe=1 + +# uncomment this if your display has a black border of unused pixels visible +# and your display can output without overscan +#disable_overscan=1 + +# uncomment the following to adjust overscan. Use positive numbers if console +# goes off screen, and negative if there is too much border +#overscan_left=16 +#overscan_right=16 +#overscan_top=16 +#overscan_bottom=16 + +# uncomment to force a console size. By default it will be display's size minus +# overscan. +#framebuffer_width=1280 +#framebuffer_height=720 + +# uncomment if hdmi display is not detected and composite is being output +#hdmi_force_hotplug=1 + +# uncomment to force a specific HDMI mode (this will force VGA) +#hdmi_group=1 +#hdmi_mode=1 + +# uncomment to force a HDMI mode rather than DVI. This can make audio work in +# DMT (computer monitor) modes +#hdmi_drive=2 + +# uncomment to increase signal to HDMI, if you have interference, blanking, or +# no display +#config_hdmi_boost=4 + +# uncomment for composite PAL +#sdtv_mode=2 + +#uncomment to overclock the arm. 700 MHz is the default. +#arm_freq=800 + +# Uncomment some or all of these to enable the optional hardware interfaces +#dtparam=i2c_arm=on +#dtparam=i2s=on +#dtparam=spi=on + +# Uncomment this to enable the lirc-rpi module +#dtoverlay=lirc-rpi + +# Additional overlays and parameters are documented /boot/overlays/README + +# Enable audio (loads snd_bcm2835) +dtparam=audio=on + +kernel=kernel8.img +device_tree= diff --git a/tools/riscv-pk/dummy_payload/dummy_payload.ac b/tools/riscv-pk/dummy_payload/dummy_payload.ac new file mode 100644 index 0000000..e69de29 diff --git a/tools/riscv-pk/dummy_payload/dummy_payload.c b/tools/riscv-pk/dummy_payload/dummy_payload.c new file mode 100644 index 0000000..e69de29 diff --git a/tools/riscv-pk/util/util.ac b/tools/riscv-pk/util/util.ac new file mode 100644 index 0000000..e69de29 diff --git a/user/user-aarch64.img b/user/user-aarch64.img new file mode 100644 index 0000000..ba673f0 Binary files /dev/null and b/user/user-aarch64.img differ