[PATCH] xen/arm: Support kick cpus and switch to hypervisor for the OMAP5

[Chen Baozi <baozich@gmail.com>]

Signed-off-by: Chen Baozi <baozich@gmail.com>
---
 xen/arch/arm/arm32/mode_switch.S      | 34 +++++++++++++++++++++++++++++++---
 xen/include/asm-arm/platforms/omap5.h | 17 +++++++++++++++++
 2 files changed, 48 insertions(+), 3 deletions(-)

diff --git a/xen/arch/arm/arm32/mode_switch.S b/xen/arch/arm/arm32/mode_switch.S
index 3500eb0..e86d3d1 100644
--- a/xen/arch/arm/arm32/mode_switch.S
+++ b/xen/arch/arm/arm32/mode_switch.S
@@ -21,11 +21,13 @@
 #include <asm/page.h>
 #include <asm/platforms/vexpress.h>
 #include <asm/platforms/exynos5.h>
+#include <asm/platforms/omap5.h>
 #include <asm/asm_defns.h>
 #include <asm/gic.h>
 
 /* Wake up secondary cpus
- * This code relies on Machine ID and only works for Vexpress and the Arndale
+ * This code relies on Machine ID and only works for Vexpress, the Arndale
+ * and the OMAP5 uEVM.
  * TODO: Move this code either later (via platform specific desc) or in a bootwrapper
  * r5: Machine ID
  * Clobber r0 r2 */
@@ -34,6 +36,10 @@ kick_cpus:
         ldr   r0, =MACH_TYPE_SMDK5250
         teq   r5, r0                          /* Are we running on the arndale? */
         beq   kick_cpus_arndale
+	/* for OMAP5432 */
+	ldr   r0, =MACH_TYPE_OMAP5_SEVM
+	teq   r5, r0
+	beq   kick_cpus_omap5
         /* otherwise versatile express */
         /* write start paddr to v2m sysreg FLAGSSET register */
         ldr   r0, =(V2M_SYS_MMIO_BASE)        /* base V2M sysreg MMIO address */
@@ -55,6 +61,20 @@ kick_cpus_arndale:
         str   r2, [r0]
         dsb
         ldr   r2, =EXYNOS5_GIC_BASE_ADDRESS   /* r2 := Exynos5 gic base address */
+	b     kick_cpus_sgi
+kick_cpus_omap5:
+	/* write start paddr to AuxCoreBoot1 where ROM code will jump */
+        ldr   r0, =(OMAP_AUX_CORE_BOOT_1)
+        ldr   r2, =start
+        add   r2, r2, r10
+        str   r2, [r0]
+        ldr   r0, =(OMAP_AUX_CORE_BOOT_0)
+        mov   r2, #0x20
+        str   r2, [r0]
+        dsb
+        sev
+        ldr   r2, =OMAP5_GIC_BASE_ADDRESS     /* r2 := OMAP5 gic base address */
+        b     kick_cpus_sgi
 kick_cpus_sgi:
         /* send an interrupt */
         ldr   r0, =GIC_DR_OFFSET              /* GIC distributor offset */
@@ -90,15 +110,23 @@ enter_hyp_mode:
         bic   r0, r0, #0xe           /* Clear EA, FIQ and IRQ */
         mcr   CP32(r0, SCR)
 
-        ldr   r2, =MACH_TYPE_SMDK5250   /* r4 := Arndale machine ID */
         /* By default load Arndale defaults values */
+        ldr   r2, =MACH_TYPE_SMDK5250       /* r2 := Arndale machine ID */
         ldr   r0, =EXYNOS5_TIMER_FREQUENCY  /* r0 := timer's frequency */
         ldr   r1, =EXYNOS5_GIC_BASE_ADDRESS /* r1 := GIC base address */
-        /* If it's not the Arndale machine ID, load VE values */
         teq   r5, r2
+        beq   1f
+        /* If it's not the Arndale machine ID, try OMAP5 uEVM */
+        ldr   r2, =MACH_TYPE_OMAP5_SEVM     /* r2 := OMAP5 uEVM machine ID */
+        ldr   r0, =OMAP5_TIMER_FREQUENCY    /* r0 := timer's frequency */
+        ldr   r1, =OMAP5_GIC_BASE_ADDRESS   /* r1 := GIC base address */
+        teq   r5, r2
+        beq   1f
+        /* If it's not the OMAP5432 machine ID, load VE values */
         ldrne r0, =V2M_TIMER_FREQUENCY
         ldrne r1, =V2M_GIC_BASE_ADDRESS
 
+1:
         /* Ugly: the system timer's frequency register is only
          * programmable in Secure state.  Since we don't know where its
          * memory-mapped control registers live, we can't find out the
diff --git a/xen/include/asm-arm/platforms/omap5.h b/xen/include/asm-arm/platforms/omap5.h
index 092f340..a8b0937 100644
--- a/xen/include/asm-arm/platforms/omap5.h
+++ b/xen/include/asm-arm/platforms/omap5.h
@@ -13,6 +13,23 @@
 #define OMAP5_CM_CLKSEL_SYS                     0x10
 #define SYS_CLKSEL_MASK                         0xfffffff8
 
+#define OMAP_AUX_CORE_BOOT_0                    0x48281800
+#define OMAP_AUX_CORE_BOOT_1                    0x48281804
+
+/* Constants below is only used in assembly because the DTS is not yet parsed */
+#ifdef __ASSEMBLY__
+
+/* GIC Base Address */
+#define OMAP5_GIC_BASE_ADDRESS                  0x48210000
+
+/* Timer's frequency */
+#define OMAP5_TIMER_FREQUENCY                   6144000 /*  6.144 Mhz */
+
+/* OMAP5432 uEVM machine ID */
+#define MACH_TYPE_OMAP5_SEVM                    3777
+
+#endif /* __ASSEMBLY__ */
+
 #endif /* __ASM_ARM_PLATFORMS_OMAP5_H */
 
 /*
-- 
1.8.1.4