Add thumb push-pop.

Not tested, cycle modeling is crap


Former-commit-id: a5092dab79a1a660fc6c96a71f0908cc2054be27

src/arm7tdmi/thumb/display.rs

@@ -6,7 +6,7 @@ use super::*;
 use crate::arm7tdmi::reg_string;
 
 impl ThumbInstruction {
-    fn fmt_move_shifted_reg(&self, f: &mut fmt::Formatter) -> fmt::Result {
+    fn fmt_thumb_move_shifted_reg(&self, f: &mut fmt::Formatter) -> fmt::Result {
         write!(
             f,
             "{op}\t{Rd}, {Rs}, #{Offset5}",
@@ -17,7 +17,7 @@ impl ThumbInstruction {
         )
     }
 
-    fn fmt_data_process_imm(&self, f: &mut fmt::Formatter) -> fmt::Result {
+    fn fmt_thumb_data_process_imm(&self, f: &mut fmt::Formatter) -> fmt::Result {
         write!(
             f,
             "{op}\t{Rd}, #{Offset8:#x}",
@@ -27,7 +27,7 @@ impl ThumbInstruction {
         )
     }
 
-    fn fmt_high_reg_op_or_bx(&self, f: &mut fmt::Formatter) -> fmt::Result {
+    fn fmt_thumb_high_reg_op_or_bx(&self, f: &mut fmt::Formatter) -> fmt::Result {
         let op = self.format5_op();
         let dst_reg = if self.flag(ThumbInstruction::FLAG_H1) {
             self.rd() + 8
@@ -52,7 +52,7 @@ impl ThumbInstruction {
         }
     }
 
-    fn fmt_ldr_pc(&self, f: &mut fmt::Formatter) -> fmt::Result {
+    fn fmt_thumb_ldr_pc(&self, f: &mut fmt::Formatter) -> fmt::Result {
         write!(
             f,
             "ldr\t{Rd}, [pc, #{Imm:#x}] ; = #{effective:#x}",
@@ -62,7 +62,7 @@ impl ThumbInstruction {
         )
     }
 
-    fn fmt_ldr_str_reg_offset(&self, f: &mut fmt::Formatter) -> fmt::Result {
+    fn fmt_thumb_ldr_str_reg_offset(&self, f: &mut fmt::Formatter) -> fmt::Result {
         write!(
             f,
             "{op}{b}\t{Rd}, [{Rb}, {Ro}]",
@@ -74,7 +74,7 @@ impl ThumbInstruction {
         )
     }
 
-    fn fmt_add_sub(&self, f: &mut fmt::Formatter) -> fmt::Result {
+    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))
         } else {
@@ -91,7 +91,30 @@ impl ThumbInstruction {
         )
     }
 
-    fn fmt_branch_with_cond(&self, f: &mut fmt::Formatter) -> fmt::Result {
+    fn fmt_thumb_push_pop(&self, f: &mut fmt::Formatter) -> fmt::Result {
+        write!(f, "{}\t{{", if self.is_load() { "pop" } else { "push" })?;
+        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))?;
+        };
+        if self.flag(ThumbInstruction::FLAG_R) {
+            let r = if self.is_load() { "pc" } else { "lr" };
+            if has_reg {
+                write!(f, ", {}", r)?;
+            } else {
+                write!(f, "{}", r)?;
+            }
+        }
+        write!(f, "}}")
+    }
+
+    fn fmt_thumb_branch_with_cond(&self, f: &mut fmt::Formatter) -> fmt::Result {
         write!(
             f,
             "b{cond}\t{addr:#x}",
@@ -107,13 +130,14 @@ impl ThumbInstruction {
 impl fmt::Display for ThumbInstruction {
     fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
         match self.fmt {
-            ThumbFormat::MoveShiftedReg => self.fmt_move_shifted_reg(f),
+            ThumbFormat::MoveShiftedReg => self.fmt_thumb_move_shifted_reg(f),
-            ThumbFormat::AddSub => self.fmt_add_sub(f),
+            ThumbFormat::AddSub => self.fmt_thumb_add_sub(f),
-            ThumbFormat::DataProcessImm => self.fmt_data_process_imm(f),
+            ThumbFormat::DataProcessImm => self.fmt_thumb_data_process_imm(f),
-            ThumbFormat::HiRegOpOrBranchExchange => self.fmt_high_reg_op_or_bx(f),
+            ThumbFormat::HiRegOpOrBranchExchange => self.fmt_thumb_high_reg_op_or_bx(f),
-            ThumbFormat::LdrPc => self.fmt_ldr_pc(f),
+            ThumbFormat::LdrPc => self.fmt_thumb_ldr_pc(f),
-            ThumbFormat::LdrStrRegOffset => self.fmt_ldr_str_reg_offset(f),
+            ThumbFormat::LdrStrRegOffset => self.fmt_thumb_ldr_str_reg_offset(f),
-            ThumbFormat::BranchConditional => self.fmt_branch_with_cond(f),
+            ThumbFormat::PushPop => self.fmt_thumb_push_pop(f),
+            ThumbFormat::BranchConditional => self.fmt_thumb_branch_with_cond(f),
             _ => write!(f, "({:?})", self),
         }
     }

src/arm7tdmi/thumb/exec.rs

@@ -1,9 +1,21 @@
 use crate::arm7tdmi::arm::*;
 use crate::arm7tdmi::bus::{Bus, MemoryAccessType::*, MemoryAccessWidth::*};
 use crate::arm7tdmi::cpu::{Core, CpuExecResult, CpuPipelineAction};
-use crate::arm7tdmi::{Addr, REG_PC, CpuState};
+use crate::arm7tdmi::{Addr, REG_PC, REG_LR, REG_SP, CpuState, reg_string};
 
 use super::*;
+fn push(cpu: &mut Core, bus: &mut Bus, r: usize) {
+            cpu.gpr[REG_SP] -= 4;
+            let stack_addr = cpu.gpr[REG_SP];
+            println!("push reg {} to addr {:#x}", reg_string(r), stack_addr);
+            bus.write_32(stack_addr, cpu.get_reg(r)).expect("bus error");
+        }
+fn pop(cpu: &mut Core, bus: &mut Bus, r: usize) {
+            let stack_addr = cpu.gpr[REG_SP];
+            let val = bus.read_32(stack_addr);
+            cpu.set_reg(r, val);
+            cpu.gpr[REG_SP] = stack_addr + 4;
+        }
 
 impl Core {
     fn exec_thumb_add_sub(&mut self, bus: &mut Bus, insn: ThumbInstruction) -> CpuExecResult {
@@ -166,6 +178,57 @@ impl Core {
         Ok(CpuPipelineAction::IncPC)
     }
 
+    fn exec_thumb_push_pop(
+        &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_pop = insn.is_load();
+        let mut pipeline_action = CpuPipelineAction::IncPC;
+
+        self.add_cycles(insn.pc, bus, NonSeq + MemoryAccess16);
+
+        let pc_lr_flag = insn.flag(ThumbInstruction::FLAG_R);
+        let rlist = insn.register_list();
+        if is_pop {
+            for r in rlist {
+                pop(self, bus, r);
+            }
+            if pc_lr_flag {
+                pop(self, bus, REG_PC);
+                pipeline_action = CpuPipelineAction::Flush;
+                self.add_cycles(
+                    self.pc,
+                    bus,
+                    Seq + MemoryAccess32
+                );
+                self.add_cycles(
+                    self.pc + (self.word_size() as u32),
+                    bus,
+                    NonSeq + MemoryAccess16
+                    );
+            }
+            self.add_cycle();
+        } else {
+            self.add_cycles(
+                self.gpr[REG_SP],
+                bus,
+                NonSeq + MemoryAccess32
+                );
+            if pc_lr_flag {
+                push(self, bus, REG_LR);
+            }
+            for r in rlist.into_iter().rev() {
+                push(self, bus ,r);
+            }
+            self.add_cycles(self.gpr[REG_SP], bus, NonSeq + MemoryAccess32);
+        }
+
+        Ok(CpuPipelineAction::IncPC)
+    }
+
     fn exec_thumb_branch_with_cond(
         &mut self,
         bus: &mut Bus,
@@ -203,8 +266,8 @@ 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::PushPop => self.exec_thumb_push_pop(bus, insn),
             ThumbFormat::BranchConditional => self.exec_thumb_branch_with_cond(bus, insn),
-
             _ => unimplemented!("thumb not implemented {:#?}", insn),
         }
     }

src/arm7tdmi/thumb/mod.rs

@@ -192,6 +192,7 @@ impl From<OpFormat5> for ArmOpCode {
 impl ThumbInstruction {
     const FLAG_H1: usize = 7;
     const FLAG_H2: usize = 6;
+    const FLAG_R: usize = 8;
 
     pub fn rd(&self) -> usize {
         match self.fmt {
@@ -266,6 +267,17 @@ impl ThumbInstruction {
     pub fn flag(&self, bit: usize) -> bool {
         self.raw.bit(bit)
     }
+
+    pub fn register_list(&self) -> Vec<usize> {
+        let list_bits = self.raw & 0xff;
+        let mut list = Vec::with_capacity(8);
+        for i in 0..=7 {
+            if (list_bits & (1 << i)) != 0 {
+                list.push(i)
+            }
+        }
+        list
+    }
 }
 
 #[cfg(test)]