Impl more thumb, Fix more things, the usual

Former-commit-id: 02f1898bfd8dd50519f103bb367e358fc55c46e7

src/arm7tdmi/arm/exec.rs

@@ -98,7 +98,7 @@ impl Core {
                 val.wrapping_shl(amount)
             }
             ArmShiftType::LSR => {
-                if amount < 32 {
+                if 0 < amount && amount < 32 {
                     self.cpsr.set_C(val.wrapping_shr(amount - 1) & 1 == 1);
                 } else {
                     self.cpsr.set_C(false);
@@ -106,7 +106,7 @@ impl Core {
                 (val as u32).wrapping_shr(amount) as i32
             }
             ArmShiftType::ASR => {
-                if amount < 32 {
+                if 0 < amount && amount < 32 {
                     self.cpsr.set_C(val.wrapping_shr(amount - 1) & 1 == 1);
                 } else {
                     self.cpsr.set_C(val >> 31 == 1);
@@ -374,7 +374,12 @@ impl Core {
                     addr = addr.wrapping_add(step);
                 }
 
-                self.store_32(addr as Addr, self.get_reg(r), bus);
+                let val = if r == REG_PC {
+                    insn.pc + 12
+                } else {
+                    self.get_reg(r)
+                };
+                self.store_32(addr as Addr, val, bus);
 
                 if !full {
                     addr = addr.wrapping_add(step);

src/arm7tdmi/arm/mod.rs

@@ -452,7 +452,6 @@ mod tests {
         let bytes = vec![];
         let mut mem = BoxedMemory::new(bytes.into_boxed_slice());
 
-
         // swi #0x1337
         let decoded = ArmInstruction::decode(0xef001337, 0).unwrap();
         assert_eq!(decoded.fmt, ArmFormat::SWI);
@@ -464,7 +463,7 @@ mod tests {
             Ok(CpuPipelineAction::Flush)
         );
 
-        assert_eq!(core.cpsr.mode() , CpuMode::Supervisor);
+        assert_eq!(core.cpsr.mode(), CpuMode::Supervisor);
         assert_eq!(core.pc, Exception::SoftwareInterrupt as u32);
     }
 
@@ -547,10 +546,12 @@ mod tests {
         core.gpr[2] = 0;
 
         let bytes = vec![
-            /* 00h: */   0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+            /* 00h: */ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-            /* 10h: */   0x00, 0x00, 0x00, 0x00, 0x37, 0x13, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+            0x00, 0x00, 0x00, 0x00, /* 10h: */ 0x00, 0x00, 0x00, 0x00, 0x37, 0x13, 0x00, 0x00,
-            /* 20h: */   0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+            0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* 20h: */ 0x00, 0x00, 0x00, 0x00,
-            /* 30h: */   0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
+            0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+            /* 30h: */ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+            0x00, 0x00, 0x00, 0x00,
         ];
         let mut mem = BoxedMemory::new(bytes.into_boxed_slice());
 
@@ -590,10 +591,12 @@ mod tests {
         core.gpr[2] = 0xabababab;
 
         let bytes = vec![
-            /* 00h: */   0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+            /* 00h: */ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-            /* 10h: */   0x00, 0x00, 0x00, 0x00, 0x37, 0x13, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+            0x00, 0x00, 0x00, 0x00, /* 10h: */ 0x00, 0x00, 0x00, 0x00, 0x37, 0x13, 0x00, 0x00,
-            /* 20h: */   0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+            0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* 20h: */ 0x00, 0x00, 0x00, 0x00,
-            /* 30h: */   0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
+            0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+            /* 30h: */ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+            0x00, 0x00, 0x00, 0x00,
         ];
         let mut mem = BoxedMemory::new(bytes.into_boxed_slice());
 

src/arm7tdmi/cpu.rs

@@ -193,7 +193,7 @@ impl Core {
     }
 
     pub fn cycle_type(&self, addr: Addr) -> MemoryAccessType {
-        if addr == self.memreq || addr == self.memreq + (self.word_size() as Addr) {
+        if addr == self.memreq || addr == self.memreq.wrapping_add(self.word_size() as Addr) {
             Seq
         } else {
             NonSeq

src/arm7tdmi/thumb/display.rs

@@ -97,6 +97,28 @@ impl ThumbInstruction {
         )
     }
 
+    fn fmt_thumb_ldr_str_shb(&self, f: &mut fmt::Formatter) -> fmt::Result {
+        write!(
+            f,
+            "{op}\t{Rd}, [{Rb}, {Ro}]",
+            op = {
+                match (
+                    self.flag(ThumbInstruction::FLAG_SIGN_EXTEND),
+                    self.flag(ThumbInstruction::FLAG_HALFWORD),
+                ) {
+                    (false, false) => "strh",
+                    (false, true) => "ldrh",
+                    (true, false) => "ldsb",
+                    (true, true) => "ldsh",
+                    _ => panic!("invalid flags"),
+                }
+            },
+            Rd = reg_string(self.rd()),
+            Rb = reg_string(self.rb()),
+            Ro = reg_string(self.ro()),
+        )
+    }
+
     fn fmt_thumb_ldr_str_imm_offset(&self, f: &mut fmt::Formatter) -> fmt::Result {
         write!(
             f,
@@ -141,6 +163,20 @@ impl ThumbInstruction {
         )
     }
 
+    fn fmt_thumb_ldr_address(&self, f: &mut fmt::Formatter) -> fmt::Result {
+        write!(
+            f,
+            "add\t{Rd}, {r}, #{Imm:#x}",
+            Rd = reg_string(self.rd()),
+            r = if self.flag(ThumbInstruction::FLAG_SP) {
+                "sp"
+            } else {
+                "pc"
+            },
+            Imm = self.word8(),
+        )
+    }
+
     fn fmt_thumb_add_sub(&self, f: &mut fmt::Formatter) -> fmt::Result {
         let operand = if self.is_immediate_operand() {
             format!("#{:x}", self.raw.bit_range(6..9))
@@ -150,7 +186,7 @@ impl ThumbInstruction {
 
         write!(
             f,
-            "{op}\t{Rd}, [{Rs}, {operand}]",
+            "{op}\t{Rd}, {Rs}, {operand}",
             op = if self.is_subtract() { "sub" } else { "add" },
             Rd = reg_string(self.rd()),
             Rs = reg_string(self.rs()),
@@ -185,6 +221,26 @@ impl ThumbInstruction {
         write!(f, "}}")
     }
 
+    fn fmt_thumb_ldm_stm(&self, f: &mut fmt::Formatter) -> fmt::Result {
+        write!(
+            f,
+            "{op}\t{Rb}!, {{",
+            op = if self.is_load() { "ldm" } else { "stm" },
+            Rb = reg_string(self.rb()),
+        )?;
+        let mut register_list = self.register_list().into_iter();
+        let mut has_reg = false;
+        if let Some(reg) = register_list.next() {
+            write!(f, "{}", reg_string(reg))?;
+            has_reg = true;
+        }
+        for reg in register_list {
+            has_reg = true;
+            write!(f, ", {}", reg_string(reg))?;
+        }
+        write!(f, "}}")
+    }
+
     fn fmt_thumb_branch_with_cond(&self, f: &mut fmt::Formatter) -> fmt::Result {
         write!(
             f,
@@ -231,11 +287,14 @@ impl fmt::Display for ThumbInstruction {
             ThumbFormat::HiRegOpOrBranchExchange => self.fmt_thumb_high_reg_op_or_bx(f),
             ThumbFormat::LdrPc => self.fmt_thumb_ldr_pc(f),
             ThumbFormat::LdrStrRegOffset => self.fmt_thumb_ldr_str_reg_offset(f),
+            ThumbFormat::LdrStrSHB => self.fmt_thumb_ldr_str_shb(f),
             ThumbFormat::LdrStrImmOffset => self.fmt_thumb_ldr_str_imm_offset(f),
             ThumbFormat::LdrStrHalfWord => self.fmt_thumb_ldr_str_halfword(f),
             ThumbFormat::LdrStrSp => self.fmt_thumb_ldr_str_sp(f),
+            ThumbFormat::LdrAddress => self.fmt_thumb_ldr_address(f),
             ThumbFormat::AddSp => self.fmt_thumb_add_sp(f),
             ThumbFormat::PushPop => self.fmt_thumb_push_pop(f),
+            ThumbFormat::LdmStm => self.fmt_thumb_ldm_stm(f),
             ThumbFormat::BranchConditional => self.fmt_thumb_branch_with_cond(f),
             ThumbFormat::Branch => self.fmt_thumb_branch(f),
             ThumbFormat::BranchLongWithLink => self.fmt_thumb_branch_long_with_link(f),

src/arm7tdmi/thumb/exec.rs

@@ -193,6 +193,43 @@ impl Core {
         self.do_exec_thumb_ldr_str(bus, insn, addr)
     }
 
+    fn exec_thumb_ldr_str_shb(&mut self, bus: &mut Bus, insn: ThumbInstruction) -> CpuExecResult {
+        let addr = self
+            .get_reg(insn.rb())
+            .wrapping_add(self.get_reg(insn.ro()));
+        let rd = insn.rd();
+        match (
+            insn.flag(ThumbInstruction::FLAG_SIGN_EXTEND),
+            insn.flag(ThumbInstruction::FLAG_HALFWORD),
+        ) {
+            (false, false) =>
+            /* strh */
+            {
+                self.store_16(addr, self.gpr[rd] as u16, bus)
+            }
+            (false, true) =>
+            /* ldrh */
+            {
+                self.gpr[rd] = self.load_16(addr, bus) as u32
+            }
+            (true, false) =>
+            /* ldsb */
+            {
+                let val = self.load_8(addr, bus) as i8 as i32 as u32;
+                self.gpr[rd] = val;
+            }
+            (true, true) =>
+            /* ldsh */
+            {
+                let val = self.load_16(addr, bus) as i16 as i32 as u32;
+                self.gpr[rd] = val;
+            }
+            _ => panic!("invalid flags"),
+        }
+
+        Ok(CpuPipelineAction::IncPC)
+    }
+
     fn exec_thumb_ldr_str_imm_offset(
         &mut self,
         bus: &mut Bus,
@@ -225,7 +262,25 @@ impl Core {
     }
 
     fn exec_thumb_ldr_str_sp(&mut self, bus: &mut Bus, insn: ThumbInstruction) -> CpuExecResult {
-        let addr = (self.gpr[REG_SP] & !0b10) + 4 + (insn.word8() as Addr);
+        let addr = self.gpr[REG_SP] + (insn.word8() as Addr);
+        self.do_exec_thumb_ldr_str_with_addr(bus, insn, addr)
+    }
+
+    fn exec_thumb_ldr_address(&mut self, bus: &mut Bus, insn: ThumbInstruction) -> CpuExecResult {
+        let addr = if insn.flag(ThumbInstruction::FLAG_SP) {
+            self.gpr[REG_SP] + (insn.word8() as Addr)
+        } else {
+            (insn.pc & !0b10) + 4 + (insn.word8() as Addr)
+        };
+        self.do_exec_thumb_ldr_str_with_addr(bus, insn, addr)
+    }
+
+    fn do_exec_thumb_ldr_str_with_addr(
+        &mut self,
+        bus: &mut Bus,
+        insn: ThumbInstruction,
+        addr: Addr,
+    ) -> CpuExecResult {
         if insn.is_load() {
             let data = self.load_32(addr, bus);
             self.add_cycle();
@@ -263,6 +318,7 @@ impl Core {
             }
             if pc_lr_flag {
                 pop(self, bus, REG_PC);
+                self.pc = self.pc & !1;
                 pipeline_action = CpuPipelineAction::Flush;
             }
             self.add_cycle();
@@ -278,6 +334,32 @@ impl Core {
         Ok(pipeline_action)
     }
 
+    fn exec_thumb_ldm_stm(&mut self, bus: &mut Bus, insn: ThumbInstruction) -> CpuExecResult {
+        // (From GBATEK) Execution Time: nS+1N+1I (POP), (n+1)S+2N+1I (POP PC), or (n-1)S+2N (PUSH).
+
+        let is_load = insn.is_load();
+        let rb = insn.rb();
+        let mut pipeline_action = CpuPipelineAction::IncPC;
+
+        let mut addr = self.gpr[rb];
+        let rlist = insn.register_list();
+        if is_load {
+            for r in rlist {
+                let val = self.load_32(addr, bus);
+                addr += 4;
+                self.add_cycle();
+                self.set_reg(r, val);
+            }
+        } else {
+            for r in rlist.into_iter().rev() {
+                self.store_32(addr, self.gpr[r], bus);
+                addr += 4;
+            }
+        }
+
+        Ok(pipeline_action)
+    }
+
     fn exec_thumb_branch_with_cond(
         &mut self,
         _bus: &mut Bus,
@@ -329,11 +411,14 @@ impl Core {
             ThumbFormat::HiRegOpOrBranchExchange => self.exec_thumb_hi_reg_op_or_bx(bus, insn),
             ThumbFormat::LdrPc => self.exec_thumb_ldr_pc(bus, insn),
             ThumbFormat::LdrStrRegOffset => self.exec_thumb_ldr_str_reg_offset(bus, insn),
+            ThumbFormat::LdrStrSHB => self.exec_thumb_ldr_str_shb(bus, insn),
             ThumbFormat::LdrStrImmOffset => self.exec_thumb_ldr_str_imm_offset(bus, insn),
             ThumbFormat::LdrStrHalfWord => self.exec_thumb_ldr_str_halfword(bus, insn),
             ThumbFormat::LdrStrSp => self.exec_thumb_ldr_str_sp(bus, insn),
+            ThumbFormat::LdrAddress => self.exec_thumb_ldr_address(bus, insn),
             ThumbFormat::AddSp => self.exec_thumb_add_sp(bus, insn),
             ThumbFormat::PushPop => self.exec_thumb_push_pop(bus, insn),
+            ThumbFormat::LdmStm => self.exec_thumb_ldm_stm(bus, insn),
             ThumbFormat::BranchConditional => self.exec_thumb_branch_with_cond(bus, insn),
             ThumbFormat::Branch => self.exec_thumb_branch(bus, insn),
             ThumbFormat::BranchLongWithLink => self.exec_thumb_branch_long_with_link(bus, insn),

src/arm7tdmi/thumb/mod.rs

@@ -199,6 +199,9 @@ impl ThumbInstruction {
     const FLAG_R: usize = 8;
     const FLAG_S: usize = 7;
     const FLAG_LOW_OFFSET: usize = 11;
+    const FLAG_SP: usize = 11;
+    const FLAG_SIGN_EXTEND: usize = 10;
+    const FLAG_HALFWORD: usize = 11;
 
     pub fn rd(&self) -> usize {
         match self.fmt {
@@ -265,7 +268,7 @@ impl ThumbInstruction {
         match self.fmt {
             ThumbFormat::LdrStrRegOffset => self.raw.bit(10),
             ThumbFormat::LdrStrImmOffset => self.raw.bit(12),
-            _ => unreachable!()
+            _ => unreachable!(),
         }
     }
 
@@ -314,6 +317,11 @@ impl ThumbInstruction {
 /// All instructions constants were generated using an ARM assembler.
 mod tests {
     use super::*;
+    use crate::arm7tdmi::{
+        cpu::{Core, CpuPipelineAction},
+        Bus,
+    };
+    use crate::sysbus::BoxedMemory;
 
     #[test]
     fn mov_low_reg() {
@@ -336,11 +344,14 @@ mod tests {
         assert_eq!(core.get_reg(0), 0x27);
     }
 
+    // #[test]
+    // fn decode_add_sub() {
+    //     let insn = ThumbInstruction::decode(0xac19, 0).unwrap();
+    //     assert!(format!("add\tr4, r4"))
+    // }
+
     #[test]
     fn ldr_pc() {
-        use crate::arm7tdmi::cpu::{Core, CpuPipelineAction};
-        use crate::sysbus::BoxedMemory;
-
         // ldr r0, [pc, #4]
         let insn = ThumbInstruction::decode(0x4801, 0x6).unwrap();
 
@@ -363,12 +374,6 @@ mod tests {
 
     #[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]
@@ -400,4 +405,44 @@ mod tests {
         );
         assert_eq!(core.get_reg(2), 0x78);
     }
+
+    #[allow(overflowing_literals)]
+    #[test]
+    fn format8() {
+        let mut core = Core::new();
+        let bytes = vec![
+            /*  0: */ 0xaa, 0xbb, 0xcc, 0xdd, 0xaa, 0xbb, 0xcc, 0xdd,
+            /*  8: */ 0xaa, 0xbb, 0xcc, 0xdd, 0xaa, 0xbb, 0xcc, 0xdd,
+            /* 10: */ 0xaa, 0xbb, 0xcc, 0xdd, 0xaa, 0xbb, 0xcc, 0xdd,
+        ];
+        let mut mem = BoxedMemory::new(bytes.into_boxed_slice());
+
+        
+        core.gpr[4] = 0x12345678;
+        core.gpr[3] = 0x2;
+        core.gpr[0] = 0x4;
+        // strh r4, [r3, r0]
+        let decoded = ThumbInstruction::decode(0x521c, 0).unwrap();
+        assert_eq!(format!("{}", decoded), "strh\tr4, [r3, r0]");
+        core.exec_thumb(&mut mem, decoded).unwrap();
+        assert_eq!(&mem.get_bytes(0x6)[..4], [0x78, 0x56, 0xaa, 0xbb]);
+
+        // ldsb r2, [r7, r1]
+        core.gpr[2] = 0;
+        core.gpr[7] = 0x10;
+        core.gpr[1] = 0x5;
+        let decoded = ThumbInstruction::decode(0x567a, 0).unwrap();
+        assert_eq!(format!("{}", decoded), "ldsb\tr2, [r7, r1]");
+        core.exec_thumb(&mut mem, decoded).unwrap();
+        assert_eq!(core.gpr[2], mem.read_8(0x15) as i8 as u32);
+
+        // ldsh r3, [r4, r2]
+        core.gpr[3] = 0x0;
+        core.gpr[4] = 0x0;
+        core.gpr[2] = 0x6;
+        let decoded = ThumbInstruction::decode(0x5ea3, 0).unwrap();
+        assert_eq!(format!("{}", decoded), "ldsh\tr3, [r4, r2]");
+        core.exec_thumb(&mut mem, decoded).unwrap();
+        assert_eq!(core.gpr[3], 0x5678);
+    }
 }

src/debugger/command.rs

@@ -3,8 +3,8 @@ use crate::arm7tdmi::{Addr, CpuState};
 use crate::disass::Disassembler;
 use crate::GBAError;
 
-use super::{parser::Value, Debugger, DebuggerError, DebuggerResult};
 use super::palette_view::create_palette_view;
+use super::{parser::Value, Debugger, DebuggerError, DebuggerResult};
 
 use ansi_term::Colour;
 
@@ -80,19 +80,19 @@ impl Command {
                     break;
                 }
                 match debugger.gba.step() {
-                    Ok(insn) => {
+                    // Ok(insn) => {
-                        println!(
+                    //     println!(
-                            "@0x{:08x}:\t{}",
+                    //         "@0x{:08x}:\t{}",
-                            insn.get_pc(),
+                    //         insn.get_pc(),
-                            Colour::Yellow.italic().paint(format!("{} ", insn))
+                    //         Colour::Yellow.italic().paint(format!("{} ", insn))
-                        );
+                    //     );
-                    }
+                    // }
                     Err(GBAError::CpuError(e)) => {
                         println!("{}: {}", "cpu encountered an error".red(), e);
                         println!("cpu: {:x?}", debugger.gba.cpu);
                         break;
                     }
-                    _ => unreachable!(),
+                    _ => (),
                 };
             },
             HexDump(addr, nbytes) => {

src/debugger/palette_view.rs

@@ -43,7 +43,6 @@ impl ColoredRect {
 }
 
 pub fn create_palette_view(palette_ram: &[u8]) {
-
     let palette = Palette::from(palette_ram);
 
     let sdl_context = sdl2::init().unwrap();