target-arm/helper.c

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