sim/arm/iwmmxt.c

   1 /*  iwmmxt.c -- Intel(r) Wireless MMX(tm) technology co-processor interface.
   2     Copyright (C) 2002, 2007, 2008, 2009, 2010 Free Software Foundation, Inc.
   3     Contributed by matthew green (mrg@redhat.com).
   4
   5     This program is free software; you can redistribute it and/or modify
   6     it under the terms of the GNU General Public License as published by
   7     the Free Software Foundation; either version 3 of the License, or
   8     (at your option) any later version.
   9
  10     This program is distributed in the hope that it will be useful,
  11     but WITHOUT ANY WARRANTY; without even the implied warranty of
  12     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13     GNU General Public License for more details.
  14
  15     You should have received a copy of the GNU General Public License
  16     along with this program.  If not, see <http://www.gnu.org/licenses/>. */
  17
  18 #include <string.h>
  19
  20 #include "armdefs.h"
  21 #include "armos.h"
  22 #include "armemu.h"
  23 #include "ansidecl.h"
  24 #include "iwmmxt.h"
  25
  26 /* #define DEBUG 1 */
  27
  28 /* Intel(r) Wireless MMX(tm) technology co-processor.
  29    It uses co-processor numbers (0 and 1).  There are 16 vector registers wRx
  30    and 16 control registers wCx.  Co-processors 0 and 1 are used in MCR/MRC
  31    to access wRx and wCx respectively.  */
  32
  33 static ARMdword wR[16];
  34 static ARMword  wC[16] = { 0x69051010 };
  35
  36 #define SUBSTR(w,t,m,n) ((t)(w <<  ((sizeof (t) * 8 - 1) - (n))) \
  37                                >> (((sizeof (t) * 8 - 1) - (n)) + (m)))
  38 #define wCBITS(w,x,y)   SUBSTR (wC[w], ARMword, x, y)
  39 #define wRBITS(w,x,y)   SUBSTR (wR[w], ARMdword, x, y)
  40 #define wCID   0
  41 #define wCon   1
  42 #define wCSSF  2
  43 #define wCASF  3
  44 #define wCGR0  8
  45 #define wCGR1  9
  46 #define wCGR2 10
  47 #define wCGR3 11
  48
  49 /* Bits in the wCon register.  */
  50 #define WCON_CUP        (1 << 0)
  51 #define WCON_MUP        (1 << 1)
  52
  53 /* Set the SIMD wCASF flags for 8, 16, 32 or 64-bit operations.  */
  54 #define SIMD8_SET(x,  v, n, b)  (x) |= ((v != 0) << ((((b) + 1) * 4) + (n)))
  55 #define SIMD16_SET(x, v, n, h)  (x) |= ((v != 0) << ((((h) + 1) * 8) + (n)))
  56 #define SIMD32_SET(x, v, n, w)  (x) |= ((v != 0) << ((((w) + 1) * 16) + (n)))
  57 #define SIMD64_SET(x, v, n)     (x) |= ((v != 0) << (32 + (n)))
  58
  59 /* Flags to pass as "n" above.  */
  60 #define SIMD_NBIT       -1
  61 #define SIMD_ZBIT       -2
  62 #define SIMD_CBIT       -3
  63 #define SIMD_VBIT       -4
  64
  65 /* Various status bit macros.  */
  66 #define NBIT8(x)        ((x) & 0x80)
  67 #define NBIT16(x)       ((x) & 0x8000)
  68 #define NBIT32(x)       ((x) & 0x80000000)
  69 #define NBIT64(x)       ((x) & 0x8000000000000000ULL)
  70 #define ZBIT8(x)        (((x) & 0xff) == 0)
  71 #define ZBIT16(x)       (((x) & 0xffff) == 0)
  72 #define ZBIT32(x)       (((x) & 0xffffffff) == 0)
  73 #define ZBIT64(x)       (x == 0)
  74
  75 /* Access byte/half/word "n" of register "x".  */
  76 #define wRBYTE(x,n)     wRBITS ((x), (n) * 8, (n) * 8 + 7)
  77 #define wRHALF(x,n)     wRBITS ((x), (n) * 16, (n) * 16 + 15)
  78 #define wRWORD(x,n)     wRBITS ((x), (n) * 32, (n) * 32 + 31)
  79
  80 /* Macro to handle how the G bit selects wCGR registers.  */
  81 #define DECODE_G_BIT(state, instr, shift)       \
  82 {                                               \
  83   unsigned int reg;                             \
  84                                                 \
  85   reg = BITS (0, 3);                            \
  86                                                 \
  87   if (BIT (8))  /* G */                         \
  88     {                                           \
  89       if (reg < wCGR0 || reg > wCGR3)           \
  90         {                                       \
  91           ARMul_UndefInstr (state, instr);      \
  92           return ARMul_DONE;                    \
  93         }                                       \
  94       shift = wC [reg];                         \
  95     }                                           \
  96   else                                          \
  97     shift = wR [reg];                           \
  98                                                 \
  99   shift &= 0xff;                                \
 100 }
 101
 102 /* Index calculations for the satrv[] array.  */
 103 #define BITIDX8(x)      (x)
 104 #define BITIDX16(x)     (((x) + 1) * 2 - 1)
 105 #define BITIDX32(x)     (((x) + 1) * 4 - 1)
 106
 107 /* Sign extension macros.  */
 108 #define EXTEND8(a)      ((a) & 0x80 ? ((a) | 0xffffff00) : (a))
 109 #define EXTEND16(a)     ((a) & 0x8000 ? ((a) | 0xffff0000) : (a))
 110 #define EXTEND32(a)     ((a) & 0x80000000ULL ? ((a) | 0xffffffff00000000ULL) : (a))
 111
 112 /* Set the wCSSF from 8 values.  */
 113 #define SET_wCSSF(a,b,c,d,e,f,g,h) \
 114   wC[wCSSF] = (((h) != 0) << 7) | (((g) != 0) << 6) \
 115             | (((f) != 0) << 5) | (((e) != 0) << 4) \
 116             | (((d) != 0) << 3) | (((c) != 0) << 2) \
 117             | (((b) != 0) << 1) | (((a) != 0) << 0);
 118
 119 /* Set the wCSSR from an array with 8 values.  */
 120 #define SET_wCSSFvec(v) \
 121   SET_wCSSF((v)[0],(v)[1],(v)[2],(v)[3],(v)[4],(v)[5],(v)[6],(v)[7])
 122
 123 /* Size qualifiers for vector operations.  */
 124 #define Bqual                   0
 125 #define Hqual                   1
 126 #define Wqual                   2
 127 #define Dqual                   3
 128
 129 /* Saturation qualifiers for vector operations.  */
 130 #define NoSaturation            0
 131 #define UnsignedSaturation      1
 132 #define SignedSaturation        3
 133
 134 \f
 135 /* Prototypes.  */
 136 static ARMword         Add32  (ARMword,  ARMword,  int *, int *, ARMword);
 137 static ARMdword        AddS32 (ARMdword, ARMdword, int *, int *);
 138 static ARMdword        AddU32 (ARMdword, ARMdword, int *, int *);
 139 static ARMword         AddS16 (ARMword,  ARMword,  int *, int *);
 140 static ARMword         AddU16 (ARMword,  ARMword,  int *, int *);
 141 static ARMword         AddS8  (ARMword,  ARMword,  int *, int *);
 142 static ARMword         AddU8  (ARMword,  ARMword,  int *, int *);
 143 static ARMword         Sub32  (ARMword,  ARMword,  int *, int *, ARMword);
 144 static ARMdword        SubS32 (ARMdword, ARMdword, int *, int *);
 145 static ARMdword        SubU32 (ARMdword, ARMdword, int *, int *);
 146 static ARMword         SubS16 (ARMword,  ARMword,  int *, int *);
 147 static ARMword         SubS8  (ARMword,  ARMword,  int *, int *);
 148 static ARMword         SubU16 (ARMword,  ARMword,  int *, int *);
 149 static ARMword         SubU8  (ARMword,  ARMword,  int *, int *);
 150 static unsigned char   IwmmxtSaturateU8  (signed short, int *);
 151 static signed char     IwmmxtSaturateS8  (signed short, int *);
 152 static unsigned short  IwmmxtSaturateU16 (signed int, int *);
 153 static signed short    IwmmxtSaturateS16 (signed int, int *);
 154 static unsigned long   IwmmxtSaturateU32 (signed long long, int *);
 155 static signed long     IwmmxtSaturateS32 (signed long long, int *);
 156 static ARMword         Compute_Iwmmxt_Address   (ARMul_State *, ARMword, int *);
 157 static ARMdword        Iwmmxt_Load_Double_Word  (ARMul_State *, ARMword);
 158 static ARMword         Iwmmxt_Load_Word         (ARMul_State *, ARMword);
 159 static ARMword         Iwmmxt_Load_Half_Word    (ARMul_State *, ARMword);
 160 static ARMword         Iwmmxt_Load_Byte         (ARMul_State *, ARMword);
 161 static void            Iwmmxt_Store_Double_Word (ARMul_State *, ARMword, ARMdword);
 162 static void            Iwmmxt_Store_Word        (ARMul_State *, ARMword, ARMword);
 163 static void            Iwmmxt_Store_Half_Word   (ARMul_State *, ARMword, ARMword);
 164 static void            Iwmmxt_Store_Byte        (ARMul_State *, ARMword, ARMword);
 165 static int             Process_Instruction      (ARMul_State *, ARMword);
 166
 167 static int TANDC    (ARMul_State *, ARMword);
 168 static int TBCST    (ARMul_State *, ARMword);
 169 static int TEXTRC   (ARMul_State *, ARMword);
 170 static int TEXTRM   (ARMul_State *, ARMword);
 171 static int TINSR    (ARMul_State *, ARMword);
 172 static int TMCR     (ARMul_State *, ARMword);
 173 static int TMCRR    (ARMul_State *, ARMword);
 174 static int TMIA     (ARMul_State *, ARMword);
 175 static int TMIAPH   (ARMul_State *, ARMword);
 176 static int TMIAxy   (ARMul_State *, ARMword);
 177 static int TMOVMSK  (ARMul_State *, ARMword);
 178 static int TMRC     (ARMul_State *, ARMword);
 179 static int TMRRC    (ARMul_State *, ARMword);
 180 static int TORC     (ARMul_State *, ARMword);
 181 static int WACC     (ARMul_State *, ARMword);
 182 static int WADD     (ARMul_State *, ARMword);
 183 static int WALIGNI  (ARMword);
 184 static int WALIGNR  (ARMul_State *, ARMword);
 185 static int WAND     (ARMword);
 186 static int WANDN    (ARMword);
 187 static int WAVG2    (ARMword);
 188 static int WCMPEQ   (ARMul_State *, ARMword);
 189 static int WCMPGT   (ARMul_State *, ARMword);
 190 static int WLDR     (ARMul_State *, ARMword);
 191 static int WMAC     (ARMword);
 192 static int WMADD    (ARMword);
 193 static int WMAX     (ARMul_State *, ARMword);
 194 static int WMIN     (ARMul_State *, ARMword);
 195 static int WMUL     (ARMword);
 196 static int WOR      (ARMword);
 197 static int WPACK    (ARMul_State *, ARMword);
 198 static int WROR     (ARMul_State *, ARMword);
 199 static int WSAD     (ARMword);
 200 static int WSHUFH   (ARMword);
 201 static int WSLL     (ARMul_State *, ARMword);
 202 static int WSRA     (ARMul_State *, ARMword);
 203 static int WSRL     (ARMul_State *, ARMword);
 204 static int WSTR     (ARMul_State *, ARMword);
 205 static int WSUB     (ARMul_State *, ARMword);
 206 static int WUNPCKEH (ARMul_State *, ARMword);
 207 static int WUNPCKEL (ARMul_State *, ARMword);
 208 static int WUNPCKIH (ARMul_State *, ARMword);
 209 static int WUNPCKIL (ARMul_State *, ARMword);
 210 static int WXOR     (ARMword);
 211 \f
 212 /* This function does the work of adding two 32bit values
 213    together, and calculating if a carry has occurred.  */
 214
 215 static ARMword
 216 Add32 (ARMword a1,
 217        ARMword a2,
 218        int * carry_ptr,
 219        int * overflow_ptr,
 220        ARMword sign_mask)
 221 {
 222   ARMword result = (a1 + a2);
 223   unsigned int uresult = (unsigned int) result;
 224   unsigned int ua1 = (unsigned int) a1;
 225
 226   /* If (result == a1) and (a2 == 0),
 227      or (result > a2) then we have no carry.  */
 228   * carry_ptr = ((uresult == ua1) ? (a2 != 0) : (uresult < ua1));
 229
 230   /* Overflow occurs when both arguments are the
 231      same sign, but the result is a different sign.  */
 232   * overflow_ptr = (   ( (result & sign_mask) && !(a1 & sign_mask) && !(a2 & sign_mask))
 233                     || (!(result & sign_mask) &&  (a1 & sign_mask) &&  (a2 & sign_mask)));
 234
 235   return result;
 236 }
 237
 238 static ARMdword
 239 AddS32 (ARMdword a1, ARMdword a2, int * carry_ptr, int * overflow_ptr)
 240 {
 241   ARMdword     result;
 242   unsigned int uresult;
 243   unsigned int ua1;
 244
 245   a1 = EXTEND32 (a1);
 246   a2 = EXTEND32 (a2);
 247
 248   result  = a1 + a2;
 249   uresult = (unsigned int) result;
 250   ua1     = (unsigned int) a1;
 251
 252   * carry_ptr = ((uresult == a1) ? (a2 != 0) : (uresult < ua1));
 253
 254   * overflow_ptr = (   ( (result & 0x80000000ULL) && !(a1 & 0x80000000ULL) && !(a2 & 0x80000000ULL))
 255                     || (!(result & 0x80000000ULL) &&  (a1 & 0x80000000ULL) &&  (a2 & 0x80000000ULL)));
 256
 257   return result;
 258 }
 259
 260 static ARMdword
 261 AddU32 (ARMdword a1, ARMdword a2, int * carry_ptr, int * overflow_ptr)
 262 {
 263   ARMdword     result;
 264   unsigned int uresult;
 265   unsigned int ua1;
 266
 267   a1 &= 0xffffffff;
 268   a2 &= 0xffffffff;
 269
 270   result  = a1 + a2;
 271   uresult = (unsigned int) result;
 272   ua1     = (unsigned int) a1;
 273
 274   * carry_ptr = ((uresult == a1) ? (a2 != 0) : (uresult < ua1));
 275
 276   * overflow_ptr = (   ( (result & 0x80000000ULL) && !(a1 & 0x80000000ULL) && !(a2 & 0x80000000ULL))
 277                     || (!(result & 0x80000000ULL) &&  (a1 & 0x80000000ULL) &&  (a2 & 0x80000000ULL)));
 278
 279   return result;
 280 }
 281
 282 static ARMword
 283 AddS16 (ARMword a1, ARMword a2, int * carry_ptr, int * overflow_ptr)
 284 {
 285   a1 = EXTEND16 (a1);
 286   a2 = EXTEND16 (a2);
 287
 288   return Add32 (a1, a2, carry_ptr, overflow_ptr, 0x8000);
 289 }
 290
 291 static ARMword
 292 AddU16 (ARMword a1, ARMword a2, int * carry_ptr, int * overflow_ptr)
 293 {
 294   a1 &= 0xffff;
 295   a2 &= 0xffff;
 296
 297   return Add32 (a1, a2, carry_ptr, overflow_ptr, 0x8000);
 298 }
 299
 300 static ARMword
 301 AddS8 (ARMword a1, ARMword a2, int * carry_ptr, int * overflow_ptr)
 302 {
 303   a1 = EXTEND8 (a1);
 304   a2 = EXTEND8 (a2);
 305
 306   return Add32 (a1, a2, carry_ptr, overflow_ptr, 0x80);
 307 }
 308
 309 static ARMword
 310 AddU8 (ARMword a1, ARMword a2, int * carry_ptr, int * overflow_ptr)
 311 {
 312   a1 &= 0xff;
 313   a2 &= 0xff;
 314
 315   return Add32 (a1, a2, carry_ptr, overflow_ptr, 0x80);
 316 }
 317
 318 static ARMword
 319 Sub32 (ARMword a1,
 320        ARMword a2,
 321        int * borrow_ptr,
 322        int * overflow_ptr,
 323        ARMword sign_mask)
 324 {
 325   ARMword result = (a1 - a2);
 326   unsigned int ua1 = (unsigned int) a1;
 327   unsigned int ua2 = (unsigned int) a2;
 328
 329   /* A borrow occurs if a2 is (unsigned) larger than a1.
 330      However the carry flag is *cleared* if a borrow occurs.  */
 331   * borrow_ptr = ! (ua2 > ua1);
 332
 333   /* Overflow occurs when a negative number is subtracted from a
 334      positive number and the result is negative or a positive
 335      number is subtracted from a negative number and the result is
 336      positive.  */
 337   * overflow_ptr = ( (! (a1 & sign_mask) &&   (a2 & sign_mask) &&   (result & sign_mask))
 338                     || ((a1 & sign_mask) && ! (a2 & sign_mask) && ! (result & sign_mask)));
 339
 340   return result;
 341 }
 342
 343 static ARMdword
 344 SubS32 (ARMdword a1, ARMdword a2, int * borrow_ptr, int * overflow_ptr)
 345 {
 346   ARMdword     result;
 347   unsigned int ua1;
 348   unsigned int ua2;
 349
 350   a1 = EXTEND32 (a1);
 351   a2 = EXTEND32 (a2);
 352
 353   result = a1 - a2;
 354   ua1    = (unsigned int) a1;
 355   ua2    = (unsigned int) a2;
 356
 357   * borrow_ptr = ! (ua2 > ua1);
 358
 359   * overflow_ptr = ( (! (a1 & 0x80000000ULL) &&   (a2 & 0x80000000ULL) &&   (result & 0x80000000ULL))
 360                     || ((a1 & 0x80000000ULL) && ! (a2 & 0x80000000ULL) && ! (result & 0x80000000ULL)));
 361
 362   return result;
 363 }
 364
 365 static ARMword
 366 SubS16 (ARMword a1, ARMword a2, int * carry_ptr, int * overflow_ptr)
 367 {
 368   a1 = EXTEND16 (a1);
 369   a2 = EXTEND16 (a2);
 370
 371   return Sub32 (a1, a2, carry_ptr, overflow_ptr, 0x8000);
 372 }
 373
 374 static ARMword
 375 SubS8 (ARMword a1, ARMword a2, int * carry_ptr, int * overflow_ptr)
 376 {
 377   a1 = EXTEND8 (a1);
 378   a2 = EXTEND8 (a2);
 379
 380   return Sub32 (a1, a2, carry_ptr, overflow_ptr, 0x80);
 381 }
 382
 383 static ARMword
 384 SubU16 (ARMword a1, ARMword a2, int * carry_ptr, int * overflow_ptr)
 385 {
 386   a1 &= 0xffff;
 387   a2 &= 0xffff;
 388
 389   return Sub32 (a1, a2, carry_ptr, overflow_ptr, 0x8000);
 390 }
 391
 392 static ARMword
 393 SubU8 (ARMword a1, ARMword a2, int * carry_ptr, int * overflow_ptr)
 394 {
 395   a1 &= 0xff;
 396   a2 &= 0xff;
 397
 398   return Sub32 (a1, a2, carry_ptr, overflow_ptr, 0x80);
 399 }
 400
 401 static ARMdword
 402 SubU32 (ARMdword a1, ARMdword a2, int * borrow_ptr, int * overflow_ptr)
 403 {
 404   ARMdword     result;
 405   unsigned int ua1;
 406   unsigned int ua2;
 407
 408   a1 &= 0xffffffff;
 409   a2 &= 0xffffffff;
 410
 411   result = a1 - a2;
 412   ua1    = (unsigned int) a1;
 413   ua2    = (unsigned int) a2;
 414
 415   * borrow_ptr = ! (ua2 > ua1);
 416
 417   * overflow_ptr = ( (! (a1 & 0x80000000ULL) &&   (a2 & 0x80000000ULL) &&   (result & 0x80000000ULL))
 418                     || ((a1 & 0x80000000ULL) && ! (a2 & 0x80000000ULL) && ! (result & 0x80000000ULL)));
 419
 420   return result;
 421 }
 422
 423 /* For the saturation.  */
 424
 425 static unsigned char
 426 IwmmxtSaturateU8 (signed short val, int * sat)
 427 {
 428   unsigned char rv;
 429
 430   if (val < 0)
 431     {
 432       rv = 0;
 433       *sat = 1;
 434     }
 435   else if (val > 0xff)
 436     {
 437       rv = 0xff;
 438       *sat = 1;
 439     }
 440   else
 441     {
 442       rv = val & 0xff;
 443       *sat = 0;
 444     }
 445   return rv;
 446 }
 447
 448 static signed char
 449 IwmmxtSaturateS8 (signed short val, int * sat)
 450 {
 451   signed char rv;
 452
 453   if (val < -0x80)
 454     {
 455       rv = -0x80;
 456       *sat = 1;
 457     }
 458   else if (val > 0x7f)
 459     {
 460       rv = 0x7f;
 461       *sat = 1;
 462     }
 463   else
 464     {
 465       rv = val & 0xff;
 466       *sat = 0;
 467     }
 468   return rv;
 469 }
 470
 471 static unsigned short
 472 IwmmxtSaturateU16 (signed int val, int * sat)
 473 {
 474   unsigned short rv;
 475
 476   if (val < 0)
 477     {
 478       rv = 0;
 479       *sat = 1;
 480     }
 481   else if (val > 0xffff)
 482     {
 483       rv = 0xffff;
 484       *sat = 1;
 485     }
 486   else
 487     {
 488       rv = val & 0xffff;
 489       *sat = 0;
 490     }
 491   return rv;
 492 }
 493
 494 static signed short
 495 IwmmxtSaturateS16 (signed int val, int * sat)
 496 {
 497   signed short rv;
 498
 499   if (val < -0x8000)
 500     {
 501       rv = - 0x8000;
 502       *sat = 1;
 503     }
 504   else if (val > 0x7fff)
 505     {
 506       rv = 0x7fff;
 507       *sat = 1;
 508     }
 509   else
 510     {
 511       rv = val & 0xffff;
 512       *sat = 0;
 513     }
 514   return rv;
 515 }
 516
 517 static unsigned long
 518 IwmmxtSaturateU32 (signed long long val, int * sat)
 519 {
 520   unsigned long rv;
 521
 522   if (val < 0)
 523     {
 524       rv = 0;
 525       *sat = 1;
 526     }
 527   else if (val > 0xffffffff)
 528     {
 529       rv = 0xffffffff;
 530       *sat = 1;
 531     }
 532   else
 533     {
 534       rv = val & 0xffffffff;
 535       *sat = 0;
 536     }
 537   return rv;
 538 }
 539
 540 static signed long
 541 IwmmxtSaturateS32 (signed long long val, int * sat)
 542 {
 543   signed long rv;
 544
 545   if (val < -0x80000000LL)
 546     {
 547       rv = -0x80000000;
 548       *sat = 1;
 549     }
 550   else if (val > 0x7fffffff)
 551     {
 552       rv = 0x7fffffff;
 553       *sat = 1;
 554     }
 555   else
 556     {
 557       rv = val & 0xffffffff;
 558       *sat = 0;
 559     }
 560   return rv;
 561 }
 562
 563 /* Intel(r) Wireless MMX(tm) technology Acessor functions.  */
 564
 565 unsigned
 566 IwmmxtLDC (ARMul_State * state ATTRIBUTE_UNUSED,
 567            unsigned      type  ATTRIBUTE_UNUSED,
 568            ARMword       instr,
 569            ARMword       data)
 570 {
 571   return ARMul_CANT;
 572 }
 573
 574 unsigned
 575 IwmmxtSTC (ARMul_State * state ATTRIBUTE_UNUSED,
 576            unsigned      type  ATTRIBUTE_UNUSED,
 577            ARMword       instr,
 578            ARMword *     data)
 579 {
 580   return ARMul_CANT;
 581 }
 582
 583 unsigned
 584 IwmmxtMRC (ARMul_State * state ATTRIBUTE_UNUSED,
 585            unsigned      type  ATTRIBUTE_UNUSED,
 586            ARMword       instr,
 587            ARMword *     value)
 588 {
 589   return ARMul_CANT;
 590 }
 591
 592 unsigned
 593 IwmmxtMCR (ARMul_State * state ATTRIBUTE_UNUSED,
 594            unsigned      type  ATTRIBUTE_UNUSED,
 595            ARMword       instr,
 596            ARMword       value)
 597 {
 598   return ARMul_CANT;
 599 }
 600
 601 unsigned
 602 IwmmxtCDP (ARMul_State * state, unsigned type, ARMword instr)
 603 {
 604   return ARMul_CANT;
 605 }
 606
 607 /* Intel(r) Wireless MMX(tm) technology instruction implementations.  */
 608
 609 static int
 610 TANDC (ARMul_State * state, ARMword instr)
 611 {
 612   ARMword cpsr;
 613
 614   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
 615     return ARMul_CANT;
 616
 617 #ifdef DEBUG
 618   fprintf (stderr, "tandc\n");
 619 #endif
 620
 621   /* The Rd field must be r15.  */
 622   if (BITS (12, 15) != 15)
 623     return ARMul_CANT;
 624
 625   /* The CRn field must be r3.  */
 626   if (BITS (16, 19) != 3)
 627     return ARMul_CANT;
 628
 629   /* The CRm field must be r0.  */
 630   if (BITS (0, 3) != 0)
 631     return ARMul_CANT;
 632
 633   cpsr = ARMul_GetCPSR (state) & 0x0fffffff;
 634
 635   switch (BITS (22, 23))
 636     {
 637     case Bqual:
 638       cpsr |= (  (wCBITS (wCASF, 28, 31) & wCBITS (wCASF, 24, 27)
 639                 & wCBITS (wCASF, 20, 23) & wCBITS (wCASF, 16, 19)
 640                 & wCBITS (wCASF, 12, 15) & wCBITS (wCASF,  8, 11)
 641                 & wCBITS (wCASF,  4,  7) & wCBITS (wCASF,  0,  3)) << 28);
 642       break;
 643
 644     case Hqual:
 645       cpsr |= (  (wCBITS (wCASF, 28, 31) & wCBITS (wCASF, 20, 23)
 646                 & wCBITS (wCASF, 12, 15) & wCBITS (wCASF,  4, 7)) << 28);
 647       break;
 648
 649     case Wqual:
 650       cpsr |= ((wCBITS (wCASF, 28, 31) & wCBITS (wCASF, 12, 15)) << 28);
 651       break;
 652
 653     default:
 654       ARMul_UndefInstr (state, instr);
 655       return ARMul_DONE;
 656     }
 657
 658   ARMul_SetCPSR (state, cpsr);
 659
 660   return ARMul_DONE;
 661 }
 662
 663 static int
 664 TBCST (ARMul_State * state, ARMword instr)
 665 {
 666   ARMdword Rn;
 667   int wRd;
 668
 669   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
 670     return ARMul_CANT;
 671
 672 #ifdef DEBUG
 673   fprintf (stderr, "tbcst\n");
 674 #endif
 675
 676   Rn  = state->Reg [BITS (12, 15)];
 677   if (BITS (12, 15) == 15)
 678     Rn &= 0xfffffffc;
 679
 680   wRd = BITS (16, 19);
 681
 682   switch (BITS (6, 7))
 683     {
 684     case Bqual:
 685       Rn &= 0xff;
 686       wR [wRd] = (Rn << 56) | (Rn << 48) | (Rn << 40) | (Rn << 32)
 687                | (Rn << 24) | (Rn << 16) | (Rn << 8) | Rn;
 688       break;
 689
 690     case Hqual:
 691       Rn &= 0xffff;
 692       wR [wRd] = (Rn << 48) | (Rn << 32) | (Rn << 16) | Rn;
 693       break;
 694
 695     case Wqual:
 696       Rn &= 0xffffffff;
 697       wR [wRd] = (Rn << 32) | Rn;
 698       break;
 699
 700     default:
 701       ARMul_UndefInstr (state, instr);
 702       break;
 703     }
 704
 705   wC [wCon] |= WCON_MUP;
 706   return ARMul_DONE;
 707 }
 708
 709 static int
 710 TEXTRC (ARMul_State * state, ARMword instr)
 711 {
 712   ARMword cpsr;
 713   ARMword selector;
 714
 715   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
 716     return ARMul_CANT;
 717
 718 #ifdef DEBUG
 719   fprintf (stderr, "textrc\n");
 720 #endif
 721
 722   /* The Rd field must be r15.  */
 723   if (BITS (12, 15) != 15)
 724     return ARMul_CANT;
 725
 726   /* The CRn field must be r3.  */
 727   if (BITS (16, 19) != 3)
 728     return ARMul_CANT;
 729
 730   /* The CRm field must be 0xxx.  */
 731   if (BIT (3) != 0)
 732     return ARMul_CANT;
 733
 734   selector = BITS (0, 2);
 735   cpsr = ARMul_GetCPSR (state) & 0x0fffffff;
 736
 737   switch (BITS (22, 23))
 738     {
 739     case Bqual: selector *= 4; break;
 740     case Hqual: selector = ((selector & 3) * 8) + 4; break;
 741     case Wqual: selector = ((selector & 1) * 16) + 12; break;
 742
 743     default:
 744       ARMul_UndefInstr (state, instr);
 745       return ARMul_DONE;
 746     }
 747
 748   cpsr |= wCBITS (wCASF, selector, selector + 3) << 28;
 749   ARMul_SetCPSR (state, cpsr);
 750
 751   return ARMul_DONE;
 752 }
 753
 754 static int
 755 TEXTRM (ARMul_State * state, ARMword instr)
 756 {
 757   ARMword Rd;
 758   int     offset;
 759   int     wRn;
 760   int     sign;
 761
 762   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
 763     return ARMul_CANT;
 764
 765 #ifdef DEBUG
 766   fprintf (stderr, "textrm\n");
 767 #endif
 768
 769   wRn    = BITS (16, 19);
 770   sign   = BIT (3);
 771   offset = BITS (0, 2);
 772
 773   switch (BITS (22, 23))
 774     {
 775     case Bqual:
 776       offset *= 8;
 777       Rd = wRBITS (wRn, offset, offset + 7);
 778       if (sign)
 779         Rd = EXTEND8 (Rd);
 780       break;
 781
 782     case Hqual:
 783       offset = (offset & 3) * 16;
 784       Rd = wRBITS (wRn, offset, offset + 15);
 785       if (sign)
 786         Rd = EXTEND16 (Rd);
 787       break;
 788
 789     case Wqual:
 790       offset = (offset & 1) * 32;
 791       Rd = wRBITS (wRn, offset, offset + 31);
 792       break;
 793
 794     default:
 795       ARMul_UndefInstr (state, instr);
 796       return ARMul_DONE;
 797     }
 798
 799   if (BITS (12, 15) == 15)
 800     ARMul_UndefInstr (state, instr);
 801   else
 802     state->Reg [BITS (12, 15)] = Rd;
 803
 804   return ARMul_DONE;
 805 }
 806
 807 static int
 808 TINSR (ARMul_State * state, ARMword instr)
 809 {
 810   ARMdword data;
 811   ARMword  offset;
 812   int      wRd;
 813
 814   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
 815     return ARMul_CANT;
 816
 817 #ifdef DEBUG
 818   fprintf (stderr, "tinsr\n");
 819 #endif
 820
 821   wRd = BITS (16, 19);
 822   data = state->Reg [BITS (12, 15)];
 823   offset = BITS (0, 2);
 824
 825   switch (BITS (6, 7))
 826     {
 827     case Bqual:
 828       data &= 0xff;
 829       switch (offset)
 830         {
 831         case 0: wR [wRd] = data | (wRBITS (wRd, 8, 63) << 8); break;
 832         case 1: wR [wRd] = wRBITS (wRd, 0,  7) | (data <<  8) | (wRBITS (wRd, 16, 63) << 16); break;
 833         case 2: wR [wRd] = wRBITS (wRd, 0, 15) | (data << 16) | (wRBITS (wRd, 24, 63) << 24); break;
 834         case 3: wR [wRd] = wRBITS (wRd, 0, 23) | (data << 24) | (wRBITS (wRd, 32, 63) << 32); break;
 835         case 4: wR [wRd] = wRBITS (wRd, 0, 31) | (data << 32) | (wRBITS (wRd, 40, 63) << 40); break;
 836         case 5: wR [wRd] = wRBITS (wRd, 0, 39) | (data << 40) | (wRBITS (wRd, 48, 63) << 48); break;
 837         case 6: wR [wRd] = wRBITS (wRd, 0, 47) | (data << 48) | (wRBITS (wRd, 56, 63) << 56); break;
 838         case 7: wR [wRd] = wRBITS (wRd, 0, 55) | (data << 56); break;
 839         }
 840       break;
 841
 842     case Hqual:
 843       data &= 0xffff;
 844
 845       switch (offset & 3)
 846         {
 847         case 0: wR [wRd] = data | (wRBITS (wRd, 16, 63) << 16); break;
 848         case 1: wR [wRd] = wRBITS (wRd, 0, 15) | (data << 16) | (wRBITS (wRd, 32, 63) << 32); break;
 849         case 2: wR [wRd] = wRBITS (wRd, 0, 31) | (data << 32) | (wRBITS (wRd, 48, 63) << 48); break;
 850         case 3: wR [wRd] = wRBITS (wRd, 0, 47) | (data << 48); break;
 851         }
 852       break;
 853
 854     case Wqual:
 855       if (offset & 1)
 856         wR [wRd] = wRBITS (wRd, 0, 31) | (data << 32);
 857       else
 858         wR [wRd] = (wRBITS (wRd, 32, 63) << 32) | data;
 859       break;
 860
 861     default:
 862       ARMul_UndefInstr (state, instr);
 863       break;
 864     }
 865
 866   wC [wCon] |= WCON_MUP;
 867   return ARMul_DONE;
 868 }
 869
 870 static int
 871 TMCR (ARMul_State * state, ARMword instr)
 872 {
 873   ARMword val;
 874   int     wCreg;
 875
 876   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
 877     return ARMul_CANT;
 878
 879 #ifdef DEBUG
 880   fprintf (stderr, "tmcr\n");
 881 #endif
 882
 883   if (BITS (0, 3) != 0)
 884     return ARMul_CANT;
 885
 886   val = state->Reg [BITS (12, 15)];
 887   if (BITS (12, 15) == 15)
 888     val &= 0xfffffffc;
 889
 890   wCreg = BITS (16, 19);
 891
 892   switch (wCreg)
 893     {
 894     case wCID:
 895       /* The wCID register is read only.  */
 896       break;
 897
 898     case wCon:
 899       /* Writing to the MUP or CUP bits clears them.  */
 900       wC [wCon] &= ~ (val & 0x3);
 901       break;
 902
 903     case wCSSF:
 904       /* Only the bottom 8 bits can be written to.
 905           The higher bits write as zero.  */
 906       wC [wCSSF] = (val & 0xff);
 907       wC [wCon] |= WCON_CUP;
 908       break;
 909
 910     default:
 911       wC [wCreg] = val;
 912       wC [wCon] |= WCON_CUP;
 913       break;
 914     }
 915
 916   return ARMul_DONE;
 917 }
 918
 919 static int
 920 TMCRR (ARMul_State * state, ARMword instr)
 921 {
 922   ARMdword RdHi = state->Reg [BITS (16, 19)];
 923   ARMword  RdLo = state->Reg [BITS (12, 15)];
 924
 925   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
 926     return ARMul_CANT;
 927
 928 #ifdef DEBUG
 929   fprintf (stderr, "tmcrr\n");
 930 #endif
 931
 932   if ((BITS (16, 19) == 15) || (BITS (12, 15) == 15))
 933     return ARMul_CANT;
 934
 935   wR [BITS (0, 3)] = (RdHi << 32) | RdLo;
 936
 937   wC [wCon] |= WCON_MUP;
 938
 939   return ARMul_DONE;
 940 }
 941
 942 static int
 943 TMIA (ARMul_State * state, ARMword instr)
 944 {
 945   signed long long a, b;
 946
 947   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
 948     return ARMul_CANT;
 949
 950 #ifdef DEBUG
 951   fprintf (stderr, "tmia\n");
 952 #endif
 953
 954   if ((BITS (0, 3) == 15) || (BITS (12, 15) == 15))
 955     {
 956       ARMul_UndefInstr (state, instr);
 957       return ARMul_DONE;
 958     }
 959
 960   a = state->Reg [BITS (0, 3)];
 961   b = state->Reg [BITS (12, 15)];
 962
 963   a = EXTEND32 (a);
 964   b = EXTEND32 (b);
 965
 966   wR [BITS (5, 8)] += a * b;
 967   wC [wCon] |= WCON_MUP;
 968
 969   return ARMul_DONE;
 970 }
 971
 972 static int
 973 TMIAPH (ARMul_State * state, ARMword instr)
 974 {
 975   signed long a, b, result;
 976   signed long long r;
 977   ARMword Rm = state->Reg [BITS (0, 3)];
 978   ARMword Rs = state->Reg [BITS (12, 15)];
 979
 980   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
 981     return ARMul_CANT;
 982
 983 #ifdef DEBUG
 984   fprintf (stderr, "tmiaph\n");
 985 #endif
 986
 987   if (BITS (0, 3) == 15 || BITS (12, 15) == 15)
 988     {
 989       ARMul_UndefInstr (state, instr);
 990       return ARMul_DONE;
 991     }
 992
 993   a = SUBSTR (Rs, ARMword, 16, 31);
 994   b = SUBSTR (Rm, ARMword, 16, 31);
 995
 996   a = EXTEND16 (a);
 997   b = EXTEND16 (b);
 998
 999   result = a * b;
1000
1001   r = result;
1002   r = EXTEND32 (r);
1003
1004   wR [BITS (5, 8)] += r;
1005
1006   a = SUBSTR (Rs, ARMword,  0, 15);
1007   b = SUBSTR (Rm, ARMword,  0, 15);
1008
1009   a = EXTEND16 (a);
1010   b = EXTEND16 (b);
1011
1012   result = a * b;
1013
1014   r = result;
1015   r = EXTEND32 (r);
1016
1017   wR [BITS (5, 8)] += r;
1018   wC [wCon] |= WCON_MUP;
1019
1020   return ARMul_DONE;
1021 }
1022
1023 static int
1024 TMIAxy (ARMul_State * state, ARMword instr)
1025 {
1026   ARMword Rm;
1027   ARMword Rs;
1028   long long temp;
1029
1030   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
1031     return ARMul_CANT;
1032
1033 #ifdef DEBUG
1034   fprintf (stderr, "tmiaxy\n");
1035 #endif
1036
1037   if (BITS (0, 3) == 15 || BITS (12, 15) == 15)
1038     {
1039       ARMul_UndefInstr (state, instr);
1040       return ARMul_DONE;
1041     }
1042
1043   Rm = state->Reg [BITS (0, 3)];
1044   if (BIT (17))
1045     Rm >>= 16;
1046   else
1047     Rm &= 0xffff;
1048
1049   Rs = state->Reg [BITS (12, 15)];
1050   if (BIT (16))
1051     Rs >>= 16;
1052   else
1053     Rs &= 0xffff;
1054
1055   if (Rm & (1 << 15))
1056     Rm -= 1 << 16;
1057
1058   if (Rs & (1 << 15))
1059     Rs -= 1 << 16;
1060
1061   Rm *= Rs;
1062   temp = Rm;
1063
1064   if (temp & (1 << 31))
1065     temp -= 1ULL << 32;
1066
1067   wR [BITS (5, 8)] += temp;
1068   wC [wCon] |= WCON_MUP;
1069
1070   return ARMul_DONE;
1071 }
1072
1073 static int
1074 TMOVMSK (ARMul_State * state, ARMword instr)
1075 {
1076   ARMdword result;
1077   int      wRn;
1078
1079   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
1080     return ARMul_CANT;
1081
1082 #ifdef DEBUG
1083   fprintf (stderr, "tmovmsk\n");
1084 #endif
1085
1086   /* The CRm field must be r0.  */
1087   if (BITS (0, 3) != 0)
1088     return ARMul_CANT;
1089
1090   wRn = BITS (16, 19);
1091
1092   switch (BITS (22, 23))
1093     {
1094     case Bqual:
1095       result = (  (wRBITS (wRn, 63, 63) << 7)
1096                 | (wRBITS (wRn, 55, 55) << 6)
1097                 | (wRBITS (wRn, 47, 47) << 5)
1098                 | (wRBITS (wRn, 39, 39) << 4)
1099                 | (wRBITS (wRn, 31, 31) << 3)
1100                 | (wRBITS (wRn, 23, 23) << 2)
1101                 | (wRBITS (wRn, 15, 15) << 1)
1102                 | (wRBITS (wRn,  7,  7) << 0));
1103       break;
1104
1105     case Hqual:
1106       result = (  (wRBITS (wRn, 63, 63) << 3)
1107                 | (wRBITS (wRn, 47, 47) << 2)
1108                 | (wRBITS (wRn, 31, 31) << 1)
1109                 | (wRBITS (wRn, 15, 15) << 0));
1110       break;
1111
1112     case Wqual:
1113       result = (wRBITS (wRn, 63, 63) << 1) | wRBITS (wRn, 31, 31);
1114       break;
1115
1116     default:
1117       ARMul_UndefInstr (state, instr);
1118       return ARMul_DONE;
1119     }
1120
1121   state->Reg [BITS (12, 15)] = result;
1122
1123   return ARMul_DONE;
1124 }
1125
1126 static int
1127 TMRC (ARMul_State * state, ARMword instr)
1128 {
1129   int reg = BITS (12, 15);
1130
1131   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
1132     return ARMul_CANT;
1133
1134 #ifdef DEBUG
1135   fprintf (stderr, "tmrc\n");
1136 #endif
1137
1138   if (BITS (0, 3) != 0)
1139     return ARMul_CANT;
1140
1141   if (reg == 15)
1142     ARMul_UndefInstr (state, instr);
1143   else
1144     state->Reg [reg] = wC [BITS (16, 19)];
1145
1146   return ARMul_DONE;
1147 }
1148
1149 static int
1150 TMRRC (ARMul_State * state, ARMword instr)
1151 {
1152   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
1153     return ARMul_CANT;
1154
1155 #ifdef DEBUG
1156   fprintf (stderr, "tmrrc\n");
1157 #endif
1158
1159   if ((BITS (16, 19) == 15) || (BITS (12, 15) == 15) || (BITS (4, 11) != 0))
1160     ARMul_UndefInstr (state, instr);
1161   else
1162     {
1163       state->Reg [BITS (16, 19)] = wRBITS (BITS (0, 3), 32, 63);
1164       state->Reg [BITS (12, 15)] = wRBITS (BITS (0, 3),  0, 31);
1165     }
1166
1167   return ARMul_DONE;
1168 }
1169
1170 static int
1171 TORC (ARMul_State * state, ARMword instr)
1172 {
1173   ARMword cpsr = ARMul_GetCPSR (state);
1174
1175   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
1176     return ARMul_CANT;
1177
1178 #ifdef DEBUG
1179   fprintf (stderr, "torc\n");
1180 #endif
1181
1182   /* The Rd field must be r15.  */
1183   if (BITS (12, 15) != 15)
1184     return ARMul_CANT;
1185
1186   /* The CRn field must be r3.  */
1187   if (BITS (16, 19) != 3)
1188     return ARMul_CANT;
1189
1190   /* The CRm field must be r0.  */
1191   if (BITS (0, 3) != 0)
1192     return ARMul_CANT;
1193
1194   cpsr &= 0x0fffffff;
1195
1196   switch (BITS (22, 23))
1197     {
1198     case Bqual:
1199       cpsr |= (  (wCBITS (wCASF, 28, 31) | wCBITS (wCASF, 24, 27)
1200                 | wCBITS (wCASF, 20, 23) | wCBITS (wCASF, 16, 19)
1201                 | wCBITS (wCASF, 12, 15) | wCBITS (wCASF,  8, 11)
1202                 | wCBITS (wCASF,  4,  7) | wCBITS (wCASF,  0,  3)) << 28);
1203       break;
1204
1205     case Hqual:
1206       cpsr |= (  (wCBITS (wCASF, 28, 31) | wCBITS (wCASF, 20, 23)
1207                 | wCBITS (wCASF, 12, 15) | wCBITS (wCASF,  4,  7)) << 28);
1208       break;
1209
1210     case Wqual:
1211       cpsr |= ((wCBITS (wCASF, 28, 31) | wCBITS (wCASF, 12, 15)) << 28);
1212       break;
1213
1214     default:
1215       ARMul_UndefInstr (state, instr);
1216       return ARMul_DONE;
1217     }
1218
1219   ARMul_SetCPSR (state, cpsr);
1220
1221   return ARMul_DONE;
1222 }
1223
1224 static int
1225 WACC (ARMul_State * state, ARMword instr)
1226 {
1227   int wRn;
1228
1229   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
1230     return ARMul_CANT;
1231
1232 #ifdef DEBUG
1233   fprintf (stderr, "wacc\n");
1234 #endif
1235
1236   wRn = BITS (16, 19);
1237
1238   switch (BITS (22, 23))
1239     {
1240     case Bqual:
1241       wR [BITS (12, 15)] =
1242           wRBITS (wRn, 56, 63) + wRBITS (wRn, 48, 55)
1243         + wRBITS (wRn, 40, 47) + wRBITS (wRn, 32, 39)
1244         + wRBITS (wRn, 24, 31) + wRBITS (wRn, 16, 23)
1245         + wRBITS (wRn,  8, 15) + wRBITS (wRn,  0,  7);
1246       break;
1247
1248     case Hqual:
1249       wR [BITS (12, 15)] =
1250           wRBITS (wRn, 48, 63) + wRBITS (wRn, 32, 47)
1251         + wRBITS (wRn, 16, 31) + wRBITS (wRn,  0, 15);
1252       break;
1253
1254     case Wqual:
1255       wR [BITS (12, 15)] = wRBITS (wRn, 32, 63) + wRBITS (wRn, 0, 31);
1256       break;
1257
1258     default:
1259       ARMul_UndefInstr (state, instr);
1260       break;
1261     }
1262
1263   wC [wCon] |= WCON_MUP;
1264   return ARMul_DONE;
1265 }
1266
1267 static int
1268 WADD (ARMul_State * state, ARMword instr)
1269 {
1270   ARMdword r = 0;
1271   ARMdword x;
1272   ARMdword s;
1273   ARMword  psr = 0;
1274   int      i;
1275   int      carry;
1276   int      overflow;
1277   int      satrv[8];
1278
1279   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
1280     return ARMul_CANT;
1281
1282 #ifdef DEBUG
1283   fprintf (stderr, "wadd\n");
1284 #endif
1285
1286   /* Add two numbers using the specified function,
1287      leaving setting the carry bit as required.  */
1288 #define ADDx(x, y, m, f) \
1289    (*f) (wRBITS (BITS (16, 19), (x), (y)) & (m), \
1290          wRBITS (BITS ( 0,  3), (x), (y)) & (m), \
1291         & carry, & overflow)
1292
1293   switch (BITS (22, 23))
1294     {
1295     case Bqual:
1296       for (i = 0; i < 8; i++)
1297         {
1298           switch (BITS (20, 21))
1299             {
1300             case NoSaturation:
1301               s = ADDx ((i * 8), (i * 8) + 7, 0xff, AddS8);
1302               satrv [BITIDX8 (i)] = 0;
1303               r |= (s & 0xff) << (i * 8);
1304               SIMD8_SET (psr, NBIT8 (s), SIMD_NBIT, i);
1305               SIMD8_SET (psr, ZBIT8 (s), SIMD_ZBIT, i);
1306               SIMD8_SET (psr, carry,     SIMD_CBIT, i);
1307               SIMD8_SET (psr, overflow,  SIMD_VBIT, i);
1308               break;
1309
1310             case UnsignedSaturation:
1311               s = ADDx ((i * 8), (i * 8) + 7, 0xff, AddU8);
1312               x = IwmmxtSaturateU8 (s, satrv + BITIDX8 (i));
1313               r |= (x & 0xff) << (i * 8);
1314               SIMD8_SET (psr, NBIT8 (x), SIMD_NBIT, i);
1315               SIMD8_SET (psr, ZBIT8 (x), SIMD_ZBIT, i);
1316               if (! satrv [BITIDX8 (i)])
1317                 {
1318                   SIMD8_SET (psr, carry,    SIMD_CBIT, i);
1319                   SIMD8_SET (psr, overflow, SIMD_VBIT, i);
1320                 }
1321               break;
1322
1323             case SignedSaturation:
1324               s = ADDx ((i * 8), (i * 8) + 7, 0xff, AddS8);
1325               x = IwmmxtSaturateS8 (s, satrv + BITIDX8 (i));
1326               r |= (x & 0xff) << (i * 8);
1327               SIMD8_SET (psr, NBIT8 (x), SIMD_NBIT, i);
1328               SIMD8_SET (psr, ZBIT8 (x), SIMD_ZBIT, i);
1329               if (! satrv [BITIDX8 (i)])
1330                 {
1331                   SIMD8_SET (psr, carry,    SIMD_CBIT, i);
1332                   SIMD8_SET (psr, overflow, SIMD_VBIT, i);
1333                 }
1334               break;
1335
1336             default:
1337               ARMul_UndefInstr (state, instr);
1338               return ARMul_DONE;
1339             }
1340         }
1341       break;
1342
1343     case Hqual:
1344       satrv[0] = satrv[2] = satrv[4] = satrv[6] = 0;
1345
1346       for (i = 0; i < 4; i++)
1347         {
1348           switch (BITS (20, 21))
1349             {
1350             case NoSaturation:
1351               s = ADDx ((i * 16), (i * 16) + 15, 0xffff, AddS16);
1352               satrv [BITIDX16 (i)] = 0;
1353               r |= (s & 0xffff) << (i * 16);
1354               SIMD16_SET (psr, NBIT16 (s), SIMD_NBIT, i);
1355               SIMD16_SET (psr, ZBIT16 (s), SIMD_ZBIT, i);
1356               SIMD16_SET (psr, carry,      SIMD_CBIT, i);
1357               SIMD16_SET (psr, overflow,   SIMD_VBIT, i);
1358               break;
1359
1360             case UnsignedSaturation:
1361               s = ADDx ((i * 16), (i * 16) + 15, 0xffff, AddU16);
1362               x = IwmmxtSaturateU16 (s, satrv + BITIDX16 (i));
1363               r |= (x & 0xffff) << (i * 16);
1364               SIMD16_SET (psr, NBIT16 (x), SIMD_NBIT, i);
1365               SIMD16_SET (psr, ZBIT16 (x), SIMD_ZBIT, i);
1366               if (! satrv [BITIDX16 (i)])
1367                 {
1368                   SIMD16_SET (psr, carry,    SIMD_CBIT, i);
1369                   SIMD16_SET (psr, overflow, SIMD_VBIT, i);
1370                 }
1371               break;
1372
1373             case SignedSaturation:
1374               s = ADDx ((i * 16), (i * 16) + 15, 0xffff, AddS16);
1375               x = IwmmxtSaturateS16 (s, satrv + BITIDX16 (i));
1376               r |= (x & 0xffff) << (i * 16);
1377               SIMD16_SET (psr, NBIT16 (x), SIMD_NBIT, i);
1378               SIMD16_SET (psr, ZBIT16 (x), SIMD_ZBIT, i);
1379               if (! satrv [BITIDX16 (i)])
1380                 {
1381                   SIMD16_SET (psr, carry,    SIMD_CBIT, i);
1382                   SIMD16_SET (psr, overflow, SIMD_VBIT, i);
1383                 }
1384               break;
1385
1386             default:
1387               ARMul_UndefInstr (state, instr);
1388               return ARMul_DONE;
1389             }
1390         }
1391       break;
1392
1393     case Wqual:
1394       satrv[0] = satrv[1] = satrv[2] = satrv[4] = satrv[5] = satrv[6] = 0;
1395
1396       for (i = 0; i < 2; i++)
1397         {
1398           switch (BITS (20, 21))
1399             {
1400             case NoSaturation:
1401               s = ADDx ((i * 32), (i * 32) + 31, 0xffffffff, AddS32);
1402               satrv [BITIDX32 (i)] = 0;
1403               r |= (s & 0xffffffff) << (i * 32);
1404               SIMD32_SET (psr, NBIT32 (s), SIMD_NBIT, i);
1405               SIMD32_SET (psr, ZBIT32 (s), SIMD_ZBIT, i);
1406               SIMD32_SET (psr, carry,      SIMD_CBIT, i);
1407               SIMD32_SET (psr, overflow,   SIMD_VBIT, i);
1408               break;
1409
1410             case UnsignedSaturation:
1411               s = ADDx ((i * 32), (i * 32) + 31, 0xffffffff, AddU32);
1412               x = IwmmxtSaturateU32 (s, satrv + BITIDX32 (i));
1413               r |= (x & 0xffffffff) << (i * 32);
1414               SIMD32_SET (psr, NBIT32 (x), SIMD_NBIT, i);
1415               SIMD32_SET (psr, ZBIT32 (x), SIMD_ZBIT, i);
1416               if (! satrv [BITIDX32 (i)])
1417                 {
1418                   SIMD32_SET (psr, carry,    SIMD_CBIT, i);
1419                   SIMD32_SET (psr, overflow, SIMD_VBIT, i);
1420                 }
1421               break;
1422
1423             case SignedSaturation:
1424               s = ADDx ((i * 32), (i * 32) + 31, 0xffffffff, AddS32);
1425               x = IwmmxtSaturateS32 (s, satrv + BITIDX32 (i));
1426               r |= (x & 0xffffffff) << (i * 32);
1427               SIMD32_SET (psr, NBIT32 (x), SIMD_NBIT, i);
1428               SIMD32_SET (psr, ZBIT32 (x), SIMD_ZBIT, i);
1429               if (! satrv [BITIDX32 (i)])
1430                 {
1431                   SIMD32_SET (psr, carry,    SIMD_CBIT, i);
1432                   SIMD32_SET (psr, overflow, SIMD_VBIT, i);
1433                 }
1434               break;
1435
1436             default:
1437               ARMul_UndefInstr (state, instr);
1438               return ARMul_DONE;
1439             }
1440         }
1441       break;
1442
1443     default:
1444       ARMul_UndefInstr (state, instr);
1445       return ARMul_DONE;
1446     }
1447
1448   wC [wCASF] = psr;
1449   wR [BITS (12, 15)] = r;
1450   wC [wCon] |= (WCON_MUP | WCON_CUP);
1451
1452   SET_wCSSFvec (satrv);
1453
1454 #undef ADDx
1455
1456   return ARMul_DONE;
1457 }
1458
1459 static int
1460 WALIGNI (ARMword instr)
1461 {
1462   int shift = BITS (20, 22) * 8;
1463
1464   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
1465     return ARMul_CANT;
1466
1467 #ifdef DEBUG
1468   fprintf (stderr, "waligni\n");
1469 #endif
1470
1471   if (shift)
1472     wR [BITS (12, 15)] =
1473       wRBITS (BITS (16, 19), shift, 63)
1474       | (wRBITS (BITS (0, 3), 0, shift) << ((64 - shift)));
1475   else
1476     wR [BITS (12, 15)] = wR [BITS (16, 19)];
1477
1478   wC [wCon] |= WCON_MUP;
1479   return ARMul_DONE;
1480 }
1481
1482 static int
1483 WALIGNR (ARMul_State * state, ARMword instr)
1484 {
1485   int shift = (wC [BITS (20, 21) + 8] & 0x7) * 8;
1486
1487   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
1488     return ARMul_CANT;
1489
1490 #ifdef DEBUG
1491   fprintf (stderr, "walignr\n");
1492 #endif
1493
1494   if (shift)
1495     wR [BITS (12, 15)] =
1496       wRBITS (BITS (16, 19), shift, 63)
1497       | (wRBITS (BITS (0, 3), 0, shift) << ((64 - shift)));
1498   else
1499     wR [BITS (12, 15)] = wR [BITS (16, 19)];
1500
1501   wC [wCon] |= WCON_MUP;
1502   return ARMul_DONE;
1503 }
1504
1505 static int
1506 WAND (ARMword instr)
1507 {
1508   ARMdword result;
1509   ARMword  psr = 0;
1510
1511   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
1512     return ARMul_CANT;
1513
1514 #ifdef DEBUG
1515   fprintf (stderr, "wand\n");
1516 #endif
1517
1518   result = wR [BITS (16, 19)] & wR [BITS (0, 3)];
1519   wR [BITS (12, 15)] = result;
1520
1521   SIMD64_SET (psr, (result == 0), SIMD_ZBIT);
1522   SIMD64_SET (psr, (result & (1ULL << 63)), SIMD_NBIT);
1523
1524   wC [wCASF] = psr;
1525   wC [wCon] |= (WCON_CUP | WCON_MUP);
1526
1527   return ARMul_DONE;
1528 }
1529
1530 static int
1531 WANDN (ARMword instr)
1532 {
1533   ARMdword result;
1534   ARMword  psr = 0;
1535
1536   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
1537     return ARMul_CANT;
1538
1539 #ifdef DEBUG
1540   fprintf (stderr, "wandn\n");
1541 #endif
1542
1543   result = wR [BITS (16, 19)] & ~ wR [BITS (0, 3)];
1544   wR [BITS (12, 15)] = result;
1545
1546   SIMD64_SET (psr, (result == 0), SIMD_ZBIT);
1547   SIMD64_SET (psr, (result & (1ULL << 63)), SIMD_NBIT);
1548
1549   wC [wCASF] = psr;
1550   wC [wCon] |= (WCON_CUP | WCON_MUP);
1551
1552   return ARMul_DONE;
1553 }
1554
1555 static int
1556 WAVG2 (ARMword instr)
1557 {
1558   ARMdword r = 0;
1559   ARMword  psr = 0;
1560   ARMdword s;
1561   int      i;
1562   int      round = BIT (20) ? 1 : 0;
1563
1564   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
1565     return ARMul_CANT;
1566
1567 #ifdef DEBUG
1568   fprintf (stderr, "wavg2\n");
1569 #endif
1570
1571 #define AVG2x(x, y, m) (((wRBITS (BITS (16, 19), (x), (y)) & (m)) \
1572                        + (wRBITS (BITS ( 0,  3), (x), (y)) & (m)) \
1573                        + round) / 2)
1574
1575   if (BIT (22))
1576     {
1577       for (i = 0; i < 4; i++)
1578         {
1579           s = AVG2x ((i * 16), (i * 16) + 15, 0xffff) & 0xffff;
1580           SIMD16_SET (psr, ZBIT16 (s), SIMD_ZBIT, i);
1581           r |= s << (i * 16);
1582         }
1583     }
1584   else
1585     {
1586       for (i = 0; i < 8; i++)
1587         {
1588           s = AVG2x ((i * 8), (i * 8) + 7, 0xff) & 0xff;
1589           SIMD8_SET (psr, ZBIT8 (s), SIMD_ZBIT, i);
1590           r |= s << (i * 8);
1591         }
1592     }
1593
1594   wR [BITS (12, 15)] = r;
1595   wC [wCASF] = psr;
1596   wC [wCon] |= (WCON_CUP | WCON_MUP);
1597
1598   return ARMul_DONE;
1599 }
1600
1601 static int
1602 WCMPEQ (ARMul_State * state, ARMword instr)
1603 {
1604   ARMdword r = 0;
1605   ARMword  psr = 0;
1606   ARMdword s;
1607   int      i;
1608
1609   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
1610     return ARMul_CANT;
1611
1612 #ifdef DEBUG
1613   fprintf (stderr, "wcmpeq\n");
1614 #endif
1615
1616   switch (BITS (22, 23))
1617     {
1618     case Bqual:
1619       for (i = 0; i < 8; i++)
1620         {
1621           s = wRBYTE (BITS (16, 19), i) == wRBYTE (BITS (0, 3), i) ? 0xff : 0;
1622           r |= s << (i * 8);
1623           SIMD8_SET (psr, NBIT8 (s), SIMD_NBIT, i);
1624           SIMD8_SET (psr, ZBIT8 (s), SIMD_ZBIT, i);
1625         }
1626       break;
1627
1628     case Hqual:
1629       for (i = 0; i < 4; i++)
1630         {
1631           s = wRHALF (BITS (16, 19), i) == wRHALF (BITS (0, 3), i) ? 0xffff : 0;
1632           r |= s << (i * 16);
1633           SIMD16_SET (psr, NBIT16 (s), SIMD_NBIT, i);
1634           SIMD16_SET (psr, ZBIT16 (s), SIMD_ZBIT, i);
1635         }
1636       break;
1637
1638     case Wqual:
1639       for (i = 0; i < 2; i++)
1640         {
1641           s = wRWORD (BITS (16, 19), i) == wRWORD (BITS (0, 3), i) ? 0xffffffff : 0;
1642           r |= s << (i * 32);
1643           SIMD32_SET (psr, NBIT32 (s), SIMD_NBIT, i);
1644           SIMD32_SET (psr, ZBIT32 (s), SIMD_ZBIT, i);
1645         }
1646       break;
1647
1648     default:
1649       ARMul_UndefInstr (state, instr);
1650       return ARMul_DONE;
1651     }
1652
1653   wC [wCASF] = psr;
1654   wR [BITS (12, 15)] = r;
1655   wC [wCon] |= (WCON_CUP | WCON_MUP);
1656
1657   return ARMul_DONE;
1658 }
1659
1660 static int
1661 WCMPGT (ARMul_State * state, ARMword instr)
1662 {
1663   ARMdword r = 0;
1664   ARMword  psr = 0;
1665   ARMdword s;
1666   int      i;
1667
1668   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
1669     return ARMul_CANT;
1670
1671 #ifdef DEBUG
1672   fprintf (stderr, "wcmpgt\n");
1673 #endif
1674
1675   switch (BITS (22, 23))
1676     {
1677     case Bqual:
1678       if (BIT (21))
1679         {
1680           /* Use a signed comparison.  */
1681           for (i = 0; i < 8; i++)
1682             {
1683               signed char a, b;
1684
1685               a = wRBYTE (BITS (16, 19), i);
1686               b = wRBYTE (BITS (0, 3), i);
1687
1688               s = (a > b) ? 0xff : 0;
1689               r |= s << (i * 8);
1690               SIMD8_SET (psr, NBIT8 (s), SIMD_NBIT, i);
1691               SIMD8_SET (psr, ZBIT8 (s), SIMD_ZBIT, i);
1692             }
1693         }
1694       else
1695         {
1696           for (i = 0; i < 8; i++)
1697             {
1698               s = (wRBYTE (BITS (16, 19), i) > wRBYTE (BITS (0, 3), i))
1699                 ? 0xff : 0;
1700               r |= s << (i * 8);
1701               SIMD8_SET (psr, NBIT8 (s), SIMD_NBIT, i);
1702               SIMD8_SET (psr, ZBIT8 (s), SIMD_ZBIT, i);
1703             }
1704         }
1705       break;
1706
1707     case Hqual:
1708       if (BIT (21))
1709         {
1710           for (i = 0; i < 4; i++)
1711             {
1712               signed int a, b;
1713
1714               a = wRHALF (BITS (16, 19), i);
1715               a = EXTEND16 (a);
1716
1717               b = wRHALF (BITS (0, 3), i);
1718               b = EXTEND16 (b);
1719
1720               s = (a > b) ? 0xffff : 0;
1721               r |= s << (i * 16);
1722               SIMD16_SET (psr, NBIT16 (s), SIMD_NBIT, i);
1723               SIMD16_SET (psr, ZBIT16 (s), SIMD_ZBIT, i);
1724             }
1725         }
1726       else
1727         {
1728           for (i = 0; i < 4; i++)
1729             {
1730               s = (wRHALF (BITS (16, 19), i) > wRHALF (BITS (0, 3), i))
1731                 ? 0xffff : 0;
1732               r |= s << (i * 16);
1733               SIMD16_SET (psr, NBIT16 (s), SIMD_NBIT, i);
1734               SIMD16_SET (psr, ZBIT16 (s), SIMD_ZBIT, i);
1735             }
1736         }
1737       break;
1738
1739     case Wqual:
1740       if (BIT (21))
1741         {
1742           for (i = 0; i < 2; i++)
1743             {
1744               signed long a, b;
1745
1746               a = wRWORD (BITS (16, 19), i);
1747               b = wRWORD (BITS (0, 3), i);
1748
1749               s = (a > b) ? 0xffffffff : 0;
1750               r |= s << (i * 32);
1751               SIMD32_SET (psr, NBIT32 (s), SIMD_NBIT, i);
1752               SIMD32_SET (psr, ZBIT32 (s), SIMD_ZBIT, i);
1753             }
1754         }
1755       else
1756         {
1757           for (i = 0; i < 2; i++)
1758             {
1759               s = (wRWORD (BITS (16, 19), i) > wRWORD (BITS (0, 3), i))
1760                 ? 0xffffffff : 0;
1761               r |= s << (i * 32);
1762               SIMD32_SET (psr, NBIT32 (s), SIMD_NBIT, i);
1763               SIMD32_SET (psr, ZBIT32 (s), SIMD_ZBIT, i);
1764             }
1765         }
1766       break;
1767
1768     default:
1769       ARMul_UndefInstr (state, instr);
1770       return ARMul_DONE;
1771     }
1772
1773   wC [wCASF] = psr;
1774   wR [BITS (12, 15)] = r;
1775   wC [wCon] |= (WCON_CUP | WCON_MUP);
1776
1777   return ARMul_DONE;
1778 }
1779
1780 static ARMword
1781 Compute_Iwmmxt_Address (ARMul_State * state, ARMword instr, int * pFailed)
1782 {
1783   ARMword  Rn;
1784   ARMword  addr;
1785   ARMword  offset;
1786   ARMword  multiplier;
1787
1788   * pFailed  = 0;
1789   Rn         = BITS (16, 19);
1790   addr       = state->Reg [Rn];
1791   offset     = BITS (0, 7);
1792   multiplier = BIT (8) ? 4 : 1;
1793
1794   if (BIT (24)) /* P */
1795     {
1796       /* Pre Indexed Addressing.  */
1797       if (BIT (23))
1798         addr += offset * multiplier;
1799       else
1800         addr -= offset * multiplier;
1801
1802       /* Immediate Pre-Indexed.  */
1803       if (BIT (21)) /* W */
1804         {
1805           if (Rn == 15)
1806             {
1807               /* Writeback into R15 is UNPREDICTABLE.  */
1808 #ifdef DEBUG
1809               fprintf (stderr, "iWMMXt: writeback into r15\n");
1810 #endif
1811               * pFailed = 1;
1812             }
1813           else
1814             state->Reg [Rn] = addr;
1815         }
1816     }
1817   else
1818     {
1819       /* Post Indexed Addressing.  */
1820       if (BIT (21)) /* W */
1821         {
1822           /* Handle the write back of the final address.  */
1823           if (Rn == 15)
1824             {
1825               /* Writeback into R15 is UNPREDICTABLE.  */
1826 #ifdef DEBUG
1827               fprintf (stderr, "iWMMXt: writeback into r15\n");
1828 #endif
1829               * pFailed = 1;
1830             }
1831           else
1832             {
1833               ARMword  increment;
1834
1835               if (BIT (23))
1836                 increment = offset * multiplier;
1837               else
1838                 increment = - (offset * multiplier);
1839
1840               state->Reg [Rn] = addr + increment;
1841             }
1842         }
1843       else
1844         {
1845           /* P == 0, W == 0, U == 0 is UNPREDICTABLE.  */
1846           if (BIT (23) == 0)
1847             {
1848 #ifdef DEBUG
1849               fprintf (stderr, "iWMMXt: undefined addressing mode\n");
1850 #endif
1851               * pFailed = 1;
1852             }
1853         }
1854     }
1855
1856   return addr;
1857 }
1858
1859 static ARMdword
1860 Iwmmxt_Load_Double_Word (ARMul_State * state, ARMword address)
1861 {
1862   ARMdword value;
1863
1864   /* The address must be aligned on a 8 byte boundary.  */
1865   if (address & 0x7)
1866     {
1867       fprintf (stderr, "iWMMXt: At addr 0x%x: Unaligned double word load from 0x%x\n",
1868                (state->Reg[15] - 8) & ~0x3, address);
1869 #ifdef DEBUG
1870 #endif
1871       /* No need to check for alignment traps.  An unaligned
1872          double word load with alignment trapping disabled is
1873          UNPREDICTABLE.  */
1874       ARMul_Abort (state, ARMul_DataAbortV);
1875     }
1876
1877   /* Load the words.  */
1878   if (! state->bigendSig)
1879     {
1880       value = ARMul_LoadWordN (state, address + 4);
1881       value <<= 32;
1882       value |= ARMul_LoadWordN (state, address);
1883     }
1884   else
1885     {
1886       value = ARMul_LoadWordN (state, address);
1887       value <<= 32;
1888       value |= ARMul_LoadWordN (state, address + 4);
1889     }
1890
1891   /* Check for data aborts.  */
1892   if (state->Aborted)
1893     ARMul_Abort (state, ARMul_DataAbortV);
1894   else
1895     ARMul_Icycles (state, 2, 0L);
1896
1897   return value;
1898 }
1899
1900 static ARMword
1901 Iwmmxt_Load_Word (ARMul_State * state, ARMword address)
1902 {
1903   ARMword value;
1904
1905   /* Check for a misaligned address.  */
1906   if (address & 3)
1907     {
1908       if ((read_cp15_reg (1, 0, 0) & ARMul_CP15_R1_ALIGN))
1909         ARMul_Abort (state, ARMul_DataAbortV);
1910       else
1911         address &= ~ 3;
1912     }
1913
1914   value = ARMul_LoadWordN (state, address);
1915
1916   if (state->Aborted)
1917     ARMul_Abort (state, ARMul_DataAbortV);
1918   else
1919     ARMul_Icycles (state, 1, 0L);
1920
1921   return value;
1922 }
1923
1924 static ARMword
1925 Iwmmxt_Load_Half_Word (ARMul_State * state, ARMword address)
1926 {
1927   ARMword value;
1928
1929   /* Check for a misaligned address.  */
1930   if (address & 1)
1931     {
1932       if ((read_cp15_reg (1, 0, 0) & ARMul_CP15_R1_ALIGN))
1933         ARMul_Abort (state, ARMul_DataAbortV);
1934       else
1935         address &= ~ 1;
1936     }
1937
1938   value = ARMul_LoadHalfWord (state, address);
1939
1940   if (state->Aborted)
1941     ARMul_Abort (state, ARMul_DataAbortV);
1942   else
1943     ARMul_Icycles (state, 1, 0L);
1944
1945   return value;
1946 }
1947
1948 static ARMword
1949 Iwmmxt_Load_Byte (ARMul_State * state, ARMword address)
1950 {
1951   ARMword value;
1952
1953   value = ARMul_LoadByte (state, address);
1954
1955   if (state->Aborted)
1956     ARMul_Abort (state, ARMul_DataAbortV);
1957   else
1958     ARMul_Icycles (state, 1, 0L);
1959
1960   return value;
1961 }
1962
1963 static void
1964 Iwmmxt_Store_Double_Word (ARMul_State * state, ARMword address, ARMdword value)
1965 {
1966   /* The address must be aligned on a 8 byte boundary.  */
1967   if (address & 0x7)
1968     {
1969       fprintf (stderr, "iWMMXt: At addr 0x%x: Unaligned double word store to 0x%x\n",
1970                (state->Reg[15] - 8) & ~0x3, address);
1971 #ifdef DEBUG
1972 #endif
1973       /* No need to check for alignment traps.  An unaligned
1974          double word store with alignment trapping disabled is
1975          UNPREDICTABLE.  */
1976       ARMul_Abort (state, ARMul_DataAbortV);
1977     }
1978
1979   /* Store the words.  */
1980   if (! state->bigendSig)
1981     {
1982       ARMul_StoreWordN (state, address, value);
1983       ARMul_StoreWordN (state, address + 4, value >> 32);
1984     }
1985   else
1986     {
1987       ARMul_StoreWordN (state, address + 4, value);
1988       ARMul_StoreWordN (state, address, value >> 32);
1989     }
1990
1991   /* Check for data aborts.  */
1992   if (state->Aborted)
1993     ARMul_Abort (state, ARMul_DataAbortV);
1994   else
1995     ARMul_Icycles (state, 2, 0L);
1996 }
1997
1998 static void
1999 Iwmmxt_Store_Word (ARMul_State * state, ARMword address, ARMword value)
2000 {
2001   /* Check for a misaligned address.  */
2002   if (address & 3)
2003     {
2004       if ((read_cp15_reg (1, 0, 0) & ARMul_CP15_R1_ALIGN))
2005         ARMul_Abort (state, ARMul_DataAbortV);
2006       else
2007         address &= ~ 3;
2008     }
2009
2010   ARMul_StoreWordN (state, address, value);
2011
2012   if (state->Aborted)
2013     ARMul_Abort (state, ARMul_DataAbortV);
2014 }
2015
2016 static void
2017 Iwmmxt_Store_Half_Word (ARMul_State * state, ARMword address, ARMword value)
2018 {
2019   /* Check for a misaligned address.  */
2020   if (address & 1)
2021     {
2022       if ((read_cp15_reg (1, 0, 0) & ARMul_CP15_R1_ALIGN))
2023         ARMul_Abort (state, ARMul_DataAbortV);
2024       else
2025         address &= ~ 1;
2026     }
2027
2028   ARMul_StoreHalfWord (state, address, value);
2029
2030   if (state->Aborted)
2031     ARMul_Abort (state, ARMul_DataAbortV);
2032 }
2033
2034 static void
2035 Iwmmxt_Store_Byte (ARMul_State * state, ARMword address, ARMword value)
2036 {
2037   ARMul_StoreByte (state, address, value);
2038
2039   if (state->Aborted)
2040     ARMul_Abort (state, ARMul_DataAbortV);
2041 }
2042
2043 static int
2044 WLDR (ARMul_State * state, ARMword instr)
2045 {
2046   ARMword address;
2047   int failed;
2048
2049   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
2050     return ARMul_CANT;
2051
2052 #ifdef DEBUG
2053   fprintf (stderr, "wldr\n");
2054 #endif
2055
2056   address = Compute_Iwmmxt_Address (state, instr, & failed);
2057   if (failed)
2058     return ARMul_CANT;
2059
2060   if (BITS (28, 31) == 0xf)
2061     {
2062       /* WLDRW wCx */
2063       wC [BITS (12, 15)] = Iwmmxt_Load_Word (state, address);
2064     }
2065   else if (BIT (8) == 0)
2066     {
2067       if (BIT (22) == 0)
2068         /* WLDRB */
2069         wR [BITS (12, 15)] = Iwmmxt_Load_Byte (state, address);
2070       else
2071         /* WLDRH */
2072         wR [BITS (12, 15)] = Iwmmxt_Load_Half_Word (state, address);
2073     }
2074   else
2075     {
2076       if (BIT (22) == 0)
2077         /* WLDRW wRd */
2078         wR [BITS (12, 15)] = Iwmmxt_Load_Word (state, address);
2079       else
2080         /* WLDRD */
2081         wR [BITS (12, 15)] = Iwmmxt_Load_Double_Word (state, address);
2082     }
2083
2084   wC [wCon] |= WCON_MUP;
2085
2086   return ARMul_DONE;
2087 }
2088
2089 static int
2090 WMAC (ARMword instr)
2091 {
2092   int      i;
2093   ARMdword t = 0;
2094   ARMword  a, b;
2095
2096   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
2097     return ARMul_CANT;
2098
2099 #ifdef DEBUG
2100   fprintf (stderr, "wmac\n");
2101 #endif
2102
2103   for (i = 0; i < 4; i++)
2104     {
2105       if (BIT (21))
2106         {
2107           /* Signed.  */
2108           signed long s;
2109
2110           a = wRHALF (BITS (16, 19), i);
2111           a = EXTEND16 (a);
2112
2113           b = wRHALF (BITS (0, 3), i);
2114           b = EXTEND16 (b);
2115
2116           s = (signed long) a * (signed long) b;
2117
2118           t = t + (ARMdword) s;
2119         }
2120       else
2121         {
2122           /* Unsigned.  */
2123           a = wRHALF (BITS (16, 19), i);
2124           b = wRHALF (BITS ( 0,  3), i);
2125
2126           t += a * b;
2127         }
2128     }
2129
2130   if (BIT (20))
2131     wR [BITS (12, 15)] = 0;
2132
2133   if (BIT (21)) /* Signed.  */
2134     wR[BITS (12, 15)] += t;
2135   else
2136     wR [BITS (12, 15)] += t;
2137
2138   wC [wCon] |= WCON_MUP;
2139
2140   return ARMul_DONE;
2141 }
2142
2143 static int
2144 WMADD (ARMword instr)
2145 {
2146   ARMdword r = 0;
2147   int i;
2148
2149   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
2150     return ARMul_CANT;
2151
2152 #ifdef DEBUG
2153   fprintf (stderr, "wmadd\n");
2154 #endif
2155
2156   for (i = 0; i < 2; i++)
2157     {
2158       ARMdword s1, s2;
2159
2160       if (BIT (21))     /* Signed.  */
2161         {
2162           signed long a, b;
2163
2164           a = wRHALF (BITS (16, 19), i * 2);
2165           a = EXTEND16 (a);
2166
2167           b = wRHALF (BITS (0, 3), i * 2);
2168           b = EXTEND16 (b);
2169
2170           s1 = (ARMdword) (a * b);
2171
2172           a = wRHALF (BITS (16, 19), i * 2 + 1);
2173           a = EXTEND16 (a);
2174
2175           b = wRHALF (BITS (0, 3), i * 2 + 1);
2176           b = EXTEND16 (b);
2177
2178           s2 = (ARMdword) (a * b);
2179         }
2180       else                      /* Unsigned.  */
2181         {
2182           unsigned long a, b;
2183
2184           a = wRHALF (BITS (16, 19), i * 2);
2185           b = wRHALF (BITS ( 0,  3), i * 2);
2186
2187           s1 = (ARMdword) (a * b);
2188
2189           a = wRHALF (BITS (16, 19), i * 2 + 1);
2190           b = wRHALF (BITS ( 0,  3), i * 2 + 1);
2191
2192           s2 = (ARMdword) a * b;
2193         }
2194
2195       r |= (ARMdword) ((s1 + s2) & 0xffffffff) << (i ? 32 : 0);
2196     }
2197
2198   wR [BITS (12, 15)] = r;
2199   wC [wCon] |= WCON_MUP;
2200
2201   return ARMul_DONE;
2202 }
2203
2204 static int
2205 WMAX (ARMul_State * state, ARMword instr)
2206 {
2207   ARMdword r = 0;
2208   ARMdword s;
2209   int      i;
2210
2211   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
2212     return ARMul_CANT;
2213
2214 #ifdef DEBUG
2215   fprintf (stderr, "wmax\n");
2216 #endif
2217
2218   switch (BITS (22, 23))
2219     {
2220     case Bqual:
2221       for (i = 0; i < 8; i++)
2222         if (BIT (21))   /* Signed.  */
2223           {
2224             int a, b;
2225
2226             a = wRBYTE (BITS (16, 19), i);
2227             a = EXTEND8 (a);
2228
2229             b = wRBYTE (BITS (0, 3), i);
2230             b = EXTEND8 (b);
2231
2232             if (a > b)
2233               s = a;
2234             else
2235               s = b;
2236
2237             r |= (s & 0xff) << (i * 8);
2238           }
2239         else            /* Unsigned.  */
2240           {
2241             unsigned int a, b;
2242
2243             a = wRBYTE (BITS (16, 19), i);
2244             b = wRBYTE (BITS (0, 3), i);
2245
2246             if (a > b)
2247               s = a;
2248             else
2249               s = b;
2250
2251             r |= (s & 0xff) << (i * 8);
2252           }
2253       break;
2254
2255     case Hqual:
2256       for (i = 0; i < 4; i++)
2257         if (BIT (21))   /* Signed.  */
2258           {
2259             int a, b;
2260
2261             a = wRHALF (BITS (16, 19), i);
2262             a = EXTEND16 (a);
2263
2264             b = wRHALF (BITS (0, 3), i);
2265             b = EXTEND16 (b);
2266
2267             if (a > b)
2268               s = a;
2269             else
2270               s = b;
2271
2272             r |= (s & 0xffff) << (i * 16);
2273           }
2274         else            /* Unsigned.  */
2275           {
2276             unsigned int a, b;
2277
2278             a = wRHALF (BITS (16, 19), i);
2279             b = wRHALF (BITS (0, 3), i);
2280
2281             if (a > b)
2282               s = a;
2283             else
2284               s = b;
2285
2286             r |= (s & 0xffff) << (i * 16);
2287           }
2288       break;
2289
2290     case Wqual:
2291       for (i = 0; i < 2; i++)
2292         if (BIT (21))   /* Signed.  */
2293           {
2294             int a, b;
2295
2296             a = wRWORD (BITS (16, 19), i);
2297             b = wRWORD (BITS (0, 3), i);
2298
2299             if (a > b)
2300               s = a;
2301             else
2302               s = b;
2303
2304             r |= (s & 0xffffffff) << (i * 32);
2305           }
2306         else
2307           {
2308             unsigned int a, b;
2309
2310             a = wRWORD (BITS (16, 19), i);
2311             b = wRWORD (BITS (0, 3), i);
2312
2313             if (a > b)
2314               s = a;
2315             else
2316               s = b;
2317
2318             r |= (s & 0xffffffff) << (i * 32);
2319           }
2320       break;
2321
2322     default:
2323       ARMul_UndefInstr (state, instr);
2324       return ARMul_DONE;
2325     }
2326
2327   wR [BITS (12, 15)] = r;
2328   wC [wCon] |= WCON_MUP;
2329
2330   return ARMul_DONE;
2331 }
2332
2333 static int
2334 WMIN (ARMul_State * state, ARMword instr)
2335 {
2336   ARMdword r = 0;
2337   ARMdword s;
2338   int      i;
2339
2340   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
2341     return ARMul_CANT;
2342
2343 #ifdef DEBUG
2344   fprintf (stderr, "wmin\n");
2345 #endif
2346
2347   switch (BITS (22, 23))
2348     {
2349     case Bqual:
2350       for (i = 0; i < 8; i++)
2351         if (BIT (21))   /* Signed.  */
2352           {
2353             int a, b;
2354
2355             a = wRBYTE (BITS (16, 19), i);
2356             a = EXTEND8 (a);
2357
2358             b = wRBYTE (BITS (0, 3), i);
2359             b = EXTEND8 (b);
2360
2361             if (a < b)
2362               s = a;
2363             else
2364               s = b;
2365
2366             r |= (s & 0xff) << (i * 8);
2367           }
2368         else            /* Unsigned.  */
2369           {
2370             unsigned int a, b;
2371
2372             a = wRBYTE (BITS (16, 19), i);
2373             b = wRBYTE (BITS (0, 3), i);
2374
2375             if (a < b)
2376               s = a;
2377             else
2378               s = b;
2379
2380             r |= (s & 0xff) << (i * 8);
2381           }
2382       break;
2383
2384     case Hqual:
2385       for (i = 0; i < 4; i++)
2386         if (BIT (21))   /* Signed.  */
2387           {
2388             int a, b;
2389
2390             a = wRHALF (BITS (16, 19), i);
2391             a = EXTEND16 (a);
2392
2393             b = wRHALF (BITS (0, 3), i);
2394             b = EXTEND16 (b);
2395
2396             if (a < b)
2397               s = a;
2398             else
2399               s = b;
2400
2401             r |= (s & 0xffff) << (i * 16);
2402           }
2403         else
2404           {
2405             /* Unsigned.  */
2406             unsigned int a, b;
2407
2408             a = wRHALF (BITS (16, 19), i);
2409             b = wRHALF (BITS ( 0,  3), i);
2410
2411             if (a < b)
2412               s = a;
2413             else
2414               s = b;
2415
2416             r |= (s & 0xffff) << (i * 16);
2417           }
2418       break;
2419
2420     case Wqual:
2421       for (i = 0; i < 2; i++)
2422         if (BIT (21))   /* Signed.  */
2423           {
2424             int a, b;
2425
2426             a = wRWORD (BITS (16, 19), i);
2427             b = wRWORD (BITS ( 0,  3), i);
2428
2429             if (a < b)
2430               s = a;
2431             else
2432               s = b;
2433
2434             r |= (s & 0xffffffff) << (i * 32);
2435           }
2436         else
2437           {
2438             unsigned int a, b;
2439
2440             a = wRWORD (BITS (16, 19), i);
2441             b = wRWORD (BITS (0, 3), i);
2442
2443             if (a < b)
2444               s = a;
2445             else
2446               s = b;
2447
2448             r |= (s & 0xffffffff) << (i * 32);
2449           }
2450       break;
2451
2452     default:
2453       ARMul_UndefInstr (state, instr);
2454       return ARMul_DONE;
2455     }
2456
2457   wR [BITS (12, 15)] = r;
2458   wC [wCon] |= WCON_MUP;
2459
2460   return ARMul_DONE;
2461 }
2462
2463 static int
2464 WMUL (ARMword instr)
2465 {
2466   ARMdword r = 0;
2467   ARMdword s;
2468   int      i;
2469
2470   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
2471     return ARMul_CANT;
2472
2473 #ifdef DEBUG
2474   fprintf (stderr, "wmul\n");
2475 #endif
2476
2477   for (i = 0; i < 4; i++)
2478     if (BIT (21))       /* Signed.  */
2479       {
2480         long a, b;
2481
2482         a = wRHALF (BITS (16, 19), i);
2483         a = EXTEND16 (a);
2484
2485         b = wRHALF (BITS (0, 3), i);
2486         b = EXTEND16 (b);
2487
2488         s = a * b;
2489
2490         if (BIT (20))
2491           r |= ((s >> 16) & 0xffff) << (i * 16);
2492         else
2493           r |= (s & 0xffff) << (i * 16);
2494       }
2495     else                /* Unsigned.  */
2496       {
2497         unsigned long a, b;
2498
2499         a = wRHALF (BITS (16, 19), i);
2500         b = wRHALF (BITS (0, 3), i);
2501
2502         s = a * b;
2503
2504         if (BIT (20))
2505           r |= ((s >> 16) & 0xffff) << (i * 16);
2506         else
2507           r |= (s & 0xffff) << (i * 16);
2508       }
2509
2510   wR [BITS (12, 15)] = r;
2511   wC [wCon] |= WCON_MUP;
2512
2513   return ARMul_DONE;
2514 }
2515
2516 static int
2517 WOR (ARMword instr)
2518 {
2519   ARMword psr = 0;
2520   ARMdword result;
2521
2522   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
2523     return ARMul_CANT;
2524
2525 #ifdef DEBUG
2526   fprintf (stderr, "wor\n");
2527 #endif
2528
2529   result = wR [BITS (16, 19)] | wR [BITS (0, 3)];
2530   wR [BITS (12, 15)] = result;
2531
2532   SIMD64_SET (psr, (result == 0), SIMD_ZBIT);
2533   SIMD64_SET (psr, (result & (1ULL << 63)), SIMD_NBIT);
2534
2535   wC [wCASF] = psr;
2536   wC [wCon] |= (WCON_CUP | WCON_MUP);
2537
2538   return ARMul_DONE;
2539 }
2540
2541 static int
2542 WPACK (ARMul_State * state, ARMword instr)
2543 {
2544   ARMdword r = 0;
2545   ARMword  psr = 0;
2546   ARMdword x;
2547   ARMdword s;
2548   int      i;
2549   int      satrv[8];
2550
2551   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
2552     return ARMul_CANT;
2553
2554 #ifdef DEBUG
2555   fprintf (stderr, "wpack\n");
2556 #endif
2557
2558   switch (BITS (22, 23))
2559     {
2560     case Hqual:
2561       for (i = 0; i < 8; i++)
2562         {
2563           x = wRHALF (i < 4 ? BITS (16, 19) : BITS (0, 3), i & 3);
2564
2565           switch (BITS (20, 21))
2566             {
2567             case UnsignedSaturation:
2568               s = IwmmxtSaturateU8 (x, satrv + BITIDX8 (i));
2569               break;
2570
2571             case SignedSaturation:
2572               s = IwmmxtSaturateS8 (x, satrv + BITIDX8 (i));
2573               break;
2574
2575             default:
2576               ARMul_UndefInstr (state, instr);
2577               return ARMul_DONE;
2578             }
2579
2580           r |= (s & 0xff) << (i * 8);
2581           SIMD8_SET (psr, NBIT8 (s), SIMD_NBIT, i);
2582           SIMD8_SET (psr, ZBIT8 (s), SIMD_ZBIT, i);
2583         }
2584       break;
2585
2586     case Wqual:
2587       satrv[0] = satrv[2] = satrv[4] = satrv[6] = 0;
2588
2589       for (i = 0; i < 4; i++)
2590         {
2591           x = wRWORD (i < 2 ? BITS (16, 19) : BITS (0, 3), i & 1);
2592
2593           switch (BITS (20, 21))
2594             {
2595             case UnsignedSaturation:
2596               s = IwmmxtSaturateU16 (x, satrv + BITIDX16 (i));
2597               break;
2598
2599             case SignedSaturation:
2600               s = IwmmxtSaturateS16 (x, satrv + BITIDX16 (i));
2601               break;
2602
2603             default:
2604               ARMul_UndefInstr (state, instr);
2605               return ARMul_DONE;
2606             }
2607
2608           r |= (s & 0xffff) << (i * 16);
2609           SIMD16_SET (psr, NBIT16 (s), SIMD_NBIT, i);
2610           SIMD16_SET (psr, ZBIT16 (s), SIMD_ZBIT, i);
2611         }
2612       break;
2613
2614     case Dqual:
2615       satrv[0] = satrv[1] = satrv[2] = satrv[4] = satrv[5] = satrv[6] = 0;
2616
2617       for (i = 0; i < 2; i++)
2618         {
2619           x = wR [i ? BITS (0, 3) : BITS (16, 19)];
2620
2621           switch (BITS (20, 21))
2622             {
2623             case UnsignedSaturation:
2624               s = IwmmxtSaturateU32 (x, satrv + BITIDX32 (i));
2625               break;
2626
2627             case SignedSaturation:
2628               s = IwmmxtSaturateS32 (x, satrv + BITIDX32 (i));
2629               break;
2630
2631             default:
2632               ARMul_UndefInstr (state, instr);
2633               return ARMul_DONE;
2634             }
2635
2636           r |= (s & 0xffffffff) << (i * 32);
2637           SIMD32_SET (psr, NBIT32 (s), SIMD_NBIT, i);
2638           SIMD32_SET (psr, ZBIT32 (s), SIMD_ZBIT, i);
2639         }
2640       break;
2641
2642     default:
2643       ARMul_UndefInstr (state, instr);
2644       return ARMul_DONE;
2645     }
2646
2647   wC [wCASF] = psr;
2648   wR [BITS (12, 15)] = r;
2649   SET_wCSSFvec (satrv);
2650   wC [wCon] |= (WCON_CUP | WCON_MUP);
2651
2652   return ARMul_DONE;
2653 }
2654
2655 static int
2656 WROR (ARMul_State * state, ARMword instr)
2657 {
2658   ARMdword r = 0;
2659   ARMdword s;
2660   ARMword  psr = 0;
2661   int      i;
2662   int      shift;
2663
2664   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
2665     return ARMul_CANT;
2666
2667 #ifdef DEBUG
2668   fprintf (stderr, "wror\n");
2669 #endif
2670
2671   DECODE_G_BIT (state, instr, shift);
2672
2673   switch (BITS (22, 23))
2674     {
2675     case Hqual:
2676       shift &= 0xf;
2677       for (i = 0; i < 4; i++)
2678         {
2679           s = ((wRHALF (BITS (16, 19), i) & 0xffff) << (16 - shift))
2680             | ((wRHALF (BITS (16, 19), i) & 0xffff) >> shift);
2681           r |= (s & 0xffff) << (i * 16);
2682           SIMD16_SET (psr, NBIT16 (s), SIMD_NBIT, i);
2683           SIMD16_SET (psr, ZBIT16 (s), SIMD_ZBIT, i);
2684         }
2685       break;
2686
2687     case Wqual:
2688       shift &= 0x1f;
2689       for (i = 0; i < 2; i++)
2690         {
2691           s = ((wRWORD (BITS (16, 19), i) & 0xffffffff) << (32 - shift))
2692             | ((wRWORD (BITS (16, 19), i) & 0xffffffff) >> shift);
2693           r |= (s & 0xffffffff) << (i * 32);
2694           SIMD32_SET (psr, NBIT32 (s), SIMD_NBIT, i);
2695           SIMD32_SET (psr, ZBIT32 (s), SIMD_ZBIT, i);
2696         }
2697       break;
2698
2699     case Dqual:
2700       shift &= 0x3f;
2701       r = (wR [BITS (16, 19)] >> shift)
2702         | (wR [BITS (16, 19)] << (64 - shift));
2703
2704       SIMD64_SET (psr, NBIT64 (r), SIMD_NBIT);
2705       SIMD64_SET (psr, ZBIT64 (r), SIMD_ZBIT);
2706       break;
2707
2708     default:
2709       ARMul_UndefInstr (state, instr);
2710       return ARMul_DONE;
2711     }
2712
2713   wC [wCASF] = psr;
2714   wR [BITS (12, 15)] = r;
2715   wC [wCon] |= (WCON_CUP | WCON_MUP);
2716
2717   return ARMul_DONE;
2718 }
2719
2720 static int
2721 WSAD (ARMword instr)
2722 {
2723   ARMdword r;
2724   int      s;
2725   int      i;
2726
2727   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
2728     return ARMul_CANT;
2729
2730 #ifdef DEBUG
2731   fprintf (stderr, "wsad\n");
2732 #endif
2733
2734   /* Z bit.  */
2735   r = BIT (20) ? 0 : (wR [BITS (12, 15)] & 0xffffffff);
2736
2737   if (BIT (22))
2738     /* Half.  */
2739     for (i = 0; i < 4; i++)
2740       {
2741         s = (wRHALF (BITS (16, 19), i) - wRHALF (BITS (0, 3), i));
2742         r += abs (s);
2743       }
2744   else
2745     /* Byte.  */
2746     for (i = 0; i < 8; i++)
2747       {
2748         s = (wRBYTE (BITS (16, 19), i) - wRBYTE (BITS (0, 3), i));
2749         r += abs (s);
2750       }
2751
2752   wR [BITS (12, 15)] = r;
2753   wC [wCon] |= WCON_MUP;
2754
2755   return ARMul_DONE;
2756 }
2757
2758 static int
2759 WSHUFH (ARMword instr)
2760 {
2761   ARMdword r = 0;
2762   ARMword  psr = 0;
2763   ARMdword s;
2764   int      i;
2765   int      imm8;
2766
2767   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
2768     return ARMul_CANT;
2769
2770 #ifdef DEBUG
2771   fprintf (stderr, "wshufh\n");
2772 #endif
2773
2774   imm8 = (BITS (20, 23) << 4) | BITS (0, 3);
2775
2776   for (i = 0; i < 4; i++)
2777     {
2778       s = wRHALF (BITS (16, 19), ((imm8 >> (i * 2) & 3)) & 0xff);
2779       r |= (s & 0xffff) << (i * 16);
2780       SIMD16_SET (psr, NBIT16 (s), SIMD_NBIT, i);
2781       SIMD16_SET (psr, ZBIT16 (s), SIMD_ZBIT, i);
2782     }
2783
2784   wC [wCASF] = psr;
2785   wR [BITS (12, 15)] = r;
2786   wC [wCon] |= (WCON_CUP | WCON_MUP);
2787
2788   return ARMul_DONE;
2789 }
2790
2791 static int
2792 WSLL (ARMul_State * state, ARMword instr)
2793 {
2794   ARMdword r = 0;
2795   ARMdword s;
2796   ARMword  psr = 0;
2797   int      i;
2798   unsigned shift;
2799
2800   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
2801     return ARMul_CANT;
2802
2803 #ifdef DEBUG
2804   fprintf (stderr, "wsll\n");
2805 #endif
2806
2807   DECODE_G_BIT (state, instr, shift);
2808
2809   switch (BITS (22, 23))
2810     {
2811     case Hqual:
2812       for (i = 0; i < 4; i++)
2813         {
2814           if (shift > 15)
2815             s = 0;
2816           else
2817             s = ((wRHALF (BITS (16, 19), i) & 0xffff) << shift);
2818           r |= (s & 0xffff) << (i * 16);
2819           SIMD16_SET (psr, NBIT16 (s), SIMD_NBIT, i);
2820           SIMD16_SET (psr, ZBIT16 (s), SIMD_ZBIT, i);
2821         }
2822       break;
2823
2824     case Wqual:
2825       for (i = 0; i < 2; i++)
2826         {
2827           if (shift > 31)
2828             s = 0;
2829           else
2830             s = ((wRWORD (BITS (16, 19), i) & 0xffffffff) << shift);
2831           r |= (s & 0xffffffff) << (i * 32);
2832           SIMD32_SET (psr, NBIT32 (s), SIMD_NBIT, i);
2833           SIMD32_SET (psr, ZBIT32 (s), SIMD_ZBIT, i);
2834         }
2835       break;
2836
2837     case Dqual:
2838       if (shift > 63)
2839         r = 0;
2840       else
2841         r = ((wR[BITS (16, 19)] & 0xffffffffffffffffULL) << shift);
2842
2843       SIMD64_SET (psr, NBIT64 (r), SIMD_NBIT);
2844       SIMD64_SET (psr, ZBIT64 (r), SIMD_ZBIT);
2845       break;
2846
2847     default:
2848       ARMul_UndefInstr (state, instr);
2849       return ARMul_DONE;
2850     }
2851
2852   wC [wCASF] = psr;
2853   wR [BITS (12, 15)] = r;
2854   wC [wCon] |= (WCON_CUP | WCON_MUP);
2855
2856   return ARMul_DONE;
2857 }
2858
2859 static int
2860 WSRA (ARMul_State * state, ARMword instr)
2861 {
2862   ARMdword     r = 0;
2863   ARMdword     s;
2864   ARMword      psr = 0;
2865   int          i;
2866   unsigned     shift;
2867   signed long  t;
2868
2869   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
2870     return ARMul_CANT;
2871
2872 #ifdef DEBUG
2873   fprintf (stderr, "wsra\n");
2874 #endif
2875
2876   DECODE_G_BIT (state, instr, shift);
2877
2878   switch (BITS (22, 23))
2879     {
2880     case Hqual:
2881       for (i = 0; i < 4; i++)
2882         {
2883           if (shift > 15)
2884             t = (wRHALF (BITS (16, 19), i) & 0x8000) ? 0xffff : 0;
2885           else
2886             {
2887               t = wRHALF (BITS (16, 19), i);
2888               t = EXTEND16 (t);
2889               t >>= shift;
2890             }
2891
2892           s = t;
2893           r |= (s & 0xffff) << (i * 16);
2894           SIMD16_SET (psr, NBIT16 (s), SIMD_NBIT, i);
2895           SIMD16_SET (psr, ZBIT16 (s), SIMD_ZBIT, i);
2896         }
2897       break;
2898
2899     case Wqual:
2900       for (i = 0; i < 2; i++)
2901         {
2902           if (shift > 31)
2903             t = (wRWORD (BITS (16, 19), i) & 0x80000000) ? 0xffffffff : 0;
2904           else
2905             {
2906               t = wRWORD (BITS (16, 19), i);
2907               t >>= shift;
2908             }
2909           s = t;
2910           r |= (s & 0xffffffff) << (i * 32);
2911           SIMD32_SET (psr, NBIT32 (s), SIMD_NBIT, i);
2912           SIMD32_SET (psr, ZBIT32 (s), SIMD_ZBIT, i);
2913         }
2914       break;
2915
2916     case Dqual:
2917       if (shift > 63)
2918         r = (wR [BITS (16, 19)] & 0x8000000000000000ULL) ? 0xffffffffffffffffULL : 0;
2919       else
2920         r = ((signed long long) (wR[BITS (16, 19)] & 0xffffffffffffffffULL) >> shift);
2921       SIMD64_SET (psr, NBIT64 (r), SIMD_NBIT);
2922       SIMD64_SET (psr, ZBIT64 (r), SIMD_ZBIT);
2923       break;
2924
2925     default:
2926       ARMul_UndefInstr (state, instr);
2927       return ARMul_DONE;
2928     }
2929
2930   wC [wCASF] = psr;
2931   wR [BITS (12, 15)] = r;
2932   wC [wCon] |= (WCON_CUP | WCON_MUP);
2933
2934   return ARMul_DONE;
2935 }
2936
2937 static int
2938 WSRL (ARMul_State * state, ARMword instr)
2939 {
2940   ARMdword     r = 0;
2941   ARMdword     s;
2942   ARMword      psr = 0;
2943   int          i;
2944   unsigned int shift;
2945
2946   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
2947     return ARMul_CANT;
2948
2949 #ifdef DEBUG
2950   fprintf (stderr, "wsrl\n");
2951 #endif
2952
2953   DECODE_G_BIT (state, instr, shift);
2954
2955   switch (BITS (22, 23))
2956     {
2957     case Hqual:
2958       for (i = 0; i < 4; i++)
2959         {
2960           if (shift > 15)
2961             s = 0;
2962           else
2963             s = ((unsigned) (wRHALF (BITS (16, 19), i) & 0xffff) >> shift);
2964
2965           r |= (s & 0xffff) << (i * 16);
2966           SIMD16_SET (psr, NBIT16 (s), SIMD_NBIT, i);
2967           SIMD16_SET (psr, ZBIT16 (s), SIMD_ZBIT, i);
2968         }
2969       break;
2970
2971     case Wqual:
2972       for (i = 0; i < 2; i++)
2973         {
2974           if (shift > 31)
2975             s = 0;
2976           else
2977             s = ((unsigned long) (wRWORD (BITS (16, 19), i) & 0xffffffff) >> shift);
2978
2979           r |= (s & 0xffffffff) << (i * 32);
2980           SIMD32_SET (psr, NBIT32 (s), SIMD_NBIT, i);
2981           SIMD32_SET (psr, ZBIT32 (s), SIMD_ZBIT, i);
2982         }
2983       break;
2984
2985     case Dqual:
2986       if (shift > 63)
2987         r = 0;
2988       else
2989         r = (wR [BITS (16, 19)] & 0xffffffffffffffffULL) >> shift;
2990
2991       SIMD64_SET (psr, NBIT64 (r), SIMD_NBIT);
2992       SIMD64_SET (psr, ZBIT64 (r), SIMD_ZBIT);
2993       break;
2994
2995     default:
2996       ARMul_UndefInstr (state, instr);
2997       return ARMul_DONE;
2998     }
2999
3000   wC [wCASF] = psr;
3001   wR [BITS (12, 15)] = r;
3002   wC [wCon] |= (WCON_CUP | WCON_MUP);
3003
3004   return ARMul_DONE;
3005 }
3006
3007 static int
3008 WSTR (ARMul_State * state, ARMword instr)
3009 {
3010   ARMword address;
3011   int failed;
3012
3013
3014   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
3015     return ARMul_CANT;
3016
3017 #ifdef DEBUG
3018   fprintf (stderr, "wstr\n");
3019 #endif
3020
3021   address = Compute_Iwmmxt_Address (state, instr, & failed);
3022   if (failed)
3023     return ARMul_CANT;
3024
3025   if (BITS (28, 31) == 0xf)
3026     {
3027       /* WSTRW wCx */
3028       Iwmmxt_Store_Word (state, address, wC [BITS (12, 15)]);
3029     }
3030   else if (BIT (8) == 0)
3031     {
3032       if (BIT (22) == 0)
3033         /* WSTRB */
3034         Iwmmxt_Store_Byte (state, address, wR [BITS (12, 15)]);
3035       else
3036         /* WSTRH */
3037         Iwmmxt_Store_Half_Word (state, address, wR [BITS (12, 15)]);
3038     }
3039   else
3040     {
3041       if (BIT (22) == 0)
3042         /* WSTRW wRd */
3043         Iwmmxt_Store_Word (state, address, wR [BITS (12, 15)]);
3044       else
3045         /* WSTRD */
3046         Iwmmxt_Store_Double_Word (state, address, wR [BITS (12, 15)]);
3047     }
3048
3049   return ARMul_DONE;
3050 }
3051
3052 static int
3053 WSUB (ARMul_State * state, ARMword instr)
3054 {
3055   ARMdword r = 0;
3056   ARMword  psr = 0;
3057   ARMdword x;
3058   ARMdword s;
3059   int      i;
3060   int      carry;
3061   int      overflow;
3062   int      satrv[8];
3063
3064   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
3065     return ARMul_CANT;
3066
3067 #ifdef DEBUG
3068   fprintf (stderr, "wsub\n");
3069 #endif
3070
3071 /* Subtract two numbers using the specified function,
3072    leaving setting the carry bit as required.  */
3073 #define SUBx(x, y, m, f) \
3074    (*f) (wRBITS (BITS (16, 19), (x), (y)) & (m), \
3075          wRBITS (BITS ( 0,  3), (x), (y)) & (m), & carry, & overflow)
3076
3077   switch (BITS (22, 23))
3078     {
3079     case Bqual:
3080       for (i = 0; i < 8; i++)
3081         {
3082           switch (BITS (20, 21))
3083             {
3084             case NoSaturation:
3085               s = SUBx ((i * 8), (i * 8) + 7, 0xff, SubS8);
3086               satrv [BITIDX8 (i)] = 0;
3087               r |= (s & 0xff) << (i * 8);
3088               SIMD8_SET (psr, NBIT8 (s), SIMD_NBIT, i);
3089               SIMD8_SET (psr, ZBIT8 (s), SIMD_ZBIT, i);
3090               SIMD8_SET (psr, carry, SIMD_CBIT, i);
3091               SIMD8_SET (psr, overflow, SIMD_VBIT, i);
3092               break;
3093
3094             case UnsignedSaturation:
3095               s = SUBx ((i * 8), (i * 8) + 7, 0xff, SubU8);
3096               x = IwmmxtSaturateU8 (s, satrv + BITIDX8 (i));
3097               r |= (x & 0xff) << (i * 8);
3098               SIMD8_SET (psr, NBIT8 (x), SIMD_NBIT, i);
3099               SIMD8_SET (psr, ZBIT8 (x), SIMD_ZBIT, i);
3100               if (! satrv [BITIDX8 (i)])
3101                 {
3102                   SIMD8_SET (psr, carry,     SIMD_CBIT, i);
3103                   SIMD8_SET (psr, overflow, SIMD_VBIT, i);
3104                 }
3105               break;
3106
3107             case SignedSaturation:
3108               s = SUBx ((i * 8), (i * 8) + 7, 0xff, SubS8);
3109               x = IwmmxtSaturateS8 (s, satrv + BITIDX8 (i));
3110               r |= (x & 0xff) << (i * 8);
3111               SIMD8_SET (psr, NBIT8 (x), SIMD_NBIT, i);
3112               SIMD8_SET (psr, ZBIT8 (x), SIMD_ZBIT, i);
3113               if (! satrv [BITIDX8 (i)])
3114                 {
3115                   SIMD8_SET (psr, carry,     SIMD_CBIT, i);
3116                   SIMD8_SET (psr, overflow, SIMD_VBIT, i);
3117                 }
3118               break;
3119
3120             default:
3121               ARMul_UndefInstr (state, instr);
3122               return ARMul_DONE;
3123             }
3124         }
3125       break;
3126
3127     case Hqual:
3128       satrv[0] = satrv[2] = satrv[4] = satrv[6] = 0;
3129
3130       for (i = 0; i < 4; i++)
3131         {
3132           switch (BITS (20, 21))
3133             {
3134             case NoSaturation:
3135               s = SUBx ((i * 16), (i * 16) + 15, 0xffff, SubU16);
3136               satrv [BITIDX16 (i)] = 0;
3137               r |= (s & 0xffff) << (i * 16);
3138               SIMD16_SET (psr, NBIT16 (s), SIMD_NBIT, i);
3139               SIMD16_SET (psr, ZBIT16 (s), SIMD_ZBIT, i);
3140               SIMD16_SET (psr, carry,      SIMD_CBIT, i);
3141               SIMD16_SET (psr, overflow,   SIMD_VBIT, i);
3142               break;
3143
3144             case UnsignedSaturation:
3145               s = SUBx ((i * 16), (i * 16) + 15, 0xffff, SubU16);
3146               x = IwmmxtSaturateU16 (s, satrv + BITIDX16 (i));
3147               r |= (x & 0xffff) << (i * 16);
3148               SIMD16_SET (psr, NBIT16 (x & 0xffff), SIMD_NBIT, i);
3149               SIMD16_SET (psr, ZBIT16 (x), SIMD_ZBIT, i);
3150               if (! satrv [BITIDX16 (i)])
3151                 {
3152                   SIMD16_SET (psr, carry,    SIMD_CBIT, i);
3153                   SIMD16_SET (psr, overflow, SIMD_VBIT, i);
3154                 }
3155               break;
3156
3157             case SignedSaturation:
3158               s = SUBx ((i * 16), (i * 16) + 15, 0xffff, SubS16);
3159               x = IwmmxtSaturateS16 (s, satrv + BITIDX16 (i));
3160               r |= (x & 0xffff) << (i * 16);
3161               SIMD16_SET (psr, NBIT16 (x), SIMD_NBIT, i);
3162               SIMD16_SET (psr, ZBIT16 (x), SIMD_ZBIT, i);
3163               if (! satrv [BITIDX16 (i)])
3164                 {
3165                   SIMD16_SET (psr, carry,    SIMD_CBIT, i);
3166                   SIMD16_SET (psr, overflow, SIMD_VBIT, i);
3167                 }
3168               break;
3169
3170             default:
3171               ARMul_UndefInstr (state, instr);
3172               return ARMul_DONE;
3173             }
3174         }
3175       break;
3176
3177     case Wqual:
3178       satrv[0] = satrv[1] = satrv[2] = satrv[4] = satrv[5] = satrv[6] = 0;
3179
3180       for (i = 0; i < 2; i++)
3181         {
3182           switch (BITS (20, 21))
3183             {
3184             case NoSaturation:
3185               s = SUBx ((i * 32), (i * 32) + 31, 0xffffffff, SubU32);
3186               satrv[BITIDX32 (i)] = 0;
3187               r |= (s & 0xffffffff) << (i * 32);
3188               SIMD32_SET (psr, NBIT32 (s), SIMD_NBIT, i);
3189               SIMD32_SET (psr, ZBIT32 (s), SIMD_ZBIT, i);
3190               SIMD32_SET (psr, carry,      SIMD_CBIT, i);
3191               SIMD32_SET (psr, overflow,   SIMD_VBIT, i);
3192               break;
3193
3194             case UnsignedSaturation:
3195               s = SUBx ((i * 32), (i * 32) + 31, 0xffffffff, SubU32);
3196               x = IwmmxtSaturateU32 (s, satrv + BITIDX32 (i));
3197               r |= (x & 0xffffffff) << (i * 32);
3198               SIMD32_SET (psr, NBIT32 (x), SIMD_NBIT, i);
3199               SIMD32_SET (psr, ZBIT32 (x), SIMD_ZBIT, i);
3200               if (! satrv [BITIDX32 (i)])
3201                 {
3202                   SIMD32_SET (psr, carry,    SIMD_CBIT, i);
3203                   SIMD32_SET (psr, overflow, SIMD_VBIT, i);
3204                 }
3205               break;
3206
3207             case SignedSaturation:
3208               s = SUBx ((i * 32), (i * 32) + 31, 0xffffffff, SubS32);
3209               x = IwmmxtSaturateS32 (s, satrv + BITIDX32 (i));
3210               r |= (x & 0xffffffff) << (i * 32);
3211               SIMD32_SET (psr, NBIT32 (x), SIMD_NBIT, i);
3212               SIMD32_SET (psr, ZBIT32 (x), SIMD_ZBIT, i);
3213               if (! satrv [BITIDX32 (i)])
3214                 {
3215                   SIMD32_SET (psr, carry,    SIMD_CBIT, i);
3216                   SIMD32_SET (psr, overflow, SIMD_VBIT, i);
3217                 }
3218               break;
3219
3220             default:
3221               ARMul_UndefInstr (state, instr);
3222               return ARMul_DONE;
3223             }
3224         }
3225       break;
3226
3227     default:
3228       ARMul_UndefInstr (state, instr);
3229       return ARMul_DONE;
3230     }
3231
3232   wR [BITS (12, 15)] = r;
3233   wC [wCASF] = psr;
3234   SET_wCSSFvec (satrv);
3235   wC [wCon] |= (WCON_CUP | WCON_MUP);
3236
3237 #undef SUBx
3238
3239   return ARMul_DONE;
3240 }
3241
3242 static int
3243 WUNPCKEH (ARMul_State * state, ARMword instr)
3244 {
3245   ARMdword r = 0;
3246   ARMword  psr = 0;
3247   ARMdword s;
3248   int      i;
3249
3250   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
3251     return ARMul_CANT;
3252
3253 #ifdef DEBUG
3254   fprintf (stderr, "wunpckeh\n");
3255 #endif
3256
3257   switch (BITS (22, 23))
3258     {
3259     case Bqual:
3260       for (i = 0; i < 4; i++)
3261         {
3262           s = wRBYTE (BITS (16, 19), i + 4);
3263
3264           if (BIT (21) && NBIT8 (s))
3265             s |= 0xff00;
3266
3267           r |= (s & 0xffff) << (i * 16);
3268           SIMD16_SET (psr, NBIT16 (s), SIMD_NBIT, i);
3269           SIMD16_SET (psr, ZBIT16 (s), SIMD_ZBIT, i);
3270         }
3271       break;
3272
3273     case Hqual:
3274       for (i = 0; i < 2; i++)
3275         {
3276           s = wRHALF (BITS (16, 19), i + 2);
3277
3278           if (BIT (21) && NBIT16 (s))
3279             s |= 0xffff0000;
3280
3281           r |= (s & 0xffffffff) << (i * 32);
3282           SIMD32_SET (psr, NBIT32 (s), SIMD_NBIT, i);
3283           SIMD32_SET (psr, ZBIT32 (s), SIMD_ZBIT, i);
3284         }
3285       break;
3286
3287     case Wqual:
3288       r = wRWORD (BITS (16, 19), 1);
3289
3290       if (BIT (21) && NBIT32 (r))
3291         r |= 0xffffffff00000000ULL;
3292
3293       SIMD64_SET (psr, NBIT64 (r), SIMD_NBIT);
3294       SIMD64_SET (psr, ZBIT64 (r), SIMD_ZBIT);
3295       break;
3296
3297     default:
3298       ARMul_UndefInstr (state, instr);
3299       return ARMul_DONE;
3300     }
3301
3302   wC [wCASF] = psr;
3303   wR [BITS (12, 15)] = r;
3304   wC [wCon] |= (WCON_CUP | WCON_MUP);
3305
3306   return ARMul_DONE;
3307 }
3308
3309 static int
3310 WUNPCKEL (ARMul_State * state, ARMword instr)
3311 {
3312   ARMdword r = 0;
3313   ARMword  psr = 0;
3314   ARMdword s;
3315   int      i;
3316
3317   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
3318     return ARMul_CANT;
3319
3320 #ifdef DEBUG
3321   fprintf (stderr, "wunpckel\n");
3322 #endif
3323
3324   switch (BITS (22, 23))
3325     {
3326     case Bqual:
3327       for (i = 0; i < 4; i++)
3328         {
3329           s = wRBYTE (BITS (16, 19), i);
3330
3331           if (BIT (21) && NBIT8 (s))
3332             s |= 0xff00;
3333
3334           r |= (s & 0xffff) << (i * 16);
3335           SIMD16_SET (psr, NBIT16 (s), SIMD_NBIT, i);
3336           SIMD16_SET (psr, ZBIT16 (s), SIMD_ZBIT, i);
3337         }
3338       break;
3339
3340     case Hqual:
3341       for (i = 0; i < 2; i++)
3342         {
3343           s = wRHALF (BITS (16, 19), i);
3344
3345           if (BIT (21) && NBIT16 (s))
3346             s |= 0xffff0000;
3347
3348           r |= (s & 0xffffffff) << (i * 32);
3349           SIMD32_SET (psr, NBIT32 (s), SIMD_NBIT, i);
3350           SIMD32_SET (psr, ZBIT32 (s), SIMD_ZBIT, i);
3351         }
3352       break;
3353
3354     case Wqual:
3355       r = wRWORD (BITS (16, 19), 0);
3356
3357       if (BIT (21) && NBIT32 (r))
3358         r |= 0xffffffff00000000ULL;
3359
3360       SIMD64_SET (psr, NBIT64 (r), SIMD_NBIT);
3361       SIMD64_SET (psr, ZBIT64 (r), SIMD_ZBIT);
3362       break;
3363
3364     default:
3365       ARMul_UndefInstr (state, instr);
3366       return ARMul_DONE;
3367     }
3368
3369   wC [wCASF] = psr;
3370   wR [BITS (12, 15)] = r;
3371   wC [wCon] |= (WCON_CUP | WCON_MUP);
3372
3373   return ARMul_DONE;
3374 }
3375
3376 static int
3377 WUNPCKIH (ARMul_State * state, ARMword instr)
3378 {
3379   ARMword  a, b;
3380   ARMdword r = 0;
3381   ARMword  psr = 0;
3382   ARMdword s;
3383   int      i;
3384
3385   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
3386     return ARMul_CANT;
3387
3388 #ifdef DEBUG
3389   fprintf (stderr, "wunpckih\n");
3390 #endif
3391
3392   switch (BITS (22, 23))
3393     {
3394     case Bqual:
3395       for (i = 0; i < 4; i++)
3396         {
3397           a = wRBYTE (BITS (16, 19), i + 4);
3398           b = wRBYTE (BITS ( 0,  3), i + 4);
3399           s = a | (b << 8);
3400           r |= (s & 0xffff) << (i * 16);
3401           SIMD8_SET (psr, NBIT8 (a), SIMD_NBIT, i * 2);
3402           SIMD8_SET (psr, ZBIT8 (a), SIMD_ZBIT, i * 2);
3403           SIMD8_SET (psr, NBIT8 (b), SIMD_NBIT, (i * 2) + 1);
3404           SIMD8_SET (psr, ZBIT8 (b), SIMD_ZBIT, (i * 2) + 1);
3405         }
3406       break;
3407
3408     case Hqual:
3409       for (i = 0; i < 2; i++)
3410         {
3411           a = wRHALF (BITS (16, 19), i + 2);
3412           b = wRHALF (BITS ( 0,  3), i + 2);
3413           s = a | (b << 16);
3414           r |= (s & 0xffffffff) << (i * 32);
3415           SIMD16_SET (psr, NBIT16 (a), SIMD_NBIT, (i * 2));
3416           SIMD16_SET (psr, ZBIT16 (a), SIMD_ZBIT, (i * 2));
3417           SIMD16_SET (psr, NBIT16 (b), SIMD_NBIT, (i * 2) + 1);
3418           SIMD16_SET (psr, ZBIT16 (b), SIMD_ZBIT, (i * 2) + 1);
3419         }
3420       break;
3421
3422     case Wqual:
3423       a = wRWORD (BITS (16, 19), 1);
3424       s = wRWORD (BITS ( 0,  3), 1);
3425       r = a | (s << 32);
3426
3427       SIMD32_SET (psr, NBIT32 (a), SIMD_NBIT, 0);
3428       SIMD32_SET (psr, ZBIT32 (a), SIMD_ZBIT, 0);
3429       SIMD32_SET (psr, NBIT32 (s), SIMD_NBIT, 1);
3430       SIMD32_SET (psr, ZBIT32 (s), SIMD_ZBIT, 1);
3431       break;
3432
3433     default:
3434       ARMul_UndefInstr (state, instr);
3435       return ARMul_DONE;
3436     }
3437
3438   wC [wCASF] = psr;
3439   wR [BITS (12, 15)] = r;
3440   wC [wCon] |= (WCON_CUP | WCON_MUP);
3441
3442   return ARMul_DONE;
3443 }
3444
3445 static int
3446 WUNPCKIL (ARMul_State * state, ARMword instr)
3447 {
3448   ARMword  a, b;
3449   ARMdword r = 0;
3450   ARMword  psr = 0;
3451   ARMdword s;
3452   int      i;
3453
3454   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
3455     return ARMul_CANT;
3456
3457 #ifdef DEBUG
3458   fprintf (stderr, "wunpckil\n");
3459 #endif
3460
3461   switch (BITS (22, 23))
3462     {
3463     case Bqual:
3464       for (i = 0; i < 4; i++)
3465         {
3466           a = wRBYTE (BITS (16, 19), i);
3467           b = wRBYTE (BITS ( 0,  3), i);
3468           s = a | (b << 8);
3469           r |= (s & 0xffff) << (i * 16);
3470           SIMD8_SET (psr, NBIT8 (a), SIMD_NBIT, i * 2);
3471           SIMD8_SET (psr, ZBIT8 (a), SIMD_ZBIT, i * 2);
3472           SIMD8_SET (psr, NBIT8 (b), SIMD_NBIT, (i * 2) + 1);
3473           SIMD8_SET (psr, ZBIT8 (b), SIMD_ZBIT, (i * 2) + 1);
3474         }
3475       break;
3476
3477     case Hqual:
3478       for (i = 0; i < 2; i++)
3479         {
3480           a = wRHALF (BITS (16, 19), i);
3481           b = wRHALF (BITS ( 0,  3), i);
3482           s = a | (b << 16);
3483           r |= (s & 0xffffffff) << (i * 32);
3484           SIMD16_SET (psr, NBIT16 (a), SIMD_NBIT, (i * 2));
3485           SIMD16_SET (psr, ZBIT16 (a), SIMD_ZBIT, (i * 2));
3486           SIMD16_SET (psr, NBIT16 (b), SIMD_NBIT, (i * 2) + 1);
3487           SIMD16_SET (psr, ZBIT16 (b), SIMD_ZBIT, (i * 2) + 1);
3488         }
3489       break;
3490
3491     case Wqual:
3492       a = wRWORD (BITS (16, 19), 0);
3493       s = wRWORD (BITS ( 0,  3), 0);
3494       r = a | (s << 32);
3495
3496       SIMD32_SET (psr, NBIT32 (a), SIMD_NBIT, 0);
3497       SIMD32_SET (psr, ZBIT32 (a), SIMD_ZBIT, 0);
3498       SIMD32_SET (psr, NBIT32 (s), SIMD_NBIT, 1);
3499       SIMD32_SET (psr, ZBIT32 (s), SIMD_ZBIT, 1);
3500       break;
3501
3502     default:
3503       ARMul_UndefInstr (state, instr);
3504       return ARMul_DONE;
3505     }
3506
3507   wC [wCASF] = psr;
3508   wR [BITS (12, 15)] = r;
3509   wC [wCon] |= (WCON_CUP | WCON_MUP);
3510
3511   return ARMul_DONE;
3512 }
3513
3514 static int
3515 WXOR (ARMword instr)
3516 {
3517   ARMword psr = 0;
3518   ARMdword result;
3519
3520   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
3521     return ARMul_CANT;
3522
3523 #ifdef DEBUG
3524   fprintf (stderr, "wxor\n");
3525 #endif
3526
3527   result = wR [BITS (16, 19)] ^ wR [BITS (0, 3)];
3528   wR [BITS (12, 15)] = result;
3529
3530   SIMD64_SET (psr, (result == 0), SIMD_ZBIT);
3531   SIMD64_SET (psr, (result & (1ULL << 63)), SIMD_NBIT);
3532
3533   wC [wCASF] = psr;
3534   wC [wCon] |= (WCON_CUP | WCON_MUP);
3535
3536   return ARMul_DONE;
3537 }
3538
3539 /* This switch table is moved to a seperate function in order
3540    to work around a compiler bug in the host compiler...  */
3541
3542 static int
3543 Process_Instruction (ARMul_State * state, ARMword instr)
3544 {
3545   int status = ARMul_BUSY;
3546
3547   switch ((BITS (20, 23) << 8) | BITS (4, 11))
3548     {
3549     case 0x000: status = WOR (instr); break;
3550     case 0x011: status = TMCR (state, instr); break;
3551     case 0x100: status = WXOR (instr); break;
3552     case 0x111: status = TMRC (state, instr); break;
3553     case 0x300: status = WANDN (instr); break;
3554     case 0x200: status = WAND (instr); break;
3555
3556     case 0x810: case 0xa10:
3557       status = WMADD (instr); break;
3558
3559     case 0x10e: case 0x50e: case 0x90e: case 0xd0e:
3560       status = WUNPCKIL (state, instr); break;
3561     case 0x10c: case 0x50c: case 0x90c: case 0xd0c:
3562       status = WUNPCKIH (state, instr); break;
3563     case 0x012: case 0x112: case 0x412: case 0x512:
3564       status = WSAD (instr); break;
3565     case 0x010: case 0x110: case 0x210: case 0x310:
3566       status = WMUL (instr); break;
3567     case 0x410: case 0x510: case 0x610: case 0x710:
3568       status = WMAC (instr); break;
3569     case 0x006: case 0x406: case 0x806: case 0xc06:
3570       status = WCMPEQ (state, instr); break;
3571     case 0x800: case 0x900: case 0xc00: case 0xd00:
3572       status = WAVG2 (instr); break;
3573     case 0x802: case 0x902: case 0xa02: case 0xb02:
3574       status = WALIGNR (state, instr); break;
3575     case 0x601: case 0x605: case 0x609: case 0x60d:
3576       status = TINSR (state, instr); break;
3577     case 0x107: case 0x507: case 0x907: case 0xd07:
3578       status = TEXTRM (state, instr); break;
3579     case 0x117: case 0x517: case 0x917: case 0xd17:
3580       status = TEXTRC (state, instr); break;
3581     case 0x401: case 0x405: case 0x409: case 0x40d:
3582       status = TBCST (state, instr); break;
3583     case 0x113: case 0x513: case 0x913: case 0xd13:
3584       status = TANDC (state, instr); break;
3585     case 0x01c: case 0x41c: case 0x81c: case 0xc1c:
3586       status = WACC (state, instr); break;
3587     case 0x115: case 0x515: case 0x915: case 0xd15:
3588       status = TORC (state, instr); break;
3589     case 0x103: case 0x503: case 0x903: case 0xd03:
3590       status = TMOVMSK (state, instr); break;
3591     case 0x106: case 0x306: case 0x506: case 0x706:
3592     case 0x906: case 0xb06: case 0xd06: case 0xf06:
3593       status = WCMPGT (state, instr); break;
3594     case 0x00e: case 0x20e: case 0x40e: case 0x60e:
3595     case 0x80e: case 0xa0e: case 0xc0e: case 0xe0e:
3596       status = WUNPCKEL (state, instr); break;
3597     case 0x00c: case 0x20c: case 0x40c: case 0x60c:
3598     case 0x80c: case 0xa0c: case 0xc0c: case 0xe0c:
3599       status = WUNPCKEH (state, instr); break;
3600     case 0x204: case 0x604: case 0xa04: case 0xe04:
3601     case 0x214: case 0x614: case 0xa14: case 0xe14:
3602       status = WSRL (state, instr); break;
3603     case 0x004: case 0x404: case 0x804: case 0xc04:
3604     case 0x014: case 0x414: case 0x814: case 0xc14:
3605       status = WSRA (state, instr); break;
3606     case 0x104: case 0x504: case 0x904: case 0xd04:
3607     case 0x114: case 0x514: case 0x914: case 0xd14:
3608       status = WSLL (state, instr); break;
3609     case 0x304: case 0x704: case 0xb04: case 0xf04:
3610     case 0x314: case 0x714: case 0xb14: case 0xf14:
3611       status = WROR (state, instr); break;
3612     case 0x116: case 0x316: case 0x516: case 0x716:
3613     case 0x916: case 0xb16: case 0xd16: case 0xf16:
3614       status = WMIN (state, instr); break;
3615     case 0x016: case 0x216: case 0x416: case 0x616:
3616     case 0x816: case 0xa16: case 0xc16: case 0xe16:
3617       status = WMAX (state, instr); break;
3618     case 0x002: case 0x102: case 0x202: case 0x302:
3619     case 0x402: case 0x502: case 0x602: case 0x702:
3620       status = WALIGNI (instr); break;
3621     case 0x01a: case 0x11a: case 0x21a: case 0x31a:
3622     case 0x41a: case 0x51a: case 0x61a: case 0x71a:
3623     case 0x81a: case 0x91a: case 0xa1a: case 0xb1a:
3624     case 0xc1a: case 0xd1a: case 0xe1a: case 0xf1a:
3625       status = WSUB (state, instr); break;
3626     case 0x01e: case 0x11e: case 0x21e: case 0x31e:
3627     case 0x41e: case 0x51e: case 0x61e: case 0x71e:
3628     case 0x81e: case 0x91e: case 0xa1e: case 0xb1e:
3629     case 0xc1e: case 0xd1e: case 0xe1e: case 0xf1e:
3630       status = WSHUFH (instr); break;
3631     case 0x018: case 0x118: case 0x218: case 0x318:
3632     case 0x418: case 0x518: case 0x618: case 0x718:
3633     case 0x818: case 0x918: case 0xa18: case 0xb18:
3634     case 0xc18: case 0xd18: case 0xe18: case 0xf18:
3635       status = WADD (state, instr); break;
3636     case 0x008: case 0x108: case 0x208: case 0x308:
3637     case 0x408: case 0x508: case 0x608: case 0x708:
3638     case 0x808: case 0x908: case 0xa08: case 0xb08:
3639     case 0xc08: case 0xd08: case 0xe08: case 0xf08:
3640       status = WPACK (state, instr); break;
3641     case 0x201: case 0x203: case 0x205: case 0x207:
3642     case 0x209: case 0x20b: case 0x20d: case 0x20f:
3643     case 0x211: case 0x213: case 0x215: case 0x217:
3644     case 0x219: case 0x21b: case 0x21d: case 0x21f:
3645       switch (BITS (16, 19))
3646         {
3647         case 0x0: status = TMIA (state, instr); break;
3648         case 0x8: status = TMIAPH (state, instr); break;
3649         case 0xc:
3650         case 0xd:
3651         case 0xe:
3652         case 0xf: status = TMIAxy (state, instr); break;
3653         default: break;
3654         }
3655       break;
3656     default:
3657       break;
3658     }
3659   return status;
3660 }
3661
3662 /* Process a possibly Intel(r) Wireless MMX(tm) technology instruction.
3663    Return true if the instruction was handled.  */
3664
3665 int
3666 ARMul_HandleIwmmxt (ARMul_State * state, ARMword instr)
3667 {
3668   int status = ARMul_BUSY;
3669
3670   if (BITS (24, 27) == 0xe)
3671     {
3672       status = Process_Instruction (state, instr);
3673     }
3674   else if (BITS (25, 27) == 0x6)
3675     {
3676       if (BITS (4, 11) == 0x0 && BITS (20, 24) == 0x4)
3677         status = TMCRR (state, instr);
3678       else if (BITS (9, 11) == 0x0)
3679         {
3680           if (BIT (20) == 0x0)
3681             status = WSTR (state, instr);
3682           else if (BITS (20, 24) == 0x5)
3683             status = TMRRC (state, instr);
3684           else
3685             status = WLDR (state, instr);
3686         }
3687     }
3688
3689   if (status == ARMul_CANT)
3690     {
3691       /* If the instruction was a recognised but illegal,
3692          perform the abort here rather than returning false.
3693          If we return false then ARMul_MRC may be called which
3694          will still abort, but which also perform the register
3695          transfer...  */
3696       ARMul_Abort (state, ARMul_UndefinedInstrV);
3697       status = ARMul_DONE;
3698     }
3699
3700   return status == ARMul_DONE;
3701 }
3702
3703 int
3704 Fetch_Iwmmxt_Register (unsigned int regnum, unsigned char * memory)
3705 {
3706   if (regnum >= 16)
3707     {
3708       memcpy (memory, wC + (regnum - 16), sizeof wC [0]);
3709       return sizeof wC [0];
3710     }
3711   else
3712     {
3713       memcpy (memory, wR + regnum, sizeof wR [0]);
3714       return sizeof wR [0];
3715     }
3716 }
3717
3718 int
3719 Store_Iwmmxt_Register (unsigned int regnum, unsigned char * memory)
3720 {
3721   if (regnum >= 16)
3722     {
3723       memcpy (wC + (regnum - 16), memory, sizeof wC [0]);
3724       return sizeof wC [0];
3725     }
3726   else
3727     {
3728       memcpy (wR + regnum, memory, sizeof wR [0]);
3729       return sizeof wR [0];
3730     }
3731 }