From 40de6bf893e25722477c80d952d9b2b863eb0916 Mon Sep 17 00:00:00 2001
From: Michel Heily <michelheily@gmail.com>
Date: Fri, 17 Jan 2020 16:09:45 +0200
Subject: [PATCH] core: arm7tdmi: Align PC according to current ISA state

Former-commit-id: 049f01247f3fcc429f07e1761ceed25e749ce77e
---
 src/core/arm7tdmi/cpu.rs | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/src/core/arm7tdmi/cpu.rs b/src/core/arm7tdmi/cpu.rs
index 5110b10..111284b 100644
--- a/src/core/arm7tdmi/cpu.rs
+++ b/src/core/arm7tdmi/cpu.rs
@@ -106,7 +106,12 @@ impl Core {
     pub fn set_reg(&mut self, r: usize, val: u32) {
         match r {
             0...14 => self.gpr[r] = val,
-            15 => self.pc = val & !1,
+            15 => self.pc = {
+                match self.cpsr.state() {
+                    CpuState::THUMB => val & !1,
+                    CpuState::ARM => val & !3
+                }
+            },
             _ => panic!("invalid register"),
         }
     }