Add support for ARM v6 instructions.

	* Makefile.in (SIM_EXTRA_CFLAGS): Add -lm.
	* armdefs.h (ARMdval, ARMfval): New types.
	(ARM_VFP_reg): New union.
	(struct ARMul_State): Add VFP_Reg and FPSCR fields.
	(VFP_fval, VFP_uword, VFP_sword, VFP_dval, VFP_dword): Accessor
	macros for the new VFP_Reg field.
	* armemu.c (handle_v6_insn): Add code to handle MOVW, MOVT,
	QADD16, QASX, QSAX, QSUB16, QADD8, QSUB8, UADD16, USUB16, UADD8,
	USUB8, SEL, REV, REV16, RBIT, BFC, BFI, SBFX and UBFX
	instructions.
	(handle_VFP_move): New function.
	(ARMul_Emulate16): Add checks for newly supported v6
	instructions.  Add support for VMRS, VMOV and MRC instructions.
	(Multiply64): Allow nRdHi == nRm and/or nRdLo == nRm when
	operating in v6 mode.
	* armemu.h (t_resolved): Define.
	* armsupp.c: Include math.h.
	(handle_VFP_xfer): New function.  Handles VMOV, VSTM, VSTR, VPUSH,
	VSTM, VLDM and VPOP instructions.
	(ARMul_LDC): Test for co-processor 10 or 11 and pass call to the
	new handle_VFP_xfer function.
	(ARMul_STC): Likewise.
	(handle_VFP_op): New function.  Handles VMLA, VMLS, VNMLA, VNMLS,
	VNMUL, VMUL, VADD, VSUB, VDIV, VMOV, VABS, VNEG, VSQRT, VCMP,
	VCMPE and VCVT instructions.
	(ARMul_CDP): Test for co-processor 10 or 11 and pass call to the
	new handle_VFP_op function.
	* thumbemu.c (tBIT, tBITS, ntBIT, ntBITS): New macros.
	(test_cond): New function.  Tests a condition and returns non-zero
	if the condition has been met.
	(handle_IT_block): New function.
	(in_IT_block): New function.
	(IT_block_allow): New function.
	(ThumbExpandImm): New function.
	(handle_T2_insn): New function.  Handles T2 thumb instructions.
	(handle_v6_thumb_insn): Add next_instr and pc parameters.
	(ARMul_ThumbDecode): Add support for IT blocks.  Add support for
	v6 instructions.
	* wrapper.c (sim_create_inferior): Detect a thumb address and call
	SETT appropriately.

9 files changed, 3881 insertions, 126 deletions

diff --git a/sim/arm/ChangeLog b/sim/arm/ChangeLog
index 1f6db4e..a6533e0 100644
--- a/sim/arm/ChangeLog
+++ b/sim/arm/ChangeLog
@@ -1,3 +1,46 @@
+2015-06-28  Nick Clifton  <nickc@redhat.com>
+
+	* Makefile.in (SIM_EXTRA_CFLAGS): Add -lm.
+	* armdefs.h (ARMdval, ARMfval): New types.
+	(ARM_VFP_reg): New union.
+	(struct ARMul_State): Add VFP_Reg and FPSCR fields.
+	(VFP_fval, VFP_uword, VFP_sword, VFP_dval, VFP_dword): Accessor
+	macros for the new VFP_Reg field.
+	* armemu.c (handle_v6_insn): Add code to handle MOVW, MOVT,
+	QADD16, QASX, QSAX, QSUB16, QADD8, QSUB8, UADD16, USUB16, UADD8,
+	USUB8, SEL, REV, REV16, RBIT, BFC, BFI, SBFX and UBFX
+	instructions.
+	(handle_VFP_move): New function.
+	(ARMul_Emulate16): Add checks for newly supported v6
+	instructions.  Add support for VMRS, VMOV and MRC instructions.
+	(Multiply64): Allow nRdHi == nRm and/or nRdLo == nRm when
+	operating in v6 mode.
+	* armemu.h (t_resolved): Define.
+	* armsupp.c: Include math.h.
+	(handle_VFP_xfer): New function.  Handles VMOV, VSTM, VSTR, VPUSH,
+	VSTM, VLDM and VPOP instructions.
+	(ARMul_LDC): Test for co-processor 10 or 11 and pass call to the
+	new handle_VFP_xfer function.
+	(ARMul_STC): Likewise.
+	(handle_VFP_op): New function.  Handles VMLA, VMLS, VNMLA, VNMLS,
+	VNMUL, VMUL, VADD, VSUB, VDIV, VMOV, VABS, VNEG, VSQRT, VCMP,
+	VCMPE and VCVT instructions.
+	(ARMul_CDP): Test for co-processor 10 or 11 and pass call to the
+	new handle_VFP_op function.
+	* thumbemu.c (tBIT, tBITS, ntBIT, ntBITS): New macros.
+	(test_cond): New function.  Tests a condition and returns non-zero
+	if the condition has been met.
+	(handle_IT_block): New function.
+	(in_IT_block): New function.
+	(IT_block_allow): New function.
+	(ThumbExpandImm): New function.
+	(handle_T2_insn): New function.  Handles T2 thumb instructions.
+	(handle_v6_thumb_insn): Add next_instr and pc parameters.
+	(ARMul_ThumbDecode): Add support for IT blocks.  Add support for
+	v6 instructions.
+	* wrapper.c (sim_create_inferior): Detect a thumb address and call
+	SETT appropriately.
+
 2015-06-23  Mike Frysinger  <vapier@gentoo.org>
 
 	* configure: Regenerate.
diff --git a/sim/arm/Makefile.in b/sim/arm/Makefile.in
index 745b62a..7605588 100644
--- a/sim/arm/Makefile.in
+++ b/sim/arm/Makefile.in
@@ -17,7 +17,7 @@
 
 ## COMMON_PRE_CONFIG_FRAG
 
-SIM_EXTRA_CFLAGS = -DMODET
+SIM_EXTRA_CFLAGS = -DMODET -lm
 
 SIM_OBJS = \
 	wrapper.o \
diff --git a/sim/arm/armdefs.h b/sim/arm/armdefs.h
index 08a61f2..4e19a62 100644
--- a/sim/arm/armdefs.h
+++ b/sim/arm/armdefs.h
@@ -49,6 +49,25 @@ typedef unsigned ARMul_CPReads (ARMul_State * state, unsigned reg,
 typedef unsigned ARMul_CPWrites (ARMul_State * state, unsigned reg,
 				 ARMword value);
 
+typedef double ARMdval;	/* FIXME: Must be a 64-bit floating point type.  */
+typedef float  ARMfval;	/* FIXME: Must be a 32-bit floating point type.  */
+
+typedef union
+{
+  ARMword  uword[2];
+  ARMsword sword[2];
+  ARMfval  fval[2];
+  ARMdword dword;
+  ARMdval  dval;
+} ARM_VFP_reg;
+
+#define VFP_fval(N)  (state->VFP_Reg[(N)>> 1].fval[(N) & 1])
+#define VFP_uword(N) (state->VFP_Reg[(N)>> 1].uword[(N) & 1])
+#define VFP_sword(N) (state->VFP_Reg[(N)>> 1].sword[(N) & 1])
+
+#define VFP_dval(N)  (state->VFP_Reg[(N)].dval)
+#define VFP_dword(N) (state->VFP_Reg[(N)].dword)
+
 struct ARMul_State
 {
   ARMword Emulate;		/* to start and stop emulation */
@@ -138,6 +157,9 @@ struct ARMul_State
   unsigned is_iWMMXt;		/* Are we emulating an iWMMXt co-processor ?  */
   unsigned is_ep9312;		/* Are we emulating a Cirrus Maverick co-processor ?  */
   unsigned verbose;		/* Print various messages like the banner */
+
+  ARM_VFP_reg  VFP_Reg[32];     /* Advanced SIMD registers.  */
+  ARMword      FPSCR;		/* Floating Point Status Register.  */
 };
 
 #define ResetPin NresetSig
diff --git a/sim/arm/armemu.c b/sim/arm/armemu.c
index 09dfeaf..f2a84eb 100644
--- a/sim/arm/armemu.c
+++ b/sim/arm/armemu.c
@@ -6,12 +6,12 @@
     it under the terms of the GNU General Public License as published by
     the Free Software Foundation; either version 3 of the License, or
     (at your option) any later version.
- 
+
     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.
- 
+
     You should have received a copy of the GNU General Public License
     along with this program; if not, see <http://www.gnu.org/licenses/>.  */
 
@@ -265,6 +265,11 @@ extern int stop_simulator;
 static int
 handle_v6_insn (ARMul_State * state, ARMword instr)
 {
+  ARMword val;
+  ARMword Rd;
+  ARMword Rm;
+  ARMword Rn;
+
   switch (BITS (20, 27))
     {
 #if 0
@@ -280,29 +285,427 @@ handle_v6_insn (ARMul_State * state, ARMword instr)
     case 0x1d: printf ("Unhandled v6 insn: ldrexb\n"); break;
     case 0x1e: printf ("Unhandled v6 insn: strexh\n"); break;
     case 0x1f: printf ("Unhandled v6 insn: ldrexh\n"); break;
-    case 0x30: printf ("Unhandled v6 insn: movw\n"); break;
     case 0x32: printf ("Unhandled v6 insn: nop/sev/wfe/wfi/yield\n"); break;
-    case 0x34: printf ("Unhandled v6 insn: movt\n"); break;
     case 0x3f: printf ("Unhandled v6 insn: rbit\n"); break;
 #endif
     case 0x61: printf ("Unhandled v6 insn: sadd/ssub\n"); break;
-    case 0x62: printf ("Unhandled v6 insn: qadd/qsub\n"); break;
     case 0x63: printf ("Unhandled v6 insn: shadd/shsub\n"); break;
-    case 0x65: printf ("Unhandled v6 insn: uadd/usub\n"); break;
-    case 0x66: printf ("Unhandled v6 insn: uqadd/uqsub\n"); break;
-    case 0x67: printf ("Unhandled v6 insn: uhadd/uhsub\n"); break;
-    case 0x68: printf ("Unhandled v6 insn: pkh/sxtab/selsxtb\n"); break;
     case 0x6c: printf ("Unhandled v6 insn: uxtb16/uxtab16\n"); break;
     case 0x70: printf ("Unhandled v6 insn: smuad/smusd/smlad/smlsd\n"); break;
     case 0x74: printf ("Unhandled v6 insn: smlald/smlsld\n"); break;
     case 0x75: printf ("Unhandled v6 insn: smmla/smmls/smmul\n"); break;
     case 0x78: printf ("Unhandled v6 insn: usad/usada8\n"); break;
-    case 0x7a: printf ("Unhandled v6 insn: usbfx\n"); break;
-    case 0x7c: printf ("Unhandled v6 insn: bfc/bfi\n"); break;
+
+    case 0x30:
+      {
+	/* MOVW<c> <Rd>,#<imm16>
+	   instr[31,28] = cond
+	   instr[27,20] = 0011 0000
+	   instr[19,16] = imm4
+	   instr[15,12] = Rd
+	   instr[11, 0] = imm12.  */
+	Rd = BITS (12, 15);
+	val = (BITS (16, 19) << 12) | BITS (0, 11);
+	state->Reg[Rd] = val;
+	return 1;
+      }
+
+    case 0x34:
+      {
+	/* MOVT<c> <Rd>,#<imm16>
+	   instr[31,28] = cond
+	   instr[27,20] = 0011 0100
+	   instr[19,16] = imm4
+	   instr[15,12] = Rd
+	   instr[11, 0] = imm12.  */
+	Rd = BITS (12, 15);
+	val = (BITS (16, 19) << 12) | BITS (0, 11);
+	state->Reg[Rd] &= 0xFFFF;
+	state->Reg[Rd] |= val << 16;
+	return 1;
+      }
+
+    case 0x62:
+      {
+	ARMword val1;
+	ARMword val2;
+	ARMsword n, m, r;
+	int i;
+
+	Rd = BITS (12, 15);
+	Rn = BITS (16, 19);
+	Rm = BITS (0, 3);
+
+	if (Rd == 15 || Rn == 15 || Rm == 15)
+	  break;
+
+	val1 = state->Reg[Rn];
+	val2 = state->Reg[Rm];
+
+	switch (BITS (4, 11))
+	  {
+	  case 0xF1: /* QADD16<c> <Rd>,<Rn>,<Rm>.  */
+	    state->Reg[Rd] = 0;
+
+	    for (i = 0; i < 32; i+= 16)
+	      {
+		n = (val1 >> i) & 0xFFFF;
+		if (n & 0x8000)
+		  n |= -1 << 16;
+
+		m = (val2 >> i) & 0xFFFF;
+		if (m & 0x8000)
+		  m |= -1 << 16;
+
+		r = n + m;
+
+		if (r > 0x7FFF)
+		  r = 0x7FFF;
+		else if (r < -(0x8000))
+		  r = - 0x8000;
+
+		state->Reg[Rd] |= (r & 0xFFFF) << i;
+	      }	
+	    return 1;
+
+	  case 0xF3: /* QASX<c> <Rd>,<Rn>,<Rm>.  */
+	    n = val1 & 0xFFFF;
+	    if (n & 0x8000)
+	      n |= -1 << 16;
+
+	    m = (val2 >> 16) & 0xFFFF;
+	    if (m & 0x8000)
+	      m |= -1 << 16;
+
+	    r = n - m;
+
+	    if (r > 0x7FFF)
+	      r = 0x7FFF;
+	    else if (r < -(0x8000))
+	      r = - 0x8000;
+
+	    state->Reg[Rd] = (r & 0xFFFF);
+
+	    n = (val1 >> 16) & 0xFFFF;
+	    if (n & 0x8000)
+	      n |= -1 << 16;
+
+	    m = val2 & 0xFFFF;
+	    if (m & 0x8000)
+	      m |= -1 << 16;
+
+	    r = n + m;
+
+	    if (r > 0x7FFF)
+	      r = 0x7FFF;
+	    else if (r < -(0x8000))
+	      r = - 0x8000;
+
+	    state->Reg[Rd] |= (r & 0xFFFF) << 16;
+	    return 1;
+
+	  case 0xF5: /* QSAX<c> <Rd>,<Rn>,<Rm>.  */
+	    n = val1 & 0xFFFF;
+	    if (n & 0x8000)
+	      n |= -1 << 16;
+
+	    m = (val2 >> 16) & 0xFFFF;
+	    if (m & 0x8000)
+	      m |= -1 << 16;
+
+	    r = n + m;
+
+	    if (r > 0x7FFF)
+	      r = 0x7FFF;
+	    else if (r < -(0x8000))
+	      r = - 0x8000;
+
+	    state->Reg[Rd] = (r & 0xFFFF);
+
+	    n = (val1 >> 16) & 0xFFFF;
+	    if (n & 0x8000)
+	      n |= -1 << 16;
+
+	    m = val2 & 0xFFFF;
+	    if (m & 0x8000)
+	      m |= -1 << 16;
+
+	    r = n - m;
+
+	    if (r > 0x7FFF)
+	      r = 0x7FFF;
+	    else if (r < -(0x8000))
+	      r = - 0x8000;
+
+	    state->Reg[Rd] |= (r & 0xFFFF) << 16;
+	    return 1;
+
+	  case 0xF7: /* QSUB16<c> <Rd>,<Rn>,<Rm>.  */
+	    state->Reg[Rd] = 0;
+
+	    for (i = 0; i < 32; i+= 16)
+	      {
+		n = (val1 >> i) & 0xFFFF;
+		if (n & 0x8000)
+		  n |= -1 << 16;
+
+		m = (val2 >> i) & 0xFFFF;
+		if (m & 0x8000)
+		  m |= -1 << 16;
+
+		r = n - m;
+
+		if (r > 0x7FFF)
+		  r = 0x7FFF;
+		else if (r < -(0x8000))
+		  r = - 0x8000;
+
+		state->Reg[Rd] |= (r & 0xFFFF) << i;
+	      }	
+	    return 1;
+
+	  case 0xF9: /* QADD8<c> <Rd>,<Rn>,<Rm>.  */
+	    state->Reg[Rd] = 0;
+
+	    for (i = 0; i < 32; i+= 8)
+	      {
+		n = (val1 >> i) & 0xFF;
+		if (n & 0x80)
+		  n |= -1 << 8;
+
+		m = (val2 >> i) & 0xFF;
+		if (m & 0x80)
+		  m |= -1 << 8;
+
+		r = n + m;
+
+		if (r > 127)
+		  r = 127;
+		else if (r < -128)
+		  r = -128;
+
+		state->Reg[Rd] |= (r & 0xFF) << i;
+	      }	
+	    return 1;
+
+	  case 0xFF: /* QSUB8<c> <Rd>,<Rn>,<Rm>.  */
+	    state->Reg[Rd] = 0;
+
+	    for (i = 0; i < 32; i+= 8)
+	      {
+		n = (val1 >> i) & 0xFF;
+		if (n & 0x80)
+		  n |= -1 << 8;
+
+		m = (val2 >> i) & 0xFF;
+		if (m & 0x80)
+		  m |= -1 << 8;
+
+		r = n - m;
+
+		if (r > 127)
+		  r = 127;
+		else if (r < -128)
+		  r = -128;
+
+		state->Reg[Rd] |= (r & 0xFF) << i;
+	      }	
+	    return 1;
+
+	  default:
+	    break;
+	  }
+	break;
+      }
+
+    case 0x65:
+      {
+	ARMword valn;
+	ARMword valm;
+	ARMword res1, res2, res3, res4;
+
+	/* U{ADD|SUB}{8|16}<c> <Rd>, <Rn>, <Rm>
+	   instr[31,28] = cond
+	   instr[27,20] = 0110 0101
+	   instr[19,16] = Rn
+	   instr[15,12] = Rd
+	   instr[11, 8] = 1111
+	   instr[ 7, 4] = opcode: UADD8 (1001), UADD16 (0001), USUB8 (1111), USUB16 (0111)
+	   instr[ 3, 0] = Rm.  */
+	if (BITS (8, 11) != 0xF)
+	  break;
+
+	Rn = BITS (16, 19);
+	Rd = BITS (12, 15);
+	Rm = BITS (0, 3);
+
+	if (Rn == 15 || Rd == 15 || Rm == 15)
+	  {
+	    ARMul_UndefInstr (state, instr);
+	    state->Emulate = FALSE;
+	    break;
+	  }
+
+	valn = state->Reg[Rn];
+	valm = state->Reg[Rm];
+	
+	switch (BITS (4, 7))
+	  {
+	  case 1:  /* UADD16.  */
+	    res1 = (valn & 0xFFFF) + (valm & 0xFFFF);
+	    if (res1 > 0xFFFF)
+	      state->Cpsr |= (GE0 | GE1);
+	    else
+	      state->Cpsr &= ~ (GE0 | GE1);
+
+	    res2 = (valn >> 16) + (valm >> 16);
+	    if (res2 > 0xFFFF)
+	      state->Cpsr |= (GE2 | GE3);
+	    else
+	      state->Cpsr &= ~ (GE2 | GE3);
+
+	    state->Reg[Rd] = (res1 & 0xFFFF) | (res2 << 16);
+	    return 1;
+
+	  case 7:  /* USUB16.  */
+	    res1 = (valn & 0xFFFF) - (valm & 0xFFFF);
+	    if (res1 & 0x800000)
+	      state->Cpsr |= (GE0 | GE1);
+	    else
+	      state->Cpsr &= ~ (GE0 | GE1);
+
+	    res2 = (valn >> 16) - (valm >> 16);
+	    if (res2 & 0x800000)
+	      state->Cpsr |= (GE2 | GE3);
+	    else
+	      state->Cpsr &= ~ (GE2 | GE3);
+
+	    state->Reg[Rd] = (res1 & 0xFFFF) | (res2 << 16);
+	    return 1;
+
+	  case 9:  /* UADD8.  */
+	    res1 = (valn & 0xFF) + (valm & 0xFF);
+	    if (res1 > 0xFF)
+	      state->Cpsr |= GE0;
+	    else
+	      state->Cpsr &= ~ GE0;
+
+	    res2 = ((valn >> 8) & 0xFF) + ((valm >> 8) & 0xFF);
+	    if (res2 > 0xFF)
+	      state->Cpsr |= GE1;
+	    else
+	      state->Cpsr &= ~ GE1;
+
+	    res3 = ((valn >> 16) & 0xFF) + ((valm >> 16) & 0xFF);
+	    if (res3 > 0xFF)
+	      state->Cpsr |= GE2;
+	    else
+	      state->Cpsr &= ~ GE2;
+
+	    res4 = (valn >> 24) + (valm >> 24);
+	    if (res4 > 0xFF)
+	      state->Cpsr |= GE3;
+	    else
+	      state->Cpsr &= ~ GE3;
+
+	    state->Reg[Rd] = (res1 & 0xFF) | ((res2 << 8) & 0xFF00)
+	      | ((res3 << 16) & 0xFF0000) | (res4 << 24);
+	    return 1;
+
+	  case 15: /* USUB8.  */
+	    res1 = (valn & 0xFF) - (valm & 0xFF);
+	    if (res1 & 0x800000)
+	      state->Cpsr |= GE0;
+	    else
+	      state->Cpsr &= ~ GE0;
+
+	    res2 = ((valn >> 8) & 0XFF) - ((valm >> 8) & 0xFF);
+	    if (res2 & 0x800000)
+	      state->Cpsr |= GE1;
+	    else
+	      state->Cpsr &= ~ GE1;
+
+	    res3 = ((valn >> 16) & 0XFF) - ((valm >> 16) & 0xFF);
+	    if (res3 & 0x800000)
+	      state->Cpsr |= GE2;
+	    else
+	      state->Cpsr &= ~ GE2;
+
+	    res4 = (valn >> 24) - (valm >> 24) ;
+	    if (res4 & 0x800000)
+	      state->Cpsr |= GE3;
+	    else
+	      state->Cpsr &= ~ GE3;
+
+	    state->Reg[Rd] = (res1 & 0xFF) | ((res2 << 8) & 0xFF00)
+	      | ((res3 << 16) & 0xFF0000) | (res4 << 24);
+	    return 1;
+
+	  default:
+	    break;
+	  }
+	break;
+      }
+
+    case 0x68:
+      {
+	ARMword res;
+
+	/* PKHBT<c> <Rd>,<Rn>,<Rm>{,LSL #<imm>}
+	   PKHTB<c> <Rd>,<Rn>,<Rm>{,ASR #<imm>}
+	   SXTAB16<c> <Rd>,<Rn>,<Rm>{,<rotation>}
+	   SXTB16<c> <Rd>,<Rm>{,<rotation>}
+	   SEL<c> <Rd>,<Rn>,<Rm>
+
+	   instr[31,28] = cond
+	   instr[27,20] = 0110 1000
+	   instr[19,16] = Rn
+	   instr[15,12] = Rd
+	   instr[11, 7] = imm5 (PKH), 11111 (SEL), rr000 (SXTAB16 & SXTB16),
+	   instr[6]     = tb (PKH), 0 (SEL), 1 (SXT)
+	   instr[5]     = opcode: PKH (0), SEL/SXT (1)
+	   instr[4]     = 1
+	   instr[ 3, 0] = Rm.  */
+
+	if (BIT (4) != 1)
+	  break;
+
+	if (BIT (5) == 0)
+	  {
+	    /* FIXME: Add implementation of PKH.  */
+	    fprintf (stderr, "PKH: NOT YET IMPLEMENTED\n");
+	    ARMul_UndefInstr (state, instr);
+	    break;
+	  }
+
+	if (BIT (6) == 1)
+	  {
+	    /* FIXME: Add implementation of SXT.  */
+	    fprintf (stderr, "SXT: NOT YET IMPLEMENTED\n");
+	    ARMul_UndefInstr (state, instr);
+	    break;
+	  }
+
+	Rn = BITS (16, 19);
+	Rd = BITS (12, 15);
+	Rm = BITS (0, 3);
+	if (Rn == 15 || Rm == 15 || Rd == 15)
+	  {
+	    ARMul_UndefInstr (state, instr);
+	    state->Emulate = FALSE;
+	    break;
+	  }
+
+	res  = (state->Reg[(state->Cpsr & GE0) ? Rn : Rm]) & 0xFF;
+	res |= (state->Reg[(state->Cpsr & GE1) ? Rn : Rm]) & 0xFF00;
+	res |= (state->Reg[(state->Cpsr & GE2) ? Rn : Rm]) & 0xFF0000;
+	res |= (state->Reg[(state->Cpsr & GE3) ? Rn : Rm]) & 0xFF000000;
+	state->Reg[Rd] = res;
+	return 1;
+      }
 
     case 0x6a:
       {
-	ARMword Rm;
 	int ror = -1;
 
 	switch (BITS (4, 11))
@@ -316,6 +719,7 @@ handle_v6_insn (ARMul_State * state, ARMword instr)
 	  case 0xf3:
 	    printf ("Unhandled v6 insn: ssat\n");
 	    return 0;
+
 	  default:
 	    break;
 	  }
@@ -345,9 +749,8 @@ handle_v6_insn (ARMul_State * state, ARMword instr)
 
     case 0x6b:
       {
-	ARMword Rm;
 	int ror = -1;
-	  
+
 	switch (BITS (4, 11))
 	  {
 	  case 0x07: ror = 0; break;
@@ -355,9 +758,57 @@ handle_v6_insn (ARMul_State * state, ARMword instr)
 	  case 0x87: ror = 16; break;
 	  case 0xc7: ror = 24; break;
 
+	  case 0xf3:
+	    {
+	      /* REV<c> <Rd>,<Rm>
+	         instr[31,28] = cond
+	         instr[27,20] = 0110 1011
+	         instr[19,16] = 1111
+	         instr[15,12] = Rd
+	         instr[11, 4] = 1111 0011
+	         instr[ 3, 0] = Rm.  */
+	      if (BITS (16, 19) != 0xF)
+		break;
+
+	      Rd = BITS (12, 15);
+	      Rm = BITS (0, 3);
+	      if (Rd == 15 || Rm == 15)
+		{
+		  ARMul_UndefInstr (state, instr);
+		  state->Emulate = FALSE;
+		  break;
+		}
+
+	      val = state->Reg[Rm] << 24;
+	      val |= ((state->Reg[Rm] << 8) & 0xFF0000);
+	      val |= ((state->Reg[Rm] >> 8) & 0xFF00);
+	      val |= ((state->Reg[Rm] >> 24));
+	      state->Reg[Rd] = val;
+	      return 1;
+	    }
+
 	  case 0xfb:
-	    printf ("Unhandled v6 insn: rev\n");
-	    return 0;
+	    {
+	      /* REV16<c> <Rd>,<Rm>.  */
+	      if (BITS (16, 19) != 0xF)
+		break;
+
+	      Rd = BITS (12, 15);
+	      Rm = BITS (0, 3);
+	      if (Rd == 15 || Rm == 15)
+		{
+		  ARMul_UndefInstr (state, instr);
+		  state->Emulate = FALSE;
+		  break;
+		}
+
+	      val = 0;
+	      val |= ((state->Reg[Rm] >> 8) & 0x00FF00FF);
+	      val |= ((state->Reg[Rm] << 8) & 0xFF00FF00);
+	      state->Reg[Rd] = val;
+	      return 1;
+	    }
+
 	  default:
 	    break;
 	  }
@@ -380,9 +831,8 @@ handle_v6_insn (ARMul_State * state, ARMword instr)
 
     case 0x6e:
       {
-	ARMword Rm;
 	int ror = -1;
-	  
+
 	switch (BITS (4, 11))
 	  {
 	  case 0x07: ror = 0; break;
@@ -394,6 +844,7 @@ handle_v6_insn (ARMul_State * state, ARMword instr)
 	  case 0xf3:
 	    printf ("Unhandled v6 insn: usat\n");
 	    return 0;
+
 	  default:
 	    break;
 	  }
@@ -411,7 +862,7 @@ handle_v6_insn (ARMul_State * state, ARMword instr)
 	Rm = ((state->Reg[BITS (0, 3)] >> ror) & 0xFF);
 
 	if (BITS (16, 19) == 0xf)
-	   /* UXTB */
+	  /* UXTB */
 	  state->Reg[BITS (12, 15)] = Rm;
 	else
 	  /* UXTAB */
@@ -421,9 +872,9 @@ handle_v6_insn (ARMul_State * state, ARMword instr)
 
     case 0x6f:
       {
-	ARMword Rm;
+	int i;
 	int ror = -1;
-	  
+
 	switch (BITS (4, 11))
 	  {
 	  case 0x07: ror = 0; break;
@@ -431,9 +882,21 @@ handle_v6_insn (ARMul_State * state, ARMword instr)
 	  case 0x87: ror = 16; break;
 	  case 0xc7: ror = 24; break;
 
+	  case 0xf3: /* RBIT */
+	    if (BITS (16, 19) != 0xF)
+	      break;
+	    Rd = BITS (12, 15);
+	    state->Reg[Rd] = 0;
+	    Rm = state->Reg[BITS (0, 3)];
+	    for (i = 0; i < 32; i++)
+	      if (Rm & (1 << i))
+		state->Reg[Rd] |= (1 << (31 - i));
+	    return 1;
+
 	  case 0xfb:
 	    printf ("Unhandled v6 insn: revsh\n");
 	    return 0;
+
 	  default:
 	    break;
 	  }
@@ -447,13 +910,138 @@ handle_v6_insn (ARMul_State * state, ARMword instr)
 	  /* UXT */
 	  state->Reg[BITS (12, 15)] = Rm;
 	else
+	  /* UXTAH */
+	  state->Reg[BITS (12, 15)] = state->Reg [BITS (16, 19)] + Rm;
+      }
+      return 1;
+
+    case 0x7c:
+    case 0x7d:
+      {
+	int lsb;
+	int msb;
+	ARMword mask;
+
+	/* BFC<c> <Rd>,#<lsb>,#<width>
+	   BFI<c> <Rd>,<Rn>,#<lsb>,#<width>
+
+	   instr[31,28] = cond
+	   instr[27,21] = 0111 110
+	   instr[20,16] = msb
+	   instr[15,12] = Rd
+	   instr[11, 7] = lsb
+	   instr[ 6, 4] = 001 1111
+	   instr[ 3, 0] = Rn (BFI) / 1111 (BFC).  */
+
+	if (BITS (4, 6) != 0x1)
+	  break;
+
+	Rd = BITS (12, 15);
+	if (Rd == 15)
 	  {
-	    /* UXTAH */
-	    state->Reg[BITS (12, 15)] = state->Reg [BITS (16, 19)] + Rm;
+	    ARMul_UndefInstr (state, instr);
+	    state->Emulate = FALSE;
+	  }
+
+	lsb = BITS (7, 11);
+	msb = BITS (16, 20);
+	if (lsb > msb)
+	  {
+	    ARMul_UndefInstr (state, instr);
+	    state->Emulate = FALSE;
 	  }
-	}
-      return 1;
 
+	mask = -1 << lsb;
+	mask &= ~(-1 << (msb + 1));
+	state->Reg[Rd] &= ~ mask;
+
+	Rn = BITS (0, 3);
+	if (Rn != 0xF)
+	  {
+	    ARMword val = state->Reg[Rn] & ~(-1 << ((msb + 1) - lsb));
+	    state->Reg[Rd] |= val << lsb;
+	  }
+	return 1;
+      }
+
+    case 0x7b:
+    case 0x7a: /* SBFX<c> <Rd>,<Rn>,#<lsb>,#<width>.  */
+      {
+	int lsb;
+	int widthm1;
+	ARMsword sval;
+
+	if (BITS (4, 6) != 0x5)
+	  break;
+
+	Rd = BITS (12, 15);
+	if (Rd == 15)
+	  {
+	    ARMul_UndefInstr (state, instr);
+	    state->Emulate = FALSE;
+	  }
+
+	Rn = BITS (0, 3);
+	if (Rn == 15)
+	  {
+	    ARMul_UndefInstr (state, instr);
+	    state->Emulate = FALSE;
+	  }
+
+	lsb = BITS (7, 11);
+	widthm1 = BITS (16, 20);
+
+	sval = state->Reg[Rn];
+	sval <<= (31 - (lsb + widthm1));
+	sval >>= (31 - widthm1);
+	state->Reg[Rd] = sval;
+	
+	return 1;
+      }
+
+    case 0x7f:
+    case 0x7e:
+      {
+	int lsb;
+	int widthm1;
+
+	/* UBFX<c> <Rd>,<Rn>,#<lsb>,#<width>
+	   instr[31,28] = cond
+	   instr[27,21] = 0111 111
+	   instr[20,16] = widthm1
+	   instr[15,12] = Rd
+	   instr[11, 7] = lsb
+	   instr[ 6, 4] = 101
+	   instr[ 3, 0] = Rn.  */
+
+	if (BITS (4, 6) != 0x5)
+	  break;
+
+	Rd = BITS (12, 15);
+	if (Rd == 15)
+	  {
+	    ARMul_UndefInstr (state, instr);
+	    state->Emulate = FALSE;
+	  }
+
+	Rn = BITS (0, 3);
+	if (Rn == 15)
+	  {
+	    ARMul_UndefInstr (state, instr);
+	    state->Emulate = FALSE;
+	  }
+
+	lsb = BITS (7, 11);
+	widthm1 = BITS (16, 20);
+
+	val = state->Reg[Rn];
+	val >>= lsb;
+	val &= ~(-1 << (widthm1 + 1));
+
+	state->Reg[Rd] = val;
+	
+	return 1;
+      }
 #if 0
     case 0x84: printf ("Unhandled v6 insn: srs\n"); break;
 #endif
@@ -465,6 +1053,91 @@ handle_v6_insn (ARMul_State * state, ARMword instr)
 }
 #endif
 
+static void
+handle_VFP_move (ARMul_State * state, ARMword instr)
+{
+  switch (BITS (20, 27))
+    {
+    case 0xC4:
+    case 0xC5:
+      switch (BITS (4, 11))
+	{
+	case 0xA1:
+	case 0xA3:
+	  {
+	    /* VMOV two core <-> two VFP single precision.  */
+	    int sreg = (BITS (0, 3) << 1) | BIT (5);
+
+	    if (BIT (20))
+	      {
+		state->Reg[BITS (12, 15)] = VFP_uword (sreg);
+		state->Reg[BITS (16, 19)] = VFP_uword (sreg + 1);
+	      }
+	    else
+	      {
+		VFP_uword (sreg)     = state->Reg[BITS (12, 15)];
+		VFP_uword (sreg + 1) = state->Reg[BITS (16, 19)];
+	      }
+	  }
+	  break;
+
+	case 0xB1:
+	case 0xB3:
+	  {
+	    /* VMOV two core <-> VFP double precision.  */
+	    int dreg = BITS (0, 3) | (BIT (5) << 4);
+
+	    if (BIT (20))
+	      {
+		if (trace)
+		  fprintf (stderr, " VFP: VMOV: r%d r%d <= d%d\n",
+			   BITS (12, 15), BITS (16, 19), dreg);
+
+		state->Reg[BITS (12, 15)] = VFP_dword (dreg);
+		state->Reg[BITS (16, 19)] = VFP_dword (dreg) >> 32;
+	      }
+	    else
+	      {
+		VFP_dword (dreg) = state->Reg[BITS (16, 19)];
+		VFP_dword (dreg) <<= 32;
+		VFP_dword (dreg) |= state->Reg[BITS (12, 15)];
+
+		if (trace)
+		  fprintf (stderr, " VFP: VMOV: d%d <= r%d r%d : %g\n",
+			   dreg, BITS (16, 19), BITS (12, 15),
+			   VFP_dval (dreg));
+	      }
+	  }
+	  break;
+
+	default:
+	  fprintf (stderr, "SIM: VFP: Unimplemented move insn %x\n", BITS (20, 27));
+	  break;
+	}
+      break;
+
+    case 0xe0:
+    case 0xe1:
+      /* VMOV single core <-> VFP single precision.  */
+      if (BITS (0, 6) != 0x10 || BITS (8, 11) != 0xA)
+	fprintf (stderr, "SIM: VFP: Unimplemented move insn %x\n", BITS (20, 27));
+      else
+	{
+	  int sreg = (BITS (16, 19) << 1) | BIT (7);
+
+	  if (BIT (20))
+	    state->Reg[DESTReg] = VFP_uword (sreg);
+	  else
+	    VFP_uword (sreg) = state->Reg[DESTReg];
+	}
+      break;
+
+    default:
+      fprintf (stderr, "SIM: VFP: Unimplemented move insn %x\n", BITS (20, 27));
+      return;
+    }
+}
+
 /* EMULATION of ARM6.  */
 
 /* The PC pipeline value depends on whether ARM
@@ -703,9 +1376,9 @@ ARMul_Emulate26 (ARMul_State * state)
 	      if (BITS (25, 27) == 5) /* BLX(1) */
 		{
 		  ARMword dest;
-		  
+
 		  state->Reg[14] = pc + 4;
-		  
+
 		  /* Force entry into Thumb mode.  */
 		  dest = pc + 8 + 1;
 		  if (BIT (23))
@@ -875,7 +1548,7 @@ check_PMUintr:
 		      ARMword temp = GetLS7RHS (state, instr);
 		      ARMword temp2 = BIT (23) ? LHS + temp : LHS - temp;
 		      ARMword addr = BIT (24) ? temp2 : LHS;
-		      
+
 		      if (BIT (12))
 			ARMul_UndefInstr (state, instr);
 		      else if (addr & 7)
@@ -884,7 +1557,7 @@ check_PMUintr:
 		      else
 			{
 			  int wb = BIT (21) || (! BIT (24));
-			  
+
 			  state->Reg[BITS (12, 15)] =
 			    ARMul_LoadWordN (state, addr);
 			  state->Reg[BITS (12, 15) + 1] =
@@ -1456,7 +2129,7 @@ check_PMUintr:
 		      ARMword op1 = state->Reg[BITS (0, 3)];
 		      ARMword op2 = state->Reg[BITS (8, 11)];
 		      ARMword Rn = state->Reg[BITS (12, 15)];
-		      
+
 		      if (BIT (5))
 			op1 >>= 16;
 		      if (BIT (6))
@@ -1468,7 +2141,7 @@ check_PMUintr:
 		      if (op2 & 0x8000)
 			op2 -= 65536;
 		      op1 *= op2;
-		      
+
 		      if (AddOverflow (op1, Rn, op1 + Rn))
 			SETS;
 		      state->Reg[BITS (16, 19)] = op1 + Rn;
@@ -1538,6 +2211,11 @@ check_PMUintr:
 		}
 	      else
 		{
+#ifdef MODE32
+		  if (state->is_v6
+		      && handle_v6_insn (state, instr))
+		    break;
+#endif
 		  UNDEF_Test;
 		}
 	      break;
@@ -1611,7 +2289,7 @@ check_PMUintr:
 		      if (BIT (5) == 0)
 			{
 			  ARMword Rn = state->Reg[BITS (12, 15)];
-			  
+
 			  if (AddOverflow (result, Rn, result + Rn))
 			    SETS;
 			  result += Rn;
@@ -1704,6 +2382,11 @@ check_PMUintr:
 #endif
 		  ARMul_FixCPSR (state, instr, temp);
 		}
+#ifdef MODE32
+	      else if (state->is_v6
+		       && handle_v6_insn (state, instr))
+		break;
+#endif
 	      else
 		UNDEF_Test;
 
@@ -1834,6 +2517,11 @@ check_PMUintr:
 		  UNDEF_MRSPC;
 		  DEST = GETSPSR (state->Bank);
 		}
+#ifdef MODE32
+	      else if (state->is_v6
+		       && handle_v6_insn (state, instr))
+		break;
+#endif
 	      else
 		UNDEF_Test;
 
@@ -1970,6 +2658,11 @@ check_PMUintr:
 		}
 	      else
 		{
+#ifdef MODE32
+		  if (state->is_v6
+		      && handle_v6_insn (state, instr))
+		    break;
+#endif
 		  UNDEF_Test;
 		}
 	      break;
@@ -2325,6 +3018,11 @@ check_PMUintr:
 	      break;
 
 	    case 0x30:		/* MOVW immed */
+#ifdef MODE32
+	      if (state->is_v6
+		  && handle_v6_insn (state, instr))
+		break;
+#endif
 	      dest = BITS (0, 11);
 	      dest |= (BITS (16, 19) << 12);
 	      WRITEDEST (dest);
@@ -2355,6 +3053,11 @@ check_PMUintr:
 	      if (DESTReg == 15)
 		/* MSR immed to CPSR.  */
 		ARMul_FixCPSR (state, instr, DPImmRHS);
+#ifdef MODE32
+	      else if (state->is_v6
+		       && handle_v6_insn (state, instr))
+		break;
+#endif
 	      else
 		UNDEF_Test;
 	      break;
@@ -2380,6 +3083,12 @@ check_PMUintr:
 	      break;
 
 	    case 0x34:		/* MOVT immed */
+#ifdef MODE32
+	      if (state->is_v6
+		  && handle_v6_insn (state, instr))
+		break;
+#endif
+	      DEST &= 0xFFFF;
 	      dest  = BITS (0, 11);
 	      dest |= (BITS (16, 19) << 12);
 	      DEST |= (dest << 16);
@@ -2422,6 +3131,11 @@ check_PMUintr:
 	    case 0x36:		/* CMN immed and MSR immed to SPSR */
 	      if (DESTReg == 15)
 		ARMul_FixSPSR (state, instr, DPImmRHS);
+#ifdef MODE32
+	      else if (state->is_v6
+		       && handle_v6_insn (state, instr))
+		break;
+#endif
 	      else
 		UNDEF_Test;
 	      break;
@@ -2739,6 +3453,11 @@ check_PMUintr:
 	    case 0x60:		/* Store Word, No WriteBack, Post Dec, Reg.  */
 	      if (BIT (4))
 		{
+#ifdef MODE32
+		  if (state->is_v6
+		      && handle_v6_insn (state, instr))
+		    break;
+#endif
 		  ARMul_UndefInstr (state, instr);
 		  break;
 		}
@@ -2820,6 +3539,11 @@ check_PMUintr:
 	    case 0x64:		/* Store Byte, No WriteBack, Post Dec, Reg.  */
 	      if (BIT (4))
 		{
+#ifdef MODE32
+		  if (state->is_v6
+		      && handle_v6_insn (state, instr))
+		    break;
+#endif
 		  ARMul_UndefInstr (state, instr);
 		  break;
 		}
@@ -2921,6 +3645,11 @@ check_PMUintr:
 	    case 0x69:		/* Load Word, No WriteBack, Post Inc, Reg.  */
 	      if (BIT (4))
 		{
+#ifdef MODE32
+		  if (state->is_v6
+		      && handle_v6_insn (state, instr))
+		    break;
+#endif
 		  ARMul_UndefInstr (state, instr);
 		  break;
 		}
@@ -3002,6 +3731,11 @@ check_PMUintr:
 	    case 0x6d:		/* Load Byte, No WriteBack, Post Inc, Reg.  */
 	      if (BIT (4))
 		{
+#ifdef MODE32
+		  if (state->is_v6
+		      && handle_v6_insn (state, instr))
+		    break;
+#endif
 		  ARMul_UndefInstr (state, instr);
 		  break;
 		}
@@ -3078,6 +3812,11 @@ check_PMUintr:
 	    case 0x71:		/* Load Word, No WriteBack, Pre Dec, Reg.  */
 	      if (BIT (4))
 		{
+#ifdef MODE32
+		  if (state->is_v6
+		      && handle_v6_insn (state, instr))
+		    break;
+#endif
 		  ARMul_UndefInstr (state, instr);
 		  break;
 		}
@@ -3087,6 +3826,11 @@ check_PMUintr:
 	    case 0x72:		/* Store Word, WriteBack, Pre Dec, Reg.  */
 	      if (BIT (4))
 		{
+#ifdef MODE32
+		  if (state->is_v6
+		      && handle_v6_insn (state, instr))
+		    break;
+#endif
 		  ARMul_UndefInstr (state, instr);
 		  break;
 		}
@@ -3102,6 +3846,11 @@ check_PMUintr:
 	    case 0x73:		/* Load Word, WriteBack, Pre Dec, Reg.  */
 	      if (BIT (4))
 		{
+#ifdef MODE32
+		  if (state->is_v6
+		      && handle_v6_insn (state, instr))
+		    break;
+#endif
 		  ARMul_UndefInstr (state, instr);
 		  break;
 		}
@@ -3145,6 +3894,11 @@ check_PMUintr:
 	    case 0x76:		/* Store Byte, WriteBack, Pre Dec, Reg.  */
 	      if (BIT (4))
 		{
+#ifdef MODE32
+		  if (state->is_v6
+		      && handle_v6_insn (state, instr))
+		    break;
+#endif
 		  ARMul_UndefInstr (state, instr);
 		  break;
 		}
@@ -3160,6 +3914,11 @@ check_PMUintr:
 	    case 0x77:		/* Load Byte, WriteBack, Pre Dec, Reg.  */
 	      if (BIT (4))
 		{
+#ifdef MODE32
+		  if (state->is_v6
+		      && handle_v6_insn (state, instr))
+		    break;
+#endif
 		  ARMul_UndefInstr (state, instr);
 		  break;
 		}
@@ -3189,6 +3948,11 @@ check_PMUintr:
 	    case 0x79:		/* Load Word, No WriteBack, Pre Inc, Reg.  */
 	      if (BIT (4))
 		{
+#ifdef MODE32
+		  if (state->is_v6
+		      && handle_v6_insn (state, instr))
+		    break;
+#endif
 		  ARMul_UndefInstr (state, instr);
 		  break;
 		}
@@ -3218,6 +3982,11 @@ check_PMUintr:
 	    case 0x7b:		/* Load Word, WriteBack, Pre Inc, Reg.  */
 	      if (BIT (4))
 		{
+#ifdef MODE32
+		  if (state->is_v6
+		      && handle_v6_insn (state, instr))
+		    break;
+#endif
 		  ARMul_UndefInstr (state, instr);
 		  break;
 		}
@@ -3247,6 +4016,11 @@ check_PMUintr:
 	    case 0x7d:		/* Load Byte, No WriteBack, Pre Inc, Reg.  */
 	      if (BIT (4))
 		{
+#ifdef MODE32
+		  if (state->is_v6
+		      && handle_v6_insn (state, instr))
+		    break;
+#endif
 		  ARMul_UndefInstr (state, instr);
 		  break;
 		}
@@ -3256,6 +4030,11 @@ check_PMUintr:
 	    case 0x7e:		/* Store Byte, WriteBack, Pre Inc, Reg.  */
 	      if (BIT (4))
 		{
+#ifdef MODE32
+		  if (state->is_v6
+		      && handle_v6_insn (state, instr))
+		    break;
+#endif
 		  ARMul_UndefInstr (state, instr);
 		  break;
 		}
@@ -3279,6 +4058,11 @@ check_PMUintr:
 		      if (!ARMul_OSHandleSWI (state, SWI_Breakpoint))
 			ARMul_Abort (state, ARMul_SWIV);
 		    }
+#ifdef MODE32
+		  else if (state->is_v6
+			   && handle_v6_insn (state, instr))
+		    break;
+#endif
 		  else
 		    ARMul_UndefInstr (state, instr);
 		  break;
@@ -3513,8 +4297,10 @@ check_PMUintr:
 	    case 0xc4:
 	      if (state->is_v5)
 		{
+		  if (CPNum == 10 || CPNum == 11)
+		    handle_VFP_move (state, instr);
 		  /* Reading from R15 is UNPREDICTABLE.  */
-		  if (BITS (12, 15) == 15 || BITS (16, 19) == 15)
+		  else if (BITS (12, 15) == 15 || BITS (16, 19) == 15)
 		    ARMul_UndefInstr (state, instr);
 		  /* Is access to coprocessor 0 allowed ?  */
 		  else if (! CP_ACCESS_ALLOWED (state, CPNum))
@@ -3540,11 +4326,11 @@ check_PMUintr:
 		    }
 		  else
 		    /* FIXME: Not sure what to do for other v5 processors.  */
-		    ARMul_UndefInstr (state, instr);		    
+		    ARMul_UndefInstr (state, instr);
 		  break;
 		}
 	      /* Drop through.  */
-	      
+
 	    case 0xc0:		/* Store , No WriteBack , Post Dec.  */
 	      ARMul_STC (state, instr, LHS);
 	      break;
@@ -3552,8 +4338,10 @@ check_PMUintr:
 	    case 0xc5:
 	      if (state->is_v5)
 		{
+		  if (CPNum == 10 || CPNum == 11)
+		    handle_VFP_move (state, instr);
 		  /* Writes to R15 are UNPREDICATABLE.  */
-		  if (DESTReg == 15 || LHSReg == 15)
+		  else if (DESTReg == 15 || LHSReg == 15)
 		    ARMul_UndefInstr (state, instr);
 		  /* Is access to the coprocessor allowed ?  */
 		  else if (! CP_ACCESS_ALLOWED (state, CPNum))
@@ -3786,8 +4574,10 @@ check_PMUintr:
 	    case 0xee:
 	      if (BIT (4))
 		{
+		  if (CPNum == 10 || CPNum == 11)
+		    handle_VFP_move (state, instr);
 		  /* MCR.  */
-		  if (DESTReg == 15)
+		  else if (DESTReg == 15)
 		    {
 		      UNDEF_MCRPC;
 #ifdef MODE32
@@ -3817,17 +4607,69 @@ check_PMUintr:
 	    case 0xef:
 	      if (BIT (4))
 		{
-		  /* MRC */
-		  temp = ARMul_MRC (state, instr);
-		  if (DESTReg == 15)
+		  if (CPNum == 10 || CPNum == 11)
 		    {
-		      ASSIGNN ((temp & NBIT) != 0);
-		      ASSIGNZ ((temp & ZBIT) != 0);
-		      ASSIGNC ((temp & CBIT) != 0);
-		      ASSIGNV ((temp & VBIT) != 0);
+		      switch (BITS (20, 27))
+			{
+			case 0xEF:
+			  if (BITS (16, 19) == 0x1
+			      && BITS (0, 11) == 0xA10)
+			    {
+			      /* VMRS  */
+			      if (DESTReg == 15)
+				{
+				  ARMul_SetCPSR (state, (state->FPSCR & 0xF0000000)
+						 | (ARMul_GetCPSR (state) & 0x0FFFFFFF));
+
+				  if (trace)
+				    fprintf (stderr, " VFP: VMRS: set flags to %c%c%c%c\n",
+					     ARMul_GetCPSR (state) & NBIT ? 'N' : '-',
+					     ARMul_GetCPSR (state) & ZBIT ? 'Z' : '-',
+					     ARMul_GetCPSR (state) & CBIT ? 'C' : '-',
+					     ARMul_GetCPSR (state) & VBIT ? 'V' : '-');
+				}
+			      else
+				{
+				  state->Reg[DESTReg] = state->FPSCR;
+
+				  if (trace)
+				    fprintf (stderr, " VFP: VMRS: r%d = %x\n", DESTReg, state->Reg[DESTReg]);
+				}
+			    }
+			  else
+			    fprintf (stderr, "SIM: VFP: Unimplemented: Compare op\n");
+			  break;
+
+			case 0xE0:
+			case 0xE1:
+			  /* VMOV reg <-> single precision.  */
+			  if (BITS (0,6) != 0x10 || BITS (8,11) != 0xA)
+			    fprintf (stderr, "SIM: VFP: Unimplemented: move op\n");
+			  else if (BIT (20))
+			    state->Reg[BITS (12, 15)] = VFP_uword (BITS (16, 19) << 1 | BIT (7));
+			  else
+			    VFP_uword (BITS (16, 19) << 1 | BIT (7)) = state->Reg[BITS (12, 15)];
+			  break;
+
+			default:
+			  fprintf (stderr, "SIM: VFP: Unimplemented: CDP op\n");
+			  break;
+			}
 		    }
 		  else
-		    DEST = temp;
+		    {
+		      /* MRC */
+		      temp = ARMul_MRC (state, instr);
+		      if (DESTReg == 15)
+			{
+			  ASSIGNN ((temp & NBIT) != 0);
+			  ASSIGNZ ((temp & ZBIT) != 0);
+			  ASSIGNC ((temp & CBIT) != 0);
+			  ASSIGNV ((temp & VBIT) != 0);
+			}
+		      else
+			DEST = temp;
+		    }
 		}
 	      else
 		/* CDP Part 2.  */
@@ -4394,7 +5236,7 @@ Handle_Load_Double (ARMul_State * state, ARMword instr)
   ARMword addr_reg;
   ARMword write_back  = BIT (21);
   ARMword immediate   = BIT (22);
-  ARMword add_to_base = BIT (23);        
+  ARMword add_to_base = BIT (23);
   ARMword pre_indexed = BIT (24);
   ARMword offset;
   ARMword addr;
@@ -4402,7 +5244,7 @@ Handle_Load_Double (ARMul_State * state, ARMword instr)
   ARMword base;
   ARMword value1;
   ARMword value2;
-  
+
   BUSUSEDINCPCS;
 
   /* If the writeback bit is set, the pre-index bit must be clear.  */
@@ -4411,13 +5253,13 @@ Handle_Load_Double (ARMul_State * state, ARMword instr)
       ARMul_UndefInstr (state, instr);
       return;
     }
-  
+
   /* Extract the base address register.  */
   addr_reg = LHSReg;
-  
+
   /* Extract the destination register and check it.  */
   dest_reg = DESTReg;
-  
+
   /* Destination register must be even.  */
   if ((dest_reg & 1)
     /* Destination register cannot be LR.  */
@@ -4438,15 +5280,18 @@ Handle_Load_Double (ARMul_State * state, ARMword instr)
     sum = base + offset;
   else
     sum = base - offset;
-  
+
   /* If this is a pre-indexed mode use the sum.  */
   if (pre_indexed)
     addr = sum;
   else
     addr = base;
 
+  if (state->is_v6 && (addr & 0x3) == 0)
+    /* Word alignment is enough for v6.  */
+    ;
   /* The address must be aligned on a 8 byte boundary.  */
-  if (addr & 0x7)
+  else if (addr & 0x7)
     {
 #ifdef ABORTS
       ARMul_DATAABORT (addr);
@@ -4477,17 +5322,17 @@ Handle_Load_Double (ARMul_State * state, ARMword instr)
       TAKEABORT;
       return;
     }
-  
+
   ARMul_Icycles (state, 2, 0L);
 
   /* Store the values.  */
   state->Reg[dest_reg] = value1;
   state->Reg[dest_reg + 1] = value2;
-  
+
   /* Do the post addressing and writeback.  */
   if (! pre_indexed)
     addr = sum;
-  
+
   if (! pre_indexed || write_back)
     state->Reg[addr_reg] = addr;
 }
@@ -4501,7 +5346,7 @@ Handle_Store_Double (ARMul_State * state, ARMword instr)
   ARMword addr_reg;
   ARMword write_back  = BIT (21);
   ARMword immediate   = BIT (22);
-  ARMword add_to_base = BIT (23);        
+  ARMword add_to_base = BIT (23);
   ARMword pre_indexed = BIT (24);
   ARMword offset;
   ARMword addr;
@@ -4516,20 +5361,20 @@ Handle_Store_Double (ARMul_State * state, ARMword instr)
       ARMul_UndefInstr (state, instr);
       return;
     }
-  
+
   /* Extract the base address register.  */
   addr_reg = LHSReg;
-  
+
   /* Base register cannot be PC.  */
   if (addr_reg == 15)
     {
       ARMul_UndefInstr (state, instr);
       return;
     }
-  
+
   /* Extract the source register.  */
   src_reg = DESTReg;
-  
+
   /* Source register must be even.  */
   if (src_reg & 1)
     {
@@ -4548,7 +5393,7 @@ Handle_Store_Double (ARMul_State * state, ARMword instr)
     sum = base + offset;
   else
     sum = base - offset;
-  
+
   /* If this is a pre-indexed mode use the sum.  */
   if (pre_indexed)
     addr = sum;
@@ -4580,17 +5425,17 @@ Handle_Store_Double (ARMul_State * state, ARMword instr)
   /* Load the words.  */
   ARMul_StoreWordN (state, addr, state->Reg[src_reg]);
   ARMul_StoreWordN (state, addr + 4, state->Reg[src_reg + 1]);
-  
+
   if (state->Aborted)
     {
       TAKEABORT;
       return;
     }
-  
+
   /* Do the post addressing and writeback.  */
   if (! pre_indexed)
     addr = sum;
-  
+
   if (! pre_indexed || write_back)
     state->Reg[addr_reg] = addr;
 }
@@ -4987,7 +5832,7 @@ StoreSMult (ARMul_State * state,
 
   for (temp = 0; !BIT (temp); temp++)
     ;	/* N cycle first.  */
-  
+
 #ifdef MODE32
   ARMul_StoreWordN (state, address, state->Reg[temp++]);
 #else
@@ -5093,9 +5938,7 @@ Multiply64 (ARMul_State * state, ARMword instr, int msigned, int scc)
       && nRdLo != 15
       && nRs   != 15
       && nRm   != 15
-      && nRdHi != nRdLo
-      && nRdHi != nRm
-      && nRdLo != nRm)
+      && nRdHi != nRdLo)
     {
       /* Intermediate results.  */
       ARMword lo, mid1, mid2, hi;
@@ -5103,6 +5946,15 @@ Multiply64 (ARMul_State * state, ARMword instr, int msigned, int scc)
       ARMword Rs = state->Reg[nRs];
       int sign = 0;
 
+#ifdef MODE32
+      if (state->is_v6)
+	;
+      else
+#endif
+	if (nRdHi == nRm || nRdLo == nRm)
+	  fprintf (stderr, "sim: MULTIPLY64 - INVALID ARGUMENTS: %d %d %d\n",
+		   nRdHi, nRdLo, nRm);
+
       if (msigned)
 	{
 	  /* Compute sign of result and adjust operands if necessary.  */
diff --git a/sim/arm/armemu.h b/sim/arm/armemu.h
index 484cc85..7f25b94d 100644
--- a/sim/arm/armemu.h
+++ b/sim/arm/armemu.h
@@ -462,6 +462,8 @@ typedef enum
 }
 tdstate;
 
+#define t_resolved t_branch
+
 /* Macros to scrutinize instructions.  The dummy do loop is to keep the compiler
    happy when the statement is used in an otherwise empty else statement.  */
 #define UNDEF_Test		do { ; } while (0)
diff --git a/sim/arm/armos.c b/sim/arm/armos.c
index e5c218d..c222e2e 100644
--- a/sim/arm/armos.c
+++ b/sim/arm/armos.c
@@ -840,8 +840,7 @@ ARMul_OSHandleSWI (ARMul_State * state, ARMword number)
 	      break;
 
 	    case 17: /* Utime.  */
-	      state->Reg[0] = (ARMword) sim_callback->time (sim_callback,
-							    (long *) state->Reg[1]);
+	      state->Reg[0] = state->Reg[1] = (ARMword) sim_callback->time (sim_callback, NULL);
 	      OSptr->ErrorNo = sim_callback->get_errno (sim_callback);
 	      break;
 
diff --git a/sim/arm/armsupp.c b/sim/arm/armsupp.c
index 113f080..e2bbf53 100644
--- a/sim/arm/armsupp.c
+++ b/sim/arm/armsupp.c
@@ -1,22 +1,23 @@
 /*  armsupp.c -- ARMulator support code:  ARM6 Instruction Emulator.
     Copyright (C) 1994 Advanced RISC Machines Ltd.
- 
+
     This program is free software; you can redistribute it and/or modify
     it under the terms of the GNU General Public License as published by
     the Free Software Foundation; either version 3 of the License, or
     (at your option) any later version.
- 
+
     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.
- 
+
     You should have received a copy of the GNU General Public License
     along with this program; if not, see <http://www.gnu.org/licenses/>. */
 
 #include "armdefs.h"
 #include "armemu.h"
 #include "ansidecl.h"
+#include <math.h>
 
 /* Definitions for the support routines.  */
 
@@ -173,7 +174,7 @@ void
 ARMul_SetSPSR (ARMul_State * state, ARMword mode, ARMword value)
 {
   ARMword bank = ModeToBank (mode & MODEBITS);
-  
+
   if (BANK_CAN_ACCESS_SPSR (bank))
     state->Spsr[bank] = value;
 }
@@ -208,12 +209,12 @@ ARMul_CPSRAltered (ARMul_State * state)
     state->Cpsr &= (CCBITS | INTBITS | R15MODEBITS);
 
   oldmode = state->Mode;
-  
+
   if (state->Mode != (state->Cpsr & MODEBITS))
     {
       state->Mode =
 	ARMul_SwitchMode (state, state->Mode, state->Cpsr & MODEBITS);
-      
+
       state->NtransSig = (state->Mode & 3) ? HIGH : LOW;
     }
   state->Cpsr &= ~MODEBITS;
@@ -291,10 +292,10 @@ ARMul_SwitchMode (ARMul_State * state, ARMword oldmode, ARMword newmode)
   unsigned i;
   ARMword  oldbank;
   ARMword  newbank;
-  
+
   oldbank = ModeToBank (oldmode);
   newbank = state->Bank = ModeToBank (newmode);
-  
+
   /* Do we really need to do it?  */
   if (oldbank != newbank)
     {
@@ -323,7 +324,7 @@ ARMul_SwitchMode (ARMul_State * state, ARMword oldmode, ARMword newmode)
 	default:
 	  abort ();
 	}
-      
+
       /* Restore the new registers.  */
       switch (newbank)
 	{
@@ -350,7 +351,7 @@ ARMul_SwitchMode (ARMul_State * state, ARMword oldmode, ARMword newmode)
 	  abort ();
 	}
     }
-  
+
   return newmode;
 }
 
@@ -466,6 +467,422 @@ ARMul_SubOverflow (ARMul_State * state, ARMword a, ARMword b, ARMword result)
   ASSIGNV (SubOverflow (a, b, result));
 }
 
+static void
+handle_VFP_xfer (ARMul_State * state, ARMword instr)
+{
+  if (TOPBITS (28) == NV)
+    {
+      fprintf (stderr, "SIM: UNDEFINED VFP instruction\n");
+      return;
+    }
+
+  if (BITS (25, 27) != 0x6)
+    {
+      fprintf (stderr, "SIM: ISE: VFP handler called incorrectly\n");
+      return;
+    }
+	
+  switch (BITS (20, 24))
+    {
+    case 0x04:
+    case 0x05:
+      {
+	/* VMOV double precision to/from two ARM registers.  */
+	int vm  = BITS (0, 3);
+	int rt1 = BITS (12, 15);
+	int rt2 = BITS (16, 19);
+
+	/* FIXME: UNPREDICTABLE if rt1 == 15 or rt2 == 15.  */
+	if (BIT (20))
+	  {
+	    /* Transfer to ARM.  */
+	    /* FIXME: UPPREDICTABLE if rt1 == rt2.  */
+	    state->Reg[rt1] = VFP_dword (vm) & 0xffffffff;
+	    state->Reg[rt2] = VFP_dword (vm) >> 32;
+	  }
+	else
+	  {
+	    VFP_dword (vm) = state->Reg[rt2];
+	    VFP_dword (vm) <<= 32;
+	    VFP_dword (vm) |= (state->Reg[rt1] & 0xffffffff);
+	  }
+	return;
+      }
+
+    case 0x08:
+    case 0x0A:
+    case 0x0C:
+    case 0x0E:
+      {
+	/* VSTM with PUW=011 or PUW=010.  */
+	int n = BITS (16, 19);
+	int imm8 = BITS (0, 7);
+
+	ARMword address = state->Reg[n];
+	if (BIT (21))
+	  state->Reg[n] = address + (imm8 << 2);
+
+	if (BIT (8))
+	  {
+	    int src = (BIT (22) << 4) | BITS (12, 15);
+	    imm8 >>= 1;
+	    while (imm8--)
+	      {
+		if (state->bigendSig)
+		  {
+		    ARMul_StoreWordN (state, address, VFP_dword (src) >> 32);
+		    ARMul_StoreWordN (state, address + 4, VFP_dword (src));
+		  }
+		else
+		  {
+		    ARMul_StoreWordN (state, address, VFP_dword (src));
+		    ARMul_StoreWordN (state, address + 4, VFP_dword (src) >> 32);
+		  }
+		address += 8;
+		src += 1;
+	      }
+	  }
+	else
+	  {
+	    int src = (BITS (12, 15) << 1) | BIT (22);
+	    while (imm8--)
+	      {
+		ARMul_StoreWordN (state, address, VFP_uword (src));
+		address += 4;
+		src += 1;
+	      }
+	  }
+      }
+      return;
+
+    case 0x10:
+    case 0x14:
+    case 0x18:
+    case 0x1C:
+      {
+	/* VSTR */
+	ARMword imm32 = BITS (0, 7) << 2;
+	int base = state->Reg[LHSReg];
+	ARMword address;
+	int dest;
+
+	if (LHSReg == 15)
+	  base = (base + 3) & ~3;
+
+	address = base + (BIT (23) ? imm32 : - imm32);
+
+	if (CPNum == 10)
+	  {
+	    dest = (DESTReg << 1) + BIT (22);
+
+	    ARMul_StoreWordN (state, address, VFP_uword (dest));
+	  }
+	else
+	  {
+	    dest = (BIT (22) << 4) + DESTReg;
+
+	    if (state->bigendSig)
+	      {
+		ARMul_StoreWordN (state, address, VFP_dword (dest) >> 32);
+		ARMul_StoreWordN (state, address + 4, VFP_dword (dest));
+	      }
+	    else
+	      {
+		ARMul_StoreWordN (state, address, VFP_dword (dest));
+		ARMul_StoreWordN (state, address + 4, VFP_dword (dest) >> 32);
+	      }
+	  }
+      }
+      return;
+
+    case 0x12:
+    case 0x16:
+      if (BITS (16, 19) == 13)
+	{
+	  /* VPUSH */
+	  ARMword address = state->Reg[13] - (BITS (0, 7) << 2);
+	  state->Reg[13] = address;
+
+	  if (BIT (8))
+	    {
+	      int dreg = (BIT (22) << 4) | BITS (12, 15);
+	      int num  = BITS (0, 7) >> 1;
+	      while (num--)
+		{
+		  if (state->bigendSig)
+		    {
+		      ARMul_StoreWordN (state, address, VFP_dword (dreg) >> 32);
+		      ARMul_StoreWordN (state, address + 4, VFP_dword (dreg));
+		    }
+		  else
+		    {
+		      ARMul_StoreWordN (state, address, VFP_dword (dreg));
+		      ARMul_StoreWordN (state, address + 4, VFP_dword (dreg) >> 32);
+		    }
+		  address += 8;
+		  dreg += 1;
+		}
+	    }
+	  else
+	    {
+	      int sreg = (BITS (12, 15) << 1) | BIT (22);
+	      int num  = BITS (0, 7);
+	      while (num--)
+		{
+		  ARMul_StoreWordN (state, address, VFP_uword (sreg));
+		  address += 4;
+		  sreg += 1;
+		}
+	    }
+	}
+      else if (BITS (9, 11) != 0x5)
+	break;
+      else
+	{
+	  /* VSTM PUW=101 */
+	  int n = BITS (16, 19);
+	  int imm8 = BITS (0, 7);
+	  ARMword address = state->Reg[n] - (imm8 << 2);
+	  state->Reg[n] = address;
+
+	  if (BIT (8))
+	    {
+	      int src = (BIT (22) << 4) | BITS (12, 15);
+
+	      imm8 >>= 1;
+	      while (imm8--)
+		{
+		  if (state->bigendSig)
+		    {
+		      ARMul_StoreWordN (state, address, VFP_dword (src) >> 32);
+		      ARMul_StoreWordN (state, address + 4, VFP_dword (src));
+		    }
+		  else
+		    {
+		      ARMul_StoreWordN (state, address, VFP_dword (src));
+		      ARMul_StoreWordN (state, address + 4, VFP_dword (src) >> 32);
+		    }
+		  address += 8;
+		  src += 1;
+		}
+	    }
+	  else
+	    {
+	      int src = (BITS (12, 15) << 1) | BIT (22);
+
+	      while (imm8--)
+		{
+		  ARMul_StoreWordN (state, address, VFP_uword (src));
+		  address += 4;
+		  src += 1;
+		}
+	    }
+	}
+      return;
+
+    case 0x13:
+    case 0x17:
+      /* VLDM PUW=101 */
+    case 0x09:
+    case 0x0D:
+      /* VLDM PUW=010 */
+	{
+	  int n = BITS (16, 19);
+	  int imm8 = BITS (0, 7);
+
+	  ARMword address = state->Reg[n];
+	  if (BIT (23) == 0)
+	    address -= imm8 << 2;
+	  if (BIT (21))
+	    state->Reg[n] = BIT (23) ? address + (imm8 << 2) : address;
+
+	  if (BIT (8))
+	    {
+	      int dest = (BIT (22) << 4) | BITS (12, 15);
+	      imm8 >>= 1;
+	      while (imm8--)
+		{
+		  if (state->bigendSig)
+		    {
+		      VFP_dword (dest) = ARMul_LoadWordN (state, address);
+		      VFP_dword (dest) <<= 32;
+		      VFP_dword (dest) |= ARMul_LoadWordN (state, address + 4);
+		    }
+		  else
+		    {
+		      VFP_dword (dest) = ARMul_LoadWordN (state, address + 4);
+		      VFP_dword (dest) <<= 32;
+		      VFP_dword (dest) |= ARMul_LoadWordN (state, address);
+		    }
+
+		  if (trace)
+		    fprintf (stderr, " VFP: VLDM: D%d = %g\n", dest, VFP_dval (dest));
+
+		  address += 8;
+		  dest += 1;
+		}
+	    }
+	  else
+	    {
+	      int dest = (BITS (12, 15) << 1) | BIT (22);
+
+	      while (imm8--)
+		{
+		  VFP_uword (dest) = ARMul_LoadWordN (state, address);
+		  address += 4;
+		  dest += 1;
+		}
+	    }
+	}
+      return;
+
+    case 0x0B:
+    case 0x0F:
+      if (BITS (16, 19) == 13)
+	{
+	  /* VPOP */
+	  ARMword address = state->Reg[13];
+	  state->Reg[13] = address + (BITS (0, 7) << 2);
+
+	  if (BIT (8))
+	    {
+	      int dest = (BIT (22) << 4) | BITS (12, 15);
+	      int num  = BITS (0, 7) >> 1;
+
+	      while (num--)
+		{
+		  if (state->bigendSig)
+		    {
+		      VFP_dword (dest) = ARMul_LoadWordN (state, address);
+		      VFP_dword (dest) <<= 32;
+		      VFP_dword (dest) |= ARMul_LoadWordN (state, address + 4);
+		    }
+		  else
+		    {
+		      VFP_dword (dest) = ARMul_LoadWordN (state, address + 4);
+		      VFP_dword (dest) <<= 32;
+		      VFP_dword (dest) |= ARMul_LoadWordN (state, address);
+		    }
+
+		  if (trace)
+		    fprintf (stderr, " VFP: VPOP: D%d = %g\n", dest, VFP_dval (dest));
+
+		  address += 8;
+		  dest += 1;
+		}
+	    }
+	  else
+	    {
+	      int sreg = (BITS (12, 15) << 1) | BIT (22);
+	      int num  = BITS (0, 7);
+
+	      while (num--)
+		{
+		  VFP_uword (sreg) = ARMul_LoadWordN (state, address);
+		  address += 4;
+		  sreg += 1;
+		}
+	    }
+	}
+      else if (BITS (9, 11) != 0x5)
+	break;
+      else
+	{
+	  /* VLDM PUW=011 */
+	  int n = BITS (16, 19);
+	  int imm8 = BITS (0, 7);
+	  ARMword address = state->Reg[n];
+	  state->Reg[n] += imm8 << 2;
+
+	  if (BIT (8))
+	    {
+	      int dest = (BIT (22) << 4) | BITS (12, 15);
+
+	      imm8 >>= 1;
+	      while (imm8--)
+		{
+		  if (state->bigendSig)
+		    {
+		      VFP_dword (dest) = ARMul_LoadWordN (state, address);
+		      VFP_dword (dest) <<= 32;
+		      VFP_dword (dest) |= ARMul_LoadWordN (state, address + 4);
+		    }
+		  else
+		    {
+		      VFP_dword (dest) = ARMul_LoadWordN (state, address + 4);
+		      VFP_dword (dest) <<= 32;
+		      VFP_dword (dest) |= ARMul_LoadWordN (state, address);
+		    }
+
+		  if (trace)
+		    fprintf (stderr, " VFP: VLDM: D%d = %g\n", dest, VFP_dval (dest));
+
+		  address += 8;
+		  dest += 1;
+		}
+	    }
+	  else
+	    {
+	      int dest = (BITS (12, 15) << 1) | BIT (22);
+	      while (imm8--)
+		{
+		  VFP_uword (dest) = ARMul_LoadWordN (state, address);
+		  address += 4;
+		  dest += 1;
+		}
+	    }
+	}
+      return;
+
+    case 0x11:
+    case 0x15:
+    case 0x19:
+    case 0x1D:
+      {
+	/* VLDR */
+	ARMword imm32 = BITS (0, 7) << 2;
+	int base = state->Reg[LHSReg];
+	ARMword address;
+	int dest;
+
+	if (LHSReg == 15)
+	  base = (base + 3) & ~3;
+
+	address = base + (BIT (23) ? imm32 : - imm32);
+
+	if (CPNum == 10)
+	  {
+	    dest = (DESTReg << 1) + BIT (22);
+
+	    VFP_uword (dest) = ARMul_LoadWordN (state, address);
+	  }
+	else
+	  {
+	    dest = (BIT (22) << 4) + DESTReg;
+
+	    if (state->bigendSig)
+	      {
+		VFP_dword (dest) = ARMul_LoadWordN (state, address);
+		VFP_dword (dest) <<= 32;
+		VFP_dword (dest) |= ARMul_LoadWordN (state, address + 4);
+	      }
+	    else
+	      {
+		VFP_dword (dest) = ARMul_LoadWordN (state, address + 4);
+		VFP_dword (dest) <<= 32;
+		VFP_dword (dest) |= ARMul_LoadWordN (state, address);
+	      }
+
+	    if (trace)
+	      fprintf (stderr, " VFP: VLDR: D%d = %g\n", dest, VFP_dval (dest));
+	  }
+      }
+      return;
+    }
+
+  fprintf (stderr, "SIM: VFP: Unimplemented: %0x\n", BITS (20, 24));
+}
+
 /* This function does the work of generating the addresses used in an
    LDC instruction.  The code here is always post-indexed, it's up to the
    caller to get the input address correct and to handle base register
@@ -477,6 +894,12 @@ ARMul_LDC (ARMul_State * state, ARMword instr, ARMword address)
   unsigned cpab;
   ARMword data;
 
+  if (CPNum == 10 || CPNum == 11)
+    {
+      handle_VFP_xfer (state, instr);
+      return;
+    }
+
   UNDEF_LSCPCBaseWb;
 
   if (! CP_ACCESS_ALLOWED (state, CPNum))
@@ -537,6 +960,12 @@ ARMul_STC (ARMul_State * state, ARMword instr, ARMword address)
   unsigned cpab;
   ARMword data;
 
+  if (CPNum == 10 || CPNum == 11)
+    {
+      handle_VFP_xfer (state, instr);
+      return;
+    }
+
   UNDEF_LSCPCBaseWb;
 
   if (! CP_ACCESS_ALLOWED (state, CPNum))
@@ -666,6 +1095,454 @@ ARMul_MRC (ARMul_State * state, ARMword instr)
   return result;
 }
 
+static void
+handle_VFP_op (ARMul_State * state, ARMword instr)
+{
+  int dest;
+  int srcN;
+  int srcM;
+
+  if (BITS (9, 11) != 0x5 || BIT (4) != 0)
+    {
+      fprintf (stderr, "SIM: VFP: Unimplemented: Float op: %08x\n", BITS (0,31));
+      return;
+    }
+
+  if (BIT (8))
+    {
+      dest = BITS(12,15) + (BIT (22) << 4);
+      srcN = LHSReg  + (BIT (7) << 4);
+      srcM = BITS (0,3) + (BIT (5) << 4);
+    }
+  else
+    {
+      dest = (BITS(12,15) << 1) + BIT (22);
+      srcN = (LHSReg << 1) + BIT (7);
+      srcM = (BITS (0,3) << 1) + BIT (5);
+    }
+
+  switch (BITS (20, 27))
+    {
+    case 0xE0:
+    case 0xE4:
+      /* VMLA VMLS */
+      if (BIT (8))
+	{
+	  ARMdval val = VFP_dval (srcN) * VFP_dval (srcM);
+
+	  if (BIT (6))
+	    {
+	      if (trace)
+		fprintf (stderr, " VFP: VMLS: %g = %g - %g * %g\n",
+			 VFP_dval (dest) - val, 
+			 VFP_dval (dest), VFP_dval (srcN), VFP_dval (srcM));
+	      VFP_dval (dest) -= val;
+	    }
+	  else
+	    {
+	      if (trace)
+		fprintf (stderr, " VFP: VMLA: %g = %g + %g * %g\n",
+			 VFP_dval (dest) + val, 
+			 VFP_dval (dest), VFP_dval (srcN), VFP_dval (srcM));
+	      VFP_dval (dest) += val;
+	    }
+	}
+      else
+	{
+	  ARMfval val = VFP_fval (srcN) * VFP_fval (srcM);
+
+	  if (BIT (6))
+	    {
+	      if (trace)
+		fprintf (stderr, " VFP: VMLS: %g = %g - %g * %g\n",
+			 VFP_fval (dest) - val, 
+			 VFP_fval (dest), VFP_fval (srcN), VFP_fval (srcM));
+	      VFP_fval (dest) -= val;
+	    }
+	  else
+	    {
+	      if (trace)
+		fprintf (stderr, " VFP: VMLA: %g = %g + %g * %g\n",
+			 VFP_fval (dest) + val, 
+			 VFP_fval (dest), VFP_fval (srcN), VFP_fval (srcM));
+	      VFP_fval (dest) += val;
+	    }
+	}
+      return;
+
+    case 0xE1:
+    case 0xE5:
+      if (BIT (8))
+	{
+	  ARMdval product = VFP_dval (srcN) * VFP_dval (srcM);
+
+	  if (BIT (6))
+	    {
+	      /* VNMLA */
+	      if (trace)
+		fprintf (stderr, " VFP: VNMLA: %g = -(%g + (%g * %g))\n",
+			 -(VFP_dval (dest) + product),
+			 VFP_dval (dest), VFP_dval (srcN), VFP_dval (srcM));
+	      VFP_dval (dest) = -(product + VFP_dval (dest));
+	    }
+	  else
+	    {
+	      /* VNMLS */
+	      if (trace)
+		fprintf (stderr, " VFP: VNMLS: %g = -(%g + (%g * %g))\n",
+			 -(VFP_dval (dest) + product),
+			 VFP_dval (dest), VFP_dval (srcN), VFP_dval (srcM));
+	      VFP_dval (dest) = product - VFP_dval (dest);
+	    }
+	}
+      else
+	{
+	  ARMfval product = VFP_fval (srcN) * VFP_fval (srcM);
+
+	  if (BIT (6))
+	    /* VNMLA */
+	    VFP_fval (dest) = -(product + VFP_fval (dest));
+	  else
+	    /* VNMLS */
+	    VFP_fval (dest) = product - VFP_fval (dest);
+	}
+      return;
+
+    case 0xE2:
+    case 0xE6:
+      if (BIT (8))
+	{
+	  ARMdval product = VFP_dval (srcN) * VFP_dval (srcM);
+
+	  if (BIT (6))
+	    {
+	      if (trace)
+		fprintf (stderr, " VFP: VMUL: %g = %g * %g\n",
+			 - product, VFP_dval (srcN), VFP_dval (srcM));
+	      /* VNMUL */
+	      VFP_dval (dest) = - product;
+	    }
+	  else
+	    {
+	      if (trace)
+		fprintf (stderr, " VFP: VMUL: %g = %g * %g\n",
+			 product, VFP_dval (srcN), VFP_dval (srcM));
+	      /* VMUL */
+	      VFP_dval (dest) = product;
+	    }
+	}
+      else
+	{
+	  ARMfval product = VFP_fval (srcN) * VFP_fval (srcM);
+
+	  if (BIT (6))
+	    {
+	      if (trace)
+		fprintf (stderr, " VFP: VNMUL: %g = %g * %g\n",
+			 - product, VFP_fval (srcN), VFP_fval (srcM));
+
+	      VFP_fval (dest) = - product;
+	    }
+	  else
+	    {
+	      if (trace)
+		fprintf (stderr, " VFP: VMUL: %g = %g * %g\n",
+			 product, VFP_fval (srcN), VFP_fval (srcM));
+
+	      VFP_fval (dest) = product;
+	    }
+	}
+      return;
+	
+    case 0xE3:
+    case 0xE7:
+      if (BIT (6) == 0)
+	{
+	  /* VADD */
+	  if (BIT(8))
+	    {
+	      if (trace)
+		fprintf (stderr, " VFP: VADD %g = %g + %g\n",
+			 VFP_dval (srcN) + VFP_dval (srcM),
+			 VFP_dval (srcN),
+			 VFP_dval (srcM));
+	      VFP_dval (dest) = VFP_dval (srcN) + VFP_dval (srcM);
+	    }
+	  else
+	    VFP_fval (dest) = VFP_fval (srcN) + VFP_fval (srcM);
+
+	}
+      else
+	{
+	  /* VSUB */
+	  if (BIT(8))
+	    {
+	      if (trace)
+		fprintf (stderr, " VFP: VSUB %g = %g - %g\n",
+			 VFP_dval (srcN) - VFP_dval (srcM),
+			 VFP_dval (srcN),
+			 VFP_dval (srcM));
+	      VFP_dval (dest) = VFP_dval (srcN) - VFP_dval (srcM);
+	    }
+	  else
+	    VFP_fval (dest) = VFP_fval (srcN) - VFP_fval (srcM);
+	}
+      return;
+
+    case 0xE8:
+    case 0xEC:
+      if (BIT (6) == 1)
+	break;
+
+      /* VDIV */
+      if (BIT (8))
+	{
+	  ARMdval res = VFP_dval (srcN) / VFP_dval (srcM);
+	  if (trace)
+	    fprintf (stderr, " VFP: VDIV (64bit): %g = %g / %g\n",
+		     res, VFP_dval (srcN), VFP_dval (srcM));
+	  VFP_dval (dest) = res;
+	}
+      else
+	{
+	  if (trace)
+	    fprintf (stderr, " VFP: VDIV: %g = %g / %g\n",
+		     VFP_fval (srcN) / VFP_fval (srcM),
+		     VFP_fval (srcN), VFP_fval (srcM));
+
+	  VFP_fval (dest) = VFP_fval (srcN) / VFP_fval (srcM);
+	}
+      return;
+
+    case 0xEB:
+    case 0xEF:
+      if (BIT (6) != 1)
+	break;
+
+      switch (BITS (16, 19))
+	{
+	case 0x0:
+	  if (BIT (7) == 0)
+	    {
+	      if (BIT (8))
+		{
+		  /* VMOV.F64 <Dd>, <Dm>.  */
+		  VFP_dval (dest) = VFP_dval (srcM);
+		  if (trace)
+		    fprintf (stderr, " VFP: VMOV d%d, d%d: %g\n", dest, srcM, VFP_dval (srcM));
+		}
+	      else
+		{
+		  /* VMOV.F32 <Sd>, <Sm>.  */
+		  VFP_fval (dest) = VFP_fval (srcM);
+		  if (trace)
+		    fprintf (stderr, " VFP: VMOV s%d, s%d: %g\n", dest, srcM, VFP_fval (srcM));
+		}
+	    }
+	  else
+	    {
+	      /* VABS */
+	      if (BIT (8))
+		{
+		  ARMdval src = VFP_dval (srcM);
+		  
+		  VFP_dval (dest) = fabs (src);
+		  if (trace)
+		    fprintf (stderr, " VFP: VABS (%g) = %g\n", src, VFP_dval (dest));
+		}
+	      else
+		{
+		  ARMfval src = VFP_fval (srcM);
+
+		  VFP_fval (dest) = fabsf (src);
+		  if (trace)
+		    fprintf (stderr, " VFP: VABS (%g) = %g\n", src, VFP_fval (dest));
+		}
+	    }
+	  return;
+
+	case 0x1:
+	  if (BIT (7) == 0)
+	    {
+	      /* VNEG */
+	      if (BIT (8))
+		VFP_dval (dest) = - VFP_dval (srcM);
+	      else
+		VFP_fval (dest) = - VFP_fval (srcM);
+	    }
+	  else
+	    {
+	      /* VSQRT */
+	      if (BIT (8))
+		{
+		  if (trace)
+		    fprintf (stderr, " VFP: %g = root(%g)\n",
+			     sqrt (VFP_dval (srcM)), VFP_dval (srcM));
+
+		  VFP_dval (dest) = sqrt (VFP_dval (srcM));
+		}
+	      else
+		{
+		  if (trace)
+		    fprintf (stderr, " VFP: %g = root(%g)\n",
+			     sqrtf (VFP_fval (srcM)), VFP_fval (srcM));
+
+		  VFP_fval (dest) = sqrtf (VFP_fval (srcM));
+		}
+	    }
+	  return;
+
+	case 0x4:
+	case 0x5:
+	  /* VCMP, VCMPE */
+	  if (BIT(8))
+	    {
+	      ARMdval res = VFP_dval (dest);
+
+	      if (BIT (16) == 0)
+		{
+		  ARMdval src = VFP_dval (srcM);
+		  
+		  if (isinf (res) && isinf (src))
+		    {
+		      if (res > 0.0 && src > 0.0)
+			res = 0.0;
+		      else if (res < 0.0 && src < 0.0)
+			res = 0.0;
+		      /* else leave res alone.   */
+		    }
+		  else
+		    res -= src;
+		}
+
+	      /* FIXME: Add handling of signalling NaNs and the E bit.  */
+
+	      state->FPSCR &= 0x0FFFFFFF;
+	      if (res < 0.0)
+		state->FPSCR |= NBIT;
+	      else
+		state->FPSCR |= CBIT;
+	      if (res == 0.0)
+		state->FPSCR |= ZBIT;
+	      if (isnan (res))
+		state->FPSCR |= VBIT;
+
+	      if (trace)
+		fprintf (stderr, " VFP: VCMP (64bit) %g vs %g res %g, flags: %c%c%c%c\n",
+			 VFP_dval (dest), BIT (16) ? 0.0 : VFP_dval (srcM), res,
+			 state->FPSCR & NBIT ? 'N' : '-',
+			 state->FPSCR & ZBIT ? 'Z' : '-',
+			 state->FPSCR & CBIT ? 'C' : '-',
+			 state->FPSCR & VBIT ? 'V' : '-');
+	    }
+	  else
+	    {
+	      ARMfval res = VFP_fval (dest);
+
+	      if (BIT (16) == 0)
+		{
+		  ARMfval src = VFP_fval (srcM);
+		  
+		  if (isinf (res) && isinf (src))
+		    {
+		      if (res > 0.0 && src > 0.0)
+			res = 0.0;
+		      else if (res < 0.0 && src < 0.0)
+			res = 0.0;
+		      /* else leave res alone.   */
+		    }
+		  else
+		    res -= src;
+		}
+
+	      /* FIXME: Add handling of signalling NaNs and the E bit.  */
+
+	      state->FPSCR &= 0x0FFFFFFF;
+	      if (res < 0.0)
+		state->FPSCR |= NBIT;
+	      else
+		state->FPSCR |= CBIT;
+	      if (res == 0.0)
+		state->FPSCR |= ZBIT;
+	      if (isnan (res))
+		state->FPSCR |= VBIT;
+
+	      if (trace)
+		fprintf (stderr, " VFP: VCMP (32bit) %g vs %g res %g, flags: %c%c%c%c\n",
+			 VFP_fval (dest), BIT (16) ? 0.0 : VFP_fval (srcM), res,
+			 state->FPSCR & NBIT ? 'N' : '-',
+			 state->FPSCR & ZBIT ? 'Z' : '-',
+			 state->FPSCR & CBIT ? 'C' : '-',
+			 state->FPSCR & VBIT ? 'V' : '-');
+	    }
+	  return;
+
+	case 0x7:
+	  if (BIT (8))
+	    {
+	      dest = (DESTReg << 1) + BIT (22);
+	      VFP_fval (dest) = VFP_dval (srcM);
+	    }
+	  else
+	    {
+	      dest = DESTReg + (BIT (22) << 4);
+	      VFP_dval (dest) = VFP_fval (srcM);
+	    }
+	  return;
+
+	case 0x8:
+	case 0xC:
+	case 0xD:
+	  /* VCVT integer <-> FP */
+	  if (BIT (18))
+	    {
+	      /* To integer.  */
+	      if (BIT (8))
+		{
+		  dest = (BITS(12,15) << 1) + BIT (22);
+		  if (BIT (16))
+		    VFP_sword (dest) = VFP_dval (srcM);
+		  else
+		    VFP_uword (dest) = VFP_dval (srcM);
+		}
+	      else
+		{
+		  if (BIT (16))
+		    VFP_sword (dest) = VFP_fval (srcM);
+		  else
+		    VFP_uword (dest) = VFP_fval (srcM);
+		}
+	    }
+	  else
+	    {
+	      /* From integer.  */
+	      if (BIT (8))
+		{
+		  srcM = (BITS (0,3) << 1) + BIT (5);
+		  if (BIT (7))
+		    VFP_dval (dest) = VFP_sword (srcM);
+		  else
+		    VFP_dval (dest) = VFP_uword (srcM);
+		}
+	      else
+		{
+		  if (BIT (7))
+		    VFP_fval (dest) = VFP_sword (srcM);
+		  else
+		    VFP_fval (dest) = VFP_uword (srcM);
+		}
+	    }
+	  return;
+	}
+
+      fprintf (stderr, "SIM: VFP: Unimplemented: Float op3: %03x\n", BITS (16,27));
+      return;
+    }
+
+  fprintf (stderr, "SIM: VFP: Unimplemented: Float op2: %02x\n", BITS (20, 27));
+  return;
+}
+
 /* This function does the Busy-Waiting for an CDP instruction.  */
 
 void
@@ -673,6 +1550,12 @@ ARMul_CDP (ARMul_State * state, ARMword instr)
 {
   unsigned cpab;
 
+  if (CPNum == 10 || CPNum == 11)
+    {
+      handle_VFP_op (state, instr);
+      return;
+    }
+
   if (! CP_ACCESS_ALLOWED (state, CPNum))
     {
       ARMul_UndefInstr (state, instr);
diff --git a/sim/arm/thumbemu.c b/sim/arm/thumbemu.c
index e4c91f6..40c365e 100644
--- a/sim/arm/thumbemu.c
+++ b/sim/arm/thumbemu.c
@@ -30,12 +30,1788 @@ existing ARM simulator.  */
 #include "armemu.h"
 #include "armos.h"
 
+#define tBIT(n)    ( (ARMword)(tinstr >> (n)) & 1)
+#define tBITS(m,n) ( (ARMword)(tinstr << (31 - (n))) >> ((31 - (n)) + (m)) )
+
+#define ntBIT(n)    ( (ARMword)(next_instr >> (n)) & 1)
+#define ntBITS(m,n) ( (ARMword)(next_instr << (31 - (n))) >> ((31 - (n)) + (m)) )
+
+static int
+test_cond (int cond, ARMul_State * state)
+{
+  switch (cond)
+    {
+    case EQ: return ZFLAG;
+    case NE: return !ZFLAG;
+    case VS: return VFLAG;
+    case VC: return !VFLAG;
+    case MI: return NFLAG;
+    case PL: return !NFLAG;
+    case CS: return CFLAG;
+    case CC: return !CFLAG;
+    case HI: return (CFLAG && !ZFLAG);
+    case LS: return (!CFLAG || ZFLAG);
+    case GE: return ((!NFLAG && !VFLAG) || (NFLAG && VFLAG));
+    case LT: return ((NFLAG && !VFLAG) || (!NFLAG && VFLAG));
+    case GT: return ((!NFLAG && !VFLAG && !ZFLAG)
+		     || (NFLAG && VFLAG && !ZFLAG));
+    case LE: return ((NFLAG && !VFLAG) || (!NFLAG && VFLAG)) || ZFLAG;
+    case AL: return TRUE;
+    case NV:
+    default: return FALSE;
+    }
+}
+
+static ARMword skipping_32bit_thumb = 0;
+
+static int     IT_block_cond = AL;
+static ARMword IT_block_mask = 0;
+static int     IT_block_first = FALSE;
+
+static void
+handle_IT_block (ARMul_State * state,
+		 ARMword       tinstr,
+		 tdstate *     pvalid)
+{
+  * pvalid = t_branch;
+  IT_block_mask = tBITS (0, 3);
+
+  if (IT_block_mask == 0)
+    // NOP or a HINT.
+    return;
+
+  IT_block_cond = tBITS (4, 7);
+  IT_block_first = TRUE;
+}
+
+static int
+in_IT_block (void)
+{
+  return IT_block_mask != 0;
+}
+
+static int
+IT_block_allow (ARMul_State * state)
+{
+  int cond;
+
+  if (IT_block_mask == 0)
+    return TRUE;
+
+  cond = IT_block_cond;
+
+  if (IT_block_first)
+    IT_block_first = FALSE;
+  else
+    {
+      if ((IT_block_mask & 8) == 0)
+	cond &= 0xe;
+      else
+	cond |= 1;
+      IT_block_mask <<= 1;
+      IT_block_mask &= 0xF;
+    }
+
+  if (IT_block_mask == 0x8)
+    IT_block_mask = 0;
+
+  return test_cond (cond, state);
+}
+
+static ARMword
+ThumbExpandImm (ARMword tinstr)
+{
+  ARMword val;
+
+  if (tBITS (10, 11) == 0)
+    {
+      switch (tBITS (8, 9))
+	{
+	case 0:	 val = tBITS (0, 7); break;
+	case 1:	 val = tBITS (0, 7) << 8; break;
+	case 2:  val = (tBITS (0, 7) << 8) | (tBITS (0, 7) << 24); break;
+	case 3:  val = tBITS (0, 7) * 0x01010101; break;
+	default: val = 0;
+	}
+    }
+  else
+    {
+      int ror = tBITS (7, 11);
+      
+      val = (1 << 7) | tBITS (0, 6);
+      val = (val >> ror) | (val << (32 - ror));
+    }
+
+  return val;
+}
+
+#define tASSERT(truth)							\
+  do									\
+    {									\
+      if (! (truth))							\
+	{								\
+	  fprintf (stderr, "unhandled T2 insn %04x|%04x detected at thumbemu.c:%d\n", \
+		   tinstr, next_instr, __LINE__);			\
+	  return ;							\
+	}								\
+    }									\
+  while (0)
+
+
+/* Attempt to emulate a 32-bit ARMv7 Thumb instruction.
+   Stores t_branch into PVALUE upon success or t_undefined otherwise.  */
+
+static void
+handle_T2_insn (ARMul_State * state,
+		ARMword       tinstr,
+		ARMword       next_instr,
+		ARMword       pc,
+		ARMword *     ainstr,
+		tdstate *     pvalid)
+{
+  * pvalid = t_undefined;
+
+  if (! state->is_v6)
+    return;
+
+  if (trace)
+    fprintf (stderr, "|%04x ", next_instr);
+
+  if (tBITS (11, 15) == 0x1E && ntBIT (15) == 1)
+    {
+      ARMsword simm32 = 0;
+      int S = tBIT (10);
+
+      * pvalid = t_branch;
+      switch ((ntBIT (14) << 1) | ntBIT (12))
+	{
+	case 0: /* B<c>.W  */
+	  {
+	    ARMword cond = tBITS (6, 9);
+	    ARMword imm6;
+	    ARMword imm11;
+	    ARMword J1;
+	    ARMword J2;
+
+	    tASSERT (cond != AL && cond != NV);
+	    if (! test_cond (cond, state))
+	      return;
+
+	    imm6 = tBITS (0, 5);
+	    imm11 = ntBITS (0, 10);
+	    J1 = ntBIT (13);
+	    J2 = ntBIT (11);
+
+	    simm32 = (J1 << 19) | (J2 << 18) | (imm6 << 12) | (imm11 << 1);
+	    if (S)
+	      simm32 |= (-1 << 20);
+	    break;
+	  }
+	  
+	case 1: /* B.W  */
+	  {
+	    ARMword imm10 = tBITS (0, 9);
+	    ARMword imm11 = ntBITS (0, 10);
+	    ARMword I1 = (ntBIT (13) ^ S) ? 0 : 1;
+	    ARMword I2 = (ntBIT (11) ^ S) ? 0 : 1;
+
+	    simm32 = (I1 << 23) | (I2 << 22) | (imm10 << 12) | (imm11 << 1);
+	    if (S)
+	      simm32 |= (-1 << 24);
+	    break;
+	  }
+	  
+	case 2: /* BLX <label>  */
+	  {
+	    ARMword imm10h = tBITS (0, 9);
+	    ARMword imm10l = ntBITS (1, 10);
+	    ARMword I1 = (ntBIT (13) ^ S) ? 0 : 1;
+	    ARMword I2 = (ntBIT (11) ^ S) ? 0 : 1;
+
+	    simm32 = (I1 << 23) | (I2 << 22) | (imm10h << 12) | (imm10l << 2);
+	    if (S)
+	      simm32 |= (-1 << 24);
+
+	    CLEART;
+	    state->Reg[14] = (pc + 4) | 1;
+	    break;
+	  }
+
+	case 3: /* BL <label>  */
+	  {
+	    ARMword imm10 = tBITS (0, 9);
+	    ARMword imm11 = ntBITS (0, 10);
+	    ARMword I1 = (ntBIT (13) ^ S) ? 0 : 1;
+	    ARMword I2 = (ntBIT (11) ^ S) ? 0 : 1;
+
+	    simm32 = (I1 << 23) | (I2 << 22) | (imm10 << 12) | (imm11 << 1);
+	    if (S)
+	      simm32 |= (-1 << 24);
+	    state->Reg[14] = (pc + 4) | 1;
+	    break;
+	  }
+	}
+
+      state->Reg[15] = (pc + 4 + simm32);
+      FLUSHPIPE;
+      if (trace_funcs)
+	fprintf (stderr, " pc changed to %x\n", state->Reg[15]);
+      return;
+    }
+  
+  switch (tBITS (5,12))
+    {
+    case 0x29:  // TST<c>.W <Rn>,<Rm>{,<shift>}
+      {
+	ARMword Rn = tBITS (0, 3);
+	ARMword Rm = ntBITS (0, 3);
+	ARMword type = ntBITS (4, 5);
+	ARMword imm5 = (ntBITS (12, 14) << 2) | ntBITS (6, 7);
+
+	tASSERT (ntBITS (8, 11) == 0xF);
+
+	* ainstr = 0xE1100000;
+	* ainstr |= (Rn << 16);
+	* ainstr |= (Rm);
+	* ainstr |= (type << 5);
+	* ainstr |= (imm5 << 7);
+	* pvalid = t_decoded;
+	break;
+      }
+
+    case 0x46:
+      if (tBIT (4) && ntBITS (5, 15) == 0x780)
+	{
+	  // Table Branch
+	  ARMword Rn = tBITS (0, 3);
+	  ARMword Rm = ntBITS (0, 3);
+	  ARMword address, dest;
+
+	  if (ntBIT (4))
+	    {
+	      // TBH
+	      address = state->Reg[Rn] + state->Reg[Rm] * 2;
+	      dest = ARMul_LoadHalfWord (state, address);
+	    }
+	  else
+	    {
+	      // TBB
+	      address = state->Reg[Rn] + state->Reg[Rm];
+	      dest = ARMul_LoadByte (state, address);
+	    }
+
+	  state->Reg[15] = (pc + 4 + dest * 2);
+	  FLUSHPIPE;
+	  * pvalid = t_branch;
+	  break;
+	}
+      /* Fall through.  */
+    case 0x42:
+    case 0x43:
+    case 0x47:
+    case 0x4A:
+    case 0x4B:
+    case 0x4E: // STRD
+    case 0x4F: // LDRD
+      {
+	ARMword Rn = tBITS (0, 3);
+	ARMword Rt = ntBITS (12, 15);
+	ARMword Rt2 = ntBITS (8, 11);
+	ARMword imm8 = ntBITS (0, 7);
+	ARMword P = tBIT (8);
+	ARMword U = tBIT (7);
+	ARMword W = tBIT (5);
+
+	tASSERT (Rt2 == Rt + 1);
+	imm8 <<= 2;
+	tASSERT (imm8 <= 255);
+	tASSERT (P != 0 || W != 0);
+
+	// Convert into an ARM A1 encoding.
+	if (Rn == 15)
+	  {
+	    tASSERT (tBIT (4) == 1);
+	    // LDRD (literal)
+	    // Ignore W even if 1.
+	    * ainstr = 0xE14F00D0;
+	  }
+	else
+	  {
+	    if (tBIT (4) == 1)
+	      // LDRD (immediate)
+	      * ainstr = 0xE04000D0;
+	    else
+	      {
+		// STRD<c> <Rt>,<Rt2>,[<Rn>{,#+/-<imm8>}]
+		// STRD<c> <Rt>,<Rt2>,[<Rn>],#+/-<imm8>
+		// STRD<c> <Rt>,<Rt2>,[<Rn>,#+/-<imm8>]!
+		* ainstr = 0xE04000F0;
+	      }
+	    * ainstr |= (Rn << 16);
+	    * ainstr |= (P << 24);
+	    * ainstr |= (W << 21);
+	  }
+
+	* ainstr |= (U << 23);
+	* ainstr |= (Rt << 12);
+	* ainstr |= ((imm8 << 4) & 0xF00);
+	* ainstr |= (imm8 & 0xF);
+	* pvalid = t_decoded;
+	break;
+      }
+
+    case 0x44:
+    case 0x45: // LDMIA
+      {
+	ARMword Rn   = tBITS (0, 3);
+	int     W    = tBIT (5);
+	ARMword list = (ntBIT (15) << 15) | (ntBIT (14) << 14) | ntBITS (0, 12);
+
+	if (Rn == 13)
+	  * ainstr = 0xE8BD0000;
+	else
+	  {
+	    * ainstr = 0xE8900000;
+	    * ainstr |= (W << 21);
+	    * ainstr |= (Rn << 16);
+	  }
+	* ainstr |= list;
+	* pvalid = t_decoded;
+	break;
+      }
+
+    case 0x48:
+    case 0x49: // STMDB
+      {
+	ARMword Rn   = tBITS (0, 3);
+	int     W    = tBIT (5);
+	ARMword list = (ntBIT (14) << 14) | ntBITS (0, 12);
+
+	if (Rn == 13 && W)
+	  * ainstr = 0xE92D0000;
+	else
+	  {
+	    * ainstr = 0xE9000000;
+	    * ainstr |= (W << 21);
+	    * ainstr |= (Rn << 16);
+	  }
+	* ainstr |= list;
+	* pvalid = t_decoded;
+	break;
+      }
+
+    case 0x50: 
+      {
+	ARMword Rd = ntBITS (8, 11);
+	ARMword Rn = tBITS (0, 3);
+	ARMword Rm = ntBITS (0, 3);
+	ARMword imm5 = (ntBITS (12, 14) << 2) | ntBITS (6, 7);
+	ARMword type = ntBITS (4, 5);
+
+	tASSERT (ntBIT (15) == 0);
+
+	if (Rd == 15)
+	  {
+	    tASSERT (tBIT (4) == 1);
+
+	    // TST<c>.W <Rn>,<Rm>{,<shift>}
+	    * ainstr = 0xE1100000;
+	  }
+	else
+	  {
+	    // AND{S}<c>.W <Rd>,<Rn>,<Rm>{,<shift>}
+	    int S = tBIT (4);
+
+	    * ainstr = 0xE0000000;
+
+	    if (in_IT_block ())
+	      S = 0;
+	    * ainstr |= (S << 20);
+	  }
+	
+	* ainstr |= (Rn << 16);
+	* ainstr |= (imm5 << 7);
+	* ainstr |= (type << 5);
+	* ainstr |= (Rm << 0);
+	* pvalid = t_decoded;
+	break;
+      }
+      
+    case 0x51: // BIC{S}<c>.W <Rd>,<Rn>,<Rm>{,<shift>}
+      {
+	ARMword Rn = tBITS (0, 3);
+	ARMword S  = tBIT(4);
+	ARMword Rm = ntBITS (0, 3);
+	ARMword Rd = ntBITS (8, 11);
+	ARMword imm5 = (ntBITS (12, 14) << 2) | ntBITS (6, 7);
+	ARMword type = ntBITS (4, 5);
+	
+	tASSERT (ntBIT (15) == 0);
+
+	* ainstr = 0xE1C00000;
+	* ainstr |= (S << 20);
+	* ainstr |= (Rn << 16);
+	* ainstr |= (Rd << 12);
+	* ainstr |= (imm5 << 7);
+	* ainstr |= (type << 5);
+	* ainstr |= (Rm << 0);
+	* pvalid = t_decoded;
+	break;
+      }
+      
+    case 0x52: 
+      {
+	ARMword Rn = tBITS (0, 3);
+	ARMword Rd = ntBITS (8, 11);
+	ARMword Rm = ntBITS (0, 3);
+	int S = tBIT (4);
+	ARMword imm5 = (ntBITS (12, 14) << 2) | ntBITS (6, 7);
+	ARMword type = ntBITS (4, 5);
+
+	tASSERT (Rd != 15);
+
+	if (in_IT_block ())
+	  S = 0;
+
+	if (Rn == 15)
+	  {
+	    tASSERT (ntBIT (15) == 0);
+
+	    switch (ntBITS (4, 5))
+	      {
+	      case 0:
+		// LSL{S}<c>.W <Rd>,<Rm>,#<imm5>
+		* ainstr = 0xE1A00000;
+		break;
+	      case 1:
+		// LSR{S}<c>.W <Rd>,<Rm>,#<imm>
+		* ainstr = 0xE1A00020;
+		break;
+	      case 2:
+		// ASR{S}<c>.W <Rd>,<Rm>,#<imm>
+		* ainstr = 0xE1A00040;
+		break;
+	      case 3:
+		// ROR{S}<c> <Rd>,<Rm>,#<imm>
+		* ainstr = 0xE1A00060;
+		break;
+	      default:
+		tASSERT (0);
+		* ainstr = 0;
+	      }
+	  }
+	else
+	  {
+	    // ORR{S}<c>.W <Rd>,<Rn>,<Rm>{,<shift>}
+	    * ainstr = 0xE1800000;
+	    * ainstr |= (Rn << 16);
+	    * ainstr |= (type << 5);
+	  }
+	
+	* ainstr |= (Rd << 12);
+	* ainstr |= (S << 20);
+	* ainstr |= (imm5 << 7);
+	* ainstr |= (Rm <<  0);
+	* pvalid = t_decoded;
+	break;
+      }
+
+    case 0x53: // MVN{S}<c>.W <Rd>,<Rm>{,<shift>}
+      {
+	ARMword Rd = ntBITS (8, 11);
+	ARMword Rm = ntBITS (0, 3);
+	int S = tBIT (4);
+	ARMword imm5 = (ntBITS (12, 14) << 2) | ntBITS (6, 7);
+	ARMword type = ntBITS (4, 5);
+
+	tASSERT (ntBIT (15) == 0);
+
+	if (in_IT_block ())
+	  S = 0;
+
+	* ainstr = 0xE1E00000;
+	* ainstr |= (S << 20);
+	* ainstr |= (Rd << 12);
+	* ainstr |= (imm5 << 7);
+	* ainstr |= (type << 5);
+	* ainstr |= (Rm <<  0);
+	* pvalid = t_decoded;
+	break;
+      }
+
+    case 0x54: 
+      {
+	ARMword Rn = tBITS (0, 3);
+	ARMword Rd = ntBITS (8, 11);
+	ARMword Rm = ntBITS (0, 3);
+	int S = tBIT (4);
+	ARMword imm5 = (ntBITS (12, 14) << 2) | ntBITS (6, 7);
+	ARMword type = ntBITS (4, 5);
+
+	if (Rd == 15 && S)
+	  {
+	    // TEQ<c> <Rn>,<Rm>{,<shift>}
+	    tASSERT (ntBIT (15) == 0);
+
+	    * ainstr = 0xE1300000;
+	  }
+	else
+	  {
+	    // EOR{S}<c>.W <Rd>,<Rn>,<Rm>{,<shift>}
+	    if (in_IT_block ())
+	      S = 0;
+
+	    * ainstr = 0xE0200000;
+	    * ainstr |= (S << 20);
+	    * ainstr |= (Rd << 8);
+	  }
+
+	* ainstr |= (Rn <<  16);
+	* ainstr |= (imm5 << 7);
+	* ainstr |= (type << 5);
+	* ainstr |= (Rm <<  0);
+	* pvalid = t_decoded;
+	break;
+      }
+
+    case 0x58: // ADD{S}<c>.W <Rd>,<Rn>,<Rm>{,<shift>}
+      {
+	ARMword Rn = tBITS (0, 3);
+	ARMword Rd = ntBITS (8, 11);
+	ARMword Rm = ntBITS (0, 3);
+	int S = tBIT (4);
+	ARMword imm5 = (ntBITS (12, 14) << 2) | ntBITS (6, 7);
+	ARMword type = ntBITS (4, 5);
+	
+	tASSERT (! (Rd == 15 && S));
+
+	if (in_IT_block ())
+	  S = 0;
+
+	* ainstr = 0xE0800000;
+	* ainstr |= (S << 20);
+	* ainstr |= (Rn << 16);
+	* ainstr |= (Rd << 12);
+	* ainstr |= (imm5 << 7);
+	* ainstr |= (type << 5);
+	* ainstr |= Rm;
+	* pvalid = t_decoded;
+	break;
+      }
+
+    case 0x5A: // ADC{S}<c>.W <Rd>,<Rn>,<Rm>{,<shift>}
+      tASSERT (ntBIT (15) == 0);
+      * ainstr = 0xE0A00000;
+      if (! in_IT_block ())
+	* ainstr |= (tBIT (4) << 20); // S
+      * ainstr |= (tBITS (0, 3) << 16); // Rn
+      * ainstr |= (ntBITS (8, 11) << 12); // Rd
+      * ainstr |= ((ntBITS (12, 14) << 2) | ntBITS (6, 7)) << 7; // imm5
+      * ainstr |= (ntBITS (4, 5) << 5); // type
+      * ainstr |= ntBITS (0, 3); // Rm
+      * pvalid = t_decoded;
+      break;
+      
+    case 0x5B: // SBC{S}<c>.W <Rd>,<Rn>,<Rm>{,<shift>}
+      {
+	ARMword Rn = tBITS (0, 3);
+	ARMword Rd = ntBITS (8, 11);
+	ARMword Rm = ntBITS (0, 3);
+	int S = tBIT (4);
+	ARMword imm5 = (ntBITS (12, 14) << 2) | ntBITS (6, 7);
+	ARMword type = ntBITS (4, 5);
+	
+	tASSERT (ntBIT (15) == 0);
+
+	if (in_IT_block ())
+	  S = 0;
+
+	* ainstr = 0xE0C00000;
+	* ainstr |= (S << 20);
+	* ainstr |= (Rn << 16);
+	* ainstr |= (Rd << 12);
+	* ainstr |= (imm5 << 7);
+	* ainstr |= (type << 5);
+	* ainstr |= Rm;
+	* pvalid = t_decoded;
+	break;
+      }
+      
+    case 0x5E: // RSB{S}<c> <Rd>,<Rn>,<Rm>{,<shift>}
+    case 0x5D: // SUB{S}<c>.W <Rd>,<Rn>,<Rm>{,<shift>}
+      {
+	ARMword Rn   = tBITS (0, 3);
+	ARMword Rd   = ntBITS (8, 11);
+	ARMword Rm   = ntBITS (0, 3);
+	ARMword S    = tBIT (4);
+	ARMword type = ntBITS (4, 5);
+	ARMword imm5 = (ntBITS (12, 14) << 2) | ntBITS (6, 7);
+
+	tASSERT (ntBIT(15) == 0);
+	
+	if (Rd == 15)
+	  {
+	    // CMP<c>.W <Rn>, <Rm> {,<shift>}
+	    * ainstr = 0xE1500000;
+	    Rd = 0;
+	  }
+	else if (tBIT (5))
+	  * ainstr = 0xE0400000;
+	else
+	  * ainstr = 0xE0600000;
+
+	* ainstr |= (S << 20);
+	* ainstr |= (Rn << 16);
+	* ainstr |= (Rd << 12);
+	* ainstr |= (imm5 << 7);
+	* ainstr |= (type << 5);
+	* ainstr |= (Rm <<  0);
+	* pvalid = t_decoded;
+	break;
+      }
+      
+    case 0x9D: // NOP.W
+      tASSERT (tBITS (0, 15) == 0xF3AF);
+      tASSERT (ntBITS (0, 15) == 0x8000);
+      * pvalid = t_branch;
+      break;
+      
+    case 0x80: // AND
+    case 0xA0: // TST
+      {
+	ARMword Rn = tBITS (0, 3);
+	ARMword imm12 = (tBIT(10) << 11) | (ntBITS (12, 14) << 8) | ntBITS (0, 7);
+	ARMword Rd = ntBITS (8, 11);
+	ARMword val;
+	int S = tBIT (4);
+
+	imm12 = ThumbExpandImm (imm12);	
+	val = state->Reg[Rn] & imm12;
+
+	if (Rd == 15)
+	  {
+	    // TST<c> <Rn>,#<const>
+	    tASSERT (S == 1);
+	  }
+	else
+	  {
+	    // AND{S}<c> <Rd>,<Rn>,#<const>
+	    if (in_IT_block ())
+	      S = 0;	    
+
+	    state->Reg[Rd] = val;
+	  }
+
+	if (S)
+	  ARMul_NegZero (state, val);
+	* pvalid = t_branch;
+	break;
+      }
+
+    case 0xA1:
+    case 0x81: // BIC.W
+      {
+	ARMword Rn = tBITS (0, 3);
+	ARMword Rd = ntBITS (8, 11);
+	ARMword S = tBIT (4);
+	ARMword imm8 = (ntBITS (12, 14) << 8) | ntBITS (0, 7);
+
+	tASSERT (ntBIT (15) == 0);
+
+	imm8 = ThumbExpandImm (imm8);
+	state->Reg[Rd] = state->Reg[Rn] & ~ imm8;
+
+	if (S && ! in_IT_block ())
+	  ARMul_NegZero (state, state->Reg[Rd]);
+	* pvalid = t_resolved;
+	break;
+      }
+      
+    case 0xA2:
+    case 0x82: // MOV{S}<c>.W <Rd>,#<const>
+      {
+	ARMword val = (tBIT(10) << 11) | (ntBITS (12, 14) << 8) | ntBITS (0, 7);
+	ARMword Rd = ntBITS (8, 11);
+
+	val = ThumbExpandImm (val);
+	state->Reg[Rd] = val;
+
+	if (tBIT (4) && ! in_IT_block ())
+	  ARMul_NegZero (state, val);
+	/* Indicate that the instruction has been processed.  */
+	* pvalid = t_branch;
+	break;
+      }
+
+    case 0xA3:
+    case 0x83: // MVN{S}<c> <Rd>,#<const>
+      {
+	ARMword val = (tBIT(10) << 11) | (ntBITS (12, 14) << 8) | ntBITS (0, 7);
+	ARMword Rd = ntBITS (8, 11);
+
+	val = ThumbExpandImm (val);
+	val = ~ val;
+	state->Reg[Rd] = val;
+
+	if (tBIT (4) && ! in_IT_block ())
+	  ARMul_NegZero (state, val);
+	* pvalid = t_resolved;
+	break;
+      }
+
+    case 0xA4: // EOR
+    case 0x84: // TEQ
+      {
+	ARMword Rn = tBITS (0, 3);
+	ARMword Rd = ntBITS (8, 11);
+	ARMword S = tBIT (4);
+	ARMword imm12 = ((tBIT (10) << 11) | (ntBITS (12, 14) << 8) | ntBITS (0, 7));
+	ARMword result;
+
+	imm12 = ThumbExpandImm (imm12);
+
+	result = state->Reg[Rn] ^ imm12;
+
+	if (Rd == 15 && S)
+	  // TEQ<c> <Rn>,#<const>
+	  ;
+	else
+	  {
+	    // EOR{S}<c> <Rd>,<Rn>,#<const>
+	    state->Reg[Rd] = result;
+
+	    if (in_IT_block ())
+	      S = 0;
+	  }
+	    
+	if (S)
+	  ARMul_NegZero (state, result);
+	* pvalid = t_resolved;
+	break;
+      }
+      
+    case 0xA8: // CMN
+    case 0x88: // ADD
+      {
+	ARMword Rd = ntBITS (8, 11);
+	int S = tBIT (4);
+	ARMword Rn = tBITS (0, 3);
+	ARMword lhs = state->Reg[Rn];
+	ARMword imm12 = (tBIT (10) << 11) | (ntBITS (12, 14) << 8) | ntBITS (0, 7);
+	ARMword rhs = ThumbExpandImm (imm12);
+	ARMword res = lhs + rhs;
+
+	if (Rd == 15 && S)
+	  {
+	    // CMN<c> <Rn>,#<const>
+	    res = lhs - rhs;
+	  }
+	else
+	  {
+	    // ADD{S}<c>.W <Rd>,<Rn>,#<const>
+	    res = lhs + rhs;
+
+	    if (in_IT_block ())
+	      S = 0;
+
+	    state->Reg[Rd] = res;
+	  }
+
+	if (S)
+	  {
+	    ARMul_NegZero (state, res);
+
+	    if ((lhs | rhs) >> 30)
+	      {
+		/* Possible C,V,N to set.  */
+		ARMul_AddCarry (state, lhs, rhs, res);
+		ARMul_AddOverflow (state, lhs, rhs, res);
+	      }
+	    else
+	      {
+		CLEARC;
+		CLEARV;
+	      }
+	  }
+	
+	* pvalid = t_branch;
+	break;
+      }
+
+    case 0xAA: 
+    case 0x8A: // ADC{S}<c> <Rd>,<Rn>,#<const>
+      {
+	ARMword Rn = tBITS (0, 3);
+	ARMword Rd = ntBITS (8, 11);
+	int S = tBIT (4);
+	ARMword imm12 = (tBIT (10) << 11) | (ntBITS (12, 14) << 8) | ntBITS (0, 7);
+	ARMword lhs = state->Reg[Rn];
+	ARMword rhs = ThumbExpandImm (imm12);
+	ARMword res;
+
+	tASSERT (ntBIT (15) == 0);
+
+	if (CFLAG)
+	  rhs += 1;
+
+	res = lhs + rhs;
+	state->Reg[Rd] = res;
+
+	if (in_IT_block ())
+	  S = 0;
+
+	if (S)
+	  {
+	    ARMul_NegZero (state, res);
+
+	    if ((lhs >= rhs) || ((rhs | lhs) >> 31))
+	      {
+		ARMul_AddCarry (state, lhs, rhs, res);
+		ARMul_AddOverflow (state, lhs, rhs, res);
+	      }
+	    else
+	      {
+		CLEARC;
+		CLEARV;
+	      }
+	  }
+
+	* pvalid = t_branch;
+	break;
+      }
+      
+    case 0xAB:
+    case 0x8B: // SBC{S}<c> <Rd>,<Rn>,#<const>
+      {
+	ARMword Rn = tBITS (0, 3);
+	ARMword Rd = ntBITS (8, 11);
+	int S = tBIT (4);
+	ARMword imm12 = (tBIT (10) << 11) | (ntBITS (12, 14) << 8) | ntBITS (0, 7);
+	ARMword lhs = state->Reg[Rn];
+	ARMword rhs = ThumbExpandImm (imm12);
+	ARMword res;
+
+	tASSERT (ntBIT (15) == 0);
+
+	if (! CFLAG)
+	  rhs += 1;
+
+	res = lhs - rhs;
+	state->Reg[Rd] = res;
+
+	if (in_IT_block ())
+	  S = 0;
+
+	if (S)
+	  {
+	    ARMul_NegZero (state, res);
+
+	    if ((lhs >= rhs) || ((rhs | lhs) >> 31))
+	      {
+		ARMul_SubCarry (state, lhs, rhs, res);
+		ARMul_SubOverflow (state, lhs, rhs, res);
+	      }
+	    else
+	      {
+		CLEARC;
+		CLEARV;
+	      }
+	  }
+
+	* pvalid = t_branch;
+	break;
+      }
+
+    case 0xAD:
+    case 0x8D: // SUB
+      {
+	ARMword Rn = tBITS (0, 3);
+	ARMword Rd = ntBITS (8, 11);
+	int S = tBIT (4);
+	ARMword imm12 = (tBIT (10) << 11) | (ntBITS (12, 14) << 8) | ntBITS (0, 7);
+	ARMword lhs = state->Reg[Rn];
+	ARMword rhs = ThumbExpandImm (imm12);
+	ARMword res = lhs - rhs;
+
+	if (Rd == 15 && S)
+	  {
+	    // CMP<c>.W <Rn>,#<const>
+	    tASSERT (S);
+	  }
+	else
+	  {
+	    // SUB{S}<c>.W <Rd>,<Rn>,#<const>	    
+	    if (in_IT_block ())
+	      S = 0;
+
+	    state->Reg[Rd] = res;
+	  }
+
+	if (S)
+	  {
+	    ARMul_NegZero (state, res);
+
+	    if ((lhs >= rhs) || ((rhs | lhs) >> 31))
+	      {
+		ARMul_SubCarry (state, lhs, rhs, res);
+		ARMul_SubOverflow (state, lhs, rhs, res);
+	      }
+	    else
+	      {
+		CLEARC;
+		CLEARV;
+	      }
+	  }
+
+	* pvalid = t_branch;
+	break;
+      }
+
+    case 0xAE:
+    case 0x8E: // RSB{S}<c>.W <Rd>,<Rn>,#<const>
+      {
+	ARMword Rn = tBITS (0, 3);
+	ARMword Rd = ntBITS (8, 11);
+	ARMword imm12 = (tBIT (10) << 11) | (ntBITS (12, 14) << 8) | ntBITS (0, 7);
+	int S = tBIT (4);
+	ARMword lhs = imm12;
+	ARMword rhs = state->Reg[Rn];
+	ARMword res = lhs - rhs;
+
+	tASSERT (ntBIT (15) == 0);
+	
+	state->Reg[Rd] = res;
+
+	if (S)
+	  {
+	    ARMul_NegZero (state, res);
+
+	    if ((lhs >= rhs) || ((rhs | lhs) >> 31))
+	      {
+		ARMul_SubCarry (state, lhs, rhs, res);
+		ARMul_SubOverflow (state, lhs, rhs, res);
+	      }
+	    else
+	      {
+		CLEARC;
+		CLEARV;
+	      }
+	  }
+	    
+	* pvalid = t_branch;
+	break;
+      }
+
+    case 0xB0:
+    case 0x90: // ADDW<c> <Rd>,<Rn>,#<imm12>
+      {
+	ARMword Rn = tBITS (0, 3);
+	ARMword Rd = ntBITS (8, 11);
+	ARMword imm12 = (tBIT (10) << 11) | (ntBITS (12, 14) << 8) | ntBITS (0, 7);
+
+	tASSERT (tBIT (4) == 0);
+	tASSERT (ntBIT (15) == 0);
+	
+	state->Reg[Rd] = state->Reg[Rn] + imm12;
+	* pvalid = t_branch;
+	break;
+      }
+
+    case 0xB2:
+    case 0x92: // MOVW<c> <Rd>,#<imm16>
+      {
+	ARMword Rd = ntBITS (8, 11);
+	ARMword imm = (tBITS (0, 3) << 12) | (tBIT (10) << 11) | (ntBITS (12, 14) << 8) | ntBITS (0, 7);
+
+	state->Reg[Rd] = imm;
+	/* Indicate that the instruction has been processed.  */
+	* pvalid = t_branch;
+	break;
+      }
+
+    case 0xb5:
+    case 0x95:// SUBW<c> <Rd>,<Rn>,#<imm12>
+      {
+	ARMword Rd = ntBITS (8, 11);
+	ARMword Rn = tBITS (0, 3);
+	ARMword imm12 = (tBIT (10) << 11) | (ntBITS (12, 14) << 8) | ntBITS (0, 7);
+
+	tASSERT (tBIT (4) == 0);
+	tASSERT (ntBIT (15) == 0);
+      
+	/* Note the ARM ARM indicates special cases for Rn == 15 (ADR)
+	   and Rn == 13 (SUB SP minus immediate), but these are implemented
+	   in exactly the same way as the normal SUBW insn.  */
+	state->Reg[Rd] = state->Reg[Rn] - imm12;
+
+	* pvalid = t_resolved;
+	break;
+      }
+      
+    case 0xB6:
+    case 0x96: // MOVT<c> <Rd>,#<imm16>
+      {
+	ARMword Rd = ntBITS (8, 11);
+	ARMword imm = (tBITS (0, 3) << 12) | (tBIT (10) << 11) | (ntBITS (12, 14) << 8) | ntBITS (0, 7);
+
+	state->Reg[Rd] &= 0xFFFF;
+	state->Reg[Rd] |= (imm << 16);
+	* pvalid = t_resolved;
+	break;
+      }
+
+    case 0x9A: // SBFXc> <Rd>,<Rn>,#<lsb>,#<width>
+      tASSERT (tBIT (4) == 0);
+      tASSERT (ntBIT (15) == 0);
+      tASSERT (ntBIT (5) == 0);
+      * ainstr = 0xE7A00050;
+      * ainstr |= (ntBITS (0, 4) << 16); // widthm1
+      * ainstr |= (ntBITS (8, 11) << 12); // Rd
+      * ainstr |= (((ntBITS (12, 14) << 2) | ntBITS (6, 7)) << 7); // lsb
+      * ainstr |= tBITS (0, 3); // Rn
+      * pvalid = t_decoded;
+      break;
+
+    case 0x9B:
+      {
+	ARMword Rd = ntBITS (8, 11);
+	ARMword Rn = tBITS (0, 3);
+	ARMword msbit = ntBITS (0, 5);
+	ARMword lsbit = (ntBITS (12, 14) << 2) | ntBITS (6, 7);
+	ARMword mask = -1 << lsbit;
+
+	tASSERT (tBIT (4) == 0);
+	tASSERT (ntBIT (15) == 0);
+	tASSERT (ntBIT (5) == 0);
+
+	mask &= ((1 << (msbit + 1)) - 1);
+
+	if (lsbit > msbit)
+	  ; // UNPREDICTABLE
+	else if (Rn == 15)
+	  {
+	    // BFC<c> <Rd>,#<lsb>,#<width>
+	    state->Reg[Rd] &= ~ mask;
+	  }
+	else
+	  {
+	    // BFI<c> <Rd>,<Rn>,#<lsb>,#<width>
+	    ARMword val = state->Reg[Rn] & (mask >> lsbit);
+
+	    val <<= lsbit;	    
+	    state->Reg[Rd] &= ~ mask;
+	    state->Reg[Rd] |= val;
+	  }
+	
+	* pvalid = t_resolved;
+	break;
+      }
+
+    case 0x9E: // UBFXc> <Rd>,<Rn>,#<lsb>,#<width>
+      tASSERT (tBIT (4) == 0);
+      tASSERT (ntBIT (15) == 0);
+      tASSERT (ntBIT (5) == 0);
+      * ainstr = 0xE7E00050;
+      * ainstr |= (ntBITS (0, 4) << 16); // widthm1
+      * ainstr |= (ntBITS (8, 11) << 12); // Rd
+      * ainstr |= (((ntBITS (12, 14) << 2) | ntBITS (6, 7)) << 7); // lsb
+      * ainstr |= tBITS (0, 3); // Rn
+      * pvalid = t_decoded;
+      break;
+      
+    case 0xC0: // STRB
+    case 0xC4: // LDRB
+      {
+	ARMword Rn = tBITS (0, 3);
+	ARMword Rt = ntBITS (12, 15);
+	
+	if (tBIT (4))
+	  {
+	    if (Rn == 15)
+	      {
+		tASSERT (Rt != 15);
+
+		/* LDRB<c> <Rt>,<label>                     => 1111 1000 U001 1111 */
+		* ainstr = 0xE55F0000;
+		* ainstr |= (tBIT (7) << 23);
+		* ainstr |= ntBITS (0, 11);
+	      }
+	    else if (tBIT (7))
+	      {
+		/* LDRB<c>.W <Rt>,[<Rn>{,#<imm12>}]         => 1111 1000 1001 rrrr */
+		* ainstr = 0xE5D00000;
+		* ainstr |= ntBITS (0, 11);
+	      }
+	    else if (ntBIT (11) == 0)
+	      {
+		/* LDRB<c>.W <Rt>,[<Rn>,<Rm>{,LSL #<imm2>}] => 1111 1000 0001 rrrr */
+		* ainstr = 0xE7D00000;
+		* ainstr |= (ntBITS (4, 5) << 7);
+		* ainstr |= ntBITS (0, 3);
+	      }
+	    else
+	      {
+		int P = ntBIT (10);
+		int U = ntBIT (9);
+		int W = ntBIT (8);
+
+		tASSERT (! (Rt == 15 && P && !U && !W));
+		tASSERT (! (P && U && !W));
+	    
+		/* LDRB<c> <Rt>,[<Rn>,#-<imm8>]             => 1111 1000 0001 rrrr
+		   LDRB<c> <Rt>,[<Rn>],#+/-<imm8>           => 1111 1000 0001 rrrr
+		   LDRB<c> <Rt>,[<Rn>,#+/-<imm8>]!          => 1111 1000 0001 rrrr */
+		* ainstr = 0xE4500000;
+		* ainstr |= (P << 24);
+		* ainstr |= (U << 23);
+		* ainstr |= (W << 21);
+		* ainstr |= ntBITS (0, 7);
+	      }
+	  }
+	else
+	  {
+	    if (tBIT (7) == 1)
+	      {
+		// STRB<c>.W <Rt>,[<Rn>,#<imm12>]
+		ARMword imm12 = ntBITS (0, 11);
+
+		ARMul_StoreByte (state, state->Reg[Rn] + imm12, state->Reg [Rt]);
+		* pvalid = t_branch;
+		break;
+	      }
+	    else if (ntBIT (11))
+	      {
+		// STRB<c> <Rt>,[<Rn>,#-<imm8>]
+		// STRB<c> <Rt>,[<Rn>],#+/-<imm8>
+		// STRB<c> <Rt>,[<Rn>,#+/-<imm8>]!
+		int P = ntBIT (10);
+		int U = ntBIT (9);
+		int W = ntBIT (8);
+		ARMword imm8 = ntBITS (0, 7);
+
+		tASSERT (! (P && U && !W));
+		tASSERT (! (Rn == 13 && P && !U && W && imm8 == 4));
+		
+		* ainstr = 0xE4000000;
+		* ainstr |= (P << 24);
+		* ainstr |= (U << 23);
+		* ainstr |= (W << 21);
+		* ainstr |= imm8;
+	      }
+	    else
+	      {
+		// STRB<c>.W <Rt>,[<Rn>,<Rm>{,LSL #<imm2>}]
+		tASSERT (ntBITS (6, 11) == 0);
+
+		* ainstr = 0xE7C00000;
+		* ainstr |= (ntBITS (4, 5) << 7);
+		* ainstr |= ntBITS (0, 3);
+	      }
+	  }
+	
+	* ainstr |= (Rn << 16);
+	* ainstr |= (Rt << 12);
+	* pvalid = t_decoded;
+	break;
+      }
+
+    case 0xC2: // LDR, STR
+      {
+	ARMword Rn = tBITS (0, 3);
+	ARMword Rt = ntBITS (12, 15);
+	ARMword imm8 = ntBITS (0, 7);
+	ARMword P = ntBIT (10);
+	ARMword U = ntBIT (9);
+	ARMword W = ntBIT (8);
+
+	tASSERT (Rn != 15);
+
+	if (tBIT (4))
+	  {
+	    if (Rn == 15)
+	      {
+		// LDR<c>.W <Rt>,<label>
+		* ainstr = 0xE51F0000;
+		* ainstr |= ntBITS (0, 11);
+	      }
+	    else if (ntBIT (11))
+	      {
+		tASSERT (! (P && U && ! W));
+		tASSERT (! (!P && U && W && Rn == 13 && imm8 == 4 && ntBIT (11) == 0));
+		tASSERT (! (P && !U && W && Rn == 13 && imm8 == 4 && ntBIT (11)));
+	  
+		// LDR<c> <Rt>,[<Rn>,#-<imm8>]
+		// LDR<c> <Rt>,[<Rn>],#+/-<imm8>
+		// LDR<c> <Rt>,[<Rn>,#+/-<imm8>]!
+		if (!P && W)
+		  W = 0;
+		* ainstr = 0xE4100000;
+		* ainstr |= (P << 24);
+		* ainstr |= (U << 23);
+		* ainstr |= (W << 21);
+		* ainstr |= imm8;
+	      }
+	    else
+	      {
+		// LDR<c>.W <Rt>,[<Rn>,<Rm>{,LSL #<imm2>}]
+
+		tASSERT (ntBITS (6, 11) == 0);
+
+		* ainstr = 0xE7900000;
+		* ainstr |= ntBITS (4, 5) << 7;
+		* ainstr |= ntBITS (0, 3);
+	      }
+	  }
+	else
+	  {
+	    if (ntBIT (11))
+	      {
+		tASSERT (! (P && U && ! W));
+		if (Rn == 13 && P && !U && W && imm8 == 4)
+		  {
+		    // PUSH<c>.W <register>
+		    tASSERT (ntBITS (0, 11) == 0xD04);
+		    tASSERT (tBITS (0, 4) == 0x0D);
+
+		    * ainstr = 0xE92D0000;
+		    * ainstr |= (1 << Rt);
+		    
+		    Rt = Rn = 0;
+		  }
+		else
+		  {
+		    tASSERT (! (P && U && !W));
+		    if (!P && W)
+		      W = 0;
+		    // STR<c> <Rt>,[<Rn>,#-<imm8>]
+		    // STR<c> <Rt>,[<Rn>],#+/-<imm8>
+		    // STR<c> <Rt>,[<Rn>,#+/-<imm8>]!
+		    * ainstr = 0xE4000000;
+		    * ainstr |= (P << 24);
+		    * ainstr |= (U << 23);
+		    * ainstr |= (W << 21);
+		    * ainstr |= imm8;
+		  }
+	      }
+	    else
+	      {
+		// STR<c>.W <Rt>,[<Rn>,<Rm>{,LSL #<imm2>}]
+		tASSERT (ntBITS (6, 11) == 0);
+
+		* ainstr = 0xE7800000;
+		* ainstr |= ntBITS (4, 5) << 7;
+		* ainstr |= ntBITS (0, 3);
+	      }
+	  }
+	
+	* ainstr |= (Rn << 16);
+	* ainstr |= (Rt << 12);
+	* pvalid = t_decoded;
+	break;
+      }
+
+    case 0xC1: // STRH
+    case 0xC5: // LDRH
+      {
+	ARMword Rn = tBITS (0, 3);
+	ARMword Rt = ntBITS (12, 15);
+	ARMword address;
+
+	tASSERT (Rn != 15);
+
+	if (tBIT (4) == 1)
+	  {
+	    if (tBIT (7))
+	      {
+		// LDRH<c>.W <Rt>,[<Rn>{,#<imm12>}]
+		ARMword imm12 = ntBITS (0, 11);
+		address = state->Reg[Rn] + imm12;
+	      }
+	    else if (ntBIT (11))
+	      {
+		// LDRH<c> <Rt>,[<Rn>,#-<imm8>]
+		// LDRH<c> <Rt>,[<Rn>],#+/-<imm8>
+		// LDRH<c> <Rt>,[<Rn>,#+/-<imm8>]!
+		ARMword P = ntBIT (10);
+		ARMword U = ntBIT (9);
+		ARMword W = ntBIT (8);
+		ARMword imm8 = ntBITS (0, 7);
+
+		tASSERT (Rn != 15);
+		tASSERT (! (P && U && !W));
+
+		* ainstr = 0xE05000B0;
+		* ainstr |= (P << 24);
+		* ainstr |= (U << 23);
+		* ainstr |= (W << 21);
+		* ainstr |= (Rn << 16);
+		* ainstr |= (Rt << 12);
+		* ainstr |= ((imm8 & 0xF0) << 4);
+		* ainstr |= (imm8 & 0xF);
+		* pvalid = t_decoded;
+		break;
+	      }
+	    else
+	      {
+		// LDRH<c>.W <Rt>,[<Rn>,<Rm>{,LSL #<imm2>}]
+		ARMword Rm = ntBITS (0, 3);
+		ARMword imm2 = ntBITS (4, 5);
+		
+		tASSERT (ntBITS (6, 10) == 0);
+
+		address = state->Reg[Rn] + (state->Reg[Rm] << imm2);
+	      }
+
+	    state->Reg[Rt] = ARMul_LoadHalfWord (state, address);
+	  }
+	else
+	  {
+	    if (tBIT (7))
+	      {
+		// STRH<c>.W <Rt>,[<Rn>{,#<imm12>}]
+		ARMword imm12 = ntBITS (0, 11);
+
+		address = state->Reg[Rn] + imm12;
+	      }
+	    else if (ntBIT (11))
+	      {
+		// STRH<c> <Rt>,[<Rn>,#-<imm8>]
+		// STRH<c> <Rt>,[<Rn>],#+/-<imm8>
+		// STRH<c> <Rt>,[<Rn>,#+/-<imm8>]!
+		ARMword P = ntBIT (10);
+		ARMword U = ntBIT (9);
+		ARMword W = ntBIT (8);
+		ARMword imm8 = ntBITS (0, 7);
+
+		tASSERT (! (P && U && !W));
+
+		* ainstr = 0xE04000B0;
+		* ainstr |= (P << 24);
+		* ainstr |= (U << 23);
+		* ainstr |= (W << 21);
+		* ainstr |= (Rn << 16);
+		* ainstr |= (Rt << 12);
+		* ainstr |= ((imm8 & 0xF0) << 4);
+		* ainstr |= (imm8 & 0xF);
+		* pvalid = t_decoded;
+		break;
+	      }
+	    else
+	      {
+		// STRH<c>.W <Rt>,[<Rn>,<Rm>{,LSL #<imm2>}]
+		ARMword Rm = ntBITS (0, 3);
+		ARMword imm2 = ntBITS (4, 5);
+		
+		tASSERT (ntBITS (6, 10) == 0);
+
+		address = state->Reg[Rn] + (state->Reg[Rm] << imm2);
+	      }
+
+	    ARMul_StoreHalfWord (state, address, state->Reg [Rt]);
+	  }
+	* pvalid = t_branch;
+	break;
+      }
+      
+    case 0xC6: // LDR.W/STR.W
+      {
+	ARMword Rn = tBITS (0, 3);
+	ARMword Rt = ntBITS (12, 15);
+	ARMword imm12 = ntBITS (0, 11);
+	ARMword address = state->Reg[Rn];
+	
+	if (Rn == 15)
+	  {
+	    // LDR<c>.W <Rt>,<label>
+	    tASSERT (tBIT (4) == 1);
+	    // tASSERT (tBIT (7) == 1)
+	  }
+
+	address += imm12;
+	if (tBIT (4) == 1)
+	  state->Reg[Rt] = ARMul_LoadWordN (state, address);
+	else
+	  ARMul_StoreWordN (state, address, state->Reg [Rt]);
+
+	* pvalid = t_resolved;
+	break;
+      }
+
+    case 0xC8:
+    case 0xCC: // LDRSB
+      {
+	ARMword Rt = ntBITS (12, 15);
+	ARMword Rn = tBITS (0, 3);
+	ARMword U = tBIT (7);
+	ARMword address = state->Reg[Rn];
+
+	tASSERT (tBIT (4) == 1);
+	tASSERT (Rt != 15); // PLI
+
+	if (Rn == 15)
+	  {
+	    // LDRSB<c> <Rt>,<label>
+	    ARMword imm12 = ntBITS (0, 11);
+	    address += (U ? imm12 : - imm12);
+	  } 
+	else if (U)
+	  {
+	    // LDRSB<c> <Rt>,[<Rn>,#<imm12>]
+	    ARMword imm12 = ntBITS (0, 11);
+	    address += imm12;
+	  }
+	else if (ntBIT (11))
+	  {
+	    // LDRSB<c> <Rt>,[<Rn>,#-<imm8>]
+	    // LDRSB<c> <Rt>,[<Rn>],#+/-<imm8>
+	    // LDRSB<c> <Rt>,[<Rn>,#+/-<imm8>]!
+	    * ainstr = 0xE05000D0;
+	    * ainstr |= ntBIT (10) << 24; // P
+	    * ainstr |= ntBIT (9) << 23; // U
+	    * ainstr |= ntBIT (8) << 21; // W
+	    * ainstr |= Rn << 16;
+	    * ainstr |= Rt << 12;
+	    * ainstr |= ntBITS (4, 7) << 8;
+	    * ainstr |= ntBITS (0, 3);
+	    * pvalid = t_decoded;
+	    break;
+	  }
+	else
+	  {
+	    // LDRSB<c>.W <Rt>,[<Rn>,<Rm>{,LSL #<imm2>}]
+	    ARMword Rm = ntBITS (0, 3);
+	    ARMword imm2 = ntBITS (4,5);
+
+	    tASSERT (ntBITS (6, 11) == 0);
+
+	    address += (state->Reg[Rm] << imm2);
+	  }
+	
+	state->Reg[Rt] = ARMul_LoadByte (state, address);
+	if (state->Reg[Rt] & 0x80)
+	  state->Reg[Rt] |= -1 << 8;
+
+	* pvalid = t_resolved;
+	break;
+      }
+      
+    case 0xC9:
+    case 0xCD:// LDRSH
+      {
+	ARMword Rt = ntBITS (12, 15);
+	ARMword Rn = tBITS (0, 3);
+	ARMword U = tBIT (7);
+	ARMword address = state->Reg[Rn];
+
+	tASSERT (tBIT (4) == 1);
+
+	if (Rn == 15 || U == 1)
+	  {
+	    // Rn==15 => LDRSH<c> <Rt>,<label>
+	    // Rn!=15 => LDRSH<c> <Rt>,[<Rn>,#<imm12>]
+	    ARMword imm12 = ntBITS (0, 11);
+
+	    address += (U ? imm12 : - imm12);
+	  }
+	else if (ntBIT (11))
+	  {
+	    // LDRSH<c> <Rt>,[<Rn>,#-<imm8>]
+	    // LDRSH<c> <Rt>,[<Rn>],#+/-<imm8>
+	    // LDRSH<c> <Rt>,[<Rn>,#+/-<imm8>]!
+	    * ainstr = 0xE05000F0;
+	    * ainstr |= ntBIT (10) << 24; // P
+	    * ainstr |= ntBIT (9) << 23; // U
+	    * ainstr |= ntBIT (8) << 21; // W
+	    * ainstr |= Rn << 16;
+	    * ainstr |= Rt << 12;
+	    * ainstr |= ntBITS (4, 7) << 8;
+	    * ainstr |= ntBITS (0, 3);
+	    * pvalid = t_decoded;
+	    break;
+	  }
+	else /* U == 0 */
+	  {
+	    // LDRSH<c>.W <Rt>,[<Rn>,<Rm>{,LSL #<imm2>}]
+	    ARMword Rm = ntBITS (0, 3);
+	    ARMword imm2 = ntBITS (4,5);
+
+	    tASSERT (ntBITS (6, 11) == 0);
+
+	    address += (state->Reg[Rm] << imm2);
+	  }
+
+	state->Reg[Rt] = ARMul_LoadHalfWord (state, address);
+	if (state->Reg[Rt] & 0x8000)
+	  state->Reg[Rt] |= -1 << 16;
+
+	* pvalid = t_branch;
+	break;
+      }
+
+    case 0x0D0: 
+      {
+	ARMword Rm = ntBITS (0, 3);
+	ARMword Rd = ntBITS (8, 11);
+
+	tASSERT (ntBITS (12, 15) == 15);
+
+	if (ntBIT (7) == 1)
+	  {
+	    // SXTH<c>.W <Rd>,<Rm>{,<rotation>}
+	    ARMword ror = ntBITS (4, 5) << 3;
+	    ARMword val;
+
+	    val = state->Reg[Rm];
+	    val = (val >> ror) | (val << (32 - ror));
+	    if (val & 0x8000)
+	      val |= -1 << 16;
+	    state->Reg[Rd] = val;
+	  }
+	else
+	  {
+	    // LSL{S}<c>.W <Rd>,<Rn>,<Rm>
+	    ARMword Rn = tBITS (0, 3);
+
+	    tASSERT (ntBITS (4, 6) == 0);
+
+	    state->Reg[Rd] = state->Reg[Rn] << (state->Reg[Rm] & 0xFF);
+	    if (tBIT (4))
+	      ARMul_NegZero (state, state->Reg[Rd]);
+	  }
+	* pvalid = t_branch;
+	break;
+      }
+
+    case 0x0D1: // LSR{S}<c>.W <Rd>,<Rn>,<Rm>
+      {
+	ARMword Rd = ntBITS (8, 11);
+	ARMword Rn = tBITS (0, 3);
+	ARMword Rm = ntBITS (0, 3);
+
+	tASSERT (ntBITS (12, 15) == 15);
+	tASSERT (ntBITS (4, 7) == 0);
+
+	state->Reg[Rd] = state->Reg[Rn] >> (state->Reg[Rm] & 0xFF);
+	if (tBIT (4))
+	  ARMul_NegZero (state, state->Reg[Rd]);
+	* pvalid = t_resolved;
+	break;
+      }
+
+    case 0xD2: 
+      tASSERT (ntBITS (12, 15) == 15);
+      if (ntBIT (7))
+	{
+	  tASSERT (ntBIT (6) == 0);
+	  // UXTB<c>.W <Rd>,<Rm>{,<rotation>}
+	  * ainstr = 0xE6EF0070;
+	  * ainstr |= (ntBITS (4, 5) << 10); // rotate
+	  * ainstr |= ntBITS (0, 3); // Rm
+	}
+      else
+	{
+	  // ASR{S}<c>.W <Rd>,<Rn>,<Rm>
+	  tASSERT (ntBITS (4, 7) == 0);
+	  * ainstr = 0xE1A00050;
+	  if (! in_IT_block ())
+	    * ainstr |= (tBIT (4) << 20);
+	  * ainstr |= (ntBITS (0, 3) << 8); // Rm
+	  * ainstr |= tBITS (0, 3); // Rn
+	}
+
+      * ainstr |= (ntBITS (8, 11) << 12); // Rd
+      * pvalid = t_decoded;
+      break;
+      
+    case 0xD3: // ROR{S}<c>.W <Rd>,<Rn>,<Rm>
+      tASSERT (ntBITS (12, 15) == 15);
+      tASSERT (ntBITS (4, 7) == 0);
+      * ainstr = 0xE1A00070;
+      if (! in_IT_block ())
+	* ainstr |= (tBIT (4) << 20);
+      * ainstr |= (ntBITS (8, 11) << 12); // Rd
+      * ainstr |= (ntBITS (0, 3) << 8); // Rm
+      * ainstr |= (tBITS (0, 3) << 0); // Rn
+      * pvalid = t_decoded;
+      break;
+      
+    case 0xD4:
+      {
+	ARMword Rn = tBITS (0, 3);
+	ARMword Rd = ntBITS (8, 11);
+	ARMword Rm = ntBITS (0, 3);
+
+	tASSERT (ntBITS (12, 15) == 15);
+
+	if (ntBITS (4, 7) == 8)
+	  {
+	    // REV<c>.W <Rd>,<Rm>
+	    ARMword val = state->Reg[Rm];
+	    
+	    tASSERT (Rm == Rn);
+	    
+	    state->Reg [Rd] =
+	      (val >> 24)
+	      | ((val >> 8) & 0xFF00)
+	      | ((val << 8) & 0xFF0000)
+	      | (val << 24);
+	    * pvalid = t_resolved;
+	  }
+	else
+	  {
+	    tASSERT (ntBITS (4, 7) == 4);
+
+	    if (tBIT (4) == 1)
+	       // UADD8<c> <Rd>,<Rn>,<Rm>
+	      * ainstr = 0xE6500F10;
+	    else
+	      // UADD16<c> <Rd>,<Rn>,<Rm>
+	      * ainstr = 0xE6500F90;
+	
+	    * ainstr |= (Rn << 16);
+	    * ainstr |= (Rd << 12);
+	    * ainstr |= (Rm <<  0);
+	    * pvalid = t_decoded;
+	  }
+	break;
+      }
+
+    case 0xD5:
+      {
+	ARMword Rn = tBITS (0, 3);
+	ARMword Rd = ntBITS (8, 11);
+	ARMword Rm = ntBITS (0, 3);
+
+	tASSERT (ntBITS (12, 15) == 15);
+	tASSERT (ntBITS (4, 7) == 8);
+
+	if (tBIT (4))
+	  {
+	    // CLZ<c> <Rd>,<Rm>
+	    tASSERT (Rm == Rn);
+	    * ainstr = 0xE16F0F10;
+	  }
+	else
+	  {
+	     // SEL<c> <Rd>,<Rn>,<Rm>
+	    * ainstr = 0xE6800FB0;
+	    * ainstr |= (Rn << 16);
+	  }
+
+	* ainstr |= (Rd << 12);
+	* ainstr |= (Rm <<  0);
+	* pvalid = t_decoded;
+	break;
+      }
+
+    case 0xD8: // MUL
+      {
+	ARMword Rn = tBITS (0, 3);
+	ARMword Rm = ntBITS (0, 3);
+	ARMword Rd = ntBITS (8, 11);
+	ARMword Ra = ntBITS (12, 15);
+
+	if (tBIT (4))
+	  {
+	    // SMLA<x><y><c> <Rd>,<Rn>,<Rm>,<Ra>
+	    ARMword nval = state->Reg[Rn];
+	    ARMword mval = state->Reg[Rm];
+	    ARMword res;
+
+	    tASSERT (ntBITS (6, 7) == 0);
+	    tASSERT (Ra != 15);
+
+	    if (ntBIT (5))
+	      nval >>= 16;
+	    else
+	      nval &= 0xFFFF;
+
+	    if (ntBIT (4))
+	      mval >>= 16;
+	    else
+	      mval &= 0xFFFF;
+
+	    res = nval * mval;
+	    res += state->Reg[Ra];
+	    // FIXME: Test and clear/set the Q bit.
+	    state->Reg[Rd] = res;
+	  }
+	else
+	  {
+	    if (ntBITS (4, 7) == 1)
+	      {
+		// MLS<c> <Rd>,<Rn>,<Rm>,<Ra>
+		state->Reg[Rd] = state->Reg[Ra] - (state->Reg[Rn] * state->Reg[Rm]); 
+	      }
+	    else
+	      {
+		tASSERT (ntBITS (4, 7) == 0);
+
+		if (Ra == 15)
+		  // MUL<c> <Rd>,<Rn>,<Rm>
+		  state->Reg[Rd] = state->Reg[Rn] * state->Reg[Rm];
+		else
+		  // MLA<c> <Rd>,<Rn>,<Rm>,<Ra>
+		  state->Reg[Rd] = state->Reg[Rn] * state->Reg[Rm] + state->Reg[Ra];
+	      }
+	  }
+	* pvalid = t_resolved;
+	break;
+      }
+
+    case 0xDC: // SMULL
+      tASSERT (tBIT (4) == 0);
+      tASSERT (ntBITS (4, 7) == 0);
+      * ainstr = 0xE0C00090;
+      * ainstr |= (ntBITS (8, 11) << 16); // RdHi
+      * ainstr |= (ntBITS (12, 15) << 12); // RdLo
+      * ainstr |= (ntBITS (0, 3) << 8); // Rm
+      * ainstr |= tBITS (0, 3); // Rn
+      * pvalid = t_decoded;
+      break;
+      
+    case 0xDD: // UMULL
+      tASSERT (tBIT (4) == 0);
+      tASSERT (ntBITS (4, 7) == 0);
+      * ainstr = 0xE0800090;
+      * ainstr |= (ntBITS (8, 11) << 16); // RdHi
+      * ainstr |= (ntBITS (12, 15) << 12); // RdLo
+      * ainstr |= (ntBITS (0, 3) << 8); // Rm
+      * ainstr |= tBITS (0, 3); // Rn
+      * pvalid = t_decoded;
+      break;
+      
+    case 0xDF: // UMLAL
+      tASSERT (tBIT (4) == 0);
+      tASSERT (ntBITS (4, 7) == 0);
+      * ainstr = 0xE0A00090;
+      * ainstr |= (ntBITS (8, 11) << 16); // RdHi
+      * ainstr |= (ntBITS (12, 15) << 12); // RdLo
+      * ainstr |= (ntBITS (0, 3) << 8); // Rm
+      * ainstr |= tBITS (0, 3); // Rn
+      * pvalid = t_decoded;
+      break;
+
+    default:  
+      fprintf (stderr, "(op = %x) ", tBITS (5,12));
+      tASSERT (0);
+      return;
+    }
+
+  /* Tell the Thumb decoder to skip the next 16-bit insn - it was
+     part of this insn - unless this insn has changed the PC.  */
+  skipping_32bit_thumb = pc + 2;
+}
+
 /* Attempt to emulate an ARMv6 instruction.
    Stores t_branch into PVALUE upon success or t_undefined otherwise.  */
 
 static void
 handle_v6_thumb_insn (ARMul_State * state,
 		      ARMword       tinstr,
+		      ARMword       next_instr,
+		      ARMword       pc,
+		      ARMword *     ainstr,
 		      tdstate *     pvalid)
 {
   if (! state->is_v6)
@@ -44,13 +1820,105 @@ handle_v6_thumb_insn (ARMul_State * state,
       return;
     }
 
+  if (tBITS (12, 15) == 0xB
+      && tBIT (10) == 0
+      && tBIT (8) == 1)
+    {
+      // Conditional branch forwards.
+      ARMword Rn = tBITS (0, 2);
+      ARMword imm5 = tBIT (9) << 5 | tBITS (3, 7);
+
+      if (tBIT (11))
+	{
+	  if (state->Reg[Rn] != 0)
+	    {
+	      state->Reg[15] = (pc + 4 + imm5 * 2);
+	      FLUSHPIPE;
+	    }
+	}
+      else
+	{
+	  if (state->Reg[Rn] == 0)
+	    {
+	      state->Reg[15] = (pc + 4 + imm5 * 2);
+	      FLUSHPIPE;
+	    }
+	}
+      * pvalid = t_branch;
+      return;
+    }
+
   switch (tinstr & 0xFFC0)
     {
-    case 0xb660: /* cpsie */
-    case 0xb670: /* cpsid */
-    case 0x4600: /* cpy */
+    case 0x4400:
+    case 0x4480:
+    case 0x4440:
+    case 0x44C0: // ADD
+      {
+	ARMword Rd = (tBIT (7) << 3) | tBITS (0, 2);
+	ARMword Rm = tBITS (3, 6);
+	state->Reg[Rd] += state->Reg[Rm];
+	break;
+      }
+      
+    case 0x4600: // MOV<c> <Rd>,<Rm>
+      {
+	// instr [15, 8] = 0100 0110
+	// instr [7]     = Rd<high>
+	// instr [6,3]   = Rm
+	// instr [2,0]   = Rd<low>
+	ARMword Rd = (tBIT(7) << 3) | tBITS (0, 2);
+	// FIXME: Check for Rd == 15 and ITblock.
+	state->Reg[Rd] = state->Reg[tBITS (3, 6)];
+	break;
+      }
+
+    case 0xBF00:
+    case 0xBF40:
+    case 0xBF80:
+    case 0xBFC0:
+      handle_IT_block (state, tinstr, pvalid);
+      return;
+
+    case 0xE840:
+    case 0xE880: // LDMIA
+    case 0xE8C0:
+    case 0xE900: // STM
+    case 0xE940:
+    case 0xE980:
+    case 0xE9C0: // LDRD
+    case 0xEA00: // BIC
+    case 0xEA40: // ORR
+    case 0xEA80: // EOR
+    case 0xEAC0:
+    case 0xEB00: // ADD
+    case 0xEB40: // SBC
+    case 0xEB80: // SUB
+    case 0xEBC0: // RSB
+    case 0xFA80: // UADD, SEL
+      handle_T2_insn (state, tinstr, next_instr, pc, ainstr, pvalid);
+      return;
+
     case 0xba00: /* rev */
+      {
+	ARMword val = state->Reg[tBITS (3, 5)];
+	state->Reg [tBITS (0, 2)] =
+	  (val >> 24)
+	  | ((val >> 8) & 0xFF00)
+	  | ((val << 8) & 0xFF0000)
+	  | (val << 24);
+	break;
+      }
+
     case 0xba40: /* rev16 */
+      {
+	ARMword val = state->Reg[tBITS (3, 5)];
+	state->Reg [tBITS (0, 2)] = (val >> 16) | (val << 16);
+	break;
+      }
+      
+    case 0xb660: /* cpsie */
+    case 0xb670: /* cpsid */
     case 0xbac0: /* revsh */
     case 0xb650: /* setend */
     default:
@@ -113,6 +1981,14 @@ ARMul_ThumbDecode (ARMul_State * state,
 {
   tdstate valid = t_decoded;	/* default assumes a valid instruction */
   ARMword next_instr;
+  ARMword  old_tinstr = tinstr;
+
+  if (skipping_32bit_thumb == pc)
+    {
+      skipping_32bit_thumb = 0;
+      return t_branch;
+    }
+  skipping_32bit_thumb = 0;
 
   if (state->bigendSig)
     {
@@ -125,6 +2001,24 @@ ARMul_ThumbDecode (ARMul_State * state,
       tinstr &= 0xFFFF;
     }
 
+  if (! IT_block_allow (state))
+    {
+      if (   tBITS (11, 15) == 0x1F
+	  || tBITS (11, 15) == 0x1E
+	  || tBITS (11, 15) == 0x1D)
+	{
+	  if (trace)
+	    fprintf (stderr, "pc: %x, SKIP  instr: %04x|%04x\n",
+		     pc & ~1, tinstr, next_instr);
+	  skipping_32bit_thumb = pc + 2;
+	}
+      else if (trace)
+	fprintf (stderr, "pc: %x, SKIP  instr: %04x\n", pc & ~1, tinstr);
+
+      return t_branch;
+    }
+  
+  old_tinstr = tinstr;
   if (trace)
     fprintf (stderr, "pc: %x, Thumb instr: %x", pc & ~1, tinstr);
 
@@ -147,38 +2041,50 @@ ARMul_ThumbDecode (ARMul_State * state,
     case 3:			/* ADD/SUB */
       /* Format 2 */
       {
-	ARMword subset[4] = {
-	  0xE0900000,		/* ADDS Rd,Rs,Rn    */
-	  0xE0500000,		/* SUBS Rd,Rs,Rn    */
-	  0xE2900000,		/* ADDS Rd,Rs,#imm3 */
-	  0xE2500000		/* SUBS Rd,Rs,#imm3 */
-	};
+	ARMword subset[4] =
+	  {
+	    0xE0900000,		/* ADDS Rd,Rs,Rn    */
+	    0xE0500000,		/* SUBS Rd,Rs,Rn    */
+	    0xE2900000,		/* ADDS Rd,Rs,#imm3 */
+	    0xE2500000		/* SUBS Rd,Rs,#imm3 */
+	  };
 	/* It is quicker indexing into a table, than performing switch
 	   or conditionals: */
 	*ainstr = subset[(tinstr & 0x0600) >> 9]	/* base opcode */
 	  | ((tinstr & 0x01C0) >> 6)	/* Rn or imm3 */
 	  | ((tinstr & 0x0038) << (16 - 3))	/* Rs */
 	  | ((tinstr & 0x0007) << (12 - 0));	/* Rd */
+
+	if (in_IT_block ())
+	  *ainstr &= ~ (1 << 20);
       }
       break;
-    case 4:			/* MOV */
-    case 5:			/* CMP */
-    case 6:			/* ADD */
-    case 7:			/* SUB */
-      /* Format 3 */
-      {
-	ARMword subset[4] = {
-	  0xE3B00000,		/* MOVS Rd,#imm8    */
-	  0xE3500000,		/* CMP  Rd,#imm8    */
-	  0xE2900000,		/* ADDS Rd,Rd,#imm8 */
-	  0xE2500000,		/* SUBS Rd,Rd,#imm8 */
-	};
-	*ainstr = subset[(tinstr & 0x1800) >> 11]	/* base opcode */
-	  | ((tinstr & 0x00FF) >> 0)	/* imm8 */
-	  | ((tinstr & 0x0700) << (16 - 8))	/* Rn */
-	  | ((tinstr & 0x0700) << (12 - 8));	/* Rd */
-      }
+    case 4:
+      * ainstr = 0xE3A00000; /* MOV  Rd,#imm8    */
+      if (! in_IT_block ())
+	* ainstr |= (1 << 20);
+      * ainstr |= tBITS (8, 10) << 12;
+      * ainstr |= tBITS (0, 7);
+      break;
+
+    case 5:
+      * ainstr = 0xE3500000;	/* CMP  Rd,#imm8    */
+      * ainstr |= tBITS (8, 10) << 16;
+      * ainstr |= tBITS (0, 7);
       break;
+      
+    case 6:
+    case 7:
+      * ainstr = tBIT (11)
+	? 0xE2400000		/* SUB  Rd,Rd,#imm8 */
+	: 0xE2800000;		/* ADD  Rd,Rd,#imm8 */
+      if (! in_IT_block ())
+	* ainstr |= (1 << 20);
+      * ainstr |= tBITS (8, 10) << 12;
+      * ainstr |= tBITS (8, 10) << 16;
+      * ainstr |= tBITS (0, 7);
+      break;
+
     case 8:			/* Arithmetic and high register transfers */
       /* TODO: Since the subsets for both Format 4 and Format 5
          instructions are made up of different ARM encodings, we could
@@ -214,6 +2120,38 @@ ARMul_ThumbDecode (ARMul_State * state,
 	    { 0xE1F00000, t_norm}	/* MVNS Rd,Rs        */
 	  };
 	  *ainstr = subset[(tinstr & 0x03C0) >> 6].opcode;	/* base */
+
+	  if (in_IT_block ())
+	    {
+	      struct
+	      {
+		ARMword opcode;
+		enum
+		  { t_norm, t_shift, t_neg, t_mul }
+		  otype;
+	      }
+	      subset[16] =
+		{
+		  { 0xE0000000, t_norm},	/* AND  Rd,Rd,Rs     */
+		  { 0xE0200000, t_norm},	/* EOR  Rd,Rd,Rs     */
+		  { 0xE1A00010, t_shift},	/* MOV  Rd,Rd,LSL Rs */
+		  { 0xE1A00030, t_shift},	/* MOV  Rd,Rd,LSR Rs */
+		  { 0xE1A00050, t_shift},	/* MOV  Rd,Rd,ASR Rs */
+		  { 0xE0A00000, t_norm},	/* ADC  Rd,Rd,Rs     */
+		  { 0xE0C00000, t_norm},	/* SBC  Rd,Rd,Rs     */
+		  { 0xE1A00070, t_shift},	/* MOV  Rd,Rd,ROR Rs */
+		  { 0xE1100000, t_norm},	/* TST  Rd,Rs        */
+		  { 0xE2600000, t_neg},		/* RSB  Rd,Rs,#0     */
+		  { 0xE1500000, t_norm},	/* CMP  Rd,Rs        */
+		  { 0xE1700000, t_norm},	/* CMN  Rd,Rs        */
+		  { 0xE1800000, t_norm},	/* ORR  Rd,Rd,Rs     */
+		  { 0xE0000090, t_mul} ,	/* MUL  Rd,Rd,Rs     */
+		  { 0xE1C00000, t_norm},	/* BIC  Rd,Rd,Rs     */
+		  { 0xE1E00000, t_norm}		/* MVN  Rd,Rs        */
+		};
+	      *ainstr = subset[(tinstr & 0x03C0) >> 6].opcode;	/* base */
+	    }
+
 	  switch (subset[(tinstr & 0x03C0) >> 6].otype)
 	    {
 	    case t_norm:
@@ -268,7 +2206,6 @@ ARMul_ThumbDecode (ARMul_State * state,
 	    case 0xA:		/* MOV Hd,Rs */
 	    case 0xB:		/* MOV Hd,Hs */
 	      *ainstr = 0xE1A00000	/* base */
-		| (Rd << 16)	/* Rn */
 		| (Rd << 12)	/* Rd */
 		| (Rs << 0);	/* Rm */
 	      break;
@@ -287,10 +2224,11 @@ ARMul_ThumbDecode (ARMul_State * state,
 		  break;
 		}
 	      /* Drop through.  */
+	    default:
 	    case 0x0:		/* UNDEFINED */
 	    case 0x4:		/* UNDEFINED */
 	    case 0x8:		/* UNDEFINED */
-	      handle_v6_thumb_insn (state, tinstr, & valid);
+	      handle_v6_thumb_insn (state, tinstr, next_instr, pc, ainstr, & valid);
 	      break;
 	    }
 	}
@@ -432,13 +2370,17 @@ ARMul_ThumbDecode (ARMul_State * state,
 		 defines this particular pattern as a BKPT instruction, for
 		 hardware assisted debugging.  We map onto the arm BKPT
 		 instruction.  */
-	      * ainstr = 0xE1200070 | ((tinstr & 0xf0) << 4) | (tinstr & 0xf);
+	      if (state->is_v6)
+		// Map to the SVC instruction instead of the BKPT instruction.
+		* ainstr = 0xEF000000 | tBITS (0, 7);
+	      else
+		* ainstr = 0xE1200070 | ((tinstr & 0xf0) << 4) | (tinstr & 0xf);
 	      break;
 	    }
 	  /* Drop through.  */
 	default:
 	  /* Everything else is an undefined instruction.  */
-	  handle_v6_thumb_insn (state, tinstr, & valid);
+	  handle_v6_thumb_insn (state, tinstr, next_instr, pc, ainstr, & valid);
 	  break;
 	}
       break;
@@ -530,7 +2472,7 @@ ARMul_ThumbDecode (ARMul_State * state,
 	}
       else
 	/* UNDEFINED : cc=1110(AL) uses different format.  */
-	handle_v6_thumb_insn (state, tinstr, & valid);
+	handle_v6_thumb_insn (state, tinstr, next_instr, pc, ainstr, & valid);
       break;
     case 28:			/* B */
       /* Format 18 */
@@ -541,15 +2483,21 @@ ARMul_ThumbDecode (ARMul_State * state,
       valid = t_branch;
       break;
     case 29:			/* UNDEFINED */
+      if (state->is_v6)
+	{
+	  handle_v6_thumb_insn (state, tinstr, next_instr, pc, ainstr, & valid);
+	  break;
+	}
+
       if (state->is_v5)
 	{
 	  if (tinstr & 1)
 	    {
-	      handle_v6_thumb_insn (state, tinstr, & valid);
+	      handle_v6_thumb_insn (state, tinstr, next_instr, pc, ainstr, & valid);
 	      break;
 	    }
 	  /* Drop through.  */
-	  
+
 	  /* Format 19 */
 	  /* There is no single ARM instruction equivalent for this
 	     instruction. Also, it should only ever be matched with the
@@ -571,10 +2519,16 @@ ARMul_ThumbDecode (ARMul_State * state,
 	  }
 	}
 
-      handle_v6_thumb_insn (state, tinstr, & valid);
+      handle_v6_thumb_insn (state, tinstr, next_instr, pc, ainstr, & valid);
       break;
 
     case 30:			/* BL instruction 1 */
+      if (state->is_v6)
+	{
+	  handle_T2_insn (state, tinstr, next_instr, pc, ainstr, & valid);
+	  break;
+	}
+
       /* Format 19 */
       /* There is no single ARM instruction equivalent for this Thumb
          instruction. To keep the simulation simple (from the user
@@ -610,6 +2564,12 @@ ARMul_ThumbDecode (ARMul_State * state,
       /* else we fall through to process the second half of the BL */
       pc += 2;			/* point the pc at the 2nd half */
     case 31:			/* BL instruction 2 */
+      if (state->is_v6)
+	{
+	  handle_T2_insn (state, old_tinstr, next_instr, pc, ainstr, & valid);
+	  break;
+	}
+
       /* Format 19 */
       /* There is no single ARM instruction equivalent for this
          instruction. Also, it should only ever be matched with the
diff --git a/sim/arm/wrapper.c b/sim/arm/wrapper.c
index d69daa3..b494354 100644
--- a/sim/arm/wrapper.c
+++ b/sim/arm/wrapper.c
@@ -257,6 +257,11 @@ sim_create_inferior (SIM_DESC sd ATTRIBUTE_UNUSED,
       mach = 0;
     }
 
+#ifdef MODET
+  if (abfd != NULL && (bfd_get_start_address (abfd) & 1))
+    SETT;
+#endif
+
   switch (mach)
     {
     default:
@@ -341,17 +346,6 @@ sim_create_inferior (SIM_DESC sd ATTRIBUTE_UNUSED,
       break;
     }
 
-  if (   mach != bfd_mach_arm_3
-      && mach != bfd_mach_arm_3M
-      && mach != bfd_mach_arm_2
-      && mach != bfd_mach_arm_2a)
-    {
-      /* Reset mode to ARM.  A gdb user may rerun a program that had entered
-	 THUMB mode from the start and cause the ARM-mode startup code to be
-	 executed in THUMB mode.  */
-      ARMul_SetCPSR (state, SVC32MODE);
-    }
-  
   memset (& info, 0, sizeof (info));
   INIT_DISASSEMBLE_INFO (info, stdout, op_printf);
   info.read_memory_func = sim_dis_read;