fel_fix.patch

diff --git a/arch/arm/cpu/armv8/fel_utils.S b/arch/arm/cpu/armv8/fel_utils.S
index 9004b3c27aa3..feb206de2b5e 100644
--- a/arch/arm/cpu/armv8/fel_utils.S
+++ b/arch/arm/cpu/armv8/fel_utils.S
@@ -69,6 +69,16 @@ fel_stash_addr:			// must immediately precede back_in_32:
 /* AArch32 code to restore the state from fel_stash and return back to FEL. */
 back_in_32:
 	.word	0xe51f000c	// ldr	r0, [pc, #-12]  ; load fel_stash address
+
+	.word	0xe10f2000	// mrs        r2,cpsr
+	.word	0xe3c2201f	// bic        r2,r2,#0x1f
+	.word	0xe38220d2	// orr        r2,r2,#0xd2
+	.word	0xe121f002	// msr        cpsr_c,r2
+
+	.word	0xe3042540	// mov r2, #0x4540
+	.word	0xe1a02202	// MOV R2, R2, lsl #4
+	.word	0xe1a0d002	// mov sp, r2
+
 	.word	0xe5901008	// ldr	r1, [r0, #8]
 	.word	0xe129f001	// msr	CPSR_fc, r1
 	.word	0xf57ff06f	// isb