target-arm/helper.c

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