Archipelago » qemu

#include <stdio.h>

#include <stdlib.h>

#include <string.h>

#include "cpu.h"

#include "exec-all.h"

#include "gdbstub.h"

#include "helpers.h"

#include "qemu-common.h"

#include "host-utils.h"

#if !defined(CONFIG_USER_ONLY)

#include "hw/loader.h"

#endif

static uint32_t cortexa9_cp15_c0_c1[8] =

{ 0x1031, 0x11, 0x000, 0, 0x00100103, 0x20000000, 0x01230000, 0x00002111 };

static uint32_t cortexa9_cp15_c0_c2[8] =

{ 0x00101111, 0x13112111, 0x21232041, 0x11112131, 0x00111142, 0, 0, 0 };

static uint32_t cortexa8_cp15_c0_c1[8] =

{ 0x1031, 0x11, 0x400, 0, 0x31100003, 0x20000000, 0x01202000, 0x11 };

static uint32_t cortexa8_cp15_c0_c2[8] =

{ 0x00101111, 0x12112111, 0x21232031, 0x11112131, 0x00111142, 0, 0, 0 };

static uint32_t mpcore_cp15_c0_c1[8] =

{ 0x111, 0x1, 0, 0x2, 0x01100103, 0x10020302, 0x01222000, 0 };

static uint32_t mpcore_cp15_c0_c2[8] =

{ 0x00100011, 0x12002111, 0x11221011, 0x01102131, 0x141, 0, 0, 0 };

static uint32_t arm1136_cp15_c0_c1[8] =

{ 0x111, 0x1, 0x2, 0x3, 0x01130003, 0x10030302, 0x01222110, 0 };

static uint32_t arm1136_cp15_c0_c2[8] =

{ 0x00140011, 0x12002111, 0x11231111, 0x01102131, 0x141, 0, 0, 0 };

static uint32_t cpu_arm_find_by_name(const char *name);

static inline void set_feature(CPUARMState *env, int feature)

{

    env->features |= 1u << feature;

}

static void cpu_reset_model_id(CPUARMState *env, uint32_t id)

{

    env->cp15.c0_cpuid = id;

    switch (id) {

    case ARM_CPUID_ARM926:

        set_feature(env, ARM_FEATURE_VFP);

        env->vfp.xregs[ARM_VFP_FPSID] = 0x41011090;

        env->cp15.c0_cachetype = 0x1dd20d2;

        env->cp15.c1_sys = 0x00090078;

        break;

    case ARM_CPUID_ARM946:

        set_feature(env, ARM_FEATURE_MPU);

        env->cp15.c0_cachetype = 0x0f004006;

        env->cp15.c1_sys = 0x00000078;

        break;

    case ARM_CPUID_ARM1026:

        set_feature(env, ARM_FEATURE_VFP);

        set_feature(env, ARM_FEATURE_AUXCR);

        env->vfp.xregs[ARM_VFP_FPSID] = 0x410110a0;

        env->cp15.c0_cachetype = 0x1dd20d2;

        env->cp15.c1_sys = 0x00090078;

        break;

    case ARM_CPUID_ARM1136_R2:

    case ARM_CPUID_ARM1136:

        set_feature(env, ARM_FEATURE_V6);

        set_feature(env, ARM_FEATURE_VFP);

        set_feature(env, ARM_FEATURE_AUXCR);

        env->vfp.xregs[ARM_VFP_FPSID] = 0x410120b4;

        env->vfp.xregs[ARM_VFP_MVFR0] = 0x11111111;

        env->vfp.xregs[ARM_VFP_MVFR1] = 0x00000000;

        memcpy(env->cp15.c0_c1, arm1136_cp15_c0_c1, 8 * sizeof(uint32_t));

        memcpy(env->cp15.c0_c2, arm1136_cp15_c0_c2, 8 * sizeof(uint32_t));

        env->cp15.c0_cachetype = 0x1dd20d2;

        env->cp15.c1_sys = 0x00050078;

        break;

    case ARM_CPUID_ARM11MPCORE:

        set_feature(env, ARM_FEATURE_V6);

        set_feature(env, ARM_FEATURE_V6K);

        set_feature(env, ARM_FEATURE_VFP);

        set_feature(env, ARM_FEATURE_AUXCR);

        env->vfp.xregs[ARM_VFP_FPSID] = 0x410120b4;

        env->vfp.xregs[ARM_VFP_MVFR0] = 0x11111111;

        env->vfp.xregs[ARM_VFP_MVFR1] = 0x00000000;

        memcpy(env->cp15.c0_c1, mpcore_cp15_c0_c1, 8 * sizeof(uint32_t));

        memcpy(env->cp15.c0_c2, mpcore_cp15_c0_c2, 8 * sizeof(uint32_t));

        env->cp15.c0_cachetype = 0x1dd20d2;

        break;

    case ARM_CPUID_CORTEXA8:

        set_feature(env, ARM_FEATURE_V6);

        set_feature(env, ARM_FEATURE_V6K);

        set_feature(env, ARM_FEATURE_V7);

        set_feature(env, ARM_FEATURE_AUXCR);

        set_feature(env, ARM_FEATURE_THUMB2);

        set_feature(env, ARM_FEATURE_VFP);

        set_feature(env, ARM_FEATURE_VFP3);

        set_feature(env, ARM_FEATURE_NEON);

        set_feature(env, ARM_FEATURE_THUMB2EE);

        env->vfp.xregs[ARM_VFP_FPSID] = 0x410330c0;

        env->vfp.xregs[ARM_VFP_MVFR0] = 0x11110222;

        env->vfp.xregs[ARM_VFP_MVFR1] = 0x00011100;

        memcpy(env->cp15.c0_c1, cortexa8_cp15_c0_c1, 8 * sizeof(uint32_t));

        memcpy(env->cp15.c0_c2, cortexa8_cp15_c0_c2, 8 * sizeof(uint32_t));

        env->cp15.c0_cachetype = 0x82048004;

        env->cp15.c0_clid = (1 << 27) | (2 << 24) | 3;

        env->cp15.c0_ccsid[0] = 0xe007e01a; /* 16k L1 dcache. */

        env->cp15.c0_ccsid[1] = 0x2007e01a; /* 16k L1 icache. */

        env->cp15.c0_ccsid[2] = 0xf0000000; /* No L2 icache. */

        env->cp15.c1_sys = 0x00c50078;

        break;

    case ARM_CPUID_CORTEXA9:

        set_feature(env, ARM_FEATURE_V6);

        set_feature(env, ARM_FEATURE_V6K);

        set_feature(env, ARM_FEATURE_V7);

        set_feature(env, ARM_FEATURE_AUXCR);

        set_feature(env, ARM_FEATURE_THUMB2);

        set_feature(env, ARM_FEATURE_VFP);

        set_feature(env, ARM_FEATURE_VFP3);

        set_feature(env, ARM_FEATURE_VFP_FP16);

        set_feature(env, ARM_FEATURE_NEON);

        set_feature(env, ARM_FEATURE_THUMB2EE);

        /* Note that A9 supports the MP extensions even for

         * A9UP and single-core A9MP (which are both different

         * and valid configurations; we don't model A9UP).

         */

        set_feature(env, ARM_FEATURE_V7MP);

        env->vfp.xregs[ARM_VFP_FPSID] = 0x41034000; /* Guess */

        env->vfp.xregs[ARM_VFP_MVFR0] = 0x11110222;

        env->vfp.xregs[ARM_VFP_MVFR1] = 0x01111111;

        memcpy(env->cp15.c0_c1, cortexa9_cp15_c0_c1, 8 * sizeof(uint32_t));

        memcpy(env->cp15.c0_c2, cortexa9_cp15_c0_c2, 8 * sizeof(uint32_t));

        env->cp15.c0_cachetype = 0x80038003;

        env->cp15.c0_clid = (1 << 27) | (1 << 24) | 3;

        env->cp15.c0_ccsid[0] = 0xe00fe015; /* 16k L1 dcache. */

        env->cp15.c0_ccsid[1] = 0x200fe015; /* 16k L1 icache. */

        env->cp15.c1_sys = 0x00c50078;

        break;

    case ARM_CPUID_CORTEXM3:

        set_feature(env, ARM_FEATURE_V6);

        set_feature(env, ARM_FEATURE_THUMB2);

        set_feature(env, ARM_FEATURE_V7);

        set_feature(env, ARM_FEATURE_M);

        set_feature(env, ARM_FEATURE_DIV);

        break;

    case ARM_CPUID_ANY: /* For userspace emulation.  */

        set_feature(env, ARM_FEATURE_V6);

        set_feature(env, ARM_FEATURE_V6K);

        set_feature(env, ARM_FEATURE_V7);

        set_feature(env, ARM_FEATURE_THUMB2);

        set_feature(env, ARM_FEATURE_VFP);

        set_feature(env, ARM_FEATURE_VFP3);

        set_feature(env, ARM_FEATURE_VFP_FP16);

        set_feature(env, ARM_FEATURE_NEON);

        set_feature(env, ARM_FEATURE_THUMB2EE);

        set_feature(env, ARM_FEATURE_DIV);

        set_feature(env, ARM_FEATURE_V7MP);

        break;

    case ARM_CPUID_TI915T:

    case ARM_CPUID_TI925T:

        set_feature(env, ARM_FEATURE_OMAPCP);

        env->cp15.c0_cpuid = ARM_CPUID_TI925T; /* Depends on wiring.  */

        env->cp15.c0_cachetype = 0x5109149;

        env->cp15.c1_sys = 0x00000070;

        env->cp15.c15_i_max = 0x000;

        env->cp15.c15_i_min = 0xff0;

        break;

    case ARM_CPUID_PXA250:

    case ARM_CPUID_PXA255:

    case ARM_CPUID_PXA260:

    case ARM_CPUID_PXA261:

    case ARM_CPUID_PXA262:

        set_feature(env, ARM_FEATURE_XSCALE);

        /* JTAG_ID is ((id << 28) | 0x09265013) */

        env->cp15.c0_cachetype = 0xd172172;

        env->cp15.c1_sys = 0x00000078;

        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:

        set_feature(env, ARM_FEATURE_XSCALE);

        /* JTAG_ID is ((id << 28) | 0x09265013) */

        set_feature(env, ARM_FEATURE_IWMMXT);

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

        env->cp15.c0_cachetype = 0xd172172;

        env->cp15.c1_sys = 0x00000078;

        break;

    default:

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

        break;

    }

}

void cpu_reset(CPUARMState *env)

{

    uint32_t id;

    if (qemu_loglevel_mask(CPU_LOG_RESET)) {

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

        log_cpu_state(env, 0);

    }

    id = env->cp15.c0_cpuid;

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

    if (id)

        cpu_reset_model_id(env, id);

#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 -kenrel 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;

#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);

    tlb_flush(env, 1);

}

static int vfp_gdb_get_reg(CPUState *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(CPUState *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)

{

    CPUARMState *env;

    uint32_t id;

    static int inited = 0;

    id = cpu_arm_find_by_name(cpu_model);

    if (id == 0)

        return NULL;

    env = qemu_mallocz(sizeof(CPUARMState));

    cpu_exec_init(env);

    if (!inited) {

        inited = 1;

        arm_translate_init();

    }

    env->cpu_model_str = cpu_model;

    env->cp15.c0_cpuid = id;

    cpu_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;

}

struct arm_cpu_t {

    uint32_t id;

    const char *name;

};

static const struct arm_cpu_t arm_cpu_names[] = {

    { ARM_CPUID_ARM926, "arm926"},

    { ARM_CPUID_ARM946, "arm946"},

    { ARM_CPUID_ARM1026, "arm1026"},

    { ARM_CPUID_ARM1136, "arm1136"},

    { ARM_CPUID_ARM1136_R2, "arm1136-r2"},

    { ARM_CPUID_ARM11MPCORE, "arm11mpcore"},

    { ARM_CPUID_CORTEXM3, "cortex-m3"},

    { ARM_CPUID_CORTEXA8, "cortex-a8"},

    { ARM_CPUID_CORTEXA9, "cortex-a9"},

    { ARM_CPUID_TI925T, "ti925t" },

    { ARM_CPUID_PXA250, "pxa250" },

    { ARM_CPUID_PXA255, "pxa255" },

    { ARM_CPUID_PXA260, "pxa260" },

    { ARM_CPUID_PXA261, "pxa261" },

    { ARM_CPUID_PXA262, "pxa262" },

    { ARM_CPUID_PXA270, "pxa270" },

    { ARM_CPUID_PXA270_A0, "pxa270-a0" },

    { ARM_CPUID_PXA270_A1, "pxa270-a1" },

    { ARM_CPUID_PXA270_B0, "pxa270-b0" },

    { ARM_CPUID_PXA270_B1, "pxa270-b1" },

    { ARM_CPUID_PXA270_C0, "pxa270-c0" },

    { ARM_CPUID_PXA270_C5, "pxa270-c5" },

    { ARM_CPUID_ANY, "any"},

    { 0, NULL}

};

void arm_cpu_list(FILE *f, fprintf_function cpu_fprintf)

{

    int i;

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

    for (i = 0; arm_cpu_names[i].name; i++) {

        (*cpu_fprintf)(f, "  %s\n", arm_cpu_names[i].name);

    }

}

/* return 0 if not found */

static uint32_t cpu_arm_find_by_name(const char *name)

{

    int i;

    uint32_t id;

    id = 0;

    for (i = 0; arm_cpu_names[i].name; i++) {

        if (strcmp(name, arm_cpu_names[i].name) == 0) {

            id = arm_cpu_names[i].id;

            break;

        }

    }

    return id;

}

void cpu_arm_close(CPUARMState *env)

{

    free(env);

}

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) {

        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 (CPUState *env)

{

    env->exception_index = -1;

}

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

                              int mmu_idx, int is_softmmu)

{

    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)(CPUState *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)(CPUState *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)(CPUState *env, uint32_t insn, uint32_t val)

{

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

}

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

{

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

}

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

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

{

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

}

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

{

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

    return 0;

}

void switch_mode(CPUState *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)(CPUState *env, uint32_t mode, uint32_t val)

{

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

}

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

{

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

    return 0;

}

#else

extern int semihosting_enabled;

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

static inline int bank_number (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(cpu_single_env, "Bad mode %x\n", mode);

    return -1;

}

void switch_mode(CPUState *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(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(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 = lduw_code(env->regs[15]) & 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);

    env->uncached_cpsr &= ~CPSR_IT;

    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 = lduw_code(env->regs[15] - 2) & 0xff;

            } else {

                mask = ldl_code(env->regs[15] - 4) & 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 = lduw_code(env->regs[15]) & 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;

            }

        }

        /* 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;

    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(CPUState *env, int ap, int domain, int access_type,

                           int is_user)

{

  int prot_ro;