fix(v5te): rework MSR/MRS handling to account for v5TE extension space

- implement clz in ARMv5TE

src/arm/cpu/arm/half_signed_data_transfer.zig

@@ -17,9 +17,11 @@ pub fn halfAndSignedDataTransfer(comptime InstrFn: type, comptime P: bool, compt
             const modified_base = if (U) base +% offset else base -% offset;
             var address = if (P) modified_base else base;
 
+            const op: u2 = @truncate(opcode >> 5);
             var result: u32 = undefined;
+
             if (L) {
-                switch (@as(u2, @truncate(opcode >> 5))) {
+                switch (op) {
                     0b01 => {
                         // LDRH
                         const value = cpu.read(u16, address);
@@ -36,15 +38,46 @@ pub fn halfAndSignedDataTransfer(comptime InstrFn: type, comptime P: bool, compt
                         // FIXME: I shouldn't have to use @as(u8, ...) here
                         result = if (address & 1 == 1) sext(u32, u8, @as(u8, @truncate(value >> 8))) else sext(u32, u16, value);
                     },
-                    0b00 => unreachable, // SWP
+                    0b00 => unreachable, // SWP / SWPB dealt with in single_data_swap.zig
                 }
             } else {
-                if (opcode >> 5 & 0x01 == 0x01) {
-                    // STRH
+                switch (op) {
+                    0b01 => {
+                        // STRH
 
-                    // FIXME: I shouldn't have to use @as(u8, ...) here
-                    cpu.write(u16, address, @as(u16, @truncate(cpu.r[rd])));
-                } else unreachable; // SWP
+                        // FIXME: I shouldn't have to use @as(u16, ...) here
+                        cpu.write(u16, address, @as(u16, @truncate(cpu.r[rd])));
+                    },
+                    0b10 => {
+                        // LDRD
+                        if (Arm32.arch != .v5te) cpu.panic("LDRD: unsupported on arm{s}", .{@tagName(Arm32.arch)});
+                        if (rd & 0 != 0) cpu.panic("LDRD: UNDEFINED behaviour when Rd is not even", .{});
+                        if (rd == 0xE) cpu.panic("LDRD: UNPREDICTABLE behaviour when rd == 14", .{});
+                        if (address & 0x7 != 0b000) cpu.panic("LDRD: UNPREDICTABLE when address (0x{X:0>8} is not double (64-bit) aligned", .{address});
+
+                        // Why do we not make use of result here?
+                        //
+                        // It's because L is not set so there's no chance of writing an undefined
+                        // value to the register
+                        //
+                        // despite this reason, this is bad design imo
+                        // TODO: Refactor this handler
+
+                        cpu.r[rd] = cpu.read(u32, address);
+                        cpu.r[rd + 1] = cpu.read(u32, address + 4);
+                    },
+                    0b11 => {
+                        // STRD
+                        if (Arm32.arch != .v5te) cpu.panic("STRD: unsupported on arm{s}", .{@tagName(Arm32.arch)});
+                        if (rd & 0 != 0) cpu.panic("STRD: UNDEFINED behaviour when Rd is not even", .{});
+                        if (rd == 0xE) cpu.panic("STRD: UNPREDICTABLE behaviour when rd == 14", .{});
+                        if (address & 0x7 != 0b000) cpu.panic("STRD: UNPREDICTABLE when address (0x{X:0>8} is not double (64-bit) aligned", .{address});
+
+                        cpu.write(u32, address, cpu.r[rd]);
+                        cpu.write(u32, address + 4, cpu.r[rd + 1]);
+                    },
+                    else => unreachable,
+                }
             }
 
             address = modified_base;

src/arm/cpu/arm/misc_instructions.zig

@@ -0,0 +1,17 @@
+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;
+}

src/arm/cpu/arm/multiply.zig

@@ -1,4 +1,4 @@
-pub fn multiply(comptime InstrFn: type, comptime A: bool, comptime S: bool) InstrFn {
+pub fn multiply(comptime InstrFn: type, comptime L: bool, comptime U: bool, comptime A: bool, comptime S: bool) InstrFn {
     const Arm32 = @typeInfo(@typeInfo(@typeInfo(InstrFn).Pointer.child).Fn.params[0].type.?).Pointer.child;
 
     return struct {
@@ -8,49 +8,41 @@ pub fn multiply(comptime InstrFn: type, comptime A: bool, comptime S: bool) Inst
             const rs = opcode >> 8 & 0xF;
             const rm = opcode & 0xF;
 
-            const temp: u64 = @as(u64, cpu.r[rm]) * @as(u64, cpu.r[rs]) + if (A) cpu.r[rn] else 0;
-            const result: u32 = @truncate(temp);
-            cpu.r[rd] = result;
+            if (L) {
+                const rd_hi = rd;
+                const rd_lo = rn;
 
-            if (S) {
-                cpu.cpsr.n.write(result >> 31 & 1 == 1);
-                cpu.cpsr.z.write(result == 0);
-                // V is unaffected, C is *actually* undefined in ARMv4
-            }
-        }
-    }.inner;
-}
+                if (U) {
+                    // Signed (WHY IS IT U THEN?)
+                    var result: i64 = @as(i64, @as(i32, @bitCast(cpu.r[rm]))) * @as(i64, @as(i32, @bitCast(cpu.r[rs])));
+                    if (A) result +%= @bitCast(@as(u64, cpu.r[rd_hi]) << 32 | @as(u64, cpu.r[rd_lo]));
 
-pub fn multiplyLong(comptime InstrFn: type, comptime U: bool, comptime A: bool, comptime S: bool) InstrFn {
-    const Arm32 = @typeInfo(@typeInfo(@typeInfo(InstrFn).Pointer.child).Fn.params[0].type.?).Pointer.child;
+                    cpu.r[rd_hi] = @bitCast(@as(i32, @truncate(result >> 32)));
+                    cpu.r[rd_lo] = @bitCast(@as(i32, @truncate(result)));
+                } else {
+                    // Unsigned
+                    var result: u64 = @as(u64, cpu.r[rm]) * @as(u64, cpu.r[rs]);
+                    if (A) result +%= @as(u64, cpu.r[rd_hi]) << 32 | @as(u64, cpu.r[rd_lo]);
 
-    return struct {
-        fn inner(cpu: *Arm32, opcode: u32) void {
-            const rd_hi = opcode >> 16 & 0xF;
-            const rd_lo = opcode >> 12 & 0xF;
-            const rs = opcode >> 8 & 0xF;
-            const rm = opcode & 0xF;
+                    cpu.r[rd_hi] = @truncate(result >> 32);
+                    cpu.r[rd_lo] = @truncate(result);
+                }
 
-            if (U) {
-                // Signed (WHY IS IT U THEN?)
-                var result: i64 = @as(i64, @as(i32, @bitCast(cpu.r[rm]))) * @as(i64, @as(i32, @bitCast(cpu.r[rs])));
-                if (A) result +%= @bitCast(@as(u64, cpu.r[rd_hi]) << 32 | @as(u64, cpu.r[rd_lo]));
-
-                cpu.r[rd_hi] = @bitCast(@as(i32, @truncate(result >> 32)));
-                cpu.r[rd_lo] = @bitCast(@as(i32, @truncate(result)));
+                if (S) {
+                    cpu.cpsr.z.write(cpu.r[rd_hi] == 0 and cpu.r[rd_lo] == 0);
+                    cpu.cpsr.n.write(cpu.r[rd_hi] >> 31 & 1 == 1);
+                    // C and V are set to meaningless values
+                }
             } else {
-                // Unsigned
-                var result: u64 = @as(u64, cpu.r[rm]) * @as(u64, cpu.r[rs]);
-                if (A) result +%= @as(u64, cpu.r[rd_hi]) << 32 | @as(u64, cpu.r[rd_lo]);
+                const temp: u64 = @as(u64, cpu.r[rm]) * @as(u64, cpu.r[rs]) + if (A) cpu.r[rn] else 0;
+                const result: u32 = @truncate(temp);
+                cpu.r[rd] = result;
 
-                cpu.r[rd_hi] = @truncate(result >> 32);
-                cpu.r[rd_lo] = @truncate(result);
-            }
-
-            if (S) {
-                cpu.cpsr.z.write(cpu.r[rd_hi] == 0 and cpu.r[rd_lo] == 0);
-                cpu.cpsr.n.write(cpu.r[rd_hi] >> 31 & 1 == 1);
-                // C and V are set to meaningless values
+                if (S) {
+                    cpu.cpsr.n.write(result >> 31 & 1 == 1);
+                    cpu.cpsr.z.write(result == 0);
+                    // V is unaffected, C is *actually* undefined in ARMv4
+                }
             }
         }
     }.inner;

src/arm/cpu/arm/psr_transfer.zig

@@ -1,42 +1,52 @@
 const std = @import("std");
-
+const rotr = @import("zba-util").rotr;
 const PSR = @import("../../../arm.zig").PSR;
 
-const log = std.log.scoped(.PsrTransfer);
+const log = std.log.scoped(.ctrl_ext_space);
 
-const rotr = @import("zba-util").rotr;
-
-pub fn psrTransfer(comptime InstrFn: type, comptime I: bool, comptime R: bool, comptime kind: u2) InstrFn {
+pub fn control(comptime InstrFn: type, comptime I: bool, comptime op: u6) InstrFn {
     const Arm32 = @typeInfo(@typeInfo(@typeInfo(InstrFn).Pointer.child).Fn.params[0].type.?).Pointer.child;
 
     return struct {
         fn inner(cpu: *Arm32, opcode: u32) void {
-            switch (kind) {
-                0b00 => {
-                    // MRS
+            if (I) {
+                // MSR (register)
+                const R = op >> 5 & 1 == 1;
+                msr(R, I, cpu, opcode);
+
+                return;
+            }
+
+            switch (op) {
+                0b00_0000, 0b10_0000 => { // MRS
+                    const R = op >> 5 & 1 == 1;
                     const rd = opcode >> 12 & 0xF;
 
                     if (R and !cpu.hasSPSR()) log.err("Tried to read SPSR from User/System Mode", .{});
                     cpu.r[rd] = if (R) cpu.spsr.raw else cpu.cpsr.raw;
                 },
-                0b10 => {
-                    // MSR
-                    const field_mask: u4 = @truncate(opcode >> 16 & 0xF);
-                    const rm_idx = opcode & 0xF;
-                    const right = if (I) rotr(u32, opcode & 0xFF, (opcode >> 8 & 0xF) * 2) else cpu.r[rm_idx];
-
-                    if (R and !cpu.hasSPSR()) log.err("Tried to write to SPSR in User/System Mode", .{});
-
-                    if (R) {
-                        // arm.gba seems to expect the SPSR to do somethign in SYS mode,
-                        // so we just assume that despite writing to the SPSR in USR or SYS mode
-                        // being UNPREDICTABLE, it just magically has a working SPSR somehow
-                        cpu.spsr.raw = fieldMask(&cpu.spsr, field_mask, right);
-                    } else {
-                        if (cpu.isPrivileged()) cpu.setCpsr(fieldMask(&cpu.cpsr, field_mask, right));
-                    }
+                0b01_0000, 0b11_0000 => { // MSR (register)
+                    const R = op >> 5 & 1 == 1;
+                    msr(R, false, cpu, opcode);
                 },
-                else => cpu.panic("[CPU/PSR Transfer] Bits 21:220 of {X:0>8} are undefined", .{opcode}),
+                else => cpu.panic("unhandled instruction: 0x{X:0>8}", .{opcode}),
+            }
+        }
+
+        inline fn msr(comptime R: bool, comptime imm: bool, cpu: *Arm32, opcode: u32) void {
+            const field_mask: u4 = @truncate(opcode >> 16 & 0xF);
+            const rm_idx = opcode & 0xF;
+            const right = if (imm) rotr(u32, opcode & 0xFF, (opcode >> 8 & 0xF) * 2) else cpu.r[rm_idx];
+
+            if (R and !cpu.hasSPSR()) log.err("Tried to write to SPSR in User/System Mode", .{});
+
+            if (R) {
+                // arm.gba seems to expect the SPSR to do somethign in SYS mode,
+                // so we just assume that despite writing to the SPSR in USR or SYS mode
+                // being UNPREDICTABLE, it just magically has a working SPSR somehow
+                cpu.spsr.raw = fieldMask(&cpu.spsr, field_mask, right);
+            } else {
+                if (cpu.isPrivileged()) cpu.setCpsr(fieldMask(&cpu.cpsr, field_mask, right));
             }
         }
     }.inner;

src/arm/v4t.zig

@@ -5,7 +5,6 @@ pub const arm = struct {
     pub const lut: [0x1000]InstrFn = populate();
 
     const processing = @import("cpu/arm/data_processing.zig").dataProcessing;
-    const psrTransfer = @import("cpu/arm/psr_transfer.zig").psrTransfer;
     const transfer = @import("cpu/arm/single_data_transfer.zig").singleDataTransfer;
     const halfSignedTransfer = @import("cpu/arm/half_signed_data_transfer.zig").halfAndSignedDataTransfer;
     const blockTransfer = @import("cpu/arm/block_data_transfer.zig").blockDataTransfer;
@@ -14,8 +13,11 @@ pub const arm = struct {
     const swi = @import("cpu/arm/software_interrupt.zig").armSoftwareInterrupt;
     const swap = @import("cpu/arm/single_data_swap.zig").singleDataSwap;
 
+    // Control Instruction Extension Space
+    const control = @import("cpu/arm/psr_transfer.zig").control;
+
+    /// Arithmetic Instruction Extension Space
     const multiply = @import("cpu/arm/multiply.zig").multiply;
-    const multiplyLong = @import("cpu/arm/multiply.zig").multiplyLong;
 
     /// Determine index into ARM InstrFn LUT
     pub fn idx(opcode: u32) u12 {
@@ -38,18 +40,15 @@ pub const arm = struct {
                 handler.* = switch (@as(u2, i >> 10)) {
                     0b00 => if (i == 0x121) blk: {
                         break :blk branchExchange(InstrFn);
-                    } else if (i & 0xFCF == 0x009) blk: {
-                        const A = i >> 5 & 1 == 1;
-                        const S = i >> 4 & 1 == 1;
-                        break :blk multiply(InstrFn, A, S);
                     } else if (i & 0xFBF == 0x109) blk: {
                         const B = i >> 6 & 1 == 1;
                         break :blk swap(InstrFn, B);
-                    } else if (i & 0xF8F == 0x089) blk: {
+                    } else if (i & 0xF0F == 0x009) blk: {
+                        const L = i >> 7 & 1 == 1;
                         const U = i >> 6 & 1 == 1;
                         const A = i >> 5 & 1 == 1;
                         const S = i >> 4 & 1 == 1;
-                        break :blk multiplyLong(InstrFn, U, A, S);
+                        break :blk multiply(InstrFn, L, U, A, S);
                     } else if (i & 0xE49 == 0x009 or i & 0xE49 == 0x049) blk: {
                         const P = i >> 8 & 1 == 1;
                         const U = i >> 7 & 1 == 1;
@@ -59,9 +58,9 @@ pub const arm = struct {
                         break :blk halfSignedTransfer(InstrFn, P, U, I, W, L);
                     } else if (i & 0xD90 == 0x100) blk: {
                         const I = i >> 9 & 1 == 1;
-                        const R = i >> 6 & 1 == 1;
-                        const kind = i >> 4 & 0x3;
-                        break :blk psrTransfer(InstrFn, I, R, kind);
+                        const op = ((i >> 5) & 0x3) << 4 | (i & 0xF);
+
+                        break :blk control(InstrFn, I, op);
                     } else blk: {
                         const I = i >> 9 & 1 == 1;
                         const S = i >> 4 & 1 == 1;

src/arm/v5te.zig

@@ -5,7 +5,6 @@ pub const arm = struct {
     pub const lut: [0x1000]InstrFn = populate();
 
     const processing = @import("cpu/arm/data_processing.zig").dataProcessing;
-    const psrTransfer = @import("cpu/arm/psr_transfer.zig").psrTransfer;
     const transfer = @import("cpu/arm/single_data_transfer.zig").singleDataTransfer;
     const halfSignedTransfer = @import("cpu/arm/half_signed_data_transfer.zig").halfAndSignedDataTransfer;
     const blockTransfer = @import("cpu/arm/block_data_transfer.zig").blockDataTransfer;
@@ -14,10 +13,14 @@ pub const arm = struct {
     const swi = @import("cpu/arm/software_interrupt.zig").armSoftwareInterrupt;
     const swap = @import("cpu/arm/single_data_swap.zig").singleDataSwap;
 
+    // Control Instruction Extension Space
+    const control = @import("cpu/arm/psr_transfer.zig").control;
+
+    // Arithmetic Instruction Extension Space
     const multiply = @import("cpu/arm/multiply.zig").multiply;
-    const multiplyLong = @import("cpu/arm/multiply.zig").multiplyLong;
 
     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 {
@@ -38,32 +41,30 @@ pub const arm = struct {
 
             for (&table, 0..) |*handler, i| {
                 handler.* = switch (@as(u2, i >> 10)) {
-                    0b00 => if (i == 0x121) blk: {
+                    0b00 => if (i == 0x121) blk: { // 12 bits
                         break :blk branchExchange(InstrFn);
-                    } else if (i & 0xFCF == 0x009) blk: {
-                        const A = i >> 5 & 1 == 1;
-                        const S = i >> 4 & 1 == 1;
-                        break :blk multiply(InstrFn, A, S);
-                    } else if (i & 0xFBF == 0x109) blk: {
+                    } 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);
-                    } else if (i & 0xF8F == 0x089) blk: {
+                    } else if (i & 0xF0F == 0x009) blk: { // 8 bits
+                        const L = i >> 7 & 1 == 1;
                         const U = i >> 6 & 1 == 1;
                         const A = i >> 5 & 1 == 1;
                         const S = i >> 4 & 1 == 1;
-                        break :blk multiplyLong(InstrFn, U, A, S);
-                    } else if (i & 0xE49 == 0x009 or i & 0xE49 == 0x049) blk: {
+                        break :blk multiply(InstrFn, L, U, A, S);
+                    } else if (i & 0xE49 == 0x009 or i & 0xE49 == 0x049) blk: { // 6 bits
                         const P = i >> 8 & 1 == 1;
                         const U = i >> 7 & 1 == 1;
                         const I = i >> 6 & 1 == 1;
                         const W = i >> 5 & 1 == 1;
                         const L = i >> 4 & 1 == 1;
                         break :blk halfSignedTransfer(InstrFn, P, U, I, W, L);
-                    } else if (i & 0xD90 == 0x100) blk: {
+                    } else if (i & 0xD90 == 0x100) blk: { // 6 bits
                         const I = i >> 9 & 1 == 1;
-                        const R = i >> 6 & 1 == 1;
-                        const kind = i >> 4 & 0x3;
-                        break :blk psrTransfer(InstrFn, I, R, kind);
+                        const op = ((i >> 5) & 0x3) << 4 | (i & 0xF);
+                        break :blk control(InstrFn, I, op);
                     } else blk: {
                         const I = i >> 9 & 1 == 1;
                         const S = i >> 4 & 1 == 1;