Add support for ARM v6 instructions.
authorNick Clifton <nickc@redhat.com>
Sun, 28 Jun 2015 18:14:36 +0000 (19:14 +0100)
committerNick Clifton <nickc@redhat.com>
Sun, 28 Jun 2015 18:14:36 +0000 (19:14 +0100)
* 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.

sim/arm/ChangeLog
sim/arm/Makefile.in
sim/arm/armdefs.h
sim/arm/armemu.c
sim/arm/armemu.h
sim/arm/armos.c
sim/arm/armsupp.c
sim/arm/thumbemu.c
sim/arm/wrapper.c

index 1f6db4e..a6533e0 100644 (file)
@@ -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.
index 745b62a..7605588 100644 (file)
@@ -17,7 +17,7 @@
 
 ## COMMON_PRE_CONFIG_FRAG
 
-SIM_EXTRA_CFLAGS = -DMODET
+SIM_EXTRA_CFLAGS = -DMODET -lm
 
 SIM_OBJS = \
        wrapper.o \
index 08a61f2..4e19a62 100644 (file)
@@ -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
index 09dfeaf..f2a84eb 100644 (file)
@@ -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.  */
index 484cc85..7f25b94 100644 (file)
@@ -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)
index e5c218d..c222e2e 100644 (file)
@@ -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;
 
index 113f080..e2bbf53 100644 (file)
@@ -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);
index e4c91f6..40c365e 100644 (file)
@@ -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
index d69daa3..b494354 100644 (file)
@@ -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;