From e6a0eab667c9b2f2e751aa66d519ad71e8e119d7 Mon Sep 17 00:00:00 2001
From: Rekai Musuka <rekai@musuka.dev>
Date: Fri, 21 Oct 2022 05:12:12 -0300
Subject: [PATCH] feat(cpu): implement THUMB format 17

---
 src/cpu.zig                |  4 ++--
 src/cpu/thumb/format17.zig | 24 ++++++++++++++++++++++++
 2 files changed, 26 insertions(+), 2 deletions(-)
 create mode 100644 src/cpu/thumb/format17.zig

diff --git a/src/cpu.zig b/src/cpu.zig
index 485452e..dc23bb9 100644
--- a/src/cpu.zig
+++ b/src/cpu.zig
@@ -37,6 +37,7 @@ const format13 = @import("cpu/thumb/format13.zig").format13;
 const format14 = @import("cpu/thumb/format14.zig").format14;
 const format15 = @import("cpu/thumb/format15.zig").format15;
 const format16 = @import("cpu/thumb/format16.zig").format16;
+const format17 = @import("cpu/thumb/format17.zig").format17;
 const format18 = @import("cpu/thumb/format18.zig").format18;
 const format19 = @import("cpu/thumb/format19.zig").format19;
 
@@ -479,8 +480,7 @@ fn thumbPopulate() [0x400]ThumbInstrFn {
 
                 lut[i] = format13(S);
             } else if (i >> 2 & 0xFF == 0xDF) {
-                // Format 17 | Software Interrupt
-                lut[i] = thumbUndefined;
+                lut[i] = format17();
             } else if (i >> 6 & 0xF == 0b1000) {
                 const L = i >> 5 & 1 == 1;
                 const offset = i & 0x1F;
diff --git a/src/cpu/thumb/format17.zig b/src/cpu/thumb/format17.zig
new file mode 100644
index 0000000..6400ca2
--- /dev/null
+++ b/src/cpu/thumb/format17.zig
@@ -0,0 +1,24 @@
+const std = @import("std");
+
+const Bus = @import("../../Bus.zig");
+const Arm7tdmi = @import("../../cpu.zig").Arm7tdmi;
+const InstrFn = @import("../../cpu.zig").ThumbInstrFn;
+
+pub fn format17() InstrFn {
+    return struct {
+        fn inner(cpu: *Arm7tdmi, _: *Bus, _: u16) void {
+            // Copy Values from Current Mode
+            const r15 = cpu.r[15];
+            const cpsr = cpu.cpsr.raw;
+
+            // Switch Mode
+            cpu.changeMode(.Supervisor);
+            cpu.cpsr.t.write(false); // Force ARM Mode
+            cpu.cpsr.i.write(true); // Disable normal interrupts
+
+            cpu.r[14] = r15; // Resume Execution
+            cpu.spsr.raw = cpsr; // Previous mode CPSR
+            cpu.r[15] = 0x0000_0008;
+        }
+    }.inner;
+}