[Xen-devel] [PATCH 2/4] arm: add a function to invoke the PSCI handler and use it

[Andre Przywara <andre.przywara@xxxxxxxxxx>]

The PSCI handler is invoked via a secure monitor call with the
arguments defined in registers [1]. Copy the function from the
Linux code and adjust it to work on both ARM32 and ARM64.
Later use that function instead of the generic GIC SEV kick to
actually bring up the secondary CPUs.

[1]: 
http://infocenter.arm.com/help/index.jsp?topic=/com.arm.doc.den0022b/index.html

Signed-off-by: Andre Przywara <andre.przywara@xxxxxxxxxx>
---
 xen/arch/arm/arm32/smpboot.c |  1 -
 xen/arch/arm/smpboot.c       | 39 +++++++++++++++++++++++++++++++++++----
 2 files changed, 35 insertions(+), 5 deletions(-)

diff --git a/xen/arch/arm/arm32/smpboot.c b/xen/arch/arm/arm32/smpboot.c
index 88fe8fb..fcf653f 100644
--- a/xen/arch/arm/arm32/smpboot.c
+++ b/xen/arch/arm/arm32/smpboot.c
@@ -10,7 +10,6 @@ int __init arch_smp_init(void)
 
 int __init arch_cpu_init(int cpu, struct dt_device_node *dn)
 {
-    /* TODO handle PSCI init */
     return 0;
 }
 
diff --git a/xen/arch/arm/smpboot.c b/xen/arch/arm/smpboot.c
index 97bd414..44326d8 100644
--- a/xen/arch/arm/smpboot.c
+++ b/xen/arch/arm/smpboot.c
@@ -89,6 +89,29 @@ smp_clear_cpu_maps (void)
     cpu_logical_map(0) = READ_SYSREG(MPIDR_EL1) & MPIDR_HWID_MASK;
 }
 
+#ifdef CONFIG_ARM_32
+#define REG_PREFIX "r"
+#else
+#define REG_PREFIX "x"
+#endif
+
+static noinline int __invoke_psci_fn_smc(u32 function_id, u32 arg0, u32 arg1,
+                                         u32 arg2)
+{
+    asm volatile(
+        __asmeq("%0", REG_PREFIX"0")
+        __asmeq("%1", REG_PREFIX"1")
+        __asmeq("%2", REG_PREFIX"2")
+        __asmeq("%3", REG_PREFIX"3")
+        "smc #0"
+        : "+r" (function_id)
+        : "r" (arg0), "r" (arg1), "r" (arg2));
+
+    return function_id;
+}
+
+#undef REG_PREFIX
+
 uint32_t psci_host_cpu_on_nr;
 
 static int __init psci_host_init(void)
@@ -393,10 +416,18 @@ int __cpu_up(unsigned int cpu)
         return rc;
     }
 
-    /* We don't know the GIC ID of the CPU until it has woken up, so just 
signal
-     * everyone and rely on our own smp_up_cpu gate to ensure only the one we
-     * want gets through. */
-    send_SGI_allbutself(GIC_SGI_EVENT_CHECK);
+    if ( psci_host_cpu_on_nr != 0 )
+    {
+        /* If the DTB provided a PSCI node, use this for kicking the CPUs */
+        __invoke_psci_fn_smc(
+            psci_host_cpu_on_nr, cpu, __pa(init_secondary), 0);
+    } else
+    {
+        /* We don't know the GIC ID of the CPU until it has woken up, so just
+         * signal everyone and rely on our own smp_up_cpu gate to ensure only
+         * the one we want gets through. */
+        send_SGI_allbutself(GIC_SGI_EVENT_CHECK);
+    }
 
     while ( !cpu_online(cpu) )
     {
-- 
1.7.12.1


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