Implemented coprocessor 0 registers access and register EPC and Cause set by exception.

Signed-off-by: Pavel Pisa <pisa@cmp.felk.cvut.cz>

11 files changed, 531 insertions, 23 deletions

diff --git a/qtmips_machine/alu.cpp b/qtmips_machine/alu.cpp
index 83c94bd..3889b44 100644
--- a/qtmips_machine/alu.cpp
+++ b/qtmips_machine/alu.cpp
@@ -253,13 +253,15 @@ std::uint32_t machine::alu_operate(enum AluOp operation, std::uint32_t s,
             return alu_op_clz(s);
         case ALU_OP_CLO:
            return alu_op_clz(~s);
-        case  ALU_OP_PASS_T: // Pass s argument without change for JAL
+        case ALU_OP_PASS_T: // Pass s argument without change for JAL
             return t;
         case ALU_OP_BREAK:
-            return 0;
-        case  ALU_OP_SYSCALL:
-            return 0;
-        case  ALU_OP_RDHWR:
+        case ALU_OP_SYSCALL:
+        case ALU_OP_RDHWR:
+        case ALU_OP_MTC0:
+        case ALU_OP_MFC0:
+        case ALU_OP_MFMC0:
+        case ALU_OP_ERET:
             return 0;
         default:
             throw QTMIPS_EXCEPTION(UnsupportedAluOperation, "Unknown ALU operation", QString::number(operation, 16));
diff --git a/qtmips_machine/cop0state.cpp b/qtmips_machine/cop0state.cpp
new file mode 100644
index 0000000..ea937ce
--- /dev/null
+++ b/qtmips_machine/cop0state.cpp
@@ -0,0 +1,180 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*******************************************************************************
+ * QtMips - MIPS 32-bit Architecture Subset Simulator
+ *
+ * Implemented to support following courses:
+ *
+ *   B35APO - Computer Architectures
+ *   https://cw.fel.cvut.cz/wiki/courses/b35apo
+ *
+ *   B4M35PAP - Advanced Computer Architectures
+ *   https://cw.fel.cvut.cz/wiki/courses/b4m35pap/start
+ *
+ * Copyright (c) 2017-2019 Karel Koci<cynerd@email.cz>
+ * Copyright (c) 2019      Pavel Pisa <pisa@cmp.felk.cvut.cz>
+ *
+ * Faculty of Electrical Engineering (http://www.fel.cvut.cz)
+ * Czech Technical University        (http://www.cvut.cz/)
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor,
+ * Boston, MA  02110-1301, USA.
+ *
+ ******************************************************************************/
+
+#include "cop0state.h"
+#include "machinedefs.h"
+#include "core.h"
+#include "qtmipsexception.h"
+
+using namespace machine;
+
+
+// sorry, unimplemented: non-trivial designated initializers not supported
+
+static enum Cop0State::Cop0Regsisters cop0reg_map[32][8] = {
+    /*0*/ {},
+    /*1*/ {},
+    /*2*/ {},
+    /*3*/ {},
+    /*4*/ {Cop0State::Unsupported, Cop0State::Unsupported, Cop0State::UserLocal},
+    /*5*/ {},
+    /*6*/ {},
+    /*7*/ {},
+    /*8*/ {Cop0State::BadVAddr},
+    /*9*/ {Cop0State::Count},
+    /*10*/ {},
+    /*11*/ {Cop0State::Compare},
+    /*12*/ {Cop0State::Status},
+    /*13*/ {Cop0State::Cause},
+    /*14*/ {Cop0State::EPC},
+    /*15*/ {Cop0State::Unsupported, Cop0State::EBase},
+    /*16*/ {Cop0State::Config},
+    /*17*/ {},
+    /*18*/ {},
+    /*19*/ {},
+    /*20*/ {},
+    /*21*/ {},
+    /*22*/ {},
+    /*23*/ {},
+    /*24*/ {},
+    /*25*/ {},
+    /*26*/ {},
+    /*27*/ {},
+    /*28*/ {},
+    /*29*/ {},
+    /*30*/ {},
+    /*31*/ {},
+};
+
+// sorry, unimplemented: non-trivial designated initializers not supported
+
+const Cop0State::cop0reg_desc_t Cop0State::cop0reg_desc[Cop0State::COP0REGS_CNT] = {
+    [Cop0State::Unsupported] = {"Unsupported", 0x00000000,
+        &Cop0State::read_cop0reg_default, &Cop0State::write_cop0reg_default},
+    [Cop0State::UserLocal] = {"UserLocal", 0xffffffff,
+        &Cop0State::read_cop0reg_default, &Cop0State::write_cop0reg_default},
+    [Cop0State::BadVAddr] = {"BadVAddr", 0x00000000,
+        &Cop0State::read_cop0reg_default, &Cop0State::write_cop0reg_default},
+    [Cop0State::Count] =    {"Count", 0x00000000,
+        &Cop0State::read_cop0reg_default, &Cop0State::write_cop0reg_default},
+    [Cop0State::Compare] =  {"Compare", 0x00000000,
+        &Cop0State::read_cop0reg_default, &Cop0State::write_cop0reg_default},
+    [Cop0State::Status] =   {"Status", 0x00000000,
+        &Cop0State::read_cop0reg_default, &Cop0State::write_cop0reg_default},
+    [Cop0State::Cause] =    {"Cause", 0x00000000,
+        &Cop0State::read_cop0reg_default, &Cop0State::write_cop0reg_default},
+    [Cop0State::EPC] =      {"EPC", 0xffffffff,
+        &Cop0State::read_cop0reg_default, &Cop0State::write_cop0reg_default},
+    [Cop0State::EBase] =    {"EBase", 0xfffffffc,
+        &Cop0State::read_cop0reg_default, &Cop0State::write_cop0reg_default},
+    [Cop0State::Config] =   {"Config", 0x00000000,
+        &Cop0State::read_cop0reg_default, &Cop0State::write_cop0reg_default},
+};
+
+Cop0State::Cop0State(Core *core) : QObject() {
+    this->core = core;
+    reset();
+}
+
+Cop0State::Cop0State(const Cop0State &orig) : QObject() {
+    this->core = orig.core;
+    for (int i = 0; i < COP0REGS_CNT; i++)
+        this->cop0reg[i] = orig.read_cop0reg((enum Cop0Regsisters)i);
+}
+
+void Cop0State::setup_core(Core *core) {
+    this->core = core;
+}
+
+std::uint32_t Cop0State::read_cop0reg(std::uint8_t rd, std::uint8_t sel) const {
+    SANITY_ASSERT(rd < 32, QString("Trying to read from cop0 register ") + QString(rd) + ',' + QString(sel));
+    SANITY_ASSERT(sel < 8, QString("Trying to read from cop0 register ") + QString(rd) + ',' + QString(sel));
+    enum Cop0Regsisters reg = cop0reg_map[rd][sel];
+    SANITY_ASSERT(reg != 0, QString("Cop0 register ") + QString(rd) + ',' + QString(sel) + "unsupported");
+    return read_cop0reg(reg);
+}
+
+void Cop0State::write_cop0reg(std::uint8_t rd, std::uint8_t sel, std::uint32_t value) {
+    SANITY_ASSERT(rd < 32, QString("Trying to write to cop0 register ") + QString(rd) + ',' + QString(sel));
+    SANITY_ASSERT(sel < 8, QString("Trying to write to cop0 register ") + QString(rd) + ',' + QString(sel));
+    enum Cop0Regsisters reg = cop0reg_map[rd][sel];
+    SANITY_ASSERT(reg != 0, QString("Cop0 register ") + QString(rd) + ',' + QString(sel) + "unsupported");
+    write_cop0reg(reg, value);
+}
+
+std::uint32_t Cop0State::read_cop0reg(enum Cop0Regsisters reg) const {
+    SANITY_ASSERT(reg < COP0REGS_CNT, QString("Trying to read from cop0 register ") + QString(reg));
+    return cop0reg[(int)reg];
+}
+
+void Cop0State::write_cop0reg(enum Cop0Regsisters reg, std::uint32_t value) {
+    SANITY_ASSERT(reg < COP0REGS_CNT, QString("Trying to write to cop0 register ") + QString(reg));
+    cop0reg[(int)reg] = value;
+}
+
+std::uint32_t Cop0State::read_cop0reg_default(enum Cop0Regsisters reg) const {
+    return cop0reg[(int)reg];
+}
+
+void Cop0State::write_cop0reg_default(enum Cop0Regsisters reg, std::uint32_t value) {
+    std::uint32_t mask = cop0reg_desc[(int)reg].write_mask;
+    cop0reg[(int)reg] = (value & mask) | (cop0reg[(int)reg] & ~mask);
+}
+
+bool Cop0State::operator==(const Cop0State &c) const {
+    for (int i = 0; i < COP0REGS_CNT; i++)
+        if (read_cop0reg((enum Cop0Regsisters)i) != c.read_cop0reg((enum Cop0Regsisters)i))
+            return false;
+    return true;
+}
+
+bool Cop0State::operator!=(const Cop0State &c) const {
+    return ! this->operator==(c);
+}
+
+void Cop0State::reset() {
+    for (int i = 0; i < COP0REGS_CNT; i++)
+        this->cop0reg[i] = 0;
+}
+
+void Cop0State::update_execption_cause(enum ExceptionCause excause, bool in_delay_slot) {
+    if (in_delay_slot)
+        cop0reg[(int)Cause] |= 0x80000000;
+    else
+        cop0reg[(int)Cause] &= ~0x80000000;
+    cop0reg[(int)Cause] &= ~0x0000007f;
+    if (excause != EXCAUSE_INT)
+        cop0reg[(int)Cause] |= (int)excause << 2;
+}
diff --git a/qtmips_machine/cop0state.h b/qtmips_machine/cop0state.h
new file mode 100644
index 0000000..880dea2
--- /dev/null
+++ b/qtmips_machine/cop0state.h
@@ -0,0 +1,108 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*******************************************************************************
+ * QtMips - MIPS 32-bit Architecture Subset Simulator
+ *
+ * Implemented to support following courses:
+ *
+ *   B35APO - Computer Architectures
+ *   https://cw.fel.cvut.cz/wiki/courses/b35apo
+ *
+ *   B4M35PAP - Advanced Computer Architectures
+ *   https://cw.fel.cvut.cz/wiki/courses/b4m35pap/start
+ *
+ * Copyright (c) 2017-2019 Karel Koci<cynerd@email.cz>
+ * Copyright (c) 2019      Pavel Pisa <pisa@cmp.felk.cvut.cz>
+ *
+ * Faculty of Electrical Engineering (http://www.fel.cvut.cz)
+ * Czech Technical University        (http://www.cvut.cz/)
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor,
+ * Boston, MA  02110-1301, USA.
+ *
+ ******************************************************************************/
+
+#ifndef COP0STATE_H
+#define COP0STATE_H
+
+#include <QObject>
+#include <cstdint>
+#include <machinedefs.h>
+namespace machine {
+
+class Core;
+
+class Cop0State : public QObject {
+    Q_OBJECT
+    friend class Core;
+public:
+    enum Cop0Regsisters {
+        Unsupported = 0,
+        UserLocal,
+        BadVAddr, // Reports the address for the most recent address-related exception
+        Count,    // Processor cycle count
+        Compare,  // Timer interrupt control
+        Status,   // Processor status and control
+        Cause,    // Cause of last exception
+        EPC,      // Program counter at last exception
+        EBase,    // Exception vector base register
+        Config,   // Configuration registers
+        COP0REGS_CNT,
+    };
+
+    Cop0State(Core *core = nullptr);
+    Cop0State(const Cop0State&);
+
+    std::uint32_t read_cop0reg(enum Cop0Regsisters reg) const;
+    std::uint32_t read_cop0reg(std::uint8_t rd, std::uint8_t sel) const; // Read coprocessor 0 register
+    void write_cop0reg(enum Cop0Regsisters reg, std::uint32_t value);
+    void write_cop0reg(std::uint8_t reg, std::uint8_t sel, std::uint32_t value); // Write coprocessor 0 register
+
+    bool operator ==(const Cop0State &c) const;
+    bool operator !=(const Cop0State &c) const;
+
+    void reset(); // Reset all values to zero
+
+protected:
+    void setup_core(Core *core);
+    void update_execption_cause(enum ExceptionCause excause, bool in_delay_slot);
+
+private:
+
+    typedef std::uint32_t (Cop0State::*reg_read_t)
+                  (enum Cop0Regsisters reg) const;
+
+    typedef void (Cop0State::*reg_write_t)
+                  (enum Cop0Regsisters reg, std::uint32_t value);
+
+    struct cop0reg_desc_t {
+        const char *name;
+        std::uint32_t write_mask;
+        reg_read_t reg_read;
+        reg_write_t reg_write;
+    };
+
+    static const cop0reg_desc_t cop0reg_desc[COP0REGS_CNT];
+
+    std::uint32_t read_cop0reg_default(enum Cop0Regsisters reg) const;
+    void write_cop0reg_default(enum Cop0Regsisters reg, std::uint32_t value);
+    Core *core;
+    std::uint32_t cop0reg[COP0REGS_CNT]; // coprocessor 0 registers
+};
+
+}
+
+Q_DECLARE_METATYPE(machine::Cop0State)
+
+#endif // COP0STATE_H
diff --git a/qtmips_machine/core.cpp b/qtmips_machine/core.cpp
index f290659..b845360 100644
--- a/qtmips_machine/core.cpp
+++ b/qtmips_machine/core.cpp
@@ -40,14 +40,18 @@
 using namespace machine;
 
 Core::Core(Registers *regs, MemoryAccess *mem_program, MemoryAccess *mem_data,
-           unsigned int min_cache_row_size) : ex_handlers(), hw_breaks() {
+           unsigned int min_cache_row_size, Cop0State *cop0state) :
+           ex_handlers(), hw_breaks() {
     cycle_c = 0;
     this->regs = regs;
+    this->cop0state = cop0state;
     this->mem_program = mem_program;
     this->mem_data = mem_data;
     this->ex_default_handler = new StopExceptionHandler();
     this->min_cache_row_size = min_cache_row_size;
     this->hwr_userlocal = 0xe0000000;
+    if (cop0state != nullptr)
+        cop0state->setup_core(this);
 }
 
 void Core::step(bool skip_break) {
@@ -68,6 +72,11 @@ Registers *Core::get_regs() {
     return regs;
 }
 
+Cop0State *Core::get_cop0state() {
+    return cop0state;
+}
+
+
 MemoryAccess *Core::get_mem_data() {
     return mem_data;
 }
@@ -123,6 +132,14 @@ bool Core::handle_exception(Core *core, Registers *regs, ExceptionCause excause,
             regs->pc_abs_jmp(inst_addr);
     }
 
+    if (cop0state != nullptr) {
+        if (in_delay_slot)
+            cop0state->write_cop0reg(Cop0State::EPC, jump_branch_pc);
+        else
+            cop0state->write_cop0reg(Cop0State::EPC, inst_addr);
+        cop0state->update_execption_cause(excause, in_delay_slot);
+    }
+
     ExceptionHandler *exhandler = ex_handlers.value(excause);
     if (exhandler != nullptr)
         return exhandler->handle_exception(core, regs, excause, inst_addr,
@@ -316,6 +333,7 @@ struct Core::dtDecode Core::decode(const struct dtFetch &dt) {
         .excause = excause,
         .in_delay_slot = dt.in_delay_slot,
         .stall = false,
+        .stop_if = !!(flags & IMF_STOP_IF),
     };
 }
 
@@ -338,7 +356,8 @@ struct Core::dtExecute Core::execute(const struct dtDecode &dt) {
         if (discard)
             regwrite = false;
 
-        if (dt.aluop == ALU_OP_RDHWR) {
+        switch (dt.aluop) {
+        case ALU_OP_RDHWR:
             switch (dt.num_rd) {
             case 0: // CPUNum
                 alu_val = 0;
@@ -358,6 +377,31 @@ struct Core::dtExecute Core::execute(const struct dtDecode &dt) {
             default:
                 alu_val = 0;
             }
+            break;
+        case ALU_OP_MTC0:
+            if (cop0state == nullptr)
+                throw QTMIPS_EXCEPTION(UnsupportedInstruction, "Cop0 not supported", "setup Cop0State");
+            cop0state->write_cop0reg(dt.num_rd, dt.inst.cop0sel(), dt.val_rt);
+            break;
+        case ALU_OP_MFC0:
+            if (cop0state == nullptr)
+                throw QTMIPS_EXCEPTION(UnsupportedInstruction, "Cop0 not supported", "setup Cop0State");
+            alu_val = cop0state->read_cop0reg(dt.num_rd, dt.inst.cop0sel());
+            break;
+        case ALU_OP_MFMC0:
+            if (cop0state == nullptr)
+                throw QTMIPS_EXCEPTION(UnsupportedInstruction, "Cop0 not supported", "setup Cop0State");
+            alu_val = cop0state->read_cop0reg(dt.num_rd, dt.inst.cop0sel());
+            if (dt.inst.funct() & 0x20)
+                cop0state->write_cop0reg(dt.num_rd, dt.inst.cop0sel(), dt.val_rt | 1);
+            else
+                cop0state->write_cop0reg(dt.num_rd, dt.inst.cop0sel(), dt.val_rt & ~1);
+            break;
+        case ALU_OP_ERET:
+            regs->pc_abs_jmp(cop0state->read_cop0reg(Cop0State::EPC));
+            break;
+        default:
+            break;
         }
     }
 
@@ -398,6 +442,7 @@ struct Core::dtExecute Core::execute(const struct dtDecode &dt) {
         .inst_addr = dt.inst_addr,
         .excause = excause,
         .in_delay_slot = dt.in_delay_slot,
+        .stop_if = dt.stop_if,
     };
 }
 
@@ -449,6 +494,7 @@ struct Core::dtMemory Core::memory(const struct dtExecute &dt) {
         .inst_addr = dt.inst_addr,
         .excause = dt.excause,
         .in_delay_slot = dt.in_delay_slot,
+        .stop_if = dt.stop_if,
     };
 }
 
@@ -542,6 +588,7 @@ void Core::dtDecodeInit(struct dtDecode &dt) {
     dt.excause = EXCAUSE_NONE;
     dt.in_delay_slot = false;
     dt.stall = false;
+    dt.stop_if = false;
 }
 
 void Core::dtExecuteInit(struct dtExecute &dt) {
@@ -555,6 +602,7 @@ void Core::dtExecuteInit(struct dtExecute &dt) {
     dt.alu_val = 0;
     dt.excause = EXCAUSE_NONE;
     dt.in_delay_slot = false;
+    dt.stop_if = false;
 }
 
 void Core::dtMemoryInit(struct dtMemory &dt) {
@@ -566,10 +614,12 @@ void Core::dtMemoryInit(struct dtMemory &dt) {
     dt.mem_addr = 0;
     dt.excause = EXCAUSE_NONE;
     dt.in_delay_slot = false;
+    dt.stop_if = false;
 }
 
-CoreSingle::CoreSingle(Registers *regs, MemoryAccess *mem_program, MemoryAccess *mem_data, bool jmp_delay_slot) : \
-    Core(regs, mem_program, mem_data) {
+CoreSingle::CoreSingle(Registers *regs, MemoryAccess *mem_program, MemoryAccess *mem_data,
+                       bool jmp_delay_slot, unsigned int min_cache_row_size, Cop0State *cop0state) :
+    Core(regs, mem_program, mem_data, min_cache_row_size, cop0state) {
     if (jmp_delay_slot)
         jmp_delay_decode = new struct Core::dtDecode();
     else
@@ -624,8 +674,10 @@ void CoreSingle::do_reset() {
     }
 }
 
-CorePipelined::CorePipelined(Registers *regs, MemoryAccess *mem_program, MemoryAccess *mem_data, enum MachineConfig::HazardUnit hazard_unit) : \
-    Core(regs, mem_program, mem_data) {
+CorePipelined::CorePipelined(Registers *regs, MemoryAccess *mem_program, MemoryAccess *mem_data,
+                             enum MachineConfig::HazardUnit hazard_unit,
+                             unsigned int min_cache_row_size, Cop0State *cop0state) :
+    Core(regs, mem_program, mem_data, min_cache_row_size, cop0state) {
     this->hazard_unit = hazard_unit;
     reset();
 }
@@ -757,10 +809,13 @@ void CorePipelined::do_step(bool skip_break) {
     printf("PC 0x%08lx\n", (unsigned long)dt_f.inst_addr);
 #endif
 
+    if (dt_e.stop_if || dt_m.stop_if)
+        stall = true;
+
     emit hu_stall_value(stall);
 
     // Now process program counter (loop connections from decode stage)
-    if (!stall) {
+    if (!stall && !dt_d.stop_if) {
         dt_d.stall = false;
         dt_f = fetch(skip_break);
         if (handle_pc(dt_d)) {
@@ -775,8 +830,12 @@ void CorePipelined::do_step(bool skip_break) {
         // Run fetch stage on empty
         fetch(skip_break);
         // clear decode latch (insert nope to execute stage)
-        dtDecodeInit(dt_d);
-        dt_d.stall = true;
+        if (!dt_d.stop_if) {
+            dtDecodeInit(dt_d);
+            dt_d.stall = true;
+        } else {
+            dtFetchInit(dt_f);
+        }
         // emit instruction_decoded(dt_d.inst, dt_d.inst_addr, dt_d.excause);
     }
 }
diff --git a/qtmips_machine/core.h b/qtmips_machine/core.h
index 1c44550..d5ac4e2 100644
--- a/qtmips_machine/core.h
+++ b/qtmips_machine/core.h
@@ -40,6 +40,7 @@
 #include <qtmipsexception.h>
 #include <machineconfig.h>
 #include <registers.h>
+#include <cop0state.h>
 #include <memory.h>
 #include <instruction.h>
 #include <alu.h>
@@ -70,7 +71,7 @@ class Core : public QObject {
     Q_OBJECT
 public:
     Core(Registers *regs, MemoryAccess *mem_program, MemoryAccess *mem_data,
-         unsigned int min_cache_row_size = 1);
+         unsigned int min_cache_row_size = 1, Cop0State *cop0state = nullptr);
 
     void step(bool skip_break = false); // Do single step
     void reset(); // Reset core (only core, memory and registers has to be reseted separately)
@@ -78,6 +79,7 @@ public:
     unsigned cycles(); // Returns number of executed cycles
 
     Registers *get_regs();
+    Cop0State *get_cop0state();
     MemoryAccess *get_mem_data();
     MemoryAccess *get_mem_program();
     void register_exception_handler(ExceptionCause excause, ExceptionHandler *exhandler);
@@ -170,6 +172,7 @@ protected:
                      bool in_delay_slot, std::uint32_t mem_ref_addr);
 
     Registers *regs;
+    Cop0State *cop0state;
     MemoryAccess *mem_data, *mem_program;
     QMap<ExceptionCause, ExceptionHandler *> ex_handlers;
     ExceptionHandler *ex_default_handler;
@@ -214,6 +217,7 @@ protected:
         enum ExceptionCause excause;
         bool in_delay_slot;
         bool stall;
+        bool stop_if;
     };
     struct dtExecute {
         Instruction inst;
@@ -227,6 +231,7 @@ protected:
         uint32_t inst_addr; // Address of instruction
         enum ExceptionCause excause;
         bool in_delay_slot;
+        bool stop_if;
     };
     struct dtMemory {
         Instruction inst;
@@ -238,6 +243,7 @@ protected:
         uint32_t inst_addr; // Address of instruction
         enum ExceptionCause excause;
         bool in_delay_slot;
+        bool stop_if;
     };
 
     struct dtFetch fetch(bool skip_break = false);
@@ -273,7 +279,8 @@ private:
 
 class CoreSingle : public Core {
 public:
-    CoreSingle(Registers *regs, MemoryAccess *mem_program, MemoryAccess *mem_data, bool jmp_delay_slot);
+    CoreSingle(Registers *regs, MemoryAccess *mem_program, MemoryAccess *mem_data, bool jmp_delay_slot,
+               unsigned int min_cache_row_size = 1, Cop0State *cop0state = nullptr);
     ~CoreSingle();
 
 protected:
@@ -286,7 +293,9 @@ private:
 
 class CorePipelined : public Core {
 public:
-    CorePipelined(Registers *regs, MemoryAccess *mem_program, MemoryAccess *mem_data, enum MachineConfig::HazardUnit hazard_unit = MachineConfig::HU_STALL_FORWARD);
+    CorePipelined(Registers *regs, MemoryAccess *mem_program, MemoryAccess *mem_data,
+                  enum MachineConfig::HazardUnit hazard_unit = MachineConfig::HU_STALL_FORWARD,
+                  unsigned int min_cache_row_size = 1, Cop0State *cop0state = nullptr);
 
 protected:
     void do_step(bool skip_break = false);
diff --git a/qtmips_machine/instruction.cpp b/qtmips_machine/instruction.cpp
index 41cfb5c..3b5f567 100644
--- a/qtmips_machine/instruction.cpp
+++ b/qtmips_machine/instruction.cpp
@@ -437,7 +437,129 @@ static const struct InstructionMap  regimm_instruction_map[] = {
     IM_UNKNOWN,
     IM_UNKNOWN,
     {"SYNCI", IT_I,  ALU_OP_ADDU, AC_CACHE_OP, nullptr, // SYNCI
-     .flags = IMF_SUPPORTED | IMF_BJR_REQ_RS},
+     .flags = IMF_SUPPORTED | IMF_STOP_IF | IMF_BJR_REQ_RS},
+};
+
+static const struct InstructionMap  cop0_func_instruction_map[] = {
+    IM_UNKNOWN,	//	0
+    IM_UNKNOWN,	//	1
+    IM_UNKNOWN,	//	2
+    IM_UNKNOWN,	//	3
+    IM_UNKNOWN,	//	4
+    IM_UNKNOWN,	//	5
+    IM_UNKNOWN,	//	6
+    IM_UNKNOWN,	//	7
+    IM_UNKNOWN,	//	8
+    IM_UNKNOWN,	//	9
+    IM_UNKNOWN,	//	10
+    IM_UNKNOWN,	//	11
+    IM_UNKNOWN,	//	12
+    IM_UNKNOWN,	//	13
+    IM_UNKNOWN,	//	14
+    IM_UNKNOWN,	//	15
+    IM_UNKNOWN,	//	16
+    IM_UNKNOWN,	//	17
+    IM_UNKNOWN,	//	18
+    IM_UNKNOWN,	//	19
+    IM_UNKNOWN,	//	20
+    IM_UNKNOWN,	//	21
+    IM_UNKNOWN,	//	22
+    IM_UNKNOWN,	//	23
+    {"ERET", IT_I, ALU_OP_ERET, NOMEM, nullptr,
+     .flags = IMF_SUPPORTED | IMF_STOP_IF},
+    IM_UNKNOWN,	//	25
+    IM_UNKNOWN,	//	26
+    IM_UNKNOWN,	//	27
+    IM_UNKNOWN,	//	28
+    IM_UNKNOWN,	//	29
+    IM_UNKNOWN,	//	30
+    IM_UNKNOWN,	//	31
+    IM_UNKNOWN,	//	32
+    IM_UNKNOWN,	//	33
+    IM_UNKNOWN,	//	34
+    IM_UNKNOWN,	//	35
+    IM_UNKNOWN,	//	36
+    IM_UNKNOWN,	//	37
+    IM_UNKNOWN,	//	38
+    IM_UNKNOWN,	//	39
+    IM_UNKNOWN,	//	40
+    IM_UNKNOWN,	//	41
+    IM_UNKNOWN,	//	42
+    IM_UNKNOWN,	//	43
+    IM_UNKNOWN,	//	44
+    IM_UNKNOWN,	//	45
+    IM_UNKNOWN,	//	46
+    IM_UNKNOWN,	//	47
+    IM_UNKNOWN,	//	48
+    IM_UNKNOWN,	//	49
+    IM_UNKNOWN,	//	50
+    IM_UNKNOWN,	//	51
+    IM_UNKNOWN,	//	52
+    IM_UNKNOWN,	//	53
+    IM_UNKNOWN,	//	54
+    IM_UNKNOWN,	//	55
+    IM_UNKNOWN,	//	56
+    IM_UNKNOWN,	//	57
+    IM_UNKNOWN,	//	58
+    IM_UNKNOWN,	//	59
+    IM_UNKNOWN,	//	60
+    IM_UNKNOWN,	//	61
+    IM_UNKNOWN,	//	62
+    IM_UNKNOWN,	//	63
+};
+
+static const struct InstructionMap  cop0_instruction_map[] = {
+    {"MFC0", IT_I, ALU_OP_MFC0, NOMEM, nullptr,
+     .flags = IMF_SUPPORTED | IMF_REGWRITE},
+    IM_UNKNOWN,	//	1
+    IM_UNKNOWN,	//	2 MFH
+    IM_UNKNOWN,	//	3
+    {"MTC0", IT_I, ALU_OP_MTC0, NOMEM, nullptr,
+     .flags = IMF_SUPPORTED | IMF_ALU_REQ_RT},
+    IM_UNKNOWN,	//	5
+    IM_UNKNOWN,	//	6 MTH
+    IM_UNKNOWN,	//	7
+    IM_UNKNOWN,	//	8
+    IM_UNKNOWN,	//	9
+    IM_UNKNOWN,	//  10 RDPGPR
+    {"MFMC0", IT_I, ALU_OP_MFMC0, NOMEM, nullptr,
+     .flags = IMF_SUPPORTED | IMF_REGWRITE},
+    IM_UNKNOWN,	//	12
+    IM_UNKNOWN,	//	13
+    IM_UNKNOWN,	//	13 WRPGPR
+    IM_UNKNOWN,	//	15
+    {"C0", IT_I, NOALU, NOMEM, cop0_func_instruction_map,
+     .flags = IMF_SUB_ENCODE(6, 0)},
+    {"C0", IT_I, NOALU, NOMEM, cop0_func_instruction_map,
+     .flags = IMF_SUB_ENCODE(6, 0)},
+    {"C0", IT_I, NOALU, NOMEM, cop0_func_instruction_map,
+     .flags = IMF_SUB_ENCODE(6, 0)},
+    {"C0", IT_I, NOALU, NOMEM, cop0_func_instruction_map,
+     .flags = IMF_SUB_ENCODE(6, 0)},
+    {"C0", IT_I, NOALU, NOMEM, cop0_func_instruction_map,
+     .flags = IMF_SUB_ENCODE(6, 0)},
+    {"C0", IT_I, NOALU, NOMEM, cop0_func_instruction_map,
+     .flags = IMF_SUB_ENCODE(6, 0)},
+    {"C0", IT_I, NOALU, NOMEM, cop0_func_instruction_map,
+     .flags = IMF_SUB_ENCODE(6, 0)},
+    {"C0", IT_I, NOALU, NOMEM, cop0_func_instruction_map,
+     .flags = IMF_SUB_ENCODE(6, 0)},
+    {"C0", IT_I, NOALU, NOMEM, cop0_func_instruction_map,
+     .flags = IMF_SUB_ENCODE(6, 0)},
+    {"C0", IT_I, NOALU, NOMEM, cop0_func_instruction_map,
+     .flags = IMF_SUB_ENCODE(6, 0)},
+    {"C0", IT_I, NOALU, NOMEM, cop0_func_instruction_map,
+     .flags = IMF_SUB_ENCODE(6, 0)},
+    {"C0", IT_I, NOALU, NOMEM, cop0_func_instruction_map,
+     .flags = IMF_SUB_ENCODE(6, 0)},
+    {"C0", IT_I, NOALU, NOMEM, cop0_func_instruction_map,
+     .flags = IMF_SUB_ENCODE(6, 0)},
+    {"C0", IT_I, NOALU, NOMEM, cop0_func_instruction_map,
+     .flags = IMF_SUB_ENCODE(6, 0)},
+    {"C0", IT_I, NOALU, NOMEM, cop0_func_instruction_map,
+     .flags = IMF_SUB_ENCODE(6, 0)},
+    {"C0", IT_I, NOALU, NOMEM, cop0_func_instruction_map,
+     .flags = IMF_SUB_ENCODE(6, 0)},
 };
 
 const std::int32_t instruction_map_opcode_field = IMF_SUB_ENCODE(6, 26);
@@ -476,7 +598,8 @@ static const struct InstructionMap instruction_map[] = {
      .flags = FLAGS_ALU_I_ZE},
     {"LUI",    IT_I, ALU_OP_LUI, NOMEM, nullptr,     // LUI
      .flags = FLAGS_ALU_I_NO_RS},
-    IM_UNKNOWN,  // 16
+    {"COP0",   IT_I, NOALU, NOMEM, cop0_instruction_map, // COP0
+     .flags = IMF_SUB_ENCODE(5, 21)},
     IM_UNKNOWN,  // 17
     IM_UNKNOWN,  // 18
     IM_UNKNOWN,  // 19
@@ -636,6 +759,10 @@ std::uint8_t Instruction::funct() const {
     return (std::uint8_t) MASK(6, 0);
 }
 
+std::uint8_t Instruction::cop0sel() const {
+    return (std::uint8_t) MASK(3, 0);
+}
+
 std::uint16_t Instruction::immediate() const {
     return (std::uint16_t) MASK(16, 0);
 }
diff --git a/qtmips_machine/instruction.h b/qtmips_machine/instruction.h
index b7ae4bd..a3bfb79 100644
--- a/qtmips_machine/instruction.h
+++ b/qtmips_machine/instruction.h
@@ -71,6 +71,7 @@ enum InstructionFlags {
     IMF_BGTZ_BLEZ  = 1L<<20, /**< BGTZ/BLEZ, else BEGT/BLTZ or BEQ, BNE when RT */
     IMF_NB_SKIP_DS = 1L<<21, /**< Skip instruction in delay slot if branch not taken */
     IMF_EXCEPTION  = 1L<<22, /**< Instruction causes synchronous exception */
+    IMF_STOP_IF    = 1L<<23, /**< Stop instruction fetch until instruction processed */
 };
 
 class Instruction {
@@ -95,6 +96,7 @@ public:
     std::uint8_t rd() const;
     std::uint8_t shamt() const;
     std::uint8_t funct() const;
+    std::uint8_t cop0sel() const;
     std::uint16_t immediate() const;
     std::uint32_t address() const;
     std::uint32_t data() const;
diff --git a/qtmips_machine/machinedefs.h b/qtmips_machine/machinedefs.h
index 16a72b0..eb7f6d4 100644
--- a/qtmips_machine/machinedefs.h
+++ b/qtmips_machine/machinedefs.h
@@ -120,6 +120,10 @@ enum AluOp : std::uint8_t {
     ALU_OP_BREAK,
     ALU_OP_SYSCALL,
     ALU_OP_RDHWR,
+    ALU_OP_MTC0,
+    ALU_OP_MFC0,
+    ALU_OP_MFMC0,
+    ALU_OP_ERET,
     ALU_OP_UNKNOWN,
     ALU_OP_LAST // First impossible operation (just to be sure that we don't overflow)
 };
diff --git a/qtmips_machine/qtmips_machine.pro b/qtmips_machine/qtmips_machine.pro
index 9964357..1332928 100644
--- a/qtmips_machine/qtmips_machine.pro
+++ b/qtmips_machine/qtmips_machine.pro
@@ -28,7 +28,8 @@ SOURCES += \
     peripheral.cpp \
     serialport.cpp \
     peripspiled.cpp \
-    symboltable.cpp
+    symboltable.cpp \
+    cop0state.cpp
 
 HEADERS += \
     qtmipsmachine.h \
@@ -47,4 +48,5 @@ HEADERS += \
     peripheral.h \
     serialport.h \
     peripspiled.h \
-    symboltable.h
+    symboltable.h \
+    cop0state.h
diff --git a/qtmips_machine/qtmipsmachine.cpp b/qtmips_machine/qtmipsmachine.cpp
index 171a50f..0ce1662 100644
--- a/qtmips_machine/qtmipsmachine.cpp
+++ b/qtmips_machine/qtmipsmachine.cpp
@@ -72,10 +72,21 @@ QtMipsMachine::QtMipsMachine(const MachineConfig &cc, bool load_symtab) :
     cch_data = new Cache(cpu_mem, &cc.cache_data(), cc.memory_access_time_read(),
                          cc.memory_access_time_write(), cc.memory_access_time_burst());
 
+    unsigned int min_cache_row_size = 16;
+    if (cc.cache_data().enabled())
+        min_cache_row_size = cc.cache_data().blocks() * 4;
+    if (cc.cache_program().enabled() &&
+        cc.cache_program().blocks() < min_cache_row_size)
+        min_cache_row_size = cc.cache_program().blocks() * 4;
+
+    cop0state = new Cop0State();
+
     if (cc.pipelined())
-        cr = new CorePipelined(regs, cch_program, cch_data, cc.hazard_unit());
+        cr = new CorePipelined(regs, cch_program, cch_data, cc.hazard_unit(),
+                               min_cache_row_size, cop0state);
     else
-        cr = new CoreSingle(regs, cch_program, cch_data, cc.delay_slot());
+        cr = new CoreSingle(regs, cch_program, cch_data, cc.delay_slot(),
+                            min_cache_row_size, cop0state);
 
     run_t = new QTimer(this);
     set_speed(0); // In default run as fast as possible
@@ -89,6 +100,9 @@ QtMipsMachine::~QtMipsMachine() {
     if (cr != nullptr)
         delete cr;
     cr = nullptr;
+    if (cop0state != nullptr)
+        delete cop0state;
+    cop0state = nullptr;
     if (regs != nullptr)
         delete regs;
     regs = nullptr;
diff --git a/qtmips_machine/qtmipsmachine.h b/qtmips_machine/qtmipsmachine.h
index 1c80b9c..51b5424 100644
--- a/qtmips_machine/qtmipsmachine.h
+++ b/qtmips_machine/qtmipsmachine.h
@@ -119,6 +119,7 @@ private:
     SerialPort *ser_port;
     PeripSpiLed *perip_spi_led;
     Cache *cch_program, *cch_data;
+    Cop0State *cop0state;
     Core *cr;
 
     QTimer *run_t;