From f7680cd82483d1bfd9b8c2b0df9f09a20dbdca82 Mon Sep 17 00:00:00 2001
From: Rekai Musuka <rekai@musuka.dev>
Date: Wed, 12 Jan 2022 07:14:41 -0400
Subject: [PATCH] feat(cpu): implement branch and exchange

If I want to continue with armwrestler, I'll have to implement
THUMB instructions now
---
 src/cpu.zig        | 7 ++++++-
 src/cpu/branch.zig | 9 +++++++++
 2 files changed, 15 insertions(+), 1 deletion(-)

diff --git a/src/cpu.zig b/src/cpu.zig
index 7715ce7..3446ad3 100644
--- a/src/cpu.zig
+++ b/src/cpu.zig
@@ -13,6 +13,7 @@ const singleDataTransfer = @import("cpu/single_data_transfer.zig").singleDataTra
 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();
@@ -57,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;
     }
 
@@ -142,6 +143,10 @@ fn populate() [0x1000]InstrFn {
                 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;
diff --git a/src/cpu/branch.zig b/src/cpu/branch.zig
index fe91c18..01d2d9d 100644
--- a/src/cpu/branch.zig
+++ b/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;
+}