Index: sys/arch/arm/arm32/exception.S
===================================================================
RCS file: /cvsroot/src/sys/arch/arm/arm32/exception.S,v
retrieving revision 1.23
diff -u -p -r1.23 exception.S
--- sys/arch/arm/arm32/exception.S	21 Jun 2015 15:00:06 -0000	1.23
+++ sys/arch/arm/arm32/exception.S	6 Jul 2017 14:20:15 -0000
@@ -258,7 +258,12 @@ ASEND(undefined_entry)
  */
 
 ENTRY_NP(undefinedinstruction_bounce)
-	PUSHFRAMEINSVC
+	PUSHXXXREGSANDSWITCH
+	and	r2, r3, #(PSR_MODE)
+	cmp	r2, #(PSR_SVC32_MODE)
+	mov	r2, sp
+	subeq	r2, r2, #(4 * 16)
+	PUSHTRAPFRAME(r2)
 	ENABLE_ALIGNMENT_FAULTS
 
 	mov	r0, sp
Index: sys/arch/arm/include/arm32/frame.h
===================================================================
RCS file: /cvsroot/src/sys/arch/arm/include/arm32/frame.h,v
retrieving revision 1.43
diff -u -p -r1.43 frame.h
--- sys/arch/arm/include/arm32/frame.h	22 Jun 2017 08:44:21 -0000	1.43
+++ sys/arch/arm/include/arm32/frame.h	6 Jul 2017 14:20:22 -0000
@@ -440,13 +440,15 @@ LOCK_CAS_DEBUG_LOCALS
 	msr     cpsr_c, tmp		/* Punch into SVC mode */
 #endif
 
-#define PUSHFRAMEINSVC							   \
+#define PUSHXXXREGSANDSWITCH						   \
 	stmdb	sp, {r0-r3};		/* Save 4 registers */		   \
 	mov	r0, lr;			/* Save xxx32 r14 */		   \
 	mov	r1, sp;			/* Save xxx32 sp */		   \
 	mrs	r3, spsr;		/* Save xxx32 spsr */		   \
-	SET_CPSR_MODE(r2, PSR_SVC32_MODE);				   \
-	bic	r2, sp, #7;		/* Align new SVC sp */		   \
+	SET_CPSR_MODE(r2, PSR_SVC32_MODE)
+
+#define PUSHTRAPFRAME(rX)						   \
+	bic	r2, rX, #7;		/* Align new SVC sp */		   \
 	str	r0, [r2, #-4]!;		/* Push return address */	   \
 	stmdb	r2!, {sp, lr};		/* Push SVC sp, lr */		   \
 	mov	sp, r2;			/* Keep stack aligned */	   \
@@ -458,6 +460,10 @@ LOCK_CAS_DEBUG_LOCALS
 	mrs	r0, spsr;		/* Get the SPSR */		   \
 	str	r0, [sp, #-TF_R0]!	/* Push the SPSR onto the stack */
 
+#define PUSHFRAMEINSVC							   \
+	PUSHXXXREGSANDSWITCH;						   \
+	PUSHTRAPFRAME(sp)
+
 /*
  * PULLFRAMEFROMSVCANDEXIT - macro to pull a trap frame from the stack
  * in SVC32 mode and restore the saved processor mode and PC.