Implement first basic interrupt handler

src/gpio.rs

@@ -11,6 +11,8 @@ const GPCLR_BASE: u32 = 0x3F20_0028;
 const GPLEV_BASE: u32 = 0x3F20_0034;
 const GPPUD: u32 = 0x3F20_0094;
 const GPPUDCLK_BASE: u32 = 0x3F20_0098;
+const GPREN_BASE: u32 = 0x3F20_004C;
+const GPFEN_BASE: u32 = 0x3F20_0058;
 
 #[repr(u32)]
 pub enum GPIOState {
@@ -112,3 +114,40 @@ fn gpio_pull_up_down(gpio: u8, val: u32) {
         write_volatile(register_addr as *mut u32, 0);
     }
 }
+
+pub fn gpio_enable_low_detect(gpio: u8, enable: bool) {
+    unsafe {
+        // Determine GPLEN Register
+        let register_addr = GPFEN_BASE + 4 * (gpio as u32 / 32);
+        let register_offset = gpio % 32;
+
+        let current = read_volatile(register_addr as *const u32);
+        let mask = 0b1 << register_offset;
+        let new_val = if enable {
+            current | mask
+        } else {
+            current & !mask
+        };
+
+        write_volatile(register_addr as *mut u32, new_val);
+    }
+}
+
+pub fn gpio_enable_high_detect(gpio: u8, enable: bool) {
+    unsafe {
+        // Determine GPHEN Register
+        let register_addr = GPREN_BASE + 4 * (gpio as u32 / 32);
+        let register_offset = gpio % 32;
+
+        let current = read_volatile(register_addr as *const u32);
+
+        let mask = 0b1 << register_offset;
+        let new_val = if enable {
+            current | mask
+        } else {
+            current & !mask
+        };
+
+        write_volatile(register_addr as *mut u32, new_val);
+    }
+}

src/interrupt.rs

@@ -0,0 +1,37 @@
+use core::{
+    arch::asm,
+    ptr::{read_volatile, write_volatile},
+};
+
+use crate::uart::print;
+
+const INTERRUPT_BASE: u32 = 0x3F00_B000;
+const ENABLE_IRQ_BASE: u32 = INTERRUPT_BASE + 0x210;
+const DISABLE_IRQ_BASE: u32 = INTERRUPT_BASE + 0x21C;
+
+#[no_mangle]
+pub unsafe extern "C" fn irq_handler() {
+    print("Interrupt\r\n");
+}
+
+pub fn enable_iqr_source(nr: u32) {
+    let register = ENABLE_IRQ_BASE + 4 * (nr / 32);
+    let register_offset = nr % 32;
+    unsafe {
+        let current = read_volatile(register as *const u32);
+        let mask = 0b1 << register_offset;
+        let new_val = current | mask;
+        write_volatile(register as *mut u32, new_val);
+    }
+}
+
+pub fn disable_iqr_source(nr: u32) {
+    let register = DISABLE_IRQ_BASE + 4 * (nr / 32);
+    let register_offset = nr % 32;
+    unsafe {
+        let current = read_volatile(register as *const u32);
+        let mask = 0b1 << register_offset;
+        let new_val = current | mask;
+        write_volatile(register as *mut u32, new_val);
+    }
+}

src/main.rs

@@ -2,20 +2,33 @@
 #![no_std]
 #![feature(asm_experimental_arch)]
 
-use core::{arch::asm, panic::PanicInfo};
+use core::{
+    arch::{asm, global_asm},
+    panic::PanicInfo,
+};
 
-use gpio::{gpio_get_state, gpio_high, gpio_low, gpio_pull_up, set_gpio_state};
+use gpio::{
+    gpio_enable_low_detect, gpio_get_state, gpio_high, gpio_low, gpio_pull_up, set_gpio_state,
+};
+use interrupt::enable_iqr_source;
 use timer::{delay_nops, sleep};
 use uart::print;
 
 mod gpio;
+mod interrupt;
 mod timer;
 mod uart;
 
+global_asm!(include_str!("vector.S"));
+
+extern "C" {
+    fn el2_to_el1();
+}
+
 #[panic_handler]
 fn panic(_panic: &PanicInfo) -> ! {
     loop {
-        uart::print("Panic");
+        uart::print("Panic\r\n");
     }
 }
 
@@ -23,13 +36,17 @@ fn panic(_panic: &PanicInfo) -> ! {
 #[link_section = ".text._start"]
 pub unsafe extern "C" fn _start() {
     // Set the stack pointer
-    asm!("ldr x0, =0x8004000", "mov sp, x0");
-    main();
+    asm!(
+        "ldr x0, =0x8008000",
+        "mov sp, x0",
+        "b main",
+        options(noreturn)
+    );
 }
 
 #[no_mangle]
-extern "C" fn main() {
-    uart::configure_uart();
+pub extern "C" fn main() -> ! {
+    uart::uart_init();
     // Set ACT Led to Outout
     let _ = set_gpio_state(21, gpio::GPIOState::Output);
 
@@ -37,16 +54,32 @@ extern "C" fn main() {
     let _ = set_gpio_state(14, gpio::GPIOState::Alternative0);
     let _ = set_gpio_state(15, gpio::GPIOState::Alternative0);
 
-    // Set GPIO 21 to Input
-    let _ = set_gpio_state(21, gpio::GPIOState::Input);
-    gpio_pull_up(21);
+    print_current_el_str();
 
     // Delay so clock speed can stabilize
     delay_nops(50000);
     uart::print("Hello World!\r\n");
 
+    unsafe {
+        el2_to_el1();
+    }
+
+    loop {}
+}
+
+#[no_mangle]
+pub extern "C" fn kernel_main() -> ! {
+    let el = get_current_el();
+    print_current_el_str();
+
     sleep(500_000);
 
+    // Set GPIO 21 to Input
+    enable_iqr_source(49); //21 is on the first GPIO bank
+    let _ = set_gpio_state(21, gpio::GPIOState::Input);
+    gpio_pull_up(21);
+    gpio_enable_low_detect(21, true);
+
     loop {
         let _ = gpio_high(29);
 
@@ -67,3 +100,29 @@ fn print_gpio_state() {
     print(s);
     print("\r\n");
 }
+
+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
+}
+
+fn print_current_el_str() {
+    let el = get_current_el();
+    let el_str = match el {
+        0b11 => "Level 3",
+        0b10 => "Level 2",
+        0b01 => "Level 1",
+        0b00 => "Level 0",
+        _ => "Unknown EL",
+    };
+
+    print(el_str);
+    print("\r\n");
+}

src/uart.rs

@@ -27,7 +27,7 @@ pub fn print(s: &str) {
     unsafe { while (core::ptr::read_volatile(UART0_FR as *const u32) >> 3) & 0b1 != 0 {} }
 }
 
-pub fn configure_uart() {
+pub fn uart_init() {
     let baud_div_times_64 = (UART_CLK * 4) / BAUD;
 
     let ibrd = baud_div_times_64 / 64;
@@ -45,6 +45,7 @@ pub fn configure_uart() {
         // Enable transmit and uart
         let mut cr = core::ptr::read_volatile(UART0_CR as *mut u32);
         cr |= UART0_CR_UARTEN | UART0_CR_TXE;
+
         core::ptr::write_volatile(UART0_CR as *mut u32, cr);
     }
 }

src/vector.S

@@ -0,0 +1,51 @@
+
+.global vector_table
+.extern irq_handler
+
+.macro    ventry    label
+.align    7
+b    \label
+.endm
+
+.section .vector_table, "ax"
+vector_table:
+    ventry .
+    ventry .
+    ventry .
+    ventry .
+
+    ventry .
+    ventry irq_handler     // IRQ(Interrupt Request) 0x280
+
+
+.align 4
+.extern main
+.global el2_to_el1
+el2_to_el1:
+
+    mov x0, #(1 << 31)
+    msr HCR_EL2, x0
+    isb
+
+    // Set SPSR_EL2: return to EL1h (EL1, using SP_EL1)
+    mov x0, #(0b0101)
+    msr SPSR_EL2, x0
+    isb
+
+    // Set return address to ELR_EL2
+    ldr x0, =kernel_main
+    msr ELR_EL2, x0
+    isb
+
+    // Set SP_EL1 to stack base
+    ldr x0, =__stack_end
+    msr SP_EL1, x0
+    isb
+
+    // Set VBAR_EL1 to vector table
+    adr x0, vector_table
+    msr VBAR_EL1, x0
+    isb
+
+    // Return to EL1
+    eret