[PATCH v6 24/27] arm: vtimer

[Stefano Stabellini <stefano.stabellini@eu.citrix.com>]

Emulation of the generic timer kernel registers.

Signed-off-by: Stefano Stabellini <stefano.stabellini@eu.citrix.com>
Signed-off-by: Ian Campbell <ian.campbell@citrix.com>
Signed-off-by: Tim Deegan <Tim.Deegan@citrix.com>
---
 xen/arch/arm/domain.c        |    4 +
 xen/arch/arm/traps.c         |    4 +-
 xen/arch/arm/vtimer.c        |  148 ++++++++++++++++++++++++++++++++++++++++++
 xen/arch/arm/vtimer.h        |   35 ++++++++++
 xen/include/asm-arm/domain.h |    7 ++
 5 files changed, 196 insertions(+), 2 deletions(-)
 create mode 100644 xen/arch/arm/vtimer.c
 create mode 100644 xen/arch/arm/vtimer.h

diff --git a/xen/arch/arm/domain.c b/xen/arch/arm/domain.c
index d33b692..ada89af 100644
--- a/xen/arch/arm/domain.c
+++ b/xen/arch/arm/domain.c
@@ -12,6 +12,7 @@
 #include <asm/irq.h>
 
 #include "gic.h"
+#include "vtimer.h"
 
 DEFINE_PER_CPU(struct vcpu *, curr_vcpu);
 
@@ -215,6 +216,9 @@ int vcpu_initialise(struct vcpu *v)
     if ( (rc = vcpu_vgic_init(v)) != 0 )
         return rc;
 
+    if ( (rc = vcpu_vtimer_init(v)) != 0 )
+        return rc;
+
     return rc;
 }
 
diff --git a/xen/arch/arm/traps.c b/xen/arch/arm/traps.c
index 4346dd7..395d0af 100644
--- a/xen/arch/arm/traps.c
+++ b/xen/arch/arm/traps.c
@@ -468,7 +468,7 @@ static void do_cp15_32(struct cpu_user_regs *regs,
         break;
     case HSR_CPREG32(CNTP_CTL):
     case HSR_CPREG32(CNTP_TVAL):
-        /* emulate timer */
+        BUG_ON(!vtimer_emulate(regs, hsr));
         break;
     default:
         printk("%s p15, %d, r%d, cr%d, cr%d, %d @ %#08x\n",
@@ -498,7 +498,7 @@ static void do_cp15_64(struct cpu_user_regs *regs,
     switch ( hsr.bits & HSR_CP64_REGS_MASK )
     {
     case HSR_CPREG64(CNTPCT):
-        /* emulate timer */
+        BUG_ON(!vtimer_emulate(regs, hsr));
         break;
     default:
         printk("%s p15, %d, r%d, r%d, cr%d @ %#08x\n",
diff --git a/xen/arch/arm/vtimer.c b/xen/arch/arm/vtimer.c
new file mode 100644
index 0000000..6b1152e
--- /dev/null
+++ b/xen/arch/arm/vtimer.c
@@ -0,0 +1,148 @@
+/*
+ * xen/arch/arm/vtimer.c
+ *
+ * ARM Virtual Timer emulation support
+ *
+ * Ian Campbell <ian.campbell@citrix.com>
+ * Copyright (c) 2011 Citrix Systems.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <xen/config.h>
+#include <xen/lib.h>
+#include <xen/timer.h>
+#include <xen/sched.h>
+#include "gic.h"
+
+extern s_time_t ticks_to_ns(uint64_t ticks);
+extern uint64_t ns_to_ticks(s_time_t ns);
+
+static void vtimer_expired(void *data)
+{
+    struct vcpu *v = data;
+    v->arch.vtimer.ctl |= CNTx_CTL_PENDING;
+    v->arch.vtimer.ctl &= ~CNTx_CTL_MASK;
+    vgic_vcpu_inject_irq(v, 30, 1);
+}
+
+int vcpu_vtimer_init(struct vcpu *v)
+{
+    init_timer(&v->arch.vtimer.timer,
+               vtimer_expired, v,
+               smp_processor_id());
+    v->arch.vtimer.ctl = 0;
+    v->arch.vtimer.offset = NOW();
+    v->arch.vtimer.cval = NOW();
+    return 0;
+}
+
+static int vtimer_emulate_32(struct cpu_user_regs *regs, union hsr hsr)
+{
+    struct vcpu *v = current;
+    struct hsr_cp32 cp32 = hsr.cp32;
+    uint32_t *r = &regs->r0 + cp32.reg;
+    s_time_t now;
+
+    switch ( hsr.bits & HSR_CP32_REGS_MASK )
+    {
+    case HSR_CPREG32(CNTP_CTL):
+        if ( cp32.read )
+        {
+            *r = v->arch.vtimer.ctl;
+        }
+        else
+        {
+            v->arch.vtimer.ctl = *r;
+
+            if ( v->arch.vtimer.ctl & CNTx_CTL_ENABLE )
+            {
+                set_timer(&v->arch.vtimer.timer,
+                          v->arch.vtimer.cval + v->arch.vtimer.offset);
+            }
+            else
+                stop_timer(&v->arch.vtimer.timer);
+        }
+
+        return 1;
+
+    case HSR_CPREG32(CNTP_TVAL):
+        now = NOW() - v->arch.vtimer.offset;
+        if ( cp32.read )
+        {
+            *r = (uint32_t)(ns_to_ticks(v->arch.vtimer.cval - now) & 0xffffffffull);
+        }
+        else
+        {
+            v->arch.vtimer.cval = now + ticks_to_ns(*r);
+            if ( v->arch.vtimer.ctl & CNTx_CTL_ENABLE )
+            {
+                set_timer(&v->arch.vtimer.timer,
+                          v->arch.vtimer.cval + v->arch.vtimer.offset);
+            }
+        }
+
+        return 1;
+
+    default:
+        return 0;
+    }
+}
+
+static int vtimer_emulate_64(struct cpu_user_regs *regs, union hsr hsr)
+{
+    struct vcpu *v = current;
+    struct hsr_cp64 cp64 = hsr.cp64;
+    uint32_t *r1 = &regs->r0 + cp64.reg1;
+    uint32_t *r2 = &regs->r0 + cp64.reg2;
+    s_time_t now;
+
+    switch ( hsr.bits & HSR_CP64_REGS_MASK )
+    {
+    case HSR_CPREG64(CNTPCT):
+        if ( cp64.read )
+        {
+            now = NOW() - v->arch.vtimer.offset;
+            *r1 = (uint32_t)(now & 0xffffffff);
+            *r2 = (uint32_t)(now >> 32);
+            return 1;
+        }
+        else
+        {
+            printk("READ from R/O CNTPCT\n");
+            return 0;
+        }
+
+    default:
+        return 0;
+    }
+}
+
+int vtimer_emulate(struct cpu_user_regs *regs, union hsr hsr)
+{
+    switch (hsr.ec) {
+    case HSR_EC_CP15_32:
+        return vtimer_emulate_32(regs, hsr);
+    case HSR_EC_CP15_64:
+        return vtimer_emulate_64(regs, hsr);
+    default:
+        return 0;
+    }
+}
+
+/*
+ * Local variables:
+ * mode: C
+ * c-set-style: "BSD"
+ * c-basic-offset: 4
+ * indent-tabs-mode: nil
+ * End:
+ */
diff --git a/xen/arch/arm/vtimer.h b/xen/arch/arm/vtimer.h
new file mode 100644
index 0000000..d87bb25
--- /dev/null
+++ b/xen/arch/arm/vtimer.h
@@ -0,0 +1,35 @@
+/*
+ * xen/arch/arm/vtimer.h
+ *
+ * ARM Virtual Timer emulation support
+ *
+ * Ian Campbell <ian.campbell@citrix.com>
+ * Copyright (c) 2011 Citrix Systems.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef __ARCH_ARM_VTIMER_H__
+#define __ARCH_ARM_VTIMER_H__
+
+extern int vcpu_vtimer_init(struct vcpu *v);
+extern int vtimer_emulate(struct cpu_user_regs *regs, union hsr hsr);
+
+#endif
+
+/*
+ * Local variables:
+ * mode: C
+ * c-set-style: "BSD"
+ * c-basic-offset: 4
+ * indent-tabs-mode: nil
+ * End:
+ */
diff --git a/xen/include/asm-arm/domain.h b/xen/include/asm-arm/domain.h
index 2cd0bd3..3372d14 100644
--- a/xen/include/asm-arm/domain.h
+++ b/xen/include/asm-arm/domain.h
@@ -57,6 +57,13 @@ struct arch_vcpu
         struct list_head inflight_irqs;
         spinlock_t lock;
     } vgic;
+
+    struct {
+        struct timer timer;
+        uint32_t ctl;
+        s_time_t offset;
+        s_time_t cval;
+    } vtimer;
 }  __cacheline_aligned;
 
 void vcpu_show_execution_state(struct vcpu *);
-- 
1.7.2.5