target-arm/helper.c

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