From 7501adfd127af584dd2d50d573ce5ebc1b37fb3f Mon Sep 17 00:00:00 2001
From: Michel Heily <michelheily@gmail.com>
Date: Mon, 22 Jul 2019 01:15:58 +0300
Subject: [PATCH] Implement thumb17 (Swi)

Former-commit-id: 62d7e14e9b84e74d9236e1f0a5e961ae805f861c
---
 src/core/arm7tdmi/arm/exec.rs      | 9 +--------
 src/core/arm7tdmi/cpu.rs           | 6 ++++++
 src/core/arm7tdmi/thumb/display.rs | 9 +++++++++
 src/core/arm7tdmi/thumb/exec.rs    | 1 +
 4 files changed, 17 insertions(+), 8 deletions(-)

diff --git a/src/core/arm7tdmi/arm/exec.rs b/src/core/arm7tdmi/arm/exec.rs
index a5267a1..2aae728 100644
--- a/src/core/arm7tdmi/arm/exec.rs
+++ b/src/core/arm7tdmi/arm/exec.rs
@@ -3,7 +3,6 @@ use crate::bit::BitIndex;
 use crate::core::arm7tdmi::alu::*;
 use crate::core::arm7tdmi::bus::Bus;
 use crate::core::arm7tdmi::cpu::{Core, CpuExecResult};
-use crate::core::arm7tdmi::exception::Exception;
 use crate::core::arm7tdmi::psr::RegPSR;
 use crate::core::arm7tdmi::{
     Addr, CpuError, CpuMode, CpuResult, CpuState, DecodedInstruction, REG_PC,
@@ -20,7 +19,7 @@ impl Core {
             ArmFormat::BX => self.exec_bx(bus, insn),
             ArmFormat::B_BL => self.exec_b_bl(bus, insn),
             ArmFormat::DP => self.exec_data_processing(bus, insn),
-            ArmFormat::SWI => self.exec_swi(bus, insn),
+            ArmFormat::SWI => self.exec_swi(),
             ArmFormat::LDR_STR => self.exec_ldr_str(bus, insn),
             ArmFormat::LDR_STR_HS_IMM => self.exec_ldr_str_hs(bus, insn),
             ArmFormat::LDR_STR_HS_REG => self.exec_ldr_str_hs(bus, insn),
@@ -70,12 +69,6 @@ impl Core {
         self.branch_exchange(self.get_reg(insn.rn()))
     }
 
-    fn exec_swi(&mut self, _bus: &mut Bus, _insn: ArmInstruction) -> CpuExecResult {
-        self.exception(Exception::SoftwareInterrupt);
-        self.flush_pipeline();
-        Ok(())
-    }
-
     fn exec_mrs(&mut self, _bus: &mut Bus, insn: ArmInstruction) -> CpuExecResult {
         let mode = self.cpsr.mode();
         let result = if insn.spsr_flag() {
diff --git a/src/core/arm7tdmi/cpu.rs b/src/core/arm7tdmi/cpu.rs
index e4856cf..d4bc33f 100644
--- a/src/core/arm7tdmi/cpu.rs
+++ b/src/core/arm7tdmi/cpu.rs
@@ -240,6 +240,12 @@ impl Core {
         }
     }
 
+    pub fn exec_swi(&mut self) -> CpuExecResult {
+        self.exception(Exception::SoftwareInterrupt);
+        self.flush_pipeline();
+        Ok(())
+    }
+
     fn step_arm_exec(&mut self, insn: u32, sb: &mut Bus) -> CpuResult<()> {
         let pc = self.pc;
         match self.pipeline_state {
diff --git a/src/core/arm7tdmi/thumb/display.rs b/src/core/arm7tdmi/thumb/display.rs
index 79c6134..2673643 100644
--- a/src/core/arm7tdmi/thumb/display.rs
+++ b/src/core/arm7tdmi/thumb/display.rs
@@ -261,6 +261,14 @@ impl ThumbInstruction {
         )
     }
 
+    fn fmt_thumb_swi(&self, f: &mut fmt::Formatter) -> fmt::Result {
+        write!(
+            f,
+            "swi\t{value:#x}",
+            value = self.raw & 0xff,
+        )
+    }
+
     fn fmt_thumb_branch(&self, f: &mut fmt::Formatter) -> fmt::Result {
         write!(
             f,
@@ -304,6 +312,7 @@ impl fmt::Display for ThumbInstruction {
             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::Swi => self.fmt_thumb_swi(f),
             ThumbFormat::Branch => self.fmt_thumb_branch(f),
             ThumbFormat::BranchLongWithLink => self.fmt_thumb_branch_long_with_link(f),
             _ => write!(f, "({:?})", self),
diff --git a/src/core/arm7tdmi/thumb/exec.rs b/src/core/arm7tdmi/thumb/exec.rs
index 6937ce3..a79e64b 100644
--- a/src/core/arm7tdmi/thumb/exec.rs
+++ b/src/core/arm7tdmi/thumb/exec.rs
@@ -436,6 +436,7 @@ impl Core {
             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::Swi => self.exec_swi(),
             ThumbFormat::Branch => self.exec_thumb_branch(bus, insn),
             ThumbFormat::BranchLongWithLink => self.exec_thumb_branch_long_with_link(bus, insn),
             _ => unimplemented!("thumb not implemented {:#x?}", insn),