From 1773a3acc8057c94a6931d6e81c09abd62dfbbdb Mon Sep 17 00:00:00 2001
From: Rekai Musuka <rekai@musuka.dev>
Date: Fri, 21 Oct 2022 05:12:11 -0300
Subject: [PATCH] fix(cpu): reimplement THUMB offset shifts

---
 src/cpu/thumb/format1.zig | 29 ++++++++++++++++++++++++++---
 1 file changed, 26 insertions(+), 3 deletions(-)

diff --git a/src/cpu/thumb/format1.zig b/src/cpu/thumb/format1.zig
index f7fb464..db24dcd 100644
--- a/src/cpu/thumb/format1.zig
+++ b/src/cpu/thumb/format1.zig
@@ -14,9 +14,32 @@ pub fn format1(comptime op: u2, comptime offset: u5) InstrFn {
             const rd = opcode & 0x7;
 
             const result = switch (op) {
-                0b00 => shifter.logicalLeft(true, &cpu.cpsr, cpu.r[rs], offset), // LSL
-                0b01 => shifter.logicalRight(true, &cpu.cpsr, cpu.r[rs], offset), // LSR
-                0b10 => shifter.arithmeticRight(true, &cpu.cpsr, cpu.r[rs], offset), // ASR
+                0b00 => blk: {
+                    // LSL
+                    if (offset == 0) {
+                        break :blk cpu.r[rs];
+                    } else {
+                        break :blk shifter.logicalLeft(true, &cpu.cpsr, cpu.r[rs], offset);
+                    }
+                },
+                0b01 => blk: {
+                    // LSR
+                    if (offset == 0) {
+                        cpu.cpsr.c.write(cpu.r[rs] >> 31 & 1 == 1);
+                        break :blk @as(u32, 0);
+                    } else {
+                        break :blk shifter.logicalRight(true, &cpu.cpsr, cpu.r[rs], offset);
+                    }
+                },
+                0b10 => blk: {
+                    // ASR
+                    if (offset == 0) {
+                        cpu.cpsr.c.write(cpu.r[rs] >> 31 & 1 == 1);
+                        break :blk @bitCast(u32, @bitCast(i32, cpu.r[rs]) >> 31);
+                    } else {
+                        break :blk shifter.arithmeticRight(true, &cpu.cpsr, cpu.r[rs], offset);
+                    }
+                },
                 else => cpu.panic("[CPU|THUMB|Fmt1] {} is an invalid op", .{op}),
             };