chore(cpu): refactor barrel shifter

src/cpu.zig

@@ -18,6 +18,8 @@ const branchAndExchange = @import("cpu/arm/branch.zig").branchAndExchange;
 
 // THUMB Instruction Groups
 const format3 = @import("cpu/thumb/format3.zig").format3;
+const format5 = @import("cpu/thumb/format5.zig").format5;
+const format12 = @import("cpu/thumb/format12.zig").format12;
 
 pub const ArmInstrFn = fn (*Arm7tdmi, *Bus, u32) void;
 pub const ThumbInstrFn = fn (*Arm7tdmi, *Bus, u16) void;
@@ -57,9 +59,12 @@ pub const Arm7tdmi = struct {
     pub fn step(self: *Self) u64 {
         if (self.cpsr.t.read()) {
             const opcode = self.thumbFetch();
+            // self.mgbaLog(@as(u32, opcode));
+
             thumb_lut[thumbIdx(opcode)](self, self.bus, opcode);
         } else {
             const opcode = self.fetch();
+            // self.mgbaLog(opcode);
 
             if (checkCond(self.cpsr, @truncate(u4, opcode >> 28))) {
                 arm_lut[armIdx(opcode)](self, self.bus, opcode);
@@ -109,7 +114,8 @@ pub const Arm7tdmi = struct {
 
         const cpsr = self.cpsr.raw;
 
-        nosuspend stderr.print("{X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} cpsr: {X:0>8} | {X:0>8}:\n", .{ r0, r1, r2, r3, r4, r5, r6, r7, r8, r9, r10, r11, r12, r13, r14, r15, cpsr, opcode }) catch return;
+        nosuspend stderr.print("{X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} cpsr: {X:0>8} | ", .{ r0, r1, r2, r3, r4, r5, r6, r7, r8, r9, r10, r11, r12, r13, r14, r15, cpsr }) catch return;
+        nosuspend if (self.cpsr.t.read()) stderr.print("{X:0>4}:\n", .{@truncate(u16, opcode)}) catch return else stderr.print("{X:0>8}:\n", .{opcode}) catch return;
     }
 };
 
@@ -156,6 +162,21 @@ fn thumbPopulate() [0x400]ThumbInstrFn {
 
                 lut[i] = format3(op, rd);
             }
+
+            if (i >> 4 & 0x3F == 0b010001) {
+                const op = i >> 2 & 0x3;
+                const h1 = i >> 1 & 1;
+                const h2 = i & 1;
+
+                lut[i] = format5(op, h1, h2);
+            }
+
+            if (i >> 6 & 0xF == 0b1010) {
+                const isSP = i >> 5 & 1 == 1;
+                const rd = i >> 2 & 0x7;
+
+                lut[i] = format12(isSP, rd);
+            }
         }
 
         return lut;

src/cpu/arm/barrel_shifter.zig

@@ -19,80 +19,75 @@ pub fn exec(comptime S: bool, cpu: *Arm7tdmi, opcode: u32) u32 {
         value = cpu.r[opcode & 0xF];
     }
 
-    if (S) {
-        return switch (@truncate(u2, opcode >> 5)) {
-            0b00 => logical_left(&cpu.cpsr, value, shift_amt),
-            0b01 => logical_right(&cpu.cpsr, value, shift_amt),
-            0b10 => arithmetic_right(&cpu.cpsr, value, shift_amt),
-            0b11 => rotate_right(&cpu.cpsr, value, shift_amt),
-        };
-    } else {
-        var dummy = CPSR{ .raw = 0x0000_0000 };
-        return switch (@truncate(u2, opcode >> 5)) {
-            0b00 => logical_left(&dummy, value, shift_amt),
-            0b01 => logical_right(&dummy, value, shift_amt),
-            0b10 => arithmetic_right(&dummy, value, shift_amt),
-            0b11 => rotate_right(&dummy, value, shift_amt),
-        };
-    }
+    return switch (@truncate(u2, opcode >> 5)) {
+        0b00 => logicalLeft(S, &cpu.cpsr, value, shift_amt),
+        0b01 => logicalRight(S, &cpu.cpsr, value, shift_amt),
+        0b10 => arithmeticRight(S, &cpu.cpsr, value, shift_amt),
+        0b11 => rotateRight(S, &cpu.cpsr, value, shift_amt),
+    };
 }
 
-pub fn logical_left(cpsr: *CPSR, rm: u32, shift_byte: u8) u32 {
-    const shift_amt = @truncate(u5, shift_byte);
+pub fn logicalLeft(comptime S: bool, cpsr: *CPSR, rm: u32, amount: u8) u32 {
+    const shift_amt = @truncate(u5, amount);
     const bit_count: u8 = @typeInfo(u32).Int.bits;
 
     var result: u32 = 0x0000_0000;
 
-    if (shift_byte < bit_count) {
+    if (amount < bit_count) {
         // We can perform a well-defined shift here
 
         // FIXME: We assume cpu.r[rs] == 0 and imm_shift == 0 are equivalent
-        if (shift_amt != 0) {
+        if (S and shift_amt != 0) {
             const carry_bit = @truncate(u5, bit_count - shift_amt);
             cpsr.c.write(rm >> carry_bit & 1 == 1);
         }
 
         result = rm << shift_amt;
-    } else if (shift_byte == bit_count) {
+    } else if (amount == bit_count) {
         // Shifted all bits out, carry bit is bit 0 of rm
-        cpsr.c.write(rm & 1 == 1);
+        if (S) cpsr.c.write(rm & 1 == 1);
     } else {
         // Shifted all bits out, carry bit has also been shifted out
-        cpsr.c.write(false);
+        if (S) cpsr.c.write(false);
     }
 
     return result;
 }
 
-pub fn logical_right(cpsr: *CPSR, rm: u32, shift_byte: u8) u32 {
-    const shift_amt = @truncate(u5, shift_byte);
+pub fn logicalRight(comptime S: bool, cpsr: *CPSR, rm: u32, amount: u32) u32 {
+    const shift_amt = @truncate(u5, amount);
     const bit_count: u8 = @typeInfo(u32).Int.bits;
 
     var result: u32 = 0x0000_0000;
 
-    if (shift_byte == 0 or shift_byte == bit_count) {
+    if (amount == 0 or amount == bit_count) {
         // Actualy LSR #32
-        cpsr.c.write(rm >> 31 & 1 == 1);
-    } else if (shift_byte < bit_count) {
+        if (S) cpsr.c.write(rm >> 31 & 1 == 1);
+    } else if (amount < bit_count) {
         // We can perform a well-defined shift
         const carry_bit = shift_amt - 1;
-        cpsr.c.write(rm >> carry_bit & 1 == 1);
+        if (S) cpsr.c.write(rm >> carry_bit & 1 == 1);
 
         result = rm >> shift_amt;
     } else {
         // All bits have been shifted out, including carry bit
-        cpsr.c.write(false);
+        if (S) cpsr.c.write(false);
     }
 
     return result;
 }
 
-pub fn arithmetic_right(_: *CPSR, _: u32, _: u8) u32 {
+pub fn arithmeticRight(comptime _: bool, _: *CPSR, _: u32, _: u8) u32 {
     // @bitCast(u32, @bitCast(i32, r_val) >> @truncate(u5, amount))
     std.debug.panic("[BarrelShifter] implement arithmetic shift right", .{});
 }
 
-pub fn rotate_right(_: *CPSR, _: u32, _: u8) u32 {
-    // std.math.rotr(u32, r_val, amount)
-    std.debug.panic("[BarrelShifter] implement rotate right", .{});
+pub fn rotateRight(comptime S: bool, cpsr: *CPSR, rm: u32, amount: u8) u32 {
+    const result = std.math.rotr(u32, rm, amount);
+
+    if (S and result != 0) {
+        cpsr.c.write(result >> 31 & 1 == 1);
+    }
+
+    return result;
 }

src/cpu/arm/data_processing.zig

@@ -22,7 +22,8 @@ pub fn dataProcessing(comptime I: bool, comptime S: bool, comptime instrKind: u4
 
             var op2: u32 = undefined;
             if (I) {
-                op2 = std.math.rotr(u32, opcode & 0xFF, (opcode >> 8 & 0xF) << 1);
+                const amount = @truncate(u8, (opcode >> 8 & 0xF) << 1);
+                op2 = BarrelShifter.rotateRight(S, &cpu.cpsr, opcode & 0xFF, amount);
             } else {
                 op2 = BarrelShifter.exec(S, cpu, opcode);
             }

src/cpu/arm/single_data_transfer.zig

@@ -4,7 +4,6 @@ const util = @import("../../util.zig");
 const BarrelShifter = @import("barrel_shifter.zig");
 const Bus = @import("../../Bus.zig");
 const Arm7tdmi = @import("../../cpu.zig").Arm7tdmi;
-const CPSR = @import("../../cpu.zig").PSR;
 const InstrFn = @import("../../cpu.zig").ArmInstrFn;
 
 pub fn singleDataTransfer(comptime I: bool, comptime P: bool, comptime U: bool, comptime B: bool, comptime W: bool, comptime L: bool) InstrFn {
@@ -55,16 +54,13 @@ pub fn singleDataTransfer(comptime I: bool, comptime P: bool, comptime U: bool,
 }
 
 fn registerOffset(cpu: *Arm7tdmi, opcode: u32) u32 {
-    const shift_byte = @truncate(u8, opcode >> 7 & 0x1F);
-
+    const amount = @truncate(u8, opcode >> 7 & 0x1F);
     const rm = cpu.r[opcode & 0xF];
 
-    var dummy = CPSR{ .raw = 0x0000_0000 };
-
     return switch (@truncate(u2, opcode >> 5)) {
-        0b00 => BarrelShifter.logical_left(&dummy, rm, shift_byte),
-        0b01 => BarrelShifter.logical_right(&dummy, rm, shift_byte),
-        0b10 => BarrelShifter.arithmetic_right(&dummy, rm, shift_byte),
-        0b11 => BarrelShifter.rotate_right(&dummy, rm, shift_byte),
+        0b00 => BarrelShifter.logicalLeft(false, &cpu.cpsr, rm, amount),
+        0b01 => BarrelShifter.logicalRight(false, &cpu.cpsr, rm, amount),
+        0b10 => BarrelShifter.arithmeticRight(false, &cpu.cpsr, rm, amount),
+        0b11 => BarrelShifter.rotateRight(false, &cpu.cpsr, rm, amount),
     };
 }

src/cpu/thumb/format12.zig

@@ -0,0 +1,16 @@
+const std = @import("std");
+
+const Bus = @import("../../Bus.zig");
+const Arm7tdmi = @import("../../cpu.zig").Arm7tdmi;
+const InstrFn = @import("../../cpu.zig").ThumbInstrFn;
+
+pub fn format12(comptime isSP: bool, comptime rd: u3) InstrFn {
+    return struct {
+        fn inner(cpu: *Arm7tdmi, _: *Bus, opcode: u16) void {
+            const left = if (isSP) cpu.r[13] else cpu.r[15] + 2 & 0xFFFF_FFFD; // fetch (+2)
+            const right = @truncate(u10, opcode & 0xFF) << 2;
+            const result = left + right; // TODO: What about overflows?
+            cpu.r[rd] = result;
+        }
+    }.inner;
+}

src/cpu/thumb/format3.zig

@@ -11,16 +11,33 @@ pub fn format3(comptime op: u2, comptime rd: u3) InstrFn {
 
             switch (op) {
                 0b00 => {
+                    // MOV
                     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 });
+                    // CMP
+                    const left = cpu.r[rd];
+                    const result = left -% offset;
+
+                    cpu.cpsr.n.write(result >> 31 & 1 == 1);
+                    cpu.cpsr.z.write(result == 0);
+                    cpu.cpsr.c.write(offset <= left);
+                    cpu.cpsr.v.write(((left ^ result) & (~offset ^ result)) >> 31 & 1 == 1);
                 },
                 0b10 => {
-                    std.debug.panic("TODO: Implement add R{}, #0x{X:0>2}", .{ rd, offset });
+                    const left = cpu.r[rd];
+
+                    var result: u32 = undefined;
+                    const didOverflow = @addWithOverflow(u32, left, offset, &result);
+                    cpu.r[rd] = result;
+
+                    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) & (offset ^ result)) >> 31 & 1 == 1);
                 },
                 0b11 => {
                     std.debug.panic("TODO: Implement sub R{}, #0x{X:0>2}", .{ rd, offset });

src/cpu/thumb/format5.zig

@@ -0,0 +1,35 @@
+const std = @import("std");
+
+const Bus = @import("../../Bus.zig");
+const Arm7tdmi = @import("../../cpu.zig").Arm7tdmi;
+const InstrFn = @import("../../cpu.zig").ThumbInstrFn;
+
+pub fn format5(comptime op: u2, comptime h1: u1, comptime h2: u1) InstrFn {
+    return struct {
+        fn inner(cpu: *Arm7tdmi, _: *Bus, opcode: u16) void {
+            const src = @as(u4, h2) << 3 | (opcode >> 3 & 0x7);
+            const dst = @as(u4, h1) << 3 | (opcode & 0x7);
+
+            switch (op) {
+                0b01 => {
+                    // CMP
+                    const left = cpu.r[dst];
+                    const right = cpu.r[src];
+                    const result = left -% right;
+
+                    cpu.cpsr.n.write(result >> 31 & 1 == 1);
+                    cpu.cpsr.z.write(result == 0);
+                    cpu.cpsr.c.write(right <= left);
+                    cpu.cpsr.v.write(((left ^ result) & (~right ^ result)) >> 31 & 1 == 1);
+                },
+                0b10 => cpu.r[dst] = cpu.r[src], // MOV
+                0b11 => {
+                    // BX
+                    cpu.cpsr.t.write(cpu.r[src] & 1 == 1);
+                    cpu.r[15] = cpu.r[src] & 0xFFFF_FFFE;
+                },
+                else => std.debug.panic("[CPU] Op #{} is invalid for THUMB Format 5", .{op}),
+            }
+        }
+    }.inner;
+}