850 lines
14 KiB
ArmAsm
850 lines
14 KiB
ArmAsm
#include "mem.h"
|
||
|
||
#define SP R29
|
||
|
||
#define NOOP NOR R0, R0, R0
|
||
#define WAIT NOOP; NOOP
|
||
#define RETURN RET; NOOP
|
||
#define CONST(i, v) MOVW $((i) & 0xffff0000), v; OR $((i) & 0xffff), v;
|
||
#define GETMACH(r) CONST(MACHADDR, r)
|
||
|
||
/*
|
||
* R4000 instructions
|
||
*/
|
||
#define ERET WORD $0x42000018
|
||
#define LL(base, rt) WORD $((060<<26)|((base)<<21)|((rt)<<16))
|
||
#define SC(base, rt) WORD $((070<<26)|((base)<<21)|((rt)<<16))
|
||
|
||
#define MFC0(src,sel,dst) WORD $(0x40000000|((src)<<11)|((dst)<<16)|(sel))
|
||
#define MTC0(src,dst,sel) WORD $(0x40800000|((dst)<<11)|((src)<<16)|(sel))
|
||
#define RDHWR(hwr, r) WORD $(0x7c00003b|((hwr)<<11)|((r)<<16))
|
||
|
||
/*
|
||
* cache manipulation
|
||
*/
|
||
#define CACHE BREAK /* overloaded op-code */
|
||
|
||
#define PI R((0 /* primary I cache */
|
||
#define PD R((1 /* primary D cache */
|
||
#define SD R((3 /* secondary combined I/D cache */
|
||
|
||
#define IWBI (0<<2))) /* index write-back invalidate */
|
||
#define ILT (1<<2))) /* index load tag */
|
||
#define IST (2<<2))) /* index store tag */
|
||
#define CDE (3<<2))) /* create dirty exclusive */
|
||
#define HINV (4<<2))) /* hit invalidate */
|
||
#define HWBI (5<<2))) /* hit write back invalidate */
|
||
#define HWB (6<<2))) /* hit write back */
|
||
#define HSV (7<<2))) /* hit set virtual */
|
||
|
||
NOSCHED
|
||
|
||
/*
|
||
* Boot only processor
|
||
*/
|
||
|
||
TEXT start(SB), $-4
|
||
MOVW $setR30(SB), R30
|
||
|
||
MOVW $CU1, R1
|
||
MOVW R1, M(STATUS)
|
||
WAIT
|
||
|
||
MOVW $(0x1C<<7), R1
|
||
MOVW R1, FCR31 /* permit only inexact and underflow */
|
||
NOOP
|
||
MOVD $0.5, F26
|
||
SUBD F26, F26, F24
|
||
ADDD F26, F26, F28
|
||
ADDD F28, F28, F30
|
||
|
||
MOVD F24, F0
|
||
MOVD F24, F2
|
||
MOVD F24, F4
|
||
MOVD F24, F6
|
||
MOVD F24, F8
|
||
MOVD F24, F10
|
||
MOVD F24, F12
|
||
MOVD F24, F14
|
||
MOVD F24, F16
|
||
MOVD F24, F18
|
||
MOVD F24, F20
|
||
MOVD F24, F22
|
||
|
||
MOVW $MACHADDR, R(MACH)
|
||
ADDU $(MACHSIZE-BY2V), R(MACH), SP
|
||
|
||
MOVW R(MACH), R1
|
||
clrmach:
|
||
MOVW R0, (R1)
|
||
ADDU $BY2WD, R1
|
||
BNE R1, SP, clrmach
|
||
NOOP
|
||
|
||
MOVW $edata(SB), R1
|
||
MOVW $end(SB), R2
|
||
clrbss:
|
||
MOVB R0, (R1)
|
||
ADDU $1, R1
|
||
BNE R1, R2, clrbss
|
||
NOOP
|
||
|
||
MOVW R0, 0(R(MACH)) /* m->machno = 0 */
|
||
MOVW R0, R(USER) /* up = nil */
|
||
|
||
JAL main(SB)
|
||
NOOP
|
||
|
||
TEXT arcs(SB), $256
|
||
MOVW R24, 0x80(SP)
|
||
MOVW R25, 0x84(SP)
|
||
MOVW R26, 0x88(SP)
|
||
MOVW R27, 0x8C(SP)
|
||
|
||
MOVW $SPBADDR, R4
|
||
MOVW 0x20(R4), R5
|
||
ADDU R1, R5
|
||
MOVW (R5), R2
|
||
|
||
MOVW 16(FP), R7
|
||
MOVW 12(FP), R6
|
||
MOVW 8(FP), R5
|
||
MOVW 4(FP), R4
|
||
|
||
JAL (R2)
|
||
NOOP
|
||
|
||
MOVW $setR30(SB), R30
|
||
|
||
MOVW 0x80(SP), R24
|
||
MOVW 0x84(SP), R25
|
||
MOVW 0x88(SP), R26
|
||
MOVW 0x8C(SP), R27
|
||
|
||
MOVW R2, R1
|
||
RETURN
|
||
|
||
/*
|
||
* Take first processor into user mode
|
||
* - argument is stack pointer to user
|
||
*/
|
||
|
||
TEXT touser(SB), $-4
|
||
MOVW M(STATUS), R4
|
||
MOVW $(UTZERO+32), R2 /* header appears in text */
|
||
MOVW R2, M(EPC)
|
||
MOVW R1, SP
|
||
AND $(~KMODEMASK), R4
|
||
OR $(KUSER|IE|EXL), R4 /* switch to user mode, intrs on, exc */
|
||
MOVW R4, M(STATUS) /* " */
|
||
WAIT
|
||
ERET /* clears EXL */
|
||
NOOP
|
||
|
||
/*
|
||
* manipulate interrupts
|
||
*/
|
||
|
||
/* enable an interrupt; bit is in R1 */
|
||
TEXT intron(SB), $0
|
||
MOVW M(STATUS), R2
|
||
WAIT
|
||
OR R1, R2
|
||
MOVW R2, M(STATUS)
|
||
WAIT
|
||
RETURN
|
||
|
||
/* disable an interrupt; bit is in R1 */
|
||
TEXT introff(SB), $0
|
||
MOVW M(STATUS), R2
|
||
WAIT
|
||
XOR $-1, R1
|
||
AND R1, R2
|
||
MOVW R2, M(STATUS)
|
||
WAIT
|
||
RETURN
|
||
|
||
TEXT splhi(SB), $0
|
||
MOVW R31, 12(R(MACH)) /* save PC in m->splpc */
|
||
MOVW M(STATUS), R1
|
||
WAIT
|
||
AND $~IE, R1, R2
|
||
MOVW R2, M(STATUS)
|
||
WAIT
|
||
RETURN
|
||
|
||
TEXT splx(SB), $0
|
||
MOVW R31, 12(R(MACH)) /* save PC in m->splpc */
|
||
MOVW M(STATUS), R2
|
||
WAIT
|
||
AND $IE, R1
|
||
AND $~IE, R2
|
||
OR R2, R1
|
||
MOVW R1, M(STATUS)
|
||
WAIT
|
||
RETURN
|
||
|
||
TEXT spllo(SB), $0
|
||
MOVW M(STATUS), R1
|
||
WAIT
|
||
OR $IE, R1, R2
|
||
MOVW R2, M(STATUS)
|
||
WAIT
|
||
RETURN
|
||
|
||
TEXT spldone(SB), $0
|
||
RETURN
|
||
|
||
TEXT islo(SB), $0
|
||
MOVW M(STATUS), R1
|
||
WAIT
|
||
AND $IE, R1
|
||
RETURN
|
||
|
||
TEXT coherence(SB), $-4
|
||
RETURN
|
||
|
||
/*
|
||
* process switching
|
||
*/
|
||
|
||
TEXT setlabel(SB), $-4
|
||
MOVW SP, 0(R1)
|
||
MOVW R31, 4(R1)
|
||
MOVW R0, R1
|
||
RETURN
|
||
|
||
TEXT gotolabel(SB), $-4
|
||
MOVW 0(R1), SP
|
||
MOVW 4(R1), R31
|
||
MOVW $1, R1
|
||
RETURN
|
||
|
||
/*
|
||
* the tlb routines need to be called at splhi.
|
||
*/
|
||
|
||
TEXT getwired(SB),$0
|
||
MOVW M(WIRED), R1
|
||
RETURN
|
||
|
||
TEXT setwired(SB),$0
|
||
MOVW R1, M(WIRED)
|
||
RETURN
|
||
|
||
TEXT getrandom(SB),$0
|
||
MOVW M(RANDOM), R1
|
||
RETURN
|
||
|
||
TEXT getpagemask(SB),$0
|
||
MOVW M(PAGEMASK), R1
|
||
RETURN
|
||
|
||
TEXT setpagemask(SB),$0
|
||
MOVW R1, M(PAGEMASK)
|
||
MOVW R0, R1 /* prevent accidents */
|
||
RETURN
|
||
|
||
TEXT puttlbx(SB), $0 /* puttlbx(index, virt, phys0, phys1, pagemask) */
|
||
MOVW 4(FP), R2
|
||
MOVW 8(FP), R3
|
||
MOVW 12(FP), R4
|
||
MOVW $((2*BY2PG-1) & ~0x1fff), R5
|
||
MOVW R2, M(TLBVIRT)
|
||
MOVW R3, M(TLBPHYS0)
|
||
MOVW R4, M(TLBPHYS1)
|
||
MOVW R5, M(PAGEMASK)
|
||
MOVW R1, M(INDEX)
|
||
NOOP
|
||
NOOP
|
||
TLBWI
|
||
NOOP
|
||
RETURN
|
||
|
||
TEXT tlbvirt(SB), $0
|
||
MOVW M(TLBVIRT), R1
|
||
NOOP
|
||
RETURN
|
||
|
||
TEXT gettlbx(SB), $0 /* gettlbx(index, &entry) */
|
||
MOVW 4(FP), R4
|
||
MOVW R1, M(INDEX)
|
||
NOOP
|
||
NOOP
|
||
TLBR
|
||
NOOP
|
||
NOOP
|
||
NOOP
|
||
MOVW M(TLBVIRT), R1
|
||
MOVW M(TLBPHYS0), R2
|
||
MOVW M(TLBPHYS1), R3
|
||
NOOP
|
||
MOVW R1, 0(R4)
|
||
MOVW R2, 4(R4)
|
||
MOVW R3, 8(R4)
|
||
RETURN
|
||
|
||
TEXT gettlbp(SB), $0 /* gettlbp(tlbvirt, &entry) */
|
||
MOVW 4(FP), R5
|
||
MOVW R1, M(TLBVIRT)
|
||
NOOP
|
||
NOOP
|
||
NOOP
|
||
TLBP
|
||
NOOP
|
||
NOOP
|
||
MOVW M(INDEX), R1
|
||
NOOP
|
||
BLTZ R1, gettlbp1
|
||
TLBR
|
||
NOOP
|
||
NOOP
|
||
NOOP
|
||
MOVW M(TLBVIRT), R2
|
||
MOVW M(TLBPHYS0), R3
|
||
MOVW M(TLBPHYS1), R4
|
||
NOOP
|
||
MOVW R2, 0(R5)
|
||
MOVW R3, 4(R5)
|
||
MOVW R4, 8(R5)
|
||
gettlbp1:
|
||
RETURN
|
||
|
||
TEXT gettlbvirt(SB), $0 /* gettlbvirt(index) */
|
||
MOVW R1, M(INDEX)
|
||
NOOP
|
||
NOOP
|
||
TLBR
|
||
NOOP
|
||
NOOP
|
||
NOOP
|
||
MOVW M(TLBVIRT), R1
|
||
NOOP
|
||
RETURN
|
||
|
||
/*
|
||
* compute stlb hash index.
|
||
*
|
||
* M(TLBVIRT) [page & asid] in arg, result in arg.
|
||
* stir in swizzled asid; we get best results with asid in both high & low bits.
|
||
*/
|
||
#define STLBHASH(arg, tmp) \
|
||
AND $0xFF, arg, tmp; \
|
||
SRL $(PGSHIFT+1), arg; \
|
||
XOR tmp, arg; \
|
||
SLL $(STLBLOG-8), tmp; \
|
||
XOR tmp, arg; \
|
||
CONST (STLBSIZE-1, tmp); \
|
||
AND tmp, arg
|
||
|
||
TEXT stlbhash(SB), $0 /* for mmu.c */
|
||
STLBHASH(R1, R2)
|
||
RETURN
|
||
|
||
TEXT utlbmiss(SB), $-4
|
||
GETMACH (R26)
|
||
MOVW R27, 12(R26) /* m->splpc = R27 */
|
||
|
||
MOVW 16(R26), R27
|
||
ADDU $1, R27
|
||
MOVW R27,16(R26) /* m->tlbfault++ */
|
||
|
||
MOVW M(TLBVIRT), R27
|
||
NOOP
|
||
STLBHASH(R27, R26)
|
||
|
||
/* scale to a byte index (multiply by 12) */
|
||
SLL $1, R27, R26 /* × 2 */
|
||
ADDU R26, R27 /* × 3 */
|
||
SLL $2, R27 /* × 12 */
|
||
|
||
GETMACH (R26)
|
||
MOVW 4(R26), R26
|
||
ADDU R26, R27 /* R27 = &m->stb[hash] */
|
||
|
||
MOVW M(BADVADDR), R26
|
||
NOOP
|
||
AND $BY2PG, R26
|
||
|
||
BNE R26, utlbodd /* odd page? */
|
||
NOOP
|
||
|
||
utlbeven:
|
||
MOVW 4(R27), R26 /* R26 = m->stb[hash].phys0 */
|
||
BEQ R26, stlbm /* nothing cached? do it the hard way */
|
||
NOOP
|
||
MOVW R26, M(TLBPHYS0)
|
||
MOVW 8(R27), R26 /* R26 = m->stb[hash].phys1 */
|
||
JMP utlbcom
|
||
MOVW R26, M(TLBPHYS1) /* branch delay slot */
|
||
|
||
utlbodd:
|
||
MOVW 8(R27), R26 /* R26 = m->stb[hash].phys1 */
|
||
BEQ R26, stlbm /* nothing cached? do it the hard way */
|
||
NOOP
|
||
MOVW R26, M(TLBPHYS1)
|
||
MOVW 4(R27), R26 /* R26 = m->stb[hash].phys0 */
|
||
MOVW R26, M(TLBPHYS0)
|
||
|
||
utlbcom:
|
||
WAIT
|
||
MOVW M(TLBVIRT), R26
|
||
MOVW 0(R27), R27 /* R27 = m->stb[hash].virt */
|
||
BEQ R27, stlbm /* nothing cached? do it the hard way */
|
||
NOOP
|
||
/* is the stlb entry for the right virtual address? */
|
||
BNE R26, R27, stlbm /* M(TLBVIRT) != m->stb[hash].virt? */
|
||
NOOP
|
||
|
||
/* if an entry exists, overwrite it, else write a random one */
|
||
CONST (PGSZ, R27)
|
||
MOVW R27, M(PAGEMASK) /* select page size */
|
||
TLBP /* probe tlb */
|
||
NOOP
|
||
NOOP
|
||
MOVW M(INDEX), R26
|
||
NOOP
|
||
BGEZ R26, utlbindex /* if tlb entry found, rewrite it */
|
||
NOOP
|
||
MOVW M(RANDOM), R26
|
||
MOVW R26, M(INDEX)
|
||
utlbindex:
|
||
NOOP
|
||
NOOP
|
||
TLBWI /* write indexed tlb entry */
|
||
NOOP
|
||
|
||
utlbret:
|
||
GETMACH (R26)
|
||
MOVW 12(R26), R27 /* R27 = m->splpc */
|
||
MOVW M(EPC), R26
|
||
JMP (R27)
|
||
NOOP
|
||
|
||
stlbm:
|
||
GETMACH (R26)
|
||
MOVW 12(R26), R27 /* R27 = m->splpc */
|
||
|
||
/* fall through */
|
||
|
||
TEXT gevector(SB), $-4
|
||
MOVW M(STATUS), R26
|
||
WAIT
|
||
AND $KUSER, R26
|
||
|
||
BNE R26, wasuser
|
||
MOVW SP, R26 /* delay slot, old SP in R26 */
|
||
|
||
waskernel:
|
||
JMP dosave
|
||
SUBU $UREGSIZE, SP /* delay slot, allocate frame on kernel stack */
|
||
|
||
wasuser: /* get kernel stack for this user process */
|
||
GETMACH (SP)
|
||
MOVW 8(SP), SP /* m->proc */
|
||
MOVW 8(SP), SP /* m->proc->kstack */
|
||
ADDU $(KSTACK-UREGSIZE), SP
|
||
|
||
dosave:
|
||
MOVW R31, 0x28(SP)
|
||
|
||
JAL saveregs(SB)
|
||
MOVW R26, 0x10(SP) /* delay slot, save old SP */
|
||
|
||
GETMACH (R(MACH))
|
||
MOVW 8(R(MACH)), R(USER) /* R24 = m->proc */
|
||
MOVW $setR30(SB), R30
|
||
|
||
BEQ R26, dosys /* set by saveregs() */
|
||
NOOP
|
||
|
||
dotrap:
|
||
MOVW $forkret(SB), R31
|
||
JMP trap(SB)
|
||
MOVW 4(SP), R1 /* delay slot, first arg to trap() */
|
||
|
||
dosys:
|
||
JAL syscall(SB)
|
||
MOVW 4(SP), R1 /* delay slot, first arg to syscall() */
|
||
|
||
/* fall through */
|
||
|
||
TEXT forkret(SB), $-4
|
||
JAL restregs(SB) /* restores old PC in R26 */
|
||
MOVW 0x14(SP), R1 /* delay slot, CAUSE */
|
||
|
||
MOVW 0x28(SP), R31
|
||
|
||
JMP (R27)
|
||
MOVW 0x10(SP), SP /* delay slot */
|
||
|
||
/*
|
||
* SP-> 0x00 --- (spill R31)
|
||
* 0x04 --- (trap()/syscall() arg1)
|
||
* 0x08 status
|
||
* 0x0C pc
|
||
* 0x10 sp/usp
|
||
* 0x14 cause
|
||
* 0x18 badvaddr
|
||
* 0x1C tlbvirt
|
||
* 0x20 hi
|
||
* 0x24 lo
|
||
* 0x28 r31
|
||
* .....
|
||
* 0x9c r1
|
||
*/
|
||
|
||
TEXT saveregs(SB), $-4
|
||
MOVW R1, 0x9C(SP)
|
||
MOVW R2, 0x98(SP)
|
||
MOVW M(STATUS), R2
|
||
ADDU $8, SP, R1
|
||
MOVW R1, 0x04(SP) /* arg to base of regs */
|
||
MOVW $~KMODEMASK, R1
|
||
AND R2, R1
|
||
MOVW R1, M(STATUS) /* so we can take another trap */
|
||
MOVW R2, 0x08(SP)
|
||
MOVW M(EPC), R2
|
||
MOVW M(CAUSE), R1
|
||
MOVW R2, 0x0C(SP)
|
||
MOVW R1, 0x14(SP)
|
||
AND $(EXCMASK<<2), R1
|
||
SUBU $(CSYS<<2), R1, R26
|
||
|
||
BEQ R26, notsaved /* is syscall? */
|
||
MOVW R27, 0x34(SP) /* delay slot */
|
||
|
||
MOVW M(BADVADDR), R1
|
||
MOVW M(TLBVIRT), R2
|
||
MOVW R1, 0x18(SP)
|
||
MOVW R2, 0x1C(SP)
|
||
|
||
MOVW HI, R1
|
||
MOVW LO, R2
|
||
MOVW R1, 0x20(SP)
|
||
MOVW R2, 0x24(SP)
|
||
|
||
MOVW R25, 0x3C(SP)
|
||
MOVW R24, 0x40(SP)
|
||
MOVW R23, 0x44(SP)
|
||
MOVW R22, 0x48(SP)
|
||
MOVW R21, 0x4C(SP)
|
||
MOVW R20, 0x50(SP)
|
||
MOVW R19, 0x54(SP)
|
||
MOVW R18, 0x58(SP)
|
||
MOVW R17, 0x5C(SP)
|
||
MOVW R16, 0x60(SP)
|
||
MOVW R15, 0x64(SP)
|
||
MOVW R14, 0x68(SP)
|
||
MOVW R13, 0x6C(SP)
|
||
MOVW R12, 0x70(SP)
|
||
MOVW R11, 0x74(SP)
|
||
MOVW R10, 0x78(SP)
|
||
MOVW R9, 0x7C(SP)
|
||
MOVW R8, 0x80(SP)
|
||
MOVW R7, 0x84(SP)
|
||
MOVW R6, 0x88(SP)
|
||
MOVW R5, 0x8C(SP)
|
||
MOVW R4, 0x90(SP)
|
||
MOVW R3, 0x94(SP)
|
||
|
||
notsaved:
|
||
MOVW R30, 0x2C(SP)
|
||
|
||
RET
|
||
MOVW R28, 0x30(SP) /* delay slot */
|
||
|
||
TEXT restregs(SB), $-4
|
||
AND $(EXCMASK<<2), R1
|
||
SUBU $(CSYS<<2), R1, R26
|
||
|
||
BEQ R26, notrestored /* is syscall? */
|
||
MOVW 0x34(SP), R27 /* delay slot */
|
||
|
||
MOVW 0x3C(SP), R25
|
||
MOVW 0x40(SP), R24
|
||
MOVW 0x44(SP), R23
|
||
MOVW 0x48(SP), R22
|
||
MOVW 0x4C(SP), R21
|
||
MOVW 0x50(SP), R20
|
||
MOVW 0x54(SP), R19
|
||
MOVW 0x58(SP), R18
|
||
MOVW 0x5C(SP), R17
|
||
MOVW 0x60(SP), R16
|
||
MOVW 0x64(SP), R15
|
||
MOVW 0x68(SP), R14
|
||
MOVW 0x6C(SP), R13
|
||
MOVW 0x70(SP), R12
|
||
MOVW 0x74(SP), R11
|
||
MOVW 0x78(SP), R10
|
||
MOVW 0x7C(SP), R9
|
||
MOVW 0x80(SP), R8
|
||
MOVW 0x84(SP), R7
|
||
MOVW 0x88(SP), R6
|
||
MOVW 0x8C(SP), R5
|
||
MOVW 0x90(SP), R4
|
||
MOVW 0x94(SP), R3
|
||
|
||
MOVW 0x24(SP), R2
|
||
MOVW 0x20(SP), R1
|
||
MOVW R2, LO
|
||
MOVW R1, HI
|
||
|
||
MOVW 0x98(SP), R2
|
||
|
||
notrestored:
|
||
MOVW 0x08(SP), R1
|
||
MOVW R1, M(STATUS)
|
||
MOVW 0x0C(SP), R26 /* old PC */
|
||
MOVW R26, M(EPC)
|
||
|
||
MOVW 0x30(SP), R28
|
||
MOVW 0x2C(SP), R30
|
||
|
||
RET
|
||
MOVW 0x9C(SP), R1 /* delay slot */
|
||
|
||
/*
|
||
* hardware interrupt vectors
|
||
*/
|
||
|
||
TEXT vector0(SB), $-4
|
||
WAIT
|
||
CONST (SPBADDR+0x18, R26)
|
||
MOVW $eret(SB), R27
|
||
MOVW (R26), R26
|
||
JMP (R26)
|
||
NOOP
|
||
|
||
TEXT vector180(SB), $-4
|
||
WAIT
|
||
CONST (SPBADDR+0x14, R26)
|
||
MOVW $eret(SB), R27
|
||
MOVW (R26), R26
|
||
JMP (R26)
|
||
NOOP
|
||
|
||
TEXT eret(SB), $-4
|
||
ERET
|
||
NOOP
|
||
|
||
/*
|
||
* floating-point stuff
|
||
*/
|
||
|
||
TEXT clrfpintr(SB), $0
|
||
MOVW M(STATUS), R3
|
||
WAIT
|
||
OR $CU1, R3
|
||
MOVW R3, M(STATUS)
|
||
NOOP
|
||
NOOP
|
||
NOOP
|
||
|
||
MOVW FCR31, R1
|
||
MOVW R1, R2
|
||
AND $~(0x3F<<12), R2
|
||
MOVW R2, FCR31
|
||
|
||
AND $~CU1, R3
|
||
MOVW R3, M(STATUS)
|
||
WAIT
|
||
RETURN
|
||
|
||
TEXT savefpregs(SB), $0
|
||
MOVW FCR31, R2
|
||
MOVW M(STATUS), R3
|
||
WAIT
|
||
AND $~(0x3F<<12), R2, R4
|
||
MOVW R4, FCR31
|
||
|
||
MOVD F0, 0x00(R1)
|
||
MOVD F2, 0x08(R1)
|
||
MOVD F4, 0x10(R1)
|
||
MOVD F6, 0x18(R1)
|
||
MOVD F8, 0x20(R1)
|
||
MOVD F10, 0x28(R1)
|
||
MOVD F12, 0x30(R1)
|
||
MOVD F14, 0x38(R1)
|
||
MOVD F16, 0x40(R1)
|
||
MOVD F18, 0x48(R1)
|
||
MOVD F20, 0x50(R1)
|
||
MOVD F22, 0x58(R1)
|
||
MOVD F24, 0x60(R1)
|
||
MOVD F26, 0x68(R1)
|
||
MOVD F28, 0x70(R1)
|
||
MOVD F30, 0x78(R1)
|
||
|
||
MOVW R2, 0x80(R1)
|
||
AND $~CU1, R3
|
||
MOVW R3, M(STATUS)
|
||
WAIT
|
||
RETURN
|
||
|
||
TEXT restfpregs(SB), $0
|
||
MOVW M(STATUS), R3
|
||
WAIT
|
||
OR $CU1, R3
|
||
MOVW R3, M(STATUS)
|
||
WAIT
|
||
MOVW fpstat+4(FP), R2
|
||
NOOP
|
||
|
||
MOVD 0x00(R1), F0
|
||
MOVD 0x08(R1), F2
|
||
MOVD 0x10(R1), F4
|
||
MOVD 0x18(R1), F6
|
||
MOVD 0x20(R1), F8
|
||
MOVD 0x28(R1), F10
|
||
MOVD 0x30(R1), F12
|
||
MOVD 0x38(R1), F14
|
||
MOVD 0x40(R1), F16
|
||
MOVD 0x48(R1), F18
|
||
MOVD 0x50(R1), F20
|
||
MOVD 0x58(R1), F22
|
||
MOVD 0x60(R1), F24
|
||
MOVD 0x68(R1), F26
|
||
MOVD 0x70(R1), F28
|
||
MOVD 0x78(R1), F30
|
||
|
||
MOVW R2, FCR31
|
||
AND $~CU1, R3
|
||
MOVW R3, M(STATUS)
|
||
WAIT
|
||
RETURN
|
||
|
||
TEXT fcr31(SB), $0 /* fp csr */
|
||
MOVW FCR31, R1
|
||
RETURN
|
||
|
||
/*
|
||
* Emulate 68020 test and set: load linked / store conditional
|
||
*/
|
||
|
||
TEXT tas(SB), $0
|
||
TEXT _tas(SB), $0
|
||
MOVW R1, R2 /* address of key */
|
||
tas1:
|
||
MOVW $1, R3
|
||
LL(2, 1)
|
||
NOOP
|
||
SC(2, 3)
|
||
NOOP
|
||
BEQ R3, tas1
|
||
NOOP
|
||
RETURN
|
||
|
||
/* used by the semaphore implementation */
|
||
TEXT cmpswap(SB), $0
|
||
MOVW R1, R2 /* address of key */
|
||
MOVW old+4(FP), R3 /* old value */
|
||
MOVW new+8(FP), R4 /* new value */
|
||
LL(2, 1) /* R1 = (R2) */
|
||
NOOP
|
||
BNE R1, R3, fail
|
||
NOOP
|
||
MOVW R4, R1
|
||
SC(2, 1) /* (R2) = R1 if (R2) hasn't changed; R1 = success */
|
||
NOOP
|
||
RETURN
|
||
fail:
|
||
MOVW R0, R1
|
||
RETURN
|
||
|
||
/*
|
||
* cache manipulation
|
||
*/
|
||
|
||
TEXT icflush(SB), $-4 /* icflush(virtaddr, count) */
|
||
MOVW M(STATUS), R10
|
||
WAIT
|
||
MOVW 4(FP), R9
|
||
MOVW $0, M(STATUS)
|
||
WAIT
|
||
ADDU R1, R9 /* R9 = last address */
|
||
MOVW $(~0x3f), R8
|
||
AND R1, R8 /* R8 = first address, rounded down */
|
||
ADDU $0x3f, R9
|
||
AND $(~0x3f), R9 /* round last address up */
|
||
SUBU R8, R9 /* R9 = revised count */
|
||
icflush1: /* primary cache line size is 16 bytes */
|
||
CACHE PD+HWB, 0x00(R8)
|
||
CACHE PI+HINV, 0x00(R8)
|
||
CACHE PD+HWB, 0x10(R8)
|
||
CACHE PI+HINV, 0x10(R8)
|
||
CACHE PD+HWB, 0x20(R8)
|
||
CACHE PI+HINV, 0x20(R8)
|
||
CACHE PD+HWB, 0x30(R8)
|
||
CACHE PI+HINV, 0x30(R8)
|
||
SUBU $0x40, R9
|
||
ADDU $0x40, R8
|
||
BGTZ R9, icflush1
|
||
MOVW R10, M(STATUS)
|
||
WAIT
|
||
RETURN
|
||
|
||
TEXT dcflush(SB), $-4 /* dcflush(virtaddr, count) */
|
||
MOVW M(STATUS), R10
|
||
WAIT
|
||
MOVW 4(FP), R9
|
||
MOVW $0, M(STATUS)
|
||
WAIT
|
||
ADDU R1, R9 /* R9 = last address */
|
||
MOVW $(~0x3f), R8
|
||
AND R1, R8 /* R8 = first address, rounded down */
|
||
ADDU $0x3f, R9
|
||
AND $(~0x3f), R9 /* round last address up */
|
||
SUBU R8, R9 /* R9 = revised count */
|
||
dcflush1: /* primary cache line size is 16 bytes */
|
||
CACHE PD+HWB, 0x00(R8)
|
||
CACHE PD+HWB, 0x10(R8)
|
||
CACHE PD+HWB, 0x20(R8)
|
||
CACHE PD+HWB, 0x30(R8)
|
||
SUBU $0x40, R9
|
||
ADDU $0x40, R8
|
||
BGTZ R9, dcflush1
|
||
MOVW R10, M(STATUS)
|
||
WAIT
|
||
RETURN
|
||
|
||
TEXT outl(SB), $0
|
||
MOVW 4(FP), R2
|
||
MOVW 8(FP), R3
|
||
SLL $2, R3
|
||
ADDU R2, R3
|
||
outl1:
|
||
BEQ R2, R3, outl2
|
||
MOVW (R2), R4
|
||
MOVW R4, (R1)
|
||
JMP outl1
|
||
ADDU $4, R2
|
||
outl2:
|
||
RETURN
|
||
|
||
/*
|
||
* access to CP0 registers
|
||
*/
|
||
|
||
TEXT prid(SB), $0
|
||
MOVW M(PRID), R1
|
||
WAIT
|
||
RETURN
|
||
|
||
TEXT rdcount(SB), $0
|
||
MOVW M(COUNT), R1
|
||
RETURN
|
||
|
||
TEXT wrcount(SB), $0
|
||
MOVW R1, M(COUNT)
|
||
RETURN
|
||
|
||
TEXT wrcompare(SB), $0
|
||
MOVW R1, M(COMPARE)
|
||
RETURN
|
||
|
||
TEXT rdcompare(SB), $0
|
||
MOVW M(COMPARE), R1
|
||
RETURN
|
||
|
||
SCHED
|