Merge tag 'arm64-upstream' of git://git.kernel.org/pub/scm/linux/kernel/git/arm64...

[mirror_ubuntu-artful-kernel.git] / arch / arm64 / kernel / head.S

diff --git a/arch/arm64/kernel/head.S b/arch/arm64/kernel/head.S
index 149fce35fd9a306df5d7ea22f51e3f7fcf8ff800..23cfc08fc8ba88683600d7618acb9bdb400ae2b9 100644 (file)
--- a/arch/arm64/kernel/head.S
+++ b/arch/arm64/kernel/head.S
@@ -31,10 +31,11 @@
 #include <asm/cputype.h>
 #include <asm/kernel-pgtable.h>
 #include <asm/memory.h>
-#include <asm/thread_info.h>
 #include <asm/pgtable-hwdef.h>
 #include <asm/pgtable.h>
 #include <asm/page.h>
+#include <asm/sysreg.h>
+#include <asm/thread_info.h>
 #include <asm/virt.h>
 
 #define __PHYS_OFFSET  (KERNEL_START - TEXT_OFFSET)
@@ -271,7 +272,10 @@ ENDPROC(preserve_boot_args)
  */
        .macro  create_pgd_entry, tbl, virt, tmp1, tmp2
        create_table_entry \tbl, \virt, PGDIR_SHIFT, PTRS_PER_PGD, \tmp1, \tmp2
-#if SWAPPER_PGTABLE_LEVELS == 3
+#if SWAPPER_PGTABLE_LEVELS > 3
+       create_table_entry \tbl, \virt, PUD_SHIFT, PTRS_PER_PUD, \tmp1, \tmp2
+#endif
+#if SWAPPER_PGTABLE_LEVELS > 2
        create_table_entry \tbl, \virt, SWAPPER_TABLE_SHIFT, PTRS_PER_PTE, \tmp1, \tmp2
 #endif
        .endm
@@ -480,6 +484,8 @@ CPU_LE(     bic     x0, x0, #(3 << 24)      )       // Clear the EE and E0E bits for EL1
        orr     x0, x0, #ICC_SRE_EL2_ENABLE     // Set ICC_SRE_EL2.Enable==1
        msr_s   ICC_SRE_EL2, x0
        isb                                     // Make sure SRE is now set
+       mrs_s   x0, ICC_SRE_EL2                 // Read SRE back,
+       tbz     x0, #0, 3f                      // and check that it sticks
        msr_s   ICH_HCR_EL2, xzr                // Reset ICC_HCR_EL2 to defaults
 
 3:
@@ -610,10 +616,17 @@ ENDPROC(__secondary_switched)
  *  x0  = SCTLR_EL1 value for turning on the MMU.
  *  x27 = *virtual* address to jump to upon completion
  *
- * other registers depend on the function called upon completion
+ * Other registers depend on the function called upon completion.
+ *
+ * Checks if the selected granule size is supported by the CPU.
+ * If it isn't, park the CPU
  */
        .section        ".idmap.text", "ax"
 __enable_mmu:
+       mrs     x1, ID_AA64MMFR0_EL1
+       ubfx    x2, x1, #ID_AA64MMFR0_TGRAN_SHIFT, 4
+       cmp     x2, #ID_AA64MMFR0_TGRAN_SUPPORTED
+       b.ne    __no_granule_support
        ldr     x5, =vectors
        msr     vbar_el1, x5
        msr     ttbr0_el1, x25                  // load TTBR0
@@ -631,3 +644,8 @@ __enable_mmu:
        isb
        br      x27
 ENDPROC(__enable_mmu)
+
+__no_granule_support:
+       wfe
+       b __no_granule_support
+ENDPROC(__no_granule_support)