.include /mit/6.004/bsim/beta.uasm
.include /mit/6.004/bsim/lab8macros.uasm
.include /mit/6.004/bsim/lab8checkoff.uasm

.macro extract_field_sext (RA, M, N, RB) {
       SHLC(RA, 31-M, RB)       | Shift left, to mask out high bits
       SRAC(RB, 31-(M-N), RB)   | Shift right, to mask out low bits.
}


regs:   RESERVE(32)                     | Array used to store register contents

UI: 
        save_all_regs(regs)
 
        LD(xp,-4,r0)                    | illegal instruction
        extract_field(r0, 31, 26, r1)   | extract opcode, bits 31:26
        CMPEQC(r1,0b010000,r2)               | OPCODE
        BT(r2, ldb)                 | yes, handle the ldb instruction.
        CMPEQC(r1,0b010001,r2)               | OPCODE
        BT(r2, stb)                 | yes, handle the ldb instruction.

        LD(r31,regs,r0)                 | Its something else.  Restore regs
        LD(r31,regs+4,r1)               | we've used, and go to the system's
        LD(r31,regs+8,r2)               | Illegal Instruction handler.
        BR(_IllegalInstruction)

swapreg:
        extract_field(r0, 25, 21, r1)   | extract rc field from trapped instruction
        MULC(r1, 4, r1)                 | convert to byte offset into regs array
        extract_field(r0, 20, 16, r2)   | extract ra field from trapped instruction
        MULC(r2, 4, r2)                 | convert to byte offset into regs array
        LD(r1, regs, r3)                | r3 <- regs[rc]
        LD(r2, regs, r4)                | r4 <- regs[ra]
        ST(r4, regs, r1)                | regs[rc] <- old regs[ra]
        ST(r3, regs, r2)                | regs[ra] <- old regs[rc]
        
        restore_all_regs(regs)
        JMP(xp)
        
ldb: 
        extract_field(r0, 25, 21, r1)   | extract rc field from trapped instruction
        MULC(r1, 4, r1)                 | convert to byte offset into regs array
        extract_field(r0, 20, 16, r2)   | extract ra field from trapped instruction
        MULC(r2, 4, r2)                 | convert to byte offset into regs array
        extract_field_sext(r0, 15, 0, r3)
        
        LD(r2, regs, r4)                | r4 <- regs[ra]
        ADD(r3, r4, r3)   			    | r3 <- ea
        ANDC(r3, 0xFFFC, r4)			| 
        ANDC(r3, 0x0003, r3)
        MULC(r3, 8, r3)
        
        LD(r4, 0, r5)
        SHR(r5, r3, r5)
        ANDC(r5, 0xFF, r5)
        
        ST(r5, regs, r1)                | regs[rc] <- old regs[ra]
        
        restore_all_regs(regs)
        JMP(xp)

stb: 
        extract_field(r0, 25, 21, r1)   | extract rc field from trapped instruction
        MULC(r1, 4, r1)                 | convert to byte offset into regs array
        extract_field(r0, 20, 16, r2)   | extract ra field from trapped instruction
        MULC(r2, 4, r2)                 | convert to byte offset into regs array
        extract_field_sext(r0, 15, 0, r3)
        
        LD(r2, regs, r4)                | r4 <- regs[ra]
        ADD(r3, r4, r3)   			    | r3 <- ea
        ANDC(r3, 0xFFFC, r4)			| 
        ANDC(r3, 0x0003, r3)
        MULC(r3, 8, r3)
        
        LD(r4, 0, r5)
		CMOVE(0xFF, r6)
		SHL(r6, r3, r6)
		CMOVE(0xFFFF, r7)
		SHLC(r7, 16, r7)
		ADDC(r7, 0xFFFF, r7)
		XOR(r6,r7,r6)
		AND(r5,r6,r5)
        LD(r1, regs, r8)                | r8 <- regs[rc]
        ANDC(r8, 0xFF, r8)
        SHL(r8, r3, r8)
        ADD(r5, r8, r5)
        ST(r5, 0, r4)

        restore_all_regs(regs)
        JMP(xp)