Archipelago » qemu

#include "cpu.h"

#include "gdbstub.h"

#include "helper.h"

#include "host-utils.h"

#if !defined(CONFIG_USER_ONLY)

#include "hw/loader.h"

#endif

#include "sysemu.h"

static void cpu_reset_model_id(CPUARMState *env, uint32_t id)

{

    switch (id) {

    case ARM_CPUID_ARM926:

        break;

    case ARM_CPUID_ARM946:

        break;

    case ARM_CPUID_ARM1026:

        break;

    case ARM_CPUID_ARM1136:

        /* This is the 1136 r1, which is a v6K core */

    case ARM_CPUID_ARM1136_R2:

        break;

    case ARM_CPUID_ARM1176:

        break;

    case ARM_CPUID_ARM11MPCORE:

        break;

    case ARM_CPUID_CORTEXA8:

        break;

    case ARM_CPUID_CORTEXA9:

        break;

    case ARM_CPUID_CORTEXA15:

        break;

    case ARM_CPUID_CORTEXM3:

        break;

    case ARM_CPUID_ANY: /* For userspace emulation.  */

        break;

    case ARM_CPUID_TI915T:

    case ARM_CPUID_TI925T:

        break;

    case ARM_CPUID_PXA250:

    case ARM_CPUID_PXA255:

    case ARM_CPUID_PXA260:

    case ARM_CPUID_PXA261:

    case ARM_CPUID_PXA262:

        break;

    case ARM_CPUID_PXA270_A0:

    case ARM_CPUID_PXA270_A1:

    case ARM_CPUID_PXA270_B0:

    case ARM_CPUID_PXA270_B1:

    case ARM_CPUID_PXA270_C0:

    case ARM_CPUID_PXA270_C5:

        break;

    case ARM_CPUID_SA1100:

    case ARM_CPUID_SA1110:

        break;

    default:

        cpu_abort(env, "Bad CPU ID: %x\n", id);

        break;

    }

}

/* TODO Move contents into arm_cpu_reset() in cpu.c,

 *      once cpu_reset_model_id() is eliminated,

 *      and then forward to cpu_reset() here.

 */

void cpu_state_reset(CPUARMState *env)

{

    uint32_t id;

    uint32_t tmp = 0;

    ARMCPU *cpu = arm_env_get_cpu(env);

    if (qemu_loglevel_mask(CPU_LOG_RESET)) {

        qemu_log("CPU Reset (CPU %d)\n", env->cpu_index);

        log_cpu_state(env, 0);

    }

    id = cpu->midr;

    tmp = env->cp15.c15_config_base_address;

    memset(env, 0, offsetof(CPUARMState, breakpoints));

    if (id)

        cpu_reset_model_id(env, id);

    env->cp15.c15_config_base_address = tmp;

    env->cp15.c0_cpuid = cpu->midr;

    env->vfp.xregs[ARM_VFP_FPSID] = cpu->reset_fpsid;

    env->vfp.xregs[ARM_VFP_MVFR0] = cpu->mvfr0;

    env->vfp.xregs[ARM_VFP_MVFR1] = cpu->mvfr1;

    env->cp15.c0_cachetype = cpu->ctr;

    env->cp15.c1_sys = cpu->reset_sctlr;

    env->cp15.c0_c1[0] = cpu->id_pfr0;

    env->cp15.c0_c1[1] = cpu->id_pfr1;

    env->cp15.c0_c1[2] = cpu->id_dfr0;

    env->cp15.c0_c1[3] = cpu->id_afr0;

    env->cp15.c0_c1[4] = cpu->id_mmfr0;

    env->cp15.c0_c1[5] = cpu->id_mmfr1;

    env->cp15.c0_c1[6] = cpu->id_mmfr2;

    env->cp15.c0_c1[7] = cpu->id_mmfr3;

    env->cp15.c0_c2[0] = cpu->id_isar0;

    env->cp15.c0_c2[1] = cpu->id_isar1;

    env->cp15.c0_c2[2] = cpu->id_isar2;

    env->cp15.c0_c2[3] = cpu->id_isar3;

    env->cp15.c0_c2[4] = cpu->id_isar4;

    env->cp15.c0_c2[5] = cpu->id_isar5;

    env->cp15.c15_i_min = 0xff0;

    env->cp15.c0_clid = cpu->clidr;

    memcpy(env->cp15.c0_ccsid, cpu->ccsidr, ARRAY_SIZE(cpu->ccsidr));

    if (arm_feature(env, ARM_FEATURE_IWMMXT)) {

        env->iwmmxt.cregs[ARM_IWMMXT_wCID] = 0x69051000 | 'Q';

    }

#if defined (CONFIG_USER_ONLY)

    env->uncached_cpsr = ARM_CPU_MODE_USR;

    /* For user mode we must enable access to coprocessors */

    env->vfp.xregs[ARM_VFP_FPEXC] = 1 << 30;

    if (arm_feature(env, ARM_FEATURE_IWMMXT)) {

        env->cp15.c15_cpar = 3;

    } else if (arm_feature(env, ARM_FEATURE_XSCALE)) {

        env->cp15.c15_cpar = 1;

    }

#else

    /* SVC mode with interrupts disabled.  */

    env->uncached_cpsr = ARM_CPU_MODE_SVC | CPSR_A | CPSR_F | CPSR_I;

    /* On ARMv7-M the CPSR_I is the value of the PRIMASK register, and is

       clear at reset.  Initial SP and PC are loaded from ROM.  */

    if (IS_M(env)) {

        uint32_t pc;

        uint8_t *rom;

        env->uncached_cpsr &= ~CPSR_I;

        rom = rom_ptr(0);

        if (rom) {

            /* We should really use ldl_phys here, in case the guest

               modified flash and reset itself.  However images

               loaded via -kernel have not been copied yet, so load the

               values directly from there.  */

            env->regs[13] = ldl_p(rom);

            pc = ldl_p(rom + 4);

            env->thumb = pc & 1;

            env->regs[15] = pc & ~1;

        }

    }

    env->vfp.xregs[ARM_VFP_FPEXC] = 0;

    env->cp15.c2_base_mask = 0xffffc000u;

    /* v7 performance monitor control register: same implementor

     * field as main ID register, and we implement no event counters.

     */

    env->cp15.c9_pmcr = (id & 0xff000000);

#endif

    set_flush_to_zero(1, &env->vfp.standard_fp_status);

    set_flush_inputs_to_zero(1, &env->vfp.standard_fp_status);

    set_default_nan_mode(1, &env->vfp.standard_fp_status);

    set_float_detect_tininess(float_tininess_before_rounding,

                              &env->vfp.fp_status);

    set_float_detect_tininess(float_tininess_before_rounding,

                              &env->vfp.standard_fp_status);

    tlb_flush(env, 1);

    /* Reset is a state change for some CPUARMState fields which we

     * bake assumptions about into translated code, so we need to

     * tb_flush().

     */

    tb_flush(env);

}

static int vfp_gdb_get_reg(CPUARMState *env, uint8_t *buf, int reg)

{

    int nregs;

    /* VFP data registers are always little-endian.  */

    nregs = arm_feature(env, ARM_FEATURE_VFP3) ? 32 : 16;

    if (reg < nregs) {

        stfq_le_p(buf, env->vfp.regs[reg]);

        return 8;

    }

    if (arm_feature(env, ARM_FEATURE_NEON)) {

        /* Aliases for Q regs.  */

        nregs += 16;

        if (reg < nregs) {

            stfq_le_p(buf, env->vfp.regs[(reg - 32) * 2]);

            stfq_le_p(buf + 8, env->vfp.regs[(reg - 32) * 2 + 1]);

            return 16;

        }

    }

    switch (reg - nregs) {

    case 0: stl_p(buf, env->vfp.xregs[ARM_VFP_FPSID]); return 4;

    case 1: stl_p(buf, env->vfp.xregs[ARM_VFP_FPSCR]); return 4;

    case 2: stl_p(buf, env->vfp.xregs[ARM_VFP_FPEXC]); return 4;

    }

    return 0;

}

static int vfp_gdb_set_reg(CPUARMState *env, uint8_t *buf, int reg)

{

    int nregs;

    nregs = arm_feature(env, ARM_FEATURE_VFP3) ? 32 : 16;

    if (reg < nregs) {

        env->vfp.regs[reg] = ldfq_le_p(buf);

        return 8;

    }

    if (arm_feature(env, ARM_FEATURE_NEON)) {

        nregs += 16;

        if (reg < nregs) {

            env->vfp.regs[(reg - 32) * 2] = ldfq_le_p(buf);

            env->vfp.regs[(reg - 32) * 2 + 1] = ldfq_le_p(buf + 8);

            return 16;

        }

    }

    switch (reg - nregs) {

    case 0: env->vfp.xregs[ARM_VFP_FPSID] = ldl_p(buf); return 4;

    case 1: env->vfp.xregs[ARM_VFP_FPSCR] = ldl_p(buf); return 4;

    case 2: env->vfp.xregs[ARM_VFP_FPEXC] = ldl_p(buf) & (1 << 30); return 4;

    }

    return 0;

}

CPUARMState *cpu_arm_init(const char *cpu_model)

{

    ARMCPU *cpu;

    CPUARMState *env;

    static int inited = 0;

    if (!object_class_by_name(cpu_model)) {

        return NULL;

    }

    cpu = ARM_CPU(object_new(cpu_model));

    env = &cpu->env;

    env->cpu_model_str = cpu_model;

    arm_cpu_realize(cpu);

    if (tcg_enabled() && !inited) {

        inited = 1;

        arm_translate_init();

    }

    cpu_state_reset(env);

    if (arm_feature(env, ARM_FEATURE_NEON)) {

        gdb_register_coprocessor(env, vfp_gdb_get_reg, vfp_gdb_set_reg,

                                 51, "arm-neon.xml", 0);

    } else if (arm_feature(env, ARM_FEATURE_VFP3)) {

        gdb_register_coprocessor(env, vfp_gdb_get_reg, vfp_gdb_set_reg,

                                 35, "arm-vfp3.xml", 0);

    } else if (arm_feature(env, ARM_FEATURE_VFP)) {

        gdb_register_coprocessor(env, vfp_gdb_get_reg, vfp_gdb_set_reg,

                                 19, "arm-vfp.xml", 0);

    }

    qemu_init_vcpu(env);

    return env;

}

typedef struct ARMCPUListState {

    fprintf_function cpu_fprintf;

    FILE *file;

} ARMCPUListState;

/* Sort alphabetically by type name, except for "any". */

static gint arm_cpu_list_compare(gconstpointer a, gconstpointer b)

{

    ObjectClass *class_a = (ObjectClass *)a;

    ObjectClass *class_b = (ObjectClass *)b;

    const char *name_a, *name_b;

    name_a = object_class_get_name(class_a);

    name_b = object_class_get_name(class_b);

    if (strcmp(name_a, "any") == 0) {

        return 1;

    } else if (strcmp(name_b, "any") == 0) {

        return -1;

    } else {

        return strcmp(name_a, name_b);

    }

}

static void arm_cpu_list_entry(gpointer data, gpointer user_data)

{

    ObjectClass *oc = data;

    ARMCPUListState *s = user_data;

    (*s->cpu_fprintf)(s->file, "  %s\n",

                      object_class_get_name(oc));

}

void arm_cpu_list(FILE *f, fprintf_function cpu_fprintf)

{

    ARMCPUListState s = {

        .file = f,

        .cpu_fprintf = cpu_fprintf,

    };

    GSList *list;

    list = object_class_get_list(TYPE_ARM_CPU, false);

    list = g_slist_sort(list, arm_cpu_list_compare);

    (*cpu_fprintf)(f, "Available CPUs:\n");

    g_slist_foreach(list, arm_cpu_list_entry, &s);

    g_slist_free(list);

}

static int bad_mode_switch(CPUARMState *env, int mode)

{

    /* Return true if it is not valid for us to switch to

     * this CPU mode (ie all the UNPREDICTABLE cases in

     * the ARM ARM CPSRWriteByInstr pseudocode).

     */

    switch (mode) {

    case ARM_CPU_MODE_USR:

    case ARM_CPU_MODE_SYS:

    case ARM_CPU_MODE_SVC:

    case ARM_CPU_MODE_ABT:

    case ARM_CPU_MODE_UND:

    case ARM_CPU_MODE_IRQ:

    case ARM_CPU_MODE_FIQ:

        return 0;

    default:

        return 1;

    }

}

uint32_t cpsr_read(CPUARMState *env)

{

    int ZF;

    ZF = (env->ZF == 0);

    return env->uncached_cpsr | (env->NF & 0x80000000) | (ZF << 30) |

        (env->CF << 29) | ((env->VF & 0x80000000) >> 3) | (env->QF << 27)

        | (env->thumb << 5) | ((env->condexec_bits & 3) << 25)

        | ((env->condexec_bits & 0xfc) << 8)

        | (env->GE << 16);

}

void cpsr_write(CPUARMState *env, uint32_t val, uint32_t mask)

{

    if (mask & CPSR_NZCV) {

        env->ZF = (~val) & CPSR_Z;

        env->NF = val;

        env->CF = (val >> 29) & 1;

        env->VF = (val << 3) & 0x80000000;

    }

    if (mask & CPSR_Q)

        env->QF = ((val & CPSR_Q) != 0);

    if (mask & CPSR_T)

        env->thumb = ((val & CPSR_T) != 0);

    if (mask & CPSR_IT_0_1) {

        env->condexec_bits &= ~3;

        env->condexec_bits |= (val >> 25) & 3;

    }

    if (mask & CPSR_IT_2_7) {

        env->condexec_bits &= 3;

        env->condexec_bits |= (val >> 8) & 0xfc;

    }

    if (mask & CPSR_GE) {

        env->GE = (val >> 16) & 0xf;

    }

    if ((env->uncached_cpsr ^ val) & mask & CPSR_M) {

        if (bad_mode_switch(env, val & CPSR_M)) {

            /* Attempt to switch to an invalid mode: this is UNPREDICTABLE.

             * We choose to ignore the attempt and leave the CPSR M field

             * untouched.

             */

            mask &= ~CPSR_M;

        } else {

            switch_mode(env, val & CPSR_M);

        }

    }

    mask &= ~CACHED_CPSR_BITS;

    env->uncached_cpsr = (env->uncached_cpsr & ~mask) | (val & mask);

}

/* Sign/zero extend */

uint32_t HELPER(sxtb16)(uint32_t x)

{

    uint32_t res;

    res = (uint16_t)(int8_t)x;

    res |= (uint32_t)(int8_t)(x >> 16) << 16;

    return res;

}

uint32_t HELPER(uxtb16)(uint32_t x)

{

    uint32_t res;

    res = (uint16_t)(uint8_t)x;

    res |= (uint32_t)(uint8_t)(x >> 16) << 16;

    return res;

}

uint32_t HELPER(clz)(uint32_t x)

{

    return clz32(x);

}

int32_t HELPER(sdiv)(int32_t num, int32_t den)

{

    if (den == 0)

      return 0;

    if (num == INT_MIN && den == -1)

      return INT_MIN;

    return num / den;

}

uint32_t HELPER(udiv)(uint32_t num, uint32_t den)

{

    if (den == 0)

      return 0;

    return num / den;

}

uint32_t HELPER(rbit)(uint32_t x)

{

    x =  ((x & 0xff000000) >> 24)

       | ((x & 0x00ff0000) >> 8)

       | ((x & 0x0000ff00) << 8)

       | ((x & 0x000000ff) << 24);

    x =  ((x & 0xf0f0f0f0) >> 4)

       | ((x & 0x0f0f0f0f) << 4);

    x =  ((x & 0x88888888) >> 3)

       | ((x & 0x44444444) >> 1)

       | ((x & 0x22222222) << 1)

       | ((x & 0x11111111) << 3);

    return x;

}

uint32_t HELPER(abs)(uint32_t x)

{

    return ((int32_t)x < 0) ? -x : x;

}

#if defined(CONFIG_USER_ONLY)

void do_interrupt (CPUARMState *env)

{

    env->exception_index = -1;

}

int cpu_arm_handle_mmu_fault (CPUARMState *env, target_ulong address, int rw,

                              int mmu_idx)

{

    if (rw == 2) {

        env->exception_index = EXCP_PREFETCH_ABORT;

        env->cp15.c6_insn = address;

    } else {

        env->exception_index = EXCP_DATA_ABORT;

        env->cp15.c6_data = address;

    }

    return 1;

}

/* These should probably raise undefined insn exceptions.  */

void HELPER(set_cp)(CPUARMState *env, uint32_t insn, uint32_t val)

{

    int op1 = (insn >> 8) & 0xf;

    cpu_abort(env, "cp%i insn %08x\n", op1, insn);

    return;

}

uint32_t HELPER(get_cp)(CPUARMState *env, uint32_t insn)

{

    int op1 = (insn >> 8) & 0xf;

    cpu_abort(env, "cp%i insn %08x\n", op1, insn);

    return 0;

}

void HELPER(set_cp15)(CPUARMState *env, uint32_t insn, uint32_t val)

{

    cpu_abort(env, "cp15 insn %08x\n", insn);

}

uint32_t HELPER(get_cp15)(CPUARMState *env, uint32_t insn)

{

    cpu_abort(env, "cp15 insn %08x\n", insn);

}

/* These should probably raise undefined insn exceptions.  */

void HELPER(v7m_msr)(CPUARMState *env, uint32_t reg, uint32_t val)

{

    cpu_abort(env, "v7m_mrs %d\n", reg);

}

uint32_t HELPER(v7m_mrs)(CPUARMState *env, uint32_t reg)

{

    cpu_abort(env, "v7m_mrs %d\n", reg);

    return 0;

}

void switch_mode(CPUARMState *env, int mode)

{

    if (mode != ARM_CPU_MODE_USR)

        cpu_abort(env, "Tried to switch out of user mode\n");

}

void HELPER(set_r13_banked)(CPUARMState *env, uint32_t mode, uint32_t val)

{

    cpu_abort(env, "banked r13 write\n");

}

uint32_t HELPER(get_r13_banked)(CPUARMState *env, uint32_t mode)

{

    cpu_abort(env, "banked r13 read\n");

    return 0;

}

#else

/* Map CPU modes onto saved register banks.  */

static inline int bank_number(CPUARMState *env, int mode)

{

    switch (mode) {

    case ARM_CPU_MODE_USR:

    case ARM_CPU_MODE_SYS:

        return 0;

    case ARM_CPU_MODE_SVC:

        return 1;

    case ARM_CPU_MODE_ABT:

        return 2;

    case ARM_CPU_MODE_UND:

        return 3;

    case ARM_CPU_MODE_IRQ:

        return 4;

    case ARM_CPU_MODE_FIQ:

        return 5;

    }

    cpu_abort(env, "Bad mode %x\n", mode);

    return -1;

}

void switch_mode(CPUARMState *env, int mode)

{

    int old_mode;

    int i;

    old_mode = env->uncached_cpsr & CPSR_M;

    if (mode == old_mode)

        return;

    if (old_mode == ARM_CPU_MODE_FIQ) {

        memcpy (env->fiq_regs, env->regs + 8, 5 * sizeof(uint32_t));

        memcpy (env->regs + 8, env->usr_regs, 5 * sizeof(uint32_t));

    } else if (mode == ARM_CPU_MODE_FIQ) {

        memcpy (env->usr_regs, env->regs + 8, 5 * sizeof(uint32_t));

        memcpy (env->regs + 8, env->fiq_regs, 5 * sizeof(uint32_t));

    }

    i = bank_number(env, old_mode);

    env->banked_r13[i] = env->regs[13];

    env->banked_r14[i] = env->regs[14];

    env->banked_spsr[i] = env->spsr;

    i = bank_number(env, mode);

    env->regs[13] = env->banked_r13[i];

    env->regs[14] = env->banked_r14[i];

    env->spsr = env->banked_spsr[i];

}

static void v7m_push(CPUARMState *env, uint32_t val)

{

    env->regs[13] -= 4;

    stl_phys(env->regs[13], val);

}

static uint32_t v7m_pop(CPUARMState *env)

{

    uint32_t val;

    val = ldl_phys(env->regs[13]);

    env->regs[13] += 4;

    return val;

}

/* Switch to V7M main or process stack pointer.  */

static void switch_v7m_sp(CPUARMState *env, int process)

{

    uint32_t tmp;

    if (env->v7m.current_sp != process) {

        tmp = env->v7m.other_sp;

        env->v7m.other_sp = env->regs[13];

        env->regs[13] = tmp;

        env->v7m.current_sp = process;

    }

}

static void do_v7m_exception_exit(CPUARMState *env)

{

    uint32_t type;

    uint32_t xpsr;

    type = env->regs[15];

    if (env->v7m.exception != 0)

        armv7m_nvic_complete_irq(env->nvic, env->v7m.exception);

    /* Switch to the target stack.  */

    switch_v7m_sp(env, (type & 4) != 0);

    /* Pop registers.  */

    env->regs[0] = v7m_pop(env);

    env->regs[1] = v7m_pop(env);

    env->regs[2] = v7m_pop(env);

    env->regs[3] = v7m_pop(env);

    env->regs[12] = v7m_pop(env);

    env->regs[14] = v7m_pop(env);

    env->regs[15] = v7m_pop(env);

    xpsr = v7m_pop(env);

    xpsr_write(env, xpsr, 0xfffffdff);

    /* Undo stack alignment.  */

    if (xpsr & 0x200)

        env->regs[13] |= 4;

    /* ??? The exception return type specifies Thread/Handler mode.  However

       this is also implied by the xPSR value. Not sure what to do

       if there is a mismatch.  */

    /* ??? Likewise for mismatches between the CONTROL register and the stack

       pointer.  */

}

static void do_interrupt_v7m(CPUARMState *env)

{

    uint32_t xpsr = xpsr_read(env);

    uint32_t lr;

    uint32_t addr;

    lr = 0xfffffff1;

    if (env->v7m.current_sp)

        lr |= 4;

    if (env->v7m.exception == 0)

        lr |= 8;

    /* For exceptions we just mark as pending on the NVIC, and let that

       handle it.  */

    /* TODO: Need to escalate if the current priority is higher than the

       one we're raising.  */

    switch (env->exception_index) {

    case EXCP_UDEF:

        armv7m_nvic_set_pending(env->nvic, ARMV7M_EXCP_USAGE);

        return;

    case EXCP_SWI:

        env->regs[15] += 2;

        armv7m_nvic_set_pending(env->nvic, ARMV7M_EXCP_SVC);

        return;

    case EXCP_PREFETCH_ABORT:

    case EXCP_DATA_ABORT:

        armv7m_nvic_set_pending(env->nvic, ARMV7M_EXCP_MEM);

        return;

    case EXCP_BKPT:

        if (semihosting_enabled) {

            int nr;

            nr = arm_lduw_code(env->regs[15], env->bswap_code) & 0xff;

            if (nr == 0xab) {

                env->regs[15] += 2;

                env->regs[0] = do_arm_semihosting(env);

                return;

            }

        }

        armv7m_nvic_set_pending(env->nvic, ARMV7M_EXCP_DEBUG);

        return;

    case EXCP_IRQ:

        env->v7m.exception = armv7m_nvic_acknowledge_irq(env->nvic);

        break;

    case EXCP_EXCEPTION_EXIT:

        do_v7m_exception_exit(env);

        return;

    default:

        cpu_abort(env, "Unhandled exception 0x%x\n", env->exception_index);

        return; /* Never happens.  Keep compiler happy.  */

    }

    /* Align stack pointer.  */

    /* ??? Should only do this if Configuration Control Register

       STACKALIGN bit is set.  */

    if (env->regs[13] & 4) {

        env->regs[13] -= 4;

        xpsr |= 0x200;

    }

    /* Switch to the handler mode.  */

    v7m_push(env, xpsr);

    v7m_push(env, env->regs[15]);

    v7m_push(env, env->regs[14]);

    v7m_push(env, env->regs[12]);

    v7m_push(env, env->regs[3]);

    v7m_push(env, env->regs[2]);

    v7m_push(env, env->regs[1]);

    v7m_push(env, env->regs[0]);

    switch_v7m_sp(env, 0);

    /* Clear IT bits */

    env->condexec_bits = 0;

    env->regs[14] = lr;

    addr = ldl_phys(env->v7m.vecbase + env->v7m.exception * 4);

    env->regs[15] = addr & 0xfffffffe;

    env->thumb = addr & 1;

}

/* Handle a CPU exception.  */

void do_interrupt(CPUARMState *env)

{

    uint32_t addr;

    uint32_t mask;

    int new_mode;

    uint32_t offset;

    if (IS_M(env)) {

        do_interrupt_v7m(env);

        return;

    }

    /* TODO: Vectored interrupt controller.  */

    switch (env->exception_index) {

    case EXCP_UDEF:

        new_mode = ARM_CPU_MODE_UND;

        addr = 0x04;

        mask = CPSR_I;

        if (env->thumb)

            offset = 2;

        else

            offset = 4;

        break;

    case EXCP_SWI:

        if (semihosting_enabled) {

            /* Check for semihosting interrupt.  */

            if (env->thumb) {

                mask = arm_lduw_code(env->regs[15] - 2, env->bswap_code) & 0xff;

            } else {

                mask = arm_ldl_code(env->regs[15] - 4, env->bswap_code)

                    & 0xffffff;

            }

            /* Only intercept calls from privileged modes, to provide some

               semblance of security.  */

            if (((mask == 0x123456 && !env->thumb)

                    || (mask == 0xab && env->thumb))

                  && (env->uncached_cpsr & CPSR_M) != ARM_CPU_MODE_USR) {

                env->regs[0] = do_arm_semihosting(env);

                return;

            }

        }

        new_mode = ARM_CPU_MODE_SVC;

        addr = 0x08;

        mask = CPSR_I;

        /* The PC already points to the next instruction.  */

        offset = 0;

        break;

    case EXCP_BKPT:

        /* See if this is a semihosting syscall.  */

        if (env->thumb && semihosting_enabled) {

            mask = arm_lduw_code(env->regs[15], env->bswap_code) & 0xff;

            if (mask == 0xab

                  && (env->uncached_cpsr & CPSR_M) != ARM_CPU_MODE_USR) {

                env->regs[15] += 2;

                env->regs[0] = do_arm_semihosting(env);

                return;

            }

        }

        env->cp15.c5_insn = 2;

        /* Fall through to prefetch abort.  */

    case EXCP_PREFETCH_ABORT:

        new_mode = ARM_CPU_MODE_ABT;

        addr = 0x0c;

        mask = CPSR_A | CPSR_I;

        offset = 4;

        break;

    case EXCP_DATA_ABORT:

        new_mode = ARM_CPU_MODE_ABT;

        addr = 0x10;

        mask = CPSR_A | CPSR_I;

        offset = 8;

        break;

    case EXCP_IRQ:

        new_mode = ARM_CPU_MODE_IRQ;

        addr = 0x18;

        /* Disable IRQ and imprecise data aborts.  */

        mask = CPSR_A | CPSR_I;

        offset = 4;

        break;

    case EXCP_FIQ:

        new_mode = ARM_CPU_MODE_FIQ;

        addr = 0x1c;

        /* Disable FIQ, IRQ and imprecise data aborts.  */

        mask = CPSR_A | CPSR_I | CPSR_F;

        offset = 4;

        break;

    default:

        cpu_abort(env, "Unhandled exception 0x%x\n", env->exception_index);

        return; /* Never happens.  Keep compiler happy.  */

    }

    /* High vectors.  */

    if (env->cp15.c1_sys & (1 << 13)) {

        addr += 0xffff0000;

    }

    switch_mode (env, new_mode);

    env->spsr = cpsr_read(env);

    /* Clear IT bits.  */

    env->condexec_bits = 0;

    /* Switch to the new mode, and to the correct instruction set.  */

    env->uncached_cpsr = (env->uncached_cpsr & ~CPSR_M) | new_mode;

    env->uncached_cpsr |= mask;

    /* this is a lie, as the was no c1_sys on V4T/V5, but who cares

     * and we should just guard the thumb mode on V4 */

    if (arm_feature(env, ARM_FEATURE_V4T)) {

        env->thumb = (env->cp15.c1_sys & (1 << 30)) != 0;

    }

    env->regs[14] = env->regs[15] + offset;

    env->regs[15] = addr;

    env->interrupt_request |= CPU_INTERRUPT_EXITTB;

}

/* Check section/page access permissions.

   Returns the page protection flags, or zero if the access is not

   permitted.  */

static inline int check_ap(CPUARMState *env, int ap, int domain_prot,

                           int access_type, int is_user)

{

  int prot_ro;

  if (domain_prot == 3) {

    return PAGE_READ | PAGE_WRITE;

  }

  if (access_type == 1)

      prot_ro = 0;

  else

      prot_ro = PAGE_READ;

  switch (ap) {

  case 0:

      if (access_type == 1)

          return 0;

      switch ((env->cp15.c1_sys >> 8) & 3) {

      case 1:

          return is_user ? 0 : PAGE_READ;

      case 2:

          return PAGE_READ;

      default:

          return 0;

      }

  case 1:

      return is_user ? 0 : PAGE_READ | PAGE_WRITE;

  case 2:

      if (is_user)

          return prot_ro;

      else

          return PAGE_READ | PAGE_WRITE;

  case 3:

      return PAGE_READ | PAGE_WRITE;

  case 4: /* Reserved.  */

      return 0;

  case 5:

      return is_user ? 0 : prot_ro;

  case 6:

      return prot_ro;

  case 7:

      if (!arm_feature (env, ARM_FEATURE_V6K))

          return 0;

      return prot_ro;

  default:

      abort();

  }

}

static uint32_t get_level1_table_address(CPUARMState *env, uint32_t address)

{

    uint32_t table;

    if (address & env->cp15.c2_mask)

        table = env->cp15.c2_base1 & 0xffffc000;

    else

        table = env->cp15.c2_base0 & env->cp15.c2_base_mask;

    table |= (address >> 18) & 0x3ffc;

    return table;

}

static int get_phys_addr_v5(CPUARMState *env, uint32_t address, int access_type,

                            int is_user, uint32_t *phys_ptr, int *prot,

                            target_ulong *page_size)

{

    int code;

    uint32_t table;

    uint32_t desc;

    int type;

    int ap;

    int domain;

    int domain_prot;

    uint32_t phys_addr;

    /* Pagetable walk.  */

    /* Lookup l1 descriptor.  */

    table = get_level1_table_address(env, address);

    desc = ldl_phys(table);

    type = (desc & 3);

    domain = (desc >> 5) & 0x0f;

    domain_prot = (env->cp15.c3 >> (domain * 2)) & 3;

    if (type == 0) {

        /* Section translation fault.  */

        code = 5;

        goto do_fault;

    }

    if (domain_prot == 0 || domain_prot == 2) {