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