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