xref: /openbmc/linux/arch/arm64/kernel/head.S (revision 9b9c2cd4)
1/*
2 * Low-level CPU initialisation
3 * Based on arch/arm/kernel/head.S
4 *
5 * Copyright (C) 1994-2002 Russell King
6 * Copyright (C) 2003-2012 ARM Ltd.
7 * Authors:	Catalin Marinas <catalin.marinas@arm.com>
8 *		Will Deacon <will.deacon@arm.com>
9 *
10 * This program is free software; you can redistribute it and/or modify
11 * it under the terms of the GNU General Public License version 2 as
12 * published by the Free Software Foundation.
13 *
14 * This program is distributed in the hope that it will be useful,
15 * but WITHOUT ANY WARRANTY; without even the implied warranty of
16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
17 * GNU General Public License for more details.
18 *
19 * You should have received a copy of the GNU General Public License
20 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
21 */
22
23#include <linux/linkage.h>
24#include <linux/init.h>
25#include <linux/irqchip/arm-gic-v3.h>
26
27#include <asm/assembler.h>
28#include <asm/ptrace.h>
29#include <asm/asm-offsets.h>
30#include <asm/cache.h>
31#include <asm/cputype.h>
32#include <asm/kernel-pgtable.h>
33#include <asm/memory.h>
34#include <asm/pgtable-hwdef.h>
35#include <asm/pgtable.h>
36#include <asm/page.h>
37#include <asm/sysreg.h>
38#include <asm/thread_info.h>
39#include <asm/virt.h>
40
41#define __PHYS_OFFSET	(KERNEL_START - TEXT_OFFSET)
42
43#if (TEXT_OFFSET & 0xfff) != 0
44#error TEXT_OFFSET must be at least 4KB aligned
45#elif (PAGE_OFFSET & 0x1fffff) != 0
46#error PAGE_OFFSET must be at least 2MB aligned
47#elif TEXT_OFFSET > 0x1fffff
48#error TEXT_OFFSET must be less than 2MB
49#endif
50
51#define KERNEL_START	_text
52#define KERNEL_END	_end
53
54/*
55 * Kernel startup entry point.
56 * ---------------------------
57 *
58 * The requirements are:
59 *   MMU = off, D-cache = off, I-cache = on or off,
60 *   x0 = physical address to the FDT blob.
61 *
62 * This code is mostly position independent so you call this at
63 * __pa(PAGE_OFFSET + TEXT_OFFSET).
64 *
65 * Note that the callee-saved registers are used for storing variables
66 * that are useful before the MMU is enabled. The allocations are described
67 * in the entry routines.
68 */
69	__HEAD
70
71	/*
72	 * DO NOT MODIFY. Image header expected by Linux boot-loaders.
73	 */
74#ifdef CONFIG_EFI
75efi_head:
76	/*
77	 * This add instruction has no meaningful effect except that
78	 * its opcode forms the magic "MZ" signature required by UEFI.
79	 */
80	add	x13, x18, #0x16
81	b	stext
82#else
83	b	stext				// branch to kernel start, magic
84	.long	0				// reserved
85#endif
86	.quad	_kernel_offset_le		// Image load offset from start of RAM, little-endian
87	.quad	_kernel_size_le			// Effective size of kernel image, little-endian
88	.quad	_kernel_flags_le		// Informative flags, little-endian
89	.quad	0				// reserved
90	.quad	0				// reserved
91	.quad	0				// reserved
92	.byte	0x41				// Magic number, "ARM\x64"
93	.byte	0x52
94	.byte	0x4d
95	.byte	0x64
96#ifdef CONFIG_EFI
97	.long	pe_header - efi_head		// Offset to the PE header.
98#else
99	.word	0				// reserved
100#endif
101
102#ifdef CONFIG_EFI
103	.globl	__efistub_stext_offset
104	.set	__efistub_stext_offset, stext - efi_head
105	.align 3
106pe_header:
107	.ascii	"PE"
108	.short 	0
109coff_header:
110	.short	0xaa64				// AArch64
111	.short	2				// nr_sections
112	.long	0 				// TimeDateStamp
113	.long	0				// PointerToSymbolTable
114	.long	1				// NumberOfSymbols
115	.short	section_table - optional_header	// SizeOfOptionalHeader
116	.short	0x206				// Characteristics.
117						// IMAGE_FILE_DEBUG_STRIPPED |
118						// IMAGE_FILE_EXECUTABLE_IMAGE |
119						// IMAGE_FILE_LINE_NUMS_STRIPPED
120optional_header:
121	.short	0x20b				// PE32+ format
122	.byte	0x02				// MajorLinkerVersion
123	.byte	0x14				// MinorLinkerVersion
124	.long	_end - stext			// SizeOfCode
125	.long	0				// SizeOfInitializedData
126	.long	0				// SizeOfUninitializedData
127	.long	__efistub_entry - efi_head	// AddressOfEntryPoint
128	.long	__efistub_stext_offset		// BaseOfCode
129
130extra_header_fields:
131	.quad	0				// ImageBase
132	.long	0x1000				// SectionAlignment
133	.long	PECOFF_FILE_ALIGNMENT		// FileAlignment
134	.short	0				// MajorOperatingSystemVersion
135	.short	0				// MinorOperatingSystemVersion
136	.short	0				// MajorImageVersion
137	.short	0				// MinorImageVersion
138	.short	0				// MajorSubsystemVersion
139	.short	0				// MinorSubsystemVersion
140	.long	0				// Win32VersionValue
141
142	.long	_end - efi_head			// SizeOfImage
143
144	// Everything before the kernel image is considered part of the header
145	.long	__efistub_stext_offset		// SizeOfHeaders
146	.long	0				// CheckSum
147	.short	0xa				// Subsystem (EFI application)
148	.short	0				// DllCharacteristics
149	.quad	0				// SizeOfStackReserve
150	.quad	0				// SizeOfStackCommit
151	.quad	0				// SizeOfHeapReserve
152	.quad	0				// SizeOfHeapCommit
153	.long	0				// LoaderFlags
154	.long	0x6				// NumberOfRvaAndSizes
155
156	.quad	0				// ExportTable
157	.quad	0				// ImportTable
158	.quad	0				// ResourceTable
159	.quad	0				// ExceptionTable
160	.quad	0				// CertificationTable
161	.quad	0				// BaseRelocationTable
162
163	// Section table
164section_table:
165
166	/*
167	 * The EFI application loader requires a relocation section
168	 * because EFI applications must be relocatable.  This is a
169	 * dummy section as far as we are concerned.
170	 */
171	.ascii	".reloc"
172	.byte	0
173	.byte	0			// end of 0 padding of section name
174	.long	0
175	.long	0
176	.long	0			// SizeOfRawData
177	.long	0			// PointerToRawData
178	.long	0			// PointerToRelocations
179	.long	0			// PointerToLineNumbers
180	.short	0			// NumberOfRelocations
181	.short	0			// NumberOfLineNumbers
182	.long	0x42100040		// Characteristics (section flags)
183
184
185	.ascii	".text"
186	.byte	0
187	.byte	0
188	.byte	0        		// end of 0 padding of section name
189	.long	_end - stext		// VirtualSize
190	.long	__efistub_stext_offset	// VirtualAddress
191	.long	_edata - stext		// SizeOfRawData
192	.long	__efistub_stext_offset	// PointerToRawData
193
194	.long	0		// PointerToRelocations (0 for executables)
195	.long	0		// PointerToLineNumbers (0 for executables)
196	.short	0		// NumberOfRelocations  (0 for executables)
197	.short	0		// NumberOfLineNumbers  (0 for executables)
198	.long	0xe0500020	// Characteristics (section flags)
199
200	/*
201	 * EFI will load stext onwards at the 4k section alignment
202	 * described in the PE/COFF header. To ensure that instruction
203	 * sequences using an adrp and a :lo12: immediate will function
204	 * correctly at this alignment, we must ensure that stext is
205	 * placed at a 4k boundary in the Image to begin with.
206	 */
207	.align 12
208#endif
209
210ENTRY(stext)
211	bl	preserve_boot_args
212	bl	el2_setup			// Drop to EL1, w20=cpu_boot_mode
213	adrp	x24, __PHYS_OFFSET
214	bl	set_cpu_boot_mode_flag
215	bl	__create_page_tables		// x25=TTBR0, x26=TTBR1
216	/*
217	 * The following calls CPU setup code, see arch/arm64/mm/proc.S for
218	 * details.
219	 * On return, the CPU will be ready for the MMU to be turned on and
220	 * the TCR will have been set.
221	 */
222	ldr	x27, =__mmap_switched		// address to jump to after
223						// MMU has been enabled
224	adr_l	lr, __enable_mmu		// return (PIC) address
225	b	__cpu_setup			// initialise processor
226ENDPROC(stext)
227
228/*
229 * Preserve the arguments passed by the bootloader in x0 .. x3
230 */
231preserve_boot_args:
232	mov	x21, x0				// x21=FDT
233
234	adr_l	x0, boot_args			// record the contents of
235	stp	x21, x1, [x0]			// x0 .. x3 at kernel entry
236	stp	x2, x3, [x0, #16]
237
238	dmb	sy				// needed before dc ivac with
239						// MMU off
240
241	add	x1, x0, #0x20			// 4 x 8 bytes
242	b	__inval_cache_range		// tail call
243ENDPROC(preserve_boot_args)
244
245/*
246 * Macro to create a table entry to the next page.
247 *
248 *	tbl:	page table address
249 *	virt:	virtual address
250 *	shift:	#imm page table shift
251 *	ptrs:	#imm pointers per table page
252 *
253 * Preserves:	virt
254 * Corrupts:	tmp1, tmp2
255 * Returns:	tbl -> next level table page address
256 */
257	.macro	create_table_entry, tbl, virt, shift, ptrs, tmp1, tmp2
258	lsr	\tmp1, \virt, #\shift
259	and	\tmp1, \tmp1, #\ptrs - 1	// table index
260	add	\tmp2, \tbl, #PAGE_SIZE
261	orr	\tmp2, \tmp2, #PMD_TYPE_TABLE	// address of next table and entry type
262	str	\tmp2, [\tbl, \tmp1, lsl #3]
263	add	\tbl, \tbl, #PAGE_SIZE		// next level table page
264	.endm
265
266/*
267 * Macro to populate the PGD (and possibily PUD) for the corresponding
268 * block entry in the next level (tbl) for the given virtual address.
269 *
270 * Preserves:	tbl, next, virt
271 * Corrupts:	tmp1, tmp2
272 */
273	.macro	create_pgd_entry, tbl, virt, tmp1, tmp2
274	create_table_entry \tbl, \virt, PGDIR_SHIFT, PTRS_PER_PGD, \tmp1, \tmp2
275#if SWAPPER_PGTABLE_LEVELS > 3
276	create_table_entry \tbl, \virt, PUD_SHIFT, PTRS_PER_PUD, \tmp1, \tmp2
277#endif
278#if SWAPPER_PGTABLE_LEVELS > 2
279	create_table_entry \tbl, \virt, SWAPPER_TABLE_SHIFT, PTRS_PER_PTE, \tmp1, \tmp2
280#endif
281	.endm
282
283/*
284 * Macro to populate block entries in the page table for the start..end
285 * virtual range (inclusive).
286 *
287 * Preserves:	tbl, flags
288 * Corrupts:	phys, start, end, pstate
289 */
290	.macro	create_block_map, tbl, flags, phys, start, end
291	lsr	\phys, \phys, #SWAPPER_BLOCK_SHIFT
292	lsr	\start, \start, #SWAPPER_BLOCK_SHIFT
293	and	\start, \start, #PTRS_PER_PTE - 1	// table index
294	orr	\phys, \flags, \phys, lsl #SWAPPER_BLOCK_SHIFT	// table entry
295	lsr	\end, \end, #SWAPPER_BLOCK_SHIFT
296	and	\end, \end, #PTRS_PER_PTE - 1		// table end index
2979999:	str	\phys, [\tbl, \start, lsl #3]		// store the entry
298	add	\start, \start, #1			// next entry
299	add	\phys, \phys, #SWAPPER_BLOCK_SIZE		// next block
300	cmp	\start, \end
301	b.ls	9999b
302	.endm
303
304/*
305 * Setup the initial page tables. We only setup the barest amount which is
306 * required to get the kernel running. The following sections are required:
307 *   - identity mapping to enable the MMU (low address, TTBR0)
308 *   - first few MB of the kernel linear mapping to jump to once the MMU has
309 *     been enabled
310 */
311__create_page_tables:
312	adrp	x25, idmap_pg_dir
313	adrp	x26, swapper_pg_dir
314	mov	x27, lr
315
316	/*
317	 * Invalidate the idmap and swapper page tables to avoid potential
318	 * dirty cache lines being evicted.
319	 */
320	mov	x0, x25
321	add	x1, x26, #SWAPPER_DIR_SIZE
322	bl	__inval_cache_range
323
324	/*
325	 * Clear the idmap and swapper page tables.
326	 */
327	mov	x0, x25
328	add	x6, x26, #SWAPPER_DIR_SIZE
3291:	stp	xzr, xzr, [x0], #16
330	stp	xzr, xzr, [x0], #16
331	stp	xzr, xzr, [x0], #16
332	stp	xzr, xzr, [x0], #16
333	cmp	x0, x6
334	b.lo	1b
335
336	ldr	x7, =SWAPPER_MM_MMUFLAGS
337
338	/*
339	 * Create the identity mapping.
340	 */
341	mov	x0, x25				// idmap_pg_dir
342	adrp	x3, __idmap_text_start		// __pa(__idmap_text_start)
343
344#ifndef CONFIG_ARM64_VA_BITS_48
345#define EXTRA_SHIFT	(PGDIR_SHIFT + PAGE_SHIFT - 3)
346#define EXTRA_PTRS	(1 << (48 - EXTRA_SHIFT))
347
348	/*
349	 * If VA_BITS < 48, it may be too small to allow for an ID mapping to be
350	 * created that covers system RAM if that is located sufficiently high
351	 * in the physical address space. So for the ID map, use an extended
352	 * virtual range in that case, by configuring an additional translation
353	 * level.
354	 * First, we have to verify our assumption that the current value of
355	 * VA_BITS was chosen such that all translation levels are fully
356	 * utilised, and that lowering T0SZ will always result in an additional
357	 * translation level to be configured.
358	 */
359#if VA_BITS != EXTRA_SHIFT
360#error "Mismatch between VA_BITS and page size/number of translation levels"
361#endif
362
363	/*
364	 * Calculate the maximum allowed value for TCR_EL1.T0SZ so that the
365	 * entire ID map region can be mapped. As T0SZ == (64 - #bits used),
366	 * this number conveniently equals the number of leading zeroes in
367	 * the physical address of __idmap_text_end.
368	 */
369	adrp	x5, __idmap_text_end
370	clz	x5, x5
371	cmp	x5, TCR_T0SZ(VA_BITS)	// default T0SZ small enough?
372	b.ge	1f			// .. then skip additional level
373
374	adr_l	x6, idmap_t0sz
375	str	x5, [x6]
376	dmb	sy
377	dc	ivac, x6		// Invalidate potentially stale cache line
378
379	create_table_entry x0, x3, EXTRA_SHIFT, EXTRA_PTRS, x5, x6
3801:
381#endif
382
383	create_pgd_entry x0, x3, x5, x6
384	mov	x5, x3				// __pa(__idmap_text_start)
385	adr_l	x6, __idmap_text_end		// __pa(__idmap_text_end)
386	create_block_map x0, x7, x3, x5, x6
387
388	/*
389	 * Map the kernel image (starting with PHYS_OFFSET).
390	 */
391	mov	x0, x26				// swapper_pg_dir
392	mov	x5, #PAGE_OFFSET
393	create_pgd_entry x0, x5, x3, x6
394	ldr	x6, =KERNEL_END			// __va(KERNEL_END)
395	mov	x3, x24				// phys offset
396	create_block_map x0, x7, x3, x5, x6
397
398	/*
399	 * Since the page tables have been populated with non-cacheable
400	 * accesses (MMU disabled), invalidate the idmap and swapper page
401	 * tables again to remove any speculatively loaded cache lines.
402	 */
403	mov	x0, x25
404	add	x1, x26, #SWAPPER_DIR_SIZE
405	dmb	sy
406	bl	__inval_cache_range
407
408	mov	lr, x27
409	ret
410ENDPROC(__create_page_tables)
411	.ltorg
412
413/*
414 * The following fragment of code is executed with the MMU enabled.
415 */
416	.set	initial_sp, init_thread_union + THREAD_START_SP
417__mmap_switched:
418	adr_l	x6, __bss_start
419	adr_l	x7, __bss_stop
420
4211:	cmp	x6, x7
422	b.hs	2f
423	str	xzr, [x6], #8			// Clear BSS
424	b	1b
4252:
426	adr_l	sp, initial_sp, x4
427	str_l	x21, __fdt_pointer, x5		// Save FDT pointer
428	str_l	x24, memstart_addr, x6		// Save PHYS_OFFSET
429	mov	x29, #0
430#ifdef CONFIG_KASAN
431	bl	kasan_early_init
432#endif
433	b	start_kernel
434ENDPROC(__mmap_switched)
435
436/*
437 * end early head section, begin head code that is also used for
438 * hotplug and needs to have the same protections as the text region
439 */
440	.section ".text","ax"
441/*
442 * If we're fortunate enough to boot at EL2, ensure that the world is
443 * sane before dropping to EL1.
444 *
445 * Returns either BOOT_CPU_MODE_EL1 or BOOT_CPU_MODE_EL2 in x20 if
446 * booted in EL1 or EL2 respectively.
447 */
448ENTRY(el2_setup)
449	mrs	x0, CurrentEL
450	cmp	x0, #CurrentEL_EL2
451	b.ne	1f
452	mrs	x0, sctlr_el2
453CPU_BE(	orr	x0, x0, #(1 << 25)	)	// Set the EE bit for EL2
454CPU_LE(	bic	x0, x0, #(1 << 25)	)	// Clear the EE bit for EL2
455	msr	sctlr_el2, x0
456	b	2f
4571:	mrs	x0, sctlr_el1
458CPU_BE(	orr	x0, x0, #(3 << 24)	)	// Set the EE and E0E bits for EL1
459CPU_LE(	bic	x0, x0, #(3 << 24)	)	// Clear the EE and E0E bits for EL1
460	msr	sctlr_el1, x0
461	mov	w20, #BOOT_CPU_MODE_EL1		// This cpu booted in EL1
462	isb
463	ret
464
465	/* Hyp configuration. */
4662:	mov	x0, #(1 << 31)			// 64-bit EL1
467	msr	hcr_el2, x0
468
469	/* Generic timers. */
470	mrs	x0, cnthctl_el2
471	orr	x0, x0, #3			// Enable EL1 physical timers
472	msr	cnthctl_el2, x0
473	msr	cntvoff_el2, xzr		// Clear virtual offset
474
475#ifdef CONFIG_ARM_GIC_V3
476	/* GICv3 system register access */
477	mrs	x0, id_aa64pfr0_el1
478	ubfx	x0, x0, #24, #4
479	cmp	x0, #1
480	b.ne	3f
481
482	mrs_s	x0, ICC_SRE_EL2
483	orr	x0, x0, #ICC_SRE_EL2_SRE	// Set ICC_SRE_EL2.SRE==1
484	orr	x0, x0, #ICC_SRE_EL2_ENABLE	// Set ICC_SRE_EL2.Enable==1
485	msr_s	ICC_SRE_EL2, x0
486	isb					// Make sure SRE is now set
487	mrs_s	x0, ICC_SRE_EL2			// Read SRE back,
488	tbz	x0, #0, 3f			// and check that it sticks
489	msr_s	ICH_HCR_EL2, xzr		// Reset ICC_HCR_EL2 to defaults
490
4913:
492#endif
493
494	/* Populate ID registers. */
495	mrs	x0, midr_el1
496	mrs	x1, mpidr_el1
497	msr	vpidr_el2, x0
498	msr	vmpidr_el2, x1
499
500	/* sctlr_el1 */
501	mov	x0, #0x0800			// Set/clear RES{1,0} bits
502CPU_BE(	movk	x0, #0x33d0, lsl #16	)	// Set EE and E0E on BE systems
503CPU_LE(	movk	x0, #0x30d0, lsl #16	)	// Clear EE and E0E on LE systems
504	msr	sctlr_el1, x0
505
506	/* Coprocessor traps. */
507	mov	x0, #0x33ff
508	msr	cptr_el2, x0			// Disable copro. traps to EL2
509
510#ifdef CONFIG_COMPAT
511	msr	hstr_el2, xzr			// Disable CP15 traps to EL2
512#endif
513
514	/* EL2 debug */
515	mrs	x0, pmcr_el0			// Disable debug access traps
516	ubfx	x0, x0, #11, #5			// to EL2 and allow access to
517	msr	mdcr_el2, x0			// all PMU counters from EL1
518
519	/* Stage-2 translation */
520	msr	vttbr_el2, xzr
521
522	/* Hypervisor stub */
523	adrp	x0, __hyp_stub_vectors
524	add	x0, x0, #:lo12:__hyp_stub_vectors
525	msr	vbar_el2, x0
526
527	/* spsr */
528	mov	x0, #(PSR_F_BIT | PSR_I_BIT | PSR_A_BIT | PSR_D_BIT |\
529		      PSR_MODE_EL1h)
530	msr	spsr_el2, x0
531	msr	elr_el2, lr
532	mov	w20, #BOOT_CPU_MODE_EL2		// This CPU booted in EL2
533	eret
534ENDPROC(el2_setup)
535
536/*
537 * Sets the __boot_cpu_mode flag depending on the CPU boot mode passed
538 * in x20. See arch/arm64/include/asm/virt.h for more info.
539 */
540ENTRY(set_cpu_boot_mode_flag)
541	adr_l	x1, __boot_cpu_mode
542	cmp	w20, #BOOT_CPU_MODE_EL2
543	b.ne	1f
544	add	x1, x1, #4
5451:	str	w20, [x1]			// This CPU has booted in EL1
546	dmb	sy
547	dc	ivac, x1			// Invalidate potentially stale cache line
548	ret
549ENDPROC(set_cpu_boot_mode_flag)
550
551/*
552 * We need to find out the CPU boot mode long after boot, so we need to
553 * store it in a writable variable.
554 *
555 * This is not in .bss, because we set it sufficiently early that the boot-time
556 * zeroing of .bss would clobber it.
557 */
558	.pushsection	.data..cacheline_aligned
559	.align	L1_CACHE_SHIFT
560ENTRY(__boot_cpu_mode)
561	.long	BOOT_CPU_MODE_EL2
562	.long	BOOT_CPU_MODE_EL1
563	.popsection
564
565	/*
566	 * This provides a "holding pen" for platforms to hold all secondary
567	 * cores are held until we're ready for them to initialise.
568	 */
569ENTRY(secondary_holding_pen)
570	bl	el2_setup			// Drop to EL1, w20=cpu_boot_mode
571	bl	set_cpu_boot_mode_flag
572	mrs	x0, mpidr_el1
573	ldr     x1, =MPIDR_HWID_BITMASK
574	and	x0, x0, x1
575	adr_l	x3, secondary_holding_pen_release
576pen:	ldr	x4, [x3]
577	cmp	x4, x0
578	b.eq	secondary_startup
579	wfe
580	b	pen
581ENDPROC(secondary_holding_pen)
582
583	/*
584	 * Secondary entry point that jumps straight into the kernel. Only to
585	 * be used where CPUs are brought online dynamically by the kernel.
586	 */
587ENTRY(secondary_entry)
588	bl	el2_setup			// Drop to EL1
589	bl	set_cpu_boot_mode_flag
590	b	secondary_startup
591ENDPROC(secondary_entry)
592
593ENTRY(secondary_startup)
594	/*
595	 * Common entry point for secondary CPUs.
596	 */
597	adrp	x25, idmap_pg_dir
598	adrp	x26, swapper_pg_dir
599	bl	__cpu_setup			// initialise processor
600
601	ldr	x21, =secondary_data
602	ldr	x27, =__secondary_switched	// address to jump to after enabling the MMU
603	b	__enable_mmu
604ENDPROC(secondary_startup)
605
606ENTRY(__secondary_switched)
607	ldr	x0, [x21]			// get secondary_data.stack
608	mov	sp, x0
609	mov	x29, #0
610	b	secondary_start_kernel
611ENDPROC(__secondary_switched)
612
613/*
614 * Enable the MMU.
615 *
616 *  x0  = SCTLR_EL1 value for turning on the MMU.
617 *  x27 = *virtual* address to jump to upon completion
618 *
619 * Other registers depend on the function called upon completion.
620 *
621 * Checks if the selected granule size is supported by the CPU.
622 * If it isn't, park the CPU
623 */
624	.section	".idmap.text", "ax"
625__enable_mmu:
626	mrs	x1, ID_AA64MMFR0_EL1
627	ubfx	x2, x1, #ID_AA64MMFR0_TGRAN_SHIFT, 4
628	cmp	x2, #ID_AA64MMFR0_TGRAN_SUPPORTED
629	b.ne	__no_granule_support
630	ldr	x5, =vectors
631	msr	vbar_el1, x5
632	msr	ttbr0_el1, x25			// load TTBR0
633	msr	ttbr1_el1, x26			// load TTBR1
634	isb
635	msr	sctlr_el1, x0
636	isb
637	/*
638	 * Invalidate the local I-cache so that any instructions fetched
639	 * speculatively from the PoC are discarded, since they may have
640	 * been dynamically patched at the PoU.
641	 */
642	ic	iallu
643	dsb	nsh
644	isb
645	br	x27
646ENDPROC(__enable_mmu)
647
648__no_granule_support:
649	wfe
650	b __no_granule_support
651ENDPROC(__no_granule_support)
652