[Xen-devel] [PATCH v7 02/15] xen/arm: basic PSCI support, implement cpu_on and cpu_off

[Stefano Stabellini <stefano.stabellini@xxxxxxxxxxxxx>]

Implement support for ARM Power State Coordination Interface, PSCI in
short. Support only HVC calls.

Changes in v7:
- replace hard tabs;
- uint32_t/register_t in psci.h function prototypes.

Changes in v6:
- do not trap SMC after all.

Changes in v5:
- remove duplicate is_initialised = 1 from arch_set_info_guest.

Changes in v3:
- move do_psci_* to psci.c;
- trap SMC;
- return PSCI error codes;
- remove Linux boot procotol from secondary cpus;
- implement cpu_off;
- use register_t as second parameter for arm_psci_fn_t;
- unconditionally reset the guest vcpu context on cpu_on.

Changes in v2:
- set is_initialised in arch_set_info_guest;
- zero vcpu_guest_context before using it.

Signed-off-by: Stefano Stabellini <stefano.stabellini@xxxxxxxxxxxxx>
Acked-by: Ian Campbell <ian.campbell@xxxxxxxxxx>
---
 xen/arch/arm/Makefile         |    1 +
 xen/arch/arm/domain_build.c   |    2 +-
 xen/arch/arm/psci.c           |   76 +++++++++++++++++++++++++++++++++++++++++
 xen/arch/arm/traps.c          |   45 ++++++++++++++++++++++++
 xen/include/asm-arm/psci.h    |   24 +++++++++++++
 xen/include/public/arch-arm.h |    2 +
 6 files changed, 149 insertions(+), 1 deletions(-)
 create mode 100644 xen/arch/arm/psci.c
 create mode 100644 xen/include/asm-arm/psci.h

diff --git a/xen/arch/arm/Makefile b/xen/arch/arm/Makefile
index 2106a4f..8f75044 100644
--- a/xen/arch/arm/Makefile
+++ b/xen/arch/arm/Makefile
@@ -5,6 +5,7 @@ subdir-y += platforms
 obj-y += early_printk.o
 obj-y += cpu.o
 obj-y += domain.o
+obj-y += psci.o
 obj-y += domctl.o
 obj-y += sysctl.o
 obj-y += domain_build.o
diff --git a/xen/arch/arm/domain_build.c b/xen/arch/arm/domain_build.c
index 17aadcf..1b2a4fa 100644
--- a/xen/arch/arm/domain_build.c
+++ b/xen/arch/arm/domain_build.c
@@ -419,7 +419,7 @@ int construct_dom0(struct domain *d)
 
     regs->pc = (uint32_t)kinfo.entry;
 
-    regs->cpsr = PSR_ABT_MASK|PSR_FIQ_MASK|PSR_IRQ_MASK|PSR_MODE_SVC;
+    regs->cpsr = PSR_GUEST_INIT;
 
 #ifdef CONFIG_ARM_64
     d->arch.type = kinfo.type;
diff --git a/xen/arch/arm/psci.c b/xen/arch/arm/psci.c
new file mode 100644
index 0000000..1761791
--- /dev/null
+++ b/xen/arch/arm/psci.c
@@ -0,0 +1,76 @@
+/*
+ * 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/errno.h>
+#include <xen/sched.h>
+#include <xen/types.h>
+
+#include <asm/current.h>
+#include <asm/psci.h>
+
+int do_psci_cpu_on(uint32_t vcpuid, register_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 PSCI_EINVAL;
+
+    if ( vcpuid >= d->max_vcpus || (v = d->vcpu[vcpuid]) == NULL )
+        return PSCI_EINVAL;
+
+    if ( (ctxt = alloc_vcpu_guest_context()) == NULL )
+        return PSCI_DENIED;
+
+    memset(ctxt, 0, sizeof(*ctxt));
+    ctxt->user_regs.pc64 = (u64) entry_point;
+    ctxt->sctlr = SCTLR_BASE;
+    ctxt->ttbr0 = 0;
+    ctxt->ttbr1 = 0;
+    ctxt->ttbcr = 0; /* Defined Reset Value */
+    ctxt->user_regs.cpsr = PSR_GUEST_INIT;
+    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 PSCI_DENIED;
+    }
+    domain_unlock(d);
+
+    vcpu_wake(v);
+
+    return PSCI_SUCCESS;
+}
+
+int do_psci_cpu_off(uint32_t power_state)
+{
+    struct vcpu *v = current;
+    if ( !test_and_set_bit(_VPF_down, &v->pause_flags) )
+        vcpu_sleep_nosync(v);
+    return PSCI_SUCCESS;
+}
+
+/*
+ * Local variables:
+ * mode: C
+ * c-file-style: "BSD"
+ * c-basic-offset: 4
+ * indent-tabs-mode: nil
+ * End:
+ */
diff --git a/xen/arch/arm/traps.c b/xen/arch/arm/traps.c
index 55cbb90..c743f2c 100644
--- a/xen/arch/arm/traps.c
+++ b/xen/arch/arm/traps.c
@@ -34,6 +34,7 @@
 #include <asm/event.h>
 #include <asm/regs.h>
 #include <asm/cpregs.h>
+#include <asm/psci.h>
 
 #include "io.h"
 #include "vtimer.h"
@@ -666,6 +667,29 @@ 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, register_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_off, 1),
+    PSCI(cpu_on, 2),
+};
+
 static void do_debug_trap(struct cpu_user_regs *regs, unsigned int code)
 {
     register_t *r;
@@ -695,6 +719,25 @@ 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) )
+    {
+        domain_crash_synchronous();
+        return;
+    }
+
+    psci_call = arm_psci_table[regs->r0].fn;
+    if ( psci_call == NULL )
+    {
+        domain_crash_synchronous();
+        return;
+    }
+    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;
@@ -1013,6 +1056,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 == 0 )
+            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/psci.h b/xen/include/asm-arm/psci.h
new file mode 100644
index 0000000..67d4c35
--- /dev/null
+++ b/xen/include/asm-arm/psci.h
@@ -0,0 +1,24 @@
+#ifndef __ASM_PSCI_H__
+#define __ASM_PSCI_H__
+
+#define PSCI_SUCCESS  0
+#define PSCI_ENOSYS  -1
+#define PSCI_EINVAL  -2
+#define PSCI_DENIED  -3
+
+int do_psci_cpu_on(uint32_t vcpuid, register_t entry_point);
+int do_psci_cpu_off(uint32_t power_state);
+int do_psci_cpu_suspend(uint32_t power_state, register_t entry_point);
+int do_psci_migrate(uint32_t vcpuid);
+
+#endif /* __ASM_PSCI_H__ */
+
+/*
+ * Local variables:
+ * mode: C
+ * c-file-style: "BSD"
+ * c-basic-offset: 4
+ * tab-width: 4
+ * indent-tabs-mode: nil
+ * End:
+ */
diff --git a/xen/include/public/arch-arm.h b/xen/include/public/arch-arm.h
index cea12b2..8aa62d3 100644
--- a/xen/include/public/arch-arm.h
+++ b/xen/include/public/arch-arm.h
@@ -237,6 +237,8 @@ typedef uint64_t xen_callback_t;
 #define PSR_IT_MASK     (0x0600fc00)  /* Thumb If-Then Mask */
 #define PSR_JAZELLE     (1<<24)       /* Jazelle Mode */
 
+#define PSR_GUEST_INIT  (PSR_ABT_MASK|PSR_FIQ_MASK|PSR_IRQ_MASK|PSR_MODE_SVC)
+
 #endif /*  __XEN_PUBLIC_ARCH_ARM_H__ */
 
 /*
-- 
1.7.2.5


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