From 54f1edaad3c6f4a19fa6b67e0b8cad1ae115dc30 Mon Sep 17 00:00:00 2001
From: Ard Biesheuvel <ardb@kernel.org>
Date: Sat, 11 May 2024 16:04:32 +0200
Subject: [PATCH] Use SMC for PSCI calls issued from EL2

---
 src/main.rs | 14 +++++++++++++-
 src/psci.rs |  7 +++++--
 src/rng.rs  | 13 +------------
 3 files changed, 19 insertions(+), 15 deletions(-)

diff --git a/src/main.rs b/src/main.rs
index a303c51..6223e28 100644
--- a/src/main.rs
+++ b/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
diff --git a/src/psci.rs b/src/psci.rs
index 28bf355..0f3e523 100644
--- a/src/psci.rs
+++ b/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)
     }
 }
 
diff --git a/src/rng.rs b/src/rng.rs
index ad77de0..c7bec51 100644
--- a/src/rng.rs
+++ b/src/rng.rs
@@ -3,6 +3,7 @@
 // Author: Ard Biesheuvel <ardb@google.com>
 
 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 {