arm: Implement Arm MSR_FLAGS

Former-commit-id: 64d2cf255304ecca02dadc55266d22bc1f92bb4c

src/arm7tdmi/arm/display.rs

@@ -2,7 +2,7 @@ use std::fmt;
 
 use super::{AluOpCode, ArmCond, ArmFormat, ArmHalfwordTransferType, ArmInstruction};
 use crate::arm7tdmi::{
-    reg_string, Addr, BarrelShiftOpCode, BarrelShifterValue, ShiftedRegister, REG_PC,
+    psr::RegPSR, reg_string, Addr, BarrelShiftOpCode, BarrelShifterValue, ShiftedRegister, REG_PC,
 };
 
 impl fmt::Display for ArmCond {
@@ -117,6 +117,26 @@ impl ArmInstruction {
         }
     }
 
+    fn fmt_operand2(&self, f: &mut fmt::Formatter) -> Result<Option<u32>, fmt::Error> {
+        let operand2 = self.operand2().unwrap();
+        match operand2 {
+            BarrelShifterValue::RotatedImmediate(_, _) => {
+                let value = operand2.decode_rotated_immediate().unwrap();
+                write!(f, "#{}\t; {:#x}", value, value)?;
+                Ok(Some(value as u32))
+            }
+            BarrelShifterValue::ShiftedRegister {
+                reg,
+                shift,
+                added: _,
+            } => {
+                write!(f, "{}", self.make_shifted_reg_string(reg, shift))?;
+                Ok(None)
+            }
+            _ => panic!("invalid operand2"),
+        }
+    }
+
     fn fmt_data_processing(&self, f: &mut fmt::Formatter) -> fmt::Result {
         use AluOpCode::*;
 
@@ -149,19 +169,8 @@ impl ArmInstruction {
             ),
         }?;
 
-        let operand2 = self.operand2().unwrap();
-        match operand2 {
-            BarrelShifterValue::RotatedImmediate(_, _) => {
-                let value = operand2.decode_rotated_immediate().unwrap();
-                write!(f, "#{}\t; {:#x}", value, value)
-            }
-            BarrelShifterValue::ShiftedRegister {
-                reg,
-                shift,
-                added: _,
-            } => write!(f, "{}", self.make_shifted_reg_string(reg, shift)),
-            _ => write!(f, "RegisterNotImpl"),
-        }
+        self.fmt_operand2(f).unwrap();
+        Ok(())
     }
 
     fn auto_incremenet_mark(&self) -> &str {
@@ -284,6 +293,27 @@ impl ArmInstruction {
         )
     }
 
+    fn fmt_msr_flags(&self, f: &mut fmt::Formatter) -> fmt::Result {
+        write!(
+            f,
+            "msr{cond}\t{psr}, ",
+            cond = self.cond,
+            psr = if self.spsr_flag() { "SPSR_f" } else { "CPSR_f" },
+        )?;
+        if let Ok(Some(op)) = self.fmt_operand2(f) {
+            let psr = RegPSR::new(op & 0xf000_0000);
+            write!(
+                f,
+                "\t; N={} Z={} C={} V={}",
+                psr.N(),
+                psr.Z(),
+                psr.C(),
+                psr.V()
+            )?;
+        }
+        Ok(())
+    }
+
     fn fmt_mul_mla(&self, f: &mut fmt::Formatter) -> fmt::Result {
         if self.accumulate_flag() {
             write!(
@@ -381,6 +411,7 @@ impl fmt::Display for ArmInstruction {
             LDM_STM => self.fmt_ldm_stm(f),
             MRS => self.fmt_mrs(f),
             MSR_REG => self.fmt_msr_reg(f),
+            MSR_FLAGS => self.fmt_msr_flags(f),
             MUL_MLA => self.fmt_mul_mla(f),
             MULL_MLAL => self.fmt_mull_mlal(f),
             LDR_STR_HS_IMM => self.fmt_ldr_str_hs(f),

src/arm7tdmi/arm/exec.rs

@@ -24,6 +24,7 @@ impl Core {
             ArmFormat::LDR_STR_HS_REG => self.exec_ldr_str_hs(bus, insn),
             ArmFormat::LDM_STM => self.exec_ldm_stm(bus, insn),
             ArmFormat::MSR_REG => self.exec_msr_reg(bus, insn),
+            ArmFormat::MSR_FLAGS => self.exec_msr_flags(bus, insn),
             ArmFormat::MUL_MLA => self.exec_mul_mla(bus, insn),
             _ => Err(CpuError::UnimplementedCpuInstruction(
                 insn.pc,
@@ -90,15 +91,49 @@ impl Core {
         Ok(CpuPipelineAction::IncPC)
     }
 
-    /// Logical/Arithmetic ALU operations
-    ///
-    /// Cycles: 1S+x+y (from GBATEK)
-    ///         Add x=1I cycles if Op2 shifted-by-register. Add y=1S+1N cycles if Rd=R15.
-    fn exec_data_processing(
+    fn exec_msr_flags(
         &mut self,
         _bus: &mut Bus,
         insn: ArmInstruction,
     ) -> CpuResult<CpuPipelineAction> {
+        let op = insn.operand2()?;
+        let op = self.decode_operand2(op)?;
+
+        let old_mode = self.cpsr.mode();
+        if insn.spsr_flag() {
+            if let Some(index) = old_mode.spsr_index() {
+                self.spsr[index].set_flag_bits(op);
+            } else {
+                panic!("tried to change spsr from invalid mode {}", old_mode)
+            }
+        } else {
+            self.cpsr.set_flag_bits(op);
+        }
+        Ok(CpuPipelineAction::IncPC)
+    }
+
+    fn decode_operand2(&mut self, op2: BarrelShifterValue) -> CpuResult<u32> {
+        match op2 {
+            BarrelShifterValue::RotatedImmediate(imm, r) => Ok(imm.rotate_right(r)),
+            BarrelShifterValue::ShiftedRegister {
+                reg,
+                shift,
+                added: _,
+            } => {
+                // +1I
+                self.add_cycle();
+                let result = self.register_shift(reg, shift)?;
+                Ok(result as u32)
+            }
+            _ => unreachable!(),
+        }
+    }
+
+    /// Logical/Arithmetic ALU operations
+    ///
+    /// Cycles: 1S+x+y (from GBATEK)
+    ///         Add x=1I cycles if Op2 shifted-by-register. Add y=1S+1N cycles if Rd=R15.
+    fn exec_data_processing(&mut self, _bus: &mut Bus, insn: ArmInstruction) -> CpuExecResult {
         // TODO handle carry flag
 
         let mut pipeline_action = CpuPipelineAction::IncPC;
@@ -109,26 +144,10 @@ impl Core {
             self.get_reg(insn.rn()) as i32
         };
         let op2 = insn.operand2()?;
+        let op2 = self.decode_operand2(op2)? as i32;
 
         let rd = insn.rd();
 
-        let op2: i32 = match op2 {
-            BarrelShifterValue::RotatedImmediate(immediate, rotate) => {
-                immediate.rotate_right(rotate) as i32
-            }
-            BarrelShifterValue::ShiftedRegister {
-                reg,
-                shift,
-                added: _,
-            } => {
-                // +1I
-                self.add_cycle();
-                let result = self.register_shift(reg, shift)?;
-                result
-            }
-            _ => unreachable!(),
-        };
-
         let opcode = insn.opcode().unwrap();
         let set_flags = opcode.is_setting_flags() || insn.set_cond_flag();
         if let Some(result) = self.alu(opcode, op1, op2, set_flags) {

src/arm7tdmi/psr.rs

@@ -45,6 +45,8 @@ impl Default for RegPSR {
     }
 }
 impl RegPSR {
+    pub const FLAG_BITMASK: u32 = 0xf000_0000;
+
     pub fn new(u: u32) -> RegPSR {
         RegPSR {
             raw: clear_reserved(u),
@@ -59,6 +61,11 @@ impl RegPSR {
         self.raw = clear_reserved(psr);
     }
 
+    pub fn set_flag_bits(&mut self, value: u32) {
+        self.raw &= !Self::FLAG_BITMASK;
+        self.raw |= Self::FLAG_BITMASK & value;
+    }
+
     pub fn state(&self) -> CpuState {
         self.raw.bit(5).into()
     }