xen: arm: handle 64-bit system register access traps.

Wire up the vtimer handling to it.

Use a simplified version of the 32-bit cp-register macros to have convenient
decoding of HSR register values. (simplified because we don't need them for
passing to the assembler on 64-bit)

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

4 files changed, 213 insertions, 44 deletions

diff --git a/xen/arch/arm/traps.c b/xen/arch/arm/traps.c
index c3a08864f3..b4828f307c 100644
--- a/xen/arch/arm/traps.c
+++ b/xen/arch/arm/traps.c
@@ -1091,6 +1091,39 @@ static void do_cp15_64(struct cpu_user_regs *regs,
 
 }
 
+#ifdef CONFIG_ARM_64
+static void do_sysreg(struct cpu_user_regs *regs,
+                      union hsr hsr)
+{
+    struct hsr_sysreg sysreg = hsr.sysreg;
+
+    switch ( hsr.bits & HSR_SYSREG_REGS_MASK )
+    {
+    case CNTP_CTL_EL0:
+    case CNTP_TVAL_EL0:
+        if ( !vtimer_emulate(regs, hsr) )
+        {
+            dprintk(XENLOG_ERR,
+                    "failed emulation of 64-bit vtimer sysreg access\n");
+            domain_crash_synchronous();
+        }
+        break;
+    default:
+        printk("%s %d, %d, c%d, c%d, %d %s x%d @ 0x%"PRIregister"\n",
+               sysreg.read ? "mrs" : "msr",
+               sysreg.op0, sysreg.op1,
+               sysreg.crn, sysreg.crm,
+               sysreg.op2,
+               sysreg.read ? "=>" : "<=",
+               sysreg.reg, regs->pc);
+        panic("unhandled 64-bit sysreg access %#x\n",
+              hsr.bits & HSR_SYSREG_REGS_MASK);
+    }
+
+    regs->pc += 4;
+}
+#endif
+
 void dump_guest_s1_walk(struct domain *d, vaddr_t addr)
 {
     uint32_t ttbcr = READ_SYSREG32(TCR_EL1);
@@ -1258,7 +1291,13 @@ asmlinkage void do_trap_hypervisor(struct cpu_user_regs *regs)
             return do_trap_psci(regs);
         do_trap_hypercall(regs, &regs->x16, hsr.iss);
         break;
+    case HSR_EC_SYSREG:
+        if ( is_pv32_domain(current->domain) )
+            goto bad_trap;
+        do_sysreg(regs, hsr);
+        break;
 #endif
+
     case HSR_EC_DATA_ABORT_GUEST:
         do_trap_data_abort_guest(regs, hsr.dabt);
         break;
diff --git a/xen/arch/arm/vtimer.c b/xen/arch/arm/vtimer.c
index aee762a07d..d58a630ad8 100644
--- a/xen/arch/arm/vtimer.c
+++ b/xen/arch/arm/vtimer.c
@@ -112,57 +112,67 @@ int virt_timer_restore(struct vcpu *v)
     return 0;
 }
 
-static int vtimer_emulate_32(struct cpu_user_regs *regs, union hsr hsr)
+static void vtimer_cntp_ctl(struct cpu_user_regs *regs, uint32_t *r, int read)
 {
     struct vcpu *v = current;
-    struct hsr_cp32 cp32 = hsr.cp32;
-    uint32_t *r = (uint32_t *)select_user_reg(regs, cp32.reg);
-    s_time_t now;
-
-    switch ( hsr.bits & HSR_CP32_REGS_MASK )
+    if ( read )
     {
-    case HSR_CPREG32(CNTP_CTL):
-        if ( cp32.read )
+        *r = v->arch.phys_timer.ctl;
+    }
+    else
+    {
+        uint32_t ctl = *r & ~CNTx_CTL_PENDING;
+        if ( ctl & CNTx_CTL_ENABLE )
+            ctl |= v->arch.phys_timer.ctl & CNTx_CTL_PENDING;
+        v->arch.phys_timer.ctl = ctl;
+
+        if ( v->arch.phys_timer.ctl & CNTx_CTL_ENABLE )
         {
-            *r = v->arch.phys_timer.ctl;
+            set_timer(&v->arch.phys_timer.timer,
+                      v->arch.phys_timer.cval + v->domain->arch.phys_timer_base.offset);
         }
         else
+            stop_timer(&v->arch.phys_timer.timer);
+    }
+}
+
+static void vtimer_cntp_tval(struct cpu_user_regs *regs, uint32_t *r, int read)
+{
+    struct vcpu *v = current;
+    s_time_t now;
+
+    now = NOW() - v->domain->arch.phys_timer_base.offset;
+
+    if ( read )
+    {
+        *r = (uint32_t)(ns_to_ticks(v->arch.phys_timer.cval - now) & 0xffffffffull);
+    }
+    else
+    {
+        v->arch.phys_timer.cval = now + ticks_to_ns(*r);
+        if ( v->arch.phys_timer.ctl & CNTx_CTL_ENABLE )
         {
-            uint32_t ctl = *r & ~CNTx_CTL_PENDING;
-            if ( ctl & CNTx_CTL_ENABLE )
-                ctl |= v->arch.phys_timer.ctl & CNTx_CTL_PENDING;
-            v->arch.phys_timer.ctl = ctl;
-
-            if ( v->arch.phys_timer.ctl & CNTx_CTL_ENABLE )
-            {
-                set_timer(&v->arch.phys_timer.timer,
-                          v->arch.phys_timer.cval +
-                          v->domain->arch.phys_timer_base.offset);
-            }
-            else
-                stop_timer(&v->arch.phys_timer.timer);
+            v->arch.phys_timer.ctl &= ~CNTx_CTL_PENDING;
+            set_timer(&v->arch.phys_timer.timer,
+                      v->arch.phys_timer.cval +
+                      v->domain->arch.phys_timer_base.offset);
         }
+    }
+}
 
+static int vtimer_emulate_cp32(struct cpu_user_regs *regs, union hsr hsr)
+{
+    struct hsr_cp32 cp32 = hsr.cp32;
+    uint32_t *r = (uint32_t *)select_user_reg(regs, cp32.reg);
+
+    switch ( hsr.bits & HSR_CP32_REGS_MASK )
+    {
+    case HSR_CPREG32(CNTP_CTL):
+        vtimer_cntp_ctl(regs, r, cp32.read);
         return 1;
 
     case HSR_CPREG32(CNTP_TVAL):
-        now = NOW() - v->domain->arch.phys_timer_base.offset;
-        if ( cp32.read )
-        {
-            *r = (uint32_t)(ns_to_ticks(v->arch.phys_timer.cval - now) & 0xffffffffull);
-        }
-        else
-        {
-            v->arch.phys_timer.cval = now + ticks_to_ns(*r);
-            if ( v->arch.phys_timer.ctl & CNTx_CTL_ENABLE )
-            {
-                v->arch.phys_timer.ctl &= ~CNTx_CTL_PENDING;
-                set_timer(&v->arch.phys_timer.timer,
-                          v->arch.phys_timer.cval +
-                          v->domain->arch.phys_timer_base.offset);
-            }
-        }
-
+        vtimer_cntp_tval(regs, r, cp32.read);
         return 1;
 
     default:
@@ -170,7 +180,7 @@ static int vtimer_emulate_32(struct cpu_user_regs *regs, union hsr hsr)
     }
 }
 
-static int vtimer_emulate_64(struct cpu_user_regs *regs, union hsr hsr)
+static int vtimer_emulate_cp64(struct cpu_user_regs *regs, union hsr hsr)
 {
     struct vcpu *v = current;
     struct hsr_cp64 cp64 = hsr.cp64;
@@ -201,16 +211,48 @@ static int vtimer_emulate_64(struct cpu_user_regs *regs, union hsr hsr)
     }
 }
 
+#ifdef CONFIG_ARM_64
+static int vtimer_emulate_sysreg(struct cpu_user_regs *regs, union hsr hsr)
+{
+    struct hsr_sysreg sysreg = hsr.sysreg;
+    register_t *x = select_user_reg(regs, sysreg.reg);
+    uint32_t r = (uint32_t)*x;
+
+    switch ( hsr.bits & HSR_SYSREG_REGS_MASK )
+    {
+    case CNTP_CTL_EL0:
+        vtimer_cntp_ctl(regs, &r, sysreg.read);
+        *x = r;
+        return 1;
+    case CNTP_TVAL_EL0:
+        vtimer_cntp_tval(regs, &r, sysreg.read);
+        *x = r;
+        return 1;
+    default:
+        return 0;
+    }
+
+}
+#endif
+
 int vtimer_emulate(struct cpu_user_regs *regs, union hsr hsr)
 {
-    if ( !is_pv32_domain(current->domain) )
-        return -EINVAL;
 
     switch (hsr.ec) {
     case HSR_EC_CP15_32:
-        return vtimer_emulate_32(regs, hsr);
+        if ( !is_pv32_domain(current->domain) )
+            return 0;
+        return vtimer_emulate_cp32(regs, hsr);
     case HSR_EC_CP15_64:
-        return vtimer_emulate_64(regs, hsr);
+        if ( !is_pv32_domain(current->domain) )
+            return 0;
+        return vtimer_emulate_cp64(regs, hsr);
+#ifdef CONFIG_ARM_64
+    case HSR_EC_SYSREG:
+        if ( is_pv32_domain(current->domain) )
+            return 0;
+        return vtimer_emulate_sysreg(regs, hsr);
+#endif
     default:
         return 0;
     }
diff --git a/xen/include/asm-arm/processor.h b/xen/include/asm-arm/processor.h
index d662f07208..fa175d905f 100644
--- a/xen/include/asm-arm/processor.h
+++ b/xen/include/asm-arm/processor.h
@@ -2,6 +2,7 @@
 #define __ASM_ARM_PROCESSOR_H
 
 #include <asm/cpregs.h>
+#include <asm/sysregs.h>
 
 /* MIDR Main ID Register */
 #define MIDR_MASK    0xff0ffff0
@@ -97,6 +98,7 @@
 #define HSR_EC_SMC                  0x13
 #ifdef CONFIG_ARM_64
 #define HSR_EC_HVC64                0x16
+#define HSR_EC_SYSREG               0x18
 #endif
 #define HSR_EC_INSTR_ABORT_GUEST    0x20
 #define HSR_EC_INSTR_ABORT_HYP      0x21
@@ -256,6 +258,21 @@ union hsr {
         unsigned long ec:6;    /* Exception Class */
     } cp64; /* HSR_EC_CP15_64, HSR_EC_CP14_64 */
 
+#ifdef CONFIG_ARM_64
+    struct hsr_sysreg {
+        unsigned long read:1;   /* Direction */
+        unsigned long crm:4;    /* CRm */
+        unsigned long reg:5;    /* Rt */
+        unsigned long crn:4;    /* CRn */
+        unsigned long op1:3;    /* Op1 */
+        unsigned long op2:3;    /* Op2 */
+        unsigned long op0:2;    /* Op0 */
+        unsigned long res0:3;
+        unsigned long len:1;    /* Instruction length */
+        unsigned long ec:6;
+    } sysreg; /* HSR_EC_SYSREG */
+#endif
+
     struct hsr_dabt {
         unsigned long dfsc:6;  /* Data Fault Status Code */
         unsigned long write:1; /* Write / not Read */
@@ -298,6 +315,21 @@ union hsr {
 #define HSR_CP64_CRM_SHIFT (1)
 #define HSR_CP64_REGS_MASK (HSR_CP64_OP1_MASK|HSR_CP64_CRM_MASK)
 
+/* HSR.EC == HSR_SYSREG */
+#define HSR_SYSREG_OP0_MASK (0x00300000)
+#define HSR_SYSREG_OP0_SHIFT (20)
+#define HSR_SYSREG_OP1_MASK (0x0001c000)
+#define HSR_SYSREG_OP1_SHIFT (14)
+#define HSR_SYSREG_CRN_MASK (0x00003800)
+#define HSR_SYSREG_CRN_SHIFT (10)
+#define HSR_SYSREG_CRM_MASK (0x0000001e)
+#define HSR_SYSREG_CRM_SHIFT (1)
+#define HSR_SYSREG_OP2_MASK (0x000e0000)
+#define HSR_SYSREG_OP2_SHIFT (17)
+#define HSR_SYSREG_REGS_MASK (HSR_SYSREG_OP0_MASK|HSR_SYSREG_OP1_MASK|\
+                              HSR_SYSREG_CRN_MASK|HSR_SYSREG_CRM_MASK|\
+                              HSR_SYSREG_OP2_MASK)
+
 /* Physical Address Register */
 #define PAR_F           (1<<0)
 
diff --git a/xen/include/asm-arm/sysregs.h b/xen/include/asm-arm/sysregs.h
new file mode 100644
index 0000000000..9c64777a72
--- /dev/null
+++ b/xen/include/asm-arm/sysregs.h
@@ -0,0 +1,56 @@
+#ifndef __ASM_ARM_SYSREGS_H
+#define __ASM_ARM_SYSREGS_H
+
+#ifdef CONFIG_ARM_64
+
+#include <xen/stringify.h>
+
+/* AArch 64 System Register Encodings */
+#define __HSR_SYSREG_c0  0
+#define __HSR_SYSREG_c1  1
+#define __HSR_SYSREG_c2  2
+#define __HSR_SYSREG_c3  3
+#define __HSR_SYSREG_c4  4
+#define __HSR_SYSREG_c5  5
+#define __HSR_SYSREG_c6  6
+#define __HSR_SYSREG_c7  7
+#define __HSR_SYSREG_c8  8
+#define __HSR_SYSREG_c9  9
+#define __HSR_SYSREG_c10 10
+#define __HSR_SYSREG_c11 11
+#define __HSR_SYSREG_c12 12
+#define __HSR_SYSREG_c13 13
+#define __HSR_SYSREG_c14 14
+#define __HSR_SYSREG_c15 15
+
+#define __HSR_SYSREG_0   0
+#define __HSR_SYSREG_1   1
+#define __HSR_SYSREG_2   2
+#define __HSR_SYSREG_3   3
+#define __HSR_SYSREG_4   4
+#define __HSR_SYSREG_5   5
+#define __HSR_SYSREG_6   6
+#define __HSR_SYSREG_7   7
+
+/* These are used to decode traps with HSR.EC==HSR_EC_SYSREG */
+#define HSR_SYSREG(op0,op1,crn,crm,op2) \
+    ((__HSR_SYSREG_##op0) << HSR_SYSREG_OP0_SHIFT) | \
+    ((__HSR_SYSREG_##op1) << HSR_SYSREG_OP1_SHIFT) | \
+    ((__HSR_SYSREG_##crn) << HSR_SYSREG_CRN_SHIFT) | \
+    ((__HSR_SYSREG_##crm) << HSR_SYSREG_CRM_SHIFT) | \
+    ((__HSR_SYSREG_##op2) << HSR_SYSREG_OP2_SHIFT)
+
+#define CNTP_CTL_EL0  HSR_SYSREG(3,3,c14,c2,1)
+#define CNTP_TVAL_EL0 HSR_SYSREG(3,3,c14,c2,0)
+#endif
+
+#endif
+
+/*
+ * Local variables:
+ * mode: C
+ * c-set-style: "BSD"
+ * c-basic-offset: 4
+ * indent-tabs-mode: nil
+ * End:
+ */