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

---
 src/cpu.zig                     |  8 +++++++
 src/cpu/arm/data_processing.zig | 38 +++++++++++++++++----------------
 src/cpu/thumb/format2.zig       | 33 ++++++++++++++++++++++++++++
 3 files changed, 61 insertions(+), 18 deletions(-)
 create mode 100644 src/cpu/thumb/format2.zig

diff --git a/src/cpu.zig b/src/cpu.zig
index 210c468..8876bbe 100644
--- a/src/cpu.zig
+++ b/src/cpu.zig
@@ -22,6 +22,7 @@ const softwareInterrupt = @import("cpu/arm/software_interrupt.zig").softwareInte
 // THUMB Instruction Groups
 const format1 = @import("cpu/thumb/format1.zig").format1;
 const format3 = @import("cpu/thumb/format3.zig").format3;
+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 format12 = @import("cpu/thumb/format12.zig").format12;
@@ -332,6 +333,13 @@ fn thumbPopulate() [0x400]ThumbInstrFn {
 
                 lut[i] = format1(op, offset);
             }
+            if (i >> 5 & 0x1F == 0b00011) {
+                const I = i >> 4 & 1 == 1;
+                const is_sub = i >> 3 & 1 == 1;
+                const rn = i & 0x7;
+
+                lut[i] = format2(I, is_sub, rn);
+            }
 
             if (i >> 7 & 0x7 == 0b001) {
                 const op = i >> 5 & 0x3;
diff --git a/src/cpu/arm/data_processing.zig b/src/cpu/arm/data_processing.zig
index fca53cb..2ff8497 100644
--- a/src/cpu/arm/data_processing.zig
+++ b/src/cpu/arm/data_processing.zig
@@ -43,23 +43,7 @@ pub fn dataProcessing(comptime I: bool, comptime S: bool, comptime instrKind: u4
                 },
                 0x2 => cpu.r[rd] = sub(S, cpu, rd, op1, op2), // SUB
                 0x3 => cpu.r[rd] = sub(S, cpu, rd, op2, op1), // RSB
-                0x4 => {
-                    // ADD
-                    var result: u32 = undefined;
-                    const didOverflow = @addWithOverflow(u32, op1, op2, &result);
-                    cpu.r[rd] = result;
-
-                    if (S) {
-                        if (rd == 0xF) {
-                            cpu.setCpsr(cpu.spsr.raw);
-                        } else {
-                            cpu.cpsr.n.write(result >> 31 & 1 == 1);
-                            cpu.cpsr.z.write(result == 0);
-                            cpu.cpsr.c.write(didOverflow);
-                            cpu.cpsr.v.write(((op1 ^ result) & (op2 ^ result)) >> 31 & 1 == 1);
-                        }
-                    }
-                },
+                0x4 => cpu.r[rd] = add(S, cpu, rd, op1, op2), // ADD
                 0x5 => {
                     // ADC
                     var result: u32 = undefined;
@@ -181,7 +165,7 @@ fn sbc(comptime S: bool, cpu: *Arm7tdmi, rd: u4, left: u32, right: u32, old_carr
     return result;
 }
 
-fn sub(comptime S: bool, cpu: *Arm7tdmi, rd: u4, left: u32, right: u32) u32 {
+pub fn sub(comptime S: bool, cpu: *Arm7tdmi, rd: u4, left: u32, right: u32) u32 {
     const result = left -% right;
 
     if (S) {
@@ -198,6 +182,24 @@ fn sub(comptime S: bool, cpu: *Arm7tdmi, rd: u4, left: u32, right: u32) u32 {
     return result;
 }
 
+pub fn add(comptime S: bool, cpu: *Arm7tdmi, rd: u4, left: u32, right: u32) u32 {
+    var result: u32 = undefined;
+    const didOverflow = @addWithOverflow(u32, left, right, &result);
+
+    if (S) {
+        if (rd == 0xF) {
+            cpu.setCpsr(cpu.spsr.raw);
+        } else {
+            cpu.cpsr.n.write(result >> 31 & 1 == 1);
+            cpu.cpsr.z.write(result == 0);
+            cpu.cpsr.c.write(didOverflow);
+            cpu.cpsr.v.write(((left ^ result) & (right ^ result)) >> 31 & 1 == 1);
+        }
+    }
+
+    return result;
+}
+
 fn logicFlags(comptime S: bool, cpu: *Arm7tdmi, rd: u4, result: u32) void {
     if (S) {
         if (rd == 0xF) {
diff --git a/src/cpu/thumb/format2.zig b/src/cpu/thumb/format2.zig
new file mode 100644
index 0000000..dfb8e9f
--- /dev/null
+++ b/src/cpu/thumb/format2.zig
@@ -0,0 +1,33 @@
+const std = @import("std");
+
+const Bus = @import("../../Bus.zig");
+const Arm7tdmi = @import("../../cpu.zig").Arm7tdmi;
+const InstrFn = @import("../../cpu.zig").ThumbInstrFn;
+
+const add = @import("../arm/data_processing.zig").add;
+const sub = @import("../arm/data_processing.zig").sub;
+
+pub fn format2(comptime I: bool, is_sub: bool, rn: u3) InstrFn {
+    return struct {
+        fn inner(cpu: *Arm7tdmi, _: *Bus, opcode: u16) void {
+            const rs = opcode >> 3 & 0x7;
+            const rd = @truncate(u3, opcode);
+
+            if (is_sub) {
+                // SUB
+                cpu.r[rd] = if (I) blk: {
+                    break :blk sub(true, cpu, rd, cpu.r[rs], @as(u32, rn));
+                } else blk: {
+                    break :blk sub(true, cpu, rd, cpu.r[rs], cpu.r[rn]);
+                };
+            } else {
+                // ADD
+                cpu.r[rd] = if (I) blk: {
+                    break :blk add(true, cpu, rd, cpu.r[rs], @as(u32, rn));
+                } else blk: {
+                    break :blk add(true, cpu, rd, cpu.r[rs], cpu.r[rn]);
+                };
+            }
+        }
+    }.inner;
+}