X-Git-Url: https://git.proxmox.com/?p=mirror_edk2.git;a=blobdiff_plain;f=ArmPlatformPkg%2FSec%2FHelper.asm;h=1649df399adae8f1dbf01f0c9d80858b4dc1b45e;hp=43a0749138acf40392c60179ed95cf45f3be0a1c;hb=513aa3497afd3b84f6f20ed53a18534cb72b2180;hpb=504d14603dab586779954485c0ca568b7bd41e25

diff --git a/ArmPlatformPkg/Sec/Helper.asm b/ArmPlatformPkg/Sec/Helper.asm
index 43a0749138..1649df399a 100644
--- a/ArmPlatformPkg/Sec/Helper.asm
+++ b/ArmPlatformPkg/Sec/Helper.asm
@@ -15,6 +15,7 @@
     EXPORT  return_from_exception
     EXPORT  enter_monitor_mode
     EXPORT  copy_cpsr_into_spsr
+    EXPORT  set_non_secure_mode
     
     AREA   Helper, CODE, READONLY
 
@@ -60,6 +61,18 @@ copy_cpsr_into_spsr
     msr     spsr_cxsf, r0
     bx      lr
 
+// Set the Non Secure Mode
+set_non_secure_mode
+    push    { r1 }
+    and	r0, r0, #0x1f     // Keep only the mode bits
+    mrs     r1, spsr          // Read the spsr
+    bic     r1, r1, #0x1f     // Clear all mode bits
+    orr	    r1, r1, r0
+    msr     spsr_cxsf, r1     // write back spsr (may have caused a mode switch)
+    isb
+    pop     { r1 }
+    bx      lr                // return (hopefully thumb-safe!)
+
 dead
     B       dead