From afc3af54fa86d7f63806def8e7ee5aa23f97be5e Mon Sep 17 00:00:00 2001 From: Mark Poliakov Date: Tue, 8 Aug 2023 20:00:24 +0300 Subject: [PATCH] aarch64: initial support for Orange Pi 3 --- Cargo.toml | 14 +- src/arch/aarch64/boot/entry.S | 77 +++++++++++ src/arch/aarch64/boot/mod.rs | 95 +++++-------- src/arch/aarch64/devtree.rs | 22 ++- src/arch/aarch64/mod.rs | 33 ++++- src/arch/aarch64/plat_orange_pi3/mod.rs | 99 +++++++++++++ src/arch/aarch64/plat_orange_pi3/r_wdog.rs | 78 +++++++++++ src/arch/aarch64/plat_orange_pi3/serial.rs | 153 +++++++++++++++++++++ src/arch/aarch64/plat_qemu/mod.rs | 12 +- src/arch/aarch64/timer.rs | 4 +- src/arch/mod.rs | 7 +- src/device/mod.rs | 1 + src/device/platform.rs | 2 + src/device/serial/mod.rs | 2 +- src/device/serial/pl011.rs | 2 +- src/device/tty.rs | 144 ++++++++++--------- src/main.rs | 15 +- src/mem/mod.rs | 71 +++++++++- src/panic.rs | 22 +-- src/util.rs | 2 +- 20 files changed, 687 insertions(+), 168 deletions(-) create mode 100644 src/arch/aarch64/boot/entry.S create mode 100644 src/arch/aarch64/plat_orange_pi3/mod.rs create mode 100644 src/arch/aarch64/plat_orange_pi3/r_wdog.rs create mode 100644 src/arch/aarch64/plat_orange_pi3/serial.rs diff --git a/Cargo.toml b/Cargo.toml index 62264045..bcb5f89a 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -18,10 +18,13 @@ spinning_top = "0.2.5" # static_assertions = "1.1.0" tock-registers = "0.8.1" cfg-if = "1.0.0" -bitmap-font = { version = "0.3.0" } -embedded-graphics = "0.8.0" git-version = "0.3.5" +aarch64-cpu = "9.3.1" + +bitmap-font = { version = "0.3.0", optional = true } +embedded-graphics = { version = "0.8.0", optional = true } + [dependencies.elf] version = "0.7.2" git = "https://git.alnyan.me/yggdrasil/yggdrasil-elf.git" @@ -30,9 +33,14 @@ features = ["no_std_stream"] [target.'cfg(target_arch = "aarch64")'.dependencies] fdt-rs = { version = "0.4.3", default-features = false } -aarch64-cpu = "9.3.1" [target.'cfg(target_arch = "x86_64")'.dependencies] yboot-proto = { git = "https://git.alnyan.me/yggdrasil/yboot-proto.git" } aml = "0.16.4" acpi = "4.1.1" + +[features] +default = [] +fb_console = ["bitmap-font", "embedded-graphics"] +aarch64_qemu = [] +aarch64_orange_pi3 = [] diff --git a/src/arch/aarch64/boot/entry.S b/src/arch/aarch64/boot/entry.S new file mode 100644 index 00000000..8bf12667 --- /dev/null +++ b/src/arch/aarch64/boot/entry.S @@ -0,0 +1,77 @@ +.set CNTHCTL_EL2_EL1PCTEN, 1 << 0 +.set CNTHCTL_EL2_EL1PCEN, 1 << 1 + +.set HCR_EL2_RW_EL1IsAArch64, 1 << 31 + +.set SCTLR_EL2_RES1, 0x30C50830 + +.set SPSR_EL2_EL1h, 0x5 +.set SPSR_EL2_MASK_DAIF, 0xF << 6 + +.macro MOV_L reg, value + mov \reg, #((\value) & 0xFFFF) + movk \reg, #((\value) >> 16), lsl #16 +.endm + +.global __aarch64_entry +.section .text.entry +__aarch64_entry: + // x0 -- dtb_phys + + // Multiple processor cores may or may not be running at this point + mrs x1, mpidr_el1 + ands x1, x1, #0xF + bne 1f + + mrs x8, CurrentEL + lsr x8, x8, #2 + cmp x8, #2 + bne .el1 + + // Leave EL2 +.el2: + mrs x8, CNTHCTL_EL2 + orr x8, x8, #(CNTHCTL_EL2_EL1PCTEN | CNTHCTL_EL2_EL1PCEN) + msr CNTHCTL_EL2, x8 + msr CNTVOFF_EL2, xzr + + MOV_L x8, SCTLR_EL2_RES1 + msr SCTLR_EL2, x8 + + mov x8, #HCR_EL2_RW_EL1IsAArch64 + msr HCR_EL2, x8 + + mov x8, #SPSR_EL2_EL1h + orr x8, x8, #SPSR_EL2_MASK_DAIF + msr SPSR_EL2, x8 + + adr x8, .el1 + msr ELR_EL2, x8 + + isb + eret +.el1: + dsb sy + isb + + // Zero .bss + adr x8, __bss_start_phys + adr x9, __bss_end_phys +1: + cmp x8, x9 + beq 2f + strb wzr, [x8] + add x8, x8, #1 + b 1b +2: + + // BSP in SMP or uniprocessor + ldr x1, ={stack_bottom} + {stack_size} - {kernel_virt_offset} + mov sp, x1 + + bl {kernel_lower_entry} - {kernel_virt_offset} + + // AP in a SMP system + // TODO spin loop for this method of init +1: + b . diff --git a/src/arch/aarch64/boot/mod.rs b/src/arch/aarch64/boot/mod.rs index 87130ca7..df2b33ea 100644 --- a/src/arch/aarch64/boot/mod.rs +++ b/src/arch/aarch64/boot/mod.rs @@ -1,8 +1,17 @@ //! Main entry point for the AArch64 platforms -use core::{arch::asm, sync::atomic::Ordering}; +use core::{ + arch::{asm, global_asm}, + sync::atomic::Ordering, +}; -use aarch64_cpu::registers::{CurrentEL, CPACR_EL1}; -use tock_registers::interfaces::{ReadWriteable, Readable}; +use aarch64_cpu::{ + asm::barrier, + registers::{ + CurrentEL, CNTHCTL_EL2, CNTVOFF_EL2, CPACR_EL1, ELR_EL2, HCR_EL2, SCTLR_EL2, SPSR_EL2, + SP_EL1, + }, +}; +use tock_registers::interfaces::{ReadWriteable, Readable, Writeable}; use super::{ cpu::Cpu, exception, kernel_main, smp::CPU_COUNT, AArch64, KernelStack, ARCHITECTURE, @@ -19,13 +28,20 @@ use crate::{ pub(super) static CPU_INIT_FENCE: SpinFence = SpinFence::new(); -fn __aarch64_common_lower_entry() { +extern "C" fn el1_bsp_lower_entry(dtb_phys: usize) -> ! { // Unmask FP operations CPACR_EL1.modify(CPACR_EL1::FPEN::TrapNothing); - if CurrentEL.read(CurrentEL::EL) != 1 { - panic!("Only EL1 is supported for now"); + unsafe { + ARCHITECTURE.init_mmu(true); } + + let sp = unsafe { BSP_STACK.data.as_ptr().add(BOOT_STACK_SIZE).virtualize() }; + let elr = absolute_address!(__aarch64_bsp_upper_entry); + + barrier::dsb(barrier::SY); + barrier::isb(barrier::SY); + enter_higher_half(sp as usize, elr, dtb_phys); } fn enter_higher_half(sp: usize, elr: usize, arg: usize) -> ! { @@ -39,27 +55,16 @@ fn enter_higher_half(sp: usize, elr: usize, arg: usize) -> ! { } pub(super) extern "C" fn __aarch64_ap_lower_entry(sp: usize) -> ! { - __aarch64_common_lower_entry(); + loop {} + // __aarch64_common_lower_entry(); - unsafe { - ARCHITECTURE.init_mmu(false); - } + // unsafe { + // ARCHITECTURE.init_mmu(false); + // } - let sp = unsafe { sp.virtualize() }; - let elr = absolute_address!(__aarch64_ap_upper_entry); - enter_higher_half(sp, elr, 0); -} - -extern "C" fn __aarch64_bsp_lower_entry(dtb_phys: usize) -> ! { - __aarch64_common_lower_entry(); - - unsafe { - ARCHITECTURE.init_mmu(true); - } - - let sp = unsafe { BSP_STACK.data.as_ptr().add(BOOT_STACK_SIZE).virtualize() }; - let elr = absolute_address!(__aarch64_bsp_upper_entry); - enter_higher_half(sp as usize, elr, dtb_phys); + // let sp = unsafe { sp.virtualize() }; + // let elr = absolute_address!(__aarch64_ap_upper_entry); + // enter_higher_half(sp, elr, 0); } extern "C" fn __aarch64_bsp_upper_entry(dtb_phys: usize) -> ! { @@ -91,38 +96,14 @@ extern "C" fn __aarch64_ap_upper_entry(_x0: usize) -> ! { } } -#[link_section = ".text.entry"] -#[no_mangle] -#[naked] -unsafe extern "C" fn __aarch64_entry() -> ! { - // Setup the stack and pass on to a proper function - asm!( - r#" - // Multiple processor cores may or may not be running at this point - mrs x1, mpidr_el1 - ands x1, x1, #0xF - bne 1f - - // BSP in SMP or uniprocessor - ldr x1, ={stack_bottom} + {stack_size} - {kernel_virt_offset} - mov sp, x1 - - bl {kernel_lower_entry} - {kernel_virt_offset} - - // AP in a SMP system - // TODO spin loop for this method of init -1: - b . -"#, - kernel_lower_entry = sym __aarch64_bsp_lower_entry, - stack_bottom = sym BSP_STACK, - stack_size = const BOOT_STACK_SIZE, - kernel_virt_offset = const KERNEL_VIRT_OFFSET, - options(noreturn) - ); -} - -#[link_section = ".bss"] static BSP_STACK: KernelStack = KernelStack { data: [0; BOOT_STACK_SIZE], }; + +global_asm!( + include_str!("entry.S"), + kernel_lower_entry = sym el1_bsp_lower_entry, + stack_bottom = sym BSP_STACK, + stack_size = const BOOT_STACK_SIZE, + kernel_virt_offset = const KERNEL_VIRT_OFFSET +); diff --git a/src/arch/aarch64/devtree.rs b/src/arch/aarch64/devtree.rs index 3f39a8d0..1058ebde 100644 --- a/src/arch/aarch64/devtree.rs +++ b/src/arch/aarch64/devtree.rs @@ -7,14 +7,16 @@ use fdt_rs::{ use crate::{debug::LogLevel, mem::phys::PhysicalMemoryRegion}; +const INDEX_BUFFER_SIZE: usize = 65536; + #[repr(C, align(0x10))] -struct FdtIndexBuffer([u8; 32768]); +struct FdtIndexBuffer([u8; INDEX_BUFFER_SIZE]); static mut FDT_INDEX_BUFFER: FdtIndexBuffer = FdtIndexBuffer::zeroed(); impl FdtIndexBuffer { const fn zeroed() -> Self { - Self([0; 32768]) + Self([0; INDEX_BUFFER_SIZE]) } } @@ -80,16 +82,24 @@ impl Iterator for FdtMemoryRegionIter<'_> { break None; }; - if item.name().unwrap_or("").starts_with("memory@") { + let name = item.name().unwrap_or(""); + + if name.starts_with("memory@") || name == "memory" { let reg = item .props() .find(|p| p.name().unwrap_or("") == "reg") .unwrap(); + #[cfg(not(feature = "aarch64_orange_pi3"))] break Some(PhysicalMemoryRegion { base: reg.u64(0).unwrap() as usize, size: reg.u64(1).unwrap() as usize, }); + #[cfg(feature = "aarch64_orange_pi3")] + break Some(PhysicalMemoryRegion { + base: reg.u32(0).unwrap() as usize, + size: reg.u32(1).unwrap() as usize, + }); } } } @@ -141,11 +151,13 @@ fn dump_node(node: &TNode, depth: usize, level: LogLevel) { log_print_raw!(level, "{:?} {{\n", node_name); for prop in node.props() { indent(level, depth + 1); - let name = prop.name().unwrap(); + let name = prop.name().unwrap_or(""); log_print_raw!(level, "{name:?} = "); match name { - "compatible" | "stdout-path" => log_print_raw!(level, "{:?}", prop.str().unwrap()), + "compatible" | "stdout-path" => { + log_print_raw!(level, "{:?}", prop.str().unwrap_or("")) + } _ => log_print_raw!(level, "{:x?}", prop.raw()), } diff --git a/src/arch/aarch64/mod.rs b/src/arch/aarch64/mod.rs index 180dbdcf..3a3cef2c 100644 --- a/src/arch/aarch64/mod.rs +++ b/src/arch/aarch64/mod.rs @@ -4,8 +4,8 @@ use core::sync::atomic::Ordering; use aarch64_cpu::registers::{DAIF, ID_AA64MMFR0_EL1, SCTLR_EL1, TCR_EL1, TTBR0_EL1, TTBR1_EL1}; use abi::error::Error; +use cfg_if::cfg_if; use fdt_rs::prelude::PropReader; -use plat_qemu::PLATFORM; use tock_registers::interfaces::{ReadWriteable, Readable, Writeable}; use crate::{ @@ -30,7 +30,19 @@ use self::{ table::{init_fixed_tables, KERNEL_TABLES}, }; -pub mod plat_qemu; +pub mod plat_orange_pi3; +cfg_if! { + if #[cfg(feature = "aarch64_qemu")] { + pub mod plat_qemu; + + pub use plat_qemu::{PlatformImpl, PLATFORM}; + } else if #[cfg(feature = "aarch64_orange_pi3")] { + + pub use plat_orange_pi3::{PlatformImpl, PLATFORM}; + } else { + compile_error!("No platform selected for AArch64"); + } +} pub mod boot; pub mod context; @@ -156,6 +168,7 @@ impl AArch64 { ); let regions = FdtMemoryRegionIter::new(dt); + phys::init_from_iter(regions) } } @@ -173,7 +186,14 @@ fn setup_initrd() { return; }; + #[cfg(feature = "aarch64_orange_pi3")] + let initrd_start = initrd_start.u32(0).unwrap() as usize; + #[cfg(feature = "aarch64_orange_pi3")] + let initrd_end = initrd_end.u32(0).unwrap() as usize; + + #[cfg(not(feature = "aarch64_orange_pi3"))] let initrd_start = initrd_start.u64(0).unwrap() as usize; + #[cfg(not(feature = "aarch64_orange_pi3"))] let initrd_end = initrd_end.u64(0).unwrap() as usize; let start_aligned = initrd_start & !0xFFF; @@ -206,8 +226,8 @@ pub fn kernel_main(dtb_phys: usize) -> ! { unsafe { AArch64::set_interrupt_mask(true); - ARCHITECTURE.init_device_tree(dtb_phys); PLATFORM.init_debug(); + ARCHITECTURE.init_device_tree(dtb_phys); } debug::reset(); @@ -217,6 +237,7 @@ pub fn kernel_main(dtb_phys: usize) -> ! { setup_initrd(); debugln!("Initializing {} platform", PLATFORM.name()); + unsafe { ARCHITECTURE .init_physical_memory(dtb_phys) @@ -240,10 +261,10 @@ pub fn kernel_main(dtb_phys: usize) -> ! { // ); // } - Cpu::init_ipi_queues(); + // Cpu::init_ipi_queues(); - CPU_INIT_FENCE.signal(); - CPU_INIT_FENCE.wait_all(CPU_COUNT.load(Ordering::Acquire)); + // CPU_INIT_FENCE.signal(); + // CPU_INIT_FENCE.wait_all(CPU_COUNT.load(Ordering::Acquire)); task::init().expect("Failed to initialize the scheduler"); diff --git a/src/arch/aarch64/plat_orange_pi3/mod.rs b/src/arch/aarch64/plat_orange_pi3/mod.rs new file mode 100644 index 00000000..7d2eb9df --- /dev/null +++ b/src/arch/aarch64/plat_orange_pi3/mod.rs @@ -0,0 +1,99 @@ +/// +/// # Booting using u-boot +/// +/// > fatload mmc 0:1 0x40000000 uRamdisk +/// > fatload mmc 0:1 0x4d000000 sun50i-h6-orangepi-3.dtb +/// > loady 0x44000000 +/// ... +/// > bootm 0x44000000 0x40000000 0x4d000000 +/// +use abi::error::Error; + +use crate::{ + arch::CpuMessage, + debug::{self, LogLevel}, + device::{ + interrupt::{ExternalInterruptController, InterruptSource, IpiDeliveryTarget}, + platform::Platform, + timer::TimestampSource, + InitializableDevice, + }, + fs::devfs::{self, CharDeviceType}, +}; + +use self::{r_wdog::RWatchdog, serial::Serial}; + +use super::{ + gic::{Gic, IrqNumber}, + timer::ArmTimer, +}; + +pub mod r_wdog; +pub mod serial; + +pub struct PlatformImpl { + uart0: Serial, + r_wdog: RWatchdog, + pub gic: Gic, + timer: ArmTimer, +} + +impl Platform for PlatformImpl { + type IrqNumber = IrqNumber; + + const KERNEL_PHYS_BASE: usize = 0x50000000; + + unsafe fn init(&'static self, is_bsp: bool) -> Result<(), Error> { + if is_bsp { + self.gic.init(())?; + + self.timer.init(())?; + self.r_wdog.init(())?; + + self.timer.init_irq()?; + self.uart0.init_irq()?; + + devfs::add_char_device(&self.uart0, CharDeviceType::TtySerial)?; + } else { + todo!(); + } + + Ok(()) + } + + unsafe fn init_debug(&'static self) { + self.uart0.init(()).unwrap(); + debug::add_sink(&self.uart0, LogLevel::Debug); + } + + fn interrupt_controller( + &self, + ) -> &dyn ExternalInterruptController { + &self.gic + } + + fn timestamp_source(&self) -> &dyn TimestampSource { + &self.timer + } + + unsafe fn send_ipi(&self, target: IpiDeliveryTarget, _msg: CpuMessage) -> Result<(), Error> { + todo!(); + } + + fn name(&self) -> &'static str { + "Orange Pi 3" + } + + unsafe fn reset(&self) -> ! { + self.r_wdog.reset_board(); + } +} + +pub static PLATFORM: PlatformImpl = unsafe { + PlatformImpl { + uart0: Serial::new(0x05000000, IrqNumber::new(32)), + r_wdog: RWatchdog::new(0x07020400), + gic: Gic::new(0x03021000, 0x03022000), + timer: ArmTimer::new(IrqNumber::new(30)), + } +}; diff --git a/src/arch/aarch64/plat_orange_pi3/r_wdog.rs b/src/arch/aarch64/plat_orange_pi3/r_wdog.rs new file mode 100644 index 00000000..4f86a935 --- /dev/null +++ b/src/arch/aarch64/plat_orange_pi3/r_wdog.rs @@ -0,0 +1,78 @@ +use abi::error::Error; +use tock_registers::{ + interfaces::Writeable, register_bitfields, register_structs, registers::ReadWrite, +}; + +use crate::{ + device::InitializableDevice, mem::device::DeviceMemoryIo, sync::IrqSafeSpinlock, + util::OneTimeInit, +}; + +register_bitfields! { + u32, + CTRL [ + KEY OFFSET(1) NUMBITS(12) [ + Value = 0xA57 + ], + RESTART OFFSET(0) NUMBITS(1) [] + ], + CFG [ + CONFIG OFFSET(0) NUMBITS(2) [ + System = 1, + ] + ], + MODE [ + EN OFFSET(0) NUMBITS(1) [], + ] +} + +register_structs! { + #[allow(non_snake_case)] + Regs { + (0x00 => IRQ_EN: ReadWrite), + (0x04 => IRQ_STA: ReadWrite), + (0x08 => _0), + (0x10 => CTRL: ReadWrite), + (0x14 => CFG: ReadWrite), + (0x18 => MODE: ReadWrite), + (0x1C => @END), + } +} + +pub struct RWatchdog { + inner: OneTimeInit>>, + base: usize, +} + +impl InitializableDevice for RWatchdog { + type Options = (); + + unsafe fn init(&self, opts: Self::Options) -> Result<(), Error> { + let regs = DeviceMemoryIo::map("r_wdog", self.base)?; + + self.inner.init(IrqSafeSpinlock::new(regs)); + + Ok(()) + } +} + +impl RWatchdog { + pub unsafe fn reset_board(&self) -> ! { + let regs = self.inner.get().lock(); + + regs.CFG.write(CFG::CONFIG::System); + regs.MODE.write(MODE::EN::SET); + regs.CTRL.write(CTRL::KEY::Value + CTRL::RESTART::SET); + + loop { + core::arch::asm!("wfe"); + } + } + + pub const unsafe fn new(base: usize) -> Self { + Self { + inner: OneTimeInit::new(), + base, + } + } +} diff --git a/src/arch/aarch64/plat_orange_pi3/serial.rs b/src/arch/aarch64/plat_orange_pi3/serial.rs new file mode 100644 index 00000000..00bea9c9 --- /dev/null +++ b/src/arch/aarch64/plat_orange_pi3/serial.rs @@ -0,0 +1,153 @@ +use abi::{error::Error, io::DeviceRequest}; +use tock_registers::{ + interfaces::{Readable, Writeable}, + register_bitfields, register_structs, + registers::{ReadOnly, ReadWrite}, +}; +use vfs::CharDevice; + +use crate::{ + arch::aarch64::gic::IrqNumber, + arch::PLATFORM, + debug::DebugSink, + device::{ + interrupt::InterruptSource, + platform::Platform, + serial::SerialDevice, + tty::{CharRing, TtyDevice}, + Device, InitializableDevice, + }, + mem::device::DeviceMemoryIo, + sync::IrqSafeSpinlock, + util::OneTimeInit, +}; + +register_bitfields! { + u32, + USR [ + TFE OFFSET(2) NUMBITS(1) [], + TFNF OFFSET(1) NUMBITS(1) [] + ] +} + +register_structs! { + #[allow(non_snake_case)] + Regs { + (0x00 => DLL: ReadWrite), + (0x04 => _0), + (0x7C => USR: ReadOnly), + (0x80 => @END), + } +} + +struct Inner { + regs: DeviceMemoryIo, +} + +pub struct Serial { + inner: OneTimeInit>, + ring: CharRing<16>, + base: usize, + irq: IrqNumber, +} + +impl CharDevice for Serial { + fn read(&'static self, _blocking: bool, data: &mut [u8]) -> Result { + self.line_read(data) + } + + fn write(&self, _blocking: bool, data: &[u8]) -> Result { + self.line_write(data) + } + + fn device_request(&self, req: &mut DeviceRequest) -> Result<(), Error> { + match req { + &mut DeviceRequest::SetTerminalGroup(id) => { + self.set_signal_group(id as _); + Ok(()) + } + _ => Err(Error::InvalidArgument), + } + } +} + +impl TtyDevice<16> for Serial { + fn ring(&self) -> &CharRing<16> { + &self.ring + } +} + +impl InterruptSource for Serial { + unsafe fn init_irq(&'static self) -> Result<(), Error> { + let intc = PLATFORM.interrupt_controller(); + + intc.register_handler(self.irq, self)?; + intc.enable_irq(self.irq)?; + + Ok(()) + } + + fn handle_irq(&self) -> Result { + let byte = self.inner.get().lock().regs.DLL.get(); + + self.recv_byte(byte as u8); + + Ok(true) + } +} + +impl DebugSink for Serial { + fn putc(&self, c: u8) -> Result<(), Error> { + self.send(c).ok(); + Ok(()) + } +} + +impl SerialDevice for Serial { + fn send(&self, byte: u8) -> Result<(), Error> { + let inner = self.inner.get().lock(); + if byte == b'\n' { + while inner.regs.USR.matches_all(USR::TFE::CLEAR) { + core::hint::spin_loop(); + } + inner.regs.DLL.set(b'\r' as u32); + } + while inner.regs.USR.matches_all(USR::TFE::CLEAR) { + core::hint::spin_loop(); + } + inner.regs.DLL.set(byte as u32); + Ok(()) + } + + fn receive(&self, blocking: bool) -> Result { + loop {} + } +} + +impl InitializableDevice for Serial { + type Options = (); + + unsafe fn init(&self, opts: Self::Options) -> Result<(), Error> { + let regs = DeviceMemoryIo::::map("h6-uart", self.base)?; + self.inner.init(IrqSafeSpinlock::new(Inner { regs })); + Ok(()) + } +} + +impl Device for Serial { + fn name(&self) -> &'static str { + "Allwinner H6 UART" + } +} + +impl Serial { + pub const unsafe fn new(base: usize, irq: IrqNumber) -> Self { + Self { + inner: OneTimeInit::new(), + ring: CharRing::new(), + + base, + irq, + } + } +} diff --git a/src/arch/aarch64/plat_qemu/mod.rs b/src/arch/aarch64/plat_qemu/mod.rs index ea019ec1..fdd0ca09 100644 --- a/src/arch/aarch64/plat_qemu/mod.rs +++ b/src/arch/aarch64/plat_qemu/mod.rs @@ -22,14 +22,14 @@ use super::{ }; /// AArch64 "virt" platform implementation -pub struct QemuPlatform { +pub struct PlatformImpl { /// ... pub gic: Gic, pl011: Pl011, local_timer: ArmTimer, } -impl Platform for QemuPlatform { +impl Platform for PlatformImpl { type IrqNumber = IrqNumber; const KERNEL_PHYS_BASE: usize = 0x40080000; @@ -78,11 +78,15 @@ impl Platform for QemuPlatform { infoln!("TODO send ipi"); loop {} } + + unsafe fn reset(&self) -> ! { + loop {} + } } /// AArch64 "virt" platform -pub static PLATFORM: QemuPlatform = unsafe { - QemuPlatform { +pub static PLATFORM: PlatformImpl = unsafe { + PlatformImpl { pl011: Pl011::new(0x09000000, IrqNumber::new(33)), gic: Gic::new(0x08000000, 0x08010000), local_timer: ArmTimer::new(IrqNumber::new(30)), diff --git a/src/arch/aarch64/timer.rs b/src/arch/aarch64/timer.rs index 63416dd7..62b69964 100644 --- a/src/arch/aarch64/timer.rs +++ b/src/arch/aarch64/timer.rs @@ -7,7 +7,7 @@ use abi::error::Error; use tock_registers::interfaces::{ReadWriteable, Readable, Writeable}; use crate::{ - arch::PLATFORM, + arch::{aarch64::gic::IrqNumber, PLATFORM}, device::{ interrupt::InterruptSource, platform::Platform, timer::TimestampSource, Device, InitializableDevice, @@ -16,7 +16,7 @@ use crate::{ task::tasklet, }; -use super::{cpu::Cpu, gic::IrqNumber}; +use super::cpu::Cpu; /// ARM Generic Timer driver pub struct ArmTimer { diff --git a/src/arch/mod.rs b/src/arch/mod.rs index 9e7cbd50..82a08831 100644 --- a/src/arch/mod.rs +++ b/src/arch/mod.rs @@ -20,14 +20,11 @@ macro_rules! absolute_address { }}; } +pub mod aarch64; cfg_if! { if #[cfg(target_arch = "aarch64")] { - pub mod aarch64; - - pub use aarch64::plat_qemu::{QemuPlatform as PlatformImpl, PLATFORM}; - pub use aarch64::{AArch64 as ArchitectureImpl, ARCHITECTURE}; - + pub use aarch64::{AArch64 as ArchitectureImpl, ARCHITECTURE, PlatformImpl, PLATFORM}; } else if #[cfg(target_arch = "x86_64")] { pub mod x86_64; diff --git a/src/device/mod.rs b/src/device/mod.rs index 0660d4a6..b0eee8b7 100644 --- a/src/device/mod.rs +++ b/src/device/mod.rs @@ -2,6 +2,7 @@ use abi::error::Error; +#[cfg(feature = "fb_console")] pub mod display; pub mod input; pub mod interrupt; diff --git a/src/device/platform.rs b/src/device/platform.rs index 1f99bf7a..5d2d4562 100644 --- a/src/device/platform.rs +++ b/src/device/platform.rs @@ -60,4 +60,6 @@ pub trait Platform { /// /// As the call may alter the flow of execution on CPUs, this function is unsafe. unsafe fn send_ipi(&self, target: IpiDeliveryTarget, msg: CpuMessage) -> Result<(), Error>; + + unsafe fn reset(&self) -> !; } diff --git a/src/device/serial/mod.rs b/src/device/serial/mod.rs index cc4765f4..b195d1e1 100644 --- a/src/device/serial/mod.rs +++ b/src/device/serial/mod.rs @@ -3,7 +3,7 @@ use abi::error::Error; use super::Device; -#[cfg(target_arch = "aarch64")] +#[cfg(all(target_arch = "aarch64", not(feature = "aarch64_orange_pi3")))] pub mod pl011; /// Generic serial device interface diff --git a/src/device/serial/pl011.rs b/src/device/serial/pl011.rs index 4144310c..e6e3fcd3 100644 --- a/src/device/serial/pl011.rs +++ b/src/device/serial/pl011.rs @@ -9,7 +9,7 @@ use vfs::CharDevice; use super::SerialDevice; use crate::{ - arch::{aarch64::gic::IrqNumber, PLATFORM}, + arch::{aarch64::gic::IrqNumber, PlatformImpl, PLATFORM}, debug::DebugSink, device::{ interrupt::InterruptSource, diff --git a/src/device/tty.rs b/src/device/tty.rs index c667c8d2..22f0ffef 100644 --- a/src/device/tty.rs +++ b/src/device/tty.rs @@ -1,91 +1,105 @@ //! Terminal driver implementation use abi::{ error::Error, - io::{ - DeviceRequest, TerminalInputOptions, TerminalLineOptions, TerminalOptions, - TerminalOutputOptions, - }, + io::{TerminalInputOptions, TerminalLineOptions, TerminalOptions, TerminalOutputOptions}, process::Signal, }; -use vfs::CharDevice; use crate::{ - device::display::fb_console::FramebufferConsole, proc::wait::Wait, sync::IrqSafeSpinlock, task::{process::Process, ProcessId}, }; -use super::{ - display::console::DisplayConsole, input::KeyboardDevice, serial::SerialDevice, Device, -}; +use super::serial::SerialDevice; -// TODO rewrite this -/// Helper device to combine a display and a keyboard input into a single terminal -pub struct CombinedTerminal { - #[allow(dead_code)] - input: &'static dyn KeyboardDevice, - output: &'static dyn DisplayConsole, +#[cfg(feature = "fb_console")] +pub mod combined { + use abi::{error::Error, io::DeviceRequest}; + use vfs::CharDevice; - input_ring: CharRing<16>, -} + use crate::device::{ + display::{console::DisplayConsole, fb_console::FramebufferConsole}, + input::KeyboardDevice, + serial::SerialDevice, + Device, + }; -impl CombinedTerminal { - /// Create a combined terminal device from a keyboard and console output devices - pub fn new(input: &'static dyn KeyboardDevice, output: &'static FramebufferConsole) -> Self { - Self { - input, - output, - input_ring: CharRing::new(), - } - } -} + use super::{CharRing, TtyDevice}; -impl TtyDevice<16> for CombinedTerminal { - fn ring(&self) -> &CharRing<16> { - &self.input_ring - } -} + // TODO rewrite this + /// Helper device to combine a display and a keyboard input into a single terminal + pub struct CombinedTerminal { + #[allow(dead_code)] + input: &'static dyn KeyboardDevice, + output: &'static dyn DisplayConsole, -impl SerialDevice for CombinedTerminal { - fn receive(&self, _blocking: bool) -> Result { - todo!() + input_ring: CharRing<16>, } - fn send(&self, byte: u8) -> Result<(), Error> { - self.output.write_char(byte); - Ok(()) - } -} - -impl Device for CombinedTerminal { - fn name(&self) -> &'static str { - "Combined terminal device" - } -} - -impl CharDevice for CombinedTerminal { - fn read(&'static self, blocking: bool, data: &mut [u8]) -> Result { - assert!(blocking); - self.line_read(data) - } - - fn write(&self, blocking: bool, data: &[u8]) -> Result { - assert!(blocking); - self.line_write(data) - } - - fn device_request(&self, req: &mut DeviceRequest) -> Result<(), Error> { - match req { - &mut DeviceRequest::SetTerminalGroup(id) => { - self.set_signal_group(id as _); - Ok(()) + impl CombinedTerminal { + /// Create a combined terminal device from a keyboard and console output devices + pub fn new( + input: &'static dyn KeyboardDevice, + output: &'static FramebufferConsole, + ) -> Self { + Self { + input, + output, + input_ring: CharRing::new(), + } + } + } + + impl TtyDevice<16> for CombinedTerminal { + fn ring(&self) -> &CharRing<16> { + &self.input_ring + } + } + + impl SerialDevice for CombinedTerminal { + fn receive(&self, _blocking: bool) -> Result { + todo!() + } + + fn send(&self, byte: u8) -> Result<(), Error> { + self.output.write_char(byte); + Ok(()) + } + } + + impl Device for CombinedTerminal { + fn name(&self) -> &'static str { + "Combined terminal device" + } + } + + impl CharDevice for CombinedTerminal { + fn read(&'static self, blocking: bool, data: &mut [u8]) -> Result { + assert!(blocking); + self.line_read(data) + } + + fn write(&self, blocking: bool, data: &[u8]) -> Result { + assert!(blocking); + self.line_write(data) + } + + fn device_request(&self, req: &mut DeviceRequest) -> Result<(), Error> { + match req { + &mut DeviceRequest::SetTerminalGroup(id) => { + self.set_signal_group(id as _); + Ok(()) + } + _ => Err(Error::InvalidArgument), } - _ => Err(Error::InvalidArgument), } } } +#[cfg(feature = "fb_console")] +pub use combined::CombinedTerminal; + struct CharRingInner { rd: usize, wr: usize, @@ -209,7 +223,7 @@ pub trait TtyDevice: SerialDevice { } else if byte == config.chars.erase { // Erase if off != 0 { - self.raw_write(b"\x1b[D \x1b[D"); + self.raw_write(b"\x1b[D \x1b[D")?; off -= 1; rem += 1; } diff --git a/src/main.rs b/src/main.rs index 27fce02b..e277d847 100644 --- a/src/main.rs +++ b/src/main.rs @@ -22,7 +22,6 @@ use abi::{ error::Error, io::{FileMode, OpenOptions, RawFd}, }; -use device::display::console::task_update_consoles; use fs::{devfs, FileBlockAllocator, INITRD_DATA}; use memfs::MemoryFilesystem; use task::tasklet; @@ -60,11 +59,15 @@ fn setup_root() -> Result { /// This function is meant to be used as a kernel-space process after all the platform-specific /// initialization has finished. pub fn kernel_main() { - tasklet::add_periodic( - "update-console", - Duration::from_millis(15), - task_update_consoles, - ); + #[cfg(feature = "fb_console")] + { + use device::display::console::task_update_consoles; + tasklet::add_periodic( + "update-console", + Duration::from_millis(15), + task_update_consoles, + ); + } let root = match setup_root() { Ok(root) => root, diff --git a/src/mem/mod.rs b/src/mem/mod.rs index fcd6fab8..02cfd4f5 100644 --- a/src/mem/mod.rs +++ b/src/mem/mod.rs @@ -1,7 +1,7 @@ //! Memory management utilities and types // use core::{alloc::Layout, mem::size_of}; -use core::{alloc::Layout, mem::size_of}; +use core::{alloc::Layout, ffi::c_void, mem::size_of}; use abi::error::Error; @@ -286,3 +286,72 @@ pub fn validate_user_region( Ok(()) } + +#[no_mangle] +unsafe extern "C" fn memcpy(p0: *mut c_void, p1: *const c_void, len: usize) -> *mut c_void { + let mut offset = 0; + while offset < len { + let c = (p1 as *const u8).add(offset).read_volatile(); + (p0 as *mut u8).add(offset).write_volatile(c); + + offset += 1; + } + p0 +} + +#[no_mangle] +unsafe extern "C" fn memcmp(p0: *const c_void, p1: *const c_void, len: usize) -> i32 { + let mut offset = 0; + + if len == 0 { + return 0; + } + + while offset < len { + let c0 = (p0 as *const u8).add(offset).read_volatile(); + let c1 = (p1 as *const u8).add(offset).read_volatile(); + + if c0 > c1 { + return (c0 - c1) as i32; + } else if c0 < c1 { + return -((c1 - c0) as i32); + } + + offset += 1; + } + + 0 +} + +#[no_mangle] +unsafe extern "C" fn memmove(dst: *mut c_void, src: *const c_void, len: usize) -> *mut c_void { + if dst as usize == src as usize { + return dst; + } + + if (src.add(len) as usize) <= (dst as usize) || (dst.add(len) as usize) <= (src as usize) { + return memcpy(dst, src, len); + } + + if (dst as usize) < (src as usize) { + let a = src as usize - dst as usize; + memcpy(dst, src, a); + memcpy(src as *mut _, src.add(a), len - a); + } else { + let a = dst as usize - src as usize; + memcpy(dst.add(a), dst, len - a); + memcpy(dst, src, a); + } + + dst +} + +#[no_mangle] +unsafe extern "C" fn memset(dst: *mut c_void, val: i32, len: usize) -> *mut c_void { + let mut offset = 0; + while offset < len { + (dst as *mut u8).add(offset).write_volatile(val as u8); + offset += 1; + } + dst +} diff --git a/src/panic.rs b/src/panic.rs index 807a62fd..0f515619 100644 --- a/src/panic.rs +++ b/src/panic.rs @@ -42,19 +42,19 @@ fn panic_handler(pi: &core::panic::PanicInfo) -> ! { .compare_exchange(false, true, Ordering::Release, Ordering::Acquire) .is_ok() { - // Let other CPUs know we're screwed - unsafe { - PLATFORM - .send_ipi(IpiDeliveryTarget::AllExceptLocal, CpuMessage::Panic) - .ok(); - } + // // Let other CPUs know we're screwed + // unsafe { + // PLATFORM + // .send_ipi(IpiDeliveryTarget::AllExceptLocal, CpuMessage::Panic) + // .ok(); + // } - let ap_count = ArchitectureImpl::cpu_count() - 1; - PANIC_HANDLED_FENCE.wait_all(ap_count); + // let ap_count = ArchitectureImpl::cpu_count() - 1; + // PANIC_HANDLED_FENCE.wait_all(ap_count); - unsafe { - hack_locks(); - } + // unsafe { + // hack_locks(); + // } log_print_raw!(LogLevel::Fatal, "--- BEGIN PANIC ---\n"); log_print_raw!(LogLevel::Fatal, "In CPU {}\n", Cpu::local_id()); diff --git a/src/util.rs b/src/util.rs index 0dc483fe..acf40ad4 100644 --- a/src/util.rs +++ b/src/util.rs @@ -99,7 +99,7 @@ impl StaticVector { /// /// Will panic if the vector is full. pub fn push(&mut self, value: T) { - if self.len == N { + if self.len >= N { panic!("Static vector overflow: reached limit of {}", N); }