From e18f10126ec6ca73c3dc6d471a453004469d806d Mon Sep 17 00:00:00 2001
From: Rekai Musuka <rekai@musuka.dev>
Date: Sat, 29 Jan 2022 23:22:10 -0400
Subject: [PATCH] feat(cpu): implement thumb push / pop and stub format 13
 thumb instrs

---
 src/cpu.zig                | 17 ++++++++++++-
 src/cpu/thumb/format13.zig | 13 ++++++++++
 src/cpu/thumb/format14.zig | 50 ++++++++++++++++++++++++++++++++++++++
 3 files changed, 79 insertions(+), 1 deletion(-)
 create mode 100644 src/cpu/thumb/format13.zig
 create mode 100644 src/cpu/thumb/format14.zig

diff --git a/src/cpu.zig b/src/cpu.zig
index cd7bf6b..75cefdf 100644
--- a/src/cpu.zig
+++ b/src/cpu.zig
@@ -20,13 +20,15 @@ const softwareInterrupt = @import("cpu/arm/software_interrupt.zig").softwareInte
 
 // THUMB Instruction Groups
 const format1 = @import("cpu/thumb/format1.zig").format1;
+const format2 = @import("cpu/thumb/format2.zig").format2;
 const format3 = @import("cpu/thumb/format3.zig").format3;
 const format4 = @import("cpu/thumb/format4.zig").format4;
-const format2 = @import("cpu/thumb/format2.zig").format2;
 const format5 = @import("cpu/thumb/format5.zig").format5;
 const format6 = @import("cpu/thumb/format6.zig").format6;
 const format9 = @import("cpu/thumb/format9.zig").format9;
 const format12 = @import("cpu/thumb/format12.zig").format12;
+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 format19 = @import("cpu/thumb/format19.zig").format19;
@@ -385,6 +387,19 @@ fn thumbPopulate() [0x400]ThumbInstrFn {
                 lut[i] = format12(isSP, rd);
             }
 
+            if (i >> 2 & 0xFF == 0xB0) {
+                const S = i >> 1 & 1 == 1;
+
+                lut[i] = format13(S);
+            }
+
+            if (i >> 6 & 0xF == 0b1011 and i >> 3 & 0x3 == 0b10) {
+                const L = i >> 11 & 1 == 1;
+                const R = i >> 8 & 1 == 1;
+
+                lut[i] = format14(L, R);
+            }
+
             if (i >> 6 & 0xF == 0b1100) {
                 const L = i >> 5 & 1 == 1;
                 const rb = i >> 2 & 0x3;
diff --git a/src/cpu/thumb/format13.zig b/src/cpu/thumb/format13.zig
new file mode 100644
index 0000000..8d41155
--- /dev/null
+++ b/src/cpu/thumb/format13.zig
@@ -0,0 +1,13 @@
+const std = @import("std");
+
+const Bus = @import("../../Bus.zig");
+const Arm7tdmi = @import("../../cpu.zig").Arm7tdmi;
+const InstrFn = @import("../../cpu.zig").ThumbInstrFn;
+
+pub fn format13(comptime _: bool) InstrFn {
+    return struct {
+        fn inner(_: *Arm7tdmi, _: *Bus, _: u16) void {
+            std.debug.panic("[CPU|THUMB|Fmt13] Implement Format 13 THUMB Instructions", .{});
+        }
+    }.inner;
+}
diff --git a/src/cpu/thumb/format14.zig b/src/cpu/thumb/format14.zig
new file mode 100644
index 0000000..25f64eb
--- /dev/null
+++ b/src/cpu/thumb/format14.zig
@@ -0,0 +1,50 @@
+const std = @import("std");
+
+const Bus = @import("../../Bus.zig");
+const Arm7tdmi = @import("../../cpu.zig").Arm7tdmi;
+const InstrFn = @import("../../cpu.zig").ThumbInstrFn;
+
+pub fn format14(comptime L: bool, comptime R: bool) InstrFn {
+    return struct {
+        fn inner(cpu: *Arm7tdmi, bus: *Bus, opcode: u16) void {
+            var address: u32 = undefined;
+            if (L) {
+                // POP
+                address = cpu.r[13];
+
+                var i: usize = 0;
+                while (i < 8) : (i += 1) {
+                    if ((opcode >> @truncate(u3, i)) & 1 == 1) {
+                        cpu.r[i] = bus.read32(address);
+                        address += 4;
+                    }
+                }
+
+                if (R) {
+                    const value = bus.read32(address);
+                    cpu.r[15] = value & 0xFFFF_FFFE;
+                    address += 4;
+                }
+            } else {
+                address = cpu.r[13] - 4;
+
+                if (R) {
+                    bus.write32(address, cpu.r[14]);
+                    address -= 4;
+                }
+
+                var i: usize = 8;
+                while (i > 0) : (i -= 1) {
+                    const j = i - 1;
+
+                    if ((opcode >> @truncate(u3, j)) & 1 == 1) {
+                        bus.write32(address, cpu.r[j]);
+                        address -= 4;
+                    }
+                }
+            }
+
+            cpu.r[13] = address;
+        }
+    }.inner;
+}