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