From 3ca9c9109ba8e028aac9ea57435bdaed25efa434 Mon Sep 17 00:00:00 2001
From: paoda <rekai@musuka.dev>
Date: Thu, 7 Sep 2023 03:36:17 -0500
Subject: [PATCH] feat: implement QADD/QSUB

---
 src/arm.zig                           |  2 ++
 src/arm/cpu/arm/misc_instructions.zig | 17 ---------------
 src/arm/cpu/arm/psr_transfer.zig      | 31 ++++++++++++++++++++++++++-
 src/arm/v5te.zig                      |  3 ---
 4 files changed, 32 insertions(+), 21 deletions(-)
 delete mode 100644 src/arm/cpu/arm/misc_instructions.zig

diff --git a/src/arm.zig b/src/arm.zig
index 65f246d..256351f 100644
--- a/src/arm.zig
+++ b/src/arm.zig
@@ -444,6 +444,8 @@ pub const PSR = extern union {
     t: Bit(u32, 5),
     f: Bit(u32, 6),
     i: Bit(u32, 7),
+
+    q: Bit(u32, 27), // ARMv5TE only
     v: Bit(u32, 28),
     c: Bit(u32, 29),
     z: Bit(u32, 30),
diff --git a/src/arm/cpu/arm/misc_instructions.zig b/src/arm/cpu/arm/misc_instructions.zig
deleted file mode 100644
index ebda272..0000000
--- a/src/arm/cpu/arm/misc_instructions.zig
+++ /dev/null
@@ -1,17 +0,0 @@
-const sext = @import("zba-util").sext;
-
-pub fn clz(comptime InstrFn: type) InstrFn {
-    const Arm32 = @typeInfo(@typeInfo(@typeInfo(InstrFn).Pointer.child).Fn.params[0].type.?).Pointer.child;
-
-    return struct {
-        pub fn inner(cpu: *Arm32, opcode: u32) void {
-            const rd = opcode >> 12 & 0xF;
-            const rm = opcode & 0xF;
-
-            if (rd == 0xF) cpu.panic("CLZ: UNPREDICTABLE behaviour when rd == 15", .{});
-            if (rm == 0xF) cpu.panic("CLZ: UNPREDICTABLE behaviour when rm == 15", .{});
-
-            cpu.r[rd] = @clz(cpu.r[rm]);
-        }
-    }.inner;
-}
diff --git a/src/arm/cpu/arm/psr_transfer.zig b/src/arm/cpu/arm/psr_transfer.zig
index 42ab6f8..66bc95e 100644
--- a/src/arm/cpu/arm/psr_transfer.zig
+++ b/src/arm/cpu/arm/psr_transfer.zig
@@ -29,7 +29,36 @@ pub fn control(comptime InstrFn: type, comptime I: bool, comptime op: u6) InstrF
                     const R = op >> 5 & 1 == 1;
                     msr(R, false, cpu, opcode);
                 },
-                else => cpu.panic("unhandled instruction: 0x{X:0>8}", .{opcode}),
+                0b01_0001 => cpu.panic("TODO: implement v5TE BX", .{}),
+                0b11_0001 => { // CLZ
+                    const rd = opcode >> 12 & 0xF;
+                    const rm = opcode & 0xF;
+
+                    if (rd == 0xF) cpu.panic("CLZ: UNPREDICTABLE behaviour when rd == 15", .{});
+                    if (rm == 0xF) cpu.panic("CLZ: UNPREDICTABLE behaviour when rm == 15", .{});
+
+                    cpu.r[rd] = @clz(cpu.r[rm]);
+                },
+                0b01_0011 => cpu.panic("TODO: implement v5TE BLX", .{}),
+                0b00_0101, 0b01_0101 => {
+                    const U = op >> 4 & 1 == 1; // A for Add? is there a convention?
+
+                    const rm = opcode & 0xF;
+                    const rd = opcode >> 12 & 0xF;
+                    const rn = opcode >> 16 & 0xF;
+
+                    const left: i32 = @bitCast(cpu.r[rm]);
+                    const right: i32 = @bitCast(cpu.r[rn]);
+
+                    cpu.r[rd] = @bitCast(if (U) left -| right else left +| right);
+
+                    if (cpu.r[rd] == if (U) 0x8000_0000 else 0x7FFF_FFFF) cpu.cpsr.q.set();
+                },
+                0b10_0101 => cpu.panic("TODO: implement QDADD", .{}),
+                0b11_0101 => cpu.panic("TODO: implement QDSUB", .{}),
+                0b01_0111 => cpu.panic("TODO: handle BKPT", .{}),
+
+                else => std.log.debug("unhandled instruction: 0x{X:0>8}", .{opcode}),
             }
         }
 
diff --git a/src/arm/v5te.zig b/src/arm/v5te.zig
index 4c2b387..77393a3 100644
--- a/src/arm/v5te.zig
+++ b/src/arm/v5te.zig
@@ -20,7 +20,6 @@ pub const arm = struct {
     const multiply = @import("cpu/arm/multiply.zig").multiply;
 
     const cop = @import("cpu/arm/coprocessor.zig");
-    const clz = @import("cpu/arm/misc_instructions.zig").clz;
 
     /// Determine index into ARM InstrFn LUT
     pub fn idx(opcode: u32) u12 {
@@ -43,8 +42,6 @@ pub const arm = struct {
                 handler.* = switch (@as(u2, i >> 10)) {
                     0b00 => if (i == 0x121) blk: { // 12 bits
                         break :blk branchExchange(InstrFn);
-                    } else if (i == 0x161) blk: { // 12 bits
-                        break :blk clz(InstrFn);
                     } else if (i & 0xFBF == 0x109) blk: { // 11 bits
                         const B = i >> 6 & 1 == 1;
                         break :blk swap(InstrFn, B);