feat(cpu): implement branch and exchange

If I want to continue with armwrestler, I'll have to implement
THUMB instructions now

src/Bus.zig

@@ -69,7 +69,7 @@ pub fn write32(self: *Self, addr: u32, word: u32) void {
         // General Internal Memory
         0x0200_0000...0x0203_FFFF => self.iwram.set32(addr - 0x0200_0000, word),
         0x0300_0000...0x0300_7FFF => self.ewram.set32(addr - 0x0300_0000, word),
-        0x0400_0000...0x0400_03FE => std.debug.panic("[Bus:32] wrote 0x{X:} to 0x{X:} in I/O", .{ word, addr }),
+        0x0400_0000...0x0400_03FE => self.io.write32(addr, word),
 
         // Internal Display Memory
         0x0500_0000...0x0500_03FF => self.ppu.palette.set32(@as(usize, addr - 0x0500_0000), word),
@@ -153,7 +153,7 @@ pub fn write8(self: *Self, addr: u32, byte: u8) void {
         // General Internal Memory
         0x0200_0000...0x0203_FFFF => self.iwram.set8(addr - 0x0200_0000, byte),
         0x0300_0000...0x0300_7FFF => self.ewram.set8(addr - 0x0300_0000, byte),
-        0x0400_0000...0x0400_03FE => self.io.set8(addr - 0x0400_0000, byte),
+        0x0400_0000...0x0400_03FE => self.io.write8(addr, byte),
 
         // External Memory (Game Pak)
         0x0E00_0000...0x0E00_FFFF => std.debug.panic("[Bus:8] write 0x{X:} to 0x{X:} in Game Pak SRAM", .{ byte, addr }),

src/bus/io.zig

@@ -6,15 +6,20 @@ const Bitfield = @import("bitfield").Bitfield;
 pub const Io = struct {
     const Self = @This();
 
-    dispcnt: DispCnt,
+    dispcnt: DisplayControl,
-    dispstat: DispStat,
+    dispstat: DisplayStatus,
     vcount: VCount,
+    /// Read / Write
+    ime: bool,
+    ie: InterruptEnable,
 
     pub fn init() Self {
         return .{
             .dispcnt = .{ .raw = 0x0000_0000 },
             .dispstat = .{ .raw = 0x0000_0000 },
             .vcount = .{ .raw = 0x0000_0000 },
+            .ime = false,
+            .ie = .{ .raw = 0x0000_0000 },
         };
     }
 
@@ -23,15 +28,27 @@ pub const Io = struct {
             0x0400_0000 => @as(u32, self.dispcnt.raw),
             0x0400_0004 => @as(u32, self.dispstat.raw),
             0x0400_0006 => @as(u32, self.vcount.raw),
+            0x0400_0200 => @as(u32, self.ie.raw),
+            0x0400_0208 => @boolToInt(self.ime),
             else => std.debug.panic("[I/O:32] tried to read from {X:}", .{addr}),
         };
     }
 
+    pub fn write32(self: *Self, addr: u32, word: u32) void {
+        switch (addr) {
+            0x0400_0200 => self.ie.raw = @truncate(u16, word),
+            0x0400_0208 => self.ime = word & 1 == 1,
+            else => std.debug.panic("[I/O:32] tried to write 0x{X:} to 0x{X:}", .{ word, addr }),
+        }
+    }
+
     pub fn read16(self: *const Self, addr: u32) u16 {
         return switch (addr) {
             0x0400_0000 => self.dispcnt.raw,
             0x0400_0004 => self.dispstat.raw,
             0x0400_0006 => self.vcount.raw,
+            0x0400_0200 => self.ie.raw,
+            0x0400_0208 => @boolToInt(self.ime),
             else => std.debug.panic("[I/O:16] tried to read from {X:}", .{addr}),
         };
     }
@@ -40,6 +57,8 @@ pub const Io = struct {
         switch (addr) {
             0x0400_0000 => self.dispcnt.raw = halfword,
             0x0400_0004 => self.dispstat.raw = halfword,
+            0x0400_0200 => self.ie.raw = halfword,
+            0x0400_0208 => self.ime = halfword & 1 == 1,
             else => std.debug.panic("[I/O:16] tried to write 0x{X:} to 0x{X:}", .{ halfword, addr }),
         }
     }
@@ -48,17 +67,19 @@ pub const Io = struct {
         return switch (addr) {
             0x0400_0000 => @truncate(u8, self.dispcnt.raw),
             0x0400_0004 => @truncate(u8, self.dispstat.raw),
+            0x0400_0200 => @truncate(u8, self.ie.raw),
             0x0400_0006 => @truncate(u8, self.vcount.raw),
             else => std.debug.panic("[I/O:8] tried to read from {X:}", .{addr}),
         };
     }
 
-    pub fn set8(_: *Self, addr: u32, byte: u8) void {
+    pub fn write8(_: *Self, addr: u32, byte: u8) void {
         std.debug.panic("[I/0:8] tried to write 0x{X:} to 0x{X:}", .{ byte, addr });
     }
 };
 
-const DispCnt = extern union {
+/// Read / Write
+const DisplayControl = extern union {
     bg_mode: Bitfield(u16, 0, 3),
     frame_select: Bit(u16, 4),
     hblank_interval_free: Bit(u16, 5),
@@ -71,7 +92,8 @@ const DispCnt = extern union {
     raw: u16,
 };
 
-const DispStat = extern union {
+/// Read / Write
+const DisplayStatus = extern union {
     vblank: Bit(u16, 0),
     hblank: Bit(u16, 1),
     coincidence: Bit(u16, 2),
@@ -82,7 +104,27 @@ const DispStat = extern union {
     raw: u16,
 };
 
+/// Read Only
 const VCount = extern union {
     scanline: Bitfield(u16, 0, 8),
     raw: u16,
 };
+
+/// Read / Write
+const InterruptEnable = extern union {
+    vblank: Bit(u16, 0),
+    hblank: Bit(u16, 1),
+    coincidence: Bit(u16, 2),
+    tm0_overflow: Bit(u16, 3),
+    tm1_overflow: Bit(u16, 4),
+    tm2_overflow: Bit(u16, 5),
+    tm3_overflow: Bit(u16, 6),
+    serial: Bit(u16, 7),
+    dma0: Bit(u16, 8),
+    dma1: Bit(u16, 9),
+    dma2: Bit(u16, 10),
+    dma3: Bit(u16, 11),
+    keypad: Bit(u16, 12),
+    game_pak: Bit(u16, 13),
+    raw: u16,
+};

src/cpu.zig

@@ -8,10 +8,12 @@ const Bitfield = @import("bitfield").Bitfield;
 const Scheduler = @import("scheduler.zig").Scheduler;
 
 const dataProcessing = @import("cpu/data_processing.zig").dataProcessing;
+const psrTransfer = @import("cpu/psr_transfer.zig").psrTransfer;
 const singleDataTransfer = @import("cpu/single_data_transfer.zig").singleDataTransfer;
 const halfAndSignedDataTransfer = @import("cpu/half_signed_data_transfer.zig").halfAndSignedDataTransfer;
 const blockDataTransfer = @import("cpu/block_data_transfer.zig").blockDataTransfer;
 const branch = @import("cpu/branch.zig").branch;
+const branchAndExchange = @import("cpu/branch.zig").branchAndExchange;
 
 pub const InstrFn = fn (*Arm7tdmi, *Bus, u32) void;
 const arm_lut: [0x1000]InstrFn = populate();
@@ -22,7 +24,7 @@ pub const Arm7tdmi = struct {
     r: [16]u32,
     sched: *Scheduler,
     bus: *Bus,
-    cpsr: CPSR,
+    cpsr: PSR,
 
     pub fn init(sched: *Scheduler, bus: *Bus) Self {
         return .{
@@ -56,7 +58,7 @@ pub const Arm7tdmi = struct {
 
     fn fetch(self: *Self) u32 {
         const word = self.bus.read32(self.r[15]);
-        self.r[15] += 4;
+        self.r[15] += if (self.cpsr.t.read()) @as(u32, 2) else @as(u32, 4);
         return word;
     }
 
@@ -96,7 +98,7 @@ fn armIdx(opcode: u32) u12 {
     return @truncate(u12, opcode >> 20 & 0xFF) << 4 | @truncate(u12, opcode >> 4 & 0xF);
 }
 
-fn checkCond(cpsr: *const CPSR, opcode: u32) bool {
+fn checkCond(cpsr: *const PSR, opcode: u32) bool {
     // TODO: Should I implement an enum?
     return switch (@truncate(u4, opcode >> 28)) {
         0x0 => cpsr.z.read(), // EQ - Equal
@@ -133,6 +135,18 @@ fn populate() [0x1000]InstrFn {
                 lut[i] = dataProcessing(I, S, instrKind);
             }
 
+            if (i >> 10 & 0x3 == 0b00 and i >> 7 & 0x3 == 0b10 and i >> 4 & 1 == 0) {
+                // PSR Transfer
+                const I = i >> 9 & 1 == 1;
+                const isSpsr = i >> 6 & 1 == 1;
+
+                lut[i] = psrTransfer(I, isSpsr);
+            }
+
+            if (i == 0x121) {
+                lut[i] = branchAndExchange;
+            }
+
             if (i >> 9 & 0x7 == 0b000 and i >> 3 & 1 == 1 and i & 1 == 1) {
                 const P = i >> 8 & 1 == 1;
                 const U = i >> 7 & 1 == 1;
@@ -174,7 +188,7 @@ fn populate() [0x1000]InstrFn {
     };
 }
 
-pub const CPSR = extern union {
+pub const PSR = extern union {
     mode: Bitfield(u32, 0, 5),
     t: Bit(u32, 5),
     f: Bit(u32, 6),

src/cpu/barrel_shifter.zig

@@ -1,7 +1,7 @@
 const std = @import("std");
 
 const Arm7tdmi = @import("../cpu.zig").Arm7tdmi;
-const CPSR = @import("../cpu.zig").CPSR;
+const CPSR = @import("../cpu.zig").PSR;
 
 pub fn exec(comptime S: bool, cpu: *Arm7tdmi, opcode: u32) u32 {
     var shift_amt: u8 = undefined;
@@ -12,21 +12,27 @@ pub fn exec(comptime S: bool, cpu: *Arm7tdmi, opcode: u32) u32 {
     }
 
     const rm = cpu.r[opcode & 0xF];
+    var value: u32 = undefined;
+    if (rm == 0xF) {
+        value = cpu.fakePC() + 4; // 12 ahead
+    } else {
+        value = cpu.r[opcode & 0xF];
+    }
 
     if (S) {
         return switch (@truncate(u2, opcode >> 5)) {
-            0b00 => logical_left(&cpu.cpsr, rm, shift_amt),
+            0b00 => logical_left(&cpu.cpsr, value, shift_amt),
-            0b01 => logical_right(&cpu.cpsr, rm, shift_amt),
+            0b01 => logical_right(&cpu.cpsr, value, shift_amt),
-            0b10 => arithmetic_right(&cpu.cpsr, rm, shift_amt),
+            0b10 => arithmetic_right(&cpu.cpsr, value, shift_amt),
-            0b11 => rotate_right(&cpu.cpsr, rm, 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, rm, shift_amt),
+            0b00 => logical_left(&dummy, value, shift_amt),
-            0b01 => logical_right(&dummy, rm, shift_amt),
+            0b01 => logical_right(&dummy, value, shift_amt),
-            0b10 => arithmetic_right(&dummy, rm, shift_amt),
+            0b10 => arithmetic_right(&dummy, value, shift_amt),
-            0b11 => rotate_right(&dummy, rm, shift_amt),
+            0b11 => rotate_right(&dummy, value, shift_amt),
         };
     }
 }

src/cpu/branch.zig

@@ -1,3 +1,4 @@
+const std = @import("std");
 const util = @import("../util.zig");
 
 const Bus = @import("../Bus.zig");
@@ -16,3 +17,11 @@ pub fn branch(comptime L: bool) InstrFn {
         }
     }.inner;
 }
+
+pub fn branchAndExchange(cpu: *Arm7tdmi, _: *Bus, opcode: u32) void {
+    const rn = opcode & 0xF;
+    cpu.cpsr.t.write(cpu.r[rn] & 1 == 1);
+
+    // TODO: Is this how I should do it?
+    cpu.r[15] = cpu.r[rn] & 0xFFFF_FFFE;
+}

src/cpu/data_processing.zig

@@ -9,24 +9,29 @@ pub fn dataProcessing(comptime I: bool, comptime S: bool, comptime instrKind: u4
     return struct {
         fn inner(cpu: *Arm7tdmi, _: *Bus, opcode: u32) void {
             const rd = opcode >> 12 & 0xF;
-            const op1 = opcode >> 16 & 0xF;
+            const rn = opcode >> 16 & 0xF;
+
+            if (S and rd == 0xF) std.debug.panic("[CPU] Data Processing Instruction w/ S set and Rd == 15", .{});
+
+            var op1: u32 = undefined;
+            if (rn == 0xF) {
+                op1 = cpu.fakePC();
+            } else {
+                op1 = cpu.r[rn];
+            }
 
             var op2: u32 = undefined;
             if (I) {
                 op2 = std.math.rotr(u32, opcode & 0xFF, (opcode >> 8 & 0xF) << 1);
             } else {
-                if (S and rd == 0xF) {
+                op2 = BarrelShifter.exec(S, cpu, opcode);
-                    std.debug.panic("[CPU] Data Processing Instruction w/ S set and Rd == 15", .{});
-                } else {
-                    op2 = BarrelShifter.exec(S, cpu, opcode);
-                }
             }
 
             switch (instrKind) {
                 0x4 => {
                     // ADD
                     var result: u32 = undefined;
-                    const didOverflow = @addWithOverflow(u32, cpu.r[op1], op2, &result);
+                    const didOverflow = @addWithOverflow(u32, op1, op2, &result);
                     cpu.r[rd] = result;
 
                     if (S and rd != 0xF) {
@@ -38,7 +43,7 @@ pub fn dataProcessing(comptime I: bool, comptime S: bool, comptime instrKind: u4
                 },
                 0x8 => {
                     // TST
-                    const result = cpu.r[op1] & op2;
+                    const result = op1 & op2;
 
                     cpu.cpsr.n.write(result >> 31 & 1 == 1);
                     cpu.cpsr.z.write(result == 0);
@@ -47,7 +52,7 @@ pub fn dataProcessing(comptime I: bool, comptime S: bool, comptime instrKind: u4
                 },
                 0x9 => {
                     // TEQ
-                    const result = cpu.r[op1] ^ op2;
+                    const result = op1 ^ op2;
 
                     cpu.cpsr.n.write(result >> 31 & 1 == 1);
                     cpu.cpsr.z.write(result == 0);
@@ -66,16 +71,16 @@ pub fn dataProcessing(comptime I: bool, comptime S: bool, comptime instrKind: u4
                 },
                 0xA => {
                     // CMP
-                    const result = cpu.r[op1] -% op2;
+                    const result = op1 -% op2;
 
                     cpu.cpsr.n.write(result >> 31 & 1 == 1);
                     cpu.cpsr.z.write(result == 0);
-                    cpu.cpsr.c.write(op2 <= cpu.r[op1]);
+                    cpu.cpsr.c.write(op2 <= op1);
                     cpu.cpsr.v.write(((op1 ^ result) & (~op2 ^ result)) >> 31 & 1 == 1);
                 },
                 0xC => {
                     // ORR
-                    const result = cpu.r[op1] | op2;
+                    const result = op1 | op2;
                     cpu.r[rd] = result;
 
                     if (S and rd != 0xF) {

src/cpu/half_signed_data_transfer.zig

@@ -13,7 +13,13 @@ pub fn halfAndSignedDataTransfer(comptime P: bool, comptime U: bool, comptime I:
             const rm = opcode & 0xF;
             const imm_offset_high = opcode >> 8 & 0xF;
 
-            const base = cpu.r[rn];
+            var base: u32 = undefined;
+            if (rn == 0xF) {
+                base = cpu.fakePC();
+                if (!L) base += 4;
+            } else {
+                base = cpu.r[rn];
+            }
 
             var offset: u32 = undefined;
             if (I) {
@@ -33,7 +39,8 @@ pub fn halfAndSignedDataTransfer(comptime P: bool, comptime U: bool, comptime I:
                     },
                     0b01 => {
                         // LDRH
-                        cpu.r[rd] = bus.read16(address);
+                        const value = bus.read16(address & 0xFFFE);
+                        cpu.r[rd] = std.math.rotr(u32, @as(u32, value), 8 * (address & 1));
                     },
                     0b10 => {
                         // LDRSB

src/cpu/psr_transfer.zig

@@ -0,0 +1,53 @@
+const std = @import("std");
+
+const Bus = @import("../Bus.zig");
+const Arm7tdmi = @import("../cpu.zig").Arm7tdmi;
+const InstrFn = @import("../cpu.zig").InstrFn;
+
+pub fn psrTransfer(comptime I: bool, comptime isSpsr: bool) InstrFn {
+    return struct {
+        fn inner(cpu: *Arm7tdmi, _: *Bus, opcode: u32) void {
+            switch (@truncate(u3, opcode >> 19)) {
+                0b001 => {
+                    // MRS
+                    const rn = opcode >> 12 & 0xF;
+
+                    if (isSpsr) {
+                        std.debug.panic("[CPU] TODO: MRS on SPSR_<current_mode> is unimplemented", .{});
+                    } else {
+                        cpu.r[rn] = cpu.cpsr.raw;
+                    }
+                },
+                0b101 => {
+                    // MSR
+                    const rm = opcode & 0xF;
+
+                    switch (@truncate(u3, opcode >> 16)) {
+                        0b000 => {
+                            const right = if (I) std.math.rotr(u32, opcode & 0xFF, opcode >> 8 & 0xF) else cpu.r[rm];
+
+                            if (isSpsr) {
+                                std.debug.panic("[CPU] TODO: MSR (flags only) on SPSR_<current_mode> is unimplemented", .{});
+                            } else {
+                                cpu.cpsr.n.write(right >> 31 & 1 == 1);
+                                cpu.cpsr.z.write(right >> 30 & 1 == 1);
+                                cpu.cpsr.c.write(right >> 29 & 1 == 1);
+                                cpu.cpsr.v.write(right >> 28 & 1 == 1);
+                            }
+                        },
+                        0b001 => {
+                            if (isSpsr) {
+                                std.debug.panic("[CPU] TODO: MSR on SPSR_<current_mode> is unimplemented", .{});
+                            } else {
+                                cpu.cpsr = .{ .raw = cpu.r[rm] };
+                            }
+                        },
+
+                        else => unreachable,
+                    }
+                },
+                else => unreachable,
+            }
+        }
+    }.inner;
+}

src/cpu/single_data_transfer.zig

@@ -4,7 +4,7 @@ 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").CPSR;
+const CPSR = @import("../cpu.zig").PSR;
 const InstrFn = @import("../cpu.zig").InstrFn;
 
 pub fn singleDataTransfer(comptime I: bool, comptime P: bool, comptime U: bool, comptime B: bool, comptime W: bool, comptime L: bool) InstrFn {
@@ -13,7 +13,14 @@ pub fn singleDataTransfer(comptime I: bool, comptime P: bool, comptime U: bool,
             const rn = opcode >> 16 & 0xF;
             const rd = opcode >> 12 & 0xF;
 
-            const base = cpu.r[rn];
+            var base: u32 = undefined;
+            if (rn == 0xF) {
+                base = cpu.fakePC();
+                if (!L) base += 4; // Offset of 12
+            } else {
+                base = cpu.r[rn];
+            }
+
             const offset = if (I) registerOffset(cpu, opcode) else opcode & 0xFFF;
 
             const modified_base = if (U) base + offset else base - offset;
@@ -25,9 +32,8 @@ pub fn singleDataTransfer(comptime I: bool, comptime P: bool, comptime U: bool,
                     cpu.r[rd] = bus.read8(address);
                 } else {
                     // LDR
-
+                    const value = bus.read32(address & 0xFFFF_FFFC);
-                    // FIXME: Unsure about how I calculate the boundary offset
+                    cpu.r[rd] = std.math.rotr(u32, value, 8 * (address & 0x3));
-                    cpu.r[rd] = std.math.rotl(u32, bus.read32(address), address % 4);
                 }
             } else {
                 if (B) {