xen/arm: trap SMC instructions and inject an UND exception

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

Signed-off-by: Ian Campbell <ian.campbell@citrix.com>
Acked-by: Stefano Stabellini <stefano.stabellini@eu.citrix.com>

xen/arch/arm/traps.c

@@ -61,7 +61,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_TWI, HCR_EL2);
+    WRITE_SYSREG(HCR_PTW|HCR_BSU_OUTER|HCR_AMO|HCR_IMO|HCR_VM|HCR_TWI|HCR_TSC, HCR_EL2);
     isb();
 }
 
@@ -241,6 +241,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;
@@ -956,6 +1004,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);

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

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)

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