Annotation of src/sys/arch/aarch64/aarch64/locore.S, Revision 1.52
1.52 ! mrg 1: /* $NetBSD: locore.S,v 1.51 2020/01/12 09:29:18 mrg Exp $ */
1.1 matt 2:
1.5 ryo 3: /*
4: * Copyright (c) 2017 Ryo Shimizu <ryo@nerv.org>
1.1 matt 5: * All rights reserved.
6: *
7: * Redistribution and use in source and binary forms, with or without
8: * modification, are permitted provided that the following conditions
9: * are met:
10: * 1. Redistributions of source code must retain the above copyright
11: * notice, this list of conditions and the following disclaimer.
12: * 2. Redistributions in binary form must reproduce the above copyright
13: * notice, this list of conditions and the following disclaimer in the
14: * documentation and/or other materials provided with the distribution.
15: *
1.5 ryo 16: * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
17: * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18: * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19: * DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT,
20: * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21: * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
22: * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
23: * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
24: * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
25: * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
1.1 matt 26: * POSSIBILITY OF SUCH DAMAGE.
27: */
28:
1.28 ryo 29: #include "opt_arm_debug.h"
30: #include "opt_console.h"
1.5 ryo 31: #include "opt_cpuoptions.h"
1.28 ryo 32: #include "opt_ddb.h"
33: #include "opt_fdt.h"
1.31 maxv 34: #include "opt_kasan.h"
1.5 ryo 35: #include "opt_multiprocessor.h"
36:
1.1 matt 37: #include <aarch64/asm.h>
1.5 ryo 38: #include <aarch64/hypervisor.h>
1.1 matt 39: #include "assym.h"
40:
1.52 ! mrg 41: RCSID("$NetBSD: locore.S,v 1.51 2020/01/12 09:29:18 mrg Exp $")
1.28 ryo 42:
1.49 jmcneill 43: #ifdef AARCH64_DEVICE_MEM_STRONGLY_ORDERED
1.48 jmcneill 44: #define MAIR_DEVICE_MEM MAIR_DEVICE_nGnRnE
45: #else
46: #define MAIR_DEVICE_MEM MAIR_DEVICE_nGnRE
47: #endif
1.49 jmcneill 48: #define MAIR_DEVICE_MEM_SO MAIR_DEVICE_nGnRnE
1.5 ryo 49:
1.28 ryo 50: /*#define DEBUG_LOCORE /* debug print */
51: /*#define DEBUG_LOCORE_PRINT_LOCK /* avoid mixing AP's output */
52: /*#define DEBUG_MMU /* dump MMU table */
53:
54: #define LOCORE_EL2
55:
56: #define BOOT_AP_STACKSIZE 256 /* size of temporally stack for APs */
57: #define BOOTPAGE_ALLOC_MAX (1024 * 1024) /* reserved size from _end[] */
1.1 matt 58:
1.8 ryo 59: #if (defined(VERBOSE_INIT_ARM) || defined(DEBUG_LOCORE)) && defined(EARLYCONS)
1.5 ryo 60: #define VERBOSE_LOCORE
61: #endif
1.2 nisimura 62:
1.28 ryo 63: #ifdef VERBOSE_LOCORE
64: #define VPRINT(string) PRINT(string)
65: #else
66: #define VPRINT(string)
67: #endif
1.1 matt 68:
1.28 ryo 69: /* DPRINTREG macro use x19 internally. x0-x15 may be broken */
70: #if (defined(DEBUG_LOCORE) && defined(EARLYCONS))
71: #define DPRINT(string) PRINT(string)
72: #define DPRINTREG(str, reg) mov x19,reg; PRINT(str); mov x0,x19; bl print_x0
73: #define DPRINTSREG(str, reg) mrs x19,reg; PRINT(str); mov x0,x19; bl print_x0
1.1 matt 74: #else
1.28 ryo 75: #define DPRINT(string)
76: #define DPRINTREG(str, reg)
77: #define DPRINTSREG(str, reg)
1.5 ryo 78: #endif
79:
1.28 ryo 80: #define PRINT(string) bl xprint; .asciz string; .align 2
81:
82:
1.8 ryo 83: /* load far effective address (pc relative) */
84: .macro ADDR, reg, addr
85: adrp \reg, \addr
86: add \reg, \reg, #:lo12:\addr
87: .endm
88:
1.28 ryo 89: .text
90: .align 3
91: ASENTRY_NP(aarch64_start)
92: /* keep lr & sp for return to bootloader if possible */
93: mov x27, lr
94: mov x28, sp
1.5 ryo 95:
1.8 ryo 96: /* set stack pointer for boot */
97: ADDR x0, bootstk
98: mov sp, x0
1.5 ryo 99:
1.28 ryo 100: bl clear_bss
1.24 skrll 101:
1.28 ryo 102: PRINT("boot NetBSD/aarch64\n")
1.5 ryo 103:
104: bl 1f
1.28 ryo 105: 1: DPRINTREG("PC = ", lr)
106: DPRINTREG("SP = ", sp)
107: mrs x20, CurrentEL
108: lsr x20, x20, #2
109: DPRINTREG("CurrentEL = ", x20)
110: cmp x20, #2
111: bcc 1f
1.5 ryo 112: /* EL2 registers can be accessed in EL2 or higher */
1.28 ryo 113: DPRINTSREG("SCTLR_EL2 = ", sctlr_el2)
114: DPRINTSREG("HCR_EL2 = ", hcr_el2)
1.5 ryo 115: 1:
1.28 ryo 116: DPRINTSREG("SPSR_EL1 = ", spsr_el1)
117: DPRINTSREG("CNTFREQ_EL0 = ", cntfrq_el0)
118: DPRINTSREG("SCTLR_EL1 = ", sctlr_el1)
119: DPRINTSREG("MIDR_EL1 = ", midr_el1)
120: DPRINTSREG("MPIDR_EL1 = ", mpidr_el1)
121: DPRINTSREG("ID_AA64MPFR0_EL1 = ", id_aa64pfr0_el1)
122: DPRINTSREG("ID_AA64MPFR1_EL1 = ", id_aa64pfr1_el1)
123: DPRINTSREG("ID_AA64ISAR0_EL1 = ", id_aa64isar0_el1)
124: DPRINTSREG("ID_AA64ISAR1_EL1 = ", id_aa64isar1_el1)
125: DPRINTSREG("ID_AA64MMFR0_EL1 = ", id_aa64mmfr0_el1)
126: DPRINTSREG("ID_AA64MMFR1_EL1 = ", id_aa64mmfr1_el1)
1.1 matt 127:
1.5 ryo 128: #ifdef LOCORE_EL2
1.28 ryo 129: VPRINT("Drop to EL1...")
1.5 ryo 130: # include <aarch64/aarch64/locore_el2.S>
1.28 ryo 131: VPRINT("OK\n")
132: mrs x20, CurrentEL
133: lsr x20, x20, #2
134: DPRINTREG("CurrentEL = ", x20)
1.5 ryo 135: #endif /* LOCORE_EL2 */
136:
137:
138: bl mmu_disable
139: bl init_sysregs
1.28 ryo 140: bl init_mmutable
141: cbnz x0, aarch64_fatal
142: bl save_ttbrs
1.5 ryo 143:
1.28 ryo 144: VPRINT("MMU Enable...")
1.5 ryo 145: bl mmu_enable
1.28 ryo 146: VPRINT("OK\n")
1.5 ryo 147:
1.28 ryo 148: ldr x20, =vstart /* virtual address of vstart */
149: DPRINTSREG("SPSR_EL1 = ", spsr_el1)
150: DPRINTSREG("DAIF = ", daif)
151: DPRINTREG("vstart = ", x20)
152: br x20 /* jump to the kernel virtual address */
153:
154: aarch64_fatal:
155: PRINT("fatal error occured while booting\n")
156: /* return to bootloader. if switched from EL2 to EL1, It might fail */
157: mov lr, x27
158: mov sp, x28
159: ret
1.5 ryo 160:
161: /*
162: * vstart is in kernel virtual address
163: */
164: vstart:
1.28 ryo 165: DPRINTREG("PC = ", x20)
166:
167: /* set exception vector */
168: ADDR x0, _C_LABEL(el1_vectors)
169: msr vbar_el1, x0
170:
171: /* set lwp0 stack */
1.8 ryo 172: ADDR x0, lwp0uspace
1.5 ryo 173: add x0, x0, #(UPAGES * PAGE_SIZE)
174: sub x0, x0, #TF_SIZE /* lwp0space + USPACE - TF_SIZE */
175: mov sp, x0 /* define lwp0 ksp bottom */
1.28 ryo 176: DPRINTREG("SP(lwp0,kvm) = ", sp)
1.5 ryo 177:
1.28 ryo 178: /* lwp-private = NULL */
179: msr tpidr_el0, xzr
1.29 ryo 180: msr tpidrro_el0, xzr
1.1 matt 181:
1.28 ryo 182: /* set curcpu() */
1.8 ryo 183: ADDR x0, cpu_info_store /* cpu_info_store is cpu_info[0] */
184: msr tpidr_el1, x0 /* curcpu is cpu_info[0] */
1.28 ryo 185: DPRINTREG("curcpu = ", x0);
186:
1.45 mlelstv 187: /* get cache configuration */
188: mrs x0, tpidr_el1 /* curcpu */
189: mrs x1, mpidr_el1
1.51 mrg 190: mov x2, #0
1.52 ! mrg 191: bl arm_cpu_topology_set
1.45 mlelstv 192: mov x0, xzr
193: bl aarch64_getcacheinfo
194:
1.31 maxv 195: #ifdef KASAN
196: ADDR x0, lwp0uspace
197: bl _C_LABEL(kasan_early_init)
198: #endif
1.5 ryo 199:
200: mov fp, #0 /* trace back starts here */
1.28 ryo 201: PRINT("initarm\n")
1.5 ryo 202: bl _C_LABEL(initarm) /* Off we go */
203:
1.28 ryo 204: PRINT("main\n")
1.5 ryo 205: bl _C_LABEL(main) /* call main() */
206:
207: adr x0, .Lmainreturned
208: b _C_LABEL(panic)
209: /* NOTREACHED */
1.28 ryo 210: ASEND(aarch64_start)
1.5 ryo 211:
212: .Lmainreturned:
213: .asciz "main() returned"
1.28 ryo 214: .align 2
215:
216:
217: ASENTRY_NP(clear_bss)
218: /* Zero the BSS. The size must be aligned 16, usually it should be. */
219: ADDR x14, __bss_start__
220: ADDR x15, __bss_end__
221: b 2f
222: 1: stp xzr, xzr, [x14], #16
223: 2: cmp x14, x15
224: b.lo 1b
225: ret
226: ASEND(clear_bss)
227:
228:
229: init_sysregs:
230: stp x0, lr, [sp, #-16]!
231:
1.32 ryo 232: /* init debug event */
233: ldr x0, mdscr_setting
234: msr mdscr_el1, x0
235: msr oslar_el1, xzr
1.28 ryo 236:
237: /* Clear context id register */
238: msr contextidr_el1, xzr
239:
240: /* No trap system register access, and Trap FP/SIMD access */
241: msr cpacr_el1, xzr
242:
243: /* allow to read CNTVCT_EL0 and CNTFRQ_EL0 from EL0 */
244: mrs x0, cntkctl_el1
245: orr x0, x0, #CNTKCTL_EL0VCTEN
246: msr cntkctl_el1, x0
247:
248: /* any exception not masked */
249: msr daif, xzr
250:
251: ldp x0, lr, [sp], #16
252: ret
1.5 ryo 253:
1.8 ryo 254:
255: #ifdef MULTIPROCESSOR
256:
1.28 ryo 257: #ifdef DEBUG_LOCORE
1.8 ryo 258: /*
1.28 ryo 259: * atomic_ops doesn't work before MMU enabled, so using Peterson's algorithm.
260: * this is only used to serialize debug print and avoid mixing output.
261: * Not absolutely necessary.
262: *
263: * x27 for cpuindex.
1.8 ryo 264: */
1.28 ryo 265: locore_lock_enter:
266: #ifdef DEBUG_LOCORE_PRINT_LOCK
267: mov x3, xzr /* x3 = level */
268: levelloop:
269: /* lock_level[] and lock_turn[] are always accessed via PA(devmap) */
270: ADDR x0, kern_vtopdiff
271: ldr x0, [x0]
272: ldr x4, =lock_level
273: sub x4, x4, x0
274: ldr x5, =lock_turn
275: sub x5, x5, x0
276:
277: strh w3, [x4, x27, lsl #1] /* lock_level[i] = level */
278: dsb sy
279: strh w27, [x5, x3, lsl #1] /* lock_turn[level] = i */
280: dsb sy
281: waitloop:
282: dmb sy
283: ldrh w0, [x5, x3, lsl #1] /* lock_turn[level] == i ? */
284: cmp x27, x0
285: bne nextlevel
286:
287: mov x2, xzr /* k = 0 */
288: levelcheck:
289: cmp x2, x27
290: beq levelcheck_next
291:
292: dmb sy
293: ldrsh w0, [x4, x2, lsl #1] /* lock_level[k] >= level */
294: cmp w0, w3
295: bge waitloop
296: levelcheck_next:
297: add x2, x2, #1 /* k++ */
298: cmp x2, #MAXCPUS
299: bne levelcheck
300: nextlevel:
301: add x3, x3, #1
302: cmp x3, #(MAXCPUS - 1)
303: bne levelloop
304: #endif /* DEBUG_LOCORE_PRINT_LOCK */
305: ret
306:
307:
308: locore_lock_exit:
309: #ifdef DEBUG_LOCORE_PRINT_LOCK
310: /* lock_level[] and lock_turn[] are always accessed via PA(devmap) */
311: ADDR x0, kern_vtopdiff
312: ldr x0, [x0]
313: ldr x1, =lock_level
314: sub x1, x1, x0
315: mvn x0, xzr
316: strh w0, [x1, x27, lsl #1] /* lock_level[i] = -1 */
317: dsb sy
318: #endif /* DEBUG_LOCORE_PRINT_LOCK */
319: ret
320:
321:
322: /* print "[CPU$x27] " (x27 for cpuindex) */
323: printcpu:
1.8 ryo 324: stp x0, lr, [sp, #-16]!
1.28 ryo 325: PRINT("[CPU"); \
326: mov x0, x27; \
327: bl _printdec_x0; \
328: PRINT("] "); \
1.8 ryo 329: ldp x0, lr, [sp], #16
330: ret
331:
1.28 ryo 332: #define CPU_DPRINT(str) \
333: bl locore_lock_enter; \
334: bl printcpu; \
335: DPRINT(str); \
336: bl locore_lock_exit
337:
338: /*
339: * CPU_DPRINTREG macro use x19 internally. x0-x15 may be broken.
340: * x27 for cpuindex.
341: */
342: #define CPU_DPRINTREG(str,reg) \
343: mov x19, reg; \
344: bl locore_lock_enter; \
345: bl printcpu; \
346: PRINT(str); \
347: mov x0, x19; \
348: bl print_x0; \
349: bl locore_lock_exit
350:
351: #define CPU_DPRINTSREG(str, reg) \
352: mrs x19, reg; \
353: CPU_DPRINTREG(str, x19)
354:
355: #else /* DEBUG_LOCORE */
356:
357: #define CPU_DPRINT(str)
358: #define CPU_DPRINTREG(str,reg)
359: #define CPU_DPRINTSREG(str, reg)
360:
361: #endif /* DEBUG_LOCORE */
1.8 ryo 362:
1.28 ryo 363: ENTRY_NP(cpu_mpstart)
1.25 ryo 364: mrs x3, mpidr_el1
365: ldr x0, =(MPIDR_AFF0|MPIDR_AFF1|MPIDR_AFF2|MPIDR_AFF3)
366: and x3, x3, x0
367:
1.28 ryo 368: /*
369: * resolve own cpuindex. my mpidr is stored in
370: * extern uint64_t cpu_mpidr[MAXCPUS]
371: */
372: ADDR x0, _C_LABEL(cpu_mpidr)
1.25 ryo 373: mov x1, xzr
374: 1:
375: add x1, x1, #1
1.44 jmcneill 376: cmp x1, #MAXCPUS /* cpuindex >= MAXCPUS ? */
1.25 ryo 377: bge toomanycpus
1.35 skrll 378: ldr x2, [x0, x1, lsl #3] /* cpu_mpidr[cpuindex] */
1.25 ryo 379: cmp x2, x3 /* == mpidr_el1 & MPIDR_AFF ? */
380: bne 1b
381:
382: mov x27, x1 /* x27 = cpuindex */
1.28 ryo 383:
384: /*
385: * x27 = cpuindex
386: */
1.8 ryo 387:
388: /* set stack pointer for boot */
1.28 ryo 389: mov x1, #BOOT_AP_STACKSIZE
1.8 ryo 390: mul x1, x1, x27
1.43 skrll 391: ADDR x0, bootstk
392: add sp, x0, x1 /* sp = bootstk + (BOOT_AP_STACKSIZE * cpuindex) */
1.8 ryo 393:
394: bl 1f
1.28 ryo 395: 1: CPU_DPRINTREG("PC = ", lr)
396: CPU_DPRINTREG("SP = ", sp)
397: mrs x20, CurrentEL
398: lsr x20, x20, #2
399: CPU_DPRINTREG("CurrentEL = ", x20)
400: cmp x20, #2
401: bcc 1f
402: /* EL2 registers can be accessed in EL2 or higher */
403: CPU_DPRINTSREG("SCTLR_EL2 = ", sctlr_el2)
404: CPU_DPRINTSREG("HCR_EL2 = ", hcr_el2)
405: 1:
406: CPU_DPRINTSREG("SPSR_EL1 = ", spsr_el1)
407: CPU_DPRINTSREG("SCTLR_EL1 = ", sctlr_el1)
408: CPU_DPRINTSREG("MIDR_EL1 = ", midr_el1)
409: CPU_DPRINTSREG("MPIDR_EL1 = ", mpidr_el1)
1.8 ryo 410:
411: #ifdef LOCORE_EL2
1.28 ryo 412: CPU_DPRINT("Drop to EL1...\n")
1.8 ryo 413: bl drop_to_el1
1.28 ryo 414: CPU_DPRINT("Drop to EL1 OK\n")
415: mrs x20, CurrentEL
416: lsr x20, x20, #2
417: CPU_DPRINTREG("CurrentEL = ", x20)
1.8 ryo 418: #endif /* LOCORE_EL2 */
419:
1.28 ryo 420:
1.8 ryo 421: bl mmu_disable
422: bl init_sysregs
423:
1.28 ryo 424: CPU_DPRINT("MMU Enable...\n")
425: bl load_ttbrs
1.8 ryo 426: bl mmu_enable
1.28 ryo 427: CPU_DPRINT("MMU Enable OK\n")
1.8 ryo 428:
429: /* jump to virtual address */
1.28 ryo 430: ldr x20, =mp_vstart
431: br x20
1.8 ryo 432:
433: mp_vstart:
1.28 ryo 434: CPU_DPRINTREG("PC = ", x20)
435:
1.8 ryo 436: /* set exception vector */
1.28 ryo 437: ADDR x0, _C_LABEL(el1_vectors)
1.8 ryo 438: msr vbar_el1, x0
439:
1.28 ryo 440: /* lwp-private = NULL */
441: msr tpidr_el0, xzr
1.29 ryo 442: msr tpidrro_el0, xzr
1.20 ryo 443:
444: /* set curcpu(), and fill curcpu()->ci_{midr,mpidr} */
445: mov x0, #CPU_INFO_SIZE
446: mul x0, x27, x0
447: ADDR x1, _C_LABEL(cpu_info_store)
448: add x0, x0, x1 /* x0 = &cpu_info_store[cpuindex] */
449: msr tpidr_el1, x0 /* tpidr_el1 = curcpu() = x0 */
450:
1.8 ryo 451: mrs x1, midr_el1
1.20 ryo 452: str x1, [x0, #CI_MIDR] /* curcpu()->ci_cpuid = midr_el1 */
1.8 ryo 453: mrs x1, mpidr_el1
1.20 ryo 454: str x1, [x0, #CI_MPIDR] /* curcpu()->ci_mpidr = mpidr_el1 */
1.8 ryo 455:
1.44 jmcneill 456: mov x0, #32
457: udiv x1, x27, x0
458: ADDR x0, _C_LABEL(aarch64_cpu_hatched)
459: add x28, x0, x1, lsl #2 /* x28 = &aarch64_cpu_hatched[cpuindex/32] */
460: mov x0, #1
461: mov x2, #32
462: msub x1, x1, x2, x27
463: lsl x29, x0, x1 /* x29 = 1 << (cpuindex % 32) */
464:
1.8 ryo 465: /*
1.44 jmcneill 466: * atomic_or_uint(&aarch64_cpu_hatched[cpuindex/32], 1<<cpuindex%32)
1.8 ryo 467: * to tell my activity to primary processor.
468: */
1.42 jmcneill 469: mov x0, x28
1.44 jmcneill 470: mov x1, x29
1.42 jmcneill 471: bl _C_LABEL(atomic_or_uint) /* hatched! */
472: dsb sy
1.8 ryo 473: sev
474:
1.44 jmcneill 475: mov x0, #32
476: udiv x1, x27, x0
477: ADDR x0, _C_LABEL(aarch64_cpu_mbox)
478: add x28, x0, x1, lsl #2 /* x28 = &aarch64_cpu_mbox[cpuindex/32] */
479:
1.42 jmcneill 480: /* wait for the mailbox start bit to become true */
1.8 ryo 481: 1:
482: dmb sy
1.44 jmcneill 483: ldr w20, [x28]
484: tst w20, w29
1.8 ryo 485: bne 9f
486: wfe
487: b 1b
488: 9:
489:
490: /* fill my cpu_info */
1.20 ryo 491: mrs x0, tpidr_el1 /* curcpu() */
1.8 ryo 492:
493: ldr x1, [x0, #CI_IDLELWP] /* x1 = curcpu()->ci_data.cpu_idlelwp */
494: str x1, [x0, #CI_CURLWP] /* curlwp is idlelwp */
495:
1.28 ryo 496: /* get my stack from lwp */
1.8 ryo 497: ldr x2, [x1, #L_PCB] /* x2 = lwp_getpcb(idlelwp) */
498: add x2, x2, #(UPAGES * PAGE_SIZE)
499: sub sp, x2, #TF_SIZE /* sp = pcb + USPACE - TF_SIZE */
500:
501:
502: mov fp, xzr /* trace back starts here */
503: bl _C_LABEL(cpu_hatch)
504: mov x0, xzr
505: b _C_LABEL(idle_loop) /* never to return */
1.30 skrll 506: END(cpu_mpstart)
1.8 ryo 507:
1.20 ryo 508: toomanycpus:
1.28 ryo 509: CPU_DPRINT("too many cpus, or MPIDR not exists in cpu_mpidr[]\n")
1.20 ryo 510: 1: wfi
511: b 1b
512:
513:
1.8 ryo 514: #else /* MULTIPROCESSOR */
515:
1.30 skrll 516: ENTRY_NP(cpu_mpstart)
1.8 ryo 517: 1: wfi
518: b 1b
1.30 skrll 519: END(cpu_mpstart)
1.8 ryo 520:
521: #endif /* MULTIPROCESSOR */
522:
1.28 ryo 523:
1.5 ryo 524: /*
525: * xprint - print strings pointed by $PC(LR)
526: * and return to the end of string.
1.28 ryo 527: * "\n" will be replaced "\r\n"
1.5 ryo 528: * e.g.)
1.28 ryo 529: * bl xprint <- call
530: * .ascii "Hello\n\0" <- wouldn't return here
1.5 ryo 531: * .align 2
1.28 ryo 532: * nop <- return to here
1.22 skrll 533: *
1.5 ryo 534: */
1.28 ryo 535: xprint:
536: mov x0, lr
537: bl _C_LABEL(uartputs)
538: add x0, x0, #3
539: bic lr, x0, #3
540: ret
1.22 skrll 541:
1.28 ryo 542: /*
543: * uartputs(str) - print strings with replacing "\n" to "\r\n".
544: * returns the address after the end of the string. (x0 = next of '\0')
545: */
546: ENTRY_NP(uartputs)
547: stp x19, lr, [sp, #-16]!
548: mov x19, x0
1.22 skrll 549: ldrb w0, [x19], #1
1.28 ryo 550: cbz w0, 9f
1.5 ryo 551: 1:
1.28 ryo 552: cmp x0, #'\n'
553: bne 2f
554: mov x0, #'\r'
555: bl uartputc
556: mov x0, #'\n'
557: 2:
1.5 ryo 558: bl uartputc
1.22 skrll 559: ldrb w0, [x19], #1
1.5 ryo 560: cbnz w0, 1b
561: 9:
1.28 ryo 562: mov x0, x19
563: ldp x19, lr, [sp], #16
1.5 ryo 564: ret
1.21 maxv 565: END(uartputs)
1.5 ryo 566:
1.28 ryo 567: /*
568: * print x0 in 16 widths hexadecimal.
569: *
570: * x0 is preserved despite being caller saved.
571: * other caller saved registers will be broken.
572: */
573: _print_x0:
1.5 ryo 574: stp x0, lr, [sp, #-16]!
1.22 skrll 575: stp x20, x21, [sp, #-16]!
1.5 ryo 576:
1.22 skrll 577: mov x21, x0 /* number to display */
578: mov x20, #60 /* num of shift */
1.5 ryo 579: 1:
1.22 skrll 580: ror x0, x21, x20
581: and x0, x0, #0xf
1.5 ryo 582: cmp x0, #10
583: blt 2f
584: add x0, x0, #('a' - 10 - '0')
585: 2: add x0, x0, #'0'
586: bl uartputc
1.22 skrll 587: subs x20, x20, #4
1.5 ryo 588: bge 1b
589:
1.22 skrll 590: ldp x20, x21, [sp], #16
1.5 ryo 591: ldp x0, lr, [sp], #16
592: ret
593:
1.28 ryo 594: /*
595: * print x0 in decimal.
596: *
597: * x0 is preserved despite being caller saved.
598: * other caller saved registers will be broken.
599: */
600: _printdec_x0:
601: stp x0, lr, [sp, #-(16+32)]!
602: add x8, sp, #(16+32)
1.5 ryo 603:
1.28 ryo 604: strb wzr, [x8, #-1]!
605: 1:
606: mov x10, #10
607: udiv x1, x0, x10 /* x1 = x0 / 10 */
608: msub x3, x1, x10, x0 /* x3 = x0 % 10 */
1.5 ryo 609: mov x0, x1
1.1 matt 610:
1.28 ryo 611: add x3, x3, #'0'
612: strb w3, [x8, #-1]!
613: cbnz x0, 1b
1.1 matt 614:
1.28 ryo 615: mov x0, x8
616: bl uartputs
1.5 ryo 617:
1.28 ryo 618: ldp x0, lr, [sp], #(16+32)
1.5 ryo 619: ret
620:
621: /*
1.28 ryo 622: * print x0 in 16 widths hexadecimal with crlf.
623: *
624: * x0 is preserved despite being caller saved.
625: * other caller saved registers will be broken.
1.5 ryo 626: */
1.28 ryo 627: print_x0:
1.5 ryo 628: stp x0, lr, [sp, #-16]!
1.28 ryo 629: bl _print_x0
630: PRINT("\n")
1.5 ryo 631: ldp x0, lr, [sp], #16
1.1 matt 632: ret
633:
1.28 ryo 634: #ifdef DEBUG_MMU
1.1 matt 635: /*
1.28 ryo 636: * tinyprintf() supports only maximum 7 '%x', '%d' and '%s' formats.
637: * width and any modifiers are ignored. '\n' will be replaced to '\r\n'.
638: *
639: * '%x' will be always expanded 16 widths hexadicimal.
640: * e.g., tinyprintf("Hello %s %x\n", "World", 0x12345)
641: * outputs "Hello World 0000000000012345\r\n"
1.2 nisimura 642: */
1.28 ryo 643: tinyprintf:
1.5 ryo 644: stp x0, lr, [sp, #-16]!
1.22 skrll 645: stp x19, x20, [sp, #-16]!
1.28 ryo 646: stp x7, x8, [sp, #-16]!
647: stp x5, x6, [sp, #-16]!
648: stp x3, x4, [sp, #-16]!
649: stp x1, x2, [sp, #-16]!
1.22 skrll 650:
1.28 ryo 651: mov x20, xzr
652: mov x19, x0
653: ldrb w0, [x19], #1
654: cbz w0, tinyprintf_done
1.5 ryo 655:
1.28 ryo 656: tinyprintf_loop:
657: cmp x0, #'\n'
658: bne 1f
659: /* '\n' -> '\r', '\n' */
660: mov x0, #'\r'
661: bl uartputc
662: mov x0, #'\n'
1.5 ryo 663: 1:
1.22 skrll 664:
1.28 ryo 665: cmp x0, #'%'
666: bne tinyprintf_putc
667: cmp x20, #8
668: bcs tinyprintf_putc
669:
670: tinyprintf_fetch_fmt:
671: ldrb w9, [x19], #1
672: cbz w9, tinyprintf_done
673:
674: /* width and modifier are ignored */
675: cmp x9, #'h'
676: beq tinyprintf_fetch_fmt
677: cmp x9, #'l'
678: beq tinyprintf_fetch_fmt
679: cmp x9, #'j'
680: beq tinyprintf_fetch_fmt
681: cmp x9, #'t'
682: beq tinyprintf_fetch_fmt
683: cmp x9, #'z'
684: beq tinyprintf_fetch_fmt
685: cmp x9, #'0'
686: bcc 1f
687: cmp x9, #'9'
688: bls tinyprintf_fetch_fmt
689: 1:
690: ldr x0, [sp, x20, lsl #3] /* get Nth argument */
1.22 skrll 691: add x20, x20, #1
1.5 ryo 692:
1.28 ryo 693: cmp x9, #'x'
694: bne 5f
695: /* "%x" format */
696: bl _print_x0
697: b tinyprintf_next
698: 5:
699: cmp x9, #'d'
700: bne 5f
701: /* "%d" format */
702: bl _printdec_x0
703: b tinyprintf_next
704: 5:
705: cmp x9, #'s'
706: bne 5f
707: /* "%s" format */
708: bl _C_LABEL(uartputs)
709: b tinyprintf_next
710: 5:
711:
712: tinyprintf_putc:
713: bl uartputc
714: tinyprintf_next:
715: ldrb w0, [x19], #1
716: cbnz w0, tinyprintf_loop
717:
718: tinyprintf_done:
719: mov x0, x19
720:
721: ldp x1, x2, [sp], #16
722: ldp x3, x4, [sp], #16
723: ldp x5, x6, [sp], #16
724: ldp x7, x8, [sp], #16
1.22 skrll 725: ldp x19, x20, [sp], #16
1.5 ryo 726: ldp x0, lr, [sp], #16
1.2 nisimura 727: ret
1.28 ryo 728: #endif /* defined(DEBUG_LOCORE) || defined(DEBUG_MMU) */
1.2 nisimura 729:
1.5 ryo 730:
1.28 ryo 731: save_ttbrs:
732: /* save ttbr[01]_el1 for AP */
733: mrs x0, ttbr0_el1
734: mrs x1, ttbr1_el1
735: ADDR x2, ttbr_save
736: stp x0, x1, [x2]
737: ret
1.5 ryo 738:
1.28 ryo 739: load_ttbrs:
740: /* load ttbr[01]_el1 */
741: ADDR x2, ttbr_save
742: ldp x0, x1, [x2]
743: msr ttbr0_el1, x0
744: msr ttbr1_el1, x1
1.3 nisimura 745: ret
1.2 nisimura 746:
1.5 ryo 747:
1.28 ryo 748: init_mmutable:
749: stp x26, lr, [sp, #-16]!
1.22 skrll 750:
1.28 ryo 751: /* first allocated page must be kernel l0pt = ARM_BOOTSTRAP_LxPT */
752: bl bootpage_alloc
753: cbz x0, init_mmutable_error
754: msr ttbr1_el1, x0
755:
756: bl bootpage_alloc
757: cbz x0, init_mmutable_error
758: msr ttbr0_el1, x0
1.22 skrll 759:
1.5 ryo 760: #ifdef DEBUG_MMU
1.28 ryo 761: adr x26, tinyprintf
762: #else
763: mov x26, xzr
1.5 ryo 764: #endif
765:
1.28 ryo 766: /*
767: * int
768: * pmapboot_enter(
769: * x0: vaddr_t va,
770: * x1: paddr_t pa,
771: * x2: psize_t size,
772: * x3: psize_t blocksize, // L[123]_SIZE
773: * x4: pt_entry_t attr, // pte attributes. LX_BLKPAG_*
774: * x5: flags,
775: * x6: pd_entry_t *(*physpage_allocator)(void),
776: * x7: void (*pr)(const char *, ...)
777: * );
778: */
1.5 ryo 779:
1.28 ryo 780: #ifdef CONSADDR
781: VPRINT("Creating VA=PA tables for CONSADDR\n")
782: mov x7, x26 /* pr func */
783: adr x6, bootpage_alloc /* allocator */
784: mov x5, xzr /* flags = 0 */
785: mov x4, #LX_BLKPAG_ATTR_DEVICE_MEM|LX_BLKPAG_AP_RW /* attr */
1.40 jmcneill 786: orr x4, x4, #LX_BLKPAG_UXN|LX_BLKPAG_PXN
1.28 ryo 787: mov x3, #L2_SIZE /* blocksize */
788: mov x2, #L2_SIZE /* size */
789: ldr x1, =CONSADDR /* pa */
790: mov x0, x1 /* va */
791: bl pmapboot_enter
792: cbnz x0, init_mmutable_error
793: #elif defined(EARLYCONS)
794: /* CONSADDR is unknown, but need to map UART */
795: VPRINT("Creating VA=PA tables (0x00000000-0xffffffff)\n")
796: mov x7, x26 /* pr func */
797: adr x6, bootpage_alloc /* allocator */
798: mov x5, xzr /* flags = 0 */
799: mov x4, #LX_BLKPAG_ATTR_DEVICE_MEM|LX_BLKPAG_AP_RW /* attr */
1.40 jmcneill 800: orr x4, x4, #LX_BLKPAG_UXN|LX_BLKPAG_PXN
1.28 ryo 801: mov x3, #L2_SIZE /* blocksize */
802: mov x2, #(1024*1024*1024*4) /* size */
803: mov x1, xzr /* pa */
804: mov x0, xzr /* va */
805: bl pmapboot_enter
806: cbnz x0, init_mmutable_error
807: #endif
808:
809: /* identity mapping for kernel image */
810: VPRINT("Creating VA=PA tables for kernel image\n")
811: mov x7, x26 /* pr func */
812: adr x6, bootpage_alloc /* allocator */
813: mov x5, xzr /* flags = 0 */
814: mov x4, #LX_BLKPAG_ATTR_NORMAL_NC|LX_BLKPAG_AP_RW /* attr */
815: mov x3, #L2_SIZE /* blocksize */
816: adr x0, start /* va = start */
817: ADDR x2, _end
818: sub x2, x2, x0 /* size = _end - start */
1.41 skrll 819: add x2, x2, #BOOTPAGE_ALLOC_MAX /* for bootpage_alloc() */
1.28 ryo 820: mov x1, x0 /* pa */
821: bl pmapboot_enter
822: cbnz x0, init_mmutable_error
823:
824: #ifdef FDT
825: ADDR x8, _C_LABEL(fdt_addr_r)
826: ldr x8, [x8]
827:
828: VPRINT("Creating VA=PA tables for FDT\n")
829: mov x7, x26 /* pr func */
830: adr x6, bootpage_alloc /* allocator */
831: mov x5, xzr /* flags = 0 */
832: mov x4, #LX_BLKPAG_ATTR_NORMAL_NC|LX_BLKPAG_AP_RW /* attr */
1.46 skrll 833: orr x4, x4, #LX_BLKPAG_UXN|LX_BLKPAG_PXN
1.28 ryo 834: mov x3, #L2_SIZE /* blocksize */
835: mov x2, #L2_SIZE /* size */
836: mov x1, x8 /* pa */
837: mov x0, x8 /* va */
838: bl pmapboot_enter
839: cbnz x0, init_mmutable_error
840: #endif
841:
842: VPRINT("Creating KVA=PA tables\n")
843: mov x7, x26 /* pr func */
844: adr x6, bootpage_alloc /* allocator */
845: mov x5, xzr /* flags = 0 */
846: mov x4, #LX_BLKPAG_ATTR_NORMAL_WB|LX_BLKPAG_AP_RW /* attr */
847: orr x4, x4, #LX_BLKPAG_UXN
848: mov x3, #L2_SIZE /* blocksize */
1.38 skrll 849: adr x1, start /* pa = start */
1.28 ryo 850: ADDR x2, _end
851: sub x2, x2, x1 /* size = _end - start */
1.37 skrll 852: ldr x0, =start /* va */
1.28 ryo 853: bl pmapboot_enter
854: cbnz x0, init_mmutable_error
1.5 ryo 855:
1.28 ryo 856: VPRINT("OK\n");
857: mov x0, xzr
858: b init_mmutable_done
859: init_mmutable_error:
860: mvn x0, xzr
861: init_mmutable_done:
862: ldp x26, lr, [sp], #16
863: ret
1.5 ryo 864:
1.28 ryo 865: /* return PA of allocated page */
866: ENTRY_NP(bootpage_alloc)
867: /* x2 = kernend_extra */
868: ADDR x3, kernend_extra
869: ldr x2, [x3]
870: /* if (kernend_extra < 0) return NULL */
871: mov x0, xzr
872: cmp x2, xzr
873: bmi bootpage_alloc_done
1.5 ryo 874:
1.28 ryo 875: /* x0 = PA of _end[] */
876: ADDR x1, kern_vtopdiff
877: ldr x1, [x1]
878: ldr x0, =ARM_BOOTSTRAP_LxPT
879: sub x0, x0, x1
880:
881: /* x0 = ARM_BOOTSTRAP_LxPT + kernend_extra */
882: add x0, x0, x2
883:
884: /* kernend_extra += PAGE_SIZE; */
885: add x2, x2, #PAGE_SIZE
886: str x2, [x3]
887:
888: /* clear allocated page */
889: mov x1, x0
890: add x2, x1, #PAGE_SIZE
891: 1: stp xzr, xzr, [x1], #16
892: cmp x1, x2
893: bcc 1b
894: bootpage_alloc_done:
895: ret
896: END(bootpage_alloc)
1.5 ryo 897:
898:
1.28 ryo 899: mmu_disable:
1.5 ryo 900: dsb sy
901: mrs x0, sctlr_el1
902: bic x0, x0, SCTLR_M /* clear MMU enable bit */
903: msr sctlr_el1, x0
904: isb
905: ret
906:
1.28 ryo 907: mmu_enable:
1.5 ryo 908: dsb sy
909:
910: /* Invalidate all TLB */
911: dsb ishst
912: #ifdef MULTIPROCESSOR
913: tlbi vmalle1is
914: #else
915: tlbi vmalle1
916: #endif
917: dsb ish
918: isb
919:
920: ldr x0, mair_setting
921: msr mair_el1, x0
922:
923:
924: /* TCR_EL1:IPS[34:32] = AA64MMFR0:PARange[3:0] */
925: ldr x0, tcr_setting
926: mrs x1, id_aa64mmfr0_el1
927: bfi x0, x1, #32, #3
928: msr tcr_el1, x0
1.1 matt 929:
930: /*
1.5 ryo 931: * configure SCTLR
1.1 matt 932: */
1.5 ryo 933: mrs x0, sctlr_el1
934: ldr x1, sctlr_clear
935: bic x0, x0, x1
936: ldr x1, sctlr_set
937: orr x0, x0, x1
938:
939: ldr x1, sctlr_ee
940: #ifdef __AARCH64EB__
941: orr x0, x0, x1 /* set: BigEndian */
942: #else
943: bic x0, x0, x1 /* clear: LittleEndian */
944: #endif
945: msr sctlr_el1, x0 /* enabling MMU! */
946: isb
1.1 matt 947:
948: ret
1.28 ryo 949:
1.1 matt 950:
1.5 ryo 951: .align 3
952: mair_setting:
953: .quad ( \
954: __SHIFTIN(MAIR_NORMAL_WB, MAIR_ATTR0) | \
955: __SHIFTIN(MAIR_NORMAL_NC, MAIR_ATTR1) | \
956: __SHIFTIN(MAIR_NORMAL_WT, MAIR_ATTR2) | \
1.49 jmcneill 957: __SHIFTIN(MAIR_DEVICE_MEM, MAIR_ATTR3) | \
958: __SHIFTIN(MAIR_DEVICE_MEM_SO, MAIR_ATTR4))
1.5 ryo 959:
960: #define VIRT_BIT 48
1.28 ryo 961:
962: #ifdef MULTIPROCESSOR
963: #define TCR_SHAREABLE (TCR_SH0_INNER | TCR_SH1_INNER)
964: #else
965: #define TCR_SHAREABLE (TCR_SH0_NONE | TCR_SH1_NONE)
966: #endif
967:
1.5 ryo 968: tcr_setting:
969: .quad ( \
970: __SHIFTIN(64 - VIRT_BIT, TCR_T1SZ) | \
971: __SHIFTIN(64 - VIRT_BIT, TCR_T0SZ) | \
972: TCR_AS64K | \
973: TCR_TG1_4KB | TCR_TG0_4KB | \
974: TCR_ORGN0_WB_WA | \
975: TCR_IRGN0_WB_WA | \
976: TCR_ORGN1_WB_WA | \
1.28 ryo 977: TCR_IRGN1_WB_WA) | TCR_SHAREABLE
1.5 ryo 978:
979:
980: #ifdef AARCH64_ALIGNMENT_CHECK
981: #define SCTLR_A_CONFIG SCTLR_A
982: #else
983: #define SCTLR_A_CONFIG 0
984: #endif
985:
986: #ifdef AARCH64_EL0_STACK_ALIGNMENT_CHECK
987: #define SCTLR_SA0_CONFIG SCTLR_SA0
988: #else
989: #define SCTLR_SA0_CONFIG 0
990: #endif
991:
992: #ifdef AARCH64_EL1_STACK_ALIGNMENT_CHECK
993: #define SCTLR_SA_CONFIG SCTLR_SA
994: #else
995: #define SCTLR_SA_CONFIG 0
1.3 nisimura 996: #endif
997:
1.5 ryo 998:
999: sctlr_ee:
1.39 skrll 1000: .quad (SCTLR_EE | SCTLR_EOE) /* Endianness of Exception and EL0 */
1.5 ryo 1001: sctlr_set:
1002: .quad ( \
1003: SCTLR_LSMAOE | /* Load/Store Multiple Atomicity and Ordering */ \
1004: SCTLR_nTLSMD | /* no Trap Load/Store Multiple to Device */ \
1.6 ryo 1005: SCTLR_UCI | /* Enables EL0 DC {CVAU,CIVAC,CVAC}, IC IVAU */ \
1.5 ryo 1006: SCTLR_SPAN | /* This field resets to 1 */ \
1007: SCTLR_UCT | /* Enables EL0 access to the CTR_EL0 */ \
1.9 ryo 1008: SCTLR_nTWE | /* EL0 WFE non-trapping */ \
1009: SCTLR_nTWI | /* EL0 WFI non-trapping */ \
1.5 ryo 1010: SCTLR_DZE | /* Enables access to the DC ZVA instruction */ \
1011: SCTLR_I | /* Instruction cache enable */ \
1012: SCTLR_SED | /* SETEND instruction disable */ \
1013: SCTLR_C | /* Cache enable */ \
1014: SCTLR_M | /* MMU Enable */ \
1015: SCTLR_SA0_CONFIG | \
1016: SCTLR_SA_CONFIG | \
1017: SCTLR_A_CONFIG | \
1018: 0)
1019: sctlr_clear:
1020: .quad ( \
1021: SCTLR_IESB | /* Enable Implicit ErrorSynchronizationBarrier */ \
1022: SCTLR_WXN | /* Write permission implies Execute Never (W^X) */ \
1023: SCTLR_UMA | /* EL0 Controls access to interrupt masks */ \
1024: SCTLR_ITD | /* IT instruction disable */ \
1025: SCTLR_THEE | /* T32EE is not implemented */ \
1026: SCTLR_CP15BEN | /* CP15 barrier enable */ \
1027: SCTLR_SA0 | /* Enable EL0 stack alignment check */ \
1028: SCTLR_SA | /* Enable SP alignment check */ \
1029: SCTLR_A | /* Alignment check enable */ \
1030: 0)
1031:
1.32 ryo 1032: mdscr_setting:
1033: .quad ( \
1034: MDSCR_TDCC | /* Trap Debug Communications Channel access */ \
1035: 0)
1036:
1.28 ryo 1037: .L_devmap_addr:
1038: .quad VM_KERNEL_IO_ADDRESS
1039:
1040: .data
1041:
1042: #ifdef DEBUG_LOCORE_PRINT_LOCK
1043: .align 2
1044: lock_level:
1045: .fill MAXCPUS, 2, -1
1046: lock_turn:
1047: .fill (MAXCPUS - 1), 2, -1
1048: #endif /* DEBUG_LOCORE_PRINT_LOCK */
1049:
1050: .align 3
1051: ttbr_save:
1052: .space 8 * 2
1.5 ryo 1053:
1.8 ryo 1054: .bss
1055:
1.5 ryo 1056: .align PGSHIFT
1057: .global _C_LABEL(lwp0uspace)
1058: _C_LABEL(lwp0uspace):
1059: .space UPAGES * PAGE_SIZE
1.8 ryo 1060: bootstk:
1061:
1062: #ifdef MULTIPROCESSOR
1.28 ryo 1063: .space BOOT_AP_STACKSIZE * (MAXCPUS - 1)
1.8 ryo 1064: #endif
1065:
1.28 ryo 1066: .section ".init_pagetable", "aw", %nobits
1.5 ryo 1067: .align PGSHIFT
1.28 ryo 1068: .global ARM_BOOTSTRAP_LxPT
1069: ARM_BOOTSTRAP_LxPT:
1070: l0pt_kern:
1.30 skrll 1071:
1072: .section "_init_memory", "aw", %nobits
1073: .align PGSHIFT
1074:
1075: /* None currently */
CVSweb <webmaster@jp.NetBSD.org>