585 lines
10 KiB
ArmAsm
585 lines
10 KiB
ArmAsm
#include "mem.h"
|
|
|
|
/* use of SPRG registers in save/restore */
|
|
#define SAVER0 SPRG0
|
|
#define SAVER1 SPRG1
|
|
#define SAVELR SPRG2
|
|
#define SAVEXX SPRG3
|
|
|
|
/* special instruction definitions */
|
|
#define BDNZ BC 16,0,
|
|
#define BDNE BC 0,2,
|
|
|
|
#define TLBIA WORD $((31<<26)|(307<<1))
|
|
#define TLBSYNC WORD $((31<<26)|(566<<1))
|
|
|
|
/* on some models mtmsr doesn't synchronise enough (eg, 603e) */
|
|
#define MSRSYNC SYNC; ISYNC
|
|
|
|
#define UREGSPACE (UREGSIZE+8)
|
|
|
|
TEXT start(SB), $-4
|
|
|
|
/*
|
|
* setup MSR
|
|
* turn off interrupts
|
|
* use 0x000 as exception prefix
|
|
* enable machine check
|
|
*/
|
|
MOVW MSR, R3
|
|
MOVW $(MSR_EE|MSR_IP), R4
|
|
ANDN R4, R3
|
|
OR $(MSR_ME), R3
|
|
ISYNC
|
|
MOVW R3, MSR
|
|
MSRSYNC
|
|
|
|
/* except during trap handling, R0 is zero from now on */
|
|
MOVW $0, R0
|
|
|
|
/* setup SB for pre mmu */
|
|
MOVW $setSB(SB), R2
|
|
MOVW $KZERO, R3
|
|
ANDN R3, R2
|
|
|
|
BL mmuinit0(SB)
|
|
|
|
/* running with MMU on!! */
|
|
|
|
/* set R2 to correct value */
|
|
MOVW $setSB(SB), R2
|
|
|
|
/* debugger sets R1 to top of usable memory +1 */
|
|
MOVW R1, memsize(SB)
|
|
|
|
BL kfpinit(SB)
|
|
|
|
/* set up Mach */
|
|
MOVW $mach0(SB), R(MACH)
|
|
ADD $(MACHSIZE-8), R(MACH), R1 /* set stack */
|
|
|
|
MOVW R0, R(USER)
|
|
MOVW R0, 0(R(MACH))
|
|
|
|
BL main(SB)
|
|
|
|
RETURN /* not reached */
|
|
|
|
GLOBL mach0(SB), $(MAXMACH*BY2PG)
|
|
GLOBL memsize(SB), $4
|
|
|
|
/*
|
|
* on return from this function we will be running in virtual mode.
|
|
* We set up the Block Address Translation (BAT) registers thus:
|
|
* 1) first 3 BATs are 256M blocks, starting from KZERO->0
|
|
* 2) remaining BAT maps last 256M directly
|
|
*/
|
|
TEXT mmuinit0(SB), $0
|
|
/* reset all the tlbs */
|
|
MOVW $64, R3
|
|
MOVW R3, CTR
|
|
MOVW $0, R4
|
|
tlbloop:
|
|
TLBIE R4
|
|
ADD $BIT(19), R4
|
|
BDNZ tlbloop
|
|
TLBSYNC
|
|
|
|
/* KZERO -> 0 */
|
|
MOVW $(KZERO|(0x7ff<<2)|2), R3
|
|
MOVW $(PTEVALID|PTEWRITE), R4
|
|
MOVW R3, SPR(IBATU(0))
|
|
MOVW R4, SPR(IBATL(0))
|
|
MOVW R3, SPR(DBATU(0))
|
|
MOVW R4, SPR(DBATL(0))
|
|
|
|
/* KZERO+256M -> 256M */
|
|
ADD $(1<<28), R3
|
|
ADD $(1<<28), R4
|
|
MOVW R3, SPR(IBATU(1))
|
|
MOVW R4, SPR(IBATL(1))
|
|
MOVW R3, SPR(DBATU(1))
|
|
MOVW R4, SPR(DBATL(1))
|
|
|
|
/* KZERO+512M -> 512M */
|
|
ADD $(1<<28), R3
|
|
ADD $(1<<28), R4
|
|
MOVW R3, SPR(IBATU(2))
|
|
MOVW R4, SPR(IBATL(2))
|
|
MOVW R3, SPR(DBATU(2))
|
|
MOVW R4, SPR(DBATL(2))
|
|
|
|
/* direct map last block, uncached, (?guarded) */
|
|
MOVW $((0xf<<28)|(0x7ff<<2)|2), R3
|
|
MOVW $((0xf<<28)|PTE1_I|PTE1_G|PTE1_RW), R4
|
|
MOVW R3, SPR(DBATU(3))
|
|
MOVW R4, SPR(DBATL(3))
|
|
|
|
/* IBAT 3 unused */
|
|
MOVW R0, SPR(IBATU(3))
|
|
MOVW R0, SPR(IBATL(3))
|
|
|
|
/* enable MMU */
|
|
MOVW LR, R3
|
|
OR $KZERO, R3
|
|
MOVW R3, SPR(SRR0)
|
|
MOVW MSR, R4
|
|
OR $(MSR_IR|MSR_DR), R4
|
|
MOVW R4, SPR(SRR1)
|
|
RFI /* resume in kernel mode in caller */
|
|
|
|
RETURN
|
|
|
|
TEXT kfpinit(SB), $0
|
|
MOVFL $0,FPSCR(7)
|
|
MOVFL $0xD,FPSCR(6) /* VE, OE, ZE */
|
|
MOVFL $0, FPSCR(5)
|
|
MOVFL $0, FPSCR(3)
|
|
MOVFL $0, FPSCR(2)
|
|
MOVFL $0, FPSCR(1)
|
|
MOVFL $0, FPSCR(0)
|
|
|
|
FMOVD $4503601774854144.0, F27
|
|
FMOVD $0.5, F29
|
|
FSUB F29, F29, F28
|
|
FADD F29, F29, F30
|
|
FADD F30, F30, F31
|
|
FMOVD F28, F0
|
|
FMOVD F28, F1
|
|
FMOVD F28, F2
|
|
FMOVD F28, F3
|
|
FMOVD F28, F4
|
|
FMOVD F28, F5
|
|
FMOVD F28, F6
|
|
FMOVD F28, F7
|
|
FMOVD F28, F8
|
|
FMOVD F28, F9
|
|
FMOVD F28, F10
|
|
FMOVD F28, F11
|
|
FMOVD F28, F12
|
|
FMOVD F28, F13
|
|
FMOVD F28, F14
|
|
FMOVD F28, F15
|
|
FMOVD F28, F16
|
|
FMOVD F28, F17
|
|
FMOVD F28, F18
|
|
FMOVD F28, F19
|
|
FMOVD F28, F20
|
|
FMOVD F28, F21
|
|
FMOVD F28, F22
|
|
FMOVD F28, F23
|
|
FMOVD F28, F24
|
|
FMOVD F28, F25
|
|
FMOVD F28, F26
|
|
RETURN
|
|
|
|
TEXT splhi(SB), $0
|
|
MOVW LR, R31
|
|
MOVW R31, 4(R(MACH)) /* save PC in m->splpc */
|
|
MOVW MSR, R3
|
|
RLWNM $0, R3, $~MSR_EE, R4
|
|
SYNC
|
|
MOVW R4, MSR
|
|
MSRSYNC
|
|
RETURN
|
|
|
|
TEXT splx(SB), $0
|
|
/* fall though */
|
|
|
|
TEXT splxpc(SB), $0
|
|
MOVW LR, R31
|
|
MOVW R31, 4(R(MACH)) /* save PC in m->splpc */
|
|
MOVW MSR, R4
|
|
RLWMI $0, R3, $MSR_EE, R4
|
|
SYNC
|
|
MOVW R4, MSR
|
|
MSRSYNC
|
|
RETURN
|
|
|
|
TEXT spllo(SB), $0
|
|
MOVW MSR, R3
|
|
OR $MSR_EE, R3, R4
|
|
SYNC
|
|
MOVW R4, MSR
|
|
MSRSYNC
|
|
RETURN
|
|
|
|
TEXT spldone(SB), $0
|
|
RETURN
|
|
|
|
TEXT islo(SB), $0
|
|
MOVW MSR, R3
|
|
RLWNM $0, R3, $MSR_EE, R3
|
|
RETURN
|
|
|
|
TEXT setlabel(SB), $-4
|
|
MOVW LR, R31
|
|
MOVW R1, 0(R3)
|
|
MOVW R31, 4(R3)
|
|
MOVW $0, R3
|
|
RETURN
|
|
|
|
TEXT gotolabel(SB), $-4
|
|
MOVW 4(R3), R31
|
|
MOVW R31, LR
|
|
MOVW 0(R3), R1
|
|
MOVW $1, R3
|
|
RETURN
|
|
|
|
TEXT touser(SB), $-4
|
|
MOVW $(UTZERO+32), R5 /* header appears in text */
|
|
MOVW $(MSR_EE|MSR_PR|MSR_ME|MSR_IR|MSR_DR|MSR_RI), R4
|
|
MOVW R4, SPR(SRR1)
|
|
MOVW R3, R1
|
|
MOVW R5, SPR(SRR0)
|
|
RFI
|
|
|
|
TEXT icflush(SB), $-4 /* icflush(virtaddr, count) */
|
|
MOVW n+4(FP), R4
|
|
RLWNM $0, R3, $~(CACHELINESZ-1), R5
|
|
SUB R5, R3
|
|
ADD R3, R4
|
|
ADD $(CACHELINESZ-1), R4
|
|
SRAW $CACHELINELOG, R4
|
|
MOVW R4, CTR
|
|
icf0: ICBI (R5)
|
|
ADD $CACHELINESZ, R5
|
|
BDNZ icf0
|
|
ISYNC
|
|
RETURN
|
|
|
|
TEXT dcflush(SB), $-4 /* dcflush(virtaddr, count) */
|
|
MOVW n+4(FP), R4
|
|
RLWNM $0, R3, $~(CACHELINESZ-1), R5
|
|
CMP R4, $0
|
|
BLE dcf1
|
|
SUB R5, R3
|
|
ADD R3, R4
|
|
ADD $(CACHELINESZ-1), R4
|
|
SRAW $CACHELINELOG, R4
|
|
MOVW R4, CTR
|
|
dcf0: DCBF (R5)
|
|
ADD $CACHELINESZ, R5
|
|
BDNZ dcf0
|
|
dcf1:
|
|
SYNC
|
|
RETURN
|
|
|
|
TEXT tas(SB), $0
|
|
TEXT _tas(SB), $0
|
|
SYNC
|
|
MOVW R3, R4
|
|
MOVW $0xdead,R5
|
|
tas1:
|
|
DCBF (R4) /* fix for 603x bug */
|
|
LWAR (R4), R3
|
|
CMP R3, $0
|
|
BNE tas0
|
|
STWCCC R5, (R4)
|
|
BNE tas1
|
|
tas0:
|
|
SYNC
|
|
ISYNC
|
|
RETURN
|
|
|
|
TEXT cmpswap(SB),$0 /* int cmpswap(long*, long, long) */
|
|
MOVW R3, R4 /* addr */
|
|
MOVW old+4(FP), R5
|
|
MOVW new+8(FP), R6
|
|
DCBF (R4) /* fix for 603x bug? */
|
|
LWAR (R4), R3
|
|
CMP R3, R5
|
|
BNE fail
|
|
STWCCC R6, (R4)
|
|
BNE fail
|
|
MOVW $1, R3
|
|
RETURN
|
|
fail:
|
|
MOVW $0, R3
|
|
RETURN
|
|
|
|
TEXT getpvr(SB), $0
|
|
MOVW SPR(PVR), R3
|
|
RETURN
|
|
|
|
TEXT getdec(SB), $0
|
|
MOVW SPR(DEC), R3
|
|
RETURN
|
|
|
|
TEXT putdec(SB), $0
|
|
MOVW R3, SPR(DEC)
|
|
RETURN
|
|
|
|
TEXT getdar(SB), $0
|
|
MOVW SPR(DAR), R3
|
|
RETURN
|
|
|
|
TEXT getdsisr(SB), $0
|
|
MOVW SPR(DSISR), R3
|
|
RETURN
|
|
|
|
TEXT getmsr(SB), $0
|
|
MOVW MSR, R3
|
|
RETURN
|
|
|
|
TEXT putmsr(SB), $0
|
|
SYNC
|
|
MOVW R3, MSR
|
|
MSRSYNC
|
|
RETURN
|
|
|
|
TEXT putsdr1(SB), $0
|
|
MOVW R3, SPR(SDR1)
|
|
RETURN
|
|
|
|
TEXT putsr(SB), $0
|
|
MOVW 4(FP), R4
|
|
MOVW R4, SEG(R3)
|
|
RETURN
|
|
|
|
TEXT gethid0(SB), $0
|
|
MOVW SPR(HID0), R3
|
|
RETURN
|
|
|
|
TEXT gethid1(SB), $0
|
|
MOVW SPR(HID1), R3
|
|
RETURN
|
|
|
|
TEXT puthid0(SB), $0
|
|
MOVW R3, SPR(HID0)
|
|
RETURN
|
|
|
|
TEXT puthid1(SB), $0
|
|
MOVW R3, SPR(HID1)
|
|
RETURN
|
|
|
|
TEXT eieio(SB), $0
|
|
EIEIO
|
|
RETURN
|
|
|
|
TEXT sync(SB), $0
|
|
SYNC
|
|
RETURN
|
|
|
|
TEXT tlbflushall(SB), $0
|
|
MOVW $64, R3
|
|
MOVW R3, CTR
|
|
MOVW $0, R4
|
|
tlbflushall0:
|
|
TLBIE R4
|
|
ADD $BIT(19), R4
|
|
BDNZ tlbflushall0
|
|
EIEIO
|
|
TLBSYNC
|
|
SYNC
|
|
RETURN
|
|
|
|
TEXT tlbflush(SB), $0
|
|
TLBIE R3
|
|
RETURN
|
|
|
|
TEXT gotopc(SB), $0
|
|
MOVW R3, CTR
|
|
MOVW LR, R31 /* for trace back */
|
|
BR (CTR)
|
|
|
|
/*
|
|
* traps force memory mapping off.
|
|
* the following code has been executed at the exception
|
|
* vector location
|
|
* MOVW R0, SPR(SAVER0)
|
|
* MOVW LR, R0
|
|
* MOVW R0, SPR(SAVELR)
|
|
* bl trapvec(SB)
|
|
*/
|
|
TEXT trapvec(SB), $-4
|
|
MOVW LR, R0
|
|
MOVW R1, SPR(SAVER1)
|
|
MOVW R0, SPR(SAVEXX) /* vector */
|
|
|
|
/* did we come from user space */
|
|
MOVW SPR(SRR1), R0
|
|
MOVW CR, R1
|
|
MOVW R0, CR
|
|
BC 4,17,ktrap
|
|
|
|
/* switch to kernel stack */
|
|
MOVW R1, CR
|
|
MOVW R2, R0
|
|
MOVW $setSB(SB), R2
|
|
RLWNM $0, R2, $~KZERO, R2 /* PADDR(setSB) */
|
|
MOVW $mach0(SB), R1 /* m-> */
|
|
RLWNM $0, R1, $~KZERO, R1 /* PADDR(m->) */
|
|
MOVW 8(R1), R1 /* m->proc */
|
|
RLWNM $0, R1, $~KZERO, R1 /* PADDR(m->proc) */
|
|
MOVW 8(R1), R1 /* m->proc->kstack */
|
|
RLWNM $0, R1, $~KZERO, R1 /* PADDR(m->proc->kstack) */
|
|
ADD $(KSTACK-UREGSIZE), R1
|
|
MOVW R0, R2
|
|
BL saveureg(SB)
|
|
BL trap(SB)
|
|
BR restoreureg
|
|
ktrap:
|
|
MOVW R1, CR
|
|
MOVW SPR(SAVER1), R1
|
|
RLWNM $0, R1, $~KZERO, R1 /* PADDR(R1) */
|
|
SUB $UREGSPACE, R1
|
|
BL saveureg(SB)
|
|
BL trap(SB)
|
|
BR restoreureg
|
|
|
|
/*
|
|
* enter with stack set and mapped.
|
|
* on return, SB (R2) has been set, and R3 has the Ureg*,
|
|
* the MMU has been re-enabled, kernel text and PC are in KSEG,
|
|
* R(MACH) has been set, and R0 contains 0.
|
|
*
|
|
*/
|
|
TEXT saveureg(SB), $-4
|
|
/*
|
|
* save state
|
|
*/
|
|
MOVMW R2, 48(R1) /* r2:r31 */
|
|
MOVW $setSB(SB), R2
|
|
RLWNM $0, R2, $~KZERO, R2 /* PADDR(setSB) */
|
|
MOVW $mach0(SB), R(MACH)
|
|
RLWNM $0, R(MACH), $~KZERO, R(MACH) /* PADDR(m->) */
|
|
MOVW 8(R(MACH)), R(USER)
|
|
MOVW $mach0(SB), R(MACH)
|
|
MOVW $setSB(SB), R2
|
|
MOVW SPR(SAVER1), R4
|
|
MOVW R4, 44(R1)
|
|
MOVW SPR(SAVER0), R5
|
|
MOVW R5, 40(R1)
|
|
MOVW CTR, R6
|
|
MOVW R6, 36(R1)
|
|
MOVW XER, R4
|
|
MOVW R4, 32(R1)
|
|
MOVW CR, R5
|
|
MOVW R5, 28(R1)
|
|
MOVW SPR(SAVELR), R6 /* LR */
|
|
MOVW R6, 24(R1)
|
|
/* pad at 20(R1) */
|
|
MOVW SPR(SRR0), R0
|
|
MOVW R0, 16(R1) /* old PC */
|
|
MOVW SPR(SRR1), R0
|
|
MOVW R0, 12(R1) /* old status */
|
|
MOVW SPR(SAVEXX), R0
|
|
MOVW R0, 8(R1) /* cause/vector */
|
|
ADD $8, R1, R3 /* Ureg* */
|
|
OR $KZERO, R3 /* fix ureg */
|
|
STWCCC R3, (R1) /* break any pending reservations */
|
|
MOVW $0, R0 /* compiler/linker expect R0 to be zero */
|
|
|
|
MOVW MSR, R5
|
|
OR $(MSR_IR|MSR_DR|MSR_FP|MSR_RI), R5 /* enable MMU */
|
|
MOVW R5, SPR(SRR1)
|
|
MOVW LR, R31
|
|
OR $KZERO, R31 /* return PC in KSEG0 */
|
|
MOVW R31, SPR(SRR0)
|
|
OR $KZERO, R1 /* fix stack pointer */
|
|
RFI /* returns to trap handler */
|
|
|
|
/*
|
|
* restore state from Ureg and return from trap/interrupt
|
|
*/
|
|
TEXT forkret(SB), $0
|
|
BR restoreureg
|
|
|
|
restoreureg:
|
|
MOVMW 48(R1), R2 /* r2:r31 */
|
|
/* defer R1 */
|
|
MOVW 40(R1), R0
|
|
MOVW R0, SPR(SAVER0)
|
|
MOVW 36(R1), R0
|
|
MOVW R0, CTR
|
|
MOVW 32(R1), R0
|
|
MOVW R0, XER
|
|
MOVW 28(R1), R0
|
|
MOVW R0, CR /* CR */
|
|
MOVW 24(R1), R0
|
|
MOVW R0, LR
|
|
/* pad, skip */
|
|
MOVW 16(R1), R0
|
|
MOVW R0, SPR(SRR0) /* old PC */
|
|
MOVW 12(R1), R0
|
|
MOVW R0, SPR(SRR1) /* old MSR */
|
|
/* cause, skip */
|
|
MOVW 44(R1), R1 /* old SP */
|
|
MOVW SPR(SAVER0), R0
|
|
RFI
|
|
|
|
TEXT fpsave(SB), $0
|
|
FMOVD F0, (0*8)(R3)
|
|
FMOVD F1, (1*8)(R3)
|
|
FMOVD F2, (2*8)(R3)
|
|
FMOVD F3, (3*8)(R3)
|
|
FMOVD F4, (4*8)(R3)
|
|
FMOVD F5, (5*8)(R3)
|
|
FMOVD F6, (6*8)(R3)
|
|
FMOVD F7, (7*8)(R3)
|
|
FMOVD F8, (8*8)(R3)
|
|
FMOVD F9, (9*8)(R3)
|
|
FMOVD F10, (10*8)(R3)
|
|
FMOVD F11, (11*8)(R3)
|
|
FMOVD F12, (12*8)(R3)
|
|
FMOVD F13, (13*8)(R3)
|
|
FMOVD F14, (14*8)(R3)
|
|
FMOVD F15, (15*8)(R3)
|
|
FMOVD F16, (16*8)(R3)
|
|
FMOVD F17, (17*8)(R3)
|
|
FMOVD F18, (18*8)(R3)
|
|
FMOVD F19, (19*8)(R3)
|
|
FMOVD F20, (20*8)(R3)
|
|
FMOVD F21, (21*8)(R3)
|
|
FMOVD F22, (22*8)(R3)
|
|
FMOVD F23, (23*8)(R3)
|
|
FMOVD F24, (24*8)(R3)
|
|
FMOVD F25, (25*8)(R3)
|
|
FMOVD F26, (26*8)(R3)
|
|
FMOVD F27, (27*8)(R3)
|
|
FMOVD F28, (28*8)(R3)
|
|
FMOVD F29, (29*8)(R3)
|
|
FMOVD F30, (30*8)(R3)
|
|
FMOVD F31, (31*8)(R3)
|
|
MOVFL FPSCR, F0
|
|
FMOVD F0, (32*8)(R3)
|
|
RETURN
|
|
|
|
TEXT fprestore(SB), $0
|
|
FMOVD (32*8)(R3), F0
|
|
MOVFL F0, FPSCR
|
|
FMOVD (0*8)(R3), F0
|
|
FMOVD (1*8)(R3), F1
|
|
FMOVD (2*8)(R3), F2
|
|
FMOVD (3*8)(R3), F3
|
|
FMOVD (4*8)(R3), F4
|
|
FMOVD (5*8)(R3), F5
|
|
FMOVD (6*8)(R3), F6
|
|
FMOVD (7*8)(R3), F7
|
|
FMOVD (8*8)(R3), F8
|
|
FMOVD (9*8)(R3), F9
|
|
FMOVD (10*8)(R3), F10
|
|
FMOVD (11*8)(R3), F11
|
|
FMOVD (12*8)(R3), F12
|
|
FMOVD (13*8)(R3), F13
|
|
FMOVD (14*8)(R3), F14
|
|
FMOVD (15*8)(R3), F15
|
|
FMOVD (16*8)(R3), F16
|
|
FMOVD (17*8)(R3), F17
|
|
FMOVD (18*8)(R3), F18
|
|
FMOVD (19*8)(R3), F19
|
|
FMOVD (20*8)(R3), F20
|
|
FMOVD (21*8)(R3), F21
|
|
FMOVD (22*8)(R3), F22
|
|
FMOVD (23*8)(R3), F23
|
|
FMOVD (24*8)(R3), F24
|
|
FMOVD (25*8)(R3), F25
|
|
FMOVD (26*8)(R3), F26
|
|
FMOVD (27*8)(R3), F27
|
|
FMOVD (28*8)(R3), F28
|
|
FMOVD (29*8)(R3), F29
|
|
FMOVD (30*8)(R3), F30
|
|
FMOVD (31*8)(R3), F31
|
|
RETURN
|