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