From 99b686b2d72c97208dcdf73916bed789dde194eb Mon Sep 17 00:00:00 2001
From: Rekai Musuka <rekai@musuka.dev>
Date: Fri, 21 Oct 2022 05:11:58 -0300
Subject: [PATCH] fix(cpu): use barrel shifter in data processing immediates

---
 src/cpu/arm/barrel_shifter.zig       | 27 ++++++++++++++++-----------
 src/cpu/arm/data_processing.zig      | 13 ++++++++++++-
 src/cpu/arm/single_data_transfer.zig |  6 +++---
 3 files changed, 31 insertions(+), 15 deletions(-)

diff --git a/src/cpu/arm/barrel_shifter.zig b/src/cpu/arm/barrel_shifter.zig
index 0f1fb76..29eee16 100644
--- a/src/cpu/arm/barrel_shifter.zig
+++ b/src/cpu/arm/barrel_shifter.zig
@@ -21,23 +21,23 @@ pub fn exec(comptime S: bool, cpu: *Arm7tdmi, opcode: u32) u32 {
 
     if (S) {
         return switch (@truncate(u2, opcode >> 5)) {
-            0b00 => logical_left(&cpu.cpsr, value, shift_amt),
-            0b01 => logical_right(&cpu.cpsr, value, shift_amt),
+            0b00 => logicalLeft(&cpu.cpsr, value, shift_amt),
+            0b01 => logicalRight(&cpu.cpsr, value, shift_amt),
             0b10 => arithmetic_right(&cpu.cpsr, value, shift_amt),
-            0b11 => rotate_right(&cpu.cpsr, value, shift_amt),
+            0b11 => rotateRight(&cpu.cpsr, value, shift_amt),
         };
     } else {
         var dummy = CPSR{ .raw = 0x0000_0000 };
         return switch (@truncate(u2, opcode >> 5)) {
-            0b00 => logical_left(&dummy, value, shift_amt),
-            0b01 => logical_right(&dummy, value, shift_amt),
+            0b00 => logicalLeft(&dummy, value, shift_amt),
+            0b01 => logicalRight(&dummy, value, shift_amt),
             0b10 => arithmetic_right(&dummy, value, shift_amt),
-            0b11 => rotate_right(&dummy, value, shift_amt),
+            0b11 => rotateRight(&dummy, value, shift_amt),
         };
     }
 }
 
-pub fn logical_left(cpsr: *CPSR, rm: u32, shift_byte: u8) u32 {
+pub fn logicalLeft(cpsr: *CPSR, rm: u32, shift_byte: u8) u32 {
     const shift_amt = @truncate(u5, shift_byte);
     const bit_count: u8 = @typeInfo(u32).Int.bits;
 
@@ -64,7 +64,7 @@ pub fn logical_left(cpsr: *CPSR, rm: u32, shift_byte: u8) u32 {
     return result;
 }
 
-pub fn logical_right(cpsr: *CPSR, rm: u32, shift_byte: u8) u32 {
+pub fn logicalRight(cpsr: *CPSR, rm: u32, shift_byte: u8) u32 {
     const shift_amt = @truncate(u5, shift_byte);
     const bit_count: u8 = @typeInfo(u32).Int.bits;
 
@@ -92,7 +92,12 @@ pub fn arithmetic_right(_: *CPSR, _: u32, _: u8) u32 {
     std.debug.panic("[BarrelShifter] implement arithmetic shift right", .{});
 }
 
-pub fn rotate_right(_: *CPSR, _: u32, _: u8) u32 {
-    // std.math.rotr(u32, r_val, amount)
-    std.debug.panic("[BarrelShifter] implement rotate right", .{});
+pub fn rotateRight(cpsr: *CPSR, rm: u32, shift_byte: u8) u32 {
+    const result = std.math.rotr(u32, rm, shift_byte);
+
+    if (result != 0) {
+        cpsr.c.write(result >> 31 & 1 == 1);
+    }
+
+    return result;
 }
diff --git a/src/cpu/arm/data_processing.zig b/src/cpu/arm/data_processing.zig
index 1a2a4b3..f94db67 100644
--- a/src/cpu/arm/data_processing.zig
+++ b/src/cpu/arm/data_processing.zig
@@ -22,7 +22,18 @@ pub fn dataProcessing(comptime I: bool, comptime S: bool, comptime instrKind: u4
 
             var op2: u32 = undefined;
             if (I) {
-                op2 = std.math.rotr(u32, opcode & 0xFF, (opcode >> 8 & 0xF) << 1);
+                const amt = @truncate(u8, (opcode >> 8 & 0xF) << 1);
+
+                if (S) {
+                    op2 = BarrelShifter.rotateRight(&cpu.cpsr, opcode & 0xFF, amt);
+                } else {
+                    const PSR = @import("../../cpu.zig").PSR;
+                    var dummy = PSR{ .raw = 0x0000_0000 };
+
+                    op2 = BarrelShifter.rotateRight(&dummy, opcode & 0xFF, amt);
+                }
+
+                // op2 = std.math.rotr(u32, opcode & 0xFF, (opcode >> 8 & 0xF) << 1);
             } else {
                 op2 = BarrelShifter.exec(S, cpu, opcode);
             }
diff --git a/src/cpu/arm/single_data_transfer.zig b/src/cpu/arm/single_data_transfer.zig
index 09d9662..4b9bcb2 100644
--- a/src/cpu/arm/single_data_transfer.zig
+++ b/src/cpu/arm/single_data_transfer.zig
@@ -62,9 +62,9 @@ fn registerOffset(cpu: *Arm7tdmi, opcode: u32) u32 {
     var dummy = CPSR{ .raw = 0x0000_0000 };
 
     return switch (@truncate(u2, opcode >> 5)) {
-        0b00 => BarrelShifter.logical_left(&dummy, rm, shift_byte),
-        0b01 => BarrelShifter.logical_right(&dummy, rm, shift_byte),
+        0b00 => BarrelShifter.logicalLeft(&dummy, rm, shift_byte),
+        0b01 => BarrelShifter.logicalRight(&dummy, rm, shift_byte),
         0b10 => BarrelShifter.arithmetic_right(&dummy, rm, shift_byte),
-        0b11 => BarrelShifter.rotate_right(&dummy, rm, shift_byte),
+        0b11 => BarrelShifter.rotateRight(&dummy, rm, shift_byte),
     };
 }