From 5ed5c5d52d7a5e4bfc033c57461385a7dbbc0349 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 like 1 THUMB instruction

---
 src/cpu.zig               | 17 +++++++++++++----
 src/cpu/thumb/format3.zig | 31 +++++++++++++++++++++++++++++++
 2 files changed, 44 insertions(+), 4 deletions(-)
 create mode 100644 src/cpu/thumb/format3.zig

diff --git a/src/cpu.zig b/src/cpu.zig
index a5dfacd..1a25476 100644
--- a/src/cpu.zig
+++ b/src/cpu.zig
@@ -7,6 +7,7 @@ const Bit = @import("bitfield").Bit;
 const Bitfield = @import("bitfield").Bitfield;
 const Scheduler = @import("scheduler.zig").Scheduler;
 
+// ARM Instruction Groups
 const dataProcessing = @import("cpu/arm/data_processing.zig").dataProcessing;
 const psrTransfer = @import("cpu/arm/psr_transfer.zig").psrTransfer;
 const singleDataTransfer = @import("cpu/arm/single_data_transfer.zig").singleDataTransfer;
@@ -15,6 +16,9 @@ const blockDataTransfer = @import("cpu/arm/block_data_transfer.zig").blockDataTr
 const branch = @import("cpu/arm/branch.zig").branch;
 const branchAndExchange = @import("cpu/arm/branch.zig").branchAndExchange;
 
+// THUMB Instruction Groups
+const format3 = @import("cpu/thumb/format3.zig").format3;
+
 pub const ArmInstrFn = fn (*Arm7tdmi, *Bus, u32) void;
 pub const ThumbInstrFn = fn (*Arm7tdmi, *Bus, u16) void;
 const arm_lut: [0x1000]ArmInstrFn = armPopulate();
@@ -141,12 +145,17 @@ fn checkCond(cpsr: PSR, cond: u4) bool {
 
 fn thumbPopulate() [0x400]ThumbInstrFn {
     return comptime {
-        @setEvalBranchQuota(0x800);
+        @setEvalBranchQuota(0xC00);
         var lut = [_]ThumbInstrFn{thumbUndefined} ** 0x400;
 
         var i: usize = 0;
         while (i < lut.len) : (i += 1) {
-            lut[i] = thumbUndefined;
+            if (i >> 7 & 0x7 == 0b001) {
+                const op = i >> 5 & 0x3;
+                const rd = i >> 2 & 0x7;
+
+                lut[i] = format3(op, rd);
+            }
         }
 
         return lut;
@@ -245,10 +254,10 @@ const Mode = enum(u5) {
 
 fn armUndefined(_: *Arm7tdmi, _: *Bus, opcode: u32) void {
     const id = armIdx(opcode);
-    std.debug.panic("[CPU:ARM] {{0x{X:}}} 0x{X:} is an illegal opcode", .{ id, opcode });
+    std.debug.panic("[CPU:ARM] ID: 0x{X:0>3} 0x{X:0>8} is an illegal opcode", .{ id, opcode });
 }
 
 fn thumbUndefined(_: *Arm7tdmi, _: *Bus, opcode: u16) void {
     const id = thumbIdx(opcode);
-    std.debug.panic("[CPU:THUMB] {{0x{X:}}} 0x{X:} is an illegal opcode", .{ id, opcode });
+    std.debug.panic("[CPU:THUMB] ID: 0b{b:0>10} 0x{X:0>2} is an illegal opcode", .{ id, opcode });
 }
diff --git a/src/cpu/thumb/format3.zig b/src/cpu/thumb/format3.zig
new file mode 100644
index 0000000..d759c7b
--- /dev/null
+++ b/src/cpu/thumb/format3.zig
@@ -0,0 +1,31 @@
+const std = @import("std");
+
+const Bus = @import("../../Bus.zig");
+const Arm7tdmi = @import("../../cpu.zig").Arm7tdmi;
+const InstrFn = @import("../../cpu.zig").ThumbInstrFn;
+
+pub fn format3(comptime op: u2, comptime rd: u3) InstrFn {
+    return struct {
+        fn inner(cpu: *Arm7tdmi, _: *Bus, opcode: u16) void {
+            const offset = @truncate(u8, opcode);
+
+            switch (op) {
+                0b00 => {
+                    cpu.r[rd] = offset;
+
+                    cpu.cpsr.n.unset();
+                    cpu.cpsr.z.write(offset == 0);
+                },
+                0b01 => {
+                    std.debug.panic("TODO: Implement cmp R{}, #0x{X:0>2}", .{ rd, offset });
+                },
+                0b10 => {
+                    std.debug.panic("TODO: Implement add R{}, #0x{X:0>2}", .{ rd, offset });
+                },
+                0b11 => {
+                    std.debug.panic("TODO: Implement sub R{}, #0x{X:0>2}", .{ rd, offset });
+                },
+            }
+        }
+    }.inner;
+}