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