ARM: omap5/dra7xx: Enable booting secondary CPU in HYP mode

If the boot loader enables HYP mode on the boot CPU, the secondary CPU
also needs to call into the ROM to switch to HYP mode before booting.
The firmwares on the omap5 and dra7xx unfortunately do not take care
of this, so it has to be handled by the kernel.

This patch is based on "[PATCH 2/2] ARM: OMAP5: Add HYP mode entry support
for secondary CPUs" by Santosh Shilimkar <santosh.shilimkar@ti.com>,
except this version does not require a compile time CONFIG to control
if it should enable HYP mode or not, it simply does it based on the mode
of the boot CPU, so it works whether the CPU boots in SVC or HYP mode,
and should even work as a guest kernel inside kvm if qemu decides to
support emulating the omap5 or dra7xx.

Cc: stable@vger.kernel.org #v3.16+
Signed-off-by: Len Sorensen <lsorense@csclub.uwaterloo.ca>
Signed-off-by: Tony Lindgren <tony@atomide.com>

diff --git a/arch/arm/mach-omap2/common.h b/arch/arm/mach-omap2/common.h
index 377eea849e7bcdaf1142f6b1087ed6857a92b046..db57741c9c8ae69175f007ddca876c2bd8afe9cc 100644 (file)
--- a/arch/arm/mach-omap2/common.h
+++ b/arch/arm/mach-omap2/common.h
@@ -249,6 +249,7 @@ extern void omap4_cpu_die(unsigned int cpu);
 extern struct smp_operations omap4_smp_ops;
 
 extern void omap5_secondary_startup(void);
+extern void omap5_secondary_hyp_startup(void);
 #endif
 
 #if defined(CONFIG_SMP) && defined(CONFIG_PM)

diff --git a/arch/arm/mach-omap2/omap-headsmp.S b/arch/arm/mach-omap2/omap-headsmp.S
index 4993d4bfe9b2a579d7adcc37726cb6828a78f130..6d1dffca6c7b6d68f3bd6e29a16bf5334c54a5fb 100644 (file)
--- a/arch/arm/mach-omap2/omap-headsmp.S
+++ b/arch/arm/mach-omap2/omap-headsmp.S
@@ -22,6 +22,7 @@
 
 /* Physical address needed since MMU not enabled yet on secondary core */
 #define AUX_CORE_BOOT0_PA                      0x48281800
+#define API_HYP_ENTRY                          0x102
 
 /*
  * OMAP5 specific entry point for secondary CPU to jump from ROM
@@ -40,6 +41,26 @@ wait:        ldr     r2, =AUX_CORE_BOOT0_PA  @ read from AuxCoreBoot0
        bne     wait
        b       secondary_startup
 ENDPROC(omap5_secondary_startup)
+/*
+ * Same as omap5_secondary_startup except we call into the ROM to
+ * enable HYP mode first.  This is called instead of
+ * omap5_secondary_startup if the primary CPU was put into HYP mode by
+ * the boot loader.
+ */
+ENTRY(omap5_secondary_hyp_startup)
+wait_2:        ldr     r2, =AUX_CORE_BOOT0_PA  @ read from AuxCoreBoot0
+       ldr     r0, [r2]
+       mov     r0, r0, lsr #5
+       mrc     p15, 0, r4, c0, c0, 5
+       and     r4, r4, #0x0f
+       cmp     r0, r4
+       bne     wait_2
+       ldr     r12, =API_HYP_ENTRY
+       adr     r0, hyp_boot
+       smc     #0
+hyp_boot:
+       b       secondary_startup
+ENDPROC(omap5_secondary_hyp_startup)
 /*
  * OMAP4 specific entry point for secondary CPU to jump from ROM
  * code.  This routine also provides a holding flag into which

diff --git a/arch/arm/mach-omap2/omap-smp.c b/arch/arm/mach-omap2/omap-smp.c
index 256e84ef0f679072324892a04d90817b81ceee36..5305ec7341eca5579398a10b72f263a2fbbe8e0e 100644 (file)
--- a/arch/arm/mach-omap2/omap-smp.c
+++ b/arch/arm/mach-omap2/omap-smp.c
@@ -22,6 +22,7 @@
 #include <linux/irqchip/arm-gic.h>
 
 #include <asm/smp_scu.h>
+#include <asm/virt.h>
 
 #include "omap-secure.h"
 #include "omap-wakeupgen.h"
@@ -227,8 +228,16 @@ static void __init omap4_smp_prepare_cpus(unsigned int max_cpus)
        if (omap_secure_apis_support())
                omap_auxcoreboot_addr(virt_to_phys(startup_addr));
        else
-               writel_relaxed(virt_to_phys(omap5_secondary_startup),
-                              base + OMAP_AUX_CORE_BOOT_1);
+               /*
+                * If the boot CPU is in HYP mode then start secondary
+                * CPU in HYP mode as well.
+                */
+               if ((__boot_cpu_mode & MODE_MASK) == HYP_MODE)
+                       writel_relaxed(virt_to_phys(omap5_secondary_hyp_startup),
+                                      base + OMAP_AUX_CORE_BOOT_1);
+               else
+                       writel_relaxed(virt_to_phys(omap5_secondary_startup),
+                                      base + OMAP_AUX_CORE_BOOT_1);
 
 }