[PATCH v2 3/3] xen/arm: Enable switch to hyp mode for OMAP5432

[Chen Baozi <baozich@gmail.com>]

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

diff --git a/xen/arch/arm/arm32/mode_switch.S b/xen/arch/arm/arm32/mode_switch.S
index ba2a462..52358b4 100644
--- a/xen/arch/arm/arm32/mode_switch.S
+++ b/xen/arch/arm/arm32/mode_switch.S
@@ -93,7 +93,7 @@ kick_cpus_sgi:
  * r5: Machine ID
  * r12: CPU number
  *
- * This code is specific to the VE model/Arndale, and not intended to be used
+ * This code is specific to the VE model/Arndale/OMAP5, and not intended to be used
  * on production systems.  As such it's a bit hackier than the main
  * boot code in head.S.  In future it will be replaced by better
  * integration with the bootloader/firmware so that Xen always starts
@@ -110,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 6657af6..a001c6a 100644
--- a/xen/include/asm-arm/platforms/omap5.h
+++ b/xen/include/asm-arm/platforms/omap5.h
@@ -26,6 +26,9 @@
 /* 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
 
-- 
1.8.1.4