feat: integrate cp15 and TCM code

src/arm.zig

@@ -4,6 +4,7 @@ const Architecture = enum { v4t, v5te };
 const Interpreter = @import("lib.zig").Interpreter;
 const Bus = @import("lib.zig").Bus;
 const Scheduler = @import("lib.zig").Scheduler;
+const Coprocessor = @import("lib.zig").Coprocessor;
 
 const Bitfield = @import("bitfield").Bitfield;
 const Bit = @import("bitfield").Bit;
@@ -46,6 +47,7 @@ pub fn Arm32(comptime isa: Architecture) type {
         // The following will be `void` on.v4t but exist on v5te
         itcm: if (is_v5te) Itcm else void,
         dtcm: if (is_v5te) Dtcm else void,
+        cp15: if (is_v5te) Coprocessor else void,
 
         const arm = switch (isa) {
             .v4t => @import("arm/v4t.zig").arm,
@@ -154,17 +156,64 @@ pub fn Arm32(comptime isa: Architecture) type {
             }
         };
 
-        pub fn init(scheduler: Scheduler, bus: Bus) Self {
-            return .{
-                .sched = scheduler,
-                .bus = bus,
-                .cpsr = .{ .raw = 0x0000_001F },
-                .spsr = .{ .raw = 0x0000_0000 },
+        // FIXME: Is this a hack or idiomatic?
+        // See https://github.com/ziglang/zig/blob/1a0e6bcdb140c844384d62b78a7f4247753f9ffd/lib/std/atomic/Atomic.zig#L156-L176
+        pub usingnamespace if (is_v5te) struct {
+            // FIXME: this is pretty NDS9 specific lol
+            pub fn init(scheduler: Scheduler, bus: Bus, cp15: Coprocessor) Self {
+                return .{
+                    .sched = scheduler,
+                    .bus = bus,
+                    .cpsr = .{ .raw = 0x0000_001F },
+                    .spsr = .{ .raw = 0x0000_0000 },
 
-                .dtcm = if (is_v5te) .{} else {},
-                .itcm = if (is_v5te) .{} else {},
-            };
-        }
+                    .cp15 = cp15,
+                    .dtcm = .{},
+                    .itcm = .{},
+                };
+            }
+
+            // FIXME: Resetting disables logging (if enabled)
+            pub fn reset(self: *Self) void {
+                self.* = .{
+                    .sched = self.sched,
+                    .bus = self.bus,
+                    .cpsr = .{ .raw = 0x0000_001F },
+                    .spsr = .{ .raw = 0x0000_0000 },
+
+                    .dtcm = .{},
+                    .itcm = .{},
+                    .cp15 = self.cp15,
+                };
+            }
+        } else struct {
+            pub fn init(scheduler: Scheduler, bus: Bus) Self {
+                return .{
+                    .sched = scheduler,
+                    .bus = bus,
+                    .cpsr = .{ .raw = 0x0000_001F },
+                    .spsr = .{ .raw = 0x0000_0000 },
+
+                    .cp15 = {},
+                    .dtcm = {},
+                    .itcm = {},
+                };
+            }
+
+            // FIXME: Resetting disables logging (if enabled)
+            pub fn reset(self: *Self) void {
+                self.* = .{
+                    .sched = self.sched,
+                    .bus = self.bus,
+                    .cpsr = .{ .raw = 0x0000_001F },
+                    .spsr = .{ .raw = 0x0000_0000 },
+
+                    .dtcm = {},
+                    .itcm = {},
+                    .cp15 = {},
+                };
+            }
+        };
 
         // CPU needs it's own read/write fns due to ICTM and DCTM present in v5te
         // I considered implementing Bus.cpu_read and Bus.cpu_write but ended up considering that a bit too leaky
@@ -172,17 +221,14 @@ pub fn Arm32(comptime isa: Architecture) type {
             const readInt = std.mem.readIntSliceLittle;
 
             if (is_v5te) {
-                const itcm_base: u32 = self.itcm.base_address;
-                const itcm_size = self.itcm.buf.len;
-                const dtcm_base: u32 = self.dtcm.base_address;
-                const dtcm_size = self.dtcm.buf.len;
+                const dtcm_base = self.dtcm.base_address;
+                const dtcm_size = self.dtcm.virt.size;
 
-                // FIXME: verify correctness + can this be faster?
-                if (itcm_base < address and address < itcm_base + itcm_size)
-                    return readInt(T, self.itcm.buf[address & 0x0000_7FFF ..][0..@sizeOf(T)]);
+                if (address < 0x0000_0000 + self.itcm.virt.size)
+                    return readInt(T, self.itcm.buf[address & self.itcm.virt.mask ..][0..@sizeOf(T)]);
 
                 if (dtcm_base < address and address < dtcm_base + dtcm_size)
-                    return readInt(T, self.itcm.buf[address & 0x0000_3FFF ..][0..@sizeOf(T)]);
+                    return readInt(T, self.dtcm.buf[address & self.dtcm.virt.mask ..][0..@sizeOf(T)]);
             }
 
             return self.bus.read(T, address);
@@ -192,35 +238,19 @@ pub fn Arm32(comptime isa: Architecture) type {
             const writeInt = std.mem.writeIntSliceLittle;
 
             if (is_v5te) {
-                const itcm_base: u32 = self.itcm.base_address;
-                const itcm_size = self.itcm.buf.len;
-                const dtcm_base: u32 = self.dtcm.base_address;
-                const dtcm_size = self.dtcm.buf.len;
+                const dtcm_base = self.dtcm.base_address;
+                const dtcm_size = self.dtcm.virt.size;
 
-                // FIXME: verify correctness + can this be faster?
-                if (itcm_base < address and address < itcm_base + itcm_size)
-                    return writeInt(T, self.itcm.buf[address & 0x0000_7FFF ..][0..@sizeOf(T)], value);
+                if (address < 0x0000_0000 + self.itcm.virt.size)
+                    return writeInt(T, self.itcm.buf[address & self.itcm.virt.mask ..][0..@sizeOf(T)], value);
 
                 if (dtcm_base < address and address < dtcm_base + dtcm_size)
-                    return writeInt(T, self.itcm.buf[address & 0x0000_3FFF ..][0..@sizeOf(T)], value);
+                    return writeInt(T, self.dtcm.buf[address & self.dtcm.virt.mask ..][0..@sizeOf(T)], value);
             }
 
             return self.bus.write(T, address, value);
         }
 
-        // FIXME: Resetting disables logging (if enabled)
-        pub fn reset(self: *Self) void {
-            self.* = .{
-                .sched = self.sched,
-                .bus = self.bus,
-                .cpsr = .{ .raw = 0x0000_001F },
-                .spsr = .{ .raw = 0x0000_0000 },
-
-                .dtcm = if (is_v5te) .{} else {},
-                .itcm = if (is_v5te) .{} else {},
-            };
-        }
-
         pub inline fn hasSPSR(self: *const Self) bool {
             const mode = Mode.getChecked(self, self.cpsr.mode.read());
             return switch (mode) {
@@ -403,6 +433,7 @@ fn Tcm(comptime count: usize, comptime default_addr: u32) type {
     return struct {
         buf: [count * KiB]u8 = [_]u8{0x00} ** (count * KiB),
         base_address: u32 = default_addr,
+        virt: struct { size: u32, mask: u32 } = .{ .size = count * KiB, .mask = (count * KiB) - 1 },
     };
 }
 

src/arm/cpu/arm/coprocessor.zig

@@ -2,34 +2,133 @@ const std = @import("std");
 
 const log = std.log.scoped(.coprocessor_handler);
 
-pub fn dataTransfer(comptime InstrFn: type, comptime P: bool, comptime U: bool, comptime N: bool, comptime W: bool, comptime L: bool) InstrFn {
-    _ = L;
-    _ = W;
-    _ = N;
-    _ = U;
-    _ = P;
+pub fn dataTransfer(
+    comptime InstrFn: type,
+    comptime P: bool,
+    comptime U: bool,
+    comptime N: bool,
+    comptime W: bool,
+    comptime L: bool,
+) InstrFn {
     const Arm32 = @typeInfo(@typeInfo(@typeInfo(InstrFn).Pointer.child).Fn.params[0].type.?).Pointer.child;
 
     return struct {
         fn inner(cpu: *Arm32, opcode: u32) void {
-            _ = cpu;
+            if (!P and !W and !U) return copExt(cpu, opcode); // Coprocessor Extension Space
 
-            log.err("TODO: handle 0x{X:0>8} which is a coprocessor data transfer instr", .{opcode});
+            const rn = opcode >> 16 & 0xF;
+            const crd = opcode >> 12 & 0xF;
+            const cp_num = opcode >> 8 & 0xF;
+            const offset = (opcode & 0xFF) << 2;
+
+            // TODO: Make sure this is comptime
+            const addr_mode: u2 = comptime @as(u2, @intFromBool(P)) << 1 | @intFromBool(W);
+
+            const start_address: u32 = switch (addr_mode) {
+                0b00 => blk: {
+                    // Unindexed Addressing
+                    std.debug.assert(U == true);
+
+                    break :blk cpu.r[rn];
+                },
+                0b01 => blk: {
+                    // Immediate Post-Indexed Addressing
+                    const addr = cpu.r[rn];
+                    cpu.r[rn] = if (U) cpu.r[rn] + offset else cpu.r[rn] - offset;
+
+                    break :blk addr;
+                },
+                0b10 => if (U) cpu.r[rn] + offset else cpu.r[rn] - offset, // Immediate Offset Addressing
+                0b11 => blk: {
+                    // Immediate Pre-Indexed Addressing
+                    cpu.r[rn] = if (U) cpu.r[rn] + offset else cpu.r[rn] - offset;
+
+                    break :blk cpu.r[rn];
+                },
+            };
+
+            // TODO: Increment address + 4 (and perform op) until coprocessor says stop
+
+            if (L) {
+                log.debug("TODO: ldc{s} p{}, c{}, 0x{X:0>8}", .{ [_]u8{if (N) 'l' else ' '}, cp_num, crd, start_address });
+            } else {
+                log.debug("TODO: stc{s} p{}, c{}, 0x{X:0>8}", .{ [_]u8{if (N) 'l' else ' '}, cp_num, crd, start_address });
+            }
+        }
+
+        fn copExt(cpu: *Arm32, opcode: u32) void {
+            _ = cpu;
+            const cp_num = opcode >> 8 & 0xF;
+            const rd = opcode >> 12 & 0xF;
+            const rn = opcode >> 16 & 0xF;
+            const crm = opcode & 0xF;
+
+            const cp_opcode = opcode >> 4 & 0xF; // FIXME: We could get this value at comptime
+
+            std.debug.assert(rd != 15); // UNPREDICTABLE
+            std.debug.assert(rn != 15); // UNPREDICTABLE
+
+            if (L) {
+                // MRRC
+                log.debug("TODO: mrrc p{}, {}, r{}, r{}, c{}", .{ cp_num, cp_opcode, rd, rn, crm });
+            } else {
+                // MCRR
+                log.debug("TODO: mcrr p{}, {}, r{}, r{}, c{}", .{ cp_num, cp_opcode, rd, rn, crm });
+            }
         }
     }.inner;
 }
 
 pub fn registerTransfer(comptime InstrFn: type, comptime opcode1: u3, comptime L: bool, comptime opcode2: u3) InstrFn {
-    _ = opcode2;
-    _ = L;
-    _ = opcode1;
     const Arm32 = @typeInfo(@typeInfo(@typeInfo(InstrFn).Pointer.child).Fn.params[0].type.?).Pointer.child;
 
     return struct {
         fn inner(cpu: *Arm32, opcode: u32) void {
-            _ = cpu;
+            const crn: u4 = @intCast(opcode >> 16 & 0xF);
+            const rd = opcode >> 12 & 0xF;
+            const cp_num = opcode >> 8 & 0xF;
+            const crm: u4 = @intCast(opcode & 0xF);
 
-            log.err("TODO: handle 0x{X:0>8} which is a coprocessor register transfer instr", .{opcode});
+            std.debug.assert(cp_num == 0xF); // There's no other coprocessor on NDS9;
+
+            if (L) {
+                // MRC
+                const value = cpu.cp15.read(opcode1, crn, crm, opcode2);
+
+                if (rd != 0xF) {
+                    cpu.r[rd] = value;
+                    return;
+                }
+
+                // TODO: I can probably do this with a mask and the like
+                cpu.cpsr.n.write(value >> 31 & 1 == 1);
+                cpu.cpsr.z.write(value >> 30 & 1 == 1);
+                cpu.cpsr.c.write(value >> 29 & 1 == 1);
+                cpu.cpsr.v.write(value >> 28 & 1 == 1);
+            } else {
+                // MCR
+                std.debug.assert(rd != 0xF); // UNPREDICTABLE
+
+                cpu.cp15.write(opcode1, crn, crm, opcode2, cpu.r[rd]);
+
+                {
+                    // OK so the idea is that I don't want to pass the coprocessor a reference to the CPU,
+                    // so there's some side effects that we need to deal with. Right now I think I'll just process
+                    // all side affects on every MCR write and hope this isn't too awful
+                    // TODO: there has to be a better way.....
+
+                    // ICTM / DTCM Stuff
+                    const dtcm_size_base = cpu.cp15.read(0, 9, 1, 0); // mrc 0, c9, c1, 0
+                    const itcm_size_base = cpu.cp15.read(0, 9, 1, 1); // mrc 0, c9, c1, 1
+
+                    cpu.dtcm.base_address = dtcm_size_base & 0xFFFF_F000;
+                    cpu.dtcm.virt.size = @as(u32, 0x200) << @truncate(std.math.clamp(dtcm_size_base >> 1 & 0x1F, 3, 23));
+                    cpu.dtcm.virt.mask = std.math.clamp(cpu.dtcm.virt.size, 0, @as(u32, @intCast(cpu.dtcm.buf.len))) - 1;
+
+                    cpu.itcm.virt.size = @as(u32, 0x200) << @truncate(std.math.clamp(itcm_size_base >> 1 & 0x1F, 3, 23));
+                    cpu.itcm.virt.mask = std.math.clamp(cpu.itcm.virt.size, 0, @as(u32, @intCast(cpu.itcm.buf.len))) - 1;
+                }
+            }
         }
     }.inner;
 }

src/arm/cpu/arm/half_signed_data_transfer.zig

@@ -38,10 +38,28 @@ 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 / SWPB dealt with in single_data_swap.zig
+                    0b00 => unreachable,
                 }
             } else {
                 switch (op) {
+                    0b00 => {
+                        const B = I;
+                        const swap_addr = cpu.r[rn];
+
+                        if (B) {
+                            // SWPB
+                            const value = cpu.read(u8, swap_addr);
+                            cpu.write(u8, swap_addr, @as(u8, @truncate(cpu.r[rm])));
+
+                            cpu.r[rd] = value;
+                        } else {
+                            // SWP
+                            const value = rotr(u32, cpu.read(u32, swap_addr), 8 * (swap_addr & 0x3));
+                            cpu.write(u32, swap_addr, cpu.r[rm]);
+
+                            cpu.r[rd] = value;
+                        }
+                    },
                     0b01 => {
                         // STRH
 
@@ -76,7 +94,6 @@ pub fn halfAndSignedDataTransfer(comptime InstrFn: type, comptime P: bool, compt
                         cpu.write(u32, address, cpu.r[rd]);
                         cpu.write(u32, address + 4, cpu.r[rd + 1]);
                     },
-                    else => unreachable,
                 }
             }
 

src/arm/cpu/arm/psr_transfer.zig

@@ -11,7 +11,7 @@ pub fn control(comptime InstrFn: type, comptime I: bool, comptime op: u6) InstrF
     return struct {
         fn inner(cpu: *Arm32, opcode: u32) void {
             if (I) {
-                // MSR (register)
+                // MSR Immediate
                 const R = op >> 5 & 1 == 1;
                 return msr(R, I, cpu, opcode);
             }
@@ -38,8 +38,18 @@ pub fn control(comptime InstrFn: type, comptime I: bool, comptime op: u6) InstrF
 
                     cpu.r[rd] = @clz(cpu.r[rm]);
                 },
-                0b01_0011 => cpu.panic("TODO: implement v5TE BLX", .{}),
-                0b00_0101, 0b01_0101 => {
+                0b01_0011 => { // BLX
+                    const rm = opcode & 0xF;
+
+                    const thumb = cpu.r[rm] & 1 == 1;
+                    cpu.r[14] = cpu.r[15] - 4; // TODO: Why - 4?
+
+                    cpu.r[15] = cpu.r[rm] & ~@as(u32, 1);
+                    cpu.cpsr.t.write(thumb);
+
+                    cpu.pipe.reload(cpu);
+                },
+                0b00_0101, 0b01_0101 => { // QADD / QSUB
                     const U = op >> 4 & 1 == 1;
 
                     const rm = opcode & 0xF;
@@ -51,10 +61,23 @@ pub fn control(comptime InstrFn: type, comptime I: bool, comptime op: u6) InstrF
 
                     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();
+                    if (cpu.r[rd] == 0x8000_0000 or cpu.r[rd] == 0x7FFF_FFFF) cpu.cpsr.q.set();
+                },
+                0b10_0101, 0b11_0101 => { // QDADD / QDSUB
+                    const U = op >> 4 & 1 == 1;
+
+                    const rm = opcode & 0xF;
+                    const rd = opcode >> 8 & 0xF;
+                    const rn = opcode >> 16 & 0xF;
+
+                    const product = @as(i32, @bitCast(cpu.r[rn])) *| 2;
+                    if (product == 0x7FFF_FFFF) cpu.cpsr.q.set();
+
+                    const left: i32 = @bitCast(cpu.r[rm]);
+
+                    cpu.r[rd] = @bitCast(if (U) left -| product else left +| product);
+                    if (cpu.r[rd] == 0x8000_0000 or cpu.r[rd] == 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", .{}),
                 0b00_1000, 0b00_1010, 0b00_1100, 0b00_1110 => { // SMLA<x><y>
                     const X = op >> 1 & 1;
@@ -65,20 +88,102 @@ pub fn control(comptime InstrFn: type, comptime I: bool, comptime op: u6) InstrF
                     const rn = opcode >> 12 & 0xF;
                     const rd = opcode >> 16 & 0xF;
 
-                    const left: i16 = @intCast((cpu.r[rm] >> 16 * X) & 0xFFFF);
-                    const right: i16 = @intCast((cpu.r[rs] >> 16 * Y) & 0xFFFF);
+                    const left: i32 = @as(i16, @bitCast(@as(u16, @truncate(cpu.r[rm] >> 16 * X))));
+                    const right: i32 = @as(i16, @bitCast(@as(u16, @truncate(cpu.r[rs] >> 16 * Y))));
                     const accumulate: i32 = @bitCast(cpu.r[rn]);
 
-                    const result = @addWithOverflow(@as(i32, left) * @as(i32, right), accumulate);
+                    const result = @addWithOverflow(left * right, accumulate);
                     cpu.r[rd] = @bitCast(result[0]);
 
                     if (result[1] == 0b1) cpu.cpsr.q.set();
                 },
 
-                else => cpu.panic("unhandled instruction: 0x{X:0>8}", .{opcode}),
+                0b10_1000, 0b10_1010, 0b10_1100, 0b10_1110 => { // SMLAL<x><y>
+                    const X = op >> 1 & 1;
+                    const Y = op >> 2 & 1;
+
+                    const rm = opcode & 0xF;
+                    const rs = opcode >> 8 & 0xF;
+                    const rd_lo = opcode >> 12 & 0xF;
+                    const rd_hi = opcode >> 16 & 0xF;
+
+                    const left: i32 = @as(i16, @bitCast(@as(u16, @truncate(cpu.r[rm] >> 16 * X))));
+                    const right: i32 = @as(i16, @bitCast(@as(u16, @truncate(cpu.r[rs] >> 16 * Y))));
+
+                    // TODO: de-clutter this lmao
+                    const rdlo_val: i32 = @bitCast(cpu.r[rd_lo]);
+                    const product = left * right;
+
+                    // WHY DOESN'T THIS WORK????????
+
+                    cpu.r[rd_lo] = @bitCast(rdlo_val + product);
+                    cpu.r[rd_hi] = blk: {
+                        // FIXME: I think this has to do with correcting sign values?
+                        const offset_thing: i32 = @bitCast(if (product < 0) 0xFFFF_FFFF else @as(u32, 0));
+
+                        const rdhi_val: i32 = @bitCast(cpu.r[rd_hi]);
+                        break :blk @bitCast(rdhi_val + offset_thing + @addWithOverflow(rdlo_val, product)[1]);
+                    };
+                },
+                0b01_1000, 0b01_1100 => { // SMLAW<y>
+                    const Y = op >> 2 & 1;
+
+                    // TODO: deduplicate all this
+                    const rm = opcode & 0xF;
+                    const rs = opcode >> 8 & 0xF;
+                    const rn = opcode >> 12 & 0xF;
+                    const rd = opcode >> 16 & 0xF;
+
+                    const right: i16 = @as(i16, @bitCast(@as(u16, @truncate(cpu.r[rs] >> 16 * Y))));
+                    const left: i48 = @as(i32, @bitCast(cpu.r[rm]));
+                    const accumulate: i32 = @bitCast(cpu.r[rn]);
+
+                    const ret = @addWithOverflow(@as(i32, @truncate((left * right) >> 16)), accumulate);
+
+                    cpu.r[rd] = @bitCast(ret[0]);
+                    if (ret[1] == 0b1) cpu.cpsr.q.set();
+                },
+
+                0b01_1010, 0b01_1110 => { // SMULW<y>
+                    const Y = op >> 2 & 1;
+
+                    const rm = opcode & 0xF;
+                    const rs = opcode >> 8 & 0xF;
+                    const rd = opcode >> 16 & 0xF;
+
+                    const right: i64 = @as(i16, @bitCast(@as(u16, @truncate(cpu.r[rs] >> 16 * Y))));
+                    const left: i64 = @as(i32, @bitCast(cpu.r[rm]));
+
+                    const product: i32 = @truncate((left * right) >> 16);
+                    cpu.r[rd] = @bitCast(product);
+                },
+                0b11_1000, 0b11_1010, 0b11_1100, 0b11_1110 => { // SMUL<x><y>
+                    const X = op >> 1 & 1;
+                    const Y = op >> 2 & 1;
+
+                    const rm = opcode & 0xF;
+                    const rs = opcode >> 8 & 0xF;
+                    const rd = opcode >> 16 & 0xF;
+
+                    const left: i32 = @as(i16, @bitCast(@as(u16, @truncate(cpu.r[rm] >> 16 * X))));
+                    const right: i32 = @as(i16, @bitCast(@as(u16, @truncate(cpu.r[rs] >> 16 * Y))));
+
+                    cpu.r[rd] = @bitCast(left *% right);
+                },
+                else => cpu.panic("0x{X:0>8} was improperly handled by the control instruction extension space", .{opcode}),
             }
         }
 
+        // TODO: make generic on any integer?
+        inline fn carryFrom(left: i32, right: i32) u1 {
+            const _left: u32 = @bitCast(left);
+            const _right: u32 = @bitCast(right);
+
+            const sum = @as(u64, _left) + @as(u64, _right);
+
+            return @intCast(sum >> 32 & 1);
+        }
+
         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;
@@ -99,16 +204,12 @@ pub fn control(comptime InstrFn: type, comptime I: bool, comptime op: u6) InstrF
 }
 
 fn fieldMask(psr: *const PSR, field_mask: u4, right: u32) u32 {
-    // This bitwise ORs bits 3 and 0 of the field mask into a u2
-    // We do this because we only care about bits 7:0 and 31:28 of the CPSR
-    const bits: u2 = @truncate((field_mask >> 2 & 0x2) | (field_mask & 1));
+    var mask: u32 = 0;
 
-    const mask: u32 = switch (bits) {
-        0b00 => 0x0000_0000,
-        0b01 => 0x0000_00FF,
-        0b10 => 0xF000_0000,
-        0b11 => 0xF000_00FF,
-    };
+    inline for (0..4) |i| {
+        if (field_mask & @as(u4, 1) << i != 0)
+            mask |= @as(u32, 0xFF) << 8 * i;
+    }
 
     return (psr.raw & ~mask) | (right & mask);
 }

src/arm/cpu/arm/single_data_swap.zig

@@ -1,29 +0,0 @@
-const rotr = @import("zba-util").rotr;
-
-pub fn singleDataSwap(comptime InstrFn: type, comptime B: bool) InstrFn {
-    const Arm32 = @typeInfo(@typeInfo(@typeInfo(InstrFn).Pointer.child).Fn.params[0].type.?).Pointer.child;
-
-    return struct {
-        fn inner(cpu: *Arm32, opcode: u32) void {
-            const rn = opcode >> 16 & 0xF;
-            const rd = opcode >> 12 & 0xF;
-            const rm = opcode & 0xF;
-
-            const address = cpu.r[rn];
-
-            if (B) {
-                // SWPB
-                const value = cpu.read(u8, address);
-
-                // FIXME: I shouldn't have to use @as(u8, ...) here
-                cpu.write(u8, address, @as(u8, @truncate(cpu.r[rm])));
-                cpu.r[rd] = value;
-            } else {
-                // SWP
-                const value = rotr(u32, cpu.read(u32, address), 8 * (address & 0x3));
-                cpu.write(u32, address, cpu.r[rm]);
-                cpu.r[rd] = value;
-            }
-        }
-    }.inner;
-}

src/arm/v4t.zig

@@ -6,18 +6,19 @@ pub const arm = struct {
 
     const processing = @import("cpu/arm/data_processing.zig").dataProcessing;
     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;
     const branch = @import("cpu/arm/branch.zig").branch;
     const branchExchange = @import("cpu/arm/branch.zig").branchAndExchange;
     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;
+    /// Load Store Instruction Extention Space
+    const loadStoreExt = @import("cpu/arm/half_signed_data_transfer.zig").halfAndSignedDataTransfer;
+
+    /// Control Instruction Extension Space
+    const controlExt = @import("cpu/arm/psr_transfer.zig").control;
 
     /// Arithmetic Instruction Extension Space
-    const multiply = @import("cpu/arm/multiply.zig").multiply;
+    const multiplyExt = @import("cpu/arm/multiply.zig").multiply;
 
     /// Determine index into ARM InstrFn LUT
     pub fn idx(opcode: u32) u12 {
@@ -38,33 +39,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 & 0xFBF == 0x109) blk: {
-                        const B = i >> 6 & 1 == 1;
-                        break :blk swap(InstrFn, B);
-                    } else if (i & 0xF0F == 0x009) 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 multiply(InstrFn, L, U, A, S);
-                    } else if (i & 0xE49 == 0x009 or i & 0xE49 == 0x049) blk: {
+                        break :blk multiplyExt(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: {
+                        break :blk loadStoreExt(InstrFn, P, U, I, W, L);
+                    } else if (i & 0xD90 == 0x100) blk: { // 5 bits
                         const I = i >> 9 & 1 == 1;
                         const op = ((i >> 5) & 0x3) << 4 | (i & 0xF);
-                        break :blk control(InstrFn, I, op);
+                        break :blk controlExt(InstrFn, I, op);
                     } else blk: {
                         const I = i >> 9 & 1 == 1;
                         const S = i >> 4 & 1 == 1;
-                        const instrKind = i >> 5 & 0xF;
-                        break :blk processing(InstrFn, I, S, instrKind);
+                        const instr_kind = i >> 5 & 0xF;
+                        break :blk processing(InstrFn, I, S, instr_kind);
                     },
                     0b01 => if (i >> 9 & 1 == 1 and i & 1 == 1) und else blk: {
                         const I = i >> 9 & 1 == 1;

src/arm/v5te.zig

@@ -6,18 +6,19 @@ pub const arm = struct {
 
     const processing = @import("cpu/arm/data_processing.zig").dataProcessing;
     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;
     const branch = @import("cpu/arm/branch.zig").branch;
     const branchExchange = @import("cpu/arm/branch.zig").branchAndExchange;
     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;
+    /// Load Store Instruction Extention Space
+    const loadStoreExt = @import("cpu/arm/half_signed_data_transfer.zig").halfAndSignedDataTransfer;
 
-    // Arithmetic Instruction Extension Space
-    const multiply = @import("cpu/arm/multiply.zig").multiply;
+    /// Control Instruction Extension Space
+    const controlExt = @import("cpu/arm/psr_transfer.zig").control;
+
+    /// Arithmetic Instruction Extension Space
+    const multiplyExt = @import("cpu/arm/multiply.zig").multiply;
 
     const cop = @import("cpu/arm/coprocessor.zig");
 
@@ -37,36 +38,35 @@ pub const arm = struct {
         return comptime comptime_blk: {
             @setEvalBranchQuota(0xE000);
             var table = [_]InstrFn{und} ** 0x1000;
+            // op : 27 26 25 24 23 22 21 20 07 06 05 04
+            // idx: 11 10 09 08 07 06 05 04 03 02 01 00
 
             for (&table, 0..) |*handler, i| {
                 handler.* = switch (@as(u2, i >> 10)) {
                     0b00 => if (i == 0x121) blk: { // 12 bits
                         break :blk branchExchange(InstrFn);
-                    } else if (i & 0xFBF == 0x109) blk: { // 11 bits
-                        const B = i >> 6 & 1 == 1;
-                        break :blk swap(InstrFn, B);
                     } 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 multiply(InstrFn, L, U, A, S);
+                        break :blk multiplyExt(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);
+                        break :blk loadStoreExt(InstrFn, P, U, I, W, L);
                     } else if (i & 0xD90 == 0x100) blk: { // 6 bits
                         const I = i >> 9 & 1 == 1;
                         const op = ((i >> 5) & 0x3) << 4 | (i & 0xF);
-                        break :blk control(InstrFn, I, op);
+                        break :blk controlExt(InstrFn, I, op);
                     } else blk: {
                         const I = i >> 9 & 1 == 1;
                         const S = i >> 4 & 1 == 1;
-                        const instrKind = i >> 5 & 0xF;
-                        break :blk processing(InstrFn, I, S, instrKind);
+                        const instr_kind = i >> 5 & 0xF;
+                        break :blk processing(InstrFn, I, S, instr_kind);
                     },
                     0b01 => if (i >> 9 & 1 == 1 and i & 1 == 1) und else blk: {
                         const I = i >> 9 & 1 == 1;

src/lib.zig

@@ -32,6 +32,38 @@ pub const Interpreter = union(enum) {
     }
 };
 
+test "create ARMv4T interface" {
+    var bus_impl = ExampleBus{};
+    var scheduler_impl = ExampleScheduler{};
+
+    const bus_interface = Bus.init(&bus_impl);
+    const scheduler_interface = Scheduler.init(&scheduler_impl);
+
+    var arm7tdmi = Arm7tdmi.init(scheduler_interface, bus_interface);
+    var icpu = arm7tdmi.interface();
+
+    icpu.reset();
+    icpu.step();
+    // TODO: call icpu.panic()
+}
+
+test "create ARMv5TE interface" {
+    var bus_impl = ExampleBus{};
+    var scheduler_impl = ExampleScheduler{};
+    var cop_impl = ExampleCoprocessor{};
+
+    const bus_interface = Bus.init(&bus_impl);
+    const scheduler_interface = Scheduler.init(&scheduler_impl);
+    const coprocessor_interface = Coprocessor.init(&cop_impl);
+
+    var arm946es = Arm946es.init(scheduler_interface, bus_interface, coprocessor_interface);
+    var icpu = arm946es.interface();
+
+    icpu.reset();
+    icpu.step();
+    // TODO: call icpu.panic();
+}
+
 pub const Bus = struct {
     ptr: *anyopaque,
     vtable: *const Vtable,
@@ -196,6 +228,118 @@ pub const Bus = struct {
     }
 };
 
+test "create Bus" {
+    var bus_impl = ExampleBus{};
+    const iface = Bus.init(&bus_impl);
+    _ = iface;
+}
+
+test "call Bus reads" {
+    var bus_impl = ExampleBus{};
+    const iface = Bus.init(&bus_impl);
+
+    _ = iface.read(u32, 0x0000_0000);
+    _ = iface.read(u16, 0x0000_0000);
+    _ = iface.read(u8, 0x0000_0000);
+
+    _ = iface.dbgRead(u32, 0x0000_0000);
+    _ = iface.dbgRead(u16, 0x0000_0000);
+    _ = iface.dbgRead(u8, 0x0000_0000);
+}
+
+test "call Bus writes" {
+    var bus_impl = ExampleBus{};
+    const iface = Bus.init(&bus_impl);
+
+    _ = iface.write(u32, 0x0000_0000, 0x0000_0000);
+    _ = iface.write(u16, 0x0000_0000, 0x0000);
+    _ = iface.write(u8, 0x0000_0000, 0x00);
+
+    _ = iface.dbgWrite(u32, 0x0000_0000, 0x0000_0000);
+    _ = iface.dbgWrite(u16, 0x0000_0000, 0x0000);
+    _ = iface.dbgWrite(u8, 0x0000_0000, 0x00);
+}
+
+pub const Coprocessor = struct {
+    ptr: *anyopaque,
+
+    // VTable
+    readFn: *const fn (ptr: *anyopaque, op1: u3, cn: u4, cm: u4, op2: u3) u32,
+    writeFn: *const fn (ptr: *anyopaque, op1: u3, cn: u4, cm: u4, op2: u3, value: u32) void,
+    resetFn: *const fn (ptr: *anyopaque) void,
+
+    pub fn init(obj: anytype) @This() {
+        const P = @TypeOf(obj);
+        const info = @typeInfo(P);
+
+        std.debug.assert(info == .Pointer); // `anytype` is a Pointer
+        std.debug.assert(info.Pointer.size == .One); // Single-Item Pointer
+        std.debug.assert(@typeInfo(info.Pointer.child) == .Struct); // Pointer Child is a `struct`
+
+        const impl = struct {
+            fn read(ptr: *anyopaque, op1: u3, cn: u4, cm: u4, op2: u3) u32 {
+                const self: P = @ptrCast(@alignCast(ptr));
+                return self.read(op1, cn, cm, op2);
+            }
+
+            fn write(ptr: *anyopaque, op1: u3, cn: u4, cm: u4, op2: u3, value: u32) void {
+                const self: P = @ptrCast(@alignCast(ptr));
+                return self.write(op1, cn, cm, op2, value);
+            }
+
+            fn reset(ptr: *anyopaque) void {
+                const self: P = @ptrCast(@alignCast(ptr));
+                return self.reset();
+            }
+        };
+
+        return .{
+            .ptr = obj,
+            .readFn = impl.read,
+            .writeFn = impl.write,
+            .resetFn = impl.reset,
+        };
+    }
+
+    pub fn read(self: @This(), op1: u3, cn: u4, cm: u4, op2: u3) u32 {
+        return self.readFn(self.ptr, op1, cn, cm, op2);
+    }
+
+    pub fn write(self: @This(), op1: u3, cn: u4, cm: u4, op2: u3, value: u32) void {
+        return self.writeFn(self.ptr, op1, cn, cm, op2, value);
+    }
+
+    pub fn reset(self: @This()) void {
+        return self.resetFn(self.ptr);
+    }
+};
+
+test "create Coprocessor" {
+    var cop_impl = ExampleCoprocessor{};
+    _ = Coprocessor.init(&cop_impl);
+}
+
+test "Coprocessor.read" {
+    var cop_impl = ExampleCoprocessor{};
+    const iface = Coprocessor.init(&cop_impl);
+
+    try testing.expectEqual(@as(u32, 0xDEADBEEF), iface.read(0, 0, 0, 0));
+}
+
+test "Coprocessor.write" {
+    var cop_impl = ExampleCoprocessor{};
+    const iface = Coprocessor.init(&cop_impl);
+
+    iface.write(0, 0, 0, 0, 0xDEADBEEF);
+}
+
+test "Coprocessor.reset" {
+    var cop_impl = ExampleCoprocessor{};
+    const iface = Coprocessor.init(&cop_impl);
+
+    iface.reset();
+}
+
 pub const Scheduler = struct {
     ptr: *anyopaque,
 
@@ -235,6 +379,27 @@ pub const Scheduler = struct {
     }
 };
 
+test "create Scheduler" {
+    var scheduler_impl = ExampleScheduler{};
+    const iface = Scheduler.init(&scheduler_impl);
+    _ = iface;
+}
+
+test "Scheduler.now()" {
+    var scheduler_impl = ExampleScheduler{};
+    const iface = Scheduler.init(&scheduler_impl);
+
+    try testing.expectEqual(@as(u64, 0), iface.now());
+}
+
+test "Scheduler.reset()" {
+    var scheduler_impl = ExampleScheduler{ .tick = std.math.maxInt(u64) };
+    const iface = Scheduler.init(&scheduler_impl);
+    iface.reset();
+
+    try testing.expectEqual(@as(u64, 0), scheduler_impl.tick);
+}
+
 // ---
 // TESTING
 // ---
@@ -282,61 +447,27 @@ const ExampleScheduler = struct {
     }
 };
 
-test "create IBus" {
-    var bus_impl = ExampleBus{};
-    const iface = Bus.init(&bus_impl);
-    _ = iface;
-}
+const ExampleCoprocessor = struct {
+    pub fn read(self: *@This(), op1: u3, cn: u4, cm: u4, op2: u3) u32 {
+        _ = op2;
+        _ = cm;
+        _ = cn;
+        _ = op1;
+        _ = self;
 
-test "call IBus reads" {
-    var bus_impl = ExampleBus{};
-    const iface = Bus.init(&bus_impl);
+        return 0xDEADBEEF;
+    }
 
-    _ = iface.read(u32, 0x0000_0000);
-    _ = iface.read(u16, 0x0000_0000);
-    _ = iface.read(u8, 0x0000_0000);
+    pub fn write(self: *@This(), op1: u3, cn: u4, cm: u4, op2: u3, value: u32) void {
+        _ = value;
+        _ = op2;
+        _ = cm;
+        _ = cn;
+        _ = op1;
+        _ = self;
+    }
 
-    _ = iface.dbgRead(u32, 0x0000_0000);
-    _ = iface.dbgRead(u16, 0x0000_0000);
-    _ = iface.dbgRead(u8, 0x0000_0000);
-}
-
-test "call IBus writes" {
-    var bus_impl = ExampleBus{};
-    const iface = Bus.init(&bus_impl);
-
-    _ = iface.write(u32, 0x0000_0000, 0x0000_0000);
-    _ = iface.write(u16, 0x0000_0000, 0x0000);
-    _ = iface.write(u8, 0x0000_0000, 0x00);
-
-    _ = iface.dbgWrite(u32, 0x0000_0000, 0x0000_0000);
-    _ = iface.dbgWrite(u16, 0x0000_0000, 0x0000);
-    _ = iface.dbgWrite(u8, 0x0000_0000, 0x00);
-}
-
-test "create ARMv4T interface" {
-    var bus_impl = ExampleBus{};
-    var scheduler_impl = ExampleScheduler{};
-
-    const bus_interface = Bus.init(&bus_impl);
-    const scheduler_interface = Scheduler.init(&scheduler_impl);
-
-    var arm7tdmi = Arm7tdmi.init(scheduler_interface, bus_interface);
-    var icpu = arm7tdmi.interface();
-
-    icpu.reset();
-    icpu.step();
-    // TODO: call icpu.panic()
-}
-
-test "create ARMv5TE interface" {
-    var bus_impl = ExampleBus{};
-    var scheduler_impl = ExampleScheduler{};
-
-    const bus_interface = Bus.init(&bus_impl);
-    const scheduler_interface = Scheduler.init(&scheduler_impl);
-
-    var arm946es = Arm946es.init(scheduler_interface, bus_interface);
-
-    _ = arm946es.interface();
-}
+    pub fn reset(self: *@This()) void {
+        self.* = .{};
+    }
+};