sys/src/9/bcm64/l.s

   1 #include "mem.h"
   2 #include "sysreg.h"
   3
   4 #undef  SYSREG
   5 #define SYSREG(op0,op1,Cn,Cm,op2)       SPR(((op0)<<19|(op1)<<16|(Cn)<<12|(Cm)<<8|(op2)<<5))
   6
   7 TEXT _start(SB), 1, $-4
   8         MOV     $setSB-KZERO(SB), R28
   9         BL      svcmode<>(SB)
  10
  11         /* use dedicated stack pointer per exception level */
  12         MOVWU   $1, R1
  13         MSR     R1, SPSel
  14
  15         BL      mmudisable<>(SB)
  16
  17         /* invalidate local caches */
  18         BL      cachedinv(SB)
  19         BL      cacheiinv(SB)
  20
  21         MOV     $(MACHADDR(0)-KZERO), R27
  22         MRS     MPIDR_EL1, R1
  23         ANDW    $(MAXMACH-1), R1
  24         MOVWU   $MACHSIZE, R2
  25         MULW    R1, R2, R2
  26         SUB     R2, R27
  27
  28         ADD     $(MACHSIZE-16), R27, R2
  29         MOV     R2, SP
  30
  31         CBNZ    R1, _startup
  32
  33         /* clear page table and machs */
  34         MOV     $(L1-KZERO), R1
  35         MOV     $(MACHADDR(-1)-KZERO), R2
  36 _zerol1:
  37         MOV     ZR, (R1)8!
  38         CMP     R1, R2
  39         BNE     _zerol1
  40
  41         /* clear BSS */
  42         MOV     $edata-KZERO(SB), R1
  43         MOV     $end-KZERO(SB), R2
  44 _zerobss:
  45         MOV     ZR, (R1)8!
  46         CMP     R1, R2
  47         BNE     _zerobss
  48
  49         /* setup page tables */
  50         MOV     $(L1-KZERO), R0
  51         BL      mmu0init(SB)
  52
  53         BL      cachedwbinv(SB)
  54         BL      l2cacheuwbinv(SB)
  55         SEVL
  56 _startup:
  57         WFE
  58         BL      mmuenable<>(SB)
  59
  60         MOV     $0, R26
  61         ORR     $KZERO, R27
  62         MSR     R27, TPIDR_EL1
  63         MOV     $setSB(SB), R28
  64
  65         BL      main(SB)
  66
  67 TEXT    stop<>(SB), 1, $-4
  68 _stop:
  69         WFE
  70         B       _stop
  71
  72 TEXT sev(SB), 1, $-4
  73         SEV
  74         WFE
  75         RETURN
  76
  77 TEXT PUTC(SB), 1, $-4
  78         MOVWU $(0x3F000000+0x215040), R14
  79         MOVB R0, (R14)
  80         RETURN
  81
  82 TEXT svcmode<>(SB), 1, $-4
  83         MSR     $0xF, DAIFSet
  84         MRS     CurrentEL, R0
  85         ANDW    $(3<<2), R0
  86         CMPW    $(1<<2), R0
  87         BEQ     el1
  88         CMPW    $(2<<2), R0
  89         BEQ     el2
  90         B       stop<>(SB)
  91 el2:
  92         MOV     $0, R0
  93         MSR     R0, MDCR_EL2
  94         ISB     $SY
  95
  96         /* HCR = RW, HCD, SWIO, BSU, FB */
  97         MOVWU   $(1<<31 | 1<<29 | 1<<2 | 0<<10 | 0<<9), R0
  98         MSR     R0, HCR_EL2
  99         ISB     $SY
 100
 101         /* SCTLR = RES1 */
 102         MOVWU   $(3<<4 | 1<<11 | 1<<16 | 1<<18 | 3<<22 | 3<<28), R0
 103         ISB     $SY
 104         MSR     R0, SCTLR_EL2
 105         ISB     $SY
 106
 107         /* set VMID to zero */
 108         MOV     $0, R0
 109         MSR     R0, VTTBR_EL2
 110         ISB     $SY
 111
 112         MOVWU   $(0xF<<6 | 4), R0
 113         MSR     R0, SPSR_EL2
 114         MSR     LR, ELR_EL2
 115         ERET
 116 el1:
 117         RETURN
 118
 119 TEXT mmudisable<>(SB), 1, $-4
 120 #define SCTLRCLR \
 121         /* RES0 */      ( 3<<30 \
 122         /* RES0 */      | 1<<27 \
 123         /* UCI */       | 1<<26 \
 124         /* EE */        | 1<<25 \
 125         /* RES0 */      | 1<<21 \
 126         /* E0E */       | 1<<24 \
 127         /* WXN */       | 1<<19 \
 128         /* nTWE */      | 1<<18 \
 129         /* RES0 */      | 1<<17 \
 130         /* nTWI */      | 1<<16 \
 131         /* UCT */       | 1<<15 \
 132         /* DZE */       | 1<<14 \
 133         /* RES0 */      | 1<<13 \
 134         /* RES0 */      | 1<<10 \
 135         /* UMA */       | 1<<9 \
 136         /* SA0 */       | 1<<4 \
 137         /* SA */        | 1<<3 \
 138         /* A */         | 1<<1 )
 139 #define SCTLRSET \
 140         /* RES1 */      ( 3<<28 \
 141         /* RES1 */      | 3<<22 \
 142         /* RES1 */      | 1<<20 \
 143         /* RES1 */      | 1<<11 )
 144 #define SCTLRMMU \
 145         /* I */         ( 1<<12 \
 146         /* C */         | 1<<2 \
 147         /* M */         | 1<<0 )
 148
 149         /* initialise SCTLR, MMU and caches off */
 150         ISB     $SY
 151         MRS     SCTLR_EL1, R0
 152         BIC     $(SCTLRCLR | SCTLRMMU), R0
 153         ORR     $SCTLRSET, R0
 154         ISB     $SY
 155         MSR     R0, SCTLR_EL1
 156         ISB     $SY
 157
 158         B       flushlocaltlb(SB)
 159
 160 TEXT mmuenable<>(SB), 1, $-4
 161         /* return to virtual */
 162         ORR     $KZERO, LR
 163         MOV     LR, -16(RSP)!
 164
 165         BL      cachedwbinv(SB)
 166         BL      flushlocaltlb(SB)
 167
 168         /* memory attributes */
 169 #define MAIRINIT \
 170         ( 0xFF << MA_MEM_WB*8 \
 171         | 0x33 << MA_MEM_WT*8 \
 172         | 0x44 << MA_MEM_UC*8 \
 173         | 0x00 << MA_DEV_nGnRnE*8 \
 174         | 0x04 << MA_DEV_nGnRE*8 \
 175         | 0x08 << MA_DEV_nGRE*8 \
 176         | 0x0C << MA_DEV_GRE*8 )
 177         MOV     $MAIRINIT, R1
 178         MSR     R1, MAIR_EL1
 179         ISB     $SY
 180
 181         /* translation control */
 182 #define TCRINIT \
 183         /* TBI1 */      ( 0<<38 \
 184         /* TBI0 */      | 0<<37 \
 185         /* AS */        | 0<<36 \
 186         /* TG1 */       | (((3<<16|1<<14|2<<12)>>PGSHIFT)&3)<<30 \
 187         /* SH1 */       | SHARE_INNER<<28 \
 188         /* ORGN1 */     | CACHE_WB<<26 \
 189         /* IRGN1 */     | CACHE_WB<<24 \
 190         /* EPD1 */      | 0<<23 \
 191         /* A1 */        | 0<<22 \
 192         /* T1SZ */      | (64-EVASHIFT)<<16 \
 193         /* TG0 */       | (((1<<16|2<<14|0<<12)>>PGSHIFT)&3)<<14 \
 194         /* SH0 */       | SHARE_INNER<<12 \
 195         /* ORGN0 */     | CACHE_WB<<10 \
 196         /* IRGN0 */     | CACHE_WB<<8 \
 197         /* EPD0 */      | 0<<7 \
 198         /* T0SZ */      | (64-EVASHIFT)<<0 )
 199         MOV     $TCRINIT, R1
 200         MRS     ID_AA64MMFR0_EL1, R2
 201         ANDW    $0xF, R2        // IPS
 202         ADD     R2<<32, R1
 203         MSR     R1, TCR_EL1
 204         ISB     $SY
 205
 206         /* load the page tables */
 207         MOV     $(L1TOP-KZERO), R0
 208         ISB     $SY
 209         MSR     R0, TTBR0_EL1
 210         MSR     R0, TTBR1_EL1
 211         ISB     $SY
 212
 213         /* enable MMU and caches */
 214         MRS     SCTLR_EL1, R1
 215         ORR     $SCTLRMMU, R1
 216         ISB     $SY
 217         MSR     R1, SCTLR_EL1
 218         ISB     $SY
 219
 220         MOV     RSP, R1
 221         ORR     $KZERO, R1
 222         MOV     R1, RSP
 223         MOV     (RSP)16!, LR
 224         B       cacheiinv(SB)
 225
 226 TEXT touser(SB), 1, $-4
 227         MSR     $0x3, DAIFSet   // interrupts off
 228         MOVWU   $0x10028, R1    // entry
 229         MOVWU   $0, R2          // psr
 230         MSR     R0, SP_EL0      // sp
 231         MSR     R1, ELR_EL1
 232         MSR     R2, SPSR_EL1
 233         ERET
 234
 235 TEXT cas(SB), 1, $-4
 236 TEXT cmpswap(SB), 1, $-4
 237         MOVW    ov+8(FP), R1
 238         MOVW    nv+16(FP), R2
 239 _cas1:
 240         LDXRW   (R0), R3
 241         CMP     R3, R1
 242         BNE     _cas0
 243         STXRW   R2, (R0), R4
 244         CBNZ    R4, _cas1
 245         MOVW    $1, R0
 246         DMB     $ISH
 247         RETURN
 248 _cas0:
 249         CLREX
 250         MOVW    $0, R0
 251         RETURN
 252
 253 TEXT tas(SB), 1, $-4
 254 TEXT _tas(SB), 1, $-4
 255         MOVW    $0xdeaddead, R2
 256 _tas1:
 257         LDXRW   (R0), R1
 258         STXRW   R2, (R0), R3
 259         CBNZ    R3, _tas1
 260         MOVW    R1, R0
 261
 262 TEXT coherence(SB), 1, $-4
 263         DMB     $ISH
 264         RETURN
 265
 266 TEXT islo(SB), 1, $-4
 267         MRS     DAIF, R0
 268         AND     $(0x2<<6), R0
 269         EOR     $(0x2<<6), R0
 270         RETURN
 271
 272 TEXT splhi(SB), 1, $-4
 273         MRS     DAIF, R0
 274         MSR     $0x2, DAIFSet
 275         RETURN
 276
 277 TEXT splfhi(SB), 1, $-4
 278         MRS     DAIF, R0
 279         MSR     $0x3, DAIFSet
 280         RETURN
 281
 282 TEXT spllo(SB), 1, $-4
 283         MSR     $0x3, DAIFClr
 284         RETURN
 285
 286 TEXT splflo(SB), 1, $-4
 287         MSR     $0x1, DAIFClr
 288         RETURN
 289
 290 TEXT splx(SB), 1, $-4
 291         MSR     R0, DAIF
 292         RETURN
 293
 294 TEXT cycles(SB), 1, $-4
 295 TEXT lcycles(SB), 1, $-4
 296         MRS     PMCCNTR_EL0, R0
 297         RETURN
 298
 299 TEXT setlabel(SB), 1, $-4
 300         MOV     LR, 8(R0)
 301         MOV     SP, R1
 302         MOV     R1, 0(R0)
 303         MOVW    $0, R0
 304         RETURN
 305
 306 TEXT gotolabel(SB), 1, $-4
 307         MOV     8(R0), LR       /* link */
 308         MOV     0(R0), R1       /* sp */
 309         MOV     R1, SP
 310         MOVW    $1, R0
 311         RETURN
 312
 313 TEXT returnto(SB), 1, $-4
 314         MOV     R0, 0(SP)
 315         RETURN
 316
 317 TEXT getfar(SB), 1, $-4
 318         MRS     FAR_EL1, R0
 319         RETURN
 320
 321 TEXT setttbr(SB), 1, $-4
 322         DSB     $ISHST
 323         MSR     R0, TTBR0_EL1
 324         DSB     $ISH
 325         ISB     $SY
 326
 327         B       cacheiinv(SB)
 328
 329 TEXT magic(SB), 1, $-4
 330         DSB     $SY
 331         ISB     $SY
 332         DSB     $SY
 333         ISB     $SY
 334         DSB     $SY
 335         ISB     $SY
 336         DSB     $SY
 337         ISB     $SY
 338         RETURN
 339
 340 /*
 341  * TLB maintenance operations.
 342  * these broadcast to all cpu's in the cluser
 343  * (inner sharable domain).
 344  */
 345 TEXT flushasidva(SB), 1, $-4
 346 TEXT tlbivae1is(SB), 1, $-4
 347         DSB     $ISHST
 348         TLBI    R0, 0,8,3,1     /* VAE1IS */
 349         DSB     $ISH
 350         ISB     $SY
 351         RETURN
 352
 353 TEXT flushasidvall(SB), 1, $-4
 354 TEXT tlbivale1is(SB), 1, $-4
 355         DSB     $ISHST
 356         TLBI    R0, 0,8,3,5     /* VALE1IS */
 357         DSB     $ISH
 358         ISB     $SY
 359         RETURN
 360
 361 TEXT flushasid(SB), 1, $-4
 362 TEXT tlbiaside1is(SB), 1, $-4
 363         DSB     $ISHST
 364         TLBI    R0, 0,8,3,2     /* ASIDE1IS */
 365         DSB     $ISH
 366         ISB     $SY
 367         RETURN
 368
 369 TEXT flushtlb(SB), 1, $-4
 370 TEXT tlbivmalle1is(SB), 1, $-4
 371         DSB     $ISHST
 372         TLBI    R0, 0,8,3,0     /* VMALLE1IS */
 373         DSB     $ISH
 374         ISB     $SY
 375         RETURN
 376
 377 /*
 378  * flush the tlb of this cpu. no broadcast.
 379  */
 380 TEXT flushlocaltlb(SB), 1, $-4
 381 TEXT tlbivmalle1(SB), 1, $-4
 382         DSB     $NSHST
 383         TLBI    R0, 0,8,7,0     /* VMALLE1 */
 384         DSB     $NSH
 385         ISB     $SY
 386         RETURN
 387
 388 TEXT fpsaveregs(SB), 1, $-4
 389         WORD    $(1<<30 | 3 << 26 | 2<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 0)  /* MOV { V0, V1, V2, V3  }, (R0)64! */
 390         WORD    $(1<<30 | 3 << 26 | 2<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 4)  /* MOV { V4, V5, V6, V7  }, (R0)64! */
 391         WORD    $(1<<30 | 3 << 26 | 2<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 8)  /* MOV { V8, V9, V10,V11 }, (R0)64! */
 392         WORD    $(1<<30 | 3 << 26 | 2<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 12) /* MOV { V12,V13,V14,V15 }, (R0)64! */
 393         WORD    $(1<<30 | 3 << 26 | 2<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 16) /* MOV { V16,V17,V18,V19 }, (R0)64! */
 394         WORD    $(1<<30 | 3 << 26 | 2<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 20) /* MOV { V20,V21,V22,V23 }, (R0)64! */
 395         WORD    $(1<<30 | 3 << 26 | 2<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 24) /* MOV { V24,V25,V26,V27 }, (R0)64! */
 396         WORD    $(1<<30 | 3 << 26 | 2<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 28) /* MOV { V28,V29,V30,V31 }, (R0)64! */
 397         RETURN
 398
 399 TEXT fploadregs(SB), 1, $-4
 400         WORD    $(1<<30 | 3 << 26 | 3<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 0)  /* MOV (R0)64!, { V0, V1, V2, V3  } */
 401         WORD    $(1<<30 | 3 << 26 | 3<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 4)  /* MOV (R0)64!, { V4, V5, V6, V7  } */
 402         WORD    $(1<<30 | 3 << 26 | 3<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 8)  /* MOV (R0)64!, { V8, V9, V10,V11 } */
 403         WORD    $(1<<30 | 3 << 26 | 3<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 12) /* MOV (R0)64!, { V12,V13,V14,V15 } */
 404         WORD    $(1<<30 | 3 << 26 | 3<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 16) /* MOV (R0)64!, { V16,V17,V18,V19 } */
 405         WORD    $(1<<30 | 3 << 26 | 3<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 20) /* MOV (R0)64!, { V20,V21,V22,V23 } */
 406         WORD    $(1<<30 | 3 << 26 | 3<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 24) /* MOV (R0)64!, { V24,V25,V26,V27 } */
 407         WORD    $(1<<30 | 3 << 26 | 3<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 28) /* MOV (R0)64!, { V28,V29,V30,V31 } */
 408         RETURN
 409
 410 // syscall or trap from EL0
 411 TEXT vsys0(SB), 1, $-4
 412         LSRW    $26, R0, R17    // ec
 413         CMPW    $0x15, R17      // SVC trap?
 414         BNE     _itsatrap       // nope.
 415
 416         MOV     R26, 224(RSP)   // special
 417         MOV     R27, 232(RSP)   // special
 418         MOV     R28, 240(RSP)   // sb
 419         MOV     R29, 248(RSP)   // special
 420
 421         MRS     SP_EL0, R1
 422         MRS     ELR_EL1, R2
 423         MRS     SPSR_EL1, R3
 424
 425         MOV     R0, 288(RSP)    // type
 426         MOV     R1, 264(RSP)    // sp
 427         MOV     R2, 272(RSP)    // pc
 428         MOV     R3, 280(RSP)    // psr
 429
 430         MOV     $setSB(SB), R28
 431         MRS     TPIDR_EL1, R27
 432         MOV     16(R27), R26
 433
 434         ADD     $16, RSP, R0    // ureg
 435         BL      syscall(SB)
 436
 437 TEXT forkret(SB), 1, $-4
 438         MSR     $0x3, DAIFSet   // interrupts off
 439
 440         ADD     $16, RSP, R0    // ureg
 441
 442         MOV     16(RSP), R0     // ret
 443         MOV     264(RSP), R1    // sp
 444         MOV     272(RSP), R2    // pc
 445         MOV     280(RSP), R3    // psr
 446
 447         MSR     R1, SP_EL0
 448         MSR     R2, ELR_EL1
 449         MSR     R3, SPSR_EL1
 450
 451         MOV     224(RSP), R26   // special
 452         MOV     232(RSP), R27   // special
 453         MOV     240(RSP), R28   // sb
 454         MOV     248(RSP), R29   // special
 455
 456         MOV     256(RSP), R30   // link
 457
 458         ADD     $TRAPFRAMESIZE, RSP
 459         ERET
 460
 461 TEXT itsatrap<>(SB), 1, $-4
 462 _itsatrap:
 463         MOV     R1, 24(RSP)
 464         MOV     R2, 32(RSP)
 465         MOV     R3, 40(RSP)
 466         MOV     R4, 48(RSP)
 467         MOV     R5, 56(RSP)
 468         MOV     R6, 64(RSP)
 469         MOV     R7, 72(RSP)
 470         MOV     R8, 80(RSP)
 471         MOV     R9, 88(RSP)
 472         MOV     R10, 96(RSP)
 473         MOV     R11, 104(RSP)
 474         MOV     R12, 112(RSP)
 475         MOV     R13, 120(RSP)
 476         MOV     R14, 128(RSP)
 477         MOV     R15, 136(RSP)
 478         MOV     R16, 144(RSP)
 479
 480         MOV     R18, 160(RSP)
 481         MOV     R19, 168(RSP)
 482         MOV     R20, 176(RSP)
 483         MOV     R21, 184(RSP)
 484         MOV     R22, 192(RSP)
 485         MOV     R23, 200(RSP)
 486         MOV     R24, 208(RSP)
 487         MOV     R25, 216(RSP)
 488
 489 // trap/irq/fiq/serr from EL0
 490 TEXT vtrap0(SB), 1, $-4
 491         MOV     R26, 224(RSP)   // special
 492         MOV     R27, 232(RSP)   // special
 493         MOV     R28, 240(RSP)   // sb
 494         MOV     R29, 248(RSP)   // special
 495
 496         MRS     SP_EL0, R1
 497         MRS     ELR_EL1, R2
 498         MRS     SPSR_EL1, R3
 499
 500         MOV     R0, 288(RSP)    // type
 501         MOV     R1, 264(RSP)    // sp
 502         MOV     R2, 272(RSP)    // pc
 503         MOV     R3, 280(RSP)    // psr
 504
 505         MOV     $setSB(SB), R28
 506         MRS     TPIDR_EL1, R27
 507         MOV     16(R27), R26
 508
 509         ADD     $16, RSP, R0    // ureg
 510         BL      trap(SB)
 511
 512 TEXT noteret(SB), 1, $-4
 513         MSR     $0x3, DAIFSet   // interrupts off
 514
 515         ADD     $16, RSP, R0    // ureg
 516
 517         MOV     264(RSP), R1    // sp
 518         MOV     272(RSP), R2    // pc
 519         MOV     280(RSP), R3    // psr
 520
 521         MSR     R1, SP_EL0
 522         MSR     R2, ELR_EL1
 523         MSR     R3, SPSR_EL1
 524
 525         MOV     224(RSP), R26   // special
 526         MOV     232(RSP), R27   // special
 527         MOV     240(RSP), R28   // sb
 528         MOV     248(RSP), R29   // special
 529
 530 _intrreturn:
 531         MOV     16(RSP), R0
 532         MOV     24(RSP), R1
 533         MOV     32(RSP), R2
 534         MOV     40(RSP), R3
 535         MOV     48(RSP), R4
 536         MOV     56(RSP), R5
 537         MOV     64(RSP), R6
 538         MOV     72(RSP), R7
 539         MOV     80(RSP), R8
 540         MOV     88(RSP), R9
 541         MOV     96(RSP), R10
 542         MOV     104(RSP), R11
 543         MOV     112(RSP), R12
 544         MOV     120(RSP), R13
 545         MOV     128(RSP), R14
 546         MOV     136(RSP), R15
 547         MOV     144(RSP), R16
 548         MOV     152(RSP), R17
 549         MOV     160(RSP), R18
 550         MOV     168(RSP), R19
 551         MOV     176(RSP), R20
 552         MOV     184(RSP), R21
 553         MOV     192(RSP), R22
 554         MOV     200(RSP), R23
 555         MOV     208(RSP), R24
 556         MOV     216(RSP), R25
 557
 558         MOV     256(RSP), R30   // link
 559
 560         ADD     $TRAPFRAMESIZE, RSP
 561         ERET
 562
 563 // irq/fiq/trap/serr from EL1
 564 TEXT vtrap1(SB), 1, $-4
 565         MOV     R29, 248(RSP)   // special
 566
 567         ADD     $TRAPFRAMESIZE, RSP, R1
 568         MRS     ELR_EL1, R2
 569         MRS     SPSR_EL1, R3
 570
 571         MOV     R0, 288(RSP)    // type
 572         MOV     R1, 264(RSP)    // sp
 573         MOV     R2, 272(RSP)    // pc
 574         MOV     R3, 280(RSP)    // psr
 575
 576         ADD     $16, RSP, R0    // ureg
 577         BL      trap(SB)
 578
 579         MSR     $0x3, DAIFSet   // interrupts off
 580
 581         MOV     272(RSP), R2    // pc
 582         MOV     280(RSP), R3    // psr
 583
 584         MSR     R2, ELR_EL1
 585         MSR     R3, SPSR_EL1
 586
 587         MOV     248(RSP), R29   // special
 588         B       _intrreturn
 589
 590 // vector tables
 591 TEXT vsys(SB), 1, $-4
 592         SUB     $TRAPFRAMESIZE, RSP
 593
 594         MOV     R0, 16(RSP)
 595         MOV     R30, 256(RSP)   // link
 596
 597         MOV     R17, 152(RSP)   // temp
 598
 599         MRS     ESR_EL1, R0     // type
 600
 601 _vsyspatch:
 602         B       _vsyspatch      // branch to vsys0() patched in
 603
 604 TEXT vtrap(SB), 1, $-4
 605         SUB     $TRAPFRAMESIZE, RSP
 606
 607         MOV     R0, 16(RSP)
 608         MOV     R1, 24(RSP)
 609         MOV     R2, 32(RSP)
 610         MOV     R3, 40(RSP)
 611         MOV     R4, 48(RSP)
 612         MOV     R5, 56(RSP)
 613         MOV     R6, 64(RSP)
 614         MOV     R7, 72(RSP)
 615         MOV     R8, 80(RSP)
 616         MOV     R9, 88(RSP)
 617         MOV     R10, 96(RSP)
 618         MOV     R11, 104(RSP)
 619         MOV     R12, 112(RSP)
 620         MOV     R13, 120(RSP)
 621         MOV     R14, 128(RSP)
 622         MOV     R15, 136(RSP)
 623         MOV     R16, 144(RSP)
 624         MOV     R17, 152(RSP)
 625         MOV     R18, 160(RSP)
 626         MOV     R19, 168(RSP)
 627         MOV     R20, 176(RSP)
 628         MOV     R21, 184(RSP)
 629         MOV     R22, 192(RSP)
 630         MOV     R23, 200(RSP)
 631         MOV     R24, 208(RSP)
 632         MOV     R25, 216(RSP)
 633
 634         MOV     R30, 256(RSP)   // link
 635
 636         MRS     ESR_EL1, R0     // type
 637
 638 _vtrappatch:
 639         B       _vtrappatch     // branch to vtrapX() patched in
 640
 641 TEXT virq(SB), 1, $-4
 642         SUB     $TRAPFRAMESIZE, RSP
 643
 644         MOV     R0, 16(RSP)
 645         MOV     R1, 24(RSP)
 646         MOV     R2, 32(RSP)
 647         MOV     R3, 40(RSP)
 648         MOV     R4, 48(RSP)
 649         MOV     R5, 56(RSP)
 650         MOV     R6, 64(RSP)
 651         MOV     R7, 72(RSP)
 652         MOV     R8, 80(RSP)
 653         MOV     R9, 88(RSP)
 654         MOV     R10, 96(RSP)
 655         MOV     R11, 104(RSP)
 656         MOV     R12, 112(RSP)
 657         MOV     R13, 120(RSP)
 658         MOV     R14, 128(RSP)
 659         MOV     R15, 136(RSP)
 660         MOV     R16, 144(RSP)
 661         MOV     R17, 152(RSP)
 662         MOV     R18, 160(RSP)
 663         MOV     R19, 168(RSP)
 664         MOV     R20, 176(RSP)
 665         MOV     R21, 184(RSP)
 666         MOV     R22, 192(RSP)
 667         MOV     R23, 200(RSP)
 668         MOV     R24, 208(RSP)
 669         MOV     R25, 216(RSP)
 670
 671         MOV     R30, 256(RSP)   // link
 672
 673         MOV     $(1<<32), R0    // type irq
 674
 675 _virqpatch:
 676         B       _virqpatch      // branch to vtrapX() patched in
 677
 678 TEXT vfiq(SB), 1, $-4
 679         SUB     $TRAPFRAMESIZE, RSP
 680
 681         MOV     R0, 16(RSP)
 682         MOV     R1, 24(RSP)
 683         MOV     R2, 32(RSP)
 684         MOV     R3, 40(RSP)
 685         MOV     R4, 48(RSP)
 686         MOV     R5, 56(RSP)
 687         MOV     R6, 64(RSP)
 688         MOV     R7, 72(RSP)
 689         MOV     R8, 80(RSP)
 690         MOV     R9, 88(RSP)
 691         MOV     R10, 96(RSP)
 692         MOV     R11, 104(RSP)
 693         MOV     R12, 112(RSP)
 694         MOV     R13, 120(RSP)
 695         MOV     R14, 128(RSP)
 696         MOV     R15, 136(RSP)
 697         MOV     R16, 144(RSP)
 698         MOV     R17, 152(RSP)
 699         MOV     R18, 160(RSP)
 700         MOV     R19, 168(RSP)
 701         MOV     R20, 176(RSP)
 702         MOV     R21, 184(RSP)
 703         MOV     R22, 192(RSP)
 704         MOV     R23, 200(RSP)
 705         MOV     R24, 208(RSP)
 706         MOV     R25, 216(RSP)
 707
 708         MOV     R30, 256(RSP)   // link
 709         MOV     $(2<<32), R0    // type fiq
 710
 711 _vfiqpatch:
 712         B       _vfiqpatch      // branch to vtrapX() patched in
 713
 714 TEXT vserr(SB), 1, $-4
 715         SUB     $TRAPFRAMESIZE, RSP
 716
 717         MOV     R0, 16(RSP)
 718         MOV     R1, 24(RSP)
 719         MOV     R2, 32(RSP)
 720         MOV     R3, 40(RSP)
 721         MOV     R4, 48(RSP)
 722         MOV     R5, 56(RSP)
 723         MOV     R6, 64(RSP)
 724         MOV     R7, 72(RSP)
 725         MOV     R8, 80(RSP)
 726         MOV     R9, 88(RSP)
 727         MOV     R10, 96(RSP)
 728         MOV     R11, 104(RSP)
 729         MOV     R12, 112(RSP)
 730         MOV     R13, 120(RSP)
 731         MOV     R14, 128(RSP)
 732         MOV     R15, 136(RSP)
 733         MOV     R16, 144(RSP)
 734         MOV     R17, 152(RSP)
 735         MOV     R18, 160(RSP)
 736         MOV     R19, 168(RSP)
 737         MOV     R20, 176(RSP)
 738         MOV     R21, 184(RSP)
 739         MOV     R22, 192(RSP)
 740         MOV     R23, 200(RSP)
 741         MOV     R24, 208(RSP)
 742         MOV     R25, 216(RSP)
 743
 744         MOV     R30, 256(RSP)   // link
 745
 746         MRS     ESR_EL1, R0
 747         ORR     $(3<<32), R0    // type
 748 _vserrpatch:
 749         B       _vserrpatch     // branch to vtrapX() patched in