From 2a416fb2c6ff645cb556e018793eaeee457c651b Mon Sep 17 00:00:00 2001
From: Rekai Musuka <rekai@musuka.dev>
Date: Mon, 17 Jan 2022 10:07:50 -0400
Subject: [PATCH] feat(cpu): implement format 12 thumb instructions

---
 src/cpu.zig                |  8 ++++++++
 src/cpu/thumb/format12.zig | 16 ++++++++++++++++
 2 files changed, 24 insertions(+)
 create mode 100644 src/cpu/thumb/format12.zig

diff --git a/src/cpu.zig b/src/cpu.zig
index 5c9e1db..168a240 100644
--- a/src/cpu.zig
+++ b/src/cpu.zig
@@ -19,6 +19,7 @@ const branchAndExchange = @import("cpu/arm/branch.zig").branchAndExchange;
 // THUMB Instruction Groups
 const format3 = @import("cpu/thumb/format3.zig").format3;
 const format5 = @import("cpu/thumb/format5.zig").format5;
+const format12 = @import("cpu/thumb/format12.zig").format12;
 
 pub const ArmInstrFn = fn (*Arm7tdmi, *Bus, u32) void;
 pub const ThumbInstrFn = fn (*Arm7tdmi, *Bus, u16) void;
@@ -169,6 +170,13 @@ fn thumbPopulate() [0x400]ThumbInstrFn {
 
                 lut[i] = format5(op, h1, h2);
             }
+
+            if (i >> 6 & 0xF == 0b1010) {
+                const isSP = i >> 5 & 1 == 1;
+                const rd = i >> 2 & 0x7;
+
+                lut[i] = format12(isSP, rd);
+            }
         }
 
         return lut;
diff --git a/src/cpu/thumb/format12.zig b/src/cpu/thumb/format12.zig
new file mode 100644
index 0000000..2e18810
--- /dev/null
+++ b/src/cpu/thumb/format12.zig
@@ -0,0 +1,16 @@
+const std = @import("std");
+
+const Bus = @import("../../Bus.zig");
+const Arm7tdmi = @import("../../cpu.zig").Arm7tdmi;
+const InstrFn = @import("../../cpu.zig").ThumbInstrFn;
+
+pub fn format12(comptime isSP: bool, comptime rd: u3) InstrFn {
+    return struct {
+        fn inner(cpu: *Arm7tdmi, _: *Bus, opcode: u16) void {
+            const left = if (isSP) cpu.r[13] else cpu.r[15] + 2 & 0xFFFF_FFFD; // fetch (+2)
+            const right = @truncate(u10, opcode & 0xFF) << 2;
+            const result = left + right; // TODO: What about overflows?
+            cpu.r[rd] = result;
+        }
+    }.inner;
+}