target-arm/helper.c

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