Fix the conditional execution of the NWFPE instructions

Starting with ARMv7-A, conditional execution of undefined instructions
can trigger an exception even if the condition check fails. This patch
modifies the NWFPE support to check the condition before emulating the
instruction.
Signed-off-by: Catalin Marinas <catalin.marinas@arm.com>

arch/arm/nwfpe/entry.S

@@ -71,19 +71,28 @@ floating point instructions.  GCC attempts to group floating point
 instructions to allow the emulator to spread the cost of the trap over
 several floating point instructions.  */
 
+#include <asm/asm-offsets.h>
+
 	.globl	nwfpe_enter
 nwfpe_enter:
 	mov	r4, lr			@ save the failure-return addresses
 	mov	sl, sp			@ we access the registers via 'sl'
 
-	ldr	r5, [sp, #60]		@ get contents of PC;
+	ldr	r5, [sp, #S_PC]		@ get contents of PC;
+	ldr	r7, [sp, #S_PSR]	@ fetch the PSR
+	mov	r6, r0			@ save the opcode
 emulate:
+	mov	r1, r7			@ prepare for checkCondition
+	bl	checkCondition		@ check the condition
+	cmp	r0, #0			@ r0 = 0 ==> condition failed
+
+	@ if condition code failed to match, next insn
+	beq	next			@ get the next instruction;
+
+	mov	r0, r6			@ prepare for EmulateAll()
 	bl	EmulateAll		@ emulate the instruction
 	cmp	r0, #0			@ was emulation successful
 	moveq	pc, r4			@ no, return failure
-	ldr	r7, [sp, #64]		@ fetch the PSR
-	tst	r7, #0x20
-	movne	pc, r9			@ return ok if in Thumb mode
 next:
 .Lx1:	ldrt	r6, [r5], #4		@ get the next instruction and
 					@ increment PC
@@ -94,18 +103,10 @@ next:
 	teqne	r2, #0x0E000000
 	movne	pc, r9			@ return ok if not a fp insn
 
-	str	r5, [sp, #60]		@ update PC copy in regs
+	str	r5, [sp, #S_PC]		@ update PC copy in regs
 
 	mov	r0, r6			@ save a copy
-	mov	r1, r7			@ fetch the condition codes
-	bl	checkCondition		@ check the condition
-	cmp	r0, #0			@ r0 = 0 ==> condition failed
-
-	@ if condition code failed to match, next insn
-	beq	next			@ get the next instruction;
-
-	mov	r0, r6			@ prepare for EmulateAll()
-	b	emulate			@ if r0 != 0, goto EmulateAll
+	b	emulate			@ check condition and emulate
 
 	@ We need to be prepared for the instructions at .Lx1 and .Lx2 
 	@ to fault.  Emit the appropriate exception gunk to fix things up.