2 files changed, 24 insertions, 0 deletions

diff --git a/arch/arm/include/asm/assembler.h b/arch/arm/include/asm/assembler.h
index ad301f107dd2..bc8d4bbd82e2 100644
--- a/arch/arm/include/asm/assembler.h
+++ b/arch/arm/include/asm/assembler.h
@@ -518,4 +518,22 @@ THUMB(	orr	\reg , \reg , #PSR_T_BIT	)
 #endif
         .endm
 
+        .macro  bug, msg, line
+#ifdef CONFIG_THUMB2_KERNEL
+1:      .inst   0xde02
+#else
+1:      .inst   0xe7f001f2
+#endif
+#ifdef CONFIG_DEBUG_BUGVERBOSE
+        .pushsection .rodata.str, "aMS", %progbits, 1
+2:      .asciz  "\msg"
+        .popsection
+        .pushsection __bug_table, "aw"
+        .align  2
+        .word   1b, 2b
+        .hword  \line
+        .popsection
+#endif
+        .endm
+
 #endif /* __ASM_ASSEMBLER_H__ */
diff --git a/arch/arm/kernel/entry-header.S b/arch/arm/kernel/entry-header.S
index 6391728c8f03..75f7a4e8541a 100644
--- a/arch/arm/kernel/entry-header.S
+++ b/arch/arm/kernel/entry-header.S
@@ -299,6 +299,8 @@
         mov     r2, sp
         ldr     r1, [r2, #\offset + S_PSR]      @ get calling cpsr
         ldr     lr, [r2, #\offset + S_PC]!      @ get pc
+        tst     r1, #0xcf
+        bne     1f
         msr     spsr_cxsf, r1                   @ save in spsr_svc
 #if defined(CONFIG_CPU_V6) || defined(CONFIG_CPU_32v6K)
         @ We must avoid clrex due to Cortex-A15 erratum #830321
@@ -313,6 +315,7 @@
                                                 @ after ldm {}^
         add     sp, sp, #\offset + PT_REGS_SIZE
         movs    pc, lr                          @ return & move spsr_svc into cpsr
+1:      bug     "Returning to usermode but unexpected PSR bits set?", \@
 #elif defined(CONFIG_CPU_V7M)
         @ V7M restore.
         @ Note that we don't need to do clrex here as clearing the local
@@ -328,6 +331,8 @@
         ldr     r1, [sp, #\offset + S_PSR]      @ get calling cpsr
         ldr     lr, [sp, #\offset + S_PC]       @ get pc
         add     sp, sp, #\offset + S_SP
+        tst     r1, #0xcf
+        bne     1f
         msr     spsr_cxsf, r1                   @ save in spsr_svc
 
         @ We must avoid clrex due to Cortex-A15 erratum #830321
@@ -340,6 +345,7 @@
         .endif
         add     sp, sp, #PT_REGS_SIZE - S_SP
         movs    pc, lr                          @ return & move spsr_svc into cpsr
+1:      bug     "Returning to usermode but unexpected PSR bits set?", \@
 #endif  /* !CONFIG_THUMB2_KERNEL */
         .endm