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.

1 files changed, 920 insertions, 68 deletions

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.  */