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