#include "PR/ultraerror.h"
#include "PR/R4300.h"
#include "sys/asm.h"
#include "sys/regdef.h"

#define index a0
#define pm a1
#define vaddr a2
#define evenpaddr a3
#define oddpaddr 16(sp)
#define asid 20(sp)
.text
.set noreorder
LEAF(osMapTLB)
#if defined (_DEBUG) && defined (__sgi)
    bgez index, 1f
    nop
    b 2f
    nop
1:
    li t0, 0x1F
.set noat
    slt AT, index, t0
    bnez AT, 3f
    nop
.set at
2:
    move a2, a0
    li a0, ERR_OSMAPTLB_INDEX
    li a1, 1
    j __osError
    nop

3:
    lw t0, asid
    li t1, -1
.set noat
    slt AT, t0, t1
    beqz AT, 4f
    nop
.set at
    b 5f
    nop
4:
    li t1, 0xFF
.set noat
    slt AT, t1, t0
    beqz AT, 6f
    nop
.set at
5:
    move a2, t0
    li a0, ERR_OSMAPTLB_ASID
    li a1, 1
    j __osError
    nop
6:
.set reorder
#endif
    STAY2(mfc0 t0, C0_ENTRYHI)
    STAY2(mtc0 index, C0_INX)
    STAY2(mtc0 pm, C0_PAGEMASK)
.set noreorder
    lw t1, asid
    beq t1, -1, 7f
    li ta0, TLBLO_G
    li t2, TLBLO_NONCOHRNT | TLBLO_D | TLBLO_V
    b 8f
     or vaddr, vaddr, t1
.set reorder

7:
    li t2, TLBLO_NONCOHRNT | TLBLO_D | TLBLO_V | TLBLO_G
8:
    STAY2(mtc0 vaddr, C0_ENTRYHI)
    beq evenpaddr, -1, 9f
    #nop
    srl t3, evenpaddr, TLBLO_PFNSHIFT
    or t3, t3, t2
    STAY2(mtc0 t3, C0_ENTRYLO0)
    b 10f
    #nop
9:
    STAY2(mtc0 ta0, C0_ENTRYLO0)
10:
    lw t3, oddpaddr
    beq t3, -1, 11f
    #nop
    srl t3, t3, TLBLO_PFNSHIFT
    or t3, t3, t2
    STAY2(mtc0 t3, C0_ENTRYLO1)
    b 12f
    #nop
11:
    STAY2(mtc0 ta0, C0_ENTRYLO1)
    bne evenpaddr, -1, 12f
    #nop
    li t3, K0BASE
    STAY2(mtc0 t3, C0_ENTRYHI)
12:
    .set noreorder
    nop
    tlbwi
    nop
    nop
    nop
    nop
    STAY2(mtc0 t0, C0_ENTRYHI)
    jr ra
    #nop
END(osMapTLB)
