From d85e0c8d059fa1eb526d231fe67bd6f6382f4375 Mon Sep 17 00:00:00 2001
From: Rekai Musuka <rekai@musuka.dev>
Date: Sat, 29 Jan 2022 01:37:50 -0400
Subject: [PATCH] feat(cpu): implement format 1 THUMB instructions

---
 src/cpu.zig               |  8 ++++++++
 src/cpu/thumb/format1.zig | 28 ++++++++++++++++++++++++++++
 2 files changed, 36 insertions(+)
 create mode 100644 src/cpu/thumb/format1.zig

diff --git a/src/cpu.zig b/src/cpu.zig
index 284647f..ab24a65 100644
--- a/src/cpu.zig
+++ b/src/cpu.zig
@@ -20,6 +20,7 @@ const branchAndExchange = @import("cpu/arm/branch.zig").branchAndExchange;
 const softwareInterrupt = @import("cpu/arm/software_interrupt.zig").softwareInterrupt;
 
 // THUMB Instruction Groups
+const format1 = @import("cpu/thumb/format1.zig").format1;
 const format3 = @import("cpu/thumb/format3.zig").format3;
 const format5 = @import("cpu/thumb/format5.zig").format5;
 const format6 = @import("cpu/thumb/format6.zig").format6;
@@ -316,6 +317,13 @@ fn thumbPopulate() [0x400]ThumbInstrFn {
 
         var i: usize = 0;
         while (i < lut.len) : (i += 1) {
+            if (i >> 7 & 0x7 == 0b000) {
+                const op = i >> 5 & 0x3;
+                const offset = i & 0x1F;
+
+                lut[i] = format1(op, offset);
+            }
+
             if (i >> 7 & 0x7 == 0b001) {
                 const op = i >> 5 & 0x3;
                 const rd = i >> 2 & 0x7;
diff --git a/src/cpu/thumb/format1.zig b/src/cpu/thumb/format1.zig
new file mode 100644
index 0000000..dbcb78c
--- /dev/null
+++ b/src/cpu/thumb/format1.zig
@@ -0,0 +1,28 @@
+const std = @import("std");
+
+const Bus = @import("../../Bus.zig");
+const Arm7tdmi = @import("../../cpu.zig").Arm7tdmi;
+const InstrFn = @import("../../cpu.zig").ThumbInstrFn;
+const shifter = @import("../arm/barrel_shifter.zig");
+
+pub fn format1(comptime op: u2, comptime offset: u5) InstrFn {
+    return struct {
+        fn inner(cpu: *Arm7tdmi, _: *Bus, opcode: u16) void {
+            const rs = opcode >> 3 & 0x7;
+            const rd = opcode & 0x7;
+
+            const result = switch (op) {
+                0b00 => shifter.logicalLeft(true, &cpu.cpsr, cpu.r[rs], offset),
+                0b01 => shifter.logicalRight(true, &cpu.cpsr, cpu.r[rs], offset),
+                0b10 => shifter.arithmeticRight(true, &cpu.cpsr, cpu.r[rs], offset),
+                else => std.debug.panic("{} is an invalid op for Format 1 THUMB Instructions", .{op}),
+            };
+
+            cpu.r[rd] = result;
+
+            // Instructions of this type are equivalent to a MOVS
+            cpu.cpsr.n.write(result >> 31 & 1 == 1);
+            cpu.cpsr.z.write(result == 0);
+        }
+    }.inner;
+}