Support booting the kernel in EL1 with a PSCI proxy

Author: m4tx

Cargo.lock

@@ -14,13 +14,19 @@ aarch64-paging = { version = "0.10", default-features = false }
 aarch64-rt = { version = "0.3", features = ["el2", "exceptions", "initial-pagetable", "psci"], default-features = false }
 arm-pl011-uart = "0.4"
 buddy_system_allocator = { version = "0.11", default-features = false, features = ["alloc", "use_spin"] }
+# TODO: replace with a crates.io version when it's released
+dtoolkit = { version = "0.1", git = "ssh://git@github.com/google/dtoolkit.git", rev = "35c8644de5fd2ff0098cfd8e493a8cffc163cf04", features = ["write"] }
 embedded-io = "0.7"
 log = "0.4"
 percore = "0.2"
 safe-mmio = "0.2"
 smccc = "0.2"
 spin = { version = "0.10", features = ["lazy", "once", "spin_mutex"], default-features = false }
 
+[patch.crates-io]
+# TODO: replace with a crates.io version when this is merged: https://github.com/google/aarch64-rt/pull/31
+aarch64-rt = { git = "https://github.com/m4tx/aarch64-rt.git", rev = "a93fdb00baa88549c7e9c90a4dd1c66b164844bf" }
+
 [lints.rust]
 deprecated-safe = "warn"
 keyword-idents = "warn"

Cargo.toml

@@ -1,4 +1,19 @@
 MEMORY
 {
-	image : ORIGIN = 0x40080000, LENGTH = 64M
+	image : ORIGIN = 0x40080000, LENGTH = 4M
+	heap (rw) : ORIGIN = 0x40400000, LENGTH = 4M
+	payload (rwx) : ORIGIN = 0x40800000, LENGTH = 64M
 }
+
+SECTIONS {
+     .heap :
+	 {
+       *(.heap);
+       . = ALIGN(4);
+     } > heap
+	.payload :
+	{
+       *(.payload);
+       . = ALIGN(4);
+     } > payload
+} INSERT AFTER .text;

linker/qemu.ld

@@ -50,6 +50,32 @@ pub fn tlbi_alle2is() {
     }
 }
 
+/// Get the current stack pointer.
+pub fn sp() -> u64 {
+    let val: u64;
+    // SAFETY: Reading from the sp register is always safe.
+    unsafe { asm!("mov {}, sp", out(reg) val, options(nostack)) };
+    val
+}
+
+pub fn esr() -> u64 {
+    let mut esr: u64;
+    // SAFETY: Reading esr is always safe.
+    unsafe {
+        asm!("mrs {esr}, esr_el2", esr = out(reg) esr);
+    }
+    esr
+}
+
+pub fn far() -> u64 {
+    let mut far: u64;
+    // SAFETY: Reading far is always safe.
+    unsafe {
+        asm!("mrs {far}, far_el2", far = out(reg) far);
+    }
+    far
+}
+
 macro_rules! sys_reg {
     ($name:ident, {$($const_name:ident: $const_val:expr),*}) => {
         pub mod $name {
@@ -109,10 +135,20 @@ sys_reg!(sctlr_el2, {
 sys_reg!(clidr_el1);
 sys_reg!(csselr_el1);
 sys_reg!(ccsidr_el1);
-sys_reg!(hcr_el2);
+sys_reg!(hcr_el2, {
+    RW: 1 << 31,
+    TID1: 1 << 19,
+    AMO: 1 << 4
+});
 sys_reg!(cntvoff_el2);
-sys_reg!(cnthctl_el2);
-sys_reg!(spsr_el2);
+sys_reg!(cnthctl_el2, {
+    ENABLE: 1 << 0,
+    IMASK: 1 << 1
+});
+sys_reg!(spsr_el2, {
+    MASK_ALL: 0b1111 << 6,
+    EL1H: 5
+});
 sys_reg!(elr_el2);
 sys_reg!(sp_el1);
 

src/arch.rs

@@ -6,6 +6,8 @@
 // option. This file may not be copied, modified, or distributed
 // except according to those terms.
 
+use core::arch::naked_asm;
+
 #[unsafe(no_mangle)]
 extern "C" fn sync_exception_current(_elr: u64, _spsr: u64) {
     panic!("Unexpected sync_exception_current");
@@ -26,9 +28,34 @@ extern "C" fn serr_current(_elr: u64, _spsr: u64) {
     panic!("Unexpected serr_current");
 }
 
+#[unsafe(naked)]
 #[unsafe(no_mangle)]
-extern "C" fn sync_lower(_elr: u64, _spsr: u64) {
-    panic!("Unexpected sync_lower");
+#[rustfmt::skip]
+unsafe extern "C" fn sync_lower() {
+    naked_asm!(
+        // We load and store the registers from/to where aarch64 exception handler expects them:
+        // https://github.com/google/aarch64-rt/blob/047f3b0962064d334f149fe5fcd46ba57ea758ab/src/exceptions.S#L16-L65
+        "stp x29, x30, [sp, #-0x10]!",
+
+        "ldp x2, x3, [sp, #0x10]",
+        "ldp x4, x5, [sp, #0x20]",
+
+        "bl {sync_lower_impl}",
+
+        "mov x1, xzr",
+        "mov x2, xzr",
+        "mov x3, xzr",
+        "stp x0, x1, [sp, #0x10]",
+        "stp x2, x3, [sp, #0x20]",
+        "ldr x4, [sp, #8 * 22 + 16]",
+        // Return after the HVC call
+        "add x4, x4, #4",
+        "str x4, [sp, #8 * 22 + 16]",
+
+        "ldp x29, x30, [sp], #0x10",
+        "ret",
+        sync_lower_impl = sym crate::hypervisor::sync_lower_impl,
+    )
 }
 
 #[unsafe(no_mangle)]