1 files changed, 28 insertions, 0 deletions

diff --git a/arch/arm/include/asm/assembler.h b/arch/arm/include/asm/assembler.h
index 5c8b3bf4d825..683a1e6b6020 100644
--- a/arch/arm/include/asm/assembler.h
+++ b/arch/arm/include/asm/assembler.h
@@ -22,6 +22,7 @@
 
 #include <asm/ptrace.h>
 #include <asm/domain.h>
+#include <asm/opcodes-virt.h>
 
 #define IOMEM(x)        (x)
 
@@ -240,6 +241,33 @@
 #endif
 
 /*
+ * Helper macro to enter SVC mode cleanly and mask interrupts. reg is
+ * a scratch register for the macro to overwrite.
+ *
+ * This macro is intended for forcing the CPU into SVC mode at boot time.
+ * you cannot return to the original mode.
+ *
+ * Beware, it also clobers LR.
+ */
+.macro safe_svcmode_maskall reg:req
+        mrs     \reg , cpsr
+        mov     lr , \reg
+        and     lr , lr , #MODE_MASK
+        cmp     lr , #HYP_MODE
+        orr     \reg , \reg , #PSR_A_BIT | PSR_I_BIT | PSR_F_BIT
+        bic     \reg , \reg , #MODE_MASK
+        orr     \reg , \reg , #SVC_MODE
+THUMB(  orr     \reg , \reg , #PSR_T_BIT        )
+        msr     spsr_cxsf, \reg
+        adr     lr, BSYM(2f)
+        bne     1f
+        __MSR_ELR_HYP(14)
+        __ERET
+1:      movs    pc, lr
+2:
+.endm
+
+/*
  * STRT/LDRT access macros with ARM and Thumb-2 variants
  */
 #ifdef CONFIG_THUMB2_KERNEL