Modularize

28 files changed, 695 insertions, 281 deletions

diff --git a/README.md b/README.md
new file mode 100644
index 0000000..dbfb491
--- /dev/null
+++ b/README.md
@@ -0,0 +1,19 @@
+# Cortex-A7 ghOSt v0.0.1
+
+## Implemented
+ - Memory Allocator
+ - Serial Writer
+ - Basic Mutex
+
+## TODO
+ - Paging/ Memory Translation Tables
+ - Scheduler (Round-robin)
+ - Exclusive Lock
+ - Semaphore
+ - Display Buffer
+ - Interrupt handler
+ - Interrupt initializer
+ - Publish-Subscribe topics
+ - Testing
+ - System Calls
+ - Timing
diff --git a/src/_arch/arm/asm.rs b/src/_arch/arm/asm.rs
index f767e08..423f456 100644
--- a/src/_arch/arm/asm.rs
+++ b/src/_arch/arm/asm.rs
@@ -38,3 +38,11 @@ pub fn spin_for_n_cycles(n: u32) {
 				bne 1b", in("r1") n);
     }
 }
+
+/// # Switch to user mode
+#[inline]
+pub fn usr_mode() {
+    unsafe {
+        core::arch::asm!("cps 0x10");
+    }
+}
diff --git a/src/bsp/drivers/gpio/mod.rs b/src/bsp/drivers/gpio/mod.rs
new file mode 100644
index 0000000..a94c6e5
--- /dev/null
+++ b/src/bsp/drivers/gpio/mod.rs
@@ -0,0 +1,28 @@
+/// # GPIO Register
+pub mod GPPUD {
+    const ADDR: u32 = 0x3F200094;
+    pub fn set(v: u32) {
+        use crate::cpu::store32;
+        store32(ADDR, v)
+    }
+    pub fn clear() {
+        set(0)
+    }
+}
+/// # GPIO Clock 0 Register
+pub mod GPPUDCLK0 {
+    const ADDR: u32 = 0x3F200098;
+    const MASK: u32 = (1 << 14) | (1 << 15);
+    pub fn set(v: u32) {
+        use crate::cpu::store32;
+        store32(ADDR, v)
+    }
+    pub fn init() {
+        use crate::cpu::spin_for_n_cycles;
+        super::GPPUD::clear();
+        spin_for_n_cycles(150);
+        set(MASK);
+        spin_for_n_cycles(150);
+        set(0);
+    }
+}
diff --git a/src/bsp/drivers/mod.rs b/src/bsp/drivers/mod.rs
new file mode 100644
index 0000000..eda17fa
--- /dev/null
+++ b/src/bsp/drivers/mod.rs
@@ -0,0 +1,2 @@
+mod gpio;
+pub mod uart;
diff --git a/src/uart.rs b/src/bsp/drivers/uart/console.rs
index e516a6c..e3ee2f2 100644
--- a/src/uart.rs
+++ b/src/bsp/drivers/uart/console.rs
@@ -1,26 +1,10 @@
 //! # UART Console Definition
+use super::*;
+use crate::bsp::drivers::gpio::*;
 use crate::cpu::*;
-use crate::sync::interface::Mutex;
-use crate::sync::NullLock;
+use crate::lib::sync::interface::Mutex;
+use crate::lib::sync::SpinLock;
 use core::fmt;
-use crate::gpio::*;
-
-/// # Data Register
-const UART0_DR: u32 = 0x3F201000;
-/// # Flag Register
-const UART0_FR: u32 = 0x3F201018;
-/// # Fractional Baud Rate Register
-const UART0_FBRD: u32 = 0x3F201028;
-/// # Line Control Register
-const UART0_LCRH: u32 = 0x3F20102C;
-/// # Control Register
-const UART0_CR: u32 = 0x3F201030;
-/// # Interrupt Mask Set/ Clear Register
-const UART0_IMSC: u32 = 0x3F201038;
-/// # Interrupt Control Register
-const UART0_ICR: u32 = 0x3F201044;
-/// # Integer Baud Rate Register
-const UART0_IBRD: u32 = 0x3F201024;
 
 /// # UART Inner Structure
 ///
@@ -33,7 +17,7 @@ struct UartInner {
 ///
 /// Wraps the UART writer in a sharable lock.
 pub struct Uart {
-    inner: NullLock<UartInner>,
+    inner: SpinLock<UartInner>,
 }
 
 impl UartInner {
@@ -48,35 +32,28 @@ impl UartInner {
     ///
     /// Set baud rate and timings
     pub fn init(&mut self) {
-        store32(UART0_CR, 0);
-        store32(GPPUD, 0);
-        spin_for_n_cycles(150);
-        store32(GPPUDCLK0, (1 << 14) | (1 << 15));
-        spin_for_n_cycles(150);
-        store32(GPPUDCLK0, 0);
-        store32(UART0_ICR, 0x7FF);
-        store32(UART0_IBRD, 9);
-        store32(UART0_FBRD, 49);
-        store32(UART0_LCRH, (1 << 4) | (1 << 5) | (1 << 6));
-        store32(
-            UART0_IMSC,
-            (1 << 1) | (1 << 4) | (1 << 5) | (1 << 6) | (1 << 7) | (1 << 8) | (1 << 9) | (1 << 10),
-        );
-        store32(UART0_CR, (1 << 0) | (1 << 8) | (1 << 9));
+        UART::CR::off();
+        GPPUDCLK0::init();
+        UART::ICR::clear();
+        UART::IBRD::set(9);
+        UART::FBRD::set(49);
+        UART::LCRH::enable8();
+        UART::IMSC::mask();
+        UART::CR::all_on();
     }
 
     /// # Write `char` to UART
     fn write_char(&mut self, ch: char) {
-        while load32(UART0_FR) & 0x20 != 0 {
+        while UART::FR::TXFF::is_set() {
             nop();
         }
-        store32(UART0_DR, ch as u32);
+        UART::DR::set(ch);
         self.chars_written += 1;
     }
 
     /// # Flush UART
     fn flush(&self) {
-        while load32(UART0_FR) & 0x08 != 0 {
+        while UART::FR::BUSY::is_set() {
             nop();
         }
     }
@@ -96,7 +73,7 @@ impl Uart {
     /// # Create sharable UART wrapper
     pub const fn new() -> Self {
         Self {
-            inner: NullLock::new(UartInner::new()),
+            inner: SpinLock::new(UartInner::new()),
         }
     }
 
@@ -107,7 +84,7 @@ impl Uart {
     }
 }
 
-impl super::console::interface::Write for Uart {
+impl crate::lib::console::interface::Write for Uart {
     /// # Write `char` to UART
     fn write_char(&self, c: char) {
         self.inner.lock(|inner| inner.write_char(c));
@@ -124,7 +101,7 @@ impl super::console::interface::Write for Uart {
     }
 }
 
-impl super::console::interface::Statistics for Uart {
+impl crate::lib::console::interface::Statistics for Uart {
     /// # Get `char` written stats
     fn chars_written(&self) -> usize {
         self.inner.lock(|inner| inner.chars_written)
@@ -132,7 +109,4 @@ impl super::console::interface::Statistics for Uart {
 }
 
 /// # UART Writer + Stats
-impl super::console::interface::All for Uart {}
-
-/// # Public reference to console.
-pub static UART_WRITER: Uart = Uart::new();
+impl crate::lib::console::interface::All for Uart {}
diff --git a/src/bsp/drivers/uart/mod.rs b/src/bsp/drivers/uart/mod.rs
new file mode 100644
index 0000000..ad11e12
--- /dev/null
+++ b/src/bsp/drivers/uart/mod.rs
@@ -0,0 +1,119 @@
+/// # UART Registers
+pub mod UART {
+    /// # Flag Register
+    pub mod FR {
+        pub mod BUSY {
+            const MASK: u32 = 1 << 3;
+            pub fn is_set() -> bool {
+                super::read() & MASK != 0
+            }
+
+            pub fn is_clear() -> bool {
+                !is_set()
+            }
+        }
+        pub mod TXFF {
+            const MASK: u32 = 1 << 5;
+            pub fn is_set() -> bool {
+                super::read() & MASK != 0
+            }
+
+            pub fn is_clear() -> bool {
+                !is_set()
+            }
+        }
+        const ADDR: u32 = 0x3F201018;
+        //pub const TXFF: u32 = 1 << 5;
+        pub fn read() -> u32 {
+            use crate::cpu::load32;
+            load32(ADDR)
+        }
+    }
+    /// # Data Register
+    pub mod DR {
+        const ADDR: u32 = 0x3F201000;
+        pub fn set(c: char) {
+            use crate::cpu::store32;
+            store32(ADDR, c as u32);
+        }
+    }
+    /// # Control Register
+    pub mod CR {
+        const ADDR: u32 = 0x3F201030;
+        const UART_ENABLE: u32 = 1 << 0;
+        const TX_ENABLE: u32 = 1 << 8;
+        const RX_ENABLE: u32 = 1 << 9;
+        pub fn set(v: u32) {
+            use crate::cpu::store32;
+            store32(ADDR, v);
+        }
+        pub fn off() {
+            set(0);
+        }
+        pub fn all_on() {
+            set(UART_ENABLE + TX_ENABLE + RX_ENABLE)
+        }
+    }
+    /// # Integer Baud Rate
+    pub mod IBRD {
+        const ADDR: u32 = 0x3F201024;
+        pub fn set(v: u32) {
+            use crate::cpu::store32;
+            store32(ADDR, v);
+        }
+    }
+    /// # Fractional Baud Rate
+    pub mod FBRD {
+        const ADDR: u32 = 0x3F201028;
+        pub fn set(v: u32) {
+            use crate::cpu::store32;
+            store32(ADDR, v);
+        }
+    }
+    /// # Interrupt Control Register
+    pub mod ICR {
+        const ADDR: u32 = 0x3F201044;
+        pub fn set(v: u32) {
+            use crate::cpu::store32;
+            store32(ADDR, v);
+        }
+        pub fn clear() {
+            set(0x7FF);
+        }
+    }
+    /// # Line Control Register
+    pub mod LCRH {
+        const ADDR: u32 = 0x3F20102C;
+        const FIFO_ENABLE: u32 = 1 << 4;
+        const FIVE_BIT: u32 = 0b00 << 5;
+        const SIX_BIT: u32 = 0b01 << 5;
+        const SEVEN_BIT: u32 = 0b10 << 5;
+        const EIGHT_BIT: u32 = 0b11 << 5;
+        pub fn set(v: u32) {
+            use crate::cpu::store32;
+            store32(ADDR, v);
+        }
+        pub fn enable8() {
+            set(FIFO_ENABLE + EIGHT_BIT);
+        }
+    }
+    /// # Interrupt Mask Set/ Clear Register
+    pub mod IMSC {
+        const ADDR: u32 = 0x3F201038;
+        const MASK: u32 =
+            (1 << 1) | (1 << 4) | (1 << 5) | (1 << 6) | (1 << 7) | (1 << 8) | (1 << 9) | (1 << 10);
+        pub fn set(v: u32) {
+            use crate::cpu::store32;
+            store32(ADDR, v);
+        }
+        pub fn mask() {
+            set(MASK);
+        }
+    }
+}
+
+mod console;
+pub use console::*;
+
+/// # Public reference to console.
+pub static UART_WRITER: Uart = Uart::new();
diff --git a/src/bsp/mod.rs b/src/bsp/mod.rs
new file mode 100644
index 0000000..2c554a7
--- /dev/null
+++ b/src/bsp/mod.rs
@@ -0,0 +1 @@
+pub mod drivers;
diff --git a/src/cpu.rs b/src/cpu/mod.rs
index 669f5f2..1a3fbc5 100644
--- a/src/cpu.rs
+++ b/src/cpu/mod.rs
@@ -1,10 +1,10 @@
 //! # Processor code.
 #[cfg(target_arch = "arm")]
-#[path = "_arch/arm/cpu.rs"]
+#[path = "../_arch/arm/cpu.rs"]
 mod arch_cpu;
 mod boot;
 
 /// # Low-level bindings
 ///
 /// Re-export low-level bindings
-pub use arch_cpu::{load32, nop, spin_for_n_cycles, store32, wait_forever};
+pub use arch_cpu::*;
diff --git a/src/draw.rs b/src/extra/draw.rs
index 9327715..9327715 100644
--- a/src/draw.rs
+++ b/src/extra/draw.rs
diff --git a/src/gpio.rs b/src/gpio.rs
deleted file mode 100644
index 5af665b..0000000
--- a/src/gpio.rs
+++ /dev/null
@@ -1,4 +0,0 @@
-/// GPIO Register
-pub const GPPUD: u32 = 0x3F200094;
-/// GPIO Clock 0 Register
-pub const GPPUDCLK0: u32 = 0x3F200098;
diff --git a/src/kernel.rs b/src/kernel.rs
index 36b6e42..8c7ab58 100644
--- a/src/kernel.rs
+++ b/src/kernel.rs
@@ -15,26 +15,17 @@
 #![feature(exclusive_range_pattern)]
 #![feature(default_alloc_error_handler)]
 #![feature(optimize_attribute)]
+#![feature(naked_functions)]
 #![no_main]
 #![no_std]
 
-extern crate alloc;
-pub use alloc::boxed::Box;
-pub use alloc::format;
-pub use alloc::string::String;
-
-mod console;
+mod bsp;
 mod cpu;
-mod draw;
-mod gpio;
-mod mem;
-mod panic_wait;
-mod print;
-mod sync;
-mod uart;
+mod lib;
 mod util;
-use crate::console::console;
-use crate::mem::alloc::allocator;
+use crate::lib::console::console;
+use crate::util::mem::alloc::allocator;
+use crate::util::mem::{format, Box, String};
 
 /// # Initialization Code
 ///
@@ -48,10 +39,13 @@ use crate::mem::alloc::allocator;
 ///    - serial_println!
 ///    - vprint!
 ///    - vserial_println!
+///  - MMU
+///    - SpinLocks
 ///
 /// After initialization, jump to
 /// the regular main.
 unsafe fn kernel_init() -> ! {
+    util::mem::mmu_init();
     console().init().unwrap();
     allocator().init().unwrap();
     kernel_main()
@@ -80,7 +74,7 @@ fn kernel_main() -> ! {
             "\x1b[94mAuthors:\x1b[0m \x1b[95m{}\x1b[0m",
             env!("CARGO_PKG_AUTHORS")
         );
-        use crate::console::interface::Statistics;
+        use crate::lib::console::interface::Statistics;
         serial_println!(
             "Characters written to UART: \x1b[91m{}\x1b[0m",
             console().chars_written()
@@ -94,7 +88,7 @@ fn kernel_main() -> ! {
 }
 
 fn run_verbose() {
-    serial_println!("U8: {:?}", mem::alloc::U8_GRAND_ALLOC);
+    serial_println!("U8: {:?}", util::mem::alloc::U8_GRAND_ALLOC);
     {
         let mut s = String::new();
         for _ in 0..128 {
@@ -130,9 +124,19 @@ fn run_verbose() {
         assert_eq!(*c, 7);
         serial_println!("{} {} {}", a, b, c);
     }
-    use crate::console::interface::Statistics;
+    use crate::lib::console::interface::Statistics;
     serial_println!(
         "Characters written to UART: \x1b[91m{}\x1b[0m",
         console().chars_written()
     );
 }
+
+#[no_mangle]
+/// # SVC Handler
+pub fn svc_handler(code: u32) {
+    match code {
+        _ => {
+            serial_println!("Unhandled Service Call!");
+        }
+    }
+}
diff --git a/src/console.rs b/src/lib/console.rs
index ae3e62b..4ff1579 100644
--- a/src/console.rs
+++ b/src/lib/console.rs
@@ -52,6 +52,6 @@ pub mod interface {
 /// # UART console
 ///
 /// Returns a borrow for the UART writer
-pub fn console() -> &'static crate::uart::Uart {
-    &crate::uart::UART_WRITER
+pub fn console() -> &'static crate::bsp::drivers::uart::Uart {
+    &crate::bsp::drivers::uart::UART_WRITER
 }
diff --git a/src/lib/fifo_queue.rs b/src/lib/fifo_queue.rs
new file mode 100644
index 0000000..a35f15f
--- /dev/null
+++ b/src/lib/fifo_queue.rs
@@ -0,0 +1,2 @@
+pub use os_pic::init_fifo_queue;
+pub use os_pic::util::fifo_queue::FifoQueue;
diff --git a/src/lib/lifo_queue.rs b/src/lib/lifo_queue.rs
new file mode 100644
index 0000000..dfc3973
--- /dev/null
+++ b/src/lib/lifo_queue.rs
@@ -0,0 +1,2 @@
+pub use os_pic::init_lifo_queue;
+pub use os_pic::util::lifo_queue::LifoQueue;
diff --git a/src/lib/mod.rs b/src/lib/mod.rs
new file mode 100644
index 0000000..66c4b37
--- /dev/null
+++ b/src/lib/mod.rs
@@ -0,0 +1,6 @@
+pub mod console;
+pub mod fifo_queue;
+pub mod lifo_queue;
+pub mod node;
+pub mod queue;
+pub mod sync;
diff --git a/src/lib/node.rs b/src/lib/node.rs
new file mode 100644
index 0000000..df69fba
--- /dev/null
+++ b/src/lib/node.rs
@@ -0,0 +1 @@
+pub use os_pic::util::node::Node;
diff --git a/src/lib/queue.rs b/src/lib/queue.rs
new file mode 100644
index 0000000..7092100
--- /dev/null
+++ b/src/lib/queue.rs
@@ -0,0 +1 @@
+pub use os_pic::util::queue::Queue;
diff --git a/src/sync.rs b/src/lib/sync.rs
index 373e8f8..373e8f8 100644
--- a/src/sync.rs
+++ b/src/lib/sync.rs
diff --git a/src/mem.rs b/src/mem.rs
deleted file mode 100644
index e32bb13..0000000
--- a/src/mem.rs
+++ /dev/null
@@ -1,5 +0,0 @@
-//! # Memory crate
-//!
-//! Provides the Allocator for the OS.
-pub mod alloc;
-mod types;
diff --git a/src/util.rs b/src/util.rs
deleted file mode 100644
index 34ae1f2..0000000
--- a/src/util.rs
+++ /dev/null
@@ -1,5 +0,0 @@
-//! # Utility crate
-//!
-//! Provides Queue structure
-mod fifo_queue;
-pub use fifo_queue::*;
diff --git a/src/util/fifo_queue.rs b/src/util/fifo_queue.rs
deleted file mode 100644
index 6748ab4..0000000
--- a/src/util/fifo_queue.rs
+++ /dev/null
@@ -1,191 +0,0 @@
-//! # FIFO Queue
-//!
-//! Provides the FIFO queue structure for allocations
-use crate::sync::interface::Mutex;
-use crate::sync::NullLock;
-use crate::serial_vprintln;
-use core::fmt;
-use core::fmt::{Debug, Formatter};
-
-/// # Initialize Queue
-/// - Name: Symbol name
-/// - Size: Number of elements
-/// - Default: Default value
-/// - Type: Data Type
-#[macro_export]
-macro_rules! init_queue {
-	($name:tt,$size:tt,$default:tt,$type:ty) => {
-		init_queue!{@gen [$name,$size,$default,$type,concat!("# ", stringify!($type), " Queue Allocator")]}
-	};
-	(@gen [$name:tt,$size:tt,$default:tt,$type:ty,$doc:expr]) => {
-		#[doc = $doc]
-		#[link_section = ".data.alloc"]
-		pub static $name: QueueAllocator<'static, $type, {$size+2}> = QueueAllocator::<$type, {$size+2}>{inner: NullLock::new([QueueItem::new($default); {$size+2}])};
-	};
-}
-
-#[derive(Copy, Clone)]
-/// # Queue Item
-///
-/// Encapsulates a data element and a pointer to
-/// the next `Queue` item
-pub struct QueueItem<'a, T: Sized> {
-    /// # Data
-    ///
-    /// The encapsulated data
-    data: T,
-    /// # Pointer to the next item
-    ///
-    /// Stores either `None` or points
-    /// to the next item.
-    next: Option<*mut QueueItem<'a, T>>,
-}
-
-impl<T> QueueItem<'_, T> {
-    /// # Constructor
-    pub const fn new(data: T) -> Self {
-        Self {
-            data: data,
-            next: None,
-        }
-    }
-    /// # Get the inner data
-    ///
-    /// Returns a borrow of the underlying data.
-    pub fn inner(&mut self) -> &mut T {
-        &mut self.data
-    }
-    /// # Get pointer to inner data
-    pub fn ptr(&mut self) -> *mut u8 {
-        self.inner() as *mut T as *mut u8
-    }
-}
-
-/// # Sharing Thread Safety for QueueItem
-unsafe impl<T> Send for QueueItem<'_, T> {}
-
-impl<T: Debug> Debug for QueueItem<'_, T> {
-    /// # Debug formatter for `QueueItem`
-    ///
-    /// Output the encapsulated data
-    fn fmt(&self, f: &mut Formatter<'_>) -> fmt::Result {
-        #[cfg(feature = "verbose")]
-        return write!(
-            f,
-            "{:?} {:x} {:?}",
-            self.data, self as *const QueueItem<'_, T> as usize, self.next
-        );
-
-        #[cfg(not(feature = "verbose"))]
-        return write!(f, "{:?}", self.data);
-    }
-}
-
-/// # Queue Allocator
-///
-/// Structure to store a pool of allocated data structures.
-pub struct QueueAllocator<'a, T: Sized, const COUNT: usize> {
-    /// # Synchronized Pool of items
-    ///
-    /// Stores synchronization wrapper around the data pool
-    pub inner: NullLock<[QueueItem<'a, T>; COUNT]>,
-}
-
-/// # Sharing Thread Safety for QueueAllocator
-unsafe impl<T, const COUNT: usize> Send for QueueAllocator<'_, T, COUNT> {}
-
-impl<'a, T: Sized, const COUNT: usize> QueueAllocator<'a, T, COUNT> {
-    /// # Initialization of Fixed-Size Pool
-    ///
-    /// Establishes the header and footer of the queue
-    /// as the first and second elements respectively.
-    /// All of the internal elements point to the next
-    /// one and the final element points to `None`
-    pub fn init(&self) {
-        serial_vprintln!("QA: Initializing Queue Allocator!");
-        self.inner.lock(|queue| {
-            serial_vprintln!("QA: Clearing internal references...");
-            for idx in 2..COUNT {
-                if idx != COUNT - 1 {
-                    queue[idx].next = Some(&mut queue[idx + 1] as *mut QueueItem<'a, T>);
-                } else {
-                    queue[idx].next = None;
-                }
-            }
-            serial_vprintln!("QA: Initializing head and tail...");
-            queue[0].next = Some(&mut queue[2] as *mut QueueItem<'a, T>);
-            queue[1].next = Some(&mut queue[COUNT - 1] as *mut QueueItem<'a, T>);
-        });
-        serial_vprintln!("QA: Initialized Queue Allocator!");
-    }
-
-    /// # Allocate Data
-    ///
-    /// If there is a data chunk available,
-    /// return it, otherwise return `None`
-    #[allow(dead_code)]
-    pub fn alloc(&self) -> Option<&mut QueueItem<'a, T>> {
-        serial_vprintln!("QA: Allocating chunk!");
-        return self.inner.lock(|pool| {
-            if let Some(entry) = pool[0].next {
-                serial_vprintln!("QA: Found chunk!");
-                pool[0].next = unsafe { (*entry).next };
-                unsafe {
-                    (*entry).next = None;
-                }
-                match pool[0].next {
-                    None => pool[1].next = None,
-                    _ => {}
-                }
-                serial_vprintln!("QA: \x1b[92mAllocated {:x}\x1b[0m", unsafe {
-                    (*entry).ptr() as usize
-                });
-                return Some(unsafe { &mut *entry as &mut QueueItem<'a, T> });
-            } else {
-                serial_vprintln!("QA: No chunks available!");
-                return None;
-            }
-        });
-    }
-
-    /// # Free
-    ///
-    /// Add the item to the end of the queue.
-    /// If there were no items, set it as the head.
-    #[allow(dead_code)]
-    pub fn free(&self, freed_item: &mut QueueItem<'a, T>) {
-        serial_vprintln!("QA: Deallocating chunk!");
-        self.inner.lock(|pool| {
-            freed_item.next = None;
-            match pool[1].next {
-                None => {
-                    pool[0].next = Some(freed_item as *mut QueueItem<'a, T>);
-                }
-                Some(entry) => unsafe {
-                    (*entry).next = Some(freed_item as *mut QueueItem<'a, T>);
-                },
-            }
-            pool[1].next = Some(freed_item as *mut QueueItem<'a, T>);
-            serial_vprintln!(
-                "QA: \x1b[91mDeallocated {:x}\x1b[0m",
-                freed_item.ptr() as usize
-            );
-        });
-    }
-}
-
-impl<T: Debug, const COUNT: usize> Debug for QueueAllocator<'_, T, COUNT> {
-    /// # Debug Formatted Output
-    ///
-    /// Output each data point in the array with
-    /// its debug formatter.
-    fn fmt(&self, f: &mut Formatter<'_>) -> fmt::Result {
-        self.inner.lock(|queue| {
-            #[cfg(feature = "verbose")]
-            return write!(f, "{:?}", queue);
-
-            #[cfg(not(feature = "verbose"))]
-            return write!(f, "{:?}", queue);
-        })
-    }
-}
diff --git a/src/util/mem/alloc.rs b/src/util/mem/alloc.rs
new file mode 100644
index 0000000..dfe8c60
--- /dev/null
+++ b/src/util/mem/alloc.rs
@@ -0,0 +1,289 @@
+//! # Allocate crate
+//!
+//! Provides the Global allocator and methods
+//! to create special purpose allocators.
+use super::types::*;
+use crate::lib::lifo_queue::init_lifo_queue;
+use crate::lib::lifo_queue::LifoQueue;
+use crate::lib::node::Node;
+use crate::lib::queue::Queue;
+use crate::lib::sync::interface::Mutex;
+use crate::lib::sync::NullLock;
+use crate::serial_vprintln;
+use crate::util::mem::{GlobalAlloc, Layout};
+
+/// # Grand Allocator
+///
+/// The structure that uses different sized pools and allocates memory chunks
+pub struct GrandAllocator {}
+
+/// # The number of elements of each size
+const GRAND_ALLOC_SIZE: usize = 64;
+
+init_lifo_queue!(U8_GRAND_ALLOC, GRAND_ALLOC_SIZE, 0, u8);
+init_lifo_queue!(U16_GRAND_ALLOC, GRAND_ALLOC_SIZE, 0, u16);
+init_lifo_queue!(U32_GRAND_ALLOC, GRAND_ALLOC_SIZE, 0, u32);
+init_lifo_queue!(U64_GRAND_ALLOC, GRAND_ALLOC_SIZE, 0, u64);
+init_lifo_queue!(U128_GRAND_ALLOC, GRAND_ALLOC_SIZE, 0, u128);
+init_lifo_queue!(U256_GRAND_ALLOC, GRAND_ALLOC_SIZE, { U256::new() }, U256);
+init_lifo_queue!(U512_GRAND_ALLOC, GRAND_ALLOC_SIZE, { U512::new() }, U512);
+init_lifo_queue!(U1024_GRAND_ALLOC, GRAND_ALLOC_SIZE, { U1024::new() }, U1024);
+init_lifo_queue!(U2048_GRAND_ALLOC, GRAND_ALLOC_SIZE, { U2048::new() }, U2048);
+init_lifo_queue!(U4096_GRAND_ALLOC, GRAND_ALLOC_SIZE, { U4096::new() }, U4096);
+
+impl GrandAllocator {
+    pub fn init(&self) -> Result<(), &'static str> {
+        serial_vprintln!("GA: \x1b[93mInit U8 Pool\x1b[0m");
+        U8_GRAND_ALLOC.init();
+        serial_vprintln!("GA: \x1b[93mInit U16 Pool\x1b[0m");
+        U16_GRAND_ALLOC.init();
+        serial_vprintln!("GA: \x1b[93mInit U32 Pool\x1b[0m");
+        U32_GRAND_ALLOC.init();
+        serial_vprintln!("GA: \x1b[93mInit U64 Pool\x1b[0m");
+        U64_GRAND_ALLOC.init();
+        serial_vprintln!("GA: \x1b[93mInit U128 Pool\x1b[0m");
+        U128_GRAND_ALLOC.init();
+        serial_vprintln!("GA: \x1b[93mInit U256 Pool\x1b[0m");
+        U256_GRAND_ALLOC.init();
+        serial_vprintln!("GA: \x1b[93mInit U512 Pool\x1b[0m");
+        U512_GRAND_ALLOC.init();
+        serial_vprintln!("GA: \x1b[93mInit U1024 Pool\x1b[0m");
+        U1024_GRAND_ALLOC.init();
+        serial_vprintln!("GA: \x1b[93mInit U2048 Pool\x1b[0m");
+        U2048_GRAND_ALLOC.init();
+        serial_vprintln!("GA: \x1b[93mInit U4096 Pool\x1b[0m");
+        U4096_GRAND_ALLOC.init();
+        serial_vprintln!("GA: \x1b[94mPools Initialized!\x1b[0m");
+        Ok(())
+    }
+}
+
+unsafe impl GlobalAlloc for GrandAllocator {
+    /// # Allocator
+    ///
+    /// Allocate the fixed size chunks
+    unsafe fn alloc(&self, layout: Layout) -> *mut u8 {
+        serial_vprintln!("GA: Allocating chunk of size {}!", layout.size());
+        match layout.size() {
+            1 => match U8_GRAND_ALLOC.pop() {
+                None => {
+                    panic!("No cells to allocate!");
+                }
+                Some(elem) => {
+                    return (*elem).ptr();
+                }
+            },
+            2 => match U16_GRAND_ALLOC.pop() {
+                None => {
+                    panic!("No cells to allocate!");
+                }
+                Some(elem) => {
+                    return (*elem).ptr();
+                }
+            },
+            3..=4 => match U32_GRAND_ALLOC.pop() {
+                None => {
+                    panic!("No cells to allocate!");
+                }
+                Some(elem) => {
+                    return (*elem).ptr();
+                }
+            },
+            5..=8 => match U64_GRAND_ALLOC.pop() {
+                None => {
+                    panic!("No cells to allocate!");
+                }
+                Some(elem) => {
+                    return (*elem).ptr();
+                }
+            },
+            9..=16 => match U128_GRAND_ALLOC.pop() {
+                None => {
+                    panic!("No cells to allocate!");
+                }
+                Some(elem) => {
+                    return (*elem).ptr();
+                }
+            },
+            17..=32 => match U256_GRAND_ALLOC.pop() {
+                None => {
+                    panic!("No cells to allocate!");
+                }
+                Some(elem) => {
+                    return (*elem).ptr();
+                }
+            },
+            33..=64 => match U512_GRAND_ALLOC.pop() {
+                None => {
+                    panic!("No cells to allocate!");
+                }
+                Some(elem) => {
+                    return (*elem).ptr();
+                }
+            },
+            65..=128 => match U1024_GRAND_ALLOC.pop() {
+                None => {
+                    panic!("No cells to allocate!");
+                }
+                Some(elem) => {
+                    return (*elem).ptr();
+                }
+            },
+            129..=256 => match U2048_GRAND_ALLOC.pop() {
+                None => {
+                    panic!("No cells to allocate!");
+                }
+                Some(elem) => {
+                    return (*elem).ptr();
+                }
+            },
+            257..=512 => match U4096_GRAND_ALLOC.pop() {
+                None => {
+                    panic!("No cells to allocate!");
+                }
+                Some(elem) => {
+                    return (*elem).ptr();
+                }
+            },
+            _ => {
+                panic!("No allocators for size {}!", layout.size());
+            }
+        }
+    }
+
+    /// # Deallocate
+    ///
+    /// Deallocate the fixed size chunks by searching for them
+    unsafe fn dealloc(&self, ptr: *mut u8, layout: Layout) {
+        serial_vprintln!("GA: Deallocating chunk of size {}!", layout.size());
+        match layout.size() {
+            1 => {
+                U8_GRAND_ALLOC.inner.lock(|pool| {
+					let spacing: usize = (pool[2].ptr() as usize) - (pool[1].ptr() as usize);
+					let diff: usize = (ptr as usize) - (pool[1].ptr() as usize);
+					let index: usize = diff/spacing;
+					assert!(index < GRAND_ALLOC_SIZE, "{} is out of the allocation bounds ({})", index, GRAND_ALLOC_SIZE);
+					assert_eq!(diff % spacing, 0, "{} is not aligned with the spacings and so it must not have been allocated by the Grand Allocator", diff % spacing);
+					U8_GRAND_ALLOC.push(&mut pool[index+1]);
+					serial_vprintln!("GA: Freeing ({}, {}, {})", index, diff, spacing);
+				});
+            }
+            2 => {
+                U16_GRAND_ALLOC.inner.lock(|pool| {
+					let spacing: usize = (pool[2].ptr() as usize) - (pool[1].ptr() as usize);
+					let diff: usize = (ptr as usize) - (pool[1].ptr() as usize);
+					let index: usize = diff/spacing;
+					assert!(index < GRAND_ALLOC_SIZE, "{} is out of the allocation bounds ({})", index, GRAND_ALLOC_SIZE);
+					assert_eq!(diff % spacing, 0, "{} is not aligned with the spacings and so it must not have been allocated by the Grand Allocator", diff % spacing);
+					U16_GRAND_ALLOC.push(&mut pool[index+1]);
+					serial_vprintln!("GA: Freeing ({}, {}, {})", index, diff, spacing);
+				});
+            }
+            3..=4 => {
+                U32_GRAND_ALLOC.inner.lock(|pool| {
+					let spacing: usize = (pool[2].ptr() as usize) - (pool[1].ptr() as usize);
+					let diff: usize = (ptr as usize) - (pool[1].ptr() as usize);
+					let index: usize = diff/spacing;
+					assert!(index < GRAND_ALLOC_SIZE, "{} is out of the allocation bounds ({})", index, GRAND_ALLOC_SIZE);
+					assert_eq!(diff % spacing, 0, "{} is not aligned with the spacings and so it must not have been allocated by the Grand Allocator", diff % spacing);
+					U32_GRAND_ALLOC.push(&mut pool[index+1]);
+					serial_vprintln!("GA: Freeing ({}, {}, {})", index, diff, spacing);
+				});
+            }
+            5..=8 => {
+                U64_GRAND_ALLOC.inner.lock(|pool| {
+					let spacing: usize = (pool[2].ptr() as usize) - (pool[1].ptr() as usize);
+					let diff: usize = (ptr as usize) - (pool[1].ptr() as usize);
+					let index: usize = diff/spacing;
+					assert!(index < GRAND_ALLOC_SIZE, "{} is out of the allocation bounds ({})", index, GRAND_ALLOC_SIZE);
+					assert_eq!(diff % spacing, 0, "{} is not aligned with the spacings and so it must not have been allocated by the Grand Allocator", diff % spacing);
+					U64_GRAND_ALLOC.push(&mut pool[index+1]);
+					serial_vprintln!("GA: Freeing ({}, {}, {})", index, diff, spacing);
+				});
+            }
+            9..=16 => {
+                U128_GRAND_ALLOC.inner.lock(|pool| {
+					let spacing: usize = (pool[2].ptr() as usize) - (pool[1].ptr() as usize);
+					let diff: usize = (ptr as usize) - (pool[1].ptr() as usize);
+					let index: usize = diff/spacing;
+					assert!(index < GRAND_ALLOC_SIZE, "{} is out of the allocation bounds ({})", index, GRAND_ALLOC_SIZE);
+					assert_eq!(diff % spacing, 0, "{} is not aligned with the spacings and so it must not have been allocated by the Grand Allocator", diff % spacing);
+					U128_GRAND_ALLOC.push(&mut pool[index+1]);
+					serial_vprintln!("GA: Freeing ({}, {}, {})", index, diff, spacing);
+				});
+            }
+            17..=32 => {
+                U256_GRAND_ALLOC.inner.lock(|pool| {
+					let spacing: usize = (pool[2].ptr() as usize) - (pool[1].ptr() as usize);
+					let diff: usize = (ptr as usize) - (pool[1].ptr() as usize);
+					let index: usize = diff/spacing;
+					assert!(index < GRAND_ALLOC_SIZE, "{} is out of the allocation bounds ({})", index, GRAND_ALLOC_SIZE);
+					assert_eq!(diff % spacing, 0, "{} is not aligned with the spacings and so it must not have been allocated by the Grand Allocator", diff % spacing);
+					U256_GRAND_ALLOC.push(&mut pool[index+1]);
+					serial_vprintln!("GA: Freeing ({}, {}, {})", index, diff, spacing);
+				});
+            }
+            33..=64 => {
+                U512_GRAND_ALLOC.inner.lock(|pool| {
+					let spacing: usize = (pool[2].ptr() as usize) - (pool[1].ptr() as usize);
+					let diff: usize = (ptr as usize) - (pool[1].ptr() as usize);
+					let index: usize = diff/spacing;
+					assert!(index < GRAND_ALLOC_SIZE, "{} is out of the allocation bounds ({})", index, GRAND_ALLOC_SIZE);
+					assert_eq!(diff % spacing, 0, "{} is not aligned with the spacings and so it must not have been allocated by the Grand Allocator", diff % spacing);
+					U512_GRAND_ALLOC.push(&mut pool[index+1]);
+					serial_vprintln!("GA: Freeing ({}, {}, {})", index, diff, spacing);
+				});
+            }
+            65..=128 => {
+                U1024_GRAND_ALLOC.inner.lock(|pool| {
+					let spacing: usize = (pool[2].ptr() as usize) - (pool[1].ptr() as usize);
+					let diff: usize = (ptr as usize) - (pool[1].ptr() as usize);
+					let index: usize = diff/spacing;
+					assert!(index < GRAND_ALLOC_SIZE, "{} is out of the allocation bounds ({})", index, GRAND_ALLOC_SIZE);
+					assert_eq!(diff % spacing, 0, "{} is not aligned with the spacings and so it must not have been allocated by the Grand Allocator", diff % spacing);
+					U1024_GRAND_ALLOC.push(&mut pool[index+1]);
+					serial_vprintln!("GA: Freeing ({}, {}, {})", index, diff, spacing);
+				});
+            }
+            129..=256 => {
+                U2048_GRAND_ALLOC.inner.lock(|pool| {
+					let spacing: usize = (pool[2].ptr() as usize) - (pool[1].ptr() as usize);
+					let diff: usize = (ptr as usize) - (pool[1].ptr() as usize);
+					let index: usize = diff/spacing;
+					assert!(index < GRAND_ALLOC_SIZE, "{} is out of the allocation bounds ({})", index, GRAND_ALLOC_SIZE);
+					assert_eq!(diff % spacing, 0, "{} is not aligned with the spacings and so it must not have been allocated by the Grand Allocator", diff % spacing);
+					U2048_GRAND_ALLOC.push(&mut pool[index+1]);
+					serial_vprintln!("GA: Freeing ({}, {}, {})", index, diff, spacing);
+				});
+            }
+            257..=512 => {
+                U4096_GRAND_ALLOC.inner.lock(|pool| {
+					let spacing: usize = (pool[2].ptr() as usize) - (pool[1].ptr() as usize);
+					let diff: usize = (ptr as usize) - (pool[1].ptr() as usize);
+					let index: usize = diff/spacing;
+					assert!(index < GRAND_ALLOC_SIZE, "{} is out of the allocation bounds ({})", index, GRAND_ALLOC_SIZE);
+					assert_eq!(diff % spacing, 0, "{} is not aligned with the spacings and so it must not have been allocated by the Grand Allocator", diff % spacing);
+					U4096_GRAND_ALLOC.push(&mut pool[index+1]);
+					serial_vprintln!("GA: Freeing ({}, {}, {})", index, diff, spacing);
+				});
+            }
+            _ => {
+                panic!("No deallocators for size {}!", layout.size());
+            }
+        }
+    }
+}
+
+/// # Grand Allocator
+///
+/// The allocator of allocators. It hands out fixed sized memory chunks.
+#[global_allocator]
+pub static ALLOCATOR: GrandAllocator = GrandAllocator {};
+
+/// # Global Allocator
+///
+/// Returns a borrow for the Global Allocator
+pub fn allocator() -> &'static crate::util::mem::alloc::GrandAllocator {
+    serial_vprintln!("AL: Getting global allocator!");
+    &crate::util::mem::alloc::ALLOCATOR
+}
diff --git a/src/util/mem/mod.rs b/src/util/mem/mod.rs
new file mode 100644
index 0000000..36c02a2
--- /dev/null
+++ b/src/util/mem/mod.rs
@@ -0,0 +1,13 @@
+//! # Memory crate
+//!
+//! Provides the Allocator for the OS.
+pub mod alloc;
+mod paging;
+mod types;
+pub use paging::*;
+
+extern crate alloc as core_alloc;
+pub use core_alloc::alloc::{GlobalAlloc, Layout};
+pub use core_alloc::boxed::Box;
+pub use core_alloc::format;
+pub use core_alloc::string::String;
diff --git a/src/util/mem/paging.rs b/src/util/mem/paging.rs
new file mode 100644
index 0000000..8a21bda
--- /dev/null
+++ b/src/util/mem/paging.rs
@@ -0,0 +1,93 @@
+//! # MMU Functions
+
+pub mod MMU {
+    pub const CACHABLE: u32 = 1 << 3;
+    pub const BUFFERABLE: u32 = 1 << 2;
+    pub const NO_PERMISSIONS_REQUIRED: u32 = 0b11 << 10;
+    pub const PERMISSIONS_REQUIRED: u32 = 0b01 << 10;
+    pub const BASE: u32 = 0x0004000;
+    pub const MASK: u32 = 0x1005;
+}
+
+/// # Start
+pub fn mmu_start(init: u32, mask: u32) {
+    unsafe {
+        core::arch::asm!("mov r2, #0
+    // Invalidate Caches
+    mcr p15,0,r2,c7,c1,6 
+    // Invalidate TLB entries
+    mcr p15,0,r2,c8,c7,0 
+    // Data synchronisation barrier
+    dsb
+
+    // Set all domains to 0b11
+    mvn r2, #0
+    bic r2, #0xC
+    mcr p15,0,r2,c3,c0,0
+
+    // Set the translation table base address (remember to align 16 KiB!)
+    mcr p15,0,r0,c2,c0,0
+    mcr p15,0,r0,c2,c0,1
+    mov r3, #0
+    mcr p15,0,r3,c2,c0,2
+
+    // Set the bits mentioned above
+    mrc p15,0,r2,c1,c0,0
+    orr r2,r2,r1
+    mcr p15,0,r2,c1,c0,0", in("r0") init, in("r1") mask);
+    }
+}
+
+/// # Stop
+pub fn mmu_stop() {
+    unsafe {
+        core::arch::asm!(
+            "mrc p15,0,r2,c1,c0,0
+    bic r2,#0x1000
+    bic r2,#0x0004
+    bic r2,#0x0001
+    mcr p15,0,r2,c1,c0,0"
+        );
+    }
+}
+
+pub fn tlb_invalidate() {
+    unsafe {
+        core::arch::asm!(
+            "mov r2, #0
+    // Invalidate Entries
+    mcr p15, 0, r2, c8, c7, 0
+    // DSB
+    mcr p15, 0, r2, c7, c10, 4"
+        );
+    }
+}
+
+pub fn mmu_section(virt: u32, phys: u32, flags: u32) {
+    use crate::cpu::store32;
+    let offset: u32 = virt >> 20;
+    let entry: u32 = MMU::BASE | (offset << 2);
+    let physv: u32 = (phys & 0xFFF00000) | (flags & 0x7FFC) | 0x00C02;
+    store32(entry, physv)
+}
+
+pub fn mmu_init() {
+    let mut addr: u32 = 0;
+    loop {
+        mmu_section(
+            addr,
+            addr,
+            MMU::CACHABLE | MMU::BUFFERABLE | MMU::NO_PERMISSIONS_REQUIRED,
+        );
+        if addr == 0xFFF00000 {
+            break;
+        }
+        addr += 0x00100000;
+    }
+    mmu_section(
+        0x3F200000,
+        0x3F200000,
+        MMU::CACHABLE | MMU::BUFFERABLE | MMU::PERMISSIONS_REQUIRED,
+    );
+    mmu_start(MMU::BASE, MMU::MASK);
+}
diff --git a/src/util/mem/types.rs b/src/util/mem/types.rs
new file mode 100644
index 0000000..ed22132
--- /dev/null
+++ b/src/util/mem/types.rs
@@ -0,0 +1,54 @@
+/// # u256 struct
+///
+/// 256 bit size field
+#[derive(Copy, Clone)]
+pub struct U256(u128, u128);
+impl U256 {
+    pub const fn new() -> Self {
+        U256(0, 0)
+    }
+}
+
+/// # u512 struct
+///
+/// 512 bit size field
+#[derive(Copy, Clone)]
+pub struct U512(U256, U256);
+impl U512 {
+    pub const fn new() -> Self {
+        U512(U256::new(), U256::new())
+    }
+}
+
+/// # u1024 struct
+///
+/// 1024 bit size field
+#[derive(Copy, Clone)]
+pub struct U1024(U512, U512);
+impl U1024 {
+    pub const fn new() -> Self {
+        U1024(U512::new(), U512::new())
+    }
+}
+
+/// # u2048 struct
+///
+/// 2048 bit size field
+#[derive(Copy, Clone)]
+pub struct U2048(U1024, U1024);
+impl U2048 {
+    pub const fn new() -> Self {
+        U2048(U1024::new(), U1024::new())
+    }
+}
+
+/// # u4096 struct
+///
+/// 4096 bit size field
+#[derive(Copy, Clone)]
+pub struct U4096(U2048, U2048);
+impl U4096 {
+    pub const fn new() -> Self {
+        U4096(U2048::new(), U2048::new())
+    }
+}
diff --git a/src/util/mod.rs b/src/util/mod.rs
new file mode 100644
index 0000000..d33f520
--- /dev/null
+++ b/src/util/mod.rs
@@ -0,0 +1,3 @@
+pub mod mem;
+pub mod panic_wait;
+pub mod print;
diff --git a/src/panic_wait.rs b/src/util/panic_wait.rs
index a8d0962..a8d0962 100644
--- a/src/panic_wait.rs
+++ b/src/util/panic_wait.rs
diff --git a/src/print.rs b/src/util/print.rs
index 56fd6c5..34e808c 100644
--- a/src/print.rs
+++ b/src/util/print.rs
@@ -1,8 +1,8 @@
 //! # Printing to UART
 //!
 //! This module contains the macros to print formatted strings to UART.
-use crate::console::interface::Write;
-use crate::uart::UART_WRITER;
+use crate::bsp::drivers::uart::UART_WRITER;
+use crate::lib::console::interface::Write;
 use core::fmt;
 
 #[doc(hidden)]
@@ -15,7 +15,7 @@ pub fn _serial_print(args: fmt::Arguments) {
 /// Print formatted arguments without a newline
 #[macro_export]
 macro_rules! serial_print {
-	($($arg:tt)*) => ($crate::print::_serial_print(format_args!($($arg)*)));
+	($($arg:tt)*) => ($crate::util::print::_serial_print(format_args!($($arg)*)));
 }
 
 /// # Print with newline
@@ -23,9 +23,9 @@ macro_rules! serial_print {
 /// Print formatted arguments with a newline
 #[macro_export]
 macro_rules! serial_println {
-	() => ($crate::print!("\n"));
+	() => ($crate::serial_print!("\n"));
 	($($arg:tt)*) => ({
-		$crate::print::_serial_print(format_args_nl!($($arg)*));
+		$crate::util::print::_serial_print(format_args_nl!($($arg)*));
 	})
 }
 
@@ -36,7 +36,7 @@ macro_rules! serial_println {
 macro_rules! serial_vprint {
 	($($arg:tt)*) => ({
 		#[cfg(feature="verbose")]
-		$crate::print::_serial_print(format_args!($($arg)*))
+		$crate::util::print::_serial_print(format_args!($($arg)*))
 	});
 }
 
@@ -47,10 +47,10 @@ macro_rules! serial_vprint {
 macro_rules! serial_vprintln {
 	() => ({
 		#[cfg(feature="verbose")]
-		$crate::print!("\n")
+		$crate::serial_print!("\n")
 	});
 	($($arg:tt)*) => ({
 		#[cfg(feature="verbose")]
-		$crate::print::_serial_print(format_args_nl!($($arg)*));
+		$crate::util::print::_serial_print(format_args_nl!($($arg)*));
 	})
 }