[Xen-devel] [PATCH] xen/arm: trap SMC instructions and inject an UND exception

[Ian Campbell <ian.campbell@xxxxxxxxxx>]

Currently only handles 32 bit guests. The 64-bit exception model is
considerably different.

Signed-off-by: Ian Campbell <ian.campbell@xxxxxxxxxx>
---
There are probably other places where injecting UND would be more appropriate
than killing the guest. It should also be reasonably easy to extend this logic
to inject DABT and PABT.
---
 xen/arch/arm/traps.c                  |   56 ++++++++++++++++++++++++++++++++-
 xen/include/asm-arm/arm32/processor.h |    4 ++-
 xen/include/asm-arm/processor.h       |    8 +++++
 xen/include/public/arch-arm.h         |    1 +
 4 files changed, 67 insertions(+), 2 deletions(-)

diff --git a/xen/arch/arm/traps.c b/xen/arch/arm/traps.c
index b7487b7..72dddb2 100644
--- a/xen/arch/arm/traps.c
+++ b/xen/arch/arm/traps.c
@@ -59,7 +59,7 @@ void __cpuinit init_traps(void)
     WRITE_SYSREG((vaddr_t)hyp_traps_vector, VBAR_EL2);
 
     /* Setup hypervisor traps */
-    WRITE_SYSREG(HCR_PTW|HCR_BSU_OUTER|HCR_AMO|HCR_IMO|HCR_VM, HCR_EL2);
+    WRITE_SYSREG(HCR_PTW|HCR_BSU_OUTER|HCR_AMO|HCR_IMO|HCR_TSC|HCR_VM, 
HCR_EL2);
     isb();
 }
 
@@ -239,6 +239,54 @@ void panic_PAR(uint64_t par)
     panic("Error during Hypervisor-to-physical address translation\n");
 }
 
+static void cpsr_switch_mode(struct cpu_user_regs *regs, int mode)
+{
+    uint32_t sctlr = READ_SYSREG32(SCTLR_EL1);
+
+    regs->cpsr &= 
~(PSR_MODE_MASK|PSR_IT_MASK|PSR_JAZELLE|PSR_BIG_ENDIAN|PSR_THUMB);
+
+    regs->cpsr |= mode;
+    regs->cpsr |= PSR_IRQ_MASK;
+    if (sctlr & SCTLR_TE)
+        regs->cpsr |= PSR_THUMB;
+    if (sctlr & SCTLR_EE)
+        regs->cpsr |= PSR_BIG_ENDIAN;
+}
+
+static vaddr_t exception_handler(vaddr_t offset)
+{
+    uint32_t sctlr = READ_SYSREG32(SCTLR_EL1);
+
+    if (sctlr & SCTLR_V)
+        return 0xffff0000 + offset;
+    else /* always have security exceptions */
+        return READ_SYSREG(VBAR_EL1) + offset;
+}
+
+/* Injects an Undefined Instruction exception into the current vcpu,
+ * PC is the exact address of the faulting instruction (without
+ * pipeline adjustments). See TakeUndefInstrException pseudocode in
+ * ARM.
+ */
+static void inject_undef_exception(struct cpu_user_regs *regs,
+                                   register_t preferred_return)
+{
+    uint32_t spsr = regs->cpsr;
+    int is_thumb = (regs->cpsr & PSR_THUMB);
+    /* Saved PC points to the instruction past the faulting instruction. */
+    uint32_t return_offset = is_thumb ? 2 : 4;
+
+    /* Update processor mode */
+    cpsr_switch_mode(regs, PSR_MODE_UND);
+
+    /* Update banked registers */
+    regs->spsr_und = spsr;
+    regs->lr_und = preferred_return + return_offset;
+
+    /* Branch to exception vector */
+    regs->pc32 = exception_handler(VECTOR32_UND);
+}
+
 struct reg_ctxt {
     uint32_t sctlr, tcr;
     uint64_t ttbr0, ttbr1;
@@ -941,6 +989,12 @@ asmlinkage void do_trap_hypervisor(struct cpu_user_regs 
*regs)
             goto bad_trap;
         do_cp15_64(regs, hsr);
         break;
+    case HSR_EC_SMC:
+        /* PC32 already contains the preferred exception return
+         * address, so no need to adjust here.
+         */
+        inject_undef_exception(regs, regs->pc32);
+        break;
     case HSR_EC_HVC:
         if ( (hsr.iss & 0xff00) == 0xff00 )
             return do_debug_trap(regs, hsr.iss & 0x00ff);
diff --git a/xen/include/asm-arm/arm32/processor.h 
b/xen/include/asm-arm/arm32/processor.h
index cd79170..d26fc85 100644
--- a/xen/include/asm-arm/arm32/processor.h
+++ b/xen/include/asm-arm/arm32/processor.h
@@ -31,7 +31,9 @@ struct cpu_user_regs
         uint32_t lr_usr;
     };
 
-    uint32_t pc; /* Return IP */
+    union {  /* Return IP, pc32 is used to allow code to be common with 64-bit 
*/
+        uint32_t pc, pc32;
+    };
     uint32_t cpsr; /* Return mode */
     uint32_t pad0; /* Doubleword-align the kernel half of the frame */
 
diff --git a/xen/include/asm-arm/processor.h b/xen/include/asm-arm/processor.h
index 1681ebf..1c9d793 100644
--- a/xen/include/asm-arm/processor.h
+++ b/xen/include/asm-arm/processor.h
@@ -85,6 +85,7 @@
 #define HSR_EC_CP14_64              0x0c
 #define HSR_EC_SVC                  0x11
 #define HSR_EC_HVC                  0x12
+#define HSR_EC_SMC                  0x13
 #define HSR_EC_INSTR_ABORT_GUEST    0x20
 #define HSR_EC_INSTR_ABORT_HYP      0x21
 #define HSR_EC_DATA_ABORT_GUEST     0x24
@@ -342,6 +343,13 @@ union hsr {
 #define CNTx_CTL_MASK     (1u<<1)  /* Mask IRQ */
 #define CNTx_CTL_PENDING  (1u<<2)  /* IRQ pending */
 
+/* Exception Vector offsets */
+#define VECTOR32_RST  0
+#define VECTOR32_UND  4
+#define VECTOR32_SVC  8
+#define VECTOR32_PABT 12
+#define VECTOR32_DABT 16
+
 #if defined(CONFIG_ARM_32)
 # include <asm/arm32/processor.h>
 #elif defined(CONFIG_ARM_64)
diff --git a/xen/include/public/arch-arm.h b/xen/include/public/arch-arm.h
index 2f5ce18..cea12b2 100644
--- a/xen/include/public/arch-arm.h
+++ b/xen/include/public/arch-arm.h
@@ -234,6 +234,7 @@ typedef uint64_t xen_callback_t;
 #define PSR_IRQ_MASK    (1<<7)        /* Interrupt mask */
 #define PSR_ABT_MASK    (1<<8)        /* Asynchronous Abort mask */
 #define PSR_BIG_ENDIAN  (1<<9)        /* Big Endian Mode */
+#define PSR_IT_MASK     (0x0600fc00)  /* Thumb If-Then Mask */
 #define PSR_JAZELLE     (1<<24)       /* Jazelle Mode */
 
 #endif /*  __XEN_PUBLIC_ARCH_ARM_H__ */
-- 
1.7.2.5


_______________________________________________
Xen-devel mailing list
Xen-devel@xxxxxxxxxxxxx
http://lists.xen.org/xen-devel