CP0 registers and MFC0 and MTC0 instructions

src/cpu.rs

@@ -1,4 +1,4 @@
-use crate::registers::{CPURegisters};
+use crate::registers::{CPURegisters, CP0Registers};
 
 pub fn params_rd_rs_rt(opcode: u32) -> (usize, usize, usize) {
     let rd = (opcode >> 11) & 0b11111;
@@ -40,6 +40,12 @@ pub fn params_rd_rt_sa(opcode: u32) -> (usize, usize, usize) {
     (rd as usize, rt as usize, sa as usize)
 }
 
+pub fn params_rt_rd(opcode: u32) -> (usize, usize) {
+    let rd = (opcode >> 11) & 0b11111;
+    let rt = (opcode >> 16) & 0b11111;
+    (rt as usize, rd as usize)
+}
+
 pub fn params_rd(opcode: u32) -> usize {
     return ((opcode >> 11) & 0b11111) as usize;
 }
@@ -50,12 +56,14 @@ pub fn params_rs(opcode: u32) -> usize {
 
 pub struct CPU {
     registers: CPURegisters,
+    cp0: CP0Registers,
 }
 
 impl CPU {
     pub fn new() -> Self {
         Self {
             registers: CPURegisters::new(),
+            cp0: CP0Registers::new(),
         }
     }
 
@@ -261,11 +269,11 @@ impl CPU {
                     // MFHI
                     0b000_0001_0000 => self.mfhi(params_rd(opcode)),
                     // MFLO
-                    0b000_0001_0010 => self.mfhi(params_rd(opcode)),
+                    0b000_0001_0010 => self.mflo(params_rd(opcode)),
                     // MTHI
-                    0b000_0001_0001 => self.mfhi(params_rs(opcode)),
+                    0b000_0001_0001 => self.mthi(params_rs(opcode)),
                     // MTLO
-                    0b000_0001_0011 => self.mfhi(params_rs(opcode)),
+                    0b000_0001_0011 => self.mtlo(params_rs(opcode)),
                     _ => unimplemented!(),
                 };
             },
@@ -324,6 +332,28 @@ impl CPU {
                 let (rt, immediate) = params_rt_immediate(opcode);
                 self.lui(rt, immediate);
             },
+            0b0100_00 => {
+                let instr = (opcode >> 21) & 11;
+                match instr {
+                    0b0100_0000_100 => {
+                        let (rt, rd) = params_rt_rd(opcode);
+                        self.mtc0(rt, rd);
+                    },
+                    0b0100_0000_000 => {
+                        let (rt, rd) = params_rt_rd(opcode);
+                        self.mfc0(rt, rd);
+                    },
+                    0b0100_0000_101 => {
+                        let (rt, rd) = params_rt_rd(opcode);
+                        self.dmtc0(rt, rd);
+                    },
+                    0b0100_0000_001 => {
+                        let (rt, rd) = params_rt_rd(opcode);
+                        self.dmfc0(rt, rd);
+                    },
+                    _ => unimplemented!(),
+                };
+            },
             _ => unimplemented!(),
         }
     }
@@ -693,6 +723,34 @@ impl CPU {
     pub fn mtlo(&mut self, rs: usize) {
         self.registers.set_lo(self.registers.get_by_number(rs));
     }
+
+    pub fn mtc0(&mut self, rt: usize, rd: usize) {
+        match CP0Registers::is_32bits(rd) {
+            true => self.cp0.set_by_number_32(rd, self.registers.get_by_number(rt) as i32),
+            false => self.cp0.set_by_number_64(rd, self.registers.get_by_number(rt)),
+        };
+    }
+
+    pub fn mfc0(&mut self, rt: usize, rd: usize) {
+        match CP0Registers::is_32bits(rd) {
+            true => self.registers.set_by_number(rt, self.cp0.get_by_number_32(rd) as i64),
+            false => self.registers.set_by_number(rt, self.cp0.get_by_number_64(rd))
+        };
+    }
+
+    pub fn dmtc0(&mut self, rt: usize, rd: usize) {
+        match CP0Registers::is_32bits(rd) {
+            true => self.cp0.set_by_number_32(rd, self.registers.get_by_number(rt) as i32),
+            false => self.cp0.set_by_number_64(rd, self.registers.get_by_number(rt)),
+        };
+    }
+
+    pub fn dmfc0(&mut self, rt: usize, rd: usize) {
+        match CP0Registers::is_32bits(rd) {
+            true => self.registers.set_by_number(rt, self.cp0.get_by_number_32(rd) as i64),
+            false => self.registers.set_by_number(rt, self.cp0.get_by_number_64(rd))
+        };
+    }
 }
 
 #[cfg(test)]
@@ -1059,6 +1117,11 @@ mod cpu_instructions_tests {
         let reg_t = 20;
         cpu.lui(reg_t, -10);
         assert_eq!(cpu.registers.get_by_number(reg_t), -655360);
+
+        let mut cpu = CPU::new();
+        let rt = 15;
+        cpu.lui(rt, 0x3400);
+        assert_eq!(cpu.registers.get_by_number(rt) as i32, 0x34000000);
     }
 
     #[test]
@@ -1258,4 +1321,44 @@ mod cpu_instructions_tests {
         cpu.mtlo(rs);
         assert_eq!(cpu.registers.get_lo(), 65535);
     }
+
+    #[test]
+    fn test_mtc0() {
+        let mut cpu = CPU::new();
+        let rt = 15;
+        let rd = 12;
+        cpu.registers.set_by_number(rt, 65535);
+        cpu.mtc0(rt, rd);
+        assert_eq!(cpu.cp0.get_by_number_32(rd), 65535);
+    }
+
+    #[test]
+    fn test_mfc0() {
+        let mut cpu = CPU::new();
+        let rt = 15;
+        let rd = 21;
+        cpu.cp0.set_by_number_64(rd, 65535);
+        cpu.mfc0(rt, rd);
+        assert_eq!(cpu.registers.get_by_number(rt), 65535);
+    }
+
+    #[test]
+    fn test_dmtc0() {
+        let mut cpu = CPU::new();
+        let rt = 15;
+        let rd = 12;
+        cpu.registers.set_by_number(rt, 65535);
+        cpu.dmtc0(rt, rd);
+        assert_eq!(cpu.cp0.get_by_number_32(rd), 65535);
+    }
+
+    #[test]
+    fn test_dmfc0() {
+        let mut cpu = CPU::new();
+        let rt = 15;
+        let rd = 21;
+        cpu.cp0.set_by_number_64(rd, 65535);
+        cpu.dmfc0(rt, rd);
+        assert_eq!(cpu.registers.get_by_number(rt), 65535);
+    }
 }

src/rdram.rs

@@ -1,5 +1,5 @@
 #[derive(Copy, Clone)]
-struct Byte {
+pub struct Byte {
     data: u16,
 }
 
@@ -13,7 +13,7 @@ impl Byte {
     }
 }
 
-struct RDRAM {
+pub struct RDRAM {
     data: [Byte; 0x400000],
 }
 

src/registers.rs

@@ -17,6 +17,7 @@ impl<T: PartialOrd + Copy> Register<T> for Generic<T> {
     fn set(&mut self, val: T) {self.0 = val}
 }
 
+
 pub const CPU_REGISTER_NAMES: [&'static str; 32] = [
     "zero", "at", "v0", "v1", "a0", "a1", "a2", "a3",
     "t0",   "t1", "t2", "t3", "t4", "t5", "t6", "t7",
@@ -76,6 +77,10 @@ impl CPURegisters {
         }
     }
 
+    fn find_index(name: &'static str) -> usize {
+        CPU_REGISTER_NAMES.iter().position(|v| *v == name).unwrap()
+    }
+
     pub fn get_by_number(&self, index: usize) -> i64 {
         if index > 31 {
             unreachable!("Register number {} not valid", index);
@@ -84,14 +89,10 @@ impl CPURegisters {
     }
 
     pub fn get_by_name(&self, name: &'static str) -> i64 {
-        let index = CPU_REGISTER_NAMES.iter().position(|v| *v == name).unwrap();
+        let index = CPURegisters::find_index(name);
         self.registers[index].get()
     }
 
-    pub fn get_program_counter(&self) -> i64 {
-        self.program_counter.get()
-    }
-
     pub fn set_by_number(&mut self, index: usize, val: i64) {
         if index > 31 {
             unreachable!("Register number {} not valid", index);
@@ -100,16 +101,21 @@ impl CPURegisters {
     }
 
     pub fn set_by_name(&mut self, name: &'static str, val: i64) {
-        let index = CPU_REGISTER_NAMES.iter().position(|v| *v == name).unwrap();
+        let index = CPURegisters::find_index(name);
         self.registers[index].set(val);
     }
 
+    pub fn get_program_counter(&self) -> i64 {
+        self.program_counter.get()
+    }
+
     pub fn set_program_counter(&mut self, val: i64) {
         self.program_counter.set(val);
     }
 
     pub fn increment_program_counter(&mut self, val: i64) {
-        self.program_counter.set(self.program_counter.get().wrapping_add(val));
+        let pc: i64 = self.program_counter.get();
+        self.program_counter.set(pc.wrapping_add(val));
     }
 
     pub fn set_hi(&mut self, val: i64) {
@@ -129,6 +135,223 @@ impl CPURegisters {
     }
 }
 
+pub const CP0_REGISTER_NAMES: [&'static str; 32] = [
+    "index", "random", "EntryLo0", "EntryLo1", "context", "PageMask", "wired", "7",
+    "BadVAddr", "count", "EntryHi", "compare", "status", "cause", "epc", "PRId",
+    "config", "LLAddr", "WatchLo", "WatchHi", "XContext", "21", "22", "23",
+    "24", "25", "ParityError", "CacheError", "TagLo", "TagHi", "ErrorEPC", "31"
+];
+
+pub struct CP0Registers {
+    index: Generic<i32>,
+    random: Generic<i32>,
+    entry_lo_0: Generic<i64>,
+    entry_lo_1: Generic<i64>,
+    context: Generic<i64>,
+    page_mask: Generic<i32>,
+    wired: Generic<i32>,
+    r7: Generic<i64>,
+    bad_v_addr: Generic<i64>,
+    count: Generic<i32>,
+    entry_hi: Generic<i64>,
+    compare: Generic<i32>,
+    status: Generic<i32>,
+    cause: Generic<i32>,
+    epc: Generic<i64>,
+    prid: Generic<i32>,
+    config: Generic<i32>,
+    lladdr: Generic<i32>,
+    watch_lo: Generic<i32>,
+    watch_hi: Generic<i32>,
+    xcontext: Generic<i64>,
+    r21: Generic<i64>,
+    r22: Generic<i64>,
+    r23: Generic<i64>,
+    r24: Generic<i64>,
+    r25: Generic<i64>,
+    parity_error: Generic<i32>,
+    cache_error: Generic<i32>,
+    tag_lo: Generic<i32>,
+    tag_hi: Generic<i32>,
+    error_epc: Generic<i64>,
+    r31: Generic<i64>,
+}
+
+impl CP0Registers {
+    pub fn new() -> Self {
+        Self {
+            index: Generic(0),
+            random: Generic(0),
+            entry_lo_0: Generic(0),
+            entry_lo_1: Generic(0),
+            context: Generic(0),
+            page_mask: Generic(0),
+            wired: Generic(0),
+            r7: Generic(0),
+            bad_v_addr: Generic(0),
+            count: Generic(0),
+            entry_hi: Generic(0),
+            compare: Generic(0),
+            status: Generic(0),
+            cause: Generic(0),
+            epc: Generic(0),
+            prid: Generic(0),
+            config: Generic(0),
+            lladdr: Generic(0),
+            watch_lo: Generic(0),
+            watch_hi: Generic(0),
+            xcontext: Generic(0),
+            r21: Generic(0),
+            r22: Generic(0),
+            r23: Generic(0),
+            r24: Generic(0),
+            r25: Generic(0),
+            parity_error: Generic(0),
+            cache_error: Generic(0),
+            tag_lo: Generic(0),
+            tag_hi: Generic(0),
+            error_epc: Generic(0),
+            r31: Generic(0),
+        }
+    }
+
+    fn find_index(name: &'static str) -> usize {
+        CP0_REGISTER_NAMES.iter().position(|v| *v == name).unwrap()
+    }
+
+    pub fn is_32bits(index: usize) -> bool {
+        match index {
+            0 | 1 | 5 | 6 | 9 | 11 | 12 | 13 | 15 | 16 | 17 | 18 | 19 | 26 | 27 | 28 | 29 => true,
+            2 | 3 | 4 | 8 | 10 | 14 | 20 | 21 | 22 | 23 | 24 | 25 | 30 => false,
+            _ => unreachable!(),
+        }
+    }
+
+    pub fn is_64bits(index: usize) -> bool {
+        !CP0Registers::is_32bits(index)
+    }
+
+    pub fn get_by_number_32(&self, index: usize) -> i32 {
+        if index > 31 {
+            unreachable!("Register number {} not valid", index);
+        }
+        match index {
+            0  => self.index.get(),
+            1  => self.random.get(),
+            5  => self.page_mask.get(),
+            6  => self.wired.get(),
+            9  => self.count.get(),
+            11 => self.compare.get(),
+            12 => self.status.get(),
+            13 => self.cause.get(),
+            15 => self.prid.get(),
+            16 => self.config.get(),
+            17 => self.lladdr.get(),
+            18 => self.watch_lo.get(),
+            19 => self.watch_hi.get(),
+            26 => self.parity_error.get(),
+            27 => self.cache_error.get(),
+            28 => self.tag_lo.get(),
+            29 => self.tag_hi.get(),
+            _ => unreachable!("Register {} is not 32bit", index),
+        }
+    }
+
+    pub fn set_by_number_32(&mut self, index: usize, val: i32) {
+        if index > 31 {
+            unreachable!("Register number {} not valid", index);
+        }
+        match index {
+            0  => self.index.set(val),
+            1  => self.random.set(val),
+            5  => self.page_mask.set(val),
+            6  => self.wired.set(val),
+            9  => self.count.set(val),
+            11 => self.compare.set(val),
+            12 => self.status.set(val),
+            13 => self.cause.set(val),
+            15 => self.prid.set(val),
+            16 => self.config.set(val),
+            17 => self.lladdr.set(val),
+            18 => self.watch_lo.set(val),
+            19 => self.watch_hi.set(val),
+            26 => self.parity_error.set(val),
+            27 => self.cache_error.set(val),
+            28 => self.tag_lo.set(val),
+            29 => self.tag_hi.set(val),
+            _ => unreachable!("Register {} is not 32bit", index),
+        };
+    }
+
+    pub fn get_by_number_64(&self, index: usize) -> i64 {
+        if index > 31 {
+            unreachable!("Register number {} not valid", index);
+        }
+        match index {
+            2  => self.entry_lo_0.get(),
+            3  => self.entry_lo_1.get(),
+            4  => self.context.get(),
+            7  => self.r7.get(),
+            8  => self.bad_v_addr.get(),
+            10 => self.entry_hi.get(),
+            14 => self.epc.get(),
+            20 => self.xcontext.get(),
+            21 => self.r21.get(),
+            22 => self.r22.get(),
+            23 => self.r23.get(),
+            24 => self.r24.get(),
+            25 => self.r25.get(),
+            30 => self.error_epc.get(),
+            31 => self.r31.get(),
+            _ => unreachable!("Register {} is not 64bit", index),
+        }
+    }
+
+    pub fn set_by_number_64(&mut self, index: usize, val: i64) {
+        if index > 31 {
+            unreachable!("Register number {} not valid", index);
+        }
+        match index {
+            2  => self.entry_lo_0.set(val),
+            3  => self.entry_lo_1.set(val),
+            4  => self.context.set(val),
+            7  => self.r7.set(val),
+            8  => self.bad_v_addr.set(val),
+            10 => self.entry_hi.set(val),
+            14 => self.epc.set(val),
+            20 => self.xcontext.set(val),
+            21 => self.r21.set(val),
+            22 => self.r22.set(val),
+            23 => self.r23.set(val),
+            24 => self.r24.set(val),
+            25 => self.r25.set(val),
+            30 => self.error_epc.set(val),
+            31 => self.r31.set(val),
+            _ => unreachable!("Register {} is not 64bit", index),
+        };
+    }
+
+    pub fn get_by_name_32(&self, name: &'static str) -> i32 {
+        let index = CP0Registers::find_index(name);
+        self.get_by_number_32(index)
+    }
+
+    pub fn set_by_name_32(&mut self, name: &'static str, val: i32) {
+        let index = CP0Registers::find_index(name);
+        self.set_by_number_32(index, val);
+    }
+
+    pub fn get_by_name_64(&self, name: &'static str) -> i64 {
+        let index = CP0Registers::find_index(name);
+        self.get_by_number_64(index)
+    }
+
+    pub fn set_by_name_64(&mut self, name: &'static str, val: i64) {
+        let index = CP0Registers::find_index(name);
+        self.set_by_number_64(index, val);
+    }
+}
+
 #[cfg(test)]
 mod cpu_registers_tests {
     use super::*;
@@ -152,3 +375,27 @@ mod cpu_registers_tests {
         assert_eq!(registers.get_by_number(4), 20);
     }
 }
+
+#[cfg(test)]
+mod cp0_registers_tests {
+    use super::*;
+
+    #[test]
+    fn test_set_by_number() {
+        let mut registers = CP0Registers::new();
+        registers.set_by_number_32(0, 20);
+        assert_eq!(registers.get_by_number_32(0), 20);
+        registers.set_by_number_64(4, 20);
+        assert_eq!(registers.get_by_number_64(4), 20);
+    }
+
+    #[test]
+    fn test_set_by_name() {
+        let mut registers = CP0Registers::new();
+        registers.set_by_name_32("index", 0);
+        assert_eq!(registers.get_by_name_32("index"), 0);
+        registers.set_by_name_64("context", 20);
+        assert_eq!(registers.get_by_name_64("context"), 20);
+        assert_eq!(registers.get_by_number_64(4), 20);
+    }
+}