sim/arm/armsupp.c

   1 /*  armsupp.c -- ARMulator support code:  ARM6 Instruction Emulator.
   2     Copyright (C) 1994 Advanced RISC Machines Ltd.
   3
   4     This program is free software; you can redistribute it and/or modify
   5     it under the terms of the GNU General Public License as published by
   6     the Free Software Foundation; either version 3 of the License, or
   7     (at your option) any later version.
   8
   9     This program is distributed in the hope that it will be useful,
  10     but WITHOUT ANY WARRANTY; without even the implied warranty of
  11     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12     GNU General Public License for more details.
  13
  14     You should have received a copy of the GNU General Public License
  15     along with this program; if not, see <http://www.gnu.org/licenses/>. */
  16
  17 #include "armdefs.h"
  18 #include "armemu.h"
  19 #include "ansidecl.h"
  20 #include <math.h>
  21
  22 /* Definitions for the support routines.  */
  23
  24 static ARMword ModeToBank (ARMword);
  25 static void    EnvokeList (ARMul_State *, unsigned long, unsigned long);
  26
  27 struct EventNode
  28 {                                       /* An event list node.  */
  29   unsigned (*func) (ARMul_State *);     /* The function to call.  */
  30   struct EventNode *next;
  31 };
  32
  33 /* This routine returns the value of a register from a mode.  */
  34
  35 ARMword
  36 ARMul_GetReg (ARMul_State * state, unsigned mode, unsigned reg)
  37 {
  38   mode &= MODEBITS;
  39   if (mode != state->Mode)
  40     return (state->RegBank[ModeToBank ((ARMword) mode)][reg]);
  41   else
  42     return (state->Reg[reg]);
  43 }
  44
  45 /* This routine sets the value of a register for a mode.  */
  46
  47 void
  48 ARMul_SetReg (ARMul_State * state, unsigned mode, unsigned reg, ARMword value)
  49 {
  50   mode &= MODEBITS;
  51   if (mode != state->Mode)
  52     state->RegBank[ModeToBank ((ARMword) mode)][reg] = value;
  53   else
  54     state->Reg[reg] = value;
  55 }
  56
  57 /* This routine returns the value of the PC, mode independently.  */
  58
  59 ARMword
  60 ARMul_GetPC (ARMul_State * state)
  61 {
  62   if (state->Mode > SVC26MODE)
  63     return state->Reg[15];
  64   else
  65     return R15PC;
  66 }
  67
  68 /* This routine returns the value of the PC, mode independently.  */
  69
  70 ARMword
  71 ARMul_GetNextPC (ARMul_State * state)
  72 {
  73   if (state->Mode > SVC26MODE)
  74     return state->Reg[15] + isize;
  75   else
  76     return (state->Reg[15] + isize) & R15PCBITS;
  77 }
  78
  79 /* This routine sets the value of the PC.  */
  80
  81 void
  82 ARMul_SetPC (ARMul_State * state, ARMword value)
  83 {
  84   if (ARMul_MODE32BIT)
  85     state->Reg[15] = value & PCBITS;
  86   else
  87     state->Reg[15] = R15CCINTMODE | (value & R15PCBITS);
  88   FLUSHPIPE;
  89 }
  90
  91 /* This routine returns the value of register 15, mode independently.  */
  92
  93 ARMword
  94 ARMul_GetR15 (ARMul_State * state)
  95 {
  96   if (state->Mode > SVC26MODE)
  97     return (state->Reg[15]);
  98   else
  99     return (R15PC | ECC | ER15INT | EMODE);
 100 }
 101
 102 /* This routine sets the value of Register 15.  */
 103
 104 void
 105 ARMul_SetR15 (ARMul_State * state, ARMword value)
 106 {
 107   if (ARMul_MODE32BIT)
 108     state->Reg[15] = value & PCBITS;
 109   else
 110     {
 111       state->Reg[15] = value;
 112       ARMul_R15Altered (state);
 113     }
 114   FLUSHPIPE;
 115 }
 116
 117 /* This routine returns the value of the CPSR.  */
 118
 119 ARMword
 120 ARMul_GetCPSR (ARMul_State * state)
 121 {
 122   return (CPSR | state->Cpsr);
 123 }
 124
 125 /* This routine sets the value of the CPSR.  */
 126
 127 void
 128 ARMul_SetCPSR (ARMul_State * state, ARMword value)
 129 {
 130   state->Cpsr = value;
 131   ARMul_CPSRAltered (state);
 132 }
 133
 134 /* This routine does all the nasty bits involved in a write to the CPSR,
 135    including updating the register bank, given a MSR instruction.  */
 136
 137 void
 138 ARMul_FixCPSR (ARMul_State * state, ARMword instr, ARMword rhs)
 139 {
 140   state->Cpsr = ARMul_GetCPSR (state);
 141
 142   if (state->Mode != USER26MODE
 143       && state->Mode != USER32MODE)
 144     {
 145       /* In user mode, only write flags.  */
 146       if (BIT (16))
 147         SETPSR_C (state->Cpsr, rhs);
 148       if (BIT (17))
 149         SETPSR_X (state->Cpsr, rhs);
 150       if (BIT (18))
 151         SETPSR_S (state->Cpsr, rhs);
 152     }
 153   if (BIT (19))
 154     SETPSR_F (state->Cpsr, rhs);
 155   ARMul_CPSRAltered (state);
 156 }
 157
 158 /* Get an SPSR from the specified mode.  */
 159
 160 ARMword
 161 ARMul_GetSPSR (ARMul_State * state, ARMword mode)
 162 {
 163   ARMword bank = ModeToBank (mode & MODEBITS);
 164
 165   if (! BANK_CAN_ACCESS_SPSR (bank))
 166     return ARMul_GetCPSR (state);
 167
 168   return state->Spsr[bank];
 169 }
 170
 171 /* This routine does a write to an SPSR.  */
 172
 173 void
 174 ARMul_SetSPSR (ARMul_State * state, ARMword mode, ARMword value)
 175 {
 176   ARMword bank = ModeToBank (mode & MODEBITS);
 177
 178   if (BANK_CAN_ACCESS_SPSR (bank))
 179     state->Spsr[bank] = value;
 180 }
 181
 182 /* This routine does a write to the current SPSR, given an MSR instruction.  */
 183
 184 void
 185 ARMul_FixSPSR (ARMul_State * state, ARMword instr, ARMword rhs)
 186 {
 187   if (BANK_CAN_ACCESS_SPSR (state->Bank))
 188     {
 189       if (BIT (16))
 190         SETPSR_C (state->Spsr[state->Bank], rhs);
 191       if (BIT (17))
 192         SETPSR_X (state->Spsr[state->Bank], rhs);
 193       if (BIT (18))
 194         SETPSR_S (state->Spsr[state->Bank], rhs);
 195       if (BIT (19))
 196         SETPSR_F (state->Spsr[state->Bank], rhs);
 197     }
 198 }
 199
 200 /* This routine updates the state of the emulator after the Cpsr has been
 201    changed.  Both the processor flags and register bank are updated.  */
 202
 203 void
 204 ARMul_CPSRAltered (ARMul_State * state)
 205 {
 206   ARMword oldmode;
 207
 208   if (state->prog32Sig == LOW)
 209     state->Cpsr &= (CCBITS | INTBITS | R15MODEBITS);
 210
 211   oldmode = state->Mode;
 212
 213   if (state->Mode != (state->Cpsr & MODEBITS))
 214     {
 215       state->Mode =
 216         ARMul_SwitchMode (state, state->Mode, state->Cpsr & MODEBITS);
 217
 218       state->NtransSig = (state->Mode & 3) ? HIGH : LOW;
 219     }
 220   state->Cpsr &= ~MODEBITS;
 221
 222   ASSIGNINT (state->Cpsr & INTBITS);
 223   state->Cpsr &= ~INTBITS;
 224   ASSIGNN ((state->Cpsr & NBIT) != 0);
 225   state->Cpsr &= ~NBIT;
 226   ASSIGNZ ((state->Cpsr & ZBIT) != 0);
 227   state->Cpsr &= ~ZBIT;
 228   ASSIGNC ((state->Cpsr & CBIT) != 0);
 229   state->Cpsr &= ~CBIT;
 230   ASSIGNV ((state->Cpsr & VBIT) != 0);
 231   state->Cpsr &= ~VBIT;
 232   ASSIGNS ((state->Cpsr & SBIT) != 0);
 233   state->Cpsr &= ~SBIT;
 234 #ifdef MODET
 235   ASSIGNT ((state->Cpsr & TBIT) != 0);
 236   state->Cpsr &= ~TBIT;
 237 #endif
 238
 239   if (oldmode > SVC26MODE)
 240     {
 241       if (state->Mode <= SVC26MODE)
 242         {
 243           state->Emulate = CHANGEMODE;
 244           state->Reg[15] = ECC | ER15INT | EMODE | R15PC;
 245         }
 246     }
 247   else
 248     {
 249       if (state->Mode > SVC26MODE)
 250         {
 251           state->Emulate = CHANGEMODE;
 252           state->Reg[15] = R15PC;
 253         }
 254       else
 255         state->Reg[15] = ECC | ER15INT | EMODE | R15PC;
 256     }
 257 }
 258
 259 /* This routine updates the state of the emulator after register 15 has
 260    been changed.  Both the processor flags and register bank are updated.
 261    This routine should only be called from a 26 bit mode.  */
 262
 263 void
 264 ARMul_R15Altered (ARMul_State * state)
 265 {
 266   if (state->Mode != R15MODE)
 267     {
 268       state->Mode = ARMul_SwitchMode (state, state->Mode, R15MODE);
 269       state->NtransSig = (state->Mode & 3) ? HIGH : LOW;
 270     }
 271
 272   if (state->Mode > SVC26MODE)
 273     state->Emulate = CHANGEMODE;
 274
 275   ASSIGNR15INT (R15INT);
 276
 277   ASSIGNN ((state->Reg[15] & NBIT) != 0);
 278   ASSIGNZ ((state->Reg[15] & ZBIT) != 0);
 279   ASSIGNC ((state->Reg[15] & CBIT) != 0);
 280   ASSIGNV ((state->Reg[15] & VBIT) != 0);
 281 }
 282
 283 /* This routine controls the saving and restoring of registers across mode
 284    changes.  The regbank matrix is largely unused, only rows 13 and 14 are
 285    used across all modes, 8 to 14 are used for FIQ, all others use the USER
 286    column.  It's easier this way.  old and new parameter are modes numbers.
 287    Notice the side effect of changing the Bank variable.  */
 288
 289 ARMword
 290 ARMul_SwitchMode (ARMul_State * state, ARMword oldmode, ARMword newmode)
 291 {
 292   unsigned i;
 293   ARMword  oldbank;
 294   ARMword  newbank;
 295
 296   oldbank = ModeToBank (oldmode);
 297   newbank = state->Bank = ModeToBank (newmode);
 298
 299   /* Do we really need to do it?  */
 300   if (oldbank != newbank)
 301     {
 302       /* Save away the old registers.  */
 303       switch (oldbank)
 304         {
 305         case USERBANK:
 306         case IRQBANK:
 307         case SVCBANK:
 308         case ABORTBANK:
 309         case UNDEFBANK:
 310           if (newbank == FIQBANK)
 311             for (i = 8; i < 13; i++)
 312               state->RegBank[USERBANK][i] = state->Reg[i];
 313           state->RegBank[oldbank][13] = state->Reg[13];
 314           state->RegBank[oldbank][14] = state->Reg[14];
 315           break;
 316         case FIQBANK:
 317           for (i = 8; i < 15; i++)
 318             state->RegBank[FIQBANK][i] = state->Reg[i];
 319           break;
 320         case DUMMYBANK:
 321           for (i = 8; i < 15; i++)
 322             state->RegBank[DUMMYBANK][i] = 0;
 323           break;
 324         default:
 325           abort ();
 326         }
 327
 328       /* Restore the new registers.  */
 329       switch (newbank)
 330         {
 331         case USERBANK:
 332         case IRQBANK:
 333         case SVCBANK:
 334         case ABORTBANK:
 335         case UNDEFBANK:
 336           if (oldbank == FIQBANK)
 337             for (i = 8; i < 13; i++)
 338               state->Reg[i] = state->RegBank[USERBANK][i];
 339           state->Reg[13] = state->RegBank[newbank][13];
 340           state->Reg[14] = state->RegBank[newbank][14];
 341           break;
 342         case FIQBANK:
 343           for (i = 8; i < 15; i++)
 344             state->Reg[i] = state->RegBank[FIQBANK][i];
 345           break;
 346         case DUMMYBANK:
 347           for (i = 8; i < 15; i++)
 348             state->Reg[i] = 0;
 349           break;
 350         default:
 351           abort ();
 352         }
 353     }
 354
 355   return newmode;
 356 }
 357
 358 /* Given a processor mode, this routine returns the
 359    register bank that will be accessed in that mode.  */
 360
 361 static ARMword
 362 ModeToBank (ARMword mode)
 363 {
 364   static ARMword bankofmode[] =
 365   {
 366     USERBANK,  FIQBANK,   IRQBANK,   SVCBANK,
 367     DUMMYBANK, DUMMYBANK, DUMMYBANK, DUMMYBANK,
 368     DUMMYBANK, DUMMYBANK, DUMMYBANK, DUMMYBANK,
 369     DUMMYBANK, DUMMYBANK, DUMMYBANK, DUMMYBANK,
 370     USERBANK,  FIQBANK,   IRQBANK,   SVCBANK,
 371     DUMMYBANK, DUMMYBANK, DUMMYBANK, ABORTBANK,
 372     DUMMYBANK, DUMMYBANK, DUMMYBANK, UNDEFBANK,
 373     DUMMYBANK, DUMMYBANK, DUMMYBANK, SYSTEMBANK
 374   };
 375
 376   if (mode >= (sizeof (bankofmode) / sizeof (bankofmode[0])))
 377     return DUMMYBANK;
 378
 379   return bankofmode[mode];
 380 }
 381
 382 /* Returns the register number of the nth register in a reg list.  */
 383
 384 unsigned
 385 ARMul_NthReg (ARMword instr, unsigned number)
 386 {
 387   unsigned bit, upto;
 388
 389   for (bit = 0, upto = 0; upto <= number; bit ++)
 390     if (BIT (bit))
 391       upto ++;
 392
 393   return (bit - 1);
 394 }
 395
 396 /* Assigns the N and Z flags depending on the value of result.  */
 397
 398 void
 399 ARMul_NegZero (ARMul_State * state, ARMword result)
 400 {
 401   if (NEG (result))
 402     {
 403       SETN;
 404       CLEARZ;
 405     }
 406   else if (result == 0)
 407     {
 408       CLEARN;
 409       SETZ;
 410     }
 411   else
 412     {
 413       CLEARN;
 414       CLEARZ;
 415     }
 416 }
 417
 418 /* Compute whether an addition of A and B, giving RESULT, overflowed.  */
 419
 420 int
 421 AddOverflow (ARMword a, ARMword b, ARMword result)
 422 {
 423   return ((NEG (a) && NEG (b) && POS (result))
 424           || (POS (a) && POS (b) && NEG (result)));
 425 }
 426
 427 /* Compute whether a subtraction of A and B, giving RESULT, overflowed.  */
 428
 429 int
 430 SubOverflow (ARMword a, ARMword b, ARMword result)
 431 {
 432   return ((NEG (a) && POS (b) && POS (result))
 433           || (POS (a) && NEG (b) && NEG (result)));
 434 }
 435
 436 /* Assigns the C flag after an addition of a and b to give result.  */
 437
 438 void
 439 ARMul_AddCarry (ARMul_State * state, ARMword a, ARMword b, ARMword result)
 440 {
 441   ASSIGNC ((NEG (a) && NEG (b)) ||
 442            (NEG (a) && POS (result)) || (NEG (b) && POS (result)));
 443 }
 444
 445 /* Assigns the V flag after an addition of a and b to give result.  */
 446
 447 void
 448 ARMul_AddOverflow (ARMul_State * state, ARMword a, ARMword b, ARMword result)
 449 {
 450   ASSIGNV (AddOverflow (a, b, result));
 451 }
 452
 453 /* Assigns the C flag after an subtraction of a and b to give result.  */
 454
 455 void
 456 ARMul_SubCarry (ARMul_State * state, ARMword a, ARMword b, ARMword result)
 457 {
 458   ASSIGNC ((NEG (a) && POS (b)) ||
 459            (NEG (a) && POS (result)) || (POS (b) && POS (result)));
 460 }
 461
 462 /* Assigns the V flag after an subtraction of a and b to give result.  */
 463
 464 void
 465 ARMul_SubOverflow (ARMul_State * state, ARMword a, ARMword b, ARMword result)
 466 {
 467   ASSIGNV (SubOverflow (a, b, result));
 468 }
 469
 470 static void
 471 handle_VFP_xfer (ARMul_State * state, ARMword instr)
 472 {
 473   if (TOPBITS (28) == NV)
 474     {
 475       fprintf (stderr, "SIM: UNDEFINED VFP instruction\n");
 476       return;
 477     }
 478
 479   if (BITS (25, 27) != 0x6)
 480     {
 481       fprintf (stderr, "SIM: ISE: VFP handler called incorrectly\n");
 482       return;
 483     }
 484
 485   switch (BITS (20, 24))
 486     {
 487     case 0x04:
 488     case 0x05:
 489       {
 490         /* VMOV double precision to/from two ARM registers.  */
 491         int vm  = BITS (0, 3);
 492         int rt1 = BITS (12, 15);
 493         int rt2 = BITS (16, 19);
 494
 495         /* FIXME: UNPREDICTABLE if rt1 == 15 or rt2 == 15.  */
 496         if (BIT (20))
 497           {
 498             /* Transfer to ARM.  */
 499             /* FIXME: UPPREDICTABLE if rt1 == rt2.  */
 500             state->Reg[rt1] = VFP_dword (vm) & 0xffffffff;
 501             state->Reg[rt2] = VFP_dword (vm) >> 32;
 502           }
 503         else
 504           {
 505             VFP_dword (vm) = state->Reg[rt2];
 506             VFP_dword (vm) <<= 32;
 507             VFP_dword (vm) |= (state->Reg[rt1] & 0xffffffff);
 508           }
 509         return;
 510       }
 511
 512     case 0x08:
 513     case 0x0A:
 514     case 0x0C:
 515     case 0x0E:
 516       {
 517         /* VSTM with PUW=011 or PUW=010.  */
 518         int n = BITS (16, 19);
 519         int imm8 = BITS (0, 7);
 520
 521         ARMword address = state->Reg[n];
 522         if (BIT (21))
 523           state->Reg[n] = address + (imm8 << 2);
 524
 525         if (BIT (8))
 526           {
 527             int src = (BIT (22) << 4) | BITS (12, 15);
 528             imm8 >>= 1;
 529             while (imm8--)
 530               {
 531                 if (state->bigendSig)
 532                   {
 533                     ARMul_StoreWordN (state, address, VFP_dword (src) >> 32);
 534                     ARMul_StoreWordN (state, address + 4, VFP_dword (src));
 535                   }
 536                 else
 537                   {
 538                     ARMul_StoreWordN (state, address, VFP_dword (src));
 539                     ARMul_StoreWordN (state, address + 4, VFP_dword (src) >> 32);
 540                   }
 541                 address += 8;
 542                 src += 1;
 543               }
 544           }
 545         else
 546           {
 547             int src = (BITS (12, 15) << 1) | BIT (22);
 548             while (imm8--)
 549               {
 550                 ARMul_StoreWordN (state, address, VFP_uword (src));
 551                 address += 4;
 552                 src += 1;
 553               }
 554           }
 555       }
 556       return;
 557
 558     case 0x10:
 559     case 0x14:
 560     case 0x18:
 561     case 0x1C:
 562       {
 563         /* VSTR */
 564         ARMword imm32 = BITS (0, 7) << 2;
 565         int base = state->Reg[LHSReg];
 566         ARMword address;
 567         int dest;
 568
 569         if (LHSReg == 15)
 570           base = (base + 3) & ~3;
 571
 572         address = base + (BIT (23) ? imm32 : - imm32);
 573
 574         if (CPNum == 10)
 575           {
 576             dest = (DESTReg << 1) + BIT (22);
 577
 578             ARMul_StoreWordN (state, address, VFP_uword (dest));
 579           }
 580         else
 581           {
 582             dest = (BIT (22) << 4) + DESTReg;
 583
 584             if (state->bigendSig)
 585               {
 586                 ARMul_StoreWordN (state, address, VFP_dword (dest) >> 32);
 587                 ARMul_StoreWordN (state, address + 4, VFP_dword (dest));
 588               }
 589             else
 590               {
 591                 ARMul_StoreWordN (state, address, VFP_dword (dest));
 592                 ARMul_StoreWordN (state, address + 4, VFP_dword (dest) >> 32);
 593               }
 594           }
 595       }
 596       return;
 597
 598     case 0x12:
 599     case 0x16:
 600       if (BITS (16, 19) == 13)
 601         {
 602           /* VPUSH */
 603           ARMword address = state->Reg[13] - (BITS (0, 7) << 2);
 604           state->Reg[13] = address;
 605
 606           if (BIT (8))
 607             {
 608               int dreg = (BIT (22) << 4) | BITS (12, 15);
 609               int num  = BITS (0, 7) >> 1;
 610               while (num--)
 611                 {
 612                   if (state->bigendSig)
 613                     {
 614                       ARMul_StoreWordN (state, address, VFP_dword (dreg) >> 32);
 615                       ARMul_StoreWordN (state, address + 4, VFP_dword (dreg));
 616                     }
 617                   else
 618                     {
 619                       ARMul_StoreWordN (state, address, VFP_dword (dreg));
 620                       ARMul_StoreWordN (state, address + 4, VFP_dword (dreg) >> 32);
 621                     }
 622                   address += 8;
 623                   dreg += 1;
 624                 }
 625             }
 626           else
 627             {
 628               int sreg = (BITS (12, 15) << 1) | BIT (22);
 629               int num  = BITS (0, 7);
 630               while (num--)
 631                 {
 632                   ARMul_StoreWordN (state, address, VFP_uword (sreg));
 633                   address += 4;
 634                   sreg += 1;
 635                 }
 636             }
 637         }
 638       else if (BITS (9, 11) != 0x5)
 639         break;
 640       else
 641         {
 642           /* VSTM PUW=101 */
 643           int n = BITS (16, 19);
 644           int imm8 = BITS (0, 7);
 645           ARMword address = state->Reg[n] - (imm8 << 2);
 646           state->Reg[n] = address;
 647
 648           if (BIT (8))
 649             {
 650               int src = (BIT (22) << 4) | BITS (12, 15);
 651
 652               imm8 >>= 1;
 653               while (imm8--)
 654                 {
 655                   if (state->bigendSig)
 656                     {
 657                       ARMul_StoreWordN (state, address, VFP_dword (src) >> 32);
 658                       ARMul_StoreWordN (state, address + 4, VFP_dword (src));
 659                     }
 660                   else
 661                     {
 662                       ARMul_StoreWordN (state, address, VFP_dword (src));
 663                       ARMul_StoreWordN (state, address + 4, VFP_dword (src) >> 32);
 664                     }
 665                   address += 8;
 666                   src += 1;
 667                 }
 668             }
 669           else
 670             {
 671               int src = (BITS (12, 15) << 1) | BIT (22);
 672
 673               while (imm8--)
 674                 {
 675                   ARMul_StoreWordN (state, address, VFP_uword (src));
 676                   address += 4;
 677                   src += 1;
 678                 }
 679             }
 680         }
 681       return;
 682
 683     case 0x13:
 684     case 0x17:
 685       /* VLDM PUW=101 */
 686     case 0x09:
 687     case 0x0D:
 688       /* VLDM PUW=010 */
 689         {
 690           int n = BITS (16, 19);
 691           int imm8 = BITS (0, 7);
 692
 693           ARMword address = state->Reg[n];
 694           if (BIT (23) == 0)
 695             address -= imm8 << 2;
 696           if (BIT (21))
 697             state->Reg[n] = BIT (23) ? address + (imm8 << 2) : address;
 698
 699           if (BIT (8))
 700             {
 701               int dest = (BIT (22) << 4) | BITS (12, 15);
 702               imm8 >>= 1;
 703               while (imm8--)
 704                 {
 705                   if (state->bigendSig)
 706                     {
 707                       VFP_dword (dest) = ARMul_LoadWordN (state, address);
 708                       VFP_dword (dest) <<= 32;
 709                       VFP_dword (dest) |= ARMul_LoadWordN (state, address + 4);
 710                     }
 711                   else
 712                     {
 713                       VFP_dword (dest) = ARMul_LoadWordN (state, address + 4);
 714                       VFP_dword (dest) <<= 32;
 715                       VFP_dword (dest) |= ARMul_LoadWordN (state, address);
 716                     }
 717
 718                   if (trace)
 719                     fprintf (stderr, " VFP: VLDM: D%d = %g\n", dest, VFP_dval (dest));
 720
 721                   address += 8;
 722                   dest += 1;
 723                 }
 724             }
 725           else
 726             {
 727               int dest = (BITS (12, 15) << 1) | BIT (22);
 728
 729               while (imm8--)
 730                 {
 731                   VFP_uword (dest) = ARMul_LoadWordN (state, address);
 732                   address += 4;
 733                   dest += 1;
 734                 }
 735             }
 736         }
 737       return;
 738
 739     case 0x0B:
 740     case 0x0F:
 741       if (BITS (16, 19) == 13)
 742         {
 743           /* VPOP */
 744           ARMword address = state->Reg[13];
 745           state->Reg[13] = address + (BITS (0, 7) << 2);
 746
 747           if (BIT (8))
 748             {
 749               int dest = (BIT (22) << 4) | BITS (12, 15);
 750               int num  = BITS (0, 7) >> 1;
 751
 752               while (num--)
 753                 {
 754                   if (state->bigendSig)
 755                     {
 756                       VFP_dword (dest) = ARMul_LoadWordN (state, address);
 757                       VFP_dword (dest) <<= 32;
 758                       VFP_dword (dest) |= ARMul_LoadWordN (state, address + 4);
 759                     }
 760                   else
 761                     {
 762                       VFP_dword (dest) = ARMul_LoadWordN (state, address + 4);
 763                       VFP_dword (dest) <<= 32;
 764                       VFP_dword (dest) |= ARMul_LoadWordN (state, address);
 765                     }
 766
 767                   if (trace)
 768                     fprintf (stderr, " VFP: VPOP: D%d = %g\n", dest, VFP_dval (dest));
 769
 770                   address += 8;
 771                   dest += 1;
 772                 }
 773             }
 774           else
 775             {
 776               int sreg = (BITS (12, 15) << 1) | BIT (22);
 777               int num  = BITS (0, 7);
 778
 779               while (num--)
 780                 {
 781                   VFP_uword (sreg) = ARMul_LoadWordN (state, address);
 782                   address += 4;
 783                   sreg += 1;
 784                 }
 785             }
 786         }
 787       else if (BITS (9, 11) != 0x5)
 788         break;
 789       else
 790         {
 791           /* VLDM PUW=011 */
 792           int n = BITS (16, 19);
 793           int imm8 = BITS (0, 7);
 794           ARMword address = state->Reg[n];
 795           state->Reg[n] += imm8 << 2;
 796
 797           if (BIT (8))
 798             {
 799               int dest = (BIT (22) << 4) | BITS (12, 15);
 800
 801               imm8 >>= 1;
 802               while (imm8--)
 803                 {
 804                   if (state->bigendSig)
 805                     {
 806                       VFP_dword (dest) = ARMul_LoadWordN (state, address);
 807                       VFP_dword (dest) <<= 32;
 808                       VFP_dword (dest) |= ARMul_LoadWordN (state, address + 4);
 809                     }
 810                   else
 811                     {
 812                       VFP_dword (dest) = ARMul_LoadWordN (state, address + 4);
 813                       VFP_dword (dest) <<= 32;
 814                       VFP_dword (dest) |= ARMul_LoadWordN (state, address);
 815                     }
 816
 817                   if (trace)
 818                     fprintf (stderr, " VFP: VLDM: D%d = %g\n", dest, VFP_dval (dest));
 819
 820                   address += 8;
 821                   dest += 1;
 822                 }
 823             }
 824           else
 825             {
 826               int dest = (BITS (12, 15) << 1) | BIT (22);
 827               while (imm8--)
 828                 {
 829                   VFP_uword (dest) = ARMul_LoadWordN (state, address);
 830                   address += 4;
 831                   dest += 1;
 832                 }
 833             }
 834         }
 835       return;
 836
 837     case 0x11:
 838     case 0x15:
 839     case 0x19:
 840     case 0x1D:
 841       {
 842         /* VLDR */
 843         ARMword imm32 = BITS (0, 7) << 2;
 844         int base = state->Reg[LHSReg];
 845         ARMword address;
 846         int dest;
 847
 848         if (LHSReg == 15)
 849           base = (base + 3) & ~3;
 850
 851         address = base + (BIT (23) ? imm32 : - imm32);
 852
 853         if (CPNum == 10)
 854           {
 855             dest = (DESTReg << 1) + BIT (22);
 856
 857             VFP_uword (dest) = ARMul_LoadWordN (state, address);
 858           }
 859         else
 860           {
 861             dest = (BIT (22) << 4) + DESTReg;
 862
 863             if (state->bigendSig)
 864               {
 865                 VFP_dword (dest) = ARMul_LoadWordN (state, address);
 866                 VFP_dword (dest) <<= 32;
 867                 VFP_dword (dest) |= ARMul_LoadWordN (state, address + 4);
 868               }
 869             else
 870               {
 871                 VFP_dword (dest) = ARMul_LoadWordN (state, address + 4);
 872                 VFP_dword (dest) <<= 32;
 873                 VFP_dword (dest) |= ARMul_LoadWordN (state, address);
 874               }
 875
 876             if (trace)
 877               fprintf (stderr, " VFP: VLDR: D%d = %g\n", dest, VFP_dval (dest));
 878           }
 879       }
 880       return;
 881     }
 882
 883   fprintf (stderr, "SIM: VFP: Unimplemented: %0x\n", BITS (20, 24));
 884 }
 885
 886 /* This function does the work of generating the addresses used in an
 887    LDC instruction.  The code here is always post-indexed, it's up to the
 888    caller to get the input address correct and to handle base register
 889    modification. It also handles the Busy-Waiting.  */
 890
 891 void
 892 ARMul_LDC (ARMul_State * state, ARMword instr, ARMword address)
 893 {
 894   unsigned cpab;
 895   ARMword data;
 896
 897   if (CPNum == 10 || CPNum == 11)
 898     {
 899       handle_VFP_xfer (state, instr);
 900       return;
 901     }
 902
 903   UNDEF_LSCPCBaseWb;
 904
 905   if (! CP_ACCESS_ALLOWED (state, CPNum))
 906     {
 907       ARMul_UndefInstr (state, instr);
 908       return;
 909     }
 910
 911   if (ADDREXCEPT (address))
 912     INTERNALABORT (address);
 913
 914   cpab = (state->LDC[CPNum]) (state, ARMul_FIRST, instr, 0);
 915   while (cpab == ARMul_BUSY)
 916     {
 917       ARMul_Icycles (state, 1, 0);
 918
 919       if (IntPending (state))
 920         {
 921           cpab = (state->LDC[CPNum]) (state, ARMul_INTERRUPT, instr, 0);
 922           return;
 923         }
 924       else
 925         cpab = (state->LDC[CPNum]) (state, ARMul_BUSY, instr, 0);
 926     }
 927   if (cpab == ARMul_CANT)
 928     {
 929       CPTAKEABORT;
 930       return;
 931     }
 932
 933   cpab = (state->LDC[CPNum]) (state, ARMul_TRANSFER, instr, 0);
 934   data = ARMul_LoadWordN (state, address);
 935   BUSUSEDINCPCN;
 936
 937   if (BIT (21))
 938     LSBase = state->Base;
 939   cpab = (state->LDC[CPNum]) (state, ARMul_DATA, instr, data);
 940
 941   while (cpab == ARMul_INC)
 942     {
 943       address += 4;
 944       data = ARMul_LoadWordN (state, address);
 945       cpab = (state->LDC[CPNum]) (state, ARMul_DATA, instr, data);
 946     }
 947
 948   if (state->abortSig || state->Aborted)
 949     TAKEABORT;
 950 }
 951
 952 /* This function does the work of generating the addresses used in an
 953    STC instruction.  The code here is always post-indexed, it's up to the
 954    caller to get the input address correct and to handle base register
 955    modification. It also handles the Busy-Waiting.  */
 956
 957 void
 958 ARMul_STC (ARMul_State * state, ARMword instr, ARMword address)
 959 {
 960   unsigned cpab;
 961   ARMword data;
 962
 963   if (CPNum == 10 || CPNum == 11)
 964     {
 965       handle_VFP_xfer (state, instr);
 966       return;
 967     }
 968
 969   UNDEF_LSCPCBaseWb;
 970
 971   if (! CP_ACCESS_ALLOWED (state, CPNum))
 972     {
 973       ARMul_UndefInstr (state, instr);
 974       return;
 975     }
 976
 977   if (ADDREXCEPT (address) || VECTORACCESS (address))
 978     INTERNALABORT (address);
 979
 980   cpab = (state->STC[CPNum]) (state, ARMul_FIRST, instr, &data);
 981   while (cpab == ARMul_BUSY)
 982     {
 983       ARMul_Icycles (state, 1, 0);
 984       if (IntPending (state))
 985         {
 986           cpab = (state->STC[CPNum]) (state, ARMul_INTERRUPT, instr, 0);
 987           return;
 988         }
 989       else
 990         cpab = (state->STC[CPNum]) (state, ARMul_BUSY, instr, &data);
 991     }
 992
 993   if (cpab == ARMul_CANT)
 994     {
 995       CPTAKEABORT;
 996       return;
 997     }
 998 #ifndef MODE32
 999   if (ADDREXCEPT (address) || VECTORACCESS (address))
1000     INTERNALABORT (address);
1001 #endif
1002   BUSUSEDINCPCN;
1003   if (BIT (21))
1004     LSBase = state->Base;
1005   cpab = (state->STC[CPNum]) (state, ARMul_DATA, instr, &data);
1006   ARMul_StoreWordN (state, address, data);
1007
1008   while (cpab == ARMul_INC)
1009     {
1010       address += 4;
1011       cpab = (state->STC[CPNum]) (state, ARMul_DATA, instr, &data);
1012       ARMul_StoreWordN (state, address, data);
1013     }
1014
1015   if (state->abortSig || state->Aborted)
1016     TAKEABORT;
1017 }
1018
1019 /* This function does the Busy-Waiting for an MCR instruction.  */
1020
1021 void
1022 ARMul_MCR (ARMul_State * state, ARMword instr, ARMword source)
1023 {
1024   unsigned cpab;
1025
1026   if (! CP_ACCESS_ALLOWED (state, CPNum))
1027     {
1028       ARMul_UndefInstr (state, instr);
1029       return;
1030     }
1031
1032   cpab = (state->MCR[CPNum]) (state, ARMul_FIRST, instr, source);
1033
1034   while (cpab == ARMul_BUSY)
1035     {
1036       ARMul_Icycles (state, 1, 0);
1037
1038       if (IntPending (state))
1039         {
1040           cpab = (state->MCR[CPNum]) (state, ARMul_INTERRUPT, instr, 0);
1041           return;
1042         }
1043       else
1044         cpab = (state->MCR[CPNum]) (state, ARMul_BUSY, instr, source);
1045     }
1046
1047   if (cpab == ARMul_CANT)
1048     ARMul_Abort (state, ARMul_UndefinedInstrV);
1049   else
1050     {
1051       BUSUSEDINCPCN;
1052       ARMul_Ccycles (state, 1, 0);
1053     }
1054 }
1055
1056 /* This function does the Busy-Waiting for an MRC instruction.  */
1057
1058 ARMword
1059 ARMul_MRC (ARMul_State * state, ARMword instr)
1060 {
1061   unsigned cpab;
1062   ARMword result = 0;
1063
1064   if (! CP_ACCESS_ALLOWED (state, CPNum))
1065     {
1066       ARMul_UndefInstr (state, instr);
1067       return result;
1068     }
1069
1070   cpab = (state->MRC[CPNum]) (state, ARMul_FIRST, instr, &result);
1071   while (cpab == ARMul_BUSY)
1072     {
1073       ARMul_Icycles (state, 1, 0);
1074       if (IntPending (state))
1075         {
1076           cpab = (state->MRC[CPNum]) (state, ARMul_INTERRUPT, instr, 0);
1077           return (0);
1078         }
1079       else
1080         cpab = (state->MRC[CPNum]) (state, ARMul_BUSY, instr, &result);
1081     }
1082   if (cpab == ARMul_CANT)
1083     {
1084       ARMul_Abort (state, ARMul_UndefinedInstrV);
1085       /* Parent will destroy the flags otherwise.  */
1086       result = ECC;
1087     }
1088   else
1089     {
1090       BUSUSEDINCPCN;
1091       ARMul_Ccycles (state, 1, 0);
1092       ARMul_Icycles (state, 1, 0);
1093     }
1094
1095   return result;
1096 }
1097
1098 static void
1099 handle_VFP_op (ARMul_State * state, ARMword instr)
1100 {
1101   int dest;
1102   int srcN;
1103   int srcM;
1104
1105   if (BITS (9, 11) != 0x5 || BIT (4) != 0)
1106     {
1107       fprintf (stderr, "SIM: VFP: Unimplemented: Float op: %08x\n", BITS (0,31));
1108       return;
1109     }
1110
1111   if (BIT (8))
1112     {
1113       dest = BITS(12,15) + (BIT (22) << 4);
1114       srcN = LHSReg  + (BIT (7) << 4);
1115       srcM = BITS (0,3) + (BIT (5) << 4);
1116     }
1117   else
1118     {
1119       dest = (BITS(12,15) << 1) + BIT (22);
1120       srcN = (LHSReg << 1) + BIT (7);
1121       srcM = (BITS (0,3) << 1) + BIT (5);
1122     }
1123
1124   switch (BITS (20, 27))
1125     {
1126     case 0xE0:
1127     case 0xE4:
1128       /* VMLA VMLS */
1129       if (BIT (8))
1130         {
1131           ARMdval val = VFP_dval (srcN) * VFP_dval (srcM);
1132
1133           if (BIT (6))
1134             {
1135               if (trace)
1136                 fprintf (stderr, " VFP: VMLS: %g = %g - %g * %g\n",
1137                          VFP_dval (dest) - val,
1138                          VFP_dval (dest), VFP_dval (srcN), VFP_dval (srcM));
1139               VFP_dval (dest) -= val;
1140             }
1141           else
1142             {
1143               if (trace)
1144                 fprintf (stderr, " VFP: VMLA: %g = %g + %g * %g\n",
1145                          VFP_dval (dest) + val,
1146                          VFP_dval (dest), VFP_dval (srcN), VFP_dval (srcM));
1147               VFP_dval (dest) += val;
1148             }
1149         }
1150       else
1151         {
1152           ARMfval val = VFP_fval (srcN) * VFP_fval (srcM);
1153
1154           if (BIT (6))
1155             {
1156               if (trace)
1157                 fprintf (stderr, " VFP: VMLS: %g = %g - %g * %g\n",
1158                          VFP_fval (dest) - val,
1159                          VFP_fval (dest), VFP_fval (srcN), VFP_fval (srcM));
1160               VFP_fval (dest) -= val;
1161             }
1162           else
1163             {
1164               if (trace)
1165                 fprintf (stderr, " VFP: VMLA: %g = %g + %g * %g\n",
1166                          VFP_fval (dest) + val,
1167                          VFP_fval (dest), VFP_fval (srcN), VFP_fval (srcM));
1168               VFP_fval (dest) += val;
1169             }
1170         }
1171       return;
1172
1173     case 0xE1:
1174     case 0xE5:
1175       if (BIT (8))
1176         {
1177           ARMdval product = VFP_dval (srcN) * VFP_dval (srcM);
1178
1179           if (BIT (6))
1180             {
1181               /* VNMLA */
1182               if (trace)
1183                 fprintf (stderr, " VFP: VNMLA: %g = -(%g + (%g * %g))\n",
1184                          -(VFP_dval (dest) + product),
1185                          VFP_dval (dest), VFP_dval (srcN), VFP_dval (srcM));
1186               VFP_dval (dest) = -(product + VFP_dval (dest));
1187             }
1188           else
1189             {
1190               /* VNMLS */
1191               if (trace)
1192                 fprintf (stderr, " VFP: VNMLS: %g = -(%g + (%g * %g))\n",
1193                          -(VFP_dval (dest) + product),
1194                          VFP_dval (dest), VFP_dval (srcN), VFP_dval (srcM));
1195               VFP_dval (dest) = product - VFP_dval (dest);
1196             }
1197         }
1198       else
1199         {
1200           ARMfval product = VFP_fval (srcN) * VFP_fval (srcM);
1201
1202           if (BIT (6))
1203             /* VNMLA */
1204             VFP_fval (dest) = -(product + VFP_fval (dest));
1205           else
1206             /* VNMLS */
1207             VFP_fval (dest) = product - VFP_fval (dest);
1208         }
1209       return;
1210
1211     case 0xE2:
1212     case 0xE6:
1213       if (BIT (8))
1214         {
1215           ARMdval product = VFP_dval (srcN) * VFP_dval (srcM);
1216
1217           if (BIT (6))
1218             {
1219               if (trace)
1220                 fprintf (stderr, " VFP: VMUL: %g = %g * %g\n",
1221                          - product, VFP_dval (srcN), VFP_dval (srcM));
1222               /* VNMUL */
1223               VFP_dval (dest) = - product;
1224             }
1225           else
1226             {
1227               if (trace)
1228                 fprintf (stderr, " VFP: VMUL: %g = %g * %g\n",
1229                          product, VFP_dval (srcN), VFP_dval (srcM));
1230               /* VMUL */
1231               VFP_dval (dest) = product;
1232             }
1233         }
1234       else
1235         {
1236           ARMfval product = VFP_fval (srcN) * VFP_fval (srcM);
1237
1238           if (BIT (6))
1239             {
1240               if (trace)
1241                 fprintf (stderr, " VFP: VNMUL: %g = %g * %g\n",
1242                          - product, VFP_fval (srcN), VFP_fval (srcM));
1243
1244               VFP_fval (dest) = - product;
1245             }
1246           else
1247             {
1248               if (trace)
1249                 fprintf (stderr, " VFP: VMUL: %g = %g * %g\n",
1250                          product, VFP_fval (srcN), VFP_fval (srcM));
1251
1252               VFP_fval (dest) = product;
1253             }
1254         }
1255       return;
1256
1257     case 0xE3:
1258     case 0xE7:
1259       if (BIT (6) == 0)
1260         {
1261           /* VADD */
1262           if (BIT(8))
1263             {
1264               if (trace)
1265                 fprintf (stderr, " VFP: VADD %g = %g + %g\n",
1266                          VFP_dval (srcN) + VFP_dval (srcM),
1267                          VFP_dval (srcN),
1268                          VFP_dval (srcM));
1269               VFP_dval (dest) = VFP_dval (srcN) + VFP_dval (srcM);
1270             }
1271           else
1272             VFP_fval (dest) = VFP_fval (srcN) + VFP_fval (srcM);
1273
1274         }
1275       else
1276         {
1277           /* VSUB */
1278           if (BIT(8))
1279             {
1280               if (trace)
1281                 fprintf (stderr, " VFP: VSUB %g = %g - %g\n",
1282                          VFP_dval (srcN) - VFP_dval (srcM),
1283                          VFP_dval (srcN),
1284                          VFP_dval (srcM));
1285               VFP_dval (dest) = VFP_dval (srcN) - VFP_dval (srcM);
1286             }
1287           else
1288             VFP_fval (dest) = VFP_fval (srcN) - VFP_fval (srcM);
1289         }
1290       return;
1291
1292     case 0xE8:
1293     case 0xEC:
1294       if (BIT (6) == 1)
1295         break;
1296
1297       /* VDIV */
1298       if (BIT (8))
1299         {
1300           ARMdval res = VFP_dval (srcN) / VFP_dval (srcM);
1301           if (trace)
1302             fprintf (stderr, " VFP: VDIV (64bit): %g = %g / %g\n",
1303                      res, VFP_dval (srcN), VFP_dval (srcM));
1304           VFP_dval (dest) = res;
1305         }
1306       else
1307         {
1308           if (trace)
1309             fprintf (stderr, " VFP: VDIV: %g = %g / %g\n",
1310                      VFP_fval (srcN) / VFP_fval (srcM),
1311                      VFP_fval (srcN), VFP_fval (srcM));
1312
1313           VFP_fval (dest) = VFP_fval (srcN) / VFP_fval (srcM);
1314         }
1315       return;
1316
1317     case 0xEB:
1318     case 0xEF:
1319       if (BIT (6) != 1)
1320         break;
1321
1322       switch (BITS (16, 19))
1323         {
1324         case 0x0:
1325           if (BIT (7) == 0)
1326             {
1327               if (BIT (8))
1328                 {
1329                   /* VMOV.F64 <Dd>, <Dm>.  */
1330                   VFP_dval (dest) = VFP_dval (srcM);
1331                   if (trace)
1332                     fprintf (stderr, " VFP: VMOV d%d, d%d: %g\n", dest, srcM, VFP_dval (srcM));
1333                 }
1334               else
1335                 {
1336                   /* VMOV.F32 <Sd>, <Sm>.  */
1337                   VFP_fval (dest) = VFP_fval (srcM);
1338                   if (trace)
1339                     fprintf (stderr, " VFP: VMOV s%d, s%d: %g\n", dest, srcM, VFP_fval (srcM));
1340                 }
1341             }
1342           else
1343             {
1344               /* VABS */
1345               if (BIT (8))
1346                 {
1347                   ARMdval src = VFP_dval (srcM);
1348
1349                   VFP_dval (dest) = fabs (src);
1350                   if (trace)
1351                     fprintf (stderr, " VFP: VABS (%g) = %g\n", src, VFP_dval (dest));
1352                 }
1353               else
1354                 {
1355                   ARMfval src = VFP_fval (srcM);
1356
1357                   VFP_fval (dest) = fabsf (src);
1358                   if (trace)
1359                     fprintf (stderr, " VFP: VABS (%g) = %g\n", src, VFP_fval (dest));
1360                 }
1361             }
1362           return;
1363
1364         case 0x1:
1365           if (BIT (7) == 0)
1366             {
1367               /* VNEG */
1368               if (BIT (8))
1369                 VFP_dval (dest) = - VFP_dval (srcM);
1370               else
1371                 VFP_fval (dest) = - VFP_fval (srcM);
1372             }
1373           else
1374             {
1375               /* VSQRT */
1376               if (BIT (8))
1377                 {
1378                   if (trace)
1379                     fprintf (stderr, " VFP: %g = root(%g)\n",
1380                              sqrt (VFP_dval (srcM)), VFP_dval (srcM));
1381
1382                   VFP_dval (dest) = sqrt (VFP_dval (srcM));
1383                 }
1384               else
1385                 {
1386                   if (trace)
1387                     fprintf (stderr, " VFP: %g = root(%g)\n",
1388                              sqrtf (VFP_fval (srcM)), VFP_fval (srcM));
1389
1390                   VFP_fval (dest) = sqrtf (VFP_fval (srcM));
1391                 }
1392             }
1393           return;
1394
1395         case 0x4:
1396         case 0x5:
1397           /* VCMP, VCMPE */
1398           if (BIT(8))
1399             {
1400               ARMdval res = VFP_dval (dest);
1401
1402               if (BIT (16) == 0)
1403                 {
1404                   ARMdval src = VFP_dval (srcM);
1405
1406                   if (isinf (res) && isinf (src))
1407                     {
1408                       if (res > 0.0 && src > 0.0)
1409                         res = 0.0;
1410                       else if (res < 0.0 && src < 0.0)
1411                         res = 0.0;
1412                       /* else leave res alone.   */
1413                     }
1414                   else
1415                     res -= src;
1416                 }
1417
1418               /* FIXME: Add handling of signalling NaNs and the E bit.  */
1419
1420               state->FPSCR &= 0x0FFFFFFF;
1421               if (res < 0.0)
1422                 state->FPSCR |= NBIT;
1423               else
1424                 state->FPSCR |= CBIT;
1425               if (res == 0.0)
1426                 state->FPSCR |= ZBIT;
1427               if (isnan (res))
1428                 state->FPSCR |= VBIT;
1429
1430               if (trace)
1431                 fprintf (stderr, " VFP: VCMP (64bit) %g vs %g res %g, flags: %c%c%c%c\n",
1432                          VFP_dval (dest), BIT (16) ? 0.0 : VFP_dval (srcM), res,
1433                          state->FPSCR & NBIT ? 'N' : '-',
1434                          state->FPSCR & ZBIT ? 'Z' : '-',
1435                          state->FPSCR & CBIT ? 'C' : '-',
1436                          state->FPSCR & VBIT ? 'V' : '-');
1437             }
1438           else
1439             {
1440               ARMfval res = VFP_fval (dest);
1441
1442               if (BIT (16) == 0)
1443                 {
1444                   ARMfval src = VFP_fval (srcM);
1445
1446                   if (isinf (res) && isinf (src))
1447                     {
1448                       if (res > 0.0 && src > 0.0)
1449                         res = 0.0;
1450                       else if (res < 0.0 && src < 0.0)
1451                         res = 0.0;
1452                       /* else leave res alone.   */
1453                     }
1454                   else
1455                     res -= src;
1456                 }
1457
1458               /* FIXME: Add handling of signalling NaNs and the E bit.  */
1459
1460               state->FPSCR &= 0x0FFFFFFF;
1461               if (res < 0.0)
1462                 state->FPSCR |= NBIT;
1463               else
1464                 state->FPSCR |= CBIT;
1465               if (res == 0.0)
1466                 state->FPSCR |= ZBIT;
1467               if (isnan (res))
1468                 state->FPSCR |= VBIT;
1469
1470               if (trace)
1471                 fprintf (stderr, " VFP: VCMP (32bit) %g vs %g res %g, flags: %c%c%c%c\n",
1472                          VFP_fval (dest), BIT (16) ? 0.0 : VFP_fval (srcM), res,
1473                          state->FPSCR & NBIT ? 'N' : '-',
1474                          state->FPSCR & ZBIT ? 'Z' : '-',
1475                          state->FPSCR & CBIT ? 'C' : '-',
1476                          state->FPSCR & VBIT ? 'V' : '-');
1477             }
1478           return;
1479
1480         case 0x7:
1481           if (BIT (8))
1482             {
1483               dest = (DESTReg << 1) + BIT (22);
1484               VFP_fval (dest) = VFP_dval (srcM);
1485             }
1486           else
1487             {
1488               dest = DESTReg + (BIT (22) << 4);
1489               VFP_dval (dest) = VFP_fval (srcM);
1490             }
1491           return;
1492
1493         case 0x8:
1494         case 0xC:
1495         case 0xD:
1496           /* VCVT integer <-> FP */
1497           if (BIT (18))
1498             {
1499               /* To integer.  */
1500               if (BIT (8))
1501                 {
1502                   dest = (BITS(12,15) << 1) + BIT (22);
1503                   if (BIT (16))
1504                     VFP_sword (dest) = VFP_dval (srcM);
1505                   else
1506                     VFP_uword (dest) = VFP_dval (srcM);
1507                 }
1508               else
1509                 {
1510                   if (BIT (16))
1511                     VFP_sword (dest) = VFP_fval (srcM);
1512                   else
1513                     VFP_uword (dest) = VFP_fval (srcM);
1514                 }
1515             }
1516           else
1517             {
1518               /* From integer.  */
1519               if (BIT (8))
1520                 {
1521                   srcM = (BITS (0,3) << 1) + BIT (5);
1522                   if (BIT (7))
1523                     VFP_dval (dest) = VFP_sword (srcM);
1524                   else
1525                     VFP_dval (dest) = VFP_uword (srcM);
1526                 }
1527               else
1528                 {
1529                   if (BIT (7))
1530                     VFP_fval (dest) = VFP_sword (srcM);
1531                   else
1532                     VFP_fval (dest) = VFP_uword (srcM);
1533                 }
1534             }
1535           return;
1536         }
1537
1538       fprintf (stderr, "SIM: VFP: Unimplemented: Float op3: %03x\n", BITS (16,27));
1539       return;
1540     }
1541
1542   fprintf (stderr, "SIM: VFP: Unimplemented: Float op2: %02x\n", BITS (20, 27));
1543   return;
1544 }
1545
1546 /* This function does the Busy-Waiting for an CDP instruction.  */
1547
1548 void
1549 ARMul_CDP (ARMul_State * state, ARMword instr)
1550 {
1551   unsigned cpab;
1552
1553   if (CPNum == 10 || CPNum == 11)
1554     {
1555       handle_VFP_op (state, instr);
1556       return;
1557     }
1558
1559   if (! CP_ACCESS_ALLOWED (state, CPNum))
1560     {
1561       ARMul_UndefInstr (state, instr);
1562       return;
1563     }
1564
1565   cpab = (state->CDP[CPNum]) (state, ARMul_FIRST, instr);
1566   while (cpab == ARMul_BUSY)
1567     {
1568       ARMul_Icycles (state, 1, 0);
1569       if (IntPending (state))
1570         {
1571           cpab = (state->CDP[CPNum]) (state, ARMul_INTERRUPT, instr);
1572           return;
1573         }
1574       else
1575         cpab = (state->CDP[CPNum]) (state, ARMul_BUSY, instr);
1576     }
1577   if (cpab == ARMul_CANT)
1578     ARMul_Abort (state, ARMul_UndefinedInstrV);
1579   else
1580     BUSUSEDN;
1581 }
1582
1583 /* This function handles Undefined instructions, as CP isntruction.  */
1584
1585 void
1586 ARMul_UndefInstr (ARMul_State * state, ARMword instr ATTRIBUTE_UNUSED)
1587 {
1588   ARMul_Abort (state, ARMul_UndefinedInstrV);
1589 }
1590
1591 /* Return TRUE if an interrupt is pending, FALSE otherwise.  */
1592
1593 unsigned
1594 IntPending (ARMul_State * state)
1595 {
1596   if (state->Exception)
1597     {
1598       /* Any exceptions.  */
1599       if (state->NresetSig == LOW)
1600         {
1601           ARMul_Abort (state, ARMul_ResetV);
1602           return TRUE;
1603         }
1604       else if (!state->NfiqSig && !FFLAG)
1605         {
1606           ARMul_Abort (state, ARMul_FIQV);
1607           return TRUE;
1608         }
1609       else if (!state->NirqSig && !IFLAG)
1610         {
1611           ARMul_Abort (state, ARMul_IRQV);
1612           return TRUE;
1613         }
1614     }
1615
1616   return FALSE;
1617 }
1618
1619 /* Align a word access to a non word boundary.  */
1620
1621 ARMword
1622 ARMul_Align (ARMul_State *state ATTRIBUTE_UNUSED, ARMword address, ARMword data)
1623 {
1624   /* This code assumes the address is really unaligned,
1625      as a shift by 32 is undefined in C.  */
1626
1627   address = (address & 3) << 3; /* Get the word address.  */
1628   return ((data >> address) | (data << (32 - address)));        /* rot right */
1629 }
1630
1631 /* This routine is used to call another routine after a certain number of
1632    cycles have been executed. The first parameter is the number of cycles
1633    delay before the function is called, the second argument is a pointer
1634    to the function. A delay of zero doesn't work, just call the function.  */
1635
1636 void
1637 ARMul_ScheduleEvent (ARMul_State * state, unsigned long delay,
1638                      unsigned (*what) (ARMul_State *))
1639 {
1640   unsigned long when;
1641   struct EventNode *event;
1642
1643   if (state->EventSet++ == 0)
1644     state->Now = ARMul_Time (state);
1645   when = (state->Now + delay) % EVENTLISTSIZE;
1646   event = (struct EventNode *) malloc (sizeof (struct EventNode));
1647   event->func = what;
1648   event->next = *(state->EventPtr + when);
1649   *(state->EventPtr + when) = event;
1650 }
1651
1652 /* This routine is called at the beginning of
1653    every cycle, to envoke scheduled events.  */
1654
1655 void
1656 ARMul_EnvokeEvent (ARMul_State * state)
1657 {
1658   static unsigned long then;
1659
1660   then = state->Now;
1661   state->Now = ARMul_Time (state) % EVENTLISTSIZE;
1662   if (then < state->Now)
1663     /* Schedule events.  */
1664     EnvokeList (state, then, state->Now);
1665   else if (then > state->Now)
1666     {
1667       /* Need to wrap around the list.  */
1668       EnvokeList (state, then, EVENTLISTSIZE - 1L);
1669       EnvokeList (state, 0L, state->Now);
1670     }
1671 }
1672
1673 /* Envokes all the entries in a range.  */
1674
1675 static void
1676 EnvokeList (ARMul_State * state, unsigned long from, unsigned long to)
1677 {
1678   for (; from <= to; from++)
1679     {
1680       struct EventNode *anevent;
1681
1682       anevent = *(state->EventPtr + from);
1683       while (anevent)
1684         {
1685           (anevent->func) (state);
1686           state->EventSet--;
1687           anevent = anevent->next;
1688         }
1689       *(state->EventPtr + from) = NULL;
1690     }
1691 }
1692
1693 /* This routine is returns the number of clock ticks since the last reset.  */
1694
1695 unsigned long
1696 ARMul_Time (ARMul_State * state)
1697 {
1698   return (state->NumScycles + state->NumNcycles +
1699           state->NumIcycles + state->NumCcycles + state->NumFcycles);
1700 }