Add exception handling and memory mapping

etc/aarch64-unknown-none-rpi3b.ld

@@ -1,22 +1,24 @@
 ENTRY(_entry);
 
-SECTIONS {
-    . = 0x80000;
+KERNEL_OFFSET = 0xFFFFFF8000000000;
 
-    .text : {
+SECTIONS {
+    . = 0x80000 + KERNEL_OFFSET;
+
+    .text : AT(. - KERNEL_OFFSET) ALIGN(4K) {
         KEEP(*(.text.boot))
         *(.text)
     }
 
-    .rodata : {
+    .rodata : AT(. - KERNEL_OFFSET) ALIGN(4K) {
         *(.rodata)
     }
 
-    .data : {
+    .data : AT(. - KERNEL_OFFSET) ALIGN(4K) {
         *(.data)
     }
 
-    .bss : {
+    .bss : AT(. - KERNEL_OFFSET) ALIGN(4K) {
         *(.bss)
     }
 }

src/boot/entry.S

@@ -0,0 +1,228 @@
+.set SPSR_EL3_EL2t, 0x8
+.set SPSR_EL3_EL2h, 0x9
+.set SPSR_EL3_EL1t, 0x4
+.set SPSR_EL3_EL1h, 0x5
+.set SCR_EL3_RW, (1 << 10)
+.set SCR_EL3_SMD, (1 << 7)
+.set SCR_EL3_RES1, (3 << 4)
+.set SCR_EL3_NS, (1 << 0)
+
+.set SPSR_EL2_EL1h, 0x5
+.set HCR_EL2_RW, (1 << 31)
+.set HCR_EL2_HCD, (1 << 29)
+.set HCR_EL2_C, (1 << 2)
+.set HCR_EL2_A, (1 << 1)
+
+.set MAIR_EL1_OUTER_NC, 4
+.set MAIR_EL1_INNER_NC, (4 << 4)
+.set MAIR_EL1_DEVICE_nGRE, 0
+.set MAIR_EL1_DEVICE, 0
+
+.set TCR_EL1_IPS_48, (5 << 32)
+.set TCR_EL1_TG1_4K, (2 << 30)
+
+.cpu cortex-a57
+.section .text.boot
+.global _entry
+_entry:
+    mrs x0, mpidr_el1
+    ands x0, x0, #3
+    beq _entry_bsp
+1:
+    b 1b
+
+.section .text
+_entry_bsp:
+    // EL3 check
+    mrs x0, CurrentEL
+    lsr x0, x0, #2
+    cmp x0, #3
+    bne 1f
+
+    // Leave EL3
+    adr x0, 1f
+    msr elr_el3, x0
+    mov x0, #SPSR_EL3_EL2h
+    // TODO mask DAIF?
+    msr spsr_el3, x0
+    mov x0, #(SCR_EL3_RW | SCR_EL3_SMD | SCR_EL3_RES1 | SCR_EL3_NS)
+    msr scr_el3, x0
+
+    eret
+1:
+    // EL2 check
+    mrs x0, CurrentEL
+    lsr x0, x0, #2
+    cmp x0, #2
+    bne 1f
+
+    // Leave EL2
+    adr x0, 1f
+    msr elr_el2, x0
+    // TODO mask DAIF?
+    mov x0, #SPSR_EL2_EL1h
+    msr spsr_el2, x0
+    mov x0, #(HCR_EL2_RW | HCR_EL2_HCD)
+    orr x0, x0, #HCR_EL2_A
+    msr hcr_el2, x0
+
+    eret
+1:
+    // Setup paging tables
+    adr x8, kernel_l1
+    mov x2, #(1 << 0)       // Present
+    orr x2, x2, #(1 << 10)  // Accessed
+    orr x2, x2, #(3 << 8)   // Inner shareable
+    // orr x2, x2, #(0 << 2)   // MAIR[0]
+    str x2, [x8]
+
+    mov x2, #(1 << 0)       // Present
+    orr x2, x2, #(1 << 10)  // Accessed
+    orr x2, x2, #(3 << 8)   // Inner shareable
+    orr x2, x2, #(1 << 2)   // MAIR[1]
+    str x2, [x8, #8]
+
+//    mov x1, #512
+//1:
+//    sub x1, x1, #1
+//
+//    lsl x0, x1, #30
+//    orr x0, x0, x2
+//
+//    str x0, [x8, x1, lsl #3]
+//
+//    cmp x1, xzr
+//    bne 1b
+
+    // Setup the MMU
+    mov x0, #(MAIR_EL1_INNER_NC | MAIR_EL1_OUTER_NC)
+    msr mair_el1, x0
+
+    // TODO clarify
+    mov x0, #TCR_EL1_IPS_48
+    orr x0, x0, #TCR_EL1_TG1_4K
+    ldr x1, =(25 << 16)
+    orr x0, x0, x1
+    ldr x1, =25
+    orr x0, x0, x1
+    msr tcr_el1, x0
+
+    isb
+
+    adr x0, kernel_l1
+    msr ttbr1_el1, x0
+    msr ttbr0_el1, x0
+
+    dsb ish
+    isb
+
+    mrs x0, sctlr_el1
+    // TODO clarify
+    ldr x1, =~((1 << 25) | (1 << 24) | (1 << 19) | (1 << 12) | (1 << 4) | (1 << 2) | (1 << 0) | (1 << 1))
+    and x0, x0, x1
+    orr x0, x0, #1
+    msr sctlr_el1, x0
+
+    isb
+
+    adr x0, upper_half
+    ldr x1, =0xFFFFFF8000000000
+    add x0, x0, x1
+    br x0
+
+upper_half:
+    // Shoot off the legs
+    msr ttbr0_el1, xzr
+
+    adr x0, el1_vectors
+    msr vbar_el1, x0
+
+    adr x0, bsp_stack_top
+    mov sp, x0
+
+    bl kernel_main
+
+    b .
+
+.section .rodata
+.p2align 7
+el1_vectors:
+// Current level with SP_EL0
+vec_el1_sp_el0_sync:
+    b .
+.p2align 7
+vec_el1_sp_el0_irq:
+    b .
+.p2align 7
+vec_el1_sp_el0_fiq:
+    b .
+.p2align 7
+vec_el1_sp_el0_serror:
+    b .
+// Current level with SL_ELx, x > 0
+.p2align 7
+vec_el1_sp_elx_sync:
+    sub sp, sp, #192
+    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]
+    mrs x0, esr_el1
+    mrs x1, far_el1
+    stp x0, x1, [sp, #176]
+
+    mov x0, sp
+
+    bl exc_handler
+
+.p2align 7
+vec_el1_sp_elx_irq:
+    b .
+.p2align 7
+vec_el1_sp_elx_fiq:
+    b .
+.p2align 7
+vec_el1_sp_elx_serror:
+    b .
+// Lower EL, AArch64
+.p2align 7
+vec_el0_aa64_sync:
+    b .
+.p2align 7
+vec_el0_aa64_irq:
+    b .
+.p2align 7
+vec_el0_aa64_fiq:
+    b .
+.p2align 7
+vec_el0_aa64_serror:
+    b .
+// Lower EL, AArch32
+.p2align 7
+vec_el0_aa32_sync:
+    b .
+.p2align 7
+vec_el0_aa32_irq:
+    b .
+.p2align 7
+vec_el0_aa32_fiq:
+    b .
+.p2align 7
+vec_el0_aa32_serror:
+    b .
+
+.section .bss
+.p2align 4
+bsp_stack_bottom:
+    .skip 32768
+bsp_stack_top:
+.p2align 12
+kernel_l1:
+    .skip 4096

src/boot/mod.rs

@@ -0,0 +1 @@
+global_asm!(include_str!("entry.S"));

src/main.rs

@@ -1,15 +1,54 @@
-#![feature(
-    global_asm,
-)]
+#![feature(global_asm, llvm_asm)]
 #![no_std]
 #![no_main]
 
-global_asm!(r#"
-.section .text.boot
-.global _entry
-_entry:
-    b .
-"#);
+pub mod boot;
+
+#[repr(C)]
+struct ExceptionContext {
+    x0: usize,
+    x1: usize,
+    x2: usize,
+    x3: usize,
+    x4: usize,
+    x5: usize,
+    x6: usize,
+    x7: usize,
+    x8: usize,
+    x9: usize,
+    x10: usize,
+    x11: usize,
+    x12: usize,
+    x13: usize,
+    x14: usize,
+    x15: usize,
+    x16: usize,
+    x17: usize,
+    x18: usize,
+    fp: usize,
+    lp: usize,
+    _r0: usize,
+    esr: usize,
+    far: usize,
+}
+
+#[no_mangle]
+extern "C" fn exc_handler(context: ExceptionContext) -> ! {
+    loop {}
+}
+
+#[no_mangle]
+extern "C" fn kernel_main() -> ! {
+    unsafe {
+        let v = *(0x1234 as *mut u64);
+    }
+
+    loop {
+        unsafe {
+            llvm_asm!("wfe");
+        }
+    }
+}
 
 use core::panic::PanicInfo;
 #[panic_handler]