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