arm: Implement MSR_REG and fix some prefetching errors

Former-commit-id: 177b8966159ed86472b0d4d031363df72d46807a

Cargo.lock

@@ -287,6 +287,67 @@ dependencies = [
  "version_check 0.1.5 (registry+https://github.com/rust-lang/crates.io-index)",
 ]
 
+[[package]]
+name = "num"
+version = "0.2.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+dependencies = [
+ "num-bigint 0.2.2 (registry+https://github.com/rust-lang/crates.io-index)",
+ "num-complex 0.2.3 (registry+https://github.com/rust-lang/crates.io-index)",
+ "num-integer 0.1.41 (registry+https://github.com/rust-lang/crates.io-index)",
+ "num-iter 0.1.39 (registry+https://github.com/rust-lang/crates.io-index)",
+ "num-rational 0.2.2 (registry+https://github.com/rust-lang/crates.io-index)",
+ "num-traits 0.2.8 (registry+https://github.com/rust-lang/crates.io-index)",
+]
+
+[[package]]
+name = "num-bigint"
+version = "0.2.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+dependencies = [
+ "num-integer 0.1.41 (registry+https://github.com/rust-lang/crates.io-index)",
+ "num-traits 0.2.8 (registry+https://github.com/rust-lang/crates.io-index)",
+]
+
+[[package]]
+name = "num-complex"
+version = "0.2.3"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+dependencies = [
+ "autocfg 0.1.4 (registry+https://github.com/rust-lang/crates.io-index)",
+ "num-traits 0.2.8 (registry+https://github.com/rust-lang/crates.io-index)",
+]
+
+[[package]]
+name = "num-integer"
+version = "0.1.41"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+dependencies = [
+ "autocfg 0.1.4 (registry+https://github.com/rust-lang/crates.io-index)",
+ "num-traits 0.2.8 (registry+https://github.com/rust-lang/crates.io-index)",
+]
+
+[[package]]
+name = "num-iter"
+version = "0.1.39"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+dependencies = [
+ "autocfg 0.1.4 (registry+https://github.com/rust-lang/crates.io-index)",
+ "num-integer 0.1.41 (registry+https://github.com/rust-lang/crates.io-index)",
+ "num-traits 0.2.8 (registry+https://github.com/rust-lang/crates.io-index)",
+]
+
+[[package]]
+name = "num-rational"
+version = "0.2.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+dependencies = [
+ "autocfg 0.1.4 (registry+https://github.com/rust-lang/crates.io-index)",
+ "num-bigint 0.2.2 (registry+https://github.com/rust-lang/crates.io-index)",
+ "num-integer 0.1.41 (registry+https://github.com/rust-lang/crates.io-index)",
+ "num-traits 0.2.8 (registry+https://github.com/rust-lang/crates.io-index)",
+]
+
 [[package]]
 name = "num-traits"
 version = "0.1.43"
@@ -421,7 +482,8 @@ dependencies = [
  "enum-primitive-derive 0.1.2 (registry+https://github.com/rust-lang/crates.io-index)",
  "hexdump 0.1.0 (registry+https://github.com/rust-lang/crates.io-index)",
  "nom 5.0.0 (registry+https://github.com/rust-lang/crates.io-index)",
- "num-traits 0.1.43 (registry+https://github.com/rust-lang/crates.io-index)",
+ "num 0.2.0 (registry+https://github.com/rust-lang/crates.io-index)",
+ "num-traits 0.2.8 (registry+https://github.com/rust-lang/crates.io-index)",
  "rustyline 5.0.0 (registry+https://github.com/rust-lang/crates.io-index)",
 ]
 
@@ -674,6 +736,12 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 "checksum nix 0.14.1 (registry+https://github.com/rust-lang/crates.io-index)" = "6c722bee1037d430d0f8e687bbdbf222f27cc6e4e68d5caf630857bb2b6dbdce"
 "checksum nodrop 0.1.13 (registry+https://github.com/rust-lang/crates.io-index)" = "2f9667ddcc6cc8a43afc9b7917599d7216aa09c463919ea32c59ed6cac8bc945"
 "checksum nom 5.0.0 (registry+https://github.com/rust-lang/crates.io-index)" = "e9761d859320e381010a4f7f8ed425f2c924de33ad121ace447367c713ad561b"
+"checksum num 0.2.0 (registry+https://github.com/rust-lang/crates.io-index)" = "cf4825417e1e1406b3782a8ce92f4d53f26ec055e3622e1881ca8e9f5f9e08db"
+"checksum num-bigint 0.2.2 (registry+https://github.com/rust-lang/crates.io-index)" = "57450397855d951f1a41305e54851b1a7b8f5d2e349543a02a2effe25459f718"
+"checksum num-complex 0.2.3 (registry+https://github.com/rust-lang/crates.io-index)" = "fcb0cf31fb3ff77e6d2a6ebd6800df7fdcd106f2ad89113c9130bcd07f93dffc"
+"checksum num-integer 0.1.41 (registry+https://github.com/rust-lang/crates.io-index)" = "b85e541ef8255f6cf42bbfe4ef361305c6c135d10919ecc26126c4e5ae94bc09"
+"checksum num-iter 0.1.39 (registry+https://github.com/rust-lang/crates.io-index)" = "76bd5272412d173d6bf9afdf98db8612bbabc9a7a830b7bfc9c188911716132e"
+"checksum num-rational 0.2.2 (registry+https://github.com/rust-lang/crates.io-index)" = "f2885278d5fe2adc2f75ced642d52d879bffaceb5a2e0b1d4309ffdfb239b454"
 "checksum num-traits 0.1.43 (registry+https://github.com/rust-lang/crates.io-index)" = "92e5113e9fd4cc14ded8e499429f396a20f98c772a47cc8622a736e1ec843c31"
 "checksum num-traits 0.2.8 (registry+https://github.com/rust-lang/crates.io-index)" = "6ba9a427cfca2be13aa6f6403b0b7e7368fe982bfa16fccc450ce74c46cd9b32"
 "checksum numtoa 0.1.0 (registry+https://github.com/rust-lang/crates.io-index)" = "b8f8bdf33df195859076e54ab11ee78a1b208382d3a26ec40d142ffc1ecc49ef"

Cargo.toml

@@ -6,8 +6,9 @@ edition = "2018"
 
 [dependencies]
 byteorder = "*"
+num = "0.2.0"
+num-traits = "0.2"
 enum-primitive-derive = "^0.1"
-num-traits = "^0.1"
 bit = "^0.1"
 clap = {version = "2.33", features = ["color", "yaml"]}
 rustyline = "5.0.0"

src/arm7tdmi/arm/exec.rs

@@ -1,9 +1,10 @@
 use crate::bit::BitIndex;
 
-use crate::arm7tdmi::bus::{Bus, MemoryAccess, MemoryAccessType::*, MemoryAccessWidth::*};
+use crate::arm7tdmi::bus::{Bus, MemoryAccessType::*, MemoryAccessWidth::*};
 use crate::arm7tdmi::cpu::{Core, CpuExecResult, CpuPipelineAction};
 use crate::arm7tdmi::exception::Exception;
 use crate::arm7tdmi::{Addr, CpuError, CpuInstruction, CpuResult, CpuState, REG_PC};
+use crate::arm7tdmi::psr::RegPSR;
 
 use crate::sysbus::SysBus;
 
@@ -29,9 +30,8 @@ impl Core {
             ArmFormat::DP => self.exec_data_processing(sysbus, insn),
             ArmFormat::SWI => self.exec_swi(sysbus, insn),
             ArmFormat::LDR_STR => self.exec_ldr_str(sysbus, insn),
-            _ => Err(CpuError::UnimplementedCpuInstruction(CpuInstruction::Arm(
+            ArmFormat::MSR_REG => self.exec_msr_reg(sysbus, insn),
-                insn,
+            _ => Err(CpuError::UnimplementedCpuInstruction(CpuInstruction::Arm(insn))),
-            ))),
         }
     }
 
@@ -48,7 +48,7 @@ impl Core {
         // +1N
         self.add_cycles(self.pc, sysbus, NonSeq + MemoryAccess32);
 
-        self.pc = (self.pc as i32).wrapping_add(insn.branch_offset()) as u32;
+        self.pc = (self.pc as i32).wrapping_add(insn.branch_offset()) as u32 & !1;
 
         // +2S
         self.add_cycles(self.pc, sysbus, Seq + MemoryAccess32);
@@ -99,6 +99,29 @@ impl Core {
         Ok(CpuPipelineAction::Flush)
     }
 
+    fn exec_msr_reg(
+        &mut self,
+        sysbus: &mut SysBus,
+        insn: ArmInstruction,
+    ) -> CpuResult<CpuPipelineAction> {
+        let new_psr = RegPSR::new(self.get_reg(insn.rm()));
+        let old_mode = self.cpsr.mode();
+        if insn.spsr_flag() {
+            if let Some(index) = old_mode.spsr_index() {
+                self.spsr[index] = new_psr;
+            } else {
+                panic!("tried to change spsr from invalid mode {}", old_mode)
+            }
+        } else {
+            if old_mode != new_psr.mode() {
+                self.change_mode(new_psr.mode());
+            }
+            self.cpsr = new_psr;
+        }
+        self.add_cycles(insn.pc, sysbus, Seq + MemoryAccess32);
+        Ok(CpuPipelineAction::IncPC)
+    }
+
     fn barrel_shift(val: i32, amount: u32, shift: ArmShiftType) -> i32 {
         match shift {
             ArmShiftType::LSL => val.wrapping_shl(amount),
@@ -183,7 +206,11 @@ impl Core {
 
         let mut pipeline_action = CpuPipelineAction::IncPC;
 
-        let op1 = self.get_reg(insn.rn()) as i32;
+        let op1 = if insn.rn() == REG_PC {
+            self.pc as i32 // prefething
+        } else {
+            self.get_reg(insn.rn()) as i32
+        };
         let op2 = insn.operand2()?;
 
         let rd = insn.rd();
@@ -212,7 +239,7 @@ impl Core {
         let set_flags = opcode.is_setting_flags() || insn.set_cond_flag();
         if let Some(result) = self.alu(opcode, op1, op2, set_flags) {
             self.set_reg(rd, result as u32);
-            if (rd == REG_PC) {
+            if rd == REG_PC {
                 pipeline_action = CpuPipelineAction::Flush;
                 // +1S
                 self.add_cycles(self.pc, sysbus, Seq + MemoryAccess32);
@@ -268,7 +295,7 @@ impl Core {
 
         let mut addr = self.get_reg(insn.rn());
         if insn.rn() == REG_PC {
-            addr += 8; // prefetching
+            addr = insn.pc + 8; // prefetching
         }
         let dest = self.get_reg(insn.rd());
 

src/arm7tdmi/mod.rs

@@ -1,5 +1,7 @@
 use std::fmt;
 
+use num::Num;
+
 pub mod arm;
 use arm::{ArmDecodeError, ArmInstruction};
 
@@ -7,8 +9,8 @@ pub mod cpu;
 pub use cpu::*;
 pub mod bus;
 pub use bus::Bus;
-mod exception;
+pub mod exception;
-mod psr;
+pub mod psr;
 
 pub const REG_PC: usize = 15;
 pub const REG_LR: usize = 14;

src/arm7tdmi/psr.rs

@@ -2,7 +2,7 @@
 use std::fmt;
 
 use crate::bit::BitIndex;
-use crate::num_traits::FromPrimitive;
+use crate::num::FromPrimitive;
 
 use super::{CpuMode, CpuState};
 
@@ -39,15 +39,18 @@ fn clear_reserved(n: u32) -> u32 {
 
 impl Default for RegPSR {
     fn default() -> RegPSR {
-        let mut psr = RegPSR {
+        let mut psr = RegPSR::new(0);
-            raw: clear_reserved(0),
-        };
         psr.set_mode(CpuMode::Supervisor);
         psr
     }
 }
-
 impl RegPSR {
+    pub fn new(u: u32) -> RegPSR {
+        RegPSR {
+            raw: clear_reserved(u),
+        }
+    }
+
     pub fn get(&self) -> u32 {
         self.raw
     }

src/lib.rs

@@ -1,5 +1,6 @@
 #[macro_use]
 extern crate enum_primitive_derive;
+extern crate num;
 extern crate num_traits;
 
 extern crate bit;