Use SMC for PSCI calls issued from EL2

src/main.rs

@@ -25,7 +25,7 @@ mod psci;
 mod rng;
 
 use core::mem::MaybeUninit;
-use core::{arch::global_asm, panic::PanicInfo};
+use core::{arch::asm, arch::global_asm, panic::PanicInfo};
 use core::ptr::addr_of_mut;
 use linked_list_allocator::LockedHeap;
 use log::{debug, error, info};
@@ -67,6 +67,18 @@ const SMBIOS3_GUID: Guid = guid!(
 #[global_allocator]
 pub static ALLOCATOR: LockedHeap = LockedHeap::empty();
 
+fn current_el() -> u64 {
+    let mut l: u64;
+    unsafe {
+        asm!(
+            "mrs {reg}, CurrentEL",
+            reg = out(reg) l,
+            options(pure, nomem, nostack, preserves_flags)
+        );
+    }
+    l >> 2
+}
+
 #[no_mangle]
 extern "C" fn efilite_main(base: *mut u8, used: isize, avail: usize) {
     // Grab the device tree blob that QEMU left for us in memory

src/psci.rs

@@ -6,13 +6,16 @@ use efiloader::runtimeservices::ResetType;
 use efiloader::status::Status;
 
 use core::arch::asm;
+use crate::current_el;
 
 const PSCI_SYSTEM_OFF: u32 = 0x84000008;
 const PSCI_SYSTEM_RESET: u32 = 0x84000009;
 
 fn psci_call(fid: u32) {
-    unsafe {
-        asm!("hvc #0", in("x0") fid);
+    match current_el() {
+      1 => unsafe { asm!("hvc #0", in("x0") fid); },
+      2 => unsafe { asm!("smc #0", in("x0") fid); },
+      l => log::warn!("Unsupported EL {}\n", l)
     }
 }
 

src/rng.rs

@@ -3,6 +3,7 @@
 // Author: Ard Biesheuvel <[email protected]>
 
 use core::arch::asm;
+use crate::current_el;
 
 const ID_AA64ISAR0_RNDR_SHIFT: usize = 60;
 
@@ -21,18 +22,6 @@ const ARM_SMCCC_TRNG_RND64: u32 = 0xc4000053;
 
 const MAX_BITS_PER_CALL: usize = 192;
 
-fn current_el() -> u64 {
-    let mut l: u64;
-    unsafe {
-        asm!(
-            "mrs {reg}, CurrentEL",
-            reg = out(reg) l,
-            options(pure, nomem, nostack, preserves_flags)
-        );
-    }
-    l >> 2
-}
-
 fn smccc_call(fid: u32, arg: u32, use_smc: bool) -> i32 {
     let mut ret: i32;
     if use_smc {