From 997dc1314c9923a45059c54a9e23b649195b7d66 Mon Sep 17 00:00:00 2001
From: Rekai Musuka <rekai@musuka.dev>
Date: Tue, 25 Jan 2022 10:33:16 -0400
Subject: [PATCH] feat(cpu): implement SWI

---
 src/cpu.zig                        | 14 ++++++++++----
 src/cpu/arm/software_interrupt.zig | 24 ++++++++++++++++++++++++
 2 files changed, 34 insertions(+), 4 deletions(-)
 create mode 100644 src/cpu/arm/software_interrupt.zig

diff --git a/src/cpu.zig b/src/cpu.zig
index 6f0c526..1817af8 100644
--- a/src/cpu.zig
+++ b/src/cpu.zig
@@ -15,6 +15,7 @@ const halfAndSignedDataTransfer = @import("cpu/arm/half_signed_data_transfer.zig
 const blockDataTransfer = @import("cpu/arm/block_data_transfer.zig").blockDataTransfer;
 const branch = @import("cpu/arm/branch.zig").branch;
 const branchAndExchange = @import("cpu/arm/branch.zig").branchAndExchange;
+const softwareInterrupt = @import("cpu/arm/software_interrupt.zig").softwareInterrupt;
 
 // THUMB Instruction Groups
 const format3 = @import("cpu/thumb/format3.zig").format3;
@@ -95,12 +96,15 @@ pub const Arm7tdmi = struct {
     }
 
     pub fn setCpsr(self: *Self, value: u32) void {
-        if (value & 0x1F != self.cpsr.raw & 0x1F) self.changeMode(@truncate(u5, value & 0x1F));
+        if (value & 0x1F != self.cpsr.raw & 0x1F) self.changeModeFromIdx(@truncate(u5, value & 0x1F));
         self.cpsr.raw = value;
     }
 
-    fn changeMode(self: *Self, next_idx: u5) void {
-        const next = getMode(next_idx);
+    fn changeModeFromIdx(self: *Self, next: u5) void {
+        self.changeMode(getMode(next));
+    }
+
+    pub fn changeMode(self: *Self, next: Mode) void {
         const now = getMode(self.cpsr.mode.read());
 
         // Bank R8 -> r12
@@ -142,7 +146,7 @@ pub const Arm7tdmi = struct {
             },
         }
 
-        self.cpsr.mode.write(next_idx);
+        self.cpsr.mode.write(@enumToInt(next));
     }
 
     pub fn skipBios(self: *Self) void {
@@ -348,6 +352,8 @@ fn armPopulate() [0x1000]ArmInstrFn {
                 const L = i >> 8 & 1 == 1;
                 lut[i] = branch(L);
             }
+
+            if (i >> 8 & 0xF == 0b1111) lut[i] = softwareInterrupt();
         }
 
         return lut;
diff --git a/src/cpu/arm/software_interrupt.zig b/src/cpu/arm/software_interrupt.zig
new file mode 100644
index 0000000..d5c5812
--- /dev/null
+++ b/src/cpu/arm/software_interrupt.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").ArmInstrFn;
+
+pub fn softwareInterrupt() InstrFn {
+    return struct {
+        fn inner(cpu: *Arm7tdmi, _: *Bus, _: u32) 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;
+}