From 97b236225e85ae1ed8c8e8a9d058230d8899a18e Mon Sep 17 00:00:00 2001
From: Rekai Musuka <rekai@musuka.dev>
Date: Fri, 21 Oct 2022 05:12:05 -0300
Subject: [PATCH] chore: implement THUMB format 4 instructions

---
 src/cpu.zig               |   7 +++
 src/cpu/thumb/format4.zig | 112 ++++++++++++++++++++++++++++++++++++++
 2 files changed, 119 insertions(+)
 create mode 100644 src/cpu/thumb/format4.zig

diff --git a/src/cpu.zig b/src/cpu.zig
index 260008b..ad1ff43 100644
--- a/src/cpu.zig
+++ b/src/cpu.zig
@@ -21,6 +21,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 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;
@@ -347,6 +348,12 @@ fn thumbPopulate() [0x400]ThumbInstrFn {
                 lut[i] = format3(op, rd);
             }
 
+            if (i >> 4 & 0x3F == 0b010000) {
+                const op = i & 0xF;
+
+                lut[i] = format4(op);
+            }
+
             if (i >> 4 & 0x3F == 0b010001) {
                 const op = i >> 2 & 0x3;
                 const h1 = i >> 1 & 1;
diff --git a/src/cpu/thumb/format4.zig b/src/cpu/thumb/format4.zig
new file mode 100644
index 0000000..23433b7
--- /dev/null
+++ b/src/cpu/thumb/format4.zig
@@ -0,0 +1,112 @@
+const std = @import("std");
+
+const Bus = @import("../../Bus.zig");
+const Arm7tdmi = @import("../../cpu.zig").Arm7tdmi;
+const InstrFn = @import("../../cpu.zig").ThumbInstrFn;
+const shifter = @import("../barrel_shifter.zig");
+
+const adc = @import("../arm/data_processing.zig").adc;
+const sbc = @import("../arm/data_processing.zig").sbc;
+const sub = @import("../arm/data_processing.zig").sub;
+const cmp = @import("../arm/data_processing.zig").cmp;
+const cmn = @import("../arm/data_processing.zig").cmn;
+const setTestOpFlags = @import("../arm/data_processing.zig").setTestOpFlags;
+const setLogicOpFlags = @import("../arm/data_processing.zig").setLogicOpFlags;
+
+pub fn format4(comptime op: u4) InstrFn {
+    return struct {
+        fn inner(cpu: *Arm7tdmi, _: *Bus, opcode: u16) void {
+            const rs = opcode >> 3 & 0x7;
+            const rd = opcode & 0x7;
+            const carry = @boolToInt(cpu.cpsr.c.read());
+
+            switch (op) {
+                0x0 => {
+                    // AND
+                    const result = cpu.r[rd] & cpu.r[rs];
+                    cpu.r[rd] = result;
+                    setLogicOpFlags(true, cpu, result);
+                },
+                0x1 => {
+                    // EOR
+                    const result = cpu.r[rd] ^ cpu.r[rs];
+                    cpu.r[rd] = result;
+                    setLogicOpFlags(true, cpu, result);
+                },
+                0x2 => {
+                    // LSL
+                    const result = shifter.logicalLeft(true, &cpu.cpsr, cpu.r[rd], @truncate(u8, cpu.r[rs]));
+                    cpu.r[rd] = result;
+                    setLogicOpFlags(true, cpu, result);
+                },
+                0x3 => {
+                    // LSR
+                    const result = shifter.logicalRight(true, &cpu.cpsr, cpu.r[rd], @truncate(u8, cpu.r[rs]));
+                    cpu.r[rd] = result;
+                    setLogicOpFlags(true, cpu, result);
+                },
+                0x4 => {
+                    // ASR
+                    const result = shifter.arithmeticRight(true, &cpu.cpsr, cpu.r[rd], @truncate(u8, cpu.r[rs]));
+                    cpu.r[rd] = result;
+                    setLogicOpFlags(true, cpu, result);
+                },
+                0x5 => {
+                    // ADC
+                    cpu.r[rd] = adc(true, cpu, cpu.r[rd], cpu.r[rs], carry);
+                },
+                0x6 => {
+                    // SBC
+                    cpu.r[rd] = sbc(true, cpu, cpu.r[rd], cpu.r[rs], carry);
+                },
+                0x7 => {
+                    // ROR
+                    const result = shifter.rotateRight(true, &cpu.cpsr, cpu.r[rd], @truncate(u8, cpu.r[rs]));
+                    cpu.r[rd] = result;
+                    setLogicOpFlags(true, cpu, result);
+                },
+                0x8 => {
+                    // TST
+                    const result = cpu.r[rd] & cpu.r[rs];
+                    setLogicOpFlags(true, cpu, result); // FIXME: Barrel Shifter?
+                },
+                0x9 => {
+                    // NEG
+                    cpu.r[rd] = sub(true, cpu, cpu.r[rs], cpu.r[rd]); // FIXME: I think this is wrong?
+                },
+                0xA => {
+                    // CMP
+                    cmp(cpu, cpu.r[rd], cpu.r[rs]);
+                },
+                0xB => {
+                    // CMN
+                    cmn(cpu, cpu.r[rd], cpu.r[rs]);
+                },
+                0xC => {
+                    // ORR
+                    const result = cpu.r[rd] | cpu.r[rs];
+                    cpu.r[rd] = result;
+                    setLogicOpFlags(true, cpu, result);
+                },
+                0xD => {
+                    // MUL
+                    const result = cpu.r[rs] * cpu.r[rd];
+                    cpu.r[rd] = result;
+                    std.debug.panic("[CPU|THUMB|MUL] TODO: Set flags on ALU MUL", .{});
+                },
+                0xE => {
+                    // BIC
+                    const result = cpu.r[rd] & ~cpu.r[rs];
+                    cpu.r[rd] = result;
+                    setLogicOpFlags(true, cpu, result);
+                },
+                0xF => {
+                    // MVN
+                    const result = ~cpu.r[rs];
+                    cpu.r[rd] = result;
+                    setLogicOpFlags(true, cpu, result);
+                },
+            }
+        }
+    }.inner;
+}