xref: /freebsd/sys/arm64/arm64/locore.S (revision d65cd7a57bf0600b722afc770838a5d0c1c3a8e1)
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 * $FreeBSD$
27 */
28
29#include "assym.inc"
30#include "opt_kstack_pages.h"
31#include <sys/syscall.h>
32#include <machine/asm.h>
33#include <machine/armreg.h>
34#include <machine/hypervisor.h>
35#include <machine/param.h>
36#include <machine/pte.h>
37#include <machine/vm.h>
38#include <machine/vmparam.h>
39
40#define	VIRT_BITS	48
41#define	DMAP_TABLES	((DMAP_MAX_ADDRESS - DMAP_MIN_ADDRESS) >> L0_SHIFT)
42
43	.globl	kernbase
44	.set	kernbase, KERNBASE
45
46
47/* U-Boot booti related constants. */
48#if defined(LINUX_BOOT_ABI)
49#define FDT_MAGIC		0xEDFE0DD0	/* FDT blob Magic */
50
51#ifndef UBOOT_IMAGE_OFFSET
52#define	UBOOT_IMAGE_OFFSET	0		/* Image offset from start of */
53#endif						/*  2 MiB page */
54
55#ifndef UBOOT_IMAGE_SIZE			/* Total size of image */
56#define	UBOOT_IMAGE_SIZE	 _end - _start
57#endif
58
59#ifndef UBOOT_IMAGE_FLAGS
60#define	UBOOT_IMAGE_FLAGS	0		/* LE kernel, unspecified */
61#endif						/*  page size */
62#endif /* defined(LINUX_BOOT_ABI) */
63
64/*
65 * We assume:
66 *  MMU      on with an identity map, or off
67 *  D-Cache: off
68 *  I-Cache: on or off
69 *  We are loaded at a 2MiB aligned address
70 */
71
72	.text
73	.globl _start
74_start:
75#if defined(LINUX_BOOT_ABI)
76	/* U-boot image header */
77	b	1f			/* code 0 */
78	.long	0			/* code 1 */
79	.quad	UBOOT_IMAGE_OFFSET	/* Image offset in 2 MiB page, LE */
80	.quad	UBOOT_IMAGE_SIZE	/* Image size, LE */
81	.quad	UBOOT_IMAGE_FLAGS	/* Flags for kernel. LE */
82	.quad 	0			/* Reserved */
83	.quad 	0			/* Reserved */
84	.quad 	0			/* Reserved */
85	.long 	0x644d5241		/* Magic  "ARM\x64", LE */
86	.long 	0			/* Reserved for PE COFF offset*/
871:
88#endif /* defined(LINUX_BOOT_ABI) */
89
90	/* Drop to EL1 */
91	bl	drop_to_el1
92
93	/*
94	 * Disable the MMU. We may have entered the kernel with it on and
95	 * will need to update the tables later. If this has been set up
96	 * with anything other than a VA == PA map then this will fail,
97	 * but in this case the code to find where we are running from
98	 * would have also failed.
99	 */
100	dsb	sy
101	mrs	x2, sctlr_el1
102	bic	x2, x2, SCTLR_M
103	msr	sctlr_el1, x2
104	isb
105
106	/* Set the context id */
107	msr	contextidr_el1, xzr
108
109	/* Get the virt -> phys offset */
110	bl	get_virt_delta
111
112	/*
113	 * At this point:
114	 * x29 = PA - VA
115	 * x28 = Our physical load address
116	 */
117
118	/* Create the page tables */
119	bl	create_pagetables
120
121	/*
122	 * At this point:
123	 * x27 = TTBR0 table
124	 * x26 = Kernel L1 table
125	 * x24 = TTBR1 table
126	 */
127
128	/* Enable the mmu */
129	bl	start_mmu
130
131	/* Jump to the virtual address space */
132	ldr	x15, .Lvirtdone
133	br	x15
134
135virtdone:
136	/* Set up the stack */
137	adr	x25, initstack_end
138	mov	sp, x25
139	sub	sp, sp, #PCB_SIZE
140
141	/* Zero the BSS */
142	ldr	x15, .Lbss
143	ldr	x14, .Lend
1441:
145	str	xzr, [x15], #8
146	cmp	x15, x14
147	b.lo	1b
148
149	/* Backup the module pointer */
150	mov	x1, x0
151
152	/* Make the page table base a virtual address */
153	sub	x26, x26, x29
154	sub	x24, x24, x29
155
156	sub	sp, sp, #BOOTPARAMS_SIZE
157	mov	x0, sp
158
159	/* Degate the delda so it is VA -> PA */
160	neg	x29, x29
161
162	str	x1,  [x0, #BP_MODULEP]
163	str	x26, [x0, #BP_KERN_L1PT]
164	str	x29, [x0, #BP_KERN_DELTA]
165	adr	x25, initstack
166	str	x25, [x0, #BP_KERN_STACK]
167	str	x24, [x0, #BP_KERN_L0PT]
168	str	x23, [x0, #BP_BOOT_EL]
169
170	/* trace back starts here */
171	mov	fp, #0
172	/* Branch to C code */
173	bl	initarm
174	bl	mi_startup
175
176	/* We should not get here */
177	brk	0
178
179	.align 3
180.Lvirtdone:
181	.quad	virtdone
182.Lbss:
183	.quad	__bss_start
184.Lend:
185	.quad	_end
186
187#ifdef SMP
188/*
189 * mpentry(unsigned long)
190 *
191 * Called by a core when it is being brought online.
192 * The data in x0 is passed straight to init_secondary.
193 */
194ENTRY(mpentry)
195	/* Disable interrupts */
196	msr	daifset, #2
197
198	/* Drop to EL1 */
199	bl	drop_to_el1
200
201	/* Set the context id */
202	msr	contextidr_el1, xzr
203
204	/* Load the kernel page table */
205	adr	x24, pagetable_l0_ttbr1
206	/* Load the identity page table */
207	adr	x27, pagetable_l0_ttbr0
208
209	/* Enable the mmu */
210	bl	start_mmu
211
212	/* Jump to the virtual address space */
213	ldr	x15, =mp_virtdone
214	br	x15
215
216mp_virtdone:
217	/* Start using the AP boot stack */
218	ldr	x4, =bootstack
219	ldr	x4, [x4]
220	mov	sp, x4
221	b	init_secondary
222END(mpentry)
223#endif
224
225/*
226 * If we are started in EL2, configure the required hypervisor
227 * registers and drop to EL1.
228 */
229drop_to_el1:
230	mrs	x23, CurrentEL
231	lsr	x23, x23, #2
232	cmp	x23, #0x2
233	b.eq	1f
234	ret
2351:
236	/* Configure the Hypervisor */
237	mov	x2, #(HCR_RW)
238	msr	hcr_el2, x2
239
240	/* Load the Virtualization Process ID Register */
241	mrs	x2, midr_el1
242	msr	vpidr_el2, x2
243
244	/* Load the Virtualization Multiprocess ID Register */
245	mrs	x2, mpidr_el1
246	msr	vmpidr_el2, x2
247
248	/* Set the bits that need to be 1 in sctlr_el1 */
249	ldr	x2, .Lsctlr_res1
250	msr	sctlr_el1, x2
251
252	/* Don't trap to EL2 for exceptions */
253	mov	x2, #CPTR_RES1
254	msr	cptr_el2, x2
255
256	/* Don't trap to EL2 for CP15 traps */
257	msr	hstr_el2, xzr
258
259	/* Enable access to the physical timers at EL1 */
260	mrs	x2, cnthctl_el2
261	orr	x2, x2, #(CNTHCTL_EL1PCTEN | CNTHCTL_EL1PCEN)
262	msr	cnthctl_el2, x2
263
264	/* Set the counter offset to a known value */
265	msr	cntvoff_el2, xzr
266
267	/* Hypervisor trap functions */
268	adr	x2, hyp_vectors
269	msr	vbar_el2, x2
270
271	mov	x2, #(PSR_F | PSR_I | PSR_A | PSR_D | PSR_M_EL1h)
272	msr	spsr_el2, x2
273
274	/* Configure GICv3 CPU interface */
275	mrs	x2, id_aa64pfr0_el1
276	/* Extract GIC bits from the register */
277	ubfx	x2, x2, #ID_AA64PFR0_GIC_SHIFT, #ID_AA64PFR0_GIC_BITS
278	/* GIC[3:0] == 0001 - GIC CPU interface via special regs. supported */
279	cmp	x2, #(ID_AA64PFR0_GIC_CPUIF_EN >> ID_AA64PFR0_GIC_SHIFT)
280	b.ne	2f
281
282	mrs	x2, icc_sre_el2
283	orr	x2, x2, #ICC_SRE_EL2_EN	/* Enable access from insecure EL1 */
284	orr	x2, x2, #ICC_SRE_EL2_SRE	/* Enable system registers */
285	msr	icc_sre_el2, x2
2862:
287
288	/* Set the address to return to our return address */
289	msr	elr_el2, x30
290	isb
291
292	eret
293
294	.align 3
295.Lsctlr_res1:
296	.quad SCTLR_RES1
297
298#define	VECT_EMPTY	\
299	.align 7;	\
300	1:	b	1b
301
302	.align 11
303hyp_vectors:
304	VECT_EMPTY	/* Synchronous EL2t */
305	VECT_EMPTY	/* IRQ EL2t */
306	VECT_EMPTY	/* FIQ EL2t */
307	VECT_EMPTY	/* Error EL2t */
308
309	VECT_EMPTY	/* Synchronous EL2h */
310	VECT_EMPTY	/* IRQ EL2h */
311	VECT_EMPTY	/* FIQ EL2h */
312	VECT_EMPTY	/* Error EL2h */
313
314	VECT_EMPTY	/* Synchronous 64-bit EL1 */
315	VECT_EMPTY	/* IRQ 64-bit EL1 */
316	VECT_EMPTY	/* FIQ 64-bit EL1 */
317	VECT_EMPTY	/* Error 64-bit EL1 */
318
319	VECT_EMPTY	/* Synchronous 32-bit EL1 */
320	VECT_EMPTY	/* IRQ 32-bit EL1 */
321	VECT_EMPTY	/* FIQ 32-bit EL1 */
322	VECT_EMPTY	/* Error 32-bit EL1 */
323
324/*
325 * Get the delta between the physical address we were loaded to and the
326 * virtual address we expect to run from. This is used when building the
327 * initial page table.
328 */
329get_virt_delta:
330	/* Load the physical address of virt_map */
331	adr	x29, virt_map
332	/* Load the virtual address of virt_map stored in virt_map */
333	ldr	x28, [x29]
334	/* Find PA - VA as PA' = VA' - VA + PA = VA' + (PA - VA) = VA' + x29 */
335	sub	x29, x29, x28
336	/* Find the load address for the kernel */
337	mov	x28, #(KERNBASE)
338	add	x28, x28, x29
339	ret
340
341	.align 3
342virt_map:
343	.quad	virt_map
344
345/*
346 * This builds the page tables containing the identity map, and the kernel
347 * virtual map.
348 *
349 * It relys on:
350 *  We were loaded to an address that is on a 2MiB boundary
351 *  All the memory must not cross a 1GiB boundaty
352 *  x28 contains the physical address we were loaded from
353 *
354 * TODO: This is out of date.
355 *  There are at least 5 pages before that address for the page tables
356 *   The pages used are:
357 *    - The Kernel L2 table
358 *    - The Kernel L1 table
359 *    - The Kernel L0 table             (TTBR1)
360 *    - The identity (PA = VA) L1 table
361 *    - The identity (PA = VA) L0 table (TTBR0)
362 *    - The DMAP L1 tables
363 */
364create_pagetables:
365	/* Save the Link register */
366	mov	x5, x30
367
368	/* Clean the page table */
369	adr	x6, pagetable
370	mov	x26, x6
371	adr	x27, pagetable_end
3721:
373	stp	xzr, xzr, [x6], #16
374	stp	xzr, xzr, [x6], #16
375	stp	xzr, xzr, [x6], #16
376	stp	xzr, xzr, [x6], #16
377	cmp	x6, x27
378	b.lo	1b
379
380	/*
381	 * Build the TTBR1 maps.
382	 */
383
384	/* Find the size of the kernel */
385	mov	x6, #(KERNBASE)
386
387#if defined(LINUX_BOOT_ABI)
388	/* X19 is used as 'map FDT data' flag */
389	mov	x19, xzr
390
391	/* No modules or FDT pointer ? */
392	cbz	x0, booti_no_fdt
393
394	/* Test if modulep points to modules descriptor or to FDT */
395	ldr	w8, [x0]
396	ldr	w7, =FDT_MAGIC
397	cmp	w7, w8
398	b.eq	booti_fdt
399#endif
400
401	/* Booted with modules pointer */
402	/* Find modulep - begin */
403	sub	x8, x0, x6
404	/* Add two 2MiB pages for the module data and round up */
405	ldr	x7, =(3 * L2_SIZE - 1)
406	add	x8, x8, x7
407	b	common
408
409#if defined(LINUX_BOOT_ABI)
410booti_fdt:
411	/* Booted by U-Boot booti with FDT data */
412	/* Set 'map FDT data' flag */
413	mov	x19, #1
414
415booti_no_fdt:
416	/* Booted by U-Boot booti without FTD data */
417	/* Find the end - begin */
418	ldr     x7, .Lend
419	sub     x8, x7, x6
420
421	/*
422	 * Add one 2MiB page for copy of FDT data (maximum FDT size),
423	 * one for metadata and round up
424	 */
425	ldr	x7, =(3 * L2_SIZE - 1)
426	add	x8, x8, x7
427#endif
428
429common:
430	/* Get the number of l2 pages to allocate, rounded down */
431	lsr	x10, x8, #(L2_SHIFT)
432
433	/* Create the kernel space L2 table */
434	mov	x6, x26
435	mov	x7, #VM_MEMATTR_WRITE_BACK
436	mov	x8, #(KERNBASE & L2_BLOCK_MASK)
437	mov	x9, x28
438	bl	build_l2_block_pagetable
439
440	/* Move to the l1 table */
441	add	x26, x26, #PAGE_SIZE
442
443	/* Link the l1 -> l2 table */
444	mov	x9, x6
445	mov	x6, x26
446	bl	link_l1_pagetable
447
448	/* Move to the l0 table */
449	add	x24, x26, #PAGE_SIZE
450
451	/* Link the l0 -> l1 table */
452	mov	x9, x6
453	mov	x6, x24
454	mov	x10, #1
455	bl	link_l0_pagetable
456
457	/* Link the DMAP tables */
458	ldr	x8, =DMAP_MIN_ADDRESS
459	adr	x9, pagetable_dmap;
460	mov	x10, #DMAP_TABLES
461	bl	link_l0_pagetable
462
463	/*
464	 * Build the TTBR0 maps.  As TTBR0 maps, they must specify ATTR_S1_nG.
465	 * They are only needed early on, so the VA = PA map is uncached.
466	 */
467	add	x27, x24, #PAGE_SIZE
468
469	mov	x6, x27		/* The initial page table */
470#if defined(SOCDEV_PA) && defined(SOCDEV_VA)
471	/* Create a table for the UART */
472	mov	x7, #(ATTR_S1_nG | ATTR_S1_IDX(VM_MEMATTR_DEVICE))
473	mov	x8, #(SOCDEV_VA)	/* VA start */
474	mov	x9, #(SOCDEV_PA)	/* PA start */
475	mov	x10, #1
476	bl	build_l1_block_pagetable
477#endif
478
479#if defined(LINUX_BOOT_ABI)
480	/* Map FDT data ? */
481	cbz	x19, 1f
482
483	/* Create the identity mapping for FDT data (2 MiB max) */
484	mov	x7, #(ATTR_S1_nG | ATTR_S1_IDX(VM_MEMATTR_UNCACHEABLE))
485	mov	x9, x0
486	mov	x8, x0		/* VA start (== PA start) */
487	mov	x10, #1
488	bl	build_l1_block_pagetable
489
4901:
491#endif
492
493	/* Create the VA = PA map */
494	mov	x7, #(ATTR_S1_nG | ATTR_S1_IDX(VM_MEMATTR_UNCACHEABLE))
495	mov	x9, x27
496	mov	x8, x9		/* VA start (== PA start) */
497	mov	x10, #1
498	bl	build_l1_block_pagetable
499
500	/* Move to the l0 table */
501	add	x27, x27, #PAGE_SIZE
502
503	/* Link the l0 -> l1 table */
504	mov	x9, x6
505	mov	x6, x27
506	mov	x10, #1
507	bl	link_l0_pagetable
508
509	/* Restore the Link register */
510	mov	x30, x5
511	ret
512
513/*
514 * Builds an L0 -> L1 table descriptor
515 *
516 * This is a link for a 512GiB block of memory with up to 1GiB regions mapped
517 * within it by build_l1_block_pagetable.
518 *
519 *  x6  = L0 table
520 *  x8  = Virtual Address
521 *  x9  = L1 PA (trashed)
522 *  x10 = Entry count
523 *  x11, x12 and x13 are trashed
524 */
525link_l0_pagetable:
526	/*
527	 * Link an L0 -> L1 table entry.
528	 */
529	/* Find the table index */
530	lsr	x11, x8, #L0_SHIFT
531	and	x11, x11, #L0_ADDR_MASK
532
533	/* Build the L0 block entry */
534	mov	x12, #L0_TABLE
535
536	/* Only use the output address bits */
537	lsr	x9, x9, #PAGE_SHIFT
5381:	orr	x13, x12, x9, lsl #PAGE_SHIFT
539
540	/* Store the entry */
541	str	x13, [x6, x11, lsl #3]
542
543	sub	x10, x10, #1
544	add	x11, x11, #1
545	add	x9, x9, #1
546	cbnz	x10, 1b
547
548	ret
549
550/*
551 * Builds an L1 -> L2 table descriptor
552 *
553 * This is a link for a 1GiB block of memory with up to 2MiB regions mapped
554 * within it by build_l2_block_pagetable.
555 *
556 *  x6  = L1 table
557 *  x8  = Virtual Address
558 *  x9  = L2 PA (trashed)
559 *  x11, x12 and x13 are trashed
560 */
561link_l1_pagetable:
562	/*
563	 * Link an L1 -> L2 table entry.
564	 */
565	/* Find the table index */
566	lsr	x11, x8, #L1_SHIFT
567	and	x11, x11, #Ln_ADDR_MASK
568
569	/* Build the L1 block entry */
570	mov	x12, #L1_TABLE
571
572	/* Only use the output address bits */
573	lsr	x9, x9, #PAGE_SHIFT
574	orr	x13, x12, x9, lsl #PAGE_SHIFT
575
576	/* Store the entry */
577	str	x13, [x6, x11, lsl #3]
578
579	ret
580
581/*
582 * Builds count 1 GiB page table entry
583 *  x6  = L1 table
584 *  x7  = Variable lower block attributes
585 *  x8  = VA start
586 *  x9  = PA start (trashed)
587 *  x10 = Entry count
588 *  x11, x12 and x13 are trashed
589 */
590build_l1_block_pagetable:
591	/*
592	 * Build the L1 table entry.
593	 */
594	/* Find the table index */
595	lsr	x11, x8, #L1_SHIFT
596	and	x11, x11, #Ln_ADDR_MASK
597
598	/* Build the L1 block entry */
599	orr	x12, x7, #L1_BLOCK
600	orr	x12, x12, #(ATTR_AF)
601#ifdef SMP
602	orr	x12, x12, ATTR_SH(ATTR_SH_IS)
603#endif
604
605	/* Only use the output address bits */
606	lsr	x9, x9, #L1_SHIFT
607
608	/* Set the physical address for this virtual address */
6091:	orr	x13, x12, x9, lsl #L1_SHIFT
610
611	/* Store the entry */
612	str	x13, [x6, x11, lsl #3]
613
614	sub	x10, x10, #1
615	add	x11, x11, #1
616	add	x9, x9, #1
617	cbnz	x10, 1b
618
619	ret
620
621/*
622 * Builds count 2 MiB page table entry
623 *  x6  = L2 table
624 *  x7  = Type (0 = Device, 1 = Normal)
625 *  x8  = VA start
626 *  x9  = PA start (trashed)
627 *  x10 = Entry count
628 *  x11, x12 and x13 are trashed
629 */
630build_l2_block_pagetable:
631	/*
632	 * Build the L2 table entry.
633	 */
634	/* Find the table index */
635	lsr	x11, x8, #L2_SHIFT
636	and	x11, x11, #Ln_ADDR_MASK
637
638	/* Build the L2 block entry */
639	lsl	x12, x7, #2
640	orr	x12, x12, #L2_BLOCK
641	orr	x12, x12, #(ATTR_AF)
642	orr	x12, x12, #(ATTR_S1_UXN)
643#ifdef SMP
644	orr	x12, x12, ATTR_SH(ATTR_SH_IS)
645#endif
646
647	/* Only use the output address bits */
648	lsr	x9, x9, #L2_SHIFT
649
650	/* Set the physical address for this virtual address */
6511:	orr	x13, x12, x9, lsl #L2_SHIFT
652
653	/* Store the entry */
654	str	x13, [x6, x11, lsl #3]
655
656	sub	x10, x10, #1
657	add	x11, x11, #1
658	add	x9, x9, #1
659	cbnz	x10, 1b
660
661	ret
662
663start_mmu:
664	dsb	sy
665
666	/* Load the exception vectors */
667	ldr	x2, =exception_vectors
668	msr	vbar_el1, x2
669
670	/* Load ttbr0 and ttbr1 */
671	msr	ttbr0_el1, x27
672	msr	ttbr1_el1, x24
673	isb
674
675	/* Clear the Monitor Debug System control register */
676	msr	mdscr_el1, xzr
677
678	/* Invalidate the TLB */
679	tlbi	vmalle1is
680
681	ldr	x2, mair
682	msr	mair_el1, x2
683
684	/*
685	 * Setup TCR according to the PARange and ASIDBits fields
686	 * from ID_AA64MMFR0_EL1 and the HAFDBS field from the
687	 * ID_AA64MMFR1_EL1.  More precisely, set TCR_EL1.AS
688	 * to 1 only if the ASIDBits field equals 0b0010.
689	 */
690	ldr	x2, tcr
691	mrs	x3, id_aa64mmfr0_el1
692
693	/* Copy the bottom 3 bits from id_aa64mmfr0_el1 into TCR.IPS */
694	bfi	x2, x3, #(TCR_IPS_SHIFT), #(TCR_IPS_WIDTH)
695	and	x3, x3, #(ID_AA64MMFR0_ASIDBits_MASK)
696
697	/* Check if the HW supports 16 bit ASIDS */
698	cmp	x3, #(ID_AA64MMFR0_ASIDBits_16)
699	/* If so x3 == 1, else x3 == 0 */
700	cset	x3, eq
701	/* Set TCR.AS with x3 */
702	bfi	x2, x3, #(TCR_ASID_SHIFT), #(TCR_ASID_WIDTH)
703
704	/*
705	 * Check if the HW supports access flag and dirty state updates,
706	 * and set TCR_EL1.HA and TCR_EL1.HD accordingly.
707	 */
708	mrs	x3, id_aa64mmfr1_el1
709	and	x3, x3, #(ID_AA64MMFR1_HAFDBS_MASK)
710	cmp	x3, #1
711	b.ne	1f
712	orr 	x2, x2, #(TCR_HA)
713	b	2f
7141:
715	cmp	x3, #2
716	b.ne	2f
717	orr 	x2, x2, #(TCR_HA | TCR_HD)
7182:
719	msr	tcr_el1, x2
720
721	/*
722	 * Setup SCTLR.
723	 */
724	ldr	x2, sctlr_set
725	ldr	x3, sctlr_clear
726	mrs	x1, sctlr_el1
727	bic	x1, x1, x3	/* Clear the required bits */
728	orr	x1, x1, x2	/* Set the required bits */
729	msr	sctlr_el1, x1
730	isb
731
732	ret
733
734	.align 3
735mair:
736	.quad	MAIR_ATTR(MAIR_DEVICE_nGnRnE, VM_MEMATTR_DEVICE)    |	\
737		MAIR_ATTR(MAIR_NORMAL_NC, VM_MEMATTR_UNCACHEABLE)   |	\
738		MAIR_ATTR(MAIR_NORMAL_WB, VM_MEMATTR_WRITE_BACK)    |	\
739		MAIR_ATTR(MAIR_NORMAL_WT, VM_MEMATTR_WRITE_THROUGH)
740tcr:
741	.quad (TCR_TxSZ(64 - VIRT_BITS) | TCR_TG1_4K | \
742	    TCR_CACHE_ATTRS | TCR_SMP_ATTRS)
743sctlr_set:
744	/* Bits to set */
745	.quad (SCTLR_LSMAOE | SCTLR_nTLSMD | SCTLR_UCI | SCTLR_SPAN | \
746	    SCTLR_nTWE | SCTLR_nTWI | SCTLR_UCT | SCTLR_DZE | \
747	    SCTLR_I | SCTLR_SED | SCTLR_SA0 | SCTLR_SA | SCTLR_C | \
748	    SCTLR_M | SCTLR_CP15BEN)
749sctlr_clear:
750	/* Bits to clear */
751	.quad (SCTLR_EE | SCTLR_EOE | SCTLR_IESB | SCTLR_WXN | SCTLR_UMA | \
752	    SCTLR_ITD | SCTLR_A)
753
754	.globl abort
755abort:
756	b abort
757
758	//.section .init_pagetable
759	.align 12 /* 4KiB aligned */
760	/*
761	 * 3 initial tables (in the following order):
762	 *           L2 for kernel (High addresses)
763	 *           L1 for kernel
764	 *           L1 for user   (Low addresses)
765	 */
766pagetable:
767	.space	PAGE_SIZE
768pagetable_l1_ttbr1:
769	.space	PAGE_SIZE
770pagetable_l0_ttbr1:
771	.space	PAGE_SIZE
772pagetable_l1_ttbr0:
773	.space	PAGE_SIZE
774pagetable_l0_ttbr0:
775	.space	PAGE_SIZE
776
777	.globl pagetable_dmap
778pagetable_dmap:
779	.space	PAGE_SIZE * DMAP_TABLES
780pagetable_end:
781
782el2_pagetable:
783	.space	PAGE_SIZE
784
785	.globl init_pt_va
786init_pt_va:
787	.quad pagetable		/* XXX: Keep page tables VA */
788
789	.align	4
790initstack:
791	.space	(PAGE_SIZE * KSTACK_PAGES)
792initstack_end:
793
794
795ENTRY(sigcode)
796	mov	x0, sp
797	add	x0, x0, #SF_UC
798
7991:
800	mov	x8, #SYS_sigreturn
801	svc	0
802
803	/* sigreturn failed, exit */
804	mov	x8, #SYS_exit
805	svc	0
806
807	b	1b
808END(sigcode)
809	/* This may be copied to the stack, keep it 16-byte aligned */
810	.align	3
811esigcode:
812
813	.data
814	.align	3
815	.global	szsigcode
816szsigcode:
817	.quad	esigcode - sigcode
818
819ENTRY(aarch32_sigcode)
820	.word 0xe1a0000d	// mov r0, sp
821	.word 0xe2800040	// add r0, r0, #SIGF_UC
822	.word 0xe59f700c	// ldr r7, [pc, #12]
823	.word 0xef000000	// swi #0
824	.word 0xe59f7008	// ldr r7, [pc, #8]
825	.word 0xef000000	// swi #0
826	.word 0xeafffffa	// b . - 16
827END(aarch32_sigcode)
828	.word SYS_sigreturn
829	.word SYS_exit
830	.align	3
831aarch32_esigcode:
832	.data
833	.global sz_aarch32_sigcode
834sz_aarch32_sigcode:
835	.quad aarch32_esigcode - aarch32_sigcode
836