Implement first instructions mfc0/sw/bne/sll

* The instruction decoding was based on the handy table in ps2tek [1]
while the instructions themselves were written based on the document
added.

* The implementation makes heavy use of bitfields and unions in C++ to
make accessing different bits/sections of registers easier and more
intuitive. You may also notice the frequent casting (uint32_t)(int16_t)
which might seem useless but is there to force the compiler to generate
instructions to sign extend the offsets.

[1] https://psi-rockin.github.io/ps2tek/#eeinstructiondecoding

include/cpu/cop0.hpp

+#pragma once
+
+/* All COP0 instructions have this */
+constexpr uint8_t COP0_OPCODE = 0b010000;
+
+union COP0Instr {
+    uint32_t value;
+    struct {
+        uint32_t input : 11; /* This is used differently depending on the action (look COP0Input) */
+        uint32_t action : 5; /* Perf or Debug or rd in MFC0 */
+        uint32_t rt : 5;
+        uint32_t type : 5;
+        uint32_t opcode : 6;
+    };
+
+    COP0Instr& operator=(uint32_t val)
+    {
+        value = val;
+        return *this;
+    }
+};
+
+union COP0Input {
+    uint32_t input;
+    struct {
+        uint32_t id : 3; /* Tells us which MF* instruction to execute */
+        uint32_t : 8;
+    } debug;
+    struct {
+        uint32_t source : 1; /* 0: MFPS 1: MFPC */ 
+        uint32_t reg : 5; /* The index of the PEV (Performance Event Specifier) or PC (Performace Counter) respectively */
+        uint32_t : 5;
+    } perf;
+
+    COP0Input& operator=(uint32_t val)
+    {
+        input = val;
+        return *this;
+    }
+};
+
+/* Instruction types */
+constexpr uint8_t COP0_MF0 = 0b00000;
+constexpr uint8_t COP0_MT0 = 0b00100;
+constexpr uint8_t COP0_C0 = 0b10000;
+
+/* The status register fields */
+union COP0Status {
+    uint32_t value;
+    struct {
+        uint32_t ie : 1; /* Interrupt Enable */
+        uint32_t exl : 1; /* Exception Level */
+        uint32_t erl : 1; /* Error Level */
+        uint32_t ksu : 2; /* Kernel/Supervisor/User Mode bits */
+        uint32_t : 5;
+        uint32_t im : 2; /* Int[1:0] signals */
+        uint32_t bem : 1; /* Bus Error Mask */
+        uint32_t : 2;
+        uint32_t im1 : 1; /* Internal timer interrupt  */
+        uint32_t eie : 1; /* Enable IE */
+        uint32_t edi : 1; /* EI/DI instruction Enable */
+        uint32_t ch : 1; /* Cache Hit */
+        uint32_t : 3;
+        uint32_t bev : 1; /* Location of TLB refill */
+        uint32_t dev : 1; /* Location of Performance counter */
+        uint32_t : 2;
+        uint32_t fr : 1; /* Additional floating point registers */
+        uint32_t : 1;
+        uint32_t cu : 4; /* Usability of each of the four coprocessors */
+    };
+};
+
+enum OperatingMode {
+    USER_MODE = 0b10,
+    SUPERVISOR_MODE = 0b01,
+    KERNEL_MODE = 0b00
+};
+
+/* The COP0 registers */
+union COP0 {
+	uint32_t regs[32];
+
+	struct {
+        uint32_t index;
+        uint32_t random;
+        uint32_t entry_lo0;
+        uint32_t entry_lo1;
+        uint32_t context;
+        uint32_t page_mask;
+        uint32_t wired;
+        uint32_t reserved0[1];
+        uint32_t bad_vaddr;
+        uint32_t count;
+        uint32_t entryhi;
+        uint32_t compare;
+        COP0Status status;
+        uint32_t cause;
+        uint32_t epc;
+        uint32_t prid;
+        uint32_t config;
+        uint32_t reserved1[6];
+        uint32_t bad_paddr;
+        uint32_t debug;
+        uint32_t perf;
+        uint32_t reserved2[2];
+        uint32_t tag_lo;
+        uint32_t tag_hi;
+        uint32_t error_epc;
+        uint32_t reserved3[1];
+	};
+
+    OperatingMode get_operating_mode()
+    {
+        if (status.exl || status.erl) /* Setting one of these enforces kernel mode */
+            return OperatingMode::KERNEL_MODE;
+        else
+            return (OperatingMode)status.ksu;
+    }
+};
\ No newline at end of file

include/cpu/ee.hpp

 #pragma once
 #include <cstdint>
+#include <cpu/cop0.hpp>
+
+/* Nice interface for instructions */
+union Instruction {
+    uint32_t value;
+    struct { /* Used when polling for the opcode */
+        uint32_t : 26;
+        uint32_t opcode : 6;
+    };
+    struct {
+        uint32_t immediate : 16;
+        uint32_t rt : 5;
+        uint32_t rs : 5;
+        uint32_t opcode : 6;
+    } i_type;
+    struct {
+        uint32_t target : 26;
+        uint32_t opcode : 6;
+    } j_type;
+    struct {
+        uint32_t funct : 6;
+        uint32_t sa : 5;
+        uint32_t rd : 5;
+        uint32_t rt : 5;
+        uint32_t rs : 5;
+        uint32_t opcode : 6;
+    } r_type;
+
+    Instruction& operator=(uint32_t val)
+    {
+        value = val;
+        return *this;
+    }
+};
+
+union Register {
+    unsigned __int128 value;
+    struct {
+        uint64_t quadword[2];
+    };
+    struct {
+        uint32_t doubleword[4];
+    };
+};
+
+constexpr uint8_t SPECIAL_OPCODE = 0b000000;
 
 class ComponentManager;
 
@@ -14,9 +60,20 @@ public:
     void reset_state();
     void fetch_instruction();
 
+    /* Opcodes */
+    void op_cop0(); void op_mfc0(); void op_sw();
+    void op_special(); void op_sll(); void op_slti();
+
 protected:
     ComponentManager* manager;
 
     /* Registers. */
+    Register gpr[32];
     uint32_t pc;
+    uint64_t hi0, hi1, lo0, lo1; /* Used to store multiplication and division results */
+    uint32_t sa; /* Specifies the shift amount used by the funnel shift instruction */
+    Instruction instr;
+
+    /* Coprocessors */
+    COP0 cop0;
 };
\ No newline at end of file

src/cpu/ee.cpp

 #include <cpu/ee.hpp>
 #include <common/manager.hpp>
+#include <bitset>
+#include <iostream>
 
 EmotionEngine::EmotionEngine(ComponentManager* parent)
 {
-    this->manager = parent;
+    manager = parent;
 
     /* Reset CPU state. */
-    this->reset_state();
+    reset_state();
 }
 
 void EmotionEngine::tick()
 {
     /* Fetch next instruction. */
-    this->fetch_instruction();
+    fetch_instruction();
 }
 
 void EmotionEngine::reset_state()
@@ -23,5 +25,98 @@ void EmotionEngine::reset_state()
 
 void EmotionEngine::fetch_instruction()
 {
+    instr.value = manager->read_memory<uint32_t>(pc);
+    std::cout << "PC: " << std::hex << pc << " Instruction: " << instr.value << '\n';
 
+    switch(instr.opcode)
+    {
+    case COP0_OPCODE: op_cop0(); break;
+    case SPECIAL_OPCODE: op_special(); break;
+    case 0b101011: op_sw(); break;
+    case 0b001010: op_slti(); break;
+    default:
+        std::cout << "Unimplemented opcode: " << std::bitset<6>(instr.opcode) << '\n';
+        std::abort();
+    }
+
+    pc += 4;
+}
+
+void EmotionEngine::op_cop0()
+{
+    /*  The manual says that COP0 is only accessible in kernel mode
+        There are exceptions but we will deal with those later.  */
+    if (cop0.get_operating_mode() != OperatingMode::KERNEL_MODE)
+        return;
+
+    COP0Instr cop0_instr{instr.value};
+	switch (cop0_instr.input)
+	{
+	case COP0_MF0:
+		if (cop0_instr.input == 0) 
+			op_mfc0();
+		break;
+	
+	default:
+		std::cout << "Unimplemented COP0 instruction: " << std::bitset<5>(cop0_instr.type) << '\n';
+		std::exit(1);
+	}
+}
+
+void EmotionEngine::op_mfc0()
+{
+	COP0Instr data{instr.value};
+    gpr[data.rt].value = cop0.regs[data.action];
+}
+
+void EmotionEngine::op_special()
+{
+    switch(instr.r_type.funct)
+    {
+    case 0b000000: op_sll(); break;
+    }
+}
+
+void EmotionEngine::op_sw()
+{
+    uint8_t base = instr.i_type.rs;
+    uint8_t rt = instr.i_type.rt;
+    uint16_t offset = instr.i_type.immediate;
+
+    uint32_t vaddr = (uint32_t)(int16_t)offset + gpr[base].doubleword[0];
+    if (vaddr & 0b11 != 0)
+        return; /* NOTE: SignalException (AddressError) */
+
+    uint32_t data = gpr[rt].doubleword[0];
+    manager->write_memory<uint32_t>(vaddr, data);
+}
+
+void EmotionEngine::op_sll()
+{
+    uint8_t rt = instr.r_type.rt;
+    uint8_t rd = instr.r_type.rd;
+    uint8_t sa = instr.r_type.sa;
+
+    uint32_t result = gpr[rt].doubleword[0] << sa;
+    gpr[rd].quadword[0] = (uint64_t)(int32_t)result;
+}
+
+void EmotionEngine::op_slti()
+{
+    uint8_t rs = instr.i_type.rs;
+    uint8_t rt = instr.i_type.rt;
+    uint16_t imm = instr.i_type.immediate;
+
+    gpr[rt].quadword[0] = gpr[rs].quadword[0] < (uint32_t)(int16_t)imm; 
+}
+
+void EmotionEngine::op_bne()
+{
+    uint8_t rt = instr.i_type.rt;
+    uint8_t rs = instr.i_type.rs;
+    uint16_t imm = instr.i_type.immediate;
+
+    uint32_t offset = (uint32_t)(int16_t)imm << 2;
+    if (gpr[rs].quadword[0] != gpr[rt].quadword[0])
+        pc += offset;
 }
\ No newline at end of file