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