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

---
 src/cpu.zig               |  9 +++++++++
 src/cpu/thumb/format5.zig | 35 +++++++++++++++++++++++++++++++++++
 2 files changed, 44 insertions(+)
 create mode 100644 src/cpu/thumb/format5.zig

diff --git a/src/cpu.zig b/src/cpu.zig
index f21b994..5c9e1db 100644
--- a/src/cpu.zig
+++ b/src/cpu.zig
@@ -18,6 +18,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;
 
 pub const ArmInstrFn = fn (*Arm7tdmi, *Bus, u32) void;
 pub const ThumbInstrFn = fn (*Arm7tdmi, *Bus, u16) void;
@@ -160,6 +161,14 @@ fn thumbPopulate() [0x400]ThumbInstrFn {
 
                 lut[i] = format3(op, rd);
             }
+
+            if (i >> 4 & 0x3F == 0b010001) {
+                const op = i >> 2 & 0x3;
+                const h1 = i >> 1 & 1;
+                const h2 = i & 1;
+
+                lut[i] = format5(op, h1, h2);
+            }
         }
 
         return lut;
diff --git a/src/cpu/thumb/format5.zig b/src/cpu/thumb/format5.zig
new file mode 100644
index 0000000..51c4e7f
--- /dev/null
+++ b/src/cpu/thumb/format5.zig
@@ -0,0 +1,35 @@
+const std = @import("std");
+
+const Bus = @import("../../Bus.zig");
+const Arm7tdmi = @import("../../cpu.zig").Arm7tdmi;
+const InstrFn = @import("../../cpu.zig").ThumbInstrFn;
+
+pub fn format5(comptime op: u2, comptime h1: u1, comptime h2: u1) InstrFn {
+    return struct {
+        fn inner(cpu: *Arm7tdmi, _: *Bus, opcode: u16) void {
+            const src = @as(u4, h2) << 3 | (opcode >> 3 & 0x7);
+            const dst = @as(u4, h1) << 3 | (opcode & 0x7);
+
+            switch (op) {
+                0b01 => {
+                    // CMP
+                    const left = cpu.r[dst];
+                    const right = cpu.r[src];
+                    const result = left -% right;
+
+                    cpu.cpsr.n.write(result >> 31 & 1 == 1);
+                    cpu.cpsr.z.write(result == 0);
+                    cpu.cpsr.c.write(right <= left);
+                    cpu.cpsr.v.write(((left ^ result) & (~right ^ result)) >> 31 & 1 == 1);
+                },
+                0b10 => cpu.r[dst] = cpu.r[src], // MOV
+                0b11 => {
+                    // BX
+                    cpu.cpsr.t.write(cpu.r[src] & 1 == 1);
+                    cpu.r[15] = cpu.r[src] & 0xFFFF_FFFE;
+                },
+                else => std.debug.panic("[CPU] Op #{} is invalid for THUMB Format 5", .{op}),
+            }
+        }
+    }.inner;
+}