cpu/arm: fix MSR by changing modes

Signed-off-by: Amneesh Singh <natto@weirdnatto.in>

src/cpu/arm/exec.cc

@@ -377,6 +377,11 @@ Instruction::exec(Cpu& cpu) {
                     pc_error(data.operand);
 
                     if (cpu.cpsr.mode() != Mode::User) {
+                        if (!data.spsr) {
+                            Psr tmp = Psr(cpu.gpr[data.operand]);
+                            cpu.chg_mode(tmp.mode());
+                        }
+
                         psr.set_all(cpu.gpr[data.operand]);
                     }
                     break;

src/cpu/cpu.cc

@@ -1,6 +1,7 @@
 #include "cpu/cpu.hh"
 #include "cpu/arm/instruction.hh"
 #include "cpu/thumb/instruction.hh"
+#include "util/bits.hh"
 #include "util/log.hh"
 #include <algorithm>
 #include <cstdio>
@@ -131,8 +132,12 @@ Cpu::chg_mode(const Mode to) {
 
 void
 Cpu::step() {
-    // Current instruction is two instructions behind PC
+    // halfword align
+    rst_bit(pc, 0);
     if (cpsr.state() == State::Arm) {
+        // word align
+        rst_bit(pc, 1);
+        // Current instruction is two instructions behind PC
         uint32_t cur_pc = pc - 2 * arm::INSTRUCTION_SIZE;
         arm::Instruction instruction(bus->read_word(cur_pc));