Add many thumb instructions..

TODO add more tests for all the instructions I've got implemented so
far.
Also, I need to rewrite the whole "sysbus" module again because it's
crap and I keep refactoring it as I go.
I've added the "Dummy" because the bios for some reason tries to memzero
an unmapped region on the work ram (the thumb loop that ends at 0x126)


Former-commit-id: 67befd0935ee10df9ac8ceeaebd14f69767a7f16

src/arm7tdmi/thumb/exec.rs

@@ -1,11 +1,37 @@
 use crate::arm7tdmi::arm::*;
 use crate::arm7tdmi::bus::{Bus, MemoryAccessType::*, MemoryAccessWidth::*};
 use crate::arm7tdmi::cpu::{Core, CpuExecResult, CpuPipelineAction};
-use crate::arm7tdmi::{Addr, REG_PC};
+use crate::arm7tdmi::{Addr, REG_PC, CpuState};
 
-use super::{ThumbFormat, ThumbInstruction};
+use super::*;
 
 impl Core {
+    fn exec_thumb_add_sub(&mut self, bus: &mut Bus, insn: ThumbInstruction) -> CpuExecResult {
+        let op1 = self.get_reg(insn.rs()) as i32;
+        let op2 = if insn.is_immediate_operand() {
+            insn.rn() as u32 as i32
+        } else {
+            self.get_reg(insn.rn()) as i32
+        };
+        let arm_alu_op = if insn.is_subtract() {
+            ArmOpCode::SUB
+        } else {
+            ArmOpCode::ADD
+        };
+
+        let result = self.alu(arm_alu_op, op1, op2, true);
+        if let Some(result) = result {
+            self.set_reg(insn.rd(), result as u32);
+        }
+        // +1S
+        self.add_cycles(
+            insn.pc + (self.word_size() as u32),
+            bus,
+            Seq + MemoryAccess32,
+        );
+        Ok(CpuPipelineAction::IncPC)
+    }
+
     fn exec_thumb_data_process_imm(
         &mut self,
         bus: &mut Bus,
@@ -19,13 +45,61 @@ impl Core {
             self.set_reg(insn.rd(), result as u32);
         }
         // +1S
+        self.add_cycles(
+            insn.pc + (self.word_size() as u32),
+            bus,
+            Seq + MemoryAccess16,
+        );
+        Ok(CpuPipelineAction::IncPC)
+    }
+
+    /// Cycles 2S+1N
+    fn exec_thumb_bx(
+        &mut self,
+        bus: &mut Bus,
+        insn: ThumbInstruction,
+    ) -> CpuExecResult {
+        let src_reg = if insn.flag(ThumbInstruction::FLAG_H2) {
+            insn.rs() + 8
+        } else {
+            insn.rs()
+        };
+
+        let addr = self.get_reg(src_reg);
+        if addr.bit(0) {
+            self.cpsr.set_state(CpuState::THUMB);
+        } else {
+            self.cpsr.set_state(CpuState::ARM);
+        }
+
+        // +1N
+        self.add_cycles(self.pc, bus, NonSeq + MemoryAccess32);
+
+        self.pc = addr & !1;
+
+        // +2S
+        self.add_cycles(self.pc, bus, Seq + MemoryAccess32);
         self.add_cycles(
             self.pc + (self.word_size() as u32),
             bus,
             Seq + MemoryAccess32,
         );
+
+        Ok(CpuPipelineAction::Flush)
+    }
+
+    fn exec_thumb_hi_reg_op_or_bx(
+        &mut self,
+        bus: &mut Bus,
+        insn: ThumbInstruction,
+    ) -> CpuExecResult {
+        if OpFormat5::BX == insn.format5_op() {
+            self.exec_thumb_bx(bus, insn)
+        } else {
+            unimplemented!("Sorry, I'm tired");
             Ok(CpuPipelineAction::IncPC)
         }
+    }
 
     fn exec_thumb_ldr_pc(&mut self, bus: &mut Bus, insn: ThumbInstruction) -> CpuExecResult {
         let addr = (insn.pc & !0b10) + 4 + (insn.word8() as Addr);
@@ -34,9 +108,9 @@ impl Core {
         self.add_cycles(addr, bus, NonSeq + MemoryAccess32);
         // +1S
         self.add_cycles(
-            self.pc + (self.word_size() as u32),
+            insn.pc + (self.word_size() as u32),
             bus,
-            Seq + MemoryAccess32,
+            Seq + MemoryAccess16,
         );
         self.set_reg(insn.rd(), data);
         // +1I
@@ -45,11 +119,93 @@ impl Core {
         Ok(CpuPipelineAction::IncPC)
     }
 
+    fn exec_thumb_ldr_str_reg_offset(
+        &mut self,
+        bus: &mut Bus,
+        insn: ThumbInstruction,
+    ) -> CpuExecResult {
+        let addr = self
+            .get_reg(insn.rb())
+            .wrapping_add(self.get_reg(insn.ro()));
+        if insn.is_load() {
+            let data = if insn.is_transfering_bytes() {
+                // +1N
+                self.add_cycles(addr, bus, NonSeq + MemoryAccess8);
+                bus.read_8(addr) as u32
+            } else {
+                // +1N
+                self.add_cycles(addr, bus, NonSeq + MemoryAccess32);
+                bus.read_32(addr)
+            };
+
+            // +1S
+            self.add_cycles(
+                insn.pc + (self.word_size() as u32),
+                bus,
+                Seq + MemoryAccess32,
+            );
+
+            self.set_reg(insn.rd(), data);
+
+            // +1I
+            self.add_cycle();
+        } else {
+            self.add_cycles(addr, bus, NonSeq + MemoryAccess32);
+            let value = self.get_reg(insn.rd());
+            if insn.is_transfering_bytes() {
+                // +1N
+                self.add_cycles(addr, bus, NonSeq + MemoryAccess8);
+                bus.write_8(addr, value as u8).expect("bus error");
+            } else {
+                // +1N
+                self.add_cycles(addr, bus, NonSeq + MemoryAccess32);
+                bus.write_32(addr, value).expect("bus error");
+            };
+        }
+
+        Ok(CpuPipelineAction::IncPC)
+    }
+
+    fn exec_thumb_branch_with_cond(
+        &mut self,
+        bus: &mut Bus,
+        insn: ThumbInstruction,
+    ) -> CpuExecResult {
+        if !self.check_arm_cond(insn.cond()) {
+            self.add_cycles(
+                insn.pc + (self.word_size() as u32),
+                bus,
+                Seq + MemoryAccess16,
+            );
+            Ok(CpuPipelineAction::IncPC)
+        } else {
+            // +1N
+            self.add_cycles(insn.pc, bus, NonSeq + MemoryAccess32);
+            let offset = insn.offset8() as i8 as i32;
+            self.pc = (insn.pc as i32).wrapping_add(offset) as u32;
+
+            // +2S
+            self.add_cycles(self.pc, bus, Seq + MemoryAccess32);
+            self.add_cycles(
+                self.pc + (self.word_size() as u32),
+                bus,
+                Seq + MemoryAccess32,
+            );
+
+            Ok(CpuPipelineAction::Flush)
+        }
+    }
+
     pub fn exec_thumb(&mut self, bus: &mut Bus, insn: ThumbInstruction) -> CpuExecResult {
         match insn.fmt {
+            ThumbFormat::AddSub => self.exec_thumb_add_sub(bus, insn),
             ThumbFormat::DataProcessImm => self.exec_thumb_data_process_imm(bus, insn),
+            ThumbFormat::HiRegOpOrBranchExchange => self.exec_thumb_hi_reg_op_or_bx(bus, insn),
             ThumbFormat::LdrPc => self.exec_thumb_ldr_pc(bus, insn),
-            _ => unimplemented!("thumb not implemented {:#}", insn),
+            ThumbFormat::LdrStrRegOffset => self.exec_thumb_ldr_str_reg_offset(bus, insn),
+            ThumbFormat::BranchConditional => self.exec_thumb_branch_with_cond(bus, insn),
+
+            _ => unimplemented!("thumb not implemented {:#?}", insn),
         }
     }
 }

src/arm7tdmi/thumb/mod.rs

@@ -151,7 +151,7 @@ impl InstructionDecoder for ThumbInstruction {
     }
 }
 
-#[derive(Debug, Primitive)]
+#[derive(Debug, Primitive, PartialEq)]
 pub enum OpFormat3 {
     MOV = 0,
     CMP = 1,
@@ -170,7 +170,7 @@ impl From<OpFormat3> for ArmOpCode {
     }
 }
 
-#[derive(Debug, Primitive)]
+#[derive(Debug, Primitive, PartialEq)]
 pub enum OpFormat5 {
     ADD = 0,
     CMP = 1,
@@ -321,4 +321,44 @@ mod tests {
         );
         assert_eq!(core.get_reg(0), 0x12345678);
     }
+
+    #[test]
+    fn ldr_str_reg_offset() {
+        use crate::arm7tdmi::{
+            cpu::{Core, CpuPipelineAction},
+            Bus,
+        };
+        use crate::sysbus::BoxedMemory;
+
+        // str	r0, [r4, r1]
+        let str_insn = ThumbInstruction::decode(0x5060, 0x6).unwrap();
+        // ldrb r2, [r4, r1]
+        let ldr_insn = ThumbInstruction::decode(0x5c62, 0x6).unwrap();
+
+        let mut core = Core::new();
+        core.set_reg(0, 0x12345678);
+        core.set_reg(2, 0);
+        core.set_reg(1, 0x4);
+        core.set_reg(4, 0xc);
+
+        let bytes = vec![
+            /*  0: */ 0xaa, 0xbb, 0xcc, 0xdd, /*  4: */ 0xaa, 0xbb, 0xcc, 0xdd,
+            /*  8: */ 0xaa, 0xbb, 0xcc, 0xdd, /*  c: */ 0xaa, 0xbb, 0xcc, 0xdd,
+            /* 10: */ 0xaa, 0xbb, 0xcc, 0xdd,
+        ];
+        let mut mem = BoxedMemory::new(bytes.into_boxed_slice());
+
+        assert_eq!(format!("{}", str_insn), "str\tr0, [r4, r1]");
+        assert_eq!(format!("{}", ldr_insn), "ldrb\tr2, [r4, r1]");
+        assert_eq!(
+            core.exec_thumb(&mut mem, str_insn),
+            Ok(CpuPipelineAction::IncPC)
+        );
+        assert_eq!(mem.read_32(0x10), 0x12345678);
+        assert_eq!(
+            core.exec_thumb(&mut mem, ldr_insn),
+            Ok(CpuPipelineAction::IncPC)
+        );
+        assert_eq!(core.get_reg(2), 0x78);
+    }
 }

src/sysbus.rs

@@ -65,6 +65,46 @@ impl Bus for BoxedMemory {
     }
 }
 
+#[derive(Debug)]
+struct DummyBus([u8; 4]);
+
+impl Bus for DummyBus {
+    fn read_32(&self, addr: Addr) -> u32 {
+        0
+    }
+
+    fn read_16(&self, addr: Addr) -> u16 {
+        0
+    }
+
+    fn read_8(&self, addr: Addr) -> u8 {
+        0
+    }
+
+    fn write_32(&mut self, addr: Addr, value: u32) -> Result<(), io::Error> {
+        Ok(())
+    }
+
+    fn write_16(&mut self, addr: Addr, value: u16) -> Result<(), io::Error> {
+        Ok(())
+    }
+
+    fn write_8(&mut self, addr: Addr, value: u8) -> Result<(), io::Error> {
+        Ok(())
+    }
+    fn get_bytes(&self, addr: Addr) -> &[u8] {
+        &self.0
+    }
+
+    fn get_bytes_mut(&mut self, addr: Addr) -> &mut [u8] {
+        &mut self.0
+    }
+
+    fn get_cycles(&self, addr: Addr, access: MemoryAccess) -> usize {
+        1
+    }
+}
+
 #[derive(Debug)]
 pub struct SysBus {
     bios: BoxedMemory,
@@ -76,6 +116,7 @@ pub struct SysBus {
     vram: BoxedMemory,
     oam: BoxedMemory,
     gamepak: Cartridge,
+    dummy: DummyBus,
 }
 
 impl SysBus {
@@ -98,6 +139,7 @@ impl SysBus {
             ),
             oam: BoxedMemory::new(vec![0; OAM_SIZE].into_boxed_slice()),
             gamepak: gamepak,
+            dummy: DummyBus([0; 4]),
         }
     }
 
@@ -111,7 +153,10 @@ impl SysBus {
             0x0600_0000...0x0601_7fff => &self.vram,
             0x0700_0000...0x0700_03ff => &self.oam,
             0x0800_0000...0x09ff_ffff => &self.gamepak,
-            _ => panic!("unmapped address @0x{:08x}", addr),
+            _ => {
+                println!("unmapped address @0x{:08x}", addr);
+                &self.dummy
+            }
         }
     }
 
@@ -126,7 +171,10 @@ impl SysBus {
             0x0600_0000...0x0601_7fff => &mut self.vram,
             0x0700_0000...0x0700_03ff => &mut self.oam,
             0x0800_0000...0x09ff_ffff => &mut self.gamepak,
-            _ => panic!("unmapped address @0x{:08x}", addr),
+            _ => {
+                println!("unmapped address @0x{:08x}", addr);
+                &mut self.dummy
+            }
         }
     }
 }