From 0598ba402dc53a63ee002417d819ac091ccffc58 Mon Sep 17 00:00:00 2001
From: Rekai Musuka <rekai@musuka.dev>
Date: Sat, 29 Jan 2022 22:34:24 -0400
Subject: [PATCH] feat(cpu): implement THUMB format 9 loads / stores

---
 src/cpu.zig               |  9 +++++++++
 src/cpu/thumb/format9.zig | 36 ++++++++++++++++++++++++++++++++++++
 2 files changed, 45 insertions(+)
 create mode 100644 src/cpu/thumb/format9.zig

diff --git a/src/cpu.zig b/src/cpu.zig
index 21d495f..cd7bf6b 100644
--- a/src/cpu.zig
+++ b/src/cpu.zig
@@ -25,6 +25,7 @@ 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 format15 = @import("cpu/thumb/format15.zig").format15;
 const format16 = @import("cpu/thumb/format16.zig").format16;
@@ -369,6 +370,14 @@ fn thumbPopulate() [0x400]ThumbInstrFn {
                 lut[i] = format6(rd);
             }
 
+            if (i >> 7 & 0x7 == 0b011) {
+                const B = i >> 6 & 1 == 1;
+                const L = i >> 5 & 1 == 1;
+                const offset = i & 0x1F;
+
+                lut[i] = format9(B, L, offset);
+            }
+
             if (i >> 6 & 0xF == 0b1010) {
                 const isSP = i >> 5 & 1 == 1;
                 const rd = i >> 2 & 0x7;
diff --git a/src/cpu/thumb/format9.zig b/src/cpu/thumb/format9.zig
new file mode 100644
index 0000000..2d807a7
--- /dev/null
+++ b/src/cpu/thumb/format9.zig
@@ -0,0 +1,36 @@
+const std = @import("std");
+
+const Bus = @import("../../Bus.zig");
+const Arm7tdmi = @import("../../cpu.zig").Arm7tdmi;
+const InstrFn = @import("../../cpu.zig").ThumbInstrFn;
+
+pub fn format9(comptime B: bool, comptime L: bool, comptime offset: u5) InstrFn {
+    return struct {
+        fn inner(cpu: *Arm7tdmi, bus: *Bus, opcode: u16) void {
+            const rb = opcode >> 3 & 0x7;
+            const rd = opcode & 0x7;
+
+            if (L) {
+                if (B) {
+                    // LDRB
+                    const address = cpu.r[rb] + offset;
+                    cpu.r[rd] = bus.read8(address);
+                } else {
+                    // LDR
+                    const address = cpu.r[rb] + (@as(u32, offset) << 2);
+                    cpu.r[rd] = bus.read32(address & 0xFFFF_FFFC);
+                }
+            } else {
+                if (B) {
+                    // STRB
+                    const address = cpu.r[rb] + offset;
+                    bus.write8(address, @truncate(u8, cpu.r[rd]));
+                } else {
+                    // STR
+                    const address = cpu.r[rb] + (@as(u32, offset) << 2);
+                    bus.write32(address & 0xFFFF_FFFC, cpu.r[rd]);
+                }
+            }
+        }
+    }.inner;
+}