Implement thumb format 13

(NOT TESTED)


Former-commit-id: 160ee3a6c09a12cab53f69c94b385ea17666bd5f

src/arm7tdmi/thumb/display.rs

@@ -91,6 +91,14 @@ impl ThumbInstruction {
         )
     }
 
+    fn fmt_thumb_add_sp(&self, f: &mut fmt::Formatter) -> fmt::Result {
+        write!(
+            f,
+            "add\tsp, #{imm:x}",
+            imm = self.sword7()
+        )
+    }
+
     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();
@@ -136,6 +144,7 @@ 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::AddSp => self.fmt_thumb_add_sp(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

@@ -7,7 +7,6 @@ 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) {
@@ -178,6 +177,24 @@ impl Core {
         Ok(CpuPipelineAction::IncPC)
     }
 
+    fn exec_thumb_add_sp(&mut self, bus: &mut Bus, insn: ThumbInstruction) -> CpuExecResult {
+        let op1 = self.gpr[REG_SP] as i32;
+        let op2 = insn.sword7();
+        let arm_alu_op = ArmOpCode::ADD;
+
+        let result = self.alu(arm_alu_op, op1, op2, false);
+        if let Some(result) = result {
+            self.gpr[REG_SP] = result as u32;
+        }
+        // +1S
+        self.add_cycles(
+            insn.pc + (self.word_size() as u32),
+            bus,
+            Seq + MemoryAccess32,
+        );
+        Ok(CpuPipelineAction::IncPC)
+    }
+
     fn exec_thumb_push_pop(
         &mut self,
         bus: &mut Bus,
@@ -266,6 +283,7 @@ 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::AddSp => self.exec_thumb_add_sp(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

@@ -193,6 +193,7 @@ impl ThumbInstruction {
     const FLAG_H1: usize = 7;
     const FLAG_H2: usize = 6;
     const FLAG_R: usize = 8;
+    const FLAG_S: usize = 7;
 
     pub fn rd(&self) -> usize {
         match self.fmt {
@@ -278,6 +279,15 @@ impl ThumbInstruction {
         }
         list
     }
+
+    pub fn sword7(&self) -> i32 {
+        let imm7 = self.raw & 0x7f;
+        if self.flag(ThumbInstruction::FLAG_S) {
+            -((imm7 << 2) as i32)
+        } else {
+            (imm7 << 2) as i32
+        }
+    }
 }
 
 #[cfg(test)]