1/*-
2 * Copyright (c) 2012-2014 Andrew Turner
3 * All rights reserved.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions
7 * are met:
8 * 1. Redistributions of source code must retain the above copyright
9 *    notice, this list of conditions and the following disclaimer.
10 * 2. Redistributions in binary form must reproduce the above copyright
11 *    notice, this list of conditions and the following disclaimer in the
12 *    documentation and/or other materials provided with the distribution.
13 *
14 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
15 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
16 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
17 * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
18 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
19 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
20 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
21 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
22 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
23 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
24 * SUCH DAMAGE.
25 */
26
27#include "assym.inc"
28#include "opt_kstack_pages.h"
29#include <sys/syscall.h>
30#include <machine/asm.h>
31#include <machine/armreg.h>
32#include <machine/cpu.h>
33#include <machine/hypervisor.h>
34#include <machine/param.h>
35#include <machine/pte.h>
36#include <machine/vm.h>
37#include <machine/vmparam.h>
38
39#define	VIRT_BITS	48
40
41#if PAGE_SIZE == PAGE_SIZE_16K
42/*
43 * The number of level 3 tables to create. 32 will allow for 1G of address
44 * space, the same as a single level 2 page with 4k pages.
45 */
46#define	L3_PAGE_COUNT	32
47#endif
48
49/*
50 * The size of our bootstrap stack.
51 */
52#define	BOOT_STACK_SIZE	(KSTACK_PAGES * PAGE_SIZE)
53
54	.globl	kernbase
55	.set	kernbase, KERNBASE
56
57/*
58 * We assume:
59 *  MMU      on with an identity map, or off
60 *  D-Cache: off
61 *  I-Cache: on or off
62 *  We are loaded at a 2MiB aligned address
63 */
64
65ENTRY(_start)
66	/* Enter the kernel exception level */
67	bl	enter_kernel_el
68
69	/*
70	 * Disable the MMU. We may have entered the kernel with it on and
71	 * will need to update the tables later. If this has been set up
72	 * with anything other than a VA == PA map then this will fail,
73	 * but in this case the code to find where we are running from
74	 * would have also failed.
75	 */
76	dsb	sy
77	mrs	x2, sctlr_el1
78	bic	x2, x2, SCTLR_M
79	msr	sctlr_el1, x2
80	isb
81
82	/* Set the context id */
83	msr	contextidr_el1, xzr
84
85	/* Get the virt -> phys offset */
86	bl	get_load_phys_addr
87
88	/*
89	 * At this point:
90	 * x28 = Our physical load address
91	 */
92
93	/* Create the page tables */
94	bl	create_pagetables
95
96	/*
97	 * At this point:
98	 * x27 = TTBR0 table
99	 * x26 = Kernel L1 table
100	 * x24 = TTBR1 table
101	 */
102
103	/* Enable the mmu */
104	bl	start_mmu
105
106	/* Load the new ttbr0 pagetable */
107	adrp	x27, pagetable_l0_ttbr0
108	add	x27, x27, :lo12:pagetable_l0_ttbr0
109
110	/* Jump to the virtual address space */
111	ldr	x15, .Lvirtdone
112	br	x15
113
114virtdone:
115	BTI_J
116
117	/* Set up the stack */
118	adrp	x25, initstack_end
119	add	x25, x25, :lo12:initstack_end
120	sub	sp, x25, #PCB_SIZE
121
122	/* Zero the BSS */
123	ldr	x15, .Lbss
124	ldr	x14, .Lend
1251:
126	str	xzr, [x15], #8
127	cmp	x15, x14
128	b.lo	1b
129
130#if defined(PERTHREAD_SSP)
131	/* Set sp_el0 to the boot canary for early per-thread SSP to work */
132	adrp	x15, boot_canary
133	add	x15, x15, :lo12:boot_canary
134	msr	sp_el0, x15
135#endif
136
137	/* Backup the module pointer */
138	mov	x1, x0
139
140	sub	sp, sp, #BOOTPARAMS_SIZE
141	mov	x0, sp
142
143	str	x1,  [x0, #BP_MODULEP]
144	adrp	x25, initstack
145	add	x25, x25, :lo12:initstack
146	str	x25, [x0, #BP_KERN_STACK]
147	str	x27, [x0, #BP_KERN_TTBR0]
148	str	x23, [x0, #BP_BOOT_EL]
149	str	x4,  [x0, #BP_HCR_EL2]
150
151#ifdef KASAN
152	/* Save bootparams */
153	mov	x19, x0
154
155	/* Bootstrap an early shadow map for the boot stack. */
156	ldr	x0, [x0, #BP_KERN_STACK]
157	ldr	x1, =BOOT_STACK_SIZE
158	bl	kasan_init_early
159
160	/* Restore bootparams */
161	mov	x0, x19
162#endif
163
164	/* trace back starts here */
165	mov	fp, #0
166	/* Branch to C code */
167	bl	initarm
168	/* We are done with the boot params */
169	add	sp, sp, #BOOTPARAMS_SIZE
170
171	/*
172	 * Enable pointer authentication in the kernel. We set the keys for
173	 * thread0 in initarm so have to wait until it returns to enable it.
174	 * If we were to enable it in initarm then any authentication when
175	 * returning would fail as it was called with pointer authentication
176	 * disabled.
177	 */
178	bl	ptrauth_start
179
180	bl	mi_startup
181
182	/* We should not get here */
183	brk	0
184
185	.align 3
186.Lvirtdone:
187	.quad	virtdone
188.Lbss:
189	.quad	__bss_start
190.Lend:
191	.quad	__bss_end
192END(_start)
193
194#ifdef SMP
195/*
196 * void
197 * mpentry_psci(unsigned long)
198 *
199 * Called by a core when it is being brought online with psci.
200 * The data in x0 is passed straight to init_secondary.
201 */
202ENTRY(mpentry_psci)
203	mov	x26, xzr
204	b	mpentry_common
205END(mpentry_psci)
206
207/*
208 * void
209 * mpentry_spintable(void)
210 *
211 * Called by a core when it is being brought online with a spin-table.
212 * Reads the new CPU ID and passes this to init_secondary.
213 */
214ENTRY(mpentry_spintable)
215	ldr	x26, =spintable_wait
216	b	mpentry_common
217END(mpentry_spintable)
218
219/* Wait for the current CPU to be released */
220LENTRY(spintable_wait)
221	/* Read the affinity bits from mpidr_el1 */
222	mrs	x1, mpidr_el1
223	ldr	x2, =CPU_AFF_MASK
224	and	x1, x1, x2
225
226	adrp	x2, ap_cpuid
2271:
228	ldr	x0, [x2, :lo12:ap_cpuid]
229	cmp	x0, x1
230	b.ne	1b
231
232	str	xzr, [x2, :lo12:ap_cpuid]
233	dsb	sy
234	sev
235
236	ret
237LEND(mpentry_spintable)
238
239LENTRY(mpentry_common)
240	/* Disable interrupts */
241	msr	daifset, #DAIF_INTR
242
243	/* Enter the kernel exception level */
244	bl	enter_kernel_el
245
246	/* Set the context id */
247	msr	contextidr_el1, xzr
248
249	/* Load the kernel page table */
250	adrp	x24, pagetable_l0_ttbr1
251	add	x24, x24, :lo12:pagetable_l0_ttbr1
252	/* Load the identity page table */
253	adrp	x27, pagetable_l0_ttbr0_bootstrap
254	add	x27, x27, :lo12:pagetable_l0_ttbr0_bootstrap
255
256	/* Enable the mmu */
257	bl	start_mmu
258
259	/* Load the new ttbr0 pagetable */
260	adrp	x27, pagetable_l0_ttbr0
261	add	x27, x27, :lo12:pagetable_l0_ttbr0
262
263	/* Jump to the virtual address space */
264	ldr	x15, =mp_virtdone
265	br	x15
266
267mp_virtdone:
268	BTI_J
269
270	/*
271	 * Allow this CPU to wait until the kernel is ready for it,
272	 * e.g. with spin-table but each CPU uses the same release address
273	 */
274	cbz	x26, 1f
275	blr	x26
2761:
277
278	/* Start using the AP boot stack */
279	adrp	x4, bootstack
280	ldr	x4, [x4, :lo12:bootstack]
281	mov	sp, x4
282
283#if defined(PERTHREAD_SSP)
284	/* Set sp_el0 to the boot canary for early per-thread SSP to work */
285	adrp	x15, boot_canary
286	add	x15, x15, :lo12:boot_canary
287	msr	sp_el0, x15
288#endif
289
290	/* Load the kernel ttbr0 pagetable */
291	msr	ttbr0_el1, x27
292	isb
293
294	/* Invalidate the TLB */
295	tlbi	vmalle1
296	dsb	sy
297	isb
298
299	/*
300	 * Initialize the per-CPU pointer before calling into C code, for the
301	 * benefit of kernel sanitizers.
302	 */
303	adrp	x18, bootpcpu
304	ldr	x18, [x18, :lo12:bootpcpu]
305	msr	tpidr_el1, x18
306
307	b	init_secondary
308LEND(mpentry_common)
309#endif
310
311/*
312 * If we are started in EL2, configure the required hypervisor
313 * registers and drop to EL1.
314 */
315LENTRY(enter_kernel_el)
316	mrs	x23, CurrentEL
317	and	x23, x23, #(CURRENTEL_EL_MASK)
318	cmp	x23, #(CURRENTEL_EL_EL2)
319	b.eq	1f
320	ret
3211:
322	/*
323	 * Disable the MMU. If the HCR_EL2.E2H field is set we will clear it
324	 * which may break address translation.
325	 */
326	dsb	sy
327	mrs	x2, sctlr_el2
328	bic	x2, x2, SCTLR_M
329	msr	sctlr_el2, x2
330	isb
331
332	/* Configure the Hypervisor */
333	ldr	x2, =(HCR_RW | HCR_APK | HCR_API)
334	msr	hcr_el2, x2
335
336	/* Stash value of HCR_EL2 for later */
337	isb
338	mrs	x4, hcr_el2
339
340	/* Load the Virtualization Process ID Register */
341	mrs	x2, midr_el1
342	msr	vpidr_el2, x2
343
344	/* Load the Virtualization Multiprocess ID Register */
345	mrs	x2, mpidr_el1
346	msr	vmpidr_el2, x2
347
348	/* Set the bits that need to be 1 in sctlr_el1 */
349	ldr	x2, .Lsctlr_res1
350	msr	sctlr_el1, x2
351
352	/*
353	 * On some hardware, e.g., Apple M1, we can't clear E2H, so make sure we
354	 * don't trap to EL2 for SIMD register usage to have at least a
355	 * minimally usable system.
356	 */
357	tst	x4, #HCR_E2H
358	mov	x3, #CPTR_RES1	/* HCR_E2H == 0 */
359	mov	x5, #CPTR_FPEN	/* HCR_E2H == 1 */
360	csel	x2, x3, x5, eq
361	msr	cptr_el2, x2
362
363	/* Don't trap to EL2 for CP15 traps */
364	msr	hstr_el2, xzr
365
366	/* Enable access to the physical timers at EL1 */
367	mrs	x2, cnthctl_el2
368	orr	x2, x2, #(CNTHCTL_EL1PCTEN | CNTHCTL_EL1PCEN)
369	msr	cnthctl_el2, x2
370
371	/* Set the counter offset to a known value */
372	msr	cntvoff_el2, xzr
373
374	/* Hypervisor trap functions */
375	adrp	x2, hyp_stub_vectors
376	add	x2, x2, :lo12:hyp_stub_vectors
377	msr	vbar_el2, x2
378
379	/* Zero vttbr_el2 so a hypervisor can tell the host and guest apart */
380	msr	vttbr_el2, xzr
381
382	mov	x2, #(PSR_DAIF | PSR_M_EL1h)
383	msr	spsr_el2, x2
384
385	/* Configure GICv3 CPU interface */
386	mrs	x2, id_aa64pfr0_el1
387	/* Extract GIC bits from the register */
388	ubfx	x2, x2, #ID_AA64PFR0_GIC_SHIFT, #ID_AA64PFR0_GIC_BITS
389	/* GIC[3:0] == 0001 - GIC CPU interface via special regs. supported */
390	cmp	x2, #(ID_AA64PFR0_GIC_CPUIF_EN >> ID_AA64PFR0_GIC_SHIFT)
391	b.ne	2f
392
393	mrs	x2, icc_sre_el2
394	orr	x2, x2, #ICC_SRE_EL2_EN	/* Enable access from insecure EL1 */
395	orr	x2, x2, #ICC_SRE_EL2_SRE	/* Enable system registers */
396	msr	icc_sre_el2, x2
3972:
398
399	/* Set the address to return to our return address */
400	msr	elr_el2, x30
401	isb
402
403	eret
404
405	.align 3
406.Lsctlr_res1:
407	.quad SCTLR_RES1
408LEND(enter_kernel_el)
409
410/*
411 * Get the physical address the kernel was loaded at.
412 */
413LENTRY(get_load_phys_addr)
414	/* Load the offset of get_load_phys_addr from KERNBASE */
415	ldr	x28, =(get_load_phys_addr - KERNBASE)
416	/* Load the physical address of get_load_phys_addr */
417	adr	x29, get_load_phys_addr
418	/* Find the physical address of KERNBASE, i.e. our load address */
419	sub	x28, x29, x28
420	ret
421LEND(get_load_phys_addr)
422
423/*
424 * This builds the page tables containing the identity map, and the kernel
425 * virtual map.
426 *
427 * It relys on:
428 *  We were loaded to an address that is on a 2MiB boundary
429 *  All the memory must not cross a 1GiB boundaty
430 *  x28 contains the physical address we were loaded from
431 *
432 *  There are 7 or 8 pages before that address for the page tables
433 *   The pages used are:
434 *    - The Kernel L3 tables (only for 16k kernel)
435 *    - The Kernel L2 table
436 *    - The Kernel L1 table
437 *    - The Kernel L0 table             (TTBR1)
438 *    - The identity (PA = VA) L2 table
439 *    - The identity (PA = VA) L1 table
440 *    - The identity (PA = VA) L0 table (Early TTBR0)
441 *    - The Kernel empty L0 table       (Late TTBR0)
442 */
443LENTRY(create_pagetables)
444	/* Save the Link register */
445	mov	x5, x30
446
447	/* Clean the page table */
448	adrp	x6, pagetable
449	add	x6, x6, :lo12:pagetable
450	mov	x26, x6
451	adrp	x27, pagetable_end
452	add	x27, x27, :lo12:pagetable_end
4531:
454	stp	xzr, xzr, [x6], #16
455	stp	xzr, xzr, [x6], #16
456	stp	xzr, xzr, [x6], #16
457	stp	xzr, xzr, [x6], #16
458	cmp	x6, x27
459	b.lo	1b
460
461	/*
462	 * Build the TTBR1 maps.
463	 */
464
465	/* Find the size of the kernel */
466	mov	x6, #(KERNBASE)
467
468#if defined(LINUX_BOOT_ABI)
469	/* X19 is used as 'map FDT data' flag */
470	mov	x19, xzr
471
472	/* No modules or FDT pointer ? */
473	cbz	x0, booti_no_fdt
474
475	/*
476	 * Test if x0 points to modules descriptor(virtual address) or
477	 * to FDT (physical address)
478	 */
479	cmp	x0, x6		/* x6 is #(KERNBASE) */
480	b.lo	booti_fdt
481#endif
482
483	/* Booted with modules pointer */
484	/* Find modulep - begin */
485	sub	x8, x0, x6
486	/*
487	 * Add space for the module data. When PAGE_SIZE is 4k this will
488	 * add at least 2 level 2 blocks (2 * 2MiB). When PAGE_SIZE is
489	 * larger it will be at least as large as we use smaller level 3
490	 * pages.
491	 */
492	ldr	x7, =((6 * 1024 * 1024) - 1)
493	add	x8, x8, x7
494	b	common
495
496#if defined(LINUX_BOOT_ABI)
497booti_fdt:
498	/* Booted by U-Boot booti with FDT data */
499	/* Set 'map FDT data' flag */
500	mov	x19, #1
501
502booti_no_fdt:
503	/* Booted by U-Boot booti without FTD data */
504	/* Find the end - begin */
505	ldr     x7, .Lend
506	sub     x8, x7, x6
507
508	/*
509	 * Add one 2MiB page for copy of FDT data (maximum FDT size),
510	 * one for metadata and round up
511	 */
512	ldr	x7, =(3 * L2_SIZE - 1)
513	add	x8, x8, x7
514#endif
515
516common:
517#if PAGE_SIZE != PAGE_SIZE_4K
518	/*
519	 * Create L3 and L3C pages. The kernel will be loaded at a 2M aligned
520	 * address, enabling the creation of L3C pages. However, when the page
521	 * size is larger than 4k, L2 blocks are too large to map the kernel
522	 * with 2M alignment.
523	 */
524#define	PTE_SHIFT	L3_SHIFT
525#define	BUILD_PTE_FUNC	build_l3_page_pagetable
526#else
527#define	PTE_SHIFT	L2_SHIFT
528#define	BUILD_PTE_FUNC	build_l2_block_pagetable
529#endif
530
531	/* Get the number of blocks/pages to allocate, rounded down */
532	lsr	x10, x8, #(PTE_SHIFT)
533
534	/* Create the kernel space PTE table */
535	mov	x6, x26
536	mov	x7, #(ATTR_S1_IDX(VM_MEMATTR_WRITE_BACK))
537	mov	x8, #(KERNBASE)
538	mov	x9, x28
539	bl	BUILD_PTE_FUNC
540
541#undef PTE_SHIFT
542#undef BUILD_PTE_FUNC
543
544#if PAGE_SIZE != PAGE_SIZE_4K
545	/* Move to the l2 table */
546	ldr	x9, =(PAGE_SIZE * L3_PAGE_COUNT)
547	add	x26, x26, x9
548
549	/* Link the l2 -> l3 table */
550	mov	x9, x6
551	mov	x6, x26
552	bl	link_l2_pagetable
553#endif
554
555	/* Move to the l1 table */
556	add	x26, x26, #PAGE_SIZE
557
558	/* Link the l1 -> l2 table */
559	mov	x9, x6
560	mov	x6, x26
561	bl	link_l1_pagetable
562
563	/* Move to the l0 table */
564	add	x24, x26, #PAGE_SIZE
565
566	/* Link the l0 -> l1 table */
567	mov	x9, x6
568	mov	x6, x24
569	mov	x10, #1
570	bl	link_l0_pagetable
571
572	/*
573	 * Build the TTBR0 maps.  As TTBR0 maps, they must specify ATTR_S1_nG.
574	 * They are only needed early on, so the VA = PA map is uncached.
575	 */
576	add	x27, x24, #PAGE_SIZE
577
578	mov	x6, x27		/* The initial page table */
579
580	/* Create the VA = PA map */
581	mov	x7, #(ATTR_S1_nG | ATTR_S1_IDX(VM_MEMATTR_WRITE_BACK))
582	adrp	x16, _start
583	and	x16, x16, #(~L2_OFFSET)
584	mov	x9, x16		/* PA start */
585	mov	x8, x16		/* VA start (== PA start) */
586	mov	x10, #1
587	bl	build_l2_block_pagetable
588
589#if defined(SOCDEV_PA)
590	/* Create a table for the UART */
591	mov	x7, #(ATTR_S1_nG | ATTR_S1_IDX(VM_MEMATTR_DEVICE))
592	ldr	x9, =(L2_SIZE)
593	add	x16, x16, x9	/* VA start */
594	mov	x8, x16
595
596	/* Store the socdev virtual address */
597	add	x17, x8, #(SOCDEV_PA & L2_OFFSET)
598	adrp	x9, socdev_va
599	str	x17, [x9, :lo12:socdev_va]
600
601	mov	x9, #(SOCDEV_PA & ~L2_OFFSET)	/* PA start */
602	mov	x10, #1
603	bl	build_l2_block_pagetable
604#endif
605
606#if defined(LINUX_BOOT_ABI)
607	/* Map FDT data ? */
608	cbz	x19, 1f
609
610	/* Create the mapping for FDT data (2 MiB max) */
611	mov	x7, #(ATTR_S1_nG | ATTR_S1_IDX(VM_MEMATTR_WRITE_BACK))
612	ldr	x9, =(L2_SIZE)
613	add	x16, x16, x9	/* VA start */
614	mov	x8, x16
615	mov	x9, x0			/* PA start */
616	/* Update the module pointer to point at the allocated memory */
617	and	x0, x0, #(L2_OFFSET)	/* Keep the lower bits */
618	add	x0, x0, x8		/* Add the aligned virtual address */
619
620	mov	x10, #1
621	bl	build_l2_block_pagetable
622
6231:
624#endif
625
626	/* Move to the l1 table */
627	add	x27, x27, #PAGE_SIZE
628
629	/* Link the l1 -> l2 table */
630	mov	x9, x6
631	mov	x6, x27
632	bl	link_l1_pagetable
633
634	/* Move to the l0 table */
635	add	x27, x27, #PAGE_SIZE
636
637	/* Link the l0 -> l1 table */
638	mov	x9, x6
639	mov	x6, x27
640	mov	x10, #1
641	bl	link_l0_pagetable
642
643	/* Restore the Link register */
644	mov	x30, x5
645	ret
646LEND(create_pagetables)
647
648/*
649 * Builds an L0 -> L1 table descriptor
650 *
651 *  x6  = L0 table
652 *  x8  = Virtual Address
653 *  x9  = L1 PA (trashed)
654 *  x10 = Entry count (trashed)
655 *  x11, x12 and x13 are trashed
656 */
657LENTRY(link_l0_pagetable)
658	/*
659	 * Link an L0 -> L1 table entry.
660	 */
661	/* Find the table index */
662	lsr	x11, x8, #L0_SHIFT
663	and	x11, x11, #L0_ADDR_MASK
664
665	/* Build the L0 block entry */
666	mov	x12, #L0_TABLE
667	orr	x12, x12, #(TATTR_UXN_TABLE | TATTR_AP_TABLE_NO_EL0)
668
669	/* Only use the output address bits */
670	lsr	x9, x9, #PAGE_SHIFT
6711:	orr	x13, x12, x9, lsl #PAGE_SHIFT
672
673	/* Store the entry */
674	str	x13, [x6, x11, lsl #3]
675
676	sub	x10, x10, #1
677	add	x11, x11, #1
678	add	x9, x9, #1
679	cbnz	x10, 1b
680
681	ret
682LEND(link_l0_pagetable)
683
684/*
685 * Builds an L1 -> L2 table descriptor
686 *
687 *  x6  = L1 table
688 *  x8  = Virtual Address
689 *  x9  = L2 PA (trashed)
690 *  x11, x12 and x13 are trashed
691 */
692LENTRY(link_l1_pagetable)
693	/*
694	 * Link an L1 -> L2 table entry.
695	 */
696	/* Find the table index */
697	lsr	x11, x8, #L1_SHIFT
698	and	x11, x11, #Ln_ADDR_MASK
699
700	/* Build the L1 block entry */
701	mov	x12, #L1_TABLE
702
703	/* Only use the output address bits */
704	lsr	x9, x9, #PAGE_SHIFT
705	orr	x13, x12, x9, lsl #PAGE_SHIFT
706
707	/* Store the entry */
708	str	x13, [x6, x11, lsl #3]
709
710	ret
711LEND(link_l1_pagetable)
712
713/*
714 * Builds count 2 MiB page table entry
715 *  x6  = L2 table
716 *  x7  = Block attributes
717 *  x8  = VA start
718 *  x9  = PA start (trashed)
719 *  x10 = Entry count (trashed)
720 *  x11, x12 and x13 are trashed
721 */
722LENTRY(build_l2_block_pagetable)
723	/*
724	 * Build the L2 table entry.
725	 */
726	/* Find the table index */
727	lsr	x11, x8, #L2_SHIFT
728	and	x11, x11, #Ln_ADDR_MASK
729
730	/* Build the L2 block entry */
731	orr	x12, x7, #L2_BLOCK
732	orr	x12, x12, #(ATTR_DEFAULT)
733	orr	x12, x12, #(ATTR_S1_UXN)
734#ifdef __ARM_FEATURE_BTI_DEFAULT
735	orr	x12, x12, #(ATTR_S1_GP)
736#endif
737
738	/* Only use the output address bits */
739	lsr	x9, x9, #L2_SHIFT
740
741	/* Set the physical address for this virtual address */
7421:	orr	x13, x12, x9, lsl #L2_SHIFT
743
744	/* Store the entry */
745	str	x13, [x6, x11, lsl #3]
746
747	sub	x10, x10, #1
748	add	x11, x11, #1
749	add	x9, x9, #1
750	cbnz	x10, 1b
751
752	ret
753LEND(build_l2_block_pagetable)
754
755#if PAGE_SIZE != PAGE_SIZE_4K
756/*
757 * Builds an L2 -> L3 table descriptor
758 *
759 *  x6  = L2 table
760 *  x8  = Virtual Address
761 *  x9  = L3 PA (trashed)
762 *  x11, x12 and x13 are trashed
763 */
764LENTRY(link_l2_pagetable)
765	/*
766	 * Link an L2 -> L3 table entry.
767	 */
768	/* Find the table index */
769	lsr	x11, x8, #L2_SHIFT
770	and	x11, x11, #Ln_ADDR_MASK
771
772	/* Build the L1 block entry */
773	mov	x12, #L2_TABLE
774
775	/* Only use the output address bits */
776	lsr	x9, x9, #PAGE_SHIFT
777	orr	x13, x12, x9, lsl #PAGE_SHIFT
778
779	/* Store the entry */
780	str	x13, [x6, x11, lsl #3]
781
782	ret
783LEND(link_l2_pagetable)
784
785/*
786 * Builds count level 3 page table entries. Uses ATTR_CONTIGUOUS to create
787 * large page (L3C) mappings when the current VA and remaining count allow
788 * it.
789 *  x6  = L3 table
790 *  x7  = Block attributes
791 *  x8  = VA start
792 *  x9  = PA start (trashed)
793 *  x10 = Entry count (trashed)
794 *  x11, x12 and x13 are trashed
795 *
796 * VA start (x8) modulo L3C_SIZE must equal PA start (x9) modulo L3C_SIZE.
797 */
798LENTRY(build_l3_page_pagetable)
799	/*
800	 * Build the L3 table entry.
801	 */
802	/* Find the table index */
803	lsr	x11, x8, #L3_SHIFT
804	and	x11, x11, #Ln_ADDR_MASK
805
806	/* Build the L3 page entry */
807	orr	x12, x7, #L3_PAGE
808	orr	x12, x12, #(ATTR_DEFAULT)
809	orr	x12, x12, #(ATTR_S1_UXN)
810#ifdef __ARM_FEATURE_BTI_DEFAULT
811	orr	x12, x12, #(ATTR_S1_GP)
812#endif
813
814	/* Only use the output address bits */
815	lsr	x9, x9, #L3_SHIFT
816
817	/* Check if an ATTR_CONTIGUOUS mapping is possible */
8181:	tst	x11, #(L3C_ENTRIES - 1)
819	b.ne	2f
820	cmp	x10, #L3C_ENTRIES
821	b.lo	3f
822	orr	x12, x12, #(ATTR_CONTIGUOUS)
823	b	2f
8243:	and	x12, x12, #(~ATTR_CONTIGUOUS)
825
826	/* Set the physical address for this virtual address */
8272:	orr	x13, x12, x9, lsl #L3_SHIFT
828
829	/* Store the entry */
830	str	x13, [x6, x11, lsl #3]
831
832	sub	x10, x10, #1
833	add	x11, x11, #1
834	add	x9, x9, #1
835	cbnz	x10, 1b
836
837	ret
838LEND(build_l3_page_pagetable)
839#endif
840
841LENTRY(start_mmu)
842	dsb	sy
843
844	/* Load the exception vectors */
845	ldr	x2, =exception_vectors
846	msr	vbar_el1, x2
847
848	/* Load ttbr0 and ttbr1 */
849	msr	ttbr0_el1, x27
850	msr	ttbr1_el1, x24
851	isb
852
853	/* Clear the Monitor Debug System control register */
854	msr	mdscr_el1, xzr
855
856	/* Invalidate the TLB */
857	tlbi	vmalle1is
858	dsb	ish
859	isb
860
861	ldr	x2, mair
862	msr	mair_el1, x2
863
864	/*
865	 * Setup TCR according to the PARange and ASIDBits fields
866	 * from ID_AA64MMFR0_EL1 and the HAFDBS field from the
867	 * ID_AA64MMFR1_EL1.  More precisely, set TCR_EL1.AS
868	 * to 1 only if the ASIDBits field equals 0b0010.
869	 */
870	ldr	x2, tcr
871	mrs	x3, id_aa64mmfr0_el1
872
873	/* Copy the bottom 3 bits from id_aa64mmfr0_el1 into TCR.IPS */
874	bfi	x2, x3, #(TCR_IPS_SHIFT), #(TCR_IPS_WIDTH)
875	and	x3, x3, #(ID_AA64MMFR0_ASIDBits_MASK)
876
877	/* Check if the HW supports 16 bit ASIDS */
878	cmp	x3, #(ID_AA64MMFR0_ASIDBits_16)
879	/* If so x3 == 1, else x3 == 0 */
880	cset	x3, eq
881	/* Set TCR.AS with x3 */
882	bfi	x2, x3, #(TCR_ASID_SHIFT), #(TCR_ASID_WIDTH)
883
884	/*
885	 * Check if the HW supports access flag and dirty state updates,
886	 * and set TCR_EL1.HA and TCR_EL1.HD accordingly.
887	 */
888	mrs	x3, id_aa64mmfr1_el1
889	and	x3, x3, #(ID_AA64MMFR1_HAFDBS_MASK)
890	cmp	x3, #1
891	b.ne	1f
892	orr 	x2, x2, #(TCR_HA)
893	b	2f
8941:
895	cmp	x3, #2
896	b.ne	2f
897	orr 	x2, x2, #(TCR_HA | TCR_HD)
8982:
899	msr	tcr_el1, x2
900
901	/*
902	 * Setup SCTLR.
903	 */
904	ldr	x2, sctlr_set
905	ldr	x3, sctlr_clear
906	mrs	x1, sctlr_el1
907	bic	x1, x1, x3	/* Clear the required bits */
908	orr	x1, x1, x2	/* Set the required bits */
909	msr	sctlr_el1, x1
910	isb
911
912	ret
913
914	.align 3
915mair:
916	.quad	MAIR_ATTR(MAIR_DEVICE_nGnRnE, VM_MEMATTR_DEVICE_nGnRnE) | \
917		MAIR_ATTR(MAIR_NORMAL_NC, VM_MEMATTR_UNCACHEABLE)   |	\
918		MAIR_ATTR(MAIR_NORMAL_WB, VM_MEMATTR_WRITE_BACK)    |	\
919		MAIR_ATTR(MAIR_NORMAL_WT, VM_MEMATTR_WRITE_THROUGH) |	\
920		MAIR_ATTR(MAIR_DEVICE_nGnRE, VM_MEMATTR_DEVICE_nGnRE)
921tcr:
922#if PAGE_SIZE == PAGE_SIZE_4K
923#define	TCR_TG	(TCR_TG1_4K | TCR_TG0_4K)
924#elif PAGE_SIZE == PAGE_SIZE_16K
925#define	TCR_TG	(TCR_TG1_16K | TCR_TG0_16K)
926#else
927#error Unsupported page size
928#endif
929
930	.quad (TCR_TxSZ(64 - VIRT_BITS) | TCR_TG | \
931	    TCR_CACHE_ATTRS | TCR_SMP_ATTRS)
932sctlr_set:
933	/* Bits to set */
934	.quad (SCTLR_LSMAOE | SCTLR_nTLSMD | SCTLR_UCI | SCTLR_SPAN | \
935	    SCTLR_nTWE | SCTLR_nTWI | SCTLR_UCT | SCTLR_DZE | \
936	    SCTLR_I | SCTLR_SED | SCTLR_SA0 | SCTLR_SA | SCTLR_C | \
937	    SCTLR_M | SCTLR_CP15BEN | SCTLR_BT1 | SCTLR_BT0)
938sctlr_clear:
939	/* Bits to clear */
940	.quad (SCTLR_EE | SCTLR_E0E | SCTLR_IESB | SCTLR_WXN | SCTLR_UMA | \
941	    SCTLR_ITD | SCTLR_A)
942LEND(start_mmu)
943
944ENTRY(abort)
945	b abort
946END(abort)
947
948.bss
949	.align	PAGE_SHIFT
950initstack:
951	.space	BOOT_STACK_SIZE
952initstack_end:
953
954	.section .init_pagetable, "aw", %nobits
955	.align PAGE_SHIFT
956	/*
957	 * 6 initial tables (in the following order):
958	 *           L2 for kernel (High addresses)
959	 *           L1 for kernel
960	 *           L0 for kernel
961	 *           L1 bootstrap for user   (Low addresses)
962	 *           L0 bootstrap for user
963	 *           L0 for user
964	 */
965	.globl pagetable_l0_ttbr1
966pagetable:
967#if PAGE_SIZE != PAGE_SIZE_4K
968	.space	(PAGE_SIZE * L3_PAGE_COUNT)
969pagetable_l2_ttbr1:
970#endif
971	.space	PAGE_SIZE
972pagetable_l1_ttbr1:
973	.space	PAGE_SIZE
974pagetable_l0_ttbr1:
975	.space	PAGE_SIZE
976pagetable_l2_ttbr0_bootstrap:
977	.space	PAGE_SIZE
978pagetable_l1_ttbr0_bootstrap:
979	.space	PAGE_SIZE
980pagetable_l0_ttbr0_bootstrap:
981	.space	PAGE_SIZE
982pagetable_l0_ttbr0:
983	.space	PAGE_SIZE
984pagetable_end:
985
986el2_pagetable:
987	.space	PAGE_SIZE
988
989	.section .rodata, "a", %progbits
990	.globl	aarch32_sigcode
991	.align 2
992aarch32_sigcode:
993	.word 0xe1a0000d	// mov r0, sp
994	.word 0xe2800040	// add r0, r0, #SIGF_UC
995	.word 0xe59f700c	// ldr r7, [pc, #12]
996	.word 0xef000000	// swi #0
997	.word 0xe59f7008	// ldr r7, [pc, #8]
998	.word 0xef000000	// swi #0
999	.word 0xeafffffa	// b . - 16
1000	.word SYS_sigreturn
1001	.word SYS_exit
1002	.align	3
1003	.size aarch32_sigcode, . - aarch32_sigcode
1004aarch32_esigcode:
1005	.data
1006	.global sz_aarch32_sigcode
1007sz_aarch32_sigcode:
1008	.quad aarch32_esigcode - aarch32_sigcode
1009