UART

Clean up warnings
Better abstraction for platform support
2021-09-03 15:48:32 +03:00 · 2021-09-03 11:01:55 +03:00 · 2021-09-03 10:53:20 +03:00 · 2021-09-02 10:31:34 +03:00
41 changed files with 1447 additions and 227 deletions
--- a/Cargo.lock
+++ b/Cargo.lock
@ -9,16 +9,58 @@ dependencies = [
 "error",
 ]

+[[package]]
+name = "autocfg"
+version = "1.0.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "cdb031dd78e28731d87d56cc8ffef4a8f36ca26c38fe2de700543e627f8a464a"
+
+[[package]]
+name = "cfg-if"
+version = "1.0.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd"
+
+[[package]]
+name = "endian-type-rs"
+version = "0.1.4"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "b6419a5c75e40011b9fe0174db3fe24006ab122fbe1b7e9cc5974b338a755c76"
+
 [[package]]
 name = "error"
 version = "0.1.0"

+[[package]]
+name = "fallible-iterator"
+version = "0.2.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "4443176a9f2c162692bd3d352d745ef9413eec5782a80d8fd6f8a1ac692a07f7"
+
+[[package]]
+name = "fdt-rs"
+version = "0.4.3"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "99a40cabc11c8258822a593f5c51f2d9f4923e715ca9e2a0630cf77ae15f390b"
+dependencies = [
+ "endian-type-rs",
+ "fallible-iterator",
+ "memoffset",
+ "num-derive",
+ "num-traits",
+ "rustc_version",
+ "static_assertions",
+ "unsafe_unwrap",
+]
+
 [[package]]
 name = "kernel"
 version = "0.1.0"
 dependencies = [
 "address",
+ "cfg-if",
 "error",
+ "fdt-rs",
 "spin",
 ]

@ -31,16 +73,87 @@ dependencies = [
 "scopeguard",
 ]

+[[package]]
+name = "memoffset"
+version = "0.5.6"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "043175f069eda7b85febe4a74abbaeff828d9f8b448515d3151a14a3542811aa"
+dependencies = [
+ "autocfg",
+]
+
+[[package]]
+name = "num-derive"
+version = "0.3.3"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "876a53fff98e03a936a674b29568b0e605f06b29372c2489ff4de23f1949743d"
+dependencies = [
+ "proc-macro2",
+ "quote",
+ "syn",
+]
+
+[[package]]
+name = "num-traits"
+version = "0.2.14"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "9a64b1ec5cda2586e284722486d802acf1f7dbdc623e2bfc57e65ca1cd099290"
+dependencies = [
+ "autocfg",
+]
+
 [[package]]
 name = "osdev4"
 version = "0.1.0"

+[[package]]
+name = "proc-macro2"
+version = "1.0.29"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "b9f5105d4fdaab20335ca9565e106a5d9b82b6219b5ba735731124ac6711d23d"
+dependencies = [
+ "unicode-xid",
+]
+
+[[package]]
+name = "quote"
+version = "1.0.9"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "c3d0b9745dc2debf507c8422de05d7226cc1f0644216dfdfead988f9b1ab32a7"
+dependencies = [
+ "proc-macro2",
+]
+
+[[package]]
+name = "rustc_version"
+version = "0.2.3"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "138e3e0acb6c9fb258b19b67cb8abd63c00679d2851805ea151465464fe9030a"
+dependencies = [
+ "semver",
+]
+
 [[package]]
 name = "scopeguard"
 version = "1.1.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "d29ab0c6d3fc0ee92fe66e2d99f700eab17a8d57d1c1d3b748380fb20baa78cd"

+[[package]]
+name = "semver"
+version = "0.9.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "1d7eb9ef2c18661902cc47e535f9bc51b78acd254da71d375c2f6720d9a40403"
+dependencies = [
+ "semver-parser",
+]
+
+[[package]]
+name = "semver-parser"
+version = "0.7.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "388a1df253eca08550bef6c72392cfe7c30914bf41df5269b68cbd6ff8f570a3"
+
 [[package]]
 name = "spin"
 version = "0.9.2"
@ -49,3 +162,32 @@ checksum = "511254be0c5bcf062b019a6c89c01a664aa359ded62f78aa72c6fc137c0590e5"
 dependencies = [
 "lock_api",
 ]
+
+[[package]]
+name = "static_assertions"
+version = "1.1.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "a2eb9349b6444b326872e140eb1cf5e7c522154d69e7a0ffb0fb81c06b37543f"
+
+[[package]]
+name = "syn"
+version = "1.0.75"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "b7f58f7e8eaa0009c5fec437aabf511bd9933e4b2d7407bd05273c01a8906ea7"
+dependencies = [
+ "proc-macro2",
+ "quote",
+ "unicode-xid",
+]
+
+[[package]]
+name = "unicode-xid"
+version = "0.2.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "8ccb82d61f80a663efe1f787a51b16b5a51e3314d6ac365b08639f52387b33f3"
+
+[[package]]
+name = "unsafe_unwrap"
+version = "0.1.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "1230ec65f13e0f9b28d789da20d2d419511893ea9dac2c1f4ef67b8b14e5da80"
--- a/address/src/phys.rs
+++ b/address/src/phys.rs
@ -55,6 +55,14 @@ impl From<u64> for PhysicalAddress {
 }

 impl PhysicalAddress {
+    pub const fn new(value: usize) -> Self {
+        Self(value)
+    }
+
+    pub const fn add(self, value: usize) -> Self {
+        Self(self.0 + value)
+    }
+
    #[inline(always)]
    pub fn diff(start: PhysicalAddress, end: PhysicalAddress) -> isize {
        if end >= start {
--- a/address/src/virt.rs
+++ b/address/src/virt.rs
@ -118,8 +118,13 @@ impl<T: AddressSpace> VirtualAddress<T> {
    }

    #[inline(always)]
-    pub unsafe fn as_mut<U>(self) -> &'static mut U {
-        &mut *(self.0 as *mut U)
+    pub unsafe fn as_mut<U>(self) -> Option<&'static mut U> {
+        (self.0 as *mut U).as_mut()
+    }
+
+    #[inline(always)]
+    pub unsafe fn from_ptr<U>(r: *const U) -> Self {
+        Self::from(r as usize)
    }

    #[inline(always)]
--- a/bcm2837-rpi-3-b.dtb
+++ b/bcm2837-rpi-3-b.dtb
--- a/build.sh
+++ b/build.sh
@ -4,6 +4,21 @@ if [ -z "${MACH}" ]; then
    MACH=rpi3b
 fi

+LLVM_DIR=$(llvm-config --bindir)
+ARCH=aarch64-unknown-none-${MACH}
+CARGO_ARGS="--target ../etc/aarch64-unknown-none-$MACH.json \
+            --features mach_$MACH,fdt-rs"
+
 set -e
-cd kernel && cargo build --target ../etc/aarch64-unknown-none-$MACH.json
+
+cd kernel
+case $1 in
+    check)
+        cargo check $CARGO_ARGS
+        ;;
+    build|"")
+        cargo build $CARGO_ARGS
+        ;;
+esac
 cd ..
+${LLVM_DIR}/llvm-objcopy -O binary target/${ARCH}/debug/kernel target/${ARCH}/debug/kernel.bin
--- a/etc/aarch64-unknown-none-rpi3b.gdb
+++ b/etc/aarch64-unknown-none-rpi3b.gdb
--- a/etc/aarch64-unknown-none-virt.gdb
+++ b/etc/aarch64-unknown-none-virt.gdb
@ -0,0 +1,5 @@
+symbol-file target/aarch64-unknown-none-virt/debug/kernel
+target remote :1234
+layout asm
+layout regs
+set scheduler-locking on
--- a/etc/aarch64-unknown-none-virt.json
+++ b/etc/aarch64-unknown-none-virt.json
@ -0,0 +1,23 @@
+{
+  "arch": "aarch64",
+  "data-layout": "e-m:e-i8:8:32-i16:16:32-i64:64-i128:128-n32:64-S128",
+  "executables": true,
+  "linker": "rust-lld",
+  "linker-flavor": "ld.lld",
+  "linker-is-gnu": true,
+  "llvm-target": "aarch64-unknown-none",
+  "features": "+a53,+strict-align",
+  "max-atomic-width": 128,
+  "os": "none",
+  "panic-strategy": "abort",
+  "position-independent-executables": false,
+  "target-c-int-width": "32",
+  "target-endian": "little",
+  "target-pointer-width": "64",
+  "disable-redzone": true,
+  "pre-link-args": {
+    "ld.lld": [
+      "-Tetc/aarch64-unknown-none-virt.ld"
+    ]
+  }
+}
--- a/etc/aarch64-unknown-none-virt.ld
+++ b/etc/aarch64-unknown-none-virt.ld
@ -0,0 +1,29 @@
+ENTRY(_entry);
+
+KERNEL_OFFSET = 0xFFFFFF8000000000;
+
+SECTIONS {
+    . = 0x40080000 + KERNEL_OFFSET;
+
+    .text : AT(. - KERNEL_OFFSET) ALIGN(4K) {
+        KEEP(*(.text.boot))
+        *(.text*)
+    }
+
+    .rodata : AT(. - KERNEL_OFFSET) ALIGN(4K) {
+        *(.rodata*)
+    }
+
+    .data : AT(. - KERNEL_OFFSET) ALIGN(4K) {
+        *(.data*)
+    }
+
+    .bss : AT(. - KERNEL_OFFSET) ALIGN(4K) {
+        *(COMMON)
+        *(.bss*)
+
+        . = ALIGN(4K);
+        PROVIDE(__kernel_end = .);
+        PROVIDE(__kernel_end_phys = . - KERNEL_OFFSET);
+    }
+}
--- a/kernel/.cargo/config.toml
+++ b/kernel/.cargo/config.toml
@ -1,5 +1,5 @@
 [unstable]
-build-std = ["core", "compiler_builtins"]
+build-std = ["core", "compiler_builtins", "alloc"]

 [build]
 target = "../etc/aarch64-unknown-none-rpi3b.json"
--- a/kernel/Cargo.toml
+++ b/kernel/Cargo.toml
@ -9,3 +9,9 @@ edition = "2018"
 address = { path = "../address" }
 error = { path = "../error" }
 spin = "0.9.2"
+cfg-if = "*"
+fdt-rs = { version = "*", optional = true, default-features = false }
+
+[features]
+mach_rpi3b = []
+mach_virt = ["fdt-rs"]
--- a/kernel/src/arch/intrin.rs
+++ b/kernel/src/arch/intrin.rs
@ -73,6 +73,10 @@ pub unsafe fn write_cntp_cval_el0(value: usize) {
    llvm_asm!("msr cntp_cval_el0, $0"::"r"(value));
 }

+pub unsafe fn write_cntp_tval_el0(value: usize) {
+    llvm_asm!("msr cntp_tval_el0, $0"::"r"(value));
+}
+
 #[inline(always)]
 pub fn dsb_sy() {
    unsafe {
--- a/kernel/src/arch/mach_bcm283x/aux.rs
+++ b/kernel/src/arch/mach_bcm283x/aux.rs
@ -0,0 +1,70 @@
+use crate::{
+    arch::{mmio_read, mmio_write},
+    dev::{irq::InterruptHandler, serial::SerialDevice, Device},
+};
+use address::PhysicalAddress;
+
+pub struct AuxUart;
+pub struct Aux;
+
+impl Aux {
+    const REG_AUX_ENABLES: PhysicalAddress = PhysicalAddress::new(0x3F215004);
+    const AUX_ENABLES_MUART: u32 = 1 << 0;
+
+    pub unsafe fn enable_uart(&self) {
+        let tmp = mmio_read(Self::REG_AUX_ENABLES);
+        mmio_write(Self::REG_AUX_ENABLES, tmp | Self::AUX_ENABLES_MUART);
+    }
+}
+
+impl AuxUart {
+    const AUX_MU_BASE: PhysicalAddress = PhysicalAddress::new(0x3F215000);
+    const REG_AUX_MU_IO: PhysicalAddress = Self::AUX_MU_BASE.add(0x40);
+    const REG_AUX_MU_IER: PhysicalAddress = Self::AUX_MU_BASE.add(0x44);
+    const REG_AUX_MU_CNTL: PhysicalAddress = Self::AUX_MU_BASE.add(0x60);
+
+    const AUX_MU_CNTL_TE: u32 = 1 << 1;
+    const AUX_MU_CNTL_RE: u32 = 1 << 0;
+
+    const AUX_MU_IER_RIE: u32 = 1 << 0;
+}
+
+impl InterruptHandler for Aux {
+    fn do_irq(&self, _irq: u32) {}
+}
+
+impl Device for AuxUart {
+    fn name(&self) -> &'static str {
+        "BCM283x Mini-UART"
+    }
+
+    unsafe fn enable(&self) {
+        AUX.enable_uart();
+
+        mmio_write(Self::REG_AUX_MU_IER, Self::AUX_MU_IER_RIE);
+        mmio_write(
+            Self::REG_AUX_MU_CNTL,
+            Self::AUX_MU_CNTL_TE | Self::AUX_MU_CNTL_RE,
+        );
+    }
+
+    unsafe fn disable(&self) {}
+}
+
+impl SerialDevice for AuxUart {
+    fn send(&self, ch: u8) {
+        unsafe {
+            mmio_write(Self::REG_AUX_MU_IO, ch as u32);
+        }
+    }
+}
+
+impl InterruptHandler for AuxUart {
+    fn do_irq(&self, _irq: u32) {
+        let byte = unsafe { mmio_read(Self::REG_AUX_MU_IO) } as u8;
+        debugln!("{}", byte as char);
+    }
+}
+
+pub static AUX: Aux = Aux;
+pub static UART: AuxUart = AuxUart;
--- a/kernel/src/arch/mach_bcm283x/intc.rs
+++ b/kernel/src/arch/mach_bcm283x/intc.rs
@ -0,0 +1,138 @@
+use crate::{
+    arch::{cpu, mmio_read, mmio_write},
+    dev::{irq::InterruptController, Device},
+};
+use address::PhysicalAddress;
+
+pub struct Qa7Intc;
+pub struct Bcm2837Intc;
+
+pub struct Intc {
+    qa7_intc: Qa7Intc,
+    bcm2837_intc: Bcm2837Intc,
+}
+
+impl Bcm2837Intc {
+    const REG_PENDING_IRQ1: PhysicalAddress = PhysicalAddress::new(0x3F00B204);
+    const REG_PENDING_IRQ2: PhysicalAddress = PhysicalAddress::new(0x3F00B208);
+
+    const REG_ENABLE_IRQ1: PhysicalAddress = PhysicalAddress::new(0x3F00B210);
+    const REG_ENABLE_IRQ2: PhysicalAddress = PhysicalAddress::new(0x3F00B214);
+}
+
+impl Qa7Intc {
+    const REG_TIMER_INTC: PhysicalAddress = PhysicalAddress::new(0x40000040);
+    const REG_INT_SRC: PhysicalAddress = PhysicalAddress::new(0x40000060);
+
+    const INTC_CNTPNSIRQ_IRQ: u32 = 1 << 1;
+}
+
+impl Device for Intc {
+    fn name(&self) -> &'static str {
+        "BCM283x Interrupt Controller"
+    }
+
+    unsafe fn enable(&self) {}
+    unsafe fn disable(&self) {}
+}
+
+impl Device for Qa7Intc {
+    fn name(&self) -> &'static str {
+        "Broadcom QA7 Interrupt Controller"
+    }
+
+    unsafe fn enable(&self) {}
+    unsafe fn disable(&self) {}
+}
+
+impl Device for Bcm2837Intc {
+    fn name(&self) -> &'static str {
+        "BCM2837 Interrupt Controller"
+    }
+
+    unsafe fn enable(&self) {}
+    unsafe fn disable(&self) {}
+}
+
+impl InterruptController for Qa7Intc {
+    unsafe fn enable_irq(&self, irq: u32) {
+        match irq {
+            super::IRQ_LOCAL_TIMER => {
+                let phys_core_id = cpu::get_phys_id();
+                let tmp = mmio_read(Self::REG_TIMER_INTC + 4 * phys_core_id);
+                mmio_write(
+                    Self::REG_TIMER_INTC + 4 * phys_core_id,
+                    tmp | Self::INTC_CNTPNSIRQ_IRQ,
+                );
+            }
+            _ => panic!("Unhandled IRQ number: {}", irq),
+        }
+    }
+
+    unsafe fn disable_irq(&self, _irq: u32) {
+        todo!();
+    }
+
+    fn is_irq_pending(&self, irq: u32) -> bool {
+        unsafe { mmio_read(Self::REG_INT_SRC) & (1 << irq) != 0 }
+    }
+
+    unsafe fn clear_irq(&self, _irq: u32) {}
+}
+
+impl InterruptController for Bcm2837Intc {
+    unsafe fn enable_irq(&self, irq: u32) {
+        if irq < 32 {
+            mmio_write(Self::REG_ENABLE_IRQ1, 1 << irq);
+        } else if irq < 64 {
+            mmio_write(Self::REG_ENABLE_IRQ2, 1 << (irq - 32));
+        }
+    }
+
+    unsafe fn disable_irq(&self, _irq: u32) {
+        todo!();
+    }
+
+    fn is_irq_pending(&self, irq: u32) -> bool {
+        if irq < 32 {
+            unsafe { mmio_read(Self::REG_PENDING_IRQ1) & (1 << irq) != 0 }
+        } else if irq < 64 {
+            unsafe { mmio_read(Self::REG_PENDING_IRQ2) & (1 << (irq - 32)) != 0 }
+        } else {
+            false
+        }
+    }
+
+    unsafe fn clear_irq(&self, _irq: u32) {
+        todo!();
+    }
+}
+
+impl InterruptController for Intc {
+    unsafe fn enable_irq(&self, irq: u32) {
+        if irq < 16 {
+            self.qa7_intc.enable_irq(irq);
+        } else {
+            self.bcm2837_intc.enable_irq(irq - 16);
+        }
+    }
+
+    unsafe fn disable_irq(&self, _irq: u32) {
+        todo!();
+    }
+
+    fn is_irq_pending(&self, irq: u32) -> bool {
+        if irq < 16 {
+            self.qa7_intc.is_irq_pending(irq)
+        } else {
+            self.bcm2837_intc.is_irq_pending(irq - 16)
+        }
+    }
+
+    unsafe fn clear_irq(&self, _irq: u32) {}
+}
+
+pub static INTC: Intc = Intc {
+    qa7_intc: Qa7Intc,
+    bcm2837_intc: Bcm2837Intc,
+};
--- a/kernel/src/arch/mach_bcm283x/mbox.rs
+++ b/kernel/src/arch/mach_bcm283x/mbox.rs
@ -1,12 +1,14 @@
-use super::{cpu, smp, intrin, mmio_read, mmio_write};
-use crate::KernelSpace;
+use crate::{
+    arch::{cpu, intrin, mmio_read, mmio_write, smp::{self, IpiDelivery, IpiMessage}},
+    KernelSpace,
+};
 use address::{PhysicalAddress, VirtualAddress};

-const MBOX_BASE: usize = 0x3F00B880;
-const MBOX_READ: usize = MBOX_BASE + 0x00;
+const MBOX_BASE: PhysicalAddress = PhysicalAddress::new(0x3F00B880);
+const MBOX_READ: PhysicalAddress = MBOX_BASE.add(0x00);
 //const MBOX_POLL: usize = MBOX_BASE + 0x10;
-const MBOX_STATUS: usize = MBOX_BASE + 0x18;
-const MBOX_WRITE: usize = MBOX_BASE + 0x20;
+const MBOX_STATUS: PhysicalAddress = MBOX_BASE.add(0x18);
+const MBOX_WRITE: PhysicalAddress = MBOX_BASE.add(0x20);

 const MBOX_STATUS_FULL: u32 = 1 << 31;
 const MBOX_STATUS_EMPTY: u32 = 1 << 30;
@ -32,27 +34,46 @@ pub struct CoreMailbox {
    index: usize,
 }

-impl CoreMailbox {
-    const REG_INTC: usize = 0x40000050;
-    const REG_SET: usize = 0x40000080;
-    const REG_RDCLR: usize = 0x400000C0;
-
-    pub unsafe fn enable(&self) {
+impl IpiDelivery for CoreMailbox {
+    fn enable(&self) {
        let phys_core_id = cpu::get_phys_id();
-        mmio_write(Self::REG_INTC + phys_core_id * 4, 1 << self.index);
+        unsafe {
+            mmio_write(Self::REG_INTC + phys_core_id * 4, 1 << self.index);
+        }
    }

-    pub unsafe fn send(core_id: u32, mbox_id: usize, value: u32) {
-        mmio_write(Self::REG_SET + core_id as usize * 16 + mbox_id * 4, value);
+    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 = unsafe { mmio_read(Self::REG_RDCLR + phys_core_id * 16 + self.index * 4) };
        if value != 0 {
-            smp::handle_ipi(value);
+            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(Self::REG_RDCLR + phys_core_id * 16 + self.index * 4, 0xFFFFFFFF);
+                mmio_write(
+                    Self::REG_RDCLR + phys_core_id * 16 + self.index * 4,
+                    0xFFFFFFFF,
+                );
            }
        }
    }
--- a/kernel/src/arch/mach_bcm283x/mod.rs
+++ b/kernel/src/arch/mach_bcm283x/mod.rs
@ -0,0 +1,36 @@
+use crate::mem::phys::{init_from_iter, SimpleMemoryIterator, UsableMemory};
+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;
+pub const IRQ_AUX: u32 = 16 + 29;
+
+// TODO as long as AUX is not used for anything else?
+pub const IRQ_UART: u32 = IRQ_AUX;
+
+pub use aux::UART as AUX_UART;
+pub use intc::INTC;
+// Configured as primary UART
+pub use AUX_UART as UART;
+
+// pub const INT_SRC_TIMER: u32 = 1 << 11;
+// pub const INT_SRC_MBOX0: u32 = 1 << 4;
+
+pub fn init_phys_memory() {
+    let arm_memory = mbox::get_arm_memory().unwrap();
+
+    let iter = SimpleMemoryIterator::new(UsableMemory {
+        start: PhysicalAddress::from(0usize),
+        end: PhysicalAddress::from(arm_memory),
+    });
+    unsafe {
+        init_from_iter(iter);
+    }
+}
+
+pub fn init() {}
--- a/kernel/src/arch/mach_bcm283x/smp.rs
+++ b/kernel/src/arch/mach_bcm283x/smp.rs
@ -0,0 +1,42 @@
+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();
+    }
+}
--- a/kernel/src/arch/mach_bcm283x/timer.rs
+++ b/kernel/src/arch/mach_bcm283x/timer.rs
--- a/kernel/src/arch/mach_virt/mod.rs
+++ b/kernel/src/arch/mach_virt/mod.rs
@ -0,0 +1,21 @@
+use crate::arch;
+
+pub mod timer;
+
+use address::PhysicalAddress;
+
+pub const IRQ_LOCAL_TIMER: u32 = 30;
+pub const IRQ_UART: u32 = 32 + 1;
+pub const IRQ_RTC: u32 = 32 + 2;
+
+pub const GICD_BASE: PhysicalAddress = PhysicalAddress::new(0x08000000usize);
+pub const GICC_BASE: PhysicalAddress = PhysicalAddress::new(0x08010000usize);
+
+pub const PL031_BASE: PhysicalAddress = PhysicalAddress::new(0x09010000usize);
+pub const PL011_BASE: PhysicalAddress = PhysicalAddress::new(0x09000000usize);
+
+pub fn init() {
+    unsafe {
+        arch::timer::enable_rtc();
+    }
+}
--- a/kernel/src/arch/mach_virt/timer.rs
+++ b/kernel/src/arch/mach_virt/timer.rs
--- a/kernel/src/arch/mod.rs
+++ b/kernel/src/arch/mod.rs
@ -5,18 +5,27 @@ pub mod cpu;
 pub mod smp;
 pub mod exception;
 pub mod intrin;
-pub mod mbox;
 pub mod timer;

-pub unsafe fn mmio_write(addr: usize, value: u32) {
+cfg_if! {
+    if #[cfg(feature = "mach_rpi3b")] {
+        pub mod mach_bcm283x;
+        pub use mach_bcm283x as machine;
+    } else if #[cfg(feature = "mach_virt")] {
+        pub mod mach_virt;
+        pub use mach_virt as machine;
+    }
+}
+
+pub unsafe fn mmio_write(addr: PhysicalAddress, value: u32) {
    core::ptr::write_volatile(
-        VirtualAddress::<KernelSpace>::from(PhysicalAddress::from(addr)).as_mut_ptr(),
+        VirtualAddress::<KernelSpace>::from(addr).as_mut_ptr(),
        value,
    );
 }

-pub unsafe fn mmio_read(addr: usize) -> u32 {
+pub unsafe fn mmio_read(addr: PhysicalAddress) -> u32 {
    core::ptr::read_volatile(
-        VirtualAddress::<KernelSpace>::from(PhysicalAddress::from(addr)).as_mut_ptr(),
+        VirtualAddress::<KernelSpace>::from(addr).as_mut_ptr(),
    )
 }
--- a/kernel/src/arch/psci.rs
+++ b/kernel/src/arch/psci.rs
@ -0,0 +1,3 @@
+pub fn init() {
+
+}
--- a/kernel/src/arch/smp.rs
+++ b/kernel/src/arch/smp.rs
@ -1,19 +1,53 @@
 use crate::{
    arch::{
-        cpu::{self, Cpu, CPUS, CPU_COUNT},
-        mbox::CoreMailbox,
+        cpu::{self, Cpu, CPU_COUNT},
        intrin,
    },
-    mem::phys::{self, PageUsage},
-    KernelSpace,
    entry_common,
 };
-use address::VirtualAddress;
-use core::hint;
 use core::sync::atomic::Ordering;

-pub const MAX_CPU: usize = 4;
-pub const IPI_HALT: u32 = 1 << 0;
+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() -> ! {
@ -21,64 +55,45 @@ extern "C" fn kernel_ap_main() -> ! {
    debugln!("cpu{}: ap wake up", index);
    cpu::init(index);

-    CPU_COUNT.fetch_add(1, Ordering::SeqCst);
+    smp_impl::cpu_ready(index);

    entry_common();
 }

-pub unsafe fn send_ipi(mask: usize, value: u32) {
+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 {
-            let dst = CPUS[index].assume_init_ref();
-            debugln!("cpu{}: send IPI to cpu{}", self_index, dst.index());
-            CoreMailbox::send(index as u32, 0, value);
+            // TODO cpus must be in phys order?
+            IpiDeliveryImpl::send_ipi(index as u32, message);
        }
    }
 }

-pub fn handle_ipi(mask: u32) {
-    debugln!("cpu{} received ipi: {}", Cpu::get().index(), mask);
+pub fn handle_ipi(message: IpiMessage) {
+    debugln!("cpu{} received ipi: {:?}", Cpu::get().index(), message);

-    if mask & IPI_HALT != 0 {
-        unsafe {
-            intrin::disable_irq();
-        }
-        loop {
+    match message {
+        IpiMessage::Halt => {
            unsafe {
                intrin::disable_irq();
            }
-            intrin::nop();
+            loop {
+                unsafe {
+                    intrin::disable_irq();
+                }
+                intrin::nop();
+            }
        }
+        IpiMessage::Tick => {}
    }
 }

-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() {
+    smp_impl::wakeup_ap_cpus();
 }

-pub fn wakeup_ap_cpus(count: usize) {
-    for _ in 1..count {
-        wakeup_single_ap();
-    }
-    debugln!("Done waking up {} ap cpus", count);
+pub fn init_ipi_delivery() {
+    IPI_IMPL.enable();
 }
--- a/kernel/src/arch/timer.rs
+++ b/kernel/src/arch/timer.rs
@ -1,99 +1,61 @@
 use crate::{
-    arch::{cpu, intrin, mmio_read, mmio_write},
+    arch::{intrin, machine},
+    dev::{
+        irq::{self, InterruptController, InterruptHandler},
+        Device,
+    },
 };
-use core::sync::atomic::{AtomicU64, Ordering};

-pub struct LocalTimer;
-pub struct GlobalTimer {
-    counter: AtomicU64,
-}
-pub enum LocalTimerMode {
-    Irq,
-    Fiq,
-}
+struct ArmTimer;

-impl LocalTimer {
-    const REG_PSC: usize = 0x40000008;
-    const REG_INTC: usize = 0x40000040;
-
-    const INTC_CNTPNSIRQ_FIQ: u32 = 1 << 5;
-    const INTC_CNTPNSIRQ_IRQ: u32 = 1 << 1;
-
-    const PRESCALER: u32 = 0x80000000;
-    const RELOAD_VALUE: usize = 1 << 18;
-
-    pub unsafe fn enable(mode: LocalTimerMode) {
-        let phys_core_id = cpu::get_phys_id();
-
-        mmio_write(Self::REG_PSC + 4 * phys_core_id, Self::PRESCALER);
-        let tmp = mmio_read(Self::REG_INTC + 4 * phys_core_id);
-        let flags = match mode {
-            LocalTimerMode::Irq => Self::INTC_CNTPNSIRQ_IRQ,
-            LocalTimerMode::Fiq => Self::INTC_CNTPNSIRQ_FIQ,
-        };
-        mmio_write(Self::REG_INTC + 4 * phys_core_id, tmp | flags);
+impl Device for ArmTimer {
+    fn name(&self) -> &'static str {
+        "ARM Generic Timer"
+    }

+    unsafe fn enable(&self) {
        intrin::write_cntp_ctl_el0(1);
    }

-    pub fn do_irq() {
-        //proc::sched_yield();
-        Self::update(Self::RELOAD_VALUE);
+    unsafe fn disable(&self) {
+        intrin::write_cntp_ctl_el0(0);
    }
+}

-    #[inline(always)]
-    pub fn update(value: usize) {
-        let current = intrin::read_cntp_cval_el0();
+impl InterruptHandler for ArmTimer {
+    fn do_irq(&self, _irq: u32) {
+        debugln!("T");
        unsafe {
-            intrin::write_cntp_cval_el0(current + value);
+            intrin::write_cntp_tval_el0(10000000);
        }
    }
 }

-impl GlobalTimer {
-    const REG_CTL: usize = 0x40000034;
-    const REG_IRQ_RT: usize = 0x40000024;
-    const REG_IRQ_CLR: usize = 0x40000038;
+pub unsafe fn enable_local_timer() {
+    LOCAL_TIMER.enable();

-    const CTL_TIMER_EN: u32 = 1 << 28;
-    const CTL_IRQ_EN: u32 = 1 << 29;
-    const IRQ_RT_IRQ0: u32 = 0;
-    const IRQ_CLR_VALUE: u32 = (1 << 31) | (1 << 30);
+    let intc = irq::get_intc();
+    irq::set_irq_handler(machine::IRQ_LOCAL_TIMER, &LOCAL_TIMER);
+    intc.enable_irq(machine::IRQ_LOCAL_TIMER);
+}

-    const BASE_FRQ: u32 = 38400000;
-    const TICK: u32 = 1000;
+// TODO bcm283x RTC?
+cfg_if! {
+    if #[cfg(feature = "mach_virt")] {
+        use crate::dev::pl031::Pl031;

-    pub const fn new() -> Self {
-        Self {
-            counter: AtomicU64::new(0),
+        static PL031: Pl031 = Pl031::new(machine::PL031_BASE);
+
+        use PL031 as RTC;
+
+        pub unsafe fn enable_rtc() {
+            RTC.enable();
+
+            let intc = irq::get_intc();
+            irq::set_irq_handler(machine::IRQ_RTC, &RTC);
+            intc.enable_irq(machine::IRQ_RTC);
        }
    }
-
-    pub unsafe fn enable(&self) {
-        let reload = Self::BASE_FRQ / Self::TICK;
-
-        mmio_write(Self::REG_IRQ_RT, Self::IRQ_RT_IRQ0);
-        mmio_write(
-            Self::REG_CTL,
-            reload | Self::CTL_TIMER_EN | Self::CTL_IRQ_EN,
-        );
-    }
-
-    pub fn clear(&self) {
-        unsafe {
-            mmio_write(Self::REG_IRQ_CLR, Self::IRQ_CLR_VALUE);
-        }
-    }
-
-    pub fn do_irq(&self) {
-        self.counter.fetch_add(1, Ordering::Release);
-
-        if self.counter.load(Ordering::Acquire) % Self::TICK as u64 == 0 {
-            debugln!("Tick");
-        }
-
-        self.clear();
-    }
 }

-pub static GLOBAL_TIMER: GlobalTimer = GlobalTimer::new();
+static LOCAL_TIMER: ArmTimer = ArmTimer;
--- a/kernel/src/arch/vectors.S
+++ b/kernel/src/arch/vectors.S
@ -44,7 +44,7 @@ vec_el1_sp_elx_fiq:
    __exc_save_ctx

    mov x0, sp
-    bl do_fiq
+    b .

    __exc_restore_ctx

--- a/kernel/src/boot/entry.S
+++ b/kernel/src/boot/entry.S
@ -4,8 +4,8 @@
 .section .text.boot
 .global _entry
 _entry:
-    mrs x0, mpidr_el1
-    ands x0, x0, #3
+    mrs x1, mpidr_el1
+    ands x1, x1, #3
    beq _entry_bsp

 1:
@ -37,6 +37,10 @@ _ap_wakeup:

 .section .text
 _entry_bsp:
+    // Store FDT address
+    adr x1, fdt_addr
+    str x0, [x1]
+
    // Setup paging tables
    // This is done once for all PEs
    adr x0, kernel_l1
@ -45,11 +49,7 @@ _entry_bsp:
    orr x1, x1, #PAGE_ISH
    str x1, [x0]

-    mov x1, #1 << 30
-    orr x1, x1, #PAGE_PRESENT
-    orr x1, x1, #PAGE_ACCESSED
-    orr x1, x1, #PAGE_ISH
-    orr x1, x1, #1 << PAGE_ATTR_SHIFT
+    orr x1, x1, #1 << 30
    str x1, [x0, #8]

    // Load BSP stack
@ -152,6 +152,8 @@ upper_half:

    msr daifset, #0xF

+    adr x1, fdt_addr
+    ldr x0, [x1]
    br x11


@ -160,6 +162,9 @@ upper_half:
 bsp_stack_bottom:
    .skip 32768
 bsp_stack_top:
+.p2align 3
+fdt_addr:
+    .skip 8
 .p2align 12
 kernel_l1:
    .skip 4096
--- a/kernel/src/debug.rs
+++ b/kernel/src/debug.rs
@ -1,3 +1,4 @@
+use crate::dev::serial::{SerialDevice, SERIAL0};
 use core::fmt;
 use spin::Mutex;

@ -5,9 +6,7 @@ struct Debug;

 impl Debug {
    fn putc(&mut self, ch: u8) {
-        unsafe {
-            core::ptr::write_volatile(0xFFFFFF803F215040 as *mut u32, ch as u32);
-        }
+        SERIAL0.send(ch);
    }
 }

--- a/kernel/src/dev/irq.rs
+++ b/kernel/src/dev/irq.rs
@ -1,17 +1,8 @@
-use crate::{
-    arch::{
-        cpu, mmio_read,
-        timer::{GLOBAL_TIMER, LocalTimer},
-        mbox::CORE_MBOX0,
-    },
-};
-
-const INT_SRC_CNTPNSIRQ: u32 = 1 << 1;
-const INT_SRC_TIMER: u32 = 1 << 11;
-const INT_SRC_MBOX0: u32 = 1 << 4;
+use crate::dev::Device;
+use alloc::collections::LinkedList;

 #[repr(C)]
-struct IrqRegisters {
+pub struct IrqRegisters {
    x0: usize,
    x1: usize,
    x2: usize,
@ -38,29 +29,66 @@ struct IrqRegisters {
    far: usize,
 }

+pub trait InterruptController: Device {
+    unsafe fn enable_irq(&self, irq: u32);
+    unsafe fn disable_irq(&self, irq: u32);
+
+    fn is_irq_pending(&self, irq: u32) -> bool;
+    unsafe fn clear_irq(&self, irq: u32);
+}
+
+pub trait InterruptHandler {
+    fn do_irq(&self, irq: u32);
+}
+
+pub struct InterruptEntry {
+    irq: u32,
+    handler: &'static dyn InterruptHandler,
+}
+
+static mut IRQ_HANDLERS: LinkedList<InterruptEntry> = LinkedList::new();
+
 #[no_mangle]
 extern "C" fn do_irq(_regs: &mut IrqRegisters) {
-    let phys_id = cpu::get_phys_id();
-    let int_src = unsafe { mmio_read(0x40000060 + phys_id * 4) };
-
-    if int_src & INT_SRC_TIMER != 0 {
-        assert!(phys_id == 0);
-        GLOBAL_TIMER.do_irq();
-        return;
+    let intc = get_intc();
+    for entry in unsafe { IRQ_HANDLERS.iter() } {
+        if intc.is_irq_pending(entry.irq) {
+            entry.handler.do_irq(entry.irq);
+            unsafe {
+                intc.clear_irq(entry.irq);
+            }
+        }
    }
-    if int_src & INT_SRC_CNTPNSIRQ != 0 {
-        LocalTimer::do_irq();
-        return;
-    }
-    if int_src & INT_SRC_MBOX0 != 0 {
-        CORE_MBOX0.do_irq();
-        return;
-    }
-    debugln!("Unhandled IRQ: int_src={:#010b}\n", int_src);
 }

-#[no_mangle]
-extern "C" fn do_fiq() {
-    // Only used by core timer
-    LocalTimer::do_irq();
+pub fn set_irq_handler(irq: u32, handler: &'static dyn InterruptHandler) {
+    unsafe {
+        IRQ_HANDLERS.push_front(InterruptEntry { irq, handler });
+    }
+}
+
+cfg_if! {
+    if #[cfg(feature = "mach_rpi3b")] {
+        use crate::arch::mach_bcm283x::INTC;
+
+        pub fn get_intc() -> &'static impl InterruptController {
+            &INTC
+        }
+    } else {
+        pub mod gic;
+        use gic::Gic;
+        use crate::arch::machine;
+
+        pub static GIC: Gic = Gic::new(
+            machine::GICD_BASE, machine::GICC_BASE
+        );
+
+        pub fn get_intc() -> &'static impl InterruptController {
+            &GIC
+        }
+    }
+}
+
+pub unsafe fn init() {
+    get_intc().enable();
 }
--- a/kernel/src/dev/irq/gic.rs
+++ b/kernel/src/dev/irq/gic.rs
@ -0,0 +1,87 @@
+use crate::{
+    arch::{mmio_read, mmio_write},
+    dev::{irq::InterruptController, Device},
+};
+use address::PhysicalAddress;
+
+pub struct Gic {
+    gicd_base: PhysicalAddress,
+    gicc_base: PhysicalAddress,
+}
+
+impl Device for Gic {
+    fn name(&self) -> &'static str {
+        "ARM Generic Interrupt Controller"
+    }
+
+    unsafe fn enable(&self) {
+        mmio_write(self.gicd_base + Self::GICD_CTLR, Self::GICD_CTLR_ENABLE);
+        mmio_write(self.gicc_base + Self::GICC_CTLR, Self::GICC_CTLR_ENABLE);
+        mmio_write(self.gicc_base + Self::GICC_PMR, 0xFF);
+    }
+
+    unsafe fn disable(&self) {
+        todo!()
+    }
+}
+
+impl InterruptController for Gic {
+    unsafe fn enable_irq(&self, irq: u32) {
+        self.set_irq_config(irq, 1);
+        self.unmask_irq(irq);
+    }
+
+    unsafe fn disable_irq(&self, _irq: u32) {
+        todo!();
+    }
+
+    fn is_irq_pending(&self, irq: u32) -> bool {
+        unsafe {
+            mmio_read(self.gicd_base + Self::GICD_ICPENDR + ((irq >> 3) & !0x3) as usize)
+                & (1 << (irq & 0x1F))
+                != 0
+        }
+    }
+
+    unsafe fn clear_irq(&self, irq: u32) {
+        mmio_write(
+            self.gicd_base + Self::GICD_ICPENDR + ((irq >> 3) & !0x3) as usize,
+            1 << (irq & 0x1F),
+        );
+    }
+}
+
+impl Gic {
+    const GICD_CTLR: usize = 0;
+    const GICD_ISENABLER: usize = 0x100;
+    const GICD_ICFGR: usize = 0xC00;
+    const GICD_ICPENDR: usize = 0x280;
+
+    const GICD_CTLR_ENABLE: u32 = 1;
+
+    const GICC_CTLR: usize = 0;
+    const GICC_PMR: usize = 4;
+
+    const GICC_CTLR_ENABLE: u32 = 1;
+
+    pub const fn new(gicd_base: PhysicalAddress, gicc_base: PhysicalAddress) -> Self {
+        Self {
+            gicd_base,
+            gicc_base,
+        }
+    }
+
+    unsafe fn set_irq_config(&self, irq: u32, value: u32) {
+        mmio_write(
+            self.gicd_base + Self::GICD_ICFGR + ((irq >> 4) & !0x3) as usize,
+            value << (1 + (irq & 0xF)),
+        );
+    }
+
+    unsafe fn unmask_irq(&self, irq: u32) {
+        mmio_write(
+            self.gicd_base + Self::GICD_ISENABLER + ((irq >> 3) & !0x3) as usize,
+            1 << (irq & 0x1F),
+        );
+    }
+}
--- a/kernel/src/dev/mod.rs
+++ b/kernel/src/dev/mod.rs
@ -1 +1,11 @@
 pub mod irq;
+pub mod serial;
+
+pub mod pl011;
+pub mod pl031;
+
+pub trait Device {
+    fn name(&self) -> &'static str;
+    unsafe fn enable(&self);
+    unsafe fn disable(&self);
+}
--- a/kernel/src/dev/pl011.rs
+++ b/kernel/src/dev/pl011.rs
@ -0,0 +1,78 @@
+use crate::{
+    arch::{mmio_read, mmio_write},
+    dev::{irq::InterruptHandler, serial::SerialDevice, Device},
+};
+use address::PhysicalAddress;
+
+pub struct Pl011 {
+    base: PhysicalAddress,
+}
+
+impl InterruptHandler for Pl011 {
+    fn do_irq(&self, _irq: u32) {
+        let tmp = unsafe { mmio_read(self.base + Self::UARTRIS) };
+        if tmp & Self::UARTRIS_RXRIS != 0 {
+            let ch = unsafe { mmio_read(self.base + Self::UARTDR) } as u8;
+            debugln!("{}", ch as char);
+            unsafe {
+                mmio_write(self.base + Self::UARTICR, Self::UARTICR_RXIC);
+            }
+        }
+    }
+}
+
+impl Device for Pl011 {
+    fn name(&self) -> &'static str {
+        "PL011 UART"
+    }
+
+    unsafe fn enable(&self) {
+        mmio_write(self.base + Self::UARTCR, 0);
+        mmio_write(self.base + Self::UARTCLR_H, 3 << 5);
+        mmio_write(
+            self.base + Self::UARTIMSC,
+            Self::UARTIMSC_RXIM,
+        );
+        mmio_write(
+            self.base + Self::UARTCR,
+            Self::UARTCR_TXE | Self::UARTCR_RXE | Self::UARTCR_UARTEN,
+        );
+    }
+
+    unsafe fn disable(&self) {}
+}
+
+impl SerialDevice for Pl011 {
+    fn send(&self, ch: u8) {
+        unsafe {
+            while mmio_read(self.base + Self::UARTFR) & Self::UARTFR_BUSY != 0 {}
+            mmio_write(self.base + Self::UARTDR, ch as u32);
+        }
+    }
+}
+
+impl Pl011 {
+    const UARTDR: usize = 0x00;
+    const UARTFR: usize = 0x18;
+    const UARTCLR_H: usize = 0x2C;
+    const UARTCR: usize = 0x30;
+    const UARTIMSC: usize = 0x38;
+    const UARTRIS: usize = 0x3C;
+    const UARTICR: usize = 0x44;
+
+    const UARTCR_UARTEN: u32 = 1 << 0;
+    const UARTCR_TXE: u32 = 1 << 8;
+    const UARTCR_RXE: u32 = 1 << 9;
+
+    const UARTFR_BUSY: u32 = 1 << 3;
+
+    const UARTIMSC_RXIM: u32 = 1 << 4;
+
+    const UARTRIS_RXRIS: u32 = 1 << 4;
+
+    const UARTICR_RXIC: u32 = 1 << 4;
+
+    pub const fn new(base: PhysicalAddress) -> Self {
+        Self { base }
+    }
+}
--- a/kernel/src/dev/pl031.rs
+++ b/kernel/src/dev/pl031.rs
@ -0,0 +1,47 @@
+use crate::{
+    arch::{mmio_read, mmio_write},
+    dev::{irq::InterruptHandler, Device},
+};
+use address::PhysicalAddress;
+
+pub struct Pl031 {
+    base: PhysicalAddress,
+}
+
+impl Pl031 {
+    const RTCDR: usize = 0x00;
+    const RTCMR: usize = 0x04;
+    const RTCCR: usize = 0x0C;
+    const RTCIMSC: usize = 0x10;
+    const RTCICR: usize = 0x1C;
+
+    pub const fn new(base: PhysicalAddress) -> Self {
+        Self { base }
+    }
+}
+
+impl Device for Pl031 {
+    fn name(&self) -> &'static str {
+        "ARM PL031 RTC"
+    }
+
+    unsafe fn enable(&self) {
+        let tmp = mmio_read(self.base + Self::RTCDR);
+        mmio_write(self.base + Self::RTCMR, tmp + 1);
+
+        mmio_write(self.base + Self::RTCIMSC, 1);
+        mmio_write(self.base + Self::RTCCR, 1);
+    }
+
+    unsafe fn disable(&self) {}
+}
+
+impl InterruptHandler for Pl031 {
+    fn do_irq(&self, _irq: u32) {
+        let time_int = unsafe { mmio_read(self.base + Self::RTCDR) };
+        unsafe {
+            mmio_write(self.base + Self::RTCICR, 1);
+            mmio_write(self.base + Self::RTCMR, time_int + 1);
+        }
+    }
+}
--- a/kernel/src/dev/serial.rs
+++ b/kernel/src/dev/serial.rs
@ -0,0 +1,23 @@
+use crate::dev::Device;
+
+pub trait SerialDevice: Device {
+    fn send(&self, ch: u8);
+}
+
+cfg_if! {
+    if #[cfg(feature = "mach_rpi3b")] {
+        use crate::arch::mach_bcm283x;
+
+        pub use mach_bcm283x::UART as SERIAL0;
+    } else {
+        use crate::{dev::pl011::Pl011, arch::machine};
+
+        pub static SERIAL0: Pl011 = Pl011::new(machine::PL011_BASE);
+    }
+}
+
+pub fn init() {
+    unsafe {
+        SERIAL0.enable();
+    }
+}
--- a/kernel/src/fdt.rs
+++ b/kernel/src/fdt.rs
@ -0,0 +1,107 @@
+use crate::{
+    mem::{
+        self,
+        phys::{PageUsage, SimpleMemoryIterator, UsableMemory},
+    },
+    KernelSpace,
+};
+use address::{PhysicalAddress, VirtualAddress};
+use core::mem::MaybeUninit;
+use fdt_rs::{
+    base::{DevTree, DevTreeProp},
+    index::DevTreeIndex,
+    prelude::*,
+};
+
+#[allow(dead_code)]
+struct FdtManager {
+    fdt: DevTree<'static>,
+    index: DevTreeIndex<'static, 'static>,
+}
+
+static mut FDT_MANAGER: MaybeUninit<FdtManager> = MaybeUninit::uninit();
+
+fn read_cell_prop(prop: &DevTreeProp, 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)
+    }
+}
+
+pub fn init(fdt_base_phys: PhysicalAddress) {
+    let fdt_base = VirtualAddress::<KernelSpace>::from(fdt_base_phys);
+    let fdt = unsafe { DevTree::from_raw_pointer(fdt_base.as_ptr()) }.unwrap();
+    let layout = DevTreeIndex::get_layout(&fdt).unwrap();
+    assert!(layout.align() <= 0x1000);
+    let page_count = (layout.size() + 0xFFF) / 0x1000;
+    debugln!("Allocating {} pages for fdt index", page_count);
+    let pages = mem::phys::alloc_contiguous_pages(PageUsage::Kernel, page_count).unwrap();
+    let pages_virt = VirtualAddress::<KernelSpace>::from(pages);
+    let index =
+        DevTreeIndex::new(fdt, unsafe { pages_virt.as_slice_mut(page_count * 0x1000) }).unwrap();
+
+    unsafe {
+        FDT_MANAGER.write(FdtManager { fdt, index });
+    }
+}
+
+pub fn init_phys_memory(fdt_base_phys: PhysicalAddress) {
+    let fdt_base = VirtualAddress::<KernelSpace>::from(fdt_base_phys);
+    let fdt = unsafe { DevTree::from_raw_pointer(fdt_base.as_ptr()) }.unwrap();
+
+    let mut address_cells = None;
+    let mut size_cells = None;
+
+    let mut nodes = fdt.nodes();
+    while let Ok(Some(node)) = nodes.next() {
+        if node.name().unwrap().is_empty() {
+            let mut props = node.props();
+            while let Ok(Some(prop)) = props.next() {
+                let name = prop.name().unwrap();
+                match name {
+                    "#address-cells" => address_cells = prop.u32(0).ok(),
+                    "#size-cells" => size_cells = prop.u32(0).ok(),
+                    _ => {}
+                }
+            }
+        }
+    }
+
+    // Find out system address and size cells
+    let address_cells = address_cells.expect("Failed to find out system's #address-cells");
+    let size_cells = size_cells.expect("Failed to find out system's #size-cells");
+
+    // TODO support multiple memory regions
+    let mut region_start = None;
+    let mut region_size = None;
+
+    let mut nodes = fdt.nodes();
+    while let Ok(Some(node)) = nodes.next() {
+        if node.name().unwrap().starts_with("memory@") {
+            let mut props = node.props();
+            while let Ok(Some(prop)) = props.next() {
+                if prop.name().unwrap() == "reg" {
+                    region_start = read_cell_prop(&prop, 0, address_cells);
+                    region_size = read_cell_prop(&prop, address_cells as usize, size_cells);
+                }
+            }
+        }
+    }
+    let region_start = region_start.unwrap();
+    let region_end = region_start + region_size.unwrap();
+
+    let iter = SimpleMemoryIterator::new(UsableMemory {
+        start: PhysicalAddress::from((region_start + 0xFFF) & !0xFFF),
+        end: PhysicalAddress::from(region_end & !0xFFF),
+    });
+    unsafe {
+        mem::phys::init_from_iter(iter);
+    }
+}
--- a/kernel/src/main.rs
+++ b/kernel/src/main.rs
@ -1,30 +1,40 @@
-#![feature(global_asm, llvm_asm, const_panic, maybe_uninit_uninit_array)]
+#![feature(
+    global_asm,
+    llvm_asm,
+    const_panic,
+    maybe_uninit_uninit_array,
+    alloc_error_handler
+)]
 #![no_std]
 #![no_main]

+#[macro_use]
+extern crate cfg_if;
+extern crate alloc;
+
 #[macro_use]
 pub mod debug;

 pub mod arch;
 pub mod boot;
 pub mod dev;
+#[cfg(feature = "fdt-rs")]
+pub mod fdt;
 pub mod mem;
+pub mod time;

 pub use mem::KernelSpace;

 use address::PhysicalAddress;
-use arch::{
-    cpu, intrin,
-    mbox::{self, CORE_MBOX0},
-    smp,
-    timer::{LocalTimer, LocalTimerMode, GLOBAL_TIMER},
-};
-use mem::phys::{SimpleMemoryIterator, UsableMemory};
+use arch::{timer, cpu, intrin, smp};
+use dev::irq::{self, InterruptController};

 pub fn entry_common() -> ! {
+    smp::init_ipi_delivery();
+
    unsafe {
-        CORE_MBOX0.enable();
-        LocalTimer::enable(LocalTimerMode::Irq);
+        irq::init();
+        timer::enable_local_timer();
        intrin::enable_irq();
    }

@ -32,25 +42,32 @@ pub fn entry_common() -> ! {
 }

 #[no_mangle]
-extern "C" fn kernel_bsp_main() -> ! {
+extern "C" fn kernel_bsp_main(fdt_base: PhysicalAddress) -> ! {
    cpu::init(0);
+    dev::serial::init();

-    let arm_memory = mbox::get_arm_memory().expect("Failed to determine ARM memory");
+    cfg_if! {
+        if #[cfg(feature = "fdt-rs")] {
+            // Initialize memory from FDT information
+            fdt::init_phys_memory(fdt_base);
+            fdt::init(fdt_base);
+        } else {
+            // Platform-specific memory init
+            mem::phys::init_from_platform();
+        }
+    }
+    mem::heap::init();
+    arch::machine::init();

-    let iter = SimpleMemoryIterator::new(UsableMemory {
-        start: PhysicalAddress::from(0usize),
-        end: PhysicalAddress::from(arm_memory),
-    });
+    // Enable IRQs for SERIAL0
+    let intc = irq::get_intc();
+    irq::set_irq_handler(arch::machine::IRQ_UART, &dev::serial::SERIAL0);
    unsafe {
-        mem::phys::initialize(iter);
+        intc.enable_irq(arch::machine::IRQ_UART);
    }

    debug!("BSP init finished\n");
-
-    unsafe {
-        GLOBAL_TIMER.enable();
-    }
-    smp::wakeup_ap_cpus(4);
+    //smp::wakeup_ap_cpus();
    entry_common();
 }

@ -62,7 +79,7 @@ fn panic_handler(pi: &PanicInfo) -> ! {
    }
    debug!("PANIC: {:?}\n", pi);
    unsafe {
-        smp::send_ipi(usize::MAX, smp::IPI_HALT);
+        smp::send_ipi(usize::MAX, smp::IpiMessage::Halt);
    }
    loop {}
 }
--- a/kernel/src/mem/heap.rs
+++ b/kernel/src/mem/heap.rs
@ -0,0 +1,209 @@
+// TODO document this module
+#![allow(missing_docs)]
+use crate::{
+    mem::{
+        self,
+        phys::{self, PageUsage},
+    },
+    KernelSpace,
+};
+use address::VirtualAddress;
+use core::alloc::{GlobalAlloc, Layout};
+use core::convert::TryFrom;
+use core::mem::{size_of, MaybeUninit};
+use core::ptr::null_mut;
+use spin::Mutex;
+
+const HEAP_MAGIC: u32 = 0xB0000BA0;
+const HEAP_SIZE: usize = 16 << 20; // 16MiB
+
+struct Heap {
+    start: VirtualAddress<KernelSpace>,
+    mutex: MaybeUninit<Mutex<()>>,
+}
+
+#[derive(Debug)]
+struct HeapBlock {
+    magic: u32,
+    size: u32,
+    previous: *mut HeapBlock,
+    next: *mut HeapBlock,
+}
+
+unsafe impl GlobalAlloc for Heap {
+    #[track_caller]
+    unsafe fn alloc(&self, layout: Layout) -> *mut u8 {
+        // Align the layout by 16 bytes
+        let count = ((layout.size() + 15) & !15) as u32;
+
+        // NOTE: that shouldn't be optimized away
+        let _lock = self.mutex.assume_init_ref().lock();
+
+        // Check if the memory is corrupted
+        let mut block_it = self.first();
+        while let Some(ref mut block) = block_it {
+            if (block.magic & HEAP_MAGIC) != HEAP_MAGIC {
+                panic!("Heap is out: block {:?}", block);
+            }
+            block_it = block.next_mut();
+        }
+
+        let mut block_it = self.first();
+        while let Some(ref mut block) = block_it {
+            if !block.is_available() {
+                block_it = block.next_mut();
+                continue;
+            }
+            if count == block.size {
+                block.take();
+                return block.data();
+            } else if count + size_of::<HeapBlock>() as u32 <= block.size {
+                block.split(count);
+                block.take();
+                return block.data();
+            }
+
+            block_it = block.next_mut();
+        }
+
+        null_mut()
+    }
+
+    unsafe fn dealloc(&self, ptr: *mut u8, layout: Layout) {
+        // NOTE: that shouldn't be optimized away
+        let _lock = self.mutex.assume_init_ref().lock();
+        let address = VirtualAddress::from_ptr(ptr);
+
+        // Check heap boundaries
+        if address < self.start
+            || address > self.start + HEAP_SIZE
+            || address + layout.size() > self.start + HEAP_SIZE
+        {
+            panic!("Deallocating out of heap");
+        };
+
+        // Get the block
+        let block: &mut HeapBlock = (address - size_of::<HeapBlock>()).as_mut().unwrap();
+
+        // Check the magic and shit
+        if (block.magic & !1) != HEAP_MAGIC {
+            panic!(
+                "Heap block is corrupted: magic {:#018x} {:?}",
+                block.magic, block
+            );
+        }
+
+        // Free that shit
+        block.free();
+
+        // Check if the adjacent blocks are available
+        // and merge them in
+        if let Some(next) = block.next.as_mut() {
+            if next.is_available() {
+                block.size += (*block.next).size;
+                block.next = (*block.next).next;
+            }
+        }
+        if let Some(previous) = block.previous.as_mut() {
+            if previous.is_available() {
+                previous.size += block.size;
+                previous.next = block.next;
+            }
+        }
+
+        // TODO: cfg debug turn the memory into junk
+    }
+}
+
+impl Heap {
+    pub unsafe fn place(addr: VirtualAddress<KernelSpace>) -> Self {
+        let block: &mut HeapBlock = addr.as_mut().unwrap();
+        block.magic = HEAP_MAGIC;
+        block.size = (HEAP_SIZE - size_of::<Heap>()) as u32;
+        block.previous = null_mut();
+        block.next = null_mut();
+        Heap {
+            start: addr,
+            mutex: MaybeUninit::new(Mutex::new(())),
+        }
+    }
+
+    fn first(&self) -> Option<&'static mut HeapBlock> {
+        unsafe { self.start.as_mut::<HeapBlock>() }
+    }
+
+    #[allow(dead_code)]
+    fn dump(&self) {
+        // NOTE: that shouldn't be optimized away
+        let _lock = unsafe { self.mutex.assume_init_ref().lock() };
+
+        let mut block_it = self.first();
+        while let Some(ref mut block) = block_it {
+            debugln!("{:p}: {:?}", *block, block);
+            block_it = block.next_mut();
+        }
+    }
+}
+
+impl HeapBlock {
+    fn next_mut(&mut self) -> Option<&'static mut HeapBlock> {
+        unsafe { self.next.as_mut() }
+    }
+
+    fn is_available(&self) -> bool {
+        self.magic & 1 == 0
+    }
+
+    fn free(&mut self) {
+        self.magic = HEAP_MAGIC;
+    }
+
+    fn take(&mut self) {
+        self.magic |= 1;
+    }
+
+    fn split(&mut self, size: u32) -> &'static mut HeapBlock {
+        assert!(size + size_of::<HeapBlock>() as u32 <= self.size);
+        unsafe {
+            let mut new_block = &mut *(self.data().add(size as usize) as *mut HeapBlock);
+            new_block.previous = self;
+            new_block.next = self.next;
+            if let Some(ref mut p) = self.next_mut() {
+                p.previous = new_block;
+            };
+            new_block.magic = HEAP_MAGIC;
+            new_block.size = self.size - size - size_of::<HeapBlock>() as u32;
+            self.next = new_block;
+            self.size = size;
+            new_block
+        }
+    }
+
+    fn data(&mut self) -> *mut u8 {
+        unsafe { (self as *mut _ as *mut u8).add(size_of::<HeapBlock>()) }
+    }
+}
+
+#[global_allocator]
+static mut KERNEL_HEAP: Heap = Heap {
+    start: VirtualAddress::null(),
+    mutex: MaybeUninit::uninit(),
+};
+
+#[alloc_error_handler]
+fn alloc_error_handler(layout: Layout) -> ! {
+    panic!("Failed to allocate {:?}", layout);
+}
+
+/// Fuck
+pub fn init() {
+    let base_addr_phys =
+        phys::alloc_contiguous_pages(PageUsage::Kernel, HEAP_SIZE / mem::PAGE_SIZE).unwrap();
+    let base_addr = VirtualAddress::try_from(base_addr_phys).unwrap();
+
+    unsafe {
+        KERNEL_HEAP = Heap::place(base_addr);
+    };
+
+    debugln!("Heap: {:?}", base_addr);
+}
--- a/kernel/src/mem/mod.rs
+++ b/kernel/src/mem/mod.rs
@ -1,6 +1,7 @@
 use address::{AddressSpace, PhysicalAddress, TrivialConvert};

 pub mod phys;
+pub mod heap;

 #[derive(Copy, Clone, PartialEq, PartialOrd)]
 pub struct KernelSpace;
--- a/kernel/src/mem/phys/manager.rs
+++ b/kernel/src/mem/phys/manager.rs
@ -19,10 +19,15 @@ pub unsafe trait Manager {

 pub struct SimpleManager {
    pages: &'static mut [Mutex<PageInfo>],
+    base_index: usize,
 }

 impl SimpleManager {
-    pub(super) unsafe fn initialize(at: PhysicalAddress, count: usize) -> Self {
+    pub(super) unsafe fn initialize(
+        base: PhysicalAddress,
+        at: PhysicalAddress,
+        count: usize,
+    ) -> Self {
        let pages: &'static mut [Mutex<PageInfo>] =
            VirtualAddress::<KernelSpace>::from(at).as_slice_mut(count);

@ -37,16 +42,19 @@ impl SimpleManager {
            ));
        }

-        Self { pages }
+        Self {
+            base_index: base.page_index(),
+            pages,
+        }
    }

    pub(super) unsafe fn add_page(&mut self, addr: PhysicalAddress) {
-        let mut page = self.pages[addr.page_index()].lock();
+        let mut page = self.pages[addr.page_index() - self.base_index].lock();
        assert!(page.refcount == 0 && page.usage == PageUsage::Reserved);
        page.usage = PageUsage::Available;

        // Fill the page with trash
-        let slice: &mut [u8; 4096] = VirtualAddress::<KernelSpace>::from(addr).as_mut();
+        let slice: &mut [u8; 4096] = VirtualAddress::<KernelSpace>::from(addr).as_mut().unwrap();
        slice.fill(0);
    }
 }
@ -59,7 +67,7 @@ unsafe impl Manager for SimpleManager {
            if page.usage == PageUsage::Available {
                page.usage = pu;
                page.refcount = 1;
-                return Ok(PhysicalAddress::from(index * PAGE_SIZE));
+                return Ok(PhysicalAddress::from((self.base_index + index) * PAGE_SIZE));
            }
        }
        Err(Errno::OutOfMemory)
@ -83,7 +91,7 @@ unsafe impl Manager for SimpleManager {
                page.refcount = 1;
            }

-            return Ok(PhysicalAddress::from(i * PAGE_SIZE));
+            return Ok(PhysicalAddress::from((self.base_index + i) * PAGE_SIZE));
        }
        Err(Errno::OutOfMemory)
    }
--- a/kernel/src/mem/phys/mod.rs
+++ b/kernel/src/mem/phys/mod.rs
@ -52,7 +52,7 @@ impl Iterator for SimpleMemoryIterator {
    }
 }

-const MAX_PAGES: usize = 1024;
+const MAX_PAGES: usize = 16 * 1024;

 pub fn alloc_page(pu: PageUsage) -> Result<PhysicalAddress, Errno> {
    MANAGER.lock().as_mut().unwrap().alloc_page(pu)
@ -98,7 +98,17 @@ fn find_contiguous<T: Iterator<Item = UsableMemory>>(
    None
 }

-pub unsafe fn initialize<T: Iterator<Item = UsableMemory> + Clone>(iter: T) {
+pub unsafe fn init_from_iter<T: Iterator<Item = UsableMemory> + Clone>(iter: T) {
+    let mut mem_base = PhysicalAddress::from(usize::MAX);
+
+    for reg in iter.clone() {
+        if reg.start < mem_base {
+            mem_base = reg.start;
+        }
+    }
+
+    debugln!("Memory base is {:?}", mem_base);
+
    // Step 1. Count available memory
    let mut total_pages = 0usize;
    for reg in iter.clone() {
@ -117,7 +127,7 @@ pub unsafe fn initialize<T: Iterator<Item = UsableMemory> + Clone>(iter: T) {
    reserved::reserve_pages(pages_base, need_pages);

    // Step 3. Initialize the memory manager with available pages
-    let mut manager = ManagerImpl::initialize(pages_base, total_pages);
+    let mut manager = ManagerImpl::initialize(mem_base, pages_base, total_pages);

    let mut usable_pages = 0usize;
    'l0: for region in iter {
@ -136,3 +146,12 @@ pub unsafe fn initialize<T: Iterator<Item = UsableMemory> + Clone>(iter: T) {

    *MANAGER.lock() = Some(manager);
 }
+
+pub fn init_from_platform() {
+    cfg_if! {
+        if #[cfg(feature = "mach_rpi3b")] {
+            use crate::arch::mach_bcm283x;
+            mach_bcm283x::init_phys_memory();
+        }
+    }
+}
--- a/kernel/src/time.rs
+++ b/kernel/src/time.rs
@ -0,0 +1,9 @@
+#[derive(Debug)]
+pub struct Time {
+    pub year: u32,
+    pub mday: u8,
+    pub mon: u8,
+    pub hour: u8,
+    pub min: u8,
+    pub sec: u8
+}
--- a/qemu.sh
+++ b/qemu.sh
@ -6,22 +6,41 @@ if [ -z "${MACH}" ]; then
    MACH=rpi3b
 fi

+if [ -z "$QEMU_BIN" ]; then
+    QEMU_BIN=qemu-system-aarch64
+fi
+
 ARCH=aarch64-unknown-none-${MACH}
 KERNEL=target/${ARCH}/debug/kernel

-QEMU_OPTS=""
+QEMU_OPTS="-chardev stdio,nowait,id=char0,mux=on \
+           -mon chardev=char0"
+
+if [ "$QEMU_DINT" = 1 ]; then
+    QEMU_OPTS="$QEMU_OPTS -d int"
+fi
+
 case ${MACH} in
    rpi3b)
        QEMU_OPTS="$QEMU_OPTS \
                   -serial null \
-                   -serial stdio \
+                   -serial chardev:char0 \
+                   -dtb bcm2837-rpi-3-b.dtb \
                   -M raspi3b"
        ;;
+    virt)
+        KERNEL=target/${ARCH}/debug/kernel.bin
+        QEMU_OPTS="$QEMU_OPTS \
+                   -serial chardev:char0 \
+                   -M virt,virtualization=on \
+                   -cpu cortex-a57 \
+                   -m 256"
 esac
 QEMU_OPTS="$QEMU_OPTS \
           -kernel ${KERNEL} \
+           -display none \
           -s"

 ./build.sh

-qemu-system-aarch64 ${QEMU_OPTS}
+${QEMU_BIN} ${QEMU_OPTS}
Author	SHA1	Message	Date
Mark Poliakov	74eedfe540	UART	2021-09-03 15:48:32 +03:00
Mark Poliakov	06f5aabdb0	Clean up warnings	2021-09-03 11:01:55 +03:00
Mark Poliakov	0e3647283c	Better abstraction for platform support	2021-09-03 10:53:20 +03:00
Mark Poliakov	2f405ef17b	Rewrite platform-specific stuff a bit	2021-09-02 10:31:34 +03:00