675 lines
13 KiB
ArmAsm
675 lines
13 KiB
ArmAsm
#include "mem.h"
|
|
#include "sysreg.h"
|
|
|
|
#undef SYSREG
|
|
#define SYSREG(op0,op1,Cn,Cm,op2) SPR(((op0)<<19|(op1)<<16|(Cn)<<12|(Cm)<<8|(op2)<<5))
|
|
|
|
TEXT _start(SB), 1, $-4
|
|
MOV R0, R26 /* save */
|
|
|
|
MOV $setSB-KZERO(SB), R28
|
|
BL svcmode<>(SB)
|
|
|
|
/* use dedicated stack pointer per exception level */
|
|
MOVWU $1, R1
|
|
MSR R1, SPSel
|
|
|
|
BL mmudisable<>(SB)
|
|
|
|
/* invalidate local caches */
|
|
BL cachedinv(SB)
|
|
BL cacheiinv(SB)
|
|
|
|
MOV $(MACHADDR(0)-KZERO), R27
|
|
MRS MPIDR_EL1, R1
|
|
ANDW $(MAXMACH-1), R1
|
|
MOVWU $MACHSIZE, R2
|
|
MULW R1, R2, R2
|
|
SUB R2, R27
|
|
|
|
ADD $(MACHSIZE-16), R27, R2
|
|
MOV R2, SP
|
|
|
|
CBNZ R1, _startup
|
|
|
|
/* clear page table and machs */
|
|
MOV $(L1-KZERO), R1
|
|
MOV $(MACHADDR(-1)-KZERO), R2
|
|
_zerol1:
|
|
MOV ZR, (R1)8!
|
|
CMP R1, R2
|
|
BNE _zerol1
|
|
|
|
/* clear BSS */
|
|
MOV $edata-KZERO(SB), R1
|
|
MOV $end-KZERO(SB), R2
|
|
_zerobss:
|
|
MOV ZR, (R1)8!
|
|
CMP R1, R2
|
|
BNE _zerobss
|
|
|
|
/* setup page tables */
|
|
MOV $(L1-KZERO), R0
|
|
BL mmu0init(SB)
|
|
|
|
SEVL
|
|
_startup:
|
|
WFE
|
|
BL mmuenable<>(SB)
|
|
|
|
MOV R26, R0
|
|
MOV $0, R26
|
|
ORR $KZERO, R27
|
|
MSR R27, TPIDR_EL1
|
|
MOV $setSB(SB), R28
|
|
|
|
BL main(SB)
|
|
|
|
TEXT stop<>(SB), 1, $-4
|
|
_stop:
|
|
WFE
|
|
B _stop
|
|
|
|
TEXT sev(SB), 1, $-4
|
|
SEV
|
|
WFE
|
|
RETURN
|
|
|
|
TEXT svcmode<>(SB), 1, $-4
|
|
MSR $0xF, DAIFSet
|
|
MRS CurrentEL, R0
|
|
ANDW $(3<<2), R0
|
|
CMPW $(1<<2), R0
|
|
BEQ el1
|
|
CMPW $(2<<2), R0
|
|
BEQ el2
|
|
B stop<>(SB)
|
|
el2:
|
|
MOV $0, R0
|
|
MSR R0, MDCR_EL2
|
|
ISB $SY
|
|
|
|
/* HCR = RW, HCD, SWIO, BSU, FB */
|
|
MOVWU $(1<<31 | 1<<29 | 1<<2 | 0<<10 | 0<<9), R0
|
|
MSR R0, HCR_EL2
|
|
ISB $SY
|
|
|
|
/* SCTLR = RES1 */
|
|
MOVWU $(3<<4 | 1<<11 | 1<<16 | 1<<18 | 3<<22 | 3<<28), R0
|
|
ISB $SY
|
|
MSR R0, SCTLR_EL2
|
|
ISB $SY
|
|
|
|
/* set VMID to zero */
|
|
MOV $0, R0
|
|
MSR R0, VTTBR_EL2
|
|
ISB $SY
|
|
|
|
MOVWU $(0xF<<6 | 4), R0
|
|
MSR R0, SPSR_EL2
|
|
MSR LR, ELR_EL2
|
|
ERET
|
|
el1:
|
|
RETURN
|
|
|
|
TEXT mmudisable<>(SB), 1, $-4
|
|
#define SCTLRCLR \
|
|
/* RES0 */ ( 3<<30 \
|
|
/* RES0 */ | 1<<27 \
|
|
/* UCI */ | 1<<26 \
|
|
/* EE */ | 1<<25 \
|
|
/* RES0 */ | 1<<21 \
|
|
/* E0E */ | 1<<24 \
|
|
/* WXN */ | 1<<19 \
|
|
/* nTWE */ | 1<<18 \
|
|
/* RES0 */ | 1<<17 \
|
|
/* nTWI */ | 1<<16 \
|
|
/* UCT */ | 1<<15 \
|
|
/* DZE */ | 1<<14 \
|
|
/* RES0 */ | 1<<13 \
|
|
/* RES0 */ | 1<<10 \
|
|
/* UMA */ | 1<<9 \
|
|
/* SA0 */ | 1<<4 \
|
|
/* SA */ | 1<<3 \
|
|
/* A */ | 1<<1 )
|
|
#define SCTLRSET \
|
|
/* RES1 */ ( 3<<28 \
|
|
/* RES1 */ | 3<<22 \
|
|
/* RES1 */ | 1<<20 \
|
|
/* RES1 */ | 1<<11 )
|
|
#define SCTLRMMU \
|
|
/* I */ ( 1<<12 \
|
|
/* C */ | 1<<2 \
|
|
/* M */ | 1<<0 )
|
|
|
|
/* initialise SCTLR, MMU and caches off */
|
|
ISB $SY
|
|
MRS SCTLR_EL1, R0
|
|
BIC $(SCTLRCLR | SCTLRMMU), R0
|
|
ORR $SCTLRSET, R0
|
|
ISB $SY
|
|
MSR R0, SCTLR_EL1
|
|
ISB $SY
|
|
|
|
B flushlocaltlb(SB)
|
|
|
|
TEXT mmuenable<>(SB), 1, $-4
|
|
/* return to virtual */
|
|
ORR $KZERO, LR
|
|
MOV LR, -16(RSP)!
|
|
|
|
BL flushlocaltlb(SB)
|
|
|
|
/* memory attributes */
|
|
#define MAIRINIT \
|
|
( 0xFF << MA_MEM_WB*8 \
|
|
| 0x33 << MA_MEM_WT*8 \
|
|
| 0x44 << MA_MEM_UC*8 \
|
|
| 0x00 << MA_DEV_nGnRnE*8 \
|
|
| 0x04 << MA_DEV_nGnRE*8 \
|
|
| 0x08 << MA_DEV_nGRE*8 \
|
|
| 0x0C << MA_DEV_GRE*8 )
|
|
MOV $MAIRINIT, R1
|
|
MSR R1, MAIR_EL1
|
|
ISB $SY
|
|
|
|
/* translation control */
|
|
#define TCRINIT \
|
|
/* TBI1 */ ( 0<<38 \
|
|
/* TBI0 */ | 0<<37 \
|
|
/* AS */ | 0<<36 \
|
|
/* TG1 */ | (((3<<16|1<<14|2<<12)>>PGSHIFT)&3)<<30 \
|
|
/* SH1 */ | SHARE_INNER<<28 \
|
|
/* ORGN1 */ | CACHE_WB<<26 \
|
|
/* IRGN1 */ | CACHE_WB<<24 \
|
|
/* EPD1 */ | 0<<23 \
|
|
/* A1 */ | 0<<22 \
|
|
/* T1SZ */ | (64-EVASHIFT)<<16 \
|
|
/* TG0 */ | (((1<<16|2<<14|0<<12)>>PGSHIFT)&3)<<14 \
|
|
/* SH0 */ | SHARE_INNER<<12 \
|
|
/* ORGN0 */ | CACHE_WB<<10 \
|
|
/* IRGN0 */ | CACHE_WB<<8 \
|
|
/* EPD0 */ | 0<<7 \
|
|
/* T0SZ */ | (64-EVASHIFT)<<0 )
|
|
MOV $TCRINIT, R1
|
|
MRS ID_AA64MMFR0_EL1, R2
|
|
ANDW $0x7, R2 // PARange
|
|
ADD R2<<32, R1 // IPS
|
|
MSR R1, TCR_EL1
|
|
ISB $SY
|
|
|
|
/* load the page tables */
|
|
MOV $(L1TOP-KZERO), R0
|
|
ISB $SY
|
|
MSR R0, TTBR0_EL1
|
|
MSR R0, TTBR1_EL1
|
|
ISB $SY
|
|
|
|
/* enable MMU and caches */
|
|
MRS SCTLR_EL1, R1
|
|
ORR $SCTLRMMU, R1
|
|
ISB $SY
|
|
MSR R1, SCTLR_EL1
|
|
ISB $SY
|
|
|
|
MOV RSP, R1
|
|
ORR $KZERO, R1
|
|
MOV R1, RSP
|
|
MOV (RSP)16!, LR
|
|
B cacheiinv(SB)
|
|
|
|
TEXT touser(SB), 1, $-4
|
|
MSR $0x3, DAIFSet // interrupts off
|
|
MOVWU $0x10028, R1 // entry
|
|
MOVWU $0, R2 // psr
|
|
MSR R0, SP_EL0 // sp
|
|
MSR R1, ELR_EL1
|
|
MSR R2, SPSR_EL1
|
|
ERET
|
|
|
|
TEXT cas(SB), 1, $-4
|
|
TEXT cmpswap(SB), 1, $-4
|
|
MOVWU ov+8(FP), R1
|
|
MOVWU nv+16(FP), R2
|
|
_cas1:
|
|
LDXRW (R0), R3
|
|
CMP R3, R1
|
|
BNE _cas0
|
|
STXRW R2, (R0), R4
|
|
CBNZ R4, _cas1
|
|
MOVW $1, R0
|
|
DMB $ISH
|
|
RETURN
|
|
_cas0:
|
|
CLREX
|
|
MOVW $0, R0
|
|
RETURN
|
|
|
|
TEXT tas(SB), 1, $-4
|
|
TEXT _tas(SB), 1, $-4
|
|
MOVW $0xdeaddead, R2
|
|
_tas1:
|
|
LDXRW (R0), R1
|
|
STXRW R2, (R0), R3
|
|
CBNZ R3, _tas1
|
|
MOVW R1, R0
|
|
|
|
TEXT coherence(SB), 1, $-4
|
|
DMB $ISH
|
|
RETURN
|
|
|
|
TEXT islo(SB), 1, $-4
|
|
MRS DAIF, R0
|
|
AND $(0x2<<6), R0
|
|
EOR $(0x2<<6), R0
|
|
RETURN
|
|
|
|
TEXT splhi(SB), 1, $-4
|
|
MRS DAIF, R0
|
|
MSR $0x2, DAIFSet
|
|
RETURN
|
|
|
|
TEXT splfhi(SB), 1, $-4
|
|
MRS DAIF, R0
|
|
MSR $0x3, DAIFSet
|
|
RETURN
|
|
|
|
TEXT spllo(SB), 1, $-4
|
|
MSR $0x3, DAIFClr
|
|
RETURN
|
|
|
|
TEXT splflo(SB), 1, $-4
|
|
MSR $0x1, DAIFClr
|
|
RETURN
|
|
|
|
TEXT splx(SB), 1, $-4
|
|
MSR R0, DAIF
|
|
RETURN
|
|
|
|
TEXT idlehands(SB), 1, $-4
|
|
DMB $ISH
|
|
MOV $nrdy(SB), R1
|
|
LDXRW (R1), R0
|
|
CBZ R0, _goodnight
|
|
CLREX
|
|
SEVL
|
|
_goodnight:
|
|
WFE
|
|
RETURN
|
|
|
|
TEXT vcycles(SB), 1, $-4
|
|
MRS CNTVCT_EL0, R0
|
|
RETURN
|
|
|
|
TEXT lcycles(SB), 1, $-4
|
|
MRS PMCCNTR_EL0, R0
|
|
RETURN
|
|
|
|
TEXT setlabel(SB), 1, $-4
|
|
MOV LR, 8(R0)
|
|
MOV SP, R1
|
|
MOV R1, 0(R0)
|
|
MOVW $0, R0
|
|
RETURN
|
|
|
|
TEXT gotolabel(SB), 1, $-4
|
|
MOV 8(R0), LR /* link */
|
|
MOV 0(R0), R1 /* sp */
|
|
MOV R1, SP
|
|
MOVW $1, R0
|
|
RETURN
|
|
|
|
TEXT returnto(SB), 1, $-4
|
|
MOV R0, 0(SP)
|
|
RETURN
|
|
|
|
TEXT getfar(SB), 1, $-4
|
|
MRS FAR_EL1, R0
|
|
RETURN
|
|
|
|
TEXT setttbr(SB), 1, $-4
|
|
DSB $ISHST
|
|
MSR R0, TTBR0_EL1
|
|
DSB $ISH
|
|
ISB $SY
|
|
RETURN
|
|
|
|
/*
|
|
* TLB maintenance operations.
|
|
* these broadcast to all cpu's in the cluser
|
|
* (inner sharable domain).
|
|
*/
|
|
TEXT flushasidva(SB), 1, $-4
|
|
TEXT tlbivae1is(SB), 1, $-4
|
|
DSB $ISHST
|
|
TLBI R0, 0,8,3,1 /* VAE1IS */
|
|
DSB $ISH
|
|
ISB $SY
|
|
RETURN
|
|
|
|
TEXT flushasidvall(SB), 1, $-4
|
|
TEXT tlbivale1is(SB), 1, $-4
|
|
DSB $ISHST
|
|
TLBI R0, 0,8,3,5 /* VALE1IS */
|
|
DSB $ISH
|
|
ISB $SY
|
|
RETURN
|
|
|
|
TEXT flushasid(SB), 1, $-4
|
|
TEXT tlbiaside1is(SB), 1, $-4
|
|
DSB $ISHST
|
|
TLBI R0, 0,8,3,2 /* ASIDE1IS */
|
|
DSB $ISH
|
|
ISB $SY
|
|
RETURN
|
|
|
|
TEXT flushtlb(SB), 1, $-4
|
|
TEXT tlbivmalle1is(SB), 1, $-4
|
|
DSB $ISHST
|
|
TLBI R0, 0,8,3,0 /* VMALLE1IS */
|
|
DSB $ISH
|
|
ISB $SY
|
|
RETURN
|
|
|
|
/*
|
|
* flush the tlb of this cpu. no broadcast.
|
|
*/
|
|
TEXT flushlocaltlb(SB), 1, $-4
|
|
TEXT tlbivmalle1(SB), 1, $-4
|
|
DSB $NSHST
|
|
TLBI R0, 0,8,7,0 /* VMALLE1 */
|
|
DSB $NSH
|
|
ISB $SY
|
|
RETURN
|
|
|
|
TEXT fpsaveregs(SB), 1, $-4
|
|
WORD $(1<<30 | 3 << 26 | 2<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 0) /* MOV { V0, V1, V2, V3 }, (R0)64! */
|
|
WORD $(1<<30 | 3 << 26 | 2<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 4) /* MOV { V4, V5, V6, V7 }, (R0)64! */
|
|
WORD $(1<<30 | 3 << 26 | 2<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 8) /* MOV { V8, V9, V10,V11 }, (R0)64! */
|
|
WORD $(1<<30 | 3 << 26 | 2<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 12) /* MOV { V12,V13,V14,V15 }, (R0)64! */
|
|
WORD $(1<<30 | 3 << 26 | 2<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 16) /* MOV { V16,V17,V18,V19 }, (R0)64! */
|
|
WORD $(1<<30 | 3 << 26 | 2<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 20) /* MOV { V20,V21,V22,V23 }, (R0)64! */
|
|
WORD $(1<<30 | 3 << 26 | 2<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 24) /* MOV { V24,V25,V26,V27 }, (R0)64! */
|
|
WORD $(1<<30 | 3 << 26 | 2<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 28) /* MOV { V28,V29,V30,V31 }, (R0)64! */
|
|
RETURN
|
|
|
|
TEXT fploadregs(SB), 1, $-4
|
|
WORD $(1<<30 | 3 << 26 | 3<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 0) /* MOV (R0)64!, { V0, V1, V2, V3 } */
|
|
WORD $(1<<30 | 3 << 26 | 3<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 4) /* MOV (R0)64!, { V4, V5, V6, V7 } */
|
|
WORD $(1<<30 | 3 << 26 | 3<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 8) /* MOV (R0)64!, { V8, V9, V10,V11 } */
|
|
WORD $(1<<30 | 3 << 26 | 3<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 12) /* MOV (R0)64!, { V12,V13,V14,V15 } */
|
|
WORD $(1<<30 | 3 << 26 | 3<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 16) /* MOV (R0)64!, { V16,V17,V18,V19 } */
|
|
WORD $(1<<30 | 3 << 26 | 3<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 20) /* MOV (R0)64!, { V20,V21,V22,V23 } */
|
|
WORD $(1<<30 | 3 << 26 | 3<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 24) /* MOV (R0)64!, { V24,V25,V26,V27 } */
|
|
WORD $(1<<30 | 3 << 26 | 3<<22 | 0x1F<<16 | 3<<10 | 0<<5 | 28) /* MOV (R0)64!, { V28,V29,V30,V31 } */
|
|
RETURN
|
|
|
|
// syscall or trap from EL0
|
|
TEXT vsys0(SB), 1, $-4
|
|
LSRW $26, R0, R17 // ec
|
|
CMPW $0x15, R17 // SVC trap?
|
|
BNE _itsatrap // nope.
|
|
|
|
MOVP R26, R27, 224(RSP)
|
|
MOVP R28, R29, 240(RSP)
|
|
|
|
MRS SP_EL0, R1
|
|
MRS ELR_EL1, R2
|
|
MRS SPSR_EL1, R3
|
|
|
|
MOV R0, 288(RSP) // type
|
|
MOV R1, 264(RSP) // sp
|
|
MOV R2, 272(RSP) // pc
|
|
MOV R3, 280(RSP) // psr
|
|
|
|
MOV $setSB(SB), R28
|
|
MRS TPIDR_EL1, R27
|
|
MOV 16(R27), R26
|
|
|
|
ADD $16, RSP, R0 // ureg
|
|
BL syscall(SB)
|
|
|
|
TEXT forkret(SB), 1, $-4
|
|
MSR $0x3, DAIFSet // interrupts off
|
|
|
|
ADD $16, RSP, R0 // ureg
|
|
|
|
MOV 16(RSP), R0 // ret
|
|
MOV 264(RSP), R1 // sp
|
|
MOV 272(RSP), R2 // pc
|
|
MOV 280(RSP), R3 // psr
|
|
|
|
MSR R1, SP_EL0
|
|
MSR R2, ELR_EL1
|
|
MSR R3, SPSR_EL1
|
|
|
|
MOVP 224(RSP), R26, R27
|
|
MOVP 240(RSP), R28, R29
|
|
|
|
MOV 256(RSP), R30 // link
|
|
|
|
ADD $TRAPFRAMESIZE, RSP
|
|
ERET
|
|
|
|
TEXT itsatrap<>(SB), 1, $-4
|
|
_itsatrap:
|
|
MOVP R1, R2, 24(RSP)
|
|
MOVP R3, R4, 40(RSP)
|
|
MOVP R5, R6, 56(RSP)
|
|
MOVP R7, R8, 72(RSP)
|
|
MOVP R9, R10, 88(RSP)
|
|
MOVP R11, R12, 104(RSP)
|
|
MOVP R13, R14, 120(RSP)
|
|
MOVP R15, R16, 136(RSP)
|
|
|
|
MOVP R18, R19, 160(RSP)
|
|
MOVP R20, R21, 176(RSP)
|
|
MOVP R22, R23, 192(RSP)
|
|
MOVP R24, R25, 208(RSP)
|
|
|
|
// trap/irq/fiq/serr from EL0
|
|
TEXT vtrap0(SB), 1, $-4
|
|
MOVP R26, R27, 224(RSP)
|
|
MOVP R28, R29, 240(RSP)
|
|
|
|
MRS SP_EL0, R1
|
|
MRS ELR_EL1, R2
|
|
MRS SPSR_EL1, R3
|
|
|
|
MOV R0, 288(RSP) // type
|
|
MOV R1, 264(RSP) // sp
|
|
MOV R2, 272(RSP) // pc
|
|
MOV R3, 280(RSP) // psr
|
|
|
|
MOV $setSB(SB), R28
|
|
MRS TPIDR_EL1, R27
|
|
MOV 16(R27), R26
|
|
|
|
ADD $16, RSP, R0 // ureg
|
|
BL trap(SB)
|
|
|
|
TEXT noteret(SB), 1, $-4
|
|
MSR $0x3, DAIFSet // interrupts off
|
|
|
|
ADD $16, RSP, R0 // ureg
|
|
|
|
MOV 264(RSP), R1 // sp
|
|
MOV 272(RSP), R2 // pc
|
|
MOV 280(RSP), R3 // psr
|
|
|
|
MSR R1, SP_EL0
|
|
MSR R2, ELR_EL1
|
|
MSR R3, SPSR_EL1
|
|
|
|
MOVP 224(RSP), R26, R27
|
|
MOVP 240(RSP), R28, R29
|
|
|
|
_intrreturn:
|
|
MOVP 16(RSP), R0, R1
|
|
MOVP 32(RSP), R2, R3
|
|
MOVP 48(RSP), R4, R5
|
|
MOVP 64(RSP), R6, R7
|
|
MOVP 80(RSP), R8, R9
|
|
MOVP 96(RSP), R10, R11
|
|
MOVP 112(RSP), R12, R13
|
|
MOVP 128(RSP), R14, R15
|
|
MOVP 144(RSP), R16, R17
|
|
MOVP 160(RSP), R18, R19
|
|
MOVP 176(RSP), R20, R21
|
|
MOVP 192(RSP), R22, R23
|
|
MOVP 208(RSP), R24, R25
|
|
|
|
MOV 256(RSP), R30 // link
|
|
|
|
ADD $TRAPFRAMESIZE, RSP
|
|
ERET
|
|
|
|
// irq/fiq/trap/serr from EL1
|
|
TEXT vtrap1(SB), 1, $-4
|
|
MOV R29, 248(RSP) // special
|
|
|
|
ADD $TRAPFRAMESIZE, RSP, R1
|
|
MRS ELR_EL1, R2
|
|
MRS SPSR_EL1, R3
|
|
|
|
MOV R0, 288(RSP) // type
|
|
MOV R1, 264(RSP) // sp
|
|
MOV R2, 272(RSP) // pc
|
|
MOV R3, 280(RSP) // psr
|
|
|
|
ADD $16, RSP, R0 // ureg
|
|
BL trap(SB)
|
|
|
|
MSR $0x3, DAIFSet // interrupts off
|
|
|
|
MOV 272(RSP), R2 // pc
|
|
MOV 280(RSP), R3 // psr
|
|
|
|
MSR R2, ELR_EL1
|
|
MSR R3, SPSR_EL1
|
|
|
|
MOV 248(RSP), R29 // special
|
|
B _intrreturn
|
|
|
|
// vector tables
|
|
TEXT vsys(SB), 1, $-4
|
|
SUB $TRAPFRAMESIZE, RSP
|
|
|
|
MOV R0, 16(RSP)
|
|
MOV R30, 256(RSP) // link
|
|
|
|
MOV R17, 152(RSP) // temp
|
|
|
|
MRS ESR_EL1, R0 // type
|
|
|
|
_vsyspatch:
|
|
B _vsyspatch // branch to vsys0() patched in
|
|
|
|
TEXT vtrap(SB), 1, $-4
|
|
SUB $TRAPFRAMESIZE, RSP
|
|
|
|
MOVP R0, R1, 16(RSP)
|
|
MOVP R2, R3, 32(RSP)
|
|
MOVP R4, R5, 48(RSP)
|
|
MOVP R6, R7, 64(RSP)
|
|
MOVP R8, R9, 80(RSP)
|
|
MOVP R10, R11, 96(RSP)
|
|
MOVP R12, R13, 112(RSP)
|
|
MOVP R14, R15, 128(RSP)
|
|
MOVP R16, R17, 144(RSP)
|
|
MOVP R18, R19, 160(RSP)
|
|
MOVP R20, R21, 176(RSP)
|
|
MOVP R22, R23, 192(RSP)
|
|
MOVP R24, R25, 208(RSP)
|
|
|
|
MOV R30, 256(RSP) // link
|
|
|
|
MRS ESR_EL1, R0 // type
|
|
|
|
_vtrappatch:
|
|
B _vtrappatch // branch to vtrapX() patched in
|
|
|
|
TEXT virq(SB), 1, $-4
|
|
SUB $TRAPFRAMESIZE, RSP
|
|
|
|
MOVP R0, R1, 16(RSP)
|
|
MOVP R2, R3, 32(RSP)
|
|
MOVP R4, R5, 48(RSP)
|
|
MOVP R6, R7, 64(RSP)
|
|
MOVP R8, R9, 80(RSP)
|
|
MOVP R10, R11, 96(RSP)
|
|
MOVP R12, R13, 112(RSP)
|
|
MOVP R14, R15, 128(RSP)
|
|
MOVP R16, R17, 144(RSP)
|
|
MOVP R18, R19, 160(RSP)
|
|
MOVP R20, R21, 176(RSP)
|
|
MOVP R22, R23, 192(RSP)
|
|
MOVP R24, R25, 208(RSP)
|
|
|
|
MOV R30, 256(RSP) // link
|
|
|
|
MOV $(1<<32), R0 // type irq
|
|
|
|
_virqpatch:
|
|
B _virqpatch // branch to vtrapX() patched in
|
|
|
|
TEXT vfiq(SB), 1, $-4
|
|
SUB $TRAPFRAMESIZE, RSP
|
|
|
|
MOVP R0, R1, 16(RSP)
|
|
MOVP R2, R3, 32(RSP)
|
|
MOVP R4, R5, 48(RSP)
|
|
MOVP R6, R7, 64(RSP)
|
|
MOVP R8, R9, 80(RSP)
|
|
MOVP R10, R11, 96(RSP)
|
|
MOVP R12, R13, 112(RSP)
|
|
MOVP R14, R15, 128(RSP)
|
|
MOVP R16, R17, 144(RSP)
|
|
MOVP R18, R19, 160(RSP)
|
|
MOVP R20, R21, 176(RSP)
|
|
MOVP R22, R23, 192(RSP)
|
|
MOVP R24, R25, 208(RSP)
|
|
|
|
MOV R30, 256(RSP) // link
|
|
MOV $(2<<32), R0 // type fiq
|
|
|
|
_vfiqpatch:
|
|
B _vfiqpatch // branch to vtrapX() patched in
|
|
|
|
TEXT vserr(SB), 1, $-4
|
|
SUB $TRAPFRAMESIZE, RSP
|
|
|
|
MOVP R0, R1, 16(RSP)
|
|
MOVP R2, R3, 32(RSP)
|
|
MOVP R4, R5, 48(RSP)
|
|
MOVP R6, R7, 64(RSP)
|
|
MOVP R8, R9, 80(RSP)
|
|
MOVP R10, R11, 96(RSP)
|
|
MOVP R12, R13, 112(RSP)
|
|
MOVP R14, R15, 128(RSP)
|
|
MOVP R16, R17, 144(RSP)
|
|
MOVP R18, R19, 160(RSP)
|
|
MOVP R20, R21, 176(RSP)
|
|
MOVP R22, R23, 192(RSP)
|
|
MOVP R24, R25, 208(RSP)
|
|
|
|
MOV R30, 256(RSP) // link
|
|
|
|
MRS ESR_EL1, R0
|
|
ORR $(3<<32), R0 // type
|
|
_vserrpatch:
|
|
B _vserrpatch // branch to vtrapX() patched in
|
|
|
|
/* fault-proof memcpy */
|
|
TEXT peek(SB), 1, $-4
|
|
MOV R0, R1
|
|
MOV dst+8(FP), R2
|
|
MOVWU len+16(FP), R0
|
|
TEXT _peekinst(SB), 1, $-4
|
|
_peekloop:
|
|
MOVBU (R1)1!, R3
|
|
MOVBU R3, (R2)1!
|
|
SUBS $1, R0
|
|
BNE _peekloop
|
|
RETURN
|