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