From c47ef37ba5fe2e9dad1a9a1b9afa02143c9243ae Mon Sep 17 00:00:00 2001
From: Rekai Musuka <rekai@musuka.dev>
Date: Wed, 29 Jun 2022 06:33:17 -0300
Subject: [PATCH] chore: add non-working pipeline

---
 src/cpu.zig            | 37 ++++++++++++++++++++++++++++++-------
 src/cpu/arm/branch.zig | 18 ++++++++++++++----
 src/main.zig           |  4 ++--
 3 files changed, 46 insertions(+), 13 deletions(-)

diff --git a/src/cpu.zig b/src/cpu.zig
index ab4f200..e56f556 100644
--- a/src/cpu.zig
+++ b/src/cpu.zig
@@ -60,6 +60,7 @@ pub const Arm7tdmi = struct {
     const Self = @This();
 
     r: [16]u32,
+    pipe: [2]u32,
     sched: *Scheduler,
     bus: Bus,
     cpsr: PSR,
@@ -82,6 +83,7 @@ pub const Arm7tdmi = struct {
     pub fn init(alloc: Allocator, sched: *Scheduler, paths: FilePaths) !Self {
         return Self{
             .r = [_]u32{0x00} ** 16,
+            .pipe = [_]u32{0xF000_0000} ** 2,
             .sched = sched,
             .bus = try Bus.init(alloc, sched, paths),
             .cpsr = .{ .raw = 0x0000_001F },
@@ -256,14 +258,14 @@ pub const Arm7tdmi = struct {
     }
 
     pub fn step(self: *Self) void {
-        if (self.cpsr.t.read()) {
-            const opcode = self.fetch(u16);
-            if (enable_logging) if (self.log_file) |file| self.debug_log(file, opcode);
+        const opcode = self.pipe[0];
+        if (enable_logging) if (self.log_file) |file| self.debug_log(file, opcode);
 
-            thumb_lut[thumbIdx(opcode)](self, &self.bus, opcode);
+        if (self.cpsr.t.read()) {
+            self.stepPipeline(u16);
+            thumb_lut[thumbIdx(@truncate(u16, opcode))](self, &self.bus, @truncate(u16, opcode));
         } else {
-            const opcode = self.fetch(u32);
-            if (enable_logging) if (self.log_file) |file| self.debug_log(file, opcode);
+            self.stepPipeline(u32);
 
             if (checkCond(self.cpsr, @truncate(u4, opcode >> 28))) {
                 arm_lut[armIdx(opcode)](self, &self.bus, opcode);
@@ -326,6 +328,27 @@ pub const Arm7tdmi = struct {
         }
     }
 
+    inline fn stepPipeline(self: *Self, comptime T: type) void {
+        self.pipe[0] = self.pipe[1]; // Decoded Instr will be executed next
+        self.pipe[1] = self.fetch(T); // Fetch new opcode to be decoded next
+    }
+
+    pub fn reloadPipeline(self: *Self, comptime T: type) void {
+        switch (T) {
+            u32 => {
+                self.pipe[0] = self.bus.read(u32, self.r[15]);
+                self.pipe[1] = self.bus.read(u32, self.r[15] + 4);
+                self.r[15] += 8;
+            },
+            u16 => {
+                self.pipe[0] = self.bus.read(u32, self.r[15]);
+                self.pipe[1] = self.bus.read(u32, self.r[15] + 2);
+                self.r[15] += 4;
+            },
+            else => @compileError("Pipeline Reload: unsupported read width"),
+        }
+    }
+
     inline fn fetch(self: *Self, comptime T: type) T {
         comptime std.debug.assert(T == u32 or T == u16); // Opcode may be 32-bit (ARM) or 16-bit (THUMB)
         defer self.r[15] += if (T == u32) 4 else 2;
@@ -461,7 +484,7 @@ pub const Arm7tdmi = struct {
                 log_str = try std.fmt.bufPrint(&buf, thumb_fmt, .{ r0, r1, r2, r3, r4, r5, r6, r7, r8, r9, r10, r11, r12, r13, r14, r15, c_psr, opcode });
             }
         } else {
-            log_str = try std.fmt.bufPrint(&buf, arm_fmt, .{ r0, r1, r2, r3, r4, r5, r6, r7, r8, r9, r10, r11, r12, r13, r14, r15, c_psr, opcode });
+            log_str = try std.fmt.bufPrint(&buf, arm_fmt, .{ r0, r1, r2, r3, r4, r5, r6, r7, r8, r9, r10, r11, r12, r13, r14, r15 - 4, c_psr, opcode });
         }
 
         _ = try file.writeAll(log_str);
diff --git a/src/cpu/arm/branch.zig b/src/cpu/arm/branch.zig
index 5f9dfe5..fdfbb29 100644
--- a/src/cpu/arm/branch.zig
+++ b/src/cpu/arm/branch.zig
@@ -9,14 +9,24 @@ const sext = @import("../../util.zig").sext;
 pub fn branch(comptime L: bool) InstrFn {
     return struct {
         fn inner(cpu: *Arm7tdmi, _: *Bus, opcode: u32) void {
-            if (L) cpu.r[14] = cpu.r[15];
-            cpu.r[15] = cpu.fakePC() +% (sext(u32, u24, opcode) << 2);
+            if (L) cpu.r[14] = cpu.r[15] - 4;
+
+            cpu.r[15] = cpu.r[15] +% (sext(u32, u24, opcode) << 2);
+            cpu.reloadPipeline(u32);
         }
     }.inner;
 }
 
 pub fn branchAndExchange(cpu: *Arm7tdmi, _: *Bus, opcode: u32) void {
     const rn = opcode & 0xF;
-    cpu.cpsr.t.write(cpu.r[rn] & 1 == 1);
-    cpu.r[15] = cpu.r[rn] & 0xFFFF_FFFE;
+
+    if (cpu.r[rn] & 1 == 1) {
+        cpu.r[15] = cpu.r[rn] & ~@as(u32, 1);
+        cpu.cpsr.t.set();
+        cpu.reloadPipeline(u16);
+    } else {
+        cpu.r[15] = cpu.r[rn] & ~@as(u32, 3);
+        cpu.cpsr.t.unset();
+        cpu.reloadPipeline(u32);
+    }
 }
diff --git a/src/main.zig b/src/main.zig
index e617c6b..3d982be 100644
--- a/src/main.zig
+++ b/src/main.zig
@@ -24,7 +24,7 @@ const expected_rate = @import("emu.zig").frame_rate;
 
 const sample_rate = @import("apu.zig").host_sample_rate;
 
-pub const enable_logging: bool = false;
+pub const enable_logging: bool = true;
 const is_binary: bool = false;
 const log = std.log.scoped(.GUI);
 pub const log_level = if (builtin.mode != .Debug) .info else std.log.default_level;
@@ -72,7 +72,7 @@ pub fn main() anyerror!void {
     var cpu = try Arm7tdmi.init(alloc, &scheduler, paths);
     defer cpu.deinit();
     cpu.bus.attach(&cpu);
-    // cpu.fastBoot(); // Uncomment to skip BIOS
+    cpu.fastBoot(); // Uncomment to skip BIOS
 
     // Copy ROM title while Emulator still belongs to this thread
     const title = cpu.bus.pak.title;