Add warm_boot_entry for suspend support

src/entry.rs

@@ -88,3 +88,64 @@ pub unsafe extern "C" fn secondary_entry(stack_end: *mut u64) -> ! {
         set_exception_vector = sym crate::set_exception_vector,
     )
 }
+
+/// An assembly entry point for warm boot (e.g. resume from suspend).
+///
+/// It will enable the MMU, disable trapping of floating point instructions,
+/// set up the exception vector, set the stack pointer to `stack_ptr`,
+/// and then jump to `entry_point(arg)`.
+///
+/// The function expects to be passed a pointer to a `SuspendContext` instance
+/// that will be valid after returning from suspend. It should therefore be
+/// a static, or allocated on the heap, to avoid being deallocated before resuming.
+///
+/// This is a low-level function that should be used as the entry point when
+/// manually calling the `CPU_SUSPEND` PSCI call. It deliberately doesn't store any
+/// data itself so that the caller has maximum flexibility over things such as
+/// where the `SuspendContext` is stored. If you need to restore any state (such
+/// as the registers), or you want to emulate returning from a function after
+/// suspending the CPU, you need to implement this functionality yourself.
+///
+/// # Safety
+///
+/// The caller guarantees that the `SuspendContext` instance passed to the function
+/// will be valid and safe to read when the CPU resumes (especially important when
+/// the data is wrapped in a mutex, for instance).
+///
+/// The `context` parameter has the following restrictions on its fields:
+/// * `stack_ptr` must be a valid stack pointer.
+/// * `entry` must be a valid function pointer taking `SuspendContext<T>`.
+#[unsafe(naked)]
+pub unsafe extern "C" fn warm_boot_entry<T>(context: *const SuspendContext<T>) -> ! {
+    naked_asm!(
+        "mov x19, x0",
+        "bl enable_mmu",
+        // Disable trapping floating point access in EL1.
+        "mrs x30, cpacr_el1",
+        "orr x30, x30, #(0x3 << 20)",
+        "msr cpacr_el1, x30",
+        "isb",
+        // Load data from SuspendContext
+        "ldr x0, [x19, #{stack_ptr_offset}]",
+        "ldr x1, [x19, #{entry_offset}]",
+        // Set the exception vector.
+        "bl {set_exception_vector}",
+        // Set the stack pointer (x0).
+        "mov sp, x0",
+        // Jump to entry point (x1) with arg (x2).
+        "mov x0, x19",
+        "br x1",
+        set_exception_vector = sym crate::set_exception_vector,
+        stack_ptr_offset = const offset_of!(SuspendContext<T>, stack_ptr),
+        entry_offset = const offset_of!(SuspendContext<T>, entry),
+    )
+}
+
+/// Data used by [`warm_boot_entry`] to restore the CPU state after resuming.
+#[derive(Debug, Copy, Clone)]
+#[repr(C)]
+pub struct SuspendContext<T> {
+    pub stack_ptr: u64,
+    pub entry: extern "C" fn(&Self) -> !,
+    pub data: T,
+}

src/lib.rs

@@ -32,7 +32,7 @@ use core::arch::asm;
 #[cfg(not(feature = "initial-pagetable"))]
 use core::arch::naked_asm;
 use core::mem::ManuallyDrop;
-pub use entry::secondary_entry;
+pub use entry::{SuspendContext, secondary_entry, warm_boot_entry};
 #[cfg(feature = "exceptions")]
 pub use exceptions::{ExceptionHandlers, RegisterState, RegisterStateRef};
 #[cfg(all(feature = "initial-pagetable", feature = "el1"))]