[PATCH 1/4] xen/arm: basic PSCI support, implement cpu_on

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

Implement support for ARM Power State Coordination Interface, PSCI in
short. The current implementation is based on HVC and only supports the
cpu_on call.

Signed-off-by: Stefano Stabellini <stefano.stabellini@eu.citrix.com>
CC: will.deacon@arm.com
CC: marc.zyngier@arm.com
---
 xen/arch/arm/domain.c        |   66 ++++++++++++++++++++++++++++++++++++++++++
 xen/arch/arm/traps.c         |   41 ++++++++++++++++++++++++++
 xen/include/asm-arm/domain.h |    4 ++
 3 files changed, 111 insertions(+), 0 deletions(-)

diff --git a/xen/arch/arm/domain.c b/xen/arch/arm/domain.c
index 2506369..6542233 100644
--- a/xen/arch/arm/domain.c
+++ b/xen/arch/arm/domain.c
@@ -584,6 +584,72 @@ void arch_dump_vcpu_info(struct vcpu *v)
 {
 }
 
+int do_psci_cpu_on(uint32_t vcpuid, uint32_t entry_point)
+{
+    struct vcpu *v;
+    struct domain *d = current->domain;
+    struct vcpu_guest_context *ctxt;
+    int rc;
+
+    if ( (vcpuid < 0) || (vcpuid >= MAX_VIRT_CPUS) )
+        return -EINVAL;
+
+    if ( vcpuid >= d->max_vcpus || (v = d->vcpu[vcpuid]) == NULL )
+        return -ENOENT;
+
+    if ( v->is_initialised )
+        return -EEXIST;
+    if ( (ctxt = alloc_vcpu_guest_context()) == NULL )
+        return -ENOMEM;
+
+    ctxt->user_regs.pc = entry_point;
+    /* Linux boot protocol. See linux.Documentation/arm/Booting. */
+    ctxt->user_regs.r0 = 0; /* SBZ */
+    /* Machine ID: We use DTB therefore no machine id */
+    ctxt->user_regs.r1 = 0xffffffff;
+    /* ATAGS/DTB: We currently require that the guest kernel to be
+     * using CONFIG_ARM_APPENDED_DTB. Ensure that r2 does not look
+     * like a valid pointer to a set of ATAGS or a DTB.
+     */
+    ctxt->user_regs.r2 = 0xffffffff;
+    ctxt->sctlr = SCTLR_BASE;
+    ctxt->ttbr0 = 0;
+    ctxt->ttbr1 = 0;
+    ctxt->ttbcr = 0; /* Defined Reset Value */
+    ctxt->user_regs.cpsr = PSR_ABT_MASK|PSR_FIQ_MASK|PSR_IRQ_MASK|PSR_MODE_SVC;
+    ctxt->flags = VGCF_online;
+
+    domain_lock(d);
+    rc = arch_set_info_guest(v, ctxt);
+    free_vcpu_guest_context(ctxt);
+
+    if ( rc < 0 )
+    {
+        domain_unlock(d);
+        return rc;
+    }
+
+    clear_bit(_VPF_down, &v->pause_flags);
+    vcpu_wake(v);
+
+    domain_unlock(d);
+
+    return 0;
+}
+
+int do_psci_cpu_off(uint32_t power_state)
+{
+    return -ENOSYS;
+}
+int do_psci_cpu_suspend(uint32_t power_state, uint32_t entry_point)
+{
+    return -ENOSYS;
+}
+int do_psci_migrate(uint32_t vcpuid)
+{
+    return -ENOSYS;
+}
+
 void vcpu_mark_events_pending(struct vcpu *v)
 {
     int already_pending = test_and_set_bit(
diff --git a/xen/arch/arm/traps.c b/xen/arch/arm/traps.c
index b313e7a..1deb92c 100644
--- a/xen/arch/arm/traps.c
+++ b/xen/arch/arm/traps.c
@@ -498,6 +498,31 @@ static arm_hypercall_t arm_hypercall_table[] = {
     HYPERCALL(grant_table_op, 3),
 };
 
+#define __PSCI_cpu_suspend 0
+#define __PSCI_cpu_off     1
+#define __PSCI_cpu_on      2
+#define __PSCI_migrate     3
+
+typedef int (*arm_psci_fn_t)(uint32_t, uint32_t);
+
+typedef struct {
+    arm_psci_fn_t fn;
+    int nr_args;
+} arm_psci_t;
+
+#define PSCI(_name, _nr_args)                                  \
+    [ __PSCI_ ## _name ] =  {                                  \
+        .fn = (arm_psci_fn_t) &do_psci_ ## _name,              \
+        .nr_args = _nr_args,                                   \
+    }
+
+static arm_psci_t arm_psci_table[] = {
+    PSCI(cpu_suspend, 2),
+    PSCI(cpu_off, 1),
+    PSCI(cpu_on, 2),
+    PSCI(migrate, 1),
+};
+
 static void do_debug_trap(struct cpu_user_regs *regs, unsigned int code)
 {
     uint32_t reg, *r;
@@ -525,6 +550,20 @@ static void do_debug_trap(struct cpu_user_regs *regs, unsigned int code)
     }
 }
 
+static void do_trap_psci(struct cpu_user_regs *regs)
+{
+    arm_psci_fn_t psci_call = NULL;
+
+    if ( regs->r0 >= ARRAY_SIZE(arm_psci_table) )
+    {
+        regs->r0 = -ENOSYS;
+        return;
+    }
+
+    psci_call = arm_psci_table[regs->r0].fn;
+    regs->r0 = psci_call(regs->r1, regs->r2);
+}
+
 static void do_trap_hypercall(struct cpu_user_regs *regs, unsigned long iss)
 {
     arm_hypercall_fn_t call = NULL;
@@ -777,6 +816,8 @@ asmlinkage void do_trap_hypervisor(struct cpu_user_regs *regs)
     case HSR_EC_HVC:
         if ( (hsr.iss & 0xff00) == 0xff00 )
             return do_debug_trap(regs, hsr.iss & 0x00ff);
+        if ( !hsr.iss )
+            return do_trap_psci(regs);
         do_trap_hypercall(regs, hsr.iss);
         break;
     case HSR_EC_DATA_ABORT_GUEST:
diff --git a/xen/include/asm-arm/domain.h b/xen/include/asm-arm/domain.h
index 4623940..60298dd 100644
--- a/xen/include/asm-arm/domain.h
+++ b/xen/include/asm-arm/domain.h
@@ -170,6 +170,10 @@ struct arch_vcpu
 void vcpu_show_execution_state(struct vcpu *);
 void vcpu_show_registers(const struct vcpu *);
 
+int do_psci_cpu_on(uint32_t vcpuid, uint32_t entry_point);
+int do_psci_cpu_off(uint32_t power_state);
+int do_psci_cpu_suspend(uint32_t power_state, uint32_t entry_point);
+int do_psci_migrate(uint32_t vcpuid);
 #endif /* __ASM_DOMAIN_H__ */
 
 /*
-- 
1.7.2.5