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 TEXT lcycles(SB), 1, $-4
 302         MRS     PMCCNTR_EL0, R0
 303         RETURN
 304
 305 TEXT setlabel(SB), 1, $-4
 306         MOV     LR, 8(R0)
 307         MOV     SP, R1
 308         MOV     R1, 0(R0)
 309         MOVW    $0, R0
 310         RETURN
 311
 312 TEXT gotolabel(SB), 1, $-4
 313         MOV     8(R0), LR       /* link */
 314         MOV     0(R0), R1       /* sp */
 315         MOV     R1, SP
 316         MOVW    $1, R0
 317         RETURN
 318
 319 TEXT returnto(SB), 1, $-4
 320         MOV     R0, 0(SP)
 321         RETURN
 322
 323 TEXT getfar(SB), 1, $-4
 324         MRS     FAR_EL1, R0
 325         RETURN
 326
 327 TEXT setttbr(SB), 1, $-4
 328         DSB     $ISHST
 329         MSR     R0, TTBR0_EL1
 330         DSB     $ISH
 331         ISB     $SY
 332         RETURN
 333
 334 /*
 335  * TLB maintenance operations.
 336  * these broadcast to all cpu's in the cluser
 337  * (inner sharable domain).
 338  */
 339 TEXT flushasidva(SB), 1, $-4
 340 TEXT tlbivae1is(SB), 1, $-4
 341         DSB     $ISHST
 342         TLBI    R0, 0,8,3,1     /* VAE1IS */
 343         DSB     $ISH
 344         ISB     $SY
 345         RETURN
 346
 347 TEXT flushasidvall(SB), 1, $-4
 348 TEXT tlbivale1is(SB), 1, $-4
 349         DSB     $ISHST
 350         TLBI    R0, 0,8,3,5     /* VALE1IS */
 351         DSB     $ISH
 352         ISB     $SY
 353         RETURN
 354
 355 TEXT flushasid(SB), 1, $-4
 356 TEXT tlbiaside1is(SB), 1, $-4
 357         DSB     $ISHST
 358         TLBI    R0, 0,8,3,2     /* ASIDE1IS */
 359         DSB     $ISH
 360         ISB     $SY
 361         RETURN
 362
 363 TEXT flushtlb(SB), 1, $-4
 364 TEXT tlbivmalle1is(SB), 1, $-4
 365         DSB     $ISHST
 366         TLBI    R0, 0,8,3,0     /* VMALLE1IS */
 367         DSB     $ISH
 368         ISB     $SY
 369         RETURN
 370
 371 /*
 372  * flush the tlb of this cpu. no broadcast.
 373  */
 374 TEXT flushlocaltlb(SB), 1, $-4
 375 TEXT tlbivmalle1(SB), 1, $-4
 376         DSB     $NSHST
 377         TLBI    R0, 0,8,7,0     /* VMALLE1 */
 378         DSB     $NSH
 379         ISB     $SY
 380         RETURN
 381
 382 TEXT fpsaveregs(SB), 1, $-4
 383         WORD    $(1<<30 | 3 << 26 | 2<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 0)  /* MOV { V0, V1, V2, V3  }, (R0)64! */
 384         WORD    $(1<<30 | 3 << 26 | 2<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 4)  /* MOV { V4, V5, V6, V7  }, (R0)64! */
 385         WORD    $(1<<30 | 3 << 26 | 2<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 8)  /* MOV { V8, V9, V10,V11 }, (R0)64! */
 386         WORD    $(1<<30 | 3 << 26 | 2<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 12) /* MOV { V12,V13,V14,V15 }, (R0)64! */
 387         WORD    $(1<<30 | 3 << 26 | 2<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 16) /* MOV { V16,V17,V18,V19 }, (R0)64! */
 388         WORD    $(1<<30 | 3 << 26 | 2<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 20) /* MOV { V20,V21,V22,V23 }, (R0)64! */
 389         WORD    $(1<<30 | 3 << 26 | 2<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 24) /* MOV { V24,V25,V26,V27 }, (R0)64! */
 390         WORD    $(1<<30 | 3 << 26 | 2<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 28) /* MOV { V28,V29,V30,V31 }, (R0)64! */
 391         RETURN
 392
 393 TEXT fploadregs(SB), 1, $-4
 394         WORD    $(1<<30 | 3 << 26 | 3<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 0)  /* MOV (R0)64!, { V0, V1, V2, V3  } */
 395         WORD    $(1<<30 | 3 << 26 | 3<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 4)  /* MOV (R0)64!, { V4, V5, V6, V7  } */
 396         WORD    $(1<<30 | 3 << 26 | 3<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 8)  /* MOV (R0)64!, { V8, V9, V10,V11 } */
 397         WORD    $(1<<30 | 3 << 26 | 3<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 12) /* MOV (R0)64!, { V12,V13,V14,V15 } */
 398         WORD    $(1<<30 | 3 << 26 | 3<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 16) /* MOV (R0)64!, { V16,V17,V18,V19 } */
 399         WORD    $(1<<30 | 3 << 26 | 3<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 20) /* MOV (R0)64!, { V20,V21,V22,V23 } */
 400         WORD    $(1<<30 | 3 << 26 | 3<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 24) /* MOV (R0)64!, { V24,V25,V26,V27 } */
 401         WORD    $(1<<30 | 3 << 26 | 3<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 28) /* MOV (R0)64!, { V28,V29,V30,V31 } */
 402         RETURN
 403
 404 // syscall or trap from EL0
 405 TEXT vsys0(SB), 1, $-4
 406         LSRW    $26, R0, R17    // ec
 407         CMPW    $0x15, R17      // SVC trap?
 408         BNE     _itsatrap       // nope.
 409
 410         MOVP    R26, R27, 224(RSP)
 411         MOVP    R28, R29, 240(RSP)
 412
 413         MRS     SP_EL0, R1
 414         MRS     ELR_EL1, R2
 415         MRS     SPSR_EL1, R3
 416
 417         MOV     R0, 288(RSP)    // type
 418         MOV     R1, 264(RSP)    // sp
 419         MOV     R2, 272(RSP)    // pc
 420         MOV     R3, 280(RSP)    // psr
 421
 422         MOV     $setSB(SB), R28
 423         MRS     TPIDR_EL1, R27
 424         MOV     16(R27), R26
 425
 426         ADD     $16, RSP, R0    // ureg
 427         BL      syscall(SB)
 428
 429 TEXT forkret(SB), 1, $-4
 430         MSR     $0x3, DAIFSet   // interrupts off
 431
 432         ADD     $16, RSP, R0    // ureg
 433
 434         MOV     16(RSP), R0     // ret
 435         MOV     264(RSP), R1    // sp
 436         MOV     272(RSP), R2    // pc
 437         MOV     280(RSP), R3    // psr
 438
 439         MSR     R1, SP_EL0
 440         MSR     R2, ELR_EL1
 441         MSR     R3, SPSR_EL1
 442
 443         MOVP    224(RSP), R26, R27
 444         MOVP    240(RSP), R28, R29
 445
 446         MOV     256(RSP), R30   // link
 447
 448         ADD     $TRAPFRAMESIZE, RSP
 449         ERET
 450
 451 TEXT itsatrap<>(SB), 1, $-4
 452 _itsatrap:
 453         MOVP    R1, R2, 24(RSP)
 454         MOVP    R3, R4, 40(RSP)
 455         MOVP    R5, R6, 56(RSP)
 456         MOVP    R7, R8, 72(RSP)
 457         MOVP    R9, R10, 88(RSP)
 458         MOVP    R11, R12, 104(RSP)
 459         MOVP    R13, R14, 120(RSP)
 460         MOVP    R15, R16, 136(RSP)
 461
 462         MOVP    R18, R19, 160(RSP)
 463         MOVP    R20, R21, 176(RSP)
 464         MOVP    R22, R23, 192(RSP)
 465         MOVP    R24, R25, 208(RSP)
 466
 467 // trap/irq/fiq/serr from EL0
 468 TEXT vtrap0(SB), 1, $-4
 469         MOVP    R26, R27, 224(RSP)
 470         MOVP    R28, R29, 240(RSP)
 471
 472         MRS     SP_EL0, R1
 473         MRS     ELR_EL1, R2
 474         MRS     SPSR_EL1, R3
 475
 476         MOV     R0, 288(RSP)    // type
 477         MOV     R1, 264(RSP)    // sp
 478         MOV     R2, 272(RSP)    // pc
 479         MOV     R3, 280(RSP)    // psr
 480
 481         MOV     $setSB(SB), R28
 482         MRS     TPIDR_EL1, R27
 483         MOV     16(R27), R26
 484
 485         ADD     $16, RSP, R0    // ureg
 486         BL      trap(SB)
 487
 488 TEXT noteret(SB), 1, $-4
 489         MSR     $0x3, DAIFSet   // interrupts off
 490
 491         ADD     $16, RSP, R0    // ureg
 492
 493         MOV     264(RSP), R1    // sp
 494         MOV     272(RSP), R2    // pc
 495         MOV     280(RSP), R3    // psr
 496
 497         MSR     R1, SP_EL0
 498         MSR     R2, ELR_EL1
 499         MSR     R3, SPSR_EL1
 500
 501         MOVP    224(RSP), R26, R27
 502         MOVP    240(RSP), R28, R29
 503
 504 _intrreturn:
 505         MOVP    16(RSP), R0, R1
 506         MOVP    32(RSP), R2, R3
 507         MOVP    48(RSP), R4, R5
 508         MOVP    64(RSP), R6, R7
 509         MOVP    80(RSP), R8, R9
 510         MOVP    96(RSP), R10, R11
 511         MOVP    112(RSP), R12, R13
 512         MOVP    128(RSP), R14, R15
 513         MOVP    144(RSP), R16, R17
 514         MOVP    160(RSP), R18, R19
 515         MOVP    176(RSP), R20, R21
 516         MOVP    192(RSP), R22, R23
 517         MOVP    208(RSP), R24, R25
 518
 519         MOV     256(RSP), R30   // link
 520
 521         ADD     $TRAPFRAMESIZE, RSP
 522         ERET
 523
 524 // irq/fiq/trap/serr from EL1
 525 TEXT vtrap1(SB), 1, $-4
 526         MOV     R29, 248(RSP)   // special
 527
 528         ADD     $TRAPFRAMESIZE, RSP, R1
 529         MRS     ELR_EL1, R2
 530         MRS     SPSR_EL1, R3
 531
 532         MOV     R0, 288(RSP)    // type
 533         MOV     R1, 264(RSP)    // sp
 534         MOV     R2, 272(RSP)    // pc
 535         MOV     R3, 280(RSP)    // psr
 536
 537         ADD     $16, RSP, R0    // ureg
 538         BL      trap(SB)
 539
 540         MSR     $0x3, DAIFSet   // interrupts off
 541
 542         MOV     272(RSP), R2    // pc
 543         MOV     280(RSP), R3    // psr
 544
 545         MSR     R2, ELR_EL1
 546         MSR     R3, SPSR_EL1
 547
 548         MOV     248(RSP), R29   // special
 549         B       _intrreturn
 550
 551 // vector tables
 552 TEXT vsys(SB), 1, $-4
 553         SUB     $TRAPFRAMESIZE, RSP
 554
 555         MOV     R0, 16(RSP)
 556         MOV     R30, 256(RSP)   // link
 557
 558         MOV     R17, 152(RSP)   // temp
 559
 560         MRS     ESR_EL1, R0     // type
 561
 562 _vsyspatch:
 563         B       _vsyspatch      // branch to vsys0() patched in
 564
 565 TEXT vtrap(SB), 1, $-4
 566         SUB     $TRAPFRAMESIZE, RSP
 567
 568         MOVP    R0, R1, 16(RSP)
 569         MOVP    R2, R3, 32(RSP)
 570         MOVP    R4, R5, 48(RSP)
 571         MOVP    R6, R7, 64(RSP)
 572         MOVP    R8, R9, 80(RSP)
 573         MOVP    R10, R11, 96(RSP)
 574         MOVP    R12, R13, 112(RSP)
 575         MOVP    R14, R15, 128(RSP)
 576         MOVP    R16, R17, 144(RSP)
 577         MOVP    R18, R19, 160(RSP)
 578         MOVP    R20, R21, 176(RSP)
 579         MOVP    R22, R23, 192(RSP)
 580         MOVP    R24, R25, 208(RSP)
 581
 582         MOV     R30, 256(RSP)   // link
 583
 584         MRS     ESR_EL1, R0     // type
 585
 586 _vtrappatch:
 587         B       _vtrappatch     // branch to vtrapX() patched in
 588
 589 TEXT virq(SB), 1, $-4
 590         SUB     $TRAPFRAMESIZE, RSP
 591
 592         MOVP    R0, R1, 16(RSP)
 593         MOVP    R2, R3, 32(RSP)
 594         MOVP    R4, R5, 48(RSP)
 595         MOVP    R6, R7, 64(RSP)
 596         MOVP    R8, R9, 80(RSP)
 597         MOVP    R10, R11, 96(RSP)
 598         MOVP    R12, R13, 112(RSP)
 599         MOVP    R14, R15, 128(RSP)
 600         MOVP    R16, R17, 144(RSP)
 601         MOVP    R18, R19, 160(RSP)
 602         MOVP    R20, R21, 176(RSP)
 603         MOVP    R22, R23, 192(RSP)
 604         MOVP    R24, R25, 208(RSP)
 605
 606         MOV     R30, 256(RSP)   // link
 607
 608         MOV     $(1<<32), R0    // type irq
 609
 610 _virqpatch:
 611         B       _virqpatch      // branch to vtrapX() patched in
 612
 613 TEXT vfiq(SB), 1, $-4
 614         SUB     $TRAPFRAMESIZE, RSP
 615
 616         MOVP    R0, R1, 16(RSP)
 617         MOVP    R2, R3, 32(RSP)
 618         MOVP    R4, R5, 48(RSP)
 619         MOVP    R6, R7, 64(RSP)
 620         MOVP    R8, R9, 80(RSP)
 621         MOVP    R10, R11, 96(RSP)
 622         MOVP    R12, R13, 112(RSP)
 623         MOVP    R14, R15, 128(RSP)
 624         MOVP    R16, R17, 144(RSP)
 625         MOVP    R18, R19, 160(RSP)
 626         MOVP    R20, R21, 176(RSP)
 627         MOVP    R22, R23, 192(RSP)
 628         MOVP    R24, R25, 208(RSP)
 629
 630         MOV     R30, 256(RSP)   // link
 631         MOV     $(2<<32), R0    // type fiq
 632
 633 _vfiqpatch:
 634         B       _vfiqpatch      // branch to vtrapX() patched in
 635
 636 TEXT vserr(SB), 1, $-4
 637         SUB     $TRAPFRAMESIZE, RSP
 638
 639         MOVP    R0, R1, 16(RSP)
 640         MOVP    R2, R3, 32(RSP)
 641         MOVP    R4, R5, 48(RSP)
 642         MOVP    R6, R7, 64(RSP)
 643         MOVP    R8, R9, 80(RSP)
 644         MOVP    R10, R11, 96(RSP)
 645         MOVP    R12, R13, 112(RSP)
 646         MOVP    R14, R15, 128(RSP)
 647         MOVP    R16, R17, 144(RSP)
 648         MOVP    R18, R19, 160(RSP)
 649         MOVP    R20, R21, 176(RSP)
 650         MOVP    R22, R23, 192(RSP)
 651         MOVP    R24, R25, 208(RSP)
 652
 653         MOV     R30, 256(RSP)   // link
 654
 655         MRS     ESR_EL1, R0
 656         ORR     $(3<<32), R0    // type
 657 _vserrpatch:
 658         B       _vserrpatch     // branch to vtrapX() patched in
 659
 660 /* fault-proof memcpy */
 661 TEXT peek(SB), 1, $-4
 662         MOV     R0, R1
 663         MOV     dst+8(FP), R2
 664         MOVWU   len+16(FP), R0
 665 TEXT _peekinst(SB), 1, $-4
 666 _peekloop:
 667         MOVBU   (R1)1!, R3
 668         MOVBU   R3, (R2)1!
 669         SUBS    $1, R0
 670         BNE     _peekloop
 671         RETURN