target-arm/helper.c

   1 #include "cpu.h"
   2 #include "gdbstub.h"
   3 #include "helper.h"
   4 #include "host-utils.h"
   5 #include "sysemu.h"
   6
   7 void cpu_state_reset(CPUARMState *env)
   8 {
   9     cpu_reset(ENV_GET_CPU(env));
  10 }
  11
  12 static int vfp_gdb_get_reg(CPUARMState *env, uint8_t *buf, int reg)
  13 {
  14     int nregs;
  15
  16     /* VFP data registers are always little-endian.  */
  17     nregs = arm_feature(env, ARM_FEATURE_VFP3) ? 32 : 16;
  18     if (reg < nregs) {
  19         stfq_le_p(buf, env->vfp.regs[reg]);
  20         return 8;
  21     }
  22     if (arm_feature(env, ARM_FEATURE_NEON)) {
  23         /* Aliases for Q regs.  */
  24         nregs += 16;
  25         if (reg < nregs) {
  26             stfq_le_p(buf, env->vfp.regs[(reg - 32) * 2]);
  27             stfq_le_p(buf + 8, env->vfp.regs[(reg - 32) * 2 + 1]);
  28             return 16;
  29         }
  30     }
  31     switch (reg - nregs) {
  32     case 0: stl_p(buf, env->vfp.xregs[ARM_VFP_FPSID]); return 4;
  33     case 1: stl_p(buf, env->vfp.xregs[ARM_VFP_FPSCR]); return 4;
  34     case 2: stl_p(buf, env->vfp.xregs[ARM_VFP_FPEXC]); return 4;
  35     }
  36     return 0;
  37 }
  38
  39 static int vfp_gdb_set_reg(CPUARMState *env, uint8_t *buf, int reg)
  40 {
  41     int nregs;
  42
  43     nregs = arm_feature(env, ARM_FEATURE_VFP3) ? 32 : 16;
  44     if (reg < nregs) {
  45         env->vfp.regs[reg] = ldfq_le_p(buf);
  46         return 8;
  47     }
  48     if (arm_feature(env, ARM_FEATURE_NEON)) {
  49         nregs += 16;
  50         if (reg < nregs) {
  51             env->vfp.regs[(reg - 32) * 2] = ldfq_le_p(buf);
  52             env->vfp.regs[(reg - 32) * 2 + 1] = ldfq_le_p(buf + 8);
  53             return 16;
  54         }
  55     }
  56     switch (reg - nregs) {
  57     case 0: env->vfp.xregs[ARM_VFP_FPSID] = ldl_p(buf); return 4;
  58     case 1: env->vfp.xregs[ARM_VFP_FPSCR] = ldl_p(buf); return 4;
  59     case 2: env->vfp.xregs[ARM_VFP_FPEXC] = ldl_p(buf) & (1 << 30); return 4;
  60     }
  61     return 0;
  62 }
  63
  64 CPUARMState *cpu_arm_init(const char *cpu_model)
  65 {
  66     ARMCPU *cpu;
  67     CPUARMState *env;
  68     static int inited = 0;
  69
  70     if (!object_class_by_name(cpu_model)) {
  71         return NULL;
  72     }
  73     cpu = ARM_CPU(object_new(cpu_model));
  74     env = &cpu->env;
  75     env->cpu_model_str = cpu_model;
  76     arm_cpu_realize(cpu);
  77
  78     if (tcg_enabled() && !inited) {
  79         inited = 1;
  80         arm_translate_init();
  81     }
  82
  83     cpu_state_reset(env);
  84     if (arm_feature(env, ARM_FEATURE_NEON)) {
  85         gdb_register_coprocessor(env, vfp_gdb_get_reg, vfp_gdb_set_reg,
  86                                  51, "arm-neon.xml", 0);
  87     } else if (arm_feature(env, ARM_FEATURE_VFP3)) {
  88         gdb_register_coprocessor(env, vfp_gdb_get_reg, vfp_gdb_set_reg,
  89                                  35, "arm-vfp3.xml", 0);
  90     } else if (arm_feature(env, ARM_FEATURE_VFP)) {
  91         gdb_register_coprocessor(env, vfp_gdb_get_reg, vfp_gdb_set_reg,
  92                                  19, "arm-vfp.xml", 0);
  93     }
  94     qemu_init_vcpu(env);
  95     return env;
  96 }
  97
  98 typedef struct ARMCPUListState {
  99     fprintf_function cpu_fprintf;
 100     FILE *file;
 101 } ARMCPUListState;
 102
 103 /* Sort alphabetically by type name, except for "any". */
 104 static gint arm_cpu_list_compare(gconstpointer a, gconstpointer b)
 105 {
 106     ObjectClass *class_a = (ObjectClass *)a;
 107     ObjectClass *class_b = (ObjectClass *)b;
 108     const char *name_a, *name_b;
 109
 110     name_a = object_class_get_name(class_a);
 111     name_b = object_class_get_name(class_b);
 112     if (strcmp(name_a, "any") == 0) {
 113         return 1;
 114     } else if (strcmp(name_b, "any") == 0) {
 115         return -1;
 116     } else {
 117         return strcmp(name_a, name_b);
 118     }
 119 }
 120
 121 static void arm_cpu_list_entry(gpointer data, gpointer user_data)
 122 {
 123     ObjectClass *oc = data;
 124     ARMCPUListState *s = user_data;
 125
 126     (*s->cpu_fprintf)(s->file, "  %s\n",
 127                       object_class_get_name(oc));
 128 }
 129
 130 void arm_cpu_list(FILE *f, fprintf_function cpu_fprintf)
 131 {
 132     ARMCPUListState s = {
 133         .file = f,
 134         .cpu_fprintf = cpu_fprintf,
 135     };
 136     GSList *list;
 137
 138     list = object_class_get_list(TYPE_ARM_CPU, false);
 139     list = g_slist_sort(list, arm_cpu_list_compare);
 140     (*cpu_fprintf)(f, "Available CPUs:\n");
 141     g_slist_foreach(list, arm_cpu_list_entry, &s);
 142     g_slist_free(list);
 143 }
 144
 145 static int bad_mode_switch(CPUARMState *env, int mode)
 146 {
 147     /* Return true if it is not valid for us to switch to
 148      * this CPU mode (ie all the UNPREDICTABLE cases in
 149      * the ARM ARM CPSRWriteByInstr pseudocode).
 150      */
 151     switch (mode) {
 152     case ARM_CPU_MODE_USR:
 153     case ARM_CPU_MODE_SYS:
 154     case ARM_CPU_MODE_SVC:
 155     case ARM_CPU_MODE_ABT:
 156     case ARM_CPU_MODE_UND:
 157     case ARM_CPU_MODE_IRQ:
 158     case ARM_CPU_MODE_FIQ:
 159         return 0;
 160     default:
 161         return 1;
 162     }
 163 }
 164
 165 uint32_t cpsr_read(CPUARMState *env)
 166 {
 167     int ZF;
 168     ZF = (env->ZF == 0);
 169     return env->uncached_cpsr | (env->NF & 0x80000000) | (ZF << 30) |
 170         (env->CF << 29) | ((env->VF & 0x80000000) >> 3) | (env->QF << 27)
 171         | (env->thumb << 5) | ((env->condexec_bits & 3) << 25)
 172         | ((env->condexec_bits & 0xfc) << 8)
 173         | (env->GE << 16);
 174 }
 175
 176 void cpsr_write(CPUARMState *env, uint32_t val, uint32_t mask)
 177 {
 178     if (mask & CPSR_NZCV) {
 179         env->ZF = (~val) & CPSR_Z;
 180         env->NF = val;
 181         env->CF = (val >> 29) & 1;
 182         env->VF = (val << 3) & 0x80000000;
 183     }
 184     if (mask & CPSR_Q)
 185         env->QF = ((val & CPSR_Q) != 0);
 186     if (mask & CPSR_T)
 187         env->thumb = ((val & CPSR_T) != 0);
 188     if (mask & CPSR_IT_0_1) {
 189         env->condexec_bits &= ~3;
 190         env->condexec_bits |= (val >> 25) & 3;
 191     }
 192     if (mask & CPSR_IT_2_7) {
 193         env->condexec_bits &= 3;
 194         env->condexec_bits |= (val >> 8) & 0xfc;
 195     }
 196     if (mask & CPSR_GE) {
 197         env->GE = (val >> 16) & 0xf;
 198     }
 199
 200     if ((env->uncached_cpsr ^ val) & mask & CPSR_M) {
 201         if (bad_mode_switch(env, val & CPSR_M)) {
 202             /* Attempt to switch to an invalid mode: this is UNPREDICTABLE.
 203              * We choose to ignore the attempt and leave the CPSR M field
 204              * untouched.
 205              */
 206             mask &= ~CPSR_M;
 207         } else {
 208             switch_mode(env, val & CPSR_M);
 209         }
 210     }
 211     mask &= ~CACHED_CPSR_BITS;
 212     env->uncached_cpsr = (env->uncached_cpsr & ~mask) | (val & mask);
 213 }
 214
 215 /* Sign/zero extend */
 216 uint32_t HELPER(sxtb16)(uint32_t x)
 217 {
 218     uint32_t res;
 219     res = (uint16_t)(int8_t)x;
 220     res |= (uint32_t)(int8_t)(x >> 16) << 16;
 221     return res;
 222 }
 223
 224 uint32_t HELPER(uxtb16)(uint32_t x)
 225 {
 226     uint32_t res;
 227     res = (uint16_t)(uint8_t)x;
 228     res |= (uint32_t)(uint8_t)(x >> 16) << 16;
 229     return res;
 230 }
 231
 232 uint32_t HELPER(clz)(uint32_t x)
 233 {
 234     return clz32(x);
 235 }
 236
 237 int32_t HELPER(sdiv)(int32_t num, int32_t den)
 238 {
 239     if (den == 0)
 240       return 0;
 241     if (num == INT_MIN && den == -1)
 242       return INT_MIN;
 243     return num / den;
 244 }
 245
 246 uint32_t HELPER(udiv)(uint32_t num, uint32_t den)
 247 {
 248     if (den == 0)
 249       return 0;
 250     return num / den;
 251 }
 252
 253 uint32_t HELPER(rbit)(uint32_t x)
 254 {
 255     x =  ((x & 0xff000000) >> 24)
 256        | ((x & 0x00ff0000) >> 8)
 257        | ((x & 0x0000ff00) << 8)
 258        | ((x & 0x000000ff) << 24);
 259     x =  ((x & 0xf0f0f0f0) >> 4)
 260        | ((x & 0x0f0f0f0f) << 4);
 261     x =  ((x & 0x88888888) >> 3)
 262        | ((x & 0x44444444) >> 1)
 263        | ((x & 0x22222222) << 1)
 264        | ((x & 0x11111111) << 3);
 265     return x;
 266 }
 267
 268 uint32_t HELPER(abs)(uint32_t x)
 269 {
 270     return ((int32_t)x < 0) ? -x : x;
 271 }
 272
 273 #if defined(CONFIG_USER_ONLY)
 274
 275 void do_interrupt (CPUARMState *env)
 276 {
 277     env->exception_index = -1;
 278 }
 279
 280 int cpu_arm_handle_mmu_fault (CPUARMState *env, target_ulong address, int rw,
 281                               int mmu_idx)
 282 {
 283     if (rw == 2) {
 284         env->exception_index = EXCP_PREFETCH_ABORT;
 285         env->cp15.c6_insn = address;
 286     } else {
 287         env->exception_index = EXCP_DATA_ABORT;
 288         env->cp15.c6_data = address;
 289     }
 290     return 1;
 291 }
 292
 293 /* These should probably raise undefined insn exceptions.  */
 294 void HELPER(set_cp)(CPUARMState *env, uint32_t insn, uint32_t val)
 295 {
 296     int op1 = (insn >> 8) & 0xf;
 297     cpu_abort(env, "cp%i insn %08x\n", op1, insn);
 298     return;
 299 }
 300
 301 uint32_t HELPER(get_cp)(CPUARMState *env, uint32_t insn)
 302 {
 303     int op1 = (insn >> 8) & 0xf;
 304     cpu_abort(env, "cp%i insn %08x\n", op1, insn);
 305     return 0;
 306 }
 307
 308 void HELPER(set_cp15)(CPUARMState *env, uint32_t insn, uint32_t val)
 309 {
 310     cpu_abort(env, "cp15 insn %08x\n", insn);
 311 }
 312
 313 uint32_t HELPER(get_cp15)(CPUARMState *env, uint32_t insn)
 314 {
 315     cpu_abort(env, "cp15 insn %08x\n", insn);
 316 }
 317
 318 /* These should probably raise undefined insn exceptions.  */
 319 void HELPER(v7m_msr)(CPUARMState *env, uint32_t reg, uint32_t val)
 320 {
 321     cpu_abort(env, "v7m_mrs %d\n", reg);
 322 }
 323
 324 uint32_t HELPER(v7m_mrs)(CPUARMState *env, uint32_t reg)
 325 {
 326     cpu_abort(env, "v7m_mrs %d\n", reg);
 327     return 0;
 328 }
 329
 330 void switch_mode(CPUARMState *env, int mode)
 331 {
 332     if (mode != ARM_CPU_MODE_USR)
 333         cpu_abort(env, "Tried to switch out of user mode\n");
 334 }
 335
 336 void HELPER(set_r13_banked)(CPUARMState *env, uint32_t mode, uint32_t val)
 337 {
 338     cpu_abort(env, "banked r13 write\n");
 339 }
 340
 341 uint32_t HELPER(get_r13_banked)(CPUARMState *env, uint32_t mode)
 342 {
 343     cpu_abort(env, "banked r13 read\n");
 344     return 0;
 345 }
 346
 347 #else
 348
 349 /* Map CPU modes onto saved register banks.  */
 350 static inline int bank_number(CPUARMState *env, int mode)
 351 {
 352     switch (mode) {
 353     case ARM_CPU_MODE_USR:
 354     case ARM_CPU_MODE_SYS:
 355         return 0;
 356     case ARM_CPU_MODE_SVC:
 357         return 1;
 358     case ARM_CPU_MODE_ABT:
 359         return 2;
 360     case ARM_CPU_MODE_UND:
 361         return 3;
 362     case ARM_CPU_MODE_IRQ:
 363         return 4;
 364     case ARM_CPU_MODE_FIQ:
 365         return 5;
 366     }
 367     cpu_abort(env, "Bad mode %x\n", mode);
 368     return -1;
 369 }
 370
 371 void switch_mode(CPUARMState *env, int mode)
 372 {
 373     int old_mode;
 374     int i;
 375
 376     old_mode = env->uncached_cpsr & CPSR_M;
 377     if (mode == old_mode)
 378         return;
 379
 380     if (old_mode == ARM_CPU_MODE_FIQ) {
 381         memcpy (env->fiq_regs, env->regs + 8, 5 * sizeof(uint32_t));
 382         memcpy (env->regs + 8, env->usr_regs, 5 * sizeof(uint32_t));
 383     } else if (mode == ARM_CPU_MODE_FIQ) {
 384         memcpy (env->usr_regs, env->regs + 8, 5 * sizeof(uint32_t));
 385         memcpy (env->regs + 8, env->fiq_regs, 5 * sizeof(uint32_t));
 386     }
 387
 388     i = bank_number(env, old_mode);
 389     env->banked_r13[i] = env->regs[13];
 390     env->banked_r14[i] = env->regs[14];
 391     env->banked_spsr[i] = env->spsr;
 392
 393     i = bank_number(env, mode);
 394     env->regs[13] = env->banked_r13[i];
 395     env->regs[14] = env->banked_r14[i];
 396     env->spsr = env->banked_spsr[i];
 397 }
 398
 399 static void v7m_push(CPUARMState *env, uint32_t val)
 400 {
 401     env->regs[13] -= 4;
 402     stl_phys(env->regs[13], val);
 403 }
 404
 405 static uint32_t v7m_pop(CPUARMState *env)
 406 {
 407     uint32_t val;
 408     val = ldl_phys(env->regs[13]);
 409     env->regs[13] += 4;
 410     return val;
 411 }
 412
 413 /* Switch to V7M main or process stack pointer.  */
 414 static void switch_v7m_sp(CPUARMState *env, int process)
 415 {
 416     uint32_t tmp;
 417     if (env->v7m.current_sp != process) {
 418         tmp = env->v7m.other_sp;
 419         env->v7m.other_sp = env->regs[13];
 420         env->regs[13] = tmp;
 421         env->v7m.current_sp = process;
 422     }
 423 }
 424
 425 static void do_v7m_exception_exit(CPUARMState *env)
 426 {
 427     uint32_t type;
 428     uint32_t xpsr;
 429
 430     type = env->regs[15];
 431     if (env->v7m.exception != 0)
 432         armv7m_nvic_complete_irq(env->nvic, env->v7m.exception);
 433
 434     /* Switch to the target stack.  */
 435     switch_v7m_sp(env, (type & 4) != 0);
 436     /* Pop registers.  */
 437     env->regs[0] = v7m_pop(env);
 438     env->regs[1] = v7m_pop(env);
 439     env->regs[2] = v7m_pop(env);
 440     env->regs[3] = v7m_pop(env);
 441     env->regs[12] = v7m_pop(env);
 442     env->regs[14] = v7m_pop(env);
 443     env->regs[15] = v7m_pop(env);
 444     xpsr = v7m_pop(env);
 445     xpsr_write(env, xpsr, 0xfffffdff);
 446     /* Undo stack alignment.  */
 447     if (xpsr & 0x200)
 448         env->regs[13] |= 4;
 449     /* ??? The exception return type specifies Thread/Handler mode.  However
 450        this is also implied by the xPSR value. Not sure what to do
 451        if there is a mismatch.  */
 452     /* ??? Likewise for mismatches between the CONTROL register and the stack
 453        pointer.  */
 454 }
 455
 456 static void do_interrupt_v7m(CPUARMState *env)
 457 {
 458     uint32_t xpsr = xpsr_read(env);
 459     uint32_t lr;
 460     uint32_t addr;
 461
 462     lr = 0xfffffff1;
 463     if (env->v7m.current_sp)
 464         lr |= 4;
 465     if (env->v7m.exception == 0)
 466         lr |= 8;
 467
 468     /* For exceptions we just mark as pending on the NVIC, and let that
 469        handle it.  */
 470     /* TODO: Need to escalate if the current priority is higher than the
 471        one we're raising.  */
 472     switch (env->exception_index) {
 473     case EXCP_UDEF:
 474         armv7m_nvic_set_pending(env->nvic, ARMV7M_EXCP_USAGE);
 475         return;
 476     case EXCP_SWI:
 477         env->regs[15] += 2;
 478         armv7m_nvic_set_pending(env->nvic, ARMV7M_EXCP_SVC);
 479         return;
 480     case EXCP_PREFETCH_ABORT:
 481     case EXCP_DATA_ABORT:
 482         armv7m_nvic_set_pending(env->nvic, ARMV7M_EXCP_MEM);
 483         return;
 484     case EXCP_BKPT:
 485         if (semihosting_enabled) {
 486             int nr;
 487             nr = arm_lduw_code(env->regs[15], env->bswap_code) & 0xff;
 488             if (nr == 0xab) {
 489                 env->regs[15] += 2;
 490                 env->regs[0] = do_arm_semihosting(env);
 491                 return;
 492             }
 493         }
 494         armv7m_nvic_set_pending(env->nvic, ARMV7M_EXCP_DEBUG);
 495         return;
 496     case EXCP_IRQ:
 497         env->v7m.exception = armv7m_nvic_acknowledge_irq(env->nvic);
 498         break;
 499     case EXCP_EXCEPTION_EXIT:
 500         do_v7m_exception_exit(env);
 501         return;
 502     default:
 503         cpu_abort(env, "Unhandled exception 0x%x\n", env->exception_index);
 504         return; /* Never happens.  Keep compiler happy.  */
 505     }
 506
 507     /* Align stack pointer.  */
 508     /* ??? Should only do this if Configuration Control Register
 509        STACKALIGN bit is set.  */
 510     if (env->regs[13] & 4) {
 511         env->regs[13] -= 4;
 512         xpsr |= 0x200;
 513     }
 514     /* Switch to the handler mode.  */
 515     v7m_push(env, xpsr);
 516     v7m_push(env, env->regs[15]);
 517     v7m_push(env, env->regs[14]);
 518     v7m_push(env, env->regs[12]);
 519     v7m_push(env, env->regs[3]);
 520     v7m_push(env, env->regs[2]);
 521     v7m_push(env, env->regs[1]);
 522     v7m_push(env, env->regs[0]);
 523     switch_v7m_sp(env, 0);
 524     /* Clear IT bits */
 525     env->condexec_bits = 0;
 526     env->regs[14] = lr;
 527     addr = ldl_phys(env->v7m.vecbase + env->v7m.exception * 4);
 528     env->regs[15] = addr & 0xfffffffe;
 529     env->thumb = addr & 1;
 530 }
 531
 532 /* Handle a CPU exception.  */
 533 void do_interrupt(CPUARMState *env)
 534 {
 535     uint32_t addr;
 536     uint32_t mask;
 537     int new_mode;
 538     uint32_t offset;
 539
 540     if (IS_M(env)) {
 541         do_interrupt_v7m(env);
 542         return;
 543     }
 544     /* TODO: Vectored interrupt controller.  */
 545     switch (env->exception_index) {
 546     case EXCP_UDEF:
 547         new_mode = ARM_CPU_MODE_UND;
 548         addr = 0x04;
 549         mask = CPSR_I;
 550         if (env->thumb)
 551             offset = 2;
 552         else
 553             offset = 4;
 554         break;
 555     case EXCP_SWI:
 556         if (semihosting_enabled) {
 557             /* Check for semihosting interrupt.  */
 558             if (env->thumb) {
 559                 mask = arm_lduw_code(env->regs[15] - 2, env->bswap_code) & 0xff;
 560             } else {
 561                 mask = arm_ldl_code(env->regs[15] - 4, env->bswap_code)
 562                     & 0xffffff;
 563             }
 564             /* Only intercept calls from privileged modes, to provide some
 565                semblance of security.  */
 566             if (((mask == 0x123456 && !env->thumb)
 567                     || (mask == 0xab && env->thumb))
 568                   && (env->uncached_cpsr & CPSR_M) != ARM_CPU_MODE_USR) {
 569                 env->regs[0] = do_arm_semihosting(env);
 570                 return;
 571             }
 572         }
 573         new_mode = ARM_CPU_MODE_SVC;
 574         addr = 0x08;
 575         mask = CPSR_I;
 576         /* The PC already points to the next instruction.  */
 577         offset = 0;
 578         break;
 579     case EXCP_BKPT:
 580         /* See if this is a semihosting syscall.  */
 581         if (env->thumb && semihosting_enabled) {
 582             mask = arm_lduw_code(env->regs[15], env->bswap_code) & 0xff;
 583             if (mask == 0xab
 584                   && (env->uncached_cpsr & CPSR_M) != ARM_CPU_MODE_USR) {
 585                 env->regs[15] += 2;
 586                 env->regs[0] = do_arm_semihosting(env);
 587                 return;
 588             }
 589         }
 590         env->cp15.c5_insn = 2;
 591         /* Fall through to prefetch abort.  */
 592     case EXCP_PREFETCH_ABORT:
 593         new_mode = ARM_CPU_MODE_ABT;
 594         addr = 0x0c;
 595         mask = CPSR_A | CPSR_I;
 596         offset = 4;
 597         break;
 598     case EXCP_DATA_ABORT:
 599         new_mode = ARM_CPU_MODE_ABT;
 600         addr = 0x10;
 601         mask = CPSR_A | CPSR_I;
 602         offset = 8;
 603         break;
 604     case EXCP_IRQ:
 605         new_mode = ARM_CPU_MODE_IRQ;
 606         addr = 0x18;
 607         /* Disable IRQ and imprecise data aborts.  */
 608         mask = CPSR_A | CPSR_I;
 609         offset = 4;
 610         break;
 611     case EXCP_FIQ:
 612         new_mode = ARM_CPU_MODE_FIQ;
 613         addr = 0x1c;
 614         /* Disable FIQ, IRQ and imprecise data aborts.  */
 615         mask = CPSR_A | CPSR_I | CPSR_F;
 616         offset = 4;
 617         break;
 618     default:
 619         cpu_abort(env, "Unhandled exception 0x%x\n", env->exception_index);
 620         return; /* Never happens.  Keep compiler happy.  */
 621     }
 622     /* High vectors.  */
 623     if (env->cp15.c1_sys & (1 << 13)) {
 624         addr += 0xffff0000;
 625     }
 626     switch_mode (env, new_mode);
 627     env->spsr = cpsr_read(env);
 628     /* Clear IT bits.  */
 629     env->condexec_bits = 0;
 630     /* Switch to the new mode, and to the correct instruction set.  */
 631     env->uncached_cpsr = (env->uncached_cpsr & ~CPSR_M) | new_mode;
 632     env->uncached_cpsr |= mask;
 633     /* this is a lie, as the was no c1_sys on V4T/V5, but who cares
 634      * and we should just guard the thumb mode on V4 */
 635     if (arm_feature(env, ARM_FEATURE_V4T)) {
 636         env->thumb = (env->cp15.c1_sys & (1 << 30)) != 0;
 637     }
 638     env->regs[14] = env->regs[15] + offset;
 639     env->regs[15] = addr;
 640     env->interrupt_request |= CPU_INTERRUPT_EXITTB;
 641 }
 642
 643 /* Check section/page access permissions.
 644    Returns the page protection flags, or zero if the access is not
 645    permitted.  */
 646 static inline int check_ap(CPUARMState *env, int ap, int domain_prot,
 647                            int access_type, int is_user)
 648 {
 649   int prot_ro;
 650
 651   if (domain_prot == 3) {
 652     return PAGE_READ | PAGE_WRITE;
 653   }
 654
 655   if (access_type == 1)
 656       prot_ro = 0;
 657   else
 658       prot_ro = PAGE_READ;
 659
 660   switch (ap) {
 661   case 0:
 662       if (access_type == 1)
 663           return 0;
 664       switch ((env->cp15.c1_sys >> 8) & 3) {
 665       case 1:
 666           return is_user ? 0 : PAGE_READ;
 667       case 2:
 668           return PAGE_READ;
 669       default:
 670           return 0;
 671       }
 672   case 1:
 673       return is_user ? 0 : PAGE_READ | PAGE_WRITE;
 674   case 2:
 675       if (is_user)
 676           return prot_ro;
 677       else
 678           return PAGE_READ | PAGE_WRITE;
 679   case 3:
 680       return PAGE_READ | PAGE_WRITE;
 681   case 4: /* Reserved.  */
 682       return 0;
 683   case 5:
 684       return is_user ? 0 : prot_ro;
 685   case 6:
 686       return prot_ro;
 687   case 7:
 688       if (!arm_feature (env, ARM_FEATURE_V6K))
 689           return 0;
 690       return prot_ro;
 691   default:
 692       abort();
 693   }
 694 }
 695
 696 static uint32_t get_level1_table_address(CPUARMState *env, uint32_t address)
 697 {
 698     uint32_t table;
 699
 700     if (address & env->cp15.c2_mask)
 701         table = env->cp15.c2_base1 & 0xffffc000;
 702     else
 703         table = env->cp15.c2_base0 & env->cp15.c2_base_mask;
 704
 705     table |= (address >> 18) & 0x3ffc;
 706     return table;
 707 }
 708
 709 static int get_phys_addr_v5(CPUARMState *env, uint32_t address, int access_type,
 710                             int is_user, uint32_t *phys_ptr, int *prot,
 711                             target_ulong *page_size)
 712 {
 713     int code;
 714     uint32_t table;
 715     uint32_t desc;
 716     int type;
 717     int ap;
 718     int domain;
 719     int domain_prot;
 720     uint32_t phys_addr;
 721
 722     /* Pagetable walk.  */
 723     /* Lookup l1 descriptor.  */
 724     table = get_level1_table_address(env, address);
 725     desc = ldl_phys(table);
 726     type = (desc & 3);
 727     domain = (desc >> 5) & 0x0f;
 728     domain_prot = (env->cp15.c3 >> (domain * 2)) & 3;
 729     if (type == 0) {
 730         /* Section translation fault.  */
 731         code = 5;
 732         goto do_fault;
 733     }
 734     if (domain_prot == 0 || domain_prot == 2) {
 735         if (type == 2)
 736             code = 9; /* Section domain fault.  */
 737         else
 738             code = 11; /* Page domain fault.  */
 739         goto do_fault;
 740     }
 741     if (type == 2) {
 742         /* 1Mb section.  */
 743         phys_addr = (desc & 0xfff00000) | (address & 0x000fffff);
 744         ap = (desc >> 10) & 3;
 745         code = 13;
 746         *page_size = 1024 * 1024;
 747     } else {
 748         /* Lookup l2 entry.  */
 749         if (type == 1) {
 750             /* Coarse pagetable.  */
 751             table = (desc & 0xfffffc00) | ((address >> 10) & 0x3fc);
 752         } else {
 753             /* Fine pagetable.  */
 754             table = (desc & 0xfffff000) | ((address >> 8) & 0xffc);
 755         }
 756         desc = ldl_phys(table);
 757         switch (desc & 3) {
 758         case 0: /* Page translation fault.  */
 759             code = 7;
 760             goto do_fault;
 761         case 1: /* 64k page.  */
 762             phys_addr = (desc & 0xffff0000) | (address & 0xffff);
 763             ap = (desc >> (4 + ((address >> 13) & 6))) & 3;
 764             *page_size = 0x10000;
 765             break;
 766         case 2: /* 4k page.  */
 767             phys_addr = (desc & 0xfffff000) | (address & 0xfff);
 768             ap = (desc >> (4 + ((address >> 13) & 6))) & 3;
 769             *page_size = 0x1000;
 770             break;
 771         case 3: /* 1k page.  */
 772             if (type == 1) {
 773                 if (arm_feature(env, ARM_FEATURE_XSCALE)) {
 774                     phys_addr = (desc & 0xfffff000) | (address & 0xfff);
 775                 } else {
 776                     /* Page translation fault.  */
 777                     code = 7;
 778                     goto do_fault;
 779                 }
 780             } else {
 781                 phys_addr = (desc & 0xfffffc00) | (address & 0x3ff);
 782             }
 783             ap = (desc >> 4) & 3;
 784             *page_size = 0x400;
 785             break;
 786         default:
 787             /* Never happens, but compiler isn't smart enough to tell.  */
 788             abort();
 789         }
 790         code = 15;
 791     }
 792     *prot = check_ap(env, ap, domain_prot, access_type, is_user);
 793     if (!*prot) {
 794         /* Access permission fault.  */
 795         goto do_fault;
 796     }
 797     *prot |= PAGE_EXEC;
 798     *phys_ptr = phys_addr;
 799     return 0;
 800 do_fault:
 801     return code | (domain << 4);
 802 }
 803
 804 static int get_phys_addr_v6(CPUARMState *env, uint32_t address, int access_type,
 805                             int is_user, uint32_t *phys_ptr, int *prot,
 806                             target_ulong *page_size)
 807 {
 808     int code;
 809     uint32_t table;
 810     uint32_t desc;
 811     uint32_t xn;
 812     int type;
 813     int ap;
 814     int domain;
 815     int domain_prot;
 816     uint32_t phys_addr;
 817
 818     /* Pagetable walk.  */
 819     /* Lookup l1 descriptor.  */
 820     table = get_level1_table_address(env, address);
 821     desc = ldl_phys(table);
 822     type = (desc & 3);
 823     if (type == 0) {
 824         /* Section translation fault.  */
 825         code = 5;
 826         domain = 0;
 827         goto do_fault;
 828     } else if (type == 2 && (desc & (1 << 18))) {
 829         /* Supersection.  */
 830         domain = 0;
 831     } else {
 832         /* Section or page.  */
 833         domain = (desc >> 5) & 0x0f;
 834     }
 835     domain_prot = (env->cp15.c3 >> (domain * 2)) & 3;
 836     if (domain_prot == 0 || domain_prot == 2) {
 837         if (type == 2)
 838             code = 9; /* Section domain fault.  */
 839         else
 840             code = 11; /* Page domain fault.  */
 841         goto do_fault;
 842     }
 843     if (type == 2) {
 844         if (desc & (1 << 18)) {
 845             /* Supersection.  */
 846             phys_addr = (desc & 0xff000000) | (address & 0x00ffffff);
 847             *page_size = 0x1000000;
 848         } else {
 849             /* Section.  */
 850             phys_addr = (desc & 0xfff00000) | (address & 0x000fffff);
 851             *page_size = 0x100000;
 852         }
 853         ap = ((desc >> 10) & 3) | ((desc >> 13) & 4);
 854         xn = desc & (1 << 4);
 855         code = 13;
 856     } else {
 857         /* Lookup l2 entry.  */
 858         table = (desc & 0xfffffc00) | ((address >> 10) & 0x3fc);
 859         desc = ldl_phys(table);
 860         ap = ((desc >> 4) & 3) | ((desc >> 7) & 4);
 861         switch (desc & 3) {
 862         case 0: /* Page translation fault.  */
 863             code = 7;
 864             goto do_fault;
 865         case 1: /* 64k page.  */
 866             phys_addr = (desc & 0xffff0000) | (address & 0xffff);
 867             xn = desc & (1 << 15);
 868             *page_size = 0x10000;
 869             break;
 870         case 2: case 3: /* 4k page.  */
 871             phys_addr = (desc & 0xfffff000) | (address & 0xfff);
 872             xn = desc & 1;
 873             *page_size = 0x1000;
 874             break;
 875         default:
 876             /* Never happens, but compiler isn't smart enough to tell.  */
 877             abort();
 878         }
 879         code = 15;
 880     }
 881     if (domain_prot == 3) {
 882         *prot = PAGE_READ | PAGE_WRITE | PAGE_EXEC;
 883     } else {
 884         if (xn && access_type == 2)
 885             goto do_fault;
 886
 887         /* The simplified model uses AP[0] as an access control bit.  */
 888         if ((env->cp15.c1_sys & (1 << 29)) && (ap & 1) == 0) {
 889             /* Access flag fault.  */
 890             code = (code == 15) ? 6 : 3;
 891             goto do_fault;
 892         }
 893         *prot = check_ap(env, ap, domain_prot, access_type, is_user);
 894         if (!*prot) {
 895             /* Access permission fault.  */
 896             goto do_fault;
 897         }
 898         if (!xn) {
 899             *prot |= PAGE_EXEC;
 900         }
 901     }
 902     *phys_ptr = phys_addr;
 903     return 0;
 904 do_fault:
 905     return code | (domain << 4);
 906 }
 907
 908 static int get_phys_addr_mpu(CPUARMState *env, uint32_t address, int access_type,
 909                              int is_user, uint32_t *phys_ptr, int *prot)
 910 {
 911     int n;
 912     uint32_t mask;
 913     uint32_t base;
 914
 915     *phys_ptr = address;
 916     for (n = 7; n >= 0; n--) {
 917         base = env->cp15.c6_region[n];
 918         if ((base & 1) == 0)
 919             continue;
 920         mask = 1 << ((base >> 1) & 0x1f);
 921         /* Keep this shift separate from the above to avoid an
 922            (undefined) << 32.  */
 923         mask = (mask << 1) - 1;
 924         if (((base ^ address) & ~mask) == 0)
 925             break;
 926     }
 927     if (n < 0)
 928         return 2;
 929
 930     if (access_type == 2) {
 931         mask = env->cp15.c5_insn;
 932     } else {
 933         mask = env->cp15.c5_data;
 934     }
 935     mask = (mask >> (n * 4)) & 0xf;
 936     switch (mask) {
 937     case 0:
 938         return 1;
 939     case 1:
 940         if (is_user)
 941           return 1;
 942         *prot = PAGE_READ | PAGE_WRITE;
 943         break;
 944     case 2:
 945         *prot = PAGE_READ;
 946         if (!is_user)
 947             *prot |= PAGE_WRITE;
 948         break;
 949     case 3:
 950         *prot = PAGE_READ | PAGE_WRITE;
 951         break;
 952     case 5:
 953         if (is_user)
 954             return 1;
 955         *prot = PAGE_READ;
 956         break;
 957     case 6:
 958         *prot = PAGE_READ;
 959         break;
 960     default:
 961         /* Bad permission.  */
 962         return 1;
 963     }
 964     *prot |= PAGE_EXEC;
 965     return 0;
 966 }
 967
 968 static inline int get_phys_addr(CPUARMState *env, uint32_t address,
 969                                 int access_type, int is_user,
 970                                 uint32_t *phys_ptr, int *prot,
 971                                 target_ulong *page_size)
 972 {
 973     /* Fast Context Switch Extension.  */
 974     if (address < 0x02000000)
 975         address += env->cp15.c13_fcse;
 976
 977     if ((env->cp15.c1_sys & 1) == 0) {
 978         /* MMU/MPU disabled.  */
 979         *phys_ptr = address;
 980         *prot = PAGE_READ | PAGE_WRITE | PAGE_EXEC;
 981         *page_size = TARGET_PAGE_SIZE;
 982         return 0;
 983     } else if (arm_feature(env, ARM_FEATURE_MPU)) {
 984         *page_size = TARGET_PAGE_SIZE;
 985         return get_phys_addr_mpu(env, address, access_type, is_user, phys_ptr,
 986                                  prot);
 987     } else if (env->cp15.c1_sys & (1 << 23)) {
 988         return get_phys_addr_v6(env, address, access_type, is_user, phys_ptr,
 989                                 prot, page_size);
 990     } else {
 991         return get_phys_addr_v5(env, address, access_type, is_user, phys_ptr,
 992                                 prot, page_size);
 993     }
 994 }
 995
 996 int cpu_arm_handle_mmu_fault (CPUARMState *env, target_ulong address,
 997                               int access_type, int mmu_idx)
 998 {
 999     uint32_t phys_addr;
1000     target_ulong page_size;
1001     int prot;
1002     int ret, is_user;
1003
1004     is_user = mmu_idx == MMU_USER_IDX;
1005     ret = get_phys_addr(env, address, access_type, is_user, &phys_addr, &prot,
1006                         &page_size);
1007     if (ret == 0) {
1008         /* Map a single [sub]page.  */
1009         phys_addr &= ~(uint32_t)0x3ff;
1010         address &= ~(uint32_t)0x3ff;
1011         tlb_set_page (env, address, phys_addr, prot, mmu_idx, page_size);
1012         return 0;
1013     }
1014
1015     if (access_type == 2) {
1016         env->cp15.c5_insn = ret;
1017         env->cp15.c6_insn = address;
1018         env->exception_index = EXCP_PREFETCH_ABORT;
1019     } else {
1020         env->cp15.c5_data = ret;
1021         if (access_type == 1 && arm_feature(env, ARM_FEATURE_V6))
1022             env->cp15.c5_data |= (1 << 11);
1023         env->cp15.c6_data = address;
1024         env->exception_index = EXCP_DATA_ABORT;
1025     }
1026     return 1;
1027 }
1028
1029 target_phys_addr_t cpu_get_phys_page_debug(CPUARMState *env, target_ulong addr)
1030 {
1031     uint32_t phys_addr;
1032     target_ulong page_size;
1033     int prot;
1034     int ret;
1035
1036     ret = get_phys_addr(env, addr, 0, 0, &phys_addr, &prot, &page_size);
1037
1038     if (ret != 0)
1039         return -1;
1040
1041     return phys_addr;
1042 }
1043
1044 void HELPER(set_cp)(CPUARMState *env, uint32_t insn, uint32_t val)
1045 {
1046     int cp_num = (insn >> 8) & 0xf;
1047     int cp_info = (insn >> 5) & 7;
1048     int src = (insn >> 16) & 0xf;
1049     int operand = insn & 0xf;
1050
1051     if (env->cp[cp_num].cp_write)
1052         env->cp[cp_num].cp_write(env->cp[cp_num].opaque,
1053                                  cp_info, src, operand, val);
1054 }
1055
1056 uint32_t HELPER(get_cp)(CPUARMState *env, uint32_t insn)
1057 {
1058     int cp_num = (insn >> 8) & 0xf;
1059     int cp_info = (insn >> 5) & 7;
1060     int dest = (insn >> 16) & 0xf;
1061     int operand = insn & 0xf;
1062
1063     if (env->cp[cp_num].cp_read)
1064         return env->cp[cp_num].cp_read(env->cp[cp_num].opaque,
1065                                        cp_info, dest, operand);
1066     return 0;
1067 }
1068
1069 /* Return basic MPU access permission bits.  */
1070 static uint32_t simple_mpu_ap_bits(uint32_t val)
1071 {
1072     uint32_t ret;
1073     uint32_t mask;
1074     int i;
1075     ret = 0;
1076     mask = 3;
1077     for (i = 0; i < 16; i += 2) {
1078         ret |= (val >> i) & mask;
1079         mask <<= 2;
1080     }
1081     return ret;
1082 }
1083
1084 /* Pad basic MPU access permission bits to extended format.  */
1085 static uint32_t extended_mpu_ap_bits(uint32_t val)
1086 {
1087     uint32_t ret;
1088     uint32_t mask;
1089     int i;
1090     ret = 0;
1091     mask = 3;
1092     for (i = 0; i < 16; i += 2) {
1093         ret |= (val & mask) << i;
1094         mask <<= 2;
1095     }
1096     return ret;
1097 }
1098
1099 void HELPER(set_cp15)(CPUARMState *env, uint32_t insn, uint32_t val)
1100 {
1101     int op1;
1102     int op2;
1103     int crm;
1104
1105     op1 = (insn >> 21) & 7;
1106     op2 = (insn >> 5) & 7;
1107     crm = insn & 0xf;
1108     switch ((insn >> 16) & 0xf) {
1109     case 0:
1110         /* ID codes.  */
1111         if (arm_feature(env, ARM_FEATURE_XSCALE))
1112             break;
1113         if (arm_feature(env, ARM_FEATURE_OMAPCP))
1114             break;
1115         if (arm_feature(env, ARM_FEATURE_V7)
1116                 && op1 == 2 && crm == 0 && op2 == 0) {
1117             env->cp15.c0_cssel = val & 0xf;
1118             break;
1119         }
1120         goto bad_reg;
1121     case 1: /* System configuration.  */
1122         if (arm_feature(env, ARM_FEATURE_V7)
1123                 && op1 == 0 && crm == 1 && op2 == 0) {
1124             env->cp15.c1_scr = val;
1125             break;
1126         }
1127         if (arm_feature(env, ARM_FEATURE_OMAPCP))
1128             op2 = 0;
1129         switch (op2) {
1130         case 0:
1131             if (!arm_feature(env, ARM_FEATURE_XSCALE) || crm == 0)
1132                 env->cp15.c1_sys = val;
1133             /* ??? Lots of these bits are not implemented.  */
1134             /* This may enable/disable the MMU, so do a TLB flush.  */
1135             tlb_flush(env, 1);
1136             break;
1137         case 1: /* Auxiliary control register.  */
1138             if (arm_feature(env, ARM_FEATURE_XSCALE)) {
1139                 env->cp15.c1_xscaleauxcr = val;
1140                 break;
1141             }
1142             /* Not implemented.  */
1143             break;
1144         case 2:
1145             if (arm_feature(env, ARM_FEATURE_XSCALE))
1146                 goto bad_reg;
1147             if (env->cp15.c1_coproc != val) {
1148                 env->cp15.c1_coproc = val;
1149                 /* ??? Is this safe when called from within a TB?  */
1150                 tb_flush(env);
1151             }
1152             break;
1153         default:
1154             goto bad_reg;
1155         }
1156         break;
1157     case 2: /* MMU Page table control / MPU cache control.  */
1158         if (arm_feature(env, ARM_FEATURE_MPU)) {
1159             switch (op2) {
1160             case 0:
1161                 env->cp15.c2_data = val;
1162                 break;
1163             case 1:
1164                 env->cp15.c2_insn = val;
1165                 break;
1166             default:
1167                 goto bad_reg;
1168             }
1169         } else {
1170             switch (op2) {
1171             case 0:
1172                 env->cp15.c2_base0 = val;
1173                 break;
1174             case 1:
1175                 env->cp15.c2_base1 = val;
1176                 break;
1177             case 2:
1178                 val &= 7;
1179                 env->cp15.c2_control = val;
1180                 env->cp15.c2_mask = ~(((uint32_t)0xffffffffu) >> val);
1181                 env->cp15.c2_base_mask = ~((uint32_t)0x3fffu >> val);
1182                 break;
1183             default:
1184                 goto bad_reg;
1185             }
1186         }
1187         break;
1188     case 3: /* MMU Domain access control / MPU write buffer control.  */
1189         env->cp15.c3 = val;
1190         tlb_flush(env, 1); /* Flush TLB as domain not tracked in TLB */
1191         break;
1192     case 4: /* Reserved.  */
1193         goto bad_reg;
1194     case 5: /* MMU Fault status / MPU access permission.  */
1195         if (arm_feature(env, ARM_FEATURE_OMAPCP))
1196             op2 = 0;
1197         switch (op2) {
1198         case 0:
1199             if (arm_feature(env, ARM_FEATURE_MPU))
1200                 val = extended_mpu_ap_bits(val);
1201             env->cp15.c5_data = val;
1202             break;
1203         case 1:
1204             if (arm_feature(env, ARM_FEATURE_MPU))
1205                 val = extended_mpu_ap_bits(val);
1206             env->cp15.c5_insn = val;
1207             break;
1208         case 2:
1209             if (!arm_feature(env, ARM_FEATURE_MPU))
1210                 goto bad_reg;
1211             env->cp15.c5_data = val;
1212             break;
1213         case 3:
1214             if (!arm_feature(env, ARM_FEATURE_MPU))
1215                 goto bad_reg;
1216             env->cp15.c5_insn = val;
1217             break;
1218         default:
1219             goto bad_reg;
1220         }
1221         break;
1222     case 6: /* MMU Fault address / MPU base/size.  */
1223         if (arm_feature(env, ARM_FEATURE_MPU)) {
1224             if (crm >= 8)
1225                 goto bad_reg;
1226             env->cp15.c6_region[crm] = val;
1227         } else {
1228             if (arm_feature(env, ARM_FEATURE_OMAPCP))
1229                 op2 = 0;
1230             switch (op2) {
1231             case 0:
1232                 env->cp15.c6_data = val;
1233                 break;
1234             case 1: /* ??? This is WFAR on armv6 */
1235             case 2:
1236                 env->cp15.c6_insn = val;
1237                 break;
1238             default:
1239                 goto bad_reg;
1240             }
1241         }
1242         break;
1243     case 7: /* Cache control.  */
1244         env->cp15.c15_i_max = 0x000;
1245         env->cp15.c15_i_min = 0xff0;
1246         if (op1 != 0) {
1247             goto bad_reg;
1248         }
1249         /* No cache, so nothing to do except VA->PA translations. */
1250         if (arm_feature(env, ARM_FEATURE_VAPA)) {
1251             switch (crm) {
1252             case 4:
1253                 if (arm_feature(env, ARM_FEATURE_V7)) {
1254                     env->cp15.c7_par = val & 0xfffff6ff;
1255                 } else {
1256                     env->cp15.c7_par = val & 0xfffff1ff;
1257                 }
1258                 break;
1259             case 8: {
1260                 uint32_t phys_addr;
1261                 target_ulong page_size;
1262                 int prot;
1263                 int ret, is_user = op2 & 2;
1264                 int access_type = op2 & 1;
1265
1266                 if (op2 & 4) {
1267                     /* Other states are only available with TrustZone */
1268                     goto bad_reg;
1269                 }
1270                 ret = get_phys_addr(env, val, access_type, is_user,
1271                                     &phys_addr, &prot, &page_size);
1272                 if (ret == 0) {
1273                     /* We do not set any attribute bits in the PAR */
1274                     if (page_size == (1 << 24)
1275                         && arm_feature(env, ARM_FEATURE_V7)) {
1276                         env->cp15.c7_par = (phys_addr & 0xff000000) | 1 << 1;
1277                     } else {
1278                         env->cp15.c7_par = phys_addr & 0xfffff000;
1279                     }
1280                 } else {
1281                     env->cp15.c7_par = ((ret & (10 << 1)) >> 5) |
1282                                        ((ret & (12 << 1)) >> 6) |
1283                                        ((ret & 0xf) << 1) | 1;
1284                 }
1285                 break;
1286             }
1287             }
1288         }
1289         break;
1290     case 8: /* MMU TLB control.  */
1291         switch (op2) {
1292         case 0: /* Invalidate all (TLBIALL) */
1293             tlb_flush(env, 1);
1294             break;
1295         case 1: /* Invalidate single TLB entry by MVA and ASID (TLBIMVA) */
1296             tlb_flush_page(env, val & TARGET_PAGE_MASK);
1297             break;
1298         case 2: /* Invalidate by ASID (TLBIASID) */
1299             tlb_flush(env, val == 0);
1300             break;
1301         case 3: /* Invalidate single entry by MVA, all ASIDs (TLBIMVAA) */
1302             tlb_flush_page(env, val & TARGET_PAGE_MASK);
1303             break;
1304         default:
1305             goto bad_reg;
1306         }
1307         break;
1308     case 9:
1309         if (arm_feature(env, ARM_FEATURE_OMAPCP))
1310             break;
1311         if (arm_feature(env, ARM_FEATURE_STRONGARM))
1312             break; /* Ignore ReadBuffer access */
1313         switch (crm) {
1314         case 0: /* Cache lockdown.  */
1315             switch (op1) {
1316             case 0: /* L1 cache.  */
1317                 switch (op2) {
1318                 case 0:
1319                     env->cp15.c9_data = val;
1320                     break;
1321                 case 1:
1322                     env->cp15.c9_insn = val;
1323                     break;
1324                 default:
1325                     goto bad_reg;
1326                 }
1327                 break;
1328             case 1: /* L2 cache.  */
1329                 /* Ignore writes to L2 lockdown/auxiliary registers.  */
1330                 break;
1331             default:
1332                 goto bad_reg;
1333             }
1334             break;
1335         case 1: /* TCM memory region registers.  */
1336             /* Not implemented.  */
1337             goto bad_reg;
1338         case 12: /* Performance monitor control */
1339             /* Performance monitors are implementation defined in v7,
1340              * but with an ARM recommended set of registers, which we
1341              * follow (although we don't actually implement any counters)
1342              */
1343             if (!arm_feature(env, ARM_FEATURE_V7)) {
1344                 goto bad_reg;
1345             }
1346             switch (op2) {
1347             case 0: /* performance monitor control register */
1348                 /* only the DP, X, D and E bits are writable */
1349                 env->cp15.c9_pmcr &= ~0x39;
1350                 env->cp15.c9_pmcr |= (val & 0x39);
1351                 break;
1352             case 1: /* Count enable set register */
1353                 val &= (1 << 31);
1354                 env->cp15.c9_pmcnten |= val;
1355                 break;
1356             case 2: /* Count enable clear */
1357                 val &= (1 << 31);
1358                 env->cp15.c9_pmcnten &= ~val;
1359                 break;
1360             case 3: /* Overflow flag status */
1361                 env->cp15.c9_pmovsr &= ~val;
1362                 break;
1363             case 4: /* Software increment */
1364                 /* RAZ/WI since we don't implement the software-count event */
1365                 break;
1366             case 5: /* Event counter selection register */
1367                 /* Since we don't implement any events, writing to this register
1368                  * is actually UNPREDICTABLE. So we choose to RAZ/WI.
1369                  */
1370                 break;
1371             default:
1372                 goto bad_reg;
1373             }
1374             break;
1375         case 13: /* Performance counters */
1376             if (!arm_feature(env, ARM_FEATURE_V7)) {
1377                 goto bad_reg;
1378             }
1379             switch (op2) {
1380             case 0: /* Cycle count register: not implemented, so RAZ/WI */
1381                 break;
1382             case 1: /* Event type select */
1383                 env->cp15.c9_pmxevtyper = val & 0xff;
1384                 break;
1385             case 2: /* Event count register */
1386                 /* Unimplemented (we have no events), RAZ/WI */
1387                 break;
1388             default:
1389                 goto bad_reg;
1390             }
1391             break;
1392         case 14: /* Performance monitor control */
1393             if (!arm_feature(env, ARM_FEATURE_V7)) {
1394                 goto bad_reg;
1395             }
1396             switch (op2) {
1397             case 0: /* user enable */
1398                 env->cp15.c9_pmuserenr = val & 1;
1399                 /* changes access rights for cp registers, so flush tbs */
1400                 tb_flush(env);
1401                 break;
1402             case 1: /* interrupt enable set */
1403                 /* We have no event counters so only the C bit can be changed */
1404                 val &= (1 << 31);
1405                 env->cp15.c9_pminten |= val;
1406                 break;
1407             case 2: /* interrupt enable clear */
1408                 val &= (1 << 31);
1409                 env->cp15.c9_pminten &= ~val;
1410                 break;
1411             }
1412             break;
1413         default:
1414             goto bad_reg;
1415         }
1416         break;
1417     case 10: /* MMU TLB lockdown.  */
1418         /* ??? TLB lockdown not implemented.  */
1419         break;
1420     case 12: /* Reserved.  */
1421         goto bad_reg;
1422     case 13: /* Process ID.  */
1423         switch (op2) {
1424         case 0:
1425             /* Unlike real hardware the qemu TLB uses virtual addresses,
1426                not modified virtual addresses, so this causes a TLB flush.
1427              */
1428             if (env->cp15.c13_fcse != val)
1429               tlb_flush(env, 1);
1430             env->cp15.c13_fcse = val;
1431             break;
1432         case 1:
1433             /* This changes the ASID, so do a TLB flush.  */
1434             if (env->cp15.c13_context != val
1435                 && !arm_feature(env, ARM_FEATURE_MPU))
1436               tlb_flush(env, 0);
1437             env->cp15.c13_context = val;
1438             break;
1439         default:
1440             goto bad_reg;
1441         }
1442         break;
1443     case 14: /* Generic timer */
1444         if (arm_feature(env, ARM_FEATURE_GENERIC_TIMER)) {
1445             /* Dummy implementation: RAZ/WI for all */
1446             break;
1447         }
1448         goto bad_reg;
1449     case 15: /* Implementation specific.  */
1450         if (arm_feature(env, ARM_FEATURE_XSCALE)) {
1451             if (op2 == 0 && crm == 1) {
1452                 if (env->cp15.c15_cpar != (val & 0x3fff)) {
1453                     /* Changes cp0 to cp13 behavior, so needs a TB flush.  */
1454                     tb_flush(env);
1455                     env->cp15.c15_cpar = val & 0x3fff;
1456                 }
1457                 break;
1458             }
1459             goto bad_reg;
1460         }
1461         if (arm_feature(env, ARM_FEATURE_OMAPCP)) {
1462             switch (crm) {
1463             case 0:
1464                 break;
1465             case 1: /* Set TI925T configuration.  */
1466                 env->cp15.c15_ticonfig = val & 0xe7;
1467                 env->cp15.c0_cpuid = (val & (1 << 5)) ? /* OS_TYPE bit */
1468                         ARM_CPUID_TI915T : ARM_CPUID_TI925T;
1469                 break;
1470             case 2: /* Set I_max.  */
1471                 env->cp15.c15_i_max = val;
1472                 break;
1473             case 3: /* Set I_min.  */
1474                 env->cp15.c15_i_min = val;
1475                 break;
1476             case 4: /* Set thread-ID.  */
1477                 env->cp15.c15_threadid = val & 0xffff;
1478                 break;
1479             case 8: /* Wait-for-interrupt (deprecated).  */
1480                 cpu_interrupt(env, CPU_INTERRUPT_HALT);
1481                 break;
1482             default:
1483                 goto bad_reg;
1484             }
1485         }
1486         if (ARM_CPUID(env) == ARM_CPUID_CORTEXA9) {
1487             switch (crm) {
1488             case 0:
1489                 if ((op1 == 0) && (op2 == 0)) {
1490                     env->cp15.c15_power_control = val;
1491                 } else if ((op1 == 0) && (op2 == 1)) {
1492                     env->cp15.c15_diagnostic = val;
1493                 } else if ((op1 == 0) && (op2 == 2)) {
1494                     env->cp15.c15_power_diagnostic = val;
1495                 }
1496             default:
1497                 break;
1498             }
1499         }
1500         break;
1501     }
1502     return;
1503 bad_reg:
1504     /* ??? For debugging only.  Should raise illegal instruction exception.  */
1505     cpu_abort(env, "Unimplemented cp15 register write (c%d, c%d, {%d, %d})\n",
1506               (insn >> 16) & 0xf, crm, op1, op2);
1507 }
1508
1509 uint32_t HELPER(get_cp15)(CPUARMState *env, uint32_t insn)
1510 {
1511     int op1;
1512     int op2;
1513     int crm;
1514
1515     op1 = (insn >> 21) & 7;
1516     op2 = (insn >> 5) & 7;
1517     crm = insn & 0xf;
1518     switch ((insn >> 16) & 0xf) {
1519     case 0: /* ID codes.  */
1520         switch (op1) {
1521         case 0:
1522             switch (crm) {
1523             case 0:
1524                 switch (op2) {
1525                 case 0: /* Device ID.  */
1526                     return env->cp15.c0_cpuid;
1527                 case 1: /* Cache Type.  */
1528                     return env->cp15.c0_cachetype;
1529                 case 2: /* TCM status.  */
1530                     return 0;
1531                 case 3: /* TLB type register.  */
1532                     return 0; /* No lockable TLB entries.  */
1533                 case 5: /* MPIDR */
1534                     /* The MPIDR was standardised in v7; prior to
1535                      * this it was implemented only in the 11MPCore.
1536                      * For all other pre-v7 cores it does not exist.
1537                      */
1538                     if (arm_feature(env, ARM_FEATURE_V7) ||
1539                         ARM_CPUID(env) == ARM_CPUID_ARM11MPCORE) {
1540                         int mpidr = env->cpu_index;
1541                         /* We don't support setting cluster ID ([8..11])
1542                          * so these bits always RAZ.
1543                          */
1544                         if (arm_feature(env, ARM_FEATURE_V7MP)) {
1545                             mpidr |= (1 << 31);
1546                             /* Cores which are uniprocessor (non-coherent)
1547                              * but still implement the MP extensions set
1548                              * bit 30. (For instance, A9UP.) However we do
1549                              * not currently model any of those cores.
1550                              */
1551                         }
1552                         return mpidr;
1553                     }
1554                     /* otherwise fall through to the unimplemented-reg case */
1555                 default:
1556                     goto bad_reg;
1557                 }
1558             case 1:
1559                 if (!arm_feature(env, ARM_FEATURE_V6))
1560                     goto bad_reg;
1561                 return env->cp15.c0_c1[op2];
1562             case 2:
1563                 if (!arm_feature(env, ARM_FEATURE_V6))
1564                     goto bad_reg;
1565                 return env->cp15.c0_c2[op2];
1566             case 3: case 4: case 5: case 6: case 7:
1567                 return 0;
1568             default:
1569                 goto bad_reg;
1570             }
1571         case 1:
1572             /* These registers aren't documented on arm11 cores.  However
1573                Linux looks at them anyway.  */
1574             if (!arm_feature(env, ARM_FEATURE_V6))
1575                 goto bad_reg;
1576             if (crm != 0)
1577                 goto bad_reg;
1578             if (!arm_feature(env, ARM_FEATURE_V7))
1579                 return 0;
1580
1581             switch (op2) {
1582             case 0:
1583                 return env->cp15.c0_ccsid[env->cp15.c0_cssel];
1584             case 1:
1585                 return env->cp15.c0_clid;
1586             case 7:
1587                 return 0;
1588             }
1589             goto bad_reg;
1590         case 2:
1591             if (op2 != 0 || crm != 0)
1592                 goto bad_reg;
1593             return env->cp15.c0_cssel;
1594         default:
1595             goto bad_reg;
1596         }
1597     case 1: /* System configuration.  */
1598         if (arm_feature(env, ARM_FEATURE_V7)
1599             && op1 == 0 && crm == 1 && op2 == 0) {
1600             return env->cp15.c1_scr;
1601         }
1602         if (arm_feature(env, ARM_FEATURE_OMAPCP))
1603             op2 = 0;
1604         switch (op2) {
1605         case 0: /* Control register.  */
1606             return env->cp15.c1_sys;
1607         case 1: /* Auxiliary control register.  */
1608             if (arm_feature(env, ARM_FEATURE_XSCALE))
1609                 return env->cp15.c1_xscaleauxcr;
1610             if (!arm_feature(env, ARM_FEATURE_AUXCR))
1611                 goto bad_reg;
1612             switch (ARM_CPUID(env)) {
1613             case ARM_CPUID_ARM1026:
1614                 return 1;
1615             case ARM_CPUID_ARM1136:
1616             case ARM_CPUID_ARM1136_R2:
1617             case ARM_CPUID_ARM1176:
1618                 return 7;
1619             case ARM_CPUID_ARM11MPCORE:
1620                 return 1;
1621             case ARM_CPUID_CORTEXA8:
1622                 return 2;
1623             case ARM_CPUID_CORTEXA9:
1624             case ARM_CPUID_CORTEXA15:
1625                 return 0;
1626             default:
1627                 goto bad_reg;
1628             }
1629         case 2: /* Coprocessor access register.  */
1630             if (arm_feature(env, ARM_FEATURE_XSCALE))
1631                 goto bad_reg;
1632             return env->cp15.c1_coproc;
1633         default:
1634             goto bad_reg;
1635         }
1636     case 2: /* MMU Page table control / MPU cache control.  */
1637         if (arm_feature(env, ARM_FEATURE_MPU)) {
1638             switch (op2) {
1639             case 0:
1640                 return env->cp15.c2_data;
1641                 break;
1642             case 1:
1643                 return env->cp15.c2_insn;
1644                 break;
1645             default:
1646                 goto bad_reg;
1647             }
1648         } else {
1649             switch (op2) {
1650             case 0:
1651                 return env->cp15.c2_base0;
1652             case 1:
1653                 return env->cp15.c2_base1;
1654             case 2:
1655                 return env->cp15.c2_control;
1656             default:
1657                 goto bad_reg;
1658             }
1659         }
1660     case 3: /* MMU Domain access control / MPU write buffer control.  */
1661         return env->cp15.c3;
1662     case 4: /* Reserved.  */
1663         goto bad_reg;
1664     case 5: /* MMU Fault status / MPU access permission.  */
1665         if (arm_feature(env, ARM_FEATURE_OMAPCP))
1666             op2 = 0;
1667         switch (op2) {
1668         case 0:
1669             if (arm_feature(env, ARM_FEATURE_MPU))
1670                 return simple_mpu_ap_bits(env->cp15.c5_data);
1671             return env->cp15.c5_data;
1672         case 1:
1673             if (arm_feature(env, ARM_FEATURE_MPU))
1674                 return simple_mpu_ap_bits(env->cp15.c5_insn);
1675             return env->cp15.c5_insn;
1676         case 2:
1677             if (!arm_feature(env, ARM_FEATURE_MPU))
1678                 goto bad_reg;
1679             return env->cp15.c5_data;
1680         case 3:
1681             if (!arm_feature(env, ARM_FEATURE_MPU))
1682                 goto bad_reg;
1683             return env->cp15.c5_insn;
1684         default:
1685             goto bad_reg;
1686         }
1687     case 6: /* MMU Fault address.  */
1688         if (arm_feature(env, ARM_FEATURE_MPU)) {
1689             if (crm >= 8)
1690                 goto bad_reg;
1691             return env->cp15.c6_region[crm];
1692         } else {
1693             if (arm_feature(env, ARM_FEATURE_OMAPCP))
1694                 op2 = 0;
1695             switch (op2) {
1696             case 0:
1697                 return env->cp15.c6_data;
1698             case 1:
1699                 if (arm_feature(env, ARM_FEATURE_V6)) {
1700                     /* Watchpoint Fault Adrress.  */
1701                     return 0; /* Not implemented.  */
1702                 } else {
1703                     /* Instruction Fault Adrress.  */
1704                     /* Arm9 doesn't have an IFAR, but implementing it anyway
1705                        shouldn't do any harm.  */
1706                     return env->cp15.c6_insn;
1707                 }
1708             case 2:
1709                 if (arm_feature(env, ARM_FEATURE_V6)) {
1710                     /* Instruction Fault Adrress.  */
1711                     return env->cp15.c6_insn;
1712                 } else {
1713                     goto bad_reg;
1714                 }
1715             default:
1716                 goto bad_reg;
1717             }
1718         }
1719     case 7: /* Cache control.  */
1720         if (crm == 4 && op1 == 0 && op2 == 0) {
1721             return env->cp15.c7_par;
1722         }
1723         /* FIXME: Should only clear Z flag if destination is r15.  */
1724         env->ZF = 0;
1725         return 0;
1726     case 8: /* MMU TLB control.  */
1727         goto bad_reg;
1728     case 9:
1729         switch (crm) {
1730         case 0: /* Cache lockdown */
1731             switch (op1) {
1732             case 0: /* L1 cache.  */
1733                 if (arm_feature(env, ARM_FEATURE_OMAPCP)) {
1734                     return 0;
1735                 }
1736                 switch (op2) {
1737                 case 0:
1738                     return env->cp15.c9_data;
1739                 case 1:
1740                     return env->cp15.c9_insn;
1741                 default:
1742                     goto bad_reg;
1743                 }
1744             case 1: /* L2 cache */
1745                 /* L2 Lockdown and Auxiliary control.  */
1746                 switch (op2) {
1747                 case 0:
1748                     /* L2 cache lockdown (A8 only) */
1749                     return 0;
1750                 case 2:
1751                     /* L2 cache auxiliary control (A8) or control (A15) */
1752                     if (ARM_CPUID(env) == ARM_CPUID_CORTEXA15) {
1753                         /* Linux wants the number of processors from here.
1754                          * Might as well set the interrupt-controller bit too.
1755                          */
1756                         return ((smp_cpus - 1) << 24) | (1 << 23);
1757                     }
1758                     return 0;
1759                 case 3:
1760                     /* L2 cache extended control (A15) */
1761                     return 0;
1762                 default:
1763                     goto bad_reg;
1764                 }
1765             default:
1766                 goto bad_reg;
1767             }
1768             break;
1769         case 12: /* Performance monitor control */
1770             if (!arm_feature(env, ARM_FEATURE_V7)) {
1771                 goto bad_reg;
1772             }
1773             switch (op2) {
1774             case 0: /* performance monitor control register */
1775                 return env->cp15.c9_pmcr;
1776             case 1: /* count enable set */
1777             case 2: /* count enable clear */
1778                 return env->cp15.c9_pmcnten;
1779             case 3: /* overflow flag status */
1780                 return env->cp15.c9_pmovsr;
1781             case 4: /* software increment */
1782             case 5: /* event counter selection register */
1783                 return 0; /* Unimplemented, RAZ/WI */
1784             default:
1785                 goto bad_reg;
1786             }
1787         case 13: /* Performance counters */
1788             if (!arm_feature(env, ARM_FEATURE_V7)) {
1789                 goto bad_reg;
1790             }
1791             switch (op2) {
1792             case 1: /* Event type select */
1793                 return env->cp15.c9_pmxevtyper;
1794             case 0: /* Cycle count register */
1795             case 2: /* Event count register */
1796                 /* Unimplemented, so RAZ/WI */
1797                 return 0;
1798             default:
1799                 goto bad_reg;
1800             }
1801         case 14: /* Performance monitor control */
1802             if (!arm_feature(env, ARM_FEATURE_V7)) {
1803                 goto bad_reg;
1804             }
1805             switch (op2) {
1806             case 0: /* user enable */
1807                 return env->cp15.c9_pmuserenr;
1808             case 1: /* interrupt enable set */
1809             case 2: /* interrupt enable clear */
1810                 return env->cp15.c9_pminten;
1811             default:
1812                 goto bad_reg;
1813             }
1814         default:
1815             goto bad_reg;
1816         }
1817         break;
1818     case 10: /* MMU TLB lockdown.  */
1819         /* ??? TLB lockdown not implemented.  */
1820         return 0;
1821     case 11: /* TCM DMA control.  */
1822     case 12: /* Reserved.  */
1823         goto bad_reg;
1824     case 13: /* Process ID.  */
1825         switch (op2) {
1826         case 0:
1827             return env->cp15.c13_fcse;
1828         case 1:
1829             return env->cp15.c13_context;
1830         default:
1831             goto bad_reg;
1832         }
1833     case 14: /* Generic timer */
1834         if (arm_feature(env, ARM_FEATURE_GENERIC_TIMER)) {
1835             /* Dummy implementation: RAZ/WI for all */
1836             return 0;
1837         }
1838         goto bad_reg;
1839     case 15: /* Implementation specific.  */
1840         if (arm_feature(env, ARM_FEATURE_XSCALE)) {
1841             if (op2 == 0 && crm == 1)
1842                 return env->cp15.c15_cpar;
1843
1844             goto bad_reg;
1845         }
1846         if (arm_feature(env, ARM_FEATURE_OMAPCP)) {
1847             switch (crm) {
1848             case 0:
1849                 return 0;
1850             case 1: /* Read TI925T configuration.  */
1851                 return env->cp15.c15_ticonfig;
1852             case 2: /* Read I_max.  */
1853                 return env->cp15.c15_i_max;
1854             case 3: /* Read I_min.  */
1855                 return env->cp15.c15_i_min;
1856             case 4: /* Read thread-ID.  */
1857                 return env->cp15.c15_threadid;
1858             case 8: /* TI925T_status */
1859                 return 0;
1860             }
1861             /* TODO: Peripheral port remap register:
1862              * On OMAP2 mcr p15, 0, rn, c15, c2, 4 sets up the interrupt
1863              * controller base address at $rn & ~0xfff and map size of
1864              * 0x200 << ($rn & 0xfff), when MMU is off.  */
1865             goto bad_reg;
1866         }
1867         if (ARM_CPUID(env) == ARM_CPUID_CORTEXA9) {
1868             switch (crm) {
1869             case 0:
1870                 if ((op1 == 4) && (op2 == 0)) {
1871                     /* The config_base_address should hold the value of
1872                      * the peripheral base. ARM should get this from a CPU
1873                      * object property, but that support isn't available in
1874                      * December 2011. Default to 0 for now and board models
1875                      * that care can set it by a private hook */
1876                     return env->cp15.c15_config_base_address;
1877                 } else if ((op1 == 0) && (op2 == 0)) {
1878                     /* power_control should be set to maximum latency. Again,
1879                        default to 0 and set by private hook */
1880                     return env->cp15.c15_power_control;
1881                 } else if ((op1 == 0) && (op2 == 1)) {
1882                     return env->cp15.c15_diagnostic;
1883                 } else if ((op1 == 0) && (op2 == 2)) {
1884                     return env->cp15.c15_power_diagnostic;
1885                 }
1886                 break;
1887             case 1: /* NEON Busy */
1888                 return 0;
1889             case 5: /* tlb lockdown */
1890             case 6:
1891             case 7:
1892                 if ((op1 == 5) && (op2 == 2)) {
1893                     return 0;
1894                 }
1895                 break;
1896             default:
1897                 break;
1898             }
1899             goto bad_reg;
1900         }
1901         return 0;
1902     }
1903 bad_reg:
1904     /* ??? For debugging only.  Should raise illegal instruction exception.  */
1905     cpu_abort(env, "Unimplemented cp15 register read (c%d, c%d, {%d, %d})\n",
1906               (insn >> 16) & 0xf, crm, op1, op2);
1907     return 0;
1908 }
1909
1910 void HELPER(set_r13_banked)(CPUARMState *env, uint32_t mode, uint32_t val)
1911 {
1912     if ((env->uncached_cpsr & CPSR_M) == mode) {
1913         env->regs[13] = val;
1914     } else {
1915         env->banked_r13[bank_number(env, mode)] = val;
1916     }
1917 }
1918
1919 uint32_t HELPER(get_r13_banked)(CPUARMState *env, uint32_t mode)
1920 {
1921     if ((env->uncached_cpsr & CPSR_M) == mode) {
1922         return env->regs[13];
1923     } else {
1924         return env->banked_r13[bank_number(env, mode)];
1925     }
1926 }
1927
1928 uint32_t HELPER(v7m_mrs)(CPUARMState *env, uint32_t reg)
1929 {
1930     switch (reg) {
1931     case 0: /* APSR */
1932         return xpsr_read(env) & 0xf8000000;
1933     case 1: /* IAPSR */
1934         return xpsr_read(env) & 0xf80001ff;
1935     case 2: /* EAPSR */
1936         return xpsr_read(env) & 0xff00fc00;
1937     case 3: /* xPSR */
1938         return xpsr_read(env) & 0xff00fdff;
1939     case 5: /* IPSR */
1940         return xpsr_read(env) & 0x000001ff;
1941     case 6: /* EPSR */
1942         return xpsr_read(env) & 0x0700fc00;
1943     case 7: /* IEPSR */
1944         return xpsr_read(env) & 0x0700edff;
1945     case 8: /* MSP */
1946         return env->v7m.current_sp ? env->v7m.other_sp : env->regs[13];
1947     case 9: /* PSP */
1948         return env->v7m.current_sp ? env->regs[13] : env->v7m.other_sp;
1949     case 16: /* PRIMASK */
1950         return (env->uncached_cpsr & CPSR_I) != 0;
1951     case 17: /* BASEPRI */
1952     case 18: /* BASEPRI_MAX */
1953         return env->v7m.basepri;
1954     case 19: /* FAULTMASK */
1955         return (env->uncached_cpsr & CPSR_F) != 0;
1956     case 20: /* CONTROL */
1957         return env->v7m.control;
1958     default:
1959         /* ??? For debugging only.  */
1960         cpu_abort(env, "Unimplemented system register read (%d)\n", reg);
1961         return 0;
1962     }
1963 }
1964
1965 void HELPER(v7m_msr)(CPUARMState *env, uint32_t reg, uint32_t val)
1966 {
1967     switch (reg) {
1968     case 0: /* APSR */
1969         xpsr_write(env, val, 0xf8000000);
1970         break;
1971     case 1: /* IAPSR */
1972         xpsr_write(env, val, 0xf8000000);
1973         break;
1974     case 2: /* EAPSR */
1975         xpsr_write(env, val, 0xfe00fc00);
1976         break;
1977     case 3: /* xPSR */
1978         xpsr_write(env, val, 0xfe00fc00);
1979         break;
1980     case 5: /* IPSR */
1981         /* IPSR bits are readonly.  */
1982         break;
1983     case 6: /* EPSR */
1984         xpsr_write(env, val, 0x0600fc00);
1985         break;
1986     case 7: /* IEPSR */
1987         xpsr_write(env, val, 0x0600fc00);
1988         break;
1989     case 8: /* MSP */
1990         if (env->v7m.current_sp)
1991             env->v7m.other_sp = val;
1992         else
1993             env->regs[13] = val;
1994         break;
1995     case 9: /* PSP */
1996         if (env->v7m.current_sp)
1997             env->regs[13] = val;
1998         else
1999             env->v7m.other_sp = val;
2000         break;
2001     case 16: /* PRIMASK */
2002         if (val & 1)
2003             env->uncached_cpsr |= CPSR_I;
2004         else
2005             env->uncached_cpsr &= ~CPSR_I;
2006         break;
2007     case 17: /* BASEPRI */
2008         env->v7m.basepri = val & 0xff;
2009         break;
2010     case 18: /* BASEPRI_MAX */
2011         val &= 0xff;
2012         if (val != 0 && (val < env->v7m.basepri || env->v7m.basepri == 0))
2013             env->v7m.basepri = val;
2014         break;
2015     case 19: /* FAULTMASK */
2016         if (val & 1)
2017             env->uncached_cpsr |= CPSR_F;
2018         else
2019             env->uncached_cpsr &= ~CPSR_F;
2020         break;
2021     case 20: /* CONTROL */
2022         env->v7m.control = val & 3;
2023         switch_v7m_sp(env, (val & 2) != 0);
2024         break;
2025     default:
2026         /* ??? For debugging only.  */
2027         cpu_abort(env, "Unimplemented system register write (%d)\n", reg);
2028         return;
2029     }
2030 }
2031
2032 void cpu_arm_set_cp_io(CPUARMState *env, int cpnum,
2033                 ARMReadCPFunc *cp_read, ARMWriteCPFunc *cp_write,
2034                 void *opaque)
2035 {
2036     if (cpnum < 0 || cpnum > 14) {
2037         cpu_abort(env, "Bad coprocessor number: %i\n", cpnum);
2038         return;
2039     }
2040
2041     env->cp[cpnum].cp_read = cp_read;
2042     env->cp[cpnum].cp_write = cp_write;
2043     env->cp[cpnum].opaque = opaque;
2044 }
2045
2046 #endif
2047
2048 /* Note that signed overflow is undefined in C.  The following routines are
2049    careful to use unsigned types where modulo arithmetic is required.
2050    Failure to do so _will_ break on newer gcc.  */
2051
2052 /* Signed saturating arithmetic.  */
2053
2054 /* Perform 16-bit signed saturating addition.  */
2055 static inline uint16_t add16_sat(uint16_t a, uint16_t b)
2056 {
2057     uint16_t res;
2058
2059     res = a + b;
2060     if (((res ^ a) & 0x8000) && !((a ^ b) & 0x8000)) {
2061         if (a & 0x8000)
2062             res = 0x8000;
2063         else
2064             res = 0x7fff;
2065     }
2066     return res;
2067 }
2068
2069 /* Perform 8-bit signed saturating addition.  */
2070 static inline uint8_t add8_sat(uint8_t a, uint8_t b)
2071 {
2072     uint8_t res;
2073
2074     res = a + b;
2075     if (((res ^ a) & 0x80) && !((a ^ b) & 0x80)) {
2076         if (a & 0x80)
2077             res = 0x80;
2078         else
2079             res = 0x7f;
2080     }
2081     return res;
2082 }
2083
2084 /* Perform 16-bit signed saturating subtraction.  */
2085 static inline uint16_t sub16_sat(uint16_t a, uint16_t b)
2086 {
2087     uint16_t res;
2088
2089     res = a - b;
2090     if (((res ^ a) & 0x8000) && ((a ^ b) & 0x8000)) {
2091         if (a & 0x8000)
2092             res = 0x8000;
2093         else
2094             res = 0x7fff;
2095     }
2096     return res;
2097 }
2098
2099 /* Perform 8-bit signed saturating subtraction.  */
2100 static inline uint8_t sub8_sat(uint8_t a, uint8_t b)
2101 {
2102     uint8_t res;
2103
2104     res = a - b;
2105     if (((res ^ a) & 0x80) && ((a ^ b) & 0x80)) {
2106         if (a & 0x80)
2107             res = 0x80;
2108         else
2109             res = 0x7f;
2110     }
2111     return res;
2112 }
2113
2114 #define ADD16(a, b, n) RESULT(add16_sat(a, b), n, 16);
2115 #define SUB16(a, b, n) RESULT(sub16_sat(a, b), n, 16);
2116 #define ADD8(a, b, n)  RESULT(add8_sat(a, b), n, 8);
2117 #define SUB8(a, b, n)  RESULT(sub8_sat(a, b), n, 8);
2118 #define PFX q
2119
2120 #include "op_addsub.h"
2121
2122 /* Unsigned saturating arithmetic.  */
2123 static inline uint16_t add16_usat(uint16_t a, uint16_t b)
2124 {
2125     uint16_t res;
2126     res = a + b;
2127     if (res < a)
2128         res = 0xffff;
2129     return res;
2130 }
2131
2132 static inline uint16_t sub16_usat(uint16_t a, uint16_t b)
2133 {
2134     if (a > b)
2135         return a - b;
2136     else
2137         return 0;
2138 }
2139
2140 static inline uint8_t add8_usat(uint8_t a, uint8_t b)
2141 {
2142     uint8_t res;
2143     res = a + b;
2144     if (res < a)
2145         res = 0xff;
2146     return res;
2147 }
2148
2149 static inline uint8_t sub8_usat(uint8_t a, uint8_t b)
2150 {
2151     if (a > b)
2152         return a - b;
2153     else
2154         return 0;
2155 }
2156
2157 #define ADD16(a, b, n) RESULT(add16_usat(a, b), n, 16);
2158 #define SUB16(a, b, n) RESULT(sub16_usat(a, b), n, 16);
2159 #define ADD8(a, b, n)  RESULT(add8_usat(a, b), n, 8);
2160 #define SUB8(a, b, n)  RESULT(sub8_usat(a, b), n, 8);
2161 #define PFX uq
2162
2163 #include "op_addsub.h"
2164
2165 /* Signed modulo arithmetic.  */
2166 #define SARITH16(a, b, n, op) do { \
2167     int32_t sum; \
2168     sum = (int32_t)(int16_t)(a) op (int32_t)(int16_t)(b); \
2169     RESULT(sum, n, 16); \
2170     if (sum >= 0) \
2171         ge |= 3 << (n * 2); \
2172     } while(0)
2173
2174 #define SARITH8(a, b, n, op) do { \
2175     int32_t sum; \
2176     sum = (int32_t)(int8_t)(a) op (int32_t)(int8_t)(b); \
2177     RESULT(sum, n, 8); \
2178     if (sum >= 0) \
2179         ge |= 1 << n; \
2180     } while(0)
2181
2182
2183 #define ADD16(a, b, n) SARITH16(a, b, n, +)
2184 #define SUB16(a, b, n) SARITH16(a, b, n, -)
2185 #define ADD8(a, b, n)  SARITH8(a, b, n, +)
2186 #define SUB8(a, b, n)  SARITH8(a, b, n, -)
2187 #define PFX s
2188 #define ARITH_GE
2189
2190 #include "op_addsub.h"
2191
2192 /* Unsigned modulo arithmetic.  */
2193 #define ADD16(a, b, n) do { \
2194     uint32_t sum; \
2195     sum = (uint32_t)(uint16_t)(a) + (uint32_t)(uint16_t)(b); \
2196     RESULT(sum, n, 16); \
2197     if ((sum >> 16) == 1) \
2198         ge |= 3 << (n * 2); \
2199     } while(0)
2200
2201 #define ADD8(a, b, n) do { \
2202     uint32_t sum; \
2203     sum = (uint32_t)(uint8_t)(a) + (uint32_t)(uint8_t)(b); \
2204     RESULT(sum, n, 8); \
2205     if ((sum >> 8) == 1) \
2206         ge |= 1 << n; \
2207     } while(0)
2208
2209 #define SUB16(a, b, n) do { \
2210     uint32_t sum; \
2211     sum = (uint32_t)(uint16_t)(a) - (uint32_t)(uint16_t)(b); \
2212     RESULT(sum, n, 16); \
2213     if ((sum >> 16) == 0) \
2214         ge |= 3 << (n * 2); \
2215     } while(0)
2216
2217 #define SUB8(a, b, n) do { \
2218     uint32_t sum; \
2219     sum = (uint32_t)(uint8_t)(a) - (uint32_t)(uint8_t)(b); \
2220     RESULT(sum, n, 8); \
2221     if ((sum >> 8) == 0) \
2222         ge |= 1 << n; \
2223     } while(0)
2224
2225 #define PFX u
2226 #define ARITH_GE
2227
2228 #include "op_addsub.h"
2229
2230 /* Halved signed arithmetic.  */
2231 #define ADD16(a, b, n) \
2232   RESULT(((int32_t)(int16_t)(a) + (int32_t)(int16_t)(b)) >> 1, n, 16)
2233 #define SUB16(a, b, n) \
2234   RESULT(((int32_t)(int16_t)(a) - (int32_t)(int16_t)(b)) >> 1, n, 16)
2235 #define ADD8(a, b, n) \
2236   RESULT(((int32_t)(int8_t)(a) + (int32_t)(int8_t)(b)) >> 1, n, 8)
2237 #define SUB8(a, b, n) \
2238   RESULT(((int32_t)(int8_t)(a) - (int32_t)(int8_t)(b)) >> 1, n, 8)
2239 #define PFX sh
2240
2241 #include "op_addsub.h"
2242
2243 /* Halved unsigned arithmetic.  */
2244 #define ADD16(a, b, n) \
2245   RESULT(((uint32_t)(uint16_t)(a) + (uint32_t)(uint16_t)(b)) >> 1, n, 16)
2246 #define SUB16(a, b, n) \
2247   RESULT(((uint32_t)(uint16_t)(a) - (uint32_t)(uint16_t)(b)) >> 1, n, 16)
2248 #define ADD8(a, b, n) \
2249   RESULT(((uint32_t)(uint8_t)(a) + (uint32_t)(uint8_t)(b)) >> 1, n, 8)
2250 #define SUB8(a, b, n) \
2251   RESULT(((uint32_t)(uint8_t)(a) - (uint32_t)(uint8_t)(b)) >> 1, n, 8)
2252 #define PFX uh
2253
2254 #include "op_addsub.h"
2255
2256 static inline uint8_t do_usad(uint8_t a, uint8_t b)
2257 {
2258     if (a > b)
2259         return a - b;
2260     else
2261         return b - a;
2262 }
2263
2264 /* Unsigned sum of absolute byte differences.  */
2265 uint32_t HELPER(usad8)(uint32_t a, uint32_t b)
2266 {
2267     uint32_t sum;
2268     sum = do_usad(a, b);
2269     sum += do_usad(a >> 8, b >> 8);
2270     sum += do_usad(a >> 16, b >>16);
2271     sum += do_usad(a >> 24, b >> 24);
2272     return sum;
2273 }
2274
2275 /* For ARMv6 SEL instruction.  */
2276 uint32_t HELPER(sel_flags)(uint32_t flags, uint32_t a, uint32_t b)
2277 {
2278     uint32_t mask;
2279
2280     mask = 0;
2281     if (flags & 1)
2282         mask |= 0xff;
2283     if (flags & 2)
2284         mask |= 0xff00;
2285     if (flags & 4)
2286         mask |= 0xff0000;
2287     if (flags & 8)
2288         mask |= 0xff000000;
2289     return (a & mask) | (b & ~mask);
2290 }
2291
2292 uint32_t HELPER(logicq_cc)(uint64_t val)
2293 {
2294     return (val >> 32) | (val != 0);
2295 }
2296
2297 /* VFP support.  We follow the convention used for VFP instrunctions:
2298    Single precition routines have a "s" suffix, double precision a
2299    "d" suffix.  */
2300
2301 /* Convert host exception flags to vfp form.  */
2302 static inline int vfp_exceptbits_from_host(int host_bits)
2303 {
2304     int target_bits = 0;
2305
2306     if (host_bits & float_flag_invalid)
2307         target_bits |= 1;
2308     if (host_bits & float_flag_divbyzero)
2309         target_bits |= 2;
2310     if (host_bits & float_flag_overflow)
2311         target_bits |= 4;
2312     if (host_bits & (float_flag_underflow | float_flag_output_denormal))
2313         target_bits |= 8;
2314     if (host_bits & float_flag_inexact)
2315         target_bits |= 0x10;
2316     if (host_bits & float_flag_input_denormal)
2317         target_bits |= 0x80;
2318     return target_bits;
2319 }
2320
2321 uint32_t HELPER(vfp_get_fpscr)(CPUARMState *env)
2322 {
2323     int i;
2324     uint32_t fpscr;
2325
2326     fpscr = (env->vfp.xregs[ARM_VFP_FPSCR] & 0xffc8ffff)
2327             | (env->vfp.vec_len << 16)
2328             | (env->vfp.vec_stride << 20);
2329     i = get_float_exception_flags(&env->vfp.fp_status);
2330     i |= get_float_exception_flags(&env->vfp.standard_fp_status);
2331     fpscr |= vfp_exceptbits_from_host(i);
2332     return fpscr;
2333 }
2334
2335 uint32_t vfp_get_fpscr(CPUARMState *env)
2336 {
2337     return HELPER(vfp_get_fpscr)(env);
2338 }
2339
2340 /* Convert vfp exception flags to target form.  */
2341 static inline int vfp_exceptbits_to_host(int target_bits)
2342 {
2343     int host_bits = 0;
2344
2345     if (target_bits & 1)
2346         host_bits |= float_flag_invalid;
2347     if (target_bits & 2)
2348         host_bits |= float_flag_divbyzero;
2349     if (target_bits & 4)
2350         host_bits |= float_flag_overflow;
2351     if (target_bits & 8)
2352         host_bits |= float_flag_underflow;
2353     if (target_bits & 0x10)
2354         host_bits |= float_flag_inexact;
2355     if (target_bits & 0x80)
2356         host_bits |= float_flag_input_denormal;
2357     return host_bits;
2358 }
2359
2360 void HELPER(vfp_set_fpscr)(CPUARMState *env, uint32_t val)
2361 {
2362     int i;
2363     uint32_t changed;
2364
2365     changed = env->vfp.xregs[ARM_VFP_FPSCR];
2366     env->vfp.xregs[ARM_VFP_FPSCR] = (val & 0xffc8ffff);
2367     env->vfp.vec_len = (val >> 16) & 7;
2368     env->vfp.vec_stride = (val >> 20) & 3;
2369
2370     changed ^= val;
2371     if (changed & (3 << 22)) {
2372         i = (val >> 22) & 3;
2373         switch (i) {
2374         case 0:
2375             i = float_round_nearest_even;
2376             break;
2377         case 1:
2378             i = float_round_up;
2379             break;
2380         case 2:
2381             i = float_round_down;
2382             break;
2383         case 3:
2384             i = float_round_to_zero;
2385             break;
2386         }
2387         set_float_rounding_mode(i, &env->vfp.fp_status);
2388     }
2389     if (changed & (1 << 24)) {
2390         set_flush_to_zero((val & (1 << 24)) != 0, &env->vfp.fp_status);
2391         set_flush_inputs_to_zero((val & (1 << 24)) != 0, &env->vfp.fp_status);
2392     }
2393     if (changed & (1 << 25))
2394         set_default_nan_mode((val & (1 << 25)) != 0, &env->vfp.fp_status);
2395
2396     i = vfp_exceptbits_to_host(val);
2397     set_float_exception_flags(i, &env->vfp.fp_status);
2398     set_float_exception_flags(0, &env->vfp.standard_fp_status);
2399 }
2400
2401 void vfp_set_fpscr(CPUARMState *env, uint32_t val)
2402 {
2403     HELPER(vfp_set_fpscr)(env, val);
2404 }
2405
2406 #define VFP_HELPER(name, p) HELPER(glue(glue(vfp_,name),p))
2407
2408 #define VFP_BINOP(name) \
2409 float32 VFP_HELPER(name, s)(float32 a, float32 b, void *fpstp) \
2410 { \
2411     float_status *fpst = fpstp; \
2412     return float32_ ## name(a, b, fpst); \
2413 } \
2414 float64 VFP_HELPER(name, d)(float64 a, float64 b, void *fpstp) \
2415 { \
2416     float_status *fpst = fpstp; \
2417     return float64_ ## name(a, b, fpst); \
2418 }
2419 VFP_BINOP(add)
2420 VFP_BINOP(sub)
2421 VFP_BINOP(mul)
2422 VFP_BINOP(div)
2423 #undef VFP_BINOP
2424
2425 float32 VFP_HELPER(neg, s)(float32 a)
2426 {
2427     return float32_chs(a);
2428 }
2429
2430 float64 VFP_HELPER(neg, d)(float64 a)
2431 {
2432     return float64_chs(a);
2433 }
2434
2435 float32 VFP_HELPER(abs, s)(float32 a)
2436 {
2437     return float32_abs(a);
2438 }
2439
2440 float64 VFP_HELPER(abs, d)(float64 a)
2441 {
2442     return float64_abs(a);
2443 }
2444
2445 float32 VFP_HELPER(sqrt, s)(float32 a, CPUARMState *env)
2446 {
2447     return float32_sqrt(a, &env->vfp.fp_status);
2448 }
2449
2450 float64 VFP_HELPER(sqrt, d)(float64 a, CPUARMState *env)
2451 {
2452     return float64_sqrt(a, &env->vfp.fp_status);
2453 }
2454
2455 /* XXX: check quiet/signaling case */
2456 #define DO_VFP_cmp(p, type) \
2457 void VFP_HELPER(cmp, p)(type a, type b, CPUARMState *env)  \
2458 { \
2459     uint32_t flags; \
2460     switch(type ## _compare_quiet(a, b, &env->vfp.fp_status)) { \
2461     case 0: flags = 0x6; break; \
2462     case -1: flags = 0x8; break; \
2463     case 1: flags = 0x2; break; \
2464     default: case 2: flags = 0x3; break; \
2465     } \
2466     env->vfp.xregs[ARM_VFP_FPSCR] = (flags << 28) \
2467         | (env->vfp.xregs[ARM_VFP_FPSCR] & 0x0fffffff); \
2468 } \
2469 void VFP_HELPER(cmpe, p)(type a, type b, CPUARMState *env) \
2470 { \
2471     uint32_t flags; \
2472     switch(type ## _compare(a, b, &env->vfp.fp_status)) { \
2473     case 0: flags = 0x6; break; \
2474     case -1: flags = 0x8; break; \
2475     case 1: flags = 0x2; break; \
2476     default: case 2: flags = 0x3; break; \
2477     } \
2478     env->vfp.xregs[ARM_VFP_FPSCR] = (flags << 28) \
2479         | (env->vfp.xregs[ARM_VFP_FPSCR] & 0x0fffffff); \
2480 }
2481 DO_VFP_cmp(s, float32)
2482 DO_VFP_cmp(d, float64)
2483 #undef DO_VFP_cmp
2484
2485 /* Integer to float and float to integer conversions */
2486
2487 #define CONV_ITOF(name, fsz, sign) \
2488     float##fsz HELPER(name)(uint32_t x, void *fpstp) \
2489 { \
2490     float_status *fpst = fpstp; \
2491     return sign##int32_to_##float##fsz((sign##int32_t)x, fpst); \
2492 }
2493
2494 #define CONV_FTOI(name, fsz, sign, round) \
2495 uint32_t HELPER(name)(float##fsz x, void *fpstp) \
2496 { \
2497     float_status *fpst = fpstp; \
2498     if (float##fsz##_is_any_nan(x)) { \
2499         float_raise(float_flag_invalid, fpst); \
2500         return 0; \
2501     } \
2502     return float##fsz##_to_##sign##int32##round(x, fpst); \
2503 }
2504
2505 #define FLOAT_CONVS(name, p, fsz, sign) \
2506 CONV_ITOF(vfp_##name##to##p, fsz, sign) \
2507 CONV_FTOI(vfp_to##name##p, fsz, sign, ) \
2508 CONV_FTOI(vfp_to##name##z##p, fsz, sign, _round_to_zero)
2509
2510 FLOAT_CONVS(si, s, 32, )
2511 FLOAT_CONVS(si, d, 64, )
2512 FLOAT_CONVS(ui, s, 32, u)
2513 FLOAT_CONVS(ui, d, 64, u)
2514
2515 #undef CONV_ITOF
2516 #undef CONV_FTOI
2517 #undef FLOAT_CONVS
2518
2519 /* floating point conversion */
2520 float64 VFP_HELPER(fcvtd, s)(float32 x, CPUARMState *env)
2521 {
2522     float64 r = float32_to_float64(x, &env->vfp.fp_status);
2523     /* ARM requires that S<->D conversion of any kind of NaN generates
2524      * a quiet NaN by forcing the most significant frac bit to 1.
2525      */
2526     return float64_maybe_silence_nan(r);
2527 }
2528
2529 float32 VFP_HELPER(fcvts, d)(float64 x, CPUARMState *env)
2530 {
2531     float32 r =  float64_to_float32(x, &env->vfp.fp_status);
2532     /* ARM requires that S<->D conversion of any kind of NaN generates
2533      * a quiet NaN by forcing the most significant frac bit to 1.
2534      */
2535     return float32_maybe_silence_nan(r);
2536 }
2537
2538 /* VFP3 fixed point conversion.  */
2539 #define VFP_CONV_FIX(name, p, fsz, itype, sign) \
2540 float##fsz HELPER(vfp_##name##to##p)(uint##fsz##_t  x, uint32_t shift, \
2541                                     void *fpstp) \
2542 { \
2543     float_status *fpst = fpstp; \
2544     float##fsz tmp; \
2545     tmp = sign##int32_to_##float##fsz((itype##_t)x, fpst); \
2546     return float##fsz##_scalbn(tmp, -(int)shift, fpst); \
2547 } \
2548 uint##fsz##_t HELPER(vfp_to##name##p)(float##fsz x, uint32_t shift, \
2549                                        void *fpstp) \
2550 { \
2551     float_status *fpst = fpstp; \
2552     float##fsz tmp; \
2553     if (float##fsz##_is_any_nan(x)) { \
2554         float_raise(float_flag_invalid, fpst); \
2555         return 0; \
2556     } \
2557     tmp = float##fsz##_scalbn(x, shift, fpst); \
2558     return float##fsz##_to_##itype##_round_to_zero(tmp, fpst); \
2559 }
2560
2561 VFP_CONV_FIX(sh, d, 64, int16, )
2562 VFP_CONV_FIX(sl, d, 64, int32, )
2563 VFP_CONV_FIX(uh, d, 64, uint16, u)
2564 VFP_CONV_FIX(ul, d, 64, uint32, u)
2565 VFP_CONV_FIX(sh, s, 32, int16, )
2566 VFP_CONV_FIX(sl, s, 32, int32, )
2567 VFP_CONV_FIX(uh, s, 32, uint16, u)
2568 VFP_CONV_FIX(ul, s, 32, uint32, u)
2569 #undef VFP_CONV_FIX
2570
2571 /* Half precision conversions.  */
2572 static float32 do_fcvt_f16_to_f32(uint32_t a, CPUARMState *env, float_status *s)
2573 {
2574     int ieee = (env->vfp.xregs[ARM_VFP_FPSCR] & (1 << 26)) == 0;
2575     float32 r = float16_to_float32(make_float16(a), ieee, s);
2576     if (ieee) {
2577         return float32_maybe_silence_nan(r);
2578     }
2579     return r;
2580 }
2581
2582 static uint32_t do_fcvt_f32_to_f16(float32 a, CPUARMState *env, float_status *s)
2583 {
2584     int ieee = (env->vfp.xregs[ARM_VFP_FPSCR] & (1 << 26)) == 0;
2585     float16 r = float32_to_float16(a, ieee, s);
2586     if (ieee) {
2587         r = float16_maybe_silence_nan(r);
2588     }
2589     return float16_val(r);
2590 }
2591
2592 float32 HELPER(neon_fcvt_f16_to_f32)(uint32_t a, CPUARMState *env)
2593 {
2594     return do_fcvt_f16_to_f32(a, env, &env->vfp.standard_fp_status);
2595 }
2596
2597 uint32_t HELPER(neon_fcvt_f32_to_f16)(float32 a, CPUARMState *env)
2598 {
2599     return do_fcvt_f32_to_f16(a, env, &env->vfp.standard_fp_status);
2600 }
2601
2602 float32 HELPER(vfp_fcvt_f16_to_f32)(uint32_t a, CPUARMState *env)
2603 {
2604     return do_fcvt_f16_to_f32(a, env, &env->vfp.fp_status);
2605 }
2606
2607 uint32_t HELPER(vfp_fcvt_f32_to_f16)(float32 a, CPUARMState *env)
2608 {
2609     return do_fcvt_f32_to_f16(a, env, &env->vfp.fp_status);
2610 }
2611
2612 #define float32_two make_float32(0x40000000)
2613 #define float32_three make_float32(0x40400000)
2614 #define float32_one_point_five make_float32(0x3fc00000)
2615
2616 float32 HELPER(recps_f32)(float32 a, float32 b, CPUARMState *env)
2617 {
2618     float_status *s = &env->vfp.standard_fp_status;
2619     if ((float32_is_infinity(a) && float32_is_zero_or_denormal(b)) ||
2620         (float32_is_infinity(b) && float32_is_zero_or_denormal(a))) {
2621         if (!(float32_is_zero(a) || float32_is_zero(b))) {
2622             float_raise(float_flag_input_denormal, s);
2623         }
2624         return float32_two;
2625     }
2626     return float32_sub(float32_two, float32_mul(a, b, s), s);
2627 }
2628
2629 float32 HELPER(rsqrts_f32)(float32 a, float32 b, CPUARMState *env)
2630 {
2631     float_status *s = &env->vfp.standard_fp_status;
2632     float32 product;
2633     if ((float32_is_infinity(a) && float32_is_zero_or_denormal(b)) ||
2634         (float32_is_infinity(b) && float32_is_zero_or_denormal(a))) {
2635         if (!(float32_is_zero(a) || float32_is_zero(b))) {
2636             float_raise(float_flag_input_denormal, s);
2637         }
2638         return float32_one_point_five;
2639     }
2640     product = float32_mul(a, b, s);
2641     return float32_div(float32_sub(float32_three, product, s), float32_two, s);
2642 }
2643
2644 /* NEON helpers.  */
2645
2646 /* Constants 256 and 512 are used in some helpers; we avoid relying on
2647  * int->float conversions at run-time.  */
2648 #define float64_256 make_float64(0x4070000000000000LL)
2649 #define float64_512 make_float64(0x4080000000000000LL)
2650
2651 /* The algorithm that must be used to calculate the estimate
2652  * is specified by the ARM ARM.
2653  */
2654 static float64 recip_estimate(float64 a, CPUARMState *env)
2655 {
2656     /* These calculations mustn't set any fp exception flags,
2657      * so we use a local copy of the fp_status.
2658      */
2659     float_status dummy_status = env->vfp.standard_fp_status;
2660     float_status *s = &dummy_status;
2661     /* q = (int)(a * 512.0) */
2662     float64 q = float64_mul(float64_512, a, s);
2663     int64_t q_int = float64_to_int64_round_to_zero(q, s);
2664
2665     /* r = 1.0 / (((double)q + 0.5) / 512.0) */
2666     q = int64_to_float64(q_int, s);
2667     q = float64_add(q, float64_half, s);
2668     q = float64_div(q, float64_512, s);
2669     q = float64_div(float64_one, q, s);
2670
2671     /* s = (int)(256.0 * r + 0.5) */
2672     q = float64_mul(q, float64_256, s);
2673     q = float64_add(q, float64_half, s);
2674     q_int = float64_to_int64_round_to_zero(q, s);
2675
2676     /* return (double)s / 256.0 */
2677     return float64_div(int64_to_float64(q_int, s), float64_256, s);
2678 }
2679
2680 float32 HELPER(recpe_f32)(float32 a, CPUARMState *env)
2681 {
2682     float_status *s = &env->vfp.standard_fp_status;
2683     float64 f64;
2684     uint32_t val32 = float32_val(a);
2685
2686     int result_exp;
2687     int a_exp = (val32  & 0x7f800000) >> 23;
2688     int sign = val32 & 0x80000000;
2689
2690     if (float32_is_any_nan(a)) {
2691         if (float32_is_signaling_nan(a)) {
2692             float_raise(float_flag_invalid, s);
2693         }
2694         return float32_default_nan;
2695     } else if (float32_is_infinity(a)) {
2696         return float32_set_sign(float32_zero, float32_is_neg(a));
2697     } else if (float32_is_zero_or_denormal(a)) {
2698         if (!float32_is_zero(a)) {
2699             float_raise(float_flag_input_denormal, s);
2700         }
2701         float_raise(float_flag_divbyzero, s);
2702         return float32_set_sign(float32_infinity, float32_is_neg(a));
2703     } else if (a_exp >= 253) {
2704         float_raise(float_flag_underflow, s);
2705         return float32_set_sign(float32_zero, float32_is_neg(a));
2706     }
2707
2708     f64 = make_float64((0x3feULL << 52)
2709                        | ((int64_t)(val32 & 0x7fffff) << 29));
2710
2711     result_exp = 253 - a_exp;
2712
2713     f64 = recip_estimate(f64, env);
2714
2715     val32 = sign
2716         | ((result_exp & 0xff) << 23)
2717         | ((float64_val(f64) >> 29) & 0x7fffff);
2718     return make_float32(val32);
2719 }
2720
2721 /* The algorithm that must be used to calculate the estimate
2722  * is specified by the ARM ARM.
2723  */
2724 static float64 recip_sqrt_estimate(float64 a, CPUARMState *env)
2725 {
2726     /* These calculations mustn't set any fp exception flags,
2727      * so we use a local copy of the fp_status.
2728      */
2729     float_status dummy_status = env->vfp.standard_fp_status;
2730     float_status *s = &dummy_status;
2731     float64 q;
2732     int64_t q_int;
2733
2734     if (float64_lt(a, float64_half, s)) {
2735         /* range 0.25 <= a < 0.5 */
2736
2737         /* a in units of 1/512 rounded down */
2738         /* q0 = (int)(a * 512.0);  */
2739         q = float64_mul(float64_512, a, s);
2740         q_int = float64_to_int64_round_to_zero(q, s);
2741
2742         /* reciprocal root r */
2743         /* r = 1.0 / sqrt(((double)q0 + 0.5) / 512.0);  */
2744         q = int64_to_float64(q_int, s);
2745         q = float64_add(q, float64_half, s);
2746         q = float64_div(q, float64_512, s);
2747         q = float64_sqrt(q, s);
2748         q = float64_div(float64_one, q, s);
2749     } else {
2750         /* range 0.5 <= a < 1.0 */
2751
2752         /* a in units of 1/256 rounded down */
2753         /* q1 = (int)(a * 256.0); */
2754         q = float64_mul(float64_256, a, s);
2755         int64_t q_int = float64_to_int64_round_to_zero(q, s);
2756
2757         /* reciprocal root r */
2758         /* r = 1.0 /sqrt(((double)q1 + 0.5) / 256); */
2759         q = int64_to_float64(q_int, s);
2760         q = float64_add(q, float64_half, s);
2761         q = float64_div(q, float64_256, s);
2762         q = float64_sqrt(q, s);
2763         q = float64_div(float64_one, q, s);
2764     }
2765     /* r in units of 1/256 rounded to nearest */
2766     /* s = (int)(256.0 * r + 0.5); */
2767
2768     q = float64_mul(q, float64_256,s );
2769     q = float64_add(q, float64_half, s);
2770     q_int = float64_to_int64_round_to_zero(q, s);
2771
2772     /* return (double)s / 256.0;*/
2773     return float64_div(int64_to_float64(q_int, s), float64_256, s);
2774 }
2775
2776 float32 HELPER(rsqrte_f32)(float32 a, CPUARMState *env)
2777 {
2778     float_status *s = &env->vfp.standard_fp_status;
2779     int result_exp;
2780     float64 f64;
2781     uint32_t val;
2782     uint64_t val64;
2783
2784     val = float32_val(a);
2785
2786     if (float32_is_any_nan(a)) {
2787         if (float32_is_signaling_nan(a)) {
2788             float_raise(float_flag_invalid, s);
2789         }
2790         return float32_default_nan;
2791     } else if (float32_is_zero_or_denormal(a)) {
2792         if (!float32_is_zero(a)) {
2793             float_raise(float_flag_input_denormal, s);
2794         }
2795         float_raise(float_flag_divbyzero, s);
2796         return float32_set_sign(float32_infinity, float32_is_neg(a));
2797     } else if (float32_is_neg(a)) {
2798         float_raise(float_flag_invalid, s);
2799         return float32_default_nan;
2800     } else if (float32_is_infinity(a)) {
2801         return float32_zero;
2802     }
2803
2804     /* Normalize to a double-precision value between 0.25 and 1.0,
2805      * preserving the parity of the exponent.  */
2806     if ((val & 0x800000) == 0) {
2807         f64 = make_float64(((uint64_t)(val & 0x80000000) << 32)
2808                            | (0x3feULL << 52)
2809                            | ((uint64_t)(val & 0x7fffff) << 29));
2810     } else {
2811         f64 = make_float64(((uint64_t)(val & 0x80000000) << 32)
2812                            | (0x3fdULL << 52)
2813                            | ((uint64_t)(val & 0x7fffff) << 29));
2814     }
2815
2816     result_exp = (380 - ((val & 0x7f800000) >> 23)) / 2;
2817
2818     f64 = recip_sqrt_estimate(f64, env);
2819
2820     val64 = float64_val(f64);
2821
2822     val = ((result_exp & 0xff) << 23)
2823         | ((val64 >> 29)  & 0x7fffff);
2824     return make_float32(val);
2825 }
2826
2827 uint32_t HELPER(recpe_u32)(uint32_t a, CPUARMState *env)
2828 {
2829     float64 f64;
2830
2831     if ((a & 0x80000000) == 0) {
2832         return 0xffffffff;
2833     }
2834
2835     f64 = make_float64((0x3feULL << 52)
2836                        | ((int64_t)(a & 0x7fffffff) << 21));
2837
2838     f64 = recip_estimate (f64, env);
2839
2840     return 0x80000000 | ((float64_val(f64) >> 21) & 0x7fffffff);
2841 }
2842
2843 uint32_t HELPER(rsqrte_u32)(uint32_t a, CPUARMState *env)
2844 {
2845     float64 f64;
2846
2847     if ((a & 0xc0000000) == 0) {
2848         return 0xffffffff;
2849     }
2850
2851     if (a & 0x80000000) {
2852         f64 = make_float64((0x3feULL << 52)
2853                            | ((uint64_t)(a & 0x7fffffff) << 21));
2854     } else { /* bits 31-30 == '01' */
2855         f64 = make_float64((0x3fdULL << 52)
2856                            | ((uint64_t)(a & 0x3fffffff) << 22));
2857     }
2858
2859     f64 = recip_sqrt_estimate(f64, env);
2860
2861     return 0x80000000 | ((float64_val(f64) >> 21) & 0x7fffffff);
2862 }
2863
2864 /* VFPv4 fused multiply-accumulate */
2865 float32 VFP_HELPER(muladd, s)(float32 a, float32 b, float32 c, void *fpstp)
2866 {
2867     float_status *fpst = fpstp;
2868     return float32_muladd(a, b, c, 0, fpst);
2869 }
2870
2871 float64 VFP_HELPER(muladd, d)(float64 a, float64 b, float64 c, void *fpstp)
2872 {
2873     float_status *fpst = fpstp;
2874     return float64_muladd(a, b, c, 0, fpst);
2875 }
2876
2877 void HELPER(set_teecr)(CPUARMState *env, uint32_t val)
2878 {
2879     val &= 1;
2880     if (env->teecr != val) {
2881         env->teecr = val;
2882         tb_flush(env);
2883     }
2884 }