sim/arm/iwmmxt.c

   1 /*  iwmmxt.c -- Intel(r) Wireless MMX(tm) technology co-processor interface.
   2     Copyright (C) 2002, 2007-2012 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 = EXTEND32 (wRWORD (BITS (16, 19), i));
1747               b = EXTEND32 (wRWORD (BITS (0, 3), i));
1748
1749               s = (a > b) ? 0xffffffff : 0;
1750               r |= s << (i * 32);
1751
1752               SIMD32_SET (psr, NBIT32 (s), SIMD_NBIT, i);
1753               SIMD32_SET (psr, ZBIT32 (s), SIMD_ZBIT, i);
1754             }
1755         }
1756       else
1757         {
1758           for (i = 0; i < 2; i++)
1759             {
1760               s = (wRWORD (BITS (16, 19), i) > wRWORD (BITS (0, 3), i))
1761                 ? 0xffffffff : 0;
1762               r |= s << (i * 32);
1763               SIMD32_SET (psr, NBIT32 (s), SIMD_NBIT, i);
1764               SIMD32_SET (psr, ZBIT32 (s), SIMD_ZBIT, i);
1765             }
1766         }
1767       break;
1768
1769     default:
1770       ARMul_UndefInstr (state, instr);
1771       return ARMul_DONE;
1772     }
1773
1774   wC [wCASF] = psr;
1775   wR [BITS (12, 15)] = r;
1776   wC [wCon] |= (WCON_CUP | WCON_MUP);
1777
1778   return ARMul_DONE;
1779 }
1780
1781 static ARMword
1782 Compute_Iwmmxt_Address (ARMul_State * state, ARMword instr, int * pFailed)
1783 {
1784   ARMword  Rn;
1785   ARMword  addr;
1786   ARMword  offset;
1787   ARMword  multiplier;
1788
1789   * pFailed  = 0;
1790   Rn         = BITS (16, 19);
1791   addr       = state->Reg [Rn];
1792   offset     = BITS (0, 7);
1793   multiplier = BIT (8) ? 4 : 1;
1794
1795   if (BIT (24)) /* P */
1796     {
1797       /* Pre Indexed Addressing.  */
1798       if (BIT (23))
1799         addr += offset * multiplier;
1800       else
1801         addr -= offset * multiplier;
1802
1803       /* Immediate Pre-Indexed.  */
1804       if (BIT (21)) /* W */
1805         {
1806           if (Rn == 15)
1807             {
1808               /* Writeback into R15 is UNPREDICTABLE.  */
1809 #ifdef DEBUG
1810               fprintf (stderr, "iWMMXt: writeback into r15\n");
1811 #endif
1812               * pFailed = 1;
1813             }
1814           else
1815             state->Reg [Rn] = addr;
1816         }
1817     }
1818   else
1819     {
1820       /* Post Indexed Addressing.  */
1821       if (BIT (21)) /* W */
1822         {
1823           /* Handle the write back of the final address.  */
1824           if (Rn == 15)
1825             {
1826               /* Writeback into R15 is UNPREDICTABLE.  */
1827 #ifdef DEBUG
1828               fprintf (stderr, "iWMMXt: writeback into r15\n");
1829 #endif
1830               * pFailed = 1;
1831             }
1832           else
1833             {
1834               ARMword  increment;
1835
1836               if (BIT (23))
1837                 increment = offset * multiplier;
1838               else
1839                 increment = - (offset * multiplier);
1840
1841               state->Reg [Rn] = addr + increment;
1842             }
1843         }
1844       else
1845         {
1846           /* P == 0, W == 0, U == 0 is UNPREDICTABLE.  */
1847           if (BIT (23) == 0)
1848             {
1849 #ifdef DEBUG
1850               fprintf (stderr, "iWMMXt: undefined addressing mode\n");
1851 #endif
1852               * pFailed = 1;
1853             }
1854         }
1855     }
1856
1857   return addr;
1858 }
1859
1860 static ARMdword
1861 Iwmmxt_Load_Double_Word (ARMul_State * state, ARMword address)
1862 {
1863   ARMdword value;
1864
1865   /* The address must be aligned on a 8 byte boundary.  */
1866   if (address & 0x7)
1867     {
1868       fprintf (stderr, "iWMMXt: At addr 0x%x: Unaligned double word load from 0x%x\n",
1869                (state->Reg[15] - 8) & ~0x3, address);
1870 #ifdef DEBUG
1871 #endif
1872       /* No need to check for alignment traps.  An unaligned
1873          double word load with alignment trapping disabled is
1874          UNPREDICTABLE.  */
1875       ARMul_Abort (state, ARMul_DataAbortV);
1876     }
1877
1878   /* Load the words.  */
1879   if (! state->bigendSig)
1880     {
1881       value = ARMul_LoadWordN (state, address + 4);
1882       value <<= 32;
1883       value |= ARMul_LoadWordN (state, address);
1884     }
1885   else
1886     {
1887       value = ARMul_LoadWordN (state, address);
1888       value <<= 32;
1889       value |= ARMul_LoadWordN (state, address + 4);
1890     }
1891
1892   /* Check for data aborts.  */
1893   if (state->Aborted)
1894     ARMul_Abort (state, ARMul_DataAbortV);
1895   else
1896     ARMul_Icycles (state, 2, 0L);
1897
1898   return value;
1899 }
1900
1901 static ARMword
1902 Iwmmxt_Load_Word (ARMul_State * state, ARMword address)
1903 {
1904   ARMword value;
1905
1906   /* Check for a misaligned address.  */
1907   if (address & 3)
1908     {
1909       if ((read_cp15_reg (1, 0, 0) & ARMul_CP15_R1_ALIGN))
1910         ARMul_Abort (state, ARMul_DataAbortV);
1911       else
1912         address &= ~ 3;
1913     }
1914
1915   value = ARMul_LoadWordN (state, address);
1916
1917   if (state->Aborted)
1918     ARMul_Abort (state, ARMul_DataAbortV);
1919   else
1920     ARMul_Icycles (state, 1, 0L);
1921
1922   return value;
1923 }
1924
1925 static ARMword
1926 Iwmmxt_Load_Half_Word (ARMul_State * state, ARMword address)
1927 {
1928   ARMword value;
1929
1930   /* Check for a misaligned address.  */
1931   if (address & 1)
1932     {
1933       if ((read_cp15_reg (1, 0, 0) & ARMul_CP15_R1_ALIGN))
1934         ARMul_Abort (state, ARMul_DataAbortV);
1935       else
1936         address &= ~ 1;
1937     }
1938
1939   value = ARMul_LoadHalfWord (state, address);
1940
1941   if (state->Aborted)
1942     ARMul_Abort (state, ARMul_DataAbortV);
1943   else
1944     ARMul_Icycles (state, 1, 0L);
1945
1946   return value;
1947 }
1948
1949 static ARMword
1950 Iwmmxt_Load_Byte (ARMul_State * state, ARMword address)
1951 {
1952   ARMword value;
1953
1954   value = ARMul_LoadByte (state, address);
1955
1956   if (state->Aborted)
1957     ARMul_Abort (state, ARMul_DataAbortV);
1958   else
1959     ARMul_Icycles (state, 1, 0L);
1960
1961   return value;
1962 }
1963
1964 static void
1965 Iwmmxt_Store_Double_Word (ARMul_State * state, ARMword address, ARMdword value)
1966 {
1967   /* The address must be aligned on a 8 byte boundary.  */
1968   if (address & 0x7)
1969     {
1970       fprintf (stderr, "iWMMXt: At addr 0x%x: Unaligned double word store to 0x%x\n",
1971                (state->Reg[15] - 8) & ~0x3, address);
1972 #ifdef DEBUG
1973 #endif
1974       /* No need to check for alignment traps.  An unaligned
1975          double word store with alignment trapping disabled is
1976          UNPREDICTABLE.  */
1977       ARMul_Abort (state, ARMul_DataAbortV);
1978     }
1979
1980   /* Store the words.  */
1981   if (! state->bigendSig)
1982     {
1983       ARMul_StoreWordN (state, address, value);
1984       ARMul_StoreWordN (state, address + 4, value >> 32);
1985     }
1986   else
1987     {
1988       ARMul_StoreWordN (state, address + 4, value);
1989       ARMul_StoreWordN (state, address, value >> 32);
1990     }
1991
1992   /* Check for data aborts.  */
1993   if (state->Aborted)
1994     ARMul_Abort (state, ARMul_DataAbortV);
1995   else
1996     ARMul_Icycles (state, 2, 0L);
1997 }
1998
1999 static void
2000 Iwmmxt_Store_Word (ARMul_State * state, ARMword address, ARMword value)
2001 {
2002   /* Check for a misaligned address.  */
2003   if (address & 3)
2004     {
2005       if ((read_cp15_reg (1, 0, 0) & ARMul_CP15_R1_ALIGN))
2006         ARMul_Abort (state, ARMul_DataAbortV);
2007       else
2008         address &= ~ 3;
2009     }
2010
2011   ARMul_StoreWordN (state, address, value);
2012
2013   if (state->Aborted)
2014     ARMul_Abort (state, ARMul_DataAbortV);
2015 }
2016
2017 static void
2018 Iwmmxt_Store_Half_Word (ARMul_State * state, ARMword address, ARMword value)
2019 {
2020   /* Check for a misaligned address.  */
2021   if (address & 1)
2022     {
2023       if ((read_cp15_reg (1, 0, 0) & ARMul_CP15_R1_ALIGN))
2024         ARMul_Abort (state, ARMul_DataAbortV);
2025       else
2026         address &= ~ 1;
2027     }
2028
2029   ARMul_StoreHalfWord (state, address, value);
2030
2031   if (state->Aborted)
2032     ARMul_Abort (state, ARMul_DataAbortV);
2033 }
2034
2035 static void
2036 Iwmmxt_Store_Byte (ARMul_State * state, ARMword address, ARMword value)
2037 {
2038   ARMul_StoreByte (state, address, value);
2039
2040   if (state->Aborted)
2041     ARMul_Abort (state, ARMul_DataAbortV);
2042 }
2043
2044 static int
2045 WLDR (ARMul_State * state, ARMword instr)
2046 {
2047   ARMword address;
2048   int failed;
2049
2050   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
2051     return ARMul_CANT;
2052
2053 #ifdef DEBUG
2054   fprintf (stderr, "wldr\n");
2055 #endif
2056
2057   address = Compute_Iwmmxt_Address (state, instr, & failed);
2058   if (failed)
2059     return ARMul_CANT;
2060
2061   if (BITS (28, 31) == 0xf)
2062     {
2063       /* WLDRW wCx */
2064       wC [BITS (12, 15)] = Iwmmxt_Load_Word (state, address);
2065     }
2066   else if (BIT (8) == 0)
2067     {
2068       if (BIT (22) == 0)
2069         /* WLDRB */
2070         wR [BITS (12, 15)] = Iwmmxt_Load_Byte (state, address);
2071       else
2072         /* WLDRH */
2073         wR [BITS (12, 15)] = Iwmmxt_Load_Half_Word (state, address);
2074     }
2075   else
2076     {
2077       if (BIT (22) == 0)
2078         /* WLDRW wRd */
2079         wR [BITS (12, 15)] = Iwmmxt_Load_Word (state, address);
2080       else
2081         /* WLDRD */
2082         wR [BITS (12, 15)] = Iwmmxt_Load_Double_Word (state, address);
2083     }
2084
2085   wC [wCon] |= WCON_MUP;
2086
2087   return ARMul_DONE;
2088 }
2089
2090 static int
2091 WMAC (ARMword instr)
2092 {
2093   int      i;
2094   ARMdword t = 0;
2095   ARMword  a, b;
2096
2097   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
2098     return ARMul_CANT;
2099
2100 #ifdef DEBUG
2101   fprintf (stderr, "wmac\n");
2102 #endif
2103
2104   for (i = 0; i < 4; i++)
2105     {
2106       if (BIT (21))
2107         {
2108           /* Signed.  */
2109           signed long s;
2110
2111           a = wRHALF (BITS (16, 19), i);
2112           a = EXTEND16 (a);
2113
2114           b = wRHALF (BITS (0, 3), i);
2115           b = EXTEND16 (b);
2116
2117           s = (signed long) a * (signed long) b;
2118
2119           t = t + (ARMdword) s;
2120         }
2121       else
2122         {
2123           /* Unsigned.  */
2124           a = wRHALF (BITS (16, 19), i);
2125           b = wRHALF (BITS ( 0,  3), i);
2126
2127           t += a * b;
2128         }
2129     }
2130
2131   if (BIT (21))
2132     t = EXTEND32 (t);
2133   else
2134     t &= 0xffffffff;
2135
2136   if (BIT (20))
2137     wR [BITS (12, 15)] = t;
2138   else
2139     wR[BITS (12, 15)] += t;
2140
2141   wC [wCon] |= WCON_MUP;
2142
2143   return ARMul_DONE;
2144 }
2145
2146 static int
2147 WMADD (ARMword instr)
2148 {
2149   ARMdword r = 0;
2150   int i;
2151
2152   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
2153     return ARMul_CANT;
2154
2155 #ifdef DEBUG
2156   fprintf (stderr, "wmadd\n");
2157 #endif
2158
2159   for (i = 0; i < 2; i++)
2160     {
2161       ARMdword s1, s2;
2162
2163       if (BIT (21))     /* Signed.  */
2164         {
2165           signed long a, b;
2166
2167           a = wRHALF (BITS (16, 19), i * 2);
2168           a = EXTEND16 (a);
2169
2170           b = wRHALF (BITS (0, 3), i * 2);
2171           b = EXTEND16 (b);
2172
2173           s1 = (ARMdword) (a * b);
2174
2175           a = wRHALF (BITS (16, 19), i * 2 + 1);
2176           a = EXTEND16 (a);
2177
2178           b = wRHALF (BITS (0, 3), i * 2 + 1);
2179           b = EXTEND16 (b);
2180
2181           s2 = (ARMdword) (a * b);
2182         }
2183       else                      /* Unsigned.  */
2184         {
2185           unsigned long a, b;
2186
2187           a = wRHALF (BITS (16, 19), i * 2);
2188           b = wRHALF (BITS ( 0,  3), i * 2);
2189
2190           s1 = (ARMdword) (a * b);
2191
2192           a = wRHALF (BITS (16, 19), i * 2 + 1);
2193           b = wRHALF (BITS ( 0,  3), i * 2 + 1);
2194
2195           s2 = (ARMdword) a * b;
2196         }
2197
2198       r |= (ARMdword) ((s1 + s2) & 0xffffffff) << (i ? 32 : 0);
2199     }
2200
2201   wR [BITS (12, 15)] = r;
2202   wC [wCon] |= WCON_MUP;
2203
2204   return ARMul_DONE;
2205 }
2206
2207 static int
2208 WMAX (ARMul_State * state, ARMword instr)
2209 {
2210   ARMdword r = 0;
2211   ARMdword s;
2212   int      i;
2213
2214   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
2215     return ARMul_CANT;
2216
2217 #ifdef DEBUG
2218   fprintf (stderr, "wmax\n");
2219 #endif
2220
2221   switch (BITS (22, 23))
2222     {
2223     case Bqual:
2224       for (i = 0; i < 8; i++)
2225         if (BIT (21))   /* Signed.  */
2226           {
2227             int a, b;
2228
2229             a = wRBYTE (BITS (16, 19), i);
2230             a = EXTEND8 (a);
2231
2232             b = wRBYTE (BITS (0, 3), i);
2233             b = EXTEND8 (b);
2234
2235             if (a > b)
2236               s = a;
2237             else
2238               s = b;
2239
2240             r |= (s & 0xff) << (i * 8);
2241           }
2242         else            /* Unsigned.  */
2243           {
2244             unsigned int a, b;
2245
2246             a = wRBYTE (BITS (16, 19), i);
2247             b = wRBYTE (BITS (0, 3), i);
2248
2249             if (a > b)
2250               s = a;
2251             else
2252               s = b;
2253
2254             r |= (s & 0xff) << (i * 8);
2255           }
2256       break;
2257
2258     case Hqual:
2259       for (i = 0; i < 4; i++)
2260         if (BIT (21))   /* Signed.  */
2261           {
2262             int a, b;
2263
2264             a = wRHALF (BITS (16, 19), i);
2265             a = EXTEND16 (a);
2266
2267             b = wRHALF (BITS (0, 3), i);
2268             b = EXTEND16 (b);
2269
2270             if (a > b)
2271               s = a;
2272             else
2273               s = b;
2274
2275             r |= (s & 0xffff) << (i * 16);
2276           }
2277         else            /* Unsigned.  */
2278           {
2279             unsigned int a, b;
2280
2281             a = wRHALF (BITS (16, 19), i);
2282             b = wRHALF (BITS (0, 3), i);
2283
2284             if (a > b)
2285               s = a;
2286             else
2287               s = b;
2288
2289             r |= (s & 0xffff) << (i * 16);
2290           }
2291       break;
2292
2293     case Wqual:
2294       for (i = 0; i < 2; i++)
2295         if (BIT (21))   /* Signed.  */
2296           {
2297             int a, b;
2298
2299             a = wRWORD (BITS (16, 19), i);
2300             b = wRWORD (BITS (0, 3), i);
2301
2302             if (a > b)
2303               s = a;
2304             else
2305               s = b;
2306
2307             r |= (s & 0xffffffff) << (i * 32);
2308           }
2309         else
2310           {
2311             unsigned int a, b;
2312
2313             a = wRWORD (BITS (16, 19), i);
2314             b = wRWORD (BITS (0, 3), i);
2315
2316             if (a > b)
2317               s = a;
2318             else
2319               s = b;
2320
2321             r |= (s & 0xffffffff) << (i * 32);
2322           }
2323       break;
2324
2325     default:
2326       ARMul_UndefInstr (state, instr);
2327       return ARMul_DONE;
2328     }
2329
2330   wR [BITS (12, 15)] = r;
2331   wC [wCon] |= WCON_MUP;
2332
2333   return ARMul_DONE;
2334 }
2335
2336 static int
2337 WMIN (ARMul_State * state, ARMword instr)
2338 {
2339   ARMdword r = 0;
2340   ARMdword s;
2341   int      i;
2342
2343   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
2344     return ARMul_CANT;
2345
2346 #ifdef DEBUG
2347   fprintf (stderr, "wmin\n");
2348 #endif
2349
2350   switch (BITS (22, 23))
2351     {
2352     case Bqual:
2353       for (i = 0; i < 8; i++)
2354         if (BIT (21))   /* Signed.  */
2355           {
2356             int a, b;
2357
2358             a = wRBYTE (BITS (16, 19), i);
2359             a = EXTEND8 (a);
2360
2361             b = wRBYTE (BITS (0, 3), i);
2362             b = EXTEND8 (b);
2363
2364             if (a < b)
2365               s = a;
2366             else
2367               s = b;
2368
2369             r |= (s & 0xff) << (i * 8);
2370           }
2371         else            /* Unsigned.  */
2372           {
2373             unsigned int a, b;
2374
2375             a = wRBYTE (BITS (16, 19), i);
2376             b = wRBYTE (BITS (0, 3), i);
2377
2378             if (a < b)
2379               s = a;
2380             else
2381               s = b;
2382
2383             r |= (s & 0xff) << (i * 8);
2384           }
2385       break;
2386
2387     case Hqual:
2388       for (i = 0; i < 4; i++)
2389         if (BIT (21))   /* Signed.  */
2390           {
2391             int a, b;
2392
2393             a = wRHALF (BITS (16, 19), i);
2394             a = EXTEND16 (a);
2395
2396             b = wRHALF (BITS (0, 3), i);
2397             b = EXTEND16 (b);
2398
2399             if (a < b)
2400               s = a;
2401             else
2402               s = b;
2403
2404             r |= (s & 0xffff) << (i * 16);
2405           }
2406         else
2407           {
2408             /* Unsigned.  */
2409             unsigned int a, b;
2410
2411             a = wRHALF (BITS (16, 19), i);
2412             b = wRHALF (BITS ( 0,  3), i);
2413
2414             if (a < b)
2415               s = a;
2416             else
2417               s = b;
2418
2419             r |= (s & 0xffff) << (i * 16);
2420           }
2421       break;
2422
2423     case Wqual:
2424       for (i = 0; i < 2; i++)
2425         if (BIT (21))   /* Signed.  */
2426           {
2427             int a, b;
2428
2429             a = wRWORD (BITS (16, 19), i);
2430             b = wRWORD (BITS ( 0,  3), i);
2431
2432             if (a < b)
2433               s = a;
2434             else
2435               s = b;
2436
2437             r |= (s & 0xffffffff) << (i * 32);
2438           }
2439         else
2440           {
2441             unsigned int a, b;
2442
2443             a = wRWORD (BITS (16, 19), i);
2444             b = wRWORD (BITS (0, 3), i);
2445
2446             if (a < b)
2447               s = a;
2448             else
2449               s = b;
2450
2451             r |= (s & 0xffffffff) << (i * 32);
2452           }
2453       break;
2454
2455     default:
2456       ARMul_UndefInstr (state, instr);
2457       return ARMul_DONE;
2458     }
2459
2460   wR [BITS (12, 15)] = r;
2461   wC [wCon] |= WCON_MUP;
2462
2463   return ARMul_DONE;
2464 }
2465
2466 static int
2467 WMUL (ARMword instr)
2468 {
2469   ARMdword r = 0;
2470   ARMdword s;
2471   int      i;
2472
2473   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
2474     return ARMul_CANT;
2475
2476 #ifdef DEBUG
2477   fprintf (stderr, "wmul\n");
2478 #endif
2479
2480   for (i = 0; i < 4; i++)
2481     if (BIT (21))       /* Signed.  */
2482       {
2483         long a, b;
2484
2485         a = wRHALF (BITS (16, 19), i);
2486         a = EXTEND16 (a);
2487
2488         b = wRHALF (BITS (0, 3), i);
2489         b = EXTEND16 (b);
2490
2491         s = a * b;
2492
2493         if (BIT (20))
2494           r |= ((s >> 16) & 0xffff) << (i * 16);
2495         else
2496           r |= (s & 0xffff) << (i * 16);
2497       }
2498     else                /* Unsigned.  */
2499       {
2500         unsigned long a, b;
2501
2502         a = wRHALF (BITS (16, 19), i);
2503         b = wRHALF (BITS (0, 3), i);
2504
2505         s = a * b;
2506
2507         if (BIT (20))
2508           r |= ((s >> 16) & 0xffff) << (i * 16);
2509         else
2510           r |= (s & 0xffff) << (i * 16);
2511       }
2512
2513   wR [BITS (12, 15)] = r;
2514   wC [wCon] |= WCON_MUP;
2515
2516   return ARMul_DONE;
2517 }
2518
2519 static int
2520 WOR (ARMword instr)
2521 {
2522   ARMword psr = 0;
2523   ARMdword result;
2524
2525   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
2526     return ARMul_CANT;
2527
2528 #ifdef DEBUG
2529   fprintf (stderr, "wor\n");
2530 #endif
2531
2532   result = wR [BITS (16, 19)] | wR [BITS (0, 3)];
2533   wR [BITS (12, 15)] = result;
2534
2535   SIMD64_SET (psr, (result == 0), SIMD_ZBIT);
2536   SIMD64_SET (psr, (result & (1ULL << 63)), SIMD_NBIT);
2537
2538   wC [wCASF] = psr;
2539   wC [wCon] |= (WCON_CUP | WCON_MUP);
2540
2541   return ARMul_DONE;
2542 }
2543
2544 static int
2545 WPACK (ARMul_State * state, ARMword instr)
2546 {
2547   ARMdword r = 0;
2548   ARMword  psr = 0;
2549   ARMdword x;
2550   ARMdword s;
2551   int      i;
2552   int      satrv[8];
2553
2554   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
2555     return ARMul_CANT;
2556
2557 #ifdef DEBUG
2558   fprintf (stderr, "wpack\n");
2559 #endif
2560
2561   switch (BITS (22, 23))
2562     {
2563     case Hqual:
2564       for (i = 0; i < 8; i++)
2565         {
2566           x = wRHALF (i < 4 ? BITS (16, 19) : BITS (0, 3), i & 3);
2567
2568           switch (BITS (20, 21))
2569             {
2570             case UnsignedSaturation:
2571               s = IwmmxtSaturateU8 (x, satrv + BITIDX8 (i));
2572               break;
2573
2574             case SignedSaturation:
2575               s = IwmmxtSaturateS8 (x, satrv + BITIDX8 (i));
2576               break;
2577
2578             default:
2579               ARMul_UndefInstr (state, instr);
2580               return ARMul_DONE;
2581             }
2582
2583           r |= (s & 0xff) << (i * 8);
2584           SIMD8_SET (psr, NBIT8 (s), SIMD_NBIT, i);
2585           SIMD8_SET (psr, ZBIT8 (s), SIMD_ZBIT, i);
2586         }
2587       break;
2588
2589     case Wqual:
2590       satrv[0] = satrv[2] = satrv[4] = satrv[6] = 0;
2591
2592       for (i = 0; i < 4; i++)
2593         {
2594           x = wRWORD (i < 2 ? BITS (16, 19) : BITS (0, 3), i & 1);
2595
2596           switch (BITS (20, 21))
2597             {
2598             case UnsignedSaturation:
2599               s = IwmmxtSaturateU16 (x, satrv + BITIDX16 (i));
2600               break;
2601
2602             case SignedSaturation:
2603               s = IwmmxtSaturateS16 (x, satrv + BITIDX16 (i));
2604               break;
2605
2606             default:
2607               ARMul_UndefInstr (state, instr);
2608               return ARMul_DONE;
2609             }
2610
2611           r |= (s & 0xffff) << (i * 16);
2612           SIMD16_SET (psr, NBIT16 (s), SIMD_NBIT, i);
2613           SIMD16_SET (psr, ZBIT16 (s), SIMD_ZBIT, i);
2614         }
2615       break;
2616
2617     case Dqual:
2618       satrv[0] = satrv[1] = satrv[2] = satrv[4] = satrv[5] = satrv[6] = 0;
2619
2620       for (i = 0; i < 2; i++)
2621         {
2622           x = wR [i ? BITS (0, 3) : BITS (16, 19)];
2623
2624           switch (BITS (20, 21))
2625             {
2626             case UnsignedSaturation:
2627               s = IwmmxtSaturateU32 (x, satrv + BITIDX32 (i));
2628               break;
2629
2630             case SignedSaturation:
2631               s = IwmmxtSaturateS32 (x, satrv + BITIDX32 (i));
2632               break;
2633
2634             default:
2635               ARMul_UndefInstr (state, instr);
2636               return ARMul_DONE;
2637             }
2638
2639           r |= (s & 0xffffffff) << (i * 32);
2640           SIMD32_SET (psr, NBIT32 (s), SIMD_NBIT, i);
2641           SIMD32_SET (psr, ZBIT32 (s), SIMD_ZBIT, i);
2642         }
2643       break;
2644
2645     default:
2646       ARMul_UndefInstr (state, instr);
2647       return ARMul_DONE;
2648     }
2649
2650   wC [wCASF] = psr;
2651   wR [BITS (12, 15)] = r;
2652   SET_wCSSFvec (satrv);
2653   wC [wCon] |= (WCON_CUP | WCON_MUP);
2654
2655   return ARMul_DONE;
2656 }
2657
2658 static int
2659 WROR (ARMul_State * state, ARMword instr)
2660 {
2661   ARMdword r = 0;
2662   ARMdword s;
2663   ARMword  psr = 0;
2664   int      i;
2665   int      shift;
2666
2667   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
2668     return ARMul_CANT;
2669
2670 #ifdef DEBUG
2671   fprintf (stderr, "wror\n");
2672 #endif
2673
2674   DECODE_G_BIT (state, instr, shift);
2675
2676   switch (BITS (22, 23))
2677     {
2678     case Hqual:
2679       shift &= 0xf;
2680       for (i = 0; i < 4; i++)
2681         {
2682           s = ((wRHALF (BITS (16, 19), i) & 0xffff) << (16 - shift))
2683             | ((wRHALF (BITS (16, 19), i) & 0xffff) >> shift);
2684           r |= (s & 0xffff) << (i * 16);
2685           SIMD16_SET (psr, NBIT16 (s), SIMD_NBIT, i);
2686           SIMD16_SET (psr, ZBIT16 (s), SIMD_ZBIT, i);
2687         }
2688       break;
2689
2690     case Wqual:
2691       shift &= 0x1f;
2692       for (i = 0; i < 2; i++)
2693         {
2694           s = ((wRWORD (BITS (16, 19), i) & 0xffffffff) << (32 - shift))
2695             | ((wRWORD (BITS (16, 19), i) & 0xffffffff) >> shift);
2696           r |= (s & 0xffffffff) << (i * 32);
2697           SIMD32_SET (psr, NBIT32 (s), SIMD_NBIT, i);
2698           SIMD32_SET (psr, ZBIT32 (s), SIMD_ZBIT, i);
2699         }
2700       break;
2701
2702     case Dqual:
2703       shift &= 0x3f;
2704       r = (wR [BITS (16, 19)] >> shift)
2705         | (wR [BITS (16, 19)] << (64 - shift));
2706
2707       SIMD64_SET (psr, NBIT64 (r), SIMD_NBIT);
2708       SIMD64_SET (psr, ZBIT64 (r), SIMD_ZBIT);
2709       break;
2710
2711     default:
2712       ARMul_UndefInstr (state, instr);
2713       return ARMul_DONE;
2714     }
2715
2716   wC [wCASF] = psr;
2717   wR [BITS (12, 15)] = r;
2718   wC [wCon] |= (WCON_CUP | WCON_MUP);
2719
2720   return ARMul_DONE;
2721 }
2722
2723 static int
2724 WSAD (ARMword instr)
2725 {
2726   ARMdword r;
2727   int      s;
2728   int      i;
2729
2730   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
2731     return ARMul_CANT;
2732
2733 #ifdef DEBUG
2734   fprintf (stderr, "wsad\n");
2735 #endif
2736
2737   /* Z bit.  */
2738   r = BIT (20) ? 0 : (wR [BITS (12, 15)] & 0xffffffff);
2739
2740   if (BIT (22))
2741     /* Half.  */
2742     for (i = 0; i < 4; i++)
2743       {
2744         s = (wRHALF (BITS (16, 19), i) - wRHALF (BITS (0, 3), i));
2745         r += abs (s);
2746       }
2747   else
2748     /* Byte.  */
2749     for (i = 0; i < 8; i++)
2750       {
2751         s = (wRBYTE (BITS (16, 19), i) - wRBYTE (BITS (0, 3), i));
2752         r += abs (s);
2753       }
2754
2755   wR [BITS (12, 15)] = r;
2756   wC [wCon] |= WCON_MUP;
2757
2758   return ARMul_DONE;
2759 }
2760
2761 static int
2762 WSHUFH (ARMword instr)
2763 {
2764   ARMdword r = 0;
2765   ARMword  psr = 0;
2766   ARMdword s;
2767   int      i;
2768   int      imm8;
2769
2770   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
2771     return ARMul_CANT;
2772
2773 #ifdef DEBUG
2774   fprintf (stderr, "wshufh\n");
2775 #endif
2776
2777   imm8 = (BITS (20, 23) << 4) | BITS (0, 3);
2778
2779   for (i = 0; i < 4; i++)
2780     {
2781       s = wRHALF (BITS (16, 19), ((imm8 >> (i * 2) & 3)) & 0xff);
2782       r |= (s & 0xffff) << (i * 16);
2783       SIMD16_SET (psr, NBIT16 (s), SIMD_NBIT, i);
2784       SIMD16_SET (psr, ZBIT16 (s), SIMD_ZBIT, i);
2785     }
2786
2787   wC [wCASF] = psr;
2788   wR [BITS (12, 15)] = r;
2789   wC [wCon] |= (WCON_CUP | WCON_MUP);
2790
2791   return ARMul_DONE;
2792 }
2793
2794 static int
2795 WSLL (ARMul_State * state, ARMword instr)
2796 {
2797   ARMdword r = 0;
2798   ARMdword s;
2799   ARMword  psr = 0;
2800   int      i;
2801   unsigned shift;
2802
2803   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
2804     return ARMul_CANT;
2805
2806 #ifdef DEBUG
2807   fprintf (stderr, "wsll\n");
2808 #endif
2809
2810   DECODE_G_BIT (state, instr, shift);
2811
2812   switch (BITS (22, 23))
2813     {
2814     case Hqual:
2815       for (i = 0; i < 4; i++)
2816         {
2817           if (shift > 15)
2818             s = 0;
2819           else
2820             s = ((wRHALF (BITS (16, 19), i) & 0xffff) << shift);
2821           r |= (s & 0xffff) << (i * 16);
2822           SIMD16_SET (psr, NBIT16 (s), SIMD_NBIT, i);
2823           SIMD16_SET (psr, ZBIT16 (s), SIMD_ZBIT, i);
2824         }
2825       break;
2826
2827     case Wqual:
2828       for (i = 0; i < 2; i++)
2829         {
2830           if (shift > 31)
2831             s = 0;
2832           else
2833             s = ((wRWORD (BITS (16, 19), i) & 0xffffffff) << shift);
2834           r |= (s & 0xffffffff) << (i * 32);
2835           SIMD32_SET (psr, NBIT32 (s), SIMD_NBIT, i);
2836           SIMD32_SET (psr, ZBIT32 (s), SIMD_ZBIT, i);
2837         }
2838       break;
2839
2840     case Dqual:
2841       if (shift > 63)
2842         r = 0;
2843       else
2844         r = ((wR[BITS (16, 19)] & 0xffffffffffffffffULL) << shift);
2845
2846       SIMD64_SET (psr, NBIT64 (r), SIMD_NBIT);
2847       SIMD64_SET (psr, ZBIT64 (r), SIMD_ZBIT);
2848       break;
2849
2850     default:
2851       ARMul_UndefInstr (state, instr);
2852       return ARMul_DONE;
2853     }
2854
2855   wC [wCASF] = psr;
2856   wR [BITS (12, 15)] = r;
2857   wC [wCon] |= (WCON_CUP | WCON_MUP);
2858
2859   return ARMul_DONE;
2860 }
2861
2862 static int
2863 WSRA (ARMul_State * state, ARMword instr)
2864 {
2865   ARMdword     r = 0;
2866   ARMdword     s;
2867   ARMword      psr = 0;
2868   int          i;
2869   unsigned     shift;
2870   signed long  t;
2871
2872   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
2873     return ARMul_CANT;
2874
2875 #ifdef DEBUG
2876   fprintf (stderr, "wsra\n");
2877 #endif
2878
2879   DECODE_G_BIT (state, instr, shift);
2880
2881   switch (BITS (22, 23))
2882     {
2883     case Hqual:
2884       for (i = 0; i < 4; i++)
2885         {
2886           if (shift > 15)
2887             t = (wRHALF (BITS (16, 19), i) & 0x8000) ? 0xffff : 0;
2888           else
2889             {
2890               t = wRHALF (BITS (16, 19), i);
2891               t = EXTEND16 (t);
2892               t >>= shift;
2893             }
2894
2895           s = t;
2896           r |= (s & 0xffff) << (i * 16);
2897           SIMD16_SET (psr, NBIT16 (s), SIMD_NBIT, i);
2898           SIMD16_SET (psr, ZBIT16 (s), SIMD_ZBIT, i);
2899         }
2900       break;
2901
2902     case Wqual:
2903       for (i = 0; i < 2; i++)
2904         {
2905           if (shift > 31)
2906             t = (wRWORD (BITS (16, 19), i) & 0x80000000) ? 0xffffffff : 0;
2907           else
2908             {
2909               t = EXTEND32 (wRWORD (BITS (16, 19), i));
2910               t >>= shift;
2911             }
2912           s = t;
2913           r |= (s & 0xffffffff) << (i * 32);
2914           SIMD32_SET (psr, NBIT32 (s), SIMD_NBIT, i);
2915           SIMD32_SET (psr, ZBIT32 (s), SIMD_ZBIT, i);
2916         }
2917       break;
2918
2919     case Dqual:
2920       if (shift > 63)
2921         r = (wR [BITS (16, 19)] & 0x8000000000000000ULL) ? 0xffffffffffffffffULL : 0;
2922       else
2923         r = ((signed long long) (wR[BITS (16, 19)] & 0xffffffffffffffffULL) >> shift);
2924       SIMD64_SET (psr, NBIT64 (r), SIMD_NBIT);
2925       SIMD64_SET (psr, ZBIT64 (r), SIMD_ZBIT);
2926       break;
2927
2928     default:
2929       ARMul_UndefInstr (state, instr);
2930       return ARMul_DONE;
2931     }
2932
2933   wC [wCASF] = psr;
2934   wR [BITS (12, 15)] = r;
2935   wC [wCon] |= (WCON_CUP | WCON_MUP);
2936
2937   return ARMul_DONE;
2938 }
2939
2940 static int
2941 WSRL (ARMul_State * state, ARMword instr)
2942 {
2943   ARMdword     r = 0;
2944   ARMdword     s;
2945   ARMword      psr = 0;
2946   int          i;
2947   unsigned int shift;
2948
2949   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
2950     return ARMul_CANT;
2951
2952 #ifdef DEBUG
2953   fprintf (stderr, "wsrl\n");
2954 #endif
2955
2956   DECODE_G_BIT (state, instr, shift);
2957
2958   switch (BITS (22, 23))
2959     {
2960     case Hqual:
2961       for (i = 0; i < 4; i++)
2962         {
2963           if (shift > 15)
2964             s = 0;
2965           else
2966             s = ((unsigned) (wRHALF (BITS (16, 19), i) & 0xffff) >> shift);
2967
2968           r |= (s & 0xffff) << (i * 16);
2969           SIMD16_SET (psr, NBIT16 (s), SIMD_NBIT, i);
2970           SIMD16_SET (psr, ZBIT16 (s), SIMD_ZBIT, i);
2971         }
2972       break;
2973
2974     case Wqual:
2975       for (i = 0; i < 2; i++)
2976         {
2977           if (shift > 31)
2978             s = 0;
2979           else
2980             s = ((unsigned long) (wRWORD (BITS (16, 19), i) & 0xffffffff) >> shift);
2981
2982           r |= (s & 0xffffffff) << (i * 32);
2983           SIMD32_SET (psr, NBIT32 (s), SIMD_NBIT, i);
2984           SIMD32_SET (psr, ZBIT32 (s), SIMD_ZBIT, i);
2985         }
2986       break;
2987
2988     case Dqual:
2989       if (shift > 63)
2990         r = 0;
2991       else
2992         r = (wR [BITS (16, 19)] & 0xffffffffffffffffULL) >> shift;
2993
2994       SIMD64_SET (psr, NBIT64 (r), SIMD_NBIT);
2995       SIMD64_SET (psr, ZBIT64 (r), SIMD_ZBIT);
2996       break;
2997
2998     default:
2999       ARMul_UndefInstr (state, instr);
3000       return ARMul_DONE;
3001     }
3002
3003   wC [wCASF] = psr;
3004   wR [BITS (12, 15)] = r;
3005   wC [wCon] |= (WCON_CUP | WCON_MUP);
3006
3007   return ARMul_DONE;
3008 }
3009
3010 static int
3011 WSTR (ARMul_State * state, ARMword instr)
3012 {
3013   ARMword address;
3014   int failed;
3015
3016
3017   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
3018     return ARMul_CANT;
3019
3020 #ifdef DEBUG
3021   fprintf (stderr, "wstr\n");
3022 #endif
3023
3024   address = Compute_Iwmmxt_Address (state, instr, & failed);
3025   if (failed)
3026     return ARMul_CANT;
3027
3028   if (BITS (28, 31) == 0xf)
3029     {
3030       /* WSTRW wCx */
3031       Iwmmxt_Store_Word (state, address, wC [BITS (12, 15)]);
3032     }
3033   else if (BIT (8) == 0)
3034     {
3035       if (BIT (22) == 0)
3036         /* WSTRB */
3037         Iwmmxt_Store_Byte (state, address, wR [BITS (12, 15)]);
3038       else
3039         /* WSTRH */
3040         Iwmmxt_Store_Half_Word (state, address, wR [BITS (12, 15)]);
3041     }
3042   else
3043     {
3044       if (BIT (22) == 0)
3045         /* WSTRW wRd */
3046         Iwmmxt_Store_Word (state, address, wR [BITS (12, 15)]);
3047       else
3048         /* WSTRD */
3049         Iwmmxt_Store_Double_Word (state, address, wR [BITS (12, 15)]);
3050     }
3051
3052   return ARMul_DONE;
3053 }
3054
3055 static int
3056 WSUB (ARMul_State * state, ARMword instr)
3057 {
3058   ARMdword r = 0;
3059   ARMword  psr = 0;
3060   ARMdword x;
3061   ARMdword s;
3062   int      i;
3063   int      carry;
3064   int      overflow;
3065   int      satrv[8];
3066
3067   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
3068     return ARMul_CANT;
3069
3070 #ifdef DEBUG
3071   fprintf (stderr, "wsub\n");
3072 #endif
3073
3074 /* Subtract two numbers using the specified function,
3075    leaving setting the carry bit as required.  */
3076 #define SUBx(x, y, m, f) \
3077    (*f) (wRBITS (BITS (16, 19), (x), (y)) & (m), \
3078          wRBITS (BITS ( 0,  3), (x), (y)) & (m), & carry, & overflow)
3079
3080   switch (BITS (22, 23))
3081     {
3082     case Bqual:
3083       for (i = 0; i < 8; i++)
3084         {
3085           switch (BITS (20, 21))
3086             {
3087             case NoSaturation:
3088               s = SUBx ((i * 8), (i * 8) + 7, 0xff, SubS8);
3089               satrv [BITIDX8 (i)] = 0;
3090               r |= (s & 0xff) << (i * 8);
3091               SIMD8_SET (psr, NBIT8 (s), SIMD_NBIT, i);
3092               SIMD8_SET (psr, ZBIT8 (s), SIMD_ZBIT, i);
3093               SIMD8_SET (psr, carry, SIMD_CBIT, i);
3094               SIMD8_SET (psr, overflow, SIMD_VBIT, i);
3095               break;
3096
3097             case UnsignedSaturation:
3098               s = SUBx ((i * 8), (i * 8) + 7, 0xff, SubU8);
3099               x = IwmmxtSaturateU8 (s, satrv + BITIDX8 (i));
3100               r |= (x & 0xff) << (i * 8);
3101               SIMD8_SET (psr, NBIT8 (x), SIMD_NBIT, i);
3102               SIMD8_SET (psr, ZBIT8 (x), SIMD_ZBIT, i);
3103               if (! satrv [BITIDX8 (i)])
3104                 {
3105                   SIMD8_SET (psr, carry,     SIMD_CBIT, i);
3106                   SIMD8_SET (psr, overflow, SIMD_VBIT, i);
3107                 }
3108               break;
3109
3110             case SignedSaturation:
3111               s = SUBx ((i * 8), (i * 8) + 7, 0xff, SubS8);
3112               x = IwmmxtSaturateS8 (s, satrv + BITIDX8 (i));
3113               r |= (x & 0xff) << (i * 8);
3114               SIMD8_SET (psr, NBIT8 (x), SIMD_NBIT, i);
3115               SIMD8_SET (psr, ZBIT8 (x), SIMD_ZBIT, i);
3116               if (! satrv [BITIDX8 (i)])
3117                 {
3118                   SIMD8_SET (psr, carry,     SIMD_CBIT, i);
3119                   SIMD8_SET (psr, overflow, SIMD_VBIT, i);
3120                 }
3121               break;
3122
3123             default:
3124               ARMul_UndefInstr (state, instr);
3125               return ARMul_DONE;
3126             }
3127         }
3128       break;
3129
3130     case Hqual:
3131       satrv[0] = satrv[2] = satrv[4] = satrv[6] = 0;
3132
3133       for (i = 0; i < 4; i++)
3134         {
3135           switch (BITS (20, 21))
3136             {
3137             case NoSaturation:
3138               s = SUBx ((i * 16), (i * 16) + 15, 0xffff, SubU16);
3139               satrv [BITIDX16 (i)] = 0;
3140               r |= (s & 0xffff) << (i * 16);
3141               SIMD16_SET (psr, NBIT16 (s), SIMD_NBIT, i);
3142               SIMD16_SET (psr, ZBIT16 (s), SIMD_ZBIT, i);
3143               SIMD16_SET (psr, carry,      SIMD_CBIT, i);
3144               SIMD16_SET (psr, overflow,   SIMD_VBIT, i);
3145               break;
3146
3147             case UnsignedSaturation:
3148               s = SUBx ((i * 16), (i * 16) + 15, 0xffff, SubU16);
3149               x = IwmmxtSaturateU16 (s, satrv + BITIDX16 (i));
3150               r |= (x & 0xffff) << (i * 16);
3151               SIMD16_SET (psr, NBIT16 (x & 0xffff), SIMD_NBIT, i);
3152               SIMD16_SET (psr, ZBIT16 (x), SIMD_ZBIT, i);
3153               if (! satrv [BITIDX16 (i)])
3154                 {
3155                   SIMD16_SET (psr, carry,    SIMD_CBIT, i);
3156                   SIMD16_SET (psr, overflow, SIMD_VBIT, i);
3157                 }
3158               break;
3159
3160             case SignedSaturation:
3161               s = SUBx ((i * 16), (i * 16) + 15, 0xffff, SubS16);
3162               x = IwmmxtSaturateS16 (s, satrv + BITIDX16 (i));
3163               r |= (x & 0xffff) << (i * 16);
3164               SIMD16_SET (psr, NBIT16 (x), SIMD_NBIT, i);
3165               SIMD16_SET (psr, ZBIT16 (x), SIMD_ZBIT, i);
3166               if (! satrv [BITIDX16 (i)])
3167                 {
3168                   SIMD16_SET (psr, carry,    SIMD_CBIT, i);
3169                   SIMD16_SET (psr, overflow, SIMD_VBIT, i);
3170                 }
3171               break;
3172
3173             default:
3174               ARMul_UndefInstr (state, instr);
3175               return ARMul_DONE;
3176             }
3177         }
3178       break;
3179
3180     case Wqual:
3181       satrv[0] = satrv[1] = satrv[2] = satrv[4] = satrv[5] = satrv[6] = 0;
3182
3183       for (i = 0; i < 2; i++)
3184         {
3185           switch (BITS (20, 21))
3186             {
3187             case NoSaturation:
3188               s = SUBx ((i * 32), (i * 32) + 31, 0xffffffff, SubU32);
3189               satrv[BITIDX32 (i)] = 0;
3190               r |= (s & 0xffffffff) << (i * 32);
3191               SIMD32_SET (psr, NBIT32 (s), SIMD_NBIT, i);
3192               SIMD32_SET (psr, ZBIT32 (s), SIMD_ZBIT, i);
3193               SIMD32_SET (psr, carry,      SIMD_CBIT, i);
3194               SIMD32_SET (psr, overflow,   SIMD_VBIT, i);
3195               break;
3196
3197             case UnsignedSaturation:
3198               s = SUBx ((i * 32), (i * 32) + 31, 0xffffffff, SubU32);
3199               x = IwmmxtSaturateU32 (s, satrv + BITIDX32 (i));
3200               r |= (x & 0xffffffff) << (i * 32);
3201               SIMD32_SET (psr, NBIT32 (x), SIMD_NBIT, i);
3202               SIMD32_SET (psr, ZBIT32 (x), SIMD_ZBIT, i);
3203               if (! satrv [BITIDX32 (i)])
3204                 {
3205                   SIMD32_SET (psr, carry,    SIMD_CBIT, i);
3206                   SIMD32_SET (psr, overflow, SIMD_VBIT, i);
3207                 }
3208               break;
3209
3210             case SignedSaturation:
3211               s = SUBx ((i * 32), (i * 32) + 31, 0xffffffff, SubS32);
3212               x = IwmmxtSaturateS32 (s, satrv + BITIDX32 (i));
3213               r |= (x & 0xffffffff) << (i * 32);
3214               SIMD32_SET (psr, NBIT32 (x), SIMD_NBIT, i);
3215               SIMD32_SET (psr, ZBIT32 (x), SIMD_ZBIT, i);
3216               if (! satrv [BITIDX32 (i)])
3217                 {
3218                   SIMD32_SET (psr, carry,    SIMD_CBIT, i);
3219                   SIMD32_SET (psr, overflow, SIMD_VBIT, i);
3220                 }
3221               break;
3222
3223             default:
3224               ARMul_UndefInstr (state, instr);
3225               return ARMul_DONE;
3226             }
3227         }
3228       break;
3229
3230     default:
3231       ARMul_UndefInstr (state, instr);
3232       return ARMul_DONE;
3233     }
3234
3235   wR [BITS (12, 15)] = r;
3236   wC [wCASF] = psr;
3237   SET_wCSSFvec (satrv);
3238   wC [wCon] |= (WCON_CUP | WCON_MUP);
3239
3240 #undef SUBx
3241
3242   return ARMul_DONE;
3243 }
3244
3245 static int
3246 WUNPCKEH (ARMul_State * state, ARMword instr)
3247 {
3248   ARMdword r = 0;
3249   ARMword  psr = 0;
3250   ARMdword s;
3251   int      i;
3252
3253   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
3254     return ARMul_CANT;
3255
3256 #ifdef DEBUG
3257   fprintf (stderr, "wunpckeh\n");
3258 #endif
3259
3260   switch (BITS (22, 23))
3261     {
3262     case Bqual:
3263       for (i = 0; i < 4; i++)
3264         {
3265           s = wRBYTE (BITS (16, 19), i + 4);
3266
3267           if (BIT (21) && NBIT8 (s))
3268             s |= 0xff00;
3269
3270           r |= (s & 0xffff) << (i * 16);
3271           SIMD16_SET (psr, NBIT16 (s), SIMD_NBIT, i);
3272           SIMD16_SET (psr, ZBIT16 (s), SIMD_ZBIT, i);
3273         }
3274       break;
3275
3276     case Hqual:
3277       for (i = 0; i < 2; i++)
3278         {
3279           s = wRHALF (BITS (16, 19), i + 2);
3280
3281           if (BIT (21) && NBIT16 (s))
3282             s |= 0xffff0000;
3283
3284           r |= (s & 0xffffffff) << (i * 32);
3285           SIMD32_SET (psr, NBIT32 (s), SIMD_NBIT, i);
3286           SIMD32_SET (psr, ZBIT32 (s), SIMD_ZBIT, i);
3287         }
3288       break;
3289
3290     case Wqual:
3291       r = wRWORD (BITS (16, 19), 1);
3292
3293       if (BIT (21) && NBIT32 (r))
3294         r |= 0xffffffff00000000ULL;
3295
3296       SIMD64_SET (psr, NBIT64 (r), SIMD_NBIT);
3297       SIMD64_SET (psr, ZBIT64 (r), SIMD_ZBIT);
3298       break;
3299
3300     default:
3301       ARMul_UndefInstr (state, instr);
3302       return ARMul_DONE;
3303     }
3304
3305   wC [wCASF] = psr;
3306   wR [BITS (12, 15)] = r;
3307   wC [wCon] |= (WCON_CUP | WCON_MUP);
3308
3309   return ARMul_DONE;
3310 }
3311
3312 static int
3313 WUNPCKEL (ARMul_State * state, ARMword instr)
3314 {
3315   ARMdword r = 0;
3316   ARMword  psr = 0;
3317   ARMdword s;
3318   int      i;
3319
3320   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
3321     return ARMul_CANT;
3322
3323 #ifdef DEBUG
3324   fprintf (stderr, "wunpckel\n");
3325 #endif
3326
3327   switch (BITS (22, 23))
3328     {
3329     case Bqual:
3330       for (i = 0; i < 4; i++)
3331         {
3332           s = wRBYTE (BITS (16, 19), i);
3333
3334           if (BIT (21) && NBIT8 (s))
3335             s |= 0xff00;
3336
3337           r |= (s & 0xffff) << (i * 16);
3338           SIMD16_SET (psr, NBIT16 (s), SIMD_NBIT, i);
3339           SIMD16_SET (psr, ZBIT16 (s), SIMD_ZBIT, i);
3340         }
3341       break;
3342
3343     case Hqual:
3344       for (i = 0; i < 2; i++)
3345         {
3346           s = wRHALF (BITS (16, 19), i);
3347
3348           if (BIT (21) && NBIT16 (s))
3349             s |= 0xffff0000;
3350
3351           r |= (s & 0xffffffff) << (i * 32);
3352           SIMD32_SET (psr, NBIT32 (s), SIMD_NBIT, i);
3353           SIMD32_SET (psr, ZBIT32 (s), SIMD_ZBIT, i);
3354         }
3355       break;
3356
3357     case Wqual:
3358       r = wRWORD (BITS (16, 19), 0);
3359
3360       if (BIT (21) && NBIT32 (r))
3361         r |= 0xffffffff00000000ULL;
3362
3363       SIMD64_SET (psr, NBIT64 (r), SIMD_NBIT);
3364       SIMD64_SET (psr, ZBIT64 (r), SIMD_ZBIT);
3365       break;
3366
3367     default:
3368       ARMul_UndefInstr (state, instr);
3369       return ARMul_DONE;
3370     }
3371
3372   wC [wCASF] = psr;
3373   wR [BITS (12, 15)] = r;
3374   wC [wCon] |= (WCON_CUP | WCON_MUP);
3375
3376   return ARMul_DONE;
3377 }
3378
3379 static int
3380 WUNPCKIH (ARMul_State * state, ARMword instr)
3381 {
3382   ARMword  a, b;
3383   ARMdword r = 0;
3384   ARMword  psr = 0;
3385   ARMdword s;
3386   int      i;
3387
3388   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
3389     return ARMul_CANT;
3390
3391 #ifdef DEBUG
3392   fprintf (stderr, "wunpckih\n");
3393 #endif
3394
3395   switch (BITS (22, 23))
3396     {
3397     case Bqual:
3398       for (i = 0; i < 4; i++)
3399         {
3400           a = wRBYTE (BITS (16, 19), i + 4);
3401           b = wRBYTE (BITS ( 0,  3), i + 4);
3402           s = a | (b << 8);
3403           r |= (s & 0xffff) << (i * 16);
3404           SIMD8_SET (psr, NBIT8 (a), SIMD_NBIT, i * 2);
3405           SIMD8_SET (psr, ZBIT8 (a), SIMD_ZBIT, i * 2);
3406           SIMD8_SET (psr, NBIT8 (b), SIMD_NBIT, (i * 2) + 1);
3407           SIMD8_SET (psr, ZBIT8 (b), SIMD_ZBIT, (i * 2) + 1);
3408         }
3409       break;
3410
3411     case Hqual:
3412       for (i = 0; i < 2; i++)
3413         {
3414           a = wRHALF (BITS (16, 19), i + 2);
3415           b = wRHALF (BITS ( 0,  3), i + 2);
3416           s = a | (b << 16);
3417           r |= (s & 0xffffffff) << (i * 32);
3418           SIMD16_SET (psr, NBIT16 (a), SIMD_NBIT, (i * 2));
3419           SIMD16_SET (psr, ZBIT16 (a), SIMD_ZBIT, (i * 2));
3420           SIMD16_SET (psr, NBIT16 (b), SIMD_NBIT, (i * 2) + 1);
3421           SIMD16_SET (psr, ZBIT16 (b), SIMD_ZBIT, (i * 2) + 1);
3422         }
3423       break;
3424
3425     case Wqual:
3426       a = wRWORD (BITS (16, 19), 1);
3427       s = wRWORD (BITS ( 0,  3), 1);
3428       r = a | (s << 32);
3429
3430       SIMD32_SET (psr, NBIT32 (a), SIMD_NBIT, 0);
3431       SIMD32_SET (psr, ZBIT32 (a), SIMD_ZBIT, 0);
3432       SIMD32_SET (psr, NBIT32 (s), SIMD_NBIT, 1);
3433       SIMD32_SET (psr, ZBIT32 (s), SIMD_ZBIT, 1);
3434       break;
3435
3436     default:
3437       ARMul_UndefInstr (state, instr);
3438       return ARMul_DONE;
3439     }
3440
3441   wC [wCASF] = psr;
3442   wR [BITS (12, 15)] = r;
3443   wC [wCon] |= (WCON_CUP | WCON_MUP);
3444
3445   return ARMul_DONE;
3446 }
3447
3448 static int
3449 WUNPCKIL (ARMul_State * state, ARMword instr)
3450 {
3451   ARMword  a, b;
3452   ARMdword r = 0;
3453   ARMword  psr = 0;
3454   ARMdword s;
3455   int      i;
3456
3457   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
3458     return ARMul_CANT;
3459
3460 #ifdef DEBUG
3461   fprintf (stderr, "wunpckil\n");
3462 #endif
3463
3464   switch (BITS (22, 23))
3465     {
3466     case Bqual:
3467       for (i = 0; i < 4; i++)
3468         {
3469           a = wRBYTE (BITS (16, 19), i);
3470           b = wRBYTE (BITS ( 0,  3), i);
3471           s = a | (b << 8);
3472           r |= (s & 0xffff) << (i * 16);
3473           SIMD8_SET (psr, NBIT8 (a), SIMD_NBIT, i * 2);
3474           SIMD8_SET (psr, ZBIT8 (a), SIMD_ZBIT, i * 2);
3475           SIMD8_SET (psr, NBIT8 (b), SIMD_NBIT, (i * 2) + 1);
3476           SIMD8_SET (psr, ZBIT8 (b), SIMD_ZBIT, (i * 2) + 1);
3477         }
3478       break;
3479
3480     case Hqual:
3481       for (i = 0; i < 2; i++)
3482         {
3483           a = wRHALF (BITS (16, 19), i);
3484           b = wRHALF (BITS ( 0,  3), i);
3485           s = a | (b << 16);
3486           r |= (s & 0xffffffff) << (i * 32);
3487           SIMD16_SET (psr, NBIT16 (a), SIMD_NBIT, (i * 2));
3488           SIMD16_SET (psr, ZBIT16 (a), SIMD_ZBIT, (i * 2));
3489           SIMD16_SET (psr, NBIT16 (b), SIMD_NBIT, (i * 2) + 1);
3490           SIMD16_SET (psr, ZBIT16 (b), SIMD_ZBIT, (i * 2) + 1);
3491         }
3492       break;
3493
3494     case Wqual:
3495       a = wRWORD (BITS (16, 19), 0);
3496       s = wRWORD (BITS ( 0,  3), 0);
3497       r = a | (s << 32);
3498
3499       SIMD32_SET (psr, NBIT32 (a), SIMD_NBIT, 0);
3500       SIMD32_SET (psr, ZBIT32 (a), SIMD_ZBIT, 0);
3501       SIMD32_SET (psr, NBIT32 (s), SIMD_NBIT, 1);
3502       SIMD32_SET (psr, ZBIT32 (s), SIMD_ZBIT, 1);
3503       break;
3504
3505     default:
3506       ARMul_UndefInstr (state, instr);
3507       return ARMul_DONE;
3508     }
3509
3510   wC [wCASF] = psr;
3511   wR [BITS (12, 15)] = r;
3512   wC [wCon] |= (WCON_CUP | WCON_MUP);
3513
3514   return ARMul_DONE;
3515 }
3516
3517 static int
3518 WXOR (ARMword instr)
3519 {
3520   ARMword psr = 0;
3521   ARMdword result;
3522
3523   if ((read_cp15_reg (15, 0, 1) & 3) != 3)
3524     return ARMul_CANT;
3525
3526 #ifdef DEBUG
3527   fprintf (stderr, "wxor\n");
3528 #endif
3529
3530   result = wR [BITS (16, 19)] ^ wR [BITS (0, 3)];
3531   wR [BITS (12, 15)] = result;
3532
3533   SIMD64_SET (psr, (result == 0), SIMD_ZBIT);
3534   SIMD64_SET (psr, (result & (1ULL << 63)), SIMD_NBIT);
3535
3536   wC [wCASF] = psr;
3537   wC [wCon] |= (WCON_CUP | WCON_MUP);
3538
3539   return ARMul_DONE;
3540 }
3541
3542 /* This switch table is moved to a seperate function in order
3543    to work around a compiler bug in the host compiler...  */
3544
3545 static int
3546 Process_Instruction (ARMul_State * state, ARMword instr)
3547 {
3548   int status = ARMul_BUSY;
3549
3550   switch ((BITS (20, 23) << 8) | BITS (4, 11))
3551     {
3552     case 0x000: status = WOR (instr); break;
3553     case 0x011: status = TMCR (state, instr); break;
3554     case 0x100: status = WXOR (instr); break;
3555     case 0x111: status = TMRC (state, instr); break;
3556     case 0x300: status = WANDN (instr); break;
3557     case 0x200: status = WAND (instr); break;
3558
3559     case 0x810: case 0xa10:
3560       status = WMADD (instr); break;
3561
3562     case 0x10e: case 0x50e: case 0x90e: case 0xd0e:
3563       status = WUNPCKIL (state, instr); break;
3564     case 0x10c: case 0x50c: case 0x90c: case 0xd0c:
3565       status = WUNPCKIH (state, instr); break;
3566     case 0x012: case 0x112: case 0x412: case 0x512:
3567       status = WSAD (instr); break;
3568     case 0x010: case 0x110: case 0x210: case 0x310:
3569       status = WMUL (instr); break;
3570     case 0x410: case 0x510: case 0x610: case 0x710:
3571       status = WMAC (instr); break;
3572     case 0x006: case 0x406: case 0x806: case 0xc06:
3573       status = WCMPEQ (state, instr); break;
3574     case 0x800: case 0x900: case 0xc00: case 0xd00:
3575       status = WAVG2 (instr); break;
3576     case 0x802: case 0x902: case 0xa02: case 0xb02:
3577       status = WALIGNR (state, instr); break;
3578     case 0x601: case 0x605: case 0x609: case 0x60d:
3579       status = TINSR (state, instr); break;
3580     case 0x107: case 0x507: case 0x907: case 0xd07:
3581       status = TEXTRM (state, instr); break;
3582     case 0x117: case 0x517: case 0x917: case 0xd17:
3583       status = TEXTRC (state, instr); break;
3584     case 0x401: case 0x405: case 0x409: case 0x40d:
3585       status = TBCST (state, instr); break;
3586     case 0x113: case 0x513: case 0x913: case 0xd13:
3587       status = TANDC (state, instr); break;
3588     case 0x01c: case 0x41c: case 0x81c: case 0xc1c:
3589       status = WACC (state, instr); break;
3590     case 0x115: case 0x515: case 0x915: case 0xd15:
3591       status = TORC (state, instr); break;
3592     case 0x103: case 0x503: case 0x903: case 0xd03:
3593       status = TMOVMSK (state, instr); break;
3594     case 0x106: case 0x306: case 0x506: case 0x706:
3595     case 0x906: case 0xb06: case 0xd06: case 0xf06:
3596       status = WCMPGT (state, instr); break;
3597     case 0x00e: case 0x20e: case 0x40e: case 0x60e:
3598     case 0x80e: case 0xa0e: case 0xc0e: case 0xe0e:
3599       status = WUNPCKEL (state, instr); break;
3600     case 0x00c: case 0x20c: case 0x40c: case 0x60c:
3601     case 0x80c: case 0xa0c: case 0xc0c: case 0xe0c:
3602       status = WUNPCKEH (state, instr); break;
3603     case 0x204: case 0x604: case 0xa04: case 0xe04:
3604     case 0x214: case 0x614: case 0xa14: case 0xe14:
3605       status = WSRL (state, instr); break;
3606     case 0x004: case 0x404: case 0x804: case 0xc04:
3607     case 0x014: case 0x414: case 0x814: case 0xc14:
3608       status = WSRA (state, instr); break;
3609     case 0x104: case 0x504: case 0x904: case 0xd04:
3610     case 0x114: case 0x514: case 0x914: case 0xd14:
3611       status = WSLL (state, instr); break;
3612     case 0x304: case 0x704: case 0xb04: case 0xf04:
3613     case 0x314: case 0x714: case 0xb14: case 0xf14:
3614       status = WROR (state, instr); break;
3615     case 0x116: case 0x316: case 0x516: case 0x716:
3616     case 0x916: case 0xb16: case 0xd16: case 0xf16:
3617       status = WMIN (state, instr); break;
3618     case 0x016: case 0x216: case 0x416: case 0x616:
3619     case 0x816: case 0xa16: case 0xc16: case 0xe16:
3620       status = WMAX (state, instr); break;
3621     case 0x002: case 0x102: case 0x202: case 0x302:
3622     case 0x402: case 0x502: case 0x602: case 0x702:
3623       status = WALIGNI (instr); break;
3624     case 0x01a: case 0x11a: case 0x21a: case 0x31a:
3625     case 0x41a: case 0x51a: case 0x61a: case 0x71a:
3626     case 0x81a: case 0x91a: case 0xa1a: case 0xb1a:
3627     case 0xc1a: case 0xd1a: case 0xe1a: case 0xf1a:
3628       status = WSUB (state, instr); break;
3629     case 0x01e: case 0x11e: case 0x21e: case 0x31e:
3630     case 0x41e: case 0x51e: case 0x61e: case 0x71e:
3631     case 0x81e: case 0x91e: case 0xa1e: case 0xb1e:
3632     case 0xc1e: case 0xd1e: case 0xe1e: case 0xf1e:
3633       status = WSHUFH (instr); break;
3634     case 0x018: case 0x118: case 0x218: case 0x318:
3635     case 0x418: case 0x518: case 0x618: case 0x718:
3636     case 0x818: case 0x918: case 0xa18: case 0xb18:
3637     case 0xc18: case 0xd18: case 0xe18: case 0xf18:
3638       status = WADD (state, instr); break;
3639     case 0x008: case 0x108: case 0x208: case 0x308:
3640     case 0x408: case 0x508: case 0x608: case 0x708:
3641     case 0x808: case 0x908: case 0xa08: case 0xb08:
3642     case 0xc08: case 0xd08: case 0xe08: case 0xf08:
3643       status = WPACK (state, instr); break;
3644     case 0x201: case 0x203: case 0x205: case 0x207:
3645     case 0x209: case 0x20b: case 0x20d: case 0x20f:
3646     case 0x211: case 0x213: case 0x215: case 0x217:
3647     case 0x219: case 0x21b: case 0x21d: case 0x21f:
3648       switch (BITS (16, 19))
3649         {
3650         case 0x0: status = TMIA (state, instr); break;
3651         case 0x8: status = TMIAPH (state, instr); break;
3652         case 0xc:
3653         case 0xd:
3654         case 0xe:
3655         case 0xf: status = TMIAxy (state, instr); break;
3656         default: break;
3657         }
3658       break;
3659     default:
3660       break;
3661     }
3662   return status;
3663 }
3664
3665 /* Process a possibly Intel(r) Wireless MMX(tm) technology instruction.
3666    Return true if the instruction was handled.  */
3667
3668 int
3669 ARMul_HandleIwmmxt (ARMul_State * state, ARMword instr)
3670 {
3671   int status = ARMul_BUSY;
3672
3673   if (BITS (24, 27) == 0xe)
3674     {
3675       status = Process_Instruction (state, instr);
3676     }
3677   else if (BITS (25, 27) == 0x6)
3678     {
3679       if (BITS (4, 11) == 0x0 && BITS (20, 24) == 0x4)
3680         status = TMCRR (state, instr);
3681       else if (BITS (9, 11) == 0x0)
3682         {
3683           if (BIT (20) == 0x0)
3684             status = WSTR (state, instr);
3685           else if (BITS (20, 24) == 0x5)
3686             status = TMRRC (state, instr);
3687           else
3688             status = WLDR (state, instr);
3689         }
3690     }
3691
3692   if (status == ARMul_CANT)
3693     {
3694       /* If the instruction was a recognised but illegal,
3695          perform the abort here rather than returning false.
3696          If we return false then ARMul_MRC may be called which
3697          will still abort, but which also perform the register
3698          transfer...  */
3699       ARMul_Abort (state, ARMul_UndefinedInstrV);
3700       status = ARMul_DONE;
3701     }
3702
3703   return status == ARMul_DONE;
3704 }
3705
3706 int
3707 Fetch_Iwmmxt_Register (unsigned int regnum, unsigned char * memory)
3708 {
3709   if (regnum >= 16)
3710     {
3711       memcpy (memory, wC + (regnum - 16), sizeof wC [0]);
3712       return sizeof wC [0];
3713     }
3714   else
3715     {
3716       memcpy (memory, wR + regnum, sizeof wR [0]);
3717       return sizeof wR [0];
3718     }
3719 }
3720
3721 int
3722 Store_Iwmmxt_Register (unsigned int regnum, unsigned char * memory)
3723 {
3724   if (regnum >= 16)
3725     {
3726       memcpy (wC + (regnum - 16), memory, sizeof wC [0]);
3727       return sizeof wC [0];
3728     }
3729   else
3730     {
3731       memcpy (wR + regnum, memory, sizeof wR [0]);
3732       return sizeof wR [0];
3733     }
3734 }