target-arm/helper.c

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