Impelement a basic logger, WIP

rework interrupts

.github/workflows/rust.yml

@@ -21,8 +21,7 @@ jobs:
       - name: Run format check
         run: cargo fmt --check
       - name: Run lint
-        run: cargo clippy -- -D warnings
+        run: cargo clippy --target aarch64-unknown-none -- -D warnings
-
   build:
     runs-on: ubuntu-latest
 
@@ -33,4 +32,16 @@ jobs:
       - name: Add AArch64 Target
         run: rustup target add aarch64-unknown-none
       - name: Build
-        run: cargo build --verbose
+        run: cargo build --verbose --target aarch64-unknown-none
+
+  test:
+    runs-on: ubuntu-latest
+
+    steps:
+      - uses: actions/checkout@v4
+      - name: Install rustfmt for nightly
+        run: rustup component add --toolchain nightly-x86_64-unknown-linux-gnu rustfmt clippy
+      - name: Add AArch64 Target
+        run: rustup target add aarch64-unknown-none
+      - name: Heap Workspace Test
+        run: cargo test -p heap

Cargo.lock

@@ -3,16 +3,37 @@
 version = 4
 
 [[package]]
-name = "NovaError"
+name = "cfg-if"
-version = "0.1.0"
+version = "1.0.4"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "9330f8b2ff13f34540b44e946ef35111825727b38d33286ef986142615121801"
+
+[[package]]
+name = "getrandom"
+version = "0.3.4"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "899def5c37c4fd7b2664648c28120ecec138e4d395b459e5ca34f9cce2dd77fd"
+dependencies = [
+ "cfg-if",
+ "libc",
+ "r-efi",
+ "wasip2",
+]
 
 [[package]]
 name = "heap"
 version = "0.1.0"
 dependencies = [
- "NovaError",
+ "nova_error",
+ "rand",
 ]
 
+[[package]]
+name = "libc"
+version = "0.2.178"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "37c93d8daa9d8a012fd8ab92f088405fb202ea0b6ab73ee2482ae66af4f42091"
+
 [[package]]
 name = "libm"
 version = "0.2.15"
@@ -23,7 +44,125 @@ checksum = "f9fbbcab51052fe104eb5e5d351cf728d30a5be1fe14d9be8a3b097481fb97de"
 name = "nova"
 version = "0.1.0"
 dependencies = [
- "NovaError",
  "heap",
  "libm",
+ "nova_error",
+]
+
+[[package]]
+name = "nova_error"
+version = "0.1.0"
+
+[[package]]
+name = "ppv-lite86"
+version = "0.2.21"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "85eae3c4ed2f50dcfe72643da4befc30deadb458a9b590d720cde2f2b1e97da9"
+dependencies = [
+ "zerocopy",
+]
+
+[[package]]
+name = "proc-macro2"
+version = "1.0.103"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "5ee95bc4ef87b8d5ba32e8b7714ccc834865276eab0aed5c9958d00ec45f49e8"
+dependencies = [
+ "unicode-ident",
+]
+
+[[package]]
+name = "quote"
+version = "1.0.42"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "a338cc41d27e6cc6dce6cefc13a0729dfbb81c262b1f519331575dd80ef3067f"
+dependencies = [
+ "proc-macro2",
+]
+
+[[package]]
+name = "r-efi"
+version = "5.3.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "69cdb34c158ceb288df11e18b4bd39de994f6657d83847bdffdbd7f346754b0f"
+
+[[package]]
+name = "rand"
+version = "0.9.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "6db2770f06117d490610c7488547d543617b21bfa07796d7a12f6f1bd53850d1"
+dependencies = [
+ "rand_chacha",
+ "rand_core",
+]
+
+[[package]]
+name = "rand_chacha"
+version = "0.9.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d3022b5f1df60f26e1ffddd6c66e8aa15de382ae63b3a0c1bfc0e4d3e3f325cb"
+dependencies = [
+ "ppv-lite86",
+ "rand_core",
+]
+
+[[package]]
+name = "rand_core"
+version = "0.9.3"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "99d9a13982dcf210057a8a78572b2217b667c3beacbf3a0d8b454f6f82837d38"
+dependencies = [
+ "getrandom",
+]
+
+[[package]]
+name = "syn"
+version = "2.0.111"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "390cc9a294ab71bdb1aa2e99d13be9c753cd2d7bd6560c77118597410c4d2e87"
+dependencies = [
+ "proc-macro2",
+ "quote",
+ "unicode-ident",
+]
+
+[[package]]
+name = "unicode-ident"
+version = "1.0.22"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "9312f7c4f6ff9069b165498234ce8be658059c6728633667c526e27dc2cf1df5"
+
+[[package]]
+name = "wasip2"
+version = "1.0.1+wasi-0.2.4"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "0562428422c63773dad2c345a1882263bbf4d65cf3f42e90921f787ef5ad58e7"
+dependencies = [
+ "wit-bindgen",
+]
+
+[[package]]
+name = "wit-bindgen"
+version = "0.46.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "f17a85883d4e6d00e8a97c586de764dabcc06133f7f1d55dce5cdc070ad7fe59"
+
+[[package]]
+name = "zerocopy"
+version = "0.8.31"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "fd74ec98b9250adb3ca554bdde269adf631549f51d8a8f8f0a10b50f1cb298c3"
+dependencies = [
+ "zerocopy-derive",
+]
+
+[[package]]
+name = "zerocopy-derive"
+version = "0.8.31"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d8a8d209fdf45cf5138cbb5a506f6b52522a25afccc534d1475dad8e31105c6a"
+dependencies = [
+ "proc-macro2",
+ "quote",
+ "syn",
 ]

Cargo.toml

@@ -15,10 +15,10 @@ panic = "abort"
 [dependencies]
 libm = "0.2.15"
 heap = {path = "heap"}
-NovaError = {path = "NovaError"}
+nova_error = {path = "nova_error"}
 
 [workspace]
 
-members = [ "NovaError",
+members = [ "nova_error",
     "heap"
 ]

README.md

@@ -2,7 +2,7 @@
 
 NovaOS is a expository project where I build a kernel from scratch for a Raspberry PI 3 B+.
 
-[Technical write-up](https://blog.leafnova.net/projects/pi3_kernel/)
+[Technical write-up](https://leafnova.net/projects/pi3_kernel/)
 
 ## Features
 

heap/Cargo.toml

@@ -4,4 +4,7 @@ version = "0.1.0"
 edition = "2024"
 
 [dependencies]
-NovaError = {path = "../NovaError"}
+nova_error = {path = "../nova_error"}
+
+[dev-dependencies]
+rand = "0.9.2"

heap/src/lib.rs

@@ -3,45 +3,38 @@
 
 use core::{
     alloc::GlobalAlloc,
-    default::Default,
     mem::size_of,
     prelude::v1::*,
-    ptr::{self, null_mut, read_volatile},
+    ptr::{self, null_mut},
     result::Result,
 };
 
-use NovaError::NovaError;
+use nova_error::NovaError;
-
-#[cfg(not(target_os = "none"))]
-extern crate std;
 
 extern crate alloc;
 
 #[repr(C, align(16))]
-pub struct HeapHeader {
+#[derive(Clone, Copy)]
-    pub next: *mut HeapHeader,
+struct HeapHeader {
-    before: *mut HeapHeader,
+    next: Option<*mut HeapHeader>,
-    pub size: usize,
+    before: Option<*mut HeapHeader>,
+    size: usize,
     free: bool,
 }
 
 const HEAP_HEADER_SIZE: usize = size_of::<HeapHeader>();
 const MIN_BLOCK_SIZE: usize = 16;
 
-// TODO: This implementation has to be reevaluated when implementing multiprocessing
-// Spinlock could be a solution but has its issues:
-// https://matklad.github.io/2020/01/02/spinlocks-considered-harmful.html
-
 pub struct Heap {
-    pub start_address: *mut HeapHeader,
+    start_address: *mut HeapHeader,
-    pub end_address: *mut HeapHeader,
+    end_address: *mut HeapHeader,
-    pub raw_size: usize,
+    raw_size: usize,
 }
 impl Heap {
     pub const fn empty() -> Self {
         Self {
-            start_address: null_mut() as *mut HeapHeader,
+            start_address: null_mut(),
-            end_address: null_mut() as *mut HeapHeader,
+            end_address: null_mut(),
             raw_size: 0,
         }
     }
@@ -50,14 +43,14 @@ impl Heap {
         self.start_address = heap_start as *mut HeapHeader;
         self.end_address = heap_end as *mut HeapHeader;
 
-        self.raw_size = heap_end - heap_start;
+        self.raw_size = heap_end - heap_start + 1;
 
         unsafe {
             ptr::write(
                 self.start_address,
                 HeapHeader {
-                    next: null_mut(),
+                    next: None,
-                    before: null_mut(),
+                    before: None,
                     size: self.raw_size - HEAP_HEADER_SIZE,
                     free: true,
                 },
@@ -67,16 +60,19 @@ impl Heap {
 
     unsafe fn find_first_fit(&self, size: usize) -> Result<*mut HeapHeader, NovaError> {
         let mut current = self.start_address;
-        while !fits(size, current) {
+        unsafe {
-            if (*self.start_address).next.is_null() {
+            while !fits(size, current) {
-                return Err(NovaError::HeapFull);
+                if let Some(next) = (*current).next {
+                    current = next;
+                } else {
+                    return Err(NovaError::HeapFull);
+                }
             }
-            current = (*current).next;
         }
         Ok(current)
     }
 
-    pub fn malloc(&self, mut size: usize) -> Result<*mut u8, NovaError> {
+    fn malloc(&self, mut size: usize) -> Result<*mut u8, NovaError> {
         if size == 0 {
             return Err(NovaError::EmptyHeapSegmentNotAllowed);
         }
@@ -112,43 +108,47 @@ impl Heap {
         let new_address = unsafe { current.byte_add(byte_offset) };
 
         // Handle case where fragmenting center free space
-        let next = (*current).next;
-        if !(*current).next.is_null() {
-            (*next).before = new_address;
-        }
-
         unsafe {
+            let next = (*current).next;
+            if let Some(next) = next {
+                (*next).before = Some(new_address);
+            }
+
             ptr::write(
-                new_address as *mut HeapHeader,
+                new_address,
                 HeapHeader {
                     next,
-                    before: current,
+                    before: Some(current),
-                    size: (*current).size - size - HEAP_HEADER_SIZE,
+                    size: (*current).size - byte_offset,
                     free: true,
                 },
-            )
+            );
-        };
+
-        (*current).next = new_address;
+            (*current).next = Some(new_address);
-        (*current).free = false;
+            (*current).free = false;
-        (*current).size = size;
+            (*current).size = size;
+        }
     }
 
-    pub fn free(&self, pointer: *mut u8) -> Result<(), NovaError> {
+    fn free(&self, pointer: *mut u8) -> Result<(), NovaError> {
-        let mut segment = unsafe { pointer.sub(HEAP_HEADER_SIZE) as *mut HeapHeader };
+        let mut segment = Self::get_header_ref_from_data_pointer(pointer);
         unsafe {
             // IF prev is free:
             // Delete header, add size to previous and fix pointers.
             // Move Head left
-            if !(*segment).before.is_null() && (*(*segment).before).free {
+            if let Some(before_head) = (*segment).before
-                let before_head = (*segment).before;
+                && (*before_head).free
+            {
                 (*before_head).size += (*segment).size + HEAP_HEADER_SIZE;
                 delete_header(segment);
                 segment = before_head;
             }
+
             // IF next is free:
             // Delete next header and merge size, fix pointers
-            if !(*segment).next.is_null() && (*(*segment).next).free {
+            if let Some(next_head) = (*segment).next
-                let next_head = (*segment).next;
+                && (*next_head).free
+            {
                 (*segment).size += (*next_head).size + HEAP_HEADER_SIZE;
                 delete_header(next_head);
             }
@@ -159,6 +159,10 @@ impl Heap {
 
         Ok(())
     }
+
+    const fn get_header_ref_from_data_pointer(pointer: *mut u8) -> *mut HeapHeader {
+        unsafe { pointer.sub(HEAP_HEADER_SIZE) as *mut HeapHeader }
+    }
 }
 
 unsafe impl GlobalAlloc for Heap {
@@ -174,21 +178,23 @@ unsafe impl GlobalAlloc for Heap {
 unsafe impl Sync for Heap {}
 
 unsafe fn fits(size: usize, header: *mut HeapHeader) -> bool {
-    (*header).free && size <= (*header).size
+    unsafe { (*header).free && size <= (*header).size }
 }
 
 unsafe fn delete_header(header: *mut HeapHeader) {
-    let before = (*header).before;
+    unsafe {
-    let next = (*header).next;
+        let before_opt = (*header).before;
+        let next_opt = (*header).next;
 
-    if !before.is_null() {
+        if let Some(before) = before_opt {
-        (*before).next = next;
+            (*before).next = next_opt;
-    }
+        }
 
-    if !next.is_null() {
+        if let Some(next) = next_opt {
-        (*next).before = before;
+            (*next).before = before_opt;
+        }
     }
 }
 
 #[cfg(test)]
-mod tests {}
+mod tests;

heap/src/tests.rs

@@ -0,0 +1,165 @@
+use super::*;
+use rand::{self, random_range};
+extern crate std;
+
+static HEAP_SIZE: usize = 1024;
+
+#[test]
+fn test_heap_allocation() {
+    let heap_vector = Box::new([0u8; HEAP_SIZE]);
+    let mut heap = Heap::empty();
+    heap.init(
+        &heap_vector[0] as *const u8 as usize,
+        &heap_vector[HEAP_SIZE - 1] as *const u8 as usize,
+    );
+
+    let root_header = heap.start_address;
+
+    let malloc_size = random_range(0..(HEAP_SIZE - HEAP_HEADER_SIZE));
+    let malloc = heap.malloc(malloc_size).unwrap();
+    let malloc_header = Heap::get_header_ref_from_data_pointer(malloc);
+
+    assert_eq!(root_header, malloc_header);
+
+    unsafe {
+        let actual_alloc_size = (*malloc_header).size;
+        let actual_raw_size = actual_alloc_size + HEAP_HEADER_SIZE;
+        // Verify sizing
+        assert!(actual_alloc_size >= malloc_size);
+        assert_eq!(actual_alloc_size % MIN_BLOCK_SIZE, 0);
+
+        // Verify section is occupied
+        assert!((*malloc_header).free == false);
+
+        // Verify next header has been created
+        let next = (*malloc_header).next.unwrap();
+
+        assert_eq!(malloc_header.byte_add(actual_raw_size), next);
+        assert!((*next).free);
+        assert_eq!((*malloc_header).next.unwrap(), next);
+        assert_eq!((*next).before.unwrap(), malloc_header);
+        assert_eq!((*next).size, HEAP_SIZE - actual_raw_size - HEAP_HEADER_SIZE)
+    }
+}
+
+#[test]
+fn test_full_heap() {
+    let heap_vector = Box::new([0u8; HEAP_SIZE]);
+    let mut heap = Heap::empty();
+    heap.init(
+        &heap_vector[0] as *const u8 as usize,
+        &heap_vector[HEAP_SIZE - 1] as *const u8 as usize,
+    );
+
+    let malloc_size = HEAP_SIZE - HEAP_HEADER_SIZE;
+    let malloc = heap.malloc(malloc_size).unwrap();
+    let malloc_header = Heap::get_header_ref_from_data_pointer(malloc);
+    unsafe {
+        assert_eq!((*malloc_header).free, false);
+        assert!((*malloc_header).next.is_none());
+    }
+
+    let malloc2 = heap.malloc(MIN_BLOCK_SIZE);
+    assert!(malloc2.is_err());
+}
+
+#[test]
+fn test_freeing_root() {
+    let heap_vector = Box::new([0u8; HEAP_SIZE]);
+    let mut heap = Heap::empty();
+    heap.init(
+        &heap_vector[0] as *const u8 as usize,
+        &heap_vector[HEAP_SIZE - 1] as *const u8 as usize,
+    );
+
+    let root_header = heap.start_address;
+    let root_header_start_size = unsafe { (*root_header).size };
+
+    let malloc_size = random_range(0..((HEAP_SIZE - HEAP_HEADER_SIZE) / 2));
+    let malloc = heap.malloc(malloc_size).unwrap();
+    let malloc_header = Heap::get_header_ref_from_data_pointer(malloc);
+    unsafe {
+        assert_eq!((*malloc_header).free, false);
+        assert!((*malloc_header).size >= malloc_size);
+        assert!((*root_header).next.is_some());
+
+        assert!(heap.free(malloc).is_ok());
+
+        assert_eq!((*root_header).size, root_header_start_size);
+        assert!((*root_header).next.is_none());
+    }
+}
+
+#[test]
+fn test_merging_free_sections() {
+    let heap_vector = Box::new([0u8; HEAP_SIZE]);
+    let mut heap = Heap::empty();
+    heap.init(
+        &heap_vector[0] as *const u8 as usize,
+        &heap_vector[HEAP_SIZE - 1] as *const u8 as usize,
+    );
+
+    let root_header = heap.start_address;
+    let root_header_start_size = unsafe { (*root_header).size };
+
+    let malloc1 = heap.malloc(MIN_BLOCK_SIZE).unwrap();
+    let malloc_header_before = unsafe { *Heap::get_header_ref_from_data_pointer(malloc1) };
+    let malloc2 = heap.malloc(MIN_BLOCK_SIZE).unwrap();
+    let _ = heap.malloc(MIN_BLOCK_SIZE).unwrap();
+
+    unsafe {
+        assert!(heap.free(malloc1).is_ok());
+
+        let malloc_header_free = *Heap::get_header_ref_from_data_pointer(malloc1);
+        assert_ne!(malloc_header_before.free, malloc_header_free.free);
+        assert_eq!(malloc_header_before.size, malloc_header_free.size);
+
+        assert!(heap.free(malloc2).is_ok());
+        let malloc_header_merge = *Heap::get_header_ref_from_data_pointer(malloc1);
+
+        assert!(malloc_header_merge.free);
+        assert_eq!(
+            malloc_header_merge.size,
+            malloc_header_free.size + MIN_BLOCK_SIZE + HEAP_HEADER_SIZE
+        );
+    }
+}
+
+#[test]
+fn test_first_fit() {
+    let heap_vector = Box::new([0u8; HEAP_SIZE]);
+    let mut heap = Heap::empty();
+    heap.init(
+        &heap_vector[0] as *const u8 as usize,
+        &heap_vector[HEAP_SIZE - 1] as *const u8 as usize,
+    );
+
+    let root_header = heap.start_address;
+    let root_header_start_size = unsafe { (*root_header).size };
+
+    let malloc1 = heap.malloc(MIN_BLOCK_SIZE).unwrap();
+    let malloc2 = heap.malloc(MIN_BLOCK_SIZE).unwrap();
+    let malloc3 = heap.malloc(MIN_BLOCK_SIZE * 3).unwrap();
+    let malloc4 = heap.malloc(MIN_BLOCK_SIZE).unwrap();
+
+    unsafe {
+        assert!(heap.free(malloc1).is_ok());
+        assert!(heap.free(malloc3).is_ok());
+        let malloc5 = heap.malloc(MIN_BLOCK_SIZE * 2).unwrap();
+        let malloc1_header = unsafe { *Heap::get_header_ref_from_data_pointer(malloc1) };
+
+        // First free block stays empty
+        assert!(malloc1_header.free);
+
+        // New allocation takes the first fit aka. malloc3
+        assert_eq!(malloc5, malloc3);
+
+        // If no free slot could be found, append to the end
+        let malloc6 = heap.malloc(MIN_BLOCK_SIZE * 2).unwrap();
+        assert!(malloc6 > malloc4);
+
+        // Malloc7 takes slot of Malloc1
+        let malloc7 = heap.malloc(MIN_BLOCK_SIZE).unwrap();
+        assert_eq!(malloc1, malloc7);
+    }
+}

link.ld

@@ -27,7 +27,7 @@ SECTIONS {
         KEEP(*(.vector_table))
     }
 
-    .heap 0x8000000 : ALIGN(16)
+    .heap : ALIGN(16)
     {
     __heap_start = .;
     . += 0x10000; #10kB
@@ -41,6 +41,13 @@ SECTIONS {
     __stack_end = .;
     }
 
+    .stack_el0 : ALIGN(16)
+    {
+    __stack_start_el0 = .;
+    . += 0x10000; #10kB stack
+    __stack_end_el0 = .;
+    }
+
 
     _end = .;
 }

nova_error/Cargo.toml

@@ -1,4 +1,4 @@
 [package]
-name = "NovaError"
+name = "nova_error"
 version = "0.1.0"
 edition = "2024"

src/configuration.rs

@@ -1,9 +1,10 @@
-static SCTLR_EL1_MMU_DISABLED: u64 = 0 << 0; //M
+static SCTLR_EL1_MMU_DISABLED: u64 = 0; //M
 static SCTLR_EL1_DATA_CACHE_DISABLED: u64 = 0 << 2; //C
 static SCTLR_EL1_INSTRUCTION_CACHE_DISABLED: u64 = 0 << 12; //I
 static SCTLR_EL1_LITTLE_ENDIAN_EL0: u64 = 0 << 24; //E0E
 static SCTLR_EL1_LITTLE_ENDIAN_EL1: u64 = 0 << 25; //EE
 
+#[allow(clippy::identity_op)]
 static SCTLR_EL1_RES: u64 = (0 << 6) | (1 << 11) | (0 << 17) | (1 << 20) | (1 << 22); //Res0 & Res1
 
 #[no_mangle]

src/framebuffer.rs

@@ -4,7 +4,10 @@ mod bitmaps;
 
 use bitmaps::BASIC_LEGACY;
 
-use crate::mailbox::{read_mailbox, write_mailbox};
+use crate::{
+    mailbox::{read_mailbox, write_mailbox},
+    println,
+};
 #[repr(align(16))]
 struct Mailbox([u32; 36]);
 
@@ -16,6 +19,7 @@ const SET_PIXEL_ORDER: u32 = 0x0004_8006;
 const GET_PITCH: u32 = 0x000_40008;
 const SET_FB_OFFSET: u32 = 0x0004_8009;
 
+#[allow(dead_code)]
 pub struct FrameBuffer {
     pixel_depth: u32, // Bits per pixel
     pitch: u32,       // Pixel per row
@@ -31,74 +35,6 @@ pub const ORANGE: u32 = 0x00FFA500;
 pub const YELLOW: u32 = 0x00FFFF00;
 
 impl FrameBuffer {
-    pub fn new() -> Self {
-        let mut mailbox = Mailbox([0; 36]);
-        mailbox.0[0] = 35 * 4;
-        mailbox.0[1] = 0;
-
-        mailbox.0[2] = SET_PHYSICAL_DISPLAY_WH;
-        mailbox.0[3] = 8;
-        mailbox.0[4] = 8;
-        mailbox.0[5] = 1920;
-        mailbox.0[6] = 1080;
-
-        mailbox.0[7] = SET_VIRTUAL_DISPLAY_WH;
-        mailbox.0[8] = 8;
-        mailbox.0[9] = 8;
-        mailbox.0[10] = 1920;
-        mailbox.0[11] = 1080;
-
-        mailbox.0[12] = SET_PIXEL_DEPTH;
-        mailbox.0[13] = 4;
-        mailbox.0[14] = 4;
-        mailbox.0[15] = 32; // 32 bit per pixel
-
-        mailbox.0[16] = SET_PIXEL_ORDER;
-        mailbox.0[17] = 4;
-        mailbox.0[18] = 4;
-        mailbox.0[19] = 0x0; // RGB
-
-        mailbox.0[20] = SET_FB_OFFSET;
-        mailbox.0[21] = 8;
-        mailbox.0[22] = 8;
-        mailbox.0[23] = 0; // X in pixels
-        mailbox.0[24] = 0; // Y in pixels
-
-        mailbox.0[25] = ALLOCATE_BUFFER;
-        mailbox.0[26] = 8;
-        mailbox.0[27] = 4;
-        mailbox.0[28] = 4096; // Alignment
-        mailbox.0[29] = 0;
-
-        mailbox.0[30] = GET_PITCH;
-        mailbox.0[31] = 4;
-        mailbox.0[32] = 0;
-        mailbox.0[33] = 0;
-
-        mailbox.0[34] = 0; // End tag
-
-        // TODO: validate responses
-
-        let addr = core::ptr::addr_of!(mailbox.0[0]) as u32;
-
-        write_mailbox(8, addr);
-
-        let _ = read_mailbox(8);
-        if mailbox.0[1] == 0 {
-            println!("Failed");
-        }
-
-        mailbox.0[28] &= 0x3FFFFFFF;
-
-        Self {
-            pixel_depth: mailbox.0[15],
-            pitch: mailbox.0[33] / (mailbox.0[15] / 8),
-            rows: mailbox.0[29] / mailbox.0[33],
-            start_addr: mailbox.0[28] as *mut u32,
-            size: mailbox.0[29],
-        }
-    }
-
     pub fn draw_pixel(&self, x: u32, y: u32, color: u32) {
         let offset = x + y * self.pitch;
         unsafe {
@@ -109,6 +45,7 @@ impl FrameBuffer {
     /*Bresenham's line algorithm
     TODO: check if its possible to optimize y1==y2 case (ARM neon?)
     */
+    #[allow(clippy::collapsible_else_if)]
     pub fn draw_line(&self, x1: u32, y1: u32, x2: u32, y2: u32, color: u32) {
         if x1 == x2 {
             for y in y1..=y2 {
@@ -218,7 +155,7 @@ impl FrameBuffer {
     }
 
     fn draw_ascii(&self, x: u32, y: u32, char: usize, scale: u32, color: u32) {
-        for (y_offset, row) in (&BASIC_LEGACY[char]).iter().enumerate() {
+        for (y_offset, row) in BASIC_LEGACY[char].iter().enumerate() {
             for bit in 0..8 {
                 match row & (1 << bit) {
                     0 => {}
@@ -241,3 +178,73 @@ impl FrameBuffer {
         }
     }
 }
+
+impl Default for FrameBuffer {
+    fn default() -> Self {
+        let mut mailbox = Mailbox([0; 36]);
+        mailbox.0[0] = 35 * 4;
+        mailbox.0[1] = 0;
+
+        mailbox.0[2] = SET_PHYSICAL_DISPLAY_WH;
+        mailbox.0[3] = 8;
+        mailbox.0[4] = 8;
+        mailbox.0[5] = 1920;
+        mailbox.0[6] = 1080;
+
+        mailbox.0[7] = SET_VIRTUAL_DISPLAY_WH;
+        mailbox.0[8] = 8;
+        mailbox.0[9] = 8;
+        mailbox.0[10] = 1920;
+        mailbox.0[11] = 1080;
+
+        mailbox.0[12] = SET_PIXEL_DEPTH;
+        mailbox.0[13] = 4;
+        mailbox.0[14] = 4;
+        mailbox.0[15] = 32; // 32 bit per pixel
+
+        mailbox.0[16] = SET_PIXEL_ORDER;
+        mailbox.0[17] = 4;
+        mailbox.0[18] = 4;
+        mailbox.0[19] = 0x0; // RGB
+
+        mailbox.0[20] = SET_FB_OFFSET;
+        mailbox.0[21] = 8;
+        mailbox.0[22] = 8;
+        mailbox.0[23] = 0; // X in pixels
+        mailbox.0[24] = 0; // Y in pixels
+
+        mailbox.0[25] = ALLOCATE_BUFFER;
+        mailbox.0[26] = 8;
+        mailbox.0[27] = 4;
+        mailbox.0[28] = 4096; // Alignment
+        mailbox.0[29] = 0;
+
+        mailbox.0[30] = GET_PITCH;
+        mailbox.0[31] = 4;
+        mailbox.0[32] = 0;
+        mailbox.0[33] = 0;
+
+        mailbox.0[34] = 0; // End tag
+
+        // TODO: validate responses
+
+        let addr = core::ptr::addr_of!(mailbox.0[0]) as u32;
+
+        write_mailbox(8, addr);
+
+        let _ = read_mailbox(8);
+        if mailbox.0[1] == 0 {
+            println!("Failed");
+        }
+
+        mailbox.0[28] &= 0x3FFFFFFF;
+
+        Self {
+            pixel_depth: mailbox.0[15],
+            pitch: mailbox.0[33] / (mailbox.0[15] / 8),
+            rows: mailbox.0[29] / mailbox.0[33],
+            start_addr: mailbox.0[28] as *mut u32,
+            size: mailbox.0[29],
+        }
+    }
+}

src/interrupt_handlers.rs

@@ -0,0 +1,280 @@
+use core::arch::asm;
+
+use alloc::vec::Vec;
+
+use crate::{
+    get_current_el,
+    interrupt_handlers::daif::unmask_irq,
+    peripherals::{
+        gpio::{read_gpio_event_detect_status, reset_gpio_event_detect_status},
+        uart::clear_uart_interrupt_state,
+    },
+    println, read_address, write_address,
+};
+
+const INTERRUPT_BASE: u32 = 0x3F00_B000;
+const IRQ_PENDING_BASE: u32 = INTERRUPT_BASE + 0x204;
+const ENABLE_IRQ_BASE: u32 = INTERRUPT_BASE + 0x210;
+const DISABLE_IRQ_BASE: u32 = INTERRUPT_BASE + 0x21C;
+
+const GPIO_PENDING_BIT_OFFSET: u64 = 0b1111 << 49;
+
+struct InterruptHandlers {
+    source: IRQSource,
+    function: fn(),
+}
+
+// TODO: replace with hashmap and check for better alternatives for option
+static mut INTERRUPT_HANDLERS: Option<Vec<InterruptHandlers>> = None;
+
+#[derive(Clone)]
+#[repr(u32)]
+pub enum IRQSource {
+    AuxInt = 29,
+    I2cSpiSlvInt = 44,
+    Pwa0 = 45,
+    Pwa1 = 46,
+    Smi = 48,
+    GpioInt0 = 49,
+    GpioInt1 = 50,
+    GpioInt2 = 51,
+    GpioInt3 = 52,
+    I2cInt = 53,
+    SpiInt = 54,
+    PcmInt = 55,
+    UartInt = 57,
+}
+
+/// Representation of the ESR_ELx registers
+///
+///  Reference: D1.10.4
+#[derive(Debug, Clone, Copy)]
+#[allow(dead_code)]
+struct EsrElX {
+    ec: u32,
+    il: u32,
+    iss: u32,
+}
+
+impl From<u32> for EsrElX {
+    fn from(value: u32) -> Self {
+        Self {
+            ec: value >> 26,
+            il: (value >> 25) & 0b1,
+            iss: value & 0x1FFFFFF,
+        }
+    }
+}
+
+#[no_mangle]
+unsafe extern "C" fn rust_irq_handler() {
+    daif::mask_all();
+    let pending_irqs = get_irq_pending_sources();
+
+    if pending_irqs & GPIO_PENDING_BIT_OFFSET != 0 {
+        handle_gpio_interrupt();
+        let source_el = get_exception_return_exception_level() >> 2;
+        println!("Source EL: {}", source_el);
+        println!("Current EL: {}", get_current_el());
+        println!("Return register address: {:#x}", get_elr_el1());
+    }
+
+    if let Some(handler_vec) = unsafe { INTERRUPT_HANDLERS.as_ref() } {
+        for handler in handler_vec {
+            if (pending_irqs & (1 << (handler.source.clone() as u32))) != 0 {
+                (handler.function)();
+                clear_interrupt_for_source(handler.source.clone());
+            }
+        }
+    }
+}
+
+#[no_mangle]
+unsafe extern "C" fn rust_synchronous_interrupt_no_el_change() {
+    daif::mask_all();
+
+    let source_el = get_exception_return_exception_level() >> 2;
+    println!("--------Sync Exception in EL{}--------", source_el);
+    println!("No EL change");
+    println!("Current EL: {}", get_current_el());
+    println!("{:?}", EsrElX::from(get_esr_el1()));
+    println!("Return register address: {:#x}", get_elr_el1());
+    println!("-------------------------------------");
+}
+
+/// Synchronous Exception Handler
+///
+/// Lower Exception level, where the implemented level
+/// immediately lower than the target level is using
+/// AArch64.
+#[no_mangle]
+unsafe extern "C" fn rust_synchronous_interrupt_imm_lower_aarch64() {
+    daif::mask_all();
+
+    let source_el = get_exception_return_exception_level() >> 2;
+    println!("--------Sync Exception in EL{}--------", source_el);
+    println!("Exception escalated to EL {}", get_current_el());
+    println!("Current EL: {}", get_current_el());
+    let esr = EsrElX::from(get_esr_el1());
+    println!("{:?}", EsrElX::from(esr));
+    println!("Return register address: {:#x}", get_elr_el1());
+
+    match esr.ec {
+        0b100100 => {
+            println!("Cause: Data Abort from a lower Exception level");
+        }
+        _ => {}
+    }
+    println!("-------------------------------------");
+
+    set_return_to_kernel_main();
+}
+
+fn clear_interrupt_for_source(source: IRQSource) {
+    match source {
+        IRQSource::UartInt => clear_uart_interrupt_state(),
+        _ => {}
+    }
+}
+
+fn set_return_to_kernel_main() {
+    unsafe {
+        asm!("ldr x0, =kernel_main", "msr ELR_EL1, x0");
+        asm!("mov x0, #(0b0101)", "msr SPSR_EL1, x0");
+    }
+}
+
+fn get_exception_return_exception_level() -> u32 {
+    let spsr: u32;
+    unsafe {
+        asm!("mrs {0:x}, SPSR_EL1", out(reg) spsr);
+    }
+    spsr & 0b1111
+}
+
+/// Read the syndrome information that caused an exception
+///
+/// ESR = Exception Syndrome Register
+fn get_esr_el1() -> u32 {
+    let esr: u32;
+    unsafe {
+        asm!(
+            "mrs {esr:x}, ESR_EL1",
+            esr = out(reg) esr
+        );
+    }
+    esr
+}
+
+/// Read the return address
+///
+/// ELR = Exception Link Registers
+fn get_elr_el1() -> u32 {
+    let elr: u32;
+    unsafe {
+        asm!(
+            "mrs {esr:x}, ELR_EL1",
+            esr = out(reg) elr
+        );
+    }
+    elr
+}
+
+fn handle_gpio_interrupt() {
+    println!("Interrupt");
+    for i in 0..=53u32 {
+        let val = read_gpio_event_detect_status(i);
+
+        if val {
+            #[allow(clippy::single_match)]
+            match i {
+                26 => {
+                    println!("Button Pressed");
+                }
+                _ => {}
+            }
+            // Reset GPIO Interrupt handler by writing a 1
+            reset_gpio_event_detect_status(i);
+        }
+    }
+    unmask_irq();
+}
+
+/// Enables IRQ Source
+pub fn enable_irq_source(state: IRQSource) {
+    let nr = state as u32;
+    let register = ENABLE_IRQ_BASE + 4 * (nr / 32);
+    let register_offset = nr % 32;
+    let current = unsafe { read_address(register) };
+    let mask = 0b1 << register_offset;
+    let new_val = current | mask;
+    unsafe { write_address(register, new_val) };
+}
+
+/// Disable IRQ Source
+pub fn disable_irq_source(state: IRQSource) {
+    let nr = state as u32;
+    let register = DISABLE_IRQ_BASE + 4 * (nr / 32);
+    let register_offset = nr % 32;
+    let current = unsafe { read_address(register) };
+    let mask = 0b1 << register_offset;
+    let new_val = current | mask;
+    unsafe { write_address(register, new_val) };
+}
+
+/// Read current IRQ Source status
+pub fn read_irq_source_status(state: IRQSource) -> u32 {
+    let nr = state as u32;
+    let register = ENABLE_IRQ_BASE + 4 * (nr / 32);
+    let register_offset = nr % 32;
+    (unsafe { read_address(register) } >> register_offset) & 0b1
+}
+
+/// Status if a IRQ Source is pending
+pub fn is_irq_source_pending(state: IRQSource) -> bool {
+    let nr = state as u32;
+    let register = IRQ_PENDING_BASE + 4 * (nr / 32);
+    let register_offset = nr % 32;
+    ((unsafe { read_address(register) } >> register_offset) & 0b1) != 0
+}
+
+/// Status if a IRQ Source is pending
+pub fn get_irq_pending_sources() -> u64 {
+    let mut pending = unsafe { read_address(IRQ_PENDING_BASE + 4) as u64 } << 32;
+    pending |= unsafe { read_address(IRQ_PENDING_BASE) as u64 };
+    pending
+}
+
+pub mod daif {
+    use core::arch::asm;
+
+    #[inline(always)]
+    pub fn mask_all() {
+        unsafe { asm!("msr DAIFSet, #0xf", options(nomem, nostack)) }
+    }
+
+    #[inline(always)]
+    pub fn unmask_all() {
+        unsafe { asm!("msr DAIFClr, #0xf", options(nomem, nostack)) }
+    }
+
+    #[inline(always)]
+    pub fn mask_irq() {
+        unsafe { asm!("msr DAIFSet, #0x2", options(nomem, nostack)) }
+    }
+
+    #[inline(always)]
+    pub fn unmask_irq() {
+        unsafe { asm!("msr DAIFClr, #0x2", options(nomem, nostack)) }
+    }
+}
+
+pub fn initialize_interrupt_handler() {
+    unsafe { INTERRUPT_HANDLERS = Some(Vec::new()) };
+}
+
+pub fn register_interrupt_handler(source: IRQSource, function: fn()) {
+    if let Some(handler_vec) = unsafe { INTERRUPT_HANDLERS.as_mut() } {
+        handler_vec.push(InterruptHandlers { source, function });
+    }
+}

src/irq_interrupt.rs

@@ -1,13 +1,10 @@
-use core::{
+use core::arch::asm;
-    arch::asm,
-    sync::atomic::{compiler_fence, Ordering},
-};
 
 use crate::{
+    get_current_el,
+    irq_interrupt::daif::unmask_irq,
     mmio_read, mmio_write,
-    peripherals::gpio::{blink_gpio, SpecificGpio},
+    peripherals::gpio::{read_gpio_event_detect_status, reset_gpio_event_detect_status},
-    print,
-    timer::sleep_s,
 };
 
 const INTERRUPT_BASE: u32 = 0x3F00_B000;
@@ -15,9 +12,6 @@ const IRQ_PENDING_BASE: u32 = INTERRUPT_BASE + 0x204;
 const ENABLE_IRQ_BASE: u32 = INTERRUPT_BASE + 0x210;
 const DISABLE_IRQ_BASE: u32 = INTERRUPT_BASE + 0x21C;
 
-// GPIO
-const GPEDS_BASE: u32 = 0x3F20_0040;
-
 #[repr(u32)]
 pub enum IRQState {
     AuxInt = 29,
@@ -35,43 +29,119 @@ pub enum IRQState {
     UartInt = 57,
 }
 
-#[no_mangle]
+/// Representation of the ESR_ELx registers
-unsafe extern "C" fn irq_handler() {
+///
-    handle_gpio_interrupt();
+///  Reference: D1.10.4
+#[derive(Debug, Clone, Copy)]
+#[allow(dead_code)]
+struct EsrElX {
+    ec: u32,
+    il: u32,
+    iss: u32,
 }
 
-#[no_mangle]
+impl From<u32> for EsrElX {
-unsafe extern "C" fn synchronous_interrupt() {
+    fn from(value: u32) -> Self {
-    loop {
+        Self {
-        println!("Sync Exception");
+            ec: value >> 26,
-        blink_gpio(SpecificGpio::OnboardLed as u8, 100);
+            il: (value >> 25) & 0b1,
-        esr_uart_dump();
+            iss: value & 0x1FFFFFF,
-        sleep_s(200);
+        }
     }
 }
 
-fn esr_uart_dump() {
+#[no_mangle]
+unsafe extern "C" fn rust_irq_handler() {
+    daif::mask_all();
+    handle_gpio_interrupt();
+    let source_el = get_exception_return_exception_level() >> 2;
+    println!("Source EL: {}", source_el);
+    println!("Current EL: {}", get_current_el());
+    println!("Return register address: {:#x}", get_elr_el1());
+}
+
+#[no_mangle]
+unsafe extern "C" fn rust_synchronous_interrupt_no_el_change() {
+    daif::mask_all();
+
+    let source_el = get_exception_return_exception_level() >> 2;
+    println!("--------Sync Exception in EL{}--------", source_el);
+    println!("No EL change");
+    println!("Current EL: {}", get_current_el());
+    println!("{:?}", EsrElX::from(get_esr_el1()));
+    println!("Return register address: {:#x}", get_elr_el1());
+    println!("-------------------------------------");
+}
+
+/// Synchronous Exception Handler
+///
+/// Lower Exception level, where the implemented level
+/// immediately lower than the target level is using
+/// AArch64.
+#[no_mangle]
+unsafe extern "C" fn rust_synchronous_interrupt_imm_lower_aarch64() {
+    daif::mask_all();
+
+    let source_el = get_exception_return_exception_level() >> 2;
+    println!("--------Sync Exception in EL{}--------", source_el);
+    println!("Exception escalated to EL {}", get_current_el());
+    println!("Current EL: {}", get_current_el());
+    let esr = EsrElX::from(get_esr_el1());
+    println!("{:?}", EsrElX::from(esr));
+    println!("Return register address: {:#x}", get_elr_el1());
+
+    match esr.ec {
+        0b100100 => {
+            println!("Cause: Data Abort from a lower Exception level");
+        }
+        _ => {}
+    }
+    println!("-------------------------------------");
+
+    set_return_to_kernel_main();
+}
+
+fn set_return_to_kernel_main() {
+    unsafe {
+        asm!("ldr x0, =kernel_main", "msr ELR_EL1, x0");
+        asm!("mov x0, #(0b0101)", "msr SPSR_EL1, x0");
+    }
+}
+
+fn get_exception_return_exception_level() -> u32 {
+    let spsr: u32;
+    unsafe {
+        asm!("mrs {0:x}, SPSR_EL1", out(reg) spsr);
+    }
+    spsr & 0b1111
+}
+
+/// Read the syndrome information that caused an exception
+///
+/// ESR = Exception Syndrome Register
+fn get_esr_el1() -> u32 {
     let esr: u32;
     unsafe {
         asm!(
-            "mrs {esr}, ESR_EL1",
+            "mrs {esr:x}, ESR_EL1",
             esr = out(reg) esr
         );
     }
-    for i in (0..32).rev() {
+    esr
-        if ((esr >> i) & 1) == 0 {
+}
-            print!("0");
-        } else {
-            print!("1");
-        }
-        if i % 4 == 0 && i > 0 {
-            print!("_");
-        }
 
-        if i == 26 || i == 25 || i == 0 {
+/// Read the return address
-            print!("\n\r");
+///
-        }
+/// ELR = Exception Link Registers
+fn get_elr_el1() -> u32 {
+    let elr: u32;
+    unsafe {
+        asm!(
+            "mrs {esr:x}, ELR_EL1",
+            esr = out(reg) elr
+        );
     }
+    elr
 }
 
 fn handle_gpio_interrupt() {
@@ -80,33 +150,18 @@ fn handle_gpio_interrupt() {
         let val = read_gpio_event_detect_status(i);
 
         if val {
+            #[allow(clippy::single_match)]
             match i {
-                26 => print!("Button Pressed"),
+                26 => {
+                    println!("Button Pressed");
+                }
                 _ => {}
             }
             // Reset GPIO Interrupt handler by writing a 1
             reset_gpio_event_detect_status(i);
         }
     }
-    enable_irq();
+    unmask_irq();
-}
-
-/// Get current interrupt status of a GPIO pin
-pub fn read_gpio_event_detect_status(id: u32) -> bool {
-    let register = GPEDS_BASE + (id / 32) * 4;
-    let register_offset = id % 32;
-
-    let val = mmio_read(register) >> register_offset;
-    (val & 0b1) != 0
-}
-
-/// Resets current interrupt status of a GPIO pin
-pub fn reset_gpio_event_detect_status(id: u32) {
-    let register = GPEDS_BASE + (id / 32) * 4;
-    let register_offset = id % 32;
-
-    mmio_write(register, 0b1 << register_offset);
-    compiler_fence(Ordering::SeqCst);
 }
 
 /// Enables IRQ Source
@@ -147,16 +202,26 @@ pub fn read_irq_pending(state: IRQState) -> bool {
     ((mmio_read(register) >> register_offset) & 0b1) != 0
 }
 
-/// Clears the IRQ DAIF Mask
+pub mod daif {
-///
+    use core::arch::asm;
-/// Enables IRQ interrupts
-pub fn enable_irq() {
-    unsafe { asm!("msr DAIFClr, #0x2") }
-}
 
-/// Clears the IRQ DAIF Mask
+    #[inline(always)]
-///
+    pub fn mask_all() {
-/// Disable IRQ interrupts
+        unsafe { asm!("msr DAIFSet, #0xf", options(nomem, nostack)) }
-pub fn disable_irq() {
+    }
-    unsafe { asm!("msr DAIFSet, #0x2") }
+
+    #[inline(always)]
+    pub fn unmask_all() {
+        unsafe { asm!("msr DAIFClr, #0xf", options(nomem, nostack)) }
+    }
+
+    #[inline(always)]
+    pub fn mask_irq() {
+        unsafe { asm!("msr DAIFSet, #0x2", options(nomem, nostack)) }
+    }
+
+    #[inline(always)]
+    pub fn unmask_irq() {
+        unsafe { asm!("msr DAIFClr, #0x2", options(nomem, nostack)) }
+    }
 }

src/lib.rs

@@ -1,12 +1,21 @@
 #![no_std]
+#![allow(clippy::missing_safety_doc)]
 
+extern crate alloc;
+
+use alloc::boxed::Box;
 use core::{
+    arch::asm,
     panic::PanicInfo,
     ptr::{read_volatile, write_volatile},
 };
 
 use heap::Heap;
 
+use crate::{interrupt_handlers::initialize_interrupt_handler, logger::DefaultLogger};
+
+static PERIPHERAL_BASE: u32 = 0x3F00_0000;
+
 unsafe extern "C" {
     unsafe static mut __heap_start: u8;
     unsafe static mut __heap_end: u8;
@@ -19,7 +28,8 @@ pub unsafe fn init_heap() {
     let start = core::ptr::addr_of_mut!(__heap_start) as usize;
     let end = core::ptr::addr_of_mut!(__heap_end) as usize;
 
-    GLOBAL_ALLOCATOR.init(start, end);
+    let heap = core::ptr::addr_of_mut!(GLOBAL_ALLOCATOR);
+    (*heap).init(start, end);
 }
 
 #[panic_handler]
@@ -29,35 +39,39 @@ fn panic(_panic: &PanicInfo) -> ! {
     }
 }
 
-#[macro_export]
-macro_rules! print {
-    () => {};
-    ($($arg:tt)*) => {
-        $crate::peripherals::uart::_print(format_args!($($arg)*))
-    };
-}
-
-#[macro_export]
-macro_rules! println {
-    () => {};
-    ($($arg:tt)*) => {
-        print!($($arg)*);
-        print!("\r\n");
-    };
-}
-
 pub mod peripherals;
 
 pub mod configuration;
 pub mod framebuffer;
-pub mod irq_interrupt;
+pub mod interrupt_handlers;
+pub mod logger;
 pub mod mailbox;
+pub mod power_management;
 pub mod timer;
 
-pub fn mmio_read(address: u32) -> u32 {
+#[inline(always)]
+pub unsafe fn read_address(address: u32) -> u32 {
     unsafe { read_volatile(address as *const u32) }
 }
 
-pub fn mmio_write(address: u32, data: u32) {
+#[inline(always)]
+pub unsafe fn write_address(address: u32, data: u32) {
     unsafe { write_volatile(address as *mut u32, data) }
 }
+
+pub fn get_current_el() -> u64 {
+    let el: u64;
+    unsafe {
+        asm!(
+            "mrs {el}, CurrentEL",
+            el = out(reg) el,
+            options(nomem, nostack, preserves_flags)
+        );
+    }
+    el >> 2
+}
+
+pub fn initialize_kernel() {
+    logger::set_logger(Box::new(DefaultLogger));
+    initialize_interrupt_handler();
+}

src/logger.rs

@@ -0,0 +1,47 @@
+use core::fmt::Write;
+
+use alloc::{boxed::Box, fmt};
+
+use crate::peripherals::uart;
+
+static mut LOGGER: Option<Box<dyn Logger>> = None;
+
+pub trait Logger: Write + Sync {
+    fn flush(&mut self);
+}
+
+pub struct DefaultLogger;
+
+impl Logger for DefaultLogger {
+    fn flush(&mut self) {}
+}
+
+impl Write for DefaultLogger {
+    fn write_str(&mut self, s: &str) -> core::fmt::Result {
+        uart::Uart.write_str(s)
+    }
+}
+
+#[macro_export]
+macro_rules! log {
+    () => {};
+    ($($arg:tt)*) => {
+        $crate::logger::log(format_args!($($arg)*))
+    };
+}
+
+pub fn log(args: fmt::Arguments) {
+    unsafe {
+        if let Some(logger) = LOGGER.as_mut() {
+            logger.write_str("\n").unwrap();
+            logger.write_fmt(args).unwrap();
+            logger.flush();
+        }
+    }
+}
+
+pub fn set_logger(logger: Box<dyn Logger>) {
+    unsafe {
+        LOGGER = Some(logger);
+    }
+}

src/mailbox.rs

@@ -1,9 +1,10 @@
-use crate::{mmio_read, mmio_write};
+use crate::{read_address, write_address};
+use nova_error::NovaError;
 
 const MBOX_BASE: u32 = 0x3F00_0000 + 0xB880;
 
 // MB0
-const MBOX_READ: u32 = MBOX_BASE + 0x00;
+const MBOX_READ: u32 = MBOX_BASE;
 const MBOX_STATUS: u32 = MBOX_BASE + 0x18;
 
 // MB1
@@ -26,10 +27,10 @@ macro_rules! max {
 #[macro_export]
 macro_rules! mailbox_command {
     ($name:ident, $tag:expr, $request_len:expr,$response_len:expr) => {
-        /// More information at: https://github.com/raspberrypi/firmware/wiki/Mailbox-property-interface
+        /// More information at: <https://github.com/raspberrypi/firmware/wiki/Mailbox-property-interface>
         pub fn $name(
             request_data: [u32; $request_len / 4],
-        ) -> Result<[u32; $response_len / 4], NovaError::NovaError> {
+        ) -> Result<[u32; $response_len / 4], NovaError> {
             let mut mailbox =
                 [0u32; (HEADER_LENGTH + max!($request_len, $response_len) + FOOTER_LENGTH) / 4];
             mailbox[0] = (HEADER_LENGTH + max!($request_len, $response_len) + FOOTER_LENGTH) as u32; // Total length in Bytes
@@ -48,7 +49,7 @@ macro_rules! mailbox_command {
             let _ = read_mailbox(8);
 
             if mailbox[1] == 0 {
-                return Err(NovaError::NovaError::Mailbox);
+                return Err(NovaError::Mailbox);
             }
 
             let mut out = [0u32; $response_len / 4];
@@ -58,16 +59,16 @@ macro_rules! mailbox_command {
     };
 }
 
-mailbox_command!(mb_read_soc_temp, 0x0003_0006, 4, 8);
+mailbox_command!(read_soc_temp, 0x0003_0006, 4, 8);
 
 // Framebuffer
-mailbox_command!(mb_get_display_resolution, 0x0004_0003, 0, 8);
+mailbox_command!(get_display_resolution, 0x0004_0003, 0, 8);
 
 pub fn read_mailbox(channel: u32) -> u32 {
     // Wait until mailbox is not empty
     loop {
-        while mmio_read(MBOX_STATUS) & MAIL_EMPTY != 0 {}
+        while unsafe { read_address(MBOX_STATUS) } & MAIL_EMPTY != 0 {}
-        let mut data = mmio_read(MBOX_READ);
+        let mut data = unsafe { read_address(MBOX_READ) };
         let read_channel = data & 0xF;
 
         data >>= 4;
@@ -79,6 +80,6 @@ pub fn read_mailbox(channel: u32) -> u32 {
 }
 
 pub fn write_mailbox(channel: u32, data: u32) {
-    while mmio_read(MBOX_STATUS) & MAIL_FULL != 0 {}
+    while unsafe { read_address(MBOX_STATUS) } & MAIL_FULL != 0 {}
-    mmio_write(MBOX_WRITE, (data & !0xF) | (channel & 0xF));
+    unsafe { write_address(MBOX_WRITE, (data & !0xF) | (channel & 0xF)) };
 }

src/main.rs

@@ -2,6 +2,7 @@
 #![no_std]
 #![feature(asm_experimental_arch)]
 #![allow(static_mut_refs)]
+#![allow(clippy::missing_safety_doc)]
 use core::{
     arch::{asm, global_asm},
     ptr::write_volatile,
@@ -9,11 +10,12 @@ use core::{
 
 extern crate alloc;
 
+use alloc::boxed::Box;
 use nova::{
     framebuffer::{FrameBuffer, BLUE, GREEN, RED},
-    init_heap,
+    get_current_el, init_heap,
-    irq_interrupt::enable_irq_source,
+    interrupt_handlers::{daif, enable_irq_source, IRQSource},
-    mailbox::mb_read_soc_temp,
+    log, mailbox,
     peripherals::{
         gpio::{
             blink_gpio, gpio_pull_up, set_falling_edge_detect, set_gpio_function, GPIOFunction,
@@ -21,15 +23,14 @@ use nova::{
         },
         uart::uart_init,
     },
-    print, println,
+    println,
-    timer::{delay_nops, sleep_us},
-    GLOBAL_ALLOCATOR,
 };
 
 global_asm!(include_str!("vector.S"));
 
 extern "C" {
     fn el2_to_el1();
+    fn el1_to_el0();
     static mut __bss_start: u32;
     static mut __bss_end: u32;
 }
@@ -56,12 +57,11 @@ pub extern "C" fn main() -> ! {
     // Set ACT Led to Outout
     let _ = set_gpio_function(21, GPIOFunction::Output);
 
-    // Delay so clock speed can stabilize
-    delay_nops(50000);
     println!("Hello World!");
+    println!("Exception level: {}", get_current_el());
 
     unsafe {
-        asm!("mrs x0, SCTLR_EL1");
+        asm!("mrs x0, SCTLR_EL1",);
         el2_to_el1();
     }
 
@@ -79,22 +79,33 @@ unsafe fn zero_bss() {
 
 #[no_mangle]
 pub extern "C" fn kernel_main() -> ! {
-    println!("EL: {}", get_current_el());
+    nova::initialize_kernel();
+    println!("Kernel Main");
+    println!("Exception Level: {}", get_current_el());
+    daif::unmask_all();
 
     unsafe {
         init_heap();
-        heap_test();
+        el1_to_el0();
     };
 
-    sleep_us(500_000);
+    #[allow(clippy::empty_loop)]
+    loop {}
+}
+
+#[no_mangle]
+pub extern "C" fn el0() -> ! {
+    println!("Jumped into EL0");
 
     // Set GPIO 26 to Input
-    enable_irq_source(nova::irq_interrupt::IRQState::GpioInt0); //26 is on the first GPIO bank
+    enable_irq_source(IRQSource::GpioInt0); //26 is on the first GPIO bank
     let _ = set_gpio_function(26, GPIOFunction::Input);
     gpio_pull_up(26);
     set_falling_edge_detect(26, true);
 
-    let fb = FrameBuffer::new();
+    enable_irq_source(IRQSource::UartInt);
+
+    let fb = FrameBuffer::default();
 
     fb.draw_square(500, 500, 600, 700, RED);
     fb.draw_square_fill(800, 800, 900, 900, GREEN);
@@ -105,42 +116,23 @@ pub extern "C" fn kernel_main() -> ! {
     fb.draw_function(cos, 100, 101, RED);
 
     loop {
-        let temp = mb_read_soc_temp([0]).unwrap();
+        let temp = mailbox::read_soc_temp([0]).unwrap();
-        println!("{} °C", temp[1] / 1000);
+        log!("{} °C", temp[1] / 1000);
 
         blink_gpio(SpecificGpio::OnboardLed as u8, 500);
-    }
-}
 
-unsafe fn heap_test() {
+        let b = Box::new([1, 2, 3, 4]);
-    let a = GLOBAL_ALLOCATOR.malloc(32).unwrap();
+        log!("{:?}", b);
-    let b = GLOBAL_ALLOCATOR.malloc(64).unwrap();
+    }
-    let c = GLOBAL_ALLOCATOR.malloc(128).unwrap();
-    let _ = GLOBAL_ALLOCATOR.malloc(256).unwrap();
-    GLOBAL_ALLOCATOR.free(b).unwrap();
-    GLOBAL_ALLOCATOR.free(a).unwrap();
-    GLOBAL_ALLOCATOR.free(c).unwrap();
 }
 
 fn cos(x: u32) -> f64 {
     libm::cos(x as f64 * 0.1) * 20.0
 }
 
-fn get_current_el() -> u64 {
-    let el: u64;
-    unsafe {
-        asm!(
-            "mrs {el}, CurrentEL",
-            el = out(reg) el,
-            options(nomem, nostack, preserves_flags)
-        );
-    }
-    el >> 2
-}
-
 fn enable_uart() {
-    uart_init();
     // Set GPIO Pins to UART
     let _ = set_gpio_function(14, GPIOFunction::Alternative0);
     let _ = set_gpio_function(15, GPIOFunction::Alternative0);
+    uart_init();
 }

src/peripherals/gpio.rs

@@ -1,17 +1,19 @@
 use core::result::Result;
 use core::result::Result::Ok;
+use core::sync::atomic::{compiler_fence, Ordering};
 
 use crate::timer::{delay_nops, sleep_ms};
-use crate::{mmio_read, mmio_write};
+use crate::{read_address, write_address};
 
 const GPFSEL_BASE: u32 = 0x3F20_0000;
 const GPSET_BASE: u32 = 0x3F20_001C;
 const GPCLR_BASE: u32 = 0x3F20_0028;
 const GPLEV_BASE: u32 = 0x3F20_0034;
+const GPEDS_BASE: u32 = 0x3F20_0040;
+const GPFEN_BASE: u32 = 0x3F20_0058;
 const GPPUD: u32 = 0x3F20_0094;
 const GPPUDCLK_BASE: u32 = 0x3F20_0098;
 const GPREN_BASE: u32 = 0x3F20_004C;
-const GPFEN_BASE: u32 = 0x3F20_0058;
 
 #[repr(u8)]
 pub enum SpecificGpio {
@@ -35,26 +37,27 @@ pub fn set_gpio_function(gpio: u8, state: GPIOFunction) -> Result<(), &'static s
     let register_index = gpio / 10;
     let register_offset = (gpio % 10) * 3;
     let register_addr = GPFSEL_BASE + (register_index as u32 * 4);
-    let current = mmio_read(register_addr);
+    let current = unsafe { read_address(register_addr) };
 
     let mask = !(0b111 << register_offset);
     let cleared = current & mask;
 
     let new_val = cleared | ((state as u32) << register_offset);
 
-    mmio_write(register_addr, new_val);
+    unsafe { write_address(register_addr, new_val) };
     Ok(())
 }
 
 /// Set the GPIO to high
 ///
-/// Should be used when GPIO function is set to `OUTPUT` via `set_gpio_function`
+/// Should be used when GPIO function is set to `OUTPUT` via `set_gpio_function`,
+/// otherwise setting is ignored
 pub fn gpio_high(gpio: u8) -> Result<(), &'static str> {
     let register_index = gpio / 32;
     let register_offset = gpio % 32;
     let register_addr = GPSET_BASE + (register_index as u32 * 4);
 
-    mmio_write(register_addr, 1 << register_offset);
+    unsafe { write_address(register_addr, 1 << register_offset) };
     Ok(())
 }
 
@@ -66,7 +69,7 @@ pub fn gpio_low(gpio: u8) -> Result<(), &'static str> {
     let register_offset = gpio % 32;
     let register_addr = GPCLR_BASE + (register_index as u32 * 4);
 
-    mmio_write(register_addr, 1 << register_offset);
+    unsafe { write_address(register_addr, 1 << register_offset) };
     Ok(())
 }
 
@@ -76,7 +79,7 @@ pub fn gpio_get_state(gpio: u8) -> u8 {
     let register_offset = gpio % 32;
     let register_addr = GPLEV_BASE + (register_index as u32 * 4);
 
-    let state = mmio_read(register_addr);
+    let state = unsafe { read_address(register_addr) };
     ((state >> register_offset) & 0b1) as u8
 }
 
@@ -100,40 +103,40 @@ fn gpio_pull_up_down(gpio: u8, val: u32) {
     let register_offset = gpio % 32;
 
     // 1. Write Pull up
-    mmio_write(GPPUD, val);
+    unsafe { write_address(GPPUD, val) };
 
     // 2. Delay 150 cycles
     delay_nops(150);
 
     // 3. Write to clock
     let new_val = 0b1 << register_offset;
-    mmio_write(register_addr, new_val);
+    unsafe { write_address(register_addr, new_val) };
 
     // 4. Delay 150 cycles
     delay_nops(150);
 
     // 5. reset GPPUD
-    mmio_write(GPPUD, 0);
+    unsafe { write_address(GPPUD, 0) };
 
     // 6. reset clock
-    mmio_write(register_addr, 0);
+    unsafe { write_address(register_addr, 0) };
 }
 
-/// Get the current status if falling edge detection is set
+/// Get the current status of the falling edge detection
 pub fn read_falling_edge_detect(gpio: u8) -> bool {
     let register_addr = GPFEN_BASE + 4 * (gpio as u32 / 32);
     let register_offset = gpio % 32;
 
-    let current = mmio_read(register_addr);
+    let current = unsafe { read_address(register_addr) };
     ((current >> register_offset) & 0b1) != 0
 }
 
-/// Get the current status if falling edge detection is set
+/// Get the current status of the rising edge detection
 pub fn read_rising_edge_detect(gpio: u8) -> bool {
     let register_addr = GPREN_BASE + 4 * (gpio as u32 / 32);
     let register_offset = gpio % 32;
 
-    let current = mmio_read(register_addr);
+    let current = unsafe { read_address(register_addr) };
     ((current >> register_offset) & 0b1) != 0
 }
 
@@ -142,7 +145,7 @@ pub fn set_falling_edge_detect(gpio: u8, enable: bool) {
     let register_addr = GPFEN_BASE + 4 * (gpio as u32 / 32);
     let register_offset = gpio % 32;
 
-    let current = mmio_read(register_addr);
+    let current = unsafe { read_address(register_addr) };
     let mask = 0b1 << register_offset;
     let new_val = if enable {
         current | mask
@@ -150,7 +153,7 @@ pub fn set_falling_edge_detect(gpio: u8, enable: bool) {
         current & !mask
     };
 
-    mmio_write(register_addr, new_val);
+    unsafe { write_address(register_addr, new_val) };
 }
 
 /// Enables rising edge detection
@@ -158,7 +161,7 @@ pub fn set_rising_edge_detect(gpio: u8, enable: bool) {
     let register_addr = GPREN_BASE + 4 * (gpio as u32 / 32);
     let register_offset = gpio % 32;
 
-    let current = mmio_read(register_addr);
+    let current = unsafe { read_address(register_addr) };
 
     let mask = 0b1 << register_offset;
     let new_val = if enable {
@@ -167,9 +170,32 @@ pub fn set_rising_edge_detect(gpio: u8, enable: bool) {
         current & !mask
     };
 
-    mmio_write(register_addr, new_val);
+    unsafe { write_address(register_addr, new_val) };
 }
 
+/// Returns with the interrupt status of an GPIO.
+///
+/// GPEDS register is used to record level and edge events on the GPIO pins.
+/// When an event is triggered by the GPIO, the corresponding bit will be set to 1.
+pub fn read_gpio_event_detect_status(id: u32) -> bool {
+    let register = GPEDS_BASE + (id / 32) * 4;
+    let register_offset = id % 32;
+
+    let val = unsafe { read_address(register) } >> register_offset;
+    (val & 0b1) != 0
+}
+
+/// Resets current interrupt status of a GPIO pin.
+pub fn reset_gpio_event_detect_status(id: u32) {
+    let register = GPEDS_BASE + (id / 32) * 4;
+    let register_offset = id % 32;
+
+    unsafe { write_address(register, 0b1 << register_offset) };
+    compiler_fence(Ordering::SeqCst);
+}
+
+// TODO: GPHEN,GPLEN,GPAREN,GPAFEN
+
 pub fn blink_gpio(gpio: u8, duration_ms: u64) {
     let _ = gpio_high(gpio);
 

src/peripherals/uart.rs

@@ -3,7 +3,7 @@ use core::{
     fmt::{self, Write},
 };
 
-use crate::{mmio_read, mmio_write};
+use crate::{println, read_address, write_address};
 
 const BAUD: u32 = 115200;
 const UART_CLK: u32 = 48_000_000;
@@ -18,33 +18,53 @@ const UART0_FBRD: u32 = 0x3F20_1028;
 
 const UART0_CR: u32 = 0x3F20_1030;
 const UART0_CR_UARTEN: u32 = 1 << 0;
+
 const UART0_CR_TXE: u32 = 1 << 8;
+const UART0_CR_RXE: u32 = 1 << 9;
 
 const UART0_LCRH: u32 = 0x3F20_102C;
 const UART0_LCRH_FEN: u32 = 1 << 4;
 
+const UART0_IMSC: u32 = 0x3F20_1038;
+const UART0_IMSC_RXIM: u32 = 1 << 4;
+
+const UART0_ICR: u32 = 0x3F20_1044;
+
 pub struct Uart;
 
 impl Write for Uart {
     fn write_str(&mut self, s: &str) -> core::fmt::Result {
         for byte in s.bytes() {
-            while (mmio_read(UART0_FR) & UART0_FR_TXFF) != 0 {
+            while (unsafe { read_address(UART0_FR) } & UART0_FR_TXFF) != 0 {
                 unsafe { asm!("nop") }
             }
-            mmio_write(UART0_DR, byte as u32);
+            unsafe { write_address(UART0_DR, byte as u32) };
         }
         // wait till uart is not busy anymore
-        while ((mmio_read(UART0_FR) >> 3) & 0b1) != 0 {}
+        while ((unsafe { read_address(UART0_FR) } >> 3) & 0b1) != 0 {}
         Ok(())
     }
 }
 
-pub fn _print(args: fmt::Arguments) {
+#[macro_export]
-    let _ = Uart.write_fmt(args);
+macro_rules! print {
+    () => {};
+    ($($arg:tt)*) => {
+        $crate::peripherals::uart::_print(format_args!($($arg)*))
+    };
 }
 
-pub fn _print_str(st: &str) {
+#[macro_export]
-    let _ = Uart.write_str(st);
+macro_rules! println {
+    () => {};
+    ($($arg:tt)*) => {
+        $crate::print!($($arg)*);
+        $crate::print!("\r\n");
+    };
+}
+
+pub fn _print(args: fmt::Arguments) {
+    let _ = Uart.write_fmt(args);
 }
 
 /// Initialize UART peripheral
@@ -55,23 +75,26 @@ pub fn uart_init() {
     let fbrd = baud_div_times_64 % 64;
 
     uart_enable(false);
-    uart_fifo_enable(false);
+    uart_fifo_enable(true);
 
-    mmio_write(UART0_IBRD, ibrd);
+    unsafe {
-    mmio_write(UART0_FBRD, fbrd);
+        write_address(UART0_IBRD, ibrd);
+        write_address(UART0_FBRD, fbrd);
+    }
 
+    uart_enable_rx_interrupt();
     uart_set_lcrh(0b11, true);
 
-    // Enable transmit and uart
+    // Enable transmit, receive and uart
-    let mut cr = mmio_read(UART0_CR);
+    let mut cr = unsafe { read_address(UART0_CR) };
-    cr |= UART0_CR_UARTEN | UART0_CR_TXE;
+    cr |= UART0_CR_UARTEN | UART0_CR_TXE | UART0_CR_RXE;
 
-    mmio_write(UART0_CR, cr);
+    unsafe { write_address(UART0_CR, cr) };
 }
 
 /// Enable UARTEN
 fn uart_enable(enable: bool) {
-    let mut cr = mmio_read(UART0_CR);
+    let mut cr = unsafe { read_address(UART0_CR) };
 
     if enable {
         cr |= UART0_CR_UARTEN;
@@ -79,12 +102,12 @@ fn uart_enable(enable: bool) {
         cr &= !UART0_CR_UARTEN;
     }
 
-    mmio_write(UART0_CR, cr);
+    unsafe { write_address(UART0_CR, cr) };
 }
 
 /// Enable UART FIFO
 fn uart_fifo_enable(enable: bool) {
-    let mut lcrh = mmio_read(UART0_LCRH);
+    let mut lcrh = unsafe { read_address(UART0_LCRH) };
 
     if enable {
         lcrh |= UART0_LCRH_FEN;
@@ -92,7 +115,11 @@ fn uart_fifo_enable(enable: bool) {
         lcrh &= !UART0_LCRH_FEN;
     }
 
-    mmio_write(UART0_LCRH, lcrh);
+    unsafe { write_address(UART0_LCRH, lcrh) };
+}
+
+fn uart_enable_rx_interrupt() {
+    unsafe { write_address(UART0_IMSC, UART0_IMSC_RXIM) };
 }
 
 /// Set UART word length and set FIFO status
@@ -101,5 +128,15 @@ fn uart_set_lcrh(wlen: u32, enable_fifo: bool) {
     if enable_fifo {
         value |= UART0_LCRH_FEN;
     }
-    mmio_write(UART0_LCRH, value);
+    unsafe { write_address(UART0_LCRH, value) };
+}
+
+pub fn read_uart_data() -> char {
+    (unsafe { read_address(UART0_DR) } & 0xFF) as u8 as char
+}
+
+pub fn clear_uart_interrupt_state() {
+    unsafe {
+        write_address(UART0_ICR, 1 << 4);
+    }
 }

src/power_management.rs

@@ -0,0 +1,27 @@
+use core::ptr::{read_volatile, write_volatile};
+
+use crate::PERIPHERAL_BASE;
+
+/// Power Management Base
+static PM_BASE: u32 = PERIPHERAL_BASE + 0x10_0000;
+static PM_RSTC: u32 = PM_BASE + 0x1c;
+static PM_WDOG: u32 = PM_BASE + 0x24;
+
+static PM_PASSWORD: u32 = 0x5a000000;
+static PM_WDOG_TIMER_MASK: u32 = 0x000fffff;
+static PM_RSTC_WRCFG_CLR: u32 = 0xffffffcf;
+static PM_RSTC_WRCFG_FULL_RESET: u32 = 0x00000020;
+
+pub fn reboot_system() {
+    unsafe {
+        let pm_rstc_val = read_volatile(PM_RSTC as *mut u32);
+        // (31:16) bits -> password
+        // (11:0) bits -> value
+        write_volatile(PM_WDOG as *mut u32, PM_PASSWORD | (1 & PM_WDOG_TIMER_MASK));
+        write_volatile(
+            PM_RSTC as *mut u32,
+            PM_PASSWORD | (pm_rstc_val & PM_RSTC_WRCFG_CLR) | PM_RSTC_WRCFG_FULL_RESET,
+        );
+    }
+    loop {}
+}

src/terminal.rs

@@ -0,0 +1,53 @@
+use core::fmt::Write;
+
+use alloc::string::String;
+use nova::{
+    interrupt_handlers::register_interrupt_handler, logger::Logger,
+    peripherals::uart::read_uart_data, print, println,
+};
+
+pub struct Terminal {
+    buffer: String,
+    input: String,
+}
+
+impl Terminal {
+    pub fn new() -> Self {
+        Self {
+            buffer: String::new(),
+            input: String::new(),
+        }
+    }
+
+    fn flush(&mut self) {
+        println!("{}", self.buffer);
+        print!("> {}", self.input);
+        self.buffer.clear();
+    }
+}
+
+impl Write for Terminal {
+    fn write_str(&mut self, s: &str) -> core::fmt::Result {
+        self.buffer.push_str(s);
+        Ok(())
+    }
+}
+
+impl Logger for Terminal {
+    fn flush(&mut self) {
+        println!("{}", self.buffer);
+        print!("> {}", self.input);
+        self.buffer.clear();
+    }
+}
+
+fn terminal_uart_rx_interrupt_handler() {
+    print!("{}", read_uart_data());
+}
+
+pub fn register_terminal_interrupt_handler() {
+    register_interrupt_handler(
+        nova::interrupt_handlers::IRQSource::UartInt,
+        terminal_uart_rx_interrupt_handler,
+    );
+}

src/vector.S

@@ -14,11 +14,14 @@ vector_table:
     ventry .
     ventry .
 
-    ventry synchronous_interrupt      // Synchronous Exception  0x200
+    ventry synchronous_interrupt_no_el_change  // Synchronous Exception  0x200
     ventry irq_handler                // IRQ(Interrupt Request) 0x280
     ventry .
     ventry .
 
+    ventry synchronous_interrupt_imm_lower_aarch64
+    ventry irq_handler
+
 
 .align 4
 .global el2_to_el1
@@ -31,7 +34,7 @@ el2_to_el1:
     mov x0, #(0b0101)
     msr SPSR_EL2, x0
 
-    // Set return address to ELR_EL2
+    // Set return address to kernel_main
     ldr x0, =kernel_main
     msr ELR_EL2, x0
 
@@ -53,7 +56,121 @@ el2_to_el1:
     orr x0,x0, x1
     msr CPACR_EL1,x0
 
-    isb
-
     // Return to EL1
     eret
+
+.align 4
+.global el1_to_el0
+el1_to_el0:
+
+    // Set SPSR_EL1: return to EL0t
+    mov x0, #(0b0000)
+    msr SPSR_EL1, x0
+
+    // Set return address to el0
+    ldr x0, =el0
+    msr ELR_EL1, x0
+
+    // Set SP_EL1 to stack base
+    ldr x0, =__stack_end_el0
+    msr SP_EL0, x0
+
+    // Return to EL0
+    eret
+
+
+.align 4
+irq_handler:
+    sub sp, sp, #176
+    stp x0, x1, [sp, #0]
+    stp x2, x3, [sp, #16]
+    stp x4, x5, [sp, #32]
+    stp x6, x7, [sp, #48]
+    stp x8, x9, [sp, #64]
+    stp x10, x11, [sp, #80]
+    stp x12, x13, [sp, #96]
+    stp x14, x15, [sp, #112]
+    stp x16, x17, [sp, #128]
+    stp x18, x29, [sp, #144]
+    stp x30, xzr, [sp, #160]
+
+    bl rust_irq_handler
+
+    ldp x0, x1, [sp, #0]
+    ldp x2, x3, [sp, #16]
+    ldp x4, x5, [sp, #32]
+    ldp x6, x7, [sp, #48]
+    ldp x8, x9, [sp, #64]
+    ldp x10, x11, [sp, #80]
+    ldp x12, x13, [sp, #96]
+    ldp x14, x15, [sp, #112]
+    ldp x16, x17, [sp, #128]
+    ldp x18, x29, [sp, #144]
+    ldp x30, xzr, [sp, #160]
+    add sp, sp, #176
+
+    eret
+
+.align 4
+synchronous_interrupt_imm_lower_aarch64:
+    sub sp, sp, #176
+    stp x0, x1, [sp, #0]
+    stp x2, x3, [sp, #16]
+    stp x4, x5, [sp, #32]
+    stp x6, x7, [sp, #48]
+    stp x8, x9, [sp, #64]
+    stp x10, x11, [sp, #80]
+    stp x12, x13, [sp, #96]
+    stp x14, x15, [sp, #112]
+    stp x16, x17, [sp, #128]
+    stp x18, x29, [sp, #144]
+    stp x30, xzr, [sp, #160]
+
+    bl rust_synchronous_interrupt_imm_lower_aarch64
+
+    ldp x0, x1, [sp, #0]
+    ldp x2, x3, [sp, #16]
+    ldp x4, x5, [sp, #32]
+    ldp x6, x7, [sp, #48]
+    ldp x8, x9, [sp, #64]
+    ldp x10, x11, [sp, #80]
+    ldp x12, x13, [sp, #96]
+    ldp x14, x15, [sp, #112]
+    ldp x16, x17, [sp, #128]
+    ldp x18, x29, [sp, #144]
+    ldp x30, xzr, [sp, #160]
+    add sp, sp, #176
+
+    eret
+
+.align 4
+synchronous_interrupt_no_el_change:
+    sub sp, sp, #176
+    stp x0, x1, [sp, #0]
+    stp x2, x3, [sp, #16]
+    stp x4, x5, [sp, #32]
+    stp x6, x7, [sp, #48]
+    stp x8, x9, [sp, #64]
+    stp x10, x11, [sp, #80]
+    stp x12, x13, [sp, #96]
+    stp x14, x15, [sp, #112]
+    stp x16, x17, [sp, #128]
+    stp x18, x29, [sp, #144]
+    stp x30, xzr, [sp, #160]
+
+    bl rust_synchronous_interrupt_no_el_change
+
+    ldp x0, x1, [sp, #0]
+    ldp x2, x3, [sp, #16]
+    ldp x4, x5, [sp, #32]
+    ldp x6, x7, [sp, #48]
+    ldp x8, x9, [sp, #64]
+    ldp x10, x11, [sp, #80]
+    ldp x12, x13, [sp, #96]
+    ldp x14, x15, [sp, #112]
+    ldp x16, x17, [sp, #128]
+    ldp x18, x29, [sp, #144]
+    ldp x30, xzr, [sp, #160]
+    add sp, sp, #176
+
+    eret

tools/deply_to_hw.sh

@@ -15,7 +15,7 @@ REMOTE_DIR="$TFTP_PATH"
 
 # BUILD
 echo "[*] Building kernel..."
-cargo build --release
+cargo build --release --target aarch64-unknown-none
 
 # CONVERT TO IMG
 echo "[*] Convert kernel elf to img..."