doc: add pci subsystem docs

refactor: rm aarch64 entry.S
fix: unify boot code
2021-10-11 19:10:07 +03:00 · 2021-10-11 18:54:48 +03:00 · 2021-10-11 18:54:36 +03:00 · 2021-10-11 18:36:25 +03:00 · 2021-10-11 18:36:08 +03:00 · 2021-10-11 18:35:42 +03:00
37 changed files with 1593 additions and 247 deletions
--- a/Cargo.lock
+++ b/Cargo.lock
@ -2,6 +2,12 @@
 # It is not intended for manual editing.
 version = 3

+[[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"
@ -17,10 +23,38 @@ dependencies = [
 "tock-registers",
 ]

+[[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"
@ -28,15 +62,116 @@ dependencies = [
 "cfg-if",
 "cortex-a",
 "error",
+ "fdt-rs",
 "tock-registers",
 ]

+[[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 = "osdev5"
 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.10"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "38bc8cc6a5f2e3655e0899c1b848643b2562f853f114bfec7be120678e3ace05"
+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 = "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 = "static_assertions"
+version = "1.1.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "a2eb9349b6444b326872e140eb1cf5e7c522154d69e7a0ffb0fb81c06b37543f"
+
+[[package]]
+name = "syn"
+version = "1.0.80"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d010a1623fbd906d51d650a9916aaefc05ffa0e4053ff7fe601167f3e715d194"
+dependencies = [
+ "proc-macro2",
+ "quote",
+ "unicode-xid",
+]
+
 [[package]]
 name = "tock-registers"
 version = "0.7.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "4ee8fba06c1f4d0b396ef61a54530bb6b28f0dc61c38bc8bc5a5a48161e6282e"
+
+[[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/29
+++ b/29
@ -25,8 +25,8 @@ ifeq ($(ARCH),x86_64)
 $(error TODO)
 else
 ifeq ($(MACH),qemu)
-QEMU_OPTS+=-kernel $(O)/kernel.bin \
-		   -M virt,virtualization=off \
+QEMU_OPTS+=-kernel $(O)/kernel \
+		   -M virt,virtualization=on \
 		   -cpu cortex-a72 \
 		   -m 512 \
 		   -serial chardev:serial0 \
@ -40,18 +40,33 @@ QEMU_OPTS+=-kernel $(O)/kernel.bin \
 endif
 endif

+ifeq ($(QEMU_DINT),1)
+QEMU_OPTS+=-d int
+endif
+ifeq ($(QEMU_PAUSE),1)
+QEMU_OPTS+=-S
+endif
+
 .PHONY: address error etc kernel src

 all: kernel

 kernel:
 	cd kernel && cargo build $(CARGO_BUILD_OPTS)
-ifeq ($(ARCH),aarch64)
-	$(OBJCOPY) -O binary $(O)/kernel $(O)/kernel.bin
-endif
 ifeq ($(MACH),orangepi3)
-	$(LLVM_BASE)/llvm-strip $(O)/kernel
-	$(LLVM_BASE)/llvm-size $(O)/kernel
+	$(LLVM_BASE)/llvm-strip -o $(O)/kernel.strip $(O)/kernel
+	$(LLVM_BASE)/llvm-size $(O)/kernel.strip
+	$(OBJCOPY) -O binary $(O)/kernel.strip $(O)/kernel.bin
+	$(MKIMAGE) \
+		-A arm64 \
+		-O linux \
+		-T kernel \
+		-C none \
+		-a 0x48000000 \
+		-e 0x48000000 \
+		-n kernel \
+		-d $(O)/kernel.bin \
+		$(O)/uImage
 endif

 clean:
--- a/address/Cargo.toml
+++ b/address/Cargo.toml
@ -0,0 +1,9 @@
+[package]
+name = "address"
+version = "0.1.0"
+edition = "2018"
+
+# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
+
+[dependencies]
+error = { path = "../error" }
--- a/address/src/base/mod.rs
+++ b/address/src/base/mod.rs
--- a/address/src/lib.rs
+++ b/address/src/lib.rs
@ -0,0 +1,16 @@
+//! Type-safe wrappers for different address kinds
+#![no_std]
+#![feature(step_trait, const_fn_trait_bound, const_trait_impl, const_panic)]
+
+#[cfg(test)]
+#[macro_use]
+extern crate std;
+
+#[deny(missing_docs)]
+pub mod phys;
+pub mod virt;
+
+trait Address {}
+
+pub use phys::PhysicalAddress;
+pub use virt::{AddressSpace, NoTrivialConvert, TrivialConvert, VirtualAddress};
--- a/address/src/phys.rs
+++ b/address/src/phys.rs
@ -0,0 +1,183 @@
+//! Module for handling physical memory addresses
+
+use crate::{AddressSpace, TrivialConvert, VirtualAddress};
+use core::convert::TryFrom;
+use core::fmt;
+use core::iter::Step;
+use core::ops::{Add, AddAssign, Sub, SubAssign};
+
+/// Wrapper struct for representing a physical address
+#[repr(transparent)]
+#[derive(PartialEq, PartialOrd, Copy, Clone)]
+pub struct PhysicalAddress(usize);
+
+// Arithmetic
+impl const Add<usize> for PhysicalAddress {
+    type Output = Self;
+
+    #[inline(always)]
+    fn add(self, rhs: usize) -> Self {
+        // Will panic on overflow
+        Self::from(self.0 + rhs)
+    }
+}
+impl<A: Into<usize>> AddAssign<A> for PhysicalAddress {
+    #[inline(always)]
+    fn add_assign(&mut self, rhs: A) {
+        // Will panic on overflow
+        *self = Self::from(self.0 + rhs.into());
+    }
+}
+impl const Sub<usize> for PhysicalAddress {
+    type Output = Self;
+
+    #[inline(always)]
+    fn sub(self, rhs: usize) -> Self {
+        Self::from(self.0 - rhs)
+    }
+}
+impl SubAssign<usize> for PhysicalAddress {
+    #[inline(always)]
+    fn sub_assign(&mut self, rhs: usize) {
+        *self = Self::from(self.0 - rhs);
+    }
+}
+
+// Construction
+impl const From<usize> for PhysicalAddress {
+    fn from(p: usize) -> Self {
+        Self(p)
+    }
+}
+
+#[cfg(target_pointer_width = "64")]
+impl const From<u64> for PhysicalAddress {
+    fn from(p: u64) -> Self {
+        Self(p as usize)
+    }
+}
+
+impl PhysicalAddress {
+    /// Computes a signed difference between `start` and `end`
+    /// addresses. Will panic if the result cannot be represented due to
+    /// overflow.
+    #[inline(always)]
+    pub fn diff(start: PhysicalAddress, end: PhysicalAddress) -> isize {
+        if end >= start {
+            isize::try_from(end.0 - start.0).expect("Address subtraction overflowed")
+        } else {
+            -isize::try_from(start.0 - end.0).expect("Address subtraction overflowed")
+        }
+    }
+
+    /// Computes a signed difference between `start` and `end`, does not check for
+    /// overflow condition.
+    ///
+    /// # Safety
+    ///
+    /// Does not check for signed integer overflow.
+    #[inline(always)]
+    pub unsafe fn diff_unchecked(start: PhysicalAddress, end: PhysicalAddress) -> isize {
+        end.0 as isize - start.0 as isize
+    }
+
+    /// Returns `true` if the address is page-aligned
+    #[inline(always)]
+    pub const fn is_paligned(self) -> bool {
+        self.0 & 0xFFF == 0
+    }
+
+    /// Returns the index of the 4K page containing the address
+    #[inline(always)]
+    pub const fn page_index(self) -> usize {
+        self.0 >> 12
+    }
+}
+
+// Trivial conversion PhysicalAddress -> VirtualAddress
+impl<T: AddressSpace + TrivialConvert> const From<PhysicalAddress> for VirtualAddress<T> {
+    fn from(p: PhysicalAddress) -> Self {
+        VirtualAddress::from(p.0 + T::OFFSET)
+    }
+}
+
+impl const From<PhysicalAddress> for usize {
+    #[inline(always)]
+    fn from(p: PhysicalAddress) -> Self {
+        p.0 as usize
+    }
+}
+
+#[cfg(target_pointer_width = "64")]
+impl const From<PhysicalAddress> for u64 {
+    #[inline(always)]
+    fn from(p: PhysicalAddress) -> Self {
+        p.0 as u64
+    }
+}
+
+// Formatting
+impl fmt::Debug for PhysicalAddress {
+    #[inline]
+    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
+        write!(f, "<phys {:#018x}>", self.0)
+    }
+}
+
+// Step
+impl Step for PhysicalAddress {
+    #[inline]
+    fn steps_between(_p0: &Self, _p1: &Self) -> Option<usize> {
+        todo!()
+    }
+
+    #[inline]
+    fn forward_checked(p: Self, steps: usize) -> Option<Self> {
+        p.0.checked_add(steps).map(Self::from)
+    }
+
+    #[inline]
+    fn backward_checked(p: Self, steps: usize) -> Option<Self> {
+        p.0.checked_sub(steps).map(Self::from)
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use crate::{AddressSpace, NoTrivialConvert, TrivialConvert, VirtualAddress};
+
+    #[derive(Copy, Clone, PartialEq, PartialOrd)]
+    struct S0;
+    impl AddressSpace for S0 {
+        const NAME: &'static str = "S0";
+        const OFFSET: usize = 0x8000;
+        const LIMIT: usize = Self::OFFSET + 0x4000;
+    }
+    impl TrivialConvert for S0 {}
+
+    #[derive(Copy, Clone, PartialEq, PartialOrd)]
+    struct S1;
+    impl AddressSpace for S1 {
+        const NAME: &'static str = "S1";
+        const OFFSET: usize = 0;
+        const LIMIT: usize = 0;
+    }
+    impl NoTrivialConvert for S1 {}
+
+    #[test]
+    fn test_virt_convert_valid() {
+        let p0 = PhysicalAddress::from(0x1234usize);
+        assert_eq!(
+            VirtualAddress::<S0>::from(p0),
+            VirtualAddress::<S0>::from(0x9234usize)
+        );
+    }
+
+    #[test]
+    #[should_panic]
+    fn test_virt_convert_invalid() {
+        let p0 = PhysicalAddress::from(0x4321usize);
+        let _v = VirtualAddress::<S0>::from(p0);
+    }
+}
--- a/address/src/virt.rs
+++ b/address/src/virt.rs
@ -0,0 +1,361 @@
+use super::PhysicalAddress;
+use core::convert::TryFrom;
+use core::fmt;
+use core::iter::Step;
+use core::marker::PhantomData;
+use core::ops::{Add, AddAssign, Neg, Sub, SubAssign};
+
+pub trait AddressSpace: Copy + Clone + PartialEq + PartialOrd {
+    const NAME: &'static str;
+    const OFFSET: usize;
+    const LIMIT: usize;
+}
+
+pub trait NoTrivialConvert {}
+pub trait TrivialConvert {}
+
+#[repr(transparent)]
+#[derive(Copy, Clone, PartialOrd, PartialEq)]
+pub struct VirtualAddress<Kind: AddressSpace>(usize, PhantomData<Kind>);
+
+// Arithmetic
+impl<T: AddressSpace> Add<usize> for VirtualAddress<T> {
+    type Output = Self;
+
+    #[inline(always)]
+    fn add(self, rhs: usize) -> Self {
+        // Will panic on overflow
+        Self::from(self.0 + rhs)
+    }
+}
+impl<T: AddressSpace> AddAssign<usize> for VirtualAddress<T> {
+    #[inline(always)]
+    fn add_assign(&mut self, rhs: usize) {
+        // Will panic on overflow
+        *self = Self::from(self.0 + rhs);
+    }
+}
+impl<T: AddressSpace> Sub<usize> for VirtualAddress<T> {
+    type Output = Self;
+
+    #[inline(always)]
+    fn sub(self, rhs: usize) -> Self {
+        // Will panic on underflow
+        Self::from(self.0 - rhs)
+    }
+}
+impl<T: AddressSpace> SubAssign<usize> for VirtualAddress<T> {
+    #[inline(always)]
+    fn sub_assign(&mut self, rhs: usize) {
+        // Will panic on underflow
+        *self = Self::from(self.0 - rhs);
+    }
+}
+
+// Trivial conversion VirtualAddress -> PhysicalAddress
+impl<T: AddressSpace + TrivialConvert> From<VirtualAddress<T>> for PhysicalAddress {
+    #[inline(always)]
+    fn from(virt: VirtualAddress<T>) -> Self {
+        assert!(virt.0 < T::LIMIT);
+        PhysicalAddress::from(virt.0 - T::OFFSET)
+    }
+}
+
+// Formatting
+impl<T: AddressSpace> fmt::Debug for VirtualAddress<T> {
+    #[inline]
+    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
+        write!(f, "<{} {:#018x}>", T::NAME, self.0)
+    }
+}
+
+impl<T: AddressSpace> VirtualAddress<T> {
+    #[inline(always)]
+    pub const fn null() -> Self {
+        Self(0, PhantomData)
+    }
+
+    pub fn try_subtract(self, p: usize) -> Option<Self> {
+        let (res, overflow) = self.0.overflowing_sub(p);
+        if overflow || res < T::OFFSET || res >= T::LIMIT {
+            None
+        } else {
+            Some(Self(res, PhantomData))
+        }
+    }
+
+    #[inline]
+    pub fn diff(start: Self, end: Self) -> isize {
+        if end >= start {
+            isize::try_from(end.0 - start.0).expect("Address subtraction overflowed")
+        } else {
+            -isize::try_from(start.0 - end.0).expect("Address subtraction overflowed")
+        }
+    }
+
+    #[inline(always)]
+    pub fn try_diff(start: Self, end: Self) -> Option<isize> {
+        if end >= start {
+            isize::try_from(end.0 - start.0).ok()
+        } else {
+            isize::try_from(start.0 - end.0).map(Neg::neg).ok()
+        }
+    }
+
+    #[inline(always)]
+    pub unsafe fn as_slice_mut<U>(self, count: usize) -> &'static mut [U] {
+        core::slice::from_raw_parts_mut(self.0 as *mut _, count)
+    }
+
+    #[inline(always)]
+    pub fn as_mut_ptr<U>(self) -> *mut U {
+        self.0 as *mut U
+    }
+
+    #[inline(always)]
+    pub fn as_ptr<U>(self) -> *const U {
+        self.0 as *const U
+    }
+
+    #[inline(always)]
+    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)]
+    pub unsafe fn from_ref<U>(r: &U) -> Self {
+        Self(r as *const U as usize, PhantomData)
+    }
+}
+
+// Step
+impl<T: AddressSpace> Step for VirtualAddress<T> {
+    #[inline]
+    fn steps_between(_p0: &Self, _p1: &Self) -> Option<usize> {
+        todo!()
+    }
+
+    #[inline]
+    fn forward_checked(p: Self, steps: usize) -> Option<Self> {
+        p.0.checked_add(steps).map(Self::from)
+    }
+
+    #[inline]
+    fn backward_checked(p: Self, steps: usize) -> Option<Self> {
+        p.0.checked_sub(steps).map(Self::from)
+    }
+}
+
+// Conversion into VirtualAddress
+impl<T: AddressSpace> const From<usize> for VirtualAddress<T> {
+    #[inline(always)]
+    fn from(p: usize) -> Self {
+        if T::LIMIT > 0 {
+            assert!(p >= T::OFFSET && p < T::LIMIT);
+        }
+        Self(p, PhantomData)
+    }
+}
+
+#[cfg(target_pointer_width = "64")]
+impl<T: AddressSpace> From<u64> for VirtualAddress<T> {
+    #[inline(always)]
+    fn from(p: u64) -> Self {
+        Self::from(p as usize)
+    }
+}
+
+// Conversion from VirtualAddress
+impl<T: AddressSpace> From<VirtualAddress<T>> for usize {
+    #[inline(always)]
+    fn from(p: VirtualAddress<T>) -> Self {
+        p.0
+    }
+}
+
+#[cfg(target_pointer_width = "64")]
+impl<T: AddressSpace> From<VirtualAddress<T>> for u64 {
+    #[inline(always)]
+    fn from(p: VirtualAddress<T>) -> Self {
+        p.0 as u64
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use crate::PhysicalAddress;
+
+    #[derive(Copy, Clone, PartialEq, PartialOrd)]
+    struct S0;
+    impl AddressSpace for S0 {
+        const NAME: &'static str = "S0";
+        const OFFSET: usize = 0x8000;
+        const LIMIT: usize = Self::OFFSET + 0x4000;
+    }
+    impl TrivialConvert for S0 {}
+
+    #[derive(Copy, Clone, PartialEq, PartialOrd)]
+    struct S1;
+    impl AddressSpace for S1 {
+        const NAME: &'static str = "S1";
+        const OFFSET: usize = 0;
+        const LIMIT: usize = 0;
+    }
+    impl NoTrivialConvert for S1 {}
+
+    #[test]
+    fn test_trivial_construct_valid() {
+        for i in 0x8000usize..0xC000 {
+            VirtualAddress::<S0>::from(i);
+        }
+    }
+
+    #[test]
+    #[should_panic]
+    fn test_trivial_construct_invalid_0() {
+        let _v = VirtualAddress::<S0>::from(0x1234usize);
+    }
+
+    #[test]
+    #[should_panic]
+    fn test_trivial_construct_invalid_1() {
+        let _v = VirtualAddress::<S0>::from(0xD123usize);
+    }
+
+    #[test]
+    fn test_trivial_convert() {
+        let v0 = VirtualAddress::<S0>::from(0x8123usize);
+        assert_eq!(PhysicalAddress::from(v0), PhysicalAddress::from(0x123usize));
+    }
+
+    #[test]
+    fn test_add_valid() {
+        let v0 = VirtualAddress::<S0>::from(0x8100usize);
+        assert_eq!(VirtualAddress::<S0>::from(0x8223usize), v0 + 0x123usize);
+    }
+
+    #[test]
+    #[should_panic]
+    fn test_add_overflow() {
+        let v0 = VirtualAddress::<S0>::from(0x8100usize);
+        let _v = v0 - 0xF123usize;
+    }
+
+    #[test]
+    fn test_subtract_valid() {
+        let v0 = VirtualAddress::<S0>::from(0x8100usize);
+        assert_eq!(VirtualAddress::<S0>::from(0x8023usize), v0 - 0xDDusize);
+    }
+
+    #[test]
+    #[should_panic]
+    fn test_subtract_overflow() {
+        let v0 = VirtualAddress::<S0>::from(0x8100usize);
+        let _v = v0 - 0x1234usize;
+    }
+
+    #[test]
+    fn test_try_subtract() {
+        let v0 = VirtualAddress::<S0>::from(0x8100usize);
+        assert_eq!(v0.try_subtract(0x1234usize), None);
+        assert_eq!(
+            v0.try_subtract(0x12usize),
+            Some(VirtualAddress::<S0>::from(0x80EEusize))
+        );
+    }
+
+    #[test]
+    fn test_add_assign_valid() {
+        let mut v0 = VirtualAddress::<S0>::from(0x8100usize);
+        v0 += 0x123usize;
+        assert_eq!(v0, VirtualAddress::<S0>::from(0x8223usize));
+    }
+
+    #[test]
+    fn test_sub_assign_valid() {
+        let mut v0 = VirtualAddress::<S0>::from(0x8321usize);
+        v0 -= 0x123usize;
+        assert_eq!(v0, VirtualAddress::<S0>::from(0x81FEusize));
+    }
+
+    #[test]
+    #[should_panic]
+    fn test_sub_assign_overflow() {
+        let mut v0 = VirtualAddress::<S0>::from(0x8321usize);
+        v0 -= 0x1234usize;
+    }
+
+    #[test]
+    #[should_panic]
+    fn test_add_assign_overflow() {
+        let mut v0 = VirtualAddress::<S0>::from(0x8321usize);
+        v0 += 0xF234usize;
+    }
+
+    #[test]
+    fn test_format() {
+        let v0 = VirtualAddress::<S0>::from(0x8123usize);
+        assert_eq!(&format!("{:?}", v0), "<S0 0x0000000000008123>");
+    }
+
+    #[test]
+    fn test_diff() {
+        let v0 = VirtualAddress::<S0>::from(0x8123usize);
+        let v1 = VirtualAddress::<S0>::from(0x8321usize);
+
+        // Ok
+        assert_eq!(VirtualAddress::diff(v0, v1), 510);
+        assert_eq!(VirtualAddress::diff(v1, v0), -510);
+        assert_eq!(VirtualAddress::diff(v0, v0), 0);
+        assert_eq!(VirtualAddress::diff(v1, v1), 0);
+    }
+
+    #[test]
+    #[should_panic]
+    fn test_diff_overflow() {
+        let v0 = VirtualAddress::<S1>::from(0usize);
+        let v1 = VirtualAddress::<S1>::from(usize::MAX);
+
+        let _v = VirtualAddress::diff(v0, v1);
+    }
+
+    #[test]
+    fn test_step() {
+        let mut count = 0;
+        for _ in VirtualAddress::<S0>::from(0x8000usize)..VirtualAddress::<S0>::from(0x8300usize) {
+            count += 1;
+        }
+        assert_eq!(count, 0x300);
+
+        let mut count = 0;
+        for _ in (VirtualAddress::<S0>::from(0x8000usize)..VirtualAddress::<S0>::from(0x8300usize))
+            .step_by(0x100)
+        {
+            count += 1;
+        }
+        assert_eq!(count, 3);
+
+        let mut count = 0;
+        for _ in
+            (VirtualAddress::<S0>::from(0x8000usize)..VirtualAddress::<S0>::from(0x8300usize)).rev()
+        {
+            count += 1;
+        }
+        assert_eq!(count, 0x300);
+
+        let mut count = 0;
+        for _ in (VirtualAddress::<S0>::from(0x8000usize)..VirtualAddress::<S0>::from(0x8300usize))
+            .rev()
+            .step_by(0x100)
+        {
+            count += 1;
+        }
+        assert_eq!(count, 3);
+    }
+}
--- a/etc/aarch64-orangepi3.ld
+++ b/etc/aarch64-orangepi3.ld
@ -1,35 +1,44 @@
 ENTRY(_entry);

-MEMORY {
-    ram : ORIGIN = 0x48000000, LENGTH = 992M
-}
+KERNEL_OFFSET = 0xFFFFFF8000000000;
+BASE_OFFSET = 0x48000000;

 SECTIONS {
+    . = BASE_OFFSET;
+
+    .text.lower : {
+        *(.text._entry)
+    }
+
+    . = ALIGN(16);
+    . = . + KERNEL_OFFSET;
+
    PROVIDE(__kernel_start = .);

-    .text : {
-        *(.text._entry)
+    .text : AT(. - KERNEL_OFFSET) {
+        *(.text._entry_upper)
        *(.text*)
-    } >ram
+    }

    . = ALIGN(4K);
-    .rodata : {
+    .rodata : AT(. - KERNEL_OFFSET) {
        *(.rodata*)
-    } >ram
+    }

    . = ALIGN(4K);
-    .data : {
+    .data : AT(. - KERNEL_OFFSET) {
        *(.data*)
-    } >ram
+    }

    . = ALIGN(4K);
-    .bss : {
-        PROVIDE(__bss_start = .);
+    PROVIDE(__bss_start_phys = . - KERNEL_OFFSET);
+    PROVIDE(__bss_start = .);
+    .bss : AT(. - KERNEL_OFFSET) {
        *(COMMON)
        *(.bss*)
        . = ALIGN(4K);
-        PROVIDE(__bss_end = .);
-    } >ram
+    }
+    PROVIDE(__bss_end_phys = . - KERNEL_OFFSET);

    PROVIDE(__kernel_end = .);
 }
--- a/etc/aarch64-qemu.json
+++ b/etc/aarch64-qemu.json
@ -9,7 +9,7 @@
  "llvm-target": "aarch64-unknown-none",
  "max-atomic-width": 128,
  "panic-strategy": "abort",
-  "relocation-model": "static",
+  "relocation-model": "pic",
  "target-pointer-width": "64",
  "pre-link-args": {
    "ld.lld": [ "-Tetc/aarch64-qemu.ld" ]
--- a/etc/aarch64-qemu.ld
+++ b/etc/aarch64-qemu.ld
@ -1,14 +1,22 @@
 ENTRY(_entry);

-KERNEL_OFFSET = 0; /* 0xFFFFFF8000000000; */
+KERNEL_OFFSET = 0xFFFFFF8000000000;
+BASE_OFFSET = 0x40080000;

 SECTIONS {
-    . = 0x40080000 + KERNEL_OFFSET;
+    . = BASE_OFFSET;
+
+    .text.lower : {
+        *(.text._entry)
+    }
+
+    . = ALIGN(16);
+    . = . + KERNEL_OFFSET;

    PROVIDE(__kernel_start = .);

    .text : AT(. - KERNEL_OFFSET) {
-        *(.text._entry)
+        *(.text._entry_upper)
        *(.text*)
    }

@ -23,13 +31,14 @@ SECTIONS {
    }

    . = ALIGN(4K);
+    PROVIDE(__bss_start_phys = . - KERNEL_OFFSET);
+    PROVIDE(__bss_start = .);
    .bss : AT(. - KERNEL_OFFSET) {
-        PROVIDE(__bss_start = .);
        *(COMMON)
        *(.bss*)
        . = ALIGN(4K);
-        PROVIDE(__bss_end = .);
    }
+    PROVIDE(__bss_end_phys = . - KERNEL_OFFSET);

    PROVIDE(__kernel_end = .);
 }
--- a/kernel/Cargo.toml
+++ b/kernel/Cargo.toml
@ -13,6 +13,7 @@ test = false
 cfg-if = "1.x.x"
 error = { path = "../error" }
 tock-registers = "0.7.x"
+fdt-rs = { version = "0.x.x", default-features = false }

 [target.'cfg(target_arch = "aarch64")'.dependencies]
 cortex-a = { version = "6.x.x" }
--- a/kernel/src/arch/aarch64/boot/entry.S
+++ b/kernel/src/arch/aarch64/boot/entry.S
@ -1,39 +0,0 @@
-// vi:ft=a64asm.asm:
-
-.macro ADR_REL reg, sym
-    adrp \reg, \sym
-    add \reg, \reg, #:lo12:\sym
-.endm
-
-.section .text._entry
-.global _entry
-_entry:
-    mrs x1, mpidr_el1
-    and x1, x1, #3
-    beq 2f
-1:
-    wfe
-    b 1b
-
-2:
-
-    // Zero .bss
-    ADR_REL x0, __bss_start
-    ADR_REL x1, __bss_end
-1:
-    cmp x0, x1
-    beq 2f
-    stp xzr, xzr, [x0], #16
-    b 1b
-2:
-
-    ADR_REL x0, bsp_stack_top
-    mov sp, x0
-
-    b __aa64_bsp_main
-
-.section .bss
-.p2align 4
-bsp_stack_bottom:
-    .skip 16384
-bsp_stack_top:
--- a/kernel/src/arch/aarch64/boot/macros.S
+++ b/kernel/src/arch/aarch64/boot/macros.S
@ -0,0 +1,16 @@
+.macro MOV_L reg, value
+    mov \reg, #((\value) & 0xFFFF)
+    movk \reg, #((\value) >> 16), lsl #16
+.endm
+
+.macro ADR_REL reg, sym
+    adrp \reg, \sym
+    add \reg, \reg, #:lo12:\sym
+.endm
+
+.macro ADR_ABS reg, sym
+    movz \reg, #:abs_g3:\sym
+    movk \reg, #:abs_g2_nc:\sym
+    movk \reg, #:abs_g1_nc:\sym
+    movk \reg, #:abs_g0_nc:\sym
+.endm
--- a/kernel/src/arch/aarch64/boot/mod.rs
+++ b/kernel/src/arch/aarch64/boot/mod.rs
@ -1,13 +1,14 @@
 //! aarch64 common boot logic

 use crate::arch::{aarch64::asm::CPACR_EL1, machine};
-use crate::dev::Device;
+use crate::dev::{Device, fdt::DeviceTree};
+use crate::mem::virt;
 use cortex_a::asm::barrier::{self, dsb, isb};
 use cortex_a::registers::{DAIF, SCTLR_EL1, VBAR_EL1};
 use tock_registers::interfaces::{ReadWriteable, Writeable};

 #[no_mangle]
-fn __aa64_bsp_main() {
+extern "C" fn __aa64_bsp_main(fdt_base: usize) {
    // Disable FP instruction trapping
    CPACR_EL1.modify(CPACR_EL1::FPEN::TrapNone);

@ -28,20 +29,33 @@ fn __aa64_bsp_main() {
        isb(barrier::SY);
    }

+    // Enable MMU
+    virt::enable().expect("Failed to initialize virtual memory");
+
    machine::init_board().unwrap();
+
+    if fdt_base != 0 {
+        debugln!("fdt_base = {:#x}", fdt_base);
+        let fdt = DeviceTree::from_phys(fdt_base + 0xFFFFFF8000000000);
+        if let Ok(fdt) = fdt {
+            fdt.dump();
+        } else {
+            debugln!("Failed to init FDT");
+        }
+    }
+
+    debugln!("Machine init finished");
+
    unsafe {
        machine::local_timer().enable().unwrap();
    }

    loop {
        DAIF.modify(DAIF::I::CLEAR);
+        cortex_a::asm::wfi();
    }
 }

-cfg_if! {
-    if #[cfg(feature = "mach_orangepi3")] {
-        global_asm!(include_str!("uboot.S"));
-    } else {
-        global_asm!(include_str!("entry.S"));
-    }
-}
+global_asm!(include_str!("macros.S"));
+global_asm!(include_str!("uboot.S"));
+global_asm!(include_str!("upper.S"));
--- a/kernel/src/arch/aarch64/boot/uboot.S
+++ b/kernel/src/arch/aarch64/boot/uboot.S
@ -10,19 +10,11 @@
 .set CNTHCTL_EL2_EL1PCEN,   1 << 1
 .set CNTHCTL_EL2_EL1PCTEN,  1 << 0

-.macro ADR_REL reg, sym
-    adrp \reg, \sym
-    add \reg, \reg, #:lo12:\sym
-.endm
-
-.macro MOV_L reg, value
-    mov \reg, #((\value) & 0xFFFF)
-    movk \reg, #((\value) >> 16), lsl #16
-.endm
-
 .section .text._entry
 .global _entry
 _entry:
+    mov x8, x0
+
    // Test for EL2
    mrs x0, CurrentEL
    lsr x0, x0, #2
@ -45,7 +37,7 @@ _entry:
    orr x0, x0, #SPSR_EL2_MASK_DAIF
    msr spsr_el2, x0

-    ADR_REL x0, 1f
+    adr x0, 1f
    msr elr_el2, x0

    isb
@ -54,8 +46,27 @@ _entry:
    dsb sy
    isb

-    ADR_REL x0, bsp_stack_top
-    mov sp, x0
+    // Zero .bss
+    ADR_ABS x0, __bss_start_phys
+    ADR_ABS x1, __bss_end_phys
+1:
+    cmp x0, x1
+    beq 2f
+
+    str xzr, [x0], #8
+
+    b 1b
+2:
+
+    ADR_ABS x9, __aa64_entry_upper
+    b __aa64_enter_upper
+
+.section .text._entry_upper
+__aa64_entry_upper:
+    // x0 -- fdt address
+
+    ADR_REL x1, bsp_stack_top
+    mov sp, x1

    mov lr, xzr
    bl __aa64_bsp_main
--- a/kernel/src/arch/aarch64/boot/upper.S
+++ b/kernel/src/arch/aarch64/boot/upper.S
@ -0,0 +1,95 @@
+// vi:ft=a64asm:
+
+.set PTE_BLOCK_AF,      1 << 10
+.set PTE_BLOCK_ISH,     3 << 8
+.set PTE_PRESENT,       1 << 0
+
+.set MAIR_EL1_Attr0_Normal_Inner_NC,    (4 << 0)
+.set MAIR_EL1_Attr0_Normal_Outer_NC,    (4 << 4)
+.set MAIR_EL1_Attr1_Device,             (0 << 12)
+.set MAIR_EL1_Attr1_Device_nGnRE,       (1 << 8)
+
+.set ID_AA64MMFR0_EL1_TGran4,           (0xF << 28)
+
+.set TCR_EL1_IPS_SHIFT,                 32
+
+.set TCR_EL1_TG1_4K,                    (2 << 30)
+.set TCR_EL1_SH1_Outer,                 (2 << 28)
+.set TCR_EL1_ORGN1_NC,                  (0 << 26)
+.set TCR_EL1_IRGN1_NC,                  (0 << 24)
+.set TCR_EL1_T1SZ_SHIFT,                16
+
+.set TCR_EL1_TG0_4K,                    (0 << 14)
+.set TCR_EL1_SH0_Outer,                 (2 << 12)
+.set TCR_EL1_ORGN0_NC,                  (0 << 10)
+.set TCR_EL1_IRGN0_NC,                  (0 << 8)
+.set TCR_EL1_T0SZ_SHIFT,                0
+
+.set TCR_EL1_ATTRS,                     (TCR_EL1_TG1_4K | TCR_EL1_SH1_Outer | TCR_EL1_TG0_4K | TCR_EL1_SH0_Outer | (25 << TCR_EL1_T1SZ_SHIFT) | (25 << TCR_EL1_T0SZ_SHIFT))
+
+.set SCTLR_EL1_I,                       (1 << 12)
+.set SCTLR_EL1_C,                       (1 << 2)
+.set SCTLR_EL1_M,                       (1 << 0)
+
+.section .text._entry
+.global __aa64_enter_upper
+.type __aa64_enter_upper, %function
+__aa64_enter_upper:
+    // x8 -- FDT base
+    // x9 -- upper entry point
+
+    // Setup TTBR1_EL1
+    // TODO fix macros
+    ADR_ABS x5, KERNEL_TTBR1
+    ADR_ABS x6, KERNEL_OFFSET
+
+    // x5 = KERNEL_TTBR1 physical address
+    sub x5, x5, x6
+
+    // Fill KERNEL_TTBR1 table with upper-mapped Normal memory
+.fill_ttbr1:
+    mov x2, #256
+1:
+    sub x2, x2, #1
+
+    // x0 = (x2 << 30) | attrs...
+    lsl x1, x2, #30
+    mov x0, #(PTE_BLOCK_ISH | PTE_BLOCK_AF | PTE_PRESENT)
+    orr x0, x0, x1
+
+    str x0, [x5, x2, lsl #3]
+
+    cbnz x2, 1b
+
+.init_mmu_regs:
+    mov x0, #(MAIR_EL1_Attr0_Normal_Outer_NC | MAIR_EL1_Attr0_Normal_Inner_NC | MAIR_EL1_Attr1_Device | MAIR_EL1_Attr1_Device_nGnRE)
+    msr mair_el1, x0
+
+    // Test for 4KiB page support
+    mrs x0, ID_AA64MMFR0_EL1
+    mov x1, ID_AA64MMFR0_EL1_TGran4
+    tst x0, x1
+    bne .no_4k_gran
+
+    // x0 = PARange
+    and x0, x0, #0xF
+    lsl x0, x0, #TCR_EL1_IPS_SHIFT
+    MOV_L x1, TCR_EL1_ATTRS
+    orr x0, x0, x1
+    msr tcr_el1, x0
+
+    msr ttbr0_el1, x5
+    msr ttbr1_el1, x5
+
+    dsb ish
+    isb
+
+    mrs x0, sctlr_el1
+    orr x0, x0, #SCTLR_EL1_M
+    msr sctlr_el1, x0
+
+    mov x0, x8
+    br x9
+.no_4k_gran:
+    b .
+.size __aa64_enter_upper, . - __aa64_enter_upper
--- a/kernel/src/arch/aarch64/irq/gic/gicc.rs
+++ b/kernel/src/arch/aarch64/irq/gic/gicc.rs
@ -1,5 +1,5 @@
-use crate::arch::MemoryIo;
 use crate::dev::irq::IrqContext;
+use crate::mem::virt::DeviceMemoryIo;
 use tock_registers::interfaces::{Readable, Writeable};
 use tock_registers::registers::ReadWrite;
 use tock_registers::{register_bitfields, register_structs};
@ -22,7 +22,7 @@ register_bitfields! {

 register_structs! {
    #[allow(non_snake_case)]
-    GiccRegs {
+    pub(super) GiccRegs {
        (0x00 => CTLR: ReadWrite<u32, CTLR::Register>),
        (0x04 => PMR: ReadWrite<u32, PMR::Register>),
        (0x08 => _res0),
@ -33,13 +33,13 @@ register_structs! {
 }

 pub(super) struct Gicc {
-    regs: MemoryIo<GiccRegs>,
+    regs: DeviceMemoryIo<GiccRegs>,
 }

 impl Gicc {
-    pub const unsafe fn new(base: usize) -> Self {
+    pub const unsafe fn new(regs: DeviceMemoryIo<GiccRegs>) -> Self {
        Self {
-            regs: MemoryIo::new(base),
+            regs,
        }
    }

--- a/kernel/src/arch/aarch64/irq/gic/gicd.rs
+++ b/kernel/src/arch/aarch64/irq/gic/gicd.rs
@ -1,4 +1,4 @@
-use crate::arch::MemoryIo;
+use crate::mem::virt::DeviceMemoryIo;
 use crate::sync::IrqSafeNullLock;
 use tock_registers::interfaces::{Readable, Writeable};
 use tock_registers::registers::{ReadOnly, ReadWrite};
@ -22,7 +22,7 @@ register_bitfields! {

 register_structs! {
    #[allow(non_snake_case)]
-    GicdSharedRegs {
+    pub(super) GicdSharedRegs {
        (0x000 => CTLR: ReadWrite<u32, CTLR::Register>),
        (0x004 => TYPER: ReadWrite<u32, TYPER::Register>),
        (0x008 => _res0),
@ -37,7 +37,7 @@ register_structs! {

 register_structs! {
    #[allow(non_snake_case)]
-    GicdBankedRegs {
+    pub(super) GicdBankedRegs {
        (0x000 => _res0),
        (0x100 => ISENABLER: ReadWrite<u32>),
        (0x104 => _res1),
@ -63,15 +63,18 @@ impl GicdSharedRegs {
 }

 pub(super) struct Gicd {
-    shared_regs: IrqSafeNullLock<MemoryIo<GicdSharedRegs>>,
-    banked_regs: MemoryIo<GicdBankedRegs>,
+    shared_regs: IrqSafeNullLock<DeviceMemoryIo<GicdSharedRegs>>,
+    banked_regs: DeviceMemoryIo<GicdBankedRegs>,
 }

 impl Gicd {
-    pub const unsafe fn new(base: usize) -> Self {
+    pub const unsafe fn new(
+        shared_mmio: DeviceMemoryIo<GicdSharedRegs>,
+        banked_mmio: DeviceMemoryIo<GicdBankedRegs>,
+    ) -> Self {
        Self {
-            shared_regs: IrqSafeNullLock::new(MemoryIo::new(base)),
-            banked_regs: MemoryIo::new(base),
+            shared_regs: IrqSafeNullLock::new(shared_mmio),
+            banked_regs: banked_mmio,
        }
    }

--- a/kernel/src/arch/aarch64/irq/gic/mod.rs
+++ b/kernel/src/arch/aarch64/irq/gic/mod.rs
@ -4,7 +4,9 @@ use crate::dev::{
    irq::{IntController, IntSource, IrqContext},
    Device,
 };
+use crate::mem::virt::{DeviceMemoryIo, DeviceMemory};
 use crate::sync::IrqSafeNullLock;
+use crate::util::InitOnce;
 use error::Errno;

 mod gicc;
@ -22,8 +24,10 @@ pub struct IrqNumber(usize);

 /// ARM Generic Interrupt Controller, version 2
 pub struct Gic {
-    gicc: Gicc,
-    gicd: Gicd,
+    gicc: InitOnce<Gicc>,
+    gicd: InitOnce<Gicd>,
+    gicd_base: usize,
+    gicc_base: usize,
    table: IrqSafeNullLock<[Option<&'static (dyn IntSource + Sync)>; MAX_IRQ]>,
 }

@ -48,8 +52,20 @@ impl Device for Gic {
    }

    unsafe fn enable(&self) -> Result<(), Errno> {
-        self.gicd.enable();
-        self.gicc.enable();
+        let gicd_mmio = DeviceMemory::map("GICv2 Distributor registers", self.gicd_base, 1)?;
+        let gicd_mmio_shared = DeviceMemoryIo::new(gicd_mmio.clone());
+        let gicd_mmio_banked = DeviceMemoryIo::new(gicd_mmio);
+        let gicc_mmio = DeviceMemoryIo::map("GICv2 CPU registers", self.gicc_base, 1)?;
+
+        let gicd = Gicd::new(gicd_mmio_shared, gicd_mmio_banked);
+        let gicc = Gicc::new(gicc_mmio);
+
+        gicd.enable();
+        gicc.enable();
+
+        self.gicd.init(gicd);
+        self.gicc.init(gicc);
+
        Ok(())
    }
 }
@ -58,12 +74,13 @@ impl IntController for Gic {
    type IrqNumber = IrqNumber;

    fn enable_irq(&self, irq: Self::IrqNumber) -> Result<(), Errno> {
-        self.gicd.enable_irq(irq);
+        self.gicd.get().enable_irq(irq);
        Ok(())
    }

    fn handle_pending_irqs<'irq_context>(&'irq_context self, ic: &IrqContext<'irq_context>) {
-        let irq_number = self.gicc.pending_irq_number(ic);
+        let gicc = self.gicc.get();
+        let irq_number = gicc.pending_irq_number(ic);
        if irq_number >= MAX_IRQ {
            return;
        }
@ -76,7 +93,7 @@ impl IntController for Gic {
            }
        }

-        self.gicc.clear_irq(irq_number as u32, ic);
+        gicc.clear_irq(irq_number as u32, ic);
    }

    fn register_handler(
@ -105,8 +122,10 @@ impl Gic {
    /// Does not perform `gicd_base` and `gicc_base` validation.
    pub const unsafe fn new(gicd_base: usize, gicc_base: usize) -> Self {
        Self {
-            gicc: Gicc::new(gicc_base),
-            gicd: Gicd::new(gicd_base),
+            gicc: InitOnce::new(),
+            gicd: InitOnce::new(),
+            gicd_base,
+            gicc_base,
            table: IrqSafeNullLock::new([None; MAX_IRQ]),
        }
    }
--- a/kernel/src/arch/aarch64/mach_orangepi3/gpio.rs
+++ b/kernel/src/arch/aarch64/mach_orangepi3/gpio.rs
@ -5,12 +5,13 @@
 //! 1. CPUS-PORT (TODO PL, PM)
 //! 2. CPUX-PORT (PC, PD, PF, PG, PH)
 //!
-use crate::arch::MemoryIo;
 use crate::dev::{
    gpio::{GpioDevice, PinConfig, PinMode, PullMode},
    Device,
 };
+use crate::mem::virt::DeviceMemoryIo;
 use crate::sync::IrqSafeNullLock;
+use crate::util::InitOnce;
 use error::Errno;
 use tock_registers::interfaces::{Readable, Writeable};
 use tock_registers::register_structs;
@ -28,11 +29,12 @@ register_structs! {
 }

 struct CpuxGpio {
-    regs: MemoryIo<[CpuxPortRegs; 8]>,
+    regs: DeviceMemoryIo<[CpuxPortRegs; 8]>,
 }

 pub struct Gpio {
-    cpux: IrqSafeNullLock<CpuxGpio>,
+    cpux: InitOnce<IrqSafeNullLock<CpuxGpio>>,
+    cpux_base: usize,
 }

 /// Structure combining bank and pin numbers
@ -142,6 +144,9 @@ impl Device for Gpio {
    }

    unsafe fn enable(&self) -> Result<(), Errno> {
+        self.cpux.init(IrqSafeNullLock::new(CpuxGpio {
+            regs: DeviceMemoryIo::map(self.name(), self.cpux_base, 1)?,
+        }));
        Ok(())
    }
 }
@ -155,7 +160,7 @@ impl GpioDevice for Gpio {

        match bank {
            0 | 1 | 4 => unimplemented!(),
-            _ => self.cpux.lock().set_pin_config(bank, pin, cfg),
+            _ => self.cpux.get().lock().set_pin_config(bank, pin, cfg),
        }
    }

@ -169,7 +174,7 @@ impl GpioDevice for Gpio {

        match bank {
            0 | 1 | 4 => unimplemented!(),
-            _ => self.cpux.lock().write_pin(bank, pin, state),
+            _ => self.cpux.get().lock().write_pin(bank, pin, state),
        }
    }

@ -179,7 +184,7 @@ impl GpioDevice for Gpio {

        match bank {
            0 | 1 | 4 => unimplemented!(),
-            _ => self.cpux.lock().toggle_pin(bank, pin),
+            _ => self.cpux.get().lock().toggle_pin(bank, pin),
        }
    }

@ -189,7 +194,7 @@ impl GpioDevice for Gpio {

        match bank {
            0 | 1 | 4 => unimplemented!(),
-            _ => Ok(self.cpux.lock().read_pin(bank, pin)),
+            _ => Ok(self.cpux.get().lock().read_pin(bank, pin)),
        }
    }
 }
@ -200,11 +205,10 @@ impl Gpio {
        self.set_pin_config(PinAddress::new(7, 1), &PinConfig::alt(2))
    }

-    pub const unsafe fn new(base: usize) -> Self {
+    pub const unsafe fn new(cpux_base: usize) -> Self {
        Self {
-            cpux: IrqSafeNullLock::new(CpuxGpio {
-                regs: MemoryIo::new(base),
-            }),
+            cpux: InitOnce::new(),
+            cpux_base,
        }
    }
 }
--- a/kernel/src/arch/aarch64/mach_orangepi3/mod.rs
+++ b/kernel/src/arch/aarch64/mach_orangepi3/mod.rs
@ -28,14 +28,17 @@ use wdog::RWdog;
 #[allow(missing_docs)]
 pub fn init_board() -> Result<(), Errno> {
    unsafe {
+        UART0.enable()?;
        GIC.enable()?;
+        GPIO.enable()?;
+
+        UART0.init_irqs()?;
+
+        R_WDOG.enable()?;

        GPIO.cfg_uart0_ph0_ph1()?;
        GPIO.set_pin_config(PinAddress::new(3, 26), &PinConfig::out_pull_down())?;

-        UART0.enable()?;
-        UART0.init_irqs()?;
-
        RTC.enable()?;
        RTC.init_irqs()?;
    }
--- a/kernel/src/arch/aarch64/mach_orangepi3/rtc.rs
+++ b/kernel/src/arch/aarch64/mach_orangepi3/rtc.rs
@ -1,13 +1,12 @@
-use crate::arch::{
-    machine::{self, IrqNumber},
-    MemoryIo,
-};
+use crate::arch::machine::{self, IrqNumber};
 use crate::dev::{
    irq::{IntController, IntSource},
    rtc::RtcDevice,
    Device,
 };
+use crate::mem::virt::DeviceMemoryIo;
 use crate::sync::IrqSafeNullLock;
+use crate::util::InitOnce;
 use error::Errno;
 use tock_registers::{
    interfaces::{Readable, Writeable},
@ -48,7 +47,8 @@ register_structs! {
 }

 pub struct Rtc {
-    regs: IrqSafeNullLock<MemoryIo<Regs>>,
+    regs: InitOnce<IrqSafeNullLock<DeviceMemoryIo<Regs>>>,
+    base: usize,
    irq: IrqNumber,
 }

@ -70,13 +70,14 @@ impl RtcDevice for Rtc {}

 impl IntSource for Rtc {
    fn handle_irq(&self) -> Result<(), Errno> {
-        self.regs.lock().arm_alarm0_irq(1);
+        self.regs.get().lock().arm_alarm0_irq(1);
+        debugln!("Tick!");
        Ok(())
    }

    fn init_irqs(&'static self) -> Result<(), Errno> {
        machine::intc().register_handler(self.irq, self)?;
-        self.regs.lock().arm_alarm0_irq(1);
+        self.regs.get().lock().arm_alarm0_irq(1);
        machine::intc().enable_irq(self.irq)?;

        Ok(())
@ -89,6 +90,7 @@ impl Device for Rtc {
    }

    unsafe fn enable(&self) -> Result<(), Errno> {
+        self.regs.init(IrqSafeNullLock::new(DeviceMemoryIo::map(self.name(), self.base, 1)?));
        Ok(())
    }
 }
@ -101,7 +103,8 @@ impl Rtc {
    /// Does not perform `base` validation.
    pub const unsafe fn new(base: usize, irq: IrqNumber) -> Self {
        Self {
-            regs: IrqSafeNullLock::new(MemoryIo::new(base)),
+            regs: InitOnce::new(),
+            base,
            irq,
        }
    }
--- a/kernel/src/arch/aarch64/mach_orangepi3/uart.rs
+++ b/kernel/src/arch/aarch64/mach_orangepi3/uart.rs
@ -1,8 +1,9 @@
 use crate::arch::{
    machine::{self, IrqNumber},
-    MemoryIo,
 };
 use crate::sync::IrqSafeNullLock;
+use crate::util::InitOnce;
+use crate::mem::virt::DeviceMemoryIo;
 use crate::dev::{
    irq::{IntController, IntSource},
    serial::SerialDevice,
@ -73,8 +74,13 @@ register_structs! {
    }
 }

+struct UartInner {
+    regs: DeviceMemoryIo<Regs>
+}
+
 pub(super) struct Uart {
-    regs: IrqSafeNullLock<MemoryIo<Regs>>,
+    inner: InitOnce<IrqSafeNullLock<UartInner>>,
+    base: usize,
    irq: IrqNumber
 }

@ -84,33 +90,41 @@ impl Device for Uart {
    }

    unsafe fn enable(&self) -> Result<(), Errno> {
+        let mut inner = UartInner {
+            regs: DeviceMemoryIo::map(self.name(), self.base, 1)?
+        };
        // TODO
+        self.inner.init(IrqSafeNullLock::new(inner));
        Ok(())
    }
 }

 impl SerialDevice for Uart {
    fn send(&self, byte: u8) -> Result<(), Errno> {
-        let regs = self.regs.lock();
-        while !regs.LSR.matches_all(LSR::THRE::SET) {
+        if !self.inner.is_initialized() {
+            return Ok(());
+        }
+
+        let inner = self.inner.get().lock();
+        while !inner.regs.LSR.matches_all(LSR::THRE::SET) {
            cortex_a::asm::nop();
        }
-        regs.DR_DLL.set(byte as u32);
+        inner.regs.DR_DLL.set(byte as u32);
        Ok(())
    }

    fn recv(&self, _blocking: bool) -> Result<u8, Errno> {
-        let regs = self.regs.lock();
-        while !regs.LSR.matches_all(LSR::DR::SET) {
+        let inner = self.inner.get().lock();
+        while !inner.regs.LSR.matches_all(LSR::DR::SET) {
            cortex_a::asm::nop();
        }
-        Ok(regs.DR_DLL.get() as u8)
+        Ok(inner.regs.DR_DLL.get() as u8)
    }
 }

 impl IntSource for Uart {
    fn handle_irq(&self) -> Result<(), Errno> {
-        let byte = self.regs.lock().DR_DLL.get();
+        let byte = self.inner.get().lock().regs.DR_DLL.get();
        debugln!("irq byte = {:#04x}!", byte);

        if byte == 0x1B {
@ -127,7 +141,7 @@ impl IntSource for Uart {

    fn init_irqs(&'static self) -> Result<(), Errno> {
        machine::intc().register_handler(self.irq, self)?;
-        self.regs.lock().IER_DLH.modify(IER::ERBFI::SET);
+        self.inner.get().lock().regs.IER_DLH.modify(IER::ERBFI::SET);
        machine::intc().enable_irq(self.irq)?;

        Ok(())
@ -137,7 +151,8 @@ impl IntSource for Uart {
 impl Uart {
    pub const unsafe fn new(base: usize, irq: IrqNumber) -> Self {
        Self {
-            regs: IrqSafeNullLock::new(MemoryIo::new(base)),
+            inner: InitOnce::new(),
+            base,
            irq
        }
    }
--- a/kernel/src/arch/aarch64/mach_orangepi3/wdog.rs
+++ b/kernel/src/arch/aarch64/mach_orangepi3/wdog.rs
@ -1,5 +1,8 @@
-use crate::arch::MemoryIo;
+use crate::dev::Device;
+use crate::mem::virt::DeviceMemoryIo;
 use crate::sync::IrqSafeNullLock;
+use crate::util::InitOnce;
+use error::Errno;
 use tock_registers::{
    interfaces::Writeable, register_bitfields, register_structs, registers::ReadWrite,
 };
@ -36,7 +39,23 @@ register_structs! {
 }

 pub(super) struct RWdog {
-    regs: IrqSafeNullLock<MemoryIo<RWdogRegs>>,
+    inner: InitOnce<IrqSafeNullLock<DeviceMemoryIo<RWdogRegs>>>,
+    base: usize,
+}
+
+impl Device for RWdog {
+    fn name(&self) -> &'static str {
+        "Allwinner H6 R_WDOG"
+    }
+
+    unsafe fn enable(&self) -> Result<(), Errno> {
+        self.inner.init(IrqSafeNullLock::new(DeviceMemoryIo::map(
+            self.name(),
+            self.base,
+            1,
+        )?));
+        Ok(())
+    }
 }

 impl RWdog {
@ -46,7 +65,7 @@ impl RWdog {
    ///
    /// Unsafe: may interrupt critical processes
    pub unsafe fn reset_board(&self) -> ! {
-        let regs = self.regs.lock();
+        let regs = self.inner.get().lock();

        regs.CFG.write(CFG::CONFIG::System);
        regs.MODE.write(MODE::EN::SET);
@ -64,7 +83,8 @@ impl RWdog {
    /// Does not perform `base` validation.
    pub const unsafe fn new(base: usize) -> Self {
        Self {
-            regs: IrqSafeNullLock::new(MemoryIo::new(base)),
+            inner: InitOnce::new(),
+            base,
        }
    }
 }
--- a/kernel/src/arch/aarch64/mach_qemu/mod.rs
+++ b/kernel/src/arch/aarch64/mach_qemu/mod.rs
@ -7,7 +7,7 @@ use crate::arch::aarch64::{
 use crate::dev::timer::TimestampSource;
 use crate::dev::{
    irq::{IntController, IntSource},
-    pci::{PciHostDevice, pcie::gpex::GenericPcieHost},
+    pci::{pcie::gpex::GenericPcieHost, PciHostDevice},
    rtc::pl031::Pl031,
    serial::{pl011::Pl011, SerialDevice},
    Device,
@ -26,9 +26,10 @@ const ECAM_BASE: usize = 0x4010000000;
 #[allow(missing_docs)]
 pub fn init_board() -> Result<(), Errno> {
    unsafe {
+        UART0.enable()?;
+
        GIC.enable()?;

-        UART0.enable()?;
        UART0.init_irqs()?;

        RTC.enable()?;
--- a/kernel/src/arch/mod.rs
+++ b/kernel/src/arch/mod.rs
@ -19,34 +19,34 @@ cfg_if! {
 }

 // TODO move to mod io
-use core::marker::PhantomData;
-use core::ops::Deref;
-
-/// Wrapper for setting up memory-mapped registers and IO
-pub struct MemoryIo<T> {
-    base: usize,
-    _pd: PhantomData<fn() -> T>,
-}
-
-impl<T> MemoryIo<T> {
-    /// Constructs a new instance of MMIO region.
-    ///
-    /// # Safety
-    ///
-    /// Does not perform `base` validation.
-    pub const unsafe fn new(base: usize) -> Self {
-        Self {
-            base,
-            _pd: PhantomData,
-        }
-    }
-}
-
-impl<T> Deref for MemoryIo<T> {
-    type Target = T;
-
-    #[inline(always)]
-    fn deref(&self) -> &Self::Target {
-        unsafe { &*(self.base as *const _) }
-    }
-}
+// use core::marker::PhantomData;
+// use core::ops::Deref;
+//
+// /// Wrapper for setting up memory-mapped registers and IO
+// pub struct MemoryIo<T> {
+//     base: usize,
+//     _pd: PhantomData<fn() -> T>,
+// }
+//
+// impl<T> MemoryIo<T> {
+//     /// Constructs a new instance of MMIO region.
+//     ///
+//     /// # Safety
+//     ///
+//     /// Does not perform `base` validation.
+//     pub const unsafe fn new(base: usize) -> Self {
+//         Self {
+//             base,
+//             _pd: PhantomData,
+//         }
+//     }
+// }
+//
+// impl<T> Deref for MemoryIo<T> {
+//     type Target = T;
+//
+//     #[inline(always)]
+//     fn deref(&self) -> &Self::Target {
+//         unsafe { &*(self.base as *const _) }
+//     }
+// }
--- a/kernel/src/dev/fdt.rs
+++ b/kernel/src/dev/fdt.rs
@ -0,0 +1,93 @@
+use error::Errno;
+use fdt_rs::prelude::*;
+use fdt_rs::{
+    base::DevTree,
+    index::{DevTreeIndex, DevTreeIndexNode},
+};
+
+#[repr(align(16))]
+struct Wrap {
+    data: [u8; 65536],
+}
+
+static mut INDEX_BUFFER: Wrap = Wrap { data: [0; 65536] };
+
+type INode<'a> = DevTreeIndexNode<'a, 'a, 'a>;
+
+#[allow(dead_code)]
+pub struct DeviceTree {
+    tree: DevTree<'static>,
+    index: DevTreeIndex<'static, 'static>,
+}
+
+fn tab(depth: usize) {
+    for _ in 0..depth {
+        debug!("\t");
+    }
+}
+
+fn dump_node(node: &INode, depth: usize) {
+    if node.name().unwrap().starts_with("virtio_mmio@") {
+        return;
+    }
+
+    tab(depth);
+    debugln!("{:?} {{", node.name().unwrap());
+
+    for prop in node.props() {
+        tab(depth + 1);
+        let name = prop.name().unwrap();
+        debug!("{:?} = ", name);
+
+        match name {
+            "compatible" => debug!("{:?}", prop.str().unwrap()),
+            "#address-cells" | "#size-cells" => debug!("{}", prop.u32(0).unwrap()),
+            "reg" => {
+                debug!("<");
+                let len = prop.length() / 4;
+                for i in 0..len {
+                    debug!("{:#010x}", prop.u32(i).unwrap());
+                    if i < len - 1 {
+                        debug!(", ");
+                    }
+                }
+                debug!(">");
+            }
+            _ => debug!("..."),
+        }
+        debugln!(";");
+    }
+
+    if node.children().next().is_some() {
+        debugln!("");
+    }
+
+    for child in node.children() {
+        dump_node(&child, depth + 1);
+    }
+
+    tab(depth);
+    debugln!("}}");
+}
+
+impl DeviceTree {
+    pub fn dump(&self) {
+        dump_node(&self.index.root(), 0);
+    }
+
+    pub fn from_phys(base: usize) -> Result<DeviceTree, Errno> {
+        // TODO virtualize address
+        let tree = unsafe { DevTree::from_raw_pointer(base as *const _) }
+            .map_err(|_| Errno::InvalidArgument)?;
+        let layout = DevTreeIndex::get_layout(&tree).unwrap();
+        if layout.size() + layout.align() >= unsafe { INDEX_BUFFER.data.len() } {
+            return Err(Errno::OutOfMemory);
+        }
+        let index = DevTreeIndex::new(tree, unsafe {
+            &mut INDEX_BUFFER.data[0..layout.size() + layout.align()]
+        })
+        .unwrap();
+
+        Ok(DeviceTree { tree, index })
+    }
+}
--- a/kernel/src/dev/mod.rs
+++ b/kernel/src/dev/mod.rs
@ -3,6 +3,8 @@
 use error::Errno;

 // Device classes
+#[allow(missing_docs)]
+pub mod fdt;
 pub mod gpio;
 pub mod irq;
 pub mod pci;
--- a/kernel/src/dev/pci/mod.rs
+++ b/kernel/src/dev/pci/mod.rs
@ -1,4 +1,4 @@
-#![allow(missing_docs)]
+//! PCI bus host and device interfaces

 use crate::dev::Device;
 use core::fmt;
@ -8,18 +8,21 @@ pub mod pcie;

 macro_rules! ecam_field {
    ($getter:ident, $off:expr, u16) => {
+        #[allow(missing_docs)]
        #[inline(always)]
        fn $getter(&self) -> u16 {
            self.readw($off)
        }
    };
    ($getter:ident, $off:expr, u8) => {
+        #[allow(missing_docs)]
        #[inline(always)]
        fn $getter(&self) -> u8 {
            self.readb($off)
        }
    };
    ($getter:ident, $setter:ident, $off:expr, u16) => {
+        #[allow(missing_docs)]
        #[inline(always)]
        unsafe fn $setter(&self, v: u16) {
            self.writew($off, v)
@ -29,22 +32,37 @@ macro_rules! ecam_field {
    };
 }

+/// PCI endpoint address struct, combining bus:dev:func parts
 #[derive(Clone, Copy)]
 #[repr(transparent)]
 pub struct PciAddress {
    value: u32,
 }

+/// Generic PCI device configuration space interface
 pub trait PciCfgSpace {
+    // TODO change readl to readl_unchecked() and perform checks at trait level
+    /// Reads an [u32] from device config space.
+    /// `off` must be aligned at a 4-byte boundary.
    fn readl(&self, off: usize) -> u32;
+
+    /// Writes an [u32] to device config space.
+    /// `off` must be aligned at a 4-byte boundary.
+    ///
+    /// # Safety
+    ///
+    /// Unsafe: allows arbitrary value writes to PCI config space.
    unsafe fn writel(&self, off: usize, val: u32);

+    /// Reads an [u16] from device config space.
+    /// `off` must be aligned at a 2-byte boundary.
    #[inline(always)]
    fn readw(&self, off: usize) -> u16 {
        assert!(off & 0x1 == 0);
        (self.readl(off & !0x3) >> ((off & 0x3) * 8)) as u16
    }

+    /// Reads an [u8] from device config space
    #[inline(always)]
    fn readb(&self, off: usize) -> u8 {
        (self.readl(off & !0x3) >> ((off & 0x3) * 8)) as u8
@ -54,17 +72,22 @@ pub trait PciCfgSpace {
    ecam_field! { device_id, 0x02, u16 }
    ecam_field! { header_type, 0x0E, u8 }

+    /// Returns `true` if device this config describes is
+    /// present on the bus
    #[inline(always)]
    fn is_valid(&self) -> bool {
        self.readl(0) != 0xFFFFFFFF
    }
 }

+/// PCI host controller interface
 pub trait PciHostDevice: Device {
+    /// Initializes and enables devices attached to the bus
    fn map(&self) -> Result<(), Errno>;
 }

 impl PciAddress {
+    /// Constructs a [PCIAddress] instance from its components
    #[inline(always)]
    pub const fn new(bus: u8, dev: u8, func: u8) -> Self {
        Self {
@ -72,21 +95,26 @@ impl PciAddress {
        }
    }

+    /// Returns `bus` field of [PCIAddress]
    #[inline(always)]
    pub const fn bus(self) -> u8 {
        (self.value >> 8) as u8
    }

+    /// Returns `dev` field of [PCIAddress]
    #[inline(always)]
    pub const fn dev(self) -> u8 {
        ((self.value >> 3) as u8) & 0x1F
    }

+    /// Returns `func` field of [PCIAddress]
    #[inline(always)]
    pub const fn func(self) -> u8 {
        (self.value as u8) & 0x7
    }

+    /// Returns a new [PCIAddress], constructed from `self`, but with
+    /// specified `func` number
    #[inline(always)]
    pub const fn with_func(self, func: u8) -> Self {
        Self::new(self.bus(), self.dev(), func)
--- a/kernel/src/dev/pci/pcie/gpex.rs
+++ b/kernel/src/dev/pci/pcie/gpex.rs
@ -1,11 +1,17 @@
+//! Generic PCIe host driver
+
 use crate::dev::{
-    pci::{pcie::EcamCfgSpace, PciCfgSpace, PciAddress, PciHostDevice},
+    pci::{pcie::EcamCfgSpace, PciAddress, PciCfgSpace, PciHostDevice},
    Device,
 };
+use crate::mem::virt::DeviceMemory;
+use crate::util::InitOnce;
 use error::Errno;

+/// GPEX host controller struct
 pub struct GenericPcieHost {
    ecam_base: usize,
+    ecam: InitOnce<DeviceMemory>,
    // TODO
    #[allow(dead_code)]
    bus_count: u8,
@ -17,13 +23,15 @@ impl Device for GenericPcieHost {
    }

    unsafe fn enable(&self) -> Result<(), Errno> {
+        self.ecam
+            .init(DeviceMemory::map(self.name(), self.ecam_base, 512 * 512)?);
        Ok(())
    }
 }

 impl PciHostDevice for GenericPcieHost {
    fn map(&self) -> Result<(), Errno> {
-        let bus0 = unsafe { EcamCfgSpace::new(self.ecam_base, PciAddress::new(0, 0, 0)) };
+        let bus0 = self.get_ecam(PciAddress::new(0, 0, 0));

        if bus0.header_type() & 0x80 == 0 {
            self.map_bus(0)?;
@ -36,22 +44,37 @@ impl PciHostDevice for GenericPcieHost {
 }

 impl GenericPcieHost {
+    fn get_ecam(&self, addr: PciAddress) -> EcamCfgSpace {
+        assert!(addr.value < 512 * 512);
+        unsafe { EcamCfgSpace::new(self.ecam.get().base(), addr) }
+    }
+
+    fn map_function(&self, addr: PciAddress, cfg: EcamCfgSpace) -> Result<(), Errno> {
+        debugln!(
+            "{:?}: {:04x}:{:04x}",
+            addr,
+            cfg.vendor_id(),
+            cfg.device_id()
+        );
+        Ok(())
+    }
+
    fn map_device(&self, addr: PciAddress) -> Result<(), Errno> {
-        let fn0 = unsafe { EcamCfgSpace::new(self.ecam_base, addr) };
+        let fn0 = self.get_ecam(addr);
        if !fn0.is_valid() {
            return Ok(());
        }
        let ty = fn0.header_type();

-        //self.map_function(addr, fn0)?;
+        self.map_function(addr, fn0)?;

        // Check if device is a multi-function one
        if ty & 0x80 != 0 {
            for func in 1..8 {
                let addr = addr.with_func(func);
-                let f = unsafe { EcamCfgSpace::new(self.ecam_base, addr) };
+                let f = self.get_ecam(addr);
                if f.is_valid() {
-                    //self.map_function(addr, f)?;
+                    self.map_function(addr, f)?;
                }
            }
        }
@ -67,8 +90,14 @@ impl GenericPcieHost {
        Ok(())
    }

+    /// Constructs an instance of GPEX device.
+    ///
+    /// # Safety
+    ///
+    /// Does not perform `ecam_base` validation.
    pub const unsafe fn new(ecam_base: usize, bus_count: u8) -> Self {
        Self {
+            ecam: InitOnce::new(),
            ecam_base,
            bus_count,
        }
--- a/kernel/src/dev/pci/pcie/mod.rs
+++ b/kernel/src/dev/pci/pcie/mod.rs
@ -1,7 +1,10 @@
+//! PCI Express access interfaces and drivers
+
 use crate::dev::pci::{PciAddress, PciCfgSpace};

 pub mod gpex;

+/// Enhanced configuration space from PCI Express
 #[derive(Clone, Copy)]
 #[repr(transparent)]
 pub struct EcamCfgSpace {
@ -9,9 +12,14 @@ pub struct EcamCfgSpace {
 }

 impl EcamCfgSpace {
+    /// Constructs an instance of ECAM struct describing PCI endpoint `addr`.
+    ///
+    /// # Safety
+    ///
+    /// `ecam_base` is not validated.
    pub const unsafe fn new(ecam_base: usize, addr: PciAddress) -> Self {
        Self {
-            base: ecam_base + (addr.value as usize) * 4096
+            base: ecam_base + (addr.value as usize) * 4096,
        }
    }
 }
@ -20,9 +28,7 @@ impl PciCfgSpace for EcamCfgSpace {
    #[inline(always)]
    fn readl(&self, off: usize) -> u32 {
        assert!(off & 0x3 == 0);
-        unsafe {
-            core::ptr::read_volatile((self.base + off) as *const u32)
-        }
+        unsafe { core::ptr::read_volatile((self.base + off) as *const u32) }
    }

    #[inline(always)]
--- a/kernel/src/dev/rtc/pl031.rs
+++ b/kernel/src/dev/rtc/pl031.rs
@ -1,10 +1,16 @@
 //! PL031 - ARM PrimeCell real-time clock implementation
-use crate::dev::{Device, rtc::RtcDevice, irq::{IntController, IntSource}};
-use crate::arch::{MemoryIo, machine::{self, IrqNumber}};
+use crate::arch::machine::{self, IrqNumber};
+use crate::dev::{
+    irq::{IntController, IntSource},
+    rtc::RtcDevice,
+    Device,
+};
+use crate::mem::virt::DeviceMemoryIo;
 use crate::sync::IrqSafeNullLock;
+use crate::util::InitOnce;
 use error::Errno;
 use tock_registers::{
-    interfaces::{Readable, Writeable, ReadWriteable},
+    interfaces::{ReadWriteable, Readable, Writeable},
    register_bitfields, register_structs,
    registers::{ReadOnly, ReadWrite, WriteOnly},
 };
@ -37,27 +43,31 @@ register_structs! {
    }
 }

-/// Device struct for PL031
-pub struct Pl031 {
-    regs: IrqSafeNullLock<MemoryIo<Regs>>,
-    irq: IrqNumber
+struct Pl031Inner {
+    regs: DeviceMemoryIo<Regs>,
 }

-impl RtcDevice for Pl031 {
+/// Device struct for PL031
+pub struct Pl031 {
+    inner: InitOnce<IrqSafeNullLock<Pl031Inner>>,
+    base: usize,
+    irq: IrqNumber,
 }

+impl RtcDevice for Pl031 {}
+
 impl IntSource for Pl031 {
    fn handle_irq(&self) -> Result<(), Errno> {
-        let regs = self.regs.lock();
-        regs.ICR.write(ICR::RTCICR::SET);
-        let data = regs.DR.get();
-        regs.MR.set(data + 1);
+        let inner = self.inner.get().lock();
+        inner.regs.ICR.write(ICR::RTCICR::SET);
+        let data = inner.regs.DR.get();
+        inner.regs.MR.set(data + 1);
        Ok(())
    }

    fn init_irqs(&'static self) -> Result<(), Errno> {
        machine::intc().register_handler(self.irq, self)?;
-        self.regs.lock().IMSC.modify(IMSC::RTCIMSC::SET);
+        self.inner.get().lock().regs.IMSC.modify(IMSC::RTCIMSC::SET);
        machine::intc().enable_irq(self.irq)?;

        Ok(())
@ -70,10 +80,16 @@ impl Device for Pl031 {
    }

    unsafe fn enable(&self) -> Result<(), Errno> {
-        let regs = self.regs.lock();
-        regs.CR.modify(CR::RTCStart::CLEAR);
-        regs.MR.set(regs.DR.get() + 1);
-        regs.CR.modify(CR::RTCStart::SET);
+        let inner = Pl031Inner {
+            regs: DeviceMemoryIo::map(self.name(), self.base, 1)?,
+        };
+
+        inner.regs.CR.modify(CR::RTCStart::CLEAR);
+        inner.regs.MR.set(inner.regs.DR.get() + 1);
+        inner.regs.CR.modify(CR::RTCStart::SET);
+
+        self.inner.init(IrqSafeNullLock::new(inner));
+
        Ok(())
    }
 }
@ -86,8 +102,9 @@ impl Pl031 {
    /// Does not perform `base` validation.
    pub const unsafe fn new(base: usize, irq: IrqNumber) -> Self {
        Self {
-            regs: IrqSafeNullLock::new(MemoryIo::new(base)),
-            irq
+            inner: InitOnce::new(),
+            base,
+            irq,
        }
    }
 }
--- a/kernel/src/dev/serial/pl011.rs
+++ b/kernel/src/dev/serial/pl011.rs
@ -1,11 +1,18 @@
 //! PL011 - ARM PrimeCell UART implementation

-use crate::arch::{MemoryIo, machine::{self, IrqNumber}};
-use crate::dev::{serial::SerialDevice, irq::{IntController, IntSource}, Device};
+use crate::arch::machine::{self, IrqNumber};
+use crate::dev::{
+    irq::{IntController, IntSource},
+    serial::SerialDevice,
+    Device,
+};
+use crate::mem::virt::DeviceMemoryIo;
 use crate::sync::IrqSafeNullLock;
+use crate::util::InitOnce;
+use core::fmt;
 use error::Errno;
 use tock_registers::{
-    interfaces::{Readable, Writeable, ReadWriteable},
+    interfaces::{ReadWriteable, Readable, Writeable},
    register_bitfields, register_structs,
    registers::{ReadOnly, ReadWrite, WriteOnly},
 };
@ -64,27 +71,77 @@ register_structs! {
    }
 }

+struct Pl011Inner {
+    regs: DeviceMemoryIo<Regs>,
+}
+
 /// Device struct for PL011
 pub struct Pl011 {
-    regs: IrqSafeNullLock<MemoryIo<Regs>>,
+    inner: InitOnce<IrqSafeNullLock<Pl011Inner>>,
+    base: usize,
    irq: IrqNumber,
 }

+impl Pl011Inner {
+    ///
+    #[inline(always)]
+    pub unsafe fn send(&mut self, byte: u8) {
+        while self.regs.FR.matches_all(FR::TXFF::SET) {
+            core::hint::spin_loop();
+        }
+        self.regs.DR.set(byte as u32);
+    }
+
+    ///
+    pub unsafe fn recv(&mut self, blocking: bool) -> Result<u8, Errno> {
+        if self.regs.FR.matches_all(FR::RXFE::SET) {
+            if !blocking {
+                return Err(Errno::WouldBlock);
+            }
+            while self.regs.FR.matches_all(FR::RXFE::SET) {
+                // TODO allow IRQs here?
+                core::hint::spin_loop();
+            }
+        }
+
+        Ok(self.regs.DR.get() as u8)
+    }
+
+    ///
+    pub unsafe fn enable(&mut self) {
+        self.regs.CR.set(0);
+        self.regs.ICR.write(ICR::ALL::CLEAR);
+        self.regs
+            .CR
+            .write(CR::UARTEN::SET + CR::TXE::SET + CR::RXE::SET);
+    }
+}
+
+impl fmt::Write for Pl011Inner {
+    fn write_str(&mut self, s: &str) -> fmt::Result {
+        for &c in s.as_bytes() {
+            unsafe {
+                self.send(c);
+            }
+        }
+        Ok(())
+    }
+}
+
 impl IntSource for Pl011 {
    fn handle_irq(&self) -> Result<(), Errno> {
-        let regs = self.regs.lock();
-        regs.ICR.write(ICR::ALL::CLEAR);
+        let inner = self.inner.get().lock();
+        inner.regs.ICR.write(ICR::ALL::CLEAR);

-        let byte = regs.DR.get();
+        let byte = inner.regs.DR.get();
        debugln!("irq byte = {:#04x}", byte);

-
        Ok(())
    }

    fn init_irqs(&'static self) -> Result<(), Errno> {
        machine::intc().register_handler(self.irq, self)?;
-        self.regs.lock().IMSC.modify(IMSC::RXIM::SET);
+        self.inner.get().lock().regs.IMSC.modify(IMSC::RXIM::SET);
        machine::intc().enable_irq(self.irq)?;

        Ok(())
@ -93,27 +150,18 @@ impl IntSource for Pl011 {

 impl SerialDevice for Pl011 {
    fn send(&self, byte: u8) -> Result<(), Errno> {
-        let regs = self.regs.lock();
-        while regs.FR.matches_all(FR::TXFF::SET) {
-            core::hint::spin_loop();
+        if !self.inner.is_initialized() {
+            // TODO early output here
+            return Ok(());
+        }
+        unsafe {
+            self.inner.get().lock().send(byte);
        }
-        regs.DR.set(byte as u32);
        Ok(())
    }

    fn recv(&self, blocking: bool) -> Result<u8, Errno> {
-        let regs = self.regs.lock();
-        if regs.FR.matches_all(FR::RXFE::SET) {
-            if !blocking {
-                return Err(Errno::WouldBlock);
-            }
-            while regs.FR.matches_all(FR::RXFE::SET) {
-                // TODO allow IRQs here?
-                core::hint::spin_loop();
-            }
-        }
-
-        Ok(regs.DR.get() as u8)
+        unsafe { self.inner.get().lock().recv(blocking) }
    }
 }

@ -123,10 +171,12 @@ impl Device for Pl011 {
    }

    unsafe fn enable(&self) -> Result<(), Errno> {
-        let regs = self.regs.lock();
-        regs.CR.set(0);
-        regs.ICR.write(ICR::ALL::CLEAR);
-        regs.CR.write(CR::UARTEN::SET + CR::TXE::SET + CR::RXE::SET);
+        let mut inner = Pl011Inner {
+            regs: DeviceMemoryIo::map(self.name(), self.base, 1)?,
+        };
+        inner.enable();
+
+        self.inner.init(IrqSafeNullLock::new(inner));

        Ok(())
    }
@ -140,8 +190,9 @@ impl Pl011 {
    /// Does not perform `base` validation.
    pub const unsafe fn new(base: usize, irq: IrqNumber) -> Self {
        Self {
-            regs: IrqSafeNullLock::new(MemoryIo::new(base)),
-            irq
+            inner: InitOnce::new(),
+            base,
+            irq,
        }
    }
 }
--- a/kernel/src/main.rs
+++ b/kernel/src/main.rs
@ -19,18 +19,16 @@ extern crate cfg_if;

 #[macro_use]
 pub mod debug;
+
 pub mod arch;
 pub mod dev;
 pub mod mem;
 pub mod sync;
+pub mod util;

 #[panic_handler]
 fn panic_handler(pi: &core::panic::PanicInfo) -> ! {
-    if let Some(msg) = pi.message() {
-        debugln!("Panic occurred: {:?}", msg);
-    } else {
-        debugln!("Panic occurred");
-    }
-    debugln!("Panic location: {:?}", pi.location());
+    debugln!("Panic: {:?}", pi);
+    // TODO
    loop {}
 }
--- a/kernel/src/mem/mod.rs
+++ b/kernel/src/mem/mod.rs
@ -1,5 +1,10 @@
 //! Memory management and functions module

+pub mod virt;
+
+/// Virtual offset applied to kernel address space
+pub const KERNEL_OFFSET: usize = 0xFFFFFF8000000000;
+
 /// See memcpy(3p).
 ///
 /// # Safety
--- a/kernel/src/mem/virt/mod.rs
+++ b/kernel/src/mem/virt/mod.rs
@ -0,0 +1,169 @@
+#![allow(missing_docs)]
+
+use crate::mem::KERNEL_OFFSET;
+use core::marker::PhantomData;
+use core::ops::Deref;
+use cortex_a::asm::barrier::{self, dsb, isb};
+use cortex_a::registers::{TCR_EL1, TTBR0_EL1};
+use error::Errno;
+use tock_registers::interfaces::{ReadWriteable, Writeable};
+
+const PTE_BLOCK_AF: u64 = 1 << 10;
+const PTE_BLOCK_OSH: u64 = 2 << 8;
+const PTE_TABLE: u64 = 1 << 1;
+const PTE_PRESENT: u64 = 1 << 0;
+const PTE_ATTR1: u64 = 1 << 2;
+
+#[repr(C, align(0x1000))]
+struct Table([u64; 512]);
+
+#[no_mangle]
+static mut KERNEL_TTBR1: Table = Table([0; 512]);
+// 1GiB
+static mut KERNEL_L1: Table = Table([0; 512]);
+// 2MiB
+static mut KERNEL_L2: Table = Table([0; 512]);
+static mut COUNT: usize = 0;
+static mut BIG_COUNT: usize = 1;
+static mut HUGE_COUNT: usize = 1;
+
+const DEVICE_MAP_OFFSET: usize = KERNEL_OFFSET + (256 << 30);
+
+#[derive(Debug)]
+#[allow(dead_code)]
+pub struct DeviceMemory {
+    name: &'static str,
+    base: usize,
+    count: usize,
+}
+
+pub struct DeviceMemoryIo<T> {
+    mmio: DeviceMemory,
+    _0: PhantomData<T>,
+}
+
+impl DeviceMemory {
+    #[inline(always)]
+    pub const fn base(&self) -> usize {
+        self.base
+    }
+
+    pub fn map(name: &'static str, phys: usize, count: usize) -> Result<Self, Errno> {
+        // TODO generalize this
+        let phys_page = phys & !0xFFF;
+
+        let base = unsafe {
+            match count {
+                262144 => {
+                    let count = HUGE_COUNT;
+                    if count == 512 {
+                        return Err(Errno::OutOfMemory);
+                    }
+                    HUGE_COUNT += 1;
+
+                    KERNEL_TTBR1.0[count + 256] =
+                        (phys_page as u64) | PTE_PRESENT | PTE_BLOCK_OSH | PTE_BLOCK_AF | PTE_ATTR1;
+                    asm!("dsb ish; isb");
+
+                    DEVICE_MAP_OFFSET + (count << 30) + (phys & 0xFFF)
+                }
+                512 => {
+                    let count = BIG_COUNT;
+                    if count == 512 {
+                        return Err(Errno::OutOfMemory);
+                    }
+                    BIG_COUNT += 1;
+
+                    KERNEL_L1.0[count] =
+                        (phys_page as u64) | PTE_PRESENT | PTE_BLOCK_OSH | PTE_BLOCK_AF | PTE_ATTR1;
+                    asm!("dsb ish; isb");
+
+                    DEVICE_MAP_OFFSET + (count << 21) + (phys & 0xFFF)
+                }
+                1 => {
+                    let count = COUNT;
+                    if count == 512 {
+                        return Err(Errno::OutOfMemory);
+                    }
+                    COUNT += 1;
+
+                    KERNEL_L2.0[count] = (phys_page as u64)
+                        | PTE_TABLE
+                        | PTE_BLOCK_OSH
+                        | PTE_PRESENT
+                        | PTE_BLOCK_AF
+                        | PTE_ATTR1;
+                    asm!("dsb ish; isb");
+
+                    DEVICE_MAP_OFFSET + (count << 12) + (phys & 0xFFF)
+                }
+                _ => unimplemented!(),
+            }
+        };
+
+        debugln!(
+            "Mapping {:#x}..{:#x} -> {:#x} ({:?})",
+            base,
+            base + count * 0x1000,
+            phys,
+            name
+        );
+
+        Ok(Self { name, base, count })
+    }
+
+    pub unsafe fn clone(&self) -> Self {
+        // TODO maybe add refcount and remove "unsafe"?
+        Self {
+            name: self.name,
+            base: self.base,
+            count: self.count
+        }
+    }
+}
+
+impl<T> DeviceMemoryIo<T> {
+    pub const fn new(mmio: DeviceMemory) -> Self {
+        Self {
+            mmio,
+            _0: PhantomData,
+        }
+    }
+
+    pub unsafe fn map(name: &'static str, phys: usize, count: usize) -> Result<Self, Errno> {
+        DeviceMemory::map(name, phys, count).map(Self::new)
+    }
+}
+
+impl<T> Deref for DeviceMemoryIo<T> {
+    type Target = T;
+
+    #[inline(always)]
+    fn deref(&self) -> &T {
+        unsafe { &*(self.mmio.base as *const T) }
+    }
+}
+
+pub fn enable() -> Result<(), Errno> {
+    unsafe {
+        // TODO function to translate kernel addresses to physical ones
+        let l1_base = (&KERNEL_L1 as *const _ as u64) - KERNEL_OFFSET as u64;
+        let l2_base = (&KERNEL_L2 as *const _ as u64) - KERNEL_OFFSET as u64;
+
+        KERNEL_L1.0[0] = l2_base | PTE_TABLE | PTE_PRESENT;
+        KERNEL_TTBR1.0[256] = l1_base | PTE_TABLE | PTE_PRESENT;
+
+        // NOTE don't think tlb needs to be invalidated when new entries are created
+    }
+
+    unsafe {
+        dsb(barrier::ISH);
+        isb(barrier::SY);
+    }
+
+    // Disable lower-half translation
+    TTBR0_EL1.set(0);
+    TCR_EL1.modify(TCR_EL1::EPD0::SET);
+
+    Ok(())
+}
--- a/kernel/src/util.rs
+++ b/kernel/src/util.rs
@ -0,0 +1,45 @@
+#![allow(missing_docs)]
+
+use core::cell::UnsafeCell;
+use core::mem::MaybeUninit;
+use core::sync::atomic::{AtomicBool, Ordering};
+
+pub struct InitOnce<T> {
+    state: AtomicBool,
+    inner: UnsafeCell<MaybeUninit<T>>,
+}
+
+impl<T> InitOnce<T> {
+    pub const fn new() -> Self {
+        Self {
+            state: AtomicBool::new(false),
+            inner: UnsafeCell::new(MaybeUninit::uninit()),
+        }
+    }
+
+    #[inline(always)]
+    pub fn is_initialized(&self) -> bool {
+        self.state.load(Ordering::Acquire)
+    }
+
+    #[allow(clippy::mut_from_ref)]
+    pub fn get(&self) -> &mut T {
+        assert!(self.is_initialized(), "Access to uninitialized InitOnce<T>");
+        unsafe { (*self.inner.get()).assume_init_mut() }
+    }
+
+    pub fn init(&self, value: T) {
+        assert!(
+            self.state
+                .compare_exchange_weak(false, true, Ordering::Acquire, Ordering::Relaxed)
+                .is_ok(),
+            "Double-initialization of InitOnce<T>"
+        );
+
+        unsafe {
+            (*self.inner.get()).write(value);
+        }
+    }
+}
+
+unsafe impl<T> Sync for InitOnce<T> {}
Author	SHA1	Message	Date
Mark Poliakov	7121e502df	doc: add pci subsystem docs	2021-10-11 19:10:07 +03:00
Mark Poliakov	4a2d646dc0	refactor: rm aarch64 entry.S	2021-10-11 18:54:48 +03:00
Mark Poliakov	f9d7af1744	fix: unify boot code	2021-10-11 18:54:36 +03:00
Mark Poliakov	bc3a2ddf99	tmp: panic message	2021-10-11 18:36:25 +03:00
Mark Poliakov	7ac20b1841	fix: orangepi3 higher-half boot with uImage	2021-10-11 18:36:08 +03:00
Mark Poliakov	2362ce6cd4	feat: meaningful fdt init errors	2021-10-11 18:35:42 +03:00
Mark Poliakov	2d00b675bf	feat: only map GICD regs once	2021-10-11 18:35:06 +03:00
Mark Poliakov	21aa6826c7	fix: lower 12 bits of DeviceMemoryIo<T>	2021-10-11 18:19:05 +03:00
Mark Poliakov	189229ca7a	feat: orangepi3 higher-half poc Reset is broken	2021-10-11 15:09:26 +03:00
Mark Poliakov	a8b7e88cfe	feat: PoC higher-half kernel	2021-10-10 21:24:22 +03:00
Mark Poliakov	45055998bd	feat: device upper mapping poc	2021-10-09 21:14:08 +03:00
Mark Poliakov	70490c9aa8	feat: add lower kernel identity-mapping	2021-10-09 12:38:30 +03:00