cpu: Kinda implement data processing instructions

When I say "Kinda", I mean that it is not tested well.

src/arm7tdmi/arm/display.rs

@@ -1,6 +1,6 @@
 use super::super::{reg_string, REG_PC};
 use super::{
-    ArmCond, ArmHalfwordTransferType, ArmInstruction, ArmInstructionFormat, ArmOpCode, ArmShift,
+    ArmCond, ArmHalfwordTransferType, ArmInstruction, ArmInstructionFormat, ArmOpCode, ArmRegisterShift,
     ArmShiftType, ArmShiftedValue,
 };
 use std::fmt;
@@ -75,23 +75,23 @@ impl fmt::Display for ArmHalfwordTransferType {
     }
 }
 
-fn is_shift(shift: &ArmShift) -> bool {
-    if let ArmShift::ImmediateShift(val, typ) = shift {
+fn is_shift(shift: &ArmRegisterShift) -> bool {
+    if let ArmRegisterShift::ShiftAmount(val, typ) = shift {
         return !(*val == 0 && *typ == ArmShiftType::LSL);
     }
     true
 }
 
 impl ArmInstruction {
-    fn make_shifted_reg_string(&self, reg: usize, shift: ArmShift) -> String {
+    fn make_shifted_reg_string(&self, reg: usize, shift: ArmRegisterShift) -> String {
         let reg = reg_string(reg).to_string();
         if !is_shift(&shift) {
             return reg;
         }
 
         match shift {
-            ArmShift::ImmediateShift(imm, typ) => format!("{}, {} #{}", reg, typ, imm),
-            ArmShift::RegisterShift(rs, typ) => format!("{}, {} {}", reg, typ, reg_string(rs)),
+            ArmRegisterShift::ShiftAmount(imm, typ) => format!("{}, {} #{}", reg, typ, imm),
+            ArmRegisterShift::ShiftRegister(rs, typ) => format!("{}, {} {}", reg, typ, reg_string(rs)),
         }
     }
 

src/arm7tdmi/arm/exec.rs

@@ -1,11 +1,61 @@
-use super::super::cpu::{Core, CpuState, CpuPipelineAction, CpuError, CpuInstruction, CpuResult, CpuExecResult};
+use super::super::cpu::{
+    Core, CpuError, CpuExecResult, CpuInstruction, CpuPipelineAction, CpuResult,
+};
+use super::super::psr::CpuState;
 use super::super::sysbus::SysBus;
-use super::{ArmInstruction, ArmInstructionFormat};
+use super::{
+    ArmCond, ArmInstruction, ArmInstructionFormat, ArmOpCode, ArmRegisterShift, ArmShiftType,
+    ArmShiftedValue,
+};
 
 use crate::bit::BitIndex;
 
 impl Core {
-    fn exec_b_bl(&mut self, sysbus: &mut SysBus, insn: ArmInstruction) -> CpuResult<CpuPipelineAction> {
+    fn check_arm_cond(&self, cond: ArmCond) -> bool {
+        use ArmCond::*;
+        match cond {
+            Equal => self.cpsr.Z(),
+            NotEqual => !self.cpsr.Z(),
+            UnsignedHigherOrSame => self.cpsr.C(),
+            UnsignedLower => !self.cpsr.C(),
+            Negative => self.cpsr.N(),
+            PositiveOrZero => !self.cpsr.N(),
+            Overflow => self.cpsr.V(),
+            NoOverflow => !self.cpsr.V(),
+            UnsignedHigher => self.cpsr.C() && !self.cpsr.Z(),
+            UnsignedLowerOrSame => !self.cpsr.C() && self.cpsr.Z(),
+            GreaterOrEqual => self.cpsr.N() == self.cpsr.V(),
+            LessThan => self.cpsr.N() != self.cpsr.V(),
+            GreaterThan => !self.cpsr.Z() && (self.cpsr.N() == self.cpsr.V()),
+            LessThanOrEqual => self.cpsr.Z() || (self.cpsr.N() != self.cpsr.V()),
+            Always => true,
+        }
+    }
+
+    pub fn exec_arm(&mut self, sysbus: &mut SysBus, insn: ArmInstruction) -> CpuExecResult {
+        let action = if self.check_arm_cond(insn.cond) {
+            match insn.fmt {
+                ArmInstructionFormat::BX => self.exec_bx(sysbus, insn),
+                ArmInstructionFormat::B_BL => self.exec_b_bl(sysbus, insn),
+                ArmInstructionFormat::DP => self.exec_data_processing(sysbus, insn),
+                _ => Err(CpuError::UnimplementedCpuInstruction(CpuInstruction::Arm(
+                    insn,
+                ))),
+            }
+        } else {
+            Ok(CpuPipelineAction::AdvanceProgramCounter)
+        }?;
+        Ok((CpuInstruction::Arm(insn), action))
+    }
+
+    fn exec_b_bl(
+        &mut self,
+        _sysbus: &mut SysBus,
+        insn: ArmInstruction,
+    ) -> CpuResult<CpuPipelineAction> {
+        if self.verbose && insn.cond != ArmCond::Always {
+            println!("branch taken!")
+        }
         if insn.link_flag() {
             self.set_reg(14, self.pc & !0b1);
         }
@@ -13,23 +63,113 @@ impl Core {
         Ok(CpuPipelineAction::Branch)
     }
 
-    fn exec_bx(&mut self, sysbus: &mut SysBus, insn: ArmInstruction) -> CpuResult<CpuPipelineAction> {
+    fn exec_bx(
+        &mut self,
+        _sysbus: &mut SysBus,
+        insn: ArmInstruction,
+    ) -> CpuResult<CpuPipelineAction> {
         let rn = self.get_reg(insn.rn());
         if rn.bit(0) {
-            self.set_state(CpuState::THUMB);
+            self.cpsr.set_state(CpuState::THUMB);
         } else {
-            self.set_state(CpuState::ARM);
+            self.cpsr.set_state(CpuState::ARM);
         }
 
         Ok(CpuPipelineAction::Branch)
     }
 
-    pub fn exec_arm(&mut self, sysbus: &mut SysBus, insn: ArmInstruction) -> CpuExecResult {
-        let action = match insn.fmt {
-            ArmInstructionFormat::BX => self.exec_bx(sysbus, insn),
-            ArmInstructionFormat::B_BL => self.exec_b_bl(sysbus, insn),
-            fmt => Err(CpuError::UnimplementedCpuInstruction(CpuInstruction::Arm(insn))),
-        }?;
-        Ok((CpuInstruction::Arm(insn), action))
+    fn do_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,
+            ArmShiftType::ASR => val.wrapping_shr(amount),
+            ArmShiftType::ROR => val.rotate_right(amount),
+        }
+    }
+
+    fn register_shift(&mut self, reg: usize, shift: ArmRegisterShift) -> i32 {
+        let val = self.get_reg(reg) as i32;
+        match shift {
+            ArmRegisterShift::ShiftAmount(amount, shift) => Core::do_shift(val, amount, shift),
+            ArmRegisterShift::ShiftRegister(reg, shift) => {
+                Core::do_shift(val, self.get_reg(reg) & 0xff, shift)
+            }
+        }
+    }
+
+    fn alu_sub_update_carry(a: i32, b: i32, carry: &mut bool) -> i32 {
+        let res = a.wrapping_sub(b);
+        *carry = res > a;
+        res
+    }
+
+    fn alu_add_update_carry(a: i32, b: i32, carry: &mut bool) -> i32 {
+        let res = a.wrapping_sub(b);
+        *carry = res < a;
+        res
+    }
+
+    fn alu(&mut self, opcode: ArmOpCode, op1: i32, op2: i32, set_cond_flags: bool) -> Option<i32> {
+        let C = self.cpsr.C() as i32;
+
+        let mut carry = self.cpsr.C();
+        let mut overflow = self.cpsr.V();
+
+        let result = match opcode {
+            ArmOpCode::AND | ArmOpCode::TST => op1 & op2,
+            ArmOpCode::EOR | ArmOpCode::TEQ => op1 ^ op2,
+            ArmOpCode::SUB | ArmOpCode::CMP => Self::alu_sub_update_carry(op1, op2, &mut carry),
+            ArmOpCode::RSB => Self::alu_sub_update_carry(op2, op1, &mut carry),
+            ArmOpCode::ADD | ArmOpCode::CMN => Self::alu_add_update_carry(op1, op2, &mut carry),
+            ArmOpCode::ADC => Self::alu_add_update_carry(op1, op2.wrapping_add(C), &mut carry),
+            ArmOpCode::SBC => Self::alu_add_update_carry(op1, op2.wrapping_sub(1 - C), &mut carry),
+            ArmOpCode::RSC => Self::alu_add_update_carry(op2, op1.wrapping_sub(1 - C), &mut carry),
+            ArmOpCode::ORR => op1 | op2,
+            ArmOpCode::MOV => op2,
+            ArmOpCode::BIC => op1 & (!op2),
+            ArmOpCode::MVN => !op2,
+        };
+
+        if set_cond_flags {
+            self.cpsr.set_N(result < 0);
+            self.cpsr.set_Z(result == 0);
+            self.cpsr.set_C(carry);
+            self.cpsr.set_V(overflow);
+        }
+
+        match opcode {
+            ArmOpCode::TST | ArmOpCode::TEQ | ArmOpCode::CMP | ArmOpCode::CMN => None,
+            _ => Some(result),
+        }
+    }
+
+    fn exec_data_processing(
+        &mut self,
+        _sysbus: &mut SysBus,
+        insn: ArmInstruction,
+    ) -> CpuResult<CpuPipelineAction> {
+        // TODO handle carry flag
+
+        let op1 = self.get_reg(insn.rn()) as i32;
+        let op2 = insn.operand2()?;
+
+        let op2: i32 = match op2 {
+            ArmShiftedValue::RotatedImmediate(immediate, rotate) => {
+                immediate.rotate_right(rotate) as i32
+            }
+            ArmShiftedValue::ShiftedRegister {
+                reg,
+                shift,
+                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()) {
+            self.set_reg(insn.rd(), result as u32)
+        }
+
+        Ok(CpuPipelineAction::AdvanceProgramCounter)
     }
 }

src/arm7tdmi/arm/mod.rs

@@ -184,12 +184,12 @@ pub enum ArmShiftType {
 }
 
 #[derive(Debug, PartialEq)]
-pub enum ArmShift {
-    ImmediateShift(u32, ArmShiftType),
-    RegisterShift(usize, ArmShiftType),
+pub enum ArmRegisterShift {
+    ShiftAmount(u32, ArmShiftType),
+    ShiftRegister(usize, ArmShiftType),
 }
 
-impl TryFrom<u32> for ArmShift {
+impl TryFrom<u32> for ArmRegisterShift {
     type Error = ArmDecodeErrorKind;
 
     fn try_from(v: u32) -> Result<Self, Self::Error> {
@@ -199,10 +199,10 @@ impl TryFrom<u32> for ArmShift {
         }?;
         if v.bit(4) {
             let rs = v.bit_range(8..12) as usize;
-            Ok(ArmShift::RegisterShift(rs, typ))
+            Ok(ArmRegisterShift::ShiftRegister(rs, typ))
         } else {
             let amount = v.bit_range(7..12) as u32;
-            Ok(ArmShift::ImmediateShift(amount, typ))
+            Ok(ArmRegisterShift::ShiftAmount(amount, typ))
         }
     }
 }
@@ -213,7 +213,7 @@ pub enum ArmShiftedValue {
     RotatedImmediate(u32, u32),
     ShiftedRegister {
         reg: usize,
-        shift: ArmShift,
+        shift: ArmRegisterShift,
         added: Option<bool>,
     },
 }
@@ -338,7 +338,7 @@ impl ArmInstruction {
         let ofs = self.raw.bit_range(0..12);
         if self.raw.bit(25) {
             let rm = ofs & 0xf;
-            let shift = ArmShift::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,
@@ -367,7 +367,7 @@ impl ArmInstruction {
             }
             ArmInstructionFormat::LDR_STR_HS_REG => Ok(ArmShiftedValue::ShiftedRegister {
                 reg: (self.raw & 0xf) as usize,
-                shift: ArmShift::ImmediateShift(0, ArmShiftType::LSL),
+                shift: ArmRegisterShift::ShiftAmount(0, ArmShiftType::LSL),
                 added: Some(self.add_offset_flag()),
             }),
             _ => Err(self.make_decode_error(DecodedPartDoesNotBelongToInstruction)),
@@ -382,7 +382,7 @@ impl ArmInstruction {
             Ok(ArmShiftedValue::RotatedImmediate(immediate, rotate))
         } else {
             let reg = op2 & 0xf;
-            let shift = ArmShift::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,

src/arm7tdmi/cpu.rs

@@ -69,7 +69,7 @@ pub struct Core {
 
 #[derive(Debug, PartialEq)]
 pub enum CpuPipelineAction {
-    AdvancePc,
+    AdvanceProgramCounter,
     Branch,
 }
 
@@ -145,7 +145,7 @@ impl Core {
             }
         }
 
-        if CpuPipelineAction::AdvancePc == pipeline_action {
+        if CpuPipelineAction::AdvanceProgramCounter == pipeline_action {
             self.advance_pc();
         }
 

src/arm7tdmi/mod.rs

@@ -1,3 +1,4 @@
+mod psr;
 pub mod arm;
 pub mod cpu;
 pub use super::sysbus;

src/main.rs

@@ -91,7 +91,7 @@ fn run_debug(matches: &ArgMatches) -> GBAResult<()> {
     println!("Loading BIOS: {}", gba_bios_path);
     let bios_bin = read_bin_file(gba_bios_path)?;
 
-    let mut sysbus = SysBus::new(bios_bin);
+    let sysbus = SysBus::new(bios_bin);
     let mut core = cpu::Core::new();
     core.set_verbose(true);
     let mut debugger = Debugger::new(core, sysbus);