From 4da863da9bca413490b4fffbfab9d0c5b746bafd Mon Sep 17 00:00:00 2001
From: Michel Heily <michelheily@gmail.com>
Date: Fri, 5 Jul 2019 18:31:08 +0300
Subject: [PATCH] Fix carry flag on barrel shifter ops

Former-commit-id: 31cf2166d97fcdcca0dd9c59591f5e28b5b5f6cc
---
 src/arm7tdmi/arm/exec.rs | 44 +++++++++++++++++++++++++++++++++-------
 1 file changed, 37 insertions(+), 7 deletions(-)

diff --git a/src/arm7tdmi/arm/exec.rs b/src/arm7tdmi/arm/exec.rs
index 2100654..ada05c6 100644
--- a/src/arm7tdmi/arm/exec.rs
+++ b/src/arm7tdmi/arm/exec.rs
@@ -82,12 +82,42 @@ impl Core {
         Ok(CpuPipelineAction::IncPC)
     }
 
-    fn barrel_shift(val: i32, amount: u32, shift: ArmShiftType) -> i32 {
+    fn barrel_shift(&mut self, val: i32, amount: u32, shift: ArmShiftType) -> i32 {
         match shift {
-            ArmShiftType::LSL => val.wrapping_shl(amount),
-            ArmShiftType::LSR => (val as u32).wrapping_shr(amount) as i32,
-            ArmShiftType::ASR => val.wrapping_shr(amount),
-            ArmShiftType::ROR => val.rotate_right(amount),
+            ArmShiftType::LSL => {
+                if amount < 32 {
+                    self.cpsr.set_C(val.wrapping_shr(32 - amount) & 1 == 1);
+                } else {
+                    if amount == 32 {
+                        self.cpsr.set_C(val & 1 == 1);
+                    } else {
+                        self.cpsr.set_C(false)
+                    }
+                }
+                val.wrapping_shl(amount)
+            }
+            ArmShiftType::LSR => {
+                if amount < 32 {
+                    self.cpsr.set_C(val.wrapping_shr(amount - 1) & 1 == 1);
+                } else {
+                    self.cpsr.set_C(false);
+                }
+                (val as u32).wrapping_shr(amount) as i32
+            }
+            ArmShiftType::ASR => {
+                if amount < 32 {
+                    self.cpsr.set_C(val.wrapping_shr(amount - 1) & 1 == 1);
+                } else {
+                    self.cpsr.set_C(val >> 31 == 1);
+                }
+                val.wrapping_shr(amount)
+            }
+            ArmShiftType::ROR => {
+                let amount = amount % 32;
+                let result = val.rotate_right(amount);
+                self.cpsr.set_C((result >> 1)  &1 == 1);
+                result
+            }
         }
     }
 
@@ -95,11 +125,11 @@ impl Core {
         let val = self.get_reg(reg) as i32;
         match shift {
             ArmRegisterShift::ShiftAmount(amount, shift) => {
-                Ok(Core::barrel_shift(val, amount, shift))
+                Ok(self.barrel_shift(val, amount, shift))
             }
             ArmRegisterShift::ShiftRegister(reg, shift) => {
                 if reg != REG_PC {
-                    Ok(Core::barrel_shift(val, self.get_reg(reg) & 0xff, shift))
+                    Ok(self.barrel_shift(val, self.get_reg(reg) & 0xff, shift))
                 } else {
                     Err(CpuError::IllegalInstruction)
                 }