cpu: Add SWI instruction

Also cleanup code, and add a test for swi decoding

src/arm7tdmi/arm/display.rs

@@ -1,9 +1,10 @@
+use std::fmt;
+
 use super::super::{reg_string, REG_PC};
 use super::{
-    ArmCond, ArmHalfwordTransferType, ArmInstruction, ArmInstructionFormat, ArmOpCode, ArmRegisterShift,
+    ArmCond, ArmHalfwordTransferType, ArmInstruction, ArmInstructionFormat, ArmOpCode,
-    ArmShiftType, ArmShiftedValue,
+    ArmRegisterShift, ArmShiftType, ArmShiftedValue,
 };
-use std::fmt;
 
 impl fmt::Display for ArmCond {
     fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
@@ -91,7 +92,9 @@ impl ArmInstruction {
 
         match shift {
             ArmRegisterShift::ShiftAmount(imm, typ) => format!("{}, {} #{}", reg, typ, imm),
-            ArmRegisterShift::ShiftRegister(rs, typ) => format!("{}, {} {}", reg, typ, reg_string(rs)),
+            ArmRegisterShift::ShiftRegister(rs, typ) => {
+                format!("{}, {} {}", reg, typ, reg_string(rs))
+            }
         }
     }
 
@@ -351,6 +354,15 @@ impl ArmInstruction {
             write!(f, "<undefined>")
         }
     }
+
+    fn fmt_swi(&self, f: &mut fmt::Formatter) -> fmt::Result {
+        write!(
+            f,
+            "swi{cond}\t#0x{comm:08x}",
+            cond = self.cond,
+            comm = self.swi_comment()
+        )
+    }
 }
 
 impl fmt::Display for ArmInstruction {
@@ -368,6 +380,7 @@ impl fmt::Display for ArmInstruction {
             MULL_MLAL => self.fmt_mull_mlal(f),
             LDR_STR_HS_IMM => self.fmt_ldr_str_hs(f),
             LDR_STR_HS_REG => self.fmt_ldr_str_hs(f),
+            SWI => self.fmt_swi(f),
             _ => write!(f, "({:?})", self),
         }
     }

src/arm7tdmi/arm/exec.rs

@@ -1,14 +1,16 @@
-use super::super::cpu::{
+use crate::bit::BitIndex;
-    Core, CpuError, CpuExecResult, CpuInstruction, CpuPipelineAction, CpuResult, CpuState
+
+use crate::arm7tdmi;
+use arm7tdmi::cpu::{
+    Core, CpuError, CpuExecResult, CpuInstruction, CpuPipelineAction, CpuResult, CpuState, Exception
 };
+
 use super::super::sysbus::SysBus;
 use super::{
     ArmCond, ArmInstruction, ArmInstructionFormat, ArmOpCode, ArmRegisterShift, ArmShiftType,
     ArmShiftedValue,
 };
 
-use crate::bit::BitIndex;
-
 impl Core {
     fn check_arm_cond(&self, cond: ArmCond) -> bool {
         use ArmCond::*;
@@ -37,6 +39,7 @@ impl Core {
                 ArmInstructionFormat::BX => self.exec_bx(sysbus, insn),
                 ArmInstructionFormat::B_BL => self.exec_b_bl(sysbus, insn),
                 ArmInstructionFormat::DP => self.exec_data_processing(sysbus, insn),
+                ArmInstructionFormat::SWI => self.exec_swi(sysbus, insn),
                 _ => Err(CpuError::UnimplementedCpuInstruction(CpuInstruction::Arm(
                     insn,
                 ))),
@@ -77,7 +80,16 @@ impl Core {
         Ok(CpuPipelineAction::Branch)
     }
 
-    fn do_shift(val: i32, amount: u32, shift: ArmShiftType) -> i32 {
+    fn exec_swi(
+        &mut self,
+        _sysbus: &mut SysBus,
+        _insn: ArmInstruction,
+    ) -> CpuResult<CpuPipelineAction> {
+        self.exception(Exception::SoftwareInterrupt);
+        Ok(CpuPipelineAction::Branch)
+    }
+
+    fn barrel_shift(val: i32, amount: u32, shift: ArmShiftType) -> i32 {
         match shift {
             ArmShiftType::LSL => val.wrapping_shl(amount),
             ArmShiftType::LSR => (val as u32).wrapping_shr(amount) as i32,
@@ -86,12 +98,18 @@ impl Core {
         }
     }
 
-    fn register_shift(&mut self, reg: usize, shift: ArmRegisterShift) -> i32 {
+    fn register_shift(&mut self, reg: usize, shift: ArmRegisterShift) -> CpuResult<i32> {
         let val = self.get_reg(reg) as i32;
         match shift {
-            ArmRegisterShift::ShiftAmount(amount, shift) => Core::do_shift(val, amount, shift),
+            ArmRegisterShift::ShiftAmount(amount, shift) => {
+                Ok(Core::barrel_shift(val, amount, shift))
+            }
             ArmRegisterShift::ShiftRegister(reg, shift) => {
-                Core::do_shift(val, self.get_reg(reg) & 0xff, shift)
+                if reg != arm7tdmi::REG_PC {
+                    Ok(Core::barrel_shift(val, self.get_reg(reg) & 0xff, shift))
+                } else {
+                    Err(CpuError::IllegalInstruction)
+                }
             }
         }
     }
@@ -154,7 +172,7 @@ impl Core {
 
         let op2: i32 = match op2 {
             ArmShiftedValue::RotatedImmediate(immediate, rotate) => {
-                immediate.rotate_right(rotate) as i32
+                Ok(immediate.rotate_right(rotate) as i32)
             }
             ArmShiftedValue::ShiftedRegister {
                 reg,
@@ -162,10 +180,11 @@ impl Core {
                 added: _,
             } => self.register_shift(reg, shift),
             _ => unreachable!(),
-        };
+        }?;
 
         let opcode = insn.opcode().unwrap();
-        if let Some(result) = self.alu(opcode, op1, op2, insn.set_cond_flag()) {
+        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(insn.rd(), result as u32)
         }
 

src/arm7tdmi/arm/mod.rs

@@ -54,31 +54,33 @@ pub enum ArmCond {
 #[derive(Debug, Copy, Clone, PartialEq)]
 #[allow(non_camel_case_types)]
 pub enum ArmInstructionFormat {
-    // Branch and Exchange
+    /// Branch and Exchange
     BX,
-    // Branch /w Link
+    /// Branch /w Link
     B_BL,
+    /// Software interrupt
+    SWI,
     // Multiply and Multiply-Accumulate
     MUL_MLA,
-    // Multiply Long and Multiply-Accumulate Long
+    /// Multiply Long and Multiply-Accumulate Long
     MULL_MLAL,
-    // Single Data Transfer
+    /// Single Data Transfer
     LDR_STR,
-    // Halfword and Signed Data Transfer
+    /// Halfword and Signed Data Transfer
     LDR_STR_HS_REG,
-    // Halfword and Signed Data Transfer
+    /// Halfword and Signed Data Transfer
     LDR_STR_HS_IMM,
-    // Data Processing
+    /// Data Processing
     DP,
-    // Block Data Transfer
+    /// Block Data Transfer
     LDM_STM,
-    // Single Data Swap
+    /// Single Data Swap
     SWP,
-    // Transfer PSR contents to a register
+    /// Transfer PSR contents to a register
     MRS,
-    // Transfer register contents to PSR
+    /// Transfer register contents to PSR
     MSR_REG,
-    // Tanssfer immediate/register to PSR flags only
+    /// Tanssfer immediate/register to PSR flags only
     MSR_FLAGS,
 }
 
@@ -102,6 +104,15 @@ pub enum ArmOpCode {
     MVN = 0b1111,
 }
 
+impl ArmOpCode {
+    pub fn is_setting_flags(&self) -> bool {
+        match self {
+            ArmOpCode::TST | ArmOpCode::TEQ | ArmOpCode::CMP | ArmOpCode::CMN => true,
+            _ => false,
+        }
+    }
+}
+
 #[derive(Debug, PartialEq, Primitive)]
 pub enum ArmHalfwordTransferType {
     UnsignedHalfwords = 0b01,
@@ -160,6 +171,8 @@ impl TryFrom<(u32, u32)> for ArmInstruction {
             Ok(LDR_STR_HS_IMM)
         } else if (0x0e00_0000 & raw) == 0x0800_0000 {
             Ok(LDM_STM)
+        } else if (0x0f00_0000 & raw) == 0x0f00_0000 {
+            Ok(SWI)
         } else if (0x0c00_0000 & raw) == 0x0000_0000 {
             Ok(DP)
         } else {
@@ -175,6 +188,14 @@ impl TryFrom<(u32, u32)> for ArmInstruction {
     }
 }
 
+impl TryFrom<u32> for ArmInstruction {
+    type Error = ArmDecodeError;
+
+    fn try_from(value: u32) -> Result<Self, Self::Error> {
+        ArmInstruction::try_from((value, 0))
+    }
+}
+
 #[derive(Debug, PartialEq, Primitive)]
 pub enum ArmShiftType {
     LSL = 0,
@@ -274,7 +295,7 @@ impl ArmInstruction {
     }
 
     pub fn branch_offset(&self) -> i32 {
-        ((((self.raw << 8) as i32) >> 8) << 2) + 8
+        ((self.raw.bit_range(0..24) << 2) as i32) + 8
     }
 
     pub fn load_flag(&self) -> bool {
@@ -338,7 +359,8 @@ impl ArmInstruction {
         let ofs = self.raw.bit_range(0..12);
         if self.raw.bit(25) {
             let rm = ofs & 0xf;
-            let shift = ArmRegisterShift::try_from(ofs).map_err(|kind| self.make_decode_error(kind))?;
+            let shift =
+                ArmRegisterShift::try_from(ofs).map_err(|kind| self.make_decode_error(kind))?;
             Ok(ArmShiftedValue::ShiftedRegister {
                 reg: rm as usize,
                 shift: shift,
@@ -382,7 +404,8 @@ impl ArmInstruction {
             Ok(ArmShiftedValue::RotatedImmediate(immediate, rotate))
         } else {
             let reg = op2 & 0xf;
-            let shift = ArmRegisterShift::try_from(op2).map_err(|kind| self.make_decode_error(kind))?; // TODO error handling
+            let shift =
+                ArmRegisterShift::try_from(op2).map_err(|kind| self.make_decode_error(kind))?; // TODO error handling
             Ok(ArmShiftedValue::ShiftedRegister {
                 reg: reg as usize,
                 shift: shift,
@@ -401,4 +424,23 @@ impl ArmInstruction {
         }
         list
     }
+
+    pub fn swi_comment(&self) -> u32 {
+        self.raw.bit_range(0..24)
+    }
+}
+
+
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    #[test]
+    fn test_swi() {
+        // swi #0x1337
+        let decoded = ArmInstruction::try_from(0xef001337).unwrap();
+        assert_eq!(decoded.fmt, ArmInstructionFormat::SWI);
+        assert_eq!(decoded.swi_comment(), 0x1337);
+    }
 }

src/arm7tdmi/cpu.rs

@@ -5,7 +5,7 @@ use crate::num_traits::FromPrimitive;
 use colored::*;
 
 use super::arm::*;
-use super::exception::Exception;
+pub use super::exception::Exception;
 use super::psr::RegPSR;
 use super::reg_string;
 use super::sysbus::SysBus;
@@ -86,7 +86,7 @@ impl fmt::Display for CpuMode {
 #[derive(Debug, PartialEq)]
 pub enum CpuError {
     ArmDecodeError(ArmDecodeError),
-    IllegalInstruction(CpuInstruction),
+    IllegalInstruction,
     UnimplementedCpuInstruction(CpuInstruction),
 }
 
@@ -109,10 +109,9 @@ impl fmt::Display for CpuError {
                 "unimplemented instruction: 0x{:08x}:\t0x{:08x}\t{}",
                 insn.pc, insn.raw, insn
             ),
-            CpuError::IllegalInstruction(CpuInstruction::Arm(insn)) => write!(
+            CpuError::IllegalInstruction => write!(
                 f,
-                "illegal instruction at address @0x{:08x} (0x{:08x})",
+                "illegal instruction"
-                insn.pc, insn.raw
             ),
             e => write!(f, "error: {:#x?}", e),
         }

src/arm7tdmi/mod.rs

@@ -1,11 +1,13 @@
 pub mod arm;
 pub mod cpu;
-mod psr;
 mod exception;
+mod psr;
 
 pub use super::sysbus;
 
 pub const REG_PC: usize = 15;
+pub const REG_LR: usize = 14;
+pub const REG_SP: usize = 13;
 
 pub fn reg_string(reg: usize) -> &'static str {
     let reg_names = &[