From 46747d15080a93cc82cac563c1b7b8ffef164bb5 Mon Sep 17 00:00:00 2001
From: Peter Maydell <peter.maydell@linaro.org>
Date: Mon, 29 Sep 2014 18:48:46 +0100
Subject: [PATCH 01/19] target-arm: Implement setting guest breakpoints

This patch adds support for setting guest breakpoints
based on values the guest writes to the DBGBVR and DBGBCR
registers. (It doesn't include the code to handle when
these breakpoints fire, so has no guest-visible effect.)

Signed-off-by: Peter Maydell <peter.maydell@linaro.org>
Message-id: 1410523465-13400-2-git-send-email-peter.maydell@linaro.org
---
 target-arm/cpu.c       |   1 +
 target-arm/cpu.h       |   1 +
 target-arm/helper.c    | 126 ++++++++++++++++++++++++++++++++++++++++-
 target-arm/internals.h |   9 +++
 target-arm/machine.c   |   1 +
 5 files changed, 136 insertions(+), 2 deletions(-)

diff --git a/target-arm/cpu.c b/target-arm/cpu.c
index 407f977742..6ab0e03dbd 100644
--- a/target-arm/cpu.c
+++ b/target-arm/cpu.c
@@ -185,6 +185,7 @@ static void arm_cpu_reset(CPUState *s)
     }
 #endif
 
+    hw_breakpoint_update_all(cpu);
     hw_watchpoint_update_all(cpu);
 }
 
diff --git a/target-arm/cpu.h b/target-arm/cpu.h
index d1e1ccb605..fa6ae0acc3 100644
--- a/target-arm/cpu.h
+++ b/target-arm/cpu.h
@@ -323,6 +323,7 @@ typedef struct CPUARMState {
     int eabi;
 #endif
 
+    struct CPUBreakpoint *cpu_breakpoint[16];
     struct CPUWatchpoint *cpu_watchpoint[16];
 
     CPU_COMMON
diff --git a/target-arm/helper.c b/target-arm/helper.c
index ece967397f..d246d36de4 100644
--- a/target-arm/helper.c
+++ b/target-arm/helper.c
@@ -2492,6 +2492,124 @@ static void dbgwcr_write(CPUARMState *env, const ARMCPRegInfo *ri,
     hw_watchpoint_update(cpu, i);
 }
 
+void hw_breakpoint_update(ARMCPU *cpu, int n)
+{
+    CPUARMState *env = &cpu->env;
+    uint64_t bvr = env->cp15.dbgbvr[n];
+    uint64_t bcr = env->cp15.dbgbcr[n];
+    vaddr addr;
+    int bt;
+    int flags = BP_CPU;
+
+    if (env->cpu_breakpoint[n]) {
+        cpu_breakpoint_remove_by_ref(CPU(cpu), env->cpu_breakpoint[n]);
+        env->cpu_breakpoint[n] = NULL;
+    }
+
+    if (!extract64(bcr, 0, 1)) {
+        /* E bit clear : watchpoint disabled */
+        return;
+    }
+
+    bt = extract64(bcr, 20, 4);
+
+    switch (bt) {
+    case 4: /* unlinked address mismatch (reserved if AArch64) */
+    case 5: /* linked address mismatch (reserved if AArch64) */
+        qemu_log_mask(LOG_UNIMP,
+                      "arm: address mismatch breakpoint types not implemented");
+        return;
+    case 0: /* unlinked address match */
+    case 1: /* linked address match */
+    {
+        /* Bits [63:49] are hardwired to the value of bit [48]; that is,
+         * we behave as if the register was sign extended. Bits [1:0] are
+         * RES0. The BAS field is used to allow setting breakpoints on 16
+         * bit wide instructions; it is CONSTRAINED UNPREDICTABLE whether
+         * a bp will fire if the addresses covered by the bp and the addresses
+         * covered by the insn overlap but the insn doesn't start at the
+         * start of the bp address range. We choose to require the insn and
+         * the bp to have the same address. The constraints on writing to
+         * BAS enforced in dbgbcr_write mean we have only four cases:
+         *  0b0000  => no breakpoint
+         *  0b0011  => breakpoint on addr
+         *  0b1100  => breakpoint on addr + 2
+         *  0b1111  => breakpoint on addr
+         * See also figure D2-3 in the v8 ARM ARM (DDI0487A.c).
+         */
+        int bas = extract64(bcr, 5, 4);
+        addr = sextract64(bvr, 0, 49) & ~3ULL;
+        if (bas == 0) {
+            return;
+        }
+        if (bas == 0xc) {
+            addr += 2;
+        }
+        break;
+    }
+    case 2: /* unlinked context ID match */
+    case 8: /* unlinked VMID match (reserved if no EL2) */
+    case 10: /* unlinked context ID and VMID match (reserved if no EL2) */
+        qemu_log_mask(LOG_UNIMP,
+                      "arm: unlinked context breakpoint types not implemented");
+        return;
+    case 9: /* linked VMID match (reserved if no EL2) */
+    case 11: /* linked context ID and VMID match (reserved if no EL2) */
+    case 3: /* linked context ID match */
+    default:
+        /* We must generate no events for Linked context matches (unless
+         * they are linked to by some other bp/wp, which is handled in
+         * updates for the linking bp/wp). We choose to also generate no events
+         * for reserved values.
+         */
+        return;
+    }
+
+    cpu_breakpoint_insert(CPU(cpu), addr, flags, &env->cpu_breakpoint[n]);
+}
+
+void hw_breakpoint_update_all(ARMCPU *cpu)
+{
+    int i;
+    CPUARMState *env = &cpu->env;
+
+    /* Completely clear out existing QEMU breakpoints and our array, to
+     * avoid possible stale entries following migration load.
+     */
+    cpu_breakpoint_remove_all(CPU(cpu), BP_CPU);
+    memset(env->cpu_breakpoint, 0, sizeof(env->cpu_breakpoint));
+
+    for (i = 0; i < ARRAY_SIZE(cpu->env.cpu_breakpoint); i++) {
+        hw_breakpoint_update(cpu, i);
+    }
+}
+
+static void dbgbvr_write(CPUARMState *env, const ARMCPRegInfo *ri,
+                         uint64_t value)
+{
+    ARMCPU *cpu = arm_env_get_cpu(env);
+    int i = ri->crm;
+
+    raw_write(env, ri, value);
+    hw_breakpoint_update(cpu, i);
+}
+
+static void dbgbcr_write(CPUARMState *env, const ARMCPRegInfo *ri,
+                         uint64_t value)
+{
+    ARMCPU *cpu = arm_env_get_cpu(env);
+    int i = ri->crm;
+
+    /* BAS[3] is a read-only copy of BAS[2], and BAS[1] a read-only
+     * copy of BAS[0].
+     */
+    value = deposit64(value, 6, 1, extract64(value, 5, 1));
+    value = deposit64(value, 8, 1, extract64(value, 7, 1));
+
+    raw_write(env, ri, value);
+    hw_breakpoint_update(cpu, i);
+}
+
 static void define_debug_regs(ARMCPU *cpu)
 {
     /* Define v7 and v8 architectural debug registers.
@@ -2533,11 +2651,15 @@ static void define_debug_regs(ARMCPU *cpu)
             { .name = "DBGBVR", .state = ARM_CP_STATE_BOTH,
               .cp = 14, .opc0 = 2, .opc1 = 0, .crn = 0, .crm = i, .opc2 = 4,
               .access = PL1_RW,
-              .fieldoffset = offsetof(CPUARMState, cp15.dbgbvr[i]) },
+              .fieldoffset = offsetof(CPUARMState, cp15.dbgbvr[i]),
+              .writefn = dbgbvr_write, .raw_writefn = raw_write
+            },
             { .name = "DBGBCR", .state = ARM_CP_STATE_BOTH,
               .cp = 14, .opc0 = 2, .opc1 = 0, .crn = 0, .crm = i, .opc2 = 5,
               .access = PL1_RW,
-              .fieldoffset = offsetof(CPUARMState, cp15.dbgbcr[i]) },
+              .fieldoffset = offsetof(CPUARMState, cp15.dbgbcr[i]),
+              .writefn = dbgbcr_write, .raw_writefn = raw_write
+            },
             REGINFO_SENTINEL
         };
         define_arm_cp_regs(cpu, dbgregs);
diff --git a/target-arm/internals.h b/target-arm/internals.h
index 64751a0798..b7e4822dc7 100644
--- a/target-arm/internals.h
+++ b/target-arm/internals.h
@@ -322,6 +322,15 @@ void hw_watchpoint_update(ARMCPU *cpu, int n);
  * suitable for use after migration or on reset.
  */
 void hw_watchpoint_update_all(ARMCPU *cpu);
+/* Update a QEMU breakpoint based on the information the guest has set in the
+ * DBGBCR<n>_EL1 and DBGBVR<n>_EL1 registers.
+ */
+void hw_breakpoint_update(ARMCPU *cpu, int n);
+/* Update the QEMU breakpoints for every guest breakpoint. This does a
+ * complete delete-and-reinstate of the QEMU breakpoint list and so is
+ * suitable for use after migration or on reset.
+ */
+void hw_breakpoint_update_all(ARMCPU *cpu);
 
 /* Callback function for when a watchpoint or breakpoint triggers. */
 void arm_debug_excp_handler(CPUState *cs);
diff --git a/target-arm/machine.c b/target-arm/machine.c
index 8dfe87cb6b..ddb7d05c28 100644
--- a/target-arm/machine.c
+++ b/target-arm/machine.c
@@ -214,6 +214,7 @@ static int cpu_post_load(void *opaque, int version_id)
         }
     }
 
+    hw_breakpoint_update_all(cpu);
     hw_watchpoint_update_all(cpu);
 
     return 0;

From 0eacea706060f9c53998896696b5f94cf49b8f03 Mon Sep 17 00:00:00 2001
From: Peter Maydell <peter.maydell@linaro.org>
Date: Mon, 29 Sep 2014 18:48:46 +0100
Subject: [PATCH 02/19] target-arm: Implement handling of breakpoint firing

Implement handling of breakpoint event firing to correctly
inject the debug exception into the guest.

Since the breakpoint and watchpoint control register format is
very similar we adjust wp_matches() to also handle breakpoints
as well rather than using a separate function.

Signed-off-by: Peter Maydell <peter.maydell@linaro.org>
Message-id: 1410523465-13400-3-git-send-email-peter.maydell@linaro.org
---
 target-arm/internals.h |  6 ++++
 target-arm/op_helper.c | 75 +++++++++++++++++++++++++++++++++---------
 2 files changed, 66 insertions(+), 15 deletions(-)

diff --git a/target-arm/internals.h b/target-arm/internals.h
index b7e4822dc7..986a7b1817 100644
--- a/target-arm/internals.h
+++ b/target-arm/internals.h
@@ -313,6 +313,12 @@ static inline uint32_t syn_watchpoint(int same_el, int cm, int wnr)
         | (cm << 8) | (wnr << 6) | 0x22;
 }
 
+static inline uint32_t syn_breakpoint(int same_el)
+{
+    return (EC_BREAKPOINT << ARM_EL_EC_SHIFT) | (same_el << ARM_EL_EC_SHIFT)
+        | ARM_EL_IL | 0x22;
+}
+
 /* Update a QEMU watchpoint based on the information the guest has set in the
  * DBGWCR<n>_EL1 and DBGWVR<n>_EL1 registers.
  */
diff --git a/target-arm/op_helper.c b/target-arm/op_helper.c
index b956216c4b..d0bcd97fbb 100644
--- a/target-arm/op_helper.c
+++ b/target-arm/op_helper.c
@@ -511,32 +511,43 @@ static bool linked_bp_matches(ARMCPU *cpu, int lbn)
     return false;
 }
 
-static bool wp_matches(ARMCPU *cpu, int n)
+static bool bp_wp_matches(ARMCPU *cpu, int n, bool is_wp)
 {
     CPUARMState *env = &cpu->env;
-    uint64_t wcr = env->cp15.dbgwcr[n];
+    uint64_t cr;
     int pac, hmc, ssc, wt, lbn;
     /* TODO: check against CPU security state when we implement TrustZone */
     bool is_secure = false;
 
-    if (!env->cpu_watchpoint[n]
-        || !(env->cpu_watchpoint[n]->flags & BP_WATCHPOINT_HIT)) {
-        return false;
-    }
+    if (is_wp) {
+        if (!env->cpu_watchpoint[n]
+            || !(env->cpu_watchpoint[n]->flags & BP_WATCHPOINT_HIT)) {
+            return false;
+        }
+        cr = env->cp15.dbgwcr[n];
+    } else {
+        uint64_t pc = is_a64(env) ? env->pc : env->regs[15];
 
+        if (!env->cpu_breakpoint[n] || env->cpu_breakpoint[n]->pc != pc) {
+            return false;
+        }
+        cr = env->cp15.dbgbcr[n];
+    }
     /* The WATCHPOINT_HIT flag guarantees us that the watchpoint is
-     * enabled and that the address and access type match; check the
-     * remaining fields, including linked breakpoints.
-     * Note that some combinations of {PAC, HMC SSC} are reserved and
+     * enabled and that the address and access type match; for breakpoints
+     * we know the address matched; check the remaining fields, including
+     * linked breakpoints. We rely on WCR and BCR having the same layout
+     * for the LBN, SSC, HMC, PAC/PMC and is-linked fields.
+     * Note that some combinations of {PAC, HMC, SSC} are reserved and
      * must act either like some valid combination or as if the watchpoint
      * were disabled. We choose the former, and use this together with
      * the fact that EL3 must always be Secure and EL2 must always be
      * Non-Secure to simplify the code slightly compared to the full
      * table in the ARM ARM.
      */
-    pac = extract64(wcr, 1, 2);
-    hmc = extract64(wcr, 13, 1);
-    ssc = extract64(wcr, 14, 2);
+    pac = extract64(cr, 1, 2);
+    hmc = extract64(cr, 13, 1);
+    ssc = extract64(cr, 14, 2);
 
     switch (ssc) {
     case 0:
@@ -560,6 +571,7 @@ static bool wp_matches(ARMCPU *cpu, int n)
      * Implementing this would require reworking the core watchpoint code
      * to plumb the mmu_idx through to this point. Luckily Linux does not
      * rely on this behaviour currently.
+     * For breakpoints we do want to use the current CPU state.
      */
     switch (arm_current_pl(env)) {
     case 3:
@@ -582,8 +594,8 @@ static bool wp_matches(ARMCPU *cpu, int n)
         g_assert_not_reached();
     }
 
-    wt = extract64(wcr, 20, 1);
-    lbn = extract64(wcr, 16, 4);
+    wt = extract64(cr, 20, 1);
+    lbn = extract64(cr, 16, 4);
 
     if (wt && !linked_bp_matches(cpu, lbn)) {
         return false;
@@ -606,7 +618,28 @@ static bool check_watchpoints(ARMCPU *cpu)
     }
 
     for (n = 0; n < ARRAY_SIZE(env->cpu_watchpoint); n++) {
-        if (wp_matches(cpu, n)) {
+        if (bp_wp_matches(cpu, n, true)) {
+            return true;
+        }
+    }
+    return false;
+}
+
+static bool check_breakpoints(ARMCPU *cpu)
+{
+    CPUARMState *env = &cpu->env;
+    int n;
+
+    /* If breakpoints are disabled globally or we can't take debug
+     * exceptions here then breakpoint firings are ignored.
+     */
+    if (extract32(env->cp15.mdscr_el1, 15, 1) == 0
+        || !arm_generate_debug_exceptions(env)) {
+        return false;
+    }
+
+    for (n = 0; n < ARRAY_SIZE(env->cpu_breakpoint); n++) {
+        if (bp_wp_matches(cpu, n, false)) {
             return true;
         }
     }
@@ -641,6 +674,18 @@ void arm_debug_excp_handler(CPUState *cs)
                 cpu_resume_from_signal(cs, NULL);
             }
         }
+    } else {
+        if (check_breakpoints(cpu)) {
+            bool same_el = (arm_debug_target_el(env) == arm_current_pl(env));
+            env->exception.syndrome = syn_breakpoint(same_el);
+            if (extended_addresses_enabled(env)) {
+                env->exception.fsr = (1 << 9) | 0x22;
+            } else {
+                env->exception.fsr = 0x2;
+            }
+            /* FAR is UNKNOWN, so doesn't need setting */
+            raise_exception(env, EXCP_PREFETCH_ABORT);
+        }
     }
 }
 

From 8f95ce2e4ceecc861dc20038871c6c0df7d40842 Mon Sep 17 00:00:00 2001
From: Peter Maydell <peter.maydell@linaro.org>
Date: Mon, 29 Sep 2014 18:48:47 +0100
Subject: [PATCH 03/19] configure: Build GDB XML for 32 bit ARM CPUs into qemu
 aarch64 binaries

The qemu-aarch64 and qemu-system-aarch64 binaries include support
for all the 32 bit ARM CPUs as well as the 64 bit ones. This means
we need to build in the GDB XML files for the 32 bit CPUs too.
Otherwise gdb will complain:
 warning: while parsing target description (at line 1): Could not load XML document "arm-core.xml"
when you try to connect to our gdbserver to debug a 32 bit CPU
running in a qemu-aarch64 or qemu-system-aarch64 binary.

Signed-off-by: Peter Maydell <peter.maydell@linaro.org>
Message-id: 1410533739-13836-1-git-send-email-peter.maydell@linaro.org
---
 configure | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/configure b/configure
index 681abfc669..9ac2600230 100755
--- a/configure
+++ b/configure
@@ -5028,7 +5028,7 @@ case "$target_name" in
   aarch64)
     TARGET_BASE_ARCH=arm
     bflt="yes"
-    gdb_xml_files="aarch64-core.xml aarch64-fpu.xml"
+    gdb_xml_files="aarch64-core.xml aarch64-fpu.xml arm-core.xml arm-vfp.xml arm-vfp3.xml arm-neon.xml"
   ;;
   cris)
   ;;

From 65731d1c3ee1f9ffc811aa290fd520cdb50ff11f Mon Sep 17 00:00:00 2001
From: Peter Maydell <peter.maydell@linaro.org>
Date: Mon, 29 Sep 2014 18:48:47 +0100
Subject: [PATCH 04/19] hw/display/blizzard.c: Delete unused function
 blizzard_rgb2yuv

The function blizzard_rgb2yuv() is unused; delete it.

Signed-off-by: Peter Maydell <peter.maydell@linaro.org>
Message-id: 1410723223-17711-2-git-send-email-peter.maydell@linaro.org
---
 hw/display/blizzard.c | 8 --------
 1 file changed, 8 deletions(-)

diff --git a/hw/display/blizzard.c b/hw/display/blizzard.c
index 55c0ddf00b..92b1fac563 100644
--- a/hw/display/blizzard.c
+++ b/hw/display/blizzard.c
@@ -134,14 +134,6 @@ static const int blizzard_iformat_bpp[0x10] = {
     0, 0, 0, 0, 0, 0,
 };
 
-static inline void blizzard_rgb2yuv(int r, int g, int b,
-                int *y, int *u, int *v)
-{
-    *y = 0x10 + ((0x838 * r + 0x1022 * g + 0x322 * b) >> 13);
-    *u = 0x80 + ((0xe0e * b - 0x04c1 * r - 0x94e * g) >> 13);
-    *v = 0x80 + ((0xe0e * r - 0x0bc7 * g - 0x247 * b) >> 13);
-}
-
 static void blizzard_window(BlizzardState *s)
 {
     DisplaySurface *surface = qemu_console_surface(s->con);

From 93c9aea9b42bedb65a03dbb3c6ff7dbfc43ef9a7 Mon Sep 17 00:00:00 2001
From: Peter Maydell <peter.maydell@linaro.org>
Date: Mon, 29 Sep 2014 18:48:47 +0100
Subject: [PATCH 05/19] hw/intc/imx_avic.c: Remove unused function
 imx_avic_set_prio()

The function imx_avic_set_prio() is unused; delete it.

Signed-off-by: Peter Maydell <peter.maydell@linaro.org>
Message-id: 1410723223-17711-3-git-send-email-peter.maydell@linaro.org
---
 hw/intc/imx_avic.c | 9 ---------
 1 file changed, 9 deletions(-)

diff --git a/hw/intc/imx_avic.c b/hw/intc/imx_avic.c
index ec5f9ad815..e48f66c8fa 100644
--- a/hw/intc/imx_avic.c
+++ b/hw/intc/imx_avic.c
@@ -97,15 +97,6 @@ static inline int imx_avic_prio(IMXAVICState *s, int irq)
     return 0xf & (s->prio[word] >> part);
 }
 
-static inline void imx_avic_set_prio(IMXAVICState *s, int irq, int prio)
-{
-    uint32_t word = irq / PRIO_PER_WORD;
-    uint32_t part = 4 * (irq % PRIO_PER_WORD);
-    uint32_t mask = ~(0xf << part);
-    s->prio[word] &= mask;
-    s->prio[word] |= prio << part;
-}
-
 /* Update interrupts.  */
 static void imx_avic_update(IMXAVICState *s)
 {

From 6ac285b824270821ac36eaaa2d92dc25ba2a5669 Mon Sep 17 00:00:00 2001
From: Peter Maydell <peter.maydell@linaro.org>
Date: Mon, 29 Sep 2014 18:48:47 +0100
Subject: [PATCH 06/19] hw/display/pxa2xx_lcd.c: Remove unused function
 pxa2xx_dma_rdst_set

The function pxa2xx_dma_rdst_set() is unused; delete it.

Signed-off-by: Peter Maydell <peter.maydell@linaro.org>
Message-id: 1410723223-17711-4-git-send-email-peter.maydell@linaro.org
---
 hw/display/pxa2xx_lcd.c | 8 --------
 1 file changed, 8 deletions(-)

diff --git a/hw/display/pxa2xx_lcd.c b/hw/display/pxa2xx_lcd.c
index 611fb174cd..ac3c018822 100644
--- a/hw/display/pxa2xx_lcd.c
+++ b/hw/display/pxa2xx_lcd.c
@@ -279,14 +279,6 @@ static inline void pxa2xx_dma_ber_set(PXA2xxLCDState *s, int ch)
         s->liidr = s->dma_ch[ch].id;
 }
 
-/* Set Read Status interrupt high and poke associated registers */
-static inline void pxa2xx_dma_rdst_set(PXA2xxLCDState *s)
-{
-    s->status[0] |= LCSR0_RDST;
-    if (s->irqlevel && !(s->control[0] & LCCR0_RDSTM))
-        s->status[0] |= LCSR0_SINT;
-}
-
 /* Load new Frame Descriptors from DMA */
 static void pxa2xx_descriptor_load(PXA2xxLCDState *s)
 {

From f59492b984934170f624487ffdec983a0102ba96 Mon Sep 17 00:00:00 2001
From: Peter Maydell <peter.maydell@linaro.org>
Date: Mon, 29 Sep 2014 18:48:48 +0100
Subject: [PATCH 07/19] hw/input/tsc210x.c: Delete unused array tsc2101_rates

The array tsc2101_rates[] is unused (and we don't implement
the TSC2101 anyway, only the 2102); delete it.

Signed-off-by: Peter Maydell <peter.maydell@linaro.org>
Message-id: 1410723223-17711-5-git-send-email-peter.maydell@linaro.org
---
 hw/input/tsc210x.c | 30 ------------------------------
 1 file changed, 30 deletions(-)

diff --git a/hw/input/tsc210x.c b/hw/input/tsc210x.c
index aa5b6886ea..fae3385636 100644
--- a/hw/input/tsc210x.c
+++ b/hw/input/tsc210x.c
@@ -215,36 +215,6 @@ typedef struct {
     int fsref;
 } TSC210xRateInfo;
 
-/*  { rate,  dsor,  fsref } */
-static const TSC210xRateInfo tsc2101_rates[] = {
-    /* Fsref / 6.0 */
-    { 7350,	7,	1 },
-    { 8000,	7,	0 },
-    /* Fsref / 5.5 */
-    { 8018,	6,	1 },
-    { 8727,	6,	0 },
-    /* Fsref / 5.0 */
-    { 8820,	5,	1 },
-    { 9600,	5,	0 },
-    /* Fsref / 4.0 */
-    { 11025,	4,	1 },
-    { 12000,	4,	0 },
-    /* Fsref / 3.0 */
-    { 14700,	3,	1 },
-    { 16000,	3,	0 },
-    /* Fsref / 2.0 */
-    { 22050,	2,	1 },
-    { 24000,	2,	0 },
-    /* Fsref / 1.5 */
-    { 29400,	1,	1 },
-    { 32000,	1,	0 },
-    /* Fsref */
-    { 44100,	0,	1 },
-    { 48000,	0,	0 },
-
-    { 0,	0, 	0 },
-};
-
 /*  { rate,   dsor, fsref }	*/
 static const TSC210xRateInfo tsc2102_rates[] = {
     /* Fsref / 6.0 */

From c0f4af171999eda4e49de5169906ce98246457f0 Mon Sep 17 00:00:00 2001
From: Peter Maydell <peter.maydell@linaro.org>
Date: Mon, 29 Sep 2014 18:48:48 +0100
Subject: [PATCH 08/19] target-arm: Don't handle c15_cpar changes via
 tb_flush()

At the moment we try to handle c15_cpar with the strategy of:
 * emit generated code which makes assumptions about its value
 * when the register value changes call tb_flush() to throw
   away the now-invalid generated code
This works because XScale CPUs are always uniprocessor, but
it's confusing because it suggests that the same approach can
be taken for other registers. It also means we do a tb_flush()
on CPU reset, which makes multithreaded linux-user binaries
even more likely to fail than would otherwise be the case.

Replace it with a combination of TB flags for the access
checks done on cp0/cp1 for the XScale and iwMMXt instructions,
plus a runtime check for cp2..cp13 coprocessor accesses.

Signed-off-by: Peter Maydell <peter.maydell@linaro.org>
Message-id: 1411056959-23070-1-git-send-email-peter.maydell@linaro.org
---
 target-arm/cpu.c       |  5 -----
 target-arm/cpu.h       |  9 +++++++++
 target-arm/helper.c    |  7 +------
 target-arm/op_helper.c | 11 +++++++++++
 target-arm/translate.c | 38 ++++++++++++++++++++------------------
 target-arm/translate.h |  2 ++
 6 files changed, 43 insertions(+), 29 deletions(-)

diff --git a/target-arm/cpu.c b/target-arm/cpu.c
index 6ab0e03dbd..248778d57a 100644
--- a/target-arm/cpu.c
+++ b/target-arm/cpu.c
@@ -173,11 +173,6 @@ static void arm_cpu_reset(CPUState *s)
     set_float_detect_tininess(float_tininess_before_rounding,
                               &env->vfp.standard_fp_status);
     tlb_flush(s, 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);
 
 #ifndef CONFIG_USER_ONLY
     if (kvm_enabled()) {
diff --git a/target-arm/cpu.h b/target-arm/cpu.h
index fa6ae0acc3..7bd3af9c07 100644
--- a/target-arm/cpu.h
+++ b/target-arm/cpu.h
@@ -1224,6 +1224,11 @@ static inline bool arm_singlestep_active(CPUARMState *env)
 #define ARM_TBFLAG_SS_ACTIVE_MASK (1 << ARM_TBFLAG_SS_ACTIVE_SHIFT)
 #define ARM_TBFLAG_PSTATE_SS_SHIFT 19
 #define ARM_TBFLAG_PSTATE_SS_MASK (1 << ARM_TBFLAG_PSTATE_SS_SHIFT)
+/* We store the bottom two bits of the CPAR as TB flags and handle
+ * checks on the other bits at runtime
+ */
+#define ARM_TBFLAG_XSCALE_CPAR_SHIFT 20
+#define ARM_TBFLAG_XSCALE_CPAR_MASK (3 << ARM_TBFLAG_XSCALE_CPAR_SHIFT)
 
 /* Bit usage when in AArch64 state */
 #define ARM_TBFLAG_AA64_EL_SHIFT    0
@@ -1258,6 +1263,8 @@ static inline bool arm_singlestep_active(CPUARMState *env)
     (((F) & ARM_TBFLAG_SS_ACTIVE_MASK) >> ARM_TBFLAG_SS_ACTIVE_SHIFT)
 #define ARM_TBFLAG_PSTATE_SS(F) \
     (((F) & ARM_TBFLAG_PSTATE_SS_MASK) >> ARM_TBFLAG_PSTATE_SS_SHIFT)
+#define ARM_TBFLAG_XSCALE_CPAR(F) \
+    (((F) & ARM_TBFLAG_XSCALE_CPAR_MASK) >> ARM_TBFLAG_XSCALE_CPAR_SHIFT)
 #define ARM_TBFLAG_AA64_EL(F) \
     (((F) & ARM_TBFLAG_AA64_EL_MASK) >> ARM_TBFLAG_AA64_EL_SHIFT)
 #define ARM_TBFLAG_AA64_FPEN(F) \
@@ -1335,6 +1342,8 @@ static inline void cpu_get_tb_cpu_state(CPUARMState *env, target_ulong *pc,
                 *flags |= ARM_TBFLAG_PSTATE_SS_MASK;
             }
         }
+        *flags |= (extract32(env->cp15.c15_cpar, 0, 2)
+                   << ARM_TBFLAG_XSCALE_CPAR_SHIFT);
     }
 
     *cs_base = 0;
diff --git a/target-arm/helper.c b/target-arm/helper.c
index d246d36de4..dd9fca56f6 100644
--- a/target-arm/helper.c
+++ b/target-arm/helper.c
@@ -1714,12 +1714,7 @@ static const ARMCPRegInfo omap_cp_reginfo[] = {
 static void xscale_cpar_write(CPUARMState *env, const ARMCPRegInfo *ri,
                               uint64_t value)
 {
-    value &= 0x3fff;
-    if (env->cp15.c15_cpar != value) {
-        /* Changes cp0 to cp13 behavior, so needs a TB flush.  */
-        tb_flush(env);
-        env->cp15.c15_cpar = value;
-    }
+    env->cp15.c15_cpar = value & 0x3fff;
 }
 
 static const ARMCPRegInfo xscale_cp_reginfo[] = {
diff --git a/target-arm/op_helper.c b/target-arm/op_helper.c
index d0bcd97fbb..25243bd487 100644
--- a/target-arm/op_helper.c
+++ b/target-arm/op_helper.c
@@ -301,6 +301,17 @@ void HELPER(set_user_reg)(CPUARMState *env, uint32_t regno, uint32_t val)
 void HELPER(access_check_cp_reg)(CPUARMState *env, void *rip, uint32_t syndrome)
 {
     const ARMCPRegInfo *ri = rip;
+
+    if (arm_feature(env, ARM_FEATURE_XSCALE) && ri->cp < 14
+        && extract32(env->cp15.c15_cpar, ri->cp, 1) == 0) {
+        env->exception.syndrome = syndrome;
+        raise_exception(env, EXCP_UDEF);
+    }
+
+    if (!ri->accessfn) {
+        return;
+    }
+
     switch (ri->accessfn(env, ri)) {
     case CP_ACCESS_OK:
         return;
diff --git a/target-arm/translate.c b/target-arm/translate.c
index 2c0b1deaea..8a2994fcb4 100644
--- a/target-arm/translate.c
+++ b/target-arm/translate.c
@@ -7001,22 +7001,18 @@ static int disas_coproc_insn(CPUARMState * env, DisasContext *s, uint32_t insn)
     const ARMCPRegInfo *ri;
 
     cpnum = (insn >> 8) & 0xf;
-    if (arm_feature(env, ARM_FEATURE_XSCALE)
-	    && ((env->cp15.c15_cpar ^ 0x3fff) & (1 << cpnum)))
-	return 1;
 
-    /* First check for coprocessor space used for actual instructions */
-    switch (cpnum) {
-      case 0:
-      case 1:
-	if (arm_feature(env, ARM_FEATURE_IWMMXT)) {
-	    return disas_iwmmxt_insn(env, s, insn);
-	} else if (arm_feature(env, ARM_FEATURE_XSCALE)) {
-	    return disas_dsp_insn(env, s, insn);
-	}
-	return 1;
-    default:
-        break;
+    /* First check for coprocessor space used for XScale/iwMMXt insns */
+    if (arm_feature(env, ARM_FEATURE_XSCALE) && (cpnum < 2)) {
+        if (extract32(s->c15_cpar, cpnum, 1) == 0) {
+            return 1;
+        }
+        if (arm_feature(env, ARM_FEATURE_IWMMXT)) {
+            return disas_iwmmxt_insn(env, s, insn);
+        } else if (arm_feature(env, ARM_FEATURE_XSCALE)) {
+            return disas_dsp_insn(env, s, insn);
+        }
+        return 1;
     }
 
     /* Otherwise treat as a generic register access */
@@ -7049,9 +7045,12 @@ static int disas_coproc_insn(CPUARMState * env, DisasContext *s, uint32_t insn)
             return 1;
         }
 
-        if (ri->accessfn) {
+        if (ri->accessfn ||
+            (arm_feature(env, ARM_FEATURE_XSCALE) && cpnum < 14)) {
             /* Emit code to perform further access permissions checks at
              * runtime; this may result in an exception.
+             * Note that on XScale all cp0..c13 registers do an access check
+             * call in order to handle c15_cpar.
              */
             TCGv_ptr tmpptr;
             TCGv_i32 tcg_syn;
@@ -7675,9 +7674,11 @@ static void disas_arm_insn(CPUARMState * env, DisasContext *s)
         } else if ((insn & 0x0e000f00) == 0x0c000100) {
             if (arm_feature(env, ARM_FEATURE_IWMMXT)) {
                 /* iWMMXt register transfer.  */
-                if (env->cp15.c15_cpar & (1 << 1))
-                    if (!disas_iwmmxt_insn(env, s, insn))
+                if (extract32(s->c15_cpar, 1, 1)) {
+                    if (!disas_iwmmxt_insn(env, s, insn)) {
                         return;
+                    }
+                }
             }
         } else if ((insn & 0x0fe00000) == 0x0c400000) {
             /* Coprocessor double register transfer.  */
@@ -10942,6 +10943,7 @@ static inline void gen_intermediate_code_internal(ARMCPU *cpu,
     dc->vfp_enabled = ARM_TBFLAG_VFPEN(tb->flags);
     dc->vec_len = ARM_TBFLAG_VECLEN(tb->flags);
     dc->vec_stride = ARM_TBFLAG_VECSTRIDE(tb->flags);
+    dc->c15_cpar = ARM_TBFLAG_XSCALE_CPAR(tb->flags);
     dc->cp_regs = cpu->cp_regs;
     dc->current_pl = arm_current_pl(env);
     dc->features = env->features;
diff --git a/target-arm/translate.h b/target-arm/translate.h
index b90d27514d..85c6f9dcb2 100644
--- a/target-arm/translate.h
+++ b/target-arm/translate.h
@@ -52,6 +52,8 @@ typedef struct DisasContext {
     bool is_ldex;
     /* True if a single-step exception will be taken to the current EL */
     bool ss_same_el;
+    /* Bottom two bits of XScale c15_cpar coprocessor access control reg */
+    int c15_cpar;
 #define TMP_A64_MAX 16
     int tmp_a64_count;
     TCGv_i64 tmp_a64[TMP_A64_MAX];

From f149e3e8479c3ac231bd58ba4955b02b67950547 Mon Sep 17 00:00:00 2001
From: "Edgar E. Iglesias" <edgar.iglesias@xilinx.com>
Date: Mon, 29 Sep 2014 18:48:48 +0100
Subject: [PATCH 09/19] target-arm: Add HCR_EL2

Reviewed-by: Greg Bellows <greg.bellows@linaro.org>
Reviewed-by: Peter Maydell <peter.maydell@linaro.org>
Signed-off-by: Edgar E. Iglesias <edgar.iglesias@xilinx.com>
Message-id: 1411718914-6608-2-git-send-email-edgar.iglesias@gmail.com
Signed-off-by: Peter Maydell <peter.maydell@linaro.org>
---
 target-arm/cpu.h    | 36 ++++++++++++++++++++++++++++++++++++
 target-arm/helper.c | 34 ++++++++++++++++++++++++++++++++++
 2 files changed, 70 insertions(+)

diff --git a/target-arm/cpu.h b/target-arm/cpu.h
index 7bd3af9c07..f5cbfddead 100644
--- a/target-arm/cpu.h
+++ b/target-arm/cpu.h
@@ -184,6 +184,7 @@ typedef struct CPUARMState {
                         MPU write buffer control.  */
         uint32_t pmsav5_data_ap; /* PMSAv5 MPU data access permissions */
         uint32_t pmsav5_insn_ap; /* PMSAv5 MPU insn access permissions */
+        uint64_t hcr_el2; /* Hypervisor configuration register */
         uint32_t ifsr_el2; /* Fault status registers.  */
         uint64_t esr_el[4];
         uint32_t c6_region[8]; /* MPU base/size registers.  */
@@ -566,6 +567,41 @@ static inline void xpsr_write(CPUARMState *env, uint32_t val, uint32_t mask)
     }
 }
 
+#define HCR_VM        (1ULL << 0)
+#define HCR_SWIO      (1ULL << 1)
+#define HCR_PTW       (1ULL << 2)
+#define HCR_FMO       (1ULL << 3)
+#define HCR_IMO       (1ULL << 4)
+#define HCR_AMO       (1ULL << 5)
+#define HCR_VF        (1ULL << 6)
+#define HCR_VI        (1ULL << 7)
+#define HCR_VSE       (1ULL << 8)
+#define HCR_FB        (1ULL << 9)
+#define HCR_BSU_MASK  (3ULL << 10)
+#define HCR_DC        (1ULL << 12)
+#define HCR_TWI       (1ULL << 13)
+#define HCR_TWE       (1ULL << 14)
+#define HCR_TID0      (1ULL << 15)
+#define HCR_TID1      (1ULL << 16)
+#define HCR_TID2      (1ULL << 17)
+#define HCR_TID3      (1ULL << 18)
+#define HCR_TSC       (1ULL << 19)
+#define HCR_TIDCP     (1ULL << 20)
+#define HCR_TACR      (1ULL << 21)
+#define HCR_TSW       (1ULL << 22)
+#define HCR_TPC       (1ULL << 23)
+#define HCR_TPU       (1ULL << 24)
+#define HCR_TTLB      (1ULL << 25)
+#define HCR_TVM       (1ULL << 26)
+#define HCR_TGE       (1ULL << 27)
+#define HCR_TDZ       (1ULL << 28)
+#define HCR_HCD       (1ULL << 29)
+#define HCR_TRVM      (1ULL << 30)
+#define HCR_RW        (1ULL << 31)
+#define HCR_CD        (1ULL << 32)
+#define HCR_ID        (1ULL << 33)
+#define HCR_MASK      ((1ULL << 34) - 1)
+
 /* Return the current FPSCR value.  */
 uint32_t vfp_get_fpscr(CPUARMState *env);
 void vfp_set_fpscr(CPUARMState *env, uint32_t val);
diff --git a/target-arm/helper.c b/target-arm/helper.c
index dd9fca56f6..d117e83c14 100644
--- a/target-arm/helper.c
+++ b/target-arm/helper.c
@@ -2225,10 +2225,44 @@ static const ARMCPRegInfo v8_el3_no_el2_cp_reginfo[] = {
       .opc0 = 3, .opc1 = 4, .crn = 12, .crm = 0, .opc2 = 0,
       .access = PL2_RW,
       .readfn = arm_cp_read_zero, .writefn = arm_cp_write_ignore },
+    { .name = "HCR_EL2", .state = ARM_CP_STATE_AA64,
+      .type = ARM_CP_NO_MIGRATE,
+      .opc0 = 3, .opc1 = 4, .crn = 1, .crm = 1, .opc2 = 0,
+      .access = PL2_RW,
+      .readfn = arm_cp_read_zero, .writefn = arm_cp_write_ignore },
     REGINFO_SENTINEL
 };
 
+static void hcr_write(CPUARMState *env, const ARMCPRegInfo *ri, uint64_t value)
+{
+    ARMCPU *cpu = arm_env_get_cpu(env);
+    uint64_t valid_mask = HCR_MASK;
+
+    if (arm_feature(env, ARM_FEATURE_EL3)) {
+        valid_mask &= ~HCR_HCD;
+    } else {
+        valid_mask &= ~HCR_TSC;
+    }
+
+    /* Clear RES0 bits.  */
+    value &= valid_mask;
+
+    /* These bits change the MMU setup:
+     * HCR_VM enables stage 2 translation
+     * HCR_PTW forbids certain page-table setups
+     * HCR_DC Disables stage1 and enables stage2 translation
+     */
+    if ((raw_read(env, ri) ^ value) & (HCR_VM | HCR_PTW | HCR_DC)) {
+        tlb_flush(CPU(cpu), 1);
+    }
+    raw_write(env, ri, value);
+}
+
 static const ARMCPRegInfo v8_el2_cp_reginfo[] = {
+    { .name = "HCR_EL2", .state = ARM_CP_STATE_AA64,
+      .opc0 = 3, .opc1 = 4, .crn = 1, .crm = 1, .opc2 = 0,
+      .access = PL2_RW, .fieldoffset = offsetof(CPUARMState, cp15.hcr_el2),
+      .writefn = hcr_write },
     { .name = "ELR_EL2", .state = ARM_CP_STATE_AA64,
       .type = ARM_CP_NO_MIGRATE,
       .opc0 = 3, .opc1 = 4, .crn = 4, .crm = 0, .opc2 = 1,

From 64e0e2de0c6735202acd575dbf32b310ec8ebad5 Mon Sep 17 00:00:00 2001
From: "Edgar E. Iglesias" <edgar.iglesias@xilinx.com>
Date: Mon, 29 Sep 2014 18:48:49 +0100
Subject: [PATCH 10/19] target-arm: Add SCR_EL3

Signed-off-by: Edgar E. Iglesias <edgar.iglesias@xilinx.com>
Message-id: 1411718914-6608-3-git-send-email-edgar.iglesias@gmail.com
[PMM: apply offsetoflow32() to correct regdef]
Signed-off-by: Peter Maydell <peter.maydell@linaro.org>
---
 target-arm/cpu.h    | 19 ++++++++++++++++++-
 target-arm/helper.c | 35 +++++++++++++++++++++++++++++++++--
 2 files changed, 51 insertions(+), 3 deletions(-)

diff --git a/target-arm/cpu.h b/target-arm/cpu.h
index f5cbfddead..8878d06904 100644
--- a/target-arm/cpu.h
+++ b/target-arm/cpu.h
@@ -172,7 +172,6 @@ typedef struct CPUARMState {
         uint64_t c1_sys; /* System control register.  */
         uint64_t c1_coproc; /* Coprocessor access register.  */
         uint32_t c1_xscaleauxcr; /* XScale auxiliary control register.  */
-        uint32_t c1_scr; /* secure config register.  */
         uint64_t ttbr0_el1; /* MMU translation table base 0. */
         uint64_t ttbr1_el1; /* MMU translation table base 1. */
         uint64_t c2_control; /* MMU translation table base control.  */
@@ -185,6 +184,7 @@ typedef struct CPUARMState {
         uint32_t pmsav5_data_ap; /* PMSAv5 MPU data access permissions */
         uint32_t pmsav5_insn_ap; /* PMSAv5 MPU insn access permissions */
         uint64_t hcr_el2; /* Hypervisor configuration register */
+        uint64_t scr_el3; /* Secure configuration register.  */
         uint32_t ifsr_el2; /* Fault status registers.  */
         uint64_t esr_el[4];
         uint32_t c6_region[8]; /* MPU base/size registers.  */
@@ -602,6 +602,23 @@ static inline void xpsr_write(CPUARMState *env, uint32_t val, uint32_t mask)
 #define HCR_ID        (1ULL << 33)
 #define HCR_MASK      ((1ULL << 34) - 1)
 
+#define SCR_NS                (1U << 0)
+#define SCR_IRQ               (1U << 1)
+#define SCR_FIQ               (1U << 2)
+#define SCR_EA                (1U << 3)
+#define SCR_FW                (1U << 4)
+#define SCR_AW                (1U << 5)
+#define SCR_NET               (1U << 6)
+#define SCR_SMD               (1U << 7)
+#define SCR_HCE               (1U << 8)
+#define SCR_SIF               (1U << 9)
+#define SCR_RW                (1U << 10)
+#define SCR_ST                (1U << 11)
+#define SCR_TWI               (1U << 12)
+#define SCR_TWE               (1U << 13)
+#define SCR_AARCH32_MASK      (0x3fff & ~(SCR_RW | SCR_ST))
+#define SCR_AARCH64_MASK      (0x3fff & ~SCR_NET)
+
 /* Return the current FPSCR value.  */
 uint32_t vfp_get_fpscr(CPUARMState *env);
 void vfp_set_fpscr(CPUARMState *env, uint32_t val);
diff --git a/target-arm/helper.c b/target-arm/helper.c
index d117e83c14..e47e7b4fdd 100644
--- a/target-arm/helper.c
+++ b/target-arm/helper.c
@@ -747,6 +747,32 @@ static void vbar_write(CPUARMState *env, const ARMCPRegInfo *ri,
     raw_write(env, ri, value & ~0x1FULL);
 }
 
+static void scr_write(CPUARMState *env, const ARMCPRegInfo *ri, uint64_t value)
+{
+    /* We only mask off bits that are RES0 both for AArch64 and AArch32.
+     * For bits that vary between AArch32/64, code needs to check the
+     * current execution mode before directly using the feature bit.
+     */
+    uint32_t valid_mask = SCR_AARCH64_MASK | SCR_AARCH32_MASK;
+
+    if (!arm_feature(env, ARM_FEATURE_EL2)) {
+        valid_mask &= ~SCR_HCE;
+
+        /* On ARMv7, SMD (or SCD as it is called in v7) is only
+         * supported if EL2 exists. The bit is UNK/SBZP when
+         * EL2 is unavailable. In QEMU ARMv7, we force it to always zero
+         * when EL2 is unavailable.
+         */
+        if (arm_feature(env, ARM_FEATURE_V7)) {
+            valid_mask &= ~SCR_SMD;
+        }
+    }
+
+    /* Clear all-context RES0 bits.  */
+    value &= valid_mask;
+    raw_write(env, ri, value);
+}
+
 static uint64_t ccsidr_read(CPUARMState *env, const ARMCPRegInfo *ri)
 {
     ARMCPU *cpu = arm_env_get_cpu(env);
@@ -873,8 +899,8 @@ static const ARMCPRegInfo v7_cp_reginfo[] = {
       .fieldoffset = offsetof(CPUARMState, cp15.vbar_el[1]),
       .resetvalue = 0 },
     { .name = "SCR", .cp = 15, .crn = 1, .crm = 1, .opc1 = 0, .opc2 = 0,
-      .access = PL1_RW, .fieldoffset = offsetof(CPUARMState, cp15.c1_scr),
-      .resetvalue = 0, },
+      .access = PL1_RW, .fieldoffset = offsetoflow32(CPUARMState, cp15.scr_el3),
+      .resetvalue = 0, .writefn = scr_write },
     { .name = "CCSIDR", .state = ARM_CP_STATE_BOTH,
       .opc0 = 3, .crn = 0, .crm = 0, .opc1 = 1, .opc2 = 0,
       .access = PL1_R, .readfn = ccsidr_read, .type = ARM_CP_NO_MIGRATE },
@@ -2309,6 +2335,11 @@ static const ARMCPRegInfo v8_el3_cp_reginfo[] = {
       .access = PL3_RW, .writefn = vbar_write,
       .fieldoffset = offsetof(CPUARMState, cp15.vbar_el[3]),
       .resetvalue = 0 },
+    { .name = "SCR_EL3", .state = ARM_CP_STATE_AA64,
+      .type = ARM_CP_NO_MIGRATE,
+      .opc0 = 3, .opc1 = 6, .crn = 1, .crm = 1, .opc2 = 0,
+      .access = PL3_RW, .fieldoffset = offsetof(CPUARMState, cp15.scr_el3),
+      .writefn = scr_write },
     REGINFO_SENTINEL
 };
 

From 9e729b57ac2a3adebee2746a2106a70eb8230a72 Mon Sep 17 00:00:00 2001
From: "Edgar E. Iglesias" <edgar.iglesias@xilinx.com>
Date: Mon, 29 Sep 2014 18:48:49 +0100
Subject: [PATCH 11/19] target-arm: A64: Refactor aarch64_cpu_do_interrupt

Introduce new_el and new_mode in preparation for future patches
that add support for taking exceptions to and from EL2 and 3.
No functional change.

Reviewed-by: Peter Maydell <peter.maydell@linaro.org>
Signed-off-by: Edgar E. Iglesias <edgar.iglesias@xilinx.com>
Message-id: 1411718914-6608-4-git-send-email-edgar.iglesias@gmail.com
Signed-off-by: Peter Maydell <peter.maydell@linaro.org>
---
 target-arm/cpu.h        |  7 +++++++
 target-arm/helper-a64.c | 24 +++++++++++++-----------
 target-arm/helper.c     | 13 +++++++++++++
 3 files changed, 33 insertions(+), 11 deletions(-)

diff --git a/target-arm/cpu.h b/target-arm/cpu.h
index 8878d06904..a3ba624b74 100644
--- a/target-arm/cpu.h
+++ b/target-arm/cpu.h
@@ -500,6 +500,12 @@ void pmccntr_sync(CPUARMState *env);
 #define PSTATE_MODE_EL1t 4
 #define PSTATE_MODE_EL0t 0
 
+/* Map EL and handler into a PSTATE_MODE.  */
+static inline unsigned int aarch64_pstate_mode(unsigned int el, bool handler)
+{
+    return (el << 2) | handler;
+}
+
 /* Return the current PSTATE value. For the moment we don't support 32<->64 bit
  * interprocessing, so we don't attempt to sync with the cpsr state used by
  * the 32 bit decoder.
@@ -755,6 +761,7 @@ static inline bool arm_el_is_aa64(CPUARMState *env, int el)
 }
 
 void arm_cpu_list(FILE *f, fprintf_function cpu_fprintf);
+unsigned int arm_excp_target_el(CPUState *cs, unsigned int excp_idx);
 
 /* Interface between CPU and Interrupt controller.  */
 void armv7m_nvic_set_pending(void *opaque, int irq);
diff --git a/target-arm/helper-a64.c b/target-arm/helper-a64.c
index 2e9ef64786..4be0784c3a 100644
--- a/target-arm/helper-a64.c
+++ b/target-arm/helper-a64.c
@@ -443,10 +443,12 @@ void aarch64_cpu_do_interrupt(CPUState *cs)
 {
     ARMCPU *cpu = ARM_CPU(cs);
     CPUARMState *env = &cpu->env;
-    target_ulong addr = env->cp15.vbar_el[1];
+    unsigned int new_el = arm_excp_target_el(cs, cs->exception_index);
+    target_ulong addr = env->cp15.vbar_el[new_el];
+    unsigned int new_mode = aarch64_pstate_mode(new_el, true);
     int i;
 
-    if (arm_current_pl(env) == 0) {
+    if (arm_current_pl(env) < new_el) {
         if (env->aarch64) {
             addr += 0x400;
         } else {
@@ -464,14 +466,14 @@ void aarch64_cpu_do_interrupt(CPUState *cs)
                       env->exception.syndrome);
     }
 
-    env->cp15.esr_el[1] = env->exception.syndrome;
-    env->cp15.far_el[1] = env->exception.vaddress;
+    env->cp15.esr_el[new_el] = env->exception.syndrome;
+    env->cp15.far_el[new_el] = env->exception.vaddress;
 
     switch (cs->exception_index) {
     case EXCP_PREFETCH_ABORT:
     case EXCP_DATA_ABORT:
         qemu_log_mask(CPU_LOG_INT, "...with FAR 0x%" PRIx64 "\n",
-                      env->cp15.far_el[1]);
+                      env->cp15.far_el[new_el]);
         break;
     case EXCP_BKPT:
     case EXCP_UDEF:
@@ -488,15 +490,15 @@ void aarch64_cpu_do_interrupt(CPUState *cs)
     }
 
     if (is_a64(env)) {
-        env->banked_spsr[aarch64_banked_spsr_index(1)] = pstate_read(env);
+        env->banked_spsr[aarch64_banked_spsr_index(new_el)] = pstate_read(env);
         aarch64_save_sp(env, arm_current_pl(env));
-        env->elr_el[1] = env->pc;
+        env->elr_el[new_el] = env->pc;
     } else {
         env->banked_spsr[0] = cpsr_read(env);
         if (!env->thumb) {
-            env->cp15.esr_el[1] |= 1 << 25;
+            env->cp15.esr_el[new_el] |= 1 << 25;
         }
-        env->elr_el[1] = env->regs[15];
+        env->elr_el[new_el] = env->regs[15];
 
         for (i = 0; i < 15; i++) {
             env->xregs[i] = env->regs[i];
@@ -505,9 +507,9 @@ void aarch64_cpu_do_interrupt(CPUState *cs)
         env->condexec_bits = 0;
     }
 
-    pstate_write(env, PSTATE_DAIF | PSTATE_MODE_EL1h);
+    pstate_write(env, PSTATE_DAIF | new_mode);
     env->aarch64 = 1;
-    aarch64_restore_sp(env, 1);
+    aarch64_restore_sp(env, new_el);
 
     env->pc = addr;
     cs->interrupt_request |= CPU_INTERRUPT_EXITTB;
diff --git a/target-arm/helper.c b/target-arm/helper.c
index e47e7b4fdd..25477bd8af 100644
--- a/target-arm/helper.c
+++ b/target-arm/helper.c
@@ -3704,6 +3704,11 @@ uint32_t HELPER(get_r13_banked)(CPUARMState *env, uint32_t mode)
     return 0;
 }
 
+unsigned int arm_excp_target_el(CPUState *cs, unsigned int excp_idx)
+{
+    return 1;
+}
+
 #else
 
 /* Map CPU modes onto saved register banks.  */
@@ -3759,6 +3764,14 @@ void switch_mode(CPUARMState *env, int mode)
     env->spsr = env->banked_spsr[i];
 }
 
+/*
+ * Determine the target EL for a given exception type.
+ */
+unsigned int arm_excp_target_el(CPUState *cs, unsigned int excp_idx)
+{
+    return 1;
+}
+
 static void v7m_push(CPUARMState *env, uint32_t val)
 {
     CPUState *cs = CPU(arm_env_get_cpu(env));

From 043b7f8d12dc922172cf7d570dc2854dbf81dcb3 Mon Sep 17 00:00:00 2001
From: "Edgar E. Iglesias" <edgar.iglesias@xilinx.com>
Date: Mon, 29 Sep 2014 18:48:49 +0100
Subject: [PATCH 12/19] target-arm: Break out exception masking to a separate
 func

Reviewed-by: Greg Bellows <greg.bellows@linaro.org>
Signed-off-by: Edgar E. Iglesias <edgar.iglesias@xilinx.com>
Message-id: 1411718914-6608-5-git-send-email-edgar.iglesias@gmail.com
[PMM: updated to account for recent cpu-exec refactoring]
Signed-off-by: Peter Maydell <peter.maydell@linaro.org>
---
 target-arm/cpu.c |  7 ++-----
 target-arm/cpu.h | 15 +++++++++++++++
 2 files changed, 17 insertions(+), 5 deletions(-)

diff --git a/target-arm/cpu.c b/target-arm/cpu.c
index 248778d57a..b7cdcd7c7a 100644
--- a/target-arm/cpu.c
+++ b/target-arm/cpu.c
@@ -187,12 +187,10 @@ static void arm_cpu_reset(CPUState *s)
 bool arm_cpu_exec_interrupt(CPUState *cs, int interrupt_request)
 {
     CPUClass *cc = CPU_GET_CLASS(cs);
-    ARMCPU *cpu = ARM_CPU(cs);
-    CPUARMState *env = &cpu->env;
     bool ret = false;
 
     if (interrupt_request & CPU_INTERRUPT_FIQ
-        && !(env->daif & PSTATE_F)) {
+        && arm_excp_unmasked(cs, EXCP_FIQ)) {
         cs->exception_index = EXCP_FIQ;
         cc->do_interrupt(cs);
         ret = true;
@@ -207,8 +205,7 @@ bool arm_cpu_exec_interrupt(CPUState *cs, int interrupt_request)
        We avoid this by disabling interrupts when
        pc contains a magic address.  */
     if (interrupt_request & CPU_INTERRUPT_HARD
-        && !(env->daif & PSTATE_I)
-        && (!IS_M(env) || env->regs[15] < 0xfffffff0)) {
+        && arm_excp_unmasked(cs, EXCP_IRQ)) {
         cs->exception_index = EXCP_IRQ;
         cc->do_interrupt(cs);
         ret = true;
diff --git a/target-arm/cpu.h b/target-arm/cpu.h
index a3ba624b74..cc2c2100b3 100644
--- a/target-arm/cpu.h
+++ b/target-arm/cpu.h
@@ -1172,6 +1172,21 @@ bool write_cpustate_to_list(ARMCPU *cpu);
 #  define TARGET_VIRT_ADDR_SPACE_BITS 32
 #endif
 
+static inline bool arm_excp_unmasked(CPUState *cs, unsigned int excp_idx)
+{
+    CPUARMState *env = cs->env_ptr;
+
+    switch (excp_idx) {
+    case EXCP_FIQ:
+        return !(env->daif & PSTATE_F);
+    case EXCP_IRQ:
+        return !(env->daif & PSTATE_I)
+               && (!IS_M(env) || env->regs[15] < 0xfffffff0);
+    default:
+        g_assert_not_reached();
+    }
+}
+
 static inline CPUARMState *cpu_init(const char *cpu_model)
 {
     ARMCPU *cpu = cpu_arm_init(cpu_model);

From dfafd0908820acf68f7c69ab3e0ebe855278d0bb Mon Sep 17 00:00:00 2001
From: "Edgar E. Iglesias" <edgar.iglesias@xilinx.com>
Date: Mon, 29 Sep 2014 18:48:49 +0100
Subject: [PATCH 13/19] target-arm: Don't take interrupts targeting lower ELs
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Reviewed-by: Alex Bennée <alex.bennee@linaro.org>
Reviewed-by: Greg Bellows <greg.bellows@linaro.org>
Reviewed-by: Peter Maydell <peter.maydell@linaro.org>
Signed-off-by: Edgar E. Iglesias <edgar.iglesias@xilinx.com>
Message-id: 1411718914-6608-6-git-send-email-edgar.iglesias@gmail.com
Signed-off-by: Peter Maydell <peter.maydell@linaro.org>
---
 target-arm/cpu.h | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/target-arm/cpu.h b/target-arm/cpu.h
index cc2c2100b3..81e7e814c1 100644
--- a/target-arm/cpu.h
+++ b/target-arm/cpu.h
@@ -1175,6 +1175,13 @@ bool write_cpustate_to_list(ARMCPU *cpu);
 static inline bool arm_excp_unmasked(CPUState *cs, unsigned int excp_idx)
 {
     CPUARMState *env = cs->env_ptr;
+    unsigned int cur_el = arm_current_pl(env);
+    unsigned int target_el = arm_excp_target_el(cs, excp_idx);
+
+    /* Don't take exceptions if they target a lower EL.  */
+    if (cur_el > target_el) {
+        return false;
+    }
 
     switch (excp_idx) {
     case EXCP_FIQ:

From 2dd081ae7642d54470225ea8c3f5cd4ef62fc732 Mon Sep 17 00:00:00 2001
From: "Edgar E. Iglesias" <edgar.iglesias@xilinx.com>
Date: Mon, 29 Sep 2014 18:48:50 +0100
Subject: [PATCH 14/19] target-arm: A64: Correct updates to FAR and ESR on
 exceptions
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Not all exception types update both FAR and ESR.

Reviewed-by: Alex Bennée <alex.bennee@linaro.org>
Reviewed-by: Greg Bellows <greg.bellows@linaro.org>
Signed-off-by: Edgar E. Iglesias <edgar.iglesias@xilinx.com>
Message-id: 1411718914-6608-7-git-send-email-edgar.iglesias@gmail.com
Signed-off-by: Peter Maydell <peter.maydell@linaro.org>
---
 target-arm/helper-a64.c | 7 +++----
 1 file changed, 3 insertions(+), 4 deletions(-)

diff --git a/target-arm/helper-a64.c b/target-arm/helper-a64.c
index 4be0784c3a..c6ef8e9543 100644
--- a/target-arm/helper-a64.c
+++ b/target-arm/helper-a64.c
@@ -466,18 +466,17 @@ void aarch64_cpu_do_interrupt(CPUState *cs)
                       env->exception.syndrome);
     }
 
-    env->cp15.esr_el[new_el] = env->exception.syndrome;
-    env->cp15.far_el[new_el] = env->exception.vaddress;
-
     switch (cs->exception_index) {
     case EXCP_PREFETCH_ABORT:
     case EXCP_DATA_ABORT:
+        env->cp15.far_el[new_el] = env->exception.vaddress;
         qemu_log_mask(CPU_LOG_INT, "...with FAR 0x%" PRIx64 "\n",
                       env->cp15.far_el[new_el]);
-        break;
+        /* fall through */
     case EXCP_BKPT:
     case EXCP_UDEF:
     case EXCP_SWI:
+        env->cp15.esr_el[new_el] = env->exception.syndrome;
         break;
     case EXCP_IRQ:
         addr += 0x80;

From 35979d71c40180a33242bf396a16b302f025fb82 Mon Sep 17 00:00:00 2001
From: "Edgar E. Iglesias" <edgar.iglesias@xilinx.com>
Date: Mon, 29 Sep 2014 18:48:50 +0100
Subject: [PATCH 15/19] target-arm: A64: Emulate the HVC insn

Signed-off-by: Edgar E. Iglesias <edgar.iglesias@xilinx.com>
Message-id: 1411718914-6608-8-git-send-email-edgar.iglesias@gmail.com
Signed-off-by: Peter Maydell <peter.maydell@linaro.org>
---
 target-arm/cpu.h           |  1 +
 target-arm/helper-a64.c    |  1 +
 target-arm/helper.c        | 20 +++++++++++++++++++-
 target-arm/helper.h        |  1 +
 target-arm/internals.h     |  6 ++++++
 target-arm/op_helper.c     | 31 +++++++++++++++++++++++++++++++
 target-arm/translate-a64.c | 31 ++++++++++++++++++++++---------
 7 files changed, 81 insertions(+), 10 deletions(-)

diff --git a/target-arm/cpu.h b/target-arm/cpu.h
index 81e7e814c1..e13b6d4baa 100644
--- a/target-arm/cpu.h
+++ b/target-arm/cpu.h
@@ -51,6 +51,7 @@
 #define EXCP_EXCEPTION_EXIT  8   /* Return from v7M exception.  */
 #define EXCP_KERNEL_TRAP     9   /* Jumped to kernel code page.  */
 #define EXCP_STREX          10
+#define EXCP_HVC            11   /* HyperVisor Call */
 
 #define ARMV7M_EXCP_RESET   1
 #define ARMV7M_EXCP_NMI     2
diff --git a/target-arm/helper-a64.c b/target-arm/helper-a64.c
index c6ef8e9543..4e6ca2631c 100644
--- a/target-arm/helper-a64.c
+++ b/target-arm/helper-a64.c
@@ -476,6 +476,7 @@ void aarch64_cpu_do_interrupt(CPUState *cs)
     case EXCP_BKPT:
     case EXCP_UDEF:
     case EXCP_SWI:
+    case EXCP_HVC:
         env->cp15.esr_el[new_el] = env->exception.syndrome;
         break;
     case EXCP_IRQ:
diff --git a/target-arm/helper.c b/target-arm/helper.c
index 25477bd8af..4d5a65329c 100644
--- a/target-arm/helper.c
+++ b/target-arm/helper.c
@@ -3769,7 +3769,25 @@ void switch_mode(CPUARMState *env, int mode)
  */
 unsigned int arm_excp_target_el(CPUState *cs, unsigned int excp_idx)
 {
-    return 1;
+    ARMCPU *cpu = ARM_CPU(cs);
+    CPUARMState *env = &cpu->env;
+    unsigned int cur_el = arm_current_pl(env);
+    unsigned int target_el;
+
+    if (!env->aarch64) {
+        /* TODO: Add EL2 and 3 exception handling for AArch32.  */
+        return 1;
+    }
+
+    switch (excp_idx) {
+    case EXCP_HVC:
+        target_el = 2;
+        break;
+    default:
+        target_el = MAX(cur_el, 1);
+        break;
+    }
+    return target_el;
 }
 
 static void v7m_push(CPUARMState *env, uint32_t val)
diff --git a/target-arm/helper.h b/target-arm/helper.h
index 1d7003b70a..75fc1b3f14 100644
--- a/target-arm/helper.h
+++ b/target-arm/helper.h
@@ -50,6 +50,7 @@ DEF_HELPER_2(exception_internal, void, env, i32)
 DEF_HELPER_3(exception_with_syndrome, void, env, i32, i32)
 DEF_HELPER_1(wfi, void, env)
 DEF_HELPER_1(wfe, void, env)
+DEF_HELPER_1(pre_hvc, void, env)
 
 DEF_HELPER_3(cpsr_write, void, env, i32, i32)
 DEF_HELPER_1(cpsr_read, i32, env)
diff --git a/target-arm/internals.h b/target-arm/internals.h
index 986a7b1817..25c5ec37d0 100644
--- a/target-arm/internals.h
+++ b/target-arm/internals.h
@@ -53,6 +53,7 @@ static const char * const excnames[] = {
     [EXCP_EXCEPTION_EXIT] = "QEMU v7M exception exit",
     [EXCP_KERNEL_TRAP] = "QEMU intercept of kernel commpage",
     [EXCP_STREX] = "QEMU intercept of STREX",
+    [EXCP_HVC] = "Hypervisor Call",
 };
 
 static inline void arm_log_exception(int idx)
@@ -215,6 +216,11 @@ static inline uint32_t syn_aa64_svc(uint32_t imm16)
     return (EC_AA64_SVC << ARM_EL_EC_SHIFT) | ARM_EL_IL | (imm16 & 0xffff);
 }
 
+static inline uint32_t syn_aa64_hvc(uint32_t imm16)
+{
+    return (EC_AA64_HVC << ARM_EL_EC_SHIFT) | ARM_EL_IL | (imm16 & 0xffff);
+}
+
 static inline uint32_t syn_aa32_svc(uint32_t imm16, bool is_thumb)
 {
     return (EC_AA32_SVC << ARM_EL_EC_SHIFT) | (imm16 & 0xffff)
diff --git a/target-arm/op_helper.c b/target-arm/op_helper.c
index 25243bd487..e5cb69062c 100644
--- a/target-arm/op_helper.c
+++ b/target-arm/op_helper.c
@@ -385,6 +385,37 @@ void HELPER(clear_pstate_ss)(CPUARMState *env)
     env->pstate &= ~PSTATE_SS;
 }
 
+void HELPER(pre_hvc)(CPUARMState *env)
+{
+    int cur_el = arm_current_pl(env);
+    /* FIXME: Use actual secure state.  */
+    bool secure = false;
+    bool undef;
+
+    /* We've already checked that EL2 exists at translation time.
+     * EL3.HCE has priority over EL2.HCD.
+     */
+    if (arm_feature(env, ARM_FEATURE_EL3)) {
+        undef = !(env->cp15.scr_el3 & SCR_HCE);
+    } else {
+        undef = env->cp15.hcr_el2 & HCR_HCD;
+    }
+
+    /* In ARMv7 and ARMv8/AArch32, HVC is undef in secure state.
+     * For ARMv8/AArch64, HVC is allowed in EL3.
+     * Note that we've already trapped HVC from EL0 at translation
+     * time.
+     */
+    if (secure && (!is_a64(env) || cur_el == 1)) {
+        undef = true;
+    }
+
+    if (undef) {
+        env->exception.syndrome = syn_uncategorized();
+        raise_exception(env, EXCP_UDEF);
+    }
+}
+
 void HELPER(exception_return)(CPUARMState *env)
 {
     int cur_el = arm_current_pl(env);
diff --git a/target-arm/translate-a64.c b/target-arm/translate-a64.c
index 8e66b6c972..f4cb56e8e6 100644
--- a/target-arm/translate-a64.c
+++ b/target-arm/translate-a64.c
@@ -1473,20 +1473,33 @@ static void disas_exc(DisasContext *s, uint32_t insn)
 
     switch (opc) {
     case 0:
-        /* SVC, HVC, SMC; since we don't support the Virtualization
-         * or TrustZone extensions these all UNDEF except SVC.
-         */
-        if (op2_ll != 1) {
-            unallocated_encoding(s);
-            break;
-        }
         /* For SVC, HVC and SMC we advance the single-step state
          * machine before taking the exception. This is architecturally
          * mandated, to ensure that single-stepping a system call
          * instruction works properly.
          */
-        gen_ss_advance(s);
-        gen_exception_insn(s, 0, EXCP_SWI, syn_aa64_svc(imm16));
+        switch (op2_ll) {
+        case 1:
+            gen_ss_advance(s);
+            gen_exception_insn(s, 0, EXCP_SWI, syn_aa64_svc(imm16));
+            break;
+        case 2:
+            if (!arm_dc_feature(s, ARM_FEATURE_EL2) || s->current_pl == 0) {
+                unallocated_encoding(s);
+                break;
+            }
+            /* The pre HVC helper handles cases when HVC gets trapped
+             * as an undefined insn by runtime configuration.
+             */
+            gen_a64_set_pc_im(s->pc - 4);
+            gen_helper_pre_hvc(cpu_env);
+            gen_ss_advance(s);
+            gen_exception_insn(s, 0, EXCP_HVC, syn_aa64_hvc(imm16));
+            break;
+        default:
+            unallocated_encoding(s);
+            break;
+        }
         break;
     case 1:
         if (op2_ll != 0) {

From 607d98b81e68a5fe23e553a0b75c0c38346a6d5f Mon Sep 17 00:00:00 2001
From: "Edgar E. Iglesias" <edgar.iglesias@xilinx.com>
Date: Mon, 29 Sep 2014 18:48:50 +0100
Subject: [PATCH 16/19] target-arm: Add a Hypervisor Trap exception type

Signed-off-by: Edgar E. Iglesias <edgar.iglesias@xilinx.com>
Message-id: 1411718914-6608-9-git-send-email-edgar.iglesias@gmail.com
Signed-off-by: Peter Maydell <peter.maydell@linaro.org>
---
 target-arm/cpu.h        | 1 +
 target-arm/helper-a64.c | 1 +
 target-arm/helper.c     | 1 +
 target-arm/internals.h  | 1 +
 4 files changed, 4 insertions(+)

diff --git a/target-arm/cpu.h b/target-arm/cpu.h
index e13b6d4baa..98610f0352 100644
--- a/target-arm/cpu.h
+++ b/target-arm/cpu.h
@@ -52,6 +52,7 @@
 #define EXCP_KERNEL_TRAP     9   /* Jumped to kernel code page.  */
 #define EXCP_STREX          10
 #define EXCP_HVC            11   /* HyperVisor Call */
+#define EXCP_HYP_TRAP       12
 
 #define ARMV7M_EXCP_RESET   1
 #define ARMV7M_EXCP_NMI     2
diff --git a/target-arm/helper-a64.c b/target-arm/helper-a64.c
index 4e6ca2631c..0a7c155bb8 100644
--- a/target-arm/helper-a64.c
+++ b/target-arm/helper-a64.c
@@ -477,6 +477,7 @@ void aarch64_cpu_do_interrupt(CPUState *cs)
     case EXCP_UDEF:
     case EXCP_SWI:
     case EXCP_HVC:
+    case EXCP_HYP_TRAP:
         env->cp15.esr_el[new_el] = env->exception.syndrome;
         break;
     case EXCP_IRQ:
diff --git a/target-arm/helper.c b/target-arm/helper.c
index 4d5a65329c..48f549af66 100644
--- a/target-arm/helper.c
+++ b/target-arm/helper.c
@@ -3781,6 +3781,7 @@ unsigned int arm_excp_target_el(CPUState *cs, unsigned int excp_idx)
 
     switch (excp_idx) {
     case EXCP_HVC:
+    case EXCP_HYP_TRAP:
         target_el = 2;
         break;
     default:
diff --git a/target-arm/internals.h b/target-arm/internals.h
index 25c5ec37d0..203c423be7 100644
--- a/target-arm/internals.h
+++ b/target-arm/internals.h
@@ -54,6 +54,7 @@ static const char * const excnames[] = {
     [EXCP_KERNEL_TRAP] = "QEMU intercept of kernel commpage",
     [EXCP_STREX] = "QEMU intercept of STREX",
     [EXCP_HVC] = "Hypervisor Call",
+    [EXCP_HYP_TRAP] = "Hypervisor Trap",
 };
 
 static inline void arm_log_exception(int idx)

From e0d6e6a5e799c91b74e157ca2cb7f4d2dbc0d56b Mon Sep 17 00:00:00 2001
From: "Edgar E. Iglesias" <edgar.iglesias@xilinx.com>
Date: Mon, 29 Sep 2014 18:48:50 +0100
Subject: [PATCH 17/19] target-arm: A64: Emulate the SMC insn

Signed-off-by: Edgar E. Iglesias <edgar.iglesias@xilinx.com>
Message-id: 1411718914-6608-10-git-send-email-edgar.iglesias@gmail.com
Signed-off-by: Peter Maydell <peter.maydell@linaro.org>
---
 target-arm/cpu.h           |  1 +
 target-arm/helper-a64.c    |  1 +
 target-arm/helper.c        |  3 +++
 target-arm/helper.h        |  1 +
 target-arm/internals.h     |  6 ++++++
 target-arm/op_helper.c     | 26 ++++++++++++++++++++++++++
 target-arm/translate-a64.c | 13 +++++++++++++
 7 files changed, 51 insertions(+)

diff --git a/target-arm/cpu.h b/target-arm/cpu.h
index 98610f0352..d104a7d704 100644
--- a/target-arm/cpu.h
+++ b/target-arm/cpu.h
@@ -53,6 +53,7 @@
 #define EXCP_STREX          10
 #define EXCP_HVC            11   /* HyperVisor Call */
 #define EXCP_HYP_TRAP       12
+#define EXCP_SMC            13   /* Secure Monitor Call */
 
 #define ARMV7M_EXCP_RESET   1
 #define ARMV7M_EXCP_NMI     2
diff --git a/target-arm/helper-a64.c b/target-arm/helper-a64.c
index 0a7c155bb8..0011488ec3 100644
--- a/target-arm/helper-a64.c
+++ b/target-arm/helper-a64.c
@@ -478,6 +478,7 @@ void aarch64_cpu_do_interrupt(CPUState *cs)
     case EXCP_SWI:
     case EXCP_HVC:
     case EXCP_HYP_TRAP:
+    case EXCP_SMC:
         env->cp15.esr_el[new_el] = env->exception.syndrome;
         break;
     case EXCP_IRQ:
diff --git a/target-arm/helper.c b/target-arm/helper.c
index 48f549af66..af2d2e670a 100644
--- a/target-arm/helper.c
+++ b/target-arm/helper.c
@@ -3784,6 +3784,9 @@ unsigned int arm_excp_target_el(CPUState *cs, unsigned int excp_idx)
     case EXCP_HYP_TRAP:
         target_el = 2;
         break;
+    case EXCP_SMC:
+        target_el = 3;
+        break;
     default:
         target_el = MAX(cur_el, 1);
         break;
diff --git a/target-arm/helper.h b/target-arm/helper.h
index 75fc1b3f14..dec3728798 100644
--- a/target-arm/helper.h
+++ b/target-arm/helper.h
@@ -51,6 +51,7 @@ DEF_HELPER_3(exception_with_syndrome, void, env, i32, i32)
 DEF_HELPER_1(wfi, void, env)
 DEF_HELPER_1(wfe, void, env)
 DEF_HELPER_1(pre_hvc, void, env)
+DEF_HELPER_2(pre_smc, void, env, i32)
 
 DEF_HELPER_3(cpsr_write, void, env, i32, i32)
 DEF_HELPER_1(cpsr_read, i32, env)
diff --git a/target-arm/internals.h b/target-arm/internals.h
index 203c423be7..1486595916 100644
--- a/target-arm/internals.h
+++ b/target-arm/internals.h
@@ -55,6 +55,7 @@ static const char * const excnames[] = {
     [EXCP_STREX] = "QEMU intercept of STREX",
     [EXCP_HVC] = "Hypervisor Call",
     [EXCP_HYP_TRAP] = "Hypervisor Trap",
+    [EXCP_SMC] = "Secure Monitor Call",
 };
 
 static inline void arm_log_exception(int idx)
@@ -222,6 +223,11 @@ static inline uint32_t syn_aa64_hvc(uint32_t imm16)
     return (EC_AA64_HVC << ARM_EL_EC_SHIFT) | ARM_EL_IL | (imm16 & 0xffff);
 }
 
+static inline uint32_t syn_aa64_smc(uint32_t imm16)
+{
+    return (EC_AA64_SMC << ARM_EL_EC_SHIFT) | ARM_EL_IL | (imm16 & 0xffff);
+}
+
 static inline uint32_t syn_aa32_svc(uint32_t imm16, bool is_thumb)
 {
     return (EC_AA32_SVC << ARM_EL_EC_SHIFT) | (imm16 & 0xffff)
diff --git a/target-arm/op_helper.c b/target-arm/op_helper.c
index e5cb69062c..03ac92afdc 100644
--- a/target-arm/op_helper.c
+++ b/target-arm/op_helper.c
@@ -416,6 +416,32 @@ void HELPER(pre_hvc)(CPUARMState *env)
     }
 }
 
+void HELPER(pre_smc)(CPUARMState *env, uint32_t syndrome)
+{
+    int cur_el = arm_current_pl(env);
+    /* FIXME: Use real secure state.  */
+    bool secure = false;
+    bool smd = env->cp15.scr_el3 & SCR_SMD;
+    /* On ARMv8 AArch32, SMD only applies to NS state.
+     * On ARMv7 SMD only applies to NS state and only if EL2 is available.
+     * For ARMv7 non EL2, we force SMD to zero so we don't need to re-check
+     * the EL2 condition here.
+     */
+    bool undef = is_a64(env) ? smd : (!secure && smd);
+
+    /* In NS EL1, HCR controlled routing to EL2 has priority over SMD.  */
+    if (!secure && cur_el == 1 && (env->cp15.hcr_el2 & HCR_TSC)) {
+        env->exception.syndrome = syndrome;
+        raise_exception(env, EXCP_HYP_TRAP);
+    }
+
+    /* We've already checked that EL3 exists at translation time.  */
+    if (undef) {
+        env->exception.syndrome = syn_uncategorized();
+        raise_exception(env, EXCP_UDEF);
+    }
+}
+
 void HELPER(exception_return)(CPUARMState *env)
 {
     int cur_el = arm_current_pl(env);
diff --git a/target-arm/translate-a64.c b/target-arm/translate-a64.c
index f4cb56e8e6..35ae3ea281 100644
--- a/target-arm/translate-a64.c
+++ b/target-arm/translate-a64.c
@@ -1470,6 +1470,7 @@ static void disas_exc(DisasContext *s, uint32_t insn)
     int opc = extract32(insn, 21, 3);
     int op2_ll = extract32(insn, 0, 5);
     int imm16 = extract32(insn, 5, 16);
+    TCGv_i32 tmp;
 
     switch (opc) {
     case 0:
@@ -1496,6 +1497,18 @@ static void disas_exc(DisasContext *s, uint32_t insn)
             gen_ss_advance(s);
             gen_exception_insn(s, 0, EXCP_HVC, syn_aa64_hvc(imm16));
             break;
+        case 3:
+            if (!arm_dc_feature(s, ARM_FEATURE_EL3) || s->current_pl == 0) {
+                unallocated_encoding(s);
+                break;
+            }
+            gen_a64_set_pc_im(s->pc - 4);
+            tmp = tcg_const_i32(syn_aa64_smc(imm16));
+            gen_helper_pre_smc(cpu_env, tmp);
+            tcg_temp_free_i32(tmp);
+            gen_ss_advance(s);
+            gen_exception_insn(s, 0, EXCP_SMC, syn_aa64_smc(imm16));
+            break;
         default:
             unallocated_encoding(s);
             break;

From 041c96666d0480380e50fe66fc66cc35bddcf741 Mon Sep 17 00:00:00 2001
From: "Edgar E. Iglesias" <edgar.iglesias@xilinx.com>
Date: Mon, 29 Sep 2014 18:48:51 +0100
Subject: [PATCH 18/19] target-arm: Add IRQ and FIQ routing to EL2 and 3

Reviewed-by: Greg Bellows <greg.bellows@linaro.org>
Signed-off-by: Edgar E. Iglesias <edgar.iglesias@xilinx.com>
Message-id: 1411718914-6608-11-git-send-email-edgar.iglesias@gmail.com
Signed-off-by: Peter Maydell <peter.maydell@linaro.org>
---
 target-arm/cpu.h    | 10 ++++++++++
 target-arm/helper.c | 17 +++++++++++++++++
 2 files changed, 27 insertions(+)

diff --git a/target-arm/cpu.h b/target-arm/cpu.h
index d104a7d704..11ba9d6c02 100644
--- a/target-arm/cpu.h
+++ b/target-arm/cpu.h
@@ -1180,6 +1180,10 @@ static inline bool arm_excp_unmasked(CPUState *cs, unsigned int excp_idx)
     CPUARMState *env = cs->env_ptr;
     unsigned int cur_el = arm_current_pl(env);
     unsigned int target_el = arm_excp_target_el(cs, excp_idx);
+    /* FIXME: Use actual secure state.  */
+    bool secure = false;
+    /* If in EL1/0, Physical IRQ routing to EL2 only happens from NS state.  */
+    bool irq_can_hyp = !secure && cur_el < 2 && target_el == 2;
 
     /* Don't take exceptions if they target a lower EL.  */
     if (cur_el > target_el) {
@@ -1188,8 +1192,14 @@ static inline bool arm_excp_unmasked(CPUState *cs, unsigned int excp_idx)
 
     switch (excp_idx) {
     case EXCP_FIQ:
+        if (irq_can_hyp && (env->cp15.hcr_el2 & HCR_FMO)) {
+            return true;
+        }
         return !(env->daif & PSTATE_F);
     case EXCP_IRQ:
+        if (irq_can_hyp && (env->cp15.hcr_el2 & HCR_IMO)) {
+            return true;
+        }
         return !(env->daif & PSTATE_I)
                && (!IS_M(env) || env->regs[15] < 0xfffffff0);
     default:
diff --git a/target-arm/helper.c b/target-arm/helper.c
index af2d2e670a..6135594252 100644
--- a/target-arm/helper.c
+++ b/target-arm/helper.c
@@ -3773,6 +3773,8 @@ unsigned int arm_excp_target_el(CPUState *cs, unsigned int excp_idx)
     CPUARMState *env = &cpu->env;
     unsigned int cur_el = arm_current_pl(env);
     unsigned int target_el;
+    /* FIXME: Use actual secure state.  */
+    bool secure = false;
 
     if (!env->aarch64) {
         /* TODO: Add EL2 and 3 exception handling for AArch32.  */
@@ -3787,6 +3789,21 @@ unsigned int arm_excp_target_el(CPUState *cs, unsigned int excp_idx)
     case EXCP_SMC:
         target_el = 3;
         break;
+    case EXCP_FIQ:
+    case EXCP_IRQ:
+    {
+        const uint64_t hcr_mask = excp_idx == EXCP_FIQ ? HCR_FMO : HCR_IMO;
+        const uint32_t scr_mask = excp_idx == EXCP_FIQ ? SCR_FIQ : SCR_IRQ;
+
+        target_el = 1;
+        if (!secure && (env->cp15.hcr_el2 & hcr_mask)) {
+            target_el = 2;
+        }
+        if (env->cp15.scr_el3 & scr_mask) {
+            target_el = 3;
+        }
+        break;
+    }
     default:
         target_el = MAX(cur_el, 1);
         break;

From 136e67e9b50b61fb03fedcea5c4fbe74cf44fdcc Mon Sep 17 00:00:00 2001
From: "Edgar E. Iglesias" <edgar.iglesias@xilinx.com>
Date: Mon, 29 Sep 2014 18:48:51 +0100
Subject: [PATCH 19/19] target-arm: Add support for VIRQ and VFIQ

This only implements the external delivery method via the GIC.

Acked-by: Greg Bellows <greg.bellows@linaro.org>
Signed-off-by: Edgar E. Iglesias <edgar.iglesias@xilinx.com>
Message-id: 1411718914-6608-12-git-send-email-edgar.iglesias@gmail.com
[PMM: adjusted following cpu-exec refactoring]
Signed-off-by: Peter Maydell <peter.maydell@linaro.org>
---
 target-arm/cpu.c        | 47 +++++++++++++++++++++++++++++++----------
 target-arm/cpu.h        | 35 +++++++++++++++++++++++++++---
 target-arm/helper-a64.c |  2 ++
 target-arm/helper.c     |  4 ++++
 target-arm/internals.h  |  2 ++
 5 files changed, 76 insertions(+), 14 deletions(-)

diff --git a/target-arm/cpu.c b/target-arm/cpu.c
index b7cdcd7c7a..8ab6d9532e 100644
--- a/target-arm/cpu.c
+++ b/target-arm/cpu.c
@@ -41,7 +41,9 @@ static void arm_cpu_set_pc(CPUState *cs, vaddr value)
 static bool arm_cpu_has_work(CPUState *cs)
 {
     return cs->interrupt_request &
-        (CPU_INTERRUPT_FIQ | CPU_INTERRUPT_HARD | CPU_INTERRUPT_EXITTB);
+        (CPU_INTERRUPT_FIQ | CPU_INTERRUPT_HARD
+         | CPU_INTERRUPT_VFIQ | CPU_INTERRUPT_VIRQ
+         | CPU_INTERRUPT_EXITTB);
 }
 
 static void cp_reg_reset(gpointer key, gpointer value, gpointer opaque)
@@ -210,6 +212,18 @@ bool arm_cpu_exec_interrupt(CPUState *cs, int interrupt_request)
         cc->do_interrupt(cs);
         ret = true;
     }
+    if (interrupt_request & CPU_INTERRUPT_VIRQ
+        && arm_excp_unmasked(cs, EXCP_VIRQ)) {
+        cs->exception_index = EXCP_VIRQ;
+        cc->do_interrupt(cs);
+        ret = true;
+    }
+    if (interrupt_request & CPU_INTERRUPT_VFIQ
+        && arm_excp_unmasked(cs, EXCP_VFIQ)) {
+        cs->exception_index = EXCP_VFIQ;
+        cc->do_interrupt(cs);
+        ret = true;
+    }
 
     return ret;
 }
@@ -218,21 +232,29 @@ bool arm_cpu_exec_interrupt(CPUState *cs, int interrupt_request)
 static void arm_cpu_set_irq(void *opaque, int irq, int level)
 {
     ARMCPU *cpu = opaque;
+    CPUARMState *env = &cpu->env;
     CPUState *cs = CPU(cpu);
+    static const int mask[] = {
+        [ARM_CPU_IRQ] = CPU_INTERRUPT_HARD,
+        [ARM_CPU_FIQ] = CPU_INTERRUPT_FIQ,
+        [ARM_CPU_VIRQ] = CPU_INTERRUPT_VIRQ,
+        [ARM_CPU_VFIQ] = CPU_INTERRUPT_VFIQ
+    };
 
     switch (irq) {
-    case ARM_CPU_IRQ:
-        if (level) {
-            cpu_interrupt(cs, CPU_INTERRUPT_HARD);
-        } else {
-            cpu_reset_interrupt(cs, CPU_INTERRUPT_HARD);
+    case ARM_CPU_VIRQ:
+    case ARM_CPU_VFIQ:
+        if (!arm_feature(env, ARM_FEATURE_EL2)) {
+            hw_error("%s: Virtual interrupt line %d with no EL2 support\n",
+                     __func__, irq);
         }
-        break;
+        /* fall through */
+    case ARM_CPU_IRQ:
     case ARM_CPU_FIQ:
         if (level) {
-            cpu_interrupt(cs, CPU_INTERRUPT_FIQ);
+            cpu_interrupt(cs, mask[irq]);
         } else {
-            cpu_reset_interrupt(cs, CPU_INTERRUPT_FIQ);
+            cpu_reset_interrupt(cs, mask[irq]);
         }
         break;
     default:
@@ -282,9 +304,12 @@ static void arm_cpu_initfn(Object *obj)
 #ifndef CONFIG_USER_ONLY
     /* Our inbound IRQ and FIQ lines */
     if (kvm_enabled()) {
-        qdev_init_gpio_in(DEVICE(cpu), arm_cpu_kvm_set_irq, 2);
+        /* VIRQ and VFIQ are unused with KVM but we add them to maintain
+         * the same interface as non-KVM CPUs.
+         */
+        qdev_init_gpio_in(DEVICE(cpu), arm_cpu_kvm_set_irq, 4);
     } else {
-        qdev_init_gpio_in(DEVICE(cpu), arm_cpu_set_irq, 2);
+        qdev_init_gpio_in(DEVICE(cpu), arm_cpu_set_irq, 4);
     }
 
     cpu->gt_timer[GTIMER_PHYS] = timer_new(QEMU_CLOCK_VIRTUAL, GTIMER_SCALE,
diff --git a/target-arm/cpu.h b/target-arm/cpu.h
index 11ba9d6c02..65a3417951 100644
--- a/target-arm/cpu.h
+++ b/target-arm/cpu.h
@@ -54,6 +54,8 @@
 #define EXCP_HVC            11   /* HyperVisor Call */
 #define EXCP_HYP_TRAP       12
 #define EXCP_SMC            13   /* Secure Monitor Call */
+#define EXCP_VIRQ           14
+#define EXCP_VFIQ           15
 
 #define ARMV7M_EXCP_RESET   1
 #define ARMV7M_EXCP_NMI     2
@@ -68,6 +70,8 @@
 
 /* ARM-specific interrupt pending bits.  */
 #define CPU_INTERRUPT_FIQ   CPU_INTERRUPT_TGT_EXT_1
+#define CPU_INTERRUPT_VIRQ  CPU_INTERRUPT_TGT_EXT_2
+#define CPU_INTERRUPT_VFIQ  CPU_INTERRUPT_TGT_EXT_3
 
 /* The usual mapping for an AArch64 system register to its AArch32
  * counterpart is for the 32 bit world to have access to the lower
@@ -83,9 +87,11 @@
 #define offsetofhigh32(S, M) (offsetof(S, M) + sizeof(uint32_t))
 #endif
 
-/* Meanings of the ARMCPU object's two inbound GPIO lines */
+/* Meanings of the ARMCPU object's four inbound GPIO lines */
 #define ARM_CPU_IRQ 0
 #define ARM_CPU_FIQ 1
+#define ARM_CPU_VIRQ 2
+#define ARM_CPU_VFIQ 3
 
 typedef void ARMWriteCPFunc(void *opaque, int cp_info,
                             int srcreg, int operand, uint32_t value);
@@ -1184,6 +1190,18 @@ static inline bool arm_excp_unmasked(CPUState *cs, unsigned int excp_idx)
     bool secure = false;
     /* If in EL1/0, Physical IRQ routing to EL2 only happens from NS state.  */
     bool irq_can_hyp = !secure && cur_el < 2 && target_el == 2;
+    /* ARMv7-M interrupt return works by loading a magic value
+     * into the PC.  On real hardware the load causes the
+     * return to occur.  The qemu implementation performs the
+     * jump normally, then does the exception return when the
+     * CPU tries to execute code at the magic address.
+     * This will cause the magic PC value to be pushed to
+     * the stack if an interrupt occurred at the wrong time.
+     * We avoid this by disabling interrupts when
+     * pc contains a magic address.
+     */
+    bool irq_unmasked = !(env->daif & PSTATE_I)
+                        && (!IS_M(env) || env->regs[15] < 0xfffffff0);
 
     /* Don't take exceptions if they target a lower EL.  */
     if (cur_el > target_el) {
@@ -1200,8 +1218,19 @@ static inline bool arm_excp_unmasked(CPUState *cs, unsigned int excp_idx)
         if (irq_can_hyp && (env->cp15.hcr_el2 & HCR_IMO)) {
             return true;
         }
-        return !(env->daif & PSTATE_I)
-               && (!IS_M(env) || env->regs[15] < 0xfffffff0);
+        return irq_unmasked;
+    case EXCP_VFIQ:
+        if (!secure && !(env->cp15.hcr_el2 & HCR_FMO)) {
+            /* VFIQs are only taken when hypervized and non-secure.  */
+            return false;
+        }
+        return !(env->daif & PSTATE_F);
+    case EXCP_VIRQ:
+        if (!secure && !(env->cp15.hcr_el2 & HCR_IMO)) {
+            /* VIRQs are only taken when hypervized and non-secure.  */
+            return false;
+        }
+        return irq_unmasked;
     default:
         g_assert_not_reached();
     }
diff --git a/target-arm/helper-a64.c b/target-arm/helper-a64.c
index 0011488ec3..8228e29486 100644
--- a/target-arm/helper-a64.c
+++ b/target-arm/helper-a64.c
@@ -482,9 +482,11 @@ void aarch64_cpu_do_interrupt(CPUState *cs)
         env->cp15.esr_el[new_el] = env->exception.syndrome;
         break;
     case EXCP_IRQ:
+    case EXCP_VIRQ:
         addr += 0x80;
         break;
     case EXCP_FIQ:
+    case EXCP_VFIQ:
         addr += 0x100;
         break;
     default:
diff --git a/target-arm/helper.c b/target-arm/helper.c
index 6135594252..2669e15cb8 100644
--- a/target-arm/helper.c
+++ b/target-arm/helper.c
@@ -3804,6 +3804,10 @@ unsigned int arm_excp_target_el(CPUState *cs, unsigned int excp_idx)
         }
         break;
     }
+    case EXCP_VIRQ:
+    case EXCP_VFIQ:
+        target_el = 1;
+        break;
     default:
         target_el = MAX(cur_el, 1);
         break;
diff --git a/target-arm/internals.h b/target-arm/internals.h
index 1486595916..b7547bbb76 100644
--- a/target-arm/internals.h
+++ b/target-arm/internals.h
@@ -56,6 +56,8 @@ static const char * const excnames[] = {
     [EXCP_HVC] = "Hypervisor Call",
     [EXCP_HYP_TRAP] = "Hypervisor Trap",
     [EXCP_SMC] = "Secure Monitor Call",
+    [EXCP_VIRQ] = "Virtual IRQ",
+    [EXCP_VFIQ] = "Virtual FIQ",
 };
 
 static inline void arm_log_exception(int idx)