armwrestler-fix: Properly handle misaligned addresses LDR/LDRH/LDRSH

Former-commit-id: 742a7c2b8413fa9d45df1575a0b14b8d1ab697c4

src/core/arm7tdmi/alu.rs

@@ -98,11 +98,12 @@ impl BarrelShifterValue {
 
 impl Core {
     /// Performs a generic barrel shifter operation
-    fn barrel_shift(
+    pub fn barrel_shift(
         &mut self,
         val: i32,
         amount: u32,
         shift: BarrelShiftOpCode,
+        carry: &mut bool,
         immediate: bool,
     ) -> i32 {
         //
@@ -128,33 +129,33 @@ impl Core {
             BarrelShiftOpCode::LSL => match amount {
                 0 => val,
                 x if x < 32 => {
-                    self.cpsr.set_C(val.wrapping_shr(32 - x) & 1 == 1);
+                    *carry = val.wrapping_shr(32 - x) & 1 == 1;
                     val << x
                 }
                 32 => {
-                    self.cpsr.set_C(val & 1 == 1);
+                    *carry = val & 1 == 1;
                     0
                 }
                 _ => {
-                    self.cpsr.set_C(false);
+                    *carry = false;
                     0
                 }
             },
             BarrelShiftOpCode::LSR => match amount {
                 0 | 32 => {
                     if immediate {
-                        self.cpsr.set_C((val as u32).bit(31));
+                        *carry = (val as u32).bit(31);
                         0
                     } else {
                         val
                     }
                 }
                 x if x < 32 => {
-                    self.cpsr.set_C(val >> (amount - 1) & 1 == 1);
+                    *carry = val >> (amount - 1) & 1 == 1;
                     ((val as u32) >> amount) as i32
                 }
                 _ => {
-                    self.cpsr.set_C(false);
+                    *carry = false;
                     0
                 }
             },
@@ -162,7 +163,7 @@ impl Core {
                 0 => {
                     if immediate {
                         let bit31 = (val as u32).bit(31);
-                        self.cpsr.set_C(bit31);
+                        *carry = bit31;
                         if bit31 {
                             -1
                         } else {
@@ -173,12 +174,12 @@ impl Core {
                     }
                 }
                 x if x < 32 => {
-                    self.cpsr.set_C(val.wrapping_shr(amount - 1) & 1 == 1);
+                    *carry = val.wrapping_shr(amount - 1) & 1 == 1;
                     val.wrapping_shr(amount)
                 }
                 _ => {
                     let bit31 = (val as u32).bit(31);
-                    self.cpsr.set_C(bit31);
+                    *carry = bit31;
                     if bit31 {
                         -1
                     } else {
@@ -192,7 +193,7 @@ impl Core {
                         if immediate {
                             /* RRX */
                             let old_c = self.cpsr.C() as i32;
-                            self.cpsr.set_C(val & 0b1 != 0);
+                            *carry = val & 0b1 != 0;
                             ((val as u32) >> 1) as i32 | (old_c << 31)
                         } else {
                             val
@@ -205,7 +206,7 @@ impl Core {
                         } else {
                             val
                         };
-                        self.cpsr.set_C((val as u32).bit(31));
+                        *carry = (val as u32).bit(31);
                         val
                     }
                 }
@@ -215,13 +216,19 @@ impl Core {
 
     pub fn register_shift(&mut self, reg: usize, shift: ShiftedRegister) -> CpuResult<i32> {
         let val = self.get_reg(reg) as i32;
+        let mut carry = self.cpsr.C();
         match shift {
             ShiftedRegister::ByAmount(amount, shift) => {
-                Ok(self.barrel_shift(val, amount, shift, true))
+                let result = self.barrel_shift(val, amount, shift, &mut carry, true);
+                self.cpsr.set_C(carry);
+                Ok(result)
             }
             ShiftedRegister::ByRegister(reg, shift) => {
                 if reg != REG_PC {
-                    Ok(self.barrel_shift(val, self.get_reg(reg) & 0xff, shift, false))
+                    let result =
+                        self.barrel_shift(val, self.get_reg(reg) & 0xff, shift, &mut carry, false);
+                    self.cpsr.set_C(carry);
+                    Ok(result)
                 } else {
                     Err(CpuError::IllegalInstruction)
                 }

src/core/arm7tdmi/arm/exec.rs

@@ -232,7 +232,7 @@ impl Core {
             let data = if insn.transfer_size() == 1 {
                 self.load_8(addr, bus) as u32
             } else {
-                self.load_32(addr, bus)
+                self.ldr_word(addr, bus)
             };
 
             self.set_reg(insn.rd(), data);
@@ -287,10 +287,8 @@ impl Core {
         if insn.load_flag() {
             let data = match insn.halfword_data_transfer_type().unwrap() {
                 ArmHalfwordTransferType::SignedByte => self.load_8(addr, bus) as u8 as i8 as u32,
-                ArmHalfwordTransferType::SignedHalfwords => {
+                ArmHalfwordTransferType::SignedHalfwords => self.ldr_sign_half(addr, bus),
-                    self.load_16(addr, bus) as u16 as i16 as u32
+                ArmHalfwordTransferType::UnsignedHalfwords => self.ldr_half(addr, bus),
-                }
-                ArmHalfwordTransferType::UnsignedHalfwords => self.load_16(addr, bus) as u16 as u32,
             };
 
             self.set_reg(insn.rd(), data);

src/core/arm7tdmi/cpu.rs

@@ -5,6 +5,7 @@ use num_traits::Num;
 
 pub use super::exception::Exception;
 use super::{
+    alu::*,
     arm::*,
     bus::{Bus, MemoryAccess, MemoryAccessType, MemoryAccessType::*, MemoryAccessWidth::*},
     psr::RegPSR,
@@ -75,7 +76,7 @@ impl Core {
         match r {
             0...14 => self.gpr[r],
             15 => self.pc,
-            _ => panic!("invalid register"),
+            _ => panic!("invalid register {}", r),
         }
     }
 
@@ -123,6 +124,48 @@ impl Core {
         }
     }
 
+    pub fn ror(&mut self, value: u32, rotation: u32) -> u32 {
+        let mut carry = false; // we don't need to update the carry flag
+        self.barrel_shift(
+            value as i32,
+            rotation,
+            BarrelShiftOpCode::ROR,
+            &mut carry,
+            false,
+        ) as u32
+    }
+
+    /// Helper function for "ldr" instruction that handles misaligned addresses
+    pub fn ldr_word(&mut self, addr: Addr, bus: &Bus) -> u32 {
+        if addr & 0x3 != 0 {
+            let rotation = (addr & 0x3) << 3;
+            let value = self.load_32(addr & !0x3, bus);
+            self.ror(value, rotation)
+        } else {
+            self.load_32(addr, bus)
+        }
+    }
+
+    /// Helper function for "ldrh" instruction that handles misaligned addresses
+    pub fn ldr_half(&mut self, addr: Addr, bus: &Bus) -> u32 {
+        if addr & 0x1 != 0 {
+            let rotation = (addr & 0x1) << 3;
+            let value = self.load_16(addr & !0x1, bus);
+            self.ror(value as u32, rotation)
+        } else {
+            self.load_16(addr, bus) as u32
+        }
+    }
+
+    /// Helper function for "ldrsh" instruction that handles misaligned addresses
+    pub fn ldr_sign_half(&mut self, addr: Addr, bus: &Bus) -> u32 {
+        if addr & 0x1 != 0 {
+            self.load_8(addr, bus) as i8 as i32 as u32
+        } else {
+            self.load_16(addr, bus) as i16 as i32 as u32
+        }
+    }
+
     pub fn get_registers(&self) -> [u32; 15] {
         self.gpr.clone()
     }
@@ -211,20 +254,20 @@ impl Core {
         }
     }
 
-    pub fn load_32(&mut self, addr: Addr, bus: &mut Bus) -> u32 {
+    pub fn load_32(&mut self, addr: Addr, bus: &Bus) -> u32 {
         self.add_cycles(addr, bus, self.cycle_type(addr) + MemoryAccess32);
         self.memreq = addr;
         bus.read_32(addr)
     }
 
-    pub fn load_16(&mut self, addr: Addr, bus: &mut Bus) -> u16 {
+    pub fn load_16(&mut self, addr: Addr, bus: &Bus) -> u16 {
         let cycle_type = self.cycle_type(addr);
         self.add_cycles(addr, bus, cycle_type + MemoryAccess16);
         self.memreq = addr;
         bus.read_16(addr)
     }
 
-    pub fn load_8(&mut self, addr: Addr, bus: &mut Bus) -> u8 {
+    pub fn load_8(&mut self, addr: Addr, bus: &Bus) -> u8 {
         let cycle_type = self.cycle_type(addr);
         self.add_cycles(addr, bus, cycle_type + MemoryAccess8);
         self.memreq = addr;

src/core/arm7tdmi/thumb/exec.rs

@@ -169,7 +169,7 @@ impl Core {
             let data = if insn.is_transferring_bytes() {
                 self.load_8(addr, bus) as u32
             } else {
-                self.load_32(addr, bus)
+                self.ldr_word(addr, bus)
             };
 
             self.set_reg(insn.rd(), data);
@@ -216,7 +216,7 @@ impl Core {
             (false, true) =>
             /* ldrh */
             {
-                self.gpr[rd] = self.load_16(addr, bus) as u32
+                self.gpr[rd] = self.ldr_half(addr, bus)
             }
             (true, false) =>
             /* ldsb */
@@ -227,7 +227,7 @@ impl Core {
             (true, true) =>
             /* ldsh */
             {
-                let val = self.load_16(addr, bus) as i16 as i32 as u32;
+                let val = self.ldr_sign_half(addr, bus);
                 self.gpr[rd] = val;
             }
         }
@@ -257,7 +257,7 @@ impl Core {
         let base = self.gpr[insn.rb()] as i32;
         let addr = base.wrapping_add((insn.offset5() << 1) as i32) as Addr;
         if insn.is_load() {
-            let data = self.load_16(addr, bus);
+            let data = self.ldr_half(addr, bus);
             self.add_cycle();
             self.gpr[insn.rd()] = data as u32;
         } else {