1 files changed, 11 insertions, 2 deletions

diff --git a/arch/arm/mach-omap2/omap-smp.c b/arch/arm/mach-omap2/omap-smp.c
index 256e84ef0f67..5305ec7341ec 100644
--- 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);
 
 }