sys/arm64/arm64/locore.S

   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*/
  87 1:
  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
 135 virtdone:
 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
 144 1:
 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  */
 194 ENTRY(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
 216 mp_virtdone:
 217         ldr     x4, =secondary_stacks
 218         mov     x5, #(PAGE_SIZE * KSTACK_PAGES)
 219         mul     x5, x0, x5
 220         add     sp, x4, x5
 221
 222         b       init_secondary
 223 END(mpentry)
 224 #endif
 225
 226 /*
 227  * If we are started in EL2, configure the required hypervisor
 228  * registers and drop to EL1.
 229  */
 230 drop_to_el1:
 231         mrs     x23, CurrentEL
 232         lsr     x23, x23, #2
 233         cmp     x23, #0x2
 234         b.eq    1f
 235         ret
 236 1:
 237         /* Configure the Hypervisor */
 238         mov     x2, #(HCR_RW)
 239         msr     hcr_el2, x2
 240
 241         /* Load the Virtualization Process ID Register */
 242         mrs     x2, midr_el1
 243         msr     vpidr_el2, x2
 244
 245         /* Load the Virtualization Multiprocess ID Register */
 246         mrs     x2, mpidr_el1
 247         msr     vmpidr_el2, x2
 248
 249         /* Set the bits that need to be 1 in sctlr_el1 */
 250         ldr     x2, .Lsctlr_res1
 251         msr     sctlr_el1, x2
 252
 253         /* Don't trap to EL2 for exceptions */
 254         mov     x2, #CPTR_RES1
 255         msr     cptr_el2, x2
 256
 257         /* Don't trap to EL2 for CP15 traps */
 258         msr     hstr_el2, xzr
 259
 260         /* Enable access to the physical timers at EL1 */
 261         mrs     x2, cnthctl_el2
 262         orr     x2, x2, #(CNTHCTL_EL1PCTEN | CNTHCTL_EL1PCEN)
 263         msr     cnthctl_el2, x2
 264
 265         /* Set the counter offset to a known value */
 266         msr     cntvoff_el2, xzr
 267
 268         /* Hypervisor trap functions */
 269         adr     x2, hyp_vectors
 270         msr     vbar_el2, x2
 271
 272         mov     x2, #(PSR_F | PSR_I | PSR_A | PSR_D | PSR_M_EL1h)
 273         msr     spsr_el2, x2
 274
 275         /* Configure GICv3 CPU interface */
 276         mrs     x2, id_aa64pfr0_el1
 277         /* Extract GIC bits from the register */
 278         ubfx    x2, x2, #ID_AA64PFR0_GIC_SHIFT, #ID_AA64PFR0_GIC_BITS
 279         /* GIC[3:0] == 0001 - GIC CPU interface via special regs. supported */
 280         cmp     x2, #(ID_AA64PFR0_GIC_CPUIF_EN >> ID_AA64PFR0_GIC_SHIFT)
 281         b.ne    2f
 282
 283         mrs     x2, icc_sre_el2
 284         orr     x2, x2, #ICC_SRE_EL2_EN /* Enable access from insecure EL1 */
 285         orr     x2, x2, #ICC_SRE_EL2_SRE        /* Enable system registers */
 286         msr     icc_sre_el2, x2
 287 2:
 288
 289         /* Set the address to return to our return address */
 290         msr     elr_el2, x30
 291         isb
 292
 293         eret
 294
 295         .align 3
 296 .Lsctlr_res1:
 297         .quad SCTLR_RES1
 298
 299 #define VECT_EMPTY      \
 300         .align 7;       \
 301         1:      b       1b
 302
 303         .align 11
 304 hyp_vectors:
 305         VECT_EMPTY      /* Synchronous EL2t */
 306         VECT_EMPTY      /* IRQ EL2t */
 307         VECT_EMPTY      /* FIQ EL2t */
 308         VECT_EMPTY      /* Error EL2t */
 309
 310         VECT_EMPTY      /* Synchronous EL2h */
 311         VECT_EMPTY      /* IRQ EL2h */
 312         VECT_EMPTY      /* FIQ EL2h */
 313         VECT_EMPTY      /* Error EL2h */
 314
 315         VECT_EMPTY      /* Synchronous 64-bit EL1 */
 316         VECT_EMPTY      /* IRQ 64-bit EL1 */
 317         VECT_EMPTY      /* FIQ 64-bit EL1 */
 318         VECT_EMPTY      /* Error 64-bit EL1 */
 319
 320         VECT_EMPTY      /* Synchronous 32-bit EL1 */
 321         VECT_EMPTY      /* IRQ 32-bit EL1 */
 322         VECT_EMPTY      /* FIQ 32-bit EL1 */
 323         VECT_EMPTY      /* Error 32-bit EL1 */
 324
 325 /*
 326  * Get the delta between the physical address we were loaded to and the
 327  * virtual address we expect to run from. This is used when building the
 328  * initial page table.
 329  */
 330 get_virt_delta:
 331         /* Load the physical address of virt_map */
 332         adr     x29, virt_map
 333         /* Load the virtual address of virt_map stored in virt_map */
 334         ldr     x28, [x29]
 335         /* Find PA - VA as PA' = VA' - VA + PA = VA' + (PA - VA) = VA' + x29 */
 336         sub     x29, x29, x28
 337         /* Find the load address for the kernel */
 338         mov     x28, #(KERNBASE)
 339         add     x28, x28, x29
 340         ret
 341
 342         .align 3
 343 virt_map:
 344         .quad   virt_map
 345
 346 /*
 347  * This builds the page tables containing the identity map, and the kernel
 348  * virtual map.
 349  *
 350  * It relys on:
 351  *  We were loaded to an address that is on a 2MiB boundary
 352  *  All the memory must not cross a 1GiB boundaty
 353  *  x28 contains the physical address we were loaded from
 354  *
 355  * TODO: This is out of date.
 356  *  There are at least 5 pages before that address for the page tables
 357  *   The pages used are:
 358  *    - The Kernel L2 table
 359  *    - The Kernel L1 table
 360  *    - The Kernel L0 table             (TTBR1)
 361  *    - The identity (PA = VA) L1 table
 362  *    - The identity (PA = VA) L0 table (TTBR0)
 363  *    - The DMAP L1 tables
 364  */
 365 create_pagetables:
 366         /* Save the Link register */
 367         mov     x5, x30
 368
 369         /* Clean the page table */
 370         adr     x6, pagetable
 371         mov     x26, x6
 372         adr     x27, pagetable_end
 373 1:
 374         stp     xzr, xzr, [x6], #16
 375         stp     xzr, xzr, [x6], #16
 376         stp     xzr, xzr, [x6], #16
 377         stp     xzr, xzr, [x6], #16
 378         cmp     x6, x27
 379         b.lo    1b
 380
 381         /*
 382          * Build the TTBR1 maps.
 383          */
 384
 385         /* Find the size of the kernel */
 386         mov     x6, #(KERNBASE)
 387
 388 #if defined(LINUX_BOOT_ABI)
 389         /* X19 is used as 'map FDT data' flag */
 390         mov     x19, xzr
 391
 392         /* No modules or FDT pointer ? */
 393         cbz     x0, booti_no_fdt
 394
 395         /* Test if modulep points to modules descriptor or to FDT */
 396         ldr     w8, [x0]
 397         ldr     w7, =FDT_MAGIC
 398         cmp     w7, w8
 399         b.eq    booti_fdt
 400 #endif
 401
 402         /* Booted with modules pointer */
 403         /* Find modulep - begin */
 404         sub     x8, x0, x6
 405         /* Add two 2MiB pages for the module data and round up */
 406         ldr     x7, =(3 * L2_SIZE - 1)
 407         add     x8, x8, x7
 408         b       common
 409
 410 #if defined(LINUX_BOOT_ABI)
 411 booti_fdt:
 412         /* Booted by U-Boot booti with FDT data */
 413         /* Set 'map FDT data' flag */
 414         mov     x19, #1
 415
 416 booti_no_fdt:
 417         /* Booted by U-Boot booti without FTD data */
 418         /* Find the end - begin */
 419         ldr     x7, .Lend
 420         sub     x8, x7, x6
 421
 422         /*
 423          * Add one 2MiB page for copy of FDT data (maximum FDT size),
 424          * one for metadata and round up
 425          */
 426         ldr     x7, =(3 * L2_SIZE - 1)
 427         add     x8, x8, x7
 428 #endif
 429
 430 common:
 431         /* Get the number of l2 pages to allocate, rounded down */
 432         lsr     x10, x8, #(L2_SHIFT)
 433
 434         /* Create the kernel space L2 table */
 435         mov     x6, x26
 436         mov     x7, #VM_MEMATTR_WRITE_BACK
 437         mov     x8, #(KERNBASE & L2_BLOCK_MASK)
 438         mov     x9, x28
 439         bl      build_l2_block_pagetable
 440
 441         /* Move to the l1 table */
 442         add     x26, x26, #PAGE_SIZE
 443
 444         /* Link the l1 -> l2 table */
 445         mov     x9, x6
 446         mov     x6, x26
 447         bl      link_l1_pagetable
 448
 449         /* Move to the l0 table */
 450         add     x24, x26, #PAGE_SIZE
 451
 452         /* Link the l0 -> l1 table */
 453         mov     x9, x6
 454         mov     x6, x24
 455         mov     x10, #1
 456         bl      link_l0_pagetable
 457
 458         /* Link the DMAP tables */
 459         ldr     x8, =DMAP_MIN_ADDRESS
 460         adr     x9, pagetable_dmap;
 461         mov     x10, #DMAP_TABLES
 462         bl      link_l0_pagetable
 463
 464         /*
 465          * Build the TTBR0 maps.  As TTBR0 maps, they must specify ATTR_S1_nG.
 466          * They are only needed early on, so the VA = PA map is uncached.
 467          */
 468         add     x27, x24, #PAGE_SIZE
 469
 470         mov     x6, x27         /* The initial page table */
 471 #if defined(SOCDEV_PA) && defined(SOCDEV_VA)
 472         /* Create a table for the UART */
 473         mov     x7, #(ATTR_S1_nG | ATTR_S1_IDX(VM_MEMATTR_DEVICE))
 474         mov     x8, #(SOCDEV_VA)        /* VA start */
 475         mov     x9, #(SOCDEV_PA)        /* PA start */
 476         mov     x10, #1
 477         bl      build_l1_block_pagetable
 478 #endif
 479
 480 #if defined(LINUX_BOOT_ABI)
 481         /* Map FDT data ? */
 482         cbz     x19, 1f
 483
 484         /* Create the identity mapping for FDT data (2 MiB max) */
 485         mov     x7, #(ATTR_S1_nG | ATTR_S1_IDX(VM_MEMATTR_UNCACHEABLE))
 486         mov     x9, x0
 487         mov     x8, x0          /* VA start (== PA start) */
 488         mov     x10, #1
 489         bl      build_l1_block_pagetable
 490
 491 1:
 492 #endif
 493
 494         /* Create the VA = PA map */
 495         mov     x7, #(ATTR_S1_nG | ATTR_S1_IDX(VM_MEMATTR_UNCACHEABLE))
 496         mov     x9, x27
 497         mov     x8, x9          /* VA start (== PA start) */
 498         mov     x10, #1
 499         bl      build_l1_block_pagetable
 500
 501         /* Move to the l0 table */
 502         add     x27, x27, #PAGE_SIZE
 503
 504         /* Link the l0 -> l1 table */
 505         mov     x9, x6
 506         mov     x6, x27
 507         mov     x10, #1
 508         bl      link_l0_pagetable
 509
 510         /* Restore the Link register */
 511         mov     x30, x5
 512         ret
 513
 514 /*
 515  * Builds an L0 -> L1 table descriptor
 516  *
 517  * This is a link for a 512GiB block of memory with up to 1GiB regions mapped
 518  * within it by build_l1_block_pagetable.
 519  *
 520  *  x6  = L0 table
 521  *  x8  = Virtual Address
 522  *  x9  = L1 PA (trashed)
 523  *  x10 = Entry count
 524  *  x11, x12 and x13 are trashed
 525  */
 526 link_l0_pagetable:
 527         /*
 528          * Link an L0 -> L1 table entry.
 529          */
 530         /* Find the table index */
 531         lsr     x11, x8, #L0_SHIFT
 532         and     x11, x11, #L0_ADDR_MASK
 533
 534         /* Build the L0 block entry */
 535         mov     x12, #L0_TABLE
 536
 537         /* Only use the output address bits */
 538         lsr     x9, x9, #PAGE_SHIFT
 539 1:      orr     x13, x12, x9, lsl #PAGE_SHIFT
 540
 541         /* Store the entry */
 542         str     x13, [x6, x11, lsl #3]
 543
 544         sub     x10, x10, #1
 545         add     x11, x11, #1
 546         add     x9, x9, #1
 547         cbnz    x10, 1b
 548
 549         ret
 550
 551 /*
 552  * Builds an L1 -> L2 table descriptor
 553  *
 554  * This is a link for a 1GiB block of memory with up to 2MiB regions mapped
 555  * within it by build_l2_block_pagetable.
 556  *
 557  *  x6  = L1 table
 558  *  x8  = Virtual Address
 559  *  x9  = L2 PA (trashed)
 560  *  x11, x12 and x13 are trashed
 561  */
 562 link_l1_pagetable:
 563         /*
 564          * Link an L1 -> L2 table entry.
 565          */
 566         /* Find the table index */
 567         lsr     x11, x8, #L1_SHIFT
 568         and     x11, x11, #Ln_ADDR_MASK
 569
 570         /* Build the L1 block entry */
 571         mov     x12, #L1_TABLE
 572
 573         /* Only use the output address bits */
 574         lsr     x9, x9, #PAGE_SHIFT
 575         orr     x13, x12, x9, lsl #PAGE_SHIFT
 576
 577         /* Store the entry */
 578         str     x13, [x6, x11, lsl #3]
 579
 580         ret
 581
 582 /*
 583  * Builds count 1 GiB page table entry
 584  *  x6  = L1 table
 585  *  x7  = Variable lower block attributes
 586  *  x8  = VA start
 587  *  x9  = PA start (trashed)
 588  *  x10 = Entry count
 589  *  x11, x12 and x13 are trashed
 590  */
 591 build_l1_block_pagetable:
 592         /*
 593          * Build the L1 table entry.
 594          */
 595         /* Find the table index */
 596         lsr     x11, x8, #L1_SHIFT
 597         and     x11, x11, #Ln_ADDR_MASK
 598
 599         /* Build the L1 block entry */
 600         orr     x12, x7, #L1_BLOCK
 601         orr     x12, x12, #(ATTR_AF)
 602 #ifdef SMP
 603         orr     x12, x12, ATTR_SH(ATTR_SH_IS)
 604 #endif
 605
 606         /* Only use the output address bits */
 607         lsr     x9, x9, #L1_SHIFT
 608
 609         /* Set the physical address for this virtual address */
 610 1:      orr     x13, x12, x9, lsl #L1_SHIFT
 611
 612         /* Store the entry */
 613         str     x13, [x6, x11, lsl #3]
 614
 615         sub     x10, x10, #1
 616         add     x11, x11, #1
 617         add     x9, x9, #1
 618         cbnz    x10, 1b
 619
 620         ret
 621
 622 /*
 623  * Builds count 2 MiB page table entry
 624  *  x6  = L2 table
 625  *  x7  = Type (0 = Device, 1 = Normal)
 626  *  x8  = VA start
 627  *  x9  = PA start (trashed)
 628  *  x10 = Entry count
 629  *  x11, x12 and x13 are trashed
 630  */
 631 build_l2_block_pagetable:
 632         /*
 633          * Build the L2 table entry.
 634          */
 635         /* Find the table index */
 636         lsr     x11, x8, #L2_SHIFT
 637         and     x11, x11, #Ln_ADDR_MASK
 638
 639         /* Build the L2 block entry */
 640         lsl     x12, x7, #2
 641         orr     x12, x12, #L2_BLOCK
 642         orr     x12, x12, #(ATTR_AF)
 643         orr     x12, x12, #(ATTR_S1_UXN)
 644 #ifdef SMP
 645         orr     x12, x12, ATTR_SH(ATTR_SH_IS)
 646 #endif
 647
 648         /* Only use the output address bits */
 649         lsr     x9, x9, #L2_SHIFT
 650
 651         /* Set the physical address for this virtual address */
 652 1:      orr     x13, x12, x9, lsl #L2_SHIFT
 653
 654         /* Store the entry */
 655         str     x13, [x6, x11, lsl #3]
 656
 657         sub     x10, x10, #1
 658         add     x11, x11, #1
 659         add     x9, x9, #1
 660         cbnz    x10, 1b
 661
 662         ret
 663
 664 start_mmu:
 665         dsb     sy
 666
 667         /* Load the exception vectors */
 668         ldr     x2, =exception_vectors
 669         msr     vbar_el1, x2
 670
 671         /* Load ttbr0 and ttbr1 */
 672         msr     ttbr0_el1, x27
 673         msr     ttbr1_el1, x24
 674         isb
 675
 676         /* Clear the Monitor Debug System control register */
 677         msr     mdscr_el1, xzr
 678
 679         /* Invalidate the TLB */
 680         tlbi    vmalle1is
 681
 682         ldr     x2, mair
 683         msr     mair_el1, x2
 684
 685         /*
 686          * Setup TCR according to the PARange and ASIDBits fields
 687          * from ID_AA64MMFR0_EL1 and the HAFDBS field from the
 688          * ID_AA64MMFR1_EL1.  More precisely, set TCR_EL1.AS
 689          * to 1 only if the ASIDBits field equals 0b0010.
 690          */
 691         ldr     x2, tcr
 692         mrs     x3, id_aa64mmfr0_el1
 693
 694         /* Copy the bottom 3 bits from id_aa64mmfr0_el1 into TCR.IPS */
 695         bfi     x2, x3, #(TCR_IPS_SHIFT), #(TCR_IPS_WIDTH)
 696         and     x3, x3, #(ID_AA64MMFR0_ASIDBits_MASK)
 697
 698         /* Check if the HW supports 16 bit ASIDS */
 699         cmp     x3, #(ID_AA64MMFR0_ASIDBits_16)
 700         /* If so x3 == 1, else x3 == 0 */
 701         cset    x3, eq
 702         /* Set TCR.AS with x3 */
 703         bfi     x2, x3, #(TCR_ASID_SHIFT), #(TCR_ASID_WIDTH)
 704
 705         /*
 706          * Check if the HW supports access flag and dirty state updates,
 707          * and set TCR_EL1.HA and TCR_EL1.HD accordingly.
 708          */
 709         mrs     x3, id_aa64mmfr1_el1
 710         and     x3, x3, #(ID_AA64MMFR1_HAFDBS_MASK)
 711         cmp     x3, #1
 712         b.ne    1f
 713         orr     x2, x2, #(TCR_HA)
 714         b       2f
 715 1:
 716         cmp     x3, #2
 717         b.ne    2f
 718         orr     x2, x2, #(TCR_HA | TCR_HD)
 719 2:
 720         msr     tcr_el1, x2
 721
 722         /*
 723          * Setup SCTLR.
 724          */
 725         ldr     x2, sctlr_set
 726         ldr     x3, sctlr_clear
 727         mrs     x1, sctlr_el1
 728         bic     x1, x1, x3      /* Clear the required bits */
 729         orr     x1, x1, x2      /* Set the required bits */
 730         msr     sctlr_el1, x1
 731         isb
 732
 733         ret
 734
 735         .align 3
 736 mair:
 737         .quad   MAIR_ATTR(MAIR_DEVICE_nGnRnE, VM_MEMATTR_DEVICE)    |   \
 738                 MAIR_ATTR(MAIR_NORMAL_NC, VM_MEMATTR_UNCACHEABLE)   |   \
 739                 MAIR_ATTR(MAIR_NORMAL_WB, VM_MEMATTR_WRITE_BACK)    |   \
 740                 MAIR_ATTR(MAIR_NORMAL_WT, VM_MEMATTR_WRITE_THROUGH)
 741 tcr:
 742         .quad (TCR_TxSZ(64 - VIRT_BITS) | TCR_TG1_4K | \
 743             TCR_CACHE_ATTRS | TCR_SMP_ATTRS)
 744 sctlr_set:
 745         /* Bits to set */
 746         .quad (SCTLR_LSMAOE | SCTLR_nTLSMD | SCTLR_UCI | SCTLR_SPAN | \
 747             SCTLR_nTWE | SCTLR_nTWI | SCTLR_UCT | SCTLR_DZE | \
 748             SCTLR_I | SCTLR_SED | SCTLR_SA0 | SCTLR_SA | SCTLR_C | \
 749             SCTLR_M | SCTLR_CP15BEN)
 750 sctlr_clear:
 751         /* Bits to clear */
 752         .quad (SCTLR_EE | SCTLR_EOE | SCTLR_IESB | SCTLR_WXN | SCTLR_UMA | \
 753             SCTLR_ITD | SCTLR_A)
 754
 755         .globl abort
 756 abort:
 757         b abort
 758
 759         //.section .init_pagetable
 760         .align 12 /* 4KiB aligned */
 761         /*
 762          * 3 initial tables (in the following order):
 763          *           L2 for kernel (High addresses)
 764          *           L1 for kernel
 765          *           L1 for user   (Low addresses)
 766          */
 767 pagetable:
 768         .space  PAGE_SIZE
 769 pagetable_l1_ttbr1:
 770         .space  PAGE_SIZE
 771 pagetable_l0_ttbr1:
 772         .space  PAGE_SIZE
 773 pagetable_l1_ttbr0:
 774         .space  PAGE_SIZE
 775 pagetable_l0_ttbr0:
 776         .space  PAGE_SIZE
 777
 778         .globl pagetable_dmap
 779 pagetable_dmap:
 780         .space  PAGE_SIZE * DMAP_TABLES
 781 pagetable_end:
 782
 783 el2_pagetable:
 784         .space  PAGE_SIZE
 785
 786         .globl init_pt_va
 787 init_pt_va:
 788         .quad pagetable         /* XXX: Keep page tables VA */
 789
 790         .align  4
 791 initstack:
 792         .space  (PAGE_SIZE * KSTACK_PAGES)
 793 initstack_end:
 794
 795
 796 ENTRY(sigcode)
 797         mov     x0, sp
 798         add     x0, x0, #SF_UC
 799
 800 1:
 801         mov     x8, #SYS_sigreturn
 802         svc     0
 803
 804         /* sigreturn failed, exit */
 805         mov     x8, #SYS_exit
 806         svc     0
 807
 808         b       1b
 809 END(sigcode)
 810         /* This may be copied to the stack, keep it 16-byte aligned */
 811         .align  3
 812 esigcode:
 813
 814         .data
 815         .align  3
 816         .global szsigcode
 817 szsigcode:
 818         .quad   esigcode - sigcode
 819
 820 ENTRY(aarch32_sigcode)
 821         .word 0xe1a0000d        // mov r0, sp
 822         .word 0xe2800040        // add r0, r0, #SIGF_UC
 823         .word 0xe59f700c        // ldr r7, [pc, #12]
 824         .word 0xef000000        // swi #0
 825         .word 0xe59f7008        // ldr r7, [pc, #8]
 826         .word 0xef000000        // swi #0
 827         .word 0xeafffffa        // b . - 16
 828 END(aarch32_sigcode)
 829         .word SYS_sigreturn
 830         .word SYS_exit
 831         .align  3
 832 aarch32_esigcode:
 833         .data
 834         .global sz_aarch32_sigcode
 835 sz_aarch32_sigcode:
 836         .quad aarch32_esigcode - aarch32_sigcode