core: arm7tdmi: Align PC according to current ISA state

Former-commit-id: 049f01247f3fcc429f07e1761ceed25e749ce77e

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"),
         }
     }