2 files changed, 73 insertions, 3 deletions

diff --git a/arch/arm64/include/asm/virt.h b/arch/arm64/include/asm/virt.h
new file mode 100644
index 000000000000..f28547d9edfa
--- /dev/null
+++ b/arch/arm64/include/asm/virt.h
@@ -0,0 +1,51 @@
+/*
+ * Copyright (C) 2012 ARM Ltd.
+ * Author: Marc Zyngier <marc.zyngier@arm.com>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __ASM__VIRT_H
+#define __ASM__VIRT_H
+
+#define BOOT_CPU_MODE_EL2       (0x0e12b007)
+
+#ifndef __ASSEMBLY__
+
+/*
+ * __boot_cpu_mode records what mode CPUs were booted in.
+ * A correctly-implemented bootloader must start all CPUs in the same mode:
+ * In this case, both 32bit halves of __boot_cpu_mode will contain the
+ * same value (either 0 if booted in EL1, BOOT_CPU_MODE_EL2 if booted in EL2).
+ *
+ * Should the bootloader fail to do this, the two values will be different.
+ * This allows the kernel to flag an error when the secondaries have come up.
+ */
+extern u32 __boot_cpu_mode[2];
+
+/* Reports the availability of HYP mode */
+static inline bool is_hyp_mode_available(void)
+{
+        return (__boot_cpu_mode[0] == BOOT_CPU_MODE_EL2 &&
+                __boot_cpu_mode[1] == BOOT_CPU_MODE_EL2);
+}
+
+/* Check if the bootloader has booted CPUs in different modes */
+static inline bool is_hyp_mode_mismatched(void)
+{
+        return __boot_cpu_mode[0] != __boot_cpu_mode[1];
+}
+
+#endif /* __ASSEMBLY__ */
+
+#endif /* ! __ASM__VIRT_H */
diff --git a/arch/arm64/kernel/head.S b/arch/arm64/kernel/head.S
index 90dec55b17a2..bc6d991f8c59 100644
--- a/arch/arm64/kernel/head.S
+++ b/arch/arm64/kernel/head.S
@@ -31,6 +31,7 @@
 #include <asm/pgtable-hwdef.h>
 #include <asm/pgtable.h>
 #include <asm/page.h>
+#include <asm/virt.h>
 
 /*
  * swapper_pg_dir is the virtual address of the initial page table. We place
@@ -115,13 +116,13 @@
 
 ENTRY(stext)
         mov     x21, x0                         // x21=FDT
+        bl      __calc_phys_offset              // x24=PHYS_OFFSET, x28=PHYS_OFFSET-PAGE_OFFSET
         bl      el2_setup                       // Drop to EL1
         mrs     x22, midr_el1                   // x22=cpuid
         mov     x0, x22
         bl      lookup_processor_type
         mov     x23, x0                         // x23=current cpu_table
         cbz     x23, __error_p                  // invalid processor (x23=0)?
-        bl      __calc_phys_offset              // x24=PHYS_OFFSET, x28=PHYS_OFFSET-PAGE_OFFSET
         bl      __vet_fdt
         bl      __create_page_tables            // x25=TTBR0, x26=TTBR1
         /*
@@ -147,11 +148,16 @@ ENTRY(el2_setup)
         mrs     x0, CurrentEL
         cmp     x0, #PSR_MODE_EL2t
         ccmp    x0, #PSR_MODE_EL2h, #0x4, ne
+        ldr     x0, =__boot_cpu_mode            // Compute __boot_cpu_mode
+        add     x0, x0, x28
         b.eq    1f
+        str     wzr, [x0]                       // Remember we don't have EL2...
         ret
 
         /* Hyp configuration. */
-1:      mov     x0, #(1 << 31)                  // 64-bit EL1
+1:      ldr     w1, =BOOT_CPU_MODE_EL2
+        str     w1, [x0, #4]                    // This CPU has EL2
+        mov     x0, #(1 << 31)                  // 64-bit EL1
         msr     hcr_el2, x0
 
         /* Generic timers. */
@@ -187,6 +193,19 @@ ENTRY(el2_setup)
         eret
 ENDPROC(el2_setup)
 
+/*
+ * We need to find out the CPU boot mode long after boot, so we need to
+ * store it in a writable variable.
+ *
+ * This is not in .bss, because we set it sufficiently early that the boot-time
+ * zeroing of .bss would clobber it.
+ */
+        .pushsection    .data
+ENTRY(__boot_cpu_mode)
+        .long   BOOT_CPU_MODE_EL2
+        .long   0
+        .popsection
+
         .align  3
 2:      .quad   .
         .quad   PAGE_OFFSET
@@ -202,6 +221,7 @@ ENDPROC(el2_setup)
          * cores are held until we're ready for them to initialise.
          */
 ENTRY(secondary_holding_pen)
+        bl      __calc_phys_offset              // x24=phys offset
         bl      el2_setup                       // Drop to EL1
         mrs     x0, mpidr_el1
         and     x0, x0, #15                     // CPU number
@@ -227,7 +247,6 @@ ENTRY(secondary_startup)
         mov     x23, x0                         // x23=current cpu_table
         cbz     x23, __error_p                  // invalid processor (x23=0)?
 
-        bl      __calc_phys_offset              // x24=phys offset
         pgtbl   x25, x26, x24                   // x25=TTBR0, x26=TTBR1
         ldr     x12, [x23, #CPU_INFO_SETUP]
         add     x12, x12, x28                   // __virt_to_phys